大学毕业论文双足机器人系统平衡控制设计论文.docVIP

重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 1 目 录 摘要 abstract 1 绪论.3 1.1 课题的研究背景和意义3 1.2 双足机器人的国内外研究状况4 1.2.1 国外研究状况.4 1.2.2 国内研究状况7 1.2.3 欠驱动双足机器人9 1.3 欠驱动双足机器人控制存在的问题11 1.4 本文的研究工作.11 1.5 论文的构成.12 2 双足机器人直立平衡控制的模型研究13 2.1.双足机器人的欠驱动姿态.13 2.1.1 模型简化的提出.13 2.1.2 双足机器人欠驱动姿态分析13 2.2 物理模型.15 2.3 数学模型.16 2.4 matlab 仿真模型.18 2.5 小结18 3 双足机器人系统的能控能观性分析19 3.1 平衡稳定控制目标分析.19 3.2 能控制性与能观测性分析.19 3.3 小结.20 4 平衡控制策略.21 4.1 lqr 控制器简介.21 4.2 线性二次型调节器（lqr）基本原理21 4.3 平衡控制的仿真实现22 4.4 小结23 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 2 5 仿真实验24 5.1 控制器仿真模型.24 5.2 仿真的结果曲线.25 5.3 小结.27 6 结论与展望28 致谢.28 参考文献29 附录.32 附录（数学模型推导）:32 附录（仿真模型）:.35 附录（matlab 程序语言）:36 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 3 1 绪论 1.11.1 课题的研究背景和意义 随着机器人技术的发展和控制理论的逐步成熟，对双足机器人的稳定性 问题、双足机器人步行移动及各种仿人动作的研究正受到国际学者们越来越 多的普遍关注 。基于控制理论 、动力学原理及 仿生学原理，人们通过对动 物和人类的运动行为、控制技巧的研究，提出和发展了一系列复杂运动控 制模态及相关算法。近 年来，研究集中在机器人的 平衡控制、 各种动作， 例如直立平衡、 步行移动 和跳跃等。 我们知道，人在保持直立平衡或者是在行走时都需要依靠各个关节来维 持身体的姿态及其运动。 一般而言， 当人们处于站立状态时，全脚掌着地， 向前后移动时分别用脚尖与脚跟先着地。同样对机器人，根据机器人脚与地 面的接触方式与用力的不同，可分为脚掌触地相、脚跟触地相、脚尖触地相。 当系统处于脚掌全触地相时，符合zmp 姿态稳定判据要求；当处于脚跟触 地相和脚尖触地相出现了欠驱动翻转情形时，zmp 位于支撑域凸多边形的 边界上， zmp 姿态稳定判据便不再适用。脚跟触地相姿态稳定类似于脚尖触 地相，本文对脚尖触地相欠驱动姿态进行研究。双足机器人的研究主要分为 全脚掌着地与一个点着地的情形。其中全脚掌着地时用zmp 控制1，一个 点着地时相当于人站立时脚尖着地或者脚跟着地的情形，这时踝关节便没有 提供驱动力，相当于前驱动 驱动量个数少于自由度个数。 与其它移动机器人 (轮式、履带式、爬行式等 )相比，双足机器人具有 高度的适应性与灵活性。由于双足机器人与地面接触点是离散的，因此可以 选择合适的落脚点来适应崎岖的路面，它既可以在平地行走，也可以在复杂 的非结构化环境中行走，比如：通过狭窄空间、跨越复杂障碍、在台阶、斜 坡和其他不平整地面上行动自如等。更重要的是，人们模仿自身行走方式设 计的双足机器人，最适合在人类生活和工作的环境中与人类协同工作，绝不 需要专门为其对环境进行大规模的改造。因此，人们对双足机器人寄予了很 大的期望：照顾老弱病人的护理人、与人类开展足球比赛的运动员，以及在 核电站、深海等危险环境中作业，甚至是与人类一起探索外星的伙伴。 最后，也可以通过对双足机器人 的研究平台来检验控制理论及控制仿发的 有效性、实用性、完整性、延伸性，即对控制理论有着检验和向前走得更远 的指示作用。 由于对双足机器人的研究具有十分重大的理论意义和实际价值，世界各国 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 4 都给予高度重视，先后推出了各自的研究计划。其中，日本在该领域的研究投 入力度最大，美、法、德、韩等国也相继启动了各自的研究计划。在国家高技 术研究发展计划(863 计划)和国家自然科学基金的支持下，我国众多高校和研究 机构也将双足机器人的研究和开发列入研究重点。 1.2 双足机器人的国内外研究状况 近年来，世界各国的科学家和研究者在双足机器人的稳定性能、步行移动 控制、样机研制等方面进行了许多有意义的探索。本节将从双足机器人的研究 状况和欠驱动行走双足机器人两个方面，来回顾双足机器人的发展历程。 1.2.1 国外研究状况 就机器人的研究历程来看，美国是最早研究的国家，但后来日本将半导 体与机械技术结合，从而在机器人这一领域超过了美国，位居世界主流。目 前，由日本研究治造的机器人大约占世界的80%。双足机器人的研制开始 于这个世纪的六十年代，虽然只有四五十年的历史，然而双足机器人的研究 工作进展十分迅速。国内外很多专家人士从事双足机器人的研究，如今已成 为机器人技术研究领域的主要方向之一。国际上许多国家都投入很大精力对 双足机器人进行研究，但各个国家都有着自己侧重的研究方向：日本偏向于 模拟人的动作与生活特性；欧洲偏向于从疗服务方面着手；美国则主要从军 事方面去考察研究。 1.日本的研究概况 （1）日本本田公司 在一九八六年日本本田公司科研人员开始对仿人机器人进行研究开发,本 田公司对人们在生活中的动作特性，对机器人朝着“机动性” 、 “智能性” 这个方向进行探索研究，他们的理念是： “机器人可以与人类互不侵犯的 相处，让机器人替代人做事情并完成人类不能完成的事情，这样可以从新的 领域为人类创造出更多的新财富。”并于 1986 年至 1993 年间先后研制出 e（experimental model）系列的实验型机器人 e1e6,，并都以双足进行 移动，他们的目的在于对双足机器人的理论方面进行更深入的探索及实现， 并运用了地面反力控制、 zmp 控制、摆动腿落地位置控制来完成双足机器 人的行走 3。从 1993 年开始，本田公司又着手对 p（prototype model） 系列 p1、p2、p3 仿人机器人进行研究，与 1996 年成功研制出 hodon 第二 代仿人机器人 p2 样机（如图 1-1a 所示） ，身高 182cm，体宽 60cm，体重 210kg，共有 30 个自由度，同时 p2 的推出成为世界上第一台无缆的防人机 器人，它能完成以一定的速度行走及上下楼梯等动作。第二年，本田公司又 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 5 在 p2 的基础上推出了 p3（如图 1-1 所示） ，它在外形上与 p2 相似，能在 斜面上行走和单腿站立。 于二零零年，日本本田公司又推出了asimo 型防人机器人（如图 1- 1c 所示） ，高 120cm，体重 43kg，共有 26 个自由度。 asimo 是 advanced step in innovative mobility 的简称，能与互联网进行无线链接，具有 语音识别和听说的功能。由于asimo 运用了新型开发的双足步行技术，可 以更加自由顺畅的行走，其速度甚至可达2.5km/h10。此外，由于 2005 年推出新一代的 asimo 防人机器人，高 130cm，体重 54kg，一共有 34 个自由度。与起初相比，新型asimo 可以与人一起受牵手走路，实现手 推车与搬运东西的功能，强化了机器人的运动能力。 图 1-1hodon 仿人机器人 .fig1-1 the hodon huamoid robot （2）日本索尼公司 索尼公司于 2000 年 11 月公布了娱乐型防人双足机器人sdr-3x，它 是 sony dream robot 的简写。其规格为身高 50cm，体宽 22cm，重 5kg， 属于小型化的机器人，共含有24 个自由度。 sdr-3x 能够实现以下几种动 作功能：第一，可以达到 14m/min 的速度行驶，并能朝前后左右四个方向 移动；第二，可以实现左右九十度的异步转身；第三：可以在斜面上行走及 完成单腿站立；第四，能够在平面上完成仰卧和府卧装填；第五，能完成踢 球、跳舞娱乐动作。 2003 年，索尼公司又推车了最新型的sdr 型防人机 器人，即 sdr-4xii 防人机器人（如图 1-2 所示） ，它是 sdr-4x 的升级。 sdr-4xii 身高 58cm，体重 7kg，一共含有 38 个自由度。现在，它的名字 改称为 qriq。由于 qriq 的驱动器将电机与控制电路集于一起，并加以空中 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 6 姿态的一些控制算法，成为一台能在 斜面上走动、能跑能跳的双足机器人， 给人们带来了乐趣和愉悦。 图 1-2 索尼仿人机器人 sdr-4xii fig.1-2the sony humanoid robot sdr-4xii （3）日本产业技术综合研究所 hpr 系列的研制 于一九九八年，日本产业技术综合研究所开始对仿人机器人进行研究， 并以本田公司 p3 为原型，经过五年时间，研制出hrp-2（hpr 是 humanoid robotics project 的简称） 。hrp-2 身高 155cm，体重 54kg， 含有 32 个自由度，能达到 2 千米/小时的行走速度。有特点的是， hrp-2 在腰部增加了两个自由度，是其在绊倒时可以像人一样爬起，与本田公司研 制的 asmio 相比，身体上部更具有灵活性。同时能实现在不平地面上行走及 在空中翻倒与恢复技术。 2005 年，日本产业综合研究所及川田工业联合研 制出了 hrp-3 防人机器人。该机器人具有防水防尘功能，在10cm/h 的雨 中也能工作，并且在易滑路面也能以大约0.4m/s 的速度行走。目前， hrp 项目更致力于应用技术上研究，已经实现与人配合抬桌椅、开铲车等功 能。 2.美国研究概况 早在 1948 年，美国阿贡实验室研制了工业机器手，主要用于一些比较 危险的境况，代替人类对危险性材料进行操作，减少工作人员的伤害。后来 于一九九八年，麻省理工学院研制出名为cog 的仿人机器人，它利用轮子 滚动方式行走，该机器人被很多科学家称作是研究智能算法的优越平台。 美国俄核俄大学郑元芳博士利用神经网络算法实现了双足机器人的步态 规划，并研制出双足机器人sd-1、sd-2，其中 sd-1 含有 4 个自由度， 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 7 sd-2 含有 8 个自由度。 1986 年 sd-2 机器人实现了前后、左右方向的行 走，并在 1990 年通过修正，实现了斜 面上的行走。 w.t.miller 在 sd-2 机器人基础上，增加了膝关节，并引入cmac 神经网络算法到机器人的行 走过程中，实现了周期性的稳定步态。另外， j.e.pratt 和 ga.partt 等人在研究 spring flamingo 双足机器人的过程中提出了虚拟模型控制策 略 vmc，对双足机器人的研究制造做了大量有益的工作。 3.韩国研究概况 于二零零二年一月，韩国先进科学研究所o.junho 教授正式开始对智 能机器人的研发，经过两年，研制出能行走的智能机器人khr-1。khr-1 身高 120cm，体重 48kg，共含有 21 个自由度。在 2003 年得 12 月，又研 制出 khr-2 的机身，经过进一步修正，于2004 年 8 月 khr-2 实现了撤线 行走功能，其中 khr-2 共有 41 个自由度，身高 120cm，体重 54kg。2004 年，科学技术院又成功研制了 “hubo”双足仿人机器人（如图 1-3 所示） ， 该机器人身高 1.25m，体重 55kg， 41 个关节能够轻柔转动，其中5 个手 指能够单独地活动，可以与人握手，跳一些简单的舞蹈。 图 1-3 khr_2 和 hubo 仿人机器人 fig .1-3the khr_2 and hubo humanoid robot 除了上述国家及单位外，还有许多机构也对双足机器人进行了深入的探 索研究，如德国的慕尼黑技术大学研制的johnnie 双足机器人，法国的 bip2000 机器人项目，英国对双足机器人的shadow 项目等，他们都对仿人 双足机器人的算法及实现作了研究，并取得重大的成果。 1.2.2 国内研究状况 我国从上个世纪八十年代初许多高校开始对双足机器人进行研究，而后 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 8 并将双足机器人的研究列入863 计划及国家自然科学基金的重要项目， 并给予很大支持。 早在 1986 年，哈尔滨工业大学开始对防人双足机器人进行研究，在傅 佩堔教授的带领下成功研制了hit-i 双足静态步行机器人，身高1.1 米， 体重 70 千克，共有 10 个自由度，能够是平地的前后左右行走及上下阶梯 运动，不幅达 0.45 米，步速约 10 秒/步。在 hit-1 的基础上，后来又成 功地研制了 hit-ii、 hit-iii，它们在步长和步速上都有了较大的改善。 于 2004 年，哈工大成功研制了可以踢足球的小型机器人；在2005 年，研 制了中国第一台具有面部表情的防人机器人。 国防科技大学于一九九八年成功地研制了双足机器人kdw-i，它具 有 6 个自由度，能前后行走、上下阶梯，最大步幅可达0.4m，步速可达 4 秒/步。第二年，又研制出 kdw-ii，高 0.69 米，重 13 千克，共有 10 个自由度，在 kdw-i 的功能基础上实现向左右方向的移动。 1990 年，在 kdw-ii 基础上增加了两个垂直关节得到kdw-iii，进一步实现转弯功能。 经过深入的研究，于 2000 年底成功研制出国内首台仿人双足机器人，取名 为“先行者” （如图 1-4 所示） 。 “步行者”高 140 厘米，体重 20 千克，可 以在具有一定偏差及不确定的环境中行走。2003 年又推出第四代仿人机器 人，该机器人具有新型的运动特性并实现了无缆行走 45。 图 1-4 hit-iii、hit 足球机器人、 “先行者” fig .1-4 hit-iii、hit football robot 、 forthgoer 北京理工大学在归国博士黄强教授的带领下，于2002 年成功研制出 仿人机器人 bhr-1（如图 1-5 所示） 。该机器人身高 1.58 米，重 76 千克， 共具有 32 个自由度，步幅可达 0.33 米，步速达每小时 1 千米。该机器人 具有多项功能，除了能前后、左右行走，完成转弯、上下阶梯外，还具有视 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 9 觉、力觉、语音觉、平衡觉等功能， 利用这些功能可以实现在未知地面的 情况下的平衡调节及完成太空拳等表 演动作。在 bhr-1 的基础上于 2005 年成功研制了 bhr-2（简称“汇童” ） ，它身高 160 厘米，体重 63 千克。 在几本功能上， bhr-2 突破了一些复杂的综合设计，能实现模仿人打太极拳、 耍刀术等复杂动作，这标志着我国研制机器人的水平有了很大的提高和突破， 并在国际上成为第二个探索出无缆行走，将控制、机构、电源、传感器集为 一起的国家。 图 1-5 bhr_1、bhr_2、thbip_1 fig.1-5 bhr_1、bhr_2、thbip_1 此外，1999 年上海交通大学研制出 sfhr 机器人，共有 46 个自由度， 步长 10 厘米，周期 3.5 秒。该机器人装有一个三轴倾斜与两个单轴陀螺， 用来检测机器人的姿态变化信息，并配有主动视觉系统，是研究机器人学、 控制算法的很好平台。在 985 计划的支持下，清华大学研制出thbip-i 防人机器人，高 170 厘米，体重 130 千克，共含有 32 个自由度。后来， 又研制了 thbip-2，并可以快速移动。南京航天航空大学曾研制了 naiwar_1,共有 8 个自由度，具有静态步行功能。除了上述高校外，国内还 有很多高校与单位都对双足机器人有深入的研究，并取得重大的研究成果。 1.2.3 欠驱动双足机器人 欠驱动双足机器人的发展历程 欠驱动双足机器人是为了研究高速动态步行而提出的一种机器人结构。由 于现实世界的地面情况是很难事先精确获取的，这就需要双足机器人的支撑踝 关节能够柔顺，适应未知地面。当支撑踝关节的控制力矩为零时，双足机器人 与地面间达到完全柔顺，其模型近似等价于踝关节完全被动的无脚双足机器人。 无脚动态步行是双足步行领域的研究难点之一，当机器人没有脚时，机器人支 撑腿与地面间无法输入控制力矩，因而机器人不能实现静态步行，必须通过不 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 10 断的迈步实现动态行走。 最早对欠驱动机器人进行研究的是 mit 的 leg laboratory。spring turkey (图 1-6) 是其 1994 到 1996 年制作的平面型机器人，具有主动的膝和 髋关节，高 60cm，重 10kg，通过无驱动横杆保持侧向平衡。 1997 年，几个法国国家实验室在法国科学研究中心(cnrs)和法国国家研究 委员会(fnrs)的资助下，启动研制平面欠驱动双足机器人的计划；1998 年美国 密歇根大学 grizzle 等人加入到该项目中，为该项目提供控制理论方面的指导 68；2003 年该研究小组研制出样机 rabbit (图 1-7)， rabbit 是一个平面型 双足机器人，由两个髋关节和两个膝关节组成，与地面点接触，通过无驱动长 杆保持侧向稳定。该机器人高 1.425 米，重 36 公斤，没有脚，前进过程中每只 脚相当于一点着地，侧面连接一根长杆防止其侧翻，但不控制其向前的运动， 机器人沿圆周行走。 图 1-6spring turkey 图 1-7 rabbit fig.1-6spring turkey robot fig.1-7rabbit robot 目前国内对欠驱动双足机器人的研究还比较少，主要有清华大学、国防科 技大学等。 欠驱动双足机器人特点及研究意义 欠驱动双足机器人是一个崭新的研究平台，相比全驱动双足机器人，具有 如下鲜明的特点： 1) 能效较高，速度较快。 全驱动机器人采用大力矩和高增益反馈的驱动系统以控制和改变机器人的 动力学特性，普遍存在能耗大等问题；虽然被动式双足步行机器人在能效方面 表现最出色，但系统不可控，实际应用价值不明显。 欠驱动双足机器人踝部没有驱动关节，机器人与地面点接触处于被动状态， 其他内关节为主动关节，机器人充分利用重力和惯性力的驱动运动，是一种介 于被动和全驱动机器人之间的机器人，有与人类步行相似的能量效率，速度较 快，具有很高的应用价值。 2) 扩展双足步行关节运动模式。 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 11 由于运动规划和控制方式的局限性，全驱动双足机器人普遍存在动作僵硬 问题；人类在行走过程中应用了多种不同的驱动模式，包括无驱动、欠驱动、 完全驱动、冗余驱动等。 欠驱动双足机器人强调充分利用机器人自身固有的被动动力学特性，这种 特点使得欠驱动双足行走机器人一方面具有较为自然的机械步态；另一方面也 有利于近一步揭示双足步行运动的机理及控制规律。 3) zmp 稳定判据不再适用。 欠驱动双足机器人需要不断变换支撑点来获得动态平衡，体现了支撑点的 变化性和离散性，更符合双足运动的特点。欠驱动双足机器人的研究将拓宽双 足机器人的稳定判定理论。 1.3 欠驱动双足机器人控制存在的问题 欠驱动双足机器人研究虽然在实验室和理想环境下已经取得了一些成果， 但要双足机器人实现象人类一样的真正动态和稳定的快速行走，还存在一系列 问题，主要有： 1) 双足步行系统模型还不完善。 目前欠驱动双足步行系统模型对足与环境间的动态接触多假设为刚性接触， 并未考虑接触点间的局部信息，而在动态步行过程中，双腿相互支撑转换时与 地面频繁接触，影响和破坏机器人行走稳定性。因此，考虑足与地面间的碰撞 冲击是双足步行系统建模中的重要组成部分。 2)欠驱动双足机器人的稳定性存在依赖 正如 rabbit 欠驱动双足机器人，当机器人与地面点接触时，通过无驱动长 杆保持侧向稳定，借助侧面连接一根长杆防止其侧翻，控制机器人沿圆周行走， 这时的稳定性在一定程度上依赖于侧杆的作用。 3) 双足机器人的仿真平台不具有普适性。 很多研究机构都开发了相应的双足机器人仿真平台，由于大多数的仿真平 台是专用的，在机器人结构等发生改变或者其他研究者需要借用时，往往需要 大量修改代码，因此其适用范围受到限制。 综上所述，根据人类生活的特性和步行的特点建立一种恰当的双足机器人 模型，利用有效的控制方法使得欠驱动双足机器人平衡得到有效控制，这将对 对双足机器人的研究有重大的作用，并借助更普适性的仿真平台来仿真研究， 为更多的研究者所接受。 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 12 1.4 本文的研究工作 本文首先需要分析双足机器人的欠驱动姿态，建立双足机器人的物 理模型；其次需要从能量和力的角度对物理模型加以数学描述，得出 数学 模型；同时还需要分析欠驱动姿态的可控性与能观测性，并借鉴 单杠体 操机器人在倒立平衡位置的控制 2，利用线性二次调节器 lqr 实现 双足机器人的欠驱动直立平衡。 仿真实现部分：结合matlab 的符号运算，搭建了simulink 控制 器模块 ,得出三节杆的角度与角速度的收敛曲线；通过分析，仿真结果曲 线的稳定性证明利用线性二次型调机器lqr 能够实现对双足机器人欠 驱动姿态的平衡控制；同时双足机器人是欠驱动多输入多输出复杂系统 的典型例子，通过研究双足机器人在欠驱动下的平衡控制，可以很好的 检验控制理论和控制方法在非线性、欠驱动、多变量复杂系统控制上的 有效性。 1.5 论文的构成 第 1 章为绪论部分，介绍了课题的研究背景和意义、双足机器人的国内 外研究状况，立足于双足机器人欠驱动姿态的平衡控制这个出发点去研究与 利用线性二次型调机器控制的方法。 第 2 章，相对于全脚掌着地的 zmp 控制，分析了双足机器人的欠驱动 姿态，建立三连杆物理模型；其次从能量和力的角度对物理模型加以数学描 述；并对仿真模型 matlab 符号运算及 simulink 模块进行介绍。 第 3 章，对双足机器人平衡的控制目标进行了分析，由于双足机器人系 统的欠驱动姿态是不稳定的，其中含有开环特征根位于s 右半平面，需加 以控制，所以对状态方程模型进行能控性与能观测性分析。 第 4 章，本章介绍了双足机器人的平衡控制策略：借鉴单杠体操机器人 在倒立平衡位置的控制，介绍线性二次型最有控制的几本原理，通过分析可 以建立状态方程利用 lqr 实现对双足机器人的欠驱动直立平衡控制。 第 5 章，本章是仿真实验部分。首先通过matlab 符号运算及函数的 功能，搭建控制器的 simulink 模块；其次是对仿真结果的有效性分析， 由仿真得出三根连杆的三个角度与三个角速度的变化曲线，收敛到零刻度平 衡位置，证明 lqr 控制器对双足机器人欠驱动姿态平衡控制的有效性。 第 6 章，本章是结论部分，一方面对本文作出了概括性地总结，另一方 面对双足机器人欠驱动姿态的平衡控制和其他研究领域的衔接及进一步研究 提出展望。 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 13 2 双足机器人直立平衡控制 的模型研究 2.1.双足机器人的欠驱动姿态 2.1.1 模型简化的提出 关于双足机器人的直立平衡与移动正在被国际上很多学者广泛的进行研 究14-21。研究的重要方法之一就是推导出双足机器人的一个最适合的精确 模型。因为最原始双足机器人模型是一个多变量、高阶次、变结构、非线性 和强耦合的复杂动力学系统，对如此复杂的系统进行研究是困难的，因此我 们必须在某种恰当的条件下进行转化和简化，当然这种转化和简化必须具备 真实可行性和复原性， 其简化模型如图 2-1-1 所示。 （a）简单的人体模型 （b）进一步简化的简单人体模型 图 2-1-1 人体简化模型 fig 2-1-1. body simplified model 研究表明：双足机器人在稳定件下，能够实现上半躯体与下半身躯体的 运动解藕，并且对下半身躯体的各个关节角和连杆很容易实施角度规划 22这为我们上面所提出的复杂模型提供了有效的理论依据 :我们便可以对 模型进行解耦，分别对上身躯与下身去进行控制，并对下身躯关节角度进行 轨迹规划。 2.1.2 双足机器人欠驱动姿态分析 我们知道，人在行走时以双腿交替的方式进行移动。当需要向前移动是， 其中一只脚为支撑，另一只脚向前迈出一步，迈出在空中的那只脚都有绕脚 尖旋转的过程，并且在落地的瞬间脚跟先着地，随后便是整只脚落地的过程。 当向后移动时，同理支撑脚保持不动，另一只脚向后退一步，退后的那整 只脚在空中有绕脚后跟旋转的过程，这时是脚尖先着地，然后整只脚落地。 其整个过程如图 2-1-2 所示。 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 14 （a）双脚着地 （b）人向前移动 （c）人向后移动 图 2-1-2 人体站立及前后移动与地接触状态 fig.2-1-2 people stand up and move back and forth with access to state 在平面上，用单腿连杆来表示双足机器人的姿态模型，躯干、大腿和小 腿、脚都分别用一连杆来表示，髋关节、膝关节及踝关节是驱动关节，并且 它们之间无摩擦。假设机器人的脚与水平地面之间的摩擦力有足够的大，没 有滑动。按机器人脚与地面间的接触情况，可分为脚跟触地相、脚尖触地相 和脚掌全触地相三种状态 23，如图 2-1-3 所示。 脚尖触地相脚掌全触地相脚跟触地相 髋关节 膝关节 踝关节 r q a q k q h q 图 2-1-3 双足机器人脚地接触状态 fig. 2-1-3 biped robot foot contact state 当系统处于脚掌全触地相时，符合zmp 姿态稳定判据要求；当处于脚 跟触地相和脚尖触地相出现了欠驱动翻转情形时，zmp 位于支撑域凸多 边形的边界上， zmp 姿态稳定判据便不再适用 1。脚跟触地相姿态稳定类 似于脚尖触地相，本文对脚尖触地相 双足机器人的 欠驱动姿态进行研究。 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 15 2.2 物理模型 人体站立受扰 后恢复平衡的实验表明，站立姿态平衡 的恢复是多关节 协调运动过程，在平衡恢复过程中 ,髋关节对平衡恢复的贡献最大，膝关节 最小24，因此本文将大腿和小腿视作一体，并将机器人各组成部分看作由 具有质量的刚性杆连接而成，分别将脚掌、大腿和小腿、躯干各视为一杆。 这样，就可以将前向平面内具有欠驱动自由度的双足机器人系统看作三关 节体操机器人系统，仿人机器人的踝、髋关节分别对应体操机器人的肩、髋 关节，并将姿态稳定控制问题转化为垂直向上不稳定平衡点的控制问题，模 型如图 2-2 所示。 1 3 2 l1 l3 l2 l1 l3 l2 u2 u1 c3 c2 c1 j2 j1 j3 图 2-2 双足机器人欠驱动姿态模型 fig.2-2 biped robot underactuated attitude model 假设，三根杆的长度分别为 l1，l2，l3；质心轴心距为 l1，l2，l3；质量为 m，m2，m3；以质心为转动轴时的转动惯量为 j1，j2，j3；三杆与垂直方向的夹 角分别为，以倒立位置为零度，顺时针方向为正；两个主动关节 321 , 上的电机质量为 mc1，mc2；三个关节上的摩擦力矩系数为 c1，c2，c3；两个主 动关节上的力矩为 u1，u2，顺时针为正。本文其它地方变量定义与此相同。 在研究中我们按人体环节统计参数27的比例特征设计了机器人的各肢体参 数，考虑到后期的实物制作以及模仿人的结构，我们把电机及编码器部分设计 在了身体内部，因此参数中均为 0，其参数如表 2-3 所示。 21 mcmc， 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 16 表 2-3 双足机器人模型参数 table .2-3biped robot model parameters 参数参数长度长度mm 质量质量 kgkg 质心距质心距 mm 转动惯量转动惯量 kgmkgm2 2 摩擦力矩系摩擦力矩系 数数nn .ms.ms 脚 033 0.03056 腿 0.6300.30.9 0.03056 躯干 0.6300.30.9 0.03056 2.3 数学模型 建立双足机器人的三关节数学模型时，可以把机器人看成是由一个个关 节连接起来的刚体。建立机器人动力学模型，主要采用下述两种理论 25： 动力学基本理论，如牛顿 欧拉方程； 拉格朗日 力学，特别是二阶 拉格朗日 方程。 基于这两种理论所建立的动力学模型都是等价的，只是描述形式不同， 因而在分析和设计方面存在差异。如牛顿 欧拉方程的方法，需要从动 力学出发求得加速度，并消去各内作用力，这对于复杂的系统就显得十分繁 杂。而基于 拉格朗日 力学的方法是从系统能量的角度出发来考虑的，它只 需计算速度而不必求内作用力，因此这是一直比较简洁的方法。 本文采用 拉格朗日 力学的方法建立三关节双足机器人的动力学模型， 也就是基于机器人运动时的能量来进行的。这种方法仅需计算机器人的动能 和势能，因而与牛顿 欧拉方程的方法相比更加简洁，而且能够充分反 映机器人动力学结构特征。 三关节双足机器人的拉格朗日方程为 3 , 2 , 1 iu x d x v x t x t dt d i iiii (2.1) 其中， t x 321 为系统所受驱动力； t uuu0 21 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 17 2 1 2 1 2 1 3 1 2 1 2 2 1 2 2 sin cos 2 1 coscos sinsin 2 1 i ik kk i ik kk i i i ii i ik kk ii i ik kk iii l l mc ll ll mjt .（2.2） 为系统在广义坐标下的动能； 2 11 3 1 1 2 coscoscos i i ik kki i i ik kkiii lgmcllgmv . （2.3） 为系统的势函数； . 3 1 2 1) ( 2 1 i iii cd （2.4） 为关节摩擦所消耗的能量； 将以上动能、势能和能量损失代入拉格朗日方程（式2.1） ，得到系统 的非线性动态方程如下： .dxcxxxbxxa)(),()( （2.5） 其中， ， 3323231313 2323221212 1313121211 )cos()cos( )cos()cos( )cos()cos( )( aaa aaa aaa xa 3322323311313 32323322112122 3131321212211 )sin()sin( )sin()sin( )sin()sin( ),( bbcb bcbbc bbcb xxb ， ， 33 22 11 sin sin sin )( c c c xc 2 1 0 u ud 是与模型参数有关的常量，具体表达式如下： iii cba， 2 121321 2 1111 )(lmcmcmmjlma ， 12232212 )(llmcmlma 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 18 13313 llma ， 2 2232 2 2222 )(lmcmjlma ， 23323 llma ， 3 2 3333 jlma )( 2111 ccb ， 12232212 )(llmcmlmb ， 13313 llmb )( 32 22 ccb 23323 llmb ， 333 cb , glmcmcmmlmc)( 12132111 ， glmcmlmc)( 223222 ， glmc 333 显然三关节双足机器人系统是典型的多输入(u1，u2)多输出()的非 321 , 线性多变量系统。 2. 4 matlab 仿真模型 作为 matlab 的重要组成部分， simulink 是对动态系统进行建模、仿 真和分析的一个软件包。它支持线性和非线性系统、连续时间系统、离散时 间系统、连续和离散混合系统，而且系统可以是多进程的。 simulink 提供了友好的图形界面（ gui） ，模型由模块组成的框图来表 示，用户建模可以通过简单的点击和拖动鼠标的动作就能完成。simulik 的模块库为用户提供了多种多样的的功能模块。其中基本功能模块有连续系 统（continuous） 、离散系统（ discrete） 、非线性系统（ nolinear）几 类基本系统构成模块，还包括连接、运算模块：函数与表（functions ang=0 0 0; vel=0 0 0; model_para=0.0033, 0.9, 0.9, 1, 30, 30, 0.1, 0.3, 0.3, 0.2, 0.6, 0.6; j1 = model_para(1); j2 = model_para(2); j3 = model_para(3); m1 = model_para(4); m2 = model_para(5); m3 = model_para(6); l1 = model_para(7); l2 = model_para(8); l3 = model_para(9); l1 = model_para(10); l2 = model_para(11); l3 = model_para(12); g=9.8; a11 = j1+j2+j3+m1*l1*l1+(m2+m3)*l1*l1+m2*(l2*l2+2*l1*l2*cos(ang(2)+m3*(l2 *l2+l3*l3+2*l1*l2*cos(ang(2)+2*l1*l3*cos(ang(3)+ang(2)+2*l2*l3*cos( ang(3); a12 = j2+j3+m2*(l2*l2+l2*l1*cos(ang(2)+m3*(l2*l2+l3*l3+l1*l2*cos(ang(2)+ l1*l3*cos(ang(3)+ang(2)+2*l2*l3*cos(ang(3); a13 = j3+m3*(l3*l3+l1*l3*cos(ang(3)+ang(2)+l2*l3*cos(ang(3); a21 = j2+j3+m2*(l2*l2+l1*l2*cos(ang(2)+m3*(l2*l2+l3*l3+l1*l2*cos(ang(2)+ l1*l3*cos(ang(3)+ang(2)+2*l2*l3*cos(ang(3); a22 = j2+j3+m2*l2*l2+m3*(l2*l2+l3*l3+2*l2*l3*cos(ang(3); a23 = j3+m3*(l3*l3+l2*l3*cos(ang(3); a31 = j3+m3*(l3*l3+l1*l3*cos(ang(3)+ang(2)+l2*l3*cos(ang(3); a32 = j3+m3*(l3*l3+l2*l3*cos(ang(3); a33 = j3+m3*l3*l3; b11 = 0; b12 = m2*(l1*l2*sin(ang(2)*(2*vel(1)+vel(2)+m3*(l1*l2*sin(ang(2)+l1*l3* sin(ang(2)+ang(3)*(vel(2)+2*vel(1); b13 = 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 37 m3*(l1*l3*sin(ang(2)+ang(3)+l2*l3*sin(ang(3)*(2*vel(1)+2*vel(2)+ve l(3); b21 = - (m2*l1*l2*sin(ang(2)+m3*(l1*l2*sin(ang(2)+l1*l3*sin(ang(2)+ang(3)* vel(1); b22 = 0; b23 = m3*l2*l3*sin(ang(3)*(2*vel(1)+2*vel(2)+vel(3); b31 = -(m3*(l1*l3*sin(ang(2)+ang(3)+l2*l3*sin(ang(3)*vel(1); b32 = -m3*l2*l3*sin(ang(3)*(2*vel(1)+vel(2); b33 = 0; c11=(m1*l1+m2*(l1+l2)+m3*(l1+l2+l3)*g; c12=(m2*l2+m3*l2+m3*l3)*g; c13=m3*g*l3; a=1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 a11 a12 a13 0 0 0 a21 a22 a23 0 0 0 a31 a32 a33; bi=0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 c11 c12 c13 b11 b12 b13 0 0 0 b21 b22 b23 0 0 0 b31 b32 b33; ub=0 0; 0 0; 0 0; 0 0; 1 0; 0 1; ia=inv(a); a=ia*bi; b=ia*ub; q= 1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; r=1 0 0 1; k=lqr(a,b,q,r) （2）lc 部分：/把上面算出的 k 用于计算输出，作为系统模型 gymsys2.m 的 重庆理工大学毕业论文 双足机器人系统直立平衡控制设计 38 u 输入（3 杆有 2 个输入）. function sys,x0 = lqr(t,x,u,flag) switch flag, case 0, sys=0 0 2 6 1 1; x0=; case 3, k1=1000*-4.3096 -3.1490 -0.8530 -1.3333 -1.0380 -0.3012 ; k2=1000*-3.6513 -2.8012 -0.6414 -1.1297 -0.8835 -0.2472 ; y1=k1(1)*u(1)+k1(2)*u(2)+k1(3)*u(3)+k1(4)*u(4)+k1(5)*u(5)+k1(6)*u(6); y2=k2(1)*u(1)+k2(2)*u(2)+k2(3)*u(3)+k2(4)*u(4)+k2(5)*u(5)+k2(6)*u(6); sys=-y1;-y2; sys; otherwise sys=; end （3）gymsys部分： / 是模型部分，含拉氏变换、及其矩阵表示，最后输出六 个量，包含三个角度与三个角速度. function sys,x0,str,ts = gymsys(t,x,u,flag) global model_para; model_para=0.0033, 0.9, 0.9, 1, 30, 30, 0.1, 0.3, 0.3, 0.2, 0.6, 0.0253762, 0.0253762, 0.0253762; switch flag, case 0, sys,x0,str,ts=mdlinitializesizes; case 1, sys=mdlderivatives(t,x,u); case 3, sys=mdloutputs(t,x,u); otherwise, sys=; end function sys,x0,str,ts=mdlinitializesizes sizes = simsizes; sizes.numcontstates = 6; sizes.numdiscstates = 0; sizes.numoutputs = 6; sizes.numinputs = 2; sizes.dirfeedthrough = 1; sizes.numsampletimes = 1; sys = simsizes(sizes); x0 = 0.1*pi/180 0 0 0 0 0 ; str = ; ts = 0