专利名称：一种基于几何统计特征的全局扫描匹配方法
技术领域：
本发明涉及智能移动机器人领域，主要用于移动机器人在未知动态环境中定位与 建模，但方法本身不限于此。扫描匹配方法的研究和应用涉及医疗、考古、救援、军事、虚拟 现实、移动机器人等诸多领域。重现非结构化未知环境的原貌，无疑对完成救灾、勘察等困 难危险的任务有很大的帮助。而从扫描匹配研究中衍生出的对于静态、动态、非刚性物体识 别的研究，也具有较大的应用价值。区别于医疗、考古等研究中对于物体模型高精确性再现 的需要，移动机器人研究对扫描匹配的实时性提出了更高的要求，以保证机器人可以准确 快速地完成既定任务，如灾后救援。扫描匹配结果的好坏直接体现了机器人在三维环境中 定位的准确性。
背景技术：
扫描匹配是寻找不同位姿下传感器扫描数据间的对应关系，估计相对位姿变换， 将扫描数据合并在一起，恢复物体或者环境原貌的一类方法的总称。扫描匹配技术的处理 对象是利用测距仪采集的扫描点集合。由于扫描匹配具有同时建立环境模型和估计相对位 姿的能力，它被引入到移动机器人定位与建图的研究中，并成为与基于滤波器(如扩展卡 尔曼滤波)的定位方法，以及基于贝叶斯理论(如马尔科夫和蒙特卡洛)的定位方法并列 的一类重要的移动机器人定位方法。该技术直接利用扫描的原始数据，建图结果更能体现 环境原貌。目前机器人领域对扫描匹配的研究日益深入，取得了一系列有价值的成果，如基 于图的同时定位与建图方法(Graph-SLAM)等。根据是否依赖扫描间相对位姿的初始估计，扫描匹配分为局部匹配和全局匹配。 局部匹配方法的位姿估计通常来自于里程计，在此基础上迭代地寻找对应扫描点，校正相 对位姿直至收敛使扫描重叠在一起。ICP(IterativeClosest/Corresponding Point)和 NDT(Normal Distribution Transformation)是此类方法的代表。局部匹配方法在静态环 境且位姿估计误差较小时取得理想结果，当环境变化或者位姿估计误差较大时则不能保证 收敛到全局最优解。这种情况下则需要全局匹配方法。全局匹配方法大都从扫描数据中提 取特征，通过特征匹配求解位姿。比如有的方法为每个扫描点建立局部坐标系，利用霍夫变 换在扫描点周围提取霍夫点构成特征。这种特征易受到噪声扫描点、观测位姿及运动物体 的影响。或者有的方法采用两种直方图描述整个扫描，利用互相关操作获得扫描间相对位 姿，但是受到重叠区域大小及环境变化的影响。还有方法利用累积角度函数描述扫描的分 割段，并为分割段间建立位置关联。该方法更能体现扫描的局部特性，但是位置关联仍然易 受到观测位姿和环境变化的影响。

发明内容
本发明从特征定义、匹配策略等角度提出了鲁棒性更高，适应动态环境的全局匹 配方法。扫描点根据是否取自同一物体表面归为一类，即看作一个分割段，扫描点集合便由 若干分割段构成。新方法从分割段和扫描点两个层次定义特征，利用扫描点法线方向的统计信息进行描述，提高特征定义的鲁棒性。分割段特征是对扫描数据整体的粗略描述。扫描 点特征定义在其所在的分割段上，不受周围运动物体干扰，可以细致地刻画分割段局部的 性质。最终的全局扫描匹配方法采用“试探-验证-求解”的策略。根据分割段的可辨别度 和相似度建立对应关系，利用扫描点特征相似度进行匹配得到试探解；依据评价法则验证 试探解的正确性；在试探解基础上找到所有可对应的分割段并求解扫描间的相对位姿。已 经在多个室内未知环境真实激光数据集上通过实验验证了方法的有效性。新方法可以正确 完成匹配，校正位姿，而动态物体和变化区域则在特征匹配的过程中被标定。新方法可应用 于静态或动态环境，在定位误差累积较大时可辅助发现闭环。虽然全局匹配方法效率略低 于局部匹配方法，但在动态环境及闭环上的应用不可替代。
四

