CN114964267A - 一种无人牵引车在多任务点环境下的路径规划方法 - Google Patents

Abstract

本发明公开了一种无人牵引车在多任务点环境下的路径规划方法，包括构建当前任务区域的全局栅格地图并划分，同时建立局部栅格地图；构建无人牵引车系统模型，并获取初始状态信息；根据设计得到的通行代价总和确定多任务点的访问顺序，得到最优全局路径；利用人工势场引导RRT路径规划算法规划当前位置到当前目标任务点的局部路径，并优化；根据优化后路径进行追踪，进行激光SLAM建图并更新所在区域的局部栅格地图；判断是否到达目标任务点，若是则等待新任务发布，否则继续规划路径。本发明通过构建全局和局部地图，设计通行代价规划行驶路径，以最小的代价通过的系列任务点，并且能够避免牵引车与拖车和障碍物发生碰撞，提高路径规划的效率。

Description

一种无人牵引车在多任务点环境下的路径规划方法

技术领域

本发明涉及自动驾驶技术领域，特别涉及一种无人牵引车在多任务点环境下的路径规划方法。

背景技术

牵引车具有载货量大、拖车节数灵活改变，能够执行劳动强度大或危险的搬运传送任务等优点。随着无人驾驶技术的发展，无人牵引车逐渐应用在物流运输业等领域。无人牵引车在物流园区的多任务点（如装货点、卸货点、充电桩、维修站）有效规划出合理的、无重复的路线对于提高物流效率，降低物流成本具有重要的意义。

任务区域由于频繁的物流调动，区域环境始终在动态变化，具有高度的不确定性。为了在多任务点环境下进行路径规划，需要构建任务区域的全局栅格地图。另外任务点的优先级、牵引车与拖车之间的转角约束、带拖车的牵引车的避障等都是路径规划需要考虑在内的因素。

现有技术的不足之处在于，现有的无人牵引车的路径规格化方法在多任务点之间执行搬运传送任务时存在规划路径不合理、没有充分考虑到任务点的优先级、不能精确识别目的地，以及重复搜索的现象。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种无人牵引车在多任务点环境下的路径规划方法，以解决上述背景技术中提出的问题。

一种无人牵引车在多任务点环境下的路径规划方法，具体步骤包括：

S1、构建当前任务区域的全局栅格地图并划分为若干区域S _i （i=1,...,N），其中任务点所在区域作为任务区域，同时建立无人牵引车所在区域的局部栅格地图；

S2、构建具有拖车的无人牵引车系统模型，并获取无人牵引车的初始状态信息；

S3、设计多任务点的通行代价并进行计算，根据得到的通行代价总和确定多任务点的访问顺序，得到最优全局路径；

S4、利用改进的人工势场引导双向RRT路径规划算法规划无人牵引车从当前位置到当前目标任务点的局部路径，并进行局部路径优化；

S5、无人牵引车根据优化后的路径进行路径追踪，同时进行激光SLAM建图并同步更新所在区域的局部栅格地图；

S6、判断无人牵引车是否到达目标任务点，若是则在目标任务点作业结束后等待下一组任务发布，否则继续规划当前位置到目标任务点的路径。

作为本发明的进一步的方案：所述S1中的具体步骤包括：

获取任务区域，以任务区域的区域中心作为目标中心建立坐标系，构建全局栅格地图，其中全局栅格地图包括任务点信息和障碍物信息；

将构建的全局栅格地图划分为若干个正方形区域，且每个正方形区域S _i 占据规定数量的栅格，当地图边缘区域的栅格数少于规定数量时，则将该区域视为由障碍物占据的区域，其中任务点所在区域作为任务区域；

通过无人牵引车的感知系统定位车辆当前所在区域，并以无人牵引车为中心，构建所在区域的局部栅格地图，同时将局部栅格地图进行匹配校正，通过坐标转换映射到全局栅格地图中；

设置全局栅格地图中每个正方形区域S _i 的属性参数，所述属性参数包括中心点坐标（S _ix ，S _iy ）、未访问的任务点数量N _Si 、任务点优先级L _Si ，以及障碍物数量O _Si 。

作为本发明的进一步的方案：所述S2中的具体步骤包括：

根据无人牵引车的动力学特性和系统连接方式建立无人牵引车与拖车的系统运动方程；

