基于人工标志的单目视觉下无人机位姿估计

吴 妍，吴 芬，戚国庆

（南京理工大学 江苏 南京 210094）

基于人工标志的单目视觉下无人机位姿估计

吴 妍，吴 芬，戚国庆

（南京理工大学 江苏 南京 210094）

就无人机的自定位问题，文中提出一种数字图像处理结合2D-3D场景重建的位姿估计算法。在已知人工标志世界坐标的前提下，对于单目视觉下机载非线性相机拍摄的单帧图像，利用图像处理技术为位姿解算提供特征点数据。位姿解算提供了一种改进的N点透视算法，该算法结合正交迭代算法，以N点透视算法计算的旋转矩阵值作为迭代算法初值，迭代得到最终估计结果。通过仿真实验和实物实验验证了算法的可行性。文中提出的算法，较N点透视算法，提高了位姿估计的精度且收敛速度快，给无人机室内位姿估计提供了的一种新的解决思路。

自定位；非线性；人工标志；N点透视；正交迭代

无人机凭借结构稳定、可操作性高、无人驾驶等特点，在航拍勘测、目标定位跟踪等领域获得了广泛应用。在利用无人机对地面/海上等目标的定位与跟踪、无人机自身位姿控制、无人机测量、无人机自主降落等问题中，无人机自身位姿的确定，成为研究的前提，是不可回避的一大难题。

目前，无人机自身的位姿信息主要依靠GPS[1]和IMU[2]获取。GPS受环境与气候的干扰严重，尤其在室内，无人机无法接收到GPS信号，因此无法获取自身的位置信息[3]。而IMU是根据加速度计和陀螺仪的姿态变化积分获取位姿信息的。实际应用中会产生累积误差从而降低位姿估计的精度。随着CCD器材等视觉传感器的制作工艺日趋精良，利用视觉传感器获取的信息进行位姿估计成为一种有利的替代手段[4]。

基于视觉的无人机位姿估计[5-7]，减少了携带的设备且价格低；同时，视觉信号稳定，鲁棒性强，不存在累积误差。利用视觉信息实现无人机的位姿估计，无疑是实现无人机自主位姿估计的又一有力技术手段，尤其是在室内无法获取GPS信号的区域[8-9]。

针对室内无人机的位姿估计，文中提出了一种基于数字图像处理技术和2D-3D场景重建的位姿解算方法。事先在实验室内铺设信息已知的人工标志，利用图像处理技术检测并提取人工标志的特征点信息，在此基础上，借助坐标系转换以及摄像机成像模型，建立图像坐标系2维平面到3维世界坐标下的转换模型，即位姿解算模型，从而解算出无人机自身位姿信息。

1 系统结构

文中以常见的四旋翼无人机作为研究对象，考虑系统的实时性与低复杂性，选用蓝色双三角平面标志物作为人工标志，在无人机上固定相机，对机载相机拍摄的图片进行图像处理，将处理结果和已知世界坐标系下的图像坐标作为位姿解算模型的输入，解算无人机位姿。假设室内无人机飞行区域中都能通过机载相机观测到架设的人工标志。位姿估计示意图如图1所示。

图1 位姿估计的示意图

视觉位姿估计系统由两个模块构成：图像处理以及位姿解算。图像处理模块完成人工标志的检测与提取。利用颜色特征完成人工标志的检测，借助Harris角点检测、Hough直线拟合获取特征点的提取。图像处理的流程如图2所示。

图2 图像处理流程图

位姿解算，实际就是对于机载相机的外参数的估计。在完成摄像机内参数标定的前提下，利用点透视算法求解一个粗精度的位姿将其作为正交迭代算法[10]的初始值，经迭代求解位姿。下文对位姿解算原理进行详细介绍。

2 位姿估计原理

位姿解算，是对世界坐标系下信息已知的人工标志进行成像信息进行特征分析处理求解。这里对坐标系转换以及摄像机成像原理进行如下说明。

2.1 摄像机模型

摄像机模型，是对摄像机成像过程的数学表达。物体在图像坐标上的像点与摄像机内部参数以及物体的空间相对位置有关。理想的摄像机模型为针孔模型，但实际中，相机一般由于镜头畸变呈非线性。非线性相机模型为：

