一个较为完整的数学表达式描述，展示如何使用最优控制方法（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 示例代码。这样，你既有理论数学表达式，也有相应的实现代码。
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()