CN105761242A - 一种基于计算机双目视觉与惯性测量的盲人行走定位方法 - Google Patents

Abstract

一种基于计算机双目视觉与惯性测量的盲人行走定位方法，一方面利用佩戴在盲人头部的双目相机采集场景图像，通过图像特征匹配方法查找场景中已知经纬度的标志物，采用计算机双目立体视觉方法计算标志物在相机坐标系下的三维坐标，另一方面利用固定在相机上的组合惯性器件测量相机的姿态角，计算出相机坐标系关于盲人所在位置的地理坐标系的转换矩阵；利用转换矩阵将标志物的相机系坐标变换为盲人所在位置的地理系坐标，进而由标志物经纬度推算出盲人所在位置的经纬度。本发明综合计算机双目视觉算法与惯性测量技术，实现盲人行走位置的定位，不仅简单易行，而且准确性较高，特别适用于盲人出行导盲的定位。

Description

一种基于计算机双目视觉与惯性测量的盲人行走定位方法

技术领域

本发明属于导航定位技术领域，涉及一种基于计算机双目视觉与惯性测量的盲人行走定位方法，该方法可以实现盲人行走的准确定位，特别适用于盲人室外行走导盲。

背景技术

盲人由于先天或后天的生理缺陷而丧失了视觉，因而丧失了获得知识和经验的最重要来源，对盲人日常生活造成了极大障碍，尤其是自身行走定位能力的丧失制约了盲人的出行活动。为了弥补盲人的行走定位能力，目前最常用的是GPS定位方法，但GPS易受遮挡、衰减和干扰等因素影响，在高楼林立的城市峡谷等复杂环境中定位精度显著下降，而人的行走范围相对较小，因而不能满足盲人定位需求。此外还采用诸如蜂窝移动通信系统定位技术和惯性导航定位系统INS等常用导航定位技术。蜂窝移动通信系统定位技术虽然在城市峡谷等环境中仍可实现定位，但其定位精度较低；惯性导航定位系统INS短时间内定位精度较高，但其定位误差随着时间积累，因而都不能满足盲人行走定位技术准确性和可靠性的要求。

发明内容

本发明技术解决问题：克服现有技术的不足，提供一种基于计算机双目视觉与惯性测量的盲人行走定位方法，为盲人出行提供准确可靠的定位。

本发明的技术解决方案:利用佩戴在盲人头部的双目相机采集场景图像，通过图像特征匹配方法查找场景中已知经纬度的标志物，采用计算机双目立体视觉方法计算标志物在相机坐标系下的三维坐标；利用固定在相机上的IMU和磁力计测量相机的姿态角，计算出相机坐标系关于盲人所在位置的地理坐标系的转换矩阵；利用转换矩阵将标志物的相机系坐标变换为盲人所在位置的地理系坐标，进而由标志物经纬度推算出盲人所在位置的经纬度。具体包括以下步骤：

1、利用固定在盲人头部的双目相机采集左右场景图像。

2、匹配已知经纬度的标志物。采用SURF算法提取左图像特征点，并与预先存储的标志物SURF特征点进行匹配，匹配相似度大于给定阈值M，则判断相机视野内存在标志物，并界定标志物左图像中区域。

3、对双目场景图像进行特征提取与匹配。采用SURF算法提取右图像中的特征点，并与左图像特征点进行匹配，得到标志物所在区域内的特征点对集合。

4、计算标志物相机坐标系的三维坐标。利用平行双目立体视觉三维测量原理计算标志物匹配点在左相机坐标系下的三维坐标，计算方法为：

$\left\{\begin{array}{c}{x}_c=\frac{B\·X_{left}}{Disparity}\\ y_c=\frac{B\·Y_{left}}{Disparity}\\ z_c=\frac{B\·f}{Disparity}\end{array}\right.$

其中，(X_left,Y_left)、(X_right,Y_right)为标志物匹配点P在左图和右图图像坐标系下的坐标；Disparity＝X_left-X_right为视差；B和f为双目相机基线距和焦距。

5、根据组合惯性器件计算出相机坐标系相对于盲人所在位置的地理坐标系的转换矩阵。根据组合惯性器件和双目相机的空间安装关系得到相机坐标系O-X_cY_cZ_c与载体坐标系O-X_bY_bZ_b的转换矩阵组合惯性器件测量载体姿态角得到载体坐标系O-X_bY_bZ_b与地理坐标系O-X_tY_tZ_t的转换矩阵计算出相机坐标系O-X_cY_cZ_c与地理坐标系O-X_tY_tZ_t之间旋转矩阵：

6、利用转换矩阵将标志物的相机坐标系坐标变换为盲人所在位置处的地理坐标。设已知标志物相机坐标系下坐标为(x_c,y_c,z_c)，当地地理坐标系下坐标为(x_t,y_t,z_t)，则：

$\left(\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right)=R_c^t\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)$

