一、串级控制总览
PositionControl.cpp 实现了一个纯算法层，不依赖任何 uORB 或 PX4 特定接口：
1
2
3
4
5
6
7
8
9
10
11
| pos_sp → [位置 P 环] → vel_sp_from_pos
+ vel_sp_feed_forward
↓
vel_sp (限幅后)
→ [速度 PID 环] → acc_sp_from_vel
+ acc_sp_feed_forward
↓
acc_sp (合成后)
→ [推力映射 + 姿态几何] → thr_sp + q_d
↓
[推力分配 + 抗饱和] → 最终输出
|
二、位置环 _positionControl()
位置环是纯 P 控制器，将位置误差转换为速度指令：
1
2
3
4
5
6
7
8
9
10
11
12
| void PositionControl::_positionControl() {
Vector2f vel_sp_position_xy = (_pos_sp.xy() - _pos.xy()) * _gain_pos_p.xy();
float vel_sp_position_z = (_pos_sp(2) - _pos(2)) * _gain_pos_p(2);
if (PX4_ISFINITE(_pos_sp(0))) _vel_sp(0) += vel_sp_position_xy(0);
if (PX4_ISFINITE(_pos_sp(1))) _vel_sp(1) += vel_sp_position_xy(1);
if (PX4_ISFINITE(_pos_sp(2))) _vel_sp(2) += vel_sp_position_z;
}
|
参数：MPC_XY_P（水平位置增益）、MPC_Z_P（垂直位置增益）
为何只用 P 不用 PID：
- 位置误差的持续积累由内环速度的 I 项补偿（稳态位置误差最终反映为持续速度误差，由速度 I 消除）
- 位置 D 项即速度反馈，已在内环中处理
速度设定点合成后进行限幅：
1
2
3
4
5
6
|
constrainXY(vel_sp_ff.xy(), vel_sp_from_pos.xy(), _lim_vel_horizontal, _vel_sp.xy());
_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
|
三、速度环 _velocityControl()
速度环是完整 PID 控制器：
注意 D 项使用的是滤波后的加速度估计（set_vehicle_states 中计算的 states.acceleration），而非速度的数值微分，有效避免了数值微分引入的噪声放大。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
| void PositionControl::_velocityControl(const float dt) {
const Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity =
vel_error.emult(_gain_vel_p) +
_vel_int.emult(_gain_vel_i) -
_vel_dot.emult(_gain_vel_d);
if (_acc_sp.isAllFinite()) {
_acc_sp += acc_sp_velocity;
} else {
_acc_sp = acc_sp_velocity;
}
_vel_int += vel_error * _gain_vel_i * dt;
}
|
参数：
MPC_XY_VEL_P_ACC、MPC_XY_VEL_I_ACC、MPC_XY_VEL_D_ACC：水平速度 PIDMPC_Z_VEL_P_ACC、MPC_Z_VEL_I_ACC、MPC_Z_VEL_D_ACC：垂直速度 PID
注：参数名后缀 _ACC 表示单位是 m/s²（加速度归一化），而非传统的无量纲增益。
四、推力映射与姿态几何 _accelerationControl()
这是整个控制器最关键的非线性环节：将期望加速度向量转换为物理可实现的推力和姿态。
4.1 物理模型
多旋翼只能沿机体 Z 轴（机体向上方向）施加推力：
在导航系（NED）中，重力方向为 ，期望加速度：
4.2 body_z 计算（目标姿态方向）
将期望加速度转换为机体 Z 轴方向（推力方向）：
1
2
3
4
|
Vector3f body_z = -(_acc_sp + Vector3f(0.f, 0.f, CONSTANTS_ONE_G));
body_z.normalize();
|
注意：NED 坐标系中 Z 向下为正，重力加速度 在 Z+ 方向。推力方向是加速度需求加上重力补偿后的方向，取反（因为推力向上）。
4.3 倾角限制 limitTilt()
1
2
3
4
5
6
7
8
9
10
11
12
13
|
Vector3f limitTilt(Vector3f body_z, Vector3f world_z, float max_tilt_rad) {
const float dot = math::constrain(body_z.dot(world_z), -1.f, 1.f);
if (acos(dot) > max_tilt_rad) {
const float b = cos(max_tilt_rad);
body_z = body_z + world_z * (b - dot);
body_z.normalize();
}
return body_z;
}
|
4.4 推力大小计算（collective_thrust）
在确定推力方向后，计算需要多大的推力才能产生期望的加速度：
1
2
3
4
5
6
|
float collective_thrust = -(_acc_sp + Vector3f(0.f, 0.f, CONSTANTS_ONE_G)).dot(body_z);
collective_thrust = collective_thrust / _hover_thrust * 0.5f;
collective_thrust = math::constrain(collective_thrust, _lim_thr_min, _lim_thr_max);
|
4.5 推力向量合成
1
2
|
_thr_sp = -body_z * collective_thrust;
|
4.6 姿态四元数生成 thrustToAttitude()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
Quatf ControlMath::thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp) {
const Vector3f body_z = -thr_sp.normalized();
const Vector3f body_x_proj = {cosf(yaw_sp), sinf(yaw_sp), 0.f};
const Vector3f body_y = body_z % body_x_proj;
const Vector3f body_x = body_y % body_z;
Dcmf R;
R.col(0) = body_x;
R.col(1) = body_y;
R.col(2) = body_z;
return Quatf(R);
}
|
五、垂直优先推力分配
多旋翼的总推力有上限 MPC_THR_MAX，当水平方向需要大倾角（大推力分量）时，不能无限增大总推力。PX4 采用垂直优先策略：
- 先保证 Z 轴推力（高度控制优先）
- 在剩余推力余量内，分配水平推力
- 若水平推力不足，按等比缩小水平推力（不影响垂直分量）
1
2
3
4
5
6
7
8
9
10
11
12
13
|
const float thrust_z = -_thr_sp(2);
const float thrust_max_xy_sq = sq(_param_mpc_thr_max.get()) - sq(thrust_z);
const float thrust_max_xy = math::max(0.f, sqrtf(thrust_max_xy_sq));
const float thrust_xy_available = math::max(0.f, thrust_max_xy - _param_mpc_thr_xy_marg.get());
if (_thr_sp.xy().norm() > thrust_xy_available) {
_thr_sp.xy() = _thr_sp.xy().normalized() * thrust_xy_available;
}
|
六、抗积分饱和（Anti-Windup）
6.1 Z 轴条件抗饱和
1
2
3
4
5
6
|
const bool saturated_up = (_thr_sp(2) < -_param_mpc_thr_max.get());
const bool saturated_down = (_thr_sp(2) > -_param_mpc_thr_min.get());
if (!saturated_up && !saturated_down) {
_vel_int(2) += vel_error(2) * _gain_vel_i(2) * dt;
}
|
6.2 XY 轴 Tracking Anti-Windup
1
2
3
4
5
|
const Vector2f acc_executed = _thr_sp.xy() * (-1.f / _hover_thrust) * CONSTANTS_ONE_G;
const Vector2f vel_int_contribution = (acc_sp_velocity.xy() - acc_executed) / _gain_vel_i.xy() * dt;
_vel_int.xy() -= vel_int_contribution;
|
这种 Tracking Anti-Windup 的原理：将实际输出与期望输出的差作为反馈，修正积分项，防止饱和时积分持续增大。
七、控制完整数据流
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| flowchart TD
POS_SP["位置设定 pos_sp"] --> POS_P["位置 P 环<br/>vel_sp_pos = (pos_sp - pos) * MPC_XY_P"]
VEL_FF["速度前馈 vel_sp_ff"] --> VEL_SUM["速度合成<br/>+ 限幅（XY 优先级 / Z 上下限）"]
POS_P --> VEL_SUM
VEL_SP["速度设定 vel_sp"] --> VEL_SUM
VEL_SUM --> VEL_ERR["速度误差 vel_error = vel_sp - vel"]
VEL_ERR --> VEL_PID["速度 PID<br/>P*e + I*∫e - D*vel_dot"]
ACC_FF["加速度前馈 acc_sp_ff"] --> ACC_SUM["加速度合成"]
VEL_PID --> ACC_SUM
ACC_SUM --> CTRL["_accelerationControl<br/>body_z → limitTilt → collective_thrust"]
CTRL --> THRUST["thrustToAttitude<br/>推力向量 → 姿态四元数"]
THRUST --> PRIO["垂直优先分配<br/>Z 保证，XY 裕量内使用"]
PRIO --> ANTI["抗积分饱和<br/>Z 条件饱和 + XY Tracking"]
ANTI --> OUTPUT_ATT["vehicle_attitude_setpoint<br/>q_d + thrust_body"]
ANTI --> OUTPUT_LOC["vehicle_local_position_setpoint<br/>pos/vel/acc/thrust sp"]
|
八、小结
PositionControl.cpp 的关键设计：
- 串级解耦：位置→速度→加速度三级串联，各级独立调参，调试清晰
- 非线性几何映射：从加速度到推力方向（姿态）的转换考虑了重力补偿，数学上严格正确
- 倾角约束：在位置控制层直接约束最大倾角，比在姿态层限制更早介入，更安全
- 垂直优先：高度控制优先于水平控制，保证 Z 轴稳定性不被水平机动牺牲
- 抗饱和：Z 方向条件抗饱和 + XY Tracking anti-windup，防止积分漂移
这一控制算法是飞行稳定性的核心，其参数调优（特别是速度 PID 增益）对飞行品质影响最大。
上一篇：MulticopterPositionControl 主循环详解
系列总目录：PX4 v1.16 源码解读总目录