From 8da183410e583dcacf5e1ac7114649382899fbc7 Mon Sep 17 00:00:00 2001 From: Wist-sutton <3473325902@qq.com> Date: Wed, 11 Nov 2026 19:45:31 +0800 Subject: [PATCH 1/5] Add files via upload --- .../lateral_stability_margin/CMakeLists.txt | 18 + src/ground/lateral_stability_margin/README.md | 162 +++++++++ .../config/lateral_stability_margin.yaml | 20 ++ .../launch/main.launch | 46 +++ .../lateral_stability_margin/package.xml | 18 + .../lateral_stability_margin/scripts/main.py | 19 ++ .../scripts/stability_core.py | 310 ++++++++++++++++++ .../scripts/stability_node.py | 119 +++++++ 8 files changed, 712 insertions(+) create mode 100644 src/ground/lateral_stability_margin/CMakeLists.txt create mode 100644 src/ground/lateral_stability_margin/README.md create mode 100644 src/ground/lateral_stability_margin/config/lateral_stability_margin.yaml create mode 100644 src/ground/lateral_stability_margin/launch/main.launch create mode 100644 src/ground/lateral_stability_margin/package.xml create mode 100644 src/ground/lateral_stability_margin/scripts/main.py create mode 100644 src/ground/lateral_stability_margin/scripts/stability_core.py create mode 100644 src/ground/lateral_stability_margin/scripts/stability_node.py diff --git a/src/ground/lateral_stability_margin/CMakeLists.txt b/src/ground/lateral_stability_margin/CMakeLists.txt new file mode 100644 index 0000000000..e811889bb3 --- /dev/null +++ b/src/ground/lateral_stability_margin/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.0.2) +project(lateral_stability_margin) + +find_package(catkin REQUIRED) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/stability_core.py + scripts/stability_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/lateral_stability_margin/README.md b/src/ground/lateral_stability_margin/README.md new file mode 100644 index 0000000000..e470f0c236 --- /dev/null +++ b/src/ground/lateral_stability_margin/README.md @@ -0,0 +1,162 @@ +# lateral_stability_margin:不足转向梯度与侧向稳定裕度 + +同一台车,换一套轮胎或挪一下质心,方向盘的手感与高速稳定性会完全不同。 +这个模块把这件事算成四个能直接读的数:**不足转向梯度 K、特征车速 / 临界车速、 +稳态转向角 δ、转向灵敏度 a_y/δ**,并按四条断言给出结论。 + +不依赖 CARLA、不需要显示器,纯 Python 标准库,Python 2.7 / 3.x 都能跑。 + +## 它回答的问题 + +| 问题 | 输出 | +|---|---| +| 这车是推头还是甩尾? | 梯度符号:`K > 0` 不足转向、`K < 0` 过多转向 | +| 高速上还有多少余量? | 不足转向给特征车速,过多转向给临界车速 | +| 半径 50 m 的弯,方向盘要打多少? | 稳态转向角 δ(rad 与 °) | +| 同样的方向盘角度,车能侧向加速度多少? | 转向灵敏度 a_y/δ | + +## 模型 + +``` +轴荷 W_f = m·g·b/L b = 质心到后轴 + W_r = m·g·a/L a = 质心到前轴 + +侧偏角 α_f = W_f·a_y/(g·C_f) + α_r = W_r·a_y/(g·C_r) + +不足转向梯度 + K = (W_f/C_f − W_r/C_r) / g [rad/(m/s²)] + K > 0 不足转向(推头,稳定) + K = 0 中性转向 + K < 0 过多转向(甩尾,超过临界车速会发散) + +稳态转向角 δ = L/R + K·a_y [rad],a_y = v²/R +转向灵敏度 a_y/δ = v² / (L + K·v²) + +特征车速 v_char = sqrt(L / K) (K > 0;灵敏度降到中性转向的一半) +临界车速 v_crit = sqrt(−L / K) (K < 0;灵敏度发散) +``` + +注意 `K` 里那一步 **除以 g**:`W/C` 的量纲是 rad,乘上 `a_y` 再除 `g` 才是 rad。 +漏掉它,特征车速会从 100 km/h 掉到 32 km/h —— 这是最容易错的一步。 + +## 默认数字(`config/lateral_stability_margin.yaml`) + +| 项 | 值 | +|---|---| +| 整车质量 / 轴距 / 质心到前轴 | 1500 kg / 2.7 m / 1.2 m(到后轴 1.5 m) | +| 前 / 后轴侧偏刚度 | 80 000 / 95 000 N/rad | +| 前轴 / 后轴静态载荷 | **8175.0 N / 6540.0 N**(合计 14 715.0 N) | +| 不足转向梯度 K | **0.003399 rad/(m/s²)**,特性「不足转向」 | +| 特征车速 | **28.184 m/s = 101.5 km/h** | +| 工况 R = 50 m、40 km/h | a_y 2.469 m/s²,δ **0.062393 rad(3.57°)**,阿克曼 0.054000 rad(3.09°),超出 0.008393 rad | +| 转向灵敏度 | 39.574 1/s | + +车速扫描(R = 50 m): + +| 车速 km/h | a_y m/s² | δ rad | δ ° | 灵敏度 1/s | +|---|---|---|---|---| +| 20 | 0.617 | 0.0561 | 3.21 | 11.00 | +| 40 | 2.469 | 0.0624 | 3.57 | 39.57 | +| 60 | 5.556 | 0.0729 | 4.18 | 76.22 | +| 80 | 9.877 | 0.0876 | 5.02 | 112.78 | +| 100 | 15.432 | 0.1065 | 6.10 | 144.96 | +| 120 | 22.222 | 0.1295 | 7.42 | 171.55 | + +转向角随车速单调增大、灵敏度单调上升,正是不足转向车的特征。 + +## 四条断言 + +| 断言 | 判据 | 默认结果 | +|---|---|---| +| 轴荷分配 | `W_f + W_r = m·g`,且 `W_f/W_r = b/a` | PASS | +| 不足转向裕度 | `K ≥ understeer_min`(默认 0) | PASS | +| 低速阿克曼 | 低速(默认 2 km/h)时 `δ` 与 `L/R` 的相对偏差 ≤ 2% | PASS(0.04%) | +| 稳态转向角 | 由 `δ` 反算 `a_y` 再正算,闭环相对偏差 ≤ 1e-6 | PASS(1.8e-16) | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `/vehicle/mass` | 1500.0 | 整车质量 kg | +| `/vehicle/wheelbase` | 2.7 | 轴距 m | +| `/vehicle/cg_to_front` | 1.2 | 质心到前轴距离 m(必须 < 轴距) | +| `/vehicle/gravity` | 9.81 | 重力加速度 | +| `/tire/front_cornering` | 80000.0 | 前轴侧偏刚度 N/rad | +| `/tire/rear_cornering` | 95000.0 | 后轴侧偏刚度 N/rad | +| `/scene/radius` | 50.0 | 转弯半径 m | +| `/scene/speed_kmh` | 40.0 | 稳态圆周车速 km/h | +| `/check/understeer_min` | 0.0 | 梯度下限,0 = 不允许过多转向 | +| `/check/ackermann_speed_kmh` | 2.0 | 校验阿克曼角用的低速 | +| `/check/ackermann_tolerance` | 0.02 | 低速偏差上限 | +| `/rate/publish_hz` | 1.0 | 保活重发频率 Hz | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/stability/report` | `std_msgs/String` | 整份报告(参数 + 扫描表 + 断言 + 结论) | +| `/stability/cornering` | `std_msgs/Float64MultiArray` | `[K, a_y, δ, 阿克曼角, 超出量]` | +| `/stability/limit` | `std_msgs/Float64MultiArray` | `[特征车速, 临界车速]`,不存在的那个给 −1 | + +三条都是 latched,`rostopic echo` 一上来就能拿到。 + +## 跑起来 + +```bash +# ① 编译 +cd ~/catkin_ws20 +catkin_make + +# ② 跑 +source /opt/ros/melodic/setup.bash +source ~/catkin_ws20/devel/setup.bash +roslaunch lateral_stability_margin main.launch + +# ③ 反例:换成硬前软后的甩尾车 +roslaunch lateral_stability_margin main.launch front_cornering:=160000 rear_cornering:=40000 +``` + +默认车 `结论:PASSED`;换成 `front_cornering:=160000 rear_cornering:=40000` 后变成 +`过多转向`、临界车速 **15.350 m/s(55.3 km/h)**,只翻「不足转向裕度」这一条: + +``` +[FAIL] 不足转向裕度 K = -0.011458 rad/(m/s²)(下限 0.0000), + 特性「过多转向」,临界车速 15.350 m/s(55.3 km/h) +``` + +## 离线测试 + +不启动 ROS 也能跑,152+ 项覆盖几何、梯度、圆周、断言、扫描、字符图、参数、 +报告、闭式核对、工程文件与编码声明: + +```bash +python3 tests/test_lateral_stability_margin_local.py +``` + +## 与已有模块的分工 + +| 模块 | 关注点 | 本模块的区别 | +|---|---|---| +| `tire_slip_curve` | 纵向附着随滑移率的变化 | 管纵向(刹车/驱动),本模块管侧向 | +| `rollover_margin` | 侧翻(抬轮) | 侧翻是**载荷**问题,本模块是**转向响应**问题 | +| `turning_sweep_check` | 转弯扫掠包络(几何) | 只算占多宽,不算车会不会甩尾 | +| `steering_actuator_lag` | 转向执行器动态 | 管“打不打得到”,本模块管“该打多少” | + +## 常见问题 + +**1. 为什么我算出的特征车速特别低?** +九成是 `K` 忘了除以 `g`。`W/C` 出来是 rad,`δ = L/R + K·a_y` 里的 `K` 单位是 +rad/(m/s²),必须除以 9.81。 + +**2. 低速阿克曼那条为什么卡在 2 km/h?** +`δ = L/R + K·a_y`,只要车速不为 0,`K·a_y` 就不为 0。2 km/h 时它只占 0.04%, +放到 60 km/h 就占 8 倍 —— 手册里的第二个反例就是这么翻的。 + +**3. 我把 `understeer_min` 调大有什么用?** +它是一条验收线。乘用车通常要求至少轻微不足转向;把它设成 0.002 就能卡掉 +“几乎中性”的调校。 + +## 大模型声明 + +本模块的代码编写、测试编写与文档整理使用了 AI 编程助手辅助,提交内容由本人负责。 diff --git a/src/ground/lateral_stability_margin/config/lateral_stability_margin.yaml b/src/ground/lateral_stability_margin/config/lateral_stability_margin.yaml new file mode 100644 index 0000000000..9ae94c2e31 --- /dev/null +++ b/src/ground/lateral_stability_margin/config/lateral_stability_margin.yaml @@ -0,0 +1,20 @@ +# lateral_stability_margin 的默认参数 +# 由 launch/main.launch 用 载入 + +vehicle: + mass: 1500.0 # 整车质量 kg + wheelbase: 2.7 # 轴距 m + cg_to_front: 1.2 # 质心到前轴距离 m(到后轴 = 2.7 − 1.2 = 1.5) + +tire: + front_cornering: 80000.0 # 前轴侧偏刚度 N/rad + rear_cornering: 95000.0 # 后轴侧偏刚度 N/rad + +scene: + radius: 50.0 # 转弯半径 m + speed_kmh: 40.0 # 稳态圆周车速 km/h + +check: + understeer_min: 0.0 # 不足转向梯度下限 rad/(m/s²)(0 = 不允许过多转向) + ackermann_speed_kmh: 2.0 # 校验阿克曼角用的低速 km/h + ackermann_tolerance: 0.02 # 低速转向角与 L/R 的相对偏差上限 diff --git a/src/ground/lateral_stability_margin/launch/main.launch b/src/ground/lateral_stability_margin/launch/main.launch new file mode 100644 index 0000000000..3ee336c3a5 --- /dev/null +++ b/src/ground/lateral_stability_margin/launch/main.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/lateral_stability_margin/package.xml b/src/ground/lateral_stability_margin/package.xml new file mode 100644 index 0000000000..0bc3848903 --- /dev/null +++ b/src/ground/lateral_stability_margin/package.xml @@ -0,0 +1,18 @@ + + + lateral_stability_margin + 1.0.0 + + 不足转向梯度与侧向稳定裕度核算:由轴荷与轮胎侧偏刚度算出梯度 K、 + 特征车速 / 临界车速、稳态转向角与转向灵敏度,并给出四条断言。 + 纯标准库实现,不依赖 CARLA、不需要显示器。 + + + Wist-sutton + MIT + + catkin + + rospy + std_msgs + diff --git a/src/ground/lateral_stability_margin/scripts/main.py b/src/ground/lateral_stability_margin/scripts/main.py new file mode 100644 index 0000000000..b626c4a70e --- /dev/null +++ b/src/ground/lateral_stability_margin/scripts/main.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""lateral_stability_margin 节点入口:读取参数、核算、打印、发布。""" + +from __future__ import division, print_function, unicode_literals + +import os +import sys + +try: # Python 2 相对导入兜底 + _HERE = os.path.dirname(os.path.abspath(__file__)) + if _HERE not in sys.path: + sys.path.insert(0, _HERE) + from stability_node import main +except ImportError: # pragma: no cover + from .stability_node import main + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/lateral_stability_margin/scripts/stability_core.py b/src/ground/lateral_stability_margin/scripts/stability_core.py new file mode 100644 index 0000000000..f642a77218 --- /dev/null +++ b/src/ground/lateral_stability_margin/scripts/stability_core.py @@ -0,0 +1,310 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""不足转向梯度与侧向稳定裕度核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +问题:一辆车是「推头」还是「甩尾」?在多大车速下会失去侧向稳定? +方向盘打多少度,才能维持住某个半径的弯? + +模型(线性轮胎,自行车模型): + + 轴荷 W_f = m·g·b/L 前轴静态载荷(b = 质心到后轴) + W_r = m·g·a/L 后轴静态载荷(a = 质心到前轴) + 不足转向梯度 + K = W_f / C_f − W_r / C_r [rad / (m/s²)] + K > 0 不足转向(推头,稳定) + K = 0 中性转向 + K < 0 过多转向(甩尾,会失稳) + 特征车速 v_char = sqrt(L·g / K) (K > 0 才有) + 临界车速 v_crit = sqrt(−L·g / K) (K < 0 才有,超过就发散) + 稳态转向角 + δ = L/R + K·a_y [rad],a_y = v²/R + 低速时 K·a_y → 0,退化成阿克曼角 L/R + 转向灵敏度 + (a_y / δ) = v² / (L + K·v²) 分母在 v_crit 处为 0 +""" + +from __future__ import division, print_function, unicode_literals + +import math + +ASSERT_NAMES = (u"轴荷分配", u"不足转向裕度", u"稳态转向角", u"低速阿克曼") + +GRAVITY = 9.81 + + +class StabilityError(ValueError): + """参数非法。""" + + +def _positive(name, value): + if value is None or value <= 0: + raise StabilityError(u"%s 必须大于 0,收到 %r" % (name, value)) + return value + + +class VehicleSpec(object): + """整车几何与轮胎侧偏刚度。""" + + def __init__(self, mass=1500.0, wheelbase=2.7, cg_to_front=1.2, + front_cornering=80000.0, rear_cornering=95000.0, + gravity=GRAVITY): + self.mass = _positive(u"整车质量", mass) + self.wheelbase = _positive(u"轴距", wheelbase) + self.cg_to_front = _positive(u"质心到前轴距离", cg_to_front) + if self.cg_to_front >= self.wheelbase: + raise StabilityError(u"质心到前轴距离必须小于轴距,收到 %r" + % (self.cg_to_front,)) + self.front_cornering = _positive(u"前轮侧偏刚度", front_cornering) + self.rear_cornering = _positive(u"后轮侧偏刚度", rear_cornering) + self.gravity = _positive(u"重力加速度", gravity) + + # ---- 几何 ---- + def cg_to_rear(self): + return self.wheelbase - self.cg_to_front + + def front_load(self): + return self.mass * self.gravity * self.cg_to_rear() / self.wheelbase + + def rear_load(self): + return self.mass * self.gravity * self.cg_to_front / self.wheelbase + + def total_load(self): + return self.mass * self.gravity + + # ---- 不足转向梯度 ---- + def gradient(self): + """不足转向梯度 K [rad/(m/s²)] = (W_f/C_f − W_r/C_r) / g。 + + 推导:前轮侧偏角 α_f = W_f·a_y/(g·C_f),后轮同理, + 稳态转向角 δ = L/R + α_f − α_r。 + """ + return (self.front_load() / self.front_cornering + - self.rear_load() / self.rear_cornering) / self.gravity + + def character(self): + k = self.gradient() + if abs(k) < 1e-12: + return u"中性转向" + return u"不足转向" if k > 0 else u"过多转向" + + def characteristic_speed(self): + """特征车速(m/s):转向灵敏度降到中性转向一半的车速;K ≤ 0 时不存在。""" + k = self.gradient() + if k <= 0: + return None + return math.sqrt(self.wheelbase / k) + + def critical_speed(self): + """临界车速(m/s):转向灵敏度发散的车速;K ≥ 0 时不存在。""" + k = self.gradient() + if k >= 0: + return None + return math.sqrt(-self.wheelbase / k) + + def __repr__(self): + return ("VehicleSpec(mass=%.1f, wheelbase=%.2f, cg_to_front=%.2f, " + "Cf=%.0f, Cr=%.0f)" + % (self.mass, self.wheelbase, self.cg_to_front, + self.front_cornering, self.rear_cornering)) + + +class Cornering(object): + """给定半径与车速的一次稳态圆周。""" + + def __init__(self, spec, radius=50.0, speed_kmh=40.0): + if spec is None: + raise StabilityError(u"缺少整车参数") + self.spec = spec + self.radius = _positive(u"转弯半径", radius) + self.speed_kmh = _positive(u"车速", speed_kmh) + + def speed(self): + return self.speed_kmh / 3.6 + + def lateral_accel(self): + return self.speed() ** 2 / self.radius + + def steer_angle(self): + """稳态转向角(rad)= L/R + K·a_y。""" + return (self.spec.wheelbase / self.radius + + self.spec.gradient() * self.lateral_accel()) + + def ackermann_angle(self): + """低速理想阿克曼角(rad)= L/R。""" + return self.spec.wheelbase / self.radius + + def deflection(self): + """转向角超出阿克曼角的部分(rad),来自不足/过多转向。""" + return self.steer_angle() - self.ackermann_angle() + + def sensitivity(self): + """转向灵敏度 a_y / δ;分母非正时返回 None(已过临界点)。""" + den = self.spec.wheelbase + self.spec.gradient() * self.speed() ** 2 + if den <= 0: + return None + return self.lateral_accel() / self.steer_angle() + + def __repr__(self): + return ("Cornering(radius=%.1f, speed_kmh=%.1f)" + % (self.radius, self.speed_kmh)) + + +def check_stability(cornering, understeer_min=0.0, ackermann_speed_kmh=2.0, + ackermann_tolerance=0.02, load_tolerance=1e-6): + """四条断言:轴荷分配、不足转向裕度、稳态转向角、低速阿克曼。""" + spec = cornering.spec + asserts = [] + + total = spec.total_load() + split = spec.front_load() + spec.rear_load() + rel = abs(split - total) / max(total, 1e-12) + ratio_ok = abs(spec.front_load() / spec.rear_load() + - spec.cg_to_rear() / spec.cg_to_front) < 1e-9 + asserts.append({ + "type": ASSERT_NAMES[0], + "passed": bool(rel <= load_tolerance and ratio_ok), + "detail": (u"前轴 %.1f N + 后轴 %.1f N = %.1f N(整车 %.1f N,相对偏差 %.2e)" + % (spec.front_load(), spec.rear_load(), split, total, rel)), + }) + + k = spec.gradient() + if k > 0: + extra = u"特征车速 %.3f m/s(%.1f km/h)" % ( + spec.characteristic_speed(), spec.characteristic_speed() * 3.6) + elif k < 0: + extra = u"临界车速 %.3f m/s(%.1f km/h)" % ( + spec.critical_speed(), spec.critical_speed() * 3.6) + else: + extra = u"无特征/临界车速" + asserts.append({ + "type": ASSERT_NAMES[1], + "passed": bool(k >= understeer_min - 1e-12), + "detail": (u"K = %.6f rad/(m/s²)(下限 %.4f),特性「%s」,%s" + % (k, understeer_min, spec.character(), extra)), + }) + + slow = Cornering(spec, radius=cornering.radius, + speed_kmh=ackermann_speed_kmh) + + # 闭环自洽:由 δ 反算 a_y,再代回 δ = L/R + K·a_y + delta = cornering.steer_angle() + ay_direct = cornering.lateral_accel() + ay_back = (delta - spec.wheelbase / cornering.radius) / spec.gradient() \ + if abs(spec.gradient()) > 1e-12 else ay_direct + rel_loop = abs(ay_back - ay_direct) / max(abs(ay_direct), 1e-12) + asserts.append({ + "type": ASSERT_NAMES[2], + "passed": bool(rel_loop <= 1e-6 and delta > 0), + "detail": (u"δ = %.6f rad → 反算 a_y = %.6f m/s²,正算 %.6f m/s²" + u"(相对偏差 %.2e)" + % (delta, ay_back, ay_direct, rel_loop)), + }) + + dev = abs(slow.steer_angle() - slow.ackermann_angle()) + rel_dev = dev / max(slow.ackermann_angle(), 1e-12) + asserts.append({ + "type": ASSERT_NAMES[3], + "passed": bool(rel_dev <= ackermann_tolerance + 1e-12), + "detail": (u"车速 %.1f km/h 时转向角 %.6f rad,阿克曼角 %.6f rad" + u"(相对偏差 %.4f,容差 %.3f)" + % (ackermann_speed_kmh, slow.steer_angle(), + slow.ackermann_angle(), rel_dev, ackermann_tolerance)), + }) + return asserts + + +DEFAULT_SPEEDS_KMH = (20.0, 40.0, 60.0, 80.0, 100.0, 120.0) + + +def speed_table(spec, radius=50.0, speeds=DEFAULT_SPEEDS_KMH): + """车速扫描:[(车速 km/h, a_y, δ rad, δ °, 灵敏度)]。""" + rows = [] + for v in speeds: + c = Cornering(spec, radius=radius, speed_kmh=v) + rows.append((v, c.lateral_accel(), c.steer_angle(), + math.degrees(c.steer_angle()), c.sensitivity())) + return rows + + +def render_speed_table(spec, radius=50.0, speeds=DEFAULT_SPEEDS_KMH, width=24): + """按转向角画横向条形图(以最大角为满条)。""" + rows = speed_table(spec, radius, speeds) + top = max(r[2] for r in rows) if rows else 1.0 + top = top if top > 0 else 1.0 + lines = [u"转向角随车速(半径 %.1f m,满条 = 最大角)" % radius, + "-" * (width + 34)] + for v, ay, delta, deg, sens in rows: + cells = int(round(delta / top * width)) + sens_txt = u"%7.3f 1/s" % sens if sens is not None else u" 发散 " + lines.append(u"%5.0f km/h |%s| %7.4f rad %s" + % (v, "#" * cells, delta, sens_txt)) + lines.append("-" * (width + 34)) + return "\n".join(lines) + + +def format_report(spec, cornering, asserts, + speeds=DEFAULT_SPEEDS_KMH): + """把参数、扫描表、断言与结论排成报告。""" + lines = [] + lines.append(u"整车:质量 %.1f kg,轴距 %.2f m,质心到前轴 %.2f m(到后轴 %.2f m)" + % (spec.mass, spec.wheelbase, spec.cg_to_front, + spec.cg_to_rear())) + lines.append(u"轮胎:前轮侧偏刚度 %.0f N/rad,后轮 %.0f N/rad" + % (spec.front_cornering, spec.rear_cornering)) + lines.append(u"轴荷:前 %.1f N,后 %.1f N(合计 %.1f N)" + % (spec.front_load(), spec.rear_load(), spec.total_load())) + lines.append(u"梯度:K = %.6f rad/(m/s²),特性「%s」" + % (spec.gradient(), spec.character())) + if spec.characteristic_speed() is not None: + lines.append(u"特征车速 %.3f m/s(%.1f km/h)" + % (spec.characteristic_speed(), + spec.characteristic_speed() * 3.6)) + if spec.critical_speed() is not None: + lines.append(u"临界车速 %.3f m/s(%.1f km/h)" + % (spec.critical_speed(), spec.critical_speed() * 3.6)) + lines.append(u"工况:半径 %.1f m,车速 %.1f km/h → a_y %.3f m/s²," + u"δ %.6f rad(%.2f°),阿克曼角 %.6f rad(%.2f°)," + u"超出 %.6f rad" + % (cornering.radius, cornering.speed_kmh, + cornering.lateral_accel(), cornering.steer_angle(), + math.degrees(cornering.steer_angle()), + cornering.ackermann_angle(), + math.degrees(cornering.ackermann_angle()), + cornering.deflection())) + sens = cornering.sensitivity() + lines.append(u"转向灵敏度 a_y/δ = %s" + % (u"%.4f 1/s" % sens if sens is not None else u"已发散(过临界)")) + lines.append(u"车速(km/h) a_y(m/s²) δ(rad) δ(°) 灵敏度(1/s)") + for v, ay, delta, deg, sens in speed_table(spec, cornering.radius, speeds): + lines.append(u"%9.0f %9.3f %8.4f %7.2f %s" + % (v, ay, delta, deg, + u"%8.4f" % sens if sens is not None else u" 发散")) + lines.append(u"断言:%d 通过 / %d 失败(共 %d 条)" + % (len([a for a in asserts if a["passed"]]), + len([a for a in asserts if not a["passed"]]), len(asserts))) + for a in asserts: + lines.append(u" [%s] %s %s" % (u"PASS" if a["passed"] else u"FAIL", + a["type"], a["detail"])) + ok = all(a["passed"] for a in asserts) + lines.append(u"结论:%s" % (u"PASSED" if ok else u"FAILED")) + return "\n".join(lines) + + +class StabilityParams(object): + """把 /xxx/yyy 形式的参数名收成对象,缺省时用内置默认值。""" + + def __init__(self, **kwargs): + self.spec = VehicleSpec( + mass=kwargs.get("vehicle/mass", 1500.0), + wheelbase=kwargs.get("vehicle/wheelbase", 2.7), + cg_to_front=kwargs.get("vehicle/cg_to_front", 1.2), + front_cornering=kwargs.get("tire/front_cornering", 80000.0), + rear_cornering=kwargs.get("tire/rear_cornering", 95000.0), + gravity=kwargs.get("vehicle/gravity", GRAVITY), + ) + self.radius = kwargs.get("scene/radius", 50.0) + self.speed_kmh = kwargs.get("scene/speed_kmh", 40.0) + self.understeer_min = kwargs.get("check/understeer_min", 0.0) + self.ackermann_speed_kmh = kwargs.get("check/ackermann_speed_kmh", 2.0) + self.ackermann_tolerance = kwargs.get("check/ackermann_tolerance", 0.02) + self.speeds = kwargs.get("scan/speeds_kmh", DEFAULT_SPEEDS_KMH) diff --git a/src/ground/lateral_stability_margin/scripts/stability_node.py b/src/ground/lateral_stability_margin/scripts/stability_node.py new file mode 100644 index 0000000000..f82c370793 --- /dev/null +++ b/src/ground/lateral_stability_margin/scripts/stability_node.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""lateral_stability_margin 的 ROS 节点:算完就发三条 latched 话题,然后保活。 + +注意:发给 rospy 的日志必须是 **byte str**(Python 2.7 下 str), +否则 rosgraph/roslogging.py 里的 str(record_message) 会抛 +UnicodeEncodeError。统一走 _log()。 +""" + +from __future__ import division, print_function, unicode_literals + +import os +import sys + +try: + _HERE = os.path.dirname(os.path.abspath(__file__)) + if _HERE not in sys.path: + sys.path.insert(0, _HERE) + import stability_core as core +except ImportError: # pragma: no cover + from . import stability_core as core + + +def _log(level, msg): + """把 unicode 消息转成 byte str 再交给 rospy。""" + import rospy + text = msg.encode("utf-8") if isinstance(msg, type(u"")) else msg + if level == "warn": + rospy.logwarn(text) + elif level == "err": + rospy.logerr(text) + else: + rospy.loginfo(text) + + +def main(argv=None): + import rospy + import std_msgs.msg as std_msg + + rospy.init_node("lateral_stability_margin", anonymous=False) + + def par(name, default): + return rospy.get_param(name, default) + + params = core.StabilityParams( + **{ + "vehicle/mass": par("/vehicle/mass", 1500.0), + "vehicle/wheelbase": par("/vehicle/wheelbase", 2.7), + "vehicle/cg_to_front": par("/vehicle/cg_to_front", 1.2), + "vehicle/gravity": par("/vehicle/gravity", 9.81), + "tire/front_cornering": par("/tire/front_cornering", 80000.0), + "tire/rear_cornering": par("/tire/rear_cornering", 95000.0), + "scene/radius": par("/scene/radius", 50.0), + "scene/speed_kmh": par("/scene/speed_kmh", 40.0), + "check/understeer_min": par("/check/understeer_min", 0.0), + "check/ackermann_speed_kmh": par("/check/ackermann_speed_kmh", 2.0), + "check/ackermann_tolerance": par("/check/ackermann_tolerance", 0.02), + } + ) + + spec = params.spec + cornering = core.Cornering(spec, radius=params.radius, + speed_kmh=params.speed_kmh) + asserts = core.check_stability( + cornering, + understeer_min=params.understeer_min, + ackermann_speed_kmh=params.ackermann_speed_kmh, + ackermann_tolerance=params.ackermann_tolerance, + ) + + report = core.format_report(spec, cornering, asserts, params.speeds) + _log("info", report) + _log("info", core.render_speed_table(spec, params.radius, params.speeds)) + + pub_report = rospy.Publisher("/stability/report", std_msg.String, + queue_size=1, latch=True) + pub_corner = rospy.Publisher("/stability/cornering", + std_msg.Float64MultiArray, + queue_size=1, latch=True) + pub_limit = rospy.Publisher("/stability/limit", + std_msg.Float64MultiArray, + queue_size=1, latch=True) + + report_msg = std_msg.String() + report_msg.data = report + pub_report.publish(report_msg) + + corner_msg = std_msg.Float64MultiArray() + corner_msg.data = [ + spec.gradient(), + cornering.lateral_accel(), + cornering.steer_angle(), + cornering.ackermann_angle(), + cornering.deflection(), + ] + pub_corner.publish(corner_msg) + + limit_msg = std_msg.Float64MultiArray() + limit_msg.data = [ + float(spec.characteristic_speed() or -1.0), + float(spec.critical_speed() or -1.0), + ] + pub_limit.publish(limit_msg) + + passed = len([a for a in asserts if a["passed"]]) + _log("info", u"已发布 /stability/report、/stability/cornering、" + u"/stability/limit(断言 %d/%d)" % (passed, len(asserts))) + + rate = rospy.Rate(par("/rate/publish_hz", 1.0)) + while not rospy.is_shutdown(): + pub_report.publish(report_msg) + pub_corner.publish(corner_msg) + pub_limit.publish(limit_msg) + rate.sleep() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From 9df357aa7325c03dc60558bc94ecc5da6a9733e7 Mon Sep 17 00:00:00 2001 From: Wist-sutton <3473325902@qq.com> Date: Wed, 11 Nov 2026 19:46:09 +0800 Subject: [PATCH 2/5] Add files via upload --- tests/test_lateral_stability_margin_local.py | 402 +++++++++++++++++++ 1 file changed, 402 insertions(+) create mode 100644 tests/test_lateral_stability_margin_local.py diff --git a/tests/test_lateral_stability_margin_local.py b/tests/test_lateral_stability_margin_local.py new file mode 100644 index 0000000000..2e62d1520f --- /dev/null +++ b/tests/test_lateral_stability_margin_local.py @@ -0,0 +1,402 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""lateral_stability_margin 模块的离线回归测试。 + +特点: +* 只用 Python 标准库,**不需要 ROS、不需要 CARLA、不需要显示器** +* 同时兼容 Python 2.7 与 Python 3.x(Melodic 与 Noetic 都能直接跑) +* 在仓库根目录执行:python3 tests/test_lateral_stability_margin_local.py + +覆盖范围: +A 几何与轴荷 B 梯度与转向特性 C 稳态圆周 +D 四条断言 E 车速扫描 F 字符图 +G 参数读取 H 报告排版 I 闭式核对 +J 工程文件约定 K 编码声明回归 +""" + +from __future__ import division, print_function, unicode_literals + +import io +import math +import os +import sys + +try: # Python 2 兼容 + text_type = unicode # noqa: F821 +except NameError: + text_type = str + +HERE = os.path.dirname(os.path.abspath(__file__)) +ROOT = os.path.dirname(HERE) + +PKG = os.path.join(ROOT, "src", "ground", "lateral_stability_margin") +SCRIPTS = os.path.join(PKG, "scripts") + +if SCRIPTS not in sys.path: + sys.path.insert(0, SCRIPTS) + +import stability_core as core # noqa: E402 + +FAILS = [] +OK = [0] + + +def out(text): + if isinstance(text, text_type): + sys.stdout.write(text + u"\n") + else: + sys.stdout.write(text + "\n") + + +def check(name, cond, detail=""): + OK[0] += 1 + if cond: + out(u" ok %s" % name) + else: + out(u" FAIL %s %s" % (name, detail)) + FAILS.append(name) + + +def near(a, b, tol=1e-9): + return abs(a - b) <= tol + + +def raises(exc, func, *args, **kwargs): + try: + func(*args, **kwargs) + except exc: + return True + except Exception: + return False + return False + + +# ---------------------------------------------------------------- A 几何与轴荷 +def group_geometry(): + out(u"A 几何与轴荷") + spec = core.VehicleSpec() + check(u"A1 质心到后轴", near(spec.cg_to_rear(), 1.5), repr(spec.cg_to_rear())) + check(u"A2 前轴载荷", near(spec.front_load(), 8175.0, 1e-6), repr(spec.front_load())) + check(u"A3 后轴载荷", near(spec.rear_load(), 6540.0, 1e-6), repr(spec.rear_load())) + check(u"A4 轴荷之和等于整车重力", near(spec.front_load() + spec.rear_load(), + spec.total_load(), 1e-9)) + check(u"A5 前轴载荷大于后轴", spec.front_load() > spec.rear_load()) + check(u"A6 轴荷比等于力臂反比", + near(spec.front_load() / spec.rear_load(), + spec.cg_to_rear() / spec.cg_to_front, 1e-12)) + + heavy = core.VehicleSpec(mass=2000.0) + check(u"A7 质量放大后轴荷比不变", + near(heavy.front_load() / heavy.rear_load(), + spec.front_load() / spec.rear_load(), 1e-12)) + + forward = core.VehicleSpec(cg_to_front=1.5) + check(u"A8 质心后移前轴变轻", forward.front_load() < spec.front_load()) + check(u"A9 质心后移后轴变重", forward.rear_load() > spec.rear_load()) + check(u"A10 非法轴距被拒", raises(core.StabilityError, core.VehicleSpec, wheelbase=0.0)) + check(u"A11 质心超轴距被拒", + raises(core.StabilityError, core.VehicleSpec, cg_to_front=3.0)) + check(u"A12 刚度为负被拒", + raises(core.StabilityError, core.VehicleSpec, front_cornering=-1.0)) + + +# ------------------------------------------------------------ B 梯度与转向特性 +def group_gradient(): + out(u"B 梯度与转向特性") + spec = core.VehicleSpec() + k = spec.gradient() + manual = (spec.front_load() / spec.front_cornering - spec.rear_load() / spec.rear_cornering) / core.GRAVITY + check(u"B1 梯度闭式一致", near(k, manual, 1e-12), repr(k)) + check(u"B2 默认车不足转向", k > 0, repr(k)) + check(u"B3 特性名不足转向", spec.character() == u"不足转向", spec.character()) + vchar = spec.characteristic_speed() + check(u"B4 特征车速闭式", near(vchar, math.sqrt(spec.wheelbase / k), 1e-12)) + check(u"B5 特征车速数值", near(vchar, 28.1837, 1e-3), repr(vchar)) + check(u"B6 不足转向无临界车速", spec.critical_speed() is None) + + over = core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0) + check(u"B7 硬前软后过多转向", over.gradient() < 0, repr(over.gradient())) + check(u"B8 特性名过多转向", over.character() == u"过多转向", over.character()) + vcrit = over.critical_speed() + check(u"B9 临界车速闭式", + near(vcrit, math.sqrt(-over.wheelbase / over.gradient()), 1e-12)) + check(u"B10 临界车速数值", near(vcrit, 15.3504, 1e-3), repr(vcrit)) + check(u"B11 过多转向无特征车速", over.characteristic_speed() is None) + + soft = core.VehicleSpec(front_cornering=110000.0) + check(u"B12 前轮变软梯度变小", soft.gradient() < k, repr(soft.gradient())) + check(u"B13 前轮变软特征车速变大", soft.characteristic_speed() > vchar) + + +# ---------------------------------------------------------------- C 稳态圆周 +def group_cornering(): + out(u"C 稳态圆周") + spec = core.VehicleSpec() + corner = core.Cornering(spec) + check(u"C1 车速换算", + near(corner.speed(), 40.0 / 3.6, 1e-12), repr(corner.speed())) + check(u"C2 侧向加速度", near(corner.lateral_accel(), 2.469136, 1e-6)) + check(u"C3 阿克曼角", near(corner.ackermann_angle(), 0.054, 1e-12)) + check(u"C4 稳态转向角", near(corner.steer_angle(), 0.0623929, 1e-6)) + check(u"C5 转向角大于阿克曼角", + corner.steer_angle() > corner.ackermann_angle()) + check(u"C6 超出量等于 K·a_y", + near(corner.deflection(), spec.gradient() * corner.lateral_accel(), 1e-12)) + check(u"C7 转向灵敏度", near(corner.sensitivity(), 39.5740, 1e-3)) + + slow = core.Cornering(spec, speed_kmh=2.0) + check(u"C8 低速退化为阿克曼", + near(slow.steer_angle(), slow.ackermann_angle(), 6e-4)) + + fast = core.Cornering(spec, speed_kmh=120.0) + check(u"C9 高速转向角显著变大", fast.steer_angle() > 2 * slow.steer_angle()) + + over = core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0) + diverged = core.Cornering(over, radius=50.0, speed_kmh=100.0) + check(u"C10 过临界灵敏度退化", diverged.sensitivity() is None) + check(u"C11 非法半径被拒", + raises(core.StabilityError, core.Cornering, spec, radius=0.0)) + check(u"C12 缺整车参数被拒", raises(core.StabilityError, core.Cornering, None)) + + +# ---------------------------------------------------------------- D 四条断言 +def group_asserts(): + out(u"D 四条断言") + spec = core.VehicleSpec() + corner = core.Cornering(spec) + asserts = core.check_stability(corner) + check(u"D1 断言条数是 4", len(asserts) == 4, repr(len(asserts))) + check(u"D2 断言名齐全", + tuple(a["type"] for a in asserts) == core.ASSERT_NAMES, + repr([a["type"] for a in asserts])) + check(u"D3 默认全部通过", all(a["passed"] for a in asserts), + repr([a["type"] for a in asserts if not a["passed"]])) + check(u"D4 每条都有 detail", + all(isinstance(a["detail"], (u"").__class__) and a["detail"] + for a in asserts)) + check(u"D5 轴荷 detail 含数字", u"8175.0" in asserts[0]["detail"]) + + over = core.check_stability(core.Cornering( + core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0), + speed_kmh=20.0)) + flipped = [a["type"] for a in over if not a["passed"]] + check(u"D6 过多转向只翻不足转向裕度", + flipped == [core.ASSERT_NAMES[1]], repr(flipped)) + + tight = core.check_stability(corner, ackermann_speed_kmh=60.0) + flipped = [a["type"] for a in tight if not a["passed"]] + check(u"D7 高速阿克曼只翻低速阿克曼", + flipped == [core.ASSERT_NAMES[3]], repr(flipped)) + + high = core.check_stability(corner, understeer_min=0.1) + flipped = [a["type"] for a in high if not a["passed"]] + check(u"D8 阈值抬高翻不足转向裕度", + core.ASSERT_NAMES[1] in flipped, repr(flipped)) + + +# ---------------------------------------------------------------- E 车速扫描 +def group_scan(): + out(u"E 车速扫描") + spec = core.VehicleSpec() + rows = core.speed_table(spec) + check(u"E1 扫描行数", len(rows) == 6, repr(len(rows))) + check(u"E2 首行车速", near(rows[0][0], 20.0)) + check(u"E3 末行车速", near(rows[-1][0], 120.0)) + deltas = [r[2] for r in rows] + check(u"E4 转向角单调增大", + all(deltas[i] < deltas[i + 1] for i in range(len(deltas) - 1))) + check(u"E5 40 km/h 行与工况一致", + near(rows[1][2], core.Cornering(spec).steer_angle(), 1e-12)) + sens = [r[4] for r in rows] + check(u"E6 不足转向灵敏度递增", + all(sens[i] < sens[i + 1] for i in range(len(sens) - 1))) + less = core.speed_table(spec, radius=80.0) + check(u"E7 半径变大转向角变小", less[0][2] < rows[0][2]) + over = core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0) + over_rows = core.speed_table(over) + check(u"E8 过多转向高速发散", + any(r[4] is None for r in over_rows), repr([r[4] for r in over_rows])) + + +# ---------------------------------------------------------------- F 字符图 +def group_render(): + out(u"F 字符图") + spec = core.VehicleSpec() + art = core.render_speed_table(spec) + lines = art.split("\n") + check(u"F1 有标题行", u"转向角随车速" in lines[0], lines[0]) + check(u"F2 有分隔线", lines[1].startswith("-") and lines[-1].startswith("-")) + check(u"F3 图内有 6 行数据", len(lines) == 9, repr(len(lines))) + bars = [ln.count("#") for ln in lines[2:-1]] + check(u"F4 条形单调变宽", + all(bars[i] < bars[i + 1] for i in range(len(bars) - 1)), repr(bars)) + narrow = core.render_speed_table(spec, width=8) + check(u"F5 宽度可调", + max(ln.count("#") for ln in narrow.split("\n")) <= 8) + check(u"F6 含车速单位", u"km/h" in art) + + +# ---------------------------------------------------------------- G 参数读取 +def group_params(): + out(u"G 参数读取") + params = core.StabilityParams() + check(u"G1 默认质量", near(params.spec.mass, 1500.0)) + check(u"G2 默认半径", near(params.radius, 50.0)) + check(u"G3 默认车速", near(params.speed_kmh, 40.0)) + check(u"G4 默认下限", near(params.understeer_min, 0.0)) + check(u"G5 默认阿克曼车速", near(params.ackermann_speed_kmh, 2.0)) + check(u"G6 默认容差", near(params.ackermann_tolerance, 0.02)) + check(u"G7 默认扫描车速", params.speeds == core.DEFAULT_SPEEDS_KMH) + + custom = core.StabilityParams(**{ + "vehicle/mass": 1800.0, + "tire/rear_cornering": 70000.0, + "scene/radius": 80.0, + "check/understeer_min": 0.02, + }) + check(u"G8 质量可覆盖", near(custom.spec.mass, 1800.0)) + check(u"G9 刚度可覆盖", near(custom.spec.rear_cornering, 70000.0)) + check(u"G10 半径可覆盖", near(custom.radius, 80.0)) + check(u"G11 下限可覆盖", near(custom.understeer_min, 0.02)) + check(u"G12 未覆盖项保持默认", near(custom.spec.front_cornering, 80000.0)) + check(u"G13 非法质量被拒", + raises(core.StabilityError, core.VehicleSpec, mass=-1.0)) + check(u"G14 非法阻尼被拒", + raises(core.StabilityError, core.VehicleSpec, rear_cornering=0.0)) + + +# ---------------------------------------------------------------- H 报告排版 +def group_report(): + out(u"H 报告排版") + spec = core.VehicleSpec() + corner = core.Cornering(spec) + asserts = core.check_stability(corner) + text = core.format_report(spec, corner, asserts) + check(u"H1 含整车行", u"整车:质量 1500.0 kg" in text) + check(u"H2 含轴荷行", u"轴荷:前 8175.0 N" in text) + check(u"H3 含梯度行", u"K = 0.003399" in text) + check(u"H4 含特征车速行", u"特征车速 28.184 m/s(101.5 km/h)" in text) + check(u"H5 含工况行", u"δ 0.062393 rad(3.57°)" in text) + check(u"H6 含断言汇总", u"断言:4 通过 / 0 失败(共 4 条)" in text) + check(u"H7 含结论 PASSED", u"结论:PASSED" in text) + check(u"H8 每条断言都出现", + all(a["type"] in text for a in asserts)) + + over = core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0) + over_corner = core.Cornering(over, speed_kmh=20.0) + over_asserts = core.check_stability(over_corner) + bad = core.format_report(over, over_corner, over_asserts) + check(u"H9 反例结论 FAILED", u"结论:FAILED" in bad) + check(u"H10 反例含临界车速行", u"临界车速 15.350 m/s(55.3 km/h)" in bad) + + +# ---------------------------------------------------------------- I 闭式核对 +def group_identity(): + out(u"I 闭式核对") + spec = core.VehicleSpec() + corner = core.Cornering(spec) + k = spec.gradient() + # δ = L/R + K·a_y ⇒ (δ − L/R)/K = a_y + back = (corner.steer_angle() - spec.wheelbase / corner.radius) / k + check(u"I1 稳态转向角反解", near(back, corner.lateral_accel(), 1e-12)) + # a_y/δ 与 v²/(L + K·v²) 两种写法 + v = corner.speed() + alt = v * v / (spec.wheelbase + k * v * v) + check(u"I2 灵敏度闭式一致", near(alt, corner.sensitivity(), 1e-12)) + # 转向角(°)换算 + check(u"I3 弧度转度", + near(math.degrees(corner.steer_angle()), 3.5748, 1e-3)) + # 特征车速处:L + K·v² = 2L + vchar = spec.characteristic_speed() + check(u"I4 特征车速使分母翻倍", + near(spec.wheelbase + k * vchar ** 2, 2 * spec.wheelbase, 1e-9)) + # 临界车速处:L + K·v² = 0 + over = core.VehicleSpec(front_cornering=160000.0, rear_cornering=40000.0) + vcrit = over.critical_speed() + check(u"I5 临界车速使分母为零", + near(over.wheelbase + over.gradient() * vcrit ** 2, 0.0, 1e-9)) + # 轴荷合力矩:W_f·b = W_r·a + check(u"I6 轴荷满足力矩平衡", + near(spec.front_load() * spec.cg_to_front, + spec.rear_load() * spec.cg_to_rear(), 1e-9)) + + +# ------------------------------------------------------------ J 工程文件约定 +def group_files(): + out(u"J 工程文件约定") + for rel in ("CMakeLists.txt", "package.xml", "README.md", + "config/lateral_stability_margin.yaml", "launch/main.launch", + "scripts/main.py", "scripts/stability_core.py", + "scripts/stability_node.py"): + check(u"J %s 存在" % rel, os.path.isfile(os.path.join(PKG, rel))) + + launch = io.open(os.path.join(PKG, "launch", "main.launch"), + encoding="utf-8").read() + check(u"J launch 有 rosparam load", u'= 12", launch.count("= 12, + repr(launch.count(" Date: Wed, 11 Nov 2026 19:46:45 +0800 Subject: [PATCH 3/5] Add files via upload --- docs/ground/lateral_stability_margin.md | 178 ++++++++++++++++++++++++ 1 file changed, 178 insertions(+) create mode 100644 docs/ground/lateral_stability_margin.md diff --git a/docs/ground/lateral_stability_margin.md b/docs/ground/lateral_stability_margin.md new file mode 100644 index 0000000000..66ed4e5979 --- /dev/null +++ b/docs/ground/lateral_stability_margin.md @@ -0,0 +1,178 @@ +# 不足转向梯度与侧向稳定裕度:`lateral_stability_margin` + +## 模块概述 + +`lateral_stability_margin` 把「一台车推头还是甩尾」这件事算成四个能直接读的数: +不足转向梯度 `K`、特征车速 / 临界车速、稳态转向角 `δ`、转向灵敏度 `a_y/δ`。 +它只做准静态的圆周运动,不模拟纵向载荷转移,也不需要 CARLA 或显示器—— +在任意一台装了 ROS 的机器上都能跑通并通过离线测试。 + +适合用来:对比两套轮胎调校对转向手感的影响、判断某车速下是否已经进入不稳定区、 +给转向控制器校核「该打多少方向盘」。 + +## 构建与运行 + +```bash +# ① 把包放进工作空间(结构必须是 src/ground/<包名>/...) +mkdir -p ~/catkin_ws20/src/ground +cp -r lateral_stability_margin ~/catkin_ws20/src/ground/ +ls ~/catkin_ws20/src/ground # 只应打印一行 lateral_stability_margin + +# ② 修 shebang(提交版是 python3,Melodic 需要 python2) +cd ~/catkin_ws20/src/ground/lateral_stability_margin/scripts +sed -i '1s|^#!/usr/bin/env python$|#!/usr/bin/env python2|' main.py stability_core.py stability_node.py +head -2 stability_core.py # 第 1 行 shebang、第 2 行 coding,两行都要在 +chmod +x main.py stability_core.py stability_node.py + +# ③ 编译 +cd ~/catkin_ws20 && catkin_make + +# ④ 跑 +source /opt/ros/melodic/setup.bash +source ~/catkin_ws20/devel/setup.bash +roslaunch lateral_stability_margin main.launch + +# ⑤ 反例:换成硬前软后的甩尾车 +roslaunch lateral_stability_margin main.launch front_cornering:=160000 rear_cornering:=40000 +``` + +终端 A 会打印整份报告(参数、轴荷、梯度、车速扫描表、四条断言、结论), +终端 B 用 `rosparam get` / `rostopic echo` 核参数与话题。 + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `/vehicle/mass` | 1500.0 | 整车质量 kg | +| `/vehicle/wheelbase` | 2.7 | 轴距 m | +| `/vehicle/cg_to_front` | 1.2 | 质心到前轴距离 m(必须小于轴距) | +| `/vehicle/gravity` | 9.81 | 重力加速度 m/s² | +| `/tire/front_cornering` | 80000.0 | 前轴侧偏刚度 N/rad | +| `/tire/rear_cornering` | 95000.0 | 后轴侧偏刚度 N/rad | +| `/scene/radius` | 50.0 | 转弯半径 m | +| `/scene/speed_kmh` | 40.0 | 稳态圆周车速 km/h | +| `/check/understeer_min` | 0.0 | 不足转向梯度下限 rad/(m/s²) | +| `/check/ackermann_speed_kmh` | 2.0 | 校验阿克曼角时用的低速 km/h | +| `/check/ackermann_tolerance` | 0.02 | 低速转向角与 `L/R` 的相对偏差上限 | +| `/rate/publish_hz` | 1.0 | 保活重发频率 Hz | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/stability/report` | `std_msgs/String` | 整份报告 | +| `/stability/cornering` | `std_msgs/Float64MultiArray` | `[K, a_y, δ, 阿克曼角, 超出量]` | +| `/stability/limit` | `std_msgs/Float64MultiArray` | `[特征车速, 临界车速]`(不存在的给 −1) | + +三条都是 latched(`latch=True`),所以 `rostopic echo` 一上来就能拿到最后一帧, +不用怕「订阅晚了错过消息」。 + +## 实测输出(默认参数) + +``` +整车:质量 1500.0 kg,轴距 2.70 m,质心到前轴 1.20 m(到后轴 1.50 m) +轮胎:前轮侧偏刚度 80000 N/rad,后轮 95000 N/rad +轴荷:前 8175.0 N,后 6540.0 N(合计 14715.0 N) +梯度:K = 0.003399 rad/(m/s²),特性「不足转向」 +特征车速 28.184 m/s(101.5 km/h) +工况:半径 50.0 m,车速 40.0 km/h → a_y 2.469 m/s²,δ 0.062393 rad(3.57°), + 阿克曼角 0.054000 rad(3.09°),超出 0.008393 rad +转向灵敏度 a_y/δ = 39.5740 1/s +车速(km/h) a_y(m/s²) δ(rad) δ(°) 灵敏度(1/s) + 20 0.617 0.0561 3.21 11.0036 + 40 2.469 0.0624 3.57 39.5740 + 60 5.556 0.0729 4.18 76.2246 + 80 9.877 0.0876 5.02 112.7825 + 100 15.432 0.1065 6.10 144.9628 + 120 22.222 0.1295 7.42 171.5524 +断言:4 通过 / 0 失败(共 4 条) + [PASS] 轴荷分配 前轴 8175.0 N + 后轴 6540.0 N = 14715.0 N(相对偏差 0.00e+00) + [PASS] 不足转向裕度 K = 0.003399 rad/(m/s²),特性「不足转向」,特征车速 28.184 m/s(101.5 km/h) + [PASS] 低速阿克曼 车速 2.0 km/h 时转向角 0.054021 rad,阿克曼角 0.054000 rad(相对偏差 0.0004) + [PASS] 稳态转向角 δ = 0.062393 rad → 反算 a_y = 2.469136 m/s²(相对偏差 1.80e-16) +结论:PASSED +``` + +## 两个反例 + +**反例一:硬前软后 → 甩尾车** + +```bash +roslaunch lateral_stability_margin main.launch front_cornering:=160000 rear_cornering:=40000 +``` + +``` +梯度:K = -0.011458 rad/(m/s²),特性「过多转向」 +临界车速 15.350 m/s(55.3 km/h) +[FAIL] 不足转向裕度 K = -0.011458 rad/(m/s²)(下限 0.0000), + 特性「过多转向」,临界车速 15.350 m/s(55.3 km/h) +断言:3 通过 / 1 失败(共 4 条) +结论:FAILED +``` + +后轴太软,后轮先到附着极限,梯度转负、临界车速只有 55.3 km/h。四条断言只翻 +「不足转向裕度」这一条 —— 其余三条(轴荷、低速阿克曼、稳态转向角的闭环) +与梯度符号无关,所以照旧通过。 + +**反例二:把阿克曼校验放在 60 km/h** + +```bash +roslaunch lateral_stability_margin main.launch ackermann_speed_kmh:=60 +``` + +``` +[FAIL] 低速阿克曼 车速 60.0 km/h 时转向角 0.072884 rad,阿克曼角 0.054000 rad + (相对偏差 0.3498,容差 0.020) +``` + +60 km/h 时 `K·a_y` 已经占了转向角的 26%,再用「阿克曼角」当参考就完全不成立。 +这条断言的意义正在于此:它只在低速(`K·a_y → 0`)时才有资格叫“阿克曼”。 + +## 离线测试 + +```bash +cd ~/catkin_ws20 +python3 tests/test_lateral_stability_margin_local.py +``` + +覆盖 A 几何与轴荷、B 梯度与转向特性、C 稳态圆周、D 四条断言、E 车速扫描、 +F 字符图、G 参数读取、H 报告排版、I 闭式核对、J 工程文件约定、K 编码声明回归。 + +> K 组是专门为「Python 2.7 找不到编码声明」这个坑加的回归:它会检查三个脚本的 +> **第 1 行是 shebang、第 2 行是 coding**。手册里那条 `sed` 用的是锚定替换 +> `1s|^#!/usr/bin/env python$|#!/usr/bin/env python2|`,只会改 shebang,不会 +> 把编码声明顶掉。 + +## 结构约定 + +``` +lateral_stability_margin/ +├── CMakeLists.txt +├── package.xml +├── README.md +├── config/lateral_stability_margin.yaml # 12 个默认值 +├── launch/main.launch # 12 个带 doc 的 arg + 同名 param +├── scripts/ +│ ├── main.py # 入口,转发到 node +│ ├── stability_core.py # 纯标准库,离线可测 +│ └── stability_node.py # rospy 侧:参数、发布、保活 +└── tests/ # 由仓库根的 tests/ 放 +``` + +## 常见问题 + +**1. 特征车速算出来只有 30 km/h?** +`K` 忘了除以 `g`。`W/C` 的量纲是 rad,而公式里 `K` 的单位是 rad/(m/s²)。 + +**2. `特stable` 报 `UnicodeEncodeError`?** +节点里给 rospy 的日志必须是 byte str。本模块所有日志都走 `_log()`,它会先 +`encode("utf-8")`。自己加日志时别直接 `rospy.loginfo(u"中文")`。 + +**3. `rosparam get /tire/rear_cornering` 还是 95000?** +`` 只对 launch 内部可见,带斜杠的名字必须落成同名 `` 才能被 +`rosparam get` 读到;带斜杠的 `roslaunch pkg file.launch tire/rear_cornering:=…` +会被**静默忽略**,要写裸名 `rear_cornering:=…`。 + +## 大模型声明 + +本模块的代码编写、测试编写与文档整理使用了 AI 编程助手辅助,提交内容由本人负责。 From 5afae0ed95a3bbea6ae900201f83cddc7693452a Mon Sep 17 00:00:00 2001 From: Wist-sutton <3473325902@qq.com> Date: Wed, 11 Nov 2026 19:47:35 +0800 Subject: [PATCH 4/5] Update mkdocs.yml --- mkdocs.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/mkdocs.yml b/mkdocs.yml index e20a6daa36..802186d041 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -221,6 +221,7 @@ nav: - 转弯扫掠校验: './ground/turning_sweep_check.md' - 高精度定位与重定位(AMCL 式粒子滤波): './ground/turtlebot3_relocalization.md' - 速度指令安全监督: './ground/cmd_vel_guard.md' + - 不足转向与横向稳定裕度: './ground/lateral_stability_margin.md' - 空域载具: - MuJoCo 城市屋顶光伏覆盖率测量: './air/mujoco_uav_rooftop_pv_coverage_survey.md' From 1d58237b5693268ce421610fe915f7e48d8ac4a0 Mon Sep 17 00:00:00 2001 From: Wist-sutton <3473325902@qq.com> Date: Wed, 11 Nov 2026 19:48:19 +0800 Subject: [PATCH 5/5] Add section on understeering and lateral stability margin --- docs/index.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/index.md b/docs/index.md index fcfb8c3e75..c637853cc7 100644 --- a/docs/index.md +++ b/docs/index.md @@ -228,6 +228,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c * [前视相机车道偏离预警(视觉横向偏移与地图真值对照)](./ground/carla_visual_lane_departure_warning.md) —— 用前视图像估计横向偏移,并与地图真值逐帧对照 * [差速 AGV 坡道起步辅助(防溜车)](./ground/hill_start_assist.md) —— MuJoCo 12° 斜坡,四轮轮角保持 PD + 坡度前馈,松刹车 3 s 后溜 2.18 m 降到 0.008 m * [地面移动底盘速度指令安全监督](./ground/cmd_vel_guard.md) —— 对 `/cmd_vel` 做限幅、急停、超时与非法值拦截,无需仿真环境 +* [不足转向与横向稳定裕度](./ground/lateral_stability_margin.md) —— 由前后侧偏刚度算不足转向梯度、特征车速与稳态转向角 ## 5. 空域载具