同时获取无人牵引车的初始状态信息，所述初始状态信息包括初始位置、速度、加速度、朝向、与拖车的初始角度，以及周边障碍物。

作为本发明的进一步的方案：所述S3中的具体步骤包括：

根据通行距离总和最小的原则，以无人牵引车所在区域中心到任务点所在区域中心之间的曼哈顿距离作为距离代价C _dis ；

根据优先访问重要任务点的原则，以任务点的优先级作为优先级代价C _lev ；

根据优先远离障碍物的原则，以任务点所在区域的障碍物数量作为障碍物代价C _obs ；

根据优先通行未访问任务点原则，以区域的未访问的任务点数量作为未访问代价C _vis ；

根据不同类型的代价建立代价函数，得到代价最小的区域S _i 作为下一个待访问的目标区域，所述代价函数为：

J _i ^cost =ω _dis C _dis +ω _lev C _lev +ω _obs C _obs +ω _vis C _vis ；

其中，ω _dis 是距离代价的权重系数，ω _lev 是优先级代价的权重系数，ω _obs 是障碍物代价的权重系数，ω _vis 是未返问代价的权重系数，C _dis 是距离代价，C _lev 是优先级代价，C _vis 是障碍物代价，C _vis 是未访问代价；

根据广度优先遍历算法得到的无人牵引车不重复通过所有任务区域的代价总和Cost为：

；

其中，i为任务区域，N是任务区域总数，f _{breadth first} ()为广度优先遍历算法；

根据广度优先遍历算法计算得到多个Cost，按照代价总和最小的原则确定多任务点的访问顺序，得到最优全局路径。

作为本发明的进一步的方案：所述S4中的具体步骤包括：

S41、首先建立无人牵引车在位置P的总势场值U _tractor 为：

；

S42、再建立拖车在位置P的总势场值U _trailer 为：

；

其中，k为位置P的引力系数，δ为无人牵引车与目标任务点之间的距离；α为增益系数，δ ₁ 为无人牵引车与障碍物之间的距离，δ ₀ 代表牵引车与障碍物的安全距离；d _goal 是目标任务点与障碍物的距离；β为拖车与障碍物的横向距离；D为通行区域的宽度；θ _turn 为牵引车与拖车之间的相对转角，θ _turnmax 为牵引车与拖车之间相对最大安全转角；

S43、得到具有拖车的无人牵引车在位置P的总势场值U _{trailer-trailer} 为：

U _{trailer-trailer} (P)=U _tractor(P)+U _trailer (P)；

S44、初始化随机双向随机树及其参数，所述参数包括初始位置q _init 、目标位置q _goal ，以及双向随机树的生长步长λ；

S45、构造随机树的新节点，以特定概率在全局栅格地图中随机采样得到q _rand ，选择距离q _rand 最近的节点q _near ，在最近的节点q _near 总势场值U _{trailer-trailer} 的引导下，新节点q _new 会靠近目标点方向和偏离障碍物方向，新节点q _new 的计算公式为：

；

其中，q _new 是新节点，q _rand 是随机采样得到的节点，q _near 是距离q _rand 最近的节点，λ是双向随机树的生长步长，U _{trailer-trailer} 是无人牵引车在q _near 的总势场值，q _goal 是目标位置；

S46、判断最近的节点q _near 与新节点q _new 之间的连线有没有障碍物，若有，则舍弃并重新采样，若没有，则继续下一步骤；

S47、构建第二棵树则以第一棵树得到的最近的节点q _near 作为扩展方向，得到q ’ _new ，若没有发生碰撞，则继续向相同的方向扩展第二步，直到扩展失败，扩展第一棵树；

S48、重复执行步骤S45至S47，直到q _new =q’ _new 表示两棵树相连；

S49、利用人工势场引导的双向RRT路径规划算法得到从起点到终点的完整轨迹，对双向搜索随机树进行剪枝处理，删除与路径无关的节点、删除转弯角度大于特定值的点，得到剪枝后的路径，并将剪枝后得到的路径使用贝塞尔曲线及逆行平滑处理，得到平滑的路径。

作为本发明的进一步的方案：所述S5中具体步骤包括：

根据构建的无人牵引车系统模型和得到的优化后的路径进行追踪；

在路径追踪的过程中，利用激光SLAM实时构建无人牵引车周边区域的环境地图，并且同步到局部栅格地图中，更新所在区域S _i 的障碍物数量；

遍历区域S _i 的每一个栅格，若栅格存在任务点，则判断此任务点是否是下一个目标点，若是，则删除该点，并将区域S _i 内未访问的任务点数量N _Si 进行减一。

作为本发明的进一步的方案：所述S6中具体步骤包括：

当无人牵引车到达目标点后，暂停下一步规划，开启视觉检测系统；

利用视觉检测系统与激光雷达三维点云数据进行校对，判断当前点是否为特定任务点，若是则等待作业结束到下一个任务点，若不是则在区域S _i 内遍历所有栅格，找到特定任务点，并规划从当前位置到目标任务点的路径；

判断当前任务点是否是最后一个任务点，若是则完成规划任务，等待下一组规划任务发布，若否则继续规划到下一个任务点的路径。

与现有技术相比，本发明存在以下技术效果：

通过采用上述的技术方案，对获取的任务区域构建全局栅格地图，并进行划分。同时获取无人牵引车所在区域建立局部栅格地图，并与全局栅格地图相互校准和投影障碍物。再建立无人牵引车系统模型，设计通行代价函数得到通行顺序，利用路径规划算法得到到的目标任务点的路径，并进行优化。将全局栅格地图划分为若干区域计算每个区域的访问代价，能够按照最小代价不重复地访问每个区域的任务点，提高无人牵引车的工作效率。同时利用改进的人工势场引导的双向RRT路径规划算法以特定概率选择随机节点，能够提高规划速度。同时解决了多任务点之间执行搬运传送任务时存在规划路径不合理、没有充分考虑到任务点的优先级、不能精确识别目的地，以及重复搜索的问题和现象。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的路径规划方法的整体步骤示意图；

图2为本申请公开的实施例的构建的全局栅格地图；

图3为本申请公开的实施例的全局栅格地图的若干个区域的示意图；

图4为本申请公开的实施例的局部栅格地图投影到全局栅格地图的示意图；

图5为本申请公开的实施例的带有拖车的牵引车的运动学关系示意图；

图6为本申请公开的实施例的带有拖车的牵引车的转弯半径示意图；

图7为本申请公开的实施例的广度优先遍历任务区域中心点的流程图；

图8为本申请公开的实施例的人工势场引导的双向RRT路径该规划算法的流程图；

图9为本申请公开的实施例的随机树构建新节点的示意图；

图10为本申请公开的实施例的人工势场引导的双向RRT算法生成的路径示意图；

图11为本申请公开的实施例的对双向RRT剪枝处理后的路径示意图；

图12为本申请公开的实施例的路径平滑处理后的路径示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，本发明实施例中，一种无人牵引车在多任务点环境下的路径规划方法，具体步骤包括：

本实施例中，基于主要场景是无人牵引车在物流运输业等领域的应用。通过无人牵引车在物流园区的多任务点，多任务点包括如装货点、卸货点、充电桩，以及维修站等，进行有效规划出合理的轨迹，能够有效提供工作效率和降低物流成本。

S1、构建物流园区的当前任务区域的全局栅格地图并将物流园区划分为若干区域S _i （i=1,...,N），其中任务点所在区域作为任务区域，同时建立无人牵引车所在区域的局部栅格地图，具体步骤包括：

获取物流园区的任务区域，以任务区域的区域中心作为目标中心建立坐标系，构建全局栅格地图，其中全局栅格地图包括任务点信息和障碍物信息；

本实施例中，如图2所示，图示为本实施例构建的全局栅格地图的示意图，将获取物流园区的任务区域中心作为全局栅格地图的中心，图中具体是以全局栅格地图的左上角作为坐标原点，向东为x轴正方向，向南为y轴正方向，构建大小为X _g ×Y _g 的全局栅格地图。其中，X _g 为全局栅格地图的东西向长度，Y _g 为全局栅格地图的南北向长度，每个正方形栅格的边长为d _g 。其中，填充矩形表示障碍物，★表示为任务点。

进一步的将构建的全局栅格地图划分为若干个正方形区域，且每个正方形区域S _i 占据规定数量的栅格，当地图边缘区域的栅格数少于规定数量时，则将该区域视为由障碍物占据的区域，其中任务点所在区域作为任务区域；

