CN113495566B - 一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法 - Google Patents

Abstract

本发明涉及一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，在D*Lite算法的基础上增加距离函数和子代节点转角约束，让算法从终点到起点反向规划出一条启发值最小的路径，规划出的路径作为算法的全局路径，为局部动态路径规划提供一定的数据基础；从起点开始，以新的子代节点拓展方式获得子代节点，同时以当前节点为圆心，R为半径建立移动窗口，当移动窗口内出现动态障碍物时，在当前节点建立势力场方程，并将合力的方向加入到子代节点的选取中；无人车以一定频率反馈的位置信息和转角信息，根据无人车反馈信息进行重规划；输出最佳的速度的大小和方向驱动无人车行驶。本发明规划出的路径满足车辆运动学模型，并具有良好的动态避障能力。

Description

一种基于曲率约束融合势场法的D*Lite无人车局部路径规划 方法

技术领域

本发明涉及无人车实时避障的局部路径规划领域，特别是一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法。

背景技术

对于无人驾驶车辆系统而言，其关键技术主要包括环境感知并精准建图、路径规划和路径追踪控制。其中，路径规划需根据已获得的环境信息，为无人驾驶车辆提供一条合理、安全的路径，路径规划作为无人驾驶架构体系的核心环节之一，其表现直接影响车辆行驶安全和用户体验，因而具有很强的理论研究意义和工程应用价值。

路径规划的分类方法有很多种，比较典型是依据无人车对作业环境信息的掌握程度来分类，一般分为完全掌握地图信息的全局路径规划和不完全掌握地图信息的局部路径规划。全局路径规划通常假设环境信息完全已知，在无人车运动之前，离线计算出一条从起点到终点的路径；局部路径规划则是在无人车运动时，可以在线的、实时的实现障碍物规避。由于局部路径规划方法有计算量小，实时性好，规划轨迹圆滑等优点，目前通常采用把全局与局部路径规划结合的二层避障结构，在第一层，通过已知的环境信息(一般为地图)，离线规划出一条从起点到目标点启发值最小的路径，该路径不需平滑但应能避开地图上已知的静态障碍物；在第二层，无人车沿着第一层规划的路径行驶，当在行驶过程中通过传感器检测到新的障碍物时，再通过获得的障碍物详细的位置信息进行局部避障。

D*Lite算法作为一种可用于局部路径规划且能快速获得最佳路径的规划算法，但在实际无人车路径规划中存在距动态障碍物过近、路径不平滑等不满足车辆运动学模型的问题。

发明内容

有鉴于此，本发明的目的是提供一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，能对动态障碍物具有良好的规避能力，且规划的路径平滑，曲率变化更线性。

本发明采用以下方案实现：一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达获得环境地图，并生成二维栅格地图，同时，将无人车初始速度为零，初始角度为零；

步骤S2：规划出全局路径：规划出从目标点到起始点的全局最优栅格路径，并建立路径场信息；

步骤S3：计算子代节点的启发值：以当前节点的位置信息和转角信息，获得各子代节点位置，并根据公式计算各子代节点的位置启发值；

步骤S4：从当前节点开始以R为半径建立移动窗口，作为无人车局部规划的范围；在局部路径规划中，当移动窗口内没有动态障碍物，无人车将以步骤S3中所计算的启发值最小的子代节点作为下一代路径节点；

步骤S5：当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行加权处理，并以加权处理后启发值最小的子代节点作为下一代节点；

步骤S6：以步骤S5所计算合力的方向对新的拓展方式的各节点进行加权处理，以获得最新的子代节点代价值，并以子代节点启发值最小节点做为下一代节点，并根据子代节点的转弯半径输出相应的前轮转角和相应的速度大小来驱动无人车行驶；

步骤S7：无人车以预设频率反馈位置信息和转角信息，根据无人车反馈信息进行重规划；

步骤S8：重复步骤S2到步骤S7循环，直至无人车到达终点。

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

