CN117109624A - 基于a*和并行teb的低速无人车的混合路径规划方法 - Google Patents

Abstract

本发明公开了一种基于A*和并行TEB的低速无人车的混合路径规划方法，包括下述步骤：S1：基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物；S2：构建并优化A*算法，并用优化后的A*算法开展全局路径规划；步骤S3：构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划；步骤S4：采用样条插值法对并行TEB算法开展局部路径进行优化，使得局部路径更加圆滑；步骤S5：判断步骤S4中局部目标点是否为轨迹终点。本发明属于无人驾驶技术领域，具体提供了一种基于A*和并行TEB的低速无人车的混合路径规划方法。

Description

基于A*和并行TEB的低速无人车的混合路径规划方法

技术领域

本发明属于无人驾驶技术领域，具体为基于A*和并行TEB的低速无人车的混合路径规划方法。

背景技术

现如今低速无人车在物流配送、港口分拣、智能居家和工业搬运等方面发挥着越来越重要的作用。低速无人车路径规划准确性取决于其导航精度，受成本限制，目前产业化低速无人车多采用激光导航和视觉导航。在做到准确规划的基础上，低速无人车的工作效率取决于路径规划控制方法，当今常用路径规划方法包括基于Dijkstra算法、A*算法等全局路径规划，以及基于动态规划窗口DWA、TEB等算法的局部路径规划方法。

传统全局路径规划方法多基于广度优先搜索，虽然能确保全局路径最优，但耗费大量的计算资源，规划效率低下，无法保证低速无人车在动态环境中的路径规划自适应能力；传统局部路径规划方法，虽然能提升低速无人车动态适应性，但在相邻障碍物非常逼近以及路径不圆滑的状况下，会易陷入局部极值和低速无人车行驶不稳定等问题，导致低速无人车无法规划轨迹而停驶。

发明内容

针对上述情况，为弥补上述现有缺陷，本发明提供了一种基于A*和并行TEB的低速无人车的混合路径规划方法，该方法在由激光雷达和相机融合建立的地图中规划路径，利用优化后的A*算法开展全局路径规划，在获取全局路径后，对传统TEB算法进行优化，基于并行TEB算法开展局部路径规划，通过计算路径函数过滤同源路径，选取代价函数最小路径作为最优局部路径，再通过样条插值法对其再进行优化，得到一条更加圆滑的局部路径。本发明能够在全局最优和局部最优之间达到平衡，提升低速无人车效率的同时提升的动态环境适应性，降低路径规划成本。

本发明提供如下的技术方案：

本发明提出了一种基于A*和并行TEB的低速无人车的混合路径规划方法，具体包括下述步骤：

步骤S1：基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物；

步骤S2：构建并优化A*算法，并用优化后的A*算法开展全局路径规划；

步骤S3：构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划；

步骤S4：采用样条插值法对并行TEB算法开展局部路径进行优化，使得局部路径更加圆滑；

步骤S5：判断步骤S4中局部目标点是否为轨迹终点，若是，则结束整个路径规划；若不是，跟随全局规划的路径，并且循环步骤S3和步骤S4，直至达到轨迹终点，整个路径规划结束。

进一步地，步骤S1所述的基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物，其详细过程如下：

采用SLAM建图法中的Gmapping建图，由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定低速无人车运动轨迹起始点位置S和终点位置D，并为若干个静态障碍物O_static添加膨胀区，构建初始运动地图，在实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物O_dynamic。

进一步地，步骤S2所述的构建并优化A*算法，并用优化后的A*算法开展全局路径规划，包括以下子步骤：

(1)构建基本对的A*算法流程

建立Close_List和Open_List两个集合，将两个集合初始设为空集，本方法将轨迹起始点S作为算法第一个起始节点C放入Close_List集合内，检测起始节点C周围是否存在附近节点N，若传感元件探测起始节点C周围均为障碍物或无附近节点N，则退出路径规划程序，输出起始点S和终点D之间不存在可行路径；若传感元件探测起始节点C周围存在附近节点N，则将所有附近节点N均移入到Open_List集合内，并计算每个附近节点Ni的代价函数f(n)，这里我们计算其中心点周围的8个点的代价函数，表达式如下式：

