标签：
笔者是来自武汉理工的小青同学，接下来为大家讲一下基于超声波的定高问题，鉴于笔者能力有限，所以如有错误请多指教，且很多仅仅是工程上的近似化应用，没有做过仿真模拟。
我讲的东西更加偏重于实践，可以帮助你切实的实现这个应用！
基于超声波的定高主要有如下几个问题：
硬件和安装技巧？
超声波测距的算法（如何编程，如何处理结果）？
是用什么样子的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是真实的目标值，不能一蹴而就，所以期待的定高，要慢慢去逼近他。不然一次给一个很大的值，很容易过冲，一点点的逼近目标，会比较稳定。这时候只需要慢慢调节逼近速度即可。
四，如何实现稳定的起降？
这个是最具工程性的，而且编程难度实际上很大。
降落来讲，首先低于一个阈值就必须关闭电机，不然四周会在地面跳跃，不稳定；
再就是从高处下落，更好地办法是开环的，慢慢的较低油门，就可以慢慢掉下来。比控制高度的方法效果更高；
起飞也是非常难得。今天先将到这里，下次好好讲一下，稳定起降。
标签：
原文地址：http://www.cnblogs.com/ypwen/p/5862783.html
