CN109648558A - 机器人曲面运动定位方法及其运动定位系统 - Google Patents

Abstract

本发明公开了一种机器人曲面运动定位方法及其运动定位系统，该方法采用RGB‑D深度相机获取机器人运动前方曲面表面彩色图像和深度图像进行特征匹配，记录惯性测量单元的三轴角速度值和三轴加速度值，优化特征匹配获取的旋转矩阵和平移矢量，根据深度图像进行点云拼接。对拼接后的点云划分近似平面的网格，同时根据轮式编码器输出值计算机器人运动速度，在网格中采用平面里程计法计算机器人位移。本发明中采用的深度点云拼接方法充分利用了机器人曲面运动的全部场景信息，采用编码器获取机器人的实时速度，提高了机器人长时间曲面上运动的位置定位和姿态定位精度，抗干扰能力强。

Description

机器人曲面运动定位方法及其运动定位系统

技术领域

本发明涉及一种计算机视觉技术，具体涉及一种机器人曲面运动定位方法及其运动定位系统。

背景技术

随着机器人技术的发展以及人力成本的增加，机器人代替人工进行高层建筑外墙清洗、大型结构件表面检测和维护、大型船舶等大型设备焊接及检测，可以提高生产效率、降低制造成本、减少人工劳动强度和操作风险。工业爬壁机器人由作业平台和能够吸附壁面运动的移动底盘组成，能实现在垂直壁面等危险环境移动和作业。工业爬壁机器人需要根据自身的姿态和位置信息调整作业单元的角度和位置，如焊接、打磨、表面清洗等等。

机器人作业于相对封闭环境内、三维复杂空间曲面上时，无法接收到GPS信号，也很难去搭建人工标记用于绝对位置定位和姿态定位，采用磁吸附方式的机器人搭载的磁强计输出值也不可靠。在相对定位方法中，采用陀螺仪估计姿态，加速度计积分估计位置的捷联惯导法在短时间内定位精度较高，但长时间运行漂移误差累计。利用相机等视觉传感器的导航技术近年来也得到了广泛应用，但是大型结构件或大型设备表面存在少纹理区域，纯视觉导航效果不佳。

采用相机和惯性测量单元组合进行位置和姿态定位的方法能够显著提高定位精度。中国专利文献CN201110417967公开了一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法、中国专利文献CN201611121973公开了一种基于遗传算法的视觉惯性组合的SLAM方法以及中国专利文献CN201711026822.7公开了一种捷联惯性导航系统/视觉里程计组合导航方法都是用双目相机和惯性导航的融合，双目相机在光滑结构件表面特征匹配中效果欠佳；中国专利文献CN201710320077公开了一种基于单目视觉和IMU信息的机器人定位与地图构建系统采用三角法提取特征点深度，在关键帧特征点数量较少时，定位误差较大；中国专利文献CN201711050182公开了一种基于单目vio的移动端AR方法利用单目相机和惯性测量单元组合的方法，其中视觉运动参数通过光流法获取，对光照条件要求苛刻，重复定位误差较大；中国专利文献CN201711077826公开了一种基于视觉惯导信息融合的定位方法及系统，采用了RGB-D相机和惯性测量单元组合的方法，仅利用了特征点的深度值，在光滑曲面位置定位中效果一般。

上述公开的技术不适用于大型结构件或大型设备表面工业机器人长时间位置定位和姿态定位。

发明内容

本发明的目的是提供一种机器人曲面运动定位方法及其运动定位系统，使其实现具有能够抑制长时间累积误差，抗干扰能力强，定位精度高等优点，从而实现机器人曲面作业自主定位功能。

本发明的技术方案如下：

一种用于机器人曲面运动的多传感器信息融合定位方法，所述方法包括如下步骤：

1)建立机器人曲面运动定位系统，定义坐标系：机器人曲面运动定位系统包括机器人及其设置在机器人上的附属机构，所述附属机构包括RGB-D相机、惯性测量单元、增量式编码器、吸附机构和运动机构，所述吸附机构用于使机器人吸附在物体表面，所述运动机构用于驱动机器人运动；

所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、图像平面坐标系、机器人坐标系；

