CN104729506B - 一种视觉信息辅助的无人机自主导航定位方法 - Google Patents

Abstract

本发明公开了一种视觉信息辅助的无人机自主导航定位方法，该定位方法目的是利用视觉传感器以及相应视觉测程算法，提供准确的无人机导航定位信息。该方法通过组合式视觉测程算法来推算无人机的速度和位置信息，用于辅助惯性导航定位。在GNSS信号可用时，利用惯性导航/GNSS组合导航来修正惯性导航系统误差，并建立视觉测程评估准则；当GNSS信号不可用时，利用视觉测程评估准则对组合式视觉测程算法获得的视觉测程结果进行评估，利用视觉测程结果和视觉测程评估结果估计并补偿惯导系统的误差，为无人机提供连续、可靠的导航定位信息。本发明能够长时间连续、稳定地为无人机控制系统提供精确的导航定位信息。

Description

一种视觉信息辅助的无人机自主导航定位方法

技术领域

本发明涉及一种视觉信息辅助的无人机自主导航定位方法，特别是一种基于视觉测程结果辅助惯性导航系统的无人机自主导航定位方法，适用于无人机自主导航和定位系统。

背景技术

无人机是一类低风险低成本，可以自主运作完成各项任务的自动化飞行载具，被广泛用于自动运输、区域监视、灾后重建、复杂环境探测以及地面目标跟踪等军民用领域中。为了在无人监测的情况下自主完成任务，一个稳定、快速、精确的导航系统对于无人机而言至关重要。由于低成本惯性导航系统误差发散很快，依靠单一的低成本惯性导航系统很无法满足实际应用要求。将惯性导航系统与GNSS信息进行融合来获得无人机精确导航定位信息的方法已被广泛应用。但由于当GNSS信号受到外界干扰或欺骗，甚至信号转发源被摧毁时，GNSS系统无法长期提供稳定可靠的定位信息。因此，在军民用领域中，尤其是室内、丛林、多雨地区等应用环境下，尚不能完全依赖于GNSS来辅助惯性导航实现无人机的精确导航定位。为了解决这个问题，引入适当的导航传感器和新的导航算法来解决GNSS短期和长期故障时无人机自主导航定位问题，已成为军民用领域无人机应用研究的重点。

视觉传感器作为一种兼具导航传感器和任务传感器双重特性的设备，因其体积小，安装简单，信息丰富等特点，在无人机上已经得到了广泛应用。而且基于视觉传感器的导航算法无需增添额外设备即可提供较准确的导航信息，是目前研究与应用的重点。

本发明中提到一种视觉信息辅助的无人机自主导航定位方法，该方法根据视觉测程原理，通过组合式视觉测程算法来推算无人机的速度和位置信息，用于辅助惯性导航定位。在GNSS信号可用时，利用惯性导航/GNSS组合导航来修正惯性导航系统误差，并建立视觉测程评估准则；当GNSS信号不可用时，利用视觉测程评估准则对组合式视觉测程算法获得的视觉测程结果进行评估，利用视觉测程结果和视觉测程评估结果估计并补偿惯导系统的误差，为无人机提供连续、可靠的导航定位信息。

本发明与其它无人机自主导航和定位方法不同在于：在当前已公开发表的文献中，依靠视觉导航结果辅助惯性导航系统提供无人机的导航定位信息，但这些导航方法在实际应用中无法根据外部环境和导航结果有针对性地采取不同的导航策略，且无法评估视觉导航信息的有效性，使视觉导航信息易受误差影响，无法提供长时间稳定的导航信息而导致无法完成预定任务，甚至导致无人机和机载设备有损毁的风险。

发明内容

本发明要解决的技术问题：为解决在GNSS信号短时间和长时间不可用时，导航系统能够长时间提供稳定的姿态和定位信息来控制无人机并稳定、可靠地完成既定任务，提出一种视觉信息辅助的无人机自主导航定位方法。

本发明采用的技术方案为：一种视觉信息辅助的无人机自主导航定位方法，其特征在于通过组合式视觉测程算法提供较准确的视觉测程结果；在GNSS信号可用时，利用惯性导航/GNSS组合导航来修正惯性导航系统误差，并建立视觉测程评估准则；当GNSS信号不可用时，利用视觉测程评估准则对组合式视觉测程算法获得的视觉测程结果进行评估，利用视觉测程结果和视觉测程评估结果估计并补偿惯导系统的误差，为无人机提供连续、可靠的导航定位信息。具体实现步骤为：

