基于多超声传感器的移动机器人同步定位与地图构建方法与流程

本发明属于机器人技术领域，特别涉及一种基于多超声传感器的移动机器人同步定位与地图构建方法，可用于智能机器人控制。

背景技术：

随着科学技术的进步，机器人发展日新月异，这种自动执行工作的移动机器人既可以接受人类指挥，又可以运行预先编排的程序，还可以根据人工智能技术指定的原则行动，从而协助或取代人类的一些工作。例如人们日常生活经常接触到的家用机器人，它与人们生活息息相关，在家庭服务、娱乐、清洁方面的应用已经非常广泛，给人们的生活带来很大的帮助，已经成为人类未来生活不可分割的一部分。然而要实现以上功能，不可避免的，机器人在移动过程中，面临三个主要问题：在哪里，去哪里，怎么去？而这些问题的核心，就是关于移动机器人的导航技术。由于机器人受体型和活动环境的限制，现有机器人通常是基于已知环境地图或预设路径实现行走，甚至随机游走，这些行走方式不仅不够灵活、效率低，且对机器人机身具有破坏性，而且不能保证在动态可变的环境中智能做出行走决策，因此，移动机器人要实现自主运动，要解决的关键问题就是自身的定位问题以及对周围环境的感知问题。

移动机器人需要知道自己在哪个位置，然后根据自己的目标和周围环境地图进行路径规划，规划出最佳的路径，然后执行、完成更高层级的自主任务。移动机器人在所处未知环境下进行路径规划，需要感知周围环境，获取环境信息并判断自己的所处位置，即同步定位及地图构建。而机器人对于自身的定位和环境感知，可以通过多种不同的传感器来实现，包括超声传感器、红外传感器、视觉传感器以及激光雷达传感器，不同传感器的工作原理各不相同，实现定位和地图构建的方案也不一样。

现有移动机器人多采用激光雷达传感器或视觉传感器进行导航。使用激光雷达传感器实现同步定位及地图构建的机器人通常装有一个旋转装置，通过不断旋转激光发射器对周围环境进行360度的扫描，得到非常稠密的数据信息，从而实现精细测量，因此定位和导航效果相对较好，然而激光雷达成本太高无法大批量应用，配合其实现360度扫描过程的旋转部分也是增大成本一个原因，并且这种机械式的旋转致使旋转部分很容易磨损，严重制约了激光雷达的使用寿命，导致大部分激光雷达的使用寿命不超过一年；使用视觉传感器来实现移动机器人导航，成本相对较低，但采集到的环境信息太过丰富，从中提取有用信息需要经过大量计算，不适合在计算能力有限的嵌入式平台上应用。

超声传感器的原理简单，成本较低，且无须对数据本身进行大量处理，因而在移动机器人平台上有具有广阔的应用前景。但对于超声传感器的使用，由于超声测量精度有限，在现有移动机器人技术中往往只利用该其进行避障，很少应用于定位及构建地图，大部分的移动机器人平台上仅配备了少量超声传感器模块，所得超声数据太过稀疏，其带来的信息不足以实现移动机器人的同步定位和地图构建，无法满足移动机器人路径规划的要求。

技术实现要素：

本发明的目的在于针对上述现有方法的不足，提出一种基于多超声传感器的移动机器人同步定位与地图构建方法，通过在移动机器人装置周围均匀布置较多数量的超声传感器，从而在不引入旋转部件的情况下增大对周围环境的扫描范围，并组合多次扫描的数据，利用粒子滤波的方法实现定位；采用随障碍物距离指数衰减的测量模型，进行精确的地图构建；从而实现低成本下的超声波高精度定位。

为了实现上述目的，本发明基于多超声传感器的移动机器人同步定位与地图构建方法的技术方案包括如下步骤：

(1)设置超声传感器：

在机器人机身周围的同一水平面上均匀布置至少16个超声传感器，且机身前后两部分的超声传感器对称分布；

(2)获取超声测量数据：

部署在机器人机身的超声传感器通过检测从发送超声波到接收到回波的时间，获取超声传感器与正前方障碍物之间的距离，即超声测量数据；在连续时刻，多次获取超声测量数据；

(3)组合所获取到的超声测量数据：

3.1)设定对所获取到的超声测量数据的组合次数为n，且n为大于0的自然数；

