简介多旋翼飞行器设计与控制理论第05讲《坐标系和姿态表示》课件聚焦飞行器姿态描述与坐标变换的核心方法适合无人机设计、飞行控制及相关专业学生、工程师参考学习。内容从右手定则出发系统梳理地球固联坐标系与机体坐标系的定义明确各坐标轴方向与旋转正方向的约定。深入讲解欧拉角偏航、俯仰、滚转的物理含义给出欧拉角变化率与机体角速度之间的矩阵关系式并指出俯仰角接近±90°时的万向节锁死奇异性问题及控制设计中的影响。随后引入旋转矩阵作为无奇异性的姿态表示工具详细推导旋转矩阵导数与机体角速度的关系同时介绍四元数在避免奇异性和简化计算方面的优势便于读者对比不同方法的适用场景。课件为PPTX格式共1个文件压缩包大小17.45MB结构清晰、公式推导完整既有教学演示框架也适合课后复习参考。已有183人学习是掌握多旋翼控制理论中姿态基础知识的一份实用资料。1. 多旋翼的姿态表示选错坐标系后边全是白干做过几轮多旋翼飞行器开发的人几乎都被同一个问题扎过心代码里存的姿态角到底绕哪个轴转、按什么顺序转多旋翼飞行器的姿态环、位置环、导航解算全都搭在坐标系和姿态表示上面。坐标系约定错一个符号姿态表示混用一种轻则仿真里曲线发散重则真机解锁就侧翻。这篇笔记把坐标系定义、欧拉角/旋转矩阵/四元数三套表示、姿态更新的最小实现一次理清也把仿真和真机联调时容易踩的坑摊开讲。适合正在写飞控代码、做半实物仿真、或者被姿态角跳变折磨的开发者和学生。2. 从地面系到机体系坐标变换与NED/ENU的来龙去脉坐标系是姿态表示的地基。很多开发者一上来就直接读陀螺仪、算姿态忽略了地面系和机体系的定义结果代码跑得通但换一架飞机、换一个仿真平台就全乱。这一章讲清楚两件事地面系为什么选NED、机体系的三根轴怎么定以及欧拉角旋转顺序对坐标变换矩阵的影响。2.1 为什么多旋翼用NED而不是ENUNED是北东地的缩写x轴指向北、y轴指向东、z轴指向下。多旋翼控制里用NED的理由非常实际静止时机体重力在z轴读数为正垂直速度向上为负、向下为正高度通道不用来回翻转符号。ENU东北天是很多视觉和惯性导航教材的默认约定它z轴向上和NED正好差一个负号。单独调一套飞控时感觉不到差别一旦把位置数据、加速度计数据、外部导航设备的输出放到同一个日志里对比符号不统一的问题立刻暴露。我把NED选为地面系的另一个原因是传感器接口层简单。机体系里加速度计的三轴方向固定NED下静止重力向量是(0, 0, 1)而在ENU下是(0, 0, -1)。姿态解算的修正项由这个参考向量和当前姿态预测的重力方向做叉积得到参考向量符号错了修正方向就反了姿态会往错误方向收敛。这类问题我见过太多次算得上是坐标系里的第一个血泪经验动手写解算之前先把加速度计在静止状态下的三轴读数和参考向量对好。机体系定义是x轴指向机头y轴指向右翼z轴指向下整体构成右手系。注意这里的“右”是站在飞机后面看过去的右而不是站在飞机前面看过去的右。定义写清楚之后所有绕轴旋转的正方向才有意义绕z轴正方向看是顺时针也就是偏航向右为正。2.2 欧拉角顺序与内转/外转约定姿态角描述的是机体系相对地面系的旋转。多旋翼里最常见的约定是Z-Y-X内转先绕地面系z轴转偏航再绕新的y轴转俯仰最后绕新的x轴转横滚。为什么强调“新的”因为内转intrinsic的后续旋转轴会随着前一次旋转移动外转extrinsic则始终绕地面系固定轴。同一组欧拉角数值用内转和外转解释得到的是完全不同的机体朝向。有些教材和飞控固件用Z-X-Y顺序也就是先偏航、再横滚、后俯仰。两种顺序没有对错之分但转换矩阵和四元数公式全部跟着变。我在接手一个姿态代码时第一件事查三样东西旋转顺序、内转还是外转、机体z轴朝上还是朝下。这三样定了姿态公式才有意义不定就直接算后边全是玄学。2.3 用旋转矩阵搭建机体系与地面系的桥旋转矩阵是最直白的表示三行三列把机体系里的向量映射到地面系。由Z-Y-X内转欧拉角构造时常规做法是先写绕z、y、x三个单轴的矩阵再按Rz(yaw) Ry(pitch) Rx(roll)连乘。连乘顺序不是随便写的它决定了“这个矩阵是机体系到地面系还是反过来”。下方代码给出euler_zyx_to_R_b2g函数名字里明写了方向避免调用时搞混。用这个函数可以算机头方向在地面系下的投影也可以把地面系里的期望速度转回机体系去控制方向对应好一套矩阵走遍。import numpy as np def euler_zyx_to_R_b2g(yaw, pitch, roll): Z-Y-X intrinsic机体系到地面系(NED)。 连乘顺序为 Rz Ry Rx先滚转、再俯仰、最后偏航。 cy, sy np.cos(yaw), np.sin(yaw) cp, sp np.cos(pitch), np.sin(pitch) cr, sr np.cos(roll), np.sin(roll) Rz np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) Ry np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) Rx np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) return Rz Ry Rx R_b2g euler_zyx_to_R_b2g(np.radians(30), np.radians(10), np.radians(5)) v_ground R_b2g np.array([1.0, 0.0, 0.0]) # 机头方向在NED下的投影 v_body R_b2g.T v_ground # 用转置做反向变换代码注释里我把两类向量变换都写了左乘R_b2g把机体系向量搬到地面系左乘它的转置把地面系向量搬回机体系。之所以用转置而不是求逆是因为旋转矩阵是正交矩阵转置等于逆计算量也小。做单元测试时可以先随机生成一组姿态验证R_b2g R_b2g.T近似于单位阵再验证零姿态时R_b2g等于单位阵。这两个测试过了坐标变换这块基本就稳了。提示把 np.allclose(R_b2g R_b2g.T, np.eye(3)) 写进单元测试能拦下绝大多数旋转矩阵抄错的问题。3. 姿态表示三选一欧拉角、旋转矩阵、四元数的选型与互转姿态的数据格式决定了后续所有算法怎么写。三种主流表示各有取舍欧拉角直观、旋转矩阵方便、四元数适合积分。实际工程里没有哪一种是全能的关键是知道边界在哪、转换公式怎么对齐。3.1 欧拉角直观但有万向锁弱点欧拉角用三个数表达姿态对应偏航、俯仰、横滚调试界面打印姿态、把姿态写进日志都靠它。缺点在于俯仰接近±90度时偏航和横滚的旋转轴几乎重合两个角度无法唯一区分这就是万向锁。多旋翼悬停和小机动飞行时俯仰角很少超过40度但视觉云台指向、翻滚保护、碰撞后姿态恢复这些场景会碰到。我的处理方式是姿态解算内部不用欧拉角做积分只在显示、日志、遥控指令换算时用。欧拉角积分最大的麻烦在于角度回绕yaw在±π之间跳变直接做差分或PID会把一个小误差算成一个大误差。与其在代码里到处处理wrap不如让内部状态始终用四元数。3.2 旋转矩阵无奇异但要不停矫正旋转矩阵的优势是任意姿态都能表示坐标变换时只需要一次矩阵乘法。缺点是9个元素对3个自由度而言严重冗余数值积分过程中矩阵会慢慢失去正交性和行列式为1的性质变成带缩放的仿射变换。要修正就得周期性做正交化比如Gram-Schmidt或SVD计算开销不小。飞控里的旋转矩阵常见于视觉位姿估计、坐标转换库、地面站显示姿态解算主链路很少用。如果你在写一个计算密集的实时解算循环建议优先考虑四元数如果是离线处理数据、需要频繁做坐标变换旋转矩阵反而更顺手只要记得在批量处理后做一次正交化就行。3.3 四元数无奇异、紧凑、适合积分四元数用四个数表示姿态q(w,x,y,z)模长恒为1。它没有万向锁四个数比旋转矩阵少五个数值积分后做一次归一化就能恢复约束。多旋翼飞控的内部姿态状态基本都用四元数。四元数乘法用来复合两次旋转顺序不要搞反q2 ⊗ q1 表示先施加q1、再施加q2。写代码时把乘法函数统一实现不要在展开的矩阵里到处复制公式。另一个常用操作是旋转向量给定旋转轴和转角直接构造四元数用于姿态误差计算和插值。四元数也不是完全没有坑一个姿态对应q和-q两个表示直接比较两个四元数是否相等时不作数插值时也要走球面线性插值否则会穿过姿态空间走弯路。3.4 欧拉角与四元数互转公式和两个参数陷阱以下代码把Z-Y-X内转欧拉角转成四元数再转回去。实现上用了原生numpy方便嵌入到不能依赖重型库的工程里。import numpy as np def euler_zyx_to_quat(yaw, pitch, roll): Z-Y-X intrinsic欧拉角(rad)转四元数(w,x,y,z)。 cy, sy np.cos(yaw/2), np.sin(yaw/2) cp, sp np.cos(pitch/2), np.sin(pitch/2) cr, sr np.cos(roll/2), np.sin(roll/2) w cy*cp*cr sy*sp*sr x cy*cp*sr - sy*sp*cr y cy*sp*cr sy*cp*sr z sy*cp*cr - cy*sp*sr return np.array([w, x, y, z]) def quat_to_euler_zyx(q): 四元数转欧拉角(rad)pitch用asin并做了clip。 q q / np.linalg.norm(q) w, x, y, z q pitch np.arcsin(np.clip(2*(w*y - z*x), -1.0, 1.0)) roll np.arctan2(2*(w*x y*z), 1 - 2*(x*x y*y)) yaw np.arctan2(2*(w*z x*y), 1 - 2*(y*y z*z)) return np.array([yaw, pitch, roll])两个陷阱值得单说。第一pitch用arcsin求输入必须先clip到[-1,1]因为四元数即使做了归一化浮点误差也可能让这个值变成1.0000001arcsin直接返回NaN。第二arctan2的分子分母和上面的欧拉角转四元数公式是一一对应的一旦你改了旋转顺序两个函数必须成对改不能只动一个。写完互转函数后我习惯随机生成一万组欧拉角转过去再转回来验证角度差小于某个阈值这个测试能拦下绝大多数公式抄错问题。三种表示怎么选可以用这张表对照工程里通常以四元数为主数据源欧拉角和旋转矩阵都从它派生按需输出。表示法维度奇异问题冗余主要用途计算成本欧拉角3有万向锁无显示、遥控、日志低旋转矩阵9无高需正交化坐标变换、视觉位姿中四元数4无低需归一化姿态积分、状态估计低4. 把姿态更新写进飞控从微分方程到最小解算实现姿态表示最终要落到解算循环里每个采样周期读一次陀螺仪更新一次姿态再用加速度计修正漂移。这一章从微分方程出发讲最小可用的实现以及每个参数为什么这样设。4.1 姿态微分方程角速度与四元数导数的关系四元数姿态更新的输入是机体系角速度也就是陀螺仪实测的(ωx, ωy, ωz)单位rad/s。微分方程写作 q_dot 0.5 * q ⊗ [0, ωx, ωy, ωz]这里的[0, ωx, ωy, ωz]是纯四元数把角速度当作矢量嵌入。注意ω必须是机体系下的因为陀螺仪固定在机体上测到的就是机体系角速度。工程上有两个常见错误一是把导航参考系里算出来的角速度当成机体系的直接用二是四元数乘法左右顺序写反。判断方法很直接绕机体系x轴正向转一个微小角度观察q的x分量变化方向是否和陀螺仪x轴读数一致方向一致顺序和符号就对了。4.2 一阶积分与更稳的二阶龙格库塔更新最简单的离散化是一阶欧拉q_{k1} q_k q_dot * dt。它对dt抖动敏感陀螺仪噪声大时模长容易漂。工程上我通常用二阶龙格库塔每个周期取两次斜率平均计算量只多一倍却明显改善高频噪声下的姿态稳定性。更新结束后必须归一化RK2也不例外。import numpy as np def quat_mul(q1, q2): Hamilton积q(w,x,y,z)。q1和q2按q1⊗q2计算。 w1, x1, y1, z1 q1 w2, x2, y2, z2 q2 return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2, ]) def quat_update_rk2(q, gyro_prev, gyro_cur, dt): 二阶龙格库塔更新四元数。 gyro_prev/gyro_cur上一拍与当前拍的机体系角速度(rad/s) dt采样周期(s) q q / np.linalg.norm(q) def dq_dt(qq, gyro): return 0.5 * quat_mul(qq, np.array([0.0, gyro[0], gyro[1], gyro[2]])) dq1 dq_dt(q, gyro_prev) * dt q_mid q dq1 dq2 dq_dt(q_mid, gyro_cur) * dt q_next q (dq1 dq2) / 2 return q_next / np.linalg.norm(q_next)这里把quat_mul单独抽出来了后续所有姿态运算共用避免散落多处、符号不一致。dt怎么选常见采样率是500Hz到1000Hz对应dt取0.002s或0.001s再高对精度提升有限反而放大高频噪声。另一个参数是陀螺仪量程定姿用小机动选±500°/s够用特技和快速翻滚要±1000°/s以上量程不足时读数截断姿态会在机动末端突然跳一下这种问题复盘起来特别像玄学。4.3 最小姿态解算循环预测加修正只做积分不够陀螺零偏会让姿态随时间漂移。飞控里的标准结构是预测加修正陀螺仪做预测加速度计提供重力方向参考修正预测的漂移。下面是一个裁剪到只剩核心逻辑的示意实现去掉了磁力计和增益调度用来展示姿态表示在解算里的完整链路。def quat_conj(q): return np.array([q[0], -q[1], -q[2], -q[3]]) def attitude_estimate(q, gyro, acc, dt): 最小姿态解算四元数预测 加速度计修正示意 q quat_update_rk2(q, gyro, gyro, dt) # 把NED下的参考重力(0,0,1)转到机体系和加速度计读数比较 ref_body quat_mul(quat_conj(q), quat_mul([0, 0, 0, 1], q))[1:] acc_body acc / (np.linalg.norm(acc) 1e-9) err np.cross(ref_body, acc_body) kp 0.02 # 修正增益大机动时应调小 q_corrected quat_update_rk2(q, kp * err, kp * err, dt) return q_corrected / np.linalg.norm(q_corrected)这段代码里ref_body的计算是把地面系的重力单位向量转到机体系和加速度计测到的方向做叉积得到姿态误差。kp是修正增益实际工程里不会是一个常数大机动时加速度计的读数混入线加速度修正权重要降低一般做法是根据总加速度幅值与重力幅值的接近程度动态调kp或者直接用阈值关闭修正。理解这个结构比背一版完整代码更有用——姿态表示在这里的作用是把“数值积分”和“向量观测”统一到同一个四元数状态里。5. 坐标系与姿态的五大坑从仿真翻车到真机排查下面五条是我在高强度调姿过程里反复遇到的典型问题按现象、原因、解决展开。它们不全是算法问题很多是坐标系约定和工程习惯造成的但每一条都能让姿态解算从正常变成不可用。5.1 真机起飞侧翻传感器方向与NED约定没对上现象仿真里姿态输出完全正常真机解锁后一推油门就往一侧翻。原因加速度计安装方向与机体系定义不一致重力向量投影符号反了修正环节把误差越修越大。解决把飞行器静止水平放好读加速度计三轴按NED约定重力应在z轴为正再绕每个轴正向转动飞行器检查陀螺仪读数极性。如果有偏差在驱动层加方向补偿矩阵不在解算层打补丁。上电自检时顺手打印一次“静止读数”能省掉后面查半天日志的功夫。5.2 大俯仰机动后姿态角跳变现象日志里俯仰超过80度后yaw和roll同时出现台阶式跳变。原因内部状态用欧拉角积分或者输出欧拉角时没做回绕处理。解决内部统一用四元数只在显示和日志输出时转欧拉角连续日志分析时先做unwrap再画图否则画出来的折线会有从179度跳到-179度的假象。工具脚本里加一行np.unwrap比每次都手动纠偏靠谱。5.3 悬停时yaw持续漂移现象机体不动、油门稳定yaw每分钟漂好几度。原因陀螺零偏没补偿或采样率太低导致量化噪声被积分放大。解决上电静止时采集几十秒陀螺仪数据求均值作为零偏减去如果漂移有方向一致性继续做温度补偿。零偏估计的最小实现很简单静止时段内对每一轴取均值保存为bias向量姿态更新前先执行gyro - bias。零偏是姿态估计里的老熟人了它不玄只是容易被忽略。5.4 坐标转换结果和搭档对不上现象同一个姿态你算出机头在NED下投影是(1,0,0)搭档算出来是(0,1,0)。原因坐标系一个用NED、一个用ENU或者旋转矩阵一个用R_b2g、一个用R_g2b代码注释又没写清。解决函数命名直接带方向后缀比如euler_to_R_b2g写单元测试用三个基向量做正反变换必须都还原成单位向量约定写在文件头部注释里谁改动谁负责更新。5.5 四元数转欧拉角偶发NaN现象姿态数据偶发NaN控制律打进去就崩日志很难稳定复现。原因四元数一段时间没归一化arcsin输入越过[-1,1]或出现了全零四元数。解决解析入口先归一化对arcsin入参做clip对模长接近0的四元数直接输出上一拍姿态不进更新。这条排起来最耗时间因为NaN只在特定姿态附近出现复现概率不稳定建议在代码里加一个输入校验一旦检测到非法四元数就打印完整上下文。6. 进阶用四元数误差算控制量让仿真和真机对得上姿态解算稳定之后姿态控制环需要的是期望姿态和当前姿态之间的误差。这个误差不能直接用欧拉角相减——两个都接近180度的姿态直接做差可能得到接近360度的结果PID看到的是一个巨大而错误的误差项。我一般把误差表示成四元数再转成旋转矢量旋转矢量的方向和模长都物理可解释可以直接进控制律。def quat_error_to_rotvec(q_des, q_cur): 期望姿态相对当前姿态的误差旋转矢量。 沿用前文的quat_mul与quat_conj。 q_err quat_mul(q_des, quat_conj(q_cur)) q_err q_err / np.linalg.norm(q_err) vec q_err[1:] norm_v np.linalg.norm(vec) if norm_v 1e-8: return np.zeros(3) angle 2.0 * np.arctan2(norm_v, q_err[0]) if angle np.pi: angle - 2.0 * np.pi return vec / norm_v * angle误差四元数取矢量部分得到转轴angle是转角。旋转矢量可以直接做比例控制angle作为角度误差进角度环vec方向告诉控制律该绕哪根轴修正。这个接口跨过万向锁区域依然连续不会像欧拉角误差那样在大机动或奇异姿态附近跳变。有了它视觉吊舱指向、翻滚保护、半实物仿真的一致性回归就可以共用同一个误差接口。在仿真和真机对接时我会用这个函数做一版随机姿态回归测试任意生成一组当前姿态和期望姿态沿误差矢量做一小步修正姿态应当逐步收敛到期望值任何一步发散说明坐标系定义或四元数符号约定有偏差回到第2章的三件套里查约定。我现在的习惯是每个涉及坐标系和姿态的工程新起点先花半天把坐标系定义、转换函数、单元测试三件套立好再动控制逻辑真机调试时姿态相关的问题九成都能在这个三件套里找到答案剩下一成才是传感器硬件本身的问题。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/22 12:09:11 · 阅读 1.2k 多旋翼姿态表示与坐标系：四元数、欧拉角及飞控实现详解
简介多旋翼飞行器设计与控制理论第05讲《坐标系和姿态表示》课件聚焦飞行器姿态描述与坐标变换的核心方法适合无人机设计、飞行控制及相关专业学生、工程师参考学习。内容从右手定则出发系统梳理地球固联坐标系与机体坐标系的定义明确各坐标轴方向与旋转正方向的约定。深入讲解欧拉角偏航、俯仰、滚转的物理含义给出欧拉角变化率与机体角速度之间的矩阵关系式并指出俯仰角接近±90°时的万向节锁死奇异性问题及控制设计中的影响。随后引入旋转矩阵作为无奇异性的姿态表示工具详细推导旋转矩阵导数与机体角速度的关系同时介绍四元数在避免奇异性和简化计算方面的优势便于读者对比不同方法的适用场景。课件为PPTX格式共1个文件压缩包大小17.45MB结构清晰、公式推导完整既有教学演示框架也适合课后复习参考。已有183人学习是掌握多旋翼控制理论中姿态基础知识的一份实用资料。1. 多旋翼的姿态表示选错坐标系后边全是白干做过几轮多旋翼飞行器开发的人几乎都被同一个问题扎过心代码里存的姿态角到底绕哪个轴转、按什么顺序转多旋翼飞行器的姿态环、位置环、导航解算全都搭在坐标系和姿态表示上面。坐标系约定错一个符号姿态表示混用一种轻则仿真里曲线发散重则真机解锁就侧翻。这篇笔记把坐标系定义、欧拉角/旋转矩阵/四元数三套表示、姿态更新的最小实现一次理清也把仿真和真机联调时容易踩的坑摊开讲。适合正在写飞控代码、做半实物仿真、或者被姿态角跳变折磨的开发者和学生。2. 从地面系到机体系坐标变换与NED/ENU的来龙去脉坐标系是姿态表示的地基。很多开发者一上来就直接读陀螺仪、算姿态忽略了地面系和机体系的定义结果代码跑得通但换一架飞机、换一个仿真平台就全乱。这一章讲清楚两件事地面系为什么选NED、机体系的三根轴怎么定以及欧拉角旋转顺序对坐标变换矩阵的影响。2.1 为什么多旋翼用NED而不是ENUNED是北东地的缩写x轴指向北、y轴指向东、z轴指向下。多旋翼控制里用NED的理由非常实际静止时机体重力在z轴读数为正垂直速度向上为负、向下为正高度通道不用来回翻转符号。ENU东北天是很多视觉和惯性导航教材的默认约定它z轴向上和NED正好差一个负号。单独调一套飞控时感觉不到差别一旦把位置数据、加速度计数据、外部导航设备的输出放到同一个日志里对比符号不统一的问题立刻暴露。我把NED选为地面系的另一个原因是传感器接口层简单。机体系里加速度计的三轴方向固定NED下静止重力向量是(0, 0, 1)而在ENU下是(0, 0, -1)。姿态解算的修正项由这个参考向量和当前姿态预测的重力方向做叉积得到参考向量符号错了修正方向就反了姿态会往错误方向收敛。这类问题我见过太多次算得上是坐标系里的第一个血泪经验动手写解算之前先把加速度计在静止状态下的三轴读数和参考向量对好。机体系定义是x轴指向机头y轴指向右翼z轴指向下整体构成右手系。注意这里的“右”是站在飞机后面看过去的右而不是站在飞机前面看过去的右。定义写清楚之后所有绕轴旋转的正方向才有意义绕z轴正方向看是顺时针也就是偏航向右为正。2.2 欧拉角顺序与内转/外转约定姿态角描述的是机体系相对地面系的旋转。多旋翼里最常见的约定是Z-Y-X内转先绕地面系z轴转偏航再绕新的y轴转俯仰最后绕新的x轴转横滚。为什么强调“新的”因为内转intrinsic的后续旋转轴会随着前一次旋转移动外转extrinsic则始终绕地面系固定轴。同一组欧拉角数值用内转和外转解释得到的是完全不同的机体朝向。有些教材和飞控固件用Z-X-Y顺序也就是先偏航、再横滚、后俯仰。两种顺序没有对错之分但转换矩阵和四元数公式全部跟着变。我在接手一个姿态代码时第一件事查三样东西旋转顺序、内转还是外转、机体z轴朝上还是朝下。这三样定了姿态公式才有意义不定就直接算后边全是玄学。2.3 用旋转矩阵搭建机体系与地面系的桥旋转矩阵是最直白的表示三行三列把机体系里的向量映射到地面系。由Z-Y-X内转欧拉角构造时常规做法是先写绕z、y、x三个单轴的矩阵再按Rz(yaw) Ry(pitch) Rx(roll)连乘。连乘顺序不是随便写的它决定了“这个矩阵是机体系到地面系还是反过来”。下方代码给出euler_zyx_to_R_b2g函数名字里明写了方向避免调用时搞混。用这个函数可以算机头方向在地面系下的投影也可以把地面系里的期望速度转回机体系去控制方向对应好一套矩阵走遍。import numpy as np def euler_zyx_to_R_b2g(yaw, pitch, roll): Z-Y-X intrinsic机体系到地面系(NED)。 连乘顺序为 Rz Ry Rx先滚转、再俯仰、最后偏航。 cy, sy np.cos(yaw), np.sin(yaw) cp, sp np.cos(pitch), np.sin(pitch) cr, sr np.cos(roll), np.sin(roll) Rz np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) Ry np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) Rx np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) return Rz Ry Rx R_b2g euler_zyx_to_R_b2g(np.radians(30), np.radians(10), np.radians(5)) v_ground R_b2g np.array([1.0, 0.0, 0.0]) # 机头方向在NED下的投影 v_body R_b2g.T v_ground # 用转置做反向变换代码注释里我把两类向量变换都写了左乘R_b2g把机体系向量搬到地面系左乘它的转置把地面系向量搬回机体系。之所以用转置而不是求逆是因为旋转矩阵是正交矩阵转置等于逆计算量也小。做单元测试时可以先随机生成一组姿态验证R_b2g R_b2g.T近似于单位阵再验证零姿态时R_b2g等于单位阵。这两个测试过了坐标变换这块基本就稳了。提示把 np.allclose(R_b2g R_b2g.T, np.eye(3)) 写进单元测试能拦下绝大多数旋转矩阵抄错的问题。3. 姿态表示三选一欧拉角、旋转矩阵、四元数的选型与互转姿态的数据格式决定了后续所有算法怎么写。三种主流表示各有取舍欧拉角直观、旋转矩阵方便、四元数适合积分。实际工程里没有哪一种是全能的关键是知道边界在哪、转换公式怎么对齐。3.1 欧拉角直观但有万向锁弱点欧拉角用三个数表达姿态对应偏航、俯仰、横滚调试界面打印姿态、把姿态写进日志都靠它。缺点在于俯仰接近±90度时偏航和横滚的旋转轴几乎重合两个角度无法唯一区分这就是万向锁。多旋翼悬停和小机动飞行时俯仰角很少超过40度但视觉云台指向、翻滚保护、碰撞后姿态恢复这些场景会碰到。我的处理方式是姿态解算内部不用欧拉角做积分只在显示、日志、遥控指令换算时用。欧拉角积分最大的麻烦在于角度回绕yaw在±π之间跳变直接做差分或PID会把一个小误差算成一个大误差。与其在代码里到处处理wrap不如让内部状态始终用四元数。3.2 旋转矩阵无奇异但要不停矫正旋转矩阵的优势是任意姿态都能表示坐标变换时只需要一次矩阵乘法。缺点是9个元素对3个自由度而言严重冗余数值积分过程中矩阵会慢慢失去正交性和行列式为1的性质变成带缩放的仿射变换。要修正就得周期性做正交化比如Gram-Schmidt或SVD计算开销不小。飞控里的旋转矩阵常见于视觉位姿估计、坐标转换库、地面站显示姿态解算主链路很少用。如果你在写一个计算密集的实时解算循环建议优先考虑四元数如果是离线处理数据、需要频繁做坐标变换旋转矩阵反而更顺手只要记得在批量处理后做一次正交化就行。3.3 四元数无奇异、紧凑、适合积分四元数用四个数表示姿态q(w,x,y,z)模长恒为1。它没有万向锁四个数比旋转矩阵少五个数值积分后做一次归一化就能恢复约束。多旋翼飞控的内部姿态状态基本都用四元数。四元数乘法用来复合两次旋转顺序不要搞反q2 ⊗ q1 表示先施加q1、再施加q2。写代码时把乘法函数统一实现不要在展开的矩阵里到处复制公式。另一个常用操作是旋转向量给定旋转轴和转角直接构造四元数用于姿态误差计算和插值。四元数也不是完全没有坑一个姿态对应q和-q两个表示直接比较两个四元数是否相等时不作数插值时也要走球面线性插值否则会穿过姿态空间走弯路。3.4 欧拉角与四元数互转公式和两个参数陷阱以下代码把Z-Y-X内转欧拉角转成四元数再转回去。实现上用了原生numpy方便嵌入到不能依赖重型库的工程里。import numpy as np def euler_zyx_to_quat(yaw, pitch, roll): Z-Y-X intrinsic欧拉角(rad)转四元数(w,x,y,z)。 cy, sy np.cos(yaw/2), np.sin(yaw/2) cp, sp np.cos(pitch/2), np.sin(pitch/2) cr, sr np.cos(roll/2), np.sin(roll/2) w cy*cp*cr sy*sp*sr x cy*cp*sr - sy*sp*cr y cy*sp*cr sy*cp*sr z sy*cp*cr - cy*sp*sr return np.array([w, x, y, z]) def quat_to_euler_zyx(q): 四元数转欧拉角(rad)pitch用asin并做了clip。 q q / np.linalg.norm(q) w, x, y, z q pitch np.arcsin(np.clip(2*(w*y - z*x), -1.0, 1.0)) roll np.arctan2(2*(w*x y*z), 1 - 2*(x*x y*y)) yaw np.arctan2(2*(w*z x*y), 1 - 2*(y*y z*z)) return np.array([yaw, pitch, roll])两个陷阱值得单说。第一pitch用arcsin求输入必须先clip到[-1,1]因为四元数即使做了归一化浮点误差也可能让这个值变成1.0000001arcsin直接返回NaN。第二arctan2的分子分母和上面的欧拉角转四元数公式是一一对应的一旦你改了旋转顺序两个函数必须成对改不能只动一个。写完互转函数后我习惯随机生成一万组欧拉角转过去再转回来验证角度差小于某个阈值这个测试能拦下绝大多数公式抄错问题。三种表示怎么选可以用这张表对照工程里通常以四元数为主数据源欧拉角和旋转矩阵都从它派生按需输出。表示法维度奇异问题冗余主要用途计算成本欧拉角3有万向锁无显示、遥控、日志低旋转矩阵9无高需正交化坐标变换、视觉位姿中四元数4无低需归一化姿态积分、状态估计低4. 把姿态更新写进飞控从微分方程到最小解算实现姿态表示最终要落到解算循环里每个采样周期读一次陀螺仪更新一次姿态再用加速度计修正漂移。这一章从微分方程出发讲最小可用的实现以及每个参数为什么这样设。4.1 姿态微分方程角速度与四元数导数的关系四元数姿态更新的输入是机体系角速度也就是陀螺仪实测的(ωx, ωy, ωz)单位rad/s。微分方程写作 q_dot 0.5 * q ⊗ [0, ωx, ωy, ωz]这里的[0, ωx, ωy, ωz]是纯四元数把角速度当作矢量嵌入。注意ω必须是机体系下的因为陀螺仪固定在机体上测到的就是机体系角速度。工程上有两个常见错误一是把导航参考系里算出来的角速度当成机体系的直接用二是四元数乘法左右顺序写反。判断方法很直接绕机体系x轴正向转一个微小角度观察q的x分量变化方向是否和陀螺仪x轴读数一致方向一致顺序和符号就对了。4.2 一阶积分与更稳的二阶龙格库塔更新最简单的离散化是一阶欧拉q_{k1} q_k q_dot * dt。它对dt抖动敏感陀螺仪噪声大时模长容易漂。工程上我通常用二阶龙格库塔每个周期取两次斜率平均计算量只多一倍却明显改善高频噪声下的姿态稳定性。更新结束后必须归一化RK2也不例外。import numpy as np def quat_mul(q1, q2): Hamilton积q(w,x,y,z)。q1和q2按q1⊗q2计算。 w1, x1, y1, z1 q1 w2, x2, y2, z2 q2 return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2, ]) def quat_update_rk2(q, gyro_prev, gyro_cur, dt): 二阶龙格库塔更新四元数。 gyro_prev/gyro_cur上一拍与当前拍的机体系角速度(rad/s) dt采样周期(s) q q / np.linalg.norm(q) def dq_dt(qq, gyro): return 0.5 * quat_mul(qq, np.array([0.0, gyro[0], gyro[1], gyro[2]])) dq1 dq_dt(q, gyro_prev) * dt q_mid q dq1 dq2 dq_dt(q_mid, gyro_cur) * dt q_next q (dq1 dq2) / 2 return q_next / np.linalg.norm(q_next)这里把quat_mul单独抽出来了后续所有姿态运算共用避免散落多处、符号不一致。dt怎么选常见采样率是500Hz到1000Hz对应dt取0.002s或0.001s再高对精度提升有限反而放大高频噪声。另一个参数是陀螺仪量程定姿用小机动选±500°/s够用特技和快速翻滚要±1000°/s以上量程不足时读数截断姿态会在机动末端突然跳一下这种问题复盘起来特别像玄学。4.3 最小姿态解算循环预测加修正只做积分不够陀螺零偏会让姿态随时间漂移。飞控里的标准结构是预测加修正陀螺仪做预测加速度计提供重力方向参考修正预测的漂移。下面是一个裁剪到只剩核心逻辑的示意实现去掉了磁力计和增益调度用来展示姿态表示在解算里的完整链路。def quat_conj(q): return np.array([q[0], -q[1], -q[2], -q[3]]) def attitude_estimate(q, gyro, acc, dt): 最小姿态解算四元数预测 加速度计修正示意 q quat_update_rk2(q, gyro, gyro, dt) # 把NED下的参考重力(0,0,1)转到机体系和加速度计读数比较 ref_body quat_mul(quat_conj(q), quat_mul([0, 0, 0, 1], q))[1:] acc_body acc / (np.linalg.norm(acc) 1e-9) err np.cross(ref_body, acc_body) kp 0.02 # 修正增益大机动时应调小 q_corrected quat_update_rk2(q, kp * err, kp * err, dt) return q_corrected / np.linalg.norm(q_corrected)这段代码里ref_body的计算是把地面系的重力单位向量转到机体系和加速度计测到的方向做叉积得到姿态误差。kp是修正增益实际工程里不会是一个常数大机动时加速度计的读数混入线加速度修正权重要降低一般做法是根据总加速度幅值与重力幅值的接近程度动态调kp或者直接用阈值关闭修正。理解这个结构比背一版完整代码更有用——姿态表示在这里的作用是把“数值积分”和“向量观测”统一到同一个四元数状态里。5. 坐标系与姿态的五大坑从仿真翻车到真机排查下面五条是我在高强度调姿过程里反复遇到的典型问题按现象、原因、解决展开。它们不全是算法问题很多是坐标系约定和工程习惯造成的但每一条都能让姿态解算从正常变成不可用。5.1 真机起飞侧翻传感器方向与NED约定没对上现象仿真里姿态输出完全正常真机解锁后一推油门就往一侧翻。原因加速度计安装方向与机体系定义不一致重力向量投影符号反了修正环节把误差越修越大。解决把飞行器静止水平放好读加速度计三轴按NED约定重力应在z轴为正再绕每个轴正向转动飞行器检查陀螺仪读数极性。如果有偏差在驱动层加方向补偿矩阵不在解算层打补丁。上电自检时顺手打印一次“静止读数”能省掉后面查半天日志的功夫。5.2 大俯仰机动后姿态角跳变现象日志里俯仰超过80度后yaw和roll同时出现台阶式跳变。原因内部状态用欧拉角积分或者输出欧拉角时没做回绕处理。解决内部统一用四元数只在显示和日志输出时转欧拉角连续日志分析时先做unwrap再画图否则画出来的折线会有从179度跳到-179度的假象。工具脚本里加一行np.unwrap比每次都手动纠偏靠谱。5.3 悬停时yaw持续漂移现象机体不动、油门稳定yaw每分钟漂好几度。原因陀螺零偏没补偿或采样率太低导致量化噪声被积分放大。解决上电静止时采集几十秒陀螺仪数据求均值作为零偏减去如果漂移有方向一致性继续做温度补偿。零偏估计的最小实现很简单静止时段内对每一轴取均值保存为bias向量姿态更新前先执行gyro - bias。零偏是姿态估计里的老熟人了它不玄只是容易被忽略。5.4 坐标转换结果和搭档对不上现象同一个姿态你算出机头在NED下投影是(1,0,0)搭档算出来是(0,1,0)。原因坐标系一个用NED、一个用ENU或者旋转矩阵一个用R_b2g、一个用R_g2b代码注释又没写清。解决函数命名直接带方向后缀比如euler_to_R_b2g写单元测试用三个基向量做正反变换必须都还原成单位向量约定写在文件头部注释里谁改动谁负责更新。5.5 四元数转欧拉角偶发NaN现象姿态数据偶发NaN控制律打进去就崩日志很难稳定复现。原因四元数一段时间没归一化arcsin输入越过[-1,1]或出现了全零四元数。解决解析入口先归一化对arcsin入参做clip对模长接近0的四元数直接输出上一拍姿态不进更新。这条排起来最耗时间因为NaN只在特定姿态附近出现复现概率不稳定建议在代码里加一个输入校验一旦检测到非法四元数就打印完整上下文。6. 进阶用四元数误差算控制量让仿真和真机对得上姿态解算稳定之后姿态控制环需要的是期望姿态和当前姿态之间的误差。这个误差不能直接用欧拉角相减——两个都接近180度的姿态直接做差可能得到接近360度的结果PID看到的是一个巨大而错误的误差项。我一般把误差表示成四元数再转成旋转矢量旋转矢量的方向和模长都物理可解释可以直接进控制律。def quat_error_to_rotvec(q_des, q_cur): 期望姿态相对当前姿态的误差旋转矢量。 沿用前文的quat_mul与quat_conj。 q_err quat_mul(q_des, quat_conj(q_cur)) q_err q_err / np.linalg.norm(q_err) vec q_err[1:] norm_v np.linalg.norm(vec) if norm_v 1e-8: return np.zeros(3) angle 2.0 * np.arctan2(norm_v, q_err[0]) if angle np.pi: angle - 2.0 * np.pi return vec / norm_v * angle误差四元数取矢量部分得到转轴angle是转角。旋转矢量可以直接做比例控制angle作为角度误差进角度环vec方向告诉控制律该绕哪根轴修正。这个接口跨过万向锁区域依然连续不会像欧拉角误差那样在大机动或奇异姿态附近跳变。有了它视觉吊舱指向、翻滚保护、半实物仿真的一致性回归就可以共用同一个误差接口。在仿真和真机对接时我会用这个函数做一版随机姿态回归测试任意生成一组当前姿态和期望姿态沿误差矢量做一小步修正姿态应当逐步收敛到期望值任何一步发散说明坐标系定义或四元数符号约定有偏差回到第2章的三件套里查约定。我现在的习惯是每个涉及坐标系和姿态的工程新起点先花半天把坐标系定义、转换函数、单元测试三件套立好再动控制逻辑真机调试时姿态相关的问题九成都能在这个三件套里找到答案剩下一成才是传感器硬件本身的问题。希望帮到你。本文还有配套的精品资源点击获取