教程文档

px4算法解析之着陆检测

2019-07-08 08:02:49 0

着陆探测对于一架无人机来说至关重要,如果无法安全检测到无人机的触地信息,飞机可能会出现触地反弹,近地面悬浮,单边着地侧翻等多种危险情况。今天我们就来看看PX4是使用怎么样的算法来检测多旋翼飞机的着陆信息的。

首先,我们打开QGC地面站的调参界面,在Land Detector一栏中,有七个参数,这七个参数分别的作用是什么,我们在后续过程中会一一阐述。

1.jpg

第一个参数LNDMC_ALT_MAX其实跟着陆没什么关系,它主要是用来限制飞行器的飞行高度的,在任务导航和高度控制时会用到,另外,代码里根据飞行器的电量对他做了一定的限制。如果你不想做高度限制,设成-1就不起作用了。

2.jpg


PX4对飞行器的着陆与否有五种状态,它们分别是1FREEFALL2LANDED3MAYBE_LANDED4GROUND_CONTACT5FLYING。这五种状态是互斥的,同一时间只能存在某一种状态。

3.jpg

对于FREEFALL,只要飞行器上的三轴加速度计均方根小于设定的阈值(参数LNDMC_FFALL_THR)持续时间达到参数LNDMC_FFALL_TTRI设定的时间阈值就判定为FREEFALL状态。

4.jpg

而对于第五种FLYING,只要不满足前面四种其中一种就判定为FLYING状态。中间的三种状态是跟着陆探测直接相关的。只有当GROUND_CONTACT满足之后才会有MAYBE_LANDED,只有当MAYBE_LANDED满足之后才会有LANDED。而且这里的状态都是飞行器保持某一状态一定时间以后才会成立的,为区分两个状态之间的区别,我们用小写表示飞行器所保持的状态,而用大写表示最终成立的状态,具体的时间如下:

5.jpg

而状态判断的函数如下,从函数上可以看到判断大写的状态需要小写的状态保持一段时间后才能成立:

6.jpg



先说GROUND_CONTACT,源码里面有考虑到没有GPS状态时的情况,不过整体来讲其实就是判断飞机水平和垂直方向是不是有移动,油门是不是比较低或者垂向期望速度大于着陆速度阈值,这里油门是不是比较低和垂向期望速度两者只要满足其一就可,因为考虑了自动飞行和人在回路以定高或者定点模式飞行的两种情况。

7.jpg

如上图所示,飞行器需要满足1、低油门或者触地;2、测距传感器条件(此处是我自己加的改善版本);3、水平无运动;4、垂直无运动。而触地所需满足的条件是垂向速度期望大于着陆速度相关的某一阈值且垂向无移动。Px4原版代码中使用的垂向速度不是vz还是v_deriv,但是明显效果不好,经过测试后我换成了vz。下图中的maxClimRate为参数LNDMC_Z_VEL_MAX设定的值。而maxVelocity为参数LNDMC_XY_VEL_MAX设定的值。

8.jpg

对于MAYBE_LANDED,第一、要满足前面ground_contact的条件;第二、油门更小(这个条件在满足ground_contact条件之后因为垂向控制会把油门打到最低位,所以一定会满足);第三、角速率条件(参数LNDMC_ROT_MAX设定的值就是下图中的maxRotation_rad_s的值)。

9.jpg

最后LANDED的状态判断比较简单,就是在MAYBE_LANDED的基础上多判断一段时间即可:

10.jpg



到这里px4代码中的着陆判断逻辑就已经阐述完了,除了参数LDNMC_LOW_T_THR用来定义MAYBE_LANDED中的has_minimal_thrust判断之外,几个参数的含义也已经在上面说明了。下面就说一说px4这套判断逻辑的缺陷以及可以如何进行改进。


方法缺陷:

1、使用垂向速度时没有用vz,用的是v_deriv,误差更大,导致飞机出现错判,从而突然掉高。

2、角速度信号在着陆接地时比空中的值更大,反而导致飞行器出现错判。

3、当出现GROUND_CONTACT判断成立后油门随机降为最小值,所以参数LDNMC_LOW_T_THR也失去调节作用,导致误判后飞机突然严重掉高。


改进方案:

目前的改进方案是临时的,应该会有更好的方案。

1、v_deriv改为vz,增加垂向速度精度;

2、添加局部测距传感器作为判断信息;

3、取消角速度判断。