基于激光扫描和迭代贝叶斯策略的定位

许亚芳，孙作雷＋，曾连荪，张 波

（1.上海海事大学 信息工程学院，上海201306；2.中国科学院 上海高等研究所，上海201210）

0 引 言

机器人在运动过程中由于地势不平等原因容易出现打滑等现象，而里程计往往无法准确记录这些信息，在进行轨迹预测时会产生较大的误差［1－3］。因此，需借助外部传感器获取的观测信息做进一步修正。多数学者采用扫描速度快、定位精度高、成本低廉的激光传感器提取环境信息，但他们的研究多集中于室内线特征或角特征等的提取［4－6］。对室外环境而言，特征的选取和检测更具挑战性，选用如校园、公园等半结构化环境中的树干、道路等物体作为路标，为机器人导航提供有效信息，已成为近年研究的趋势［7，8］。

从统计学角度看，机器人定位是一种在线滤波问题，即给定一系列观测值，估计机器人的当前状态［9］。贝叶斯数据融合算法从概率上解决了机器人自治问题，许多现存方法在处理非线性系统问题时，多采用线性化方法，比较通用的为基于扩展卡尔曼滤波 （extended Kalman filter，EKF）的方法，该方法利用建立的运动模型和观测模型，通过时间更新和观测更新递归求得机器人各时刻的位姿，且其收敛性也已得到很好的证明［10，11］，在理论上提供了可靠的解决方案，但该方法存在的线性化所带来的误差问题，使其在应用中受到了制约。

针对机器人定位中的上述问题，本文结合激光扫描数据与里程计信息，采用迭代贝叶斯数据融合策略，从非线性最优角度出发，通过一系列线性点逐步接近最优解，以提高机器人的定位精度，抑制算法的发散现象。

1 基于激光点云的特征提取

1.1 特征描述

室外环境难以用一种或多种参数化的几何形式表示，通常选取易于辨识且具有区分度的树干作为检测对象。针对树干的形状，将其近似为圆柱体处理。当激光扫描仪离地面的高度确定时，可将树干等效为圆特征，如图1所示。从传感器发射的激光束经由树干的表面，再次返回传感器。根据激光的TOF （time－of－flight）原理，可以得到关于树干表面的距离和角度的原始数据信息［12］。通过特征提取算法可估算圆心位置及对应的半径，进而确定树干相对于传感器的具体位置。

图1 等效为圆特征的树干

1.2 特征提取方法

室外环境中，激光扫描仪的观测距离是1 m－80 m，扫描区域180°。获得的扫描数据为距离－角度信息，根据扫描点个数N＝361及对应的角分辨率0.5°，可计算得到每个扫描点的极坐标形式，表示为Sn＝ （dn，θn）T，n＝1，2，3，…，N。对于树干的检测，首先采用点簇聚类分割方法将激光点分类，再利用求平均的方法计算每个分类区域中对应树干的位置信息及直径。以一组激光点簇为例说明：

聚类分割将相邻激光点的距离和对应的角度分别作差，找出相邻距离差或相邻角度差大于预定阈值的激光点。以符合上述条件筛选得到的激光点为分割界限，将一组数据中的361个激光点分成不同的区域Ai（i＝1，2，3，…，m），每个区域中包含一系列的激光扫描点 （（d1，θ1），…，（dn，θn），…，（dj，θj）），其中n＝1，2，…，j，（d1，θ1）是该区域的起始点， （dj，θj）是结束点。同时，计算起始激光点和结束激光点的笛卡尔坐标

具体分割方法描述如下：

步骤1 从原始数据中找出距离小于threshold＿range（设定为80m）的扫描点。若有符合条件的扫描点，计算并保存扫描点的距离和角度数据。执行步骤2；否则，将该组数据判为无效的干扰数据。

步骤2 分别将相邻扫描点的距离和角度作差。通过设定门限，从结果差中找出距离大于threshold＿range2 （设置为1.5m）或角度大于threshold＿bearing2 （设置为5°）的扫描点，确定属于同类区域的首尾扫描点，并依据式（1）计算保存首尾扫描点的笛卡尔坐标，执行步骤3；

步骤3 对相邻分类区域同样按照设定角度和距离门限的方法进行筛选。剔除几何距离较近和被遮挡的树干所对应的区域。

步骤4 对满足以上步骤的分类区域内的首尾扫描点进行筛选，选择对应直径小于1m 的区域。如区域Ai的激光扫描点对应树干直径的近似计算公式为

这里记L＝ （d1＋dj）／2，表示物体到传感器的平均距离。至此，经过以上筛选后得到符合条件的扫描点即为树干返回的激光扫描点。

位置估算：经过聚类分割后，在机器人坐标系中，每个分割区域中的激光扫描点可依据图2所示模型提取圆柱形路标的距离－角度信息。图中XOY 和XvOvYv分别表示世界坐标系和机器人坐标系，小圆点表示特征圆的圆心，以该原点为圆心的大圆表示树干特征。由图2可知

