CN110533718A - 一种单目视觉人工路标的辅助ins的导航定位方法 - Google Patents

Abstract

本发明为一种单目视觉人工路标的辅助INS的导航定位方法，包括以下步骤：设计人工路标；标定尺度因子；建立视觉路标库；基于路标库对相机位置和姿态进行帧间位姿估计；单目视觉人工路标辅助INS定位。本发明的优点是：利用已知视觉路标库信息来推算采集当前人工路标图像时相机的位置和姿态信息，将相机当前位置和INS数据利用卡尔曼滤波算法进行融合，能够减小传统INS定位方法中累积误差，使定位更准确、成本低、操作简单。

Description

一种单目视觉人工路标的辅助INS的导航定位方法

技术领域

本发明属于定位导航领域，尤其涉及一种单目视觉人工路标的辅助INS的导航定位方法。

背景技术

传统的定位方法主要包括卫星、惯性导航定位等，卫星导航定位应用广泛、精度高，但非常容易受到环境的影响，在室内无卫星信号和卫星信号有遮挡区域都将无法使用。惯性导航***是一种自主式的导航***，其优点是可提供连续的导航信息，短期的定位精度和稳定性好，但是由于导航信息是由积分产生，定位误差随时间而增大，不能长期独立工作。

近年来，视觉导航作为新的定位方法得到越来越多的关注。视觉里程计(visualodometer，VO)的概念是由Nistér提出，它是通过相邻图像匹配特征点坐标的变化来估计载体的位姿变化情况。因为VO是通过一系列前后帧的对比来估计相机的运动情况，所以不可避免的会造成误差的累计。基于视觉的同时定位与地图构建技术(visual simultaneouslocalization and mapping，VSLAM)，由VO前端、后端优化、回环检测以及建图几个部分组成，在机器人自主定位和路径规划中起到重要的作用。VSLAM相比于VO计算量大大提高，对数据处理终端计算能力有很高的要求，很难实现实时定位与建图。

发明内容

本发明主要解决了传统室内定位方法定位误差会随着时间增长而增大的问题，提出成本低、操作简单、准确性高的单目视觉人工路标的辅助INS的导航定位方法。

本发明解决其技术问题所采用的技术方案是，一种单目视觉人工路标的辅助INS的导航定位方法，包括以下步骤：

S1：设计人工路标；

S2：标定尺度因子；

S3：建立视觉路标库；

S4：基于路标库对相机位置和姿态进行单目视觉估计；

S5：单目视觉人工路标辅助INS定位。

本发明在已知位置设置人工路标图像，建立视觉路标库，根据视觉路标库中图像与当前采集图像的关系，结合已知的路标库图像获取时的相机位置和姿态从而获得当前相机的位置和姿态，利用相机的定位信息对INS定位数据进行误差修正，从而减小INS的误差累积，使定位更加精确。

作为上述一种优选方案，所述步骤S1人工路标***尺寸为R，正中设有用于路标标号的二维码，二维码到人工路标边缘的区域设有标识图案。标识图案能够使人工路标具有足够多的ORB特征点。

作为上述一种优选方案，所述步骤S2中标定尺度因子，包括以下步骤：

S21：利用相机在已知距离D正对人工路标进行拍摄；

S22：获取人工路标的像素尺寸r；

S23：根据小孔成像原理，获取尺度因子f

f＝D*r/R。

作为上述一种优选方案，所述步骤S3中建立视觉路标库，包括以下步骤：

S31：将多个人工路标设置在带有相机的载体的移动线路上；

S32：使相机在已知位置正对人工路标进行拍摄，记录相机位置、姿态；

S33：检测拍摄图像中的路标区域以及像素尺寸，根据尺度因子得到路标到相机的距离，并对拍摄图像中的非人工路标区域进行填充；

S34：将相机位置、姿态、相机到路标的距离及填充后的图像对应存入到视觉路标库中。

作为上述一种优选方案，所述步骤S4中基于路标库对相机位置和姿态进行帧间位姿估计，包括以下步骤：

S41：根据相机拍摄图像中人工路标的像素尺寸获得相机到人工路标的距离；

S42：识别拍摄图像中人工路标中的二维码，在视觉路标库中寻找对应的人工路标数据；

S43：对拍摄图像中非人工路标区域进行填充，与路标库中对应图像进行特征提取和匹配；

