一级倒立摆摆起与稳态控制仿真系统

资 源 简 介

本项目基于MATLAB环境开发，完整实现了一级倒立摆（Single Inverted Pendulum）系统的动力学建模与控制仿真。程序的核心功能包括非线性动力学方程的构建、摆起控制与稳态平衡控制的自动切换策略。在摆起阶段，采用基于能量的控制算法（Energy Shaping）或Lyapunov方法，通过对小车施加作用力将能量注入系统，使自然下垂的摆杆逐渐摆动并直至接近垂直向上的不稳定平衡点。一旦系统状态进入预设的线性区域，控制器自动平滑切换至线性稳定控制器（如LQR线性二次型调节器或PID控制器），以维持摆杆的垂直平衡并控制小车在导轨上的位移。项目包含完整的数值仿真代码，利用ode45等求解器解算微分方程，并提供直观的动画演示功能，实时展示小车运动和摆杆姿态。同时，系统具备详细的数据分析功能，能够记录并绘制摆杆角度、角速度、小车位移、速度及控制电压/力随时间变化的曲线，用于验证控制算法的有效性和鲁棒性。

详 情 说 明

一级倒立摆摆起与稳态控制仿真系统

项目简介

本项目是一个基于MATLAB开发的一级倒立摆（Single Inverted Pendulum）动力学仿真与控制系统。该系统完整实现了从倒立摆自然下垂状态下的能量起摆（Swing-Up），到垂直平衡位置的稳态控制（Stabilization）的全过程仿真。

核心逻辑采用混合控制策略：在远离平衡点时使用基于能量的非线性控制算法，当系统进入平衡点附近的线性区域后，自动切换至LQR（线性二次型调节器）进行最优控制。项目完全使用MATLAB原生代码编写，集成了数值计算、动态仿真解算、数据后处理以及实时动画演示功能。

功能特性

• 高精度非线性建模：基于拉格朗日力学构建了完整的小车-摆杆系统非线性动力学微分方程，考虑了转动惯量、摩擦系数及耦合作用。
• 混合控制策略：

* 能量起摆 (Energy Shaping)：利用Lyapunov能量函数方法，将系统能量注入摆杆，使其摆动幅度逐渐增大。 * LQR稳态控制：在垂直向上位置附近进行线性化，利用LQR计算最优反馈增益，实现抗干扰的稳定悬停。 * 自动平滑切换：根据摆杆角度和角速度阈值，自动判定并切换控制模式。

• 位置约束保护：在起摆阶段引入了小车位移和速度的PD负反馈，防止小车在大幅度摆动过程中超出导轨范围。
• 数值仿真求解：使用MATLAB内置的 ode45 (Runge-Kutta) 求解器处理非线性微分方程组。
• 交互式可视化：

* 提供包含小车、摆杆的实时运动动画。 * 绘制位移、角度随时间变化的状态响应曲线。 * 绘制包含控制模式切换指示的控制输入（电压/力）曲线。

• 零外部依赖：代码结构紧凑，仅需要MATLAB基础环境（仅LQR计算依赖Control System Toolbox）。

系统要求

• MATLAB R2016b 及以上版本
• Control System Toolbox（用于计算LQR增益，若无此工具箱代码将报错）

使用方法

1. 确保MATLAB环境已安装并包含控制系统工具箱。
2. 将主程序文件保存到本地目录。
3. 在MATLAB命令行窗口运行主函数或直接点击“运行”按钮。
4. 系统将自动弹出仿真窗口，展示倒立摆从下垂状态摆起并稳定的动画过程，并在仿真结束后输出最终的角度误差和位移误差。

详细算法与实现逻辑

本项目的主程序采用函数式编程结构，主要包含以下核心模块：

1. 物理建模与参数定义

程序首先定义了系统的物理参数，包括小车质量(M=1.0kg)、摆杆质量(m=0.2kg)、摆杆半长(l=0.3m)、摩擦系数等。 为了实现LQR控制，代码在零时刻预先计算了系统在垂直向上平衡点（theta=0）处的线性化状态空间矩阵（A, B），状态向量定义为 [位移, 速度, 角度, 角速度]。

