CN117346797A - 一种基于蒙特卡洛搜索树的b样条路径规划方法 - Google Patents

Abstract

本发明公开了一种基于蒙特卡洛搜索树的B样条路径规划方法，属于路径规划领域，涉及蒙特卡洛搜索树，通过起始点与目标点的位置坐标以及角度生成起始方向控制点以及目标方向控制点；且分别设置在起始点的起点角度延长线以及目标点的终点角度延长线上；设置有安全保守距离，以安全保守距离生成以起始方向控制点、目标方向控制点为圆心的n层圆点候选点。将起始方向控制点的圆点候选点设置为n层的原因是单纯靠一个首尾方向控制点很多时候并不能解决方向的约束，比如起点和终点方向同向，位置相反的情况；可以最大限度的保持起终点的方向控制，适应更大规模的情况。

Description

一种基于蒙特卡洛搜索树的B样条路径规划方法

技术领域

本发明属于路径规划领域，涉及蒙特卡洛搜索树，具体是一种基于蒙特卡洛搜索树的B样条路径规划方法。

背景技术

由于仓储环境复杂，视觉检测托盘得到的目标点与叉车自身之间的环境可能存在着无法自旋转的情况。这就需要对叉车的起点方向进行约束，并且为了消除安全隐患，达到托盘目标点的整个路径需要尽可能平滑且符合叉车运动特性等优点。

目前自动驾驶叉车生成的路径的方法主要有两种，一种是五次多项式曲线生成之后进行拟合的方法和Beizer曲线的方法。

五次多项式方法，其主要思想是最小化Jerk，通过六个边界条件建立五次多项式生成满足条件的轨迹，再进行拟合成样条的方式。此方法生成的路径无法使得起终点的方向有更好的约束，并且拟合过程中会有精度损失。

Bezier曲线方法，是通过设置一定的距离阈值，来迭代起终点的方向控制点。以曲率为代价计算最优的曲线。由于bezier曲线为4个控制点，其中可以变动的两个方向控制点无法很好的表达需要生成的曲线，会导致很多地方曲率较大的问题。

为此，本发明提出一种基于蒙特卡洛搜索树的B样条路径规划方法。

发明内容

本申请的目的是提供一种基于蒙特卡洛搜索树的B样条路径规划方法。

为实现上述目的，本申请提供了一种基于蒙特卡洛搜索树的B样条路径规划方法，包括以下步骤：

获取起始点StartPoint、起点角度startAngle、目标点GoalPoint以及终点角度goalAngle；

生成起始方向控制点startDirectionCP以及目标方向控制点goalDirectionCP；

设置安全保守距离safeDist，以安全保守距离safeDist生成以起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP为圆心的多层圆点候选点；

连接起始方向控制点startDirectionCP和目标方向控制点goalDirectionCP，设置间隔距离，生成中间候选点；

选取任一起始方向控制点startDirectionCP的圆点候选点为起点控制点（父节点），获取当前起点控制点（父节点）的当前状态，其中，当前状态包括当前起点控制点（父节点）的层级、访问次数、以及动作列表；

判断当前起点控制点（父节点）的所有移动动作是否全部执行；

当全部执行时，获取下一层控制点（子节点）的奖励值UCB index，其中的奖励值UCB index的计算公式为：

UCB index= average reward + sqrt((2 × log(total visits)) / nodevisits)

其中，average reward为下一层控制点（子节点）的平均奖励；total visits为所有下一层控制点（子节点）的总访问次数；node visits为该下一层控制点（子节点）的访问次数；

设置模拟深度s，并依次在当前起点控制点（父节点）的后s层选取s次控制点，并依次获取计算获取奖励值；

将该奖励值累加到下一层控制点（子节点）中，累加访问次数，并回溯起点控制点（父节点）进行相同的操作，直到达到起始点；

设定循环次数，获取最优控制点；并设置模拟深度为总候选点的层数；对过目标点的状态节点进行回溯获得所有连接的控制点，完成路径规划。

优选地，起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP分别设置在起始点的起点角度延长线以及目标点的终点角度延长线上；

且起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的直线最大距离DirectionDist为起始点StartPoint和目标点GoalPoint之间欧氏距离DistSG的1/3，即：

DirectionDist≤DistSG/3。

优选地，起始方向控制点startDirectionCP的圆点候选点和目标方向控制点goalDirectionCP的圆点候选点均为等间距圆采样点；

每层半径增量：

deltaRadius=(DirectionDist – safeDist)/n。

优选地，所述起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的圆点候选点层数不一致。

优选地，中间候选点为多层，且总候选点的层数为中间候选点的层数加起始方向控制点startDirectionCP的圆点候选点层数加目标方向控制点goalDirectionCP的圆点候选点层数。