7、根据标志物经纬度推算盲人所在位置处的经纬度。若已知标志物的经纬度为(λ0，φ0)，盲人当前位置经纬度为(λ_p，φ_p)，地理坐标系采用西北天坐标系；不考虑盲人和标志物的高度信息，则n＝(x_ty_t)为盲人与标志物之间当地水平面内向量；选用WGS-84大地坐标系模型，地球长轴为a，短轴为b，第一偏心率为e。计算方法如下：

$\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ \mathrm{\Δ}\mathrm{\φ}\end{array}\right)=\left(\begin{array}{c}\frac{y_t\sqrt{1-e^2{\sin }^2{\mathrm{\φ}}_0}}{2\mathrm{\π}a{\mathrm{cos\φ}}_0}\\ \frac{x_t}{2\mathrm{\π}b}\end{array}\right)\·360$

$\left(\begin{array}{c}{\mathrm{\λ}}_p\\ {\mathrm{\φ}}_p\end{array}\right)=\left(\begin{array}{c}{\mathrm{\λ}}_0\\ {\mathrm{\φ}}_0\end{array}\right)-\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ -\mathrm{\Δ}\mathrm{\φ}\end{array}\right)$

本发明的优点在于：(1)定位不受城市峡谷等复杂环境的影响。(2)定位误差不随时间积累，定位可靠性高。(3)采用图像匹配和双目立体技术，算法简单易行。

附图说明

图1：本发明总体流程图；

图2：本发明标志物场景图像匹配示意图；

图3：本发明实验装置示意图；

图4：本发明实验模型示意图；

具体实施方式

下面结合说明书附图对本发明方法做详细说明：

本发明利用佩戴在盲人头部的双目相机采集场景图像，通过图像特征匹配方法查找场景中已知经纬度的标志物，采用计算机双目立体视觉方法计算标志物在相机坐标系下的三维坐标；利用固定在相机上的组合惯性器件测量相机的姿态角，计算出相机坐标系关于盲人所在位置的地理坐标系的转换矩阵；利用转换矩阵将标志物的相机系坐标变换为盲人所在位置的地理系坐标，进而由标志物经纬度推算出盲人所在位置的经纬度。

如图1所示，本发明具体包括以下步骤：

步骤1：利用固定在盲人头部的双目相机采集左右场景图像。

步骤1-1.将双目相机固定在头部，并预先对双目相机进行标定，得到平行双目相机下归一化焦距f，左右相机主点位置(X_centerL,Y_centerL)和(X_centerR,Y_centerR)以及基线距B。

步骤1-2.采集左右场景图像。

步骤2：将采集到的左场景图像与已知的标志物进行特征点匹配，若匹配成功，则界定标志物左图像中区域，若匹配失败，则双目相机继续采集场景图像进行匹配，直至匹配成功。

步骤2-1.采用SURF算法提取左场景图像的特征信息并保存，将左场景图像中的每个特征点的特征描述子与标志物中的特征点的描述子进行比较，找出得分最高的一对描述子，则其对应的一组特征点对是匹配点。

步骤2-2.对粗匹配点进行对称性测试和极线约束，得到最终的可靠匹配点集合。

步骤2-3.设定阈值M，特征匹配点数大于M时，表明左相机场景中存在标志物，匹配成功(如图2所示)，得到标志物在左图像中区域。若匹配不成功，继续采集场景图像匹配标志物。

步骤3：对左右场景图像进行匹配，得到标志物在左右图像中坐标(X_left,Y_left)和(X_right,Y_right)。

