适应未知动态空间的快速路径规划方法、装置及存储介质与流程

本发明涉及自动驾驶技术，尤其与一种适应未知动态空间的快速路径规划方法、装置及存储介质有关。

背景技术：

自动驾驶技术已经发展了数十年，针对城市道路环境下的路径规划技术发展相对成熟。不同路况环境的前提下，可以选取不同的规划算法匹配和适应环境特征。面对未知动态环境，地图极不充分，地图区域大小和复杂性无法提前判断，使得单一的路径规划算法难以胜任。此外，面对未知环境，导航算法精度和传感器误差都会给载具的位姿估计引入一定误差。同时，如果将轨迹跟随控制和路径规划割裂，在环境的高度不确定性的影响下，载具性能约束可能引起路径无法实现的后果。因此，如果在路径规划中考虑轨迹跟随控制的影响，则载具定位定姿的不确定性和环境不确定性会进一步提升路径规划的难度。

技术实现要素：

针对上述现状，本发明提供一种适应未知动态空间的快速路径规划方法、装置及存储介质，提升未知不确定环境下路径规划的效率，极大减少搜索空间点的数量，提升未知不确定环境下路径规划的效率；并能自适应选择适合地图特性的规划算法，确保路径规划效率和路径的最优性。

为了实现本发明的目的，拟采用以下方案：

一种适应未知动态空间的快速路径规划方法，其特征在于，包括步骤：

根据载具的外形尺寸约束，在栅格地图基础上建立拓展地图；

根据拓展地图获取不可通行区域的凸顶点；

以凸顶点为节点，自适应选择路径规划获取粗路径；

根据局部环境特点，与轨迹跟随控制相结合，对粗路径进行修正，快速形成平滑、最优的局部路径。

进一步，在栅格地图基础上建立拓展地图，包括步骤：

遍历栅格地图中的不可通行栅格点；

将不可通行栅格点及与其距离小于载具转弯半径的所有点都记为拓展地图中的不可通行栅格点；具体是，将栅格地图中不可通行栅格点周围所有的半径为r内栅格点都标记为不可通行栅格点，其中r表示载具转弯半径与单位栅格长度的比值向上取整。

进一步，栅格地图有三种形式，分别是2维的平面投影图，或3维的空间占有图，或2.5维的平面投影+地形跟随标高地图。

进一步，栅格地图为可修整的栅格地图，栅格地图的修整主要有两种方式：一种是通过地图修整算法进行新地图的修改或局部地图的叠加，另一种是通过人为选定方式添加、改变或删除地图中的通行区域、未知区域、不可通行区域。

进一步，根据拓展地图获取不可通行区域的凸顶点，具体方法为：

遍历拓展地图中不可通行栅格点i，判断与该点相邻的栅格点对的关系；栅格点对由点ij，和与点ij关于点i的对称点i-j组成；若所有相邻栅格点对(ij,i-j)都满足至多有一个点是不可通行栅格点，则点i是凸顶点；

具体是，将不可通行栅格点i周围所有的（3ⁿ-1）个栅格点，组成(3ⁿ-1)/2个栅格点对(ij,i-j)，其中n表示栅格地图的维数取四舍五入后的值，点i-j表示点ij关于点i的对称点；判断ij和i-j是否可通行，若对于不可通行栅格点i，所有(3ⁿ-1)/2个栅格点对(ij,i-j)每对都满足至多有一个点是不可通行栅格点，将点i作为凸顶点。

进一步，若需要判别的不可通行栅格点在拓展地图边界，则将栅格点对中超出边界的点视为通行点。

进一步，以凸顶点为节点，自适应选择路径规划获取粗路径时：

路径规划算法根据局部地图大小、全局地图大小、两种地图大小之比，自适应选择全局或局部规划进行实时在线规划；

对于备选的每种路径规划算法，代价函数采用广义距离和/或其他指标的单项指标或多项加权指标进行优化。

进一步，路径规划算法选择判别阈值参考规划算力，备选的路径规划算法可以为适应地图的任意全局或局部规划算法。优选地，采用启发式算法。优选地，采用改进的a*算法、d*算法。

进一步，所述广义距离，在2维或3维地图中是指欧氏直线距离；在2.5维平面投影+地形跟随标高地图中是指平面距离复合高度爬升指标。

进一步，所述其他指标，根据所载荷传感器的不同选择不同的记录参数进行综合，包括时间、能量消耗、威胁暴露概率、交通流量、任务完成评价等中的一项指标或多项指标加权。

进一步，对粗路径进行修正，以路径跟随控制作为内回路引入局部路径修正优化，快速修正得到局部路径，并形成最优控制：通过引入人工势场，以当前目标粗路径点求取引力、以阈值范围内特定点求取斥力；以局部引力和斥力、载具转弯半径、速度限制作为约束，采用最优控制直接解算载具运动指令。

一种适应未知动态空间的快速路径规划装置，其特征在于，包括：

拓展地图单元，用于根据载具的外形尺寸约束，在栅格地图基础上建立拓展地图；

凸顶点单元，用于根据拓展地图获取不可通行区域的凸顶点；

粗路径单元，用于以凸顶点为节点，自适应选择路径规划获取粗路径；

局部规划控制单元，用于根据局部环境特点，与轨迹跟随控制相结合，对粗路径进行修正，快速形成平滑、最优的局部路径。

进一步，拓展地图单元，用于遍历栅格地图中的不可通行栅格点，将栅格地图中不可通行栅格点周围所有的半径为r内栅格点都标记为不可通行栅格点，其中r表示载具转弯半径与单位栅格长度的比值向上取整。

进一步，凸顶点单元，用于遍历拓展地图中的不可通行栅格点i，将不可通行栅格点i周围所有的（3ⁿ-1）个栅格点，组成(3ⁿ-1)/2个栅格点对(ij,i-j)，其中n表示栅格地图的维数取四舍五入后的值，点i-j表示点ij关于点i的对称点；并判断ij和i-j是否可通行，若对于不可通行栅格点i，所有(3ⁿ-1)/2个栅格点对(ij,i-j)每对都满足至多有一个点是不可通行栅格点，将点i作为凸顶点。其中，若需要判别的不可通行栅格点在拓展地图边界，则将栅格点对中超出边界的点视为通行点。

进一步，粗路径单元，用于根据局部地图空间大小、全局地图空间大小、局部/全局地图比设定判别阈值；粗路径单元在进行路径规划时可以自适应选择适宜的全局或局部算法；对于备选的每种路径规划算法，代价函数均采用多目标优化，其中代价指标是广义距离和其他指标的单项指标或多项加权指标。

进一步，局部规划控制单元用于引入人工势场，以当前目标粗路径点求取引力、以阈值范围内最近不可通行点求取斥力；以局部引力和斥力、载具的机动能力，包括转弯半径、速度限制等作为约束，采用最优控制直接解算载具运动指令。

一种计算机可读存储介质，其上存储有计算机程序，其特征在于，在所述计算机程序被处理器运行时控制所述存储介质所在设备执行前文所述的适应未知动态空间的快速路径规划方法。

本发明的有益效果在于：

1、针对地图中不可通行栅格点数目巨大的现象，提出用搜索凸顶点的方式代替搜索所有不可通行栅格点，可以大大减少搜索空间点的数量，从而实现快速规划。

2、能够自适应选择适合地图特性的规划算法，确保路径规划效率和路径的最优性。

3、将粗路径和局部路径分层解算，仅在局部环境下考虑路径修正；同时局部路径修正和轨迹跟随控制一体解决，便于载具实现路径跟随控制。

附图说明

本文描述的附图只是为了说明所选实施例，而不是所有可能的实施方案，更不是意图限制本发明的范围。

图1为本申请实施例的快速路径规划方法流程图；

图2为本申请实施例的地图拓展方法流程图；

图3为本申请实施例的凸顶点获取流程图；

图4为本申请实施例的以2维平面投影栅格图为例的栅格点对判定凸顶点示意图。

图5为本申请实施例的速路径规划装置结构框图。

图6为本申请实施例的局部规划控制单元结构框图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明做进一步的详细描述。

实施例一

结合附图1所述的示意图，对本申请实施例的快速路径规划方法作进一步说明，该方法包括以下步骤：

步骤s100

在栅格地图基础上，考虑载具的外形尺寸约束，建立拓展地图。

具体的，在本实例中，建立拓展地图的具体方法可参见如图2所示的地图拓展方法流程：

s110、建立与原始栅格地图同样空间大小的空地图作为拓展地图；

