CN102338867A - 一种适用于移动机器人导航的激光束匹配误差估计方法 - Google Patents

Abstract

本发明公开了一种适用于移动机器人导航的激光束匹配误差估计方法，所述方法首先将移动机器人在相邻两个采样时刻测得的激光束作为一对，针对激光束匹配误差特性构建条件随机场概率图模型，并提取出所述激光束对的局部特征和配对特征，然后将所述特征及相应权重输入至所构建的条件随机场，通过和积概率推理得到推理概率表，最后根据推理概率表分别求得紧凑节点集和紧凑状态集，并通过采样上述紧凑节点集和紧凑状态集求取表征激光束匹配误差的协方差。本发明的优点在于：考虑了激光束匹配中的主要误差源，以机器学习的手段智能地管理并归纳激光点局部及全局的多重几何特征，有效地提高了对激光束匹配误差的估计准确度，同时降低了方法执行时间。

Description

一种适用于移动机器人导航的激光束匹配误差估计方法

技术领域

本发明涉及到误差估计领域，具体地说，特别涉及到一种适用于移动机器人导航的激光束匹配误差估计方法。

背景技术

移动机器人导航广泛应用于外星球探索，采矿自动化与安全，水下制导和深海勘探等领域。使用全球定位系统(GPS)对移动机器人定位简单易行，但GPS信号会因植被、山脉和高楼的遮挡以及多路径效应而在很多区域精度变差甚至无法输出结果，因此自包含的航位估测方法被应用到移动机器人定位中，通过匹配激光束是该类方法的经典技术，方法是，将激光扫描传感器安装在移动机器人前端，则依据激光测距原理，传感器在每个采样时刻观测到的激光束由若干距离读数构成，每个读数表征了从传感器当前方位到环境景物表面一个点的距离。通过极坐标到笛卡尔坐标的转换，每个距离读数都可等效成机器人中心坐标系下的一个二维激光点。当机器人的移动的速度不太快或传感器采样频率足够高时，相邻两采样时刻(这里称这两个相邻采样时刻分别为：新时刻、参考时刻，在这两个时刻所观测到的激光束分别称为：新激光束、参考激光束)机器人在相邻两方位处的视野会有部分重叠，即，新激光束中的一些激光点分别与参考激光束中的一些激光点两两对应于实际环境景物表面的一些相同的物理点，那么，在全局空间中，对应同一个物理点的激光点相关联。搜寻新激光束和参考激光束中所包含激光点的关联关系，进而匹配这两帧激光束，并基于此通过迭代优化空间距离的误差函数可求取移动机器人相邻时刻间的相对运动，但是，当把由此求取的相对运动与其它传感器观测(例如，GPS观测)相融合时，对激光束匹配误差的合理估计是重要技术环节。

经对现有技术文献的检索发现，Ola Bengtsson等人在《Robotics andAutonomous Systems》2003，44卷，29-40页上发表了“Robot localizationbased on scan-matching--estimating the covariance matrix for the IDCalgorithm”(《机器人和自动化系统》，基于激光束匹配的机器人定位——估计IDC方法的协方差矩阵)。该文介绍了一种估计激光束匹配误差的方法——离线法(off-line)，该方法能捕捉到当机器人移动过程中激光束匹配定位所产生的误差，并使用协方差矩阵来表征这种误差。这种激光束匹配误差估计的方法存在一定问题：(a)该方法估计的仅是传感器观测误差所引起的最终匹配误差，而假设两帧激光束中激光点之间的关联是准确的。事实上，对于现代的基于激光测距扫描原理的商用激光传感器的精度有了显著的改善，因此，与激光传感器观测噪声所引起的最终激光束匹配误差与激光点间误关联所引起的误差相比基本可以忽略。引证文献所提供的方法忽略了激光束匹配问题中的主要误差源。(b)基于简单的欧式距离尺度，并使用基于最小距离累加和的迭代优化函数确定误差，忽略了这种迭代优化方法本身的局部最小现象，因此，该方法估计出的最终误差是不全面的。

综上所述，针对现有技术的缺陷，特别需要一种适用于移动机器人导航的激光束匹配误差估计方法，以解决现有技术的不足。