步骤3-1.对右场景图像进行SURF特征提取，然后与之前左场景图像提取的SURF特征点进行匹配，并采用对称性测试和极线约束，得到可靠的匹配点集。根据标志物在左图像的图像区域确定标志物在左右场景图像的SURF特征点集。

步骤3-2.对获得的标志物左右图像的SURF特征点集，经处理得到标志物的左右图像坐标(X_left,Y_left)和(X_right,Y_right)。

步骤4：根据步骤3得到标志物在左右场景图像的图像坐标(X_left,Y_left)和(X_right,Y_right)，采用平行双目测量原理，计算标志物相机坐标系下坐标，如下所示：

$\left\{\begin{array}{c}{x}_c=\frac{B\·X_{left}}{Disparity}\\ y_c=\frac{B\·Y_{left}}{Disparity}\\ z_c=\frac{B\·f}{Disparity}\end{array}\right.$

其中，(X_left,Y_left)、(X_right,Y_right)为匹配点P在左图和右图图像坐标系下的坐标；Disparity＝X_left-X_right为视差；B和f为步骤1-1标定的双目相机基线距和焦距。

步骤5：根据组合惯性器件获取载体的姿态矩阵。

步骤5-1：预先将组合惯性器件固定在双目相机上(如图3所示)，根据两者空间安装关系得到相机坐标系和载体坐标系之间旋转矩阵

$R_c^b=\left(\begin{array}{ccc}r1& r2& r3\\ r4& r5& r6\\ r7& r8& r9\end{array}\right)$

步骤5-2.采用在线误差补偿方法减少IMU误差，并结合磁力计通过卡尔曼滤波实现组合姿态测量，得到载体坐标系与地理坐标系之间的旋转矩阵

步骤6：通过坐标变换计算相机坐标系与地理坐标系之间旋转矩阵

$R_c^t=R_b^t\·R_c^b=\left(\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right)$

其中地理坐标系采用北西天坐标系(如图4所示)。

步骤7：计算标志物地理坐标系的三维坐标。设(x_c,y_c,z_c)为标志物相机坐标系坐标，则标志物在盲人所处位置为原点的当地地理坐标系下坐标(x_t,y_t,z_t)

$\left(\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right)=R_c^{t}d=R_c^t\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)$

其中d＝(x_cy_cz_c)为标志物在相机坐标系下向量(如图4所示)。

步骤8：利用已知的标志物经纬度(λ₀，φ₀)，计算盲人WGS-84坐标系下经纬度(λ_p，φ_p)。

步骤8-1.如图4所示，(λ₀，φ₀)为已知的标志物经纬度，(λ_p，φ_p)为盲人位置处的经纬度，采用WGS-84地心坐标系模型，a为地球长轴，b为地球短轴，e为第一偏心率。忽略盲人和标志物的高度信息，则n＝(x_ty_t)为盲人与标志物之间当地水平面内距离向量，其中x_t为北向距离分量，y_t为西向距离分量。

步骤8-2.如图4所示，标志物所在位置的纬线圈周长为经线圈周长为不考虑地球表面水平化的影响，则

$\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ \mathrm{\Δ}\mathrm{\φ}\end{array}\right)=\left(\begin{array}{c}\frac{y_t}{L_{{\mathrm{\φ}}_0}}\\ \frac{x_t}{L_{{\mathrm{\λ}}_0}}\end{array}\right)\·360=\left(\begin{array}{c}\frac{y_t\sqrt{1-e^2{\sin }^2{\mathrm{\φ}}_0}}{2\mathrm{\π}a{\mathrm{cos\φ}}_0}\\ \frac{x_t}{2\mathrm{\π}b}\end{array}\right)\·360$

$\left(\begin{array}{c}{\mathrm{\λ}}_p\\ {\mathrm{\φ}}_p\end{array}\right)=\left(\begin{array}{c}{\mathrm{\λ}}_0\\ {\mathrm{\φ}}_0\end{array}\right)-\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ -\mathrm{\Δ}\mathrm{\φ}\end{array}\right)$

其中(λ_p，φ_p)为计算得到盲人行走位置处WGS-84坐标模型下经纬度。

Claims (8)

