基于鸽群算法的路径规划代码实现（直接运行，带注释）

程序说明

本代码用于鸽群算法论文的复现，在二维平面，代价只考虑躲避威胁与油耗。
所用到的参考文献：
[1] Haibin, Duan, Peixin, et al. Pigeon-inspired optimization: a new swarm intelligence optimizer for air robot
path planning[J]. International Journal of Intelligent Computing & Cybernetics, 2008.
[2] Zhu W , Duan H . Chaotic Predator-Prey Biogeography-Based Optimization Approach for UCAV Path Planning[J]. Aerospace
Science and Technology, 2013, 32(1):153-161.

备注：
[1] 第一篇参考文献是本鸽群算法用于路径规划的核心文献，第二篇文献主要用于补充第一篇文献对fitness函数没说清楚的地方；
[2] 本程序基本是按照以上两篇文献来实现的，但是因为对于原英文文献理解的原因，可能有不够准确的地方。如有不当敬请指出，谢谢！

实现代码

import numpy as np
import matplotlib.pyplot as plt
import copy


class Pigeon:
    def __init__(self, search_range_pos, search_range_v, D, num):

        pos_x = np.linspace(0, 1, D+2)
        pos_y = np.random.rand(D) * search_range_pos
        pos_y = np.insert(pos_y, 0, 0)
        pos_y = np.insert(pos_y, len(pos_y), 0)
        self.pos = np.vstack((pos_x, pos_y)).T

        v_start = np.array([0, 0])
        v_end = np.array([0, 0])
        self.v = np.vstack((v_start, np.random.rand(D, 2) * search_range_v))
        self.v = np.vstack((self.v, v_end))

        self.f = fitness_cal(self)
        self.num = num

    def update1(self, pigeon_swarm_cur, t):
        global R
        pos_pre = self.pos
        self.pos = pos_pre + self.v
        self.v = self.v * np.exp(-R * t) + np.random.rand() * (x_g_find(pigeon_swarm_cur) - pos_pre)
        self.f = fitness_cal(self)  # 这一步非常重要！任何时候对每一只鸽子的参数进行了改变，都要记得相应地修改fitness值


def normalize(start, target, threat, R, search_range_v, search_range_pos):
    # 这个函数进行地图的坐标变换和归一化，主要应用线性代数的知识

    l_line = np.linalg.norm(target - start)
    cos_theta = np.dot(target - start, np.array([1, 0])) / l_line
    sin_theta = np.dot(target - start, np.array([0, 1])) / l_line
    mat = np.array([[cos_theta, sin_theta], [-sin_theta, cos_theta]])

    start_new = np.dot(mat, start - start) / l_line
    target_new = np.dot(mat, target - start) / l_line

    start_multi = start
    for i in range(len(threat)-1):
        start_multi = np.vstack((start_multi, start))
    threat_xy_new = np.dot(mat, (threat[:, 0:2] - start_multi).T).T
    threat_new = np.hstack((threat_xy_new, threat[:, 2:]))
    for i in range(len(threat_new)):
        threat_new[i, 0:3] = threat_new[i, 0:3] / l_line

    R_new = R / l_line
    search_range_v_new = search_range_v / l_line
    search_range_pos_new = search_range_pos / l_line

    mat_trans_inverse = np.linalg.inv(mat)

    return start_new, target_new, threat_new, R_new, search_range_v_new, search_range_pos_new, mat_trans_inverse, l_line


def fitness_cal(pigeon_cur):
    # 这个函数用来计算fitness，顾名思义：fitness_calculate

    global w, threat, D

    # 第一部分：路径长度代价
    cost1 = 0
    for i in range(D+1):
        cost1 = cost1 + sum(np.square(pigeon_cur.pos[i+1] - pigeon_cur.pos[i]))

    # 第二部分：威胁代价
    cost2 = 0
    for i in range(D+1):
        cost_tem = 0
        for j in range(len(threat)):
            l_rec = np.array([])
            for k in range(5):
                pos_cur = pigeon_cur.pos[i] + (2 * k + 1) / 10 * (pigeon_cur.pos[i+1] - pigeon_cur.pos[i])
                l_rec = np.append(l_rec, np.linalg.norm(threat[j][0:2] - pos_cur))
            if min(l_rec) < threat[j][2]:  # 这里参考文献1原文用的是平均距离，但是并未给出具体定义，这里自作主张改成了最小值
                cost_tem = cost_tem + sum(1 / (l_rec ** 4))
            else:
                cost_tem = cost_tem + 0
            cost_tem = cost_tem * threat[j][3]
        cost_tem = cost_tem / 5 * np.linalg.norm(pigeon_cur.pos[i+1] - pigeon_cur.pos[i])
        cost2 = cost2 + cost_tem

    cost = np.dot(w, np.array([cost1, cost2]))
    return cost


def x_g_find(pigeon_swarm_cur):
    min_f = pigeon_swarm_cur[0].f
    best_pos = pigeon_swarm_cur[0].pos
    for pigeon_cur in pigeon_swarm_cur:
        if pigeon_cur.f < min_f:
            min_f = pigeon_cur.f
            best_pos = pigeon_cur.pos
    return best_pos  # 注意返回的是pos