使用增加节点约束和距离函数的D*Lite反向规划出全局路径，用以避免规划出的全局路径出现直角及锐角转弯的情况；对栅格地图引入距离启发值，即在障碍物周边r之内可得一个随着与障碍物之间的距离越远，距离启发值越小的数值，r的取值为2倍车身宽度；节点s的启发距离值dist(s)为：

在搜索过程中，保存不连续节点的优先队列定义为U，在U中的节点按其key值从小到大排列，其中key是一个二维向量,定义为：

由前节点s后继节点得到s到目标节点的启发路径记为rhs(s)，其也作为全局路径获取的依据，rhs(s)定义为：

从目标点开始，计算当前点各子代节点的距离函数dist(s)，再计算各点的key(s)，将子代各节点的坐标值和key(s)加入到优先队列U中，在优先队列U中会依据每个节点的key值进行排序，通过排序后的U不断将key最小的值取出来，将相关邻接节点或者状态变动的节点加入到队列中，并获得优先队列U中最小值节点的节点启发值rhs，直到到达起始点即获得全局栅格路径；根据所遍历的节点，计算过启发值rhs的节点，获得地图路径场信息；

令S_start为当前节点，S_parent为当前节点的父代节点，S_child为当前节点的子代节点，邻近节点S_near为子代节点的下一代节点；从当前节点S_start出发，为控制当前节点的子代节点S_child与当前节点的连线和当前节点S_start与当前节点的父代节点S_parent为的连线保持在±45°内，需满足公式(1.4)；同时，为让转弯长D满足相应要求，要让当前节点S_start与邻近节点S_near共线，需满足公式(1.5)；这让子代搜索节点减少为3个，并且满足车辆运动学模型；

dist(S_child,S_parent)≥2d (1.4)

dist(S_near,S_start)＝L*dist(S_child,S_start) (1.5)。

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

在获得全局路径后，以当前节点的位置和转角，根据新的子代节点拓展方式，即以S_start为当前节点，以车身长度为参考使用不同的转弯半径，使用等步长的方式获得新的子代节点；根据当前节点的位置和角度，获得各子代节点的位置信息和角度信息，根据各节点位置信息，按照特定规则计算各子代节点的启发值；

在状态空间中，(x,y)为车辆后轴中心点在全局坐标系中的位置；θ为车辆的前轮转向角，且|θ|小于车辆最大转向角θ_max；L为车辆的轴距，R为转弯半径；由于车辆的运动约束，车辆前轮转向角与轴距、转弯半径满足公式(1.6)。

当车辆的轴距固定时，不同的前轮转向角能获得不同的车辆转弯半径；当取前轮转角为0°、10°、15°、20°，获得的车辆转弯半径为∞、5.67L、3.73L、2.75L；根据不同的转弯半径给无人车输出相应的速度v，0.8v，0.6v，0.4v。

进一步地，所述按照特定规则计算各子代节点的启发值的具体内容为：

根据步骤S2所述获得节点启发值的公式和公式(1.7)计算各子代节点的启发值rhs；

在节点代价函数的中加入对曲率变化的惩罚值，并考虑节点拓展时的步长代价，用以使远端节点优先选取；各子节点的最终启发值

G_i的计算公式如下：

式中:G_i为子节点的最终启发值；G_p为父节点的启发值；α，β，γ，k为权重系数，能够通过多次试验获得；F_c(X)为合力；f_1,i和f_2,i分别为步长代价因子和曲率变化惩罚系数；d_i为父代节点到子代节点的步长；c_i，c_p分别为子代和父代的曲率。

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

当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行矢量转换获得引力，将各矢量进行矢量求和获得合力的方向，在将合力的大小和方向对各子代节点进行加权处理；将斥力函数进行负梯度处理得到斥力引力F_rep(X)，斥力、引力、合力如下：

斥力：

引力：F_att(X)＝βe(X,X_G)(1.8)

合力：F_c(X)＝F_att(X)+F_rep(X)(1.9)

上式中:α为斥力增益系数、β为引力增益系数、ρ_i为智能车与障碍物之间的最短距离、ρ0为障碍物斥力有效影响范围、d是需保持的最小安全距离、e(X,X_i)为障碍物对无人车产生斥力的单位向量、e(X,X_G)为平行于当前节点的全局路径的单位向量；

在获得合力的大小方向后，需要对新拓展方式的各子代节点进行加权处理，用以获得局部规划能力；

RHS_(i)＝-rhs_(i)×α|θ_i-θ_F| (1.10)

上式中:RHS_(i)为加权处理后的子代节点启发值，rhs_(i)为加权处理前的子代节点启发值，α为加权增益系数，θ_i为节点i当前的转角，θ_F为合力的转角。与现有技术相比，本发明具有以下有益效果：

本发明在启发函数中增加限定范围的势场函数模型，并重新构建子节点拓展方式，解决了D*Lite算法规划出路径距动态障碍物过近和路径不满足车辆运动学模型的问题，实现了无人车的实时局部路径规划，并将其应用了复杂环境下仿真，仿真结果表明无人车能在局部动态环境中合理规划出一条合理、满足车辆运动学模型的路径。

附图说明

图1为本发明实施例的全局探索中子代节点拓展方式图。

图2为本发明实施例的车辆运动学约束示意图。

图3为本发明实施例的势场法动态避障示意图。

图4为本发明实施例的新的子代节点拓展方式图。

图5为本发明实施例的在matlab中的动态仿真效果图。

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

具体实施方式

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

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

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

如图3、4所示，本实施例提供一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达获得环境地图，并生成二维栅格地图，同时，将无人车初始速度为零，初始角度为零；

步骤S2：规划出全局路径：规划出从目标点到起始点的全局最优栅格路径，并建立路径场信息；

步骤S3：计算子代节点的启发值：以当前节点的位置信息和转角信息，获得各子代节点位置，并根据公式计算各子代节点的位置启发值；

步骤S4：从当前节点开始以R为半径建立移动窗口，作为无人车局部规划的范围；在局部路径规划中，当移动窗口内没有动态障碍物，无人车将以步骤S3中所计算的启发值最小的子代节点作为下一代路径节点；

步骤S5：当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行加权处理，并以加权处理后启发值最小的子代节点作为下一代节点；

步骤S6：以步骤S5所计算合力的方向对新的拓展方式的各节点进行加权处理，以获得最新的子代节点代价值，并以子代节点启发值最小节点做为下一代节点，并根据子代节点的转弯半径输出相应的前轮转角和相应的速度大小来驱动无人车行驶；

步骤S7：无人车以预设频率(仿真算法使用20Hz)反馈的位置信息和转角信息，根据无人车反馈信息进行重规划；

步骤S8：重复步骤S2到步骤S7循环，直至无人车到达终点。

较佳的，在本实施例中，通过无人车自身携带的摄像头、激光雷达、GPS、惯导、里程计以及加速度和角速度等传感器的数据融合进行建模获取障碍物坐标信息、建立栅格地图和初始状态信息。

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

使用增加节点约束和距离函数的D*Lite反向规划出全局路径，用以避免规划出的全局路径出现直角及锐角转弯的情况，由于距离函数的加入，让远离障碍物的节点优先获取，让规划的路径能与障碍物保持一定的距离；

对栅格地图引入距离启发值，即在障碍物周边r之内可得一个随着与障碍物之间的距离越远，距离启发值越小的数值，r的取值为2倍车身宽度；节点s的启发距离值dist(s)为：

在搜索过程中，保存不连续节点的优先队列定义为U，在U中的节点按其key值从小到大排列，其中key是一个二维向量,定义为：

由前节点s后继节点得到s到目标节点的启发路径记为rhs(s)，其也作为全局路径获取的依据，rhs(s)定义为：

从目标点开始，计算当前点各子代节点的距离函数dist(s)，再计算各点的key(s)，将子代各节点的坐标值和key(s)加入到优先队列U中，在优先队列U中会依据每个节点的key值进行排序，通过排序后的U不断将key最小的值取出来，将相关邻接节点或者状态变动的节点加入到队列中，并获得优先队列U中最小值节点的节点启发值rhs，直到到达起始点即获得全局栅格路径；根据所遍历的节点，计算过启发值rhs的节点，获得地图路径场信息；

如图1所示，令S_start为当前节点，S_parent为当前节点的父代节点，S_child为当前节点的子代节点，邻近节点S_near为子代节点的下一代节点；如附图1所示，从当前节点S_start出发，为控制当前节点的子代节点S_child与当前节点的连线和当前节点S_start与当前节点的父代节点S_parent为的连线保持在±45°内，需满足公式(1.4)；同时，为让转弯长D满足相应要求，要让当前节点S_start与邻近节点S_near共线，需满足公式(1.5)；这让子代搜索节点减少为3个，并且满足车辆运动学模型；

di_st(S_child,S_parent)≥2d (1.4)

dist(S_near,S_start)＝L*dist(S_child,S_start) (1.5)。

传统D*Lite算法采用8领域探索，导致路径会出现直角，甚至出现锐角等不合符车辆运动学的路径，为了让规划出的全局路径更贴合车辆的运动状态，在全局路径规划中考虑车辆的当前朝向角，让子代节点减少为3个，这不仅加快了路径获取速度，还让规划出的全局路径更平滑，并在原算法的基础上添加距离函数，让规划出路径能与障碍物保持一定的安全距离。在规划全局路径的同时，建立的“节点代价”信息也为接下来的局部路径规划提供数据基础。

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

在获得全局路径后，以当前节点的位置和转角，根据新的子代节点拓展方式，如附图3所示，即以S_start为当前节点，以车身长度为参考使用不同的转弯半径，使用等步长的方式获得新的子代节点；根据当前节点的位置和角度，获得各子代节点的位置信息和角度信息，根据各节点位置信息，按照特定规则计算各子代节点的启发值。

如图2，在状态空间中，(x,y)为车辆后轴中心点在全局坐标系中的位置；θ为车辆的前轮转向角，且|θ|小于车辆最大转向角θ_max；L为车辆的轴距，R为转弯半径；由于车辆的运动约束，车辆前轮转向角与轴距、转弯半径满足公式(1.6)。

当车辆的轴距固定时，不同的前轮转向角能获得不同的车辆转弯半径；当取前轮转角为0°、10°、15°、20°，获得的车辆转弯半径为∞、5.67L、3.73L、2.75L；根据不同的转弯半径给无人车输出相应的速度v，0.8v，0.6v，0.4v。

在本实施例中，所述按照特定规则计算各子代节点的启发值的具体内容为：

根据步骤S2所述获得节点启发值的公式和公式(1.7)计算各子代节点的启发值rhs；

在节点代价函数的中加入对曲率变化的惩罚值，并考虑节点拓展时的步长代价，用以使远端节点优先选取；各子节点的最终启发值G_i的计算公式如下：

式中:G_i为子节点的最终启发值；G_p为父节点的启发值；α，β，γ，k为权重系数，能够通过多次试验获得；F_c(X)为合力；f_1,i和f_2,i分别为步长代价因子和曲率变化惩罚系数；d_i为父代节点到子代节点的步长；c_i，c_p分别为子代和父代的曲率。

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

如图1所示，当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行矢量转换获得引力，将各矢量进行矢量求和获得合力的方向，在将合力的大小和方向对各子代节点进行加权处理；将斥力函数进行负梯度处理得到斥力引力F_rep(X)，斥力、引力、合力如下：斥力：

引力：F_att(X)＝βe(X,X_G)(1.9)合力：F_c(X)＝F_att(X)+F_rep(X)(1.10)

上式中:α为斥力增益系数、β为引力增益系数、ρ_i为智能车与障碍物之间的最短距离、ρ₀为障碍物斥力有效影响范围、d是需保持的最小安全距离、e(X,X_i)为障碍物对无人车产生斥力的单位向量、e(X,X_G)为平行于当前节点的全局路径的单位向量；

