CN116804879B - 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 - Google Patents
一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 Download PDFInfo
- Publication number
- CN116804879B CN116804879B CN202310912043.6A CN202310912043A CN116804879B CN 116804879 B CN116804879 B CN 116804879B CN 202310912043 A CN202310912043 A CN 202310912043A CN 116804879 B CN116804879 B CN 116804879B
- Authority
- CN
- China
- Prior art keywords
- path
- algorithm
- robot
- dung
- dung beetle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 210000003608 fece Anatomy 0.000 title claims abstract description 114
- 241000254173 Coleoptera Species 0.000 title claims abstract description 112
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000011156 evaluation Methods 0.000 claims abstract description 47
- 230000006872 improvement Effects 0.000 claims abstract description 5
- 238000005070 sampling Methods 0.000 claims description 40
- 230000006399 behavior Effects 0.000 claims description 30
- 230000008569 process Effects 0.000 claims description 15
- 238000005096 rolling process Methods 0.000 claims description 12
- 238000010586 diagram Methods 0.000 claims description 10
- 238000010606 normalization Methods 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 230000002431 foraging effect Effects 0.000 claims description 8
- 239000000446 fuel Substances 0.000 claims description 8
- 238000009499 grossing Methods 0.000 claims description 8
- 230000007613 environmental effect Effects 0.000 claims description 7
- 241001229848 Aphodius granarius Species 0.000 claims description 6
- 238000009826 distribution Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 230000019637 foraging behavior Effects 0.000 claims description 4
- 230000000007 visual effect Effects 0.000 claims description 4
- 230000006978 adaptation Effects 0.000 claims description 3
- 238000011160 research Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 claims description 2
- 238000005457 optimization Methods 0.000 abstract description 15
- 230000001133 acceleration Effects 0.000 description 9
- 239000011664 nicotinic acid Substances 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 4
- 238000013138 pruning Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 102100029469 WD repeat and HMG-box DNA-binding protein 1 Human genes 0.000 description 2
- 101710097421 WD repeat and HMG-box DNA-binding protein 1 Proteins 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000001488 breeding effect Effects 0.000 description 2
- 238000004904 shortening Methods 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000004791 biological behavior Effects 0.000 description 1
- 238000009395 breeding Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 235000001968 nicotinic acid Nutrition 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000009966 trimming Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种路径规划的新型群智能算法—蜣螂算法，并结合DWA算法形成一种全局路径规划和局部路径规划的框架。在新算法的基础上做出了4点改进，本文改进了蜣螂优化算法的目标函数，使得改进后的蜣螂算法在路径搜索上大大减少了转折点；其次，本文还考虑了转折处距离障碍物过近的约束方案，使得路径不斜线经过障碍物顶点；再其次使用贪婪算法思想通过删除冗余节点对生成的路径进行简化处理；在DWA算法评价函数中进行了改进，增加了一种目标距离评价函数，使得局部路径质量更高。最后，改进蜣螂算法结合DWA算法，能够在实时环境下快速生成安全、可行的路径规划结果。
Description
技术领域
本发明涉及智能驾驶领域，尤其是涉及到一种改进蜣螂算法融合DWA算法的机器人路径规划框架方法。
背景技术
路径规划是智能驾驶中必不可少的关键技术之一，它通过算法寻找带有障碍物的交通***中从起点到终点的最佳路径。这个过程中需要考虑各种因素，如道路状况、转弯角度、路径长度、障碍物等，以确保智能车辆能够在道路上安全、高效、舒适地行驶。
目前，路径规划算法在智能驾驶、机器人、物流等领域得到了广泛应用，成为这些领域的核心技术之一。不同学者对路径规划算法进行了大量的研究，针对不同应用场景，发展出了各种不同的算法。这些算法在效率、准确性、实时性、鲁棒性等方面都有不同的特点和优缺点，具体选择哪种算法需要根据具体应用场景和需求来决定。
除了算法本身的优缺点外，路径规划还需要考虑实际应用场景中的各种限制条件，比如车辆的最大速度、转弯半径、避障能力、道路限速等等。因此，在选择路径规划算法时，需要充分考虑这些限制条件，并根据具体情况进行相应的调整和优化。总之，路径规划算法是智能驾驶、机器人、物流等领域中不可或缺的核心技术之一，其应用前景广阔，但也面临着各种挑战和问题。只有不断地进行研究和创新，并结合实际应用需求进行相应的调整和优化，才能够推动路径规划算法的发展和应用。
智能仿生算法是一类以仿生学为基础的智能算法，通过模拟自然界的生物行为，来解决优化问题。近年来，由于其在解决各种实际问题中表现出色，越来越多的学者开始对其进行研究。与传统的数学优化算法相比，智能仿生算法具有很多优点。首先，智能仿生算法可以处理大量的不确定信息，并具有很好的鲁棒性和适应性。其次，智能仿生算法可以处理复杂的非线性问题，如多模态、多目标问题等。最后，智能仿生算法具有强大的搜索能力，可以在高维空间中找到全局最优解。智能仿生算法有：人工鱼群算法、蚁群算法、粒子群算法、蚁群算法。智能优化算法是一种在路径规划中广泛应用的方法，可以用于解决多种复杂问题。这些算法各有特点，需要根据具体应用场景和需求选择最适合的算法。
蜣螂优化(Dung Beetle Optimizer,DBO)算法是2022年11月27日由东华大学的沈教授团队提出的一种全新的群智能优化算法。新出炉的DBO算法，其灵感来自于蜣螂的滚球、跳舞、觅食、偷窃和繁殖行为。总的来说该算法兼顾了全局探索和局部开发，具有收敛速度快、求解精度高的特点。
在路径规划中，使用蜣螂算法在简单场景中能获得比较好的路线，在稍微复杂环境下，路径转折点就会增多，并且转折角度过大，路径质量较差。
蜣螂算法可以用于全局路径规划，即在整个地图环境中搜索全局最优路径。DWA算法(dynamic window approach)，其原理主要是在速度空间(v,w)中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的(v,w)驱动机器人运动。DWA算法则主要用于局部路径规划，即在机器人当前位置周围的动态窗口内搜索最优速度和转角，以避开障碍物和优化路径。
将蜣螂算法和DWA算法结合起来，可以发挥它们各自的优势，实现更加高效和准确的路径规划。具体来说，蜣螂算法可以在全局范围内搜索最优路径，得到一个粗略的路径轨迹，在经过修整得到全局路径。然后，DWA算法可以在机器人当前位置周围的动态窗口内搜索最优速度和转角，通过局部路径规划来进一步优化路径规划结果，并且可以及时响应环境变化，避开障碍物和优化路径规划。
因此，蜣螂算法和DWA算法的结合可以实现全局路径规划和局部路径规划的有机结合，从而在路径规划中取得更好的效果。通过在全局和局部两个层次上进行路径规划，可以使机器人在不同场景下表现出更加智能化和高效化的行为，从而实现更加准确和稳定的路径规划效果。
发明内容
本发明目的就在于为了解决上述的问题，而提供一种改进蜣螂算法融合DWA算法的机器人路径规划框架方法。
为实现上述目的，本发明采取的技术方案为：一种基于MS-DSC和I_CBAM的红外和可见光图像融合方法，包含以下步骤：
步骤1：以移动机器人为研究对象，构建栅格地图，设置障碍物的位置，并标注起点目标点位置。
步骤2：使用改进蜣螂算法进行全局路径规划，生成初始路径。
步骤3：借助贪婪算法思想，将上述生成的初始路径进行路径平滑处理，剔除初始路径中的冗余节点并且提取到路径中的关键节点，大大减少了路径长度以及减少了转折点的个数，使得整体路径更加平滑。
步骤4：将关键点顺次作为DWA算法的目标点，进行速度采样、轨迹预测。
步骤5：通过4个评价函数进行评优，选取评价最优的速度直至找到目标点。
进一步地，所述步骤1具体包括：
使用MATLAB构建栅格图，栅格图中黑色方块代表障碍物，白色方块代表可行驶区域，地图上设置起点、和终点坐标位置。
进一步地，所述步骤2具体包括：
所述蜣螂算法，主要由蜣螂滚球行为、蜣螂繁殖行为、蜣螂觅食行为、蜣螂偷窃行为。通过蜣螂4种行为来进行搜索。
所述蜣螂滚球行为主要包括：无障碍模式和有障碍模式。
所述无障碍模式，通过设置参数r∈(0,1)<0.9,来实现使蜣螂以大概率进行无障碍滚球行为，其位置更新公式为：
其中，t表示当前迭代次数，表示种群中第i只蜣螂在第t次迭代时的位置，表示种群中第i只蜣螂在第t次迭代时的位置，k∈(0,0.2]表示一个常值，代表偏转系数，b∈(0,1)之间的一个常值，a是赋值为-1或1的自然系数，1表示无偏差，-1表示偏离原方向。表示当前种群中的最差位置，用于模拟光强的变化。
所述有障碍模式是指当蜣螂遇到障碍物无法通行时，需要通过跳舞行为来获得新的前进方向，使用切线函数来模仿跳舞行为，其位置更新公式为：
当角度时，不做更新，否则按照上述公式更新位置。
所述蜣螂繁殖行为，首先设定一个边界区域限制蜣螂产卵区域：
Lb*＝max(X*×(1-R),Lb),
Ub*＝min(X*×(1+R),Ub)
R＝e-hgen
其中h是常数，这里对参数R做了自适应调整，gen是当前迭代次数。
所述产卵行为中产卵区域动态更新，所以雏球位置也在以下公式更新：
其中，为第i个雏球在第t次迭代时的位置，b1和b2是两个独立随机向量。
所述蜣螂觅食行为，是针对小蜣螂而言。同样有一个觅食区域也是动态更新：
其中，为当前种群的局部最优位置。
所述小蜣螂位置更新如下：
其中，C1为服从正态分布的随机数，C2为属于(0,1)之间的随机向量。
所述蜣螂偷窃行为，会有一些蜣螂在从其他蜣螂那里偷粪球，所以盗贼蜣螂的位置更新公式为：
其中，g为随机向量，服从正态分布；s表示一个常数值。
在每只蜣螂都完成自己的行为之后，更新适应度fitness。
所述考虑多目标函数，包括：燃油经济性、安全性、舒适性。
这里，在适应度这里做了改进，公式如下：
fitness＝a*f1+b*f2+c*f3
其中On为经过障碍物数量，体现了安全性。
其中(xi，yi)，(i＝1，2，…，n)是路径点pi的坐标，这一项考虑了路径长度，体现了燃油经济性。
f3＝numTurns其中numTurns为转转折点数量，这一项惩罚了转弯，考虑到了转弯角度，体现了舒适性。
通过增加多个目标函数，保证了生成路径的质量。
进一步地，增加约束，使得路径不斜线穿越障碍物顶点，具体步骤如下：
步骤1：遍历生成路径path中每个点，将障碍物坐标存入obs中。
步骤2：通过比较pathi和pathi+1和每个障碍物的x、y坐标来检测是否穿过障碍物顶点。
步骤3：通过以下判据来判断是否有这样的点存在
l1+l2＝2
其中l1＝|pathi(x)-obs(x)|+|pathi+1(x)-obs(x)|
l2＝|pathi(y)-obs(y)|+|pathi+1(y)-obs(y)|
obs＝(obs(x),obs(y))
步骤4：当存在这样的两个点时，需要求解新的点pathnd，公式如下：
pathnd＝2*z-obs
经过上述三个对蜣螂算法的改进，路径长度以及转折点数量大大减少。
进一步地，所述步骤3具体包括：
进一步优化路径，虽然蜣螂算法能够有效地缩短路径和剔除经过障碍物顶点的节点，但是在避障过程中，生成的路径可能包含一些冗余节点，导致路径较为曲折。为了得到更加平和路径更优的路径，可以使用路径平滑算法去除冗余点，这里主要借助了贪婪算法思想。这个算法的主要思路是连接相邻的节点，然后依次检查每一个节点，如果这个节点与后面的节点之间没有其它路径节点，可以删除这个节点。这样就可以得到更平滑、更直观的路径，降低机器人的转弯次数，提高运动效率。
步骤1：将蜣螂算法得到的路径上相邻的节点用一条直线连接起来，得到初始路径。
步骤2：从初始路径的起点开始，依次检查每一个节点，如果这个节点与路径上后面的节点能够被一条直线连接，并且这条直线上没有其它路径节点，则删除这个节点，同时更新其前后节点的连接关系。
步骤3：得到新的路径后，重复步骤2，直到没有节点被删除为止。
步骤4：返回最终路径。
进一步地，所述步骤4具体包括：
DWA算法是一种用于机器人路径规划的算法，其核心思想是基于当前机器人状态和环境信息，在一定时间内采集一系列可能的运动状态，然后选择最优的运动状态来实现路径规划。为了预测机器人的运动轨迹，DWA算法需要选取采样角度和角速度，然后根据这些采样点进行运动状态的预测。具体步骤如下：
步骤1：选取采样角度和角速度：DWA算法可以根据机器人的运动能力和环境要求来选择一组合适的采样角度和角速度，这些采样点将用于计算机器人可能的运动状态。
步骤2：计算运动状态：对于每个采样点，DWA算法需要计算机器人在这个采样点的运动状态，包括机器人的位置、方向和速度等信息。这些运动状态将用于预测机器人的运动轨迹。
步骤3：预测轨迹：基于机器人的当前状态和选定的采样点，DWA算法可以预测机器人的运动轨迹。这个预测轨迹可以帮助机器人做出最优的运动决策，以达到目标点并避开障碍物。
进一步地，所述步骤5具体包括：
在选择最优运动状态时，DWA算法考虑了机器人的运动约束和环境障碍物的影响，以确保机器人能够安全到达目标点。此外还考虑了距离目标评价函数，用来保证了路径质量。对每条轨迹进行评估的评价函数如下：
G(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*tw(v,w))
heading(v,ω)：方位角评价函数，用作评估在当前采样速度下产生的轨迹终点位方向与目标点连线的夹角的误差Δθ；由于要用评价函数越大表示越优，所以用π-Δθ来参与评价，即heading(v,ω)＝π-Δθ。
dist(v,ω)：距离障碍物评价函数，表示当前速度下对应模拟轨迹与障碍物之间的最近距离；如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。
velocity(v,ω)：速度评价函数，表示当前的速度大小，可以直接用当前线速度的大小来表示。它越大，表示规划轨迹上的速度越快，评价得分越高。
tw(v,ω)：距离目标评价函数，评价局部路径末端点到终点的距离函数，目的是不断缩短与终点的距离。
α、β、γ、δ为评价函数的系数。由于局部路径规划的过程需要多传感器的采集,采集信息无法做到连续,这样也会使得评价后差别较大,所以可以进行归一化处理，其中σ表示归一化。
归一化处理过程如下式所示:
其中，i代表第i条模拟轨迹,n为约束条件下的全部采样轨迹总数。由上述公式可以得出一条满足避开障碍物并朝着目标点快速行进的路径，使得机器人完成局部路径最优规划。
本发明的有益点：
1.全局搜索能力：蜣螂算法是一种启发式优化算法，通过模拟蜣螂之间的滚球、跳舞、觅食、偷窃和繁殖行为来进行全局搜索。它可以在整个搜索空间中寻找最优路径。蜣螂算法具有较强的全局搜索能力，具有收敛速度快、求解精度高的特点，可以帮助机器人在复杂的环境中找到全局最优解。
2.局部优化能力：DWA算法是一种基于动态窗口的局部规划方法，通过考虑机器人的动力学约束和环境信息来生成局部可行的速度轨迹。DWA算法能够根据机器人当前状态和周围环境的动态变化，实时调整速度和方向，从而生成适应性强的局部路径。
3.多目标优化：考虑多个目标函数，燃油经济性、安全性、舒适性，使得路径规划算法可以通过优化方法来寻找最优的路径。通过调整各个目标函数之间的权重或设置约束条件来找到合适的平衡解。这样可以生成一系列最优或近最优的路径选择，以供用户根据具体需求进行选择。
4.设置转折约束：增加一种转折约束使其不斜线通过障碍物顶点，通过增加转折约束，可以更大程度地保障安全性，确保路径规划结果避开障碍物顶点，从而减少潜在的碰撞风险。
5.删除冗余节点：利用贪心算法思想，将生成的路径进行修剪，得到更优质路径。通过贪心修剪，可以剔除路径中对整体质量影响较小或不利的路径点，从而提高路径的质量和性能。例如，移除冗余的路径点可以减少路径的长度和燃料消耗，或避免直线通过障碍物顶点，提高安全性。
附图说明
图1为本发明实施例提供的全局和局部路径规划流程图；
图2为本发明实施例提供的改进蜣螂算法的流程图；
图3为本发明实施例提供的增加转折点约束的示意图；
图4为本发明实施例提供的修剪冗余节点、平滑路径的示意图；
图5为本发明实施例提供的DWA算法轨迹预测的示意图；
图6为本发明实施例提供的DWA算法的流程图；
图7为本发明实施例提供的人工鱼群算法(AFSA)路径规划结果；
图8为本发明实施例提供的蜣螂优化算法(DBO)路径规划结果。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
本发明针对蜣螂算法在路径规划中的一些不足，提出了一种改进的蜣螂算法，并且与局部算法—DWA算法融合，形成一种全局规划加局部规划的框架。
为了解决上述问题提出了一种改进的蜣螂算法，主要内容包括：
(1)自适应参数R，可以进一步平衡DBO算法的探索和开发状态；
(2)考虑燃油经济性、安全性、舒适性的多目标函数的算法；
(3)增加一种转折约束使其不斜线通过障碍物顶点；
(4)利用贪心算法思想，将生成的路径进行修剪，得到更优质路径；
将上述改进蜣螂算法融合DWA算法，使得局部规划功能更加完善。
一方面，改进的蜣螂优化算法在路径规划中的应用主要有以下步骤：
设置场景地图
初始化参数，包括种群数量NP，最大迭代次数Maxgen，种群占比参数，以及蜣螂初始位置。
计算每只蜣螂的适应度并排序，找到最差适应度蜣螂的位置。
设置参数r∈(0,1)，目的为了使蜣螂以大概率进行无障碍滚球行为。
对NP只蜣螂开始分组进行滚球、繁殖、觅食、偷窃行为。
所述蜣螂滚球行为分为有障碍模式和无障碍模式，并且设置r<0.9来大概率进行无障碍行为。
所述蜣螂行为中无障碍模式下，位置更新公式为：
其中，t表示当前迭代次数，表示种群中第i只蜣螂在第t次迭代时的位置，表示种群中第i只蜣螂在第t次迭代时的位置，k∈(0,0.2]表示一个常值，代表偏转系数，b∈(0,1)之间的一个常值，a是赋值为-1或1的自然系数，1表示无偏差，-1表示偏离原方向。表示当前种群中的最差位置，用于模拟光强的变化。
对于系数α设计了以下公式确定它的值。
设定一个概率值λ，使用一个(0,1)随机数赋值给η，比较η和λ来确定α。
当蜣螂遇到障碍物无法前进时，需要通过跳舞行为来获得新的前进方向，使用切线函数模仿跳舞行为，蜣螂位置更新公式为：
当角度时，不做更新，否则按照上述公式更新位置。
所述蜣螂繁殖行为，首先设定一个边界区域限制蜣螂产卵区域：
Lb*＝max(X*×(1-R),Lb),
Ub*＝min(X*×(1+R),Ub)
R＝e-hgen，其中h是常数，gen是当前迭代次数。
所述产卵行为中产卵区域动态更新，所以雏球位置也在以下公式更新：
其中，为第i个雏球在第t次迭代时的位置，b1和b2是两个独立随机向量。
所述蜣螂觅食行为，是针对小蜣螂而言。同样有一个觅食区域也是动态更新：
其中，为当前种群的局部最优位置。
所述小蜣螂位置更新如下：
其中，C1为服从正态分布的随机数，C2为属于(0,1)之间的随机向量。
所述蜣螂偷窃行为，会有一些蜣螂在从其他蜣螂那里偷粪球，所以盗贼蜣螂的位置更新公式为：
其中，g为随机向量，服从正态分布；s表示一个常数值。
在每只蜣螂都完成自己的行为之后，更新适应度fitness。
这里，在适应度这里做了改进，公式如下：
fitness＝a*f1+b*f2+c*f3
其中On为经过障碍物数量，体现了安全性。
其中(xi，yi)，(i＝1，2，…，n)是路径点pi的坐标，这一项考虑了路径长度，体现了燃油经济性。
f3＝numTurns其中numTurns为转转折点数量，这一项惩罚了转弯，考虑到了转弯角度，体现了舒适性。
更新个体最优值和全局最优值，然后生成路径，在此引入了缩短节点函数，对生成路径中有两个相同节点，则可以将两个相同节点之间段去掉，将给定的路径path进行缩短，使得该路径不包含多余的节点，即去掉中间可以直接到达的节点，从而使得路径更短。
在路径生成过程中考虑了不斜线穿越障碍物顶点，这样使得转弯时有了更安全的空间。为了获得成本最小的路径，蜣螂算法在连接节点时通常会选择靠近目标节点的节点，这样可以更快地接近目标，并且在搜索过程中逐渐优化路径。然而，靠近障碍物的节点可能会导致生成的路径与障碍物较近，比如：斜线穿越障碍物顶点，从而增加机器人与障碍物发生碰撞的风险。为了确保实体机器人的安全运行，最终规划出的路径需要与障碍物保持一定的距离。这可以通过排查通过障碍物顶点的节点，并剔除再选择新的节点来实现。具体而言，当选择连接节点时，如果路径中某两个节点与障碍物顶点成一条直线，该节点会被剔除，不会被用于生成路径。这样可以筛选出可用的安全节点，使得生成的路径能够避开障碍物，并确保实体机器人在运行时不会与障碍物发生碰撞。
进一步优化路径，虽然蜣螂算法能够有效地缩短路径和剔除经过障碍物顶点的节点，但是在避障过程中，生成的路径可能包含一些冗余节点，导致路径较为曲折。为了得到更加平和路径更优的路径，可以使用路径平滑算法去除冗余点，这里主要借助了贪婪算法思想。这个算法的主要思路是连接相邻的节点，然后依次检查每一个节点，如果这个节点与后面的节点之间没有其它路径节点，可以删除这个节点。这样就可以得到更平滑、更直观的路径，降低机器人的转弯次数，提高运动效率。
通过路径平滑算法去除冗余节点，不仅可以得到更平滑、更直观的路径，而且还可以减少机器人在路径上的转弯次数，提高运动效率，极大的提高了规划路径的质量。
另一方面，DWA算法是一种典型的动作采样算法，其原理主要是在速度空间(v,w)中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的(v,w)驱动机器人运动。这里，我们采用了一种考虑4个评价函数的DWA算法。
DWA(Dynamic Window Approach)算法是一种针对机器人路径规划的局部运动控制算法，具有较好的实时性和可靠性。
需要注意的是，DWA算法需要结合机器人的传感器数据、地图信息以及运动控制***的特性进行调整和优化，才能实现更加有效和快速的路径规划。
DWA算法中最重要的三个步骤：速度采样、轨迹预测(推算)、轨迹评价。
一、速度采样。
由于移动机器人硬件、结构和环境等限制条件，移动机器人的速度采样空间Vs中(v,ω)有一定的范围限制。主要分为三类：
1.速度边界限制
根据移动机器人的硬件条件和环境的限制，移动机器人的速度存在边界限制，此时可采样的速度空间Vm为：
Vm＝{(v,ω)∣v∈[vmin,vmax],ω∈[ωmin,ωmax]}
其中vmin、vmax分别为移动机器人最小线速度和最大线速度，ωmin、ωmax分别为移动机器人最小角速度和最大角速度。
2.加速度限制
由于移动机器人的驱动电机的限制，故移动机器人的线加速度和角加速度均存在边界限制，假设最大加速度和减速度大小一样，故考虑加速度时可采样的速度空间Vd为：
式中vc、ωc分别为移动机器人当前时刻的线速度和角速度，avmax、awmax分别为移动机器人最大线加速度和最大角加速度。
3.环境障碍物限制
局部规划还需要有动态实时的避障功能。考虑移动机器人的周围的障碍物因素，某-时刻移动机器人不与周围障碍物发生碰撞的可约束条件为：
式中dist(v,ω)表示当前速度下对应模拟轨迹与障碍物之间的最近距离。在无障碍物的情况下dist(v,ω)会是一个很大的常数值。当机器人运行采样速度在公式范围时，能够以最大减速度的约束实现安全减速直至避开障碍物。
结合上述三类速度限制，最终的移动机器人速度采样空间是三个速度空间的交集，即：
Vs＝Vm∩Vd∩Va
在上述三条约束条件的限制下，速度空间(v,w)会有一定的范围，另外会随着电机的线加速度、角加速度进行变换，速度空间会动态变化，将其称为动态窗口，在满足约束条件的情况下，进行采样(v,w)，可以得到相应的轨迹。
二、轨迹预测
在确定速度采样空间Vs后，DWA算法以一定的采样间距(分辨率)在该空间均匀采样。在速度空间中，分别对线速度和角速度设置分辨率，分别用Ev、Ew表示采样分辨率，那么采样速度组的个数就可以确定：
n＝[(vhigh-vlow)/Ev]·[(whigh-wlow)/Ew]
式中的vhigh,vlow,whigh,wlow是速度空间的上下限。上式说明线速度每间隔一个Ew，大小取一个值，角速度每间隔一个Ew大小取一个值，由此组成了一系列的速度组。当采样了一组(v,ω)后，通过移动机器人的运动学模型进行轨迹预测。
使用差分驱动移动机器人的运动模型，得到轨迹预测公式：
xk＝xk-1+v·cos(θk-1)Δt
yk＝yk-1+v·sin(θk-1)Δt
θk＝θk-1+ωΔt
式中，(x,y,θ)代表机器人的位姿,k代表采样时刻，△t表示采样间隔。
三、轨迹评价
确定了机器人约束速度范围后，有一些速度模拟的轨迹是可行的，但是还有不达标的轨迹，这需要对采样得到的多组轨迹进行评价择优。通过标准评价轨迹，比较评分来选出最优轨迹，然后选取最优轨迹对应的速度作为驱动速度。对每条轨迹进行评估的评价函数如下：
g(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*tw(v,w))
heading(v,ω)：方位角评价函数，用作评估在当前采样速度下产生的轨迹终点位方向与目标点连线的夹角的误差Δθ；由于要用评价函数越大表示越优，所以用π-Δθ来参与评价，即heading(v,ω)＝π-Δθ。
dist(v,ω)：距离障碍物评价函数，表示当前速度下对应模拟轨迹与障碍物之间的最近距离；如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。
velocity(v,ω)：速度评价函数，表示当前的速度大小，可以直接用当前线速度的大小来表示。它越大，表示规划轨迹上的速度越快，评价得分越高。
tw(v,ω)：距离目标评价函数，评价局部路径末端点到终点的距离函数，目的是不断缩短与终点的距离。
α、β、γ、δ为评价函数的系数。由于局部路径规划的过程需要多传感器的采集,采集信息无法做到连续,这样也会使得评价后差别较大,所以可以进行归一化处理，其中σ表示归一化。
归一化处理过程如下式所示:
其中，i代表第i条模拟轨迹,n为约束条件下的全部采样轨迹总数。由上述公式可以得出一条满足避开障碍物并朝着目标点快速行进的路径，使得机器人完成局部路径最优规划。
通过改进蜣螂算法全局规划和DWA算法局部规划，完成路径规划。
为本发明的技术方案能得到充分的理解，现对总体框架简要介绍如下：
图1是本发明的整体框架流程图，详细步骤如下：
步骤1：初始化地图、起始点、终点、障碍物等参数，并且设置蜣螂算法需要设置的参数。
步骤2：使用改进自适应参数、目标函数以及加入不斜线穿越障碍物顶点约束的蜣螂算法进行全局规划，生成一条初始路径，为后续的改进以及算法融合提供已知的先验信息。
步骤3：借助贪婪算法思想，将上述生成的初始路径进行路径平滑处理，剔除初始路径中的冗余节点并且提取到路径中的关键节点，大大减少了路径长度以及减少了转折点的个数，使得整体路径更加平滑。
步骤4：将关键点顺次作为DWA算法的目标点，进行速度采样。
步骤5：通过4个评价函数对评优，选取评价最优的速度直至找到目标点。
图2是蜣螂算法流程图，详细步骤如下：
步骤1：设定蜣螂算法所需参数、最大迭代次数、种群数量、以及每组行为的蜣螂比例等。
步骤2：初始化所有蜣螂群，并计算其适应度，找到全局最优、全局最差适应度蜣螂的位置。
步骤3：进入主程序开始迭代，每次迭代依次执行蜣螂滚球、蜣螂繁殖、蜣螂觅食、蜣螂偷窃行为。
步骤4：达到最大迭代次数以后，更新个体以及全局最优值
图3是对蜣螂算法改进的一个方面，增加约束使其不斜线穿越障碍物顶点，详细步骤如下：
步骤1：遍历生成路径path中每个点，将障碍物坐标存入obs中。
步骤2：通过比较pathi和pathi+1和每个障碍物的x、y坐标来检测是否穿过障碍物顶点。
步骤3：通过以下判据来判断是否有这样的点存在
l1+l2＝2
其中l1＝|pathi(x)-obs(x)|+|pathi+1(x)-obs(x)|
l2＝|pathi(y)-obs(t)|+|pathi+1(y)-obs(y)|
obs＝(obs(x),obs(y))
步骤4：当存在这样的两个点时，需要求解新的点pathnd，公式如下：
pathnd＝2*z-obs
图4是借助贪婪算法思想进行路径平滑处理的示意图，详细步骤如下：
步骤1：将蜣螂算法得到的路径上相邻的节点用一条直线连接起来，得到初始路径。
步骤2：从初始路径的起点开始，依次检查每一个节点，如果这个节点与路径上后面的节点能够被一条直线连接，并且这条直线上没有其它路径节点，则删除这个节点，同时更新其前后节点的连接关系。
步骤3：得到新的路径后，重复步骤2，直到没有节点被删除为止。
步骤4：返回最终路径。
图5是DWA算法的流程图，详细步骤如下：
步骤1：确定机器人的动态窗口，包括机器人在当前时刻内能够到达的速度范围、角速度范围以及对应的轨迹集合。
步骤2：根据机器人传感器数据和目标点信息计算机器人的目标速度和目标角速度。
步骤3：从动态窗口中选择一个速度和角速度，使得机器人能够在一定时间内到达目标点，同时保证不与障碍物相撞。
步骤4：计算所选速度和角速度对应的轨迹，并评估这些轨迹的安全性和优良性。
步骤5：从评估结果中选择最佳的轨迹作为机器人的运动路径。
步骤6：将所选路径上的速度和角速度输出给机器人的运动控制***，实现机器人的运动控制。
图6是轨迹预测示意图，为了预测机器人的运动轨迹，DWA算法需要选取采样角度和角速度，然后根据这些采样点进行运动状态的预测。
图7和图8是人工鱼群算法(AFSA)路径规划结果和本申请的蜣螂优化算法(DBO)路径规划结果，经过对比可以明显看出本申请提出的方法更加快速便捷，有效的减少运动路径的长度。
综上所述，蜣螂算法作为全局规划方法，与DWA算法作为局部规划方法的结合，能够充分发挥它们的优势，提供全局搜索和局部优化能力，自适应调整以适应复杂环境，并综合全局和局部信息，从而实现高效的路径规划。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。
Claims (3)
1.一种改进蜣螂算法融合DWA算法的机器人路径规划框架方法，其特征在于，包括如下步骤：
步骤1：以移动机器人为研究对象，构建栅格地图，设置障碍物的位置，并标注起点目标点位置；
步骤2：使用改进蜣螂算法进行全局路径规划，生成初始路径；
所述蜣螂算法，由蜣螂滚球行为、蜣螂繁殖行为、蜣螂觅食行为、蜣螂偷窃行为，通过蜣螂4种行为来进行搜索；
所述蜣螂滚球行为包括：无障碍模式和有障碍模式，
所述无障碍模式，通过设置参数r∈(0,1)，r∈(0,1)<0.9,来实现使蜣螂以大概率进行无障碍滚球行为，其位置更新公式为：
其中，t表示当前迭代次数，表示种群中第i只蜣螂在第t次迭代时的位置，k∈(0,0.2]表示一个常值，代表偏转系数，b∈(0,1)之间的一个常值，a是赋值为-1或1的自然系数，1表示无偏差，-1表示偏离原方向，表示当前种群中的最差位置，用于模拟光强的变化，
所述有障碍模式是指当蜣螂遇到障碍物无法通行时，通过跳舞行为来获得新的前进方向，使用切线函数来模仿跳舞行为，其位置更新公式为：
当角度时，不做更新，否则按照公式(2)更新位置；
所述蜣螂繁殖行为，首先设定一个边界区域限制蜣螂产卵区域：
R＝e-hgen (4)
其中h是常数，这里对参数R做了自适应调整，gen是当前迭代次数；
产卵行为中产卵区域动态更新，所以雏球位置按照公式(5)更新：
其中，为第i个雏球在第t次迭代时的位置，b1和b2是两个独立随机向量；
所述蜣螂觅食行为，是针对小蜣螂而言，同样有一个觅食区域是动态更新：
其中，为当前种群的局部最优位置；
所述小蜣螂位置更新如下：
其中，C1为服从正态分布的随机数，C2为属于(0,1)之间的随机向量；
所述蜣螂偷窃行为，会有一些蜣螂在从其他蜣螂那里偷粪球，所以盗贼蜣螂的位置更新公式为：
其中，g为随机向量，服从正态分布；s表示一个常数值；
在每只蜣螂都完成自己的行为之后，更新适应度fitness；
考虑多目标函数，包括：燃油经济性、安全性、舒适性，
这里，在适应度这里做了改进，公式如下：
fitness＝a*f1+b*f2+c*f3 (9)
其中On为经过障碍物数量，体现了安全性；
其中(xi，yi)是路径点pi的坐标，i＝1，2，…，n，i表示各路径点的编号，这一项考虑了路径长度，体现了燃油经济性；
f3＝numTurns其中numTurns为转折点数量，这一项惩罚了转弯，考虑到了转弯角度，体现了舒适性；
通过增加多个目标函数，保证了生成路径的质量，具体步骤如下：
步骤2.1：遍历生成路径path中每个点，将障碍物坐标存入obs中；
步骤2.2：通过比较pathi和pathi+1和每个障碍物的x、y坐标来检测是否穿过障碍物顶点；
步骤2.3：通过以下判据来判断是否有这样的点存在
l1+l2＝2 (10)
其中，
l1＝|pathi(x)-obs(x)|+|pathi+1(x)-obs(x)| (11)
l2＝|pathi(y)-obs(y)|+|pathi+1(y)-obs(y)| (12)
obs＝(obs(x),obs(y)) (14)
步骤2.4：当存在这样的两个点时，需要求解新的点pathnd，公式如下：
pathnd＝2*z-obs (15)
经过公式(9)对蜣螂算法的改进，路径长度以及转折点数量大大减少；
步骤3：借助贪婪算法思想，将上述生成的初始路径进行路径平滑处理，剔除初始路径中的冗余节点并且提取到路径中的关键节点，大大减少了路径长度以及减少了转折点的个数，使得整体路径更加平滑；
步骤4：将关键点顺次作为DWA算法的目标点，进行速度采样、轨迹预测，具体包括：
DWA算法是一种用于机器人路径规划的算法，其核心思想是基于当前机器人状态和环境信息，在一定时间内采集一系列的运动状态，然后选择最优的运动状态来实现路径规划，为了预测机器人的运动轨迹，DWA算法需要选取采样角度和角速度，然后根据这些采样点进行运动状态的预测，具体步骤如下：
步骤4.1：选取采样角度和角速度：DWA算法根据机器人的运动能力和环境要求来选择一组合适的采样角度和角速度，这些采样点将用于计算机器人的运动状态；
步骤4.2：计算运动状态：对于每个采样点，DWA算法需要计算机器人在这个采样点的运动状态，包括机器人的位置、方向和速度信息，这些运动状态将用于预测机器人的运动轨迹；
步骤4.3：预测轨迹：基于机器人的当前状态和选定的采样点，DWA算法预测机器人的运动轨迹，这个预测轨迹帮助机器人做出最优的运动决策，以达到目标点并避开障碍物
步骤5：通过4个评价函数进行评优，选取评价最优的速度直至找到目标点，具体包括：
在选择最优运动状态时，DWA算法考虑了机器人的运动约束和环境障碍物的影响，以确保机器人能够安全到达目标点，此外还考虑了距离目标评价函数，用来保证了路径质量，对每条轨迹进行评估的评价函数如下：
G(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*tw(v,w)) (16)
heading(v,ω)：方位角评价函数，用作评估在当前采样速度下产生的轨迹终点位方向与目标点连线的夹角的误差Δθ；由于要用评价函数越大表示越优，所以用π-Δθ来参与评价，即heading(v,ω)＝π-Δθ；
dist(v,ω)：距离障碍物评价函数，表示当前速度下对应模拟轨迹与障碍物之间的最近距离；如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个常数值；
velocity(v,ω)：速度评价函数，表示当前的速度大小，直接用当前线速度的大小来表示，它越大，表示规划轨迹上的速度越快，评价得分越高；
tw(v,ω)：距离目标评价函数，评价局部路径末端点到终点的距离函数，目的是不断缩短与终点的距离；
α、β、γ、δ为评价函数的系数，由于局部路径规划的过程需要多传感器的采集,采集信息无法做到连续,这样会使得评价后差别较大,对其进行归一化处理，其中σ表示归一化；
归一化处理过程如下式所示：
其中，i代表第i条模拟轨迹,n为约束条件下的全部采样轨迹总数，基于评价函数(16)得出一条满足避开障碍物并朝着目标点快速行进的路径，使得机器人完成局部路径最优规划。
2.根据权利要求1所述改进蜣螂算法融合DWA算法的机器人路径规划框架方法，其特征在于，所述步骤1具体包括：
使用MATLAB构建栅格图，栅格图中黑色方块代表障碍物，白色方块代表可行驶区域，地图上设置起点和终点坐标位置。
3.根据权利要求1所述改进蜣螂算法融合DWA算法的机器人路径规划框架方法，其特征在于，所述步骤3具体包括：
进一步优化路径，虽然蜣螂算法能够有效地缩短路径和剔除经过障碍物顶点的节点，但是在避障过程中，生成的路径包含一些冗余节点，导致路径较为曲折，为了得到更加平和路径更优的路径，使用路径平滑算法去除冗余点，这里借助了贪婪算法思想，这个算法的思路是连接相邻的节点，然后依次检查每一个节点，如果这个节点与后面的节点之间没有其它路径节点，删除这个节点，得到更平滑、更直观的路径，降低机器人的转弯次数，提高运动效率；
步骤3.1：将蜣螂算法得到的路径上相邻的节点用一条直线连接起来，得到初始路径；
步骤3.2：从初始路径的起点开始，依次检查每一个节点，如果这个节点与路径上后面的节点能够被一条直线连接，并且这条直线上没有其它路径节点，则删除这个节点，同时更新其前后节点的连接关系；
步骤3.3：得到新的路径后，重复步骤2，直到没有节点被删除为止；
步骤3.4：返回最终路径。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310912043.6A CN116804879B (zh) | 2023-07-25 | 2023-07-25 | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310912043.6A CN116804879B (zh) | 2023-07-25 | 2023-07-25 | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116804879A CN116804879A (zh) | 2023-09-26 |
| CN116804879B true CN116804879B (zh) | 2024-07-05 |
Family
ID=88079565
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310912043.6A Active CN116804879B (zh) | 2023-07-25 | 2023-07-25 | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116804879B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117804476B (zh) * | 2023-12-07 | 2024-06-18 | 无锡物联网创新中心有限公司 | 基于改进dwa和prm算法的移动机器人动态路径规划方法 |
| CN117872307B (zh) * | 2024-01-12 | 2024-06-11 | 兰州理工大学 | 基于蜣螂优化算法的多扩展目标跟踪优化方法 |
| CN117591849B (zh) * | 2024-01-18 | 2024-04-05 | 南京先维信息技术有限公司 | 一种高维网络流量数据的特征选择方法及*** |
| CN117784852B (zh) * | 2024-02-28 | 2024-05-14 | 山东工商学院 | 一种基于鱼鳞仿生优化算法的多模态传感器温度控制方法 |
| CN118181305B (zh) * | 2024-05-16 | 2024-08-23 | 华南农业大学 | 一种茶叶采摘机械臂的运动路径规划方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112631294A (zh) * | 2020-12-16 | 2021-04-09 | 上海应用技术大学 | 一种移动机器人智能路径规划方法 |
| CN115079705A (zh) * | 2022-08-08 | 2022-09-20 | 海相(江苏)科技有限公司 | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115729238A (zh) * | 2022-11-15 | 2023-03-03 | 湘潭大学 | 一种用于移动机器人自主避障的动态路径规划方法 |
| CN116430873A (zh) * | 2023-05-06 | 2023-07-14 | 李思远 | 一种机器人的路径规划方法 |
-
2023
- 2023-07-25 CN CN202310912043.6A patent/CN116804879B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112631294A (zh) * | 2020-12-16 | 2021-04-09 | 上海应用技术大学 | 一种移动机器人智能路径规划方法 |
| CN115079705A (zh) * | 2022-08-08 | 2022-09-20 | 海相(江苏)科技有限公司 | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116804879A (zh) | 2023-09-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116804879B (zh) | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 | |
| Francis et al. | Long-range indoor navigation with PRM-RL | |
| CN111780777B (zh) | 一种基于改进a*算法和深度强化学习的无人车路径规划方法 | |
| Sun et al. | Probabilistic prediction of interactive driving behavior via hierarchical inverse reinforcement learning | |
| CN111142522B (zh) | 一种分层强化学习的智能体控制方法 | |
| CN112356830B (zh) | 一种基于模型强化学习的智能泊车方法 | |
| CN107063280A (zh) | 一种基于控制采样的智能车辆路径规划***及方法 | |
| Grigorescu et al. | Neurotrajectory: A neuroevolutionary approach to local state trajectory learning for autonomous vehicles | |
| CN112378408A (zh) | 一种实现轮式移动机器人实时避障的路径规划方法 | |
| Wang et al. | Efficient autonomous exploration with incrementally built topological map in 3-D environments | |
| CN114397896A (zh) | 一种改进粒子群算法的动态路径规划方法 | |
| Zhu et al. | A hierarchical deep reinforcement learning framework with high efficiency and generalization for fast and safe navigation | |
| CN114077256A (zh) | 一种水上无人船路径规划方法 | |
| CN113391633A (zh) | 一种面向城市环境的移动机器人融合路径规划方法 | |
| Yin et al. | Diverse critical interaction generation for planning and planner evaluation | |
| Short et al. | Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps | |
| CN116448134B (zh) | 基于风险场与不确定分析的车辆路径规划方法及装置 | |
| Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
| CN115031753B (zh) | 基于安全势场和dqn算法的行车工况局部路径规划方法 | |
| JP2020149095A (ja) | 倒立振子型ロボット | |
| Wang et al. | Efficient reinforcement learning for autonomous ship collision avoidance under learning experience reuse | |
| Xia et al. | Research on Path Planning Algorithm of Service Robot in Dynamic Environment | |
| Liu et al. | Deep Reinforcement Learning-Based Driving Policy at Intersections Utilizing Lane Graph Networks | |
| Moreira | Deep Reinforcement Learning for Automated Parking | |
| Wu et al. | Smooth path planning method of agricultural vehicles based on improved Hybrid A |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |