一种用于机器人操作系统的无人车自主脱困方法及系统

1.本发明涉及机器人操作系统技术领域，特别涉及一种用于机器人操作系统的无人车自主脱困方法及系统。


背景技术：

2.机器人操作系统ros(robot operating system)是一个适用于机器人的开源元操作系统。它提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及包管理；它也提供用于获取、编译、编写和跨计算机运行代码所需的工具和库函数。ros的主要目标是为机器人研究和开发提供代码复用的支持。从软件构架的角度说，ros是一个强大而灵活的机器人编程框架，它采用了一种基于消息传递通信的分布式多进程框架。ros很早就被机器人行业使用，很多知名的机器人开源库，比如基于quaternion的坐标转换、3d点云处理驱动、定位算法slam等都是开源贡献者基于ros开发的。因为ros本身是基于消息机制的，开发者可以根据功能把软件拆分成为各个模块，每个模块只是负责读取和分发消息，模块间通过消息关联。目前，已有大量第三方企业和科研机构投入到ros的开发，越来越多的机器人以ros作为开发平台。
3.ros可以分成两层，低层是操作系统层，高层则是实现不同功能的各种软件包。其中，导航包集包含costmap_2d、global_planner、base_local_planner等多个功能包，运行时对外表现为move_base功能节点，可基于感知系统，控制机器人的自主行驶。在非结构化的复杂环境中，无人车没有高精度地图可依托，因此其需要实时创建感知地图进行路线规划。由于高速行驶下的规划、控制频率要求与计算处理能力的矛盾，以及非结构化的静止与移动障碍物特征导致构建的感知地图的不精确性，使得无人车易发生撞上障碍物停止行驶，或驶入死角受困不能到达既定目标点的情形，而现有的ros导航包集却未提供相关机制解决这一问题。


技术实现要素：

