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. 空域载具