式（1）中 f为焦距，xc，yc，zc为摄像机坐标系下坐标，xI，yI为图像坐标系下坐标。 k1，k2为畸变系数，取二阶径向模型。摄像机的焦距，像元尺寸，光心，偏移量，不垂直度，畸变系数等内部参数都是未知的，要实现位姿估计还需对其标定。

2.2 坐标系转换

位姿解算本质上是2D-3D的场景重建，在此过程中，涉及到世界坐标系、机体坐标系、摄像机坐标系及图像坐标系的相互转换。

系统将无人机与摄像机固连，只需解算出摄像机在世界坐标系下的位姿，即认为获取无人机的位姿。下文的机体坐标系，均以摄像机坐标系取代。就研究涉及的三个坐标系间的转换关系介绍如下：

1）世界坐标系-摄像机坐标系

世界坐标系到摄像机坐标系的转换如图3所示。

图3 世界坐标系-摄像机坐标系

两不同的右手坐标系可通过旋转平移转换重合。旋转变换可视为三轴以一定顺序分别旋转的合运动。取绕各轴逆时针旋转的角度为正，按照X-YZ旋转顺序，得到符合系统的旋转矩阵：

式（3）中 准，θ，ψ 对应横滚角、俯仰角、偏航角。下文用r11-r33代替R矩阵的各个元素。

2）摄像机坐标系-图像坐标系

摄像机坐标系到图像坐标系的转换是基于摄像机模型的。上一章节已给出详细的阐述

其中 u，v 是图像坐标，xc，yc，zc是摄像机的坐标，u0，v0为图像中心，fx，fy为像素单位下的焦距，k1，k2为畸变系数。

2.3 位姿估计算法

1）N点透视算法

考虑到无人机系统机动性较大，位姿估计需兼顾实时性，不宜取过多特征点。本系统拟采用平面标志物的6个特征点分析解算。理论上，对于平面目标，只需要4个共面且任意三点不在一条直线上的点集即可确定出无人机的位姿[11]。

对于图像处理后获取的点坐标进行畸变校正。将校正后的坐标ui，vi代入模型。从摄像机坐标系到图像坐标系，是一个3D-2D的转换过程。转换中，深度信息丢失，但是可以在求解过程中消去该变量。求解的方程组如下：为归一化的摄像机坐标。 假设 tz=1，有 8个未知量，根据PnP理论和线性代数知识，需要任意三点不共线的4个点才能求解。对于式（5）可记为：Am=b。通过最小二乘法可以解出 m=（ATA）-1ATb。结合旋转矩阵的单位性和正交性可以求解出tz、RJS、TJS。

至此，位姿估计的初始值求解完成。

2）正交迭代算法

对于N点透视算法，各个特征点提取都存在着量测误差，估计精度随之降低。基于此，本文参考了一种正交迭代思想[12-13]来对N点透视算法的估计值进行进一步优化。

已知任意空间点与其成像在同一条过光心的直线上[14]。利用这个性质，以量测像点到实际像点的距离构造误差方程。假设人工标志的特征点在世界坐标系下的坐标为wi，根据提取的图像坐标解算出投影在归一化平面上的像点vi。基于文献[13]的物空间误差模型建立目标函数如下：

令E（R，T）值最小的R，T为最优估计。对于这两个参数的最小平方的估计，文献[11]给出利用奇异值分解法的求解方案。旋转矩阵的精度与平移向量精度息息相关，利用迭代算法计算得到旋转矩阵R*。在此基础，计算关于R*的T的最优解作为平移变量T的初始值：

根据vi=R*wi+T，更新vi，迭代若干次，计算使E最小的R和T。其中最优R*的求解。采用SVD分解法[11，13-15]进行求解。

3）位姿求解

在旋转矩阵和平移向量求解完成后，位姿估计就十分直观了。对于位置的求解，根据世界坐标-摄像机坐标系的转换关系以及旋转矩阵的定义，可以得到位置t和姿态角为：

3 实验验证

