CN118068843B - 一种基于参考轨迹优化的AStar路径规划方法及设备 - Google Patents

Abstract

本发明公开了一种基于参考轨迹优化的AStar路径规划方法及设备，其中，基于参考轨迹优化的AStar路径规划方法，通过选取起点S和目标点G以及在启发信息中加入参考轨迹信息，使AStar规划出的路径能够尽量贴合原参考轨迹；从而使无人集卡能够很好的按照参考轨迹行驶，充分发挥原参考轨迹的作用。

Description

一种基于参考轨迹优化的AStar路径规划方法及设备

技术领域

本发明涉及无人集卡局部路径规划技术领域，特别是涉及一种基于参考轨迹优化的AStar路径规划方法及设备。

背景技术

 港口作为交通运输的枢纽，在促进国际贸易和地区发展中起着举足轻重的作用，全球贸易中约90%的贸易由海运承载，作业效率对于港口至关重要。传统的无人集卡通常是根据TMC下发的参考轨迹控制车辆行驶至目的地进行作业。

现有的无人集卡在行驶过程中，自驾***会根据参考轨迹、车道和障碍物等信息进行局部路径规划，但在没有车道信息或在路***汇处可能出现局部路径规划失败的情况。 A*(以下为A Star)算法是局部路径规划领域常用的路径查找和图形遍历算法，它有较好的性能和准确度。

传统的A Star规划通常是在考虑障碍物情况下，搜索出一条从起点到终点的无碰撞路径。A Star规划主要考虑的是与障碍物的安全距离以及离目标点的最短路径，在实际路径规划中存在规划出的局部路径即使在没有障碍物的情况下也会偏离参考轨迹的问题，使无人集卡不能很好的按照参考轨迹行驶，无法充分发挥参考轨迹的作用。

发明内容

为解决上述技术问题，本发明提供一种基于参考轨迹优化的AStar路径规划方法及设备，其中，基于参考轨迹优化的AStar路径规划方法，应用于无人集卡上的车载终端，所述车载终端与障碍物信息获取模块通讯连接；所述车载终端用于获取无人集卡位置信息、障碍物信息和参考轨迹信息，以及根据障无人集卡位置信息、障碍物信息对无人集卡进行AStar路径规划；

所述方法步骤具体如下：

S10、获取当前无人集卡的位置、障碍物位置和参考轨迹信息，并将当前无人集卡的位置设为AStar搜索的起点S；

S20、根据当前无人集卡的位置、障碍物位置和参考轨迹信息，选取参考轨迹附近的障碍物为障碍物子集；

S30、在障碍物子集中选取距离起点S最远的障碍物投影在参考轨迹上作为原点，将该原点沿参考轨迹延伸预设距离至G点，以G点为AStar搜索的目标点；

S40、根据起点S和目标点G确定AStar的搜索区域；

S50、搜索区域栅格化，将栅格到目标点G的欧式距离作为该栅格的代价，若该栅格存在障碍物，则增大该栅格的代价；并将搜索区域中各个栅格的代价作为第一启发信息；

S60、降低搜索区域中参考轨迹上的栅格的代价作为第一参考轨迹代价，将第一参考轨迹代价作为第二启发信息；

S70、根据第一启发信息和第二启发信息进行AStar路径规划。

优选的，所述步骤S60中获取第一参考轨迹代价的方法如下：将起点S投影在参考轨迹上，从起点S的投影点到G点的参考轨迹上选取若干节点；在搜索区域中分别找到距离各个节点最近的栅格作为参考轨迹栅格，将参考轨迹栅格的代价乘以系数k₁(0＜k₁＜1)作为第一参考轨迹代价。

优选的，所述步骤S60还包括：S61、将与参考轨迹栅格相邻的栅格作为参考轨迹相邻栅格；将参考轨迹相邻栅格的代价乘以系数k₂作为第二参考轨迹代价，其中，0＜k₂＜1；将第一参考轨迹代价和第二参考轨迹代价共同作为第二启发信息。优选的，将距离参考轨迹栅格小于D₁距离的栅格作为参考轨迹相邻栅格；计算参考轨迹相邻栅格与其相邻的参考轨迹栅格的距离D₂，k₂=(D₁距离/D₂距离)*P+k₁；其中，P为离参考轨迹的距离权重，0＜P＜1-k₁；0m＜D₁距离≤2m。

