一种基于VINS/FINS组合导航方法

袁 珊,万 游,孟佳杰,汪雨婷,钱伟行,古翠红

(南京师范大学南瑞电气与自动化学院,江苏 南京 210023)

近年来,随着智能型机器人技术的快速发展,自主式移动机器人在各行各业逐渐显现出自己的作用和优势. 按移动方式进行划分,机器人可分为双足步行机器人、轮式移动机器人和履带式移动机器人等. 与其他种类移动机器人相比,双足步行机器人与支撑点的接触是非连续的,是目前的研究热点之一[1]. 但是要想普及双足步行机器人仍有许多问题需要解决,环境感知与导航定位问题尤为重要.

同时定位与地图构建(simultaneous localization and mapping,SLAM)在机器人导航定位领域有着举足轻重的作用[2]. 视觉传感器以较高的分辨率和相对较快的速度感知丰富的外部环境信息,但也存在着对运动变化敏感等问题,而惯性传感器(inertial measurement unit,IMU)可以提供高频率的精确运动信息,弥补视觉跟踪运动模糊的问题[3]. 当前研究的视觉惯性导航系统(visual-inertial navigation system,VINS)多使用集成度高的低精度IMU,其加速度计和陀螺仪的零偏较大,器件误差会对组合导航系统的定位精度造成影响[4].

基于MEMS的足部惯性导航系统(foot inertial navigation system,FINS)因为其成本低,体积小,重量轻且功耗低的特点,已成为行人导航定位领域的研究热点[5]. 双足步行机器人的外表酷似人体的结构和形态[6],因此也多采用足部惯性导航方式,其主要思路是通过对微惯性导航系统进行零速修正或航位推算的方式实现导航定位. 通过对系统误差状态量的可观测性分析发现,微惯性足部导航方式的航向角误差的可观测性弱,卡尔曼滤波器将估计其航向角误差. 因此,足部航向角误差将逐步积累,而无法通过零速修正的方式对航向角进行修正.

针对以上问题,本文研究了一种基于信息双向融合的VINS/FINS多传感器组合导航方法. 将视觉传感器和消费级惯性测量组件安装于机器人躯干部分,另一套器件精度较高的惯性测量组件安装于机器人足部,利用足部FINS系统零速修正后的位置信息实时修正躯干VINS系统中的低精度惯性器件误差,同时躯干VINS系统可以获得相对准确的航向角. 基于以上特性构建信息双向融合的组合导航系统结构,提高双足步行机器人的航向精度和导航定位精度.

1 组合导航定位方案及系统结构设计

本文提出的导航方案如图1所示.

图1 基于VINS和FINS相互辅助的定位系统工作流程图Fig.1 The principle chart of positioning system based on VINS and FINS

图1左边部分为VINS导航系统,由安装于躯干的惯性测量组件和视觉传感器分别采集惯性信息和视觉图像信息. 对图像特征点进行特征提取与匹配,之后进行位姿估计,利用相邻的地图点,寻找更多能够成果匹配的特征点,并选取关键帧完成地图构建,系统使用基于流形的IMU预积分思想进行惯性数据预处理,以解决两种传感器数据输出频率不一致的问题,从而完成视觉信息和惯性信息的紧耦合. 图1右侧部分为FINS导航系统,通过步态相位检测对机器人足部导航系统进行间断性的零速误差修正. 中间部分为误差修正系统,以躯干惯性信息误差方程作为状态变量,其状态量与足部惯导系统相同,前行过程中系统输出位置信息时,将FINS系统解算的位置信息作为基准,建立卡尔曼滤波器,此时VINS中惯性信息捷联解算得到的位置与FINS位置信息的差值即为位置误差观测量;根据躯干VINS系统提供的图像信息和惯导信息判断双足步行机器人的行走方式,当判断为直线行走方式时,使用VINS系统中精度较高的航向角替代FINS中的低精度航向角. 利用VINS/FINS导航系统不同的误差发散特性进行信息交互,完成导航信息双向融合,同步修正系统误差,提高双足步行机器人的导航定位与航向精度.

2 导航机理分析

2.1 基于紧耦合优化的VINS导航机理

纯视觉SLAM方案在工作过程中,通常采用匀速运动模型预测当前帧位姿. 当相机运动比较缓慢时,该模型算法有效,但是视觉范围内出现快速运动的物体或相机突然抖动,可能会导致跟踪失败[6]. 初始化是保证导航系统正常运行的重要前提,视觉和惯性信息的紧耦合优化是将视觉传感器对状态的约束量与IMU对状态的约束量来进行联合优化来估计机器人位姿[7]. 不同于松耦合方式的是将IMU和相机分别对自身运动估计后再进行融合. 而紧耦合方法通过将视觉约束和IMU约束联立进行非线性优化,更能达到传感器之间的优势互补的目的[8]. IMU可以为单目视觉提供尺度信息,视觉位姿可以矫正IMU的漂移问题. 本文采用的视觉惯性信息紧耦合优化方案框架如图2所示.

图2 视觉惯性紧耦合方案框架Fig.2 Visual/inertial tight coupling scheme framework

采集视觉传感器图像信息,提取具有方向信息的多尺度FAST特征点,并使用32字节的BRIEF作为描述符,前后帧图像中的特征点进行快速匹配,通过匹配特征点满足的极线约束关系建立前后帧图像几何关系,从而实时获取相机初始位姿信息. 惯性测量组件提供角速度和加速度的离散时间样本,其测量频率通常远高于视觉测量频率,IMU预积分的目的在于将相邻两个视觉关键帧间的IMU测量值进行统一积分得到运动状态的相对增量,避免因为之前状态量的变化引起的重复积分问题. IMU预积分理论建立在欧拉积分基础上[9-10],记i时刻和j时刻相邻两帧IMU观测数据的时间间隔为Δt,并定义相对旋转增量ΔRij、相对速度增量ΔVij和相对位置增量ΔPij,由加速度测量数据aB和角速度测量数据wB计算得到的预积分项如下:

(1)

式中,下标a和g分别表示IMU中的加速度计和陀螺仪,bg和ba分别为两个传感器的零偏,ηa和ηg为测量高斯白噪声. 上标k表示当前时刻,Gw表示世界坐标系下的重力矢量,RwB表示IMU系到世界坐标系的旋转矩阵的转置.

通过控制局部地图规模大小来控制非线性优化的计算量,定义视觉惯性紧耦合优化方案中的各误差代价函数,主要分为3部分误差:视觉重投影误差、预积分误差和由于滑动窗口产生的先验信息. 通过最小化各测量误差马氏距离的平方和可以得到系统的最优估计:

(2)

2.2 基于零速修正的VINS导航机理

基于惯性测量组件足部安装方式的双足步行机器人导航系统,以捷联惯性导航为主提供导航信息,通过机器人足部运动信息进行零速检测触发基于卡尔曼滤波的零速修正,补偿导航系统误差,并通过四元数算法提供机器人行进过程中的姿态信息[11-12]. 机器人处于不同步态时,系统通过足部加速度计和陀螺仪的输出,设计相应的阈值,对机器人足部运动进行零速检测.

零速检测算法由以下4个条件组成,C1、C2、C3和C4分别是根据加速度计的矢量和、加速度计Z轴的值、陀螺仪的矢量和和陀螺仪Y轴的值来判断是否满足零速的条件.C1=1表示满足该条零速检测的条件,C1=0表示不满足该条零速检测的条件,C2、C3和C4类似. 对于每个时刻的IMU信息,零速检测条件为C1&C2&C3&C4,即4个逻辑条件必须同时满足,才能判断足部是静止的.

(3)

(4)

(5)

(6)

2.3 VINS/FINS信息双向融合导航机理

采用足部惯性测量组件进行机器人定位时,由于航向角误差的可观测性很差,卡尔曼滤波器不能精确地将航向角误差估计出来[13]. 视觉/惯性组合导航系统通过相机图像读取、视觉里程计、后端优化和回环检测等步骤后可以获得比较准确的航向角[14]. 另外,视觉/惯性组合导航系统的IMU多为低精度传感器,其加速度偏差较大. IMU初始化阶段,若忽略加速度偏差,将其加到重力加速度中,容易导致重力加速度发散,进而导致初始化失败,VINS导航系统不仅无法获得准确的初始位姿,还会为非线性优化带来错误的IMU约束,IMU的器件误差很有可能导致VINS导航系统无法定位[15].

为了提高视觉/惯性组合导航系统中的IMU器件精度,本文以躯干惯导误差方程作为状态方程,其状态量与足部INS相同. 机器人行进过程中每当系统输出位置信息,以足部INS位置信息作为基准,将VINS中的惯性信息捷联解算得到的位置与足部INS位置信息作差值即为位置误差观测量,建立卡尔曼滤波器.

卡尔曼滤波器的量测:

Zk=(δL,δλ,δh).

(7)

式中,δL、δλ和δh分别表示经度、纬度和高度误差观测量.

定义导航系统的观测矢量:

(8)

式中,λb,Lb,hb分别为VINS中的较低精度惯性器件提供的经度、纬度和高度,λf,Lf,hf分别为FINS提供的经度、纬度和高度,Z(t)为t时刻的观测矢量,H(t)为t时刻的观测矩阵,X(t)表示t时刻的状态量,Nf(t)为t时刻的观测噪声.

首先利用视觉/惯性组合导航系统中的加速度计和陀螺仪采集的惯性信息进行导航解算,更新15维状态量和相应的四元数,根据状态量和四元数推算出卡尔曼滤波器的状态转移和协方差矩阵. 同时,每当FINS导航系统和躯干消费级惯性测量组件经捷联导航解算后输出位置信息时,将二者的位置作差作为系统的位置误差观测量,计算卡尔曼增益,修正躯干惯性测量组件捷联导航解算后的速度、姿态转移阵及位置误差和躯干消费级惯性测量组件的器件误差. 通过提高躯干IMU的器件精度,即扣除加速度计的零偏,使躯干VINS导航系统能成功初始化,获得准确的初始位姿并且在非线性优化阶段提供正确的惯性测量约束. 同时通过VINS组合导航系统的图像信息和惯性信息判断行人的行走方式,当行人为直线行走时,用VINS系统的精度较高的航向角替代足部惯性导航系统的低精度航向角. 通过以上步骤,完成两种导航信息的双向融合.

