CN103901891A - 一种基于层次结构的动态粒子树slam算法 - Google Patents

Abstract

本发明属于人工智能领域，具体为一种基于层次结构的动态粒子树SLAM算法。本发明算法能有效的提升SLAM算法的稳定性，通过层次化的结构使得概率模型具有两种不同分辨率视角，低分辨率的全局模型有利于全局信息的生成与维护，并且能够保持地图的重叠与闭合，为导航系统生成拓扑图等信息打下基础；另一方面具有较高精度的低层次局部信息能够很好的保持小块区域内的导航精度，并不重复带入累积误差，使得整个算法在长时间运行的状态下仍然保持误差在可接受的水平范围内。动态更新的粒子树可以很好的降低时间、空间复杂度。

Description

一种基于层次结构的动态粒子树SLAM算法

技术领域

本发明属于人工智能领域，具体涉及一种基于层次结构的动态粒子树SLAM算法。

背景技术

机器人学从上世纪90年代起发展迅速。在导航定位领域，机器人需要对自己的位置和整个场景地图进行估计，这对于概率建模而言，无论在计算上，还是可操作性上都是很难实现的，因此引入统计学中的采样来完成这一任务。粒子滤波算法的提出使得后续的各种基于蒙特卡罗方法的导航算法在实际应用中有比较稳定的表现，特别是SLAM（Simultaneous localization and mapping）算法的提出，让机器人导航算法有了巨大的进步。

目前机器人导航问题可以划分为2大类：一类是室外自然环境下基于GPS系统的导航，另外一类是室内环境下的导航定位。室内环境又可分为大面积空旷场地（例如：实验室，大厅，楼层等）和窄小空间场地（例如：家庭环境、过道、窄巷等），有些室内机器人需要事先在场景中人为放置标志物(如RFID)，才能进行定位导航。本发明所研究家庭环境下的导航的特点是环境复杂，空间狭小，而且会经常出现动态物体的移动，同时机器人在移动的时候，还必须避免与家具及墙壁的碰撞。由于家庭环境的复杂性和多变性，一般的GPS定位算法和基于标定物的算法无法满足家庭环境使用需求，而SLAM算法非常适合解决该问题。

SLAM算法是一种不断更新环境与自身信息的动态算法。由于在SLAM过程中存在很多不确定性因素，如环境干扰、测量误差、运动误差、移动障碍而导致性能下降，并且在有大吞吐量的传感器(如激光扫描传感器)输入情况下，SLAM算法的更新与存储速度难以满足机器人实际实时应用需求。

发明内容

本发明的目的在于一种提供能满足机器人实时应用需求的，定位高精度高，适应复杂的家庭环境及其变化的基于层次的动态粒子树SLAM算法。

本发明提出的基于层次的动态粒子树SLAM算法，是通过层次式存储、动态粒子树更新的方式实现了增量式信息更新保存，大幅度减少动态更新时地图的存储量，并且可以在线性时间内完成更新操作，满足实时性需求，同时能够高效地完成导航和定位任务，提高精度，适应复杂的家庭环境及其变化。具体步骤为：

1，建立数据模型，数据模型包括机器人运动模型和激光数据模型；

1.1机器人运动模型。在机器人系统采用的运动模型中，把机器人抽象成笛卡尔坐标系中的一点，它的正向方向与x轴的夹角是机器人的转向角θ，坐标(x,y)是机器人在地图中的位置。