def update2(pigeon_swarm_cur):
    # 这个函数用来完成鸽群算法第二部分优化

    # 这里要筛选f值位于前一半的实例，并且取这些实例输出，使用中位数np.median()函数可以方便实现
    f_rec = np.array([])
    for pigeon_cur in pigeon_swarm_cur:
        f_rec = np.append(f_rec, pigeon_cur.f)
    f_middle = np.median(f_rec)

    # 进行筛选
    swarm_better = []
    for pigeon_cur in pigeon_swarm_cur:
        if pigeon_cur.f <= f_middle:
            swarm_better.append(pigeon_cur)

    # 筛选完之后，按照算法规则进行更新
    pos_ave = np.zeros(swarm_better[0].pos.shape)
    f_rec_better = np.array([])
    for pigeon_cur in swarm_better:
        pos_ave = pos_ave + pigeon_cur.pos * pigeon_cur.f
        f_rec_better = np.append(f_rec_better, pigeon_cur.f)
    pos_ave = pos_ave / sum(f_rec_better)
    for pigeon_cur in swarm_better:
        pigeon_cur.pos = pigeon_cur.pos + np.random.rand() * (pos_ave - pigeon_cur.pos)
        pigeon_cur.f = fitness_cal(pigeon_cur)  # 这一步非常重要！任何时候对每一只鸽子的参数进行了改变，都要记得相应地修改fitness值
    return swarm_better


def result_show(x_g, threat, start, target):
    # 这个函数用来最后展示路径规划结果
    ax = plt.gca()
    ax.scatter([start[0]], [start[1]], c='b')
    ax.scatter([target[0]], [target[1]], c='b')
    for i in range(len(threat)):
        ax.scatter([threat[i][0]], [threat[i][1]], c='r')
        circle = plt.Circle((threat[i][0], threat[i][1]), threat[i][2], color='r', fill=True, alpha=0.1)
        ax.add_artist(circle)
    ax.plot(x_g[0], x_g[1], c='b')
    ax.axis('equal')
    plt.show()


if __name__ == '__main__':
    # input & initialization
    D = 10  # 路径空间的维度（简单地说：有多少个中间节点）
    R_origin = 20  # 参数R，参见参考文献1
    search_range_v_origin = 0.1  # 初始化各个节点v时的范围
    search_range_pos_origin = 20  # 初始化各个节点垂直于起点和终点连线的位置范围
    Nc1max = 20  # 鸽群算法第一部分优化的迭代次数
    Nc2max = Nc1max + 20  # 鸽群算法第二部分优化的迭代次数，但是在本程序中，实际上迭代到只剩一只鸽子的时候鸽群算法第二部分优化就已经停止了
    Np = 100  # 鸽群中个体数量
    start_origin = np.array([5, 2])  # 路径规划的起点
    target_origin = np.array([65, 95])  # 路径规划的终点
    w = np.array([0.9, 0.1])  # w[0]是对于路径长度的权重，w[1]是对于威胁侵入的权重，因为威胁往往能优化到全是0. 所以可以放大w[0]可能会平滑路径
    threat_origin = np.array([[10, 20, 10, 1], [30, 80, 5, 1], [20, 50, 6, 1], [45, 55, 8, 1],
                              [70, 70, 10, 1], [35, 20, 10, 1], [60, 85, 5, 1], [40, 70, 8, 1]])  # 平面上存在的威胁

    # 坐标变换并归一化以简化问题，具体可见参考文献2
    start, target, threat, R, search_range_v, search_range_pos, mat_trans_inverse, l_line = \
        normalize(start_origin, target_origin, threat_origin, R_origin, search_range_v_origin, search_range_pos_origin)

    # 初始化鸽群
    pigeon_swarm = []
    for i in range(Np):
        pigeon_swarm.append(Pigeon(search_range_pos, search_range_v, D, i))

    # 这里开始鸽群算法第一部分优化
    for t in range(Nc1max):
        print('number of iterations in 1st stage: %d/%d' % (t, Nc1max))
        pigeon_swarm_cur = copy.deepcopy(pigeon_swarm)
        for pigeon_cur in pigeon_swarm:
            pigeon_cur.update1(pigeon_swarm_cur, t)

    # 这里开始鸽群算法第二部分优化
    for t in range(Nc1max, Nc2max):
        print('number of pigeons in 2nd stage: %d' % len(pigeon_swarm))
        if len(pigeon_swarm) > 1:
            pigeon_swarm = update2(pigeon_swarm)
        else:
            break  # 迭代到鸽群数量等于1的时候就停止，这时候输出的就是搜索到的最好的解（表现最佳的鸽子）
    x_g = x_g_find(pigeon_swarm)  # 输出最好的解

    # 下面几行是要把x_g变换为实际地图中的形式，就是简单的旋转和放缩（线性变换）
    x_g_origin = (np.dot(mat_trans_inverse, x_g.T) * l_line).T
    start_origin_multi = start_origin
    for i in range(len(x_g_origin) - 1):
        start_origin_multi = np.vstack((start_origin_multi, start_origin))
    x_g_origin = (x_g_origin + start_origin_multi).T

    result_show(x_g_origin, threat_origin, start_origin, target_origin)  # 绘制实际地图中的路径规划情况
    result_show(x_g.T, threat, start, target)  # 绘制坐标变换并归一化之后的路径规划情况（这是程序计算情况，非实际情况）

结果示例

因为本程序结果具有随机性，以下结果只是其中可能的一种。