CN113821935B - 基于对称约束的动力学模型的建立方法及其*** - Google Patents

Abstract

本发明涉及一种基于对称约束的动力学模型的建立方法及其***，该建模方法用于建立一种双移动机械臂空间协作***在空间协作搬运时的动力学模型。该建模方法将空间协作***拆分为两个单独的移动机械臂，将负载简化为连杆，并将负载视作断开，并设计相应的对称约束。首先建立起单个移动机械臂的一般形式动力学方程，再引入协作***约束的二阶微分形式，最后通过引入Udwadia‑Kalaba方程计算出空间协作***受到的约束力，并以此计算得到空间协作***的动力学方程，由于并未从连杆与电机的连接处视作断开，故无需考虑驱动电机转轴与连杆转角的约束缺失问题，从而确保动力学模型具有足够的准确性以及可行性。

Description

基于对称约束的动力学模型的建立方法及其***

技术领域

本发明涉及协作移动机械臂技术领域，特别是涉及一种基于对称约束的动力学模型的建立方法及其***。

背景技术

协作机械臂通过安装不同末端执行器，可广泛应用于各种不同场合，如用于风电叶片、火箭壳体、高铁车厢等大尺寸零部件表面的抛磨加工、焊接，底刚度、底稳定性零件的搬运、装配等工作。相比于单机械臂，协作机械臂效率更高，应用范围更广，相对于固定机械臂，移动机械臂工作范围更大也更为灵活，其冗余自由度可实现避障等功能。相对于三坐标机械臂，移动机械臂占地面积更小，且能够适应大范围工作，但由于其存在复杂的耦合关系，使得动力学模型的建立极为困难。

长久以来，协作机械臂的建模与控制问题受到了众多研究者的青睐。许多研究者基于Lagrange乘子，探索动力学模型的建立方法，并提出了李群方法、凯恩方法等多种求解方法。虽然如此，Lagrange乘子需要进行大量计算，对复杂***难以应用。

刘佳和刘荣提出了一篇名为“双臂协调机械手动力学建模的新方法”的论文，该文的建模方法选择将机械臂末端关节与平台铰接处进行断开的方法，会导致引入约束时无法引入关节转角与连杆转角之间的固有物理关系，且应用此方法并没有简化计算量，机械臂部分自由度较多，计算仍然非常复杂。王业光提出了一篇名为“双全方位移动机械臂的协作研究”的论文，该文只是建立起单个移动机械臂的动力学模型，并未给出整体协作***的模型，无法应用各种控制器，在控制时过度依赖各移动机械臂间的通信，实时性及精度较差。

目前针对协作***动力学建模的研究，多数是针对线性协作***或单独的机械臂进行研究，自由度较低。若采用传统建模方法，计算过程繁琐，且无法引入转角的约束，这进一步限制了建立的动力学模型的准确性以及可行性。

发明内容

基于此，有必要针对现有技术中，针对协作***动力学建模的研究，多数是针对线性协作***或单独的机械臂进行研究，自由度较低。若采用传统建模方法，计算过程繁琐，且无法引入转角的约束，这进一步限制了建立的动力学模型的准确性以及可行性的技术问题，本发明提供一种基于对称约束的动力学模型的建立方法及其***。

本发明提供一种基于对称约束的动力学模型的建立方法，其用于建立一种双移动机械臂空间协作***在空间协作搬运时的动力学模型。空间协作***包括：两个移动机械臂以及负载。每个移动机械臂包括移动平台、旋转平台、第一连杆、第二连杆和第三连杆。第一连杆的一端通过第一转动副与旋转平台转动连接，另一端通过第二转动副与第二连杆的一端连接。第二连杆的另一端通过第三转动副与第三连杆的一端连接。旋转平台通过第四转动副与移动平台转动连接。移动平台的移动轨迹平行于水平面。第一转动副、第二转动副以及第三转动副三者的轴线相互平行，且均平行于水平面。第四转动副的轴线垂直于水平面。负载的两端分别与两个第三连杆的另一端固定连接。

动力学模型的建立方法包括以下步骤：

S1、以水平面为基准建立一个X-Y-Z坐标系。

S2、将负载简化为连杆，并假设负载为断开状态。

S3、基于对称约束进行建模，将所述空间协作***划分为两个对称的子***；分别获取每个移动平台的移动距离、每个移动机械臂的各个旋转关节的旋转角度，以及对空间协作***所施加的外力。

其中，两个子***具有对称的约束关系，且每个子***为其中一个所述移动机械臂。

S4、根据步骤S3中获取的各个量，建立单个移动机械臂的无约束***的动力学方程，无约束***的动力学方程为：

式中，M(q)为惯性矩阵；为向心力与Coriolis力矩阵；G(q)为重力矩阵。

S5、先构建空间协作***的约束方程，并获取空间协作***的约束方程的二阶微分形式，再通过引入Udwadia-Kalaba方程(U-K方程)计算得到空间协作***受到的约束力Q^c，约束力Q^c的计算公式为：

式中，上标“+”表示矩阵的Moore-Penrose广义逆矩阵；空间协作***具有n个自由度；M为n×n维惯性矩阵；A为m×n维矩阵；b为m维向量；m维表示给定约束的个数；Q表示施加在空间协作***上并用于维持空间协作***平衡的n维广义力。

S6、根据无约束***的动力学方程以及约束力Q^c，计算得到空间协作***的动力学方程；空间协作***的动力学方程为：

式中，t表示时间；表示广义力。

在其中一个实施例中，在步骤S4中，通过Lagrange方法建立单个移动机械臂的无约束***动力学方程。

在其中一个实施例中，广义坐标q的表达公式为：

q＝[Δx₁，Δy₁，δ₁，θ₁，θ₂，θ₃，Δx₂，Δy₂，δ₂，θ₄，θ₅，θ₆]^T

式中，Δx₁和Δy₁分别表示其中一个所述移动平台的移动距离；Δx₂和Δy₂分别表示另外一个所述移动平台的移动距离；δ₁、θ₁、θ₂以及θ₃分别表示其中一个所述移动机械臂的四个所述旋转关节的旋转角度；δ₂、θ₄、θ₅以及θ₆分别表示另外一个所述移动机械臂的四个所述旋转关节的旋转角度。

在其中一个实施例中，步骤S4中，外力τ的表达公式为：

τ＝[F_x1，F_y1，τ₁，u₁，u₂，u₃，F_x2，F_y2，τ₂，u₄，u₅，u₆]^T

式中，F_x1和F_y1分别表示与所述移动距离Δx₁和Δy₁相对应的外力；τ₁、u₁、u₂和u₃分别表示与所述旋转角度δ₁、θ₁、θ₂以及θ₃相对应的外力；F_x2和F_y2分别表示与所述移动距离Δx₂和Δy₂相对应的外力；τ₂、u₄、u₅和u₆分别表示与所述旋转角度δ₂、θ₄、θ₅以及θ₆相对应的外力。

在其中一个实施例中，在步骤S5中，在建立空间协作***的动力学模型之前，还包括以下步骤：

根据步骤S3获取的各个量，分别计算出空间协作***的n×n维惯性矩阵M。根据空间协作***的约束方程，分别计算出m×n维矩阵A，m维向量b，以及n维广义力Q。

本发明还提供一种双移动机械臂空间协作***，其包括：两个移动机械臂以及负载。每个移动机械臂包括移动平台、旋转平台、第一连杆、第二连杆和第三连杆。第一连杆的一端通过第一转动副与旋转平台转动连接，另一端通过第二转动副与第二连杆的一端连接。第二连杆的另一端通过第三转动副与第三连杆的一端连接。旋转平台通过第四转动副与移动平台转动连接。移动平台的移动轨迹平行于水平面。第一转动副、第二转动副以及第三转动副三者的轴线相互平行，且均平行于水平面。第四转动副的轴线垂直于水平面。负载的两端分别与两个第三连杆的另一端固定连接。