s120、找到原始栅格地图中不可通行栅格点，记录其坐标pi；

s130、找到pi周围所有满足条件的栅格点pij，使得pij到pi的距离小于等于载具转弯半径，记录所有坐标pij；

s140、将坐标为pi和相应的所有pij在拓展地图中标记为不可通行栅格点；

s150、重复s120~s140，直至遍历完原始地图中的栅格点。

更加具体的是，栅格地图可以是2维的平面投影图，也可以是3维的空间占有图，还可以是2.5维的平面投影+地形跟随标高地图；

在此基础上，栅格地图是可以修整的，既可以通过地图修整算法进行新地图的修改或局部地图的叠加，也可以通过人为选定方式添加、改变或删除地图中的通行区域、未知区域、不可通行区域。

步骤s200

基于拓展地图求取不可通行区域的凸顶点。

具体的，在本实例中，凸顶点的求取或获取，可采用如图3所示的流程：

s210、找到拓展地图中的不可通行栅格点i，判断是否在拓展地图的边界上；

s220、图4为以2维平面投影栅格图为例的栅格点对判定凸顶点示意图，图中只描绘了4种有代表性的栅格点对和待判定栅格点之间的关系，其他情况可以利用对称性对地图进行旋转得到。对于3维地图，可以类比得到相应结论。对于不在地图边界上的不可通行栅格点i，检索坐标在其周围相邻1个栅格的所有（3ⁿ-1）个栅格点，对于相对栅格点i的坐标在各维度坐标分别+1和-1的两个栅格点，可以组成一个栅格点对(ij,i-j)；

s230、判断栅格点对(ij,i-j)是否可通行，若两个栅格点均为不可通行点，则原栅格点i不是凸顶点；

s240、遍历(3ⁿ-1)/2组栅格点对，重复2.3，若点i没有被判定为“不是凸顶点”，则是一个凸顶点；

s250、对于在地图边界上的不可通行栅格点i，按照2.2~2.4所述方法检索相邻栅格点对，其中栅格点对中超出边界的点认为是通行点；

s260、重复s210~s250，直至遍历完拓展地图中的栅格点。

步骤s300

以凸顶点为节点，根据局部地图空间大小、全局地图空间大小、局部/全局地图比设定判别阈值，自适应选择路径规划求取粗路径。

本实例中采用的代价函数形式为：

式中，表示控制指令范数，指代能量消耗指标；表示威胁暴露概率指标；表示交通流量指标；表示任务完成度评价指标，表示到达目标总计迭代步数，指代时间；表示广义距离，在2维平面投影图和3维空间占有图中定义为：

为维度，在2.5维平面投影+地形跟随标高地图中定义为：

其中=2；并满足

步骤s400

根据局部环境特点，与轨迹跟随控制相结合，对粗路径进行修正，快速形成平滑、最优的局部路径。

具体的，不用计算势场函数，人工势力由最近点定义，即只用检索最近引力点和斥力点。

最近引力点和斥力点的具体检索分别方法为：

s410、检索最近引力点：

s411、当前位姿投影到局部路径修正用规划地图上；

s412、从粗路径点串中找到距离当前位姿最近的粗路径点，记录其在粗路径点串中的序号m，判断该点是否为最后一个粗路径点；

s413、如果m不是最后一个粗路径点，找到序号为m+1的路径点，将其作为引力点记录坐标；

s414、如果m是最后一个粗路径点，则直接记为引力点并记录坐标。

s420、检索最近斥力点：

s421、当前位姿投影到局部路径修正用规划地图上；

s422、遍历局部路径修正用规划地图设上定区域内不可通行栅格点，找到与当前坐标距离最短的点q，并记录最短距离；

s423、如果最短距离大于斥力计算阈值，则没有斥力点；

s424、如果最短距离小于等于斥力计算阈值，则将点q记为斥力点。

实施例二

结合附图5所述的结构框图，对本申请实施例的快速路径规划装置作进一步说明，该装置包括：拓展地图单元、凸顶点单元、粗路径单元及局部规划控制单元。

具体的，在本实例中：

拓展地图单元连接凸顶点单元，凸顶点单元连接粗路径单元，粗路径单元连接局部规划控制单元。

