位置控制器PX4代码解析（文中有福利！！！）

号外号外！！！本公众号将联合电子工业出版社于9月11号送出15本价值98元的全权老师著作《多旋翼飞行器设计与控制》，关注本公众号的朋友均可参加，敬请期待~~还没关注的朋友赶紧关注吧！！！

引言

上一讲开源飞控PX4姿态控制代码解析我们对PX4姿态控制的代码进行了详细解析，趁大家对PX4代码还有点熟悉的时候我们把位置控制器的代码部分也拿出来解读一下吧。位置控制器的代码主要是以下两个函数：

1、void PositionControl::_positionController()位于Firmware\src\modules\mc_pos_control\PositionControl.cpp文件中；

2、void PositionControl::_velocityController(const float &dt)也是位于Firmware\src\modules\mc_pos_control\PositionControl.cpp文件中；

我们先贴出两个函数的源码：

void PositionControl::_positionController()
{
  // P-position controller
  const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
           _param_mpc_z_p.get()));
  _vel_sp = vel_sp_position + _vel_sp;

  // Constrain horizontal velocity by prioritizing the velocity component along the
  // the desired position setpoint over the feed-forward term.
  const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
           Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
  _vel_sp(0) = vel_sp_xy(0);
  _vel_sp(1) = vel_sp_xy(1);
  // Constrain velocity in z-direction.
  _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

void PositionControl::_velocityController(const float &dt)
{
  // Generate desired thrust setpoint.
  // PID
  // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
  // Umin <= u_des <= Umax
  //
  // Anti-Windup:
  // u_des = _thr_sp; r = _vel_sp; y = _vel
  // u_des >= Umax and r - y >= 0 => Saturation = true
  // u_des >= Umax and r - y <= 0 => Saturation = false
  // u_des <= Umin and r - y <= 0 => Saturation = true
  // u_des <= Umin and r - y >= 0 => Saturation = false
  //
  //   Notes:
  // - PID implementation is in NED-frame
  // - control output in D-direction has priority over NE-direction
  // - the equilibrium point for the PID is at hover-thrust
  // - the maximum tilt cannot exceed 90 degrees. This means that it is
  //    not possible to have a desired thrust direction pointing in the positive
  //    D-direction (= downward)
  // - the desired thrust in D-direction is limited by the thrust limits
  // - the desired thrust in NE-direction is limited by the thrust excess after
  //    consideration of the desired thrust in D-direction. In addition, the thrust in
  //    NE-direction is also limited by the maximum tilt.

  const Vector3f vel_err = _vel_sp - _vel;

  // Consider thrust in D-direction.
  float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
           2) - _param_mpc_thr_hover.get();

  // The Thrust limits are negated and swapped due to NED-frame.
  float uMax = -_param_mpc_thr_min.get();
  float uMin = -_param_mpc_thr_max.get();

  // make sure there's always enough thrust vector length to infer the attitude
  uMax = math::min(uMax, -10e-4f);

  // Apply Anti-Windup in D-direction.
  bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
             (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

  if (!stop_integral_D) {
    _thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

    // limit thrust integral
    _thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
  }

  // Saturate thrust setpoint in D-direction.
  _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);

  if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
    // Thrust set-point in NE-direction is already provided. Only
    // scaling by the maximum tilt is required.
    float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    _thr_sp(0) *= thr_xy_max;
    _thr_sp(1) *= thr_xy_max;

  } else {
    // PID-velocity controller for NE-direction.
    Vector2f thrust_desired_NE;
    thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
    thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

    // Get maximum allowed thrust in NE based on tilt and excess thrust.
    float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
    thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

    // Saturate thrust in NE-direction.
    _thr_sp(0) = thrust_desired_NE(0);
    _thr_sp(1) = thrust_desired_NE(1);

    if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
      float mag = thrust_desired_NE.length();
      _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
      _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
    }

    // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
    // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
    float arw_gain = 2.f / _param_mpc_xy_vel_p.get();

    Vector2f vel_err_lim;
    vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
    vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

    // Update integral
    _thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
    _thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
  }
}

代码解读

位置控制器外环位置控制函数解读：

void PositionControl::_positionController()
{
  // P-position controller

//根据位置误差和位置环P参数计算速度期望（NED系）

  const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
           _param_mpc_z_p.get()));

//叠加位置误差产生的速度期望和速度期望前馈为总速度期望

  _vel_sp = vel_sp_position + _vel_sp;

  // Constrain horizontal velocity by prioritizing the velocity component along the
  // the desired position setpoint over the feed-forward term.

//根据设置的最大水平速度限制水平方向期望速度，优先保证满足位置误差引起的期望速度

  const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
           Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
  _vel_sp(0) = vel_sp_xy(0);
  _vel_sp(1) = vel_sp_xy(1);
  // Constrain velocity in z-direction.

