图像分析与传感器融合的室内导航定位系统设计及实践

王培云

(华芯拓远(天津)科技有限公司 天津 300457)

0 引言

步入21世纪，传感器技术和相应计算软件的开发正处于迅猛发展的态势，促使基于位置的服务(location based service,LBS)愈发进步[1]。该服务存在的最终目标是能够在任何时间和地点为用户提供精准的定位服务和导航功能，以解决在陌生地区方向辨认和路径规划困难的问题。

LBS由室外定位系统和室内定位系统两类组成。室外定位系统研究较为成熟，常用的包括全球定位系统(global positioning system,GPS)和我国自主研发的北斗卫星导航系统，这类系统在户外精度高、信号稳定性强，配合相应设备通常可达到亚米级。然而当处于室内、隧道等卫星信号衰弱甚至屏蔽的地点，定位将产生剧烈漂移，因此室内导航定位已经成为当前的一大研究热点。

在室内导航定位系统的研究中，目前主流研究思想是搭建无线传感器网络的实现方案，常用的方法包括无线网络[2-3]、蓝牙[4-6]和超宽带[7]等，但是这些方法通常受制于设备网络系统的搭建。为提高对于陌生环境的适应性，惯性定位导航逐步进入人们的研究视野。本文主要研究将图像分析应用于室内惯性导航系统中，采用多传感融合的思想提高室内导航定位的精度，实现其关键技术和应用。

1 惯性导航技术相关理论

惯性导航最初应用于战机的作战姿态计算，随着科技不断发展，目前该技术已经成为室内导航定位主流方法之一。其基本原理是使用多个不同种类的惯性测量单元(inertial measurement unit,IMU)收集被测目标的加速度、角速度等运动学信息，通过积分等计算求解目标的速度和位置，从而实现对目标进行定位的功能。IMU安装在被测目标之上，在工作时既不需要依赖于外界数据，也不需要向外界传递信息。其关键技术包括坐标系统换算、姿态更新方法、惯性测量单元和数据解算等。

1.1 坐标系统

在任何一种导航定位系统中，坐标系的建立都是实现定位的基础。导航坐标系即在测绘学中常用的N系，是导航解算的坐标系[8]。N系以运动物体本身为坐标原点，X、Y、Z三坐标轴分别指向东、北、天三个方向，因此也被称为东北天坐标系。载体坐标系，即B系，是运动物体结构形成的坐标系，其原点在物体的重心，由XOY组成一水平面，以右手螺旋法则确定Z轴的方向垂直于XOY面指向天。

1.2 姿态更新

针对被研究运动物体，其运动姿态是采用上述坐标系的转换和解算求得的，在惯性导航中，通常根据求解从B系相对于N系的改变。

由于B系和N系均满足右手螺旋法则，在坐标系中坐标轴两两互相垂直，因此坐标系的变换可以分为三种不同的类型。载体只有原点距离发生变化但未进行旋转，此时只需进行坐标系平移使两坐标系原点重合；载体只有坐标系间角度发生变化但原点没有改变，此时需进行某一坐标系旋转即可求得相对于另一坐标系变化；同时发生角度旋转和距离改变必须同时调整原点的位置和坐标系的角度，使用一个可表达目标运动姿态的矩阵进行姿态更新。

1.3 惯性测量单元

从上述基本原理可以看出，惯导测量的数据是目标加速度和角速度，由惯性测量单元实现。常用的IMU必须包含用于测量角速度的三轴陀螺仪和用于测量加速度的加速度仪，再通过运算求得运动目标的速度、空间位置等信息。

三轴陀螺仪是测定角速度的装置，其由两相同质量的块构成，两质量块速度相反，其基本工作原理是科里奥利力，角速度公式[9]为

(1)

式中，wm(t)为角速度真值；w′(t)为测量值；bg(t)为零点漂移；ng(t)为测量噪声。

三轴加速度计是一种惯性传感器，其是测定目标相对于载体坐标系的重力加速度和绝对加速度之和的作用，其测量值基本公式如下：

(2)

2 双目视觉图像处理设计

在传统的图像处理领域，一般是单目视觉，即采用单个图片采集设备采集到的图片，这种图片不具有纵深，一般只能获得一个相对于图片边缘的二维坐标，因此采用双目视觉图像处理。

2.1 图像处理流程

双目视觉图像处理的具体流程如图1所示，首先将每个相机的固有参数和两个相机之间的非固有变换进行离线校准，采用两个图像采集设备获取的图片后对每一张图片提取角点[10]，对相应角点进行匹配和光流法跟踪，最后对结果进行紧耦合优化。

图1 双目视觉图像处理流程

2.2 视觉残差计算

在紧耦合优化滑动窗口时，需要进行视觉残差计算，其是指在当前帧上某一特征点的投影误差。对于特征点i，在第b帧进行观察时视觉残差为

(3)

2.3 双目视觉方向检测

在室内环境中，视觉消失点与坐标系中坐标轴正交方向相同，通过这种正交约束，即可判断图像采集设备相对于室内环境的方向。为进行方向检测，本文采用Jana等人提出的预期最大化(expectation maximization,EM)算法进行计算[11]，并将上述正交约束引入以提高鲁棒性。

在EM算法中，通常采用梯度下降求目标函数，结果相对粗糙。本文使用高斯球体进行基于EM的方向检测估计。如图2所示，高斯球球心位于成像设备焦点，图像平面上每一条线段及其在真实世界中对应直线都是由该直线和投影中心定义的平面法线表示。平面和高斯球角点构成一大圆，在真实世界中平行线大圆相交于球体上一点。从中心到这个交点方向为消失方向，平行线投影相交点即为消失点。