其中，该双移动机械臂空间协作***采用如上述任意一项所述的基于对称约束的动力学模型的建立方法建立在空间协作搬运时的动力学模型。

在其中一个实施例中，移动平台用于实现在水平面内的全向移动。每个移动机械臂具有四个旋转关节。每个移动机械臂具有四个旋转自由度和两个平移自由度。即：空间协作***共具有12个自由度。

在其中一个实施例中，空间协作***采用伺服电机提供外部控制力。

本发明还提供一种计算机终端，其包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，处理器在执行程序时，实现上述任意一项基于对称约束的动力学模型的建立方法。

本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，程序被处理器执行时，实现上述任意一项基于对称约束的动力学模型的建立方法。

相较于现有技术，本发明提供的基于对称约束的动力学模型的建立方法及其***具有如下有益效果：

1、由于双移动机械臂空间协作***存在复杂的约束与非线性耦合，使得直接建立动力学模型极为困难，故本发明提出一种基于对称约束的动力学模型的建立方法，该方法通过将搬运物体的质心处断开，应用Lagrange方法建立了单个移动机械臂的动力学方程，减轻了***的耦合现象，解决了引入约束方程时无法对末端电机转角与末端连杆转角加以限制的问题。该方法还给出了将双移动机械臂空间协作***的固有几何约束及其二阶导数，基于对称约束建立了双移动机械臂空间协作***的动力学模型。该方法思路清晰，无需引入辅助变量，极大简化了计算量，探索了将U-K方法应用到三维工作空间的情况。

2、本发明的双移动机械臂空间协作***的工作效率高于单个机械臂平台，且能够胜任更多工作，相较于传统单机械臂平台，具有更高的实用性。本发明通过对空间协作***建立动力学模型，能够实现对空间协作***实施各种基于模型的控制算法，从而能够使空间协作***能够完成各类精确复杂的动作，满足更多作业要求。

3、本发明还对建立的模型进行了数值仿真，对模型添加期望的轨迹约束，给定了符合约束方程的初始条件，仿真显示模型运动轨迹与期望轨迹基本重合，证明了该方法建立动力学模型的可行性。

附图说明

图1为本发明实施例1单个移动机械臂的立体结构示意图；

图2为本发明实施例1的空间协作***的主视图；

图3为图2中空间协作***的主视图；

图4为图3中空间协作***的俯视图；

图5为本发明实施例1的基于对称约束的动力学模型的建立方法的流程图；

图6为本发明实施例1的仿真结果的关节角度约束示意图；

图7为本发明实施例1的仿真结果的断开处轨迹示意图；

图8为本发明实施例1的仿真结果的移动平台位移示意图；

图9为本发明实施例1的仿真结果的子***转角示意图。

主要元件符号说明

1、第一连杆；2、第二连杆；3、第三连杆；4、移动平台；5、旋转平台；6、负载。

以上主要元件符号说明结合附图及具体实施方式对本发明作进一步详细的说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，当组件被称为“安装于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。当一个组件被认为是“固定于”另一个组件，它可以是直接固定在另一个组件上或者可能同时存在居中组件。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“或/及”包括一个或多个相关的所列项目的任意的和所有的组合。

实施例1

本实施例提供一种基于对称约束的动力学模型的建立方法，该方法用于建立一种双移动机械臂空间协作***在空间协作搬运时的动力学模型。

请参阅图1和图2，空间协作***包括：两个移动机械臂以及负载6。

每个移动机械臂包括移动平台4、旋转平台5、第一连杆1、第二连杆2和第三连杆3。第一连杆1的一端通过第一转动副与旋转平台5转动连接，另一端通过第二转动副与第二连杆2的一端连接。第二连杆2的另一端通过第三转动副与第三连杆3的一端连接。旋转平台5通过第四转动副与移动平台4转动连接。移动平台4用于实现在水平面内的全向移动，即移动平台4的移动轨迹平行于水平面。第一转动副、第二转动副以及第三转动副三者的轴线相互平行，且均平行于水平面。第四转动副的轴线垂直于水平面。本实施例中，每个移动机械臂具有四个旋转关节。每个移动机械臂具有四个旋转自由度和两个平移自由度。即：空间协作***共具有12个自由度。

