（机械设计及理论专业论文）欠驱动平面耦合刚体动力学与运动规划研究.pdf

摘要 摘要 欠驱动系统是指控制输入小于系统自由度的系统。由于驱动器的减少，系 统具有重量轻、成本低、能耗少等诸多优点。对于欠驱动系统的力学意义的描 述是二阶非完整系统，其非完整约束由二阶微分方程表示，即指系统加速度的 不可积性。二阶非完整系统已有前人证明过是可控的。 本文讨论欠驱动平面耦合刚体系统的动力学与运动规划问题。首先，讨论 末端为欠驱动杆的平面机械臂系统。为简化模型，仅考虑欠驱动杆的动力学问 题，结合撞击中心的力学特性，建立欠驱动杆的动力学模型，导出杆的二阶非 完整约束方程。利用二阶非完整约束的特性，通过控制欠驱动关节的加速度， 欠驱动杆可产生不同形式的平动、转动和平面运动。组合这些不同形式的运动， 建立一种开环运动规划方法，实现机械臂系统从一种初始状态到另一种目标状 态的运动规划。然后，作为对末端为欠驱动杆的平面欠驱动机械臂系统的延伸， 讨论平面耦合双刚体欠驱动系统、三刚体耦合欠驱动系统及t 个刚体耦合欠驱 动系统。结合撞击中心处的力学特性，建立系统的动力学模型。由系统的动力 学模型导出各刚体的二阶非完整约束方程。利用系统的二阶非完整约束特性， 通过控制第一个刚体的加速度输入，可使系统产生不同形式的转动和平动。组 合这些不同形式的运动，对系统进行运动规划，实现平面刚体耦合系统从一种 初始状态运动到另一种目标状态。文章最后通过仿真实验验证了规划方法的有 效性与可行性。 关键词：欠驱动系统：非完整约束；动力学；运动规划 a b s t r a c t a b s t r a c t u n d e r a c t u a t e ds y s t e m sa r em e c h a n i c a ls y s t e m sw i t hf e w e rc o n t r o li n p u t st h a n d e g r e e so ff r e e d o m t h i sc l a s so fs y s t e m sh a sm a n ya d v a n t a g e ss u c ha sl i g h tw e i 曲t ， l o wc o s ta n dl o we n e r g yc o n s u m p t i o n u n d e m c t u a t e ds y s t e mo nw h i c hd y n a m i c c o n s t r a i n ti s e x p r e s s e db y s e c o n d o r d e rd i f f e r e n t i a l e g u a t i o n ，i s a l s oc a l l e d s e c o n d - o r d e rn o n h o l o n o m i c s y s t e m t h ec o n t r o l l a b i l i t y o ft h es y s t e mh a sb e e n p r o v e d t h ed y n a m i cc o n s t r a i n t o nt h es e c o n d - o r d e rn o n h o l o n o m i cs y s t e mi s u n i n t e g r a lt oa c c e l e r a t i o n i nt h i sp a p e r , w ei n v e s t i g a t e dd y n a m i c sa n dm o t i o np l a n n i n gf o rt w os y s t e m so f c o u p l e dr i g i db o d i e so nh o r i z o n t a lp l a n e o n ei su n d e r a c t u a t e dm a n i p u l a t o rw i ma n u n d e r a e t u a t e dl i n k 砒t h ee n do ft h es y s t e m h e r e ，w eo n l yi n v e s t i g a t e dt h ed y n a m i c s o ft h eu n d e r a e t u a t e dl i n k c o n s i d e r i n gt h ep h y s i c a lc h a r a c t e r i s t i co ft h ec e n t e ro f p e r c u s s i o n ，w ec a nd e d u c et h ek i n e t i cm o d e lo ft h el i n k i nt h i sw a y , t h es e c o n d o r d e r n o n h o l o n o m i ek i n e t i cc o n s t r a i n to ft h eu n d e r a c t u a t e dl i n kc a nb eo b t a i n e d b yt h e t r a n s l a t i o n a la c c e l e r a t i o ni n p u t sa tt h ep a s s i v er e v o l u t ej o i n t , t h el i n kh a ss e v e r a l d i f f e r e n tk i n d so fr o t a t i o na n dt r a n s l a t i o n b yp r o p o s i n ga no p e n - l o o pm o t i o n p l a n n i n gm e t h o d w ec a np l a nt h em o t i o no fm a n i p t d a t o rf r o m 孤i n i t i a ls t a t et oa d e s i r e do n e t h eo t h e ri st h er i g i db o d i e sa l es e r i a l l yc o n n e c t e db yp a s s i v er e v o l u t e j o i n t s i nt h es a m ew a yo ft h es y s t e mo fu n d e r a c t u a t e dm a n i p u l a t o r , w ec a ng e tt h e s e c o n d o r d e rn o n h o l o n o m i ck i n e t i cc o n s t r a i n to fe a c hr i g i db o d y c o n s i d e r i n gt h e c h a r a c t e r i s t i co fc o n s t r a i n t ，t h es y s t e mh a ss e v e r a ld i f f e r e n tk i n d so fr o t a t i o na n d t r a n s l a t i o nb yt h et r a n s l a t i o n a la c c e l e r a t i o ni n p u t sa tt h ef i r s tr i g i db o d y a s s e m b l i n g t h em o t i o n s ，w ec a np l a nt h em o t i o no fs y s t e mf r o ma ni n i t i a ls t a t et oad e s i r e do n e s i m u l a t i o n ss h o wt h a tt h em o t i o np l a n n i n gi se f f e c t i v ea n df e a s i b l e k e yw o r d s ：u n d e r a c t u a t e ds y s t e m ；n o n h o l o n o m i cc o n s t r a i n t ；d y n a m i c s ；m o t i o n p l a n n i n g i i 学位论文版权使用授权书 本人完全了解北京机械工业学院关于收集、保存、使用学位论文 的规定，同意如下各项内容：按照学校要求提交学位论文的印刷本和 电子版本；学校有权保存学位论文的印刷本和电子版，并采用影印、 缩印、扫描、数字化或其它手段保存论文；学校有权提供目录检索以 及提供本学位论文全文或者部分的阅览服务；学校有权按有关规定向 国家有关部门或者机构送交论文的复印件和电子版；在不以赢利为目 的的前提下，学校可以适当复制论文的部分或全部内否用于学术活 动。 学位论文作者签名：阐、弗 加6 年月功日 经指导教师同意，本学位论文属于保密，在年解密后适用 本授权书。 指导教师签名：学位论文作者签名： 年月日年月 e 1 硕士学位论文原创性声明 本人郑重声明：所呈交的学位论文，是本人在导师指导下，进行 研究工作所取得的成果。除文中已经注明引用的内容外，本学位论文 的研究成果不包含任何他人创作的、已公开发表或者没有公开发表的 作品的内容。对本论文所涉及的研究工作做出贡献的其他个人和集 体，均己在文中以明确方式标明。本学位论文原创性声明的法律责任 由本人承担。 签名：哟、伽 加6 年月功日 第一章绪论 第一章绪论 1 1 引言 近年来，随着多用途、多功能机器人技术的不断发展，欠驱动系统引起人 们的广泛关注。所谓欠驱动就是指控制输入少于系统广义坐标的维数。在自动 机械系统中运用欠驱动技术，可以简化系统结构，提高系统的l t 冈i j 度，减少系 统重量，降低制造成本，还可以减少系统能耗。但是欠驱动系统完全不同于一 般的完整系统，因此得到了广泛和深入的研究，成为机械，自动控制等领域的 研究热点。 非完整约束是含有系统广义坐标导数且不可积的约束。大多数关于非完整 系统的研究都是处理一阶非完整系统，这类系统多来自于含有滚动接触或角动 量守恒的系统。欠驱动系统是指由控制输入向量构成空间的维数小于位形空间 维数的系统，即指控制输入数小于系统广义自由度维数的系统。对于欠驱动系 统的另外一种描述是二阶非完整系统，其非完整约束是由二阶微分方程表示，即 指对于系统加速度的不可积性【l 】：而一阶非完整系统则是指对于系统速度的约 束，即关于速度的不可积性【2 】。因此认为对于欠驱动系统的研究是对于一阶非完 整系统研究的延续d 4 1 。典型的一阶非完整欠驱动机器人系统包括空间机器人 5 l 、 轮式移动机器人f 6 】、球形滚动机器人川、跳跃机器入嗍、多指灵巧手等 9 1 。 一般而言，在如下情况下会运用到欠驱动机器人技术【l 州4 1 ：( 1 ) 空间机器人、 微重力环境中或其他结构特别紧凑的系统，使用欠驱动关节可以大大减轻重量、 降低成本：( 2 ) 设计时有意减少驱动装置以增加系统灵巧性；( 3 ) 系统本身存在 一些运动约束而成为欠驱动系统，如移动机器人：( 4 ) 某些紧急情况下，驱动电 机失灵而无法更换的，将其处理成欠驱动关节可以满足应急实现，具有实际意 义：( 5 ) 出现在全驱动机器人系统，如可以将柔性操作臂处理成含有被动关节的 欠驱动刚性机械臂进行研究。工程技术中常见的欠驱动机器人分别有空间机器 人、水下机器人、移动机器人、行走机器人、并联机器人、伺服机器人和柔性 机器人等【1 5 】。 1 2 欠驱动非完整系统研究现状 第一章绪论 近十多年来，人们对欠驱动系统展开了深入的研究，取得了一些成果，主 要集中于欠驱动力学特性分析及控制、可控性分析、位置控制、运动规划和轨 迹跟踪、系统的稳定性和鲁捧性控制等领域i l ”。 在动力学特性分析方面，b e r g e r m a n ! 1 6 l 就欠驱动操作臂主、被动关节的动力 学耦合，提出耦合指数成= 丌盯，( 其中盯为耦合矩阵的奇异值) 、全局耦合指 乞 l 反d e 数彬= 旦_ r 一( p 为全部关节空间) 等量化指标，详尽讨论了动力学耦合和耦 j d 9 e 合指标的本质，及其在分析和设计欠驱动系统、控制和规划机器人运动位形时 耦合指标的作用；s h i n 1 分析了欠驱动机器人的动力学和运动学，在一定假设 基础上提出笛卡儿空间反馈线性化解耦动力控制器，解决了动力学奇异问题； n a k a m u r a ! 1 8 】首次从非线性动力学角度分析了平面欠驱动机器人的非线性特性。 在系统的可控性分析方面，有相当多的文献对欠驱动系统的可控性分析进 行了探讨。s u s s m a n n 1 引、b i a n c h i n i1 2 m 、h e r m a n n 2 1 1 等人的早期研究从数学角度 为欠驱动机器人的可控性分析提供了理论基础。p o p e s c u z z l 分析了不可控平面欠 驱动双摆机器人，在增加弹簧等被动元件后系统成为可控的，从而证明势能对 可控性的重要影响。针对非线性系统中没有一般的解析方法研究其自然可控性， l u c a 和i a n n i t t i 阱l 研究了欠驱动机器人系统的小时间局部可控性问题( s t l c ) ， 提出直接建立在系统惯性矩阵各项上的充分条件，对检验具有n 个转动自由度和 n - 1 个控制输入的欠驱动系统的s t l c 是一简单宜行的方法。m a h i n d r a k a r 和 b a n a v a r ! ”l 提出具有两个输入的3 r 欠驱动机械臂，在垂直平面运动时，不论哪个 关节是被动的，系统都是线性可控的；但是在水平面运动时，证明除第一被动 系统外，其他都只是s t l c 。a r a i 【2 5 j 等证明了末端为欠驱动关节的平面三自由度 欠驱动系统的可控性，继而又研究了惯性矩阵的非奇异条件检验欠驱动操作臂 的可控性方法。m u l l h a u p t 和s r i n i v a s a n l 2 6 】分析了两杆欠驱动系统的可控性，提 出小时间局部输出可控性( s t l o c ) 这一新概念，与s t l c 进行比较，说明除病态 情况外，两杆欠驱动机器人系统几乎处处是小范围局部可控的。b u l i o 2 7 】针对二 阶欠驱动机械系统引入运动可控性新提法，使其成为运动规划的基础。 在系统的位置控制和运动控制方面，y u 2 8 】等运用施加在被动关节上的摩擦 作用控制被动关节的自由和锁定，并开发了两种位置控制算法。a r a i1 2 9 】针对末 2 第一章绪论 端为欠驱动关节的平面三自由度欠驱动系统，结合简单的平动和转动分段轨迹， 运用反馈方法对系统进行位置控制。m i t a 3 0 】对具有漂移项的高阶非完整链式系 统提出变周期无振荡的数字控制方法。l u c a ”】分析了串联机械臂轨迹规划、轨 迹跟踪和设定点调节的可行性，用动力学反馈线性化有效解决了四自由度含有 两个被动关节的系统和末端为欠驱动杆的平面三自由度欠驱动机械臂的运动规 划和轨迹跟踪。y o s h i k a w a 3 2 1 针对平面三自由度欠驱动系统，使轨迹能从任意 给定初始点出发，中途经过任意给定点，收敛到任意期望点。z h e n d 3 i 运动 t a k a g i s u g e n o 模糊模型完成对末端为欠驱动杆的耦合双刚体系统的轨迹跟踪 任务。 在系统稳定性和鲁棒性方面，z h a n g 3 4 】首次对非线性和欠驱动机械系统提出 混合切换控制策略，给出所建立混合控制律的充分条件和稳定判据，并检测了 应用系统的鲁棒性。s u 3 5 】对欠驱动机械系统的控制提出基于模型的自适应变结 构控制方案，使不确定约束仅取决于系统的惯量参数在李亚普诺夫意义上建 立了全局渐近稳定性。l i z a r r a g a i 蚓设计了基于混合反馈开环策略的控制定律， 能保证二阶链式系统的动力学指数稳定。朱齐丹【37 】针对欠驱动机器人提出基于 神经网络的位置开环控制方法，利用李亚普诺夫稳定性方法设计了欠驱动关节 的位置闭环控制。b u l l 0 1 3 剐研究了欠驱动系统的稳定性，通过适当的分离控制判 据，得到系统的局部指数稳定。s i q u e i r a 3 9 运用非线性技术控制欠驱动系统， 来克服系统的不稳定性。 1 3 课题背景及意义 本课题结合国家自然科学基金资助项目“欠驱动非完整多体系统动力学与 控制研究” 项目编号：1 0 3 7 2 0 1 4 ，开展了对末端为欠驱动杆的平面机械臂 系统和平面刚体耦合欠驱动系统的动力学与运动规划研究。 欠驱动非完整系统是一种非常特殊的非线性系统，其研究能丰富和发展非 线性控制的理论，也有助于解决复杂机械系统的动力学和控制问题。因而欠驱 动非完整系统的研究有着重要的理论研究价值和意义。 欠驱动系统的控制问题得到广泛重视并成为非线性控制领域的热点之一， 主要原因有以下两点：( 1 ) 在实际工程中，许多控制系统都是欠驱动控制系统， 如何用少于标准数目的执行器来控制系统是一个有意义的尝试；( 2 ) 欠驱动系统 3 第一章绪论 是一类特殊的非线性系统，对于该类系统的研究有助于研究一般非线性系统的 控制问题【4 0 1 。欠驱动系统的控制问题可分为三大类：运动规划、轨迹跟踪( 包 括几何路径跟踪) 和平衡点反馈镇定。本文主要讨论运动规划问题，运动规划 是一个开环控制问题，目标是寻找控制输入将系统从指定的初始状态移动至指 定的目标状态。 1 4 本文的主要研究工作 本文主要做了以下三方面的研究工作： 1 对末端为欠驱动杆的平面机械臂系统建立动力学模型，得到欠驱动杆的 约束方程。通过控制欠驱动关节的加速度，并考虑撞击中心的运动，欠驱动杆 可产生三种不同形式的运动。从而建立一种开环运动控制方法，实现系统从一 种初始状态到另一种目标状态的运动规划： 2 分别对欠驱动平面耦合双刚体系统、三刚体耦合系统及r 个刚体耦合系 统建模，并得到各系统中各剐体的约束方程。由系统的几何关系，得到各关节 加速度关系。结合约束方程，解得系统发生不同运动所需的控制输入加速度。 利用这些不同的运动形式，对系统进行运动规划； 3 对欠驱动机械臂系统和欠驱动平面耦合刚体系统进行仿真实验，以验证 文中运动规划的可行性及有效性。 4 第二章非完整系统简介 第二章非完整系统简介 2 1 引言 欠驱动系统是指由控制输入向量构成空间的维数小于位形空间维数的系 统，即控制输入数小于系统自由度的系统。其特点是可由较少的控制输入确定 其在比控制输入空间维数大的位形空间内的运动。对于欠驱动系统的另外一种 描述是二阶非完整系统即指对于系统加速度的不可积性；而一阶非完整系统 则是指对于系统速度的约束，即关于速度的不可积。因此认为对于欠驱动系统 的研究是对于一阶非完整系统研究的延续【柏l 。本章简单介绍了非完整约束概念， 非完整系统的动力学模型及其运动规划。 2 2 非完整约束 从运动学观点理解，约束不仅限制物体的空间位置，而且也限制物体的速 度，也可以说，约束是对物体的运动状态的限制。这种限制可以用状态变量和 时间变量的约束方程表示。其约束方程的一般形式为 巾( q ，香，t ) = 0 ( 2 1 ) 在任一瞬时，约束方程( 2 1 ) 对应于系统的状态空间中确定的光滑超曲面， 称为约束曲面。当约束方程中显含时间变量时，约束曲面随时间丽不断改变其 几何形状，这种约束称为非定常约束。不显含时间变量的约束方程对应于状态 空间中固定不变的约束曲面，称为定常约束，其一般形式为 中( q ，口) = 0 ( 2 2 ) 如果约束将系统的运动限制在位形空间q 中的一个光滑超曲面上，则该约 束就称为是完整的。从局部来说，完整约束可表示为位形空间中的一组代数约 束 中，( g ) = 0 ，i = 1 , 2 ，k ( 2 3 ) 每一个中，都是从q 到贸的映射，这种限制系统的位置约束称为几何约束。 s 第二章非完整系统简介 几何约束不仅对系统的位形有了限制，相应地也同时对系统的速度分量有限制。 这个限制方程可以通过将原几何约束方程对时间求全导数而得到。由式( 2 3 ) 得 善争，= 。或詈删 亿t ， 式( 2 4 ) 称为几何约束的微商形式。可以看到，几何约束的微分形式都是恰当微 商形式。 一般情况下，对于位形空间为q 的系统，可将速度约束写为如下形式 z ( q ) o = 0 ( 2 5 ) 其中a ( q ) 贸“表示七个速度约束的集合。具有这种形式的约束称为p f a f f i a n 约 束。由于p f a f f i a n 约束只是限制了系统的许可速度，而不是必需的位移，所以不 能将其表示为位形空间的代数约束。如果存在一个矢量值函数o ：q 专锨，使 得 a ( q ) 口：0 营娑口：0 ( 2 6 ) 叼 则认为p f a f i s a a 约束是可积的。因此可积的p f a f f i a n 约束等价于完整约束；反之 如不可积，则称为非完整约束。 2 3 非完整系统动力学模型 非完整约束的力学系统建模己有多种方法，其中d a l e m b e r t - l a g r a n g e 动力 学方程及非完整约束方程描述如下 竺9 ) 牙+ m ，口) = 地) a + b ( g 弦 ( 2 7 ) 【j 1 ( q ) q = 0 其中m ( q ) 为n 正定对称的惯性矩阵，j ( q ) 为n 一肌) 满秩阵， 是n - m 维 的l a g r a n g e 矢量乘子，b ( q ) 是胛。p 阵，f 是p 维的矢量力矩。令g l ，9 2 ，g 。表 示，( g ) 的零空间的一组基，g ( g ) = k ( g ) ，g ：( g ) ，g 。( q ) 】，有j r ( g ) g ( g ) = 0 ， 且 第二章非完整系统简介 牙= g i ( q ) v i + + g 。( g ) v 。 ( 2 8 ) 其中y = ( v ，v ：，) 可看成为系统的速度控制量或广义速度控制量。对式( 2 8 ) 微分得 百：g ( g ) i + 掣口 叫 将上式代入式( 2 7 ) 第一个方程，两边乘以9 7 ( g ) 可得 g r ( g ) 彳( g ) g ( g ) p + f ( q ，g ) = g r ( g ) b ( g ) r ( 2 9 ) 设矩阵9 7 ( g ) b ( g ) 为可逆，取输入变换 f = 【9 1 ( g ) b ( g ) 】。1 【g ( g ) ，( g ) g ( g ) “+ f ( q ，口) 】 ( 2 1 0 ) 则式( 2 1 0 ) 可被反馈线性化为形式p = ”，于是，得到基于动力学的非完整控制系 统模型 f 口3g t ( g ) l ，t + + g m ( g ) v m ，m j i l 一) ? 心 目标位喜 图3 5 欠驱动杆的运动规划( 算例i ) ( a ) 1 6 茎三兰签翌垫垫堡壁堕塾塑型 _ _ _ - _ _ _ 一一 掣 捌 暑 ( c ) 图3 6 欠驱动杆运动规划仿真结果( 算例1 ) 汐j 碱y ，彩 ，： j s | 目标也形 图3 7 欠驱动杆运动规划( 算例2 ) 1 7 第三章欠驱动机械臂运动规划 善 暂 ( a ) i 憾 髓 层 图3 8 欠驱动杆运动规划仿真结果( 算例2 ) 1 8 第三章欠驱动机械臂运动规划 3 5 本章小结 本章讨论了平面三自由度欠驱动机械臂的非完整运动规划问题。通过简化 模型，只考虑欠驱动杆的动力学约束特性，并在其关节点给定一个加速度输入 信号，可以获得欠驱动杆三种不同运动形式。利用这三种运动形式的特点，建 立一种开环运动规划方法，实现系统从任意一种初始状态到任意一种目标状态 的运动规划。仿真实验结果证实了文中方法的有效性。其他形式末端为欠驱动 杆的欠驱动机械臂系统的非完整运动规划同平面三自由度欠驱动机械臂的规划 方法。 1 9 第四章欠驱动平面耦合刚体建模 第四章欠驱动平面耦合刚体建模 4 1 引言 人类对知识的渴求是无止境的，正因如此人类文明才得以迅速发展。在上一 章中我们讨论了仅末端为欠驱动杆的平面机械臂的运动规划。强烈的求知欲促 使我们想了解不仅末端为欠驱动杆，而且所有杆均为欠驱动杆的机械臂的运动 规划情况。 作为对末端为欠驱动杆的平面欠驱动机械臂的延伸，本章讨论所有杆件均由 被动关节连接的平面耦合双刚体系统和平面三刚体耦合系统的建模，并在此基 础上总结出用n 个被动关节连接的平面n 个耦合刚体系统的建模。 4 2 欠驱动平面耦合双刚体的建模 由被动关节鼻连接韵双刚体b 和b ：耦合模型如图4 1 所示。由于系统在水 平面运动，故不考虑重力的作用。设只和最分别是刚体蜀和b ：的撞击中心，g 和g ：分别是刚体马和b ：的质心，p o 至g 。和只的距离分别为，l 和og ，至只的 距离为b l ( 见图4 2 ) 。 图4 1 欠驱动平面耦合双刚体模型 设刚体且在只点的作用力为z 。和兀，因此晶点的工和y 方向分别产生加 速度。只点上为刚体b ：对刚体b 的作用力。刚体b 的动力学方程可写为 m l 葺g = _ 。s i n o l + ：，c o s o l e ， ( 4 1 ) 2 0 第四章欠驱动平面耦合刚体建模 m l 夕l g = 一一。c o s 9 l + z ，s i n o l 只y ( 4 2 ) ，l q = - ，；。+ b l ( 鼻，s i n 岛一e ，c o s 0 1 ) ( 4 3 ) 式中m 和以分别表示刚体b ，的质量和相对质一c , g 的转动惯量。刚体b ，各点的几 何关系如下 。i = 。1 ( j + 6 ic o s 8 t ( 4 4 ) y l = y + b ls i n 0 i ( 4 5 ) 图4 2 刚体受力示意图 式中b 是刚体b 。与x 轴正方向的夹角。因只点是刚体b ，的撞击中心，所以有 6 l ：土m 1 。由方程( 4 1 ) ( 4 5 ) ，可以推导出下列方程 m l ，l m t 戈，+ 曩，( 1 + 了m lq 2 ) = c 。s q ( i ，+ b ? ( f i x c 。s 日+ e ys i n 岛) 一聊，6 l 卯) 4 6 ) 肌t y ”一十f t y ( 1 + 了m l 。t 2 ) = s i n q ( z ，+ 争砰( e s q + 曩y s i n 岛) 川b , 0 1 z ) ( 4 7 ) 将方程( 4 6 ) ( 4 7 ) 两边分别相除，并考虑，= m ：戈：g ，只。= m 2 j j 2 g ，得 到刚体b ，的约束方程 一( m l f i x “l + i i m 2 王2 6 ) s i n 鼠+ ( m l ，l 萝l + ，i m 2 歹2 g ) c o s o l = 0 ( 4 8 ) 2 1 第四章欠驱动平面耦合刚体建模 由图4 2 - b 可列出刚体鼠的动力学方程 m 2 譬2 6 = e ， m 2 膏2 g = e v ，2 0 2 = r 2 ( e ，s i n 0 2 一e ，c o s o z ) 同理，导出刚体既的约束方程 一聊2 r 2 王2s i n 0 2 + m 2 ，2 夕2c o s 口2 = 0 ( 4 9 ) ( 4 1 0 ) ( 4 1 1 ) ( 4 1 2 ) 由图4 2 中几何关系可得到点只( i = 0 ，1 ) 和g ( ，= 1 ，2 ) 的加速度 2 置，= 戈2 + 丘( c o s 幺绑+ s i n o k o k ) k = i + l 2 歹= 夕2 + l k ( s i n a , t ，；- c o s a , g , ) 卅+ i 2 i 芦= 章2 + ，i ( c o s 吼钟+ s i n 幺坑) 一o ( c o s 嘭彬+ s i n o s o s ) t z j 2 少归= 萝：+ ( s i n o k 0 ；一c o s 吼筑) 一o ( s i n q 彰一c o s q 谚) k ；i 由方程( 4 1 2 ) 到方程( 4 1 6 ) 可知， 动来决定。即如果已知刚体岛的运动， ( 4 1 3 ) ( 4 1 4 ) ( 4 1 5 ) ( 4 1 6 ) 刚体蜀的各运动量可以通过刚体岛的运 那么刚体b i 的运动可以计算出来。 4 3 平面三刚体耦合欠驱动系统的建模 由被动关节只和只连接的三刚体e 、b ，和b 模型如图4 3 所示。同平面耦 合双刚体系统一样，由于系统在水平面运动，故不考虑重力的作用。设关节点只、 b 和只分别是刚体蜀、b ，和e 的撞击中心，点g 、g ，和g ，分别是刚体b t 、b ： 和b 的质心，只点至g ，点和只点的距离分别为和，g ，点至只点的距离为b ， ( 见图4 4 ) 。 设刚体且在r 点的作用力为工。和z ，因此r 点的x 和y 方向分别产生加 速度。只点上为刚体b ：对杆蜀的作用力。刚体e 的动力学方程可写为 第四章欠驱动平面耦合刚体建模 f m l 薯6 = z 。s i n o l + ；，c o s o l f ， 枷l 步l g = 一；。c o s o l + ；，s i n o i e y ( 4 1 7 ) i j l 0 1 = 1 ：+ b l ( 只，s i n o , 一鼻，c o s 0 1 ) & ，1 7 ，磊群 ，。、： 卜开、p 鼠。k g j 上“j ua o 图4 3 平面三刚体耦合欠驱动系统模型 式中n l 。和分别表示刚体蜀的质量和相对质心g 的转动惯量。刚体马各点的几 何关系如下 一2 + 6 l c 。s q ( 4 1 8 ) l y i = y 1 6 + b ls i n 0 式中日是刚体b 与x 轴正方向的夹角。因只点是刚体b 的撞击中心，所以有 抚：- l 。由方程( 4 1 7 ) 和( 4 1 8 ) ，可以推导出下列方程 m 1 川1 戈1 + 只，( 1 + _ m l6 ) = c o s 鼠( 石，+ 孕砰( 鼻xc o s 臼1 + e ，s i n 0 ，) 一m 。6 l 谚2 ) ( 4 1 9 ) 7 夕l + f l y ( 1 + _ m lq 2 ) = s i n 岛( z ，+n 鼠) 一m 。6 1 卯) ( 4 孕1 b 1 2 ( f i x c o s e i + f l ys i 2 0 ) l 将以上两式的两边分别相除，并考虑e ，= m ：i ：。+ i 。，= r r l ：萝：6 + 川3 夕，6 t 得到刚体b i 的约束方程 一( m 耍t + ，l ( ，竹，舅， + m l j f l n ) ) s i n o , 2 3 第四章欠驱动平面耦合刚体建模 + ( m l _ 夕l + l l ( m 2 夕2 g + ，”3 j g ) ) c o s e , = 0 ( 4 2 1 ) 图4 4 刚体b 2 受力示意图 由图4 4 可列出刚体b ：的动力学方程 l m 2 j 2 g = 鼻，一f 2 ， m 2 耍2 g = e ，一e ， ( 4 2 2 ) l 2 岛= ，2 ( e ，s i n 0 2 一e ，c o s 0 2 ) + 6 2 ( 最，s i n 0 2 一只) c o s 0 2 ) 其中e ，= 职，墨。r ，= m ，j ；。，同理可得剐体坟豹约束方程 一( 刀? 2 吃置2 + ，2 m 3 量3 g ) s i n 0 2 + ( m 2r 2 夕2 + 1 2 m 3 歹3 g ) c o s = 0 ( 4 2 3 ) 同图4 2 - b ，可列出刚体b ，的动力学方程 i m 3 王3 g = e ， m 3 膏3 g = ey l ，3 岛= _ ( e ，s i n 0 3 一五yc o s 0 3 ) 同理，导出刚体b ，的约束方程 一m 3 王3s i n 0 3 + m 3 吩j j 3c o s 0 3 = 0 ( 4 2 4 ) ( 4 2 5 ) 由图d3 中几何关系可得到点只( i = 0 ，1 ，2 ) 和g ( ，= 1 ，2 ，3 ) 的加 3 = 搦+ ( c o s o k o ；+ s i n 0 ；k ) ( 4 2 6 ) 3 = 兄+ ( s i n o k o ：一c o s 吼反) ( 4 2 7 ) 第四章欠驱动平面耦合刚体建模 3 膏j = 量，+ 以( c o s o k 0 7 + s i n s t o k ) - r ( c o s e j o j + s i n o j o j ) ( 4 2 8 ) = 3 歹；= j ，3 + z 1 女( s i n o k o ? - c o s e , # , ) 一r j ( s i n o j o j c o s 已e ) ( 4 2 9 ) k = j 由方程( 4 2 5 ) 到方程( 4 2 9 ) 可知，刚体b 的各运动量可以通过刚体b ：和b 的运动来决定。即如果已知刚体b 3 的运动，可以反推出刚体b ：的运动，进而把 剐体e 的运动计算出来。 4 4 平面栉个刚体耦合欠驱动系统的建模 图4 5 所示为由系列被动转动关节连接并在水平面运动的n 个耦合刚体模 型，同样不考虑重力的作用。图中标号为从蜀到b 。的刚体由一系列被动转动关 节连接着，标号从鼻到只一的关节均为欠驱动关节。系统的控制输入加速度作用 在关节咒点上把r 关节点看作第一个关节，这样连接刚体b 。和e 的就是第i 个关节点e 一，( = - 2 ，疗) 。且假设质心q 和剐体置的只一。和只关节点在同一 条直线上。图4 6 中标出了刚体b 模型的各个量，设刚体b 在只一点的作用力为 兀和，因此只一点的x 和y 方向分别产生加速度。只点上为刚体口。对杆b i 的 作用力。刚体b 的动力学方程可写为 m 。i 6 = f 。s i n o j + f * e o s e , 一f 。 m 。6 = 一f 。c o s s i + ，。y s i n o ，一f 。y ( 4 3 0 ) ( 4 3 1 ) j ，0 i = 厶+ b ，( 瓦s i n 只一毛c o s o , ) ( 4 3 2 ) 上式中m ，和，分别表示刚体e 的质量和刚体相对质心的转动惯量。只是x 轴正 方向与刚体b i 的夹角。只一点至g ，点和只点的距离分别为r f 和，g ，点至只点的 距离为6 ，( 见图4 6 ) 。点p 和g ，的坐标关系如下 x ，= x f g - i - b ，c o s o f ( 4 3 3 ) y ? = y g + b | s i n o , ( 4 3 4 ) 由方程( 4 3 0 ) ( 4 3 4 ) ，可以得到下列方程 2 5 第四章欠驱动平面耦合刚体建模 m 赢+ ( 1 + 孚) r 卸一t m , r ，b i 胎i n 口 + ( + 弓 ( r c 。s 谚+ s i n 只) 一鸭6 p 2 ) c 。s 只 ( 4 3 5 ) 哪，+ ( 1 + 孚) _ ( 1 一竽拈。s 口 + ( 矗+ _ 了m , b _ 2 ( 气c 。s 只+ 矗s i n 只) 一棚，6 ，唧) s i n 只 ( 4 3 6 ) 图4 5 平面”个刚体耦合模型 一 图4 6 刚体受力示意图 l 第四章欠驱动平面耦合刚体建模 假设刚体e 和e + ，连接在刚体b i 的撞击中心i 口一l 处( f = 1 ， 一1 ) 。因此p 点就是刚体e 的撞击中心，；g e e b ，：土】。由方程( 4 3 5 ) 和方程( 4 3 6 ) ， m ， 可得 m ，+ 以( 1 + 孕6 7 ) ：c o s e , ( l 一， + 手酽( 以c o s 只+ s i n 只) 一6 ，印) ( 4 3 7 ) 小，j j + ( 1 + 手6 护s i n b ( 厶 + 了m iq 2 ( 兄c 。s 只+ 易s i n 只) 一脚，6 t 印) ( 4 - 3 8 ) 把( 4 3 7 ) 和( 4 3 8 ) 两个方程的左右两边分别相除，可得 s i n o , ( m ? r ? i ，+ f j ? 、+ c o s 0 , ( m i r 。? + f ，l t 、= 0 4 3 9 ) 9 , 1 郅1 体e + 。到e 的作用力可以用下面的式子表达： 兄= 耽 毛= 由方程( 4 3 9 ) ( 4 4 1 ) 可以推导出 ( 4 4 0 ) ( 4 4 1 ) nn - ( m ，j f ，+ ，m j f m ) s i n o ，+ ( m ，l j ) ，+ ，m 少) c o s o i = 0 ( 4 4 2 ) k = p ik l 方程( 4 4 2 ) 是一个二阶非完整约束方程。如果己知刚体b 。( i = l ，h - 1 ) 的各参数，且m r f i ，+ ，z m 。戈* 和m i - i ) ，+ j ) * 不同时为零时，n i l 。f i k b , 的 2 7 第四章欠驱动平面耦合刚体建模 坐标和只都可以用方程( 4 4 2 ) 计算出来。 由图4 6 中几何关系可得到点p 和g 的加速度 蕈，= i 。+ ，女( c o s 吼爵+ s i n 0 , o , ) ( 4 4 3 ) ；件1 歹，= 少。+ ( s i n o , t g ：- c o s o 女o , ) ( 4 4 4 ) 女；t + l = 叠。+ ，( c o s o j g + s i n o j o j ) 一( c o s 只卯+ s i n o , 0 , ) ( 4 4 5 ) 纠 j k = 或+ ，( s i n o j o j c o s 嘭谚) 一l ( s i n 只卯- c o s o , o , ) ( 4 4 6 ) j t 运用方程( 4 4 2 ) 到方程( 4 4 6 ) ，刚体垦的运动量可以通过刚体e 。( i = 1 ， n - 1 ) 的运动来决定。即如果刚体最的运动已知，则所有刚体的运动可以逐个 地计算出来。所有刚体的运动可以只用刚体b 。的各个运动量来表达。 4 5 本章小结 作为对末端为欠驱动杆的平面欠驱动机械臂的延伸，本章讨论了所有杆件均 由被动转动关节连接的平面耦合双刚体系统和平面三刚体耦合系统，以及平面一 个刚体耦合系统的建模。通过对各刚体进行动力学分析，结合刚体与刚体间的 几何关系，并考虑撞击中心处运动的特殊性，得到各关节点处加速度间的相互 关系。 第五章欠驱动平面耦合剐体运动规划 第五章欠驱动平面耦合刚体运动规划 5 1引言 上一章讨论了欠驱动平面耦合刚体系统建模以及欠驱动杆的运动学关系。本 章讨论运用这些关系式推导出只点处的控制输入加速度。通过控制输入的加速 度，系统能产生不同形式的转动和平动，组合这些运动形式使系统从种初始 状态运动到种目标状态，从而实现对欠驱动平面耦合刚体系统的运动规划。 5 2 欠驱动平面耦合双刚体的运动规划 5 2 1 欠驱动平面耦合双剐体的转动 如图5 1 所示的在水平面运动的由被动关节铰连接的双刚体耦合模型，在只 点的x 和y 方向上有加速度输入，用这两个加速度来控制系统的运动。下面详细 讨论在只点处输入加速度的求解。 将方程( 4 1 3 ) ( 4 1 6 ) 代入方程( 4 8 ) 中，用只点的加速度来表示， 可得如下式子 - x os i a e , + 夕o e o s o , + 嗣+ 五( 哦t o s s , 2 + 劣s i n # 1 2 ) = 0 ( 5 1 ) 式中如2 瓦署拿纛，b ：2 q p z 。将方程( 4 1 3 ) “1 6 ) 代入到方程“1 2 ) 中，也用点只的加速度来表示，得如下式子 一j os i n 0 2 + 萝oc o s 0 2 + ，2 岛+ ，l ( qc o s1 2 一卯s i n b2 ) = 0 ( 5 2 ) 方程( 5 1 ) 和( 5 2 ) 分别表示刚体骂和岛的二阶非完整约束。若刚体最不转 动( 幺= 馥= 0 ) ，则方程( 5 1 ) 可写成下式 一戈os i n o l + 歹oc o s o l + ，l 鼠= 0 ( 5 3 ) 由上式可知，刚体蜀可以绕暑点转动，而此过程中刚体岛位形始终保持不变， 如图5 1 - a 所示。 2 9 第五章欠驱动平面耦合刚体运动规划 将幺= 破= 0 代入方程( 5 1 ) 和( 5 2 ) ，可得在r 点的加速度输入 如= ，l ( 巍c o s 0 2 一c o s 鼠( 巨c o s 0 , 2 一卯s i n o , 2 ) ) s i n 0 , 2 ( 5 4 ) 歹o = ，l ( 鼋s i n 8 2 一s i n o , ( o , c o s 0 , 2 一卯s i n o , 2 ) ) s i n o , 2 ( 5 5 ) ( a ) 图5 1 刚体绕撞击中心的转动 如果冈体蜀和岛都有转动，系统将发生如图5 1 - b 所示的运动，即耦合 双刚体绕最点转动。用最点的加速度量来表示方程( 5 1 ) ，