[发明专利]一种基于改进点云匹配算法的室内移动机器人视觉导航方法

权利要求书

1.一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，该方法具体包括以下步骤：

S1：使用平面棋盘格校准Kinect相机，通过Kinect相机的外部参数对齐多个点云；

S2：得到离线校准的外部参数形成的变换矩阵，计算出多视点变换矩阵，用于全局匹配；

S3：使用Kinect相机获取周围环境的ORB特征点进行特征提取和匹配；

S4：估算相邻图像间相机运动构建局部地图，使用RANSAC消除误匹配，对获取的特征点进行PNP求解，用于机器人相邻位姿的增量估计；其中，RANSAC表示随机抽样一致算法，PNP表示Perspective-N-Point；

S5：引入闭环检测机制确保机器人得到全局一致的轨迹和地图；

S6：结合闭环检测机制，采用基于局部与全局特征点的相机位姿闭环约束点云优化算法，用于抑制系统的累积误差；

所述步骤S1具体包括：通过旋转RGB-D相机或平移机器人位置得到棋盘不同角度的对应参数，相机视图的所有变换矩阵T_i⁰从正向运动学中分析得到：

其中，T_i⁰为相机视图的所有变换矩阵，为相机的第k-1和k个关键帧之间的变换矩阵，为相机转向角，为相机倾斜角，为平台与棋盘转向角，为平台与棋盘倾斜角度，α₂为方程偏导数；

所述步骤S2具体包括：

S21：得到离线校准的外部参数形成的变换矩阵为：

其中，K为固定值，为旋转矩阵，为机器人坐标到相机坐标的平移矢量，[u v 1]^T是二维平面上2D像素坐标，[x y z 1]^T是全局坐标系中三维点坐标，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S22：计算多视点变换矩阵：

基于多视点摄像机标定生成多视角变换矩阵的离线标定方法：

其中，P_r是棋盘上坐标系的关键点坐标，P_i为第i帧点云图，为第i帧处棋盘特征点矩阵，是将P_r映射到P_i的平移矢量，T_rⁱ是将P_r映射到P_i的变换矩阵；假设定义F₀为全局坐标W，并把所有的P_i都转换为F₀，得到：

其中，F₀为初始坐标，为平移矢量，为初始平移矢量，是旋转矩阵；

定义旋转矩阵用于将P_i变换到F₀坐标，平移矢量计算出相机视图的所有变换矩阵为：

所述步骤S5中，引入的闭环检测机制具体包括：利用机器人位姿优化的目标函数：

其中，x_k表示第k时刻机器人位姿即pose节点，y_j表示第k时刻观测到的路标即point节点，z_k,j表示x_k和y_j之间的约束；在闭环检测过程中，e(x_k,y_j,z_k,j)为pose节点与point节点之间所产生的向量误差，即z_k,j满足约束的程度，当其值为0时，表示完全满足约束条件；g(x_k,y_j)表示第k时刻的位姿节点和路标之间的函数，C表示二维矩阵，Ω_kj表示所有位姿节点和路标节点；

其次，在机器人运动过程中，误差函数符合高斯分布函数，将SLAM等效为一个最小二乘问题，利用李群代数模型求解出目标函数与各节点的导数F_X和F_y，求解方程为：

e(x+δ_x,y+δ_y)＝e(x,y)+F_xδ_x+F_yδ_y

其中，δ_x为机器人位姿误差，δ_y为机器人路标误差，为李群代数，e_i为代价函数，矩阵H为稀疏特征矩阵，F_X和F_y分别表示相机运动和机器人运动后像素变化；通过使用变换矩阵对稀疏特征点进行匹配，包含所有SLAM求解变量与边缘值的问题；通过多次迭代得到适合目标函数的位姿节点x的值；通过上述转化把SLAM问题转换成节点和边，节点包含pose节点和point节点，表示优化变量；边为不同节点的约束，表示为误差约束条件，point-pose约束取决于系统中闭环产生的约束，pose-point取决于相机观测产生的约束；其中，SLAM表示视觉同步定位与映射；

所述步骤S6中，基于局部与全局特征点的相机位姿闭环约束点云优化算法，具体步骤为：

S61：根据帧间相似性选出关键帧序列，机器人局部状态与关键帧进行匹配；

S62：根据求得的相机多视角变换矩阵，得到优化后的相机位姿，对获取的点云进行坐标变换，初步实现点云融合；

S63：闭环检测采用K均值分类方法构建视觉词典树，提高闭环识别率；

S64：闭环检测完成后，遍历关键帧与当前帧进行相似变换矩阵Sim3求解与优化，闭环成功则跳出Sim3，得到对应相似性变换矩阵为：

其中，SO(3)为特殊正交群，R为点云对准时旋转矩阵，t为平移向量，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S65：点云优化：使用因子图优化算法对点云进行优化，进行点云拼接融合，实现点云优化，构建精确的三维点云地图。

2.根据权利要求1所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S5中，在闭环检测中为保证机器人闭环检测实时性设定相似性阈值，其中相似性计算函数为：

其中，V_l表示上一帧图像加权量，V_c表示当前帧加权向量，X越大说明图像相似性越高；因此，当X大于相似性阈值则舍弃当前帧。