CN113608531A - 基于安全a*引导点的动态窗口的无人车实时全局路径规划方法 - Google Patents

Abstract

本发明涉及一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，首先在A＊算法的基础上提出了节点安全扩展策略以及路径节点二次优化方法，称之为安全A*算法，运用安全A*算法快速找出最优的安全虚拟目标点；第二，以上述的虚拟目标点作为动态窗口法的局部目标，进行速度采样，实现路径规划与避障，并在MATLAB环境下进行仿真，结果表明无人车可以在全局路径的引领下，安全平稳的避开障碍物，验证了本发明方法的安全性和稳定性。

Description

基于安全A*引导点的动态窗口的无人车实时全局路径规划 方法

技术领域

本发明涉及无人车实时避障的全局路径规划领域，特别地一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法。

背景技术

无人车路径规划就是在综合考虑行驶时间、速度、油耗和安全等因素的前提下，为无人车规划出最优的行驶轨迹。无人车为了完成在复杂的道路上的行驶，其自主移动能力则是基础，其中路径规划和导航是保证无人车自主移动能力的核心算法，是体现车辆智慧水平的关键。

从路径规划的目标范围看，通常分为全局路径规划和局部路径规划；从规划环境是否随时间变化方面看，还可以分为静态路径规划和动态路径规划。全局路径规划通常假设环境信息完全已知，在无人车运动之前，离线计算出一条从起点到终点的路径；局部路径规划则是在无人车运动时，可以在线的、实时的实现障碍物规避；但是局部路径规划由于缺乏全局路径信息的引导，往往容易陷入局部最小的状况。由于局部路径规划方法有计算量小，实时性好，规划轨迹圆滑等优点，目前通常采用把全局与局部路径规划结合的二层避障结构，在第一层，通过已知的环境信息(一般为地图)，离线规划出一条从起点到目标点代价值最小的路径，该路径不需平滑但应能避开地图上已知的静态障碍物；在第二层，无人车沿着第一层规划的路径行驶，当在行驶过程中通过传感器检测到新的障碍物时，再通过获得的障碍物详细的位置信息进行局部避障。

发明内容

有鉴于此，本发明的目的是提供一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，无人车可以在全局路径的引领下，安全平稳的避开障碍物，达到良好的路径规划效果。

本发明采用以下方案实现：一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达传感器生成环境地图，并生成栅格地图，并在栅格地图上设置起始点和目标点信息，同时设置无人车的初始速度、角速度为零；

步骤S2：从无人车的当前位置向周围上、下、左、右、左上、左下、右上、右下八个方向扩展节点；可行的扩展节点的扩展原则为：有障碍物不扩展，已扩展过的节点不扩展；

步骤S3：分别计算步骤S2中可行的扩展节点的总代价值f(m),并按从小到大的原则进行排序；

步骤S4：安全A*算法节点安全扩展：在A*算法的节点扩展、节点代价值计算和排序环节后增加节点扩展安全检测策略，定义为安全A*算法；当安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点；

步骤S5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤S2，重复步骤S2-步骤S5规划出一条无碰撞的最优安全路径；

步骤S6：对步骤S5得到的最优安全路径进行二次优化，选取转折点，去除冗余节点，得到引导点；

步骤S7：初始化局部目标点：将步骤S6中得到的引导点作为动态窗口法的当前局部目标点，无人车在到达局部目标点时并不会停止运动，而是接着向下一个局部目标点运动，并且当前局部目标点消失不再对无人车起导向作用，直到下一个局部目标点为终点；

步骤S8：根据采样的线速度和角速度，计算动态速度窗口，并进行速度采样；

步骤S9：根据步骤S8中的采样速度，模拟无人车的行驶轨迹，生成轨迹空间；

步骤S10：根据步骤S9中无人车的模拟轨迹，判断是否需要开始避障；

步骤S11：依据评价函数评价所有可行的模拟轨迹对应的采样速度，得出最优的线速度和角速度；

