一种用于无人环卫车贴边清扫的图优化路径规划方法与流程

1.本发明涉及路径规划技术领域，具体为一种用于无人环卫车贴边清扫的图优化路径规划方法。


背景技术：

2.路径规划是无人驾驶车辆决策模块中的重要一环，它综合车辆周围障碍物的位置信息和事先建立的环境地图，来为车辆规划出一条符合运动规律同时又能快速到达目的地的路径。这里我们主要关注非规则路面上，适用于低速无人环卫车的路径规划问题。
3.近几年无人驾驶技术快速发展，适用于各种车辆的路径规划算法也在不断涌现和完善。可当把无人驾驶模块部署到传统环卫车上时，新的挑战随之而来。传统路径规划算法只需将车辆从起点导引到目标点同时避开障碍物即可，但是环卫车辆在行驶的同时还需要执行清扫任务，沿着马路边沿清扫即是其中一个比较典型的场景。
4.环卫车辆的贴边清扫作业要求路径规划模块不仅安全地将车辆从起点导引到目标点，还应该在最短的时间内尽可能地让车辆清扫盘覆盖所有的路沿，而当车辆靠路沿过近无法前进时，还应该规划出可行的恢复路径，继续清扫任务。这些特点是传统的采样式、搜索式路径规划算法无法满足的。传统方法将所有待确定的路径点假设为需要计算的未知状态，状态中包含车辆位置和偏航角，这些状态自身受障碍物位置的约束，状态之间受车辆运动规律和行驶时间的约束，当所有的状态符合最小转弯半径限制同时和障碍物保持足够的冗余距离时，状态的集合即为一条可行驶的路径。
5.如图8所示，{x0,x1,x2}代表需要计算的车辆位置和角度信息，车辆从x0行驶到x2时需要避开四边形障碍物，所以x1需要位于障碍物左侧；在避开障碍物的同时还不能花费太常时间，要尽量保证时间差值δt较小。
6.为实时地规划出无人车辆的行驶路径，基于采样或搜索的规划方法通常利用车辆的运动约束简化搜索空间，例如在前向可行驶区域中以一定的间隔搜索出最优的一条路径，但这种方法受限搜索空间范围的限制，通常无法规划出合适的倒车恢复路径。而传统基于图优化的路径规划为了安全地避开障碍物，无法规划出稳定贴边的行驶路径。
7.基于此，本发明设计了一种用于无人环卫车贴边清扫的图优化路径规划方法，以解决上述问题。


技术实现要素：