f(n)＝g(n)+h(n)

式中，g(n)为实际代价，为起始节点C到附近节点Ni之间的实际距离；h(n)为启发代价，代表附近节点Ni到轨迹终点D之间的估计距离；其中采用曼哈顿距离表示附近节点Ni的启发代价，计算方法如下所示：

h(n)＝|x_n-x₁|+|y_n-y₁|

式中，x_n表示当前附近节点Ni在二维栅格地图中的横坐标；y_n表示当前附近节点Ni在二维栅格地图中的纵坐标；x₁为终点D在二维栅格地图中的横坐标，y₁为终点D在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作N*，并判断N*是否达到了终点D，若已经达到终点D，则退出路径规划程序，若没有达到终点D，就将N*节点作为下一轮循环规划的初始节点移入到Close_List集合内，同时清空Open_List集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；

(2)优化A*算法

子步骤(1)中A*算法运算过程中，存在下述问题：当其中附近点的代价值相同时，算法无法准确选择下一个路径点，需要对A*算法进行优化。

进一步地，子步骤(2)中所述优化A*算法，其详细过程如下：

假设此时遇到两个代价估计一样的邻近节点n1和n2，先将当前位置点和目标位置点连接构成一条直线，然后将邻近节点n1和n2分别向直线这条直线作垂线，得到距离值分别为d1和d2，然后比较d1和d2大小，得到优化后的代价函数为：

进一步地，步骤S3所述的构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划，其详细过程如下：

(1)将步骤S2规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，低速无人车一直沿这些关键点运行；若突然出现动态障碍物，则将低速无人车当前位置PS作为局部路径起点，将局部子目标点PD作为局部路径终点，检测动态障碍物O_dynamic的上边界O1和下边界O2，构建局部拓扑地图，得到若干条可行路径；本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，它们具有相同的路径函数H，计算方法如下：

H＝ln A-ln B+min(arg(P_S-ε)-arg(P_D-ε))

式中，A表示路径起点与障碍物边界间距离，B表示路径终点与障碍物边界间距离，arg(P_S-ε)-arg(P_D-ε)为由两条距离线段组成的邻边角度之差，利用相同H值，过滤掉同源路径；

(2)为了低速无人车保证行驶安全，本方法引入障碍物距离约束条件，如下式：

d_obstacle＜d_min

式中，d_obstacle表示低速无人车与障碍物之间的距离，通过传感器测距获取，初始标定低速无人车与障碍物的最小极限距离为dmin；

距离代价函数f_obstacle如下式：

当低速无人车与障碍物距离大于极限距离时，距离约束权重ρobstacle为0，不考虑距离约束条件；

(3)在满足安全性的基础上，本方法引入时间约束，要求运行时间最短，利用A低速无人车各段局部路程时间间隔构建时间代价函数f_time，如下式：

(4)为保证低速无人车运行不超速，本方法引入速度约束，其代价函数为f_v，表达式如下：

其中，v_i为小车的速度；

(5)为避免低速无人车旋转角度过大，本方法引入角速度约束，其代价函数为f_w，表达式如下：

其中，w_i为小车的角速度；

(6)为保证低速无人车行驶平顺，本方法引入加速度约束和角加速度约束，其代价函数分别记为f_α和f_β，表达式如下：

其中α_i和β_i分别为低速无人车加速度和角加速度；

(7)本方法利用上述所有约束构成评价路径优劣的总代价函数F，计算过滤掉同源路径后，剩余路径的代价函数，如下式：

F＝ρ₁f_obstacle+ρ₂f_time+ρ₃f_v+ρ₄f_w+ρ₅f_α+ρ₆f_β

