MPC用优化求解器 - 解决无人机轨迹跟踪

一个较为完整的数学表达式描述，展示如何使用最优控制方法（ACADO 思想）解决无人机轨迹跟踪问题。这里以二维无人机（简化为双积分模型）为例，其数学表达式如下：

1. 问题描述

无人机在平面内的运动可以用状态变量
$\begin{bmatrix} x(t) \\ y(t) \\ v_x(t) \\ v_y(t) \end{bmatrix} \in \mathbb{R}^4$
和控制输入
$\begin{bmatrix} a_x(t) \\ a_y(t) \end{bmatrix} \in \mathbb{R}^2$
描述。其动力学方程为
$\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)) 跟踪给定的参考轨迹 (x_{\mathrm{ref}}(t))，即误差
$x_{\mathrm{ref}}(t)$
尽可能小。

2. 最优控制问题的数学表达式

2.1 目标函数

构造一个二次型的性能指标，该性能指标综合了状态误差和控制代价：
$\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)),$
其中：

(Q \in \mathbb{R}^{4 \times 4}) 是状态误差的权重矩阵，
(R \in \mathbb{R}^{2 \times 2}) 是控制代价的权重矩阵，
(Q_f) 为终端状态的权重矩阵，
(T) 是规划时域。

2.2 动力学约束

无人机的连续时间动力学模型为：
$\dot{x}(t) = \begin{bmatrix} v_x(t) \\ v_y(t) \\ a_x(t) \\ a_y(t) \end{bmatrix}, \quad x(0) = x_0.$

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 + \Delta t\, f(x_k,u_k), \quad k = 0, 1, \dots, N-1.$

对应的离散化目标函数为：
$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}).$

3. ACADO（或 CasADi）求解流程

ACADO 工具箱（以及类似的工具，如 CasADi）会按如下步骤求解该最优控制问题：

建模与离散化
将连续时间动力学模型离散化，得到：
$x_{k+1} = x_k + \Delta t\, f(x_k,u_k).$
构造目标函数
写出离散化后的目标函数 (J_d)。
设置约束
初始条件约束：
$x_0 = x_{\mathrm{init}},$
以及可能的控制与状态约束（如控制输入上下限、状态范围等）。
求解优化问题
使用多重射击或直接法，将最优控制问题转化为一个非线性规划问题（NLP），再采用 SQP、IPOPT 等求解器求解。

4. 数学表达式总结

将上述内容整理成完整的最优控制问题数学表达式：

目标：
$\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})$

受限于：
$x_{k+1} = x_k + \Delta t\, f(x_k,u_k), \quad k = 0,1,\dots,N-1,$
$x_0 = x_{\mathrm{init}}.$

其中，
$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}.$

这种数学表达式正是 ACADO 等最优控制工具箱求解 UAV 轨迹跟踪问题的基础。通过离散化和数值优化，我们可以获得一个随时间变化的最优控制输入序列 $\{u_0,\ldots,u_{N-1}\}$ ，使得无人机状态 $x (t)$ 能够跟踪参考轨迹 $x_{\mathrm{ref}}(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 = ayreturn 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()