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

权利要求书

1.服务机器人导航路径跟踪方法，所述服务机器人带有摄像头和惯性导航系统，所述摄像头获取服务机器人前方原始地面图像，所述原始地面图像包括导航路径；所述惯性导航系统获取服务机器人的位置和前进方向；所述导航路径为设置在地面的指引线；其特征在于，包括如下步骤：

(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)对原始地面图像进行预处理，包括彩色图像转换为灰度图像、图像去噪；

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

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

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

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

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

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

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

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

其中，保护距离d_p＝P，左右检测窗口的测试距离为d_r1＝R₁+P；

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

其中上下检测窗口的测试距离为d_r2＝R₂+P；

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

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

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

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

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

如果X_i,jT_i,j或Rat_i,jα_i,j，该像素(i,j)设置为保留像素；图像中保留像素之外的像素都为背景；

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