图2 高斯球模型示意

从图2可以看出，消失点、图片采集设备方向、消失方向是具有几何关系的，设三个主消失方向分别为Vdi,i=1,2,3，通过EM对目标检索获取的线段lj聚类，被检索线段对应的法线为Nl，设Z是图像潜在变量先的簇，那么有相应的高斯分布[12]为

(4)

3 紧耦合的视觉-惯导数据融合设计

基于以上惯导原理和双目图像分析设计进行视觉惯导数据融合设计，实现步行者的室内导航。设计采用基于滑动窗口的紧耦合优化方法进行目标估计，将滑动窗口内的各变量整合在统一优化框架内，变量包括IMU零偏、速度、三维位置坐标、深度等。

3.1 紧耦合融合设计

在视觉惯导数据融合室内导航定位系统中，本文采用紧耦合设计方法，充分使用通过双目视觉单元所获取信息与惯性测量单元所获取信息进行对比，使用两者原始数据共同实现对于同组变量估算，从而追求更高定位精度，基本流程设计见图3。

图3 紧耦合系统设计

3.2 视觉惯导数据融合

在本系统设计中，采用紧耦合数据融合，滑动窗口内的状态向量为

(5)

把滑动窗口中视觉残差和惯导单元中预积分数据约束放在同一框架之下进行优化处理，最终就构成了一个非线性最小二乘问题见式(6)。

(6)

对于实时方向的估计更新，本文将从视频帧获得的方向信息与来自IMU的角度测量相结合，估计模型如式(7)～式(9)所示。

式中,errort+1为误差观测；H为测量矩阵。

在对于偏航角和俯仰角的估计中，可以采用从加速计提取的重力分量表示，再用上述方法融合。

由定理2.1—2.2可以看到,F(X)上的度量H与[0，1]中的度量ρ的形式联系非常紧密。那么H和ρ具体是怎么联系的?[0,1]剩余格上的逻辑度量ρ的性质是怎样影响度量空间(F(X),H)的性质?文献[6,8]详细的论证了(F(X),H)的拓扑性质与逻辑推理的鲁棒性分析之间的关系。如果我们研究清楚[0,1]剩余格上的逻辑度量空间的拓扑性质及其与拓扑空间(F(X),H)的关系,就可以对逻辑推理系统的扰动参数的选取和鲁棒性分析起到一定的指导作用。

式中,βg、αg分别为偏航角和俯仰角；xg、yg、zg分别为重力在每个方向上的分量。

3.3 边缘化信息

系统采集的数据随着时间不断增加，状态数也随之增长，计算复杂程度不断提高。为了提高计算效率，约束计算复杂性的同时尽量减少可用信息损失，本文采用边缘化先验法解决，对于无关状态标量进行消元整合。

高斯牛顿法求解非线性最小二乘问题公式为[13]

(15)

式中，Δxa、Δxb为状态变量，通过高斯消元消去CT，求得

(16)

从而舍弃状态Δxa，仅保留系统中的Δxb以提高计算效率。

4 实验测试及分析

为验证本文提出的基于双目视觉图像处理与传感器融合的室内导航系统技术性能，笔者使用了相同设备在相同环境下分别对视觉导航、惯性导航和本文提出方法进行了多组测试，并同时收集各个方法的信息。

4.1 测试仪器及环境

在测试中，图片采集设备使用机器人搭载奥比中光双目相机，帧率为30 fps；使用的惯性导航设备是NGIMU，帧率为60 fps，安装在足部。

图4 测试环境平面图及实景图分析估算

4.2 测试结果分析

在七次试验中，仅使用惯导系统和使用本文系统所计算得到的运动轨迹长度及相应误差评价指标见表1。

表1 运动轨迹长度及误差评价指标 单位：m

从表1可以看出，使用本文系统时平均值误差仅有0.22 m，精度可以达到分米级。将该系统的结果与单独惯导系统进行比较，可以发现单独惯导系统的整体距离数值偏低，这是由于在测试的室内行进路线中存在一直角转弯，一些研究表明[14-15]惯导系统在面向旋转运动载体进行目标姿态更新时存在转弯半径较实际距离计算变小的情况，而图像分析系统矫正了该问题的存在。两者绝对偏差平均值相近，但均方根误差本文系统远小于单独惯导系统，说明本文系统测试出的距离分布更接近真实数据。该实例证明，本文图像分析与传感器融合的室内定位精度基本可满足日常在室内的定位需求。使用视觉信息绘制运动图形的效果见图5。

图5 测试结果制图

图5中，黑色线段为双目视觉系统建立起的廊道二维平面，黑色曲线为人的实际运动轨迹，该轨迹采用北醒CE30-C三维激光点云系统精准构建，准确度可达厘米级，浅色曲线为本文方法所建立的测试者运动轨迹，该轨迹为7次测试结果采用Matlab拟合获得，制图软件为EVO轨迹评估工具。从图中可以看出，所建立起的廊道二维平面与实际平面图较为符合，同时测试者的运动轨迹也较为准确，接近实际的运动曲线，具有良好的导航作用。

5 结束语

本文提出了一种采用紧耦合的图像分析与传感器融合的室内导航定位系统，并在工作室楼层进行了测试。测试结果表明，通过图像分析与惯导融合系统能够在一陌生环境中有效建立起虚拟地图并提供相对准确的导航和定位服务，一方面弥补了惯导系统中需要先验环境的特点，另一方面解决了室内卫星定位信号弱的弊端，是一种有效的室内定位方法。