优选地，还包括：

获取若干障碍物的位置，并设定阻碍距离，以障碍物为圆心、阻碍距离为半径划定若干剔除区域，当候选点位于剔除区域内时，则删除候选点。

优选地，当起点控制点的移动动作未全部执行时，创建一个新子状态节点，其子状态节点包含下一层连接的控制点的位置、层数等。并将其保存到当前起点控制点（父节点）的子节点容器中，并以此起点控制点（父节点）中删除此动作，返回当前起点控制点（父节点）。

与现有技术相比，本发明的有益效果是：

1．本发明提出的路径规划方法主要包括候选点生成，通过起始点与目标点的位置坐标以及角度生成起始方向控制点以及目标方向控制点；且分别设置在起始点的起点角度延长线以及目标点的终点角度延长线上；设置有安全保守距离，以安全保守距离生成以起始方向控制点、目标方向控制点为圆心的多层圆点候选点。将起始方向控制点的圆点候选点设置为n层的原因是，单纯靠一个首尾方向控制点很多时候并不能解决方向的约束，比如起点和终点方向同向，位置相反的情况。可以最大限度的保持起终点的方向控制，适应更大规模的情况，比如起点和终点相离很远。

2．灵活度高，可以适应更多场景，搜索算法无需启发函数也能很快的得到结果。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对现有技术和实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明起始点与目标点的位置关系图；

图2为本发明中圆点候选点的示意图；

图3为本发明起始点和目标点方向同向，位置相反的情况示意图。

图4为本发明中所有候选点的示意图；

图5为三次bezier曲线示意图；

图6为本发明基于蒙特卡洛搜索树的B样条路径规划方法流程图。

具体实施方式

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

具体请参照图1-6，一种基于蒙特卡洛搜索树的B样条路径规划方法，包括以下步骤：

如图1所示，获取起始点与目标点，并标记起始点为StartPoint、目标点为GoalPoint；同时获取起始点的起点角度与目标点的终点角度，并标记起始点的起点角度为startAngle、目标点的终点角度为goalAngle；

参照图2，通过起始点与目标点的位置坐标以及角度生成起始方向控制点startDirectionCP以及目标方向控制点goalDirectionCP，并将起始方向控制点标记为startDirectionCP、目标方向控制点标记为goalDirectionCP；

其中需要说明的是，起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP分别设置在起始点的起点角度延长线以及目标点的终点角度延长线上；

且起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的直线最大距离DirectionDist为起始点StartPoint和目标点GoalPoint之间欧氏距离DistSG的1/3，即：

DirectionDist≤DistSG/3

设置安全保守距离safeDist，以安全保守距离safeDist生成以起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP为圆心的多层圆点候选点。

需要说明的是，每层半径增量：

deltaRadius=(DirectionDist – safeDist)/n；

其中，n为层数；

且，起始方向控制点startDirectionCP的圆点候选点和目标方向控制点goalDirectionCP的圆点候选点均为等间距圆采样点；以及所述起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的圆点候选点层数可以不一致；

在本申请中，如图3，起始方向控制点startDirectionCP的圆点候选点设置为n层的原因是，单纯靠一个首尾方向控制点很多时候并不能解决方向的约束，比如起点和终点方向同向，位置相反的情况。

具体请参照图4，连接起始方向控制点startDirectionCP和目标方向控制点goalDirectionCP，设置间隔距离，在起始方向控制点startDirectionCP和目标方向控制点goalDirectionCP的连接线上生成中间候选点；

其中，中间候选点为多层，且总候选点的层数为中间候选点的层数加起始方向控制点startDirectionCP的圆点候选点层数加目标方向控制点goalDirectionCP的圆点候选点层数；

获取若干障碍物的位置，并设定阻碍距离，以障碍物为圆心、阻碍距离为半径划定若干剔除区域，当候选点位于剔除区域内时，则删除候选点；

选取任一起始方向控制点startDirectionCP的圆点候选点为起点控制点（父节点），获取当前起点控制点（父节点）的当前状态，其中，当前状态包括当前起点控制点（父节点）的层级、访问次数、以及动作列表；

需要说明的是，动作列表为当前起点控制点（父节点）到下一层控制点（子节点）的所有移动动作；

判断当前起点控制点（父节点）的所有移动动作是否全部执行；

当全部执行时，获取下一层控制点（子节点）的奖励值UCB index，其中的奖励值UCB index的计算公式为：

UCB index= average reward + sqrt((2 × log(total visits)) / nodevisits)

其中，average reward为下一层控制点（子节点）的平均奖励；total visits为所有下一层控制点（子节点）的总访问次数；node visits为该下一层控制点（子节点）的访问次数；

