基于飞行器的视觉导航与惯性导航的组合导航方法与流程

本发明属于组合导航技术领域，具体涉及一种基于飞行器的视觉导航与惯性导航的组合导航方法。

背景技术：

飞行器具有机械结构简单、运动灵活、姿态多样、可扩展性好和易维护等优点，因而在勘探、测绘、救援、航拍等领域扮演越来越重要的角色。而这些领域无一不要求相当高的精确性、可靠性和自主性，现有的飞行器导航方法多采用gps导航，在室外的定位精度可以达到1.5米，对于一般用户的定为服务可以满足。然而在无人机上，普通廉价的gps接收机的精度不能满足各种复杂姿态的导航精度，并且在复杂环境下难以定位，如大型工厂内部、山区和矿井下方等环境下，因此，现如今缺少一种基于飞行器的视觉导航与惯性导航的组合导航方法，通过对惯性传感器所采集的数据进行四元数解算，得出飞行器空间运动姿态的欧拉角。视觉导航通过光流场的三维重建得出飞行器运动的航迹速度矢量，然后结合传感器采集的实时高度数据，得出载体位置和姿态信息，并融合惯性导航数据，来实现无人机高精度、高可靠性和低噪声的导航。

技术实现要素：

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于飞行器的视觉导航与惯性导航的组合导航方法，通过视觉导航得出飞行器运动的航迹速度矢量，融合惯性导航数据，对视觉导航的误差进行补偿修正，最终实现对飞行器的运动轨迹的重建，从而使飞行器稳定的自主飞行，便于推广使用。

为解决上述技术问题，本发明采用的技术方案是：基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于，该方法包括以下步骤：

步骤一、建立二维平面直角图片坐标系：采用飞行器控制单元建立二维平面直角图片坐标系，在飞行器所处平行于水平面的平面且以飞行器飞行起点作为坐标原点建立二维平面直角图片坐标系o^k-x^ky^k，其中，以飞行器正东方向为二维平面直角图片坐标系o^k-x^ky^k的x轴，以飞行器正北方向为二维平面直角图片坐标系o^k-x^ky^k的y轴；

所述飞行器为四轴飞行器，所述四轴飞行器包括矩形镂空的机身和四个分别沿机身的四个直角所指方向向外安装的镂空的机翼，机翼远离机身的一端安装有螺旋桨；

步骤二、获取视频图像数据和飞行器位姿数据，并对图像进行预处理：采用飞行器控制单元控制飞行器起飞，通过高清摄像头实时采集飞行器飞行过程中视频图像序列并对所述视频图像序列进行灰度预处理生成预处理图像序列in(u,v)，高清摄像头安装在机身的底部，其中，n为视频图像序列中高清摄像头采集的图像序列的编号且n取不小于2的正整数，u为所述预处理图像序列in(u,v)中的像素点与二维平面直角图片坐标系o^k-x^ky^k的x轴同向的图片横坐标，v为所述预处理图像序列in(u,v)中的像素点与二维平面直角图片坐标系o^k-x^ky^k的y轴同向的图片纵坐标，同时通过惯性传感器实时采集飞行器飞行过程中位姿数据，并将飞行器飞行过程中位姿数据传输至飞行器控制单元；

步骤三、特征点的连续跟踪，过程如下：

步骤301、飞行器控制单元设置每帧预处理图像序列中特征点数量阈值th，并采用harris角点检测法对首帧预处理图像序列i1(u,v)进行特征点提取；

步骤302、判断预处理图像序列ii(u,v)中特征点数量是否小于特征点数量阈值th：当预处理图像序列ii(u,v)中特征点数量小于特征点数量阈值th时，通过飞行器控制单元对预处理图像序列ii(u,v)采用harris角点检测法进行特征点提取后执行步骤303；当预处理图像序列ii(u,v)中特征点数量不小于特征点数量阈值th时，执行步骤303，其中，i为正整数且i＝1,2，…，n-1；

