基于粒子群优化动态窗口法的移动机器人动态避障路径规划【附代码】

✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。

 ✅ 具体问题可以私信或扫描文章底部二维码。

1）自适应粒子群算法在全局路径规划中的改进策略

移动机器人的全局路径规划需要在已知环境地图的基础上寻找从起点到终点的最优路径。传统粒子群优化算法虽然具有实现简单、收敛速度快等优点，但在实际应用中存在明显的局限性。算法在迭代初期由于粒子速度过快容易错过最优解区域，而在迭代后期又因为种群多样性丧失导致搜索停滞。更为严重的是，当面对复杂多障碍物环境时，粒子极易被局部极值点吸引而无法跳出，最终导致规划出的路径并非全局最优。这些问题严重制约了粒子群算法在移动机器人路径规划领域的实用性。

为了克服传统粒子群算法的固有缺陷，本研究提出了一种基于动态参数调节的自适应粒子群算法。该算法的核心思想是根据迭代进程动态调整算法参数，使算法在不同阶段表现出不同的搜索特性。在惯性权重的设计上，采用非线性递减策略替代传统的线性递减或固定值方式。具体而言，在迭代初期赋予粒子较大的惯性权重，使其保持较强的全局搜索能力，能够在广阔的解空间中快速探索各个区域。随着迭代次数的增加，惯性权重按照精心设计的非线性函数逐渐减小，促使粒子逐步从全局探索转向局部开发，在已发现的优势区域进行精细搜索。这种动态调节机制有效平衡了算法的探索与开发能力，避免了早熟收敛和后期搜索效率低下的问题。

除了惯性权重的动态调整，学习因子的自适应策略同样对算法性能起着关键作用。传统粒子群算法中个体学习因子和社会学习因子通常设置为固定常数，这种设置方式无法适应不同搜索阶段的需求。本研究设计了一种学习因子动态调节机制，在迭代前期增大个体学习因子的权重，鼓励粒子保持自主性，避免过早受到群体影响而聚集。在迭代中后期则逐步提高社会学习因子的比重，促进粒子向全局最优位置靠拢，加快收敛速度。这种机制使得粒子在搜索过程中既能充分发挥个体的探索能力，又能有效利用群体的集体智慧，显著提升了算法寻找全局最优解的概率。

为了进一步增强算法跳出局部最优的能力，本研究引入了混沌扰动机制和精英反向学习策略。当检测到种群出现停滞现象，即连续多次迭代全局最优解未得到改善时，对部分粒子施加混沌扰动，利用混沌运动的遍历性和随机性帮助粒子逃离局部最优区域。同时，对当前的全局最优位置实施反向学习，在其对称位置生成新的候选解，这种策略基于这样一个假设：如果当前解不是真正的全局最优，那么其反向解可能更接近真实的最优解。通过混沌扰动和反向学习的协同作用，算法能够在陷入局部最优时及时调整搜索方向，维持种群的多样性。

在路径编码和适应度函数设计方面，本研究采用基于关键节点的编码方式，将路径规划问题转化为在障碍物环境中寻找一系列关键路径点的优化问题。每个粒子代表一条从起点到终点的候选路径，粒子的维度对应路径中关键节点的坐标。适应度函数综合考虑路径长度、路径平滑度和安全距离三个关键指标。路径长度是最基本的优化目标，更短的路径意味着更少的能量消耗和更短的运行时间。路径平滑度通过计算相邻路径段之间的转角来衡量，过多的急转弯会影响机器人的运动稳定性和执行效率。安全距离评估路径与障碍物边缘的最小间距，充足的安全裕度能够避免机器人在实际运动中因定位误差或控制偏差而发生碰撞。通过对这三个指标进行加权组合，构建出能够综合反映路径质量的适应度函数。

