（4-5）轨迹规划算法和优化：使用粒子群优化（PSO）对无人机路径进行规划和优化

作者：代码探险家 | 2024-08-14 10:01:24

踩

粒子群优化（Particle Swarm Optimization，PSO）是一种启发式算法，灵感来源于鸟群或鱼群中个体协同行为。PSO 通过模拟群体中个体间的合作与信息共享来搜索问题的解空间，尤其适用于全局优化问题。实现PSO 算法的基本步骤如下所示。

（1）初始化粒子群：在解空间中随机生成一定数量的粒子，每个粒子表示一个可能的解，具有位置和速度。这些粒子的初始化可以是随机的或基于先验知识的。

（2）定义适应性函数：问题的适应性函数（fitness function）用于评估每个粒子的解的优劣。适应性函数是 PSO 的引导力量，粒子试图优化适应性函数的值。

（3）更新速度和位置：每个粒子根据其个体历史最优解和整个群体历史最优解来更新速度和位置。这个更新过程使得粒子在解空间中移动，朝着更优的方向搜索。

速度更新公式如下：

速度新=w×速度旧+c1×r1×(个体最优位置−当前位置)+c×r2×(群体最优位置−当前位置)

其中，w 是惯性权重，c 1和 c 2是学习因子，r1和 r2是随机数。

位置更新公式如下：

位置新=位置旧+速度新

（4）更新个体历史最优解和群体历史最优解：对每个粒子，根据当前的适应性函数值更新其个体历史最优解。然后，通过比较所有粒子的适应性函数值，更新群体历史最优解。

（5）重复迭代过程：重复执行前面的步骤（3）和步骤（4），直到满足停止条件（例如达到最大迭代次数或适应性函数值足够小）。

PSO 算法的关键参数包括群体中的粒子数量、学习因子 c 1和 c2、惯性权重 w 等，这些参数的选择可能影响算法的性能，因此需要根据具体问题进行调整和优化。

总体来说，PSO 算法是一种简单而有效的优化方法，适用于解决多种连续、离散、非线性的优化问题，包括轨迹规划、神经网络训练、组合优化等。粒子群优化（PSO）在轨迹规划中可以发挥重要作用，尤其是在全局搜索问题和高维配置空间中。请看下面的例子，展示了使用粒子群优化（PSO）对无人机（UAV）路径进行规划和优化的过程。

实例4-10：使用PSO对无人机路径进行规划和优化（源码路径：codes\4\gui\uav-pso.ipynb）

在本项目中，使用粒子群优化（PSO）算法实现了无人机的路径规划与优化功能。通过考虑最小化路径长度和能量消耗，同时避免环境中的障碍物，PSO调整多个粒子在多维搜索空间中的位置和速度，以寻找优化的无人机轨迹。通过迭代优化，算法逐步演进，记录全局最优解的代价，最终绘制出无人机在环境中的最佳路径图，提供了一种在实际环境中规划无人机路径的方法。实例文件uav-pso.ipynb的具体实现流程如下所示。

（1）导入所需的库

引入必要的库，例如 NumPy 用于数学计算，Matplotlib 用于绘图，Pandas进行数据处理和CSV文件I/O，Matplotlib进行图形绘制，使用scipy.io中的savemat保存.mat文件，使用mpl_toolkits.mplot3d中的Axes3D实现3D绘图。

（2）问题建模

定义函数create_model，用于创建一个简单的路径规划模型。在模型中包含了起始点、目标点、障碍物位置及半径、路径离散点数量等信息。这个模型的三维空间范围在 x 方向为 0 到 6，y 方向为 0 到 6，z 方向为 -10 到 10。函数返回一个包含模型参数的字典。


def create_model():
    # Source
    xs = 0
    ys = 0
    zs = 0    
    # Target (Destination)
    xt = 4
    yt = 5
    zt = 5
    xobs = [1.5, 4.0, 1.2]
    yobs = [4.5, 3.0, 1.5]
    zobs = [4.5, 3.0, 1.5]
    robs = [0.6, 0.6, 0.8]   
    n = 10    
    xmin = 0
    xmax = 6
    ymin = 0
    ymax = 6   
    zmin = -10
    zmax = 10
    model = {
        "xs": xs,
        "ys": ys,
        "zs": zs,
        "xt": xt,
        "yt": yt,
        "zt": zt,
        "xobs": xobs,
        "yobs": yobs,
        "zobs": zobs,
        "robs": robs,
        "n": n,
        "xmin": xmin,
        "xmax": xmax,
        "ymin": ymin,
        "ymax": ymax,
        "zmin": zmin,
        "zmax": zmax,
    }
    return model

