基于激光雷达和超宽带的SLAM算法

夏 磊，郝 钢

(黑龙江大学 电子工程学院,哈尔滨 150080)

0 引 言

移动机器人同时定位与地图创建(Simultaneous Localization and Mapping，SLAM)可以描述为：移动机器人在未知环境下依靠传感器设备获取环境信息，并利用这些环境信息增量式地构建环境地图和自主定位[1]。SLAM最先由Smith R C和Cheeseman P提出，并结合扩展卡尔曼滤波(EKF)来解决移动机器人SLAM(EKF-SLAM)问题[2]。EKF-SLAM算法的实质是对非线性的系统模型和观测模型一阶泰勒级数展开，将模型线性化后再通过标准的卡尔曼滤波处理，这种方法实现简单，但是会引入较大的截断误差，并且计算量较大。EKF算法在执行时需要假设系统服从高斯分布，且存在线性化问题[3]，因此，文献[4]将无迹卡尔曼滤波(unscented Kalman filter，UKF)引入SLAM领域，在保证复杂度同阶的情况下，通过无迹变换计算估计SLAM问题中的后验密度。但利用UKF处理高维数(维数n> 4)系统时，滤波过程中协方差可能出现非正定情况，导致滤波数值不稳定甚至发散，需要“调整”UKF中的参数。为克服UKF在高维系统中出现的数值不稳定及精度降低问题，英国莱斯特大学的研究人员提出了基于容积卡尔曼滤波(CKF)的SLAM算法[5]，降低了估计误差，但是在递推过程中，存在计算量大、数值不稳定等情况。

随着粒子滤波(Particle Filter,PF)的出现及其在非线性系统中表现出的高估计精度性能，粒子滤波很快被用于SLAM系统(PF-SLAM)。PF-SLAM是将联合后验概率分布因式分解为机器人路径部分及以机器人路径为条件的地图部分[6]。在实际应用中，PF-SLAM算法由于环境中路标的增加会产生PF-SLAM算法状态向“级数灾难”问题，导致求解计算过程十分复杂。文献[7]将Rao-Blackwellised粒子滤波器应用于SLAM算法中，提出一种RBPF-SLAM算法。该算法将机器人定位和建图进行分解[8-9]，用粒子滤波器对机器人实现定位，卡尔曼滤波器进行建图。RBPF进行定位和建图需要较多的粒子，计算量较大，并且频繁执行重采样造成粒子退化。文献[10]基于RBPF-SLAM提出一种激光SLAM算法Gmapping，该算法已成功用于基于激光雷达的SLAM算法之中。由于里程计易受外部环境的干扰，该算法将高精度、鲁棒性较强的激光雷达测量数据加入到建议分布求取中，使建议分布更接近实际后验分布，Gmapping在重采样的时通过阈值判断是否进行重采样，从而减少了粒子数，提高了算法的准确度和效率。但随着场景增大所需的粒子增加，每个粒子携带一幅地图，在构建大地图时所需内存和计算量增加，Gmapping算法不适合构建大场景地图。

现有的基于激光雷达的经典的SLAM算法多数采用两段方式进行同时定位与地图创建，即定位-建图。定位阶段基于里程计或者惯性传感器(IMU)，结合激光雷达修正累积误差。建图阶段依赖定位数据，基于激光雷达进行地图构建。显然两个阶段存在耦合，不能很好地解决定位阶段产生的累积误差，以及定位误差带来的地图形变。基于以上原因，提出了一种基于激光雷达和超宽带(UWB)定位的SLAM算法，该算法通过采用UWB技术[11-12]和CKF滤波来获取目标的位姿。通过车载激光雷达获取路标点与目标的距离和角度，用泰勒展开更新观测方程，再结合容积Kalman滤波器(CKF)得到路标点的位置。该算法流程见图1。

图1 算法流程Fig.1 Algorithm flow chart

提出的系统采用UWB定位网络，避免了定位阶段的累积误差，为高精度建图提供了良好支撑。定位算法以及建图算法采用了CKF，降低了系统的计算负担。基于SLAM算法原理见图2，其中UWB基站为基站A、B和C，负责目标的定位；移动目标带有UWB标签和2D激光雷达，激光雷达可以根据检测的障碍物的距离R以及角度α构建地图信息。

图2 基于激光雷达和UWB的SLAM算法原理Fig.2 Schematic of SLAM algorithm based on Lidar and UWB

1 基于UWB和滤波算法的定位算法

1.1 CKF滤波算法

系统的状态方程与观测方程可描述为

(1)

其中，xk-1为在k-1时刻系统的状态；fk-1(·)为系统状态转移函数；hk(·)和zk分别为k时刻系统传感器的观测函数和观测值，且假设wk和vk为均值为零的互不相关的白噪声，Qk为wk的方差，Rk为vk的方差。

2009年，Arasaratnam提出了一种新的非线性滤波算法容积卡尔曼滤波(cubature Kalman filter，CKF)[13]。CKF根据容积准则，通过一组具有相同权重的点经过非线性系统方程转换后产生新的点来给出下一时刻系统状态的预测，避免了对非线性模型的线性化处理，其精度达Taylor级数展开3阶[14]，因此CKF较EKF的精度更高。CKF主要是由时间更新和观测更新两个部分组成：

