CN113436276B - 基于视觉相对定位的多无人机编队方法 - Google Patents

Abstract

本发明涉及多无人机基于视觉的相对定位和编队控制技术，为提出解决GPS拒止环境下的多无人机定位问题的方法。为此，本发明采取的技术方案是，基于视觉相对定位的多无人机编队方法，步骤如下：首先建立相机成像和运动的数学模型‑针孔相机模型；由于相机在运动，相机平面的运动在世界坐标系下由旋转矩阵R和平移向量t表示，R与t被称为相机的外参数，这也是视觉定位算法需要解算的目标；设计二维码标签识别与视觉定位。本发明主要应用于多无人机基于视觉的相对定位和编队控制场合。

Description

基于视觉相对定位的多无人机编队方法

技术领域

本发明涉及多无人机基于视觉的相对定位和编队控制技术，具体讲,涉及针对无人机编队利用视觉信息确定相对距离和进行编队控制的方法。

背景技术

近年来，多旋翼无人机在高空摄影、灾后救援、环境监测等军事和民用领域得到越来越广泛的应用。精确可靠的位置反馈是多无人机实现编队的必要基础。对于单个无人机而言，通过机载的惯性测量单元(Inertial Measurement Unit,IMU)可以测量无人机三轴姿态角以及三维加速度，从而可以实现快速、稳定的姿态控制。对于多无人机系统，编队的基本要求是多无人机在三维空间内的位置排列与变换，因此，在多无人机编队系统中，无人机不但要准确的知道自身所在的位置，更需要获知其他无人机的位置。随着无人机定位技术的逐渐成熟，基于全球定位系统(Global Positioning System，GPS)的定位方法被广泛应用于无人机室外作业。然而，GPS的定位精度并不高，无法满足精细作业任务的定位要求。对于分布较为密集的无人机编队，GPS所能捕获的最小位置变化甚至超过了无人机之间的距离，这将对编队系统造成严重的伤害。此外，在室内场景、森林、楼宇等环境下，GPS的信号微弱，无法进行定位。

对于上述无GPS环境下的定位问题，国内外很多研究单位，如捷克理工大学、瑞士联邦理工大学、新加坡南洋理工大学、香港科技大学、北京航空航天大学等，基于多种定位方法开展了相关研究，如基于运动捕捉系统(Motion Capture)的定位技术、即时定位与地图构建(Simultaneous Localization and Mapping,SLAM)定位技术、超宽带(Ultra WideBand,UWB)定位技术、基于标签(Marker)的定位技术以及实时动态(Real-Time Kinematic,RTK)载波差分定位技术，并且对这些方法的定位效果进行飞行实验的验证(期刊：Proceedings IEEE International Conference on Robotics and Automation；著者：Shaojie Shen，Nathan Michael，Vijay Kumar；出版年月：2015年；文章题目：Tightly-coupled monocular visual-inertial fusion for autonomous flight ofrotorcraftMAVs；页码：5303–5310)。

但是，当前各种定位方法均有其各自的局限性。其中，光学运动捕捉系统的定位精度最高，定位误差可达毫米级，但其价格昂贵，并且需要固定摄像机的位置，进行标定后才能使用，大大限制了无人机应用的场地环境。相比而言，视觉SLAM定位技术对无人机本身的运动范围的限制最小，对场地的要求低，具有较强的环境适应能力，但是其计算量大，对机载计算机或外部计算机的性能要求较高，定位的实时性较差。相比较于上述定位方法，基于标签的定位技术对场地和环境的依赖远低于运动捕捉系统、UWB和RTK，而且计算量较小，可以直接在无人机的机载嵌入式计算板上进行数据处理，因此适合应用于多无人机的目标跟踪与定位。

发明内容

为克服现有技术的不足，本发明旨在提出解决GPS拒止环境下的多无人机定位问题的方法。为此，本发明采取的技术方案是，基于视觉相对定位的多无人机编队方法，步骤如下：

首先建立相机成像和运动的数学模型-针孔相机模型，设O-x-y-z为相机坐标系，z轴指向相机前方，x轴向右，y轴向下。O为相机的光心，O′-x′-y′为成像平面，物理世界的点P,经过小孔O投影之后的成像点为P′，P点坐标为[X,Y,Z]与P′点坐标[X′,Y′,Z′]之间有如下关系：

其中f为相机的焦距，定义像素坐标系o-u-v，在像素坐标系与成像平面之间存在一个平面的缩放和原点的平移变化，u轴缩放的倍数为α，v轴的缩放倍数为β，原点的平移量为[c_x,c_y]^T，则P′点的像素坐标可以表示为：

令f_x＝fα,f_y＝fβ,得到：

其中f的单位为米，α,β的单位为像素/米，f_x,f_y的单位为像素，将上式写成矩阵形式：

上式等号右边的矩阵为相机的内参数矩阵，在使用过程中不会发生变化，但实际使用之前需要对相机进行标定；