步骤303、飞行器控制单元通过图像金字塔并采用光流法逐层对连续两帧预处理图像序列ii(u,v)和ii+1(u,v)进行特征点匹配，当ii(ux,uy)＝ii+1(px,py)+ε成立，则预处理图像序列ii(u,v)中的特征点u＝(ux,uy)与预处理图像序列ii+1(u,v)中的特征点p＝(px,py)匹配成功，其中，ε为图像灰度误差值，特征点p＝(px,py)满足p＝u+d，d为预处理图像序列ii+1(u,v)中的特征点p至预处理图像序列ii(u,v)中的特征点u的运动位移矢量；

步骤304、多次重复步骤302至步骤303，直至实现连续两帧预处理图像序列ii(u,v)和ii+1(u,v)的全部特征点的匹配；

步骤四、建立三维航迹坐标系：首先，根据公式计算图像中的坐标变换到实际中的坐标的坐标变换系数ζ，其中，σ为图像中特征点的尺寸，h为通过声呐传感器或气压计实时采集所述飞行器相对地面飞行高度，b为高清摄像头的放大倍数，声呐传感器和气压计均安装在机身的底部；然后，对二维平面直角图片坐标系o^k-x^ky^k，增加以飞行器相对地面升高的方向为z轴正方向，建立三维航迹坐标系o^k-x^ky^kz^k；

步骤五、根据公式计算航迹速度矢量vk，其中，f为高清摄像头的图像采集频率，pj和uj分别为连续两帧预处理图像序列对应第j个特征点在三维航迹坐标系o^k-x^ky^kz^k中的坐标，其中，j为非零正整数，hpj为连续两帧预处理图像序列中获取后一帧图像序列时飞行器相对地面飞行高度，huj为连续两帧预处理图像序列中获取前一帧图像序列时飞行器相对地面飞行高度，pxj和pyj分别为所述后一帧图像序列中第j个特征点的图片横坐标和图片纵坐标，uxj和uyj分别为所述前一帧图像序列中第j个特征点的图片横坐标和图片纵坐标；

步骤六、飞行器的姿态解算：首先，采用惯性传感器采集四轴飞行器运行状态参数并通过惯性传感器自身数据融合向处理器传输四轴飞行器的姿态四元数q且姿态四元数q＝q4+q1i+q2j+q3k，q4为姿态四元数q的实部，q1、q2和q3分别为姿态四元数q的三个虚部；然后，根据计算四轴飞行器的翻滚角俯仰角θ和偏航角ψ，得到四轴飞行器姿态的欧拉角矩阵ω(ω(t))，其中，欧拉角矩阵

步骤七、数据融合及航迹角解算：首先，通过处理器融合飞行器的姿态数据和图像数据，根据公式获取相对速度矢量vυ；然后，根据公式获取航迹方位角χ和航迹倾角γ；

步骤八、坐标转换并获取飞行器的位移矩阵，过程如下：

步骤801、根据公式将相对速度矢量vυ从三维航迹坐标系o^k-x^ky^kz^k变换到地面坐标系o^g-x^gy^gz^g，得到相对地面的真实速度矢量vt；

步骤802、根据公式st＝∫(vxt,vyt,vzt)dt，得到飞行器的位移矢量st；

步骤803、将位移矢量st在地面坐标系o^g-x^gy^gz^g中分解为x轴位移sxt、y轴位移syt、z轴位移szt，得到位移矩阵

步骤九、(n-1)次重复步骤三至步骤八，直至完成连续采集多帧图像的位移矩阵集合，实现连续组合导航。

上述的基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于：所述飞行器控制单元包括处理器和蓄电池，以及均与处理器相接的存储器、定时器和用于与地面监测中心数据传输的无线通信模块，处理器的输入端接有避障模块，处理器的输出端接有用于驱动螺旋桨转动的电机，处理器采用电机驱动器驱动电机转动，惯性传感器的信号输出端与处理器的输入端相接，采用处理器对高清摄像头实时采集的图像数据进行灰度预处理；螺旋桨上安装有用于指示所述飞行器位置的照明指示灯，照明指示灯的输入端与处理器的输出端相接。