//根据D向速度最大最小值限制D向速度期望

  _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

位置控制器内环速度控制函数解读：

void PositionControl::_velocityController(const float &dt)
{
  // Generate desired thrust setpoint.
  // PID
  // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
  // Umin <= u_des <= Umax
  //
  // Anti-Windup:
  // u_des = _thr_sp; r = _vel_sp; y = _vel
  // u_des >= Umax and r - y >= 0 => Saturation = true
  // u_des >= Umax and r - y <= 0 => Saturation = false
  // u_des <= Umin and r - y <= 0 => Saturation = true
  // u_des <= Umin and r - y >= 0 => Saturation = false
  //
  //   Notes:
  // - PID implementation is in NED-frame
  // - control output in D-direction has priority over NE-direction
  // - the equilibrium point for the PID is at hover-thrust
  // - the maximum tilt cannot exceed 90 degrees. This means that it is
  //    not possible to have a desired thrust direction pointing in the positive
  //    D-direction (= downward)
  // - the desired thrust in D-direction is limited by the thrust limits
  // - the desired thrust in NE-direction is limited by the thrust excess after
  //    consideration of the desired thrust in D-direction. In addition, the thrust in
  //    NE-direction is also limited by the maximum tilt.

//计算速度误差（NED系）

  const Vector3f vel_err = _vel_sp - _vel;

  // Consider thrust in D-direction.

//根据速度误差计算D向的升力大小（也就是控制器的输出，控制输入，这里我们就统一以实际物理意义升力称呼）：thr_sp_d=P+I+D+thr_hover，注意方向D是向下的，所以加中立油门时会是负的

  float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
           2) - _param_mpc_thr_hover.get();

//因为D向是向下的，所以将上下限加上负号。

  // The Thrust limits are negated and swapped due to NED-frame.
  float uMax = -_param_mpc_thr_min.get();
  float uMin = -_param_mpc_thr_max.get();
  
  // make sure there's always enough thrust vector length to infer the attitude
  uMax = math::min(uMax, -10e-4f);

  // Apply Anti-Windup in D-direction.

//积分抗饱和，如果算出D向期望升力到达上下限，且误差方向同向，则停止积分。不然则正常积分并对积分上下限进行限制。

  bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
             (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

  if (!stop_integral_D) {
    _thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

    // limit thrust integral

//限制D向升力积分

    _thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
  }

  // Saturate thrust setpoint in D-direction.

//限制D向升力期望值

  _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);

//当NE方向的期望已经被赋值时，用最大倾斜角度限制倾斜方向即可。这个是由飞控上层规划的，这里不需要考虑，一般也不会跑这段代码。

  if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
    // Thrust set-point in NE-direction is already provided. Only
    // scaling by the maximum tilt is required.
    float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    _thr_sp(0) *= thr_xy_max;
    _thr_sp(1) *= thr_xy_max;

//正常情况下根据速度误差计算升力在NE平面的期望的过程

  } else {
    // PID-velocity controller for NE-direction.

//NE平面内的速度误差PID计算，得到NE平面内的升力期望

Vector2f thrust_desired_NE;
thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

//根据允许的最大倾斜角度和当前D向期望升力以及最大允许油门计算NE平面允许的最大升力，这个其实很好理解，整个飞机产生的升力可以分解为NED系的D向以及NE平面内的分量，它们分配到的值是跟飞行器的倾斜角度有直接关系的。同时两者平方的和也与飞行器的油门平方是相等的，这里都是无量纲归一化后的关系。

// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

// Saturate thrust in NE-direction.
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);

//当升力在NE平面的期望超过上面计算的允许最大值时进行限制

if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
  float mag = thrust_desired_NE.length();
  _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
  _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}

//积分抗饱和处理，当控制律输出没到极限时，_thr_sp和thrust_desired_NE是相等的，所以正常积分，当输出达到极限后，在积分的同时就会向反方向减去一个超限的值，超得越多减得越多，这可以起到抗饱和的作用，避免超限后积分还在持续增大。

    // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
    // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
    float arw_gain = 2.f / _param_mpc_xy_vel_p.get();

    Vector2f vel_err_lim;
    vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
    vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

    // Update integral
    _thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
    _thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
  }
}

总结

本篇内容对PX4中实现位置控制器的代码进行了详细解读，代码进行了逐行分析，关于算法公式的推导，大家可以参考之前的文章：无人机控制器设计（二）：位置控制器设计，现在大家应该对PX4中多旋翼飞行器的整个控制器设计与实现有了清晰的认识，除了外环控制器和内环姿态控制器的实现外，还有一个外环控制器输出到内环指令生成的函数我们下一次内容再进行阐述。