sonar-perception

name	sonar-perception
description	声呐感知技能 - 前视声呐、侧扫声呐、多波束声呐、目标检测、SLAM、ROS2 集成
argument-hint	声呐 OR sonar OR 水下感知 OR 前视声呐 OR 多波束 OR AUV
user-invocable	true

声呐感知技能

用于开发水下机器人的声呐感知系统，涵盖前视声呐(Forward-Looking sonar)、侧扫声呐(Side-Scan)、多波束声呐、目标检测和 ROS2 集成

---

何时使用

当需要以下帮助时使用此技能：

• 配置和标定前视声呐
• 声呐图像处理和目标检测
• 声呐 SLAM 和地图构建
• 水下定位和障碍规避
• 声呐-视觉融合

---

快速参考

声呐类型

声呐类型:
├── 前视声呐 (FLS/sonar) → 主动发射，扇形扫描，2D/3D成像
├── 侧扫声呐 (SSS) → 拖曳式，条带图像，地形测绘
├── 多波束测深声呐 (MBES) → 3D点云，高精度地形
└── 声学调制解调器 (Modem) → 通信用，非成像

声呐参数

sonar:
  frequency: 675 kHz        # 工作频率
  range: 50 m              # 最大探测距离
  resolution: 0.01 m      # 距离分辨率
  field_of_view: 120°      # 水平波束宽度
  ping_rate: 30 Hz         # 扫描频率
  beam_count: 512          # 波束数量

ROS2 依赖

sudo apt install -y ros-humble-pointcloud-to-laserscan

---

前视声呐 (Forward-Looking Sonar)

工作原理

发射器 → 声波 → 障碍物 → 反射 → 接收器
         ↓
      扇形扫描区域 (120° x 20°)

声呐图像表示

import numpy as np
from dataclasses import dataclass
from typing import List, Tuple


@dataclass
class SonarBeam:
    """单个声呐波束"""
    angle: float          # 波束角度 (rad)
    ranges: np.ndarray    # 每个采样点的距离
    intensities: np.ndarray  # 回波强度 (dB)
    timestamp: float      # 时间戳


class SonarImage:
    """声呐图像"""

    def __init__(
        self,
        beams: List[SonarBeam],
        sonar_pose: np.ndarray,  # 声呐位置姿态
        range_max: float,
        beam_count: int
    ):
        self.beams = beams
        self.sonar_pose = sonar_pose
        self.range_max = range_max
        self.beam_count = beam_count

        # 极坐标 → 直角坐标 图像
        self.cartesian_image = self._polar_to_cartesian()

    def _polar_to_cartesian(self, resolution: float = 0.01) -> np.ndarray:
        """将极坐标声呐图像转换为笛卡尔坐标图像"""
        # 图像尺寸
        img_size = int(2 * self.range_max / resolution)
        image = np.zeros((img_size, img_size), dtype=np.float32)

         beam  .beams:
            angle = beam.angle
             r, intensity  (beam.ranges, beam.intensities):
                 r < .range_max  r > :
                    x = (r * np.cos(angle) / resolution) + img_size // 
                    y = (r * np.sin(angle) / resolution) + img_size // 
                      <= x < img_size   <= y < img_size:
                        image[y, x] = intensity

         image


 :
    

     ():
        .range_max = range_max
        .beam_count = beam_count
        .fov = fov
        .frequency = frequency

        
        .sound_speed = 

        
        .range_resolution = .sound_speed / ( * .frequency)

     () -> SonarImage:
        
        beams = []
        angle_step = .fov / .beam_count
        start_angle = -.fov / 

         i  (.beam_count):
            angle = start_angle + i * angle_step
            ranges = []
            intensities = []

            
            direction = np.array([
                np.cos(angle),
                np.sin(angle),
                
            ])

            
             obs_pos, obs_radius  obstacles:
                
                rel_pos = obs_pos - robot_pose[:]

                
                projection = np.dot(rel_pos, direction)

                 projection < :
                      

                
                perp_dist = np.linalg.norm(
                    rel_pos - projection * direction
                )

                 perp_dist > obs_radius:
                      

                
                hit_dist = projection - np.sqrt(obs_radius** - perp_dist**)

                  < hit_dist < .range_max:
                    
                    intensity =  * np.log10(obs_radius / hit_dist + )
                    intensity = (, (, intensity + ))

                    ranges.append(hit_dist)
                    intensities.append(intensity)

              ranges:
                ranges.append(.range_max)
                intensities.append()

            beams.append(SonarBeam(
                angle=angle,
                ranges=np.array(ranges),
                intensities=np.array(intensities),
                timestamp=
            ))

         SonarImage(beams, robot_pose, .range_max, .beam_count)

---

声呐图像处理

目标检测

import numpy as np
import cv2
from scipy import ndimage


class SonarDetector:
    """声呐目标检测"""

    def __init__(self, min_blob_size: int = 5):
        self.min_blob_size = min_blob_size

    def detect(
        self,
        sonar_image: np.ndarray,
        threshold: float = 100.0
    ) -> List[Tuple[float, float, float]]:
        """
        检测声呐图像中的目标

        Returns:
            [(x, y, radius), ...] 目标位置和大小
        """
        # 阈值化
        binary = (sonar_image > threshold).astype(np.uint8)

        # 连通域分析
        labeled, num_features = ndimage.label(binary)
        centers = ndimage.center_of_mass(
            sonar_image, labeled, range(1, num_features + 1)
        )

        # 计算大小
        targets = []
        for i, center in enumerate(centers):
            y, x = int(center[0]), int(center[1])

            # 找目标边界
            blob_mask = labeled == (i + 1)
            ys, xs = np.where(blob_mask)
            radius = max(
                (xs.max() - xs.min()) / 2,
                (ys.max() - ys.()) / 
            )

             radius >= .min_blob_size:
                targets.append((
                    (x - sonar_image.shape[] // ) * ,  
                    (y - sonar_image.shape[] // ) * ,
                    radius * 
                ))

         targets


 :
    


     () -> np.ndarray:
        
         cv2.medianBlur(image.astype(np.uint8), kernel_size)


     () -> np.ndarray:
        
         cv2.bilateralFilter(image.astype(np.uint8), d, sigma_color, sigma_space)


     () -> np.ndarray:
        
         cv2.adaptiveThreshold(
            image.astype(np.uint8), ,
            cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
            cv2.THRESH_BINARY,
            block_size, c
        )


     () -> np.ndarray:
        
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (kernel_size, kernel_size))
         cv2.morphologyEx(image, cv2.MORPH_CLOSE, kernel)

---

声呐 SLAM

import numpy as np
from typing import List, Tuple, Optional


class SonarICP:
    """
    声呐点云 ICP 匹配

    用于声呐 SLAM 的扫描匹配
    """

    def __init__(self, max_iterations: int = 50):
        self.max_iterations = max_iterations
        self.max_distance = 0.5  # 匹配阈值 (m)

    def align(
        self,
        source: np.ndarray,
        target: np.ndarray,
        initial_transform: np.ndarray = np.eye(3)
    ) -> Tuple[np.ndarray, float]:
        """
        ICP 对齐

        Args:
            source: 源点云 Nx2
            target: 目标点云 Mx2
            initial_transform: 初始变换矩阵 3x3

        Returns:
            (transform, fitness_score)
        """
        T = initial_transform.copy()
        prev_error = float('inf')

        for iteration in range(self.max_iterations):
            # 步骤1: 用当前变换变换源点云
            source_transformed = (T[:2, :2] @ source.T).T + T[:2, 2]

            # 步骤2: 找最近邻
            indices = self._nearest_neighbor(source_transformed, target)

            # 步骤3: 过滤远距离匹配
            distances = np.linalg.norm(source_transformed - target[indices], axis=1)
            valid = distances < .max_distance

             np.(valid) < :
                  

            
            source_valid = source_transformed[valid]
            target_valid = target[indices[valid]]

            centroid_s = np.mean(source_valid, axis=)
            centroid_t = np.mean(target_valid, axis=)

            ss = (source_valid - centroid_s).T @ (target_valid - centroid_t)
            U, _, Vt = np.linalg.svd(ss)
            R = Vt.T @ U.T

             np.linalg.det(R) < :
                Vt[-, :] *= -
                R = Vt.T @ U.T

            t = centroid_t - R @ centroid_s

            
            T_new = np.eye()
            T_new[:, :] = R
            T_new[:, ] = t

            
            error = np.mean(distances[valid])

             error < prev_error:
                T = T_new
                prev_error = error
            :
                  

        
        source_transformed = (T[:, :] @ source.T).T + T[:, ]
        distances = np.linalg.norm(source_transformed - target[indices], axis=)
        fitness = np.mean(distances[distances < .max_distance])

         T, fitness


     () -> np.ndarray:
        
         scipy.spatial  KDTree
        tree = KDTree(target)
        distances, indices = tree.query(source)
         indices


 :
    

     ():
        .scans = []  
        .poses = []  
        .map_resolution =   
        .map_size = (, )  

        
        .occupancy = np.zeros(.map_size, dtype=np.float32)
        .occupancy_prob =   

     ():
        
          .poses:
            
            pose = np.array([.map_size[] // , .map_size[] // , ])
            .poses.append(pose)
            .scans.append(scan)
            

        
        last_pose = .poses[-]
        new_pose = ._apply_odometry(last_pose, odometry)
        .poses.append(new_pose)

        
        icp = SonarICP()
        T, fitness = icp.align(scan, .scans[-])

         fitness < :
            
            correction = ._transform_to_correction(T)
            new_pose = ._apply_correction(new_pose, correction)

        
        ._update_map(scan, new_pose)

        .scans.append(scan)

     () -> np.ndarray:
        
        x, y, theta = pose
        dx, dy, dtheta = odom
        new_x = x + dx * np.cos(theta) - dy * np.sin(theta)
        new_y = y + dx * np.sin(theta) + dy * np.cos(theta)
        new_theta = theta + dtheta
         np.array([new_x, new_y, new_theta])

     () -> np.ndarray:
        
         np.array([T[, ], T[, ], np.arctan2(T[, ], T[, ])])

     () -> np.ndarray:
         pose + correction *   

     ():
        
        x, y, theta = pose
        cx, cy = (x / .map_resolution), (y / .map_resolution)

         point  scan:
            
            gx = cx + (point[] / .map_resolution)
            gy = cy + (point[] / .map_resolution)

              <= gx < .map_size[]   <= gy < .map_size[]:
                
                .occupancy[gy, gx] += np.log(.occupancy_prob / ( - .occupancy_prob))

---

ROS2 声呐节点

#!/usr/bin/env python3
"""声呐感知节点"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2
from geometry_msgs.msg import PoseArray, PoseStamped
from nav_msgs.msg import Odometry
import numpy as np
from dataclasses import dataclass


@dataclass
class SonarConfig:
    range_max: float = 50.0
    beam_count: int = 512
    fov: float = 2.094  # 120 度
    frequency: float = 675e3
    frame_id: str = 'sonar_link'


class SonarPerceptionNode(Node):
    def __init__(self):
        super().__init__('sonar_perception')

        self.config = SonarConfig()

        # 检测器
        self.detector = SonarDetector()
        self.slam = SonarSLAM()

        # 状态
        self.last_odom = None
        self.scan_count = 0

        # 订阅
        self.odom_sub = .create_subscription(
            Odometry,
            ,
            .odom_callback,
            
        )

        
        .detection_pub = .create_publisher(
            PoseArray,
            ,
            
        )
        .map_pub = .create_publisher(
            Image,
            ,
            
        )

        .get_logger().info()

     ():
        
        
        
        current_odom = np.array([
            msg.pose.pose.position.x,
            msg.pose.pose.position.y,
            
        ])

         .last_odom   :
            odom_delta = current_odom - .last_odom

            
            scan = ._simulate_scan(current_odom)

            
            .slam.add_scan(scan, odom_delta)

            
            targets = .detector.detect(.slam.occupancy)

            
            ._publish_detections(targets)

        .last_odom = current_odom

     () -> np.ndarray:
        
        
         np.zeros((, ))

     ():
        
        msg = PoseArray()
        msg.header.stamp = .get_clock().now().to_msg()
        msg.header.frame_id = 

         x, y, r  targets:
            pose = PoseStamped()
            pose.pose.position.x = x
            pose.pose.position.y = y
            msg.poses.append(pose.pose)

        .detection_pub.publish(msg)


 ():
    rclpy.init(args=args)
    node = SonarPerceptionNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


 __name__ == :
    main()

---

故障排查

问题	原因	解决方案
声呐图像噪声大	水体散射/气泡	增加滤波，使用自适应阈值
目标检测漏检	阈值过高	降低检测阈值，添加形态学处理
SLAM 漂移大	ICP 匹配失败	减小scan_period，增加特征密度
声呐数据丢失	硬件连接问题	检查 Ethernet/USB 连接
图像畸变	坐标系配置错误	验证 frame_id 和 TF 变换

调试命令

# 查看声呐话题
ros2 topic list | grep sonar

# 监听声呐数据
ros2 topic echo /sonar/image

# 查看声呐可视化
ros2 run rqt_image_view rqt_image_view /sonar/image:=/sonar/rendered

# 录制声呐数据
ros2 bag record /sonar/detections /odom -o sonar_data

---

相关技能

• underwater/auv-control — AUV 控制
• perception/sensor-fusion/lidar-camera-fusion — 传感器融合
• navigation/slam — SLAM 算法
• perception/sensor-fusion/multi-object-tracking — 多目标跟踪