在获得合力的大小方向后，需要对新拓展方式的各子代节点进行加权处理，用以获得局部规划能力；

RHS_(i)＝-rhs_(i)×α|θ_i-θ_F| (1.11)

上式中:RHS_(i)为加权处理后的子代节点启发值，rhs_(i)为加权处理前的子代节点启发值，α为加权增益系数，θ_i为节点i当前的转角，θ_F为合力的转角。

较佳的，本实施例通过重新构建D*Lite子代节点拓展方式，并引进势场法的思想，提出一种新的可应用于无人车动态路径规划的方法。并在动态环境在进行仿真验证，新算法规划出的路径满足车辆运动学模型，并具有良好的动态避障能力。

较佳的，在本实施例中，第一，本实施例在D*Lite算法的基础上增加距离函数和子代节点转角约束，让算法从终点到起点反向规划出一条启发值最小的路径，规划出的路径作为算法的全局路径，为局部动态路径规划提供一定的数据基础；第二，从起点开始，以新的子代节点拓展方式获得子代节点，同时以当前节点为圆心，R为半径建立移动窗口，当移动窗口内出现动态障碍物时，在当前节点建立势力场方程，并将合力的方向加入到子代节点的选取中；如图5、6所示第三，在MATLAB环境下进行仿真，结果表明，本实施例在动态环境中，能规划出一条合理、安全、满足车辆运动学模型的路径。以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (5)

1.一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，其特征在于：包括以下步骤：

步骤S1：初始化：通过无人车上的激光雷达获得环境地图，并生成二维栅格地图，同时，将无人车初始速度为零，初始角度为零；

步骤S2：规划出全局路径：规划出从目标点到起始点的全局最优栅格路径，并建立路径场信息；

步骤S3：计算子代节点的启发值：以当前节点的位置信息和转角信息，获得各子代节点位置，并根据公式计算各子代节点的位置启发值；

步骤S4：从当前节点开始以R为半径建立移动窗口，作为无人车局部规划的范围；在局部路径规划中，当移动窗口内没有动态障碍物，无人车将以步骤S3中所计算的启发值最小的子代节点作为下一代路径节点；

步骤S5：当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行加权处理，并以加权处理后启发值最小的子代节点作为下一代节点；

步骤S6：以步骤S5所计算合力的方向对新的拓展方式的各节点进行加权处理，以获得最新的子代节点代价值，并以子代节点启发值最小节点做为下一代节点，并根据子代节点的转弯半径输出相应的前轮转角和相应的速度大小来驱动无人车行驶；

步骤S7：无人车以预设频率反馈位置信息和转角信息，根据无人车反馈信息进行重规划；

步骤S8：重复步骤S2到步骤S7循环，直至无人车到达终点。

2.根据权利要求1所述的一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，其特征在于：所述步骤S2的具体内容为：

使用增加节点约束和距离函数的D*Lite反向规划出全局路径，用以避免规划出的全局路径出现直角及锐角转弯的情况；对栅格地图引入距离启发值，即在障碍物周边r之内可得一个随着与障碍物之间的距离越远，距离启发值越小的数值，r的取值为2倍车身宽度；节点s的启发距离值dist(s)为：

在搜索过程中，保存不连续节点的优先队列定义为U，在U中的节点按其key值从小到大排列，其中key是一个二维向量,定义为：

由前节点s后继节点得到s到目标节点的启发路径记为rhs(s)，其也作为全局路径获取的依据，rhs(s)定义为：

从目标点开始，计算当前点各子代节点的距离函数dist(s)，再计算各点的key(s)，将子代各节点的坐标值和key(s)加入到优先队列U中，在优先队列U中会依据每个节点的key值进行排序，通过排序后的U不断将key最小的值取出来，将相关邻接节点或者状态变动的节点加入到队列中，并获得优先队列U中最小值节点的节点启发值rhs，直到到达起始点即获得全局栅格路径；根据所遍历的节点，计算过启发值rhs的节点，获得地图路径场信息；

