[发明专利]一种基于多传感器融合的定位方法及装置

权利要求书

1.一种基于多传感器融合的定位方法，其特征在于，应用于一种搭载有多种传感器的可移动物体上，所述方法，包括：

实时获得可移动物体上搭载的各种传感器采集的传感器数据；

对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；

将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；

获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；

根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。

2.根据权利要求1所述的方法，其特征在于，所述各种传感器包括惯性测量单元IMU、轮速计和激光雷达；其中，所述IMU包括加速度计和陀螺仪。

3.根据权利要求2所述的方法，其特征在于，所述状态信息为状态变量，所述状态变量包括平移向量、速度向量、旋转矩阵、IMU中加速度计偏置和IMU中陀螺仪偏置；

所述获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息，包括：

在用于联合优化的传感器数据未经过初始化时，实时确定预先设置的滑动窗口中的数据是否满足初始化条件；所述初始化条件包括所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励；

在所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励时，对状态变量进行初始化，形成初始化状态变量；

在对状态变量进行初始化完成后，且当前时刻之前的上一时刻进行联合优化求解结束后，获得当前时刻之前的各时刻的联合优化后的当前状态变量。

4.根据权利要求3所述的方法，其特征在于，所述实时获得可移动物体上搭载的各种传感器采集的传感器数据，包括：

实时获得IMU测量的IMU数据、轮速计测量的可移动物体轮速数据和激光雷达测量的点云数据。

5.根据权利要求4所述的方法，其特征在于，对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据，包括：

根据IMU、轮速计和激光雷达之间预先标定的外参，将可移动物体轮速数据和点云数据对齐到IMU坐标系下，并根据IMU、轮速计和激光雷达各自的数据采集帧率，将可移动物体轮速数据和点云数据的采集时的每帧与IMU的时间戳对齐，形成IMU坐标系下的，时间戳对齐的IMU数据、可移动物体轮速数据和点云数据。

6.根据权利要求4所述的方法，其特征在于，将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据，包括：

将进行时间空间同步后的IMU数据中处于连续两帧激光雷达时间戳之间的IMU数据使用预先设置的积分算法进行传播，并通过预先设置的预积分方法进行处理，得到位移预积分量、速度预积分量和旋转增量预积分量；

根据当前时刻的上一时刻在预先设置的滑动窗口中联合优化后的状态变量，以及进行时间空间同步后的IMU数据进行下一时刻的状态变量预测，得到下一时刻的状态变量预测数据；

将进行时间空间同步后的点云数据，采用下一时刻的状态变量预测数据进行去畸变处理；

对去畸变处理后的点云数据进行线面特征提取，获得线特征数据和面特征数据；

将所述线特征数据和面特征数据与预先生成的特征地图上的线特征和面特征分别进行配准，得到线特征约束关系数据和面特征约束关系数据；

将进行时间空间同步后的可移动物体轮速数据，通过预先设置的车辆动力学模型，确定在IMU坐标系下前进方向线速度、水平方向线速度和偏航角速度。