一种基于改进多体系统传递矩阵的机械臂建模与求解方法

摘要

本发明公开了一种基于改进多体系统传递矩阵的机械臂模型建立与求解方法。本发明根据平面机械臂的结构特点、运动传递和力传递关系，对多刚体系统离散时间传递矩阵方法进行改进，建立n自由度串联机械臂的统一模型，并针对机械臂关节空间和操作空间之间的两种已知运动情况，通过设置边界条件，提出系统模型的运动学与动力学的求解方法及流程。本发明的研究对象是平面串联机器人系统，利用多体系统离散时间传递矩阵改进方法建立的数学模型，不但具有建模灵活、计算规模小等特点，而且能同时涵盖系统的运动学和动力学特征，较传统机器人建模方法的求解更为有效，体现了新方法的优越性，达到了思路清晰、编程简单和易于实现的目的。

说明书

技术领域

本发明涉及一种基于改进多体系统传递矩阵的机械臂建模方法与求解方法。

本发明根据平面机械臂的结构特点、运动传递和力传递关系，对多刚体系统离散时间传递矩阵方法进行改进，提出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,q_x,q_y,1]^T，这里x、y分别为刚体之间的联接点在惯性系中的位置坐标，θ为刚体相对z轴的角位移，m为联接点内力矩在z轴的坐标，q_x、q_y分别为联接点内力在该惯性系中坐标；

第2、杆件的模型

机械臂的杆件与关节固结在一起运动，既有绕关节的转动运动，又有牵连引起的平动运动；采用多体系统离散时间传递矩阵方法建立模型时，将杆件假设为均质连杆，其输入端位于靠近基座的一端，输出端为远离基座的一端，直接利用一端输入一端输出平面运动刚体的离散时间传递矩阵，得杆件的传递方程为

z_i,i+1＝U_iz_i,i-1   (1)

式中i为杆件在全部刚体中的排列顺序，这里i＝2,4,…,2n；z_i,i-1和z_i,i+1分别为第i杆件的输入端和输出端状态矢量；U_i为第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个刚体的输出端状态矢量中的角位移取值是不同的，用矩阵的形式表示为

$>z_{I_j}=U_j^{\prime }·z_{O_{j-1}}---\left(2\right)$>

这里是第j个刚体输入端的状态矢量，是第j-1个刚体输出端的状态矢量，U′_j表示第j个刚体的输入端与第j-1个刚体输出端之间的传递矩阵，即

$>U_j^{\prime }=\left(\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& {\theta }_j^{\prime }\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right)---\left(3\right);$>

第3.2、关节的模型

根据第3.1改进的离散时间传递矩阵方法对机械臂关节建模，从基座开始标号为j的关节刚体的输入端和输出端之间的传递方程为

z_j,j+1＝U_jU′_jz_j,j-1   (4)

式中j为关节的刚体序号，这里j＝1,3,…,2n-1；U_j表示第j个刚体的输出端与输入端之间的传递矩阵，用多体系统离散时间传递矩阵法直接得出，而U′_j由式(3)计算得出，z_j,j+1和z_j,j-1分别为第j个刚体的输出端和第j-1个刚体输出端状态矢量；

第4、机械臂动力学统一模型

平面运动的n自由度串联机械臂有n个关节和n个杆件，共有2n个平面刚体，由于这些刚体元件是串联链式连接，这里采用多刚体系统传递矩阵方法，将上述对关节和杆件的模型进行统一，因此整个机械臂的总传递矩阵就是将2n个刚体的传递矩阵相乘，这时整个机械臂的动力学统一模型为

z_2n,2n+1＝U_allz_1,0   (5)

其中2n+1为机械臂的末端，U_all为系统的总传递矩阵，即

U_all＝U_2nU_2n-1U′_2n-1…U₄U₃U′₃U₂U₁U′₁   (6)

式中U_k由传递矩阵离散时间方法确定，k＝1,2,3,…2n，U′_k由式(3)确定，由此，式(5)和(6)就是串联n自由度机械臂的运动学动力学统一模型。

上述方法构建的机械臂运动学动力学统一模型的求解方法如下：

对串联多关节机械臂来说，按照运动学规律有两种情况，其一为已知关节运动规律求解末端的信息，即正向运动学求解；其二为已知末端点运动规律求解关节信息，属于运动学反解问题；对于上述模型中式(5)所示的n自由度机械臂的动力学总体方程，上述任何一种运动情况的求解，都需要根据已知、未知条件，设置总体方程的边界条件，然后通过传递矩阵的迭代得到各状态矢量的未知参量；这些参量包含机械臂中各个关节和杆件位置及受力的全部信息，因此两种运动情况的求解都是运动学问题与逆动力学问题的叠加；

运动情况一：正向运动及统一模型的求解

当机械臂每个关节的运动规律给定时，由于关节和杆件是固结连接，此时系统中各个刚体元件的角位移都为已知参量，这时系统的边界条件为

$>\left(\begin{array}{c}{z}_{\mathrm{1,0}}={[\mathrm{0,0},{\theta }_{\mathrm{1,0}},m_{\mathrm{1,0}},q_{x_{\mathrm{1,0}}},q_{y_{\mathrm{1,0}}},1]}_{\mathrm{1,0}}^T\\ z_{2n+\mathrm{1,2}n}={[x_{2n+\mathrm{1,2}n},y_{2n+\mathrm{1,2}n},{\theta }_{2n+\mathrm{1,2}n},\mathrm{0,0,0,1}]}_{2n+\mathrm{1,2}n}^T\end{array}\right)---\left(7\right)$>

式中表示，刚体1及末端角位移θ_1,0、θ_2n+1,2n为已知量，刚体1的输入端位移x_I、y_I始终为0，受到的内力和内力矩m_1,0为未知参量，而标号为2n+1的机械臂末端的平动位移x_2n+1,2n、y_2n+1,2n为未知量，受到的内力和内力矩为0；

将式(7)代入式(5)，可解得z_1,0和z_2n+1,2n中的未知参量，并利用各个刚体元件的传递方程，即可求得系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。

所述机械臂运动学动力学统一模型的运动情况二：即运动反解及统一模型的求解方法如下：

当机械臂的末端运动规律已知时，求解的首要问题就是确定关节的运动规律，这是因为刚体各个点的运动和受力，即传递矩阵中的各元素都是角位移参量的函数；此时系统的边界条件表达式(7)中，关节1输入端的角位移θ_1,0、内力和内力矩m_1,0均为未知量，而机器人末端转角θ_2n+1,2n为未知量，平动位移x_2n+1,2n、y_2n+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,q_x,q_y,1]^T的形式定义每个刚体元件的状态矢量。这里定义杆件和关节刚体质量：M₁＝1kg，M₂＝1kg，M₃＝1kg，M₄＝1kg；关节半径：r₁＝0.1m，r₃＝0.1m；关节质心转动惯量：J_C1＝0.005kgm²，J_C3＝0.005kgm²；杆件长度：l₂＝1m，l₄＝0.1m；杆件质心转动惯量：J_C2＝0.005kgm²，J_C4＝0.005kgm²。

第2、杆件的模型

二自由度机械臂的杆件2和4分别与关节1和3固结在一起运动，既有绕关节的转动运动，又有牵连引起的平动运动。将刚体杆件2和4假设为均质连杆，采用多体系统离散时间传递矩阵方法建立模型时，定义杆件输入端分别位于靠近基座的一端，输出端为远离基座的一端，直接利用一端输入一端输出平面运动刚体的离散时间传递矩阵，得刚体2和4的传递方程分别为

z_2,3＝U₂z_2,1和z_4,5＝U₄z_4,3   (1)

式中z_2,1和z_4,3分别为刚体2和4的输入端状态矢量，而z_2,3和z_4,5分别为刚体2和4的输出端状态矢量。U₂和U₄分别为刚体2和4的传递矩阵。

第3、关节的模型

第3.1、多体离散时间传递矩阵法的改进

机械臂的关节，只有转动运动，没有平动运动。建模时，将二自由度机械臂的关节1和3看作中心刚体，其输入端即为刚体的中心位置，输出端为中心刚体的整个圆周。对作平面运动的二自由度串联机械臂来说，由于在关节1和3处有电动机驱动，因此关节前后的角位移是不同的，因此，不能直接利用平面运动刚体的离散时间传递矩阵法建模，需要对传递矩阵进行改进。

如图2所示，从基座开始序号为第1、第2和第3个刚体的角位移分别为θ₁、θ₂和θ₃，；由于第2与第1个刚体是固结联接，因此满足θ₁＝θ₂。而第3个刚体由电动机驱动，在运动过程中会相对第2个刚体转过一个角度θ₃'，因而角位移满足θ₃＝θ₂+θ₃'，这样第3个刚体的输入端状态矢量和第2个刚体的输出端状态矢量中的角位移取值是不同的，用矩阵的形式可以表示为

$>z_{I_3}=U_3^{\prime }·z_{O_2}---\left(2\right)$>

这里是第3个刚体输入端的状态矢量，是第2个刚体输出端的状态矢量，U′₃表示第3个刚体的输入端与第2个刚体输出端之间的传递矩阵，即

$>U_3^{\prime }=\left(\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& {\theta }_3^{\prime }\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right)---\left(3\right)$>

第3.2、关节的模型

对于关节1，由于它的输入端是整个机械臂系统的输入端，因此可以采用上述改进多体系统传递矩阵方法建模，也可以将其角位移的变化直接作为状态矢量的输入值给出，这里为了简化方程，采用后者。而在对机械臂关节3进行建模时，必须采用上述改进的离散时间传递矩阵方法，得出其输入端和输出端之间的传递方程为

z_3,4＝U₃U′₃z_3,2   (4)

式中U₃表示关节3的输出端与输入端之间的传递矩阵，用多体系统离散时间传递矩阵法直接可得，而U′₃可由式(3)计算得出，z_3,4和z_3,2分别为关节3和杆件2的输出端状态矢量。

第4、机械臂的总体模型

平面运动的二自由度串联机械臂有2个关节和2个杆件，共有4个平面刚体，由于这些刚体元件是串联链式连接，这里采用多刚体系统传递矩阵方法，将上述对关节和杆件的模型进行统一，因此整个机械臂的总传递矩阵就是将4个刚体的传递矩阵相乘，这时整个机械臂的动力学统一模型为

z_4,5＝U_allz_1,0   (5)

如前所述序号5为机械臂的末端，U_all为系统的总传递矩阵，即

U_all＝U₄U₃U′₃U₂U₁   (6)

式中U₁、U₂、U₃和U₄由传递矩阵离散时间方法确定，当刚体的质量、转动惯量、几何参量和运动情况都为已知量时，U₂和U₄是可直接计算的，U′₃由式(3)确定，它是关节1和3运动角位移的函数，由此，式(5)和(6)就是串联二自由度机械臂的动力学统一模型。

实施例2、模型的求解

对串联多关节机械臂来说，按照运动学规律有两种情况，其一为已知关节运动规律求解末端的信息，即正向运动学求解；其二为已知末端点运动规律求解关节信息，属于运动学反解问题。对于式(5)所示的二自由度机械臂的动力学总体方程，上述任何一种运动情况的求解，都需要根据已知、未知条件，设置总体方程的边界条件，然后通过传递矩阵的迭代得到各状态矢量的未知参量。这些参量包含机械臂中各个关节和杆件位置及受力的全部信息，因此两种运动情况的求解都是运动学问题与逆动力学问题的叠加。

1、求解方法一、正向运动及模型的求解

假设机械臂初始位置为两连杆展开成一条直线，且与惯性坐标系的x轴平行，当机械臂关节1和3的运动规律给定时，由于关节和杆件是固结连接，此时系统中1-4刚体元件的角位移都为已知参量。这里，设关节1和关节2的运动规律满足

$>{\theta }_i=\left(\begin{array}{cc}0& t\le T_1\\ {\theta }_i^d·(\frac{t-T_1}{T_2-{T_1}^{\prime }-T_1}-\frac{1}{2\pi }\sin \frac{2\mathrm{\pi t}}{T_2-{T_1}^{\prime }-T_1})& T_1\le t\le T_2-{T_1}^{\prime }\\ {\theta }_i^d& T_2-{T_1}^{\prime }\le t\le T_2\end{array}\right)---\left(7\right)$>

式中为关节运动期望值，且因此有θ_1,0＝θ_2,1＝θ₁，θ_3,2＝θ_4,3＝θ_5,4＝θ₁+θ₂。T₂为运动总历时时间，T₁、T₁′分别为运动起始、运动终止延时时间，且T₁＝20ms，T₁′＝40ms，T₂＝5s。

根据式(7)中的关节规律，直接求解刚体1-4的各个时刻的角位移，并代入式(6)中，可以将动力学模型中各时刻的总传递矩阵U_all直接计算出，而且，根据上述条件设置机械臂系统的边界条件为

$>\left(\begin{array}{c}{z}_{\mathrm{1,0}}={[\mathrm{0,0},{\theta }_{\mathrm{1,0}},m_{\mathrm{1,0}},q_{x_{\mathrm{1,0}}},q_{y_{\mathrm{1,0}}},1]}_{\mathrm{1,0}}^T\\ z_{\mathrm{5,4}}={[x_{\mathrm{5,4}},y_{\mathrm{5,4}},{\theta }_{\mathrm{5,4}},\mathrm{0,0,0,1}]}_{\mathrm{5,4}}^T\end{array}\right)---\left(8\right)$>

