本文主要探讨使用模型预测控制技术MPC实现小车对预定轨迹的追踪。
采用简单的线性的一阶模型:
x ˙ = v x y ˙ = v y (1) \dot{x}=v_{x} \\ \dot{y}=v_{y} \tag{1} x˙=vxy˙=vy(1)
使用采样周期T进行离散化,并得到状态空间表达式:
[ x ( k + 1 ) y ( k + 1 ) ] = ( 1 0 0 1 ) [ x ( k ) y ( k ) ] + ( T 0 0 T ) [ v x ( k ) v y ( k ) ] (2) \left[\begin{array}{lcr} x(k+1)\\ y(k+1) \end{array}\right] =\left(\begin{array}{lcr} 1 & 0\\ 0 & 1 \end{array}\right)\left[\begin{array}{lcr} x(k)\\y(k) \end{array}\right] +\left(\begin{array}{lcr} T & 0\\ 0 & T \end{array}\right)\left[\begin{array}{lcr} v_{x}(k)\\v_{y}(k) \end{array}\right] \tag{2} [x(k+1)y(k+1)]=(1001)[x(k)y(k)]+(T00T)[vx(k)vy(k)](2)
记系数矩阵为
A = ( 1 0 0 1 ) , B = ( T 0 0 T ) A=\left(\begin{array}{lcr} 1 & 0\\ 0 & 1 \end{array}\right), B=\left(\begin{array}{lcr} T & 0\\ 0 & T \end{array}\right) A=(1001),B=(T00T)
预定运动轨迹是已知的且随时间变化,动态为
x r = x r ( k ) y r = y r ( k ) (3) x_{r}=x_{r}(k) \\ y_{r}=y_{r}(k) \tag{3} xr=xr(k)yr=yr(k)(3)
使小车能够准确跟踪预定轨迹,即 x → x r , y → y r x→x_{r},y→y_{r} x→xr,y→yr。
简单来说,模型预测控制 其实就是 预测被控对象未来的输出,计算 使未来系统输出 等于或接近预定输出 的控制信号。因此控制信号的计算需要两个步骤:1.预测系统未来输出——构建预测模型。 2.求解使预测输出接近预定值的控制信号——滚动优化。 实际上还有第三步通过实际输出和预测输出的偏差修正预测模型,使预测模型更加贴近实际模型——反馈校正。
若当前时刻为k时刻,则预测模型是构建系统未来控制量
v x ( k ) , v x ( k + 1 ) , v x ( k + 2 ) , . . . 和 v y ( k ) , v y ( k + 1 ) , v y ( k + 2 ) , . . . v_{x}(k),v_{x}(k+1),v_{x}(k+2),... 和v_{y}(k),v_{y}(k+1),v_{y}(k+2),... vx(k),vx(k+1),vx(k+2),...和vy(k),vy(k+1),vy(k+2),...
和系统未来输出(本文未来输出就是小车的位置)
x ( k + 1 ) , x ( k + 2 ) , . . . x(k+1),x(k+2),... x(k+1),x(k+2),...和 y ( k + 1 ) , y ( k + 2 ) , . . . y(k+1),y(k+2),... y(k+1),y(k+2),...
之间的关系。
在本文中,显然预测模型可以通过(1)递推计算,但在计算之前需要解决一个问题:预测未来多久的输出(优化时域) 和 计算未来多久的控制信号(控制时域)。
优化时域timep:根据系统要求自定义,优化时域决定着预测模型矩阵的维数,所以要从计算量和快速性做权衡 确定了timep和timec,由(2)确定系统的预测模型,即 [ x ( k + 1 ) y ( k + 1 ) ] = A [ x ( k ) y ( k ) ] + B [ v x ( k ) v y ( k ) ] \left[\begin{array}{lcr} x(k+1)\\ y(k+1) \end{array}\right] =A\left[\begin{array}{lcr} x(k)\\y(k) \end{array}\right] +B\left[\begin{array}{lcr} v_{x}(k)\\v_{y}(k) \end{array}\right] [x(k+1)y(k+1)]=A[x(k)y(k)]+B[vx(k)vy(k)] [ x ( k + 2 ) y ( k + 2 ) ] = A [ x ( k + 1 ) y ( k + 1 ) ] + B [ v x ( k + 1 ) v y ( k + 1 ) ] \left[\begin{array}{lcr} x(k+2)\\ y(k+2) \end{array}\right] =A\left[\begin{array}{lcr} x(k+1)\\y(k+1) \end{array}\right] +B\left[\begin{array}{lcr} v_{x}(k+1)\\v_{y}(k+1) \end{array}\right] [x(k+2)y(k+2)]=A[x(k+1)y(k+1)]+B[vx(k+1)vy(k+1)] 将上述式子联立,去掉等号右边的 x ( k + 1 ) , x ( k + 2 ) , . . . , x ( k + t i m e p − 1 ) x(k+1),x(k+2),...,x(k+timep-1) x(k+1),x(k+2),...,x(k+timep−1)和 y ( k + 1 ) , y ( k + 2 ) , . . . , y ( k + t i m e p − 1 ) y(k+1),y(k+2),...,y(k+timep-1) y(k+1),y(k+2),...,y(k+timep−1)得到(4)。 预测模型得出,并将(4)重新写作(5)。 [ x ( k + 1 ) y ( k + 1 ) . . . . . . x ( k + t i m e c ) y ( k + t i m e c ) . . . . . . x ( k + t i m e p ) y ( k + t i m e p ) ] = F [ x ( k ) y ( k ) ] + G [ v x ( k ) v y ( k ) . . . . . . v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] (5) \left[\begin{array}{lcr} x(k+1)\\ y(k+1)\\ ...... \\ x(k+timec)\\ y(k+timec)\\ ...... \\ x(k+timep)\\ y(k+timep)\\ \end{array}\right] =F \left[\begin{array}{lcr} x(k)\\y(k) \end{array}\right] +G \left[\begin{array}{lcr} v_{x}(k)\\ v_{y}(k)\\ ...... \\ v_{x}(k+timec-1)\\ v_{y}(k+timec-1)\\ \end{array}\right] \tag{5} ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x(k+1)y(k+1)......x(k+timec)y(k+timec)......x(k+timep)y(k+timep)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=F[x(k)y(k)]+G⎣⎢⎢⎢⎢⎡vx(k)vy(k)......vx(k+timec−1)vy(k+timec−1)⎦⎥⎥⎥⎥⎤(5) 预定轨迹已知,所以未来timep个时刻的预定输出也可以通过(3)得出。如何求解控制信号 v x ( k ) , v x ( k + 1 ) , v x ( k + 2 ) , . . . , v x ( k + t i m e c − 1 ) 和 v y ( k ) , v y ( k + 1 ) , v y ( k + 2 ) , . . . , v y ( k + t i m e c − 1 ) v_{x}(k),v_{x}(k+1),v_{x}(k+2),...,v_{x}(k+timec-1) 和v_{y}(k),v_{y}(k+1),v_{y}(k+2),...,v_{y}(k+timec-1) vx(k),vx(k+1),vx(k+2),...,vx(k+timec−1)和vy(k),vy(k+1),vy(k+2),...,vy(k+timec−1) 使预测输出和理想输出一致?通过**二次规划(QP问题)**求解。 首先根据控制目标写出代价函数,注意代价函数还考虑了控制信号尽量小且不发生突变。 综上所述,我们需要求解满足(6)且使 J J J最小的 U U U,这就构成了一个二次优化问题,通常通过计算机函数包求解。 反馈校正的含义是将 v x ( k ) , v y ( k ) v_{x}(k),v_{y}(k) vx(k),vy(k)输入至实际模型和预测模型中,将二者输出的偏差反馈至预测模型,从而对预测模型的预测值进行修正。 通过1.2.的内容可以看出,控制信号的计算其实是通过 J J J计算出来的,而 J J J需要计算矩阵 W 、 F 、 X 、 G 、 Q 、 R W、F、X、G、Q、R W、F、X、G、Q、R,因此只需要将这些矩阵计算出来再求解 J J J即可。 从1.2.节看出整个控制信号 v x ( k ) 和 v y ( k ) v_x(k)和v_y(k) vx(k)和vy(k)的计算需要很多矩阵、参数的计算,但是有些参数和矩阵在 每个采样周期 都是相同的,而有些矩阵是需要在每个采样周期进行更新的。 采样周期T、控制时域timec、优化时域timep、控制信号边界 v m i n v_{min} vmin和 v m a x v_{max} vmax、因子矩阵 Q Q Q和 R R R 两个矩阵 两个矩阵 MATLAB中求解上述二次优化问题的函数为quadprog,在MATLAB的workspace中输入help quadprog可以了解其基本用法,这里只介绍本文需要知道的用法。 显然quadprog的标准问题与本文描述的 J J J表达式不同,因此需要做变换。 一共四个程序: 此处只展示部分结果,详细结果及分析可以自行完成。 轨迹方程为 轨迹方程为 本文的实际模型是简单的线性无扰动模型,而在工程应用中无人车、无人机的运动模型常常为如下非线性模型 方法有很多:小扰动模型、输入输出线性化、T-S模糊模型等。 如果实际系统和预测模型有差别,就必须引入反馈校正对预测模型进行校正。设 k k k时刻系统实际输出为 p ( k ) p(k) p(k),预测模型的预测输出为 p ^ ( k ) \hat{p}(k) p^(k)且预测模型为
控制时域timec:根据系统要求自定义,控制时域也要从计算量和快速性之间做权衡。注意控制时域要比优化时域短,即timec
. . . . . . ...... ......
[ x ( k + t i m e c ) y ( k + t i m e c ) ] = A [ x ( k + t i m e c − 1 ) y ( k + t i m e c − 1 ) ] + B [ v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] \left[\begin{array}{lcr} x(k+timec)\\ y(k+timec) \end{array}\right] =A\left[\begin{array}{lcr} x(k+timec-1)\\y(k+timec-1) \end{array}\right] +B\left[\begin{array}{lcr} v_{x}(k+timec-1)\\v_{y}(k+timec-1) \end{array}\right] [x(k+timec)y(k+timec)]=A[x(k+timec−1)y(k+timec−1)]+B[vx(k+timec−1)vy(k+timec−1)]
. . . . . . ...... ......
[ x ( k + t i m e p ) y ( k + t i m e p ) ] = A [ x ( k + t i m e p − 1 ) y ( k + t i m e p − 1 ) ] + B [ v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] \left[\begin{array}{lcr} x(k+timep)\\ y(k+timep) \end{array}\right] =A\left[\begin{array}{lcr} x(k+timep-1)\\y(k+timep-1) \end{array}\right] +B\left[\begin{array}{lcr} v_{x}(k+timec-1)\\v_{y}(k+timec-1) \end{array}\right] [x(k+timep)y(k+timep)]=A[x(k+timep−1)y(k+timep−1)]+B[vx(k+timec−1)vy(k+timec−1)]
[ x ( k + 1 ) y ( k + 1 ) . . . . . . x ( k + t i m e c ) y ( k + t i m e c ) . . . . . . x ( k + t i m e p ) y ( k + t i m e p ) ] = ( A A 2 . . . . . . A t i m e p ) [ x ( k ) y ( k ) ] + ( B 0 A B B . . . . . . A t i m e c − 1 B A t i m e c − 2 B . . . . B . . . . . . A t i m e p − 1 B A t i m e p − 2 B . . . . ∑ i = 0 t i m e p − t i m e c A i B ) [ v x ( k ) v y ( k ) . . . . . . v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] (4) \left[\begin{array}{lcr} x(k+1)\\ y(k+1)\\ ...... \\ x(k+timec)\\ y(k+timec)\\ ...... \\ x(k+timep)\\ y(k+timep)\\ \end{array}\right] =\left(\begin{array}{lcr} A\\ A^2\\ ...... \\ A^{timep}\\ \end{array}\right) \left[\begin{array}{lcr} x(k)\\y(k) \end{array}\right] +\\ \left(\begin{array}{lcr} B & & 0\\ AB & B\\ ...... \\ A^{timec-1}B & A^{timec-2}B & .... & B \\ ...... \\ A^{timep-1}B & A^{timep-2}B & .... & \sum^{timep-timec}_{i=0}A^{i}B\\ \end{array}\right) \left[\begin{array}{lcr} v_{x}(k)\\ v_{y}(k)\\ ...... \\ v_{x}(k+timec-1)\\ v_{y}(k+timec-1)\\ \end{array}\right] \\ \tag{4} ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x(k+1)y(k+1)......x(k+timec)y(k+timec)......x(k+timep)y(k+timep)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎝⎜⎜⎛AA2......Atimep⎠⎟⎟⎞[x(k)y(k)]+⎝⎜⎜⎜⎜⎜⎜⎛BAB......Atimec−1B......Atimep−1BBAtimec−2BAtimep−2B0........B∑i=0timep−timecAiB⎠⎟⎟⎟⎟⎟⎟⎞⎣⎢⎢⎢⎢⎡vx(k)vy(k)......vx(k+timec−1)vy(k+timec−1)⎦⎥⎥⎥⎥⎤(4)滚动优化
即
J ′ = ∥ [ x r ( k + 1 ) y r ( k + 1 ) . . . . . . x r ( k + t i m e c − 1 ) y r ( k + t i m e c − 1 ) ] − [ x ( k + 1 ) y ( k + 1 ) . . . . . . x ( k + t i m e c − 1 ) y ( k + t i m e c − 1 ) ] ∥ 2 + ∥ [ v x ( k ) v y ( k ) . . . . . . v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] ∥ 2 J'=\left\| \left[\begin{array}{lcr} x_{r}(k+1)\\ y_{r}(k+1)\\ ...... \\ x_{r}(k+timec-1)\\ y_{r}(k+timec-1)\\ \end{array}\right] -\left[\begin{array}{lcr} x_(k+1)\\ y(k+1)\\ ...... \\ x(k+timec-1)\\ y(k+timec-1)\\ \end{array}\right] \right\|^2 +\left\| \left[\begin{array}{lcr} v_{x}(k)\\ v_{y}(k)\\ ...... \\ v_{x}(k+timec-1)\\ v_{y}(k+timec-1)\\ \end{array}\right] \right\|^2 J′=∥∥∥∥∥∥∥∥∥∥⎣⎢⎢⎢⎢⎡xr(k+1)yr(k+1)......xr(k+timec−1)yr(k+timec−1)⎦⎥⎥⎥⎥⎤−⎣⎢⎢⎢⎢⎡x(k+1)y(k+1)......x(k+timec−1)y(k+timec−1)⎦⎥⎥⎥⎥⎤∥∥∥∥∥∥∥∥∥∥2+∥∥∥∥∥∥∥∥∥∥⎣⎢⎢⎢⎢⎡vx(k)vy(k)......vx(k+timec−1)vy(k+timec−1)⎦⎥⎥⎥⎥⎤∥∥∥∥∥∥∥∥∥∥2
由于 J ′ J' J′既考虑了追踪的精确和控制信号的值,实际计算中常常对这两个方面有所侧重,因此为这两项分别设置增益来表示更加看重哪个目标。
设定
W = [ x r ( k + 1 ) y r ( k + 1 ) . . . . . . x r ( k + t i m e c − 1 ) y r ( k + t i m e c − 1 ) ] , U = [ v x ( k ) v y ( k ) . . . . . . v x ( k + t i m e c − 1 ) v y ( k + t i m e c − 1 ) ] , X = [ x ( k ) y ( k ) ] W= \left[\begin{array}{lcr} x_{r}(k+1)\\ y_{r}(k+1)\\ ...... \\ x_{r}(k+timec-1)\\ y_{r}(k+timec-1)\\ \end{array}\right],U=\left[\begin{array}{lcr} v_{x}(k)\\ v_{y}(k)\\ ...... \\ v_{x}(k+timec-1)\\ v_{y}(k+timec-1)\\ \end{array}\right],X=\left[\begin{array}{lcr} x(k)\\ y(k)\\ \end{array}\right] W=⎣⎢⎢⎢⎢⎡xr(k+1)yr(k+1)......xr(k+timec−1)yr(k+timec−1)⎦⎥⎥⎥⎥⎤,U=⎣⎢⎢⎢⎢⎡vx(k)vy(k)......vx(k+timec−1)vy(k+timec−1)⎦⎥⎥⎥⎥⎤,X=[x(k)y(k)]
从而 J ′ J' J′重新写成 J J J,即
J = ( W − F X − G U ) T Q ( W − F X − G U ) + U T R U J=(W-FX-GU)^TQ(W-FX-GU)+U^TRU J=(W−FX−GU)TQ(W−FX−GU)+UTRU
其中 Q Q Q和 R R R为正定矩阵,实际应用中还考虑控制输入的取值范围,即
v m i n ≤ v x ( k + i ) , v y ( k + i ) ≤ v m a x , ∀ i ∈ Z + (6) v_{min}≤v_{x}(k+i),v_{y}(k+i)≤v_{max},\forall i \in Z^+ \tag{6} vmin≤vx(k+i),vy(k+i)≤vmax,∀i∈Z+(6)
求解成功之后,将矩阵 U U U中的k时刻的控制输入作用至实际系统,即 v x ( k ) , v y ( k ) v_{x}(k),v_{y}(k) vx(k),vy(k),完成控制信号计算。反馈校正
但本文考虑了最简单的情形,即实际模型和预测模型完全一致为(2),因此本文不作反馈校正。在实际系统中,系统的非线性性以及不可避免的外部扰动,都会使实际模型和预测模型有差别,因此反馈校正是必不可少的。2. 仿真验证
2.1. 仿真框架
本文通过MATLAB中的SIMULINK搭建闭环控制系统,如图所示
图中car表示小车的实际模型式(2),MPCcontroller表示控制信号的计算程序,包括预定轨迹更新和 J J J的求解。car和MPCcontroller均是通过S-function编写。2.2. 离线计算与在线计算
将相同的参数提前确定并输入至系统中(离线计算),更新的参数就在每个采样周期进行更新即可(在线计算)提前自定义的参数
离线计算的参数
F = ( A A 2 . . . . . . A t i m e p ) G = ( B 0 A B B . . . . . . A t i m e c − 1 B A t i m e c − 2 B . . . . B . . . . . . A t i m e p − 1 B A t i m e p − 2 B . . . . ∑ i = 0 t i m e p − t i m e c A i B ) F=\left(\begin{array}{lcr} A\\ A^2\\ ...... \\ A^{timep}\\ \end{array}\right) \\ G=\left(\begin{array}{lcr} B & & 0\\ AB & B\\ ...... \\ A^{timec-1}B & A^{timec-2}B & .... & B \\ ...... \\ A^{timep-1}B & A^{timep-2}B & .... & \sum^{timep-timec}_{i=0}A^{i}B\\ \end{array}\right) F=⎝⎜⎜⎛AA2......Atimep⎠⎟⎟⎞G=⎝⎜⎜⎜⎜⎜⎜⎛BAB......Atimec−1B......Atimep−1BBAtimec−2BAtimep−2B0........B∑i=0timep−timecAiB⎠⎟⎟⎟⎟⎟⎟⎞在线计算的参数
W = [ x r ( k + 1 ) y r ( k + 1 ) . . . . . . x r ( k + t i m e c − 1 ) y r ( k + t i m e c − 1 ) ] X = [ x ( k ) y ( k ) ] W= \left[\begin{array}{lcr} x_{r}(k+1)\\ y_{r}(k+1)\\ ...... \\ x_{r}(k+timec-1)\\ y_{r}(k+timec-1)\\ \end{array}\right] \\ X=\left[\begin{array}{lcr} x(k)\\ y(k)\\ \end{array}\right] W=⎣⎢⎢⎢⎢⎡xr(k+1)yr(k+1)......xr(k+timec−1)yr(k+timec−1)⎦⎥⎥⎥⎥⎤X=[x(k)y(k)]
综上所述我们可以先写一个m文件叫parameterdefinition用于定义参数和矩阵离线计算,MPCcontroller则完成在线计算即可。2.3. 二次规划问题求解器quadprog
调用:U=quadprog(H,f,b,d)
含义:求解 m i n 1 2 x T H x + f T x s . t . b x ≤ d min \frac{1}{2}x^THx+f^Tx \qquad s.t. \quad bx≤d min21xTHx+fTxs.t.bx≤d 并返回 x x x
J = ( W − F X − G U ) T Q ( W − F X − G U ) + U T R U = ( W − F X ) T Q ( W − F X ) + U T ( G T Q G + R ) U − 2 ( W − F X ) T Q G U = ( W − F X ) T Q ( W − F X ) + J q J=(W-FX-GU)^TQ(W-FX-GU)+U^TRU \\ =(W-FX)^TQ(W-FX)+U^T(G^TQG+R)U-2(W-FX)^TQGU \\=(W-FX)^TQ(W-FX)+J_q J=(W−FX−GU)TQ(W−FX−GU)+UTRU=(W−FX)TQ(W−FX)+UT(GTQG+R)U−2(W−FX)TQGU=(W−FX)TQ(W−FX)+Jq
由于 ( W − F X ) T Q ( W − F X ) (W-FX)^TQ(W-FX) (W−FX)TQ(W−FX)与 U U U无关,因此只需考虑 J q J_q Jq最小即可。通过 J q J_q Jq的表达式可以得出quadprog函数中H和f为
H = G T Q G + R , f = − 2 G T Q T ( W − F X ) H=G^TQG+R,\quad f=-2G^TQ^T(W-FX) H=GTQG+R,f=−2GTQT(W−FX)
再考虑控制信号的取值范围的问题,即
( v m i n . . . . . . v m i n ) ≤ U ≤ ( v m a x . . . . . . v m a x ) \left(\begin{array}{lcr} v_{min}\\ ...... \\ v_{min}\\ \end{array}\right) ≤U≤\left(\begin{array}{lcr} v_{max}\\ ...... \\ v_{max}\\ \end{array}\right) ⎝⎛vmin......vmin⎠⎞≤U≤⎝⎛vmax......vmax⎠⎞
做变换
( I t i m e c × t i m e c 0 0 − I t i m e c × t i m e c ) U ≤ ( v m a x . . . . . . v m a x − v m i n . . . . . . − v m i n ) \left(\begin{array}{lcr} I_{timec×timec} & 0\\ 0 & -I_{timec×timec}\\ \end{array}\right) U≤\left(\begin{array}{lcr} v_{max}\\ ...... \\ v_{max}\\ -v_{min}\\ ...... \\ -v_{min}\\ \end{array}\right) (Itimec×timec00−Itimec×timec)U≤⎝⎜⎜⎜⎜⎜⎜⎛vmax......vmax−vmin......−vmin⎠⎟⎟⎟⎟⎟⎟⎞
即 b x ≤ d bx≤d bx≤d2.4. 仿真程序
ParameterDefinition.m
xstart=0;
ystart=0; %initial state
T=0.05; %sample time
timep=10; %predictive time
timec=3; %controllable time
vmax=10;
vmin=10;
D=[vmax;vmax]; %threshold matrix D
for i=2:timec
D=[D;vmax;vmax];
end
for i=1:timec
D=[D;vmin;vmin];
end
Q=eye(2*timep); %gain matrix Q
R=0.5.*eye(2*timec); %gain matrix R
% predictive model
A=eye(2);
B=T.*eye(2);
% MPC state space matrix
% [x(k+1),..,x(k+timep)]'= Fx(k) + G[u(k),...,u(k+timec-1)]'
% calculate MPC state space matrix F
F=cell(timep,1);
F{1,1}=A;
for i=2:timep
F{i,1}=F{i-1,1}*A;
end
F=cell2mat(F);
% calculate MPC state space matrix G
g=B;
for i=1:timec-1
g=[g,zeros(2,2)];
end
G=g;
for i=2:timec
g=A*g+[ zeros(2,2*(i-1)),B,zeros(2,2*(timec-i)) ];
G=[G;g];
end
for i=timec+1:timep
g=A*g+[zeros( 2,2*(timec-1) ),B];
G=[G;g];
end
car.m
function[sys,x0,str,ts]=car(t,x,u,flag,xstart,ystart)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes(xstart,ystart);
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
error(['Unhandled flag=',num2str(flag)]);
end
function[sys,x0,str,ts]=mdlInitializeSizes(xstart,ystart)
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=2;
sizes.NumOutputs=2;
sizes.NumInputs=2;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;
sys=simsizes(sizes);
x0=[xstart;ystart];
str=[];
ts=[0.05,0];
function sys=mdlDerivatives(t,x,u)
sys=[];
function sys=mdlUpdate(t,x,u)
xk=x(1);yk=x(2);
x(1)=xk+u(1)*0.05;
x(2)=yk+u(2)*0.05;
sys=x;
function sys=mdlOutputs(t,x,u)
sys=x;
function sys=mdlTerminate(t,x,u)
sys=[];
MPCcontroller.m
function[sys,x0,str,ts]=MPCcontroller(t,x,u,flag,T,timep,timec,G,Q,R,F,D)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 3,
sys=mdlOutputs(t,x,u,vr,T,timep,timec,G,Q,R,F,N,D);
case {1,2,4,9}
sys=[];
otherwise
error(['Unhandled flag=',num2str(flag)]);
end
function[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=0;
sizes.NumOutputs=4;
sizes.NumInputs=2;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;
sys=simsizes(sizes);
x0=[];
str=[];
ts=[0.05,0];
function sys=mdlOutputs(t,x,u,vr,T,timep,timec,G,Q,R,F,N,D)
% virtual leader and reference outputs
xr=25*sin(0.2*t);
yr=25-25*cos(0.2*t);
W=[25*sin(0.2*t+0.2*T);25-25*cos(0.2*t+0.2*T)]; %reference matrix W
for i=2:timep
W=[W;25*sin(0.2*t+0.2*i*T);25-25*cos(0.2*t+0.2*i*T)];
end
xk=u(1);yk=u(2);
U=quadprog(2*G'*Q*G+2*R,-2*( (W-F*[xk;yk]-N)'*Q*G )',[eye(2*timec);-1.*eye(2*timec)],D);
sys=[U(1);U(2);xr;yr];
2.5. 仿真结果
直线追踪
x r ( k + 1 ) = x r ( k ) + 5 T y r ( k + 1 ) = y r ( k ) + 5 T x_{r}(k+1)=x_{r}(k)+5T \\ y_{r}(k+1)=y_{r}(k)+5T xr(k+1)=xr(k)+5Tyr(k+1)=yr(k)+5T曲线追踪
x r ( k ) = 25 s i n ( 0.2 k T ) y r ( k ) = 25 − 25 c o s ( 0.2 k T ) x_{r}(k)=25sin(0.2kT) \\ y_{r}(k)=25-25cos(0.2kT) xr(k)=25sin(0.2kT)yr(k)=25−25cos(0.2kT)3. 不足
3.1. 实际模型过于简单
x ˙ = v c o s ( θ ) y ˙ = v s i n ( θ ) θ ˙ = w \dot{x}=vcos(\theta) \\ \dot{y}=vsin(\theta) \\ \dot{\theta}=w x˙=vcos(θ)y˙=vsin(θ)θ˙=w
如果再考虑动力模型和外部扰动,问题将非常复杂。
由于二次规划问题只能依靠线性的预测模型才能求解,所以只能建立线性预测模型,这就涉及到非线性模型的系统辨识问题——如何将非线性模型近似为某个线性模型?
也可以从二次规划问题入手,使用梯度下降法求解局部极小值,这样就不用一定是线性模型了,例如神经网络的预测模型。3.2. 未加入反馈校正
[ p ^ ( k + 1 ) , . . . , p ^ ( k + t i m e p ) ] T = f ( p ( k ) , u ( k ) , . . . , u ( k + t i m e c − 1 ) ) [\hat{p}(k+1),...,\hat{p}(k+timep)]^T=f(p(k),u(k),...,u(k+timec-1)) [p^(k+1),...,p^(k+timep)]T=f(p(k),u(k),...,u(k+timec−1))
根据 e r r o r ( k ) = p ( k ) − p ^ ( k ) error(k)=p(k)-\hat{p}(k) error(k)=p(k)−p^(k)可以对预测模型进行修正,即
[ p ^ ( k + 1 ) , . . . , p ^ ( k + t i m e p ) ] T = f ( p ( k ) , u ( k ) , . . . , u ( k + t i m e c − 1 ) ) + [ l 1 , . . . , l t i m e p ] T e r r o r ( k ) [\hat{p}(k+1),...,\hat{p}(k+timep)]^T=f(p(k),u(k),...,u(k+timec-1)) \\ +[l_1,...,l_{timep}]^Terror(k) [p^(k+1),...,p^(k+timep)]T=f(p(k),u(k),...,u(k+timec−1))+[l1,...,ltimep]Terror(k)
其中 [ l 1 , . . . , l t i m e p ] T [l_1,...,l_{timep}]^T [l1,...,ltimep]T取值可根据实际情况确定。