MPC用优化求解器 - 解决无人机轨迹跟踪
一个较为完整的数学表达式描述，展示如何使用最优控制方法（ACADO 思想）解决无人机轨迹跟踪问题。这里以二维无人机（简化为双积分模型）为例，其数学表达式如下：
1. 问题描述
无人机在平面内的运动可以用状态变量
x
(
t
)
=
[
x
(
t
)
y
(
t
)
v
x
(
t
)
v
y
(
t
)
]
∈
R
4
x(t) = \begin{bmatrix} x(t) \\ y(t) \\ v_x(t) \\ v_y(t) \end{bmatrix} \in \mathbb{R}^4
x(t)=
x(t)y(t)vx(t)vy(t)
∈R4
和控制输入
u
(
t
)
=
[
a
x
(
t
)
a
y
(
t
)
]
∈
R
2
u(t) = \begin{bmatrix} a_x(t) \\ a_y(t) \end{bmatrix} \in \mathbb{R}^2
u(t)=[ax(t)ay(t)]∈R2
描述。其动力学方程为
x
˙
(
t
)
=
f
(
x
(
t
)
,
u
(
t
)
)
=
[
v
x
(
t
)
v
y
(
t
)
a
x
(
t
)
a
y
(
t
)
]
.
\dot{x}(t) = f(x(t),u(t)) = \begin{bmatrix} v_x(t) \\ v_y(t) \\ a_x(t) \\ a_y(t) \end{bmatrix}.
x˙(t)=f(x(t),u(t))=
vx(t)vy(t)ax(t)ay(t)
.
目标是使无人机的状态 (x(t)) 跟踪给定的参考轨迹 (x_{\mathrm{ref}}(t))，即误差
e
(
t
)
=
x
(
t
)
−
x
r
e
f
(
t
)
e(t) = x(t) - x_{\mathrm{ref}}(t)
e(t)=x(t)−xref(t)
尽可能小。
2. 最优控制问题的数学表达式
2.1 目标函数
构造一个二次型的性能指标，该性能指标综合了状态误差和控制代价：
J
=
∫
0
T
[
(
x
(
t
)
−
x
r
e
f
(
t
)
)
⊤
Q
(
x
(
t
)
−
x
r
e
f
(
t
)
)
+
u
(
t
)
⊤
R
u
(
t
)
]
d
t
+
(
x
(
T
)
−
x
r
e
f
(
T
)
)
⊤
Q
f
(
x
(
T
)
−
x
r
e
f
(
T
)
)
,
J = \int_{0}^{T} \left[ (x(t)-x_{\mathrm{ref}}(t))^\top Q (x(t)-x_{\mathrm{ref}}(t)) + u(t)^\top R u(t) \right] dt + (x(T)-x_{\mathrm{ref}}(T))^\top Q_f (x(T)-x_{\mathrm{ref}}(T)),
J=∫0T[(x(t)−xref(t))⊤Q(x(t)−xref(t))+u(t)⊤Ru(t)]dt+(x(T)−xref(T))⊤Qf(x(T)−xref(T)),
其中：
- (Q \in \mathbb{R}^{4 \times 4}) 是状态误差的权重矩阵，
- (R \in \mathbb{R}^{2 \times 2}) 是控制代价的权重矩阵，
- (Q_f) 为终端状态的权重矩阵，
- (T) 是规划时域。
2.2 动力学约束
无人机的连续时间动力学模型为：
x
˙
(
t
)
=
[
v
x
(
t
)
v
y
(
t
)
a
x
(
t
)
a
y
(
t
)
]
,
x
(
0
)
=
x
0
.
\dot{x}(t) = \begin{bmatrix} v_x(t) \\ v_y(t) \\ a_x(t) \\ a_y(t) \end{bmatrix}, \quad x(0) = x_0.
x˙(t)=
vx(t)vy(t)ax(t)ay(t)
,x(0)=x0.
2.3 离散化（直接法）
为了数值求解，我们通常将时间离散化为 (N) 个步长，每步时间间隔为 (\Delta t = \frac{T}{N})。
令 (x_k \approx x(k\Delta t)) 以及 (u_k \approx u(k\Delta t))，利用欧拉积分（或更高阶的积分方法，如 RK4）离散化系统：
x
k
+
1
=
x
k
+
Δ
t
f
(
x
k
,
u
k
)
,
k
=
0
,
1
,
…
,
N
−
1.
x_{k+1} = x_k + \Delta t\, f(x_k,u_k), \quad k = 0, 1, \dots, N-1.
xk+1=xk+Δtf(xk,uk),k=0,1,…,N−1.
对应的离散化目标函数为：
J
d
=
∑
k
=
0
N
−
1
[
(
x
k
−
x
r
e
f
,
k
)
⊤
Q
(
x
k
−
x
r
e
f
,
k
)
+
u
k
⊤
R
u
k
]
+
(
x
N
−
x
r
e
f
,
N
)
⊤
Q
f
(
x
N
−
x
r
e
f
,
N
)
.
J_d = \sum_{k=0}^{N-1} \left[ (x_k - x_{\mathrm{ref},k})^\top Q (x_k - x_{\mathrm{ref},k}) + u_k^\top R u_k \right] + (x_N - x_{\mathrm{ref},N})^\top Q_f (x_N - x_{\mathrm{ref},N}).
Jd=k=0∑N−1[(xk−xref,k)⊤Q(xk−xref,k)+uk⊤Ruk]+(xN−xref,N)⊤Qf(xN−xref,N).
3. ACADO（或 CasADi）求解流程
ACADO 工具箱（以及类似的工具，如 CasADi）会按如下步骤求解该最优控制问题：
-
建模与离散化
将连续时间动力学模型离散化，得到：
x k + 1 = x k + Δ t f ( x k , u k ) . x_{k+1} = x_k + \Delta t\, f(x_k,u_k). xk+1=xk+Δtf(xk,uk). -
构造目标函数
写出离散化后的目标函数 (J_d)。 -
设置约束
初始条件约束：
x 0 = x i n i t , x_0 = x_{\mathrm{init}}, x0=xinit,
以及可能的控制与状态约束（如控制输入上下限、状态范围等）。 -
求解优化问题
使用多重射击或直接法，将最优控制问题转化为一个非线性规划问题（NLP），再采用 SQP、IPOPT 等求解器求解。
4. 数学表达式总结
将上述内容整理成完整的最优控制问题数学表达式：
目标：
min
{
x
k
,
u
k
}
J
d
=
∑
k
=
0
N
−
1
[
(
x
k
−
x
r
e
f
,
k
)
⊤
Q
(
x
k
−
x
r
e
f
,
k
)
+
u
k
⊤
R
u
k
]
+
(
x
N
−
x
r
e
f
,
N
)
⊤
Q
f
(
x
N
−
x
r
e
f
,
N
)
\min_{\{x_k,u_k\}} \quad J_d = \sum_{k=0}^{N-1} \left[ (x_k - x_{\mathrm{ref},k})^\top Q (x_k - x_{\mathrm{ref},k}) + u_k^\top R u_k \right] + (x_N - x_{\mathrm{ref},N})^\top Q_f (x_N - x_{\mathrm{ref},N})
{xk,uk}minJd=k=0∑N−1[(xk−xref,k)⊤Q(xk−xref,k)+uk⊤Ruk]+(xN−xref,N)⊤Qf(xN−xref,N)
受限于：
x
k
+
1
=
x
k
+
Δ
t
f
(
x
k
,
u
k
)
,
k
=
0
,
1
,
…
,
N
−
1
,
x_{k+1} = x_k + \Delta t\, f(x_k,u_k), \quad k = 0,1,\dots,N-1,
xk+1=xk+Δtf(xk,uk),k=0,1,…,N−1,
x
0
=
x
i
n
i
t
.
x_0 = x_{\mathrm{init}}.
x0=xinit.
其中，
f
(
x
k
,
u
k
)
=
[
v
x
,
k
v
y
,
k
a
x
,
k
a
y
,
k
]
,
x
k
=
[
x
k
y
k
v
x
,
k
v
y
,
k
]
,
u
k
=
[
a
x
,
k
a
y
,
k
]
.
f(x_k,u_k)=\begin{bmatrix} v_{x,k} \\ v_{y,k} \\ a_{x,k} \\ a_{y,k} \end{bmatrix}, \quad x_k = \begin{bmatrix} x_k \\ y_k \\ v_{x,k} \\ v_{y,k} \end{bmatrix}, \quad u_k = \begin{bmatrix} a_{x,k} \\ a_{y,k} \end{bmatrix}.
f(xk,uk)=
vx,kvy,kax,kay,k
,xk=
xkykvx,kvy,k
,uk=[ax,kay,k].
这种数学表达式正是 ACADO 等最优控制工具箱求解 UAV 轨迹跟踪问题的基础。通过离散化和数值优化，我们可以获得一个随时间变化的最优控制输入序列 { u 0 , … , u N − 1 } \{u_0,\ldots,u_{N-1}\} {u0,…,uN−1}，使得无人机状态 x ( t ) x(t) x(t) 能够跟踪参考轨迹 x r e f ( t ) x_{\mathrm{ref}}(t) xref(t)。
如果需要进一步将上述数学表达式转换为可运行的代码（例如用 CasADi 实现），前面已经给出了一个详细的 Python 示例代码。这样，你既有理论数学表达式，也有相应的实现代码。
!pip install casadi
import casadi as ca
import numpy as np
import matplotlib.pyplot as plt
# ------------------------------
# 1. 问题参数设定
# ------------------------------
T = 5.0 # 总预测时域 [s]
N = 50 # 离散时间步数
dt = T / N # 每步时间
nx = 4 # 状态变量数量 [x, y, vx, vy]
nu = 2 # 控制变量数量 [ax, ay]
# ------------------------------
# 2. 参考轨迹定义
# ------------------------------
# 这里定义一个简单参考轨迹：位置随时间变化，x 均匀增加，y 为正弦曲线
tgrid = np.linspace(0, T, N+1)
x_ref = np.zeros((nx, N+1))
for k, t in enumerate(tgrid):
x_ref[0, k] = 1.0 * t # 参考 x 位置
x_ref[1, k] = np.sin(t) # 参考 y 位置
x_ref[2, k] = 1.0 # 参考 x 速度（常数）
x_ref[3, k] = np.cos(t) # 参考 y 速度（正弦曲线的导数）
# ------------------------------
# 3. 权重矩阵设定
# ------------------------------
Q = np.diag([10, 10, 1, 1]) # 状态误差权重
R = np.diag([0.1, 0.1]) # 控制代价权重
# ------------------------------
# 4. 构造最优控制问题（使用 CasADi Opti）
# ------------------------------
opti = ca.Opti()
# 变量：状态轨迹 X (nx x N+1) 和 控制轨迹 U (nu x N)
X = opti.variable(nx, N+1)
U = opti.variable(nu, N)
# 参数：初始状态
X0 = opti.parameter(nx)
# ------------------------------
# 5. 无人机动力学模型（双积分模型）
# ------------------------------
def f(x, u):
# 状态 x = [x, y, vx, vy]，控制 u = [ax, ay]
# 动力学：dx/dt = vx, dy/dt = vy, dvx/dt = ax, dvy/dt = ay
return ca.vertcat(x[2], x[3], u[0], u[1])
# ------------------------------
# 6. 动力学约束（使用欧拉积分）
# ------------------------------
# 初始条件
opti.subject_to(X[:, 0] == X0)
# 离散化动力学：X[k+1] = X[k] + dt * f(X[k], U[k])
for k in range(N):
x_next = X[:, k] + dt * f(X[:, k], U[:, k])
opti.subject_to(X[:, k+1] == x_next)
# ------------------------------
# 7. 目标函数构造
# ------------------------------
cost = 0
for k in range(N):
x_err = X[:, k] - x_ref[:, k]
cost += ca.mtimes([x_err.T, Q, x_err]) + ca.mtimes([U[:, k].T, R, U[:, k]])
# 加上末端代价
x_err_terminal = X[:, N] - x_ref[:, N]
cost += ca.mtimes([x_err_terminal.T, Q, x_err_terminal])
opti.minimize(cost)
# ------------------------------
# 8. 初始条件及求解器设定
# ------------------------------
# 初始状态（例如：起始于原点，静止状态）
x0_val = np.array([0, 0, 0, 0])
opti.set_value(X0, x0_val)
# 设置初始猜测
opti.set_initial(X, np.tile(x0_val.reshape(-1, 1), (1, N+1)))
opti.set_initial(U, 0)
# 选择 IPOPT 求解器
opti.solver('ipopt')
# ------------------------------
# 9. 求解最优控制问题
# ------------------------------
sol = opti.solve()
# 提取求解结果
X_sol = sol.value(X)
U_sol = sol.value(U)
# ------------------------------
# 10. 绘制轨迹对比图
# ------------------------------
plt.figure()
plt.plot(X_sol[0, :], X_sol[1, :], 'b-o', label='solve trajectory')
plt.plot(x_ref[0, :], x_ref[1, :], 'r--', label='ref trajectory')
plt.xlabel('x')
plt.ylabel('y')
plt.title('uav follow (CasADi solver)')
plt.legend()
plt.grid(True)
plt.show()
