6.1、MPC简介
MPC的基本思想为:在每一个采样时刻,根据获得的当前测量信息,在线求解一个有限时间开环优化问题,并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻,重复上述过程,用新的测量值作为此时预测系统未来动态的初始条件,刷新优化问题并重新求解 。
即MPC算法包括三个步骤:
(1)预测系统未来动态;
(2)求解开环优化问题;
(3)将优化解的第一个元素(或者说第一部分)作用于系统;
这三步是在每个采样时刻重复进行的,且无论采用什么样的模型,每个采样时刻得到的测量值都作为当前时刻预测系统未来动态的初始条件。
6.2、MPC原理
现在有一个线性系统
x
k
+
1
=
A
x
k
+
B
u
k
y
k
=
C
x
k
x_{k+1}=Ax_k+Bu_k\\ y_k=Cx_k
xk+1=Axk+Bukyk=Cxk
则输入量
u
u
u与状态量
x
x
x之间的关系可以表示为
x
k
=
A
k
x
0
+
∑
j
=
0
k
−
1
A
j
B
u
k
−
1
−
j
x_k=A^kx_0+\sum^{k-1}_{j=0}A^jBu_{k-1-j}
xk=Akx0+j=0∑k−1AjBuk−1−j
将上式写成矩阵的形式
[
x
1
x
2
⋮
x
N
−
1
x
N
]
=
[
B
0
…
0
A
B
B
…
0
⋮
⋮
⋱
⋮
A
N
−
1
B
A
N
−
2
B
…
B
]
[
u
0
u
1
⋮
u
N
−
1
]
+
[
A
A
2
⋮
A
N
]
x
0
\left[\begin{array}{c}x_1\\x_2\\\vdots\\x_{N-1}\\x_N\end{array}\right]= \left[\begin{array}{cccc}B&0&\dots&0\\AB&B&\dots&0\\\vdots&\vdots&\ddots&\vdots\\A^{N-1}B&A^{N-2}B&\dots&B\end{array}\right] \left[\begin{array}{c}u_0\\u_1\\\vdots\\u_{N-1}\end{array}\right]+ \left[\begin{array}{c}A\\A^2\\\vdots\\A^N\end{array}\right]x_0
x1x2⋮xN−1xN
=
BAB⋮AN−1B0B⋮AN−2B……⋱…00⋮B
u0u1⋮uN−1
+
AA2⋮AN
x0
则代价函数可以写作
J
=
x
N
T
P
X
N
+
∑
k
=
0
N
−
1
(
x
k
T
Q
x
k
+
u
k
T
R
u
k
)
J=x_N^TPX_N+\sum_{k=0}^{N-1}(x_k^TQx_k+u_k^TRu_k)
J=xNTPXN+k=0∑N−1(xkTQxk+ukTRuk)
其中
P
=
P
T
>
0
Q
=
Q
T
>
0
R
=
R
T
>
0
P=P^T>0\\ Q=Q^T>0\\ R=R^T>0
P=PT>0Q=QT>0R=RT>0
我们的目标是寻找一个最优的控制序列
u
0
:
N
−
1
∗
u^*_{0:N-1}
u0:N−1∗,从而使代价函数
j
j
j最小
J
(
z
,
x
0
)
=
x
0
T
Q
x
0
+
[
x
1
x
2
⋮
x
N
−
1
x
N
]
T
[
Q
0
0
…
0
0
Q
0
…
0
⋮
⋮
⋱
⋮
⋮
0
…
0
Q
0
0
0
…
0
P
]
+
[
u
0
u
1
⋮
u
N
−
1
]
T
[
R
0
…
0
0
R
…
0
⋮
⋮
⋱
⋮
0
…
0
R
]
[
u
0
u
1
⋮
u
N
−
1
]
J(z,x_0)=x_0^TQx_0+\left[\begin{array}{c}x_1\\x_2\\\vdots\\x_{N-1}\\x_N\end{array}\right]^T \left[\begin{array}{ccccc}Q&0&0&\dots&0\\0&Q&0&\dots&0\\\vdots&\vdots&\ddots&\vdots&\vdots\\0&\dots&0&Q&0\\0&0&\dots&0&P\end{array}\right]+ \left[\begin{array}{c}u_0\\u_1\\\vdots\\u_{N-1}\end{array}\right]^T \left[\begin{array}{cccc}R&0&\dots&0\\0&R&\dots&0\\\vdots&\vdots&\ddots&\vdots\\0&\dots&0&R\end{array}\right] \left[\begin{array}{c}u_0\\u_1\\\vdots\\u_{N-1}\end{array}\right]
J(z,x0)=x0TQx0+
x1x2⋮xN−1xN
T
Q0⋮000Q⋮…000⋱0………⋮Q000⋮0P
+
u0u1⋮uN−1
T
R0⋮00R⋮………⋱000⋮R
u0u1⋮uN−1
令
S
ˉ
=
[
B
0
…
0
A
B
B
…
0
⋮
⋮
⋱
⋮
A
N
−
1
B
A
N
−
2
B
…
B
]
\bar{S}=\left[\begin{array}{cccc}B&0&\dots&0\\AB&B&\dots&0\\\vdots&\vdots&\ddots&\vdots\\A^{N-1}B&A^{N-2}B&\dots&B\end{array}\right]
Sˉ=
BAB⋮AN−1B0B⋮AN−2B……⋱…00⋮B
,
z
=
[
u
0
u
1
⋮
u
N
−
1
]
z=\left[\begin{array}{c}u_0\\u_1\\\vdots\\u_{N-1}\end{array}\right]
z=
u0u1⋮uN−1
,
T
ˉ
=
[
A
A
2
⋮
A
N
]
\bar{T}=\left[\begin{array}{c}A\\A^2\\\vdots\\A^N\end{array}\right]
Tˉ=
AA2⋮AN
,
Q
ˉ
=
[
Q
0
0
…
0
0
Q
0
…
0
⋮
⋮
⋱
⋮
⋮
0
…
0
Q
0
0
0
…
0
P
]
\bar{Q}=\left[\begin{array}{ccccc}Q&0&0&\dots&0\\0&Q&0&\dots&0\\\vdots&\vdots&\ddots&\vdots&\vdots\\0&\dots&0&Q&0\\0&0&\dots&0&P\end{array}\right]
Qˉ=
Q0⋮000Q⋮…000⋱0………⋮Q000⋮0P
,
R
ˉ
=
[
R
0
…
0
0
R
…
0
⋮
⋮
⋱
⋮
0
…
0
R
]
\bar{R}=\left[\begin{array}{cccc}R&0&\dots&0\\0&R&\dots&0\\\vdots&\vdots&\ddots&\vdots\\0&\dots&0&R\end{array}\right]
Rˉ=
R0⋮00R⋮………⋱000⋮R
,
则代价函数改写为
J
(
z
,
x
0
)
=
(
S
ˉ
z
+
T
ˉ
x
0
)
T
Q
ˉ
(
S
ˉ
z
+
T
ˉ
x
0
)
+
z
T
R
ˉ
z
+
x
0
T
Q
x
0
=
1
2
z
T
2
(
R
ˉ
+
S
ˉ
T
Q
ˉ
S
ˉ
)
z
+
x
0
T
2
T
ˉ
T
Q
ˉ
S
ˉ
z
+
1
2
x
0
T
2
(
Q
+
T
ˉ
T
Q
ˉ
T
ˉ
)
x
0
=
1
2
z
T
H
z
+
x
0
T
F
z
+
1
2
x
0
T
E
x
0
\begin{aligned} J(z,x_0)&=(\bar{S}z+\bar{T}x_0)^T\bar{Q}(\bar{S}z+\bar{T}x_0)+z^T\bar{R}z+x_0^TQx_0\\ &=\frac{1}{2}z^T2(\bar{R}+\bar{S}^T\bar{Q}\bar{S})z+x_0^T2\bar{T}^T\bar{Q}\bar{S}z+\frac{1}{2}x_0^T2(Q+\bar{T}^T\bar{Q}\bar{T})x_0\\ &=\frac{1}{2}z^THz+x_0^TFz+\frac{1}{2}x_0^TEx_0 \end{aligned}
J(z,x0)=(Sˉz+Tˉx0)TQˉ(Sˉz+Tˉx0)+zTRˉz+x0TQx0=21zT2(Rˉ+SˉTQˉSˉ)z+x0T2TˉTQˉSˉz+21x0T2(Q+TˉTQˉTˉ)x0=21zTHz+x0TFz+21x0TEx0
令
H
=
2
(
R
ˉ
+
S
ˉ
T
Q
ˉ
S
ˉ
)
H=2(\bar{R}+\bar{S}^T\bar{Q}\bar{S})
H=2(Rˉ+SˉTQˉSˉ),
F
=
2
T
ˉ
T
Q
ˉ
S
ˉ
F=2\bar{T}^T\bar{Q}\bar{S}
F=2TˉTQˉSˉ,
E
=
2
(
Q
+
T
ˉ
T
Q
ˉ
T
ˉ
)
E=2(Q+\bar{T}^T\bar{Q}\bar{T})
E=2(Q+TˉTQˉTˉ)
我们的目标是选择一个合适的控制序列 z z z,使得代价函数的值最小。
等式两边同时对
z
z
z求偏导,得
▽
z
J
(
z
,
x
0
)
=
H
z
+
F
T
x
0
=
0
z
∗
=
−
H
−
1
F
T
x
0
\bigtriangledown_zJ(z,x_0)=Hz+F^Tx_0=0\\ z^*=-H^{-1}F^Tx_0
▽zJ(z,x0)=Hz+FTx0=0z∗=−H−1FTx0
这样就得到最优控制序列
z
∗
z^*
z∗,它与初始状态
x
0
x_0
x0呈线性关系。
在这里, z ∗ z^* z∗的计算量主要体现在 H H H矩阵的求逆上,由于 H H H是一个稠密矩阵(非0元素占所有元素比例较大的矩阵),该矩阵的求逆运算计算量很大,如何加速计算呢?
一个比较直观的方法是将稠密矩阵稀疏化,将系统未来时刻的输入和状态拼成一个控制序列
z
=
[
u
0
u
1
⋮
u
N
−
1
x
1
⋮
x
N
]
z=\left[\begin{array}{c}u_0\\u_1\\\vdots\\u_{N-1}\\x_1\\\vdots\\x_{N}\end{array}\right]
z=
u0u1⋮uN−1x1⋮xN
,则系统代价函数就转化为
J
(
z
,
x
0
)
=
x
0
T
Q
x
0
+
z
T
[
R
0
0
…
0
0
R
0
…
0
⋮
⋮
⋱
⋮
⋮
0
…
0
Q
0
0
0
…
0
P
]
z
J(z,x_0)=x_0^TQx_0+z^T\left[\begin{array}{ccccc}R&0&0&\dots&0\\0&R&0&\dots&0\\\vdots&\vdots&\ddots&\vdots&\vdots\\0&\dots&0&Q&0\\0&0&\dots&0&P\end{array}\right]z
J(z,x0)=x0TQx0+zT
R0⋮000R⋮…000⋱0………⋮Q000⋮0P
z
代价函数就变成了一个对角矩阵,
H
H
H矩阵就变成了一个稀疏矩阵,其求逆操作计算量比较小。但是这样引入了新约束,在无约束情况下不会使用。
另一种方法运用了动态规划的思想,动态规划算法与分治法类似,其基本思想也是将待求解问题分解成若干个子问题,先求解子问题,然后从这些子问题的解得到原问题的解。
对于代价函数 J = x N T P X N + ∑ k = 0 N − 1 ( x k T Q x k + u k T R u k ) J=x_N^TPX_N+\sum_{k=0}^{N-1}(x_k^TQx_k+u_k^TRu_k) J=xNTPXN+∑k=0N−1(xkTQxk+ukTRuk)求解最小值的问题,假设我们已知 x k x_k xk,则 u k ∗ u_k^* uk∗是 x K x_K xK到 x N x_N xN这个子问题的最优解。
假设我们已知 x N x_N xN,则第 N N N个时刻的代价函数 J N ∗ ( x N ) = x N T P x N J_N^*(x_N)=x_N^TPx_N JN∗(xN)=xNTPxN;
假设我们已知 x N − 1 x_{N-1} xN−1,则第 N − 1 N-1 N−1个时刻的代价函数为 J N − 1 ∗ = x N − 1 T Q x N − 1 + u N − 1 T R u N − 1 + J N ∗ ( x N ) J_{N-1}^*=x^T_{N-1}Qx_{N-1}+u_{N-1}^TRu_{N-1}+J_N^*(x_N) JN−1∗=xN−1TQxN−1+uN−1TRuN−1+JN∗(xN);
由于
x
N
=
A
x
N
−
1
+
B
u
N
−
1
x_N=Ax_{N-1}+Bu_{N-1}
xN=AxN−1+BuN−1,则第
N
−
1
N-1
N−1个时刻的控制量
u
N
−
1
(
x
N
−
1
)
=
a
r
g
m
i
n
x
N
−
1
T
Q
x
N
−
1
+
u
N
−
1
T
R
u
N
−
1
+
J
N
∗
(
A
x
N
−
1
+
B
u
N
−
1
)
=
a
r
g
m
i
n
x
N
−
1
T
(
A
T
P
A
+
Q
)
x
N
−
1
+
U
N
−
1
T
(
B
T
P
B
+
R
)
u
N
−
1
+
2
x
N
−
1
T
A
T
P
B
u
N
−
1
\begin{aligned} u_{N-1}(x_{N-1})&=arg \ min\ x^T_{N-1}Qx_{N-1}+u^T_{N-1}Ru_{N-1}+J^*_N(Ax_{N-1}+Bu_{N-1})\\ &=arg \ min\ x^T_{N-1}(A^TPA+Q)x_{N-1}+U^T_{N-1}(B^TPB+R)u_{N-1}+2x^T_{N-1}A^TPBu_{N-1} \end{aligned}
uN−1(xN−1)=arg min xN−1TQxN−1+uN−1TRuN−1+JN∗(AxN−1+BuN−1)=arg min xN−1T(ATPA+Q)xN−1+UN−1T(BTPB+R)uN−1+2xN−1TATPBuN−1
求导得,
2
(
B
T
P
B
+
R
)
u
N
−
1
∗
+
2
B
T
P
A
x
N
−
1
=
0
2(B^TPB+R)u^*_{N-1}+2B^TPAx_{N-1}=0
2(BTPB+R)uN−1∗+2BTPAxN−1=0;
解得 u N − 1 ∗ ( x N − 1 ) = K x N − 1 u_{N-1}^*(x_{N-1})=Kx_{N-1} uN−1∗(xN−1)=KxN−1,其中 K = − ( B T P B + R ) − 1 B T P A K=-(B^TPB+R)^{-1}B^TPA K=−(BTPB+R)−1BTPA;
在每次计算中,矩阵 P P P不断更新, P = A ′ P A + Q − A ′ P B ( B T P B + R ) − 1 B T P A P=A^{\prime}PA+Q-A^{\prime}PB(B^TPB+R)^{-1}B^TPA P=A′PA+Q−A′PB(BTPB+R)−1BTPA;
不断重复上述过程,就可求得第0时刻的控制量 u 0 ∗ ( x 0 ) = K x 0 u^*_0(x_0)=Kx_0 u0∗(x0)=Kx0。
6.3、车辆运动模型构建
在本节中介绍另一种车辆模型, C u r v i l i n e a r C o o r d i n a t e s V e h i c l e M o d e l Curvilinear Coordinates Vehicle Model CurvilinearCoordinatesVehicleModel,该模型能够很好地描述车辆与参考轨迹之间的关系。
如图所示,选取状态量 x = [ s n μ v a δ δ ˙ ] x=\left[\begin{array}{c}s\\n\\\mu\\v\\a\\\delta\\\dot{\delta}\end{array}\right] x= snμvaδδ˙ ,其中
s s s代表车辆沿轨迹行驶过的距离;
n n n代表车辆与参考轨迹的横向距离;
μ \mu μ代表轨迹上参考点切线方向与车辆行驶方向的夹角;
v v v代表车辆速度;
a a a代表车辆加速度;
δ \delta δ代表车辆转向角;
δ ˙ \dot{\delta} δ˙则代表车辆转向角的变化率;
选取控制量 u = [ u j e r k u δ ¨ ] u=\left[\begin{array}{c}u_{jerk}\\u_{\ddot{\delta}}\end{array}\right] u=[ujerkuδ¨],其中
u j e r k u_{jerk} ujerk是速度的二阶导数;
u δ ¨ u_{\ddot{\delta}} uδ¨是转向角度的二阶导数;
车辆沿着轨迹运动的速度可以表示为
v
s
=
v
x
cos
μ
−
v
y
sin
μ
=
(
R
−
n
)
ϕ
˙
S
v_s=v_x \cos{\mu}-v_y\sin{\mu}=(R-n)\dot{\phi}_S
vs=vxcosμ−vysinμ=(R−n)ϕ˙S
因为
s
˙
=
R
ϕ
˙
s
\dot{s}=R\dot{\phi}_s
s˙=Rϕ˙s,进而可以推出
s
˙
=
R
R
−
n
(
v
x
cos
μ
−
v
y
sin
μ
)
\dot{s}=\frac{R}{R-n}(v_x\cos{\mu}-v_y\sin{\mu})
s˙=R−nR(vxcosμ−vysinμ)
因为曲率
k
=
1
R
k=\frac{1}{R}
k=R1,继续可以推出
s
˙
=
v
x
cos
μ
−
v
y
sin
β
j
sin
μ
1
−
n
k
\dot{s}=\frac{v_x\cos{\mu}-v_y\sin{\beta}j\sin{\mu}}{1-nk}
s˙=1−nkvxcosμ−vysinβjsinμ
因为
v
x
=
v
cos
β
v_x=v\cos{\beta}
vx=vcosβ,
v
y
=
v
sin
β
v_y=v\sin{\beta}
vy=vsinβ,继续推出
s
˙
=
v
cos
β
cos
μ
−
v
sin
β
sin
μ
1
−
n
k
=
v
cos
(
β
+
μ
)
1
−
n
k
\dot{s}=\frac{v\cos{\beta}\cos{\mu}-v\sin{\beta}\sin{\mu}}{1-nk}=\frac{v\cos({\beta+\mu})}{1-nk}
s˙=1−nkvcosβcosμ−vsinβsinμ=1−nkvcos(β+μ)
横向距离的变化率可以表示为
n
˙
=
v
x
sin
μ
+
v
y
cos
μ
=
v
cos
β
sin
μ
+
v
sin
β
cos
μ
=
v
sin
(
β
+
μ
)
\dot{n}=v_x\sin{\mu}+v_y\cos{\mu}=v\cos{\beta}\sin{\mu}+v\sin{\beta}\cos{\mu}=v\sin{(\beta+\mu)}
n˙=vxsinμ+vycosμ=vcosβsinμ+vsinβcosμ=vsin(β+μ)
同样的,航向偏差的变化率可以表示为
μ
˙
=
r
−
ϕ
˙
s
=
v
R
v
−
k
v
cos
(
β
+
μ
)
1
−
n
k
=
v
sin
β
l
r
−
k
v
cos
(
β
+
μ
)
1
−
n
k
\dot{\mu}=r-\dot{\phi}_s=\frac{v}{R_v}-k\frac{v\cos{(\beta+\mu)}}{1-nk}=\frac{v\sin{\beta}}{l_r}-k\frac{v\cos{(\beta+\mu)}}{1-nk}
μ˙=r−ϕ˙s=Rvv−k1−nkvcos(β+μ)=lrvsinβ−k1−nkvcos(β+μ)
其中,
r
r
r代表车辆自身的转速;
整理以上的推导,得
x
˙
=
[
s
˙
n
˙
μ
˙
v
˙
v
˙
a
˙
δ
˙
δ
¨
]
=
[
v
cos
(
μ
+
β
)
1
−
n
k
v
sin
(
μ
+
β
)
v
l
r
sin
(
β
)
−
k
v
cos
(
μ
+
β
)
1
−
n
k
a
u
j
e
r
k
δ
˙
u
δ
¨
]
\dot{x}=\left[\begin{array}{c}\dot{s}\\\dot{n}\\\dot{\mu}\\\dot{v}\\\dot{v}\\\dot{a}\\\dot{\delta}\\\ddot{\delta}\end{array}\right]= \left[\begin{array}{c} \frac{v\cos{(\mu+\beta)}}{1-nk}\\v\sin{(\mu+\beta)}\\\frac{v}{l_r}\sin{(\beta)}-k\frac{v\cos{(\mu+\beta)}}{1-nk}\\a\\u_{jerk}\\\dot{\delta}\\u_{\ddot{\delta}}\end{array}\right]
x˙=
s˙n˙μ˙v˙v˙a˙δ˙δ¨
=
1−nkvcos(μ+β)vsin(μ+β)lrvsin(β)−k1−nkvcos(μ+β)aujerkδ˙uδ¨
其中,
β
=
tan
−
1
(
l
r
l
r
+
l
f
tan
(
δ
f
)
)
\beta=\tan^{-1}{(\frac{l_r}{l_r+l_f}\tan{(\delta_f)})}
β=tan−1(lr+lflrtan(δf));
6.4、代价函数构建
代价函数主要由三个方面构成,一是保持轨迹跟踪精确性的代价,二是保持行驶中乘员舒适性的代价,三是保持行驶安全性能的代价。即
J
=
w
t
⋅
J
t
r
a
c
k
i
n
g
+
w
C
⋅
J
c
o
m
f
o
r
t
+
w
s
⋅
J
s
a
f
e
t
y
J=w_t\cdot J_{tracking}+w_C\cdot J_{comfort}+w_s \cdot J_{safety}
J=wt⋅Jtracking+wC⋅Jcomfort+ws⋅Jsafety
其中
w t w_t wt是轨迹追踪性能的权重;
w c w_c wc是舒适性能的权重;
w s w_s ws是安全性能的权重;
如图所示,对于轨迹追踪性能的代价函数
J
t
r
a
c
k
i
n
g
=
w
s
˙
(
s
˙
−
s
˙
r
e
f
)
2
+
w
n
⋅
n
2
+
w
μ
(
μ
+
β
)
2
J_{tracking}=w_{\dot{s}}(\dot{s}-\dot{s}_{ref})^2+w_n\cdot n^2+w_{\mu}(\mu+\beta)^2
Jtracking=ws˙(s˙−s˙ref)2+wn⋅n2+wμ(μ+β)2
其中
( s ˙ − s ˙ r e f ) 2 (\dot{s}-\dot{s}_{ref})^2 (s˙−s˙ref)2代表沿着参考轨迹的速度偏差;
n 2 n^2 n2代表侧向位移偏差;
( μ + β ) 2 (\mu+\beta)^2 (μ+β)2代表角度偏差;
而 w s ˙ w_{\dot{s}} ws˙、 w n w_n wn、 w μ w_{\mu} wμ则代表相应的权重;
对于舒适性的代价函数
J
c
o
m
f
o
r
t
=
w
j
x
⋅
j
x
2
+
w
a
x
⋅
a
x
2
+
w
a
y
⋅
a
y
2
+
w
δ
˙
⋅
δ
˙
+
w
δ
¨
⋅
δ
¨
2
J_{comfort}=w_{jx}\cdot j_x^2+w_{a_x}\cdot a_x^2+w_{a_y}\cdot a_y^2+w_{\dot{\delta}}\cdot \dot{\delta}+w_{\ddot{\delta}}\cdot \ddot{\delta}^2
Jcomfort=wjx⋅jx2+wax⋅ax2+way⋅ay2+wδ˙⋅δ˙+wδ¨⋅δ¨2
其中,
j
x
j_x
jx代表纵向速度;
a x a_x ax代表横向加速度;
a y a_y ay代表纵向加速度;
δ ˙ \dot{\delta} δ˙代表转向角的变化率;
δ ¨ \ddot{\delta} δ¨代表转向角的加速度;
而 w j x w_{jx} wjx、 w a x w_{a_{x}} wax、 w a y w_{a_y} way、 w δ ˙ w_{\dot{\delta}} wδ˙、 w δ ¨ w_{\ddot{\delta}} wδ¨则代表相应的权重;
对于安全性能的代价函数,我们需要定义一个约束,在约束以内的行为,看作是安全的行为,而在约束以外的行为,则看作不安全的行为。
约束又可以分为硬约束和软约束,硬约束是指车辆在行驶过程中绝对达不到的某种状态而作出的约束,在优化过程中,这种约束不能违反;而软约束指车辆在行驶过程中可以达到的但是不安全的状态,这类约束在特殊情况下可以违反,但是其相应的代价很大。
在工程中,一般把输入量设置为硬约束,而将状态量设置为软约束。
在这里需要引入一个新的变量——松弛因子 λ k \lambda_k λk,它将系统对约束的违反程度转化为代价函数,从而实现优化求解。
所以,安全性能的代价函数可以表示为
J
s
a
f
e
t
y
=
λ
s
o
f
t
T
E
λ
s
o
f
t
+
H
ˉ
λ
h
a
r
d
J_{safety}=\lambda_{soft}^TE\lambda_{soft}+\bar{H}\lambda_{hard}
Jsafety=λsoftTEλsoft+Hˉλhard
其中,
E
E
E为代表软约束权重的对角矩阵,
E
=
d
i
a
g
(
w
λ
n
,
s
o
f
t
,
w
λ
v
,
s
o
f
t
,
w
λ
a
,
s
o
f
t
)
E=diag\ (w_{\lambda_{n,soft}},w_{\lambda_{v,soft}},w_{\lambda_{a,soft}})
E=diag (wλn,soft,wλv,soft,wλa,soft);
H ˉ \bar{H} Hˉ为代表硬约束权重的行向量, H ˉ = [ w λ n , w λ a , w λ s ] \bar{H}=[w_{\lambda_{n}},w_{\lambda_a},w_{\lambda_s}] Hˉ=[wλn,wλa,wλs]
λ s o f t \lambda_{soft} λsoft为软约束的松弛因子, λ s o f t = [ λ n , s o f t λ v , s o f t λ q , s o f t ] \lambda_{soft}=\left[\begin{array}{c}\lambda_{n,soft}\\\lambda_{v,soft}\\\lambda_{q,soft}\end{array}\right] λsoft= λn,softλv,softλq,soft , λ n , s o f t \lambda_{n,soft} λn,soft代表违反横向偏差软约束的代价, λ v , s o f t \lambda_{v,soft} λv,soft代表违反速度偏差软约束的代价, λ a , s o f t \lambda_{a,soft} λa,soft代表违反加速度偏差软约束的代价;
λ h a r d \lambda_{hard} λhard为硬约束的松弛因子, λ h a r d = [ λ n λ v λ q ] \lambda_{hard}=\left[\begin{array}{c}\lambda_{n}\\\lambda_{v}\\\lambda_{q}\end{array}\right] λhard= λnλvλq , λ n , s o f t \lambda_{n,soft} λn,soft代表违反横向偏差硬约束的代价, λ v , s o f t \lambda_{v,soft} λv,soft代表违反速度偏差硬约束的代价, λ a , s o f t \lambda_{a,soft} λa,soft代表违反加速度偏差硬约束的代价;
以上,我们就得到了三个性能指标的代价函数。
J
t
r
a
c
k
i
n
g
=
w
s
˙
(
s
˙
−
s
˙
r
e
f
)
+
w
n
⋅
n
2
+
w
μ
⋅
(
μ
+
β
)
2
J
c
o
m
f
o
r
t
=
w
j
x
⋅
j
x
2
+
w
a
x
⋅
a
x
2
+
w
a
y
⋅
a
y
2
+
w
δ
˙
⋅
δ
˙
+
w
δ
¨
⋅
δ
¨
2
J
s
a
f
e
t
y
=
λ
s
o
f
t
T
E
λ
s
o
f
t
+
H
ˉ
λ
h
a
r
d
J_{tracking}=w_{\dot{s}}(\dot{s}-\dot{s}_{ref})+w_n\cdot n^2+w_{\mu}\cdot(\mu+\beta)^2\\ J_{comfort}=w_{jx}\cdot j_x^2+w_{a_x}\cdot a_x^2+w_{a_y}\cdot a_y^2+w_{\dot{\delta}}\cdot \dot{\delta}+w_{\ddot{\delta}}\cdot \ddot{\delta}^2\\ J_{safety}=\lambda_{soft}^TE\lambda_{soft}+\bar{H}\lambda_{hard}
Jtracking=ws˙(s˙−s˙ref)+wn⋅n2+wμ⋅(μ+β)2Jcomfort=wjx⋅jx2+wax⋅ax2+way⋅ay2+wδ˙⋅δ˙+wδ¨⋅δ¨2Jsafety=λsoftTEλsoft+Hˉλhard
接着,为了使
M
P
C
MPC
MPC有解,我们还需要对有限时域内的终端状态进行约束。
综上,总的代价函数可以表示为
J
(
x
k
,
U
)
=
∑
k
=
0
N
−
1
J
s
t
a
g
e
(
x
k
,
u
k
,
λ
k
)
+
J
t
e
r
m
i
n
a
l
(
x
N
)
J(x_k,U)=\sum^{N-1}_{k=0}J_{stage}(x_k,u_k,\lambda_k)+J_{terminal}(x_N)
J(xk,U)=k=0∑N−1Jstage(xk,uk,λk)+Jterminal(xN)
6.5、约束构建
在自动驾驶控制问题中,除了要满足输入输出以及系统状态的约束,还将考虑以下约束
c
s
t
a
t
i
o
n
(
x
,
λ
s
)
≤
0
c
v
e
l
(
x
,
λ
v
,
s
o
f
t
)
≤
0
c
t
u
b
e
,
h
a
r
d
(
x
,
λ
n
)
≤
0
c
t
u
b
e
,
s
o
f
t
(
x
,
λ
n
,
s
o
f
t
)
≤
0
c
a
,
h
a
r
d
(
x
,
λ
a
)
≤
0
c
a
,
s
o
f
t
(
x
,
λ
a
,
s
o
f
t
)
≤
0
c
δ
¨
(
x
)
≤
0
c^{station}(x,\lambda_s)\le0\\ c^{vel}(x,\lambda_{v,soft})\le0\\ c^{tube,hard}(x,\lambda_n)\le0\\ c^{tube,soft}(x,\lambda_{n,soft})\le0\\ c^{a,hard}(x,\lambda_a)\le0\\ c^{a,soft}(x,\lambda_{a,soft})\le0\\ c^{\ddot{\delta}}(x)\le0
cstation(x,λs)≤0cvel(x,λv,soft)≤0ctube,hard(x,λn)≤0ctube,soft(x,λn,soft)≤0ca,hard(x,λa)≤0ca,soft(x,λa,soft)≤0cδ¨(x)≤0
其中包括了对安全距离的约束、对车辆速度的软约束、空间上的软硬约束、加速度的软硬约束以及转向角加速度的约束,下面将一一讨论。
首先是纵向安全距离的约束
c
k
s
t
a
t
i
o
n
(
x
k
,
λ
s
)
=
{
s
k
−
s
k
m
a
x
−
λ
s
,
k
≤
0
−
s
k
+
s
k
m
i
n
−
λ
s
,
k
≤
0
,
∀
k
∈
{
0
,
…
,
N
}
c_k^{station}(x_k,\lambda_s)= \begin{cases} s_k-s_k^{max}-\lambda_{s,k}\ \le0\\ -s_k+s_k^{min}-\lambda_{s,k}\ \le0 \end{cases},\qquad \forall k\in\{0,\dots,N\}
ckstation(xk,λs)={sk−skmax−λs,k ≤0−sk+skmin−λs,k ≤0,∀k∈{0,…,N}
其次是速度约束
v
−
v
m
a
x
−
λ
≤
0
v-v_{max}-\lambda\le0
v−vmax−λ≤0
接着是横向空间约束,对于硬约束,有
KaTeX parse error: Undefined control sequence: \forallk at position 350: …d{cases},\quad \̲f̲o̲r̲a̲l̲l̲k̲\in\{0,\dots,N\…
其中,
n
k
n_k
nk是横向偏差,
d
c
a
r
,
l
e
f
t
,
f
r
o
n
t
(
μ
k
)
d_{car,left,front}(\mu_k)
dcar,left,front(μk)是左前方由于转角引起的横向偏差,
d
r
o
a
d
,
l
e
f
t
,
f
r
o
n
t
,
k
d_{road,left,front,k}
droad,left,front,k是车辆左前方偏差极限,
λ
n
,
k
\lambda_{n,k}
λn,k是松弛因子。
接着是加速度的硬约束,有
c
k
a
,
h
a
r
d
(
x
k
,
λ
a
)
=
a
y
2
a
y
,
m
a
x
m
k
2
+
a
x
2
a
x
,
m
a
x
,
k
2
−
1
−
λ
a
≤
0
,
∀
k
∈
{
0
,
…
,
N
}
c_k^{a,hard}(x_k,\lambda_a)=\sqrt{\frac{a_y^2}{a_{y,maxmk}^2}+\frac{a_x^2}{a_{x,max,k}^2}}-1-\lambda_a\le0,\quad\forall k\in\{0,\dots,N\}
cka,hard(xk,λa)=ay,maxmk2ay2+ax,max,k2ax2−1−λa≤0,∀k∈{0,…,N}
对于加速度的软约束,有
c
k
a
,
s
o
f
t
(
x
k
,
λ
a
,
s
o
f
t
)
=
{
a
k
−
a
c
o
m
f
o
r
t
−
λ
a
,
s
o
f
t
≤
0
−
a
k
−
d
e
c
e
l
c
o
m
f
o
r
t
−
λ
a
,
s
o
f
t
≤
0
c_k^{a,soft}(x_k,\lambda_{a,soft})= \begin{cases} a_k-a_{comfort}-\lambda_{a,soft}\le0\\ -a_k-decel_{comfort}-\lambda_{a,soft}\le0 \end{cases}
cka,soft(xk,λa,soft)={ak−acomfort−λa,soft≤0−ak−decelcomfort−λa,soft≤0
6.5、主流MPC求解器及MPC加速
主流 M P C MPC MPC求解器如下:
使用 M P C MPC MPC进行车辆控制时有一个很现实的问题,如何加快求解器的求解速度?
工程中一般有如下几个方法:
(1)简化模型的复杂度,去除影响比较小的状态参数;
(2)减小预测时长;
(3)减少约束;
(4)降低精确度;文章来源:https://www.toymoban.com/news/detail-583514.html
(5)使用离线 M P C MPC MPC控制;文章来源地址https://www.toymoban.com/news/detail-583514.html
到了这里,关于六、基于MPC的车辆控制及轨迹规划的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!