根据上式可得到

图2 特征位置估算模型

这里ri代表树干的半径，L 是激光传感器与扫描点之间的最小距离。求出的ρ和α 就是从原始激光数据中提取的树干特征相对于传感器的距离和角度。

图3为对某一组原始激光点的特征提取结果。机器人位于坐标 （0，0）处。

图3中原始激光点用圆点表示，从原始激光点提取出的特征圆用圆圈表示，而加号表示特征的圆心，为更清晰地观察提取的结果，选取其中两个提取点进行放大。左下方图中的圆圈表示根据分散于圆周围的激光点计算得出的特征的轮廓。右下图中只有激光扫描点，说明该区域的激光扫描点为干扰点。

图3 特征提取的结果

2 迭代贝叶斯数据融合

2.1 基于信度网络的贝叶斯数据融合

机器人定位问题可以等效为一个无环的信度网络模型［13］。在该模型中，包含一系列条件独立的变量，每个变量只依赖于其前一时刻的变量。xt表示机器人在t 时刻的状态，u1：t表示从时刻1到t的控制量，z1：t和zt分别表示从时刻1到t的观测量和当前t时刻的观测量。那么，当机器人在t时刻执行了动作ut后，融合t时刻的观测信息，继而得到关于信度网络模型的后验概率模型为

这里

由上式可知，计算后验概率需要3 个概率分布，即运动模型P（xt｜xt－1，ut）、观测模型P（zt｜xt）和机器人初始状态的先验概率P（x0）。

2.2 迭代策略

根据信度网络模型及数学理论，采用最大似然估计方法可求得后验概率的最优解

为研究机器人定位问题，所使用的机器人系统为含噪的运动模型和观测模型。假设机器人的初始状态满足高斯分布，描述里程计传感器的运动模型为高斯测量模型

式中：wt——均值为零，方差为Qt的高斯白噪声。若观测模型也为高斯测量模型

式中：vt——均值为零，协方差为Γt的高斯白噪声。

将过程模型和观测模型代入式 （8）可得到

在xi（即xt的第i个迭代值，为简化表达，以下均省略关于时刻的下角标）处，对J 进行二阶泰勒级数展开

这里Ji、梯度和海塞矩阵Jixx具体表达如下

对式 （12）求偏导并使之为零，可求得式 （12）的最小值，求解结果如下

其中

这里令Hi），表示测量方程h（·）在第i个迭代估计点＾xit的雅克比矩阵。使用矩阵逆准则可得到

将式 （16）和式 （17）代入式 （15）得到状态的迭代公式

迭代策略的预测过程和传统的贝叶斯滤波的预测过程是相同的，不同的是更新过程，描述如下：

For i＝0，N－1

依次执式 （18）、式 （19）

End

执行式 （19）

实际系统中运动模型和观测模型通常是非线性的，通过线性化方法无法获得最优线性化点，进而无法保证获得的值为最佳收敛值。采用非线性最优方法的迭代策略，可通过每次迭代过程获得的线性化点逐步接近最优解，最终达到降低系统定位误差目的。

3 实 验

利用Jose Guivant等录制的Victoria Park数据集［14］进行实验验证。该数据集中，将环境中的树干作为自然特征，通过人工驾驶车辆在指定路径中采集车辆及树干信息。实验车上配备了GPS、里程计传感器和激光扫描仪。其中GPS用于获取车辆的实际运动轨迹，以评估定位算法的性能；里程计测算车辆的速度和航向角，用于推算车辆的航迹；激光传感器用于感知路标在机器人坐标系中的距离－角度信息，为车辆定位提供观测信息。由图4 可以明显看出，由于公园地势、环境噪声等原因使里程计的测量结果 （用实线表示）和GPS的定位轨迹 （图中点线表示的不连续的路径）严重不符。这表明，仅仅依靠里程计信息不足以正确地定位机器人的位置，甚至会导致推算轨迹的严重发散。

图4 GPS定位轨迹和里程计的航位推算轨迹

这时结合激光扫描仪传回的原始激光数据，利用本文所述特征提取方法，提取有效观测信息。为简化信息融合过程，舍弃了特征的半径信息，仅保留特征的圆心位置信息，即将树干等效为点特征处理。选取传统贝叶斯滤波（EKF）和所提迭代贝叶斯策略融合里程计信息和观测信息。将过程噪声的标准差分别设为：σx＝0.2m，σy＝0.04m，σφ＝2°，将观测噪声的标准差分别为：σρ＝0.3m，σα＝0.3°，图5中的左图为传统贝叶斯算法 （EKF）生成的估计轨迹 （用实线表示）和地图 （用加号表示组成地图的特征点），右图为迭代贝叶斯算法 （IEKF）估计的结果（同样使用实线和加号分别表示估计轨迹和地图中的特征点）。从两图的比较可以看出，两种算法在右侧部分的路径均与真实路径相符合，这是由于实验车重复在此运动并获取观测信息，形成很多闭合环路，经过反复地数据融合，使得定位的精度越来越精确。而在两图的左侧部分，EKF估计的路径与实际GPS定位的路径相比，出现了明显的偏差；在通过迭代数据融合方法进行改善后，轨迹的定位结果有了一定的改善，且从卫星地图上看，估计的特征点几乎全部落在了相应的树木上。具体可参见本实验的配套视频：http：／／www.dwz.cn／iekfonVictoriaPark。

