脚本功能概述

本项目围绕无人艇（USV）的 LOS+PID 路径跟踪，在原始 los+pid.py 的基础上，新增了基于粒子群优化（PSO）的参数自动整定能力（见 优化.py / new.py）。核心目标是通过优化 PID 增益 [Kp, Ki, Kd]，在保证跟踪精度的同时抑制控制抖动与能耗。

模块结构图（Mermaid）

flowchart TB
    A[path_waypoints.csv] --> B[simulate]
    B --> C[ShipModel]
    B --> D[LOSController]
    B --> E[PIDController]
    subgraph Optimizer
    F[BaseOptimizer]
    G[PSOOptimizer]
    F --> G
    end
    G --> H[cost_wrapper]
    H --> I[calculate_cost]
    I --> B

Loading

模块结构图（ASCII）

       +------------------- 优化.py -------------------+
       |   parse_args()                                  |
       |   run_optimization_and_validate()               |
       |       +-- PSOOptimizer.optimize()               |
       |       |       +-- cost_wrapper -> calculate_cost|
       |       |                           -> simulate   |
       |       +-- best_x -> simulate (验证)             |
       |                                                  |
       |   simulate()                                     |
       |       +-- LOSController.update()                 |
       |       +-- PIDController.control()                |
       |       +-- ShipModel.update()                     |
       +--------------------------------------------------+

功能模块说明

• Config

  • 作用：集中管理仿真与 LOS 参数（dt、look_ahead、R_switch、tau_x）
  • 输出：全局配置对象 config

• ShipModel

  • 作用：一阶阻尼 yaw-rate + 2D 位置更新；状态 (x,y,psi,r)
  • 主要方法：update(tau_x, tau_n, dt) -> (x, y, psi)；_ssa(angle) 角度规范化

• LOSController

  • 作用：几何导引，计算 psi_d/r_d，并在接近航段末端时切段
  • 主要方法：update(x, y, crab_angle=0) -> (psi_d, r_d)

• PIDController

  • 作用：根据 e_psi/e_r 计算航向力矩 tau_n，包含积分抗饱和
  • 初始化：直接传入 Kp/Ki/Kd
  • 方法：control(psi, r, psi_d, r_d, dt) -> (tau_x, tau_n)

• simulate

  • 作用：完整仿真流程（加载航点 → LOS/PID → 船模更新 → 记录/绘图）
  • 返回：历史 history = {t,x,y,psi,psi_d,rudder,cross_track}

• calculate_cost

  • 作用：在无绘图模式下运行一次仿真，返回加权代价：IAE + 0.1*|Δrudder| + 0.01*|rudder|

• BaseOptimizer / PSOOptimizer

  • 作用：优化器接口与原生PSO实现；optimize(cost_fn) 返回最优参数与适应度历史

• run_optimization_and_validate

  • 作用：阶段化流程（优化 + 验证 + 可选收敛曲线）；返回最优参数与适应度历史

依赖项清单

• Python ≥ 3.9
• numpy ≥ 1.20（数组与数值计算）
• matplotlib ≥ 3.5（绘图与可视化）
• 标准库：dataclasses、typing、argparse

使用示例

1. 运行PSO并完成验证（禁用绘图）：

python 优化.py --iters 30 --swarm 20 --sim-opt-time 60 --sim-time 120 --no-plot

2. 正常绘图展示：

python 优化.py --iters 30 --swarm 20 --sim-opt-time 60 --sim-time 120

3. 生成新旧参数对比与报告（可选脚本）：

python los_pso_compare.py --sim-time 120 --swarm 20 --iters 30

预期输出

• 终端打印：PSO优化完成：最佳参数 [Kp, Ki, Kd] = [...], 最优Cost = ...
• 图像：轨迹/航向/控制/误差四图（simulate）、适应度收敛曲线（run_optimization_and_validate）
• 可选：los_compare.png 与 performance_report.txt

注意事项

• 运行环境为桌面 Python；Windows 下需保证字体配置支持中文（脚本已设置 SimHei）。
• 需在当前目录提供 path_waypoints.csv（两列：x,y）。
• PSOOptimizer 当前将 swarm_size 固定为 100（代码中覆盖传入值），如需按命令行生效，请改为 self.swarm_size = swarm_size。
• 代价函数权重可根据任务侧重调整；若希望同时压低航向误差，可在 calculate_cost 中加入航向误差项并增大其权重。

UML 类图（Mermaid）

classDiagram
    class Config
    class ShipModel{
        +float x
        +float y
        +float psi
        +float r
        +update(tau_x:float, tau_n:float, dt:float) Tuple
    }
    class LOSController{
        +update(x:float, y:float, crab_angle:float) Tuple
    }
    class PIDController{
        +control(psi:float, r:float, psi_d:float, r_d:float, dt:float) Tuple
    }
    class BaseOptimizer{
        +optimize(cost_fn:Callable) Tuple
    }
    class PSOOptimizer{
        +optimize(cost_fn:Callable) Tuple
    }
    class Functions{
        +simulate(...)
        +calculate_cost(...)
        +run_optimization_and_validate(...)
        +parse_args()
    }
    PSOOptimizer --|> BaseOptimizer
    Functions --> ShipModel
    Functions --> LOSController
    Functions --> PIDController

Loading