发明内容

本发明针对上述现有技术的不足和缺陷，提供了一种适用于移动机器人导航的激光束匹配误差估计方法，解决了背景技术中存在的不足，提高了激光束匹配误差估计的可靠性和准确性。

本发明所解决的技术问题可以采用以下技术方案来实现：

一种适用于移动机器人导航的激光束匹配误差估计方法，其特征在于，所述方法包括如下步骤：

1)针对激光束匹配误差估计问题的特点构建条件随机场概率图模型，此步骤仅需执行一次；

2)将移动机器人在相邻两个采样时刻测得的激光束作为一对；

3)提取得到2)中所述激光对中激光点之间的局部特征和配对特征；

4)如果具有现成的局部特征和配对特征的权重，则直接进行下一步，否则先针对所构建的条件随机场概率图模型进行权重参数的学习，再进行下一步；

5)将相邻时刻采集的激光束、局部特征和配对特征、局部特征和配对特征的相应权重输入至步骤1)中所构建的条件随机场，通过和积概率推理得到推理概率表；

6)将5)中得到的推理概率表每一列中最大元素对应孤立点状态的列去除，仅保留可找到关联关系的节点，相应地演化出新的推理概率表；

7)计算6)中新推理概率表中每一列的熵，并根据熵值的大小对节点再次选择，得到紧凑节点集，相应地演化出新的推理概率表；

8)将7)中的推理概率表的每一个节点的状态按照各自的推理概率降序排列，然后依据该节点所有可能关联状态推理概率的全局均值和局部均值，确定该节点的紧凑状态集，相应地，演化出新的推理概率表；

9)利用步骤7)得到的紧凑节点集和步骤8)得到的紧凑状态集，通过采样，求取激光束匹配的协方差。

在本发明的一个实施例中，所述对局部特征和配对特征的权重的学习过程包括如下步骤：

a.依据步骤2)采集相邻的两帧激光束，用手工的方法对它们所包含激光点之间的匹配关系进行标定；

b.提取该相邻两帧激光束中激光点的局部特征和配对特征；

c.运行步骤a和步骤b 10次，得到10组激光束对和它们所对应的10组标定关联关系、局部特征和配对特征；

d.将步骤c中的结果输入步骤1)所构建的条件随机场概率图模型中，并使用最大伪似然学习法得到各种特征的最佳权重。

在本发明的一个实施例中，求得所述紧凑节点集的具体步骤为：

①将去除了孤立点状态的推理概率表的列依据节点的熵从大到小排列；

②计算所有节点熵的全局熵均值和局部熵均值，并找到熵值最接近这些熵均值的节点，将这些节点连同具有最大熵和最小熵的节点作为n个基准点

③围绕每个基准点选取m个节点，所选取的m×n个节点组成紧凑的节点集N_com。

在本发明的一个实施例中，所述求取激光束匹配的协方差的具体步骤为：

①在N_com中，分别从围绕n个节点基准点的节点子集中选取n个节点，即从每一个子集中都随机地选取一个节点；

②对于选取的n个节点中的每一个节点，分别从围绕g个状态基准点的状态子集中选取g个状态；

③基于该n个节点及其各自的g个状态，构建gⁿ个相邻激光束中激光点关联配置；依据该n个节点的索引，从参考激光束中找出对应的n个激光点的机器人中心坐标系下的笛卡尔坐标；依据步骤②的结果，对应每个节点，重新在激光束中找出g个激光点的机器人中心坐标系下的笛卡尔坐标；

④基于步骤③的结果，计算出gⁿ组相对运动的解；

⑤在构建每条激光点关联配置时，同时记录相应推理概率表中每个对应元素的值，即所涉及节点的某个状态的推理概率，然后将它们相乘便得要求的似然；关联配置的似然作为每组相对运动解的权重，据此求取激光束匹配的加权协方差；

⑥重复以上步骤30次。

与现有技术相比，本发明具有如下有益效果：

1.考虑了激光束匹配中的主要误差源，更全面地建模了激光束匹配中的误差。所估计的激光束匹配误差更符合真实情况。