步骤S12：输出线速度和角速度驱动无人车行驶；

步骤S13：重复以上循环，直至无人车到达终点；

进一步地，步骤S3中所述代价值f(m)的计算方式如下：

f(m)＝g(m)+h(m)

其中，g(m)为起始节点到当前扩展节点的实际代价值，h(m)为当前扩展节点到目标点的代价估计值；代价值采用欧几里德距离计算。

进一步地，步骤S4中所述的节点安全扩展策略的具体内容为：安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，针对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点。

进一步地，所述步骤S6的具体内容为：

步骤S5中，安全A*算法规划出的最优安全路径中还包含诸多的冗余点及不必要的转折点，针对此问题对路径进行二次优化，得到引导点；首先，从规划路径的第2个节点开始，分别计算该节点与其父节点和子节点的夹角，若夹角相同，则该节点为冗余点，删除该节点；若夹角不同，则该节点为转折点，保留该节点；更新路径，依次遍历所有的路径节点，最终得到包含起点、转折点、终点的路径节点集；得到转折点后，继续去除路径节点中的冗余点，所以从第一个节点开始，依次计算节点连线与障碍物的距离，若节点连线到障碍物的最小距离大于无人车能通过的最小安全距离即无人车的车身宽度，则删除这两个节点之间的中间节点，从下一个节点开始，依次重复计算距离和删除节点的操作，直至所有的冗余点都去除，剩下的路径节点则为引导点；

其中，α表示夹角，(x_a，y_a)表示a点坐标，(x_b，y_b)表示b点坐标。

进一步地，所述步骤S8的具体内容为：

根据无人车的运动学限制，计算出动态窗口的运动学约束：

V_s＝{(v,ω)|v∈[0,v_max]∧ω∈[-ω_max,ω_max]}

动力学速度约束，由于无人车驱动电机转矩限制，在动态窗口采样时间间隔Δt内,无人车加减速能达到的速度:

障碍物碰撞约束，为保证无人车不与障碍物发生碰撞，采样速度必须保证智能车能在碰到障碍物前停止；

最终智能车可行的采样速度窗口为上述3个速度约束的交集：

V_r＝V_s∩V_d∩V_a。

其中，Vs、Vd、Va表示上述三个速度约束；(v,w)中v表示线速度，w表示角速度；Vr表示动态速度窗口。

进一步地，所述步骤S9的具体内容为：

通过对无人车在速度空间内采样得到的每一组速度(v，ω)，进行行驶轨迹模拟，得到无人车在当前采样速度下的轨迹空间；其中行驶轨迹

按照以下公式模拟：

其中，Δt表示无人车轨迹模拟时间；(v，ω)为无人车采样速度；(x_t，y_t)表示无人车当前位置，θ_t表示无人车当前方向；(x_t+Δt,y_t+Δt)代表无人车模拟位置，θ_t+Δt表示无人车在模拟位置的方向。

进一步地，所述步骤S10的具体内容为：

通过判断动态窗口中每条轨迹上是否存在障碍物以及与最近障碍物之间的距离大小，判断是否需要开始避障；具体为，求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist；若dist＞Stopdist，则此时刻速度为安全速度，需刹车减速防止碰撞障碍物；若dist＜Stopdist，则需要舍弃动态窗口中的会与障碍物发生碰撞的预测轨迹，留下不会发生碰撞的预测轨迹。

进一步地，所述步骤S11的具体内容为：

评价函数为：

G(v,ω)＝δ·(α·H_G(v,ω)+β·D(v,ω)+γ·V(v,ω))

式中，G(v,ω)表示采样速度的总代价值；H_G(v,ω)表示采样速度点方向与当前局部目标点之间的方位角偏差的方向代价；D(v,ω)表示采样速度终点到障碍物的距离；V(v,ω)表示采样速度的速度代价；α、β分别为方向代价、速度代价的代价增益；

其中，总代价函数G(v,ω)中各代价函数H_G(v,ω)、V(v,ω)的具体定义为：

式中，

表示智能车预测位置的运动方向与第i个引导点G_i之间的夹角；P_predict、P_obstacle分别代表预测位置坐标和障碍物的坐标；v_t+Δt表示采样速度的线速度；使评价函数G(v,w)值最大的一组(v,w)即为最优线速度和角速度。

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

(1)本发明首先在A＊算法的基础上提出了节点安全扩展策略以及路径节点二次优化方法，称之为安全A*算法，运用安全A*算法快速找出最优的安全引导点，引导点可以满足规划路径到障碍物的安全距离；保证无人车的通过性。

(2)，以局部目标点作为动态窗口法的局部目标，进行速度采样，实现路径规划与避障，并在MATLAB环境下进行仿真，结果表明无人车可以在全局路径的引领下，安全平稳的避开障碍物，验证了本发明方法的安全性和稳定性。

(3)本发明通过安全A*算法寻找出若干引导点作为动态窗口法的局部目标点，综合了A*算法和动态窗口法的优势，解决了动态窗口法存在的局部最小的问题，实现了无人车的实时全局路径规划及避障功能。

附图说明

图1为本发明实施例的传统A＊算法轨迹图。

图2为本发明实施例的安全A*算法轨迹图。

图3为本发明实施例的传统DWA算法轨迹图。

图4为本发明实施例的融合安全A*-DWA算法轨迹图。

图5为本发明实施例的方法流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图5所示，本实施例提供一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达传感器生成环境地图，并生成栅格地图，并在栅格地图上设置起始点和目标点信息，同时设置无人车的初始速度、角速度为零；

步骤S2：从无人车的当前位置向周围上、下、左、右、左上、左下、右上、右下八个方向扩展节点；可行的扩展节点的扩展原则为：有障碍物不扩展，已扩展过的节点不扩展；即可行的扩展节点同时满足两点原则：1)非障碍节点；2)非已扩展节点。

步骤S3：分别计算步骤S2中可行的扩展节点的总代价值f(m),并按从小到大的原则进行排序；

步骤S4：安全A*算法节点安全扩展：现有的A*算法在进行对角线节点扩展时，不对周围障碍物进行安全检测，造成路径安全性降低，针对此问题，在现有的A*算法的节点扩展、节点代价值计算和排序环节后增加节点扩展安全检测策略，定义为安全A*算法；当安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点；

步骤S5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤S2，重复步骤S2-步骤S5规划出一条无碰撞的最优安全路径；

步骤S6：对步骤S5得到的最优安全路径进行二次优化，选取转折点，去除冗余节点，得到引导点；

步骤S7：初始化局部目标点：将步骤S6中得到的引导点作为动态窗口法的当前局部目标点，无人车在到达局部目标点时并不会停止运动，而是接着向下一个局部目标点运动，并且当前局部目标点消失不再对无人车起导向作用，直到下一个局部目标点为终点；如图3、4所示；

步骤S8：根据采样的线速度和角速度，计算动态速度窗口，并进行速度采样；

步骤S9：根据步骤S8中的采样速度，模拟无人车的行驶轨迹，生成轨迹空间；

步骤S10：根据步骤S9中无人车的模拟轨迹，判断是否需要开始避障；

步骤S11：依据评价函数评价所有可行的模拟轨迹对应的采样速度，得出最优的线速度和角速度；

步骤S12：输出线速度和角速度驱动无人车行驶；

步骤S13：重复以上循环，直至无人车到达终点；

较佳的，在本实施例的步骤S1中初始化；

通过无人车自身携带的摄像头、激光雷达、GPS、惯导、里程计以及加速度和角速度等传感器获取障碍物坐标信息、建立栅格地图和初始状态信息；同时获取目标点信息。

在本实施例中，步骤S3中所述代价值f(m)的计算方式如下：

f(m)＝g(m)+h(m)

其中，g(m)为起始节点到当前扩展节点的实际代价值，h(m)为当前扩展节点到目标点的代价估计值；代价值采用欧几里德距离计算。

在本实施例中，步骤S4中所述的节点安全扩展策略的具体内容为：如图1、2所示，传统A*算法在沿对角线方向扩展时，会存在规划路径穿越障碍物的情况出现，考虑实际情况下无人车的尺寸，以这种路径搜索得到的节点作为局部目标点是不可行的。本实施例中安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，针对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点。

在本实施例中，所述步骤S6的具体内容为：

步骤S5中，安全A*算法规划出的最优安全路径中还包含诸多的冗余点及不必要的转折点，针对此问题对路径进行二次优化，得到引导点；首先，从规划路径的第2个节点开始，分别计算该节点与其父节点和子节点的夹角，若夹角相同，则该节点为冗余点，删除该节点；若夹角不同，则该节点为转折点，保留该节点；更新路径，依次遍历所有的路径节点，最终得到包含起点、转折点、终点的路径节点集；得到转折点后，继续去除路径节点中的冗余点，所以从第一个节点开始，依次计算节点连线与障碍物的距离，若节点连线到障碍物的最小距离大于无人车能通过的最小安全距离(最小安全距离设置为无人车的车身宽度)，则删除这两个节点之间的中间节点，从下一个节点开始，依次重复计算距离和删除节点的操作，直至所有的冗余点都去除，剩下的路径节点则为引导点；

其中α表示夹角，(x_a，y_a)表示a点坐标，(x_b，y_b)表示b点坐标。

在本实施例中，所述步骤S8的具体内容为：

根据无人车的运动学限制，计算出动态窗口的运动学约束：

V_s＝{(v,ω)|v∈[0,v_max]∧ω∈[-ω_max,ω_max]}

动力学速度约束，由于无人车驱动电机转矩限制，在动态窗口采样时间间隔Δt内,无人车加减速能达到的速度:

障碍物碰撞约束，为保证无人车不与障碍物发生碰撞，采样速度必须保证智能车能在碰到障碍物前停止；

最终智能车可行的采样速度窗口为上述3个速度约束的交集：

V_r＝V_s∩V_d∩V_a。

其中，Vs、Vd、Va表示上述三个速度约束；(v,w)中v表示线速度，w表示角速度；Vr表示动态速度窗口。

在本实例中，所述步骤S9的具体内容为：

通过对无人车在速度空间内采样得到的每一组速度(v，ω)，进行行驶轨迹模拟，得到无人车在当前采样速度下的轨迹空间。其中行驶轨迹按照以下公式模拟：

其中Δt表示无人车轨迹模拟时间；(v，ω)为无人车采样速度；(x_t，y_t)表示无人车当前位置，θ_t表示无人车当前方向；(x_t+Δt，y_t+Δt)代表无人车模拟位置，θ_t+Δt表示无人车在模拟位置的方向。在本实施例中，所述步骤S10的具体内容为：

通过判断动态窗口中每条轨迹上是否存在障碍物以及与最近障碍物之间的距离大小，判断是否需要开始避障；具体为，求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist；若dist＞Stopdist，则此时刻速度为安全速度，需刹车减速防止碰撞障碍物；若dist＜Stopdist，则需要舍弃动态窗口中的会与障碍物发生碰撞的预测轨迹，留下不会发生碰撞的预测轨迹。

在本实施例中，所述步骤S11的具体内容为：

采用安全A*算法进行全局路径规划，提取出若干引导点，再以引导点作为动态窗口法的局部目标点，发挥动态窗口法的优势，实现在线实时规划，改善传统动态窗口法局部最优和不可达的问题，同时提高规划路径的平滑度。本实施例算法流程如图5所示。

为了融合A*算法，传统动态窗口法的不足本实施例重新设计了一种满足全局最优路径的动态窗口评价函数。由于本实施例在前述改善A*算法中已经考虑了智能车与障碍物的避障，所以对于传统动态窗口法评价函数中的表示采样速度到障碍物距离的一项可以去掉。所以，重新设计的评价函数为：

