基于PSO的三维无人机避障航迹规划

基于粒子群优化(Particle Swarm Optimization, PSO)算法的无人机路径的避障规划。​

random: 用于生成随机数,比如初始化粒子的位置和速度。

numpy (np): 用于数学计算,如向量运算和距离计算。

matplotlib.pyplot (plt): 用于绘制图形。

mpl_toolkits.mplot3d.Axes3D 和 mpl_toolkits.mplot3d.art3d.Poly3DCollection, pathpatch_2d_to_3d: 用于在3D空间中绘制图形。

import random
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection, pathpatch_2d_to_3d

class Drone:
    def __init__(self, start_pos, target_pos):
        self.start_pos = start_pos
        self.target_pos = target_pos

class PSO:
    def __init__(self, drone, nPop, MaxIt, w, c1, c2, obstacles):
        self.drone = drone
        self.nPop = nPop
        self.MaxIt = MaxIt
        self.w = w
        self.c1 = c1
        self.c2 = c2
        self.obstacles = obstacles
        self.global_best = {'position': None, 'cost': float('inf')}
        self.particles = self.init_particles()

    def init_particles(self):
        """ 初始化粒子群 """
        particles = []
        for _ in range(self.nPop):
            position = [(random.uniform(0, 10), random.uniform(0, 10), random.uniform(0, 10)) for _ in range(10)]
            velocity = [(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) for _ in range(10)]
            cost = self.cost_function(position)
            particles.append({
                'position': position,
                'velocity': velocity,
                'cost': cost,
                'best_position': position.copy(),
                'best_cost': cost
            })
            if cost < self.global_best['cost']:
                self.global_best['position'] = position.copy()
                self.global_best['cost'] = cost
        return particles

    def cost_function(self, path):
        """ 计算成本,即路径长度和障碍物碰撞惩罚 """
        total_distance = 0
        collision_penalty = 0
        prev_pos = self.drone.start_pos
        for pos in path:
            total_distance += np.sqrt((pos[0] - prev_pos[0])**2 + (pos[1] - prev_pos[1])**2 + (pos[2] - prev_pos[2])**2)
            for obstacle in self.obstacles:
                if self.is_collision(pos, obstacle):
                    collision_penalty += 1000  # 高惩罚值以避免碰撞
            prev_pos = pos
        total_distance += np.sqrt((self.drone.target_pos[0] - prev_pos[0])**2 +
                                  (self.drone.target_pos[1] - prev_pos[1])**2 +
                                  (self.drone.target_pos[2] - prev_pos[2])**2)
        return total_distance + collision_penalty

    def is_collision(self, pos, obstacle):
        """ 检查是否与障碍物碰撞 """
        x, y, z = pos
        center_x, center_y, center_z, radius, height = obstacle
        if (x - center_x)**2 + (y - center_y)**2 <= radius**2 and center_z <= z <= center_z + height:
            return True
        return False

    def update_velocity(self, particle):
        """ 更新速度 """
        new_velocity = []
        for i in range(len(particle['position'])):
            r1, r2 = random.random(), random.random()
            cognitive = self.c1 * r1 * (np.array(particle['best_position'][i]) - np.array(particle['position'][i]))
            social = self.c2 * r2 * (np.array(self.global_best['position'][i]) - np.array(particle['position'][i]))
            new_velocity.append((self.w * particle['velocity'][i][0] + cognitive[0] + social[0],
                                self.w * particle['velocity'][i][1] + cognitive[1] + social[1],
                                self.w * particle['velocity'][i][2] + cognitive[2] + social[2]))
        return new_velocity

    def update_position(self, particle):
        """ 更新位置 """
        new_position = []
        for i in range(len(particle['position'])):
            new_x = particle['position'][i][0] + particle['velocity'][i][0]
            new_y = particle['position'][i][1] + particle['velocity'][i][1]
            new_z = particle['position'][i][2] + particle['velocity'][i][2]
            new_position.append((new_x, new_y, new_z))
        return new_position

    def run(self):
        """ 运行PSO """
        for it in range(self.MaxIt):
            for particle in self.particles:
                particle['velocity'] = self.update_velocity(particle)
                particle['position'] = self.update_position(particle)
                cost = self.cost_function(particle['position'])
                particle['cost'] = cost
                if cost < particle['best_cost']:
                    particle['best_position'] = particle['position'].copy()
                    particle['best_cost'] = cost
                    if cost < self.global_best['cost']:
                        self.global_best['position'] = particle['position'].copy()
                        self.global_best['cost'] = cost
            self.w *= 0.98  # 减小惯性权重
        return self.global_best['position'], self.global_best['cost']

