多波束测深声呐海模型模拟

多波束声呐，深测、地形、鱼类、生物态势的一个基础模型实现过程。

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.interpolate import griddata

# 定义海底地形
def generate_seafloor(x_range, y_range, n_points):
    x = np.random.uniform(x_range[0], x_range[1], n_points)
    y = np.random.uniform(y_range[0], y_range[1], n_points)
    z = np.random.uniform(-100, -50, n_points)  # 海底深度
    return x, y, z

# 生成海底地形数据
x_range = (0, 300)
y_range = (0, 300)
n_points = 1000
x, y, z = generate_seafloor(x_range, y_range, n_points)

# 插值生成网格
grid_x, grid_y = np.mgrid[x_range[0]:x_range[1]:100j, y_range[0]:y_range[1]:100j]
grid_z = griddata((x, y), z, (grid_x, grid_y), method='cubic')

# 添加大鱼和小鱼
def add_fish(n_fish, x_range, y_range, z_range):
    fish_x = np.random.uniform(x_range[0], x_range[1], n_fish)
    fish_y = np.random.uniform(y_range[0], y_range[1], n_fish)
    fish_z = np.random.uniform(z_range[0], z_range[1], n_fish)
    return fish_x, fish_y, fish_z

n_large_fish = 5
n_small_fish = 20
large_fish_x, large_fish_y, large_fish_z = add_fish(n_large_fish, x_range, y_range, (-50, -30))
small_fish_x, small_fish_y, small_fish_z = add_fish(n_small_fish, x_range, y_range, (-30, -10))

# 定义声呐中心和扫描参数
center = (150, 150, 0)  # 声呐中心位置
scan_angle = 120  # 扫描的角度范围
angle_step = 5  # 每次扫描的角度步长
max_distance = 150  # 最大扫描距离
num_beams = 10  # 多波束的数量

# 模拟多波束扫描
def scan_3d(center, seafloor, fish, angle, max_distance, beam_angle):
    x1, y1, z1 = center
    distances = []
    for i in range(num_beams):
        angle_i = angle + (i - num_beams // 2) * beam_angle
        for d in np.linspace(0, max_distance, num=1000):  # 在最大距离内逐步增加
            x2 = x1 + d * np.cos(np.radians(angle_i))
            y2 = y1 + d * np.sin(np.radians(angle_i))
            z2 = griddata((seafloor[0], seafloor[1]), seafloor[2], (x2, y2), method='linear')
            if z2 is not None and z2 < z1:
                distances.append(d)
                break
            for fx, fy, fz in fish:
                if np.isclose(x2, fx, atol=1) and np.isclose(y2, fy, atol=1) and np.isclose(z2, fz, atol=1):
                    distances.append(d)
                    break
            else:
                continue
            break
        else:
            distances.append(max_distance)  # 没有遇到障碍物
    return distances

# 开始扫描
angles = np.arange(-scan_angle/2, scan_angle/2, angle_step)
beam_angle = scan_angle / num_beams

# 创建3D画布和子图表
fig = plt.figure(figsize=(15, 8))
ax3d = fig.add_subplot(121, projection='3d')
ax2d = fig.add_subplot(122)

# 绘制3D海底地形
ax3d.plot_surface(grid_x, grid_y, grid_z, cmap='Blues', alpha=0.5)

# 绘制3D大鱼和小鱼
ax3d.scatter(large_fish_x, large_fish_y, large_fish_z, color='green', s=100, label='Large Fish')
ax3d.scatter(small_fish_x, small_fish_y, small_fish_z, color='blue', s=50, label='Small Fish')

# 绘制3D声呐中心
ax3d.scatter(center[0], center[1], center[2], color='red', s=100, label='Sonar Center')

# 绘制3D扫描结果
for angle in angles:
    distances = scan_3d(center, (x, y, z), list(zip(large_fish_x, large_fish_y, large_fish_z)), angle, max_distance, beam_angle)
    for i, d in enumerate(distances):
        x2 = center[0] + d * np.cos(np.radians(angle + (i - num_beams // 2) * beam_angle))
        y2 = center[1] + d * np.sin(np.radians(angle + (i - num_beams // 2) * beam_angle))
        z2 = griddata((x, y), z, (x2, y2), method='linear')
        ax3d.plot([center[0], x2], [center[1], y2], [center[2], z2], 'r-')

# 设置3D图形属性
ax3d.set_xlabel('X (m)')
ax3d.set_ylabel('Y (m)')
ax3d.set_zlabel('Z (m)')
ax3d.set_title('3D Multibeam Sonar Scan Simulation')
ax3d.legend()
ax3d.grid(True)

# 绘制2D平面地形扫描结果
ax2d.contourf(grid_x, grid_y, grid_z, levels=20, cmap='Blues', alpha=0.5)
ax2d.scatter(large_fish_x, large_fish_y, color='green', s=100, label='Large Fish')
ax2d.scatter(small_fish_x, small_fish_y, color='blue', s=50, label='Small Fish')
ax2d.scatter(center[0], center[1], color='red', s=100, label='Sonar Center')

# 绘制2D扫描结果
for angle in angles:
    distances = scan_3d(center, (x, y, z), list(zip(large_fish_x, large_fish_y, large_fish_z)), angle, max_distance, beam_angle)
    for i, d in enumerate(distances):
        x2 = center[0] + d * np.cos(np.radians(angle + (i - num_beams // 2) * beam_angle))
        y2 = center[1] + d * np.sin(np.radians(angle + (i - num_beams // 2) * beam_angle))
        ax2d.plot([center[0], x2], [center[1], y2], 'r-')

# 设置2D图形属性
ax2d.set_xlabel('X (m)')
ax2d.set_ylabel('Y (m)')
ax2d.set_title('2D Top-Down View of Sonar Scan')
ax2d.legend()
ax2d.grid(True)

# 显示图形
plt.tight_layout()
plt.show()