笔者是来自武汉理工的小青同学，接下来为大家讲一下基于超声波的定高问题，鉴于笔者能力有限，所以如有错误请多指教，且很多仅仅是工程上的近似化应用，没有做过仿真模拟。

我讲的东西更加偏重于实践，可以帮助你切实的实现这个应用！

基于超声波的定高主要有如下几个问题：

硬件和安装技巧？

超声波测距的算法（如何编程，如何处理结果）？

是用什么样子的PID算法？

如何调试更加快捷？

实际开发将会遇到哪些棘手问题？

定高控制如何实现较为稳定为起降？

笔者做定高用了一个星期，而且参考了别的算法，实现比较简单，但是真正达到一个很好的效果，却经历了很多挫折。

那么我将展开讨论了：

如图1是我搭建的四轴平台，花了2000左右，PCB是自己做的，模块是自己的配的，调试地面站是匿名地面站，通过蓝牙通信；当然这不是今天的主题，笔者假设你已经得到了很好地姿态控制。

一.硬件和安装技巧：

1.超声波的特性是这样的，当模块被东西突然遮住的时候（1cm之内或者更小），那么测量值就会发生突变，非常之大，那么这对控制来说是毁灭性的。所以你的超声波要安装到飞行器的正下方，而且要避免跟电池放在一起。容易干扰。此外，需要做阈值处理，如果测量值突变，则舍弃测量结果。

理论上讲超声波放在飞行器的正下面的中心是最好的，这样测出来的距离更加精确。如图2。如果飞行器俯仰角不为0，从数学上讲要进行补偿，但是实际上你们可以算一下角度变化10以内的时候对这个距离造成的距离影响很小。所以并不需要PPT里面所讲的这种几何补偿，而且在STM32中三角函数的计算量非常大，就算是换成快速三角函数，造成的计算误差，远远大于实际测量误差，补偿反而得不偿失。而且超声波也没有明显的偏移。PPT的作者可能并没有实际实现过。超声波也不需要装在正下方，装在支架的两边可以。因为正下方可是要放摄像头的。

如果在程序上不精密设计，不合理的线程设计，不合理的计算设计，都会导致程序失控。

2.如何测量和处理数据：

超声波有的采用串口协议，我个人觉得串口协议的蛮好用，下面是程序截图，语句1意思是测出的数据是两个字节得到的，第一个字节左移八位，加上第二个字节。两者距离，单位是mm,笔者实测，超声波模块的精度很高。

测量得到原始的ultra_distance，需要处理，不然得到的数据是上下抖动的。

笔者没有保存图，所以下面这个不是很明显，还是可以看出跳动。通常有3种滤波算法：

1.均值滤波

2.低通滤波

3.kalman滤波

笔者实测，第二三中效果差不多，且低通滤波更加易于实现，第一种的效果很差，那么我们来看一下低通滤波。

这里针对不同的高度做了不同频率的低通滤波，这个形式是基于滤波频率的，比较难懂，大家可以看下下面这种：

Yn=a* Xn+（1-a）*Yn-1 

Yn-1是上次测量值，Yn是这次测量值，得到输出，截止频率是

F = a /(2*3.14*T)

T为采样周期；

如上就是代码实现，其中通过右移8位的方式实现除法，速度更快。计算可得截止频率是50HZ

二、用了什么样子的PID？

当然是串级PID了。

一阶PID也是可以实现的。但是明显不稳定，而且控制复杂。需要在不同的高度调节不同的参数。串级PID则比较稳定，问题较少。上图是PixHawk的PID算法。实际上我用的内环是速度pID，即飞行器上升下降的速度，外环当然是高度PID了。

整体实现不讲了，就是PID算法，说一下细节。比如说，如何得到速度，这里需要用到融合滤波算法：

    y = (a)*(y) + (1-a)*(x);

这个是不同于低通滤波是一种一阶融合滤波，将两种测量结果，融合在一起，兼顾两种测量好的效果，抵消掉不利的效果。

wz-speed是通过重力加速度积分测得的速度，h_speed是超声波差分得到的。代码的一个句就是对加速度进行积分，第二句是死区控制，有点麻烦，这里不提。

三四句对加速度测量做了低通滤波，第五句，通过融合两种速度，得到更加稳定的结果。你可以在地面站上去看这个曲线，非常漂亮，只是速度回复稍微有点慢。

外环就是对高度进行控制，这里需要注意几点：

1.高度要做限制，不能过高；

2.无人机启动从地面起飞的时候，要设定一个固定的较大的高度值，提供一个稳定的PID控制，超过了一定的高度再更改目标高度，比如说无人机静止时候测量得到20CM，那么打开油门后设定目标高度为40,飞机上升到40，在继续加高度。因为无人机有地面效应，在地面的时候有更大的推力。

这里是工程实践得到的经验。

三、如何调试更加快捷？

实际开发将会遇到哪些棘手问题？

如何调试定高，真的挺费劲的。需要反复更改PID。

而且还有很多棘手问题，我当时调试，就是因为回来一个头，结果没看住无人机飞到天下，把天花板撞烂了。四轴的底层很难调，经常出问题。出一个问题弄不好就是毁灭性的。

我是在四轴的两个方向上加了两个支柱，这样飞起来的时候，不好的话，可以抓住它。还有第二个好处，就是定高的时候悬停是没有的，所以飞机四处飘，很难调定高，你可以是不是的戳一下杆子，让他保持在一个范围内。还有一个好处测量你姿态的稳定度。好的稳定度，你去剧烈摇摆杆子，姿态可以很快地恢复。不会崩溃。但是千万不要用杆子去旋转四轴，那样基本就雪崩。

首先先调内环。外环不管。把油门加到可以让无人机起飞。接下里，调P，将P调到一个合适的位置，你剧烈的上下晃动四轴，无人机会出下一个很大的阻尼，阻尼越平滑，反应越快越好，你将四轴向上推，四轴转速降低，向下推转速开始对看你的推力。调的较好以后就徒手悬空放开四轴，会发现四轴慢慢的飘落，如果速度保持为0的性能越好，则P越好。然后调D，D差不多就行了。关键是I ，这个需要长时间调试去感受了。没有I的话，效果很差。

然后就是调高度环，先调50cm定高，再调100cm定高，再是30cm，越低越难调。四轴能够稳定的到达目的地那是最好的、

这里有一个技巧：

exp_height是定高控制值。height_value是真实的目标值，不能一蹴而就，所以期待的定高，要慢慢去逼近他。不然一次给一个很大的值，很容易过冲，一点点的逼近目标，会比较稳定。这时候只需要慢慢调节逼近速度即可。

四，如何实现稳定的起降？

这个是最具工程性的，而且编程难度实际上很大。

降落来讲，首先低于一个阈值就必须关闭电机，不然四周会在地面跳跃，不稳定；

再就是从高处下落，更好地办法是开环的，慢慢的较低油门，就可以慢慢掉下来。比控制高度的方法效果更高；

起飞也是非常难得。今天先将到这里，下次好好讲一下，稳定起降。

基于超声波的四轴定高控制简析