模型预测控制（MPC）与PID、纯追踪法相比有更好的路径跟踪效果，在自动驾驶领域有广泛应用。本文将以运动学为基础详细推导差分轮移动机器人模型预测控制（MPC）
运动学模型
根据移动机器人的运动学结构可得移动机器人的状态方程：
x
˙
=
v
cos
θ
y
˙
=
vsin
θ
θ
˙
=
w
(1)
\begin{array}{l} \dot{x}=v \cos \theta \\ \dot{y}=\operatorname{vsin} \theta \\ \dot{\theta}=w \end{array} \tag{1}
x˙=vcosθy˙=vsinθθ˙=w(1)
式（1）为非线性方程，不利于后面进行优化求解，便采用一阶泰勒公式将状态方程（1）进行线性化得：
[
x
˙
y
˙
θ
˙
]
=
[
v
r
cos
θ
r
v
r
sin
θ
r
w
r
]
+
[
0
0
−
v
r
sin
θ
r
0
0
v
r
cos
θ
r
0
0
0
]
[
x
−
x
r
y
−
y
r
θ
−
θ
r
]
+
[
cos
θ
r
0
sin
θ
r
0
0
1
]
[
v
−
v
r
w
−
w
r
]
(2)
\left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]=\left[\begin{array}{ccc} v_{r} \cos \theta_{r} \\ v_{r} \sin \theta_{r} \\ w_{r} \end{array}\right]+\left[\begin{array}{ccc} 0 & 0 & -v_{r} \sin \theta_{r} \\ 0 & 0 & v_{r} \cos \theta_{r} \\ 0 & 0 & 0 \end{array}\right]\left[\begin{array}{cc} x-x_{r} \\ y-y_{r} \\ \theta-\theta_{r} \end{array}\right] +\left[\begin{array}{cc} \cos \theta_{r} & 0 \\ \sin \theta_{r} & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} v-v_{r} \\ w-w_{r} \end{array}\right] \tag{2}
⎣⎡x˙y˙θ˙⎦⎤=⎣⎡vrcosθrvrsinθrwr⎦⎤+⎣⎡000000−vrsinθrvrcosθr0⎦⎤⎣⎡x−xry−yrθ−θr⎦⎤+⎣⎡cosθrsinθr0001⎦⎤[v−vrw−wr](2)
其中，
x
r
,
y
r
,
θ
r
,
v
r
,
w
r
为
移
动
机
器
人
当
前
的
状
态
。
x_r,y_r,\theta _r,v_r,w_r为移动机器人当前的状态。
xr,yr,θr,vr,wr为移动机器人当前的状态。为了方便后续书写，记：
X
=
[
x
y
θ
]
X=\left[\begin{array}{lll} x \\ y \\ \theta \end{array}\right]
X=⎣⎡xyθ⎦⎤，
A
=
[
0
0
−
v
r
s
i
n
θ
r
0
0
v
r
c
o
s
θ
0
0
0
]
A=\left[\begin{array}{ccc} 0 & 0 & -v_r sin \theta _r \\ 0 & 0 & v_r cos \theta \\ 0 & 0 & 0 \end{array}\right]
A=⎣⎡000000−vrsinθrvrcosθ0⎦⎤，
B
=
[
c
o
s
θ
r
0
s
i
n
θ
r
0
0
1
]
B=\left[\begin{array}{cc} cos \theta _r & 0 \\ sin \theta _r & 0 \\ 0 & 1 \end{array}\right]
B=⎣⎡cosθrsinθr0001⎦⎤，
O
=
−
A
[
x
r
y
r
θ
r
]
O = - A\left[\begin{array}{c} x _r \\ y _r \\ \theta _r \end{array}\right]
O=−A⎣⎡xryrθr⎦⎤，
u
=
[
v
w
]
u = \left[\begin{array}{c} v\\ w \end{array}\right]
u=[vw]得：
X
˙
=
A
X
+
B
u
+
O
(3)
\dot X = AX + Bu + O \tag{3}
X˙=AX+Bu+O(3)
为了得到移动机器人的位姿状态，就需要求解状态方程式（3），有多种方法求解式（3），本文只介绍使用一阶差分的方法求解状态方程得：
X
n
−
X
n
−
1
T
=
A
X
n
−
1
+
B
u
n
−
1
+
O
(4)
\frac{X_n - X_{n-1}}{T} = AX_{n-1} + Bu_{n-1} + O \tag{4}
TXn−Xn−1=AXn−1+Bun−1+O(4)
其中
T
T
T为控制周期，整理式(4)得：
X
n
=
(
I
+
T
A
)
X
n
−
1
+
T
B
u
n
−
1
+
T
O
(5)
X_n = (I+TA)X_{n-1}+TBu_{n-1}+TO \tag{5}
Xn=(I+TA)Xn−1+TBun−1+TO(5)
至此，完成差分轮移动机器人数学模型的建立。由于存在近似化的过程，式（5）并不能精准预测移动机器人的状态，但线性化方程便于优化求解。
状态预测
本文以3个滚动时域为例介绍此过程。通过式（5）可知移动机器人在一系列输入
u
0
,
u
1
,
u
2
u_0,u_1,u_2
u0,u1,u2的作用下未来位姿的估计。
X
1
=
(
I
+
T
A
)
X
0
+
T
B
u
0
+
T
O
X
2
=
(
I
+
T
A
)
X
1
+
T
B
u
1
+
T
O
X
3
=
(
I
+
T
A
)
X
2
+
T
B
u
2
+
T
O
(6)
\begin{array}{c} X_1 = (I+TA)X_0+TBu_0+TO \\ X_2 = (I+TA)X_1+TBu_1+TO \\ X_3 = (I+TA)X_2+TBu_2+TO \tag{6} \end{array}
X1=(I+TA)X0+TBu0+TOX2=(I+TA)X1+TBu1+TOX3=(I+TA)X2+TBu2+TO(6)
其中，
X
0
X_0
X0表示移动机器人当前的位置。MPC的目的是使移动机器人跟踪一条给定的路径，为此，应当计算出移动机器人预测位姿与路径的偏差，本文选择一个控制周期长度间隔的路径点
X
r
e
f
1
,
X
r
e
f
2
,
X
r
e
f
3
X_{ref1},X_{ref2},X_{ref3}
Xref1,Xref2,Xref3作为参考点计算偏差。
X
1
−
X
r
e
f
1
=
(
I
+
T
A
)
X
0
+
T
B
u
0
+
T
O
−
X
r
e
f
1
X
2
−
X
r
e
f
2
=
(
I
+
T
A
)
X
1
+
T
B
u
1
+
T
O
−
X
r
e
f
2
X
3
−
X
r
e
f
3
=
(
I
+
T
A
)
X
2
+
T
B
u
2
+
T
O
−
X
r
e
f
3
(7)
\begin{array}{c} X_1 - X_{ref1} = (I+TA)X_0+TBu_0+TO - X_{ref1} \\ X_2 - X_{ref2} = (I+TA)X_1+TBu_1+TO - X_{ref2} \\ X_3 - X_{ref3} = (I+TA)X_2+TBu_2+TO - X_{ref3} \tag{7} \end{array}
X1−Xref1=(I+TA)X0+TBu0+TO−Xref1X2−Xref2=(I+TA)X1+TBu1+TO−Xref2X3−Xref3=(I+TA)X2+TBu2+TO−Xref3(7)
令
Y
=
[
X
1
−
X
r
e
f
1
X
2
−
X
r
e
f
2
X
3
−
X
r
e
f
3
]
Y=\left[\begin{array}{c} X_1 - X_{ref1} \\ X_2 - X_{ref2} \\ X_3 - X_{ref3} \end{array}\right]
Y=⎣⎡X1−Xref1X2−Xref2X3−Xref3⎦⎤，
U
=
[
u
0
u
1
u
2
]
U = \left[\begin{array}{c} u_0 \\ u_1 \\ u_2 \end{array}\right]
U=⎣⎡u0u1u2⎦⎤，
O
ˉ
=
[
T
O
−
X
r
e
f
1
(
I
+
T
A
+
I
)
T
O
−
X
r
e
f
2
(
(
I
+
T
A
)
2
+
I
+
T
A
+
I
)
T
O
−
X
r
e
f
3
]
\bar O = \left[\begin{array}{c} TO - X_{ref1}\\ (I+TA+I)TO - X_{ref2}\\ ((I+TA)^2+I+TA+I)TO - X_{ref3} \end{array}\right]
Oˉ=⎣⎡TO−Xref1(I+TA+I)TO−Xref2((I+TA)2+I+TA+I)TO−Xref3⎦⎤，
A
ˉ
=
[
I
+
T
A
(
I
+
T
A
)
2
(
I
+
T
A
)
3
]
\bar A = \left[\begin{array}{c} I+TA\\ (I+TA)^2\\ (I+TA)^3 \end{array}\right]
Aˉ=⎣⎡I+TA(I+TA)2(I+TA)3⎦⎤，
B
ˉ
=
[
T
B
0
0
(
I
+
T
A
)
T
B
T
B
0
(
I
+
T
A
)
2
T
B
(
I
+
T
A
)
T
B
T
B
]
\bar B = \left[\begin{array}{ccc} TB & 0 & 0\\ (I+TA)TB & TB & 0\\ (I+TA)^2TB & (I+TA)TB & TB \end{array}\right]
Bˉ=⎣⎡TB(I+TA)TB(I+TA)2TB0TB(I+TA)TB00TB⎦⎤得：
Y
=
A
ˉ
X
0
+
B
ˉ
U
+
O
ˉ
(8)
Y=\bar A X_0 + \bar BU + \bar O \tag{8}
Y=AˉX0+BˉU+Oˉ(8)
控制
为了达到一个良好的控制效果，需要定义一个指标描述控制的效果，即一个目标函数。直观上，移动机器人运行轨迹越贴近参考路径越好，这与偏差
Y
Y
Y相关，而且机器人运行越平稳越好，即速度变化量小，这与输出量
U
U
U相关。可以简要定义目标函数
J
J
J:
J
=
Y
T
Q
Y
+
U
T
R
U
m
≤
g
U
≤
n
(9)
\begin{array}{c} J=Y^{T} Q Y+U^{T} R U \\ m \leq g U \leq n \tag{9} \end{array}
J=YTQY+UTRUm≤gU≤n(9)
将式（8）带入式（9），并消去无关常数项，整理得：
min
U
J
=
U
T
(
B
ˉ
T
Q
B
ˉ
+
R
)
U
+
2
(
A
ˉ
X
0
+
O
ˉ
)
T
Q
B
ˉ
U
m
≤
g
U
≤
n
(10)
\begin{array}{c} \min \limits_{U} J=U^T(\bar B^TQ \bar B+R)U+2(\bar AX_0+ \bar O)^TQ \bar BU \\ m \leq g U \leq n \tag{10} \end{array}
UminJ=UT(BˉTQBˉ+R)U+2(AˉX0+Oˉ)TQBˉUm≤gU≤n(10)
可以使用二次规划器对上述方程进行求解，如OSQP或matlab的quadprog函数，并将解的第一项
u
0
u_0
u0做为控制输出。
注意事项
- 单纯的MPC控制在实际运行过程中，并不能完全保证优秀的路径跟踪能力，比如当路径的曲率变化比较大时，如果不减速处理，很容易导致移动机器人偏离路径。因此，参考点的选取也是一个关键的操作，从经验的角度来看，路径曲率变化较大的地方，移动机器人的线速度要比较小，此处路径段参考点之间的距离应小一点。
- 由于运用运动学模型，因此，此MPC只适合低速下运行，无法应用与高速运行的情况，如无人驾驶。
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