2.以机器学习的手段智能地管理并归纳激光点局部及全局的多重几何特征，比基于简单欧式距离的同类方法(见引证背景文献)具有更严密的概率基础。

3.基于熵分级和推理似然分级限定了粒子采样空间，有效地降低了方法执行时间。

附图说明

图1为本发明所述方法的流程示意图；

图2为本发明实施例中进行实验验证的环境；

图3为本发明实施例中所构建的条件随机场概率图模型示意图；

图4为本发明实施例中求得的推理概率表示意图；

图5为本发明实施例中求得的推理概率表的三维示意图；

图6为本发明实施例中对推理概率表的节点选择示意图；

图7为本发明实施例中激光束匹配误差估计的效果图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

本实施例中，将激光传感器安装于机器人平台前端，并通过无线自组网远程遥控移动机器人平台的运动。激光传感器的有效量程被设定为3.5米，可视范围为180度，每个采样周期内输出361个激光测距值，可根据机器人当前的运动方位将其等效为激光点在机器人中心坐标系下的笛卡尔坐标。实验在图2所示的办公室环境中进行。

本实施例包括如下步骤：

第一步，针对激光束匹配误差估计问题的特点，构建条件随机场(CRF)概率图模型，该步骤仅需运行一次即可。本实施例中所构建的链式条件随机场图模型示于图3，它只包含两种势函数：局部势(Local potential)和配对势(Pairwisepotential)。其中将参考激光束和新激光束所包含激光点作为观测量z。

表示参考激光束中的N个激光点。它们各自的可能的关联状态映射为条件随机场图中的节点。

表示新激光束中的M个激光点。条件随机场图中的任一节点x_i都对应了M+1种状态的似然。例如，x_i的状态1表示x_i所对应的参考激光束中的激光点

与新激光束的激光点关联的似然。x_i状态2表示

与新激光束的激光点

关联的似然，依次类推，状态M+1表示

在新激光束中找不到相关联的激光点的似然，即，

是孤立点的似然。φ_l(x_i)表示局部势，φ_p(x_i，x_j)表示配对势。它们基于局部和配对特征，及学习得到的相应权重，从两帧激光束的激光点在机器人中心坐标系下的笛卡尔坐标算得。

第二步，将移动机器人在相邻两个采样时刻测得的激光束作为一对。相邻两激光束分别为：参考激光束L_ref和新激光束L_new，每束激光束由所包含的激光点在机器人坐标系下的笛卡尔坐标组成。它们可进一步表示为：

$L_{\mathrm{ref}}\stackrel{\mathrm{\Δ}}{=}{\left\{\right[L_{{\mathrm{ref}}_x}^i,L_{{\mathrm{ref}}_y}^i\left]\right\}}_{i=1}^N,$ $L_{\mathrm{new}}\stackrel{\mathrm{\Δ}}{=}{\left\{\right[L_{{\mathrm{new}}_x}^i,L_{{\mathrm{new}}_y}^i\left]\right\}}_{i=1}^M$

其中参考激光束和新激光束分别包含N和M个激光点。

第三步，提取第一步得到的相邻时刻的激光束中激光点之间的局部特征和配对特征。本实施例中提取两束激光束中激光点之间的距离、角、测地线和辐射等几何特征作为局部特征，提取配对序列、配对孤立点和配对距离作为配对特征。

第四步，如果已有现成的学习参数，则直接跳转下一步；在第一次使用本方法进行激光束匹配的误差估计时，需通过学习确定所构建条件随机场中各种特征权重。该步骤仅需进行一次即可，具体步骤是：

①.依据第二步，采集相邻的两帧激光束。用手工的方法标定它们所包含激光点之间的匹配关系，可使用图像等传感器的观测作为参考。

②.依据第三步，提取该相邻两帧激光束中激光点的局部特征和配对特征。

③.运行①②10次，得到10组激光束对和它们所对应的10组标定关联关系、局部特征和配对特征。

④.将③中的结果输入第一步所构建的条件随机场中，并使用最大伪似然(Maximum pseudo-likelihood，MPL)方法学习得到各种特征的最佳权重。