上述的基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于：所述存储器中存储有飞行器的高度阈值，当声呐传感器采集的飞行器高度小于存储器中存储的飞行器的高度阈值时，采用声呐传感器获取所述飞行器相对地面飞行高度；当声呐传感器采集的飞行器高度大于存储器中存储的飞行器的高度阈值时，采用气压计获取所述飞行器相对地面飞行高度。

上述的基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于：所述j的取值范围满足：200≤j≤600。

上述的基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于：所述预处理图像序列in(u,v)的灰度值为0～255，所述图像灰度误差值ε的取值范围满足：0≤ε≤30。

上述的基于飞行器的视觉导航与惯性导航的组合导航方法，其特征在于：所述高清摄像头的放大倍数b为15～30。

本发明与现有技术相比具有以下优点：

1、本发明建立图片坐标系，获取视频图像数据，采用光流法逐层对连续两帧预处理图像序列进行特征点匹配，实现图片中的特征点匹配现实空间实际物体的连续跟踪导航，同时获取飞行器位姿数据，即引入飞行器的高度数据，获取飞行器航迹速度矢量的三维运动轨迹，其中，根据摄像头的放大倍数以及飞行器的实际飞行高度，将物体位于二维图片中的图片尺寸转换为现实尺寸，将二维平面直角图片坐标系转换为三维航迹坐标系，便于推广使用。

2、本发明航迹速度矢量计算过程中，由于同一帧图像中不同特征点的速度矢量方向和大小均不相同，因而会造成导航数据的不稳定，采用矢量的叠加原理，对同一时刻同一帧图像中的所有特征点的速度矢量进行叠加，从而得到一个合成运动的航迹速度矢量，可靠稳定，使用效果好。

3、本发明方法步骤简单，在视觉导航数据和惯性导航数据融合时，视觉导航数据由于飞行器的翻转震荡会产生不可避免的误差，融合惯性导航数据的目的可抑制并修正其所产生的误差；另外，通过三维航迹坐标系o^k-x^ky^kz^k到地面坐标系o^g-x^gy^gz^g的变换获取飞行器相对地面的飞行轨迹，实现连续导航，便于推广使用。

综上所述，本发明通过视觉导航得出飞行器运动的航迹速度矢量，融合惯性导航数据，对视觉导航的误差进行补偿修正，最终实现对飞行器的运动轨迹的重建，从而使飞行器稳定的自主飞行，便于推广使用。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明采用的组合导航设备的结构示意图。

图2为本发明采用的组合导航设备的电路原理框图。

图3为本发明的方法流程图。

图4为本发明数据融合前的航迹方位角频率的分布直方图。

图5为本发明数据融合后的航迹方位角频率的分布直方图。

图6为本发明飞行器位于地面坐标系中的位移矩阵实测导航图。

附图标记说明:

1—机身；2—机翼；3—螺旋桨；

4—飞行器控制单元；4-1—处理器；4-2—蓄电池；

4-3—电机驱动器；4-4—电机；4-5—定时器；

4-6—存储器；4-7—无线通信模块；5—高清摄像头；

6—声呐传感器；7—气压计；8—惯性传感器；

9—避障模块；10—照明指示灯；11—地面监测中心；

12—显示器。

具体实施方式

如图1至图3所示，本发明的基于飞行器的视觉导航与惯性导航的组合导航方法，包括以下步骤：

步骤一、建立二维平面直角图片坐标系：采用飞行器控制单元4建立二维平面直角图片坐标系，在飞行器所处平行于水平面的平面且以飞行器飞行起点作为坐标原点建立二维平面直角图片坐标系o^k-x^ky^k，其中，以飞行器正东方向为二维平面直角图片坐标系o^k-x^ky^k的x轴，以飞行器正北方向为二维平面直角图片坐标系o^k-x^ky^k的y轴；