负载6的两端分别与两个第三连杆3的另一端固定连接。

本实施例中的空间协作***可采用伺服电机提供外部控制力，空间协作***采用了伺服控制，从而使得协作机械臂满足预期的运动要求。伺服电机的数量为多个，更具体地，伺服电机可以安装在移动平台4上，用于驱动移动平台4移动以及旋转平台5旋转，也可以安装在各个连杆之间形成的旋转关节处，用于驱动连杆的摆动。

请结合图3和图4，在本实施例中，由于空间协作***中具有两个移动机械臂，为了便于区分，以下定义每个机械臂中各个部件的量：

定义两个移动平台分别为v1和v2，两个旋转平台分别为r1和r2，在其中一个移动机械臂中，移动平台v1的质量为m_v1，旋转平台r1的质量、转动惯量和长度分别为m_r1、I_r1、l_r1，第一连杆的质量、转动惯量和长度可以分别为m₁、I₁和l₁，第二连杆的质量、转动惯量和长度可以分别为m₂、I₂和l₂，第三连杆的质量、转动惯量和长度可以分别为m₃、I₃和l₃。相应地，在另外一个移动机械臂中，移动平台υ2的质量为m_υ2，旋转平台r2的质量、转动惯量和长度分别为m_r2、I_r2、l_r2，第一连杆的质量、转动惯量和长度可以分别为m4、I₄和l₄，第二连杆的质量、转动惯量和长度可以分别为m₅、I₅和l₅，第三连杆的质量、转动惯量和长度可以分别为m₆、I₆和l₆。

以下本实施例将介绍基于对称约束建模的一般步骤：

对称约束建模在稳定无约束子***的动力学方程的基础上，向子***施加约束力，子***同时受到平衡力与约束力的作用，使其稳定且始终满足给定的约束条件。基于对称约束，建立具有n个自由度的动力学***模型的步骤如下：

(I)建立无约束***动力学方程，其表达公式为：

式中，t为时间，M为***的n×n维惯性矩阵，q为广义坐标，Q为施加在***上的广义力。

(II)建立约束方程对于显含/>的非完整约束进行一次求导，对不显含/>的完整约束进行二次求导，得到约束方程的二阶标准微分形式：

式中，A为m×n维矩阵，b为m维向量。

(III)计算约束力，受约束***动力学方程为：

式中，Q^c为约束力，上标“+”表示矩阵的Moore-Penrose广义逆矩阵。

对于一般的机械***，现提出如下假设：

假设1：对于任意q∈Rⁿ，总有|M(q)|＞0。

假设2：对于任意q∈Rⁿ，矩阵的秩r(A)≥1。

本假设也保证了矩阵的Moore-Penrose广义逆矩阵存在且唯一。

假设3：对于任意初始条件q∈Rⁿ均满足约束方程且约束连续。

本实施例提出了基于对称约束的对空间移动机械臂建模的改进方法，其用于建立前述双移动机械臂空间协作***在空间协作搬运时的动力学模型，即：双移动机械臂空间协作搬运建模。

请参阅图5，本实施例提供的基于对称约束的动力学模型的建立方法包括以下步骤，即步骤S1至S6。

S1、以水平面为基准建立一个X-Y-Z坐标系。

S2、将负载简化为连杆，并假设负载为断开状态。

请再次参阅图3，在图3所示的坐标系中，m₃与m₆为机械臂末端公共负载，可简化为连杆，并假想该连杆分离进行建模。

S3、基于对称约束进行建模，将空间协作***划分为两个对称的子***；分别获取每个移动平台的移动距离、每个移动机械臂的各个旋转关节的旋转角度，以及对空间协作***所施加的外力。

其中，两个子***具有对称的约束关系，且每个子***为其中一个所述移动机械臂。在本实施例中，在此动力学***的每个移动机械臂具有6个自由度。不考虑约束条件下，该动力学***共拥有12个自由度度，可以用12个状态变量进行描述，现选择如下n维广义坐标q，其表达公式为：

