飞控算法从入门到精通 | 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(yyzz), 2(xy-wz), 2(xzwy)], [2(xywz), 1-2(xxzz), 2(yz-wx)], [2(xz-wy), 2(yzwx), 1-2(xxyy)] ]其中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参数划算得多。