六、基于MPC的车辆控制及轨迹规划

这篇具有很好参考价值的文章主要介绍了六、基于MPC的车辆控制及轨迹规划。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

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=0k1AjBuk1j
将上式写成矩阵的形式
[ 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 x1x2xN1xN = BABAN1B0BAN2B00B u0u1uN1 + AA2AN 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=0N1(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:N1,从而使代价函数 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+ x1x2xN1xN T Q0000Q0000Q0000P + u0u1uN1 T R000R000R u0u1uN1
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ˉ= BABAN1B0BAN2B00B 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= u0u1uN1 T ˉ = [ A A 2 ⋮ A N ] \bar{T}=\left[\begin{array}{c}A\\A^2\\\vdots\\A^N\end{array}\right] Tˉ= AA2AN 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ˉ= Q0000Q0000Q0000P 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ˉ= R000R000R ,

则代价函数改写为
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=H1FTx0
这样就得到最优控制序列 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= u0u1uN1x1xN ,则系统代价函数就转化为
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 R0000R0000Q0000P 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=0N1(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} xN1,则第 N − 1 N-1 N1个时刻的代价函数为 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) JN1=xN1TQxN1+uN1TRuN1+JN(xN)

由于 x N = A x N − 1 + B u N − 1 x_N=Ax_{N-1}+Bu_{N-1} xN=AxN1+BuN1,则第 N − 1 N-1 N1个时刻的控制量
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} uN1(xN1)=arg min xN1TQxN1+uN1TRuN1+JN(AxN1+BuN1)=arg min xN1T(ATPA+Q)xN1+UN1T(BTPB+R)uN1+2xN1TATPBuN1
求导得, 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)uN1+2BTPAxN1=0

