diff --git a/docs/classical_control/cartpole_swingup.md b/docs/classical_control/cartpole_swingup.md
new file mode 100644
index 0000000000..4861167e66
--- /dev/null
+++ b/docs/classical_control/cartpole_swingup.md
@@ -0,0 +1,194 @@
+# 倒立摆能量摆起与 LQR 平衡实验
+
+仓库里已有的 `cartpole_balance` 只演示「摆杆初始倾斜约 0.2 rad 时用 LQR 拉回竖直」。
+本实验处理更难的起点:摆杆从**自然下垂**(`theta = π`)出发,先用能量控制把能量泵到
+竖直向上的目标能量,接近平衡点时再切换 LQR 保持不倒,并把小车拉回原点。
+
+## 一、实验目的与环境说明
+
+### 1.1 实验目的
+
+1. 掌握倒立摆非线性动力学方程,以及用 RK4 积分仿真的方法;
+2. 理解能量控制的思想:通过小车加速度改变摆杆机械能,把摆杆“荡”到竖直附近;
+3. 掌握在竖直平衡点线性化、离散 Riccati 迭代求 LQR 增益的方法;
+4. 设计「能量摆起 + LQR 平衡」的模式切换逻辑,理解切换阈值对成功率和稳定性的影响;
+5. 用离线仿真复现并量化摆起时间、稳定时间和控制力。
+
+### 1.2 实验环境
+
+| 项目 | 配置 |
+| ---- | ---- |
+| 操作系统 | Ubuntu 20.04(ROS 端);Windows/Linux(离线仿真) |
+| ROS 发行版 | ROS Noetic(Python 3) |
+| 依赖 | rospy、std_msgs、python3-numpy |
+| 离线仿真依赖 | Python 3、numpy、matplotlib |
+| 被控对象 | 需要外部提供 `/cartpole/state` 的倒立摆仿真器或实车 |
+
+功能包目录结构:
+
+```text
+cartpole_swingup/
+├── main.py # ROS 节点入口:状态订阅、控制力发布
+├── cartpole_model.py # 动力学、能量、LQR、双模控制器(纯 Python)
+├── offline_sim.py # 离线仿真与出图
+├── launch/main.launch # roslaunch 入口
+├── config/params.yaml
+├── package.xml
+├── CMakeLists.txt
+└── README.md
+```
+
+## 二、实验原理
+
+### 2.1 动力学模型
+
+记小车质量 $m_c$、摆杆质量 $m_p$、摆杆半长 $l$,摆角 $\theta$ 从竖直向上方向量起。
+小车位置 $x$、控制力 $F$,则
+
+$$
+(m_c + m_p)\ddot{x} + m_p l \ddot{\theta}\cos\theta - m_p l \dot{\theta}^2\sin\theta = F
+$$
+
+$$
+m_p l^2 \ddot{\theta} + m_p l \ddot{x}\cos\theta - m_p g l \sin\theta = 0
+$$
+
+每个仿真步求解这个二元一次方程组得到 $\ddot{x}$、$\ddot{\theta}$,再用 RK4 积分。
+参数取 $m_c = 1.0\ \mathrm{kg}$、$m_p = 0.1\ \mathrm{kg}$、$l = 0.5\ \mathrm{m}$、$g = 9.81\ \mathrm{m/s^2}$。
+
+### 2.2 能量控制
+
+相对自然下垂位置的总机械能为
+
+$$
+E = \frac{1}{2}(m_c + m_p)\dot{x}^2 + m_p l \dot{x}\dot{\theta}\cos\theta
+ + \frac{1}{2}m_p l^2 \dot{\theta}^2 + m_p g l(\cos\theta + 1)
+$$
+
+竖直静止时的目标能量为 $E^* = 2 m_p g l$。对时间求导并代入动力学可得
+
+$$
+\frac{dE}{dt} = -m_p l\, \dot{\theta}\, \ddot{x} \cos\theta
+$$
+
+因此沿着 $-\mathrm{sign}(\dot{\theta}\cos\theta)$ 的方向给小车加速,就能稳定地增加能量。
+实际使用力(近似与加速度同号)作为控制量:
+
+$$
+F = -k_E (E^* - E)\,\mathrm{sign}(\dot{\theta}\cos\theta) - k_x x - k_v \dot{x}
+$$
+
+后两项让小车不要越跑越远。
+
+### 2.3 LQR 平衡
+
+在 $\theta = 0$、$\dot{\theta}=0$ 附近线性化,状态取 $[x, \dot{x}, \theta, \dot{\theta}]$:
+
+$$
+A = \begin{bmatrix}
+0 & 1 & 0 & 0\\
+0 & 0 & -\frac{m_p g}{m_c} & 0\\
+0 & 0 & 0 & 1\\
+0 & 0 & \frac{(m_c+m_p)g}{m_c l} & 0
+\end{bmatrix},\qquad
+B = \begin{bmatrix}
+0\\ \frac{1}{m_c}\\ 0\\ -\frac{1}{m_c l}
+\end{bmatrix}
+$$
+
+对 $(A, B)$ 离散化后迭代求解离散 Riccati 方程,得到增益 $K$,控制力为 $F = -K\mathbf{x}$。
+本实验取 $Q = \mathrm{diag}(1, 0.1, 10, 0.1)$、$R = 1$。
+
+### 2.4 模式切换
+
+```mermaid
+stateDiagram-v2
+ [*] --> SWINGUP
+ SWINGUP --> BALANCE: |theta| < 0.4 且 |theta_dot| < 2.0
+ BALANCE --> SWINGUP: |theta| > 0.7
+```
+
+切换窗口不能太窄:能量控制会让摆杆以较高角速度经过竖直位置,窗口过窄时
+(例如 0.25 rad / 1.0 rad/s)几乎抓不住;放宽到 0.4 rad / 2.0 rad/s 后
+LQR 能可靠接管。
+
+## 三、运行步骤
+
+### 3.1 ROS 节点
+
+```bash
+mkdir -p ~/catkin_ws/src
+cp -r <本仓库>/src/classical_control/cartpole_swingup ~/catkin_ws/src/
+chmod +x ~/catkin_ws/src/cartpole_swingup/main.py
+cd ~/catkin_ws && catkin_make
+source devel/setup.bash
+
+# 先启动能发布 /cartpole/state、订阅 /cartpole/force 的仿真器
+roslaunch cartpole_swingup main.launch
+```
+
+节点启动时打印 LQR 增益,摆起过程中打印 `SWINGUP -> BALANCE` 模式切换日志。
+
+### 3.2 离线仿真
+
+```bash
+cd <本仓库>
+python -m pip install numpy matplotlib
+python src/classical_control/cartpole_swingup/offline_sim.py
+```
+
+脚本用 0.002 s 步长的 RK4 积分、50 Hz 控制器,复现同一套控制逻辑,
+并额外跑一组「只用 LQR」的对照。
+
+## 四、实验结果
+
+初始状态:$x=0$、$\dot{x}=0$、$\theta=\pi-0.1$、$\dot{\theta}=0$。
+
+### 4.1 摆起与平衡全过程
+
+
+
+四个子图分别为摆角、总能量、小车位置和控制力,绿色区域为 LQR 平衡阶段。
+能量在前 5 s 从 0 泵到目标值附近,摆杆多次经过竖直方向;5.26 s 切入 LQR 后
+角度迅速收敛,小车从约 −0.86 m 缓慢回到原点。
+
+### 4.2 相图
+
+
+
+橙色为摆起阶段:轨迹从下垂点出发,能量逐渐增加,相轨迹一圈圈抬高;
+绿色为 LQR 阶段:轨迹收敛到 $(0, 0)$ 平衡点。
+
+### 4.3 与“只用 LQR”的对比
+
+
+
+LQR 是在竖直平衡点附近线性化的。从下垂位置出发时,它虽然偶尔能把摆杆甩到竖直附近
+($\cos\theta$ 短暂接近 1),但无法稳定,之后继续在 −1~1 之间大幅摆动;
+而能量摆起 + LQR 在约 5.5 s 后就稳定保持在 $\cos\theta \approx 1$。
+这说明**大范围摆起需要非线性能量控制**,LQR 只负责平衡点附近的镇定。
+
+### 4.4 指标
+
+| 指标 | 数值 |
+| ---- | ---- |
+| 切入 LQR 时间 | 5.26 s |
+| 角度稳定时间(|θ|<0.05 rad) | 7.00 s |
+| 角度与小车都稳定时间 | 10.10 s |
+| 最终角度误差 | < 0.001° |
+| 最终小车位置 | ≈ 0 m |
+| 小车最大行程 | 0.86 m |
+| 最大控制力 | 8.73 N |
+
+## 五、讨论与限制
+
+1. 能量控制没有考虑执行器延迟和摩擦,实机上需要增加速度反馈或降低 $k_E$;
+2. 切换阈值是手工整定的,窗口过窄会“抓不住”,过宽会让 LQR 在非线性较强时接管;
+3. 仿真为理想模型,不含传感器噪声、电机死区和轨道限位;
+4. 本机为 Windows,没有 ROS 与 Gazebo,**ROS 端实际运行未验证**,
+ 需要在 ROS Noetic + 倒立摆仿真器/实车上按 3.1 节复验。
+
+## 说明
+
+本文档与配套代码在整理过程中使用了 AI 助手(DeepSeek)辅助,实验参数、运行结果
+和图表均由作者实际运行核对;作者对本文内容负全部责任。
diff --git a/docs/img/classical_control/cartpole_swingup/lqr_only_comparison.png b/docs/img/classical_control/cartpole_swingup/lqr_only_comparison.png
new file mode 100644
index 0000000000..cade2e94ae
Binary files /dev/null and b/docs/img/classical_control/cartpole_swingup/lqr_only_comparison.png differ
diff --git a/docs/img/classical_control/cartpole_swingup/phase_portrait.png b/docs/img/classical_control/cartpole_swingup/phase_portrait.png
new file mode 100644
index 0000000000..36df9e958a
Binary files /dev/null and b/docs/img/classical_control/cartpole_swingup/phase_portrait.png differ
diff --git a/docs/img/classical_control/cartpole_swingup/swingup_response.png b/docs/img/classical_control/cartpole_swingup/swingup_response.png
new file mode 100644
index 0000000000..0d5bccf049
Binary files /dev/null and b/docs/img/classical_control/cartpole_swingup/swingup_response.png differ
diff --git a/docs/index.md b/docs/index.md
index c68c4d5ed7..c6516c6404 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -695,6 +695,7 @@ ___
## 7. 经典控制
* [倒立摆平衡小车 (cart-pole, LQR 最优控制)](./classical_control/cartpole_balance.md) —— 用 LQR 让倒立摆从倾斜被拉回竖直保持不倒,演示 ROS 发布/订阅闭环与最优控制
+* [倒立摆能量摆起与 LQR 平衡](./classical_control/cartpole_swingup.md) —— 摆杆从下垂位置出发,能量控制泵能摆起、接近竖直后切换 LQR 保持平衡,并对比“只用 LQR 无法摆起”
* [滑模控制(SMC):二阶系统的鲁棒轨迹跟踪](./classical_control/sliding_mode_control.md) —— 边界层切换律,到达条件 K > D_max;常值扰动下 SMC 平均误差 0.012,PD 0.255
diff --git a/mkdocs.yml b/mkdocs.yml
index 76308e30f4..b0722465d0 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -606,6 +606,7 @@ nav:
- Stanley 前视路径跟踪(含曲率前馈): 'motion_planning/stanley_tracking.md'
- 经典控制:
- 倒立摆平衡小车 (cart-pole, LQR): './classical_control/cartpole_balance.md'
+ - 倒立摆能量摆起与 LQR 平衡: './classical_control/cartpole_swingup.md'
- 滑模控制(SMC):二阶系统的鲁棒轨迹跟踪: './classical_control/sliding_mode_control.md'
- 反应式导航与覆盖:
- 人工势场(APF):梯度下降式避障与局部极小逃逸: './reactive_navigation/potential_field_avoidance.md'
diff --git a/src/classical_control/cartpole_swingup/CMakeLists.txt b/src/classical_control/cartpole_swingup/CMakeLists.txt
new file mode 100644
index 0000000000..bbaf82824b
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/CMakeLists.txt
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(cartpole_swingup)
+
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+)
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+ main.py
+ cartpole_model.py
+ offline_sim.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch config
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
diff --git a/src/classical_control/cartpole_swingup/README.md b/src/classical_control/cartpole_swingup/README.md
new file mode 100644
index 0000000000..e65299c6ba
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/README.md
@@ -0,0 +1,74 @@
+# cartpole_swingup — 倒立摆能量摆起 + LQR 平衡
+
+摆杆从自然下垂位置出发:先用**能量控制**给系统泵入能量,把摆杆摆到竖直附近;
+进入设定窗口后切换 **LQR** 保持平衡,并把小车拉回原点。订阅
+`/cartpole/state`,发布 `/cartpole/force`。配套教程见
+[docs/classical_control/cartpole_swingup.md](../../docs/classical_control/cartpole_swingup.md)。
+
+## 运行环境
+
+| 项目 | 配置 |
+| ---- | ---- |
+| 操作系统 | Ubuntu 20.04(ROS 节点) |
+| ROS | ROS Noetic(Python 3) |
+| 依赖 | rospy、std_msgs、python3-numpy |
+| 离线仿真 | Windows / Linux,Python 3 + numpy + matplotlib(不需要 ROS) |
+| 被控对象 | 需要外部提供 `/cartpole/state` 的仿真器或实车驱动 |
+
+## 运行步骤
+
+### 方式一:ROS 节点
+
+```bash
+# 1. 拷贝到工作空间并编译
+mkdir -p ~/catkin_ws/src
+cp -r <本仓库>/src/classical_control/cartpole_swingup ~/catkin_ws/src/
+chmod +x ~/catkin_ws/src/cartpole_swingup/main.py
+cd ~/catkin_ws && catkin_make
+source devel/setup.bash
+
+# 2. 启动一个能发布 /cartpole/state、订阅 /cartpole/force 的倒立摆仿真器
+# 例如仓库里的 cartpole_balance 模块中的 cartpole_sim.py
+
+# 3. 启动摆起节点
+roslaunch cartpole_swingup main.launch
+```
+
+节点启动后会打印 LQR 增益,摆起成功时终端输出模式切换日志
+`SWINGUP -> BALANCE`。
+
+### 方式二:离线仿真(不需要 ROS)
+
+```bash
+cd <本仓库>
+python -m pip install numpy matplotlib
+python src/classical_control/cartpole_swingup/offline_sim.py
+```
+
+脚本用 RK4 积分非线性动力学,复现同样的控制器,并给出「只用 LQR」的对照组。
+
+## 参数
+
+| 参数 | 默认值 | 说明 |
+| ---- | ---- | ---- |
+| `~k_energy` | 8.0 | 能量误差到控制力的增益(N/J) |
+| `~k_cart_p` / `~k_cart_d` | 0.8 / 0.6 | 摆起阶段小车位置/速度反馈 |
+| `~switch_angle` | 0.4 | 切入 LQR 的角度阈值(rad) |
+| `~switch_rate` | 2.0 | 切入 LQR 的角速度阈值(rad/s) |
+| `~fall_angle` | 0.7 | 超过该角度退回能量摆起(rad) |
+| `~control_rate` | 50.0 | 控制频率(Hz) |
+| `~state_timeout` | 0.5 | 状态超时(s),超时后控制力归零 |
+
+## 话题接口
+
+| 方向 | 话题 | 类型 | 说明 |
+| ---- | ---- | ---- | ---- |
+| 订阅 | `/cartpole/state` | `std_msgs/Float64MultiArray` | `[x, x_dot, theta, theta_dot]`,theta 从竖直向上方向量起(rad) |
+| 发布 | `/cartpole/force` | `std_msgs/Float64` | 小车水平力(N),向右为正 |
+
+## 预期效果
+
+摆杆从下垂附近开始,约 5 s 摆到竖直附近并切换 LQR,约 10 s 后角度误差小于 0.01°、
+小车回到原点附近;最大控制力约 8.7 N,小车最大行程约 0.86 m。
+只用 LQR(按竖直平衡点线性化)从下垂位置出发时会在 −1~1 之间持续大幅摆动,
+无法稳定在竖直位置。
diff --git a/src/classical_control/cartpole_swingup/cartpole_model.py b/src/classical_control/cartpole_swingup/cartpole_model.py
new file mode 100644
index 0000000000..4988d0157f
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/cartpole_model.py
@@ -0,0 +1,190 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""倒立摆(cart-pole)模型、能量摆起与 LQR 平衡控制(纯 Python + numpy)。
+
+坐标与符号约定
+--------------
+- 状态 ``[x, x_dot, theta, theta_dot]``;
+- ``x`` 为小车位置(m),向右为正;
+- ``theta`` 为摆杆与**竖直向上方向**的夹角(rad),逆时针为正;
+ 因此 ``theta = 0`` 是竖直向上(平衡点),``theta = ±pi`` 是自然下垂;
+- ``force`` 为作用在小车上的水平力(N),向右为正。
+
+本模块提供:
+1. ``cartpole_dynamics``:非线性动力学(供 RK4 积分);
+2. ``total_energy``:相对下垂位置的总机械能;
+3. ``lqr_gain``:用离散 Riccati 迭代求 LQR 反馈增益;
+4. ``CartPoleController``:能量摆起 + LQR 平衡的双模控制器。
+"""
+from __future__ import print_function
+
+import math
+
+import numpy as np
+
+# 默认物理参数(与仓库 cartpole_balance 一致)
+DEFAULT_PARAMS = {
+ 'mc': 1.0, # 小车质量 kg
+ 'mp': 0.1, # 摆杆质量 kg
+ 'l': 0.5, # 摆杆半长 m
+ 'g': 9.81, # 重力加速度
+ 'force_limit': 10.0,
+}
+
+MODE_SWINGUP = 'SWINGUP'
+MODE_BALANCE = 'BALANCE'
+
+
+def wrap_angle(theta):
+ """把角度归一化到 [-pi, pi]。"""
+ return math.atan2(math.sin(theta), math.cos(theta))
+
+
+def cartpole_dynamics(state, force, params=None):
+ """返回状态导数 [x_dot, x_ddot, theta_dot, theta_ddot]。"""
+ p = dict(DEFAULT_PARAMS)
+ if params:
+ p.update(params)
+ mc, mp, l, g = p['mc'], p['mp'], p['l'], p['g']
+
+ x, x_dot, theta, theta_dot = state
+ total_mass = mc + mp
+ cos_t = math.cos(theta)
+ sin_t = math.sin(theta)
+
+ # 线性方程组:
+ # [M, m*l*cos] [x_ddot] [F + m*l*theta_dot^2*sin]
+ # [m*l*cos, m*l^2 ] [theta_ddot] = [m*g*l*sin]
+ a11 = total_mass
+ a12 = mp * l * cos_t
+ a21 = mp * l * cos_t
+ a22 = mp * l * l
+ b1 = force + mp * l * theta_dot * theta_dot * sin_t
+ b2 = mp * g * l * sin_t
+
+ det = a11 * a22 - a12 * a21
+ if abs(det) < 1e-9:
+ det = 1e-9
+ x_ddot = (b1 * a22 - a12 * b2) / det
+ theta_ddot = (a11 * b2 - b1 * a21) / det
+ return [x_dot, x_ddot, theta_dot, theta_ddot]
+
+
+def total_energy(state, params=None):
+ """相对自然下垂位置(theta = ±pi,静止)的总机械能(J)。"""
+ p = dict(DEFAULT_PARAMS)
+ if params:
+ p.update(params)
+ mp, l, g = p['mp'], p['l'], p['g']
+ x_dot, theta, theta_dot = state[1], state[2], state[3]
+ kinetic = 0.5 * (p['mc'] + mp) * x_dot * x_dot \
+ + mp * l * x_dot * theta_dot * math.cos(theta) \
+ + 0.5 * mp * l * l * theta_dot * theta_dot
+ potential = mp * g * l * (math.cos(theta) + 1.0)
+ return kinetic + potential
+
+
+def upright_energy(params=None):
+ """摆杆静止在竖直向上时的能量,即摆起目标能量。"""
+ p = dict(DEFAULT_PARAMS)
+ if params:
+ p.update(params)
+ return 2.0 * p['mp'] * p['g'] * p['l']
+
+
+def linearize_upright(params=None):
+ """在竖直向上平衡点线性化,返回连续系统矩阵 A、B。"""
+ p = dict(DEFAULT_PARAMS)
+ if params:
+ p.update(params)
+ mc, mp, l, g = p['mc'], p['mp'], p['l'], p['g']
+ total_mass = mc + mp
+ A = np.array([
+ [0.0, 1.0, 0.0, 0.0],
+ [0.0, 0.0, -mp * g / mc, 0.0],
+ [0.0, 0.0, 0.0, 1.0],
+ [0.0, 0.0, total_mass * g / (mc * l), 0.0],
+ ])
+ B = np.array([[0.0], [1.0 / mc], [0.0], [-1.0 / (mc * l)]])
+ return A, B
+
+
+def lqr_gain(params=None, dt=0.02, q_diag=(1.0, 0.1, 10.0, 0.1), r=1.0,
+ iterations=20000, tol=1e-12):
+ """离散 Riccati 迭代求 LQR 增益 K(使 u = -K x)。"""
+ A, B = linearize_upright(params)
+ Ad = np.eye(4) + A * dt
+ Bd = B * dt
+ Q = np.diag(q_diag)
+ R = np.array([[r]])
+ P = Q.copy()
+ for _ in range(iterations):
+ middle = np.linalg.inv(R + Bd.T.dot(P).dot(Bd))
+ P_next = Ad.T.dot(P).dot(Ad) - Ad.T.dot(P).dot(Bd).dot(middle).dot(Bd.T).dot(P).dot(Ad) + Q
+ if np.max(np.abs(P_next - P)) < tol:
+ P = P_next
+ break
+ P = P_next
+ K = np.linalg.inv(R + Bd.T.dot(P).dot(Bd)).dot(Bd.T).dot(P).dot(Ad)
+ return K.flatten()
+
+
+class CartPoleController(object):
+ """能量摆起 + LQR 平衡的双模控制器。"""
+
+ def __init__(self, params=None, k_energy=8.0, k_cart_p=0.8, k_cart_d=0.6,
+ switch_angle=0.4, switch_rate=2.0, fall_angle=0.7,
+ balance_dt=0.02, allow_swingup=True):
+ self.params = dict(DEFAULT_PARAMS)
+ if params:
+ self.params.update(params)
+ self.force_limit = self.params['force_limit']
+ self.allow_swingup = bool(allow_swingup)
+
+ self.k_energy = float(k_energy)
+ self.k_cart_p = float(k_cart_p)
+ self.k_cart_d = float(k_cart_d)
+ self.switch_angle = float(switch_angle)
+ self.switch_rate = float(switch_rate)
+ self.fall_angle = float(fall_angle)
+
+ self.k_lqr = lqr_gain(self.params, dt=balance_dt)
+ self.e_target = upright_energy(self.params)
+ self.mode = MODE_SWINGUP if self.allow_swingup else MODE_BALANCE
+
+ def _clamp(self, force):
+ return max(-self.force_limit, min(self.force_limit, force))
+
+ def balance_force(self, state):
+ return float(-self.k_lqr.dot(np.asarray(state, dtype=float)))
+
+ def swingup_force(self, state):
+ x, x_dot, theta, theta_dot = state
+ energy = total_energy(state, self.params)
+ deficit = self.e_target - energy
+ # dE/dt = -m*l*theta_dot*x_ddot*cos(theta)
+ # 因此力取 -(theta_dot*cos(theta)) 的符号方向可增加能量
+ direction = 1.0 if theta_dot * math.cos(theta) >= 0.0 else -1.0
+ force = -self.k_energy * deficit * direction
+ force += -self.k_cart_p * x - self.k_cart_d * x_dot
+ return self._clamp(force)
+
+ def update(self, state, dt=None):
+ """根据当前状态返回控制力(N),并更新模式。"""
+ theta = wrap_angle(state[2])
+ theta_dot = state[3]
+
+ if not self.allow_swingup:
+ return self._clamp(self.balance_force(
+ [state[0], state[1], theta, theta_dot]))
+
+ if self.mode == MODE_SWINGUP:
+ if abs(theta) < self.switch_angle and abs(theta_dot) < self.switch_rate:
+ self.mode = MODE_BALANCE
+ else:
+ if abs(theta) > self.fall_angle:
+ self.mode = MODE_SWINGUP
+
+ if self.mode == MODE_BALANCE:
+ return self._clamp(self.balance_force([state[0], state[1], theta, theta_dot]))
+ return self.swingup_force([state[0], state[1], theta, theta_dot])
diff --git a/src/classical_control/cartpole_swingup/config/params.yaml b/src/classical_control/cartpole_swingup/config/params.yaml
new file mode 100644
index 0000000000..a2f945cfa4
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/config/params.yaml
@@ -0,0 +1,9 @@
+# 摆起与平衡参数(launch 中作为节点私有参数加载)
+k_energy: 8.0 # 能量误差 -> 控制力的增益(N/J)
+k_cart_p: 0.8 # 摆起阶段小车位置的 P 项(N/m)
+k_cart_d: 0.6 # 摆起阶段小车速度的 D 项(N·s/m)
+switch_angle: 0.4 # 切入 LQR 的角度阈值(rad)
+switch_rate: 2.0 # 切入 LQR 的角速度阈值(rad/s)
+fall_angle: 0.7 # 超过该角度退回能量摆起(rad)
+control_rate: 50.0 # 控制频率(Hz)
+state_timeout: 0.5 # 状态超时(s),超时后控制力归零
diff --git a/src/classical_control/cartpole_swingup/launch/main.launch b/src/classical_control/cartpole_swingup/launch/main.launch
new file mode 100644
index 0000000000..1efef8214d
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/launch/main.launch
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/classical_control/cartpole_swingup/main.py b/src/classical_control/cartpole_swingup/main.py
new file mode 100644
index 0000000000..120c4b7744
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/main.py
@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""倒立摆能量摆起 + LQR 平衡 ROS 节点(模块入口 main.py)。
+
+订阅 `/cartpole/state`(std_msgs/Float64MultiArray,依次为
+[x, x_dot, theta, theta_dot],theta 从竖直向上方向量起),
+按「能量摆起 → 接近竖直后切换 LQR 平衡」输出控制力到
+`/cartpole/force`(std_msgs/Float64,N,向右为正)。
+
+运行:
+ rosrun cartpole_swingup main.py
+ roslaunch cartpole_swingup main.launch
+"""
+from __future__ import print_function
+
+import rospy
+from std_msgs.msg import Float64, Float64MultiArray
+
+from cartpole_model import MODE_BALANCE, MODE_SWINGUP, CartPoleController
+
+
+class CartPoleSwingUpNode(object):
+
+ def __init__(self):
+ self.state_topic = rospy.get_param('~state_topic', '/cartpole/state')
+ self.force_topic = rospy.get_param('~force_topic', '/cartpole/force')
+ self.state_timeout = rospy.get_param('~state_timeout', 0.5)
+
+ self.controller = CartPoleController(
+ k_energy=rospy.get_param('~k_energy', 8.0),
+ k_cart_p=rospy.get_param('~k_cart_p', 0.8),
+ k_cart_d=rospy.get_param('~k_cart_d', 0.6),
+ switch_angle=rospy.get_param('~switch_angle', 0.4),
+ switch_rate=rospy.get_param('~switch_rate', 2.0),
+ fall_angle=rospy.get_param('~fall_angle', 0.7),
+ )
+
+ self.state = None
+ self.last_state_time = None
+ self.last_mode = None
+
+ self.pub = rospy.Publisher(self.force_topic, Float64, queue_size=10)
+ rospy.Subscriber(self.state_topic, Float64MultiArray, self.on_state)
+ rospy.loginfo('倒立摆摆起节点已启动:state=%s -> force=%s(LQR 增益 %s)',
+ self.state_topic, self.force_topic,
+ [round(v, 3) for v in self.controller.k_lqr])
+
+ def on_state(self, msg):
+ if len(msg.data) < 4:
+ return
+ self.state = [float(v) for v in msg.data[:4]]
+ self.last_state_time = rospy.Time.now()
+
+ def publish_force(self, force):
+ self.pub.publish(Float64(data=float(force)))
+
+ def run(self):
+ rate = rospy.Rate(rospy.get_param('~control_rate', 50.0))
+ while not rospy.is_shutdown():
+ now = rospy.Time.now()
+ if self.state is None or self.last_state_time is None:
+ self.publish_force(0.0)
+ rate.sleep()
+ continue
+
+ if (now - self.last_state_time).to_sec() > self.state_timeout:
+ self.publish_force(0.0)
+ rospy.logwarn_throttle(2.0, '超过 %.1f 秒没有收到倒立摆状态,控制力已归零',
+ self.state_timeout)
+ rate.sleep()
+ continue
+
+ force = self.controller.update(self.state)
+ self.publish_force(force)
+
+ if self.controller.mode != self.last_mode:
+ rospy.loginfo('控制模式切换:%s', self.controller.mode)
+ self.last_mode = self.controller.mode
+
+ rate.sleep()
+
+ self.publish_force(0.0)
+
+
+def main():
+ rospy.init_node('cartpole_swingup')
+ node = CartPoleSwingUpNode()
+ try:
+ node.run()
+ except rospy.ROSInterruptException:
+ pass
+ finally:
+ node.publish_force(0.0)
+ rospy.loginfo('节点退出,控制力已归零')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/classical_control/cartpole_swingup/offline_sim.py b/src/classical_control/cartpole_swingup/offline_sim.py
new file mode 100644
index 0000000000..94110d4bfb
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/offline_sim.py
@@ -0,0 +1,231 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""倒立摆能量摆起 + LQR 平衡的离线仿真与出图(不需要 ROS / Gazebo)。
+
+用法(在仓库根目录执行):
+ python src/classical_control/cartpole_swingup/offline_sim.py
+
+仿真内容:
+1. 摆杆从自然下垂附近出发,能量控制器把能量泵到竖直向上的目标能量,
+ 接近竖直时切换到 LQR 保持平衡;
+2. 对照组:只有 LQR(按竖直平衡点线性化)从下垂位置出发,验证它无法摆起;
+3. 输出角度、能量、小车位置、控制力曲线与相图。
+
+输出:
+ docs/img/classical_control/cartpole_swingup/swingup_response.png
+ docs/img/classical_control/cartpole_swingup/phase_portrait.png
+ docs/img/classical_control/cartpole_swingup/lqr_only_comparison.png
+"""
+from __future__ import print_function
+
+import json
+import math
+import os
+import sys
+
+import numpy as np
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+REPO_ROOT = os.path.abspath(os.path.join(HERE, '..', '..'))
+sys.path.insert(0, HERE)
+
+from cartpole_model import ( # noqa: E402
+ MODE_BALANCE, MODE_SWINGUP, CartPoleController, cartpole_dynamics,
+ total_energy, upright_energy, wrap_angle,
+)
+
+OUTPUT_DIR = os.path.join(REPO_ROOT, 'docs', 'img', 'classical_control', 'cartpole_swingup')
+
+DT = 0.002 # 积分步长
+CONTROL_DT = 0.02 # 控制周期(50 Hz)
+DURATION = 20.0
+INITIAL_STATE = [0.0, 0.0, math.pi - 0.1, 0.0]
+
+
+def rk4_step(state, force, dt):
+ k1 = cartpole_dynamics(state, force)
+ k2 = cartpole_dynamics([s + 0.5 * dt * d for s, d in zip(state, k1)], force)
+ k3 = cartpole_dynamics([s + 0.5 * dt * d for s, d in zip(state, k2)], force)
+ k4 = cartpole_dynamics([s + dt * d for s, d in zip(state, k3)], force)
+ return [s + dt / 6.0 * (a + 2 * b + 2 * c + d)
+ for s, a, b, c, d in zip(state, k1, k2, k3, k4)]
+
+
+def simulate(use_swingup=True, duration=DURATION):
+ controller = CartPoleController(
+ switch_angle=0.4, switch_rate=2.0, fall_angle=0.7,
+ allow_swingup=use_swingup)
+
+ state = list(INITIAL_STATE)
+ t = 0.0
+ control_timer = 0.0
+ force = 0.0
+
+ times = [t]
+ states = [list(state)]
+ forces = [force]
+ modes = [controller.mode]
+
+ switch_time = None
+ settle_time = None
+ theta_settle_time = None
+
+ while t < duration:
+ if control_timer <= 1e-12:
+ force = controller.update(state, CONTROL_DT)
+ control_timer += CONTROL_DT
+ state = rk4_step(state, force, DT)
+ t += DT
+ control_timer -= DT
+
+ times.append(t)
+ states.append(list(state))
+ forces.append(force)
+ modes.append(controller.mode)
+
+ if use_swingup and switch_time is None and controller.mode == MODE_BALANCE:
+ switch_time = t
+ if (use_swingup and switch_time is not None and theta_settle_time is None
+ and abs(wrap_angle(state[2])) < 0.05 and abs(state[3]) < 0.2):
+ theta_settle_time = t
+ if (use_swingup and switch_time is not None and settle_time is None
+ and abs(wrap_angle(state[2])) < 0.05 and abs(state[3]) < 0.2
+ and abs(state[1]) < 0.1 and abs(state[0]) < 0.1):
+ settle_time = t
+
+ return {
+ 't': np.array(times),
+ 'state': np.array(states),
+ 'force': np.array(forces),
+ 'mode': np.array(modes),
+ 'switch_time': switch_time,
+ 'theta_settle_time': theta_settle_time,
+ 'settle_time': settle_time,
+ }
+
+
+def metrics(data):
+ state = data['state']
+ theta = np.array([wrap_angle(v) for v in state[:, 2]])
+ energy = np.array([total_energy(s) for s in state])
+ forces = data['force']
+ return {
+ 'swing_up_time_s': data['switch_time'],
+ 'theta_settle_time_s': data['theta_settle_time'],
+ 'settle_time_s': data['settle_time'],
+ 'final_angle_deg': float(math.degrees(theta[-1])),
+ 'final_cart_position_m': float(state[-1, 0]),
+ 'max_cart_travel_m': float(np.max(np.abs(state[:, 0]))),
+ 'max_force_N': float(np.max(np.abs(forces))),
+ 'energy_target_J': float(upright_energy()),
+ 'final_energy_J': float(energy[-1]),
+ }
+
+
+def plot_swingup(data):
+ import matplotlib
+ matplotlib.use('Agg')
+ import matplotlib.pyplot as plt
+
+ os.makedirs(OUTPUT_DIR, exist_ok=True)
+
+ t = data['t']
+ state = data['state']
+ theta = np.array([wrap_angle(v) for v in state[:, 2]])
+ energy = np.array([total_energy(s) for s in state])
+ force = data['force']
+ balance = data['mode'] == MODE_BALANCE
+
+ fig, axes = plt.subplots(2, 2, figsize=(10, 6.5), sharex=True)
+ panels = [
+ (axes[0, 0], theta, 'theta (rad)', 'Pendulum angle (0 = upright)'),
+ (axes[0, 1], energy, 'energy (J)', 'Total mechanical energy'),
+ (axes[1, 0], state[:, 0], 'x (m)', 'Cart position'),
+ (axes[1, 1], force, 'force (N)', 'Control force'),
+ ]
+ for ax, values, ylabel, title in panels:
+ ax.plot(t, values, color='#1f77b4', linewidth=1.2)
+ ax.axvspan(t[balance][0] if balance.any() else t[-1], t[-1],
+ color='#2ca02c', alpha=0.12)
+ ax.set_ylabel(ylabel)
+ ax.set_title(title, fontsize=9)
+ ax.grid(True, alpha=0.3)
+ axes[0, 1].axhline(upright_energy(), color='gray', linestyle='--', linewidth=1)
+ axes[1, 0].axhline(0.0, color='gray', linestyle='--', linewidth=0.8)
+ axes[1, 1].axhline(0.0, color='gray', linestyle='--', linewidth=0.8)
+ for ax in axes[1]:
+ ax.set_xlabel('time (s)')
+ fig.suptitle('Energy-based swing-up and LQR balance (green: balance mode)')
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'swingup_response.png'), dpi=150)
+ plt.close(fig)
+
+
+def plot_phase(data):
+ import matplotlib
+ matplotlib.use('Agg')
+ import matplotlib.pyplot as plt
+
+ theta = np.array([wrap_angle(v) for v in data['state'][:, 2]])
+ rate = data['state'][:, 3]
+ balance = data['mode'] == MODE_BALANCE
+
+ fig, ax = plt.subplots(figsize=(7, 5))
+ ax.plot(theta[~balance], rate[~balance], color='#ff7f0e', linewidth=1.0,
+ label='swing-up')
+ ax.plot(theta[balance], rate[balance], color='#2ca02c', linewidth=1.4,
+ label='LQR balance')
+ ax.scatter([theta[0]], [rate[0]], color='black', s=30, label='start')
+ ax.scatter([theta[-1]], [rate[-1]], color='red', marker='x', s=50, label='end')
+ ax.axvline(0.0, color='gray', linestyle='--', linewidth=0.8)
+ ax.set_xlabel('theta (rad)')
+ ax.set_ylabel('theta_dot (rad/s)')
+ ax.set_title('Phase portrait: swinging up to the upright equilibrium')
+ ax.grid(True, alpha=0.3)
+ ax.legend(loc='upper right', fontsize=8)
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'phase_portrait.png'), dpi=150)
+ plt.close(fig)
+
+
+def plot_comparison(swingup, lqr_only):
+ import matplotlib
+ matplotlib.use('Agg')
+ import matplotlib.pyplot as plt
+
+ def cos_theta(data):
+ return np.cos(np.array([wrap_angle(v) for v in data['state'][:, 2]]))
+
+ fig, ax = plt.subplots(figsize=(8, 4))
+ ax.plot(swingup['t'], cos_theta(swingup), label='energy swing-up + LQR')
+ ax.plot(lqr_only['t'], cos_theta(lqr_only), '--', label='LQR only (from hanging)')
+ ax.axhline(1.0, color='gray', linestyle=':', linewidth=0.8)
+ ax.set_xlabel('time (s)')
+ ax.set_ylabel('cos(theta) (1 = upright, -1 = hanging)')
+ ax.set_title('Why swing-up is needed: LQR alone never reaches the upright position')
+ ax.grid(True, alpha=0.3)
+ ax.legend(loc='lower right', fontsize=8)
+ fig.tight_layout()
+ fig.savefig(os.path.join(OUTPUT_DIR, 'lqr_only_comparison.png'), dpi=150)
+ plt.close(fig)
+
+
+def main():
+ swingup = simulate(use_swingup=True)
+ lqr_only = simulate(use_swingup=False)
+
+ plot_swingup(swingup)
+ plot_phase(swingup)
+ plot_comparison(swingup, lqr_only)
+
+ result = metrics(swingup)
+ result['lqr_only_final_angle_deg'] = float(
+ math.degrees(wrap_angle(lqr_only['state'][-1, 2])))
+ print(json.dumps(result, ensure_ascii=False, indent=2))
+ for name in ('swingup_response.png', 'phase_portrait.png', 'lqr_only_comparison.png'):
+ print('figure:', os.path.join(OUTPUT_DIR, name))
+ return 0
+
+
+if __name__ == '__main__':
+ sys.exit(main())
diff --git a/src/classical_control/cartpole_swingup/package.xml b/src/classical_control/cartpole_swingup/package.xml
new file mode 100644
index 0000000000..ca8315e5bd
--- /dev/null
+++ b/src/classical_control/cartpole_swingup/package.xml
@@ -0,0 +1,18 @@
+
+
+ cartpole_swingup
+ 0.1.0
+
+ 倒立摆能量摆起 + LQR 平衡模块:订阅倒立摆状态,先用能量控制把摆杆从
+ 下垂位置摆到竖直附近,再切换 LQR 保持平衡,发布小车控制力。
+ 配套文档见 docs/classical_control/cartpole_swingup.md。
+
+ xinyangtang91
+ MIT
+
+ catkin
+
+ rospy
+ std_msgs
+ python3-numpy
+
diff --git a/tests/test_cartpole_swingup_local.py b/tests/test_cartpole_swingup_local.py
new file mode 100644
index 0000000000..c8f3ef0576
--- /dev/null
+++ b/tests/test_cartpole_swingup_local.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""cartpole_swingup 本地测试(不需要 ROS / Gazebo).
+
+A. 动力学与能量函数
+B. LQR 增益的闭环稳定性
+C. 摆起 / 平衡模式切换
+D. 能量控制确实能把能量泵上去
+E. 工程文件
+
+用法: python3 tests/test_cartpole_swingup_local.py
+"""
+from __future__ import annotations
+
+import math
+import os
+import sys
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+PASS, FAIL = [], []
+
+
+def check(cond, label):
+ (PASS if cond else FAIL).append(label)
+ print(" [%s] %s" % ("PASS" if cond else "FAIL", label))
+
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+ROOT = os.path.abspath(os.path.join(HERE, ".."))
+PKG = os.path.join(ROOT, "src", "classical_control", "cartpole_swingup")
+sys.path.insert(0, PKG)
+
+from cartpole_model import ( # noqa: E402
+ MODE_BALANCE, MODE_SWINGUP, CartPoleController, DEFAULT_PARAMS,
+ cartpole_dynamics, lqr_gain, total_energy, upright_energy, wrap_angle,
+)
+
+
+def rk4(state, force, dt):
+ k1 = cartpole_dynamics(state, force)
+ k2 = cartpole_dynamics([s + dt / 2 * d for s, d in zip(state, k1)], force)
+ k3 = cartpole_dynamics([s + dt / 2 * d for s, d in zip(state, k2)], force)
+ k4 = cartpole_dynamics([s + dt * d for s, d in zip(state, k3)], force)
+ return [s + dt / 6 * (a + 2 * b + 2 * c + d)
+ for s, a, b, c, d in zip(state, k1, k2, k3, k4)]
+
+
+# ================================================================= A. 模型
+def test_model():
+ print("== A. 动力学与能量 ==")
+ check(abs(abs(wrap_angle(3 * math.pi)) - math.pi) < 1e-9,
+ "角度归一化到 [-pi, pi]")
+
+ derivative = cartpole_dynamics([0.0, 0.0, 0.0, 0.0], 0.0)
+ check(max(abs(v) for v in derivative) < 1e-12, "竖直静止时状态导数为 0")
+
+ check(abs(total_energy([0.0, 0.0, math.pi, 0.0])) < 1e-9,
+ "下垂静止时总能量为 0")
+ check(abs(total_energy([0.0, 0.0, 0.0, 0.0]) - upright_energy()) < 1e-9,
+ "竖直静止时总能量等于目标能量")
+ check(abs(upright_energy() - 2 * DEFAULT_PARAMS['mp'] * DEFAULT_PARAMS['g'] * DEFAULT_PARAMS['l']) < 1e-9,
+ "目标能量 = 2 m g l")
+
+
+# ================================================================= B. LQR
+def test_lqr():
+ print("== B. LQR 闭环稳定性 ==")
+ from cartpole_model import linearize_upright
+ K = lqr_gain(dt=0.02)
+ A, B = linearize_upright()
+ Ad = np.eye(4) + A * 0.02
+ Bd = B * 0.02
+ eigenvalues = np.linalg.eigvals(Ad - Bd.dot(K.reshape(1, -1)))
+ check(np.max(np.abs(eigenvalues)) < 1.0,
+ "离散闭环特征值都在单位圆内(max=%.3f)" % np.max(np.abs(eigenvalues)))
+ check(K.shape == (4,), "LQR 增益为 4 维")
+
+
+# ================================================================= C. 模式切换
+def test_switch():
+ print("== C. 摆起 / 平衡模式切换 ==")
+ controller = CartPoleController()
+ check(controller.mode == MODE_SWINGUP, "初始为 SWINGUP")
+
+ controller.update([0.0, 0.0, 0.2, 0.5])
+ check(controller.mode == MODE_BALANCE, "接近竖直且角速度小时切到 BALANCE")
+
+ force = controller.update([0.0, 0.0, 0.3, 0.0])
+ check(math.isfinite(force) and abs(force) <= DEFAULT_PARAMS['force_limit'],
+ "BALANCE 输出有限且在限幅内")
+
+ controller.update([0.0, 0.0, 1.0, 0.0])
+ check(controller.mode == MODE_SWINGUP, "角度过大时退回 SWINGUP")
+
+ lqr_only = CartPoleController(allow_swingup=False)
+ lqr_only.update([0.0, 0.0, 3.0, 0.0])
+ check(lqr_only.mode == MODE_BALANCE, "allow_swingup=False 时始终为 BALANCE")
+
+
+# ================================================================= D. 能量泵入
+def test_swingup():
+ print("== D. 能量摆起 ==")
+ controller = CartPoleController()
+ state = [0.0, 0.0, math.pi - 0.1, 0.0]
+ initial_energy = total_energy(state)
+ dt = 0.02
+ for _ in range(150): # 3 s
+ force = controller.update(state, dt)
+ state = rk4(state, force, dt)
+
+ check(total_energy(state) > initial_energy + 0.2,
+ "3 s 内能量明显增加(%.2f -> %.2f J)" % (initial_energy, total_energy(state)))
+ check(abs(wrap_angle(state[2])) < math.pi - 0.02, "摆杆已经开始向上运动")
+
+
+# ================================================================= E. 工程文件
+def test_project_files():
+ print("== E. 工程文件 ==")
+ pkg = ET.parse(os.path.join(PKG, "package.xml")).getroot()
+ check(pkg.findtext("name").strip() == "cartpole_swingup", "package.xml 包名正确")
+ deps = [d.text.strip() for d in pkg.findall("depend")] + \
+ [d.text.strip() for d in pkg.findall("exec_depend")]
+ for dep in ("rospy", "std_msgs", "python3-numpy"):
+ check(dep in deps, "package.xml 声明依赖 %s" % dep)
+
+ cmake = open(os.path.join(PKG, "CMakeLists.txt"), encoding="utf-8").read()
+ for name in ("main.py", "cartpole_model.py", "offline_sim.py"):
+ check(name in cmake, "CMakeLists.txt 安装 %s" % name)
+
+ launch = ET.parse(os.path.join(PKG, "launch", "main.launch")).getroot()
+ nodes = [n.get("type") for n in launch.findall("node")]
+ check("main.py" in nodes, "main.launch 启动 main.py")
+ args = [a.get("name") for a in launch.findall("arg")]
+ for name in ("state_topic", "force_topic"):
+ check(name in args, "main.launch 暴露参数 %s" % name)
+
+ check(os.path.exists(os.path.join(PKG, "README.md")), "README.md 存在")
+ for rel in (
+ "docs/classical_control/cartpole_swingup.md",
+ "docs/img/classical_control/cartpole_swingup/swingup_response.png",
+ "docs/img/classical_control/cartpole_swingup/phase_portrait.png",
+ "docs/img/classical_control/cartpole_swingup/lqr_only_comparison.png",
+ ):
+ check(os.path.exists(os.path.join(ROOT, rel)), "文件存在:%s" % rel)
+
+
+def main():
+ test_model()
+ test_lqr()
+ test_switch()
+ test_swingup()
+ test_project_files()
+ print("\n==================== 结果 ====================")
+ print("通过 %d 项,失败 %d 项" % (len(PASS), len(FAIL)))
+ if FAIL:
+ for f in FAIL:
+ print(" FAIL: %s" % f)
+ return 1
+ print("全部通过")
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())