服务机器人导航路径跟踪方法及服务机器人与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

其中(xw,yw,zw)为点p在图像坐标系下的坐标，(z*u，z*v)为点p在世界坐标系下的坐标；o^t为0向量；

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

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

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

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

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

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

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

(2.2)采用ca-cfar2d算法对预处理后的地面图像检测目标与虚警，得到二值图；所述ca-cfar2d算法中采用十字形结构的检测模板；

(2.3)对二值图进行形态学处理得到图像中连通区域的结构，并去除离心率低的虚警噪声；

(2.4)对处理后的二值图采用canny边缘检测算法检测出导航路径上指引线的边缘，再使用hough变换检测出直线边缘；将检测出的直线线段端点作为导航路径上的特征点。

所述步骤(2.2)中采用ca-cfar2d算法对预处理后的地面图像检测目标与虚警，包括步骤：

(2.2.1)设计十字形结构的检测模板，所述检测模板为：

设检测单元的坐标为(p,q)，将横坐标在[p-p,p+p]范围、同时纵坐标在[q-p,q+p]范围的单元去掉检测单元后的区域设置为保护单元；将横坐标在[p-p,p+p]范围、同时纵坐标在[q-p-r2,q-p]∪[q+p,q+p+r2]范围的单元设置为上下参考单元；将横坐标在[p-p-r1,p-p]∪[p+p,p+p+r1]范围、同时纵坐标在[q-p,q+p]范围的单元设置为左右参考单元；其中p、r1、r2分别为保护单元参数、左右参考单元参数和上下参考单元参数，且p、r1、r2均为正整数；

(2.2.2)计算预处理后的地面图像的灰度值累加矩阵ga，所述灰度值累加矩阵ga为h×w维矩阵，w和h分别为地面图像的宽和高；ga第j行i列元素gai,j与地面图像(i,j)处的像素对应，其中xu,v为预处理后的地面图像在(u,v)处的像素灰度值，i＝1,2,…,w，j＝1,2,…,h；

(2.2.3)计算预处理后的地面图像中每个像素点处左右检测窗口内像素灰度累加值

其中，保护距离dp＝p，左右检测窗口的测试距离为dr1＝r1+p；

计算预处理后的地面图像中每个像素点处上下检测窗口内像素灰度累加值

其中上下检测窗口的测试距离为dr2＝r2+p；

计算预处理后的地面图像中每个像素点处保护单元内像素灰度累加值pai,j：

计算预处理后的地面图像中每个像素点处左右参考单元灰度平均值

计算预处理后的地面图像中每个像素点处上下参考单元灰度平均值

(2.2.4)计算预处理后的地面图像中每个像素点处的检测阈值ti,j、乘法器系数αi,j；

(2.2.5)对于预处理后的地面图像中每一个像素，比较灰度值xi,j和检测阈值ti,j、检测比值rati,j和乘法器系数αi,j的值，

如果xi,j<ti,j或rati,j<αi,j，该像素(i,j)设置为保留像素；像中保留像素之外的像素都为背景；

(2.2.6)计算预处理后地面图像中保留像素的灰度平均值比较保留像素的灰度值与的大小，如果小于该保留像素为目标，否则为背景。

步骤2.2.4中检测阈值ti,j和乘法器系数αi,j的计算步骤包括：

(s1)计算像素(i,j)处的乘法器系数αi,j：

其中pfa为预设的虚警概率常数；rci,j为以像素(i,j)为检测单元时参考单元的个数；

(s2)估计像素(i,j)处检测阈值

其中为以像素(i,j)为检测单元时第l个参考单元的灰度值；将作为像素(i,j)处的检测阈值ti,j。

本发明采用高斯滤波对图像进行去噪。

步骤(2)中判断导航路径是否满足直线判断条件，包括如下步骤：

统计检测到的导航路径上的每个直线线段与横坐标轴的夹角θ，计算所有直线线段与横坐标轴的夹角的方差σθ；如果σθ<tθ，认为检测到的导航路径为直线，即满足直线判断条件；否则，认为不满足直线判断条件；其中tθ为预设的线段夹角方差阈值。

另一方面，本发明公开了一种服务机器人，包括：机器人本体1，以及安装于机器人本体1上的摄像头2、惯性导航系统3、导航路径检测模块6、主控模块7、电机8和方向控制器9；

所述摄像头2用于获取服务机器人前方原始地面图像并发送给导航路径检测模块6，所述原始地面图像包括导航路径；所述惯性导航系统3获取服务机器人的位置和前进方向并发送给导航路径检测模块6；所述导航路径检测模块6计算服务机器人的位置与导航直线路径的距离d和服务机器人前进方向与导航直线路径的夹角ω；所述主控模块7将夹角ω发送至方向控制器9，方向控制器9控制服务机器人的前进方向，使夹角ω和距离d减小；所述电机8用于驱动服务机器人的车轮；

