# ARR_LQR_controller
**Repository Path**: elikit/ARR_LQR_controller
## Basic Information
- **Project Name**: ARR_LQR_controller
- **Description**: Applying LQR lateral control algorithm with error dynamic model to Articulated-Road-Roller Vehicle with trivial kinematic model assumption
- **Primary Language**: Matlab
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 2
- **Forks**: 0
- **Created**: 2021-08-02
- **Last Updated**: 2023-04-18
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
ARR-LQR Controller
Young Jie
July 30，2021
## 1 Kinematic Model of ARR Vehicle
The kinematic model of an articulated road roller can be simplified as two frames (front and rear) and an active revolute joint. Slip angle is always considered while constructing a kinematic model of wheel robot, so we also introduced slip angle beta and alpha while building up kinematic model of road roller. Here, we are using the alphabet f representing the front frame rigid body, the rear one is using the alphabet r.
As such, the relation between line velocity of the front frames and coordinate change of the front rigid body’s midpoint in global Cartesian coordinate system could be revealed, and be written as the follow equations (1-1)，according to the depicted geometrical characteristics of ARR-vehicle.
$$
\dot{x_f} = v_f*cos(\theta_f+\beta) \\
\dot{y_f} = v_f*sin(\theta_f+\beta) \tag{1-1}
$$
In considering the geometry constraints of a road-roller，its two rigid-body frame‘s line velocity and angular velocity follow the simple mathematical rules as the following matrix form：
$$
\begin{bmatrix} cos\alpha \quad \quad 0 \\ sin\alpha \quad -l_r \end{bmatrix} \begin{bmatrix} v_r \\ \dot{\theta_r} \end{bmatrix} = \begin{bmatrix} cos\gamma \quad -sin\gamma \\ sin\gamma \quad\quad cos\gamma \end{bmatrix} \times \begin{bmatrix} cos\beta \quad 0 \\ sin\beta \quad l_f \end{bmatrix} \begin{bmatrix} v_f \\ \dot{\theta_f} \end{bmatrix} \tag{1-2}
$$
And it is easy to know the equation of front rigid-body yaw angle and rear rigid-body yaw angle：
$$
\theta_r = \theta_f + \gamma \\
\dot{\theta_r} = \dot{\theta_f} + \dot{\gamma} \tag{1-3}
$$
Combine equations （1-2）and （1-3）, and rewrite it, we have:
$$
\begin{align}
&v_r sin\alpha - l_r \dot{\theta_r} = v_f sin\gamma cos\beta + v_f sin\beta cos\gamma + l_f \dot{\theta_f} cos\gamma \\
&v_r cos\alpha = v_f cos\beta cos\gamma - v_f sin\beta sin\gamma - l_f \dot{\theta_f} sin\gamma \tag{1-4}
\end{align}
$$
and then
$$
\begin{align}
&v_r sin\alpha - l_r \dot{\theta_f} - l_r \dot{\gamma} = v_f sin(\gamma + \beta) + l_f \dot{\theta_f} cos\gamma \\
&v_r cos\alpha = v_f cos(\gamma + \beta) - l_f \dot{\theta_f} sin\gamma \tag{1-5}
\end{align}
$$
and then
$$
\begin{align}
&v_f cos(\gamma + \beta) sin\alpha - l_f \dot{\theta_f} sin\gamma sin\alpha - l_r \dot{\theta_f} cos\alpha - l_r \dot{\gamma} cos\alpha = v_f sin(\gamma + \beta) cos\alpha + l_f \dot{\theta_f} cos\gamma cos\alpha \\
&-v_f sin(\gamma + \beta - \alpha) - l_r \dot{\gamma} cos\alpha = \dot{\theta_f}(l_r cos\alpha + l_f cos(\gamma - \alpha)) \tag{1-6}
\end{align}
$$
As a result, we could draw a conclusion of kinematic formulas about angular velocity as the equations （1-7）:
$$
\dot{\theta_f} = \frac{-v_f sin(\gamma + \beta - \alpha) - l_r \dot{\gamma} cos\alpha}{l_r cos\alpha + l_f cos(\gamma - \alpha)} \\
\dot{\theta_r} = \frac{-v_f sin(\gamma + \beta - \alpha) + l_f \dot{\gamma} cos(\gamma - \alpha)}{l_r cos\alpha + l_f cos(\gamma - \alpha)} \tag{1-7}
$$
## 2 Error Dynamic Modeling
$$
\begin{align}
&\Delta \epsilon_l = (r \cdot \Delta\omega) \cdot sin\epsilon_h \\
&sin\epsilon_h \approx \epsilon_h \\
&\Delta \epsilon_l = (r \cdot \Delta\omega) \cdot \epsilon_h \tag{2-1} \\
&\dot{\epsilon_l} = \Delta\epsilon_l / \Delta t = (r \cdot \Delta\omega / \Delta t) \cdot \epsilon_h \\
&v_f = r \cdot \dot{\omega} = r \cdot \Delta\omega / \Delta t
\end{align}
$$
$$
\dot{\epsilon_l} = v_f \cdot \epsilon_h \tag{2-2}
$$
$$
\begin{align}
&(R - \epsilon_l) \cdot \Delta\Omega = r \cdot \Delta\omega \\
&R >> \epsilon_l \\
&\Delta\epsilon_h = \Delta\omega - \Delta\Omega = \Delta\omega \cdot (1 - \frac{r}{R}) \\
&\epsilon_c = \frac{1}{r} - \frac{1}{R}\\
&\dot{\epsilon_h} = \Delta\epsilon_h / \Delta t = \frac{\Delta\omega}{\Delta t} \cdot r \cdot (\frac{1}{r} - \frac{1}{R}) \tag{2-3}
\end{align}
$$
$$
\dot{\epsilon_h} = v_f \cdot \epsilon_c \tag{2-4}
$$
$$
\begin{align}
&\epsilon_c = \frac{1}{r} - \frac{1}{R}\\
&\frac{1}{r} = \dot{\theta_f}/v_f = \frac{-v_f sin(\gamma + \beta - \alpha) + l_f \dot{\gamma} cos(\gamma - \alpha)}{v_f(l_r cos\alpha + l_f cos(\gamma - \alpha))} \\
&\dot{\epsilon_c} = \frac{d\frac{1}{r}}{dt} \tag{2-5}
\end{align}
$$
$$
\dot{\epsilon_c} = \dot{\gamma} \cdot \frac{(l_f+l_r)+l_f(\gamma^2 + \beta\gamma - 2\alpha\gamma - \alpha\beta + \alpha^2)}{(l_f+l_r)^2} \tag{2-6}
$$
$$
\begin{bmatrix} \dot{\epsilon_l} \\ \dot{\epsilon_h} \\ \dot{\epsilon_c} \end{bmatrix} = \begin{bmatrix} 0 \quad v_f \quad 0 \\ 0 \quad 0 \quad v_f \\ 0 \quad 0 \quad 0 \end{bmatrix} \begin{bmatrix} \epsilon_l \\ \epsilon_h \\ \epsilon_c \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ \frac{(l_f+l_r)+l_f(\gamma^2 + \beta\gamma - 2\alpha\gamma - \alpha\beta + \alpha^2)}{(l_f+l_r)^2} \end{bmatrix} \dot{\gamma} \tag{2-7}
$$
$$
\begin{align}
&\dot{\boldsymbol{x}} = \boldsymbol{A}\boldsymbol{x} + \boldsymbol{B}\boldsymbol{u} \\
&\boldsymbol{x} = \begin{bmatrix} \epsilon_l \\ \epsilon_h \\ \epsilon_c \end{bmatrix} \\
&\boldsymbol{u} = \dot{\gamma}\\
&\boldsymbol{A} = \begin{bmatrix} 0 \quad v_f \quad 0 \\ 0 \quad 0 \quad v_f \\ 0 \quad 0 \quad 0 \end{bmatrix}\\
&\boldsymbol{B} = \begin{bmatrix} 0 \\ 0 \\ \frac{(l_f+l_r)+l_f(\gamma^2 + \beta\gamma - 2\alpha\gamma - \alpha\beta + \alpha^2)}{(l_f+l_r)^2} \end{bmatrix} \tag{2-8}
\end{align}
$$ {u}
$$
\begin{align}
&\tilde{\boldsymbol{A}} = (I + \Delta t \boldsymbol{A}/2)(I + \Delta t \boldsymbol{A}/2)^{-1} \\
&or \\
&\tilde{\boldsymbol{A}} = I + \Delta t \boldsymbol{A} \\
\\
&\tilde{\boldsymbol{B}} = \Delta t \boldsymbol{B} \\
&\boldsymbol{x}_{t+1} = \tilde{\boldsymbol{A}}\boldsymbol{x}_t + \tilde{\boldsymbol{B}}\boldsymbol{u}_t \tag{2-9}
\end{align}
$$
## 3 LQR-controller
$$
\begin{align}
&Min \quad J = \frac{1}{2} \displaystyle\sum_{t=0}^{N-1} (\boldsymbol{x}_t^T\boldsymbol{Q}\boldsymbol{x}_t + \boldsymbol{u}_t^T\boldsymbol{R}\boldsymbol{u}_t) + \boldsymbol{x}_N^T\boldsymbol{Q}_f\boldsymbol{x}_N \\
&s.t. \quad \\
& \quad \quad \boldsymbol{x}_{t+1} = \tilde{\boldsymbol{A}}\boldsymbol{x}_t + \tilde{\boldsymbol{B}}\boldsymbol{u}_t \quad \quad \forall \ t=0,1,...,N-1 \tag{3-1}
\end{align}
$$
$$
\boldsymbol{P}_{N}=\boldsymbol{Q}_{f} \tag{3-2}
$$
$$
\boldsymbol{P}_{t-1}=\boldsymbol{Q}+\tilde{\boldsymbol{A}}^T\boldsymbol{P}_{t}\tilde{\boldsymbol{A}}-\tilde{\boldsymbol{A}}^T\boldsymbol{P}_{t}\tilde{\boldsymbol{B}}(\boldsymbol{R}+\tilde{\boldsymbol{B}}^T\boldsymbol{P}_{t}\tilde{\boldsymbol{B}})^{-1} \tilde{\boldsymbol{B}}^T\boldsymbol{P}_{t}\tilde{\boldsymbol{A}} \tag{3-3}
$$
$$
\boldsymbol{K}=-(\boldsymbol{R}+\tilde{\boldsymbol{B}}^T\boldsymbol{P}\tilde{\boldsymbol{B}})^{-1} \tilde{\boldsymbol{B}}^T\boldsymbol{P}\tilde{\boldsymbol{A}} \tag{3-4}
$$
## 4 Reference
[1] Altafini C . A Path-Tracking Criterion for an LHD Articulated Vehicle[J]. Int.j.robot.res, 1999, 18(5):435-441.
[2] Corke P , Ridley P . Autonomous Control of an Underground Mining Vehicle. 2001.
[3] D Yao, Xie H , Qiang W , et al. Accurate trajectory tracking with disturbance-resistant and heading estimation method for self-driving vibratory roller[J]. IFAC-PapersOnLine, 2018, 51( 31):754-758.
[4] Nayl T , Nikolakopoulos G , Gustafsson T . Switching model predictive control for an articulated vehicle under varying slip angle[M]. 2012.
[5] Thaker, Nayl, George, Nikolakopoulos, Thomas, & Gustafsson. A full error dynamics switching modeling and control scheme for an articulated vehicle[J]. International Journal of Control Automation & Systems, 2015.
[6] Yang M , Bian Y , Liu G , et al. Path Tracking Control of an Articulated Road Roller with Sideslip Compensation[J]. IEEE Access, 2020, PP(99):1-1.