基于MPC的移动机器人轨迹跟踪控制matlab例程
github地址
https://github.com/zzy5510/MPC_control_robot
移动机器人建模
因为所有机器人均可转化为独轮车模型,因此本项目采用独轮车模型。
(
x
˙
c
y
˙
c
θ
˙
c
)
=
(
cos
θ
c
0
sin
θ
c
0
0
1
)
(
v
c
ω
c
)
\left(\begin{array}{c}\dot{x}_{\mathrm{c}} \\ \dot{y}_{\mathrm{c}} \\ \dot{\theta}_{\mathrm{c}}\end{array}\right)=\left(\begin{array}{cc}\cos \theta_{\mathrm{c}} & 0 \\ \sin \theta_{\mathrm{c}} & 0 \\ 0 & 1\end{array}\right)\left(\begin{array}{c}v_{\mathrm{c}} \\ \omega_{\mathrm{c}}\end{array}\right)
⎝⎛x˙cy˙cθ˙c⎠⎞=⎝⎛cosθcsinθc0001⎠⎞(vcωc)
vc为输入速度,wc为输入角速度。积分后离散,得到离散化的状态方程:
(
x
k
+
1
y
k
+
1
φ
k
+
1
)
=
(
x
k
+
v
k
T
s
cos
φ
k
y
k
+
v
k
T
s
sin
φ
k
φ
k
+
w
k
T
s
)
\left(\begin{array}{c}x_{k+1} \\ y_{k+1} \\ \varphi_{k+1}\end{array}\right)=\left(\begin{array}{c}x_{k}+v_{k} T_{s} \cos \varphi_{k} \\ y_{k}+v_{k} T_{s} \sin \varphi_{k} \\ \varphi_{k}+w_{k} T_{s}\end{array}\right)
⎝⎛xk+1yk+1φk+1⎠⎞=⎝⎛xk+vkTscosφkyk+vkTssinφkφk+wkTs⎠⎞
可以看出,该状态方程组是非线性的,角度和位置之间存在耦合。然而基于状态方程的MPC要求方程为线性,因此需要对该方程进行处理。线性化方法参考了文献《基于模型预测控制的自主移动机器人跟踪控制研究_周靖期》。
将方程绕
(
x
r
,
u
r
)
\left(x_{\mathrm{r}}, u_{\mathrm{r}}\right)
(xr,ur)展开,得:
x
˙
=
f
(
x
r
,
u
r
)
+
f
x
,
r
(
x
−
x
r
)
+
f
u
,
r
(
u
−
u
r
)
\dot{x}=f\left(x_{r}, u_{r}\right)+f_{x, r}\left(x-x_{r}\right)+f_{u, r}\left(u-u_{r}\right)
x˙=f(xr,ur)+fx,r(x−xr)+fu,r(u−ur)
其中,
x
r
x_{r}
xr,
y
r
y_{r}
yr为待跟踪的轨迹位置。
f
x
,
r
f_{x, r}
fx,r,
f
y
,
r
f_{y,r}
fy,r为f对x和y的偏导,即:
x
~
˙
=
f
x
,
r
x
~
+
f
u
,
r
u
~
\dot{\tilde{x}}=f_{x, \mathrm{r}} \tilde{x}+f_{u, \mathrm{r}} \tilde{u}
x~˙=fx,rx~+fu,ru~
离散化后,得:
x
~
(
k
+
1
)
=
A
(
k
)
x
~
(
k
)
+
B
(
k
)
u
~
(
k
)
\tilde{x}(k+1)=A(k) \tilde{x}(k)+B(k) \tilde{u}(k)
x~(k+1)=A(k)x~(k)+B(k)u~(k)
A
(
k
)
≜
(
1
0
−
v
r
(
k
)
sin
θ
r
(
k
)
T
0
1
v
r
(
k
)
cos
θ
r
(
k
)
T
0
0
1
)
B
(
k
)
≜
(
cos
θ
r
(
k
)
T
0
sin
θ
r
(
k
)
T
0
0
T
)
A(k) \triangleq\left(\begin{array}{ccc} 1 & 0 & -v_{r}(k) \sin \theta_{r}(k) T \\ 0 & 1 & v_{r}(k) \cos \theta_{r}(k) T \\ 0 & 0 & 1 \end{array}\right) \quad B(k) \triangleq\left(\begin{array}{ccc} \cos \theta_{r}(k) T & 0 \\ \sin \theta_{r}(k) \mathrm{T} & 0 \\ 0 & \mathrm{~T} \end{array}\right)
A(k)≜⎝⎛100010−vr(k)sinθr(k)Tvr(k)cosθr(k)T1⎠⎞B(k)≜⎝⎛cosθr(k)Tsinθr(k)T000 T⎠⎞
离散化的方法参考https://wenku.baidu.com/view/ab37b916fbb069dc5022aaea998fcc22bdd14314.html
预测模型
预测模型的过程如下:
将预测过程中的所有量结合成向量:
x
ˉ
(
k
+
1
)
≜
(
x
~
(
k
+
1
∣
k
)
x
~
(
k
+
2
∣
k
)
⋮
x
~
(
k
+
N
∣
k
)
)
u
ˉ
(
k
)
≜
(
u
~
(
k
∣
k
)
u
~
(
k
+
1
∣
k
)
⋮
u
~
(
k
+
N
−
1
∣
k
)
)
\bar{x}(k+1) \triangleq\left(\begin{array}{c} \tilde{x}(k+1 \mid k) \\ \tilde{x}(k+2 \mid k) \\ \vdots \\ \tilde{x}(k+N \mid k) \end{array}\right) \quad \bar{u}(k) \triangleq\left(\begin{array}{c} \tilde{u}(k \mid k) \\ \tilde{u}(k+1 \mid k) \\ \vdots \\ \tilde{u}(k+N-1 \mid k) \end{array}\right)
xˉ(k+1)≜⎝⎜⎜⎜⎛x~(k+1∣k)x~(k+2∣k)⋮x~(k+N∣k)⎠⎟⎟⎟⎞uˉ(k)≜⎝⎜⎜⎜⎛u~(k∣k)u~(k+1∣k)⋮u~(k+N−1∣k)⎠⎟⎟⎟⎞
x
ˉ
(
k
+
1
)
=
A
ˉ
(
k
)
x
~
(
k
∣
k
)
+
B
ˉ
(
k
)
u
ˉ
(
k
)
\bar{x}(k+1)=\bar{A}(k) \tilde{x}(k \mid k)+\bar{B}(k) \bar{u}(k)
xˉ(k+1)=Aˉ(k)x~(k∣k)+Bˉ(k)uˉ(k)
其中,
A
ˉ
(
k
)
≜
(
A
(
k
∣
k
)
A
(
k
∣
k
)
A
(
k
+
1
∣
k
)
⋮
A
(
k
∣
k
)
A
(
k
+
1
∣
k
)
.
.
.
A
(
k
+
P
−
1
∣
k
)
)
\bar{A}(k) \triangleq\left(\begin{array}{c} A(k \mid k) \\ A(k \mid k) A(k+1 \mid k) \\ \vdots \\ \\A(k \mid k) A(k+1 \mid k)...A(k+P-1\mid k) \end{array}\right)
Aˉ(k)≜⎝⎜⎜⎜⎜⎜⎛A(k∣k)A(k∣k)A(k+1∣k)⋮A(k∣k)A(k+1∣k)...A(k+P−1∣k)⎠⎟⎟⎟⎟⎟⎞
B
ˉ
(
k
)
≜
(
B
(
k
∣
k
)
0
⋯
0
A
(
k
+
1
∣
k
)
B
(
k
∣
k
)
B
(
k
+
1
∣
k
)
⋯
0
⋮
⋱
⋮
A
(
k
+
1
∣
k
)
.
.
.
A
(
k
+
P
−
1
∣
k
)
B
(
k
∣
k
)
A
(
k
+
2
∣
k
)
.
.
.
A
(
k
+
P
−
1
∣
k
)
B
(
k
+
1
∣
k
)
⋯
B
(
k
+
N
−
1
∣
k
)
)
\bar{B}(k) \triangleq\left(\begin{array}{cccc} B(k \mid k) & 0 & \cdots & 0 \\ A(k+1 \mid k) B(k \mid k) & B(k+1 \mid k) & \cdots & 0 \\ \vdots & & \ddots & \vdots \\ \\A(k+1 \mid k)...A(k+P-1\mid k) B(k \mid k) & A(k+2 \mid k)...A(k+P-1\mid k) B(k+1 \mid k) & \cdots & B(k+N-1 \mid k) \end{array}\right)
Bˉ(k)≜⎝⎜⎜⎜⎜⎜⎛B(k∣k)A(k+1∣k)B(k∣k)⋮A(k+1∣k)...A(k+P−1∣k)B(k∣k)0B(k+1∣k)A(k+2∣k)...A(k+P−1∣k)B(k+1∣k)⋯⋯⋱⋯00⋮B(k+N−1∣k)⎠⎟⎟⎟⎟⎟⎞
这里将预测时域和控制时域设为同一个值P。
反馈校正
将模型预测的结果与车辆的实际位置做对比,得到误差值e。该值之后会用在滚动优化上。
滚动优化
待优化的二次型多项式为:
ϕ
(
k
)
=
1
2
u
ˉ
T
(
k
)
H
(
k
)
u
ˉ
(
k
)
+
f
T
(
k
)
u
ˉ
(
k
)
+
a
(
k
)
\phi(k)=\frac{1}{2} \bar{u}^{\mathrm{T}}(k) H(k) \bar{u}(k)+f^{\mathrm{T}}(k) \bar{u}(k)+a(k)
ϕ(k)=21uˉT(k)H(k)uˉ(k)+fT(k)uˉ(k)+a(k)
其中,
H
(
k
)
≜
2
(
B
ˉ
T
(
k
)
Q
ˉ
B
ˉ
(
k
)
+
R
ˉ
)
f
(
k
)
≜
2
B
ˉ
T
(
k
)
Q
ˉ
A
ˉ
(
k
)
x
~
(
k
∣
k
)
a
(
k
)
≜
x
~
T
(
k
∣
k
)
A
ˉ
T
(
k
)
Q
ˉ
A
ˉ
(
k
)
x
~
(
k
∣
k
)
\begin{array}{l} H(k) \triangleq 2\left(\bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{B}(k)+\bar{R}\right) \\ f(k) \triangleq 2 \bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \\ a(k) \triangleq \tilde{x}^{\mathrm{T}}(k \mid k) \bar{A}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \end{array}
H(k)≜2(BˉT(k)QˉBˉ(k)+Rˉ)f(k)≜2BˉT(k)QˉAˉ(k)x~(k∣k)a(k)≜x~T(k∣k)AˉT(k)QˉAˉ(k)x~(k∣k)
a与待求解的u无关。得到一个标准的QP问题,可以用matlab内置函数直接求解,也可以手动求解。