（3）创建随机解

定义函数 create_random_solution，用于根据给定的路径规划模型生成一个随机解。函数从模型中获取路径的离散点数量 n，以及 x 和 y 方向的范围。然后，通过在指定范围内生成随机坐标，创建一个包含 x 和 y 值的字典作为随机解，并返回该字典。


def create_random_solution(model):
    n = model["n"]
    xmin = model["xmin"]
    xmax = model["xmax"]
    ymin = model["ymin"]
    ymax = model["ymax"]
    sol1 = {
        "x": np.random.uniform(xmin, xmax, size=n),
        "y": np.random.uniform(ymin, ymax, size=n),
    }
    
    return sol1

（4）定义成本函数

定义问题的成本函数 my_cost，也称为适应性函数，该函数计算给定路径规划模型和解的代价。函数 my_cost首先调用 parse_solution 函数将解解析为可计算的格式，然后使用解的路径长度 sol["L"] 和违反约束的程度 sol["Violation"] 计算代价 z。在代价计算中引入了一个权重因子 beta，最终返回代价 z 和解的详细信息 sol。


def my_cost(sol1, model):   
    sol = parse_solution(sol1, model)   
    beta = 100
    z = sol["L"] * (1 + beta * sol["Violation"])
    return z, sol

（5）解析解决方案

定义解的析解函数 parse_solution，功能是将给定的解映射到可用于计算代价的格式。函数parse_solution根据路径规划模型中的起始点、目标点、障碍物位置等信息，使用三次样条插值方法对解进行插值，生成平滑的路径。随后，计算插值路径的长度 L 和路径与障碍物的违反程度 Violation。最终，返回一个包含解析后的解信息的字典。


import numpy as np
from scipy.interpolate import splprep, splev, splrep, interp1d
 
def parse_solution(sol1, model): 
    x = sol1["x"]
    y = sol1["y"]
    
    xs = model["xs"]
    ys = model["ys"]
    xt = model["xt"]
    yt = model["yt"]
    xobs = model["xobs"]
    yobs = model["yobs"]
    robs = model["robs"] 
    XS = [xs] + x.tolist() + [xt]
    YS = [ys] + y.tolist() + [yt]
#     print(XS)    
    k = len(XS)
    TS = np.linspace(0, 1, k)
    
    tt = np.linspace(0, 1, 100)
    xx = splrep(TS, XS, k=3)
    yy = splrep(TS, YS, k=3)
    x_interpolated = splev(tt, xx)
    y_interpolated = splev(tt, yy)
 
    dx = np.diff(x_interpolated)
    dy = np.diff(y_interpolated)
    L = np.sqrt(np.sum(np.square(dx) + np.square(dy)))    
    nobs = len(xobs)
    Violation = 0
    for k in range(nobs):
        d = np.sqrt((x_interpolated - xobs[k])**2 + (y_interpolated - yobs[k])**2)
        v = np.maximum(1 - d/robs[k], 0)
        Violation += np.mean(v)   
#     print(x_interpolated)
    sol2 = {
        "TS": TS,
        "XS": XS,
        "YS": YS,
        "tt": np.linspace(0, 1, 100),
        "xx": x_interpolated,
        "yy": y_interpolated,
        "dx": dx,
        "dy": dy,
        "L": L,
        "Violation": Violation,
        "IsFeasible": (Violation == 0),
    } 
    return sol2

（6）绘制解决方案

这一步是可选步骤，主要功能是绘制问题的解决方案，以便直观地了解路径规划和优化结果。例如通过下面的两个函数绘制路径规划模型的解，提供不同的视觉效果。