S44：根据匹配的特征点像素坐标和相机到路标的距离得到路标图像中匹配特征点在相机坐标系下的三维点集合P＝{p₁，...，p_n}，以及当前图像中匹配特征点在相机坐标系下的三维点集合P′＝{p′₁，...，p′_n}；

S45：匹配的三维点之间满足欧式变化，使得

p_i＝Rp′_i+t

解上式，获得旋转矩阵R和平移向量t；

S46：结合旋转矩阵R和平移向量t及已知的库图像获取时相机的位置、姿态信息，获得相机拍摄当前图像时的位置和姿态信息。

作为上述一种优选方案，所述步骤S44中通过以下方法获取匹配点的三维点集合：在相机成像的像平面上建立像素坐标系，以相机光心为原点建立三维相机坐标系，已知图像上特征点p′在像素坐标系下坐标(u，v)，得实际特征点p在三维相机坐标系上的坐标(X，Y，Z)

(c_x，c_y)为光心于像平面上的投影点在像素坐标系上的坐标，以光心于像平面上的投影点为原点建立像坐标系，图像上特征点p′在像坐标系的坐标为(x′，y′)，根据像坐标系和像素坐标系的关系可得

同时

最终可得

其中f₁为相机焦距，αf₁＝f_x，βf₁＝f_y，α为图像上特征点p′的像素坐标系横坐标在像坐标系横坐标轴上的缩放倍数，β为图像上特征点p′的像素坐标系纵坐标在像坐标系纵坐标轴上的缩放倍数。

作为上述一种优选方案，所述步骤S45中通过奇异值分解的方法获得旋转矩阵R和平移向量t：计算当前图像和库图像中匹配点的质心位置p，p′，再计算每个点的去质心坐标

q_i＝p_i-p，q′_i＝p′_i-p′

计算旋转矩阵R和平移向量t

根据旋转矩阵R求平移向量t

t^*＝p-Rp′。

作为上述一种优选方案，所述S5中单目视觉人工路标辅助INS定位：以INS航位推算公式为预测模型建立状态方程

以单目视觉人工路标定位方式为观测模型建立观测方程

Y_k＝HX_k+v(k)

其中，X_k、X_k-1为小车相邻的时刻对应的状态向量，X_k＝[x_k，y_k，z_k]，(x_k，y_k，z_k)为小车当前时刻在三维导航坐标系上的坐标，三维导航坐标系为载体所处活动空间的坐标系；A为状态转移矩阵；w(k)为***噪声误差；Y_k为观测值，即人工视觉路标定位结果；H为观测矩阵；v(k)为观测过程中的噪声误差；得到状态更新方程

K_k为Kalman增益；是由状态方程得到的该时刻状态估计量；为由观测方程得到的该时刻的观测量；即为融合后的位置信息。

本发明的优点是：利用已知视觉路标库信息来推算采集当前人工路标图像时相机的位置和姿态信息，将相机当前位置和INS数据利用卡尔曼滤波算法进行融合，能够减小传统INS定位方法中累积误差，使定位更准确、成本低、操作简单。

附图说明

图1为本发明的一种流程示意图。

图2为本发明中人工路标的一种结构示意图。

图3为本发明中标定尺寸因子的一种流程示意图。

图4为本发明中建立视觉路标库的一种流程示意图。

图5为本发明中基于路标库对相机位置和姿态进行帧间位姿估计的一种流程示意图。

图6为本发明中相机坐标系模型。

图7为本发明中帧间运动估计图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步的说明。

实施例：

本实施例一种单目视觉人工路标的辅助INS的导航定位方法，如图1所示，包括以下步骤：

S1：设计人工路标；如图2所示，人工路标为已知***半径R的圆形，便于使用霍夫圆变换对圆形路标进行检测；在圆内设置路标标号的二维码，便于路标的识别。在圆形路标区域内设计各种标识图案，使得路标区域内有足够多的ORB特征点，用于之后的特征提取和匹配，不同路标只需替换中间区域的标号二维码。

S2：标定尺度因子，如图3所示，包括以下步骤：

S21：利用相机在已知距离D正对人工路标进行拍摄；

S22：对拍摄的图像采用霍夫圆变换检测圆形路标的像素半径，获取人工路标的像素半径r，已知人工路标实际半径R；

S23：根据小孔成像原理

R/r＝D/f

，获取尺度因子f

f＝D*r/R

