牵引车-挂车运动学与折刀判定（trailer_kinematics）

这一页解决什么问题

挂车不是「加长版单车」。它的航向是第二个状态量，由铰接点的横向速度驱动， 因此多出一个牵引车没有的现象：折刀（trailer jackknife）。倒车时这个状态还是 不稳定的——方向盘保持一个角度，铰接角就会一直涨到折刀。

这套东西在实车和仿真器里都很贵：要么开 CARLA 等世界跑起来，要么上路试。但它的核心 只是两个一阶微分方程加一个闭式解，完全可以在离线、无图形界面、任何 Python 解释器 上把「余量够不够」算清楚。

本模块因此定位成挂车相关逻辑的回归护栏：不产生画面、不需要 GPU、不订阅外部数据， 只吃参数、吐报告和字符图。

运动学模型

记牵引车后轴处航向为 θ₁、挂车航向为 θ₂、铰接角 φ = θ₁ − θ₂：

θ₁' = v · tan(δ) / L1
θ₂' = ( v·sin(φ) − d·θ₁'·cos(φ) ) / L2

积分方式是显式欧拉，步长默认 0.02 s。两个状态量每步都折算回 [-π, π)， 避免长时间仿真把角度累加成一圈以上。

稳态有闭式解，拿来当自检

令 φ' = 0，记 A = tan δ / L1，可得

sin(φ) − d·A·cos(φ) = L2·A

写成 R·sin(φ − ψ) = L2·A（R = √(1+(dA)²)、ψ = atan(dA)）后解出

φ_steady = atan(dA) + asin( L2·A / √(1 + (dA)²) )

• 默认参数：φ_steady ≈ 52.09°
• 把 d 设成 0：退化为经典的 φ_steady = asin(L2·tan δ / L1) ≈ 46.50°
• 当 L2·A/√(1+(dA)²) > 1 时不存在稳态，铰接角会一直涨到折刀

这个闭式解写进了离线测试（C 组），也写进了「30 秒长仿真是否收敛到 52.09°」这条 检查（C8）。积分代码一旦改坏，测试立刻红。

四条断言

「通道宽度」的算法：把所有采样点上牵引车与挂车的 8 个角点投影到世界坐标，取 y 方向 （即起步时垂直于车身的方向）的跨度。默认 90° 转弯需要 10.96 m。

参数

命令行全部可覆盖，例如 roslaunch trailer_kinematics main.launch steer_deg:=30.0。

话题

运行与真实输出

catkin_make
source devel/setup.bash
roslaunch trailer_kinematics main.launch

牵引车-挂车运动学与折刀判定（纯几何 + 显式欧拉，不依赖 CARLA）
牵引车：轴距 2.70 m，铰接偏移 0.60 m，最大转角 35.00 度
挂车：铰接点到轴线 4.20 m，组合体约 7.50 m
最小转弯半径：3.856 m
采样：456 个点，时长 9.10 s，速度 1.00 m/s
起点：(0.000, 0.000)，终点：(5.800, 5.785)
牵引车转角：90.05 度
铰接角：最大 43.27 度，末段 42.52 度，首段 4.75 度
占用通道宽度：10.96 m
断言：4 通过 / 0 失败（共 4 条）
[PASS] 折刀余量     最大铰接角 43.27 度，折刀角 90.00 度（余量 46.73 度）
[PASS] 积分精度     步长 0.020 与 0.010 的位置差 0.00707 m，上限 0.010 m
[PASS] 通道宽度     占用宽度 10.96 m，通道 12.00 m
[PASS] 倒车稳定性    前进：首段 4.75 度 → 末段 42.52 度，不适用倒车发散判定
结论：PASSED

两个反例

倒车（会折刀）：

roslaunch trailer_kinematics main.launch gear:=-1

铰接角：最大 167.36 度，末段 161.78 度，首段 5.46 度
断言：2 通过 / 2 失败（共 4 条）
[FAIL] 折刀余量     最大铰接角 167.36 度，折刀角 90.00 度（余量 -77.36 度）
[PASS] 积分精度     步长 0.020 与 0.010 的位置差 0.00707 m，上限 0.010 m
[PASS] 通道宽度     占用宽度 10.91 m，通道 12.00 m
[FAIL] 倒车稳定性    倒车：首段 5.46 度 → 末段 161.78 度，铰接角持续增大
结论：FAILED

通道不够宽：

roslaunch trailer_kinematics main.launch channel_width:=4.0

只有 [FAIL] 通道宽度 占用宽度 10.96 m，通道 4.00 m 一条失败，其余三条仍然 PASS—— 这证明四条断言是各管一件事，不是一起翻红。

离线测试

python3 tests/test_trailer_kinematics_local.py
python3 -m pytest tests/test_trailer_kinematics_local.py -q

只依赖 Python 标准库（不需要 ROS、CARLA、显示器），Melodic 的 Python 2.7 与 Noetic 的 Python 3 都能直接跑。测试覆盖 A 几何参数、B 通用小工具、C 稳态闭式解、 D 仿真推进、E 铰接角指标、F 通道宽度、G 积分精度、H 四条断言、I 字符图、 J 参数读取、K 工程文件约定共 11 组。

与已有模块的分工

常见问题

为什么默认跑 9.10 秒？ L1 = 2.70 m、δ = 25° 时航向角速度是 0.1727 rad/s，转 90° 正好 9.10 s。这样一个默认 场景就是「路口转弯」，通道宽度这个指标才有意义。

倒车为什么一定失败？ 不是代码坏了。倒车时铰接角的不动点是不稳定的，方向盘保持一个角度，铰接角就单调 增大到折刀。gear:=-1 存在的意义就是把这条写进证据里。

为什么还要检查积分精度？ 显式欧拉在 0.02 s 下误差只有 7 mm，放大到 0.5 s 就变成 17.5 cm。这条断言让文档里 「误差可忽略」的说法有依据，也顺手抓「有人把步长调大图省事」这类回归。

和 CARLA 的 scenario runner 什么关系？ CARLA 的 scenario runner 管「整个世界怎么演」，需要仿真器；本模块只管挂车这一个 自由度的运动学和稳定性，任何 Python 都能跑，适合做挂车相关代码的回归护栏。

所使用具体的大模型声明：使用大模型辅助代码编写、测试编写与文档整理，提交内容由本人负责。

TF 广播（ROS 原生形态）

节点算完轨迹后会持续广播变换（tf/enabled 默认 true，频率 tf/rate_hz 默认 10 Hz）， 因此除了看终端报告，还可以用 ROS 自带的工具核对位姿：

$ rosrun tf tf_echo map tractor
$ rosrun tf view_frames     # 导出一份 tf 树到 frames.pdf

默认参数下 map -> tractor 的平移是 (5.800, 5.785, 0.000)、yaw 约 1.5716 rad； tractor -> trailer 的平移是 (-2.458, 2.879, 0.000)。两张变换的相对航向 -0.7552 rad 正是 trailer_yaw - tractor_yaw，与报告中的 铰接角 最大 43.27° 互相印证。

依赖：tf2_ros、geometry_msgs（package.xml 已声明）。关掉广播用 roslaunch trailer_kinematics main.launch tf_enabled:=false。