[发明专利]室内动态环境下基于激光雷达的定位方法及计算机存储介质

权利要求书

1.一种室内动态环境下基于激光雷达的定位方法，其特征在于，所述方法包括以下步骤：

S1、给定机器人初始位姿，并对机器人初始位姿进行采样生成多个初始粒子；

S2、根据运动模型更新粒子状态；

S3、采用激光雷达进行数据采样，获得点云数据；

S4、根据点云数据更新粒子权重；

S5、根据粒子权重对粒子进行重采样；

S6、更新机器人位姿；

S7、判断粒子集是否收敛，若未收敛，则跳转至步骤S2，若收敛，则进入步骤S8；

S8、对步骤S6更新后的机器人位姿进行二次重采样；

S9、跳转至步骤S2。

2.根据权利要求1所述的方法，其特征在于，在步骤S1中，给定机器人初始位姿x_t，基于高斯分布对x_t进行采样生成N个初始粒子，记为：

其中，粒子权重初始化为

3.根据权利要求2所述的方法，其特征在于，在步骤S2中，根据运动模型更新粒子状态，记为

其中，u_t为运动控制量，v_t为噪声。

4.根据权利要求3所述的方法，其特征在于，步骤S3包括：

S31、将所述激光雷达的数据按扫描角度分块，每20度为一块区域，最后不足以构成一块区域的激光雷达数据与前一组数据合并为一组；

S32、提取每块区域的激光雷达数据中距离最大的5个数据作为点云数据。

5.根据权利要求4所述的方法，其特征在于，在步骤S4中，根据步骤S3获得的点云数据，依据各个粒子的位姿，将点云数据映射到地图并计算点云与地图的匹配值，

点云匹配评价函数为：

其中，σ＝0.1，d为点云在地图中与障碍点最近的距离，

记点云数据为m个，在地图中第j个点云离障碍点最近距离记为d_j，则第i个粒子的点云匹配值为

取点云的匹配值为粒子的权重，则

归一化权重，则

6.根据权利要求5所述的方法，其特征在于，在步骤S5中，根据粒子权重对粒子进行重采样，并重新设置权重为

7.根据权利要求6所述的方法，其特征在于，在步骤S6中，更新机器人位姿，记为：

t＝t+1。

8.根据权利要求7所述的方法，其特征在于，在步骤S8中，基于高斯分布对机器人位姿进行二次重采样以更新粒子集，即X_t～N(x_t,Σ₁)，其中，X_t为更新后的粒子集，x_t为步骤S6更新后的机器人位姿，Σ₁为采样协方差。

9.根据权利要求8所述的方法，其特征在于，在步骤S8中，根据更新后的粒子集，计算位姿x_t的点云匹配值，记为s1，若s10.8，则采样协方差为：

否则采样协方差为：

设置粒子权重为

10.一种计算机存储介质，其特征在于，包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现如权利要求1-9中任一项所述的方法。