3.2)对所获取的超声测量数据进行坐标变换，设第一个测量时刻移动机器人在世界坐标系下的位姿为ta，第一个测量时刻的测量值za，第二个测量时刻移动机器人的位姿为tb，第二个测量时刻的测量值为zb；

通过下式变换得到第二个测量时刻测量值zb在第一个测量时刻对应坐标系下的测量值zb2：

zb2＝ta^{-1}*tb*zb；

将zb2与za叠加作为新的第一时刻测量值za0；

3.3)根据步骤3.2)的变换方式亦可得到第三个测量时刻测量值zc在第一个测量时刻对应坐标系下的测量值zc2：

zc2＝ta^{-1}*tc*zc；

其中，tc为第三个测量时刻移动机器人的位姿，zc为第三个测量时刻的测量值；将zc2再与za0叠加，以此类推，便可得到多个超声测量数据在同一坐标系下的叠加值，即组合超声测量数据；

(4)判断是否达到设定的组合次数：

判断步骤(3)中的测量数据的叠加是否达到设定的组合次数n，得到其全部数据的叠加值；若是则执行步骤(5)，若不是则返回步骤(2)；

(5)预测机器人的当前位置：

设上一时刻的机器人位置已知，根据上一时刻到当前时刻机器人收到的运动指令，结合定位算法，对当前时刻的机器人位置进行预测；

5.1)求解机器人收到运动指令后的当前位置估计：

设上一个时刻机器人的坐标为(x1，y1，theta1)，接收到的运动指令为(dx，dy，dtheta)，得到机器人收到运动指令后的坐标为(x2，y2，theta2)，即当前位置估计；

其中：

x2＝x1+dx；

y2＝y1+dy；

theta2＝theta1+dtheta；

5.2)计算机器人的当前时刻预测位置：

在位置估计的基础上加上一定噪声，通过随机采样的方式，选择均值为零的高斯噪声作为一种扰动(dx’，dy’，dtheta’)，设选取三十个粒子，其中每一个粒子表示机器人位置的一种可能性，这三十个粒子可产生三十个扰动，在当前位置估计的基础上加上这个扰动，得到三十个不同的位置，将这些位置作为机器人的当前时刻预测位置；

(6)确定机器人的当前时刻最终位置：

在根据预测位置已经构建出了部分地图的情况下，通过假设机器人的当前位置，计算出机器人相对于已知的部分地图应该有的测量值，即预测的测量值，将这个预测的测量值与实际测量值对比，可得知机器人的当前时刻预测位置是否准确，即预测的测量值和实际测量值是否匹配；根据组合测量数据与已有地图的匹配情况，对不同粒子进行打分，得分最高的即为最匹配的位置，将该位置确定为机器人的当前时刻最终位置。

(7)根据机器人的当前时刻最终位置更新环境地图：

以步骤(6)中得到的机器人当前时刻最终位置作为基准，结合当前超声传感器的测量数据，更新地图中未部分知的区域，同时修正部分已知位置的地图值，从而更新地图。

本发明与现有技术相比具有如下优点：

第一，本发明使用超声传感器实现移动机器人同步定位及构建地图，相对于现有技术中采用其它传感器进行机器人导航，其成本较低；

第二，本发明采用组合多个超声传感器模块对周围环境进行感知，易于实现，其组合方式简单，便于安装，且无须旋转即可实现对周围环境的探知，不必受到旋转装置磨损所带来的不良影响；

第三，本发明方法考虑到机器人的微小位移，采用融合多次扫描数据的方式，获取小段时间内的扫描结果，弥补了超声数据太过稀疏的不足，从而提高了定位的精度和鲁棒性；

第四，本发明在给机器人位置进行匹配打分和地图更新时，采用指数衰减对超声数据进行建模，相比于单纯判断有没有障碍物的二值模型，提高了匹配度和更新地图的准确度。

附图说明

图1为本发明方法的实现流程图；

图2为本发明中移动机器人的超声传感器分布示意图；

图3为本发明中移动机器人构建的地图。

具体实施方法

下面结合附图对本发明作进一步详细描述。

参附图1，本发明的实现步骤如下。

步骤1，设置超声传感器。

