一种融合RGBD深度传感器与编码器的机器人定位方法技术领域
本发明属于机器人定位技术领域，更为具体地讲，涉及一种融合RGBD深度传感器
与编码器的机器人定位方法。
背景技术
机器人定位技术是机器人导航领域中关键技术，根据所用传感器的不同，可将定
位技术分为两大类，一种是传统定位，比如利用编码器、激光雷达、WiFi、蓝牙等定位方法，
另一种是视觉定位，比如单目视觉定位，双目视觉定位以及基于RGBD相机定位。
传统定位算法目前已经比较成熟，其中基于编码器的定位算法是一种自主定位，
在比较平坦的地面定位精度较高，一旦机器人发生滑动，这种定位会出现较大误差，而且无
法消除定位累积误差。基于激光雷达定位鲁棒性很高，目前在室外定位精度很差，而且计算
量很大，传感器成本也较高。另外，基于WiFi或者蓝牙定位精度很低，误差在1m左右。
视觉定位是目前比较流行的机器人定位方法，近年来也出现了不少优秀的即时定
位和建图(SLAM)系统，比如ORB-SLAM，LSD-SLAM，ELASTICFUSION等，这些基于视觉的定位系
统都是大同小异，主要还是提取图像特征，然后特征匹配，接着利用BA、ICP等算法把机器人
位姿矩阵求出来，最后做闭环检测，减少累计误差。这种基于视觉的定位方法对场景要求比
较严，当场景中的特征比较少，或者出现动态物体时，定位误差比较大。
就目前的定位技术而言，无论是传统定位还是视觉定位，定位精度或鲁棒性都比
较差，很难在实际环境中使用。
发明内容
本发明的目的在于克服现有技术的不足，提供一种融合RGBD深度传感器与编码器
的机器人定位方法，基于RGBD深度传感器定位对编码器定位误差进行矫正，提高机器人定
位精度和鲁棒性。
为实现上述发明目的，本发明融合RGBD深度传感器与编码器的机器人定位方法包
括以下步骤：
S1：在机器人运动区域内，确定机器人起点位置，将起点位置作为原点建立世界坐
标系，然后在起点及关键位置选择或配置路标，令机器人运动至路标附近时采集路标图像，
并记录下对应的位置坐标Tm，m＝1,2,…,M，M表示路标数量，从而建立得到路标数据库；
S2：机器人从起点位置开始运动，运动过程中基于编码器定位得到机器人当前位
置坐标P(t)＝(P(t)x P(t)y P(t)θ)T，并将位置坐标P(t)转换得到位姿矩阵Twe(t)；
S3：机器人在运动过程中，持续采用RGBD深度传感器采集图像，与路标数据库中的
路标图像进行匹配，如果没有路标图像匹配，不作任何操作，基于RGBD深度传感器定位失
败，否则计算当前图像与最匹配路标图像间的相对变换矩阵Tt*，在路标数据库中查询得到
最匹配路标图像对应的机器人位置坐标Tv，得到基于RGBD深度传感器定位的位置坐标P′
(t)＝(P′(t)x P′(t)y P′(t)θ)T＝Tv*Tt*，将位置坐标P′(t)转换得到位姿矩阵Twv(t)；
S4：如果步骤S3中的基于RGBD深度传感器定位成功，进入步骤S5，否则进入步骤
S6；
S5：根据当前时刻基于编码器定位得到的机器人位姿矩阵Twe(t)和基于RGBD深度
传感器定位得到的位姿矩阵Twv(t)，计算得到变换矩阵DT＝[Twe(t)]-1*Twv(t)，对原有变换
矩阵进行更新，并令机器人当前位姿矩阵T(t)＝Twv(t)；
S6：通过变换矩阵DT来对位姿矩阵Twe(t)进行定位误差矫正，得到机器人当前位姿
矩阵T(t)＝Twe(t)*DT。
本发明融合RGBD深度传感器与编码器的机器人定位方法，首先建立得到机器人运
动区域内的路标数据库，路标数据库中包括路标图像和对应的机器人位置坐标，机器人运
动过程中基于编码器定位得到机器人位姿矩阵Twe(t)，同时基于RGBD深度传感器定位，如果
基于RGBD深度传感器定位成功，则根据当前时刻基于编码器定位得到的机器人位姿矩阵Twe
(t)和基于RGBD深度传感器定位得到的位姿矩阵Twv(t)计算变换矩阵，并令机器人当前位姿
矩阵为Twv(t)，如果不成功则通过变换矩阵对Twe(t)进行定位误差矫正，得到机器人当前位
姿矩阵。
本发明具有以下技术效果：
1)本发明采用的编码器和RGBD深度传感器具有低成本优势，易于推广应用；
2)本发明定位的计算量要远小于纯粹的视觉定位算法，对机器人硬件配置要求较
低；
3)本发明的定位精度和鲁棒性较高，可以在动态和特征少的场景中使用，在室内
环境中能够实现高精度的定位。
附图说明
图1是本发明融合RGBD深度传感器与编码器的机器人定位方法的具体实施方式流
程图；
图2是本实施例中机器人运动区域和路标地点示意图；
图3是本实施例中所采用的机器人图像；
图4是图3所示机器人基于编码器定位的示意图。
具体实施方式
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地
理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许
会淡化本发明的主要内容时，这些描述在这里将被忽略。
实施例
图1是本发明融合RGBD深度传感器与编码器的机器人定位方法的具体实施方式流
程图。如图1所示，本发明融合RGBD深度传感器与编码器的机器人定位方法的具体步骤包
括：
S101：建立路标数据库：
在机器人运动区域内，确定机器人起点位置，将起点位置作为原点建立世界坐标
系，然后在起点及关键位置选择或配置路标，路标一般采用纹理丰富且清晰的图片或者二
维码来制作，令机器人运动至路标附近时采集路标图像，并记录下对应的位置坐标Tm，m＝
1,2,…,M，M表示路标数量，从而建立得到路标数据库。路标数据库用于为机器人视觉定位
提供参照。
为了使路标数据库中的信息更加可靠有效，路标地点最好按照以下要求进行选
择：路标配置在视角阔的墙面上，且两个相邻路标间的距离不能过大，按照目前的技术水
平，本实施例设置距离阈值为10m，即当一个路标地点固定之后，下一个路标点需要在上一
个路标为中心，10m为半径的圆内。图2是本实施例中机器人运动区域和路标地点示意图。如
图2所示，本实施例中机器人运动区域为室内区域，包括一个客厅和一个卧室，在该运动区
域中设置了5个路标，分别在5个路标地点采集路标图像，并记录对应的位置坐标T1～T5，将
T1作为机器人起点位置。为了在视觉定位时进行有效的匹配，路标数据库中的路标图像一
般需要根据路标的特点提取出路标图像特征，本实施例中采用ORB(oriented FAST and
rotated BRIEF)特征。
S102：基于编码器定位：
机器人在从起点位置开始运动，运动过程中基于编码器定位得到机器人当前位置
坐标P(t)＝(P(t)x P(t)y P(t)θ)T，上标T表示转置，并将位置坐标P(t)转换得到位姿矩阵
Twe(t)。
图3是本实施例中所采用的机器人图像。如图3所示，本实施例中所采用的机器人
配置了左右两个运动轮，安装有增量式500线编码器，其视觉定位设备即RGBD深度传感器位
于顶部，采用xtion相机。基于编码器定位主要通过机器人底盘两个驱动轮的编码器数据，
建立机器人运动模型，并将机器人位置估计出来。图4是图3所示机器人基于编码器定位的
示意图。如图4所示，在t1时刻，记机器人坐标系与世界坐标系的夹角为α，机器人的右边轮
子角速度为ω1，左边轮子角速度为ω2，则此时机器人在世界坐标系下的速度ξw(t1)可以表
示为：
其中，r表示机器人运动轮半径，L表示机器人两个运动轮距离的一半，上标圆点表
示求导。
在Δt时间内，机器人位置变化量为：
那么机器人在时刻ti在世界坐标系下的位置为：
本实施例中机器人的位姿矩阵采用6自由度位姿矩阵，则根据位置坐标P(t)＝(P
(t)x P(t)y P(t)θ)T可以得到位姿矩阵Twe(t)为：
虽然基于编码器可以实现鲁棒性定位，由于编码器无法自主消除定位累积误差以
及机器人可能会发生滑动，本发明加入基于RGBD深度传感器定位，来矫正定位误差。
S103：基于RGBD深度传感器定位：
机器人在运动过程中，持续采用RGBD深度传感器采集得到图像，该图像中包括了
颜色信息RGB和深度信息D，与路标数据库中的路标图像进行匹配，如果没有路标图像匹配，
不作任何操作，基于RGBD深度传感器定位失败，否则计算当前图像与最匹配路标图像间的
相对变换矩阵Tt*，在路标数据库中查询得到最匹配路标图像对应的机器人位置坐标Tv，得
到基于RGBD深度传感器定位的位置坐标P′(t)＝(P′(t)x P′(t)y P′(t)θ)T＝Tv*Tt*，同样
地，将位置坐标P′(t)转换得到位姿矩阵Twv(t)。
由于本实施例中路标数据库中各路标图像提取了ORB特征，为了提高匹配效率，采
用Visual Search算法来进行当前图像与路标图像的匹配，其具体方法为：对于路标数据库
中每个路标图像提取ORB特征，根据提取得到的ORB特征建立得到ORB词典；对于机器人在运
动过程中采集到的图像，首先提取ORB特征，利用ORB词典生成相应词汇，以词汇作为关键词
在路标数据库中搜索，统计每个路标图像的搜索命中次数，当某幅路标图像的搜索命中次
数与当前图像的ORB特征总数比值超过预设阈值(通过实验得到该阈值为90％时效果较
好)，将该幅路标图像作为候选匹配图像；如果只有一幅候选匹配图像，则将该图像作为当
前图像的最匹配路标图像，否则分别将候选匹配图像的特征点重映射到当前图像，并计算
重映射误差，选择最小重映射误差所对应的候选匹配图像作为当前图像的最匹配路标图
像。
对于当前图像与最匹配路标图像间的相对变换矩阵Tt*的计算，目前机器人定位领
域已有多种算法，根据实际需要选择即可，本实施例中采用BundleAdjustment算法来计算，
Bundle Adjustment算法是本领域的常用算法，其具体过程在此不再赘述。
S104：判断基于RGBD深度传感器定位是否成功，如果成功则进入步骤S105，否则进
入步骤S106。
S105：更新变换矩阵：
根据当前时刻基于编码器定位得到的机器人位姿矩阵Twe(t)和基于RGBD深度传感
器定位得到的位姿矩阵Twv(t)，计算得到变换矩阵DT＝[Twe(t)]-1*Twv(t)，上标-1表示求逆，
对原有变换矩阵进行更新，并令机器人当前位姿矩阵T(t)＝Twv(t)，即以基于RGBD深度传感
器定位得到的位姿矩阵Twv(t)作为机器人当前位姿矩阵。
由于本发明在起点位置设置了路标图像，因此机器人从起点位置出发时即可完成
对变换矩阵DT的初始化，在此后运动过程中，每当在图像中匹配得到路标图像后，则计算一
次变换矩阵，对原有的变换矩阵进行替换。
S106：矫正定位误差：
当基于RGBD深度传感器定位失败，即当前只存在基于编码器定位得到的机器人位
姿矩阵Twe(t)，此时需要通过变换矩阵DT来对位姿矩阵Twe(t)进行定位误差矫正，得到机器
人当前位姿T(t)＝Twe(t)*DT。
根据以上描述可知，本发明融合RGBD深度传感器与编码器的机器人定位方法，通
过同时刻基于编码器定位得到的机器人位姿矩阵和基于RGBD深度传感器定位得到的位姿
矩阵来计算变换矩阵，对基于编码器定位得到的机器人位姿矩阵进行矫正，消除了基于编
码器定位的累计误差，可以有效提高机器人定位的精度。
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术
人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技
术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些
变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。