由于相机在运动，相机平面的运动在世界坐标系下由旋转矩阵R和平移向量t表示，R与t被称为相机的外参数，这也是视觉定位算法需要解算的目标；设计二维码标签识别与视觉定位的具体步骤如下：

1)检测线段

在获得由相机捕获到的图像之后，首先对图片进行预处理，先对图片进行灰度化处理，再用高斯模糊方法处理灰度图，以平衡图像输入时的噪声，降低目标误检测的概率，然后计算图像中每个像素点的梯度，将每个像素点的梯度幅值按照给定的阈值进行划分，从而检测出边缘点，使用最小生成树聚类算法，将划分得到的边缘点聚类，完成图像中所有点的聚类之后，使用加权最小二乘法来拟合直线的方程，从而检测出图像中的线段；

2)四边形识别

检测出图像中的线段之后，接下来要判断哪些线段是二维码标签所在区域的边缘，首先对所有线段进行分组，将按逆时针方向连接，且连接处两个端点的距离小于给定阈值的线段分在同一组，对所有分组，选择起始线段，搜索到线段末端时，按照逆时针方向寻找距离足够小的下一条线段，四次搜索后，若最后一条边与第一条边构成一个闭环，则认为该分组可以组成一个四边形，符合要求；

3)标签解码与匹配

第2)步可能检测出多个四边形，或者检测出的四边形可能并不是需要的标签，所以需要对得到的四边形进行编码匹配和检查，在标签中，四边形是以四个顶点的形式进行存储的，根据这四个点可以确定其四边形内部点阵的坐标；

首先提取点阵最外围的像素点的平均灰度值v₁，再提取次外围像素点的平均灰度值v₂，由于标签本身最外一圈所有点的灰度都是黑色的，而次外圈的灰度既有黑色也有白色，因此在同一光照环境下，v₁与v₂的值相差较大，选出分割效果明显的阈值，设定区阈值为M＝(v₁+v₂)/2，遍历点阵内所有点，灰度高于阈值的像素点编码为0，而低于阈值的像素点编码为1，由此得到了一串二进制编码；最后将得到的二进制编码在已知的编码库中进行匹配，并进一步识别出标签的ID；

4)求解单应矩阵(Homography Matrix)

单应矩阵描述了物理世界中同一个点在两个不同的成像平面上的坐标之间的变换关系，通过求解单应矩阵，估计出相机的外参数；利用第2)步检测出来的标签的四个顶点与第3)步匹配到的库中标签的四个顶点，组成四对匹配的特征点，通过直接线性变换法DLT(Direct Linear Transform)求解单应矩阵，并进一步将单应矩阵分解成摄像头运动的旋转矩阵R和平移向量t；

四个特征点p₁～p₄在世界坐标系下的坐标分别为(x_wi，y_wi，z_wi),在相机坐标系下的坐标分别为(x_ci，y_ci，z_ci)，对于到成像平面上的坐标为(x_i，y_i)，另外定义标签坐标系，以标签对称中心为坐标原点O_t，以标签平面为x_t-O_t-y_t平面，平行与正方向标签上下边沿水平向右为x_t轴，垂直x_t竖直向上为y_t轴，由右手法则得到z_t轴，则特征点在标签坐标系下的坐标为(x_ti，y_ti，0),根据坐标系变换关系，得到：

其中

为标签坐标系相对与相机坐标系的旋转矩阵，

为标签坐标系原点在相机坐标系下的三维位置坐标，其三个坐标系上的分量分别为

展开得到：

其中

分别表示标签坐标系x_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，同理，

分别表示标签坐标系y_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，

分别表示标签坐标系z_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，由公式(4)得

将式(6)代入式(7)中得到：

对于4个特征点，存在8个方程，共8个未知数，进行求解；

接下来分析无人机受到的相机视场约束，在光学仪器中，以光学仪器的镜头为起点，以被测目标的物像可通过镜头的最大范围的两条边缘构成的夹角，称为视场角，对于直线投射影像，视角w的计算公式如下：

其中f为相机的有效焦距，d为相机内部图像传感器的大小在一个方向的计算，对于已知视场角为w的相机，视场中的物体的位置满足以下约束：

设计一种编队控制算法，将相机的视场约束融合进来，使无人机快速、稳定地跟踪标签：

为方便对四旋翼无人机的动力学模型进行数学描述，采用北东地(North EastDown,NED)坐标系进行建模描述；

基于牛顿欧拉方程(Newton-Euler equation)，四旋翼无人机的运动学及动力学模型由下面的公式表示：

其中ξ＝[x,y,z]^T为惯性坐标系的三维位置向量，V无人机在惯性坐标系下的三维线速度向量，R∈SO(3)表示从机体坐标系到惯性坐标系的旋转矩阵，m表示无人的质量，Ω为无人机在机体坐标系下的角速度向量，g为重力加速度，J为无人机惯量矩阵，

代表机体坐标系下无人机的力矩输入，f为垂直于机体平面的总推力，e₃＝[0,0,1]^T为单位向量,