在机器人机身周围的同一水平面上均匀布置16个及其以上的超声传感器，且机身前后两部分的超声传感器对称分布；通过部署在机器人机身周围的这些超声传感器在不引入旋转部件的情况下增大对环境中信息的感知，获得距离信息。

参照图2，以在机器人身上设置16个超声传感器为例，其设置方式如下：

分别在机器人机身前侧设置1至8号超声传感器，其间距相同、均匀放置；在机器人机身的后侧设置9至16号超声传感器，其间距相同、均匀放置；并且机器人机身前后两部分的超声传感器对称设置。

步骤2，获取超声测量数据。

利用步骤1中部署在机器人机身的多个超声传感器，获取机器人行走环境中的超声测量数据。

超声传感器通过检测从发送超声波到接收到回波的时间，从而获取超声传感器到正前方障碍物的距离，即超声测量数据；在连续时刻，多次获取超声测量数据。若机器人周围设置的超声传感器为16个，则一次获得16个读数，用以反应出机器人到周边物体的距离。

步骤3，组合所获取到的超声测量数据。

尽管机器人周围已经布置了16个超声传感器，同一时刻有16个方向的输入，但是相对激光雷达一圈有180或者360个数据来说还是太稀疏了，为此，需要组合一段较短连续时间内的超声传感器数据，形成多次扫描的数据，然后将其作为一次的输入，放到后续的定位算法中进行解算。例如，在连续10次的测量中，每次分别得到16个测量数据，理论上就共有160个测量数据，需要先把这160个测量数据组合起来，形成一个较为稠密的测量，然后把这个测量放到定位算法中进行处理，而不是直接把每一次的16个测量数据进行处理。

在连续的时刻之间，机器人也会有微小的位移，因此在组合多次扫描数据时，必须考虑到连续时刻之间机器人的微小移动。在平面内，移动机器人的位姿由平移量和朝向来表示，平移量在欧式空间，即为机器人的大地坐标，所以可以直接相加减，但朝向不是在欧式空间，所以必须采用变换的形式，然后对应的求逆和相乘，而不能直接相加减；所以在组合成多次扫描的数据时，必须加上一定的变换。

由于机器人是在平面上运动，其坐标用2d的位置(x，y)和朝向theta来表达。超声传感器的测量值为超声传感器位置到物体的距离，但是超声传感器的安装位置和朝向在机器人上是不同的，所以不妨把每个测量值用超声波和物体的接触点的坐标来表达，即三个自由度(x，y，theta)，将这个坐标点记为一个端点，那么组合扫描数据所要做的就是把多个端点都组合到一次测量中；

移动机器人总是处于不断运动的状态，即便是非行走中也会有很小的位移发生，因而其超声传感器的测量值，即端点的坐标总是相对于机器人自身坐标系而言，为了实现将多个端点组合到一次测量中这一目的，便需要把不同时刻下的端点坐标变换到同一个世界坐标系下，这样才能把多个端点的数据组合构成一次的测量值。

3.1)设定对所获取到的超声测量数据的组合次数为n，且n为大于0的自然数；

3.2)对所获取的超声测量数据进行坐标变换，设第一个测量时刻移动机器人在世界坐标系下的位姿为ta，第一个测量时刻的测量值za，第二个测量时刻移动机器人的位姿为tb，第二个测量时刻的测量值为zb；

通过下式变换得到第二个测量时刻测量值zb在第一个测量时刻对应坐标系下的测量值zb2：

zb2＝ta^{-1}*tb*zb；

将zb2与za叠加作为新的第一时刻测量值za0；

3.3)根据步骤3.2)的变换方式亦可得到第三个测量时刻测量值zc在第一个测量时刻对应坐标系下的测量值zc2：

zc2＝ta^{-1}*tc*zc；

其中，tc为第三个测量时刻移动机器人的位姿，zc为第三个测量时刻的测量值；将zc2再与za0叠加，以此类推，便可得到多个超声测量数据在同一坐标系下的叠加值，即组合超声测量数据。从而可实现小段时间内测量数据的组合，采用这种考虑到微小位移的融合多次测量数据的方式，能弥补超声数据太过稀疏的不足，提高了定位的精度和鲁棒性。

步骤4，判断是否达到设定的组合次数。

判断步骤3中测量数据的叠加是否达到设定的组合次数n，得到其全部数据的叠加值；若是则执行步骤5，若不是则返回步骤2；

