[发明专利]服务机器人导航路径跟踪方法及服务机器人

说明书

本发明公开了一种服务机器人导航路径跟踪方法和应用该方法的服务机器人，其中导航路径跟踪方法包括：1、获取服务机器人前方原始地面图像；获取机器人在图像坐标系下的位置和前进方向；2、检测导航路径上的特征点；判断该导航路径是否满足直线判断条件；3、如果满足，对检测的特征点做直线拟合得到导航直线路径；计算机器人与导航直线路径的距离d，前进方向与导航直线路径的夹角ω；4、如果不满足，对检测出的导航路径分段，检测最靠近机器人的子路径是否满足直线判断条件并按照步骤3来计算距离d和夹角ω；5、控制机器人前进方向，使d和ω减小；6、机器人不断前进，重复步骤2‑5，不断地跟踪导航路径。该方法在使用较低成本的设备下，能够得到较高精度的导航性能。

技术领域

本发明属于机器人视觉导航技术领域，具体涉及一种服务机器人的导航路径跟踪方法及应用此方法的服务机器人。

背景技术

随着技术的发展，服务机器人技术取得了飞速的发展，在工业生产中发挥着越来越重要的作用，被广泛用于完成重复性高、危险性大和精度要求高的工作。人类希望机器人可以更好地服务于人类甚至代替人类完成多种多样的工作。这需要机器人不仅有完成任务的能力，还要有根据要求自主前往任务地点的能力，也就是服务机器人自主导航技术。

服务机器人常用的导航方式分为雷达导航、惯性导航、卫星导航、视觉导航。其中雷达导航规避障碍准确，但价格较高；惯性导航灵活性强，但随着路程增加，误差会累积；卫星导航适合远距离导航，但定位精度低，无法进行室内导航；视觉导航价格较低，但对障碍物探测和路径规划需要配合对应的算法。通常情况下，服务机器人的运行路线固定，通过设置指引线来实现路径跟踪。

发明内容

发明目的：本发明旨在提供一种成本低、导航精度高的服务机器人导航路径跟踪方法。

技术方案：本发明一方面公开了一种服务机器人导航路径跟踪方法，所述服务机器人带有摄像头和惯性导航系统，所述摄像头获取服务机器人前方原始地面图像，所述原始地面图像包括导航路径；所述惯性导航系统获取服务机器人的位置和前进方向；所述导航路径为设置在地面的指引线；包括如下步骤：

(1)对摄像头进行标定，获取内外参数矩阵K、R和t；得到图像坐标系下坐标与世界坐标系的转换关系：

其中(X_w,Y_w,Z_w)为点p在图像坐标系下的坐标，(Z*u,Z*v)为点p在世界坐标系下的坐标；O^T为0向量；

(2)根据原始地面图像检测导航路径上的特征点；判断原始地面图像中的导航路径是否满足直线判断条件；

(3)如果满足，对检测得到的导航路径上的特征点做直线拟合，得到导航直线路径；将惯性导航获取的服务机器人在世界坐标系下的位置转换到图像坐标系中，计算图像坐标系中服务机器人的位置与导航直线路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；

(4)如果不满足，对检测出的导航路径分段，分为N_path个子路径；计算最靠近服务机器人的子路径是否满足直线判断条件，如果满足，对所述子路径做直线拟合，得到导航直线子路径，计算图像坐标系中服务机器人的位置与所述导航直线子路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；

(5)控制服务机器人的前进方向，使距离d和夹角ω减小；

(6)服务机器人不断前进，根据最新获取到的原始地面图像，重复步骤2-5，不断地跟踪导航路径。

步骤(2)中根据原始地面图像检测导航路径，包括：

(2.1)对原始地面图像进行预处理，包括彩色图像转换为灰度图像、图像去噪；