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/index.md b/docs/index.md index beaa3a6c05..1d2a40605f 100644 --- a/docs/index.md +++ b/docs/index.md @@ -243,6 +243,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c * [VRU 冲突风险图](./ground/vru_conflict_risk_map.md) —— 按时间对齐自车与行人、儿童和骑行者轨迹,输出最小距离、TTC 和风险等级 * [里程计健康监视(打滑 / 堵转 / 反向检测)](./ground/odom_health_monitor.md) —— 比较 `/cmd_vel` 与 `/odom` 的速度比值,识别打滑、堵转与反向运动 * [轮胎滑移率-附着曲线](./ground/tire_slip_curve.md) —— 用简化刷子轮胎模型输出滑移率与附着系数关系,标出峰值附着与 ABS 目标滑移带 +* [横向载荷转移与侧翻裕度](./ground/rollover_margin.md) —— 用 SSF、临界侧向加速度和内外侧轮垂向力算「这车会不会翻」,纯代数、不依赖 CARLA ## 5. 空域载具 diff --git a/mkdocs.yml b/mkdocs.yml index 98320ade86..820df82230 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -237,6 +237,7 @@ nav: - VRU 冲突风险图: './ground/vru_conflict_risk_map.md' - 里程计健康监视: './ground/odom_health_monitor.md' - 轮胎滑移率-附着曲线: './ground/tire_slip_curve.md' + - 横向载荷转移与侧翻裕度: './ground/rollover_margin.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/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/tests/test_rollover_margin_local.py b/tests/test_rollover_margin_local.py new file mode 100644 index 0000000000..783fd2c66e --- /dev/null +++ b/tests/test_rollover_margin_local.py @@ -0,0 +1,450 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""rollover_margin 的离线回归测试。 + +特点: +* 只用标准库,不需要 ROS、不需要 CARLA、不需要显示器; +* 兼容 Python 2.7 与 Python 3.x; +* 在仓库根目录执行:python3 tests/test_rollover_margin_local.py + +覆盖:A 参数 / B 车辆几何 / C 工况 / D 载荷 / E 四条断言 / F 车速扫描 / + G 字符图与报告 / H 参数读取 / I 工程文件 / J pytest 包装 +""" +from __future__ import division, print_function + +import io +import math +import os +import sys + +try: + 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", "rollover_margin") +SCRIPTS = os.path.join(PKG, "scripts") +DOC_PATH = os.path.join(ROOT, "docs", "ground", "rollover_margin.md") + +sys.path.insert(0, SCRIPTS) +import rollover_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 默认轮距 1.54", close(core.DEFAULT_TRACK, 1.54)) + check("A2 默认质心高 0.55", close(core.DEFAULT_CG_HEIGHT, 0.55)) + check("A3 默认质量 1500", close(core.DEFAULT_MASS, 1500.0)) + check("A4 默认半径 30", close(core.DEFAULT_CURVE_RADIUS, 30.0)) + check("A5 默认车速 60", close(core.DEFAULT_SPEED_KMH, 60.0)) + check("A6 默认裕度 0.70", close(core.DEFAULT_ROLLOVER_MARGIN, 0.70)) + check("A7 默认重力 9.81", close(core.DEFAULT_GRAVITY, 9.81)) + check("A8 断言名 4 条", core.ASSERT_NAMES == + (u"侧翻裕度", u"内侧轮载荷", u"极限车速", u"载荷转移")) + check("A9 PARAM_KEYS 12 个", len(core.PARAM_KEYS) == 12) + check("A10 车速表 7 档", len(core.SPEED_TABLE_KMH) == 7) + check("A11 车速表 30~90", core.SPEED_TABLE_KMH[0] == 30.0 + and core.SPEED_TABLE_KMH[-1] == 90.0) + + spec = core.VehicleSpec() + check("A12 默认构造", close(spec.track, 1.54) and close(spec.cg_height, 0.55)) + check("A13 as_tuple 四项", len(spec.as_tuple()) == 4) + check("A14 轮距为 0 报错", raises(core.RolloverError, core.VehicleSpec, 0.0)) + check("A15 质心高为负报错", raises(core.RolloverError, core.VehicleSpec, 1.54, -0.1)) + check("A16 质量为 0 报错", raises(core.RolloverError, core.VehicleSpec, 1.54, 0.55, 0.0)) + check("A17 非数字报错", raises(core.RolloverError, core.VehicleSpec, "abc")) + check("A18 布尔值报错", raises(core.RolloverError, core.VehicleSpec, True)) + check("A19 NaN 报错", raises(core.RolloverError, core.VehicleSpec, float("nan"))) + check("A20 字符串数字可用", close(core.VehicleSpec("1.6").track, 1.6)) + check("A21 半径为 0 报错", raises(core.RolloverError, core.Cornering, None, 0.0)) + check("A22 车速为负报错", raises(core.RolloverError, core.Cornering, None, 30.0, -5.0)) + check("A23 裕度为 0 报错", raises(core.RolloverError, core.check_cornering, + core.Cornering(), 0.0)) + + +# ── B 车辆几何 ─────────────────────────────────────────────────────────── + +def group_vehicle(): + spec = core.VehicleSpec() + check("B1 SSF = 1.4", close(spec.ssf(), 1.4)) + check("B2 SSF 公式 = track/(2h)", close(spec.ssf(), 1.54 / (2 * 0.55))) + check("B3 临界加速度 13.734", close(spec.critical_accel(), 13.734, 1e-9)) + check("B4 临界加速度 = SSF·g", close(spec.critical_accel(), 1.4 * 9.81)) + check("B5 单侧静态载荷 7357.5", close(spec.wheel_load_static(), 7357.5)) + check("B6 静态载荷 = m·g/2", close(spec.wheel_load_static(), 1500 * 9.81 / 2)) + + check("B7 轮距变大 → SSF 变大", + core.VehicleSpec(1.8).ssf() > 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())