令n＝10，则应对超声测量数据进行10次组合，以保证最终所得组合超声测量数据的精确程度符合要求，若没有达到10次，则返回步骤2继续获取超声测量数据，随后进入步骤3进行坐标变换及数据叠加，直到达到10次的组合设定次数。

步骤5，预测机器人的当前位置。

设上一时刻的机器人位置已知，根据上一时刻到当前时刻机器人收到的运动指令，结合定位算法，对当前时刻的机器人位置进行预测；

5.1)求解机器人收到运动指令后的当前位置估计：

由于机器人是连续运动的，其运动是一个累加的过程，所以当前的位置是由上一个时刻的位置和当前机器人对环境的测量值得到。假设上一个时刻的机器人位置已知，那么需要预测机器人的当前位置，预测的过程可以由从上一个时刻到当前时刻机器人收到的运动指令来决定。

设上一个时刻机器人的坐标为(x1，y1，theta1)，接收到的运动指令为(dx，dy，dtheta)，得到机器人收到运动指令后的坐标为(x2，y2，theta2)，即当前位置估计；

其中：

x2＝x1+dx；

y2＝y1+dy；

theta2＝theta1+dtheta；

5.2)计算机器人的当前时刻预测位置：

在位置估计的基础上加上一定噪声，对机器人的当前位置进行预测；由于机器人在运动过程中的噪声和打滑等因素，上面的运动模型所预测的机器人位置离机器人真实位置存在一定的偏差，如果只是单纯这样的累加下去，那么估计的机器人的位置会离真实值越来越远。而本算法的目的就是在有超声传感器的辅助下，能够实现持续、精确的对机器人位置进行估计。本算法的框架是基于粒子滤波，该算法的总体思想是用每一个粒子来表示机器人位置的一种可能性，那么比如三十个粒子就会代表有三十个不同的机器人的位置，然后通过超声传感器的测量数据，结合一定的评价体系，对这三十个位置进行打分，选择得分最高的位置作为机器人的位置。从而实现高精度的机器人的位置估计。

在步骤5.1)中已经获得了机器人在收到运动指令后对当前位置的估计，在位置估计的基础上加上一定噪声，通过随机采样的方式，选择均值为零的高斯噪声作为一种扰动(dx’，dy’，dtheta’)，设选取三十个粒子，其中每一个粒子表示机器人位置的一种可能性，这三十个粒子可产生三十个扰动，在当前位置估计的基础上加上这个扰动，得到三十个不同的位置，这些位置的分布都会比较靠近步骤5.1)中估计的位置，将这些位置作为机器人的当前时刻预测位置。

步骤6，确定机器人的当前时刻最终位置。

根据融合后的超声数据选择最佳的机器人位置，在步骤2中已经通过机器人机身的多个超声传感器，获取机器人在当前位置对周围环境的测量数据，即机器人到周围环境的距离；在步骤5中已经获取到机器人的当前时刻预测位置。

本算法是一个随着机器人的运动不断累加的过程，在这个过程中，同时也在不断地构建周围环境的地图，随着机器人运动范围的不断扩大，对周围环境构建的地图也在不断扩大。本算法采用的地图表示为栅格地图，即把整个环境分为很多小的栅格，设机器人的整体运动环境为10m*10m的空间，设栅格地图的分辨率为0.05m，即每0.05m*0.05m作为一个小栅格，把整个环境划分成200*200个格子，然后往每个格子里填一个0到1的数字，来表示这个小栅格中有物体的概率，数字越大，格子中有物体的概率越大，具体来说，用0表示这个格子中肯定没有物体，用1表示这个格子中肯定有物体，用0.5来表示这个格子中有没有物体是未知的，是机器人还没有探索过的区域，下文中称这个概率值为占据值，即这个栅格被占据的概率。这种表示方法虽然比较粗糙，只能表示在地图中的某片区域中有没有物体，但是对于机器人的导航与避障来说足够使用，机器人只要知道自己是在地图中的哪个位置，以及哪些格子中是有物体的，是需要避开的，就可以实现自主运动。本步骤在当前已经构建出了部分地图的情况下，通过假设机器人的当前位置，就可以计算出机器人相对于已知的部分地图应该有的测量值，通过对比这个估计出的测量值和真实的超声传感器的测量值数据，就可以得到预测的这个机器人当前位置是否准确，即对预测的机器人位置进行打分，并最后选择得分最高的位置，也就是预测的测量值和实际的真实超声传感器测量值最为匹配的位置，将该位置作为机器人的当前时刻最终位置；在下一个步骤中，以这个最终位置为准，结合当前的真实超声传感器的测量值，去更新部分地图中还未知的区域，同时也修正部分已知位置的地图值，实现更新地图的目的。

由于超声是以一个扇形发散出去的，距离越远，扇形的圆弧越大；只要圆弧上有较明显的障碍物，就会产生回波，而从这个回波并不能直接得到障碍物所在的精确位置。所以再利用传感器数据对可能的粒子位置进行打分时，必须精确的设计超声传感器的模型，根据这个模型来计算每次测量对应的后验概率密度，以此作为打分的基础。如果模型设计不够准确，会导致选择错误的粒子，最后导致不同粒子间的得分类似，难以区分，从而算法不收敛，表现出来就是定位和地图一直在抖动；或者收敛到错误的位置，导致最后的定位和地图的精度很差。

在打分过程中，我们设计了一种超声传感器和已经建立的地图之间的超声测量模型。假设在产生回波的点周围建立一个指数分布，随着离障碍物的距离，地图的占据值迅速衰减，以此作为预测值，和测量值相减得到误差，用这个误差来计算这个粒子的概率密度，从而得到该粒子的得分。超声传感器通过发射超声波，碰到物体后产生回波，为表述方便，不妨设超声波和物体接触的这个点为端点。在一般的模型中，是直接假设这个端点所对应的栅格是肯定有物体的，即有物体的概率为1，那么地图中的这个栅格应该填入1。而在超声传感器到端点的连线上，则是没有物体，那么这条连线上所对应的多个栅格都是肯定没有问题的，应该填入0。这种简单的模型虽然容易使用，但也存在多次的测量值可能相互矛盾的情况，如：由于超声传感器的精度所限，很可能存在上一个时刻的测量值中某个栅格是对应端点，下一个时刻这个栅格又对应连线上的点。为了解决这个问题，本算法设计了一个更为精细的超声传感器测量模型，具体步骤如下：

6.1)在产生回波的端点周围建立一个指数分布，用这个指数分布值来得到端点附近的栅格所对应的占据值，随着离端点的距离增大，占据值呈指数迅速衰减，与实际超声测量数据相符；

6.2)用实际超声测量数据和已经建立的地图进行对比，根据两者之间的匹配程度来计算这个粒子位置的得分；假设地图中障碍物的位置为x0，那么在其附近位置x的预测值val可由下式计算得出：

val＝a*exp(x-x0)，

其中a为调节系数；

6.3)将预测值与测量值相减得到误差，用这个误差可计算得到这个粒子的概率密度，从而获得该粒子的得分；遍历所有粒子，选择得分最高的位置，即为所确定的机器人当前时刻最终位置，从而实现对机器人位置的最终估计。

前文所述当前时刻最终位置是指经过步骤1-6所有过程，进行判断、计算后最终确定出的机器人当前时刻所在位置，也就是使用本发明方法对移动机器人进行定位后得到的位置信息。

步骤7，根据机器人的当前时刻最终位置更新环境地图。

在步骤6得到对机器人当前位置的最终估计后，我们需要以这个位置作为基准，结合当前超声传感器的测量数据，来更新部分地图中还未知的区域，同时也修正部分已知位置的地图值，实现更新地图的目的；这里对超声传感器数据的使用中也使用了步骤6中提到的超声测量模型，利用超声传感器读数，计算出某些栅格中新的被占据的概率，从而更新地图。

参照图3，实际场景为移动机器人在一个矩形的走廊中行走，期间根据周围环境构建地图，黑色表示地图中的物体，即墙壁，白色表示环境中的自由空间，即没有障碍物的区域，灰色表示环境中的未知区域。

以上描述仅是本发明的一个具体实例，不构成对本发明的任何限制，显然对于本领域的专业人员来说，在了解本发明内容和原理后，都可能在不背离本发明原理、结构的情况下，进行形式和细节上的各种修正和改变，但是这些基于本发明思想的修正和改变仍在本发明的权利要求保护范围之内。