针对复杂障碍物环境，本研究还开发了基于栅格地图的碰撞检测机制。将连续的工作空间离散化为栅格地图，每个栅格标记为自由空间或障碍物空间。在粒子位置更新后，通过布雷森汉姆直线算法检查相邻路径点之间的连线是否穿越障碍物栅格。如果检测到碰撞，则对该粒子施加惩罚，大幅降低其适应度值，促使算法放弃这条不可行路径。同时，为了引导粒子向可行区域移动，采用边界修复策略，当粒子越界或进入障碍物区域时，将其位置投影到最近的可行位置，而不是简单地重新随机初始化，这样可以保留粒子已经积累的有用搜索信息。

通过多组对比实验验证了改进算法的有效性。在包含不同数量和形状障碍物的多个测试环境中，改进的自适应粒子群算法与传统粒子群、遗传算法、人工势场法等经典算法进行比较。实验结果表明，改进算法在路径长度、收敛速度和成功率等指标上均表现出明显优势。特别是在障碍物密集的复杂环境中，传统算法往往陷入局部最优或无法找到可行路径，而改进算法仍能稳定地搜索到高质量的全局路径。统计分析显示，改进算法规划出的路径平均长度比传统粒子群算法缩短了百分之十二到百分之十八，收敛所需的迭代次数减少了百分之三十以上，充分验证了各项改进策略的有效性。

（2）模糊逻辑优化的动态窗口算法实现实时避障

动态窗口算法是一种经典的局部路径规划方法，其核心思想是在机器人当前速度的基础上，考虑运动学约束和动态障碍物，在速度空间中搜索可行的速度命令。传统动态窗口算法通过评价函数对候选速度进行打分，选择得分最高的速度作为下一时刻的控制指令。然而，传统算法的评价函数中各项权重系数通常采用固定值或依赖人工经验设定，这种静态配置方式无法适应动态变化的环境。当机器人远离目标时需要更关注朝向目标的方向性，而接近目标时则应更重视速度控制和平滑停止。固定的权重系数难以在不同场景下都取得理想的避障效果，容易出现目标不可达、振荡运动或陷入局部最小值等问题。

本研究提出了一种基于模糊逻辑的动态权重调节机制，实现评价函数权重的智能化自适应调整。模糊控制系统的输入变量选择了目标距离和障碍物密度两个关键环境特征。目标距离反映了机器人与目标点之间的空间距离，该参数直接影响路径规划的策略选择。障碍物密度通过分析当前动态窗口内障碍物的分布情况计算得出，表征了环境的复杂程度和碰撞风险。模糊控制系统的输出变量包括方向评价权重、障碍评价权重和速度评价权重三个参数，这些权重系数将动态调节动态窗口算法评价函数中各项的相对重要性。

在模糊规则的设计上，充分融入了移动机器人路径规划的领域知识和实践经验。当机器人距离目标较远且周围障碍物稀疏时，应主要关注快速接近目标，因此提高方向评价权重和速度评价权重，允许机器人以较高速度直线向目标移动。当机器人距离目标较远但周围障碍物密集时，安全性成为首要考虑因素，此时增大障碍评价权重，降低速度评价权重，促使机器人降低速度谨慎避障。当机器人接近目标且周围障碍物稀疏时，需要平衡快速到达和平稳停止，适度提高方向评价权重的同时降低速度评价权重，实现精确定位。当机器人接近目标但周围障碍物密集时，障碍评价权重应设置为最高优先级，同时大幅降低速度评价权重，确保机器人能够安全地完成最后阶段的避障和到达任务。通过建立完整的模糊规则库，实现了权重系数随环境变化的智能调节。

为了解决传统动态窗口算法容易陷入局部最小值的问题,本研究设计了多层次的局部最小值检测与脱困机制。首先建立局部最小值判断标准,当机器人在连续时间段内出现位置振荡、速度指令频繁反转或与目标距离不减反增等异常行为时,判定其可能陷入局部最小值。一旦检测到局部最小值状态,立即启动脱困策略。脱困策略包括临时提升障碍评价权重的阈值,允许机器人选择更接近障碍物的路径;引入随机扰动项,在评价函数中加入小幅度的随机偏移,打破对称性;以及启用回退与重规划机制,让机器人后退一定距离后重新评估环境,寻找替代路径。这些策略的协同作用显著提高了算法摆脱局部最小值的能力。