本实施例中，如图3所示，图示为全局栅格地图划分为若干个区域的示意图，具体是以任务区域中心为基础划分出大小为D _g ×D _g 的正方形区域，进一步划分出周围的正方形区域，直到遇到部分区域栅格长度小于D _g 时，将该区域视为由障碍物占据的区域，其中任务点所在的区域视为任务区域。

通过无人牵引车的感知系统定位车辆当前所在区域，并以无人牵引车为中心，构建所在区域的局部栅格地图，同时将局部栅格地图进行匹配校正，通过坐标转换映射到全局栅格地图中；

本实施例中，感知系统主要包括定位系统、激光雷达、毫米波雷达，以及智能相机等传感器，用于获取当前位置周围区域的障碍物信息以及任务点信息；

如图4所示，图中灰色区域为构建的局部栅格地图，局部栅格地图长为X _i ，方向为车辆的行进方向，宽为Y _i ，方向为垂直于X _i 的方向。将局部栅格地图进行匹配校正，通过坐标转换将地图信息映射到全局栅格地图。

设置全局栅格地图中每个正方形区域S _i 的属性参数，所述属性参数包括中心点坐标（S _ix ，S _iy ）、未访问的任务点数量N _Si 、任务点优先级L _Si ，以及障碍物数量O _Si 。

S2、构建具有拖车的无人牵引车系统模型，并获取无人牵引车的初始状态信息，具体步骤包括：

根据无人牵引车的动力学特性和系统连接方式建立无人牵引车与拖车的系统运动方程；

本实施例中，如图5和图6所示，图示为带有拖车的无人牵引车的运动学关系示意图和转弯半径示意图，具体以一节拖车为例，建立带有拖车的牵引车的运动学关系以及转弯半径模型，具体系统运动方程如下：

设定(x _i ，y _i )表示每节车体轮轴中心点的坐标；θ _i 表示每节车体的方位角，即车体固定坐标系与空间固定坐标系的夹角（其中i = 1时表示牵引车，i = 2时表示拖车）；

首先求解牵引车的x，y方向的速度V _x1 和V _y1 ，以及角速度ω ₁ ，计算公式为：

式中，(x _1， y ₁ ) _、 θ ₁ 分别表示牵引车的坐标与方向角，V _x1 、V _y1 ，以及ω ₁ 分别表示牵引车的x,y方向的速度、角速度，L ₁ 为牵引车驱动轮和转向轮的轮距，v表示牵引车前导向轮的速度；

再求解拖车的x，y方向的速度V _x2 和V _y2 ，以及角速度ω ₂ ，计算公式为：

式中，θ ₁ 和θ ₂ 分别表示牵引车的方向角和拖车的方向角，

表示牵引车转向角，即牵引车导向轮与车体固定坐标系横轴的夹角，L ₁ 为牵引车驱动轮和转向轮的轮距，L ₂ 表示拖车的轮轴到链接点的距离，L _t 表示连接轴的长度，v表示牵引车前导向轮的速度；

求解牵引车与拖车允许的最大相对转角θ _turnmax ，计算公式为：

；

式中，θ ₁ 和θ ₂ 分别表示牵引车的方向角和拖车的方向角，L ₁ 为牵引车驱动轮和转向轮的轮距，W ₁ 表示牵引车的宽度；

根据牵引车驱动轮和转向轮的轮距L ₁ 和得到的最大相对转角θ _turnmax ，求解牵引车的可通行的最小转弯半径r _1min ，计算公式为：

；

求解拖车转弯的半径r ₂ ，计算公式为：

；

式中，r ₁ 表示牵引车转弯半径，L ₁ 为牵引车驱动轮和转向轮的轮距，L ₂ 表示拖车的轮轴到链接点的距离；

式中，W ₁ 表示牵引车的宽度，W ₂ 表示拖车的宽度，r ₁ 表示牵引车转弯半径，r ₂ 表示拖车转弯的半径，R ₁ 表示牵引车转弯的安全半径，R ₂ 表示拖车转弯的安全半径。D _in 表示牵引车转弯时内侧安全距离，D _out 表示牵引车转弯时外侧安全距离，D _in 和D _out 主要用于计算得到通行区域的宽度D。

根据上述运动学关系和转弯半径模型公式计算，从而获取后续运算步骤中所需要的参数；

同时通过车载设备获取无人牵引车的初始状态信息，所述初始状态信息包括初始位置、速度、加速度、朝向、与拖车的初始角度，以及周边障碍物。

例如，本实施例中，通过无人牵引车通过装备的惯导、陀螺仪等设备获得当前无人牵引车的速度、加速度，通过所述感知系统获得初始位置、速度、加速度、朝向，以及周边障碍物等信息，通过角度传感器获得与拖车的初始角度。

S3、设计多任务点的通行代价并进行计算，根据得到的通行代价总和确定多任务点的访问顺序，得到最优全局路径，具体步骤包括：

本实施例中，采用基于多种不同的原则条件下，进行多任务点的通行代价设计；

具体为，根据物流园区多任务点之间的通行距离总和最小的原则，以无人牵引车所在区域中心到任务点所在区域中心之间的曼哈顿距离作为距离代价C _dis ；

根据物流园区多任务点的优先访问重要任务点的原则，以任务点的优先级作为优先级代价C _lev ；本实施例中，会预先设置经过任务点的优先级，具体是依据物流工作实际情况判定；

根据优先远离障碍物的原则，以任务点所在区域的障碍物数量作为障碍物代价C _obs ；因为当判定任务点的障碍物过多，导致产生不必要的成本，因此加入障碍物代价；

根据优先通行未访问任务点原则，以区域的未访问的任务点数量作为未访问代价C _vis ；为了避免其中可能产生多次未访问的任务点，以未访问代价减少当前多次未访问的可能性；

依据上述距离代价C _dis 、优先级代价C _lev 、障碍物代价C _obs ，以及未访问代价C _vis 建立代价函数，并选取其中代价最小的区域S _i 作为下一个待访问的目标区域，所述代价函数为：

J _i ^cost =ω _dis C _dis +ω _lev C _lev +ω _obs C _obs +ω _vis C _vis ；

其中，ω _dis 是距离代价的权重系数，ω _lev 是优先级代价的权重系数，ω _obs 是障碍物代价的权重系数，ω _vis 是未返问代价的权重系数，C _dis 是距离代价，C _lev 是优先级代价，C _vis 是障碍物代价，C _vis 是未访问代价；

根据广度优先遍历算法得到的无人牵引车不重复通过所有任务区域的代价总和Cost为：

；

其中，i为任务区域，N是任务区域总数，f _{breadth first} ()为广度优先遍历算法；

根据广度优先遍历算法计算得到多个Cost，按照代价总和最小的原则确定多任务点的访问顺序，得到最优全局路径，具体为，分别将每一个任务点作为初始点，通过广度优先算法遍历所有的任务点，会得到N组Cost，按照代价总和最小的原则确认多个任务点的访问顺序，即为无人牵引车的最优全局路径。

本实施例中，如图7所示，图示为广度优先遍历任务区域中心点的流程图，无人牵引车通过感知系统获取周围特定范围内的任务点，并将其加入队列。无人牵引车根据当前的位置分别计算出周围任务区域的通行代价总和，选择代价总和最小的任务区域作为当前目标点V0，将该点V0入队，判断队列是否为空，如果为空，结束访问；如果不为空，将队列的对头V出队，找到对头V相邻的任务点N，判断N是否存在，如果点N不存在，判断队列是否为空；如果N存在，判断N是否被访问过，没有访问过就访问N任务点，并将标志置为已访问，将N入队，搜索N的相邻节点，重复此过程，直到无人牵引车访问完所有的任务点为止。

S4、利用改进的人工势场引导双向RRT路径规划算法规划无人牵引车从当前位置到当前目标任务点的局部路径，并进行局部路径优化，具体步骤包括：

S41、首先建立无人牵引车在位置P的总势场值U _tractor 为：

；

公式的说明：

无人牵引车在P点位置的势场计算由两部分组成：

第一部分是目标任务点对于牵引车的引力场计算：当牵引车与目标任务点之间的距离小于障碍物与目标点的距离时，即牵引车与目标点之间没有障碍物，引力势场通过δ小于等于d _goal 部分的公式求解，否则通过δ大于d _goal 部分的公式求解，具体为目标任务点的引力场减去障碍物的斥力场；

第二部分是障碍物对牵引车的斥力场计算：当牵引车与障碍物之间的距离大于牵引车与障碍物的安全距离时，障碍物对牵引车不构成危险，斥力场为0，否则通过δ ₁ 小于等于δ ₀ 部分的公式求解。

