Skip to content

feat(classical_control): 新增倒立摆能量摆起与 LQR 平衡模块 - #1160

Merged
tianyutu981 merged 1 commit into
OpenHUTB:masterfrom
xinyangtang91:feat/cartpole_swingup
Nov 13, 2026
Merged

tianyutu981 merged 1 commit into
OpenHUTB:masterfrom
xinyangtang91:feat/cartpole_swingup

Conversation

@xinyangtang91

Copy link
Copy Markdown
Contributor

修改目的与内容

现有 cartpole_balance 只演示「摆杆初始倾斜约 0.2 rad,LQR 把它拉回竖直」,没有覆盖从自然下垂摆起这一大范围非线性控制问题,也没有对照说明为什么单靠 LQR 不行。本 PR 新增 cartpole_swingup,把「能量摆起 + LQR 平衡」这一经典组合完整做出来。

  • src/cartpole_swingup/:cartpole_model.py 实现非线性动力学、机械能函数、离散 Riccati 迭代求 LQR 增益,以及「能量摆起 → 接近竖直切 LQR」双模控制器;main.py 订阅 /cartpole/state,发布 /cartpole/force;含 main.launch、config/params.yaml、README.md 和离线仿真。
  • 安全处理:超过 state_timeout(0.5 s)没有状态更新时控制力归零;节点退出时同样归零。
  • docs/classical_control/cartpole_swingup.md:动力学与能量控制推导、LQR 线性化、模式切换逻辑、ROS 与离线运行步骤、三张结果图和指标表。
  • 首页 docs/index.md 与 mkdocs.yml 导航加入口。
  • tests/test_cartpole_swingup_local.py:不依赖 ROS 的模型、能量、LQR 稳定性、模式切换和工程文件检查。

已知限制:仿真为理想模型,不含传感器噪声、电机死区和轨道限位;切换阈值是手工整定的;本机没有 ROS 与倒立摆仿真器,ROS 端实际运行尚未验证。

测试环境与结果

  • 实际系统、ROS、Python、模拟器及关键依赖版本:
    • 离线仿真与文档构建:Windows 10.0.26200,Python 3.12.14,numpy 2.5.4,matplotlib 3.11.2,mkdocs 1.6.1
    • ROS 端目标环境:Ubuntu 20.04 + ROS Noetic(本次未在本机运行,因为本机没有 ROS/倒立摆仿真器)
  • 构建与运行命令(注明执行目录):
    • 在仓库根目录:python src/cartpole_swingup/offline_sim.py
    • 在仓库根目录:python tests/test_cartpole_swingup_local.py
    • 在仓库根目录:python -m mkdocs build
    • 在仓库根目录:python -m mkdocs serve --dev-addr 127.0.0.1:8004 --livereload
    • ROS 端(需 Ubuntu + Noetic,本机未执行):cp -r src/cartpole_swingup ~/catkin_ws/src/ && cd ~/catkin_ws && catkin_make && source devel/setup.bash && roslaunch cartpole_swingup main.launch
  • 验证操作、结果及尚未验证的内容:
    • 离线仿真实际运行:5.26 s 切入 LQR、7.00 s 角度稳定(|θ|<0.05 rad)、10.10 s 角度与小车都稳定、最大控制力 8.73 N、小车最大行程 0.86 m、最终角度误差 < 0.001°;
    • 只用 LQR 的对照组从下垂出发,$\cos\theta$ 在 −1~1 之间持续大幅摆动,无法稳定;
    • 本地测试 29 项全部通过;mkdocs build 通过(31.88 秒),新页面、导航和三张图片均返回 200;
    • 尚未验证:main.py 在 ROS 中与倒立摆仿真器的联调、catkin_make 编译、main.launch 启动。

运行效果

摆起与平衡全过程(绿区为 LQR 平衡阶段):

摆起与平衡响应

相图(橙:能量摆起,绿:LQR 平衡):

相图

与「只用 LQR」的对比:

只用 LQR 的对比

三张图由 offline_sim.py 在本机实际运行生成;ROS 端接入仿真器/实车后的效果待另一台机器复验后补充。