优选的，以起点S在参考轨迹的投影点为坐标零点，参考轨迹方向为X轴，垂直于参考轨迹方向为Y轴；

所述步骤S40中的搜索区域在X轴的范围为[min(X_s，X_G) - off_{_X}，max(X_s，X_G)]，在Y轴的范围为[min(Y_s，Y_G) - off_{_Y}，max(Y_s，Y_G) + off_{_Y}]，0＜off_{_X}≤30，0＜off_{_Y}≤15；其中，起点S的坐标为（X_s, Y_s）；目标点G的坐标为（X_G, Y_G），off_{_X}为搜索区域在X轴上的偏移量；off_{_Y}为搜索区域在Y轴上的偏移量。

优选的，所述步骤S50还包括：S51、对搜索区域中的栅格建立KD-Tree；建立KD-Tree的方法具体如下：

使搜索区域中的每个栅格代表空间中的一个区域，该区域具有唯一的坐标；

根据栅格的坐标确定该栅格在KD-Tree中的节点位置，并为该节点上的栅格分配唯一的ID；

在所述节点中存储父节点ID、子节点ID信息以及当前节点的坐标、代价和ID。

优选的，所述步骤S60中获取第一参考轨迹代价时，在搜索区域中通过KD-Tree 搜索分别找到距离各个节点最近的栅格作为参考轨迹栅格。

本发明另外提供一种计算机可读介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上任意所述的基于参考轨迹优化的AStar路径规划方法。

本发明还提供一种基于参考轨迹优化的AStar路径规划设备，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上任意所述的基于参考轨迹优化的AStar路径规划方法。

本发明提供的基于参考轨迹优化的AStar路径规划方法，通过选取起点S和目标点G以及在启发信息中加入参考轨迹信息，使AStar规划出的路径能够尽量贴合原参考轨迹；从而使无人集卡能够很好的按照参考轨迹行驶，充分发挥原参考轨迹的作用。

附图说明

图1为本发明提供的基于参考轨迹优化的AStar路径规划方法流程图；

图2为障碍物子集选取示意图；

图3为搜索区域确定图；

图4为根据欧式距离确定的搜索区域的启发信息地图；

图5为降低搜索区域中参考轨迹上的栅格代价后的启发信息地图；

图6为降低搜索区域中参考轨迹相邻区域代价后的发信息地图；

图7为规划出的路径一实施例图；

图8为规划出的路径另一实施例图；

图9为本发明提供的基于参考轨迹优化的AStar路径规划设备示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施例，进一步阐述本发明，但下述实施例仅仅为本发明的优选实施例，并非全部，基于实施方式中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得其它实施例，都属于本发明的保护范围，下述实施例中的实验方法，如无特殊说明，均为常规方法，下述实施例中所用的材料、试剂等，如无特殊说明，均可从商业途径得到。

在本发明的描述中，需要说明的是，术语“上”、“下”、“内”、“外”“前端”、“后端”、“两端”、“一端”、“另一端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“设有”、“连接”等，应做广义理解，例如“连接”，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

此外，附图仅为本公开的示意性图解，并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分，因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。

本发明实施例提供一种基于参考轨迹优化的AStar路径规划方法及设备，其中，基于参考轨迹优化的AStar路径规划方法，如图1所示，基于参考轨迹优化的AStar路径规划方法，应用于无人集卡上的车载终端，车载终端与障碍物信息获取模块通讯连接；所述障碍物信息获取模块设于无人集卡上或设置于其他现场位置；障碍物信息获取模块如激光雷达传感器等；车载终端用于获取无人集卡位置信息、障碍物信息和参考轨迹信息，以及根据障无人集卡位置信息、障碍物信息对无人集卡进行AStar路径规划；

方法步骤具体如下：