q＝[Δx₁，Δy₁，δ₁，θ₁，θ₂，θ₃，Δx₂，Δy₂，δ₂，θ₄，θ₅，θ₆]^T

式中，Δx₁和Δy₁分别表示其中一个所述移动平台的移动距离；Δx₂和Δy₂分别表示另外一个所述移动平台的移动距离；δ₁、θ₁、θ₂以及θ₃分别表示其中一个所述移动机械臂的四个所述旋转关节的旋转角度；δ₂、θ₄、θ₅以及θ₆分别表示另外一个所述移动机械臂的四个所述旋转关节的旋转角度。

在本实施例中，外力τ的表达公式为：

τ＝[F_x1，F_y1，τ₁，u₁，u₂，u₃，F_x2，F_y2，τ₂，u₄，u₅，u₆]^T

式中，F_x1和F_y1分别表示与所述移动距离Δx₁和Δy₁相对应的外力；τ₁、u₁、u₂和u₃分别表示与所述旋转角度δ₁、θ₁、θ₂以及θ₃相对应的外力；F_x2和F_y2分别表示与所述移动距离Δx₂和Δy₂相对应的外力；τ₂、u₄、u₅和u₆分别表示与所述旋转角度δ₂、θ₄、θ₅以及θ₆相对应的外力。

由于两个子***(移动机械臂)的动力学特性相同，故可以只针对其中一个子***进行建模。

首先计算各部分速度的Jacobi矩阵，可知各个机构质心处的线速度与角速度及其对应的Jacobi矩阵之间的关系式分别为：

上式中，k表示上述子***中的移动平台、旋转关节及所有连杆中的其中一个机构，ν_k和ω_k分别为机构k的质心处的线速度与角速度，和/>分别为与机构k线速度和角速度对应的Jacobi矩阵，/>和/>分别为矩阵/>和/>的第j列。

通过计算得到各部件质心处的线速度，由上述线速度及其对应的Jacobi矩阵之间的关系式可计算得到Jacobi矩阵J_v，左半子***线速度矩阵J_vl如下：

式中， 以及/>分别为与移动平台v1、旋转平台r1以及连杆i对应的线速度Jacobi矩阵。

同理，得到各部件质心处角速度，由上述角速度及其对应的Jacobi矩阵之间的关系式可以计算得到Jacobi矩阵J_w，左半子***角速度矩阵J_wl如下：

经过***性的理论推导，可得惯性矩阵M∈R^12×12的计算公式如下：

针对左半子***，惯性矩阵M的具体计算结果为：

M_1，1＝M_2，2＝m_v1+m_r1+m₁+m₂+m₃

M_4，5＝M_5，4＝(m₂+m₃)/2·l₁l₂cos(θ₁-θ₂)

M_4，6＝M_6，4＝m₃l₁l₃/2·cos(θ₁-θ₃)，M_5，6＝M_6，5＝m₃l₂l₃/2·cos(θ₂-θ₃)

同理可得到矩阵G(q)∈R¹²的计算公式如下：

式中，G_i为矩阵G中的第i个元素，g为广义坐标下的重力加速度。

同理，针对左半子***，矩阵G_i的具体计算结果为：

G₁＝G₂＝G₃＝0，

机械臂在转动时会受到向心力与Coriolis力，可统一表示为其中C∈R^12×12。向量/>是唯一确定的，但矩阵C并不唯一。可以将矩阵/>规范为反对称矩阵，以此确定矩阵C，得到矩阵C的计算公式如下：

式中，i、j分别表示行、列的序号。C_ij为矩阵C中第(i，j)个元素，M_ij为矩阵M中第(i，j)个元素。

同理，针对左半子***，矩阵C的计算结果为

至此，已经获取到了建立无约束***动力学方程所需的各个量。

S4、根据步骤S3中获取的各个量，可建立子***的一般动力学方程，该方程为：

