[发明专利]一种基于闭环直接稀疏视觉里程计的移动位姿估计方法

权利要求书

1.一种基于闭环直接稀疏视觉里程计的移动位姿估计方法，其特征在于，包括以下步骤：

步骤S1、采集机器人周边图像序列；

步骤S2、基于前两帧图像进行初始化，构建后续图像序列的参考帧：令第二帧为I_v，第一帧为I_u，I_v基于I_u中选取的梯度最大的关键点，构建帧间光度误差函数：

E_u，v，w＝ω_p(I_v[p`]-a_vuI_u[p]-b_vu)

其中ω_p为补偿加权参数，p为I_u中梯度的点，p`表示p在当前帧I_v中的反投影坐标，a_vu为曝光时间校正参数、b_vu为图像亮度基底校正参数；

通过高斯牛顿法迭代最小化光度误差；若误差经优化未收敛，则判定初始化失败，需要重新设置第一帧，重复上述过程；否则初始化成功，将第一帧设置为关键帧，将第一帧中提取的梯度点激活，将其加入滑动窗口的优化中；第二帧在初始化后，进入滑动窗口优化；

步骤S3、获取帧间相对位姿变化并判断是否为关键帧；

步骤S4、新关键帧加入后端滑动窗口，同窗口中所有关键帧位姿与其中提取出的关键点联合构建总的光度误差函数，优化前端估计中的相对位姿初值；

步骤S5、旧关键帧边缘化，同时基于全部关键帧的描述子构建全局位姿图进行闭环检测，估计机器人的位姿。

2.根据权利要求1所述的一种基于闭环直接稀疏视觉里程计的移动位姿估计方法，其特征在于：步骤S2进一步的，在里程计前端首先要对相机传感器的输入进行初始化；所述初始化需要两帧图像，第一帧是作为基准帧，视第一帧所在的相机坐标系为世界坐标系，对图像构建高斯金字塔，统计图像金字塔第0图层中所有像素点的梯度直方图，并以其中位数作为梯度阈值，逐层对图像提取不同的梯度点，并对每一个梯度点构建最近邻索引，在同一层中找到s个与其距离最接近的点构建邻域关系，通过在下一层的最近邻索引中，找到与当前点距离最小的点，设置为关联点，关联当前层与下一层，便于光流金字塔和逆深度值的传播。

3.根据权利要求2所述的一种基于闭环直接稀疏视觉里程计的移动位姿估计方法，其特征在于：所述梯度点激活分为激活点和未成熟点，所述未成熟点是在图像中提取的或者深度未收敛的点；所述激活点通过深度滤波器在帧间不断地滤波，使得点的深度不断收敛，最终得到的点会被激活，加入滑动窗口中。

4.根据权利要求3所述的一种基于闭环直接稀疏视觉里程计的移动位姿估计方法，其特征在于：步骤S3进一步为，针对输入的每一帧新图像基于最近关键帧进行位姿解算，将最近关键帧的梯度点投影到当前帧中构建光度误差函数：

E_u，v，w＝ω_p(I_v[p`]-a_vuI_u[p]-b_vu)；

其中ω_p为补偿加权参数，p为I_u中梯度的点，p`表示p在当前帧I_v中的反投影坐标，a_vu为曝光时间校正参数、b_vu为图像亮度基底校正参数；通过高斯牛顿法迭代最小化光度误差，若误差经优化收敛，则将新帧视为关键帧加入滑动窗口进行优化。