(1)将采集到的图像分为普通帧与关键帧，利用光流估计方法获得普通帧的帧间匹配，利用特征匹配方法获得关键帧的帧间匹配，进而根据最小误差准则，选取基于单应性矩阵的视觉测程方式和基于基础矩阵的视觉测程方式中的最优者来分别计算普通帧与关键帧的帧间运动参数，并将两种帧间运动参数进行组合来获得视觉测程结果，其具体方法为：

a1、以视觉传感器输出的图像序列作为普通帧序列，利用金字塔Lucas-Kanade光流估计方法估计普通帧间的光流场，实现普通帧的帧间匹配；同时，取第一帧为关键帧，且每隔八个普通帧取出一帧作为关键帧，利用基于SURF的特征匹配算法实现关键帧的帧间匹配；利用随机采样一致性RANSAC算法分别剔除普通帧的帧间匹配和关键帧的帧间匹配中的误匹配点，当关键帧的帧间匹配数小于设定的阈值，给出错误标识1；

a2、从普通帧的帧间匹配点和关键帧的帧间匹配点中分别随机选取匹配点总数的50％来建立图像帧间单应性矩阵和基础矩阵约束方程分别为：

x'＝Hx (1)

x'^TFx＝0 (2)

通过求解式(1)和(2)获得图像帧间单应性矩阵与基础矩阵；重复上述计算过程三次，分别获得独立的三组单应性矩阵和基础矩阵，如果单应性矩阵的误差小于基础矩阵的误差，则无人机外部环境为平坦空旷的二维环境，通过对单应性矩阵进行分解来获得无人机相对运动参数；如果单应性矩阵的误差大于基础矩阵的误差，则无人机外部环境为复杂的三维环境，通过对基础矩阵进行分解来获得无人机相对运动参数；在针对关键帧的帧间匹配展开计算时，如果得到的两种矩阵误差均大于设定的阈值，给出错误标识2；

a3、如果新采集的图像未被选为关键帧，按照a2中计算过程来计算普通帧的帧间运动参数，并根据前一个普通帧计算出的无人机位置和速度信息来计算当前帧无人机的位置和速度信息作为视觉测程结果；

a4、如果新采集的图像被选取为关键帧，按照a2中计算过程来计算关键帧的帧间运动参数，并对关键帧的帧间运动参数计算过程进行评估检测；如果未检测到错误标识，则当前关键帧为有效关键帧，利用当前关键帧的帧间运动参数和前一关键帧计算出的无人机位置和速度计算当前帧无人机的位置和速度信息作为视觉测程结果；如果出现在a1中所述的错误标识1或在a2中所述的错误标识2，则当前关键帧为无效关键帧，并将当前关键帧按普通帧进行处理，按a3中的计算过程来计算视觉测程结果。

(2)当GNSS信号可用时，利用一个七状态Kalman滤波器实现惯导系统/GNSS组合，估计并补偿惯导系统的误差，获得精确的无人机的位置、速度和姿态全状态导航定位信息，并以全状态导航定位信息为基准值，计算视觉测程结果的误差，建立视觉测程评估准则，其具体方法为：

b1、根据惯性导航系统误差方程建立惯性导航系统状态方程为：

式中，X(t)^T＝[φ_E,φ_N,φ_U,δV_E,δV_N,δL,δλ]为连续时间系统状态；上标T为转置；E、N和U分别为东北天地理坐标系的坐标轴指向；t为时间；φ_E、φ_N和φ_U分别为惯性导航系统在东、北和天向的平台误差角；δV_E和δV_N分别为惯性导航系统东向和北向的速度误差；δL和δλ分别为惯性导航系统北向和东向位置误差；W(t)为系统噪声；F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵；R_N和R_M分别为卯酉圈和子午面半径；

b2、利用GNSS输出的位置和速度与惯性导航系统输出的位置和速度的差值为观测量建立系统的量测方程为：

式中，Z_S为GNSS与惯性导航系统输出值相减获得的量测值；和分别为惯性导航系统输出的东向和北向的速度；L_I和λ_I分别为惯性导航系统输出北向位置和东向位置；和分别为GNSS输出的东向和北向的速度；L_S和λ_S分别为GNSS输出的北向位置和东向位置；上标或下标中的I和S分别为惯性导航系统和GNSS的输出值；V_S＝[v_E v_N v_L v_λ]^T为GNSS接收机的噪声；

