2. 多无人系统运动学与动力学建模

碰撞避免算法要判断“会不会撞”，首先必须能准确回答“谁在哪里、朝哪个方向、以多快速度运动”。这依赖统一的位姿描述与运动学/动力学模型。无人机、无人车、无人船的运动自由度、约束形式和控制输入差异很大，若各自用一套坐标与状态定义，后续的间隔计算、冲突检测和协同避碰就无从谈起。本节先建立位姿描述的共同语言，再分别梳理三类平台的运动约束，最后给出异构系统统一建模的落地方法。

一、位姿描述：从坐标系到状态向量

位姿（pose）包含位置与姿态两部分。工程上普遍采用“世界坐标系—机体坐标系”两级描述：世界系 W 固定于地面或水面，用于统一表达所有平台的位置；机体系 B 固连于平台本体，原点通常取质心或几何中心，用于描述平台自身的运动。位置用三维向量 p = [x, y, z]^T 表示，姿态用旋转矩阵 R ∈ SO(3) 或四元数 q 表示，二者满足 R = R(q)。

姿态角采用 Z-Y-X 欧拉角约定：先绕 z 轴转偏航角 ψ，再绕新 y 轴转俯仰角 θ，最后绕新 x 轴转横滚角 φ。这一约定必须在全文和所有图示中保持一致，否则速度投影和间隔方向会出现符号错误。对于水面无人船，通常只关心偏航角 ψ，可视为平面运动特例。

完整状态向量一般写成 η = [p^T, Θ^T]^T（位姿）与 ν = [v^T, ω^T]^T（机体系线速度与角速度）。世界系速度与机体系速度通过旋转矩阵联系：ṗ = R·v，Θ̇ = T(Θ)·ω，其中 T(Θ) 是欧拉角速率到机体角速度的转换矩阵。这一步是后续所有运动学推导的基础。

二、三类平台的运动约束差异

无人机（多旋翼）具有 6 自由度，但控制输入只有 4 个：总推力 f 与三轴力矩 τ = [τ_x, τ_y, τ_z]^T。其平动由推力方向决定，必须倾斜机身才能产生水平加速度，因此存在“位置—姿态耦合”约束：想往某方向加速，就必须先改变姿态。多旋翼不能长时间倒飞，俯仰/横滚角通常限制在 ±30°～±45°。

无人车（阿克曼转向）是非完整约束系统。后轴中心速度沿车体纵轴，横向速度近似为零，即 ẋ·sinψ − ẏ·cosψ = 0。其最小转弯半径 R_min = L / tan(δ_max)，其中 L 为轴距，δ_max 为最大前轮转角。这意味着无人车不能原地横向平移，路径规划必须满足曲率连续。

无人船（差速或舵桨）在水面运动，主要约束来自流体阻力与舵效。其运动学与差速车类似，但存在侧滑和漂移，横向速度不可忽略，通常用三自由度模型 [u, v, r]（纵荡、横荡、艏摇）描述。低速时舵效弱，转向响应滞后明显，这是与无人车最大的区别。

三、异构系统统一建模方法

统一建模的核心思路是“状态统一、约束分离、接口标准化”。具体分三步：

1. 状态统一：所有平台统一用 ξ = [x, y, z, ψ, v, ω]^T 描述。无人机保留 z 与 θ, φ，无人车和无人船将 z 固定为 0、θ, φ 置零，仅保留平面分量。这样上层避碰算法只需处理统一维度。
2. 约束分离：把各平台的运动约束写成通用形式 ξ̇ = f(ξ, u)，其中 u 为控制输入。无人机 u = [f, τ]，无人车 u = [a, δ]（加速度与前轮转角），无人船 u = [n, δ_r]（转速与舵角）。约束矩阵 A(ξ) 按平台类型分别定义，但对外接口一致。
3. 接口标准化：定义统一的“运动状态发布接口”，输出 {id, type, p, ψ, v, ω, timestamp}。避碰模块只订阅该接口，不关心底层是哪种平台。时间戳用于多机同步，避免因采样不同步导致虚假冲突。

下面用一张结构图展示三类平台从各自模型到统一状态接口的映射关系，以及约束矩阵在其中的位置。

四、建模落地要点与示例参数

实际实现时，建议用参数表把各平台的关键几何与动力学参数集中管理，避免散落在代码各处。下表给出一组教学示例参数（非实测值，仅用于说明建模流程）：

以无人车为例，若轴距 L = 2.7 m、最大前轮转角 δ_max = 30°，则 R_min = 2.7 / tan(30°) ≈ 4.68 m。在避碰规划中，若两车最小间隔要求为 3 m，则任何避让轨迹的曲率半径不得小于 4.68 m，否则车辆无法跟踪。这个约束必须提前写入规划器的可行域，而不是等轨迹生成后再检查。

最后强调时间同步。三类平台的传感器采样率差异很大：无人机 IMU 可达 200 Hz，无人车 GPS 约 10 Hz，无人船罗经约 5 Hz。统一接口中的 timestamp 必须来自同一时钟源（如 PTP 或 GPS 秒脉冲），并在融合时做插值对齐。否则，即使模型再精确，不同步的状态也会让间隔计算产生米级误差，直接威胁避碰安全。