根据尺度因子，在检测到人路标的像素半径为r′时，可知相机到路标的距离为

d＝f*R/r′。

S3：建立视觉路标库，如图4所示，包括以下步骤：

S31：将所有人工路标分别设置在带有相机的小车移动线路的墙壁上，高度和相机距离地面的高度相同；

S32：使相机在已知位置正对依次人工路标进行拍摄，记录相机位置、姿态；

S33：利用霍夫圆变换检测检测拍摄图像中的路标区域以及像素半径，根据S23中的尺度因子得到路标到相机的距离，并对拍摄图像中的非人工路标区域用黑色进行填充；

S34：将相机位置、姿态、相机到路标的距离及填充后的图像对应存入到视觉路标库中；

S4：基于路标库对相机位置和姿态进行帧间位姿估计，如图5所示，包括以下步骤：

S41：根据相机拍摄图像中人工路标的像素尺寸获得相机到人工路标的距离；

S42：识别拍摄图像中人工路标中的二维码，在视觉路标库中寻找对应的人工路标数据；

S43：对拍摄图像中非人工路标区域进行黑色填充，与路标库中对应图像进行ORB特征提取和匹配；

S44：根据匹配的特征点像素坐标和相机到路标的距离得到路标图像中匹配特征点在相机坐标系下的三维点集合P＝{p₁，...，p_n}，以及当前图像中匹配特征点在相机坐标系下的三维点集合P′＝{p′₁，...，p′_n}。三维点集合获取方法如下：如图6所示在相机成像的像平面上建立像素坐标系o″-uv，以相机光心为原点建立三维相机坐标系o-xyz，已知图像上特征点p′在像素坐标系下坐标(u，v)，得实际特征点p在三维相机坐标系上的坐标(X，Y，Z)

(c_x，c_y)为光心于像平面上的投影点在像素坐标系上的坐标，以光心于像平面上的投影点为原点建立像坐标系o′-x′y′，图像上特征点p′在像坐标系的坐标为(x′，y′)，根据像坐标系和像素坐标系的关系可得

同时

最终可得

其中f₁为相机焦距，αf₁＝f_x，βf₁＝f_y，α为图像上特征点p′的像素坐标系横坐标在像坐标系横坐标轴上的缩放倍数，β为图像上特征点p′的像素坐标系纵坐标在像坐标系纵坐标轴上的缩放倍数。

S45：匹配的三维点之间满足欧式变化，使得

p_i＝Rp′_i+t

通过奇异值分解的方法解上式，获得旋转矩阵R和平移向量t，求解过程如下：

计算当前图像和库图像中匹配点的质心位置p，p′，再计算每个点的去质心坐标

q_i＝p_i-p，q′_i＝p′_i-p′

计算旋转矩阵R和平移向量t

根据旋转矩阵R求平移向量t

t^*＝p-Rp′。

S46：如图7所示，结合由奇异值分解(SVD)方法获取旋转矩阵R和平移向量t及已知的库图像获取时相机的位置、姿态信息获得相机拍摄当前图像时的位置和姿态信息；

S5：单目视觉人工路标辅助INS定位，以INS/里程计作为小车的主要导航方式，而将基于视觉人工路标的定位方式作为辅助，对INS/里程计定位数据进行误差修正。因为由视觉求解的姿态变化较为准确，这里直接用视觉得到的航向角对惯性导航向的累积误差进行修正，而对位置信息则采用Kalman滤波的方式进行融合：以INS航位推算公式为预测模型建立状态方程

以单目视觉人工路标定位方式为观测模型建立观测方程

Y_k＝HX_k+v(k)

其中，X_k、X_k-1为小车相邻的时刻对应的状态向量，X_k＝[x_k，y_k，z_k]，(x_k，y_k，z_k)为小车当前时刻在三维导航坐标系上的坐标，导航三维坐标系为小车所处活动空间的坐标系；A为状态转移矩阵；w(k)为***噪声误差；Y_k为观测值，即人工视觉路标定位结果；H为观测矩阵；v(k)为观测过程中的噪声误差；

得到状态更新方程

其中，Kk为Kalman增益；是由状态方程得到的该时刻状态估计量；为由观测方程得到的该时刻的观测量；即为融合后的位置信息。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (8)

1.一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：包括以下步骤：

S1：设计人工路标；

S2：标定尺度因子；

S3：建立视觉路标库；

