一种基于合作目标的视觉定位方法

聂 烜， 陆满君， 席超敏， 戚漫华

(1. 西北工业大学， 陕西 西安 710072； 2．上海无线电设备研究所， 上海 200090)

一种基于合作目标的视觉定位方法

聂 烜1， 陆满君2， 席超敏1， 戚漫华1

(1. 西北工业大学， 陕西 西安 710072； 2．上海无线电设备研究所， 上海 200090)

提出一种基于合作目标特征点的无人机当前空间位姿的测量算法。以合作目标为参照对象，在图像滤波、阈值分割、边缘提取和角点提取等算法的基础上，提取了合作目标特征点，并建立了描述目标图像坐标、世界坐标和摄像机参数之间关系的数学模型，然后根据SVD算法求解该模型得到无人机旋转和平移矩阵的初值；最后利用正交迭代算法寻找最优解，求得无人机的位姿信息。并针对合作目标分级的特点设计了适应不同高度的特征点选取算法，提高了目标识别的可靠性。仿真实验分析验证了本文方法的有效性。

视觉测量； 合作目标； 仿真试验

0 引言

无人机依靠机载的导航设备和飞行控制系统来进行定位导航并最终控制无人机降落在平台或跑道等预定场地上，目前用于无人机的主流导航技术包括：惯性导航系统(INS)，全球定位系统(GPS)，INS/GPS组合导航系统[1-2]。惯性导航系统及全球定位系统均有各自的缺点，INS单独使用时定位误差会随时间的积累不断增加，而GPS动态响应能力差，易受电磁干扰，信号容易被遮挡，对于低速运动效果差[3]。随着计算机视觉技术的发展，视觉导航由于其高自主性、廉价性、高可靠性等特点被越来越多地应用在无人机自主导航中[4]。相比于惯性导航方法，视觉导航没有精度时间漂移、误差累积的问题；相比于GPS导航方法，视觉导航不易受干扰，对导航适应能力强；相比于INS和GPS传感器，视觉传感器具有体积小、成本低、功耗小的优势，适合用于无人机平台。本文通过摄像机的标定与合作目标特征提取，基于单目视觉以计算无人机相对位置和相对姿态。

1 摄像机模型和标定

首先提取拍摄到的棋盘格图像的内角点，在不考虑畸变参数的情况下，根据图像角点的像素坐标与棋盘内角点实际坐标的投影关系计算摄像机的几何投影参数；得到几何投影参数后，根据畸变模型建立方程组，计算畸变参数并优化投影参数得到摄像机内参数。

本文利用图1所示10×7的棋盘格进行摄像机标定，大小为19 mm×19 mm，在不同角度拍摄多幅棋盘图像，利用OpenCV函数库的findChessboardCorners()函数寻找棋盘格内角点，然后利用calibrateCamera()函数求解内参数和畸变参数。求解得到的摄像机内参数和畸变参数，如式(1)所示。

图1 摄像机标定棋盘格

式中：fx，fy，u0与v0分别以像素为单位。采集的标定棋盘格图像个数越多，标定精度就越高，但是当图像数量达到一定数量的时候[5]，标定结果不会有太大差异，一般采集30幅图像就能满足要求。摄像机拍摄的图像大小为640×480，摄像机标定的主点与理想主点坐标(320,240)较接近。对图2中的原图根据式(1)进行校正的结果如图3所示，由图可以看出图像边缘的畸变得到改善。

图2 原图

图3 校正后图像

2 基于合作目标的无人机视觉定位

本文通过计算机视觉方法测量无人机与合作目标的相对位置。图像特征点提取和无人机位姿解算算法是测量系统的两个重要部分，合作目标的实际尺寸已知，提取图像特征点坐标，根据位姿解算算法解算无人机相对位置，解算无人机相对位置的过程如图4所示。

图4 无人机定位流程图

(1) 合作目标

摄像机采集图片，受光照等条件的影响，会导致图像的明暗程度发生改变，而降落地点的不同，意味着合作目标背景图像的不确定性。无人机在飞行过程会产生持续的高频振动，由旋翼经由机体结构传至摄像机，虽然大部分振动被减震装置过滤了，但仍会使图像出现细微模糊的现象[6]。另外，无人机在空中的飘移也会对图像造成影响，水平方向的位移会使合作目标移出视野，而垂直方向的变化，则使摄像机的可视范围发生变化。在高空状态下，可视范围广，但是单位像素代表实际尺寸变大，造成合作目标图像细节缺失，会影响图像处理及解算；低空状态下可视范围变小，合作目标容易移出视野。

