基于激光扫描测距的机器人粒子滤波定位技术研究*

戈广双， 李子龙， 杨 凯， 马瑞鑫

(交通运输部 天津水运工程科学研究所，天津 300000)

基于激光扫描测距的机器人粒子滤波定位技术研究*

戈广双， 李子龙， 杨 凯， 马瑞鑫

(交通运输部天津水运工程科学研究所，天津300000)

传统的粒子滤波即时定位与地图构建(SLAM)算法在构建地图和目标进行自主定位时，粒子数量大，占用的内存高，重采样之后容易出现粒子匮乏现象，为了提高机器人自主定位的效率，提出了一种改进的重采样策略和粒子更新策略，融入系统模型。在装有机器人操作系统(ROS)的旅行家移动机器人上进行测试，实验结果表明：方法能够有效提升粒子滤波定位的效率。

即时定位与地图构建； 粒子滤波； 蒙特卡洛定位算法； 机器人操作系统

0 引 言

即时定位与地图构建(simultaneous localization and mapping,SLAM)是实现机器人自主行为控制的关键技术，机器人从任意位置开始，在移动过程中，用自身的传感器获取环境数据，构建增量地图，进行自主定位。在已知环境的自主定位研究已经取得了一些实用的方法，但在未知环境下，如何构建地图和利用构建的地图进行自主定位和导航，还有待进一步研究[1,2]。

粒子滤波(particle filtering，PF)算法基于概率统计的思想具有可逼近任意概率分布的特性，适用于移动机器人定位、地图构建。在此算法基础上，Montermerlo等人提出的FastSLAM算法降低了粒子数目和运算复杂度，是SLAM最具影响力的算法。宋凯楠等人[3]提出了一种用室内环境的特征点替代激光扫描数据进行ICP匹配的方法，减少了匹配时间，提高了SLAM算法的快速性。任春明等人[4]提出了一种机器人局部地图构建的新方法，采用加权最小二乘法对机器人在导航过程中的局部地图进行构建，具有实现容易、精度较高等特点。但为了很好地近似系统的后验概率密度，PF需要采用大量的粒子样本，样本数量越多，算法的复杂度就越高。因此，优化粒子的采样策略，减少粒子样本数量，同时避免重采样阶段造成的粒子有效性和多样性损失导致的样本匮乏现象为本文算法研究的重点[5～10]。

本文主要研究在已知环境下，利用激光雷达作为外部传感器，探索周围环境，使机器人进行栅格地图构建和自主定位，增强机器人的自主行为控制能力，针对粒子滤波算法复杂度高的问题，优化重采样策略、降低计算复杂度，通过选用合适的重要性函数，解决粒子匮乏和算法复杂度高的问题，并基于机器人操作系统(robot operating system,ROS)对优化的算法进行验证。

1 粒子滤波定位基本原理

1.1 激光测距扫描传感器数学模型

激光测距扫描传感器是在某一个平面上，按照一定的角度分辨率进行距离扫描，激光测距传感器的数据以极坐标的形式sn=(ρn,φn)T给出，则其直角坐标系表示为

zn=(xn,yn)T,n=1,2,…,N

(1)

式中xn=ρncosφn;yn=ρnsinφn;N为激光测距扫描传感器单个扫描周期的扫描点数。

(2)

1.2 机器人运动模型

为简化计算，设定机器人坐标系和激光测距传感器的坐标系重合，如图1所示。

图1 机器人坐标系与全局坐标系

机器人本体中通过监测轮胎转过的角度计算轮胎的移动距离，采用角速度陀螺仪角度积分监测角度的变化量，机器人移动姿态的增量为

un={Δsn,Δθn}T

(3)

式中 Δsn为里程计增量信息；Δθn为机器人角度变化量。

1.3 系统模型

地面自主移动机器人SLAM系统模型如图2所示，假定机器人在已知环境中移动，环境信息用mi表示，xt代表t时刻机器人的位姿状态向量，ut代表里程计数据，zt代表激光传感器的相对地图的观测数据。

1.4 蒙特卡洛粒子滤波定位原理

蒙特卡洛粒子滤波定位原理，用一系列样本(粒子)估计机器人的位置信息，用S表示

S={〈s[i],w[i]〉|i=1,2,…,N}

(4)

