[发明专利]基于多超声传感器的移动机器人同步定位与地图构建方法

权利要求书

1.一种基于多超声传感器的移动机器人同步定位与地图构建方法，其特征在于包括如下步骤：

(1)设置超声传感器：

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

⑵获取超声测量数据：

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

⑶组合所获取到的超声测量数据：

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

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

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

Zb2＝Ta^{-1}*Tb*Zb；

将Zb2与Za叠加作为新的第一时刻测量值ZaO；

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

Zc2＝Ta^{-1}*Tc*Zc；

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

⑷判断是否达到设定的组合次数：

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

⑸预测机器人的当前位置：

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

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

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

其中：

x2＝xl+dx；

y2＝yl+dy；

theta2＝thetal+dtheta；

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

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

⑹确定机器人的当前时刻最终位置：

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

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

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