自动驾驶控制算法-横向控制与流程&代码仿真
本文是学习自动驾驶控制算法第八讲(一）横向控制算法与流程图以及后续两节的学习笔记。
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:5000
vx=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:5000
vx=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
运行代码，效果如下：
原文地址：https://blog.csdn.net/qq_42846675/article/details/144754001
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！