做过多旋翼底层飞控的人应该都有体会位置环的输出不是舵量不是转速而是一个期望姿态。飞控要把“水平方向上该加速多少、垂直方向上该爬升多少”翻译成机体的倾斜方向和倾斜角度这个“方向加角度”在数学上最稳的表达就是期望姿态矩阵。PX4多旋翼期望姿态矩阵生成算法就是把位置控制得到的期望加速度和期望偏航角转换成姿态环能直接消费的期望四元数或期望旋转矩阵的那一步。网上讲PX4姿态控制的文章不少但大多数直接跳到四元数PID很少有人把这一小段单独拆开讲。这篇文章打算补齐这个缺口先讲它到底在解什么数学问题再给一份能落地的伪代码最后聊聊参数和实测里常见的坑。想改底层控制行为、研究位置环和姿态环接口的人应该能从里面拿到真正能用的东西。1. 期望姿态矩阵到底是什么先理清它在整条控制链路中的位置1.1 位置控制到姿态控制的“翻译”过程PX4多旋翼的底层控制是典型的串级结构外环是位置控制接收期望位置、期望速度输出期望加速度中间这一层就是期望姿态生成把期望加速度和期望偏航角变成期望姿态矩阵内环才是姿态控制接收期望姿态输出期望角速率最底层是角速率环输出力矩给电机。这中间最容易被忽略的一环就是“期望加速度到期望姿态”的翻译过程。你可以把外环想象成导航员内环是执行者。导航员说“你现在水平方向欠了一个向右的加速度”但执行者听不懂“加速度”它只听得懂“把身体向右倾斜多少度”。期望姿态矩阵生成算法就是那个翻译官。这里有个很重要的认知期望姿态矩阵不是直接从期望位置算出来的而是从加速度、推力向量、机体z轴一步步构造出来的。为什么不用三个欧拉角因为多旋翼在做大机动时俯仰和横滚会耦合欧拉角表述容易出现万向锁和优先级不清的问题。为什么不用纯四元数公式硬解因为从加速度反推姿态旋转矩阵的物理意义最直观每一列就是机体三个坐标轴在地轴系下的投影。所以实际工程里常见的做法是先生成旋转矩阵再转成四元数交给姿态环。这套思路在PX4里贯穿得相当彻底。1.2 期望加速度从哪来内环外环的分工期望加速度不是凭空出现的。位置控制器会把位置误差、速度误差、前馈项加在一起输出一个三轴的期望加速度向量。水平通道给水平加速度垂直通道给垂直加速度手动模式或外部指令则直接提供速度或加速度设定值。无论哪种来源最终都会汇成一个acc_sp向量。但这里有一个非常容易踩雷的细节为了让飞机悬停时不飘推力向量不能简单等于acc_sp。位置控制器输出的期望加速度通常要经过重力补偿才能得到真正的推力方向。如果不做这一步悬停时推力向量会是零向量后面一归一化就直接出问题。我在一些二次开发的代码里见过有人没把重力补偿算进位置控制器结果飞机一解锁就往一个方向窜日志里期望姿态矩阵的第一列全是乱的问题根源其实就是坐标系和重力符号处理错了。另外要意识到期望加速度的限幅直接决定了期望倾斜角。位置控制器通常会限制最大水平加速度、最大上升和下降加速度这些限制会反映到推力向量上。如果限制太激进期望姿态会频繁请求大倾角姿态环响应不过来整个飞控看起来就像“反应过度”。所以看期望姿态矩阵生成问题不能只看生成那几行代码还得连带看位置控制器的输出限幅。1.3 为什么“矩阵”这件事值得单独写一笔很多人看PX4的代码看到某个函数返回四元数就以为完事了没有去追究中间为什么先拼出一个3x3矩阵。矩阵的核心价值在于它用非常直观的方式表达了“期望机体坐标系”在地轴系下的完整朝向。旋转矩阵的三列分别是机体x轴、y轴、z轴在地轴系下的单位向量。这意味着你可以直接读出机头朝哪、机腹朝哪检查正交性也简单。四元数在迭代运算里很高效但它不直观数值漂移后也难以快速定位。所以把期望姿态先生成矩阵、再转四元数是一种兼顾工程调试和计算效率的设计。理解这一点对后面排查姿态问题特别有用。比如飞机出现机头方向偏差时你可以从日志里的期望旋转矩阵第一列直接看出来期望机头到底指向哪。如果只用四元数日志很多时候你得先在脑内做一次四元数到欧拉角的转换才能判断麻烦得多。2. 数学推导一个期望旋转矩阵的三步构造法2.1 第一步用推力方向定机体z轴多旋翼的机体z轴在NED坐标系里定义为从机体重心指向机腹也就是向下。推力方向则相反沿机体z轴负方向。已知期望推力向量之后机体z轴方向就是推力方向取反归一化。这里我先统一约定假设位置控制器给出的acc_sp是“期望总加速度”那么经过重力补偿后的推力向量可以写成thrust acc_sp - g_vec其中g_vec是NED系下的重力向量通常写作(0, 0, 9.81)。悬停时acc_sp是零向量推力向量就等于(0, 0, -9.81)方向向上完全合理。如果飞机需要水平加速度thrust就会偏斜平台自然要往对应方向倾斜。得到thrust之后归一化thrust_unit thrust / ||thrust|| z_axis -thrust_unit注意z_axis是期望机体z轴的指向不是推力方向本身。很多初学者在这里会搞反飞机要往右加速机体不是往右倾而是往左倾因为往左倾才产生向右的推力分量。这个直觉虽然反直觉但完全符合动力学。实际代码里归一化之前一定要防除零float norm thrust.norm(); if (norm 1e-6f) { // 推力太小姿态没有意义直接返回当前姿态或水平姿态 } z_axis -thrust / norm;还有一种情况是不同源码里acc_sp的定义不一样。有的位置控制器已经把重力扣除掉了有的没有。这会导致thrust公式里的正负号不同。我在实际项目里见到的错误十有八九出在这个符号上。最好的验证方式是做悬停静态测试飞机水平解锁时日志里的z_axis应该接近(0, 0, 1)。如果反了飞机会带着倾斜角去“纠正”一个根本不存在的错误整机表现就是没法稳定悬停。2.2 第二步用偏航角定机体x轴并做正交化有了z轴旋转矩阵还缺两个轴。但四旋翼绕z轴旋转偏航不改变推力方向所以必须额外给定一个期望偏航角来确定机头指向。给定期望偏航角yaw_sp后先在地轴系水平面上构造一个候选前向向量x_c [cos(yaw_sp), sin(yaw_sp), 0]这个向量表达的是如果机体完全水平机头应该指向的方向。但问题来了机体倾斜后x_c和z_axis不一定垂直。旋转矩阵的三列必须两两正交、单位长度不能直接拿x_c当矩阵第一列。标准做法是 Gram-Schmidt 正交化x_axis x_c - z_axis * (z_axis.dot(x_c)) x_axis.normalize()这里的几何含义是把x_c沿z_axis方向的分量减掉剩下的部分就是与机体z轴垂直的“前向”分量。正交化之后机体x轴在地平面上的投影方向依然会尽量靠近期望偏航角。这个操作是整个算法里最核心的一步也是最需要理解的一步。要注意如果z_axis恰好与x_c平行正交化后x_axis会接近零向量这时候偏航自由度退化了。工程上不会让飞机真的倾斜到90度去追一个水平加速度因为最大倾斜角会限制在远小于90度的范围但数值上还是可能出现这个分支。真实代码里通常会对x_axis.norm()做一个阈值检查一旦退化就改用上一帧的姿态信息来构造x轴避免机头瞬间乱跳。2.3 第三步补完y轴、组装矩阵、转四元数有了单位正交的z_axis和x_axis第三列不能用独立构造必须用叉乘保证坐标系是右手系。在NED坐标系中机体x轴向前、y轴向右、z轴向下满足x × y z。因此y_axis z_axis.cross(x_axis)这里需要特别注意叉乘顺序。如果写反整个坐标系会变成左手系姿态矩阵的行列式变成-1飞控会直接疯掉。我调试时遇到过飞机在原地翻滚的情况最后排查发现就是某一行代码把z_axis.cross(x_axis)写成了x_axis.cross(z_axis)。组装旋转矩阵Matrix3f R_des; R_des.col(0) x_axis; R_des.col(1) y_axis; R_des.col(2) z_axis;矩阵组装好后再转成四元数Quaternionf q_des(R_des); q_des.normalize();很多矩阵库在从旋转矩阵构造四元数时已经做了处理但建议还是再归一化一次。矩阵到四元数的转换对数值误差比较敏感如果矩阵不是严格正交得到的四元数可能不够干净。归一化这一步代价很低能省去后面很多奇怪的小角度漂移。2.4 矩阵转四元数的工程细节旋转矩阵转四元数并不是直接把四个分量读出来那么简单。经典方法是从矩阵对角元素计算w sqrt(1 R00 R11 R22) / 2但为了避免数值开方时的符号问题很多库会先判断最大对角元素再选择不同的计算分支。Eigen 的Quaternionf(R_des)内部已经处理好了用起来很方便。如果自己实现一定要小心当旋转角接近180度时四元数的实部接近0这时候只用一个简单公式容易产生数值灾难。另一个工程细节是旋转矩阵通常在每拍都从公式重建理论上不会累积误差。但如果你的代码把R_des当状态存在成员变量里下一拍在上一拍基础上做小量修正那么浮点误差就会慢慢累积。能看到的现象是矩阵的行列式逐渐偏离1姿态开始出现缓慢漂移。解决方法是定期做一次 Gram-Schmidt 重正交化或者直接把R_des每拍都重新生成不要保存长期状态。3. 工程实现能落地的伪代码与边界条件处理3.1 一份可直接改造的C参考实现下面给一份我自己整理过的C伪代码和PX4里常见实现思路接近。为了阅读方便省略了模式切换、参数读取和异常保护细节。#include cmath struct DesiredAttitude { Quaternionf q_des; float thrust_norm; }; DesiredAttitude computeDesiredAttitude( const Vector3f acc_sp, float yaw_sp, float max_tilt_rad, float dt) { DesiredAttitude out; // 1. 重力补偿得到期望推力向量 // 注意acc_sp 按“期望总加速度”理解 const Vector3f g_vec(0.0f, 0.0f, 9.81f); Vector3f thrust acc_sp - g_vec; float thrust_norm thrust.norm(); if (thrust_norm 1e-4f) { // 推力几乎为零保持水平姿态或由上层决定保持当前姿态 out.q_des Quaternionf::Identity(); out.thrust_norm 0.0f; return out; } // 2. 期望机体z轴 Vector3f z_axis -thrust / thrust_norm; // 3. 期望偏航角投影到水平面再做正交化 Vector3f x_c(std::cos(yaw_sp), std::sin(yaw_sp), 0.0f); Vector3f x_axis x_c - z_axis * (z_axis.dot(x_c)); if (x_axis.norm() 1e-4f) { // 偏航自由度退化用当前y轴叉乘z轴兜底 // 稳妥做法是传上一帧的x_axis进来避免机头瞬跳 x_axis Vector3f::UnitY().cross(z_axis); } x_axis.normalize(); // 4. 补完y轴组装旋转矩阵 Vector3f y_axis z_axis.cross(x_axis); Matrix3f R_des; R_des.col(0) x_axis; R_des.col(1) y_axis; R_des.col(2) z_axis; // 5. 转四元数 out.q_des Quaternionf(R_des).normalized(); out.thrust_norm thrust_norm; return out; }这段代码里有几个地方值得展开说。第一thrust_norm被返回不是没用的。它就是油门前馈用的“该给多大力”的信号。姿态矩阵只解决了“往哪偏”没解决“用多大推力”。如果只把期望姿态传给姿态环却丢掉推力大小动力响应会慢半拍垂直方向尤其明显。第二退化分支里我写的是Vector3f::UnitY().cross(z_axis)这只是一个兜底不包含任何偏航跟踪。真正工程上应该把这个分支设计成“保持上一拍x轴方向”或者“基于当前姿态的y轴重新构造x轴”否则飞机可能在退化的瞬间发生机头跳变。第三如果你的控制器里acc_sp不包含重力补偿而是“扣除重力后的控制加速度”那第一步就要改成thrust acc_sp g_vec或者thrust -acc_sp - g_vec具体符号要根据你的坐标定义来定。不要照抄如果直接跑一个悬停仿真就能验证。3.2 夹角限制、奇异退化和数值正交化真实飞控不会让z_axis随便指。如果期望水平加速度太大z_axis会过分靠近水平面带来两个连锁问题一是升力垂直分量不足容易掉高二是正交化时数值精度变差。所以位置控制器必须在生成thrust之前先把期望加速度的水平分量限制在一个安全范围。工程上常用最大倾斜角max_tilt来反推最大水平加速度max_hor_acc tan(max_tilt) * 9.81例如最大倾斜角45度时最大水平加速度约等于1g。这个换算关系在PX4里不会直接写出来而是通过MPC_TILTMAX_AIR、MPC_MAN_TILT_MAX等参数间接控制。如果你自定义过位置控制器最好把这个限幅加在姿态生成之前而不是靠姿态环去兜底。关于奇异退化还有一个容易被忽略的场景垂直起降阶段。这时期望加速度几乎只有垂直分量z_axis几乎贴合(0,0,1)偏航自由度理论上不收敛。如果外部偏航信号有一点噪声x_axis就会绕着z轴漂表现为机头慢转。处理办法是检测z_axis与垂直轴的夹角小于某个阈值时不再直接使用外部yaw_sp而是沿用上一拍的偏航角等推力方向明显倾斜后再逐步切回外部给定。数值正交化方面虽然每一拍都是从公式重建矩阵但旋转矩阵转四元数再转回来、或者中间做了插值都可能导致轻微非正交。如果你把q_des传给姿态环前不确定矩阵干不干净可以显式做一次重正交化。代码量不大但长期运行的稳定性会好很多。3.3 从期望矩阵到姿态控制误差下游怎么用期望姿态矩阵生成之后姿态控制器要做的是计算“从当前姿态到期望姿态”的最短旋转误差。最常见的做法是用四元数q_err q_cur.inverse() * q_des然后提取旋转向量在小角度近似下rate_sp 2.0f * q_err.vec()这个rate_sp就是期望角速率交给角速率环执行。很多控制代码会用旋转向量来做比例控制效果比直接对欧拉角做PID要好因为旋转向量天然表达了绕哪个轴转多少。如果你更喜欢直接用矩阵也可以用三对轴的叉积和来构造误差Vector3f err R_des.col(0).cross(R_cur.col(0)) R_des.col(1).cross(R_cur.col(1)) R_des.col(2).cross(R_cur.col(2));这个误差量与四元数误差是等价的只是表现形式不同。理解“期望矩阵只是中间产物下游还要算误差”这一点非常重要。如果只改期望姿态生成而不看下游很容易踩到响应不匹配的坑。比如有人觉得飞机机动太慢把位置环增益调得很高结果期望姿态一步跳了30度姿态环跟不上机身在天空里来回“乒乓”这就是典型的期望姿态生成没有做平滑。4. 参数、日志与常见问题排查实录4.1 影响期望姿态生成的关键参数速查表在排查问题之前建议先把下面这些参数与本文算法的对应关系记下来。这样看到飞机异常时你能快速判断是哪个环节出了岔子。参数作用与期望姿态生成的关系MPC_ACC_HOR_MAX水平期望加速度上限等效限制水平倾斜角保护正交化计算MPC_ACC_UP_MAX上升加速度上限影响推力向量长度间接影响z轴方向MPC_ACC_DOWN_MAX下降加速度上限同上下降限制通常更严格MPC_TILTMAX_AIR自动模式最大倾斜角直接约束z_axis与垂直轴的夹角MPC_MAN_TILT_MAX手动模式最大倾斜角手动飞行时的倾斜限制MPC_THR_HOVER悬停油门用于油门前馈与推力大小配套MC_ATT_P姿态环比例增益影响对期望姿态的跟踪快慢不改变矩阵生成很多开发新手只盯着MPC_ACC_HOR_MAX调却忽略MPC_TILTMAX_AIR。实际上位置控制器应该先把水平加速度限制在tan(max_tilt) * g的范围内如果自定义代码里没有这一层限幅单纯调加速度上限是兜不住的。我个人习惯是把加速度上限和倾斜角上限绑成一个约束函数改一个同时改另一个不让两边打架。4.2 三个典型故障的表现、归因与处理故障一大机动时飞机突然“甩头”表现是期望偏航角没变但机头在水平加速瞬间剧烈晃动。这种问题优先怀疑期望姿态生成里的正交化退化当推力水平分量占主导时z_axis变得很斜x_c在z_axis上的投影被放大正交化后x_axis的噪声也跟着放大。解决方向有两个一是把最大倾斜角限制得更保守二是在位置控制中增加加速度变化率限制防止期望姿态突变。我实际操作中会先看日志里的期望四元数如果它在抖问题就在位置环和姿态生成如果期望四元数平滑但真实姿态在抖问题在姿态环增益。故障二悬停或垂直起降时机头慢飘悬停时推力向量几乎垂直偏航自由度收敛很弱。只要期望偏航角有一点抖动x_axis就会绕z轴漂。很多飞控在这种状态下会锁定偏航当z_axis与垂直轴夹角小于阈值时不使用外部偏航设定值而是保持上一帧的q_des。这个保护层非常值得加。我看到过有团队在垂直起降段把偏航设定值强制清零结果飞机一边下落一边慢慢转最后落地姿态是歪的。正确做法是检测夹角小于比如5度就沿用上一帧偏航夹角大了再平滑切回外部给定。故障三期望推力大小和姿态方向不匹配导致掉高如果你改动了位置控制器里的重力补偿或者用了非标准坐标系常见现象是飞机悬停时姿态看着水平但高度缓慢下降日志里thrust_norm明显异常。这时不要急着调电机油门先确认acc_sp是否已经包含重力补偿。可以在地面站给一个纯垂直的阶跃指令看z_axis是否还是垂直。如果z_axis偏了说明坐标符号有误。这种问题在自研飞控里很常见通常就是某位开发者把重力项符号写反了导致悬停时期望姿态本身带着几度倾斜飞控为了拉回水平又不断输出矫正力矩整机表现就是“抽风”。4.3 调试建议怎么用日志验证矩阵生成是否正确调试这种底层的姿态算法不能只看飞机飞得好不好要看中间量。建议至少记录以下几组数据期望加速度acc_sp三轴都要推力向量或者归一化后的z_axis期望偏航角yaw_sp期望四元数q_des当前四元数q_cur拿到日志后先做静态测试解锁但不上桨或者上桨但锁定转速在地面站给定一组固定的加速度和偏航角手动检查q_des是否对应正确的倾转方向。比如给定一个向前的水平加速度期望机头应该抬起来给定一个向右的加速度机身应该往左倾。对着这个方向验证能很快发现坐标符号反了、叉乘顺序错了这类低级问题。还有一个很有效的检查方法把期望旋转矩阵的三个列向量画成三维箭头叠加在机体模型上。如果第二列与第一、第三列不正交或者矩阵行列式明显偏离1就说明算法在某处构造错了。这种可视化排查比盯着四元数盲猜快得多。我在一次比赛前调试时就是靠这个三维箭头发现第二列始终和另外两列不垂直最后定位到是叉乘顺序写反了。这个坑如果在真机上跑估计飞机又要炸一次。最后说一点个人体会。这套期望姿态矩阵生成算法看起来只是一小段数学加一百行代码但它把位置控制的“力”和姿态控制的“角”焊在了一起。我改造飞控时先后在坐标符号、退化处理和限幅顺序上各栽过一次跟头最后列出来的检查清单就差打印贴在工位上。如果你也只改这一个模块建议先把上面的伪代码在仿真环境里跑一遍静态用例再上真机。真机上出现奇怪行为时优先看中间量日志不要一上来就调PID。期望姿态生成的问题很多时候不是PID能救回来的。