对PX4姿态控制器代码的解读(deepseek分析结果)

姿态控制器的代码在文件Firmware\src\modules\mc_att_control\AttitudeControl\AttitudeControl.cpp 链接 的update函数中 ( https://mp.weixin.qq.com/s?__biz=MzUzNTkzMjI5Mw==&mid=2247484251&idx=1&sn=275b2b9ed1173f14d78d1a43daff1be5&chksm=fafcbfb1cd8b36a73c72e7798dd833d71dc8b3727b9a47cbb11dca0f3b4ab92cf74b0d1557f3&scene=21#wechat_redirect )

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
{
  // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
  q.normalize();
  qd.normalize();

  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
    // full attitude control anyways generates no yaw input and directly takes the combination of
    // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
    qd_red = qd;

  } else {
    // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
    qd_red *= q;
  }

  // mix full and reduced desired attitude
  Quatf q_mix = qd_red.inversed() * qd;
  q_mix *= math::signNoZero(q_mix(0));
  // catch numerical problems with the domain of acosf and asinf
  q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
  q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
  qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

  // quaternion attitude control law, qe is rotation from q to qd
  const Quatf qe = q.inversed() * qd;

  // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
  // also taking care of the antipodal unit quaternion ambiguity
  const Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();

  // calculate angular rates setpoint
  matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

  // Feed forward the yaw setpoint rate.
  // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
  // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
  // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
  // and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
  // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
  // such that it can be added to the rates setpoint.
  rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

  // limit rates
  for (int i = 0; i < 3; i++) {
    rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
  }

  return rate_setpoint;
}

对PX4姿态控制器代码的解读(deepseek分析结果)

这段代码是PX4飞控中姿态控制的核心部分，主要用于计算飞行器从当前姿态调整到期望姿态所需的角速率设定值。以下是对代码的逐步解析：

---

1. 输入参数及归一化处理

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward) {
    q.normalize();
    qd.normalize();

• 输入参数：
  • q: 当前姿态四元数。
  • qd: 期望姿态四元数。
  • yawspeed_feedforward: 偏航角速度前馈（世界坐标系下的绕Z轴旋转速率）。
• 归一化：确保四元数严格归一化，避免后续计算中的数值错误（如acosf的参数超出范围导致NaN）。

---

2. 计算简化的期望姿态（忽略偏航）

const Vector3f e_z = q.dcm_z();        // 当前姿态的Z轴方向（机体坐标系下）
const Vector3f e_z_d = qd.dcm_z();     // 期望姿态的Z轴方向
Quatf qd_red(e_z, e_z_d);              // 构造四元数，将e_z旋转到e_z_d（仅调整横滚和俯仰）

• 目标：优先保证横滚和俯仰正确，忽略偏航。
• 原理：通过当前和期望的Z轴方向，生成一个四元数qd_red，仅对齐Z轴（即调整横滚和俯仰），不关心偏航。

---

3. 处理奇异情况

if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    qd_red = qd; // 直接使用完整期望姿态qd
} else {
    qd_red *= q; // 转换到世界坐标系
}

• 奇异情况：当当前与期望的Z轴方向几乎相反时，qd_red可能出现数值不稳定。此时直接使用完整的qd。
• 正常情况：将qd_red转换到世界坐标系，确保后续计算正确性。

---

4. 混合完整与简化姿态

Quatf q_mix = qd_red.inversed() * qd;  // 计算qd_red到qd的旋转差异（偏航调整）
q_mix *= math::signNoZero(q_mix(0));   // 保证四元数实部非负
q_mix(0) = math::constrain(...);       // 约束值域，避免acosf/asinf错误
qd = qd_red * Quatf(...);              // 按_yaw_w权重混合偏航调整

• 目标：在优先保证横滚和俯仰的基础上，按权重_yaw_w调整偏航。
• 数学操作：
  • q_mix表示从qd_red到qd的旋转差异（主要包含偏航）。
  • 通过acosf(q_mix(0))和asinf(q_mix(3))提取偏航角，并用_yaw_w缩放。
  • 最终生成的qd是横滚/俯仰优先、偏航按权重调整的混合姿态。

---

5. 姿态误差计算（比例控制）

const Quatf qe = q.inversed() * qd;        // 当前姿态到期望姿态的误差四元数
const Vector3f eq = 2.f * sign(qe(0)) * qe.imag(); // 转化为旋转轴误差
rate_setpoint = eq.emult(_proportional_gain);      // 比例控制输出

• 误差四元数：qe表示从q到qd的旋转。
• 旋转轴误差：四元数的虚部对应旋转轴，乘以2和符号函数得到姿态误差向量eq。
• 比例控制：误差向量与比例增益相乘，生成角速率设定值。

---

6. 偏航前馈处理

rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

• 物理意义：将世界坐标系下的偏航速率前馈转换到机体坐标系。
• 数学操作：q.inversed().dcm_z()是世界Z轴在机体坐标系中的方向，乘以前馈速率得到机体坐标系下的角速率分量。

---

7. 速率限幅

for (int i = 0; i < 3; i++) {
    rate_setpoint(i) = math::constrain(..., -_rate_limit(i), _rate_limit(i));
}

• 目标：限制各轴角速率不超过预设值_rate_limit，避免执行器饱和。

---

总结

1. 横滚/俯仰优先：通过简化姿态qd_red确保推力方向正确。
2. 偏航混合控制：按权重_yaw_w调整偏航，避免过度响应。
3. 比例+前馈控制：误差比例项结合前馈，快速响应且减少稳态误差。
4. 数值安全：严格的归一化和值域约束，避免计算错误。

这段代码体现了多旋翼无人机姿态控制的典型设计：优先保证飞行稳定性（横滚/俯仰），再兼顾偏航控制，并通过前馈提升动态性能。

下面这篇有相同解读，先俯仰横滚再偏航
https://blog.csdn.net/weixin_38693938/article/details/83794666
为什么这里要把两个Z轴单独拿出来呢？是因为某篇论文上写的，由于无人机的俯仰，横滚的控制比航向的控制要快，所以把航向控制与俯仰、横滚的控制独立出来对于控制效果更好。（下载地址：https://pan.baidu.com/s/1TxPFfoVXaU6Xh9NpEj7kmg）所以PX4的姿态控制，是先把两个Z轴重合，此时得出的过渡姿态的俯仰横滚与期望姿态的俯仰横滚是一致的，除了它们航向不一样。然后再求过渡姿态与期望姿态的航向角误差，对航向控制进行各种抑制，限幅后再叠加到姿态控制上。也就是说，正常PX4的姿态控制是先大幅度的旋转俯仰横滚到与期望姿态的俯仰横滚一致，再进行航向的旋转。当然，这只是一种比较容易的理解方法，实际是转俯仰横滚的同时也会转航向，但是航向的优先级是最低。毕竟对于多旋翼无人机来说，俯仰，横滚不乱就不会炸机，航向只是为了更好的控制位置。对于PX4的姿态控制，输入是期望角度，实际角度，输出是期望角速度（机体坐标系）。