第五步，将第二步得到的相邻两帧激光束和第三步得到的局部特征和配对特征，以及学习得到的特征的权重输入第一步所构建的条件随机场图模型，通过和积(Sum-product)概率推理得到推理概率表P。P的每一列元素值对应了图模型中相应节点处于不同状态的似然，即参考激光束中任一激光点与新激光束中所有激光点相关联的似然。图4为推理概率表的示意图(以参考激光束和新激光束中均包含4个激光点为例)，推理概率表中的一列表征了参考激光束中一个点的所有可能状态关联的概率。例如，图中第一列的前四个元素分别表示参考激光束中的第一个激光点与新激光束中第一个、第二个、第三个、第四个激光点关联的概率；第五个元素表示该参考激光点为孤立点(找不到关联)的概率。图5是本实施例实测得到的推理概率表的三维可视化图(两激光束均包含240个激光点)。

第六步，将P中每一列中最大元素对应孤立点状态的列去除，即仅保留可找到关联关系的节点集{N_in}，对应地，整理P为P₁。

第七步，计算P₁中每一列的熵，并依据熵值的大小对节点集{N_in}中的节点进行选择，得到紧凑节点集N_com，具体步骤是：

①.将推理概率表P₁中的列依据节点集{N_in}中节点的熵从大到小排列。相应地，P₁演变为P₂。

②.计算所有节点熵的全局熵均值和局部熵均值，并找到熵值最接近这些熵均值的节点，将这些节点连同具有最大熵和最小熵的节点作为n个基准点

全局熵均值指用全部节点的熵求得的均值。局部熵均值指基于降序排列的节点的熵，在一个区间内求得的熵均值。图6显示了多种局部熵均值，比如，接近全局熵均值的节点将全局区间分为两个子区间：从最大熵的节点到全局熵均值节点的区间、从全局熵均值节点到最小熵节点的区间。在这两个区间内可分别计算1/2局部熵均值。依次类推，可计算1/4局部熵均值，1/8局部熵均值。在本实施例中n选取为5。

③.围绕每个基准点B_i选取m个节点。所选取的m×n个节点组成紧凑的节点集，它们可记为：对应N_com提取P₂中的m×n个列组成新的推理概率表P₃。在本实施例中m选取为5。

第八步，将P₃中每个节点的状态按照各自的推理概率降序排列，然后通过计算推理概率的全局均值和局部均值确定g个基准点

相应地，P₃重新组合为P₄，然后围绕各基准点选取h个状态，并最终确定该节点的紧凑的状态集。因每个节点均有各自的关联状态空间，所以状态空间限定需对紧凑节点集N_com中的所有节点依次操作，而且所对应的节点的紧凑状态集S_com也表现为(m×n)×(h×g)矩阵，相应的，推理概率表P₄进一步演化为P₅。在本实施例中g和h分别选取为3和5。

第九步，通过采样紧凑节点集N_com和紧凑状态集S_com，求取激光束匹配的协方差，具体步骤是：

①.重复如下操作30次

②.在N_com中，分别从围绕n个节点基准点的节点子集中选取n个节点。即，从每一个

子集中都随机地选取一个节点。则该n个节点分别来自不同误差等级的节点子集。

③.对于选取的n个节点中的每一个，分别从围绕g个状态基准点的状态子集中选取g个状态。

④.基于该n个节点，及其各自的g个状态，构建gⁿ个相邻激光束中激光点关联配置。依据该n个节点的索引，从参考激光束中找出对应的n个激光点的机器人中心坐标系下的笛卡尔坐标。依据步骤③的结果，对应每个节点，从新激光束中找出g个激光点的机器人中心坐标系下的笛卡尔坐标。

⑤.基于步骤④的结果，计算出gⁿ组相对运动的解。每组解RT＝[R，T](R代表机器人的航位角，T代表机器人的位移)可通过优化下式来求得，

$\mathrm{RT}\←\arg \underset{R,T}{\min }\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left|\right|R{L}_{\mathrm{new}}^{S_i}+T-L_{\mathrm{ref}}^i\left|\right|$