为了能提取有效的合作目标特征信息，并通过解算实时输出可靠的位置信息，合理地设计合作目标显得尤为重要。本文中的位置参数是通过特征图案中特征点的世界坐标及其图像象素坐标之间的转换关系进行计算得到的，因此要求合作目标图案中应包含足够多的特征点，这些特征点应该容易被精确提取，并且应该考虑在不同高度下的识别问题。

本文设计的合作目标背景为黑色，图案中心为白色正方形，在其外依次包围两个白色正方形环，如图5所示。

图5 合作目标

该合作目标具有如下特点：

a) 合作目标为三级白色正方形嵌套，中间用黑色分割，目标各级矩形边长比值为固定值，能唯一的识别目标级数；

b) 各级目标内外轮廓为正方形，每个正方形提供四个角点信息，用以求解无人机位置；

c) 合作目标只有黑白两种颜色，使得合作目标灰度差距大，便于阈值分割；

d) 合作目标分为三级，当目标部分被遮挡时仍可正常工作，提高了鲁棒性。

(2) 特征提取

无人机视觉辅助过程中，机载摄像机拍摄合作目标图像，由于多种因素的影响，不可避免地存在噪声干扰，因而必须经过图像处理算法后，才能进行下一阶段位置解算。进行图像处理时，要选择并提取典型的、稳定的图像特征，且要能完整体现整幅图像的特征，比较有代表性的图像特征是点和线两种。

根据世界坐标系和图像坐标系的透视投影关系，以世界坐标系下点的坐标和图像坐标系下对应像点的坐标为基础，进行无人直升机位置的解算，所以本文采用基于点特征的图像处理算法来提取合作目标特征点。

为此，本文提取合作目标上的特征点，而角点是图像的重要特征，其具有旋转不变性，且不随光照强度改变而变化。角点具有明显的特征，表现为图像中亮度和边缘曲率变化剧烈的地方[7]。图3中合作目标包含3个白色正方形，选择白色正方形顶点，即角点作为特征点，共12个，从外向内按顺时针顺序将12个特征角点按0～11标号，如图6所示。

图6 合作目标特征点

图像处理算法流程包括：

a) 图像获取；

b) 图像预处理，主要包括色彩空间转换、图像滤波；

c) 合作目标图像边缘提取；

d) 角点信息提取。

机载摄像头得到原始图像，在生成、传输、转换过程中由于多种因素影响，最终得到的图像会含有高斯噪声、椒盐噪声等噪声。为得到更为准确的目标信息，需要在图像边缘检测之前进行图像预处理，包括色彩空间转换、图像滤波处理。

合作目标图像边缘提取的目的是将目标边缘从阈值分割后的二值图像中提取出来，便于进行角点提取。针对合作目标阈值分割图像，边缘提取的目的是提取图像中正方形的边缘。阈值分割得到的二值图像，目标和背景被赋予不同的像素值，轮廓在像素值发生突变的区域，本文用链码标记图像中像素值突变的区域，提取目标轮廓。

在识别目标时，通过顺序找出边缘点来跟踪边界。本文的轮廓跟踪算法迭代扫描图像直到遇到连通区域的一个点，以它为起点，跟踪它的轮廓，标记边界上的像素，当轮廓完整闭合，扫描回到上一个位置，直到再次发现新的成分。跟踪轮廓时，轮廓序列用链码表示，链码步长固定且方向数目有限，链码的步长为一个像素，方向如图7所示。

图7 链码方向

只要轮廓的起点用坐标表示，其余点都可只用接续方向来表示偏移量。链码跟踪轮廓的具体算法如下：

a) 首先按从上到下，从左到右的顺序扫描图象，寻找没有标记跟踪结束记号的第一个边界起始点A0，它是具有最小行和列值的边界点。定义一个扫描方向变量dir，该变量用于记录上一步中沿着前一个边界点到当前边界点的移动方向，其初始化取值为dir=7；

b) 按逆时针方向搜索当前象素的3×3邻域，若dir为奇数搜索方向设定为(dir+6)%8，若dir为偶数搜索方向设定为(dir+7)% 8，在3×3邻域中搜索到的第一个与当前像素值相同的像素便为新的边界点An，同时更新变量dir为新方向值；

