[发明专利]一种用轮式里程计-IMU和单目相机实现定位的方法

权利要求书

1.一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，包括以下步骤：

(1)标定单目相机的内参矩阵、单目相机和车体之间的外参、单目相机和IMU之间的外参；

(2)对单目相机的图像进行去畸变后，对图像提取ORB特征点，找到对应的最优匹配特征点后，执行单目SLAM初始化操作，选择初始化成功时的当前帧的位姿作为世界坐标系的位姿；

(3)根据轮式里程计和IMU测到的方向角预测车体在某一时刻的位姿；

(4)根据位姿将地图点投影到图像上，进行特征跟踪和匹配；

(5)通过三角化得到地图点的三维坐标，同时插入关键帧，从而维护一个局部地图；

(6)利用相机的图像特征点构建重投影误差最小的优化问题，对局部地图中的地图点和所有位姿进行光束平差法(bundle adjustment，BA)优化，得到优化后的位姿，用优化后的位姿去更新车体位姿；

(7)采用回环检测和回环校正，得到全局一致的车体位姿。

2.根据权利要求1所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述步骤(1)中，车体坐标系(base frame)位于车体的正中心，世界坐标系(worldframe)固定在一个位置，相机坐标系(camera frame)位于相机中心，用表示车体坐标系B到世界坐标系W的旋转矩阵，表示车体坐标系B到世界坐标系W的平移向量，则T_WB表示从车体坐标系B到世界坐标系W之间的变换矩阵，用r表示车体在地面上的世界坐标(x,y)，Φ表示方向角，则有

其中，p_x、p_y表示取三维向量的x、y部分,表示将矩阵变换为李代数，即旋转向量，表示取旋转向量在z轴部分的分量；

路标在世界坐标系中的坐标l转换为相机坐标系下的坐标l_C的公式如下：

其中，为世界坐标系W到相机坐标系C的旋转矩阵，为世界坐标系W到相机坐标系C的平移向量，l为路标在世界坐标系W中的坐标，l_C为路标在相机坐标系C下的坐标；

通过外参标定，得到相机坐标系C到车体坐标系B之间的旋转矩阵和平移向量用于后续的优化过程。