CN109785388B - 一种基于双目摄像头的短距离精确相对定位方法 - Google Patents

Abstract

本发明涉及一种基于双目摄像头的短距离精确相对定位方法，能够测量被遮挡界址点的GPS坐标，并能够保证精度，并且引入SLAM方法求解已标定和未标定界址点的相对位姿，从而获得未标定点的GPS坐标，此种方法提高了测量效率，同时本发明将CNN模型引入能够自动判别界址点在图像中的坐标满足了线上实时处理的要求，减轻了后期人为标定界址点的难度。

Description

一种基于双目摄像头的短距离精确相对定位方法

技术领域

本发明涉及一种基于双目摄像头的短距离精确相对定位方法，属于相对定位技术领域。

背景技术

信息化已经成为当今世界不可逆转的发展潮流和趋势，而国土资源支撑着国计民生，国土资源信息化始终是一个战略性和挑战性的重大问题。土地调查过程中完成信息化的工作已经成为我国现代化进程中占有重要的地位，目前我国城镇土地调查主要使用第二次全国土地调查的遥感系统、GPS-RTK系统与惯导组合和光学全站仪。原有的侦测手段效率低，受到局部地形地貌等因素的影响造成部分遮挡严重的界址点获取难度大。

相比于传统的定位方案，SLAM技术拥有较高的精确度和无可比拟的高效率。RTK技术为了保证观测值的精确度，常常需要延长观测时间、选择作业窗口来保证测量数据的可靠性，这样一来就降低了GPS测量的工作效率。遥感收到地表覆盖、天气情况、电磁干扰的影响，无法保证对土地的全采样。光学全站仪作为最为传统的方法，其测量人员需要接受培训，同时对于遮挡严重的环境下需要选取适当的角度对角点进行定标。SLAM技术与上述传统的定位技术不同，其主要依赖于激光和视觉传感器，通过对周围环境的感知，并将其作为后端算法优化的主要手段，进而来确定在空间中的精确位姿。其优势在于能够在较为复杂的环境下进行空间特征点的位姿计算，从而保证不受遮挡环境的影响，满足全采样的要求。激光传感器成本较高同时，高精度的激光传感器体积较大不易携带，单目视觉传感器存在初始化尺度标定的问题，无形为技术方案实现增加了难度，考虑到技术成本和经济成本本系统中选用双目视觉传感器。

在对实际的空间特征点计算的过程中，需要首先提取出目标特征点在图像中的确切像素坐标位置才能进行绝对位置的解算。第一种方法是通过线下处理的方式使用人工对其目标界址点进行图像标定，此种方法只能通过线下进行处理无法实时的得到结果，对于误差较大的点无法做到及时的纠正；第二种方法是在目标界址点处放置特殊形状设计的标定物，通过物体识别分类算法检测出在图像中的位置并进行精确位置的计算。在传统的分类学习算法中例如KNN、PCA、NMF、GMM等算法中，都受到其各自模型的缺陷无法达到百分之百的识别率。

发明内容

本发明所要解决的技术问题是提供一种基于双目摄像头的短距离精确相对定位方法，能够在实际复杂环境中识别出标定物并对其进行定位，高效实现目标区域内的相对定位。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种基于双目摄像头的短距离精确相对定位方法，通过双目摄像头实现目标区域内的相对定位，包括如下步骤：

步骤A.针对CNN模型进行训练，获得用于检测标定物在图像中像素位置的CNN模型，然后进入步骤B；

步骤B.在目标区域中选择一个未遮挡的地理位置点，并通过RTK方法获得该地理位置点的卫星定位坐标，作为已标定点位置，然后进入步骤C；

步骤C.在目标区域中任意选择一个目标界址点，作为未标定点位置，并在已标定点位置、未标定点位置分别放置一个标定物，然后将双目摄像头设于目标区域中，且双目摄像头的图像捕获方向指向已标定点位置的标定物，并获得已标定点位置的标定物在图像中像素位置，再进入步骤D；

步骤D.针对双目摄像头进行移动或转向，直至识别到位于未标定点位置的标定物时，停止对双目摄像头的控制，并获得未标定点位置的标定物在图像中像素位置；

在上述对双目摄像头的整个控制过程中，同时使用SLAM算法，获得双目摄像机在世界坐标系下的位姿变化；

然后进入步骤E；

步骤E.根据已标定点位置的标定物在图像中像素位置、未标定点位置的标定物在图像中像素位置、以及双目摄像机在世界坐标系下的位姿变化，获得已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，然后进入步骤F；

