牵引车适配案例

概述 / Overview

牵引车（也称双阿克曼车型、tow-truck、tractor）的关键特征是 有非完整约束（不能瞬时横移），加上 后挂多节拖车时存在 逆向跟踪难与 折叠/失稳 风险。MDCS 通过专用 Movement 与额外的拖车包络处理这些问题。

The tractor / double-Ackermann (tow-truck) vehicle has non-holonomic constraints (can't translate laterally) and, when towing trailers, the difficulties of reverse-tracking, jackknife avoidance, and trailer swing. MDCS handles these via dedicated Movements and trailer envelopes.

参考：浙江中力 (`D:\src\cookbook\adaption-reference\浙江中力\`) 等拖车 / 牵引适配案例。 References: Zhejiang Zhongli (`adaption-reference\浙江中力\`) and similar.

车型变体 / Variants

子型 / Sub-type	说明 / Description
单阿克曼 / Single-Ackermann	前轮转向，后轮驱动，无拖车 / Front-steer, rear-drive, no trailer
双阿克曼 / Double-Ackermann	前后都可转向（蟹行）/ Both axles steer (can crab-walk)
牵引 + 1 挂 / Tractor + 1 trailer	单个铰接点 / single hitch
牵引 + N 挂 / Tractor + N trailers	多铰接，每节带额外的运动学约束
双向牵引 / Bidirectional tractor	前后都能作为牵引头 / can lead from either end

1. Medulla 适配 / Medulla side

关键 IO（以双阿克曼为例）：

public class TowCart : CartDefinition
{
    [AsInitParam] public string canIface = "can0";
    [AsInitParam] public float  wheelBase = 1450;   // mm
    [AsInitParam] public float  trackFront = 1100;
    [AsInitParam] public float  trackRear  = 1100;
    [AsInitParam] public bool   doubleAckermann = true;

    [AsUpperIO] public float vCmd;
    [AsUpperIO] public float steerFrontCmd;     // 前轮转角 / rad
    [AsUpperIO] public float steerRearCmd;      // 后轮转角 / rad  (sign for crab vs co-turn)
    [AsUpperIO] public bool  brakeOn;
    [AsUpperIO] public bool  hitchEngaged;      // 挂钩控制 / hitch control

    [AsLowerIO] public float vEst;
    [AsLowerIO] public float steerFrontEst, steerRearEst;
    [AsLowerIO] public float hitchAngle1, hitchAngle2, hitchAngle3; // 每节拖车的折角
    [AsLowerIO] public bool  hitchOK;
    [AsLowerIO] public bool  eStop;
}

铰接角 / Hitch angles：每节拖车与前一节的相对角度，是逆向跟踪与失稳预警的关键状态。`hitchAngleN` 用 IMU 双轴或单点编码器测。

The hitch angle of each trailer relative to the one in front is the key state for reverse-tracking and jackknife warning. Measure via dual-axis IMUs or a hitch encoder.

2. Clumsy 自动驾驶适配 / Clumsy side

关键 Movement

Movement	用途 / Use
`SteeringLineFollowing`	正向行驶（单 / 双阿克曼通用）/ forward driving
`SteeringLineFollowingReverse`	逆向行驶（带 trailer）/ reverse with trailer
`CrabLineFollowing`	蟹行（仅双阿克曼）/ crab walk (double-Ackermann only)
`TractorTurnAround`	调头动作（多段曲线）/ multi-segment U-turn
`JackknifeGuard`	守护线程：检测 hitch 角度过大时刹停 / guard thread for excessive hitch angle

逆向跟踪（reverse-track with trailer）

带 N 节拖车做逆向跟踪时，运动学是 非线性、不稳定。MDCS 采用：

Reverse-tracking with N trailers is nonlinear and unstable. MDCS uses:

在路径规划时禁止带挂逆向超过 5 m。
仅允许在专用的 逆向缓冲区做带挂倒车，且车速 ≤ 100 mm/s。
控制器：以 最后一节拖车的姿态为状态，通过非线性逆向控制律解算前轴转角。
任何 hitch 角度 ≥ 30° 立即停车 + 报警。

Path planning forbids reverse-with-trailer > 5 m.
Reverse-with-trailer only permitted in dedicated reverse buffer zones at ≤ 100 mm/s.
Controller takes the rear-most trailer pose as the state and inverts to front-axle angle.
Any hitch angle ≥ 30° → e-stop + alarm.

调头 / U-turn

不带挂的牵引车 必须用最小转弯半径以内的曲线段拼接来调头，常见 K-turn 或 U-turn。MDCS 的 `TractorTurnAround` Movement 包含一个解析的多段 Dubin's 路径生成器，已考虑 wheelBase + minTurnRadius。

A tractor without a trailer needs a multi-segment path within its minimum turning radius. The `TractorTurnAround` Movement uses an analytic Dubin's path generator parameterised by wheelBase and minTurnRadius.

3. SimpleComposer 端 / SimpleComposer side

拖车包络 / Trailer envelope

牵引 + N 挂的包络必须由 拖车折角计算 —— 直行时是简单的长矩形，弯路时各节会摆向不同方向，整体包络变成扇形。

The envelope of a tractor + N trailers is shaped by hitch angles. Straight-line travel gives a long rectangle; turning causes each segment to swing differently, producing a fan-shaped envelope. SimpleComposer dynamically recomputes this from hitch-angle readings.

调度约束 / Scheduling constraints

最小转弯半径声明在车型属性中，调度寻路时排除半径过小的弯。
带挂与 不带挂是两种不同的可达性状态（可达性状态编程）。
拖车数量影响交管锁；多挂车一次锁连续多个站点。

Minimum turn radius declared in car props; pathfinder excludes tighter turns.
Towing vs not-towing are two reachability states.
Multi-trailer cars take multi-site locks at once.

4. 标定与调试 / Calibration & tuning

前轮 / 后轮转角极性 / Steer polarity: 单独测各轴的左 / 右极性，确认与 IMU 一致。
单阿克曼直线度 / Single-Ackermann straight: 直行 10 m 后侧向偏差 < 30 mm。
双阿克曼蟹行 / Crab walk: 命令 30° 蟹行 5 m，实际方向偏差 < 1°。
拖车铰接角度 / Hitch zero: 静态直拖时 hitchAngle 应 = 0；偏 > 1° 必须重做 IMU 零点。
极限工况 / Boundary: 满载 + 满拖 + 最小半径转弯，确认无失稳。

Steer polarity per axle; confirm IMU sign agreement.
Single-Ack straight-line: < 30 mm drift over 10 m.
Double-Ack crab-walk: 30° crab × 5 m → heading drift < 1°.
Hitch zero: static straight tow → hitchAngle = 0 ± 1°.
Boundary: fully-loaded, full trailer train, min-radius turn → no jackknife.

5. 常见问题 / Common pitfalls

逆向失控 / Reverse instability: 拖车超 2 节时控制律不收敛；改成 分段倒车（每次只倒 0.5 m 复位）。
弯道挂车摆动 / Trailer swing on curves: 把 `basespeed` 在弯道前先降到 50%，并加大 弯道前提早转向时间。
hitch 编码器漂 / Hitch encoder drift: 检查机械连接 + IMU 零点；每天上电后做一次自检。
带挂掉头空间不足 / Insufficient room for U-turn: 调度寻路前必须知道拖车节数，否则 `TractorTurnAround` 会规划出穿墙路径。

6. 双向牵引 / Bidirectional tractor

双向牵引车前后都可拖挂；切换"主拉端"时，Detour 的车体坐标需要同步 翻转 180°，并切换前向 / 后向激光雷达为 SLAM 主源。

A bidirectional tractor can pull from either end. When swapping the "lead end", flip the vehicle frame 180° in Detour and switch the primary SLAM lidar from front to rear.