CN109676604B - 机器人曲面运动定位方法及其运动定位*** - Google Patents

Abstract

本发明涉及机器人曲面运动定位方法及其运动定位***，该方法采用RGB‑D深度相机采集机器人运动前方曲面的彩色图像和深度图像信息，通过特征匹配的方法计算机器人旋转矩阵和位移；同时采用惯性测量单元IMU的三轴加速度数据优化机器人的俯仰角和横滚角，利用特征点的重投影误差修正机器人的航向角和平移向量，拼接由深度图像获取的机器人运动曲面点云；根据机器人与曲面点云的位置约束和惯性测量单元IMU误差最小化约束对位置和姿态进行非线性优化，获取最终位姿信息；将关键帧中特征点制成词典，用于回环检测。本发明能够对机器人曲面运动进行位置定位和姿态定位，***结构简单，定位精度高，能够长时间可靠工作。

Description

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

技术领域

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

背景技术

随着机器人技术的飞速发展，工业移动机器人得到广泛使用。爬壁机器人作为工业移动机器人的一种，通过磁吸附或气压吸附在大型结构件或设备表面进行代替人工进行作业。自主位置定位和姿态定位是爬壁机器人实现自动或半自动工作的基础条件。

在封闭空间、周围环境存在强磁场干扰及缺乏典型的结构化特征的工作环境下，GPS信号难以接收，磁强计容易受到周围磁场干扰，机器人驱动轮打滑也限制了测程法的应用。单一导航***很难同时满足各种环境的导航要求。因此通过把具有不同特点的两种或以上的设备组合在一起构成一个综合***，能充分发挥各自的优势达到整体性能最佳。视觉传感器和惯性传感器组合的方式具有价格低、结构简单、信息量丰富等优势得到关注和应用。现有的视觉惯性定位方法中，当采用单目相机作为视觉传感器时，需要在亮度不变假设的前提下应用光流法获取运动参数，受周围环境光照影响较大；当采用双目相机作为视觉传感器时，采用三角法计算和优化特征点的三维坐标，对特征点的数量和可识别度要求高，大型设备或结构件表面特征点相似度较高，容易出现误匹配；当采用RGB-D(深度图像)深度相机作为视觉传感器时，中国专利文献CN201711077826公开了一种基于视觉惯导信息融合的定位方法及***采用harris角点计算描述子作为特征点，但是舍弃了非harris角点的深度信息，在曲面运动的位置定位中误差较大，损失信息较多。

发明内容

本发明的目的是提供一种机器人曲面运动定位方法及其运动定位***，使其实现具有结构简单，抗干扰能力强，定位精度高等优点，从而进一步提升机器人自主定位水平。

本发明的技术方案如下：

机器人曲面运动定位方法，所述方法包括如下步骤：

1)建立机器人曲面运动定位***，定义坐标系：机器人曲面运动定位***包括机器人，所述机器人上设置有RGB-D相机、惯性测量单元IMU、吸附装置和运动机构，所述吸附装置用于使机器人吸附在物体表面，所述运动机构用于驱动机器人运动；

所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系；

对RGB-D相机的彩色图像与深度图像进行配准；

2)机器人运动，同时通过固联在机器人前方的RGB-D相机实时采集物体表面的彩色图像和深度图像，通过配备的惯性测量单元IMU采集机器人在曲面上运动的三轴角速度Ω^k(ω_x,ω_y,ω_z)和三轴加速度A^k(a_x,a_y,a_z)；

3)在步骤2)中采集的彩色图像中利用ORB特征提取方法提取特征点，采用BRIEF算法计算描述子，利用最近邻搜索方法完成特征匹配，计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

4)在每两帧彩色图像之间，根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算第k帧的欧拉角

将惯性测量单元IMU三轴加速度计输出值作为重力加速度在三轴上的分量A^k(a_x,a_y,a_z)，并利用所述分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k从而修正第k帧的欧拉角，利用更新后的横滚角β_k和俯仰角γ_k修正步骤3)旋转矩阵

再计算上一帧特征点在第k帧中的重投影误差，利用最小二乘法修正航向角α_k和平移向量

根据修正后的第k帧的欧拉角和平移向量计算机器人的三维坐标 P_t，利用最小二乘法更新惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)，并将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值；

5)利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，生成机器人运动曲面点云，对曲面点云进行体素栅格滤波；

6)根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k帧的平移向量

7)执行闭环检测，对比第k帧中特征点的BRIEF描述子与BoW词袋模型中描述子，当相似度大于设定值就认定为相似，根据BoW词袋模型中的描述子对应的特征点三维坐标更新机器人位姿信息，否则将第k帧中特征点加入BoW词袋模型中，返回步骤3)。

优选地，步骤4)中根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU 输出值计算第k帧的欧拉角

的公式如式(1)和式(2)：

R_k-1＝DCMFromEuler(α_k-1,β_k-1,γ_k-1) (1)

其中，DCMFromEuler是将欧拉角转换为旋转矩阵；

EulerFromDCM是将旋转矩阵转换为欧拉角；

Ω^k是惯性测量单元IMU采集机器人在运动曲面上运动的第k帧的三轴角速度；

b^k-1是第k-1帧的惯性测量单元IMU的三轴角速度误差；

R_k-1是机器人在第k-1帧空间惯性坐标系下的旋转矩阵；

Δt是第k帧与第k-1帧的时间差。

优选地，构造第k帧特征点在第k-1帧中的重投影误差的非线性方程修正航向角α_k和平移向量

如式(3)：

其中，

分别是特征点在第k帧和第k-1帧的三维坐标；

α_k是机器人航向角；

R_k(α_k)是从第k-1帧到到第k帧的旋转矩阵。

优选地，所述的惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)计算步骤如下：

首先，采用式(4)根据惯性测量单元IMU输出值计算出从k-m帧到第k帧的旋转矩阵

其中，J_r(Ω^jΔt)是计算Ω^jΔt在李群上的右雅克比矩阵；

k是当前帧；

Ω^j是惯性测量单元IMU采集的机器人在运动曲面上运动的第j帧的三轴角速度(ω_x,ω_y,ω_z)；

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

R_k-m是机器人在第k-m帧时对应的空间惯性坐标系下的旋转矩阵；

然后采用式(5)根据图像特征匹配计算出从k-m帧到第k帧的旋转矩阵

是空间惯性坐标系下机器人在第j帧的旋转矩阵；

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

利用式(4)、式(5)，对惯性测量单元IMU的三轴角速度误差b^k构造非线性方程如式(6) 所示，

通过高斯牛顿法迭代求解式(6)得到最优b^k。

优选地，步骤6)中，对修正后的平移向量进行优化的方法包括：

在机器人坐标系中设垂直运动曲面向上方向单位向量

根据步骤4)中获取的第k帧的欧拉角和平移向量初步估计机器人位置P_t，在运动曲面Σ上选取P_t附近离散点，拟合出P_t点的切平面，并求出对应的法向量

残差r₁计算如式(7)：

其中R_k是机器人在第k帧空间惯性坐标系下的旋转矩阵；

是机器人坐标系中垂直运动曲面向上方向的单位向量；

计算机器人位置坐标到运动曲面距离r₂，计算公式如式(8)：

其中P_t表示机器人位置计算值，M_t表示步骤5)中机器人运动曲面点云上与机器人计算位置最近的一点，

是待优化的机器人平移向量；

对平移向量进行优化构造的非线性方程如式(9)：

通过高斯牛顿法迭代求解机器人每两帧图像间的平移向量最优值。

优选地，所述的惯性测量单元IMU包括陀螺仪和三轴加速度计。

优选地，所述物体表面的曲率半径大于等于5米。

优选地，利用重力加速度在加速度计三轴方向的分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k，具体方法是先将步骤3)旋转矩阵

转换成欧拉角表示方式，将横滚角更新为β_k，将俯仰角更新为γ_k，再将欧拉角转换成新的旋转矩阵R_k。

本发明还提供一种机器人曲面运动定位***，采用以上所述的机器人曲面运动定位方法进行机器人曲面运动定位，该运动定位***包括：RGB-D深度相机、惯性测量单元IMU、爬壁机器人、坐标系构建模块、配准模块、旋转矩阵和平移向量获取模块、旋转矩阵和平移向量修正模块、RGB-D深度图像拼接模块、平移向量优化模块、相似度比较模块，其中：

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

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

坐标系构建模块用于构建空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系；

配准模块用于对RGB-D相机的彩色图像与深度图像进行配准；

旋转矩阵和平移向量获取模块用于在步骤2)中采集的彩色图像中利用ORB特征提取方法提取特征点，采用BRIEF算法计算描述子，利用最近邻搜索方法完成特征匹配，计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