c) 如果An等于第二个边界点A1且前一个边界点An-1等于第一个边界点A0，则停止搜索，结束跟踪，否则重复步骤2继续搜索；

d) 由边界点A0，A1，A2，…，An-2构成的边界便为要跟踪的边界。

上述算法适用于跟踪目标轮廓的外边界，图8为上述算法提取的目标轮廓。

图8 目标轮廓

无人机位置坐标的解算是根据目标特征点的实际坐标与目标特征点的图像坐标计算，本文研究目标特征点的图像坐标的提取。本文先对提取的轮廓进行多边形拟合，然后根据约束条件得到目标的四边形轮廓，接着计算四边形的顶点坐标得到目标的特征点坐标，最后将特征点坐标精确到亚像素级精度。

多边形拟合是采用极少量的点作为多边形的顶点，以这个多边形来逼近原始的目标物体的数字轮廓曲线。本文采用的多边形拟合算法首先从轮廓选择两个距离最远的点，然后这两个线段连成一个线段，在查找轮廓上到线段距离最远的点，添加到逼近后的新轮廓；算法反复迭代，不断将最远的点添加到结果中，直到所有的点到多边形的边的最短距离小于指定的阈值。轮廓多边形拟合步骤如下：

a) 设定一个距离阈值T；

b) 选择轮廓序列中距离最大的两个点，记做A、B；

c) 计算在轮廓A，B两点间所有点到AB直线间的距离，确定距AB直线的距离最大的点C；

d) 比较C点到直线AB的距离和阈值T，如果小于T，迭代结束；

e) 标记C点为顶点，C点将目标的边界曲线分为AC和CB两条曲线，对这两条曲线重复步骤c)、d)、e)。

由于重复上述步骤时都要产生两个分支，因此在算法上要采用递归调用的方法求解，直到目标的轮廓曲线上所有点与拟合多边形各边的距离小于设定的阈值T。图9是用以上算法得到的轮廓逼近的多变形。

图9 目标多边形拟合图像

(3) 相对位置解算

无人机视觉过程中，摄像机不断拍摄合作目标图像，经过图像处理算法提取出特征点的图像坐标后，下一步即是根据合作目标尺寸、方位、上述图像处理后所获得的特征点等信息，计算无人机对于合作目标的相对位置。

旋翼无人机机体坐标系的定义与惯性传感器的安装方向有关，无人机与摄像机的相对位置固定。合作目标的位置用世界坐标系的坐标表示，无人机的位置可用摄像机坐标系中的坐标表示，无人机的相对位置就可以用机体坐标系和世界坐标系之间的转换关系表示。

无人机的相对位置是由世界坐标系与摄像机坐标系之间平移关系确定的，摄像机坐标系沿三个轴方向分别平移一定距离，直至摄像机坐标系与世界坐标系重合，此时无人机沿三个轴方向移动的距离即是它与合作目标的相对位置，这个平移可以用一个平移向量T=(Tx,Ty,Tz)T表示。

无人机的姿态角可根据世界坐标系与摄像机坐标系之间的旋转关系确定，包括俯仰角、滚转角和偏航角。滚转角φ是摄像机绕X轴旋转，角度范围[-180°，+180°]，右滚转为正，旋转矩阵如式(2)所示。

(2)

俯仰角θ是摄像机绕Y轴旋转，角度范围[-90°，+90°]，抬头为正，旋转矩阵为

(3)

偏航角ψ是摄像机绕Z轴旋转，角度范围[-180°，+180°]，右偏航为正，旋转矩阵为

(4)

无人机视觉过程中，合作目标特征点在世界坐标系下的坐标与对应图像特征点在图像坐标系下的坐标，有如下关系：

(5)

式中：λ为比例因子；x是图像特征点在图像坐标系下的齐次坐标；X是目标特征点在世界坐标系下的齐次坐标；K是摄像机内参矩阵；R和T分别为摄像机坐标系与世界坐标系之间的旋转和平移矩阵。

式(5)中比例因子λ是空间点在摄像机坐标系下的Zc分量，因此合作目标特征点X对应的λ的值为

(6)

式中：e=[0，0，1]。

将式(5)和式(6)结合，可得到如式(7)所示的约束：

(7)