式中s[i]为第i个样本粒子的状态向量;w[i]为第i个样本的相关权重系数，w[i]为非零且和为1。粒子集概率分布

(5)

式中δs[i]为第i个粒子的狄拉克函数。

粒子滤波算法通过t-1时刻的粒子序列St-1估算t时刻的粒子序列St，计算过程由以下3步骤组成:

通常，步骤(1)中使用里程计信息创建粒子样本，运动模型简化为p(xt|xt-1,ut-1)。

步骤(2)中相关权重系数w[i]的取值通过观测量计算。假设p(s1:t|d)是需要被求解的相关粒子的后验概率，d代表相关数据,则

(6)

(7)

采用卡尔曼滤波计算粒子相对于地图的均值和方差

(8)

(9)

(10)

μt

(11)

(12)

式中

μt|t-1E{mt|z1:t-1,u1:t-2},μtE{mt|z1:t,u1:t-1}

∑t|t-1cov(mt|z1:t-1,u1:t-2),∑tcov(mt|z1:t,u1:t-1),Stcov((zt,ut)|z1:t-1,u1:t-2)

对机器人位置的求解，等价于求解粒子的联合后验概率，该联合后验概率可以分解为激光测距数据对已知地图观测量的后验概率和里程计在已知地图情况下的后验概率之积，在已知地图的情况下，求解机器人位置的递归公式为

p(x1:t|m,z1:t,u1:t-1)=ηp(zt|m,x1:t,z1:t-1,u1:t-1)×

p(x1:t|m,z1:t-1,u1:t-1)

(13)

式中x为机器人的运动轨迹x1:t=x1,…,xt;η为归一化因子;m为地图数据;z为观测信息，里程数据u1:t-1=u1,…,ut-1，观测数据z1:t=z1,…,zt。

粒子滤波算法1)输入:粒子集St-1,控制测量变量ut-1,观测量zt-12)参数初始化:S't=ϕ,重采样函数初始化,观测量函数初始化,状态方程初始化3)fori=1toNdo4)draws^s～π(st|s[i]t-1,d)5)^w=η[p(^s|s[i]t-1,d)][π(s|s[i]t-1,d)]-16)S't=S't+<^s,^w>7)End8)St=ϕ9)Forj=1toNdo10)从粒子序列S't获取样本粒子s[i]t,s[i]t的取用依据相关权重系数w[i]t的取值11)St=St+12)End13)returnSt

2 改进的粒子滤波定位方法

机器人位置的定位与机器人的姿态和传感器的测量数据高度关联，蒙特卡洛定位的思想是通过递归的方式以当前接收到的数据为条件，对机器人状态变量x进行估计，状态变量x在笛卡尔坐标系下表示为(x,y,θ)，相关数据分为两类：环境感知信息z和感知信息和机器人姿态更新信息u。移动机器人在导航过程中受到激光传感器自身的非线性和车体颠簸的干扰影响会造成其自身测量结果不准确，从而造成观测信息的误差；受到里程计、角速度陀螺仪积分存在误差的影响，也会造成里程及信息存在误差。因此,在改进的粒子滤波定位方法中，引入两类数据的误差参数，减小两类误差对系统运算的影响。

2.1 改进粒子采样算法

对粒子滤波定位中的粒子采样，分3步骤实现：

(14)

(15)

式中Q为前文推导出的激光测距仪噪声的协方差矩阵。

(16)

(17)

3)通过上述结果得出的新的粒子样本的分布

(18)

(19)

2.2 改进卡尔曼更新步骤

采用改进的优化方法[11]:

Fori=2 toNdo

else

End if