为未知扰动，无人机的模型简化为

其中

分别为机体坐标系下无人机的位置和速度。

在外环中分别对无人机的位置和速度进行PID(Proportional IntegralDerivative)控制，设计的外环控制器结构如下式：

其中

为期望的相对位置输入,K_pv、K_iv、K_dv分别为速度环的P、I、D控制器参数，K_px、K_ix、K_dx分别为位置环的P、I、D控制器参数。

本发明的特点及有益效果是：

本发明针对GPS拒止环境下的多无人机定位问题，采用基于单目视觉的相对定位方法进行研究。该方法既能对无人机之间的相对位置和姿态进行有效估计，同时免去了特征点的检测和匹配过程，大大地减少了计算量，提高了识别和定位的实时性。此外，在单目视觉定位的基础上，加入了对相机视场约束条件的考虑，设计了视觉跟踪控制器，实现多无人机编队。实验表明，该方法具有较好的定位精度，且多无人机取得了较好的跟踪效果。

附图说明：

图1是针孔相机成像模型。

图2是单目相机标签检测过程示意图。

图3是本发明使用的标签及其解码示意图。

图4是标签识别与定位算法的流程示意图。

图5是相机的视场示意图。

图6是视觉跟踪控制器框图。

图7是多无人机编队平台实物图。

图8是四旋翼无人机的硬件框架。

图9是四旋翼无人机系统的硬件连接示意图。

图10是机载计算板的软件框架图.。

图11是无人机室内标签定位实验场景。

图12是室内标签定位实验中无人机在X方向的运动曲线图。

图13是是室内标签定位实验中无人机在Y方向的运动曲线图。

图14是室外标签跟踪实验场景。

图15是室外标签跟踪实验中无人机/标签在X方向的位置曲线。

图16是室外标签跟踪实验中X方向的跟踪误差与视觉解算的相对位置曲线。

图17是室外标签跟踪实验中无人机/标签在Y方向的位置曲线。

图18是室外标签跟踪实验中Y方向的跟踪误差与视觉解算的相对位置曲线。

图19是室外视觉跟踪无人机编队试验场景。

图20是室外视觉编队实验中无人机在X方向的位置曲线。

图21是室外视觉编队实验中X方向的跟踪误差与视觉解算的相对位置曲线。

图22是室外视觉编队实验中无人机在Y方向的位置曲线。

图23是室外视觉编队实验中Y方向的跟踪误差与视觉解算的相对位置曲线。

具体实施方式

克服现有定位方法的缺陷，针对多无人机在无GPS环境下的定位与编队问题开展研究。本发明采用的技术方案是，多无人机基于视觉的相对定位和编队方法，步骤如下：首先建立相机成像和运动的数学模型。如图1所示的针孔相机模型，设O-x-y-z为相机坐标系，z轴指向相机前方，x轴向右，y轴向下。O为相机的光心，O′-x′-y′为成像平面。物理世界的点P,经过小孔O投影之后的成像点为P′。P点坐标为[X,Y,Z]与P′点坐标[X′,Y′,Z′]之间有如下关系：

其中f为相机的焦距。定义像素坐标系o-u-v，可以认为在像素坐标系与成像平面之间存在一个平面的缩放和原点的平移变化，假设u轴缩放的倍数为α，v轴的缩放倍数为β，原点的平移量为[c_x,c_y]^T，则P′点的像素坐标可以表示为：

令f_x＝fα,f_y＝fβ,得到：

其中f的单位为米，α,β的单位为像素/米，f_x,f_y的单位为像素，将上式写成矩阵形式：

上式等号右边的矩阵为相机的内参数矩阵，在使用过程中不会发生变化，但实际使用之前需要对相机进行标定。本文使用张正友标定法进行相机标定，标定结果为:f_x＝1125.0000像素，f_y＝31.4000像素，c_x＝317.1000像素，c_y＝127.3000像素。

由于相机在运动，相机平面的运动在世界坐标系下由旋转矩阵R和平移向量t表示。R与t被称为相机的外参数，这也是视觉定位算法需要解算的目标。设计二维码标签识别与视觉定位的算法步骤如下：

1)检测线段

在获得由相机捕获到的图像之后，首先对图片进行预处理，先对图片进行灰度化处理，再用高斯模糊方法处理灰度图，以平衡图像输入时的噪声，降低目标误检测的概率。然后计算图像中每个像素点的梯度，将每个像素点的梯度幅值按照给定的阈值进行划分，从而检测出边缘点。使用最小生成树聚类算法，将划分得到的边缘点聚类。完成了图像中所有点的聚类之后，就可以使用加权最小二乘法来拟合直线的方程，从而检测出图像中的线段。

2)四边形识别

检测出图像中的线段之后，接下来要判断哪些线段是二维码标签所在区域的边缘。首先对所有线段进行分组，将按逆时针方向连接，且连接处两个端点的距离小于给定阈值的线段分在同一组。对所有分组，选择起始线段，搜索到线段末端时，按照逆时针方向寻找距离足够小的下一条线段。四次搜索后，若最后一条边与第一条边构成一个闭环，则认为该分组可以组成一个四边形，符合要求，如图2所示。

3)标签解码与匹配

第2)步可能检测出多个四边形，或者检测出的四边形可能并不是我们需要的标签，所以需要对得到的四边形进行编码匹配和检查。在标签中，四边形是以四个顶点的形式进行存储的，根据这四个点可以确定其四边形内部点阵的坐标，如图3所示。

首先提取点阵最外围的像素点的平均灰度值v₁，再提取次外围像素点的平均灰度值v₂。由于标签本身最外一圈所有点的灰度都是黑色的，而次外圈的灰度既有黑色也有白色，因此在同一光照环境下，v₁与v₂的值相差较大，可以选出分割效果明显的阈值。设定区阈值为M＝(v₁+v₂)/2，遍历点阵内所有点，灰度高于阈值的像素点编码为0，而低于阈值的像素点编码为1，由此得到了一串二进制编码。最后将得到的二进制编码在已知的编码库中进行匹配，并进一步识别出标签的ID。

4)求解单应矩阵(Homography Matrix)

单应矩阵描述了物理世界中同一个点在两个不同的成像平面上的坐标之间的变换关系。通过求解单应矩阵，可以估计出相机的外参数。利用第2)步检测出来的标签的四个顶点与第3)步匹配到的库中标签的四个顶点，组成四对匹配的特征点，通过直接线性变换法(Direct Linear Transform,DLT)求解单应矩阵，并进一步将单应矩阵分解成摄像头运动的旋转矩阵R和平移向量t。

假设四个特征点p₁～p₄在世界坐标系下的坐标分别为(x_wi，y_wi，z_wi),在相机坐标系下的坐标分别为(x_ci，y_ci，z_ci)，对于到成像平面上的坐标为(x_i，y_i)。另外定义标签坐标系，以标签对称中心为坐标原点O_t，以标签平面为x_t-O_t-y_t平面，平行与正方向标签上下边沿水平向右为x_t轴，垂直x_t竖直向上为y_t轴，由右手法则得到z_t轴。则特征点在标签坐标系下的坐标为(x_ti，y_ti，0),根据坐标系变换关系，可以得到：

其中

为标签坐标系相对与相机坐标系的旋转矩阵，

为标签坐标系原点在相机坐标系下的三维位置坐标，其三个坐标系上的分量分别为

展开得到：

其中

分别表示标签坐标系x_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，同理，

别表示标签坐标系y_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，

分别表示标签坐标系z_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影。由公式(4)可得

将式(6)代入式(7)中得到：

对于4个特征点，存在8个方程，共8个未知数，可以求解。标签识别与定位算法的流程图如图4所示。

接下来分析无人机受到的相机视场约束。在光学仪器中，以光学仪器的镜头为起点，以被测目标的物像可通过镜头的最大范围的两条边缘构成的夹角，称为视场角，如图5所示。视场角的大小决定了光学仪器的视场范围，视场角越大，视场就越大，光学倍率就越小。通俗地说，目标物体超过这个角就不会被收在镜头里。

对于直线投射影像，视角w的计算公式如下：

其中f为相机的有效焦距，d为相机内部图像传感器的大小在一个方向的计算。类似地，对于已知视场角为w的相机，视场中的物体的位置满足以下约束：

对于无人机编队而言，由于无人机的机动性大，后面的无人机通过捕捉前面无人机的标签实现跟踪的过程中，标签很容易会脱离相机的视场，从而导致编队的失败。针对这个问题，本文设计一种编队控制算法，将相机的视场约束融合进来，使无人机快速、稳定地跟踪标签。

为方便对四旋翼无人机的动力学模型进行数学描述，本文采用NED坐标系进行建模描述。

基于牛顿欧拉方程(Newton-Euler equation)，四旋翼无人机的运动学及动力学模型可以由下面的公式表示：

其中ξ＝[x,y,z]^T为惯性坐标系的三维位置向量，V无人机在惯性坐标系下的三维线速度向量，R∈SO(3)表示从机体坐标系到惯性坐标系的旋转矩阵，m表示无人的质量，Ω为无人机在机体坐标系下的角速度向量，g为重力加速度，J为无人机惯量矩阵，

代表机体坐标系下无人机的力矩输入，f为垂直于机体平面的总推力，e₃＝[0,0,1]^T为单位向量,

为未知扰动。无人机的模型简化为

其中

分别为机体坐标系下无人机的位置和速度。

在外环中分别对无人机的位置和速度进行PID(Proportional IntegralDerivative)控制，设计的外环控制器结构如下式：

其中

为期望的相对位置输入,K_pv、K_iv、K_dv分别为速度环的P、I、D控制器参数，K_px、K_ix、K_dx分别为位置环的P、I、D控制器参数。视觉跟踪控制器的结构框图如图6所示。

