CN111288989B

CN111288989B - 一种小型无人机视觉定位方法

Info

Publication number: CN111288989B
Application number: CN202010116259.8A
Authority: CN
Inventors: 赵文杰; 尹文财
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2020-02-25
Filing date: 2020-02-25
Publication date: 2021-11-05
Anticipated expiration: 2040-02-25
Also published as: CN111288989A

Abstract

本发明公开了一种小型无人机视觉定位方法，所述方法应用在无人机上，为无人机提供准确而可靠的定位，包括以下步骤：从多路摄像头中提取视觉特征点；使用深度滤波器对特征点进行深度融合，得到特征点的深度和方差；使用预积分技术对IMU数据进行预处理；使用IMU数据、视觉特征点对系统进行初始化；使用滑窗优化方式估计系统关键帧位姿，并利用特征点深度方差构建信息矩阵。本发明可通过提取无人机周围环境的视觉特征，实现复杂环境下的小型无人机视觉定位。

Description

一种小型无人机视觉定位方法

技术领域

本发明涉及视觉定位领域，具体涉及一种基于多相机的环视视觉定位方法，属于无人机自主导航与飞行控制等技术领域。

背景技术

可靠而准确的定位是无人系统实现运动控制、避障、路径规划以及抓取等各种复杂任务的基础。传统无人机为实现导航与定位功能，对GPS、IMU、磁罗盘等传感器有着较强的依赖性。在传统结构化、合作场景下，该定位方式尚可匹配无人机定位需求。然而，在丛林穿越等复杂场景下，GPS信号容易被遮挡和丢失，而IMU的漂移等特性也使得无人机的长时间定位存在风险。同时，磁罗盘等装置容易受到环境的电磁干扰。基于以上种种原因，无人机的飞行环境受到限制。

随着无人机的任务场景越来越复杂，传统的定位手段面临了上述诸多挑战，随着计算机技术和人工智能科技的不断发展，基于视觉惯性传感器的导航系统成为了近年来的研究热点，其中最主要的一个研究领域是SLAM(Simultaneous Localization andMapping)。无人机利用激光雷达、相机、IMU等传感器感知环境信息进而实现无人机自身的位置姿态求解，同时对周围信息建模，生成全局一致的地图。

主流的视觉SLAM方案依赖于单个相机或者一对双目相机进行环境信息的感知，在复杂光照场景下很容易发生特征的丢失。现有的多相机方案中，虽然对各相机的数据进行了融合，但对各相机的特征没有一个很好的区分，导致定位结果精度不高。

发明内容

本发明提出了一种基于多相机的环视视觉定位方法，该方法可在非结构化、复杂光照场景下，实现高精度视觉定位。在丛林等复杂光照环境下，针对基于传统组合导航的小型无人机定位困难的情况，本发明提出基于视觉的无人机定位方法。目前，基于单目和双目的视觉惯性导航系统VINS(visual inertial navigation system)在精确性、稳定性和鲁棒性方面已经有一些较成熟的研究方法。但由于传感器本身的缺陷，对复杂光照环境下的应用还需要进一步的研究。

为解决上述问题，本发明采用的技术方案为：

为了获得更大的视场角从而提升系统对环境的适应性，本发明采用多路摄像头来构建视觉信息。

为了快速初始化以及获得尽可能大的视场角，本发明综合采用了单目相机与双目相机，从而构成围绕载体的环视视觉传感器方案。

为了限制滑窗优化中特征的数量，同时也为了解决特征点差异问题，本发明提出了基于深度滤波器(DF)的信息矩阵的构建方法。

本发明小型无人机视觉定位方法，包括如下步骤：

步骤1：从多路摄像头中提取特征点，具体为从两路单目摄像头，两路双目摄像头中接收图像数据，输出视觉帧的特征点信息；

为了尽可能的提取出环境中的视觉特征，同时保证特征点数量不至于过高，影响系统解算速度，本发明采用极大值抑制算法对新特征点位置进行了限制。

步骤2：使用深度滤波器对特征点进行深度融合，得到特征点的深度和方差；