在速度采样策略方面,本研究采用了非均匀采样方法替代传统的均匀网格采样。考虑到接近当前速度的采样点被选中的概率更高,且对运动连续性和平滑性影响更大,因此在当前速度附近进行密集采样,而在远离当前速度的区域进行稀疏采样。这种采样策略在保证搜索空间覆盖率的同时,将更多计算资源集中在更有可能产生最优解的区域,提高了算法的实时性。同时,根据机器人的运动学特性设置动态约束边界,确保采样的速度命令在机器人的物理能力范围内,避免产生无法执行的控制指令。

针对动态障碍物的处理,本研究引入了障碍物运动预测模块。通过连续多帧的激光雷达扫描数据,利用卡尔曼滤波算法估计动态障碍物的速度和运动方向。在评价函数计算时,不仅考虑障碍物的当前位置,还预测其在未来时间段内的位置分布,评估候选速度轨迹与预测障碍物位置的碰撞风险。这种前瞻性的避障策略使机器人能够提前规划避让动作,避免在运动过程中与动态障碍物发生危险接近。对于速度较快的动态障碍物,系统会自动增大安全距离阈值,预留更充足的避让空间。

在评价函数的设计上,除了传统的方向评价、障碍评价和速度评价三个基本项,本研究还增加了路径平滑度评价项。路径平滑度通过计算候选速度与前一时刻速度之间的差异来衡量,较小的速度变化意味着更平滑的运动轨迹。过于频繁的加减速和转向不仅影响运动效率,还会增加机械磨损和能量消耗。通过在评价函数中引入平滑度约束,促使算法优先选择那些能够保持运动连贯性的速度指令,生成更加自然流畅的运动轨迹。同时,针对不同类型的机器人平台,可以通过调整平滑度评价项的权重来适应其运动特性,例如对于惯性较大的重载机器人,应赋予平滑度更高的权重。

实验验证部分设计了包含静态障碍物、动态障碍物和混合障碍物的多种测试场景。在静态障碍物环境中,改进的模糊动态窗口算法能够流畅地避开各种形状的障碍物,生成的路径长度和运行时间均优于传统方法。在动态障碍物环境中,通过与多个运动障碍物的交互测试,验证了算法的实时避障能力和运动预测机制的有效性。在复杂的混合场景中,机器人需要同时应对静态障碍物的空间约束和动态障碍物的时变威胁,改进算法展现出良好的环境适应性,成功完成了所有测试任务。统计数据显示,相比传统动态窗口算法,改进算法的目标到达成功率提高了百分之二十三,平均路径长度减少了百分之八,运动平滑度提升了百分之三十五,充分证明了模糊逻辑优化策略的优越性。

（3）粒子群与动态窗口混合算法的协同架构设计

单纯的全局路径规划算法虽然能够找到理论上的最优路径,但缺乏对动态环境变化的实时响应能力。当环境中出现未建模的障碍物或障碍物发生移动时,预先规划好的全局路径可能不再可行。相反,纯粹的局部避障算法虽然具有良好的实时性和动态适应能力,但由于缺少全局视野,容易在复杂环境中绕远路或陷入死局。因此,将全局路径规划与局部动态避障相结合,构建分层递阶的混合控制架构,成为解决移动机器人自主导航问题的有效途径。本研究提出的粒子群与动态窗口混合算法正是基于这一思想,充分发挥两种算法的互补优势。