验证步骤具体是，进行室外无人机视觉跟踪实验，测试控制器对多无人机编队的控制性能。同时将定位数据与差分GPS记录的值进行对比，验证视觉定位算法的精度和可靠性。

下面结合附图和具体实例进一步详细说明本发明。

本发明涉及多无人机基于视觉的相对定位和编队技术。针对在GPS拒止环境下多无人机的定位与编队控制问题，提出一种基于单目视觉的多无人机相对定位及编队控制方法。

克服现有定位方法的缺陷，针对多无人机在无GPS环境下的定位与编队问题开展研究。本发明采用的技术方案是，多无人机基于视觉的相对定位和编队方法，步骤如下：首先建立相机成像和运动的数学模型。如图1所示的针孔相机模型，设O-x-y-z为相机坐标系，z轴指向相机前方，x轴向右，y轴向下。O为相机的光心，O′-x′-y′为成像平面。物理世界的点P,经过小孔O投影之后的成像点为P′。P点坐标为[X,Y,Z]与P′点坐标[X′,Y′,Z′]之间有如下关系：

其中f为相机的焦距。定义像素坐标系o-u-v，可以认为在像素坐标系与成像平面之间存在一个平面的缩放和原点的平移变化，假设u轴缩放的倍数为α，v轴的缩放倍数为β，原点的平移量为[c_x,c_y]^T，则P′点的像素坐标可以表示为：

令f_x＝fα,f_y＝fβ,得到：

其中f的单位为米，α,β的单位为像素/米，f_x,f_y的单位为像素，将上式写成矩阵形式：

上式等号右边的矩阵为相机的内参数矩阵，在使用过程中不会发生变化，但实际使用之前需要对相机进行标定。本文使用张正友标定法进行相机标定，标定结果为:f_x＝1125.0000像素，f_y＝31.4000像素，c_x＝317.1000像素，c_y＝127.3000像素。

由于相机在运动，相机平面的运动在世界坐标系下由旋转矩阵R和平移向量t表示。R与t被称为相机的外参数，这也是视觉定位算法需要解算的目标。设计二维码标签识别与视觉定位的算法步骤如下：

1)检测线段

在获得由相机捕获到的图像之后，首先对图片进行预处理，先对图片进行灰度化处理，再用高斯模糊方法处理灰度图，以平衡图像输入时的噪声，降低目标误检测的概率。然后计算图像中每个像素点的梯度，将每个像素点的梯度幅值按照给定的阈值进行划分，从而检测出边缘点。使用最小生成树聚类算法，将划分得到的边缘点聚类。完成了图像中所有点的聚类之后，就可以使用加权最小二乘法来拟合直线的方程，从而检测出图像中的线段。

2)四边形识别

检测出图像中的线段之后，接下来要判断哪些线段是二维码标签所在区域的边缘。首先对所有线段进行分组，将按逆时针方向连接，且连接处两个端点的距离小于给定阈值的线段分在同一组。对所有分组，选择起始线段，搜索到线段末端时，按照逆时针方向寻找距离足够小的下一条线段。四次搜索后，若最后一条边与第一条边构成一个闭环，则认为该分组可以组成一个四边形，符合要求，如图2所示。

3)标签解码与匹配

第2)步可能检测出多个四边形，或者检测出的四边形可能并不是我们需要的标签，所以需要对得到的四边形进行编码匹配和检查。在标签中，四边形是以四个顶点的形式进行存储的，根据这四个点可以确定其四边形内部点阵的坐标，如图3所示。

首先提取点阵最外围的像素点的平均灰度值v₁，再提取次外围像素点的平均灰度值v₂。由于标签本身最外一圈所有点的灰度都是黑色的，而次外圈的灰度既有黑色也有白色，因此在同一光照环境下，v₁与v₂的值相差较大，可以选出分割效果明显的阈值。设定区阈值为M＝(v₁+v₂)/2，遍历点阵内所有点，灰度高于阈值的像素点编码为0，而低于阈值的像素点编码为1，由此得到了一串二进制编码。最后将得到的二进制编码在已知的编码库中进行匹配，并进一步识别出标签的ID。

4)求解单应矩阵(Homography Matrix)

单应矩阵描述了物理世界中同一个点在两个不同的成像平面上的坐标之间的变换关系。通过求解单应矩阵，可以估计出相机的外参数。利用第2)步检测出来的标签的四个顶点与第3)步匹配到的库中标签的四个顶点，组成四对匹配的特征点，通过直接线性变换法(Direct Linear Transform,DLT)求解单应矩阵，并进一步将单应矩阵分解成摄像头运动的旋转矩阵R和平移向量t。

假设四个特征点p₁～p₄在世界坐标系下的坐标分别为(x_wi，y_wi，z_wi),在相机坐标系下的坐标分别为(x_ci，y_ci，z_ci)，对于到成像平面上的坐标为(x_i，y_i)。另外定义标签坐标系，以标签对称中心为坐标原点O_t，以标签平面为x_t-O_t-y_t平面，平行与正方向标签上下边沿水平向右为x_t轴，垂直x_t竖直向上为y_t轴，由右手法则得到z_t轴。则特征点在标签坐标系下的坐标为(x_ti，y_ti，0),根据坐标系变换关系，可以得到：