通过对同一特征点的多次观测进行融合，得到特征点深度的最优估计。在提升特征点深度估计精度的同时，对特征点深度方差进行了估计，使得来自不同构型的相机具有不同的深度方差，为滑窗优化中视觉特征的信息矩阵构建提供了参考值。

步骤3：使用预积分技术对IMU(Inertial measurement unit惯性测量单元)数据进行预处理；

通过预积分技术，避免了因系统线性化点的变化而造成的IMU重复积分，提升系统解算效率。

步骤4：使用IMU数据、视觉特征点对系统进行初始化；

使用前视双目摄像头对系统进行初始化，在保证精度的同时提高了系统初始化速度。

步骤5：使用滑窗优化方式估计系统关键帧位姿。

使用IMU，视觉特征点数据构建基于滑窗的联合优化方法。首先，对特征点进行甄选，选择一定规模的，具有良好约束的特征点送入滑窗进行优化。其次，采用步骤2中特征点的方差构建视觉特征的信息矩阵，调整特征点在优化中的权重，保证了优化方向，从而使得系统能适应不同构型的相机。

本发明与现有技术相比，其显著优点为：(1)使用多路异构相机，构成基于无人机机体的环视视觉传感方案，最大可能的提取环境信息，提高了环境特征的跟踪能力；(2)采用一种新的策略对滑窗内的特征点数量进行了约束，在保证实时性的情况下，提高了系统的精度；(3)采用一种基于深度滤波器(DF)的信息矩阵的构建方式，使得系统能够对不同特征点的优化权值进行调整，从而让系统能够适应不同类型的视觉传感器。本发明在公开数据集和仿真环境下进行了实验验证，实验结果表明，本发明定位精度高，环境适应性好，可以实现复杂光照环境下的小型无人机高精度定位。

附图说明

图1是基于多相机的环视视觉定位方法流程图。

图2是视觉特征提取示意图。

图3是特征点深度估计示意图。

图4是系统初始化示意图。

图5是系统滑窗优化示意图。

图6是使用深度滤波器在KITTI数据集下的实验结果示意图。

图7是GAZABAL仿真环境下视觉信息示意图。

图8是本发明在GAZABAL仿真环境下的实验结果示意图。

具体实施方式

为了克服在未知复杂光照环境下无人机定位问题，本发明创新性的提出使用多路摄像头进行视觉定位，同时使用深度滤波器技术对视觉特征进行了优化和区分，使得本发明能够融合不同构型的摄像头信息。总体流程图如图1所示，下面对本发明的具体步骤进行详细说明：

步骤1，从多路摄像头中提取特征点；

从两路单目摄像头、两路双目摄像头中接收图像数据，输出视觉帧的特征点信息，1个视觉帧对应6个图像帧；

如图2所示，步骤1具体包括:

步骤1.1特征提取与跟踪

特征提取与跟踪模块接收一帧新图像，用KLT算法来跟踪前一帧的稀疏特征点。同时，为了保证有足够的特征信息被使用，当最新帧跟踪的特征点小于规定值时，本模块提取新的Shi-Tomasi角点，使得每一帧图像所对应的特征点数量都保持一定的值。同时为了使得角点不会过于集中，采用极大值抑制算法对新特征点位置进行了限制。最后，采用基于本质矩阵的RANSAC算法，对特征点的误匹配进行了消除。

步骤1.2关键帧判断

对当前帧图像中的特征点进行统计分析，从而判断当前帧是否作为关键帧。判断依据有两个：第一，如果新提取特征点数量超过一定阈值，则认为是新的关键帧；第二，如果旧特征点的平均像素移动距离超过一定的阈值，则认为是新的关键帧。如果以上两个条件都不满足，则认为该帧不是关键帧。

步骤2，使用深度滤波器对特征点进行深度融合，得到特征点的深度和方差。

为了解决不同构型相机的特征点差异问题，本发明在特征点深度处理上采用了深度滤波器的策略，对于滑窗内所有的点，用深度方差来描述特征估计的可信度，由于一个特征点能够同时被多个关键帧观测到，因此特征点的深度估计可以通过迭代的方式融合多个图像的观测，从而有效的估计出深度的均值和方差，方差可以在优化时调整该特征点的权值，从而进一步提高优化效率和精度。

