CN110018691A - 小型多旋翼无人机飞行状态估计系统和方法 - Google Patents
小型多旋翼无人机飞行状态估计系统和方法 Download PDFInfo
- Publication number
- CN110018691A CN110018691A CN201910319878.4A CN201910319878A CN110018691A CN 110018691 A CN110018691 A CN 110018691A CN 201910319878 A CN201910319878 A CN 201910319878A CN 110018691 A CN110018691 A CN 110018691A
- Authority
- CN
- China
- Prior art keywords
- state
- airborne
- information
- unmanned plane
- aerial vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000005259 measurement Methods 0.000 claims abstract description 61
- 230000000007 visual effect Effects 0.000 claims abstract description 56
- 238000013461 design Methods 0.000 claims abstract description 9
- 238000004891 communication Methods 0.000 claims abstract description 4
- 230000008569 process Effects 0.000 claims description 39
- 230000004927 fusion Effects 0.000 claims description 26
- 238000001914 filtration Methods 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 3
- 241000208340 Araliaceae Species 0.000 claims 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 claims 1
- 235000003140 Panax quinquefolius Nutrition 0.000 claims 1
- 235000008434 ginseng Nutrition 0.000 claims 1
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 abstract description 4
- 238000004519 manufacturing process Methods 0.000 abstract 1
- 230000000694 effects Effects 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 230000007812 deficiency Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 238000005267 amalgamation Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000000386 athletic effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明属于小型多旋翼无人机自主飞行控制领域，为综合输入的运动状态测量信息，得到稳定可靠的无人机完整飞行状态信息，本发明，小型多旋翼无人机飞行状态估计系统和方法,由机载计算机、机载视觉设备、机载惯性测量单元和飞行姿态控制器四部分机载设备构成，机载计算机采用小型通用计算机，并与其他机载设备进行有线通信连接，该机载计算机负责接收来自机载视觉设备的图像信息以及机载惯性测量单元的惯性测量数据，该机载计算机设置有视觉里程计模块、无人机飞行状态估计模块，视觉里程计模块利用由机载视觉设备获取的图像信息计算得到无人机位姿和速度状态。本发明主要应用于小型多旋翼无人机设计制造场合。
Description
技术领域
本发明属于小型多旋翼无人机自主飞行控制研究领域。针对小型多旋翼无人机在运动控制方面需求，提出了一套基于多类型传感器数据融合的小型多旋翼无人机飞行状态估计系统。
背景技术
多旋翼无人机平台已经被广泛应用于多类实际任务，如搜救、巡检和自动化工业等方面。在这些场景中，无人机可能需要在复杂环境中执行任务，例如通过树木丛生的灌木丛、在楼宇间穿梭飞行等任务。
精准、可靠且高频的多旋翼无人机飞行状态信息是实现无人机自主飞行的重要保障。而现今可用于无人机状态测量的传感器具有各类特性，如基于SLAM技术的视觉里程计以及激光雷达里程计都可以提供相对准确的位姿信息，但数据频率较低且受环境影响较大；机载惯性测量单元可以提供机体坐标系下高频的加速度以及角速率信息，但数据噪声较大。目前国内外已有多种基于GPS、机载激光雷达、机载视觉设备的无人机飞行状态测量方法。2014年，德国慕尼黑工业大学提出了一种基于直接法的稠密单目视觉SLAM算法来构建大规模的环境地图。2017年，香港科技大学HKUST Aerial Robotics Group提出了一种飞行状态估计的单目视觉惯性系统，实现了无人机自主导航飞行。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]∈R3，无人机在惯性坐标系下的姿态可以表示为R∈SO(3)，姿态角速率表示为ω＝[ω1ω2ω3]T∈R3，定义映射关系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＝[pm，vm，φ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-KPyyKT。
本发明的特点及有益效果是：
此项发明对于无人机自主飞行控制方法的研究和发展具有十分重要的意义。本发明可以作为无人机自主飞行研究的试验平台的设计方案，进而有助于推进小型多旋翼无人机自主定位控制理论的发展。
本发明的功能和技术特点，是一种基于多传感器信息融合的小型多旋翼无人机飞行状态测量方法，利用机载视觉里程计为多旋翼无人机提供飞行位姿信息和速度信息，通过机载惯性测量单元获取加速度、角速率信息；利用上述输入，应用多传感器信息融合方法，得到一个准确、实时且高频的无人机飞行全状态信息。
附图说明：
附图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]∈R3，无人机在惯性坐标系下的姿态可以表示为R∈SO(3)，姿态角速率表示为ω＝[ω1ω2ω3]T∈R3，定义映射关系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＝[pm，vm，φ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-KPyyKT
下面给出具体的实例：
系统硬件选择及其配置：
1、小型多旋翼无人机
本实例采用如图5所示的轴距700mm无人机，并搭载相应的实验设备，其无人机总重量约为5kg，满载飞行时间最长15分钟，飞行控制器采用Pixhawk开源飞控，机载处理器采用Intel NUC 7代机载计算机。
2、机载感知设备
本案例采用3DM-GX5-25工业级惯性测量单元作为机载惯性测量设备，这是一款基于微型MEMS惯性传感技术的工业级IMU，该传感器能够以1KHz高频输出加速度计以及陀螺仪信息，采用USB3.0接口直接将传感器信息传给机载计算机进行处理。采用Point Greyflea3系列工业相机作为机载视觉设备，采用USB3.1接口将图像信息高速传给机载计算机进行处理。
实验结果
本案例对上述实验平台进行了多组基于多传感器融合的无人机机动过程中的自主定位实验。实验结果如图6-图9所示，图6中点对应视觉里程计的姿态信息输出，实线对应传感器融合系统输出的运动姿态估计，其估计误差可以保证在±5°以内；图7点对应视觉里程计的输出，实线对应传感器融合系统输出的速度状态估计，其估计误差可以保证在±0.2m/s的误差范围内；图8点对应视觉里程计的位置状态输出，实线对应传感器融合系统输出的位置状态估计，其估计误差可以保证在±0.1m的范围内；图9为无人机整个机动过程里程计的位置状态输出以及传感器融合系统的位置状态输出的三维显示。
Claims (6)
1.一种小型多旋翼无人机飞行状态估计系统，其特征是，由机载计算机、机载视觉设备、机载惯性测量单元和飞行姿态控制器四部分机载设备构成，机载计算机采用小型通用计算机，并与其他机载设备进行有线通信连接，该机载计算机负责接收来自机载视觉设备的图像信息以及机载惯性测量单元的惯性测量数据，该机载计算机设置有视觉里程计模块、无人机飞行状态估计模块，视觉里程计模块利用由机载视觉设备获取的图像信息计算得到无人机位姿和速度状态；无人机飞行状态估计模块结合机载惯性测量单元获取的惯性测量数据，进行多传感器状态融合得到无人机自身的飞行状态信息，完成无人机自身飞行状态的估计和上层位置环的稳定性控制；飞行姿态控制器负责底层多旋翼无人机的姿态控制。
2.一种小型多旋翼无人机飞行状态估计方法，其特征是，小型多旋翼无人机飞行状态估计方法，利用安装在多旋翼无人机上的双目视觉设备，通过机载计算机运行的视觉里程计实时获取无人机的位姿以及速度信息；再通过机载惯性测量单元的加速度计和陀螺仪获取高频的加速度和角速率信息，其中，惯性测量信息经低通滤波后与视觉里程计实时获取的无人机的位姿以及速度信息，采用无迹卡尔曼滤波进行多传感器信息融合，得到无人机飞行全状态信息。
3.如权利要求2所述的小型多旋翼无人机飞行状态估计方法，其特征是，无迹卡尔曼滤波器由预测和更新两部分组成，通过传感器IMU数据对无人机运动状态进行预测更新，并利用视觉里程计的信息对运动状态和状态协方差矩阵进行更新，从而获得对应的无人机全运动状态。
4.如权利要求2所述的小型多旋翼无人机飞行状态估计方法，其特征是，具体步骤细化如下：
1)视觉里程计获取状态信息延迟的处理
过程如下：(1)针对每次图像获取时间点的时间戳，并将该时间戳作为由该图像计算得到的位姿和速度信息的时间戳；再利用该时间戳与当前获取到的惯性测量信息的时间戳进行比较，得到视觉里程计信息滞后时间tdelay；(2)又针对惯性测量状态设计缓冲区，存储历史惯性测量信息，在tcurrent时刻应用视觉里程计信息对状态进行校正；(3)里程计状态信息包含了tdelay毫秒的时间滞后，则提取(tcurrent-tdelay)时刻所对应的惯性测量信息和状态参与多传感器数据融合；
2)惯性测量信息的数据预处理
通过低通滤波后的数据发送至状态估计器中再进行多传感器融合，其中，IIR低通滤波算法频域表示如下所示：
其中，m、n、aio,io∈{1,…,m}以及bj,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]∈R3,无人机在惯性坐标系下的姿态可以表示为R∈SO(3)，姿态角速率表示为ω＝[ω1 ω2 ω3]T∈R3,定义映射关系f如下：
该映射关系的逆映射表示为f-1，小型多旋翼无人机的全状态运动学描述表述如下：
v、a分别表示惯性坐标系下的速度、加速度状态，系统状态向量选择为其中，φ,θ,ψ分别表示每次更新状态时在无人机姿态R在欧拉空间上的表达形式，并定义R＝fR(φ θ ψ)，分别表示加速度计和陀螺仪的偏差；
无迹卡尔曼滤波器包括预测和更新两个部分，其中预测部分是指将惯性测量单元的加速度计和陀螺仪测量值作为输入，根据每次更新之间的时间差dt预测出下一时刻的无迹卡尔曼滤波器状态；而更新部分是当获取视觉里程计的数据时运行的，通过视觉里程计的观测数据对无迹卡尔曼滤波器状态进行修正。
5.如权利要求3所述的小型多旋翼无人机飞行状态估计方法，其特征是，预测过程包括如下几步：
首先，以滤波器在第k次预测迭代为例，其系统状态为xk,则定义状态均值为状态的协方差矩阵为Pk,定义传感器的过程噪声均值为噪声的协方差矩阵为Qk，由此，得到滤波器的状态以及对应协方差矩阵
然后，通过应用无迹变换得到sigma点的集合χf(k)的定义如下：
i＝1,…,L
其中，(·)i表示在矩阵·中取第i列，L表示滤波器状态的维度，λ是无迹卡尔曼滤波器的参数；
又以惯性测量单元中加速度计以及陀螺仪的测量值作为输入，通过预测过程模型得到预测的sigma点的集合χf(k+1|k)。
isigma＝0,…,2L
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.ψ)
6.如权利要求2所述的小型多旋翼无人机飞行状态估计方法，其特征是，滤波器运行更新过程，其更新过程包括如下几步：
首先，定义视觉里程计的测量值y＝[pm,vm,φ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-KPyyKT。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910319878.4A CN110018691B (zh) | 2019-04-19 | 2019-04-19 | 小型多旋翼无人机飞行状态估计系统和方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910319878.4A CN110018691B (zh) | 2019-04-19 | 2019-04-19 | 小型多旋翼无人机飞行状态估计系统和方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110018691A true CN110018691A (zh) | 2019-07-16 |
| CN110018691B CN110018691B (zh) | 2022-04-15 |
Family
ID=67191987
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910319878.4A Active CN110018691B (zh) | 2019-04-19 | 2019-04-19 | 小型多旋翼无人机飞行状态估计系统和方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110018691B (zh) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110807942A (zh) * | 2019-09-24 | 2020-02-18 | 联创汽车电子有限公司 | 智能驾驶汽车航迹更新方法及其更新系统 |
| CN110865650A (zh) * | 2019-11-19 | 2020-03-06 | 武汉工程大学 | 基于主动视觉的无人机位姿自适应估计方法 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN111338369A (zh) * | 2020-03-19 | 2020-06-26 | 南京理工大学 | 一种基于非线性逆补偿的多旋翼飞行控制方法 |
| CN111880576A (zh) * | 2020-08-20 | 2020-11-03 | 西安联飞智能装备研究院有限责任公司 | 基于视觉的无人机飞行控制方法及装置 |
| CN111983660A (zh) * | 2020-07-06 | 2020-11-24 | 天津大学 | Gnss拒止环境下四旋翼无人机定位系统和方法 |
| CN112162563A (zh) * | 2020-09-15 | 2021-01-01 | 郑州轻工业大学 | 基于自适应弱敏无迹Kalman滤波的直升机状态估计方法 |
| CN112911225A (zh) * | 2021-01-19 | 2021-06-04 | 深圳科盾量子信息科技有限公司 | 基于量子加密的视频监控方法 |
| CN112991443A (zh) * | 2021-01-13 | 2021-06-18 | 哈尔滨工业大学（深圳） | 基于视觉编码器融合的悬挂负载状态估计器及估计方法 |
| CN113342026A (zh) * | 2021-06-30 | 2021-09-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种小型无人直升机智能平衡控制装置 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN207095576U (zh) * | 2017-08-03 | 2018-03-13 | 上海工程技术大学 | 一种基于imu和视觉里程计的小型四轴飞行器 |
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及系统 |
| CN109029417A (zh) * | 2018-05-21 | 2018-12-18 | 南京航空航天大学 | 基于混合视觉里程计和多尺度地图的无人机slam方法 |
| CN109282808A (zh) * | 2018-11-23 | 2019-01-29 | 重庆交通大学 | 用于桥梁三维巡航检测的无人机与多传感器融合定位方法 |
| CN109358638A (zh) * | 2018-09-10 | 2019-02-19 | 南京航空航天大学 | 基于分布式地图的无人机视觉避障方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
-
2019
- 2019-04-19 CN CN201910319878.4A patent/CN110018691B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN207095576U (zh) * | 2017-08-03 | 2018-03-13 | 上海工程技术大学 | 一种基于imu和视觉里程计的小型四轴飞行器 |
| CN109029417A (zh) * | 2018-05-21 | 2018-12-18 | 南京航空航天大学 | 基于混合视觉里程计和多尺度地图的无人机slam方法 |
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及系统 |
| CN109358638A (zh) * | 2018-09-10 | 2019-02-19 | 南京航空航天大学 | 基于分布式地图的无人机视觉避障方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN109282808A (zh) * | 2018-11-23 | 2019-01-29 | 重庆交通大学 | 用于桥梁三维巡航检测的无人机与多传感器融合定位方法 |
Non-Patent Citations (4)
| Title |
|---|
| HANCHEN LU 等: "State Estimate and Control for Multi-rotors UAV: Theory and Experimentation", 《PROCEEDINGS OF THE 38TH CHINESE CONTROL CONFERENCE》 * |
| TIANRUI HAN 等: "A Semi-dense Direct Visual Inertial Odometry for State Estimator", 《PROCEEDINGS OF THE 38TH CHINESE CONTROL CONFERENCE》 * |
| 王兆圣: "视觉惯性导航的融合与评测方法研究", 《CNKI》 * |
| 鲁夕源: "基于机械臂混合视觉的位姿估计方法研究", 《CNKI》 * |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110807942A (zh) * | 2019-09-24 | 2020-02-18 | 联创汽车电子有限公司 | 智能驾驶汽车航迹更新方法及其更新系统 |
| CN110807942B (zh) * | 2019-09-24 | 2021-11-02 | 上海汽车工业(集团)总公司 | 智能驾驶汽车航迹更新方法及其更新系统 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN110865650B (zh) * | 2019-11-19 | 2022-12-20 | 武汉工程大学 | 基于主动视觉的无人机位姿自适应估计方法 |
| CN110865650A (zh) * | 2019-11-19 | 2020-03-06 | 武汉工程大学 | 基于主动视觉的无人机位姿自适应估计方法 |
| CN111338369A (zh) * | 2020-03-19 | 2020-06-26 | 南京理工大学 | 一种基于非线性逆补偿的多旋翼飞行控制方法 |
| CN111338369B (zh) * | 2020-03-19 | 2022-08-12 | 南京理工大学 | 一种基于非线性逆补偿的多旋翼飞行控制方法 |
| CN111983660A (zh) * | 2020-07-06 | 2020-11-24 | 天津大学 | Gnss拒止环境下四旋翼无人机定位系统和方法 |
| CN111880576A (zh) * | 2020-08-20 | 2020-11-03 | 西安联飞智能装备研究院有限责任公司 | 基于视觉的无人机飞行控制方法及装置 |
| CN111880576B (zh) * | 2020-08-20 | 2024-02-02 | 西安联飞智能装备研究院有限责任公司 | 基于视觉的无人机飞行控制方法及装置 |
| CN112162563A (zh) * | 2020-09-15 | 2021-01-01 | 郑州轻工业大学 | 基于自适应弱敏无迹Kalman滤波的直升机状态估计方法 |
| CN112162563B (zh) * | 2020-09-15 | 2023-01-31 | 郑州轻工业大学 | 基于自适应弱敏无迹Kalman滤波的直升机状态估计方法 |
| CN112991443A (zh) * | 2021-01-13 | 2021-06-18 | 哈尔滨工业大学（深圳） | 基于视觉编码器融合的悬挂负载状态估计器及估计方法 |
| CN112991443B (zh) * | 2021-01-13 | 2022-09-27 | 哈尔滨工业大学（深圳） | 基于视觉编码器融合的悬挂负载状态估计器及估计方法 |
| CN112911225A (zh) * | 2021-01-19 | 2021-06-04 | 深圳科盾量子信息科技有限公司 | 基于量子加密的视频监控方法 |
| CN113342026A (zh) * | 2021-06-30 | 2021-09-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种小型无人直升机智能平衡控制装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110018691B (zh) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110018691A (zh) | 小型多旋翼无人机飞行状态估计系统和方法 | |
| CN106643737B (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
| Kingston et al. | Real-time attitude and position estimation for small UAVs using low-cost sensors | |
| CN109885080B (zh) | 自主控制系统及自主控制方法 | |
| Lai et al. | Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration | |
| CN104460685A (zh) | 一种四旋翼飞行器的控制系统及其控制方法 | |
| CN105021183A (zh) | 多旋翼飞行器gps和ins低成本组合导航系统 | |
| Wang et al. | High-order nonlinear differentiator and application to aircraft control | |
| Burri et al. | A framework for maximum likelihood parameter identification applied on MAVs | |
| CN104280022A (zh) | 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统 | |
| CN108759814B (zh) | 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 | |
| Fresk et al. | A generalized reduced-complexity inertial navigation system for unmanned aerial vehicles | |
| Yang et al. | Multi-sensor data fusion for UAV navigation during landing operations | |
| Lyu et al. | A model-aided optical flow/inertial sensor fusion method for a quadrotor | |
| Rhudy et al. | Wide-field optical flow aided inertial navigation for unmanned aerial vehicles | |
| CN108693372B (zh) | 一种四旋翼飞行器的航向轴角速度估计方法 | |
| Wang et al. | Hardware in the loop based 6DoF test platform for multi-rotor UAV | |
| CN109521785A (zh) | 一种随身拍智能旋翼飞行器系统 | |
| CN116295342A (zh) | 一种用于飞行器勘测的多传感状态估计器 | |
| Yu et al. | Autonomous flight control law for an indoor UAV quadrotor | |
| Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
| Youn et al. | Model-aided synthetic airspeed estimation of UAVs for analytical redundancy | |
| Zheng et al. | UAV attitude measurement in the presence of wind disturbance | |
| Haotian et al. | Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test | |
| Xing et al. | Big data tracking and automatic measurement technology for unmanned aerial vehicle trajectory based on MEMS sensor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |