CN109360240B - 一种基于双目视觉的小型无人机定位方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉的小型无人机定位方法，包括以下步骤：首先以某个地面目标为参照物，将目标质心选定为导航坐标系的原点，机上固定的双目摄像头基于V4L2接口实时采集包含目标的图像，通过基于颜色模型设计的目标检测算法和人工神经网络算法去除图像中的干扰区域，精确提取左右视图中的目标区域，两幅图中目标质心像素宽度之差即为该目标点的视差，结合双目测距和相机标定参数计算出目标点在左摄像机坐标系下的位置，再结合IMU得到的姿态角信息计算出当前机体坐标系到初始时刻机体坐标系旋转矩阵，从而解算出无人机在导航坐标系下的坐标。所述方法有效缩短了无人机定位时间，有利于飞机位置的实时处理。

Description

一种基于双目视觉的小型无人机定位方法

技术领域

本发明涉及双目视觉及无人机定位技术领域，具体涉及一种基于双目视觉的小型无人机定位方法。

背景技术

目前无人机在快递运输、灾难救援、测绘、航拍等领域应用十分广泛，在今后很长一段时间内，无人机仍会是十分炙热的研究课题，它将进一步改变人们今后的工作和生活。在无人机执行几乎所有的工作时，例如火灾检测、测绘、运输等，都需要实时获得它的定位信息，以便后续处理。因此关于无人机领域的导航定位研究十分必要。

当前无人机的定位系统大多是基于GPS而设计的，可在室外GPS信号不好及室内无GPS信号的环境下无人机的定位变得十分困难。而双目立体视觉是一门有着广阔应用前景的学科，上世纪80年代初，Marr首次将图像处理、心理物理学、神经生理学和临床精神病学的研究成果从信息处理的角度进行概括，创立了视觉计算理论框架。这一基本理论对立体视觉技术的发展产生了极大的推动作用，在这一领域已形成了从图像的获取到最终的三维场景可视表面重构的完整体系，使得立体视觉已成为计算机视觉中一个非常重要的分支。双目立体视觉模仿人的双眼感知图像，依据视差获取深度信息，成为工业检测、生物医学、虚拟现实等领域的关键技术，在国外双目立体视觉技术已广泛应用于生产、生活中。双目立体视觉测量方法具有效率高、精度合适、系统结构简单、成本低等优点，非常适合于制造现场的在线、非接触产品检测和质量控制。对运动物体(包括动物和人体形体)测量中，由于图像获取是在瞬间完成的，因此立体视觉方法是一种更有效的测量方法。双目立体视觉系统是计算机视觉的关键技术之一，获取空间三维场景的距离信息也是计算机视觉研究中最基础的内容。经过几十年来的发展，立体视觉在机器人视觉、航空测绘、反求工程、军事运用、医学成像和工业检测等领域中的运用越来越广，因此双目视觉被用到无人机领域实现自主定位很有必要，也可解决GPS带来的局限。

发明内容

本发明的目的是针对现有技术的不足，为克服GPS在无人机导航定位中的缺陷，发挥双目视觉在无人机定位领域的优势，提供了一种基于双目视觉的小型无人机定位方法，所述方法以某个地面目标物质心作为导航坐标系的原点，无人机上固定的双目摄像头基于V4L2接口实时采集包含目标的图像，通过设计目标检测算法，人工神经网络算法去除图像中的干扰区域，精确提取校正后的左右图像中的目标区域，通过计算左右视图中目标质心像素宽度差的绝对值获得目标点的视差，从而得到目标点相对于左相机坐标系的位置信息，通过IMU得出的姿态角计算出无人机当前时刻机体坐标系到导航坐标系的旋转矩阵，则无人机在导航坐标系下的坐标便可通过旋转矩阵计算出来。

本发明的目的可以通过如下技术方案实现：

一种基于双目视觉的小型无人机定位方法，所述方法包括以下步骤：

S1、小型无人机的双目视觉定位系统初始化以及开启地面站系统；

S2、树莓派固定于无人机上，其上CSI接口分别接通两个树莓派摄像头，使左摄像头固定于飞机重心正下方，并让各个时刻机体坐标系和左相机坐标系三轴同向，地面站通过ssh远程登录树莓派执行图片采集程序，双目通过v4l2接口采集左右图像，选取目标物，并将其质心设为导航坐标系原点，x和y轴与初始位置左摄像机坐标系x和y轴同向，z轴与初始位置左摄像机坐标系z轴反向；

