多推进器航行体作为水下作业的核心装备，其高精度动力定位能力直接决定作业效率与任务安全性。传统数值仿真方法因模型简化、干扰模拟理想化等问题，难以精准验证控制算法与推力分配策略的工程实用性。基于 LabVIEW 图形化编程平台，构建了一套集多源感知、实时控制、模块化扩展于一体的多推进器航行体动力定位控制系统。该系统融合全站仪与惯性导航的组合测量方案，采用 PID 控制算法与二次规划推力分配算法，通过上下位机协同架构实现航行体多自由度运动的精准调控。

原理架构

1. 动力定位控制逻辑闭环

系统以 “感知 - 决策 - 执行” 为核心闭环，通过多源传感器融合获取航行体状态，经控制算法解算后驱动多推进器协同工作，实现高精度定位与姿态稳定。

• 感知层：采用全站仪 + 惯性导航组合导航方案，全站仪提供北东天坐标系下的绝对位置数据（精度 ±0.01m），惯性导航系统输出航向、横倾、纵倾等姿态参数（更新频率 100Hz），两者互补消除单一传感器的漂移与延迟问题。

• 决策层：基于 PID 控制算法计算三方向力（纵向、横向、垂向）和三方向力矩（横摇、纵摇、艏摇），再通过二次规划推力分配算法，将总控制力 / 力矩合理分配至 10 个推进器，确保推进系统输出最优且无耦合干涉。

• 执行层：通过 PWM 脉宽调制技术控制推进器转速，实现推力连续可调，正向最大推力 50N、反向 30N，满足多自由度运动的动力需求。

2. 关键算法原理

（1）PID 控制算法

采用增量式 PID 控制策略，忽略多自由度耦合影响，独立控制各姿态与位置参数，算法公式如下：

F=kpe(t)+ki∫0te(t)dt+kddtde(t)

• 其中 e(t)为目标值与实际测量值的偏差，kp=5.2、ki=0.08、kd=1.3（经试验优化的最优参数），通过比例项快速响应偏差、积分项消除静差、微分项抑制超调。

• 针对航向控制超调问题，引入分段积分策略：当偏差绝对值大于 5° 时，积分项失效；偏差小于 1° 时，积分增益减半，有效降低稳态波动。

（2）二次规划推力分配算法

考虑推进器物理约束（最大推力、安装角度），以 “总推力消耗最小” 为优化目标，建立推力分配数学模型：

minJ=∑i=110Fi2

s.t.TF=τcmd,Fi,min≤Fi≤Fi,max

• 式中 T为推进器配置矩阵（10×6 维，包含每个推进器在体坐标系下的力臂与方向参数），τcmd为 PID 输出的总控制力 / 力矩向量，Fi为单个推进器推力。

• 利用 LabVIEW 自带的 “带约束非线性最优化控件” 求解，求解周期≤10ms，确保推力分配实时性，避免推进器过载或动作冲突。

（3）坐标转换机制

• 棱镜位置→重心位置转换：因全站仪测量的是碳纤维杆顶端棱镜位置（立杆长度 0.8m），需通过姿态参数修正至航行体重心，公式如下：

pNpEH=pLNpLEHL−(L0+HG)cos(θ)sin(φ)cos(φ)sin(θ)cos(φ)cos(θ)

• 北东坐标系→体坐标系转换：为适配 PID 算法输入，将绝对位置转换为相对目标位置的体坐标系分量：

[xy]=[cos(ψ)−sin(ψ)sin(ψ)cos(ψ)][pNpE]

• 式中 θ为纵倾角、φ为横倾角、ψ为航向角，坐标转换误差≤0.02m，确保控制精度。

系统设计

1. 硬件选型与接口设计

2. LabVIEW 软件模块化设计

（1）下位机程序设计

• 数据采集模块：采用 “串口中断 + 缓冲区” 机制，每 10ms 读取一次惯导数据，解析报文后通过网络变量上传至上位机，数据丢包率≤0.1%。

• PWM 控制模块：接收上位机下发的推力指令，通过公式 Cpwm=Fcmd/Fmax×40+150转换为寄存器值（范围 110~190），10 路 PWM 信号同步输出，相位差≤1μs。

• 自启动配置：通过 LabVIEW Real-Time 模块设置启动项，下位机上电后自动加载程序，无需人工干预，提升系统可靠性。

（2）上位机程序设计

• 运动参数显示模块：利用 LabVIEW 二维图像控件构建航向标尺（精度 0.5°）和三维航行体模型，实时刷新位置（N/E/H）、姿态（ψ/θ/φ）数据，刷新频率 20Hz，直观呈现运动状态。

• 参数设置模块：分为水平面、垂直面、推进器三类参数区，支持 PID 系数、目标位置、推力限制等参数在线修改，修改后实时生效，无需重启系统。

• 手动测试模块：提供力 / 力矩手动输入界面，可单独控制单个或多个推进器动作，用于推力分配逻辑验证和故障排查（如推进器堵转检测）。

• 数据记录模块：自动存储试验数据（位置、姿态、推力值），格式为 TDMS，支持离线导出至 Excel，采样间隔可设置（10ms~1s）。

（3）通讯协议设计

• 上下位机采用 “TCP/IP 网络变量” 通讯，设置通讯超时时间 500ms，超时后自动重连，确保通讯稳定性。

• 数据帧格式：采用 “帧头（0xAA）+ 数据长度 + 数据段 + 校验和（CRC16）+ 帧尾（0x55）”，避免数据传输错误，校验通过率 100%。

3. 关键工程优化细节

• 抗干扰设计：下位机串口采用光电隔离模块，通讯线采用屏蔽电缆，降低水下电磁干扰对 PWM 信号的影响，推力输出波动≤±0.5N。

• 故障保护机制：设置推进器电流监测（阈值 10A）和位置超量程保护（偏离目标位置 ±2m），触发保护后自动切断推进器电源，并在上位机弹窗报警。

• 模块化替换设计：测量模块、控制算法模块均采用 “类封装” 形式，更换传感器（如激光雷达替代全站仪）或算法（如模型预测控制替代 PID）时，仅需修改对应类文件，无需改动整体程序架构。

试验验证

1. 试验条件

• 试验环境：50m×30m×5m 试验水池，模拟平静海况（水流速度≤0.1m/s）。

• 测试参数：目标北向位置 - 13m、东向位置 - 12m、垂向位置 - 3.8m，航向角 193°，横倾 / 纵倾 0°。

2. 核心性能指标

3. 典型问题与优化方案

• 问题 1：航向控制超调量达 8°，导致位置稳态波动。

优化：引入动态 PID 参数，超调阶段降低kp至 3.5，稳态阶段恢复 5.2，超调量降至 3° 以内。

• 问题 2：垂向位置存在 0.15m 稳态偏差。

优化：在 PID 积分项中加入浮力补偿系数（0.02N/s），修正推进器推力输出，偏差降至 0.08m。

• 问题 3：多推进器同步性差，动作延迟不一致。

优化：采用 “硬件触发同步” 机制，下位机通过 GPIO 口同步启动 10 路 PWM 输出，延迟差≤0.5μs。