一种单主多从协同定位的公路遗落物清理系统和方法

1.本发明属于智能高速公路机器人技术领域，具体涉及一种单主多从协同定位的公路遗落物清理系统和方法。


背景技术：

2.随着高速公路的长度的不断增加，以及私家车保有量的逐年增长，造成高速公路上道路遗落物的增多，进而增加了高速公路道路养护任务的难度和复杂度。同时，现有高速公路的道路养护过程为将单条道路封锁，派出专业养护人员对道路上的遗落物进行清理，该种清理方式引入了人员直接接触高速公路的危险，同时高速公路上高温、极寒等恶劣环境以及较长的养护里程会增加大量人员成本并对养护人员的人身安全造成危害。因此，实现高速公路道路养护、设计用于道路遗落物自动捡拾的智慧高速公路机器人是将来的必然要求。
3.目前，用于高速公路封道养护时的遗落物清理机器人主要有如下缺陷：对于主机挂载在护栏上的单主-单从的协作机器人，其主机同时能完成交通路况巡检工作和本发明所述的遗落物清理检测工作，但是若养护车道非路旁车道，即定位距离较远时，限制于其设计，会大大降低对从机的定位精度，限制了从机遗落物清理功能的精准性与可靠性。


技术实现要素：

4.本发明的目的是为了解决背景技术中提及的问题，提供一种单主多从协同定位的公路遗落物清理系统和方法，能够提高主从公路遗落物清理系统定位的精度与泛用性。
5.为实现上述技术目的，本发明采取的技术方案为：
6.一种单主多从协同定位的公路遗落物清理系统，包括主机器人和从机器人，所述主机器人为巡检机器人，所述从机器人包括捡拾机器人和至少两台辅助定位从机；所述巡检机器人包括uwb主基站、双目摄像头、gps定位模块和行走机构，所述行走机构用于将巡检机器人滑动连接在公路护栏上，并驱动其滑行；所述捡拾机器人包括uwb标签、回收槽、回收机构和第一驱动机构，所述回收机构用于拾捡目标物，并将其置于回收槽内，所述第一驱动机构用于驱动捡拾机器人在地面行走；所述辅助定位从机包括uwb从基站和第二驱动机构，所述第二驱动机构用于驱动辅助定位从机在地面行走。
7.作为优选，所述巡检机器人还包括外壳，所述uwb主基站和双目摄像头设置在外壳顶部，所述gps定位模块设置在外壳内部，所述行走结构设置在外壳的内侧，所述行走机构包括外从动轮、内从动轮和主动轮，所述外从动轮和内从动轮转动连接在对应的转轴上，内从动轮位于外从动轮的内侧，所述外从动轮和内从动轮用于将巡检机器人扣置于公路护栏上；所述主动轮与驱动电机的输出轴连接；所述外从动轮、内从动轮和主动轮的侧壁与公路护栏相抵接触。
8.作为优选，所述捡拾机器人还包括壳体、摄像头、姿态传感器和里程计，所述摄像头设置在壳体的前端；所述姿态传感器和里程计设置在壳体内部，所述uwb标签设置在壳体
的顶部，所述回收槽也设置在壳体顶部，所述回收槽为顶部开口槽；所述回收机构壳体前端设有机械臂和铲斗，所述铲斗连接在机械臂的前端；所述第一驱动机构包括设置在壳体底部的四个车轮，其中至少一个车轮与驱动电机连接。
9.作为优选，所述uwb从基站设置在辅助定位从机的顶部，所述第二驱动机构包括两个驱动车轮和一个万向轮；所述驱动车轮与驱动电机连接，驱动车轮安装在辅助定位从机底部后方两侧，所述万向轮安装在辅助定位从机底部的前端。
10.一种利用上述任一项所述系统的公路遗落物清理方法，包括以下步骤：
11.s1、启动巡检机器人使其沿公路护栏滑行，捡拾机器人和辅助定位从机跟随巡检机器人在公路上移动；
12.s2、巡检机器人的双目摄像头获取沿途的路面图像并判断是否存在遗落物，如果存在则进入步骤s3，否则进入步骤s4；
13.s3、捡拾遗落物，具体包括以下分步骤：
14.s31、停止巡检机器人、捡拾机器人和辅助定位从机的移动，确认捡拾机器人和遗落物的二维坐标；
15.s32、根据步骤s31的结果，驱动捡拾机器人不断修正自身的运动轨迹前往遗落物所在位置并捡拾遗落物；
16.s33、巡检机器人的双目摄像头检测并确认是否完成所有遗落物的回收，如果已完成，则进入步骤s4，否则返回步骤s31；
17.s4、将捡拾机器人移动回公路边，并根据巡检机器人的gps定位模块确认是否抵达巡检终点，如果未抵达则返回步骤s1，否则停止所有设备运转，结束巡检。
18.作为优选，步骤s2中巡检机器人通过图像阈值与深度学习模型检测双目摄像头获取的路面图像中的遗落物。
19.作为优选，步骤s31确定遗落物的二维坐标包括以下分步骤：
20.s501、根据步骤s2中检测到的遗落物在路面图像中的位置划定感兴趣区域；
21.s502、通过双目摄像头获得视差图，并计算感兴趣区域的平均视差；
22.s503、通过双目视觉定位算法确定遗落物相对于巡检机器人的二维坐标。
23.作为优选，步骤s31确定捡拾机器人位置包括以下具体步骤：
24.s601、将辅助定位从机上的uwb从基站轮流切换为标签模式，获取辅助定位从机相对于巡检机器人的二维坐标；
25.s602、根据捡拾机器人上uwb标签与uwb主基站和至少两个uwb从基站间的定位信息，利用toa算法确定捡拾机器人相对于巡检机器人的二维坐标。
26.作为优选，步骤s32包括以下分步骤：
27.s321、根据步骤s31获取的捡拾机器人和遗落物的二维坐标，计算两点间的直线斜率和距离；
28.s322、根据步骤s321所得的两点间的直线斜率，通过捡拾机器人的姿态传感器调整自身角度；通过里程计计算已行进距离，当行进距离等于步骤s321所得的两点间的距离时，停止运动。
29.作为优选，步骤s322通过摄像头采集图像数据，利用视觉算法辅助调整捡拾机器人与遗落物的相对位置。
30.本发明的有益效果是：
31.1、通过巡检机器人和至少两台辅助定位机器人，通过uwb基站的位置信号检测和toa算法能实现捡拾机器人的精准定位，捡拾机器人的定位精度仅受uwb基站对uwb标签测距精度的影响，相较于单巡检机器人和捡拾机器人的组合，定位效果更精准，且减少了主从机定位系统涉及的难度和定位数据计算的复杂度。
32.2、对捡拾机器人和遗落物进行精准定位后，可以通过捡拾机器人自动捡拾遗落物，能够实现公路封道养护时遗落物清理的自动化，减少人员直接接触公路路面的危险、降低人工成本。
33.3、本发明的计算压力集中于所述巡检机器人主机，可以降低所述三台从机(两台辅助定位从机和捡拾机器人)的功耗与成本，提高本发明整体作业时的续航能力。
附图说明
34.图1为本发明实施方法的整体流程图；
35.图2为本发明的主从机器人部署场景示意图；
36.图3为本发明的巡检机器人的结构示意图；
37.图4为本发明的捡拾机器人的结构示意图；
38.图5为本发明的辅助定位从机的结构示意图；
39.图6为本发明uwb三基站相对位置测定示意图；
40.图7为本发明捡拾机器人uwb标签定位示意图。
41.图中标记名称：
42.1、巡检机器人；2、捡拾机器人；3、辅助定位从机；4、公路护栏；5、遗落物；6、公路；
43.11、外壳；12、行走机构；121、外从动轮；122、内从动轮；123、主动轮；13、uwb主基站；14、双目相机；
44.21、壳体；211、车轮；22、uwb标签；23、回收槽；24、摄像头；25、回收机构；
45.31、uwb从基站；32、车轮；33、万向轮。
具体实施方式
46.以下结合附图对本发明的实施例作进一步详细描述。
47.需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
48.如图1所示，一种单主多从协同定位的公路遗落物清理系统，包括主机器人和从机器人，所述主机器人为巡检机器人1，所述从机器人包括捡拾机器人2和两台辅助定位从机3(本实施例采用两台辅助定位从机3，也可以根据实际需求设置更多数量，但优选是两台，是能实现精准定位的最小辅助定位从机数量)；
49.如图2所示，所述巡检机器人1包括外壳11、uwb主基站13、双目摄像头14、gps定位模块和行走机构12，所述uwb主基站13和双目摄像头14设置在外壳11顶部，所述uwb主基站用于与uwb从基站交互信号，从而确定各个基站的相对位置(具体原理在后续方法介绍中会详述)，所述双目摄像头14用于采集公路图像；所述gps定位模块设置在外壳11内部，所述
gps定位模块用于确定巡检机器人1的位置，从而判断其是否抵达巡检终点；
50.所述行走结构12设置在外壳11的内侧，所述行走机构12包括外从动轮121、内从动轮122和主动轮123，所述外从动轮121和内从动轮122转动连接在对应的转轴上，内从动轮122位于外从动轮121的内侧，所述外从动轮121和内从动轮122用于将巡检机器人1扣置于公路护栏4上；本实施例中内从动轮122位于外从动轮121上方(实际两者的上下方位颠倒过来也可以)，公路护栏4卡接在外从动轮121和内从动轮122之间；
51.所述主动轮123与驱动电机的输出轴连接；所述外从动轮121、内从动轮122和主动轮123的侧壁与公路护栏相抵接触，实际使用过程中，驱动电机带动主动轮123转动，从而带动巡检机器人1沿着公路护栏4滑动。
52.如图3所示，所述捡拾机器人2包括壳体21、uwb标签22、回收槽23、摄像头24、回收机构25、第一驱动机构、姿态传感器和里程计，所述摄像头24设置在壳体21的前端，摄像头24用于拍摄捡拾机器人2前方图像，用于判断前方物体情况，用于辅助捡拾机器人2定位，提高其定位精度，从而更快且有效的捡拾遗留物5；
53.所述姿态传感器和里程计设置在壳体21内部，姿态传感器用于感知捡拾机器人2自身朝向，据此调整捡拾机器人2自身的角度(朝向)，所述里程计用于记录捡拾机器人2已行进距离；
54.所述uwb标签设置在壳体21的顶部，所述壳体21的顶部还有回收槽23，所述回收槽23为顶部开口槽；所述回收机构25壳体21前端设有机械臂和铲斗，所述铲斗连接在机械臂的前端；通过机械臂控制铲斗将遗留物5铲起并放入回收槽内；需注意回收机构25采用机械臂与铲斗的组合结构仅是本实施例的优选方案，也可以采用其他方案，比如机械臂加机械爪的组合结构，只要能捡拾遗留物5并将其置于回收槽23内即可；
55.所述第一驱动机构包括设置在壳体21底部的四个车轮211，其中至少一个车轮211与驱动电机连接，且该车轮211相对的那个车轮211要能与其同步转向，从而控制捡拾机器人2的移动方向。
56.如图4所示，所述辅助定位从机3包括uwb从基站31和第二驱动机构，所述uwb从基站31设置在辅助定位从机3的顶部，所述第二驱动机构包括两个驱动车轮32和一个万向轮33；所述驱动车轮32与驱动电机连接，驱动车轮32安装在辅助定位从机3底部后方两侧，所述驱动车轮32可同步转向，从而控制辅助定位从机3的移动方向，所述万向轮33安装在辅助定位从机3底部的前端。
57.如图5-7所示，本发明还提供一种利用上述系统的公路遗落物清理方法，包括以下步骤：
58.s0、相关部门对巡检路段进行封锁，设置警示牌；(此步骤为优选步骤，实际使用过程中，能保证捡拾机器人2和公路上行驶车辆的安全，但即使不设置此步骤，该方法依然可运行，只是安全效果和捡拾效果会比较差)；
59.s1、启动巡检机器人1使其沿公路护栏4滑行，捡拾机器人2和辅助定位从机3跟随巡检机器人1在公路6上移动；
60.s2、巡检机器人1的双目摄像头14获取沿途的路面图像，通过图像阈值与深度学习模型检测双目摄像头14获取的路面图像中是否存在遗落物5，如果存在则进入步骤s3，否则进入步骤s4；
61.s3、捡拾遗落物5，具体包括以下分步骤：
62.s31、停止巡检机器人1、捡拾机器人2和辅助定位从机3的移动，确认捡拾机器人2和遗落物5的二维坐标；
63.确认遗落物5的二维坐标包括以下步骤：
64.s501、根据步骤s2中检测到的遗落物在路面图像中的位置划定感兴趣区域；
65.s502、通过双目摄像头14获得视差图，并计算感兴趣区域的平均视差；
66.s503、通过双目视觉定位算法确定遗落物相对于巡检机器人1的二维坐标(也就是以巡检机器人1作为坐标原点)。
67.通过双目摄像头获取视差图，以及双目视觉定位算法属于现有技术，此处不再赘述。
68.确认捡拾机器人2的二维坐标包括以下步骤：
69.s601、将辅助定位从机3上的uwb从基站31轮流切换为标签模式，获取辅助定位从机3相对于巡检机器人1的二维坐标；
70.具体包括以下步骤：
71.前提是建立以巡检机器人1(也就是图6中的bm)为坐标原点，巡检机器人1前进方向(也可以以相反的方向)为x轴正方向，以朝向公路侧(也可以是相反方向)为y轴正方向的二维坐标系。
72.第一、将两个从基站轮流工作于标签模式，获得uwb主基站13与两个标签(也就是uwb从基站31)之间的距离和以及两个标签之间的距离d
s1s2
，以及两个标签与y轴的相对夹角α
s1
与α
s2
。
73.第二、通过以下公式求解两个辅助定位从机b
s1
与b
s2
的相对于巡检机器人1的坐标(y
s1
,x
s1
)和(y
s2
,x
s2
)
74.x
s1
＝d
ms1
*sin(α
s1
)
75.y
s1
＝d
ms1
*cos(α
s1
)
[0076][0077][0078]
s602、根据捡拾机器人2上uwb标签22与uwb主基站13和两个uwb从基站31间的定位信息，利用toa算法确定捡拾机器人2相对于巡检机器人1的二维坐标。
[0079]
s32、根据步骤s31的结果，驱动捡拾机器人2不断修正自身的运动轨迹前往遗落物所在位置并捡拾遗落物；
[0080]
具体包括以下分步骤：s321、根据步骤s31获取的捡拾机器人2和遗落物的二维坐标，计算两点间的直线斜率和距离；
[0081]
s322、根据步骤s321所得的两点间的直线斜率，通过捡拾机器人2的姿态传感器调整自身角度；通过里程计计算已行进距离，当行进距离等于步骤s321所得的两点间的距离时，停止运动；还可以通过摄像头24采集图像数据，利用视觉算法辅助调整捡拾机器人与遗落物的相对位置。
[0082]
s33、巡检机器人1的双目摄像头14检测并确认是否完成所有遗落物的回收，如果已完成，则进入步骤s4，否则返回步骤s31；
[0083]
s4、将捡拾机器人2移动回公路边，并根据巡检机器人1的gps定位模块确认是否抵达巡检终点，如果未抵达则返回步骤s1，否则停止所有设备运转，结束巡检。
[0084]
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

