基于梯度的前向蚁群算法无人车路径规划方法

1.本发明涉及一种基于梯度的前向蚁群算法无人车路径规划方法。


背景技术：

2.车辆的路径规划指的是已知车辆的行驶环境，同时在环境中给定无人驾驶汽车行驶的起始点和目标点，在符合车辆行驶条件的前提下，避开行驶环境中的所有障碍物，同时生成有效的行驶路径，该路径为该行驶环境中的最优路径。无人驾驶汽车的路径规划主要包含以下两个方面：一是利用多种传感器采集获得所在行驶环境的地图，二是利用路径规划的搜索算法在获得的行驶环境地图上搜索规划一条可行的路径。其决策标准可以是基于最小的路径长度或是从初始位置到目标位置的最小运动时间。
3.路径规划可以分为静态环境下的全局路径规划和动态环境下的局部路径规划。静态环境下障碍物不会运动，因此全局路径规划算法的核心思想为在已知全局环境下各个障碍物的位置、大小等相关信息，以此进行最优路径的求取。而动态环境是在静态环境下，存在一些移动且运动轨迹未知的障碍物，传感器需要捕捉运动障碍物的信息，实时的进行路径规划。因此，局部路径规划算法则注重无人车的动态避障能力，它的核心思想为在未知环境下通过传感器获取局部信息来进行路径的求取。


技术实现要素：

