该博客用以记录MPC轨迹控制的理论和matlab程序
一. 运动学
1.1. 运动学建模
被控对象运动学模型，为无人艇USV模型：
x
˙
(
k
)
=
f
(
x
,
u
)
=
{
x
˙
=
u
cos
(
ψ
)
−
v
sin
(
ψ
)
y
˙
=
u
sin
(
ψ
)
+
v
cos
(
ψ
)
ψ
˙
=
r
\begin{equation} \dot{\bold{x}}(k)= f(\bold{x},\bold{u})=\left\{ \begin{aligned} &\dot{x}=u\cos(\psi)-v\sin(\psi) \\ &\dot{y}=u\sin(\psi)+v\cos(\psi) \\ &\dot{\psi}=r \end{aligned} \right. \end{equation}
x˙(k)=f(x,u)=⎩
⎨
⎧x˙=ucos(ψ)−vsin(ψ)y˙=usin(ψ)+vcos(ψ)ψ˙=r
其中状态向量：
x
=
[
x
y
ψ
]
T
\bold{x}=[x \quad y \quad \psi]^T
x=[xyψ]T；
控制量向量：
u
=
[
u
v
r
]
T
\bold{u}=[u \quad v \quad r]^T
u=[uvr]T
离散化状态更新方程：
x
(
k
+
1
)
=
x
(
k
)
+
x
˙
(
k
)
T
=
x
(
k
)
+
f
(
x
(
k
)
,
u
(
k
)
)
T
\bold{x(k+1)}=\bold{x(k)}+\dot{\bold{x}}(k)T= \bold{x(k)}+f(\bold{x(k),u(k)})T
x(k+1)=x(k)+x˙(k)T=x(k)+f(x(k),u(k))T
其中
T
T
T为采样时间，将上式展开：
x
(
k
+
1
)
=
[
x
(
k
)
y
(
k
)
ψ
(
k
)
]
+
[
u
(
k
)
cos
(
ψ
(
k
)
)
−
v
(
k
)
sin
(
ψ
(
k
)
)
u
(
k
)
sin
(
ψ
(
k
)
)
+
v
(
k
)
cos
(
ψ
(
k
)
)
r
(
k
)
]
T
=
[
x
(
k
)
+
u
(
k
)
cos
(
ψ
(
k
)
)
T
−
v
(
k
)
sin
(
ψ
(
k
)
)
T
y
(
k
)
+
u
(
k
)
sin
(
ψ
(
k
)
)
T
+
v
(
k
)
cos
(
ψ
(
k
)
)
T
ψ
(
k
)
+
r
(
k
)
T
]
=
f
(
x
(
k
)
,
u
(
k
)
)
\begin{aligned} \bold{x(k+1)}&= \left[ \begin{array}{ll} x(k) \\ y(k) \\ \psi(k) \end{array} \right]+ \left[ \begin{array}{ccc} u(k)\cos(\psi(k))-v(k)\sin(\psi(k)) \\ u(k)\sin(\psi(k))+v(k)\cos(\psi(k)) \\ r(k) \end{array} \right]T \\ &=\left[ \begin{array}{ccc} x(k)+u(k)\cos(\psi(k))T-v(k)\sin(\psi(k))T \\ y(k)+u(k)\sin(\psi(k))T+v(k)\cos(\psi(k))T \\ \psi(k)+r(k)T \end{array} \right] \\ &= f(\bold{x(k),u(k)}) \end{aligned}
x(k+1)=
x(k)y(k)ψ(k)
+
u(k)cos(ψ(k))−v(k)sin(ψ(k))u(k)sin(ψ(k))+v(k)cos(ψ(k))r(k)
T=
x(k)+u(k)cos(ψ(k))T−v(k)sin(ψ(k))Ty(k)+u(k)sin(ψ(k))T+v(k)cos(ψ(k))Tψ(k)+r(k)T
=f(x(k),u(k))
上式为该系统的状态更新式，其为一非线性系统。
1.2. 非线性系统线性化
在原点
x
(
0
)
,
u
(
0
)
\bold{x(0),u(0)}
x(0),u(0)处对上式进行泰勒展开，保留一阶项：
x
(
k
+
j
)
=
f
(
x
(
k
)
,
u
(
k
)
)
+
∂
f
(
x
(
k
)
,
u
(
k
)
)
∂
x
(
x
(
k
+
j
−
1
)
−
x
(
k
)
)
+
∂
f
(
x
(
k
)
,
u
(
k
)
)
∂
u
(
u
(
k
+
j
−
1
)
−
u
(
k
)
)
=
f
(
x
(
k
)
,
u
(
k
)
)
+
A
⋅
(
x
(
k
+
j
−
1
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
+
j
−
1
)
−
u
(
k
)
)
\begin{aligned} \bold{x(k+j)}=&f(\bold{x(k),u(k)})+\frac{\partial f(\bold{x(k),u(k)})}{\partial{\bold{x}}}(\bold{x(k+j-1)-x(k)}) \\ &+\frac{\partial f(\bold{x(k),u(k)})}{\partial{\bold{u}}}(\bold{u(k+j-1)-u(k)}) \\ =&f(\bold{x(k),u(k)})+A\cdot(\bold{x(k+j-1)})-\bold{x(k)})+B\cdot(\bold{u(k+j-1)-u(k)}) \end{aligned}
x(k+j)==f(x(k),u(k))+∂x∂f(x(k),u(k))(x(k+j−1)−x(k))+∂u∂f(x(k),u(k))(u(k+j−1)−u(k))f(x(k),u(k))+A⋅(x(k+j−1))−x(k))+B⋅(u(k+j−1)−u(k))
其中
A
=
∂
f
(
x
(
k
)
,
u
(
k
)
)
∂
x
=
[
1
0
[
−
u
(
k
)
sin
(
ψ
(
k
)
)
−
v
(
k
)
cos
(
ψ
(
k
)
)
]
T
0
1
[
u
(
k
)
cos
(
ψ
(
k
)
)
−
v
(
k
)
sin
(
ψ
(
k
)
)
]
T
0
0
1
]
\begin{aligned} A = \frac{\partial f(\bold{x(k),u(k)})}{\partial{\bold{x}}}= \left[ \begin{array}{ccc} 1 & 0 & [-u(k)\sin(\psi(k))-v(k)\cos(\psi(k))]T\\ 0 & 1 & [u(k)\cos(\psi(k))-v(k)\sin(\psi(k))]T\\ 0 & 0 & 1 \\ \end{array} \right] \end{aligned}
A=∂x∂f(x(k),u(k))=
100010[−u(k)sin(ψ(k))−v(k)cos(ψ(k))]T[u(k)cos(ψ(k))−v(k)sin(ψ(k))]T1
B
=
∂
f
(
x
(
k
)
,
u
(
k
)
)
∂
u
=
[
cos
(
ψ
(
k
)
)
−
sin
(
ψ
(
k
)
)
0
sin
(
ψ
(
k
)
)
cos
(
ψ
(
k
)
)
0
0
0
1
]
T
\begin{aligned} B = \frac{\partial f(\bold{x(k),u(k)})}{\partial{\bold{u}}}= \left[ \begin{array}{ccc} \cos(\psi(k)) & -\sin(\psi(k)) & 0\\ \sin(\psi(k)) & \cos(\psi(k)) & 0\\ 0 & 0 & 1 \\ \end{array} \right]T \end{aligned}
B=∂u∂f(x(k),u(k))=
cos(ψ(k))sin(ψ(k))0−sin(ψ(k))cos(ψ(k))0001
T
A,B矩阵在每个更新步中都需要重新构建，因为
x
(
k
)
,
u
(
k
)
\bold{x(k),u(k)}
x(k),u(k)每个更新步都会更新。
ps：这里简单复习一下向量对向量求偏导：
∂
f
(
x
(
k
)
,
u
(
k
)
)
∂
u
\frac{\partial f(\bold{x(k),u(k)})}{\partial{\bold{u}}}
∂u∂f(x(k),u(k))，其中
f
=
[
f
1
f
2
f
3
]
T
,
u
=
[
u
1
u
2
u
3
]
T
\bold{f}=[f_1 \quad f_2 \quad f_3]^T,\bold{u}=[u_1 \quad u_2 \quad u_3]^T
f=[f1f2f3]T,u=[u1u2u3]T
∂
f
∂
u
=
[
∂
f
1
∂
u
1
∂
f
1
∂
u
2
∂
f
1
∂
u
3
∂
f
2
∂
u
1
∂
f
2
∂
u
2
∂
f
2
∂
u
3
∂
f
3
∂
u
1
∂
f
3
∂
u
2
∂
f
3
∂
u
3
]
\begin{aligned} \frac{\partial \bold{f}}{\partial{\bold{u}}}= \left[ \begin{array}{ccc} \frac{\partial {f_1}}{\partial{{u_1}}} & \frac{\partial {f_1}}{\partial{{u_2}}} & \frac{\partial {f_1}}{\partial{{u_3}}}\\ \\ \frac{\partial {f_2}}{\partial{{u_1}}} & \frac{\partial {f_2}}{\partial{{u_2}}} & \frac{\partial {f_2}}{\partial{{u_3}}}\\ \\ \frac{\partial {f_3}}{\partial{{u_1}}} & \frac{\partial {f_3}}{\partial{{u_2}}} & \frac{\partial {f_3}}{\partial{{u_3}}} \\ \end{array} \right] \end{aligned}
∂u∂f=
∂u1∂f1∂u1∂f2∂u1∂f3∂u2∂f1∂u2∂f2∂u2∂f3∂u3∂f1∂u3∂f2∂u3∂f3
另外这里我们关心的控制量是
u
\bold{u}
u的增量
Δ
u
\bold{\Delta u}
Δu，其中
Δ
u
(
k
)
=
u
(
k
+
1
)
−
u
(
k
)
\bold{\Delta u(k)}=\bold{u(k+1)-u(k)}
Δu(k)=u(k+1)−u(k)
1.3 状态量 x ( k ) x(k) x(k)的堆栈形式推导
令
x
ˉ
=
[
x
(
k
+
1
)
x
(
k
+
2
)
…
x
(
k
+
N
)
]
T
∈
R
3
N
\bold{\bar{x}}=[\bold{x(k+1)} \quad \bold{x(k+2)} \quad \dots \quad \bold{x(k+N)}]^T \in \mathbb{R}^{3N}
xˉ=[x(k+1)x(k+2)…x(k+N)]T∈R3N，表示模型对于接下来的N步状态的预测，关于该式的形式，可以将
x
(
k
+
j
)
\bold{x(k+j)}
x(k+j)多写几条看看规律：
x
(
k
+
1
)
=
f
(
x
(
0
)
,
u
(
0
)
)
+
A
⋅
(
x
(
k
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
)
−
u
(
k
)
)
=
f
(
x
(
0
)
,
u
(
0
)
)
=
x
(
k
+
1
)
\begin{aligned} \bold{x(k+1)}&=f(\bold{x(0),u(0)})+A\cdot(\bold{x(k)})-\bold{x(k)})+B\cdot(\bold{u(k)-u(k)}) \\ &=f(\bold{x(0),u(0)})=\bold{x(k+1)} \end{aligned}
x(k+1)=f(x(0),u(0))+A⋅(x(k))−x(k))+B⋅(u(k)−u(k))=f(x(0),u(0))=x(k+1)
x
(
k
+
2
)
=
f
(
x
(
0
)
,
u
(
0
)
)
+
A
⋅
(
x
(
k
+
1
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
+
1
)
−
u
(
k
)
)
=
−
A
⋅
x
(
k
)
+
(
A
+
I
)
⋅
x
(
k
+
1
)
+
B
⋅
Δ
u
(
k
)
\begin{aligned} \bold{x(k+2)}&=f(\bold{x(0),u(0)})+A\cdot(\bold{x(k+1)})-\bold{x(k)})+B\cdot(\bold{u(k+1)-u(k)}) \\ &=-A\cdot \bold{x(k)}+(A+I)\cdot \bold{x(k+1)}+B\cdot \bold{\Delta{u(k)}} \end{aligned}
x(k+2)=f(x(0),u(0))+A⋅(x(k+1))−x(k))+B⋅(u(k+1)−u(k))=−A⋅x(k)+(A+I)⋅x(k+1)+B⋅Δu(k)
x
(
k
+
3
)
=
f
(
x
(
0
)
,
u
(
0
)
)
+
A
⋅
(
x
(
k
+
2
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
+
2
)
−
u
(
k
)
)
=
−
(
A
2
+
A
)
⋅
x
(
k
)
+
(
A
2
+
A
+
I
)
⋅
x
(
k
+
1
)
+
(
A
B
+
B
)
⋅
Δ
u
(
k
)
+
B
Δ
u
(
k
+
1
)
\begin{aligned} \bold{x(k+3)}&=f(\bold{x(0),u(0)})+A\cdot(\bold{x(k+2)})-\bold{x(k)})+B\cdot(\bold{u(k+2)-u(k)}) \\ &=-(A^2+A)\cdot \bold{x(k)}+(A^2+A+I)\cdot \bold{x(k+1)} \\ &+(AB+B)\cdot \bold{\Delta{u(k)}}+B\bold{\Delta u(k+1)} \end{aligned}
x(k+3)=f(x(0),u(0))+A⋅(x(k+2))−x(k))+B⋅(u(k+2)−u(k))=−(A2+A)⋅x(k)+(A2+A+I)⋅x(k+1)+(AB+B)⋅Δu(k)+BΔu(k+1)
x
(
k
+
4
)
=
f
(
x
(
0
)
,
u
(
0
)
)
+
A
⋅
(
x
(
k
+
3
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
+
3
)
−
u
(
k
)
)
=
−
(
A
3
+
A
2
+
A
)
⋅
x
(
k
)
+
(
A
3
+
A
2
+
A
+
I
)
⋅
x
(
k
+
1
)
+
(
A
2
B
+
A
B
+
B
)
⋅
Δ
u
(
k
)
+
(
A
B
+
B
)
Δ
u
(
k
+
1
)
+
B
Δ
u
(
k
+
2
)
\begin{aligned} \bold{x(k+4)}&=f(\bold{x(0),u(0)})+A\cdot(\bold{x(k+3)})-\bold{x(k)})+B\cdot(\bold{u(k+3)-u(k)}) \\ &=-(A^3+A^2+A)\cdot \bold{x(k)}+(A^3+A^2+A+I)\cdot \bold{x(k+1)} \\ &+(A^2B+AB+B)\cdot \bold{\Delta{u(k)}}+(AB+B)\bold{\Delta u(k+1)}+B\bold{\Delta u(k+2)} \end{aligned}
x(k+4)=f(x(0),u(0))+A⋅(x(k+3))−x(k))+B⋅(u(k+3)−u(k))=−(A3+A2+A)⋅x(k)+(A3+A2+A+I)⋅x(k+1)+(A2B+AB+B)⋅Δu(k)+(AB+B)Δu(k+1)+BΔu(k+2)
.
.
.
...
...
x
(
k
+
N
)
=
f
(
x
(
0
)
,
u
(
0
)
)
+
A
⋅
(
x
(
k
+
N
−
1
)
)
−
x
(
k
)
)
+
B
⋅
(
u
(
k
+
N
−
1
)
−
u
(
k
)
)
=
−
∑
j
=
1
N
−
1
(
A
j
)
⋅
x
(
k
)
+
∑
j
=
0
N
−
1
(
A
j
)
⋅
x
(
k
+
1
)
+
∑
j
=
0
N
−
2
(
A
j
)
B
⋅
Δ
u
(
k
)
+
∑
j
=
0
N
−
3
(
A
j
)
B
Δ
u
(
k
+
1
)
+
⋯
+
∑
j
=
0
N
−
N
u
+
1
(
A
j
)
B
Δ
u
(
k
+
N
u
−
1
)
\begin{aligned} \bold{x(k+N)}&=f(\bold{x(0),u(0)})+A\cdot(\bold{x(k+N-1)})-\bold{x(k)})+B\cdot(\bold{u(k+N-1)-u(k)}) \\ &=-\sum\limits_{j=1}^{N-1}(A^j) \cdot \bold{x(k)}+\sum\limits_{j=0}^{N-1}(A^j) \cdot \bold{x(k+1)} \\ &+\sum\limits_{j=0}^{N-2}(A^j)B \cdot \bold{\Delta{u(k)}}+\sum\limits_{j=0}^{N-3}(A^j)B\bold{\Delta u(k+1)}+\dots+\sum\limits_{j=0}^{N-N_u+1}(A^j)B \bold{\Delta u(k+N_u-1)} \end{aligned}
x(k+N)=f(x(0),u(0))+A⋅(x(k+N−1))−x(k))+B⋅(u(k+N−1)−u(k))=−j=1∑N−1(Aj)⋅x(k)+j=0∑N−1(Aj)⋅x(k+1)+j=0∑N−2(Aj)B⋅Δu(k)+j=0∑N−3(Aj)BΔu(k+1)+⋯+j=0∑N−Nu+1(Aj)BΔu(k+Nu−1)
其中
N
u
N_u
Nu为控制时域，满足条件
1
≤
N
u
≤
N
1\leq N_u \leq N
1≤Nu≤N
以下给出
x
ˉ
\bold{\bar{x}}
xˉ的形式：
x
ˉ
=
M
1
⋅
x
(
k
)
+
M
2
⋅
x
(
k
+
1
)
+
M
3
⋅
Δ
u
ˉ
\bold{\bar{x}}=M_1\cdot \bold{x(k)}+M_2\cdot \bold{x(k+1)}+M_3\cdot \bold\Delta{\bar{u}}
xˉ=M1⋅x(k)+M2⋅x(k+1)+M3⋅Δuˉ
其中：
M
1
=
−
[
O
A
A
+
A
2
…
A
+
A
2
+
…
A
N
−
1
]
∈
R
3
N
×
3
M_1=- \left[ \begin{array}{cccc} O \\ A \\ A+A^2 \\ \dots \\ A+A^2+\dots A^{N-1} \end{array} \right]\in \mathbb{R}^{3N\times3}
M1=−
OAA+A2…A+A2+…AN−1
∈R3N×3,
M
2
=
[
I
I
+
A
I
+
A
+
A
2
…
I
+
A
+
A
2
+
…
A
N
−
1
]
=
1
⊗
I
−
M
1
M_2= \left[ \begin{array}{cccc} I \\ I+A \\ I+A+A^2 \\ \dots \\ I+A+A^2+\dots A^{N-1} \end{array} \right]=\bold{1}\otimes I-M_1
M2=
II+AI+A+A2…I+A+A2+…AN−1
=1⊗I−M1
M
3
=
[
O
O
…
O
B
O
…
O
A
B
+
B
B
…
O
…
…
…
…
∑
j
=
0
N
−
2
(
A
j
)
B
∑
j
=
0
N
−
3
(
A
j
)
B
…
∑
j
=
0
N
−
N
u
+
1
(
A
j
)
B
]
∈
R
3
N
×
3
N
u
M_3= \left[ \begin{array}{cccc} O &O &\dots & O \\ B &O &\dots &O\\ AB+B & B &\dots &O \\ \dots &\dots &\dots & \dots \\ \sum\limits_{j=0}^{N-2}(A^j)B &\sum\limits_{j=0}^{N-3}(A^j)B &\dots &\sum\limits_{j=0}^{N-N_u+1}(A^j)B \end{array} \right]\in \mathbb{R}^{3N\times3N_u}
M3=
OBAB+B…j=0∑N−2(Aj)BOOB…j=0∑N−3(Aj)B……………OOO…j=0∑N−Nu+1(Aj)B
∈R3N×3Nu
Δ
u
ˉ
=
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
…
Δ
u
(
k
+
N
u
−
1
)
]
T
∈
R
3
N
u
\bold{\Delta \bar{u}}=[\bold{\Delta{{u}(k)}} \quad \bold{\Delta{{u}(k+1)}} \quad \dots \bold{\Delta{{u}(k+N_u-1)}}]^T \in \mathbb{R}^{3N_u}
Δuˉ=[Δu(k)Δu(k+1)…Δu(k+Nu−1)]T∈R3Nu
到此为止，状态量的堆栈形式由
x
ˉ
=
M
1
⋅
x
(
k
)
+
M
2
⋅
x
(
k
+
1
)
+
M
3
⋅
Δ
u
ˉ
\bold{\bar{x}}=M_1\cdot \bold{x(k)}+M_2\cdot \bold{x(k+1)}+M_3\cdot \bold\Delta{\bar{u}}
xˉ=M1⋅x(k)+M2⋅x(k+1)+M3⋅Δuˉ 给出
二. 代价函数和优化目标构建
我们引入二次型代价函数：
J
=
1
2
x
T
H
x
+
f
T
x
J =\frac{1}{2}x^THx+f^Tx
J=21xTHx+fTx，该问题可以由matlab的内置QP优化函数quadprog求解。
2.1 目标函数构建
定义状态量误差，其中期望状态量也即期望轨迹
x
ˉ
∗
∈
R
3
N
u
\bold{\bar{x}}^* \in \mathbb{R}^{3N_u}
xˉ∗∈R3Nu应该是一个已知量，程序中可以自定义，例如定义一个虚拟参考USV，给定速度
u
∗
\bold{u}^*
u∗，则其接下来的期望轨迹是已知的。则状态量误差定义为：
x
ˉ
~
=
x
ˉ
−
x
ˉ
∗
\tilde{\bold{\bar{x}}}=\bold{\bar{x}}-\bold{\bar{x}}^*
xˉ~=xˉ−xˉ∗
那么定义如下二次型代价函数：
J
=
x
ˉ
~
T
Q
ˉ
x
ˉ
~
+
Δ
u
ˉ
T
R
ˉ
Δ
u
ˉ
J=\tilde{\bold{\bar{x}}}^T\bar{Q}\tilde{\bold{\bar{x}}}+\bold\Delta{\bar{u}}^T \bar{R}\bold\Delta{\bar{u}}
J=xˉ~TQˉxˉ~+ΔuˉTRˉΔuˉ
其中
Q
=
d
i
a
g
(
[
q
1
q
2
q
3
]
)
Q=diag([q_1 \quad q_2 \quad q_3])
Q=diag([q1q2q3]),
R
=
d
i
a
g
(
[
r
1
r
2
r
3
]
)
R=diag([r_1 \quad r_2 \quad r_3])
R=diag([r1r2r3])分别为状态量和控制量增量的权重矩阵，
Q
ˉ
=
I
N
⊗
Q
∈
R
3
N
×
3
N
\bar{Q}=I_N \otimes Q \in \mathbb{R}^{3N\times3N}
Qˉ=IN⊗Q∈R3N×3N,
R
ˉ
=
I
N
u
⊗
R
∈
R
3
N
u
×
3
N
u
\bar{R}=I_{N_u} \otimes R \in \mathbb{R}^{3N_u \times 3N_u}
Rˉ=INu⊗R∈R3Nu×3Nu为适配以上矩阵乘法的Q，R的增广矩阵（PS：如果对于终态状态有不同的权重需求，可以将
Q
ˉ
,
R
ˉ
\bar{Q},\bar{R}
Qˉ,Rˉ的最后一个对角块替换为所需的权重矩阵）。
则将以上
x
ˉ
=
M
1
⋅
x
(
k
)
+
M
2
⋅
x
(
k
+
1
)
+
M
3
⋅
Δ
u
ˉ
\bold{\bar{x}}=M_1\cdot \bold{x(k)}+M_2\cdot \bold{x(k+1)}+M_3\cdot \bold\Delta{\bar{u}}
xˉ=M1⋅x(k)+M2⋅x(k+1)+M3⋅Δuˉ代入
J
J
J中，得到以下仅关于待优化量
Δ
u
ˉ
\bold\Delta{\bar{u}}
Δuˉ的形式：
J
=
D
T
Q
ˉ
D
+
Δ
u
ˉ
T
(
M
3
T
Q
ˉ
M
3
+
R
ˉ
)
Δ
u
ˉ
+
(
2
D
T
Q
ˉ
M
3
)
Δ
u
ˉ
J=D^T\bar QD+ \bold{\Delta\bar{u}}^T(M_3^T \bar Q M_3+\bar R)\bold{\Delta\bar{u}}+(2D^T\bar Q M_3)\bold{\Delta \bar{u}}
J=DTQˉD+ΔuˉT(M3TQˉM3+Rˉ)Δuˉ+(2DTQˉM3)Δuˉ
其中：
D
=
M
1
⋅
x
(
k
)
+
M
2
⋅
x
(
k
+
1
)
−
x
ˉ
∗
D=M_1\cdot \bold{x(k)}+M_2 \cdot \bold{x(k+1)}-\bold{\bar x}^*
D=M1⋅x(k)+M2⋅x(k+1)−xˉ∗，以上的项
D
T
Q
ˉ
D
D^T\bar QD
DTQˉD是一个常数值，和优化无关，因此该问题优化为：
J
=
1
2
Δ
u
ˉ
T
⋅
2
(
M
3
T
Q
ˉ
M
3
+
R
ˉ
)
⋅
Δ
u
ˉ
+
(
2
D
T
Q
ˉ
M
3
)
Δ
u
ˉ
J=\frac{1}{2} \bold{\Delta \bar{u}}^T\cdot2(M_3^T \bar Q M_3+\bar R)\cdot\bold{\Delta\bar{u}}+(2D^T\bar Q M_3)\bold{\Delta \bar{u}}
J=21ΔuˉT⋅2(M3TQˉM3+Rˉ)⋅Δuˉ+(2DTQˉM3)Δuˉ
因此，对应形式
J
=
1
2
x
T
H
x
+
f
T
x
J =\frac{1}{2}x^THx+f^Tx
J=21xTHx+fTx，可得
H
=
2
(
M
3
T
Q
ˉ
M
3
+
R
ˉ
)
H=2(M_3^T \bar Q M_3+\bar R)
H=2(M3TQˉM3+Rˉ)，
f
T
=
2
D
T
Q
ˉ
M
3
f^T=2D^T\bar Q M_3
fT=2DTQˉM3
2.2 约束条件构建
约束条件：为了简化问题，这里对于状态 x \bold{x} x没有特殊要求，如果是有避障等特殊需求的任务，则应加上相应的约束。有约束的是控制量 u \bold{u} u(即速度上下限)和控制量增量 Δ u \bold{\Delta{u}} Δu(可以看作加速度，当然实际的无人艇的加速度到速度是有比较复杂的动力学约束的).
设 u \bold{u} u的上下界为下界 l b ∈ R 3 l_b\in \mathbb{R}^{3} lb∈R3，上界 u b ∈ R 3 u_b\in \mathbb{R}^{3} ub∈R3， Δ u \bold{\Delta{u}} Δu的下界 Δ l b ∈ R 3 \Delta l_b\in \mathbb{R}^{3} Δlb∈R3，上界 Δ u b ∈ R 3 \Delta u_b\in \mathbb{R}^{3} Δub∈R3，则有： l b ≤ u ≤ u b l_b \leq \bold{u} \leq u_b lb≤u≤ub， Δ l b ≤ Δ u ≤ Δ u b \Delta l_b \leq \bold{\Delta{u}} \leq \Delta u_b Δlb≤Δu≤Δub；
该约束的含义是：速度量是有上下限的，而且速度变化不能太大(太大会产生跳变，不符合实际的USV运动规律)
以下推导，
Δ
u
\bold{\Delta{u}}
Δu和
u
\bold{{u}}
u的关系：
[
u
(
k
+
1
)
u
(
k
+
2
)
…
u
(
k
+
N
u
)
]
=
[
I
3
I
3
…
I
3
]
⋅
u
(
k
)
+
[
I
3
O
…
O
I
3
I
3
…
O
…
…
…
…
I
3
I
3
…
I
3
]
⋅
[
Δ
u
(
k
)
Δ
u
(
k
+
1
)
…
Δ
u
(
k
+
N
u
−
1
)
]
\left[ \begin{array}{c} \bold{u(k+1)} \\ \bold{u(k+2)} \\ \dots \\ \bold{u(k+N_u)} \end{array} \right]=\left[ \begin{array}{ll} I_3 \\ I_3 \\ \dots \\ I_3 \end{array} \right] \cdot \bold{u(k)} + \left[ \begin{array}{ll} I_3 & O & \dots & O \\ I_3 & I_3 & \dots & O \\ \dots & \dots & \dots & \dots \\ I_3 & I_3 & \dots & I_3 \\ \end{array} \right] \cdot \left[ \begin{array}{c} \bold{\Delta u(k)} \\ \bold{\Delta u(k+1)} \\ \dots \\ \bold{\Delta u(k+N_u-1)} \end{array} \right]
u(k+1)u(k+2)…u(k+Nu)
=
I3I3…I3
⋅u(k)+
I3I3…I3OI3…I3…………OO…I3
⋅
Δu(k)Δu(k+1)…Δu(k+Nu−1)
写成紧凑形式：
u
ˉ
=
1
N
u
⊗
I
3
⋅
u
(
k
)
+
t
r
i
l
(
1
N
u
)
⊗
I
3
⋅
Δ
u
ˉ
(
k
)
\bold{\bar{u}}=\bold{1}_{N_u} \otimes I_3 \cdot \bold{u(k)}+ tril(\bold{1}_{N_u})\otimes I_3 \cdot \bold{\Delta \bar u(k)}
uˉ=1Nu⊗I3⋅u(k)+tril(1Nu)⊗I3⋅Δuˉ(k)，
t
r
i
l
(
1
N
u
)
tril(\bold{1}_{N_u})
tril(1Nu)表示
R
3
N
u
×
3
N
u
\mathbb{R}^{3N_u\times 3N_u}
R3Nu×3Nu的下三角和对角线元素全为1的方阵。
令
a
=
1
N
u
⊗
I
3
⋅
u
(
k
)
\bold{a}=\bold{1}_{N_u} \otimes I_3 \cdot \bold{u(k)}
a=1Nu⊗I3⋅u(k)，
I
~
=
t
r
i
l
(
1
N
u
)
⊗
I
3
\tilde I=tril(\bold{1}_{N_u})\otimes I_3
I~=tril(1Nu)⊗I3，则可以得到以下不等式（这个不等式在程序里面要用）：
1
N
u
⊗
l
b
≤
I
~
⋅
Δ
u
ˉ
(
k
)
+
a
≤
1
N
u
⊗
u
b
\bold{1}_{N_u} \otimes l_b \leq \tilde I\cdot \bold{\Delta \bar u(k)}+\bold{a} \leq \bold{1}_{N_u} \otimes u_b
1Nu⊗lb≤I~⋅Δuˉ(k)+a≤1Nu⊗ub
[
I
~
−
I
~
]
⋅
Δ
u
ˉ
(
k
)
≤
[
1
N
u
⊗
u
b
−
a
−
1
N
u
⊗
l
b
+
a
]
\left[ \begin{array}{cc} \tilde I \\ -\tilde I \end{array} \right] \cdot \bold{\Delta \bar u(k)} \leq \left[ \begin{array}{ll} \bold{1}_{N_u} \otimes u_b-\bold{a} \\ -\bold{1}_{N_u} \otimes l_b+\bold{a} \end{array} \right]
[I~−I~]⋅Δuˉ(k)≤[1Nu⊗ub−a−1Nu⊗lb+a]
2.3 汇总：优化问题定义
m
i
n
1
2
Δ
u
ˉ
T
⋅
2
(
M
3
T
Q
ˉ
M
3
+
R
ˉ
)
⋅
Δ
u
ˉ
+
(
2
D
T
Q
ˉ
M
3
)
Δ
u
ˉ
s
.
t
.
[
I
~
−
I
~
]
⋅
Δ
u
ˉ
(
k
)
≤
[
1
N
u
⊗
u
b
−
a
−
1
N
u
⊗
l
b
+
a
]
Δ
l
b
≤
Δ
u
ˉ
≤
Δ
u
b
\begin{aligned} min &\quad \frac{1}{2} \bold{\Delta \bar u}^T\cdot2(M_3^T \bar Q M_3+\bar R)\cdot\bold{\Delta \bar{u}}+(2D^T\bar Q M_3)\bold{\Delta \bar{u}} \\ s.t. &\qquad \left[ \begin{array}{cc} \tilde I \\ -\tilde I \end{array} \right] \cdot \bold{\Delta \bar u(k)} \leq \left[ \begin{array}{ll} \bold{1}_{N_u} \otimes u_b-\bold{a} \\ -\bold{1}_{N_u} \otimes l_b+\bold{a} \end{array} \right] \\ &\qquad \qquad \quad \Delta l_b \leq \bold{\Delta \bar{u}} \leq \Delta u_b \end{aligned}
mins.t.21ΔuˉT⋅2(M3TQˉM3+Rˉ)⋅Δuˉ+(2DTQˉM3)Δuˉ[I~−I~]⋅Δuˉ(k)≤[1Nu⊗ub−a−1Nu⊗lb+a]Δlb≤Δuˉ≤Δub
如前面提到过的：这个优化问题可以通过matlab函数quadprog求解，具体用法用matlab命令doc quadprog查看
PS:该博客我不停强调矩阵和向量的维度，因为这个东西确实一复杂就容易搞混淆，为了便于自己理解，我尽量把这个标的很清楚
三. matlab程序和仿真结果
函数：构建大矩阵 M 1 , M 2 , M 3 M_1,M_2,M_3 M1,M2,M3
function M1 = getM1_New(A, N)
%GETM1 此处显示有关此函数的摘要
% 构建 M1矩阵
M1 = [];
for i = 1:N
ele = zeros(3);
for j = 1:i-1
ele = ele - A^j;
end
M1 = [M1; ele];
end
end
function M2 = getM2_New(A, N)
%GETM1 此处显示有关此函数的摘要
% 构建 M2矩阵
M1 = getM1_New(A, N);
M2 = kron(ones(N, 1), eye(3)) - M1;
end
function M3 = getM3(A, B, N)
%GETM1 此处显示有关此函数的摘要
% 构建 M3矩阵
M3 = [];
M2 = getM2(A, B, N);
% M2 = M2(1:3*(N-1), :);
for i = 1:N
ele_top = zeros(3, 3);
ele_mid = zeros(3*(i-1), 3);
ele_bot = M2(1:3*(N-i), :);
ele = [ele_top; ele_mid; ele_bot];
M3 = [M3 ele];
end
end
主程序：该主程序差一些自己编的工具函数，直接复制不能运行。博客最下方提供的云盘里的程序mpc_test03_by_ycx.m可以运行。
%% 带有 Δu限制的 mpc
% author by: Alexdar Y
% 该程序需要同文件中的函数
% getM1_New.m getM2_New.m getM3.m toRadian.m
clear;
clc;
% 预测区间
N = 5;
Nu = 2;
T = 0.1;
% 限制条件
% Δu的上下限
% lb = [-0.5; -0.5; -1];
% ub = [0.5; 0.5; 1];
lb = [-0.5; -0.5; -toRadian(20)];
ub = [0.5; 0.5; toRadian(20)];
% u的上下限
lb_u = [-1; -1; -toRadian(60)];
ub_u = [1; 1; toRadian(60)];
I_Nu = ones(Nu, 1);
lb_stacked = kron(I_Nu, lb);
ub_stacked = kron(I_Nu, ub);
lb_u_stacked = kron(I_Nu, lb_u);
ub_u_stacked = kron(I_Nu, ub_u);
I_tilde = kron(tril(ones(Nu)), eye(3));
% 初始状态设置
x_ref_init = [10; 8; toRadian(90)];
x1_init = [6; 6; toRadian(70)];
u_ref = [.2; 0; toRadian(5)];
% u_ref = [.4; 0; 0];
u1_init = [0.5; 0; 0];
% Δu(0) ~ Δu(Nu-1)
u_delta_stacked = zeros(3*Nu, 1);
%
u_stacked = kron(ones(Nu, 1), u1_init) + I_tilde*u_delta_stacked;
% 以下矩阵需要每个循环重新构建
z1 = x1_init;
u1 = u1_init;
% 这里之前写错了，所以有一个转置
A = [0 0 0;
0 0 0;
-u1(1)*sin(z1(3))-u1(2)*cos(z1(3)) u1(1)*cos(z1(3))-u1(2)*sin(z1(3)) 0]';
B = [ cos(z1(3)) sin(z1(3)) 0;
-sin(z1(3)) cos(z1(3)) 0;
0 0 1]';
A_new = eye(3)+A*T;
B_new = B*T;
% 更新状态 - 用真实非线性模型更新
x_update = @(x_cur, u_cur)( ...
x_cur + [u_cur(1)*cos(x_cur(3))-u_cur(2)*sin(x_cur(3));
u_cur(1)*sin(x_cur(3))+u_cur(2)*cos(x_cur(3));
u_cur(3) ] * T);
% 生成参考轨迹
iter_num = 100;
x_ref_whole = [];
x_ref = x_ref_init;
for i = 0:T:iter_num
if (i > iter_num/3) && (i < iter_num*2/3)
u_ref = [.4, 0, -toRadian(20)];
elseif i >= iter_num*2/3
u_ref = [.5, 0, 0];
end
x_ref_whole = [x_ref_whole; x_ref];
x_ref = x_update(x_ref, u_ref);
end
% 代价函数权重 Q R
Q = diag([20 20 1]);
F = diag([40 40 2]);
R = diag([1 1 .2]);
Q_ext = kron(eye(N), Q);
Q_ext((length(Q_ext(:, 1))-3+1):(length(Q_ext(:, 1))),...
(length(Q_ext(:, 1))-3+1):(length(Q_ext(:, 1)))) = F;
R_ext = kron(eye(Nu), R);
% MPC更新
% z(k)
z1 = x1_init;
u1 = u1_init;
% z(k+1)
z_11 = x_update(z1, u1);
z1_restore = [];
u1_restore = [];
J_restore = [];
idx = 0;
% 关键在于 多元函数的一阶泰勒展开 线性化
tic;
for i = 0:T:(iter_num-1)
if idx >= iter_num / T-20
break;
end
disp(idx);
z1_restore = [z1_restore; z1];
u1_restore = [u1_restore; u1];
% x_ref = x_ref_whole(3*idx+1:3*(idx+1));
x_ref = x_ref_whole(3*idx+1:3*idx+3*N);
% x_ref_stacked = kron(ones(N, 1), x_ref);
% 构建新的 线性系统矩阵
A = [0 0 0;
0 0 0;
-u1(1)*sin(z1(3))-u1(2)*cos(z1(3)) u1(1)*cos(z1(3))-u1(2)*sin(z1(3)) 0]';
B = [ cos(z1(3)) sin(z1(3)) 0;
-sin(z1(3)) cos(z1(3)) 0;
0 0 1]';
A_new = eye(3)+A*T;
B_new = B*T;
% 构建 栈形式 的整体矩阵
M1 = getM1_New(A_new, N);
M2 = getM2_New(A_new, N);
M3 = getM3(A_new, B_new, N);
M3 = M3(:, 1:3*Nu);
D = M1 * z1 + M2 * z_11 - x_ref;
H = (M3' * Q_ext * M3 + R_ext);
% H = (M3' * Q_ext * M3);
f = 2 * D' * Q_ext * M3;
% f = f';
% quadprog A_less*u < b_less
a = kron(ones(Nu, 1), u1);
A_less = [ I_tilde;
-I_tilde];
b_less = [ ub_u_stacked-a;
-lb_u_stacked+a];
% 代价函数值
J = (D+M3*u_delta_stacked)' * Q_ext * (D+M3*u_delta_stacked) + ...
u_delta_stacked' * R_ext * u_delta_stacked;
J_restore = [J_restore; J];
[u_delta_Nu, fval, exitflag] = quadprog(H, f', A_less, b_less, [], [], lb_stacked, ub_stacked, ...
u_delta_stacked);
z1_predict_stack = M1 * z1 + M2 * u1 + M3*u_delta_Nu;
z1_error_stack = z1_predict_stack - x_ref;
z_err_valued = norm(z1_error_stack);
u_stacked = kron(ones(Nu, 1), u1) + I_tilde * u_delta_stacked;
% J_restore = [J_restore; fval];
u_delta = u_delta_Nu(1:3);
% u_delta = u_delta_Nu(4:6);
u1 = u1 + u_delta;
% z1 = x_update(z1, u1);
z1 = z_11;
z_11 = x_update(z1, u1);
u_delta_stacked = [u_delta_Nu(4:3*Nu); u_delta_Nu(3*Nu-2:(3*Nu))];
idx = idx + 1;
end
toc;
%%
cla;
z1_x = z1_restore(1:3:length(z1_restore));
z1_y = z1_restore(2:3:length(z1_restore));
x_ref_x = x_ref_whole(1:3:length(x_ref_whole));
y_ref_x = x_ref_whole(2:3:length(x_ref_whole));
% 轨迹图
figure(1);
hold on
plot(x_ref_x, y_ref_x, Color='g', LineStyle='--', LineWidth=2);
plot(z1_x, z1_y, Color='r', LineStyle='-', LineWidth=2);
axis equal;
hold off
% 速度曲线
figure(2);
u1_u = u1_restore(1:3:length(u1_restore));
u1_v = u1_restore(2:3:length(u1_restore));
u1_r = u1_restore(3:3:length(u1_restore));
hold on
plot(u1_u, Color='r');
plot(u1_v, Color='g');
plot(u1_r, Color='b');
hold off
% 目标函数值
figure(3)
plot(J_restore, Color='r');
下图为轨迹跟踪结果，绿色的是参考轨迹(自定义的)，红色的是实际轨迹
文章来源：https://www.toymoban.com/news/detail-803933.html
四. 总结
- 这里处理非线性系统的方式是线性化，用到一阶泰勒展开；
- 如果需要处理本质非线性系统需要用到matlab函数
fmincon，应该说非线性系统的优化还是比较复杂的，与初值，优化算法等诸多因素有关，有时还不一定能找到最优解； - 这里的USV仅仅涉及到一阶运动学方程，如果被控模型是二阶动力学模型，则这里得到的一阶控制量 u \bold{u} u可以作为二阶系统的期望值，而二阶系统也可以按照以上方法设计MPC控制；
- MPC的前提是系统动态模型已知，若模型中存在未知量(这种情况再正常不过了，例如水动力系数)，MPC的运用就会受限。
百度云盘：
链接：https://pan.baidu.com/s/1GnDThG6x1jvzK9H2bhyDPQ
提取码：aiif文章来源地址https://www.toymoban.com/news/detail-803933.html
到了这里，关于USV的MPC轨迹跟踪控制的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
