[发明专利]机器人曲面运动定位方法及其运动定位系统

摘要

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

主权项

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)。