8.本发明的目的在于采用基于代价函数最小化的图优化方法来研究环卫车辆贴边清扫时的路径规划问题，提供一种用于无人环卫车贴边清扫的图优化路径规划方法。
9.为实现上述目的，本发明提供如下技术方案：一种用于无人环卫车贴边清扫的图优化路径规划方法，包括两个步骤：一是障碍物目标的分类；二是图优化中节点和边的设计；
10.障碍物目标的分类：传统无人驾驶车辆一般将路沿归属为不可碰撞接触的障碍
物，需要与其保持足够的安全冗余距离，但是无人环卫车经常需要进行贴边清扫作业。因此首先需要将车辆周围的障碍物分为无法越过但需要尽量贴近的路沿，或是环卫车不能靠近且需要绕行的人或车辆等障碍。实践中发现，路侧的绿植是最容易和路沿产生混淆的物体之一，特别地，当绿植的枝条延伸到道路上或有树叶连续地落下时，路径规划算法需要不受其影响，继续进行贴边规划，而当确实存在障碍物挡住贴边路径时，车辆需要安全地绕行。由于障碍物分类不是本专利重点，此处不做深入说明。
11.图优化中节点和边的设计：当需要规划的车辆状态数目较多时，需要将其表示为由节点和边组成的超图结构，以便于分析状态节点之间的联系和约束。在图优化中，将需要规划的车辆位置状态表示为节点，将节点所受的约束表示为边。
12.节点分为两类，一是表示车辆位置、偏航角的状态节点；二是表示车辆状态之间行驶时间的节点。边的类型也分为两类：一是相邻车辆状态节点之间所受的约束，例如转弯半径、角速度、加速度的限制；二是车辆状态节点自身所受的约束，例如距离某个障碍物或路沿的最小距离。
13.在求解最优的车辆状态集合前，需要将图结构转化为数学描述。用x表示车辆的状态节点，包括平面坐标和偏航角度，用q表示某段路径中这些状态的集合：
14.q＝{x1,x2...,x
n
}
15.用δt表示车辆的行驶时间节点，t表示其集合：
16.t＝{δt1,δt2...,δt
n
}
17.用b表示所有节点集合：
18.b＝(q,t)
19.将路径规划过程转为求解目标函数f(b)最小值时的解b
*
，f(b)由若干个约束子函数f
k
(b)组成，b
*
中的车辆状态集合即为最终的车辆路径：
20.f(b)＝∑γ
k
f
k
(b)
21.b
*
＝arg
min f(b)
22.虽然f
k
(b)对于车辆状态的约束目标不同，但都是将车辆状态优化到某一固定区间，因此具有相似的计算模式，其中下边界约束函数计算如下：
[0023][0024]
利用归一化参数s,n来将不同尺度的约束函数统一到同一水平，b
r
满足该约束子函数的最优状态，e为优化冗余量。
[0025]
对于无人环卫车贴边清扫时路径规划问题，在保持常规约束的同时，应着重考虑以下三种约束：一是路沿对车辆状态节点的吸引和排斥作用，车辆应尽量和路沿贴合但又不能越过路沿；二是车辆在贴边过程中的稳定性，尽量保持贴边的平稳性；三是保持稳定的车速以提高清扫效率，车速过快时清扫不干净，过慢时清扫效率太低。
[0026]
在对车辆周围的障碍物进行分类后，车辆状态节点的一元边在受路沿排斥作用的同时，还会根据所需的贴边距离来和路沿保持一定的吸引力，此约束子函数f
edge
计算如下：
[0027]
[0028]
其中为同时具有上下边界的三段式目标函数，
±
d表征了环卫车贴边时允许范围内的距离偏移。
[0029]
环卫车在清扫过程中保持贴边距离的稳定性不仅可以提高硬件电机的使用寿命还能提高清扫的整洁度，因此可将贴边距离的变化成度加入到约束函数中：
[0030][0031]
其中，是贴边距离的均值，n是车辆状态节点的个数，δ是该约束函数的归一化参数。
[0032]
在环卫车辆贴边清扫过程中，在保持贴边的同时还需要适配扫盘和车辆的行驶速度，以达到尽可能高的清扫效率，将此约束添加到子函数中：
[0033][0034]
其中δ
p
,δ
v
是行驶距离和速度的归一化参数，是车辆状态之间的距离，v
i
,v
s
是车辆状态节点对应的车速和清扫盘的转速。
[0035]
与现有技术相比，本发明的有益效果是：本发明采用基于代价函数最小化的图优化方法来研究环卫车辆贴边清扫时的路径规划问题，在对图优化中的障碍节点进行分类并添加稳定贴边约束后，该方法可以让环卫车安全有效地执行贴边清扫任务。
[0036]
当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。
附图说明
[0037]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0038]
图1为本发明路径规划方法实施状态图；
[0039]
图2为本发明路沿在“吸引”清扫盘边界时的状态图；
[0040]
图3为本发明路径规划方法添加新的稳定性约束边方法实施状态图；
[0041]
图4为传统图直线贴边状态图；
[0042]
图5为本发明方法优化直线贴边状态图；
[0043]
图6为传统图转弯贴边图；
[0044]
图7为本发明方法优化转弯贴边状态图；
[0045]
图8为传统方法计算路径时的实施状态图。。
具体实施方式
[0046]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它
实施例，都属于本发明保护的范围。
[0047]
请参阅图1
‑
7，本发明提供一种技术方案：一种用于无人环卫车贴边清扫的图优化路径规划方法，包括两个步骤：一是障碍物目标的分类；二是图优化中节点和边的设计。
[0048]
障碍物目标的分类：传统无人驾驶车辆一般将路沿归属为不可碰撞接触的障碍物，需要与其保持足够的安全冗余距离。但是无人环卫车首先需要区分周围的障碍目标是需要一直贴近的路沿还是需要远离绕行的其他物体。实践中发现，路侧的绿植是最容易和路沿产生混淆的物体之一。特别地，当绿植的枝条延伸到道路上或有树叶连续地落下时，路径规划算法需要不受其影响，继续进行贴边规划，而当确实存在障碍物挡住贴边路径时，车辆需要安全地绕行。本文假设障碍物种类信息已知。
[0049]
图优化中节点和边的设计：当需要规划的车辆状态的数目较多时，可将状态变量和变量所受约束表示为超图中的节点和边，便于分析节点之间的关联和约束。
[0050]
如图1所示，在车辆路径规划问题中，可将车辆的位置状态表示为图中的节点，将节点所受的约束表示为边。节点分为两类，一是表示车辆横纵坐标和偏航角的状态节点{x0,x1,x2}；二是表示车辆状态之间行驶时间的节点{δt1,δt2}。边的类型也分为两类：一是相邻车辆状态节点之间所受的约束，例如转弯半径、角速度、加速度的限制，图中velocity矩形框即表示线速度的上下界限制；二是车辆状态节点自身所受的约束，例如距离某个障碍物或路沿的最小距离，图中obstacle矩形框即表示x1和x2所对应的障碍物。
[0051]
在求解最优的车辆状态集合前，需要将图结构转化为数学描述。用x表示车辆的状态节点，包括平面坐标和偏航角度，用q表示某段路径中这些状态的集合：
[0052]
q＝{x1,x2...,x
n
}
[0053]
用δt表示车辆的行驶时间节点，t表示其集合：
[0054]
t＝{δt1,δt2...,δt
n
}
[0055]
用b表示所有节点集合：
[0056]
b＝(q,t)
[0057]
将路径规划过程转为求解目标函数f(b)最小值时的解b
*
，f(b)由若干个约束子函数f
k
(b)组成，b
*
中的车辆状态集合即为最终的车辆路径：
[0058]
f(b)＝∑γ
k
f
k
(b)
[0059]
b
*
＝arg
min f(b)
[0060]
虽然f
k
(b)对于车辆状态的约束目标不同，但都是将车辆状态优化到某一固定区间，因此具有相似的计算模式，其中下边界约束函数计算如下：
[0061][0062]
利用归一化参数s,n来将不同尺度的约束函数统一到同一水平，b
r
满足该约束子函数的最优状态，e为优化冗余量。
[0063]
虽然传统的图优化方法可以为无人扫地车规划出一条从起点到终点的路径。但是，由于扫地车不能撞到路沿，在将路沿作为障碍物时，由于障碍物约束函数的“推离”作用，规划出来的路径点并不贴合路沿，使得扫地车无法清扫到路沿的垃圾。当减小障碍物约束函数推离作用时，路径点又无法与车辆、人等障碍物保持安全距离。因此，本文在路沿对
车辆状态节点的约束函数中同时添加“推离”和“吸引”作用，使得车辆和路沿保持稳定的贴边行驶距离。
[0064]
如图2所示，路沿在“吸引”清扫盘边界的同时，也对无人扫地车车的车身保持推离作用，当清扫盘边界和路沿恰巧重合时达到最优。
[0065]
如图3所示，本文的图结构在传统节点和边的基础上，添加了斥力和引力共存的“edge”约束边。此约束子函数计算如下：
[0066][0067]
其中为同时具有上下边界的三段式目标函数，
±
d表征了环卫车贴边时允许范围内的距离偏移。
[0068]
为避免车身的左右震荡，将车身和路沿的垂直距离变化程度作为新的稳定性约束边添加到了图中：
[0069][0070]
其中，是垂直距离的均值，n是车辆状态节点的个数，δ是该约束函数的归一化参数。
[0071]
在环卫车辆贴边清扫过程中，在保持贴边的同时还需要适配扫盘和车辆的行驶速度，以达到尽可能高的清扫效率，将此约束添加到图中：
[0072][0073]
其中δ
p
,δ
v
是行驶距离和速度的归一化参数，是车辆状态之间的距离，v
i
,v
s
是车辆状态节点对应的车速和清扫盘的转速。
[0074]
如图4所示，传统图优化路径规划直线贴边情形：距离路沿较远，且出现倒车进入目标点的冗余机动；
[0075]
如图5所示，本发明方法的直线贴边情形：刚好将扫盘贴到路沿，路经点分布均匀稳定；
[0076]
如图6所示，传统图优化路径规划的转弯贴边情形：距离路沿较远，且出现路径点不均匀状况；
[0077]
如图7所示，本发明方法转弯贴边情况：距离路沿较近，转弯流畅稳定。
[0078]
综上所述，本发明采用基于代价函数最小化的图优化方法来研究环卫车辆贴边清扫时的路径规划问题，在对障碍物进行分类并在图中添加稳定贴边约束后，该方法可以让环卫车安全有效地执行贴边清扫任务。
[0079]
在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0080]
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。