技术特征：
1.一种单主多从协同定位的公路遗落物清理系统，其特征在于：包括主机器人和从机器人，所述主机器人为巡检机器人（1），所述从机器人包括捡拾机器人（2）和至少两台辅助定位从机（3）；所述巡检机器人（1）包括uwb主基站（13）、双目摄像头（14）、gps定位模块和行走机构（12），所述行走机构（12）用于将巡检机器人（1）滑动连接在公路护栏上，并驱动其滑行；所述捡拾机器人（2）包括uwb标签（22）、回收槽（23）、回收机构（25）和第一驱动机构，所述回收机构（25）用于拾捡目标物，并将其置于回收槽（23）内，所述第一驱动机构用于驱动捡拾机器人（2）在地面行走；所述辅助定位从机（3）包括uwb从基站（31）和第二驱动机构，所述第二驱动机构用于驱动辅助定位从机（3）在地面行走。2.根据权利要求1所述的一种单主多从协同定位的公路遗落物清理系统，其特征在于：所述巡检机器人（1）还包括外壳（11），所述uwb主基站（13）和双目摄像头（14）设置在外壳（11）顶部，所述gps定位模块设置在外壳（11）内部，所述行走结构（12）设置在外壳（11）的内侧，所述行走机构（12）包括外从动轮（121）、内从动轮（122）和主动轮（123），所述外从动轮（121）和内从动轮（122）转动连接在对应的转轴上，内从动轮（122）位于外从动轮（121）的内侧，所述外从动轮（121）和内从动轮（122）用于将巡检机器人（1）扣置于公路护栏上；所述主动轮（123）与驱动电机的输出轴连接；所述外从动轮（121）、内从动轮（122）和主动轮（123）的侧壁与公路护栏相抵接触。3.根据权利要求1所述的一种单主多从协同定位的公路遗落物清理系统，其特征在于：所述捡拾机器人（2）还包括壳体（21）、摄像头（24）、姿态传感器和里程计，所述摄像头（24）设置在壳体（21）的前端；所述姿态传感器和里程计设置在壳体（21）内部，所述uwb标签设置在壳体（21）的顶部，所述回收槽（23）也设置在壳体（21）顶部，所述回收槽（23）为顶部开口槽；所述回收机构（25）壳体（21）前端设有机械臂和铲斗，所述铲斗连接在机械臂的前端；所述第一驱动机构包括设置在壳体（21）底部的四个车轮（211），其中至少一个车轮（211）与驱动电机连接。4.根据权利要求1所述的一种单主多从协同定位的公路遗落物清理系统，其特征在于：所述uwb从基站（31）设置在辅助定位从机（3）的顶部，所述第二驱动机构包括两个驱动车轮（32）和一个万向轮（33）；所述驱动车轮（32）与驱动电机连接，驱动车轮（32）安装在辅助定位从机（3）底部后方两侧，所述万向轮（33）安装在辅助定位从机（3）底部的前端。5.一种利用权利要求1-4中任一项所述系统的公路遗落物清理方法，其特征在于：包括以下步骤：s1、启动巡检机器人（1）使其沿公路护栏滑行，捡拾机器人（2）和辅助定位从机（3）跟随巡检机器人（1）在公路上移动；s2、巡检机器人（1）的双目摄像头（14）获取沿途的路面图像并判断是否存在遗落物，如果存在则进入步骤s3，否则进入步骤s4；s3、捡拾遗落物，具体包括以下分步骤：s31、停止巡检机器人（1）、捡拾机器人（2）和辅助定位从机（3）的移动，确认捡拾机器人（2）和遗落物的二维坐标；s32、根据步骤s31的结果，驱动捡拾机器人（2）不断修正自身的运动轨迹前往遗落物所在位置并捡拾遗落物；s33、巡检机器人（1）的双目摄像头（14）检测并确认是否完成所有遗落物的回收，如果
已完成，则进入步骤s4，否则返回步骤s31；s4、根据巡检机器人（1）的gps定位模块确认是否抵达巡检终点，如果未抵达则返回步骤s1，否则停止所有设备运转，结束巡检。6.根据权利要求5所述的公路遗落物清理方法，其特征在于：步骤s2中巡检机器人（1）通过图像阈值与深度学习模型检测双目摄像头（14）获取的路面图像中的遗落物。7.根据权利要求6所述的公路遗落物清理方法，其特征在于：步骤s31确定遗落物的二维坐标包括以下分步骤：s501、根据步骤s2中检测到的遗落物在路面图像中的位置划定感兴趣区域；s502、通过双目摄像头（14）获得视差图，并计算感兴趣区域的平均视差；s503、通过双目视觉定位算法确定遗落物相对于巡检机器人（1）的二维坐标。8.根据权利要求5所述的公路遗落物清理方法，其特征在于：步骤s31确定捡拾机器人（2）位置包括以下具体步骤：s601、将辅助定位从机（3）上的uwb从基站（31）轮流切换为标签模式，获取辅助定位从机（3）相对于巡检机器人（1）的二维坐标；s602、根据捡拾机器人（2）上uwb标签（22）与uwb主基站（13）和至少两个uwb从基站（31）间的定位信息，利用toa算法确定捡拾机器人（2）相对于巡检机器人（1）的二维坐标。9.根据权利要求5所述的公路遗落物清理方法，其特征在于：步骤s32包括以下分步骤：s321、根据步骤s31获取的捡拾机器人（2）和遗落物的二维坐标，计算两点间的直线斜率和距离；s322、根据步骤s321所得的两点间的直线斜率，通过捡拾机器人（2）的姿态传感器调整自身角度；通过里程计计算已行进距离，当行进距离等于步骤s321所得的两点间的距离时，停止运动。10.根据权利要求9所述的公路遗落物清理方法，其特征在于：步骤s322通过摄像头（24）采集图像数据，利用视觉算法辅助调整捡拾机器人与遗落物的相对位置。

技术总结
本发明提供一种单主多从协同定位的公路遗落物清理系统和方法，属于智能高速公路机器人技术领域，方法包括以下步骤：S1、启动主从机器人；S2、主机器人的双目摄像头获取沿途的路面图像并判断是否存在遗落物，如果存在则进入步骤S3，否则进入步骤S4；S3、捡拾遗落物；S4、根据巡检机器人的GPS定位模块确认是否抵达巡检终点，如果未抵达则返回步骤S1，否则停止所有设备运转，结束巡检；以及与该方法配套的系统，包括主机器人和从机器人，所述主机器人为巡检机器人（1），所述从机器人包括捡拾机器人（2）和至少两台辅助定位从机（3）；本发明能够提高主从公路遗落物清理系统定位的精度与泛用性。从公路遗落物清理系统定位的精度与泛用性。从公路遗落物清理系统定位的精度与泛用性。


技术研发人员：陈卓 乔贵方 刘娣 林均泽 沈重霖 张颖 邹旭
受保护的技术使用者：南京工程学院
技术研发日：2023.06.27
技术公布日：2023/10/11