[发明专利]一种移动机器人定位与建图的方法

权利要求书

1.一种移动机器人定位与建图的方法，其特征在于，所述移动机器人包括移动机器人主体、单目相机、IMU传感器和轮速计传感器，所述单目相机镜头朝向所述移动机器人主体的正前方，其中IMU传感器包括IMU加速度计和IMU陀螺仪；

处理单目相机获取的每帧图像，获得单目相机的处理值，对每两相邻帧图像间的IMU传感器测量值进行预积分，获得IMU传感器的处理值，对每两相邻帧图像间的轮速计传感器测量值进行预积分，获得轮速计传感器的处理值；

将所述单目相机、IMU传感器和轮速计传感器的处理值加入滑动窗口；

获取滑动窗口的处理值，对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子进行初始化；

初始化成功后，构建因子图模型获得待估计状态值，对滑动窗口的处理值和待估计状态值进行非线性优化最小化残差项，得到所述移动机器人的位姿和地图。

2.根据权利要求1所述的方法，其特征在于，成功完成对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子的初始化的方法包括：对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子进行初始化，若初始化失败，对所述滑动窗口进行更新，之后再次获取所述滑动窗口的处理值，直至成功初始化。

3.根据权利要求1所述的方法，其特征在于，得到所述移动机器人的位姿和地图后，对所述滑动窗口进行更新，对更新后的滑动窗口的处理值和待估计状态值再次进行非线性优化，得到所述更新后的移动机器人的位姿和地图。

4.根据权利要求2或3所述的方法，其特征在于，所述更新的方法包括：

读取滑动窗口内的最新两帧，判断倒数第二帧是否为关键帧，若是关键帧，则删除滑动窗口内的最老关键帧的单目相机、IMU传感器和轮速计传感器的处理值，并保留当前帧的单目相机、IMU传感器和轮速计传感器的处理值；

若不是关键帧，则删除当前帧的单目相机的处理值，保留IMU传感器和轮速计传感器的处理值。

5.根据权利要求1所述的方法，其特征在于，所述轮速计传感器测量值进行预积分方法包括：构建轮速计传感器的运动测量模型、测量预积分模型、误差传播模型以及协方差传播模型。

6.根据权利要求1所述的方法，其特征在于，初始化IMU陀螺仪偏差方法包括：

获取滑动窗口内的相邻的两个关键帧，读取关键帧中的单目相机坐标系、IMU传感器坐标系、轮速计传感器坐标系在每个关键帧时刻的旋转量，将所述单目相机坐标系、IMU传感器坐标系的旋转量和所述单目相机坐标系、轮速计传感器坐标的旋转量分别构建最小二乘方程，将求出的值进行求和与平均，初始化所述IMU陀螺仪偏差。

7.根据权利要求1所述的方法，其特征在于，初始化线速度和尺度因子的方法包括：

获取滑动窗口内的相邻的两个关键帧，读取关键帧中的IMU传感器和轮速计传感器的预积分平移量、速度变化量；以及单目相机的平移量、速度变化量和重力分量；

将IMU传感器和轮速计传感器的预积分平移量、速度变化量，以及单目相机的平移量、速度变化量和重力分量构建最小二乘方程，初始化所述线速度和尺度因子。

8.根据权利要求1所述的方法，其特征在于，所述构建因子图模型获得待估计状态值的方法包括：

所述残差项包括：IMU传感器残差、轮速计传感器残差、单目相机残差、平面残差；所述每个关键帧滑动窗口的待估计状态值包括：在世界坐标系下的平移量、速度、旋转量、IMU加速度计偏差、IMU陀螺仪偏差、特征点逆深度；

所述IMU传感器残差约束所述世界坐标系下的平移量、旋转量、速度和IMU加速度计偏差、IMU陀螺仪偏差；所述轮速计传感器约束所述世界坐标系下的平移量、旋转量和IMU传感器到轮速计的外参；所述单目相机残差约束所述世界坐标系下的特征点逆深度、平移量、旋转量、速度和IMU加速度计偏差、IMU陀螺仪偏差、IMU到单目相机的外参；所述平面残差约束所述世界坐标系下的平移量和旋转量；完成所述构建因子图模型获得待估计状态值。