2)机器人开始运动，采集RGB-D深度相机的深度图像和彩色图像，从当前帧中提取ORB特征点，计算三维坐标，如果是第一帧，保存所述第一帧特征点和对应深度，否则与上一帧图像进行ORB特征匹配，形成特征点对，并将特征点的ORB特征和对应特征点坐标加入BoW中，以特征点对在图像中的坐标(u，v)和在RGB-D相机坐标系中的三维坐标(x，y，z)作为输入参数，调用OpenCV中的SolvePnPRansac函数求解匹配连续两帧的图像特征点，获得机器人在两帧图像之间的旋转矩阵R_k和平移向量并对深度图像点云拼接成曲面；

3)对步骤2)拼接好的点云表面进行网格划分，对这些网格编号；将每个网格称为近似平面S_i(i＝1，2，3…)；计算近似平面S_i到空间惯性坐标系的变换矩阵C_is；

4)根据步骤3)判断机器人的空间位置及所处的近似平面，同时采集机器人两只驱动轮编码器输出值，计算机器人运动速度，在所在近似平面内采用平面里程计法计算机器人位移，求得机器人在空间惯性坐标系中的三维坐标；

5)根据步骤4)计算的机器人的三维坐标和构成近似平面的离散点，采用PNPoly算法判断机器人是否停留在步骤3)的近似平面内，如果仍在该近似平面内，继续采用平面里程计法计算，否则根据机器人位置坐标寻找其所在的近似平面，并返回步骤4)；

6)选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在第k帧的重投影误差、第k帧时由步骤2中的特征点对获得的机器人旋转矩阵和惯性测量单元输出的第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值、第k帧时步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值、第k帧时步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离；

7)对步骤6)计算的误差构造非线性最小二乘优化函数，通过高斯牛顿法迭代求解出第k帧时机器人的旋转矩阵和平移向量；

8)寻找关键帧中特征点是否出现在词袋中，如果是，根据关键帧特征点重投影误差修正当前旋转矩阵和位置，否则将特征点并入词袋，返回步骤2)。

优选地，步骤2)中，计算特征点在RGB-D相机坐标系中对应的三维坐标公式如式(1)(2)：

其中f_x和f_y是相机在X和Y两轴上的焦距；

c_x和c_y是相机光圈中心；

s是深度图像的缩放因子；

u和v是特征点在图像中的坐标；

(x，y，z)是特征点在图像坐标系中的三维坐标。

优选地，步骤3)中，划分网格的方法是：在彩色图像中，设定筛选半径为R，在步骤2)提取的ORB特征点中，筛选出欧式距离半径R内特征点数量超过20的点p_j，根据步骤2)点云曲面计算点p_j所在的切平面法向量构造成一个离散点以构造的离散点为节点，并随机器人运动形成离散点集，划分成网格。

优选地，步骤4)中，获取机器人在空间惯性坐标系中的三维坐标的步骤为：当机器人在物体表面运动时，在时刻t，根据其位置坐标判断其所处近似平面S_i，在近似平面S_i内运动时，驱动轮编码器采样次数为N，采样时间间隔恒定为f，计算近似平面内的位移d(t+Nf)如式(3)所示：

其中机器人平面内运动方向

v_l，v_r分别为左右驱动轮的速度；

由近似平面到空间坐标系的变换矩阵C_is将机器人在近似平面S_i中的位移变化映射到空间惯性坐标系中，求得机器人在下一时刻的三维坐标

优选地，步骤6)中选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在当前图像第k帧的重投影误差如式(4)所示，

其中，α是图像帧序数，β是第α帧图像中共同特征点的个数；

是第α帧图像中第β个共同特征点在第k帧彩色图像中对应的RGB-D相机坐标系下的三维坐标；

是第α帧图像中第β个共同特征点在第α帧彩色图像中对应的RGB-D相机坐标系下的三维坐标；

R_α+1是第α帧和第α+1帧之间的旋转矩阵；

是第α帧和第α+1帧之间的位移。

优选地，与第k帧的对应时刻，通过步骤2)的特征点对获得的机器人旋转矩阵和惯性测量单元输出第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值如下式(5)所示：

其中，Ω^j是惯性测量单元从第k-1帧开始计数采集的第j次的三轴角速度(ω_x，ω_y，ω_z)；

[Ω^j]_×是Ω^j的反对称矩阵；

R_CB是从RGB-D相机坐标系到惯性测量单元坐标系的旋转矩阵；

Δt是惯性测量单元采集三轴角速度的时间间隔；

R_k机器人在第k-1帧与第k帧之间的旋转矩阵；

J是第k-1帧到第k帧之间惯性测量单元采集三轴角速度的总次数。

优选地，与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值如下式(6)所示：

其中，是机器人在S_i平面从开始运动到k时刻由图像特征匹配获得的机器人位移的矢量和；

d(t+Nf)是机器人在近似平面S_i中的位移。

优选地，与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离如下式(7)所示：

其中，是第k帧时机器人在近似平面上的位置；

是第k帧时步骤2)点云曲面上距离机器人在近似平面上的位置欧式距离最小的点的坐标；

步骤7)中构造的最小二乘优化函数如下式(8)(9)所示：

通过高斯牛顿法迭代求解第k帧对应的位置向量及机器人旋转矩阵R_k的最优值。

优选地，所述惯性测量单元包括：三轴加速度计、三轴角速度计，所述的编码器为增量式光电旋转编码器，所述编码器安装在驱动轮轴上。

本发明还提供一种机器人曲面运动定位系统，采用以上所述的机器人曲面运动定位方法进行机器人曲面运动定位，该运动定位系统包括：RGB-D相机、惯性测量单元、增量式编码器、吸附机构、运动机构、坐标系构建模块、旋转矩阵和平移向量获取模块、网格划分模块、机器人三维坐标获取模块、机器人是否在近似平面判断模块、误差值获取模块、旋转矩阵和平移向量优化模块、旋转矩阵和位置修正模块，

其中：

所述吸附机构用于使机器人吸附在物体表面；

所述运动机构用于驱动机器人运动；

RGB-D相机用于实时采集物体表面的彩色图像和深度图像；

惯性测量单元用于采集机器人在曲面上运动的三轴角速度Ω^k(ω_x，ω_y，ω_z)和三轴加速度A^k(a_x，a_y，a_z)；

坐标系构建模块用于构建空间惯性坐标系、RGB-D相机坐标系、图像平面坐标系、机器人坐标系；

旋转矩阵和平移向量获取模块，用于在机器人运动时采集RGB-D深度相机的深度图像和彩色图像，从当前帧中提取ORB特征点，计算三维坐标，如果是第一帧，保存所述第一帧特征点和对应深度，否则与上一帧图像进行ORB特征匹配，形成特征点对，并将特征点的ORB特征和对应特征点坐标加入BoW中，以特征点对在图像中的坐标(u，v)和在RGB-D相机坐标系中的三维坐标(x，y，z)作为输入参数，调用OpenCV中的SolvePnPRansac函数求解匹配连续两帧的图像特征点，获得机器人在两帧图像之间的旋转矩阵R_k和平移向量并对深度图像点云拼接成曲面；

网格划分模块，用于对步骤2)拼接好的点云表面进行网格划分，对这些网格编号；将每个网格称为近似平面S_i(i＝1，2，3…)；计算近似平面S_i到空间惯性坐标系的变换矩阵C_is；

机器人三维坐标获取模块，用于根据步骤3)判断机器人的空间位置及所处的近似平面，同时采集机器人两只驱动轮编码器输出值，计算机器人运动速度，在所在近似平面内采用平面里程计法计算机器人位移，求得机器人在空间惯性坐标系中的三维坐标；

机器人是否在近似平面判断模块，用于根据步骤4)计算的机器人的三维坐标和构成近似平面的离散点，采用PNPoly算法判断机器人是否停留在步骤3)的近似平面内，如果仍在该近似平面内，继续采用平面里程计法计算，否则根据机器人位置坐标寻找其所在的近似平面，并返回步骤4)；

误差值获取模块，用于选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在第k帧的重投影误差、第k帧时由步骤2中的特征点对获得的机器人旋转矩阵和惯性测量单元输出的第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值、第k帧时步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值、第k帧时步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离；

旋转矩阵和平移向量优化模块，用于对步骤6)计算的误差构造非线性最小二乘优化函数，通过高斯牛顿法迭代求解出第k帧时机器人的旋转矩阵和平移向量；

旋转矩阵和位置修正模块，用于寻找关键帧中特征点是否出现在词袋中，如果是，根据关键帧特征点重投影误差修正当前旋转矩阵和位置，否则将特征点并入词袋，返回步骤2)。

本发明具有以下优点及突出性的技术效果：①本发明解决了在封闭空间、磁干扰条件下机器人在大型结构件或大型设备表面的曲面空间定位问题。②本发明通过融合RGB-D彩色图像和深度图像、惯性测量单元数据及编码器输出的速度信息，实现多传感器优势互补，抗干扰能力强。③本发明采用了机器人位置坐标约束、机器人运动平面约束、惯性测量单元残差约束及特征点重投影误差约束来对姿态和位置测量值进行非线性优化，实现更加平稳和精确的曲面运动定位。

附图说明

图1是本发明实施例的机器人曲面运动的多传感器信息融合定位方法流程图；

图2是本发明实施例的机器人传感器装配分布和工作示意图；

图3是本发明实施例的网格拼接和划分模型。

图中：1-RGB-D深度相机；2-惯性测量单元；3-增量式编码器；4-机器人；5-结构件；6-驱动轮；7-RGB-D深度相机视野区域；8-深度点云曲面；9-深度点云拼接块。

具体实施方式

下面将结合附图和实施例，对本发明的技术方案进行清楚、完整地描述：

一种用于机器人曲面运动的多传感器信息融合定位方法，如图1所示，所述方法包括如下步骤：

1)建立机器人曲面运动定位系统，定义坐标系：机器人曲面运动定位系统包括机器人以及安装在机器人上的附属机构，所述附属机构包括RGB-D相机、惯性测量单元(IMU)、增量式编码器、吸附机构和运动机构，所述吸附机构用于吸附在物体表面，所述运动机构用于驱动机器人运动；所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、图像平面坐标系、机器人坐标系。如图2所示，机器人4在结构件5的表面上移动，在机器人4的前端设置有RGB-D深度相机1，7为RGB-D深度相机视野区域。在爬壁机器人4上还设置有惯性测量单元2，驱动轮6与增量式编码器3同轴安装。其中，惯性测量单元包括三轴加速度计、三轴角速度计，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而角速度计检测载体的角速度信号。

2)机器人开始运动，采集RGB-D深度相机的深度图像和彩色图像，从当前帧中提取ORB(Oriented FAST and Rotated BRIEF，是一种快速提取特征点和描述子的算法)特征点，提取特征点的方法基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大(这里所说的灰度值差别够大是指与设定的阈值进行比较，大于该设定的阈值，则认为灰度值差别够大)，则认为该候选点为一个特征点。在提取了特征点之后，计算所提取特征点在RGB-D相机坐标系中的三维坐标，获得深度图像点云数据(即深度点云拼接块9)，其公式如下式(1)、(2)所示：

其中f_x和f_y是相机在X和Y两轴上的焦距，c_x和c_y是相机光圈中心，s是深度图像的缩放因子，u和v是特征点在图像中的坐标，(x，y，z)是特征点在RGB-D相机坐标系中的三维坐标。

如果是第一帧，保存该帧特征点和对应深度，否则与上一帧图像进行ORB特征匹配，形成特征点对，并将特征点的ORB特征和对应特征点坐标加入BoW中(Bag of Words，词袋模型)。具体说，是根据连续两帧的图像特征点采用FLANN算法(Fast Library forApproximate Nearest Neighbors，快速最近邻搜索算法)进行匹配获取特征点对，以特征点对在图像中的坐标(u，v)和在RGB-D相机坐标系中的三维坐标(x，y，z)作为输入参数，调用OpenCV(基于BSD许可(开源)发行的跨平台计算机视觉库)中的SolvePnPRansac函数求解匹配连续两帧的图像特征点，获得机器人在两帧图像之间的旋转矩阵R_k和平移向量并将深度图像的深度点云拼接块9拼接成深度点云曲面8；

3)对步骤2)拼接好的深度点云曲面8进行网格划分如附图3所示，对这些网格编号，具体步骤为：在彩色图像中，设定筛选半径为R，在步骤2)提取的ORB特征点中，筛选出欧式距离半径R内特征点数量超过20的点p_j(选中点p_j后，以点p_j为圆心，欧式距离半径R范围内的特征点将不再参与点p_j以外的点的筛选)，根据步骤2)点云曲面计算点p_j所在的切平面法向量构造成一个离散点以这些构造的离散点为节点，并随机器人运动形成离散点集，划分成网格；将每个网格视为近似平面S_i(i＝1，2，3…)；计算近似平面S_i到空间惯性坐标系的变换矩阵C_is；

