1.理论计算
本文主要探讨使用模型预测控制技术MPC实现小车对预定轨迹的追踪。
1.1. 问题阐述
小车数学模型
采用简单的线性的一阶模型：
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. MPC控制器设计
简单来说，模型预测控制 其实就是 预测被控对象未来的输出，计算 使未来系统输出 等于或接近预定输出 的控制信号。因此控制信号的计算需要两个步骤：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:根据系统要求自定义，优化时域决定着预测模型矩阵的维数，所以要从计算量和快速性做权衡
控制时域timec：根据系统要求自定义，控制时域也要从计算量和快速性之间做权衡。注意控制时域要比优化时域短，即timec<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
+
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
)
,
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)。
[
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)
预测模型得出，并将(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问题)**求解。
首先根据控制目标写出代价函数，注意代价函数还考虑了控制信号尽量小且不发生突变。
即
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)
综上所述，我们需要求解满足(6)且使
J
J
J最小的
U
U
U，这就构成了一个二次优化问题，通常通过计算机函数包求解。
求解成功之后，将矩阵
U
U
U中的k时刻的控制输入作用至实际系统，即
v
x
(
k
)
,
v
y
(
k
)
v_{x}(k),v_{y}(k)
vx(k),vy(k)，完成控制信号计算。
反馈校正
反馈校正的含义是将
v
x
(
k
)
,
v
y
(
k
)
v_{x}(k),v_{y}(k)
vx(k),vy(k)输入至实际模型和预测模型中，将二者输出的偏差反馈至预测模型，从而对预测模型的预测值进行修正。
但本文考虑了最简单的情形，即实际模型和预测模型完全一致为(2)，因此本文不作反馈校正。在实际系统中，系统的非线性性以及不可避免的外部扰动，都会使实际模型和预测模型有差别，因此反馈校正是必不可少的。
2. 仿真验证
2.1. 仿真框架
通过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即可。
本文通过MATLAB中的SIMULINK搭建闭环控制系统，如图所示
图中car表示小车的实际模型式(2)，MPCcontroller表示控制信号的计算程序，包括预定轨迹更新和
J
J
J的求解。car和MPCcontroller均是通过S-function编写。
2.2. 离线计算与在线计算
从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
离线计算的参数
两个矩阵
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
MATLAB中求解上述二次优化问题的函数为quadprog，在MATLAB的workspace中输入help 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
显然quadprog的标准问题与本文描述的
J
J
J表达式不同，因此需要做变换。
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≤d
2.4. 仿真程序
一共四个程序：
- trajectorytracking.mdl用于SIMULINK搭建
- ParameterDefinition.m为参数定义和离线计算
- car.m为实际小车模型的S-function
- MPCcontroller为在线计算和 控制信号计算
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
如果再考虑动力模型和外部扰动，问题将非常复杂。
由于二次规划问题只能依靠线性的预测模型才能求解，所以只能建立线性预测模型，这就涉及到非线性模型的系统辨识问题——如何将非线性模型近似为某个线性模型？
方法有很多：小扰动模型、输入输出线性化、T-S模糊模型等。
也可以从二次规划问题入手，使用梯度下降法求解局部极小值，这样就不用一定是线性模型了，例如神经网络的预测模型。
3.2. 未加入反馈校正
如果实际系统和预测模型有差别，就必须引入反馈校正对预测模型进行校正。设
k
k
k时刻系统实际输出为
p
(
k
)
p(k)
p(k)，预测模型的预测输出为
p
^
(
k
)
\hat{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
)
)
[\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取值可根据实际情况确定。
282
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
