4、位置PID控制器设计：水平位置控制、高度控制

好，咱们继续往下走。前面聊了姿态控制，那是无人机的「内环」。现在要说的位置控制，是「外环」。说白了，姿态控制是让飞机「站稳」，位置控制是让飞机「走到哪儿」。这两个环串在一起，才能实现真正的自主飞行。

我个人习惯把位置控制拆成两块：水平位置（X、Y轴）和高度（Z轴）。别看都是位置，控制逻辑差别挺大的。水平位置依赖姿态，高度控制依赖油门。嗯，咱们一个一个来。

4.1 水平位置控制：你让它往东，它不能往西

水平位置控制的目标很简单：让无人机飞到目标点 (x_des, y_des)。但怎么飞过去？这里有个关键点——无人机本身不能直接产生水平方向的力，它得先倾斜机身，让旋翼拉力产生水平分量。

所以，水平位置控制器的输出，不是直接给电机，而是给姿态控制器一个「期望角度」。你想想看，这是一个典型的串级结构：

• 外环（位置环）：输入是位置误差，输出是期望速度或期望加速度。
• 内环（速度环）：输入是速度误差，输出是期望倾斜角度（roll/pitch）。
• 最内环（姿态环）：输入是角度误差，输出是电机控制量。

我在项目中遇到过一种情况：有人直接把位置误差映射成角度，结果飞机飞起来像抽风一样。为什么？因为位置环和姿态环的响应速度差太多，直接耦合会震荡。

具体公式长这样：

// 水平位置PID（以X轴为例）
float pos_err_x = pos_des_x - pos_cur_x;
float vel_des_x = Kp_pos_x * pos_err_x;  // P控制，输出期望速度

// 速度PID（X轴）
float vel_err_x = vel_des_x - vel_cur_x;
float acc_des_x = Kp_vel_x * vel_err_x + Kd_vel_x * (vel_err_x - vel_err_last_x) / dt;

// 将期望加速度转换为期望俯仰角
float pitch_des = atan2(acc_des_x, g);  // g是重力加速度

这里有个细节：位置环一般只用P控制。为什么？因为位置误差本身就有积分效果——你一直有误差，飞机就会一直往那个方向飞，直到误差为零。再加I项反而容易引入积分饱和。

但速度环不一样。速度环我建议用PI控制，甚至加一点D。因为速度控制需要快速响应，而且外界风扰会带来稳态误差，I项能消除它。

4.2 高度控制：油门不是你想推，想推就能推

高度控制比水平控制「单纯」一些，但坑也不少。高度控制的核心是：控制油门，让无人机悬停在目标高度。

你可能会想：「这不就是一个PID吗？」对，但问题在于——油门和升力不是线性关系。电池电压变化、电机老化、桨叶效率，都会影响同样的油门值产生不同的升力。

所以，我习惯在高度控制器里加一个「前馈项」。说白了，就是先估算一个基础油门，让飞机能悬停，PID只负责修正误差。

// 高度PID控制器
float alt_err = alt_des - alt_cur;
float vel_des_z = Kp_alt * alt_err;  // 位置环输出期望速度

float vel_err_z = vel_des_z - vel_cur_z;
float thrust_adjust = Kp_vel_z * vel_err_z 
                    + Ki_vel_z * integral_vel_z 
                    + Kd_vel_z * (vel_err_z - vel_err_last_z) / dt;

// 最终油门 = 悬停油门 + PID修正
float throttle = throttle_hover + thrust_adjust;

这里有个关键参数：throttle_hover。这个值怎么来？我一般会在飞机起飞后，让它稳定悬停，然后记录当前的油门值。每次起飞前重新校准一次，因为电池电压不一样。

4.3 位置控制与高度控制的耦合

嗯，这里要提一个容易被忽略的问题：水平运动和高度运动是耦合的。

当飞机倾斜时，总升力在垂直方向的分量会减少。也就是说，你让飞机往前飞，它就会掉高度。反过来，你猛推油门，飞机会抬头，水平位置也会偏移。

怎么处理？我见过两种方案：

• 解耦控制：在高度控制器里，根据当前倾斜角度，补偿升力损失。比如倾斜30度时，把油门提高15%。
• 统一控制：把高度和水平位置放在一个控制器里，用总推力矢量的方式计算。这种方法更精确，但计算量大。

我个人偏向第一种，简单实用。代码实现也不复杂：

// 倾斜角度补偿
float tilt_angle = sqrt(roll_cur * roll_cur + pitch_cur * pitch_cur);
float compensation = 1.0 / cos(tilt_angle);  // 升力损失补偿系数
throttle = throttle_hover * compensation + thrust_adjust;

注意，这个补偿只在倾斜角度较大时（比如超过15度）才启用。小角度时影响不大，没必要增加计算量。

4.4 参数整定：别指望一次搞定

位置控制器的参数整定，我建议分三步走：

1. 先调高度：让飞机能稳定悬停。只调高度环，水平位置用手扶着或者绑在架子上。
2. 再调水平速度环：给一个速度指令，看飞机能不能平稳加速和减速。这时候不要管位置，只看速度响应。
3. 最后调位置环：给一个位置指令，看飞机能不能准确到达并稳定。

每一步都要确保内环比外环快。怎么判断？我有个土办法：给一个阶跃指令，看响应时间。姿态环应该在0.1秒内响应，速度环在0.3秒内，位置环在1秒内。如果外环比内环还快，那肯定震荡。

好了，位置PID控制器就聊到这儿。下一章咱们会讲更高级的控制方法，比如LQR和自适应控制。但说实话，把PID调好了，90%的飞行场景都能搞定。剩下的10%，那是给博士们发论文用的。