改进的卡尔曼滤波方式对粒子进行重采样，对权值高的粒子进行卡尔曼滤波，只需要对K(K

2.3 粒子滤波定位算法流程设计

改进算法流程如图3所示。

图3 算法流程设计

3 改进粒子滤波定位算法实现

实验平台为装载有SICK LMS2xx系列的激光测距机器人，该机器人由2个驱动轮和1个被动轮组成，安装有里程计，可以通过多通道的无线遥控器对机器人的行走进行控制，机器人本身配备了装有ROS的笔记本电脑，如图4所示。

图4 实验机器人平台

建立机器人自主移动环境，旅行家机器人在已知室内环境中做连续绕圈实验，由于实验环境的确定性，可以更好地检验算法的计算效率。实验环境为9 m×9 m实验室，将已知地图数据化，形成已知路标，建立全局坐标系，如图5所示。

图5 实验环境数据化

根据SLAM栅格地图构建原理，将实验环境数据化，定义室内环境全局坐标系，机器人移动速度0.5 m/s，记录机器人在室内的实际运动路径，与算法估算的路径进行比对，结果如图6所示。

图6 粒子分布

选取特征节点位置，以观测机器人定位性能，实验结果表明：改进前、后算法所需粒子数目为300，200个，运行时间为5.362，4.264 s。本文提出的改进方法能有效减少粒子滤波器的数量，提高SLAM算法的实时性。

4 结 论

对在已知地图环境下的粒子滤波定位技术进行了研究，对于在未知环境下的栅格地图构建和轨迹规划的研究比较薄弱，有待进一步研究。同时，加强自主行为机器人的多传感器数据融合、优化数据关联与回环检测、与前端机构处理器集成、提升鲁棒性和重定位精度，均为自主行为机器人SLAM技术接下来的发展方向，也是从单一机器人自主行为逐步向为多机器人协同工作发展的必要条件。

[1] 张 文,刘 勇,张超凡,等.基于语义建图的室内机器人实时场景分类[J].传感器与微系统,2017,36(8):18-21，28.

[2] 马润泽,余志军,刘海涛.一种距离无关的无线传感器网络定位算法[J].传感器与微系统,2011,30(11):131-134.

[3] 宋凯楠,王 彪,唐超颖.基于激光测距扫描的室内定位方法研究[J].激光与红外,2016,46(8):938-942.

[4] 任春明,张建勋.基于激光传感器的机器人地图构建方法[J].光电工程,2008(8):83-87.

[5] 张 毅,程铁凤,罗 元,等.鲁棒的机器人粒子滤波即时定位与地图构建的实现[J].计算机应用研究,2015,32(8):2339-2342.

[6] 刘洞波.移动机器人粒子滤波定位与地图创建方法研究[D].长沙：湖南大学,2013.

[7] 刘金华.基于粒子滤波单站无源定位跟踪技术研究[D].成都：电子科技大学,2008.

[8] 聂建亮,杨元喜,吴富梅.一种基于改进粒子滤波的动态精密单点定位算法[J].测绘学报,2010,39(4):338-343.

[9] 张 弦.基于改进粒子滤波算法的移动机器人定位[D].北京：北京邮电大学,2010.

[10] 涂刚毅,金世俊,祝雪芬,等.基于粒子滤波的移动机器人SLAM算法[J].东南大学学报：自然科学版,2010,40(1):117-122.

[11] 罗 元,余佳航,汪龙峰,等.改进RBPF的移动机器人同步定位与地图构建[J].智能系统学报,2015,10(3):460-464.

Researchonrobotparticlefilteringlocalizationtechnologybasedonlaserscanningranging*

GE Guang-shuang， LI Zi-long， YANG Kai， MA Rui-xin

(TianjinResearchInstituteofWaterTransportEngineering,MinistryofTransportation,Tianjin300000,China)

The traditional particle filtering simultaneous localization and mapping(SLAM) algorithm in the construction of mapping and autonomous positioning,usually need large number of particles and take up a lot of memory,particles appear deprivation after the resampling process,in order to improve the efficiency of robot autonomous positioning,aiming at particle filtering algorithm,propose an improved resampling strategy and particle update strategy,fuse system model.By testing the algorithm on a mobile robot equipped with robot operating system (ROS),the algorithm is validated by experimental method.The experimental results show that the method can improve the efficiency of particle filtering positioning.

simultaneous localization and mapping(SLAM); particle filtering(PF); Monte Carlo positioning algorithm; robot operating system(ROS)

10.13873/J.1000—9787(2017)12—0036—04

TP 242.6

A

1000—9787(2017)12—0036—04

2017—10—11

中央级公益性科研院所基本科研业务费项目(TKS160212)

戈广双(1989-)，硕士，主要研究方向为机器人智能控制技术。李子龙(1987-)，通讯作者，博士，主要从事工程机械自动化与工程机械无人驾驶技术研究，E—mail:zilonglibaifoshan@163.com。