S10、获取当前无人集卡的位置、障碍物位置和参考轨迹信息，并将当前无人集卡的位置设为AStar搜索的起点S；该步骤中，当需要进局部路径规划或局部路径规划失败时，启动AStar算法进行路径规划；启动AStar算法后；

当前无人集卡、障碍物的位置信息通过北斗***、设置于无人集卡上或设置于其他现场位置的定位传感器、激光雷达传感器等方式获取；参考轨迹信息为TMC下发的参考轨迹；将当前无人集卡的位置设为AStar搜索的起点S（X_s, Y_s）；

S20、根据当前无人集卡的位置、障碍物位置和参考轨迹信息，选取参考轨迹附近的障碍物为障碍物子集；该步骤中，在路径规划区域内沿参考轨迹方向60m、100m可标定，根据当前无人集卡的位置、障碍物位置和参考轨迹信息，将距离参考轨迹小于第一距离（如图2所示，如1.6m，2m可标定）的障碍物作为障碍物子集；其中，0＜第一距离≤50m；

S30、在障碍物子集中选取距离起点S（X_s, Y_s）最远的障碍物投影在参考轨迹上作为原点，将该原点沿参考轨迹延伸预设距离至以G（X_G, Y_G）为AStar搜索的目标点；该步骤中，将障碍物子集中的各障碍物投影在参考轨迹上；将距离起点S（X_s, Y_s）最远的投影点作为原点，将该原点沿参考轨迹方向延伸预设距离（如图3所示，如50m,可标定）至G（X_G, Y_G）点，以G（X_G, Y_G）为AStar搜索的目标点；其中，0m＜预设距离≤100m；本步骤中，通过选取距离起点S（X_s, Y_s）最远的投影点并沿参考轨迹延伸预设距离，一方面，能够使规划出的路径末端更贴合参考轨迹；另一方面，能够避免未采用最远处的投影点往后预设距离作为G（X_G,Y_G），可能出现规划失败（障碍物在目的地点上）或规划成功后无人集卡行驶到目的地点后前方还有障碍物，可能会导致停车或触发路线重新规划的问题；从而保证无人集卡在行驶到目的地点时，车辆已经完全绕过了障碍物；

S40、根据起点S（X_s, Y_s）和目标点G（X_G, Y_G）确定AStar的搜索区域；

S50、搜索区域栅格化，将栅格到目标点G（X_G, Y_G）的欧式距离作为该栅格的代价，并将搜索区域中各个栅格的代价作为第一启发信息；该步骤中，将确定的搜索区域栅格化是一种把空间分成很多小块的方法，让你快速找到空间中的点，能大大提高搜索效率。栅格化建立过程为：

a. 选一个维度(如宽或高)，找中间的点分成两块；

b.对每块重复这个过程，直到每块只有一个点；

然后根据每个栅格到目标点G（X_G, Y_G）的欧式距离得到启发信息（即确定每个栅格的代价值）；将各个栅格到目标点G（X_G, Y_G）的欧式距离作为该栅格的代价，若该栅格存在障碍物，则增大该栅格的代价；并将搜索区域中各个栅格的代价作为第一启发信息；该步骤中，增加该栅格代价的方法为：将该栅格的代价乘以系数W（w>1），并限定有障碍物栅格代价的最小值m;如当前栅格代价为1，若该栅格存在障碍物，将该栅格代价设置为max(1*W,m)，若将w标定为100，m标定为1000，则该栅格代价为max(1*100,1000)。

较佳地，所述W≥100。通过设置比较大的W系数以及设置有障碍物栅格代价的最小值，能够避免W过小或有障碍物栅格代价过小而引起的第一启发信息选取不精准的问题。

S60、降低搜索区域中参考轨迹上的栅格的代价作为第一参考轨迹代价，将第一参考轨迹代价作为第二启发信息；该步骤，栅格的代价值越大，在AStar规划时采用该栅格作为路径点的概率就越小；本步骤通过降低搜索区域中参考轨迹上的栅格的代价，从而提高了把该栅格选为路径规划点的概率，从而使得规划出的路线能尽可能贴合原参考轨迹；

S70、根据第一启发信息和第二启发信息进行AStar路径规划；该步骤中，AStar规划算法是一种启发式搜索算法，用于在图（graph）中找到从起点到终点的最短路径。AStar算法的性能依赖于启发式函数h(n)，该函数用于估计从任意节点n到目标节点的最低成本。

AStar算法选择路径的成本基于两个主要组成部分：

实际成本g(n)：从起点到当前节点n的实际路径成本。

启发式成本h(n)：从当前节点n到目标的估计最低成本。

AStar算法使用这两个成本的和f(n)=g(n)+h(n) 作为评价函数，以决定哪个节点最有可能导向一个效率高且成本低的解决方案。

其中：上述的节点即栅格，h(n)即代价值。

该步骤中，利用获取的第一启发信息和第二启发信息进行AStar规划，将规划出的AStar路线与原参考轨迹进行拼接（如图7、图8所示，图中☆为本发明AStar规划出的局部路径；◇为现有AStar规划出的局部路径），并对拼接后的路线进行平滑，即得到可绕行障碍物的路线。

本发明实施例提供的基于参考轨迹优化的AStar路径规划方法，通过选取起点S和目标点G以及在启发信息中加入参考轨迹信息，使AStar规划出的路径能够尽量贴合原参考轨迹；从而使无人集卡能够很好的按照参考轨迹行驶，充分发挥原参考轨迹的作用。

进一步地，所述步骤S60中获取第一参考轨迹代价的方法如下：

将起点S投影在参考轨迹上，从起点S的投影点到G点的参考轨迹上选取若干节点；在搜索区域中分别找到距离各个节点最近的栅格作为参考轨迹栅格，将参考轨迹栅格的代价乘以系数k₁(0＜k₁＜1)作为第一参考轨迹代价。该步骤中，当无人集在参考轨迹上行驶或偏离参考轨迹行驶时，将起点S（X_s, Y_s）投影在参考轨迹上，从起点S（X_s, Y_s）投影点到G（X_G,Y_G）的参考轨迹上均匀或不均匀选取若干节点（如5个、10个、20个，可标定）；将起点在搜索区域中分别找到距离各个节点最近的栅格作为参考轨迹栅格，将参考轨迹栅格的代价乘以系数k₁(0＜k₁＜1，如k₁=0.1可标定)以降低参考轨迹栅格的代价；其中，若参考轨迹栅格上有障碍物，则该参考轨迹栅格的代价为没有障碍物时的栅格代价（栅格到目标点G的欧氏距离）*W，降低后的参考轨迹栅格代价为没有障碍物时的栅格代价*W*k₁；并将降低后的参考轨迹栅格作为第一参考轨迹代价；通过选取若干节点，一方面能够更好地确定参考轨迹栅格，另一方面能够降低数据处理量；提高路径规划效率。

进一步地，所述步骤S60还包括：S61、将与参考轨迹栅格相邻的栅格作为参考轨迹相邻栅格；将参考轨迹相邻栅格的代价乘以系数k₂作为第二参考轨迹代价，其中，0＜k₂＜1；将第一参考轨迹代价和第二参考轨迹代价共同作为第二启发信息。如图4所示为原始的启发信息地图，该启发信息地图只包含了每个栅格到终点（即图4中坐标为(14, 14)的五角星位置）的代价；

图5为加入参考轨迹信息处理后的结果（假设参考轨迹是(0, 0)到(14, 14)的直线），但该结果相对局限，如果参考轨迹附近有障碍物，那么该栅格的代价仍很大；在此基础上，找到参考轨迹一定范围的栅格，并减小其代价，可以使其更具灵活性，如图5所示。

进一步地，将距离参考轨迹栅格小于D₁距离（如1m，可标定）的栅格作为参考轨迹相邻栅格；计算参考轨迹相邻栅格与其相邻的参考轨迹栅格的距离D₂，k₂=(D₁距离/D₂距离)*P+k₁，其中，P为离参考轨迹的距离权重，0＜P＜1-k₁；0m＜D₁距离≤2m。该步骤中，k₂=(D₁距离/D₂距离)*P+k₁，其中，P为离参考轨迹的距离权重，0＜P＜1-k₁；0m＜D₁距离≤2m₁能保证该代价乘以k₂后的结果大于k₁。图6为加入参考轨迹相邻信息处理后的结果，由于参考轨迹栅格是最靠近参考轨迹的栅格，由于其代价乘以系数k₁，而在参考轨迹附近搜索的栅格比参考轨迹栅格离参考轨迹远，因此参考轨迹附近栅格的代价乘以的系数应该大于k₁。保障AStar进行正确的栅格选择。

进一步地，以起点S在参考轨迹的投影点为坐标零点，参考轨迹方向为X轴，垂直于参考轨迹方向为Y轴；

所述步骤S40中的搜索区域在X轴的范围为[min(X_s，X_G) - off_{_X}，max(X_s，X_G)]，在Y轴的范围为[min(Y_s，Y_G) - off_{_Y}，max(Y_s，Y_G) + off_{_Y}]，0＜off_{_X}≤30，0＜off_{_Y}≤15；其中，起点S的坐标为（X_s, Y_s）；目标点G的坐标为（X_G, Y_G），off_{_X}为搜索区域在X轴上的偏移量；off_{_Y}为搜索区域在Y轴上的偏移量。该搜索区域包括了无人集卡的车身区域，由于无人集卡有拖挂时，车身较长，该区域的选择能够保障无人集卡整个车身正常通过障碍物；能够适应较长拖挂的无人集卡的局部路径规划。

进一步地，所述步骤S50还包括：S51、对搜索区域中的栅格建立KD-Tree；建立KD-Tree的方法具体如下：

使搜索区域中的每个栅格代表空间中的一个区域，该区域具有唯一的坐标；

根据栅格的坐标确定该栅格在KD-Tree中的节点位置，并为该节点上的栅格分配唯一的ID；

在所述节点中存储父节点ID、子节点ID信息以及当前节点的坐标、代价和ID。所述步骤S60中获取第一参考轨迹代价时，在搜索区域中通过KD-Tree 搜索分别找到距离各个节点最近的栅格作为参考轨迹栅格。具体地，在 KD-Tree 中，每个节点代表了输入数据集中的一个点（在这个场景下，是栅格及其代价）。节点中存储的信息，如父节点 ID、子节点ID、当前节点的坐标（栅格的坐标）、ID 和代价值，使得 KD-Tree 能有效地进行空间搜索和数据组织。以下是如何将节点 ID 和代价 值与栅格及其代价 对应起来的具体方法：

节点 ID：在 KD-Tree 的构建过程中，每个***树中的点（栅格）可以被分配一个唯一的 ID。这个ID 可以是简单的递增整数，也可以是基于栅格坐标生成的唯一标识符。

栅格：每个栅格代表空间中的一个区域，具有唯一的坐标（比如，在二维空间中是(x, y)）。栅格的坐标用于确定 KD-Tree 中节点的位置。

代价值：在 KD-Tree 中，每个节点可以存储一个额外的数据值，这在本例中是栅格的启发式代价值。

将栅格与 KD-Tree 节点对应方法如下：

1、构建时的对应：在构建 KD-Tree 的过程中，每个栅格被读取并***到树中。***操作基于栅格的坐标（作为 KD-Tree 的键）进行。这时，栅格的坐标、其代价值以及分配给栅格的唯一 ID 存储在对应的 KD-Tree 节点中。

2、搜索时的对应：当通过 KD-Tree 进行搜索（例如，查找特定坐标附近的栅格）时，搜索算法遍历树，访问各个节点。每个访问的节点代表了一个栅格，通过节点存储的信息（坐标和代价值），可以直接关联到原始的栅格数据。本发明实施例提供的基于参考轨迹优化的AStar路径规划方法，在通过对启发信息建立kd-tree，并在此基础上进行数据的搜索和处理。提高了运算效率，在保证效率同时，生成了一条贴合原参考轨迹的优化路径，使车辆有效地规避障碍物。

