CN110631577A - 服务机器人导航路径跟踪方法及服务机器人 - Google Patents

服务机器人导航路径跟踪方法及服务机器人 Download PDF

Info

Publication number
CN110631577A
CN110631577A CN201910885238.XA CN201910885238A CN110631577A CN 110631577 A CN110631577 A CN 110631577A CN 201910885238 A CN201910885238 A CN 201910885238A CN 110631577 A CN110631577 A CN 110631577A
Authority
CN
China
Prior art keywords
navigation
path
service robot
image
navigation path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201910885238.XA
Other languages
English (en)
Other versions
CN110631577B (zh
Inventor
马国军
丁安
朱琎
郑威
周大年
曾庆军
夏健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN201910885238.XA priority Critical patent/CN110631577B/zh
Publication of CN110631577A publication Critical patent/CN110631577A/zh
Application granted granted Critical
Publication of CN110631577B publication Critical patent/CN110631577B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

服务机器人导航路径跟踪方法及服务机器人
技术领域
本发明属于机器人视觉导航技术领域，具体涉及一种服务机器人的导航路径跟踪方法及应用此方法的服务机器人。
背景技术
随着技术的发展，服务机器人技术取得了飞速的发展，在工业生产中发挥着越来越重要的作用，被广泛用于完成重复性高、危险性大和精度要求高的工作。人类希望机器人可以更好地服务于人类甚至代替人类完成多种多样的工作。这需要机器人不仅有完成任务的能力，还要有根据要求自主前往任务地点的能力，也就是服务机器人自主导航技术。
服务机器人常用的导航方式分为雷达导航、惯性导航、卫星导航、视觉导航。其中雷达导航规避障碍准确，但价格较高；惯性导航灵活性强，但随着路程增加，误差会累积；卫星导航适合远距离导航，但定位精度低，无法进行室内导航；视觉导航价格较低，但对障碍物探测和路径规划需要配合对应的算法。通常情况下，服务机器人的运行路线固定，通过设置指引线来实现路径跟踪。
发明内容
发明目的：本发明旨在提供一种成本低、导航精度高的服务机器人导航路径跟踪方法。
技术方案：本发明一方面公开了一种服务机器人导航路径跟踪方法，所述服务机器人带有摄像头和惯性导航系统，所述摄像头获取服务机器人前方原始地面图像，所述原始地面图像包括导航路径；所述惯性导航系统获取服务机器人的位置和前进方向；所述导航路径为设置在地面的指引线；包括如下步骤：
(1)对摄像头进行标定，获取内外参数矩阵K、R和t；得到图像坐标系下坐标与世界坐标系的转换关系：
其中(Xw,Yw,Zw)为点p在图像坐标系下的坐标，(Z*u，Z*v)为点p在世界坐标系下的坐标；OT为0向量；
(2)根据原始地面图像检测导航路径上的特征点；判断原始地面图像中的导航路径是否满足直线判断条件；
(3)如果满足，对检测得到的导航路径上的特征点做直线拟合，得到导航直线路径；将惯性导航获取的服务机器人在世界坐标系下的位置转换到图像坐标系中，计算图像坐标系中服务机器人的位置与导航直线路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；
(4)如果不满足，对检测出的导航路径分段，分为Npath个子路径；计算最靠近服务机器人的子路径是否满足直线判断条件，如果满足，对所述子路径做直线拟合，得到导航直线子路径，计算图像坐标系中服务机器人的位置与所述导航直线子路径的距离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-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)处的像素对应，
Figure BDA0002207115250000031
其中Xu,v为预处理后的地面图像在(u,v)处的像素灰度值，i＝1,2,…,w，j＝1,2,…,h；
(2.2.3)计算预处理后的地面图像中每个像素点处左右检测窗口内像素灰度累加值
Figure BDA0002207115250000032
Figure BDA0002207115250000033
其中，保护距离dp＝P，左右检测窗口的测试距离为dr1＝R1+P；
计算预处理后的地面图像中每个像素点处上下检测窗口内像素灰度累加值
Figure BDA0002207115250000034
Figure BDA0002207115250000035
其中上下检测窗口的测试距离为dr2＝R2+P；
计算预处理后的地面图像中每个像素点处保护单元内像素灰度累加值Pai,j：
Figure BDA0002207115250000036
计算预处理后的地面图像中每个像素点处左右参考单元灰度平均值
Figure BDA0002207115250000037
Figure BDA0002207115250000038
计算预处理后的地面图像中每个像素点处上下参考单元灰度平均值
Figure BDA0002207115250000039
(2.2.4)计算预处理后的地面图像中每个像素点处的检测阈值Ti,j、乘法器系数αi,j；
(2.2.5)对于预处理后的地面图像中每一个像素，比较灰度值Xi,j和检测阈值Ti,j、检测比值Rati,j和乘法器系数αi,j的值，
Figure BDA0002207115250000041
如果Xi,j<Ti,j或Rati,j<αi,j，该像素(i,j)设置为保留像素；像中保留像素之外的像素都为背景；
(2.2.6)计算预处理后地面图像中保留像素的灰度平均值
Figure BDA0002207115250000042
比较保留像素的灰度值与
Figure BDA0002207115250000043
的大小，如果小于
Figure BDA0002207115250000044
该保留像素为目标，否则为背景。
步骤2.2.4中检测阈值Ti,j和乘法器系数αi,j的计算步骤包括：
(s1)计算像素(i,j)处的乘法器系数αi,j：
Figure BDA0002207115250000045
其中Pfa为预设的虚警概率常数；Rci,j为以像素(i,j)为检测单元时参考单元的个数；
(s2)估计像素(i,j)处检测阈值
Figure BDA0002207115250000046
Figure BDA0002207115250000047
其中
Figure BDA0002207115250000048
为以像素(i,j)为检测单元时第l个参考单元的灰度值；将
Figure BDA0002207115250000049
作为像素(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-CFAR 2D算法检测地面图像中目标与背景的流程图；
图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；得到图像坐标系下坐标与世界坐标系的转换关系：
Figure BDA0002207115250000061
其中(Xw,Yw,Zw)为点p在图像坐标系下的坐标，(Z*u,Z*v)为点p在世界坐标系下的坐标；OT为0向量；
步骤2、根据原始地面图像检测导航路径上的特征点；判断原始地面图像中的导航路径是否满足直线判断条件；
如图3所示，检测导航路径包括如下步骤：
(2.1)对原始地面图像进行预处理，包括彩色图像转换为灰度图像、图像去噪；本实施例中采用高斯滤波对图像进行去噪；
(2.2)采用CA-CFAR 2D算法对预处理后的地面图像检测目标与虚警，得到二值图；所述CA-CFAR 2D算法中采用十字形结构的检测模板；
CA-CFAR 2D(Cell Average–Constant False AlarmRate in 2-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)处的像素对应，
Figure BDA0002207115250000071
其中Xu,v为预处理后的地面图像在(u,v)处的像素灰度值，i＝1,2,…,w，j＝1,2,…,h；
(2.2.3)计算预处理后的地面图像中每个像素点处左右检测窗口内像素灰度累加值
Figure BDA0002207115250000072
Figure BDA0002207115250000073
其中，保护距离dp＝P，左右检测窗口的测试距离为dr1＝R1+P；
上式中，等号右侧相加的4项下标为左右检测窗口在灰度值累加矩阵上的4个顶点，其中：i+dr1,j+dr1是左右检测窗口的右下角空间坐标，
Figure BDA0002207115250000081
表示此坐标处的灰度值累加值；i-dr1-1,j+dr1是左下角空间坐标，
Figure BDA0002207115250000082
需要从总累加值中减去；i+dr1,j-dr1-1是右上角空间坐标，
Figure BDA0002207115250000083
需要从总累加值中减去；i-dr1-1,j-dr1-1是左上角空间坐标，
Figure BDA0002207115250000084
必须加到累加的总数中，因为第二个和第三个项减去了两次，必须加一次。
计算预处理后的地面图像中每个像素点处上下检测窗口内像素灰度累加值
Figure BDA0002207115250000085
Figure BDA0002207115250000086
其中上下检测窗口的测试距离为dr2＝R2+P；
计算预处理后的地面图像中每个像素点处保护单元内像素灰度累加值Pai,j：
Figure BDA0002207115250000087
计算预处理后的地面图像中每个像素点处左右参考单元灰度平均值
Figure BDA0002207115250000088
Figure BDA0002207115250000089
计算预处理后的地面图像中每个像素点处上下参考单元灰度平均值
Figure BDA00022071152500000810
Figure BDA00022071152500000811
(2.2.4)计算预处理后的地面图像中每个像素点处的检测阈值Ti,j、乘法器系数αi,j；
检测阈值Ti,j和乘法器系数αi,j的计算步骤包括：
(s1)计算像素(i,j)处的乘法器系数αi,j：
Figure BDA00022071152500000812
其中Pfa为预设的虚警概率常数；Rci,j为以像素(i,j)为检测单元时参考单元的个数；
(s2)估计像素(i,j)处检测阈值
Figure BDA0002207115250000091
Figure BDA0002207115250000092
其中
Figure BDA0002207115250000093
为以像素(i,j)为检测单元时第l个参考单元的灰度值；
将作为像素(i,j)处的检测阈值Ti,j。
(2.2.5)对于预处理后的地面图像中每一个像素，比较灰度值Xi,j和检测阈值Ti,j、检测比值Rati,j和乘法器系数αi,j的值，
Figure BDA0002207115250000095
如果Xi,j<Ti,j或Rati,j<αi,j，该像素(i,j)设置为保留像素；像中保留像素之外的像素都为背景；
(2.2.6)计算预处理后地面图像中保留像素的灰度平均值
Figure BDA0002207115250000096
比较保留像素的灰度值与
Figure BDA0002207115250000097
的大小，如果小于
Figure BDA0002207115250000098
该保留像素为目标，否则为背景。
(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)2
其中(xk,yk)为直线上的点，本实施例中为检测得到的导航路径上的特征点；
对其求偏导可得：
Figure BDA0002207115250000101
Figure BDA0002207115250000102
化简得：
Figure BDA0002207115250000103
a∑xk+b×S＝∑yk
通过计算S个特征点(xk,yk)的
Figure BDA0002207115250000104
∑(xk×yk)、∑yk、∑xk，得到关于a，b的二元方程，求解即得到a，b的值。其中，S是特征点(xk,yk)的个数，即k＝1,2,…,S。
步骤4、如果不满足，对检测出的导航路径分段，分为Npath个子路径；计算最靠近服务机器人的子路径是否满足直线判断条件，如果满足，对所述子路径做直线拟合，得到导航直线子路径，计算图像坐标系中服务机器人的位置与所述导航直线子路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；
步骤5、控制服务机器人的前进方向，使距离d和夹角ω减小；
步骤6、服务机器人不断前进，根据最新获取到的原始地面图像，重复步骤2-5，不断地跟踪导航路径。

Claims (9)

1.服务机器人导航路径跟踪方法，所述服务机器人带有摄像头和惯性导航系统，所述摄像头获取服务机器人前方原始地面图像，所述原始地面图像包括导航路径；所述惯性导航系统获取服务机器人的位置和前进方向；所述导航路径为设置在地面的指引线；其特征在于，包括如下步骤：
(1)对摄像头进行标定，获取内外参数矩阵K、R和t；得到图像坐标系下坐标与世界坐标系的转换关系：
Figure FDA0002207115240000011
其中(Xw,Yw,Zw)为点p在图像坐标系下的坐标，(Z*u,Z*v)为点p在世界坐标系下的坐标；OT为0向量；
(2)根据原始地面图像检测导航路径上的特征点；判断原始地面图像中的导航路径是否满足直线判断条件；
(3)如果满足，对检测得到的导航路径上的特征点做直线拟合，得到导航直线路径；将惯性导航获取的服务机器人在世界坐标系下的位置转换到图像坐标系中，计算图像坐标系中服务机器人的位置与导航直线路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；
(4)如果不满足，对检测出的导航路径分段，分为Npath个子路径；计算最靠近服务机器人的子路径是否满足直线判断条件，如果满足，对所述子路径做直线拟合，得到导航直线子路径，计算图像坐标系中服务机器人的位置与所述导航直线子路径的距离d，以及服务机器人前进方向与导航直线路径的夹角ω；
(5)控制服务机器人的前进方向，使距离d和夹角ω减小；
(6)服务机器人不断前进，根据最新获取到的原始地面图像，重复步骤2-5，不断地跟踪导航路径。
2.根据权利要求1所述的服务机器人导航路径跟踪方法，其特征在于，所述步骤(2)中根据原始地面图像检测导航路径，包括：
(2.1)对原始地面图像进行预处理，包括彩色图像转换为灰度图像、图像去噪；
(2.2)采用CA-CFAR 2D算法对预处理后的地面图像检测目标与虚警，得到二值图；所述CA-CFAR 2D算法中采用十字形结构的检测模板；
(2.3)对二值图进行形态学处理得到图像中连通区域的结构，并去除离心率低的虚警噪声；
(2.4)对处理后的二值图采用Canny边缘检测算法检测出导航路径上指引线的边缘，再使用Hough变换检测出直线边缘；将检测出的直线线段端点作为导航路径上的特征点。
3.根据权利要求2所述的服务机器人导航路径跟踪方法，其特征在于，所述步骤(2.2)中采用CA-CFAR 2D算法对预处理后的地面图像检测目标与虚警，包括步骤：
(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)处的像素对应，
Figure FDA0002207115240000021
其中Xu,v为预处理后的地面图像在(u,v)处的像素灰度值，i＝1,2,…,w，j＝1,2,…,h；
(2.2.3)计算预处理后的地面图像中每个像素点处左右检测窗口内像素灰度累加值
Figure FDA0002207115240000022
Figure FDA0002207115240000023
其中，保护距离dp＝P，左右检测窗口的测试距离为dr1＝R1+P；
计算预处理后的地面图像中每个像素点处上下检测窗口内像素灰度累加值
Figure FDA0002207115240000031
Figure FDA0002207115240000032
其中上下检测窗口的测试距离为dr2＝R2+P；
计算预处理后的地面图像中每个像素点处保护单元内像素灰度累加值Pai,j：
Figure FDA0002207115240000033
计算预处理后的地面图像中每个像素点处左右参考单元灰度平均值
Figure FDA0002207115240000034
Figure FDA0002207115240000035
计算预处理后的地面图像中每个像素点处上下参考单元灰度平均值
Figure FDA0002207115240000037
(2.2.4)计算预处理后的地面图像中每个像素点处的检测阈值Ti,j、乘法器系数αi,j；
(2.2.5)对于预处理后的地面图像中每一个像素，比较灰度值Xi,j和检测阈值Ti,j、检测比值Rati,j和乘法器系数αi,j的值，
Figure FDA0002207115240000038
如果Xi,j<Ti,j或Rati,j<αi,j，该像素(i,j)设置为保留像素；图像中保留像素之外的像素都为背景；
(2.2.6)计算预处理后地面图像中保留像素的灰度平均值
Figure FDA0002207115240000039
比较保留像素的灰度值与
Figure FDA00022071152400000310
的大小，如果小于
Figure FDA00022071152400000311
该保留像素为目标，否则为背景。
4.根据权利要求3所述的服务机器人导航路径跟踪方法，其特征在于，所述步骤2.2.4中检测阈值Ti,j和乘法器系数αi,j的计算步骤包括：
(s1)计算像素(i,j)处的乘法器系数αi,j：
Figure FDA00022071152400000312
其中Pfa为预设的虚警概率常数；Rci,j为以像素(i,j)为检测单元时参考单元的个数；
(s2)估计像素(i,j)处检测阈值
Figure FDA0002207115240000041
Figure FDA0002207115240000042
其中
Figure FDA0002207115240000043
为以像素(i,j)为检测单元时第l个参考单元的灰度值；
将
Figure FDA0002207115240000044
作为像素(i,j)处的检测阈值Ti,j。
5.根据权利要求2所述的服务机器人导航路径跟踪方法，其特征在于，采用高斯滤波对图像进行去噪。
6.根据权利要求2所述的服务机器人导航路径跟踪方法，其特征在于，所述步骤(2)中判断导航路径是否满足直线判断条件，包括如下步骤：
统计检测到的导航路径上的每个直线线段与横坐标轴的夹角θ，计算所有直线线段与横坐标轴的夹角的方差σθ；如果σθ<Tθ，认为检测到的导航路径为直线，即满足直线判断条件；否则，认为不满足直线判断条件；其中Tθ为预设的线段夹角方差阈值。
7.服务机器人，其特征在于，包括：机器人本体(1)，以及安装于机器人本体(1)上的摄像头(2)、惯性导航系统(3)、导航路径检测模块(6)、主控模块(7)、电机(8)和方向控制器(9)；
所述摄像头(2)用于获取服务机器人前方原始地面图像并发送给导航路径检测模块(6)，所述原始地面图像包括导航路径；所述惯性导航系统(3)获取服务机器人的位置和前进方向并发送给导航路径检测模块(6)；所述导航路径检测模块(6)计算服务机器人的位置与导航直线路径的距离d和服务机器人前进方向与导航直线路径的夹角ω；所述主控模块(7)将夹角ω发送至方向控制器(9)，方向控制器(9)控制服务机器人的前进方向，使夹角ω和距离d减小；所述电机(8)用于驱动服务机器人的车轮；
导航路径检测模块(6)采用权利要求1-6中任一项所述的服务机器人导航路径跟踪方法来计算服务机器人的位置与导航直线路径的距离d和服务机器人前进方向与导航直线路径的夹角ω。
8.根据权利要求7所述的服务机器人，其特征在于，还包括设置于机器人本体(1)上的光敏传感器(4)和照明灯(5)；所述光敏传感器(4)和照明灯(5)的开关均与主控模块(7)连接，当光敏传感器(4)检测到环境光照强度小于阈值时，主控模块(7)控制照明灯的开关导通，开启照明灯；当光敏传感器(4)检测到环境光照强度大于阈值时，主控模块(7)控制照明灯的开关关断，关闭照明灯。
9.根据权利要求7所述的服务机器人，其特征在于，所述主控模块(7)采用STM32控制板。
CN201910885238.XA 2019-09-19 2019-09-19 服务机器人导航路径跟踪方法及服务机器人 Active CN110631577B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910885238.XA CN110631577B (zh) 2019-09-19 2019-09-19 服务机器人导航路径跟踪方法及服务机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910885238.XA CN110631577B (zh) 2019-09-19 2019-09-19 服务机器人导航路径跟踪方法及服务机器人

Publications (2)

Publication Number Publication Date
CN110631577A true CN110631577A (zh) 2019-12-31
CN110631577B CN110631577B (zh) 2021-03-19

Family

ID=68971666

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910885238.XA Active CN110631577B (zh) 2019-09-19 2019-09-19 服务机器人导航路径跟踪方法及服务机器人

Country Status (1)

Country Link
CN (1) CN110631577B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111982094A (zh) * 2020-08-25 2020-11-24 北京京东乾石科技有限公司 导航方法及其装置和系统以及可移动设备
CN112083718A (zh) * 2020-08-13 2020-12-15 惠州拓邦电气技术有限公司 视觉导航机器人的控制方法、装置及计算机可读存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8509965B2 (en) * 2006-12-12 2013-08-13 American Gnc Corporation Integrated collision avoidance system for air vehicle
CN103822625A (zh) * 2013-12-01 2014-05-28 兰州大学 一种智能机器人寻线导航的方法与装置
CN104483966A (zh) * 2014-11-17 2015-04-01 范良志 一种agv导航控制系统
CN105608691A (zh) * 2015-12-17 2016-05-25 武汉大学 一种高分辨率sar影像单体建筑提取方法
CN105651286A (zh) * 2016-02-26 2016-06-08 中国科学院宁波材料技术与工程研究所 一种移动机器人视觉导航方法与系统、以及仓库系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8509965B2 (en) * 2006-12-12 2013-08-13 American Gnc Corporation Integrated collision avoidance system for air vehicle
CN103822625A (zh) * 2013-12-01 2014-05-28 兰州大学 一种智能机器人寻线导航的方法与装置
CN104483966A (zh) * 2014-11-17 2015-04-01 范良志 一种agv导航控制系统
CN105608691A (zh) * 2015-12-17 2016-05-25 武汉大学 一种高分辨率sar影像单体建筑提取方法
CN105651286A (zh) * 2016-02-26 2016-06-08 中国科学院宁波材料技术与工程研究所 一种移动机器人视觉导航方法与系统、以及仓库系统

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
XIUZHI LI 等: "Autonomous mobile robot guidance based on ground line mark", 《2012 PROCEEDINGS OF SICE ANNUAL CONFERENCE (SICE)》 *
丁安: "声纳图像水下管线检测与跟踪技术研究", 《中国优秀硕士学位论文全文数据库》 *
万好: "基于视觉引导的AGV车载系统研究", 《中国优秀硕士学位论文全文数据库》 *
李金超: "基于视觉的自动寻迹 AGV 控制系统研究", 《中国优秀硕士学位论文全文数据库》 *
田梦: "全方位移动机器人巡线导航技术研究", 《中国优秀硕士学位论文全文数据库》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112083718A (zh) * 2020-08-13 2020-12-15 惠州拓邦电气技术有限公司 视觉导航机器人的控制方法、装置及计算机可读存储介质
CN112083718B (zh) * 2020-08-13 2023-08-04 惠州拓邦电气技术有限公司 视觉导航机器人的控制方法、装置及计算机可读存储介质
CN111982094A (zh) * 2020-08-25 2020-11-24 北京京东乾石科技有限公司 导航方法及其装置和系统以及可移动设备
CN111982094B (zh) * 2020-08-25 2022-06-07 北京京东乾石科技有限公司 导航方法及其装置和系统以及可移动设备

Also Published As

Publication number Publication date
CN110631577B (zh) 2021-03-19

Similar Documents

Publication Publication Date Title
US10424081B2 (en) Method and apparatus for calibrating a camera system of a motor vehicle
US6470271B2 (en) Obstacle detecting apparatus and method, and storage medium which stores program for implementing the method
CN103065323B (zh) 一种基于单应性变换矩阵的分段空间对准方法
CN111089590B (zh) 一种视觉和激光融合的移动机器人跟踪人腿方法
CN107972662A (zh) 一种基于深度学习的车辆前向碰撞预警方法
CN107844750A (zh) 一种水面全景图像目标检测识别方法
CN107315410B (zh) 一种机器人自动排障方法
KR102296520B1 (ko) 단안 카메라를 이용한 경로추정에 의한 곡선차선 검출 방법
CN110631577B (zh) 服务机器人导航路径跟踪方法及服务机器人
EP2293588A1 (en) Method for using a stereovision camera arrangement
CN115592324B (zh) 基于人工智能的自动焊接机器人控制系统
Wang et al. A real-time weld line detection for derusting wall-climbing robot using dual cameras
CN112045655B (zh) 用于大尺度多站点场景的移动机器人位姿测量方法及系统
CN113110451A (zh) 一种深度相机与单线激光雷达融合的移动机器人避障方法
CN105300390B (zh) 障碍物体运动轨迹的确定方法及装置
KR101165122B1 (ko) 실내 주행을 위한 빛 줄무늬 감지 방법 및 그를 이용한주차 보조 장치
CN114413958A (zh) 无人物流车的单目视觉测距测速方法
CN111046809B (zh) 一种障碍物检测方法、装置、设备及计算机可读存储介质
US11904843B2 (en) Autonomous parking systems and methods for vehicles
CN111860270B (zh) 一种基于鱼眼相机的障碍物检测方法及装置
CN109815812B (zh) 一种基于水平边缘信息累积的车辆下底边定位方法
CN112215214A (zh) 调整智能车载终端的摄像头偏移的方法及系统
JP7043787B2 (ja) 対象物検知システム
CN116091603A (zh) 一种基于点特征的箱体工件位姿测量方法
Van Crombrugge et al. Fast free floor detection for range cameras

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant