[发明专利]一种基于RGBD传感器和IMU传感器的定位方法

权利要求书

1.一种基于RGBD传感器和IMU传感器的定位方法，应用于搭载RGBD传感器和IMU传感器的移动机器人，实现对移动机器人的定位，其特征在于，所述的定位方法包括以下步骤：

S1、传感器数据采集：移动机器人搭载RGBD传感器和IMU传感器在三维空间中自由移动，采集RGBD传感器数据和IMU传感器数据；

S2、IMU预积分：采用视觉SLAM算法根据RGBD传感器不同帧的图像数据质量选定关键帧，基于选定的关键帧，采用IMU预积分方程计算关键帧之间的IMU传感器数据的预积分值；其中，计算IMU预积分，通过IMU预积分由当前时刻的状态推算出下一时刻的状态，计算公式如下：

其中，表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵，v_i+1、p_i+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息，Δt_i,i+1表示IMU传感器i时刻到i+1时刻的间隔，/分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置，g_w表示在世界坐标系下的重力加速度，ΔR_i,i+1、Δv_i,i+1、Δp_i,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分，/分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置/变化方程的一阶近似的雅克比矩阵，/分别为Δv、Δp随IMU传感器加速度计偏置/变化方程的一阶近似的雅克比矩阵；

S3、移动机器人初始化：采用视觉SLAM算法对RGBD传感器初始化得到RGBD传感器的位姿信息，然后根据RGBD传感器的位姿信息和对应关键帧的IMU预积分对移动机器人初始化获取移动机器人的初始化状态信息，其中，所述的移动机器人的状态信息包括移动机器人的位姿信息、速度和IMU传感器的偏置；

S4、移动机器人状态的跟踪优化：采用基于重投影误差的优化算法对估计的移动机器人的状态信息进行优化跟踪，然后采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化，过程如下：

S41、采用视觉SLAM算法估计RGBD传感器的位姿信息ξ；

S42、建立关键帧之间的约束关系，根据IMU传感器误差项和移动机器人的位姿信息，基于重投影误差建立最小二乘关系：

E_proj(k，j)为位姿信息和特征点的重投影误差，表述式如下：

E_IMU(i，j)为IMU传感器的误差项，表述式如下：

e_b＝b_j-b_i

其中k表示图像的特征点序号，i表示关键帧序号，j表示当前关键帧序号，s表示深度图的尺度因子，u_k表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标，K表示RGBD传感器内参矩阵，ξ^表示RGBD传感器位姿信息ξ的反对称矩阵，表示第k个特征点X_k在RGBD传感器坐标系C下的坐标，∑_k是特征点X_k和对应像素点的尺度信息矩阵，R^CB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵，/表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵，/表示第k个特征点X_k在世界坐标系W下的坐标，/表示第j关键帧时刻IMU传感器在世界坐标系下的坐标，/表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，ΔR_i，j、Δv_i，j、Δp_i，j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分，/分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，/分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，/表示IMU传感器在第j关键帧时刻的速度，/表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵，分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置，∑_R、∑_I分别为IMU传感器偏置随即游走和预积分的信息矩阵，b_i、b_j分别表示第i和第j关键帧的IMU传感器偏置，Δt_i，j表示第i关键帧到第j关键帧的时间差；

S43、对于当前关键帧j的移动机器人的状态跟踪，当前关键帧j仅与相邻关键帧j-1建立约束关系：

其中φ表示需要优化对象的集合，φ^*表示优化后的φ对象集合，k表示特征点的序号，j表示当前关键帧序号，表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，/表示IMU传感器关键帧j时刻在世界坐标系下的平移，/表示IMU传感器在第j关键帧时刻的速度，/分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；

求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪；

S44、采用基于滑动窗口的优化方式对移动机器人的状态进行非线性优化，对于当前关键帧之前的M帧关键帧，通过对移动机器人的位姿信息、特征点和IMU传感器误差项建立约束关系进行优化：

其中φ表示需要优化对象的集合，φ^*表示优化后的φ对象集合，k表示特征点的序号，i表示关键帧序号，j表示当前关键帧序号，表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，/表示IMU传感器关键帧j时刻在世界坐标系下的平移，/表示IMU传感器在第j关键帧时刻的速度，/分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置，对于当前关键帧M+1关键帧前的关键帧，仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化：

其中ψ表示需要优化对象的集合，ψ^*表示优化后的ψ对象集合；

S5、回环检测：根据RGBD传感器数据，采用基于词袋模型的回环优化算法对RGBD传感器的位姿信息和特征点进行回环优化，然后采用基于滑动窗口的优化算法对IMU传感器误差项进行优化。