[云讷科技] Kerloud PX4飞控的EKF2程序导航

一、介绍

[云讷科技] Kerloud PX4飞控的EKF2程序导航EKF拓展卡尔曼滤波器是px4开源飞控框架采用的核心状态估计方法,EKF2是px4飞控中的对应的软件模块,可以支持各类传感器信号,包括IMU,磁感计,激光测距仪,气压计,激光定位和视觉定位的数据等。
本文以Kerloud飞控支持的px4软件版本为例,系统地介绍下EKF的程序流程,Kerloud飞控是云讷科技的核心产品之一。
Kerloud飞控支持维护的px4软件库位于:https://github.com/cloudkernel-tech/Firmware
Kerloud飞控的详细说明位于:http://cloudkernel-tech.gitee.io/kerloud_mini/

二、工作原理

EKF是经典卡尔曼滤波器的非线性版本,通过对非线性系统进行线性化实现,它是非线性状态估计,导航和GPS设备中的标杆性算法。
[云讷科技] Kerloud PX4飞控的EKF2程序导航[云讷科技] Kerloud PX4飞控的EKF2程序导航

三、程序框架

1. 基本程序框架

[云讷科技] Kerloud PX4飞控的EKF2程序导航

2. 视觉SLAM融合说明

这里以视觉SLAM的位置融合说明下EKF的融合细节,
首先,假设从机载端电脑或者其他信息可以得到视觉SLAM输出的位姿,可以通过mavlink 协议的VISION_POSITION_ESTIMATE或者ODOMETRY消息输入给飞控,区别在于VISION_POSITION_ESTIMATE只包含位置信息,而ODOMETRY还可以得到速度、姿态和姿态角速度等。

飞控中对应的代码处理在
https://github.com/cloudkernel-tech/Firmware/blob/master_kerloud/src/modules/mavlink/mavlink_receiver.cpp

对应的函数为handle_message_vision_position_estimate()和handle_message_odometry(),我们需要注意的是kerloud飞控默认支持的坐标系为NED坐标系,这点很重要。得到的SLAM信息会通过uorb topic vehicle_visual_odometry发布出来。

EKF2模块(src/modules/ekf2/ekf2_main.cpp)在line 1127开始会接受vehicle_visual_odometry topic的值,给对应位置、速度或者姿态赋值。

External vision innovation 计算:
水平方向计算在: src/lib/ecl/EKF/control.cpp的line 307
竖直方向计算在: vel_pos_fusion.cpp的line 150
Innovation variance计算在vel_pos_fusion.cpp的line 167,对应K值修正计算在line 234行,
最后的状态修正在line 315行。

在使用视觉SLAM融合时,我们需要注意底层飞控的融合设置,包括方差估计,高度,磁感计等,好的效果都需要充分的测试,同时飞行器平台的振动要求也需要考虑在内。

四、更多信息

云讷科技公众号二维码:

[云讷科技] Kerloud PX4飞控的EKF2程序导航

上一篇:XPath语法和lxml模块


下一篇:Android LeakCanary的使用和原理,移动端app开发框架