融合惯导信息的单目视觉室内定位方法

李梦珍,余敏,俞佳豪

(江西师范大学 计算机信息工程学院,江西 南昌 330022)

0 引 言

随着定位技术的发展,人们的生活也越来越便利．在户外,诸如车载导航仪、百度地图、滴滴打车等基于位置服务的产品及应用,已成为人们生活中不可或缺的部分[1];人们对于室内精准位置服务的需求同样强烈,比如消防救援时的人员定位,大型商场中的导购服务,医院、养老院的智能看护等[2]．

室内定位技术种类多种多样,同时定位与制图(SLAM)以其可在未知环境中自主作业、无需提前布设相关设备、传感器体积小、功耗低、信息获取丰富等特点,成为当前研究的热点．基于惯性测量单元 (IMU)的定位方法以实时性高、抗干扰能力强、成本低、功耗低的特性持续被室内定位领域所关注．

单目视觉SLAM作为视觉SLAM的一个分支,依靠单目摄像机获取的环境纹理信息作为信息源,其处理效果直接受环境纹理条件影响,难以在纹理贫乏、光线不足或过强的区域中可靠地工作[3]．而IMU具备完全自主的测量特性,可稳定获取姿态数据,能有效弥补视觉SLAM在纹理贫乏、光线不足或过强的区域中环境信息的缺失,提升系统定位的稳定性,同时高精度的视觉定位结果可以有效修正IMU存在的累计误差,提升系统定位的精度．因此,针对单目视觉传感器与IMU的数据融合技术研究成为了新的研究热点．

近年来,国内外在视觉与惯导的融合定位方面取得了较多的研究进展,其中基于滤波的单目视觉与惯导融合方法越来越多[4]．现有的技术多采用卡尔曼滤波进行数据融合[5],且直接将惯性组件测量的角速度与视觉信息融合．但是陀螺仪积分得到的角度随着时间的增加，积分漂移和温度漂移带来的误差比较大,且测量数据与预测数据间存在非线性关系．依据单目视觉与IMU融合现状,本文以单目视觉传感器与IMU数据为基础,开展了融合两种传感器数据的室内定位方法研究．首先利用四参数拟合模型将单目摄像机得到的图像数据处理成定位定姿数据;同时,引用互补滤波对陀螺仪读数进行修正;最后将两种传感器处理得到的数据作为观测值输入到扩展卡尔曼滤波器中,得到载体最优位置信息．系统架构如图1所示．

图1 融合定位方法架构图

1 惯性导航定位

1.1 基本流程

该定位方法是一种利用上一时刻位置信息与当前时刻位姿变化对当前时刻载体位置信息进行计算的定位方法．定位步骤如下:

1)初始对准获取载体的初始位姿;

2)通过惯性传感器测量载体的加速度值和角速度值;

3)对步骤2)中测得数据进行处理获得载体的速度位移、旋转角度．

4)利用上一时刻载体位置以及该时刻发生的速度位移、旋转角度进行航迹推算,获得载体当前时刻的位置．

1.2 初始对准

本文使用的加速度计、陀螺仪精度较低,对地球重力以及地球自转角速度不敏感,所以无法使用双矢量定姿进行对准．若传感器坐标系与载体坐标系重合,在载体不剧烈运动的情况下,可认为加速度计测出的加速度表示重力加速度,根据这一特性,可解算出俯仰角α和滚转角γ,计算方法如下:

(1)

式(1)便是利用加速度计解算俯仰角和滚转角的方法,其中ax、ay、az为三轴方向上加速度计的读数．对于航偏角β,由于手机中自带磁力计,可输出地磁信息,因此本文利用磁力计数据来计算航偏角,计算公式如式(2)所示．

(2)

式中:mx、my、mz分别代表磁传感器在三个轴上的输出．通过上述方法可以得到一个粗对准的结果,为了使对准结果更加精确,还要进行精对准,精对准一般是利用卡尔曼滤波实现的,本文参照文献[6]提出的一种把比力的东向和北向测量值作为观测量的方法进行了初始精对准．

1.3 平移矩阵解算

本文选用以载体重心为原点的“东北天”世界坐标系作为导航系,为了获取载体载在导航坐标系中的位置信息,我们首先需要将惯性组件所测量的加速度转换到导航坐标系中,转换公式为

(3)

获取三轴方向上相对位移的方法有很多,本文采用最简便的方法:对三轴方向上的加速度值进行积分．计算方法为

(4)

式中:Vm、Vn分别表示tm、tn时刻的线速度;at为tm时刻载体在某一个方向上的加速度;Sn为tn时刻某一方向上的位移;Sm为tm时刻对应方向上的位移;gt为对应方向上的重力加速度．

1.4 旋转矩阵解算

本文采用欧拉法表示旋转,因此解算出三个欧拉角是获取旋转矩阵的关键步骤．

1.4.1 积分法

对三轴方向上陀螺仪读数进行积分是最常用的获取三个欧拉角的方法,三轴上的角度计算方法为