旋转矩阵和平移向量修正模块用于在每两帧彩色图像之间，根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算第k帧的欧拉角

将惯性测量单元IMU三轴加速度计输出值作为重力加速度在三轴上的分量A^k(a_x,a_y,a_z)，并利用所述分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k从而修正第k帧的欧拉角，利用更新后的横滚角β_k和俯仰角γ_k修正步骤3)旋转矩阵

再计算上一帧特征点在第k帧中的重投影误差，利用最小二乘法修正航向角α_k和平移向量

根据修正后的第k帧的欧拉角和平移向量计算机器人的三维坐标P_t，利用最小二乘法更新惯性测量单元IMU的三轴角速度误差 b^k(b_x,b_y,b_z)，并将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值；

RGB-D深度图像拼接模块利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，生成机器人运动曲面点云，对曲面点云进行体素栅格滤波；

平移向量优化模块用于根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k帧的平移向量

相似度比较模块用于执行闭环检测，对比第k帧中特征点的BRIEF描述子与BoW词袋模型中描述子，当相似度大于设定值就认定为相似，根据BoW词袋模型中的描述子对应的特征点三维坐标更新机器人位姿信息，否则将第k帧中特征点加入BoW词袋模型中。

本发明提出的基于RGB-D和IMU信息的机器人曲面运动定位方法能够利用视觉传感器所拍摄区域的全部深度信息，根据***状态输出，逐块拼接深度图片，构造机器人运动所在的曲面模型并对机器人位置定位进行优化，适用于机器人在曲面运动中的长期位置定位和姿态定位。本发明具有以下优点及突出性的技术效果：①采用RGB-D深度相机直接获取特征点三维坐标，省去三角测量法深度值计算。②采用惯性测量单元IMU辅助测量的方法避免了单目视觉中机器人旋转条件下深度无法计算的问题。③采用RGB-D深度图像拼接获取机器人运动曲面点云，对机器人姿态和位置参数进行二次优化，增加了定位精度。

附图说明

图1是本发明实施例的机器人曲面运动定位***流程示意图；

图2是本发明实施例的机器人曲面运动及RGB-D相机采集方法示意图；

图3是本发明实施例中利用旋转矩阵和平移向量对RGB-D深度图像拼接生成机器人运动曲面点云示意图；

图4是本发明实施例中利用曲面点云优化位置定位和姿态定位参数原理图。

图中：1-RGB-D深度相机；2-惯性测量单元IMU；3-爬壁机器人；4-大型结构件；5-RGB-D 深度相机视野区域；6-深度点云曲面；7-深度点云拼接块；8-深度点云拼接重复部分。

具体实施方式

下面将参考附图来描述本发明所述的机器人曲面运动定位方法及其运动定位***的实施例。本领域的普通技术人员可以认识到，在不偏离本发明的精神和范围的情况下，可以用各种不同的方式或其组合对所描述的实施例进行修正。因此，附图和描述在本质上是说明性的，而不是用于限制权利要求的保护范围。此外，在本说明书中，附图未按比例画出，并且相同的附图标记表示相同的部分。

机器人曲面运动定位方法的流程示意图如图1所示，所述方法包括如下步骤：

1)建立机器人曲面运动定位***，定义坐标系：机器人曲面运动定位***包括机器人，所述机器人上设置有RGB-D相机、惯性测量单元IMU、吸附装置和运动机构，所述吸附装置用于吸附在物体表面，所述运动机构用于驱动机器人运动；所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系；对RGB-D相机的彩色图像与深度图像进行配准。如图2所示，爬壁机器人3在大型结构件4的表面上移动，在爬壁机器人3的前端设置有RGB-D深度相机1，在爬壁机器人3上还设置有IMU惯性测量单元2。5标识的是RGB-D深度相机1的摄像区域。

2)机器人在大型设备或结构件(曲率半径大于5米)表面运动如附图2所示，同时通过固联在机器人前方拍摄方向向下的RGB-D相机实时采集彩色图像和深度图像，通过配备的惯性测量单元IMU采集机器人在曲面上运动的三轴角速度Ω^k(ω_x,ω_y,ω_z)和三轴加速度 A^k(a_x,a_y,a_z)。

