基于联邦滤波的室内多传感器融合导航定位方法研究

2021-07-30敖龙辉

测控技术 2021年7期

张逵，郭杭，敖龙辉

(南昌大学信息工程学院，江西南昌 330031)

近些年来，室内导航定位技术受到了广泛的关注，服务机器人已经成为机器人未来发展的重要方向，高精度的室内多传感器融合导航定位技术有着广阔的市场应用前景。目前，应用比较普遍的室内定位方法包括WiFi定位、蓝牙定位、LiDAR定位等[1-3]。然而这些方法受外部环境特征影响很大，例如LiDAR定位需要具有明显的外部环境特征，当外部环境特征不够明显时，便很难提供高精度的定位结果[4-5]。

针对单一的定位导航方法精度、鲁棒性、容错性不高的问题，也有许多研究者提出大量的融合方法，包括：IMU/激光雷达(LiDAR)、IMU/GPS、视觉SLAM/惯导等[6-8]。总的来说，融合导航定位技术能有效提高定位效果，但这些方法多采用集中式滤波，当系统局部出现异常时，滤波效果会受到很大影响。联邦滤波是一种分散式滤波，由多个子级滤波器组成。在部分子级滤波器异常时，系统仍能输出可靠的定位结果[9-12]。

不同的环境下，传感器的稳定性和可靠性会有很大的不同，甚至会导致定位结果失败[13-15]。因此，本文设计了一种基于联邦滤波的INS/3D LASER/单目视觉组合导航定位方法，利用分散式滤波的方式将IMU、3D LASER和单目视觉的信息进行分级融合处理。本文设计了混合联邦滤波器，通过最小二乘的思路解决了系统自适应问题，在滤波过程中使用最小方差准则和最优系数加权法处理数据，进而得到系统全局最优估计结果。

1 数据融合方法

联邦滤波能够对需要融合的信息进行块估计，然后分两步进行级联分散滤波。滤波系统由一个主滤波器和多个子级滤波器构成，主级滤波器和子级滤波器能够同时进行数据处理。由于惯性导航部分能够稳定地输出测量结果，故本文采用惯性导航与视觉SLAM的融合滤波器和惯性导航与激光SLAM的融合滤波器独立并行进行滤波测量更新，然后将各自融合处理的数据传输到主滤波器中进行全局最优融合处理。该并行分散的方法能够有效降低系统的融合处理计算量，同时也能够避免因某个子级滤波器出现处理错误而产生的错误信息，极大地提高了容错能力。本文的融合算法流程如图1所示。

图1 滤波算法流程图

首先构建惯性导航系统(INS)作为公共参考系统，其状态表达为

XINS=[δLIE,δλIN,δvIE,δvIN,φIE,φIN,φIU,▽E,▽N,εE,εN,εU]

(1)

式中，δLIE,δλIN为纬度、经度误差；δvIE，δvIN为东向、北向速度误差；φIE，φIN，φIU为平台航向角误差、横滚角误差和俯仰角误差;▽E，▽N为东向加速度计零偏和北向加速度计零偏；εE、εN、εU为东向陀螺仪漂移、北向陀螺仪漂移、方位陀螺仪漂移。

1.1 INS与3D LASER滤波

3D LASER技术是通过发射激光来获取物体表面的空间三维信息及物理信息，有着准确、快速和非接触性的特点。3D LASER状态变量为

XLiDAR=[δLL,δλL,δvEL,δvNL,δψL,δθL,δγL]

(2)

式中，δLL，δλL为3D LASER输出的纬度误差和经度误差；δvEL，δvNL为东向速度误差和北向速度误差；δψL为航向角误差;δθL为俯仰角误差;δγL为横滚角误差。假设噪声均值为0，且为随机过程，则INS/3D LASER组合导航系统状态方程为

(3)

W=[WδLI,WδλI,WδvEI,WδvNI,WδψI,WδθI,WδγI,0,0,0,0,0,WδLL,WδλL,WδvEL,WδvNL,WδψL,WδθL,WδγL]T

(4)

式中，W为均值为零方差为Q的白噪声；FINS为12×12矩阵;FLiDAR为7×7矩阵;

(5)

其中,相关时间τLL,τλL,τvEL,τvNL,τψL,τθL,τγL在100～200 s之间选取。假设位置误差、速度误差、姿态误差都是一阶马尔科夫过程。由于传统3D LASER模块无法实时输出高精度的姿态信息，一般的INS/3D LASER 组合系统仅能够采用速度、位置组合方式。随着3D LASER技术的发展，现在，高精度的姿态信息获取不是难事，本文将姿态与速度、位置三者信息进行组合。INS/3D LASER组合的量测方程为

(6)

式中，HLiDAR=-17×7,

(7)

量测噪声V是方差为R、均值为0的高斯白噪声，且V与W互不相关。

1.2 INS与单目视觉组合滤波

INS/单目视觉组合系统采用速度组合模式，将INS和单目视觉解算得出的载体速度之差作为观测量。选取状态量为

Xk2=[δLk2,δλk2,δvNk2,δvEk2,δφNk2,δφEk2,δφDk2,δk]T

(8)

式中,δLk2，δλk2为载体纬度误差、经度误差;δvNk2，δvEk2为载体北向和东向速度;δφNk2，δφEk2和δφDk2分别为载体北向姿态角度误差、东向姿态角度误差和天向姿态角度误差;δk为视觉相机标度因数误差。对于δk，本文用随机常数代替了常值误差，而随机误差用一阶马尔科夫过程表示，反时间相关常数设为1/τc。系统状态方程为

(9)

(10)

观测矩阵为

(11)

1.3 全局滤波最优估计

(12)

式中，a和b为需要确定的权重值。取k个时刻的测量数据进行最小二乘计算：

(13)

2 组合导航实验与分析

为了验证本文方法的可行性，实验采用了Turtlebot2移动机器人实验平台(由团队设计开发)进行传感器扫描和实验数据的采集，室内试验是在实验室内完成的。用于实验的机器人平台安装了9自由度惯性传感器(三轴加速度计、三轴陀螺仪和三轴磁力计)、Kinect2摄像头和R-fans16线激光雷达。IMU的采集频率是200 Hz，相机的采集频率是30 Hz，图像分辨率是640像素×480像素。实验环境位于办公室区域。实验平台、环境已经机器人路线规划布局如图2～图4所示。

图2 实验平台及环境

图3 折形轨迹定位图

图4 弧形轨迹定位图

本文实验比较了纯视觉里程计(DSO)方法、三维激光里程表方法和本文所提出的组合导航定位方法在相同环境下两种不同类型轨迹的定位结果。实验中机器人以5 m/min的速度保持匀速行驶，将机器人控制在预设好的路径上行驶40 m以上，这样可以保证定位时间在8 min以上，以考验系统对惯性测量部分产生的累积误差的消减能力。

图3和图4为3种方法在折形与弧形路径上的定位测试结果。图中黑色菱形表示起点和终点，黄色实线表示地面真实轨迹，绿色虚线表示融合方法的定位轨迹，红色虚线表示纯视觉里程计方法的定位轨迹，蓝色虚线表示三维激光雷达方法的定位轨迹。总体来看，纯视觉里程计的直接法(DSO)的定位轨迹偏离真值最多，3D LASER里程表方法与本文方法定位效果更加准确，但本文方法的定位轨迹更加接近真实路径值。实验中收集了8个控制点误差数值，对3种方法的平均误差和第95百分位误差进行了计算并制表，如表1和表2所示。由表1和表2可知，本文提出的方法具有更高的定位精度。