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

权利要求书

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

根据组合惯导采集的行驶设备的状态数据，确定行驶设备的预测状态空间向量；其中，所述预测状态空间向量包括基础状态空间向量和基础状态空间向量的误差状态空间向量；

获取至少两个传感器中的各个传感器的数据，并根据所述至少两个传感器中的各个传感器的数据，分别确定所述至少两个传感器中的各个传感器对应的测量矩阵；

根据所述预测状态空间向量和所述至少两个传感器中的各个传感器对应的测量矩阵，确定所述至少两个传感器中的各个传感器对应的观测状态空间向量；

对所述至少两个传感器中的各个传感器对应的观测状态空间向量进行插值处理，确定当前状态空间向量。

2.根据权利要求1所述的方法，其特征在于，所述对所述至少两个传感器中的各个传感器对应的观测状态空间向量进行插值处理，确定当前状态空间向量，包括：

根据所述至少两个传感器中的各个传感器的数据的时间戳和当前时间，确定补偿因子；

根据所述补偿因子，对所述至少两个传感器中的各个传感器对应的观测状态空间向量进行数据计算，得到所述各个传感器对应的观测状态空间向量的增加向量；

从所述至少两个传感器中的各个传感器对应的观测状态空间向量中，找到时间戳与当前时间最接近的观测状态空间向量，并对所述时间戳与当前时间最接近的观测状态空间向量加上所述增加向量，得到当前时间的当前状态空间向量。

3.根据权利要求2所述的方法，其特征在于，所述根据所述补偿因子，对所述至少两个传感器中的各个传感器对应的观测状态空间向量进行数据计算，得到所述各个传感器对应的观测状态空间向量的增加向量，包括：

从所述至少两个传感器中的各个传感器对应的观测状态空间向量中，提取出相邻时间戳的至少两个观测状态空间向量，并分别对所述相邻时间戳的至少两个观测状态空间向量进行处理，得到所述相邻时间戳的至少两个观测状态空间向量中各个观测状态空间向量对应的旋转矩阵；

根据所述补偿因子，对所述相邻时间戳的至少两个观测状态空间向量中各个观测状态空间向量对应的旋转矩阵进行数据计算，得到相邻时间戳的观测状态空间向量的旋转矩阵增量；

根据所述补偿因子，对所述相邻时间戳的至少两个观测状态空间向量进行数据计算，得到相邻时间戳的观测状态空间向量的偏移增量。

4.根据权利要求1所述的方法，其特征在于，所述根据所述至少两个传感器中的各个传感器的数据，分别确定所述至少两个传感器中的各个传感器对应的测量矩阵，包括：

若所述至少两个传感器的其中一个传感器为全球卫星导航系统GNSS，则根据所述GNSS的数据和所述基础状态空间向量中的位姿信息，得到所述GNSS对应的测量矩阵；

若所述至少两个传感器的其中一个传感器为雷达传感器，则根据所述雷达传感器的位置信息在组合惯导坐标系下的位置信息和所述基础状态空间向量中的位姿信息，得到所述雷达传感器对应的测量矩阵；

若所述至少两个传感器的其中一个传感器为轮速传感器，则根据所述轮速传感器所探测的车速、轮速数据、轮速传感器的位姿信息在组合惯导坐标系下的位姿信息和所述基础状态空间向量中的位姿信息、速度信息和角速度误差，得到所述轮速传感器对应的测量矩阵。

5.根据权利要求1所述的方法，其特征在于，所述根据所述预测状态空间向量和所述至少两个传感器中的各个传感器对应的测量矩阵，确定所述至少两个传感器中的各个传感器对应的观测状态空间向量，包括：

根据初始滤波器数据，确定所述行驶设备的所述预测状态空间向量的预测误差协方差矩阵；

根据所述预测误差协方差矩阵、观测方程和所述至少两个传感器中的各个传感器对应的测量矩阵，对所述预测状态空间向量中的误差状态空间向量进行矫正，得到所述至少两个传感器中的各个传感器对应的矫正之后的误差状态空间向量；

根据所述矫正之后的误差状态空间向量和所述预测状态空间向量中的基础状态空间向量，得到所述至少两个传感器中的各个传感器对应的观测状态空间向量。