步骤F.基于标定位置的坐标即为标定物的坐标，根据已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，结合已标定点位置的卫星定位坐标，转换获得未标定点位置的卫星定位坐标，即实现目标区域内的相对定位。

作为本发明的一种优选技术方案，所述步骤A包括如下步骤：

步骤A1.将标定物设置于各种不同环境下，并分别进行图像捕获，获得各张样本图像，然后进入步骤A2；

步骤A2.分别针对各张样本图像，获取样本图像中各指定环境因素的数据，并线性组合构成该样本图像的环境参数，同时，获取该样本图像中标定物在图像中的像素位置，然后进入步骤A3；

步骤A3.以各样本图像的环境参数为输入，样本图像中标定物在图像中的像素位置为输出，针对CNN模型进行训练，即获得用于检测标定物在图像中像素位置的CNN模型。

作为本发明的一种优选技术方案：所述步骤A2中的各指定环境因素包括环境亮度数据、环境尺度数据、环境遮挡数据。

作为本发明的一种优选技术方案：所述步骤E中，按如下操作，根据已标定点位置的标定物在图像中像素位置、未标定点位置的标定物在图像中像素位置、以及双目摄像机在世界坐标系下的位姿变化，获得已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标；

首先根据如下公式：

Z为标定物在相机坐标系下的深度坐标，(u₀,v₀)为已标定点位置的标定物在图像中像素位置，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，K为相机内参，(R,t)为相机在世界坐标系的位姿，由于对双目摄像头控制之前、相机坐标系和世界坐标系重合，即(R,t)＝(E，0)，则：

则

计算获得已标定点位置的标定物在世界坐标系下的坐标，其中，Z由公示

计算得到；

然后根据如下公式：

计算未标定点位置的标定物在世界坐标系下的坐标，其中，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，(u₁,v₁)为未标定点位置的标定物在图像中像素位置。

作为本发明的一种优选技术方案：所述步骤F中，基于标定位置的坐标即为标定物的坐标，根据已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，结合已标定点位置的卫星定位坐标，按如下公式：

P_GPS,t1＝P_w,t1-P_w,t0+P_GPS,t0

转换获得未标定点位置的卫星定位坐标；其中，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，P_GPS,t0表示双目摄像头在t0时刻、GPS坐标系下的坐标。

本发明所述一种基于双目摄像头的短距离精确相对定位方法采用以上技术方案与现有技术相比，具有以下技术效果：

本发明所设计基于双目摄像头的短距离精确相对定位方法，能够测量被遮挡界址点的GPS坐标，并能够保证精度，并且引入SLAM方法求解已标定和未标定界址点的相对位姿，从而获得未标定点的GPS坐标，此种方法提高了测量效率，同时本发明将CNN模型引入能够自动判别界址点在图像中的坐标满足了线上实时处理的要求，减轻了后期人为标定界址点的难度。

附图说明

图1是本发明设计一种基于双目摄像头的短距离精确相对定位方法的工作示意图；

图2是SLAM算法框架；

图3是地图点优化图。

具体实施方式

下面结合说明书附图对本发明的具体实施方式作进一步详细的说明。

土地调查过程中完成信息化的工作已经成为我国现代化进程中占有重要的地位，目前我国城镇土地调查主要使用第二次全国土地调查的遥感系统、GPS-RTK系统与惯导组合和光学全站仪。原有的侦测手段效率低，受到局部地形地貌等因素的影响造成部分遮挡严重的界址点获取难度大。因此，本发明设计了一种基于双目摄像头的短距离精确相对定位方法，通过双目摄像头实现目标区域内的相对定位，如图1所示，包括如下步骤。

步骤A.针对CNN模型进行训练，获得用于检测标定物在图像中像素位置的CNN模型，然后进入步骤B。

上述步骤A具体包括如下步骤：

步骤A1.将标定物设置于各种不同环境下，并分别进行图像捕获，获得各张样本图像，然后进入步骤A2。

步骤A2.分别针对各张样本图像，获取样本图像中各指定环境因素的数据，并线性组合构成该样本图像的环境参数，同时，获取该样本图像中标定物在图像中的像素位置，然后进入步骤A3；其中，各指定环境因素包括环境亮度数据、环境尺度数据、环境遮挡数据。