提交前检查

  • 已检查 Files changed 和冲突,无无关生成文件、重复内容或误删公共文件。(本地 git diff 与 PR 页面均只有本模块的 15 个文件;分支基于最新 master,无冲突。)
  • 源码与教程按模块分别放在 src、docs,文件使用英文下划线命名。
  • 模块提供 main.* 和 launch,依赖及安装路径正确,README 步骤已实际运行。(main.py、main.launch、package.xml、CMakeLists.txt 已提供,依赖与安装路径已检查;但 README 的 ROS 运行步骤未在本机执行,因为本机没有 ROS,待另一台机器验证。)
  • 新模块已添加首页和导航链接;已运行 mkdocs serve --livereload 检查相关页面。
  • 功能描述与验证证据一致;PNG 小于 3 MB,GIF 小于 5 MB。(三张图分别为 129 KB、80 KB、168 KB;文档已注明是离线仿真结果。)

大模型使用声明

  • 使用工具:DeepSeek 助手(DeepSeek Harness)。
  • 辅助范围:模块代码与文档初稿、测试和离线仿真脚本、命令核对;运行数据和三张图由本机实际运行生成,不是模型编造。
  • 作者已逐项核对上述内容,并对提交内容的正确性负全部责任;ROS 端复验项见“尚未验证”和未勾选的检查项。

@tianyutu981

Copy link
Copy Markdown
Contributor
  1. 【目录规范】源码目录 src/cartpole_swingup/ 缺少分类层,应与文档保持一致放到 src/classical_control/cartpole_swingup/(仓库其他模块都是 src/<分类>/<包名>/,例如 src/ground/、src/water/)。同时同步更新 README/文档里的 cp -r <本仓库>/src/... 路径和测试里 PKG = .../src/cartpole_swingup 的路径。

  2. 【会编译失败】CMakeLists.txt 里写了 install(DIRECTORY launch config ...),但 main.launch 在包根目录、并没有 launch/ 子目录,catkin_make 会因找不到 launch/ 报错。请把 main.launch 移到 launch/main.launch(与 feat(ground): 新增差速小车沿墙行驶模块 #1150 已修复的问题相同)。

现有 cartpole_balance 只演示摆杆初始倾斜约 0.2 rad 时用 LQR 拉回,
没有覆盖「从自然下垂摆起」这一大范围非线性控制问题,也没有对照说明
为什么单靠 LQR 不行。

- 新增 src/cartpole_swingup:cartpole_model.py 实现非线性动力学、能量
  函数、离散 Riccati 求 LQR 增益和「能量摆起 + LQR 平衡」双模控制器;
  main.py 订阅 /cartpole/state、发布 /cartpole/force;含 launch、参数
  文件和离线仿真脚本。
- 新增 docs/classical_control/cartpole_swingup.md:动力学与能量控制推导、
  LQR 线性化、切换逻辑、ROS/离线运行、三张结果图与指标表。
- 首页与 mkdocs 导航加入口。
- 新增 tests/test_cartpole_swingup_local.py(29 项通过)。

验证:离线仿真在 Windows + Python 3.12 实际运行,5.26 s 切入 LQR,
7.00 s 角度稳定,10.10 s 小车与角度都稳定,最大控制力 8.73 N,最终
角度误差 < 0.001°;只用 LQR 的对照组在 −1~1 之间持续大幅摆动、无法
稳定。mkdocs build 通过,页面、导航和三张图片在 mkdocs serve 下均返回
200。ROS 端未在本机实机验证(没有 ROS 与倒立摆仿真器),需在 Noetic +
仿真器上复验。

按评审调整:
- 源码目录移到 src/classical_control/cartpole_swingup/,与 docs 及其它模块
  的 src/<分类>/<包名>/ 结构一致;
- main.launch 移到 launch/main.launch,与 CMakeLists.txt 的
  install(DIRECTORY launch config ...) 匹配;
- 同步更新 README、文档和测试中的路径;
- 分支重建到最新 master,解决 docs/index.md 与 mkdocs.yml 的冲突。
@xinyangtang91

Copy link
Copy Markdown
Contributor Author

已按评审意见修改并重建分支:

  • 源码目录从 src/cartpole_swingup/ 移到 src/classical_control/cartpole_swingup/,与 docs/classical_control/ 以及仓库其它模块的 src/<分类>/<包名>/ 结构一致;
  • main.launch 移到 launch/main.launch,与 CMakeLists.txt 中 install(DIRECTORY launch config ...) 匹配,catkin_make 不会再因找不到 launch/ 目录报错;
  • README、文档里的 cp -r/离线运行路径,以及测试里的 PKG 和 launch 文件路径都已同步更新;
  • 分支已重建到最新 master,解决 docs/index.md 与 mkdocs.yml 的冲突,PR 现在 mergeable=true(15 个文件)。

本地验证:29 项测试通过、mkdocs build 通过。

@tianyutu981
tianyutu981 merged commit 35b8bae into OpenHUTB:master Nov 13, 2026
2 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants