[发明专利]一种蛇形机器人基于激光测距仪的地图创建方法

权利要求书

1.一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于，包括以下步骤：

步骤(1)：通过激光测距仪进行环境感知，获取环境信息；

步骤(2)：将当前激光测距仪的测量值与当前地图进行匹配；

步骤(3)：对蛇形机器人粒子集的粒子位姿及权值进行更新；所述的对蛇形机器人粒子集的粒子位姿及权值进行更新，采用的是Monte-Carlo算法；每次蛇形机器人感知完环境后这些粒子会重新采样；算法结束时，粒子会收敛于蛇形机器人的实际位置；在每个时刻，由初始状态和到当前时刻所有的测量值Z^k＝{z_i,i＝1...k}估计当前蛇形机器人的状态；所述Monte-Carlo算法包含3个阶段：

预测阶段：预测阶段用运动模型以概率密度函数的形式来预测当前蛇形机器人的位姿；假设当前的状态x_k仅依赖于之前的状态x_k-1和已知的控制输入u_k-1，该运动模型被认定为条件密度p(x_k|x_k-1,u_k-1)，对于一阶马尔可夫过程，先验概率密度就可以通过积分得到

p(x_k|Z^k-1)＝∫p(x_k|x_k-1,u_k-1)p(x_k-1|Z^k-1)dx_k-1

更新阶段：预测阶段用测量模型整合传感器的信息来获得后验概率密度p(x_k|Z^k)；假定对于x_k测量值z_k与之前的测量值Z^k-1是条件独立的；测量模型以似然函数p(z_k|x_k)的形式给出，这种形式表示在观测值z_k的情况下，蛇形机器人在x_k的可能性；通过贝叶斯公式更新先验值，得到后验概率密度

$p\left(x_k\right|Z^k)=\frac{p\left(z_k\right|x_k)p\left(x_k\right|Z^{k-1})}{p\left(z_k\right|Z^{k-1})}$

重采样阶段：重采样的目的是剔除权值较小的粒子，复制权值较大的粒子,通过对后验概率密度再采样N_s次，得到新的粒子；步骤(4)：进行地图更新；所述的地图更新方法采用的是改进的Bresenham算法，只采用整数运算。

2.根据权利要求1所述的一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于：所述激光测距仪为测距范围20mm到4000mm的URG04-LX激光测距仪，该激光测距仪的最大测量距离小于4m；采用阈值滤波方法对扫描的激光数据进行预处理滤除噪声点；并采用一个恒定的纵向和转速值对每次激光扫描值进行纠正。

3.根据权利要求1所述的一种蛇形机器人基于激光测距仪的地图创建方法，其特征在于：所述激光测距仪每一次扫描完后，会将观测值与当前地图中的特征点进行匹配；当与环境中的特征点较远时，采用最近邻数据关联准则，选取距离观测值最近的环境路标作为其匹配点；环境特征点间的距离相距越远，误匹配率越低，当误匹配率小于最大匹配误差阈值时，认为匹配成功。