3)在步骤2)中采集的彩色图像中利用ORB(Oriented FAST and Rotated BRIEF，是一种快速提取特征点和描述子的算法)特征提取特征点，提取特征点的方法基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大(这里所说的灰度值差别够大是指与设定的阈值进行比较，大于该设定的阈值，则认为灰度值差别够大)，则认为该候选点为一个特征点。采用BRIEF特征描述子提取算法计算描述子，利用最近邻搜索方法完成特征匹配，也就是搜索与一个特征点的描述子最近邻的描述子，从而完成特征匹配。根据连续两帧的图像特征点匹配，采用最小二乘法求解匹配连续两帧的图像特征点的机器人旋转矩阵和平移向量，具体说，最小化连续两帧的图像特征点的三维坐标的差值的平方和，从而计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

4)在每两帧图像之间，根据上一帧图像对应的机器人的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算当前帧的欧拉角

式(1)是将上一帧图像对应的欧拉角转换为旋转矩阵R_k-1方便计算，式(2)是根据惯性测量单元IMU输出值进行递推求解下一时刻的旋转矩阵，并将机器人的旋转矩阵转换为欧拉角表示：

R_k-1＝DCMFromEuler(α_k-1,β_k-1,γ_k-1) (1)

机器人在大曲率曲面低速运动，其加速度近似为0，惯性测量单元IMU三轴加速度计输出值近似为重力加速度在三轴上的投影，利用重力加速度在三轴加速度计三轴方向的分量 A^k(a_x,a_y,a_z)计算机器人横滚角β_k和俯仰角γ_k，替代式(2)中由惯性测量单元IMU计算出的机器人横滚角

和俯仰角

具体方法是先将步骤3)旋转矩阵

转换成欧拉角表示方式，然后根据式(3)计算机器人横滚角β_k，根据式(4)计算机器人俯仰角γ_k，

β_k＝atan2(a_x,a_z) (3)

其中a_x,a_y,a_z分别是惯性测量单元IMU三轴加速度计输出值；

atan2(a_x,a_z)是求a_x与a_z比值的四象限反正切函数；

arcsin是反正弦函数。

将式(2)中由惯性测量单元IMU计算出的机器人横滚角

更新为β_k，将机器人俯仰角

更新为γ_k，然后再将更新后的欧拉角转换成新的旋转矩阵R_k。将R_k当做α_k的函数，记为R_k(α_k)；在空间惯性坐标系中，将上一帧中的特征点

通过旋转矩阵R_k(α_k)和平移向量

投影到当前帧，与当前帧对应的特征点

相减获得重投影误差，将所有特征点重投影误差求和，利用最小二乘法修正航向角α_k和平移向量

如式(5)：

其中，

分别是特征点在当前帧和上一帧的三维坐标；

根据修正后的当前帧的欧拉角和平移向量计算机器人的三维坐标P_t(x_t,y_t,z_t)，再次构造出关于惯性测量单元IMU的三轴角速度误差的非线性方程，计算出惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)。将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值。

具体方法是首先根据惯性测量单元IMU输出值计算出从k-m帧到当前帧(第k帧)的旋转矩阵，如式(6)：

其中J_r(Ω^jΔt)是计算Ω^jΔt在李群上的右雅克比矩阵；

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

再根据图像特征匹配计算出从k-m帧到当前帧(第k帧)的旋转矩阵，如式(7)：

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

根据旋转矩阵的正交特性，在没有误差的条件下，

和

应该是相等的，

和

乘积为单位阵，构造惯性测量单元IMU的三轴角速度误差非线性方程如式 (8)：

5)利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，如附图3所示，图3-1显示的是已有的深度点云曲面6，图3-2显示的深度点云拼接块7，其中阴影部分是在拼接中重复的部分。将新的点云坐标添加到已有的深度点云平面6中，同时去除深度点云拼接重复部分8，生成新的机器人运动曲面点云，如图3-3所示。然后对曲面点云进行体素栅格滤波。

6)根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k时刻平移向量

下面具体说明如下：机器人在曲面上运动，由于加速度二次积分的方式容易导致误差累计，采用优化步骤4)所得的平移向量可以获取更可靠的位置定位。机器人在任意时刻的速度方向在运动曲面上对应位置的切平面内，本例中采用轮式爬壁机器人，大型设备或结构件曲率很小，可以近似成瞬时平面运动，在机器人坐标系中设垂直运动曲面向上方向的单位向量

机器人沿