2. 控制器设计

控制系统包含两部分，通过 ctrl_params 结构体打包传递给求解器：

• LQR控制器：设计了状态权重矩阵 Q 和控制权重 R。其中 Q 矩阵对角度偏差给予了极高的权重 (1000)，以优先保证摆杆的垂直度。程序调用 lqr 函数计算最优反馈增益矩阵 K。
• 能量起摆控制器：设定目标能量 E_ref 为摆杆垂直静止时的势能。定义了起摆增益 k_swing 用于调节能量注入的速率。

3. 数值仿真 (ode45)

系统初始状态设定为小车静止、摆杆垂直向下（pi rad）。仿真时间跨度为15秒。 使用 ode45 求解器调用 sys_dynamics 函数。由于 ode45 变步长求解的特性，控制输入 $u$ 是在动力学方程内部实时计算的。为了后续绘图分析，主函数在仿真结束后，利用计算出的状态轨迹 $X$ 再次反向推算了一遍每一时刻的控制力 $U$ 和控制模式 $Mode$。

4. 动力学解算 (sys_dynamics 函数)

这是描述系统物理特性的核心函数。它接收当前状态和控制输入，计算状态导数。

• 根据当前的控制力 $u$ 和系统状态，构建包含质量矩阵、科里奥利力项和重力项的非线性方程组。
• 采用显式公式（基于质量矩阵求逆的推导结果）计算小车加速度和摆杆角加速度，实现了精确的非线性耦合模拟。

5. 控制策略实现 (get_control 函数)

该函数是控制系统的“大脑”，负责计算每一时刻的控制输出 $u$：

• 状态规范化：将角度规范化到 $[-pi, pi]$ 区间，以便正确判断偏离垂直方向的角度。
• 切换逻辑：设定阈值为：角度偏差小于 20度 且 角速度小于 2.0 rad/s。满足该条件则判定进入“稳定区域”。
• 模式 0 (能量起摆)：

* 计算当前系统的总机械能（动能+势能）。 * 计算能量误差 $E_{err} = E_{total} - E_{ref}$。 * 控制律形式为 $u = k cdot E_{err} cdot dot{theta} cdot cos(theta)$。该算法利用摆杆速度和角度的相位关系，在最有效率的时刻施加推力以增加或减少系统能量。 * 关键细节：为了防止起摆过程中小车跑飞，额外叠加了一个针对小车位置和速度的PD负反馈项 ($-k_{pos}x - k_{vel}v$)。

• 模式 1 (LQR稳态)：

* 计算相对于平衡点的状态误差。 * 应用状态反馈控制律 $u = -K cdot x$。

• 限幅保护：最后对计算出的控制力 $u$ 进行物理限幅（最大20N），模拟真实电机的出力限制。

6. 数据可视化与动画

仿真结束后，程序创建一个包含四个子图区域的窗口（实际布局为 2x2，左侧合并）：

• 动画窗口：左侧大图。通过循环更新 rectangle（小车）、plot（摆杆）和 text（实时数据）对象的属性，实现了流畅的动画效果。包含时间同步逻辑 (pause) 以匹配真实仿真时间。
• 状态响应：右上图。绘制小车位移和摆杆角度随时间的变化，展示从震荡到收敛的过程。
• 控制输入：右下图。绘制控制力曲线，并使用绿色背景区域标记出何时系统切换到了LQR稳态控制模式。

代码结构分析

• main (主函数)：负责参数初始化、控制器增益计算、调用ode45求解、数据重构以及绘图和动画循环。
• sys_dynamics (子函数)：定义倒立摆的非线性微分方程，输入为时间、状态、参数，输出为状态导数。
• get_control (子函数)：封装了具体的控制算法逻辑（能量起摆与LQR的判断与计算），实现了控制器与物理模型的解耦。