S3、根据所选目标物的颜色模型分析RGB值的特征，在校正后的左右视图中通过该特征找出目标区域及其干扰区域，并求出各区域的圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量；

S4、根据样本实验中采集的多幅图片的多个目标区域和干扰区域的特征向量数据，离线训练BP人工神经网络，得到分类器，得到分类器，记录训练好的权重和偏置参数，并将此分类器用于步骤S3中在线目标区域和干扰区域的分类；实验中选择sigmoid函数作为BP人工神经网络隐藏层的激活函数，则选择输出值最接近1的区域为目标区域，并分别求出左右视图该区域的质心；将左右视图质心像素宽度相减取绝对值，即得到目标点视差；

S5、根据质心视差结合相机标定数据和双目测距原理求出目标质心在左摄像机坐标系下的位置信息，再结合左相机相对飞机重心的相对位置得到目标质心在机体坐标系下的位置信息；

S6、根据ADIS16350和电子罗盘传感器得到的姿态角信息求出当前时刻机体坐标系到导航坐标系的旋转矩阵；通过旋转矩阵结合此时目标质心在机体坐标系下的位置信息解算出当前时刻飞机在导航坐标系下的位置；

S7、将各个时刻飞机在导航坐标系下的位置基于UDP协议通过socket发送给机载系统和地面站，地面站实时跟踪飞机各时刻位置，将各时刻定位信息保存于MySQL数据库中，便于数据查询和管理。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明提供的基于双目视觉的小型无人机定位方法，实现了以树莓派核心板作为图像采集、处理和计算的平台，在处理速度上有较好效果，并通过V4L2接口采集双目图片，在图片同步上比通过OPenCV函数库采集有所改进，在目标检测和提取上运用颜色模型提取可疑区域，然后根据圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量训练BP神经网络排除干扰区域，另外本发明并未使用传统立体匹配算法，而是分别对校正后的左右视图提取目标质心，它们像素宽度相减的绝对值即为目标点视差，在处理时间上有所缩短，有利于飞机的实时处理。

2、本发明提供的基于双目视觉的小型无人机定位方法，将导航坐标系原点设在目标质心，其x、y轴与初始位置机体坐标系x、y轴同向，z轴与初始位置机体坐标系z轴反向，通过树莓派核心板上惯性测量单元获取的姿态角计算出当前时刻机体坐标系到初始时刻机体坐标系的旋转矩阵，通过坐标转换关系便可解算出各个时刻飞机在导航坐标系下的位置，方便对飞机位置进行跟踪。

附图说明

图1为本发明实施例中基于双目视觉的小型无人机定位方法的硬件系统结构框图。

图2为本发明实施例中基于双目视觉的小型无人机定位方法的流程图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

如图1所示，是本实施例提供的基于双目视觉的小型无人机定位方法的硬件系统结构框图，机载系统是以ARM7为主控制器，飞机正常工作的处理过程都是基于ARM7而实现，而图像采集、处理和计算的平台是基于树莓派CM3，其上两个CSI接口连接的双目采集平台用于采集双目图像，惯性测量单元用于解算飞机各个时刻的姿态角和机体坐标相对初始时刻位置的旋转矩阵，机载系统和树莓派CM3核心板都是通过socket编程与地面站通信，地面站可以实时接受各个时刻飞机在导航坐标系下的位置。

本实施例还提供了一种基于双目视觉的小型无人机定位方法，所述方法的流程图如图2所示，包括以下步骤：

S1、小型无人机的双目视觉定位系统初始化以及开启地面站系统；

S2、树莓派固定于无人机上，其上CSI接口分别接通两个树莓派摄像头，使左摄像头固定于飞机重心正下方，并让各个时刻机体坐标系和左相机坐标系三轴同向，地面站通过ssh远程登录树莓派执行图片采集程序，双目通过v4l2接口采集左右图像，选取目标物，并将其质心设为导航坐标系原点，x和y轴与初始位置左摄像机坐标系x和y轴同向，z轴与初始位置左摄像机坐标系z轴反向；

S3、根据所选目标物的颜色模型分析RGB值的特征，在校正后的左右视图中通过该特征找出目标区域及其干扰区域，并求出各区域的圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量；