(5)

式中:Gx、Gy、Gz为三轴方向上的陀螺仪的读数;C为采样频率．

1.4.2 互补滤波

加速度计的静态稳定性较好,但在载体运动时所得到的加速度数据相对不可靠,取瞬时值计算倾角误差比较大且无法测得航偏角;陀螺仪积分得到的角度不受载体加速度的影响,但是随着时间的增加积分漂移和温度漂移带来的误差比较大[7]．

为了得到更精准的姿态信息,本文利用互补滤波对陀螺仪读数进行修正．首先利用加速度计的读数计算出三轴方向上的重力分量;随后利用上一时刻姿态信息估计该时刻各轴上的重力分量;然后求出估算结果与实际测量的重力分量之间的误差;最终利用重力分量间的误差、比例系数对陀螺仪读数进行修正．计算方法为

(6)

其中:

(7)

(8)

式中:gx、gy、gz为修正后的陀螺仪数据;IntegralX、IntegralY、IntegralZ为三轴方向上陀螺仪的积分结果;ehx、ehy、ehz为利用加速度计读数估算出的三轴方向上的重力向量和实际测量的重力向量之间的差;sf为采样频率；kp为比例系数．对修正后的陀螺仪数据按式(5)进行积分即可获取三个欧拉角从而得到旋转矩阵．

2 单目视觉数据解算

根据坐标转换模型,若已知空间中不共线的3点在两个坐标系下的坐标,便能唯一确定两坐标系间的位置关系[9]．单目摄像头获取的图像信息可以构成一个像素坐标系,像素坐标系为二维平面直角坐标系,因此两个像素坐标系之间的转换可以用四参数拟合模型表示,其矩阵表示如下:

(9)

式中：x1、y1为特征点在前一时刻的像素位置;x2、y2为同一特征点在下一时刻的像素位置;Δx、Δy分别连续两场图像中的平移参数;θ指连续两像素坐标系间的旋转参数;k为两像素坐标系的尺度比．

令C=kcosθ,S=ksinθ,将式(9)展开,可得矩阵的线性方程组如下:

(10)

上述线性方程组可用式(11)的矩阵表达:

A×X=B，

(11)

其中:

利用最小二乘法可解算出矩阵X的值,从而可获取平移参数Δx、Δy．

随后利用C、S的计算结果及三角函数诱导公式,可得旋转参数θ的计算公式如下:

(12)

通过上述方式得到的是在像素坐标系下的位姿变化信息．为了能将导航数据与视觉数据有效融合,我们首先利用“张氏标定法”确立像素坐标系与世界坐标系之间的转换关系,然后将两幅图像中的像素坐标转换成世界坐标,最后利用上述原理对单目视觉数据进行解算．

3 数据融合

为了充分利用惯导、单目视觉传感器所得到的信息资源,发挥多个传感器的联合优势,消除单一传感器的局限性,越来越多的研究人员开始注重数据融合技术的发展．数据融合技术可在一定的准则下对两者及以上传感器得到的数据加以自动分析、综合、支配和使用,从而获取对被测对象的一致性描述,得到最优估计[10]．

卡尔曼滤波是数据融合常用的方法之一,它是一种利用线性系统状态方程对系统状态进行最优估计的算法．考虑到姿态数据及位置数据间的非线性关系,本文采用卡尔曼滤波的非线性版本—扩展卡尔曼滤波融合定位定姿数据．

本文中的多传感器信息融合基本原理如下:惯性传感器的观测方程更新后得到的系统状态量及系统协方差矩阵,将作为视觉传感器更新过程中的系统预测状态量和系统预测协方差矩阵进行姿态更新．将视觉传感器得到的更新后的系统状态量及系统协方差矩阵作为下一时刻的预测值进行迭代．

3.1 融合模型

扩展卡尔曼滤波主要分为预测和修正两个部分．预测模型主要通过上一时刻最优估计以及外部干扰因素来预测下一时刻的状态和协方差矩阵;修正模型主要通过测量值来修正估计结果和协方差矩阵．本文中所利用的模型公式如下:

预测模型:

xk+1=f(xk)，

(13)

pk+1=F×pk×FT+Q，

(14)

(15)

随后依据本文算法需求设置状态向量如下:

(16)

式中:px、py表示载体的位置;vx、vy表示载体的速度．

预测方程设置为:

(17)

由上述预测方程可知:预测过程是在载体做匀速运动的基础上进行的,因此速度成了本文中的干扰项．速度对应的解算方法如下:

(18)

(19)

修正模型:

xk+1=xk+K×(zk+1-h(xk))，

(20)

pk+1=(I-K×Hj)×pk，

(21)

(22)

(23)

本文中由于两种传感器所得观测数据不同,因此为两者设置不同的观测向量,视觉传感器的观测向量zm及惯性传感器的观测向量zi设置如下:

(24)

(25)