3.1 仿真实验

在Matlab2012b平台上，对位姿估计算法进行仿真实验。通过给定姿态角位置和摄像机模型求解出对应的图上坐标，对求解的图上坐标加不同强度（单位为像素）的随机噪声 q×（rand（1）-0.5），通过位姿估计算法，将结果与参考值对比。仿真实验中影响位姿估计精度的因素，可以归纳为：量测噪声强度、人工标志距离光轴的远近、人工标志的大小/距摄像机距离以及人工标志的特征点数。下文就以上因素给出仿真结果。

1）量测噪声强度——取6个特征点作为人工标志的输入，保持位置一定，对于噪声强度从0-10，以0.5为间隔，每个强度取1 000次估计结果的平均值。在不同强度的噪声下，解算出来的位姿误差如如图4所示，随着噪声的增强，姿态角和位置估计的误差逐渐增大。在噪声强度为10像素之内，角度偏差最大为2°。当无人机距人工标志物的距离在1 600 mm左右，对于强度为10像素的随机噪声会造成最大x方向2cm的偏差，y方向6 cm的偏差，z方向4 cm的偏差。

图4 不同噪声强度下位姿估计误差

图5 距光轴远近不同对位姿估计的影响

2）人工标志距离光轴的远近——仿真取相同噪声条件下，初始状态为[-pi，0，0]时，可视为距光轴为0，横滚角以0.1 rad递增，距离逐渐变大，位姿解算的误差如图5所示。人工标志距光轴的距离随着roll角的减小而增大，位姿估计误差随着光轴的距离的增大而减小。

图6 人工标志大小/相机距离对位姿估计的影响

图7 特征点个数对位姿估计的影响

3）人工标志的大小/距摄像机距离——简单地，取相机位于目标的正上方，在相同的噪声强度下，同样大小的人工标志，取不同的高度，即对应不同的得到的仿真结果如图6所示。结果表明，距离越大位姿估计精度越低。

4）平面特征点的个数——平面特征点至少要4个，考虑到这里要保证其他因素不变，取标志物上的4/5/6个特征点观察一下趋势，仿真结果如图7所示。特征点给的个数越多，位姿估计精度越高，但实际系统中太多的特征点在图像处理中会耗时多，无法满足实时性的要求。

5）除此以外，仿真实验比较了本文算法和单独的

N点透视算法的估计精度。对于不同噪声强度下，取：

作为衡量位置和角度与标准值的平均偏差，仿真结果如图8。

实验结果表明，加入正交迭代，对于位姿估计的精度有明显提升，尤其是在噪声强度大的时候，效果更明显。

图8 两种算法的位姿误差

3.2 实物实验

由于无人机在室内无法获取GPS信号，因此利用VICON红外三维运动捕捉系统，来获取无人机的位置姿态信息，以此作为无人机位姿的标准值。实物实验选取的机载相机型号为Gopro hero4 silver。采用加州理工大学开发的camera calibration toolbox在MATLAB2012b下进行棋盘标定，标定结果如表1所示。

图9 图像处理过程

表1 摄像机参数标定结果

实物实验使用的人工标志物为蓝色的挖空的三角形图像，对机载相机拍摄的图像做离线的图像处理，由于篇幅限制，截取的部分图像处理的过程如图9所示。

人工标志固定，选取无人机任意位置拍摄的两组图片进行位姿解算验证算法。相机拍摄的两组位姿图像如图10所示。解算结果如表2所示。

图10 两组位姿图像

表2 位姿解算结果

实验表明，该位姿估计算法能实现无人机的位姿估计，并达到一定的精度。在实物实验中，出现了上文例子2中，偏航角的解算出复数形式，即死锁问题，这是由于欧拉角表示的旋转矩阵的局限，这一问题可通过四元数法表示的旋转矩阵进行修正。

4 结 论