所述飞行器为四轴飞行器，所述四轴飞行器包括矩形镂空的机身1和四个分别沿机身1的四个直角所指方向向外安装的镂空的机翼2，机翼2远离机身1的一端安装有螺旋桨3；

需要说明的是，所述飞行器采用四轴飞行器是为了方便控制飞行器飞行平衡，易于控制，四轴飞行器的机身1采用矩形镂空结构，四轴飞行器的机翼2也采用镂空结构，是为了实现四轴飞行器整体自重的减小，减少螺旋桨起飞的阻力，同时减少电机4-4功耗，进而减少蓄电池4-2电能的消耗，延长飞行器飞行续航时间，四个机翼2分别沿机身1的四个直角所指方向向外安装，是为了更好的使用矩形机身1有效面积，增大机翼2与机身1连接的面积，增加飞行器牢固性，优选的机身1采用正方形结构，实现飞行器整理结构对称，制作简单。

步骤二、获取视频图像数据和飞行器位姿数据，并对图像进行预处理：采用飞行器控制单元4控制飞行器起飞，通过高清摄像头5实时采集飞行器飞行过程中视频图像序列并对所述视频图像序列进行灰度预处理生成预处理图像序列in(u,v)，高清摄像头5安装在机身1的底部，其中，n为视频图像序列中高清摄像头5采集的图像序列的编号且n取不小于2的正整数，u为所述预处理图像序列in(u,v)中的像素点与二维平面直角图片坐标系o^k-x^ky^k的x轴同向的图片横坐标，v为所述预处理图像序列in(u,v)中的像素点与二维平面直角图片坐标系o^k-x^ky^k的y轴同向的图片纵坐标，同时通过惯性传感器8实时采集飞行器飞行过程中位姿数据，并将飞行器飞行过程中位姿数据传输至飞行器控制单元4；

需要说明的是，惯性传感器8的设置是为了采集飞行器的飞行姿态，与高清摄像头5采集的数据图像信息结合实现视觉导航与惯性导航的组合导航。

步骤三、特征点的连续跟踪，过程如下：

步骤301、飞行器控制单元4设置每帧预处理图像序列中特征点数量阈值th，并采用harris角点检测法对首帧预处理图像序列i1(u,v)进行特征点提取；

步骤302、判断预处理图像序列ii(u,v)中特征点数量是否小于特征点数量阈值th：当预处理图像序列ii(u,v)中特征点数量小于特征点数量阈值th时，通过飞行器控制单元4对预处理图像序列ii(u,v)采用harris角点检测法进行特征点提取后执行步骤303；当预处理图像序列ii(u,v)中特征点数量不小于特征点数量阈值th时，执行步骤303，其中，i为正整数且i＝1,2，…，n-1；

需要说明的是，由于飞行在飞行过程中移动变化，高清摄像头5采集的相邻两帧图像序列的特征点在不断的更新变化，飞行器控制单元4设置每帧预处理图像序列中特征点数量阈值th的目的是保证相邻两帧图像序列匹配时，有足够的特征点匹配使用。

步骤303、飞行器控制单元4通过图像金字塔并采用光流法逐层对连续两帧预处理图像序列ii(u,v)和ii+1(u,v)进行特征点匹配，当ii(ux,uy)＝ii+1(px,py)+ε成立，则预处理图像序列ii(u,v)中的特征点u＝(ux,uy)与预处理图像序列ii+1(u,v)中的特征点p＝(px,py)匹配成功，其中，ε为图像灰度误差值，特征点p＝(px,py)满足p＝u+d，d为预处理图像序列ii+1(u,v)中的特征点p至预处理图像序列ii(u,v)中的特征点u的运动位移矢量；

步骤304、多次重复步骤302至步骤303，直至实现连续两帧预处理图像序列ii(u,v)和ii+1(u,v)的全部特征点的匹配；

本实施例中，所述预处理图像序列in(u,v)的灰度值为0～255，所述图像灰度误差值ε的取值范围满足：0≤ε≤30。