方向运动速度为0，根据步骤4)中获取的当前帧的欧拉角和平移向量初步估计机器人位置P_t，在运动曲面Σ上选取P_t附近离散点，拟合出P_t点的切平面，并求出对应的法向量

理想情况下

与

的向量积为0，因此选择

与

的向量积作为残差进行优化，如式(9)：

其中R_k是机器人在k时刻空间惯性坐标系下的旋转矩阵；

是机器人坐标系中垂直运动曲面向上方向的单位向量；

机器人坐标在理想状态下应该处于步骤5)获取的运动曲面内，但是由于惯性测量单元 IMU输出三轴角速度和加速度误差、图像特征匹配误差以及计算精度误差等因素影响，机器人位置坐标对应的三维空间点存在偏离运动曲面的情况，如附图4所示，图中a、b、c点是机器人理想条件下在点云曲面上的位置，a^*、b^*、c^*是误差存在条件下机器人位置，保持机器人位置坐标到运动曲面距离平方和最小可以有效减小机器人定位误差，平移向量优化计算公式如式(10)：

其中P_t表示机器人位置计算值，M_t表示机器人运动曲面点云上与机器人计算位置最近的一点，

是待优化的机器人平移向量；

对平移向量进行优化构造的非线性方程如式(11)：

通过高斯牛顿法迭代求解机器人每两帧图像间的平移向量

的最优值。

7)执行闭环检测，对比当前帧中特征点的BRIEF描述算子与BoW词袋模型中描述算子，当相似度大于设定值就认为相似，根据BoW词袋模型中的描述算子对应的特征点三维坐标更新机器人位姿信息，否则将新特征点加入BoW词袋模型中。

在一个可选实施例中，所述的惯性测量单元IMU包括陀螺仪和三轴加速度计。

本发明还提供一种机器人曲面运动定位***，采用以上所述的方法进行机器人曲面运动定位，包括RGB-D深度相机1、惯性测量单元IMU2、爬壁机器人3。如图2所示，爬壁机器人3在大型结构件4的表面上移动，在爬壁机器人3的前端设置有RGB-D深度相机1，在爬壁机器人3上还设置有IMU惯性测量单元2、吸附装置(未示出)和运动机构(未示出)。所述吸附装置用于吸附在物体表面，所述运动机构用于驱动机器人运动。5标识的是RGB-D 深度相机1的摄像区域。通过固联在机器人前方拍摄方向向下的RGB-D相机实时采集彩色图像和深度图像，通过配备的惯性测量单元IMU采集机器人在曲面上运动的三轴角速度Ω^k(ω_x,ω_y,ω_z)和三轴加速度A^k(a_x,a_y,a_z)。

进一步地，机器人曲面运动定位***还包括坐标系构建模块，用于建立机器人曲面运动定位***，定义坐标系：所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系。

进一步地，还包括配准模块，用于对RGB-D相机的彩色图像与深度图像进行配准。

进一步地，还包括旋转矩阵和平移向量获取模块，用于在步骤2)中采集的彩色图像中利用ORB(Oriented FAST and Rotated BRIEF，是一种快速提取特征点和描述子的算法)特征提取特征点，提取特征点的方法基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大(这里所说的灰度值差别够大是指与设定的阈值进行比较，大于该设定的阈值，则认为灰度值差别够大)，则认为该候选点为一个特征点。采用BRIEF特征描述子提取算法计算描述子，利用最近邻搜索方法完成特征匹配，也就是搜索与一个特征点的描述子最近邻的描述子，从而完成特征匹配。根据连续两帧的图像特征点匹配，采用最小二乘法求解匹配连续两帧的图像特征点的机器人旋转矩阵和平移向量，具体说，最小化连续两帧的图像特征点的三维坐标的差值的平方和，从而计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

进一步地，还包括旋转矩阵和平移向量修正模块，用于在每两帧图像之间，根据上一帧图像对应的机器人的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算当前帧的欧拉角

式(1)是将上一帧图像对应的欧拉角转换为旋转矩阵R_k-1方便计算，式(2)是根据惯性测量单元IMU输出值进行递推求解下一时刻的旋转矩阵，并将机器人的旋转矩阵转换为欧拉角表示：

R_k-1＝DCMFromEuler(α_k-1,β_k-1,γ_k-1) (1)

