CN106708066B - 基于视觉/惯导的无人机自主着陆方法 - Google Patents

Abstract

本发明提供了一种基于视觉/惯导的无人机自主着陆方法，首先利用视觉导航算法求解无人机位置及位姿；然后利用视觉导航求解的无人机位姿作为惯导的初始值，开始求解惯性导航参数；利用惯导相邻时间获取的参数剔除实时图像与参考图像SURF匹配的误匹配点对；最后利用无迹卡尔曼滤波组合导航参数，实时调整无人机位姿进行着陆引导。本发明可以提高视觉导航算法的实时性，使视觉系统保持长时间的高精度；解决了单一惯导的误差发散不可以单独使用的问题，在视觉导航解算失败的情况下也能提供载体的导航参数。

Description

基于视觉/惯导的无人机自主着陆方法

技术领域

本发明涉及一种无人机自主着陆方法，属于组合导航领域。

背景技术

视觉导航技术是利用数字图像处理技术，对机载图像传感器所获取的航拍图像进行处理和分析，最终得到无人机导航控制所需要的位姿参数的技术。图像处理的大信息量等特点使得无人机在视觉导航定位时实时性不能得到满足，因此需要一种快速实时图像匹配算法来解决这一问题。

惯导技术是一种传统的不接收外来的无线电信号，也不向外部辐射能量的自主式导航技术。惯性导航设备具有全天候、全时空工作能力和很好的隐蔽性，能够提供短期高精度、高稳定性的导航参数。但是，惯性导航设备是一个时间积分系统，其误差随着时间迅速累积且垂直定位功能不好，不可以单独使用，因此需要通过引入另一种导航源对惯导长时间累积的误差进行修正。

发明内容

为了克服现有技术的不足，本发明提供一种基于视觉/惯导的无人机自主着陆方法，通过视觉和惯性导航组合的方法来设计无人机自主着陆系统，利用惯性导航来快速剔除视觉导航图像匹配的误匹配点提高视觉导航的实时性，通过无迹卡尔曼滤波 (UKF)的方式将视觉导航输出参数与惯导进行组合，用视觉来修正惯性导航误差，解决惯导随时间误差累积较大的问题。

本发明解决其技术问题所采用的技术方案包括以下步骤：

1)利用视觉导航算法求解无人机位置及位姿，步骤如下：

1.1)利用无人机机载摄像头获取实时图像；

1.2)判断获取的实时图像是否为第一帧图像，若是，转到步骤1.3)，否则转到步骤1.4)；

1.3)提取实时图像的SURF特征点，与参考图像的SURF特征点进行SURF匹配，利用RANSAC算法剔除SURF误匹配点对，获取正确的SURF匹配点对，转到步骤 1.7)；

1.4)判断实时图像帧数是否为设定的固定周期T的倍数，若是，转到步骤1.5)，否则转到步骤1.6)；

1.5)提取实时图像的SURF特征点，与参考图像的SURF特征点进行SURF匹配，转到步骤3)；

1.6)利用LK光流法跟踪上一帧实时图像提取的SURF特征点；

1.7)利用获取的SURF匹配点对进行无人机位姿解算；

2)利用无人机位姿作为惯导的初始值，开始求解惯性导航参数，步骤如下：

2.1)利用陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转换成姿态矩阵k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

2.2)根据姿态矩阵中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

2.3)利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，得到导航坐标系下的比例观测值

2.4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度其中，Ω^L _ie为地球自转角速度三维分量构成的反对称阵，Ω^L _el为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

2.5)利用加速度进行积分得到速度增量，得到当前时刻的速度v^L _k+1＝v^L _k+a.Δt；

2.6)利用速度进行积分得到位置增量，进而得到当前时刻的位置

3)利用惯导相邻时间获取的参数剔除实时图像与参考图像SURF匹配的误匹配点对，步骤如下：

3.1)获取时刻t与时刻t+Δt的惯导参数；

3.2)求取时刻t与时刻t+Δt之间的惯导参数变换；

3.3)获取t时刻特征点在摄像头成像平面上的投影点坐标为W_t＝[u,v]^T，其中，u、v表示投影点在图像坐标系中的坐标值；

3.4)根据特征点匹配方法得到t+Δt时刻特征点在摄像头成像平面上的投影点坐标为W_t+Δt；

3.5)获取视觉导航t时刻到t+Δt时刻投影点坐标变化D＝w_t+Δt-w_t＝[x_i,y_i]^T；

3.6)预测t+Δt时刻投影点坐标 为时刻t到t+Δt之间飞行器航向变化，T为时刻t到t+Δt之间投影点平移向量；

3.7)得到特征点坐标变化向量

3.8)定义匹配误差

3.9)误匹配点P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}，其中d_x，d_y分别为预先设定的匹配误差阈值；

3.10)将误匹配点剔除；返回步骤1.7)；

4)利用无迹卡尔曼滤波组合导航参数；

5)利用获取的无人机参数实时调整无人机位姿进行着陆引导。

本发明的有益效果是：

(1)本发明利用惯性导航计算得到的位置和姿态来对视觉导航匹配算法中的误匹配点实现快速剔除，不仅可以提高视觉导航算法的实时性，而且可以使视觉系统保持长时间的高精度；

(2)本发明采用视觉导航修正惯导，解决了单一惯导在导航着陆阶段，垂直定位功能较差，误差发散不可以单独使用的问题，且在视觉导航解算失败的情况下，组合系统则完全依靠惯性导航来提供载体的导航参数。

附图说明

图1是本发明步骤流程示意图；

图2是惯导系统修正视觉导航系统原理图；

图3是视觉导航修正惯导系统的原理图；

图4是仿真验证系统程序流程图；

图5是仿真结果图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明的组合导航方法是按以下步骤1)至步骤4)的基础上循环运行：

步骤1)利用视觉导航算法求解无人机位置及位姿包括1.1)～1.8)；

1.1)利用无人机机载摄像头获取实时图像，转到步骤1.2)；

1.2)判断获取的实时图像是否为第一帧图像，若是转到步骤1.3)，否则转到步骤1.4)；

1.3)第一帧图像处理过程如下：

1.3.1)对实时获取的图像进行预处理，提取实时图像的SURF特征点，转到步骤1.3.2)；

1.3.2)将实时图像提取的SURF特征点与参考图像的SURF特征点进行 SURF匹配，转到步骤1.3.3)；

1.3.3)利用RANSAC算法剔除SURF误匹配点对，获取正确的SURF匹配点对，转到步骤1.8)，执行完后转到步骤2)；

1.4)判断当前获取的实时图像帧数是否为T的倍数(T为预先设定的固定周期，本算法取T＝5)，若是转到步骤1.5)，否则转到步骤1.7)；

1.5)对实时获取的图像进行预处理，提取实时图像的SURF特征点，转到步骤1.6)；

1.6)将实时图像提取的SURF特征点与参考图像的SURF特征点进行SURF匹配，转到步骤3)，执行完后转到步骤1.8)；

1.7)利用LK光流法跟踪上一帧实时图像提取的SURF特征点，转到步骤1.8)；

1.8)利用获取的SURF匹配点对进行无人机位姿解算；

步骤2)利用视觉导航求解的无人机位姿作为惯导的初始值开始求解惯性导航参数包括2.1)～2.6)顺序执行；

2.1)姿态更新：利用陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转换成姿态矩阵，记为k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

2.2)姿态提取：根据姿态矩阵中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

2.3)比力分解：利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，

2.4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度，如下所示： a＝f^L-(2Ω^L _ie+Ω^L _el)v^L+g^L其中Ω^L _ie为地球自转角速度三维分量构成的反对称阵，Ω^L _el为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

2.5)积分求取速度；利用加速度进行积分得到速度增量，进而得到当前时刻的速度v^L _k+1如下所示：v^L _k+1＝v^L _k+a.Δt；

2.6)积分求取位置：利用速度进行积分得到位置增量，进而得到当前时刻的位置r^L _k+1，如下所示：

步骤3)利用惯导相邻时间获取的参数剔除实时图像与参考图像SURF匹配的误匹配点对，包括3.1)～3.10)顺序执行；

3.1)获取时刻t与时刻t+Δt的惯导参数(t与t+Δt为视觉导航mT帧数对应时刻与(m+1)T帧数对应的时刻，m为整数)；

3.2)求取时刻T与时刻t+Δt之间的惯导参数变换；

3.3)获取t时刻特征点在摄像头成像平面上的投影点坐标为W_t＝[u,v]^T，其中，u、v表示投影点在图像坐标系中的坐标值；

3.4)根据特征点匹配方法得到t+Δt时刻特征点在摄像头成像平面上的投影点坐标为W_t+Δt；

3.5)获取视觉导航t时刻到t+Δt时刻投影点坐标变化D＝w_t+Δt-w_t＝[x_i,y_i]^T；

3.6)根据惯导参数预测t+Δt时刻投影点坐标为: 为时刻t到t+Δt之间飞行器航向变化，T为时刻t到 t+Δt之间投影点平移向量；

3.7)根据惯导系统得到的特征点坐标变化向量

3.8)定义匹配误差:

3.9)误匹配点可以表示为:P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}其中d_x，d_y分别为预先设定的匹配误差阈值；