由于观测向量的设置不同,针对于惯性观测空间、视觉观测空间到状态空间的映射也不同．本文中设置惯导观测空间到状态空间的映射关系hi(x)以及视觉观测空间到状态空间的映射关系hm(x)如下:

(26)

(27)

式中,Ra、Rb的值为

(28)

4 数据融合

完成了前期的一系列研究工作后,根据姿态解算方法及数据融合方法,将单目视觉传感器和惯导的融合定位方法应用于室内定位中进行实验验证．

4.1 互补滤波算法验证

本文采用互补滤波修正陀螺仪读数,首先通过实验分析该方法修正后的陀螺仪读数积分结果与直接对陀螺仪积分结果之间的差异．

实验之前利用六位置标定法对所用到的加速度计与陀螺仪进行标定,标定后的加速度计与陀螺仪的输出模型如下:

实验利用华为MATE 8内置的加速度计、陀螺仪采集行人在变速行走过程中的加速度值与角速度值,之后利用采集到的数据计算行人航偏角,200 ms内的计算结果如图2所示,实线为加速度与陀螺仪互补之后得到的角度值,虚线为陀螺仪积分得到的角度值．从图中可以看出,两者得到的角度值存在一定的差异．

图2 角度值波形对比图

为了更直观地对比两者的差异,实验将手机水平保持固定角度为0°,随后进行变速运动．实验结束后分别求互补滤波得到的角度值、陀螺仪积分得到的角度值与实际角度之间的差值,最终得到的结果如图3所示.

图3 角度值误差对比图

从图中可以看出:陀螺仪积分得到的角度随着时间的增加积分漂移和温度漂移带来的误差越来越大;互补滤波利用加速度计的读数和上一次融合后的角度对陀螺仪的读数进行了修正,有效地抑制了陀螺仪的温漂,提高了行人航偏角计算的准确度．

4.2 融合定位方法验证

4.2.1 实验环境设置

实验选择在江西师范大学瑶湖校区先骕楼进行,实验场所是长、宽均为6 m的分布式实验研究室,室内有若干L型工作台、沙发、桌子等办公设置．实验路径规划如图4中的红线所示,从圆圈处开始到三角形处结束．

图4 实验路径选择图

4.2.2 融合定位方法验证

实验按图4所示的路径行走,在行走的同时利用华为MATE 8内置的加速度计、陀螺仪与单目视觉相机进行数据采集,在利用摄像头拍摄视频的同时,以0.01 s的频率收集手机加速度计与陀螺仪数据,数据采集过程用时约为1.5 min,总位移约14 m．

为了明确各种定位方法定位结果的差异,接下来分别采用IMU定位算法、单目视觉定位算法与融合定位算法进行了对比实验．图5为使用IMU定位过程中在27.05 s、47.50 s产生的效果图,图6为使用单目视觉定位过程中角点提取与轨迹绘制的效果图．

(a)27.05 s 定位效果图 (b)47.50 s 定位效果图图5 惯性导航定位实验

(a)特征提取效果图 (b)视觉定位建图过程图6 单目视觉定位实验

将IMU与单目相机所得数据利用扩展卡尔曼滤波及matlab进行融合仿真实验,得到行人最优位置估计,最后将三种定位结果绘制成如图7所示的轨迹对比图．

图7 实验轨迹对比图

由图可知:IMU定位方法在短时间内具有较高的定位精度,但随着时间的推移,轨迹偏差越来越大;单目视觉定位方法定位精度较高,但由于单目摄像头易受环境特征影响,定位轨迹在局部发生偏移;融合定位方法最终所得轨迹与实际行走轨迹重合度最高．

为进一步分析融合定位方法的有效性,求出1.5 min内各定位方法的最大定位误差,结果如表1所示．相比较于IMU定位方法与单目视觉定位方法,融合定位方法最大定位误差有所降低．

表1 定位误差 m

因此,融合定位方法能有效地融合IMU与单目视觉传感器所得数据,减少IMU定位所产生的累计误差以及单目视觉定位因环境特征变换较快而产生的跟踪丢失等不利影响,使得室内定位的准确性和稳定性得到有效提升．

5 结束语

本文利用扩展卡尔曼滤波方法对单目视觉传感器与惯性传感器数据进行融合以提高室内定位的准确性和稳定性．首先利用四参数拟合模型将图像数据转换成姿态数据,然后引入互补滤波对陀螺仪数据进行修正,滤除传感器温漂,提高数据源的质量,最后将姿态数据作为观测值输入到扩展卡尔曼滤波器中,得到载体的最优位置．实验结果表明,相较于IMU定位方法与单目视觉定位方法,融合定位方法能有效地降低室内定位误差,使得室内定位的准确性和稳定性得到有效提升．但本文在融合算法上,主要采用扩展卡尔曼滤波技术,扩展卡尔曼滤波在利用非线性关系时会引入线性误差,且计算复杂度比较高．因此,引入更好的传感器数据融合是下一步需要解决的重要问题之一．