其中

为标签坐标系相对与相机坐标系的旋转矩阵，

为标签坐标系原点在相机坐标系下的三维位置坐标，其三个坐标系上的分量分别为

展开得到：

其中

分别表示标签坐标系x_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，同理，

分别表示标签坐标系y_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，

分别表示标签坐标系z_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影。由公式(4)可得

将式(6)代入式(7)中得到：

对于4个特征点，存在8个方程，共8个未知数，可以求解。标签识别与定位算法的流程图如图4所示。

接下来分析无人机受到的相机视场约束。在光学仪器中，以光学仪器的镜头为起点，以被测目标的物像可通过镜头的最大范围的两条边缘构成的夹角，称为视场角，如图5所示。视场角的大小决定了光学仪器的视场范围，视场角越大，视场就越大，光学倍率就越小。通俗地说，目标物体超过这个角就不会被收在镜头里。

对于直线投射影像，视角w的计算公式如下：

其中f为相机的有效焦距，d为相机内部图像传感器的大小在一个方向的计算。类似地，对于已知视场角为w的相机，视场中的物体的位置满足以下约束：

对于无人机编队而言，由于无人机的机动性大，后面的无人机通过捕捉前面无人机的标签实现跟踪的过程中，标签很容易会脱离相机的视场，从而导致编队的失败。针对这个问题，本文设计一种编队控制算法，将相机的视场约束融合进来，使无人机快速、稳定地跟踪标签。

为方便对四旋翼无人机的动力学模型进行数学描述，本文采用NED坐标系进行建模描述。

基于牛顿欧拉方程(Newton-Euler equation)，四旋翼无人机的运动学及动力学模型可以由下面的公式表示：

其中ξ＝[x,y,z]^T为惯性坐标系的三维位置向量，V无人机在惯性坐标系下的三维线速度向量，R∈SO(3)表示从机体坐标系到惯性坐标系的旋转矩阵，m表示无人的质量，Ω为无人机在机体坐标系下的角速度向量，g为重力加速度，J为无人机惯量矩阵，

代表机体坐标系下无人机的力矩输入，f为垂直于机体平面的总推力，e₃＝[0,0,1]^T为单位向量,

为未知扰动。无人机的模型简化为

其中

分别为机体坐标系下无人机的位置和速度。

在外环中分别对无人机的位置和速度进行PID(Proportional IntegralDerivative)控制，设计的外环控制器结构如下式：

其中

为期望的相对位置输入,K_pv、K_iv、K_dv分别为速度环的P、I、D控制器参数，K_px、K_ix、K_dx分别为位置环的P、I、D控制器参数。视觉跟踪控制器的结构框图如图6所示。

验证步骤具体是，进行室外无人机视觉跟踪实验，测试控制器对多无人机编队的控制性能。同时将定位数据与差分GPS记录的值进行对比，验证视觉定位算法的精度和可靠性。

一、实验平台简介

实验平台如图7所示。该实验平台采用三架四旋翼无人机进行编队，每架无人机的硬件框架如图8、9所示。使用Pixhawk飞行控制器进行姿态控制，使用光流模块配合单目摄像头进行相对定位，在机载ARM计算板上，设计并实现了标签识别与定位、飞行控制器外部程序接口、以及无人机视觉跟踪控制器，如图10所示。

二、基于视觉的无人机定位实验

为了验证视觉定位算法的精度，本实验使用一架无人机进行测试。将摄像头安装在无人机的底板下方，镜头朝下，并在地面上贴上了尺寸为11.5厘米×11.5厘米的标签，无人机通过捕捉与标签的相对位置进行定点悬停，如图11所示。

实验流程为：0-5秒时，首先由操作人员通过遥控器操作无人机飞行到指定高度附近；5-15秒，此时切换至定高模式，无人机开始稳定在目标高度，并捕获到视野中的标签；15-100秒，通过遥控器切换至定点模式，无人机开始通过摄像头捕获标签解算出来的数据进行定点悬停；100秒之后，切换遥控模式，由操作人员控制无人机降落。

为了对定位算法进行准确的分析和说明，在无人机上安装定位球，通过运动捕捉系统(OptiTrack)记录无人机的飞行轨迹，将其作为真值与摄像头的估计数据进行对比。实验数据如图12、13所示。

在高度方向上，使用激光测距仪测量高度并使无人机维持在距离地面1.5米的高度，水平面方向上通过摄像头的数据进行定点，故此处给出在X-Y方向上OptiTrack与摄像头测量的数据曲线图。图12为X方向的位置曲线，图13为Y方向的位置曲线，其中红线表示通过摄像头算法解算出来的位置，绿线代表由OptiTrack采集的数据。