G(v,ω)＝δ·(α·H_G(v,ω)+β·D(v,ω)+γ·V(v,ω))

式中，G(v,ω)表示采样速度的总代价值；H_G(v,ω)表示采样速度点方向与当前局部目标点之间的方位角偏差的方向代价；D(v,ω)表示采样速度终点到障碍物的距离；V(v,ω)表示采样速度的速度代价；α、β分别为方向代价、速度代价的代价增益；

其中，总代价函数G(v,ω)中各代价函数H_G(v,ω)、V(v,ω)的具体定义为：

式中，

表示智能车预测位置的运动方向与第i个引导点G_i之间的夹角；P_predict、P_obstacle分别代表预测位置坐标和障碍物的坐标；v_t+Δt表示采样速度的线速度；使评价函数G(v,w)值最大的一组(v,w)即为最优线速度和角速度。

在本实施例中，所述步骤S12的具体内容为：步骤S11中求出评价函数最大时对应轨迹的(v,ω)，并将其发送至无人车的执行机构，通过电信号控制电机的转速和方向盘转角，控制无人车沿着最优避障路径行驶。

较佳的，在本实施例中，动态窗口法是一种典型的局部路径规划算法，实际应用时，却存在无人车陷入局部最小的问题。本实施例通过安全A*算法寻找出若干引导点作为动态窗口法的局部目标点，综合了A*算法和动态窗口法的优势，解决了动态窗口法存在的局部最小的问题，实现了无人车的实时全局路径规划，并将其应用了复杂环境下仿真，仿真结果表明无人车能够有效的避开障碍物，完成全局路径的行驶。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (8)

1.一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达传感器生成环境地图，并生成栅格地图，并在栅格地图上设置起始点和目标点信息，同时设置无人车的初始速度、角速度为零；

步骤S2：从无人车的当前位置向周围上、下、左、右、左上、左下、右上、右下八个方向扩展节点；可行的扩展节点的扩展原则为：有障碍物不扩展，已扩展过的节点不扩展；

步骤S3：分别计算步骤S2中可行的扩展节点的总代价值f(m)，并按从小到大的原则进行排序；

步骤S4：安全A*算法节点安全扩展：在A*算法的节点扩展、节点代价值计算和排序环节后增加节点扩展安全检测策略，定义为安全A*算法；当安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点；

步骤S5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤S2，重复步骤S2-步骤S5规划出一条无碰撞的最优安全路径；

步骤S6：对步骤S5得到的最优安全路径进行二次优化，选取转折点，去除冗余节点，得到引导点；

步骤S7：初始化局部目标点：将步骤S6中得到的引导点作为动态窗口法的当前局部目标点，无人车在到达局部目标点时并不会停止运动，而是接着向下一个局部目标点运动，并且当前局部目标点消失不再对无人车起导向作用，直到下一个局部目标点为终点；

步骤S8：根据采样的线速度和角速度，计算动态速度窗口，并进行速度采样；

步骤S9：根据步骤S8中的采样速度，模拟无人车的行驶轨迹，生成轨迹空间；

步骤S10：根据步骤S9中无人车的模拟轨迹，判断是否需要开始避障；

步骤S11：依据评价函数评价所有可行的轨迹对应的采样速度，得出最优的线速度和角速度；

步骤S12：输出线速度和角速度驱动无人车行驶；

步骤S13：重复以上循环，直至无人车到达终点。

2.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：步骤S3中所述代价值f(m)的计算方式如下：

f(m)＝g(m)+h(m)

其中，g(m)为起始节点到当前扩展节点的实际代价值，h(m)为当前扩展节点到目标点的代价估计值；代价值采用欧几里德距离计算。

3.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：步骤S4中所述的节点安全扩展策略的具体内容为：安全A*算法扩展节点代价值最小值的节点为对角线方向的节点时，针对与当前节点和待扩展节点都相邻的两个节点进行安全检测，若相邻的两个栅格有一个为障碍物，则不扩展该对角线方向的节点，而扩展代价值仅次于该节点的其他节点。

