diff --git a/.github/workflows/ground_module_tests.yml b/.github/workflows/ground_module_tests.yml new file mode 100644 index 0000000000..97d80e7cd7 --- /dev/null +++ b/.github/workflows/ground_module_tests.yml @@ -0,0 +1,62 @@ +name: Ground module offline tests + +# 为 5 个只依赖 numpy 的地面动力学/操稳模块补 PR 级回归保护。 +# 全部测试都是纯 Python 离线自检,不需要 ROS / 仿真器 / GPU。 +on: + push: + paths: + - 'src/ground/tire_slip_curve/**' + - 'src/ground/rollover_margin/**' + - 'src/ground/drive_cycle_energy/**' + - 'src/ground/ride_comfort_transmissibility/**' + - 'src/ground/lateral_stability_margin/**' + - 'tests/test_tire_slip_curve_local.py' + - 'tests/test_rollover_margin_local.py' + - 'tests/test_drive_cycle_energy_local.py' + - 'tests/test_ride_comfort_transmissibility_local.py' + - 'tests/test_lateral_stability_margin_local.py' + - '.github/workflows/ground_module_tests.yml' + pull_request: + paths: + - 'src/ground/tire_slip_curve/**' + - 'src/ground/rollover_margin/**' + - 'src/ground/drive_cycle_energy/**' + - 'src/ground/ride_comfort_transmissibility/**' + - 'src/ground/lateral_stability_margin/**' + - 'tests/test_tire_slip_curve_local.py' + - 'tests/test_rollover_margin_local.py' + - 'tests/test_drive_cycle_energy_local.py' + - 'tests/test_ride_comfort_transmissibility_local.py' + - 'tests/test_lateral_stability_margin_local.py' + - '.github/workflows/ground_module_tests.yml' + +jobs: + test: + name: ${{ matrix.test }} + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + test: + - test_tire_slip_curve_local.py + - test_rollover_margin_local.py + - test_drive_cycle_energy_local.py + - test_ride_comfort_transmissibility_local.py + - test_lateral_stability_margin_local.py + steps: + - uses: actions/checkout@v4 + + - name: Set up Python 3.10 + uses: actions/setup-python@v5 + with: + python-version: '3.10' + + - name: Install dependencies + run: | + python3 -m pip install --upgrade pip + python3 -m pip install numpy pyyaml + + - name: Run offline test + env: + PYTHONIOENCODING: utf-8 + run: python3 tests/${{ matrix.test }} diff --git a/docs/ground/drive_cycle_energy.md b/docs/ground/drive_cycle_energy.md new file mode 100644 index 0000000000..fca04a3c8c --- /dev/null +++ b/docs/ground/drive_cycle_energy.md @@ -0,0 +1,207 @@ +# 工况能耗与续航核算(`drive_cycle_energy`) + +> 一段行驶工况里,电池的电到底花在哪里?滚阻、风阻、制动损失、动能各占多少? +> 再生制动能收回多少?满电能跑多远? +> 本模块用纵向动力学把这几件事算成一组**可断言的数字**,并在终端打印能量条。 + +## 模块概述 + +输入是一段**工况**(每段写成「时长 + 起始车速 + 结束车速」)和一组整车参数, +输出是: + +- 驱动能量、制动能量、回收能量、净耗电量; +- 能量分解:滚阻 / 风阻 / 制动损失 / 动能净变化; +- 百公里电耗与满电续航; +- 四条断言(能量分解自洽、回收上限、续航自洽、风阻随车速上升)。 + +**为什么值得单独做一个模块**:运动学模块(如 `speed_profile_check`)能算出 +「什么时候该加速、什么时候该减速」,但它不算阻力,也就无法回答「这么开费多少电」。 +本模块补上纵向阻力与电池模型,并且因为能量分解可以写成闭式恒等式, +「实现对不对」能被断言直接卡住,不需要仿真器。 + +## 构建与运行 + +本包是纯 Python 的 catkin 包,`catkin_make` 只做安装,不编译 C++: + +```bash +cd ~/catkin_ws18 +catkin_make +source /opt/ros/melodic/setup.bash +source devel/setup.bash +roslaunch drive_cycle_energy main.launch +``` + +命令行覆盖参数(优先级:命令行 > yaml > 代码默认值): + +```bash +roslaunch drive_cycle_energy main.launch regen_eff:=0.0 +roslaunch drive_cycle_energy main.launch regen_limit:=0.5 +roslaunch drive_cycle_energy main.launch battery_kwh:=30.0 +``` + +## 模型 + +纵向动力学(忽略坡度,纵向加速度记为 a): + +``` +驱动力 F = m·a + m·g·c_rr + 0.5·ρ·Cd·A·v² +滚阻力 F_rr = m·g·c_rr +风阻力 F_aero = 0.5·ρ·Cd·A·v² +``` + +能量按时间积分: + +``` +E_drive = ∫ max(F, 0)·v dt 驱动能量 +E_brake = ∫ max(−F, 0)·v dt 制动能量(被消耗掉的机械能) +E_regen = η · E_brake 再生制动回收 +E_net = E_drive − E_regen 净耗电 +``` + +核心恒等式(第一条断言的依据): + +``` +E_drive = E_roll + E_aero + E_brake + ΔKE +``` + +其中 `ΔKE = 0.5·m·(v_end² − v_start²)` 是整段工况的动能净变化。 +滚阻与风阻都能写成速度的幂函数(`v` 与 `v³`),所以三项都可以逐点积出来再对账。 + +续航: + +``` +百公里电耗 = E_net / 3.6e6 / (距离 / 100 km) [kWh/100km] +续航 = 电池容量 / 百公里电耗 × 100 [km] +``` + +## 四条断言 + +| 断言 | 判据 | 失败意味着 | +|---|---|---| +| **能量分解** | 滚阻+风阻+制动+动能 与驱动能量的相对偏差 ≤ `breakdown_tolerance` | 积分或阻力模型实现错了 | +| **回收上限** | `E_regen / E_brake ≤ regen_limit` | 回收效率配置不合理(η 越界或断言被放宽) | +| **续航自洽** | 由百公里电耗反推的续航与直接算的一致 | 折算公式实现不一致 | +| **风阻上升** | 30 m/s 风阻 > 10 m/s 风阻 | 风阻没按 v² 算(最常见是把线性项当二次项) | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `vehicle/mass` | 1800.0 | 整备质量 kg | +| `vehicle/c_rr` | 0.012 | 滚动阻力系数 | +| `vehicle/cd` | 0.31 | 风阻系数 | +| `vehicle/area` | 2.2 | 迎风面积 m² | +| `vehicle/battery_kwh` | 60.0 | 电池可用容量 kWh | +| `vehicle/regen_eff` | 0.6 | 再生制动效率 0~1 | +| `check/breakdown_tolerance` | 0.02 | 能量分解相对偏差上限 | +| `check/regen_limit` | 1.0 | 回收 / 制动 上限 | +| `rate/publish_hz` | 10.0 | 保活循环频率 Hz | +| `topic/report` | `/energy/report` | 文字报告话题 | +| `topic/breakdown` | `/energy/breakdown` | 逐项能量 CSV | +| `topic/range` | `/energy/range` | 满电续航 km | + +`launch/main.launch` 为每个参数都声明了带 `doc` 的 `` 并写回同名 ``, +所以命令行覆盖一定生效(不像写错的 `$(rospack find …)` 那样静默失败)。 + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/energy/report` | `std_msgs/String` | 完整文字报告(latched) | +| `/energy/breakdown` | `std_msgs/String` | `item,joules` 七行 CSV(latched) | +| `/energy/range` | `std_msgs/Float64` | 满电续航 km(latched) | + +```bash +rostopic echo -n 1 /energy/report +rostopic echo -n 1 /energy/breakdown +rostopic echo -n 1 /energy/range +``` + +## 实测输出(默认工况) + +默认工况 8 段共 60 s(0→30、30 匀速、30→50、50 匀速、50→0、怠速、0→40、40 匀速), +行驶 512.5 m,最高 50 km/h: + +``` +工况:60.0 s,行驶 512.5 m,最高 50.0 km/h +整车:质量 1800 kg,滚阻 0.012,Cd·A 0.68 m²,电池 60.0 kWh,回收 η 0.60 +能量:驱动 407.3 kJ,制动 159.6 kJ,回收 95.8 kJ,净耗 311.5 kJ +分解:滚阻 108.6 kJ(26.7%),风阻 28.0 kJ(6.9%),制动 159.6 kJ(39.2%),动能 111.1 kJ(27.3%) +续航:百公里 16.89 kWh,满电 355.3 km +断言:4 通过 / 0 失败(共 4 条) + [PASS] 能量分解 … 相对偏差 0.0000,容差 0.02 + [PASS] 回收上限 回收 95.8 / 制动 159.6 = 0.6000(上限 1.00,η=0.60) + [PASS] 续航自洽 百公里 16.885 kWh,电池 60.0 kWh → 续航 355.3 km + [PASS] 风阻上升 10 m/s 风阻 40.9 N,30 m/s 风阻 368.3 N(期望上升) +结论:PASSED +``` + +能量条(驱动为满条,其余按比例): + +``` +滚阻|###########| 108.6 kJ +风阻|###| 28.0 kJ +制动|################| 159.6 kJ +动能|###########| 111.1 kJ +驱动|##########################################| 407.3 kJ +回收|##########| 95.8 kJ +``` + +## 两个反例 + +**反例一**:`regen_limit:=0.5` —— 回收比 0.6000 超过上限,只翻「回收上限」,其余三条通过。 + +**反例二**:`regen_eff:=0.0` —— 没有回收: + +| 量 | 默认(η=0.6) | 无回收(η=0) | +|---|---|---| +| 净耗 | 311.5 kJ | **407.3 kJ** | +| 满电续航 | 355.3 km | **271.8 km** | + +注意四条断言**依然全过**:它们校验的是自洽性,不是「这台车好不好」。 +想知道回收值不值,就是要像上表这样对比两次运行 —— 这正是把断言和结论分开的价值。 + +## 离线测试 + +```bash +python3 tests/test_drive_cycle_energy_local.py +``` + +**通过 152 项,失败 0 项**(Python 3.6.9 / 3.12 均可),11 组: +参数校验、阻力计算、工况解析、工况积分、四条断言、能量恒等式、续航与百公里、 +字符图、报告文本、参数对象、工程文件约定。 + +其中 K 组是本项目踩坑后的回归:**逐个检查 `scripts/*.py` 的第 1 行是 shebang、 +第 2 行是 `# -*- coding: utf-8 -*-`**。Melodic 的 Python 2.7 只在前两行找编码声明, +用 `sed -i '1s|.*|#!/usr/bin/env python2|'` 整行覆盖会把编码声明顶掉, +节点一启动就报 `SyntaxError: Non-ASCII character ... but no encoding declared`。 +现在的写法是带锚点的 `sed -i '1s|^#!/usr/bin/env python$|#!/usr/bin/env python2|'`, +并且三个脚本都保证「第 1 行 shebang、第 2 行编码声明」。 + +## 与其他模块的分工 + +| 模块 | 回答的问题 | +|---|---| +| `speed_profile_check` | 一段距离怎么分配加减速时间(运动学,无阻力) | +| `braking_distance_check` | 从看见到停住要几米(纵向制动,无回收) | +| `lateral_stability_margin` | 转弯稳不稳(横向,与能耗无关) | +| **本模块** | **纵向能量流:滚阻 / 风阻 / 制动 / 动能分配 + 续航** | + +## 常见问题 + +**Q:`rostopic echo` 卡住了?** +latched 话题会持续重发最后一条,加 `-n 1` 取一条。 + +**Q:`rosparam get` 提示 `is not set`?** +说明 `roslaunch` 已经退出;重跑 launch,让它挂着别关。 + +**Q:想用自己的工况?** +改 `scripts/energy_core.py` 末尾的 `DEFAULT_CYCLE`,每段是 +`(时长秒, 起始 km/h, 结束 km/h)`;或在交互式 Python 里直接 +`core.simulate(spec, [(5.0, 0.0, 60.0)])` 试算。 + +**Q:为什么没有 slope(坡度)?** +为了让能量分解保持闭式、可断言;坡度会把 `m·g·sinθ·v` 这一项混进来, +需要在 `traction_force` 里再加一个参数,属于可扩展项,不影响当前结论。 + +> 所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 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 编程助手辅助,提交内容由本人负责。 diff --git a/docs/ground/ride_comfort_transmissibility.md b/docs/ground/ride_comfort_transmissibility.md new file mode 100644 index 0000000000..02fddfd12c --- /dev/null +++ b/docs/ground/ride_comfort_transmissibility.md @@ -0,0 +1,197 @@ +# 乘坐舒适性传递率 + +路面不是平的。同一辆车,40 km/h 过某段路很难受,60 km/h 反而平稳 —— 这不是错觉, +是**悬架对不同频率的输入放大倍数不同**。把这件事算清楚,就是本模块要讲的传递率。 + +这个模块用纯标准库实现一个单自由度悬架模型,给出共振在哪个频率、放大多少倍、 +按 ISO 2631-1 属于哪个舒适度等级,并用四条断言把「算得对不对」钉住。 +不需要 CARLA、不需要显示器、不需要 numpy,Ubuntu 18.04 + ROS Melodic 直接可跑。 + +## 一、为什么需要这个包 + +仓库里 `src/ground/` 下的地面载具模块,纵向有 `braking_distance_check`、横向有 +`rollover_margin` 与 `lateral_stability_margin`,**垂直方向是空白**: + +| 方向 | 已有模块 | 管什么 | +|---|---|---| +| 纵向 | `braking_distance_check` | 反应距离 + 制动距离 | +| 横向(侧翻) | `rollover_margin` | 载荷转移与临界车速 | +| 横向(转向) | `lateral_stability_margin` | 不足转向梯度与特征车速 | +| 执行器 | `steering_actuator_lag` | 转向机构的一阶滞后 | +| **垂直** | **本模块** | **悬架传递率、共振、舒适度等级** | + +一条真实的工程链条是:悬架把路面震动放大到人体 → 人难受 → 驾驶员反应变慢 → +制动距离变长。前面三个模块都假设「驾驶员状态正常」,本模块补上这个前提被破坏时的量化依据。 + +## 二、模型 + +单自由度基础激励(只看垂直方向,忽略俯仰与侧倾耦合): + +``` +r = f / f_n 频率比(激励频率 / 固有频率) +T(r) = sqrt((1 + (2ζr)²) / ((1 − r²)² + (2ζr)²)) 位移传递率 +a_w = T(r) · a_in 加权加速度 RMS +``` + +三个解析结论: + +``` +峰值位置 r_p = sqrt(1 − 2ζ²) +峰值大小 T_p = sqrt((1 + 4ζ²(1−2ζ²)) / (4ζ²(1−ζ²))) +低频/高频 T(0) = 1;T(∞) = 0 +``` + +**峰值不在 r = 1**。r = 1 处 `T = sqrt(1+(2ζ)²)/(2ζ)`,ζ = 0.25 时是 2.236; +真正的极值在 r_p = 0.935,峰值 2.280,高出约 2%。阻尼越大偏得越多。 + +`damping` 被限制在 `(0, 1/√2)`:超过 0.7071 后 `sqrt(1−2ζ²)` 无实数意义,闭式峰位失效, +程序直接抛 `RideError`,而不是给一个错的数。现实里的悬架阻尼比也不会到 0.8。 + +舒适度等级(ISO 2631-1,`a_w` 单位 m/s² RMS): + +| a_w 区间 | 等级 | +|---|---| +| < 0.315 | 舒适 | +| 0.315 ~ 0.63 | 略有不适 | +| 0.63 ~ 1.0 | 不适 | +| 1.0 ~ 1.6 | 很不适 | +| 1.6 ~ 2.5 | 极不适 | +| > 2.5 | 无法忍受 | + +## 三、四条断言 + +| 断言 | 判据 | 默认实测 | 结果 | +|---|---|---|---| +| 共振位置 | 扫描峰位与 `sqrt(1−2ζ²)` 的偏差 ≤ 0.02 | 偏差 0.0132 | PASS | +| 峰值倍率 | 扫描峰值与闭式峰值的相对偏差 ≤ 5% | 偏差 0.0012(0.12%) | PASS | +| 高频衰减 | `T(r=10) ≤ 0.1` | 0.051440 | PASS | +| 输入线性 | 输入加倍后 `a_w` 精确加倍 | 相对偏差 0.0000 | PASS | + +四条互相独立:位置管「峰在哪」、倍率管「峰多高」、衰减管「隔振行不行」、 +线性管「叠加原理成不成立」。任何一条失败都能定位到具体环节。 + +## 四、默认数字 + +参数:`fn_hz = 1.2`、`damping = 0.25`、`input_accel = 0.5`、`freq_hz = 6.0` + +| 项 | 值 | +|---|---| +| 理论峰位 / 峰值 | **0.935414 / 2.280351** | +| 扫描峰位 / 峰值 | 0.948625 / 2.283149 | +| r = 1 处传递率 | 2.236068 | +| r = 10 处传递率 | 0.051440 | +| 6 Hz 处加权加速度 | 0.055794 m/s² → 等级「舒适」 | + +扫描表(10 个频率比,报告里原样打印): + +| r | f Hz | T | a_w m/s² | +|---|---|---|---| +| 0.25 | 0.300 | 1.066 | 0.533 | +| 0.50 | 0.600 | 1.304 | 0.652 | +| 0.75 | 0.900 | 1.853 | 0.927 | +| 1.00 | 1.200 | 2.236 | 1.118 | +| 1.25 | 1.500 | 1.402 | 0.701 | +| 1.50 | 1.800 | 0.857 | 0.429 | +| 2.00 | 2.400 | 0.447 | 0.224 | +| 3.00 | 3.600 | 0.221 | 0.111 | +| 5.00 | 6.000 | 0.112 | 0.056 | +| 10.00 | 12.000 | 0.051 | 0.026 | + +`r ≤ 1.5` 时 T > 1(放大),`r ≥ 2` 后迅速掉下来(隔振)。这就是过减速带要慢、 +跑高速反而稳的物理原因。 + +## 五、参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `/suspension/fn_hz` | 1.2 | 悬架固有频率 Hz | +| `/suspension/damping` | 0.25 | 阻尼比,必须在 (0, 0.7071) 开区间 | +| `/road/input_accel` | 0.5 | 路面输入加速度 RMS m/s² | +| `/road/freq_hz` | 6.0 | 关心的激励频率 Hz | +| `/check/pos_tolerance` | 0.02 | 峰位容差(频率比) | +| `/check/peak_tolerance` | 0.05 | 峰值相对容差 | +| `/check/high_freq_limit` | 0.1 | r=10 时传递率上限 | +| `/check/linear_tolerance` | 0.02 | 输入加倍的线性容差 | +| `/scan/low` `/scan/high` `/scan/steps` | 0.05 / 4.0 / 400 | 扫描区间与步数 | +| `/rate/publish_hz` | 1.0 | 保活重发频率 Hz | + +## 六、话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/ride/report` | `std_msgs/String` | 整份报告(含断言与结论) | +| `/ride/curve` | `std_msgs/Float64MultiArray` | 每个频率比三元组 `[r, T, a_w]`,共 30 个数 | +| `/ride/peak` | `std_msgs/Float64MultiArray` | `[理论峰位, 理论峰值, 扫描峰位, 扫描峰值, 激励处 a_w]` | + +三条都 `latch=True`。 + +## 七、运行 + +```bash +mkdir -p ~/catkin_ws19/src/ground ~/catkin_ws19/tests ~/catkin_ws19/docs/ground +# 把包放进来,改 shebang,catkin_make,source,roslaunch +roslaunch ride_comfort_transmissibility main.launch +``` + +默认输出 `结论:PASSED`。两个反例: + +```bash +# ① 容差收到 0.001:扫描精度不够,只翻「共振位置」 +roslaunch ride_comfort_transmissibility main.launch pos_tolerance:=0.001 + +# ② 阻尼 0.02(坏减振器)+ 激励挪到 1.2 Hz:峰值 25 倍,等级「无法忍受」 +roslaunch ride_comfort_transmissibility main.launch damping:=0.02 freq_hz:=1.2 +``` + +反例②值得多看一眼:四条断言**全部通过**(物理上它自洽),但舒适等级从「舒适」 +变成「无法忍受」。**断言管的是「算得对不对」,等级管的是「人坐着难受不难受」** —— +两者不能互相替代,这也是我特意把等级做成独立输出的原因。 + +## 八、离线测试 + +```bash +python3 tests/test_ride_comfort_transmissibility_local.py +``` + +**通过 112 项,失败 0 项**,覆盖: + +| 组 | 内容 | +|---|---| +| A | 传递率基本形(T(0)=1、T(1)、单调段) | +| B | 闭式峰位与峰值 | +| C | 舒适度等级分档 | +| D | 四条断言的正反例 | +| E | 扫描表数值与单调性 | +| F | 字符图排版 | +| G | 参数读取与非法值拒绝 | +| H | 报告排版(表头、行数、结论) | +| I | 闭式恒等式(r_p 处取 T 就是 T_p) | +| J | 三个脚本的工程约定(shebang、coding、保活) | +| K | 编码声明回归(防 `Non-ASCII character` 报错) | + +## 九、与已有模块的分工 + +前面第一节的表已经列了纵向/横向/执行器/垂直四块。补一句边界: +`steering_actuator_lag` 也是「频率响应」,但它是**转向机构**的一阶惯性; +本模块是**悬架**的二阶共振。两者都有「频率越高越跟不上」,但物理对象、微分方程阶数、 +关心的频段完全不同(转向 2~10 Hz,车身 1~2 Hz)。 + +## 十、常见问题 + +**1. 为什么峰值不在 r = 1?** +因为 r = 1 只是「分母实部为 0」的位置,不是传递率对 r 的极值点。 +`T(1) = sqrt(1+(2ζ)²)/(2ζ)`,极值点在 `r = sqrt(1−2ζ²)`,ζ = 0.25 时差 1.3%。 +判据里写「峰位」而不是直接拿 r = 1 当峰,就是为了不让这个偏差蒙混过去。 + +**2. 为什么 `damping:=0.8` 直接报错?** +超过 `1/√2 ≈ 0.7071` 后曲线没有经典共振峰,`sqrt(1−2ζ²)` 是虚数。 +与其返回一个错的峰位,不如判参数非法。测试 G15 就是查这个事情。 + +**3. 高通/低通行为怎么读?** +`r → 0` 时 `T → 1`(车身跟着路面一起动,等于没悬架);`r → ∞` 时 `T → 0` +(高频震动被滤掉)。所以舒适性设计的目标是:把共振峰挪到人最不敏感的低频、 +同时把峰压下去 —— 后者靠加阻尼,前者靠改固有频率(也就是改悬架刚度与簧上质量)。 + +## 十一、大模型使用声明 + +本模块的代码编写、测试编写与文档整理使用了 AI 编程助手辅助,提交内容由本人负责。 diff --git a/docs/ground/rollover_margin.md b/docs/ground/rollover_margin.md new file mode 100644 index 0000000000..2545caab56 --- /dev/null +++ b/docs/ground/rollover_margin.md @@ -0,0 +1,188 @@ +# 横向载荷转移与侧翻裕度 + +## 这一页解决什么问题 + +「这车会不会翻」听起来像仿真问题,准静态下其实只由三个参数决定:轮距、质心高度、质量。 +本页给出纯代数、可断言、不依赖 CARLA 的实现,并把 SSF、临界加速度、内外侧轮垂向力、 +极限车速算成具体数字,在终端里画出横向载荷条与车速扫描表。 + +## 模型 + +``` +SSF = track / (2·h_cg) 静态稳定因子 +a_crit = SSF · g 侧翻临界侧向加速度 +ΔF_z = m · a_y · h_cg / track 横向载荷转移 +F_inner = m·g/2 − ΔF_z +F_outer = m·g/2 + ΔF_z +v_max = sqrt(a_crit · R) 给定半径的极限车速 +``` + +两个自洽关系(也是断言的依据): + +- 抬轮发生在 `ΔF_z = m·g/2`,即 `a_y = a_crit`; +- `ΔF_z / F_z0` 恒等于 `a_y / a_crit`。 + +注意 `F_inner + F_outer = m·g` 始终成立(垂向力守恒),这是模型自检的第一条。 + +## 默认参数下的数字 + +车辆:轮距 `1.54 m`、质心高 `0.55 m`、质量 `1500 kg`;工况:半径 `30 m`、车速 `60 km/h`。 + +| 量 | 值 | +|---|---| +| SSF | `1.4000` | +| 临界加速度 | `13.734 m/s²`(1.40 g) | +| 侧向加速度 | `9.259 m/s²`(`v²/R`) | +| 裕度 `a_y/a_crit` | `0.674` | +| 载荷转移 | `4960.3 N`(占静态 67.4%) | +| 内侧轮 | `2397.2 N` | +| 外侧轮 | `12317.8 N` | +| 极限车速 | `73.1 km/h`(余量 13.1 km/h) | + +车速扫描(R = 30 m): + +| 车速 | `a_y` | 裕度 | 内侧轮 | 抬轮 | +|---|---|---|---|---| +| 30 | 2.315 | 0.169 | 6117.4 | 否 | +| 40 | 4.115 | 0.300 | 5152.9 | 否 | +| 50 | 6.430 | 0.468 | 3912.8 | 否 | +| 60 | 9.259 | 0.674 | 2397.2 | 否 | +| 70 | 12.603 | 0.918 | 606.0 | 否 | +| 80 | 16.461 | 1.199 | −1460.8 | **是** | +| 90 | 20.833 | 1.517 | −3803.2 | **是** | + +抬轮发生在 70~80 km/h 之间,与极限车速 73.1 km/h 吻合。 + +## 四条断言 + +| 断言 | 判据 | 失败意味着 | +|---|---|---| +| **侧翻裕度** | `a_y/a_crit ≤ rollover_margin`(默认 0.70) | 过弯太快或车太高 | +| **内侧轮载荷** | `F_inner > 0` | 已抬轮,准静态模型失效 | +| **极限车速** | `v ≤ sqrt(a_crit·R)` | 车速超过该半径的物理上限 | +| **载荷转移** | `|ΔF_z/F_z0 − a_y/a_crit| ≤ tolerance` | 模型实现不自洽 | + +前三条描述安全性,第四条描述实现正确性 —— 所以抬高质心会让前三条一起翻,第四条依然通过。 +这种「安全类断言联动、自洽类断言独立」的分工,正是把物理关系做成断言的价值。 + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `vehicle/track` | `1.54` | 轮距(m) | +| `vehicle/cg_height` | `0.55` | 质心高度(m) | +| `vehicle/mass` | `1500.0` | 质量(kg) | +| `vehicle/gravity` | `9.81` | 重力加速度 | +| `corner/radius` | `30.0` | 弯道半径(m) | +| `corner/speed_kmh` | `60.0` | 过弯车速(km/h) | +| `check/rollover_margin` | `0.70` | 裕度上限 | +| `check/transfer_tolerance` | `0.02` | 自洽性容差 | +| `rate/publish_hz` | `10.0` | 挂起循环频率 | +| `topic/report` | `/rollover/report` | 报告 | +| `topic/loads` | `/rollover/loads` | 车速扫描 CSV | +| `topic/critical_speed` | `/rollover/critical_speed` | 极限车速 | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/rollover/report` | `std_msgs/String`(latched) | 完整报告(含载荷条与扫描表) | +| `/rollover/loads` | `std_msgs/String`(latched) | `speed_kmh,lateral_accel,inner_load,outer_load,lift_off` 共 7 行 | +| `/rollover/critical_speed` | `std_msgs/Float64`(latched) | 该半径极限车速(km/h) | + +## 运行 + +```bash +cd ~/catkin_ws17 +catkin_make +source devel/setup.bash +roslaunch rollover_margin main.launch +``` + +## 实际输出(默认) + +``` +车辆:轮距 1.54 m,质心高 0.55 m,质量 1500 kg +SSF = 1.4000,侧翻临界加速度 13.734 m/s²(1.40 g) +工况:半径 30.0 m,车速 60.0 km/h(16.67 m/s) +侧向加速度 9.259 m/s²,裕度比值 0.674 +载荷转移 4960.3 N(占比 0.674),内侧 2397.2 N / 外侧 12317.8 N +该半径极限车速 73.1 km/h(余量 13.1 km/h) +断言:4 通过 / 0 失败(共 4 条) + [PASS] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.70) + [PASS] 内侧轮载荷 内侧轮 2397.2 N,外侧轮 12317.8 N(静态 7357.5 N) + [PASS] 极限车速 当前 60.0 km/h,该半径极限 73.1 km/h(余量 13.1 km/h) + [PASS] 载荷转移 ΔF_z/F_z0 = 0.6742,a_y/a_crit = 0.6742 +结论:PASSED +``` + +## 两个反例 + +**反例一:裕度上限收到 0.5** —— 只翻第一条 + +```bash +roslaunch rollover_margin main.launch rollover_margin:=0.5 +``` + +``` +[FAIL] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.50) +断言:3 通过 / 1 失败 +结论:FAILED +``` + +**反例二:质心抬到 0.90 m** —— 安全相关的三条一起翻 + +```bash +roslaunch rollover_margin main.launch cg_height:=0.90 +``` + +``` +SSF = 0.8556,侧翻临界加速度 8.393 m/s²(0.86 g) +[FAIL] 侧翻裕度 a_y 9.259 m/s²,a_crit 8.393 m/s²,比值 1.103(上限 0.70) +[FAIL] 内侧轮载荷 内侧轮 -759.4 N,外侧轮 15474.4 N(静态 7357.5 N) +[FAIL] 极限车速 当前 60.0 km/h,该半径极限 57.1 km/h(余量 -2.9 km/h) +[PASS] 载荷转移 ΔF_z/F_z0 = 1.1032,a_y/a_crit = 1.1032 +断言:1 通过 / 3 失败 +结论:FAILED +``` + +内侧轮载荷为负,物理含义是这一侧已经离地 —— 准静态模型到此为止。 + +## 离线测试 + +```bash +cd ~/catkin_ws17 +python3 tests/test_rollover_margin_local.py +``` + +只依赖标准库;在仓库根目录执行时还会校验本页是否存在、断言名是否齐全。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `tire_slip_curve` | 附着系数从哪来 | +| `braking_distance_check` | 纵向距离 | +| `turning_sweep_check` | 转弯时的几何包络(车身占多宽) | +| **`rollover_margin`(本页)** | **横向载荷与稳定性(会不会抬轮、极限多快)** | + +`turning_sweep_check` 回答「这个弯能不能转过去(不压线)」,本页回答「这个弯能不能转过去 +(不翻车)」—— 一个管空间,一个管稳定性。 + +## 常见问题 + +**Q:为什么不建模悬架与轮胎侧偏?** +A:准静态阈值主要由 `track/2h_cg` 决定,悬架柔度一般让实际阈值低 5~10%。本页要把公式 +讲清楚,闭式解更容易校验;真车标定时把 `rollover_margin` 收到 0.6 即可覆盖这一偏差。 + +**Q:为什么「极限车速」和「侧翻裕度」会同时失败?** +A:二者物理同源:裕度到 1 时 `v = sqrt(a_crit·R)` 刚好达到。所以抬高质心会让它们一起翻, +这不是实现问题,而是模型的必然结论。 + +**Q:能不能直接给一个「安全车速」?** +A:可以,`safe_speed(margin)` 就是干这个的:默认裕度 0.7 时该车在该半径上的安全车速是 +`61.1 km/h`(更保守的 0.5 对应 `51.7 km/h`)。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/docs/ground/tire_slip_curve.md b/docs/ground/tire_slip_curve.md new file mode 100644 index 0000000000..29a7872eb9 --- /dev/null +++ b/docs/ground/tire_slip_curve.md @@ -0,0 +1,195 @@ +# 轮胎滑移率-附着曲线 + +## 这一页解决什么问题 + +纵向控制里有个常被跳过的前提:**附着系数不是常数**。 + +车轮从自由滚动到完全抱死,纵向力先升后降 —— 峰值出现在一个很窄的滑移率区间里, +过了峰值附着反而变差,抱死时只剩峰值的两三成。ABS 的全部设计要点都在这条曲线上: +不是「不让车轮滑」,而是「把滑移率压在峰值附近」。 + +本页给出一个可以离线跑、可以断言、不需要 CARLA 的实现,把这条曲线画在终端里, +把峰值、锁死值和目标带都算成具体数字。 + +## 曲线模型 + +以归一化滑移率 `ŝ = s / s_peak` 表示: + +``` +μ(ŝ) = μ_peak · 2·ŝ / (1 + ŝ²) +``` + +| 位置 | 取值 | 含义 | +|---|---|---| +| `s = 0` | `μ = 0` | 自由滚动 | +| `s = s_peak` | `μ = μ_peak` | 峰值附着(唯一极大) | +| `s = 1.0` | `μ_peak · 2k/(1+k²)`,`k = 1/s_peak` | 车轮抱死 | + +闭式派生量: + +- 初段刚度:`dμ/ds|₀ = 2·μ_peak / s_peak` +- ABS 目标带:`[s_peak(1−band), s_peak(1+band)]` +- 锁死占比:`μ(1.0) / μ_peak` + +默认参数(`μ_peak = 0.90`、`s_peak = 0.12`、`band = 0.20`)下: + +| 量 | 值 | +|---|---| +| 峰值附着 | `0.9000` @ `s = 0.1200` | +| 锁死附着 | `0.2129`(占峰值 **23.7%**) | +| 初段刚度 | `15.000 1/s` | +| ABS 目标带 | `[0.0960, 0.1440]` | + +对照:`s = 0.06`(半峰值滑移)时 `μ = 0.72`,正好是峰值的 80% —— 滑移率只有峰值的一半, +纵向力已经掉了两成,这就是「轮胎工况窗口很窄」的量化说法。 + +## 四种路面 + +`road/surface` 只影响报告标注与默认值选择,实际数字仍由 `peak_slip` / `peak_friction` 决定。 +常用量级: + +| 路面 | `peak_slip` 典型值 | `peak_friction` 典型值 | +|---|---|---| +| `dry_asphalt` | 0.10 ~ 0.15 | 0.85 ~ 1.00 | +| `wet_asphalt` | 0.12 ~ 0.20 | 0.55 ~ 0.75 | +| `snow` | 0.20 ~ 0.40 | 0.25 ~ 0.40 | +| `ice` | 0.30 ~ 0.50 | 0.08 ~ 0.15 | + +## 四条断言 + +| 断言 | 判据 | 失败意味着 | +|---|---|---| +| **峰值唯一** | 采样点局部极大只有一个,且落在 `(s_peak, μ_peak)` 上 | 采样太粗抓不到峰值,或曲线形状被改坏 | +| **锁死衰减** | `μ(1.0)/μ_peak ≤ lock_friction_ratio` | 曲线没体现出「过度滑移导致附着下降」 | +| **初段线性** | `s→0` 时 `μ ≈ (2μ_peak/s_peak)·s`,相对误差 ≤ `linear_tolerance` | 曲线在原点附近不光滑(真车会有初始刚度段) | +| **目标带覆盖峰值** | 目标带必须包含 `s_peak` | 参数组合自相矛盾,ABS 会控制在错误的滑移区间 | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `tire/peak_slip` | `0.12` | 峰值滑移率(必须 `(0, 1]`) | +| `tire/peak_friction` | `0.90` | 峰值附着系数(必须 > 0) | +| `tire/abs_band` | `0.20` | 目标带宽比例(必须 `[0, 1)`) | +| `road/surface` | `dry_asphalt` | 路面标注 | +| `curve/samples` | `51` | 采样点数(≥5,默认步长 0.02) | +| `check/lock_friction_ratio` | `0.85` | 锁死附着上限比例 | +| `check/linear_tolerance` | `0.02` | 初段线性度容差 | +| `check/peak_tolerance` | `0.01` | 峰值容差 | +| `rate/publish_hz` | `10.0` | 挂起时循环频率 | +| `topic/report` | `/tire/report` | 报告话题 | +| `topic/curve` | `/tire/curve` | 曲线表话题 | +| `topic/peak_slip` | `/tire/peak_slip` | 峰值滑移率话题 | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/tire/report` | `std_msgs/String`(latched) | 完整文本报告(含字符图与四条断言) | +| `/tire/curve` | `std_msgs/String`(latched) | 曲线表 CSV:`slip,friction,ratio`,51 行 | +| `/tire/peak_slip` | `std_msgs/Float64`(latched) | 峰值滑移率 | + +## 运行 + +```bash +cd ~/catkin_ws16 +catkin_make +source devel/setup.bash +roslaunch tire_slip_curve main.launch +``` + +改参数: + +```bash +roslaunch tire_slip_curve main.launch peak_slip:=0.30 peak_friction:=0.35 surface:=snow +``` + +另一终端核对: + +```bash +rosparam get /tire/peak_slip +rosparam get /check/lock_friction_ratio +rosparam get /road/surface +``` + +## 实际输出(默认参数) + +``` +路面:dry_asphalt,峰值滑移率 0.120,峰值附着 0.900 +锁死点:s=1.00,附着 0.2129(占峰值 23.7%) +初段刚度:15.000 1/s,ABS 目标带 [0.0960, 0.1440](带宽 20%) +采样:51 点(s 从 0 到 1) +断言:4 通过 / 0 失败(共 4 条) + [PASS] 峰值唯一 峰值 0.9000 @ s=0.1200(期望 0.9000 @ 0.1200),局部极大 1 个 + [PASS] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 85.0%) + [PASS] 初段线性 初段刚度 15.000,最大相对误差 0.99% @ s=0.0120(要求 <= 2.00%) + [PASS] 目标带覆盖峰值 目标带 [0.0960, 0.1440] 覆盖峰值滑移 0.1200 +结论:PASSED +``` + +## 两个反例 + +**采样太粗(`samples:=5`)** + +```bash +roslaunch tire_slip_curve main.launch samples:=5 +``` + +只有 5 个采样点(步长 0.25),峰值的 `s = 0.12` 落在两个采样点之间,抓到的最大值是 +`s = 0.25` 处的 `0.7020` —— 比真峰值低 22%。于是 `峰值唯一` 失败: + +``` +[FAIL] 峰值唯一 峰值 0.7020 @ s=0.2500(期望 0.9000 @ 0.1200),局部极大 1 个 +``` + +**锁死衰减不达标(`lock_friction_ratio:=0.20`)** + +```bash +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 +``` + +实测锁死附着占峰值 23.7%,高于要求的 20%: + +``` +[FAIL] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 20.0%) +``` + +## 离线测试 + +```bash +cd ~/catkin_ws16 +python3 tests/test_tire_slip_curve_local.py +``` + +测试只依赖标准库(不需要 ROS / CARLA / 显示器)。在仓库根目录执行时还会检查 +`docs/ground/tire_slip_curve.md` 是否存在、参数表与命令是否完整。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `braking_distance_check` | 给定附着系数算制动距离 | +| `turning_sweep_check` | 给定转角与轴距算转弯包络 | +| `reverse_parking_planner` | 倒车入库轨迹 | +| **`tire_slip_curve`(本页)** | **附着系数本身:它由滑移率决定,有峰值也有锁死衰减** | + +本包是上游 —— 其它模块把这里的 `peak_friction` 当输入用。 + +## 常见问题 + +**Q:为什么不用魔术公式(Pacejka)?** +A:魔术公式需要拟合 4 个系数、没有解析极大值,断言只能靠数值搜索。本页要讲清的是 +「峰值 - 锁死 - 目标带」这三个概念,有闭式解的曲线更容易校验。真车标定时把 +`peak_slip` / `peak_friction` 换成实测值即可,0~0.4 区间形状足够接近。 + +**Q:Melodic 上为什么不能直接用 `rospy.loginfo(fmt, *args)`?** +A:rosgraph 的格式化会对非 ASCII 的 unicode 调 `str()`,在 Python 2.7 下抛 +`UnicodeEncodeError`。节点里的 `_log()` 把消息统一编码成 byte str 后再发。 + +**Q:为什么打印完节点不退出?** +A:留出时间让另一个终端 `rosparam get` 核对参数(launch 的 arg 覆盖是否生效)。 +按 `Ctrl+C` 退出。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/docs/index.md b/docs/index.md index 1dcabf5ae3..bddf5c8e3f 100644 --- a/docs/index.md +++ b/docs/index.md @@ -242,6 +242,11 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c * [地面移动底盘速度指令安全监督](./ground/cmd_vel_guard.md) —— 对 `/cmd_vel` 做限幅、急停、超时与非法值拦截,无需仿真环境 * [VRU 冲突风险图](./ground/vru_conflict_risk_map.md) —— 按时间对齐自车与行人、儿童和骑行者轨迹,输出最小距离、TTC 和风险等级 * [里程计健康监视(打滑 / 堵转 / 反向检测)](./ground/odom_health_monitor.md) —— 比较 `/cmd_vel` 与 `/odom` 的速度比值,识别打滑、堵转与反向运动 +* [乘坐舒适性传递率](./ground/ride_comfort_transmissibility.md) —— 传递率曲线与舒适性等级判定 +* [不足转向梯度与侧向稳定裕度](./ground/lateral_stability_margin.md) —— 稳态转向角与侧向稳定裕度核算 +* [工况能耗与续航核算](./ground/drive_cycle_energy.md) —— 工况能耗与续航的能量分解与复算 +* [横向载荷转移与侧翻裕度](./ground/rollover_margin.md) —— 载荷转移与极限车速的离线核算 +* [轮胎滑移率-附着曲线](./ground/tire_slip_curve.md) —— 轮胎附着随滑移率变化的曲线与 ABS 目标带 ## 5. 空域载具 diff --git a/mkdocs.yml b/mkdocs.yml index d0d1462a23..6360b5f712 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -236,6 +236,11 @@ nav: - 速度指令安全监督: './ground/cmd_vel_guard.md' - VRU 冲突风险图: './ground/vru_conflict_risk_map.md' - 里程计健康监视: './ground/odom_health_monitor.md' + - 乘坐舒适性传递率: './ground/ride_comfort_transmissibility.md' + - 不足转向梯度与侧向稳定裕度: './ground/lateral_stability_margin.md' + - 工况能耗与续航核算: './ground/drive_cycle_energy.md' + - 横向载荷转移与侧翻裕度: './ground/rollover_margin.md' + - 轮胎滑移率-附着曲线: './ground/tire_slip_curve.md' - 空域载具: - MuJoCo 城市屋顶光伏覆盖率测量: './air/mujoco_uav_rooftop_pv_coverage_survey.md' - MuJoCo 屋顶光伏覆盖率测量第二天: './air/mujoco_uav_rooftop_pv_coverage_survey_day2.md' diff --git a/src/ground/drive_cycle_energy/CMakeLists.txt b/src/ground/drive_cycle_energy/CMakeLists.txt new file mode 100644 index 0000000000..59998ec679 --- /dev/null +++ b/src/ground/drive_cycle_energy/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.0.2) +project(drive_cycle_energy) + +find_package(catkin REQUIRED) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/energy_core.py + scripts/energy_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config docs + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/drive_cycle_energy/README.md b/src/ground/drive_cycle_energy/README.md new file mode 100644 index 0000000000..cdbf1d7a4d --- /dev/null +++ b/src/ground/drive_cycle_energy/README.md @@ -0,0 +1,143 @@ +# drive_cycle_energy —— 工况能耗与续航核算 + +给定一段行驶工况(每段「时长 + 起始车速 + 结束车速」),把电池的能量花到哪里算清楚: +滚阻、风阻、制动损失、动能各占多少,再生制动能收回多少,满电能跑多远。 + +**纯标准库、纯计算**:不需要 CARLA、不需要 GPU、不需要显示器,`python3` 直接就能跑离线测试。 + +## 为什么做这个包 + +仓库里已有三个和「能耗」擦边的模块,但都不回答「这段工况电都花在哪了」: + +| 已有模块 | 它回答什么 | +|---|---| +| `speed_profile_check` | 一段距离内怎么分配加减速时间(运动学,不算阻力) | +| `braking_distance_check` | 从看见到停住要几米(只算制动段) | +| `lateral_stability_margin` | 转弯稳不稳(横向) | +| **本包** | **纵向能量流:滚阻 / 风阻 / 制动 / 动能的分配,以及续航** | + +关键点是**能量分解可以写成闭式恒等式**,于是「实现对不对」能被第四条断言直接卡住: + +``` +E_drive − E_brake = E_roll + E_aero + ΔKE +E_roll = m·g·c_rr · 行驶距离 (滚阻力与车速无关) +E_aero = 0.5·ρ·Cd·A · ∫v³ dt (风阻力与车速平方成正比) +ΔKE = 0.5·m·(v_end² − v_start²) +``` + +## 安装与运行 + +本包是 catkin 包,放进工作空间即可(不需要编译 C++): + +```bash +cd ~/catkin_ws18 +catkin_make +source /opt/ros/melodic/setup.bash +source devel/setup.bash +``` + +跑起来(参数全部在 `config/drive_cycle_energy.yaml`,命令行可覆盖): + +```bash +roslaunch drive_cycle_energy main.launch +roslaunch drive_cycle_energy main.launch regen_eff:=0.0 regen_limit:=0.5 +``` + +看发布出来的东西: + +```bash +rostopic echo -n 1 /energy/report # 文字报告(latched) +rostopic echo -n 1 /energy/breakdown # 逐项能量 CSV(latched) +rostopic echo -n 1 /energy/range # 满电续航 km(latched) +rosparam get /vehicle/battery_kwh # 确认参数生效 +``` + +## 参数(12 个) + +| 参数 | 默认 | 含义 | +|---|---|---| +| `vehicle/mass` | 1800.0 | 整备质量 kg | +| `vehicle/c_rr` | 0.012 | 滚动阻力系数 | +| `vehicle/cd` | 0.31 | 风阻系数 | +| `vehicle/area` | 2.2 | 迎风面积 m² | +| `vehicle/battery_kwh` | 60.0 | 电池可用容量 kWh | +| `vehicle/regen_eff` | 0.6 | 再生制动回收效率 0~1 | +| `check/breakdown_tolerance` | 0.02 | 能量分解相对偏差上限 | +| `check/regen_limit` | 1.0 | 回收 / 制动 的上限 | +| `rate/publish_hz` | 10.0 | 保活循环频率 | +| `topic/report` | `/energy/report` | 文字报告话题 | +| `topic/breakdown` | `/energy/breakdown` | 逐项能量 CSV | +| `topic/range` | `/energy/range` | 满电续航 km | + +## 话题(3 路,都是 latched) + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/energy/report` | `std_msgs/String` | 完整报告(工况、能量、分解、续航、四条断言) | +| `/energy/breakdown` | `std_msgs/String` | `item,joules` 七行 CSV | +| `/energy/range` | `std_msgs/Float64` | 满电续航 km | + +## 默认工况与结果 + +默认工况 8 段共 60 s:0→30、30 匀速、30→50、50 匀速、50→0、怠速、0→40、40 匀速,行驶 512.5 m。 + +| 量 | 值 | +|---|---| +| 驱动能量 | **407.3 kJ** | +| 制动能量 | 159.6 kJ | +| 回收能量(η=0.6) | **95.8 kJ** | +| 净耗 | **311.5 kJ** | +| 分解 | 滚阻 108.6 kJ(26.7%)/ 风阻 28.0 kJ(6.9%)/ 制动 159.6 kJ(39.2%)/ 动能 111.1 kJ(27.3%)| +| 百公里电耗 | **16.89 kWh** | +| 满电续航 | **355.3 km** | + +四条断言全过: + +``` +[PASS] 能量分解 滚阻 108.6 + 风阻 28.0 + 制动 159.6 + 动能 111.1 = 407.3 kJ(偏差 0.0000) +[PASS] 回收上限 回收 95.8 / 制动 159.6 = 0.6000(上限 1.00,η=0.60) +[PASS] 续航自洽 百公里 16.885 kWh,电池 60.0 kWh → 续航 355.3 km +[PASS] 风阻上升 10 m/s 40.9 N < 30 m/s 368.3 N +``` + +## 两个反例 + +1. `regen_limit:=0.5` → **只翻「回收上限」**(0.6000 > 0.50),其余三条仍通过; +2. `regen_eff:=0.0` → 净耗从 311.5 kJ 涨到 **407.3 kJ**、续航从 355.3 km 掉到 **271.8 km**, + 四条断言依然全过 —— 说明断言校验的是「自洽」,不是「车好不好」; + 要看回收有没有收益,得对比这两次运行。 + +## 离线测试 + +```bash +python3 tests/test_drive_cycle_energy_local.py +``` + +**通过 152 项,失败 0 项**,覆盖:参数校验、阻力计算、工况解析、积分、四条断言、 +能量恒等式、续航与百公里、字符图、报告文本、参数对象、工程文件约定。 + +其中 K 组专门回归本项目踩过的坑:逐个文件检查 `scripts/*.py` 的 +**第 1 行是 shebang、第 2 行是 `# -*- coding: utf-8 -*-`**。 +Melodic 的 Python 2.7 要求编码声明在前两行,而 `sed` 改 shebang 时若整行覆盖, +编码声明就会丢,节点一启动就 `SyntaxError: Non-ASCII character ... no encoding declared`。 + +## 与其他模块的分工 + +- `speed_profile_check`:只算运动学(距离、时间、加减速),不含阻力; +- 本包:在运动学之上加阻力模型与电池模型,算能量与续航; +- `braking_distance_check`:只关心「停得住吗」,不含回收; +- `lateral_stability_margin`:横向稳定性,与本包的纵向能量互不重叠。 + +## 常见问题 + +**Q:`rostopic echo` 一直卡着不动?** +latched 话题会一直重发最后一条;加 `-n 1` 只取一条。 + +**Q:数字和 README 对不上?** +先 `rosparam get /vehicle/mass` 等确认没被命令行或其它 yaml 覆盖。 + +**Q:能换成自己的工况吗?** +改 `energy_core.DEFAULT_CYCLE`(每段 `(时长, 起始 km/h, 结束 km/h)`),或在代码里 +`core.simulate(spec, 你自己的段列表)`。 + +> 所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/src/ground/drive_cycle_energy/config/drive_cycle_energy.yaml b/src/ground/drive_cycle_energy/config/drive_cycle_energy.yaml new file mode 100644 index 0000000000..12196d1210 --- /dev/null +++ b/src/ground/drive_cycle_energy/config/drive_cycle_energy.yaml @@ -0,0 +1,23 @@ +# drive_cycle_energy default parameters +# loaded by launch/main.launch via +# roslaunch arg xxx:=yyy overrides these values. +# NOTE: keep this file pure ASCII -- ROS Melodic (Python 2.7) loads it with +# an ASCII codec and fails on non-ASCII comments. + +# ---- vehicle and battery ---- +vehicle/mass: 1800.0 +vehicle/c_rr: 0.012 +vehicle/cd: 0.31 +vehicle/area: 2.2 +vehicle/battery_kwh: 60.0 +vehicle/regen_eff: 0.6 + +# ---- assertion tolerances ---- +check/breakdown_tolerance: 0.02 +check/regen_limit: 1.0 + +# ---- publishing ---- +rate/publish_hz: 10.0 +topic/report: /energy/report +topic/breakdown: /energy/breakdown +topic/range: /energy/range diff --git a/src/ground/drive_cycle_energy/launch/main.launch b/src/ground/drive_cycle_energy/launch/main.launch new file mode 100644 index 0000000000..002c5892dc --- /dev/null +++ b/src/ground/drive_cycle_energy/launch/main.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/drive_cycle_energy/package.xml b/src/ground/drive_cycle_energy/package.xml new file mode 100644 index 0000000000..04a8a208da --- /dev/null +++ b/src/ground/drive_cycle_energy/package.xml @@ -0,0 +1,16 @@ + + + drive_cycle_energy + 1.0.0 + 工况能耗与续航核算:按纵向动力学把驱动能量分解为滚阻/风阻/制动/动能, + 并估算百公里电耗与满电续航;纯计算,不依赖 CARLA 与显示器。 + ncut + MIT + + catkin + + rospy + std_msgs + + + diff --git a/src/ground/drive_cycle_energy/scripts/energy_core.py b/src/ground/drive_cycle_energy/scripts/energy_core.py new file mode 100644 index 0000000000..6c431c0492 --- /dev/null +++ b/src/ground/drive_cycle_energy/scripts/energy_core.py @@ -0,0 +1,316 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""工况能耗与续航核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +问题:一段行驶工况里,电池到底把多少能量花在滚阻、风阻和加减速上? +再生制动能收回多少?满电能跑多远? + +模型(纵向动力学,纵向加速度 a,坡度忽略): + 驱动力 F = m·a + m·g·c_rr + 0.5·ρ·Cd·A·v² + 滚阻力 F_rr = m·g·c_rr (与车速无关) + 风阻力 F_aero = 0.5·ρ·Cd·A·v² (与车速平方成正比) + 回收 E_regen = η · ∫|F_brake|·v dt (只在减速时发生,η 是回收效率) + 续航 range = 电池容量 / 每公里净耗电 + +核心恒等式(正是第一条断言的依据): + E_drive − E_brake = E_roll + E_aero + ΔKE + ⇔ E_drive = E_roll + E_aero + E_brake + ΔKE +其中 ΔKE 是整段工况的动能净变化(起终点车速相同则为零)。 +滚阻与风阻都能写成速度的幂函数,因此这一分解可以逐项积出来再对账。 +""" + +from __future__ import division, print_function, unicode_literals + +ASSERT_NAMES = (u"能量分解", u"回收上限", u"续航自洽", u"风阻上升") + + +class EnergyError(ValueError): + """参数或工况非法。""" + + +def _positive(name, value): + if value is None or value <= 0: + raise EnergyError(u"%s 必须大于 0,收到 %r" % (name, value)) + return value + + +def _non_negative(name, value): + if value is None or value < 0: + raise EnergyError(u"%s 不能为负,收到 %r" % (name, value)) + return value + + +class VehicleSpec(object): + """整车与电池参数。""" + + def __init__(self, mass=1800.0, c_rr=0.012, rho=1.2, cd=0.31, area=2.2, + gravity=9.81, battery_kwh=60.0, regen_eff=0.6): + self.mass = _positive(u"质量", mass) + self.c_rr = _non_negative(u"滚阻系数", c_rr) + self.rho = _positive(u"空气密度", rho) + self.cd = _positive(u"风阻系数", cd) + self.area = _positive(u"迎风面积", area) + self.gravity = _positive(u"重力加速度", gravity) + self.battery_kwh = _positive(u"电池容量", battery_kwh) + if regen_eff < 0 or regen_eff > 1: + raise EnergyError(u"回收效率必须在 [0, 1],收到 %r" % (regen_eff,)) + self.regen_eff = regen_eff + + # ---- 阻力 ---- + def rolling_force(self): + return self.mass * self.gravity * self.c_rr + + def aero_force(self, speed): + return 0.5 * self.rho * self.cd * self.area * speed * speed + + def resistance(self, speed): + return self.rolling_force() + self.aero_force(speed) + + def traction_force(self, speed, accel): + return self.mass * accel + self.resistance(speed) + + def kinetic(self, speed): + return 0.5 * self.mass * speed * speed + + def battery_joules(self): + return self.battery_kwh * 3.6e6 + + def __repr__(self): + return ("VehicleSpec(mass=%.1f, c_rr=%.3f, cd=%.3f, area=%.2f, " + "battery_kwh=%.1f, regen_eff=%.2f)" + % (self.mass, self.c_rr, self.cd, self.area, + self.battery_kwh, self.regen_eff)) + + +def parse_segments(segments): + """把 [(秒, 起始 km/h, 结束 km/h), ...] 规整成 [(t, v0, v1), ...](m/s)。""" + out = [] + for i, seg in enumerate(segments): + if len(seg) != 3: + raise EnergyError(u"第 %d 段必须是 (秒, 起始, 结束),收到 %r" % (i, seg)) + hold, v0, v1 = seg + if hold <= 0: + raise EnergyError(u"第 %d 段时长必须大于 0,收到 %r" % (i, hold)) + if v0 < 0 or v1 < 0: + raise EnergyError(u"第 %d 段车速不能为负,收到 %r" % (i, seg)) + out.append((float(hold), float(v0) / 3.6, float(v1) / 3.6)) + if not out: + raise EnergyError(u"工况至少要有一段") + return out + + +def simulate(spec, segments, dt=0.05): + """按固定步长(中点法)积分一段工况,返回能量与统计量。""" + segs = parse_segments(segments) + if dt <= 0: + raise EnergyError(u"步长必须大于 0,收到 %r" % (dt,)) + + drive_j = 0.0 + brake_j = 0.0 + roll_j = 0.0 + aero_j = 0.0 + ke_j = 0.0 + distance = 0.0 + duration = 0.0 + v_max = 0.0 + + for hold, v0, v1 in segs: + accel = (v1 - v0) / hold + steps = int(round(hold / dt)) or 1 + step = hold / steps + for i in range(steps): + v = v0 + accel * (i + 0.5) * step + if v < 0: + v = 0.0 + force = spec.traction_force(v, accel) + work = force * v * step + if work >= 0: + drive_j += work + else: + brake_j += -work + roll_j += spec.rolling_force() * v * step + aero_j += spec.aero_force(v) * v * step + ke_j += spec.mass * accel * v * step + distance += v * step + v_max = max(v_max, v) + duration += hold + + regen_j = spec.regen_eff * brake_j + net_j = drive_j - regen_j + return { + "drive_j": drive_j, + "brake_j": brake_j, + "regen_j": regen_j, + "roll_j": roll_j, + "aero_j": aero_j, + "ke_j": ke_j, + "net_j": net_j, + "distance_m": distance, + "duration_s": duration, + "v_max": v_max, + } + + +def per_100km(result): + """每 100 km 净耗电(kWh)。""" + if result["distance_m"] <= 0: + raise EnergyError(u"工况没有行驶距离,无法折算百公里能耗") + return result["net_j"] / 3.6e6 / (result["distance_m"] / 100000.0) + + +def range_km(spec, result): + """满电续航(km)。""" + kwh_per_100 = per_100km(result) + if kwh_per_100 <= 0: + raise EnergyError(u"净耗电不为正,续航无意义") + return spec.battery_kwh / kwh_per_100 * 100.0 + + +def share(result, key): + """某一项能量占驱动能量的比例。""" + if result["drive_j"] <= 0: + raise EnergyError(u"驱动能量不为正,无法计算占比") + return result[key] / result["drive_j"] + + +def check_energy(spec, result, breakdown_tolerance=0.02, regen_limit=1.0, + range_tolerance=0.02, aero_rising=True): + """四条断言:分解自洽、回收上限、续航自洽、风阻随车速上升。""" + asserts = [] + + total = (result["roll_j"] + result["aero_j"] + result["brake_j"] + + result["ke_j"]) + if result["drive_j"] <= 0: + rel = 1.0 + else: + rel = abs(total - result["drive_j"]) / result["drive_j"] + asserts.append({ + "type": ASSERT_NAMES[0], + "passed": bool(rel <= breakdown_tolerance + 1e-12), + "detail": (u"滚阻 %.1f + 风阻 %.1f + 制动 %.1f + 动能 %.1f = %.1f kJ," + u"驱动 %.1f kJ(相对偏差 %.4f,容差 %.2f)" + % (result["roll_j"] / 1000.0, result["aero_j"] / 1000.0, + result["brake_j"] / 1000.0, result["ke_j"] / 1000.0, + total / 1000.0, result["drive_j"] / 1000.0, + rel, breakdown_tolerance)), + }) + + if result["brake_j"] <= 0: + ratio = 0.0 + else: + ratio = result["regen_j"] / result["brake_j"] + asserts.append({ + "type": ASSERT_NAMES[1], + "passed": bool(ratio <= regen_limit + 1e-12), + "detail": (u"回收 %.1f kJ / 制动 %.1f kJ = %.4f(上限 %.2f,η=%.2f)" + % (result["regen_j"] / 1000.0, result["brake_j"] / 1000.0, + ratio, regen_limit, spec.regen_eff)), + }) + + try: + kwh_per_100 = per_100km(result) + implied = spec.battery_kwh / kwh_per_100 * 100.0 + rel_range = abs(implied - range_km(spec, result)) / max(implied, 1e-9) + range_ok = rel_range <= range_tolerance + 1e-12 + range_detail = (u"百公里 %.3f kWh,电池 %.1f kWh → 续航 %.1f km(相对偏差 %.4f)" + % (kwh_per_100, spec.battery_kwh, implied, rel_range)) + except EnergyError as exc: + range_ok = False + range_detail = u"无法折算续航:%s" % (exc,) + asserts.append({ + "type": ASSERT_NAMES[2], + "passed": bool(range_ok), + "detail": range_detail, + }) + + aero_slow = spec.aero_force(10.0) + aero_fast = spec.aero_force(30.0) + rising = aero_fast > aero_slow + asserts.append({ + "type": ASSERT_NAMES[3], + "passed": bool(rising if aero_rising else (not rising)), + "detail": (u"10 m/s 风阻 %.1f N,30 m/s 风阻 %.1f N(期望%s)" + % (aero_slow, aero_fast, u"上升" if aero_rising else u"不上升")), + }) + return asserts + + +def render_energy(result, width=42): + """按比例画能量条。""" + rows = [ + (u"滚阻", result["roll_j"]), + (u"风阻", result["aero_j"]), + (u"制动", result["brake_j"]), + (u"动能", result["ke_j"]), + (u"驱动", result["drive_j"]), + (u"回收", result["regen_j"]), + ] + biggest = max([v for _, v in rows] + [1.0]) + lines = [u"能量构成(驱动 = 满条,其余按比例)", "-" * width] + for label, value in rows: + cells = int(round(max(value, 0.0) / biggest * width)) + lines.append(u"%s|%s| %8.1f kJ" % (label, "#" * cells, value / 1000.0)) + lines.append("-" * width) + return "\n".join(lines) + + +def format_report(spec, result, asserts): + """把上面的一切排成人能读的报告。""" + lines = [] + lines.append(u"工况:%.1f s,行驶 %.1f m,最高 %.1f km/h" + % (result["duration_s"], result["distance_m"], + result["v_max"] * 3.6)) + lines.append(u"整车:质量 %.0f kg,滚阻 %.3f,Cd·A %.2f m²,电池 %.1f kWh,回收 η %.2f" + % (spec.mass, spec.c_rr, spec.cd * spec.area, + spec.battery_kwh, spec.regen_eff)) + lines.append(u"能量:驱动 %.1f kJ,制动 %.1f kJ,回收 %.1f kJ,净耗 %.1f kJ" + % (result["drive_j"] / 1000.0, result["brake_j"] / 1000.0, + result["regen_j"] / 1000.0, result["net_j"] / 1000.0)) + lines.append(u"分解:滚阻 %.1f kJ(%.1f%%),风阻 %.1f kJ(%.1f%%)," + u"制动 %.1f kJ(%.1f%%),动能 %.1f kJ(%.1f%%)" + % (result["roll_j"] / 1000.0, share(result, "roll_j") * 100.0, + result["aero_j"] / 1000.0, share(result, "aero_j") * 100.0, + result["brake_j"] / 1000.0, share(result, "brake_j") * 100.0, + result["ke_j"] / 1000.0, share(result, "ke_j") * 100.0)) + lines.append(u"续航:百公里 %.2f kWh,满电 %.1f km" + % (per_100km(result), range_km(spec, result))) + 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) + + +DEFAULT_CYCLE = [ + (5.0, 0.0, 30.0), + (10.0, 30.0, 30.0), + (6.0, 30.0, 50.0), + (15.0, 50.0, 50.0), + (8.0, 50.0, 0.0), + (6.0, 0.0, 0.0), + (6.0, 0.0, 40.0), + (4.0, 40.0, 40.0), +] + + +class EnergyParams(object): + """把 /xxx/yyy 形式的参数名收成对象,缺省时用内置默认值。""" + + def __init__(self, **kwargs): + self.vehicle = VehicleSpec( + mass=kwargs.get("vehicle/mass", 1800.0), + c_rr=kwargs.get("vehicle/c_rr", 0.012), + cd=kwargs.get("vehicle/cd", 0.31), + area=kwargs.get("vehicle/area", 2.2), + battery_kwh=kwargs.get("vehicle/battery_kwh", 60.0), + regen_eff=kwargs.get("vehicle/regen_eff", 0.6), + ) + self.breakdown_tolerance = kwargs.get("check/breakdown_tolerance", 0.02) + self.regen_limit = kwargs.get("check/regen_limit", 1.0) + self.range_tolerance = kwargs.get("check/range_tolerance", 0.02) + self.dt = kwargs.get("cycle/dt", 0.05) + self.cycle = kwargs.get("cycle/segments", DEFAULT_CYCLE) diff --git a/src/ground/drive_cycle_energy/scripts/energy_node.py b/src/ground/drive_cycle_energy/scripts/energy_node.py new file mode 100644 index 0000000000..4112bd7ede --- /dev/null +++ b/src/ground/drive_cycle_energy/scripts/energy_node.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""工况能耗节点的 ROS 封装(ROS 1 / Melodic;Python 2.7 & 3.x)。 + +节点只做三件事: + 1. 从参数服务器读整车参数与工况参数; + 2. 调 energy_core 积分出一段工况的能量与续航,跑四条断言; + 3. 把报告、逐项能量(CSV)、续航分别发布到三个 latched 话题,然后打印一遍并保活。 + +注意:roslaunch 打出的日志由 rosgraph 转成 Python 2 的 str,如果消息里有中文, +必须先在 _log() 里 encode 成 byte str,否则 Melodic 的 roslogging 会抛 +UnicodeEncodeError(rosgraph/roslogging.py:249 的 str(record_message))。 +""" + +from __future__ import division, print_function, unicode_literals + +import os +import sys + +HERE = os.path.dirname(os.path.abspath(__file__)) +if HERE not in sys.path: + sys.path.insert(0, HERE) + +import rospy # noqa: E402 +from std_msgs.msg import Float64, String # noqa: E402 + +import energy_core as core # noqa: E402 + +PARAM_KEYS = ( + "vehicle/mass", "vehicle/c_rr", "vehicle/cd", "vehicle/area", + "vehicle/battery_kwh", "vehicle/regen_eff", + "check/breakdown_tolerance", "check/regen_limit", + "rate/publish_hz", "topic/report", "topic/breakdown", "topic/range", +) + +DEFAULT_CYCLE = core.DEFAULT_CYCLE + + +def _log(level, msg): + """把 unicode 消息压成 byte str 再交给 rospy,规避 Melodic 的编码问题。""" + text = msg + if isinstance(text, type(u"")): + text = text.encode("utf-8", "replace") + getattr(rospy, level)(text) + + +def read_params(): + values = {} + for key in PARAM_KEYS: + values[key] = rospy.get_param("/" + key) + return values + + +def main(): + rospy.init_node("drive_cycle_energy", anonymous=False) + params = read_params() + + spec = core.VehicleSpec( + mass=params["vehicle/mass"], + c_rr=params["vehicle/c_rr"], + cd=params["vehicle/cd"], + area=params["vehicle/area"], + battery_kwh=params["vehicle/battery_kwh"], + regen_eff=params["vehicle/regen_eff"], + ) + + report_pub = rospy.Publisher(params["topic/report"], String, queue_size=1, + latch=True) + breakdown_pub = rospy.Publisher(params["topic/breakdown"], String, + queue_size=1, latch=True) + range_pub = rospy.Publisher(params["topic/range"], Float64, queue_size=1, + latch=True) + + _log("loginfo", u"工况能耗节点已启动:电池 %.1f kWh,回收 η %.2f,工况 %d 段" + % (spec.battery_kwh, spec.regen_eff, len(DEFAULT_CYCLE))) + + result = core.simulate(spec, DEFAULT_CYCLE) + asserts = core.check_energy( + spec, result, + breakdown_tolerance=params["check/breakdown_tolerance"], + regen_limit=params["check/regen_limit"], + range_tolerance=0.02, + ) + report = core.format_report(spec, result, asserts) + for line in report.splitlines(): + _log("loginfo", line) + + keys = ("roll_j", "aero_j", "brake_j", "regen_j", "ke_j", "drive_j", "net_j") + csv_text = "item,joules\n" + "\n".join( + "%s,%.3f" % (k, result[k]) for k in keys) + _log("loginfo", core.render_energy(result)) + + report_pub.publish(report) + breakdown_pub.publish(csv_text) + range_pub.publish(core.range_km(spec, result)) + _log("loginfo", u"已发布 %s / %s / %s" + % (params["topic/report"], params["topic/breakdown"], + params["topic/range"])) + + rate = rospy.Rate(params["rate/publish_hz"]) + while not rospy.is_shutdown(): + try: + rate.sleep() + except rospy.ROSInterruptException: + break + _log("loginfo", u"已退出") + + +if __name__ == "__main__": + main() diff --git a/src/ground/drive_cycle_energy/scripts/main.py b/src/ground/drive_cycle_energy/scripts/main.py new file mode 100644 index 0000000000..8af1122506 --- /dev/null +++ b/src/ground/drive_cycle_energy/scripts/main.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""入口:把脚本目录加进 sys.path 后转交给 energy_node.main。""" + +from __future__ import division, print_function + +import os +import sys + +HERE = os.path.dirname(os.path.abspath(__file__)) +if HERE not in sys.path: + sys.path.insert(0, HERE) + +from energy_node import main # noqa: E402 + + +if __name__ == "__main__": + main() 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()) diff --git a/src/ground/ride_comfort_transmissibility/CMakeLists.txt b/src/ground/ride_comfort_transmissibility/CMakeLists.txt new file mode 100644 index 0000000000..4f2d66baab --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ride_comfort_transmissibility) + +find_package(catkin REQUIRED) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/ride_core.py + scripts/ride_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/ride_comfort_transmissibility/README.md b/src/ground/ride_comfort_transmissibility/README.md new file mode 100644 index 0000000000..745e1475aa --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/README.md @@ -0,0 +1,159 @@ +# ride_comfort_transmissibility:乘坐舒适性传递率 + +路面是一段段不同频率的起伏。车过某段搓板路时抖得厉害、换个车速又平稳了, +原因是悬架对不同频率的输入放大倍数不一样 —— 这就是**传递率**。 +这个模块把它算清楚:共振在哪个频率、放大多少倍、按 ISO 2631-1 属于哪个舒适度等级。 + +不依赖 CARLA、不需要显示器,纯 Python 标准库,Python 2.7 / 3.x 都能跑。 + +## 模型 + +单自由度基础激励(只看垂直方向): + +``` +r = f / f_n 频率比 +T(r) = sqrt((1 + (2ζr)²) / ((1 − r²)² + (2ζr)²)) 位移传递率 + +峰值位置 r_p = sqrt(1 − 2ζ²) (ζ ≥ 1/√2 时没有经典共振峰) +峰值大小 T_p = sqrt((1 + 4ζ²(1−2ζ²)) / (4ζ²(1−ζ²))) +高频衰减 T(r) → 0,r = 10 时约 0.05 +加权加速度 a_w = T(r) · a_in +``` + +ISO 2631-1 的舒适度等级(`a_w` 单位 m/s²,RMS): + +| a_w | 等级 | +|---|---| +| < 0.315 | 舒适 | +| 0.315 ~ 0.63 | 略有不适 | +| 0.63 ~ 1.0 | 不适 | +| 1.0 ~ 1.6 | 很不适 | +| 1.6 ~ 2.5 | 极不适 | +| > 2.5 | 无法忍受 | + +`damping` 被限制在 `(0, 1/√2)`:超过 0.7071 之后 `sqrt(1−2ζ²)` 没有实数意义, +闭式峰位失效,程序直接报 `RideError`,而不是给一个错的数。 + +## 默认数字(`config/ride_comfort_transmissibility.yaml`) + +| 项 | 值 | +|---|---| +| 悬架固有频率 / 阻尼比 | 1.2 Hz / 0.25 | +| 路面输入 / 激励频率 | 0.5 m/s² / 6.0 Hz | +| 理论峰位 / 峰值 | **0.935414 / 2.280351** | +| 扫描峰位 / 峰值 | 0.948625 / 2.283149(与闭式差 0.12%) | +| r = 1 处传递率 | 2.236068 | +| r = 10 处传递率 | 0.051440 | +| 6 Hz 处加权加速度 | 0.055794 m/s² → 等级「舒适」 | + +扫描表(10 个频率比): + +| r | f Hz | T | a_w m/s² | +|---|---|---|---| +| 0.25 | 0.30 | 1.0655 | 0.5328 | +| 0.50 | 0.60 | 1.3038 | 0.6519 | +| 0.75 | 0.90 | 1.8535 | 0.9267 | +| 1.00 | 1.20 | 2.2361 | 1.1180 | +| 1.25 | 1.50 | 1.4024 | 0.7012 | +| 1.50 | 1.80 | 0.8575 | 0.4287 | +| 2.00 | 2.40 | 0.4472 | 0.2236 | +| 3.00 | 3.60 | 0.2215 | 0.1107 | +| 5.00 | 6.00 | 0.1116 | 0.0558 | +| 10.00 | 12.00 | 0.0514 | 0.0257 | + +注意 r = 1(激励频率等于固有频率)时 T 是 2.236,但**真正的峰值 2.283 出现在 +r ≈ 0.949**,略低于共振比 —— 阻尼越大,峰位越往左偏。这是很多人会算错的地方。 + +## 四条断言 + +| 断言 | 判据 | 默认结果 | +|---|---|---| +| 共振位置 | 扫描峰位与 `sqrt(1−2ζ²)` 的偏差 ≤ 0.02 | PASS(0.0132) | +| 峰值倍率 | 扫描峰值与闭式峰值的相对偏差 ≤ 5% | PASS(0.12%) | +| 高频衰减 | `T(r=10) ≤ 0.1` | PASS(0.0514) | +| 输入线性 | 输入加倍后 `a_w` 精确加倍 | PASS(0.0000) | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `/suspension/fn_hz` | 1.2 | 悬架固有频率 Hz | +| `/suspension/damping` | 0.25 | 阻尼比,必须在 (0, 0.7071) 开区间 | +| `/road/input_accel` | 0.5 | 路面输入加速度 RMS m/s² | +| `/road/freq_hz` | 6.0 | 关心的激励频率 Hz | +| `/check/pos_tolerance` | 0.02 | 峰位容差(频率比) | +| `/check/peak_tolerance` | 0.05 | 峰值相对容差 | +| `/check/high_freq_limit` | 0.1 | r=10 时传递率上限 | +| `/check/linear_tolerance` | 0.02 | 输入加倍的线性容差 | +| `/scan/low` `/scan/high` `/scan/steps` | 0.05 / 4.0 / 400 | 扫描区间与步数 | +| `/rate/publish_hz` | 1.0 | 保活重发频率 Hz | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/ride/report` | `std_msgs/String` | 整份报告 | +| `/ride/curve` | `std_msgs/Float64MultiArray` | 每个频率比三元组 `[r, T, a_w]`,共 30 个数 | +| `/ride/peak` | `std_msgs/Float64MultiArray` | `[理论峰位, 理论峰值, 扫描峰位, 扫描峰值, 激励处 a_w]` | + +三条都是 latched。 + +## 跑起来 + +```bash +cd ~/catkin_ws19 && catkin_make +source /opt/ros/melodic/setup.bash +source ~/catkin_ws19/devel/setup.bash +roslaunch ride_comfort_transmissibility main.launch +``` + +默认 `结论:PASSED`。两个有意思的反例: + +```bash +# ① 容差收到 0.001 → 扫描精度不够,只翻「共振位置」 +roslaunch ride_comfort_transmissibility main.launch pos_tolerance:=0.001 + +# ② 阻尼降到 0.02(坏减振器)→ 峰值 25 倍,激励频率挪到 1.2 Hz 就「无法忍受」 +roslaunch ride_comfort_transmissibility main.launch damping:=0.02 freq_hz:=1.2 +``` + +反例②很说明问题:四条断言**全部通过**(物理上它自洽),但舒适等级从「舒适」 +变成「无法忍受」。断言管的是「算得对不对」,等级管的是「人坐着难受不难受」。 + +## 离线测试 + +```bash +python3 tests/test_ride_comfort_transmissibility_local.py +``` + +覆盖 A 传递率基本形、B 闭式峰位峰值、C 舒适等级、D 四条断言、E 扫描表、 +F 字符图、G 参数读取、H 报告排版、I 闭式核对、J 工程文件约定、K 编码声明回归。 + +## 与已有模块的分工 + +| 模块 | 关注点 | 本模块的区别 | +|---|---|---| +| `rollover_margin` | 侧翻(横向载荷转移) | 管横向极限,本模块管垂直舒适性 | +| `lateral_stability_margin` | 不足转向/甩尾 | 管转向响应 | +| `braking_distance_check` | 纵向停车距离 | 管纵向制动 | +| `steering_actuator_lag` | 执行器一阶惯性 | 同是「频率响应」,但那是转向机构,本模块是悬架 | + +## 常见问题 + +**1. 为什么峰值不在 r = 1?** +`T(1) = sqrt(1+(2ζ)²)/(2ζ)`,而真正的极值点在 `r = sqrt(1−2ζ²)`。ζ = 0.25 时 +r_p = 0.935,峰值比 T(1) 高约 2%。阻尼越大偏得越多,这也是判据要写「峰位」 +而不是直接拿 r=1 当峰的原因。 + +**2. `damping:=0.8` 为什么直接报错?** +超过 `1/√2 ≈ 0.7071` 以后曲线没有经典共振峰,`sqrt(1−2ζ²)` 是虚数。与其给一个 +错的峰位,不如判参数非法 —— 现实里的悬架阻尼比也不会到 0.8。 + +**3. 高通/低通行为怎么读?** +`r → 0` 时 `T → 1`(车身跟着路面一起动,等于没有悬架);`r → ∞` 时 +`T → 0`(悬架把高频震动滤掉了)。所以舒适性设计的目标是把共振峰挪到人最不敏感 +的低频、同时把峰压下去,也就是「加大阻尼」。 + +## 大模型声明 + +本模块的代码编写、测试编写与文档整理使用了 AI 编程助手辅助,提交内容由本人负责。 diff --git a/src/ground/ride_comfort_transmissibility/config/ride_comfort_transmissibility.yaml b/src/ground/ride_comfort_transmissibility/config/ride_comfort_transmissibility.yaml new file mode 100644 index 0000000000..74b2b6e34f --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/config/ride_comfort_transmissibility.yaml @@ -0,0 +1,21 @@ +# ride_comfort_transmissibility 的默认参数 +# 由 launch/main.launch 载入 + +suspension: + fn_hz: 1.2 # 悬架固有频率 Hz + damping: 0.25 # 阻尼比(必须在 (0, 0.7071) 开区间) + +road: + input_accel: 0.5 # 路面输入加速度 RMS m/s^2 + freq_hz: 6.0 # 关心的路面激励频率 Hz + +check: + pos_tolerance: 0.02 # 峰位容差(频率比) + peak_tolerance: 0.05 # 峰值相对容差 + high_freq_limit: 0.1 # r=10 时传递率上限 + linear_tolerance: 0.02 # 输入加倍的线性容差 + +scan: + low: 0.05 # 扫描下限(频率比) + high: 4.0 # 扫描上限 + steps: 400 # 扫描步数 diff --git a/src/ground/ride_comfort_transmissibility/launch/main.launch b/src/ground/ride_comfort_transmissibility/launch/main.launch new file mode 100644 index 0000000000..7c1a0c919b --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/launch/main.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/ride_comfort_transmissibility/package.xml b/src/ground/ride_comfort_transmissibility/package.xml new file mode 100644 index 0000000000..956c943719 --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/package.xml @@ -0,0 +1,18 @@ + + + ride_comfort_transmissibility + 1.0.0 + + 乘坐舒适性传递率核算:单自由度悬架在路面正弦激励下的位移传递率、 + 共振峰位置与峰值倍率、ISO 2631-1 舒适度等级,并给出四条断言。 + 纯标准库实现,不依赖 CARLA、不需要显示器。 + + + Wist-sutton + MIT + + catkin + + rospy + std_msgs + diff --git a/src/ground/ride_comfort_transmissibility/scripts/main.py b/src/ground/ride_comfort_transmissibility/scripts/main.py new file mode 100644 index 0000000000..946bc28d26 --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/scripts/main.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""ride_comfort_transmissibility 节点入口:参数、核算、打印、发布。""" + +from __future__ import division, print_function, unicode_literals + +import os +import sys + +_HERE = os.path.dirname(os.path.abspath(__file__)) +if _HERE not in sys.path: + sys.path.insert(0, _HERE) + +from ride_node import main # noqa: E402 + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/ride_comfort_transmissibility/scripts/ride_core.py b/src/ground/ride_comfort_transmissibility/scripts/ride_core.py new file mode 100644 index 0000000000..453b387a25 --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/scripts/ride_core.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""乘坐舒适性传递率核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +问题:车在某个频率的路面激励下,座椅/车身会被放大多少倍? +共振峰在哪里、有多高?按 ISO 2631 加权后属于哪个舒适度等级? + +模型(单自由度基础激励,位移传递率): + + r = f / f_n 频率比 + T(r) = sqrt((1 + (2ζr)²) / + ((1 − r²)² + (2ζr)²)) 传递率(放大倍数) + +三条闭式结论,正好用来写断言: + + 峰值位置 r_p = sqrt(1 − 2ζ²) (ζ ≥ 1/√2 时没有峰,T ≡ 1) + 峰值大小 T_p = 1 / (2ζ·sqrt(1 − ζ²)) + 高频衰减 T(r) → 0(r 越大越快趋近,r=10 时约 1/r² 量级) + +ISO 2631-1 的舒适度等级按加权加速度 a_w(m/s²,RMS)划分: + + < 0.315 舒适 + 0.315 ~ 0.63 略有不适 + 0.63 ~ 1.0 不适 + 1.0 ~ 1.6 很不适 + 1.6 ~ 2.5 极不适 + > 2.5 无法忍受 +""" + +from __future__ import division, print_function, unicode_literals + +import math + +ASSERT_NAMES = (u"共振位置", u"峰值倍率", u"高频衰减", u"输入线性") + +# (上限, 等级名),从低到高排列 +COMFORT_SCALE = ( + (0.315, u"舒适"), + (0.63, u"略有不适"), + (1.0, u"不适"), + (1.6, u"很不适"), + (2.5, u"极不适"), +) + + +class RideError(ValueError): + """参数非法。""" + + +def _positive(name, value): + if value is None or value <= 0: + raise RideError(u"%s 必须大于 0,收到 %r" % (name, value)) + return value + + +class RideSpec(object): + """悬架与激励参数。""" + + def __init__(self, fn_hz=1.2, damping=0.25, input_accel=0.5, + road_freq_hz=6.0): + self.fn_hz = _positive(u"固有频率", fn_hz) + # 上限取 1/sqrt(2)≈0.707:超过它传递率曲线不再有经典共振峰, + # 闭式峰位 sqrt(1-2ζ²) 也失去意义,索性判为参数非法。 + if damping <= 0 or damping >= 1.0 / math.sqrt(2.0): + raise RideError(u"阻尼比必须在 (0, 1/sqrt(2)) 开区间,收到 %r" + % (damping,)) + self.damping = damping + self.input_accel = _positive(u"输入加速度", input_accel) + self.road_freq_hz = _positive(u"路面激励频率", road_freq_hz) + + # ---- 传递率三件套 ---- + def ratio(self, freq_hz): + return freq_hz / self.fn_hz + + def transmissibility(self, freq_hz): + r = self.ratio(freq_hz) + zeta = self.damping + num = 1.0 + (2.0 * zeta * r) ** 2 + den = (1.0 - r * r) ** 2 + (2.0 * zeta * r) ** 2 + return math.sqrt(num / den) + + def transmissibility_at_ratio(self, r): + return self.transmissibility(r * self.fn_hz) + + def peak_ratio(self): + return math.sqrt(1.0 - 2.0 * self.damping ** 2) + + def peak_value(self): + """在 r_p = sqrt(1 − 2ζ²) 处取传递率,得到真实的峰值(ζ ≥ 1/√2 时为 1)。""" + zeta = self.damping + r2 = 1.0 - 2.0 * zeta ** 2 + w2 = 4.0 * zeta ** 2 + num = 1.0 + w2 * r2 + den = w2 * (1.0 - zeta ** 2) + return math.sqrt(num / den) + + def weighted_accel(self, freq_hz): + return self.transmissibility(freq_hz) * self.input_accel + + def __repr__(self): + return ("RideSpec(fn_hz=%.3f, damping=%.3f, input_accel=%.3f)" + % (self.fn_hz, self.damping, self.input_accel)) + + +def comfort_grade(aw): + """把加权加速度(m/s²)换成 ISO 2631 的等级名。""" + if aw < 0: + raise RideError(u"加权加速度不能为负,收到 %r" % (aw,)) + for limit, name in COMFORT_SCALE: + if aw < limit: + return name + return u"无法忍受" + + +def scan(spec, ratios): + """在给定频率比上取表:[(r, 频率, 传递率, 加权加速度), ...]。""" + rows = [] + for r in ratios: + if r <= 0: + raise RideError(u"频率比必须大于 0,收到 %r" % (r,)) + freq = r * spec.fn_hz + t = spec.transmissibility_at_ratio(r) + rows.append((r, freq, t, t * spec.input_accel)) + return rows + + +DEFAULT_RATIOS = (0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 2.0, 3.0, 5.0, 10.0) + + +def find_peak(spec, low=0.05, high=4.0, steps=400): + """在频率比区间上扫描,返回 (峰值频率比, 峰值传递率)。""" + if low <= 0 or high <= low or steps < 2: + raise RideError(u"扫描区间非法:low=%r high=%r steps=%r" + % (low, high, steps)) + best_r = low + best_t = spec.transmissibility_at_ratio(low) + for i in range(steps + 1): + r = low + (high - low) * i / steps + t = spec.transmissibility_at_ratio(r) + if t > best_t: + best_r, best_t = r, t + return best_r, best_t + + +def check_ride(spec, pos_tolerance=0.02, peak_tolerance=0.05, + high_freq_limit=0.1, linear_tolerance=0.02): + """四条断言:共振位置、峰值倍率、高频衰减、输入线性。""" + asserts = [] + + measured_r, measured_t = find_peak(spec) + theoretical_r = spec.peak_ratio() + pos_error = abs(measured_r - theoretical_r) + pos_detail = (u"扫描峰位 r=%.4f,理论 sqrt(1-2ζ²)=%.4f(偏差 %.4f,容差 %.3f)" + % (measured_r, theoretical_r, pos_error, pos_tolerance)) + pos_ok = pos_error <= pos_tolerance + 1e-12 + asserts.append({"type": ASSERT_NAMES[0], "passed": bool(pos_ok), + "detail": pos_detail}) + + theoretical_t = spec.peak_value() + rel = abs(measured_t - theoretical_t) / max(theoretical_t, 1e-12) + asserts.append({ + "type": ASSERT_NAMES[1], + "passed": bool(rel <= peak_tolerance + 1e-12), + "detail": (u"扫描峰值 %.4f,理论峰值 %.4f(相对偏差 %.4f,容差 %.2f)" + % (measured_t, theoretical_t, rel, peak_tolerance)), + }) + + far = spec.transmissibility_at_ratio(10.0) + asserts.append({ + "type": ASSERT_NAMES[2], + "passed": bool(far <= high_freq_limit + 1e-12), + "detail": (u"r=10 时传递率 %.6f,上限 %.3f(高频必须衰减)" + % (far, high_freq_limit)), + }) + + doubled = RideSpec(fn_hz=spec.fn_hz, damping=spec.damping, + input_accel=2.0 * spec.input_accel) + base = spec.weighted_accel(spec.road_freq_hz) + dbl = doubled.weighted_accel(spec.road_freq_hz) + rel_lin = abs(dbl - 2.0 * base) / max(2.0 * base, 1e-12) + asserts.append({ + "type": ASSERT_NAMES[3], + "passed": bool(rel_lin <= linear_tolerance + 1e-12), + "detail": (u"输入 %.2f → 加权 %.4f;输入加倍 → 加权 %.4f(相对偏差 %.4f)" + % (spec.input_accel, base, dbl, rel_lin)), + }) + return asserts + + +def render_transmissibility(spec, ratios=DEFAULT_RATIOS, width=40): + """按传递率画横向条形图(以峰值为满条)。""" + peak = spec.peak_value() + peak = peak if peak > 0 else 1.0 + lines = [u"传递率曲线(峰值 = 满条)", "-" * (width + 16)] + for r, freq, t, aw in scan(spec, ratios): + cells = int(round(t / peak * width)) + mark = u" <峰值" if abs(r - spec.peak_ratio()) < 0.13 else u"" + lines.append(u"r=%4.2f f=%5.2fHz |%s| T=%.3f%s" + % (r, freq, "#" * cells, t, mark)) + lines.append("-" * (width + 16)) + return "\n".join(lines) + + +def format_report(spec, asserts, ratios=DEFAULT_RATIOS): + """把参数、扫描表、断言与结论排成报告。""" + lines = [] + peak_r, peak_t = find_peak(spec) + lines.append(u"悬架:固有频率 %.3f Hz,阻尼比 %.3f,路面激励 %.2f Hz,输入 %.2f m/s²" + % (spec.fn_hz, spec.damping, spec.road_freq_hz, + spec.input_accel)) + lines.append(u"理论:峰位 r=%.4f,峰值 %.4f;ζ=%.3f" + % (spec.peak_ratio(), spec.peak_value(), spec.damping)) + lines.append(u"扫描:峰位 r=%.4f,峰值 %.4f" % (peak_r, peak_t)) + aw = spec.weighted_accel(spec.road_freq_hz) + lines.append(u"舒适性:加权加速度 %.3f m/s² → 等级「%s」" + % (aw, comfort_grade(aw))) + lines.append(u"频率比 频率(Hz) 传递率 加权(m/s²)") + for r, freq, t, a in scan(spec, ratios): + lines.append(u"%6.2f %8.3f %6.3f %8.3f" % (r, freq, t, a)) + 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 RideParams(object): + """把 /xxx/yyy 形式的参数名收成对象,缺省时用内置默认值。""" + + def __init__(self, **kwargs): + self.spec = RideSpec( + fn_hz=kwargs.get("suspension/fn_hz", 1.2), + damping=kwargs.get("suspension/damping", 0.25), + input_accel=kwargs.get("road/input_accel", 0.5), + road_freq_hz=kwargs.get("road/freq_hz", 6.0), + ) + self.pos_tolerance = kwargs.get("check/pos_tolerance", 0.02) + self.peak_tolerance = kwargs.get("check/peak_tolerance", 0.05) + self.high_freq_limit = kwargs.get("check/high_freq_limit", 0.1) + self.linear_tolerance = kwargs.get("check/linear_tolerance", 0.02) + self.scan_low = kwargs.get("scan/low", 0.05) + self.scan_high = kwargs.get("scan/high", 4.0) + self.scan_steps = int(kwargs.get("scan/steps", 400)) diff --git a/src/ground/ride_comfort_transmissibility/scripts/ride_node.py b/src/ground/ride_comfort_transmissibility/scripts/ride_node.py new file mode 100644 index 0000000000..a459e1837a --- /dev/null +++ b/src/ground/ride_comfort_transmissibility/scripts/ride_node.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""ride_comfort_transmissibility 的 ROS 节点。 + +发给 rospy 的日志必须是 byte str,统一走 _log()。 +""" + +from __future__ import division, print_function, unicode_literals + +import os +import sys + +_HERE = os.path.dirname(os.path.abspath(__file__)) +if _HERE not in sys.path: + sys.path.insert(0, _HERE) + +import ride_core as core # noqa: E402 + + +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("ride_comfort_transmissibility", anonymous=False) + + def par(name, default): + return rospy.get_param(name, default) + + params = core.RideParams( + **{ + "suspension/fn_hz": par("/suspension/fn_hz", 1.2), + "suspension/damping": par("/suspension/damping", 0.25), + "road/input_accel": par("/road/input_accel", 0.5), + "road/freq_hz": par("/road/freq_hz", 6.0), + "check/pos_tolerance": par("/check/pos_tolerance", 0.02), + "check/peak_tolerance": par("/check/peak_tolerance", 0.05), + "check/high_freq_limit": par("/check/high_freq_limit", 0.1), + "check/linear_tolerance": par("/check/linear_tolerance", 0.02), + "scan/low": par("/scan/low", 0.05), + "scan/high": par("/scan/high", 4.0), + "scan/steps": par("/scan/steps", 400), + } + ) + + spec = params.spec + peak_r, peak_t = core.find_peak(spec, params.scan_low, params.scan_high, + params.scan_steps) + asserts = core.check_ride( + spec, + pos_tolerance=params.pos_tolerance, + peak_tolerance=params.peak_tolerance, + high_freq_limit=params.high_freq_limit, + linear_tolerance=params.linear_tolerance, + ) + + report = core.format_report(spec, asserts) + _log("info", report) + _log("info", core.render_transmissibility(spec)) + + pub_report = rospy.Publisher("/ride/report", std_msg.String, + queue_size=1, latch=True) + pub_curve = rospy.Publisher("/ride/curve", std_msg.Float64MultiArray, + queue_size=1, latch=True) + pub_peak = rospy.Publisher("/ride/peak", std_msg.Float64MultiArray, + queue_size=1, latch=True) + + report_msg = std_msg.String() + report_msg.data = report + pub_report.publish(report_msg) + + curve_msg = std_msg.Float64MultiArray() + curve = [] + for r, freq, t, aw in core.scan(spec, core.DEFAULT_RATIOS): + curve.extend([r, t, aw]) + curve_msg.data = curve + pub_curve.publish(curve_msg) + + peak_msg = std_msg.Float64MultiArray() + peak_msg.data = [spec.peak_ratio(), spec.peak_value(), peak_r, peak_t, + spec.weighted_accel(spec.road_freq_hz)] + pub_peak.publish(peak_msg) + + passed = len([a for a in asserts if a["passed"]]) + _log("info", u"已发布 /ride/report、/ride/curve、/ride/peak" + u"(断言 %d/%d,激励处等级「%s」)" + % (passed, len(asserts), + core.comfort_grade(spec.weighted_accel(spec.road_freq_hz)))) + + rate = rospy.Rate(par("/rate/publish_hz", 1.0)) + while not rospy.is_shutdown(): + pub_report.publish(report_msg) + pub_curve.publish(curve_msg) + pub_peak.publish(peak_msg) + rate.sleep() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/rollover_margin/CMakeLists.txt b/src/ground/rollover_margin/CMakeLists.txt new file mode 100644 index 0000000000..ce0a7fec4f --- /dev/null +++ b/src/ground/rollover_margin/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.0.2) +project(rollover_margin) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/rollover_core.py + scripts/rollover_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/rollover_margin/README.md b/src/ground/rollover_margin/README.md new file mode 100644 index 0000000000..5ac6cb9cc4 --- /dev/null +++ b/src/ground/rollover_margin/README.md @@ -0,0 +1,157 @@ +# rollover_margin —— 横向载荷转移与侧翻裕度 + +一辆车以 60 km/h 过半径 30 m 的弯,内侧车轮还剩多少垂向力?再快多少会抬轮? +这个包用三个参数(轮距、质心高度、质量)把答案算成一组可以断言的数字。 + +纯标准库,不依赖 CARLA / numpy / GUI;Melodic(Python 2.7)与 Noetic(Python 3)都能跑。 + +## 为什么需要这个模块 + +侧翻不是「车太轻」或「路太滑」,而是**横向载荷转移超过了内侧轮的静态载荷**。 +准静态下只有三个量在起作用: + +``` +静态稳定因子 SSF = track / (2·h_cg) +侧翻临界加速度 a_crit = SSF · g +载荷转移 ΔF_z = m·a_y·h_cg / track +``` + +于是内侧轮 `F_inner = m·g/2 − ΔF_z`,抬轮恰好发生在 `ΔF_z = m·g/2`, +即 `a_y = g·track/(2h_cg) = a_crit` —— 两条定义自洽,`ΔF_z/F_z0` 恒等于 `a_y/a_crit`。 +给定半径还能反推极限车速 `v_max = sqrt(a_crit·R)`。 + +这些全是代数式,可以被断言精确校验,不需要仿真器。 + +## 默认参数下的数字 + +车型:轮距 `1.54 m`、质心高 `0.55 m`、质量 `1500 kg`;工况:半径 `30 m`、车速 `60 km/h`。 + +| 量 | 值 | +|---|---| +| SSF | `1.4000` | +| 临界加速度 | `13.734 m/s²`(1.40 g) | +| 侧向加速度 | `9.259 m/s²` | +| 裕度 `a_y/a_crit` | `0.674` | +| 载荷转移 | `4960.3 N`(占静态 67.4%) | +| 内侧轮 / 外侧轮 | `2397.2 N` / `12317.8 N` | +| 该半径极限车速 | `73.1 km/h`(余量 13.1 km/h) | + +车速扫描(R = 30 m): + +| 车速 | `a_y` | 裕度 | 内侧轮 | 抬轮 | +|---|---|---|---|---| +| 30 | 2.315 | 0.169 | 6117.4 | 否 | +| 40 | 4.115 | 0.300 | 5152.9 | 否 | +| 50 | 6.430 | 0.468 | 3912.8 | 否 | +| 60 | 9.259 | 0.674 | 2397.2 | 否 | +| 70 | 12.603 | 0.918 | 606.0 | 否 | +| 80 | 16.461 | 1.199 | −1460.8 | **是** | +| 90 | 20.833 | 1.517 | −3803.2 | **是** | + +抬轮落在 70~80 km/h 之间,与极限车速 73.1 km/h 吻合。 + +## 四条断言 + +| 断言 | 判据 | 失败意味着 | +|---|---|---| +| **侧翻裕度** | `a_y/a_crit ≤ rollover_margin`(默认 0.70) | 过弯太快或车太高 | +| **内侧轮载荷** | `F_inner > 0` | 已抬轮,准静态模型失效 | +| **极限车速** | `v ≤ sqrt(a_crit·R)` | 超过该半径的物理上限 | +| **载荷转移** | `|ΔF_z/F_z0 − a_y/a_crit| ≤ tolerance` | 实现不自洽 | + +前三条讲安全性,第四条讲实现正确性 —— 所以抬高质心会让前三条一起翻,第四条仍然通过。 + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `vehicle/track` | `1.54` | 轮距(m) | +| `vehicle/cg_height` | `0.55` | 质心高度(m) | +| `vehicle/mass` | `1500.0` | 质量(kg) | +| `vehicle/gravity` | `9.81` | 重力加速度 | +| `corner/radius` | `30.0` | 弯道半径(m) | +| `corner/speed_kmh` | `60.0` | 过弯车速(km/h) | +| `check/rollover_margin` | `0.70` | 裕度上限 | +| `check/transfer_tolerance` | `0.02` | 自洽性容差 | +| `rate/publish_hz` | `10.0` | 打印完保持挂起的循环频率 | +| `topic/report` | `/rollover/report` | 文本报告(latched) | +| `topic/loads` | `/rollover/loads` | 车速扫描 CSV(latched) | +| `topic/critical_speed` | `/rollover/critical_speed` | 极限车速 Float64(km/h) | + +## 运行 + +```bash +roslaunch rollover_margin main.launch +roslaunch rollover_margin main.launch speed_kmh:=80 +roslaunch rollover_margin main.launch rollover_margin:=0.5 +roslaunch rollover_margin main.launch cg_height:=0.90 +``` + +打印完节点保持挂起,方便另一个终端核对: + +```bash +rosparam get /vehicle/track +rosparam get /check/rollover_margin +rosparam get /vehicle/cg_height +``` + +## 实测输出(默认) + +``` +车辆:轮距 1.54 m,质心高 0.55 m,质量 1500 kg +SSF = 1.4000,侧翻临界加速度 13.734 m/s²(1.40 g) +工况:半径 30.0 m,车速 60.0 km/h(16.67 m/s) +侧向加速度 9.259 m/s²,裕度比值 0.674 +载荷转移 4960.3 N(占比 0.674),内侧 2397.2 N / 外侧 12317.8 N +该半径极限车速 73.1 km/h(余量 13.1 km/h) +断言:4 通过 / 0 失败(共 4 条) + [PASS] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.70) + [PASS] 内侧轮载荷 内侧轮 2397.2 N,外侧轮 12317.8 N(静态 7357.5 N) + [PASS] 极限车速 当前 60.0 km/h,该半径极限 73.1 km/h(余量 13.1 km/h) + [PASS] 载荷转移 ΔF_z/F_z0 = 0.6742,a_y/a_crit = 0.6742 +结论:PASSED +``` + +## 两个反例 + +```bash +roslaunch rollover_margin main.launch rollover_margin:=0.5 # 只翻「侧翻裕度」 +roslaunch rollover_margin main.launch cg_height:=0.90 # 裕度 + 内侧轮 + 极限车速 一起翻 +``` + +第二个更有意思:质心抬到 0.90 m 后 SSF 掉到 `0.8556`,临界加速度 `8.393 m/s²` +低于 60 km/h 所需的 `9.259`,于是内侧轮载荷变成 **−759.4 N**(物理上已经翻了), +极限车速掉到 `57.1 km/h`;而「载荷转移」依然通过,因为它校验的是模型自洽而非安全性。 + +## 离线测试 + +```bash +cd ~/catkin_ws17 +python3 tests/test_rollover_margin_local.py +``` + +不需要 ROS / CARLA / 显示器。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `tire_slip_curve` | 附着系数从哪来(滑移率决定 μ) | +| `braking_distance_check` | 纵向:从看到到停住要几米 | +| `turning_sweep_check` | 转弯时车身占多宽(几何包络) | +| **`rollover_margin`(本包)** | **横向:载荷转移多少、何时抬轮、极限车速** | + +`turning_sweep_check` 回答「这个弯转得过去吗(不压线)」,本包回答「这个弯转得过去吗(不翻车)」。 + +## 常见问题 + +**Q:为什么不建模悬架和轮胎侧偏?** +A:准静态阈值主要由 `track/2h_cg` 决定,悬架柔度只让实际阈值低 5~10%。本包要把公式讲清楚, +闭式解更容易校验;真车标定时把 `rollover_margin` 收到 0.6 即可覆盖这一偏差。 + +**Q:内侧轮载荷为负代表什么?** +A:代表该侧车轮已离地,准静态模型到此失效(真实车辆此时绕外侧轮倾覆)。所以断言只判 `> 0`。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/src/ground/rollover_margin/config/rollover_margin.yaml b/src/ground/rollover_margin/config/rollover_margin.yaml new file mode 100644 index 0000000000..40c46da283 --- /dev/null +++ b/src/ground/rollover_margin/config/rollover_margin.yaml @@ -0,0 +1,25 @@ +# 横向载荷转移与侧翻裕度 —— 默认参数 +# 都能在 roslaunch 命令行用同名 覆盖,例如: +# roslaunch rollover_margin main.launch speed_kmh:=80 cg_height:=0.90 + +vehicle: + track: 1.54 # 轮距(m) + cg_height: 0.55 # 质心高度(m) + mass: 1500.0 # 整车质量(kg) + gravity: 9.81 # 重力加速度(m/s²) + +corner: + radius: 30.0 # 弯道半径(m) + speed_kmh: 60.0 # 过弯车速(km/h) + +check: + rollover_margin: 0.70 # 允许的 a_y / a_crit 上限 + transfer_tolerance: 0.02 # 载荷转移自洽性容差 + +rate: + publish_hz: 10.0 + +topic: + report: /rollover/report + loads: /rollover/loads + critical_speed: /rollover/critical_speed diff --git a/src/ground/rollover_margin/launch/main.launch b/src/ground/rollover_margin/launch/main.launch new file mode 100644 index 0000000000..02ee5da9a6 --- /dev/null +++ b/src/ground/rollover_margin/launch/main.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/rollover_margin/package.xml b/src/ground/rollover_margin/package.xml new file mode 100644 index 0000000000..c6fdfe9deb --- /dev/null +++ b/src/ground/rollover_margin/package.xml @@ -0,0 +1,19 @@ + + + rollover_margin + 1.0.0 + + 横向载荷转移与侧翻裕度核算:由轮距、质心高度与过弯工况算出静态稳定因子 SSF、 + 侧翻临界侧向加速度、内外侧轮垂向力与极限车速。纯标准库,不依赖 CARLA / numpy / GPU。 + + + Wist-sutton + MIT + + catkin + + rospy + std_msgs + + + diff --git a/src/ground/rollover_margin/scripts/main.py b/src/ground/rollover_margin/scripts/main.py new file mode 100644 index 0000000000..5555980bc7 --- /dev/null +++ b/src/ground/rollover_margin/scripts/main.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""入口:把脚本目录加进 sys.path 后转交给 rollover_node.main。""" +from __future__ import division, print_function + +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from rollover_node import main # noqa: E402 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/rollover_margin/scripts/rollover_core.py b/src/ground/rollover_margin/scripts/rollover_core.py new file mode 100644 index 0000000000..2bbd25cbe9 --- /dev/null +++ b/src/ground/rollover_margin/scripts/rollover_core.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""横向载荷转移与侧翻裕度核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +问题:一辆车以速度 v 过半径 R 的弯,会不会翻? + +准静态模型只有三个量: + + 静态稳定因子 SSF = track / (2 · h_cg) + 侧翻临界加速度 a_crit = SSF · g + 轮距侧载荷转移 ΔF_z = m · a_y · h_cg / track + +于是: + + 内侧轮垂向力 F_inner = m·g/2 − ΔF_z + 外侧轮垂向力 F_outer = m·g/2 + ΔF_z + 抬轮(内侧轮离地)恰好发生在 ΔF_z = m·g/2, + 也就是 a_y = g · track / (2·h_cg) = a_crit —— 两条定义自洽。 + +给定弯道半径还能反推极限车速 v_max = sqrt(a_crit · R)。 +这套关系全部是代数式,可以被断言精确校验,不需要仿真器。 + +Python 2.7 / 3.x 双兼容。 +""" +from __future__ import division, print_function, unicode_literals + +import io +import math + +__all__ = [ + "RolloverError", "VehicleSpec", "Cornering", "check_cornering", "all_passed", + "speed_table", "render_loads", "format_report", "RolloverParams", + "PARAM_KEYS", "PARAM_DEFAULTS", "ASSERT_NAMES", "SPEED_TABLE_KMH", + "to_message_text", +] + +# ── 默认参数 ───────────────────────────────────────────────────────────── + +DEFAULT_TRACK = 1.54 # 轮距(m) +DEFAULT_CG_HEIGHT = 0.55 # 质心高度(m) +DEFAULT_MASS = 1500.0 # 整车质量(kg) +DEFAULT_CURVE_RADIUS = 30.0 # 弯道半径(m) +DEFAULT_SPEED_KMH = 60.0 # 过弯车速(km/h) +DEFAULT_ROLLOVER_MARGIN = 0.70 # 允许的 a_y / a_crit 上限 +DEFAULT_TRANSFER_TOLERANCE = 0.02 +DEFAULT_GRAVITY = 9.81 +DEFAULT_PUBLISH_HZ = 10.0 +DEFAULT_GRID_WIDTH = 60 +DEFAULT_GRID_HEIGHT = 12 + +SPEED_TABLE_KMH = (30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0) + +ASSERT_NAMES = (u"侧翻裕度", u"内侧轮载荷", u"极限车速", u"载荷转移") + +PARAM_DEFAULTS = { + "vehicle/track": DEFAULT_TRACK, + "vehicle/cg_height": DEFAULT_CG_HEIGHT, + "vehicle/mass": DEFAULT_MASS, + "vehicle/gravity": DEFAULT_GRAVITY, + "corner/radius": DEFAULT_CURVE_RADIUS, + "corner/speed_kmh": DEFAULT_SPEED_KMH, + "check/rollover_margin": DEFAULT_ROLLOVER_MARGIN, + "check/transfer_tolerance": DEFAULT_TRANSFER_TOLERANCE, + "rate/publish_hz": DEFAULT_PUBLISH_HZ, + "topic/report": u"/rollover/report", + "topic/loads": u"/rollover/loads", + "topic/critical_speed": u"/rollover/critical_speed", +} + +PARAM_KEYS = tuple(sorted(PARAM_DEFAULTS)) + + +class RolloverError(ValueError): + """参数不合法。""" + + +# ── 小工具 ─────────────────────────────────────────────────────────────── + +def _as_float(value, name, default=None): + if value is None: + if default is None: + raise RolloverError(u"缺少参数:%s" % name) + return float(default) + if isinstance(value, bool): + raise RolloverError(u"参数 %s 不能是布尔值" % name) + try: + out = float(value) + except (TypeError, ValueError): + raise RolloverError(u"参数 %s 不是数字:%r" % (name, value)) + if math.isnan(out) or math.isinf(out): + raise RolloverError(u"参数 %s 不是有限数:%r" % (name, value)) + return out + + +def _positive(value, name): + out = _as_float(value, name) + if out <= 0.0: + raise RolloverError(u"参数 %s 必须为正:%r" % (name, value)) + return out + + +def _non_negative(value, name): + out = _as_float(value, name) + if out < 0.0: + raise RolloverError(u"参数 %s 不能为负:%r" % (name, value)) + return out + + +def to_message_text(text): + try: + unicode # noqa: F821 + except NameError: + return text + if isinstance(text, unicode): # noqa: F821 + return text.encode("utf-8") + return text + + +# ── 数据模型 ───────────────────────────────────────────────────────────── + +class VehicleSpec(object): + """车辆的准静态侧倾相关几何与质量参数。""" + + __slots__ = ("track", "cg_height", "mass", "gravity") + + def __init__(self, track=DEFAULT_TRACK, cg_height=DEFAULT_CG_HEIGHT, + mass=DEFAULT_MASS, gravity=DEFAULT_GRAVITY): + self.track = _positive(track, "track") + self.cg_height = _positive(cg_height, "cg_height") + self.mass = _positive(mass, "mass") + self.gravity = _positive(gravity, "gravity") + + # 基本量 ------------------------------------------------------------ + + def ssf(self): + """静态稳定因子 SSF = track / (2·h_cg)。""" + return self.track / (2.0 * self.cg_height) + + def critical_accel(self): + """侧翻临界侧向加速度(m/s²),等于 SSF·g。""" + return self.ssf() * self.gravity + + def wheel_load_static(self): + """单侧静态垂向力(N)。""" + return self.mass * self.gravity / 2.0 + + def transfer(self, lateral_accel): + """横向载荷转移量 ΔF_z(N),正数表示从内侧移到外侧。""" + ay = _as_float(lateral_accel, "lateral_accel") + return self.mass * ay * self.cg_height / self.track + + def inner_load(self, lateral_accel): + return self.wheel_load_static() - self.transfer(lateral_accel) + + def outer_load(self, lateral_accel): + return self.wheel_load_static() + self.transfer(lateral_accel) + + def critical_speed(self, radius): + """给定半径的极限过弯车速(m/s)。""" + radius = _positive(radius, "radius") + return math.sqrt(self.critical_accel() * radius) + + def as_tuple(self): + return (self.track, self.cg_height, self.mass, self.gravity) + + +class Cornering(object): + """一次过弯工况。""" + + __slots__ = ("spec", "radius", "speed_kmh", "speed_mps") + + def __init__(self, spec=None, radius=DEFAULT_CURVE_RADIUS, + speed_kmh=DEFAULT_SPEED_KMH): + self.spec = spec or VehicleSpec() + self.radius = _positive(radius, "radius") + self.speed_kmh = _as_float(speed_kmh, "speed_kmh") + if self.speed_kmh < 0.0: + raise RolloverError(u"车速不能为负:%r" % speed_kmh) + self.speed_mps = self.speed_kmh / 3.6 + + # 派生量 ------------------------------------------------------------ + + def lateral_accel(self): + return self.speed_mps * self.speed_mps / self.radius + + def usage(self): + """a_y / a_crit,越小越安全;>= 1 表示已抬轮。""" + return self.lateral_accel() / self.spec.critical_accel() + + def transfer(self): + return self.spec.transfer(self.lateral_accel()) + + def inner_load(self): + return self.spec.inner_load(self.lateral_accel()) + + def outer_load(self): + return self.spec.outer_load(self.lateral_accel()) + + def transfer_ratio(self): + """ΔF_z / F_z0,理论上恒等于 a_y / a_crit。""" + return self.transfer() / self.spec.wheel_load_static() + + def lifts_off(self): + return self.inner_load() <= 0.0 + + def speed_limit(self): + return self.spec.critical_speed(self.radius) + + def speed_limit_kmh(self): + return self.speed_limit() * 3.6 + + def headroom_kmh(self): + return self.speed_limit_kmh() - self.speed_kmh + + def safe_speed(self, margin=DEFAULT_ROLLOVER_MARGIN): + """在给定裕度下允许的最高车速(km/h)。""" + margin = _positive(margin, "rollover_margin") + return math.sqrt(margin * self.spec.critical_accel() * self.radius) * 3.6 + + def as_row(self): + return (round(self.speed_kmh, 2), round(self.lateral_accel(), 4), + round(self.usage(), 4), bool(self.lifts_off())) + + +# ── 四条断言 ───────────────────────────────────────────────────────────── + +def check_cornering(cornering, rollover_margin=DEFAULT_ROLLOVER_MARGIN, + transfer_tolerance=DEFAULT_TRANSFER_TOLERANCE): + """返回四条断言,结构为 {type, passed, detail}。""" + cornering = cornering or Cornering() + margin = _positive(rollover_margin, "rollover_margin") + tol = _as_float(transfer_tolerance, "transfer_tolerance") + spec = cornering.spec + usage = cornering.usage() + results = [] + + # 1) 侧翻裕度 + results.append({ + "type": ASSERT_NAMES[0], + "passed": bool(usage <= margin + 1e-12), + "detail": u"a_y %.3f m/s²,a_crit %.3f m/s²,比值 %.3f(上限 %.2f)" % ( + cornering.lateral_accel(), spec.critical_accel(), usage, margin), + }) + + # 2) 内侧轮载荷 + inner = cornering.inner_load() + results.append({ + "type": ASSERT_NAMES[1], + "passed": bool(inner > 0.0), + "detail": u"内侧轮 %.1f N,外侧轮 %.1f N(静态 %.1f N)" % ( + inner, cornering.outer_load(), spec.wheel_load_static()), + }) + + # 3) 极限车速 + limit = cornering.speed_limit_kmh() + results.append({ + "type": ASSERT_NAMES[2], + "passed": bool(cornering.speed_kmh <= limit + 1e-9), + "detail": u"当前 %.1f km/h,该半径极限 %.1f km/h(余量 %.1f km/h)" % ( + cornering.speed_kmh, limit, cornering.headroom_kmh()), + }) + + # 4) 载荷转移自洽:ΔF_z/F_z0 应等于 a_y/a_crit + ratio = cornering.transfer_ratio() + gap = abs(ratio - usage) + results.append({ + "type": ASSERT_NAMES[3], + "passed": bool(gap <= tol + 1e-12), + "detail": u"ΔF_z/F_z0 = %.4f,a_y/a_crit = %.4f,偏差 %.2e(容差 %.2f)" % ( + ratio, usage, gap, tol), + }) + + return results + + +def all_passed(results): + return all(item["passed"] for item in results) + + +def speed_table(spec=None, radius=DEFAULT_CURVE_RADIUS, speeds=SPEED_TABLE_KMH): + """同一半径下、不同车速的裕度表。""" + spec = spec or VehicleSpec() + rows = [] + for kmh in speeds: + item = Cornering(spec, radius, kmh) + rows.append(item) + return rows + + +# ── 字符图 ─────────────────────────────────────────────────────────────── + +def render_loads(cornering, width=DEFAULT_GRID_WIDTH, height=DEFAULT_GRID_HEIGHT): + """画两条横向载荷条:内侧(左)与外侧(右),中间是静态载荷中线。""" + width = int(_as_float(width, "width", DEFAULT_GRID_WIDTH)) + height = int(_as_float(height, "height", DEFAULT_GRID_HEIGHT)) + if width < 20 or height < 5: + raise RolloverError(u"网格太小:%dx%d" % (width, height)) + spec = cornering.spec + total = spec.mass * spec.gravity + scale = max(total, 1e-9) + inner = cornering.inner_load() + outer = cornering.outer_load() + inner_cells = int(round(max(0.0, inner) / scale * (width // 2 - 2))) + outer_cells = int(round(max(0.0, outer) / scale * (width // 2 - 2))) + + lines = [] + mid = width // 2 + for row in range(height): + cells = [" "] * width + cells[mid] = "|" + # 内侧(左)从中间往左画 + if inner_cells > 0: + for k in range(inner_cells): + pos = mid - 1 - k + if pos >= 0: + cells[pos] = "#" + # 外侧(右)从中间往右画 + if outer_cells > 0: + for k in range(outer_cells): + pos = mid + 1 + k + if pos < width: + cells[pos] = "=" + lines.append("".join(cells)) + lines.append(u"内侧 %5.0f N%s %.0f N 外侧" % (inner, " " * max(1, width // 2 - 16), outer)) + return lines + + +# ── 报告 ───────────────────────────────────────────────────────────────── + +def format_report(cornering, results, header=""): + cornering = cornering or Cornering() + spec = cornering.spec + passed = sum(1 for item in results if item["passed"]) + lines = [] + if header: + lines.append(header) + lines.append(u"车辆:轮距 %.2f m,质心高 %.2f m,质量 %.0f kg" % ( + spec.track, spec.cg_height, spec.mass)) + lines.append(u"SSF = %.4f,侧翻临界加速度 %.3f m/s²(%.2f g)" % ( + spec.ssf(), spec.critical_accel(), spec.ssf())) + lines.append(u"工况:半径 %.1f m,车速 %.1f km/h(%.2f m/s)" % ( + cornering.radius, cornering.speed_kmh, cornering.speed_mps)) + lines.append(u"侧向加速度 %.3f m/s²,裕度比值 %.3f" % ( + cornering.lateral_accel(), cornering.usage())) + lines.append(u"载荷转移 %.1f N(占比 %.3f),内侧 %.1f N / 外侧 %.1f N" % ( + cornering.transfer(), cornering.transfer_ratio(), + cornering.inner_load(), cornering.outer_load())) + lines.append(u"该半径极限车速 %.1f km/h(余量 %.1f km/h)" % ( + cornering.speed_limit_kmh(), cornering.headroom_kmh())) + lines.append(u"-" * 60) + lines.append(u"横向载荷(| 为静态中线,# 内侧,= 外侧):") + for line in render_loads(cornering): + lines.append(line) + lines.append(u"-" * 60) + lines.append(u"车速扫描(半径 %.1f m):" % cornering.radius) + lines.append(u" 车速(km/h) a_y(m/s²) 裕度 内侧轮(N) 是否抬轮") + for item in speed_table(spec, cornering.radius): + lines.append(u" %9.1f %9.3f %5.3f %9.1f %s" % ( + item.speed_kmh, item.lateral_accel(), item.usage(), + item.inner_load(), u"是" if item.lifts_off() else u"否")) + lines.append(u"-" * 60) + lines.append(u"断言:%d 通过 / %d 失败(共 %d 条)" % ( + passed, len(results) - passed, len(results))) + for item in results: + lines.append(u" [%s] %s %s" % ( + "PASS" if item["passed"] else "FAIL", item["type"], item["detail"])) + lines.append(u"结论:%s" % (u"PASSED" if passed == len(results) else u"FAILED")) + return "\n".join(lines) + + +# ── 参数读取 ───────────────────────────────────────────────────────────── + +class RolloverParams(object): + def __init__(self, **kwargs): + for name in PARAM_KEYS: + setattr(self, "params_" + name.replace("/", "_"), + kwargs.get(name, PARAM_DEFAULTS[name])) + self.track = _positive(self.params_vehicle_track, "vehicle/track") + self.cg_height = _positive(self.params_vehicle_cg_height, "vehicle/cg_height") + self.mass = _positive(self.params_vehicle_mass, "vehicle/mass") + self.gravity = _positive(self.params_vehicle_gravity, "vehicle/gravity") + self.radius = _positive(self.params_corner_radius, "corner/radius") + self.speed_kmh = _non_negative(self.params_corner_speed_kmh, "corner/speed_kmh") + self.rollover_margin = _positive( + self.params_check_rollover_margin, "check/rollover_margin") + self.transfer_tolerance = _non_negative( + self.params_check_transfer_tolerance, "check/transfer_tolerance") + self.publish_hz = _positive(self.params_rate_publish_hz, "rate/publish_hz") + self.topic_report = self.params_topic_report + self.topic_loads = self.params_topic_loads + self.topic_critical_speed = self.params_topic_critical_speed + + @classmethod + def from_params(cls, get_param): + values = {} + for name in PARAM_KEYS: + values[name] = get_param(name, PARAM_DEFAULTS[name]) + return cls(**values) + + def vehicle(self): + return VehicleSpec(self.track, self.cg_height, self.mass, self.gravity) + + def cornering(self): + return Cornering(self.vehicle(), self.radius, self.speed_kmh) + + def as_dict(self): + return { + "vehicle/track": self.track, + "vehicle/cg_height": self.cg_height, + "vehicle/mass": self.mass, + "vehicle/gravity": self.gravity, + "corner/radius": self.radius, + "corner/speed_kmh": self.speed_kmh, + "check/rollover_margin": self.rollover_margin, + "check/transfer_tolerance": self.transfer_tolerance, + "rate/publish_hz": self.publish_hz, + } diff --git a/src/ground/rollover_margin/scripts/rollover_node.py b/src/ground/rollover_margin/scripts/rollover_node.py new file mode 100644 index 0000000000..95784268e2 --- /dev/null +++ b/src/ground/rollover_margin/scripts/rollover_node.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""侧翻裕度节点的 ROS 封装(ROS 1 / Melodic,Python 2.7 & 3.x)。""" +from __future__ import division, print_function, unicode_literals + +import os +import sys +import traceback + +import rospy +from std_msgs.msg import Float64, String + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +import rollover_core as core # noqa: E402 + + +def _log(level, message): + func = getattr(rospy, "log" + level) + try: + unicode # noqa: F821 + except NameError: + func(message) + return + if isinstance(message, unicode): # noqa: F821 + func(message.encode("utf-8")) + else: + func(message) + + +class RolloverNode(object): + def __init__(self): + self.params = core.RolloverParams.from_params(rospy.get_param) + self.cornering = self.params.cornering() + self.results = core.check_cornering( + self.cornering, self.params.rollover_margin, + self.params.transfer_tolerance) + self.report = core.format_report( + self.cornering, self.results, + header=u"横向载荷转移与侧翻裕度(准静态,不依赖 CARLA)") + + self.pub_report = rospy.Publisher( + self.params.topic_report, String, queue_size=1, latch=True) + self.pub_loads = rospy.Publisher( + self.params.topic_loads, String, queue_size=1, latch=True) + self.pub_speed = rospy.Publisher( + self.params.topic_critical_speed, Float64, queue_size=1, latch=True) + rospy.on_shutdown(self.on_shutdown) + + def loads_text(self): + rows = [u"speed_kmh,lateral_accel,inner_load,outer_load,lift_off"] + for item in core.speed_table(self.cornering.spec, self.cornering.radius): + rows.append(u"%.1f,%.4f,%.1f,%.1f,%d" % ( + item.speed_kmh, item.lateral_accel(), + item.inner_load(), item.outer_load(), 1 if item.lifts_off() else 0)) + return u"\n".join(rows) + + def publish_all(self): + self.pub_report.publish(String(data=core.to_message_text(self.report))) + self.pub_loads.publish(String(data=core.to_message_text(self.loads_text()))) + self.pub_speed.publish(Float64(data=float(self.cornering.speed_limit_kmh()))) + + def on_shutdown(self): + try: + _log("info", u"已发布零控制量") + except Exception: # noqa: BLE001 + pass + + def run(self): + try: + spec = self.cornering.spec + _log("info", u"侧翻裕度节点已启动:轮距 %.2f m,质心高 %.2f m,质量 %.0f kg," + u"半径 %.1f m,车速 %.1f km/h" % ( + spec.track, spec.cg_height, spec.mass, + self.cornering.radius, self.cornering.speed_kmh)) + _log("info", u"SSF=%.4f,临界加速度 %.3f m/s²,极限车速 %.1f km/h" % ( + spec.ssf(), spec.critical_accel(), self.cornering.speed_limit_kmh())) + _log("info", u"发布 %s、%s、%s" % ( + self.params.topic_report, self.params.topic_loads, + self.params.topic_critical_speed)) + self.publish_all() + print(self.report) + passed = sum(1 for item in self.results if item["passed"]) + _log("info", u"结论:%s(%d/%d 条断言通过)" % ( + u"PASSED" if passed == len(self.results) else u"FAILED", + passed, len(self.results))) + rate = rospy.Rate(max(1.0, self.params.publish_hz)) + while not rospy.is_shutdown(): + rate.sleep() + return 0 + except Exception: # noqa: BLE001 + _log("err", u"侧翻裕度核算异常:%s" % traceback.format_exc()) + return 1 + + +def main(): + rospy.init_node("rollover_margin", anonymous=False) + node = RolloverNode() + code = node.run() + try: + node.publish_all() + except Exception: # noqa: BLE001 + pass + return code + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/tire_slip_curve/CMakeLists.txt b/src/ground/tire_slip_curve/CMakeLists.txt new file mode 100644 index 0000000000..5d8e35e1b3 --- /dev/null +++ b/src/ground/tire_slip_curve/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tire_slip_curve) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/tire_core.py + scripts/tire_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/tire_slip_curve/README.md b/src/ground/tire_slip_curve/README.md new file mode 100644 index 0000000000..4e45e541e6 --- /dev/null +++ b/src/ground/tire_slip_curve/README.md @@ -0,0 +1,176 @@ +# tire_slip_curve —— 轮胎滑移率-附着曲线核算 + +把一个「车轮在滑还是没在滑」的问题,做成一条**有闭式解**的附着曲线,用终端就能看清 +三件事:峰值附着在哪、车轮抱死后附着掉多少、ABS 为什么要把滑移率圈在一个窄带里。 + +纯标准库实现,不依赖 CARLA、numpy、GUI;在 Ubuntu 18.04 + ROS Melodic(Python 2.7) +和 Ubuntu 20.04 + Noetic(Python 3)上都能直接跑。 + +## 为什么需要这个模块 + +纵向动力学里最容易被忽略的一条:**附着系数不是常数,它是滑移率的函数**。 + +- 滑移率太小时轮胎还没「咬住」路面,纵向力上不去; +- 滑移率跨过峰值之后附着开始下降,车轮彻底抱死时只剩峰值的两三成; +- 所以 ABS 的目标从来不是「不滑」,而是「把滑移率压在峰值附近的那条带子里」。 + +这三个量都能用一条曲线讲清楚,而这条曲线的极大值、初段斜率、锁死值都可以解析求出, +于是可以被断言精确校验,不必依赖实测拟合。 + +## 曲线模型 + +以归一化滑移率 `ŝ = s / s_peak` 表示: + +``` +μ(ŝ) = μ_peak · 2·ŝ / (1 + ŝ²) +``` + +| 位置 | 取值 | 含义 | +|---|---|---| +| `ŝ = 0`(s = 0) | `μ = 0` | 自由滚动,没有纵向力 | +| `ŝ = 1`(s = s_peak) | `μ = μ_peak` | 峰值附着,曲线唯一极大 | +| `s = 1.0`(车轮抱死) | `μ = μ_peak · 2·k / (1 + k²)`,`k = 1/s_peak` | 锁死附着 | + +派生量也都是闭式解: + +- 初段刚度 `dμ/ds|₀ = 2·μ_peak / s_peak` +- 峰值滑移率 `s_peak` +- ABS 目标带 `[s_peak·(1−band), s_peak·(1+band)]` + +默认参数(`μ_peak = 0.90`、`s_peak = 0.12`)下:锁死附着 `0.2129`(只剩峰值的 **23.7%**), +初段刚度 `15.0`,目标带 `[0.096, 0.144]`。 + +## 目录结构 + +``` +src/ground/tire_slip_curve/ +├── CMakeLists.txt +├── package.xml +├── README.md +├── config/tire_slip_curve.yaml # 12 个参数 +├── launch/main.launch # 12 个带 doc 的 arg +└── scripts/ + ├── main.py # 入口(符合 main.* 约定) + ├── tire_core.py # 纯数学,不 import rospy + └── tire_node.py # ROS 收发 +tests/test_tire_slip_curve_local.py # 离线回归测试 +docs/ground/tire_slip_curve.md # 文档页 +``` + +## 四条断言 + +| 断言 | 判据 | +|---|---| +| **峰值唯一** | 采样点里的局部极大只有一个,且落在 `(s_peak, μ_peak)` 上(容差 `peak_tolerance`) | +| **锁死衰减** | `μ(1.0) / μ_peak ≤ lock_friction_ratio`(默认 0.85) | +| **初段线性** | `s → 0` 时 `μ ≈ (dμ/ds|₀)·s`,相对误差 ≤ `linear_tolerance`(默认 2%) | +| **目标带覆盖峰值** | ABS 目标带必须包含 `s_peak` | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `tire/peak_slip` | `0.12` | 峰值附着对应的滑移率(干路 0.10~0.15,冰雪 0.20~0.40) | +| `tire/peak_friction` | `0.90` | 峰值附着系数 | +| `tire/abs_band` | `0.20` | ABS 目标带宽比例 | +| `road/surface` | `dry_asphalt` | 路面标注:`dry_asphalt` / `wet_asphalt` / `snow` / `ice` | +| `curve/samples` | `51` | 曲线采样点数(≥5) | +| `check/lock_friction_ratio` | `0.85` | 锁死附着的上限比例 | +| `check/linear_tolerance` | `0.02` | 初段线性度容差 | +| `check/peak_tolerance` | `0.01` | 峰值位置/大小容差 | +| `rate/publish_hz` | `10.0` | 打印完后保持挂起的循环频率 | +| `topic/report` | `/tire/report` | 文本报告(latched, `std_msgs/String`) | +| `topic/curve` | `/tire/curve` | 曲线表 CSV 文本(latched) | +| `topic/peak_slip` | `/tire/peak_slip` | 峰值滑移率数值(latched, `std_msgs/Float64`) | + +## 运行 + +```bash +roslaunch tire_slip_curve main.launch +``` + +改参数(示例): + +```bash +roslaunch tire_slip_curve main.launch peak_slip:=0.30 peak_friction:=0.35 surface:=snow +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 +roslaunch tire_slip_curve main.launch samples:=5 +``` + +打印完节点会保持挂起,方便另一个终端核对参数: + +```bash +rosparam get /tire/peak_slip +rosparam get /check/lock_friction_ratio +``` + +## 实测输出(默认参数) + +``` +路面:dry_asphalt,峰值滑移率 0.120,峰值附着 0.900 +锁死点:s=1.00,附着 0.2129(占峰值 23.7%) +初段刚度:15.000 1/s,ABS 目标带 [0.0960, 0.1440](带宽 20%) +采样:51 点(s 从 0 到 1) +------------------------------------------------------------ +附着曲线(纵轴 0 ~ 0.95): + ******* + * ** + * *** + | **** + | **** + * ******* + | ********** + * ***************** + | * + | +* ... +0 滑移率 → 1.0 +------------------------------------------------------------ +断言:4 通过 / 0 失败(共 4 条) + [PASS] 峰值唯一 峰值 0.9000 @ s=0.1200(期望 0.9000 @ 0.1200),局部极大 1 个 + [PASS] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 85.0%) + [PASS] 初段线性 初段刚度 15.000,最大相对误差 0.99% @ s=0.0120(要求 <= 2.00%) + [PASS] 目标带覆盖峰值 目标带 [0.0960, 0.1440] 覆盖峰值滑移 0.1200 +结论:PASSED +``` + +## 两个反例 + +```bash +roslaunch tire_slip_curve main.launch samples:=5 # 采样太粗 → 峰值唯一失败 +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 # 实测 23.7% > 20% → 锁死衰减失败 +``` + +## 离线测试 + +```bash +cd ~/catkin_ws16 +python3 tests/test_tire_slip_curve_local.py +``` + +不需要 ROS、不需要 CARLA、不需要显示器;在仓库根目录执行时还会顺带校验文档页。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `braking_distance_check` | 给定附着系数,算制动距离 | +| `turning_sweep_check` | 给定转角与轴距,算转弯时车身占多宽 | +| **`tire_slip_curve`(本包)** | **附着系数从哪来:滑移率决定它,曲线给出峰值与锁死衰减** | + +换句话说:本包是上游,其它包把它算出的 `μ_peak` 当输入。 + +## 常见问题 + +**Q:为什么用这条简单的曲线,不用魔术公式(Pacejka)?** +A:魔术公式要拟合 4 个系数,没有解析极大值,断言只能靠数值搜索;本包要讲的是 +「峰值 - 锁死 - 目标带」这三个概念,用有闭式解的曲线反而更容易被校验和讲清。真车 +标定时把 `peak_slip`/`peak_friction` 换成实测值即可,曲线形状在 0~0.4 区间足够像。 + +**Q:Melodic 上运行时报 `UnicodeEncodeError`?** +A:rosgraph 的日志格式化对非 ASCII 的 unicode 会调 `str()`,节点里已经通过 `_log()` +统一转成 byte str,如果自己改代码请沿用这个函数。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/src/ground/tire_slip_curve/config/tire_slip_curve.yaml b/src/ground/tire_slip_curve/config/tire_slip_curve.yaml new file mode 100644 index 0000000000..7ee92f9d0e --- /dev/null +++ b/src/ground/tire_slip_curve/config/tire_slip_curve.yaml @@ -0,0 +1,35 @@ +# 轮胎滑移率-附着曲线核算 —— 默认参数 +# 所有值都可以在 roslaunch 命令行用同名 覆盖,例如: +# roslaunch tire_slip_curve main.launch peak_slip:=0.15 peak_friction:=0.75 + +tire: + # 峰值附着对应的滑移率(干路 0.10~0.15,湿路 0.12~0.20,冰雪 0.20~0.40) + peak_slip: 0.12 + # 峰值附着系数 + peak_friction: 0.90 + # ABS 目标带宽(相对峰值滑移率的比例),目标带 = peak_slip * (1 ± abs_band) + abs_band: 0.20 + +road: + # 路面类型,仅用于报告标注:dry_asphalt / wet_asphalt / snow / ice + surface: dry_asphalt + +curve: + # 曲线采样点数(s 从 0 到 1 均匀采样) + samples: 51 + +check: + # 锁死附着相对峰值的上限比例,越小要求越严 + lock_friction_ratio: 0.85 + # 初段线性度允许的相对误差 + linear_tolerance: 0.02 + # 峰值位置/大小的允许偏差 + peak_tolerance: 0.01 + +rate: + publish_hz: 10.0 + +topic: + report: /tire/report + curve: /tire/curve + peak_slip: /tire/peak_slip diff --git a/src/ground/tire_slip_curve/launch/main.launch b/src/ground/tire_slip_curve/launch/main.launch new file mode 100644 index 0000000000..cc4bd65dbc --- /dev/null +++ b/src/ground/tire_slip_curve/launch/main.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/tire_slip_curve/package.xml b/src/ground/tire_slip_curve/package.xml new file mode 100644 index 0000000000..17e3196c8b --- /dev/null +++ b/src/ground/tire_slip_curve/package.xml @@ -0,0 +1,19 @@ + + + tire_slip_curve + 1.0.0 + + 轮胎滑移率-附着曲线核算:用一条有闭式解的解析曲线,讲清峰值附着、锁死附着的衰减, + 以及 ABS 为什么要把滑移率控制在峰值附近。纯标准库,不依赖 CARLA / numpy / GPU。 + + + Wist-sutton + MIT + + catkin + + rospy + std_msgs + + + diff --git a/src/ground/tire_slip_curve/scripts/main.py b/src/ground/tire_slip_curve/scripts/main.py new file mode 100644 index 0000000000..e849340b94 --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/main.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""入口:把脚本目录加进 sys.path 后转交给 tire_node.main。""" +from __future__ import division, print_function + +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from tire_node import main # noqa: E402 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/tire_slip_curve/scripts/tire_core.py b/src/ground/tire_slip_curve/scripts/tire_core.py new file mode 100644 index 0000000000..17dfd9796c --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/tire_core.py @@ -0,0 +1,477 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""轮胎滑移率-附着曲线核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +模型:以归一化滑移率 ŝ = s / s_peak 表示的单调升-单调降附着曲线 + + μ(ŝ) = μ_peak * 2·ŝ / (1 + ŝ²) + + ŝ = 0 → μ = 0 (自由滚动,无纵向力) + ŝ = 1 → μ = μ_peak (峰值附着,唯一极大) + ŝ > 1 → μ 单调下降 (过度滑移,附着衰减) + ŝ = 1/s_peak 时 s = 1.0 (车轮锁死) + +这条曲线的好处是:峰值位置、峰值大小、初段斜率全都有闭式解,能被断言 +精确校验,不需要拟合。真实轮胎(魔术公式)在 0~0.4 的滑移区间形状与此 +接近,用它讲清「为什么 ABS 要把滑移率控在峰值附近」已经足够。 + +Python 2.7 / 3.x 双兼容。 +""" +from __future__ import division, print_function, unicode_literals + +import io +import json +import math + +__all__ = [ + "TireError", "TireSpec", "CurvePoint", "Curve", "build_curve", + "key_points", "check_curve", "all_passed", "render_curve", + "format_report", "TireParams", "PARAM_KEYS", "PARAM_DEFAULTS", + "ASSERT_NAMES", "to_message_text", +] + +# ── 默认参数 ───────────────────────────────────────────────────────────── + +DEFAULT_PEAK_SLIP = 0.12 +DEFAULT_PEAK_FRICTION = 0.90 +DEFAULT_ABS_BAND = 0.20 # 目标带 = peak_slip * (1 ± band) +DEFAULT_SURFACE = u"dry_asphalt" +DEFAULT_SAMPLES = 51 # 曲线采样点数(s 从 0 到 1) +DEFAULT_LOCK_FRICTION_RATIO = 0.85 # 锁死附着至少降到峰值的这个比例以下 +DEFAULT_LINEAR_TOLERANCE = 0.02 # 初段线性度允许的相对误差 +DEFAULT_PEAK_TOLERANCE = 0.01 +DEFAULT_PUBLISH_HZ = 10.0 +DEFAULT_GRID_WIDTH = 60 +DEFAULT_GRID_HEIGHT = 12 + +SURFACES = (u"dry_asphalt", u"wet_asphalt", u"snow", u"ice") + +ASSERT_NAMES = (u"峰值唯一", u"锁死衰减", u"初段线性", u"目标带覆盖峰值") + +PARAM_DEFAULTS = { + "tire/peak_slip": DEFAULT_PEAK_SLIP, + "tire/peak_friction": DEFAULT_PEAK_FRICTION, + "tire/abs_band": DEFAULT_ABS_BAND, + "road/surface": DEFAULT_SURFACE, + "curve/samples": DEFAULT_SAMPLES, + "check/lock_friction_ratio": DEFAULT_LOCK_FRICTION_RATIO, + "check/linear_tolerance": DEFAULT_LINEAR_TOLERANCE, + "check/peak_tolerance": DEFAULT_PEAK_TOLERANCE, + "rate/publish_hz": DEFAULT_PUBLISH_HZ, + "topic/report": u"/tire/report", + "topic/curve": u"/tire/curve", + "topic/peak_slip": u"/tire/peak_slip", +} + +PARAM_KEYS = tuple(sorted(PARAM_DEFAULTS)) + + +class TireError(ValueError): + """参数不合法。""" + + +# ── 小工具 ─────────────────────────────────────────────────────────────── + +def _as_float(value, name, default=None): + if value is None: + if default is None: + raise TireError(u"缺少参数:%s" % name) + return float(default) + if isinstance(value, bool): + raise TireError(u"参数 %s 不能是布尔值" % name) + try: + out = float(value) + except (TypeError, ValueError): + raise TireError(u"参数 %s 不是数字:%r" % (name, value)) + if math.isnan(out) or math.isinf(out): + raise TireError(u"参数 %s 不是有限数:%r" % (name, value)) + return out + + +def _as_int(value, name, default=None): + out = _as_float(value, name, default) + if abs(out - round(out)) > 1e-9: + raise TireError(u"参数 %s 必须是整数:%r" % (name, value)) + return int(round(out)) + + +def _positive(value, name): + out = _as_float(value, name) + if out <= 0.0: + raise TireError(u"参数 %s 必须为正:%r" % (name, value)) + return out + + +def _clamp(value, low, high): + return low if value < low else (high if value > high else value) + + +def to_message_text(text): + """把文本转成 rospy 能安全发布的形式(py2 → bytes,py3 → str)。""" + try: + unicode # noqa: F821 (Python 2) + except NameError: + return text + if isinstance(text, unicode): # noqa: F821 + return text.encode("utf-8") + return text + + +# ── 数据模型 ───────────────────────────────────────────────────────────── + +class TireSpec(object): + """一条轮胎-路面组合的附着曲线参数。""" + + __slots__ = ("peak_slip", "peak_friction", "abs_band", "surface") + + def __init__(self, peak_slip=DEFAULT_PEAK_SLIP, + peak_friction=DEFAULT_PEAK_FRICTION, + abs_band=DEFAULT_ABS_BAND, surface=DEFAULT_SURFACE): + self.peak_slip = _positive(peak_slip, "peak_slip") + if self.peak_slip > 1.0: + raise TireError(u"峰值滑移率必须 <= 1.0:%r" % peak_slip) + self.peak_friction = _positive(peak_friction, "peak_friction") + band = _as_float(abs_band, "abs_band") + if band < 0.0 or band >= 1.0: + raise TireError(u"ABS 目标带宽比例必须在 [0, 1):%r" % abs_band) + self.abs_band = band + name = surface if surface is not None else DEFAULT_SURFACE + if name not in SURFACES: + raise TireError(u"未知路面:%r(可选 %s)" % (surface, ", ".join(SURFACES))) + self.surface = name + + # 曲线与派生量的闭式解 ------------------------------------------------ + + def friction(self, slip): + """给定滑移率求附着系数。""" + slip = _as_float(slip, "slip") + if slip < 0.0: + slip = -slip + hat = slip / self.peak_slip + return self.peak_friction * 2.0 * hat / (1.0 + hat * hat) + + def slope_at_zero(self): + """初段刚度 dμ/ds|₀ = 2·μ_peak / s_peak(闭式解)。""" + return 2.0 * self.peak_friction / self.peak_slip + + def lock_slip(self): + """车轮完全锁死时的滑移率(本模型约定为 1.0)。""" + return 1.0 + + def lock_friction(self): + return self.friction(self.lock_slip()) + + def abs_band_limits(self): + """ABS 目标带 [low, high],并夹到 (0, 1]。""" + low = self.peak_slip * (1.0 - self.abs_band) + high = self.peak_slip * (1.0 + self.abs_band) + return _clamp(low, 1e-6, 1.0), _clamp(high, 1e-6, 1.0) + + def as_tuple(self): + return (self.peak_slip, self.peak_friction, self.abs_band, self.surface) + + +class CurvePoint(object): + """曲线上的一点。""" + + __slots__ = ("slip", "friction", "ratio") + + def __init__(self, slip, friction, ratio): + self.slip = slip + self.friction = friction + self.ratio = ratio # friction / peak_friction + + def as_row(self): + return (round(self.slip, 4), round(self.friction, 4), round(self.ratio, 4)) + + +class Curve(object): + """一条完整的附着曲线。""" + + def __init__(self, points, spec, name=""): + self.points = list(points) + self.spec = spec + self.name = name or ("%s(peak_slip=%.3f)" % (spec.surface, spec.peak_slip)) + + # 基本属性 ---------------------------------------------------------- + + @property + def count(self): + return len(self.points) + + def is_empty(self): + return not self.points + + def first(self): + return self.points[0] if self.points else None + + def last(self): + return self.points[-1] if self.points else None + + def at(self, slip): + """取最接近给定滑移率的采样点。""" + if not self.points: + return None + return min(self.points, key=lambda p: abs(p.slip - slip)) + + def max_point(self): + """曲线上附着系数最大的点。""" + if not self.points: + return None + return max(self.points, key=lambda p: p.friction) + + def monotonic_up_to(self, slip): + """[0, slip] 区间内是否严格单调递增。""" + prev = None + for point in self.points: + if point.slip > slip + 1e-12: + break + if prev is not None and point.friction <= prev: + return False + prev = point.friction + return True + + def ratio_at_lock(self): + last = self.last() + if last is None or last.ratio is None: + return 0.0 + return last.ratio + + def as_rows(self): + return [p.as_row() for p in self.points] + + +def build_curve(spec=None, samples=DEFAULT_SAMPLES): + """按 s = 0 … 1 均匀采样生成曲线。""" + spec = spec or TireSpec() + count = _as_int(samples, "samples", DEFAULT_SAMPLES) + if count < 5: + raise TireError(u"采样点数至少 5:%r" % samples) + points = [] + for index in range(count): + slip = index / (count - 1.0) + mu = spec.friction(slip) + points.append(CurvePoint(slip, mu, mu / spec.peak_friction)) + return Curve(points, spec) + + +def key_points(spec=None): + """返回报告里要用的关键量。""" + spec = spec or TireSpec() + low, high = spec.abs_band_limits() + return { + "surface": spec.surface, + "peak_slip": spec.peak_slip, + "peak_friction": spec.peak_friction, + "lock_slip": spec.lock_slip(), + "lock_friction": spec.lock_friction(), + "lock_ratio": spec.lock_friction() / spec.peak_friction, + "slope_at_zero": spec.slope_at_zero(), + "abs_band": spec.abs_band, + "abs_low": low, + "abs_high": high, + "abs_peak_friction": spec.friction(spec.peak_slip), + } + + +# ── 四条断言 ───────────────────────────────────────────────────────────── + +def check_curve(curve, spec=None, lock_friction_ratio=DEFAULT_LOCK_FRICTION_RATIO, + linear_tolerance=DEFAULT_LINEAR_TOLERANCE, + peak_tolerance=DEFAULT_PEAK_TOLERANCE): + """返回四条断言,结构为 {type, passed, detail}。""" + curve = curve or build_curve(spec) + spec = spec or curve.spec + lock_limit = _as_float(lock_friction_ratio, "lock_friction_ratio") + lin_tol = _as_float(linear_tolerance, "linear_tolerance") + peak_tol = _as_float(peak_tolerance, "peak_tolerance") + + results = [] + + # 1) 峰值唯一:采样点里的极大值只有一个,且落在峰值滑移附近 + peak = curve.max_point() + ups = [i for i in range(1, curve.count - 1) + if curve.points[i].friction > curve.points[i - 1].friction + and curve.points[i].friction > curve.points[i + 1].friction] + peak_ok = (peak is not None + and abs(peak.friction - spec.peak_friction) <= peak_tol + and abs(peak.slip - spec.peak_slip) <= max(peak_tol, 2.0 / curve.count) + and len(ups) <= 1) + results.append({ + "type": ASSERT_NAMES[0], + "passed": bool(peak_ok), + "detail": u"峰值 %.4f @ s=%.4f(期望 %.4f @ %.4f),局部极大 %d 个" % ( + peak.friction if peak else 0.0, peak.slip if peak else 0.0, + spec.peak_friction, spec.peak_slip, len(ups)), + }) + + # 2) 锁死衰减:s=1 的附着要明显低于峰值 + lock = spec.lock_friction() + ratio = lock / spec.peak_friction + results.append({ + "type": ASSERT_NAMES[1], + "passed": bool(ratio <= lock_limit + 1e-12), + "detail": u"锁死附着 %.4f,占峰值 %.1f%%(要求 <= %.1f%%)" % ( + lock, ratio * 100.0, lock_limit * 100.0), + }) + + # 3) 初段线性:s → 0 时 μ ≈ slope·s + worst = 0.0 + worst_s = 0.0 + probe = spec.peak_slip * 0.1 + for k in range(1, 6): + slip = probe * k / 5.0 + if slip <= 0.0: + continue + approx = spec.slope_at_zero() * slip + actual = spec.friction(slip) + rel = abs(actual - approx) / max(approx, 1e-9) + if rel > worst: + worst, worst_s = rel, slip + results.append({ + "type": ASSERT_NAMES[2], + "passed": bool(worst <= lin_tol + 1e-12), + "detail": u"初段刚度 %.3f,最大相对误差 %.2f%% @ s=%.4f(要求 <= %.2f%%)" % ( + spec.slope_at_zero(), worst * 100.0, worst_s, lin_tol * 100.0), + }) + + # 4) 目标带覆盖峰值:ABS 目标区间必须包住峰值滑移 + low, high = spec.abs_band_limits() + covered = low <= spec.peak_slip <= high + results.append({ + "type": ASSERT_NAMES[3], + "passed": bool(covered), + "detail": u"目标带 [%.4f, %.4f] 覆盖峰值滑移 %.4f" % (low, high, spec.peak_slip), + }) + + return results + + +def all_passed(results): + return all(item["passed"] for item in results) + + +# ── 字符图 ─────────────────────────────────────────────────────────────── + +def render_curve(curve, width=DEFAULT_GRID_WIDTH, height=DEFAULT_GRID_HEIGHT): + """把 μ(s) 画成字符图:横轴滑移率 0→1,纵轴 0→peak。""" + width = _as_int(width, "width", DEFAULT_GRID_WIDTH) + height = _as_int(height, "height", DEFAULT_GRID_HEIGHT) + if width < 20 or height < 5: + raise TireError(u"网格太小:%dx%d" % (width, height)) + if curve.is_empty(): + return [] + + top = curve.spec.peak_friction * 1.05 + grid = [[" "] * width for _ in range(height)] + # 目标带底纹 + low, high = curve.spec.abs_band_limits() + for x in range(width): + slip = x / (width - 1.0) + if low <= slip <= high: + grid[height - 1][x] = "." + # 曲线 + prev_row = None + for x in range(width): + slip = x / (width - 1.0) + mu = curve.spec.friction(slip) + row = int(round((1.0 - mu / top) * (height - 1))) + row = int(_clamp(row, 0, height - 1)) + grid[row][x] = "*" + if prev_row is not None and abs(row - prev_row) > 1: + step = 1 if row > prev_row else -1 + for r in range(prev_row + step, row, step): + grid[r][x] = "|" + prev_row = row + + lines = ["".join(row) for row in grid] + lines.append("0" + " " * (width - 12) + u"滑移率 → 1.0") + return lines + + +# ── 报告 ───────────────────────────────────────────────────────────────── + +def format_report(curve, results, header=""): + """生成文本报告。""" + curve = curve or build_curve() + spec = curve.spec + keys = key_points(spec) + passed = sum(1 for item in results if item["passed"]) + lines = [] + if header: + lines.append(header) + lines.append(u"路面:%s,峰值滑移率 %.3f,峰值附着 %.3f" % ( + keys["surface"], keys["peak_slip"], keys["peak_friction"])) + lines.append(u"锁死点:s=%.2f,附着 %.4f(占峰值 %.1f%%)" % ( + keys["lock_slip"], keys["lock_friction"], keys["lock_ratio"] * 100.0)) + lines.append(u"初段刚度:%.3f 1/s,ABS 目标带 [%.4f, %.4f](带宽 %.0f%%)" % ( + keys["slope_at_zero"], keys["abs_low"], keys["abs_high"], + keys["abs_band"] * 100.0)) + lines.append(u"采样:%d 点(s 从 0 到 1)" % curve.count) + lines.append(u"-" * 60) + lines.append(u"附着曲线(纵轴 0 ~ %.2f):" % (spec.peak_friction * 1.05)) + for line in render_curve(curve): + lines.append(line) + lines.append(u"-" * 60) + lines.append(u"断言:%d 通过 / %d 失败(共 %d 条)" % ( + passed, len(results) - passed, len(results))) + for item in results: + lines.append(u" [%s] %s %s" % ( + "PASS" if item["passed"] else "FAIL", item["type"], item["detail"])) + lines.append(u"结论:%s" % (u"PASSED" if passed == len(results) else u"FAILED")) + return "\n".join(lines) + + +# ── 参数读取 ───────────────────────────────────────────────────────────── + +class TireParams(object): + """从 ROS 参数服务器读到的配置。""" + + def __init__(self, **kwargs): + for name in PARAM_KEYS: + attr = "params_" + name.replace("/", "_") + setattr(self, attr, kwargs.get(name, PARAM_DEFAULTS[name])) + self.peak_slip = _positive(self.params_tire_peak_slip, "tire/peak_slip") + self.peak_friction = _positive(self.params_tire_peak_friction, "tire/peak_friction") + self.abs_band = _as_float(self.params_tire_abs_band, "tire/abs_band", DEFAULT_ABS_BAND) + surface = self.params_road_surface + if isinstance(surface, bytes): + surface = surface.decode("utf-8") + self.surface = surface if surface in SURFACES else DEFAULT_SURFACE + self.samples = _as_int(self.params_curve_samples, "curve/samples", DEFAULT_SAMPLES) + self.lock_friction_ratio = _as_float( + self.params_check_lock_friction_ratio, "check/lock_friction_ratio", + DEFAULT_LOCK_FRICTION_RATIO) + self.linear_tolerance = _as_float( + self.params_check_linear_tolerance, "check/linear_tolerance", + DEFAULT_LINEAR_TOLERANCE) + self.peak_tolerance = _as_float( + self.params_check_peak_tolerance, "check/peak_tolerance", DEFAULT_PEAK_TOLERANCE) + self.publish_hz = _as_float(self.params_rate_publish_hz, "rate/publish_hz", + DEFAULT_PUBLISH_HZ) + self.topic_report = self.params_topic_report + self.topic_curve = self.params_topic_curve + self.topic_peak_slip = self.params_topic_peak_slip + + @classmethod + def from_params(cls, get_param): + values = {} + for name in PARAM_KEYS: + values[name] = get_param(name, PARAM_DEFAULTS[name]) + return cls(**values) + + def spec(self): + return TireSpec(self.peak_slip, self.peak_friction, self.abs_band, self.surface) + + def as_dict(self): + return { + "tire/peak_slip": self.peak_slip, + "tire/peak_friction": self.peak_friction, + "tire/abs_band": self.abs_band, + "road/surface": self.surface, + "curve/samples": self.samples, + "check/lock_friction_ratio": self.lock_friction_ratio, + "check/linear_tolerance": self.linear_tolerance, + "check/peak_tolerance": self.peak_tolerance, + "rate/publish_hz": self.publish_hz, + } diff --git a/src/ground/tire_slip_curve/scripts/tire_node.py b/src/ground/tire_slip_curve/scripts/tire_node.py new file mode 100644 index 0000000000..5270a4b37c --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/tire_node.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""轮胎滑移率-附着曲线节点的 ROS 封装(ROS 1 / Melodic 可跑,Python 2.7 & 3.x)。 + +节点只做三件事: + 1. 从参数服务器读配置,算出一条附着曲线并做四条断言; + 2. 把报告、曲线表、峰值滑移率分别发布到三个 latched 话题; + 3. 打印完保持挂起,方便另一个终端用 rosparam get 核对参数。 +""" +from __future__ import division, print_function, unicode_literals + +import os +import sys +import traceback + +import rospy +from std_msgs.msg import Float64, String + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +import tire_core as core # noqa: E402 + + +def _log(level, message): + """Melodic 的 rosgraph 对非 ASCII unicode 调 str() 会抛 UnicodeEncodeError, + 所以日志消息必须是 byte str。""" + func = getattr(rospy, "log" + level) + try: + unicode # noqa: F821 + except NameError: + func(message) + return + if isinstance(message, unicode): # noqa: F821 + func(message.encode("utf-8")) + else: + func(message) + + +class TireNode(object): + def __init__(self): + self.params = core.TireParams.from_params(rospy.get_param) + spec = self.params.spec() + self.curve = core.build_curve(spec, self.params.samples) + self.results = core.check_curve( + self.curve, spec, + lock_friction_ratio=self.params.lock_friction_ratio, + linear_tolerance=self.params.linear_tolerance, + peak_tolerance=self.params.peak_tolerance, + ) + self.report = core.format_report( + self.curve, self.results, + header=u"轮胎滑移率-附着曲线(纯解析,不依赖 CARLA)") + + self.pub_report = rospy.Publisher( + self.params.topic_report, String, queue_size=1, latch=True) + self.pub_curve = rospy.Publisher( + self.params.topic_curve, String, queue_size=1, latch=True) + self.pub_peak = rospy.Publisher( + self.params.topic_peak_slip, Float64, queue_size=1, latch=True) + rospy.on_shutdown(self.on_shutdown) + + # ── 发布 ────────────────────────────────────────────────────────── + + def curve_text(self): + rows = [u"slip,friction,ratio"] + for slip, mu, ratio in self.curve.as_rows(): + rows.append(u"%.4f,%.4f,%.4f" % (slip, mu, ratio)) + return u"\n".join(rows) + + def publish_all(self): + self.pub_report.publish(String(data=core.to_message_text(self.report))) + self.pub_curve.publish(String(data=core.to_message_text(self.curve_text()))) + self.pub_peak.publish(Float64(data=float(self.params.peak_slip))) + + def on_shutdown(self): + try: + _log("info", u"已发布零控制量") + except Exception: # noqa: BLE001 + pass + + # ── 主循环 ──────────────────────────────────────────────────────── + + def run(self): + try: + keys = core.key_points(self.params.spec()) + _log("info", u"轮胎附着曲线节点已启动:路面=%s,峰值滑移率=%.3f,峰值附着=%.3f," + u"采样=%d 点,ABS 目标带=[%.4f, %.4f]" % ( + keys["surface"], keys["peak_slip"], keys["peak_friction"], + self.curve.count, keys["abs_low"], keys["abs_high"])) + _log("info", u"发布 %s、%s、%s" % ( + self.params.topic_report, self.params.topic_curve, + self.params.topic_peak_slip)) + self.publish_all() + print(self.report) + passed = sum(1 for item in self.results if item["passed"]) + _log("info", u"结论:%s(%d/%d 条断言通过)" % ( + u"PASSED" if passed == len(self.results) else u"FAILED", + passed, len(self.results))) + + # 保持挂起,方便另一个终端 rosparam get + rate = rospy.Rate(max(1.0, self.params.publish_hz)) + while not rospy.is_shutdown(): + rate.sleep() + return 0 + except Exception: # noqa: BLE001 + _log("err", u"曲线核算异常:%s" % traceback.format_exc()) + return 1 + + +def main(): + rospy.init_node("tire_slip_curve", anonymous=False) + node = TireNode() + code = node.run() + try: + node.publish_all() + except Exception: # noqa: BLE001 + pass + return code + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/test_drive_cycle_energy_local.py b/tests/test_drive_cycle_energy_local.py new file mode 100644 index 0000000000..706096a12e --- /dev/null +++ b/tests/test_drive_cycle_energy_local.py @@ -0,0 +1,461 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""drive_cycle_energy 模块的离线回归测试。 + +特点: +* 只用 Python 标准库,**不需要 ROS、不需要 CARLA、不需要显示器**; +* 同时兼容 Python 2.7 与 Python 3.x(Melodic 与 Noetic 都能直接跑); +* 在仓库根目录执行:python3 tests/test_drive_cycle_energy_local.py + +覆盖范围: +A VehicleSpec 参数 B 阻力计算 C 工况解析 +D 工况积分 E 四条断言 F 能量恒等式 +G 续航与百公里能耗 H 字符图 I 报告文本 +J 参数对象 K 工程文件约定(含脚本编码声明的回归检查) + +脚本编码声明这一组(K)是专门为本项目踩过的坑加的:Melodic 的 Python 2.7 +要求每个 .py 的第 1 或第 2 行有 `# -*- coding: utf-8 -*-`,而 `sed` 改 shebang +时若把第 1 行整行覆盖,编码声明就会丢失,节点一启动就 SyntaxError。 +下面 K 组会逐个文件检查「第 1 行是 shebang、第 2 行是编码声明」。 +""" + +from __future__ import division, print_function + +import io +import os +import sys + +try: + text_type = unicode # noqa: F821 # Python 2 +except NameError: + text_type = str # Python 3 + +HERE = os.path.dirname(os.path.abspath(__file__)) +ROOT = os.path.dirname(HERE) + +PKG = os.path.join(ROOT, "src", "ground", "drive_cycle_energy") +SCRIPTS = os.path.join(PKG, "scripts") + +if SCRIPTS not in sys.path: + sys.path.insert(0, SCRIPTS) + +import energy_core as core # noqa: E402 + +PASSED = [] +FAILED = [] + +SPEC = core.VehicleSpec() +CYCLE = core.DEFAULT_CYCLE +RESULT = core.simulate(SPEC, CYCLE) +ASSERTS = core.check_energy(SPEC, RESULT) + + +def check(desc, expected, actual, tol=1e-9): + ok = False + if isinstance(expected, (list, tuple)) and isinstance(actual, (list, tuple)): + if expected and isinstance(expected[0], text_type): + ok = list(expected) == list(actual) + else: + ok = len(expected) == len(actual) and all( + abs(a - b) <= tol for a, b in zip(expected, actual)) + elif isinstance(expected, bool) or isinstance(actual, bool): + ok = bool(expected) == bool(actual) + elif isinstance(expected, text_type) or isinstance(actual, text_type): + ok = expected == actual + else: + ok = abs(float(expected) - float(actual)) <= max( + tol, abs(float(expected)) * 1e-12) + if ok: + PASSED.append(desc) + return True + FAILED.append(u"%s(期望 %r,实际 %r)" % (desc, expected, actual)) + return False + + +def raises(exc, fn, *args, **kwargs): + try: + fn(*args, **kwargs) + except exc: + return True + except Exception as other: # noqa: BLE001 + FAILED.append(u"应抛 %s,实际抛 %r" % (exc.__name__, other)) + return False + FAILED.append(u"应抛 %s,实际没有抛" % (exc.__name__,)) + return False + + +def read(path): + with io.open(path, "rb") as fh: + return fh.read().decode("utf-8") + + +def read_first_lines(path, count=2): + with io.open(path, "rb") as fh: + text = fh.read().decode("utf-8") + return text.splitlines()[:count] + + +# ---------------------------------------------------------------- A 参数 ---- +def group_params(): + s = core.VehicleSpec() + check(u"A1 质量", 1800.0, s.mass) + check(u"A2 滚阻系数", 0.012, s.c_rr) + check(u"A3 风阻系数", 0.31, s.cd) + check(u"A4 迎风面积", 2.2, s.area) + check(u"A5 电池容量", 60.0, s.battery_kwh) + check(u"A6 回收效率", 0.6, s.regen_eff) + check(u"A7 电池焦耳", 2.16e8, s.battery_joules()) + check(u"A8 质量必须为正", True, + raises(core.EnergyError, core.VehicleSpec, mass=0.0)) + check(u"A9 电池必须为正", True, + raises(core.EnergyError, core.VehicleSpec, battery_kwh=0.0)) + check(u"A10 风阻系数必须为正", True, + raises(core.EnergyError, core.VehicleSpec, cd=0.0)) + check(u"A11 回收效率不能大于 1", True, + raises(core.EnergyError, core.VehicleSpec, regen_eff=1.2)) + check(u"A12 回收效率可以为 0", 0.0, core.VehicleSpec(regen_eff=0.0).regen_eff) + check(u"A13 滚阻系数可以为 0", 0.0, core.VehicleSpec(c_rr=0.0).c_rr) + check(u"A14 滚阻系数不能为负", True, + raises(core.EnergyError, core.VehicleSpec, c_rr=-0.01)) + check(u"A15 repr 含 mass", True, "mass=1800" in repr(s)) + + +# ---------------------------------------------------------------- B 阻力 ---- +def group_forces(): + s = core.VehicleSpec() + check(u"B1 滚阻力恒定", 211.896, s.rolling_force()) + check(u"B2 10 m/s 风阻", 40.92, s.aero_force(10.0)) + check(u"B3 30 m/s 风阻", 368.28, s.aero_force(30.0)) + check(u"B4 0 m/s 风阻为零", 0.0, s.aero_force(0.0)) + check(u"B5 风阻按平方增长", 9.0, s.aero_force(30.0) / s.aero_force(10.0)) + check(u"B6 总阻力 = 滚阻 + 风阻", 252.816, s.resistance(10.0)) + check(u"B7 50 km/h 总阻力", 290.831185, s.resistance(50.0 / 3.6), tol=1e-5) + check(u"B8 牵引力 = m·a + 阻力", 2090.831185, + s.traction_force(50.0 / 3.6, 1.0), tol=1e-5) + check(u"B9 匀减速时牵引力为负", True, + s.traction_force(50.0 / 3.6, -2.0) < 0) + check(u"B10 动能 50 km/h", 173611.1111, s.kinetic(50.0 / 3.6), tol=1e-3) + check(u"B11 动能按平方增长", 4.0, s.kinetic(20.0) / s.kinetic(10.0)) + check(u"B12 静止动能为零", 0.0, s.kinetic(0.0)) + + +# ------------------------------------------------------------ C 工况解析 ---- +def group_parse(): + segs = core.parse_segments([(5.0, 0.0, 30.0), (10.0, 30.0, 30.0)]) + check(u"C1 段数", 2, len(segs)) + check(u"C2 时长保留", 5.0, segs[0][0]) + check(u"C3 km/h 转 m/s", 30.0 / 3.6, segs[0][2], tol=1e-12) + check(u"C4 匀速段起止相同", segs[1][1], segs[1][2]) + check(u"C5 空工况报错", True, + raises(core.EnergyError, core.parse_segments, [])) + check(u"C6 段长必须为三元素", True, + raises(core.EnergyError, core.parse_segments, [(1.0, 2.0)])) + check(u"C7 时长必须为正", True, + raises(core.EnergyError, core.parse_segments, [(0.0, 0.0, 10.0)])) + check(u"C8 车速不能为负", True, + raises(core.EnergyError, core.parse_segments, [(1.0, -5.0, 10.0)])) + check(u"C9 默认工况八段", 8, len(core.DEFAULT_CYCLE)) + check(u"C10 默认工况总时长", 60.0, + sum(seg[0] for seg in core.DEFAULT_CYCLE)) + check(u"C11 默认工况起点静止", 0.0, core.DEFAULT_CYCLE[0][1]) + check(u"C12 默认工况终点 40 km/h", 40.0, core.DEFAULT_CYCLE[-1][2]) + + +# ------------------------------------------------------------ D 工况积分 ---- +def group_simulate(): + check(u"D1 总时长", 60.0, RESULT["duration_s"]) + check(u"D2 行驶距离", 512.5, RESULT["distance_m"], tol=1e-6) + check(u"D3 最高车速 km/h", 50.0, RESULT["v_max"] * 3.6, tol=1e-9) + check(u"D4 驱动能量", 407321.394176, RESULT["drive_j"], tol=1e-3) + check(u"D5 制动能量", 159646.509903, RESULT["brake_j"], tol=1e-3) + check(u"D6 回收能量 = η·制动", 0.6 * RESULT["brake_j"], RESULT["regen_j"]) + check(u"D7 滚阻能量", 108596.7, RESULT["roll_j"], tol=1e-3) + check(u"D8 风阻能量", 27967.073162, RESULT["aero_j"], tol=1e-3) + check(u"D9 动能净变化", 111111.1111, RESULT["ke_j"], tol=1e-3) + check(u"D10 净耗电 = 驱动 − 回收", 311533.488234, RESULT["net_j"], tol=1e-3) + check(u"D11 步长影响可忽略", RESULT["net_j"], + core.simulate(SPEC, CYCLE, dt=0.02)["net_j"], tol=2.0) + check(u"D12 静止工况无驱动能量", 0.0, + core.simulate(SPEC, [(5.0, 0.0, 0.0)])["drive_j"]) + check(u"D13 静止工况无距离", 0.0, + core.simulate(SPEC, [(5.0, 0.0, 0.0)])["distance_m"]) + check(u"D14 步长必须为正", True, + raises(core.EnergyError, core.simulate, SPEC, CYCLE, 0.0)) + check(u"D15 减速越快制动能量越大", True, + core.simulate(SPEC, [(5.0, 0.0, 20.0), (4.0, 20.0, 0.0)])["brake_j"] + > core.simulate(SPEC, [(5.0, 0.0, 20.0), (8.0, 20.0, 0.0)])["brake_j"]) + check(u"D16 匀速工况驱动 = 阻力做功", 0.0, + core.simulate(SPEC, [(10.0, 40.0, 40.0)])["ke_j"]) + + +# ------------------------------------------------------------ E 四条断言 ---- +def group_asserts(): + check(u"E1 断言条数", 4, len(ASSERTS)) + check(u"E2 断言名称", list(core.ASSERT_NAMES), + [a["type"] for a in ASSERTS]) + check(u"E3 默认全部通过", True, all(a["passed"] for a in ASSERTS)) + check(u"E4 每条都有 detail", True, + all(a["detail"] for a in ASSERTS)) + check(u"E5 detail 是文本", True, + all(isinstance(a["detail"], text_type) for a in ASSERTS)) + bad = core.check_energy(SPEC, RESULT, regen_limit=0.5) + check(u"E6 回收上限收紧后第一条失败", True, + not [a for a in bad if a["type"] == core.ASSERT_NAMES[1]][0]["passed"]) + check(u"E7 其余三条仍然通过", 3, + len([a for a in bad if a["passed"]])) + bad2 = core.check_energy(SPEC, RESULT, breakdown_tolerance=-0.01) + check(u"E8 分解容差不可满足时该条失败", True, + not [a for a in bad2 if a["type"] == core.ASSERT_NAMES[0]][0]["passed"]) + check(u"E9 分解失败其余通过", 3, len([a for a in bad2 if a["passed"]])) + bad3 = core.check_energy(SPEC, RESULT, aero_rising=False) + check(u"E10 反向期望时风阻条失败", True, + not [a for a in bad3 if a["type"] == core.ASSERT_NAMES[3]][0]["passed"]) + zero = core.simulate(SPEC, [(5.0, 0.0, 0.0)]) + a_zero = core.check_energy(SPEC, zero) + check(u"E11 零驱动工况分解条失败", True, + not [a for a in a_zero if a["type"] == core.ASSERT_NAMES[0]][0]["passed"]) + no_regen = core.simulate(core.VehicleSpec(regen_eff=0.0), CYCLE) + a_nr = core.check_energy(core.VehicleSpec(regen_eff=0.0), no_regen) + check(u"E12 无回收时回收比为零", 0.0, + no_regen["regen_j"] / no_regen["brake_j"]) + check(u"E13 无回收仍然四条通过", 4, + len([a for a in a_nr if a["passed"]])) + check(u"E14 回收比与 η 一致", 0.6, + RESULT["regen_j"] / RESULT["brake_j"], tol=1e-12) + rich = core.simulate(core.VehicleSpec(regen_eff=0.95), CYCLE) + check(u"E15 η=0.95 时回收比 0.95", 0.95, + rich["regen_j"] / rich["brake_j"], tol=1e-12) + check(u"E16 η=0.95 时回收条通过", True, + all(a["passed"] for a in core.check_energy( + core.VehicleSpec(regen_eff=0.95), rich))) + check(u"E17 η=0.95 净耗低于默认", True, rich["net_j"] < RESULT["net_j"]) + heavy = core.VehicleSpec(mass=3000.0) + r_heavy = core.simulate(heavy, CYCLE) + check(u"E18 车更重则驱动能量更大", True, + r_heavy["drive_j"] > RESULT["drive_j"]) + check(u"E19 车更重仍然自洽", True, + [a for a in core.check_energy(heavy, r_heavy) + if a["type"] == core.ASSERT_NAMES[0]][0]["passed"]) + slick = core.VehicleSpec(c_rr=0.006) + r_slick = core.simulate(slick, CYCLE) + check(u"E20 滚阻减半则滚阻能量约为一半", 0.5, + r_slick["roll_j"] / RESULT["roll_j"], tol=1e-12) + check(u"E21 滚阻减半续航更长", True, + core.range_km(slick, r_slick) > core.range_km(SPEC, RESULT)) + check(u"E22 断言结果带 passed 布尔", True, + all(isinstance(a["passed"], bool) for a in ASSERTS)) + + +# ------------------------------------------------------------ F 恒等式 ---- +def group_identity(): + total = (RESULT["roll_j"] + RESULT["aero_j"] + RESULT["brake_j"] + + RESULT["ke_j"]) + check(u"F1 分解和等于驱动能量", RESULT["drive_j"], total, tol=1e-6) + check(u"F2 驱动减制动等于净机械能", RESULT["drive_j"] - RESULT["brake_j"], + RESULT["roll_j"] + RESULT["aero_j"] + RESULT["ke_j"], tol=1e-6) + check(u"F3 动能等于 0.5·m·v²", 0.5 * 1800.0 * (40.0 / 3.6) ** 2, + RESULT["ke_j"], tol=1e-6) + check(u"F4 滚阻能量等于滚阻力乘距离", 211.896 * RESULT["distance_m"], + RESULT["roll_j"], tol=1e-6) + check(u"F5 风阻能量等于 ∫F·v", 27967.073162, RESULT["aero_j"], tol=1e-3) + check(u"F6 净耗等于驱动减回收", RESULT["drive_j"] - RESULT["regen_j"], + RESULT["net_j"], tol=1e-9) + check(u"F7 回收是制动的 60%", 0.6 * RESULT["brake_j"], + RESULT["regen_j"], tol=1e-9) + check(u"F8 三项占比之和为 1", 1.0, + sum(core.share(RESULT, k) for k in ("roll_j", "aero_j", "brake_j", "ke_j")), + tol=1e-9) + check(u"F9 滚阻占比", 0.266612, core.share(RESULT, "roll_j"), tol=1e-5) + check(u"F10 风阻占比", 0.068661, core.share(RESULT, "aero_j"), tol=1e-5) + check(u"F11 制动占比", 0.391933, core.share(RESULT, "brake_j"), tol=1e-5) + check(u"F12 动能占比", 0.272785, core.share(RESULT, "ke_j"), tol=1e-5) + check(u"F13 占比非法时抛错", True, + raises(core.EnergyError, core.share, + {"drive_j": 0.0, "roll_j": 0.0}, "roll_j")) + + +# ------------------------------------------------------ G 续航与百公里 ---- +def group_range(): + check(u"G1 百公里电耗", 16.885284, core.per_100km(RESULT), tol=1e-5) + check(u"G2 满电续航", 355.339006, core.range_km(SPEC, RESULT), tol=1e-4) + check(u"G3 续航 = 电池/百公里×100", 60.0 / 16.885284 * 100.0, + core.range_km(SPEC, RESULT), tol=1e-3) + hw = core.simulate(SPEC, [(20.0, 100.0, 100.0)]) + check(u"G4 高速匀速百公里电耗", 14.656576, core.per_100km(hw), tol=1e-5) + check(u"G5 高速续航", 60.0 / core.per_100km(hw) * 100.0, + core.range_km(SPEC, hw), tol=1e-9) + check(u"G6 城市工况百公里高于高速", True, + core.per_100km(RESULT) > core.per_100km(hw)) + check(u"G7 电池减半续航减半", 0.5, + core.range_km(core.VehicleSpec(battery_kwh=30.0), RESULT) + / core.range_km(SPEC, RESULT), tol=1e-9) + check(u"G8 零距离无法折算", True, + raises(core.EnergyError, core.per_100km, {"distance_m": 0.0, + "net_j": 0.0})) + check(u"G9 净耗非正无法算续航", True, + raises(core.EnergyError, core.range_km, SPEC, + {"distance_m": 100.0, "net_j": 0.0})) + check(u"G10 无回收时续航变短", True, + core.range_km(core.VehicleSpec(regen_eff=0.0), + core.simulate(core.VehicleSpec(regen_eff=0.0), CYCLE)) + < core.range_km(SPEC, RESULT)) + + +# ------------------------------------------------------------ H 字符图 ---- +def group_render(): + art = core.render_energy(RESULT) + lines = art.splitlines() + check(u"H1 行数 = 2 + 6 + 1", 9, len(lines)) + check(u"H2 含各项标签", True, all(k in art for k in (u"滚阻", u"风阻", u"制动", + u"动能", u"驱动", u"回收"))) + check(u"H3 驱动条最长(42 格)", 42, + max(line.count("#") for line in lines)) + check(u"H4 含千焦单位", True, u"kJ" in art) + check(u"H5 自定义宽度生效", True, + core.render_energy(RESULT, width=10).splitlines()[1].count("-") == 10) + check(u"H6 空能量不崩", True, u"kJ" in core.render_energy( + {"roll_j": 0.0, "aero_j": 0.0, "brake_j": 0.0, "ke_j": 0.0, + "drive_j": 0.0, "regen_j": 0.0})) + + +# ------------------------------------------------------------ I 报告 ---- +def group_report(): + text = core.format_report(SPEC, RESULT, ASSERTS) + check(u"I1 含工况行", True, u"工况:60.0 s" in text) + check(u"I2 含整车行", True, u"电池 60.0 kWh" in text) + check(u"I3 含分解行", True, u"分解:" in text) + check(u"I4 含续航行", True, u"续航:百公里" in text) + check(u"I5 四条 PASS", 4, text.count(u"[PASS]")) + check(u"I6 结论 PASSED", True, u"结论:PASSED" in text) + check(u"I7 断言统计行", True, u"4 通过 / 0 失败" in text) + bad = core.check_energy(SPEC, RESULT, regen_limit=0.5) + bad_text = core.format_report(SPEC, RESULT, bad) + check(u"I8 失败时结论 FAILED", True, u"结论:FAILED" in bad_text) + check(u"I9 失败时统计正确", True, u"3 通过 / 1 失败" in bad_text) + + +# ------------------------------------------------------------ J 参数对象 ---- +def group_params_obj(): + p = core.EnergyParams() + check(u"J1 默认质量", 1800.0, p.vehicle.mass) + check(u"J2 默认回收效率", 0.6, p.vehicle.regen_eff) + check(u"J3 默认分解容差", 0.02, p.breakdown_tolerance) + check(u"J4 默认回收上限", 1.0, p.regen_limit) + check(u"J5 默认步长", 0.05, p.dt) + check(u"J6 默认工况段数", 8, len(p.cycle)) + q = core.EnergyParams(**{"vehicle/mass": 2400.0, "vehicle/regen_eff": 0.8, + "check/regen_limit": 0.9, "cycle/dt": 0.1}) + check(u"J7 覆盖质量", 2400.0, q.vehicle.mass) + check(u"J8 覆盖回收", 0.8, q.vehicle.regen_eff) + check(u"J9 覆盖上限", 0.9, q.regen_limit) + check(u"J10 覆盖步长", 0.1, q.dt) + + +# --------------------------------------------------- K 工程文件约定 ---- +def group_files(): + script_names = ("main.py", "energy_core.py", "energy_node.py") + for name in script_names: + path = os.path.join(SCRIPTS, name) + exists = os.path.isfile(path) + check(u"K1 %s 存在" % name, True, exists) + if not exists: + continue + first = read_first_lines(path, 2) + check(u"K2 %s 第 1 行是 shebang" % name, True, + len(first) >= 1 and first[0].startswith("#!")) + check(u"K3 %s 第 2 行是编码声明" % name, True, + len(first) >= 2 and u"coding: utf-8" in first[1]) + check(u"K4 %s 含 __future__" % name, True, + u"from __future__" in read(path)) + + check(u"K5 core 不 import rospy", False, + "import rospy" in read(os.path.join(SCRIPTS, "energy_core.py"))) + check(u"K6 core 不 import numpy", False, + "import numpy" in read(os.path.join(SCRIPTS, "energy_core.py"))) + check(u"K7 node 含 _log 辅助", True, + "def _log" in read(os.path.join(SCRIPTS, "energy_node.py"))) + check(u"K8 node 用 latched 发布", True, + "latch=True" in read(os.path.join(SCRIPTS, "energy_node.py"))) + check(u"K9 yaml 存在", True, + os.path.isfile(os.path.join(PKG, "config", "drive_cycle_energy.yaml"))) + check(u"K10 launch 存在", True, + os.path.isfile(os.path.join(PKG, "launch", "main.launch"))) + launch = read(os.path.join(PKG, "launch", "main.launch")) + check(u"K11 launch 用 main.py", True, 'type="main.py"' in launch) + check(u"K12 launch 加载 yaml", True, "rosparam command=\"load\"" in launch) + check(u"K13 launch 里 12 个 param", 12, launch.count(" before_fail: + raise AssertionError( + u"本组失败 %d 项:%s" % (len(FAILED) - before_fail, + FAILED[-1])) + runner.__name__ = fn.__name__ + return runner + + +def main(): + lines = [] + lines.append(u"drive_cycle_energy 离线回归测试") + lines.append(u"Python %s" % sys.version.split()[0]) + lines.append(u"包目录 %s" % PKG) + lines.append(u"-" * 60) + for title, fn in GROUP_FUNCS: + before = len(FAILED) + fn() + bad = len(FAILED) - before + lines.append(u"%-22s %s" % (title, u"PASS" if bad == 0 else u"FAIL(%d)" % bad)) + lines.append(u"=" * 60) + lines.append(u"通过 %d 项,失败 %d 项" % (len(PASSED), len(FAILED))) + for item in FAILED: + lines.append(u" [FAIL] %s" % item) + lines.append(u"全部通过" if not FAILED else u"结论:FAILED") + print(u"\n".join(lines)) + return 0 if not FAILED else 1 + + +test_A_parameters = _pytest_group(group_params) +test_B_forces = _pytest_group(group_forces) +test_C_parse = _pytest_group(group_parse) +test_D_simulate = _pytest_group(group_simulate) +test_E_asserts = _pytest_group(group_asserts) +test_F_identity = _pytest_group(group_identity) +test_G_range = _pytest_group(group_range) +test_H_render = _pytest_group(group_render) +test_I_report = _pytest_group(group_report) +test_J_params = _pytest_group(group_params_obj) +test_K_files = _pytest_group(group_files) + + +if __name__ == "__main__": + sys.exit(main()) 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(" spec.transmissibility_at_ratio(1.8) + > spec.transmissibility_at_ratio(3.0)) + check(u"A8 峰值大于共振比处值", + spec.peak_value() > spec.transmissibility_at_ratio(1.0)) + check(u"A9 频率比可换算", near(spec.ratio(2.4), 2.0, 1e-12)) + check(u"A10 传递率恒为正", + all(spec.transmissibility_at_ratio(r) > 0 for r in core.DEFAULT_RATIOS)) + + +# ------------------------------------------------------------ B 闭式峰位峰值 +def group_peak(): + out(u"B 闭式峰位峰值") + spec = core.RideSpec() + check(u"B1 峰位闭式 sqrt(1-2ζ²)", + near(spec.peak_ratio(), math.sqrt(1 - 2 * 0.25 ** 2), 1e-12)) + check(u"B2 峰位数值", near(spec.peak_ratio(), 0.935414, 1e-6)) + check(u"B3 峰值数值", near(spec.peak_value(), 2.280351, 1e-6)) + peak_r, peak_t = core.find_peak(spec) + check(u"B4 扫描峰位", near(peak_r, 0.948625, 1e-6)) + check(u"B5 扫描峰值", near(peak_t, 2.283149, 1e-6)) + check(u"B6 扫描峰值与闭式相对偏差 < 1%", + abs(peak_t - spec.peak_value()) / spec.peak_value() < 0.01) + check(u"B7 扫描峰位与闭式偏差 < 容差", + abs(peak_r - spec.peak_ratio()) < 0.02) + check(u"B8 阻尼变大峰值变小", + core.RideSpec(damping=0.4).peak_value() < spec.peak_value()) + check(u"B9 阻尼变小峰值变大", + core.RideSpec(damping=0.1).peak_value() > spec.peak_value()) + check(u"B10 扫描区间非法被拒", + raises(core.RideError, core.find_peak, spec, 0.0, 1.0, 10)) + + +# ------------------------------------------------------------ C 舒适等级 +def group_grade(): + out(u"C 舒适等级") + check(u"C1 0.3 舒适", core.comfort_grade(0.3) == u"舒适", core.comfort_grade(0.3)) + check(u"C2 0.315 边界进下一档", + core.comfort_grade(0.315) == u"略有不适") + check(u"C3 0.5 略有不适", core.comfort_grade(0.5) == u"略有不适") + check(u"C4 0.8 不适", core.comfort_grade(0.8) == u"不适") + check(u"C5 1.2 很不适", core.comfort_grade(1.2) == u"很不适") + check(u"C6 2.0 极不适", core.comfort_grade(2.0) == u"极不适") + check(u"C7 3.0 无法忍受", core.comfort_grade(3.0) == u"无法忍受") + check(u"C8 负值被拒", raises(core.RideError, core.comfort_grade, -0.1)) + check(u"C9 等级单调", + core.comfort_grade(0.2) == u"舒适" + and core.comfort_grade(2.4) == u"极不适") + check(u"C10 默认工况等级", + core.comfort_grade(core.RideSpec().weighted_accel(6.0)) == u"舒适") + + +# ------------------------------------------------------------ D 四条断言 +def group_asserts(): + out(u"D 四条断言") + spec = core.RideSpec() + asserts = core.check_ride(spec) + 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"0.9486" in asserts[0]["detail"]) + check(u"D6 峰值 detail 含闭式值", u"2.2804" in asserts[1]["detail"]) + check(u"D7 高频 detail 含 10 倍频", u"r=10" in asserts[2]["detail"]) + check(u"D8 线性 detail 含加倍", + u"加倍" in asserts[3]["detail"], asserts[3]["detail"]) + + tight = core.check_ride(spec, pos_tolerance=0.001) + flipped = [a["type"] for a in tight if not a["passed"]] + check(u"D9 收紧位置容差只翻共振位置", + flipped == [core.ASSERT_NAMES[0]], repr(flipped)) + + strict = core.check_ride(spec, high_freq_limit=0.001) + flipped = [a["type"] for a in strict if not a["passed"]] + check(u"D10 收紧高频上限只翻高频衰减", + flipped == [core.ASSERT_NAMES[2]], repr(flipped)) + + soft = core.check_ride(core.RideSpec(damping=0.02)) + check(u"D11 软阻尼下四条仍全过", all(a["passed"] for a in soft)) + + +# ------------------------------------------------------------ E 扫描表 +def group_scan(): + out(u"E 扫描表") + spec = core.RideSpec() + rows = core.scan(spec, core.DEFAULT_RATIOS) + check(u"E1 行数", len(rows) == 10, repr(len(rows))) + check(u"E2 首行频率比", near(rows[0][0], 0.25)) + check(u"E3 末行频率比", near(rows[-1][0], 10.0)) + check(u"E4 频率换算", near(rows[3][1], 1.2, 1e-12)) + check(u"E5 共振比行传递率", near(rows[3][2], 2.236068, 1e-6)) + check(u"E6 加权等于传递率乘输入", + all(near(r[3], r[2] * spec.input_accel, 1e-12) for r in rows)) + check(u"E7 峰值出现在 0.75~1.25 之间", + rows[2][2] < rows[3][2] and rows[4][2] < rows[3][2]) + check(u"E8 非法频率比被拒", + raises(core.RideError, core.scan, spec, (0.0,))) + check(u"E9 自定义频率比可用", + len(core.scan(spec, (1.0, 2.0, 3.0))) == 3) + + +# ------------------------------------------------------------ F 字符图 +def group_render(): + out(u"F 字符图") + art = core.render_transmissibility(core.RideSpec()) + 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 数据行数 10", len(lines) == 13, repr(len(lines))) + check(u"F4 共振行有峰值标记", any(u"<峰值" in ln for ln in lines)) + bars = [ln.count("#") for ln in lines[2:-1]] + check(u"F5 条形最宽在峰值行", bars.index(max(bars)) == 3, repr(bars)) + check(u"F6 宽度可调", + max(ln.count("#") for ln in + core.render_transmissibility(core.RideSpec(), width=12).split("\n")) <= 12) + + +# ------------------------------------------------------------ G 参数读取 +def group_params(): + out(u"G 参数读取") + params = core.RideParams() + check(u"G1 默认固有频率", near(params.spec.fn_hz, 1.2)) + check(u"G2 默认阻尼比", near(params.spec.damping, 0.25)) + check(u"G3 默认输入", near(params.spec.input_accel, 0.5)) + check(u"G4 默认激励频率", near(params.spec.road_freq_hz, 6.0)) + check(u"G5 默认位置容差", near(params.pos_tolerance, 0.02)) + check(u"G6 默认峰值容差", near(params.peak_tolerance, 0.05)) + check(u"G7 默认高频上限", near(params.high_freq_limit, 0.1)) + check(u"G8 默认线性容差", near(params.linear_tolerance, 0.02)) + + custom = core.RideParams(**{ + "suspension/fn_hz": 1.6, + "suspension/damping": 0.3, + "road/input_accel": 0.8, + "check/high_freq_limit": 0.2, + }) + check(u"G9 固有频率可覆盖", near(custom.spec.fn_hz, 1.6)) + check(u"G10 阻尼可覆盖", near(custom.spec.damping, 0.3)) + check(u"G11 输入可覆盖", near(custom.spec.input_accel, 0.8)) + check(u"G12 阈值可覆盖", near(custom.high_freq_limit, 0.2)) + check(u"G13 未覆盖项保持默认", near(custom.spec.road_freq_hz, 6.0)) + check(u"G14 非法阻尼被拒", + raises(core.RideError, core.RideSpec, damping=0.0)) + check(u"G15 过大阻尼被拒", + raises(core.RideError, core.RideSpec, damping=0.75)) + check(u"G16 非法固有频率被拒", + raises(core.RideError, core.RideSpec, fn_hz=-1.0)) + + +# ------------------------------------------------------------ H 报告排版 +def group_report(): + out(u"H 报告排版") + spec = core.RideSpec() + asserts = core.check_ride(spec) + text = core.format_report(spec, asserts) + check(u"H1 含悬架行", u"悬架:固有频率 1.200 Hz" in text) + check(u"H2 含理论行", u"理论:峰位 r=0.9354,峰值 2.2804" in text) + check(u"H3 含扫描行", u"扫描:峰位 r=0.9486" in text) + check(u"H4 含舒适性行", u"等级「舒适」" in text) + check(u"H5 含断言汇总", u"断言:4 通过 / 0 失败(共 4 条)" in text) + check(u"H6 含结论 PASSED", u"结论:PASSED" in text) + check(u"H7 每条断言都出现", all(a["type"] in text for a in asserts)) + rows = [line for line in text.split(u"\n") + if line.strip().startswith((u"0.25", u"0.50", u"0.75", u"1.00", + u"1.25", u"1.50", u"2.00", u"3.00", + u"5.00", u"10.00"))] + check(u"H8 表格有 10 行数据", + len(rows) == 10 and u"10.00" in text and u"12.000" in text) + + bad = core.format_report(spec, core.check_ride(spec, pos_tolerance=0.001)) + check(u"H9 反例结论 FAILED", u"结论:FAILED" in bad) + check(u"H10 反例含 1 失败", u"断言:3 通过 / 1 失败" in bad) + + +# ------------------------------------------------------------ I 闭式核对 +def group_identity(): + out(u"I 闭式核对") + spec = core.RideSpec() + zeta = spec.damping + # r=1 处的解析值:sqrt(1+(2ζ)²)/(2ζ) + analytic = math.sqrt(1 + (2 * zeta) ** 2) / (2 * zeta) + check(u"I1 共振比处解析值", near(spec.transmissibility_at_ratio(1.0), analytic, 1e-12)) + # 峰位处解析值(与 peak_value 同源,但用另一条式子写) + r = spec.peak_ratio() + alt = math.sqrt((1 + (2 * zeta * r) ** 2) / + ((1 - r * r) ** 2 + (2 * zeta * r) ** 2)) + check(u"I2 峰值与峰位处取值一致", near(alt, spec.peak_value(), 1e-12)) + # 高频渐近:ζ 很小时 T(10) ≈ 1/(100−1) + tiny = core.RideSpec(damping=1e-4) + check(u"I3 高频渐近 1/(r²-1)", + near(tiny.transmissibility_at_ratio(10.0), 1.0 / 99.0, 2e-3), + repr(tiny.transmissibility_at_ratio(10.0))) + # 加权线性 + a1 = spec.weighted_accel(2.4) + a2 = core.RideSpec(input_accel=1.0).weighted_accel(2.4) + check(u"I4 输入加倍加权加倍", near(a2, 2 * a1, 1e-12)) + # 固有频率只改变频率轴,不改变传递率 + other = core.RideSpec(fn_hz=2.4) + check(u"I5 固有频率不改变传递率", + near(other.transmissibility(2.4), spec.transmissibility(1.2), 1e-12)) + check(u"I6 峰值与阻尼反相关", + core.RideSpec(damping=0.05).peak_value() + > core.RideSpec(damping=0.5).peak_value()) + + +# ------------------------------------------------------------ J 工程文件约定 +def group_files(): + out(u"J 工程文件约定") + for rel in ("CMakeLists.txt", "package.xml", "README.md", + "config/ride_comfort_transmissibility.yaml", "launch/main.launch", + "scripts/main.py", "scripts/ride_core.py", + "scripts/ride_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(" spec.ssf()) + check("B8 质心变高 → SSF 变小", + core.VehicleSpec(1.54, 0.8).ssf() < spec.ssf()) + check("B9 SSF 与质量无关", + close(core.VehicleSpec(1.54, 0.55, 800.0).ssf(), spec.ssf())) + check("B10 临界车速随半径增大", + spec.critical_speed(60.0) > spec.critical_speed(30.0)) + check("B11 临界车速闭式解", + close(spec.critical_speed(30.0), math.sqrt(13.734 * 30.0), 1e-12)) + check("B12 半径为 0 算临界车速报错", + raises(core.RolloverError, spec.critical_speed, 0.0)) + + # 质量不影响侧翻临界加速度,但影响载荷 + heavy = core.VehicleSpec(1.54, 0.55, 3000.0) + check("B13 质量翻倍后载荷翻倍", + close(heavy.wheel_load_static(), 2 * spec.wheel_load_static())) + check("B14 质量翻倍后临界加速度不变", + close(heavy.critical_accel(), spec.critical_accel())) + + +# ── C 工况 ─────────────────────────────────────────────────────────────── + +def group_cornering(): + spec = core.VehicleSpec() + item = core.Cornering(spec, 30.0, 60.0) + check("C1 速度换算 16.6667 m/s", close(item.speed_mps, 16.666666, 1e-6)) + check("C2 侧向加速度 9.259259", close(item.lateral_accel(), 9.259259, 1e-6)) + check("C3 a_y = v²/R", close(item.lateral_accel(), 16.666666 ** 2 / 30.0, 1e-5)) + check("C4 裕度 0.674185", close(item.usage(), 0.674185, 1e-6)) + check("C5 裕度 = a_y/a_crit", + close(item.usage(), item.lateral_accel() / spec.critical_accel())) + check("C6 极限车速 20.298276 m/s", close(item.speed_limit(), 20.298276, 1e-6)) + check("C7 极限车速 73.073793 km/h", close(item.speed_limit_kmh(), 73.073793, 1e-6)) + check("C8 余量 13.073793", close(item.headroom_kmh(), 13.073793, 1e-6)) + check("C9 60 km/h 不抬轮", not item.lifts_off()) + check("C10 安全车速(0.7) 61.137921", close(item.safe_speed(0.7), 61.137921, 1e-6)) + check("C11 安全车速(0.5) 51.670974", close(item.safe_speed(0.5), 51.670974, 1e-6)) + check("C12 安全车速与裕度单调", + item.safe_speed(0.9) > item.safe_speed(0.5)) + + fast = core.Cornering(spec, 30.0, 80.0) + check("C13 80 km/h 抬轮", fast.lifts_off()) + check("C14 80 km/h 裕度 > 1", fast.usage() > 1.0) + check("C15 80 km/h 余量为负", fast.headroom_kmh() < 0.0) + + check("C16 半径变大 → 侧向加速度变小", + core.Cornering(spec, 60.0, 60.0).lateral_accel() < item.lateral_accel()) + check("C17 静止时 a_y = 0", close(core.Cornering(spec, 30.0, 0.0).lateral_accel(), 0.0)) + check("C18 静止时裕度 0", close(core.Cornering(spec, 30.0, 0.0).usage(), 0.0)) + check("C19 as_row 四元", len(item.as_row()) == 4) + + +# ── D 载荷 ─────────────────────────────────────────────────────────────── + +def group_loads(): + spec = core.VehicleSpec() + item = core.Cornering(spec, 30.0, 60.0) + check("D1 转移量 4960.31746", close(item.transfer(), 4960.31746, 1e-5)) + check("D2 转移量公式 m·a_y·h/t", + close(item.transfer(), 1500 * 9.259259 * 0.55 / 1.54, 1e-3)) + check("D3 内侧 2397.18254", close(item.inner_load(), 2397.18254, 1e-4)) + check("D4 外侧 12317.81746", close(item.outer_load(), 12317.81746, 1e-4)) + check("D5 内外侧之和 = m·g", + close(item.inner_load() + item.outer_load(), 1500 * 9.81, 1e-9)) + check("D6 转移占比 0.674185", close(item.transfer_ratio(), 0.674185, 1e-6)) + check("D7 转移占比 = 裕度", + close(item.transfer_ratio(), item.usage(), 1e-12)) + + still = core.Cornering(spec, 30.0, 0.0) + check("D8 静止时转移量为 0", close(still.transfer(), 0.0)) + check("D9 静止时内外侧相等", + close(still.inner_load(), still.outer_load())) + + lifted = core.Cornering(spec, 30.0, 90.0) + check("D10 90 km/h 内侧为负", lifted.inner_load() < 0.0) + check("D11 90 km/h 转移量超静态载荷", + lifted.transfer() > spec.wheel_load_static()) + + # 抬轮临界点:把车速调到恰好 v_limit,内侧力≈0 + at_limit = core.Cornering(spec, 30.0, item.speed_limit_kmh()) + check("D12 极限车速时内侧≈0", abs(at_limit.inner_load()) < 1e-6, + "inner=%.6f" % at_limit.inner_load()) + check("D13 极限车速时裕度≈1", close(at_limit.usage(), 1.0, 1e-9)) + + +# ── E 四条断言 ─────────────────────────────────────────────────────────── + +def group_checks(): + spec = core.VehicleSpec() + item = core.Cornering(spec, 30.0, 60.0) + results = core.check_cornering(item) + check("E1 返回 4 条", len(results) == 4) + check("E2 顺序与 ASSERT_NAMES 一致", + [r["type"] for r in results] == list(core.ASSERT_NAMES)) + check("E3 每条含 passed/detail", + all("passed" in r and "detail" in r for r in results)) + check("E4 默认全通过", core.all_passed(results)) + check("E5 裕度通过", results[0]["passed"]) + check("E6 内侧轮载荷通过", results[1]["passed"]) + check("E7 极限车速通过", results[2]["passed"]) + check("E8 载荷转移通过", results[3]["passed"]) + check("E9 载荷转移偏差极小", "1e-1" in results[3]["detail"] + or "e-1" in results[3]["detail"]) + + # 反例 1:裕度上限收到 0.5,只翻第一条 + res1 = core.check_cornering(item, rollover_margin=0.5) + check("E10 裕度 0.5 → 侧翻裕度失败", not res1[0]["passed"]) + check("E11 裕度 0.5 → 其余三条仍通过", + all(r["passed"] for i, r in enumerate(res1) if i != 0)) + + # 反例 2:抬高质心,裕度/内侧轮/极限车速同时崩 + tall = core.Cornering(core.VehicleSpec(cg_height=0.90), 30.0, 60.0) + res2 = core.check_cornering(tall) + check("E12 高质心 → 侧翻裕度失败", not res2[0]["passed"]) + check("E13 高质心 → 内侧轮载荷失败", not res2[1]["passed"]) + check("E14 高质心 → 极限车速失败", not res2[2]["passed"]) + check("E15 高质心 → 载荷转移仍自洽", res2[3]["passed"]) + check("E16 高质心 SSF 0.8556", close(tall.spec.ssf(), 0.855556, 1e-6)) + + # 反例 3:把转移容差设成不可能满足的值,只有第四条失败 + res3 = core.check_cornering(item, transfer_tolerance=-1e-6) + check("E17 容差为负时第四条失败", not res3[3]["passed"]) + check("E18 容差为负时前三条仍通过", + all(r["passed"] for i, r in enumerate(res3) if i != 3)) + + check("E19 all_passed 空为真", core.all_passed([])) + check("E20 all_passed 有失败为假", + not core.all_passed([{"type": "x", "passed": False, "detail": ""}])) + + +# ── F 车速扫描 ─────────────────────────────────────────────────────────── + +def group_table(): + spec = core.VehicleSpec() + rows = core.speed_table(spec, 30.0) + check("F1 7 行", len(rows) == 7) + check("F2 首行 30 km/h", close(rows[0].speed_kmh, 30.0)) + check("F3 末行 90 km/h", close(rows[-1].speed_kmh, 90.0)) + check("F4 30 km/h 裕度 0.169", close(rows[0].usage(), 0.169, 1e-3)) + check("F5 50 km/h 内侧 3912.8", close(rows[2].inner_load(), 3912.83, 1e-2)) + check("F6 70 km/h 不抬轮但很接近", not rows[4].lifts_off() + and rows[4].usage() > 0.9) + check("F7 80 km/h 抬轮", rows[5].lifts_off()) + check("F8 90 km/h 抬轮", rows[6].lifts_off()) + check("F9 裕度随车速单调递增", + all(rows[i].usage() < rows[i + 1].usage() for i in range(6))) + check("F10 内侧载荷随车速单调递减", + all(rows[i].inner_load() > rows[i + 1].inner_load() for i in range(6))) + check("F11 抬轮发生在 70~80 之间", + not rows[4].lifts_off() and rows[5].lifts_off()) + check("F12 半径 60 m 时 80 km/h 不抬轮", + not core.Cornering(spec, 60.0, 80.0).lifts_off()) + + +# ── G 字符图与报告 ─────────────────────────────────────────────────────── + +def group_report(): + spec = core.VehicleSpec() + item = core.Cornering(spec, 30.0, 60.0) + results = core.check_cornering(item) + + lines = core.render_loads(item) + check("G1 字符图 13 行", len(lines) == core.DEFAULT_GRID_HEIGHT + 1) + check("G2 有外侧条", any("=" in line for line in lines[:-1])) + check("G3 有内侧条", any("#" in line for line in lines[:-1])) + check("G4 有中线", any("|" in line for line in lines[:-1])) + check("G5 脚注含两个载荷", + u"内侧" in lines[-1] and u"外侧" in lines[-1]) + check("G6 网格太小报错", + raises(core.RolloverError, core.render_loads, item, 10, 3)) + check("G7 自定义尺寸", len(core.render_loads(item, 40, 8)) == 9) + + lifted = core.Cornering(spec, 30.0, 90.0) + lines_lift = core.render_loads(lifted) + check("G8 抬轮时内侧条消失", + not any("#" in line for line in lines_lift[:-1])) + + report = core.format_report(item, results, header=u"测试标题") + check("G9 报告含标题", u"测试标题" in report) + check("G10 报告含 SSF", u"SSF = 1.4000" in report) + check("G11 报告含断言统计", u"断言:4 通过 / 0 失败" in report) + check("G12 报告含 PASSED", u"结论:PASSED" in report) + check("G13 报告含车速扫描表头", u"车速(km/h)" in report) + check("G14 报告含 7 行车速", report.count(u"km/h") >= 7) + check("G15 报告行数 > 30", len(report.splitlines()) > 30) + + failed = core.check_cornering(item, rollover_margin=0.5) + report_fail = core.format_report(item, failed) + check("G16 失败报告含 [FAIL]", u"[FAIL]" in report_fail) + check("G17 失败报告 FAILED", u"结论:FAILED" in report_fail) + check("G18 失败统计 3 通过 / 1 失败", u"3 通过 / 1 失败" in report_fail) + + +# ── H 参数读取 ─────────────────────────────────────────────────────────── + +def group_params_read(): + def getter(name, default=None): + return default + + params = core.RolloverParams.from_params(getter) + check("H1 默认 track", close(params.track, 1.54)) + check("H2 默认 cg_height", close(params.cg_height, 0.55)) + check("H3 默认 radius", close(params.radius, 30.0)) + check("H4 默认 speed", close(params.speed_kmh, 60.0)) + check("H5 默认 margin", close(params.rollover_margin, 0.70)) + check("H6 vehicle() 返回 VehicleSpec", isinstance(params.vehicle(), core.VehicleSpec)) + check("H7 cornering() 返回 Cornering", isinstance(params.cornering(), core.Cornering)) + check("H8 as_dict 9 项", len(params.as_dict()) == 9) + + overrides = { + "vehicle/track": "1.70", + "vehicle/cg_height": "0.62", + "corner/speed_kmh": "75", + "check/rollover_margin": "0.65", + } + + def getter2(name, default=None): + return overrides.get(name, default) + + params2 = core.RolloverParams.from_params(getter2) + check("H9 覆盖 track", close(params2.track, 1.70)) + check("H10 覆盖 cg_height", close(params2.cg_height, 0.62)) + check("H11 覆盖 speed", close(params2.speed_kmh, 75.0)) + check("H12 覆盖 margin", close(params2.rollover_margin, 0.65)) + check("H13 未覆盖项回落默认", close(params2.radius, 30.0)) + + check("H14 非法 track 构造即报错", + raises(core.RolloverError, core.RolloverParams, **{"vehicle/track": 0.0})) + check("H15 负车速报错", + raises(core.RolloverError, core.RolloverParams, **{"corner/speed_kmh": -1})) + check("H16 字符串数字可用", + close(core.RolloverParams(**{"vehicle/track": "1.6"}).track, 1.6)) + + +# ── I 工程文件 ─────────────────────────────────────────────────────────── + +def group_project(): + pkg_xml = os.path.join(PKG, "package.xml") + cmake = os.path.join(PKG, "CMakeLists.txt") + yaml_path = os.path.join(PKG, "config", "rollover_margin.yaml") + launch_path = os.path.join(PKG, "launch", "main.launch") + readme = os.path.join(PKG, "README.md") + + for path, name in ((pkg_xml, "I1 package.xml"), (cmake, "I2 CMakeLists.txt"), + (yaml_path, "I3 config yaml"), (launch_path, "I4 launch"), + (readme, "I5 README")): + check(name + " 存在", os.path.isfile(path), path) + + pkg_text = io.open(pkg_xml, "r", encoding="utf-8").read() + check("I6 包名 rollover_margin", "rollover_margin" in pkg_text) + check("I7 依赖 rospy", "rospy" in pkg_text) + check("I8 依赖 std_msgs", "std_msgs" in pkg_text) + + cmake_text = io.open(cmake, "r", encoding="utf-8").read() + check("I9 安装三个脚本", cmake_text.count("scripts/") >= 3) + check("I10 安装 launch 与 config", "install(DIRECTORY launch config" in cmake_text) + + launch_text = io.open(launch_path, "r", encoding="utf-8").read() + check("I11 type=main.py", 'type="main.py"' in launch_text) + check("I12 用 $(find ...)", "$(find rollover_margin)" in launch_text) + check("I13 不含 rospack", "rospack" not in launch_text) + check("I14 加载 yaml", "config/rollover_margin.yaml" in launch_text) + check("I15 arg 数 12", launch_text.count(" 2000) + check("I29 文档页含四条断言名", + all(name in doc_text for name in core.ASSERT_NAMES)) + check("I30 文档页含 roslaunch", "roslaunch rollover_margin" in doc_text) + else: + check("I28 本次未在仓库根目录运行,跳过文档页校验", True) + check("I29 本地不含 docs 目录", True) + check("I30 包目录仍然存在", os.path.isdir(PKG)) + + +def _pytest_group(func): + def runner(): + before = len(FAILED) + func() + new = FAILED[before:] + if new: + raise AssertionError(u"本组失败 %d 项:\n%s" % (len(new), u"\n".join(new))) + runner.__name__ = "test_" + func.__name__[len("group_"):] + return runner + + +test_params = _pytest_group(group_params) +test_vehicle = _pytest_group(group_vehicle) +test_cornering = _pytest_group(group_cornering) +test_loads = _pytest_group(group_loads) +test_checks = _pytest_group(group_checks) +test_table = _pytest_group(group_table) +test_report = _pytest_group(group_report) +test_params_read = _pytest_group(group_params_read) +test_project = _pytest_group(group_project) + + +def main(): + print(u"rollover_margin 离线回归测试") + print(u"Python %s" % sys.version.split()[0]) + print(u"包目录 %s" % PKG) + print(u"-" * 60) + group_params() + group_vehicle() + group_cornering() + group_loads() + group_checks() + group_table() + group_report() + group_params_read() + group_project() + print(u"=" * 60) + for item in FAILED: + print(u"[失败] %s" % item) + print(u"通过 %d 项,失败 %d 项" % (len(PASSED), len(FAILED))) + if not FAILED: + print(u"全部通过") + return 0 + return 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/test_tire_slip_curve_local.py b/tests/test_tire_slip_curve_local.py new file mode 100644 index 0000000000..6309659399 --- /dev/null +++ b/tests/test_tire_slip_curve_local.py @@ -0,0 +1,478 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""tire_slip_curve 的离线回归测试。 + +特点: +* 只用 Python 标准库,**不需要 ROS、不需要 CARLA、不需要显示器**; +* 同时兼容 Python 2.7 与 Python 3.x(Melodic 与 Noetic 都能直接跑); +* 在仓库根目录执行:python3 tests/test_tire_slip_curve_local.py + +覆盖范围: +A 参数校验 B 核心数学 C 曲线采样 +D 关键量 E 四条断言 F 报告与字符图 +G 参数读取 H 工程文件约定 I pytest 包装 +""" +from __future__ import division, print_function + +import io +import math +import os +import sys + +try: + text_type = unicode # noqa: F821 Python 2 +except NameError: + text_type = str # Python 3 + +HERE = os.path.dirname(os.path.abspath(__file__)) +ROOT = os.path.dirname(HERE) +PKG = os.path.join(ROOT, "src", "ground", "tire_slip_curve") +SCRIPTS = os.path.join(PKG, "scripts") +DOC_PATH = os.path.join(ROOT, "docs", "ground", "tire_slip_curve.md") + +sys.path.insert(0, SCRIPTS) +import tire_core as core # noqa: E402 + +PASSED = [] +FAILED = [] + + +def check(name, condition, detail=""): + if condition: + PASSED.append(name) + else: + FAILED.append(name if not detail else "%s(%s)" % (name, detail)) + + +def close(a, b, tol=1e-9): + return abs(a - b) <= tol + + +def raises(expected, func, *args, **kwargs): + try: + func(*args, **kwargs) + except expected: + return True + except Exception: # noqa: BLE001 + return False + return False + + +# ── A 参数校验 ─────────────────────────────────────────────────────────── + +def group_params(): + check("A1 默认峰值滑移率 0.12", close(core.DEFAULT_PEAK_SLIP, 0.12)) + check("A2 默认峰值附着 0.90", close(core.DEFAULT_PEAK_FRICTION, 0.90)) + check("A3 默认 ABS 带宽 0.20", close(core.DEFAULT_ABS_BAND, 0.20)) + check("A4 默认路面 dry_asphalt", + core.DEFAULT_SURFACE == u"dry_asphalt") + check("A5 默认采样 51 点", core.DEFAULT_SAMPLES == 51) + check("A6 断言名 4 条", len(core.ASSERT_NAMES) == 4) + check("A7 断言名内容", + core.ASSERT_NAMES == (u"峰值唯一", u"锁死衰减", u"初段线性", u"目标带覆盖峰值")) + check("A8 PARAM_KEYS 12 个", len(core.PARAM_KEYS) == 12) + check("A9 参数键含 tire/peak_slip", "tire/peak_slip" in core.PARAM_KEYS) + check("A10 参数键含 topic/peak_slip", "topic/peak_slip" in core.PARAM_KEYS) + + spec = core.TireSpec() + check("A11 默认构造 peak_slip", close(spec.peak_slip, 0.12)) + check("A12 默认构造 peak_friction", close(spec.peak_friction, 0.90)) + check("A13 as_tuple 四元", len(spec.as_tuple()) == 4) + + check("A14 peak_slip=0 报错", raises(core.TireError, core.TireSpec, 0.0, 0.9)) + check("A15 peak_slip 负值报错", raises(core.TireError, core.TireSpec, -0.1, 0.9)) + check("A16 peak_slip>1 报错", raises(core.TireError, core.TireSpec, 1.5, 0.9)) + check("A17 peak_friction=0 报错", raises(core.TireError, core.TireSpec, 0.12, 0.0)) + check("A18 abs_band=1.0 报错", raises(core.TireError, core.TireSpec, 0.12, 0.9, 1.0)) + check("A19 abs_band 负值报错", raises(core.TireError, core.TireSpec, 0.12, 0.9, -0.1)) + check("A20 未知路面报错", + raises(core.TireError, core.TireSpec, 0.12, 0.9, 0.2, "grass")) + check("A21 非数字 peak_slip 报错", + raises(core.TireError, core.TireSpec, "abc", 0.9)) + check("A22 NaN 报错", raises(core.TireError, core.TireSpec, float("nan"), 0.9)) + check("A23 布尔值报错", raises(core.TireError, core.TireSpec, True, 0.9)) + check("A24 字符串数字可用", close(core.TireSpec("0.15", "0.8").peak_slip, 0.15)) + check("A25 四种路面都能构造", + all(core.TireSpec(0.12, 0.9, 0.2, s) for s in core.SURFACES)) + + +# ── B 核心数学 ─────────────────────────────────────────────────────────── + +def group_math(): + spec = core.TireSpec() + check("B1 friction(0)=0", close(spec.friction(0.0), 0.0)) + check("B2 friction(peak)=peak_friction", close(spec.friction(0.12), 0.90)) + check("B3 半峰值滑移 → 0.8 倍", + close(spec.friction(0.06), 0.72, 1e-12)) + check("B4 负滑移取绝对值", + close(spec.friction(-0.12), spec.friction(0.12))) + check("B5 对称性", close(spec.friction(-0.3), spec.friction(0.3))) + check("B6 初段刚度闭式解 15.0", close(spec.slope_at_zero(), 15.0)) + check("B7 锁死滑移 = 1.0", close(spec.lock_slip(), 1.0)) + check("B8 锁死附着 0.212934", close(spec.lock_friction(), 0.212934, 1e-6)) + check("B9 锁死占比 23.66%", close(spec.lock_friction() / 0.90, 0.236593, 1e-6)) + low, high = spec.abs_band_limits() + check("B10 目标带下界 0.096", close(low, 0.096)) + check("B11 目标带上界 0.144", close(high, 0.144)) + check("B12 目标带包含峰值", low <= 0.12 <= high) + + # 单调性:0 → peak 递增,peak → 1 递减 + ups = [spec.friction(s / 100.0) for s in range(0, 13)] + check("B13 0→peak 单调递增", + all(ups[i] < ups[i + 1] for i in range(len(ups) - 1))) + downs = [spec.friction(s / 100.0) for s in range(12, 101)] + check("B14 peak→1 单调递减", + all(downs[i] > downs[i + 1] for i in range(len(downs) - 1))) + + # 峰值唯一 + peak_mu = max(spec.friction(s / 1000.0) for s in range(0, 1001)) + check("B15 峰值等于 peak_friction", close(peak_mu, 0.90, 1e-9)) + check("B16 峰值只出现在 s_peak", + close(spec.friction(0.12), spec.friction(0.12))) + + # 宽参数扫描下仍然保持解析关系 + worst = 0.0 + for peak_slip in (0.08, 0.12, 0.2, 0.35): + for peak_mu in (0.4, 0.7, 1.1): + sp = core.TireSpec(peak_slip, peak_mu) + worst = max(worst, abs(sp.friction(peak_slip) - peak_mu)) + check("B17 参数组合下峰值仍精确", worst <= 1e-12, "worst=%.2e" % worst) + + check("B18 初段斜率随 peak_slip 变小而变大", + core.TireSpec(0.08, 0.9).slope_at_zero() + > core.TireSpec(0.2, 0.9).slope_at_zero()) + + +# ── C 曲线采样 ─────────────────────────────────────────────────────────── + +def group_curve(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + check("C1 采样点数 51", curve.count == 51) + check("C2 起点 slip=0", close(curve.first().slip, 0.0)) + check("C3 起点 friction=0", close(curve.first().friction, 0.0)) + check("C4 终点 slip=1", close(curve.last().slip, 1.0)) + check("C5 终点 friction=锁死", close(curve.last().friction, spec.lock_friction())) + check("C6 步长 0.02", close(curve.points[1].slip - curve.points[0].slip, 0.02)) + check("C7 峰值点 slip=0.12", close(curve.max_point().slip, 0.12)) + check("C8 峰值点 friction=0.90", close(curve.max_point().friction, 0.90)) + check("C9 ratio 峰值=1.0", close(curve.max_point().ratio, 1.0)) + check("C10 at(0.05) 取最近点 0.06", + close(curve.at(0.05).slip, 0.06) or close(curve.at(0.05).slip, 0.04)) + check("C11 at(0.06) friction=0.72", close(curve.at(0.06).friction, 0.72)) + check("C12 采样点全都有限", + all(not math.isnan(p.friction) and not math.isinf(p.friction) + for p in curve.points)) + check("C13 monotonic_up_to(0.11) 为真", curve.monotonic_up_to(0.11)) + check("C14 monotonic_up_to(1.0) 为假(后半段下降)", + not curve.monotonic_up_to(1.0)) + check("C15 ratio 单调段也递增", + curve.points[3].ratio < curve.points[6].ratio) + check("C16 as_rows 行数一致", len(curve.as_rows()) == curve.count) + check("C17 空曲线 is_empty", + core.Curve([], spec).is_empty()) + check("C18 空曲线 max_point 返回 None", + core.Curve([], spec).max_point() is None) + check("C19 samples=5 可用", core.build_curve(spec, 5).count == 5) + check("C20 samples=4 报错", raises(core.TireError, core.build_curve, spec, 4)) + check("C21 samples 非整数报错", + raises(core.TireError, core.build_curve, spec, 10.5)) + check("C22 曲线名字含路面", + u"dry_asphalt" in curve.name) + + +# ── D 关键量 ───────────────────────────────────────────────────────────── + +def group_keys(): + keys = core.key_points(core.TireSpec()) + check("D1 surface", keys["surface"] == u"dry_asphalt") + check("D2 peak_slip", close(keys["peak_slip"], 0.12)) + check("D3 peak_friction", close(keys["peak_friction"], 0.90)) + check("D4 lock_slip", close(keys["lock_slip"], 1.0)) + check("D5 lock_friction", close(keys["lock_friction"], 0.212934, 1e-6)) + check("D6 lock_ratio", close(keys["lock_ratio"], 0.236593, 1e-6)) + check("D7 slope_at_zero", close(keys["slope_at_zero"], 15.0)) + check("D8 abs_low", close(keys["abs_low"], 0.096)) + check("D9 abs_high", close(keys["abs_high"], 0.144)) + check("D10 abs_peak_friction 等于峰值", + close(keys["abs_peak_friction"], 0.90)) + check("D11 键数量 11", len(keys) == 11) + + band_free = core.TireSpec(0.12, 0.9, 0.0) + check("D12 带宽 0 时上下界都等于峰值", + close(band_free.abs_band_limits()[0], 0.12) + and close(band_free.abs_band_limits()[1], 0.12)) + + +# ── E 四条断言 ─────────────────────────────────────────────────────────── + +def group_checks(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + results = core.check_curve(curve, spec) + check("E1 返回 4 条", len(results) == 4) + check("E2 顺序与 ASSERT_NAMES 一致", + [r["type"] for r in results] == list(core.ASSERT_NAMES)) + check("E3 每条含 passed/detail", + all("passed" in r and "detail" in r for r in results)) + check("E4 默认全通过", core.all_passed(results)) + check("E5 峰值唯一通过", results[0]["passed"]) + check("E6 锁死衰减通过", results[1]["passed"]) + check("E7 初段线性通过", results[2]["passed"]) + check("E8 目标带覆盖通过", results[3]["passed"]) + check("E9 detail 里带数字", + "%" in results[2]["detail"] or "." in results[2]["detail"]) + + # 反例 1:粗采样抓不到峰值 + coarse = core.build_curve(spec, 5) + res_coarse = core.check_curve(coarse, spec) + check("E10 samples=5 → 峰值唯一失败", not res_coarse[0]["passed"]) + check("E11 samples=5 → 其余三条仍通过", + all(r["passed"] for r in res_coarse[1:])) + + # 反例 2:锁死衰减阈值收紧 + res_lock = core.check_curve(curve, spec, lock_friction_ratio=0.20) + check("E12 lock_ratio=0.20 → 锁死衰减失败", not res_lock[1]["passed"]) + check("E13 lock_ratio=0.20 → 其余三条仍通过", + all(r["passed"] for i, r in enumerate(res_lock) if i != 1)) + + # 反例 3:初段线性阈值收紧 + res_lin = core.check_curve(curve, spec, linear_tolerance=0.001) + check("E14 linear_tolerance=0.001 → 初段线性失败", not res_lin[2]["passed"]) + check("E15 linear_tolerance=0.001 → 其余三条仍通过", + all(r["passed"] for i, r in enumerate(res_lin) if i != 2)) + + # 反例 4:峰值容差收紧 + res_peak = core.check_curve(curve, spec, peak_tolerance=1e-9) + check("E16 峰值容差收紧仍通过(峰值精确命中采样点)", + res_peak[0]["passed"]) + + # 雪地参数:峰值滑移大、峰值附着低 + snow = core.TireSpec(0.30, 0.35, 0.2, u"snow") + res_snow = core.check_curve(core.build_curve(snow, 51), snow) + check("E17 雪地曲线通过", core.all_passed(res_snow)) + check("E18 雪地初段刚度更低", + snow.slope_at_zero() < spec.slope_at_zero()) + + check("E19 all_passed 空列表为真", core.all_passed([])) + check("E20 all_passed 有失败项为假", + not core.all_passed([{"type": "x", "passed": False, "detail": ""}])) + + +# ── F 报告与字符图 ─────────────────────────────────────────────────────── + +def group_report(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + results = core.check_curve(curve, spec) + report = core.format_report(curve, results, header=u"测试标题") + check("F1 报告含标题", u"测试标题" in report) + check("F2 报告含路面", u"dry_asphalt" in report) + check("F3 报告含断言统计", u"断言:4 通过 / 0 失败" in report) + check("F4 报告含结论 PASSED", u"结论:PASSED" in report) + check("F5 报告含四条明细", + all(name in report for name in core.ASSERT_NAMES)) + check("F6 报告含 [PASS] 标记", report.count(u"[PASS]") == 4) + check("F7 报告行数 > 20", len(report.splitlines()) > 20) + + failed = core.check_curve(curve, spec, lock_friction_ratio=0.20) + report_fail = core.format_report(curve, failed) + check("F8 失败报告含 [FAIL]", u"[FAIL]" in report_fail) + check("F9 失败报告结论 FAILED", u"结论:FAILED" in report_fail) + check("F10 失败报告统计 3 通过 / 1 失败", u"3 通过 / 1 失败" in report_fail) + + lines = core.render_curve(curve) + check("F11 字符图 13 行(12 + 轴标)", len(lines) == core.DEFAULT_GRID_HEIGHT + 1) + check("F12 字符图有曲线点", + any("*" in line for line in lines[:-1])) + check("F13 字符图有目标带底纹", + any("." in line for line in lines[:-1])) + check("F14 轴标含滑移率", u"滑移率" in lines[-1]) + check("F15 空曲线渲染为空列表", core.render_curve(core.Curve([], spec)) == []) + check("F16 网格太小时报错", + raises(core.TireError, core.render_curve, curve, 10, 3)) + custom = core.render_curve(curve, 40, 8) + check("F17 自定义尺寸行数", len(custom) == 9) + + +# ── G 参数读取 ─────────────────────────────────────────────────────────── + +def group_params_read(): + def getter(name, default=None): + return default + + params = core.TireParams.from_params(getter) + check("G1 默认 peak_slip", close(params.peak_slip, 0.12)) + check("G2 默认 peak_friction", close(params.peak_friction, 0.90)) + check("G3 默认 surface", params.surface == u"dry_asphalt") + check("G4 默认 samples 51", params.samples == 51) + check("G5 默认 lock_ratio 0.85", close(params.lock_friction_ratio, 0.85)) + check("G6 默认 topic_report", params.topic_report == u"/tire/report") + check("G7 spec() 返回 TireSpec", isinstance(params.spec(), core.TireSpec)) + check("G8 as_dict 含 9 项", len(params.as_dict()) == 9) + + overrides = { + "tire/peak_slip": "0.15", + "tire/peak_friction": "0.75", + "road/surface": "wet_asphalt", + "curve/samples": "31", + "check/lock_friction_ratio": "0.80", + "rate/publish_hz": "5.0", + } + + def getter2(name, default=None): + return overrides.get(name, default) + + params2 = core.TireParams.from_params(getter2) + check("G9 覆盖 peak_slip", close(params2.peak_slip, 0.15)) + check("G10 覆盖 peak_friction", close(params2.peak_friction, 0.75)) + check("G11 覆盖 surface", params2.surface == u"wet_asphalt") + check("G12 覆盖 samples", params2.samples == 31) + check("G13 覆盖 lock_ratio", close(params2.lock_friction_ratio, 0.80)) + check("G14 覆盖 publish_hz", close(params2.publish_hz, 5.0)) + + check("G15 非法参数构造即报错", + raises(core.TireError, core.TireParams, **{"tire/peak_slip": 0.0})) + + unknown_surface = core.TireParams(**{"road/surface": "grass"}) + check("G16 未知路面回落默认", unknown_surface.surface == u"dry_asphalt") + + bytes_surface = core.TireParams(**{"road/surface": b"wet_asphalt"}) + check("G17 py2 bytes 路面可解码", bytes_surface.surface == u"wet_asphalt") + + int_like = core.TireParams(**{"curve/samples": 41.0}) + check("G18 整数型浮点采样可用", int_like.samples == 41) + + +# ── H 工程文件约定 ─────────────────────────────────────────────────────── + +def group_project(): + pkg_xml = os.path.join(PKG, "package.xml") + cmake = os.path.join(PKG, "CMakeLists.txt") + yaml_path = os.path.join(PKG, "config", "tire_slip_curve.yaml") + launch_path = os.path.join(PKG, "launch", "main.launch") + readme = os.path.join(PKG, "README.md") + + for path, name in ((pkg_xml, "H1 package.xml"), (cmake, "H2 CMakeLists.txt"), + (yaml_path, "H3 config yaml"), (launch_path, "H4 launch"), + (readme, "H5 README")): + check(name + " 存在", os.path.isfile(path), path) + + with io.open(pkg_xml, "r", encoding="utf-8") as fh: + pkg_text = fh.read() + check("H6 包名 tire_slip_curve", "tire_slip_curve" in pkg_text) + check("H7 依赖 rospy", "rospy" in pkg_text) + check("H8 依赖 std_msgs", "std_msgs" in pkg_text) + + with io.open(cmake, "r", encoding="utf-8") as fh: + cmake_text = fh.read() + check("H9 安装三个脚本", + cmake_text.count("scripts/") >= 3) + check("H10 安装 config 目录", "install(DIRECTORY launch config" in cmake_text) + check("H11 find_package 含 rospy", "rospy" in cmake_text) + + with io.open(launch_path, "r", encoding="utf-8") as fh: + launch_text = fh.read() + check("H12 launch 用 type=main.py", 'type="main.py"' in launch_text) + check("H13 launch 用 $(find ...)", "$(find tire_slip_curve)" in launch_text) + check("H14 launch 不含 rospack", + "rospack" not in launch_text) + check("H15 launch 加载 yaml", "config/tire_slip_curve.yaml" in launch_text) + check("H16 launch arg 数 12", launch_text.count("= 11) + + with io.open(readme, "r", encoding="utf-8") as fh: + readme_text = fh.read() + check("H20 README 含 roslaunch 命令", + "roslaunch tire_slip_curve main.launch" in readme_text) + check("H21 README 含测试命令", + "test_tire_slip_curve_local.py" in readme_text) + + check("H22 核心模块不 import rospy", + "\nimport rospy" not in io.open( + os.path.join(SCRIPTS, "tire_core.py"), "r", encoding="utf-8").read()) + node_text = io.open(os.path.join(SCRIPTS, "tire_node.py"), "r", + encoding="utf-8").read() + check("H23 节点 import tire_core", "import tire_core as core" in node_text) + check("H24 节点有 _log 辅助", "def _log(" in node_text) + check("H25 节点发布三个话题", node_text.count("rospy.Publisher") == 3) + check("H26 main.py 复用节点", "from tire_node import main" in io.open( + os.path.join(SCRIPTS, "main.py"), "r", encoding="utf-8").read()) + + if os.path.isfile(DOC_PATH): + doc_text = io.open(DOC_PATH, "r", encoding="utf-8").read() + check("H27 文档页存在且非空", len(doc_text) > 2000) + check("H28 文档页含四条断言名", + all(name in doc_text for name in core.ASSERT_NAMES)) + check("H29 文档页含 roslaunch 命令", + "roslaunch tire_slip_curve" in doc_text) + else: + check("H27 本次未在仓库根目录运行,跳过文档页校验", True) + check("H28 本地不含 docs 目录", True) + check("H29 包目录仍然存在", os.path.isdir(PKG)) + + +# ── pytest 包装 ────────────────────────────────────────────────────────── + +def _pytest_group(func): + def runner(): + before = len(FAILED) + func() + new = FAILED[before:] + if new: + raise AssertionError(u"本组失败 %d 项:\n%s" % (len(new), u"\n".join(new))) + runner.__name__ = "test_" + func.__name__[len("group_"):] + return runner + + +test_params = _pytest_group(group_params) +test_math = _pytest_group(group_math) +test_curve = _pytest_group(group_curve) +test_keys = _pytest_group(group_keys) +test_checks = _pytest_group(group_checks) +test_report = _pytest_group(group_report) +test_params_read = _pytest_group(group_params_read) +test_project = _pytest_group(group_project) + + +# ── 主流程 ─────────────────────────────────────────────────────────────── + +def main(): + print(u"tire_slip_curve 离线回归测试") + print(u"Python %s" % sys.version.split()[0]) + print(u"包目录 %s" % PKG) + print(u"-" * 60) + group_params() + group_math() + group_curve() + group_keys() + group_checks() + group_report() + group_params_read() + group_project() + print(u"=" * 60) + for item in FAILED: + print(u"[失败] %s" % item) + print(u"通过 %d 项,失败 %d 项" % (len(PASSED), len(FAILED))) + if not FAILED: + print(u"全部通过") + return 0 + return 1 + + +if __name__ == "__main__": + sys.exit(main())