S42、再建立拖车在位置P的总势场值U _trailer 为：

；

公式的说明：

拖车在点P位置的势场计算由两部分组成：

第一部分是障碍物对拖车的斥力场：当拖车与障碍物的横向距离小于拖车通行的宽度，对拖车安全造成了影响，产生一个斥力场：通过β小于等于D的公式部分求解，否则为0；

第二部分是牵引车与拖车的转角计算：当牵引车与拖车的相对转角小于允许的最大安全转角的时候，计算斥力场：通过θ _turn 小于等于θ _turnmax 的公式部分求解，否则为0。

其中，k为位置P的引力系数，δ为无人牵引车与目标任务点之间的距离；α为增益系数，δ ₁ 为无人牵引车与障碍物之间的距离，δ ₀ 代表牵引车与障碍物的安全距离；d _goal 是目标任务点与障碍物的距离；β为拖车与障碍物的横向距离；D为通行区域的宽度；θ _turn 为牵引车与拖车之间的相对转角，θ _turnmax 为牵引车与拖车之间相对最大安全转角；

S43、得到具有拖车的无人牵引车在位置P的总势场值U _{trailer-trailer} 为：

U _{trailer-trailer} (P)=U _tractor(P)+U _trailer (P)；

总势场值能够保证生成的轨迹符合牵引车的运动学模型，并且可以引导轨迹偏向目标点，远离障碍物点。

如图8所示，图示为人工势场引导的双向RRT路径规划算法的流程图，算法的主要流程步骤包括：

S44、首先初始化随机双向随机树及其参数，所述参数包括初始位置q _init 、目标位置q _goal ，以及双向随机树的生长步长λ；

具体为，根据无人牵引车的初始状态与第一个目标任务点初始化两棵双向随机树G1，G2，车辆的初始位置作为G1的q _init 、G2的q _goal 目标任务点的目标位置作为G2的q _init 、G1的q _goal ，设定双向随机树的生长步长为λ；

S45、构造随机树的新节点。如图9所示，图示为随机树构造新节点的示意图，以特定概率在全局栅格地图中随机采样得到q _rand ，选择距离q _rand 最近的节点q _near ，从q _near 到q _rand 方向延伸λ长度得到一个未经引导的q _{new_init} ，计算q _near 这一点的总势场值U _{trailer-trailer，} 在该总势场值的引导下q _{new_init} 会靠近目标点方向、偏离障碍物方向，扩展生成节点q _new ，q _new 的计算公式为：

；

其中，q _new 是新节点，q _rand 是随机采样得到的节点，q _near 是距离q _rand 最近的节点，λ是双向随机树的生长步长，U _{trailer-trailer} 是无人牵引车在q _near 的总势场值，q _goal 是目标位置；

S46、开展碰撞检测：判断q _near 与q _new 之间的连线有没有障碍物，若没有，则继续下一步骤；若有障碍物，继续判断随机树G2的节点数是否多于G1，如果多余G1，则交换G1于G2的节点并重新在全局地图采样，否则在全局地图重新采样得到采样节点。将作为合理的新节点连接父节点q _near 与新节点q _new 。

S47、第二棵树G2以第一棵树G1得到的q _new 作为扩展目标寻找最近点，按照上述方法得到q’ _new ，若没有发生碰撞，则继续往相同的方向扩展第二步，直到扩展失败，扩展第一棵树G1；

S48、重复执行S45至S47，直到q _new =q’ _new 表示两棵数相连，具体实施方式中，再通过回溯法提取路径；

S49、如图10所示，图示为双向RRT路径规划算法生成的完整轨迹，具体是利用人工势场引导的双向RRT路径规划算法得到从起点到终点的完整轨迹；

如图11所示，图是为剪枝处理后的路径，具体为对双向搜索随机树进行剪枝处理，删除与路径无关的节点、删除转弯角度大于特定值的点，得到剪枝后的路径；

最后，将剪枝后得到的路径使用贝塞尔曲线及逆行平滑处理，得到优化后平滑的路径。

如图12所示，图示为优化后平滑的路径的示意图。

S5、无人牵引车根据优化后的路径进行路径追踪，同时进行激光SLAM建图并同步更新所在区域的局部栅格地图，具体步骤包括：

根据步骤S2中构建的无人牵引车系统模型和步骤S4中得到的优化后平滑的路径进行追踪；

在对优化后平滑的路径进行追踪的过程中，本实施例中，通过采用激光SLAM进行实时构建无人牵引车周边区域的环境地图，并且同步到之前建立的局部栅格地图中，同时更新当前所在区域S _i 的障碍物数量；

遍历区域S _i 的每一个栅格，此时进行栅格是否存在任务点判断，若栅格存在任务点，则判断此任务点是否是下一个目标点，若是，则删除该点，并将该区域内的未访问的任务点数量N _Si 进行数量减一。

S6、判断无人牵引车是否到达目标任务点，若是则在目标任务点作业结束后等待下一组任务发布，否则继续规划当前位置到目标任务点的路径，具体步骤包括：

本实施例中，在无人牵引车到达目标点后，需要暂停下一步规划，并且无人牵引车自动开启视觉检测系统，对周围环境进行检测；

通过利用视觉检测系统检测到的数据信息与激光雷达三维点云数据进行校对，判断当前点是否为特定任务点，若是则等待作业结束到下一个任务点，若不是则在区域S _i 内遍历所有栅格，找到特定任务点，并规划从当前位置到目标任务点的路径；

判断当前任务点是否是最后一个任务点，若是则完成规划任务，等待下一组规划任务发布，若否则继续规划到下一个任务点的路径。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。

Claims (7)

1.一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，具体步骤包括：

S1、构建当前任务区域的全局栅格地图并划分为若干区域S _i （i=1,...,N），其中任务点所在区域作为任务区域，同时建立无人牵引车所在区域的局部栅格地图；

S2、构建具有拖车的无人牵引车系统模型，并获取无人牵引车的初始状态信息；

S3、设计多任务点的通行代价并进行计算，根据得到的通行代价总和确定多任务点的访问顺序，得到最优全局路径；

S4、利用改进的人工势场引导双向RRT路径规划算法规划无人牵引车从当前位置到当前目标任务点的局部路径，并进行局部路径优化；

S5、无人牵引车根据优化后的路径进行路径追踪，同时进行激光SLAM建图并同步更新所在区域的局部栅格地图；

S6、判断无人牵引车是否到达目标任务点，若是则在目标任务点作业结束后等待下一组任务发布，否则继续规划当前位置到目标任务点的路径。

2.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S1中的具体步骤包括：

获取任务区域，以任务区域的区域中心作为目标中心建立坐标系，构建全局栅格地图，其中全局栅格地图包括任务点信息和障碍物信息；

将构建的全局栅格地图划分为若干个正方形区域，且每个正方形区域S _i 占据规定数量的栅格，当地图边缘区域的栅格数少于规定数量时，则将该区域视为由障碍物占据的区域，其中任务点所在区域作为任务区域；

通过无人牵引车的感知系统定位车辆当前所在区域，并以无人牵引车为中心，构建所在区域的局部栅格地图，同时将局部栅格地图进行匹配校正，通过坐标转换映射到全局栅格地图中；

设置全局栅格地图中每个正方形区域S _i 的属性参数，所述属性参数包括中心点坐标（S _ix ，S _iy ）、未访问的任务点数量N _Si 、任务点优先级L _Si ，以及障碍物数量O _Si 。

3.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S2中的具体步骤包括：

根据无人牵引车的动力学特性和系统连接方式建立无人牵引车与拖车的系统运动方程；

同时获取无人牵引车的初始状态信息，所述初始状态信息包括初始位置、速度、加速度、朝向、与拖车的初始角度，以及周边障碍物。

4.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S3中的具体步骤包括：

根据通行距离总和最小的原则，以无人牵引车所在区域中心到任务点所在区域中心之间的曼哈顿距离作为距离代价C _dis ；

根据优先访问重要任务点的原则，以任务点的优先级作为优先级代价C _lev ；

根据优先远离障碍物的原则，以任务点所在区域的障碍物数量作为障碍物代价C _obs ；

根据优先通行未访问任务点原则，以区域的未访问的任务点数量作为未访问代价C _vis ；

根据不同类型的代价建立代价函数，得到代价最小的区域S _i 作为下一个待访问的目标区域，所述代价函数为：