4.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：所述步骤S6的具体内容为：

步骤S5中，安全A*算法规划出的最优安全路径中还包含诸多的冗余点及不必要的转折点，针对此问题对路径进行二次优化，得到引导点；首先，从规划路径的第2个节点开始，分别计算该节点与其父节点和子节点的夹角，若夹角相同，则该节点为冗余点，删除该节点；若夹角不同，则该节点为转折点，保留该节点；更新路径，依次遍历所有的路径节点，最终得到包含起点、转折点、终点的路径节点集；得到转折点后，继续去除路径节点中的冗余点，所以从第一个节点开始，依次计算节点连线与障碍物的距离，若节点连线到障碍物的最小距离大于无人车能通过的最小安全距离即无人车的车身宽度，则删除这两个节点之间的中间节点，从下一个节点开始，依次重复计算距离和删除节点的操作直至所有的冗余点都去除，剩下的路径节点则为引导点；

其中α表示夹角，(x_a，y_a)表示a点坐标，(x_b，y_b)表示b点坐标。

5.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：所述步骤S8的具体内容为：

根据无人车的运动学限制，计算出动态窗口的运动学约束：

V_s＝{(v，ω)|v∈[0，v_max]∧ω∈[-ω_max，ω_max]}

动力学速度约束，由于无人车驱动电机转矩限制，在动态窗口采样时间间隔Δt内，无人车加减速能达到的速度：

障碍物碰撞约束，为保证无人车不与障碍物发生碰撞，采样速度必须保证智能车能在碰到障碍物前停止；

最终智能车可行的采样速度窗口为上述3个速度约束的交集：

V_r＝V_s∩V_d∩V_a。

其中，Vs、Vd、Va表示上述三个速度约束；(v，w)中v表示线速度，w表示角速度；Vr表示动态速度窗口。

6.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：所述步骤S9的具体内容为：

通过对无人车在速度空间内采样得到的每一组速度(v，ω)，进行行驶轨迹模拟，得到无人车在当前采样速度下的轨迹空间；其中行驶轨迹

按照以下公式模拟：

其中，Δt表示无人车轨迹模拟时间；(v，ω)为无人车采样速度；(x_t，y_t)表示无人车当前位置，θ_t表示无人车当前方向；(x_t+Δt，y_t+Δt)代表无人车模拟位置，θ_t+Δt表示无人车在模拟位置的方向。

7.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：所述步骤S10的具体内容为：

通过判断动态窗口中每条轨迹上与最近障碍物之间的距离大小，判断是否需要开始避障；具体为，求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist；若dist＞Stopdist，则此时刻速度为安全速度，需刹车减速防止碰撞障碍物；若dist＜Stopdist，则需要舍弃动态窗口中的会与障碍物发生碰撞的预测轨迹，留下不会发生碰撞的预测轨迹。

8.根据权利要求1所述的一种基于安全A*引导点的动态窗口的无人车实时全局路径规划方法，其特征在于：所述步骤S11的具体内容为：

评价函数为：

G(v，ω)＝δ·(α·H_G(v，ω)+β·D(v，ω)+γ·V(v，ω))

式中，G(v，ω)表示采样速度的总代价值；H_G(v，ω)表示采样速度点方向与当前局部目标点之间的方位角偏差的方向代价；D(v，ω)表示采样速度终点到障碍物的距离；V(v，ω)表示采样速度的速度代价；α、β分别为方向代价、速度代价的代价增益；

其中，总代价函数G(v，ω)中各代价函数H_G(v，ω)、V(v，ω)的具体定义为：

式中，

表示智能车预测位置的运动方向与第i个引导点G_i之间的夹角；P_predict、P_obstacle分别代表预测位置坐标和障碍物的坐标；v_i+Δt表示采样速度的线速度；使评价函数G(v，w)值最大的一组(v，w)即为最优线速度和角速度。

Images