导航路径检测模块6采用上述的服务机器人导航路径跟踪方法来计算服务机器人的位置与导航直线路径的距离d和服务机器人前进方向与导航直线路径的夹角ω。

为了避免光照的影响，提高导航精度，还包括设置于机器人本体1上的光敏传感器4和照明灯5；所述光敏传感器4和照明灯5的开关均与主控模块7连接，当光敏传感器4检测到环境光照强度小于阈值时，主控模块7控制照明灯的开关导通，开启照明灯；当光敏传感器4检测到环境光照强度大于阈值时，主控模块7控制照明灯的开关关断，关闭照明灯。

本发明中主控模块7采用stm32控制板。

有益效果：与现有技术相比，本发明公开的服务机器人导航路径跟踪方法及服务机器人采用成本低廉的摄像头作为视觉传感器，利用图像处理来检测导航路径，能够快速精确地获取导航直线路径，从而实现精确地跟踪导航路径。

附图说明

图1为服务机器人的组成结构示意图；

图2为服务机器人导航路径跟踪方法的流程图；

图3为检测地面图像中导航路径的流程图；

图4为采用ca-cfar2d算法检测地面图像中目标与背景的流程图；

图5为十字形结构的检测模板图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

本发明公开了服务机器人导航路径跟踪方法及应用此方法的服务机器人，如图1所示，为服务机器人的组成示意图。包括：机器人本体1，以及安装于机器人本体1上的摄像头2、惯性导航系统3、导航路径检测模块6、主控模块7、电机8和方向控制器9；其中摄像头2用于获取服务机器人前方原始地面图像并发送给导航路径检测模块6，所述原始地面图像包括导航路径，导航路径为设置在地面的指引线；所述惯性导航系统3获取服务机器人在世界坐标系下的位置和前进方向并发送给导航路径检测模块6；所述导航路径检测模块6计算服务机器人的位置与导航直线路径的距离d和服务机器人前进方向与导航直线路径的夹角ω；所述主控模块7将夹角ω发送至方向控制器9，方向控制器9控制服务机器人的前进方向，使夹角ω和距离d减小；所述电机8用于驱动服务机器人的车轮。为了避免光照的影响，提高导航精度，还包括设置于机器人本体1上的光敏传感器4和照明灯5；所述光敏传感器4和照明灯5的开关均与主控模块7连接，当光敏传感器4检测到环境光照强度小于阈值时，主控模块7控制照明灯的开关导通，开启照明灯；当光敏传感器4检测到环境光照强度大于阈值时，主控模块7控制照明灯的开关关断，关闭照明灯。

本发明中主控模块7采用stm32控制板。

如图2所示，上述服务机器人跟踪导航路径，包括如下步骤：

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

其中(xw,yw,zw)为点p在图像坐标系下的坐标，(z*u,z*v)为点p在世界坐标系下的坐标；o^t为0向量；

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

如图3所示，检测导航路径包括如下步骤：

(2.1)对原始地面图像进行预处理，包括彩色图像转换为灰度图像、图像去噪；本实施例中采用高斯滤波对图像进行去噪；

(2.2)采用ca-cfar2d算法对预处理后的地面图像检测目标与虚警，得到二值图；所述ca-cfar2d算法中采用十字形结构的检测模板；

ca-cfar2d(cellaverage–constantfalsealarmratein2-d，二维平均恒虚警率)算法是一种自适应阈值检测算法，其常规的检测结构是一个正方形结构。由于上下左右的像素都会对阈值产生影响，所以其适用于点目标。直线是典型的线目标，沿着前进方向的目标灰度值较低，两边的背景灰度值较高。如果使用正方形检测结构，目标灰度值将会过多的参与计算，使阈值过低影响结果。本发明采用十字形结构的检测模板来检测目标与虚警，如图4所示，包括步骤：

(2.2.1)设计十字形结构的检测模板，如图5所示，检测模板为：

设检测单元的坐标为(p,q)，将横坐标在[p-p,p+p]范围、同时纵坐标在[q-p,q+p]范围的单元去掉检测单元后的区域设置为保护单元，如图3中斜线填充部分所示；将横坐标在[p-p,p+p]范围、同时纵坐标在[q-p-r2,q-p]∪[q+p,q+p+r2]范围的单元设置为上下参考单元，如图3中灰色填充部分所示；将横坐标在[p-p-r1,p-p]∪[p+p,p+p+r1]范围、同时纵坐标在[q-p,q+p]范围的单元设置为左右参考单元；其中p、r1、r2分别为保护单元参数、左右参考单元参数和上下参考单元参数，且p、r1、r2均为正整数；本实施例中，p＝1、r1＝r2＝2。