其中ρ₁，ρ₂，ρ₃，ρ₄，ρ₅，ρ₆，分别对应各自约束权重，可根据代价主次进行标定，选取代价函数F最小的路径作为局部最优路径。

进一步地，步骤S4所述的采用样条插值法对并行TEB算法开展局部路径进行优化，其详细过程如下：

在进行样条插值法优化之前首先需要定义一个二维数组，用来储存调整减少之后的位姿点，设每个位姿点为(x_i,y_i)，然后将每两个相邻的位姿点的连线进行弹性优化，使其路径更加平滑；如果将每个位姿点直接连起来的直线显然不能够作为路径，因此需要使用样条插值法会将任意两个位姿点拟合成一个平滑曲线函数；

本方法将用最简单的二次函数将任意两个位姿点拟合成一个平滑曲线函数；

设函数表达式y(x)如下所示：

y(x)＝a+bx+cx²

式中，a、b和c为满足该函数条件的代求系数，只需要根据已知位姿点的坐标和导数关系，即可求得a、b、c三个未知量。

采用上述结构本发明取得的有益效果如下：本发明提出的一种基于A*和并行TEB的低速无人车的混合路径规划方法，本方法相较于现有技术，具体具有下述优点：

(1)本发明针对以往A*算法的全局路径规划，提出了优化方法，解决了当附近点的代价值相同时，算法无法准确选择下一个路径点的缺陷，使得A*算法的全局路径规划更加完善；

(2)本发明基于TEB算法的局部路径规划，提出了并行TEB算法，很好地解决了路径优化中的局部最值问题，使得局部路径规划更为合理和有效；此外，针对并行TEB算法计算出的最优路径，本方法又采用了样条插值法对其路径进一步完善，使得路径更加圆滑，更加便于低速无人车实现避障；

(3)本发明提出一种基于优化的A*和并行TEB的混合路径规划方法，能降低全局规划时间复杂度，压缩规划过程中的节点数和计算量，提高规划效率，在满足静态路径行驶安全的基础上，还提升了低速无人车对动态障碍物的适应能力；

(4)与现有技术相比，本发明融合了高效的全局路径规划方法和克服局部极值问题的局部路径规划方法，有助于提升低速无人车运行效率、降低路径规划成本和提升车辆的动态适应能力。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明的整体流程图；

图2为本发明优化的全局路径规划算法流程示意图；

图3为本发明优化的局部路径规划算法流程示意图。

具体实施方式

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

需要说明的是，下面描述中使用的词语“前”、“后”、“左”、“右”、“上”和“下”指的是附图中的方向，词语“内”和“外”分别指的是朝向或远离特定部件几何中心的方向。

实施例

参阅图1-3，本实施例提出了一种基于A*和并行TEB的低速无人车的混合路径规划方法，具体包括下述步骤：

步骤S1：基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物；

步骤S2：构建并优化A*算法，并用优化后的A*算法开展全局路径规划；

步骤S3：构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划；

步骤S4：采用样条插值法对并行TEB算法开展局部路径进行优化，使得局部路径更加圆滑；

步骤S5：判断步骤S4中局部目标点是否为轨迹终点，若是，则结束整个路径规划；若不是，跟随全局规划的路径，并且循环步骤S3和步骤S4，直至达到轨迹终点，整个路径规划结束。

具体地，在本实施例中，步骤S1所述的基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物，其详细过程如下：

采用SLAM建图法中的Gmapping建图，由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定低速无人车运动轨迹起始点位置S和终点位置D，并为若干个静态障碍物O_static添加膨胀区，构建初始运动地图，在实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物O_dynamic；

具体地，在本实施例中，步骤S2所述的构建并优化A*算法，并用优化后的A*算法开展全局路径规划，包括以下子步骤：

(1)构建基本对的A*算法流程

建立Close_List和Open_List两个集合，将两个集合初始设为空集，本方法将轨迹起始点S作为算法第一个起始节点C放入Close_List集合内，检测起始节点C周围是否存在附近节点N，若传感元件探测起始节点C周围均为障碍物或无附近节点N，则退出路径规划程序，输出起始点S和终点D之间不存在可行路径；若传感元件探测起始节点C周围存在附近节点N，则将所有附近节点N均移入到Open_List集合内，并计算每个附近节点Ni的代价函数f(n)，这里我们计算其中心点周围的8个点的代价函数，表达式如下式：

f(n)＝g(n)+h(n)

式中，g(n)为实际代价，为起始节点C到附近节点Ni之间的实际距离；h(n)为启发代价，代表附近节点Ni到轨迹终点D之间的估计距离；其中采用曼哈顿距离表示附近节点Ni的启发代价，计算方法如下所示：

h(n)＝|x_n-x₁|+|y_n-y₁|

式中，x_n表示当前附近节点Ni在二维栅格地图中的横坐标；y_n表示当前附近节点Ni在二维栅格地图中的纵坐标；x₁为终点D在二维栅格地图中的横坐标，y₁为终点D在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作N*，并判断N*是否达到了终点D，若已经达到终点D，则退出路径规划程序，若没有达到终点D，就将N*节点作为下一轮循环规划的初始节点移入到Close_List集合内，同时清空Open_List集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；

(2)优化A*算法

子步骤(1)中A*算法运算过程中，存在下述问题：当其中附近点的代价值相同时，算法无法准确选择下一个路径点，需要对A*算法进行优化。

具体地，在本实施例中，子步骤(2)中所述优化A*算法，其详细过程如下：

假设此时遇到两个代价估计一样的邻近节点n1和n2，先将当前位置点和目标位置点连接构成一条直线，然后将邻近节点n1和n2分别向直线这条直线作垂线，得到距离值分别为d1和d2，然后比较d1和d2大小，得到优化后的代价函数为：

具体地，在本实施例中，步骤S3所述的构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划，其详细过程如下：

(1)将步骤S2规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，低速无人车一直沿这些关键点运行；若突然出现动态障碍物，则将低速无人车当前位置PS作为局部路径起点，将局部子目标点PD作为局部路径终点，检测动态障碍物O_dynamic的上边界O1和下边界O2，构建局部拓扑地图，得到若干条可行路径；本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，它们具有相同的路径函数H，计算方法如下：

H＝ln A-ln B+min(arg(P_S-ε)-arg(P_D-ε))

式中，A表示路径起点与障碍物边界间距离，B表示路径终点与障碍物边界间距离，arg(P_S-ε)-arg(P_D-ε)为由两条距离线段组成的邻边角度之差，利用相同H值，过滤掉同源路径；

(2)为了低速无人车保证行驶安全，本方法引入障碍物距离约束条件，如下式：

d_obstacle＜d_min

式中，d_obstacle表示低速无人车与障碍物之间的距离，通过传感器测距获取，初始标定低速无人车与障碍物的最小极限距离为dmin；

距离代价函数f_obstacle如下式：

当低速无人车与障碍物距离大于极限距离时，距离约束权重ρobstacle为0，不考虑距离约束条件；

(3)在满足安全性的基础上，本方法引入时间约束，要求运行时间最短，利用A低速无人车各段局部路程时间间隔构建时间代价函数f_time，如下式：

(4)为保证低速无人车运行不超速，本方法引入速度约束，其代价函数为f_v，表达式如下：

其中，v_i为小车的速度；

(5)为避免低速无人车旋转角度过大，本方法引入角速度约束，其代价函数为f_w，表达式如下：

其中，w_i为小车的角速度；

(6)为保证低速无人车行驶平顺，本方法引入加速度约束和角加速度约束，其代价函数分别记为f_α和f_β，表达式如下：

其中α_i和β_i分别为低速无人车加速度和角加速度；

(7)本方法利用上述所有约束构成评价路径优劣的总代价函数F，计算过滤掉同源路径后，剩余路径的代价函数，如下式：

F＝ρ₁f_obstacle+ρ₂f_time+ρ₃f_v+ρ₄f_w+ρ₅f_α+ρ₆f_β

其中ρ₁，ρ₂，ρ₃，ρ₄，ρ₅，ρ₆，分别对应各自约束权重，可根据代价主次进行标定，选取代价函数F最小的路径作为局部最优路径。

具体地，在本实施例中，步骤S4所述的采用样条插值法对并行TEB算法开展局部路径进行优化，其详细过程如下：

在进行样条插值法优化之前首先需要定义一个二维数组，用来储存调整减少之后的位姿点，设每个位姿点为(x_i,y_i)，然后将每两个相邻的位姿点的连线进行弹性优化，使其路径更加平滑；如果将每个位姿点直接连起来的直线显然不能够作为路径，因此需要使用样条插值法会将任意两个位姿点拟合成一个平滑曲线函数；

本方法将用最简单的二次函数将任意两个位姿点拟合成一个平滑曲线函数；

设函数表达式y(x)如下所示：

y(x)＝a+bx+cx²

式中，a、b和c为满足该函数条件的代求系数，只需要根据已知位姿点的坐标和导数关系，即可求得a、b、c三个未知量。

要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物料或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物料或者设备所固有的要素。

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

Claims (6)

1.基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，具体包括下述步骤：

步骤S1：基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物；

步骤S2：构建并优化A*算法，并用优化后的A*算法开展全局路径规划；

步骤S3：构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划；

步骤S4：采用样条插值法对并行TEB算法开展局部路径进行优化，使得局部路径更加圆滑；

步骤S5：判断步骤S4中局部目标点是否为轨迹终点，若是，则结束整个路径规划；若不是，跟随全局规划的路径，并且循环步骤S3和步骤S4，直至达到轨迹终点，整个路径规划结束。

2.根据权利要求1所述的基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，步骤S1所述的基于SLAM建好的地图，设定低速无人车的轨迹起始点、终点和障碍物，其详细过程如下：

采用SLAM建图法中的Gmapping建图，由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定低速无人车运动轨迹起始点位置S和终点位置D，并为若干个静态障碍物O_static添加膨胀区，构建初始运动地图，在实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物O_dynamic。

3.根据权利要求2所述的基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，步骤S2所述的构建并优化A*算法，并用优化后的A*算法开展全局路径规划，包括以下子步骤：

(1)构建基本对的A*算法流程

建立Close_List和Open_List两个集合，将两个集合初始设为空集，本方法将轨迹起始点S作为算法第一个起始节点C放入Close_List集合内，检测起始节点C周围是否存在附近节点N，若传感元件探测起始节点C周围均为障碍物或无附近节点N，则退出路径规划程序，输出起始点S和终点D之间不存在可行路径；若传感元件探测起始节点C周围存在附近节点N，则将所有附近节点N均移入到Open_List集合内，并计算每个附近节点Ni的代价函数f(n)，这里我们计算其中心点周围的8个点的代价函数，表达式如下式：

f(n)＝g(n)+h(n)

式中，g(n)为实际代价，为起始节点C到附近节点Ni之间的实际距离；h(n)为启发代价，代表附近节点Ni到轨迹终点D之间的估计距离；其中采用曼哈顿距离表示附近节点Ni的启发代价，计算方法如下所示：

h(n)＝|x_n-x₁|+|y_n-y₁|

式中，x_n表示当前附近节点Ni在二维栅格地图中的横坐标；y_n表示当前附近节点Ni在二维栅格地图中的纵坐标；x₁为终点D在二维栅格地图中的横坐标，y₁为终点D在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作N*，并判断N*是否达到了终点D，若已经达到终点D，则退出路径规划程序，若没有达到终点D，就将N*节点作为下一轮循环规划的初始节点移入到Close_List集合内，同时清空Open_List集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；

(2)优化A*算法

子步骤(1)中A*算法运算过程中，存在下述问题：当其中附近点的代价值相同时，算法无法准确选择下一个路径点，需要对A*算法进行优化。

