【无人机控制】四旋翼无人机经典双闭环反馈轨迹跟踪控制

  本人之前硕士主攻方向为机械臂控制，后面由于升学换了方向，现在主要学习飞行器的模态辨识和主动控制器设计。其实对于咱们控制领域的人来讲，重要的还是控制方法的学习，以及附带学习一些优化算法、辨识方法和一些机器学习（如强化学习、神经网络等）来更好的辅助提升控制器性能，毕竟咱们不是学结构设计的，从这个角度来讲，所面对的对象其实就是从一个二阶系统换成了另外一个二阶系统。

固定翼和四旋翼均属于飞行器一类，为了拓宽知识面，对四旋翼的的经典控制方法进行了学习。

1. 四旋翼无人机动力学模型

1.1 建模假设

• 刚体假设：机体为刚体，质量分布均匀。
• 对称结构：四个旋翼对称分布于机体中心，臂长相等。
• 忽略空气阻力（或简化为线性阻尼）。
• 旋翼仅提供垂直于旋翼平面的升力，无侧向力。

1.2 坐标系定义

• 惯性坐标系（世界坐标系）：{E} = (X, Y, Z)，Z轴向上。
• 机体坐标系：{B} = (x, y, z)，此处采用 x 前，y 左，z 上（右手系）

1.3 状态变量

• 位置：$\mathbf{p}={\left[x,y,z\right]}^T$
• 线速度：$\dot{\mathbf{p}}={\left[v_x,v_y,v_z\right]}^T$
• 姿态角（欧拉角）：$\mathbf{\eta }={\left[\varphi ,\theta ,\psi \right]}^T$（滚转、俯仰、偏航）
• 角速度：$\mathbf{\omega }={\left[p,q,r\right]}^T$
• 控制输入（四个电机推力）：$\mathbf{u}={\left[u_1,u_2,u_3,u_4\right]}^T$

1.4 动力学方程

  具体推导过程省略，直接给出最终的表达式如下：

• 位置加速度（平动）：$$\{\begin{array}{l}\ddot{x}=\frac{u_1}{m}\left(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi \right)\\ \ddot{y}=\frac{u_1}{m}\left(\cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi \right)\\ \ddot{z}=\frac{u_1}{m}\cos \varphi \cos \theta -g\end{array}$$
• 姿态变化率（运动学）：$$\{\begin{array}{l}\dot{\varphi }=p+\left(q\sin \varphi +r\cos \varphi \right)\tan \theta \\ \dot{\theta }=q\cos \varphi -r\sin \varphi \\ \dot{\psi }=\frac{q\sin \varphi +r\cos \varphi }{\cos \theta }\end{array}$$
• 角加速度（转动）：$$\{\begin{array}{l}\dot{p}=\frac{I_y-I_z}{I_x}qr+\frac{1}{I_x}{u}_2\\ \dot{q}=\frac{I_z-I_x}{I_y}pr+\frac{1}{I_y}{u}_3\\ \dot{r}=\frac{I_x-I_y}{I_z}pq+\frac{1}{I_z}{u}_4\end{array}$$
    以上为四旋翼无人机的完整非线性方程，如需进行悬停控制可根据小角度近似理论在悬停平衡点进行线性化得到线性模型进而设计悬停控制器即可。

2. 双闭环控制器设计

  对于较大幅度、高速度的三维空间正弦运动，必须基于非线性模型设计控制器，此时平衡点线性模型不再适用。目前工程上经典的四旋翼无人机控制方法为分层控制，外环——位置控制（基于非线性模型），内环——姿态控制（期望姿态小角度范围内可用线性模型）。
  外层位置变化慢；内层姿态角变化快；位置与姿态之间存在非线性耦合；内外环分离巧妙解耦了非线性问题；极大提升了工程可行性与鲁棒性。
  双闭环控制结构如下图所示：

  外环控制器设计（根据期望位置计算期望加速度和期望姿态）：给定期望轨迹，通过轨迹规划算法获取其一阶导数（速度）和二阶导数（加速度）。利用这些信息，结合当前状态估计误差，设计滑模控制律或其他类型控制器并使用完整旋转矩阵 $R$ 来生成期望加速度 ${\mathbf{a}}_d$，随后可计算得到期望推力 $T_d$；随后根据推力可反解得到期望姿态角 $\left[{\varphi }_d,{\theta }_d\right]$ ，通常假设期望偏航角 ${\psi }_d$为0。
  内环控制器设计：用于跟踪期望姿态角。可以采用 LQR 控制器或 PID 控制器来调节滚转角 ϕ、俯仰角 θ 和偏航角 ψ 至期望值。
  整合内外环控制器：将外环生成的期望加速度转化为期望姿态，并传递给内环。内环输出控制信号至执行机构，完成闭环控制。

3. 仿真分析

  考虑如下参数的四旋翼无人机模型$$m=1,I_x=0.015,I_y=0.015,I_z=0.03$$期望轨迹为三维正弦轨迹：$$\{\begin{array}{l}{x}_d\left(t\right)=2\sin \left(0.5t\right)\\ y_d\left(t\right)=2\cos \left(0.5t\right)\\ z_d\left(t\right)=5+\sin \left(0.3t\right)\end{array}$$
控制器采用：

• 外环滑模控制：滑模面 $\mathbf{s}=\lambda \mathbf{e}+\dot{\mathbf{e}}$，引入虚拟控制律 ${\mathbf{a}}_{\mathrm{d}}={\ddot{\mathbf{r}}}_d-\lambda \dot{\mathbf{e}}-K_{sw}\cdot \mathrm{sat}\left(\mathbf{s}\right)$。
• 内环LQR控制器：基于线性模型设计反馈矩阵K，角度权重设为较大的值。

搭建的simulink仿真框图如下所示：

仿真分析

  对位置正弦参考信号的跟踪效果如下所示：

总结

  综上所述，通过设计双闭环控制器实现了四旋翼无人机三维正弦信号的跟踪控制，动态响应好，控制精度高。双闭环控制的特点在于：

• 结构清晰、模块化：外环负责轨迹跟踪（位置/速度），内环负责快速稳定姿态。
• 解耦复杂非线性系统：将 6-DOF 强耦合非线性系统分解为两个较易处理的子系统：位置动力学（慢） + 姿态动力学（快）
• 分层带宽设计：内环带宽 >> 外环带宽（通常 5~10 倍），使外环可将内环视为“理想跟随器”，简化控制器设计。
• 鲁棒性增强：内环抑制高频扰动（如风扰、电机噪声），外环专注低频轨迹误差，提升整体抗干扰能力。
• 工程通用性强：是 PX4、ArduPilot、DJI 等主流飞控的标准架构，具有高度实用性。

需要注意的点在于：

• 坐标系与符号一致性：动力学模型中的推力方向 必须与 控制器中期望合力方向 一致。最好统一为标准右手系：x前, y左, z上。
• 期望姿态角解算必须精确：禁止使用小角度近似。
• 防止数值奇异与溢出，解算期望姿态角时须在 asin 前限幅。