[发明专利]一种基于多传感器融合的定位方法、装置和存储介质

权利要求书

1.一种基于多传感器融合的定位方法，其特征在于，包括：

获取图像数据、IMU观测数据以及激光点云数据；

联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程，以获取机器人的位姿，其中，所述先验约束因子为所述激光点云数据或所述IMU观测数据对应的误差约束因子，所述无结构化视觉约束因子为所述图像数据对应的误差约束因子，所述IMU预积分约束因子为所述IMU观测数据对应的误差约束因子，所述激光匹配约束因子为所述激光点云数据对应的误差约束因子；

利用回环检测对所述机器人的位姿进行修正，得到修正位姿；

对所述修正位姿进行全局位姿优化，以得到全局一致的位姿信息。

2.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，所述图像数据包括多个图像帧，所述图像帧包括利用左目相机采集到的左目图像帧以及利用右目相机采集到的右目图像帧，所述方法还包括：

对所述左目图像帧进行特征提取，生成左目特征点；

利用所述左目特征点得到同一时刻所述右目图像帧中的右目特征点；

对所述左目特征点与对应的所述右目特征点进行处理，以剔除所述左目图像帧中的外点与所述右目图像帧中的外点；

对前后两帧IMU观测数据进行特征跟踪，以剔除前一图像帧中的外点。

3.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，所述利用所述左目特征点得到同一时刻所述右目图像帧中的右目特征点的步骤，包括：

利用所述左目特征点进行左右目的立体匹配，得到所述右目特征点，将所述右目特征点作为光流跟踪的初始值；

进行光流跟踪得到左目跟踪特征点与右目跟踪特征点；

对所述左目图像帧中的左目特征点和所述右目图像帧中的右目特征点进行处理，以筛除所述左目图像帧中的外点与所述右目图像帧中的外点；

分别求取所述左目跟踪特征点与所述右目跟踪特征点在对应相机坐标系下的归一化坐标，得到左目坐标和右目坐标；

利用所述左目相机的外参和所述右目相机的外参构建本质矩阵，利用对极几何约束进行外点剔除，得到剩余特征点；

将所述左目图像帧与所述右目图像帧划分为第一预设数量的网格，将所述剩余特征点划分到对应的网格中，其中，每个所述网格中的特征点的数量小于第二预设数量。

4.根据权利要求3所述的基于多传感器融合的定位方法，其特征在于，所述利用对极几何约束进行外点剔除的步骤，包括：

判断每个所述特征点对应的所述左目坐标、所述本质矩阵以及所述右目坐标的乘积是否大于预设值；

若是，则所述特征点为所述外点。

5.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，所述对前后两帧IMU观测数据进行特征跟踪，以剔除前一图像帧中的外点的步骤，包括：

利用所述前后两帧IMU观测数据，得到前一图像帧相对当前图像帧的相对姿态；

利用所述前一图像帧中的像素坐标和所述相对姿态，预测所述当前图像帧的像素坐标；

将所述当前图像帧的像素坐标作为前后帧光流跟踪的初值，进行光流跟踪，得到当前图像帧中的左目跟踪特征点和右目跟踪特征点；

对所述左目跟踪特征点和所述右目跟踪特征点进行立体匹配，以筛除所述前一图像帧中的外点；

分别对所述前后两帧图像中的左目图像帧和右目图像帧进行处理，以筛除所述前一图像帧中的外点。

6.根据权利要求5所述的基于多传感器融合的定位方法，其特征在于，所述方法还包括：

在所述当前图像帧中的特征点的数量小于第三预设数量时，对所述当前图像帧进行特征提取，得到新的左目特征点；

对所述新的左目特征点进行立体匹配，得到新的右目特征点；

对所述新的左目特征点与所述新的右目特征点进行立体匹配，以筛除外点，得到新增的左目特征点与新增的右目特征点；

将所述新增的左目特征点添加到所述左目图像帧对应的网格中，将所述新增的右目特征点添加到所述右目图像帧对应的网格中。