一种6自由度并联机器人的动力学建模方法及系统

1.本发明属于软件检测技术领域，本发明涉及一种6自由度并联机器人的动力学建模方法及系统。


背景技术：

2.6自由度并联机器人，例如6-ups gough-stewart（6-ups 高夫-斯图尔特）并联机构，是一种重要的并联机器人结构，广泛应用于工业生产线、航空航天、医疗手术等领域。其具有高刚度、高精度、高负载能力和多自由度等特点，能够实现复杂的运动轨迹和精确的定位控制。在工业生产线上，6-ups并联机构可以用于装配、焊接、喷涂等任务，提高生产效率和产品质量。在航空航天领域，6-ups并联机构可以用于飞行模拟、飞行器维修等任务，提供真实的环境仿真和高精度的操作能力。在医疗手术中，6-ups并联机构可以用于微创手术和精确定位，实现精细操作和减少手术创伤。
3.然而，6-ups并联机构的控制难度较大。其动力学模型的建立是控制系统设计的关键环节。动力学模型描述了系统的运动规律和力学特性，对于控制器的设计和性能分析至关重要。状态方程是动力学方程的一种形式，描述了系统状态随时间的演变规律。状态方程的推导对于系统的状态估计、状态反馈控制和系统稳定性分析具有重要意义。通过状态方程，可以实现对机构状态的实时监测和控制，提高控制系统的响应速度和稳定性。然而，由于6-ups并联机构的复杂结构和多自由度特性，动力学模型的推导和其状态方程形式的导出存在一定的困难。传统的动力学建模方法存在着模型复杂、计算量大、求解困难等问题，使得状态方程难以导出、难以程序化、控制系统的设计和优化变得复杂而繁琐。
4.目前，针对6-ups gough-stewart（6-ups 高夫-斯图尔特）并联机构的动力学建模方法已经有多种研究成果。然而，这些方法各有优缺点。基于牛顿-欧拉法的方法可避免了拉格朗日法的大量求导和符号定义，使得模型推导更加简化且易于编程仿真，然而，目前对于利用牛顿-欧拉法的推导过程存在两个主要缺点：一是未考虑万向节的运动限制，假设了万向节的力矩沿着对应腿的方向，支腿的角速度和角加速度始终垂直于支腿，这种假设对模型精度有较大影响，存在一定的误差；二是考虑了万向节的限制，但在运动学分析时需将支腿角速度和角加速度按照万向节模型参考坐标系进行分析，这会产生复杂的中间变量，造成动力学迭代次数增加，解算速度降低，且最终不易导出状态方程。目前基于虚功原理的方法推导动力学方程，虽成功获得了动力学的状态方程形式，并验证了其可用性，然而，该方法在处理带有万向节的gough-stewart并联机构时仍利用了与上述相同的假设条件，且对于6-ups构型的动力学仿真的实际效果有限，适用性较差。基于凯恩方法的方法摒弃了部分简化条件，相比于前两种方法更加快，然而，该方法同样采取支腿的角速度垂直于支腿的假设，模型精确度上难以保证。拉格朗日法能够有效地导出动力学方程，但大量的求导和偏导运算往往给程序化带来困难。


技术实现要素：

