首页 > 代码库 > 基于超声波的四轴定高控制简析

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

 

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

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

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

硬件和安装技巧?

超声波测距的算法(如何编程,如何处理结果)?

是用什么样子的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是真实的目标值,不能一蹴而就,所以期待的定高,要慢慢去逼近他。不然一次给一个很大的值,很容易过冲,一点点的逼近目标,会比较稳定。这时候只需要慢慢调节逼近速度即可。

四,如何实现稳定的起降?

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

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

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

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

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