1.一种基于计算机双目视觉与惯性测量的盲人行走定位方法，其特征在于：一方面利用佩戴在盲人头部的双目相机采集场景图像，通过图像特征匹配方法查找场景中已知经纬度的标志物，采用计算机双目立体视觉方法计算标志物在相机坐标系下的三维坐标，另一方面利用固定在相机上的组合惯性器件计测量相机的姿态角，计算出相机坐标系关于盲人所在位置的地理坐标系的转换矩阵；利用转换矩阵将标志物的相机系坐标变换为盲人所在位置的地理系坐标，进而由标志物经纬度推算出盲人所在位置的经纬度。

2.根据权利要求1所述的匹配已知经纬度的标志物，其特征在于：采用SURF算法提取左图像特征点，并与预先存储的标志物SURF特征点进行匹配，匹配相似度大于给定阈值M，则判断相机视野内存在标志物，并界定标志物左图像中区域。

3.根据权利要求1所述的采用计算机双目立体视觉方法，其特征在于：所述方法包括双目场景图像特征点提取与匹配和计算标志物在相机坐标系下坐标两部分。

4.根据权利要求3所述的双目场景图像特征点提取与匹配，其特征在于：采用SURF算法提取右图像中的特征点，并与左图像特征点进行匹配，得到标志物所在区域内的特征点对集合。

5.根据权利要求3所述的计算标志物相机坐标系的三维坐标，其特征在于：利用平行双目立体视觉三维测量原理计算标志物匹配点在左相机坐标系下的三维坐标，计算方法为：

$\left\{\begin{array}{c}{x}_c=\frac{B\·X_{left}}{Disparity}\\ y_c=\frac{B\·Y_{left}}{Disparity}\\ z_c=\frac{B\·f}{Disparity}\end{array}\right.$

其中，(X_left,Y_left)和(X_right,Y_right)分别为标志物匹配点P在左图和右图图像坐标系下的坐标；Disparity＝X_left-X_right为视差；B和f为双目相机基线距和焦距。

6.根据权利要求1所述的利用IMU和磁力计测量相机的姿态角，其特征在于：组合惯性器件由IMU和磁力计构成，根据组合惯性器件和双目相机的空间安装关系得到相机坐标系O-X_cY_cZ_c与载体坐标系O-X_bY_bZ_b的转换矩阵组合惯性器件测量载体姿态角得到载体坐标系O-X_bY_bZ_b与地理坐标系O-X_tY_tZ_t的转换矩阵计算出相机坐标系O-X_cY_cZ_c与地理坐标系O-X_tY_tZ_t之间旋转矩阵： $R_c^t=R_b^t\·R_c^b.$

7.根据权利要求1所述的利用转换矩阵将标志物的相机系坐标变换为盲人所在位置的地理系坐标，其特征在于：若标志物在相机坐标系和地理坐标系下的三维坐标为(x_c,y_c,z_c)和(x_t,y_t,z_t)，则：

$\left(\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right)=R_c^t\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right)$

8.根据权利要求1所述的由标志物经纬度推算出盲人所在位置的经纬度，其特征在于：若已知标志物的经纬度为(λ₀，φ₀)，盲人所在位置的经纬度为(λ_p，φ_p)，标志物在盲人所在位置地理坐标系下的坐标为(x_t,y_t,z_t)，地理坐标系采用西北天右手直角坐标系，则n＝(x_ty_t)为盲人与标志物之间当地水平面内向量，选用WGS-84大地坐标系模型，地球长轴为a，短轴为b，第一偏心率为e。计算方法如下：

$\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ \mathrm{\Δ}\mathrm{\φ}\end{array}\right)=\left(\begin{array}{c}\frac{y_t\sqrt{1-e^2{\sin }^2{\mathrm{\φ}}_0}}{2\mathrm{\π}a{\mathrm{cos\φ}}_0}\\ \frac{x_t}{2\mathrm{\π}b}\end{array}\right)\·360$

$\left(\begin{array}{c}{\mathrm{\λ}}_p\\ {\mathrm{\φ}}_p\end{array}\right)=\left(\begin{array}{c}{\mathrm{\λ}}_0\\ {\mathrm{\φ}}_0\end{array}\right)-\left(\begin{array}{c}\mathrm{\Δ}\mathrm{\λ}\\ -\mathrm{\Δ}\mathrm{\φ}\end{array}\right)$