CN104573255B - 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 - Google Patents
一种基于改进多体系统传递矩阵的机械臂建模与求解方法 Download PDFInfo
- Publication number
- CN104573255B CN104573255B CN201510031193.1A CN201510031193A CN104573255B CN 104573255 B CN104573255 B CN 104573255B CN 201510031193 A CN201510031193 A CN 201510031193A CN 104573255 B CN104573255 B CN 104573255B
- Authority
- CN
- China
- Prior art keywords
- mtd
- mrow
- rigid body
- joint
- mechanical arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Manipulator (AREA)
Abstract
本发明公开了一种基于改进多体系统传递矩阵的机械臂模型建立与求解方法。本发明根据平面机械臂的结构特点、运动传递和力传递关系，对多刚体系统离散时间传递矩阵方法进行改进，建立n自由度串联机械臂的统一模型，并针对机械臂关节空间和操作空间之间的两种已知运动情况，通过设置边界条件，提出系统模型的运动学与动力学的求解方法及流程。本发明的研究对象是平面串联机器人系统，利用多体系统离散时间传递矩阵改进方法建立的数学模型，不但具有建模灵活、计算规模小等特点，而且能同时涵盖系统的运动学和动力学特征，较传统机器人建模方法的求解更为有效，体现了新方法的优越性，达到了思路清晰、编程简单和易于实现的目的。
Description
技术领域
本发明涉及一种基于改进多体系统传递矩阵的机械臂建模方法与求解方法。
本发明根据平面机械臂的结构特点、运动传递和力传递关系，对多刚体系统离散时间传递矩阵方法进行改进，提出n关节串联机械臂且涵盖系统运动学和动力学特征的统一模型及其求解方法，属于关节型串联机器人技术领域。
背景技术
随着科学技术的快速发展，机器人在工业自动化生产、医学治疗、军事、半导体制造以及太空探索等领域得到了广泛应用。关节型串联机器人是模拟人的上臂而构成的，具有运动灵活、结构紧凑、运动惯性小、通用性强等优点，是机器人中使用最多的一种结构形式。
一、机器人的建模与分析是研究的基础和重点
串联多关节机器人系统是一个多自由度、多变量、高度非线性、多参数耦合的复杂系统，其运动学和动力学建模与分析是研究的基础和重点，对于系统的稳定性、可靠性及控制设计起着举足轻重的作用。因此，能够提出一种灵活快捷、计算效率高的建模方法对于设计、研发、控制机器人显得尤为重要。
二、机器人运动学和动力学统一建模与分析的需要
对于关节型串联机器人的运动学研究来说，普遍采用的方法是Denavit-Hartenberg(D-H)法，而常用的机器人动力学建模方法主要有Lagrange方法、Newton-Euler方程以及Kane方程等方法。然而，在串联多关节机械臂数学模型的研究中，几乎所有的建模方法都是针对系统运动学或动力学单独进行的，采用上述经典方法通常很难将这两方面的研究统一在一个数学描述中，而实际上，机械臂的运动学和动力学特性是相辅相成、相互关联的。因此，寻找一种能同时涵盖系统运动学与动力学特征的建模和求解方法，不但能更好的反映系统自身本质特性，而且能达到思路清晰和易于实现的目的。
发明内容
本发明的目的是解决现有方法很难将运动学与动力学两方面的研究统一在一个数学描述中的问题，利用改进的多体系统离散时间传递矩阵方法建立平面机械臂的动力学统一模型，并提出系统运动学和动力学有效的数值求解方法。
本发明所采用的技术方案是：首先根据串联多关节机械臂的结构特点、运动及力传递关系，分解系统为各个刚体元件组成的链式系统，并对多体系统离散时间传递矩阵方法进行改进，分别建立各关节和连杆的刚体元件传递矩阵方程；其次，由所有元件的传递矩阵 拼装n自由度串联平面运动机械臂的总体动力学方程；再次，基于这个既能描述机器人运动学规律又涵盖系统的动力学特性的统一模型，设置边界条件，获得系统的运动学和动力学的求解方法。
一种基于改进多体系统传递矩阵的机械臂建模方法，该方法具体实现步骤为：
第1、分解机械臂系统
平面运动串联机械臂由杆件和转动关节组成，将这些部件均看作一端输入一端输出的平面运动刚体，则整个机械臂就是一个由刚体元件组成的链式系统；假设机械臂有n个自由度，则系统共有2n个刚体元件，因此，从基座开始，第1、3、5…2n-1个刚体都是关节，第2、4、6…2n个刚体都是杆件；根据多体系统离散时间传递矩阵方法的建模思想，分析机械臂链式系统中每个刚体元件的输入端和输出端，并确定每个刚体元件的状态矢量为z＝[x,y,θ,m,qx,qy,1]T，这里x、y分别为刚体之间的联接点在惯性系中的位置坐标，θ为刚体相对z轴的角位移，m为联接点内力矩在z轴的坐标，qx、qy分别为联接点内力在该惯性系中坐标；
第2、杆件的模型
机械臂的杆件与关节固结在一起运动，既有绕关节的转动运动，又有牵连引起的平动运动；采用多体系统离散时间传递矩阵方法建立模型时，将杆件假设为均质连杆，其输入端位于靠近基座的一端，输出端为远离基座的一端，直接利用一端输入一端输出平面运动刚体的离散时间传递矩阵，得杆件的传递方程为
zi,i+1＝Uizi,i-1 (1)
式中i为杆件在全部刚体中的排列顺序，这里i＝2,4,…,2n；zi,i-1和zi,i+1分别为第i杆件的输入端和输出端状态矢量；Ui为第i杆件输入端与输出端之间的传递矩阵，是一个7×7的方阵，是刚体质量、转动惯量、几何参量和运动位置的函数，该矩阵能反映刚体之间的几何关系、运动关系和动力学关系；
第3、关节的模型
第3.1、多体离散时间传递矩阵法的改进
机械臂的关节，只有转动运动，没有平动运动；将关节看作中心刚体，其输入端为刚体的中心位置，输出端为中心刚体的整个圆周；对作平面运动的串联多关节机械臂来说，由于在关节处有电动机驱动，关节前后的角位移是不同的，因此，不能直接利用平面运动刚体的传递矩阵法建模，需要对离散时间传递矩阵进行改进；
假设从基座开始序号为第j-2个、第j-1和第j个刚体，j＝1,3,…,2n-1，的角位移分别为θj-2、θj-1和θj，这里第j-2个、第j个刚体为机械臂关节，第j-1个刚体为杆件；由于第j-2与第j-1个刚体是固结联接，因此满足θj-2＝θj-1；而第j个刚体由电动机驱动，在运动过程中会相对第j-2、第j-1个刚体转过一个角度θj'，因而满足θj＝θj-1+θj'，这样第j个刚体的输入端状态矢量和第j-1个刚体的输出端状态矢量中的角位移取值是不同的，用矩阵 的形式表示为
这里是第j个刚体输入端的状态矢量，是第j-1个刚体输出端的状态矢量，U′j表示第j个刚体的输入端与第j-1个刚体输出端之间的传递矩阵，即
第3.2、关节的模型
根据第3.1改进的离散时间传递矩阵方法对机械臂关节建模，从基座开始标号为j的关节刚体的输入端和输出端之间的传递方程为
zj,j+1＝UjU′jzj,j-1 (4)
式中j为关节的刚体序号，这里j＝1,3,…,2n-1；Uj表示第j个刚体的输出端与输入端之间的传递矩阵，用多体系统离散时间传递矩阵法直接得出，而U′j由式(3)计算得出，zj,j+1和zj,j-1分别为第j个刚体的输出端和第j-1个刚体输出端状态矢量；
第4、机械臂动力学统一模型
平面运动的n自由度串联机械臂有n个关节和n个杆件，共有2n个平面刚体，由于这些刚体元件是串联链式连接，这里采用多刚体系统传递矩阵方法，将上述对关节和杆件的模型进行统一，因此整个机械臂的总传递矩阵就是将2n个刚体的传递矩阵相乘，这时整个机械臂的动力学统一模型为
z2n,2n+1＝Uallz1,0 (5)
其中2n+1为机械臂的末端，Uall为系统的总传递矩阵，即
Uall＝U2nU2n-1U′2n-1…U4U3U′3U2U1U′1 (6)
式中Uk由传递矩阵离散时间方法确定，k＝1,2,3,…2n，U′k由式(3)确定，由此，式(5)和(6)就是串联n自由度机械臂的运动学动力学统一模型。
上述方法构建的机械臂运动学动力学统一模型的求解方法如下：
对串联多关节机械臂来说，按照运动学规律有两种情况，其一为已知关节运动规律求解末端的信息，即正向运动学求解；其二为已知末端点运动规律求解关节信息，属于运动学反解问题；对于上述模型中式(5)所示的n自由度机械臂的动力学总体方程，上述任何一 种运动情况的求解，都需要根据已知、未知条件，设置总体方程的边界条件，然后通过传递矩阵的迭代得到各状态矢量的未知参量；这些参量包含机械臂中各个关节和杆件位置及受力的全部信息，因此两种运动情况的求解都是运动学问题与逆动力学问题的叠加；
运动情况一：正向运动及统一模型的求解
当机械臂每个关节的运动规律给定时，由于关节和杆件是固结连接，此时系统中各个刚体元件的角位移都为已知参量，这时系统的边界条件为
式中表示，刚体1及末端角位移θ1,0、θ2n+1,2n为已知量，刚体1的输入端位移xI、yI始终为0，受到的内力和内力矩m1,0为未知参量，而标号为2n+1的机械臂末端的平动位移x2n+1,2n、y2n+1,2n为未知量，受到的内力和内力矩为0；
将式(7)代入式(5)，可解得z1,0和z2n+1,2n中的未知参量，并利用各个刚体元件的传递方程，即可求得系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。
所述机械臂运动学动力学统一模型的运动情况二：即运动反解及统一模型的求解方法如下：
当机械臂的末端运动规律已知时，求解的首要问题就是确定关节的运动规律，这是因为刚体各个点的运动和受力，即传递矩阵中的各元素都是角位移参量的函数；此时系统的边界条件表达式(7)中，关节1输入端的角位移θ1,0、内力和内力矩m1,0均为未知量，而机器人末端转角θ2n+1,2n为未知量，平动位移x2n+1,2n、y2n+1,2n为已知量，受到的内力和内力矩均为0；
具体求解过程，就是根据系统初始时刻的边界条件代入式(7)，计算初始时刻关节的初始角位移，然后将已知条件代入式(5)中，求解机械臂各状态矢量的全部信息，并利用此时刻传递矩阵的参数和下一时刻的边界条件，继续计算下一时刻关节角位移和状态矢量各未知参量，这样反复迭代，即可求得所有时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。
本发明的优点和积极效果：
本发明主要涉及基于改进多体系统传递矩阵的机械臂动力学建模与求解方法，其优势在于：(1)基于改进的离散时间传递矩阵n机械臂建模方法仍然保留多体系统传递矩阵方法在建模简洁、阶次低、易于编程、无需求解微分方程、便于实际应用等方面的优点；(2)所提机械臂的数学模型将运动学和动力学两方面因素同时纳入一个统一方程中，在求解时可以根据运动情况设置边界条件，同时获得运动学与动力学特性，具有灵活便捷、便于实现的特点。
附图说明
图1是串联二自由度平面机械臂示意图。
图2是关节运动角位移示意图。
图3是正向运动与机械臂统一模型求解流程。
图4是运动反解与统一动力学模型求解流程。
图5是正向运动机械臂运动位形图。
图6是正向运动机械臂各刚体部件的x方向位移。
图7是正向运动机械臂各刚体部件的y方向位移。
图8是正向运动机械臂各刚体部件的角位移。
图9是正向运动机械臂各刚体部件的内力矩。
图10是正向运动机械臂各刚体部件的x方向内力。
图11是正向运动机械臂各刚体部件的y方向内力。
图12是运动学反解机械臂运动位形图。
图13是运动学反解机械臂各刚体部件的x方向位移。
图14是运动学反解机械臂各刚体部件的y方向位移。
图15是运动学反解机械臂各刚体部件的角位移。
图16是运动学反解机械臂各刚体部件的内力矩。
图17是运动学反解运动学反解机械臂各刚体部件的x方向内力。
图18是运动学反解机械臂各刚体部件的y方向内力。
图19是运动学反解机械臂末端y方向位移、速度和加速度。
具体实施方式
实施例1、机械臂动力学建模
为详细说明本发明对多体系统传递矩阵方法的改进及其应用于机械臂动力学建模与求解的技术方法，下面以平面运动二自由度串联机械臂为例加以说明，具体步骤如下：
第1、分解机械臂系统
如图1所示为二自由度平面运动机械臂，对此刚体系统进行分解，则整个机械臂是由4个一端输入一端输出的平面刚体元件组成的链式系统。图中，序号0表示基座，从基座开始，序号为第1和第3的刚体表示关节，序号为第2和第4的刚体表示杆件，序号5表示机械臂末端。根据多体系统离散时间传递矩阵的建模思想，按照z＝[x,y,θ,m,qx,qy,1]T的形式定义每个刚体元件的状态矢量。这里定义杆件和关节刚体质量：M1＝1kg，M2＝1kg，M3＝1kg，M4＝1kg；关节半径：r1＝0.1m，r3＝0.1m；关节质心转动惯量：JC1＝0.005kgm2， JC3＝0.005kgm2；杆件长度：l2＝1m，l4＝0.1m；杆件质心转动惯量：JC2＝0.005kgm2，JC4＝0.005kgm2。
第2、杆件的模型
二自由度机械臂的杆件2和4分别与关节1和3固结在一起运动，既有绕关节的转动运动，又有牵连引起的平动运动。将刚体杆件2和4假设为均质连杆，采用多体系统离散时间传递矩阵方法建立模型时，定义杆件输入端分别位于靠近基座的一端，输出端为远离基座的一端，直接利用一端输入一端输出平面运动刚体的离散时间传递矩阵，得刚体2和4的传递方程分别为
z2,3＝U2z2,1和z4,5＝U4z4,3 (1)
式中z2,1和z4,3分别为刚体2和4的输入端状态矢量，而z2,3和z4,5分别为刚体2和4的输出端状态矢量。U2和U4分别为刚体2和4的传递矩阵。
第3、关节的模型
第3.1、多体离散时间传递矩阵法的改进
机械臂的关节，只有转动运动，没有平动运动。建模时，将二自由度机械臂的关节1和3看作中心刚体，其输入端即为刚体的中心位置，输出端为中心刚体的整个圆周。对作平面运动的二自由度串联机械臂来说，由于在关节1和3处有电动机驱动，因此关节前后的角位移是不同的，因此，不能直接利用平面运动刚体的离散时间传递矩阵法建模，需要对传递矩阵进行改进。
如图2所示，从基座开始序号为第1、第2和第3个刚体的角位移分别为θ1、θ2和θ3，；由于第2与第1个刚体是固结联接，因此满足θ1＝θ2。而第3个刚体由电动机驱动，在运动过程中会相对第2个刚体转过一个角度θ3'，因而角位移满足θ3＝θ2+θ3'，这样第3个刚体的输入端状态矢量和第2个刚体的输出端状态矢量中的角位移取值是不同的，用矩阵的形式可以表示为
这里是第3个刚体输入端的状态矢量，是第2个刚体输出端的状态矢量，U′3表示第3个刚体的输入端与第2个刚体输出端之间的传递矩阵，即
第3.2、关节的模型
对于关节1，由于它的输入端是整个机械臂系统的输入端，因此可以采用上述改进多体系统传递矩阵方法建模，也可以将其角位移的变化直接作为状态矢量的输入值给出，这里为了简化方程，采用后者。而在对机械臂关节3进行建模时，必须采用上述改进的离散时间传递矩阵方法，得出其输入端和输出端之间的传递方程为
z3,4＝U3U′3z3,2 (4)
式中U3表示关节3的输出端与输入端之间的传递矩阵，用多体系统离散时间传递矩阵法直接可得，而U′3可由式(3)计算得出，z3,4和z3,2分别为关节3和杆件2的输出端状态矢量。
第4、机械臂的总体模型
平面运动的二自由度串联机械臂有2个关节和2个杆件，共有4个平面刚体，由于这些刚体元件是串联链式连接，这里采用多刚体系统传递矩阵方法，将上述对关节和杆件的模型进行统一，因此整个机械臂的总传递矩阵就是将4个刚体的传递矩阵相乘，这时整个机械臂的动力学统一模型为
z4,5＝Uallz1,0 (5)
如前所述序号5为机械臂的末端，Uall为系统的总传递矩阵，即
Uall＝U4U3U′3U2U1 (6)
式中U1、U2、U3和U4由传递矩阵离散时间方法确定，当刚体的质量、转动惯量、几何参量和运动情况都为已知量时，U2和U4是可直接计算的，U′3由式(3)确定，它是关节1和3运动角位移的函数，由此，式(5)和(6)就是串联二自由度机械臂的动力学统一模型。
实施例2、模型的求解
对串联多关节机械臂来说，按照运动学规律有两种情况，其一为已知关节运动规律求解末端的信息，即正向运动学求解；其二为已知末端点运动规律求解关节信息，属于运动学反解问题。对于式(5)所示的二自由度机械臂的动力学总体方程，上述任何一种运动情况的求解，都需要根据已知、未知条件，设置总体方程的边界条件，然后通过传递矩阵的迭代得到各状态矢量的未知参量。这些参量包含机械臂中各个关节和杆件位置及受力的全部信息，因此两种运动情况的求解都是运动学问题与逆动力学问题的叠加。
1、求解方法一、正向运动及模型的求解
假设机械臂初始位置为两连杆展开成一条直线，且与惯性坐标系的x轴平行，当机械臂关节1和3的运动规律给定时，由于关节和杆件是固结连接，此时系统中1-4刚体元件的角位移都为已知参量。这里，设关节1和关节2的运动规律满足
式中为关节运动期望值，且因此有θ1,0＝θ2,1＝θ1，θ3,2＝θ4,3＝θ5,4＝θ1+θ2。T2为运动总历时时间，T1、T1′分别为运动起始、运动终止延时时间，且T1＝20ms，T1′＝40ms，T2＝5s。
根据式(7)中的关节规律，直接求解刚体1-4的各个时刻的角位移，并代入式(6)中，可以将动力学模型中各时刻的总传递矩阵Uall直接计算出，而且，根据上述条件设置机械臂系统的边界条件为
式中刚体1及末端角位移θ1,0、θ5,4为已知量，刚体1的输入端位移xI、yI始终为0，受到的内力和内力矩m1,0为未知参量，而序号为5的机械臂末端的平动位移x5,4、y5,4为未知量，受到的内力和内力矩为0。
将式(8)初始时刻代入式(5)，则可解得该时刻z1,0和z5,4中的未知参量，并利用各个刚体元件的传递方程，即可求得该时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。这样反复迭代，即可求得所有时刻刚体的全部信息。正向运动模型求解如图3所示流程。
2、求解方法二、运动反解及模型的求解
当机械臂的末端运动规律已知时，求解的首要问题就是确定关节的运动规律，这是因为刚体各个点的运动和受力，即传递矩阵中的各元素都是角位移参量的函数。这里假设二自由度机器人末端运动的起始点为(0.3,0)，且x方向始终为定值，即x＝xstart＝xend＝0.3，y方向的运动遵循如下规律
式中，ystart＝0，且y≤T1期间，y作匀加速运动，在T1≤y≤T期间，y作匀加速运动，T2≤y≤T期间，y作匀减速运动，且有T1＝1s，T2＝4s，T＝5s。
而在系统的边界条件表达式(8)中，关节1输入端的角位移θ1,0、内力和内力矩m1,0均为未知量，而机械臂末端转角θ2n+1,2n为未知量，平动位移x2n+1,2n、y2n+1,2n为已知量，受到的内力和内力矩均为0。
对二自由度平面运动机械臂进行运动学反解的求解过程中，首先根据式(8)中系统初始时刻的边界条件，计算初始时刻关节的初始角位移，然后将已知条件代入式(6)和式(5)中，求解机械臂传递矩阵和各状态矢量的全部信息，并利用此时刻传递矩阵的参数和下一时刻的边界条件，继续计算下一时刻关节角位移、总传递矩阵和状态矢量各未知参量，这样反 复迭代，即可求得所有时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。运动学反解求解流程如图4所示。
3、正向运动模型求解数值仿真与分析
采用说明书所述方法对实施例的平面运动二自由度机械臂进行模型正向运动求解，如图5-图11为机械臂正向运动运动学、动力学特性曲线。其中，图5所示为关节运动时，机械臂整体的位形图。图6和图7分别为采用传递矩阵改进方法计算的机械臂刚体1～4输出端x、y方向的位移曲线，图中的x1、x2、x3、x4和y1、y2、y3、y4。为了验证此方法的正确性，图中还示出了采用D-H坐标方法计算各部件的位移曲线，如图中标了下标为robot的曲线，两种方法计算结果完全吻合。图8则表示机械臂中各刚体运动过程的角位移，其中关节1和连杆2的转角位移相同，而关节3和连杆4的转角位移相同。图9是各刚体的内力矩，图中所示m1和m3曲线即是机械臂实现运动关节1和关节3所提供的驱动力矩。图10和图11分别为各刚体x、y方向的内力，从图中可知，在所有刚体中，关节1受到的力最大，其数值与杆件2输入端受到的力相等。
4、运动学反解模型求解数值仿真与分析
采用说明书所述方法对实施例的平面运动二自由度机械臂进行运动学反解和逆动力学求解，如图12-图18为机械臂运动学反解运动学、动力学特性曲线。其中，图12所示为机械臂运动位形图。图13和图14分别为各刚体部件的x、y方向位移，两图中的结果与运用机器人学方法计算结果一致，说明多体系统传递矩阵方法的正确性。图15表示各刚体运动的角位移，图中说明中心刚体1与连杆2角位移运动一致，关节3与杆件4角位移运动一致。图16为各刚体的内力矩，图中m1和m3分别是两个关节的驱动力矩，这里关节1所受力矩最大。图17和18分别为各刚体x、y方向的内力，图中表示，刚体1受到的力最大。图19为末端y方向的位移、速度和加速度。
Claims (3)
1.一种基于改进多体系统传递矩阵的机械臂建模方法，其特征在于具体实现步骤为：
第1、分解机械臂系统
平面运动串联机械臂由杆件和转动关节组成，将这些部件均看作一端输入一端输出的平面运动刚体，则整个机械臂就是一个由刚体元件组成的链式系统；假设机械臂有n个自由度，则系统共有2n个刚体元件，因此，从基座开始，第1、3、5…2n-1个刚体都是关节，第2、4、6…2n个刚体都是杆件；根据多体系统离散时间传递矩阵方法的建模思想，分析机械臂链式系统中每个刚体元件的输入端和输出端，并确定每个刚体元件的状态矢量为z＝[x,y,θ,m,qx,qy,1]T，这里x、y分别为刚体之间的联接点在惯性系中的位置坐标，θ为刚体相对z轴的角位移，m为联接点内力矩在z轴的坐标，qx、qy分别为联接点内力在该惯性系中坐标；
第2、杆件的模型
机械臂的杆件与关节固结在一起运动，既有绕关节的转动运动，又有牵连引起的平动运动；采用多体系统离散时间传递矩阵方法建立模型时，将杆件假设为均质连杆，其输入端位于靠近基座的一端，输出端为远离基座的一端，直接利用一端输入一端输出平面运动刚体的离散时间传递矩阵，得杆件的传递方程为
zi,i+1＝Uizi,i-1 (1)
式中i为杆件在全部刚体中的排列顺序，这里i＝2,4,…,2n；zi,i-1和zi,i+1分别为第i杆件的输入端和输出端状态矢量；Ui为第i杆件输入端与输出端之间的传递矩阵，是一个7×7的方阵，是刚体质量、转动惯量、几何参量和运动位置的函数，能反映刚体之间的几何关系、运动关系和动力学关系；
第3、关节的模型
第3.1、多体离散时间传递矩阵法的改进
机械臂的关节，只有转动运动，没有平动运动；将关节看作中心刚体，其输入端为刚体的中心位置，输出端为中心刚体的整个圆周；对作平面运动的串联多关节机械臂来说，由于在关节处有电动机驱动，关节前后的角位移是不同的，因此，不能直接利用平面运动刚体的传递矩阵法建模，需要对离散时间传递矩阵进行改进；
假设从基座开始序号为第j-2个、第j-1和第j个刚体，j＝1,3,…,2n-1，的角位移分别为θj-2、θj-1和θj，这里第j-2个、第j个刚体为机械臂关节，第j-1个刚体为杆件；由于第j-2与第j-1个刚体是固结联接，因此满足θj-2＝θj-1；而第j个刚体由电动机驱动，在运动过程中会相对第j-2、第j-1个刚体转过一个角度θj'，因而满足θj＝θj-1+θj'，这样第j个刚体的输入端状态矢量和第j-1个刚体的输出端状态矢量中的角位移取值是不同的，用矩阵的形式表示为
<mrow>
<msub>
<mi>z</mi>
<msub>
<mi>I</mi>
<mi>j</mi>
</msub>
</msub>
<mo>=</mo>
<msubsup>
<mi>U</mi>
<mi>j</mi>
<mo>&prime;</mo>
</msubsup>
<mo>&CenterDot;</mo>
<msub>
<mi>z</mi>
<msub>
<mi>O</mi>
<mrow>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
这里是第j个刚体输入端的状态矢量，是第j-1个刚体输出端的状态矢量，U′j表示第j个刚体的输入端与第j-1个刚体输出端之间的传递矩阵，即
<mrow>
<msubsup>
<mi>U</mi>
<mi>j</mi>
<mo>&prime;</mo>
</msubsup>
<mo>=</mo>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&theta;</mi>
<mi>j</mi>
<mo>&prime;</mo>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
式中θj'是第j-1个刚体转过的角度；
第3.2、关节的模型
根据第3.1改进的离散时间传递矩阵方法对机械臂关节建模，从基座开始标号为j的关节刚体的输入端和输出端之间的传递方程为
zj,j+1＝UjU′jzj,j-1 (4)
式中j为关节的刚体序号，这里j＝1,3,…,2n-1；Uj表示第j个刚体的输出端与输入端之间的传递矩阵，用多体系统离散时间传递矩阵法直接得出，而U′j由式(3)计算得出，zj,j+1和zj,j-1分别为第j个刚体的输出端和第j-1个刚体输出端状态矢量；
第4、机械臂动力学统一模型
平面运动的n自由度串联机械臂有n个关节和n个杆件，共有2n个平面刚体，由于这些刚体元件是串联链式连接，这里采用多刚体系统传递矩阵方法，将上述关节和杆件的模型进行统一，因此整个机械臂的总传递矩阵就是将2n个刚体的传递矩阵相乘，这时整个机械臂的动力学统一模型为
z2n,2n+1＝Uallz1,0 (5)
其中2n+1为机械臂的末端，Uall为系统的总传递矩阵，即
Uall＝U2nU2n-1U′2n-1…U4U3U′3U2U1U′1 (6)
式中Uk由传递矩阵离散时间方法确定，k＝1,2,3,…2n，U′m由式(3)确定，由此，式(5)和(6)就是串联n自由度机械臂的运动学动力学统一模型。
2.一种权利要求1所述方法构建的机械臂运动学动力学统一模型的求解方法，其特征在于所述求解方法如下：
对串联多关节机械臂来说，按照运动学规律有两种情况，其一为已知关节运动规律求解末端的信息，即正向运动学求解；其二为已知末端点运动规律求解关节信息，属于运动学反解问题；对于权利要求1中式(5)所示的n自由度机械臂的动力学总体方程，上述任何一种运动情况的求解，都需要根据已知、未知条件，设置总体方程的边界条件，然后通过传递矩阵的迭代得到各状态矢量的未知参量；这些参量包含机械臂中各个关节和杆件位置及受力的全部信息，因此两种运动情况的求解都是运动学问题与逆动力学问题的叠加；运动情况一：正向运动及统一模型的求解
当机械臂每个关节的运动规律给定时，由于关节和杆件是固结连接，此时系统中各个刚体元件的角位移都为已知参量，这时系统的边界条件为
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
</msub>
<mo>=</mo>
<msubsup>
<mrow>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>m</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>q</mi>
<msub>
<mi>x</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
</msub>
</msub>
<mo>,</mo>
<msub>
<mi>q</mi>
<msub>
<mi>y</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
</msub>
</msub>
<mo>,</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>0</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mrow>
<mn>2</mn>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mi>n</mi>
</mrow>
</msub>
<mo>=</mo>
<msubsup>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mi>x</mi>
<mrow>
<mn>2</mn>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mi>n</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>y</mi>
<mrow>
<mn>2</mn>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mi>n</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>&theta;</mi>
<mrow>
<mn>2</mn>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mi>n</mi>
</mrow>
</msub>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<mn>0</mn>
<mo>,</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mn>2</mn>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mi>n</mi>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
2
式中表示，刚体1及末端角位移θ1,0、θ2n+1,2n为已知量，刚体1的输入端位移xI、yI始终为0，受到的内力和内力矩m1,0为未知参量，而标号为2n+1的机械臂末端的平动位移x2n+1,2n、y2n+1,2n为未知量，受到的内力和内力矩为0；
将式(7)代入式(5)，可解得z1,0和z2n+1,2n中的未知参量，并利用各个刚体元件的传递方程，即可求得系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。
3.根据权利要求2所述的机械臂运动学动力学统一模型的求解方法，其特征在于所述机械臂运动学动力学统一模型的运动情况二：即运动反解及统一模型的求解方法如下：
当机械臂的末端运动规律已知时，求解的首要问题就是确定关节的运动规律，这是因为刚体各个点的运动和受力，即传递矩阵中的各元素都是角位移参量的函数；此时系统的边界条件表达式(7)中，关节1输入端的角位移θ1,0、内力和内力矩m1,0均为未知量，而机器人末端转角θ2n+1,2n为未知量，平动位移x2n+1,2n、y2n+1,2n为已知量，受到的内力和内力矩均为0；
具体求解过程，就是根据系统初始时刻的边界条件代入式(7)，计算初始时刻关节的初始角位移，然后将已知条件代入式(5)中，求解机械臂各状态矢量的全部信息，并利用此时刻传递矩阵的参数和下一时刻的边界条件，继续计算下一时刻关节角位移和状态矢量各未知参量，这样反复迭代，即可求得所有时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510031193.1A CN104573255B (zh) | 2015-01-22 | 2015-01-22 | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510031193.1A CN104573255B (zh) | 2015-01-22 | 2015-01-22 | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104573255A CN104573255A (zh) | 2015-04-29 |
| CN104573255B true CN104573255B (zh) | 2017-10-31 |
Family
ID=53089309
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510031193.1A Expired - Fee Related CN104573255B (zh) | 2015-01-22 | 2015-01-22 | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104573255B (zh) |
Families Citing this family (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106777475B (zh) * | 2016-11-17 | 2019-11-26 | 贵州大学 | 一种有限空间约束的注塑机械臂动力学协同仿真方法 |
| CN108362483B (zh) * | 2017-12-11 | 2021-05-18 | 中国船舶重工集团公司第七一九研究所 | 一种管路系统的监测方法和监测系统 |
| CN108733917B (zh) * | 2018-05-17 | 2022-04-19 | 桂林电子科技大学 | 一种基于传递矩阵的载货汽车动力学建模方法 |
| CN108803350B (zh) * | 2018-08-16 | 2020-06-23 | 居鹤华 | 基于轴不变量的动基座多轴机器人动力学建模与解算方法 |
| CN108972558B (zh) | 2018-08-16 | 2020-02-21 | 居鹤华 | 一种基于轴不变量的多轴机器人动力学建模方法 |
| CN109086544B (zh) * | 2018-08-16 | 2019-12-24 | 居鹤华 | 基于轴不变量的闭链机器人动力学建模与解算方法 |
| CN108942942B (zh) | 2018-08-16 | 2020-01-07 | 居鹤华 | 一种基于轴不变量的多轴机器人逆运动学建模与解算方法 |
| CN108983705B (zh) * | 2018-08-16 | 2020-01-07 | 居鹤华 | 一种基于轴不变量的多轴机器人系统正运动学建模与解算方法 |
| CN109117451B (zh) * | 2018-08-16 | 2020-03-13 | 居鹤华 | 基于轴不变量的树链机器人动力学建模与解算方法 |
| EP3754837B1 (de) | 2019-06-17 | 2024-01-17 | Schneider Electric Industries SAS | Verfahren zur überwachung einer maschine |
| CN110879872B (zh) * | 2019-11-15 | 2021-09-17 | 中国科学院长春光学精密机械与物理研究所 | 一种基于传递矩阵的车辆系统传递函数求解方法 |
| CN113408072B (zh) * | 2021-06-25 | 2023-10-13 | 扬州大学 | 风力机柔塔系统固有振动特性快速建模及仿真方法 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101982822A (zh) * | 2010-11-10 | 2011-03-02 | 哈尔滨工业大学 | 空间六自由度运动系统模态建模方法 |
-
2015
- 2015-01-22 CN CN201510031193.1A patent/CN104573255B/zh not_active Expired - Fee Related
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101982822A (zh) * | 2010-11-10 | 2011-03-02 | 哈尔滨工业大学 | 空间六自由度运动系统模态建模方法 |
Non-Patent Citations (2)
| Title |
|---|
| Transfer Matrix Method for Multi-body Systems Dynamics Modeling of Rescue Robot;Yue Guo et al;《Proceedings of 2014 IEEE International Conference on Mechatronics and Automation》;20141231;全文 * |
| 多体系统传递矩阵法研究进展;芮筱亭等;《力学进展》;20120125(第1期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104573255A (zh) | 2015-04-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104573255B (zh) | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 | |
| CN107756400A (zh) | 一种基于旋量理论的6r机器人逆运动学几何求解方法 | |
| Nugroho et al. | Design and implementation of kinematics model and trajectory planning for NAO humanoid robot in a tic-tac-toe board game | |
| Faudzi et al. | Soft manipulator using thin McKibben actuator | |
| CN106041932A (zh) | 一种ur机器人的运动控制方法 | |
| Gharahsofloo et al. | An efficient algorithm for workspace generation of delta robot | |
| Aghajanzadeh et al. | An offline geometric model for controlling the shape of elastic linear objects | |
| Zhao et al. | Research on path planning of robot arm based on RRT-connect algorithm | |
| Zhao et al. | In-hand manipulation using a 3-PRS-finger-based parallel dexterous hand with bidirectional pinching capability | |
| CN112560262B (zh) | 一种三指灵巧手动力学建模方法、系统、设备及存储介质 | |
| Dehghani et al. | Dynamics modeling of planar continuum robots by finite circular elements for motion control | |
| Wu et al. | Research on modeling and simulation of cable-driven bionic octopus arm based on simmechanics | |
| Saafi et al. | Development of a spherical parallel manipulator as a haptic device for a tele-operation system: Application to robotic surgery | |
| Syaifuddin et al. | Designing 8 degrees of freedom humanoid robotic arm | |
| Jingwei et al. | Welding Robot Kinematics Analysis and Trajectory Planning. | |
| Kuo et al. | Motion planning and control of interactive humanoid robotic arms | |
| Zhang et al. | Analysis and Simulation of Mechanical Arm Dynamics Model Based on Simulink | |
| Liu et al. | A Novel Hole-Tendon-Approximation-Model Based Control for Humanoid Dexterous Hand | |
| Zou et al. | Six-axis industrial manipulator modeling and MATLAB simulation | |
| Xu et al. | Kinematic Analysis and Trajectory Planning Simulation of Citrus Picking Manipulator | |
| CN109571483B (zh) | 一种空间机械臂任务轨迹规划域构建方法 | |
| Amar et al. | Dynamics and Control of Tree-Type Manipulator Systems using Exponential Coordinates | |
| Eichhorn et al. | Modelling of the 4-axis kinematic manipulator airarm driven by pneumatic muscle actuators | |
| Lu et al. | Kinematics analysis and simulation on transfer robot with six degrees of freedom | |
| Zhu et al. | Kinematics of Three-Fingered Dexterous Hands Driven by Pneumatic Artificial Muscle Based on Vector Product Method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20171031 Termination date: 20190122 |