本发明另外提供一种计算机可读介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上任意所述的基于参考轨迹优化的AStar路径规划方法。

本发明还提供一种基于参考轨迹优化的AStar路径规划设备，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上任意所述的基于参考轨迹优化的AStar路径规划方法。

具体实施时，在再一个实施例中，提供一种基于参考轨迹优化的AStar路径规划设备，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上任意所述的基于参考轨迹优化的AStar路径规划方法。

本实施例的设备能够实现上述的基于参考轨迹优化的AStar路径规划方法，具体实现方式可参见方法实施例在此不再赘述。

下面参照图9来描述根据本发明的这种实施方式的设备900。图9显示的设备900仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图9所示，设备900以通用计算设备的形式表现。设备900的组件可以包括但不限于：上述至少一个处理单元910、上述至少一个存储单元920、连接不同***组件（包括存储单元920和处理单元910）的总线930。

其中，所述存储单元存储有程序代码，所述程序代码可以被所述处理单元910执行，使得所述处理单元910执行本说明书上述“示例性方法”部分中描述的根据本发明各种示例性实施方式的步骤。例如，所述处理单元910可以执行如图1中所示的基于参考轨迹优化的AStar路径规划方法。

存储单元920可以包括易失性存储单元形式的可读介质，例如随机存取存储单元（RAM）9201和/或高速缓存存储单元9202，还可以进一步包括只读存储单元（ROM）9203。

存储单元920还可以包括具有一组（至少一个）程序模块9205的程序/实用工具9204，这样的程序模块9205包括但不限于：操作***、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

总线930可以为表示几类总线结构中的一种或多种，包括存储单元总线或者存储单元控制器、***总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。

设备900也可以与一个或多个外部设备1000（例如键盘、指向设备、蓝牙设备等）通信，还可与一个或者多个使得用户能与该设备900交互的设备通信，和/或与使得该设备900能与一个或多个其它计算设备进行通信的任何设备（例如路由器、调制解调器等等）通信。这种通信可以通过输入/输出（I/O）接口950进行。并且，设备900还可以通过网络适配器960与一个或者多个网络（例如局域网（LAN），广域网（WAN）和/或公共网络，例如因特网）通信。如图所示，网络适配器960通过总线930与设备900的其它模块通信。应当明白，尽管图中未示出，可以结合设备900使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID***、磁带驱动器以及数据备份存储***等。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质（可以是CD-ROM，U盘，移动硬盘等）中或网络上，包括若干指令以使得一台计算设备（可以是个人计算机、服务器、终端设备、或者网络设备等）执行根据本公开实施方式的方法。

在另一个实施例中，提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任意一项所述的基于激光点云的无人集卡局部路线规划方法。

本实施例提供的一种计算机可读存储介质，其上存储有能够实现本说明书上述方法的程序产品。在一些可能的实施方式中，本发明的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当所述程序产品在终端设备上运行时，所述程序代码用于使所述终端设备执行本说明书上述“示例性方法”部分中描述的根据本发明各种示例性实施方式的步骤。

描述了根据本发明的实施方式的用于实现上述方法的程序产品，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、设备或者器件使用或者与其结合使用。

所述程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的***、设备或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行***、设备或者器件使用或者与其结合使用的程序。可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网（LAN）或广域网（WAN），连接到用户计算设备，或者，可以连接到外部计算设备（例如利用因特网服务提供商来通过因特网连接）。

应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本公开的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。

此外，尽管在附图中以特定顺序描述了本公开中方法的各个步骤，但是，这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质（可以是CD-ROM，U盘，移动硬盘等）中或网络上，包括若干指令以使得一台计算设备（可以是个人计算机、服务器、移动终端、或者网络设备等）执行根据本公开实施方式的方法。

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

Claims (8)

1.一种基于参考轨迹优化的AStar路径规划方法，其特征在于：应用于无人集卡上的车载终端，所述车载终端与障碍物信息获取模块通讯连接；所述车载终端用于获取无人集卡位置信息、障碍物信息和参考轨迹信息，以及根据障无人集卡位置信息、障碍物信息对无人集卡进行AStar路径规划；