如图3所示，步骤2具体按如下步骤实施:

步骤2.1：三角测量对特征点深度做单次估计；首先，利用特征点首次观测帧与当前帧的位姿，使用三角测量方法，估算特征点深度。

s₁x₁ ^Tx₁-s₂x₁ ^TRx₂＝x₁ ^Tt

s₁(Rx₂)^Tx₁-s₂(Rx₂)^TRx₂＝(Rx₂)^Tt

特征点深度满足上述方程组，其中x₁，x₂分别表示特征点在首次观测帧与当前帧中的归一化坐标，R,t表示两帧间位姿变换，s₁，s₂特征点在首次观测帧与当前帧中的深度。本发明使用Cramer's法则对该方程组中的深度值s₁进行求解。

步骤2.2：求解特征点单次估计的深度方差：本发明使用几何不确定度来对特征点深度方差进行估计，δp＝|‖p‖-‖p′‖|。‖p‖表示特征点深度估计值，‖p′‖表示对特征点像素值进行一个像素的扰动后，新的特征点深度估计值。δp即为深度的一个几何不确定度，也就是深度融合的一个测量值所对应的均方误差。

步骤2.3：将上一状态最优深度与当前测量深度融合：对于滑动窗口内所有特征点，本发明用一个近似均匀高斯分布来描述特征点的深度分布，

其中，

为以真值

为中心的高斯分布，

为测量方差，ρ表示为该测量值属于有效测量的概率。

为均匀分布，d_min，d_max为其上下限。进一步，将将真实的后验分布近似表达为

由四个参数控制，分别为Z，σ，a，b。当新的测量值d_k到来时，深度滤波器的更新做如下处理：

步骤2.3.1：计算深度融合初值：

Z_old，

分别表示上一次深度融合后深度均值与方差，Z_est，

分别表示当前观测所计算的深度均值与方差。

步骤2.3.2：计算权重系数c₁:

a_old，b_old别表示上一次深度融合后均匀分布的左右边界权重系数c₁决定着新的观测深度值和方差对本次融合深度值和方差的加权比重；

步骤2.3.3：计算权重系数c₂:

a_est，b_est别表示当前观测所计算的均匀分布的左右边界；Z_range为场景的平均深度，权重系数c₂由上一次融合后的Beta分布参数决定，控制由上一次融合后的深度得到的深度估计值对本次深度融合的加权比重。

步骤2.3.4：归一化系数c₁，c₂:

步骤2.3.5：计算系数f,e用于更新Beta分布的参数a,b:

步骤2.3.6：更新融合后的深度值：

Z_new＝c₁·m+c₂·Z_est

步骤2.3.7：更新融合后的深度值方差：

步骤2.3.8：更新融合后的a,b:

步骤3，使用预积分技术对IMU数据进行预处理。IMU预积分借鉴纯视觉SLAM中图优化的思想，将帧与帧之间IMU相对测量信息转换为约束节点(载体位姿)的边参与到优化框架中。

步骤4，使用IMU数据、视觉特征点对环视视觉SLAM系统进行初始化。为了初始化的快速性和准确性，本发明使用前视双目摄像头初始化本系统。初始化采用视觉和IMU的松耦合方案。

如图4所示，所述的步骤4具体按如下步骤实施：

步骤4.1：使用左右摄像头信息，利用深度滤波的方式求解第一帧特征点的深度和方差。

步骤4.2：使用PNP的方式求解第二帧相对于第一帧的位姿。

步骤4.3：深度滤波求解第一帧与第二帧中尚未三角化的特征点的深度和方差。

步骤4.4：使用PNP的方式求解第三帧相对于第二帧的位姿。

步骤4.5：深度滤波求解前三帧中尚未初始化的特征点的深度和方差。

步骤4.6：按照步骤4.4、4.5的方式，迭代计算，直到求出滑窗内所有帧的位姿和特征点深度及方差。