def PlotSolution(sol, model):
    xs = model['xs']
    ys = model['ys']
    xt = model['xt']
    yt = model['yt']
    xobs = model['xobs']
    yobs = model['yobs']
    robs = model['robs']
    
    XS = sol['XS']
    YS = sol['YS']
    xx = sol['xx']
    yy = sol['yy']
    
    theta = np.linspace(0, 2*np.pi, 100)
    for k in range(len(xobs)):
        plt.fill(xobs[k] + robs[k]*np.cos(theta), yobs[k] + robs[k]*np.sin(theta), [0.5, 0.7, 0.8])
    plt.plot(xx, yy, 'k', linewidth=2)
    plt.plot(XS, YS, 'ro')
    plt.plot(xs, ys, 'bs', markersize=12, markerfacecolor='y')
    plt.plot(xt, yt, 'kp', markersize=16, markerfacecolor='g')
    plt.grid(True)
    plt.axis('equal')
    plt.show()
 
def PlotSolution2(sol, model):
    
    fig, ax = plt.subplots()
    xs = model['xs']
    ys = model['ys']
    xt = model['xt']
    yt = model['yt']
    xobs = model['xobs']
    yobs = model['yobs']
    robs = model['robs']
    
    XS = sol['XS']
    YS = sol['YS']
    xx = sol['xx']
    yy = sol['yy']    
    ax.clear()
    
    theta = np.linspace(0, 2*np.pi, 100)
    for k in range(len(xobs)):
        ax.fill(xobs[k] + robs[k]*np.cos(theta), yobs[k] + robs[k]*np.sin(theta), [0.5, 0.7, 0.8])
    ax.plot(xx, yy, 'k', linewidth=2)
    ax.plot(XS, YS, 'ro')
#     ax.plot(xs, ys, 'bs', markersize=12, markerfacecolor='y')
    ax.plot(xs, ys, marker=(3, 0, np.rad2deg(np.pi/4)), markersize=12, markerfacecolor='y')
 
    ax.plot(xt, yt, 'kp', markersize=16, markerfacecolor='g')
    ax.grid(True)
    ax.axis('equal')
    plt.show()

对上述代码的具体说明如下所示：

PlotSolution函数：绘制包括起始点、目标点、障碍物位置和路径的简单图形。通过使用填充的障碍物区域、红色圆点表示路径离散点、蓝色正方形表示起始点、绿色五边形表示目标点，实现对路径规划的可视化。
PlotSolution2函数：利用动态绘图的方式，实时更新路径规划模型的解。同样，它显示了起始点、目标点、障碍物和路径，但与PlotSolution函数不同，它在每次调用时清除前一帧的图形，从而创建动画效果。此外，起始点采用黄色标记以区别。

这两个函数用于直观地展示路径规划的结果，并可以通过交互式地更新图形来更好地理解无人机的路径。

（7）粒子群优化设置

在下面的代码中，使用粒子群优化（PSO）算法进行路径规划的初始化阶段，其中包括定义问题的参数和初始化粒子群。


model = create_model()
model["n"] = 3  # 处理点数量
CostFunction = lambda x: my_cost(x, model)  # 成本函数
nVar = model["n"]  # 决策变量数量
VarSize = (1, nVar)  # 决策变量矩阵大小
VarMin = {"x": model["xmin"], "y": model["ymin"]}  # 变量的下界
VarMax = {"x": model["xmax"], "y": model["ymax"]}  # 变量的上界
 
## PSO 参数
MaxIt = 220  # 最大迭代次数
nPop = 150  # 种群大小（群体大小）
w = 1  # 惯性权重
wdamp = 0.98  # 惯性权重阻尼比
c1 = 1.5  # 个体学习系数
c2 = 1.5  # 全局学习系数
alpha = 0.1
VelMax = {"x": alpha * (VarMax["x"] - VarMin["x"]), "y": alpha * (VarMax["y"] - VarMin["y"])}  # 最大速度
VelMin = {"x": -VelMax["x"], "y": -VelMax["y"]}  # 最小速度
 
## 初始化
 
# 创建空粒子结构
empty_particle = {"Position": None,
                  "Velocity": None,
                  "Cost": None,
                  "Sol": None,
                  "Best": {"Position": None,
                           "Cost": None,
                           "Sol": None}}
 