S4、根据样本实验中采集的多幅图片的多个目标区域和干扰区域的特征向量数据，离线训练BP人工神经网络，得到分类器，记录训练好的权重和偏置参数，并将此分类器用于步骤S3中在线目标区域和干扰区域的分类；实验中选择sigmoid函数作为BP人工神经网络隐藏层的激活函数，则选择输出值最接近1的区域为目标区域，并分别求出左右视图该区域的质心；将左右视图质心像素宽度相减取绝对值，即得到目标点视差；

S5、根据质心视差结合相机标定数据和双目测距原理求出目标质心在左摄像机坐标系下的位置信息，再结合左相机相对飞机重心的相对位置得到目标质心在机体坐标系下的位置信息；

S6、根据ADIS16350和电子罗盘传感器得到的姿态角信息求出当前时刻机体坐标系到导航坐标系的旋转矩阵；通过旋转矩阵结合此时目标质心在机体坐标系下的位置信息解算出当前时刻飞机在导航坐标系下的位置；

S7、将各个时刻飞机在导航坐标系下的位置基于UDP协议通过socket发送给机载系统和地面站，地面站实时跟踪飞机各时刻位置，将各时刻定位信息保存于MySQL数据库中，便于数据查询和管理。

进一步地，所述定位方法通过树莓派核心板上的摄像头获得双目图片和惯性测量单元获得三轴姿态角，并将树莓派核心板作为图像处理和计算定位信息的平台。

进一步地，所述步骤S3中应尽量选取颜色模型与背景颜色模型差异大的目标区域，分析该区域颜色模型，即RGB各自取值及其相互关系，通过找出的模型提取出目标区域和一些干扰区域。

进一步地，步骤S3中所述各区域的圆形度指度量区域轮廓与圆形接近程度的特征，计算公式如下：

x₁＝4π*S/L²

其中，S为区域的面积，L为区域周长；

所述灰度共生矩阵指通过研究灰度的空间相关特性来描述纹理的特征，取图像(M×N)中任意一点(x，y)及偏离它的另一点(x+m，y+n)，设该点对的灰度值为(g1，g2)，令点(x，y)在整个画面上移动，则会得到各种(g1，g2)值，设灰度值的级数为k，则(g1，g2)的组合共有k*k种；对于整个画面，统计出每一种(g1，g2)值出现的次数，然后排列成一个方阵，再用(g1，g2)出现的总次数将它们归一化为出现的概率P(g1，g2)，这样的方阵称为灰度共生矩阵；

所述灰度共生矩阵的对比度反映图像的清晰度和纹理的沟纹深浅，公式如下所示：

con＝Σ_iΣ_j(i-j)²P(i,j)

其中，i表示灰度共生矩阵的行坐标，j表示灰度共生矩阵的列坐标，P(i,j)表示灰度共生矩阵在第i行第j列的值；

所述灰度共生矩阵的能量是对图像纹理的灰度变换稳定程度的度量，公式如下所示：

asm＝Σ_iΣ_jP(i,j)²

所述灰度共生矩阵的熵是图像包含信息量的随机性度量，公式如下所示：

ent＝-Σ_iΣ_jP(i,j)logP(i,j)。

进一步地，所述步骤S4中通过提取实验样本中的各区域的圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量来离线训练BP人工神经网络，选择sigmoid作为激活函数，交叉熵函数作为代价函数，得到分类器去在线提取左右视图目标区域，进而根据两幅图目标质心像素宽度之差的绝对值得到目标点视差。

进一步地，步骤S5中，使用张正友标定法进行标定，得到内外参数和左右相机旋转矩阵和平移向量及相机畸变系数；使用Bouguet算法对双目摄像头进行极线校正，使两相机图像平面达到同行共面的效果；分别提取左右视图目标点像素位置之差的绝对值来求取目标点的视差，这样既能够避免一些匹配算法的效果不好，又能避免选择较好匹配算法例如sgbm、nlca等算法带来的匹配时间过长，有利于飞机的实时处理，接着便根据双目测距原理和标定参数求出目标质心在左摄像机坐标系下的位置信息。

进一步地，步骤S6中，将双目采集平台左摄像头固定于机体重心正下方，并让各个时刻机体坐标系和左相机坐标系三轴同向，目标质心设为导航坐标系原点，x和y轴与初始位置左摄像机坐标系x和y轴同向，z轴与初始位置左摄像机坐标系z轴反向，通过IMU单元每个时刻得到的姿态角信息能够得到当前时刻机体坐标系到初始时刻机体坐标系的旋转矩阵，通过坐标转换关系解算出各个时刻飞机在导航坐标系下的位置，步骤如下所示：

若横滚角、俯仰角和偏航角分别用φ、θ和

表示，则旋转矩阵

如下所示：

飞机初始位置机体坐标系到当前时刻机体坐标系的旋转矩阵

和当前时刻机体坐标系到初始位置机体坐标系的旋转矩阵

关系为：

故可得

的式子如下所示：

而导航坐标系与初始位置机体坐标系x、y轴同向，z轴反向，故先用三轴都同向的坐标系代替导航坐标系求出结果后z轴值取反即可，设每一时刻机体中心在代替的导航坐标系下的位置为T_nb'，所以机体坐标系到代替的导航坐标系的转换关系如下所示：

其中，N表示目标点在导航坐标系下x轴坐标，E表示目标点在导航坐标系下y轴坐标，D表示目标点在导航坐标系下z轴坐标，x_b表示目标点在机体坐标系下的x轴坐标，y_b表示目标点在机体坐标系下的y轴坐标，z_b表示目标点在机体坐标系下的z轴坐标，左摄像机光心位于机体重心下方h毫米处得到了目标在机体坐标系下的位置为(x_c,y_c,z_c+h)，则能够计算出目标在代替的导航坐标系下的位置，而目标在代替的导航坐标系下的位置为(0,0,0)，故能够得到如下式子：

所以根据上面式子能够得到每一时刻机体重心在代替导航坐标系下的位置为T_nb'，将T_nb'的z轴反向则得到每一时刻机体中心在导航坐标系下的位置T_nb，式子如下所示：

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明专利构思加以等同替换或改变，都属于本发明专利的保护范围。

Claims (7)

1.一种基于双目视觉的小型无人机定位方法，其特征在于，所述方法包括以下步骤：

S1、小型无人机的双目视觉定位系统初始化以及开启地面站系统；

S2、树莓派固定于无人机上，其上CSI接口分别接通两个树莓派摄像头，使左摄像头固定于飞机重心正下方，并让各个时刻机体坐标系和左相机坐标系三轴同向，地面站通过ssh远程登录树莓派执行图片采集程序，双目通过v4l2接口采集左右图像，选取目标物，并将其质心设为导航坐标系原点，x和y轴与初始位置左摄像机坐标系x和y轴同向，z轴与初始位置左摄像机坐标系z轴反向；

S3、根据所选目标物的颜色模型分析RGB值的特征，在校正后的左右视图中通过该特征找出目标区域及其干扰区域，并求出各区域的圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量；

S4、根据样本实验中采集的多幅图片的多个目标区域和干扰区域的特征向量数据，离线训练BP人工神经网络，得到分类器，记录训练好的权重和偏置参数，并将此分类器用于步骤S3中在线目标区域和干扰区域的分类；实验中选择sigmoid函数作为BP人工神经网络隐藏层的激活函数，则选择输出值最接近1的区域为目标区域，并分别求出左右视图该区域的质心；将左右视图质心像素宽度相减取绝对值，即得到目标点视差；

S5、根据质心视差结合相机标定数据和双目测距原理求出目标质心在左摄像机坐标系下的位置信息，再结合左相机相对飞机重心的相对位置得到目标质心在机体坐标系下的位置信息；

S6、根据ADIS16350和电子罗盘传感器得到的姿态角信息求出当前时刻机体坐标系到导航坐标系的旋转矩阵；通过旋转矩阵结合此时目标质心在机体坐标系下的位置信息解算出当前时刻飞机在导航坐标系下的位置；

S7、将各个时刻飞机在导航坐标系下的位置基于UDP协议通过socket发送给机载系统和地面站，地面站实时跟踪飞机各时刻位置，将各时刻定位信息保存于MySQL数据库中，便于数据查询和管理。

2.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：所述定位方法通过树莓派核心板上的摄像头获得双目图片和惯性测量单元获得三轴姿态角，并将树莓派核心板作为图像处理和计算定位信息的平台。

3.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：所述步骤S3中应尽量选取颜色模型与背景颜色模型差异大的目标区域，分析该区域颜色模型，即RGB各自取值及其相互关系，通过找出的模型提取出目标区域和一些干扰区域。

4.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：步骤S3中所述各区域的圆形度指度量区域轮廓与圆形接近程度的特征，计算公式如下：