因为合作目标所有特征点都在一个平面内，目标特征点在世界坐标系下的Zc分量X3=0，于是X=[X1，X2，0，1]T，令R=[r1，r2，r3],T=[t1，t2，t3]T，于是式(7)可变换为

(8)

代入摄像机成像方程，特征点的世界坐标和对应的图像坐标都满足式(9)：

(9)

其中：

如果已知n个特征点的世界坐标和对应的图像坐标，根据式(9)可以建立2n个方程的方程组，进而求解得到[r1，r2，T]。又因为旋转矩阵R是3×3的正交矩阵，且行列式的值为1，则r3可由式(10)计算得

r3=r1×r2

(10)

求解式(9)组成的方程组，再结合式(10)就可以得到摄像机坐标系相对于世界坐标系的旋转和平移矩阵，并进一步得到摄像机(即无人机)在世界坐标系中的坐标，即无人机对于合作目标的相对位置。

关于平移向量T的求解，因合作目标总是位于摄像机的前方，所以平移向量的第三项总是大于零，如果经过SVD分解后V的最后一项是负数，则让V=-V，然后根据式(11)求解平移向量，其中T为SVD分解后的最后一列，T′是所求的平移向量。

(11)

最后进行旋转矩阵R的求解，旋转矩阵是正交矩阵，故其行列式det(R)=1，对[r1,r2,0]进行SVD分解[r1,r2,0]=U∑VT，则旋转矩阵R=UVT，如果det(UVT)=-1，则旋转矩阵的最后一列反向。

上述算法求解的旋转和平移矩阵一般得不到较高的精度，通常需要对上述结果使用非线性迭代方法进行优化，以精确计算无人机位姿。

正交迭代算法通过最小化空间点与由重投影像点确定的空间直线之间的距离求解旋转和平移矩阵[8]，该距离称为像方残差，像方残差目标函数为

(12)

式中：R和T分别为旋转和平移矩阵；Pi为空间点在世界坐标系下的坐标；I为单位矩阵；Vi=qiqiT/qiTqi表示视线投影矩阵；qi=[u,v,1]为空间点投影到归一化图像平面的像点坐标。

如果已知旋转矩阵R，目标函数是关于T的二次函数，其解为

(13)

正交迭代算法的具体步骤如下：

a) 第一步：给定初始的旋转矩阵R0，然后根据式(13)求平移矩阵T0，令K=0；

b) 第二步：用qi表示空间点在归一化图像平面的像点坐标，根据下式计算qi点坐标

qi(R,T)=Vi(RPi+T)

(14)

c) 第三步：进一步得到式(15)

(15)

最小化上式，求得旋转矩阵R；

d) 第四步：令Rk+1=R，同时根据式(3-31)计算Tk+1=T(Rk+1)；

e) 第五步：若|E(Rk+1，Tk+1)-E(Rk，Tk)|<ε，算法结束，输出旋转和平移矩阵，否则k=k+1，返回第二步。

(4) 分级处理

本文设计的合作目标是三级嵌套的正方形，主要是为了解决因为不同高度或者其他干扰因素造成无人机在只提取到目标的部分特征点的情况下能正常工作的问题。

如果摄像头拍摄的图像能提取到三个正方形的特征点，即目标特征点完全提取，图像特征点与目标实际特征点是一一对应的，不需要做处理；如果从图像中只提取到一个或两个正方形的特征点，需要对图像特征点进行分析以确定正确的目标特征点。

若提取到两个正方形特征点，因为平行直线投影变换保持简比不变，可以根据图像中两个正方形的边长比与目标实际边长比进行区分，而本文设计的合作目标相邻边长的比具有明显的差异，适合用简比不变的性质区分；若提取到一个正方形特征点，利用提取到的图像特征点坐标和每组正方形的特征点坐标分别计算无人机对于合作目标的高度，然后将计算得到的高度与上一帧的高度或者惯性传感器测量的高度进行比较，选择最接近的作为计算结果。

上述方法很好地解决了无人机在不同高度对目标的部分识别而引起的特征点对应问题，提高了无人机定位的可靠性。

3 实验分析

本文分别测量摄像机平移前和平移后相对于 合作目标的坐标，然后将这两个坐标的差与摄像机实际的平移进行对比分析，以验证算法有效性。

