小型多旋翼无人机飞行状态估计系统和方法与流程

本发明属于小型多旋翼无人机自主飞行控制研究领域。针对小型多旋翼无人机在运动控制方面需求，提出了一套基于多类型传感器数据融合的小型多旋翼无人机飞行状态估计系统。

背景技术：

多旋翼无人机平台已经被广泛应用于多类实际任务，如搜救、巡检和自动化工业等方面。在这些场景中，无人机可能需要在复杂环境中执行任务，例如通过树木丛生的灌木丛、在楼宇间穿梭飞行等任务。

精准、可靠且高频的多旋翼无人机飞行状态信息是实现无人机自主飞行的重要保障。而现今可用于无人机状态测量的传感器具有各类特性，如基于slam技术的视觉里程计以及激光雷达里程计都可以提供相对准确的位姿信息，但数据频率较低且受环境影响较大；机载惯性测量单元可以提供机体坐标系下高频的加速度以及角速率信息，但数据噪声较大。目前国内外已有多种基于gps、机载激光雷达、机载视觉设备的无人机飞行状态测量方法。2014年，德国慕尼黑工业大学提出了一种基于直接法的稠密单目视觉slam算法来构建大规模的环境地图。2017年，香港科技大学hkustaerialroboticsgroup提出了一种飞行状态估计的单目视觉惯性系统，实现了无人机自主导航飞行。2018年，美国德雷帕实验室发明了一种状态估计方法，实现了无人机在高速飞行下的自主状态估计，并将该系统命名为“融合惯性传感器的定位和建图”；国内的清华大学、北京航空航天大学、哈尔滨工业大学、天津大学等也开展了相关研究。由此可见无人机在复杂环境下的自主定位与建图是目前国际无人机领域的研究热点之一，上述方案在无人机的飞行状态估方法上取得了一定成果，但仍存在各自方面的不足。传统基于欧拉角的状态估计方法难以实现全自由度的位姿估计；小型多旋翼无人机的运动模型是一种高耦合、强非线性模型，基于扩展卡尔曼滤波方法的状态估计方法难以对其非线性变化进行准确估计。

为了克服现有技术的不足，本发明旨在提出一套适应与复杂环境的小型多旋翼无人机飞行状态方法。方法的实际案例实现中，所用的元器件具有体积小、重量轻的有点。为此，本发明采用的技术方案是，基于多传感器信息融合的小型多旋翼无人机飞行状态估计方法，利用机载视觉里程计采集低频的无人机位姿以及速度信息；通过机载惯性测量单元获取无人机在机体坐标系下高频的三轴加速度和角速率信息，利用上述运动状态测量信息作为状态估计系统输入，应用多传感器信息融合方法，得到一个高频可靠的无人机飞行全状态信息。

技术实现要素：

为克服现有技术的不足，本发明旨在提出一种进行多传感器数据融合算法应用于多旋翼无人机自主飞行系统设计方案中。具体而言，就是利用机载视觉里程计获取无人机飞行位姿以及速度信息，利用机载惯性测量单元获取无人机机体坐标系下的加速度以及角速率信息，综合上述运动状态测量信息作为输入，应用多传感器信息融合方法，得到一个稳定可靠的无人机完整飞行状态信息。

本发明在实际飞行中可实时记录飞行状态数据，以便实验结束后进行数据分析，加以改进自主飞行控制算法，进而促进理论研究的发展。为此，本发明采取的技术方案是，小型多旋翼无人机飞行状态估计系统，由机载计算机、机载视觉设备、机载惯性测量单元和飞行姿态控制器四部分机载设备构成，机载计算机采用小型通用计算机，并与其他机载设备进行有线通信连接，该机载计算机负责接收来自机载视觉设备的图像信息以及机载惯性测量单元的惯性测量数据，该机载计算机设置有视觉里程计模块、无人机飞行状态估计模块，视觉里程计模块利用由机载视觉设备获取的图像信息计算得到无人机位姿和速度状态；无人机飞行状态估计模块结合机载惯性测量单元获取的惯性测量数据，进行多传感器状态融合得到无人机自身的飞行状态信息，完成无人机自身飞行状态的估计和上层位置环的稳定性控制；飞行姿态控制器负责底层多旋翼无人机的姿态控制。