3 组合导航系统的性能实验与分析

本文实验中安装于双足步行机器人躯干部分的传感器为小觅S1000-120-ir,主要包括视觉传感器和消费级惯性测量组件,单目相机的帧率是20 fps,分辨率是640*480,IMU的数据采集频率为200 Hz. 安装于双足步行机器人足部的惯性测量组件的型号为XSENS MTI-300. 通过采集静态IMU数据,进行静态数据分析,得到两组陀螺仪和加速度计的零偏稳定性如表1所示,MTi-300为工业级惯性测量组件,其陀螺仪和加速度计的零偏值较小,说明其器件精度明显优异于小觅S1000-120-ir中的消费级惯性测量组件. 实验采用的计算机配置为i7-9750U,2.60GHz CPU,8GB RAM,操作系统为Ubuntu16.04,传感器具体安装位置如图3所示.

表1 MTi-300与小觅S-1000中惯性测量组件的基本性能参数Table 1 Basic performance parameters of inertial measurement unit in mti-300 and Xiaomi S-1000

行进路线选取某地下停车场的一条闭合路线,并使实验起点与终点重合,实验环境及行进路线如图4所示,实验总路程约为220 m,除了拐弯处,其他路段均保持直线行走,标准轨迹由激光测距仪和人工测量得到. 采用如下导航解算方案验证基于VINS/FINS导航信息双向融合方案的有效性:

图3 传感器安装位置示意图Fig.3 Installation position diagram of sensor

图4 实验环境及路线图Fig.4 Experimental environment and roadmap

图5 误差修正前后轨迹结果对比图Fig.5 Comparison of positioning results before and after error correction of VINS system

(1)验证VINS系统中的低精度IMU器件误差得到修正,将修正前后的VINS系统在同一条闭合路线上进行实验,实验结果如图5(a)所示. 实验选用的开源VINS方案为Mur等[3]于2017年提出的VIORB-SLAM. 由于单目VINS的器件误差较大,导致初始化失败,初始化阶段无法获得准确的初始位姿,非线性优化阶段的IMU约束为VINS带来较大的误差,行进轨迹如图5(a)中点划线所示,最大的定位误差为 5.3 m,占总行进路程的2.4%. 以足部惯性导航系统的位置信息为观测量,估计VINS中的IMU的加速度零偏,当加速度零偏被扣除后,加速度计的精度得到提高,单目VINS中低精度IMU误差得到修正后,系统的定位精度得到明显提升,最大定位误差为3 m,占总行进距离的1.3%. 如图5(a)中虚线所示. 说明本文所研究的器件误差修正方法可有效提高VINS系统的定位精度.

(2)验证FINS系统的航向角误差得到修正,将修正前后的FINS系统在同一条闭合路线上进行实验,实验结果如图5(b)所示. 实验中选用的改进前的FINS系统为赵小明等[5]研究的鞋式惯性导航系统,FINS系统的航向角被修正之前的定位曲线如图5(b)中点划线所示航向角被修正之前,其航向角误差较大,并且随着时间的延长逐渐增大,最大航向角误差为4.2°. 在直线行走阶段,利用单目VINS的航向角替代足部惯性导航系统的航向角,定位曲线如图5(b)中虚线所示,最大航向角误差为1.1°. 实验说明本文所研究的航向角修正方法可有效提高FINS系统的航向角精度.

图6 不同方案轨迹对比结果图Fig.6 Trajectory comparison of different schemes

(3)为验证VINS/FINS导航信息双向融合方案的有效性,将误差修正前的VINS系统和FINS系统与修正后的VINS/FINS组合导航方案在同一条路线上进行实验,实验结果如图6所示. 在实验结束时刻VINS/FINS组合导航方案定位误差约为1.8 m,占总行进距离的0.82%,最大航向角误差为0.9°. 由图6可知,与VINS和FINS方案相比,本文研究的VINS/FINS双向信息融合方案导航轨迹波动较为平缓,鲁棒性更高,且定位精度和航向精度有了明显的提升,综合验证了本文研究方案的有效性.

4 结论

本文研究了一种VINS/FINS导航信息双向融合的多传感器组合导航方案. 首先,针对FINS导航系统的航向角误差逐渐累积导致的航向角误差较大的问题,提出了在某种行走方式下,通过VINS组合导航系统的航向角替代足部惯性导航系统的航向角的方法. 其次,针对VINS组合导航系统的IMU器件精度差的问题,以FINS导航系统的位置信息为观测量,构建卡尔曼滤波器,以估计并修正VINS导航系统的低精度IMU器件误差. 最后,设计了双足步行机器人多传感器组合导航系统的性能验证实验,综合验证了本文研究方案的有效性.