PX4原生固件，position_estimator_inav解读

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的测量值进行积分的话，就失去了利用量测值进行补偿的作用。

    这个地方说的比较模糊，后面会严格推倒公式，就一目了然了。