小型多旋翼无人机飞行状态估计方法，利用安装在多旋翼无人机上的双目视觉设备，通过机载计算机运行的视觉里程计实时获取无人机的位姿以及速度信息；再通过机载惯性测量单元的加速度计和陀螺仪获取高频的加速度和角速率信息，其中，惯性测量信息经低通滤波后与视觉里程计实时获取的无人机的位姿以及速度信息，采用无迹卡尔曼滤波进行多传感器信息融合，得到无人机飞行全状态信息。

无迹卡尔曼滤波器由预测和更新两部分组成，通过传感器imu数据对无人机运动状态进行预测更新，并利用视觉里程计的信息对运动状态和状态协方差矩阵进行更新，从而获得对应的无人机全运动状态。

1)视觉里程计获取状态信息延迟的处理

过程如下：(1)针对每次图像获取时间点的时间戳，并将该时间戳作为由该图像计算得到的位姿和速度信息的时间戳；再利用该时间戳与当前获取到的惯性测量信息的时间戳进行比较，得到视觉里程计信息滞后时间tdelay；(2)又针对惯性测量状态设计缓冲区，存储历史惯性测量信息，在tcurrent时刻应用视觉里程计信息对状态进行校正；(3)里程计状态信息包含了tdelay毫秒的时间滞后，则提取(tcurrent-tdelay)时刻所对应的惯性测量信息和状态参与多传感器数据融合；

2)惯性测量信息的数据预处理

通过低通滤波后的数据发送至状态估计器中再进行多传感器融合，其中，iir低通滤波算法频域表示如下所示：

其中，m、n、aio，io∈{1，...，m}以及b，，j∈{1，...，n}均为滤波器参数，则该离散数字滤波器的时域形式表达如下：

y(t)＝a0u(t)+a1u(t-1)+…+amu(t-m)-b1y(t-1)-…-bny(t-n)

基于无迹卡尔曼滤波方法的多传感器数据融合

根据右手惯性坐标系定义惯性坐标为w＝[xw，yw，zw]，惯性坐标系中无人机机体坐标系设定为b＝[xb，yb，zb]，则无人机在空间中的位置坐标表示为p＝[x，y，z]∈r³，无人机在惯性坐标系下的姿态可以表示为r∈so(3)，姿态角速率表示为ω＝[ω1ω2ω3]^t∈r³，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，小型多旋翼无人机的全状态运动学描述表述如下：

v、a分别表示惯性坐标系下的速度、加速度状态，系统状态向量选择为其中，φ，θ，ψ分别表示每次更新状态时在无人机姿态r在欧拉空间上的表达形式，并定义r＝fr(φθψ)，分别表示加速度计和陀螺仪的偏差；

无迹卡尔曼滤波器包括预测和更新两个部分，其中预测部分是指将惯性测量单元的加速度计和陀螺仪测量值作为输入，根据每次更新之间的时间差dt预测出下一时刻的无迹卡尔曼滤波器状态；而更新部分是当获取视觉里程计的数据时运行的，通过视觉里程计的观测数据对无迹卡尔曼滤波器状态进行修正。

其预测过程包括如下几步：

首先，以滤波器在第k次预测迭代为例，其系统状态为xk，则定义状态均值为状态的协方差矩阵为pk，定义传感器的过程噪声均值为噪声的协方差矩阵为qk，由此，得到滤波器的状态以及对应协方差矩阵

然后，通过应用无迹变换得到sigma点的集合χ^f(k)的定义如下：

i＝1，...，l

其中，(·)i表示在矩阵·中取第i列，l表示滤波器状态的维度，λ是无迹卡尔曼滤波器的参数；

又以惯性测量单元中加速度计以及陀螺仪的测量值作为输入，通过预测过程模型得到预测的sigma点的集合χ^f(k+1|k)。