步骤S4中，可将子***的一般动力学方程变化为前述基于对称约束建模中的公式的形式，可得：

式中，

S5、先构建空间协作***的约束方程，并获取空间协作***的约束方程的二阶微分形式，结合约束方程计算得到空间协作***受到的约束力Q^c，约束力Q^c的计算公式为：

式中，上标“+”表示矩阵的Moore-Penrose广义逆矩阵；空间协作***具有n个自由度；M为n×n维惯性矩阵；A为m×n维矩阵；b为m维向量；m维表示给定约束的个数；Q表示施加在空间协作***上并用于维持空间协作***平衡的n维广义力。

在步骤S5中，假定机械臂进行水平搬运，假想断开连杆实际为一整体，故假想末端x，y，z坐标应重合，且转角δ₁，δ₂及θ₃，θ₆应满足物理约束条件，由此建立以下约束方程组：

显然该约束为完整约束，将上述约束方程组对时间t求二阶导数，得到：

式中，

将约束方程二阶导数写成前述公式所示形式，可得：

矩阵A与b的具体计算结果分别如下：

至此，已经得到矩阵M、A、Q与向量b，根据前述基于对称约束建模的一般步骤中的公式可以计算出约束力Q^c，求得的约束力Q^c为***受到上述约束方程组的约束时产生的力。/>

S6、根据无约束***的动力学方程以及约束力Q^c，计算得到空间协作***的动力学方程；空间协作***的动力学方程为：

式中，t表示时间；表示广义力；Q^c为约束力。

本实施例中，在无约束***的动力学方程的右侧添加求得的约束力Q^c，使得两个子***式中满足上述约束方程组中的约束，从而得到空间协作***的动力学方程。

从以上建模过程中可以看出，基于对称约束建模，不需要引入Lagrange乘子或者辅助变量，得到的是约束力的解析解，并且求解过程相对简洁。

本实施例还通过仿真软件进行数值仿真，以验证模型的准确性。为了验证模型的准确性，需要验证两假想断开末端是否始终重合，δ₁、δ₁及θ₃、θ₆是否符合前述约束方程组的约束。为此，为假想断开连杆左端添加期望轨迹约束：

式中，x_i，y_i，z_i为机械臂末端初始位置的坐标常量。

求二阶导数得：

从而得到矩阵A_t与b_t：

将矩阵A_t与b_t和约束所求得的约束矩阵A与b合并，得到新的约束矩阵：

将上述得到的新的约束矩阵带入***动力学模型中进行数值仿真。

动力学***中的参数m_i，I_i，l_i为常数，与***物理特性有关，可通过测量与计算得出，仿真所用相应参数见表3.1。

表3.1：***动力学参数表

此外，还需要知道***的初始位置信息、初始速度信息以及初始加速度信息，且初始状态必须满足前述约束方程组的约束条件，给定如下初始位置、速度以及加速度：q＝[00 0 π/2 π/3 0 2.2 0 -π π/2 π/3 0]^T，取g＝[0 0 9.8]^T，给定初始任意初值τ。

请参阅图6-图9，图6-图9显示的是各项仿真结果，图6为关节角度约束示意图，图7为断开处轨迹示意图，图8为移动平台位移示意图，图9为子***转角示意图。

其中，图6中θ₃与θ₆误差量级为10^-12，δ₁与δ₂方向误差量级为10^-13，可视作满足固有几何约束。图7中x方向误差量级为10^-14，y、z方向误差量级为10^-13，可视作负载断开处运动时重合，x方向无位移，y、z方向做周期往复运动，满足固有几何约束及给定运动约束。移动平台在x方向保持静止，在y方向做周期往复运动，满足给定运动约束，机械臂关节转角在满足约束的条件下做周期运动。仿真结果表明该方法构建的空间协作动力学模型准确可用。

综上所述，相较于传统技术，本发明具有如下优点：