步骤4.7：跟IMU预积分值进行对齐，求解重力方向，IMU的bias误差以及每一帧对应的速度。

步骤4.8：根据滑窗内所有帧的位姿以及相机外参，初始化其他相机观测的特征点深度及方差。

步骤5，使用滑窗优化方式估计SLAM系统关键帧位姿。由于深度滤波器的使用，当SLAM系统初始化完成之后，虽然滑窗内其他相机的特征点未参与初始化的优化操作，但其深度已经进行了有效的估计，初始化完成后，SLAM系统可以平滑的从双目传感器切换到环视传感。本发明使用滑动窗口优化手段来进行SLAM系统的状态估计，从而有效估计出无人机的运动状态，路标点的深度，IMU的偏置。SLAM系统的状态量为

其中，

表示路标点l在第k相机系统内的首观测帧的深度，l＝{1,2,…，m₁,m₁+1,m₁+2，…，m₂，…,m_k-1+1,…,m_k}；x_i表示第i个关键帧的运动状态信息，

b_ai,b_gi分别表示位置，速度，姿态，IMU加速度偏置，IMU角速度偏置；在此，取IMU的运动状态为系统的运动状态，包括位置、姿态、速度、IMU加速度偏置、IMU角速度偏置；

表示IMU与第j相机系统的外参；通过最小化所有测量值残差的马氏距离的方式，来获得SLAM系统状态量的最大后验估计。SLAM系统的最大后验估计可以转化为如下最小二乘问题：

如图5所示，所述的步骤5具体按如下步骤实施：

步骤5.1：特征点筛选；根据特征点深度方差，对特征点进行排序。优先选择方差小，被跟踪次数高的特征点构建优化方程，同时保证滑窗内特征点数目不超过一定的阈值，提高求解速度。

步骤5.2：构建视觉重投影误差项；采用视觉重投影误差来构建视觉残差项，对于路标点，在同一相机系统下的不同帧之间构建重投影误差。对于路标点l，假设在第k个相机系统观测下，在第i帧被首次观测到，其归一化坐标为

第j帧观测下，其归一化坐标为

则两帧观测间的重投影误差表示为:

其中，

表示

所在切平面的一组基；

表示路标点l首次被第k个相机系统观测到的像素坐标；

表示路标点l在第j帧观测下，被第k个相机系统观测到的像素坐标；

表示第k个相机系统的反投影函数，由相机内参决定；

表示第k个相机系统与IMU的外参；λ_l为特征点逆深度值；

表示第i帧的位姿；

表示第j帧的位姿该误差项的信息矩阵由特征点深度方差确定。

步骤5.3：构建IMU预积分残差项；关键帧b_k与b_k+1之间的IMU预积分残差项如下所示：

其中，

δb_a，δb_g分别表示预积分位移增量、速度增量、旋转增量、IMU加速度偏置、IMU角速度偏置残差；

分别表示预积分位移增量的真值和测量值；

分别表示预积分速度增量的真值和测量值；

分别表示预积分旋转增量的真值和测量值在SO3空间上的表示，取对数转换到se3空间中；

分别表示b_k+1帧与b_k帧的IMU加速度偏置；

分别表示b_k+1帧与b_k帧的IMU角速度偏置。

步骤5.4：边缘化残差项；边缘化将滑窗内的某些较旧或者不满足要求的视觉帧剔除，使用舒尔补的方式，在边缘化过程将相关联的约束转变为一个先验的约束条件放到优化问题中。

步骤5.5：求解优化方程：使用LM算法对优化方程进行求解，得到SLAM系统所有状态，进而得到无人机当前的位置、姿态、速度等信息。

为了测试深度滤波器的效果，本发明使用KITTI的灰度双目摄像头数据进行了实验，实验结果见图6。结果表明，在相同环境和参数配置下，采用深度滤波器的策略使得本发明在复杂环境下有着更好的表现力。