b3、对式(3)进行离散化，利用Kalman滤波实时估计并补偿惯性导航系统的误差，并获得无人机精确的位置、速度和姿态信息；

b4、以惯导系统/GNSS组合导航系统获得精确的位置信息为基准值，计算视觉测程结果的误差为：

式中e_e、e_n和e_u分别为视觉测程结果的东向、北向和天向误差，计算视觉测程结果的误差e的均值e_a和最大值e_m，并设置测量值的可信度为{i}_i＝1…n，且n满足如下关系

(n-1)e_a<e_m<ne_a (7)

式中，e、e_a、e_m、n和{i}_i＝1…n的值在每次计算新的视觉测程结果后自动更新。

(3)当GNSS信号不可用时，利用运动模型预测无人机的运动信息，计算无人机运动的预测值与视觉测程结果的误差，根据这一误差和视觉测程评估准则得出视觉测程评估结果，其具体方法为：

c1、根据在两次图像采样之间无人机加速度保持不变，建立无人机运动预测方程为：

a(k+1)＝a(k)

v(k+1)＝v(k)+a(k)T (8)

s(k+1)＝s(k)+v(k)T

式中，a(k)、v(k)和s(k)分别为无人机在第k次图像采样时的加速度、速度和位置；a(k+1)、v(k+1)和s(k+1)分别为无人机在第k+1次图像采样时的加速度、速度和位置，T为采样周期；计算视觉测程和模型预测获得无人机位置的误差e_c，当误差e_c满足

(i-1)e_a<e_c<ie_a (9)

则该次量测值的可信度为i，将i作为视觉测程评估结果输出。

(4)根据视觉测程评估结果，利用七状态Kalman滤波融合视觉测程结果和惯性导航系统信息，估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息，其具体方法为：

d1、利用视觉测程结果中的水平速度和位置与惯性导航系统输出的水平速度和位置的差值为量测值，建立系统的量测方程为：

式中，Z_V为视觉测程结果与惯性导航系统输出信息相减获得的量测值；和分别为视觉测程结果中的东向和北向的速度；L_V和λ_V分别为视觉测程结果中的北向位置和东向位置；上标或下标中的I和V分别为惯性导航系统的输出值和视觉测程结果；V_V＝[v_E v_Nv_L v_λ]^T为视觉测程结果的噪声；i为c1中确定的视觉测程评估结果；

d2、对式(3)进行离散化，利用七状态Kalman滤波方法实时估计并补偿惯性导航系统的误差，获得视觉信息辅助惯性导航的无人机精确的位置、速度和姿态信息。

本发明和现有技术相比的优点在于：

本发明利用不同的帧间匹配方法和不同的帧间关系约束，构建组合式视觉测程算法，并根据运算过程和结果智能提供准确的视觉测程信息；当GNSS信号可用时，建立惯导/GNSS组合导航系统，并利用其导航信息建立视觉测程评估准则；根据视觉测程信息及其评估结果，利用七状态Kalman滤波方法估计并补偿惯性导航系统的误差，从而实现了在GNSS信号不可用时，利用视觉信息辅助惯性导航实时获得无人机精确的位置、速度和姿态信息，保证无人机长时间稳定可靠地飞行和操控。

附图说明

图1为本发明一种视觉信息辅助的无人机自主导航定位方法的原理图；

图2为本发明中组合式视觉测程算法原理图；

图3为本发明在GNSS信号可用时视觉测程评估准则确定方法示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进一步说明。

本发明一种视觉信息辅助的无人机自主导航定位方法的具体实现步骤如下：

(1)对加速度计、陀螺仪、磁传感器、气压高度计和视觉传感器进行标定和误差补偿；

(2)如图1所示，根据惯性导航系统力学方程编排，利用加速度计和陀螺仪的输出值实时计算无人机的运动信息，包括位置、速度和姿态；

(3)如图2所示，将采集到的图像分为普通帧与关键帧，利用光流估计方法获得普通帧的帧间匹配，利用特征匹配方法获得关键帧的帧间匹配，进而根据最小误差准则，选取基于单应性矩阵的视觉测程方式和基于基础矩阵的视觉测程方式中的最优者来分别计算普通帧与关键帧的帧间运动参数，并将两种帧间运动参数进行组合来获得视觉测程结果，其具体方法为：

