[发明专利]无人机平台基于单目SLAM的多关键帧协同地面目标定位方法

摘要

本发明公开了一种无人机平台基于单目SLAM的多关键帧协同地面目标定位方法，用于解决现有目标定位方法定位精度低的技术问题。技术方案是首先采用无人机及其内嵌的单目相机作为数据的采集平台，再结合视觉ORB‑SLAM算法完成多关键帧选取以及其位姿解算，由于ORB‑SLAM可实时获得相机精确姿态，相比于通过静态场景三维点求解平面具有速度快、精度高等特点。最后利用射影重构理论通过多关键帧协同的方式求解目标的三维坐标。该方法适用于各种复杂的环境，具有较高的测量精度。经测试，本发明方法在室内具有厘米级定位精度，在室外20米飞行高度定位误差<0.3m。

主权项

一种无人机平台基于单目SLAM的多关键帧协同地面目标定位方法，其特征在于包括以下步骤：步骤一、标定单目相机内参；采用张氏标定法，调整标定板，连续采集20幅图像，进行角点检测，计算图像和棋盘格之间的单应变换H，利用棋盘格的几何关系，建立各视图图像中各角点的对应关系，棋盘格上所有角点的空间坐标是满足共面约束的，假设其Z坐标为0，对单应性进行计算：uv1=HXY1---(1)]]>其中，u,v为角点的像素坐标，X,Y为角点在棋盘格上的空间坐标；分解H如下：H＝sK[r1 r2 t]   (2)其中，r1,r2,t为棋盘的位置姿态，s为标量系数，K为相机内参矩阵，利用r1和r2的正交性求解K；步骤二、基于ORB‑SLAM的关键帧姿态估计；采用ORB‑SLAM，在已知相机内参的情况下，针对输入的视频序列，ORB‑SLAM算法在经过初始化操作完成后，跟踪、地图构建以及闭环检测三个线程并行操作，完成相机实时的姿态估计同时构建增量式地图，其中得到的关键帧记录了整个场景的关键信息；均匀选取五个关键帧{frel1,frel2,frel3,frel4,frel5}并记录姿态信息，作为单目尺度解算基础；步骤三、解算场景中基于标定板的单目尺度；单目视觉自身存在物理尺度不确定的特点，经过步骤二，ORB‑SLAM算法估计得到的关键帧姿态是相对的，为了获得关键帧的真实姿态，必须恢复场景的尺度；步骤二选取的五个关键帧{frel1,frel2,frel3,frel4,frel5}画面中都含有放置的标定板，通过OPENCV开发的标定程序完成相机外参的标定，同时结合之前的相机内参，计算关键帧实际的投影矩阵得{P1,P2,P3,P4,P5}并进一步解算关键帧之间的真实姿态{Pose1,Pose2,Pose3,Pose4,Pose5}:Posei＝PiP1‑1   (3)其中，P1是第一基准关键帧的投影矩阵，Pi是第i个关键帧的投影矩阵，其中i＝2,3,4,5；通过比较真实姿态和ORB‑SLAM估计得到的相对姿态，解算场景的尺度Scale；步骤四、TLD目标跟踪；采用TLD目标跟踪算法，首先对待测定位的目标进行圈选，并针对视频序列实时的得到目标图像坐标，然后保存关键帧中目标图像坐标{x1,x2......xn}；步骤五、解算多关键帧协同的目标三维坐标；采用多关键帧协同的目标定位方法，首先依据对极几何的原理采用双关键帧实现目标世界三维坐标初值的解算；三维坐标X，其在两关键帧中的观测点是x,x'，现假设分别是观测点x,x'满足极限约束的附近点；依据最小化投影误差，求解如下：C(x^,x^′)=d(x,x^)2+d(x′,x^′)2---(4)]]>其中，F是两个关键帧之间的基本矩阵，和是观测点的投影误差；求解分为两步，首先通过DLT得到初始值，然后LM非线性迭代优化设满足x×PX＝0，x'×P'X＝0，故有：x1(P3T)-(P1TX)=0y1(P3T)-(P2TX)=0x1(P2T)-y1(P1TX)=0x2(P′3TX)-(P′1TX)=0y2(P′3TX)-(P′2TX)=0x2(P′2TX)-y2(P′1TX)=0---(5)]]>其中，PiT是P矩阵的第i行,P'jT是P'矩阵的第j列，其次坐标x＝(x1,y1,1)，x'＝(x2,y2,1)；转化X的线性方程组为AX＝0；每一组对应点可以确定三个方程，但是只有两个线性无关，故转化A为下式：A=x1P3T-P1Ty1P3T-P2Tx2P′3T-P′1Ty2P′3T-P′2T---(6)]]>由于齐次坐标X有三个自由度，而AX＝0含四个方程，故(6)式为一个超定方程组，通过得到AX＝0的最优解，其中||AX||＝1；采用多关键帧协同，使用捆束调整最小化多视图重投影误差，进一步优化X，如下式：minΣi=1nvid(Q(X,Ii),xi)2---(7)]]>其中，Ii代表的式第i个关键帧，vi由下式确定：vi=1,xi∈Ii0,xi∉Ii---(8)]]>Q(X,Ii)代表目标点X在第i个关键帧上的投影点，d(Q(X,Ii),xi)2表示目标点在第i个关键帧上的重投影误差。