两轮平衡车LQR控制

本案例基于 LQR（线性二次型调节器）构建双轮自平衡小车（Segway 型）的建模、控制与演示系统，实现从初始倾斜恢复平衡、脉冲扰动抑制、速度指令跟踪与开环/闭环对比等典型控制实验，并提供 Modelica 物理建模 与 Sysblock 框图控制 两种控制器实现方式，是状态空间法、最优控制与多体建模仿真的典型教学示例。

适用课程、行业与场景

本案例库适用于自动控制原理、现代控制理论、线性系统理论、最优控制、机器人学与移动机器人、多体系统动力学、机电系统建模与仿真、嵌入式控制等课程教学；面向机器人、平衡车/两轮电动车、自动驾驶、工业自动化与运动控制等行业的研发与教学人员；可用于 LQR 最优控制与状态反馈的教学演示，轮式倒立摆这类本质不稳定系统的镇定实验，Modelica 物理建模与 Sysblock 框图控制实现的混合建模讲解，以及平衡机器人、两轮自平衡车的方案验证与产品展示等场景。

库结构

系统模型与控制原理（技术要点）

状态向量

双轮自平衡机器人本质上是轮式倒立摆，状态向量为：

• x —— 水平位移（m）
• v —— 水平速度（m/s）
• θ —— 车身相对竖直方向的倾斜角（rad，正值前倾）
• ω —— 倾斜角速度（rad/s）

线性化状态空间模型

在 θ=0 附近线性化（小角度近似 sinθ≈θ、cosθ≈1）后，被控对象为：

dx/dt   = v
dv/dt   = A23·θ + B2·F
dθ/dt   = ω
dω/dt   = A43·θ + B4·F

其中 F 为等效水平驱动力（电机转矩之和 ÷ 车轮半径）。系数 A23、A43、B2、B4 由两轮倒立摆耦合运动方程导出，考虑了车身质量、车轮质量、车轮惯量与质心高度：

M_eff = (mBody + 2·mWheel) + 2·IWheel/rWheel²     // 含车轮惯量的等效质量
I_total = IBody + mBody·L²                          // 绕轴总转动惯量
D = M_eff·I_total − (mBody·L)²                     // 解耦行列式
A23 = mBody²·g·L² / D        B2 = I_total / D
A43 = M_eff·mBody·g·L / D    B4 = −mBody·L / D

LQR 最优控制

LQR 通过最小化二次型代价函数求解全状态反馈增益：

最优控制律为 u = −K·x，增益 K 通过求解连续时间代数 Riccati 方程（CARE）得到。

• 默认加权：Q = diag([1, 0.1, 100, 1])，R = 0.1（对倾斜角偏差施加较大惩罚 Q₃₃=100）。
• 默认增益：K = [-3.162, -5.039, -292.56, -42.85]，即 u = −(3.162x + 5.039v + 292.56θ + 42.85ω)。
• 控制量经饱和限幅到 [-200, 200] N，防止不切实际的指令导致发散。
• 控制器附带 cost 输出，实时给出瞬时代价 x'Qx + u'Ru。

控制器实现方式

两种实现控制效果一致，用于对比「方程建模」与「框图数据流建模」两种建模范式。

实验场景

场景要点

• 初始倾斜平衡：小车以 0.15 rad（约 8.6°）前倾起步，倾斜角应在约 3~5 s 内收敛至零。
• 开环 vs 闭环对比：开环对象零力输入、快速倾倒发散；LQR 闭环对象倾斜角收敛，直观说明反馈镇定对本质不稳定系统的必要性。
• 脉冲扰动抑制：在 t=2 s 施加幅值 50 N、持续 0.1 s 的矩形脉冲，LQR 快速消除扰动。
• 速度指令跟踪：t=1 s 施加速度阶跃，控制器在平衡 LQR 基础上叠加前馈补偿，小车在保持直立的同时逼近目标速度。

关键物理参数

仿真配置与推荐设置

• 平衡恢复 / 速度跟踪场景：StopTime = 15~20 s；开环对比、扰动抑制场景：StopTime = 5 s。
• 求解器：Dassl / CVODE，默认容差（场景默认 Tolerance=1e-6，Interval=0.01）。
• 关键观测量：plant.theta_out（倾斜角）、plant.omega_out（倾斜角速度）、plant.x_out/v_out（位移/速度）、controller.u（控制力）、controller.cost（LQR 代价）。
• 动画场景在 3D 视图中观察车体的倾斜与恢复过程。

目录内容

使用许可

本模型库版权由MoHub版权所有，未经许可，不得用于商业用途。