[发明专利]一种基于深度相机和惯性融合的移动机器人定位方法

权利要求书

1.一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述方法具体包括以下步骤：

步骤1、读取当前来自深度相机的单目RGB图像，深度图像和IMU测量序列，再对单目RGB图像，深度图像和IMU测量序列进行时间同步；

所述步骤1的具体过程为：

以单目RGB图像的时间为基准，获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合，在匹配深度图像时，若深度图像与当前帧单目RGB图像的时间相差在t₀以内，则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目RGB图像的时间相差不小于t₀，则去除当前的深度图像，并以单目惯性模式进行处理；

步骤2、对前一帧单目RGB图像和当前帧单目RGB图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；

步骤3、建立深度相机深度测量误差模型；

所述深度相机深度测量误差模型为：

σ_z(z)＝a₁+a₂z+a₃z²

其中，z为当前特征点在深度图像上的深度测量值，σ_z(z)为当前特征点在深度图像上的深度测量值为z时的标准差，a₁,a₂,a₃为待标定的系数；

所述系数a₁,a₂,a₃的标定方式为：

将深度相机置于滑轨上，当深度相机与白墙的距离为L₁时采集多张深度图像，再分别从采集的每张深度图像中截取出感兴趣区域，所述感兴趣区域在每张深度图像中的位置均相同；

对于感兴趣区域内某个位置的像素点，计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差，再对感兴趣区域内全部位置上的像素点所对应的方差求平均，将得到的平均值作为标准差σ；再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ；

深度相机在滑轨上滑动，当深度相机与白墙的距离每增加ΔL时均采集到对应距离下的多张深度图像，直至深度相机与白墙的距离达到L₂时停止，同理，分别计算出每个距离下的标准差和均值；

利用获得的各个距离下的标准差和均值对σ＝a₁+a₂μ+a₃μ²进行拟合，得到系数a₁,a₂,a₃的值；

步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目RGB图像c_k具有最大视差的关键帧记为c₀，计算关键帧c₀与当前帧c_k之间的平移和旋转初值其中，为关键帧c₀与当前帧之间的旋转初值，为关键帧c₀与当前帧之间的平移初值；

根据得到的平移和旋转初值得到关键帧c₀与当前帧c_k间的位姿估计结果

所述根据得到的平移和旋转初值得到关键帧c₀与当前帧c_k间的位姿估计结果其具体过程为：

采用LM算法对平移和旋转初值进行迭代更新，分别获得各次迭代更新后的平移和旋转值；

遍历当前帧单目RGB图像上的特征点，从当前帧单目RGB图像对应的深度图像获得当前帧单目RGB图像上每个特征点的深度测量值；

并建立如下的误差方程：

其中，代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值S_3D表示由深度测量值在3m以内的特征点对组成的集合，S_2D表示由全部特征点对组成的集合，r_re表示重投影误差方程，r_ep表示对极约束误差方程，表示S_3D中第l₁对特征点在关键帧c₀中的深度测量值，表示S_3D中第l₁对特征点在当前帧中的深度测量值，表示S_3D中第l₁对特征点对应的归一化特征点对，表示S_2D中第l₂对特征点对应的归一化特征点对，||·||代表范数，Ω_re代表相机测量的重投影误差的协方差矩阵，Ω_ep代表相机测量的对极约束误差的协方差矩阵；

步骤5、以关键帧c₀作为参考帧，基于关键帧c₀与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3D路标点l₁,...l_m以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果

所述步骤5的具体过程为：

步骤51、根据关键帧c₀与当前帧间的位姿估计结果以及关键帧c₀与当前帧间的特征点对的坐标值，计算出关键帧c₀与当前帧间具有共视关系的3D路标点在关键帧c₀坐标系下的坐标值；

步骤52、根据步骤51中计算出的坐标值以及关键帧c₀与当前帧间具有共视关系的3D路标点在滑窗内其它关键帧的相机观测值，得到滑窗内其它关键帧相对于参考帧的位姿估计初值；

步骤53、基于步骤52中的位姿估计初值，利用PnP优化算法得到滑窗内所有3D路标点l₁,...l_m和其它各关键帧相对于参考帧的视觉位姿估计结果

