002、坐标系定义与变换基础
飞控算法从入门到精通 | 002 坐标系定义与变换基础
一次炸机事故的教训
去年调试一架四轴，飞着飞着突然朝一个方向猛冲，遥控切自稳都拉不回来。落地检查日志，发现IMU输出的加速度数据在水平悬停时居然有0.3g的Z轴分量。当时第一反应是传感器坏了，换了块MPU6050还是老样子。折腾三天，最后发现是坐标系定义搞反了——我把机体坐标系的Z轴朝下了，而姿态解算默认Z轴朝上。这个低级错误直接导致加速度计补偿方向反了180度，飞控以为飞机在倒着飞，拼命修正，结果越修越乱。
从那以后，我养成了一个习惯：任何飞控项目开工前，先花半小时把坐标系定义写死在代码注释里，贴到每个涉及坐标变换的函数头上。
飞控里到底需要几种坐标系
实际工程中，我们打交道的主要是四个坐标系。别被教科书上那些“地心地固系”“导航坐标系”绕晕，飞控里真正频繁使用的就这几个：
机体坐标系（Body Frame）
原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向下（右手定则）。注意这里Z轴朝下是航空界的惯例，和很多教材里Z轴朝上不一样。我见过有人把Z轴朝上定义，结果欧拉角计算时俯仰角正负号全反了。
地理坐标系（NED系）
北东地坐标系。X轴指北，Y轴指东，Z轴指向地心。这个坐标系是飞控的“绝对参考系”，所有位置、速度的期望值都在这个系里定义。GPS给出的经纬高数据，最终也要转换到NED系才能用。
航向坐标系（Heading Frame）
有些资料叫“机头坐标系”，其实就是机体坐标系绕Z轴旋转到水平面后的结果。这个系在计算水平速度控制时特别有用，因为你可以把期望速度分解成“向前”和“向右”，而不是“向北”和“向东”。
世界坐标系（World Frame）
严格来说，NED系就是世界坐标系的一种。但有些飞控会把原点设在起飞点，X轴指向起飞时的机头方向。这种定义在固定翼里常见，多旋翼一般直接用NED。
欧拉角：最直观也最坑爹的表示法
欧拉角是三个旋转角：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。直观，但坑多。
旋转顺序
教科书上写“Z-Y-X”顺序，但实际工程里用的是“Z-Y-X”还是“Z-X-Y”？我见过一个开源飞控，姿态解算用Z-Y-X，控制输出却按Z-X-Y算，结果横滚和俯仰耦合得一塌糊涂。这里踩过坑：一定要统一旋转顺序，推荐使用Z-Y-X（先偏航，再俯仰，最后横滚），这是航空界标准。
万向锁
当俯仰角接近±90度时，横滚和偏航会失去一个自由度。多旋翼一般不会飞到这种姿态，但做特技飞行的固定翼必须处理。解决方案是用四元数替代欧拉角做内部运算，只在需要人机交互时才转成欧拉角显示。
数值范围
横滚和俯仰的范围是±180度，偏航是0-360度。但实际控制中，偏航通常用±180度表示，因为飞控要处理最短路径转向。别这样写：把偏航限制在0-360度然后直接做PID，当目标偏航是10度，当前是350度时，误差会算成340度而不是20度。
四元数：看着复杂用着真香
四元数是个超复数，形式是q = w + xi + yj + zk，其中w是实部，x,y,z是虚部。它没有万向锁，插值平滑，计算效率高。
初始化
从欧拉角转四元数时，注意角度单位。我见过有人把度直接传进去，结果四元数模长变成1.57。这里踩过坑：所有三角函数运算必须用弧度。
更新
用陀螺仪数据更新四元数时，核心公式是：
q_new = q_old + 0.5 * dt * q_old * omega其中omega是角速度四元数（0, wx, wy, wz）。这个公式是近似解，dt不能太大。我一般限制dt不超过0.01秒，否则误差累积会漂移。
归一化
每次更新后必须归一化，否则四元数模长会逐渐偏离1，导致姿态解算发散。别这样写：只在初始化时归一化一次，后面就不管了。飞控跑久了，四元数模长能漂到1.2，姿态全乱。
旋转矩阵：坐标变换的瑞士军刀
旋转矩阵是3x3的正交矩阵，把向量从一个坐标系变换到另一个坐标系。从机体到NED的旋转矩阵C_b^n，可以用四元数或欧拉角构造。
构造方法
用四元数构造旋转矩阵效率最高：
C_b^n = [ [1-2(yy+zz), 2(xy-wz), 2(xz+wy)], [2(xy+wz), 1-2(xx+zz), 2(yz-wx)], [2(xz-wy), 2(yz+wx), 1-2(xx+yy)] ]其中w,x,y,z是四元数分量。这个矩阵是正交的，逆矩阵等于转置。
应用场景
- 加速度计数据从机体系转到NED系：a_ned = C_b^n * a_body
- 期望姿态从NED系转到机体系：att_body = (C_bn)T * att_ned
- 地磁数据从机体系转到NED系：m_ned = C_b^n * m_body
注意：旋转矩阵的精度取决于四元数的精度。如果四元数有误差，旋转矩阵也会歪。我习惯在每次姿态更新后，用旋转矩阵的列向量做一次正交化检查，如果列向量点积大于0.001，就重新正交化。
实际工程中的坐标系转换流程
以多旋翼飞控为例，典型的坐标系转换流程是这样的：
传感器数据采集
IMU输出的是机体坐标系下的加速度和角速度。GPS输出的是经纬高，需要先转成NED系下的位置和速度。姿态解算
用陀螺仪和加速度计（有时加磁力计）融合出四元数，然后转成旋转矩阵。位置估计
把加速度从机体系转到NED系，减去重力，积分得到速度，再积分得到位置。同时用GPS数据做卡尔曼滤波修正。控制计算
期望位置在NED系，实际位置也在NED系，算出位置误差后，通过PID得到期望速度（也在NED系）。期望速度再转到航向坐标系，得到期望姿态角。姿态控制
期望姿态角转成四元数，和当前四元数做差，得到姿态误差，然后通过PID输出力矩。
这里踩过坑：第4步中，期望速度转到航向坐标系时，很多人直接用偏航角做旋转，忘了考虑横滚和俯仰的影响。实际上，航向坐标系只绕Z轴旋转，所以只需要偏航角。但如果你用机体坐标系代替航向坐标系，横滚和俯仰会引入耦合，导致水平控制震荡。
调试技巧：如何验证坐标系定义正确
悬停测试
飞机水平悬停时，加速度计在机体系下的输出应该是(0, 0, -g)（Z轴朝下）。如果读到(0, 0, g)，说明Z轴方向反了。如果读到非零的水平分量，说明传感器安装有角度偏差。
旋转测试
手动绕X轴旋转飞机，观察NED系下的加速度变化。绕X轴旋转时，NED系下的Y轴加速度应该不变，Z轴加速度应该随俯仰角变化。如果Y轴也跟着变，说明旋转矩阵构造有误。
GPS航向验证
在GPS信号好的地方，让飞机朝北，观察偏航角是否接近0度。如果偏航角是180度，说明NED系的X轴定义反了。
个人经验
坐标系定义一定要写在代码里。我习惯在每个涉及坐标变换的函数开头，用注释写明“输入：机体系下的加速度，输出：NED系下的加速度”，并附上坐标系定义图（文字描述即可）。
统一使用弧度。角度和弧度的混用是飞控bug的重灾区。我所有内部运算都用弧度，只在显示和日志记录时转成角度。
四元数归一化要勤快。每次更新后、每次使用前都归一化一次。性能损失可以忽略，但能避免很多诡异问题。
旋转矩阵的正交性检查。如果发现旋转矩阵的列向量不满足正交条件（点积不为0），说明四元数或欧拉角有误差，需要重新初始化。
不要相信教科书上的坐标系定义。每个飞控框架都有自己的约定，比如PX4和ArduPilot的坐标系定义就不完全一样。移植代码时，一定要先搞清楚目标框架的坐标系定义。
最后说一句：坐标系定义看似基础，但80%的飞控调试问题都出在这里。花时间把坐标系理清楚，比花时间调PID参数划算得多。