解得 u N − 1 ∗ ( x N − 1 ) = K x N − 1 u_{N-1}^*(x_{N-1})=Kx_{N-1} uN1(xN1)=KxN1,其中 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=APA+QAPB(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,该模型能够很好地描述车辆与参考轨迹之间的关系。

mpc轨迹规划,自动驾驶规划与控制,算法,人工智能

mpc轨迹规划,自动驾驶规划与控制,算法,人工智能

如图所示,选取状态量 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μ=(Rn)ϕ˙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˙=RnR(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˙=1nkvxcosμ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˙=1nkvcosβcosμvsinβsinμ=1nkvcos(β+μ)

横向距离的变化率可以表示为
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=Rvvk1nkvcos(β+μ)=lrvsinβk1nkvcos(β+μ)
其中, 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˙δ˙δ¨ = 1nkvcos(μ+β)vsin(μ+β)lrvsin(β)k1nkvcos(μ+β)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)})} β=tan1(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=wtJtracking+wCJcomfort+wsJsafety
其中

w t w_t wt是轨迹追踪性能的权重;

w c w_c wc是舒适性能的权重;

w s w_s ws是安全性能的权重;

mpc轨迹规划,自动驾驶规划与控制,算法,人工智能

如图所示,对于轨迹追踪性能的代价函数
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+wnn2+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=wjxjx2+waxax2+wayay2+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)+wnn2+wμ(μ+β)2Jcomfort=wjxjx2+waxax2+wayay2+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=0N1Jstage(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)={skskmaxλs,k 0sk+skminλs,k 0,k{0,,N}
其次是速度约束
v − v m a x − λ ≤ 0 v-v_{max}-\lambda\le0 vvmaxλ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λa0,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)={akacomfortλa,soft0akdecelcomfortλa,soft0

6.5、主流MPC求解器及MPC加速

主流 M P C MPC MPC求解器如下:

mpc轨迹规划,自动驾驶规划与控制,算法,人工智能

使用 M P C MPC MPC进行车辆控制时有一个很现实的问题,如何加快求解器的求解速度?

工程中一般有如下几个方法:

(1)简化模型的复杂度,去除影响比较小的状态参数;

(2)减小预测时长;

(3)减少约束;

(4)降低精确度;

(5)使用离线 M P C MPC MPC控制;文章来源地址https://www.toymoban.com/news/detail-583514.html

到了这里,关于六、基于MPC的车辆控制及轨迹规划的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 【轨迹跟踪】模型预测控制MPC无人机轨迹跟踪【含Matlab源码 3958期】

    获取代码方式1: 完整代码已上传我的资源:【轨迹跟踪】基于matlab模型预测控制MPC无人机轨迹跟踪【含Matlab源码 3958期】 点击上面蓝色字体,直接付费下载,即可。 获取代码方式2: 付费专栏Matlab物理应用(初级版) 备注: 点击上面蓝色字体 付费专栏Matlab物理应用(初级

    2024年02月21日
    浏览(48)
  • 【轨迹跟踪】MPC模型无人机轨迹跟踪【含Matlab源码 3500期】

    1 模型预测控制原理 模型预测控制(MPC)的最核心思想就是利用三维的空间模型加上时间构成四维时空模型,然后在这个时空模型的基础上,求解最优控制器。MPC控制器基于一段时间的时空模型,因此得到的控制输出也是系统在未来有限时间步的控制序列。 由于,理论构建的

    2024年02月02日
    浏览(46)
  • MATLAB 模型预测控制(MPC)控制入门 —— 设计并仿真 MPC 控制器

    MATLAB 模型预测控制(MPC) 模型预测控制工具箱™ 提供了用于开发模型预测控制 (MPC) 的函数、应用程序、Simulink® 模块和参考示例。对于线性问题,该工具箱支持设计隐式、显式、自适应和增益调度 MPC。对于非线性问题,您可以实现单级和多级非线性 MPC。该工具箱提供可部

    2024年02月02日
    浏览(53)
  • 自动驾驶路径规划——轨迹规划(详解插值法)

    目录 前言 1. 轨迹规划 1.1 轨迹规划包括以下几个问题: 2. 三次多项式插值 ​​​​​​3.  过路径点的三次多项式插值 4. 用抛物线过渡的线性插值 过路径点的用抛物线过渡的线性插值 5. 高阶多项式插值 声明        这个学期学校开设了相应的课程,同时也在学习古月居

    2024年01月22日
    浏览(44)
  • 自动驾驶轨迹规划之碰撞检测(三)

    欢迎大家关注我的B站: 偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com) 目录 1.基于圆覆盖 2.BVH 3.MATLAB自动驾驶工具箱  4 ROS内置的模型 自动驾驶轨迹规划之碰撞检测(一)-CSDN博客 自动驾驶轨迹规划之碰撞检测(二)-CSDN博客 大家可以

    2024年01月18日
    浏览(42)
  • 自动驾驶轨迹规划之碰撞检测(一)

    欢迎大家关注我的B站: 偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com) 目录 1.碰撞检测的意义 2.安全走廊 3 计算几何 4 AABB与OBB 对于自动驾驶汽车或机器人的路径规划,碰撞检测是其中非常重要的一个模块,因为碰撞检测不仅仍然是路

    2024年01月17日
    浏览(47)
  • 自动驾驶轨迹规划之碰撞检测(二)

    欢迎大家关注我的B站: 偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com) 目录 1.基于凸优化 2.具身足迹 3. ESDF 自动驾驶轨迹规划之碰撞检测(一)-CSDN博客 大家可以先阅读之前的博客  以此为代表的算法则是 OBCA 无论是自车还是障碍物都

    2024年01月17日
    浏览(44)
  • 自动驾驶车辆运动规划方法综述 - 论文阅读

    本文旨在对自己的研究方向做一些记录,方便日后自己回顾。论文里面有关其他方向的讲解读者自行阅读。 参考论文:自动驾驶车辆运动规划方法综述 1 摘要 规划决策模块中的运动规划环节负责生成车辆的 局部运动轨迹 ,决定车辆行驶质量的决定因素 未来关注的重点: (

    2024年01月17日
    浏览(61)
  • 自动驾驶轨迹规划之kinodynamic planning

    欢迎大家关注我的B站: 偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com) 本文PPT来自深蓝学院《移动机器人的运动规划》  目录 1.kinodynamic的背景 2. old-school pipline 3.example kinodynamic是一个合成词,由运动学与动力学组成 这样的一个规划问

    2024年02月19日
    浏览(39)
  • 【OpenAI】Python:基于 Gym-CarRacing 的自动驾驶项目(4) | 车辆控制功能的实现 | 开环控制 | 闭环控制 | 启停式控制 | PID 控制 | Stanley 控制器

       猛戳!跟哥们一起玩蛇啊  👉 《一起玩蛇》🐍  ​ 💭 写在前面: 本篇是关于多伦多大学自动驾驶专业项目的博客。GYM-Box2D CarRacing 是一种在 OpenAI Gym 平台上开发和比较强化学习算法的模拟环境。它是流行的 Box2D 物理引擎的一个版本,经过修改以支持模拟汽车在赛道上

    2024年02月08日
    浏览(48)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包