Repository navigation
feat(classical_control): 新增倒立摆能量摆起与 LQR 平衡模块 - #1160
Merged
Merged
Conversation
Contributor
|
4 of 5 tasks
xinyangtang91
force-pushed
the
feat/cartpole_swingup
branch
from
November 13, 2026 11:32
1dce05a to
d18f418
Compare
现有 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
force-pushed
the
feat/cartpole_swingup
branch
from
November 13, 2026 11:32
d18f418 to
782ab86
Compare
Contributor
Author
|
已按评审意见修改并重建分支:
本地验证:29 项测试通过、 |
tianyutu981
approved these changes
Nov 13, 2026
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
修改目的与内容
现有
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 端实际运行尚未验证。
测试环境与结果
python src/cartpole_swingup/offline_sim.pypython tests/test_cartpole_swingup_local.pypython -m mkdocs buildpython -m mkdocs serve --dev-addr 127.0.0.1:8004 --livereloadcp -r src/cartpole_swingup ~/catkin_ws/src/ && cd ~/catkin_ws && catkin_make && source devel/setup.bash && roslaunch cartpole_swingup main.launchmkdocs build通过(31.88 秒),新页面、导航和三张图片均返回 200;main.py在 ROS 中与倒立摆仿真器的联调、catkin_make编译、main.launch启动。运行效果
摆起与平衡全过程(绿区为 LQR 平衡阶段):
相图(橙:能量摆起,绿:LQR 平衡):
与「只用 LQR」的对比:
三张图由
offline_sim.py在本机实际运行生成;ROS 端接入仿真器/实车后的效果待另一台机器复验后补充。提交前检查
git diff与 PR 页面均只有本模块的 15 个文件;分支基于最新 master,无冲突。)main.py、main.launch、package.xml、CMakeLists.txt已提供,依赖与安装路径已检查;但 README 的 ROS 运行步骤未在本机执行,因为本机没有 ROS,待另一台机器验证。)大模型使用声明