2. 无人机运动学与动力学基础

三维避障与局部规划算法最终要落到无人机的运动执行上。如果对坐标系、姿态表示和刚体运动方程没有清晰认识，规划器输出的“期望位置”或“期望速度”就可能被错误地解释为机体系指令，导致避障轨迹在仿真中正确、在实机上却撞墙。本章从工程实现角度梳理无人机运动学与动力学的核心概念，为后续局部规划中的状态预测、约束建模和控制接口打下基础。

一、坐标系：从世界到机体的统一语言

无人机规划与控制中通常涉及三类坐标系。第一类是世界坐标系（World Frame，记为 W），也称惯性系或地图系，原点固定在地面某点，Z 轴垂直向上，用于表达全局地图、障碍物位置和目标点。第二类是机体坐标系（Body Frame，记为 B），原点位于无人机质心，X 轴指向机头，Y 轴指向右侧，Z 轴指向下方或上方（取决于采用 NED 还是 ENU 约定），用于表达机载传感器测量和电机推力方向。第三类是传感器坐标系（Sensor Frame），如相机、激光雷达、IMU 各自的安装坐标系，需要通过外参标定转换到机体或世界系。

工程上最容易出错的是坐标轴方向约定。例如 PX4 默认使用 NED（北-东-地）世界系和 FRD（前-右-下）机体系，而 ROS/ROS2 的 nav_msgs/Odometry 常使用 ENU（东-北-天）和 FLU（前-左-上）。两者之间不是简单取反，而是涉及轴交换和符号翻转。规划器若直接混用，会出现“向上飞却下降”“左转却右转”的典型故障。

二、姿态表示：旋转矩阵、欧拉角与四元数

姿态描述的是机体坐标系相对于世界坐标系的旋转关系。常用表示方法有三种：旋转矩阵、欧拉角和四元数。旋转矩阵是 3×3 正交矩阵，直观但冗余且易累积数值误差；欧拉角（滚转 roll、俯仰 pitch、偏航 yaw）便于人类理解，但存在万向节死锁，且插值不连续；四元数用四个数表示旋转，无死锁、插值平滑，是飞控和状态估计内部的主流表示。

在局部规划中，姿态通常不直接参与避障几何计算，但会影响速度指令的执行方向。例如，当无人机大幅俯仰时，机体 Z 轴推力在水平方向的分量会显著改变实际加速度。因此，规划器若只输出世界系速度而忽略姿态约束，可能给出超出姿态执行能力的激进轨迹。

# 欧拉角转四元数（ZYX 顺序，即 yaw-pitch-roll）
def euler_to_quaternion(roll, pitch, yaw):
    cr, sr = cos(roll/2), sin(roll/2)
    cp, sp = cos(pitch/2), sin(pitch/2)
    cy, sy = cos(yaw/2), sin(yaw/2)
    qw = cr*cp*cy + sr*sp*sy
    qx = sr*cp*cy - cr*sp*sy
    qy = cr*sp*cy + sr*cp*sy
    qz = cr*cp*sy - sr*sp*cy
    return [qw, qx, qy, qz]

三、刚体运动方程：平动与转动的耦合

无人机作为刚体，其运动方程分为平动和转动两部分。平动方程描述质心加速度与合外力之间的关系：m * a = F_thrust + F_gravity + F_drag。其中推力沿机体 Z 轴，重力沿世界 Z 轴向下，气动阻力与速度相关。转动方程描述角加速度与力矩的关系：I * ω̇ + ω × (I * ω) = τ，其中 I 为惯性张量，τ 为电机差速产生的力矩。

对于多旋翼，控制输入通常简化为四个电机转速或四个虚拟控制量：总推力 T、滚转力矩 τ_x、俯仰力矩 τ_y、偏航力矩 τ_z。局部规划器一般工作在“位置-速度-加速度”层，将期望加速度通过姿态解算转换为期望推力和期望姿态，再交给姿态控制器跟踪。这一层解耦是工程上最常用的简化，但在高机动或强扰动场景下需要更精确的动力学模型。

四、控制输入与状态估计：规划与执行的桥梁

局部规划器输出的通常是世界系下的期望位置、速度或加速度。这些量需要经过两个关键转换才能作用于电机：第一，将期望加速度转换为期望推力和期望姿态，这一步依赖当前姿态和重力方向；第二，姿态控制器根据期望姿态与估计姿态的误差计算力矩，再通过混控矩阵分配到各电机。状态估计则提供闭环所需的实时状态，包括位置、速度、姿态和角速度。

状态估计的精度直接影响避障安全性。视觉惯性里程计（VIO）在纹理丰富、光照稳定的室内表现良好，但在快速旋转或低纹理墙面附近容易漂移；激光雷达里程计在室外和结构化环境中更稳健，但成本与重量更高。规划器应结合估计协方差动态调整安全距离，而不是使用固定膨胀半径。

五、案例与误区复盘

某室内巡检项目在仿真中避障轨迹平滑，实机却频繁触发紧急悬停。排查发现，规划器使用 ENU 世界系输出速度指令，而飞控内部按 NED 解释，导致垂直方向速度符号相反。无人机在接近天花板时规划器认为“向上避让”，实际却向下加速，触发了下视避障。修正坐标变换后问题消失。

另一个常见误区是忽略姿态动力学带宽。局部规划器以 50 Hz 输出加速度指令，但姿态控制器带宽只有 5 Hz，快速变化的避障指令无法被跟踪，实际轨迹严重滞后。工程上应在规划器中加入加速度变化率约束，或降低规划频率与姿态带宽匹配。