4.有鉴于此，本发明的目的在于提供一种基于梯度的前向蚁群算法无人车路径规划方法，缩短了路径规划时间，实现了良好的路径规划与避障功能。
5.为实现上述目的，本发明采用如下技术方案：
6.一种基于梯度的前向蚁群算法无人车路径规划方法，包括以下步骤：
7.步骤s1：初始化无人车状态参数，通过传感器扫描环境生成灰度地图，生成栅格地图，并在栅格地图上设置起始点和目标点信息；
8.步骤s2：基于无人车的当前位置，以八个领域扩展的方式向周为扩展节点，获取可行的扩展节点；
9.步骤s3：结合动态规划与di jkstra算法分别计算可行的扩展节点的总代价值g
i,j
,并生成动态规划地图；
10.步骤s4：基于动态规划地图，采用前向蚁群算法节点选择策略，获取最优路径；
11.步骤s5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤s2；
12.步骤s6：对最优路径进行二次优化，去除冗余路径节点，得到关键节点；
13.步骤s7：对相邻的关键节点进行等距离插值；
14.步骤s8：构建目标函数，并采用梯度下降法进行求解；
15.步骤s9：对路径三次样条插值，保证光滑的路径具有二次连续性，获取最终的最优路径。
16.进一步的，所述步骤s1，具体为：根据无人车的状态参数构建运动学单车模型；通过无人车自身携带的摄像头、激光雷达、gps、惯导、里程计、加速度和角速度传感器的数据融合进行建模获取障碍物坐标信息、建立栅格地图和初始状态信息；同时获取目标点信息。
17.进一步的，所述步骤s3，具体为：
18.将终点代价值记为0，代价值g
i,j
的计算方式如下：
[0019][0020]
其中g
i,j
表示位于第i行第j列栅格的代价值，代价值采用欧几里德距离计算，即第i行第j列栅格的代价值与其上下左右栅格的代价值相差为1，与其左上，右上，左下，右下的栅格的代价值相差为
[0021]
进一步的，所述前向蚁群算法节点选择策略，具体为：
[0022]
将地图划分为若干个大小相等的栅格，可达节点与初始航向的夹角用θ表示；获取所有可达节点与初始航向的夹角θ
ij
，其中i、j为节点序号；将无人车转向角定义为h
ij
，由于θ
ij
越大，转弯代价越大，为降低转弯代价，蚂蚁会以更大的概率选择较小的θ
ij
[0023]
将蚁群算法的全局最优性和向前走节点选择策略具备的较低转弯代价的能力结合起来，构造顾及路径距离和行驶效率的转移概率函数：
[0024][0025]
式中：为第k只蚂蚁从i节点到节点j的转移概率；τ
ij
为由节点i到节点j的信息素强度；η
ij
为启发函数，是节点i到节点j之间距离的倒数；allowk为访问的节点集合；α和β分别为信息素启发因子和启发函数权重值；μ为转向因子，表示蚂蚁向前走的重要程度。
[0026]
进一步的，所述步骤s6具体为：
[0027]
(1)获取向前走节点选择策略蚁群算法所规划的全部节点集p＝{pi,1《i≤n},其中p1表示规划路径的起点，pn表示规划路径的终点；创建初始值只包含起点p1和pn的关键节点集v＝{p1,pn}用来存放算法关键节点提取算法优化后的关键转折点；
[0028]
(2)从p1开始依次连接p3,p4,
…
,pk,若直到直线p1pk经过障碍物，则将p
k-1
为关键节点，并添加到集合v中。并从p
k-1
继续重复以上方式选取关键节点，直到连接到终点pn；
[0029]
(3)关键节点筛选完后，集合v＝{p1,p
k-1
,
…
,pn}包含所有关键节点，连接集合v中所有的节点，得到优化后的路径。
[0030]
进一步的，步骤s8中考虑路径的最优性，平滑性，安全性，运动学可行性构建目标函数，具体为：
[0031]
将路径优化问题看作非线性优化问题，把等距插值后的各路径点的曲率和至最近障碍物的距离作为目标函数的约束条件，同时，约束优化后的路径点与原始路径点的距离
[0032]
设等距插值后得到的路径点坐标序列为xi＝(xi,yi)，i∈[1,n]，其中n为路径点的数量；优化后的路径点坐标序列列为yi＝(x'i,y'i)，i∈[1,n]；考虑路径的平滑性fs，最优性fd，安全性f
ε
和运动学可行性fk，分别定义了四个惩罚函数和对应的惩罚权重，将四项相
加得到如下的目标函数：
[0033]
ω＝ωs·fs
+ωd·
fd+ωd·fo
+ωd·fk
[0034]
其中，ωs、ωd、ωo、ωk为四个惩罚函数对应的惩罚权重；
[0035]
式中：
[0036][0037][0038][0039][0040][0041]
δxi＝|x
i+1-x
i-1
|
[0042]
其中，yi为第i个路径点的纵坐标；r
min
为无人车最小转弯半径；oi为距离yi最近的障碍物坐标；d
max
为预设的安全距离；ωs、ωd、ωo、ωk为各项权重。
[0043]
进一步的，步骤s8中采用梯度下降法，将最优路径等距采样后的路径点作为梯度下降算法的初始解，求解出全局最优路径点，具体为：
[0044]
其中平滑性fs、最优性fd和安全性f
ε
三项的梯度如下：
[0045][0046][0047][0048]
在运动学可行性指标fk中，由曲率定义得：
[0049][0050]
其中是δx'i和δx'
i+1
之间的夹角，由两者表示：
[0051][0052]
计算相关的三个点的偏导：
[0053][0054][0055][0056]
其中
[0057][0058]
进一步的，所述步骤s9具体为：
[0059]
(1)设将路径区间[a,b]分成n个区间(x0,x1),(x1,x2),
…
(x
n-1
,xn)，其中路径端点x0＝a，xn＝b；定义si，(i＝1,2,
…
,n)代表每个小区间的曲线；定义原始路径点为(x0,y0),(x1,y1),
…
,(xn,yn)，任意分段曲线由该线段的起点(xi,yi)和终点(x
i+1
,y
i+1
)定义，则每段样条曲线定义如下：
[0060]
si(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3[0061]
(2)设步长hi＝x
i+1-xi，为保证分段曲线在交点处至少有二阶光滑:s
′i(x
i+1
)＝s
′
i+1
(x
i+1
)，s
″i(x
i+1
)＝s
″
i+1
(x
i+1
)，即
[0062][0063]
2ci+6h
idi
＝2c
i+1
[0064]
(3)设mi＝s
″i(xi)＝2ci，得
[0065][0066]
构造一个关于m的线性方程组，考虑到夹持边界条件：s
′0(x0)＝a，s
′
n-1
＝b，即b0＝a，b
n-1
＝b，则有：
[0067][0068]
因为左侧系数矩阵为严格对角占优矩阵，所以该线性方程组有唯一解由即可求出样条函数的四个系数ai，bi，ci，di。
[0069]
本发明与现有技术相比具有以下有益效果：
[0070]
本发明采用基于梯度的前向蚁群算法作全局路径规划，改善蚁群算法冗余节点的缺陷，缩短了路径规划时间，实现了良好的路径规划与避障功能。
附图说明
[0071]
图1是本发明一实施例中非完整约束车辆行驶轨迹；
[0072]
图2是本发明一实施例中动态规划地图；
[0073]
图3是本发明一实施例中关键节点选择；
[0074]
图4是本发明一实施例中等距离插值；
[0075]
图5是本发明一实施例中固定边界三次样条插值平滑后路径和轨迹曲率。
具体实施方式
[0076]
下面结合附图及实施例对本发明做进一步说明。
[0077]
请参照图1-5，本发明提供一种基于梯度的前向蚁群算法无人车路径规划方法，包括以下步骤：
[0078]
步骤s1：根据无人车的状态参数构建运动学单车模型。通过无人车自身携带的摄像头、激光雷达、gps、惯导、里程计以及加速度和角速度等传感器的数据融合进行建模获取障碍物坐标信息、建立栅格地图和初始状态信息；同时获取目标点信息。
[0079]
步骤s2：从无人车的当前位置向周围八个方向扩展节点；节点的扩展原则为：有障碍物不扩展，已扩展过的节点不扩展，获取可行的扩展节点；
[0080]
步骤s3：结合动态规划与dijkstra算法分别计算可行的扩展节点的总代价值g
i,j
,并生成动态规划地图；
[0081]
扩展节点的总代价值g
i,j
,并生成动态规划地图
[0082]
将终点代价值记为0，代价值g
i,j
的计算方式如下：
[0083][0084]
步骤s4：基于动态规划地图，采用前向蚁群算法节点选择策略，获取最优路径；
[0085]
在本实施例中，前向蚁群算法节点选择策略，具体为：
[0086]
如图1所示，将地图划分为若干个大小相等的栅格，其中黑色栅格表示障碍物，白色栅格代表自由空间。
[0087]
设蚂蚁在图1中行走，节点
①
和
⑧
是障碍物栅格。蚂蚁在起点
⑦
行走时，无论去哪个节点都相当于沿初始航向行走，从第二个节点
⑤
开始，蚂蚁有了实际的初始航向，如图中绿色箭头所示。此时蚂蚁可达的节点有5个，分别是
②
、
③
、
④
、
⑥
、
⑨
，可达节点与初始航向的夹角用θ表示。式(2)列出了所有可达节点与初始航向的夹角θ
ij
，其中i、j为节点序号，且i＝5。本文将无人车转向角定义为h
ij
，由于θ
ij
越大，转弯代价越大，为降低转弯代价，蚂蚁会以更大的概率选择较小的θ
ij
。
[0088][0089]hij
＝180
°‑
θ
ij
ꢀꢀ
(3)
[0090]
将蚁群算法的全局最优性和“向前走”节点选择策略具备的较低转弯代价的能力结合起来。构造了顾及路径距离和行驶效率的转移概率函数，将式(3)融入到蚁群算法的转移概率公式中，得到改进的转移概率公式(4)：
[0091][0092]
式中：μ为转向因子，表示蚂蚁“向前走”的重要程度。此转移概率函数通过调整信息素浓度、启发值、和“向前走”的权重，使得规划路径在保证路径距离较优的前提下，打破“路径对称性”，减少路径转折点数量。
[0093]
步骤s5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤s2；
[0094]
步骤s6：对最优路径进行二次优化，去除冗余路径节点，得到关键节点；
[0095]
在本实施例中，针对路径中可能还存在的冗余节点和冗余路段，提出一种关键节点策略过滤“向前走”节点选择策略蚁群算法所规划出路径上的冗余节点，保留路径必要的关键节点，使优化后的路径节点更少，路径更短。简化节点的具体步骤如下：
[0096]
(1)获取“向前走”节点选择策略蚁群算法所规划的全部节点集p＝{pi,1《i≤n},其中p1表示规划路径的起点，pn表示规划路径的终点。创建初始值只包含起点p1和pn的关键
节点集v＝{p1,pn}用来存放算法关键节点提取算法优化后的关键转折点。
[0097]
(2)从p1开始依次连接p3,p4,
…
,pk,若直到直线p1pk经过障碍物，则将p
k-1
为关键节点，并添加到集合v中。并从p
k-1
继续重复以上方式选取关键节点，直到连接到终点pn。
[0098]
(3)关键节点筛选完后，集合v＝{p1,p
k-1
,
…
,pn}包含所有关键节点，连接集合v中所有的节点。得到优化后的路径。
[0099]
步骤s7：对相邻的关键节点进行等距离插值；
[0100]
步骤s8：构建目标函数，并采用梯度下降法进行求解；
[0101]
在本实施例中，针对初始路径在转折点处曲率发生突变，不利于无人车平顺性以及没有考虑环境中无人车与障碍物之间的距离，导致所得路径可能会紧贴障碍栅格，路径安全性较低这两个问题。将路径优化问题看作非线性优化问题，把等距插值后的各路径点的曲率和至最近障碍物的距离作为目标函数的约束条件，同时，约束优化后的路径点与原始路径点的距离，保证优化后的路径与初始路径的位置相差不能过大。
[0102]
设等距插值后得到的路径点坐标序列为xi＝(xi,yi)，i∈[1,n]，其中n为路径点的数量；优化后的路径点坐标序列列为yi＝(x'i,y'i)，i∈[1,n]。考虑路径的平滑性fs，最优性fd，安全性f
ε
和运动学可行性fk，分别定义了四个惩罚函数和对应的惩罚权重，将四项相加得到如下的目标函数：
[0103]
ω＝ωs·fs
+ωd·
fd+ω0·fo
+ωk·fk
ꢀꢀ
(5)
[0104]
式中：
[0105][0106][0107][0108][0109][0110]
δx
′i＝|x
i+1-x
i-1
|
[0111]
其中，r
min
为无人车最小转弯半径；oi为距离yi最近的障碍物坐标；d
max
为预设的安全距离；ωs、ωd、ωo、ωk为各项权重；由于路径的起点和终点位置不能改变，故将起点和终点设置为硬约束，不参与优化。
[0112]
在本实施例中，针对上述非线性优化问题，因为目标函数中的四个惩罚项均为可微函数，容易计算出各项的梯度以及前端改进的蚁群算法已搜索出最优路径，所以采用梯度下降法，将最优路径等距采样后的路径点作为梯度下降算法的初始解，从而可以求解出全局最优路径点。
[0113]
其中平滑性fs、最优性fd和安全性f
ε
三项的梯度如下：
[0114][0115][0116][0117]
在运动学可行性指标fk中，由曲率定义得：
[0118][0119]
其中是δx'i和δx'
i+1
之间的夹角，由两者表示：
[0120][0121]
计算相关的三个点的偏导：
[0122][0123][0124][0125]
其中
[0126][0127]
步骤s9：对路径三次样条插值，保证光滑的路径具有二次连续性，获取最终的最优路径。
[0128]
在本实施例中，通过步骤s9，能够得到一条相比于前向蚁群算法更加安全平滑的路径，但在每个离散路径点之间仍然是分段线性的。这将引起车辆在行驶时出现突然的转向，为了提高光滑程度，势必需要成倍提高离散点的分辨率，增大了梯度优化算法的复杂度，从而降低了梯度下降法的收敛效率。为此，使用固定边界三次样条来插值梯度优化后的路径点，以保证光滑后的路径至少有二阶连续性。实现如下：
[0129]
(1)假设将路径区间[a，b]分成n个区间(x0，x1)，(x1，x2)，...(x
n-1
，xn)，其中路径端点x0＝a，xn＝b；定义si，(i＝1，2，...，n)代表每个小区间的曲线；定义原始路径点为(x0，y0)，(x1，y1)，...，(xn，yn)，任意分段曲线由该线段的起点(xi，yi)和终点(x
i+1
，y
i+1
)定义，则每段样条曲线可定义如下：
[0130]
si(x)＝ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3(13)
[0131]
(2)设步长hi＝x
i+1-xi，为保证分段曲线在交点处至少有二阶光滑:s
′i(x
i+1
)＝s
′
i+1
(x
i+1
)，s
″i(x
i+1
)＝s
″
i+1
(x
i+1
)，即
[0132][0133]
2ci+6h
idi
＝2c
i+1
ꢀꢀ
(15)
[0134]
(3)设mi＝s
″i(xi)＝2ci，代入(20)式，得
[0135][0136]
由(15)式可构造一个关于m的线性方程组，考虑到夹持边界条件：s'0(x0)＝a,s'
n-1
＝b,即b0＝a,b
n-1
＝b,则有：
[0137][0138]
因为左侧系数矩阵为严格对角占优矩阵，所以该线性方程组有唯一解由即可求出样条函数的四个系数ai,bi,ci,di。
[0139]
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

技术特征：
1.一种基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，包括以下步骤：步骤s1：初始化无人车状态参数，通过传感器扫描环境生成灰度地图，生成栅格地图，并在栅格地图上设置起始点和目标点信息；步骤s2：基于无人车的当前位置，以八个领域扩展的方式向周为扩展节点，获取可行的扩展节点；步骤s3：结合动态规划与dijkstra算法分别计算可行的扩展节点的总代价值g
i,j
,并生成动态规划地图；步骤s4：基于动态规划地图，采用前向蚁群算法节点选择策略，获取最优路径；步骤s5：判断新扩展的节点是否为目标点，若是，则进行下一个步骤；若不是，则返回步骤s2；步骤s6：对最优路径进行二次优化，去除冗余路径节点，得到关键节点；步骤s7：对相邻的关键节点进行等距离插值；步骤s8：构建目标函数，并采用梯度下降法进行求解；步骤s9：对路径三次样条插值，保证光滑的路径具有二次连续性，获取最终的最优路径。2.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，所述步骤s1，具体为：根据无人车的状态参数构建运动学单车模型；通过无人车自身携带的摄像头、激光雷达、gps、惯导、里程计、加速度和角速度传感器的数据融合进行建模获取障碍物坐标信息、建立栅格地图和初始状态信息；同时获取目标点信息。3.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，所述步骤s3，具体为：将终点代价值记为0，代价值g
i,j
的计算方式如下：其中，g
i,j
表示位于第i行第j列栅格的代价值，代价值采用欧几里德距离计算，即第i行第j列栅格的代价值与其上下左右栅格的代价值相差为1，与其左上，右上，左下，右下的栅格的代价值相差为4.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，所述前向蚁群算法节点选择策略，具体为：将地图划分为若干个大小相等的栅格，可达节点与初始航向的夹角用θ表示；获取所有可达节点与初始航向的夹角θ
ij
，其中i、j为节点序号；将无人车转向角定义为h
ij
，由于θ
ij
越大，转弯代价越大，为降低转弯代价，蚂蚁会以更大的概率选择较小的θ
ij
将蚁群算法的全局最优性和向前走节点选择策略具备的较低转弯代价的能力结合起来，构造顾及路径距离和行驶效率的转移概率函数：
式中：为第k只蚂蚁从i节点到节点j的转移概率；τ
ij
为由节点i到节点j的信息素强度；η
ij
为启发函数，是节点i到节点j之间距离的倒数；allow
k
为访问的节点集合；α和β分别为信息素启发因子和启发函数权重值；μ为转向因子，表示蚂蚁向前走的重要程度。5.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，所述步骤s6具体为：(1)获取向前走节点选择策略蚁群算法所规划的全部节点集p＝{p
i
,1<i≤n},其中p1表示规划路径的起点，p
n
表示规划路径的终点；创建初始值只包含起点p1和p
n
的关键节点集v＝{p1,p
n
}用来存放算法关键节点提取算法优化后的关键转折点；(2)从p1开始依次连接p3,p4,
…
,p
k
,若直到直线p1p
k
经过障碍物，则将p
k-1
为关键节点，并添加到集合v中。并从p
k-1
继续重复以上方式选取关键节点，直到连接到终点p
n
；(3)关键节点筛选完后，集合v＝{p1,p
k-1
,
…
,p
n
}包含所有关键节点，连接集合v中所有的节点，得到优化后的路径。6.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，步骤s8中考虑路径的最优性，平滑性，安全性，运动学可行性构建目标函数，具体为：将路径优化问题看作非线性优化问题，把等距插值后的各路径点的曲率和至最近障碍物的距离作为目标函数的约束条件，同时，约束优化后的路径点与原始路径点的距离设等距插值后得到的路径点坐标序列为x
i
＝(x
i
,y
i
)，i∈[1,n]，其中n为路径点的数量；优化后的路径点坐标序列列为y
i
＝(x'
i
,y'
i
)，i∈[1,n]；考虑路径的平滑性f
s
，最优性f
d
，安全性f
ε
和运动学可行性f
k
，分别定义了四个惩罚函数和对应的惩罚权重，将四项相加得到如下的目标函数：ω＝ω
s
·
f
s
+ω
d
·
f
d
+ω
o
·
f
o
+ω
k
·
f
k
其中，ω
s
、ω
d
、ω
o
、ω
k
为四个惩罚函数对应的惩罚权重；式中：式中：式中：式中：式中：
δx
′
i
＝|x
′
i+1-x
′
i-1
|其中，y
i
为第i个路径点的纵坐标；r
min
为无人车最小转弯半径；o
i
为距离y
i
最近的障碍物坐标；d
max
为预设的安全距离；ω
s
、ω
d
、ω
o
、ω
k
为各项权重。7.根据权利要求6所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，步骤s8中采用梯度下降法，将最优路径等距采样后的路径点作为梯度下降算法的初始解，求解出全局最优路径点，具体为：其中平滑性f
s
、最优性f
d
和安全性f
ε
三项的梯度如下：三项的梯度如下：三项的梯度如下：在运动学可行性指标f
k
中，由曲率定义得：其中是δx'
i
和δx'
i+1
之间的夹角，由两者表示：计算相关的三个点的偏导：计算相关的三个点的偏导：计算相关的三个点的偏导：其中8.根据权利要求1所述的基于梯度的前向蚁群算法无人车路径规划方法，其特征在于，所述步骤s9具体为：(1)设将路径区间[a,b]分成n个区间(x0,x1),(x1,x2),
…
(x
n-1
,x
n
)，其中路径端点x0＝a，x
n
＝b；定义s
i
，(i＝1,2,
…
,n)代表每个小区间的曲线；定义原始路径点为(x0,y0),(x1,
y1),
…
,(x
n
,y
n
)，任意分段曲线由该线段的起点(x
i
,y
i
)和终点(x
i+1
,y
i+1
)定义，则每段样条曲线定义如下：s
i
(x)＝a
i
+b
i
(x-x
i
)+c
i
(x-x
i
)2+d
i
(x-x
i
)3(2)设步长h
i
＝x
i+1-x
i
，为保证分段曲线在交点处至少有二阶光滑:s
′
i
(x
i+1
)＝s
′
i+1
(x
i+1
),s
″
i
(x
i+1
)＝s
″
i+1
(x
i+1
)，即2c
i
+6h
i
d
i
＝2c
i+1
(3)设m
i
＝s
″
i
(x
i
)＝2c
i
，得由(15)式构造一个关于m的线性方程组，考虑到夹持边界条件：s'0(x0)＝a,s'
n-1
＝b,即b0＝a,b
n-1
＝b,则有：因为左侧系数矩阵为严格对角占优矩阵，所以该线性方程组有唯一解由即可求出样条函数的四个系数a
i
,b
i
,c
i
,d
i
。

技术总结
本发明涉及一种基于梯度的前向蚁群算法无人车路径规划方法，包括以下步骤：步骤S1：初始化无人车状态参数，生成栅格地图，并设置起始点和目标点信息；步骤S2：以八个领域扩展的方式向周为扩展节点，获取可行的扩展节点；步骤S3：结合动态规划与Dijkstra算法分别计算可行的扩展节点的总代价值,并生成动态规划地图；步骤S4：采用前向蚁群算法节点选择策略，获取最优路径；步骤S5：判断新扩展的节点是否为目标点，若不是，则返回步骤S2；步骤S6：对最优路径进行二次优化，得到关键节点；步骤S7：对相邻的关键节点进行等距离插值；步骤S8：构建目标函数，并采用梯度下降法进行求解；步骤S9：对路径三次样条插值，获取最终的最优路径。获取最终的最优路径。获取最终的最优路径。


技术研发人员：张卫波 封士宇 温珍林 黄晓军 罗星 黄赐坤
受保护的技术使用者：福州大学
技术研发日：2023.03.09
技术公布日：2023/8/21