为了测试多目异构环视视觉方法的效果，本发明在GAZABAL仿真环境下进行了实验，包括1路前视双目摄像头，1路后视双目摄像头，左右2路单目摄像头，1个低精度IMU。实验结果见图8。实验结果表明，多路摄像头的引入使得结果的精度有了较大的提升，多相机的视觉特征点更加丰富，本发明可以根据特征点的方差，选择有利于求解优化方程的特征点进行优化。同时由于特征点在空间的分布更加的均匀，对于估计无人机的6DOF位姿而言，相比于单一视角是更加有利的。在某些场景下，多相机算法的精度提升达到了50％以上。在图7所示的场景下，由于前视双目摄像头视场角内的图像缺少视觉特征，导致里程计产生了比较明显的锯齿状，无人机存在失控的风险。然而对于多相机算法而言，由于视场角的增大，周围的环境中可提取的特征点依然丰富，本发明依然处于有效运行状态。

Claims

1.一种小型无人机视觉定位方法，其特征在于包括如下步骤：

步骤1：从多路摄像头中提取特征点

从两路单目摄像头、两路双目摄像头中接收图像数据，输出视觉帧的特征点信息；

所述的步骤2)具体为：

步骤2.1：利用特征点首次观测帧与当前帧的位姿，使用三角测量方法，估算特征点深度，特征点深度满足下述方程组：

s₁x₁ ^Tx₁-s₂x₁ ^TRx₂＝x₁ ^Tt

s₁(Rx₂)^Tx₁-s₂(Rx₂)^TRx₂＝(Rx₂)^Tt

其中x₁，x₂分别表示特征点在首次观测帧与当前帧中的归一化坐标，R表示帧间姿态的变换，t表示帧间位置的变换，s₁，s₂表示特征点在首次观测帧与当前帧中的深度，对该方程组中的深度值进行求解；

步骤2.2：求解特征点单次估计的深度方差

使用几何不确定度来对特征点深度方差进行估计，δp＝|||p||-||p′|||；

||p||表示特征点深度估计值，||p′||表示对特征点像素值进行一个像素的扰动后的新特征点深度估计值；δp即为深度的一个几何不确定度，也就是深度融合的一个测量值所对应的均方误差；

步骤2.3：将上一状态最优深度与当前测量深度融合

对于系统内所有特征点，用一个近似均匀高斯分布来描述特征点的深度分布，

其中，

为以真值

为中心的高斯分布，

为测量方差，ρ表示为该测量值属于有效测量的概率，

为均匀分布，d_min、d_max为其上下限；将真实的后验分布近似表达为

由四个参数控制，分别为Z，σ，a，b；当新的测量值d_k到来时，深度滤波器的更新做如下处理：

步骤2.3.1：计算深度融合初值：

Z_old，

分别表示上一次深度融合后深度均值与方差，Z_est，

分别表示当前观测所计算的深度均值与方差；

步骤2.3.2：计算权重系数c₁：

a_old，b_old分别表示上一次深度融合后均匀分布的左右边界，权重系数c₁决定着新的观测深度值和方差对本次融合深度值和方差的加权比重；

步骤2.3.3：计算权重系数c₂：

a_est，b_est分别表示当前观测所计算的均匀分布的左右边界；Z_range为场景的平均深度，权重系数c₂由上一次融合后的Beta分布参数决定，控制由上一次融合后的深度得到的深度估计值对本次深度融合的加权比重；

步骤2.3.4：归一化系数c₁，c₂：

步骤2.3.5：计算系数f、e用于更新Beta分布的参数a、b：

步骤2.3.6：更新融合后的深度值：

Z_new＝c₁·m+c₂·Z_est

步骤2.3.7：更新融合后的深度值方差：

步骤2.3.8：更新融合后的a、b：

步骤3：使用预积分技术对IMU数据进行预处理；

步骤4：使用IMU数据、视觉特征点对系统进行初始化；

步骤5：使用滑窗优化方式估计系统关键帧位姿。

2.根据权利要求1所述的小型无人机视觉定位方法，其特征在于所述的步骤1)具体为：

步骤1.1特征提取与跟踪