文中对无人机室内飞行时的位姿实时获取提供了一种由粗到精的位姿估计方法。根据不同位姿下机载相机下的人工标志不同的成像信息，在世界坐标系和图像坐标系下建立转换关系，利用点透视算法解位姿初值代入正交迭代算法，最终实现位姿的精确解算。经实验验证，在噪声影响较小的情况下，本算法能实现室内无人机位姿的实时高精度估计。但该算法是基于点特征，受噪声影响较大，一旦噪声过大，则估计不准确，下一步人工目标的特征设计可朝着曲线特征[16]以及与IMU信息融合的方向改进。

[1]孙罡.低成本微小型无人机惯性组合导航技术研究[D].南京：南京理工大学，2014.

[2]沈浩.无人机MIMU/GPS组合导航系统研究[D].哈尔滨：哈尔滨工程大学，2012.

[3]张勇.基于合作目标的无人机位姿估计算法研究[D].南京：南京航空航天大学，2010.

[4]窦川川.基于惯性传感和视觉方法的无人直升机位姿估计比较研究[D].哈尔滨：哈尔滨工业大学，2013.

[5]Li J H，Xie X Z.Study of Monocular Measuring Technique Based on Homography Matrix[M].Berlin Heidelberg:Springer 2011:316-324.

[6]Jayatilleke L，Zhang N.Landmark-based localization for Unmanned Aerial Vehicles[C].//Systems Conference （SysCon）， 2013 IEEE International IEEE，2013:448-451.

[7]Zhu L， Fan B，Du Y， et al.A tracking and locating method for UAVs Vision System[C].//Information and Automation （ICIA）， 2010 IEEE International Conference onIEEE，2010:1953-1957.

[8]Wang T，Zhang Y，Wang C，et al.Indoor visual navigation system based on paired-landmark for smallUAVs [C].Robotics and Biomimetics（ROBIO）， 2014 IEEE International Conference onIEEE，2014:1703-1708.

[9]杨世保.无人机着陆位姿参数视觉估计研究[D].南京：南京航空航天大学，2012.

[10]李鑫，龙古灿，刘进博，等.相机位姿估计的加速正交迭代算法[J].光学学报，2015（1）:266-273.

[11]王鹏，孙长库，张子淼.单目视觉位姿测量的线性求解[J].仪器仪表学报， 2011，32（5）:1126-1131.

[12]Morbidi F，Mariottini G L.Active Target Tracking and Cooperative Localization for Teams of Aerial Vehicles[J].IEEE Transactions on Control Systems Technology， 2013，21（5）:1694-1707.

[13]Mirota D J， Ishii M， Hager G D.Vision-based navigation in image-guided interventions.[J].Annual Review of Biomedical Engineering，2011，13（13）:297-319.

[14]李飞，唐大全，毕波，等.基于点线对应的无人机位姿估计算法 [J].计算机测量与控制，2011，19(12):3108-3110.

[15]许允喜，蒋云良，陈方，等.基于点线对应的多摄像机全局位姿估计迭代算法[J].光子学报，2010，39（10）:1881-1888.

[16]尚晓红.一种基于圆或平行线特征的位姿估计方法[D].昆明：云南大学，2012.

Pose estimation of UAV based on artificial marks with monocular vision

WU Yan， WU Fen， QI Guo-qing
（Nanjing University of Science and Technology， Nanjing 210094， China）

This article proposes a pose estimation algorithm with a combination of digital image processing and 2D-3D scene reconstruction to deal with UAV self localization.Under the premise of the known world coordinates of the artificial mark，we obtain the information from the images taken by a nonlinear onboard camera and take some methods of digital image processing to provide feature point datas for subsequent calculation.Pose estimation provides a improved N-point perspective algorithm which combines orthogonal iteration.N-point perspective calculates attitude angles matrix as the initial values of orthogonal iteration.The feasibility of the algorithm is verified by simulation experiments and real experiments.Compared with N-point perspective algorithm，this method improves the pose estimation accuracy and fast convergence and provide a new idea for the UAV indoor pose estimation.

self localization； nonlinear； artificial marks； PnP； orthogonal iteration

TN29

A

1674－6236（2017）12-0143-06

2016-08-04稿件编号：201608037

国家自然科学基金项目（61104186；61273076）

吴妍（1992—），女，江苏南通人，硕士研究生。研究方向：四旋翼无人机的定位与导航、图像处理等。