1、由于双移动机械臂空间协作***存在复杂的约束与非线性耦合，使得直接建立动力学模型极为困难，故本发明提出一种基于对称约束的动力学模型的建立方法，该方法通过将搬运物体的质心处断开，应用Lagrange方法建立了单个移动机械臂的动力学方程，减轻了***的耦合现象，解决了引入约束方程时无法对末端电机转角与末端连杆转角加以限制的问题。该方法还给出了将双移动机械臂空间协作***的固有几何约束及其二阶导数，基于对称约束建立了双移动机械臂空间协作***的动力学模型。该方法思路清晰，无需引入辅助变量，极大简化了计算量，探索了将U-K方法应用到三维工作空间的情况。

2、本发明的双移动机械臂空间协作***的工作效率高于单个机械臂平台，且能够胜任更多工作，相较于传统单机械臂平台，具有更高的实用性。本发明通过对空间协作***建立动力学模型，能够实现对空间协作***实施各种基于模型的控制算法，从而能够使空间协作***能够完成各类精确复杂的动作，满足更多作业要求。

3、本发明还对建立的模型进行了数值仿真，对模型添加期望的轨迹约束，给定了符合约束方程的初始条件，仿真显示模型运动轨迹与期望轨迹基本重合，证明了该方法建立动力学模型的可行性。

实施例2

本实施例提供一种计算机终端，其包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序。处理器执行程序时实现实施例1的基于对称约束的动力学模型的建立方法。

实施例1的模型建立方法在应用时，可以软件的形式进行应用，如设计成独立运行的程序，安装在计算机终端上，计算机终端可以是电脑、智能手机以及其他物联网设备等。实施例1的方法也可以设计成嵌入式运行的程序，安装在计算机终端上，如安装在单片机上。

实施例3

本实施例提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，程序被处理器执行时，实现实施例1中的基于对称约束的动力学模型的建立方法。

实施例1的方法在应用时，可以通过软件的形式进行应用，如设计成计算机可读存储介质可独立运行的程序，计算机可读存储介质可以是U盘，设计成U盾，通过U盘设计成通过外在触发启动整个方法的程序。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。

Claims (8)

1.一种基于对称约束的动力学模型的建立方法，其用于建立一种双移动机械臂空间协作***在空间协作搬运时的动力学模型；所述空间协作***包括：

两个移动机械臂；每个移动机械臂包括移动平台、旋转平台、第一连杆、第二连杆和第三连杆；所述第一连杆的一端通过第一转动副与所述旋转平台转动连接，另一端通过第二转动副与所述第二连杆的一端连接；所述第二连杆的另一端通过第三转动副与所述第三连杆的一端连接；所述旋转平台通过第四转动副与所述移动平台转动连接；所述移动平台的移动轨迹平行于水平面；所述第一转动副、所述第二转动副以及所述第三转动副三者的轴线相互平行，且均平行于水平面；所述第四转动副的轴线垂直于水平面；以及

负载，其两端分别与两个所述第三连杆的另一端固定连接；

其特征在于，所述动力学模型的建立方法包括以下步骤：

S1、以水平面为基准建立一个X-Y-Z坐标系；

S2、将所述负载简化为连杆，并假设所述负载为断开状态；

S3、基于对称约束进行建模，将所述空间协作***划分为两个对称的子***；分别获取每个所述移动平台的移动距离、每个所述移动机械臂的各个旋转关节的旋转角度，以及对所述空间协作***所施加的外力；

其中，两个所述子***具有对称的约束关系，且每个所述子***为其中一个所述移动机械臂；

S4、根据步骤S3中获取的各个量，建立单个所述移动机械臂的无约束***的动力学方程，所述无约束***的动力学方程为：

式中，M(q)为惯性矩阵；为向心力与Coriolis力矩阵；G(q)为重力矩阵；τ表示对所述空间协作***所施加的外力；q为广义坐标；

其中，所述外力τ的表达公式为：

τ＝[F_x1,F_y1,τ₁,u₁,u₂,u₃,F_x2,F_y2,τ₂,u₄,u₅,u₆]^T

