[发明专利]一种行驶设备的定位方法、装置、设备及存储介质

权利要求书

1.一种行驶设备的定位方法，其特征在于，所述方法包括:

根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定所述行驶设备的预测状态空间向量和所述预测状态空间向量对应的预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量；

根据所述预测状态空间向量的一般状态空间向量中的位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的匹配位姿信息和观测噪声矩阵；

根据所述匹配位姿信息、所述预测状态空间向量对应的预测误差协方差矩阵和所述观测噪声矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。

2.根据权利要求1所述的方法，其特征在于，所述根据所述预测状态空间向量的一般状态空间向量中的位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，包括：

将所述预测状态空间向量的一般状态空间向量中的位姿信息从惯导坐标系转换到雷达坐标系，并将转换到所述雷达坐标系下的预测状态空间向量的一般状态空间向量中的位姿信息作为初始位姿矩阵；

基于正态分布变换算法和所述初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵。

3.根据权利要求2所述的方法，其特征在于，所述根据迭代匹配结果，确定所述行驶设备的匹配位姿信息和观测噪声矩阵，包括：

将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的匹配位姿信息；

根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；

根据所述匹配置信度和旋转矩阵，确定所述观测噪声矩阵。

4.根据权利要求1所述的方法，其特征在于，所述根据所述匹配位姿信息、所述预测状态空间向量对应的预测误差协方差矩阵和所述观测噪声矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，包括：

根据所述匹配位姿信息、所述预测状态空间向量对应的预测误差协方差矩阵和所述观测噪声矩阵，确定所述当前状态空间向量的误差状态空间向量；

根据所述当前状态空间向量的误差状态空间向量和所述行驶设备的上一时刻状态空间向量的一般状态空间向量，确定并输出所述行驶设备的当前状态空间向量的一般状态空间向量；

根据所述预测状态空间向量对应的预测误差协方差矩阵和所述观测噪声矩阵，确定所述当前误差协方差矩阵。

5.根据权利要求4所述的方法，其特征在于，所述重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵，包括：

将所述当前状态空间向量的误差状态空间向量重置为零；

根据单位矩阵和所述预测状态空间向量的误差状态空间向量中的姿态角度误差，确定重置矩阵；根据所述重置矩阵，重置所述当前误差协方差矩阵。

6.根据权利要求1-5任一项所述的方法，其特征在于，所述方法还包括：

根据轮速计采集的轮速信息，确定所述行驶设备的测量速度信息；

若所述测量速度信息为零，则对所述行驶设备的当前状态空间向量中的速度信息进行零速矫正，并重置所述当前误差协方差矩阵。

7.根据权利要求6所述的方法，其特征在于，所述方法还包括：

根据所述先验地图数据和所述激光雷达采集的点云数据，设置所述行驶设备的初始位姿信息；

根据所述初始位姿信息和惯导参数进行初始化，得到所述初始滤波器参数。