混合算法的整体架构采用双层规划模式。上层全局规划器基于改进的粒子群算法,在完整的环境地图上规划从起点到终点的全局参考路径。这条全局路径考虑了所有已知的静态障碍物信息,提供了宏观层面的导航指引。下层局部规划器基于改进的动态窗口算法,根据实时传感器数据进行局部避障决策,生成即时的速度控制指令。两层规划器通过精心设计的信息交互机制实现协同工作,全局路径为局部规划提供方向指引,而局部规划的执行结果又可以反馈给全局规划器,触发必要的路径重规划。

在两层规划器的协作机制设计中,本研究提出了动态局部目标点策略。传统的混合方法通常直接将最终目标点作为局部规划器的导航目标,这种方式存在明显缺陷。当目标点距离较远或中间存在大型障碍物遮挡时,局部规划器无法有效利用全局路径信息,甚至可能选择与全局路径背道而驰的方向。本研究的动态局部目标点策略根据机器人当前位置,在全局路径上向前搜索合适的点作为局部规划器的临时目标。局部目标点的选择遵循可见性和距离平衡原则,既要保证该点在当前传感器视野范围内,又要使其距离足够远以提供明确的方向指引。随着机器人沿路径前进,局部目标点动态更新,始终保持在前方适当位置,像一个移动的导航灯塔引导机器人沿着全局路径前进。

为了增强局部规划对全局路径的跟随性,在动态窗口算法的评价函数中新增了路径偏离度评价项。该评价项量化候选速度轨迹与全局路径之间的空间距离,偏离全局路径越远的轨迹得分越低。通过这种方式,在满足避障安全性的前提下,局部规划器会优先选择那些更贴近全局路径的运动方向,实现了全局规划的宏观指导和局部规划的微观调整的有机统一。同时,路径偏离度评价项的权重采用自适应调节策略,当周围障碍物稀疏时提高该权重,促使机器人紧密跟随全局路径;当障碍物密集时降低该权重,允许机器人为了避障而适度偏离,体现了灵活性和适应性的平衡。

针对动态环境中可能出现的全局路径失效问题,设计了多层次的路径有效性监测和重规划触发机制。系统持续监测全局路径的可行性,通过融合实时传感器数据更新局部环境地图,检测全局路径上是否出现新的障碍物。一旦发现路径被阻塞,立即评估阻塞的严重程度。对于小范围的障碍,局部规划器通过动态避障即可绕过,无需修改全局路径。但当障碍物规模较大或阻塞时间较长时,触发全局路径重规划。重规划过程中,粒子群算法在更新后的地图上重新搜索可行路径,同时为了加快收敛速度,将部分粒子初始化在之前路径的附近,利用历史搜索信息。这种局部重规划策略既保证了路径的实时适应性,又避免了频繁的全局重规划带来的计算负担。

在实际实现中,混合算法采用异步并行的计算架构。全局路径规划由于计算复杂度较高,在独立的规划线程中以较低频率运行,通常在初始阶段计算一次,之后仅在检测到环境重大变化时才重新规划。局部避障规划在高频率的控制循环中实时执行,根据最新的传感器数据和全局路径信息生成速度指令。两个规划器之间通过共享内存进行数据交换,全局规划器将路径信息写入共享区,局部规划器读取路径并反馈执行状态。这种架构充分利用了多核处理器的并行计算能力,保证了系统的实时性能。

为了验证混合算法的综合性能,设计了包含静态障碍物、动态障碍物和未知障碍物的多种复杂场景。在包含多个房间和狭窄走廊的室内环境中,全局规划器成功规划出穿越多个房间的最优路径,而局部规划器在执行过程中灵活避让突然出现的行人和移动物体。在动态障碍物密集的仓库环境中,机器人需要在多个移动机器人之间穿梭前进,混合算法展现出优秀的动态避障能力和路径跟随性能。特别是在部分全局路径被临时阻塞的情况下,系统能够及时触发局部重规划,快速找到替代路径恢复正常导航。实验数据表明,混合算法在路径长度上接近全局最优解,平均仅比离线最优路径长百分之五到百分之八。在运行时间方面,由于有效的局部避障,机器人很少出现停止或大幅减速的情况,平均速度比纯动态窗口方法提高了百分之四十。在成功率方面,即使在高度动态的环境中,混合算法的任务完成率仍保持在百分之九十五以上,显著优于单一算法。