机器人在大曲率曲面低速运动，其加速度近似为0，惯性测量单元IMU三轴加速度计输出值近似为重力加速度在三轴上的投影，利用重力加速度在三轴加速度计三轴方向的分量 A^k(a_x,a_y,a_z)计算机器人横滚角β_k和俯仰角γ_k，替代式(2)中由惯性测量单元IMU计算出的机器人横滚角

和俯仰角

具体方法是先将步骤3)旋转矩阵

转换成欧拉角表示方式，然后根据式(3)计算机器人横滚角β_k，根据式(4)计算机器人俯仰角γ_k，

β_k＝atan2(a_x,a_z) (3)

其中a_x,a_y,a_z分别是惯性测量单元IMU三轴加速度计输出值；

atan2(a_x,a_z)是求a_x与a_z比值的四象限反正切函数；

arcsin是反正弦函数。

将式(2)中由惯性测量单元IMU计算出的机器人横滚角

更新为β_k，将机器人俯仰角

更新为γ_k，然后再将更新后的欧拉角转换成新的旋转矩阵R_k。将R_k当做α_k的函数，记为R_k(α_k)；在空间惯性坐标系中，将上一帧中的特征点

通过旋转矩阵R_k(α_k)和平移向量

投影到当前帧，与当前帧对应的特征点

相减获得重投影误差，将所有特征点重投影误差求和，利用最小二乘法修正航向角α_k和平移向量

如式(5)：

其中，

分别是特征点在当前帧和上一帧的三维坐标；

根据修正后的当前帧的欧拉角和平移向量计算机器人的三维坐标P_t(x_t,y_t,z_t)，再次构造出关于惯性测量单元IMU的三轴角速度误差的非线性方程，计算出惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)。将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值。

具体方法是首先根据惯性测量单元IMU输出值计算出从k-m帧到当前帧(第k帧)的旋转矩阵，如式(6)：

其中J_r(Ω^jΔt)是计算Ω^jΔt在李群上的右雅克比矩阵；

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

再根据图像特征匹配计算出从k-m帧到当前帧(第k帧)的旋转矩阵，如式(7)：

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

根据旋转矩阵的正交特性，在没有误差的条件下，

和

应该是相等的，

和

乘积为单位阵，构造惯性测量单元IMU的三轴角速度误差非线性方程如式 (8)：

进一步地，还包括RGB-D深度图像拼接模块，用于利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，如附图3所示，图3-1显示的是已有的深度点云曲面6，图3-2显示的深度点云拼接块7，其中阴影部分是在拼接中重复的部分。将新的点云坐标添加到已有的深度点云平面6中，同时去除深度点云拼接重复部分8，生成新的机器人运动曲面点云，如图3-3所示。然后对曲面点云进行体素栅格滤波。

进一步地，还包括平移向量优化模块，用于根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k时刻平移向量

下面具体说明如下：机器人在曲面上运动，由于加速度二次积分的方式容易导致误差累计，采用优化步骤4)所得的平移向量可以获取更可靠的位置定位。机器人在任意时刻的速度方向在运动曲面上对应位置的切平面内，本例中采用轮式爬壁机器人，大型设备或结构件曲率很小，可以近似成瞬时平面运动，在机器人坐标系中设垂直运动曲面向上方向的单位向量

机器人沿

方向运动速度为0，根据步骤4)中获取的当前帧的欧拉角和平移向量初步估计机器人位置P_t，在运动曲面Σ上选取P_t附近离散点，拟合出P_t点的切平面，并求出对应的法向量

理想情况下

与

的向量积为0，因此选择

与

的向量积作为残差进行优化，如式(9)：

其中R_k是机器人在k时刻空间惯性坐标系下的旋转矩阵；

是机器人坐标系中垂直运动曲面向上方向的单位向量；

机器人坐标在理想状态下应该处于步骤5)获取的运动曲面内，但是由于惯性测量单元 IMU输出三轴角速度和加速度误差、图像特征匹配误差以及计算精度误差等因素影响，机器人位置坐标对应的三维空间点存在偏离运动曲面的情况，如附图4所示，图中a、b、c点是机器人理想条件下在点云曲面上的位置，a^*、b^*、c^*是误差存在条件下机器人位置，保持机器人位置坐标到运动曲面距离平方和最小可以有效减小机器人定位误差，平移向量优化计算公式如式(10)：

其中P_t表示机器人位置计算值，M_t表示机器人运动曲面点云上与机器人计算位置最近的一点，

是待优化的机器人平移向量；

对平移向量进行优化构造的非线性方程如式(11)：

