PX4飞控代码阅读（2）位置环

多旋翼控制器结构

整体控制器结构如下

控制器结构

各模块分别位于：

• Position Control, Velocity Control, Acceleration&Yaw to Attitude: src\modules\mc_pos_control
• Angle Control: src\modules\mc_att_control
• Angular Rate Control: src\modules\mc_rate_control
• Mixer: src\modules\control_allocator

速度控制量计算

位置控制的代码位于src\modules\mc_pos_control\PositionControl\PositionControl.cpp，核心函数为update：

bool PositionControl::update(const float dt)
{
	bool valid = _inputValid();

	if (valid) {
		_positionControl();
		_velocityControl(dt);

		_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
		_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
	}

	// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
	return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite();
}

第6行根据位置设定点计算速度设定点：

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

首先通过一个P控制器计算速度控制量（第4行）：

$${\mathbf{V}}_{sp,position}={\mathbf{K}}_{P,pos}\cdot ({\mathbf{X}}_{sp}-\mathbf{X})$$

然后加上一个前馈项（来自期望轨迹上的期望速度，或是用户输入，或是简单设置为0）（第6行）：

$${\mathbf{V}}_{sp}={\mathbf{V}}_{ff}+{\mathbf{V}}_{sp,position}$$

最后分别对水平XY方向的速度和垂直Z方向的速度进行限制。水平方向上优先保证${\mathbf{V}}_{sp,position}$，垂直方向则是简单的标量clip。

Note

水平方向的限制原理为：

• 如果${\mathbf{V}}_{sp,position}+{\mathbf{V}}_{ff}$不超过限制，则直接使用它

• 如果${\mathbf{V}}_{sp,position}$超过限制，则将它缩放到max

• 如果两个向量相等，则使用${\mathbf{V}}_{sp,position}$缩放到max

• 如果${\mathbf{V}}_{sp,position}$为0，则将${\mathbf{V}}_{ff}$缩放到max

• 否则，使用下面的代码，将${\mathbf{V}}_{ff}$缩放使得最终的值满足限制。这是通过解二次方程$||\overrightarrow{v}+s\overrightarrow{u}|{|}^2=ma{x}^2$得到的

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;

加速度控制量计算

加速度控制量计算的代码：

void PositionControl::_velocityControl(const float dt)
{
	// Constrain vertical velocity integral
	_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);

	// PID velocity control
	Vector3f vel_error = _vel_sp - _vel;
	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);

	// No control input from setpoints or corresponding states which are NAN
	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);

	_accelerationControl();

	// ......
}

核心的PID计算为：

$${\mathbf{A}}_{sp}={\mathbf{K}}_{P,vel}\cdot ({\mathbf{V}}_{sp}-\mathbf{V})+{\mathbf{I}}_{vel}+{\mathbf{K}}_{D,vel}(0-\dot{\mathbf{V}})$$

这里的微分并不是误差项的微分，可以理解为期望速度为0时的误差项的微分，这是PX4为了使速度变化平缓进行的工程优化，后面角速度的控制也有类似设计。

接下来调用_accelerationControl()来计算期望姿态和油门设定，暂且按下不表，后面的代码进行抗饱和积分和推力分配。首先是对垂直方向的积分采用条件积分法进行抗饱和：

// Integrator anti-windup in vertical direction
if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
    (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) {
    vel_error(2) = 0.f;
}

然后提取水平方向的推力，进行推力的预分配。预分配的原则是，优先满足垂直方向的控制，但保留一部分水平控制能力，因此首先预分配水平方向的推力：

const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);

thrust_sp_xy_norm是水平推力的大小，将其限制在_lim_thr_xy_margin以内。接下来计算可供垂直方向使用的剩余推力：

const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);

这就是用最大推力的平方减去上一步分配给水平方向的推力平方。接下来使用这个值对垂直推力进行裁剪：

_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));

然后再计算除掉垂直推力后真正剩余给水平推力的裕度：

const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0.f;

if (thrust_max_xy_squared > 0.f) {
    thrust_max_xy = sqrtf(thrust_max_xy_squared);
}

再用这个裕度裁剪水平推力，得到真正分配给水平方向的推力大小：

if (thrust_sp_xy_norm > thrust_max_xy) {
    _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}

最后，对水平方向的加速度进行跟踪法抗饱和积分，其原理是将裁剪后的输出和期望输出作差，然后通过一个反馈增益反馈给积分环节，来调节积分器的饱和程度。

// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);

// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
// The ARW loop needs to run if the signal is saturated only.
if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared()) {
    const float arw_gain = 2.f / _gain_vel_p(0);
    const Vector2f acc_sp_xy = _acc_sp.xy();

    vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced);
}

// ......

// Update integral part of velocity control
_vel_int += vel_error.emult(_gain_vel_i) * dt;

代码首先计算水平推力能产生的加速度，用$g$除以悬停推力来归一化。然后，如果出现饱和，则将速度的水平误差减去跟踪增益arw_gain乘饱和量，再以这个值进行积分。

期望姿态和推力计算

如图所示，升力、重力和无人机的加速度方向有着这样的关系，而升力始终垂直于无人机平面，因此可以通过它来算出期望的姿态。采用NED（北、东、下方向为正）坐标系，则机身方向向量：

$${\mathbf{b}}_z=-{\mathbf{a}}_{lift}=-\frac{1}{m}(m{\mathbf{A}}_{sp}-\mathbf{G})=\left[\begin{array}{c}-a_x\\ -a_y\\ -a_z+g\end{array}\right]$$

对应的代码：

void PositionControl::_accelerationControl()
{
	// Assume standard acceleration due to gravity in vertical direction for attitude generation
	float z_specific_force = -CONSTANTS_ONE_G;

	if (!_decouple_horizontal_and_vertical_acceleration) {
		// Include vertical acceleration setpoint for better horizontal acceleration tracking
		z_specific_force += _acc_sp(2);
	}

	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
    // ...

这里通过_decouple_horizontal_and_vertical_acceleration来设置是否耦合纵向加速度。

然后限制倾斜在安全范围内：

ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);

计算出的机身方向向量在PositionControl::getAttitudeSetpoint函数中被转换成四元数，作为机身姿态控制环的输入。

接下来计算油门，垂直方向的油门大小为（向下为正）

$${\sigma }_z=a_z\frac{{\sigma }_0}{g}-{\sigma }_0$$

这里${\sigma }_0$是_hover_thrust即悬停油门，相当于$mg$。最后减去悬停油门是因为向上的方向为负。这里计算出来的一般是一个负数。

然后计算垂直机体方向的总油门大小：

$$\sigma =\frac{{\sigma }_z}{{\mathbf{b}}_z\cdot [001]}$$

这里的分母是机身方向单位向量在z轴方向的分量。代码中实际上还进行了一次裁剪以确保不超过油门限值。最后，乘以机身单位向量来得到矢量的油门：

$$\overrightarrow{\sigma }=\sigma {\mathbf{b}}_z$$

对应的代码如下：

// Convert to thrust assuming hover thrust produces standard gravity
const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude
const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
_thr_sp = body_z * collective_thrust;