(2.2.2)计算预处理后的地面图像的灰度值累加矩阵ga，所述灰度值累加矩阵ga为h×w维矩阵，w和h分别为地面图像的宽和高；ga第j行i列元素gai,j与地面图像(i,j)处的像素对应，其中xu,v为预处理后的地面图像在(u,v)处的像素灰度值，i＝1,2,…,w，j＝1,2,…,h；

(2.2.3)计算预处理后的地面图像中每个像素点处左右检测窗口内像素灰度累加值

其中，保护距离dp＝p，左右检测窗口的测试距离为dr1＝r1+p；

上式中，等号右侧相加的4项下标为左右检测窗口在灰度值累加矩阵上的4个顶点，其中：i+dr1,j+dr1是左右检测窗口的右下角空间坐标，表示此坐标处的灰度值累加值；i-dr1-1,j+dr1是左下角空间坐标，需要从总累加值中减去；i+dr1,j-dr1-1是右上角空间坐标，需要从总累加值中减去；i-dr1-1,j-dr1-1是左上角空间坐标，必须加到累加的总数中，因为第二个和第三个项减去了两次，必须加一次。

计算预处理后的地面图像中每个像素点处上下检测窗口内像素灰度累加值

其中上下检测窗口的测试距离为dr2＝r2+p；

计算预处理后的地面图像中每个像素点处保护单元内像素灰度累加值pai,j：

计算预处理后的地面图像中每个像素点处左右参考单元灰度平均值

计算预处理后的地面图像中每个像素点处上下参考单元灰度平均值

(2.2.4)计算预处理后的地面图像中每个像素点处的检测阈值ti,j、乘法器系数αi,j；

检测阈值ti,j和乘法器系数αi,j的计算步骤包括：

(s1)计算像素(i,j)处的乘法器系数αi,j：

其中pfa为预设的虚警概率常数；rci,j为以像素(i,j)为检测单元时参考单元的个数；

(s2)估计像素(i,j)处检测阈值

其中为以像素(i,j)为检测单元时第l个参考单元的灰度值；

将作为像素(i,j)处的检测阈值ti,j。

(2.2.5)对于预处理后的地面图像中每一个像素，比较灰度值xi,j和检测阈值ti,j、检测比值rati,j和乘法器系数αi,j的值，

如果xi,j<ti,j或rati,j<αi,j，该像素(i,j)设置为保留像素；像中保留像素之外的像素都为背景；

(2.2.6)计算预处理后地面图像中保留像素的灰度平均值比较保留像素的灰度值与的大小，如果小于该保留像素为目标，否则为背景。

(2.3)对二值图进行形态学处理得到图像中连通区域的结构，并去除离心率低的虚警噪声；

形态学处理是通过在图像上应用结构元素来产生输出图像。腐蚀和膨胀是形态学处理的两个基础操作。膨胀就是求局部最大值的操作。结构元素b与图像a卷积，即计算核b覆盖区域的像素点的最大值，并把这个最大值赋给参考点指定的像素。腐蚀与膨胀相反，腐蚀就是求局部最小值的操作。结构元素b与图像a卷积，即计算b覆盖的区域的像素点的最小值，并把这个最小值赋给参考点指定的像素。膨胀会扩大图像的组成部分，腐蚀会缩小图像中的组成部分。使用先腐蚀后膨胀的开运算可以平滑目标的轮廓。

由于直线的离心率较高，因此去除离心率低的部分起到去噪的效果。

(2.4)对处理后的二值图采用canny边缘检测算法检测出导航路径上指引线的边缘，再使用hough变换检测出直线边缘；将检测出的直线线段端点作为导航路径上的特征点。

判断导航路径是否满足直线判断条件，包括如下步骤：

统计检测到的导航路径上的每个直线线段与横坐标轴的夹角θ，计算所有直线线段与横坐标轴的夹角的方差σθ；如果σθ<tθ，认为检测到的导航路径为直线，即满足直线判断条件；否则，认为不满足直线判断条件；其中tθ为预设的线段夹角方差阈值；

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

设直线方程为：f＝ax+b-y＝0，直线拟合即找到参数a，b值，使f趋近于0，即求如下方程的最小值：

f(a,b)＝∑(axk+b-yk)²

其中(xk,yk)为直线上的点，本实施例中为检测得到的导航路径上的特征点；

对其求偏导可得：

化简得：

a∑xk+b×s＝∑yk

通过计算s个特征点(xk,yk)的∑(xk×yk)、∑yk、∑xk，得到关于a，b的二元方程，求解即得到a，b的值。其中，s是特征点(xk,yk)的个数，即k＝1,2,…,s。

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

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

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