当起点控制点的移动动作未全部执行时，创建一个新子状态节点，其子状态节点包含下一层连接的控制点的位置、层数等。并将其保存到当前起点控制点（父节点）的子节点容器中，并以此起点控制点（父节点）中删除此动作，返回当前起点控制点（父节点）。

设置模拟深度s，并依次在当前起点控制点（父节点）的后s层选取s次控制点，并依次获取计算获取奖励值；

将该奖励值累加到下一层控制点（子节点）中，累加访问次数，并回溯起点控制点（父节点）进行相同的操作，直到达到起始点；

设定循环次数，获取最优控制点；并设置模拟深度为总候选点的层数；对过目标点的状态节点进行回溯获得所有连接的控制点，完成路径规划；

其中，需要进行说明的是，奖励值为控制点与控制点连接的bezier曲线曲率打分；

如图5所示，图中曲线为三次bezier曲线，因为中间各层控制点方向都是知道的，都是指向下一层的方向。根据方向就可以很简单的得到一个四个控制点的bezier曲线，通过bezier曲线的曲率变化率和当做此次连接的代价打分即为奖励值。

上述公式均是去除量纲取其数值计算，公式是由采集大量数据进行软件模拟得到最接近真实情况的一个公式，公式中的预设参数和预设阈值由本领域的技术人员根据实际情况设定或者大量数据模拟获得。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其他实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (5)

1.一种基于蒙特卡洛搜索树的B样条路径规划方法，其特征在于，包括以下步骤：

获取起始点StartPoint、起点角度startAngle、目标点GoalPoint以及终点角度goalAngle；

生成起始方向控制点startDirectionCP以及目标方向控制点goalDirectionCP；

其中，起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP分别设置在起始点的起点角度延长线以及目标点的终点角度延长线上；

且起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的直线最大距离DirectionDist为起始点StartPoint和目标点GoalPoint之间欧氏距离DistSG的1/3，即：

DirectionDist≤DistSG/3；

设置安全保守距离safeDist，以安全保守距离safeDist生成以起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP为圆心的多层圆点候选点；

连接起始方向控制点startDirectionCP和目标方向控制点goalDirectionCP，设置间隔距离，生成中间候选点；

选取任一起始方向控制点startDirectionCP的圆点候选点为起点控制点即父节点，获取当前父节点的当前状态，其中，当前状态包括当前父节点的层级、访问次数、以及动作列表；

判断当前父节点的所有移动动作是否全部执行；

当起点控制点的移动动作未全部执行时，创建一个新子状态节点，其子状态节点包含下一层连接的控制点的位置、层数；并将其保存到当前父节点的子节点容器中，并以此父节点中删除此动作，返回当前父节点；

当全部执行时，获取下一层控制点即子节点的奖励值UCB index，其中的奖励值UCBindex的计算公式为：

UCB index= average reward + sqrt((2× log(total visits)) / node visits)

其中，average reward为子节点的平均奖励；total visits为所有子节点的总访问次数；node visits为该子节点的访问次数；

设置模拟深度s，并依次在当前父节点的后s层选取s次控制点，并依次获取计算获取奖励值；

将该奖励值累加到子节点中，累加访问次数，并回溯父节点进行相同的操作，直到达到起始点；

设定循环次数，获取最优控制点；并设置模拟深度为总候选点的层数；对对过目标点的状态节点进行回溯获得所有连接的控制点，完成路径规划。

2.如权利要求1所述的一种基于蒙特卡洛搜索树的B样条路径规划方法，其特征在于，起始方向控制点startDirectionCP的圆点候选点和目标方向控制点goalDirectionCP的圆点候选点均为等间距圆采样点；

每层半径增量：

deltaRadius=(DirectionDist – safeDist)/n；

其中n为层数。

3.如权利要求3所述的一种基于蒙特卡洛搜索树的B样条路径规划方法，其特征在于，所述起始方向控制点startDirectionCP、目标方向控制点goalDirectionCP的圆点候选点层数不一致。

4.如权利要求1所述的一种基于蒙特卡洛搜索树的B样条路径规划方法，其特征在于，中间候选点为多层，且总候选点的层数为中间候选点的层数加起始方向控制点startDirectionCP的圆点候选点层数加目标方向控制点goalDirectionCP的圆点候选点层数。

5.如权利要求4所述的一种基于蒙特卡洛搜索树的B样条路径规划方法，其特征在于，还包括：

获取若干障碍物的位置，并设定阻碍距离，以障碍物为圆心、阻碍距离为半径划定若干剔除区域，当候选点位于剔除区域内时，则删除候选点。