【PX4飞控】位置控制器源码阅读与算法分析（带速度饱和的比例控制）

代码版本为 v1.14.0，v1.15 没有更改文章涉及的这部分代码，所以也是通用的。这篇文章来分析 PX4 飞控运动学控制中的位置控制，也是整个串级PID控制框架中最靠外环的部分。位置控制器输入期望位置与实际位置，结合速度前馈指令，输出期望速度指令。

1.1 控制框图

参考：

控制器图解 | PX4 自动驾驶用户指南 (v1.14)

1.2 源码

主要的源码在

PX4-Autopilot/src/modules/mc_pos_control/PositionControl/PositionControl.cpp

• PositionControl::_positionControl()

  void PositionControl::_positionControl()
  {
  	// P-position controller
  	Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
  	// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
  	ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
  	// make sure there are no NAN elements for further reference while constraining
  	ControlMath::setZeroIfNanVector3f(vel_sp_position);
  
  	// Constrain horizontal velocity by prioritizing the velocity component along the
  	// the desired position setpoint over the feed-forward term.
  	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
  	// Constrain velocity in z-direction.
  	_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
  }
  

调用了一些数学库

PX4-Autopilot/src/modules/mc_pos_control/PositionControl/ControlMath.cpp

• 相关函数

  void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition)
  {
  	for (int i = 0; i < 3; i++) {
  		addIfNotNan(setpoint(i), addition(i));
  	}
  }
  
  void addIfNotNan(float &setpoint, const float addition)
  {
  	if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) {
  		// No NAN, add to the setpoint
  		setpoint += addition;
  
  	} else if (!PX4_ISFINITE(setpoint)) {
  		// Setpoint NAN, take addition
  		setpoint = addition;
  	}
  
  	// Addition is NAN or both are NAN, nothing to do
  }
  
  void setZeroIfNanVector3f(Vector3f &vector)
  {
  	// Adding zero vector overwrites elements that are NaN with zero
  	addIfNotNanVector3f(vector, Vector3f());
  }
  
  Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
  {
  	if (Vector2f(v0 + v1).norm() <= max) {
  		// vector does not exceed maximum magnitude
  		return v0 + v1;
  
  	} else if (v0.length() >= max) {
  		// the magnitude along v0, which has priority, already exceeds maximum.
  		return v0.normalized() * max;
  
  	} else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
  		// the two vectors are equal
  		return v0.normalized() * max;
  
  	} else if (v0.length() < 0.001f) {
  		// the first vector is 0.
  		return v1.normalized() * max;
  
  	} else {
  		Vector2f u1 = v1.normalized();
  		float m = u1.dot(v0);
  		float c = v0.dot(v0) - max * max;
  		float s = -m + sqrtf(m * m - c);
  		return v0 + u1 * s;
  	}
  }
  

1.3 控制方法分析

1. 计算 P 控制器输出vel_sp_position。

   Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
   

   水平 P 增益通过参数MPC_XY_P设定

   垂直 P 增益通过参数MPC_Z_P设定

2. 速度前馈_vel_sp = 速度前馈_vel_sp + 期望速度vel_sp_position，

   • 如果两个分量都是有限值，则相加，并将最终期望速度赋值给速度前馈_vel_sp；
   • 如果期望速度是有限值，速度前馈是NAN，令_vel_sp=vel_sp_position；
   • 如果两个分量都是NAN，就什么也不做。

   💡 注意 void addIfNotNan(float &setpoint, const float addition) 函数第一个输入参数传入了变量引用

3. 将 vel_sp_position 中值为 NAN 的元素置为 0。