在这一模型下，机器人的每一次运动都可以看作是从坐标(x,y,θ)到(x',y',θ')，其中位置移动为两点间的欧式距离，如公式(1)所示。

${\mathrm{\δ}}_{\mathrm{trans}}=\sqrt{{(x^{\′}-x)}^2+{(y^{\′}-y)}^2}---\left(1\right)$

转向角的变化分为移动转向角和原地转向角，如下：

δ_rot1=a tan2(y'-y,x'-x)-θ   (2)

δ_rot2=θ'-θ-δ_rot1   (3)

实际应用中，通常存在着较大的误差，采用高斯误差进行估计，即在原有的(x，y，θ)上添加均值为0，方差为σ²的高斯分布N(0，σ²）。所以从粒子滤波角度来说，机器人的位置是一个在地图空间上概率分布p(s_t|u_t，s_t-1)。

其分布如图1，图1是使用不同参数的高斯噪声后，对运动模型概率分布的采样结果。可以看出机器人每一次运动后，新的位置会分布在真实位置周围。因此机器人行进的路程越长，累计的误差会越大，实际实验情况也符合这一推论。

1.2激光数据模型。激光传感器可以抽象成从原点发射出一定数量的射线（通常为180条），当遇到障碍时则停止发射，返回遇到障碍时的距离。

图2中的黑圆代表机器人当前所在的位置，从它为中心出发的射线在遇到障碍时会停下，可以得出障碍里机器人的大致距离，同时估计机器人周围的地图存在障碍的概率。右图中的颜色越深，则代表存在障碍的概率越大。当某条射线超过激光的最大可探测距离时，认为在那个位置上存在障碍。

对于每一条射线，它的实际作用范围是一个扇形，如图3所示。在扇形的圆弧上，障碍物存在的概率是一个平缓的高斯分布（可使用线性模型进行简化），中央点拥有最高的可能性Pocc(max),而圆弧两边只有Pocc(min)。同时在扇形内部，也存在一个障碍概率下降曲线，离原点越近，该点是非障碍的概率越高。原点的概率可设定为P_empty(max)。在每次得到激光的数据后，就可以得到类似图3的概率地图。

根据这一模型，对每条激光射线都可以估计出在射线扫描范围内地图主要信息，即p(z_i|s,m)。因为每条射线都是相互独立的，因此激光概率模型可以让计算机按公式(4)估算出概率分布。

p(z|s,m),z=(z₁,z₂,...,z_k)   (4)

2，进行动态粒子树SLAM算法的计算

2.1基于粒子滤波的状态估计。SLAM算法的原理是机器人在未知环境中从一个未知位置开始运动，在运动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。SLAM算法的核心部分之一，就是在已知观察序列（各个时刻传感器的数据）和操作序列（各个时刻对机器人运动指令）的条件下，计算当前机器人的状态(位置，转向角)以及更新地图信息，并持续对该序列进行更新。由于机器人的运动以及传感器的数据采集都属于概率模型，在给定的时刻t以及从0-t的观测序列后，根据贝叶斯概率模型，可以得到公式(5)：

p(s_1：t，m|z_1：t，u_0：t)＝p(m|s_1：t，z_1：t)p(s_1：t|z_1：t，u_0：t)   (5)

其中，s_1:t是机器人的状态信息向量s_i=(x_i,y_i,θ_i)。m代表二维地图信息，它是一个矩阵，每一个元素代表实际场景中对于采样块的障碍信息，即该采样块是障碍的概率。z_1:t代表每一个时刻传感器的数据信息，它是对机器人周围环境的具体描述，也是绘制地图的重要信息。u_0:t则是对机器人的基本操作指令。

但是直接对这个概率进行计算是不可行的，虽然在实际计算过程中可以将运动轨迹s_1:t简化为前一阶段的数据s_t，但是在一般的室内环境下，m本身是一个高维信息，所以需要根据概率分布进行采样。选择粒子滤波算法对这个概率模型进行描述，机器人状态粒子Particle为机器人位置与地图信息：

Particle_n={x,y,θ,m_n}；

粒子滤波的主要步骤包括：

（1）采样。已知前一阶段的粒子状态s_t-1，根据p(s_t|z_1:t,u_0:t)求出这一阶段所有粒子可能产生的状态s_t。

（2）计算每一个粒子的权重。每个粒子的和真实概率的比值，对于那些和实际概率差距很大的粒子将会在下一步舍去。

（3）重采样。为了保持一定的粒子数量，舍去那些权重比较低的粒子。在一段迭代次数后，机器人的状态和地图信息将会收敛到与真实环境较接近的位置。

（4）地图信息估计。对于每一个粒子，都对应了一个地图，可以根据p(m_t|s_1：t，z_1：t)来进行计算。

在第（4）步中，SLAM算法将为机器人的整个运动轨迹和环境地图的信息构建一个概率分布，并从中得出最有可能的数据。

2.2动态粒子树SLAM算法

动态粒子树SLAM算法的核心思想是为环境地图的每一个采样点（块）维护一棵粒子树，树中的所有粒子都已完成它所在的采样点的更新。因为每次激光所能扫描到的范围远远小于整个场景地图的面积，而这些数据只是更新了地图的很小的部分，所以没有必要为每个粒子保存整张地图，只需保存每个采样点（块）的历史更新记录。

图4中是一棵粒子树，它的叶子结点是重采样后产生的新粒子(p₁,p₂,p₃,p₄)，其中p₁对应了地图中方格(5,1)（粒子树右边的矩阵方格代表地图）；(p₂,p₃,p₄)则同时对(4,1)方格做了更新。所以对每个粒子来说，它都拥有了一张完整的环境地图，而树模型的引入则大幅度地减少了存储量。

在粒子树中，父结点在重采样过程后，生成一些新的粒子，作为下一阶段使用，这些粒子作为它的子结点。同时，某一粒子在对地图作更新时，直接把传感器的数据信息保存在树的结点中，无需到地图上更改。所以在粒子树的任意一个结点为根的子树中，所有粒子都满足祖先所观察的数据。

另一方面，粒子树的任何内结点都至少有两个子节点。对于没有子节点的粒子结点，说明这个粒子没有产生有效的新粒子，它所包含的信息与真实的数据误差过大，因此可以把这个粒子从树中删除；对于只有一个子节点的粒子，可以把子节点和父节点的传感器数据进行合并，等效成一个粒子，然后将子结点删除。通过这一步，整个粒子树在导航过程中将始终保持一定的大小，且不会无限制的增长。

最后，在生成地图时，对于每一个采样点，只需检查该方格上最近访问的粒子，然后提取它们所保存的数据信息，计算这个方格存在障碍物的概率。

3，层次化粒子信息分析

传统的SLAM算法受各种误差的影响，使得在实际情况下，定位和绘制地图的效果不佳。误差的来源主要有：（1）电机读数的误差，使得机器人位置不确定性增大；（2）进行粒子滤波时，重采样产生的误差（舍去了权重低的粒子）；（3）对环境建模时采样大小的误差，在构建地图时，每一个最小单元都对应于实际空间中的一块固定大小的矩形。其中第一部分产生的误差，在机器人在一个比较大的封闭场景扫描一遍后，造成绘制的地图无法闭合，甚至发生扭曲和重叠。

图5显示了电机误差所造成的影响，(b)是正确的环境地图，白色是无障碍，黑色是边沿障碍物，灰色是未探索区域；(a)是单纯地使用电机的数据来对位置进行更新计算，最后绘成的地图。可以看出在机器人扫描一定距离之后，机器人的真实位置和算法估计出的位置偏差逐渐增大，尤其是转向角的偏移，所以在(a)图中会出现几个不同的重影，因为角度的偏移造成了前后数据无法重合到一起。

所以在SLAM算法中，导航时地图的闭合是一个很重要的任务。本发明提出层次算法能够很好的解决这一问题，它的基本思想是在低层依然使用前一部分所提到的算法，但低层只是处理整个环境中的一部分，然后将这一部分的结果“提交”给高层过程，它对各个低层所产生的局部地图进行重组，制作出完整的地图（如图6）。

在一块小的区域内，机器人的位置误差不会很大，而且也不会累积到下一次低层SLAM计算，因为每次低层SLAM都只处理所给的一块，相当于低层SLAM所在的环境只是整个地图中的一小块；同时也可以减少粒子数的使用。

层次化SLAM算法给导航和地图绘制提供了多分辨率操作：低层的SLAM可以比较精确地探知一小块地图，然后得到机器人的行走轨迹和小地图的概率分布p(s_1:t,m|z_1:t,u_0:t)，而传统的SLAM算法一般只针对当前的状态，而不对整个轨迹进行计算。将得到的轨迹和地图信息提交给高层后，高层中的每个粒子包含的将是机器人的轨迹和地图数据。最后高层SLAM过程便可以对这些粒子进行权重计算，重采样等操作，得到新的高层地图。

本发明在动态粒子树SLAM算法基础上提出了一种基于层次结构的动态粒子树算法，通过分层次操作有效的维持了局部区域内的精度，并在全局上能保证地图的闭合，这使得算法能够很好的适应空间的变化。

附图说明

图1：对运动模型的采样结果。

图2：激光探寻数据示意图。

图3：激光概率模型。

图4：粒子粒子树模型。

图5：电机里程误差对绘制地图的影响。

图6：层次化算法描述。

图7：高层次操作。

图8：低层次操作。

图9：FAST-SLAM、LABDPF-SLAM算法下机器人轨迹图与虚拟采样点。

图10：误差对比图。

具体实施方式

本系统的试验平台是肢体化智能实验室开发完成的Artoo机器人。本导航系统所涉及到的硬件环境包括以下二个部分（这里省略了与导航关系不大的组件）。

表1运动单元和传感器参数

实验在仿真环境下进行，场景为室内，总体是一个环形的走廊。为了视图的清晰，图片中灰色部分为空白区域，黑色为障碍（墙壁）。图7将分别列出运行过程中高层次全局操作和低层次局部操作产生的地图信息。

图7从(a)-(f)按照时间序排列，相邻两幅之间，因为低层次操作的作用，更新了其中一块地方。而图8则是低层次产生整个环境地图中的一部分的情况。

可以看出，低层次的每一部分都对应了整个地图的一小块，而且由于规模比较小，所以控制了造成的误差的范围，最后生成的地图和实际比较接近。

采用FAST-SLAM算法与基于层次结构的动态粒子树SLAM算法进行比对，在实验环境中使用激光测距仪精确测量（精度误差<2CM），将实际机器人轨迹与算法预测位置进行比较。

机器人采用定时采样的方式，每隔30s，进行虚拟采样，将比照机器人实际轨迹位置坐标与机器人导航系统中自建地图预计点坐标进行比对，并采用其欧式距离作为统计变量。图10中每个虚线小方格边长代表实际一米距离，对比两种算法在每个时刻采样点上的误差。

从误差分析结果来看，FAST-SLAM算法在累积探索的过程当中，不断地累积运动误差与环境误差，使得导航过程随着时间增长逐渐并变得误差更大，误差水平无法保持在一个能够接受的范围内，而基于层次结构的动态粒子树SLAM算法由于采用了层次化结构，从而使得局部的低层次信息保持较高的精度，使得运动估计误差相对稳定，能够获得更高的可靠性。

Claims (1)

1.一种基于层次的动态粒子树SLAM算法，其特征在于具体步骤为： 

（1）建立数据模型，数据模型包括机器人运动模型和激光数据模型： 

（1.1） 建立机器人运动模型

在机器人系统采用的运动模型中，把机器人抽象成笛卡尔坐标系中的一点，它的正向方向与                                                轴的夹角是机器人的转向角

，坐标

是机器人在地图中的位置；

在这一模型下，机器人的每一次运动都可以看作是从坐标

到

，其中位置移动为两点间的欧式距离，如公式(1)所示：

                         (1)

转向角的变化分为移动转向角和原地转向角变化，具体如下：

                   (2)

                                (3)

对于存在着较大的误差，采用高斯误差进行估计，即在原有的

上添加均值为0，方差为

的高斯分布

；所以从粒子滤波角度来说，机器人的位置是一个在地图空间上概率分布

；

机器人每一次运动后，新的位置会分布在真实位置周围；

（1.2） 建立激光数据模型

把激光传感器抽象成从原点发射出一定数量的射线，当遇到障碍时则停止发射，返回遇到障碍时的距离；

机器人当前所在的位置为中心，从它出发的射线在遇到障碍时会停下，从而得出障碍里机器人的大致距离，同时估计机器人周围的地图存在障碍的概率；当某条射线超过激光的最大可探测距离时，认为在那个位置上存在障碍；

对于每一条射线，它的实际作用范围是一个扇形；在扇形的圆弧上，障碍物存在的概率是一个平缓的高斯分布，中央点拥有最高的可能性

, 而圆弧两边只有

；同时在扇形内部，也存在一个障碍概率下降曲线，离原点越近，该点是非障碍的概率越高；原点的概率设定为；在每次得到激光的数据后，就得到概率地图；

根据这一模型，对每条激光射线估计出在射线扫描范围内地图主要信息，即

；激光概率模型按公式(4)估算出概率分布：

                          (4)

（2）采用动态粒子树SLAM算法进行计算 

（2.1）进行基于粒子滤波的状态估计 

SLAM 算法的原理是机器人在未知环境中从一个未知位置开始运动，在运动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图；其核心部分之一，就是在已知观察序列和操作序列的条件下，计算当前机器人的状态以及更新地图信息，并持续对该序列进行更新；由于机器人的运动以及传感器的数据采集都属于概率模型，在给定的时刻

以及从的观测序列后，根据贝叶斯概率模型，有公式(5)：

   (5)

其中，

是机器人的状态信息向量

，

代表二维地图信息，它是一个矩阵，每一个元素代表实际场景中对于采样块的障碍信息，即该采样块是障碍的概率，

代表每一个时刻传感器的数据信息，它是对机器人周围环境的具体描述，也是绘制地图的重要信息，

则是对机器人的基本操作指令；

根据概率分布进行采样，选择粒子滤波算法对这个概率模型进行描述，机器人状态粒子为机器人位置与地图信息：

粒子滤波的步骤包括：

（1）采样，已知前一阶段的粒子状态

，根据

求出这一阶段所有粒子可能产生的状态

；

（2）计算每一个粒子的权重，每个粒子的和真实概率的比值，对于那些和实际概率差距很大的粒子在下一步舍去；

（3）重采样，为了保持一定的粒子数量，舍去那些权重比较低的粒子；在一段迭代次数后，机器人的状态和地图信息会收敛到与真实环境较接近的位置；

（4）地图信息估计，对于每一个粒子，都对应了一个地图，根据

进行计算；

在步骤（4）中，SLAM算法为机器人的整个运动轨迹和环境地图的信息构建一个概率分布，并从中得出最有可能的数据；

（2.2） 动态粒子树SLAM算法

动态粒子树SLAM算法是为环境地图的每一个采样点维护一棵粒子树，树中的所有粒子都已完成它所在的采样点的更新；

粒子树的叶子结点是重采样后产生的新粒子，对每个粒子来说，它都拥有一张完整的环境地图； 

在粒子树中，父结点在重采样过程后，生成一些新的粒子，作为下一阶段使用，这些粒子作为它的子结点；同时，某一粒子在对地图作更新时，直接把传感器的数据信息保存在树的结点中，无需到地图上更改；所以在粒子树的任意一个结点为根的子树中，所有粒子都满足祖先所观察的数据；

另一方面，粒子树的任何内结点都至少有两个子节点；对于没有子节点的粒子结点，把它从树中删除；对于只有一个子节点的粒子，把子节点和父节点的传感器数据进行合并，等效成一个粒子，然后将子结点删除； 

最后，在生成地图时，对于每一个采样点，只需检查该方格上最近访问的粒子，然后提取它们所保存的数据信息，计算这个方格存在障碍物的概率；

（三）层次化粒子信息分析

采用层次算法进行分析，层次算法的基本思想是在低层依然使用前一部分所提到的算法，但低层只是处理整个环境中的一部分，然后将这一部分的结果“提交”给高层过程，它对各个低层所产生的局部地图进行重组，制作出完整的地图；具体来说：

层次化SLAM算法给导航和地图绘制提供多分辨率操作：低层的SLAM算法可以比较精确地探知一小块地图，然后得到机器人的行走轨迹和小地图的概率分布

；将得到的轨迹和地图信息提交给高层后，高层中的每个粒子包含的是机器人的轨迹和地图数据；最后高层SLAM算法过程对这些粒子进行权重计算，重采样等操作，得到新的高层地图。

Images