4)根据步骤3)判断机器人的空间位置及所处的网格，同时采集机器人两只驱动轮编码器输出值，计算机器人运动速度，在所在网格内采用平面里程计法计算机器人位移，求得机器人三维坐标，其具体步骤为：当机器人在物体表面运动时，在时刻t，根据其位置坐标判断其所处近似平面S_i。在近似平面S_i内运动时，驱动轮编码器采样次数为N，采样时间间隔恒定为f，计算近似平面内的位移d(t+Nf)如式(3)所示：

其中机器人平面内运动方向v_l和v_r分别为左右驱动轮的速度。

由近似平面到空间坐标系的变换矩阵C_is将机器人在近似平面S_i中的位移变化映射到空间惯性坐标系中，求得机器人在下一时刻的三维坐标

5)根据步骤4)计算的机器人三维坐标和构成近似平面S_i的离散点，采用PNPoly算法(判断坐标点是否在不规则多边形内部的算法)判断机器人是否停留在步骤4)的网格内，如果仍在该近似平面内，继续采用步骤4中的方法计算位移变化，否则根据机器人位置坐标寻找其所在的新网格平面，并重复步骤4)；

6)选取从第k-n帧到第k帧(当前帧)图像帧，计算共同特征点(即步骤2中形成的多个特征点对对应的同一特征点)在当前图像第k帧的重投影误差如式(4)所示，

其中，α是图像帧序数，β是第α帧图像中共同特征点的个数；

是第α帧图像中第β个共同特征点在第k帧中对应的RGB-D相机坐标系下的三维坐标，是第α帧图像中第β个共同特征点在第α帧中对应的RGB-D相机坐标系下的三维坐标；

R_α+1是第α帧和第α+1帧之间的旋转矩阵；

是第α帧和第α+1帧之间的位移。

与第k帧的对应时刻，步骤2)通过特征匹配获得的机器人旋转矩阵和惯性测量单元输出第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值如下式(5)所示：

其中，Ω^j是惯性测量单元从第k-1帧开始计数采集的第j次的三轴角速度(ω_x，ω_y，ω_z)；

[Ω^j]_×是Ω^j的反对称矩阵；

R_CB是从RGB-D相机坐标系到惯性测量单元坐标系的旋转矩阵；

Δt是惯性测量单元采集三轴角速度的时间间隔。

R_k机器人在第k-1帧与第k帧之间的旋转矩阵。

J是第k-1帧到第k帧之间惯性测量单元采集三轴角速度的总次数。

与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值如下式(6)所示，其中是机器人在S_i平面从开始运动到k时刻由步骤2中的图像特征匹配获得的机器人位移(即平移向量)的矢量和：

与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离如下式(7)所示，其中是步骤2)拼接好的点云曲面上距离机器人在近似平面上的位置最近点的坐标：

7)对步骤6)计算的残差构造的最小二乘优化函数如下式(8)(9)所示，将(4)(5)(6)(7)式代入(8)(9)式：

通过高斯牛顿法迭代求解第k帧对应的平移向量及机器人旋转矩阵R_k的最优值。

8)寻找第k帧中特征点是否出现在词袋BoW中，如果是，根据关键帧特征点重投影误差修正当前旋转矩阵和平移向量，否则将特征点并入词袋，返回步骤2)；

可选地，所述的编码器为增量式光电旋转编码器，安装在驱动轮轴上。

可选地，机器人运动所在的物体表面的曲率半径大于等于5米。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种机器人曲面运动定位方法，其特征在于，所述方法包括如下步骤：

1)建立机器人曲面运动定位系统，定义坐标系：机器人曲面运动定位系统包括机器人及其设置在机器人上的附属机构，所述附属机构包括RGB-D相机、惯性测量单元、增量式编码器、吸附机构和运动机构，所述吸附机构用于使机器人吸附在物体表面，所述运动机构用于驱动机器人运动；

所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、图像平面坐标系、机器人坐标系；

2)机器人开始运动，采集RGB-D深度相机的深度图像和彩色图像，从当前帧中提取ORB特征点，计算三维坐标，如果是第一帧，保存所述第一帧特征点和对应深度，否则与上一帧图像进行ORB特征匹配，形成特征点对，并将特征点的ORB特征和对应特征点坐标加入BoW中，以特征点对在图像中的坐标(u，v)和在RGB-D相机坐标系中的三维坐标(x，y，z)作为输入参数，调用OpenCV中的SolvePnPRansac函数求解匹配连续两帧的图像特征点，获得机器人在两帧图像之间的旋转矩阵R_k和平移向量并对深度图像点云拼接成曲面；

