多旋翼无人机时间最优轨迹规划与MATLAB实现

1. 无人机轨迹规划概述

多旋翼无人机在当今社会的应用已经渗透到各个领域，从最初的航拍摄影到如今的精准农业、三维测绘、基础设施巡检，甚至医疗物资运输等关键场景。在这些应用中，无人机的飞行性能直接决定了任务执行的效率和质量。而飞行性能的核心指标之一，就是无人机能否在最短时间内完成既定轨迹，同时保持稳定可控。

传统PID控制虽然能够实现基本的轨迹跟踪，但在面对复杂机动任务时往往显得力不从心。特别是在需要快速避障或执行高动态动作的场景下，单纯的位置控制难以充分发挥无人机的性能潜力。这就引出了我们今天要探讨的核心问题——如何为多旋翼无人机规划时间最优的飞行轨迹。

时间最优轨迹规划的本质，是在满足无人机动力学约束的前提下，找到从起点到终点耗时最短的运动路径。这听起来简单，实则涉及复杂的数学建模和优化计算。我们需要同时考虑无人机的物理限制（如最大推力、角速度限制）和运动学约束（如速度、加速度边界），还要处理各种非线性因素。

2. 双模型动力学框架构建

2.1 基本动力学模型

多旋翼无人机通常采用四旋翼或六旋翼配置，其完整动力学模型包含12个状态变量（位置、速度、姿态角、角速度）。但为了简化问题，我们可以将三维空间运动解耦，单独分析横向运动。这就引出了我们的第一个简化模型——不考虑旋转动力学的平面运动模型。

在这个简化模型中，我们主要关注四个状态变量：

• 水平位置x和垂直位置z
• 水平速度vx和垂直速度vz
• 俯仰角θ

对应的动力学方程可以表示为：

code复制ẋ = vx
ż = vz
v̇x = -(uT/m)sinθ
v̇z = (uT/m)cosθ - g
θ̇ = ω

其中uT为总推力，m为无人机质量，g为重力加速度，ω为角速度。

2.2 引入旋转动力学

虽然简化模型便于分析，但忽略了电机响应延迟和机身转动惯量等实际因素。为此，我们引入第二个模型——考虑旋转动力学的增强模型。这个模型在原有基础上增加了角速度状态变量θ̇，并引入旋转控制输入uR。

增强后的动力学方程为：

code复制θ̈ = uR/Iyy

其中Iyy表示绕y轴的转动惯量。这个新增的方程捕捉了无人机姿态变化的动态过程，使模型更接近真实物理系统。

3. 时间最优控制理论框架

3.1 庞特里亚金极小值原理

要解决时间最优控制问题，我们需要借助庞特里亚金极小值原理这一强大工具。该原理将最优控制问题转化为一组微分方程求解问题，通过引入协状态变量（costate variables）构建哈密顿函数。

对于我们的系统，哈密顿函数定义为：

code复制H = 1 + λ1*vx + λ2*vz + λ3*(-(uT/m)sinθ) + λ4*((uT/m)cosθ - g) + λ5*ω

其中λ1-λ5为协状态变量，满足：

code复制λ̇i = -∂H/∂xi

3.2 控制输入求解

根据极小值原理，最优控制输入应使哈密顿函数取极小值。通过对H关于uT和uR求导并令导数为零，我们可以得到最优控制律：

code复制uT* = {
    uT_max, 当S < 0
    uT_min, 当S > 0
}
其中S = -λ3*sinθ + λ4*cosθ

类似地，对于旋转控制输入：

code复制uR* = {
    uR_max, 当λ5 < 0
    -uR_max, 当λ5 > 0
}

这种"bang-bang"控制形式是时间最优控制的典型特征，控制输入总是在其边界值之间切换。

4. MATLAB实现详解

4.1 问题离散化处理

为了数值求解最优控制问题，我们需要将连续时间问题离散化。采用欧拉离散化方法，将时间划分为N个区间，每个区间长度为Δt（也是优化变量之一）。这样，原问题就转化为一个非线性规划(NLP)问题。

在MATLAB中，我们使用fmincon函数求解这个NLP问题。关键步骤包括：

1. 定义状态变量和控制输入序列：

matlab复制X = [x1,...,xN; vx1,...,vxN; z1,...,zN; vz1,...,vzN; θ1,...,θN];
U = [uT1,...,uTN; uR1,...,uRN];

2. 设置目标函数为总时间：

matlab复制objective = @(vars) sum(vars.dt);

3. 定义动力学约束：

matlab复制for k = 1:N-1
    ceq = [ceq; x(k+1)-(x(k)+vx(k)*dt(k))];
    ceq = [ceq; vx(k+1)-(vx(k)-(uT(k)/m)*sin(θ(k))*dt(k))];
    % 其他状态变量类似...
end

4.2 旋转动力学实现

当考虑旋转动力学时，需要在状态变量中增加θ̇，并添加对应的动力学约束：