J _i ^cost =ω _dis C _dis +ω _lev C _lev +ω _obs C _obs +ω _vis C _vis ；

其中，ω _dis 是距离代价的权重系数，ω _lev 是优先级代价的权重系数，ω _obs 是障碍物代价的权重系数，ω _vis 是未返问代价的权重系数，C _dis 是距离代价，C _lev 是优先级代价，C _vis 是障碍物代价，C _vis 是未访问代价；

根据广度优先遍历算法得到的无人牵引车不重复通过所有任务区域的代价总和Cost为：

；

其中，i为任务区域，N是任务区域总数，f _{breadth first} ()为广度优先遍历算法；

根据广度优先遍历算法计算得到多个Cost，按照代价总和最小的原则确定多任务点的访问顺序，得到最优全局路径。

5.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S4中的具体步骤包括：

S41、首先建立无人牵引车在位置P的总势场值U _tractor 为：

；

S42、再建立拖车在位置P的总势场值U _trailer 为：

；

其中，k为位置P的引力系数，δ为无人牵引车与目标任务点之间的距离；α为增益系数，δ ₁ 为无人牵引车与障碍物之间的距离，δ ₀ 代表牵引车与障碍物的安全距离；d _goal 是目标任务点与障碍物的距离；β为拖车与障碍物的横向距离；D为通行区域的宽度；θ _turn 为牵引车与拖车之间的相对转角，θ _turnmax 为牵引车与拖车之间相对最大安全转角；

S43、得到具有拖车的无人牵引车在位置P的总势场值U _{trailer-trailer} 为：

U _{trailer-trailer} (P)=U _tractor(P)+U _trailer (P)；

S44、初始化随机双向随机树及其参数，所述参数包括初始位置q _init 、目标位置q _goal ，以及双向随机树的生长步长λ；

S45、构造随机树的新节点，以特定概率在全局栅格地图中随机采样得到q _rand ，选择距离q _rand 最近的节点q _near ，在最近的节点q _near 总势场值U _{trailer-trailer} 的引导下，新节点q _new 会靠近目标点方向和偏离障碍物方向，新节点q _new 的计算公式为：

；

其中，q _new 是新节点，q _rand 是随机采样得到的节点，q _near 是距离q _rand 最近的节点，λ是双向随机树的生长步长，U _{trailer-trailer} 是无人牵引车在q _near 的总势场值，q _goal 是目标位置；

S46、判断最近的节点q _near 与新节点q _new 之间的连线有没有障碍物，若有，则舍弃并重新采样，若没有，则继续下一步骤；

S47、构建第二棵树则以第一棵树得到的最近的节点q _near 作为扩展方向，得到q’ _new ，若没有发生碰撞，则继续向相同的方向扩展第二步，直到扩展失败，扩展第一棵树；

S48、重复执行S45至S47，直到q _new =q’ _new 表示两棵树相连；

S49、利用人工势场引导的双向RRT路径规划算法得到从起点到终点的完整轨迹，对双向搜索随机树进行剪枝处理，删除与路径无关的节点、删除转弯角度大于特定值的点，得到剪枝后的路径，并将剪枝后得到的路径使用贝塞尔曲线及逆行平滑处理，得到平滑的路径。

6.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S5中具体步骤包括：

根据构建的无人牵引车系统模型和得到的优化后的路径进行追踪；

在路径追踪的过程中，利用激光SLAM实时构建无人牵引车周边区域的环境地图，并且同步到局部栅格地图中，更新所在区域S _i 的障碍物数量；

遍历区域S _i 的每一个栅格，若栅格存在任务点，则判断此任务点是否是下一个目标点，若是，则删除该点，并将区域S _i 内未访问的任务点数量N _Si 进行减一。

7.根据权利要求1所述一种无人牵引车在多任务点环境下的路径规划方法，其特征在于，所述S6中具体步骤包括：

当无人牵引车到达目标点后，暂停下一步规划，开启视觉检测系统；

利用视觉检测系统与激光雷达三维点云数据进行校对，判断当前点是否为特定任务点，若是则等待作业结束到下一个任务点，若不是则在区域S _i 内遍历所有栅格，找到特定任务点，并规划从当前位置到目标任务点的路径；

判断当前任务点是否是最后一个任务点，若是则完成规划任务，等待下一组规划任务发布，若否则继续规划到下一个任务点的路径。

Images