步骤A3.以各样本图像的环境参数为输入，样本图像中标定物在图像中的像素位置为输出，针对CNN模型进行训练，即获得用于检测标定物在图像中像素位置的CNN模型。

步骤B.在目标区域中选择一个未遮挡的地理位置点，并通过RTK方法获得该地理位置点的卫星定位坐标，作为已标定点位置，然后进入步骤C。

步骤C.在目标区域中任意选择一个目标界址点，作为未标定点位置，并在已标定点位置、未标定点位置分别放置一个标定物，然后将双目摄像头设于目标区域中，且双目摄像头的图像捕获方向指向已标定点位置的标定物，并获得已标定点位置的标定物在图像中像素位置，再进入步骤D。

步骤D.针对双目摄像头进行移动或转向，直至识别到位于未标定点位置的标定物时，停止对双目摄像头的控制，并获得未标定点位置的标定物在图像中像素位置。

在上述对双目摄像头的整个控制过程中，同时使用SLAM算法，获得双目摄像机在世界坐标系下的位姿变化，然后进入步骤E。

上述步骤D中，如图2所示，使用SLAM算法，获得双目摄像机在世界坐标系下的位姿变化，包括传感器数据获取、视觉里程计、后端优化、建图和回环检测；其中，

1)传感器数据获取通过双目视觉传感器获得左右两幅图像的信息；

2)视觉里程计：其主要作用是通过图像之间的局部信息构建相机在世界坐标下的位姿，通过提取出相邻帧图像之间的特征点求解本质矩阵，从而得到相机在世界坐标系下的位姿，因为SLAM对于算法的实时性要求较高因此对于计算能力要求较高的SIFT特征点方法并不适用于SLAM算法，则本发明应用ORB特征提取方法，其克服了FAST关键点没有方向的缺点和BRIEF描述子不具有旋转不变性的特点，其基本算法步骤如下：

a.在图像中选取像素p，假设它的亮度为I_p；

b.设置阈值T＝20％；

c.以像素p为中心，选取半径为3的圆上的16个像素点；

d.假如选取的圆上有连续N个点的亮度大于I_p+T或小于I_p-T，那么像素p可以被认为是特征点；

e.如果此点是特征点，则计算其特征点的方向：

θ＝arctan(m₀₁/m₁₀)  (3)

f.如果是特征点，则计算其BRIEF描述子：BRIEF描述子由0和1组成，0和1编码了关键点旁的两个像素p和q的大小关系，如果p比q大，则取1反之就取0；首先将此特征点周围的像素旋转θ角度，随机选取特征点周围的128个像素，得到了128维的向量用于描述此特征点；

g.循环以上四步对每一个像素执行相同的操作；

获得相邻帧之间的特征点和描述子后，根据描述子进行特征点的匹配，同时对误匹配的特征点进行筛选：

a.计算已经匹配特征点的描述子距离，得到最下的距离mindist；

b.对所有匹配的特征点筛选，如果其描述子的距离小于2*mindist则认为是有效的特征点，否则舍弃此特征点对；

对已经筛选过的n个特征点计算其在摄像机坐标系下的坐标：

其中u，v为特征点在图像中的像素坐标，(X，Y，Z)为特征点在相机坐标系下的坐标，

为相机内参，f＝αf_x，b是基线(左右两个相机的距离)，d为视差(左右相机对应特征点对应的物理成像平面的坐标)。通过获得的深度信息使用BundleAdjustment方法对相机的位姿和空间点的位置进行优化：

考虑n个三维空间点P及其投影p，待求解的相机位姿R和t的李代数为ε，某空间点坐标P_i＝[X_i,Y_i,Z_i]，其投影的像素坐标为u_i＝[u_i,v_i]，则像素与空间点的位置的关系如下：

其中，ε^表示向量ε的反对称矩阵。构建最小二乘问题：

对误差项进行线性化：

e(x+Δx)≈e(x)+JΔx  (8)

首先计算相机位姿的雅克比矩阵：

计算特征点空间位置的雅克比矩阵：

其中(X’,Y’,Z’)是特征点匹配计算得到的相机坐标系下的坐标；

构建最小二乘问题并得到各个变量的雅克比矩阵后便可使用列文伯格-马夸尔特方法求解最优解ε^*，此种方法是高斯牛顿方法和梯度下降方法的结合能够更加快更加精确的得到最优解：

a.首先选取相机的初始位姿R和t，特征点在世界坐标下的初始位置，计算

b.初始化算法参数k＝0，λ₀＝10^-3，v＝10；

c.计算相机和世界坐标的对应的雅克比矩阵J_k，计算

构造增量方程

d.求解增量方程得到δ_k，

如果

则令p_k+1＝δ_k，若||δ_k||<ξ,停止迭代，输出结果；否则另λ_k+1＝λ_k/v，转到步骤b；

如果

则令λ_k+1＝λ_k*v，重新解正规方程得到δ_k，返回步骤a；

3)后端优化：后端优化和视觉里程计的最大区别在于视觉里程计在局部地图中进行相机位姿和特征点空间位置的优化，而后端优化主要是从全局范围内进行优化。同样后端优化也使用Bundle Adjustment方法，只是代价函数与视觉里程有些区别：

待优化的变量为x＝[ξ₁,...ξ_m,p₁,...p_n]，其中ξ_i表示相机的位姿，p_i表示特征点在空间中的位置，对代价函数自变量x增加一个增量Δx时方程变为如下形式：

在使用列文伯格-马卡尔特方法时计算

对其进行稀疏化处理后能够有效的应用于优化算法中；

4)建图：将双目视觉的数据转换成点云，并采用更便于机器人理解的Voxel建立占据网格地图的方式，同时对于三维点云，先进行滤波；

其中，点云滤波算法：

a)生成每帧点云时，去掉深度值太大或无效的点；

b)利用统计滤波器方法去除孤立点，该滤波器统计每个点与距离它最近的N个点的距离值的分布，去除距离均值过大的点，即去除了孤立的噪声点；

c)最后，利用体素滤波器进行降采样，并采用八叉树形式地图进行存储；

在三维空间中将大正方体分成大小相同的8块大小相同的空间，如此进行细分直到分隔成最小分辨率的小方块，整个大方块可以看成根节点，最小的块可以看做是叶子节点，八叉树中在各个节点中存储是否被占据的信息，当某个方块的所有子节点都被占据或者不被占据时，就不对这个节点进行细分，同时每个节点中存储的是此节点被占据的概率，其具体的概率计算方式如下：

其中y为此节点被占据的次数，x为此节点被占据的概率；

5)回环检测用于计算图像间的相似性，当两幅图像的相似性评分高于预设阈值时，就认为出现了回环：

s(A,B)＞τ：loop closure

s(A,B)＜＝τ：not loop closure

其中A和B为两幅图像，s为相似性评分函数，τ为设定的阈值；

本系统使用的是词袋模型，词袋模型的工作原理是通过构建聚类特征点的词典，进行两幅图像相似度的比较；

首先需要对众多特征点使用K-means方法实现聚类，其算法步骤如下：

a、随机选取k个中心点：c₁...c_k；

b、对每一个样本，计算它与中心点之间的距离，取最小的作为它的归类；

c、重新计算每个类的中心点；

d、如果每个中心点都变换很小，则算法收敛，退出；否则返回步骤b；

构造完成词典后，将图像中的特征点与上面聚类构造的单词进行比较，在搜索的过程中使用多分叉数据结构，其具体的数据结构如下：

a、在根节点，使用K-means将所有样本聚成k类。这样得到了第一层；

b、对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层；

c、以此类推，最后得到叶子层。叶子层即为所谓的单词；

其中，在进行与单词进行比较时使用如下公式：

其中范数取L1范数。在两个特征点完全相同时的达到1，完全相反时得到0。

步骤E.根据已标定点位置的标定物在图像中像素位置、未标定点位置的标定物在图像中像素位置、以及双目摄像机在世界坐标系下的位姿变化，获得已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，然后进入步骤F。

上述步骤E中，按如下操作，根据已标定点位置的标定物在图像中像素位置、未标定点位置的标定物在图像中像素位置、以及双目摄像机在世界坐标系下的位姿变化，获得已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标。

首先根据如下公式：

Z为标定物在相机坐标系下的深度坐标，(u₀,v₀)为已标定点位置的标定物在图像中像素位置，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，K为相机内参，(R,t)为相机在世界坐标系的位姿，由于对双目摄像头控制之前、相机坐标系和世界坐标系重合，即(R,t)＝(E，0)，则：

则

计算获得已标定点位置的标定物在世界坐标系下的坐标，其中，Z由公示

计算得到。

然后根据如下公式：

计算未标定点位置的标定物在世界坐标系下的坐标，其中，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，(u₁,v₁)为未标定点位置的标定物在图像中像素位置。

步骤F.基于标定位置的坐标即为标定物的坐标，根据已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，结合已标定点位置的卫星定位坐标，转换获得未标定点位置的卫星定位坐标，即实现目标区域内的相对定位。

上述步骤F中，基于标定位置的坐标即为标定物的坐标，根据已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，结合已标定点位置的卫星定位坐标，按如下公式：

P_GPS,t1＝P_w,t1-P_w,t0+P_GPS,t0

转换获得未标定点位置的卫星定位坐标；其中，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，P_GPS,t0表示双目摄像头在t0时刻、GPS坐标系下的坐标。

图3为地图点保存算法voxel的排列方式，首先对空间进行较粗的划分，如果在较大的立方体内没有地图点则不对此立方体再进行细分，如果此立方体有地图点则将其进行细分。其实现结构类似于排序中的二叉树结构。以此减少存储内存。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (3)

1.一种基于双目摄像头的短距离精确相对定位方法，通过双目摄像头实现目标区域内的相对定位，其特征在于，包括如下步骤：

步骤A.针对CNN模型进行训练，获得用于检测标定物在图像中像素位置的CNN模型，然后进入步骤B；

步骤B.在目标区域中选择一个未遮挡的地理位置点，并通过RTK方法获得该地理位置点的卫星定位坐标，作为已标定点位置，然后进入步骤C；

步骤C.在目标区域中任意选择一个目标界址点，作为未标定点位置，并在已标定点位置、未标定点位置分别放置一个标定物，然后将双目摄像头设于目标区域中，且双目摄像头的图像捕获方向指向已标定点位置的标定物，并获得已标定点位置的标定物在图像中像素位置，再进入步骤D；

步骤D.针对双目摄像头进行移动或转向，直至识别到位于未标定点位置的标定物时，停止对双目摄像头的控制，并获得未标定点位置的标定物在图像中像素位置；

在上述对双目摄像头的整个控制过程中，同时使用SLAM算法，获得双目摄像机在世界坐标系下的位姿变化；

然后进入步骤E；

步骤E.按如下操作，根据已标定点位置的标定物在图像中像素位置、未标定点位置的标定物在图像中像素位置、以及双目摄像机在世界坐标系下的位姿变化，获得已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，然后进入步骤F；

首先根据如下公式：

Z为标定物在相机坐标系下的深度坐标，(u₀,v₀)为已标定点位置的标定物在图像中像素位置，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，K为相机内参，(R,t)为相机在世界坐标系的位姿，由于对双目摄像头控制之前、相机坐标系和世界坐标系重合，即(R,t)＝(E，0)，则：

则

计算获得已标定点位置的标定物在世界坐标系下的坐标，其中，Z由公示

计算得到；

然后根据如下公式：

计算未标定点位置的标定物在世界坐标系下的坐标，其中，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，(u₁,v₁)为未标定点位置的标定物在图像中像素位置；

步骤F.基于标定位置的坐标即为标定物的坐标，根据已标定点位置的标定物在世界坐标系下的坐标、以及未标定点位置的标定物在世界坐标系下的坐标，结合已标定点位置的卫星定位坐标，按如下公式：

P_GPS,t1＝P_w,t1-P_w,t0+P_GPS,t0

转换获得未标定点位置的卫星定位坐标，即实现目标区域内的相对定位；其中，P_w,t0为已标定点位置的标定物在世界坐标系下的坐标，P_w,t1表示未标定点位置的标定物在世界坐标系下的坐标，P_GPS,t0表示双目摄像头在t0时刻、GPS坐标系下的坐标。

2.根据权利要求1所述一种基于双目摄像头的短距离精确相对定位方法，其特征在于，所述步骤A包括如下步骤：

步骤A1.将标定物设置于各种不同环境下，并分别进行图像捕获，获得各张样本图像，然后进入步骤A2；

步骤A2.分别针对各张样本图像，获取样本图像中各指定环境因素的数据，并线性组合构成该样本图像的环境参数，同时，获取该样本图像中标定物在图像中的像素位置，然后进入步骤A3；

步骤A3.以各样本图像的环境参数为输入，样本图像中标定物在图像中的像素位置为输出，针对CNN模型进行训练，即获得用于检测标定物在图像中像素位置的CNN模型。

3.根据权利要求2所述一种基于双目摄像头的短距离精确相对定位方法，其特征在于：所述步骤A2中的各指定环境因素包括环境亮度数据、环境尺度数据、环境遮挡数据。

Images