x₁＝4π*S/L²

其中，S为区域的面积，L为区域周长；

所述灰度共生矩阵指通过研究灰度的空间相关特性来描述纹理的特征，取图像(M×N)中任意一点(x，y)及偏离它的另一点(x+m，y+n)，设该点对的灰度值为(g1，g2)，令点(x，y)在整个画面上移动，则会得到各种(g1，g2)值，设灰度值的级数为k，则(g1，g2)的组合共有k*k种；对于整个画面，统计出每一种(g1，g2)值出现的次数，然后排列成一个方阵，再用(g1，g2)出现的总次数将它们归一化为出现的概率P(g1，g2)，这样的方阵称为灰度共生矩阵；

所述灰度共生矩阵的对比度反映图像的清晰度和纹理的沟纹深浅，公式如下所示：

con＝∑_i∑_j(i-j)²P(i,j)

其中，i表示灰度共生矩阵的行坐标，j表示灰度共生矩阵的列坐标，P(i,j)表示灰度共生矩阵在第i行第j列的值；

所述灰度共生矩阵的能量是对图像纹理的灰度变换稳定程度的度量，公式如下所示：

asm＝∑_i∑_jP(i,j)²

所述灰度共生矩阵的熵是图像包含信息量的随机性度量，公式如下所示：

ent＝-∑_i∑_jP(i,j)logP(i,j)。

5.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：所述步骤S4中通过提取实验样本中的各区域的圆形度、灰度共生矩阵的对比度、能量和熵作为特征向量来离线训练BP人工神经网络，选择sigmoid作为激活函数，交叉熵函数作为代价函数，得到分类器去在线提取左右视图目标区域，进而根据两幅图目标质心像素宽度之差的绝对值得到目标点视差。

6.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：步骤S5中，使用张正友标定法进行标定，得到内外参数和左右相机旋转矩阵和平移向量及相机畸变参数；使用Bouguet算法对双目摄像头进行极线校正，使两相机图像平面达到同行共面的效果；分别提取左右视图目标点像素位置之差的绝对值来求取目标点的视差，接着便根据双目测距原理和标定参数求出目标质心在左摄像机坐标系下的位置信息。

7.根据权利要求1所述的一种基于双目视觉的小型无人机定位方法，其特征在于：步骤S6中，将双目采集平台左摄像头固定于机体重心正下方，并让各个时刻机体坐标系和左相机坐标系三轴同向，目标质心设为导航坐标系原点，x和y轴与初始位置左摄像机坐标系x和y轴同向，z轴与初始位置左摄像机坐标系z轴反向，通过IMU单元每个时刻得到的姿态角信息能够得到当前时刻机体坐标系到初始时刻机体坐标系的旋转矩阵，通过坐标转换关系解算出各个时刻飞机在导航坐标系下的位置，步骤如下所示：

若横滚角、俯仰角和偏航角分别用φ、θ和

表示，则旋转矩阵

如下所示：

飞机初始位置机体坐标系到当前时刻机体坐标系的旋转矩阵

和当前时刻机体坐标系到初始位置机体坐标系的旋转矩阵

关系为：

故可得

的式子如下所示：

而导航坐标系与初始位置机体坐标系x、y轴同向，z轴反向，故先用三轴都同向的坐标系代替导航坐标系求出结果后z轴值取反即可，设每一时刻机体中心在代替的导航坐标系下的位置为T_nb'，所以机体坐标系到代替的导航坐标系的转换关系如下所示：

其中，N表示目标点在导航坐标系下x轴坐标，E表示目标点在导航坐标系下y轴坐标，D表示目标点在导航坐标系下z轴坐标，x_b表示目标点在机体坐标系下的x轴坐标，y_b表示目标点在机体坐标系下的y轴坐标，z_b表示目标点在机体坐标系下的z轴坐标，左摄像机光心位于机体重心下方h毫米处得到了目标在机体坐标系下的位置为(x_c,y_c,z_c+h)，则能够计算出目标在代替的导航坐标系下的位置，而目标在代替的导航坐标系下的位置为(0,0,0)，故能够得到如下式子：

所以根据上面式子能够得到每一时刻机体重心在代替导航坐标系下的位置为T_nb'，将T_nb'的z轴反向则得到每一时刻机体中心在导航坐标系下的位置T_nb，式子如下所示：

Images