ardupilot 高度控制算法

51fly

本节主要讲解 ardupilot 的高度控制器的实现逻辑。如何维持四旋翼在期望的高度？首先我们需要知道无人机当前的高度和目标高度，实际高度是气压计与加速度计通过 EKF 算法融合处理得到的惯导高度，目标高度就是飞行员期望飞行器所达到的高度值，比如这里设定飞行器的目标高度是 5m，那么我们的目标高度值就是 500cm,无人机通过 PID 控制算法调节四旋翼的四个电机的转速值实现稳定在目标高度范围内。Ardupilot 官网给出的高度控制算法框图，如图 1 所示，不过这个基于比较老的版本的代码实现流程；最新的代码也可以用图 2 表示。

图 1 ardupilot 高度控制算法流程

图 2 ardupilot 高度控制算法流程1.初始化高度控制高度控制器初始化主要完成初始化高度控制器的目标高度为当前惯导计算的高度，初始化目标速度为当前垂直速度方向的速度，如果已经激活执行过初始化配置，将不会进行配置，直接返回真。

图 3 ardupilot 高度控制算法初始化流程

2.运行高度控制算法ardupilot 的高度控制器采用三级串级 PID 控制算法，即垂直位置、垂直速度、垂直加速度的三级控制。根据设定的目标高度与实际的高度的误差调用开平方控制算法得到垂直方向上的目标速度控制量，并对目标速度进行限制；然后根据当前垂直方向上的目标速度与从惯导获取的垂直速度的误差调用垂直速度PID 算法的比例控制得到垂直方向上的目标加速度控量，并对目标加速度进行速度前馈；最后进行垂直方向上加速度的 PID 控制，最终把加速度 PID 控制量的输出转换成控制电调的 PWM 信号，结合悬停油门值调节四个电机的转速的大小实现垂直升降，已达到目标高度的过程。代码实现过程可以用如图 4 过程表示



图 4 定高算法控制流程图
从流程图可以看出代码中主要采用开平方控制器和 PID 控制，大量采用前馈的思想。我们首先看下高度控制算法整体代码：









从高度运行代码可以看出，整个实现过程思路比较清晰，高度控制运行代码，把高度控制的过程按照状态机机制分成四种状态：停止、起飞、飞行、着陆。然后针对每种情况进行高度控制，这样做更精细的处理过渡过程。

下面主要针对高度运行代码中的核心代码进行讲述：1 获取目标高度进行高度控制
进行高度控制，那么我们首先要得到目标高度。

对于ardupilot中目标高度的获取主要来自下面三种情况：

1.遥控器的油门值转换成爬升率进而转化成目标高度

2.手机APP发送给无人机的目标高度

3.地面站发送给无人机的目标高度值

本节主要针对第一种情况进行阐述，对于其他两种情况会在自动飞行模式控制中讲述。对于遥控器的油门值转换成爬升率进而转化成目标高度，也就意味着当油门摇杆值大于中立值时无人机的爬升率为正，无人机会爬升；当油门摇杆值值在中立值以下时，爬升率为负，无人机高度下降；当油门摇杆值在中立位置时，无人机的爬升率为零，高度固定不变。

如何获取遥控器的油门值转换成爬升率进而转化成目标高度？ardupilot获取爬升率的代码：


其 中 channel_throttle->get_control_in() 就 是 我 们 要 找 的 值 ， 最 终 传 递 到get_pilot_desired_climb_rate()函数中，得到爬升速率。具体函数实现：





限 制 爬 升 速 度 ， 从 代 码 可 以 看 出 target_climb_rate 的范围是 【 -g2.pilot_speed_dn ，g.pilot_speed_up】，后面分析这个值在哪里使用。

2 获取高度控制状态机

这里主要针对起飞代码进行讲解，其他三种读者可以自己慢慢分析。a) 起飞状态首先获取飞起爬升率，其中起飞阶段的判断条件通过设定目标高度来评判，从 takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));可以得到结论





起飞相关参数设置：

采用起飞爬升速率调节目标高度



到这里我们可以看到遥控器油门量映射的目标爬升率通过积分转换成了目标高度。通过油门摇杆调节目标高度



运行垂直位置控制器，获取满足高度的电机控制量的 PWM 值

高度控制器的运行周期不要超过 0.2s，则任务该任务控制器是运行状态。

下面代码主要实现：1. 获取目标位置与实际位置的误差值，对误差进行判断是否大于以当前给定的最大加速度和速度计算的刹车距离，如果大于最大刹车距离，则更改当前位置目标值为当前位置加上最大刹车距离_pos_target.z = curr_alt + _leash_up_z;，位置误差设置成最大值为刹车误差，并且限制无人机继续上升标志位生效。如果当前的误差小于当前给定的下降最大刹车距离的负数，更改当前目标高度_pos_target.z = curr_alt - _leash_down_z; 位置误差设置成下降的最大距离的相反数，同时限制无人机下降标志位生效。

2. 采用开平方控制器计算目标速度

3. 获取垂直位置方向上的速度误差

上面函数主要实现判断当前的目标速度是不是小于最大的下降速度和大于最大的上升速度，如果条件满足，则限制当前的目标速度为极限值。并且期望速度的前馈_vel_target.z += _vel_desired.z;

添加期望加速度前馈，为目标加速度的获取做前馈。

存储当前的目标速度，获取当前无人机的垂直速度计算目标速度与实际的速度误差，并且进行 2Hz 的滤波处理。获取目标加速度，增加目标加速度的期望加速度前馈。





上述代码主要实现：1.获取地理坐标系下的垂直加速度；2.计算目标垂直加速度和实际的加速度的误差，并申请低通滤波；3.进行 PID 控制，得到控制调节量；4.转换控制输出量到有效的 PWM 值。5.设置有效的油门输出值，进而控制电机的转速达到目标高度。整体的 run_z_controller()函数实现如下：












>>>> 推荐阅读（点击下方文章标题可直接阅读）

水上飞行摩托战场上的AI无人机Skygauge公司|新型矢量四旋翼荷兰初创企业的“新式”农用无人机加拿大Avidrone双旋翼货运无人机实拍|DJI 大疆创新生产制造作业场景
浅析共轴双旋翼无人直升机系统设计下一个十年的出行利器--飞行汽车(eVTOL）
基于模型设计的一体化无人机飞控开发设计平台

点击下方“阅读原文”学习更多无人机知识