双臂强耦合系统的运动规划与控制的研究(学术型).pdf

工学硕士学位论文工学硕士学位论文 双臂强耦合系统的运动规划与控制的研究 （学术型） research on motion planning and control of a dual-manipulator strongly coupled system （academic） 禹超禹超 哈尔滨工业大学哈尔滨工业大学 2012 年年 7 月月 国内图书分类号：v476.4 学校代码：10213 国际图书分类号：621 密级：公开 工学硕士学位论文工学硕士学位论文 双臂强耦合系统的运动规划与控制的研究 (学术型) 硕 士 研 究 生：禹超 导师：刘宏 教授 申请学位：工学硕士 学 科 、 专 业：机械电子工程 所在单位：机电工程学院 答辩日期：2012 年 7 月 6 日 授 予 学 位 单 位：哈尔滨工业大学 classified index: v476.4 u.d.c.: 621 dissertation for the master degree in mechanical engineering research on motion planning and control of a dual-manipulator strongly coupled system （academic） candidate： yu chao supervisor： prof. liu hong academic degree applied for：master of mechanical engineering speciality： mechanical engineering unit： department of mechanical engineering date of oral examination： july, 6th, 2012 university： harbin institute of technology 哈尔滨工业大学工学硕士学位论文 - i - 摘摘 要要 为了满足复杂任务的需求，双臂冗余度机器人应运而生，相比于非冗余度 单臂机器人而言，双冗余度机械臂在控制器设计、逆运动学求解、运动规划和 协调控制方面对研发者提出了更高的要求。 本文根据双臂系统的控制需要，采用了中央控制器多关节控制器的分 层控制构架。根据关节控制的需要，设计了以现场可编程门阵列（field- programmable gate array，fpga）为核心，嵌入式 nios ii 微处理器为平台的 关节控制器。关节控制器能够快速采集传感器信息，并与中央控制器通过高速 总线通信，保证了控制的实时性。 其次，针对双机械臂夹持刚性物体这一强耦合系统，分析了系统的运动约 束，结合双臂系统的任务需求，提出了一种七段笛卡尔轨迹规划方法，该方法 规划的轨迹具有速度、加速度连续的特点，此外速度、加速度和 jerk 值（加 速度的导数）均被限定在给定范围内，保证了机械臂系统的安全。 然后，建立了双臂系统的 d-h 坐标系，推导了机械臂的正运动学。为了 得到速度的逆解，提出了一种改进的梯度投影法，该方法能够有效的处理机械 臂的奇异位姿，对关节进行速度限制，并能保证关节速度与笛卡尔速度同步。 针对具有偏置腕关节的七自由度机械臂，分析了常用解析方法的不足，提出了 一种利用虚拟关节求解逆运动学的解析方法，该方法具有很高的位置精度。 最后，建立了双臂系统的动力学模型，设计了 pd+重力补偿的位置控制 器。在 simmechanics 平台中按照机械臂的名义动力学参数建立了双臂系统， 并联合 matlab simulink 建立了仿真环境，通过对双臂的位置控制仿真发现， 双臂均采用位置控制会导致系统接触力过大，继而使相对位置误差增大。为了 实现对接触力的控制，基于给定力的笛卡尔阻抗控制被应用到机械臂中，从而 建立了位置/阻抗协调控制器。仿真结果表明，采用位置/阻抗协调控的双臂系 统能够很好的跟踪给定的接触力并能减小双臂间的相对位置误差。 关键词关键词 双臂强耦合系统；七段式轨迹规划；虚拟关节法；阻抗控制；协调 控制 哈尔滨工业大学工学硕士学位论文 - ii - abstract for the single non-redundant manipulator(nrsm), there exist big limitations in flexibility, adaptability and operating capacity for the current level of robot technology. in order to meet the needs of complex tasks, dual-redundant manipulators (drm) come into being, compared with the nrsm, the controller design, solution for inverse kinematics, motion planning and coordination control for the drm are more complex and difficult. according to the needs of the drms control, the hierarchical control architecture of central controller multi joint controller is used. according to the needs for joint control, joint controller based on fpga(field-programmable gate array) which embedded nios ii microprocessor is designed, the testing results showed that the joint controller can quickly acquire the sensor information, and communicate with the central controller at a high speed. firstly, kinematic constraints for the strongly coupled dual-manipulator system (scdms) which the drm grip a rigid body is given. combined with the requirements of the mission, a seven-segment cartesian trajectory planning method was used, the trajectory planned by this method owns continuous velocity and acceleration, and the velocity, acceleration and jerk are limited. then, an improved gradient projection method is deduced to solve the velocity; this method can effectively handle manipulator singular position and avoiding joint speed limits. for the 7dof manipulator with offset-wrist, the deficiencies of the commonly used analytical method for inverse solution of position is given, and then an analytical solution called virtual joint method is proposed, the simulation result shows that this method has a high position accuracy. finally, the dynamic model of the system is assigned, and an arm position controller using a pd +gravity compensation technique is designed. model of scdms is build based on simmechanics, and the simulation result showed that the contact force between manipulators and object changes frequently and leads to big relative position error. in order to control the contact force, impedance control in cartesian space is applied and a position/impedance coordination controller is designed. the simulation result shows that this controller can track the given contact force well and reduce the relative position error. keywords: dual-manipulator strongly coupled system, seven-segment cartesian trajectory planning method, virtual-joint method, coordination control 目 录 - iii - 目 录 摘 要i abstract . ii 第 1 章 绪论.1 1.1 课题研究的目的和意义.1 1.2 冗余度双臂机器人的国内外研究现状分析 1 1.2.1 国内外双臂机器人的研究现状.1 1.2.2 冗余度机械臂的逆运动学.5 1.2.3 笛卡尔空间轨迹规划 7 1.2.4 双臂协调操作概述 8 1.3 主要研究内容 9 1.4 本章小结 10 第 2 章 双臂系统关节控制器的设计 11 2.1 双臂控制系统的总体硬件设计 .11 2.2 关节控制器的设计.12 2.2.1 电源设计13 2.2.2 传感器系统14 2.2.3 电机驱动器16 2.2.4 失电制动器电路设计 16 2.2.5 片上关节控制器的设计.16 2.2.6 关节控制器测试实验 17 2.3 本章小结 20 第 3 章 双臂强耦合系统的运动规划 21 3.1 双臂强耦合系统的运动学约束 .21 3.1.1 位置约束22 3.1.2 速度约束22 3.2 双臂强耦合系统的运动规划 .24 3.2.1 七段式笛卡尔轨迹规划方法.24 3.2.2 点对点直线轨迹规划 25 3.2.3 多点连续轨迹规划 27 3.2.4 双臂强耦合系统的轨迹规划.29 3.3 本章小结 29 哈尔滨工业大学工学硕士学位论文 - iv - 第 4 章 7 自由度机械臂的运动学研究30 4.1 机械臂运动学建模及正运动学 .30 4.1.1 机械臂的运动学建模 30 4.1.2 机械臂的正运动学 31 4.2 关节速度的求解.32 4.2.1 改进型梯度投影法 33 4.3 位置的逆运动学求解.35 4.3.1 虚拟关节法36 4.3.2 运动仿真验证41 4.5 本章小结 .43 第 5 章 双臂强耦合系统的协调控制 44 5.1 双臂强耦合系统的动力学模型 .44 5.1.1 机械臂的静力学 44 5.1.2 机械臂的逆动力学 44 5.1.3 双臂强耦合系统的动力学分析.45 5.2 双臂强耦合系统的位置控制 .47 5.2.1 基于 simmechanics 的位置控制仿真试验47 5.2.2 双臂强耦合系统的位置控制.51 5.3 笛卡尔空间阻抗控制的研究 .52 5.3.1 机械臂的操作空间动力学模型.52 5.3.2 基于位置的阻抗控制 53 5.3.3 笛卡尔空间基于给定力的阻抗控制 .55 5.3.4 双臂强耦合系统的协调控制.56 5.4 实验研究 58 5.4.1 实验平台介绍58 5.4.2 位置控制实验58 5.4.3 阻抗控制实验60 5.5 本章小节 61 结 论.62 参考文献.63 攻读学位期间发表的学术论文68 哈尔滨工业大学硕士学位论文原创性声明.69 致 谢.70 哈尔滨工业大学工学硕士学位论文 - 1 - 第1章 绪论 1.1 课题研究的目的和意义 本课题来源于哈工大德宇航空间联合实验室“机器人宇航员”的预研课 题。 自机械臂被发明并投入实际应用以来，机械臂技术得到了极大的发展，其 应用从工业制造逐渐扩展到医疗救援、娱乐服务、军事排爆甚至航天太空探索 等领域。在机械臂的应用中，大部分任务比较单一，往往只需要单个机械臂配 合专用的末端执行器就能完成。但是随着行业的进一步深入的发展，操作任务 越来越复杂，对机械臂的适应性和智能性提出了更高的要求。此外机械臂在运 动过程中，往往会对机械臂提出避障，避奇异等要求，为了满足任务需求，双 臂冗余度机器人应运而生。其相对于单臂机器人有如下的特点： （1）灵活性高：冗余度机械臂能够利用其冗余特性克服奇异性。避关节 角极限、避障等1-4。 （2）容错性：由于带有冗余自由度，当某一个关节遇到故障时，可以利 用剩余的六个关节进行任务再规划，具有较高的可靠性。 （3）工作能力强：能够完成单一机械臂无法完成的任务，如夹持柔性的 薄板、搬运较重的物体（超过单臂承载范围）、拉锯等任务； （4）任务适应性高：能够根据任务的需要，可以进行单臂控制和双臂协 调控制，完成简单或复杂的任务； 虽然双臂冗余自由度机器人具有上述良好的性能，但是冗余双臂间的协调 运动是一个复杂的过程，从运动学角度来看，冗余自由度机械臂的运动学存在 多解性，选择合适的优化解可以更好的发挥其性能；从路径规划角度来看，采 用一种平滑的安全的轨迹能避免机械臂的震荡，保证机械臂和使用人的安全； 从控制角度来看，使双臂系统按照指定的路径运动并能控制其输出力，可以保 证最终任务的完成并能对操作物体不造成伤害。因此本文对冗余度双臂强耦合 系统的研究是具有十分重要的意义的。 1.2 冗余度双臂机器人的国内外研究现状分析 1.2.1 国内外双臂机器人的研究现状 1.2.1.1 德国宇航中心的双臂机器人 justin 哈尔滨工业大学工学硕士学位论文 - 2 - 德国宇航中心(dlr)研制的仿人上半身系统justin4 f5-6是目前最灵活的仿人 形机器人之一。如图 1-1 所示，它由两条 7 自由度仿人机械臂、两个 12 自由 度灵巧手，一个 3 自由度腰部一个 2 自由度头部组成，其上半身最大可伸展高 度 90cm，重量 45kg，可以完成夹持重物，扭瓶盖等复杂任务。整个机器人的 核心是由两条dlr新一代轻型臂（dlr-lwr-iii）构成的双臂系统。整个双臂 系统的控制器分为笛卡尔控制层和关节控制层，关节层采用基于dsp/fpga构 架的关节控制器，主要负责传感信号的采集和电机的驱动。笛卡尔控制层采用 了基于pc机的上层控制器，笛卡尔控制层和关节控制层间采用总线周期为 1ms的实时串行通信总线进行通信。 图 1-1 德国宇航中心的双臂机器人 justin 1.2.1.2 美国 nasa 双臂机器人 robonaut2 2011 年 2 月 25 日“发现号”航天飞机携带人类首个机器人宇航员 “robonaut2”进入空间站。如图 1-2 所示，robonaut27-8全身装备多种的传感 器，拥有灵巧的上肢，可帮助人类宇航员完成枯燥、重复或者危险的任务。 robonaut 2 也采用了两级控制构架，中央控制器采用了双处理器构架，其中一 个处理器主要实现对关节信号的接收和处理，此外对整个系统实行动力学和运 动学监测和保护；另一个处理器主要运行控制算法，运动学计算等。它们之间 通过cpci总线进行通信和数据共享；关节控制器采用嵌入式微处理器，具有 极强的驱动能力。两级控制器之间采用m-lvds总线代替robonaut1 中的 rs485 点对点通信，大大的减少了通信导线的数量，提高了通信频率，最高通 信频率可达 10khz。robonaut2 采用了操作空间阻抗控制和关节空间阻抗控制 两级阻抗控制，整个系统表现出极强的柔性，能够适应多种环境。 哈尔滨工业大学工学硕士学位论文 - 3 - 图 1-2 美国 nasa 研制的机器人宇航员 robonaut2 1.2.1.3 2 9 bhonda公司的asimo机器人 如图 1-3 所示，asimo9-10是日本本田公司研制的最新一代的仿人形机器 人，asimo 与 honda 开发的前几代机器人相比，最为显著的特点是其上肢 活动能力得到大大的增强，其灵活的 7 自由度双臂能够轻松完成一些服务动 作。如开锁，端盘子等9 f。 图 1-3 honda 公司的 asimo 机器人 1.2.1.4 hrp 仿人形机器人 日本产经省于 1998 年至 2002 年间组织推动了hrp（humanoid robotics project）项目，推出了hrp系列机器人，并于 2008 年推出了最新一代仿人机 器人hrp-311。如图 1-4 所示，整个机器人身高 160cm，重 68kg，具有 42 个 自由度。机械臂采用与人手臂相同的构型即三自由度肩关节、单自由度肘关节 和三自由度腕关节组成，关节内集成了位置传感器并使用直流无刷电机进行驱 动，在手臂处集成了加速度传感器和力传感器以实现手臂和身体的协调控制。 上层控制器采用奔腾处理器，并集成了art-linux实时操作系统，保证了机器 人控制的实时性。 哈尔滨工业大学工学硕士学位论文 - 4 - 图 1-4 日本开发的 hpr-3 双臂机器人 1.2.1.5 3 0 babb 公司frida工业机器人 如图 1-5 所示。abb公司开发的frida1 1 f12工业机器人仅由两条机械臂构 成，每条机械臂含有 7 个自由度，在机械臂的末端装有六维力矩传感器，整个 机器人结构紧凑，重量轻，可以任意安放。机器人的控制器采用abb公司开 发的abb irc5 robot control system，该控制系统有开放的外部接口，能够与 外部传感系统进行连接进行控制。 图 1-5 abb 公司开发的 frida 双臂工业机器人 1.2.1.6 国内双臂机器人的研究现状 国内对双臂机器人的研发处于战略决策和预研阶段，在使用方面几乎是一 片空白。早期开发的双臂系统往往用国外已成型的单机械臂组合而成，如首钢 莫托曼机器人有限公司开发的双机器人协调弧焊系统、双机器人协调倒水、下 棋演示系统13、北京工业大学的power cube模块化双臂协调机器人14及北京 航天航空大学搭建的冗余双臂空间机器人系统实验平台15。在自主研发方 面，近期也取得了一定的成果，如复旦大学研发的“复旦一号”16，上海交通 大学研究开发的类人型双臂机器人17，哈工大研发的服务机器人18等。其中 最具代表性的是如图 1-6 所示北京理工大学研制的机器人“汇童19”，“汇童”灵 哈尔滨工业大学工学硕士学位论文 - 5 - 活的双臂能够完成复杂的动作，如舞剑，打太极等。 图 1-6 北京理工大学研制的机器人“汇童” 1.2.2 冗余度机械臂的逆运动学 相比于非冗余度机械臂，冗余度机械臂有更灵活的性能，如避动力学奇异 位置，避障碍物、避关节极限、实现关节力矩优化等。但是机械臂的冗余自由 度却给手臂的逆运动学求解带来了困难。在过去的 20 多年时间内，大量的学 者对冗余自由度机器人的逆运动学展开了深入的研究，提出了大量的解法及优 化方法。 1.2.2.1 固定角度法 固定角度法的思想非常简单，即将冗余度机械臂的冗余自由度固定，转化 为六自由度机械臂，然后通过反变换方法20或者几何方法进行求解。该种方 法思想简单，但相比于其它方法，没有将机械臂的自运动分离出来，失去了冗 余性。 1.2.2.2 梯度投影法 梯度投影法(gradient projection method, gpm)21-23是应用最广泛的一种逆 运动学求解方法。根据机器人雅可比矩阵的定义，关节速度 q ? 和末端运动速度 x ? 存在如下的关系： ( )xj q q=? (1-1) 式中( )j q机械臂的雅可比矩阵； x ? 机械臂末端在基座标中的速度； q ? 机械臂的关节速度。 根据式(1-1)，我们可以得出关节速度和末端执行器速度的关系 ( -)qj xij j v + =+? (1-2) 式中 + jj的伪逆； i单位矩阵； 哈尔滨工业大学工学硕士学位论文 - 6 - v任意向量； 梯度投影法求解的速度包含两项，第一项称为最小范数解，它能完成所有 的机械臂的运动；第二项为式(1-1)的通解，( -)ij j vn j + （ ）为方程的齐次 解，n j（ ）为雅可比矩阵 j 的零空间，齐次解不影响机械臂末端在笛卡尔空间 的速度矢量。 1.2.2.3 加权最小泛数法 加权最小泛数法(weighted least-norm，wln)24-25最初由whiney提出用来 解决冗余度问题，其思想是通过使用惯性矩阵w为权值矩阵来减少能量。设 目标优化函数为( )h q，则 1 2 0.0 0.0 .0 00. n w = (1-4) 式中 i 权重系数， ( ) 1 i i h q q = + 引进变换 1/2 w jjw =， 1/2 w qwq=?，则利用雅可比矩阵的定义可得 1/21/2 ww xjqjwwqj q =? (1-5) 最终可得 111 () tt qwjjwjx =? (1-6) 1.2.2.4 臂角法 臂角法26-28即选择机械臂的某一个参数为臂角，然后通过臂角表示其它 关节角度，由于臂角的定义不同，得到的关节的求解公式以及复杂程度也会存 在差异。 lee和bejczy26提出了一种以某一关节为参数的逆运动学解法，虽然能够 求解机械臂的逆运动学，但是如何选择合适的关节为参数是一个很复杂的过 程，不利于逆运动学的优化。 masayuki28根据机械臂的构型，将7自由度机械臂的前三个关节和最后 三个关节分别简化为球形肩关节和球形腕关节，从而将机械臂的自运动转换为 绕某一连线的转动，将转动角度定义为臂角，从而可以利用臂角表示其它关 节。这种方法很好的表现了机械臂的自运动特性，但是仅仅能够处理具有球形 关节的机械臂，从而具有一定的局限性。 1.2.2.5 其它方法 鉴于梯度投影法良好的优化性能，祖迪29等人将梯度投影法和固定角度 哈尔滨工业大学工学硕士学位论文 - 7 - 法结合起来，提出了二次计算方法，该种方法利用梯度投影法计算出了某一个 关节的优化解，然后将该关节角度固定，再计算其它关节角度，该方法虽然没 有位置误差，但是固定关节的选择是没有理论依据的，而且计算量也比其它方 法大。 上述的方法可以分为二类，第一类为基于速度的逆解，如梯度投影法和加 权最小泛数法，这种方法都需要计算机械臂的雅可比矩阵及其逆矩阵，该种方 法的位置精度会受到控制周期的影响。第二类为基于位置的逆解，如固定角度 法和参数化方法，本文研究的机械臂有特殊的腕关节构型，一般的固定角度法 和参数化方法无法得到有效应用。寻找一种快速有效的逆运动学求解方法是本 文的一个主要研究内容。 1.2.3 笛卡尔空间轨迹规划 机械臂的轨迹规划的目的是使机器人从起始状态移动到某个目标状态。如 何快速有效的对机械臂的运动进行规划直接影响机械臂的运动效率，此外轨迹 的平滑性也会直接影响机械臂的运动性能。 paul30利用齐次变换矩阵表示目标位置，并将机械臂的笛卡尔空间轨迹表 示为一系列的齐次变换矩阵，并利用齐次变换方程来描述机械臂的运动，机械 臂在运动过程中，加速度保持为常值。taylor31提出了在起始点和目标点插入 足够多中间点的方法进行笛卡尔轨迹规划，并利用四元数描述机械臂的姿态， 通过对四元数的插值，使规划的旋转运动连续。andersson32采用单一的多项 式来拟合整个笛卡尔轨迹，该种方法虽然能保证加速度的连续，但往往造成位 置的超调；bobrow33采用七次多项式拟合笛卡尔轨迹，并对jerk进行了限制。 kroger34提出了一种能从任意初始条件进行规划的算法，但是该方法未能考虑 机械臂的运动限制条件。ignacio35针对人机交互机器人提出机械臂在运动过 程中，机械臂在各个方向的运动均要受到限制，基于此，提出了7段式轨迹规 划方法，该方法对各个方向的jerk，速度、加速度都进行了限制并保证了速度 和加速度的连续性。黄剑斌36提出了基于余弦二阶函数的轨迹规划方法，该 方法对机械臂的速度、加速度以及jerk进行了限制，并通过余弦的无限次可导 特性保证了速度、加速度及jerk的连续性。 双臂机器人在协调运动过程中，物体的期望路径决定了每一个操作臂机器 人的操作空间运动轨迹。对双臂系统的路径规划实际上是对物体的路径进行规 划。出于稳定性和安全性考虑，对规划的路径提出了如下的要求：(1)平稳易 跟踪：规划的轨迹不能发生速度，加速度的突变；(2)jerk值限定：要求规划 的轨迹的jerk值限定在一定范围内；(3)各个方向要满足运动约束：双臂系统 哈尔滨工业大学工学硕士学位论文 - 8 - 是一个人机交互的系统，对各个方向进行运动约束有利于系统的安全；(4)时 间最优：在满足上述限定条件下实现时间最优化。 1.2.4 双臂协调操作概述 双臂机器人的协调操作任务可以分为两类：松协调操作任务和紧协调操作 任务。松协调是指双臂机器人在同一工作空间内分别执行各自无关的任务。如 一个臂在要求的位置以要求的姿态抓持一个物体，另一个机械臂在该物体上进 行操作，避碰撞的轨迹规划是该类任务的主要研究问题37-38。紧协调是指双 臂机器人在同一工作空间内执行同一或多项作业任务，与松协调相比，紧协调 任务中机械臂之间存在更加严格的运动约束和动力约束。本文主要研究的是含 有强直接物理相互作用操作的系统，这类系统被称为强耦合系统，这类操作协 调的关键是控制系统的内力。 双臂机器人的协调控制的方法主要有纯位置控制、主从控制、力/位置混 合控制和阻抗控制等。 在早期的双臂机器人控制的研究中，zheng39等研究了双臂在协调操作时 双臂的位置约束，指出如果双臂能够始终维持这种约束，就能实现双臂的协调 操作。lim和chyung首先对物体的轨迹进行了规划，然后通过微分变换转换 到各个机械臂上，机械臂采用微小的位移增量进行控制，实现了双臂的纯位置 协调控制，这种纯位置的控制方法在实际运用过程中，只有当双臂系统的柔性 比较大时方可实现，即容许有较大的协调误差。但是事实上，在双臂系统的运 动过程中，由于本身的制造误差，双臂末端的位置不能始终按照规划好的路径 运动，在机械臂和物体之间表现出较大的接触力和扭矩，对机械臂和物体造成 损坏，因此需要考虑力的影响。 正是考虑纯位置控制带来的影响，zheng39和kopf40等提出了主从控制方 式，这种控制方法的思想是：把双臂系统中的某一条臂定义为主臂，另一条臂 定义为从臂，预先规划好主臂的运动轨迹，对于从臂，要求对物体施加一定的 力，这些力用来满足运动及力约束。在这种控制方式中，主臂采集位置信息， 从臂测量力信息，从臂通过力变化来跟踪主臂的运动轨迹。但是该方法也存在 一定的问题：(1)需要高响应速度：从臂是通过力传感器来感应主臂的运动规 律的，这就要求从臂具有高响应速度，但是当具有快速响应速度时，系统容易 失去稳定性。(2)从臂的跟踪精度：当主臂突然转向或者改变速度时，从臂难 以继续跟踪，会导致跟踪精度很差。 dauchez41等分析了上述控制方法失效的主要原因是因为双臂系统是一个 完整的系统，采用解耦独立的控制方法进行控制难免会带来协调控制的问题。 哈尔滨工业大学工学硕士学位论文 - 9 - raiben和craig于1981年首次提出了机器人力/位置混合控制，并取得了良好的 效果。随后uchiyama43提出了双臂机器人的力/位置混合控制方法，双臂系统 的两条机械臂在协调作用中不再有主从之分，而是起着相同的作用，此外两条 机械臂不再使用独立的控制器，而是采用一个控制器进行控制。位置/力混合 控制器接收期望位置和期望力，期望位置包括机器人末端执行器在工作空间中 的位置和姿态，期望力包含机械臂末端作用在物体上的力和力矩。当机械臂在 自由环境中运动时，对环境不施加任何力，当运动受限时，施加给定的力，这 种控制方式的切换采用一种选择矩阵。dauchez等人研究了两个机械臂刚性抓 取一个物体时的位置/力混合控制，该控制方法被成功应用在两个六自由度机 械臂上。ramadorai44、hu45等也将位置/力混合控制成功的应用到了双臂协 调控制上。位置/力混合控制虽然能很好的对机械臂的位置和力进行控制，但 是当机械臂从自由空间运动到受限空间时或由受限空间运动到自由空间时，控 制方式的切换容易导致系统的不稳定，此外该种方法计算复杂，环境约束难以 描述，所以在实际使用中较难实现。 为了避免位置/力混合控制在由自由环境到受限环境中切换带来的不稳 定，hogan提出了阻抗控制46方法。阻抗控制是通过调节末端接触点位置和力 的关系实现的，期望的阻抗可以通过反馈控制来得到。阻抗控制在单臂上已经 得到了广泛的应用并取得了预期的效果47。在双臂协调控制方面，阻抗控制 也占有越来越重要的地位，文献48-49给出了阻抗控制在协调机器人中的应用 试验。schneider和cannon50提出了物体阻抗控制策略，利用动力学控制器， 给出了基于物体（负载）的阻抗控制，这种控制方法相当于在物体的每个自由 度上安装了一个弹簧，但是这种阻抗关系严重依赖物体的动力学特性，当负载 改变时，控制器需要重新设计。bonitz和hsia52提出了一种基于内力的阻抗控 制方法，在机械臂末端的跟踪误差和内力跟踪误差之间建立阻抗，在由两 puma机器人搭建的实验平台上进行实验发现该种控制方法能够有效的控制内 力并表现出较小的位置跟踪误差。caccavale和villani53-55总结了上述两种控制 思想，同时考虑了物体的阻抗和内力的影响，并能通过切换开关实现对物体阻 抗和内力的单独控制。最终在两台六自由度机械臂上实现了这一控制方法。 1.3 主要研究内容 本课题的主要任务是根据双臂机器人的控制需要，研发高效率、高集成度 的关节控制器，最终在双臂强耦合系统上进行运动规划并进行协调控制。具体 的研究内容如下： （1）根据双臂机器人的控制需要，制定机械臂关节控制器的电气方案。 哈尔滨工业大学工学硕士学位论文 - 10 - 设计以fpga为核心、集成位置传感器、力传感器、高速通信总线的关节控 制器，实现对位置信号、力信号的采集、电机的驱动等功能。 （2）建立机械臂的运动模型，研究偏置腕关节7自由度冗余机械臂的逆 运动学。根据任务的需要，提出相应的优化目标函数，寻找逆运动学优化解。 （3）分析双臂强耦合系统的运动学约束约束，根据双臂机器人的安全性 要求，寻找合适的轨迹规划算法。 （4）采用合适的协调控制算法，使机械臂能够跟踪规划的轨迹，并能满 足力约束要求。 1.4 本章小结 本章首先指出了课题的研究目的和意义，分析了国内外双臂机器人的研究 现状，接着对冗余度机械臂的逆运动学解法和笛卡尔轨迹规划方法进行了分析 和总结，并指出了现有方法的优缺点，最后介绍了双臂协调控制的方法并给出 了本课题的研究内容。 哈尔滨工业大学工学硕士学位论文 - 11 - 第2章 双臂系统关节控制器的设计 关节控制器作为双臂控制系统的基础模块，主要完成传感器信号的采集、 电机的驱动、上位机的通信。本章首先分析了双臂系统的整体控制方案，然后 根据对关节控制的要求设计了以fpga为核心的关节控制器。在fpga中嵌 入nios ii微处理器，采用vhdl硬件编程语言编写了传感器采集模块、电 机驱动模块、掉电保护模块，将这些模块与dlr-hit实验室自主开发的点对 点高速串行通信系统集成到nios ii处理器中，实现了对关节传感器信号的实 时采集、电机的大负载驱动、掉电制动保护以及与上位机的实时通信等功能。 2.1 双臂控制系统的总体硬件设计 图 2-1 预研的机器人宇航员模型 本文研究的双臂系统为hit-dlr实验室预研的机器人宇航员中的双臂部 分，如图2-1所示，两条机械臂为构型一样的7自由度机械臂，其自由度分布 与人手臂一致，前三个关节对应肩关节的三个自由度，第四个关节对应人手臂 的肘关节，后三个关节对应人手臂的腕关节。两条手臂可以独立运动，也可以 协调运动。这就要求整个硬件系统具有一定的灵活性。此外双臂在协调运动过 程中需要大量的数据交换和数据处理，为了实现对双机械臂的协调控制，其硬 件系统应该满足如下要求：（1）高通信速率：保证数据的及时交换；（2）高 处理速度：快速计算机械臂的运动学、动力学，实现避障、避碰撞算法，实现 相应的控制算法等；（3）短控制周期：实现实时和精确控制（4）高感知能 力：传感信号快速采集与处理能力。 哈尔滨工业大学工学硕士学位论文 - 12 - 在以往的研究中发现，机械臂控制系统面临两种任务，一种为低计算量高 频率的任务，如传感信号采集与关节底层的电流、速度和位置控制，控制周期 小于1ms；另一种任务则要求较大的计算量，但是次数较少，如机械臂逆运动 学的求解，机械臂的轨迹规划和动力学，控制周期大于50ms。如果采用单层 控制结构即所有的任务都由一个控制器来控制，那么将会导致任务切换频繁， 影响控制器的处理速度，延长控制器的控制周期，不利于机械臂的实时反应。 根据这两种任务的要求，双臂系统采用如图2-2所示的分层控制结构。 图 2-2 分层控制构架 整个系统分为中央控制层、通信层和关节层三个部分。中央控制层主要进 行机械臂的正逆运动学，动力学计算、笛卡尔轨迹规划，上层位置及力控制 等；中央控制器采用hit-dlr实验室自主开发的基于奔腾处理器的嵌入式控 制平台，处理速度可达2.0ghz，内存可达1g，能够很好的满足中央层的数据 处理要求；通信层作为两者的连接通道分别隐含于中央控制层和关节控制层 中，通信层采用hit-dlr自主开发的基于m-lvds的点对点的高速串行通信 系统（point to point high speed serial communication system，ppseco），该 总线的通信速度可达25mbits/s，保证通信的实时性。关节控制器主要采集并 上传关节信息，接收上层控制命令控制关节运动。 2.2 关节控制器的设计 关节是机械臂构成的基本单元，也是机械臂运动的最小模块，为了满足系 统的控制要求，关节控制器必须满足如下要求： （1） 传感器信息采集功能：包括关节的位置信息和力矩信息； （2） 足够的电机驱动能力：实现对电机的驱动，包括高载荷下的正转、 哈尔滨工业大学工学硕士学位论文 - 13 - 反转、停止； （3） 高速的通信速度：快速上传关节信息； （4） 一定的数据处理能力：处理传感器信息； （5） 一定的控制能力：一般的关节底层控制； 此外考虑到设计的重复性和机械臂的安全性，关节控制器还要满足如下要求： （1） 通用性：避免重复设计，便于维修和替换； （2） 安全性：掉电保护，过流保护等； （3） 体积小：便于集成到关节中去，机械臂体积小，美观，集成度高。 为了满足关节控制器的功能要求，设计了基于nios ii嵌入式微处理器的 关节控制器，如图2-3所示，在nios ii上集成了ppseco通信模块，pwm发 生模块，磁编码器数据采集处理模块，光电编码器力矩传感器数据采集模块。 该 控 制 器 充 分 利 用 了fpga的 可 编 程 片 上 系 统 技 术(system-on-a- programmable-chip，sopc)，简化了电路的设计，具有很强的通用性。 图 2-3 关节控制器整体方案示意图 2.2.1 电源设计 电源系统是关节得以正常工作的必要条件，整个关节控制器分为了驱动板 和控制板，其中驱动板主要为电机及传感器进行供电，所需电压较大，控制板 主要为fpga，电压较小，为了减小电路板的体积，两部分独立进行供电。 2.2.1.1 电机驱动供电系统 电机的驱动系统包括电机驱动，制动驱动，传感器驱动等，各个部分的所 哈尔滨工业大学工学硕士学位论文 - 14 - 需电压如表2-1所示，按照电压的不同需求，进行相应的电压转换。 表 2-1 驱动板电源分配方案 部件 电压 电机驱动 36v 抱闸 36v 霍尔传感器 5v 光电编码器、力矩传感器 5v 磁编码器（相对位置） 5v 2.2.1.2 控制板供电系统 控制板的核心为fpga，fpga的各个模块需要不同的电压进行供电，此 外各个传感器的上传电压为5v，与fpga的端口电压不一致，需要对其电平 进行转换，3.3v作为参考电压进行供电。电源方案如图2- 4 所示。 图 2-4 控制板的电源分配方案 2.2.2 传感器系统 机械臂的运动状态主要包括机械臂的位置和力信息，这些信息往往不能直 接在笛卡尔空间中直接测量，而需要经过关节空间到笛卡尔空间的转换，因此 需要在关节中集成大量的传感器，以获得关节的位置和力信息。单关节的传感 器分布如表2-2所示。 表 2-2 关节内传感器 传感器 数量 测量类型 磁编码器 1 电机位置 光电编码器 1 关节的位置 力矩传感器 2 关节力矩 霍尔传感器 1 电机转轴位置 电流传感器 2 电机控制两相电流 2.2.2.1 磁编码器 磁编码器主要测量电机转子的位置，如图2-5所示，磁编码器安装在电机 端，磁圈和电机转子一起转动，同时输出6路信号，分别为a相正负，b相 正负，z相正负。其中z相为圈脉冲信号，ab两相为周期和幅值相同、相差 哈尔滨工业大学工学硕士学位论文 - 15 - 90度的矩形波信号。输出信号幅值为5v，而fpga的接口电压为3.3v，在设 计过程中，对及电平进行转换，采用max3094e接收其a、b、z差分信号， 输出正脉冲信号到fpga中，实现了电平转换和隔离。 图 2- 5 磁编码器实物图及单周期信号图 从图2-5可以看出，将ab信号进行组合，可得到四种状态，即11、10、 00、10，将这些状态进行组合，即可以辨别电机的转动方向。当正转时，ab 的变化为11-01-00-10-11；当反转时，ab的值为01-00-01-11-01。即在一个脉 冲周期内ab的状态会变化4次，结合z相信号，则电机每旋转一周，ab状 态变化4096 4 16384=次，通过对ab状态进行计数，就可求的电机的相对 位置。 2.2.2.2 光电编码器和力矩传感器 15位高精度光电编码器安装在关节输出端，直接测量关节转过的角度；2 路力矩传感器采用全桥应变片电路，测量关节末端力矩。解码后的光电编码器 信息与经a/d转换后的力矩信息经数据整合，通过rs485总线发送到控制器 中，在关节控制器端，采用半双工485接收芯片sp3485对传输的信号进行采 集，并经电平转换后输出至fpga中。数据格式为st-0xff-sp-st-0x81-sp- st-0x00-sp-st-data1h-sp-st-data1l-sp-st-data2h-sp-st-data2l-sp- st-data3h-sp-st-data3l-sp。其中oxff、ox00、ox81为校验码，st为 字开始信号，sp为字终止信号，data1为光电编码器数据，data2为正向转 动力矩传感器数据，data3为反向转动力矩传感器数据，h与l分别表示字 的高位和低位。传感信号采集板以及数据传输波形如图2-6所示。 图 2-6 光电传感器与力矩传感器采集电路与输出信号波形 哈尔滨工业大学工学硕士学位论文 - 16 - 2.2.2.3 霍尔传感器 电机在转动过程中，通电线圈切割磁场会产生感应电动势，这种感应电动 势即为霍尔信号，霍尔信号根据绕组的a、b、c三相也分为a、b、c三相 霍尔，霍尔信号是电机转子位置的测量单元，是电机驱动控制的主要位置参考 量。 2.2.3 电机驱动器 关节电机采用哈尔滨工业大学微特电机研究所研制的85电机和72电机， 供电电压为36v。由于fpga的输出电压无法直接驱动电机，采用驱动芯片控 制mosfet桥的方式进行驱动。驱动芯片采用ir23364d，该芯片控制电压为 3.3v，响应时间为530ns，该芯片带有过流保护功能，防止电机错误运转导致 大电流的产生。 图 2- 7 直流电机驱动电路 2.2.4 失电制动器电路设计 失电制动器采用永磁离合器，安装在关节的输出端，制动器由永磁铁离合 器，衔铁和线圈组成。当掉电时，线圈不产生磁场，衔铁在永磁力的作用下被 吸引到离合器转子的摩擦片上；当通电达到一定值后，线圈产生的磁场与永磁 铁产生的磁场相抵消，衔铁与离合器分离。fpga对制动器的控制即对线圈电 流的控制，线圈的供电电压为36v，无法由fpga直接驱动，采用三极管对其 开断进行控制。在实际运用过程中，对线圈的控制采用分段控制，首先是启动 阶段，在该阶段输出大电流，产生大的磁场使衔铁和离合器分离，然后采用较 小电流进行状态保持。这样可以减小线圈的发热量，减小功耗。 2.2.5 片上关节控制器的设计 关节控制器的大部分功能在fpga中实现，在fpga中嵌入nios ii微处 理器，利用vhdl语言（very-high-speed integrated circuit hardware descript -ion language）编写了关节控制器的各个功能模块，并利用sopc builder将 哈尔滨工业大学工学硕士学位论文 - 17 - 各个模块封装为外部设备挂载到nios ii 总线上，在nios ii中通过控制这些 外设来达到接收传感信息、驱动关节的作用，nios ii外设结构图如图2-8所 示。 图 2-8 nios ii 外设模块结构图 pin_r3 vcc joint_rxd input pin_a5 vcc rxd2 input pin_d5 vcc fault_monitor input pin_a13 vc