4.有鉴于此，本发明实施例的目的在于提供一种用于机器人操作系统的无人车自主脱困方法及系统，当无人车碰撞到障碍物或规划失败时，该机制能够有效帮助无人车脱困，最终到达既定目标点。
5.第一方面，本发明实施例提供了一种用于机器人操作系统的无人车自主脱困方法，其中，包括：
6.当机器人操作系统的导航中心枢纽节点(move_base)反馈无人车出现不能到达既定目标点的情况时，基于代价地图(costmap)，自主生成无人车可到达的临时目标点。
7.当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点(precious_goal)给导航中心枢纽节点(move_base)，进行路径规划。
8.其中，无人车不能到达既定目标点的情况包括局部轨迹控制失效、路线规划失败或反复振荡。
9.其中，代价地图(costmap)由为栅格地图中的每一个栅格分配一个代价值形成。
10.结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，所述基于代价地图(costmap)，自主生成无人车可到达的临时目标点，包括：
11.遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方法，选择其中一个栅格对应的坐标为临时目标点位置坐标。
12.将当前无人车的朝向信息作为临时目标点的朝向约束，将临时目标点发送至中心枢纽节点(move_base)，并将临时目标点发布变量(repub_goal)设置为true。
13.结合第一方面，本发明实施例提供了第一方面的第二种可能的实施方式，其中，所述遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方法，选择其中一个栅格对应的坐标为临时目标点位置坐标，包括：
14.选取无障碍自由空间栅格进行坐标变换，根据临时预设的目标点生成函数对栅格坐标进行过滤。
15.设置生成临时目标点时的相关参数，对栅格坐标再次进行过滤，得到临时目标点位置坐标。
16.结合第一方面，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所述选取无障碍自由空间栅格进行坐标变换，根据临时预设的目标点生成函数对栅格坐标进行过滤，包括：
17.将代价值为0的栅格坐标和其与无人车的距离信息存入一号容器中。
18.以无人车位置为原点，无人车朝向为x轴正向的坐标系为准，对栅格坐标进行变换。
19.若无人车执行后退行为过程中受困，则在车前方区域内选择临时目标点，若无人车执行前进行为过程中受困，则在车后方区域内选择临时目标点。
20.判断无人车位置点与临时目标点连线斜率绝对值是否小于斜率绝对值参数设定阈值，若小于，则将符合该条件的栅格放入二号容器中。
21.设置临时目标点候选栅格与无人车的最大距离，次距离与最小距离三个参数。
22.结合第一方面，本发明实施例提供了第一方面的第四种可能的实施方式，其中，所述设置生成临时目标点时的相关参数，对栅格坐标再次进行过滤，得到临时目标点位置坐标，包括：
23.如未有符合上述条件的，选择二号容器中位于最小距离和次距离之间的栅格，将符合该条件的栅格放入三号容器中。
24.如未有符合上述条件的，选择二号容器中小于最小距离值的栅格放入三号容器中。
25.选择二号容器中大于次距离且小于最大距离值的栅格放入三号容器中。
26.若三号容器最终为空，则返回无人车脱困失败信息，重新执行预设的目标点生成方法。
27.若三号容器最终不为空，则选择无人车位置点与栅格点坐标连线斜率绝对值最小的栅格，将其对应的坐标作为临时目标点位置坐标。
28.结合第一方面，本发明实施例提供了第一方面的第五种可能的实施方式，其中，所述当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点给导航中心枢纽节点
(move_base)，进行路径规划，包括：
29.当无人车到达临时目标点后，发送原始目标点(precious_goal)，并将临时目标点发布变量(repub_goal)设置为false。
30.当中心枢纽节点(move_base)接收到原始目标点(precious_goal)后，若当前临时目标点发布变量(repub_goal)为false，则将原始目标点(precious_goal)设为当前目标点。
31.第二方面，本发明实施例还提供了一种用于机器人操作系统的无人车自主脱困系统，其中，包括：
32.临时目标点生成装置，用于当机器人操作系统的导航中心枢纽节点(move_base)反馈无人车出现不能到达既定目标点的情况时，基于代价地图(costmap)，自主生成无人车可到达的临时目标点。
33.原始目标点发送装置，用于当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点(precious_goal)给导航中心枢纽节点(move_base)，进行路径规划。
34.结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，所述临时目标点生成装置包括：
35.临时目标点位置设置模块，用于遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方法，选择其中一个栅格对应的坐标为临时目标点位置坐标。
36.临时目标点变量设置模块，用于将当前无人车的朝向信息作为临时目标点的朝向约束，将临时目标点发送至中心枢纽节点(move_base)，并将临时目标点发布变量(repub_goal)设置为true。
37.结合第二方面，本发明实施例提供了第二方面的第二种可能的实施方式，其中，所述临时目标点位置设置模块包括：
38.无障碍自由空间栅格选取单元，用于将代价值为0的栅格坐标和其与无人车的距离信息存入一号容器中。
39.函数过滤单元，用于以无人车位置为原点，无人车朝向为x轴正向的坐标系为准，对栅格坐标进行变换；若无人车执行后退行为过程中受困，则在车前方区域内选择临时目标点，若无人车执行前进行为过程中受困，则在车后方区域内选择临时目标点；判断无人车位置点与临时目标点连线斜率绝对值是否小于斜率绝对值参数设定阈值，若小于，则将符合该条件的栅格放入二号容器中。
40.参数设置单元，用于设置临时目标点候选栅格与无人车的最大距离，次距离与最小距离三个参数。
41.二次过滤单元，用于选择二号容器中位于最小距离和次距离之间的栅格，将符合该条件的栅格放入三号容器中，选择二号容器中小于最小距离值的栅格放入三号容器中，选择二号容器中大于次距离且小于最大距离值的栅格放入三号容器中。
42.三次过滤单元，用于若三号容器最终为空，则返回无人车脱困失败信息，重新执行预设的目标点生成方法，若三号容器最终不为空，则选择无人车位置点与栅格点坐标连线斜率绝对值最小的栅格，将其对应的坐标作为临时目标点位置坐标。
43.结合第二方面，本发明实施例提供了第二方面的第三种可能的实施方式，其中，所述原始目标点发送装置包括：
44.原始目标点设置模块，用于当无人车到达临时目标点后，发送原始目标点(precious_goal)，并将临时目标点发布变量(repub_goal)设置为false。
45.原始目标点变量设置模块，用于当中心枢纽节点(move_base)接收到原始目标点(precious_goal)后，若当前临时目标点发布变量(repub_goal)为false，则将原始目标点(precious_goal)设为当前目标点。
46.本发明实施例的有益效果是：
47.本发明的用于机器人操作系统的无人车自主脱困方法及系统设计了一种无人车自主脱困机制，当机器人操作系统导航中心枢纽节点(move_base)反馈局部轨迹控制失效、路线规划失败或反复振荡情况信息时，可基于代价地图自主生成无人车可到达的临时目标点，在无人车到达临时目标点摆脱受困局面后，再次发送原始目标点给导航包中的中心枢纽节点(move_base)进行路径规划。当无人车碰撞到障碍物或规划失败时，该机制可有效帮助无人车脱困，最终到达既定目标点。
附图说明
48.为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
49.图1为本发明用于机器人操作系统的无人车自主脱困方法的流程图；
50.图2为本发明用于机器人操作系统的无人车自主脱困方法的完整流程示意图；
51.图3为本发明基于无人车位姿、受困时行驶行为和栅格位置的临时目标点过滤示意图。
具体实施方式
52.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件能够以各种不同的配置来布置和设计。
53.请参照图1至图3，本发明的第一个实施例提供一种用于机器人操作系统的无人车自主脱困方法，其中，包括：
54.(11)当机器人操作系统的导航中心枢纽节点(move_base)反馈无人车出现不能到达既定目标点的情况时，基于代价地图(costmap)，自主生成无人车可到达的临时目标点。
55.(12)当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点(precious_goal)给导航中心枢纽节点(move_base)，进行路径规划。
56.其中，无人车不能到达既定目标点的情况包括局部轨迹控制失效、路线规划失败或反复振荡。
57.其中，代价地图(costmap)由为栅格地图中的每一个栅格分配一个代价值形成。
58.其中，所述基于代价地图(costmap)，自主生成无人车可到达的临时目标点，包括：
59.(112)遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方
法，选择其中一个栅格对应的坐标为临时目标点位置坐标。
60.(113)将当前无人车的朝向信息作为临时目标点的朝向约束，将临时目标点发送至中心枢纽节点(move_base)，并将临时目标点发布变量(repub_goal)设置为true。
61.其中，所述遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方法，选择其中一个栅格对应的坐标为临时目标点位置坐标，包括：
62.选取无障碍自由空间栅格进行坐标变换，根据临时预设的目标点生成函数对栅格坐标进行过滤。
63.设置生成临时目标点时的相关参数，对栅格坐标再次进行过滤，得到临时目标点位置坐标。
64.其中，所述选取无障碍自由空间栅格进行坐标变换，根据临时预设的目标点生成函数对栅格坐标进行过滤，包括：
65.(111)将代价值为0的栅格坐标和其与无人车的距离信息存入一号容器中。
66.(1121)以无人车位置为原点，无人车朝向为x轴正向的坐标系为准，对栅格坐标进行变换。
67.若无人车执行后退行为过程中受困，则在车前方区域内选择临时目标点，若无人车执行前进行为过程中受困，则在车后方区域内选择临时目标点。
68.判断无人车位置点与临时目标点连线斜率绝对值是否小于斜率绝对值参数设定阈值，若小于，则将符合该条件的栅格放入二号容器中。
69.(1122)设置临时目标点候选栅格与无人车的最大距离，次距离与最小距离三个参数。
70.其中，所述设置生成临时目标点时的相关参数，对栅格坐标再次进行过滤，得到临时目标点位置坐标，包括：
71.(1123)如未有符合上述条件的，选择二号容器中位于最小距离和次距离之间的栅格，将符合该条件的栅格放入三号容器中。
72.(1124)如未有符合上述条件的，选择二号容器中小于最小距离值的栅格放入三号容器中。
73.(1125)选择二号容器中大于次距离且小于最大距离值的栅格放入三号容器中。
74.(1126)若三号容器最终为空，则返回无人车脱困失败信息，重新执行预设的目标点生成方法。
75.(1127)若三号容器最终不为空，则选择无人车位置点与栅格点坐标连线斜率绝对值最小的栅格，将其对应的坐标作为临时目标点位置坐标。
76.其中，所述当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点给导航中心枢纽节点(move_base)，进行路径规划，包括：
77.(121)当无人车到达临时目标点后，发送原始目标点(precious_goal)，并将临时目标点发布变量(repub_goal)设置为false。
78.(122)当中心枢纽节点(move_base)接收到原始目标点(precious_goal)后，若当前临时目标点发布变量(repub_goal)为false，则将原始目标点(precious_goal)设为当前目标点。
79.本发明的第二个实施例提供一种用于机器人操作系统的无人车自主脱困系统，其
中，包括：
80.临时目标点生成装置，用于当机器人操作系统的导航中心枢纽节点(move_base)反馈无人车出现不能到达既定目标点的情况时，基于代价地图(costmap)，自主生成无人车可到达的临时目标点。
81.原始目标点发送装置，用于当无人车到达临时目标点，摆脱受困局面后，再次发送原始目标点(precious_goal)给导航中心枢纽节点(move_base)，进行路径规划。
82.其中，所述临时目标点生成装置包括：
83.临时目标点位置设置模块，用于遍历当前代价地图(costmap)的所有栅格，根据临时预设的目标点生成方法，选择其中一个栅格对应的坐标为临时目标点位置坐标。
84.临时目标点变量设置模块，用于将当前无人车的朝向信息作为临时目标点的朝向约束，将临时目标点发送至中心枢纽节点(move_base)，并将临时目标点发布变量(repub_goal)设置为true。
85.其中，所述临时目标点位置设置模块包括：
86.无障碍自由空间栅格选取单元，用于将代价值为0的栅格坐标和其与无人车的距离信息存入一号容器中。
87.函数过滤单元，用于以无人车位置为原点，无人车朝向为x轴正向的坐标系为准，对栅格坐标进行变换；若无人车执行后退行为过程中受困，则在车前方区域内选择临时目标点，若无人车执行前进行为过程中受困，则在车后方区域内选择临时目标点；判断无人车位置点与临时目标点连线斜率绝对值是否小于斜率绝对值参数设定阈值，若小于，则将符合该条件的栅格放入二号容器中。
88.参数设置单元，用于设置临时目标点候选栅格与无人车的最大距离，次距离与最小距离三个参数。
89.二次过滤单元，用于选择二号容器中位于最小距离和次距离之间的栅格，将符合该条件的栅格放入三号容器中，选择二号容器中小于最小距离值的栅格放入三号容器中，选择二号容器中大于次距离且小于最大距离值的栅格放入三号容器中。
90.三次过滤单元，用于若三号容器最终为空，则返回无人车脱困失败信息，重新执行预设的目标点生成方法，若三号容器最终不为空，则选择无人车位置点与栅格点坐标连线斜率绝对值最小的栅格，将其对应的坐标作为临时目标点位置坐标。
91.其中，所述原始目标点发送装置包括：
92.原始目标点设置模块，用于当无人车到达临时目标点后，发送原始目标点(precious_goal)，并将临时目标点发布变量(repub_goal)设置为false。
93.原始目标点变量设置模块，用于当中心枢纽节点(move_base)接收到原始目标点(precious_goal)后，若当前临时目标点发布变量(repub_goal)为false，则将原始目标点(precious_goal)设为当前目标点。
94.本发明的第三个实施例提供一种用于机器人操作系统的无人车自主脱困方法的具体实施步骤，包括：
95.1.开启临时目标点自主生成机制
96.当控制、规划失败或无人车震荡时，在ros导航包move_base.cpp中的bool movebase::executecycle(geometry_msgs::posestamped&goal,std::vector《geometry_
msgs::posestamped》&global_plan)函数中，调用添加的受困后恢复行驶行为函数。
97.2.添加受困后恢复行驶行为函数，选取无障碍自由空间栅格
98.在ros导航包的move_base节点中添加受困后恢复行驶行为函数，该函数遍历实时生成的代价地图所有栅格，获取车辆的当前位置信息，如果栅格代价值为0，则将该无障碍自由空间栅格的坐标和其与无人车的距离放入容器1中；然后，调用添加的临时目标点生成函数。
99.3.设计临时目标点生成方法，实现临时目标点生成函数
100.3.1无障碍自由空间栅格进行第一次坐标变换：在受困后恢复行驶行为函数中调用的临时目标点生成函数，其首先对容器1中的无障碍自由空间栅格进行第一次坐标变换，如图2所示，xy坐标系为原始地图坐标系，右下角为地图原点；以无人车当前坐标为坐标原点，形成x1y1坐标系，进而求得栅格在x1y1坐标系中的坐标；
101.3.2无障碍自由空间栅格第二次坐标变换：如图2所示，以无人车朝向为x坐标轴正方向，对x1y1坐标系进行旋转，形成x2y2坐标系，进而求得栅格在x2y2坐标系中的坐标；
102.3.3；第一次过滤：如果无人车倒退时受困，选择车前方且与无人车连线的斜率绝对值小于斜率绝对值设定阈值的无障碍自由空间栅格(基于x2y2坐标系)；否则，选择车后方且与无人车连线的斜率绝对值小于斜率绝对值设定阈值的栅格(基于x2y2坐标系)；将过滤后符合条件的栅格放入容器2中；
103.3.4设置生成临时目标点时相关参数：最大距离、次距离和最小距离参数；
104.3.5：第二次过滤：在容器2中，选择位于最小距离和次距离之间的栅格，如符合条件，则将其放入容器3；如未有符合的，则选择小于最小距离值的栅格，如符合条件，则将其放入容器3；如未有符合的，则选择大于次距离且小于最大距离值的栅格，如符合条件，则将其放入容器3；
105.3.6：第三次过滤：如容器3为空，则返回无人车脱困失败信息；否则，遍历容器3内所有栅格，选出坐标点与x2y2坐标系原点连线的斜率绝对值最小的栅格。
106.4.发送临时目标点
107.清空容器1、2和3，第三次过滤后获取的栅格对应的世界坐标作为临时目标点的位置属性，将当前车的朝向信息作为临时目标点的朝向约束，发送临时目标点给move_base节点，并设置临时目标点发布变量(repub_goal)为true；如无人车不能到达临时目标点，则返回步骤1再次基于当前代价地图生成新的临时目标点。
108.5.发送原目标点
109.当无人车到达临时目标点后，发送原始目标点(precious_goal)，并设置临时目标点发布变量(repub_goal)为false，move_base接收到目标点后，如repub_goal为false，则设置precious_goal为当前目标点。
110.本发明实施例旨在保护一种用于机器人操作系统的无人车自主脱困方法及系统，具备如下效果：
111.本发明设计了一种无人车自主脱困机制，当机器人操作系统导航中心枢纽节点(move_base)反馈局部轨迹控制失效、路线规划失败或反复振荡情况信息时，可基于代价地图自主生成无人车可到达的临时目标点，在无人车到达临时目标点摆脱受困局面后，再次发送原始目标点给导航包中的中心枢纽节点(move_base)进行路径规划。当无人车碰撞到
障碍物或规划失败时，该机制可有效帮助无人车脱困，最终到达既定目标点。
112.本发明实施例所提供的用于机器人操作系统的无人车自主脱困方法及装置的计算机程序产品，包括存储了程序代码的计算机可读存储介质，程序代码包括的指令可用于执行前面方法实施例中的方法，具体实现可参见方法实施例，在此不再赘述。
113.具体地，该存储介质能够为通用的存储介质，如移动磁盘、硬盘等，该存储介质上的计算机程序被运行时，能够执行上述用于机器人操作系统的无人车自主脱困方法，从而当无人车碰撞到障碍物或规划失败时，能够有效帮助无人车脱困，最终到达既定目标点。
114.所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、磁碟或者光盘等各种可以存储程序代码的介质。
115.最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。