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

权利要求书

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帧的时间差。