差分轮移动机器人模型预测控制MPC
模型预测控制（MPC）与PID、纯追踪法相比有更好的路径跟踪效果，在自动驾驶领域有广泛应用。本文将以运动学为基础详细推导差分轮移动机器人模型预测控制（MPC）
运动学模型
根据移动机器人的运动学结构可得移动机器人的状态方程：
x˙=vcosθ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˙θ˙]=[vrcosθrvrsinθrwr]+[00−vrsinθr00vrcosθr000][x−xry−yrθ−θr]+[cosθr0sinθr001][v−vrw−wr](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)
其中，xr,yr,θr,vr,wr为移动机器人当前的状态。x_r,y_r,\theta _r,v_r,w_r为移动机器人当前的状态。xr,yr,θr,vr,wr为移动机器人当前的状态。为了方便后续书写，记：X=[xyθ]X=\left[\begin{array}{lll}
x \\ y \\ \theta
\end{array}\right]X=⎣⎡xyθ⎦⎤，A=[00−vrsinθr00vrcosθ000]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=[cosθr0sinθr001]B=\left[\begin{array}{cc}
cos \theta _r & 0 \\
sin \theta _r & 0 \\
0 & 1
\end{array}\right]B=⎣⎡cosθrsinθr0001⎦⎤，O=−A[xryrθr]O = - A\left[\begin{array}{c}
x _r \\
y _r \\
\theta _r
\end{array}\right]O=−A⎣⎡xryrθr⎦⎤，u=[vw]u = \left[\begin{array}{c}
v\\
w
\end{array}\right]u=[vw]得：
X˙=AX+Bu+O(3)
\dot X = AX + Bu + O
\tag{3}
X˙=AX+Bu+O(3)
为了得到移动机器人的位姿状态，就需要求解状态方程式（3），有多种方法求解式（3），本文只介绍使用一阶差分的方法求解状态方程得：
Xn−Xn−1T=AXn−1+Bun−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)
其中TTT为控制周期，整理式(4)得：
Xn=(I+TA)Xn−1+TBun−1+TO(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）可知移动机器人在一系列输入u0,u1,u2u_0,u_1,u_2u0,u1,u2的作用下未来位姿的估计。
X1=(I+TA)X0+TBu0+TOX2=(I+TA)X1+TBu1+TOX3=(I+TA)X2+TBu2+TO(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)
其中，X0X_0X0表示移动机器人当前的位置。MPC的目的是使移动机器人跟踪一条给定的路径，为此，应当计算出移动机器人预测位姿与路径的偏差，本文选择一个控制周期长度间隔的路径点Xref1,Xref2,Xref3X_{ref1},X_{ref2},X_{ref3}Xref1,Xref2,Xref3作为参考点计算偏差。
X1−Xref1=(I+TA)X0+TBu0+TO−Xref1X2−Xref2=(I+TA)X1+TBu1+TO−Xref2X3−Xref3=(I+TA)X2+TBu2+TO−Xref3(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=[X1−Xref1X2−Xref2X3−Xref3]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=[u0u1u2]U = \left[\begin{array}{c}
u_0 \\
u_1 \\
u_2
\end{array}\right]U=⎣⎡u0u1u2⎦⎤，Oˉ=[TO−Xref1(I+TA+I)TO−Xref2((I+TA)2+I+TA+I)TO−Xref3]\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+TA(I+TA)2(I+TA)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ˉ=[TB00(I+TA)TBTB0(I+TA)2TB(I+TA)TBTB]\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ˉX0+BˉU+Oˉ(8)
Y=\bar A X_0 + \bar BU + \bar O
\tag{8}
Y=AˉX0+BˉU+Oˉ(8)
控制
为了达到一个良好的控制效果，需要定义一个指标描述控制的效果，即一个目标函数。直观上，移动机器人运行轨迹越贴近参考路径越好，这与偏差YYY相关，而且机器人运行越平稳越好，即速度变化量小，这与输出量UUU相关。可以简要定义目标函数JJJ:
J=YTQY+UTRUm≤gU≤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），并消去无关常数项，整理得：
minUJ=UT(BˉTQBˉ+R)U+2(AˉX0+Oˉ)TQBˉUm≤gU≤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函数，并将解的第一项u0u_0u0做为控制输出。
注意事项
- 单纯的MPC控制在实际运行过程中，并不能完全保证优秀的路径跟踪能力，比如当路径的曲率变化比较大时，如果不减速处理，很容易导致移动机器人偏离路径。因此，参考点的选取也是一个关键的操作，从经验的角度来看，路径曲率变化较大的地方，移动机器人的线速度要比较小，此处路径段参考点之间的距离应小一点。
- 由于运用运动学模型，因此，此MPC只适合低速下运行，无法应用与高速运行的情况，如无人驾驶。
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
8
0- 0
已为社区贡献7条内容
所有评论(0)