图1扫描数据预处理前(a)后(b)示意图。在该数据集中每次激光扫描采集180° 范围共361个点。(a)图为预处理前的情况，预处理后(b)同一分割段上的扫描点通过线段 连接在一起，法线方向通过短线段表示。图2分割段(特征圆弧)可辨别度示意图。该图反映了使用特征圆弧表示的不规 则分割段，阴影区域表示分割段的可辨别度大小。图3扫描点特征抽取示意图。该图描述了扫描点P的特征如何定义，α表示其法 线方向，L表示扫描点关联微段划分的长度，1和r分别表示投影的左右边界，分割段上扫描 点的法线方向使用有向线段表示。图4两次扫描分别运用里程计(a)和新方法(b)匹配的结果，图中圆点代表激光 扫描点。位置1对应扫描1，位置2对应扫描2。(a)图反映出里程计误差较大，扫描不一 致；(b)图反映出利用新方法可以完好地完成匹配，矫正位姿。(c)图放大了墙角处运用新 方法进行匹配的结果。图5MingUeZ J.数据集上连续匹配149个扫描建立环境平面图。其中矩形内为标 记的动态区域。图中标注了机器人运行的轨迹，以及通过新方法发现的环境中可能动态变 化的区域。图6Fr079数据集上连续匹配1525个扫描建立环境平面图。图中1和2标注了两 个墙角。图7Albert-b-laSer数据集上连续匹配401个扫描建立环境平面图。图中矩形框 内为不规则区域，标记了环境中的动态区域。
五具体实施例方式5· 1 ^>害1] 牛寺 11 (Segment feature description)5.1.1 预处理在提取分割段特征之前需要对激光数据进行预处理，包括分割和估计法线方向两 个步骤。分割是根据扫描点的位置分布将可能采集自同一物体的扫描点划分为一个分割 段。分割段描述了扫描点的整体分布；扫描点依托于分割段便拥有了更多信息。分割方法综合考虑激光点之间的距离，以及激光点连线与激光束之间夹角的关 系，降低角度过小或者距离过远时因为采点稀疏所带来的影响。扫描点数过少的分割段在 该步骤被过滤。这些分割段代表的环境信息少，有些则是错误的激光读数或者动态物体。
分割之后对于每个扫描点P两侧一定范围内的扫描点，采用线性回归的方法估计 回归直线，将垂线方向作为该扫描点P的法线方向。法线方向经变换后均指向传感器。后 文提及的法线方向与此一致。图1显示了预处理前后的结果，同一分割段上的扫描点通过 线段连接在一起。5. 1.2分割段特征分割段形状各异，为了统一描述和比较，本文定义分割段S的特征Fs如下Fs=(LsICs)(1)Ls表示顺序连接分割段上所有扫描点的总长度，δ/表示扫描点法线方向变化 的方差，Cs e {-1，0，1}表示分割段整体的凹凸性(数值依次表示凹、平、凸)。由于不同区 域扫描点疏密程度不同，在统计5sa时根据扫描点两侧一定范围内扫描点的密度加权。分 割段可以呈现多次凹凸变化的形态，在此分别统计分割段整体及左半侧区域的平均法线方 向，据此获知分割段整体的凹凸性Cs。以上特征定义实际将复杂的分割段简化为线段或者圆弧。假设法线方向满足一致 分布，当《#0时分割段对应一段长度！^的圆弧，圆心角范围、-忑5as,忑δ , Cs决定开口
方向。当分割段对应圆周时，圆心角取得最大范围(-1 ^0，相应的5sa取极值π/芯。当 Sas =0时，分割段对应一条长度Ls的线段。5. 1. 3分割段可辨别度通常越长越不规则的分割段越容易与其他分割段相区别，对匹配的贡献越大。为 了描述不同分割段可被辨别的程度，本文定义分割段S的可辨别度Ds为1)若分割段特征表示为线段，Ds为0 ；2)若分割段特征表示为圆弧，Ds表示如图2所示的阴影面积，即将特征圆弧圆心 置于纵轴并与横轴相切，圆弧与横轴以及外接矩形所围成的面积。Ds越大表示分割段可辨别度越高。由于激光读数存在误差，真实线段对应的分割 段的5sa通常不为0，表示为圆心角很小的圆弧。假设激光读数误差满足高斯白噪声，则真 实线段越长越容易辨别。5. 1. 4分割段相似度为了在不同扫描上寻找相似的分割段进行匹配，定义任意分割段S1* &间的相似 度$为 ⑵其中凡tAnaX^。』，凡=LYmax^d ,L -min(Z L } ’
{L,mn/Anax ’ A x ^ 0[Αη η /^max ’ ^max 本 0^min _ mm ( , ‘ ^ }，
Anax = max{zv ZsJ ,Jmin,Jmax。相似度实际描述了特征线段或者
特征圆弧之间几何形状上的相似程度。&表示分割段长度比例，RS表示圆心角范围重叠比 例，表示凹凸性及圆心角的总体差异。根据定义，
且$越大相似
度越高。5. 2(Scan point feature description)5. 2. 1扫描点特征
分割段特征是对扫描数据整体的粗略描述，精确匹配还需要更好地刻画扫描数据 局部的性质，为此本文引入分割段S上扫描点P的特征FP。如图3所示，以点P为原点，法 线方向α ρ为横轴建立局部坐标系。沿点P处切线向两侧依次等距离划分，每段记为Si称 为“扫描点关联微段”，将S上的点依次投影到对应微段Si中直至S的端点。Si中统计的是 S上连续的一段，S卩如果S投影的过程中由于弯曲而投影到了之前统计结束的微段上则提 前终止投影过程。由此点P的特征Fp记为
FP=[L,UBP{l,r),{FSi \i = [llL\,...,\rlL-\))(3)L为划分长度，UPB(1，r)表示投影距离原点P最远的左右边界，
I / = [//1」，…，「r/Z]j是所有微段Si特征的集合，扫描点关联的微段数目为「r/q-μ/Ζ」。根
据定义，扫描点特征不随观测位姿变化而改变。5. 2. 2扫描点关联微段特征扫描点关联微段Si实际是具有局部坐标系信息的分割段，其特征 <记为F、=[LSj,Ul^l+S5as),CSi,U^{t,d))(4)、表示微段长度，&表示微段整体凹凸性，Us"表示扫描点投影垂直距离的分布， 表示微段内扫描点相对α ρ的平均偏角，《表示偏角的方差。与分割段特征一致，假设微
段上法线方向满足一致分布，关联微段简化为特征线段或者圆弧，表示圆心角范围，其 位置由Si和V5"决定，开口方向由 <决定。分割段特征Fs可以看作关联微段特征《的特殊形式
Fsi-(LsrSlXs)^ FSi =[LSi,Ul(-SdaSi,SSas),Csr0)(5)5. 2. 3扫描点关联微段相似度与分割段相似度类似，微段相似度S' F是用于在匹配扫描点时描述不同微段S1* &之间的相似程度
_4] A=Wulc^1-CAi⑷&表示微段长度比例，Rs表示圆心角范围重叠比例，&表示投影分布的重叠比例。 根据定义，S' Fe
且S' F越大相似度越高。5. 2. 4扫描点匹配度在本文提出方法中，精确匹配是通过扫描点匹配完成的。任意扫描点P1和P2，将 其局部坐标系重叠在一起，则部分关联微段对应，二者相匹配的程度Mp表示如下Mp= (ηι; n2, n3' n4) = (sF，oB，s' F，o' B) (7)Sf表示对应微段的平均相似度，Ob表示扫描点特征的跨度UpB的重叠比例。将相 似度大于一定阈值的对应微段看作好的匹配，则S'B分别表示好匹配微段的平均 相似度和所占比例。$和％刻画了扫描点特征的平均匹配情况，而S'B则可以避 免因分割段局部变化导致平均刻画较差时，错误地认为扫描点不匹配。最好的匹配度描述 为Mbest = (1,1,1,1),匹配度的数值表示如下
权利要求
1.一种基于几何统计特征的全局扫描匹配方法，其特征在于，步骤包括(1)激光传感 器数据预处理；( 提取预处理后数据分割段的特征；C3)提取预处理后激光扫描点的特 征；⑷采用“试探-验证-求解”的策略获取不同扫描间相对位姿。其中步骤⑴在步骤 ⑵⑶⑷之前；步骤⑵⑶顺序不限，可颠倒、可并行；步骤⑷在最后。
2.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤(1)激光传感器数据预处理中包含(1.1)激光数据分割和(1. 激光扫描点法线方向估计 两步。
3.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤(2)中数据分割段特征的定义符合以下三元组
4.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤(3)中激光扫描点特征的定义符合以下三元组
5.如权利要求4所述的基于几何统计特征的全局扫描匹配方法，其特征在于激光扫描 点特征定义三元组中·Kh.=L以」，...，「推1丨是所有扫描点关联微段“特征的集合，其中扫描 点关联微段特征的定义符合以下四元组
6.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤(4)中“试探-验证-求解”中试探过程为从不同扫描间选取一对分割段，根据分割段相似 度以及扫描点匹配度寻找对应扫描点，利用最小二乘法或者牛顿方法求解分割段间相对位 姿作为试探解。
7.如权利要求6所述的基于几何统计特征的全局扫描匹配方法，其特征在于其中的分 割段相似度按照以下方式计算
8.如权利要求6所述的基于几何统计特征的全局扫描匹配方法，其特征在于其中的扫 描点匹配度按照以下方式计算Sf = Rh ■ Rl . Ra--j-rMp= (η” n2, η3，n4) = (SF, 0B, S' f, 0' b)r 4\ 4VmF =cos ？K"'-1)2 'Σω· =1V L /=1J i=l其中S' F表示扫描点关联微段相似度，4和&2分别表示不同微段的凹凸性，&和\分 别表示法线偏角的方差，&表示投影分布的重叠比例，&表示微段长度比例，R5表示圆心 角范围重叠比例。而四元组Mp描述了不同扫描间的匹配结果，艮表示对应微段的平均相似 度，Ob表示扫描点特征的跨度U/的重叠比例，S'B分别表示好匹配微段的平均相似度和所占比例。则是最终扫描点匹配度的计算值。
9.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤 (4)中“试探-验证-求解”中验证过程为根据一定的准则判断试探解正确与否以及解的好 坏，准则包含以下三项中任意几项(a)扫描点对平均匹配误差艮小于一定阈值；(b)扫描 间互相可见区域的最大比例孓大于一定阈值；(c)可见区域中重叠区域比例&大于一定阈 值。结果评价按照下式计算
10.如权利要求1所述的基于几何统计特征的全局扫描匹配方法，其特征在于其步骤 (4)中“试探-验证-求解”中求解过程为在试探解的基础上将不同扫描转换到同一坐标 系下，根据分割段覆盖的角度范围获得对应分割段，求解所有对应分割段的相对位姿，过滤 其中求解结果与试探解不一致的对应分割段，利用剩余对应分割段上的对应扫描点，利用 最小二乘法或者牛顿方法等求解扫描间相对位姿。将验证结果评价最高的试探解作为最终 解。
全文摘要
本发明中基于几何统计特征的全局扫描匹配方法是在研究如何同步完成移动机器人定位、环境轮廓刻画、环境变化区域判别等任务时提出并实现的。方法除可用于移动机器人环境建模外，还可作为灾难救援、矿井勘察等特定任务的辅助工具。方法利用了激光扫描数据上几何信息的统计结果定义特征，首先通过分割扫描，定义分割段特征简化扫描整体描述，进而采用扫描点特征刻画扫描局部性质，扫描间相对位姿则通过“试探-验证-求解”的策略获得匹配分割段获得位姿试探解，验证解正确性，验证成功则利用对应点特征及最小二乘法求解相对位姿。验证最佳的解作为最终解。这样方法刻画了环境轮廓同时矫正了机器人位姿，环境变化则根据特征匹配结果进行了辨别。
文档编号G01C22/00GK102135620SQ20101003230
公开日2011年7月27日 申请日期2010年1月21日 优先权日2010年1月21日
发明者郭瑞 申请人:郭瑞