3.10)将满足P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}的误匹配点剔除。

步骤4)利用无迹卡尔曼滤波组合导航参数

4.1)求解状态方程；

4.2)求解量测方程；

4.3)无迹卡尔曼滤波器设计；

步骤5)利用获取的无人机参数实时调整无人机位姿进行着陆引导。

本发明的实施例如图1所示，在以下步骤1)至步骤4)的基础上循环运行：

步骤1)：利用视觉导航算法求解无人机位置及位姿包括1.1)～1.8)；

1.1)利用无人机机载摄像头获取实时图像，转到步骤1.2)；

1.2)判断获取的实时图像是否为第一帧图像，若是转到步骤1.3)，否则转到步骤1.4)；

1.3)第一帧图像处理过程如下：

1.3.1)对实时获取的图像进行预处理，提取实时图像的SURF特征点，转到步骤1.3.2)；

1.3.2)将实时图像提取的SURF特征点与参考图像的SURF特征点进行SURF匹配，转到步骤1.3.3)；

1.3.3)利用RANSAC算法剔除SURF误匹配点对，获取正确的SURF匹配点对，转到步骤1.8)，执行完后转到步骤2)；

1.4)判断当前获取的实时图像帧数是否为T的倍数(T为预先设定的固定周期，本算法取T＝5)，若是转到步骤1.5)，否则转到步骤1.7)；

1.5)对实时获取的图像进行预处理，提取实时图像的SURF特征点，转到步骤1.6)；

1.6)将实时图像提取的SURF特征点与参考图像的SURF特征点进行SURF匹配，转到步骤3)，执行完后转到步骤1.8)；

1.7)利用LK光流法跟踪上一帧实时图像提取的SURF特征点，转到步骤1.8)；

1.8)利用获取的SURF匹配点对进行无人机位姿解算；

步骤2)；利用视觉导航求解的无人机位姿作为惯导的初始值开始求解惯性导航参数包括2.1)～2.6)；

2.1)姿态更新：利用陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转换成姿态矩阵，记为k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

2.2)姿态提取：根据姿态矩阵中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

2.3)比力分解：利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，

2.4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度，如下所示：

a＝f^L-(2Ω^L _ie+Ω^L _el)v^L+g^L其中Ω^L _ie为地球自转角速度三维分量构成的反对称阵，Ω^L _el为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

2.5)积分求取速度；利用加速度进行积分得到速度增量，进而得到当前时刻的速度v^L _k+1如下所示：v^L _k+1＝v^L _k+a.Δt；

2.6)积分求取位置：利用速度进行积分得到位置增量，进而得到当前时刻的位置r^L _k+1，如下所示：

步骤3)；利用惯导相邻时间获取的参数剔除实时图像与参考图像SURF匹配的误匹配点对，包括3.1)～3.10)；

3.1)获取时刻t与时刻t+Δt的惯导参数(t与t+Δt为视觉导航mT帧数对应时刻与(m+1)T帧数对应的时刻，m为整数)；

3.2)求取时刻T与时刻t+Δt之间的惯导参数变换；

3.3)获取t时刻特征点在摄像头成像平面上的投影点坐标为W_t＝[u,v]^T；

3.4)根据特征点匹配方法得到t+Δt时刻特征点在摄像头成像平面上的投影点坐标为W_t+Δt；

3.5)获取视觉导航t时刻到t+Δt时刻投影点坐标变化D＝w_t+Δt-w_t＝[x_i,y_i]^T；

3.6)根据惯导参数预测t+Δt时刻投影点坐标为: 为时刻t到t+Δt之间飞行器航向变化，T为时刻t到t+Δt之间投影点平移向量；

3.7)根据惯导系统得到的特征点坐标变化向量

3.8)定义匹配误差:

3.9)误匹配点可以表示为:P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}其中d_x，d_y分别为预先设定的匹配误差阈值；

3.10)将满足P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}的误匹配点剔除。

步骤4)利用无迹卡尔曼滤波组合导航参数具体实施过程见图3所示：

4.1)系统状态方程求解：

X_k+1=f(X_k,W_k)

式中，状态向量X为：

L-无人机所在位置的纬度；λ-无人机所在位置的经度；h-无人机的高度；δV_e-无人机在地理系中的东向速度；δV_n-无人机在地理系中的北向速度；δV_u-无人机在地理系中的天向速度；-无人机俯仰角；-无人机横滚角；-无人机航向角；f(.)为9维向量函数，W_k为9维系统过程噪声。

4.2)系统量测方程求解：

由算机视觉定位几何关系图得量测方程如下所示：

Z_k=h(X_k,V_k)