在0-15秒时。无人机处于起飞和调整阶段，摄像头不能稳定地捕捉到完整地标签，因而测量地数据误差较大。在无人机的稳定状态下，由图12可以看出，本文使用的标签定位方法在X方向的测量值与真值的误差在0.15米以内。同理由图13可以看出，标签识别与定位算法在Y方向上与真值的误差在0.25米以内。

从实验数据来看，本文采用的基于标签识别与定位算法的实时性比较理想，相对于运动捕捉系统而言，其定位数据的延迟时间约为0.9秒，用于无人机定位飞行的效果较好。

三、基于视觉的无人机标签跟踪试验

为了验证视觉跟踪控制算法的有效性，进行室外无人机跟踪标签飞行的试验。如图14所示，在无人机的前方(机头方向)安装摄像头，镜头朝前。操作人员手持标签走在无人机前方，无人机通过跟踪标签，与标签保持固定的期望距离0.6米(X方向)。在本试验中，使用激光测距仪测量高度，并保持高度固定不变。

为了记录无人机和标签的实际轨迹，在无人机和标签上分别安装差分GPS的移动端，并记录差分GPS的数据，变换成以无人机起飞点为原点的局部坐标数据。实验数据如图15、16、17、18所示。

图15、17分别为差分GPS测得的X方向和Y方向的位置曲线，其中红线表示手持标签的实际位置，绿线表示无人机的实际位置。图16、18分别为X方向和Y方向的位置误差曲线，其中红线表示由差分GPS测得的标靶及无人机之间相对位置与期望值的误差，绿线表示由无人机上的摄像头捕获标签之后解算出来的相对位置数据与期望值的误差。从图15、17可以看出，无人机通过视觉跟踪手持标签的X方向的误差在1.2米以内，Y方向的误差在1.5米以内，视觉跟踪的效果较好。在无人机跟踪标签过程中，标签的位置始终维持在摄像头的视场内，证明了第二章设计的视觉跟踪控制算法的有效性。

四、基于视觉跟踪的多无人机编队实验

在验证定位精度和视觉跟踪控制算法的可靠性之后，如图19所示，将标签固定在Leader无人机的后方，将摄像头安置在Follower无人机的机头方向，无人机之间保持前后(X方向)1.5米的距离。通过跟踪带标签的无人机实现多机编队，以此来验证第二章设计的基于视觉相对定位的多无人机编队系统的可行性和效果。在本实验中，Leader无人机由实验人员通过遥控器进行操控，为了提高Leader飞行过程的稳定性，将飞行模式切换至定点模式，通过激光测距仪保持高度，使用光流传感器进行水平面方向的速度控制。试验结果如图20、21、22、23所示。

图20、22分别为差分GPS测得的X方向和Y方向的位置曲线，其中红线表示装有标签的Leader无人机的实际位置，绿线表示装有摄像头的Follower无人机的实际位置。图21、23分别为X方向和Y方向的位置误差曲线，其中红线表示由差分GPS测得的两个无人机之间的位置相对值于期望值的误差，绿线表示由无人机上的摄像头捕获标签之后解算出来的相对位置数据与期望值的误差。

从图20、22可以看出，Follower无人机通过视觉跟踪Leader的X方向的误差在1.2米以内，Y方向的误差在0.9米以内,编队效果较好。同时，在视觉跟踪的过程中，无人机的标签始终保持在摄像头的视场范围之内。试验结果验证了本文设计的多无人机编队基于视觉的相对定位方法的有效性和设计的视觉跟踪控制算法的可行性。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于视觉相对定位的多无人机编队方法，其特征是，步骤如下：

首先建立相机成像和运动的数学模型-针孔相机模型，设O-x-y-z为相机坐标系，z轴指向相机前方，x轴向右，y轴向下，O为相机的光心，O′-x′-y′为成像平面，物理世界的点P,经过小孔O投影之后的成像点为P′，P点坐标为[X,Y,Z]与P′点坐标[X′,Y′,Z′]之间有如下关系：

其中f为相机的焦距，定义像素坐标系o-u-v，在像素坐标系与成像平面之间存在一个平面的缩放和原点的平移变化，u轴缩放的倍数为α，v轴的缩放倍数为β，原点的平移量为[c_x,c_y]^T，则P′点的像素坐标可以表示为：

令f_x＝fα,f_y＝fβ,得到：

其中f的单位为米，α,β的单位为像素/米，f_x,f_y的单位为像素，将上式写成矩阵形式：

上式等号右边的矩阵为相机的内参数矩阵，在使用过程中不会发生变化，但实际使用之前需要对相机进行标定；

由于相机在运动，相机平面的运动在世界坐标系下由旋转矩阵R和平移向量t表示，R与t被称为相机的外参数，这也是视觉定位算法需要解算的目标；设计二维码标签识别与视觉定位的具体步骤如下：

1)检测线段