所述方法步骤具体如下：

S10、获取当前无人集卡的位置、障碍物位置和参考轨迹信息，并将当前无人集卡的位置设为AStar搜索的起点S；

S20、根据当前无人集卡的位置、障碍物位置和参考轨迹信息，选取参考轨迹附近的障碍物为障碍物子集；

S30、在障碍物子集中选取距离起点S最远的障碍物投影在参考轨迹上作为原点，将该原点沿参考轨迹延伸预设距离至G点，以G点为AStar搜索的目标点；

S40、根据起点S和目标点G确定AStar的搜索区域；

S50、搜索区域栅格化，将栅格到目标点G的欧式距离作为该栅格的代价，若该栅格存在障碍物，则增大该栅格的代价；并将搜索区域中各个栅格的代价作为第一启发信息；

S60、将起点S投影在参考轨迹上，从起点S的投影点到G点的参考轨迹上选取若干节点；在搜索区域中分别找到距离各个节点最近的栅格作为参考轨迹栅格，将参考轨迹栅格的代价乘以系数k₁(0＜k₁＜1)作为第一参考轨迹代价，将第一参考轨迹代价作为第二启发信息；

S70、根据第一启发信息和第二启发信息进行AStar路径规划。

2.根据权利要求1所述的基于参考轨迹优化的AStar路径规划方法，其特征在于：所述步骤S60还包括：S61、将与参考轨迹栅格相邻的栅格作为参考轨迹相邻栅格；将参考轨迹相邻栅格的代价乘以系数k₂作为第二参考轨迹代价，其中，0＜k₂＜1；将第一参考轨迹代价和第二参考轨迹代价共同作为第二启发信息。

3.根据权利要求2所述的基于参考轨迹优化的AStar路径规划方法，其特征在于：将距离参考轨迹栅格小于D₁距离的栅格作为参考轨迹相邻栅格；计算参考轨迹相邻栅格与其相邻的参考轨迹栅格的距离D₂，k₂=(D₁距离/D₂距离)*P+k₁；其中，P为离参考轨迹的距离权重，0＜P＜1-k₁；0m＜D₁距离≤2m。

4.根据权利要求1所述的基于参考轨迹优化的AStar路径规划方法，其特征在于：以起点S在参考轨迹的投影点为坐标零点，参考轨迹方向为X轴，垂直于参考轨迹方向为Y轴；

所述步骤S40中的搜索区域在X轴的范围为[min(X_s，X_G) - off__X，max(X_s，X_G)]，在Y轴的范围为[min(Y_s，Y_G) - off__Y，max(Y_s，Y_G) + off__Y]，0＜off__X≤30，0＜off__Y≤15；其中，起点S的坐标为（X_s, Y_s）；目标点G的坐标为（X_G, Y_G），off__X为搜索区域在X轴上的偏移量；off__Y为搜索区域在Y轴上的偏移量。

5.根据权利要求1所述的基于参考轨迹优化的AStar路径规划方法，其特征在于：所述步骤S50还包括：S51、对搜索区域中的栅格建立KD-Tree；建立KD-Tree的方法具体如下：

使搜索区域中的每个栅格代表空间中的一个区域，该区域具有唯一的坐标；

根据栅格的坐标确定该栅格在KD-Tree中的节点位置，并为该节点上的栅格分配唯一的ID；

在所述节点中存储父节点ID、子节点ID信息以及当前节点的坐标、代价和ID。

6.根据权利要求5所述的基于参考轨迹优化的AStar路径规划方法，其特征在于：所述步骤S60中获取第一参考轨迹代价时，在搜索区域中通过KD-Tree 搜索分别找到距离各个节点最近的栅格作为参考轨迹栅格。

7.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6中任一项所述的基于参考轨迹优化的AStar路径规划方法。

8.一种基于参考轨迹优化的AStar路径规划设备，其特征在于，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1-6中任一项所述的基于参考轨迹优化的AStar路径规划方法。