S4：基于路标库对相机位置和姿态进行帧间位姿估计；

S5：单目视觉人工路标辅助INS定位。

2.根据权利要求1所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S1人工路标***尺寸为R，正中设有用于路标标号的二维码，二维码到人工路标边缘的区域设有标识图案。

3.根据权利要求1所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S2中标定尺度因子，包括以下步骤：

S21：利用相机在已知距离D正对人工路标进行拍摄；

S22：获取人工路标的像素尺寸r；

S23：根据小孔成像原理，获取尺度因子f

f＝D*r/R。

4.根据权利要求1所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S3中建立视觉路标库，包括以下步骤：

S31：将多个人工路标设置在带有相机的载体的移动线路上；

S32：使相机在已知位置正对人工路标进行拍摄，记录相机位置、姿态；

S33：检测拍摄图像中的路标区域以及像素尺寸，根据尺度因子得到路标到相机的距离，并对拍摄图像中的非人工路标区域进行填充；

S34：将相机位置、姿态、相机到路标的距离及填充后的图像对应存入到视觉路标库中。

5.根据权利要求1所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S4中基于路标库对相机位置和姿态进行帧间位姿估计，包括以下步骤：

S41：根据相机拍摄图像中人工路标的像素尺寸获得相机到人工路标的距离；

S42：识别拍摄图像中人工路标中的二维码，在视觉路标库中寻找对应的人工路标数据；

S43：对拍摄图像中非人工路标区域进行填充，与路标库中对应图像进行特征提取和匹配；

S44：根据匹配的特征点像素坐标和相机到路标的距离得到路标图像中匹配特征点在相机坐标系下的三维点集合P＝{p₁，...，p_n}，以及当前图像中匹配特征点在相机坐标系下的三维点集合P′＝{p′₁，...，p′_n}；

S45：匹配的三维点之间满足欧式变化，使得

p_i＝Rp′_i+t

解上式，获得旋转矩阵R和平移向量t；

S46：结合旋转矩阵R和平移向量t及已知的库图像获取时相机的位置、姿态信息，获得相机拍摄当前图像时的位置和姿态信息。

6.根据权利要求5所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S44中通过以下方法获取匹配点的三维点集合：在相机成像的像平面上建立像素坐标系，以相机光心为原点建立三维相机坐标系，已知图像上特征点p′在像素坐标系下坐标(u，v)，得实际特征点p在三维相机坐标系上的坐标(X，Y，Z)

(c_x，c_y)为光心于像平面上的投影点在像素坐标系上的坐标，以光心于像平面上的投影点为原点建立像坐标系，图像上特征点p′在像坐标系的坐标为(x′，y′)，根据像坐标系和像素坐标系的关系可得

同时

最终可得

其中f₁为相机焦距，αf₁＝f_x，βf₁＝f_y，α为图像上特征点p′的像素坐标系横坐标在像坐标系横坐标轴上的缩放倍数，β为图像上特征点p′的像素坐标系纵坐标在像坐标系纵坐标轴上的缩放倍数。

7.根据权利要求5所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述步骤S45中通过奇异值分解的方法获得旋转矩阵R和平移向量t：计算当前图像和库图像中匹配点的质心位置p，p′，再计算每个点的去质心坐标

q_i＝p_i-p，q′_i＝p′_i-p′

计算旋转矩阵R和平移向量t

根据旋转矩阵R求平移向量t

t^*＝p-Rp′。

8.根据权利要求1所述的一种单目视觉人工路标的辅助INS的导航定位方法，其特征是：所述S5中单目视觉人工路标辅助INS定位：以INS航位推算公式为预测模型建立状态方程

以单目视觉人工路标定位方式为观测模型建立观测方程

Y_k＝HX_k+v(k)

其中，X_k、X_k-1为小车相邻的时刻对应的状态向量，X_k＝[x_k，y_k，z_k]，(x_k，y_k，z_k)为小车当前时刻在三维导航坐标系上的坐标，三维导航坐标系为载体所处活动空间的坐标系；A为状态转移矩阵；w(k)为***噪声误差；Y_k为观测值，即人工视觉路标定位结果；H为观测矩阵；v(k)为观测过程中的噪声误差；得到状态更新方程

K_k为Kalman增益；是由状态方程得到的该时刻状态估计量；为由观测方程得到的该时刻的观测量；即为融合后的位置信息。