3)对步骤2)拼接好的点云表面进行网格划分，对这些网格编号；将每个网格称为近似平面S_i(i＝1,2，3…)；计算近似平面S_i到空间惯性坐标系的变换矩阵C_is；

4)根据步骤3)判断机器人的空间位置及所处的近似平面，同时采集机器人两只驱动轮编码器输出值，计算机器人运动速度，在所在近似平面内采用平面里程计法计算机器人位移，求得机器人在空间惯性坐标系中的三维坐标；

5)根据步骤4)计算的机器人的三维坐标和构成近似平面的离散点，采用PNPoly算法判断机器人是否停留在步骤3)的近似平面内，如果仍在该近似平面内，继续采用平面里程计法计算，否则根据机器人位置坐标寻找其所在的近似平面，并返回步骤4)；

6)选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在第k帧的重投影误差、第k帧时由步骤2中的特征点对获得的机器人旋转矩阵和惯性测量单元输出的第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值、第k帧时步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值、第k帧时步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离；

7)对步骤6)计算的误差构造非线性最小二乘优化函数，通过高斯牛顿法迭代求解出第k帧时机器人的旋转矩阵和平移向量；

8)寻找关键帧中特征点是否出现在词袋中，如果是，根据关键帧特征点重投影误差修正当前旋转矩阵和位置，否则将特征点并入词袋，返回步骤2)。

2.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：步骤2)中，计算特征点在RGB-D相机坐标系中对应的三维坐标公式如式(1)(2)：

其中f_x和f_y是相机在X和Y两轴上的焦距；

c_x和c_y是相机光圈中心；

s是深度图像的缩放因子；

u和v是特征点在图像中的坐标；

(x，y，z)是特征点在图像坐标系中的三维坐标。

3.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：步骤3)中，划分网格的方法是：在彩色图像中，设定筛选半径为R，在步骤2)提取的ORB特征点中，筛选出欧式距离半径R内特征点数量超过20的点p_j，根据步骤2)点云曲面计算点p_j所在的切平面法向量构造成一个离散点以构造的离散点为节点，并随机器人运动形成离散点集，划分成网格。

4.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：步骤4)中，获取机器人在空间惯性坐标系中的三维坐标的步骤为：当机器人在物体表面运动时，在时刻t，根据其位置坐标判断其所处近似平面S_i，在近似平面S_i内运动时，驱动轮编码器采样次数为N，采样时间间隔恒定为f，计算近似平面内的位移d(t+Nf)如式(3)所示：

其中机器人平面内运动方向

v_l，v_r分别为左右驱动轮的速度；

由近似平面到空间坐标系的变换矩阵C_is将机器人在近似平面S_i中的位移变化映射到空间惯性坐标系中，求得机器人在下一时刻的三维坐标

5.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：步骤6)中选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在当前图像第k帧的重投影误差如式(4)所示，

其中，α是图像帧序数，β是第α帧图像中共同特征点的个数；

是第α帧图像中第β个共同特征点在第k帧彩色图像中对应的RGB-D相机坐标系下的三维坐标；

是第α帧图像中第β个共同特征点在第α帧彩色图像中对应的RGB-D相机坐标系下的三维坐标；

R_α+1是第α帧和第α+1帧之间的旋转矩阵；

是第α帧和第α+1帧之间的位移。

6.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：

与第k帧的对应时刻，通过步骤2)的特征点对获得的机器人旋转矩阵和惯性测量单元输出第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值如下式(5)所示：

其中，Ω_j是惯性测量单元从第k-1帧开始计数采集的第j次的三轴角速度(ω_x，ω_y，ω_z)；

[Ω^j]_×是Ω^j的反对称矩阵；

R_CB是从RGB-D相机坐标系到惯性测量单元坐标系的旋转矩阵；

Δt是惯性测量单元采集三轴角速度的时间间隔；

R_k机器人在第k-1帧与第k帧之间的旋转矩阵；

J是第k-1帧到第k帧之间惯性测量单元采集三轴角速度的总次数。

7.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：

与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值如下式(6)所示：

其中，是机器人在S_i平面从开始运动到k时刻由图像特征匹配获得的机器人位移的矢量和；

