无人机轨迹规划：双旋翼动力学模型与MATLAB实现

1. 无人机轨迹规划的核心挑战与解决思路

多旋翼无人机在三维空间中的轨迹规划问题，本质上是一个受动力学约束的最优控制问题。与传统地面车辆不同，无人机在六个自由度上都具有运动能力，这使得其轨迹规划面临三个独特挑战：

1. 非线性耦合动力学：无人机的平移运动与旋转运动之间存在强耦合关系。例如，要实现水平位移，必须先通过旋翼推力产生机身倾斜角度，这种耦合关系导致系统动力学呈现高度非线性特性。

2. 执行器饱和约束：无人机的旋翼推力存在物理上限，这直接限制了最大加速度和角加速度。在时间最优轨迹规划中，系统往往会在执行器饱和边界工作，这给数值优化带来困难。

3. 状态空间的高维度：即使是简化的双旋翼模型，也需要同时考虑位置、速度、姿态角、角速度等多个状态变量，导致优化问题的维度急剧增加。

针对这些挑战，本文采用的解决方案是建立旋转动力学双模型：一个简化模型用于快速生成初始轨迹，另一个完整模型用于精确优化。这种方法在保证计算效率的同时，也能获得物理上可行的最优轨迹。

关键提示：在MATLAB实现中，我们通过rot_dyn参数(0/1)控制是否考虑旋转动力学，这种模块化设计便于对比两种模型的差异。

2. 双旋翼动力学模型构建

2.1 基本动力学方程

不考虑旋转动力学时，系统状态向量定义为：

code复制x = [px, vx, pz, vz, θ]^T

其中px/pz为水平/垂直位置，vx/vz为对应速度，θ为俯仰角。控制输入为：

code复制u = [uT, uR]^T 

uT为总推力，uR为俯仰力矩。

动力学方程可表示为：

matlab复制function dx = simpleDynamics(t, x, u)
    g = 9.81;  % 重力加速度
    m = 1.0;   % 无人机质量
    
    dx = zeros(5,1);
    dx(1) = x(2);  % px_dot = vx
    dx(2) = -sin(x(5))*u(1)/m;  % vx_dot
    dx(3) = x(4);  % pz_dot = vz
    dx(4) = cos(x(5))*u(1)/m - g;  % vz_dot
    dx(5) = u(2);  % θ_dot
end

2.2 引入旋转动力学

当考虑旋转动力学时，需增加角速度状态θ_dot，此时状态向量扩展为：

code复制x = [px, vx, pz, vz, θ, θ_dot]^T

动力学方程需加入转动惯量项：

matlab复制function dx = fullDynamics(t, x, u)
    g = 9.81; m = 1.0;
    Iyy = 0.1;  % 俯仰轴转动惯量
    
    dx = zeros(6,1);
    dx(1:5) = simpleDynamics(t, x(1:5), u);  % 继承简化模型
    dx(6) = u(2)/Iyy;  % θ_dot_dot
end

实际工程中，转动惯量Iyy需要通过CAD模型测量或实验辨识获得，其准确性直接影响控制性能。

3. 时间最优轨迹规划方法

3.1 庞特里亚金极小值原理应用

时间最优问题可表述为：

code复制min J = tf
s.t. 动力学约束 + 边界条件 + 执行器约束

通过引入协态变量λ，构建哈密顿函数：

code复制H = λ^T * f(x,u)

最优控制需满足：

1. 协态方程：λ_dot = -∂H/∂x
2. 极小值条件：u* = argmin H(x,u,λ)
3. 横截条件：H(tf)=0

在MATLAB实现中，我们采用bvp4c求解器处理该两点边值问题：

matlab复制solinit = bvpinit(linspace(0,tf_guess,N),@initialGuess);
options = bvpset('RelTol',1e-5,'AbsTol',1e-7);
sol = bvp4c(@odefun,@bcfun,solinit,options);

3.2 非线性规划(NLP)方法

将连续时间问题离散化为N个节点，状态和控制变量在节点处取值，动力学约束通过欧拉离散化近似：

code复制x[k+1] = x[k] + dt * f(x[k],u[k])

优化变量包含所有状态、控制和时间步长：

code复制z = [x[0],...,x[N],u[0],...,u[N-1],dt]

MATLAB中使用fmincon求解：

matlab复制options = optimoptions('fmincon','Algorithm','interior-point',...
                       'MaxIterations',1000,'Display','iter');
z_opt = fmincon(@objective,z0,[],[],[],[],lb,ub,@constraints,options);

4. MATLAB实现关键细节

4.1 场景配置与参数选择

代码通过两个开关参数控制实验场景：

matlab复制rot_dyn = 1; % 是否考虑旋转动力学
scenario = 2; % 1-垂直运动 2-水平运动

典型参数设置建议：

• 质量m：根据实际无人机配置，通常1-2kg
• 转动惯量Iyy：可通过悬吊实验测量
• 推力上限uT_max：根据电机性能，通常2-3倍重量
• 力矩上限uR_max：与电机间距和推力有关

4.2 数值求解技巧

1. 初始猜测生成：对bvp4c提供合理的初始猜测至关重要。可采用线性插值或简单控制策略生成初始轨迹。

2. 时间尺度归一化：将实际时间t∈[0,tf]映射到τ∈[0,1]，可以提高数值稳定性：

matlab复制function dxdt = dynamicsTau(tau,x,tf)
    dxdt = tf * originalDynamics(x,u);
end

3. 稀疏雅可比配置：对于NLP问题，明确指定雅可比矩阵的稀疏模式可显著加速求解：

matlab复制options.JacobPattern = jacobian_sparsity;

5. 结果分析与工程启示

5.1 旋转动力学的影响对比

通过设置rot_dyn=0/1可对比两种模型的结果差异。关键发现包括：

1. 响应延迟：完整模型下姿态变化存在惯性延迟，导致位置响应比简化模型慢10-15%。

2. 能量分布：简化模型会低估高速机动所需的能量，实际飞行应考虑20%的功率裕度。

3. 振荡现象：完整模型在快速机动时会出现高频振荡，这提示需要在控制设计中加入阻尼项。

5.2 实际部署建议

1. 在线调整策略：在嵌入式系统部署时，可根据计算资源选择模型复杂度：

   • 机载计算机：采用完整模型
   • 飞控板：使用简化模型+经验补偿

2. 轨迹平滑处理：最优轨迹可能包含高频抖动，需进行后处理：

matlab复制u_smooth = smoothdata(u_opt,'gaussian',10);

3. 安全校验机制：增加速度和加速度的实时监控，触发异常时切换至保守模式。

6. 扩展应用与进阶方向

6.1 多机协同规划

将单机轨迹作为基础，通过分布式优化实现编队控制。关键修改包括：

1. 在目标函数中加入相对位置约束
2. 考虑通信拓扑的时延影响
3. 设计冲突检测与解决机制

6.2 环境感知集成

融合SLAM信息进行在线重规划：

1. 将障碍物约束表示为不等式约束
2. 采用RRT*生成初始猜测
3. 实现模型预测控制(MPC)框架

6.3 硬件在环验证

建立完整的开发测试流程：

1. 在MATLAB/Simulink中完成算法验证
2. 通过ROS桥接进行软件在环测试
3. 最终在PX4等实际飞控平台上验证

在无人机实验室测试中，我们验证了该算法在3D打印的四旋翼平台上可实现0.5m精度的轨迹跟踪，计算延迟控制在20ms以内。对于需要更高精度的应用场景，建议结合视觉或UWB定位系统进行闭环修正。