其中，滑窗的长度为10，即滑窗内有包括当前帧在内的10个关键帧，代表滑窗内的各关键帧相对于参考帧的视觉位姿估计结果；

步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换；

所述步骤6的具体过程为：

其中，和为第i个IMU时刻IMU测量得到的IMU坐标系下加速度和角速度，b_a和b_g为第i个IMU时刻的加速度计和陀螺仪的偏置，b_k表示第k帧IMU坐标系，和分别为第i个IMU时刻相对于b_k的位移预积分，速度预积分和旋转预积分，为当前时刻相对于b_k的旋转矩阵，代表旋转运算符，δt为当前时刻与上一时刻的时间间隔，是从IMU陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值，和分别为第i+1个IMU时刻相对于b_k的位移预积分，速度预积分和旋转预积分；

在理想情况下，视觉运动估计所得的旋转等于IMU预积分得到的旋转值，则有目标函数：

其中，为由视觉运动估计得到的b_k+1相对于第c₀帧IMU坐标系的旋转，为由视觉运动估计得到的b_k相对于第c₀帧IMU坐标系的旋转，b_k+1表示第k+1帧IMU坐标系，为理想情况下IMU旋转预积分得到的b_k+1相对于b_k的旋转，B代表滑窗中的所有帧的序号，||·||代表范数；

已知实际的IMU旋转预积分的值理想的IMU旋转预积分由下式求得：

其中，为旋转预积分相对于陀螺仪偏置的雅克比矩阵，δb_g为陀螺仪偏置的变化量；

经过化简和Cholesky分解得：

其中，上角标T代表矩阵的转置，上角标-1代表矩阵的逆；

求解出陀螺仪的偏置的变化值δb_g后，利用新的偏置b_g＝b_g+δb_g对IMU预积分值进行重新计算；

步骤7、将步骤5和步骤6的结果对齐，求得参考帧c₀到世界坐标系的旋转矩阵以及各帧的速度值

再利用旋转矩阵得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值得到各帧相对于世界坐标系的速度；

步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；

所述步骤8的具体过程为：

对于滑窗内的任意一个3D路标点，遍历该3D路标点在滑窗中每个关键帧上的测量值，测量值包括该3D路标点在相机归一化平面上的2D坐标和在深度图像上的深度测量值；

若该3D路标点在关键帧上存在深度测量值，且深度测量值小于3m，则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差将该3D路标点的2D像素坐标测量噪声方差记为则该3D路标点的三维测量协方差为：

其中，I₂为2阶单位矩阵，为该3D路标点在相机中2D像素坐标(u_i,v_i)和深度测量值z_i的雅克比；

其中，f_x,f_y表示相机x,y方向的焦距，c_x,c_y表示相机x,y方向的光心；

通过滑窗内帧间的位姿变换将该3D路标点转换到c₀帧相机坐标系中，则将协方差转换到c₀帧相机坐标系下为：

其中，为c₀帧相机坐标系下的协方差；

利用得到新的深度测量噪声方差将该3D路标点的深度测量值小于3m且重投影后2D观测误差小于设定阈值的N个观测投影到相机坐标系下，每个观测的投影记为d_i′，i＝1,2,…,N，则该3D路标点在c₀帧中的联合深度估计值为

若该3D路标点在滑窗内的跟踪次数大于等于4，则表明该3D路标点为稳健特征点，将该3D路标点归为A类，A类的3D路标点无需参与下一轮的联合深度估计；

若该3D路标点在滑窗内的跟踪次数小于4，则表明该3D路标点仍属新特征点，将该3D路标点归为B类，B类的3D路标点在下一循环仍需进行联合深度估计；

对于没有实现联合深度估计的3D路标点，根据共视帧的2D观测值采用多视图三角化的方法实现联合深度估计，对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3D路标点，将其归为A类，对于三角化误差小于等于设定阈值且跟踪次数小于4的3D路标点，则将其归为B类，对于三角化误差大于设定阈值的3D路标点，则直接将其归为C类，C类特征点为离群点不再参与后续优化，在本轮循环结束后被剔除；

步骤9、根据筛选后的路标点建立点特征投影误差模型；

步骤10、建立平面约束误差模型；

步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立IMU预积分约束模型；

步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。