# 初始化全局最优解
GlobalBest = {"Cost": np.inf}
 
# 创建粒子矩阵
particle = np.tile(empty_particle, (nPop, 1))

上述代码的实现流程如下：
创建路径规划模型：使用 create_model() 函数创建路径规划模型，并通过设置 model["n"] = 3 指定路径中的处理点数量。
定义成本函数：使用匿名函数 CostFunction 定义成本函数，其中调用 my_cost 函数计算给定解的代价。
PSO参数设置：设置PSO算法的参数，包括最大迭代次数 MaxIt、种群大小 nPop、惯性权重 w、惯性权重衰减比率 wdamp、个体学习系数 c1、全局学习系数 c2、以及最大和最小速度。
初始化粒子群：使用 empty_particle 创建一个空的粒子结构，初始化全局最优解 GlobalBest 的代价为无穷大。创建粒子矩阵 particle，其中包含 nPop 个粒子，每个粒子具有位置、速度、代价、解以及个体最优解等属性。

上述代码初始化了PSO算法所需的参数和数据结构，为接下来的PSO迭代提供了初始条件。

（8）PSO 初始循环

实现PSO算法的初始化循环操作，初始化粒子群中的每个粒子。在循环中，每个粒子被初始化，并通过随机生成的解进行位置初始化。粒子的代价通过成本函数计算，然后更新个体最优解和全局最优解。最终，所有粒子被添加到列表中。


# 创建一个空的粒子列表
particle = []
 
# 初始化循环
for i in range(nPop):
    
    # 初始化粒子
    xx = np.array([0.1, 3, 5.0])
    yy = np.array([0.1, 3, 5.0])
 
    new_particle = {
        'Position': {'x': xx, 'y': yy},
        'Velocity': {'x': None, 'y': None},
        'Cost': None,
        'Sol': None,
        'Best': {
            'Position': {'x': None, 'y': None},
            'Cost': None,
            'Sol': None
        }
    }
    
    # 初始化位置
    if i > 0:
        solu_1 = create_random_solution(model)
        new_particle['Position'] = {'x': solu_1['x'], 'y': solu_1['y']}
    
    # 初始化速度
    new_particle['Velocity']['x'] = np.zeros(VarSize)
    new_particle['Velocity']['y'] = np.zeros(VarSize)
    
    # 评估
    new_particle['Cost'], new_particle['Sol'] = CostFunction(new_particle['Position'])
    
    # 更新个体最优解
    new_particle['Best']['Position'] = new_particle['Position']
    new_particle['Best']['Cost'] = new_particle['Cost']
    new_particle['Best']['Sol'] = new_particle['Sol']
    
    # 将粒子添加到列表中
    particle.append(new_particle)
    
    # 更新全局最优解
    if new_particle['Best']['Cost'] < GlobalBest['Cost']:
        GlobalBest = new_particle['Best']

上述代码是粒子群优化（PSO）算法的初始化阶段，通过循环初始化了种群中的每个粒子。每个粒子包含位置、速度、代价、解以及个体最优解等属性。其中，位置通过随机生成或使用预定义的初始值进行初始化，代价通过成本函数计算。个体最优解和全局最优解分别根据粒子的当前状态进行更新。整体流程为创建并初始化粒子群，为后续PSO迭代提供了起始状态。

（9）PSO 主循环

下面代码实现了粒子群优化（PSO）算法的主循环，其中包含粒子位置和速度的更新、评估成本、个体最优解和全局最优解的更新等步骤。迭代执行粒子群优化的主循环，调整每个粒子的位置和速度，以搜索最优解。


BestCost = np.zeros(MaxIt)  # 用于记录每次迭代的全局最优解代价
frames = []  # 用于存储迭代过程中的图形帧
 
