非线性倒立摆能量起摆与LQR稳摆控制系统

ROS2 Humble + Gazebo Fortress 仿真 | 200Hz 实时闭环 | NumPy 手撕 Hamilton 矩阵法求解 CARE 方程

项目概述

个人独立完成的非线性控制仿真项目，实现从自然下垂状态通过能量泵入快速起摆，在竖直不稳定平衡点由 LQR 状态反馈接管稳定。
核心难点在于非线性起摆到lqr阶段的过渡设计，以及高动态条件下的 LQR 捕获。

• 控制频率：200 Hz（5 ms 周期）
• 起摆时间：约1.4 秒
• LQR 接管窗口：偏离竖直 ≤25°、角速度 ≤5 rad/s
• 位置保护：全过程小车位移控制在 ±0.3 m 内

本项目记录了从"摆杆静止"到"摆杆稳定受控"的完整调试迭代，每个设计决策都有物理根因分析，详见调试迭代记录

---

技术栈

控制理论：能量起摆（Swing-up）| LQR 最优控制 | 状态机切换 | 相位检测
软件版本：ROS 2 Humble | Gazebo Fortress | Ubuntu 22.04 |Python 3.10+
工程实现：Python 3 | NumPy（Hamilton 矩阵法解CARE 方程）| 自定义 Controller 控制器

---

核心亮点

1. 双模态分层控制架构

非线性起摆 + 线性化稳定，各司其职：

• Swing-Up 阶段：基于机械能守恒的相位检测起摆，通过小车往复运动注入能量
• Balance 阶段：LQR 状态反馈，在倒立邻域内实现渐进稳定
• 滞环切换：进入条件 |θ−π|<25° ∧ |θ̇|<5 rad/s ∧ |x|<4 m，退出条件 |θ−π|>45°，避免抖动

2. 能量起摆 + 三区域控制策略

将摆杆运动空间划分为三个区域，动态调整控制律：

区域	角度范围	控制策略
下半区 |θ−π|>90°	能量注入	相位检测 phase = θ̇·cos(θ) 决定推力方向；能量过冲时采用固定制动力（6 N），防止比例制动导致的能量泄漏
过渡区 25°<|θ−π|<90°	渐进制动	按角速度分层制动（>8 / >4 / <4 rad/s 三级），平滑减速准备捕获
捕获区 |θ−π|<25°	LQR 接管 / 制动	满足切换条件后进入 LQR；不满足时采用 PD 角度纠正 + 阻尼 + 位置回中

3. Hamilton 矩阵法求解 Riccati 方程

不依赖 scipy.linalg.solve_continuous_are，纯 NumPy 实现 CARE 求解：

• 构造 2n×2n Hamilton 矩阵，提取稳定特征子空间
• 实时计算 LQR 增益，便于嵌入式移植，可适应同模型下参数修改
• 权重设计：Q = diag([100, 2000, 50, 100])，R = 1，角度权重最高优先保竖直
• LQR 反馈增益：K = [-10.00, -67.50, -5.24, -13.63]（标准形式 u = -K·x_err）

4. 自适应位置保护 + 多层安全约束

• 位置保护权重：能量低时 pos_weight=0.05（几乎不管位置），能量接近目标时渐增至 0.25，实现起摆效率与位置安全的折中
• 速度软墙：v_limit = 4.0 m/s，超速时施加与速度方向相反的阻尼力，按区域分级增益（6.0 / 4.0 / 10.0）
• 硬限位：|x| > 3.0 m 时强制 35 N 回中力，最后一道防线
• 力饱和：起摆阶段 ±45 N，LQR 阶段 ±20 N

5. 200 Hz 实时控制闭环

• 控制周期 0.005 s，从 /joint_states 读取状态 → 控制律计算 → /cart_effort_controller/commands 输出力矩
• 状态机切换 hold_count = 10（50 ms 确认），防止瞬时误判

---

控制架构