需要说明的是，采用光流法逐层对连续两帧预处理图像序列进行特征点匹配，由于图像的采集不可避免的存在干扰，预处理图像序列ii(u,v)中的特征点u＝(ux,uy)与预处理图像序列ii+1(u,v)中的特征点p＝(px,py)对应的灰度值接近则，说明一个特征点匹配成功，所述图像灰度误差值ε的设置是为了衡量预处理图像序列ii(u,v)中的特征点u＝(ux,uy)与预处理图像序列ii+1(u,v)中的特征点p＝(px,py)的近似度，对连续两帧预处理图像序列中的特征点进行一一的匹配，实现图片中的特征点匹配现实空间实际物体的连续跟踪导航。

步骤四、建立三维航迹坐标系：首先，根据公式计算图像中的坐标变换到实际中的坐标的坐标变换系数ζ，其中，σ为图像中特征点的尺寸，h为通过声呐传感器6或气压计7实时采集所述飞行器相对地面飞行高度，b为高清摄像头5的放大倍数，声呐传感器6和气压计7均安装在机身1的底部；然后，对二维平面直角图片坐标系o^k-x^ky^k，增加以飞行器相对地面升高的方向为z轴正方向，建立三维航迹坐标系o^k-x^ky^kz^k；

本实施例中，所述高清摄像头5的放大倍数b为15～30。

本实施例中，所述存储器4-6中存储有飞行器的高度阈值，当声呐传感器6采集的飞行器高度小于存储器4-6中存储的飞行器的高度阈值时，采用声呐传感器6获取所述飞行器相对地面飞行高度；当声呐传感器6采集的飞行器高度大于存储器4-6中存储的飞行器的高度阈值时，采用气压计7获取所述飞行器相对地面飞行高度。

需要说明的是，声呐传感器6和气压计7均安装在机身1的底部，采用声呐传感器6和气压计7采集所述飞行器相对地面飞行高度，其中，声呐传感器6采集近地高度精度高，气压计7采集远地高度精度高，由于飞行器近地飞行时，螺旋桨产生的气流与地面作用产生翼地效应导致气压计7采集的飞行器高度数据偏差较大，声呐传感器6是为了配合气压计7采集飞行器近地飞行的高度。

步骤五、根据公式计算航迹速度矢量vk，其中，f为高清摄像头5的图像采集频率，pj和uj分别为连续两帧预处理图像序列对应第j个特征点在三维航迹坐标系o^k-x^ky^kz^k中的坐标，其中，j为非零正整数，hpj为连续两帧预处理图像序列中获取后一帧图像序列时飞行器相对地面飞行高度，huj为连续两帧预处理图像序列中获取前一帧图像序列时飞行器相对地面飞行高度，pxj和pyj分别为所述后一帧图像序列中第j个特征点的图片横坐标和图片纵坐标，uxj和uyj分别为所述前一帧图像序列中第j个特征点的图片横坐标和图片纵坐标；

本实施例中，所述j的取值范围满足：200≤j≤600。

需要说明的是，航迹速度矢量计算过程中，由于同一帧图像中不同特征点的速度矢量方向和大小均不相同，因而会造成导航数据的不稳定，采用矢量的叠加原理，对同一时刻同一帧图像中的所有特征点的速度矢量进行叠加，从而得到一个合成运动的航迹速度矢量

步骤六、飞行器的姿态解算：首先，采用惯性传感器8采集四轴飞行器运行状态参数并通过惯性传感器8自身数据融合向处理器4-1传输四轴飞行器的姿态四元数q且姿态四元数q＝q4+q1i+q2j+q3k，q4为姿态四元数q的实部，q1、q2和q3分别为姿态四元数q的三个虚部；然后，根据计算四轴飞行器的翻滚角俯仰角θ和偏航角ψ，得到四轴飞行器姿态的欧拉角矩阵ω(ω(t))，其中，欧拉角矩阵

步骤七、数据融合及航迹角解算：首先，通过处理器4-1融合飞行器的姿态数据和图像数据，根据公式获取相对速度矢量vυ；然后，根据公式获取航迹方位角χ和航迹倾角γ；

需要说明的是，在视觉导航数据和惯性导航数据融合时，视觉导航数据由于飞行器的翻转震荡会产生不可避免的误差，融合惯性导航数据的目的可抑制并修正其所产生的误差；如图4所示，从数据融合前的航迹方位角分布可以看出由于飞行器的抖动造成航迹方位角出现剧烈变化，导致数据夹杂着噪声，难以用于导航；如图5所示，数据融合的目的是通过矢量合成后，航迹方位角则集中趋于真实的方向，航迹方位角分布集中，这表明该算法很好的抑制了飞行器抖动造成的误差。

步骤八、坐标转换并获取飞行器的位移矩阵，过程如下：

步骤801、根据公式将相对速度矢量vυ从三维航迹坐标系o^k-x^ky^kz^k变换到地面坐标系o^g-x^gy^gz^g，得到相对地面的真实速度矢量vt；

步骤802、根据公式st＝∫(vxt,vyt,vzt)dt，得到飞行器的位移矢量st；

步骤803、将位移矢量st在地面坐标系o^g-x^gy^gz^g中分解为x轴位移sxt、y轴位移syt、z轴位移szt，得到位移矩阵

需要说明的是，三维航迹坐标系o^k-x^ky^kz^k只能表现出相邻两帧图像的相对速度，无法直观的显示真是的航迹轨迹，通过将相对速度矢量vυ从三维航迹坐标系o^k-x^ky^kz^k变换到地面坐标系o^g-x^gy^gz^g，得到相对地面的真实速度矢量vt，对速度积分获取飞行器的航行空间位置。

步骤九、(n-1)次重复步骤三至步骤八，直至完成连续采集多帧图像的位移矩阵集合，实现连续组合导航。

本实施例中，所述飞行器控制单元4包括处理器4-1和蓄电池4-2，以及均与处理器4-1相接的存储器4-6、定时器4-5和用于与地面监测中心11数据传输的无线通信模块4-7，处理器4-1的输入端接有避障模块9，处理器4-1的输出端接有用于驱动螺旋桨3转动的电机4-4，处理器4-1采用电机驱动器4-3驱动电机4-4转动，惯性传感器8的信号输出端与处理器4-1的输入端相接，采用处理器4-1对高清摄像头5实时采集的图像数据进行灰度预处理；螺旋桨3上安装有用于指示所述飞行器位置的照明指示灯10，照明指示灯10的输入端与处理器4-1的输出端相接。

地面监测中心11输出端接有用于显示所述飞行器飞行路径的显示器12，地面监测中心11优选的采用计算机；所述惯性传感器8优选的采用惯性传感器mpu6050，惯性传感器mpu6050的设置整合了6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量的封装空间。

需要说明的是，避障模块9的设置是为了避免飞行器在高空飞行时，遇到障碍物而损坏。电机驱动器4-3的设置是为了精确的控制电机4-4转动，进而带动螺旋桨3转动产生向上的上升力。照明指示灯10的设置一是为了飞行器在夜晚飞行时为高清摄像机5提供有效光照，便于图像信息的清晰采集；二是为了飞行器在夜晚飞行时便于操作者观察飞行器的位置，照明指示灯10的移动轨迹确定了飞行器的移动轨迹。

如图6所示，本发明使用时，针对地表环境复杂的情况，对视觉导航有一定的干扰，采用惯性导航数据实现组合式导航，数据融合及航迹角解算后飞行器的轨迹，很好的抑制了噪声，数据在很小范围内震荡，并趋于一个稳定的值，使用效果好，克服了传统gps导航对环境的依赖的问题，很好的实现了四轴飞行器在复杂地形下的自主飞行，具有很高的实效性、准确性和广泛性。

以上所述，仅是本发明的较佳实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。