接收一帧新图像，用KLT算法跟踪前一帧的稀疏特征点，当最新帧跟踪的特征点小于规定值时，提取新的Shi-Tomasi角点，使得每一帧图像所对应的特征点数量都保持一定值；为了使角点不过于集中，采用极大值抑制算法对新特征点位置进行限制；最后，采用基于本质矩阵的RANSAC算法对特征点的误匹配进行消除；

步骤1.2关键帧判断

对当前帧图像中的特征点进行统计分析，从而判断当前帧是否作为关键帧，判断依据有两个：第一，如果新提取特征点数量超过一定阈值，则认为当前帧是新的关键帧；第二，如果旧特征点的平均像素移动距离超过一定的阈值，则认为当前帧是新的关键帧；如果以上两个条件都不满足，则认为该帧不是关键帧。

3.根据权利要求1所述的小型无人机视觉定位方法，其特征在于所述的步骤4)具体为：

步骤4.1：使用左右摄像头信息，利用深度滤波的方式求解第一帧特征点的深度和方差；

步骤4.2：使用PNP的方式求解第二帧相对于第一帧的位姿；

步骤4.3：深度滤波求解第一帧与第二帧中尚未三角化的特征点的深度和方差；

步骤4.4：使用PNP的方式求解第三帧相对于第二帧的位姿；

步骤4.5：深度滤波求解前三帧中尚未初始化的特征点的深度和方差；

步骤4.6：按照步骤4.4、4.5，迭代计算，直到求出滑窗内所有帧的位姿和特征点深度及方差；

步骤4.7：跟IMU预积分值进行对齐，求解重力方向，IMU的bias误差以及每一帧对应的速度；

4.根据权利要求1所述的小型无人机视觉定位方法，其特征在于所述的步骤5)具体为：

步骤5.1：特征点筛选

根据特征点深度方差，对特征点进行排序；选择方差小，被跟踪次数高的特征点构建优化方程，同时保证滑窗内特征点数目不超过一定的阈值；

步骤5.2：构建视觉重投影误差项

采用视觉重投影误差来构建视觉残差项，对于路标点，在同一相机系统下的不同帧之间构建重投影误差；对于路标点l，假设在第k个相机系统观测下，在第i帧被首次观测到，其归一化坐标为

第j帧观测下，其归一化坐标为

则两帧观测间的重投影误差表示为：

其中，

表示

所在切平面的一组基；

表示路标点l首次被第k个相机系统观测到的像素坐标；

表示第k个相机系统的反投影函数，由相机内参决定；

表示第k个相机系统与IMU的外参；λ，为特征点逆深度值；

表示第i帧的位姿；

表示第j帧的位姿该误差项的信息矩阵由特征点深度方差确定；χ表示系统的状态量；

步骤5.3：构建IMU预积分残差项

关键帧b_k与b_k+1之间的IMU预积分残差项如下所示：

其中，

分别表示预积分位移增量的真值和测量值；

分别表示预积分速度增量的真值和测量值；

分别表示b_k+1帧与b_k帧的IMU加速度偏置；

分别表示b_k+1帧与b_k帧的IMU角速度偏置；

步骤5.4：边缘化残差项

边缘化过程将相关联的约束通过舒尔补的方式转变为一个先验的约束条件放到优化问题中；

步骤5.5：求解系统方程

系统的状态量为

其中，

表示路标点l在第k相机系统内的首观测帧的深度，l＝{1，2，...，m₁，m₁+1，m₁+2，...，m₂，...，m_k-1+1，...，m_k}；x_i表示第i个关键帧的运动状态信息，

b_ai，b_gi分别表示位置，速度，姿态，IMU加速度偏置，IMU角速度偏置；在此，取IMU的运动状态为系统的运动状态，包括位置、姿态、速度、IMU加速度偏置、IMU角速度偏置；

表示IMU与第j相机系统的外参；

通过最小化所有测量值残差的马氏距离的方式，来获得系统状态量的最大后验估计；系统的最大后验估计转化为如下最小二乘问题：

其中，||r_p-H_pχ||²为边缘化残差项，通过舒尔补的方式获得；ρ为测量值属于有效测量的概率；使用LM算法对优化方程进行求解，得到系统所有状态，从而得到无人机当前的位置、姿态、速度信息。