INAV-----integrated navigation组合导航。
对于多旋翼的位置姿态估计系统:
PX4原生固件如今已经默认使用EKF2了,另一种情况是 使用local_position_estimator, attitude_estimator_q。
对于INAV则已经弃用。所以如果是基础比较好的同仁,可以直接研究EKF2。
不过博主是新进小白,刚开始研究位置和姿态解算。从互补滤波入手或许是一个快速入门的选择。
INAV干了什么:
INAV,顾名思义,隶属于导航系统,且是组合导航系统。导航,简而言之,就是对运动物体运动状态的测量。
这个运动状态对于飞行器来讲,主要是三个线位置,三个线速率,三个欧拉角,三个角速度。
在PX4中,INAV提供了飞行器的三轴线位置和三轴线速率,是对飞行器线运动的测量。
INAV使用什么方法:
互补滤波。
既然是组合导航,自然是使用了多种传感器,需要多种传感器数据进行融合。互补滤波是一种融合两种或者多种
不同来源数据的方法。
INAV使用了什么传感器:
加速度计,GPS,PX4FLOW,气压计,vision,lidar,mocap。
加速度计,测量飞行器本体系下的三轴加速度;
GPS ,测量飞行器的经纬高,和北东地坐标系下的速度;
PX4FLOW, 测量飞行器在当前位置水平方向上的运动;
气压计,测量飞行器的当前位置的气压值,可转化为北东地坐标系下的海拔高度;
vision ,视觉定位,博主不具备,一般小型多旋翼也不具备;
lidar,测量飞行器当前位置的当地高度;
mocap,运动捕捉系统,一般不具备。
INAV 融合方法:
全部体现在下面两个函数。
inertial_filter_predict 估计函数。
void inertial_filter_predict(float dt, float x[2], float acc)
{
if (PX4_ISFINITE(dt)) {
if (!PX4_ISFINITE(acc)) {
acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
x[1] += acc * dt;
}
}
inertial_filter_correct 校正函数。
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) {
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
}
}
}
估计函数:这里虽然叫预估,但是实际就是数值积分。输入是上一时刻的状态X。X[0]是位置,X[1]是速率。
X[0] = X[0] + x[1] * dt + acc * dt * dt / 2.0f -------> s = v*t + 0.5*a*t*t 位移公式
X[1] = X[1] + acc*dt --------> v = a*t 速率公式
显然如果我们能够获得准确的acc,就能够通过上述公式经过积分获得飞行器的位置和速率。
那我们如何才能获得准确的acc呢?
acc是由加速度计测量得到的,但是acc掺杂了加速度计本身的测量噪声,机体的振动,载体的运动加速度等干扰因素。
如果直接使用,势必会影响积分的精度,而且积分本身就会产生累积误差。
显然直接使用加速度计测出的acc是不可行的。
这就需要用其他传感器对acc进行校正了。
校正函数:校正分为两个部分。
一个部分是对位置和速率的直接校正,这也是校正函数做的事情,可以称之为直接校正。
另一部分是对acc的校正,这是在程序过程中做的,后面会讲到,可以称之为间接校正。
校正函数的形参解释:
e 代表由加速度得到的位置和速率,与其他传感器得到的位置和速率分别做差得到的偏差。
w 代表权重,或者补偿系数。
所以校正函数的意思就是如果由加速度积分得到的位置和速率,与其他传感器得到的位置
和速率有偏差的时候,就用这个w乘以e再乘以dt补偿到加速度得到的位置和速率上。
这是为什么呢?
因为加速度积分得到的结果,随着时间的推移,数值会发散。而其他传感器的结果则是有
界的。所以,直接校正可以立即使加速度的积分结果趋近于真实结果,是对其发散的一种限制。
但是其他传感器数据的获取频率是比较低的,并不能满足实时性的要求。
所以加速度积分仍然是必须的。由此,便引出了间接校正。
我们认为加速度积分得到的数值的误差是由于acc不准确引起的,所以每一次直接校正后,都对
acc进行补偿,也就是间接校正。这样在其他传感器没有数据的时候,利用校正后的acc积分,
短时间内,精度也是可以接受的。
以上介绍了INAV模块的整体思路,整个程序就是估计和校正的循环往复的过程。
///
下面我们一起看看INAV的具体代码实现吧:
首先,介绍一下params.c中的参数。这些参数在程序中反复出现,从命名可以区分它们的含义和作用。
INAV+权重+方向+传感器+含义 默认值 最大 最小 简单描述
INAV_W_Z_BARO 0.5 10 0 气压计高度方向 位置补偿系数
INAV_W_Z_GPS_P 0.005 10 0 GPS高度方向 位置补偿系数,只用来校正气压计的零偏
INAV_W_Z_GPS_V 0 10 0 GPS高度方向 速率补偿系数
INAV_W_Z_VIS_P 5 10 0 视觉高度方向 位置补偿系数,只用来校正气压计的零偏
INAV_W_Z_LIDAR 3 10 0 雷达高度方向 位置补偿系数
INAV_W_XY_GPS_P 1 10 0 GPS水平方向 位置补偿系数
INAV_W_XY_GPS_V 2 10 0 GPS水平方向 速率补偿系数
INAV_W_XY_VIS_P 7 10 0 视觉水平方向 位置补偿系数
INAV_W_XY_VIS_V 0 10 0 视觉水平方向 速率补偿系数
INAV_W_MOC_P 10 10 0 运动捕捉系统 位置补偿系数
INAV_W_XY_FLOW 0.8 10 0 光流水平方向 位置补偿系数
***************************************************************************************************************************************
INAV_W_XY_RES_V 0.5 10 0 速度源丢失时,缓慢的降低估计的水平速度时,使用的权重
INAV_W_GPS_FLOW 0.1 1 0 光流有效时,用这个系数,降低GPS的位置和速率的补偿系数
INAV_W_ACC_BIAS 0.05 0.1 0 加速度计零偏估计 补偿系数
INAV_FLOW_K 1.35 10 0 光流线性度的系数
INAV_FLOW_Q_MIN 0.3 1 0 最小可以接受的光流质量
INAV_LIDAR_ERR 0.2 1 0 雷达最大误差,用于判断是否到达了新的表面
INAV_LAND_T 3 10 0 落地检测时间,没有高度改变,在低油门的时候,单位s
INAV_LAND_DISP 0.7 10 0 落地检测高度,进入该高度,触发落地检测
INAV_LAND_THR 0.2 1 0 落地检测油门,进入该油门,触发落地检测,最好为悬停油门的一半
INAV_DELAY_GPS 0.2 1 0 GPS延时,单位s
INAV_FLOW_DIST_X 0 1 -1 光流模块在X方向偏离质心的距离,单位m
INAV_FLOW_DIST_Y 0 1 -1 光流模块在Y方向偏离质心的距离,单位m
INAV_DISAB_MOCAP 0 1 0 0使能运动捕捉,1禁用运动捕捉,如果使用虚拟GPS,设为0
INAV_LIDAR_EST 0 布尔量,雷达用于高度估计
INAV_LIDAR_OFF 0 20 -20 雷达零偏,单位m
CBRK_NO_VISION 0 328754 0 禁用视觉输入,设为328754可以禁用视觉输入
这些参数主要是补偿用的系数,有一些判定条件的阈值,还有几个开关量。
下面开始介绍程序实现。
INAV的程序结构非常简单,并不是本文的重点,在这里就不进行介绍了。
主要介绍INAV估计和校正的实现函数:int position_estimator_inav_thread_main(int argc, char *argv[])。
所有的数据处理都在这个函数中。
先介绍下大致的工作流程:
主要分三大部分:
第一部分,是变量定义和初始化。还有就是对uorb的主题规定订购那些主题,发布那些主题。
第二部分,是计算气压高的零偏。我们知道气压计是能够测出海拔高度的,但是用海拔高度
表示,没有直观性。我们都利用当地高度来衡量飞机的高度。因此,在起飞之前
测出飞机起飞点的海拔就很有必要。这里是按多次取平均的方法计算原点高度的。
第三部分,是位置估计和校正。
下面着重介绍第三部分的程序:
通过上面的介绍,我们可以将位置估计和校正的过程分成三个环节。
1 计算合适的e
e就是估计值和测量值的差值。测量值是通过传感器直接测得的位置和速率的数值。通过消除这个e,使估计值始终不会发散。
2 计算合适的w
我们使用了很多种传感器。每个传感器能够提供的信息是有限的,并且精度也不一样。
所以,由每个传感器得出的e,需要乘以一个系数,表示它们在估计过程中的权重。
例如气压计只能提供高度位置信息,不能提供高度速率信息。gps既能提供xyz三个方向的位置信息,也能提供速率信息。
但是气压计高度方向的位置精度高,能够达到20~30cm。gps的高度方向的位置精度则只能达到10m。
这就决定了它们在融合过程中的权重会依据自身的特点而各有侧重。
另一方面,有些传感器会受到环境的影响,而导致其测量精度是时变的。其发送回来的数据包里面经常包含quality的数据。
这是用来衡量传感器的工作状态的值,根据这个值得大小,也许进行权重的调整。
3 合理的补偿acc
有了e和w就可以利用上面介绍的估计和校正函数进行数据融合了。但是这样实际上并没有发挥出加速度计本身的特点。
对于结果而言,只是把所有量测数据进行了加权。对于量测噪声也没有很好抑制作用。
而对于估计值,仍然使用acc的测量值进行积分的话,就失去了利用量测值进行补偿的作用。
这个地方说的比较模糊,后面会严格推倒公式,就一目了然了。