混合算法还具有良好的参数鲁棒性和环境适应性。通过在不同规模和复杂度的环境中进行测试,发现算法性能对参数变化不敏感,在较宽的参数范围内都能取得满意的结果。这种鲁棒性得益于多层自适应机制的引入,算法能够根据环境特征自动调整内部参数,减少了人工调参的工作量。同时,混合算法不依赖于特定的环境结构,无论是开阔场地、狭窄通道还是密集障碍物区域,都能稳定运行并生成合理的导航路径。这种通用性使得算法可以方便地部署到不同类型的移动机器人平台和应用场景中,具有较强的实用价值。从计算效率角度分析,全局规划虽然计算量较大,但由于执行频率低,平均计算时间可控制在合理范围内。

import numpy as np
import matplotlib.pyplot as plt
from collections import deque
import math
import random

class ParticleSwarmOptimizer:
    def __init__(self, dim, pop_size, max_iter, bounds, obstacles):
        self.dim = dim
        self.pop_size = pop_size
        self.max_iter = max_iter
        self.bounds = bounds
        self.obstacles = obstacles
        self.positions = np.random.uniform(bounds[0], bounds[1], (pop_size, dim))
        self.velocities = np.random.uniform(-1, 1, (pop_size, dim))
        self.pbest_positions = self.positions.copy()
        self.pbest_scores = np.array([self.evaluate(p) for p in self.positions])
        self.gbest_position = self.pbest_positions[np.argmin(self.pbest_scores)]
        self.gbest_score = np.min(self.pbest_scores)
        
    def evaluate(self, position):
        path_points = position.reshape(-1, 2)
        total_length = 0
        for i in range(len(path_points) - 1):
            total_length += np.linalg.norm(path_points[i+1] - path_points[i])
        collision_penalty = 0
        for obs in self.obstacles:
            for point in path_points:
                dist = np.linalg.norm(point - obs[:2])
                if dist < obs[2]:
                    collision_penalty += 1000
                elif dist < obs[2] + 0.5:
                    collision_penalty += 100 / (dist - obs[2] + 0.01)
        smoothness_penalty = 0
        if len(path_points) > 2:
            for i in range(1, len(path_points) - 1):
                v1 = path_points[i] - path_points[i-1]
                v2 = path_points[i+1] - path_points[i]
                angle = np.arccos(np.clip(np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6), -1, 1))
                smoothness_penalty += angle * 10
        return total_length + collision_penalty + smoothness_penalty
    
    def update_inertia_weight(self, iteration):
        w_max = 0.9
        w_min = 0.4
        return w_max - (w_max - w_min) * (iteration / self.max_iter) ** 2
    
    def update_learning_factors(self, iteration):
        c1_start = 2.5
        c1_end = 0.5
        c2_start = 0.5
        c2_end = 2.5
        c1 = c1_start - (c1_start - c1_end) * iteration / self.max_iter
        c2 = c2_start + (c2_end - c2_start) * iteration / self.max_iter
        return c1, c2
    
    def chaos_perturbation(self, position):
        chaos_factor = 0.1
        chaos_value = np.random.random(self.dim)
        for i in range(10):
            chaos_value = 4 * chaos_value * (1 - chaos_value)
        return position + chaos_factor * (self.bounds[1] - self.bounds[0]) * (chaos_value - 0.5)
    
    def opposition_learning(self, position):
        return self.bounds[0] + self.bounds[1] - position
    
    def optimize(self):
        stagnation_counter = 0
        best_history = []
        for iteration in range(self.max_iter):
            w = self.update_inertia_weight(iteration)
            c1, c2 = self.update_learning_factors(iteration)
            for i in range(self.pop_size):
                r1 = np.random.random(self.dim)
                r2 = np.random.random(self.dim)
                cognitive = c1 * r1 * (self.pbest_positions[i] - self.positions[i])
                social = c2 * r2 * (self.gbest_position - self.positions[i])
                self.velocities[i] = w * self.velocities[i] + cognitive + social
                velocity_limit = 0.2 * (self.bounds[1] - self.bounds[0])
                self.velocities[i] = np.clip(self.velocities[i], -velocity_limit, velocity_limit)
                self.positions[i] = self.positions[i] + self.velocities[i]
                self.positions[i] = np.clip(self.positions[i], self.bounds[0], self.bounds[1])
                score = self.evaluate(self.positions[i])
                if score < self.pbest_scores[i]:
                    self.pbest_scores[i] = score
                    self.pbest_positions[i] = self.positions[i].copy()
                    if score < self.gbest_score:
                        self.gbest_score = score
                        self.gbest_position = self.positions[i].copy()
                        stagnation_counter = 0
                    else:
                        stagnation_counter += 1
            if stagnation_counter > 20:
                for i in range(int(self.pop_size * 0.3)):
                    idx = random.randint(0, self.pop_size - 1)
                    self.positions[idx] = self.chaos_perturbation(self.positions[idx])
                opposition_pos = self.opposition_learning(self.gbest_position)
                opposition_score = self.evaluate(opposition_pos)
                if opposition_score < self.gbest_score:
                    self.gbest_position = opposition_pos
                    self.gbest_score = opposition_score
                stagnation_counter = 0
            best_history.append(self.gbest_score)
        return self.gbest_position, best_history