a1、以视觉传感器输出的图像序列作为普通帧序列，利用金字塔Lucas-Kanade光流估计方法估计普通帧间的光流场，实现普通帧的帧间匹配；同时，取第一帧为关键帧，且每隔八个普通帧取出一帧作为关键帧，利用基于SURF的特征匹配算法实现关键帧的帧间匹配；利用随机采样一致性RANSAC算法分别剔除普通帧的帧间匹配和关键帧的帧间匹配中的误匹配点，当关键帧的帧间匹配数小于设定的阈值，给出错误标识1；

a2、从普通帧的帧间匹配点和关键帧的帧间匹配点中分别随机选取匹配点总数的50％来建立图像帧间单应性矩阵和基础矩阵约束方程分别为：

x'＝Hx (1)

x'^TFx＝0 (2)

通过求解式(1)和(2)获得图像帧间单应性矩阵与基础矩阵；重复上述计算过程三次，分别获得独立的三组单应性矩阵和基础矩阵，如果单应性矩阵的误差小于基础矩阵的误差，则无人机外部环境为平坦空旷的二维环境，通过对单应性矩阵进行分解来获得无人机相对运动参数；如果单应性矩阵的误差大于基础矩阵的误差，则无人机外部环境为复杂的三维环境，通过对基础矩阵进行分解来获得无人机相对运动参数；在针对关键帧的帧间匹配展开计算时，如果得到的两种矩阵误差均大于设定的阈值，给出错误标识2；

a3、如果新采集的图像未被选为关键帧，按照a2中计算过程来计算普通帧的帧间运动参数，并根据前一个普通帧计算出的无人机位置和速度信息来计算当前帧无人机的位置和速度信息作为视觉测程结果；

a4、如果新采集的图像被选取为关键帧，按照a2中计算过程来计算关键帧的帧间运动参数，并对关键帧的帧间运动参数计算过程进行评估检测；如果未检测到错误标识，则当前关键帧为有效关键帧，利用当前关键帧的帧间运动参数和前一关键帧计算出的无人机位置和速度计算当前帧无人机的位置和速度信息作为视觉测程结果；如果出现在a1中所述的错误标识1或在a2中所述的错误标识2，则当前关键帧为无效关键帧，并将当前关键帧按普通帧进行处理，按a3中的计算过程来计算视觉测程结果。

(4)如图3所示，当GNSS信号可用时，利用一个七状态Kalman滤波器实现惯导系统/GNSS组合，估计并补偿惯导系统的误差，获得精确的无人机的位置、速度和姿态全状态导航定位信息，并以全状态导航定位信息为基准值，计算视觉测程结果的误差，建立视觉测程评估准则，其具体方法为：

b1、根据惯性导航系统误差方程建立惯性导航系统状态方程为：

式中，X(t)^T＝[φ_E,φ_N,φ_U,δV_E,δV_N,δL,δλ]为连续时间系统状态；上标T为转置；E、N和U分别为东北天地理坐标系的坐标轴指向；t为时间；φ_E、φ_N和φ_U分别为惯性导航系统在东、北和天向的平台误差角；δV_E和δV_N分别为惯性导航系统东向和北向的速度误差；δL和δλ分别为惯性导航系统北向和东向位置误差；W(t)为系统噪声；F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵；R_N和R_M分别为卯酉圈和子午面半径；

b2、利用GNSS输出的位置和速度与惯性导航系统输出的位置和速度的差值为观测量建立系统的量测方程为：

式中，Z_S为GNSS与惯性导航系统输出值相减获得的量测值；和分别为惯性导航系统输出的东向和北向的速度；L_I和λ_I分别为惯性导航系统输出北向位置和东向位置；和分别为GNSS输出的东向和北向的速度；L_S和λ_S分别为GNSS输出的北向位置和东向位置；上标或下标中的I和S分别为惯性导航系统和GNSS的输出值；V_S＝[v_E v_N v_L v_λ]^T为GNSS接收机的噪声；

b3、对式(3)进行离散化，利用Kalman滤波实时估计并补偿惯性导航系统的误差，并获得无人机精确的位置、速度和姿态信息；

b4、以惯导系统/GNSS组合导航系统获得精确的位置信息为基准值，计算视觉测程结果的误差为：

