【MATLAB】无人驾驶车辆的模型预测控制技术(精简讲解和代码)【运动学轨迹规划】

文章目录

  • <font color=#19C>0.友情链接
  • <font color=#19C>1.引言
  • <font color=#19C>2.预测模型
  • <font color=#19C>3.滚动优化
    • <font color=#08CF>3.1.线性化
    • 3.2.UrU_rUr的求取
    • <font color=#08CF>3.3.离散化与序列化
    • <font color=#08CF>3.4.实现增量控制
  • <font color=#19C>4.仿真示例

0.友情链接

  • B站链接1-北京理工大学无人驾驶技术课程
  • B站链接2-MATLAB实现模型预测控制
  • B站链接3-北京理工大学的学生讲解无人驾驶车辆的模型预测控制
  • B站链接4-英国谢菲尔德大学模型预测控制网课(全英文)
  • IEEE论文参考:Model Predictive Control for Visual Servo Steering of Nonholonomic Mobile Robots(非完整移动机器人视觉伺服转向的模型预测控制)
  • 书籍参考:国之重器出版社《无人驾驶车辆模型预测控制(第二版)》
  • 书籍配套代码下载链接
  • 本文完整配套代码链接
  • 阿克曼转向原理介绍

1.引言

\qquadMATLAB自身是带预测控制模块的,但是看了一些MATLAB的预测模块使用教程视频,仍然不如自己书写代码原理清晰,而且MATLAB的预测控制是基于状态方程的滑模控制,并不包含所有预测控制的种类,使用范围有限。因此我想按照预测控制的原理,逐步为大家讲解预测控制算法及MATLAB代码的书写步骤。
\qquadSimulink自身也携带一个关于无人驾驶的模块Car_sim,但是Car_sim使用的模型比一般的模型更复杂,因此本人参考《无人驾驶车辆的模型预测控制》中的
\qquad预测控制都包含三个主要步骤——预测模型、滚动优化和反馈矫正。按照模型的不同,可以分为动态矩阵算法、广义极点配置、基于状态方程的滑模控制。现代控制理论中,以状态方程为主要模型,本文也以其为例建立模型,其他的模型原理与其类似,只是求解方法有所差异。
\qquad为了方便大家理解,本人不直接列出所有代码,而是按照讲解和代码对照的方式,逐步展现每个环节的代码,最终展现全部代码和效果。那我们就从第一步开始吧。

2.预测模型

\qquad建立预测模型的目的在于获取输入量(控制量)和输出量(以及其他干扰项)的映射关系。在本例(汽车变道)示例中,控制量是前轮角度ϕ\phiϕ和汽车速度大小vvv,而输出量则是汽车的质心与待变道直线的距离。控制的目的是让汽车的质心尽快接近待变道直线,并维持其在该直线上运动。
\qquad我们的思路已经很清晰了,但是别着急,我们似乎忽略了什么。例如汽车的速度不能是无穷大,前轮的角度也是有限制的。我们取车辆的后轴中心速度大小限制为[vmin,vmax][v_{min},v_{max}][vmin,vmax],前轮角度绝对值限幅为ϕm\phi_mϕm。前轮转动的这个角度,又称为车辆的横摆角。变道的情况如下图所示:
汽车变道示意图
\qquad汽车是前轮驱动还是后轮驱动直接决定了控制量是前轮的速度还是后轮速度,当然前后轮速度是可以通过后轴中心速度和车辆横摆角转换得到的。因此设小汽车后轴速度为vdv_dvd,车辆横摆角为ϕ\phiϕ,同时设车长为lll。以上图左下角为原点建立坐标系。(不知道阿克曼转向原理的可以看一下友情链接的介绍,这里用的是简化的两轮模型)
在这里插入图片描述

\qquad黑色虚线代表了汽车朝向,而红色虚线则是前轮的朝向。二者夹角为横摆角ϕ\phiϕ,以逆时针为正方向。汽车可以看成是二维平面的刚体,因此只需要三个参数就可以确定其状态。我们选取它质心的横纵坐标x,yx,yx,y加汽车朝向与x轴逆时针方向的夹角θ\thetaθ作为状态变量。(在一篇IEEE的文章上看到选取极坐标作为状态变量的方法,也是可行的,但对于变道这种单移线运动来说,直角坐标更加方便)。采用SSS表示状态向量,UUU表示控制向量。
\qquad汽车的后轮速度大小vdv_dvd和汽车的朝向θ\thetaθ决定了一小段时间内汽车的位移(δx,δy)(\delta x,\delta y)(δx,δy),而前轮的转向角ϕ\phiϕ则决定了一小段时间汽车朝向的变化量δθ\delta \thetaδθ。状态方程列写如下:
S′=[x′y′θ′]=[vdcosθvdsinθvdtanϕ/l]=f(S,U)S'= \begin{bmatrix} x' \\[1ex] y' \\[1ex] \theta'\\[1ex] \end{bmatrix} = \begin{bmatrix} v_d cos\theta \\ v_d sin \theta \\ v_d tan\phi/l \end{bmatrix}= f(S,U) S=xyθ=vdcosθvdsinθvdtanϕ/l=f(S,U)
\qquad模拟阿克曼转向的车辆的MATLAB代码如下:

function [X,Y,Th] = car_run(X,Y,Th,L,dt,vd,phi)X = X + vd*cos(Th)*dt;Y = Y + vd*sin(Th)*dt;Th = Th + vd*tan(phi)/L*dt;
end

X和Y为车辆后轴中心的位置信息,Th为车辆的朝向,L为车长,dt为仿真采样间隔,vd为车辆后轴速度,phi为车辆横摆角。

3.滚动优化

3.1.线性化

\qquad滚动优化的目的是根据预测模型和控制目的,建立目标函数和约束条件,并在约束条件内,求得目标函数的最优解(即期望控制量),从而达到预测控制的目的。
\qquad求目标函数极小值的方法有很多,智能优化算法依赖于蒙特卡洛模拟,数值优化算法依赖于函数的导数。前者比后者的实用性更大,但是速度更慢。汽车行驶中的在线规划对运算速度的要求高,因此普遍采用后者进行求函数极小值的操作。
\qquad二次规划是数值优化算法中一类特殊的优化问题,即特指目标函数为二次型12xTHx+cTx\frac{1}{2}x^THx+c^Tx21xTHx+cTx,约束条件为线性约束的(线性不等式约束+线性灯等式约束)的有约束优化问题。将预测模型的目标函数转换为二次规划问题的模型预测控制(MPC)又称为线性预测控制(LPC),其运算速度是MPC中最快的。本文将以无人驾驶领域为例,介绍模型的线性化过程。
\qquad汽车在变道时,给定期望的轨迹(xr,yr)(x_r,y_r)(xr,yr),并通过控制变量使汽车在时域上经过的路径逼近期望轨迹。当然,我们期望汽车的朝向θ\thetaθ最终逼近于0(即直线行驶),期望的状态量使用SrS_rSr表示,则我们的目的是使∣S−Sr∣|S-S_r|SSr(S−Sr)2/2(S-S_r)^2/2(SSr)2/2最小。当期望状态向量与实际状态向量相差较小且期望控制量与实际控制量相差较小时,可采用在SrS_rSr处的导数dy/dxdy/dxdy/dx来代替变化率Δy/Δx\Delta y/ \Delta xΔy/Δx
S′−Sr′=f(S,U)−f(Sr,Ur)≈∂f∂S∣Sr⋅(S−Sr)+∂f∂U∣Ur⋅(U−Ur)=A⋅(S−Sr)+B⋅(U−Ur)\begin{array}{l} S'-S'_r=f(S,U)-f(S_r,U_r)\\[2ex] ≈\frac {\partial f}{\partial S}|_{S_r}\cdot(S-S_r)+\frac {\partial f}{\partial U}|_{U_r}\cdot(U-U_r)\\[2ex] =A\cdot(S-S_r)+B\cdot(U-U_r) \end{array} SSr=f(S,U)f(Sr,Ur)SfSr(SSr)+UfUr(UUr)=A(SSr)+B(UUr)
其中
A=∂f∂S∣Sr=[∂f1∂S1∂f1∂S2∂f1∂S3∂f2∂S1∂f2∂S2∂f2∂S3∂f3∂S1∂f3∂S2∂f3∂S3]=[10−vdr∗sin(θr)01vdr∗cos(θr)001]B=∂f∂U∣Ur=[∂f1∂U1∂f1∂U2∂f2∂U1∂f2∂U2∂f3∂U1∂f3∂U2]=[cos(θr)0sin(θr)0tan(ϕr)/lvdr∗sec2(ϕr)/l]A=\frac {\partial f}{\partial S}|_{S_r}= \begin{bmatrix} \frac{\partial f_1}{\partial S_1} & \frac{\partial f_1}{\partial S_2} & \frac{\partial f_1}{\partial S_3}\\[2ex] \frac{\partial f_2}{\partial S_1} & \frac{\partial f_2}{\partial S_2} & \frac{\partial f_2}{\partial S_3}\\[2ex] \frac{\partial f_3}{\partial S_1} & \frac{\partial f_3}{\partial S_2} & \frac{\partial f_3}{\partial S_3}\\[2ex] \end{bmatrix}= \begin{bmatrix} 1 & 0 & -v_{dr}*sin(\theta_r) \\ 0 & 1 & v_{dr}*cos(\theta_r)\\ 0 & 0 & 1\\ \end{bmatrix} \\ B=\frac {\partial f}{\partial U}|_{U_r}= \begin{bmatrix} \frac{\partial f_1}{\partial U_1} & \frac{\partial f_1}{\partial U_2}\\[2ex] \frac{\partial f_2}{\partial U_1} & \frac{\partial f_2}{\partial U_2}\\[2ex] \frac{\partial f_3}{\partial U_1} & \frac{\partial f_3}{\partial U_2}\\[2ex] \end{bmatrix}= \begin{bmatrix} cos(\theta_r) & 0 \\[1ex] sin(\theta_r) & 0 \\[1ex] tan(\phi_r)/l & v_{dr}*sec^2(\phi_r)/l \\[1ex] \end{bmatrix} A=SfSr=S1f1S1f2S1f3S2f1S2f2S2f3S3f1S3f2S3f3=100010vdrsin(θr)vdrcos(θr)1B=UfUr=U1f1U1f2U1f3U2f1U2f2U2f3=cos(θr)sin(θr)tan(ϕr)/l00vdrsec2(ϕr)/l
S′~=S′−Sr′,S~=S−Sr,U~=U−Ur\widetilde{S'}=S'-S'_r,\widetilde{S}=S-S_r,\widetilde{U}=U-U_rS=SSr,S=SSr,U=UUr
则线性化后的模型为
S′~=AS~+BU~\widetilde{S'}=A\widetilde{S}+B\widetilde{U} S=AS+BU
即我们所熟知的线性状态方程。我们的目标是使得汽车按照既定的轨迹形式,因此希望实际状态逼近于期望状态,即∣∣S~∣∣→0||\widetilde{S}||\rightarrow0S0,因此输出方程即为
Y=S~Y=\widetilde{S}Y=S

3.2.UrU_rUr的求取

\qquad期望状态向量SrS_rSr是容易理解的,无人驾驶车辆需要提前规划形式的路径(xr,yr)(x_r,y_r)(xr,yr),有时还对车辆的朝向有所要求(θr\theta_rθr),那么期望控制量UrU_rUr又是怎么得出的呢?其实很简单,在没有约束条件的情况下,根据状态方程即可得到UrU_rUr的取值。根据状态方程我们可以得知:
{xr′=vdrcos(θr)yr′=vdrsin(θr)θr′=vdrtanϕr/l⇒{vdr=(xr′)2+(yr′)2ϕr=atan(l⋅θr′/vdr)\begin{cases} x'_r=v_{dr}cos(\theta_r)\\ y'_r=v_{dr}sin(\theta_r)\\ \theta'_r=v_{dr}tan\phi_r /l\\ \end{cases} \Rightarrow \begin{cases} v_{dr}=\sqrt{(x'_r)^2+(y'_r)^2}\\ \phi_r=atan(l\cdot \theta'_r/v_{dr}) \end{cases} xr=vdrcos(θr)yr=vdrsin(θr)θr=vdrtanϕr/l{vdr=(xr)2+(yr)2ϕr=atan(lθr/vdr)
\qquad在实际系统中,若S=(xr,yr,θr)S=(x_r,y_r,\theta_r)S=(xr,yr,θr)是离散取值的,可以借助三次样条插值的方法补全需要的值,并用差分替代微分求取状态量的导数。样条差值的方法在此不再赘述,仿真时建议使用MATLAB的pchip函数。
本部分对应的MATLAB代码如下

X_r = [0,1,2,3,4,5,6]*3;  % X的期望坐标
Y_r = [3,3,2,1,0,0,0];  % Y的期望坐标
L=2.5; % 车身长度
v0 = 10; % 期望速度
T = 20e-3; % 控制周期20ms
dt = 1e-3; % 仿真周期1ms
Xr = linspace(X_r(1),X_r(numel(X_r)),(X_r(numel(X_r))-X_r(1))/(T*v0));  % 横坐标期望值的插值序列
Yr = pchip(X_r,Y_r,Xr);  % 通过三次样条插值算出纵坐标坐标插值序列
n = numel(Yr);  % 预测模型中所有规划点
FYr = Yr(2:n);
dYr = zeros(1,n);
dYr(1:n-1) = FYr - Yr(1:n-1);
dYr(n) = dYr(n-1);%保证导数连续
FXr = Xr(2:n);
dXr = zeros(1,n);
dXr(1:n-1) = FXr - Xr(1:n-1);
dXr(n) = dXr(n-1);
Thr = atan(dYr./dXr);  % 车朝向角的期望值
FThr = Thr(2:n);
dThr = zeros(1,n);
dThr(1:n-1) = FThr-Thr(1:n-1);
dThr(n)=dThr(n-1);
vdr = sqrt(dXr.^2+dYr.^2)/T;  % 车后轮的期望速度
Phr = atan(L./vdr.*dThr/T);  % 车前轮的偏转角

这里以变道为例,绘制期望轨迹(Xr,Yr)(X_r,Y_r)(Xr,Yr)如下图
在这里插入图片描述
\qquad可以发现,车辆从上道(y=3)逐步变化至下道(y=0)。样条插值后的轨迹并不是折线,但由于不一定满足车辆的物理约束,因此不能用期望控制量作为实际控制量进行控制。

3.3.离散化与序列化

\qquad预测控制依赖于计算机,计算机的控制信号是离散的,因此我们还需要把线性化后的状态方程离散化。假设控制周期为TTT,则离散化后的状态方程为:
S~(k+1)−S~(k)T=AkS~(k)+BkU~(k)⇒S~(k+1)=(AkT+I)S~(k)+BkTU~(k)⇒S~(k+1)=Ak∗S~(k)+Bk∗U~(k)\begin{array}{l} \frac{\widetilde{S}(k+1)-\widetilde{S}(k)}{T}=A_k\widetilde{S}(k)+B_k\widetilde{U}(k) \\[2ex] \Rightarrow \widetilde{S}(k+1)=(A_kT+I)\widetilde{S}(k)+B_kT\widetilde{U}(k) \\ \Rightarrow \widetilde{S}(k+1)=A^*_k\widetilde{S}(k)+B^*_k\widetilde{U}(k)\\ \end{array} TS(k+1)S(k)=AkS(k)+BkU(k)S(k+1)=(AkT+I)S(k)+BkTU(k)S(k+1)=AkS(k)+BkU(k)
\qquad接着我们需要选择接下来需要预测的未来若干个采样时刻的状态量的个数和控制量个数,即预测时域NNN和控制时域MMMM≤NM≤NMN)。我们采取最简单的情形M=NM=NM=N讲解。
\qquad首先列写未来NNN个时刻的离散状态方程
{S~(k+1)=Ak∗S~(k)+Bk∗U~(k)S~(k+2)=Ak+1∗S~(k+1)+Bk+1∗U~(k+1)⋯S~(k+N)=Ak+N−1∗S~(k+N−1)+Bk+N−1∗U~(k+N−1)⇒S~(k+i)=AAiS~(k)+AB1U~(k)+AB2U~(k+1)+...+ABiU~(k+i−1)\begin{cases} \widetilde{S}(k+1)=A^*_k\widetilde{S}(k)+B^*_k\widetilde{U}(k)\\ \widetilde{S}(k+2)=A^*_{k+1}\widetilde{S}(k+1)+B^*_{k+1}\widetilde{U}(k+1)\\ \qquad \qquad \cdots \\ \widetilde{S}(k+N)=A^*_{k+N-1}\widetilde{S}(k+N-1)+B^*_{k+N-1}\widetilde{U}(k+N-1)\\ \end{cases}\\ \Rightarrow \widetilde{S}(k+i)=AA_i\widetilde{S}(k)+AB_1\widetilde{U}(k)+AB_2\widetilde{U}(k+1)+...+AB_i\widetilde{U}(k+i-1) S(k+1)=AkS(k)+BkU(k)S(k+2)=Ak+1S(k+1)+Bk+1U(k+1)S(k+N)=Ak+N1S(k+N1)+Bk+N1U(k+N1)S(k+i)=AAiS(k)+AB1U(k)+AB2U(k+1)+...+ABiU(k+i1)
其中
{AAi=Ak+i−1∗Ak+i−2∗...Ak∗AB1(i)=Ak+i−1∗Ak+i−2∗...Ak+1∗Bk∗AB2(i)=Ak+i−1∗Ak+i−2∗...Ak+2∗Bk+1∗⋯ABi−1(i)=Ak+i−1∗Bk+i−2∗ABi(i)=Bk+i−1∗\begin{cases} AA_i=A^*_{k+i-1}A^*_{k+i-2}...A^*_{k} \\ AB_1^{(i)}=A^*_{k+i-1}A^*_{k+i-2}...A^*_{k+1}B^*_k \\ AB_2^{(i)}=A^*_{k+i-1}A^*_{k+i-2}...A^*_{k+2}B^*_{k+1} \\ \qquad \cdots \\ AB_{i-1}^{(i)}=A^*_{k+i-1}B^*_{k+i-2} \\ AB_{i}^{(i)}=B^*_{k+i-1} \\ \end{cases} AAi=Ak+i1Ak+i2...AkAB1(i)=Ak+i1Ak+i2...Ak+1BkAB2(i)=Ak+i1Ak+i2...Ak+2Bk+1ABi1(i)=Ak+i1Bk+i2ABi(i)=Bk+i1
\qquad已知量为S~(k),Ak∗,Bk∗\widetilde{S}(k),A^*_k,B^*_kS(k),Ak,Bk,后两者是因为期望状态量和期望控制量已知因此也已知,但由于期望状态量是一个时间序列,因此矩阵Ak∗,Bk∗A^*_k,B^*_kAk,Bk也是一个时间序列。输入为U~(k),U~(k+1),...,U~(k+N)\widetilde{U}(k),\widetilde{U}(k+1),...,\widetilde{U}(k+N)U(k),U(k+1),...,U(k+N),输出为S~(k+1),S~(k+2),....,S~(k+N)\widetilde{S}(k+1), \widetilde{S}(k+2),...., \widetilde{S}(k+N)S(k+1),S(k+2),....,S(k+N),记
S~^=[S~(k+1),S~(k+2),....,S~(k+N)]TU~^=[U~(k),U~(k+1),...,U~(k+N−1)]TU^=[U(k),U(k+1),...,U(k+N−1)]T\widehat{ \widetilde{S}}=[\widetilde{S}(k+1), \widetilde{S}(k+2),...., \widetilde{S}(k+N)]^T \\ \widehat{\widetilde{U}}=[\widetilde{U}(k),\widetilde{U}(k+1),...,\widetilde{U}(k+N-1)]^T \\ \widehat{U}=[U(k),U(k+1),...,U(k+N-1)]^T S=[S(k+1),S(k+2),....,S(k+N)]TU=[U(k),U(k+1),...,U(k+N1)]TU=[U(k),U(k+1),...,U(k+N1)]T
根据上面的推导,可以推出序列化后的状态方程:
S~^=AA⋅S~(k)+BB⋅U~^s.t.{vmin≤vd≤vmax−ϕm≤ϕ≤ϕm\widehat{ \widetilde{S}}=AA\cdot \widetilde{S}(k)+BB\cdot\widehat{\widetilde{U}} \\ s.t.\begin{cases} v_{min}≤v_d≤v_{max} \\ -\phi _m ≤\phi≤\phi_m \end{cases} S=AAS(k)+BBUs.t.{vminvdvmaxϕmϕϕm
其中
AA=[AA1AA2⋮AAN]BB=[AB1(1)0⋯0AB1(2)AB2(2)⋯0⋮⋮⋱⋮AB1(N)AB2(N)⋯ABN(N)]AA= \begin{bmatrix} AA_1 \\AA_2\\ \vdots \\AA_N \end{bmatrix}\\[2ex] BB = \begin{bmatrix} AB_1^{(1)} & 0 &\cdots & 0 \\ AB_1^{(2)} & AB_2^{(2)} & \cdots & 0\\ \vdots & \vdots & \ddots & \vdots \\ AB_1^{(N)} & AB_2^{(N)} &\cdots & AB_{N}^{(N)} \end{bmatrix} AA=AA1AA2AANBB=AB1(1)AB1(2)AB1(N)0AB2(2)AB2(N)00ABN(N)
优化问题为
U^=argmin⁡U^12S~^TQS~^+12U^TPU^\widehat{U}=arg\min_{\widehat{U}}{\frac{1}{2}\widehat{ \widetilde{S}}^TQ\widehat{ \widetilde{S}}+\frac{1}{2}\widehat{U}^TP\widehat{U}} U=argUmin21STQS+21UTPU

3.4.实现增量控制

\qquad除了保证车辆基本的约束外,为了保证车辆的舒适性,还应防止控制量的突变,即车辆的加速度和打方向盘的速度不至于过快。控制量的加速度不宜过大,需要在约束条件中增加关于控制量变化率的硬约束和在目标函数增加关于控制量变化率的软约束
\qquad硬约束很好理解,即对控制增量进行限幅
{amin≤vd′≤amax−dϕm≤ϕ′≤dϕm\begin{cases} a_{min}≤v'_d≤a_{max} \\ -d\phi_m≤\phi'≤d\phi_m \end{cases} {aminvdamaxdϕmϕdϕm
\qquad软约束则是通过在目标函数中加入关于控制增量的二次型进行约束,目的是使得目标函数在求取极小值的过程中兼顾到控制增量的最小化。因此上述3.3.节的目标函数则需要修改为
ΔU^=argmin⁡ΔU^12S~^TQS~^+12(ΔU^)TP(ΔU^)\Delta\widehat{U}=arg\min_{\Delta\widehat{U}}{\frac{1}{2}\widehat{ \widetilde{S}}^TQ\widehat{ \widetilde{S}}+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U})} ΔU=argΔUmin21STQS+21(ΔU)TP(ΔU)
\qquad仅此而已是不够的,我们还需要重写线性状态方程,记
{Gu=[u(k−1)−ur(k),u(k−1)−ur(k+1),...,u(k−1)−ur(k+N−1)]TEN=tril(ones(1,N))(元素均为1的下三角阵)U~^=Gu+T∗EN⋅ΔU^\left \{ \begin{array}{l} G_u=[u(k-1)-u_r(k),u(k-1)-u_r(k+1),...,u(k-1)-u_r(k+N-1)]^T \\ E_N = tril(ones(1,N))\text{(元素均为1的下三角阵)} \\ \widehat{\widetilde{U}}=G_u+T*E_N\cdot\Delta\widehat{U} \end{array} \right. Gu=[u(k1)ur(k),u(k1)ur(k+1),...,u(k1)ur(k+N1)]TEN=tril(ones(1,N))(元素均为1的下三角阵)U=Gu+TENΔU
因此目标函数为
J=12S~^TQS~^+12(ΔU^)TP(ΔU^)=12(AA⋅S~(k)+BB⋅U~^)TQ(AA⋅S~(k)+BB⋅U~^)+12(ΔU^)TP(ΔU^)=12(AA⋅S~(k)+BB(Gu+T∗ENΔU^))TQ(AA⋅S~(k)+BB(Gu+T∗ENΔU^))+12(ΔU^)TP(ΔU^)\begin{aligned} J= &\frac{1}{2}\widehat{ \widetilde{S}}^TQ\widehat{ \widetilde{S}}+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U}) \\ =&\frac{1}{2}(AA\cdot \widetilde{S}(k)+BB\cdot\widehat{\widetilde{U}})^TQ(AA\cdot \widetilde{S}(k)+BB\cdot\widehat{\widetilde{U}})+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U}) \\ =&\frac{1}{2}(AA\cdot \widetilde{S}(k)+BB(G_u+T*E_N\Delta\widehat{U}))^TQ(AA\cdot \widetilde{S}(k)+BB(G_u+T*E_N\Delta\widehat{U}))+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U}) \end{aligned} J===21STQS+21(ΔU)TP(ΔU)21(AAS(k)+BBU)TQ(AAS(k)+BBU)+21(ΔU)TP(ΔU)21(AAS(k)+BB(Gu+TENΔU))TQ(AAS(k)+BB(Gu+TENΔU))+21(ΔU)TP(ΔU)

GAB=AA⋅S~(k)+BB⋅GuENB=T∗BB⋅ENG_{AB}=AA\cdot \widetilde{S}(k)+BB\cdot G_u \\ E_{NB}=T*BB\cdot E_N GAB=AAS(k)+BBGuENB=TBBEN

J=12(GAB+ENB⋅ΔU^)TQ(GAB+ENB⋅ΔU^)+12(ΔU^)TP(ΔU^)=12GABTQGAB+GABTQENB⋅ΔU^+12(ΔU^)T(ENBTQENB+P)(ΔU^)⟹ΔU^=argmin⁡ΔU^J(ΔU^)s.t.{amin≤vd′≤amax−dϕm≤ϕ′≤dϕm\begin{aligned} J=&\frac{1}{2}(G_{AB}+E_{NB}\cdot\Delta\widehat{U})^TQ(G_{AB}+E_{NB}\cdot \Delta\widehat{U})+\frac{1}{2}(\Delta\widehat{U})^TP(\Delta\widehat{U})\\ =&\frac{1}{2}G_{AB}^TQG_{AB}+G_{AB}^TQE_{NB}\cdot\Delta\widehat{U}+\frac{1}{2}(\Delta\widehat{U})^T(E_{NB}^TQE_{NB}+P)(\Delta\widehat{U})\\[2ex] \Longrightarrow &\Delta\widehat{U}=arg\min_{\Delta\widehat{U}}J(\Delta\widehat{U})\\ &s.t.\begin{cases} a_{min}≤v'_d≤a_{max} \\ -d\phi_m≤\phi'≤d\phi_m \end{cases} \end{aligned} J==21(GAB+ENBΔU)TQ(GAB+ENBΔU)+21(ΔU)TP(ΔU)21GABTQGAB+GABTQENBΔU+21(ΔU)T(ENBTQENB+P)(ΔU)ΔU=argΔUminJ(ΔU)s.t.{aminvdamaxdϕmϕdϕm
\qquad此处参考了友情链接中IEEE的论文的写法,与原书中的写法不一样。这种方法推导出的关于控制增量的目标函数更加直接,也不需要再修改状态向量了。所有MPC的原理讲解部分就到此为止了。
\qquad完整的MPC函数的MATLAB代码如下:

    function u = MPC(Thr_list,ur_list,pSk,u0,L,N,T)u_max = [20;pi/6];u_min = [5;-pi/6];du_max = [2/T;25*pi/180/T];du_min = -du_max;A = cell(N,1);B = cell(N,1);for i=1:NThr = Thr_list(i);   ur = ur_list(:,i);vdr = ur(1);Phr = ur(2);A{i} = [1 0 -vdr*sin(Thr)*T;0 1 vdr*cos(Thr)*T;0 0     1];B{i} = [cos(Thr)*T  0;sin(Thr)*T  0;tan(Phr)*T/L vdr*T*sec(Phr)^2/L];endAA = zeros(3*N,3);  % 序列化之后的ABB = zeros(3*N,2*N);  % 序列化之后的B% S = AA*Sk + BB*u for i = 1:NAA(3*(i-1)+1:3*i,:) = eye(3);for j = 1:iAA(3*(i-1)+1:3*i,:) = A{j}*AA(3*(i-1)+1:3*i,:);endendfor i = 1:N  %for j = i:N  %BB(3*(j-1)+1:3*j,2*(i-1)+1:2*i) = B{i}; % 3×2for t = i+1:jBB(3*(j-1)+1:3*j,2*(i-1)+1:2*i) = A{t}*BB(3*(j-1)+1:3*j,2*(i-1)+1:2*i);endendendGu = cell(N,1);U0 = cell(N,1);for i = 1:NGu{i} = u0 - ur_list(:,i);U0{i} = u0;endGu = cell2mat(Gu);  %2N*2U0 = cell2mat(U0);  %2N*1EN = zeros(2*N,2*N);for i = 1:Nfor j = 1:iEN(2*(i-1)+1,2*(j-1)+1)=T;EN(2*i,2*j)=T;endendENB = BB*EN;GAB = AA*pSk + BB*(Gu); % (33)×(3×1)+(32N)×(21)31       Q = zeros(3*N,3*N);P = zeros(2*N,2*N);Qu = [3,3,1]; % 各状态量的加权Pu = [1,0.2];  % 各控制量的加权for i = 1:NQ(3*(i-1)+1:3*i,3*(i-1)+1:3*i) = diag(Qu*1e5*0.9^(i-1));P(2*(i-1)+1:2*i,2*(i-1)+1:2*i) = diag(Pu*0.9^(i-1));endAb = [EN;-EN];  %4N*2Numax = cell(N,1);umax(:)={u_max};umax = cell2mat(umax);  % 21umin = cell(N,1);umin(:)={u_min};umin = cell2mat(umin);  % 21dumax = cell(N,1);dumax(:)={du_max};dumax =cell2mat(dumax);  % 21dumin = cell(N,1);dumin(:)={du_min};dumin =cell2mat(dumin);  % 21b = cat(1,umax-U0,-umin+U0);  % 41H = ENB'*Q*ENB + P;  % 22NH = 1/2*(H+H');  % 使其严格对称c = (GAB'*Q*ENB)'; % 21opt = optimoptions('quadprog','Display','off');du0 = zeros(2*N,1);du0(1:2) = (ur_list(:,1)-u0)/T;for i = 1:N-1du0(2*i+1:2*i+2) = (ur_list(:,i+1)-ur_list(:,i))/T;enddu = quadprog(H,c,Ab,b,[],[],dumin,dumax,du0,opt);u = u0 +du(1:2)*T;  end

Thr_list是车辆朝向角θ\thetaθ的期望值序列,ur_list是车辆的控制量期望值序列,pSk是S(k)−Sr(k)S(k)-S_r(k)S(k)Sr(k),u0是u(k−1)u(k-1)u(k1),N是预测时域,L是车长,T是控制周期。

4.仿真示例

车辆变道的轨迹图、后轴中心速度图及横摆角图如下
轨迹图
速度图
横摆角图

仿真动画

\qquad可以发现车辆的变道轨迹与期望轨迹基本一致,横摆角成功限制在了-30°~30°之间,速度限幅也符合要求(这里限制上限为20,下限为5)。
\qquad希望本文对您有帮助,谢谢阅读。本文的完整代码请见第0节友情链接处。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/544459.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

顶级Javaer,常用的 14 个类库

作者&#xff1a;小姐姐味道&#xff08;微信公众号ID&#xff1a;xjjdog&#xff09;昨天下载下来Java16尝尝鲜。一看&#xff0c;好家伙&#xff0c;足足有176MB大。即使把jmc和jvisualvm给搞了出去&#xff0c;依然还是这么大&#xff0c;真的是让人震惊不已。但即使JDK足够…

单层神经网络线性回归_单层神经网络| 使用Python的线性代数

单层神经网络线性回归A neural network is a powerful tool often utilized in Machine Learning because neural networks are fundamentally very mathematical. We will use our basics of Linear Algebra and NumPy to understand the foundation of Machine Learning usin…

用express、mongodb、nodejs开发简单的登陆

原文http://my.oschina.net/chenhao901007/blog/312367 npm i -g express serve-favicon morgan cookie-parser body-parser kerberos mongoose(注意:kerberos不安装&#xff0c;mongoose会卡住)2. 调试下面讲讲如何调试服务器端的代码&#xff1a;我们最好借助一个叫node-insp…

Lua元表(Metatable)简易教程

文章目录0.友情链接1.引言2.创建一个元表2.1.__tostring方法2.2.__add和__mul方法2.3.__index方法2.4.__call方法3.完整代码0.友情链接 GitHUb上下载Lua编译器Lua菜鸟教程中的元表介绍&#xff08;较全&#xff0c;但功能性受限&#xff09;博客园内元表的介绍&#xff08;较详…

面试官:说一下 final 和 final 的 4 种用法?

作者 | 王磊来源 | Java中文社群&#xff08;ID&#xff1a;javacn666&#xff09;转载请联系授权&#xff08;微信ID&#xff1a;GG_Stone&#xff09;重要说明&#xff1a;本篇为博主《面试题精选-基础篇》系列中的一篇&#xff0c;查看系列面试文章请关注我。Gitee 开源地址…

ruby 将字符串转为数组_Ruby程序将数组打印为字符串

ruby 将字符串转为数组将数组打印为字符串 (Printing an array as string) Given an array and we have to print it as a string in Ruby. 给定一个数组&#xff0c;我们必须在Ruby中将其打印为字符串。 Ruby provides you various alternatives for the single problem. The…

超定方程组的最小二乘解

\qquad看了很多关于最小二乘解的博客&#xff0c;事实上都没有找到自己想要的证明过程&#xff0c;后来学了矩阵函数时才彻底搞明白了这件事情&#xff0c;所以和大家简单分享如下&#xff1a; \qquad已知矩阵Amn(m&#xff1e;n)A_{mn}(m&#xff1e;n)Amn​(m&#xff1e;n)是…

面试官:int和Integer有什么区别?为什么要有包装类?

作者 | 磊哥来源 | Java面试真题解析&#xff08;ID&#xff1a;aimianshi666&#xff09;转载请联系授权&#xff08;微信ID&#xff1a;GG_Stone&#xff09;重要说明&#xff1a;本篇为博主《面试题精选-基础篇》系列中的一篇&#xff0c;查看系列面试文章请关注我。Gitee 开…

shell从小做起:将100以内整除3的数列出来

#!/bin/bash for i in $(seq 1 100) do a$[ $i%3 ] #注&#xff1a; 在取余的时候 需要 运算 所以需要加运算符号 $[ ] if [ $a -eq 0 ]; thenecho "$i" fi done转载于:https://blog.51cto.com/286577399/1676501

c# 用空格分割字符串_C#| 左用空格填充字符串

c# 用空格分割字符串PadLeft() method is a library method of the String class. It is used to pad the string from the left side with spaces. PadLeft()方法是String类的库方法。 它用于从左侧用空格填充字符串。 Syntax: 句法&#xff1a; string string.PadLeft(int …

innodb是如何存数据的?yyds

前言如果你使用过mysql数据库&#xff0c;对它的存储引擎&#xff1a;innodb&#xff0c;一定不会感到陌生。众所周知&#xff0c;在mysql5以前&#xff0c;默认的存储引擎是&#xff1a;myslam。但mysql5之后&#xff0c;默认的存储引擎已经变成了&#xff1a;innodb&#xff…

【MATLAB】卡尔曼滤波器的原理及仿真(初学者专用)

文章目录0.引言1.场景预设2.卡尔曼滤波器3.仿真及效果0.引言 \qquad本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档&#xff08;Kalman Filter&#xff09;。官方教程的B站链接如下&#xff0c;在此对分享资源的Up主表示感谢。(如不能正常播放或需要看中文字幕&#xff0…

Go实现查找目录下(包括子目录)替换文件内容

为什么80%的码农都做不了架构师&#xff1f;>>> 【功能】 按指定的目录查找出文件&#xff0c;如果有子目录&#xff0c;子目录也将进行搜索&#xff0c;将其中的文件内容进行替换。 【缺陷】 1. 没有过滤出文本文件 2. 当文件过大时&#xff0c;效率不高 【代码】…

卡诺模板_无关条件的卡诺地图

卡诺模板Till now, the Boolean expressions which have been discussed by us were completely specified, i.e., for each combination of input variable we have specified a minterm by representing them as 1 in the K-Map. But, there may arise a case when for a giv…

面试官:final、finally、finalize 有什么区别?

作者 | 磊哥来源 | Java面试真题解析&#xff08;ID&#xff1a;aimianshi666&#xff09;转载请联系授权&#xff08;微信ID&#xff1a;GG_Stone&#xff09;重要说明&#xff1a;本篇为博主《面试题精选-基础篇》系列中的一篇&#xff0c;查看系列面试文章请关注我。Gitee 开…

【Matlab】扩展卡尔曼滤波器原理及仿真(初学者入门专用)

文章目录0.引言及友情链接1.场景预设2.扩展卡尔曼滤波器3.仿真及效果0.引言及友情链接 \qquad卡尔曼滤波器&#xff08;Kalman Filter, KF&#xff09;是传感器融合&#xff08;Sensor Fusion&#xff09;的基础&#xff0c;虽然知乎、CSDN、GitHub等平台已有大量的学习资料&am…

Windows 8.1 升级到专业版

本例将一台 Windows 8.1 平板升级到专业版。升级前&#xff1a;升级的原因&#xff0c;是因为用户发现这台平板不能启用远程桌面管理。查看计算机属性&#xff0c;显示如下&#xff1a;从上面的信息可以看出&#xff0c;目前这台平板安装的不是专业版。具体是什么版本呢&#x…

【MATLAB】求点到多边形的最短距离

文章目录0.引言1.原理2.代码及实用教程0.引言 \qquad点与多边形的关系无非三种——内部、上、外部。本文定义点在多边形内部距离为负&#xff0c;点在多边形边上距离为0&#xff0c;到多边形外部距离为正。 1.原理 计算点到多边形的距离分为3个步骤&#xff1a; 判断点与多边…

绝了,66道并发多线程面试题汇总

&#x1f446;&#x1f3fb;一个专注于 Java 面试的原创公众号。我花了点时间整理了一些多线程,并发相关的面试题&#xff0c;虽然不是很多&#xff0c;但是偶尔看看还是很有用的哦&#xff01;话不多说&#xff0c;直接开整&#xff01;01 什么是线程&#xff1f;线程是操作系…

ruby array_Array.select! Ruby中的示例方法

ruby arrayArray.select&#xff01; 方法 (Array.select! Method) In this article, we will study about Array.select! Method. You all must be thinking the method must be doing something related to the selection of objects from the Array instance. It is not as …