基于计算机视觉/GPS/MG姿态测量的盲人行走组合导航方法研究

贾文渊，魏 彤

(北京航空航天大学仪器科学与光电工程学院，北京100191）

基于计算机视觉/GPS/MG姿态测量的盲人行走组合导航方法研究

贾文渊，魏 彤

(北京航空航天大学仪器科学与光电工程学院，北京100191）

电子导盲辅助装置(ETA）是解决盲人出行困难的重要手段，而导航是ETA的关键技术。现有的ETA主要用GPS来定位定向，但在城市环境中经常存在GPS信号遮挡导致导航信息丢失的问题。针对该问题，利用视觉导航短时间内定位精度高，输出连续的优点以及MG(Magnetic Gravity）姿态测量可补偿姿态积累误差的优点，提出一种基于视觉、GPS和MG姿态测量的盲人行走组合导航算法。该方法构建系统误差模型并以Kalman滤波为框架。仿真和实验结果表明，提出的组合导航算法准确度优于单独的导航算法，满足盲人户外安全出行导航的需求。

盲人出行；视觉导航；GPS；加速度计；磁力计；组合导航；Kalman滤波

Abstract：ETA(electronic travel aids）is an important means to solve the difficulties of the blind travel，in which navigation is the crucial technology of ETA.The existing ETA mainly uses the GPS to get the position and the orientation，however，the navigation information is often lost because of occlusion in the urban.Aiming at the problem，this paper propo⁃ses an integrated navigation method of the blind walking based on vision，GPS and MG attitude measurement.The method takes advantage of the vision navigation which has high positioning accuracy in short time and continuous output，and MG attitude measurement compensated the attitude accumulation errors.The method constructs the system error model and uses Kalman filter as the framework.Simulation and experimental results show that the proposed integrated navigation algorithm is superior to the single navigation algorithm.

Key words：blind travel;visual navigation;GPS;accelerometer;magnetometer;integrated navigation;Kalman filter

0 引言

人类80%的信息都是通过视觉获得。盲人丧失了视觉，也就丧失了信息获取的最重要渠道，对盲人日常生活造成极大的障碍，尤其制约了盲人的户外出行活动［1］。盲人作为社会上的一类特殊群体，如何让他们安全独立的行走，成为当前研究的重要方向。

电子导盲辅助装置是帮助盲人安全出行的重要手段，主要解决盲人出行中避障和导航的问题。避障问题主要是盲人能够通过诱导准确绕过障碍物，导航问题主要是指盲人知道自身所处的位置以及行走方向。导航的准确性直接决定盲人能否使用导盲装置大范围地安全出行。

目前，导航定位技术包括绝对定位和相对定位两种。其中，常用的绝对定位技术有卫星导航定位［2］和无线电定位；相对定位技术有惯性导航［3］、激光定位、航位推测技术和视觉导航［4］等。为实现盲人大范围安全出行，须获得盲人的全局位置和行走方向。本文采用GPS绝对定位方法，GPS在信号受环境影响较小的条件下，具有定位精度高、定位全局性和高可靠的优点，能满足盲人基本定位的需求，但对于城市、峡谷等区域，信号容易受到干扰，GPS定位效果较差［5］，且GPS无法获得准确航向信息。相对定位方法具有小范围内定位精度高、输出连续性和导航参数全等优点，可实现对GPS的优势互补。广泛应用于盲人行走的相对定位技术有惯性导航和视觉导航。惯性导航具有短时间定位精度高和输出航向信息的特点，但其误差随时间迅速积累；采用较高精度的惯性器件，可以保证一段时间导航的准确性，但成本也高。成本较低的MEMS惯性器件则多采用行人航位推算方法，能保证一定程度的导航精度，但受行人行走模型的影响，并不普遍适用于盲人。视觉导航技术通过刚性挂载在目标上的摄像机采集图像数据，根据图像特征和前后图像帧间的运动约束进行位姿估计，具有短距离导航精度高的特性。但该技术也是通过累积计算的局部导航，故存在误差随距离累积的缺陷［6］。因此，需采用一种或多种相对定位技术来对GPS实现组合。

一般的GPS/INS组合导航方式虽然能在一定时间内有效提高定位精度，可是系统误差随着时间而逐渐积累，并且在遮挡严重区域又得不到准确的GPS定位结果来修正其初始化，最终导致目标定位失败。

MG姿态测量是指利用加速度计和磁力计实现载体姿态测量的方法，测量误差主要受载体运动状态影响，不存在误差随时间累积的缺陷。考虑到盲人用户的特殊性，本文提出一种基于视觉、GPS 和 MG 姿态测量的组合导航定位方法［7⁃8］。GPS具有定位误差不随时间积累的优点，同样MG测量姿态也不随时间积累，可实现与视觉导航的互补性，能在一定区域内实现盲人准确导航。

1 坐标系定义

本文涉及的坐标系均为右手坐标系，主要包括地球坐标系(e）、地理坐标系(t）、导航坐标系(n）、载体坐标系(b）和相机坐标系(c），如图 1所示。

图1 坐标系定义Fig.1 Coordinate frames

地球坐标系(Oe⁃XeYeZe）以地球质心为坐标原点，Ze轴指向地球北极，Xe轴指向地球赤道平面与格林尼治子午圈的交点。

地理坐标系(Ot⁃XtYtZt）以盲人所在点为原点，Xt轴沿当地纬线指向东，Yt轴沿当地子午线指向北，Zt轴沿当地地理垂线指向上。

导航坐标系(On⁃XnYnZn）是导航系统在求解导航参数时所采用的坐标系，通常选取地理坐标系。

载体坐标系(Ob⁃XbYbZb）为MEMS惯性器件坐标系，主要用于系统初始化。

相机坐标系(Oc⁃XcYcZc）以左相机光心为原点，Xc轴沿像素阵列指向右，Yc轴沿像素阵列指向下，Zc轴沿光轴指向视场方向。

通过确定各坐标系之间关系可实现不同导航方式之间的信息融合。

2 视觉导航原理

视觉导航主要通过特征检测、特征匹配、特征点跟踪和运动估计来实现相机坐标系相邻时刻的位姿变换［9］。考虑两个相邻的成像时刻tk－1和tk，时间间隔为Δt。设tk－1时刻相机载体的速度和角速度分别为。记tk－1到tk时刻匹配且跟踪成功的N个特征点的相机坐标为，tk时刻相机坐标系相对于tk－1时刻的旋转矩阵为，平移向量为。相机坐标系的旋转和平移运动如图2所示。

图2 相机坐标系的运动Fig.2 The motion of the camera coordinate system

由前后图像帧之间特征点关系可知，对第i个特征点有：

为实现视觉导航和GPS组合定位，需将tk时刻获得相机位置转换为导航坐标系下位置，因此要对初始时刻相机坐标系进行初始对准，可通过标定MG载体与相机之间的安装矩阵以及MG载体的初始捷联矩阵来实现。初始对准矩阵为：

故tk时刻视觉获得的载体的导航信息如下：

3 MG姿态测量系统的姿态解算

MG姿态测量器件主要包括加速度计和磁力计，只在载体处于匀速或静止状态时才能准确测量，当载体具有较大线加速度时，该方法误差很大［10］。盲人行走过程大部分可以看作是匀速运动状态，因此通过载体所在位置的重力场和磁场可解算得到载体当前位置的姿态，转换后可得相机当前位置的姿态。

设参数θ、γ、ψ为载体姿态的俯仰角、滚转角、航向角，导航坐标系与载体坐标系间的方向余弦矩阵为。

当载体处于匀速运动状态或静止状态时，当地重力加速度gb在载体坐标系下的投影可由加速度计三轴输出直接给出，表示为：

由于导航坐标系定义为东北天，所以导航坐标系下重力加速度为：

其中，g为载体所在处的重力加速度。在获得了当地重力加速度的准确值后，有：

由式(8）可解算得到：

式中，ψ为理论值。由于磁北极和真北极不重合，需要通过当地磁偏角进行修正。设当地磁偏角为d，航向角真值为：

4 视觉/GPS/MG姿态测量组合导航Kalman滤波设计

系统整体设计如图3所示，通过双目视觉导航的方式获得视觉的导航信息，GPS用于获得较准确的绝对位置，加速度计和磁力计得到姿态，在导航滤波器中对导航系统误差状态进行估计，最后对误差进行补偿获取高精度的导航结果。

图3 导航算法方案框图Fig.3 Diagram of the navigation algorithm

4.1 系统状态模型

组合导航系统以地理坐标系为导航坐标系，设盲人在导航坐标系下的位置误差为Δp＝［ΔxΔyΔz］，由于计算误差，用于计算的导航坐标系(系）与理想导航坐标系(n系）不完全重合，两个坐标系之间存在姿态角误差φ，将误差角写成φn＝［φEφNφU］T。选择位置误差和姿态误差作为系统状态向量：

系统位置误差的导数是速度误差：

速度误差可视为白噪声处理。

其中，φn＝［φEφNφU］T。

计算导航坐标系相对于相机坐标系的转动角速率，可表示为：

从而姿态误差角的微分方程为：

根据刚体转动定理有：

从而三轴加速度误差为：

相机姿态角速度误差视为白噪声。

综上所述，系统状态方程可以写为：

以上即为系统的状态方程。

4.2 系统量测模型

由图3可知，当GPS有效时，可以获取GPS的量测信息，这时导航滤波器进行时间更新和量测更新；当GPS无效时，只对导航滤波器进行时间跟新。本文组合导航系统中共有两组量测值：一组是位置量测值，即GPS给出的绝对位置和视觉导航推算得到的相对位置；另一组是加速度计和磁力计计算的姿态值和视觉推算得到的姿态信息。通过对量测值进行误差分析可以建立量测量和状态量之间的关系，导出量测模型。

(1）位置误差量测方程

GPS接收机和视觉导航的位置量测信息，可分别表示为：

式中，xr、yr、zr表示真实位置；δxE、δyN、δzU表示GPS的位值误差；δx、δy、δz表示视觉导航的位值误差。

位置量测误差模型为：

(2）姿态误差量测方程

视觉导航和MG姿态测量得到的姿态值可表示为：

式中，θr、γr、ψr表示真实的姿态，ϕx、ϕy、ϕz表示视觉导航姿态误差，ϕmx、ϕmy、ϕmz表示MG测量姿态的误差值。

姿态量测误差模型为：

通过把位置量测矢量和姿态量测矢量合并，得到系统观测方程：

(3）基于加速度计的自适应姿态量测噪声方差阵

由于利用MG测量姿态的方法受相机载体线加速度的影响很大，在高动态下会产生较大的姿态误差。为增强导航滤波器的稳定性，保证姿态精度，采用自适应的估计算法，利用姿态量测噪声矩阵Rφ的变化实时调整导航滤波器对量测信息的依赖程度［11］。

若系统无加速度，则加速度计的输出满足：

此时加速度计的输出矢量的模值可近似等于重力加速度，以此作为载体运动状态的判别依据，判别过程如下：

5 仿真和实验验证

5.1 仿真条件

为验证本文算法的可靠性，以盲人为对象进行仿真导航实验。通过轨迹发生器生成标准轨迹，视觉导航模块生成视觉导航数据，GPS模块生成仿真GPS数据，加速度计和磁力计模块生成姿态仿真数据。

仿真轨迹时间为400s，累计路程为168m，GPS水平位置均方误差为2m，加速度计和磁力计模块生成姿态角的均方误差为1°。

5.2 仿真结果

图4为仿真模拟实验的导航位置输出，视觉导航输出的位置误差随时间积累，而组合导航的位置曲线较接近真实值。

图4 仿真实验导航位置Fig.4 Navigation position of simulation experiment

图5和图6分别为北向和东向位置误差误差曲线，从图中可以看出单独的视觉位置输出存在误差累积，而组合后位置误差明显减小。

图5 北向位置误差曲线Fig.5 Error curve of the north position

图6 东向位置误差曲线Fig.6 Error curve of the east position

图7为仿真模拟实验的姿态误差曲线，视觉导航输出的姿态误差逐渐积累并发散，而组合导航能很好地校正姿态误差。

图7 仿真实验姿态误差Fig.7 Attitude error of simulation experiment

5.3 实验验证

为了验证本文算法的可靠性，采用实验室自行研制的穿戴式导盲装置实验平台，如图8所示，该装置搭载双目摄像头、MEMS姿态测量器件和GPS模块。

图8 穿戴式导盲实验装置Fig.8 Wearable guide experiment device

其中，MG姿态测量器件和GPS模块，采用Xsens公司的 MTi⁃G⁃700，该器件集成加速度计、磁力计和GPS。

本次实验场地为北航图书馆广场，实验场景主要是石泥板，实验路线为从原点向左沿矩形行走，最后回到原点处。实验结果如图9、图10所示。

图9 组合导航实验位置输出Fig.9 Location of integrated navigation experiment

图10 组合导航实验航向角输出Fig.10 Yaw angle of integrated navigation experiment

从图9中可以看出，随着行走距离的增大，视觉导航定位误差积累明显，GPS定位误差受信号稳定影响，组合后的导航定位精度较高，精度在5m内。图10表明视觉导航航向角输出误差积累发散，组合后的航向角输出误差明显减小，误差在10°范围内。

6 结论

本文提出一种基于视觉、GPS和MG姿态测量的盲人组合导航算法，该算法利用GPS和视觉导航定位的互补性以及MG姿态测量不随时间累积的优点，实现组合导航，有效解决了盲人安全出行的导航问题。仿真和实验结果表明，本文提出的算法优于单独的GPS和视觉导航系统，定位精度可达5m以内，姿态测量误差在10°范围内，满足盲人户外行走的导航要求。

[1]何婧,聂旻,罗兰,等.听觉引导助盲系统[J].生物医学工程学杂志,2010,27(2）:467⁃470.HE Jing,NIE Min,LUO Lan,et al.Auditory guidance systems for the visually impaired people[J].Journal of Bi⁃omedical Engineering,2010,27(2）:467⁃470.

[2]庄春华,赵治华,张益青,等.卫星导航定位技术综述[J].导航定位学报,2014,2(1）:34⁃40.ZHUANG Chun⁃hua,ZHAO Zhi⁃hua,ZHANG Yi⁃qing,et al.Overview on seamless positioning technologies of satellite navigation[J].Journal of Navigation and Location,2014,2(1）:34⁃40.

[3]高钟毓.惯性导航系统技术[M].北京:清华大学出版社,2012.GAO Zhong⁃yu.Technology of inertial navigation system[M].Beijing:Tsinghua University Press,2012.

[4]Lu W,Xiang Z,Liu J.High⁃performance visual odometry with two⁃stage local binocular BA and GPU[C].Intelligent Vehicles Symposium（IV）,2013 IEEE,2013:1107⁃1112.

[5]Helal A S,Moore S E,Ramachandran B.Drishti:an inte⁃grated navigation system for visually impaired and disabled[C].Wearable Computers,2001 Proceedings 5thInterna⁃tional Symposium on IEEE,2001:49⁃156.

[6]李宇波,朱效洲,卢惠民,等.视觉里程计技术综述[J].计算机应用研究,2012,29(8）:2801⁃2805＋2810.LI Yu⁃bo,ZHU Xiao⁃zhou,LU Hui⁃min,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8）:2801⁃2805＋2810.

[7]徐勇志,宁晓琳.一种巡视器惯性/视觉组合导航新方法[J].空间科学学报,2015,35(6）:721⁃729.XU Yong⁃zhi,NING Xiao⁃lin.A new INS/VNS integrated navigation method for planetary exploration rover[J].Chi⁃nese Journal of Space Science,2015,35(6）:721⁃729.

[8]夏凌楠,张波,王营冠,等.基于惯性传感器和视觉里程计的机器人定位[J].仪器仪表学报,2013,34(1）:166⁃172.XIA Ling⁃nan,ZHANG Bo,WANG Ying⁃guan,et al.Robot localization algorithm based on inertial sensor and video odometry[J].Chinese Journal of Scientific Instrument,2013,34(1）:166⁃172.

[9]张洋,王新龙.一种地标点修正的高精度双目视觉导航方法[J].北京航空航天大学学报,2014,40(9）:1305⁃1311.ZHANG Yang,WANG Xin⁃long.Landmark fixed high⁃precision binocular visual navigation method[J].Journal of Beijing University of Aeronautics and Astronautics,2014,40(9）:1305⁃1311.

[10]Bird J,Arden D.Indoor navigation with foot⁃mounted strapdown inertial navigation and magnetic sensors[J].IEEE Wireless Communications,2011,18(2）:28⁃35.

[11]刘兴川,张盛,李丽哲,等.基于四元数的MARG传感器姿态测量算法[J].清华大学学报（自然科学版）,2012,52(5）:627⁃631.LIU Xing⁃chuan,ZHANG Sheng,LI Li⁃zhe,et al.Quaternion⁃based algorithm for orientation estimation from MARG sensors[J].Journal of Tsinghua University（Science and Technology）,2012,52(5）:627⁃631.

Integrated Navigation Method of the Blind Walking Based on Computer Vision/GPS /MG Attitude Measurement

JIA Wen⁃yuan,WEI Tong
(School of Instrument Science and Optoelectronic Engineering,Beihang University,Beijing 100191）

U666.1

A

1674⁃5558(2017）01⁃01336

10.3969/j.issn.1674⁃5558.2017.05.003

2016⁃11⁃21

北京市科技计划项目(编号：Z151100002115022）

贾文渊，男，硕士，研究方向为计算机视觉和导航定位。