自动驾驶控制算法-横向误差微分方程LQR前馈控制

本文是学习自动驾驶控制算法第六讲 前馈控制与航向误差以及前两节的学习笔记。

1 横向误差微分方程

以规划的轨迹作为自然坐标系，计算自车在轨迹上的投影点，进而计算误差：

如图所示，横向误差为$d$，航向误差为$\theta -{\theta }_r$，投影点的速度大小为$\dot{s}$，注意这里的$\theta$是航向角，与横摆角$\phi$相差一个侧偏角$\beta$
$$\begin{array}{c}\theta =\phi +\beta \end{array}$$
根据之前所介绍的笛卡尔坐标系与自然坐标系的转换关系可知：
$$\begin{array}{c}\dot{d}=v\sin (\theta -{\theta }_r)\end{array}$$
$$\begin{array}{c}\dot{s}=\frac{v\cos (\theta -{\theta }_r)}{1-k_{r}d}\end{array}$$
这里$k_r$是投影点处的曲率。
结合式1和2
$$\begin{array}{c}\dot{d}=v\sin (\phi +\beta -{\theta }_r)=v\sin \beta \cos (\phi -\theta )+v\cos \beta \sin (\phi -\theta )\end{array}$$
$\phi -{\theta }_r$为小量，所以上式进一步简化为
$$\begin{array}{c}\dot{d}=v_y+v_x(\phi -{\theta }_r)\end{array}$$
令
$$\begin{array}{c}{e}_d=d\end{array}$$
$$\begin{array}{c}{e}_{\phi }=\phi -{\theta }_r\end{array}$$
求一阶二阶导数则有
$$\begin{array}{c}\dot{e_d}=v_x{e}_{\phi }+v_y\end{array}$$
假设$v_x$是常数
$$\begin{array}{c}\dot{v_y}=\ddot{e_d}-v_x\dot{e_{\phi }}\end{array}$$
$$\begin{array}{c}{\ddot{e}}_{\phi }=\ddot{\phi }-{\ddot{\theta }}_r\approx \ddot{\phi }\end{array}$$
这里${\ddot{\theta }}_r$约等于0，是因为轨迹通常比较平滑。
综合可得
$$\begin{array}{c}\{\begin{array}{l}{v}_y={\dot{e}}_d-v_x{e}_{\phi }\\ {\dot{v}}_y={\ddot{e}}_d-v_x{\dot{e}}_{\phi }\\ \dot{\phi }={\dot{e}}_{\phi }+{\dot{\theta }}_r\\ \ddot{\phi }={\ddot{e}}_{\phi }\end{array}\end{array}$$
由上节的公式：
$$\begin{array}{c}\left[\begin{array}{c}\dot{v_y}\\ \ddot{\phi }\end{array}\right]=\left[\begin{array}{cc}\frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}-v_x\\ \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I{v}_x}& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\end{array}\right]\left[\begin{array}{c}{v}_y\\ \dot{\phi }\end{array}\right]+\left[\begin{array}{c}-\frac{C_{\alpha f}}{m}\\ -\frac{a{C}_{\alpha f}}{I}\end{array}\right]\delta \end{array}$$
结合式11和式12可得
$$\begin{array}{c}{\ddot{e}}_d=\frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}{\dot{e}}_d+(-\frac{C_{\alpha f}+C_{\alpha r}}{m})e_{\phi }+\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}{\dot{e}}_{\phi }+(\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}-v_x){\dot{\theta }}_r+(-\frac{C_{\alpha f}}{m})\delta \end{array}$$
$$\begin{array}{c}{\ddot{e}}_{\phi }=\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I{v}_x}{\dot{e}}_d+(-\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I})e_{\phi }+\frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}{\dot{e}}_{\phi }+(\frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}){\dot{\theta }}_r+(-\frac{a{C}_{\alpha f}}{I})\delta \end{array}$$
进而有
$$\begin{array}{c}\left[\begin{array}{c}{\dot{e}}_d\\ {\ddot{e}}_d\\ {\dot{e}}_{\phi }\\ {\ddot{e}}_{\phi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& -\frac{C_{\alpha f}+C_{\alpha r}}{m}& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I{v}_x}& -\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I}& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\end{array}\right]\left[\begin{array}{c}{e}_d\\ {\dot{e}}_d\\ e_{\phi }\\ {\dot{e}}_{\phi }\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -\frac{a{C}_{\alpha f}}{I}\end{array}\right]\delta +\left[\begin{array}{c}0\\ \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}-v_x\\ 0\\ \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\end{array}\right]{\dot{\theta }}_r\end{array}$$
$$\begin{array}{c}{\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r\end{array}$$

2 LQR原理

对于式16，先暂时不考虑最后一项，那么有
$$\begin{array}{c}{\dot{e}}_{rr}=A{e}_{rr}+Bu\end{array}$$
目的是选择合适的$u$使得$|{\bar{\mathbf{e}}}_{\mathbf{rr}}|$尽可能小，也即式
$$\begin{array}{c}J=w_a{\mathbf{e}}_{\mathbf{rr}}^2+w_b{u}^2\end{array}$$
尽可能小，进一步也即
$$\begin{array}{c}J={\mathbf{e}}_{\mathbf{rr}}^{T}Q{\mathbf{e}}_{\mathbf{rr}}+u^{T}Ru\end{array}$$
尽可能小，其中$Q$和$R$是对角矩阵，问题就变成了在式17的约束下使$J$取最小值。

2.1 连续方程离散化

式17写成一般形式
$$\begin{array}{c}\dot{x}=Ax+Bu\end{array}$$
上式两边积分
$$\begin{array}{c}{\int }_t^{t+dt}\dot{x}(\tau )d\tau ={\int }_t^{t+dt}Ax(\tau )d\tau +{\int }_t^{t+dt}Bu(\tau )d\tau \end{array}$$
得到
$$\begin{array}{c}x(t+dt)-x(t)=Ax(\xi )dt+Bu(\xi )dt\end{array}$$
对$A(\xi )$采用中值欧拉法，对$u(\xi )$采用向前欧拉法（因为$u(t+dt)$未知）得到：
$$\begin{array}{c}x(t+dt)=x(t)+Adt(\frac{x(t+dt)+x(t)}{2})+Bu(t)dt\end{array}$$
$$\begin{array}{c}(I-\frac{Adt}{2})x(t+dt)=(I+\frac{Adt}{2})x(t)+Bu(t)dt\end{array}$$
$$\begin{array}{c}\begin{array}{rl}x(t+dt)& =(I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})x(t)+(I-\frac{Adt}{2}{)}^{-1}Bdtu(t)\\ & \approx (I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})x(t)+Bdtu(t)\end{array}\end{array}$$
$$\begin{array}{c}{x}_{k+1}=\bar{A}{x}_k+\bar{B}{u}_k\end{array}$$