式中，F_x1和F_y1分别表示与所述移动距离Δx₁和Δy₁相对应的外力；τ₁、u₁、u₂和u₃分别表示与所述旋转角度δ₁、θ₁、θ₂以及θ₃相对应的外力；F_x2和F_y2分别表示与所述移动距离Δx₂和Δy₂相对应的外力；τ₂、u₄、u₅和u₆分别表示与所述旋转角度δ₂、θ₄、θ₅以及θ₆相对应的外力；

所述广义坐标q的表达公式为：

q＝[Δx₁，Δy₁，δ₁，θ₁，θ₂，θ₃，Δx₂，Δy₂，δ₂，θ₄，θ₅，θ₆]^T

式中，Δx₁和Δy₁分别表示其中一个所述移动平台的移动距离；Δx₂和Δy₂分别表示另外一个所述移动平台的移动距离；δ₁、θ₁、θ₂以及θ₃分别表示其中一个所述移动机械臂的四个所述旋转关节的旋转角度；δ₂、θ₄、θ₅以及θ₆分别表示另外一个所述移动机械臂的四个所述旋转关节的旋转角度；

S5、先构建所述空间协作***的约束方程，并获取所述空间协作***的约束方程的二阶微分形式，再通过引入Udwadia-Kalaba方程计算得到所述空间协作***受到的约束力Q^c，所述约束力Q^c的计算公式为：

式中，上标“+”表示矩阵的Moore-Penrose广义逆矩阵；所述空间协作***具有n个自由度；M为n×n维惯性矩阵；A为m×n维约束矩阵；b为m维向量；m维表示给定约束的个数；Q表示施加在所述空间协作***上并用于维持所述空间协作***平衡的n维广义力；

S6、根据所述无约束***的动力学方程以及所述约束力Q^c，计算得到所述空间协作***的动力学方程；所述空间协作***的动力学方程为：

式中，t表示时间；表示广义力。

2.根据权利要求1所述的基于对称约束的动力学模型的建立方法，其特征在于，在步骤S4中，通过Lagrange方法建立单个所述移动机械臂的无约束***动力学方程。

3.根据权利要求1所述的基于对称约束的动力学模型的建立方法，其特征在于，在步骤S5中，在引入Udwadia-Kalaba方程建立所述空间协作***的动力学模型之前，还包括以下步骤：

根据步骤S3获取的各个量，分别计算出所述空间协作***的n×n维惯性矩阵M；根据所述空间协作***的约束方程，分别计算出所述m×n维矩阵A，所述m维向量b，以及所述n维广义力Q。

4.一种双移动机械臂空间协作***，其特征在于，所述空间协作***包括：

两个移动机械臂；每个移动机械臂包括移动平台、旋转平台、第一连杆、第二连杆和第三连杆；所述第一连杆的一端通过第一转动副与所述旋转平台转动连接，另一端通过第二转动副与所述第二连杆的一端连接；所述第二连杆的另一端通过第三转动副与所述第三连杆的一端连接；所述旋转平台通过第四转动副与所述移动平台转动连接；所述移动平台的移动轨迹平行于水平面；所述第一转动副、所述第二转动副以及所述第三转动副三者的轴线相互平行，且均平行于水平面；所述第四转动副的轴线垂直于水平面；以及

负载，其两端分别与两个所述第三连杆的另一端固定连接；

其中，所述空间协作***采用如权利要求1至3中任意一项所述的基于对称约束的动力学模型的建立方法建立在空间协作搬运时的动力学模型。

5.根据权利要求4所述的双移动机械臂空间协作***，其特征在于，所述移动平台用于实现在水平面内的全向移动；每个所述移动机械臂具有四个旋转关节；每个所述移动机械臂具有四个旋转自由度和两个平移自由度；即：所述空间协作***共具有12个自由度。

6.根据权利要求4所述的双移动机械臂空间协作***，其特征在于，所述空间协作***采用伺服电机提供外部控制力。

7.一种计算机终端，其包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行程序时，实现如权利要求1至3中任意一项所述的基于对称约束的动力学模型的建立方法。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时，实现如权利要求1至3中任意一项所述的基于对称约束的动力学模型的建立方法。