CN109543264A - 一种基于多维度重构校正的柔性多体机器人建模与求解方法 - Google Patents

一种基于多维度重构校正的柔性多体机器人建模与求解方法 Download PDF

Info

Publication number
CN109543264A
CN109543264A CN201811338110.3A CN201811338110A CN109543264A CN 109543264 A CN109543264 A CN 109543264A CN 201811338110 A CN201811338110 A CN 201811338110A CN 109543264 A CN109543264 A CN 109543264A
Authority
CN
China
Prior art keywords
dynamics
robot
formula
vector
flexible
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.)
Granted
Application number
CN201811338110.3A
Other languages
English (en)
Other versions
CN109543264B (zh
Inventor
刘凉
史晨阳
赵新华
刘童
李培鑫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin University of Technology
Original Assignee
Tianjin University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Tianjin University of Technology filed Critical Tianjin University of Technology
Priority to CN201811338110.3A priority Critical patent/CN109543264B/zh
Publication of CN109543264A publication Critical patent/CN109543264A/zh
Application granted granted Critical
Publication of CN109543264B publication Critical patent/CN109543264B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/007—Means or methods for designing or fabricating manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/06—Power analysis or power optimisation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Mechanical Engineering (AREA)
  • Software Systems (AREA)
  • Robotics (AREA)
  • Computing Systems (AREA)
  • Automation & Control Theory (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • Operations Research (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种基于多维度重构校正的柔性多体机器人建模与求解方法，该方法依次通过：分别构建刚性构件和柔性构件数学模型并设置参数、建立柔性多体机器人系统的完整约束条件、获得完整约束条件下柔性多体机器人系统的静力学方程和动力学方程，根据t时刻的已知状态参数并利用多维度重构与校正法对完整约束条件下动力学方程迭代求解得到t时刻的状态参数、以及重复迭代过程求出t+h时刻的状态参数直至算法结束；该建模与求解方法建立了完整约束条件下柔性多体机器人的正、逆动力学模型，利用多维度重构与校正算法迭代求解动力学方程，具有建模完整、求解计算规模小的特点，比传统求解方法相比更易满足收敛条件，提高了计算和求解的效率。

Description

一种基于多维度重构校正的柔性多体机器人建模与求解方法
技术领域
本发明涉及机器人多体动力学求解与计算领域，特别涉及一种基于多维度重构校正的柔性多体机器人建模与求解方法。
背景技术
随着工业对生产效率和降低能耗要求的不断提升，促使工业机器人不断向轻型化和小型化发展。因此，机器人的结构中除了包含刚性杆、刚性板、刚性平台和刚性关节外，还引入了柔性梁、板、壳、绳索、关节等构件。机器人通过运动关节把各种构件联接起来，按照联接方式不同可分为串联、并联和混联机构，其中并联机构和混联机构中包含复杂的闭链回路。由于柔性多体机器人建模时要考虑柔性构件的弹性变形与大范围刚体运动之间的耦合关系，同时求解过程必须遵循多体系统的运动学和动力学约束条件，因此合理构建和高效求解系统动力学模型对于柔性多体机器人的设计、性能优化与控制都显得十分重要。
柔性多体机器人是一个多变量、非线性、强耦合的时变动力学系统，因此基于运动学层面的建模方法无法准确描述系统特性，普遍采用的动力学建模理论有运动弹性动力学法(Kineto-Elasto-Dynamics法)、浮动坐标系法以及绝对节点坐标法等方法。由于机器人系统中的运动关节对各运动构件具有确定的约束作用，因此，构建系统动力学方程时必须考虑由运动学约束条件和动力学约束条件构成的完整约束条件，而目前建立柔性多体动力学模型时往往只包含系统几何约束条件，忽略了速度约束条件、加速度约束条件和非有势广义力约束条件，这会导致求解出的系统参数无法完整精确地描述各构件的运动状态，甚至迭代出错误的结果。
完整约束条件下柔性多体机器人动力学模型是由一组刚性微分方程来描述，通常难以获得解析解，虽然可以基于数值积分法进行求解，但极大地限制了迭代步长的上限值，使得求解效率很低。常用的数值积分算法有龙格-库塔法、Adams法、Newmark法、HHT-Alpha法和广义-alpha法等。数值积分算法的缺点在于，一方面在迭代过程中需要计算复杂的系统弹性广义力向量及其雅克比矩阵，造成求解效率变低；另一方面，虽然可以对高频分量引入可控的数值耗散，使得动力学方程迭代过程趋于收敛，但是在积分过程中会使系统中约束方程的迭代误差不断累积增加，其原因是不同类型构件的状态参数混合在一起积分，更新速率的失调破坏了系统动力学方程与约束方程的相容性，使得迭代过程无法收敛到正确解。因此，传统数值积分算法缺乏有效地重构校正环节来提高算法的求解效率和解决违约失效情况的能力。
发明内容
本发明的目的是提供一种解决完整约束条件下柔性多体机器人动力学模型在数值积分过程中求解效率不高和违约失效问题的基于多维度重构校正的柔性多体机器人建模与求解方法。
为此，本发明技术方案如下：
一种基于多维度重构校正的柔性多体机器人建模与求解方法，步骤如下：
S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；同时，获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；
其中，在建立数学模型时，一般选取不参与运动的构件的中心点为坐标系原点，如果存在多个不参与运动的构件，则任意择一选取其中一个不参与运动的构件的中心点为坐标系原点。
S2、根据步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；
其中，有势广义力向量包括重力广义向量和弹性广义向量；非有势广义力向量包括驱动力广义向量、阻尼力广义向量和摩擦力广义向量；
根据步骤S1所设置的各刚性构件和柔性构件的几何参数和材料物理特性建立柔性多体机器人的完整约束条件，包括运动学约束条件和动力学约束条件；其中，柔性多体机器人的完整约束条件通过完整约束方程表示，包括运动学约束方程和动力学约束方程；
S3、基于拉格朗日方程，利用步骤S2得到的结果组建柔性多体机器人的广义坐标向量、质量矩阵、有势广义力向量、非有势广义力向量和约束方程的雅克比矩阵Φq，进而得到完整约束条件下柔性多体机器人的静力学方程和完整约束条件下柔性多体机器人的动力学方程；
S4、预先设置柔性多体机器人的运动轨迹，然后根据柔性多体机器人在时刻t的已知状态参数，采用正动力学求解方法或逆动力学求解方法，同时利用多维度重构校正法对完整约束条件下柔性多体机器人的力学方程进行迭代求解，得到t时刻柔性多体机器人状态参数；
S5、将经过步骤S4得到的t时刻柔性多体机器人状态参数作为初始值，利用多维度重构校正法在时间步[t，t+h]上进行迭代求解，得到t+h时刻柔性多体机器人的状态参数；其中，h为积分步长；
S6、重复步骤S5，直到计算时间达到预先设定的仿真总时间T，完成全部求解计算并输出柔性多体机器人末端执行器的位置、速度和加速度，或者柔性多体机器人各个驱动构件的位置、速度、加速度和驱动力值，其中，驱动构件由S1步中的刚性构件或者柔性构件组成，确定仿真时间内柔性多体机器人的运动状态，即完成整个建模与求解过程。
进一步地，在所述步骤S3中，完整约束条件下柔性多体机器人的静力学方程为：
其中，
在式(1)中，q为广义坐标向量，QG(q)为柔性多体机器人重力广义力向量，QE(q)为柔性多体机器人弹性广义力向量，为广义速度向量，为柔性多体机器人的广义驱动力向量、阻尼力和摩擦力的非有势广义力向量，为对Φq的矩阵转置，Φq为Φ(q，t)对q求偏导数，λ为拉格朗日乘子向量，t为时间，τ为驱动力向量，τmin和τmax分别为柔性多体机器人驱动力向量的下限约束值与上限约束值；Φ(q，t)为柔性多体机器人几何约束方程向量，其包含各刚性构件和各柔性构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；为柔性多体机器人速度约束方程向量，是Φ(q，t)对t求全导数，其包含速度约束条件；为柔性多体机器人加速度约束方程向量，是Φ(q，t)对t求二阶全导数，其包含加速度约束条件；
在式(2)中，Φqq为Φ(q，t)对q求二阶偏导数，Φqt为Φ(q，t)依次对q和t求一阶偏导数，Φtt为Φ(q，t)对时间t求二阶偏导数；
在所述步骤S3中，完整约束条件下柔性多体机器人的动力学方程为：
在式(3)中，M(q)为柔性多体机器人的质量矩阵，其由步骤S1中构建的各刚性构件的数学模型和各柔性构件的数学模型确定。
进一步地，在步骤S4中，采用正动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：
S401、根据驱动构件在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、q0、和其中，拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件同时，设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：
在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；ec、ecv、eca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10-15，10-4]；ed为驻留方程向量Dn的迭代求解精度，其取值范围为[10-15，10-5]；Dn包含两种情况：当柔性多体机器人处于静力平衡状态时，Dn由式(5)表示，而当柔性多体机器人处于动力平衡状态时，Dn由式(6)表示：
S402、利用牛顿法递推公式，如下式(7)所示，迭代计算柔性多体机器人的待求状态参数：
在式(7)中，n为迭代次数，初始值为0，每迭代一次n自动加1，Dn为柔性多体机器人驻留方程向量，其根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，H为迭代变换矩阵，其包含两个分块矩阵H1和H2，对应表达式为：
在式(8)中，h为积分步长，[]-1表示对矩阵求逆，[]-T表示对矩阵求逆和转置；为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时由式(9)表示，当柔性多体机器人处于动力平衡状态时由式(10)表示：
在式(9)和式(10)中，为向量函数对广义加速度向量的偏导数，而上式(7)中的迭代参数为αf、αm和β采用如下广义-alpha隐式积分算法的参数：
在式(11)中，ρ为算法谱半径，ρ∈[0，1]；
根据式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S403、利用式(7)再迭代计算N-1次待求状态参数，其中，N为正整数且N≤50；记录第N次迭代后Φn和Dn的值，并基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和进行重构计算：
在式(12)中，迭代次数n的初始值为N，N的取值优选为4；tn和tn+1分别为积分下限和上限，且tn+1＝tn+h；
基于Adams法对qn+1和进行修正并重构计算和λn+1：
按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求：
在式(14)、式(15)和式(16)中，和分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；为向量函数对广义速度向量的偏导数，而矩阵和则用Broyden拟牛顿法进行计算；
||Dn||满足收敛条件要求，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代得到t时刻柔性多体机器人状态参数，并求出柔性多体机器人末端执行器的状态参数。
进一步的，当步骤S4采用正动力学求解方法得到t时刻的柔性多体机器人状态参数，步骤S5的具体步骤为：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据驱动构件在t+h时刻已知的状态参数，按照上述步骤S4中正动力学求解方法的求解步骤进行计算，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人末端执行器的状态参数。
进一步地，在步骤S4中，采用逆动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：
S401、与正动力学求解方法的步骤S401相同，根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、τ0、q0、和其中，拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件驱动力向量满足条件τmin≤τ≤τmax；并按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件；
S402、利用如下式(17)所示的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：
在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1，柔性多体机器人的几何约束方程向量Φ(qn，tn)包含Φb(qn,tn)和Φa(qn,tn)两个分量，它们分别由末端执行器和运动构件的状态参数决定，Dn为柔性多体机器人驻留方程向量，根据柔性多体机器人的状态不同可分别由式(5)和式(6)表示；迭代变换矩阵H包含三个分块矩阵H1、H2和H3，其表达式为：
利用式(17)、式(18)以及式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λn、τn、qn、就是t时刻的柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S403、利用式(17)再迭代计算N-1次待求状态参数，N为正整数且N≤50，其中，N的取值优选为4；记录第N次迭代后Φn和Dn的值，并基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、和λ进行重构计算，然后利用下式(19)对τ进行重构计算：
τn+1＝H2[Φa(qn+1,tn+1) Φb(qn+1,tn+1) Dn+1]T 式(19)
按照式(4)判断所求迭代结果是否满足迭代收敛条件：若满足，则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求；若||Dn||和τn满足收敛条件要求，则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、τn、qn、 设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、τn、qn、设置为迭代初值，重复执行S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代计算得到t时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
进一步地，当步骤S4采用逆动力学求解方法得到t时刻的柔性多体机器人状态参数，则步骤S5的具体步骤为：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据末端执行器在t+h时刻已知的状态参数，按照上述步骤S4中逆动力学求解方法的求解步骤进行计算，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
与现有技术相比，该基于多维度重构校正的柔性多体机器人建模与求解方法的有益效果包括：
(1)该方法利用自然坐标法或相对坐标法描述刚性构件的数学模型，利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述柔性构件的数学模型，构建柔性多体机器人动力学方程时同时考虑由运动学约束条件和动力学约束条件构成的完整约束条件，保证了求出的柔性多体机器人参数能够完整精确地描述各构件的运动状态，避免了出现错误迭代解的现象；
(2)该方法基于Adams方法对拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量进行多维度重构迭代计算，迭代过程不需要计算复杂的弹性广义力向量及其雅克比矩阵，在相同的求解精度要求下，可以获得更高的求解效率；
(3)该方法基于运动学约束条件对广义坐标向量、广义速度向量和广义加速度向量进行了校正，解决了动力学模型在迭代求解过程中出现的违约问题，在校正过程中利用了Broyden拟牛顿法求解迭代矩阵，能够对完整约束条件下多体动力学方程进行稳定、高效的求解。
附图说明
图1为本发明的实施例1和实施例2中的三自由度3-RRRU柔性多体机器人示意图；
图2为本发明的实施例1和实施例2中的三自由度3-RRRU柔性多体机器人的各刚性构件和各柔性构件建模所用的广义坐标向量示意图；
图3为本发明的实施例1和实施例2中柔性多体机器人使用相对坐标法对驱动关节角进行定义的示意图；
图4为本发明的实施例1中柔性多体机器人的正动力学求解方法的步骤流程图；
图5为本发明的实施例1中柔性多体机器人的正动力学求解方法中驱动关节角位移的示意图；
图6为本发明的实施例1中柔性多体机器人的正动力学求解方法中驱动关节角速度的示意图；
图7为本发明的实施例1中柔性多体机器人的正动力学求解方法中驱动关节角加速度的示意图；
图8为本发明的实施例1中柔性多体机器人的正动力学求解方法中驱动关节驱动力矩的示意图；
图9为本发明的实施例1中柔性多体机器人的正动力学求解方法中末端执行器运动轨迹的示意图；
图10为本发明的实施例1中柔性多体机器人的正动力学求解方法中末端执行器加速度的示意图；
图11为本发明的实施例1中柔性多体机器人的正动力学求解方法中动平台与ZG轴之间夹角的示意图；
图12为本发明的实施例1中柔性多体机器人的正动力学求解方法的求解过程中动力学方程误差的示意图；
图13为本发明的实施例1中柔性多体机器人的正动力学求解方法的求解过程中约束方程误差的示意图；
图14为本发明的实施例1中柔性多体机器人的正动力学求解方法的求解过程中广义-alpha方法下的约束方程误差的示意图；
图15为本发明的实施例2中柔性多体机器人的逆动力学求解方法的步骤流程图；
图16为本发明的实施例2中柔性多体机器人的逆动力学求解方法中末端执行器运动轨迹的示意图；
图17为本发明的实施例2中柔性多体机器人的逆动力学求解方法中末端执行器速度的示意图；
图18为本发明的实施例2中柔性多体机器人的逆动力学求解方法中末端执行器加速度的示意图；
图19为本发明的实施例2中柔性多体机器人的逆动力学求解方法中驱动关节角位移的示意图；
图20为本发明的实施例2中柔性多体机器人的逆动力学求解方法中驱动关节角速度的示意图；
图21为本发明的实施例2中柔性多体机器人的逆动力学求解方法中驱动关节角加速度的示意图；
图22为本发明的实施例2中柔性多体机器人的逆动力学求解方法中驱动关节驱动力矩的示意图；
图23为本发明的实施例2中柔性多体机器人的逆动力学求解方法的求解过程中动力学方程误差的示意图；
图24为本发明的实施例2中柔性多体机器人的逆动力学求解方法的求解过程中约束方程误差的示意图；
图25为本发明的实施例2中柔性多体机器人的逆动力学求解方法的求解过程中广义-alpha方法下的约束方程误差的示意图。
具体实施方式
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
实施例1
如图1所示为一个三自由度3-RRRU柔性多体机器人，其由静平台、动平台及三条结构对称的支链组成；其中，三条支链采用并联方式设置，且每条支链的两端分别与静平台和动平台连接。由于其结构中包括柔性构件，因此需要利用本发明公开的基于多维度重构校正的柔性多体机器人建模与求解方法对该机器人的系统特性进行描述。
为此，具体的建模与求解方法的步骤如下：
S1、构建柔性多体机器人各构件的数学模型并设置参数：
该3-RRRU柔性多体机器人包括1个静平台、1个动平台、6根刚性杆和3根柔性杆，其中，1根柔性杆和与之相连接的2根串联的刚性杆构成一条支链，每条支链的两端分别与动平台和静平台相连接；柔性杆与刚性杆之间、刚性杆与静平台之间通过刚性转动关节形成活动链接，柔性杆与动平台之间均通过刚性虎克铰形成活动链接；刚性杆与静平台之间通过刚性驱动关节形成活动链接；
如图1所示，序号0表示的刚性静平台和序号10表示的刚性动平台具有等边三角形的特征，各支链均包含3个运动杆件；其中，序号为1、4和7的构件为形状和密度相同的刚性杆，为该柔性多体机器人的驱动构件；序号为2、5和8的构件为形状和密度相同的刚性杆，序号为3、6和9的构件为形状和密度相同的柔性杆，各支链均包含3个运动副，其中点A0i、A1i、A2i(i＝1，2，3)处为刚性转动关节，点A0i(i＝1，2，3)处转动关节为驱动关节，点A4i(i＝1，2，3)处为刚性虎克铰；
获取参与运动的构件的质量、尺寸和质心分布情况，不参与运动的构件的尺寸，以及柔性构件的相关参数；具体为获取该3-RRRU柔性多体机器人的刚性杆、柔性杆和动平台的质量、刚性杆和柔性杆的杆长，动平台和静平台的外接圆半径，柔性杆杨氏弹性模量、泊松比，以及每个构件的质心分布情况。
其中，静平台外接圆半径R＝0.175m，动平台外接圆半径r＝0.06m，动平台质量m10＝0.3kg各刚性杆长度l1＝l4＝l7＝0.375m，l2＝l5＝l8＝0.09m，各刚性杆质量m1＝m4＝m7＝2kg，m2＝m5＝m8＝0.3kg，各柔性杆在未变形状态下的原始长度l3＝l6＝l9＝0.855m，各柔性杆质量m3＝m6＝m9＝1.2kg，各柔性杆件的杨氏弹性模量E＝6.9×108Pa，泊松比ν＝0.3。由于所有构件的质量分布都是均匀的，其质心均位于构件的几何中心处。
将柔性多体机器人的末端执行器设置在动平台的几何中心点处，即动平台上的P点；对该柔性多体机器人中的静平台、动平台和六根刚性杆使用自然坐标法进行建模，而三个柔性杆则使用绝对节点坐标法进行建模；在建模过程中，选取静平台的中心点作为建立坐标系的原点。S2、根据步骤S1中，对该3-RRRU柔性多体机器人的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；具体地，
如图2所示，每个支链中的两根刚性杆分别用广义坐标向量q1＝(x1，z1)T、q2＝(x4，z4)T和q3＝(x7，z7)T表示，其中，(x1，z1)为A11点的全局坐标，(x4，z4)为A12点的全局坐标，(x7，z7)为A13点的全局坐标；每个支链中的柔性杆分别用广义坐标向量 和表示；动平台用广义坐标向量表示，其中，[]T表示对列向向量或矩阵的转置；
各刚性杆、各柔性杆和动平台的质量矩阵用Mi(i＝1，2，…，10)表示；对应地，作用在上述各构件上的重力有势广义力向量用QGi(i＝1，2，…，10)表示；作用在上述各构件上的非有势广义力向量用Qi(i＝1，2，…，10)表示；三个柔性杆上的弹性有势广义力向量用QEi(i＝1，2，3)表示；
其中，根据该装置结构特点，只考虑序号为第1、第4和第7构件上所受的驱动力，各构件所受的摩擦力和阻尼力在此不考虑；在上述表达式中，i＝1，2，…，10依次表示9个杆和1个动平台；
根据各刚性杆、柔性杆、转动副和动平台的运动学约束关系建立柔性多体机器人的运动学约束条件和动力学约束条件；具体地，运动学约束条件包括：几何约束方程向量Φ(q，t)、速度约束方程向量和加速度约束方程向量其中，每个向量均涉及21个分量，共计63个维度，即Φ(q，t)∈R21，动力学约束条件为τmin≤τ≤τmax，该条件涉及3个约束方程，因此，驱动力向量τ涉及3个维度，即τ∈R3，上述运动学约束条件和动力学约束条件构成了该柔性多体机器人完整约束条件。
S3、基于拉格朗日方程，利用步骤S2得到的结果组建柔性并联多体机器人的广义坐标向量为q＝[q1 T，e1 T，q2 T，e2 T，q3 T，e3 T]T，q∈R78，质量矩阵为M，重力有势广义力向量为QG，弹性有势广义力向量为QE，非有势广义力向量为Q，约束方程的雅克比矩阵Φq(q，t)，得到完整约束条件下柔性多体机器人的静力学方程，如式(1)所示，
其中，
在式(1)中QG(q)∈R78为重力广义力向量，其维度与广义坐标向量q的维度相同；QE(q)∈R78为弹性广义力向量，其维度与广义坐标向量q的维度相同；为非有势广义力向量，其维度与广义坐标向量q的维度相同；[]T表示对矩阵进行转置；λ为拉格朗日乘子向量，其维度与几何约束方程向量Φ(q，t)的维度相同；q∈R78为广义坐标向量，为广义速度向量，为广义加速度向量；τ∈R3为驱动力向量，包含了作用在点A0i(i＝1，2，3)处的驱动关节的驱动力，其下限约束值与上限约束值分别为τmin和τmax，τmax为三个驱动关节上驱动电机的最大驱动力向量，τmin为三个驱动关节上驱动电机的最小驱动力向量；t为时间，Φ(q，t)为几何约束方程向量，其由21个几何约束条件组成，合计共包含六个刚性杆件的长度约束条件、A2i(i＝1，2，3)处九个刚性转动关节轴线约束条件、A4i(i＝1，2，3)处三个刚性虎克铰轴线正交约束条件和动平台A4i(i＝1，2，3)处三个端点长度约束条件；在式(2)中，为速度约束方程向量，是Φ(q，t)对t求全导数，包含速度约束条件，为加速度约束方程向量，是Φ(q，t)对t求二阶全导数，包含加速度约束条件。
进而，得到完整约束条件下柔性多体机器人的动力学方程为：
在式(3)中，M(q)∈R78×78为柔性多体机器人质量矩阵，其维度与广义坐标向量q的维度相匹配，由步骤S1中刚性构件和柔性构件分别构建的数学模型来确定。
步骤S4、预先设置柔性多体机器人的运动轨迹，根据柔性多体机器人在时刻t的已知状态参数，采用正动力学求解方法，并利用多维度重构校正法对完整约束条件下3-RRRU柔性多体机器人的力学方程进行迭代求解，以得到t时刻柔性多体机器人状态参数。
如图4所示，该正动力学求解方法的具体步骤为：
S401、根据驱动构件在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、q0、和其中拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：
在式(4)中，||·||为对向量求模，n为迭代次数，初始值为0，几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度ec、ecv和eca均设为10-8，驻留方程向量Dn的迭代求解精度ed＝10-6，驱动力向量下限约束值与上限约束值分别为τmin＝[-20，-20，-20]T，τmax＝[20，20，20]T，Dn包含两种情况：当柔性多体机器人处于静力平衡状态时Dn由式(5)表示，当柔性多体机器人处于动力平衡状态时Dn由式(6)表示：
S402、利用如下牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：
在式(8)中，n为迭代次数，初始值为0，每迭代一次n自动加1，Φ(qn，tn)为几何约束方程向量，Dn为柔性多体机器人驻留方程向量，根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，迭代变换矩阵H包含两个分块矩阵H1和H2，其表达式为：
在式(8)中，积分步长h＝0.002，()-1为对矩阵求逆，()-T为对矩阵求逆和转置，为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时由式(9)表示，当柔性多体机器人处于动力平衡状态时由式(10)表示：
在式(9)和式(10)中，为向量函数对广义加速度向量的偏导数，在式(7)中，迭代参数αf、αm和β采用如下广义-alpha隐式积分算法的参数：
在式(11)中，ρ为算法谱半径，ρ＝0.4；
按照上式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S403、设定N＝4，利用式(7)再迭代计算N-1次待求状态参数，记录第N次迭代后Φn和Dn的值，基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和进行重构计算：
在式(12)中，迭代次数n的初始值为N，积分下限和上限分别tn和tn+1，且tn+1＝tn+h；
基于Adams法对qn+1和进行修正并重构计算和λn+1：
按照上式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求：
在上式(14)～式(16)中，和分别为此刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量，三者各涉及15个分量，共计15个维度，，其中，合计共包含六个刚性杆件的长度约束条件、A4i(i＝1，2，3)点到给定平面的三个距离约束条件、A4i(i＝1，2，3)处三个刚性虎克铰轴线正交约束条件和动平台A4i(i＝1，2，3)处三个端点长度约束条件；在式(15)中，为向量函数对广义速度向量的偏导数，矩阵和用Broyden拟牛顿法进行计算；
若||Dn||满足收敛条件要求，则λn、qn、就是t时刻系统的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代得到t时刻柔性多体机器人的状态参数，并求出柔性多体机器人末端执行器的状态参数。
S5、将t时刻求出的状态参数作为初始值，利用多维度重构校正法在时间步[t，t+h]上进行迭代求解，得到t+h时刻柔性多体机器人状态参数，其中h为积分步长；具体步骤如下：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据驱动构件在t+h时刻已知的状态参数，按照步骤S4中正动力学求解类型的求解步骤和求解方法进行计算，其中N＝4，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人末端执行器的状态参数。
S6、重复步骤S5，直到计算时间达到预设定的仿真总时间T时，完成全部求解计算，输出柔性多体机器人末端执行器的位置、速度、加速度值以及驱动构件的位置、速度、加速度和驱动力值，确定仿真时间内柔性多体机器人的运动状态，完成整个建模与求解过程。
对该3-RRRU柔性多体机器人采用正动力学模型求解后进行仿真计算：
第1、第4和第7构件的运动状态可由点A0i(i＝1，2，3)处驱动关节角θ1i(i＝1，2，3)的运动状态来描述，θ1i的定义方式如图3所示，φi(i＝1，2，3)为坐标轴XG绕ZG轴旋转到与A0i处坐标轴x0i(i＝1，2，3)共线时的角度。已知三个驱动关节角的位移、速度和加速度如图5至图7所示，三个驱动关节所受的驱动力矩如图8所示，为验证算法的有效性和正确性，采用说明书中的方法对3-RRRU柔性多体机器人的正动力学模型进行数值迭代求解，求出末端执行器的运动轨迹和加速度如图9和图10所示，动平台与ZG轴的夹角如图11所示，图12为迭代过程中动力学方程误差，其迭代求解精度满足ed的设定值10-6，图13为迭代过程中约束方程误差，它将所有几何约束方程向量、速度约束方程向量和加速度约束方程向量的模进行求和，其迭代精度均满足ec、ecv和eca的设定值10-8，全部求解过程平稳并且收敛。
图14是使用广义-alpha方法对3-RRRU柔性多体机器人的正动力学模型迭代求解时约束方程的误差，通过对比发现，该误差值逐渐增加，超过了约束方程迭代精度的设定值，无法满足完整约束条件下柔性多体机器人模型求解的迭代收敛条件。
实施例2
一种基于多维度重构校正的柔性多体机器人建模与求解方法，其同样针对如图1所示的三自由度3-RRRU柔性多体机器人的特性进行描述。
与实施例1不同之处在于，在本实施例中的步骤S4中，根据柔性多体机器人在时刻t的已知状态参数，采用逆动力学求解方法，利用多维度重构校正法对完整约束条件下3-RRRU柔性多体机器人的力学方程进行迭代求解，以得到t时刻柔性多体机器人的状态参数。
如图15所示，该逆动力学求解方法的具体步骤为：
S401、根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、τ0、q0、和其中，拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件驱动力向量满足条件τmin≤τ≤τmax；按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件，此处条件设置方式与正动力学求解方法中步骤S401的设置方式使一致的；
S402、利用下式(17)的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：
在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1；由于驱动力向量未知，几何约束方程向量Φ(qn，tn)应包含Φa(qn,tn)∈R21和Φb(qn,tn)∈R3两组分量；其中，Φa(qn,tn)是正动力学模型求解中使用的21个几何约束条件，Φb(qn,tn)则由已知的末端执行器三个空间坐标值决定，Dn为柔性多体机器人的驻留方程向量，根据状态不同可分别由式(5)和式(6)表示，迭代变换矩阵H包含三个分块矩阵H1、H2和H3，其表达式为：
利用式(17)、式(18)和式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S403、设定N＝4，利用式(17)再迭代计算N-1次待求状态参数，，记录第N次迭代后Φn和Dn的值，基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、和λ进行重构计算，利用下式(19)对τ进行重构计算：
τn+1＝H2[Φa(qn+1,tn+1) Φb(qn+1,tn+1) Dn+1]T 式(19)
按照式(4)判断所求迭代结果是否满足迭代收敛条件，若满足则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求：若||Dn||和τn满足收敛条件要求，则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、τn、qn、 设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、τn、qn、设置为迭代初值，重复执行S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代计算得到t时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
S5、将t时刻求出的状态参数作为初始值，利用多维度重构校正法在时间步[t，t+h]上进行迭代求解，得到t+h时刻柔性多体机器人状态参数，其中h为积分步长；具体步骤如下：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据末端执行器在t+h时刻已知的状态参数，按照步骤S4中逆动力学求解类型的求解步骤和求解方法进行计算，其中设定N＝4，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
S6、重复步骤S5，直到计算时间达到预设定的仿真总时间T时，完成全部求解计算，输出柔性多体机器人末端执行器的位置、速度、加速度值以及驱动构件的位置、速度、加速度和驱动力值，确定仿真时间内柔性多体机器人的运动状态，完成整个建模与求解过程。
与实施例1相同，对该3-RRRU柔性多体机器人采用逆动力学模型求解后进行仿真计算：
已知3-RRRU柔性多体机器人的末端执行器的运动轨迹、运动速度和加速度分别如图16至图18所示，为验证算法的有效性和正确性，采用说明书中的方法对3-RRRU柔性多体机器人的逆动力学模型进行数值迭代求解，求出第1、第4和第7构件的运动状态，它们可由点A0i(i＝1，2，3)处驱动关节角θ1i(i＝1，2，3)的运动状态来描述，求出的驱动关节角的位移、速度和加速度如图19和图21所示，三个驱动关节所受的驱动力矩如图22所示，所有驱动力矩均满足驱动力向量τ的上限约束值τmax＝[20，20，20]和下限约束值τmin＝[-20，-20，-20]T，图23为迭代过程中动力学方程误差，其迭代求解精度满足ed的设定值10-6，图24为迭代过程中约束方程误差，它将所有几何约束方程向量、速度约束方程向量和加速度约束方程向量的模进行求和，其迭代精度均满足ec、ecv和eca的设定值10-8，全部求解过程平稳并且收敛。
图25是使用广义-alpha方法对3-RRRU柔性多体机器人的逆动力学模型迭代求解时约束方程的误差，与该方法的处理结果通过对比发现，该误差值逐渐增加，超过了约束方程迭代精度的设定值，无法满足完整约束条件下柔性多体机器人模型求解的迭代收敛条件。

Claims (9)

1.一种基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，步骤如下：
S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；
S2、根据步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；根据步骤S1所设置的各刚性构件和柔性构件的几何参数和材料物理特性建立柔性多体机器人的完整约束方程，包括运动学约束方程和动力学约束方程；
S3、基于拉格朗日方程，利用步骤S2得到的结果组建柔性多体机器人的广义坐标向量、质量矩阵、有势广义力向量、非有势广义力向量和约束方程的雅克比矩阵，进而得到完整约束条件下柔性多体机器人的静力学方程和完整约束条件下柔性多体机器人的动力学方程；
S4、设置柔性多体机器人的运动轨迹，并根据柔性多体机器人在时刻t的已知状态参数，采用正动力学求解方法或逆动力学求解方法，同时利用多维度重构校正法对完整约束条件下柔性多体机器人的力学方程进行迭代求解，得到t时刻柔性多体机器人状态参数；
S5、将经过步骤S4得到的t时刻柔性多体机器人状态参数作为初始值，利用多维度重构校正法在时间步[t，t+h]上进行迭代求解，得到t+h时刻时柔性多体机器人的状态参数；其中，h为积分步长；
S6、重复步骤S5，直到计算时间达到预先设定的仿真总时间T，完成全部求解计算并输出柔性多体机器人末端执行器的位置、速度和加速度，或者各驱动构件的位置、速度、加速度和驱动力值，确定仿真时间内柔性多体机器人的运动状态，即完成整个建模与求解过程。
2.根据权利要求1所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，在所述步骤S3中，完整约束条件下柔性多体机器人的静力学方程为：
其中，
在式(1)中，q为广义坐标向量，QG(q)为柔性多体机器人重力广义力向量，QE(q)为柔性多体机器人弹性广义力向量，为广义速度向量，为柔性多体机器人的广义驱动力向量、阻尼力和摩擦力的非有势广义力向量，为对Φq的矩阵转置，Φq为Φ(q，t)对q求偏导数，λ为拉格朗日乘子向量，t为时间，τ为驱动力向量，τmin和τmax分别为柔性多体机器人驱动力向量的下限约束值与上限约束值；Φ(q，t)为柔性多体机器人几何约束方程向量，其包含各刚性构件和各柔性构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；为柔性多体机器人速度约束方程向量，是Φ(q，t)对t求全导数，其包含速度约束条件；为柔性多体机器人加速度约束方程向量，是Φ(q，t)对t求二阶全导数，其包含加速度约束条件；在式(2)中，Φqq为Φ(q，t)对q求二阶偏导数，Φqt为Φ(q，t)依次对q和t求一阶偏导数，Φtt为Φ(q，t)对时间t求二阶偏导数；
在所述步骤S3中，完整约束条件下柔性多体机器人的动力学方程为：
在式(3)中，M(q)为柔性多体机器人的质量矩阵，其由步骤S1中构建的各刚性构件的数学模型和各柔性构件的数学模型确定。
3.根据权利要求1所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，在所述步骤S4中，采用正动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：
S401、根据驱动构件在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、q0、和其中，拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件同时，设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：
在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；ec、ecv、eca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10-15，10-4]；ed为驻留方程向量Dn的迭代求解精度，其取值范围为[10-15，10-5]；Dn包含两种情况：当柔性多体机器人处于静力平衡状态时，Dn由式(5)表示，而当柔性多体机器人处于动力平衡状态时，Dn由式(6)表示：
S402、利用牛顿法递推公式，如下式(7)所示，迭代计算柔性多体机器人的待求状态参数：
在式(7)中，n为迭代次数，初始值为0，每迭代一次n自动加1，Dn为柔性多体机器人驻留方程向量，其根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，H为迭代变换矩阵，其包含两个分块矩阵H1和H2，对应表达式为：
在式(8)中，h为积分步长，[]-1表示对矩阵求逆，[]-T表示对矩阵求逆和转置；为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时由式(9)表示，当柔性多体机器人处于动力平衡状态时由式(10)表示：
在式(9)和式(10)中，为向量函数对广义加速度向量的偏导数，而上式(7)中的迭代参数为αf、αm和β采用如下广义-alpha隐式积分算法的参数：
在式(11)中，ρ为算法谱半径，ρ∈[0，1]；
根据式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S403、利用式(7)再迭代计算N-1次待求状态参数，其中，N为正整数且N≤50；记录第N次迭代后Φn和Dn的值，并基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和进行重构计算：
在式(12)中，迭代次数n的初始值为N；tn和tn+1分别为积分下限和上限，且tn+1＝tn+h；
基于Adams法对qn+1和进行修正并重构计算和λn+1：
按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求：
在式(14)、式(15)和式(16)中，和分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；为向量函数对广义速度向量的偏导数，而矩阵和则用Broyden拟牛顿法进行计算；
若||Dn||满足收敛条件要求，则λn、qn、就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、qn、设置为迭代初值，重复执行步骤S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代得到t时刻柔性多体机器人状态参数，并求出柔性多体机器人末端执行器的状态参数。
4.根据权利要求1所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，在所述步骤S4中，采用逆动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：
S401、根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ0、τ0、q0、和其中，拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件驱动力向量满足条件τmin≤τ≤τmax；并按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：
在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；ec、ecv、eca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10-15，10-4]；ed为驻留方程向量Dn的迭代求解精度，其取值范围为[10-15，10-5]；Dn包含两种情况：当柔性多体机器人处于静力平衡状态时，Dn由式(5)表示，而当柔性多体机器人处于动力平衡状态时，Dn由式(6)表示：
S402、利用式(17)的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：
在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1，柔性多体机器人的几何约束方程向量Φ(qn，tn)包含Φb(qn,tn)和Φa(qn,tn)两个分量，它们分别由末端执行器和运动构件的状态参数决定，Dn为柔性多体机器人驻留方程向量，根据柔性多体机器人的状态不同可分别由式(5)和式(6)表示；迭代变换矩阵H包含三个分块矩阵H1、H2和H3，其表达式为：
利用式(17)、式(18)以及式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，
其中，为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时由式(9)表示，当柔性多体机器人处于动力平衡状态时由式(10)表示：在式(9)和式(10)中，为向量函数对广义加速度向量的偏导数，而上式(17)中的迭代参数为αf、αm和β采用如下广义-alpha隐式积分算法的参数：
在式(11)中，ρ为算法谱半径，ρ∈[0，1]；
按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λn、τn、qn、就是t时刻的柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S403、利用式(17)再迭代计算N-1次待求状态参数，N为正整数且N≤50，其中，记录第N次迭代后Φn和Dn的值，并基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、和λ进行重构计算，然后利用下式(19)对τ进行重构计算：
τn+1＝H2[Φa(qn+1,tn+1) Φb(qn+1,tn+1) Dn+1]T 式(19)
其中，在式(12)中，迭代次数n的初始值为N；tn和tn+1分别为积分下限和上限，且tn+1＝tn+h；按照式(4)判断所求迭代结果是否满足迭代收敛条件：若满足，则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
S404、当收敛条件中||Φ(qn，tn)||、和不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对qn、和进行校正直到它们全部满足求解精度要求；若||Dn||和τn满足收敛条件要求，则λn、τn、qn、就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；
在式(14)、式(15)和式(16)中，和分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；为向量函数对广义速度向量的偏导数，而矩阵和则用Broyden拟牛顿法进行计算；
S405、若相邻两次迭代结果求出的||Dn||值是逐渐递减的，将当前状态参数λn、τn、qn、 设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||Dn||值是逐渐递增的，则将当前状态参数λn、τn、qn、设置为迭代初值，重复执行S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代计算得到t时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
5.根据权利要求3所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，设置拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件
6.根据权利要求4所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，设置拉格朗日乘子向量迭代初值为λ0＝0，广义坐标向量q0迭代初值满足条件QE(q0)＝0、广义速度向量迭代初值满足条件广义加速度向量的迭代初值满足条件驱动力向量满足条件τmin≤τ≤τmax。
7.根据权利要求3或4所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，在步骤S403中，迭代计算次数N值为4。
8.根据权利要求3所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，步骤S5的具体步骤为：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据末端执行器在t+h时刻已知的状态参数，按照上述步骤S4中正动力学求解方法的求解步骤进行计算，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人末端执行器的状态参数。
9.根据权利要求4所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，步骤S5的具体步骤为：
S501、将t时刻求出的迭代解作为迭代初始值；
S502、根据末端执行器在t+h时刻已知的状态参数，按照上述步骤S4中逆动力学求解方法的求解步骤进行计算，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。
CN201811338110.3A 2018-11-12 2018-11-12 一种基于多维度重构校正的柔性多体机器人建模与求解方法 Active CN109543264B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811338110.3A CN109543264B (zh) 2018-11-12 2018-11-12 一种基于多维度重构校正的柔性多体机器人建模与求解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811338110.3A CN109543264B (zh) 2018-11-12 2018-11-12 一种基于多维度重构校正的柔性多体机器人建模与求解方法