fp(χ^f(k)，u(k))表示预测过程模型函数，其具体计算过程如下：

rk＝fr(φk，ψk，θk)

a＝am-ba

ω＝ωm-bω

pk+1＝pk+vkdt

vk+1＝vk+(rka-zwg)dt

rk+1＝rk(i3+[ω]×dt)

最后，通过预测过程得到sigma点集，由此推出预测均值以及协方差矩阵pk+1|k：

其中，iω＝1，...，2n

其中，pconv定义为滤波器状态预测过程协方差，α、β均为无迹卡尔曼滤波器参数，定义如下：

ep＝xa·p-xb·p

ev＝xa·v-xb·v

ra＝fr(xa.φ，xa.θ，xa.ψ)，rb＝fr(xb.φ，xb.θ，xb.ψ)

滤波器运行更新过程，其更新过程包括如下几步：

首先，定义视觉里程计的测量值y＝[p^m，v^m，φ^m，θ^m，ψ^m]，当滤波器第k次迭代，收到视觉里程计的测量值yk+1，将预测得到的sigma点的集合通过测量模型函数h(·)得到系统的测量sigma点集合，并计算得到测量均值以及测量协方差矩阵pyy，h(·)的具体过程如下：

rk+1|k＝fr(φk+1|k，θk+1|k，ψk+1|k)

其中，表示测量过程协方差矩阵，定义[ep，ev，eψ，eθ，eφ]＝fey(xa，xb)如下：

ep＝xa·p-xb·p

ev＝xa.v-xb.v

ra＝fr(xa.φ，xa.θ，xa.ψ)，rb＝fr(xb.φ，xb.θ，xb.ψ)

最后，通过如下过程完成状态的更新迭代。

pk+1＝pk+1|k-kpyyk^t。

本发明的特点及有益效果是：

此项发明对于无人机自主飞行控制方法的研究和发展具有十分重要的意义。本发明可以作为无人机自主飞行研究的试验平台的设计方案，进而有助于推进小型多旋翼无人机自主定位控制理论的发展。

本发明的功能和技术特点，是一种基于多传感器信息融合的小型多旋翼无人机飞行状态测量方法，利用机载视觉里程计为多旋翼无人机提供飞行位姿信息和速度信息，通过机载惯性测量单元获取加速度、角速率信息；利用上述输入，应用多传感器信息融合方法，得到一个准确、实时且高频的无人机飞行全状态信息。

附图说明：

附图1多传感器数据融合系统结构图。

附图2信息延迟处理机制原理图。

附图3低通滤波器系统结构图。

附图4无迹卡尔曼滤波器原理图。

附图5多旋翼无人机实验平台实物图。

附图6多传感器融合姿态估计效果图。

附图7多传感器融合速度状态估计效果图。

附图8多传感器融合位置状态估计效果图。

附图9多传感器融合位置状态估计三维效果图。

具体实施方式

本发明所涉及的小型多旋翼无人机状态估计系统主要由硬件部分和软件部分两个部分组成。

硬件部分包括机载计算机、机载视觉设备、机载惯性测量单元和飞行姿态控制器，其中机载计算机采用小型通用计算机，并与其他机载设备进行有线通信连接，该机载计算机负责接收来自机载视觉设备的图像信息以及机载惯性测量单元的惯性测量数据，并运行本发明的软件部分，完成无人机自身飞行状态的估计和上层位置环的稳定性控制；飞行姿态控制器一般采用pixhawk开源飞行控制器，负责底层多旋翼无人机的姿态控制。

软件部分包括无人机飞行状态估计软件，该软件均于linux环境下结合ros框架进行开发，负责融合多传感器信息得到无人机飞行状态。

小型多旋翼无人机状态估计系统工作原理为：首先机载计算机运行视觉里程计软件，利用由机载视觉设备获取的图像信息计算得到无人机位姿和速度状态；再利用无人机飞行状态估计软件，结合机载惯性测量单元获取的惯性测量数据，进行多传感器状态融合得到无人机自身的飞行状态信息。

下面结合附图对本发明的小型多旋翼无人机自主状态估计方案给出详细说明。