d(t+Nf)是机器人在近似平面S_i中的位移。

8.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：

与第k帧的对应时刻，步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离如下式(7)所示：

其中，是第k帧时机器人在近似平面上的位置；

是第k帧时步骤2)点云曲面上距离机器人在近似平面上的位置欧式距离最小的点的坐标；

步骤7)中构造的最小二乘优化函数如下式(8)(9)所示：

通过高斯牛顿法迭代求解第k帧对应的位置向量及机器人旋转矩阵R_k的最优值。

9.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：所述惯性测量单元包括：三轴加速度计、三轴角速度计，所述的编码器为增量式光电旋转编码器，所述编码器安装在驱动轮轴上。

10.一种机器人曲面运动定位系统，其特征在于，采用权利要求1至9中任一项所述的机器人曲面运动定位方法进行机器人曲面运动定位，该运动定位系统包括：RGB-D相机、惯性测量单元、增量式编码器、吸附机构、运动机构、坐标系构建模块、旋转矩阵和平移向量获取模块、网格划分模块、机器人三维坐标获取模块、机器人是否在近似平面判断模块、误差值获取模块、旋转矩阵和平移向量优化模块、旋转矩阵和位置修正模块，

其中：

所述吸附机构用于使机器人吸附在物体表面；

所述运动机构用于驱动机器人运动；

RGB-D相机用于实时采集物体表面的彩色图像和深度图像；

惯性测量单元用于采集机器人在曲面上运动的三轴角速度Ω^k(ω_x，ω_y，ω_z)和三轴加速度A^k(a_x，a_y，a_z)；

坐标系构建模块用于构建空间惯性坐标系、RGB-D相机坐标系、图像平面坐标系、机器人坐标系；

旋转矩阵和平移向量获取模块，用于在机器人运动时采集RGB-D深度相机的深度图像和彩色图像，从当前帧中提取ORB特征点，计算三维坐标，如果是第一帧，保存所述第一帧特征点和对应深度，否则与上一帧图像进行ORB特征匹配，形成特征点对，并将特征点的ORB特征和对应特征点坐标加入BoW中，以特征点对在图像中的坐标(u，v)和在RGB-D相机坐标系中的三维坐标(x，y，z)作为输入参数，调用OpenCV中的SolvePnPRansac函数求解匹配连续两帧的图像特征点，获得机器人在两帧图像之间的旋转矩阵R_k和平移向量并对深度图像点云拼接成曲面；

网格划分模块，用于对步骤2)拼接好的点云表面进行网格划分，对这些网格编号；将每个网格称为近似平面S_i(i＝1，2，3…)；计算近似平面S_i到空间惯性坐标系的变换矩阵C_is；

机器人三维坐标获取模块，用于根据步骤3)判断机器人的空间位置及所处的近似平面，同时采集机器人两只驱动轮编码器输出值，计算机器人运动速度，在所在近似平面内采用平面里程计法计算机器人位移，求得机器人在空间惯性坐标系中的三维坐标；

机器人是否在近似平面判断模块，用于根据步骤4)计算的机器人的三维坐标和构成近似平面的离散点，采用PNPoly算法判断机器人是否停留在步骤3)的近似平面内，如果仍在该近似平面内，继续采用平面里程计法计算，否则根据机器人位置坐标寻找其所在的近似平面，并返回步骤4)；

误差值获取模块，用于选取从第k-n帧到第k帧的图像帧，计算特征点对对应的共同特征点在第k帧的重投影误差、第k帧时由步骤2中的特征点对获得的机器人旋转矩阵和惯性测量单元输出的第k-1帧与第k帧之间的三轴角速度积分获取的旋转矩阵的误差值、第k帧时步骤4)中平面里程计法计算的机器人位移与步骤2)机器人位移的误差值、第k帧时步骤4)中平面里程计法计算的机器人三维坐标对应点与步骤2)点云曲面的距离；

旋转矩阵和平移向量优化模块，用于对步骤6)计算的误差构造非线性最小二乘优化函数，通过高斯牛顿法迭代求解出第k帧时机器人的旋转矩阵和平移向量；

旋转矩阵和位置修正模块，用于寻找关键帧中特征点是否出现在词袋中，如果是，根据关键帧特征点重投影误差修正当前旋转矩阵和位置，否则将特征点并入词袋，返回步骤2)。