时间更新：在k-1时刻的后验概率密度为

(2)

将Pk-1|k-1进行Cholesky分解可得

(3)

k-1时刻的容积点：

(4)

其中，点集ξi为

(5)

计算经系统方程传递后的容积点：

(6)

估计k时刻的状态预测值：

(7)

估计k时刻时的误差协方差阵：

(8)

观测更新：对Pk|k-1进行Cholesky分解：

(9)

计算新的容积点：

(10)

k时刻的观测预测值为

Zi,k|k-1=hk(Xi,k|k-1,0)

(11)

k时刻的观测值为

(12)

k时刻的自相关协方差阵为

(13)

k时刻的互协方差阵为

(14)

系统在k时刻的增益估计为

(15)

k时刻的状态估计值为

(16)

k时刻的误差协方差为

(17)

1.2 结合UWB和CKF的滤波定位算法

UWB技术是利用频谱极宽的超宽基带脉冲进行通信，具有低功耗、强穿透力、高时间分辨率和高传输数据速率等优势，而且可覆盖较大的范围。UWB技术主要应用在室内定位和测距领域，误差为10～30 cm[15-16]。

目前无线定位方法主要是通过已知坐标的定位基站和未知坐标的标签进行通信，再将得到的测距信息进行相应的解算从而得到标签的坐标[17-22]。本文UWB测距原理采用飞行时间测距法(Time of Flight,TOF)[23-26]中的双边双向测距(Double-side Two-way Ranging)方式[27]。基于测距的定位方法可分为三角解算与滤波解算两种。其中三角解算法是在获取测距信息后，按照数学公式对位置坐标进行解算。三角解算法优点是计算简单，实时性好，但由于传感器误差等因素，通常情况下3个基站的测距半径无法交于一点，需要估算。在没有更多先验概率的情况下，估计点经常位于几何图形的中心，显然是不合理的。而滤波方法可根据更多的先验概率(各个传感器误差方差等)合理地估计被测目标的位置信息，因此滤波方法得到的位置估计更加准确可靠。

将选用CKF滤波解算法对目标进行位置解算，考虑如下具有M个传感器的离散非线性随机系统：

xC(k+1)=f(xC(k))+ΓCwC(k)

(18)

(19)

(20)

其中，Ε为数学期望，δkt和δil为克罗内克函数，即δkk=1，δkt=0(k≠t)。

设平面定位系统有M≥3个基站，目标在k时刻接收到M个基站的测距信息，因此系统的测量方程为

(21)

式中，(x(i),y(i))为已知的第i个基站的坐标(i=1,…,M)；(xC(k),yC(k))为k时刻目标的坐标。采用集中式融合方式将各个传感器测量方程增广到集中式观测融合方程：

(22)

(23)

h(0)(xC(k))=[(h(1)(xC(k)))Τ,…,(h(3)(xC(k)))Τ]Τ

(24)

(25)

(26)

其中，diag(·)为对角矩阵。将式(18)和式(22)代入CKF滤波算法，可解算出目标的位姿。

2 激光雷达估计路标点位置

激光雷达具有极高的距离分辨率、角分辨率和速度分辨率，以及极强的抗干扰能力。2D激光雷达可测量周围障碍物的距离信息以及角度信息。UWB定位方法获取目标的位姿后，激光雷达可获取当前时刻目标在激光雷达检测范围内与路标点之间的距离和角度，并利用这两个信息定位路标点在世界坐标系下的位置。

设在世界坐标系下环境路标是静止的，因此，路标点定位模型为

xL(k+1)=xL(k)

(27)

(28)

(29)

(30)

(31)

由于vL为白噪声，且与wC相互独立，则vd与vL相互独立，式(31)可写为

(32)

路标点观测模型更新为

(33)

且vs(k)∈N(0,Rs)。将式(27)和式(33)代入CKF滤波算法，可解算出路标点的坐标。

3 仿真与分析

设目标在22 m×22 m的平面做匀转弯率运动(CT模型)。UWB定位系统共有3个基站，并设置了6个 路标点。目标的系统模型为

xC(k+1)=ΦCxC(k)+ΓCwC(k)

(34)

(35)

(36)

估计性能为k时刻路标点位置的累积均方误差(AMSE)：

(37)

图3 目标定位跟踪Fig.3 Target location tracking

图4 路标点两次估计AMSE对比Fig.4 Comparison of twice estimated AMSE for landmarks

4 结 论

提出了一种基于激光雷达和UWB的SLAM算法，该算法将整个SLAM过程分解为定位和建图两部分，采用两段容积卡尔曼滤波对目标和路标点进行估计，用UWB定位系统，结合CKF和集中式融合估计目标位姿；估计路标点时分为两种方法：①利用CKF估计路标点位置，②在CKF滤波估计的基础上对第一段CKF估计目标位姿产生的误差利用泰勒展开进行误差补偿。在相同的实验条件下，对这两种方法进行了仿真实验对比。仿真结果表明，引入误差补偿提高了估计路标点的精确度。