Next time I could do better. To view the topic part, click here, for some substantiation, I’ve skipped them, as they’re not presented clearly from the beginning.
To view the problems encoutered during implementation, click here.
N. Prologue
I didn’t expect this to begin so early. So I’ll try to make this a combination of paper writing and the project designing. Instead, the paper writing technique will be separated to here.
Oh, and mabe if the article is too long, I’ll consider to make it two.
I. Previously on inverted pendulum
Some priorities:
- Remodelling the invert pendulum due to the conflict between two models.
Construct mathematical model
Well there are many ways to make such model such as from
- Lagrange equation
- Force analysis
So maybe I can go through most of them.
Lagrange equation
For cart:
For the pole:
Note that $l$ is the total length of the pole.
The moment of inertia to center of mass:
The moment of intertia to one end of the pole, perpendicular
The kinematic and potential energy for cart:
The position of the pole:
From Eq. 5, derive the pole’s kinematic and potential energy:
where
Substitute Eq. 4, 6, 7 into Eq. 2
Using Eq. 1 for displacement:
Substitute:
Using Eq. 1 for angle:
Substuting:
Now we have the following relation:
Rewrite the system into matrix form:
Introduce
and whose inverse matrix:
under the condition $\theta\ll 1$
Expand states, perform linearlization,
In state-space representation
Force analysis
As there are some notation error in the reference document, I assume I shouold do the process once again to ensure this is the right thing.
For the torque of pole with respect to one end:
(note that the sign for couter-clockwise torque is negative, while the positive direction of $F_y$ is downward)
Some process is eliminated to cut redundant part.
Newton’s third law on the pole’s center of mass (refer to Eq 1.5):
And the motion of cart:
Considering the formulas above Eq 2.1, 2, 3, 4, obtain the mathematical model:
Currently, this part will be suspended until the main thesis is complete.
II. Thesis Topic
The Design of Controllers for Two-wheeled Self-balancing Robot
As the robot itself, the double-pendulum structure is selected.
Notations for parameters of the robot are as follows:
| Parameter Name | Symbol | Unit |
|---|
| Mass of First pendulum | $M_1$ | $kg$ |
| Mass of Second Pendulum | $M_2$ | $kg$ |
| Length to the CoM the First pendulum | $L_1$ | $m$ |
| Length to the CoM the Second pendulum | $L_2$ | $m$ |
| Length between center of wheels | $D_m$ | $m$ |
| Radius of wheels | $R_w$ | $m$ |
| Mass of the wheels | $M_L=M_R=M_w$ | $kg$ |
| Inertia of the wheels | $J_L=J_R=J_w$ | $N\cdot m/rad$ |
Notations for variables of the robot are as follows
| Parameter Name | Symbol | Unit |
|---|
| Pitch angle of lower pendulum | $\theta_1$ | $rad$ |
| Pitch angle of upper pendulum | $\theta_2$ | $rad$ |
| Yaw angle | $\psi$ | $rad$ |
| Angular Displacement of wheels | $\theta_L, \theta_R$ | $rad/s$ |
| Linear speed of the wheels | $x_L, x_R$ | $m/s$ |
| System forward speed | $x =(x_L+x_R)/2$ | $m/s$ |
1. Kinematics Modelling
Describing its relation without considering internal effects.
Velocity Analysis
1.1 The velocity of the CoM (Center of Mass) for lower body:
Yaw angle is not considered in the reasearch.
1.3 The horizontal linear velocity of wheels (CoM):
1.4 The velocity for $B_1,\ B_2$ in angle coordinates. (deprecated)
1.5 The velocity for wheels in angle coordinates: (deprecated)
1.6 Plane motion
2. Dynamics Modelling
2.1 Select 3 DoF of the robots as general coordinates.
2.2 The kinematic engergy for the system
Wheel:
The kinetic engergy include the translational kinetic engergy $T_L$ and $T_R$, and the rotational kinetic engergy when wheels rotates around the shaft.
Similarly
As for ratational kinetic engery:
As the turning is not considered, $\dot\theta_L=\dot\theta_R$
Cart:
Cart - Translational
Cart - Rotational
Cart - whole
Pendulum:
Pendulum - Translational
Pendulum - Rotational
Pendulum - Whole
Thus the whole kinetic engergy for the system:
Potential engergy:
The potential engergy for the sysstem equals:
2.3 Lagrangian
Now we have the Lagrangian:
To simplify the lagrangian, using $M_{ij}$ to replace constant coefficients:
where,
Lagrangian equation:
Generalized coordinates: $[x\ \theta_1\ \theta_2]$
Simplified equation:
2.4 Linearization
Perform linearization around $[x, 0, 0]$, thus that $\cos\theta=1,\ \sin\theta=\theta,\ \dot\theta^2=0$, so that:
Also treat $\theta_1\theta_2=0$, we have
Transform into matrix form:
Set $M=\left[\begin{array}{ccc}2M_{11} & M_{31} & M_{32}\\M_{31} & 2M_{21} & M_{41}\\M_{32} & M_{41} & 2M_{22}\end{array}\right],\ M_{aux}=\left[\begin{array}{ccc}0 & 0 & 0\\0 & -M_{51} & 0\\0 & 0 & -M_{52}\end{array}\right]$
Then multiply its inverse on both side (if inverse exist)
Regard $M_f=M^{-1}\cdot(-M_{aux}),\ \ M_b=M^{-1}\cdot[\begin{array}{ccc}1& -2& 0\end{array}]^\intercal$
Thus we have
where
This is an implementation of the simplest expansion, other possible methods are: Tylor expansion (Maclaurin expansion), these two are more accurate, meanwhile it takes time to derive the corresponding
2.5 Nonlinear model
For original relation:
Apply transform:
Using trigonometric function to simplify:
Correspondingly:
where,
Transform
where,
3. Algorithms
SMC
target:
When $x_d$ is constant
We have
Set
Select Lynapnov function
and to do so, using exponential reaching law, let:
Then the equation becomes:
As $d_1$ is bounded and $|d_1|\le d_M$
Its easy to find that when $\eta>d_M$
The sliding surface is stable.
HSMC
Set Sliding Surface
Correspondingly we will have $n$ equivalent control
Set larger level of sliding surface:
To satisfy the Lyapunov function
Take 3-layer as an example
Let $(\alpha b_1+\beta b_2+\gamma b_3)u_{sw}+[(\beta b_2+\gamma b_3)u_{eq1}+(\alpha b_1+\gamma b_3)u_{eq2}+(\alpha b_1+\beta b_2)u_{eq3}]=-\eta sgn(S)-kS$
he remaining Lyapunov function proof is similar
As $d_i$ is bounded, assume that $|(\alpha d_1+\beta d_2+\gamma d_3)|\le D_M$ so if $\eta >D_M$
And hence,
4. Controller Design
For linearized model:
Set first-level sliding surface:
Thus we have equivalent control law:
Set second level sliding surface:
We have switching control law:
Combining two terms:
Problems encountered
1. Modelling
In system modeling, for the convenience of theoretical research, factors such as internal mechanical friction and external disturbances are ignored, and the changes in the z-axis position of the robot are not considered, making the model less accurate and affecting the design of the controller. Therefore, in subsequent research, these factors need to be taken into account.
2. Linearization
In linearizing the model near the balance point, the method used is too crude. In subsequent research, the Maclaurin expansion method can be used and the high-order terms can be discarded to make the linearized model more accurate. Fortunately, MALTAB provides function for the Taylor series, see here
3. S-function
In Version 2023a, level-1 s-function template in MATLAB language (sfunctmpl.m) is removed, and existing template in the cooresponding files are for level-2. The description web pages are link1, link2. Gladly the function is maintained, so the previous template can be used, as long as you can find it. According to the documentation, the storage of variables are indeed much safer, inspite that it could take some time convert a level-1 code into level-2. It is said that level-1 s-function runs faster than level-2, I’m not sure. At first I intended to use template in C, and only discovered that I did not have any embedded C coding experience of any sort. So lastly I stick to the matlab language for consistency. In future, I would like to try it.
4. Tuning
In the HSMC controller, due to the large number of adjustable parameters, trial and error method is finally used to determine the parameters, resulting in the performance of LQR and HSMC controllers not being optimized. Therefore, in future research, methods such as genetic algorithms can be considered to determine the parameters.
5. Switching Law, Controller Order
In the HSMC controller used in this paper, only the most basic exponential reaching law is used, which involves the sign function $sgn()$, leading to the emergence of chattering phenomenon, and it is impossible to achieve such a high switching frequency in reality. Therefore, in subsequent research, the saturation function can be considered, or higher-order sliding mode can be applied.
6. Intelligent Control
From the simulation results of controllers designed based on linear and nonlinear models, the HSMC controller based on nonlinear models cannot stabilize the system, which is due to the lack of handling coupling problems in the nonlinear model. The decoupling method in this paper only involves linearization, but this makes the model less accurate. Therefore, in subsequent research, more complex controllers can be considered, such as recursive LQR controllers, PSO optimized LQR controllers or fuzzy controllers, etc.
]]>