[joint_states] ──▶ 状态解析 (x, ẋ, θ, θ̇) ──▶ 状态机判断 ──▶ 三区域控制律
                                                      │
                                    ┌─────────────────┼─────────────────┐
                                    ▼                 ▼                 ▼
                              [下半区:能量注入]  [过渡区:渐进制动]  [捕获区:LQR/制动]
                                    │                 │                 │
                                    └─────────────────┴─────────────────┘
                                                      │
                                              [安全层:力饱和/软墙/硬限位]
                                                      │
                                              [cart_effort_controller/commands]

关键参数

参数	数值	说明
控制周期	0.005 s	200 Hz
小车质量 M	0.135 kg	刚体质量
摆杆质量 m	0.1 kg	均匀细杆
摆杆半长 l	0.2 m	质心到转轴
转动惯量 I	0.00533 kg·m²	I = (1/3)mL²
目标能量 E_r	0.3924 J	2mgl，倒立位置势能
能量增益 k_E	25.0	能量误差到力的映射
最大出力 u_max	45.0 N	起摆阶段力饱和
LQR 进入阈值	25° / 5 rad/s / 4 m	角度 / 角速度 / 位置同时满足，hold_count=10（50 ms 确认）
LQR 退出阈值	45°	滞环设计
速度软墙 v_limit	4.0 m/s	小车速度限制
硬限位 x_hard	±3.0 m	导轨边界保护
LQR 增益 K	[-10.00, -67.50, -5.24, -13.63]	标准形式 u = -K·x_err，代码中通过 K_lqr = -K 取反后 force = dot(K_lqr, x_err) 实现与标准式等价

---

快速运行

# 1. 编译项目
cd ~/your_ws   # 替换为你的工作空间路径
colcon build --packages-select pendulum_swing
source install/setup.bash

# 2. 启动 Gazebo 仿真（含倒立摆模型和控制器）
ros2 launch pendulum_swing pendulum_swing.launch.py

# 3. 观察日志输出
# [SWITCH] BALANCE: |th-pi|=12.3° thd=+1.234 x=+0.156

---

项目结构

pendulum_swing/
├── config/
│   └── pendulum_controllers.yaml     # ROS 2 Control 配置
├── launch/
│   └── pendulum_swing.launch.py      # Gazebo + 控制器管理器启动
├── urdf/
│   └── pendulum_swing.urdf.xacro     # 带 prismatic 关节的 URDF 模型
├── pendulum_swing/
│   └── swing_up_controller.py        # 主控制器
├── docs/
│   ├── DEBUG_LOG.md                  # 完整调试迭代记录（14 条）
│   └── assets/
│       └── swing_up_demo.gif         # 演示动画
└── README.md                         # 本文件

---

调试迭代记录

完整 14 条迭代记录见 docs/DEBUG_LOG.md

阶段一：能量起摆 —— 从"撞墙"到"可控注入"

现象	根因分析	解决方案
起摆即撞限位墙	能量注入时小车位置失控，位置保护力与能量注入力硬冲突，导致两者都失效	引入自适应位置保护权重：能量低时 pos_weight=0.05（几乎不管位置），能量接近目标时渐增至 0.25
能量过冲，摆杆甩飞转圈	过冲制动力与 E_err 成正比，误差越大制动力越大，反而把摆杆往回拉，造成能量泄漏和方向混乱	能量过冲时改用固定制动力 6 N（方向与 dir_sign 相反），严禁比例制动
起摆方向错误，能量越加越乱	相位检测逻辑不完善，在最低点死区（θ̇≈0）时方向判断抖动	增加死区处理：角速度 <0.3 rad/s 且在底部时给一个默认方向；否则保持 last_dir_sign
小车速度太快，摆杆下不来	能量注入时小车速度无约束，导致小车"追着"摆杆跑，摆杆相对运动被破坏	引入速度软墙：v_limit=4.0 m/s，超速时施加反向阻尼力，且按区域分级增益（下半区 6.0 / 过渡区 4.0 / 捕获区 10.0）

阶段二：三区域划分 —— 从"单一起摆律"到"分段控制"

现象	根因分析	解决方案
摆杆进入上半区后速度过高，直接飞过倒立点	单一起摆律在摆杆接近倒立点时仍在注入能量，没有减速机制	划分过渡区（25°~90°）：按角速度三级制动（>8 / >4 / <4 rad/s），渐进减速
过渡区小车位置失控	过渡区过度关注摆杆制动，忽略小车位置，导致小车滑向边界	过渡区增加位置救援逻辑：|x|>1.5 m 时优先回中，弱化摆杆制动
捕获区角速度仍过高，LQR 无法接管	捕获区（|θ−π|<25°）内角速度仍达 10+ rad/s，远超 LQR 线性化假设	捕获区增加分层制动：|θ̇|>12 强力制动 → |θ̇|>6 温和制动 → |θ̇|<6 PD 纠正

阶段三：LQR 稳定 —— 从"跨过去"到"稳得住"

现象	根因分析	解决方案
LQR 切换后摆杆在最高点"跨过去"，停不住	切换条件过严（原 15° / 3 rad/s），导致切换时角速度仍过高；且发现 K 值符号与 Gazebo 坐标系不匹配	放宽切换条件至 25° / 5 rad/s / 4 m；修正 K 值符号；增加 hold_count=10（50 ms 确认）
角度归一化错误，导致误差计算跳变	直接用 θ − π 计算误差，当 θ 从 2π 跳变到 0 时误差从 π 跳变到 −π	改用 arctan2(sin(θ−π), cos(θ−π))，自动处理 2π 跳变，误差映射到 [−π, π]
LQR 稳不住，小车匀速运动"托着"摆杆	小车位置无约束，LQR 为保摆杆竖直让小车持续漂移；Q 矩阵中位置权重过低	最终调整到 Q = diag([100, 2000, 50, 100])，位置权重 100 保证小车回中；硬限位 ±3.0 m 兜底

阶段四：物理参数与 LQR 的耦合调试

现象	根因分析	解决方案
修改摆杆质量后 LQR 完全失效	LQR 的 A/B 矩阵直接依赖 m, l, I，参数变化后特征值偏移，原 K 值不再最优	在 __init__ 中在线计算 K，而非硬编码；使用 Hamilton 矩阵法实时求解 CARE
Q 权重调整后系统振荡或收敛过慢	Q的角度权重过高时 LQR 过于激进，导致小车剧烈抖动；过低时摆杆收敛慢	最终调到 Q = diag([100, 2000, 50, 100]) 的测试，确定角度权重 2000 为保竖直的合适值

---

关键设计决策总结

设计点	为什么这样做	如果反过来会怎样
三区域划分	单一起摆律无法同时满足"快速注入能量"和"精确捕获倒立点"的矛盾需求	不分区：下半区能量不足，上半区过冲甩飞
能量过冲固定制动	比例制动会随误差增大而反向拉扯，导致能量泄漏	比例制动：摆杆被拉回，永远无法达到倒立能量
速度软墙（非硬截断）	硬截断速度会导致力矩突变，引发振荡；软墙是渐进阻尼	硬限速：小车速度跳变，摆杆受冲击失稳
hold_count 切换确认	摆杆穿过倒立点时瞬时满足条件，但并未稳定	即时切换：LQR 在摆杆仍高速时接管，瞬间失稳
在线 Hamilton 求解	物理参数调整后 K 值必须同步更新，硬编码会失效	硬编码 K：每次调参需手动重算，易出错，采用scipy计算，无法移植到嵌入式设备

---

作者

杨国桂
📧 邮箱：puplc@outlook.com 📝 CSDN：https://blog.csdn.net/weixin_51365313
💻 GitHub：https://github.com/7type/Inverted-pendulum

---