4.根据权利要求3所述的基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，子步骤(2)中所述优化A*算法，其详细过程如下：

假设此时遇到两个代价估计一样的邻近节点n1和n2，先将当前位置点和目标位置点连接构成一条直线，然后将邻近节点n1和n2分别向直线这条直线作垂线，得到距离值分别为d1和d2，然后比较d1和d2大小，得到优化后的代价函数为：

5.根据权利要求1所述的基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，步骤S3所述的构建并行TEB算法，当遇到突发的障碍物时，使用并行TEB算法开展局部路径规划，其详细过程如下：

(1)将步骤S2规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，低速无人车一直沿这些关键点运行；若突然出现动态障碍物，则将低速无人车当前位置PS作为局部路径起点，将局部子目标点PD作为局部路径终点，检测动态障碍物O_dynamic的上边界O1和下边界O2，构建局部拓扑地图，得到若干条可行路径；本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，它们具有相同的路径函数H，计算方法如下：

H＝lnA-lnB+min(arg(P_S-ε)-arg(P_D-ε))

式中，A表示路径起点与障碍物边界间距离，B表示路径终点与障碍物边界间距离，arg(P_S-ε)-arg(P_D-ε)为由两条距离线段组成的邻边角度之差，利用相同H值，过滤掉同源路径；

(2)为了低速无人车保证行驶安全，本方法引入障碍物距离约束条件，如下式：

d_obstacle＜d_min

式中，d_obstacle表示低速无人车与障碍物之间的距离，通过传感器测距获取，初始标定低速无人车与障碍物的最小极限距离为dmin；

距离代价函数f_obstacle如下式：

当低速无人车与障碍物距离大于极限距离时，距离约束权重ρobstacle为0，不考虑距离约束条件；

(3)在满足安全性的基础上，本方法引入时间约束，要求运行时间最短，利用A低速无人车各段局部路程时间间隔构建时间代价函数f_time，如下式：

(4)为保证低速无人车运行不超速，本方法引入速度约束，其代价函数为f_v，表达式如下：

其中，v_i为小车的速度；

(5)为避免低速无人车旋转角度过大，本方法引入角速度约束，其代价函数为f_w，表达式如下：

其中，w_i为小车的角速度；

(6)为保证低速无人车行驶平顺，本方法引入加速度约束和角加速度约束，其代价函数分别记为f_α和f_β，表达式如下：

其中α_i和β_i分别为低速无人车加速度和角加速度；

(7)本方法利用上述所有约束构成评价路径优劣的总代价函数F，计算过滤掉同源路径后，剩余路径的代价函数，如下式：

F＝ρ₁f_obstacle+ρ₂f_time+ρ₃f_v+ρ₄f_w+ρ₅f_α+ρ₆f_β

其中ρ₁，ρ₂，ρ₃，ρ₄，ρ₅，ρ₆，分别对应各自约束权重，可根据代价主次进行标定，选取代价函数F最小的路径作为局部最优路径。

6.根据权利要求1所述的基于A*和并行TEB的低速无人车的混合路径规划方法，其特征在于，步骤S4所述的采用样条插值法对并行TEB算法开展局部路径进行优化，其详细过程如下：

在进行样条插值法优化之前首先需要定义一个二维数组，用来储存调整减少之后的位姿点，设每个位姿点为(x_i,y_i)，然后将每两个相邻的位姿点的连线进行弹性优化，使其路径更加平滑；如果将每个位姿点直接连起来的直线显然不能够作为路径，因此需要使用样条插值法会将任意两个位姿点拟合成一个平滑曲线函数；

本方法采用二次函数将任意两个位姿点拟合成一个平滑曲线函数；设函数表达式y(x)如下所示：

y(x)＝a+bx+cx²

式中，a、b和c为满足该函数条件的代求系数，只需要根据已知位姿点的坐标和导数关系，即可求得a、b、c三个未知量。