其中，S_i代表构建一条激光点关联配置时，为参考激光束中激光点

所对应节点选取的状态，它是新激光束中一个激光点的索引。

⑥在构建每条激光点关联配置时，同时记录推理概率表P₅中每个对应元素的值，即，所涉及节点的某个状态的推理概率，然后将它们相乘便得要求的似然。关联配置的似然

作为每组相对运动解RT的权重。

加权协方差计算为：

${\mathrm{\Σ}}_{\mathrm{RT}}=\frac{1}{V}\underset{i=1}{\overset{g^n}{\mathrm{\Σ}}}{\mathrm{\Λ}}_{C_i}({\mathrm{RT}|}_{C_i}-{\mathrm{\μ}}_{\mathrm{RT}}){({\mathrm{RT}|}_{C_i}-{\mathrm{\μ}}_{\mathrm{RT}})}^T$

其中 ${\mathrm{\Λ}}_{C_i}={\mathrm{\Π}}_{S_{N_j}\∈C_i}P\left(S_{N_j}\right),$ $V={\mathrm{\Σ}}_{i=1}^{g^n}{\mathrm{\Λ}}_{C_i}$

这里，表示基于激光点关联配置C_i计算出的相对运动解。

本发明中激光束匹配误差的估计依据机器学习的原理得到，即通过紧凑地采样概率图模型的推理概率表，将激光点关联误差映射到相对运动解空间的协方差。

本实施例中，实验地点为某办公室内的一个真实长廊环境。依据引证背景文献所述，在长廊环境中实施基于激光束匹配的机器人定位时，因缺少空间限定，沿长廊方向上的激光束匹配的误差将远远大于其它方向。该特性可验证本发明所提方法的有效性。如图7所示，本实施例中机器人平台在长廊中沿A点至E点方向运动并采集激光束数据。由本发明提供方法估计的机器人处于A，B，C，D和E五个方位时的激光束匹配误差的3-σ误差椭圆分别以这五个方位点为圆心绘制于图7。相应的标准差列于下表：

同时，同类方法(见引证背景文献)所求得的激光束匹配的协方差用来与本发明所提供方法做对比，其标准差也列于上表，相应的误差椭圆也同样以机器人方位为圆心绘制于图7。为便于可视化，仅展示了在X-Y方向的相对运动估计误差。将每一组激光束的测量值基于激光束获得时刻机器人的方位通过空间合成由机器人中心坐标系转换至全局坐标系下可绘制出视野内景物的轮廓，本实施例中实验生成的激光束地图也示于图7。分析结果可知，当机器人运动至A点处，传感器视野内的空间限定完全来自长廊的两面墙壁，这接近典型长廊环境，如上表第一行所示，使用本发明所提供方法估计的X方向激光束匹配的标准差是Y方向的2.5倍，而同类方法的估计结果却近似为等方的，即X方向上的估计误差近似与Y方向上相等。图7所示，A点处本发明所提供方法估计误差的3-σ误差椭圆X方向的轴明显长于Y方向的轴。机器人运动至B点时，传感器的视野内出现一扇打开的门，为激光束匹配在X方向的估计提供了一定的空间限定，且缺少了少量Y方向的空间限定，这引起激光束匹配沿X方向的估计标准差增大，而Y方向的估计标准差减小，但X方向的估计误差依然大于Y方向。同类方法此时的估计结果却与此相反。机器人运行至C点时，传感器的视野中又呈现为接近理想的长廊环境，X方向估计的不确定性再次增大，且仍大于Y方向的误差。同类方法同样捕捉到了这点。机器人运行至D和E点时，长廊尽头墙壁为激光束匹配提供了X方向的空间约束，因此，X方向的估计误差持续变小，其中机器人处于E点时离提供空间约束的墙壁比处于D点时近，所以E点处激光束匹配在X和Y方向的估计误差都小于D点，但在这两点上Y方向的估计误差依然大于X方向的估计误差。如图7和上表所示本发明所提供方法可以捕捉到这种变化趋势，但同类方法却得到了相反的结果。