将摄像机分别沿x，y，z轴平移，在每一个方向测量10次，将其结果均值作为该点坐标的测量值。在距离目标1 500 mm处(此时测得x轴,y轴,z轴的坐标分别为261.71，-162.69和302.14)，让摄像机分别沿x轴，y轴和z轴移动，每移动100 mm测量一次坐标值。x方向测量结果如表1所示，y方向测量结果如表2所示，z方向测量结果如表3所示。

图8，图9和图10分别为x轴，y轴和z轴对应测量点的坐标曲线。以x轴举例说明。摄像机在x轴的初始坐标为261.71，在第一次移动后，其理论偏移量应为100 mm，在实际测量后，此时实际坐标为164.03，偏移量为97.68，相对于100的理论值误差为2.32,之后继续进行10次测量，并记录数据。图10中的(a)图绘制出了位移的理论值和实际值的曲线，(b)图绘制出了误差的曲线。y轴和z轴同理。

表1 x方向测量值

表2 y方向测量值

表3 z方向测量值

图10-12中：(a)图表示测量值与真值；(b)图表示测量值与真值的误差，根据图10(b)和图11(b)图可以看出，在距离目标1 500 mm处，x和y的视场范围内，这两个方向测量100 mm的绝对误差在15 mm内，图12(b)表明z方向的绝对误差也在15 mm内。

4 结束语

本文研究基于合作目标的无人机视觉定位方法，首先分析了合作目标的设计思想；然后研究从图像中提取合作目标的图像处理算法，包括图像滤波、阈值分割、边缘提取、特征点提取等算法，提出了分级目标的识别算法，并对算法做了验证；最后研究了已知图像特征点坐标和实际特征点坐标解算无人机相对位置的算法和合作目标分级处理的方法。

图10 x方向测量值与误差

图11 y方向测量值与误差

图12 z方向测量值与误差

[1] Wendel J, Meister O, Schlaile C, et al.An Integrated GPS/MEMS-IMU Navigation System for an Autonomous Helicopter [J]. Aerospace Science and Technology, 2006, 10(6): 527-533.

[2] Ronnbaek S.Development of a INS/GPS Navigation Loop for an UAV[J].Master,Dissertation: Lulea University of Technology, Swedish; Lulea, 2000.

[3] 张伟.导航定位装备[M].北京:航空工业出版社, 2010: 101-130.

[4] 黄显临,姜肖楠,卢鸿谦,等.自主视觉导航方法综述[D].吉林大学学报(信息科学版), 2010， 28(2): 158-163.

[5] Saaidi A, Halli A, Tairi H, et al. Self-calibration Using a Particular Motion of Camera[J]. WSEAS Transactions on Computer Research, 2008, 3(5)： 295-299.5

[6] 郑晓平.基于视觉的小型无人直升机自主降落导航系统的研究[D].浙江大学硕士学位论文, 2013： 12-30.

[7] Harris C,Stephens M. A Combined Corner and Edge Detector[A]. Fourth Alvey Vision Conference[C]. 1988: 147-151.

[8] 龙古灿.无人机视觉引导中的位姿估计问题研究[D].长沙:国防科学技术大学, 2011: 15-23.

AMethodofUAV’SVisionPositioningUsingaCooperativeObject

NIEXuan1,LUMan-jun2,XIChao-min1,QIMan-hua1

(1. Northwestern Polytechnical University, Xi’an Shaanxi 710072, China; 2. Shanghai Radio Equipment Research Institute, Shanghai 200090, China)

A measurement algorithm based on the Unmanned Aerial Vehicle (UAV) current spatial position and gesture of the cooperative object feature points is proposed. With the cooperative object as a reference object, on the basis of image filtering algorithm, threshold segmentation algorithm, edge extraction algorithm and corner extraction algorithm, extract the cooperative object feature points and establish the mathematical model describing the correlativity among image coordinate, world coordinate and the parameters of camera, and then solve this model to get the initial value of the UAV rotation and transition matrix according to SVD algorithm. Finally, use orthogonal iteration algorithm to find optimization and get the position and gesture information of the UAV. Besides, the algorithm of feature extraction adapting to different altitude specific to the hierarchical cooperative object is designed, the reliability of the object recognition is improved. The analysis of the simulation experiments verifies the validity of this method.

vision positioning; cooperative object; simulated test

1671-0576(2017)02-0042-09

2017-01-18

聂 烜(1976-)，男，博士，硕士生导师，副教授，主要从事软件工程和计算机应用技术研究。

V443.2

A