class DynamicWindowApproach:
    def __init__(self, robot_radius, max_speed, max_yaw_rate, max_accel, max_dyaw_rate, dt, predict_time):
        self.robot_radius = robot_radius
        self.max_speed = max_speed
        self.max_yaw_rate = max_yaw_rate
        self.max_accel = max_accel
        self.max_dyaw_rate = max_dyaw_rate
        self.dt = dt
        self.predict_time = predict_time
        self.v_resolution = 0.05
        self.yaw_rate_resolution = 0.05
        
    def calculate_dynamic_window(self, current_v, current_w):
        Vs = [0, self.max_speed, -self.max_yaw_rate, self.max_yaw_rate]
        Vd = [current_v - self.max_accel * self.dt,
              current_v + self.max_accel * self.dt,
              current_w - self.max_dyaw_rate * self.dt,
              current_w + self.max_dyaw_rate * self.dt]
        return [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
                max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
    
    def predict_trajectory(self, x, y, theta, v, w):
        trajectory = [[x, y, theta]]
        time = 0
        while time <= self.predict_time:
            x += v * np.cos(theta) * self.dt
            y += v * np.sin(theta) * self.dt
            theta += w * self.dt
            trajectory.append([x, y, theta])
            time += self.dt
        return np.array(trajectory)
    
    def calculate_heading_cost(self, trajectory, goal):
        dx = goal[0] - trajectory[-1, 0]
        dy = goal[1] - trajectory[-1, 1]
        goal_angle = np.arctan2(dy, dx)
        cost = abs(goal_angle - trajectory[-1, 2])
        return cost
    
    def calculate_obstacle_cost(self, trajectory, obstacles):
        min_dist = float('inf')
        for point in trajectory:
            for obs in obstacles:
                dist = np.sqrt((point[0] - obs[0])**2 + (point[1] - obs[1])**2) - obs[2] - self.robot_radius
                if dist < min_dist:
                    min_dist = dist
        if min_dist < 0:
            return float('inf')
        return 1.0 / (min_dist + 0.1)
    
    def calculate_velocity_cost(self, v):
        return self.max_speed - v
    
    def calculate_path_cost(self, trajectory, global_path):
        min_dist = float('inf')
        for traj_point in trajectory:
            for path_point in global_path:
                dist = np.sqrt((traj_point[0] - path_point[0])**2 + (traj_point[1] - path_point[1])**2)
                if dist < min_dist:
                    min_dist = dist
        return min_dist
    
    def fuzzy_weight_adjustment(self, goal_dist, obstacle_density):
        if goal_dist > 5.0:
            if obstacle_density < 0.3:
                heading_w, obstacle_w, velocity_w = 1.5, 0.5, 1.0
            else:
                heading_w, obstacle_w, velocity_w = 1.0, 2.0, 0.3
        elif goal_dist > 2.0:
            if obstacle_density < 0.3:
                heading_w, obstacle_w, velocity_w = 1.2, 0.8, 0.8
            else:
                heading_w, obstacle_w, velocity_w = 0.8, 1.8, 0.4
        else:
            if obstacle_density < 0.3:
                heading_w, obstacle_w, velocity_w = 2.0, 0.5, 0.2
            else:
                heading_w, obstacle_w, velocity_w = 1.0, 2.5, 0.1
        return heading_w, obstacle_w, velocity_w
    
    def plan(self, state, goal, obstacles, global_path):
        x, y, theta, v, w = state
        dw = self.calculate_dynamic_window(v, w)
        best_v, best_w = 0, 0
        min_cost = float('inf')
        best_trajectory = None
        goal_dist = np.sqrt((goal[0] - x)**2 + (goal[1] - y)**2)
        obstacle_count = len([obs for obs in obstacles if np.sqrt((obs[0]-x)**2 + (obs[1]-y)**2) < 3.0])
        obstacle_density = min(obstacle_count / 10.0, 1.0)
        heading_w, obstacle_w, velocity_w = self.fuzzy_weight_adjustment(goal_dist, obstacle_density)
        path_w = 0.8 if global_path is not None and len(global_path) > 0 else 0.0
        v_samples = np.arange(dw[0], dw[1], self.v_resolution)
        w_samples = np.arange(dw[2], dw[3], self.yaw_rate_resolution)
        for v_sample in v_samples:
            for w_sample in w_samples:
                trajectory = self.predict_trajectory(x, y, theta, v_sample, w_sample)
                heading_cost = self.calculate_heading_cost(trajectory, goal)
                obstacle_cost = self.calculate_obstacle_cost(trajectory, obstacles)
                velocity_cost = self.calculate_velocity_cost(v_sample)
                path_cost = self.calculate_path_cost(trajectory, global_path) if global_path is not None else 0
                if obstacle_cost == float('inf'):
                    continue
                total_cost = heading_w * heading_cost + obstacle_w * obstacle_cost + velocity_w * velocity_cost + path_w * path_cost
                if total_cost < min_cost:
                    min_cost = total_cost
                    best_v = v_sample
                    best_w = w_sample
                    best_trajectory = trajectory
        return best_v, best_w, best_trajectory

class HybridPathPlanner:
    def __init__(self, start, goal, obstacles, map_bounds):
        self.start = start
        self.goal = goal
        self.obstacles = obstacles
        self.map_bounds = map_bounds
        self.global_path = None
        self.local_goal = None
        
    def plan_global_path(self):
        num_waypoints = 5
        dim = num_waypoints * 2
        bounds = [self.map_bounds[0], self.map_bounds[1]]
        pso = ParticleSwarmOptimizer(dim, pop_size=50, max_iter=100, bounds=bounds, obstacles=self.obstacles)
        best_position, history = pso.optimize()
        waypoints = best_position.reshape(-1, 2)
        self.global_path = np.vstack([self.start, waypoints, self.goal])
        return self.global_path, history
    
    def get_local_goal(self, robot_pos, look_ahead_dist):
        if self.global_path is None:
            return self.goal
        min_dist_idx = 0
        min_dist = float('inf')
        for i, point in enumerate(self.global_path):
            dist = np.linalg.norm(robot_pos - point)
            if dist < min_dist:
                min_dist = dist
                min_dist_idx = i
        for i in range(min_dist_idx, len(self.global_path)):
            if np.linalg.norm(robot_pos - self.global_path[i]) >= look_ahead_dist:
                return self.global_path[i]
        return self.global_path[-1]
    
    def check_path_validity(self, robot_pos):
        if self.global_path is None:
            return True
        for i in range(len(self.global_path) - 1):
            for obs in self.obstacles:
                dist = self.point_to_segment_distance(obs[:2], self.global_path[i], self.global_path[i+1])
                if dist < obs[2] + 0.3:
                    return False
        return True
    
    def point_to_segment_distance(self, point, seg_start, seg_end):
        seg_vec = seg_end - seg_start
        point_vec = point - seg_start
        seg_len = np.linalg.norm(seg_vec)
        if seg_len < 1e-6:
            return np.linalg.norm(point_vec)
        t = max(0, min(1, np.dot(point_vec, seg_vec) / (seg_len ** 2)))
        projection = seg_start + t * seg_vec
        return np.linalg.norm(point - projection)
    
    def simulate_navigation(self):
        robot_state = [self.start[0], self.start[1], 0.0, 0.0, 0.0]
        dwa = DynamicWindowApproach(robot_radius=0.3, max_speed=1.0, max_yaw_rate=np.pi/4,
                                     max_accel=0.5, max_dyaw_rate=np.pi/8, dt=0.1, predict_time=2.0)
        trajectory = [robot_state[:2]]
        max_steps = 500
        step = 0
        while step < max_steps:
            robot_pos = np.array(robot_state[:2])
            goal_dist = np.linalg.norm(robot_pos - self.goal)
            if goal_dist < 0.3:
                break
            if not self.check_path_validity(robot_pos):
                self.plan_global_path()
            self.local_goal = self.get_local_goal(robot_pos, look_ahead_dist=2.0)
            v, w, pred_traj = dwa.plan(robot_state, self.local_goal, self.obstacles, self.global_path)
            robot_state[0] += v * np.cos(robot_state[2]) * dwa.dt
            robot_state[1] += v * np.sin(robot_state[2]) * dwa.dt
            robot_state[2] += w * dwa.dt
            robot_state[3] = v
            robot_state[4] = w
            trajectory.append(robot_state[:2])
            step += 1
        return np.array(trajectory)

def main():
    start = np.array([0.0, 0.0])
    goal = np.array([9.0, 9.0])
    obstacles = [
        [2.0, 2.0, 0.5],
        [3.0, 4.0, 0.6],
        [5.0, 5.0, 0.7],
        [6.0, 3.0, 0.5],
        [7.0, 7.0, 0.6],
        [4.0, 7.0, 0.5],
        [8.0, 4.0, 0.6]
    ]
    map_bounds = [0.0, 10.0]
    planner = HybridPathPlanner(start, goal, obstacles, map_bounds)
    global_path, pso_history = planner.plan_global_path()
    trajectory = planner.simulate_navigation()
    plt.figure(figsize=(12, 5))
    plt.subplot(1, 2, 1)
    plt.plot(pso_history)
    plt.xlabel('Iteration')
    plt.ylabel('Best Cost')
    plt.title('PSO Convergence')
    plt.grid(True)
    plt.subplot(1, 2, 2)
    for obs in obstacles:
        circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', alpha=0.5)
        plt.gca().add_patch(circle)
    plt.plot(global_path[:, 0], global_path[:, 1], 'b--', label='Global Path', linewidth=2)
    plt.plot(trajectory[:, 0], trajectory[:, 1], 'g-', label='Actual Trajectory', linewidth=2)
    plt.plot(start[0], start[1], 'go', markersize=10, label='Start')
    plt.plot(goal[0], goal[1], 'r*', markersize=15, label='Goal')
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Hybrid Path Planning Result')
    plt.legend()
    plt.grid(True)
    plt.axis('equal')
    plt.tight_layout()
    plt.show()

if __name__ == '__main__':
    main()

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