本发明所采用的技术方案是：利用安装在多旋翼无人机上的双目视觉设备，通过机载计算机运行的视觉里程计软件实时获取无人机的位姿以及速度信息；再通过机载惯性测量单元的加速度计和陀螺仪获取高频的加速度和角速率信息，采用如图1所示的自主状态估计方法实现对无人机自身运动状态的估计。

本发明所采用的自主状态估计方案主要包括如下几个步骤：

1)视觉里程计获取状态信息延迟的处理

利用视觉里程计技术进行无人机位姿测量时，视觉里程计软件对图像信息的计算过程占用较多计算资源，且会消耗较长的计算时间，其测量信息包含了一定的时间延迟，而非当前时刻的真实值。设计如图2所示的信息延迟处理机制对惯性测量信息进行处理，具体过程如下：(1)针对每次图像获取时间点的时间戳，并将该时间戳作为由该图像计算得到的位姿和速度信息的时间戳；再利用该时间戳与当前获取到的惯性测量信息的时间戳进行比较，得到视觉里程计信息滞后时间tdelay(ms)；(2)又针对惯性测量状态设计缓冲区，存储历史惯性测量信息，其中惯性测量信息一般较为高频，在实例中惯性测量的频率约为1khz，其时间间隔约为1ms，假设在tcurrent时刻应用视觉里程计信息对状态进行校正时；(3)里程计状态信息包含了tdelay(ms)的时间滞后，则提取(tcurrent-tdelay)时刻所对应的惯性测量信息和状态参与多传感器数据融合。

2)惯性测量信息的数据预处理

利用惯性测量单元直接获取无人机的惯性测量信息，机载惯性测量单元可以提供高频的机体坐标系下的惯性测量信息，而其状态信息包含较大噪声，产生噪声的原因有多种，主要包括惯性测量单元的测量噪声以及旋翼转动时震动所产生的噪声，考虑小型多旋翼无人机控制系统的带宽，设计结构如图3所示截止频率约为30hz的iir低通滤波器，通过低通滤波后的数据发送至状态估计器中再进行多传感器融合。其中，iir低通滤波算法频域表示如下所示：

其中，m、n、ai，i∈{1，...，m}以及bj，j∈{1，...，n}均为滤波器参数，则该离散数字滤波器的时域形式表达如下：

y(t)＝a0u(t)+a1u(t-1)+…+amu(t-m)-b1y(t-1)-…-bny(t-n)

3)基于无迹卡尔曼滤波方法的多传感器数据融合

基于多传感器融合的小型多旋翼无人机状态估计方法，其特征是以经过低通滤波后的惯性测量数据和由视觉里程计软件提供的无人机位姿信息作为输入，无迹卡尔曼滤波器的应用流程如图4所示，整体滤波器分为预测和更新两部分组成，通过imu数据对无人机运动状态进行预测更新，并利用视觉里程计的信息对运动状态和状态协方差矩阵进行更新，通过改进的无迹卡尔曼滤波器获得对应的无人机全运动状态。

设计右手惯性坐标系定义惯性坐标为w＝[xw，yw，zw]，惯性坐标系中无人机机体坐标系设定为b＝[xb，yb，zb]，则无人机在空间中的位置坐标可以表示为p＝[x，y，z]∈r³，无人机在惯性坐标系下的姿态可以表示为r∈so(3)，姿态角速率表示为ω＝[ω1ω2ω3]^t∈r³，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，小型多旋翼无人机的全状态运动学描述表述如下：

v、a分别表示惯性坐标系下的速度、加速度状态。系统状态向量选择为其中，φ，θ，ψ分别表示每次更新状态时在无人机姿态r在欧拉空间上的表达形式，并定义r＝fr(φθψ)，分别表示加速度计和陀螺仪的偏差，整体系统数据流程图如图1所示。

无迹卡尔曼滤波器包括预测和更新两个部分，其中预测部分是指将惯性测量单元的加速度计和陀螺仪测量值作为输入，根据每次更新之间的时间差dt预测出下一时刻的滤波器状态；而更新部分是当获取视觉里程计的数据时运行的，通过视觉里程计的观测数据对滤波器状态进行修正。其预测过程包括如下几步：

