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 摆起与平衡全过程 + +![摆起与平衡响应](../img/classical_control/cartpole_swingup/swingup_response.png) + +四个子图分别为摆角、总能量、小车位置和控制力,绿色区域为 LQR 平衡阶段。 +能量在前 5 s 从 0 泵到目标值附近,摆杆多次经过竖直方向;5.26 s 切入 LQR 后 +角度迅速收敛,小车从约 −0.86 m 缓慢回到原点。 + +### 4.2 相图 + +![相图](../img/classical_control/cartpole_swingup/phase_portrait.png) + +橙色为摆起阶段:轨迹从下垂点出发,能量逐渐增加,相轨迹一圈圈抬高; +绿色为 LQR 阶段:轨迹收敛到 $(0, 0)$ 平衡点。 + +### 4.3 与“只用 LQR”的对比 + +![只用 LQR 的对比](../img/classical_control/cartpole_swingup/lqr_only_comparison.png) + +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())