专利名称：适用于移动机器人相对运动估计的激光粒子影像配准方法
技术领域：
本发明涉及ー种影像配准方法，具体涉及ー种适用于移动机器人相对运动估计的激光粒子影像配准方法。
背景技术：
移动机器人相对运动估计广泛应用于外星球探索，采矿自动化与安全，水下制导和深海勘探等领域。当前使用全球定位系统(GPS)对移动机器人定位简单易行，但GPS信号会因植被、山脉和高楼的遮挡以及多路径效应而在很多区域精度变差甚至无法输出结果，因此自包含的航位估测方法被应用到移动机器人定位中，通过配准激光粒子影像是该类方法的经典技术，依据激光测距原理，传感器在每个采样时刻观测到的激光粒子影像由若干距离读数构成，每个读数表征了从传感器当前方位到环境景物表面一个点的距离。通 过极坐标到笛卡尔坐标的转换，每个距离读数都可等效成机器人中心坐标系下的一个激光点。当机器人的移动的速度不太快或传感器采样频率足够高时，相邻两采样时刻(这里称这两个相邻采样时刻分别为新时刻、參考时刻，在这两个时刻所观测到的激光粒子影像分别称为新激光粒子影像、參考激光粒子影像)机器人在相邻两方位处的视野会有部分重叠，即，新激光粒子影像中的ー些激光点分别与參考激光粒子影像中的ー些激光点两两对应于实际环境景物表面的一些相同的物理点，那么，在全局空间中，对应同一个物理点的激光点相关联。搜寻新激光粒子影像和參考激光粒子影像中所包含激光点的关联关系，进而配准这两帧激光粒子影像，并基于此通过迭代优化空间距离的误差函数可估计移动机器人相邻时刻间的相对运动，实现移动机器人定位和导航。经对现有技术文献的检索发现，Lu,F等人在《智能和机器人系统杂志》(《Journal of Intelligent and Robotic Systems)))1997，18卷，249-275页上发表了 “未知环境中基于配准ニ维距离束的机器人位置估计”(Robot pose estimation in unknown environments by matching 2D range scansノ。该文介绍了ー种基于配准两相邻激光粒子影像实现移动机器人定位的方法——迭代最近点法(Iterative closest point, ICP)。这种激光粒子影像配准的方法存在一定问题(a)估计激光点关联时所依据的尺度过于单一，用于判断两激光点关联关系的尺度为经坐标投影后的两激光点间的空间欧式距离。在配准大量程、形状复杂、采集于高动态运动状态下的相邻激光粒子影像时性能不佳。(b)依赖对于相对运动的猜测。猜测得到的相对运动作为优化迭代的初始值，对它的选取直接影响迭代的次数和方法最終收敛值的精度。(c)没有对激光粒子影像配准质量表征的概率依据。当把基于激光粒子影像配准的相对运动估计嵌入滤波过程时，对于估计不确定性的定量分析是必不可少的。迭代最近点方法在方法设计上并没有考虑对配准品质的自映射机制。
发明内容
本发明提供了一种适用于移动机器人相对运动估计的激光粒子影像配准方法，能够以机器学习的手段智能地管理并归纳局部及全局的多重几何特性，提高了激光粒子影像配准的准确性，尤其是在大量程、形状复杂、高动态运动状态下的影像配准精度，并且构建于严密的概率统计基础之上，为定量分析估计结果的误差将其嵌入概率滤波框架提供了可靠的依据。本发明采用以下技术方案来实现
一种用于移动机器人相对运动估计的激光粒子影像配准方法，其特点是，所述方法包括如下步骤
步骤1，在机器人上设置激光扫描传感器，当机器人开始移动时，激光扫描传感器开始按照固定频率采集激光粒子影像；
步骤2，将激光扫描传感器在相邻时刻采集的激光粒子影像作为一対，并在第一次运行系统的时候构建条件随机场概率图模型；
步骤3，通过构建函数并计算，从采集的相邻时刻激光粒子影像对数据中提取局部特征和配对特征；·
步骤4，在数据中进行检索，进行权重判断，如果已有局部特征和配对特征的权重，则直接进入步骤5，否则针对步骤2所构建的条件随机场概率图模型进行局部特征和配对特征的权重的学习，再进行步骤5;
步骤5，将相邻时刻采集的激光粒子影像、局部特征和配对特征、局部特征权重和配对特征权重输入至所述的条件随机场。其中，激光粒子影像为观测值，局部特征和配对特征及其权重输入条件随机场中所设置的势函数，通过计算得到一个矩阵，该矩阵的列向量即为条件随机场概率图模型中的节点，体现了两相邻激光粒子影像中激光点间的配准关系。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中步骤I中所述激光扫描传感器设置在机器人前端，其重心与机器人重心重合。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于，所述步骤2中还包含
步骤2. 1，通过坐标转换，将步骤I中采集到的距离读数转换为以激光扫描传感器为原点的笛卡尔坐标系下的ニ维点，使某一时刻测量到的激光粒子影像成为ー组由若干ニ维激光点组成的ニ维向量序列；
步骤2. 2，将相邻两个采样时刻测得的激光粒子影像作为ー对，分别为參考激光粒子影像和新激光粒子影像；
步骤2. 3，构建条件随机场概率图模型，其中，将步骤2. 2中的參考激光粒子影像和新激光粒子影像中所包含的激光点作为条件随机场概率图模型中的观测值，它们各自的关联状态映射为条件随机场概率图模型中的节点，并设定条件随机场图模型中的势函数，观测值、节点、势函数共同构成条件随机场概率图模型，其中节点向量即代表了參考激光粒子影像中的激光点与新激光粒子影像中的激光点的配准关系。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中所述步骤3中还包含
步骤3. I，首先设定多种基础几何特征函数，对步骤2. 2中的參考激光粒子影像和新激光粒子影像向量序列进行计算，从向量序列中提取出相邻时刻激光粒子影像中激光点之间的多种基础几何特征；
步骤3. 2，将基础几何特征输入到分类器中，生成激光粒子影像的局部特征；步骤3. 3，对參考激光粒子影像中两激光点与新激光粒子影像中两激光点之间的多种关系建模，从中提取出相邻时刻的激光粒子影像中激光点之间的多种配对特征。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中所述步骤3. 2提取局部特征还包含
步骤3. 2. 1，将所有的基础几何特征输入到分类器中，生成数据特征；
步骤3. 2. 2，将各基础几何特征中最小的元素提出；
步骤3. 2. 3，将步骤3. 2. 2提取出的各基础几何特征中最小的元素输入到分类器中，生成孤立点特征；
步骤3. 2. 4，将步骤3. 2. I生成的数据特征和步骤3. 2. 3生成的孤立点特征拼接成为激光粒子影像的局部特征。 上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中所述步骤3. 3还包含
步骤3. 3. 1，设定没有噪音且机器人的相对运动近似零理想状态下不存在孤立点，当与关联时，ム:ゴ很大似然上与[彳=关联，设置一系列带有主对角线向上偏移的単位矩阵来映射这种关联关系，这ー系列设置的単位矩阵即为配对序列特征；
步骤3. 3. 2，设置函数，将參考激光粒子影像第i个激光点与该粒子影像内的其它所有激光点两两之间的欧式距离与通过遍历求得的新激光粒子影像第j个激光点与该粒子影像内的其它所有激光点两两之间的欧式距离相减求差，这些差值组成的:W X M的矩阵即为配对距离特征。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中步骤4中还包含对局部特征和配对特征的权重的学习过程如下
步骤4. 1，对步骤2所采集的相邻两帧激光粒子影像，手工对它们所包含激光点之间的配准关系进行标定，根据手工标定的配准关系建立一组向量序列以表明參考激光粒子影像中的激光点与新激光粒子影像中的激光点的配准关系；
步骤4. 2，采集多组激光粒子影像对，运行步骤4. I和步骤3多次，获得多组激光粒子影像对和它们所对应的多组标定关联关系、局部特征和配对特征；
步骤4. 3，将步骤4. 3得到的多组局部特征、配对特征和相应的影像对输入步骤2所构建的条件随机场概率图模型中，其中标定的关联关系对应于模型中的节点，局部特征和配对特征代入势函数，通过概率计算倒推出各种特征的最佳权重。上述的用于移动机器人相对运动估计的激光粒子影像配准方法，其中所述对局部特征和配对特征的权重的学习过程只需要在每种不同类型的场景中第一次运行系统时执行一次。与现有技术相比，本发明具有如下有益效果
(I)本发明以机器学习的手段智能地管理并归纳激光点局部及全局的多重几何特征，由于在初次运行时，构建条件随机场概率图模型，并针对所构建的模型，使系统对已标定的多组影像对进行局部特征和配对特征的学习，从而倒推得到局部特征权重和配对特征权重，并在之后的实际影像配准过程中使用这些学习来的权重參与计算；因此，它的估计结果是建立在对同类环境中的影像学习的基础上，提高了激光粒子影像的配准精度。(2)本发明由于通过构建多种不同的函数，由測量得到的距离读数提取多种基础几何特征，并由基础几何特征提取局部特征和多种配对特征，在估计激光点关联时提供了多种依据，从而使本发明的配准方法在机器人作业的非结构化环境中，对激光粒子影像的配准精度比传统方法高。(3)本发明由于设定多种不同的函数，从已标定的激光粒子影像倒推计算出各种特征的最佳权重，并将这些权重与测量值共同运用于计算中，因此，本发明所提供的方法是构建于严密的概率基础之上，这为定量分析估计结果的误差将其嵌入概率滤波框架提供了可靠的依据。
图I为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法的流程示意 图2为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中构建条件 随机场概率图模型的流程 图3为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中提取相邻时刻采集的激光粒子影像的局部特征和配对特征的流程 图4为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中对局部特征和配对特征的权重的学习过程的流程 图5为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中相邻两时刻移动机器人采集激光粒子影像的示意 图6为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中构建的条件随机场概率图模型示意 图7为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中局部特征构建的示意 图8为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法中对相邻两帧激光粒子影像的标定效果 图9为使用本发明适用于移动机器人相对运动估计的激光粒子影像配准方法配准十组激光粒子影像的准确率与传统方法的对比 图10为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中第二组激光粒子影像的真实配准效果 图11为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中使用现有技术同类方法对第二组激光粒子影像配准效果 图12为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中使用本发明方法对第二组激光粒子影像配准效果 图13为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中第七组激光粒子影像的真实配准效果 图14为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中现有技术使用同类方法对第七组激光粒子影像配准效果 图15为本发明适用于移动机器人相对运动估计的激光粒子影像配准方法实施例中使用本发明方法对第七组激光粒子影像配准效果图。
具体实施例方式以下结合附图，通过详细说明ー个较佳的具体实施例，对本发明做进ー步阐述。本实施例的测试环境为都市街道，测得了二十组激光粒子影像，其中十组作为学习样本，另外十组作为配准测试样本。如图I所示，本发明的实施例包括如下步骤
步骤I，在机器人上设置激光扫描传感器。传感器安装于机器人前端，扫描窗朝向移动机器人前方。传感器重心尽量与机器人重心重合。伴随机器人移动，激光扫描传感器开始按照固定频率采集激光粒子影像。激光扫描传感器的ー组测量值由若干距离读数构成，每个读数表征了从传感器当前方位到环境景物表面一个点的距离。本实施例中使用了 SICK室外激光扫描传感器，其最大量程为70m，视角为180度，视角的分辨率设置为0. 5度。步骤2，将安装在移动机器人上的激光扫描传感器在相邻两个采样时刻测得的激光粒子影像作为一対。请配合參见图2，通过极坐标到笛卡尔坐标的转换，步骤I采集到的 激光粒子影像中每个距离读数都可等效成机器人中心坐标系(以移动的机器人重心为坐标原点的笛卡尔坐标系)下的ー个ニ维激光点。某一时刻测量得到的激光粒子影像为ー组由若干ニ维激光点组成的ニ维向量序列。请配合參见图5，机器人相继途经參考方位IVt 和新方位吋，测得两帧激光粒子影像參考激光粒子影像和新激光粒子影像L_.,图中
“ら表示參考激光粒子影像中的激光点，■表示新激光粒子影像中的激光点。针对激光粒子影像配准问题的特点，在第一次运行系统的时候构建条件随机场(CRF)概率图模型，将參考激光粒子影像和新激光粒子影像中的所包含的激光点作为条件随机场概率图模型中的观测值，它们各自的可能的关联状态映射为条件随机场概率图模型中的节点，并设置条件随机场图模型中的势函数，观测值、节点、势函数共同构成条件随机场概率图模型。构建条件随机场概率图模型仅需在第一次运行系统时运行一次即可。本实施例中所构建的链式条件随机场图模型示于图6，在该模型中将參考激光粒子影像和新激光粒子影像所包含激光点作为观测量z, {i4f. !4'i’.…￡ふ-}表示參考激光粒子影像中的N个激光点，H Liw, Liw..…/4し}表示新激光粒子影像中的M个激光点。它们各自的可能的关联状态映射为条件随机场图中的节点め，条件随机场图中的任ー节点A都对应了 M+1种状态的似然。例如，.r, 的状态I表示.所对应的參考激光粒子影像中的激光点
与新激光粒子影像的激光点w关联的似然，A状态2表示/4,.f与新激光粒子影像的激光点/4,..,关联的似然，依次类推，状态M+1表示14在新激光粒子影像中找不到相关联的激光点的似然，g卩，￡。是孤立点的似然。该模型中只包含两种势函数局部势(Local potential)办(ろ)和配对势(Pairwise potential)。这两个势函数将在下文中通过计算得
至IJ。图中不同的小方格口、分别象征性的代表了由小到大的向量元素值。作为观测量z的d Ulf……/ 和{らしLlew. /4ず-…[ニ.}、节点.i‘,■、局部势配对势％<.ヱ;-jJ )共同构成ー个条件随机场概率图模型。步骤3，通过构建函数并计算，从采集的相邻时刻激光粒子影像对数据中提取局部特征和配对特征。请配合參见图3，首先设定多种基础几何特征函数，对步骤2中的參考激光粒子影像和新激光粒子影像序列进行计算，从向量序列中提取相邻时刻的激光粒子影像中激光点之间的多种基础几何特征；然后将多种基础几何特征输入到分类器中，生成激光粒子影像的数据特征，并将各基础几何特征中最小的元素提出，输入到分类器中，生成孤立点特征，将数据特征和孤立点特征拼接成为局部特征；同时，对參考激光粒子影像中两激光点与新激光粒子影像中两激光点之间的多种关系建摸，从中提取出相邻时刻的激光粒子影像中激光点之间的多种配对特征。本实施例中选定以下四种函数，提取四种基础几何特征
距离
权利要求
1.一种用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于所述方法包括如下步骤 步骤1，在机器人上设置激光扫描传感器，当机器人开始移动时，激光扫描传感器开始按照固定频率采集激光粒子影像； 步骤2，将激光扫描传感器在相邻时刻采集的激光粒子影像作为一对，并在第一次运行系统的时候构建条件随机场概率图模型； 步骤3，通过构建函数并计算，从采集的相邻时刻激光粒子影像对数据中提取局部特征和配对特征； 步骤4，在数据中进行检索，进行权重判断，如果已有局部特征和配对特征的权重，则直接进入步骤5，否则针对步骤2所构建的条件随机场概率图模型进行局部特征和配对特征的权重的学习，再进行步骤5; 步骤5，将相邻时刻采集的激光粒子影像、局部特征和配对特征、局部特征权重和配对特征权重输入至所述的条件随机场，其中，激光粒子影像为观测值，局部特征和配对特征及其权重输入条件随机场中所设置的势函数，通过计算得到一个矩阵，该矩阵的列向量即为条件随机场概率图模型中的节点，体现了两相邻激光粒子影像中激光点间的配准关系。
2.如权利要求I所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于步骤I中所述激光扫描传感器设置在机器人前端，其重心与机器人重心重合。
3.如权利要求I所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于，所述步骤2中还包含 步骤2. 1，通过坐标转换，将步骤I中采集到的距离读数转换为以激光扫描传感器为原点的笛卡尔坐标系下的二维点，使某一时刻测量到的激光粒子影像成为一组由若干二维激光点组成的二维向量序列； 步骤2. 2，将相邻两个采样时刻测得的激光粒子影像作为一对，分别为参考激光粒子影像和新激光粒子影像； 步骤2. 3，构建条件随机场概率图模型，其中，将步骤2. 2中的参考激光粒子影像和新激光粒子影像中所包含的激光点作为条件随机场概率图模型中的观测值，它们各自的关联状态映射为条件随机场概率图模型中的节点，并设定条件随机场图模型中的势函数，观测值、节点、势函数共同构成条件随机场概率图模型，其中节点向量即代表了参考激光粒子影像中的激光点与新激光粒子影像中的激光点的配准关系。
4.如权利要求I所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于，所述步骤3中还包含 步骤3. I，首先设定多种基础几何特征函数，对步骤2. 2中的参考激光粒子影像和新激光粒子影像向量序列进行计算，从向量序列中提取出相邻时刻激光粒子影像中激光点之间的多种基础几何特征； 步骤3. 2，将基础几何特征输入到分类器中，生成激光粒子影像的局部特征； 步骤3. 3，对参考激光粒子影像中两激光点与新激光粒子影像中两激光点之间的多种关系建模，从中提取出相邻时刻的激光粒子影像中激光点之间的多种配对特征。
5.如权利要求4所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于所述步骤3. 2提取局部特征还包含步骤3. 2. 1，将所有的基础几何特征输入到分类器中，生成数据特征； 步骤3. 2. 2，将各基础几何特征中最小的元素提出； 步骤3. 2. 3，将步骤3. 2. 2提取出的各基础几何特征中最小的元素输入到分类器中，生成孤立点特征； 步骤3. 2. 4，将步骤3. 2. I生成的数据特征和步骤3. 2. 3生成的孤立点特征拼接成为激光粒子影像的局部特征。
6.如权利要求4所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于，所述步骤3. 3还包含 步骤3. 3. 1，设定没有噪音且机器人的相对运动近似零理想状态下不存在孤立点，当与,.关联时，很大似然上与4 .关联，设置一系列带有主对角线向上偏移的单位矩阵来映射这种关联关系，这一系列设置的单位矩阵即为配对序列特征； 步骤3. 3. 2，设置函数，将参考激光粒子影像第i个激光点与该粒子影像内的其它所有激光点两两之间的欧式距离与通过遍历求得的新激光粒子影像第j个激光点与该粒子影像内的其它所有激光点两两之间的欧式距离相减求差，这些差值组成的的矩阵即为配对距离特征。
7.如权利要求I所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于，其中步骤4中还包含对局部特征和配对特征的权重的学习过程如下 步骤4. 1，对步骤2所采集的相邻两帧激光粒子影像，手工对它们所包含激光点之间的配准关系进行标定，根据手工标定的配准关系建立一组向量序列以表明参考激光粒子影像中的激光点与新激光粒子影像中的激光点的配准关系； 步骤4. 2，采集多组激光粒子影像对，运行步骤4. I和步骤3多次，获得多组激光粒子影像对和它们所对应的多组标定关联关系、局部特征和配对特征； 步骤4. 3，将步骤4. 3得到的多组局部特征、配对特征和相应的影像对输入步骤2所构建的条件随机场概率图模型中，其中标定的关联关系对应于模型中的节点，局部特征和配对特征代入势函数，通过概率计算倒推出各种特征的最佳权重。
8.如权利要求I所述的用于移动机器人相对运动估计的激光粒子影像配准方法，其特征在于所述对局部特征和配对特征的权重的学习过程只需要在每种不同类型的场景中第一次运行系统时执行一次。
全文摘要
本发明公开了一种用于移动机器人相对运动估计的激光粒子影像配准方法，所述方法先为机器人装配激光扫描传感器，从而可以按照固定频率采集激光粒子影像；然后将相邻两个采样时刻测得的激光粒子影像作为一对，构建条件随机场概率图模型；通过构建函数，从激光粒子影像对中提取出局部特征和配对特征；最后将相邻两帧激光粒子影像和提取出的局部特征和配对特征，以及通过机器学习得到的局部特征和配对特征的权重输入至所构建的条件随机场，通过最大和概率推理得到两相邻激光粒子影像中激光点间的配准关系。本发明的优点在于能够以机器学习的手段智能地管理并归纳激光点局部及全局的多重几何特征，提高了激光粒子影像配准的精确性。
文档编号G06T7/00GK102819844SQ20121030028
公开日2012年12月12日 申请日期2012年8月22日 优先权日2012年8月22日
发明者孙作雷, 曾连荪 申请人:上海海事大学