4. 对 _vel_sp 中的 XY 分量进行饱和处理。

   XY平面—速度饱和算法

   1. 如果前馈速度 + 比例控制器输出的期望速度的 $XY$ 分量的长度小于 $\max$，即

      $$\Vert \text{\_vel\_sp(0), \_vel\_sp(1)}\Vert \le \max$$

      就返回向量 $[\text{\_vel\_sp(0),\_vel\_sp(1)}]$，其中 $\max$ 由 PX4 参数 MPC_XY_VEL_MAX 决定；否则继续判断。

   2. 如果比例控制器输出的期望速度的 $XY$ 分量的长度大于 $\max$，即

      $$\Vert \text{vel\_sp\_position(0), vel\_sp\_position(1)}\Vert \ge \max$$

      就返回向量 $[\text{vel\_sp\_position(0), vel\_sp\_position(1)}]$。优先使用沿目标位置设定点的速度分量，并将模限制在 $\max$，不使用前馈项；否则继续判断。

   3. 如果前馈速度与比例控制器输出的期望速度相近，即

      $$\Vert [\text{vel\_sp\_position(0), vel\_sp\_position(1)}]-[\text{\_vel\_sp(0),\_vel\_sp(1)}]\Vert \le 0.001$$

      就返回向量 $\max \cdot \frac{[\text{vel\_sp\_position(0), vel\_sp\_position(1)}]}{\Vert \text{vel\_sp\_position(0), vel\_sp\_position(1)}\Vert }$ 。优先使用沿目标位置设定点的速度分量，并将模限制在 $\max$，不使用前馈项；否则继续判断。

   4. 如果比例控制器输出的期望速度很小，即

      $$\Vert [\text{vel\_sp\_position(0), vel\_sp\_position(1)}]\Vert \le 0.001$$

      就返回向量 $\max \cdot \frac{[\text{\_vel\_sp(0),\_vel\_sp(1)}]}{\Vert \text{\_vel\_sp(0),\_vel\_sp(1)}\Vert }$ 。使用前馈速度，并将模限制在 $\max$，不使用比例控制器输出的期望速度；否则继续判断。

   5. （两个速度分量加起来的模超过最大速度，同时都不是很小，但各自模都没超过 $\max$，且方向不接近），优先使用不缩放的沿目标位置设定点的速度分量，加上一定比例 $s(0<s,s<1)$ 的前馈速度分量，合成最终的期望速度。

      代码对应

      		Vector2f u1 = v1.normalized();
      		float m = u1.dot(v0);
      		float c = v0.dot(v0) - max * max;
      		float s = -m + sqrtf(m * m - c);
      		return v0 + u1 * s;
      

      • 公式推导

        KaTeX parse error: No such environment: align at position 10: \begin{̲a̲l̲i̲g̲n̲}̲\vec{v_f} & = \…

        定义 $s=\Vert \overrightarrow{v_f}-\overrightarrow{v_0}\Vert$，需要求解 $s$，从而获得最终期望速度 $\overrightarrow{v_f}$。可以求解以下方程：

        $$(\overrightarrow{v_0}+s\overrightarrow{u_1},\overrightarrow{v_0}+s\overrightarrow{u_1})={\max }^2$$

        $$(\overrightarrow{v_0},\overrightarrow{v_0})+2s(\overrightarrow{v_0},\overrightarrow{u_1})+s^2={\max }^2$$

        化简

        $$(\overrightarrow{v_0},\overrightarrow{v_0})+2s(\overrightarrow{v_0},\overrightarrow{u_1})+s^2={\max }^2$$

        其中，$c={\overrightarrow{v_0}}^2-{\max }^2$。根据一元二次求根公式，以及事实 $s>0$（要与前馈速度方向相同），可以得到 $s$ 的计算公式：

        $$s=-m+\sqrt{m^2-c}$$

   参数 MPC_XY_VEL_MAX设定了可能的最大水平速度

   自主模式下的最大期望速度通过参数MPC_XY_CRUISE设定

   手动位置控制模式下的最大期望速度通过参数 MPC_VEL_MANUAL设定

5. 对 _vel_sp 中的 Z 分量进行饱和处理。

   将 _vel_sp(2) 限制在 MPC_Z_VEL_MAX_DN 与 MPC_Z_VEL_MAX_UP 之间