本文是学习自动驾驶控制算法第八讲(一）横向控制算法与流程图以及后续两节的学习笔记。
1 1. 算法流程图
2 A，B计算模块
A = [ 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 ] \begin{equation} A=\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} \end{equation} A= 00001mvxCαf+Cαr0IvxaCαf−bCαr0−mCαf+Cαr0−IaCαf−bCαr0mvxaCαf−bCαr1Ivxa2Cαf+b2Cαr
B = [ 0 − C α f m 0 − a C α f I ] \begin{equation} B=\begin{bmatrix} 0\\ -\frac{C_{\alpha{f}}}{m} \\ 0 \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix} \end{equation} B= 0−mCαf0−IaCαf
注意需要考虑 v x = 0 v_x=0 vx=0时的奇异性。
3 LQR计算模块
整车参数可近似认为不变，但是车辆急加速或急减速时前后承载力变化，轮胎侧偏刚度会变化，高速过弯时（比如 a y ＞ 0.4 g a_y＞0.4g ay＞0.4g时），车辆左右不对称，自行车模型不适合。
在整车参数可近似认为不变的假设下， A ( A ˉ ) , B ( B ˉ ) A(\bar{A}),B(\bar{B}) A(Aˉ),B(Bˉ)只与 v x v_x vx有关，那么由
K = l q r ( A ˉ , B ˉ , Q , R ) \begin{equation} K=lqr(\bar{A},\bar{B},Q,R) \end{equation} K=lqr(Aˉ,Bˉ,Q,R)
可知每个 v x v_x vx对应一个 K K K，这样可以提前计算好 v x v_x vx和 K K K的查找表，不用再去解Riccati方程。 v x v_x vx以 0.01 m / s 0.01m/s 0.01m/s为间隔，算到 50 m / s 50m/s 50m/s。
for i=1:5000vx=0.01*i;A=[0,1,0,0;0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);0,0,0,1;0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];B=[0;-cf/m;0;-a*cf/Iz];Q=1*eye(4);R=10;k(i,:)=lqr(A,B,Q,R);
end
这里车速最小是 0.01 m / s 0.01m/s 0.01m/s，后续计算车速为0时令 K = 0 K=0 K=0，另外这里的 A A A和 B B B是连续状态方程中的表达式，使用matlab中lqr函数计算 K K K值。
之前的章节中，计算了离散的状态方程
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
这里试一下用 A ˉ \bar{A} Aˉ和 B ˉ \bar{B} Bˉ来计算 K K K，使用matlab中的dlqr，取 d t = 0.1 dt=0.1 dt=0.1
for i=1:5000vx=0.01*i;A=[0,1,0,0;0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);0,0,0,1;0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];B=[0;-cf/m;0;-a*cf/Iz];A=inv((eye(4)-dt/2*A))*(eye(4)+dt/2*A);B=dt*B;Q=1*eye(4);R=10;k(i,:)=dlqr(A,B,Q,R);
end
两种方式计算得到的控制量 u u u的结果对比如下：
4 预测模块
前面计算的误差都是相对于自车当前位置的投影点来做计算的，正常人开车都会有一定的预见性，会朝着前方的某个目标点开，这里就假设自车在预测时间 t s t_s ts内匀速行驶
x p r e d = x + v t s cos θ = x + v t s cos ( β + φ ) = x + v x t s cos φ − v y t s sin φ \begin{equation} x_{pred}=x+vt_s\cos{\theta}=x+vt_s\cos{(\beta+\varphi)}=x+v_xt_s\cos{\varphi}-v_yt_s\sin{\varphi} \end{equation} xpred=x+vtscosθ=x+vtscos(β+φ)=x+vxtscosφ−vytssinφ
y p r e d = y + v t s sin θ = x + v t s sin ( β + φ ) = x + v x t s cos φ + v y t s sin φ \begin{equation} y_{pred}=y+vt_s\sin{\theta}=x+vt_s\sin{(\beta+\varphi)}=x+v_xt_s\cos{\varphi}+v_yt_s\sin{\varphi} \end{equation} ypred=y+vtssinθ=x+vtssin(β+φ)=x+vxtscosφ+vytssinφ
φ p r e d = φ + φ ˙ t s \begin{equation} \varphi_{pred}=\varphi+\dot{\varphi}t_s \end{equation} φpred=φ+φ˙ts
v x p r e d = v x \begin{equation} v_{xpred}=v_x \end{equation} vxpred=vx
v y p r e d = v y \begin{equation} v_{ypred}=v_y \end{equation} vypred=vy
φ ˙ p r e d = φ \begin{equation} \dot{\varphi}_{pred}=\varphi \end{equation} φ˙pred=φ
5 误差和曲率计算模块
注意角度的多值性，代码实现中使用
e φ = sin ( φ − θ r ) \begin{equation} e_{\varphi}=\sin(\varphi-\theta_r) \end{equation} eφ=sin(φ−θr)
替代
e φ = φ − θ r \begin{equation} e_{\varphi}=\varphi-\theta_r \end{equation} eφ=φ−θr
6 前馈控制计算模块&控制计算模块
实现很简单，把前面计算的结果按公式算一下就好
function forword_angle = fcn(vx,a,b,m,cf,cr,k,kr)forword_angle=kr*(a+b-b*k(3)-(m*vx*vx/(a+b))*((b/cf)+(a/cr)*k(3)-(a/cr)));
end
function angle = fcn(k,err,forword_angle)angle=-k*err+forword_angle;
end
运行代码，效果如下：