在获得由相机捕获到的图像之后，首先对图片进行预处理，先对图片进行灰度化处理，再用高斯模糊方法处理灰度图，以平衡图像输入时的噪声，降低目标误检测的概率，然后计算图像中每个像素点的梯度，将每个像素点的梯度幅值按照给定的阈值进行划分，从而检测出边缘点，使用最小生成树聚类算法，将划分得到的边缘点聚类，完成图像中所有点的聚类之后，使用加权最小二乘法来拟合直线的方程，从而检测出图像中的线段；

2)四边形识别

检测出图像中的线段之后，接下来要判断哪些线段是二维码标签所在区域的边缘，首先对所有线段进行分组，将按逆时针方向连接，且连接处两个端点的距离小于给定阈值的线段分在同一组，对所有分组，选择起始线段，搜索到线段末端时，按照逆时针方向寻找距离足够小的下一条线段，四次搜索后，若最后一条边与第一条边构成一个闭环，则认为该分组可以组成一个四边形，符合要求；

3)标签解码与匹配

第2)步可能检测出多个四边形，或者检测出的四边形可能并不是需要的标签，所以需要对得到的四边形进行编码匹配和检查，在标签中，四边形是以四个顶点的形式进行存储的，根据这四个点可以确定其四边形内部点阵的坐标；

首先提取点阵最外围的像素点的平均灰度值v₁，再提取次外围像素点的平均灰度值v₂，由于标签本身最外一圈所有点的灰度都是黑色的，而次外圈的灰度既有黑色也有白色，因此在同一光照环境下，v₁与v₂的值相差较大，选出分割效果明显的阈值，设定区阈值为M＝(v₁+v₂)/2，遍历点阵内所有点，灰度高于阈值的像素点编码为0，而低于阈值的像素点编码为1，由此得到了一串二进制编码；最后将得到的二进制编码在已知的编码库中进行匹配，并进一步识别出标签的ID；

4)求解单应矩阵(Homography Matrix)

单应矩阵描述了物理世界中同一个点在两个不同的成像平面上的坐标之间的变换关系，通过求解单应矩阵，估计出相机的外参数；利用第2)步检测出来的标签的四个顶点与第3)步匹配到的库中标签的四个顶点，组成四对匹配的特征点，通过直接线性变换法DLT(Direct Linear Transform)求解单应矩阵，并进一步将单应矩阵分解成摄像头运动的旋转矩阵R和平移向量t；

四个特征点p₁～p₄在世界坐标系下的坐标分别为(x_wi，y_wi，z_wi),在相机坐标系下的坐标分别为(x_ci，y_ci，z_ci)，对于到成像平面上的坐标为(x_i，y_i)，另外定义标签坐标系，以标签对称中心为坐标原点O_t，以标签平面为x_t-O_t-y_t平面，平行与正方向标签上下边沿水平向右为x_t轴，垂直x_t竖直向上为y_t轴，由右手法则得到z_t轴，则特征点在标签坐标系下的坐标为(x_ti，y_ti，0),根据坐标系变换关系，得到：

其中

为标签坐标系相对与相机坐标系的旋转矩阵，

为标签坐标系原点在相机坐标系下的三维位置坐标，其三个坐标系上的分量分别为

展开得到：

其中

分别表示标签坐标系x_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，同理，

分别表示标签坐标系y_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，

分别表示标签坐标系z_t轴上的单位向量在相机坐标系x_c、y_c轴上的投影，由公式(4)得

将式(6)代入式(7)中得到：

对于4个特征点，存在8个方程，共8个未知数，进行求解；

接下来分析无人机受到的相机视场约束，在光学仪器中，以光学仪器的镜头为起点，以被测目标的物像可通过镜头的最大范围的两条边缘构成的夹角，称为视场角，对于直线投射影像，视角w的计算公式如下：

其中，d为相机内部图像传感器的大小在一个方向的计算，对于已知视场角为w的相机，视场中的物体的位置满足以下约束：

设计一种编队控制算法，将相机的视场约束融合进来，使无人机快速、稳定地跟踪标签：

为方便对四旋翼无人机的动力学模型进行数学描述，采用北东地(North East Down,NED)坐标系进行建模描述；

基于牛顿欧拉方程(Newton-Euler equation)，四旋翼无人机的运动学及动力学模型由下面的公式表示：

其中ξ＝[x,y,z]^T为惯性坐标系的三维位置向量，V无人机在惯性坐标系下的三维线速度向量，R∈SO(3)表示从机体坐标系到惯性坐标系的旋转矩阵，m表示无人的质量，Ω为无人机在机体坐标系下的角速度向量，g为重力加速度，J为无人机惯量矩阵，

代表机体坐标系下无人机的力矩输入，F为垂直于机体平面的总推力，e₃＝[0,0,1]^T为单位向量,

为未知扰动，无人机的模型简化为

其中

分别为机体坐标系下无人机的位置和速度；

在外环中分别对无人机的位置和速度进行PID(Proportional Integral Derivative)控制，设计的外环控制器结构如下式：

其中

为期望的相对位置输入,K_pv、K_iv、K_dv分别为速度环的P、I、D控制器参数，K_px、K_ix、K_dx分别为位置环的P、I、D控制器参数。

Images