开源飞控PX4姿态控制代码解析

本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。

本公众号将于9月11号联合电子工业出版社送出15本价值98元的《多旋翼飞行器设计与控制》书籍，敬请期待。关注：无人机系统技术，了解详情！！！

引言

上一讲PX4姿态控制算法详解我们对PX4姿态控制的算法进行了详细解析，还没看过的朋友先打开第三条内容仔细阅读后再来继续浏览吧。本讲内容主要是摘取PX4中关于姿态控制律实现的代码进行逐行分析，让各位朋友在代码实践中感受控制算法的魅力。姿态控制器的代码在文件Firmware\src\modules\mc_att_control\AttitudeControl\AttitudeControl.cpp的update函数中，我们先把代码贴上来，然后再进行逐行解读：

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;
}

代码解读

为方便大家阅读，代码中原有的英文注释给大家保存，每一行代码的注释我们放在代码上方。

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

//对当前姿态四元数进行归一化，其实四元数本来就是归一化的，但是在计算机中存储传输时1.0可能会被存储成1.000001，而后面在运算反余弦函数时就会出现acosf(1.00001) == NaN

  q.normalize();

//对期望姿态四元数进行归一化，原因同当前姿态四元数

  qd.normalize();

  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch

//计算当前姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示

  const Vector3f e_z = q.dcm_z();

//计算期望姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示

 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;

  }

//将旋转轴从N系转换到B系：

//结合当前姿态，我们可以得到仅代表倾斜运动的期望四元数：

//所以qd_red *= q是把上面两步骤合成了一步

  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一定可以表示为（cos(α_mix/2),0,0,sin(α_mix/2))

  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;

//根据实际意义选取姿态误差为：eq=2*sign(qe(0))*qe(1:3)

  // 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中实现姿态控制器的代码进行了详细解读，经过算法公式的推导，代码的逐行分析，大家应该对PX4中多旋翼飞行器的姿态控制器设计与实现有了清晰的认识，不过纸上得来终觉浅，绝知此事要躬行，光看不练无异于纸上谈兵，大家可以在matlab中做做仿真，或者在搭建的飞控开发平台上根据自己的理解来实现姿态控制，在实战中感受飞控算法的魅力。

本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。