Publications (2)

Publication Number Publication Date
CN109543264A true CN109543264A (zh) 2019-03-29
CN109543264B CN109543264B (zh) 2023-01-20

Family

ID=65846790

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811338110.3A Active CN109543264B (zh) 2018-11-12 2018-11-12 一种基于多维度重构校正的柔性多体机器人建模与求解方法

Country Status (1)

Country Link
CN (1) CN109543264B (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110427686A (zh) * 2019-07-29 2019-11-08 中国科学院长春光学精密机械与物理研究所 一种考虑约束条件的大刚体位移参量计算方法
CN110421564A (zh) * 2019-08-06 2019-11-08 浙江大学 一种基于关节能耗评估的机器人工作单元布局优化方法
CN111080720A (zh) * 2019-12-26 2020-04-28 重庆盟讯电子科技有限公司 一种模组矫正方法
CN111515956A (zh) * 2020-05-13 2020-08-11 中科新松有限公司 杆件及关节柔性的机器人运动学标定方法
CN111723536A (zh) * 2020-06-16 2020-09-29 岭南师范学院 一种瓦斯抽采钻机系统的多体动力学分析方法
CN112001087A (zh) * 2020-08-27 2020-11-27 南京航空航天大学 一种旋转关节型工业机器人非线性动力学建模分析方法
WO2020259132A1 (zh) * 2019-06-27 2020-12-30 清华大学深圳国际研究生院 绳驱动联动式机械臂的动力学建模及其绳索张力优化方法
CN112818481A (zh) * 2021-01-19 2021-05-18 中国科学院沈阳自动化研究所 弹性平面约束的细长软体机器人的建模与控制方法
CN113001537A (zh) * 2019-12-20 2021-06-22 深圳市优必选科技股份有限公司 机械臂控制方法、机械臂控制装置及终端设备
CN113255268A (zh) * 2021-05-21 2021-08-13 北京华大九天科技股份有限公司 一种电路仿真中瞬态分析不收敛的检测及修复方法
CN113297730A (zh) * 2021-05-13 2021-08-24 广东工业大学 基于自适应模态的柔性多体系统动态响应计算方法和系统
CN116362151A (zh) * 2023-02-24 2023-06-30 深圳市人工智能与机器人研究院 微螺旋组合马达驱动的微型执行器设计方法及相关设备

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101639681A (zh) * 2008-07-29 2010-02-03 深圳市大族激光科技股份有限公司 一种电子装备运动机构性能参数优化方法
CN107220421A (zh) * 2017-05-18 2017-09-29 北京理工大学 一种空间复杂柔性结构多体系统动力学建模与计算方法
CN107943748A (zh) * 2017-11-22 2018-04-20 南京理工大学 基于Bathe积分策略的多体动力学方程求解方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101639681A (zh) * 2008-07-29 2010-02-03 深圳市大族激光科技股份有限公司 一种电子装备运动机构性能参数优化方法
CN107220421A (zh) * 2017-05-18 2017-09-29 北京理工大学 一种空间复杂柔性结构多体系统动力学建模与计算方法
CN107943748A (zh) * 2017-11-22 2018-04-20 南京理工大学 基于Bathe积分策略的多体动力学方程求解方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘凉 等: "空间刚柔耦合并联机构的逆动力学建模与控制", 《光学 精密工程》 *
刘凉 等: "考虑关节摩擦的并联机器人平滑轨迹规划", 《机械工程学报》 *

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020259132A1 (zh) * 2019-06-27 2020-12-30 清华大学深圳国际研究生院 绳驱动联动式机械臂的动力学建模及其绳索张力优化方法
CN110427686A (zh) * 2019-07-29 2019-11-08 中国科学院长春光学精密机械与物理研究所 一种考虑约束条件的大刚体位移参量计算方法
CN110427686B (zh) * 2019-07-29 2022-03-29 中国科学院长春光学精密机械与物理研究所 一种考虑约束条件的大刚体位移参量计算方法
CN110421564A (zh) * 2019-08-06 2019-11-08 浙江大学 一种基于关节能耗评估的机器人工作单元布局优化方法
CN113001537A (zh) * 2019-12-20 2021-06-22 深圳市优必选科技股份有限公司 机械臂控制方法、机械臂控制装置及终端设备
CN111080720A (zh) * 2019-12-26 2020-04-28 重庆盟讯电子科技有限公司 一种模组矫正方法
CN111515956A (zh) * 2020-05-13 2020-08-11 中科新松有限公司 杆件及关节柔性的机器人运动学标定方法
CN111515956B (zh) * 2020-05-13 2021-09-03 中科新松有限公司 杆件及关节柔性的机器人运动学标定方法
CN111723536A (zh) * 2020-06-16 2020-09-29 岭南师范学院 一种瓦斯抽采钻机系统的多体动力学分析方法
CN111723536B (zh) * 2020-06-16 2022-10-04 岭南师范学院 一种瓦斯抽采钻机系统的多体动力学分析方法
CN112001087B (zh) * 2020-08-27 2021-10-19 南京航空航天大学 一种旋转关节型工业机器人非线性动力学建模分析方法
CN112001087A (zh) * 2020-08-27 2020-11-27 南京航空航天大学 一种旋转关节型工业机器人非线性动力学建模分析方法
CN112818481A (zh) * 2021-01-19 2021-05-18 中国科学院沈阳自动化研究所 弹性平面约束的细长软体机器人的建模与控制方法
CN112818481B (zh) * 2021-01-19 2023-11-07 中国科学院沈阳自动化研究所 弹性平面约束的细长软体机器人的建模与控制方法
CN113297730A (zh) * 2021-05-13 2021-08-24 广东工业大学 基于自适应模态的柔性多体系统动态响应计算方法和系统
CN113255268A (zh) * 2021-05-21 2021-08-13 北京华大九天科技股份有限公司 一种电路仿真中瞬态分析不收敛的检测及修复方法
CN113255268B (zh) * 2021-05-21 2022-05-24 北京华大九天科技股份有限公司 一种电路仿真中瞬态分析不收敛的检测及修复方法
CN116362151A (zh) * 2023-02-24 2023-06-30 深圳市人工智能与机器人研究院 微螺旋组合马达驱动的微型执行器设计方法及相关设备
CN116362151B (zh) * 2023-02-24 2024-02-06 深圳市人工智能与机器人研究院 微螺旋组合马达驱动的微型执行器设计方法及相关设备

Also Published As

Publication number Publication date
CN109543264B (zh) 2023-01-20

Similar Documents

Publication Publication Date Title
CN109543264A (zh) 一种基于多维度重构校正的柔性多体机器人建模与求解方法
Boyer et al. Dynamics of continuum and soft robots: A strain parameterization based approach
CN110076775B (zh) 一种绳驱动连续型机械臂的三维静力学建模方法
CN104573255B (zh) 一种基于改进多体系统传递矩阵的机械臂建模与求解方法
CN109991847A (zh) 一种柔性多体机器人近似时间最优轨迹规划方法
CN107529630A (zh) 一种空间机器人建立动力学模型的方法
CN109634111A (zh) 一种高速重载机器人动态变形计算方法
Wang et al. A multi-objective approach for the trajectory planning of a 7-DOF serial-parallel hybrid humanoid arm
Nabavi et al. Closed-form dynamic formulation of a general 6-P US robot
SE544631C2 (en) Method, system and computer program for controlling dynamic manipulations by a robot
CN107330185A (zh) 线缆的离散点坐标的获取方法及装置
CN113910238A (zh) 机器人刚度建模、辨识与修正方法及实验系统
CN110909438B (zh) 一种基于动力学模型的轻载关节型并联机器人控制方法
CN105260499A (zh) 一种三维柔性梁系统力学的运动仿真方法及系统
CN110750925A (zh) 柔性机械手力变形模型建模方法及装置
CN109434838A (zh) 线驱动连续机器人内窥操作的协同运动规划方法及系统
Li et al. Simulation and comparison research of Lagrange and Kane dynamics modeling for the 4-DOF modular industrial robot
Leines et al. LQR control of an under actuated planar biped robot
CN108959829B (zh) 基于轴不变量的非理想关节机器人动力学建模方法
CN114888812A (zh) 一种基于刚度性能优化的移动机械臂站位规划方法
CN109117451B (zh) 基于轴不变量的树链机器人动力学建模与解算方法
Rivas et al. Inverse engineering design and construction of an ABS plastic, six DOF robotic arm structure
Kilicaslan et al. Control of constrained spatial three-link flexible manipulators
Amar et al. Dynamics and Control of Tree-Type Manipulator Systems using Exponential Coordinates
Zeng et al. Mobility Characteristics Analysis of a Dual-Continuum-Joint Translator

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
OL01 Intention to license declared
OL01 Intention to license declared