三维路径规划粒子群优化算法实现

import numpy as np
import matplotlib.pyplot as plt

def defMap(mapRange):
    x = np.linspace(0, mapRange[0], 100)
    y = np.linspace(0, mapRange[1], 100)
    X, Y = np.meshgrid(x, y)
    Z = np.sin(X) + np.cos(Y)
    return X, Y, Z

def plotFigure(startPos, goalPos, X, Y, Z, GlobalBest):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(startPos[0], startPos[1], startPos[2], c='y', marker='s', s=100)
    ax.scatter(goalPos[0], goalPos[1], goalPos[2], c='y', marker='p', s=100)
    ax.plot_surface(X, Y, Z, cmap='viridis', edgecolor='none')
    ax.plot(GlobalBest['path'][:, 0], GlobalBest['path'][:, 1], GlobalBest['path'][:, 2], 'r', linewidth=2)
    plt.xlabel('X')
    plt.ylabel('Y')
    ax.set_zlabel('Z')
    plt.show()

def calFitness(startPos, goalPos, X, Y, Z, pos):
    x_seq = [startPos[0], pos['x'], goalPos[0]]
    y_seq = [startPos[1], pos['y'], goalPos[1]]
    z_seq = [startPos[2], pos['z'], goalPos[2]]

    k = len(x_seq)
    i_seq = np.linspace(0, 1, k)
    I_seq = np.linspace(0, 1, 100)
    X_seq = np.interp(I_seq, i_seq, x_seq)
    Y_seq = np.interp(I_seq, i_seq, y_seq)
    Z_seq = np.interp(I_seq, i_seq, z_seq)
    path = np.column_stack((X_seq, Y_seq, Z_seq))

    flag = 0
    for i in range(1, path.shape[0]):
        x = path[i, 0]
        y = path[i, 1]
        z_interp = np.interp([x], X.ravel(), Y.ravel(), Z.ravel())[0]
        if path[i, 2] < z_interp:
            flag = 1
            break

    dx = np.diff(X_seq)
    dy = np.diff(Y_seq)
    dz = np.diff(Z_seq)
    fitness = np.sum(np.sqrt(dx**2 + dy**2 + dz**2))

    return flag, fitness, path

startPos = [1, 1, 1]
goalPos = [100, 100, 80]
mapRange = [100, 100, 100]
X, Y, Z = defMap(mapRange)

N = 100
M = 50
pointNum = 3
w = 1.2
c1 = 2
c2 = 2

posBound = np.array([[0, 0, 0], mapRange])
alpha = 0.1
velBound = np.zeros((3, 2))
velBound[:, 1] = alpha * (posBound[:, 1] - posBound[:, 0])
velBound[:, 0] = -velBound[:, 1]

particles = np.zeros(M, dtype=[
    ('pos', [('x', float), ('y', float), ('z', float)]),
    ('v', [('x', float), ('y', float), ('z', float)]),
    ('fitness', float),
    ('path', float, (100, 3)),
    ('Best', [('pos', [('x', float), ('y', float), ('z', float)]), ('fitness', float), ('path', float, (100, 3))])
])

GlobalBest = {'fitness': np.inf}

for i in range(M):
    particles[i]['pos']['x'] = np.random.uniform(posBound[0, 0], posBound[0, 1], pointNum)
    particles[i]['pos']['y'] = np.random.uniform(posBound[1, 0], posBound[1, 1], pointNum)
    particles[i]['pos']['z'] = np.random.uniform(posBound[2, 0], posBound[2, 1], pointNum)

    particles[i]['v']['x'] = np.zeros(pointNum)
    particles[i]['v']['y'] = np.zeros(pointNum)
    particles[i]['v']['z'] = np.zeros(pointNum)

    flag, fitness, path = calFitness(startPos, goalPos, X, Y, Z, particles[i]['pos'])

    if flag == 1:
        particles[i]['fitness'] = 1000 * fitness
        particles[i]['path'] = path
    else:
        particles[i]['fitness'] = fitness
        particles[i]['path'] = path

    particles[i]['Best']['pos'] = particles[i]['pos'].copy()
    particles[i]['Best']['fitness'] = particles[i]['fitness']
    particles[i]['Best']['path'] = particles[i]['path'].copy()

    if particles[i]['Best']['fitness'] < GlobalBest['fitness']:
        GlobalBest = particles[i]['Best'].copy()

