基于逆动力学方法的关节型机器人轨迹控制

第４１卷　增刊Ⅰ ２０１３年　１０月　 华 中 科 技 大 学 学 报 （自 然 科 学 版） Ｊ．Ｈｕａｚｈｏｎｇ　Ｕｎｉｖ．ｏｆ　Ｓｃｉ．＆Ｔｅｃｈ．（Ｎａｔｕｒａｌ　Ｓｃｉｅｎｃｅ　Ｅｄｉｔｉｏｎ） Ｖｏｌ．４１Ｓｕｐ．Ⅰ 　Ｏｃｔ．　２０１３ 收稿日期　２０１３－０７－１０． 作者简介　陈培华（１９８５－），男，博士研究生，Ｅ－ｍａｉｌ：ｃｐｈ＠ｓｊｔｕ．ｅｄｕ．ｃｎ． 基金项目　国家高技术研究发展计划资助项目 （２００７ＡＡ０４１６０２，２０１１ＡＡ０４０８０１，２０１２ＡＡ０４１４０１）． 基于逆动力学方法的关节型机器人轨迹控制 陈培华ａ　曹其新ａ，ｂ （上海交通大学ａ机器人研究所；ｂ机械系统与振动国家重点实验室，上海２００２４０） 摘要　在已知机器人关节空间中起点和终点时刻运动参数的情况下，研究了基于五次多项式插值拟合的关 节型机器人轨迹规划算法，获取了机器人运动中各个时刻所对应的关节位置、速度和加速度等数据信息．分析 了基于迭代牛顿－欧拉算法的机器人逆动力学方程，依据规划好的机器人轨迹求取用于控制机器人运动或进 行动力学仿真所需的驱动力矩，并建立了基于逆动力学方程的机器人轨迹控制系统．最后，比较了三种关节型 机器人的轨迹控制方法并进行了相应的动力学仿真，对轨迹跟踪误差和力矩大小等性能参数进行比较， 证明 住所证明下载场所使用证明下载诊断证明下载住所证明下载爱问住所证明下载爱问 了基于ＲＮＥＣ和加速度反馈的机器人轨迹控制系统具有较高的跟随精度、较明显的轨迹控制效果，是一种较 优的力矩控制方法． 关键词　关节型机器人；逆动力学；动力学仿真；轨迹规划；轨迹控制；关节空间 中图分类号　ＴＰ２４２　　文献标志码　Ａ　　文章编号　１６７１－４５１２（２０１３）Ｓ１－００１７－０４ Ｔｒａｊｅｃｔｏｒｙ　ｃｏｎｔｒｏｌ　ｏｆ　ｔｈｅ　ａｒｔｉｃｕｌａｔｅｄ　ｒｏｂｏｔ　ｂａｓｅｄ　ｏｎ　ｉｎｖｅｒｓｅ　ｄｙｎａｍｉｃｓ Ｃｈｅｎ　Ｐｅｉｈｕａａ　Ｃａｏ　Ｑｉｘｉｎａ，ｂ （ａ　Ｒｅｓｅａｒｃｈ　Ｉｎｓｔｉｔｕｔｅ　ｏｆ　Ｒｏｂｏｔｉｃｓ；ｂ　Ｔｈｅ　Ｓｔａｔｅ　Ｋｅｙ　Ｌａｂｏｒａｔｏｒｙ　ｏｆ　Ｍｅｃｈａｎｉｃａｌ　Ｓｙｓｔｅｍ ａｎｄ　Ｖｉｂｒａｔｉｏｎ，Ｓｈａｎｇｈａｉ　Ｊｉａｏ　Ｔｏｎｇ　Ｕｎｉｖｅｒｓｉｔｙ，Ｓｈａｎｇｈａｉ　２００２４０，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ　Ｔｈｅ　ｔｒａｊｅｃｔｏｒｙ　ｐｌａｎｎｉｎｇ　ａｌｇｏｒｉｔｈｍ　ｏｆ　ｔｈｅ　ａｒｔｉｃｕｌａｔｅｄ　ｒｏｂｏｔ　ｂａｓｅｄ　ｏｎ　ｑｕｉｎｔｉｃ　ｐｏｌｙｎｏｍｉａｌｓ　ｉｎ ｔｈｅ　ｊｏｉｎｔ　ｓｐａｃｅ　ｗａｓ　ｓｔｕｄｉｅｄ　ｕｎｄｅｒ　ｔｈｅ　ｃｏｎｄｉｔｉｏｎ　ｏｆ　ｓｔａｒｔｉｎｇ　ｐｏｉｎｔ′ｓ　ａｎｄ　ｅｎｄｉｎｇ　ｐｏｉｎｔ′ｓ　ｍｏｔｉｏｎ　ｐａｒａｍｅ－ ｔｅｒｓ　ｋｎｏｗｎ，ａｎｄ　ｔｈｅ　ｃｏｒｒｅｓｐｏｎｄｉｎｇ　ｊｏｉｎｔ　ｐｏｓｉｔｉｏｎｓ，ｓｐｅｅｄｓ　ａｎｄ　ａｃｃｅｌｅｒａｔｉｏｎｓ　ａｔ　ｅｖｅｒｙ　ｍｏｍｅｎｔ　ｗｅｒｅ　ｏｂ－ ｔａｉｎｅｄ．Ｒｅｓｅａｒｃｈ　ｗａｓ　ａｌｓｏ　ｃａｒｒｉｅｄ　ｏｕｔ　ｏｎ　ｔｈｅ　ｒｏｂｏｔ　ｉｎｖｅｒｓｅ　ｄｙｎａｍｉｃｓ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ｉｔｅｒａｔｉｖｅ　Ｎｅｗｔｏｎ－Ｅｕｌ－ ｅｒ　ａｌｇｏｒｉｔｈｍ，ａｎｄ　ｔｈｅ　ｄｒｉｖｅｎ　ｔｏｒｑｕｅｓ　ｆｏｒ　ｒｏｂｏｔ　ｃｏｎｔｒｏｌ　ｏｒ　ｄｙｎａｍｉｃｓ　ｓｉｍｕｌａｔｉｏｎ　ｗｅｒｅ　ｃａｌｃｕｌａｔｅｄ　ａｃｃｏｒｄ－ ｉｎｇ　ｔｏ　ｔｈｅ　ｐｌａｎｎｅｄ　ｒｏｂｏｔ　ｔｒａｊｅｃｔｏｒｙ．Ｍｅａｎｗｈｉｌｅ，ｔｈｅ　ｒｏｂｏｔ　ｔｒａｊｅｃｔｏｒｙ　ｃｏｎｔｒｏｌ　ｓｙｓｔｅｍ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ｉｎ－ ｖｅｒｓｅ　ｄｙｎａｍｉｃｓ　ｗａｓ　ｅｓｔａｂｌｉｓｈｅｄ．Ｆｉｎａｌｌｙ，ｔｈｒｅｅ　ｋｉｎｄｓ　ｏｆ　ａｒｔｉｃｕｌａｔｅｄ　ｒｏｂｏｔ’ｓ　ｔｒａｊｅｃｔｏｒｙ　ｃｏｎｔｒｏｌ　ｍｅｔｈｏｄｓ ｗｅｒｅ　ｃｏｍｐａｒｅｄ　ａｎｄ　ｔｅｓｔｅｄ　ｉｎ　ｔｈｅ　ｄｙｎａｍｉｃｓ　ｓｉｍｕｌａｔｉｏｎ．Ａｃｃｏｒｄｉｎｇ　ｔｏ　ｔｈｅ　ｃｏｍｐａｒｉｓｏｎｓ　ｏｆ　ｔｈｅ　ｐａｒａｍｅｔｅｒｓ ｏｆ　ｔｒａｊｅｃｔｏｒｙ　ｆｏｌｌｏｗｉｎｇ　ｅｒｒｏｒ　ａｎｄ　ｄｒｉｖｅｎ　ｔｏｒｑｕｅ，ｉｔ　ｐｒｏｖｅｄ　ｔｈａｔ　ｔｈｅ　ａｒｔｉｃｕｌａｔｅｄ　ｒｏｂｏｔ　ｒａｊｅｃｔｏｒｙ　ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ｃｏｍｂｉｎａｔｉｏｎ　ｏｆ　ＲＮＥＣ　ａｎｄ　ａｃｃｅｌｅｒａｔｉｏｎ　ｆｅｅｄｂａｃｋ　ｈａｓ　ｈｉｇｈ　ｆｏｌｌｏｗｉｎｇ　ａｃｃｕｒａｃｙ　ａｎｄ ｏｂｖｉｏｕｓ　ｔｒａｊｅｃｔｏｒｙ　ｃｏｎｔｒｏｌ　ｅｆｆｅｃｔ，ｗｈｉｃｈ　ｉｓ　ａｎ　ｅｘｃｅｌｌｅｎｔ　ｔｏｒｑｕｅ　ｃｏｎｔｒｏｌ　ｍｅｔｈｏｄ． Ｋｅｙ　ｗｏｒｄｓ　ａｒｔｉｃｕｌａｔｅｄ　ｒｏｂｏｔ；ｉｎｖｅｒｓｅ　ｄｙｎａｍｉｃｓ；ｄｙｎａｍｉｃｓ　ｓｉｍｕｌａｔｉｏｎ；ｔｒａｊｅｃｔｏｒｙ　ｐｌａｎｎｉｎｇ；ｔｒａｊｅｃｔｏｒｙ ｃｏｎｔｒｏｌ；ｊｏｉｎｔ　ｓｐａｃｅ 　　当前，机器人的应用范围已经扩展到人类生 产和生活的各个方面，极大地影响和促进了工业、 国防和科技事业的发展［１－３］．但同时人们对机器 人的工作效率和运动平稳度的要求也日渐增高， 使得提高机器人的工作效率、优化机器人的轨迹 控制 方案 气瓶 现场处置方案 .pdf气瓶 现场处置方案 .doc见习基地管理方案.doc关于群访事件的化解方案建筑工地扬尘治理专项方案下载 成为机器人应用亟待解决的关键性问 题［４－６］．本课题针对机器人关节空间内的轨迹规 划和基于逆动力学方程的关节型机器人轨迹控制 的方法进行了研究，并建立了基于逆动力学控制 器和加速度反馈的关节型机器人的轨迹控制系 统，同时在二自由度关节型机器人上进行了仿真 验证，达到了较好的轨迹控制和跟踪性能并优化 了控制方案． １　机器人关节空间内的轨迹规划 在关节型机器人的运动过程中，当仅给定初 始点和目标位置定位点时，还必须对未给定位置 的区间设定适当的轨迹．因为机器人在任务空间 中的末端轨迹可以通过逆运动学转换为其在关节 坐标系下的运动轨迹，所以此处仅讨论机器人在 关节空间内的轨迹规划．为获得在关节空间内连 续、平滑的运动和轨迹，研究者通常采用抛物线、 三次、四次、五次等多项式插值拟合的方法［７－９］． 在自由度为ｎ的机器人关节空间中，若已知 机器人各个关节的初始位置θ０ 和终点位置θｆ （θ０，θｆ∈Ｒｎ），当机器人的运动轨迹满足边界条件 θ（０）＝θ０，θ（ｔｆ）＝θｆ； θ（０）＝θ０，θ（ｔｆ）＝θｆ； θ¨０ ＝θ¨ｆ＝ 烅 烄 烆 ０ （１） 时，可以用一个五次多项式函数插值拟合机器人 的运动轨迹 θ（ｔ）＝ａｔ５＋ｂｔ４＋ｃｔ３＋ｄｔ２＋ｅｔ＋ｆ， （２） 式中ａ，ｂ，ｃ，ｄ，ｅ，ｆ均为ｎ维向量． 分别代入约束条件（１）中，可以求得各变量的 值： ｆ＝θ０，　ｅ＝θ０，　ｄ＝０， ｃ＝ ［１０（θｆ－θ０）－（６θ０＋４θｆ）ｔｆ］／ｔ３ｆ， ｂ＝ ［－１５（θｆ－θ０）＋（８θ０＋７θｆ）ｔｆ］／ｔ４ｆ， ａ＝ ［６（θｆ－θ０）－３（θ０＋θｆ）ｔｆ］／ｔ５ｆ． （３） 　　将以上各值代入式（２）即可求得机器人的运 动轨迹，进而确定机器人在各时刻下各关节的位 置、速度和加速度等信息．采用五次多项式函数插 值拟合的机器人时间最优轨迹规划，可以提高关 节型机器人的工作效率． ２　关节型机器人的逆动力学分析 基于逆动力学方法的关节型机器人轨迹控 制，主要依靠计算关节型机器人各关节的驱动力 矩使机器人按照期望的轨迹运动．为了计算机器 人各关节的驱动力矩，通常有ＰＩＤ控制和逆动力 学计算等方法．本文采用计算复杂度为Ｏ（ｎ）的迭 代牛顿 －欧拉算法（ＲＮＥ），实现了关节型机器人 的快速力矩求解． 根据牛顿 －欧拉方程，描述关节型机器人中一 个连杆ｉ的动力学方程为 ｆΣｉ ＝ｍｉ¨ｃｉ； （４） τ（ｃ）ｉ ＝Ｉｉωｉ＋ωｉ×Ｉｉωｉ， （５） 式中：ｆΣｉ为作用在质心上的合外力；ｍｉ 为连杆的 质量；ｃｉ为连杆质心在世界坐标系中的位置；τｉ（ｃ） 为绕质心的外力矩；Ｉｉ和ωｉ分别代表连杆在世界 坐标系中绕质心的惯量矩阵和角速度． 由空间向量代数［１０－１１］，连杆在世界坐标系中 的速度ｖｉ＝ｃｉ－ωｉ×ｃｉ．将其微分后的结果代入式 （４），则有 ｆΣｉ ＝ｍｉ［ｖｉ－ｃｉ×ωｉ＋ωｉ×（ｖｉ＋ωｉ×ｃｉ）］． （６） 　　另外，作用于质心上的合力ｆΣｉ和τｃｉ 产生的 世界坐标系下的合力矩为 　τΣｉ ＝τｃｉ＋ｃｉ×ｆΣｉ ＝Ｉｉωｉ＋ωｉ×Ｉｉωｉ＋ｍｉ× ｃｉ×［ｖｉ－ｃｉ×ωｉ＋ωｉ×（ｖｉ＋ωｉ×ｃｉ）］． （７） 　　将式（６）和式（７）整理成矩阵形式，并用空间 速度矢量Ｘｉ＝［ｖｉ，ｗｉ］Ｔ 来表示 ［ｆΣｉ，τΣｉ］Ｔ ＝ＩＳｉ¨Ｘｉ＋Ｘｉ×ＩＳｉＸｉ， （８） 式中ＩＳｉ 是６×６的空间惯性对称矩阵， ＩＳｉ ≡ ｍｉＥ　 ｍｉ＾ｃ　Ｔｉ ｍｉ＾ｃｉ ｍｉ＾ｃｉ＾ｃ　Ｔｉ ＋［ ］Ｉ ． （９） 　　在关节型机器人的连杆系统中，作用于连杆 ｉ上的合力和合力矩可表示为 ｆΣｉ τΣ［ ］ｉ ＝ ｆｉ τ［ ］ｉ ＋ ｆＥｉ τＥ［ ］ｉ － ｆｉ＋１ τｉ＋［ ］１ ， （１０） 式中：ｆｉ 和τｉ 为连杆ｉ上来自母连杆的力和力 矩；ｆＥｉ 和τＥｉ 为来自外界环境的作用力和力矩（如 冲击力、重力等）；ｆｉ＋１和τｉ＋１为来自其子连杆的 反作用力和力矩．将式（１０）代入式（８）并整理，可 得到关于力和力矩的递归方程 　［ｆｉ，τｉ］Ｔ ＝ＩＳｉ¨Ｘｉ＋Ｘｉ×ＩＳｉＸｉ－ ［ｆＥｉ，τＥｉ］Ｔ＋［ｆｉ＋１，τｉ＋１］Ｔ． （１１） 　　如果连杆ｉ为关节型机器人的末端连杆，那 么计算ｆｉ，τｉ 时，令ｆｉ＋１和τｉ＋１为零．当该连杆不 受外部施加力时，ｆＥｉ 和τＥｉ 仅指重力和重力矩；反 之，ｆＥｉ 和τＥｉ 包括了重力项和外力项．因此，可以 由机器人末端连杆逐步向身体端计算所有关节上 的力和力矩，而关节轴上的扭矩为 ｕｉ ＝ｓＴｉ［ｆｉ，τｉ］Ｔ， （１２） 式中ｓｉ∈Ｒ６ 是一个由关节坐标系到世界坐标系 的映射向量． 根据前文轨迹规划所求得的各时刻下各关节 的位置、速度和加速度等数据，代入式（１１）和 （１２），即可计算出关节型机器人各关节在不同时 ·８１· 　　 　 华 中 科 技 大 学 学 报 （自 然 科 学 版） 　 第４１卷 刻下的驱动力矩，进而实现对关节型机器人的快 速、高精度的轨迹控制． ３　关节型机器人轨迹控制系统设计 　　根据前文所述，关节型机器人的轨迹控制系 统设计包括关节空间内的轨迹规划、力矩控制器 和机器人系统等，系统结构如图１所示． 图１　关节型机器人的轨迹控制系统组成 关节型机器人的轨迹规划部分，根据输入的 初始位置θ０、终点位置θｆ、步长ｔｓ和总时间Ｔ，按 式（２）可以计算出一条连续、光滑的运动轨迹，同 时输出各时刻下的位置θ、速度θ和加速度θ¨．依 据以上输出结果以及机器人自身动力学参数等信 息，关节型机器人的力矩控制器可以按照式（１１） 和式（１２）计算出各关节轨迹控制所需的驱动力 矩，并输出至机器人系统，促使其按照期望的轨迹 进行运动． 为了实现关节型机器人的力矩控制，传统的 有ＰＩＤ控制方法．图２即为一个基于ＰＤ控制器 的机器人轨迹控制系统，其中ＪＳＴＰ代表关节空 图２　基于ＰＤ控制的轨迹控制系统 间轨迹规划器（ｊｏｉｎｔ　ｓｐａｃｅ　ｔｒａｊｅｃｔｏｒｙ　ｐｌａｎｎｅｒ）．该 方法容易实现，其驱动力矩的计算如下 τ＝Ｋｐ（θｒ－θ）＋Ｋｄ（θｒ－θ）， 式中：θｒ和θ分别为期望和实际的关节运动；Ｋｐ， Ｋｄ∈Ｒｎ×ｎ为常数对角矩阵；θｒ－θ＝ｅ为机器人的 轨迹跟踪误差． 虽然基于ＰＤ控制的轨迹控制系统容易实 现，但是ＰＤ参数调节繁琐且机器人无法实现精 确的动力学建模，导致很难达到理想控制目标．采 用逆动力学方法的力矩控制器，可以避免上述问 题，实现准确的力矩控制．此处可引入一种采用迭 代牛顿－欧拉算法的力矩控制器（ＲＮＥＣ）及其轨 迹控制系统．为简化说明，此处的动力学方程可写 成如下 格式 pdf格式笔记格式下载页码格式下载公文格式下载简报格式下载 ［１２］ τ＝Ｄ（θ）·¨θ＋Ｃ（θ，θ）·θ＋ｇ（θ）， 式中：θ，θ和θ¨ 为 ＲＮＥＣ的输入，此时分别等于 θｒ，θｒ和θ¨ｒ；Ｄ（θ）∈Ｒｎ×ｎ为惯性矩阵；Ｃ（θ，θ）·θ 为离心力和科氏力的向量；ｇ（θ）为重力项． 若机器人的初始位置和速度与期望的初始位 置和速度一致，机器人将很好地跟踪期望轨迹；若 不一致，基于ＲＮＥＣ的轨迹控制系统则不能修正 初始条件误差．为解决该问题并提高轨迹跟踪性 能，引入了状态反馈，即增加一个加速度反馈环 节，如图３所示．此时，机器人系统动力学方程为 τ＝Ｄ（θ）·（¨θｒ＋Ｋｄｅ＋Ｋｐｅ）＋Ｃ（θ，θ）·θ＋ｇ（θ）， 逆动力学控制器（ＲＮＥＣ）的输入分别为θｒ，θｒ 和 θ¨ｒ＋Ｋｄｅ＋Ｋｐｅ． 图３　基于ＲＮＥＣ和加速度反馈的轨迹控制 系统的Ｓｉｍｕｌｉｎｋ模型 ４　仿真实验与结果分析 为了评价上述轨迹规划算法以及逆动力学计 算方法的有效性和实用性，在 Ｍａｔｌａｂ中对三种关 节型机器人的轨迹控制系统进行了动力学的仿真 验证和比较．实验对象为一个二自由度的关节型 机器人，具体参数为：实验要求机器人从初始位置 θ０＝［００］Ｔ 运动到终点位置θｆ＝［π／４，－π／４］Ｔ， 初始和终点时刻时各关节的角速度均为０ｒａｄ／ｓ， 运动总时间Ｔ＝５ｓ，采样时间ｔｓ＝５ｍｓ，自由度 ｎ＝２，关节行程θｌｉｍ为（－１６０°～１６０°），ＤＨ 参数 （ａ，α，ｄ，θ）为（０．２，０，０，θ１）和（０．２，０，０，θ２）， 连杆质量ｍｉ＝２ｋｇ，连杆质心ｃｍｉ＝（－０．１，０， ０）Ｔ，连杆惯性张量Ｉ＝ｄｉａｇ（０．０２６７，０．０２６７， ０．０２６７），连 杆 科 氏 摩 擦 系 数 Ｔｃ ＝ ［０．３９５， －０．３９５］Ｔ，电机惯量Ｊｍ＝２．０×１０－４，电机齿轮 齿数比Ｇ＝６０，ｉ＝１，２． 利用 Ｍａｔｌａｂ／Ｓｉｍｕｌｉｎｋ对上述三种关节型机 器人的轨迹控制系统进行建模和仿真［１３－１４］．第１ 种方案为基于ＰＤ控制的轨迹控制系统，其中包 括ＪＳＴＰ，ＰＤ控制器、二自由度关节型机器人Ｒｏ－ ｂｏｔ和一个动力学仿真显示模块ｐｌｏｔ．第２种方案 为基于 ＲＮＥＣ 的轨迹控制系统，其中包括了 ＪＳＴＰ，ＲＮＥＣ控制器、Ｒｏｂｏｔ模块和ｐｌｏｔ模块．第 ３种方案即为基于ＲＮＥＣ和加速度反馈的轨迹控 ·９１·增刊Ⅰ 　 　 陈培华，等：基于逆动力学方法的关节型机器人轨迹控制　 　　 制系统方案，如图３所示．该控制系统包括了 ＪＳＴＰ规划器、ＲＮＥＣ控制器、二自由度关节型机 器人Ｒｏｂｏｔ、动力学仿真显示模块ｐｌｏｔ和一个加 速度反馈控制器． 方案１　机器人进行动力学仿真后得到的关 节轨迹跟踪误差和关节驱动力矩（即扭矩）如图４ 所示．实验中，Ｋｐ＝ｄｉａｇ（９　０００，９　０００），Ｋｄ＝ ｄｉａｇ（１６０，１６０）． 图４　跟踪误差和扭矩曲线（ＰＤ控制） 方案２　在机器人的动力学仿真中所得到的 关节轨迹跟踪误差和驱动力扭矩曲线见图５． 图５　跟踪误差和扭矩曲线（ＲＮＥＣ控制） 采用图３所示的第３种方案进行机器人的动 力学仿真，结果见图６．控制系统内加速度反馈环 节中的参数取值为 Ｋｐ＝ｄｉａｇ（８０，８０），Ｋｄ＝ ｄｉａｇ（１０，１０）． 实验结果比较见表１．由表１可以得出：在方 案２中，机器人关节的最大跟踪误差约为１．７ 图６　跟踪误差和扭矩曲线 （带反馈的ＲＮＥＣ控制） ｍｒａｄ，最大扭矩约为０．１７Ｎ·ｍ，均小于方案１ 中的结果；采用ＲＮＥＣ和加速度反馈的轨迹控制 系统时，关节最大的轨迹跟踪误差仅为０．０３７　８ ｍｒａｄ，远小于前两种控制方案，最大扭矩约为 ０．１７Ｎ·ｍ，同方案２的结果相似． 表１　实验结果 性能参数 轨迹控制方案 １　 ２　 ３ ｍａｘ（｜ｅ｜）／ｍｒａｄ　 ２．６４０　０　１．７００　０　０．０３７　８ ｍａｘ（｜τ｜）／（Ｎ·ｍ） ３０．７３　 ０．１７　 ０．１７ 　　综上，本研究采用五次多项式插值拟合的方 法，在关节空间内对机器人进行轨迹规划，有效提 高了机器人的工作效率；同时，建立了基于迭代牛 顿－欧拉算法的关节型机器人逆动力学方程以及 三种机器人的轨迹控制系统；经过动力学仿真实 验的比较，证明了基于ＲＮＥＣ和加速度反馈的机 器人轨迹控制系统具有较高的跟随精度、较明显 的轨迹控制效果，是一种较优的力矩控制方法． 参 考 文 献 ［１］Ｂｒｏｇａｒｄｈ　Ｔ．Ｐｒｅｓｅｎｔ　ａｎｄ　ｆｕｔｕｒｅ　ｒｏｂｏｔ　ｃｏｎｔｒｏｌ　ｄｅｖｅｌ－ ｏｐｍｅｎｔ—ａｎ　ｉｎｄｕｓｔｒｉａｌ　ｐｅｒｓｐｅｃｔｉｖｅ［Ｊ］．Ａｎｎｕａｌ　Ｒｅ－ ｖｉｅｗｓ　ｉｎ　Ｃｏｎｔｒｏｌ，２００７，３１（１）：６９－７９． ［２］Ｐｕｔｚ　Ｐ．Ｓｐａｃｅ　ｒｏｂｏｔｉｃｓ［Ｊ］．Ｒｅｐｏｒｔｓ　ｏｎ　Ｐｒｏｇｒｅｓｓ　ｉｎ Ｐｈｙｓｉｃｓ，２００２，６５（３）：４２１－４６３． ［３］Ｌａｎｆｒａｎｃｏ　Ａ　Ｒ，Ｃａｓｔｅｌｌａｎｏｓ　Ａ　Ｅ，Ｄｅｓａｉ　Ｊ　Ｐ，ｅｔ　ａｌ． Ｒｏｂｏｔｉｃ　ｓｕｒｇｅｒｙ：ａ　ｃｕｒｒｅｎｔ　ｐｅｒｓｐｅｃｔｉｖｅ［Ｊ］．Ａｎｎａｌｓ　ｏｆ Ｓｕｒｇｅｒｙ，２００４，２３９（１）：１４－２１． ［４］Ｓｕ　Ｙ　Ｘ，Ｚｈｅｎｇ　Ｃ　Ｈ．Ｇｌｏｂａｌ　ｆｉｎｉｔｅ－ｔｉｍｅ　ｉｎｖｅｒｓｅ　ｔｒａｃｋ－ ｉｎｇ　ｃｏｎｔｒｏｌ　ｏｆ　ｒｏｂｏｔ　ｍａｎｉｐｕｌａｔｏｒｓ［Ｊ］．Ｒｏｂｏｔｉｃｓ　ａｎｄ Ｃｏｍｐｕｔｅｒ－Ｉｎｔｅｇｒａｔｅｄ　Ｍａｎｕｆａｃｔｕｒｉｎｇ，２０１１，２７（３）： ５５０－５５７． （下转第２４页） ·０２· 　　 　 华 中 科 技 大 学 学 报 （自 然 科 学 版） 　 第４１卷 ５　结论 本文在已提出的基于能量平衡的被动蜿蜒控 制方法的基础上，对控制算法的方向可控性进行 研究，根据被动蜿蜒运动的性能，利用补偿力矩调 节头部力矩，头部力矩输入改变头部关节角度，实 现运动方向不断靠近期望方向．方向控制参数直 接采用头部连杆与期望方向的夹角，无须计算，并 且具有实时性，给方向控制带来便利．通过仿真和 实验证明这种方法的有效性． 参 考 文 献 ［１］Ｈｉｒｏｓｅ　Ｓ．Ｂｉｏｌｏｇｉｃａｌｌｙ　ｉｎｓｐｉｒｅｄ　ｒｏｂｏｔｓ（ｓｎａｋｅ－ｌｉｋｅ　ｌｏ－ ｍｏｔｏｒ　ａｎｄ　ｍａｎｉｐｕｌａｔｏｒ）［Ｍ］．Ｏｘｆｏｒｄ：Ｏｘｆｏｒｄ　Ｕｎｉ－ ｖｅｒｓｉｔｙ　Ｐｒｅｓｓ，１９９３． ［２］郁树梅，马 书 关于书的成语关于读书的排比句社区图书漂流公约怎么写关于读书的小报汉书pdf 根，李斌，等．蛇形机器人步态产生及步 态分析［Ｊ］．机器人，２０１１，３３（３）：３７１－３７８． ［３］Ｋｉｍ　Ｄ，Ｈｗａｎｇ　Ｈ，Ｐａｒｋ　Ｓ．Ｔｕｒｎｉｎｇ　ｍｅｃｈａｎｉｓｍ　ｏｆ　ａ ｓｍｏｏｔｈ　ｂｏｄｙ　ｂｙ　ａｍｐｌｉｔｕｄｅ　ａｎｄ　ｐｅｒｉｏｄ　ｃｏｎｔｒｏｌ　ｉｎ　ｃｕｒ－ ｖａｔｕｒｅ［Ｃ］∥Ｐｒｏｃｅｅｄｉｎｇ　ｏｆ　ｔｈｅ　ＩＥＥＥ　Ｃｏｎｆ　Ｃｏｎｔｒｏｌ， Ａｕｔｏｍａｔｉｏｎ　ａｎｄ　Ｓｙｓｔｅｍｓ．Ｓｅｏｕｌ：ＩＥＥＥ，２００８：１７６５－ １７６８． ［４］唐超权，王明辉，李斌，等．融合机械元的蛇形机器人 循环抑制中枢模式发生器控制方法［Ｊ］．机器人， ２０１３，３５（１）：１２３－１２８． ［５］Ｗｕ　Ｘ，Ｍａ　Ｓ．Ｓｅｎｓｏｒ－ｄｒｉｖｅｎ　ｎｅｕｒａｌ　ｃｏｎｔｒｏｌｌｅｒ　ｆｏｒ　ｓｅｌｆ－ ａｄａｐｔｉｖｅ　ｃｏｌｌｉｓｉｏｎ－ｆｒｅｅ　ｂｅｈａｖｉｏｒ　ｏｆ　ａ　ｓｎａｋｅ－ｌｉｋｅ　ｒｏｂｏｔ ［Ｃ］∥Ｐｒｏｃｅｅｄｉｎｇ　ｏｆ　ｔｈｅ　ＩＥＥＥ　Ｃｏｎｆ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕ－ ｔｏｍａｔｉｏｎ．Ｓｈａｎｇｈａｉ：ＩＥＥＥ，２０１１：１９１－１９６． ［６］王智锋，马书根，李斌，等．基于能量的蛇形机器人蜿 蜒运动控制方法的仿真与实验研究［Ｊ］．自动化学 报，２０１１，３７（５）：６０４－６１４． ［７］王智锋．蛇形机器人的动力学模型与蜿蜒运动控制 方法研究［Ｄ］．沈阳：中国科学院沈阳自动化研究 所， 檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪檪 ２０１１． （上接第２０页） ［５］Ｗａｎｇ　Ｈ，Ｘｉｅ　Ｙ．Ａｄａｐｔｉｖｅ　ｉｎｖｅｒｓｅ　ｄｙｎａｍｉｃｓ　ｃｏｎｔｒｏｌ ｏｆ　ｒｏｂｏｔｓ　ｗｉｔｈ　ｕｎｃｅｒｔａｉｎ　ｋｉｎｅｍａｔｉｃｓ　ａｎｄ　ｄｙｎａｍｉｃｓ［Ｊ］． Ａｕｔｏｍａｔｉｃａ，２００９，４５（９）：２１１４－２１１９． ［６］Ｃｕｉ　Ｍ　Ｙ，Ｘｉｅ　Ｘ　Ｊ，Ｗｕ　Ｚ　Ｊ．Ｄｙｎａｍｉｃｓ　ｍｏｄｅｌｉｎｇ　ａｎｄ ｔｒａｃｋｉｎｇ　ｃｏｎｔｒｏｌ　ｏｆ　ｒｏｂｏｔ　ｍａｎｉｐｕｌａｔｏｒｓ　ｉｎ　ｒａｎｄｏｍ　ｖｉ－ ｂｒａｔｉｏｎ　ｅｎｖｉｒｏｎｍｅｎｔ［Ｊ］．ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ａｕｔｏ－ ｍａｔｉｃ　Ｃｏｎｔｒｏｌ，２０１３，５８（６）：１５４０－１５４５． ［７］Ｅｒｋｏｒｋｍａｚ　Ｋ，Ａｌｔｉｎｔａｓ　Ｙ．Ｈｉｇｈ　ｓｐｅｅｄ　ＣＮＣ　ｓｙｓｔｅｍ ｄｅｓｉｇｎ，ｐａｒｔ　Ｉ：ｊｅｒｋ　ｌｉｍｉｔｅｄ　ｔｒａｊｅｃｔｏｒｙ　ｇｅｎｅｒａｔｉｏｎ　ａｎｄ ｑｕｉｎｔｉｃ　ｓｐｌｉｎｅ　ｉｎｔｅｒｐｏｌａｔｉｏｎ［Ｊ］．Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｊｏｕｒｎａｌ ｏｆ　Ｍａｃｈｉｎｅ　Ｔｏｏｌｓ　ａｎｄ　Ｍａｎｕｆａｃｔｕｒｅ，２００１，４１（９）： １３２３－１３４５． ［８］Ｍａｃｆａｒｌａｎｅ　Ｓ，Ｃｒｏｆｔ　Ｅ　Ａ．Ｊｅｒｋ－ｂｏｕｎｄｅｄ　ｍａｎｉｐｕｌａｔｏｒ ｔｒａｊｅｃｔｏｒｙ　ｐｌａｎｎｉｎｇ：ｄｅｓｉｇｎ　ｆｏｒ　ｒｅａｌ－ｔｉｍｅ　ａｐｐｌｉｃａｔｉｏｎｓ ［Ｊ］．ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａ－ ｔｉｏｎ，２００３，１９（１）：４２－５２． ［９］Ｇａｓｐａｒｅｔｔｏ　Ａ，Ｚａｎｏｔｔｏ　Ｖ．Ａ　ｔｅｃｈｎｉｑｕｅ　ｆｏｒ　ｔｉｍｅ－ｊｅｒｋ ｏｐｔｉｍａｌ　ｐｌａｎｎｉｎｇ　ｏｆ　ｒｏｂｏｔ　ｔｒａｊｅｃｔｏｒｉｅｓ［Ｊ］．Ｒｏｂｏｔｉｃｓ ａｎｄ　Ｃｏｍｐｕｔｅｒ－Ｉｎｔｅｇｒａｔｅｄ　Ｍａｎｕｆａｃｔｕｒｉｎｇ，２００８，２４ （３）：４１５－４２６． ［１０］Ｍｕｒｒａｙ　Ｒ　Ｍ，Ｌｉ　Ｚ，Ｓａｓｔｒｙ　Ｓ　Ｓ．Ａ　ｍａｔｈｅｍａｔｉｃａｌ　ｉｎ－ ｔｒｏｄｕｃｔｉｏｎ　ｔｏ　ｒｏｂｏｔｉｃ　ｍａｎｉｐｕｌａｔｉｏｎ［Ｍ］．Ｂｏｃａ　Ｒａｔｏｎ： ＣＲＣ，１９９４． ［１１］Ｆｅａｔｈｅｒｓｔｏｎｅ　Ｒ．Ｒｉｇｉｄ　ｂｏｄｙ　ｄｙｎａｍｉｃｓ　ａｌｇｏｒｉｔｈｍｓ ［Ｍ］．Ｂｅｒｌｉｎ：Ｓｐｒｉｎｇｅｒ，２００８． ［１２］Ｓｐｏｎｇ　Ｍ　Ｗ，Ｖｉｄｙａｓａｇａｒ　Ｍ．Ｒｏｂｏｔ　ｄｙｎａｍｉｃｓ　ａｎｄ ｃｏｎｔｒｏｌ［Ｍ］．Ｎｅｗ　Ｙｏｒｋ：Ｊｏｈｎ　Ｗｉｌｅｙ　＆Ｓｏｎｓ，２００４． ［１３］Ｔｕｒａｎ　Ａ，Ｂｏｇｏｓｙａｎ　Ｓ，Ｇｏｋａｓａｎ　Ｍ．Ｄｅｖｅｌｏｐｍｅｎｔ　ｏｆ ａ　ｃｌｉｅｎｔ－ｓｅｒｖｅｒ　ｃｏｍｍｕｎｉｃａｔｉｏｎ　ｍｅｔｈｏｄ　ｆｏｒ　Ｍａｔｌａｂ／ Ｓｉｍｕｌｉｎｋ　ｂａｓｅｄ　ｒｅｍｏｔｅ　ｒｏｂｏｔｉｃｓ　ｅｘｐｅｒｉｍｅｎｔｓ［Ｃ］∥ Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｓｙｍｐｏｓｉｕｍ　ｏｎ　Ｉｎ－ ｄｕｓｔｒｉａｌ　Ｅｌｅｃｔｒｏｎｉｃｓ　２００６．Ｐｉｓｃａｔａｗａｙ：ＩＥＥＥ，２００６： ３２０１－３２０６． ［１４］Ｃｏｒｋｅ　Ｐ．ＭＡＴＬＡＢ　ｔｏｏｌｂｏｘｅｓ：ｒｏｂｏｔｉｃｓ　ａｎｄ　ｖｉｓｉｏｎ ｆｏｒ　ｓｔｕｄｅｎｔｓ　ａｎｄ　ｔｅａｃｈｅｒｓ［Ｊ］．ＩＥＥＥ　Ｒｏｂｏｔｉｃｓ　ａｎｄ Ａｕｔｏｍａｔｉｏｎ　Ｍａｇａｚｉｎｅ，２００７，１４（４）：１６－１７． ·４２· 　　 　 华 中 科 技 大 学 学 报 （自 然 科 学 版） 　 第４１卷