[发明专利]机器人多传感器融合感知与空间定位的方法、系统及装置

权利要求书

1.一种机器人多传感器融合感知与空间定位的方法，其特征在于，该方法包括：

步骤S10，获取机器人多传感器数据序列；所述机器人多传感器数据序列包括结构光深度相机采集的视觉数据序列、MARG传感器采集的运动状态数据序列和关节编码器采集的角位移数据序列；

步骤S20，标定与校准结构光深度相机、MARG传感器和关节编码器，并基于标定与校准结果对所述机器人多传感器数据序列进行修正；

步骤S30，通过预设的深度相机视觉增强方法对修正后的机器人多传感器数据序列中的视觉数据序列进行增强与修复；

步骤S40，进行增强与修复的视觉数据序列的预融合处理，获得第一预融合位姿序列；进行修正后的运动状态数据序列的预融合处理，获得预融合姿态序列；进行修正后的角位移数据序列的预融合处理，获得第二预融合位姿序列；

步骤S50，基于所述第一预融合位姿序列、第二预融合位姿序列和预融合姿态序列对机器人在环境中的空间状态进行融合定位，获得机器人在环境中的位姿序列及空间坐标序列。

2.根据权利要求1所述的机器人多传感器融合感知与空间定位的方法，其特征在于，所述视觉数据序列包括深度相机采集的彩色图序列和深度图序列；所述运动状态数据序列包括MARG传感器采集的三轴加速度序列、角速度序列和地磁场强度序列；所述角位移数据序列包括关节编码器采集的机器人关节的角位移数据序列。

3.根据权利要求2所述的机器人多传感器融合感知与空间定位的方法，其特征在于，标定与校准结构光深度相机、MARG传感器和关节编码器包括：

所述结构光深度相机的标定与校准包括彩色相机标定、红外相机标定、深度图漂移修正以及彩色图与深度图配准；

所述MARG传感器的标定与校准包括加速度零漂移校准以及磁场椭球拟合校准；

所述关节编码器的标定与校准包括编码器电气原点与关节旋转机构机械原点的配准。

4.根据权利要求2所述的机器人多传感器融合感知与空间定位的方法，其特征在于，所述增强与修复的视觉数据序列的预融合处理包括：

步骤S411，提取所述增强与修复的视觉数据序列中一帧彩色图的ORB二维特征点，并在与当前帧彩色图配准的深度图中提取所述ORB二维特征点对应的深度测量值；

步骤S412，若能提取深度测量值，则将该ORB二维特征点作为ORB三维特征点；否则，则仍作为ORB二维特征点；

步骤S413，将当前帧的所有特征点与当前帧的前一帧的所有的特征点进行匹配追踪，获得特征点对；

步骤S414，若所述特征点对中的特征点均为ORB三维特征点，则通过ICP算法得到两帧之间的位姿变换矩阵；否则，通过PnP算法得到两帧之间的位姿变换矩阵；

步骤S415，判断所述ORB三维特征点的追踪是否丢失，若未丢失，则直接将追踪得到的相机位姿变换矩阵作为新的关键帧加入已有的关键帧序列中；若已丢失，则调用当前帧对应的关节编码器和MARG传感器的预融合测量值作为新关键帧序列中初始关键帧的相机位姿变换矩阵；

步骤S416，将获取的关键帧序列进行位姿图优化，获得待优化相机位姿序列，并进行所述待优化相机位姿序列的闭环修正，获得第一预融合位姿序列。