Self-Balancing Car Racing

Project Description

This project simulates a two-wheeled robot that is similar to the segway robotic mobility platform. It has actuators that allow you to specify the torque applied to each wheel. It also has sensors that are able to measure the forward speed, the turning rate, the turning radius of a road along which the robot can drive, as well as the error in lateral position and the error in heading relative to this road. The goal is to make the robot race along this road—without crashing—as fast as possible. LQR controller will be used to achieve such a goal. The simulation is provided by [1]

To be more specific, the goal is to start from the origin and reach the final destination of the random generated roads in 50 seconds without running off the road or dropping the chassis. We will verify the system by first generating a random road, and then the robot will need to start from the origin and reach the destination without running off the road or dropping the chassis. We will record the time duration for the robot to reach the destination. Repeat the above step for ten times with different random generated roads. If the robot never runs off the road or drops the chassis for ten trials and the average time duration for ten trials is smaller than 50 seconds, the requirement is met.

Model

The motion of the robot is governed by ordinary differential equations with the form

$$\begin{bmatrix} \ddot{\phi} \\ \dot{v} \\ \dot{w} \end{bmatrix} = f(\phi,\dot{\phi},v,w,\tau_{R},\tau_{L})$$

where $\phi$ is the pitch angle of the chassis, $\dot{\phi}$ is the pitch angular velocity, $v$ is the forward speed, $w$ is the turning rate, and $\tau_{R}$ and $\tau_{L}$ are the torques applied by the chassis to the right and left wheel, respectively. [2,3]

To keep track of the position and orientation {\em relative to the road}. $r_\text{road}$ is the radius of curvature of the road along which the robot is currently moving. $v_\text{road}$ is the speed at which you would like to travel along the road, the turning rate $w_\text{road}$ necessary to follow its centerline can be computed as $w_\text{road} = \frac{v_\text{road}}{r_\text{road}}$. Now, define $e_\text{lateral}$ as the perpendicular distance from the centerline of the road to the position $(x,y)$ of the robot, and define $e_\text{heading}$ as the difference between the orientation $\theta$ of the robot and the direction of the road. It is possible to show that

$$ \dot{e}_{lateral} = -v\sin(\dot{e}_{heading}) $$

$$ \dot{e}_{heading} = w-(\frac{v\cos(e_{heading})}{v_{road}+w_{road}e_{lateral}})w_{road} $$

Linearizing the nonlinear system and put the linearized equation into space state model. The equilibrium state and equilibrium input will be set up as

$$ \begin{bmatrix} \phi_{e} \\ \dot{\phi}_{e} \\ v_{e} \\ w_{e} \\ e_{lateral_e} \\ e_{heading_e} \\ \end{bmatrix} = \begin{bmatrix} 0\\ 0\\ 3\\ 0\\ 0\\ 0\\ \end{bmatrix} \qquad \begin{bmatrix} \tau_{R_e}\\ \tau_{L_e} \end{bmatrix} = \begin{bmatrix} 0\\ 0 \end{bmatrix} $$

We will linearize the system using Jacobian, we get A and B: $$ A = \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0\\ 27.443 & 0 & 0 & 0 & 0 & 0\\ -3.454 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & -3\\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \qquad B = \begin{bmatrix} 0 & 0\\ -2.2255 & -2.2255\\ 0.5874 & 0.5874\\ 2.4814 & -2.4814\\ 0 & 0\\ 0 & 0 \end{bmatrix} $$

We will then find C and D. Since the sensors provide us only the data for $v$, $w$, $e_{lateral}$, and $e_{heading}$ the first two variables should be zero. The output does not depend on the input of the system, so D is zero.

$$ C = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad D = \begin{bmatrix} 0 \end{bmatrix} $$

We plug matrix A, B, C, and D into the space state model. $$ \dot{x} = Ax+Bu $$

$$ y = Cx+Du $$

Controller

Controllability

Finding controllability matrix $W_{c}$ in Matlab.

Wc = ctrb(A,B)

The rank for $W_{c}$ is 6, which is in full rank, so the open-loop linear system is controllable.

Stability

Find the eigenvalues of A. The eigenvalues are $S_{1} = 0$, $S_{2} = 5.2386$, $S_{3} = -5.2386$, $S_{4} = 0$, $S_{5} = 0$, and $S_{6} = 0$ There are five eigenvalues that has no negative real part, so the open-loop linear system is not asymptotically stable

State Feedback

Finding matrix $K$ using $lqr$ with

Q_c = diag([1 1 3 1 1000 1000]);
R_c = eye(2)

The result of $lqr$ is: $$ K = \begin{bmatrix} -14.297 & -2.9744 & -1.2247 & 3.6275 & -22.361 & 31.412\\ -14.297 & -2.9744 & -1.2247 & -3.6275 & 22.361 & -31.412 \end{bmatrix} $$ Check the stability with the calculated K by calculating the eigenvalues of ($A-BK$). The eigenvalues are $S_{1} = -7.1312$, $S_{2} = -0.73654$, $S_{3} = -3.9326$, $S_{4} = -7.4994+7.3889i$, $S_{5} = -7.4994-7.3889i$, and $S_{6} = -3.0036$. All eigenvalues have negative real part, so the closed-loop linear system is asymptotically stable.

Observer

Finding observability matrixWofor in Matlab.

Wo = obsv(A,C)

the rank for $W_{o}$ is 6, which is in full rank, so the closed-loop linear system is observable.

Finding matrix $L$ using $lqr$ function in Matlab with

Q_o = eye(4);
R_o = diag([1 1 1 1 0.001 0.001])

The Matlab code for finding matrix L is:

L = lqr(A',C',inv(R_o),inv(Q_o))';

Check the stability with the calculated L by calculating the eigenvalue of ($A-LC$). They are $S_{1} = -5.2489+0.32902i$, $S_{2} = -5.2489-0.32902i$, $S_{3} = -31.658+1.4983i$, $S_{4} = -31.658-1.4983i$, $S_{5} = -1$, and $S_{6} = -1.0005$. All eigenvalues have negative real part, so the observer is asymptotic stable.

Experiment

Control Design Version 1

Q_c = diag([1 1 1 1 1 1]); R_c = eye(2);
Q_o = eye(4); R_o = diag([1 1 1 1 1 1]);

The robot goes straight and crashes on curve. The failure occurs because the Q and R have a ratio of one, which means it will converge to the target direction very slowly, so when the robot counters a curve, it does not have enough time to adjust to the correct direction which leads to crashing. Increasing the ratio of Q and R in both controller and observer can solve this problem. By implementing this change and following the step in verification, we verify that the requirement is met.

Control Design Version 2

Qc = diag([1000 1 30 300 1000 1000]); Rc = eye(2);
Qo = diag([1 1 1 1]); Ro = diag([1 1 1 1 0.001 0.001]);

The robot always drops the chassis at the beginning. The failure occurs because the acceleration is too fast. When we decrease the third diagonal element, which corresponding to the speed, in $Q_{c}$ to 3, the robot does not fall down anymore. By implementing this change and following the step in verification, we verify that the requirement is met.

Control Design Version 3

Qc = diag([1 1 3 1 1 1]); Rc = eye(2);
Qo = diag([1 1 1 1]); Ro = diag([1 1 1 1 0.001 1]);

The robot always drives off the road at the curve. The failure occurs because we do not penalize $e_{lateral}$ enough, so the robot does not turn enough at the curve. We need to increase the fifth diagonal element of $Q_{c}$ to 1000 to solve this problem. By implementing this change and following the step in verification, we verify that the requirement is met.

Control Design Final Version

The final version of the control design is described in the Controller and Observer at the above section. To verify the final design, we follow the step in verification by generating 10 different random roads and running the simulator on the ten different random roads. The robot crashes in none of the trial. We then calculate the average finish time for the ten trial, which is 42.958 seconds, so the requirement is met.

[1] UIUC AE353 Aerospace Control System by Timothy Bretl
[2] Y. X. Mak, “Realization of mini segway robot using ni myrio,” Essay (Bachelor),University of Twente, 2015
[3] Z. Tobias and F. Matthias, “Tracking control of a balancing robot—a model-basedapproach,”Archive of Mechanical Engineering, vol. 61, no. 2, pp. 331–346, Apr. 2014.