首先，以滤波器在第k次预测迭代为例，其系统状态为xk，则定义状态均值为状态的协方差矩阵为pk，定义传感器的过程噪声均值为噪声的协方差矩阵为qk。由此，可以得到滤波器的状态以及对应协方差矩阵

然后，通过应用无迹变换得到sigma点的集合χ^f(k)的定义如下：

i＝1，...，l

其中，(·)i表示在矩阵·中取第i列，l表示滤波器状态的维度，λ是无迹卡尔曼滤波器的参数。

又以惯性测量单元中加速度计以及陀螺仪的测量值作为输入，通过预测过程模型得到预测的sigma点的集合χ^f(k+1|k)。

fp(χ^f(k)，u(k))表示预测过程模型函数，其具体计算过程如下：

rk＝fr(φk，ψk，θk)

a＝am-ba

ω＝ωm-bω

pk+1＝pk+vkdt

vk+1＝vk+(rka-zwg)dt

rk+1＝rk(i3+[ω]×dt)

最后，通过预测过程得到sigma点集，由此可以推出预测均值以及协方差矩阵pk+1|k。

其中，iω＝1，...，2n

其中，pconv定义为滤波器状态预测过程协方差，α、β均为无迹卡尔曼滤波器参数，定义如下：

ep＝xa·p-xb·p

ev＝xa·v-xb·v

ra＝fr(xa.φ，xa.θ，xa.ψ)，rb＝fr(xb.φ，xb.θ，xb.ψ)

而滤波器运行更新过程，其更新过程包括如下几步：

首先，定义视觉里程计的测量值y＝[p^m，v^m，φ^m，θ^m，ψ^m]，当滤波器第k次迭代，收到视觉里程计的测量值yk+1，将预测得到的sigma点的集合通过测量模型函数h(·)得到系统的测量sigma点集合，并计算得到测量均值以及测量协方差矩阵pyy，h(·)的具体过程如下：

rk+1|k＝fr(φk+1|k，θk+1|kψk+1|k)

其中，表示测量过程协方差矩阵，定义如下：

ep＝xa·p-xb·p

ev＝xa·v-xb·v

ra＝fr(xa.φ，xa.θ，xa.ψ)，rb＝fr(xb.φ，xb.θ，xb.ψ)

最后，通过如下过程完成状态的更新迭代。

pk+1＝pk+1|k-kpyyk^t

下面给出具体的实例：

系统硬件选择及其配置：

1、小型多旋翼无人机

本实例采用如图5所示的轴距700mm无人机，并搭载相应的实验设备，其无人机总重量约为5kg，满载飞行时间最长15分钟，飞行控制器采用pixhawk开源飞控，机载处理器采用intelnuc7代机载计算机。

2、机载感知设备

本案例采用3dm-gx5-25工业级惯性测量单元作为机载惯性测量设备，这是一款基于微型mems惯性传感技术的工业级imu，该传感器能够以1khz高频输出加速度计以及陀螺仪信息，采用usb3.0接口直接将传感器信息传给机载计算机进行处理。采用pointgreyflea3系列工业相机作为机载视觉设备，采用usb3.1接口将图像信息高速传给机载计算机进行处理。

实验结果

本案例对上述实验平台进行了多组基于多传感器融合的无人机机动过程中的自主定位实验。实验结果如图6-图9所示，图6中点对应视觉里程计的姿态信息输出，实线对应传感器融合系统输出的运动姿态估计，其估计误差可以保证在±5°以内；图7点对应视觉里程计的输出，实线对应传感器融合系统输出的速度状态估计，其估计误差可以保证在±0.2m/s的误差范围内；图8点对应视觉里程计的位置状态输出，实线对应传感器融合系统输出的位置状态估计，其估计误差可以保证在±0.1m的范围内；图9为无人机整个机动过程里程计的位置状态输出以及传感器融合系统的位置状态输出的三维显示。