从实施例的结果看，采用本实施例所述方法的激光束匹配误差估计比同类方法在真实环境中更合理。本实施例所提出的方法在激光束匹配的误差估计中有良好的效果。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (4)

1.一种适用于移动机器人导航的激光束匹配误差估计方法，其特征在于，所述方法包括如下步骤：1)针对激光束匹配误差估计问题的特点构建条件随机场概率图模型，此步骤仅需执行一次；

2)将移动机器人在相邻两个采样时刻测得的激光束作为一对；

3)提取得到2)中所述激光对中激光点之间的局部特征和配对特征；

4)如果具有现成的局部特征和配对特征的权重，则直接进行下一步，否则先针对所构建的条件随机场概率图模型进行权重参数的学习，再进行下一步；

5)将相邻时刻采集的激光束、局部特征和配对特征、局部特征和配对特征的相应权重输入至步骤1)中所构建的条件随机场，通过和积概率推理得到推理概率表P；

6)将P中每一列中最大元素对应孤立点状态的列去除，仅保留可找到关联关系的节点集{N_in}，相应地演化出新的推理概率表P₁；

7)计算P₁中每一列的熵，并根据熵值的大小对节点集{N_in}的节点进行选择，得到紧凑节点集N_com，相应的，演化出新的推理概率表P₃；

8)将每一个节点的状态按照各自的推理概率降序排列，然后通过计算推理概率的全局均值和局部均值，确定该节点的紧凑状态集S_com，相应的，推理概率表进一步演化为P₄，随后又演化为P₅；

9)利用步骤7)得到的紧凑节点集N_com和步骤8)得到的紧凑状态集S_com，通过采样，求取激光束匹配的协方差。

2.如权利要求1所述的一种适用于移动机器人导航的激光束匹配误差估计方法，其特征在于，所述对局部特征和配对特征的权重的学习过程包括如下步骤：

a.依据步骤2)采集相邻的两帧激光束，用手工的方法标定它们所包含激光点之间的匹配关系；

b.提取该相邻两帧激光束中激光点的局部特征和配对特征；

c.运行步骤a和步骤b10次，得到10组激光束对和它们所对应的10组标定关联关系、局部特征和配对特征；

d.将步骤c中的结果输入步骤1)所构建的条件随机场概率图模型中，并使用最大伪似然学习法得到各种特征的最佳权重。

3.如权利要求1所述的一种适用于移动机器人导航的激光束匹配误差估计方法，其特征在于，求得所述紧凑节点集N_com的具体步骤为：

(1)将推理概率表P₁中的列依据节点集{N_in}中节点的熵从大到小排列，相应地，P₁演变为P₂；

(2)计算所有节点熵的全局熵均值和局部熵均值，并找到熵值最接近这些熵均值的节点，将这些节点连同具有最大熵和最小熵的节点作为n个基准点

(3)围绕每个基准点选取m个节点，所选取的m×n个节点组成紧凑的节点集N_com，对应N_com提取P₂中的m×n个列组成新的推理概率表P₃。

4.如权利要求1所述的一种适用于移动机器人导航的激光束匹配误差估计方法，其特征在于，所述求取激光束匹配的协方差的具体步骤为：

①在N_com中，分别从围绕n个节点基准点的节点子集

中选取n个节点，即从每一个

子集中都随机地选取一个节点；

②对于选取的n个节点中的每一个节点，分别从围绕g个状态基准点的状态子集中选取g个状态；

③基于该n个节点及其各自的g个状态，构建gⁿ个相邻激光束中激光点关联配置；依据该n个节点的索引，从参考激光束中找出对应的n个激光点的机器人中心坐标系下的笛卡尔坐标；依据步骤②的结果，对应每个节点，重新在激光束中找出g个激光点的机器人中心坐标系下的笛卡尔坐标；

④基于步骤③的结果，计算出gⁿ组相对运动的解；

⑤在构建每条激光点关联配置时，同时记录推理概率表P₅中每个对应元素的值，即所涉及节点的某个状态的推理概率，然后将它们相乘便得要求的似然；关联配置的似然作为每组相对运动解的权重，据此求取激光束匹配的加权协方差；

⑥重复以上步骤30次。

Images