拓展地图单元根据载具的外形尺寸约束，在栅格地图基础上建立拓展地图；凸顶点单元根据拓展地图获取不可通行区域的凸顶点；粗路径单元以凸顶点为节点，自适应选择路径规划获取粗路径；局部规划控制单元根据局部环境特点，与轨迹跟随控制相结合，对粗路径进行修正，快速形成平滑、最优的局部路径。

具体的，拓展地图单元，用于遍历原始地图中的不可通行栅格点；将原始地图中不可通行栅格点周围所有的半径为r内栅格点都标记为不可通行栅格点，其中r表示载具转弯半径与单位栅格长度的比值向上取整，完成拓展地图建立。

更加的具体的，拓展地图单元用于建立与原始栅格地图同样空间大小的空地图作为拓展地图，并找到原始地图中不可通行栅格点，记录其坐标pi，并找到pi周围所有满足条件的栅格点pij，使得pij到pi的距离小于等于载具转弯半径，记录所有坐标pij；并将坐标为pi和相应的所有pij在拓展地图中标记为不可通行栅格点；直至遍历完原始地图中的栅格点，完成拓展地图建立。

其中，拓展地图单元采用的栅格地图，可以是2维的平面投影图，也可以是3维的空间占有图，还可以是2.5维的平面投影+地形跟随标高地图。

其中，栅格地图是可以修整的，既可以通过地图修整算法进行新地图的修改或局部地图的叠加，也可以通过人为选定方式添加、改变或删除地图中的通行区域、未知区域、不可通行区域。

具体的，凸顶点单元，用于遍历拓展地图中的不可通行栅格点i，将不可通行栅格点i周围所有的（3ⁿ-1）个栅格点，组成(3ⁿ-1)/2个栅格点对(ij,i-j)，其中n表示栅格地图的维数取四舍五入后的值，点i-j表示点ij关于点i的对称点；并判断ij和i-j是否可通行，若对于不可通行栅格点i，所有(3ⁿ-1)/2个栅格点对(ij,i-j)每对都满足至多有一个点是不可通行栅格点，将点i作为凸顶点。

凸顶点单元在用于进行凸顶点获取时，具体可以通过执行实施例一所述的方法s210~s260步骤以完成凸顶点获取。其中，若需要判别的不可通行栅格点在拓展地图边界，则将栅格点对中超出边界的点视为通行点。

具体的，粗路径单元用于以凸顶点为节点，根据局部地图空间大小、全局地图空间大小、局部/全局地图比设定判别阈值，自适应选择路径规划求取粗路径。

其中，粗路径单元采用的代价函数形式为：

式中，表示控制指令范数，指代能量消耗指标；表示威胁暴露概率指标；表示交通流量指标；表示任务完成度评价指标，表示到达目标总计迭代步数，指代时间；表示广义距离，在2维平面投影图和3维空间占有图中定义为：

为维度，在2.5维平面投影+地形跟随标高地图中定义为：

其中=2；并满足

具体的，局部规划控制单元用于优化局部路径，形成最优控制。局部规划控制单元不用计算势场函数，人工势力由最近点定义，即只用检索最近引力点和斥力点。

局部规划控制单元包括引力点子单元和斥力点子单元，如图6所示。

引力点子单元，用于将当前位姿投影到局部路径修正用规划地图上；并从粗路径点串中找到距离当前位姿最近的粗路径点，记录其在粗路径点串中的序号m，判断该点是否为最后一个粗路径点；如果m不是最后一个粗路径点，找到序号为m+1的路径点，将其作为引力点记录坐标；如果m是最后一个粗路径点，则直接记为引力点并记录坐标。

斥力点子单元，用于将当前位姿投影到局部路径修正用规划地图上；并遍历局部路径修正用规划地图设上定区域内不可通行栅格点，找到与当前坐标距离最短的点q，并记录最短距离；如果最短距离大于斥力计算阈值，则没有斥力点；如果最短距离小于等于斥力计算阈值，则将点q记为斥力点。

实施例三

一种计算机可读存储介质，其上存储有计算机程序，在计算机程序被处理器运行时控制存储介质所在设备执行前述实施例的适应未知动态空间的快速路径规划方法。

以上仅为本发明的优选实施例，并不表示是唯一的或是限制本发明。本领域技术人员应理解，在不脱离本发明的范围情况下，对本发明进行的各种改变或同等替换，均属于本发明保护的范围。