式中e_e、e_n和e_u分别为视觉测程结果的东向、北向和天向误差，计算视觉测程结果的误差e的均值e_a和最大值e_m，并设置测量值的可信度为{i}_i＝1…n，且n满足如下关系

(n-1)e_a<e_m<ne_a (7)

式中，e、e_a、e_m、n和{i}_i＝1…n的值在每次计算新的视觉测程结果后自动更新。

(5)当GNSS信号不可用时，利用运动模型预测无人机的运动信息，计算无人机运动的预测值与视觉测程结果的误差，根据这一误差和视觉测程评估准则得出视觉测程评估结果，其具体方法为：

c1、根据在两次图像采样之间无人机加速度保持不变，建立无人机运动预测方程为：

a(k+1)＝a(k)

v(k+1)＝v(k)+a(k)T (8)

s(k+1)＝s(k)+v(k)T

式中，a(k)、v(k)和s(k)分别为无人机在第k次图像采样时的加速度、速度和位置；a(k+1)、v(k+1)和s(k+1)分别为无人机在第k+1次图像采样时的加速度、速度和位置，T为采样周期；计算视觉测程和模型预测获得无人机位置的误差e_c，当误差e_c满足

(i-1)e_a<e_c<ie_a (9)

则该次量测值的可信度为i，将i作为视觉测程评估结果输出。

(6)根据视觉测程评估结果，利用七状态Kalman滤波融合视觉测程结果和惯性导航系统信息，估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息，其具体方法为：

d1、利用视觉测程结果中的水平速度和位置与惯性导航系统输出的水平速度和位置的差值为量测值，建立系统的量测方程为：

式中，Z_V为视觉测程结果与惯性导航系统输出信息相减获得的量测值；和分别为视觉测程结果中的东向和北向的速度；L_V和λ_V分别为视觉测程结果中的北向位置和东向位置；上标或下标中的I和V分别为惯性导航系统的输出值和视觉测程结果；V_V＝[v_E v_Nv_L v_λ]^T为视觉测程结果的噪声；i为c1中确定的视觉测程评估结果；

d2、对式(3)进行离散化，利用七状态Kalman滤波方法实时估计并补偿惯性导航系统的误差，获得视觉信息辅助惯性导航的无人机精确的位置、速度和姿态信息。

惯性导航系统力学方程编排、惯性导航系统误差方程以及内插和外推算法以及视觉测程中基于SURF的特征匹配算法、金字塔Lucas-Kanade光流估计方法、单应性矩阵估计与分解方法、基础矩阵估计与分解方法和RANSAC方法都是相关领域公知和常用的方法，不再赘述。

本发明中涉及到的本领域公知技术未详细阐述。

Claims (1)

1.一种视觉信息辅助的无人机自主导航定位方法，其特征在于：利用组合式视觉测程算法获取无人机的视觉测程结果，当GNSS信号可用时，利用惯性导航/GNSS组合导航来修正惯性导航系统误差，建立视觉测程评估准则；当GNSS信号不可用时，利用视觉测程评估准则对视觉测程结果进行评估，将经评估后的视觉测程信息与惯性导航系统信息进行融合，估计并补偿惯导系统的误差，为无人机提供连续、可靠的导航定位信息，其具体实现过程：

步骤(1)、将采集到的图像分为普通帧与关键帧，利用光流估计方法获得普通帧的帧间匹配，利用特征匹配方法获得关键帧的帧间匹配，进而根据最小误差准则，选取基于单应性矩阵的视觉测程方式和基于基础矩阵的视觉测程方式中的最优者来分别计算普通帧与关键帧的帧间运动参数，并将两种帧间运动参数进行组合来获得视觉测程结果；

步骤(2)、当GNSS信号可用时，利用一个七状态Kalman滤波器实现惯导系统/GNSS组合，估计并补偿惯导系统的误差，获得精确的无人机的位置、速度和姿态全状态导航定位信息，并以全状态导航定位信息为基准值，计算视觉测程结果的误差，建立视觉测程评估准则；

步骤(3)、当GNSS信号不可用时，利用运动模型预测无人机的运动信息，计算无人机运动的预测值与视觉测程结果的误差，根据这一误差和视觉测程评估准则得出视觉测程评估结果；

步骤(4)、根据视觉测程评估结果，利用七状态Kalman滤波融合视觉测程结果和惯性导航系统信息，估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息。