通过高斯牛顿法迭代求解机器人每两帧图像间的平移向量

的最优值。

进一步地，还包括相似度比较模块，用于执行闭环检测，对比当前帧中特征点的BRIEF 描述算子与BoW词袋模型中描述算子，当相似度大于设定值就认为相似，根据BoW词袋模型中的描述算子对应的特征点三维坐标更新机器人位姿信息，否则将新特征点加入BoW词袋模型中，返回步骤3)。

Claims (9)

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

1)建立机器人曲面运动定位***，定义坐标系：机器人曲面运动定位***包括机器人，所述机器人上设置有RGB-D相机、惯性测量单元IMU、吸附装置和运动机构，所述吸附装置用于使机器人吸附在物体表面，所述运动机构用于驱动机器人运动；

所述坐标系包括空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系；

对RGB-D相机的彩色图像与深度图像进行配准；

2)机器人运动，同时通过固联在机器人前方的RGB-D相机实时采集物体表面的彩色图像和深度图像，通过配备的惯性测量单元IMU采集机器人在曲面上运动的三轴角速度Ω^k(ω_x,ω_y,ω_z)和三轴加速度A^k(a_x,a_y,a_z)；

3)在步骤2)中采集的彩色图像中利用ORB特征提取方法提取特征点，采用BRIEF算法计算描述子，利用最近邻搜索方法完成特征匹配，计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

4)在每两帧彩色图像之间，根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算第k帧的欧拉角

将惯性测量单元IMU三轴加速度计输出值作为重力加速度在三轴上的分量A^k(a_x,a_y,a_z)，并利用所述分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k从而修正第k帧的欧拉角，利用更新后的横滚角β_k和俯仰角γ_k修正步骤3)旋转矩阵

再计算上一帧特征点在第k帧中的重投影误差，利用最小二乘法修正航向角α_k和平移向量

根据修正后的第k帧的欧拉角和平移向量计算机器人的三维坐标P_t，利用最小二乘法更新惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)，并将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值；

5)利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，生成机器人运动曲面点云，对曲面点云进行体素栅格滤波；

6)根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k帧的平移向量

7)执行闭环检测，对比第k帧中特征点的BRIEF描述子与BoW词袋模型中描述子，当相似度大于设定值就认定为相似，根据BoW词袋模型中的描述子对应的特征点三维坐标更新机器人位姿信息，否则将第k帧中特征点加入BoW词袋模型中，返回步骤3)。

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

步骤4)中根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算第k帧的欧拉角

的公式如式(1)和式(2)：

R_k-1＝DCMFromEuler(α_k-1，β_k-1，γ_k-1) (1)

其中，DCMFromEuler是将欧拉角转换为旋转矩阵；

EulerFromDCM是将旋转矩阵转换为欧拉角；

Ω^k是惯性测量单元IMU采集机器人在运动曲面上运动的第k帧的三轴角速度；

b^k-1是第k-1帧的惯性测量单元IMU的三轴角速度误差；

R_k-1是机器人在第k-1帧空间惯性坐标系下的旋转矩阵；

Δt是第k帧与第k-1帧的时间差。

3.根据权利要求2所述的机器人曲面运动定位方法，其特征在于：构造第k帧特征点在第k-1帧中的重投影误差的非线性方程修正航向角α_k和平移向量

如式(3)：

其中，

分别是特征点在第k帧和第k-1帧的三维坐标；

α_k是机器人航向角；

R_k(α_k)是从第k-1帧到到第k帧的旋转矩阵。

4.根据权利要求3所述的机器人曲面运动定位方法，其特征在于：所述的惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)计算步骤如下：

首先，采用式(4)根据惯性测量单元IMU输出值计算出从k-m帧到第k帧的旋转矩阵

其中，J_r(Ω^jΔt)是计算Ω^jΔt在李群上的右雅克比矩阵；

k是当前帧；

Ω^j是惯性测量单元IMU采集的机器人在运动曲面上运动的第j帧的三轴角速度(ω_x,ω_y,ω_z)；

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

R_k-m是机器人在第k-m帧时对应的空间惯性坐标系下的旋转矩阵；

然后采用式(5)根据图像特征匹配计算出从k-m帧到第k帧的旋转矩阵

是空间惯性坐标系下机器人在第j帧的旋转矩阵；

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

利用式(4)、式(5)，对惯性测量单元IMU的三轴角速度误差b^k构造非线性方程如式(6)所示，

通过高斯牛顿法迭代求解式(6)得到最优b^k。

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

步骤6)中，对修正后的平移向量进行优化的方法包括：

在机器人坐标系中设垂直运动曲面向上方向单位向量

根据步骤4)中获取的第k帧的欧拉角和平移向量初步估计机器人位置P_t，在运动曲面Σ上选取P_t附近离散点，拟合出P_t点的切平面，并求出对应的法向量

残差r₁计算如式(7)：

其中R_k是机器人在第k帧空间惯性坐标系下的旋转矩阵；

是机器人坐标系中垂直运动曲面向上方向的单位向量；

计算机器人位置坐标到运动曲面距离r₂，计算公式如式(8)：

其中P_t表示机器人位置计算值，M_t表示步骤5)中机器人运动曲面点云上与机器人计算位置最近的一点，

是待优化的机器人平移向量；

对平移向量进行优化构造的非线性方程如式(9)：

通过高斯牛顿法迭代求解机器人每两帧图像间的平移向量最优值。

6.根据权利要求1所述的机器人曲面运动定位方法，其特征在于：所述的惯性测量单元IMU包括陀螺仪和三轴加速度计。

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

所述物体表面的曲率半径大于等于5米。

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

利用重力加速度在加速度计三轴方向的分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k，具体方法是先将步骤3)旋转矩阵

转换成欧拉角表示方式，将横滚角更新为β_k，将俯仰角更新为γ_k，再将欧拉角转换成新的旋转矩阵R_k。

9.一种机器人曲面运动定位***，其特征在于，采用权利要求1至8中任一项所述的机器人曲面运动定位方法进行机器人曲面运动定位，该运动定位***包括：RGB-D深度相机、惯性测量单元IMU、爬壁机器人、坐标系构建模块、配准模块、旋转矩阵和平移向量获取模块、旋转矩阵和平移向量修正模块、RGB-D深度图像拼接模块、平移向量优化模块、相似度比较模块，其中：

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

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

坐标系构建模块用于构建空间惯性坐标系、RGB-D相机坐标系、惯性测量单元IMU坐标系、机器人坐标系；

配准模块用于对RGB-D相机的彩色图像与深度图像进行配准；

旋转矩阵和平移向量获取模块用于在步骤2)中采集的彩色图像中利用ORB特征提取方法提取特征点，采用BRIEF算法计算描述子，利用最近邻搜索方法完成特征匹配，计算出空间惯性坐标系下机器人的旋转矩阵

和平移向量

旋转矩阵和平移向量修正模块用于在每两帧彩色图像之间，根据第k-1帧图像对应的欧拉角(α_k-1,β_k-1,γ_k-1)和惯性测量单元IMU输出值计算第k帧的欧拉角

将惯性测量单元IMU三轴加速度计输出值作为重力加速度在三轴上的分量A^k(a_x,a_y,a_z)，并利用所述分量A^k(a_x,a_y,a_z)更新横滚角β_k和俯仰角γ_k从而修正第k帧的欧拉角，利用更新后的横滚角β_k和俯仰角γ_k修正步骤3)旋转矩阵

再计算上一帧特征点在第k帧中的重投影误差，利用最小二乘法修正航向角α_k和平移向量

根据修正后的第k帧的欧拉角和平移向量计算机器人的三维坐标P_t，利用最小二乘法更新惯性测量单元IMU的三轴角速度误差b^k(b_x,b_y,b_z)，并将三轴角速度误差b^k(b_x,b_y,b_z)返回给惯性测量单元IMU以修正惯性测量单元IMU的测量值；

RGB-D深度图像拼接模块利用步骤4)修正后的旋转矩阵和平移向量对RGB-D深度图像进行拼接，生成机器人运动曲面点云，对曲面点云进行体素栅格滤波；

平移向量优化模块用于根据机器人姿态应与机器人运动曲面适应的原则，结合机器人在曲面上的运动约束构造非线性方程对修正后的平移向量进行优化，得到机器人在第k帧的平移向量

相似度比较模块用于执行闭环检测，对比第k帧中特征点的BRIEF描述子与BoW词袋模型中描述子，当相似度大于设定值就认定为相似，根据BoW词袋模型中的描述子对应的特征点三维坐标更新机器人位姿信息，否则将第k帧中特征点加入BoW词袋模型中。

Images