for it in range(MaxIt):
    for i in range(nPop):
        # 更新 x 部分
        # 更新速度
        particle[i]['Velocity']['x'] = w * particle[i]['Velocity']['x'] \
            + c1 * np.random.rand(*VarSize) * (particle[i]['Best']['Position']['x'] - particle[i]['Position']['x']) \
            + c2 * np.random.rand(*VarSize) * (GlobalBest['Position']['x'] - particle[i]['Position']['x'])
        # 更新速度边界
        particle[i]['Velocity']['x'] = np.maximum(particle[i]['Velocity']['x'], VelMin['x'])
        particle[i]['Velocity']['x'] = np.minimum(particle[i]['Velocity']['x'], VelMax['x'])
        # 更新位置
        particle[i]['Position']['x'] = particle[i]['Position']['x'] + particle[i]['Velocity']['x']
        # 速度镜像
        OutOfTheRange = (particle[i]['Position']['x'] < VarMin['x']) | (particle[i]['Position']['x'] > VarMax['x'])
        particle[i]['Velocity']['x'][OutOfTheRange] = -particle[i]['Velocity']['x'][OutOfTheRange]
        # 更新位置边界
        particle[i]['Position']['x'] = np.maximum(particle[i]['Position']['x'], VarMin['x']).flatten()
        particle[i]['Position']['x'] = np.minimum(particle[i]['Position']['x'], VarMax['x']).flatten()
 
        # 更新 y 部分
        # 更新速度
        particle[i]['Velocity']['y'] = w * particle[i]['Velocity']['y'] \
            + c1 * np.random.rand(*VarSize) * (particle[i]['Best']['Position']['y'] - particle[i]['Position']['y']) \
            + c2 * np.random.rand(*VarSize) * (GlobalBest['Position']['y'] - particle[i]['Position']['y'])
        # 更新速度边界
        particle[i]['Velocity']['y'] = np.maximum(particle[i]['Velocity']['y'], VelMin['y'])
        particle[i]['Velocity']['y'] = np.minimum(particle[i]['Velocity']['y'], VelMax['y'])
        # 更新位置
        particle[i]['Position']['y'] = particle[i]['Position']['y'] + particle[i]['Velocity']['y']
        # 速度镜像
        OutOfTheRange = (particle[i]['Position']['y'] < VarMin['y']) | (particle[i]['Position']['y'] > VarMax['y'])
        particle[i]['Velocity']['y'][OutOfTheRange] = -particle[i]['Velocity']['y'][OutOfTheRange]
        # 更新位置边界
        particle[i]['Position']['y'] = np.maximum(particle[i]['Position']['y'], VarMin['y']).flatten()
        particle[i]['Position']['y'] = np.minimum(particle[i]['Position']['y'], VarMax['y']).flatten()
 
        # 评估
        particle[i]['Cost'], particle[i]['Sol'] = CostFunction(particle[i]['Position'])
 
        # 更新个体最优解
        if particle[i]['Cost'] < particle[i]['Best']['Cost']:
            particle[i]['Best']['Position'] = particle[i]['Position']
            particle[i]['Best']['Cost'] = particle[i]['Cost']
            particle[i]['Best']['Sol'] = particle[i]['Sol']
 
            # 更新全局最优解
            if particle[i]['Best']['Cost'] < GlobalBest['Cost']:
                GlobalBest = particle[i]['Best']
 
    # 记录历史最优代价
    BestCost[it] = GlobalBest['Cost']
 
    # 惯性权重衰减
    w = w * wdamp
 
    # 显示迭代信息
    if GlobalBest['Sol']['IsFeasible']:
        Flag = ' *'
    else:
        Flag = ', Violation = ' + str(GlobalBest['Sol']['Violation'])
#     print('Iteration', it, ': Best Cost =', BestCost[it], Flag)
 
# 绘制最优解
PlotSolution2(GlobalBest['Sol'], model)

执行后绘制路径规划的可视化图，展示了通过粒子群优化（PSO）算法得到的无人机路径，如图4-8所示。具体而言，PlotSolution2 函数用于绘制全局最优解的路径图，其中包括起始点、目标点、障碍物以及无人机路径。这个可视化图展示了PSO算法优化后的最佳路径规划结果，帮助我们理解无人机在环境中的运动轨迹。

图4-8 PSO优化得到的无人机路径

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/代码探险家/article/detail/979260

（4-5） 轨迹规划算法和优化：使用粒子群优化（PSO）对无人机路径进行规划和优化

（4-5）轨迹规划算法和优化：使用粒子群优化（PSO）对无人机路径进行规划和优化