其中Z_k为量测矩阵，X为状态向量，V_k为系统观测噪声。

4.3)组合滤波器设计：

4.3.1)在滤波开始时设定初始估计值和初始误差方程方差：

4.3.2)计算Sigma点设状态变量为n维，计算2n+1个采样点及其权值,这里n取9；

4.3.3)计算时间更新方程

4.3.5)滤波更新

式中：和分别为计算y的均值和方差权值： i＝1,...,2n；α,β,λ为3个参数，其中 1e^-4≤α≤1,λ＝0或3-n,β＝0

步骤5)利用获取的无人机参数实时调整无人机位姿进行着陆引导。

将获取的无人机姿态参数送入飞行控制系统，控制无人机进行着陆。

以上为无人机基于视觉/惯导组合的自主着陆方法步骤。

本发明的仿真验证系统利用虚拟现实技术和可视化技术，具体实施图4所示：模拟了真实的无人机飞行环境，具有沉浸式的真实感，解决了无人机自主着陆视景仿真模拟的问题，将视觉、惯导组合着陆算法与三维视景仿真软件结合，通过多通道展示整个着陆阶段并实时的计算和显示组合着陆导航所需数据。

给定实验体初始时刻的位置(L₀,λ₀),姿态以及速度即实验体初始时刻的导航信息。仿真结果如下:经过100s的仿真，从图5中可以看出，实验体的位置误差、速度误差以及姿态误差的误差基本上都是在0附近，误差曲线是收敛的。从这些结果可以看出，实验体导航信息的误差得到了较好的控制，同时也说明了本文中所提出的组合导航方案是可行的。

Claims (1)

1.一种基于视觉/惯导的无人机自主着陆方法，其特征在于包括下述步骤：

1)利用视觉导航算法求解无人机位置及位姿，步骤如下：

1.1)利用无人机机载摄像头获取实时图像；

1.2)判断获取的实时图像是否为第一帧图像，若是，转到步骤1.3)，否则转到步骤1.4)；

1.3)提取实时图像的SURF特征点，与参考图像的SURF特征点进行SURF匹配，利用RANSAC算法剔除SURF误匹配点对，获取正确的SURF匹配点对，转到步骤1.7)；

1.4)判断实时图像帧数是否为设定的固定周期T的倍数，若是，转到步骤1.5)，否则转到步骤1.6)；

1.5)提取实时图像的SURF特征点，与参考图像的SURF特征点进行SURF匹配，转到步骤3)；

1.6)利用LK光流法跟踪上一帧实时图像提取的SURF特征点；

1.7)利用获取的SURF匹配点对进行无人机位姿解算；

2)利用无人机位姿作为惯导的初始值，开始求解惯性导航参数，步骤如下：

2.1)利用陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转换成姿态矩阵k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

2.2)根据姿态矩阵中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

2.3)利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，得到导航坐标系下的比例观测值

2.4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度a＝f^L-(2Ω^L _ie+Ω^L _el)v^L+g^L，其中，Ω^L _ie为地球自转角速度三维分量构成的反对称阵，Ω^L _el为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

2.5)利用加速度进行积分得到速度增量，得到当前时刻的速度v^L _k+1＝v^L _k+a.Δt；

2.6)利用速度进行积分得到位置增量，进而得到当前时刻的位置

3)利用惯导相邻时间获取的参数剔除实时图像与参考图像SURF匹配的误匹配点对，步骤如下：

3.1)获取时刻t与时刻t+Δt的惯导参数；

3.2)求取时刻t与时刻t+Δt之间的惯导参数变换；

3.3)获取t时刻特征点在摄像头成像平面上的投影点坐标为W_t＝[u,v]^T，其中，u、v表示投影点在图像坐标系中的坐标值；

3.4)根据特征点匹配方法得到t+Δt时刻特征点在摄像头成像平面上的投影点坐标为W_t+Δt；

3.5)获取视觉导航t时刻到t+Δt时刻投影点坐标变化D＝w_t+Δt-w_t＝[x_i,y_i]^T；

3.6)预测t+Δt时刻投影点坐标 为时刻t到t+Δt之间飞行器航向变化，T为时刻t到t+Δt之间投影点平移向量；

3.7)得到特征点坐标变化向量

3.8)定义匹配误差

3.9)误匹配点P_em＝{P_(x,y)|e_x＞d_x,e_y＞d_y}，其中d_x，d_y分别为预先设定的匹配误差阈值；

3.10)将误匹配点剔除；返回步骤1.7)；

4)利用无迹卡尔曼滤波组合导航参数；

5)利用获取的无人机参数实时调整无人机位姿进行着陆引导。