式中刚体1及末端角位移θ_1,0、θ_5,4为已知量，刚体1的输入端位移x_I、y_I始终为0，受到的内力和内力矩m_1,0为未知参量，而序号为5的机械臂末端的平动位移x_5,4、y_5,4为未知量，受到的内力和内力矩为0。

将式(8)初始时刻代入式(5)，则可解得该时刻z_1,0和z_5,4中的未知参量，并利用各个刚体元件的传递方程，即可求得该时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。这样反复迭代，即可求得所有时刻刚体的全部信息。正向运动模型求解如图3所示流程。

2、求解方法二、运动反解及模型的求解

当机械臂的末端运动规律已知时，求解的首要问题就是确定关节的运动规律，这是因为刚体各个点的运动和受力，即传递矩阵中的各元素都是角位移参量的函数。这里假设二自由度机器人末端运动的起始点为(0.3,0)，且x方向始终为定值，即x＝x_start＝x_end＝0.3，y方向的运动遵循如下规律

$>y=\left(\begin{array}{cc}0.1·t^2+y_{\mathrm{start}}& y\le T_1\\ 0.2·t+0.1·t^2-0.2·T_1& T_1\le y\le T_2\\ -0.1·t^2+t+0.1·(T_2^2+{T_1}^2)-0.8·T_2-0.2·T_1& T_2\le y\le T\end{array}\right)---\left(9\right)$>

式中，y_start＝0，且y≤T₁期间，y作匀加速运动，在T₁≤y≤T期间，y作匀加速运动，T₂≤y≤T期间，y作匀减速运动，且有T₁＝1s，T₂＝4s，T＝5s。

而在系统的边界条件表达式(8)中，关节1输入端的角位移θ_1,0、内力和内力矩m_1,0均为未知量，而机械臂末端转角θ_2n+1,2n为未知量，平动位移x_2n+1,2n、y_2n+1,2n为已知量，受到的内力和内力矩均为0。

对二自由度平面运动机械臂进行运动学反解的求解过程中，首先根据式(8)中系统初始时刻的边界条件，计算初始时刻关节的初始角位移，然后将已知条件代入式(6)和式(5)中，求解机械臂传递矩阵和各状态矢量的全部信息，并利用此时刻传递矩阵的参数和下一时刻的边界条件，继续计算下一时刻关节角位移、总传递矩阵和状态矢量各未知参量，这样反复迭代，即可求得所有时刻系统各个关节和杆件联接点的状态矢量，进而可通过线性化得到各点的速度、加速度、角速度和角加速度。运动学反解求解流程如图4所示。

3、正向运动模型求解数值仿真与分析

采用说明书所述方法对实施例的平面运动二自由度机械臂进行模型正向运动求解，如图5-图11为机械臂正向运动运动学、动力学特性曲线。其中，图5所示为关节运动时，机械臂整体的位形图。图6和图7分别为采用传递矩阵改进方法计算的机械臂刚体1～4输出端x、y方向的位移曲线，图中的x₁、x₂、x₃、x₄和y₁、y₂、y₃、y₄。为了验证此方法的正确性，图中还示出了采用D-H坐标方法计算各部件的位移曲线，如图中标了下标为robot的曲线，两种方法计算结果完全吻合。图8则表示机械臂中各刚体运动过程的角位移，其中关节1和连杆2的转角位移相同，而关节3和连杆4的转角位移相同。图9是各刚体的内力矩，图中所示m₁和m₃曲线即是机械臂实现运动关节1和关节3所提供的驱动力矩。图10和图11分别为各刚体x、y方向的内力，从图中可知，在所有刚体中，关节1受到的力最大，其数值与杆件2输入端受到的力相等。

4、运动学反解模型求解数值仿真与分析

采用说明书所述方法对实施例的平面运动二自由度机械臂进行运动学反解和逆动力学求解，如图12-图18为机械臂运动学反解运动学、动力学特性曲线。其中，图12所示为机械臂运动位形图。图13和图14分别为各刚体部件的x、y方向位移，两图中的结果与运用机器人学方法计算结果一致，说明多体系统传递矩阵方法的正确性。图18表示各刚体运动的角位移，图中说明中心刚体1与连杆2角位移运动一致，关节3与杆件4角位移运动一致。图19为各刚体的内力矩，图中m₁和m₃分别是两个关节的驱动力矩，这里关节1所受力矩最大。图20和21分别为各刚体x、y方向的内力，图中表示，刚体1受到的力最大。图18为末端y方向的位移、速度和加速度。