图5 估计的路径及特征

继续调整系统的噪声，可看出两种算法对噪声的敏感程度。当过程噪声调整为最初的3倍时，得到如图6所示的定位结果。图6 （a）使用传统贝叶斯滤波进行定位，得到左侧轨迹已明显向左偏移，相比之下，文中所提算法估计的结果仍然与真实路径相符。当调节至最初噪声的5倍时，如图7所示，传统算法的估计结果已出现了严重的发散现象，迭代贝叶斯滤波算法虽然出现了局部的偏差，但相比传统算法，已较优地控制了定位误差。

图6 估计的路径 （过程噪声为图5的3倍）

图7 估计的路径 （过程噪声为图5的5倍）

4 结束语

本文从原始激光扫描数据中提取有效的观测信息，通过迭代贝叶斯数据融合方法，将其与里程计信息相结合，为验证所提算法的有效性，在著名的Victoria Park数据集上，与传统算法进行比较。从实验结果可看出，本文算法融入迭代思想后，抑制了发散现象的出现，对机器人的定位有了明显地提高，且相比传统算法，其噪声的容忍能力更强。

［1］Siegwart R，Nourbakhsh I R，Scaramuzza D.Introduction to autonomous mobile robots［M］.MIT Press，2011.

［2］ZHANG Qi，MENG Heqing，ZHOU Rong，et al.A modified self－localization algorithm on mobile robot localization ［J］.Journal of Wuhan University of Technology （Information ＆Management Engineering），2014，36 （1）：9－13 （in Chinese）.［张琪，蒙禾青，周嵘，等.一种改进的移动机器人自定位算法 ［J］.武汉理工大学学报：信息与管理工程版，2014，36 （1）：9－13.］

［3］ZHAO Yilu，CHEN Xiong，HAN Jianda.Scan matching based SLAM in outdoor environment ［J］.Robot，2010，32（5）：655－60 （in Chinese）.［赵一路，陈雄，韩建达.基于扫描匹配的室外环境SLAM 方法 ［J］.机器人，2010，32 （5）：655－60.］

［4］ZHU Jianguo，GAO Junyao，LI Kejie，et al.Research on feature map building of mobile robot in indoor unknown environment［J］.Computer Measurement ＆ Contorl，2012，19（12）：3044－3046 （in Chinese）. ［朱建国，高峻峣，李科杰，等.室内未知环境下移动机器人特征地图创建研究 ［J］.计算机测量与控制，2012，19 （12）：3044－3046.］

［5］Lin S－Y，Chen Y－C.SLAM and navigation in indoor environments ［M］.Advances in Image and Video Technology.Springer，2012：48－60.

［6］He X，Cai Z.Feature extraction from 2Dlaser range data for indoor navigation of aerial robot［C］／／Proceedings of the Chinese Automation Congress.IEEE，2013：306－309.

［7］Boyko A，Funkhouser T.Extracting roads from dense point clouds in large scale urban environment［J］.ISPRS Journal of Photogrammetry and Remote Sensing，2011，66 （6）：S2－S12.

［8］Wurm K M，Kretzschmar H，Kümmerle R，et al.Identifying vegetation from laser data in structured outdoor environments［J］.Robot Auton Syst，2014，62 （5）：675－684.

［9］Dudek G，Jenkin M.Computational principles of mobile robotics［M］.Cambridge University Press，2010.

［10］Li H－P，Xu D－M，Zhang F－B，et al.Consistency analysis of EKF－based SLAM by measurement noise and observation times［J］.Acta Automatica Sinica，2009，35 （9）：1177－1184.

［11］Zhang HQ，Dou LH，Chen J，et al.Consistency analysis of EKF－SLAM for a basic scenario ［J］.Transactions of Beijing Institute of Technology，2011，31 （10）：1194－1197.

［12］Fu G，Corradi P，Menciassi A，et al.An integrated triangulation laser scanner for obstacle detection of miniature mobile robots in indoor environment［J］.IEEE／ASME Transactions on Mechatronics，2011，16 （4）：778－783.

［13］Kaess M，Ila V，Roberts R，et al.The Bayes tree：An algorithmic foundation for probabilistic robot mapping ［M］.Algorithmic Foundations of Robotics IX.Springer，2011：157－173.

［14］Nebot E.Victoria park dataset［EB／OL］.Available：http：／／www.personal.acfr.usyd.edu.au／nebot／dataset.htm，2012.