令S_start为当前节点，S_parent为当前节点的父代节点，S_child为当前节点的子代节点，邻近节点S_near为子代节点的下一代节点；从当前节点S_start出发，为控制当前节点的子代节点S_child与当前节点的连线和当前节点S_start与当前节点的父代节点S_parent为的连线保持在±45°内，需满足公式(1.4)；同时，为让转弯长D满足相应要求，要让当前节点S_start与邻近节点S_near共线，需满足公式(1.5)；这让子代搜索节点减少为3个，并且满足车辆运动学模型；

dist(S_child,S_parent)≥2d (1.4)

dist(S_near,S_start)＝L*dist(S_child,S_start) (1.5)

3.根据权利要求1所述的一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，其特征在于：所述步骤S3的具体内容为：

在获得全局路径后，以当前节点的位置和转角，根据新的子代节点拓展方式，即以S_start为当前节点，以车身长度为参考使用不同的转弯半径，使用等步长的方式获得新的子代节点；根据当前节点的位置和角度，获得各子代节点的位置信息和角度信息，根据各节点位置信息，按照特定规则计算各子代节点的启发值；

在状态空间中，(x,y)为车辆后轴中心点在全局坐标系中的位置；θ为车辆的前轮转向角，且|θ|小于车辆最大转向角θ_max；L为车辆的轴距，R为转弯半径；由于车辆的运动约束，车辆前轮转向角与轴距、转弯半径满足公式(1.6);

当车辆的轴距固定时，不同的前轮转向角能获得不同的车辆转弯半径；当取前轮转角为0°、10°、15°、20°，获得的车辆转弯半径为∞、5.67L、3.73L、2.75L；根据不同的转弯半径给无人车输出相应的速度v，0.8v，0.6v，0.4v。

4.根据权利要求3所述的一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，其特征在于：所述按照特定规则计算各子代节点的启发值的具体内容为：

根据步骤S2所述获得节点启发值的公式和公式(1.7)计算各子代节点的启发值rhs；

在节点代价函数的中加入对曲率变化的惩罚值，并考虑节点拓展时的步长代价，用以使远端节点优先选取；各子节点的最终启发值G_i的计算公式如下：

式中:G_i为子节点的最终启发值；G_p为父节点的启发值；α，β，γ，k为权重系数，能够通过多次试验获得；F_c(X)为合力；f_1,i和f_2,i分别为步长代价因子和曲率变化惩罚系数；d_i为父代节点到子代节点的步长；c_i，c_p分别为子代和父代的曲率。

5.根据权利要求1所述的一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法，其特征在于：所述步骤S5的具体内容为：

当移动窗口内出现动态障碍物，无人车将在当前节点中建立势场方程，并将所计算的合力方向和步骤S3所计算的子代节点进行矢量转换获得引力，将各矢量进行矢量求和获得合力的方向，在将合力的大小和方向对各子代节点进行加权处理；将斥力函数进行负梯度处理得到斥力引力F_rep(X)，斥力、引力、合力如下：

斥力：

引力：F_att(X)＝βe(X,X_G) (1.9)合力：F_c(X)＝F_att(X)+F_rep(X) (1.10)

上式中:α为斥力增益系数、β为引力增益系数、ρ_i为智能车与障碍物之间的最短距离、ρ₀为障碍物斥力有效影响范围、d是需保持的最小安全距离、e(X,X_i)为障碍物对无人车产生斥力的单位向量、e(X,X_G)为平行于当前节点的全局路径的单位向量；

在获得合力的大小方向后，需要对新拓展方式的各子代节点进行加权处理，用以获得局部规划能力；

RHS_(i)＝-rhs_(i)×α|θ_i-θ_F| (1.11)

上式中:RHS_(i)为加权处理后的子代节点启发值，rhs_(i)为加权处理前的子代节点启发值，α为加权增益系数，θ_i为节点i当前的转角，θ_F为合力的转角。

Images