基于二级倒立摆控制算法的研究与实验

摘 要二 级 倒 立 摆 系 统 是 一 个 多 变 量 、不 稳 定 的 系 统 ，常 规 的 PID 算 法 无 法 实 现 其 稳定 控 制 。依 据 二 级 倒 立 摆 自 身 特 点 选 取 线 性 二 次 型 LQR、极 点 配 置 和 模 糊 控 制 这 三种算法。通过 Simulink 仿真和实验室实物操作结合的方式实现基于三种算法的二级 倒 立 摆 稳 定 控 制 。对 比 仿 真 曲 线 和 实 体 控 制 效 果 ，分 析 得 出 每 种 算 法 的 优 缺 点 及适用场合。实验结果表明：LQR 的稳定性强，小 车位移的偏差 0.06m，适用于 稳定性 的 场 合 ；极 点 配 置 的 瞬 时 响 应 快 ，调 节 时 间 仅 为 4s，适 用 于 要 求 快 速 的 场 合 ；模糊控制在综合效果上明显优于另两种算法，且抗干扰能力最强。在实体控制中，基 于 该 法 设 计 出 的 控 制 器 遇 到 干 扰 仅 需 1.2s 便 可 使 系 统 恢 复 稳 定 。关 键 字 ：二 级 倒 立 摆 系 统 ,线 性 二 次 型 LQR，极 点 配 置 ，模 糊 控 制 ，Simulink仿真，实体控制I基于二级倒立摆控制算法的研究与实验AbstractDouble inverted pendulum system is a multi-variable and unstable system thatconventional PID algorithm can not achieve its stable control. According to thecharacteristics of control of double inverted pendulum, three control algorithms areselected, which are linear quadratic LQR, pole placement and fuzzy control. The stablecontrol of double inverted pendulum system that is based on three algorithms is achievedby the combination of Simulink simulation and operation in laboratory. Compared withthe simulation curve and the effect of physical control, the advantages, disadvantagesand proper occasions of each algorithm are analyzed. The result shows that stability ofLQR is strong, and the deviation of car shift is 0.06m,is applied for the occasions of highstability.Transient response of pole assignment is fast, and the adjustment time is only4s,is achieved in the occasion of rapid response. Fuzzy control is more perfect thanothers in comprehensive effect and anti-jamming capability is the strongest. Theinterference is overcome in 1.2s by applying this algorithm in physical control.Key words：double inverted pendulum system, linear quadratic LQR, pole assignment,fuzzy control, Simulink simulation, physical controlII基于二级倒立摆控制算法的研究与实验目 录第一章 前 言 .11.1 研 究 倒 立 摆 系 统 的 意 义 .11.2 倒 立 摆 系 统 国 内 外 研 究 现 状 .11.3 倒 立 摆 系 统 常 见 的 控 制 算 法 的 比 较 及 本 论 文 重 点 .2第二章 二 级 倒 立 摆 系 统 的 数 学 模 型 .42.1 二 级 倒 立 摆 系 统 组 成 .42.2 二 级 倒 立 摆 机 构 示 意 图 及 控 制 框 图 .42.3 二 级 倒 立 摆 数 学 模 型 .6第三章 二 级 倒 立 摆 系 统 性 能 分 析 .133.1 系 统 可 控 性 分 析 .133.2 系 统 可 观 性 分 析 .143.3 系 统 稳 定 性 分 析 .15第四章 利 用 线 性 二 次 型 LQR 算 法 控 制 二 级 倒 立 摆 系 统 .174.1 线 性 二 次 型 LQR 的 基 本 原 理 .174.2 线 性 二 次 型 LQR 算 法 设 计 .184.3 利 用 线 性 二 次 型 LQR 算 法 进 行 二 级 倒 立 摆 系 统 的 SIMULINK 仿 真 .204.4 实 体 控 制 .22第五章 利 用 极 点 配 置 算 法 控 制 二 级 倒 立 摆 系 统 .255.1 极 点 配 置 算 法 的 基 本 原 理 .255.2 单 输 入 的 极 点 配 置 .265.3 极 点 配 置 算 法 设 计 及 主 导 极 点 的 影 响 和 稳 定 域 .275.4 利 用 极 点 配 置 算 法 进 行 二 级 倒 立 摆 系 统 的 SIMULINK 仿 真 .315.5 实 体 控 制 .32第六章 利 用 模 糊 控 制 算 法 控 制 二 级 倒 立 摆 系 统 .356.1 模 糊 控 制 算 法 的 基 本 思 想 和 基 本 原 理 .356.2 二 级 倒 立 摆 基 于 模 糊 控 制 算 法 的 稳 定 控 制 思 想 .366.3 融 合 函 数 设 计 .376.4 基 于 二 级 倒 立 摆 的 模 糊 控 制 器 的 设 计 .37III基于二级倒立摆控制算法的研究与实验6.5 利 用 模 糊 控 制 算 法 进 行 二 级 倒 立 摆 系 统 的 SIMULINK 仿 真 .446.6 实 体 控 制 .46第七章 总 结 与 展 望 .497.1 三 种 控 制 算 法 控 制 效 果 的 比 较 与 分 析 .497.2 实 验 结 论 .517.3 倒 立 摆 控 制 的 展 望 .52参 考 文 献 .53致 谢 .54IV基于二级倒立摆控制算法的研究与实验第一章 前言1.1 研究倒立摆系统的意义研 究 倒 立 摆 系 统 具 有 重 要 的 理 论 意 义 。它 是 进 行 控 制 理 论 研 究 的 经 典 的 试 验 台 ，许多新的控制理论算法都要通过它来验证，比如：模糊控制、神经网络等智能控制算法。二 级 倒 立 摆 系 统 作 为 控 制 理 论 的 一 个 成 熟 且 典 型 范 例 ，它 具 有 结 构 简 单 、成 本 低 、便 于用 模 拟 或 数 字 算 法 进 行 控 制 的 特 点 1 。作 为 一 个 被 控 对 象 ，它 本 身 是 一 个 高 阶 次 、不 稳定、多变量、非线性、 强耦合系统，只有采取行之有效的控制算法才能使它保持 稳定。因 此 研 究 倒 立 摆 系 统 的 稳 定 问 题 在 控 制 理 论 研 究 中 有 重 要 的 意 义 。通 过 对 它 的 研 究 不 仅可 以 解 决 控 制 中 的 理 论 问 题 ，还 能 将 控 制 理 论 主 要 涉 及 的 力 学 、数 学 和 电 学 进 行 有 机 的综 合 应 用 2 。研 究 倒 立 摆 系 统 还 具 有 重 要 的 实 践 意 义 。在 近 代 机 械 系 统 控 制 中 ，如 火 箭 发 射 中 的垂 直 姿 态 控 制 、双 足 行 走 机 器 人 的 姿 态 控 制 和 机 器 人 的 举 重 平 衡 控 制 等 都 涉 及 倒 立 问题 ，即 倒 立 摆 系 统 的 多 样 化 ，为 验 证 各 种 算 法 的 有 效 性 提 供 了 试 验 设 备 的 多 选 择 性 。因此其控制算法在军工、航天、机器人领域和一般工业过程中有着重要的实践意义 3 ，是一个从控制理论通往实践的桥梁。1.2 倒立摆系统国内外研究现状1.2.1 国内研究情况虽 然 我 国 对 倒 立 摆 系 统 研 究 的 起 步 较 晚 ，但 是 近 年 来 ，随 着 许 多 高 校 都 配 备 倒 立 摆系统的教学设备，研究成果也随之丰富，主要的研究成果如下：1994 年 北 航 自 动 化 系 在 世 界 上 首 先 成 功 实 现 单 电 机 二 级 摆 系 统 稳 定 。同 年 ，北 航 张民 廉 教 授 在 国 际 上 最 早 提 出 将 人 工 智 能 与 自 动 控 制 理 论 相 结 合 ，实 现 用 单 电 机 对 直 线 三级 倒 立 摆 的 智 能 控 制 并 在 实 体 中 验 证 。1995 年 ，程 福 雁 利 用 传 统 控 制 理 论 与 智 能 控 制 理论 相 结 合 的 方 法 对 直 线 二 级 倒 立 摆 进 行 了 稳 定 控 制 。1996 年 ，马 小 军 设 计 了 模 糊 控 制 器和 模 糊 观 测 器 对 倒 立 摆 系 统 进 行 了 稳 定 控 制 。1999 年 ，李 德 毅 利 用 云 控 制 算 法 有 效 实 现单 电 机 控 制 一 、二 、三 级 倒 立 摆 的 多 种 不 同 动 平 衡 姿 态 ，并 给 出 详 细 实 验 结 果 。2000年 ，林 红 利 用 最 优 反 馈 调 节 器 不 但 使 摆 在 倒 立 位 置 保 持 平 衡 ，而 且 在 锯 齿 波 信 号 的 作 用下 有 规 律 地 移 动 ，直 至 无 限 远 处 。2001 年 ，单 波 通 过 预 测 控 制 算 法 ，对 倒 立 摆 系 统 进 行了 稳 定 控 制 。2007 年 ，段 学 超 设 计 自 适 应 滑 模 模 糊 控 制 算 法 ,实 现 在 基 座 小 车 沿 圆 周 行走条件下对摆杆的稳定控制。1基于二级倒立摆控制算法的研究与实验在 倒 立 摆 领 域 研 究 贡 献 最 突 出 的 是 北 京 师 范 大 学 的 李 洪 兴 教 授 ，他 采 用 动 力 学 分 析的 算 法 推 导 出 任 意 级 直 线 倒 立 摆 的 非 线 性 模 型 。此 外 ，李 洪 兴 教 授 领 导 的 复 杂 系 统 智 能控 制 实 验 室 采 用 “变 论 域 自 适 应 模 糊 控 制 ”的 算 法 成 功 在 2002 年 8 月 11 日 ，实 现 了 全球 首 例 四 级 倒 立 摆 实 物 系 统 控 制 ，填 补 了 当 时 的 世 界 空 白 。该 系 统 具 有 良 好 的 稳 定 性 、鲁 棒 性 和 定 位 功 能 4 。2003 年 4 月 ，该 实 验 室 应 用 具 有 高 维 PID调 节 功 能 的 变 论 域 自 适应控制理论实现对平面运动二级倒立摆实物系统的控制。同年 10 月还成功实现世界上首 个 平 面 三 级 倒 立 摆 实 物 系 统 的 控 制 。不 但 控 制 效 果 稳 定 ，而 且 还 使 倒 立 摆 行 走 到 指 定位 置 。之 后 他 在 2005 年 ，成 功 实 现 平 面 三 级 倒 立 摆 的 稳 定 控 制 。1.2.2 国外研究情况国 外 对 于 倒 立 摆 系 统 研 究 的 起 步 较 早 ，早 在 1976 年 ，Mori 等 人 就 开 始 研 究 了 倒 立摆 的 稳 定 控 制 。近 年 来 ，国 外 许 多 科 研 人 员 在 多 级 倒 立 摆 上 面 进 行 了 深 入 的 研 究 有 了 如下成果：日本的古田教授从 1984 年 就 着 手 三 级 倒 立 摆 的 研 究 5 ，采用的算法是传统的控 制 理 论 ，虽 然 控 制 了 三 级 倒 立 摆 ，但 在 系 统 中 有 两 个 控 制 机 构 ，即 有 两 个 电 动 机 。德国 学 者 从 1990 年 也 进 行 了 类 似 的 研 究 1 ，也用两个控制机构控制了三级倒立摆。1983 年 ，Wattes 研 究 和 应 用 LQR（LinearQuadraticRegulator）算 法 控 制 倒 立 摆 ，该 控 制 算 法 实 际 上 是 寻 找 一 个 最 优 状 态 反 馈 矩 阵 K，从 而 设 计 一 个 最 优 反 馈 控 制 器 。1984年，Wattes 使用现代控制理论中的 LQR 算法完成了对直线摆的控制。1992 年，Furuta等 人 利 用 变 结 构 控 制 理 论 完 成 了 对 直 线 二 级 倒 立 摆 的 稳 定 控 制 6 。1993 年，Booslama利 用 一 个 简 单 的 神 经 网 络 来 学 习 模 糊 控 制 器 的 输 入 输 出 数 据 ，设 计 了 新 型 控 制 器 。1997年 ，T.H.Hung 等 人 设 计 了 利 用 模 糊 控 制 器 调 节 PI 参 数 的 算 法 并 应 用 于 直 线 一 级 倒 立 摆控 制 ，最 终 实 现 了 对 系 统 的 稳 定 控 制 7 。2000 年 ，SigeruOmatu 等 人 将 神 经 网 络 与 PID结合，实现了直线一级、二级倒立摆的稳定控制。 2001 年，D.Hougen 等人利用参数均值 的 SONNET（Self-organizing neural network eligibility trace）网络模型，对倒立 摆 系 统 进 行 有 效 控 制 。随 着 智 能 控 制 的 发 展 ，模 糊 控 制 、神 经 网 络 等 智 能 控 制 算 法 在倒 立 摆 的 稳 定 控 制 问 题 中 得 到 了 广 泛 应 用 。2003 年 ，Hyun Taek Cho 等 人 实 现 了 平 面 一级 倒 立 摆 的 神 经 网 络 模 型 参 考 控 制 7 ，并于 2007 年，针对平面一级倒立摆的每个轴方向 上 的 位 置 与 角 度 变 量 ，分 别 设 计 神 经 网 络 模 型 参 考 控 制 器 ，使 得 角 度 变 量 达 到 平 衡 状态的同时，达到了很好的位置跟踪效果。1.3 倒立摆系统常见的控制算法的比较及本论文重点经 典 控 制 理 论 和 现 代 控 制 理 论 算 法 在 处 理 倒 立 摆 系 统 问 题 时 都 采 用 了 近 似 的 处 理算 法 ，但 是 使 用 上 述 两 种 理 论 研 究 就 具 有 非 常 明 显 的 局 限 性 ，研 究 己 证 明 了 这 种 近 似 处2基于二级倒立摆控制算法的研究与实验理 算 法 的 控 制 效 果 是 不 能 令 人 满 意 的 ，要 解 决 倒 立 摆 系 统 的 控 制 问 题 ，需 要 引 入 智 能 控制算法的思路。对 于 一 级 倒 立 摆 成 功 的 控 制 算 法 有 ：PID 控 制 、神 经 网 络 控 制 等 。二 级 倒 立 摆 成 功的 控 制 算 法 有 ：线 性 二 次 型 LQR，状 态 反 馈 控 制 、模 糊 控 制 、应 用 BP 神 经 网 络 和 遗 传 算法 相 结 合 的 控 制 算 法 、用 前 向 或 递 归 神 经 网 络 并 结 合 最 优 控 制 、模 糊 控 制 算 法 、鲁 棒 最优 控 制 8 。三 级 倒 立 摆 成 功 的 控 制 算 法 有 ：拟 人 工 智 能 控 制 、用 壮 态 变 量 合 成 模 糊 神 经网络控制等。四级倒立摆成功的控制算法是变论域自适应模糊控制。当前,倒立摆的控制算法主要有：(1)线 性 二 次 型 LQR 算 法 (2)极 点 配 置 法 (3)模 糊 控 制 (4) PID 控 制 (5)神 经 网 络 控 制(6)遗传算法 ( 7)自 适 应 控 制 9,10,11表 1.1 倒立摆主要控制算法的特点及适用范围控制算法 特点 适用范围稳 态 特 性 很 好 且 控 制 器 设 计 简线 性 二 次 型 LQR 单 ，设 计 出 的 系 统 考 虑 能 量 消 耗 稳定性要求高的场合问题极点配置法 与 系 统 性 能 指 标 紧 密 相 连 ，动 态响应迅速 动态响应要求高的场合适用范围广，不依赖于对象的数模糊控制 学模型，鲁棒性强，模糊控制器 特别适合非线性系统的控制的设计参数易调整。算法易实现PID 控 制 结 构 简 单 、稳 定 性 好 、工 作 可 靠 、调整方便当不能完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，最适合用神 经 网 络 具 有 信 息 的 分 布 存 储 、 适 用 于 在 结 构 、实 现 机 理 和 功 能神经网络 并 行 处 理 、容 错 能 力 强 以 及 自 学习 能 力 等 优 点 ，用 强 化 学 习 算 法来实现倒立摆的平衡控制上模拟生物 神 经 网 络 的 系 统 ，主要用于用途依 据 的 关 于 模 型 和 扰 动 的 先 验 对 象 特 性 或 扰 动 特 性 变 化 范 围自适应控制 知 识 较 少 ，需 要 在 系 统 的 运 行 很 大 ，同 时 又 要 求 经 常 保 持 高过 程 中 提 取 有 关 模 型 的 信 息 性 能 指 标 的 一 类 系 统所 以 ，基 于 我 校 实 验 室 实 际 情 况 ，本 文 研 究 的 主 要 内 容 是 ：采 用 了 线 性 二 次 型 LQR、极 点 配 置 和 模 糊 控 制 分 别 对 二 级 倒 立 摆 系 统 进 行 控 制 并 通 过 比 较 它 们 的 控 制 效 果 得 出三种算法各自特点及适用场合。3基于二级倒立摆控制算法的研究与实验第二章 二级倒立摆系统的数学模型2.1 二级倒立摆系统组成我校使用的倒立摆系统为基于固高科技有限公司开发的运动控制器为核心的系统，它 主 要 的 部 分 GT-400-SVPCI总 线 四 轴 伺 服 运 动 控 制 卡 ，MINAS-A 系 列 全 数 字 式 交 流 伺 服电 机 和 驱 动 器 ，型 号 为 GLP2002 的 二 级 直 线 倒 立 摆 和 型 号 为 GCB202V 的 控 制 箱 12 ，包 括 计算 机 、运 动 控 制 卡 、伺 服 系 统 ，倒 立 摆 本 体 和 光 电 码 盘 反 馈 测 量 元 件 几 大 部 分 组 成 一 个闭环系统。图 1.1为固高倒立摆系统实物图。图 2.1 固高倒立摆系统实物图2.2 二级倒立摆机构示意图及控制框图由图 1.2 可知二级倒立摆系统包括电气和机械部分组成，机械部分主要有小车、下摆、上摆、 导轨、皮 带轮、传动皮带等,控制对象由小车、下摆、上摆组成。电气部分由 电 机 、晶 体 管 直 流 功 率 放 大 器 、传 感 器 以 及 保 护 电 路 组 成 。可 在 轨 道 上 做 直 线 运 动 的小 车 被 力 矩 电 机 通 过 带 轮 、传 动 带 驱 动 、下 摆 与 小 车 铰 合 ，上 摆 与 下 摆 的 一 端 铰 接 ，二摆 均 可 在 与 轨 道 平 行 的 垂 直 平 面 内 自 由 移 动 。检 测 点 P 、P 、P 电 位 器 ，分 别 检 测 小 车1 2 34基于二级倒立摆控制算法的研究与实验相 对 轨 道 中 心 点 的 偏 移 、下 摆 与 铅 垂 线 之 间 的 角 位 移 、上 摆 与 铅 垂 线 之 间 的 角 位 移 ，测量所得的电压信号经由三个运算放大器进行放大、标定(将一定量的机械信号转化为一定量的电信号)后，作为二级倒立摆系统的三个输出量，被送入控制器。控制器 输出的控制信号经功率放大器放大后驱动力矩电机，使二级倒立摆在不稳定的平衡点稳定。图 2.2 二级倒立摆机构示意图二级倒立摆控制系统框图：图 2.3 二级倒立摆控制系统框图5基于二级倒立摆控制算法的研究与实验二 级 倒 立 摆 控 制 系 统 控 制 过 程 ：光 电 码 盘 1 将 小 车 的 位 移 、速 度 信 号 反 馈 给 伺 服 电机 和 运 动 控 制 卡 ，下 摆 （和 小 车 相 连 ）的 角 度 、角 速 度 信 号 由 光 电 码 盘 2 反馈回控制卡和 伺 服 电 机 ，上 摆 的 角 度 和 角 速 度 信 号 则 由 光 电 码 盘 3 反 馈 。计 算 机 从 运 动 控 制 卡 中 读取 实 时 数 据 ，确 定 控 制 决 策 ，并 由 运 动 控 制 卡 来 实 现 该 决 策 ，产 生 相 应 的 控 制 量 ，使 电机转动，带动小车运动，保持两节摆杆的平衡。2.3 二级倒立摆数学模型在忽略了空气流动，各种摩擦之后，可将倒立摆系统抽象成小车、匀 质杆和 质 量 块 组 成 的 系 统 ，系 统 简 化 图 如 图 2.1 所示图 2.4 直线二级倒立摆系统简化图系 统 各 部 分 物 理 结 构 的 实 际 参 数 如 表 1 所列：表 2.1 倒立摆系统物理参数表M 小车质量 1.32Kg l1 摆杆 1转动中心到杆质心的距离 0.09mm 摆杆 1质量 0.04Kg1l 摆杆 2转动中心到杆 质心的距离0.27m2m 摆杆 1质量 0.132Kg F 作用在系统的外力2m 质 量块的质量 0.208Kg3 摆杆 2 与垂直向上方向的夹角2 摆杆 1 与垂直向上方向的夹角16基于二级倒立摆控制算法的研究与实验利用拉格朗日推导倒立摆运动方程：拉格朗日方程为：L(q,q&) = T(q,q&) V (q,q&) (2-1)其 中 ，L 为 拉 格 朗 日 算 子 ，q 为 系 统 的 广 义 坐 标 ，T 为 系 统 的 动 能 ，V 为系统的势能 。拉 格 朗 日 方 程 由 广 义 坐 标 q 和 L 表示为：iL Ld =dt q q&i ifi(2-2)其 中 ，i = 1,2,3Ln ， f 为 系 统 沿 该 广 义 坐 标 方 向 上 的 外 力 ，在 本 系 统 中 ，设 系 统 的i三个广义坐标分别是 x, 。1,2首先计算系统的动能：T = T + + + (2-3)M T T Tm1 m2 m3T 小 车 动 能 ，T 下 摆 (摆 杆 1)动 能 ，T 上 摆 (摆 杆 2)动 能 ，T 质量块动能。M m1 m2 m3而 T = + ，其中，1 m1 m1m T TT = + ，其中，2 m2 m2m T T Tm2 Tm2摆杆 2 质心平动动能摆杆 2 绕质心转动动能1T MxM = & (2-4)22 2 2 d(x l sin ) d(l cos ) 1 11 m x& m l x& m l &T = m + m = +2 2 21 1 1 1cos1 1 1 1 1 1 1 1 11 22 dt dt 2 1 J m l & 1 m l &1 1Tm = = = 2 2 2 2 2p 1 1 11 1 6 1 1 12 2 3 1 m x& m l x& 2 m l &则 T T T cos 2m = + = 2 + 2 (2-5)1 1 1 1 1 11 m m 1 3 1 1 12同样可以求出7基于二级倒立摆控制算法的研究与实验Tm2= 1 (x2 sin ld lm 1 1 222 dtsin22+ + 21 d(2l cos l cos )m 1 1 2 222 dt= 1 1m x& 2l cos l cos + m 2l sin +l sin( ) ( )& & & &2 22 1 1 1 2 2 2 2 1 1 1 2 2 22 21 J m l m l &1 1 1m = 2 = = 2 2 2 2T2 2 2 2 2 2 2 22 62 2 3 Tm2=Tm2+Tm2=1m x& x& l & l & m l & + l & + l l & 14( ( ) ( )2 cos + 4 cos22 2 2 2 2 + cos 42 1 1 1 2 2 2 2 1 1 2 2 1 2 1 2 2 12 2 3 Tm3= 12 d (x m 32l1dtsin )12+ d l )(2 cos 2(2-6)1 1 dt= 12 m x& 2m l x cos + 2m l 22 2& &3 3 1 1 1 3 1 1因此，可以得到系统动能T =TM+Tm1+Tm2+Tm3= 12 Mx&2+ 12 m l x& cosm x& &21 1 1 11+23&m l2 21 1 1+12m x& 2x& 2l cos l cos( ( )2 & + &2 1 1 1 2 2 2(2-7)+ 12 + +4 4 l cos& & & & ( )m 4l 2 2 2 2l l +2 1 1 2 2 1 2 1 22 1312m x&23 &2m l x& cos3 1 11+ & 2m l2 23 1 1系统的势能为：V =Vm1 +V +V = m gl cos +2m gl cos +m g 2l cos +l cos (2-8)( )m2 m3 1 1 1 3 1 1 2 1 1 2 2至 此 得 到 拉 格 朗 日 算 子 L:L = T V=12Mx&2+12m x& m l x cos&2 &1 1 1 1 1+ & 2m l x& cos3 1 11+ & 2m l2 23 1 1(2-9)由于因为在广义坐标1 , 上均无外力作用，有以下等式成立：28基于二级倒立摆控制算法的研究与实验ddt L&1L1= 0 (2-10)ddtL&2 L2= 0 (2-11)展开式(2-10)、(2-11)，分别得到式(2-12)、(2-13)6m l sin(& 22 2 2 2+(m + 2(m +1 2 ) + 4(m +3(m + m )l &1 1 2 3 1 1m )(gsin + &xcos ) = 0&3 1 1 & 3( 2m l2 2 2cos(2 )1 (2-12) g l & l & l &x& &x& (2-13)3 sin 2 6 + + =sin( ) 4 6 cos( ) 3 cos 021 1 2 1 2 2 1 2 1 2将式(2-12)、(2-13)对&1 ,& 求解代数方程，得到以下两式2&1= (3(2gm sin1 14gm2sin1 4m gsin3 1+3m gcos(2 2 )sin1 2+6m l cos( )sin( ) +4m l sin( )&2 22 1 1 2 1 2 1 2 2 1 2 24m xcos 4m xcos +3m xcos( )cos )/& &2 1 3 1 2 1 2 22m xcos&1 1(2-14)(2l (4m1 112m212m3+ 9m2cos (2 1 )2&2= ( 49m (m2 1+3(m2+m )l l2 (36l &21 1sin(1 )3xcos )&2 2gsin3 1 22+ 23m l l22 1 2cos(1 & )(6m l sin(22 2 2 2 1 )3(m2 1+2(m2+m )(gsin3 1+ xcos )/&116( m (m2 19+3(m2+m )l l2 23 1 2+4m l l2 2 22 1 2cos (2 1 )2(2-15)表示成以下形式：& = ( , , , , , , ) (2-16)1 f x x& & & &x&1 1 2 1 22 = f x x& & & &x& (2-17)& ( , , , , , , )2 1 2 1 2取平衡位置时各变量的初值为零，( x 1 x& & & &x& =, , , , , , )2 1 2(0,0,0,0,0,0,0)将式(2-16)在平衡位置进行泰勒级数展开，并线性化，令K11= f1xx=0, 0, =0,x&=0, =0, =0,&x&=0 = & &1 2 1 2= 09基于二级倒立摆控制算法的研究与实验f 3(2gm 4gm 4gm )K = =1 1 2 3 x 0, =0 x= = = 0 = = & & & &0, , 0, 0, 0,x=12 2( 4 3 12 )2 1 21 m m m l1 1 2 3 1f 9m gK = =1 2 = ,x=0= = =0 & & & &0 ,x 0, =0, =013 2( 4 3 12 )x 0 , , 1 m m m l2 1 22 1 2 3 1K14= f1& xx=0, =0, x& 0, 0, 0,x=0, 0= & = & =&1 2 1 2= 0K15= f1 =& x 0, 0, 0,& 0, 0, 0,& 0= = x= & = & = x=1 2 1 21= 0K16= f1 =& = x 0, 0, 0,& 0, 0, 0,& 0= = x & = = x& =1 2 1 22= 0f 3(2m m 4m )K = =1 1 2 3& x=x& 1 m m m lx=0, 0, & & & & =0, = x=0, =0, =0, 017 2( 4 3 12 )2 1 21 2 3 1带入式(2-14)，得到线性化之后的公式1 = + + (2-18)&K K K &x&12 1 13 2 17将式(2-17)在平衡位置进行泰勒级数展开，并线性化，令K21= f2xx 0, 0, 0, , 0,& = = x= & & = x0, = & =0 =01 2 1 2= 0K22= f 21x= & & & &0, =0, =0,x=0, =0, =0,x=01 2 1 2= 2gm (m + 2(m2 1 2164m l l m (m +2 2 2 2 1 2 2 19+m )l l23 1 23(m2+ m )l l2 23 1 2K23= f22x= & & & &0, =0, =0,x=0, =0, =0,x=01 2 1 2= 4gm (m + 3(m + m )l l22 1 2 3 1 2163(4m l l m (m + 3(m + m )l l )2 2 2 2 22 1 2 2 1 2 3 1 29K24= f2x& x=0, =0, =0,x 0, =0, 0,x=0& &= & =1 2 1 2= 0K25= f 2& = & =x=0, 0, =0,x=0, 0, =0,x 0= & &1 2 1 21= 0K26= f2& = = x= & xx 0, 0, =0, 0, =0, =0, =0& &1 2 1 22= 010基于二级倒立摆控制算法的研究与实验K27=f2& x&x=0 &= & & = &, =0, =0,x 0, =0, 0,x=01 21 2=2m (m2 1+2(m24m l l2 2 22 1 2+m )l l23 1 2169m (m2 143m (m+ 3(m2 12+3(m2+ m )l l2 23 1 2m )l l23 1 2带入式(2-15)，得到&2 = + + (2-19) K K K &x&22 1 23 2 27现在得到了两个线性微分方程，采用加速度作为输入，因此还需加上一个方程u = & (2-20)x取状态变量如下：x1= xx2x3x4=12= x&x5= &1x6=&2由式(2-18)，(2-19)，(2-20)得到状态空间方程如下：x &1 x& 2 =000000K120000K131000000100000x 1 0 x 2 1 x 3 0 0 0+ u x&3 x& 4 x& 5x & 60000K22K230 x 4 0 x 50x 61 K 17K 27将以下参数代入M =1.32m1= 0.04m2= 0.132g = 9.8l1= 0.09l2= 0.27求 出 各 个 K 值 如 下 ：11基于二级倒立摆控制算法的研究与实验K12= 77.0642K13= 21.1927K17= 5.7012K22= 38.5321K23= 37.8186K27= 0.0728得到状态方程各个参数矩阵：000A=000000077.064238.5321000021.192737.8186100000010000001000 0 0 0B = 1 5.7012 0.0728 由 于 系 统 输 出 为 小 车 的 位 移 x,摆 杆 1 与 垂 直 方 向 的 角 度 , 摆 杆 2 与 垂 直 方 向 的1角度 所以系 统的状态方程和输出方程为：2x&1x&2 x& 3x&4x&56=000000000077.064238.5321000021.192737.81861000000100000x 1 0 x 2 1 x 3 0 x 4 0 x 5 0 + 0 0 0 u1 5.7012 0.0728 x& x 6y =1000000100000010000001000000100x120 x0 x 30 x40 x561x+0 0 0 u0 0 0 12基于二级倒立摆控制算法的研究与实验第三章 二级倒立摆系统性能分析可 控 性 、可 观 测 性 、稳 定 性 都 是 控 制 系 统 的 重 要 属 性 。系 统 的 可 控 性 和 可 观 性 对 系统 的 设 计 至 关 重 要 ：因 为 系 统 的 可 控