PX4原生固件，position_estimator

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

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

PX4原生固件，position_estimator_inav解读相关推荐

PX4原生固件SPI驱动动编写与IMU传感器替换
适用于PX4原生固件核心目标:完成XSENS的MTI3,IMU替换.MTI3是一款航姿参考系统,可以独立的输出四元数,加速度,磁力计等,角速度等航姿信息.里面有完整的卡尔曼滤波,可以替换飞控本身里面 ...
Pixhawk原生固件PX4之常用函数解读
PX4Firmware 经常有人将Pixhawk.PX4.APM还有ArduPilot弄混.这里首先还是简要说明一下: Pixhawk是飞控硬件平台,PX4和ArduPilot都是开源的可以烧写到Pi ...
Windows / Ubuntu操作系统下Pixhawk原生固件PX4的编译方法
欢迎交流~ 个人 Gitter 交流平台,点击直达: 更新于2017.3.13 FAQ 本文说明针对 PX4 Firmware 1.6.0 问题 1: 找不到python jinja2模块 CMake ...
网件使用计算机mac地址吗,网件原生固件设置无线MAC地址过滤详细步骤
摘要原标题:"网件R6300v2原生固件设置无线MAC地址过滤方法"相关路由器设置经验分享. - 来源:路由器之家路由器具有判断网络地址和选择IP路径的功能,它能在多网络互联 ...
Pixhawk原生固件PX4之位姿控制算法解读
参考文献:Minimum Snap Trajectory Generation and Control for Quadrotors PX4中多旋翼无人机的控制分为姿态控制和位置控制两个部分. 大致解 ...
Pixhawk原生固件PX4之顶层软件结构
下图所示为PX4的软件顶层架构,很重要. 左边是整个飞控系统的控制功能实现用户通过地面站或者遥控器发出模式切换以及摇杆操作对飞行器进行控制,commander根据飞行器的当前状态对用户想要切换到的状 ...
Pixhawk原生固件PX4之串口添加读取传感器实现
本博客承接前一篇,对FreeApe的串口添加超声波传感器博文后半部分进行学习. 为什么叫前奏呢,因为用了伪传感器,把单片机用串口发送的有规律的数据当作了传感器读取到的数据.但是无碍的. 开发环境:Ub ...
Pixhawk原生固件PX4之添外置传感器MPU6500
欢迎交流~ 个人 Gitter 交流平台,点击直达: 成功的在Pixhawk上添加了一个自定义的传感器MPU6500. Pixhawk飞控板上空余出一个SPI4接口提示: 多出来的GPIO_EXT引 ...
Pixhawk原生固件PX4之驱动ID
欢迎交流~ 个人 Gitter 交流平台,点击直达: 驱动ID PX4使用驱动ID将独立传感器贯穿于整个系统.这些ID存储于配置参数中,用于匹配传感器校正值,以及决定哪些传感器被记录到log中. 传感 ...
Pixhawk原生固件PX4之HIL硬件在环仿真
欢迎交流~ 个人 Gitter 交流平台,点击直达: 硬件在环仿真可以接遥控器进行调试. 串口设置下载minicom sudo apt-get install minicom 配置飞控的设备号(/d ...

PX4原生固件，position_estimator_inav解读

PX4原生固件，position_estimator_inav解读相关推荐

最新文章

热门文章