fitness_beat_iters = np.zeros(N)

for iter in range(N):
    for i in range(M):
        particles[i]['v']['x'] = w * particles[i]['v']['x'] \
            + c1 * np.random.rand(pointNum) * (particles[i]['Best']['pos']['x'] - particles[i]['pos']['x']) \
            + c2 * np.random.rand(pointNum) * (GlobalBest['pos']['x'] - particles[i]['pos']['x'])
        particles[i]['v']['y'] = w * particles[i]['v']['y'] \
            + c1 * np.random.rand(pointNum) * (particles[i]['Best']['pos']['y'] - particles[i]['pos']['y']) \
            + c2 * np.random.rand(pointNum) * (GlobalBest['pos']['y'] - particles[i]['pos']['y'])
        particles[i]['v']['z'] = w * particles[i]['v']['z'] \
            + c1 * np.random.rand(pointNum) * (particles[i]['Best']['pos']['z'] - particles[i]['pos']['z']) \
            + c2 * np.random.rand(pointNum) * (GlobalBest['pos']['z'] - particles[i]['pos']['z'])

        particles[i]['v']['x'] = np.minimum(particles[i]['v']['x'], velBound[0, 1])
        particles[i]['v']['x'] = np.maximum(particles[i]['v']['x'], velBound[0, 0])
        particles[i]['v']['y'] = np.minimum(particles[i]['v']['y'], velBound[1, 1])
        particles[i]['v']['y'] = np.maximum(particles[i]['v']['y'], velBound[1, 0])
        particles[i]['v']['z'] = np.minimum(particles[i]['v']['z'], velBound[2, 1])
        particles[i]['v']['z'] = np.maximum(particles[i]['v']['z'], velBound[2, 0])

        particles[i]['pos']['x'] += particles[i]['v']['x']
        particles[i]['pos']['y'] += particles[i]['v']['y']
        particles[i]['pos']['z'] += particles[i]['v']['z']

        particles[i]['pos']['x'] = np.maximum(particles[i]['pos']['x'], posBound[0, 0])
        particles[i]['pos']['x'] = np.minimum(particles[i]['pos']['x'], posBound[0, 1])
        particles[i]['pos']['y'] = np.maximum(particles[i]['pos']['y'], posBound[1, 0])
        particles[i]['pos']['y'] = np.minimum(particles[i]['pos']['y'], posBound[1, 1])
        particles[i]['pos']['z'] = np.maximum(particles[i]['pos']['z'], posBound[2, 0])
        particles[i]['pos']['z'] = np.minimum(particles[i]['pos']['z'], posBound[2, 1])

        flag, fitness, path = calFitness(startPos, goalPos, X, Y, Z, particles[i]['pos'])

        if flag == 1:
            particles[i]['fitness'] = 1000 * fitness
            particles[i]['path'] = path
        else:
            particles[i]['fitness'] = fitness
            particles[i]['path'] = path

        if particles[i]['fitness'] < particles[i]['Best']['fitness']:
            particles[i]['Best']['pos'] = particles[i]['pos'].copy()
            particles[i]['Best']['fitness'] = particles[i]['fitness']
            particles[i]['Best']['path'] = particles[i]['path'].copy()

            if particles[i]['Best']['fitness'] < GlobalBest['fitness']:
                GlobalBest = particles[i]['Best'].copy()

    fitness_beat_iters[iter] = GlobalBest['fitness']

    print('第{}代: 最优适应度 = {}'.format(iter, fitness_beat_iters[iter]))

    plotFigure(startPos, goalPos, X, Y, Z, GlobalBest)
    plt.pause(0.001)

fitness_best = np.linalg.norm(startPos - goalPos)
print('理论最优适应度 =', fitness_best)

plt.figure()
plt.plot(fitness_beat_iters, linewidth=2)
plt.xlabel('迭代次数')
plt.ylabel('最优适应度')
plt.show()