5.本发明的目的在于提供一种6自由度并联机器人的动力学建模方法及系统，可快速、准确地建立6自由度并联机器人的动力学模型，并得出相应的状态方程。该方法不仅具有高精度和高效性，还能有效降低计算复杂度，提高控制性能和运动规划的准确性。
6.实现本发明目的的技术解决方案为：一种6自由度并联机器人的动力学建模方法，包括以下步骤：s01：建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；s02：将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；s03：根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。
7.优选的技术方案中，所述步骤s01中建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系的方法包括：s11：建立整体参考坐标系：由于在结构上上下6对支点呈对称关系，仅需要分析一个支腿的几何环路即可，坐标系、分别固连在底座和动平台上，坐标系固连在第个万向节中心处，方向与相同；坐标系同样固连在万向节中心处，其轴沿着整个支腿的单位向量方向，即，沿着万向节的旋转轴的单位向量，垂直于、所组成的平面，坐标系方向与相同，位置在动平台第个球副中心处；s12：建立万向节的局部坐标系：用单位向量、、表示第个万向节模型，为沿着第个万向节的固定轴可测量的固定单位向量，与上述定义相同，垂直于、所组成的平面；s13：定义运动平台系与底座系间用欧拉角描述的旋转变换矩阵为，同时利用广义坐标表示动平台的位姿，为动平台相对于底座的位置向量，为欧拉角，表示动平台的广义速度，为动平台速度，为广义加速度，为动平台加速度，、为动平台的角速度和角加速度，为在中的位置向量，为在中的表示，有；s14：定义为从系到系的旋转变换矩阵：。
8.优选的技术方案中，所述步骤s01中简化并求解支腿的角速度、角加速度的方法包括：s15：对单支腿几何环路分析，设为沿着支腿轴线方向的向量，根据第个环路的
几何关系，求得及其模长和单位向量：
9.s16：对上式进行微分得到点速度和加速度：
[0010][0011]
s17：同时用支腿的角速度和角加速度表示点速度和加速度：
[0012]
其中，为的一阶导；
[0013]
其中，为的二阶导；s18：直接提取和，直接从用支腿的角加速度表示点加速度中提取支腿的角加速度，对上式两边同时叉乘，并利用：
[0014]
得：
[0015]
其中，为单位向量，为关于斜对称矩阵，为3
×
3的单位矩阵，，为关于的斜对称矩阵；s19：首先对两边同时叉乘：
[0016]
对上式两边同时左乘，并除以标量，同时将代入得：
[0017]
其中，。
[0018]
优选的技术方案中，所述步骤s02中万向节、缸体与活塞杆间的移动副以及球副的摩擦为：
[0019]
其中，，为系数矩阵，，为关于的斜对称矩阵，、分别为万向节和球副处的黏性阻尼系数，为移动副的黏性摩擦系数，为关于的斜对称矩阵。
[0020]
优选的技术方案中，所述步骤s03中并联机器人的动力学方程为：
[0021]
其中，为整体的惯性矩阵, 为整体的科氏力矩阵,为重力项,为运动平台的惯性矩阵、为六个支腿的惯性矩阵之和，为运动平台的科氏力矩阵、为六个支腿的科氏力矩阵之和、为运动平台的重力项、为六个支腿的重力项之和，为摩擦力项，为广义外力项，为雅可比矩阵，为关节力。
[0022]
本发明还公开了一种6自由度并联机器人的逆动力学分散pid控制方法，包括上述的6自由度并联机器人的动力学建模方法，在传统逆动力学分散pd控制方案的基础上加入积分项，用于消除系统的静态误差，表述为：
[0023][0024]
其中，带有下标表示期望值，带有下标的表示实际值，即、、为期望关节位移、速度以及加速度，、为实际关节位移、速度，关节空间下的动态矩阵，，分别为惯性矩阵、科氏力矩阵和重力项的估计值，、分别为在关节空间表示的外力和摩擦力，、、分别为比例项系数、积分项系数和微分项系数，，为关节位移和速度误差，即，，即为校正后的关节加速度。
[0025]
本发明又公开了一种6自由度并联机器人的动力学建模系统，包括：坐标系建立模块，建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；摩擦计算模块，将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；动力学建模模块，根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动
平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。
[0026]
优选的技术方案中，所述摩擦计算模块中万向节、缸体与活塞杆间的移动副以及球副的摩擦为：
[0027]
其中，，为系数矩阵，，为关于的斜对称矩阵，、分别为万向节和球副处的黏性阻尼系数，为移动副的黏性摩擦系数，为关于的斜对称矩阵。
[0028]
优选的技术方案中，所述动力学建模模块中并联机器人的动力学方程为：
[0029]
其中，为整体的惯性矩阵,为整体的科氏力矩阵,为重力项,为运动平台的惯性矩阵、为六个支腿的惯性矩阵之和，为运动平台的科氏力矩阵、为六个支腿的科氏力矩阵之和、为运动平台的重力项、为六个支腿的重力项之和，为摩擦力项，为广义外力项，为雅可比矩阵，为关节力。
[0030]
本发明又公开了一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的6自由度并联机器人的动力学建模方法。
[0031]
本发明与现有技术相比，其显著优点为：本发明提供了一种基于简化直接的数学方法和涵盖完整模型的新方法，可快速、准确地建立6-ups gough-stewart并联机构的动力学模型，并推导出相应的状态方程。该方法不仅具有高精度和高效性，还能有效降低计算复杂度，提高控制性能和运动规划的准确性，具有广泛的应用前景。本发明还涉及用于gough-stewart并联机构的逆动力学分散pid控制方案，相较于传统的拟动力学分散pd控制方案，逆动力学分散pid控制方案更容易使控制gough-stewart并联机构过程中产生的静态误差收敛。
附图说明
[0032]
图1为本实施例6自由度并联机器人的动力学建模方法的流程图；图2为本实施例6-ups整体参考坐标系及几何环路；图3为万向节模型及其局部参考坐标系；图4为单支腿受力分析；图5为运动平台受力分析；图6为逆动力学分散pid控制流程图；图7为本发明模型与虚功法模型的跟踪误差比较。
具体实施方式
[0033]
本发明的原理是：基于简化直接的数学方法和涵盖完整模型的新方法，旨在实现全面、精确、仿真解算快速的动力学建模和状态方程导出。通过这一发明，旨在改善当前方法中的缺点，提高建模精度，并适用于带有万向节的6-ups gough-stewart平台。鉴于牛顿-欧拉法法能够准确地显示刚体内部运动情况和力约束，在控制分析以及程序化上优势明显。本发明改进了传统基于牛顿-欧拉法的动力学建模方法，考虑了6-ups gough-stewart并联机构中万向节的运动限制和，考虑了万向节、移动副、球副的摩擦，考虑了支腿缸体的形状结构不对称性，以获得全面、精确的动力学模型。本发明通过重新利用牛顿-欧拉法，简化了运动学部分对于支腿角速度、角加速度的分析，实现了利于程序化且仿真解算快速。通过克服现有方法的局限性和建模时的全面考虑，该发明将有助于改善6-ups gough-stewart并联机构的控制系统设计和性能分析。
[0034]
实施例1：
[0035]
如图1所示，一种6自由度并联机器人的动力学建模方法，包括以下步骤：s01：建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；s02：将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；s03：根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。
[0036]
一较佳的实施例中，步骤s01中建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系的方法包括：s11：建立整体参考坐标系：由于在结构上上下6对支点呈对称关系，仅需要分析一个支腿的几何环路即可，坐标系、分别固连在底座和动平台上，坐标系固连在第个（=1~6）万向节中心处，方向与相同；坐标系同样固连在万向节中心处，其轴沿着整个支腿的单位向量方向，即，沿着万向节的旋转轴的单位向量，垂直于、所组成的平面，坐标系方向与相同，位置在动平台第个（=1~6）球副中心处；s12：建立万向节的局部坐标系：用单位向量、、表示第个（=1~6）万向节模型，为沿着第个（=1~6）万向节的固定轴可测量的固定单位向量，与上述定义相同，垂直于、所组成的平面；s13：定义运动平台系与底座系间用欧拉角描述的旋转变换矩阵为，同时利用广义坐标表示动平台的位姿，为动平台相对于底座的位置向量，为欧拉角，表示动平台的广义速度，为动平台速度，为广
义加速度，为动平台加速度，其中、为动平台的角速度和角加速度，为在中的位置向量，为在中的表示，有；s14：定义为从系到系的旋转变换矩阵：。
[0037]
一较佳的实施例中，步骤s01中简化并求解支腿的角速度、角加速度的方法包括：s15：对单支腿几何环路分析，设为沿着支腿轴线方向的向量，根据第个（=1~6）环路的几何关系，求得及其模长和单位向量：
[0038]
s16：对上式进行微分得到点速度和加速度：
[0039][0040]
s17：同时用支腿的角速度和角加速度表示点速度和加速度：
[0041]
其中，为的一阶导；
[0042]
其中，为的二阶导；s18：直接提取和，直接从用支腿的角加速度表示点加速度中提取支腿的角加速度，对上式两边同时叉乘，并利用：
[0043]
得：
[0044]
其中，为单位向量，为关于斜对称矩阵，为3
×
3的单位矩阵，为第个（=1~6）支腿的角速度关于点速度的系数矩阵，，为关于的斜对称矩阵；s19：首先对两边同时叉乘：
[0045]
对上式两边同时左乘，并除以标量，同时将代入得：
[0046]
其中，为第个（=1~6）支腿的角加速度关于点速度的系数矩阵，。
[0047]
一较佳的实施例中，步骤s02中万向节、缸体与活塞杆间的移动副以及球副的摩擦为：
[0048]
其中，，为系数矩阵，，为关于的斜对称矩阵 、分别为万向节和球副处的黏性阻尼系数，为移动副的黏性摩擦系数，为关于的斜对称矩阵。
[0049]
一较佳的实施例中，步骤s03中并联机器人的动力学方程为：
[0050]
其中，为整体的惯性矩阵，为整体的科氏力矩阵,为重力项,为运动平台的惯性矩阵、为六个支腿的惯性矩阵之和，为运动平台的科氏力矩阵、为六个支腿的科氏力矩阵之和、为运动平台的重力项、为六个支腿的重力项之和，为摩擦力项，为广义外力项，为雅可比矩阵，为关节力。
[0051]
另一实施例中，一种6自由度并联机器人的逆动力学分散pid控制方法，包括上述的6自由度并联机器人的动力学建模方法，在传统逆动力学分散pd控制方案的基础上加入积分项，用于消除系统的静态误差，表述为：
[0052][0053]
其中，带有下标表示期望值，带有下标的表示实际值，即、、为期望关节位移、速度以及加速度，、为实际关节位移、速度，关节空间下的动态矩阵，，
分别为惯性矩阵、科氏力矩阵和重力项的估计值，、分别为在关节空间表示的外力和摩擦力，、、分别为比例项系数、积分项系数和微分项系数，，为关节位移和速度误差，即，，即为校正后的关节加速度。
[0054]
另一实施例中，一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的6自由度并联机器人的动力学建模方法。
[0055]
另一实施例中，一种6自由度并联机器人的动力学建模系统，包括：坐标系建立模块，建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；摩擦计算模块，将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；动力学建模模块，根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。
[0056]
具体的，下面以一较佳的实施例为例对6自由度并联机器人的动力学建模系统的工作流程说明如下：6自由度并联机器人可以为6-ups gough-stewart并联机构，6-ups gough-stewart并联机构包括以下部分，分别为一个底座、六个万向节、六个缸体、六个活塞杆、六球副以及一个运动平台。
[0057]
包括以下步骤：步骤一：建立并联机构的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，为能够顺利导出状态方程，简化并求解支腿的角速度、角加速度；步骤二：进行动力学方程的推导，考虑万向节对支腿的约束力矩的影响，考虑了万向节、缸体与活塞杆间的移动副以及球副的摩擦影响，并将其单独提出，进行全面完整的建模；步骤三：导出6-ups gough-stewart并联机构的状态方程，与传统的虚功法原理模型在matlab仿真环境下进行比较，控制算法应用为在传统的逆动力学分散pd控制基础上改进的逆动力学分散pid控制方案。
[0058]
具体的，包括以下步骤：步骤1：建立6-ups并联机构的参考坐标系，包括并联机构的整体参考坐标系、万向节的局部参考坐标系。整体参考坐标系如图2，由于在结构上上下6对支点呈对称关系，因此仅需要分析一个支腿的几何环路即可。坐标系、分别固连在底座和动平台上，坐标系固连在第个（=1~6）万向节中心处，方向与相同。坐标系同样固连在万向节中心处，其轴沿着整个支腿的单位向量方向，即，沿着万向节的旋转轴的单位向量，垂直于、所组成的平面。系方向与相同，位置在动平台第个（=1~6）球副中心处。
[0059]
步骤2：建立万向节的局部坐标系，如图3所示，用单位向量、、表示第个（=1~6）万向节模型。为沿着第个（=1~6）万向节的固定轴可测量的固定单位向量，与上述定义相同，垂直于、所组成的平面。
[0060]
步骤3：定义运动平台系与底座系间用欧拉角描述的旋转变换矩阵为，同时利用广义坐标表示动平台的位姿，为动平台相对于底座的位置向量，为欧拉角，表示动平台的广义速度，为动平台速度，为广义加速度，为动平台加速度，、为动平台的角速度和角加速度。为在中的位置向量，为在中的表示，有。
[0061]
步骤4：定义为从系到系的旋转变换矩阵：（1）
[0062]
步骤5：单支腿几何环路分析。设为沿着支腿轴线方向的向量，由图1展示的第个（=1~6）环路的几何关系，可求得及其模长和单位向量：（2）
[0063]
步骤6：对式（2）进行微分得到点速度和加速度：（3）（4）
[0064]
步骤7：同时用支腿的角速度和角加速度表示点速度和加速度：（5）（6）
[0065]
步骤8：传统的方法上一种是忽略万向节的限制，这种建模方法是不精确的。另一种是摒弃这种假设，但这种方法复杂难推，并且难以导出动力学模型的紧凑的显式状态方程，有一定的限制。在本发明中，将直接从（5）、（6）式直接提取和，对（6）式两边同时叉乘，并利用：（7）其中，为单位向量，为关于斜对称矩阵，为3
×
3的单位矩阵，得：（8）
[0066]
其中为第个（=1~6）支腿的角速度关于点速度的系数矩阵，
。
[0067]
以下给出关于（7）的简单证明：设且有，所以，所以式（7）左边为，式（7）右边为，即等式（7）成立。
[0068]
步骤9：对于角加速度的推导一般思路便是对（8）式进行求导或者将角加速度按万向节局部坐标系分解建立其它等式关系。求导的思路即得到形如的角加速度表达式，事实上对于系数的求导，即得到的导数较为困难且不利于后续程序化仿真，另一方面分解构建新等式的方式将增加模型的复杂程度，影响解算速度。因此在本发明中，首先对（6）两边同时叉乘：（9）
[0069]
并观察式（8）与上述求导的思路得到的表达式，可得仅需将（9）式中的提取出来并将（8）式代入便能得到的具体表达式而无需求导，因此对（9）式两边同时左乘，并除以标量，同时将（8）代入可得：（10）
[0070]
其中，为第个（=1~6）支腿的角加速度关于点速度的系数矩阵，，实际上，。
[0071]
步骤10：分析活塞杆上质心的加速度可为后续动力学部分服务，活塞杆质心相对于系的位置矢量为：
（11）
[0072]
其中，为在系中的表示，，为从系到系的旋转变换矩阵，为活塞杆质心在中的位置矢量。
[0073]
步骤11：依据实际情况，活塞杆被认为是结构对称的，即在活塞杆对称轴线上。由此可以得到活塞杆质心的加速度为：（12）
[0074]
步骤12：本发明基于牛顿-欧拉法推导动力学方程。根据图4单支腿的受力分析可列出缸筒和活塞杆的欧拉方程，缸筒：
[0075]
其中，、为缸筒对活塞杆的作用力和力矩，为缸筒对活塞杆的力和力矩作用点的位置矢量在中的表示；
[0076]
活塞杆：
[0077]
其中，为活塞杆质心相对于系的位置矢量，为缸筒质心在中的表示，为缸筒质心在中的位置矢量；、分别为第个（=1~6）缸筒和活塞杆的质量；为万向节处约束力矩，该力矩仅沿方向；为球副处约束力；、分别为万向节和球副处的黏性阻尼系数；、分别为缸筒和活塞杆相对点的转动惯量，利用平行轴原理，有有，其中，、分别为缸筒和活塞杆相对各自质心的转动惯量，，为活塞杆质心在中的位置矢量。
[0078]
步骤13：将式(13）,(14)相加得：
（15）
[0079]
步骤14：将式(15)两边同时点乘后再乘,可得:（17)
[0080]
步骤15：(15)可进一步化为：(18）
[0081]
步骤16：将分为沿着的分量和垂直的分量,即：（19）其中，为沿着方向的分量的标量值。
[0082]
步骤17：将（19）代入进式（18），并对代入后的等式两边同时叉乘，得：（20）
[0083]
其中，为系数矩阵，。
[0084]
步骤18：将（8）、（10）、（16）代入到（20）中，得：（21）
[0085]
其中，,，,。
[0086]
步骤19：同理，我们可以将式（11）所表达的活塞杆质心的加速度做同样的处理：（22）
[0087]
其中，为关于的斜对称矩阵，为关于的斜对称矩阵，。
[0088]
步骤20：运动平台动力学分析。根据图5，对运动平台利用牛顿-欧拉方程：
（23）（24）
[0089]
其中, 为运动平台相对于点的转动惯量，依据平行轴原理，为运动平台相对于其质心的转动惯量，为运动平台质心在中的位置矢量，、分别为作用在动平台的外力和外力矩。
[0090]
步骤21：将（23）、（24）相加并写成矩阵形式，便可以得到运动平台的封闭式动力学方程：（25）
[0091]
其中，,，,为广义外力项，，其中为关于的斜对称矩阵。
[0092]
步骤22：并联机构整体动力学方程。若想求得整个机构的动力学方程需要将上述工作均映射到工作空间表示，因此需先对式（21）、（22）进行映射。考虑雅可比矩阵可将（2）、（4）式化为：（26）（27）
[0093]
其中，，实际为的导数，为关于的斜对称矩阵。
[0094]
步骤 23：进一步（21）、（22）式可化为：（28）（29）
[0095]
其中，,,
,。
[0096]
步骤 24：在求得整个机构的动力学方程的过程中，另外一个目标便是能够表示出缸筒对活塞杆的轴向驱动力（即关节力），这需要考虑在上的分量，因此需要借助活塞杆的牛顿平衡方程：（30）
[0097]
其中为移动副的黏性摩擦系数。
[0098]
步骤 25：对式（30）两边同时点乘可得第个（=1~6）支腿的关节力：（31）
[0099]
步骤 26：最后通过消除式（25）中的，并将摩擦项单独提出，得到最终整个机构的动力学方程，也即动力学模型的状态方程形式：（32）
[0100]
其中，, , ,,，,，,。
[0101]
在传统逆动力学分散pd控制方案的基础上加入积分项，来消除系统的静态误差，表述为：
[0102]
（33）
[0103]
其中，带有下标表示期望值，带有下标的表示实际值，即、、为期望关节位移、速度以及加速度，、为实际关节位移、速度，关节空间下的动态矩阵，，
分别为惯性矩阵、科氏力矩阵和重力项的估计值，、分别为在关节空间表示的外力和摩擦力，、、分别为比例项系数、积分项系数和微分项系数，，为关节位移和速度误差，即，，即为校正后的关节加速度。
[0104]
在一般情况下动态矩阵不完全已知，其误差动态系统较为复杂，往往需要借助更高级的控制算法或控制律来完成，这里不多赘述，同时这也是导出动力学方程的状态方程形式的意义所在。在本文我们假定动态矩阵完全已知，以简化的动态误差系统完成控制验证。其简化的误差系统可表示为：（34）
[0105]
为了验证所述模型的正确性，将与传统含有假设条件且能够导出状态方程的虚功法的动力学模型比较。控制算法选取适合利用状态方程的并联机器人控制的典型算法，即逆动力学分散pd控制方案，并将该控制方案升级为逆动力学分散pid控制以更好地使静态误差收敛，提高控制效果，其控制流程图如图6。仿真实验为在solidworks和matlab联合仿真的环境下进行，首先在solidworks中设计合适的6-ups gough-stewart并联机构模型，再将模型导入到matlab中进行仿真实验。两种动力学模型均采用相同的期望轨迹如下（为动平台期望轨迹，单位：m、t为仿真时间，单位：s）：
[0106]
表1 6-ups gough-stewart并联机构结构参数
[0107]
为观察本发明方法的可用性和优越性，将两种模型下的轨迹跟踪误差放进一张图中比较。从图7中可以看出所改提出的动力学模型无论在关节位移误差波动上还是收敛速
度上均优于传统模型，且相比之下经典模型的静态误差要比改进模型大的多，精度差距明显。同时在仿真环境下对模型的实际解算运行时间进行计时，且两种模型均在同一台计算机（intel(r) core(tm) 2.30 ghz 8.00 gb ram）运行5次取平均，两者仿真的停止时间均为15s。根据计时结果（表2）可以看出，本发明模型平均运行时间为17.698 s，虚功法模型运行时间为551.098 s，在仿真解算速度上本发明模型之于虚功法模型提高了近31倍，因此本发明模型能够显著提高效率。
[0108]
表2 本发明模型与虚功法模型仿真运行时间对比
[0109]
本发明重新利用牛顿-欧拉法作为动力学建模的基础。牛顿-欧拉法是一种常用的刚体动力学分析方法，它基于牛顿第二定律和欧拉定理，可以有效地描述并联机构的运动规律和力学特性。同时考虑万向节的运动限制，通过准确描述万向节的运动规律，可以更准确地建立机构的动力学模型，提高系统的建模精度。本发明还加入了模型精度和摩擦效应对系统性能的影响，通过综合考虑摩擦力的影响，并将摩擦部分单独提取，可以更准确地描述并联机构的运动特性，提高建模精度和控制性能。为了提高建模计算效率，本发明在模型推导过程中采用了简化方法，减少了计算量和复杂度。可以更快速地得到动力学方程的显式状态方程形式，便于后续控制系统设计和分析。
[0110]
上述实施例为本发明优选地实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

技术特征：
1.一种6自由度并联机器人的动力学建模方法，其特征在于，包括以下步骤：s01：建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；s02：将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；s03：根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。2.根据权利要求1所述的6自由度并联机器人的动力学建模方法，其特征在于，所述步骤s01中建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系的方法包括：s11：建立整体参考坐标系：6对支点呈对称关系，分析一个支腿的几何环路得到其余支腿，坐标系、分别固连在底座和动平台上，坐标系固连在第个万向节中心处，方向与相同；坐标系同样固连在万向节中心处，其轴沿着整个支腿的单位向量方向，即，沿着万向节的旋转轴的单位向量，垂直于、所组成的平面，坐标系方向与相同，位置在动平台第个球副中心处；s12：建立万向节的局部坐标系：用单位向量、、表示第个万向节模型，为沿着第个万向节的固定轴可测量的固定单位向量，与上述定义相同，垂直于、所组成的平面；s13：定义运动平台系与底座系间用欧拉角描述的旋转变换矩阵为，同时利用广义坐标表示动平台的位姿，为动平台相对于底座的位置向量，为欧拉角，表示动平台的广义速度，为动平台速度，为广义加速度，为动平台加速度，、为动平台的角速度和角加速度，为在中的位置向量，为在中的表示，有；s14：定义为从系到系的旋转变换矩阵：。3.根据权利要求1所述的6自由度并联机器人的动力学建模方法，其特征在于，所述步骤s01中简化并求解支腿的角速度、角加速度的方法包括：s15：对单支腿几何环路分析，设为沿着支腿轴线方向的向量，根据第个环路的几何关系，求得及其模长和单位向量：，s16：对上式进行微分得到点速度和加速度：
，，s17：同时用支腿的角速度和角加速度表示点速度和加速度：，其中，为的一阶导；，其中，为的二阶导；s18：直接提取和，直接从用支腿的角加速度表示点加速度中提取支腿的角加速度，对上式两边同时叉乘，并利用：，得：，其中，为单位向量，为关于斜对称矩阵，为3
×
3的单位矩阵，，为关于的斜对称矩阵；s19：对两边同时叉乘：，对上式两边同时左乘，并除以标量，同时将代入得：，其中，。4.根据权利要求1所述的6自由度并联机器人的动力学建模方法，其特征在于，所述步骤s02中万向节、缸体与活塞杆间的移动副以及球副的摩擦为：，其中，，为系数矩阵，
，为关于的斜对称矩阵 ，、分别为万向节和球副处的黏性阻尼系数，为移动副的黏性摩擦系数，为关于的斜对称矩阵。5.根据权利要求1所述的6自由度并联机器人的动力学建模方法，其特征在于，所述步骤s03中并联机器人的动力学方程为：，其中，为整体的惯性矩阵, 为整体的科氏力矩阵，为重力项，为运动平台的惯性矩阵、为六个支腿的惯性矩阵之和，为运动平台的科氏力矩阵、为六个支腿的科氏力矩阵之和、为运动平台的重力项、为六个支腿的重力项之和，为摩擦力项，为广义外力项，为雅可比矩阵，为关节力。6.一种6自由度并联机器人的逆动力学分散pid控制方法，其特征在于，包括权利要求1-5任一项所述的6自由度并联机器人的动力学建模方法，在传统逆动力学分散pd控制方案的基础上加入积分项，用于消除系统的静态误差，表述为：，，其中，带有下标表示期望值，带有下标的表示实际值，即、、为期望关节位移、速度以及加速度，、为实际关节位移、速度，关节空间下的动态矩阵，，分别为惯性矩阵、科氏力矩阵和重力项的估计值，、分别为在关节空间表示的外力和摩擦力，、、分别为比例项系数、积分项系数和微分项系数，，为关节位移和速度误差，即，，即为校正后的关节加速度。7.一种6自由度并联机器人的动力学建模系统，其特征在于，包括：坐标系建立模块，建立6自由度并联机器人的整体参考坐标系、万向节的局部参考坐标系，进行运动学分析，简化并求解支腿的角速度、角加速度；摩擦计算模块，将万向节、缸体与活塞杆间的移动副以及球副的摩擦加入到了动力学的分析之中；动力学建模模块，根据单支腿的受力分析得到缸筒和活塞杆的欧拉方程，对运动平台利用牛顿-欧拉方程得到运动平台的封闭式动力学方程，获得缸筒对活塞杆的轴向驱动力，并将摩擦项单独分离，得到最终整个并联机器人的动力学方程。8.根据权利要求7所述的6自由度并联机器人的动力学建模系统，其特征在于，所述摩擦计算模块中万向节、缸体与活塞杆间的移动副以及球副的摩擦为：
，其中，，为系数矩阵，，为关于的斜对称矩阵 ，、分别为万向节和球副处的黏性阻尼系数，为移动副的黏性摩擦系数，为关于的斜对称矩阵。9.根据权利要求7所述的6自由度并联机器人的动力学建模系统，其特征在于，所述动力学建模模块中并联机器人的动力学方程为：，其中，为整体的惯性矩阵, 为整体的科氏力矩阵, 为重力项,为运动平台的惯性矩阵、为六个支腿的惯性矩阵之和，为运动平台的科氏力矩阵、为六个支腿的科氏力矩阵之和、为运动平台的重力项、为六个支腿的重力项之和，为摩擦力项，为广义外力项，为雅可比矩阵，为关节力。10.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被执行时实现权利要求1-5任一项所述的6自由度并联机器人的动力学建模方法。

技术总结
本发明公开了一种6自由度并联机器人的动力学建模方法及系统，结合多种理论推导方案改进了基于牛顿-欧拉法的动力学建模过程。首先，建立完整的参考坐标系系统，其次，考虑万向节模型、摩擦模型、机械部件的不对称。简化支腿的角速度、角加速度的求解过程，为传统牛顿-欧拉法无法导出状态方程形式的缺点提供条件。最后在传统逆动力学分散PD控制的基础上加入积分项，并在Matlab环境下与基于虚功法原理的动力学模型进行了比较，验证所提出模型的可用性和高效性。本发明提供了基于牛顿-欧拉法的全面、精确、仿真解算快速的动力学建模和状态方程推导方法，对6-UPS Gough-Stewart的研究以及仿真建模有一定指导意义。真建模有一定指导意义。真建模有一定指导意义。


技术研发人员：朱培逸 刘成成 温嘉懿 阮昕玥 颜健 孟祥飞 芈家豪 孙铠 李鑫
受保护的技术使用者：常熟理工学院
技术研发日：2023.09.04
技术公布日：2023/10/11