[发明专利]一种融合RGBD深度传感器与编码器的机器人定位方法

权利要求书

1.一种融合RGBD深度传感器与编码器的机器人定位方法，其特征在于，包括以下步骤：

S1：在机器人运动区域内，确定机器人起点位置，将起点位置作为原点建立世界坐标系，然后在起点及关键位置选择或配置路标，令机器人运动至路标附近时采集路标图像，并记录下对应的位置坐标T_m，m＝1,2,…,M，M表示路标数量，从而建立得到路标数据库；

S2：机器人从起点位置开始运动，运动过程中基于编码器定位得到机器人当前位置坐标P(t)＝(P(t)_x P(t)_y P(t)_θ)^T，并将位置坐标P(t)转换得到位姿矩阵T_we(t)；

S3：机器人在运动过程中，持续采用RGBD深度传感器采集图像，与路标数据库中的路标图像进行匹配，如果没有路标图像匹配，不作任何操作，基于RGBD深度传感器定位失败，否则计算当前图像与最匹配路标图像间的相对变换矩阵T_t^*，在路标数据库中查询得到最匹配路标图像对应的机器人位置坐标T_v，得到基于RGBD深度传感器定位的位置坐标P′(t)＝(P′(t)_x P′(t)_y P′(t)_θ)^T＝T_v*T_t^*，将位置坐标P′(t)转换得到位姿矩阵T_wv(t)；

S4：如果步骤S3中的基于RGBD深度传感器定位成功，进入步骤S5，否则进入步骤S6；

S5：根据当前时刻基于编码器定位得到的机器人位姿矩阵T_we(t)和基于RGBD深度传感器定位得到的位姿矩阵T_wv(t)，计算得到变换矩阵DT＝[T_we(t)]^-1*T_wv(t)，对原有变换矩阵进行更新，并令机器人当前位姿矩阵T(t)＝T_wv(t)；

S6：通过变换矩阵DT来对位姿矩阵T_we(t)进行定位误差矫正，得到机器人当前位姿矩阵T(t)＝T_we(t)*DT。

2.根据权利要求1所述的机器人定位方法，其特征在于，所述步骤S3中当前图像与路标数据库中的路标图像进行匹配的方法为：对于路标数据库中每个路标图像提取ORB特征，根据提取得到的ORB特征建立得到ORB词典；对于机器人在运动过程中采集到的图像，首先提取ORB特征，利用ORB词典生成相应词汇，以词汇作为关键词在路标数据库中搜索，统计每个路标图像的搜索命中次数，当某幅路标图像的搜索命中次数与当前图像的ORB特征总数比值超过预设阈值，将该幅路标图像作为候选匹配图像；如果只有一幅候选匹配图像，则将该图像作为当前图像的最匹配路标图像，否则分别将候选匹配图像的特征点重映射到当前图像，并计算重映射误差，选择最小重映射误差所对应的候选匹配图像作为当前图像的最匹配路标图像。

3.根据权利要求1所述的机器人定位方法，其特征在于，所述步骤S3中当前图像与最匹配路标图像间的相对变换矩阵的计算方法采用Bundle Adjustment算法。