def plot_3d_paths(paths, start_pos, target_pos, obstacles):
    fig = plt.figure(figsize=(10, 10))
    ax = fig.add_subplot(111, projection='3d')

    # 绘制地表
    X, Y = np.meshgrid(np.linspace(0, 10, 100), np.linspace(0, 10, 100))
    Z = np.zeros_like(X)
    ax.plot_surface(X, Y, Z, alpha=0.3, color='lightblue')

    # 绘制路径
    colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k']
    for i, (best_path, best_distance) in enumerate(paths):
        x_path = [start_pos[0]] + [pos[0] for pos in best_path] + [target_pos[0]]
        y_path = [start_pos[1]] + [pos[1] for pos in best_path] + [target_pos[1]]
        z_path = [start_pos[2]] + [pos[2] for pos in best_path] + [target_pos[2]]
        ax.plot(x_path, y_path, z_path, f'{colors[i % len(colors)]}-o', label=f'Path {i+1} (Dist: {best_distance:.2f})')

    # 绘制起始点和目标点
    ax.scatter(start_pos[0], start_pos[1], start_pos[2], label='Start', color='green')
    ax.scatter(target_pos[0], target_pos[1], target_pos[2], label='Target', color='red')

    # 绘制障碍物
    for obstacle in obstacles:
        center_x, center_y, center_z, radius, height = obstacle
        circle = plt.Circle((center_x, center_y), radius, color='gray', alpha=0.5)
        ax.add_patch(circle)
        pathpatch_2d_to_3d(circle, z=center_z, zdir="z")
        pathpatch_2d_to_3d(circle, z=center_z + height, zdir="z")

        # 绘制柱状体侧面
        for z in np.linspace(center_z, center_z + height, 10):
            circle = plt.Circle((center_x, center_y), radius, color='gray', alpha=0.5)
            pathpatch_2d_to_3d(circle, z=z, zdir="z")
            ax.add_patch(circle)

    ax.set_xlim(-1, 12)
    ax.set_ylim(-1, 12)
    ax.set_zlim(-1, 12)
    ax.set_xlabel('X Position')
    ax.set_ylabel('Y Position')
    ax.set_zlabel('Z Position')
    ax.set_title('Drone Path Planning using PSO in 3D')
    ax.legend()
    plt.show()

if __name__ == "__main__":
    start_pos = (0, 0, 0)  # 无人机起始位置
    target_pos = (10, 10, 10)  # 目标位置

    # 定义障碍物,格式为 (中心x, 中心y, 中心z, 半径, 高度)
    num_obstacles = 50  # 增加障碍物数量
    obstacles = [
        (random.uniform(1, 9), random.uniform(1, 9), 0, 0.1, 10) for _ in range(num_obstacles)
    ]

    drone = Drone(start_pos, target_pos)
    num_paths = 7  # 路径数量
    paths = []

    for i in range(num_paths):
        # 将已规划的路径作为新的障碍物
        for j in range(i):
            for pos in paths[j][0]:
                obstacles.append((pos[0], pos[1], pos[2], 0.1, 0.1))

        pso = PSO(drone, nPop=100, MaxIt=500, w=1, c1=1.5, c2=1.5, obstacles=obstacles)
        best_path, best_distance = pso.run()
        paths.append((best_path, best_distance))

    # 可视化结果
    plot_3d_paths(paths, start_pos, target_pos, obstacles)
相关推荐
网易独家音乐人Mike Zhou33 分钟前
【卡尔曼滤波】数据预测Prediction观测器的理论推导及应用 C语言、Python实现(Kalman Filter)
c语言·python·单片机·物联网·算法·嵌入式·iot
安静读书35 分钟前
Python解析视频FPS(帧率)、分辨率信息
python·opencv·音视频
小二·2 小时前
java基础面试题笔记(基础篇)
java·笔记·python
小喵要摸鱼4 小时前
Python 神经网络项目常用语法
python
一念之坤5 小时前
零基础学Python之数据结构 -- 01篇
数据结构·python
wxl7812275 小时前
如何使用本地大模型做数据分析
python·数据挖掘·数据分析·代码解释器
NoneCoder5 小时前
Python入门(12)--数据处理
开发语言·python
LKID体6 小时前
Python操作neo4j库py2neo使用(一)
python·oracle·neo4j
小尤笔记7 小时前
利用Python编写简单登录系统
开发语言·python·数据分析·python基础
FreedomLeo17 小时前
Python数据分析NumPy和pandas(四十、Python 中的建模库statsmodels 和 scikit-learn)
python·机器学习·数据分析·scikit-learn·statsmodels·numpy和pandas