auv-control

name	auv-control
description	AUV 控制技能 - 水下潜航器动力学、螺旋桨控制、深度控制、航向控制、ROS2 集成
argument-hint	AUV OR 水下控制 OR 潜航器 OR ROV OR depth control
user-invocable	true

AUV 控制技能

用于开发水下自主潜航器(AUV)的控制系统，涵盖动力学建模、螺旋桨控制、深度控制和 ROS2 集成

---

何时使用

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

• AUV/ROV 动力学建模
• 螺旋桨和推进器配置
• 深度/航向/姿态控制
• 水下导航和安全机制
• 水声通信集成

---

快速参考

AUV 配置

auv:
  mass: 300              # 质量 (kg)
  volume: 0.31           # 排水体积 (m³)
  length: 1.5            # 长度 (m)
  max_depth: 100          # 最大工作深度 (m)
  max_speed: 3.0         # 最大速度 (m/s)
  thrusters:
    count: 6             # 推进器数量
    config: "矢量布置"    # 矢量推进配置

---

动力学模型

import numpy as np
from dataclasses import dataclass


@dataclass
class AUVState:
    """AUV 状态"""
    position: np.ndarray      # [x, y, z]  世界坐标
    velocity: np.ndarray      # [u, v, w]  体坐标系速度
    orientation: np.ndarray   # [phi, theta, psi]  roll, pitch, yaw
    angular_velocity: np.ndarray  # [p, q, r]  体坐标系角速度


class AUVDynamics:
    """
    AUV 6-DOF 动力学模型

    参考: Fossen 船舶动力学
    """

    def __init__(self, params: dict):
        # 质量矩阵 (包含附加质量)
        self.M = np.diag([
            params['M_u'], params['M_v'], params['M_w'],
            params['M_p'], params['M_q'], params['M_r']
        ])

        # 科里奥利向心矩阵
        self.C = np.zeros((6, 6))

        # 水动力阻尼矩阵
        self.D = np.diag([
            params['D_u'], params['D_v'], params['D_w'],
            params['D_p'], params['D_q'], params['D_r']
        ])

        # 重力和浮力
        self.g = params['gravity']
        self.W = params['weight']      # 重力
        .B = params[]   
        .z_meta = params[]  

        
        .thruster_config = params.get(, [])

     () -> np.ndarray:
        
        nu = np.concatenate([state.velocity, state.angular_velocity])

        
        C = ._compute_C_matrix(nu)

        
        D_nu = D @ nu

        
        g_eta = ._compute_gravity_buoyancy(state.orientation)

        
        tau_thrust = ._thrust_mapping(thrust_commands)

        
        tau = tau_thrust + g_eta

        
        nu_dot = np.linalg.inv(M) @ (tau - C @ nu - D_nu)

         nu_dot

     () -> np.ndarray:
        
        u, v, w, p, q, r = nu
        C = np.zeros((, ))
        C[, ] = -.M[,] * r
        C[, ] = .M[,] * q
        
         C

     () -> np.ndarray:
        
        phi, theta, psi = orientation
        g = np.zeros()
        g[] = -(W - B) * np.cos(phi) * np.cos(theta)
        g[] = -(W - B) * .z_meta * np.sin(theta)
        g[] = (W - B) * .z_meta * np.sin(phi) * np.cos(theta)
         g

     () -> np.ndarray:
        
        tau = np.zeros()
         i, cmd  (commands):
            pos = .thruster_config[i][]
             = .thruster_config[i][]
            tau += .thruster_config[i][] * cmd * 
         tau


 :
    

     ():
        .kp, .ki, .kd = kp, ki, kd
        .prev_error = 
        .integral = 

     () -> :
        error = setpoint - actual
        .integral += error * dt
        derivative = (error - .prev_error) / dt  dt >   
        .prev_error = error
         .kp * error + .ki * .integral + .kd * derivative

---

ROS2 AUV 控制节点

#!/usr/bin/env python3
"""AUV 控制节点"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu, FluidPressure
from nav_msgs.msg import Odometry
import numpy as np


class AUVControlNode(Node):
    def __init__(self):
        super().__init__('auv_control')

        # 控制器
        self.depth_pid = PIDController(kp=2.0, ki=0.1, kd=1.0)
        self.yaw_pid = PIDController(kp=1.5, ki=0.05, kd=0.5)
        self.pitch_pid = PIDController(kp=1.0, ki=0.0, kd=0.5)

        # 状态
        self.current_depth = 0.0
        self.target_depth = 0.0
        self.current_yaw = 0.0
        self.target_yaw = 0.0
        self.vehicle_state = AUVState(
            position=np.zeros(3),
            velocity=np.zeros(3),
            orientation=np.zeros(3),
            angular_velocity=np.zeros()
        )

        
        .cmd_sub = .create_subscription(
            Twist, , .cmd_callback, )
        .imu_sub = .create_subscription(
            Imu, , .imu_callback, )
        .pressure_sub = .create_subscription(
            FluidPressure, , .pressure_callback, )

        
        .thrust_pub = .create_publisher(
            Twist, , )

        .timer = .create_timer(, .control_loop)  

     ():
        .target_depth = (, (, -msg.linear.z))  
        .target_yaw = msg.angular.z

     ():
        
        q = msg.orientation
        roll, pitch, yaw = ._quaternion_to_euler(q.x, q.y, q.z, q.w)
        .vehicle_state.orientation = np.array([roll, pitch, yaw])
        .current_yaw = yaw

     ():
        
        pressure_ Pa = msg.fluid_pressure
        .current_depth = (pressure - ) / 

     ():
        dt = 

        
        depth_cmd = .depth_pid.compute(
            .target_depth, .current_depth, dt
        )

        
        yaw_cmd = .yaw_pid.compute(
            .target_yaw, .current_yaw, dt
        )

        
        thrust = ._allocate_thrust(
            surge=,  
            sway=,   
            heave=depth_cmd,
            roll=,
            pitch=.pitch_pid.compute(, .vehicle_state.orientation[], dt),
            yaw=yaw_cmd
        )

        
        cmd = Twist()
        cmd.linear.x = thrust[]
        cmd.linear.y = thrust[]
        cmd.linear.z = thrust[]
        cmd.angular.x = thrust[]
        cmd.angular.y = thrust[]
        cmd.angular.z = thrust[]
        .thrust_pub.publish(cmd)

     ():
        
        
         np.array([
            thrust[] * ,   
            , , , , thrust[] * , thrust[]
        ])


     ():
        
        roll = np.arctan2(*(w*x + y*z),  - *(x*x + y*y))
        pitch = np.arcsin(*(w*y - z*x))
        yaw = np.arctan2(*(w*z + x*y),  - *(y*y + z*z))
         roll, pitch, yaw

---

故障排查

问题	原因	解决方案
AUV 不下潜	浮力过大	调整 ballast 或配重
航向漂移	偏航 PID 增益不对	调整 yaw_pid 参数
深度振荡	微分项不足	增加 kd，减少 kp
推进器响应慢	命令限幅	检查 thruster_manager 参数
姿态不稳	传感器噪声	添加滤波器

---

相关技能

• underwater/sonar-perception — 声呐感知
• navigation/nav2-integration — 导航集成