[发明专利]一种基于图优化和粒子滤波的融合定位方法

权利要求书

1.一种基于图优化和粒子滤波的融合定位方法，其特征在于，包括如下步骤：

S1：基于图优化的方法生成子地图；

S2：全局采样生成粒子集，在地图中的所有自由区域进行随机采样生成表示机器人位姿的粒子集；

S3：粒子筛选；

S4：粒子聚类；

S5：粒子重采样；

S6：根据运动模型更新粒子集；

S7：更新粒子权重；

S8：根据粒子权重对粒子进行重采样，并重新设置权重；

S9：跳转至S6直至粒子收敛，对收敛的粒子求加权平均即为最终定位结果；

所述步骤S1中基于图优化的方法生成子地图，包括，

1)机器人原地旋转一周，根据机器人运动模型记录运动过程中机器人位姿x_1:t和该位姿下的观测值z_1:t，激光点云数据即为观测值；

2)构建图，以机器人的位姿作为顶点，即需要优化的变量，以机器人运动模型和观测模型的约束作为边，则有目标函数如下：

式中x₀为初始位姿，Ω₀为初始位姿协方差，g(u_t,x_t-1)为机器人运动模型，u_t为控制量，R_t为运动噪声协方差，h(m_t,x_t)为观测模型，m_t为地图特征，Q_t为观测噪声协方差；

3)优化图，以顶点为优化变量对构建的图进行优化，从而得到优化后的机器人位姿；

4)根据优化后的机器人位姿和激光点云数据生成子地图。

2.根据权利要求1所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤3)中的优化图进一步包括：调整各个时刻机器人位姿x_1:t，使其尽量满足边的约束，也即使目标函数J最小化。

3.根据权利要求2所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S3中的粒子筛选，进一步包括，根据每一个粒子的位姿，将S1中的子地图映射到全局地图中去。

4.根据权利要求3所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S4中的粒子聚类，进一步包括，基于层次聚类算法，对S3中保留的粒子进行聚类，聚类结果记为X＝{X₁,X₂,...,X_n}，其中n为类别数量。

5.根据权利要求4所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S5中的粒子重采样，进一步包括，将S4中聚类后的粒子，分别求平均值得到基于高斯分布在中位姿附近采样生成新的粒子集，记为：

其中，粒子权重初始化为

6.根据权利要求5所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S6中的更新粒子集，进一步包括，根据运动模型更新粒子状态，即

式中，u_t为控制量，v_t为噪声。

7.根据权利要求6所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S7中的更新粒子权重，进一步包括，根据每一个粒子的位姿，将当前激光点云数据映射至全局地图，依据匹配程度更新粒子权重，即匹配程度高的粒子获得大权重，匹配程度低的粒子获得小权重；归一化权重有：

8.根据权利要求7所述的基于图优化和粒子滤波的融合定位方法，其特征在于，所述步骤S8中重新设置权重为

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