Differential-Flatness Trajectory Tracking

基于微分平坦（Differential Flatness）和几何 PD 控制的四旋翼无人机轨迹跟踪仿真，使用 MuJoCo 物理引擎进行动力学模拟。

原理概述

四旋翼动力学系统具有微分平坦性：完整的系统状态（位置、速度、姿态角、角速度）和控制输入（总推力、三轴力矩）都可以由四个平坦输出及其时间导数代数推导得出：

• 位置：x, y, z
• 偏航角：ψ

项目通过 Flat 类实现了这一映射关系，结合五次样条轨迹生成和几何 PD 反馈控制，实现高精度轨迹跟踪。

项目结构

├── flat.py                          # 微分平坦映射：从平坦输出计算全状态和控制量
├── simplemath.py                    # 数学工具：四元数/欧拉角/旋转矩阵转换
├── spline5.py                       # 五次 C3 连续样条轨迹生成
├── spline_circle_main.py            # 圆形轨迹跟踪演示
├── spline_eight_main.py             # 8字形轨迹跟踪演示（固定偏航）
├── spline_eight_heading_main.py     # 8字形轨迹跟踪演示（偏航跟随速度方向）
└── wkfly/                           # MuJoCo 四旋翼模型
    ├── scene.xml                    # 场景定义
    ├── wkfly.xml                    # 飞行器模型（惯量、执行器、传感器）
    └── assets/                      # STL 网格文件

依赖

• Python 3
• mujoco — MuJoCo Python 绑定
• numpy
• matplotlib

pip install mujoco numpy matplotlib

运行

# 圆形轨迹
python spline_circle_main.py

# 8字形轨迹（固定偏航）
python spline_eight_main.py

# 8字形轨迹（偏航跟随速度方向）
python spline_eight_heading_main.py

每个脚本启动后会打开 MuJoCo 可视化窗口展示飞行过程，关闭窗口后将自动绘制跟踪误差、姿态跟随、推力/力矩分解等结果图表。

控制架构

• 前馈控制：由微分平坦根据期望轨迹直接计算理想推力和力矩
• 反馈控制：几何 PD 控制器基于 SO(3) 姿态误差修正跟踪偏差
• 控制量：总推力 u1（限幅 [0, 10] N）和三轴力矩 u_xyz（XY 限幅 ±0.4 Nm，Z 限幅 ±0.1 Nm）

飞行器参数

参数	数值
质量	0.25 kg
转动惯量 Ixx, Iyy	7.99×10⁻⁴ kg·m²
转动惯量 Izz	1.52×10⁻³ kg·m²
重力加速度	9.81 m/s²

演示轨迹

演示	轨迹方程	偏航
Circle	半径 0.8 m，高度 1.0 m，角速度 2.5 rad/s	ψ = 0
Figure-8	x = 2sin(1.2t), y = sin(2.4t), z = 0.8	ψ = 0
Figure-8 Heading	同上	ψ = atan2(v_y, v_x)