[发明专利]基于视觉SLAM的复杂环境下无人机自主降落区域选择方法

摘要

本发明公开了一种基于视觉SLAM的复杂环境下无人机自主降落区域选择方法，用于解决现有无人机降落区域控制方法实用性差的技术问题。技术方案是通过无人机移动平台搭载的俯视单目相机得到图像序列，通过SLAM算法对无人机的位姿进行实时解算并建立稀疏的点云地图，将点云地图网格化，构建二维网格高度地图。然后结合Means shift图像分割算法根据高度对网格地图进行划分，最后根据降落高度要求，筛选出离潜在障碍物距离最远的适合无人机降落的区域。本发明采用单目视觉SLAM对无人机进行位姿的解算，实时估计出无人机的位姿，构建二维网格高度地图，筛选出适合无人机降落的区域。由于本发明不依赖于标志物，因此实用性好。

主权项

一种基于视觉SLAM的复杂环境下无人机自主降落区域选择方法，其特征在于包括以下步骤：步骤一、用无人机机载的单目相机采集20张标定板的图像，采用张氏标定法，用Matlab工具包对相机内参进行标定；步骤二、估计基础矩阵F和单应矩阵H，通过比较重投影误差选择模型；地图初始化算法步骤：提取参考帧和当前帧的特征点pc，然后对两帧之间进行特征匹配如果匹配点数不够，则重置参考帧；通过对极约束计算出单应矩阵Hpc＝Hpr                             (1)其中pc和pr是两帧检测出的特征点对，H是单应矩阵；计算基础矩阵FpcTFpr=0E=KTFK---(2)]]>其中，K为相机的内参矩阵，E为本质矩阵；从本质矩阵恢复运动，其中本质矩阵E的自由度为8，通过经典的八点法计算得到，对参考帧和当前帧进行特征匹配，通过匹配上的特征点解算出本质矩阵E；u1v11e1e2e3e4e5e6e7e8e9u2v21=0---(3)]]>两帧图片匹配上特征点满足公式(3)，将公式(3)转换成线性方程组的形式，求解出本质矩阵E的参数；得到本质矩阵E之后，通过奇异值分解进行求解：E＝UΣVT                              (4)其中，U,V为正交阵，Σ为奇异值矩阵；通过SVD分解得到t，R；通过4对匹配特征点计算出单应矩阵H；将单应矩阵H看成是一个向量，通过直接线性变换法恢复单应矩阵H；通过分解得t，R；通过对极约束得到相机的位姿；然后求解得到这些匹配点的3D坐标；这些3D坐标就是地图初始化的地图点；在地图初始化成功之后，用PnP求解3D到2D点对的运动；通过参考帧的3D地图点和当前帧的2D特征点匹配，通过PnP解算出3D到2D点的运动，获取当前帧的位姿；提取三维的地图点用于后续的聚类算法；采用闭环检测对相似的场景进行检测；在闭环检测部分采用词袋模型DBoW2来实现；词袋模型通过提取特征并通过聚类的方法将特征词进行分类，然后利用视觉词袋量化图像特征，利用统计的词频直方图将图像进行归类；当相机来到之前到过的地方，用过闭环检测部分检测闭环，同时为了有效的完成闭环，使用本质图优化位姿图，将闭环的误差分散到整个图中；步骤三、通过相似变换的求解估计单目视觉SLAM尺度因子，单目视觉SLAM系统的导航参考坐标系为地图初始化时的相机坐标系OCXCYCZC，GPS的导航坐标系为OtXtYtZt；假设SLAM系统输出的相机位置为对应的GPS输出真实的位置信息根据图像变换的原理；t＝sRC+t0                          (5)其中，R为OCXCYCZC坐标系到坐标系OtXtYtZt的旋转矩阵，t0平移向量；R和t0通过组合导航系统的标定参数直接获得，s为SLAM系统的尺度因子，在初始化阶段需要估计此参数的值；由于系统测量误差的存在，式(5)的左右两边是不等的，记残差为ei＝ti‑sR·Ci‑t0；尺度因子s的估计问题转化为求取最优值s使得minsΣi=1n||ei||2---(6)]]>最小，最后解算出最优解s；步骤四、将地平面划分成一个个小网格，将SLAM算法检测得到的世界坐标系下的地图点投影到二维的网格地图中；定义每个网格高度如下式：h(i,j)=Σk∈N(i,j)hk-hmin-hmaxN(i,j)-2---(7)]]>其中，hk表示网格Grid(i,j)中的地图点，hmin和hmax表示网格中高度值最大和最小的地图点，去除掉这两个点之后将落在网格中的地图点的高度均值赋值给网格高度h(i,j)；步骤五、对步骤四得到的二维网格高度地图进行平滑与划分，输入二维网格高度地图的大小与每一个网格的高度，将每一个网格作为最小单元；先使用Mean shift算法对网格高度进行聚类，确定网格高度分类的类别总数和各类别的类心，然后以类别数和类心为输入，使用Mean shift算法得到网格地图最终的划分结果；具体步骤如下：运用Mean shift算法对网格地图进行平滑，对于每一个网格初始化j＝1，并且使模值点yi,1＝xi；运用公式计算yi,j+1直到收敛，记收敛后的值为yi,c；对图像进行mean shift平滑，将收敛结果存储在zi中，zi＝yi,c；将zi中所有空间距离小于hs，高度值距离小于hr的网格划分到不同的类别{Cp}p＝1,…,m中去；对于每一个网格i＝1,2…,n，类别标志L i＝{p|zi∈Cp}；{xi}i＝1,2,…,n,{zi}i＝1,2…n分别表示原始地图中的网格和平滑后的网格，上标r,s分别表示网格的高度信息和坐标，{Li}i＝1,2,…n表示分割后每一个网格的类别标签；步骤六、由步骤五得到的网格高度类别，选择出合适降落的高度hUAV，根据无人机大小设定合适降落的区域面积sUAV，通过两部筛选，确定无人机最佳降落地点；定义网格是否适合降落公式：T(i,j)=Σm,n∈R(i,j,r)||h(m,n)-h(i,j)||2---(8)]]>其中，h(m,n)义为二维网格地图上地图点(m,n)的高度值，h(i,j)表示网格坐标下(i,j)网格的高度值；其中r表示搜索的半径；根据实际无人机的大小搜索以坐标h(i,j)为原点，半径为r的区域；遍历每一个网格，筛选出适合无人机降落的区域；将没有投影点的网格区域标志为不可降落区域；最后根据实际的应用设定公式T(i,j)的一个阈值用于判断适合无人机降落的网格，同时将适合无人机降落的网格标注出来；对标志过适合降落区域的网格地图进行遍历，选择出满足条件：每一个网格标签为hUAV对应的类别，且面积大于等于sUAV的每一个区域，并对其进行编号areai；计算出每一个区域areai距离高度值大于hUAV的区域的距离，选择距离最大的areai为降落点。