matlab复制if rot_dyn == 1
    X = [X; thetadot1,...,thetadotN];
    for k = 1:N-1
        ceq = [ceq; thetadot(k+1)-(thetadot(k)+uR(k)/Iyy*dt(k))];
    end
end

4.3 结果可视化

仿真完成后，我们可以绘制状态轨迹和控制输入曲线：

matlab复制figure(1)
plot(tt, px, 'linewidth', 2);
hold on
plot(tt, vx, 'linewidth', 2);
% 其他状态变量...
legend({'$x^*$','$\dot{x}^*$','$z^*$'},...
    'Interpreter','latex','Fontsize',15);

5. 仿真结果对比分析

5.1 不考虑旋转动力学的案例

在简单模型中，无人机可以瞬时改变姿态角θ。仿真结果显示，控制输入uT和uR呈现典型的bang-bang特性，状态轨迹平滑但存在突变点。这种理想化模型虽然计算简单，但实际无人机由于转动惯量限制无法实现这种瞬时响应。

5.2 考虑旋转动力学的案例

引入旋转动力学后，姿态角变化率θ̇成为状态变量，控制输入uR直接影响θ̇而非直接控制θ。仿真结果显示：

1. 状态轨迹变得更加平滑，没有突变点
2. 完成相同机动所需时间增加约15-20%
3. 控制输入切换频率降低，更符合实际执行器特性

特别值得注意的是，在考虑旋转动力学后，最大俯仰角的到达时间明显延后，这与实际飞行体验一致——无人机需要时间来完成姿态调整。

6. 工程实践建议

6.1 模型选择原则

1. 对于低速、小角度机动任务，可以使用简化模型快速获得近似解
2. 对于高动态、大角度机动（如翻转、急转弯），必须使用完整旋转动力学模型
3. 在初步设计阶段可先用简化模型确定性能边界，再用完整模型进行验证

6.2 参数调优技巧

1. 质量m和转动惯量Iyy的准确测量至关重要，误差超过10%会显著影响规划结果
2. 控制输入边界(uT_max, uR_max)应留有一定余量（建议15-20%），以应对模型不确定性
3. 离散化步数N的选择需要权衡计算精度和速度，通常100-200步可获得良好平衡

6.3 实际部署注意事项

1. 将离线规划得到的轨迹作为参考输入，配合在线跟踪控制器使用
2. 考虑执行器延迟（约50-100ms），在实际控制中引入相应补偿
3. 定期更新模型参数，特别是电池消耗导致的重量变化

7. 扩展应用方向

本文介绍的方法可以扩展到更复杂的场景：

1. 三维空间轨迹规划：增加横滚和偏航通道
2. 多无人机协同规划：引入避碰约束
3. 动态避障：将障碍物表示为时变约束
4. 能量最优规划：修改目标函数为能量消耗最小

在MATLAB实现上，这些扩展主要涉及约束条件的增加和目标函数的修改，核心优化框架保持不变。例如，多机避碰可以通过添加如下约束实现：

matlab复制for i = 1:N_drones
    for j = i+1:N_drones
        ceq = [ceq; norm(pos_i-pos_j) > safe_distance];
    end
end

8. 常见问题排查

8.1 优化不收敛

可能原因及解决方案：

1. 初始猜测不合理：使用线性插值生成初始轨迹
2. 约束冲突：逐步放松约束条件，找到可行解
3. 步长过大：减小离散化步长或增加步数

8.2 轨迹震荡

解决方案：

1. 在目标函数中添加控制变化率惩罚项
2. 增加动力学约束的平滑性要求
3. 检查控制输入边界是否设置过小

8.3 计算时间过长

优化策略：

1. 使用并行计算加速梯度计算
2. 采用warm-start策略，用上一次的解初始化新问题
3. 尝试不同的求解算法（interior-point, SQP等）

9. 代码优化建议

1. 向量化运算：避免循环，使用矩阵运算

matlab复制% 不佳实现
for k = 1:N-1
    x(k+1) = x(k) + vx(k)*dt(k);
end

% 优化实现
x(2:N) = x(1:N-1) + vx(1:N-1).*dt(1:N-1);

2. 使用解析梯度：提供自定义梯度函数，大幅提升求解速度
3. 缓存计算结果：特别是重复使用的中间变量
4. 采用稀疏矩阵：处理大规模约束时显著减少内存使用

10. 不同场景配置示例

10.1 垂直起降场景

matlab复制rot_dyn = 0; % 简化模型
scenario = 1; % 垂直运动
initial_guess.x = linspace(0, 10, N);
initial_guess.z = linspace(0, 5, N);

10.2 水平避障场景

matlab复制rot_dyn = 1; % 完整模型
scenario = 2; % 水平运动
% 添加障碍物约束
for k = 1:N
    ceq = [ceq; (x(k)-obs_x)^2 + (z(k)-obs_z)^2 - obs_r^2];
end

在实际应用中，我发现将理论最优轨迹与实测数据进行比对非常重要。通过记录实际飞行数据并反推等效模型参数，可以不断修正理论模型，使规划结果更加准确。这个过程通常需要3-5次迭代才能获得满意的匹配精度。