2.2 LQR

问题就是在式26的约束下，求$u$使式
$$\begin{array}{c}J=\sum _{k=1}^{\infty }{x}_k^{T}Q{x}_k+u_k^{T}R{u}_k\end{array}$$
取得最小值。
$u$的形式为
$$\begin{array}{c}u=-Kx\end{array}$$
$$\begin{array}{c}K=(R+{\bar{B}}^{T}P\bar{B}{)}^{-1}{\bar{B}}^{T}P\bar{A}\end{array}$$
其中其$P$就是离散时间$Riccati$方程
$$\begin{array}{c}P=Q+{\bar{A}}^{T}P\bar{A}-{\bar{A}}^{T}P\bar{B}(R+{\bar{B}}^{T}P\bar{B}{)}^{-1}{\bar{B}}^{T}P\bar{A}\end{array}$$
的解。

3 前馈控制与航向误差

对于式16，如果使用上一节的LQR结果（式28、29），
$$\begin{array}{c}{\dot{e}}_{rr}=(A-BK)e_{rr}+C{\dot{\theta }}_r\end{array}$$
无论$K$取何值，${\dot{e}}_{rr}$和$e_{rr}$不可能同时为0，那么$e_{rr}$也就不会为0，系统存在稳态误差
引入前馈控制消除稳态误差
$$\begin{array}{c}u=-Kx+{\delta }_f\end{array}$$

$$\begin{array}{c}{\dot{e}}_{rr}=(A-BK)e_{rr}+B{\delta }_f+C{\dot{\theta }}_r\end{array}$$
系统稳定后，${\dot{e}}_{rr}=0$
$$\begin{array}{c}{e}_{rr}=-(A-BK{)}^{-1}(B{\delta }_f+C{\dot{\theta }}_r)\end{array}$$
选取合适的${\delta }_f$，使$e_{rr}$尽可能接近0。
式34展开后得：
e r r = [ 1 k 1 { δ f − θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b c f + a c r k 3 − a c r ) ] } 0 − θ ˙ r v x ( b + a a + b m v x 2 c α f ) 0 ] \begin{equation} e_{rr}= \begin{bmatrix} \frac{1}{k_1}\{\delta_f-\frac{\dot{\theta}_r}{v_x}[a+b-bk_3-\frac{mv^2_x}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}k_3-\frac{a}{c_r})]\} \\ 0\\ -\frac{\dot{\theta}_r}{v_x}(b+\frac{a}{a+b}\frac{mv^2_x}{c_{\alpha{f}}})\\ 0 \end{bmatrix} \end{equation} err​= ​k1​1​{δf​−vx​θ˙r​​[a+b−bk3​−a+bmvx2​​(cf​b​+cr​a​k3​−cr​a​)]}0−vx​θ˙r​​(b+a+ba​cαf​mvx2​​)0​ ​​​
可知当
$$\begin{array}{c}{\delta }_f=\frac{{\dot{\theta }}_r}{v_x}[a+b-b{k}_3-\frac{m{v}_x^2}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}{k}_3-\frac{a}{c_r})]\end{array}$$
时，$e_d$等于0，其中$k_3$是反馈$K$中的第三个元素。
通过一系列化简，式35的第三个元素可近似等于$-\beta$，即
$$\begin{array}{c}{e}_{\phi }=-\beta \end{array}$$
因为目的是$\theta -{\theta }_r=0$，那么$e_{\phi }$的稳态误差刚好就是$-\beta$。