本文是学习自动驾驶控制算法第六讲 前馈控制与航向误差以及前两节的学习笔记。
1 横向误差微分方程
以规划的轨迹作为自然坐标系，计算自车在轨迹上的投影点，进而计算误差：
如图所示，横向误差为 d d d，航向误差为 θ − θ r \theta-\theta_r θ−θr，投影点的速度大小为 s ˙ \dot{s} s˙，注意这里的 θ \theta θ是航向角，与横摆角 φ \varphi φ相差一个侧偏角 β \beta β
θ = φ + β \begin{equation} \theta=\varphi+\beta \end{equation} θ=φ+β
根据之前所介绍的笛卡尔坐标系与自然坐标系的转换关系可知：
d ˙ = v sin ( θ − θ r ) \begin{equation} \dot{d}=v\sin(\theta-\theta_r) \end{equation} d˙=vsin(θ−θr)
s ˙ = v cos ( θ − θ r ) 1 − k r d \begin{equation} \dot{s}=\frac{v\cos(\theta-\theta_r)}{1-k_rd} \end{equation} s˙=1−krdvcos(θ−θr)
这里 k r k_r kr是投影点处的曲率。
结合式1和2
d ˙ = v sin ( φ + β − θ r ) = v sin β cos ( φ − θ ) + v cos β sin ( φ − θ ) \begin{equation} \dot{d}=v\sin(\varphi+\beta-\theta_r)=v\sin{\beta}\cos{(\varphi-\theta)}+v\cos{\beta}\sin{(\varphi-\theta)} \end{equation} d˙=vsin(φ+β−θr)=vsinβcos(φ−θ)+vcosβsin(φ−θ)
φ − θ r \varphi-\theta_r φ−θr为小量，所以上式进一步简化为
d ˙ = v y + v x ( φ − θ r ) \begin{equation} \dot{d}=v_y+v_x(\varphi-\theta_r) \end{equation} d˙=vy+vx(φ−θr)
令
e d = d \begin{equation} e_d=d \end{equation} ed=d
e φ = φ − θ r \begin{equation} e_{\varphi}=\varphi-\theta_{r} \end{equation} eφ=φ−θr
求一阶二阶导数则有
e d ˙ = v x e φ + v y \begin{equation} \dot{e_d}=v_xe_{\varphi}+v_y \end{equation} ed˙=vxeφ+vy
假设 v x v_x vx是常数
v y ˙ = e d ¨ − v x e φ ˙ \begin{equation} \dot{v_y}=\ddot{e_d}-v_x\dot{e_{\varphi}} \end{equation} vy˙=ed¨−vxeφ˙
e ¨ φ = φ ¨ − θ ¨ r ≈ φ ¨ \begin{equation} \ddot{e}_{\varphi}=\ddot{\varphi}-\ddot{\theta}_{r}≈\ddot{\varphi} \end{equation} e¨φ=φ¨−θ¨r≈φ¨
这里 θ ¨ r \ddot{\theta}_r θ¨r约等于0，是因为轨迹通常比较平滑。
综合可得
{ v y = e ˙ d − v x e φ v ˙ y = e ¨ d − v x e ˙ φ φ ˙ = e ˙ φ + θ ˙ r φ ¨ = e ¨ φ \begin{equation} \begin{cases} v_y=\dot{e}_d-v_xe_{\varphi}\\ \dot{v}_y=\ddot{e}_d-v_x\dot{e}_{\varphi} \\ \dot{\varphi}=\dot{e}_{\varphi}+\dot{\theta}_r \\ \ddot{\varphi}=\ddot{e}_{\varphi} \end{cases} \end{equation} ⎩ ⎨ ⎧vy=e˙d−vxeφv˙y=e¨d−vxe˙φφ˙=e˙φ+θ˙rφ¨=e¨φ
由上节的公式：
[ v y ˙ φ ¨ ] = [ C α f + C α r m v x a C α f − b C α r m v x − v x a C α f − b C α r I v x a 2 C α f + b 2 C α r I v x ] [ v y φ ˙ ] + [ − C α f m − a C α f I ] δ \begin{equation} \begin{bmatrix} \dot{v_y} \\ \ddot{\varphi} \end{bmatrix}= \begin{bmatrix} \frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} & \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} & \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} v_y \\ \dot{\varphi} \end{bmatrix}+ \begin{bmatrix} -\frac{C_{\alpha{f}}}{m} \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta \end{equation} [vy˙φ¨]=[mvxCαf+CαrIvxaCαf−bCαrmvxaCαf−bCαr−vxIvxa2Cαf+b2Cαr][vyφ˙]+[−mCαf−IaCαf]δ
结合式11和式12可得
e ¨ d = C α f + C α r m v x e ˙ d + ( − C α f + C α r m ) e φ + a C α f − b C α r m v x e ˙ φ + ( a C α f − b C α r m v x − v x ) θ ˙ r + ( − C α f m ) δ \begin{equation} \ddot{e}_d=\frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} \dot{e}_d+(-\frac{C_{\alpha{f}}+C_{\alpha{r}}}{m})e_{\varphi}+\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}\dot{e}_{\varphi}+(\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x)\dot{\theta}_r+(-\frac{C_{\alpha{f}}}{m})\delta \end{equation} e¨d=mvxCαf+Cαre˙d+(−mCαf+Cαr)eφ+mvxaCαf−bCαre˙φ+(mvxaCαf−bCαr−vx)θ˙r+(−mCαf)δ
e ¨ φ = a C α f − b C α r I v x e ˙ d + ( − a C α f − b C α r I ) e φ + a 2 C α f + b 2 C α r I v x e ˙ φ + ( a 2 C α f + b 2 C α r I v x ) θ ˙ r + ( − a C α f I ) δ \begin{equation} \ddot{e}_{\varphi}=\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} \dot{e}_d+(-\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{I})e_{\varphi}+\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x}\dot{e}_{\varphi}+(\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x})\dot{\theta}_r+(-\frac{aC_{\alpha{f}}}{I})\delta \end{equation} e¨φ=IvxaCαf−bCαre˙d+(−IaCαf−bCαr)eφ+Ivxa2Cαf+b2Cαre˙φ+(Ivxa2Cαf+b2Cαr)θ˙r+(−IaCαf)δ
进而有
[ e ˙ d e ¨ d e ˙ φ e ¨ φ ] = [ 0 1 0 0 0 C α f + C α r m v x − C α f + C α r m a C α f − b C α r m v x 0 0 0 1 0 a C α f − b C α r I v x − a C α f − b C α r I a 2 C α f + b 2 C α r I v x ] [ e d e ˙ d e φ e ˙ φ ] + [ 0 − C α f m 0 − a C α f I ] δ + [ 0 a C α f − b C α r m v x − v x 0 a 2 C α f + b 2 C α r I v x ] θ ˙ r \begin{equation} \begin{bmatrix} \dot{e}_d \\ \ddot{e}_{d} \\ \dot{e}_{\varphi} \\ \ddot{e}_{\varphi} \end{bmatrix}= \begin{bmatrix} 0&1&0&0 \\ 0&\frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} &-\frac{C_{\alpha{f}}+C_{\alpha{r}}}{m}&\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x} \\ 0&0&0&1 \\ 0&\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x}&-\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{I}&\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} e_d \\ \dot{e}_d \\ e_{\varphi} \\ \dot{e}_{\varphi} \end{bmatrix}+ \begin{bmatrix} 0\\ -\frac{C_{\alpha{f}}}{m} \\ 0 \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta+ \begin{bmatrix} 0 \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ 0 \\ \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix}\dot{\theta}_r \end{equation} e˙de¨de˙φe¨φ = 00001mvxCαf+Cαr0IvxaCαf−bCαr0−mCαf+Cαr0−IaCαf−bCαr0mvxaCαf−bCαr1Ivxa2Cαf+b2Cαr ede˙deφe˙φ + 0−mCαf0−IaCαf δ+ 0mvxaCαf−bCαr−vx0Ivxa2Cαf+b2Cαr θ˙r
e ˙ r r = A e r r + B u + C θ ˙ r \begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r \end{equation} e˙rr=Aerr+Bu+Cθ˙r
2 LQR原理
对于式16，先暂时不考虑最后一项，那么有
e ˙ r r = A e r r + B u \begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu \end{equation} e˙rr=Aerr+Bu
目的是选择合适的 u u u使得 ∣ e ˉ r r ∣ |\boldsymbol{\bar{e}}_{\boldsymbol{rr}}| ∣eˉrr∣尽可能小，也即式
J = w a e r r 2 + w b u 2 \begin{equation} J=w_a{\boldsymbol{e}}^2_{\boldsymbol{rr}}+w_bu^2 \end{equation} J=waerr2+wbu2
尽可能小，进一步也即
J = e r r T Q e r r + u T R u \begin{equation} J={\boldsymbol{e}}^T_{\boldsymbol{rr}}Q{\boldsymbol{e}}_{\boldsymbol{rr}}+u^TRu \end{equation} J=errTQerr+uTRu
尽可能小，其中 Q Q Q和 R R R是对角矩阵，问题就变成了在式17的约束下使 J J J取最小值。
2.1 连续方程离散化
式17写成一般形式
x ˙ = A x + B u \begin{equation} \dot{x}=Ax+Bu \end{equation} x˙=Ax+Bu
上式两边积分
∫ t t + d t x ˙ ( τ ) d τ = ∫ t t + d t A x ( τ ) d τ + ∫ t t + d t B u ( τ ) d τ \begin{equation} \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{equation} ∫tt+dtx˙(τ)dτ=∫tt+dtAx(τ)dτ+∫tt+dtBu(τ)dτ
得到
x ( t + d t ) − x ( t ) = A x ( ξ ) d t + B u ( ξ ) d t \begin{equation} x(t+dt)-x(t)=Ax(\xi)dt+Bu(\xi)dt \end{equation} x(t+dt)−x(t)=Ax(ξ)dt+Bu(ξ)dt
对 A ( ξ ) A(\xi) A(ξ)采用中值欧拉法，对 u ( ξ ) u(\xi) u(ξ)采用向前欧拉法（因为 u ( t + d t ) u(t+dt) u(t+dt)未知）得到：
x ( t + d t ) = x ( t ) + A d t ( x ( t + d t ) + x ( t ) 2 ) + B u ( t ) d t \begin{equation} x(t+dt)=x(t)+Adt(\frac{x(t+dt)+x(t)}{2})+Bu(t)dt \end{equation} x(t+dt)=x(t)+Adt(2x(t+dt)+x(t))+Bu(t)dt
( I − A d t 2 ) x ( t + d t ) = ( I + A d t 2 ) x ( t ) + B u ( t ) d t \begin{equation} (I-\frac{Adt}{2})x(t+dt)=(I+\frac{Adt}{2})x(t)+Bu(t)dt \end{equation} (I−2Adt)x(t+dt)=(I+2Adt)x(t)+Bu(t)dt
x ( t + d t ) = ( I − A d t 2 ) − 1 ( I + A d t 2 ) x ( t ) + ( I − A d t 2 ) − 1 B d t u ( t ) ≈ ( I − A d t 2 ) − 1 ( I + A d t 2 ) x ( t ) + B d t u ( t ) \begin{equation} \begin{split} x(t+dt) &= (I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+(I-\frac{Adt}{2})^{-1}Bdtu(t) \\ &≈(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+Bdtu(t) \end{split} \end{equation} x(t+dt)=(I−2Adt)−1(I+2Adt)x(t)+(I−2Adt)−1Bdtu(t)≈(I−2Adt)−1(I+2Adt)x(t)+Bdtu(t)
x k + 1 = A ˉ x k + B ˉ u k \begin{equation} x_{k+1}=\bar{A}x_k+\bar{B}{u_k} \end{equation} xk+1=Aˉxk+Bˉuk
2.2 LQR
问题就是在式26的约束下，求 u u u使式
J = ∑ k = 1 ∞ x k T Q x k + u k T R u k \begin{equation} J=\sum_{k=1}^\infty{x^T_{k}Qx_k}+u^T_kRu_k \end{equation} J=k=1∑∞xkTQxk+ukTRuk
取得最小值。
u u u的形式为
u = − K x \begin{equation} u=-Kx \end{equation} u=−Kx
K = ( R + B ˉ T P B ˉ ) − 1 B ˉ T P A ˉ \begin{equation} K=(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation} K=(R+BˉTPBˉ)−1BˉTPAˉ
其中其 P P P就是离散时间 R i c c a t i Riccati Riccati方程
P = Q + A ˉ T P A ˉ − A ˉ T P B ˉ ( R + B ˉ T P B ˉ ) − 1 B ˉ T P A ˉ \begin{equation} P=Q+\bar{A}^TP\bar{A}-\bar{A}^TP\bar{B}(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation} P=Q+AˉTPAˉ−AˉTPBˉ(R+BˉTPBˉ)−1BˉTPAˉ
的解。
3 前馈控制与航向误差
对于式16，如果使用上一节的LQR结果（式28、29），
e ˙ r r = ( A − B K ) e r r + C θ ˙ r \begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+C\dot{\theta}_r \end{equation} e˙rr=(A−BK)err+Cθ˙r
无论 K K K取何值， e ˙ r r \dot{e}_{rr} e˙rr和 e r r {e}_{rr} err不可能同时为0，那么 e r r {e}_{rr} err也就不会为0，系统存在稳态误差
引入前馈控制消除稳态误差
u = − K x + δ f \begin{equation} u=-Kx+\delta_f \end{equation} u=−Kx+δf
e ˙ r r = ( A − B K ) e r r + B δ f + C θ ˙ r \begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+B\delta_f+C\dot{\theta}_r \end{equation} e˙rr=(A−BK)err+Bδf+Cθ˙r
系统稳定后， e ˙ r r = 0 \dot{e}_{rr}=0 e˙rr=0
e r r = − ( A − B K ) − 1 ( B δ f + C θ ˙ r ) \begin{equation} e_{rr}=-(A-BK)^{-1}(B\delta_f+C\dot{\theta}_r) \end{equation} err=−(A−BK)−1(Bδf+Cθ˙r)
选取合适的 δ f \delta_f δf，使 e r r {e}_{rr} err尽可能接近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= k11{δf−vxθ˙r[a+b−bk3−a+bmvx2(cfb+crak3−cra)]}0−vxθ˙r(b+a+bacαfmvx2)0
可知当
δ 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 ) ] \begin{equation} \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})] \end{equation} δf=vxθ˙r[a+b−bk3−a+bmvx2(cfb+crak3−cra)]
时， e d {e}_{d} ed等于0，其中 k 3 k_3 k3是反馈 K K K中的第三个元素。
通过一系列化简，式35的第三个元素可近似等于 − β -\beta −β，即
e φ = − β \begin{equation} e_{\varphi}=-\beta \end{equation} eφ=−β
因为目的是 θ − θ r = 0 \theta-\theta_r=0 θ−θr=0，那么 e φ {e}_{\varphi} eφ的稳态误差刚好就是 − β -\beta −β。
 windows Core Audio API及声音的播放)
