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

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为广义坐标向量，Q_G(q)为柔性多体机器人重力广义力向量，Q_E(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时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、q₀、

和

其中，拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

同时，设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：

在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；e_c、e_cv、e_ca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10^-15，10^-4]；e_d为驻留方程向量D_n的迭代求解精度，其取值范围为[10^-15，10^-5]；D_n包含两种情况：当柔性多体机器人处于静力平衡状态时，D_n由式(5)表示，而当柔性多体机器人处于动力平衡状态时，D_n由式(6)表示：

S402、利用牛顿法递推公式，如下式(7)所示，迭代计算柔性多体机器人的待求状态参数：

在式(7)中，n为迭代次数，初始值为0，每迭代一次n自动加1，D_n为柔性多体机器人驻留方程向量，其根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，H为迭代变换矩阵，其包含两个分块矩阵H₁和H₂，对应表达式为：

在式(8)中，h为积分步长，[]^-1表示对矩阵求逆，[]^-T表示对矩阵求逆和转置；

为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时

由式(9)表示，当柔性多体机器人处于动力平衡状态时

由式(10)表示：

在式(9)和式(10)中，

为向量函数对广义加速度向量的偏导数，而上式(7)中的迭代参数为α_f、α_m和β采用如下广义-alpha隐式积分算法的参数：

在式(11)中，ρ为算法谱半径，ρ∈[0，1]；

根据式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S403、利用式(7)再迭代计算N-1次待求状态参数，其中，N为正整数且N≤50；记录第N次迭代后Φ_n和D_n的值，并基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和

进行重构计算：

在式(12)中，迭代次数n的初始值为N，N的取值优选为4；t_n和t_n+1分别为积分下限和上限，且t_n+1＝t_n+h；

基于Adams法对q_n+1和

进行修正并重构计算

和λ_n+1：

按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求：

在式(14)、式(15)和式(16)中，

和

分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；

为向量函数对广义速度向量的偏导数，而矩阵

和

则用Broyden拟牛顿法进行计算；

||D_n||满足收敛条件要求，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代得到t时刻柔性多体机器人状态参数，并求出柔性多体机器人末端执行器的状态参数。

进一步的，当步骤S4采用正动力学求解方法得到t时刻的柔性多体机器人状态参数，步骤S5的具体步骤为：

S501、将t时刻求出的迭代解作为迭代初始值；

S502、根据驱动构件在t+h时刻已知的状态参数，按照上述步骤S4中正动力学求解方法的求解步骤进行计算，最终通过迭代得到t+h时刻柔性多体机器人的状态参数，并求出柔性多体机器人末端执行器的状态参数。

进一步地，在步骤S4中，采用逆动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：

S401、与正动力学求解方法的步骤S401相同，根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、τ₀、q₀、

和

其中，拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

驱动力向量满足条件τ_min≤τ≤τ_max；并按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件；

S402、利用如下式(17)所示的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：

在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1，柔性多体机器人的几何约束方程向量Φ(q_n，t_n)包含Φ^b(q_n,t_n)和Φ^a(q_n,t_n)两个分量，它们分别由末端执行器和运动构件的状态参数决定，D_n为柔性多体机器人驻留方程向量，根据柔性多体机器人的状态不同可分别由式(5)和式(6)表示；迭代变换矩阵H包含三个分块矩阵H₁、H₂和H₃，其表达式为：

利用式(17)、式(18)以及式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λ_n、τ_n、q_n、

就是t时刻的柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S403、利用式(17)再迭代计算N-1次待求状态参数，N为正整数且N≤50，其中，N的取值优选为4；记录第N次迭代后Φ_n和D_n的值，并基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、

和λ进行重构计算，然后利用下式(19)对τ进行重构计算：

τ_n+1＝H₂[Φ^a(q_n+1,t_n+1) Φ^b(q_n+1,t_n+1) D_n+1]^T 式(19)

按照式(4)判断所求迭代结果是否满足迭代收敛条件：若满足，则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求；若||D_n||和τ_n满足收敛条件要求，则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行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中柔性多体机器人的正动力学求解方法中动平台与Z_G轴之间夹角的示意图；

图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个运动副，其中点A_0i、A_1i、A_2i(i＝1，2，3)处为刚性转动关节，点A_0i(i＝1，2，3)处转动关节为驱动关节，点A_4i(i＝1，2，3)处为刚性虎克铰；

获取参与运动的构件的质量、尺寸和质心分布情况，不参与运动的构件的尺寸，以及柔性构件的相关参数；具体为获取该3-RRRU柔性多体机器人的刚性杆、柔性杆和动平台的质量、刚性杆和柔性杆的杆长，动平台和静平台的外接圆半径，柔性杆杨氏弹性模量、泊松比，以及每个构件的质心分布情况。

其中，静平台外接圆半径R＝0.175m，动平台外接圆半径r＝0.06m，动平台质量m₁₀＝0.3kg各刚性杆长度l₁＝l₄＝l₇＝0.375m，l₂＝l₅＝l₈＝0.09m，各刚性杆质量m₁＝m₄＝m₇＝2kg，m₂＝m₅＝m₈＝0.3kg，各柔性杆在未变形状态下的原始长度l₃＝l₆＝l₉＝0.855m，各柔性杆质量m₃＝m₆＝m₉＝1.2kg，各柔性杆件的杨氏弹性模量E＝6.9×10⁸Pa，泊松比ν＝0.3。由于所有构件的质量分布都是均匀的，其质心均位于构件的几何中心处。

将柔性多体机器人的末端执行器设置在动平台的几何中心点处，即动平台上的P点；对该柔性多体机器人中的静平台、动平台和六根刚性杆使用自然坐标法进行建模，而三个柔性杆则使用绝对节点坐标法进行建模；在建模过程中，选取静平台的中心点作为建立坐标系的原点。S2、根据步骤S1中，对该3-RRRU柔性多体机器人的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；具体地，

如图2所示，每个支链中的两根刚性杆分别用广义坐标向量q₁＝(x₁，z₁)^T、q₂＝(x₄，z₄)^T和q₃＝(x₇，z₇)^T表示，其中，(x₁，z₁)为A₁₁点的全局坐标，(x₄，z₄)为A₁₂点的全局坐标，(x₇，z₇)为A₁₃点的全局坐标；每个支链中的柔性杆分别用广义坐标向量

和

表示；动平台用广义坐标向量

表示，其中，[]^T表示对列向向量或矩阵的转置；

各刚性杆、各柔性杆和动平台的质量矩阵用M_i(i＝1，2，…，10)表示；对应地，作用在上述各构件上的重力有势广义力向量用Q_Gi(i＝1，2，…，10)表示；作用在上述各构件上的非有势广义力向量用Q_i(i＝1，2，…，10)表示；三个柔性杆上的弹性有势广义力向量用Q_Ei(i＝1，2，3)表示；

其中，根据该装置结构特点，只考虑序号为第1、第4和第7构件上所受的驱动力，各构件所受的摩擦力和阻尼力在此不考虑；在上述表达式中，i＝1，2，…，10依次表示9个杆和1个动平台；

根据各刚性杆、柔性杆、转动副和动平台的运动学约束关系建立柔性多体机器人的运动学约束条件和动力学约束条件；具体地，运动学约束条件包括：几何约束方程向量Φ(q，t)、速度约束方程向量

和加速度约束方程向量

其中，每个向量均涉及21个分量，共计63个维度，即Φ(q，t)∈R²¹，

动力学约束条件为τ_min≤τ≤τ_max，该条件涉及3个约束方程，因此，驱动力向量τ涉及3个维度，即τ∈R³，上述运动学约束条件和动力学约束条件构成了该柔性多体机器人完整约束条件。

S3、基于拉格朗日方程，利用步骤S2得到的结果组建柔性并联多体机器人的广义坐标向量为q＝[q₁ ^T，e₁ ^T，q₂ ^T，e₂ ^T，q₃ ^T，e₃ ^T]^T，q∈R⁷⁸，质量矩阵为M，重力有势广义力向量为Q_G，弹性有势广义力向量为Q_E，非有势广义力向量为Q，约束方程的雅克比矩阵Φ_q(q，t)，得到完整约束条件下柔性多体机器人的静力学方程，如式(1)所示，

其中，

在式(1)中Q_G(q)∈R⁷⁸为重力广义力向量，其维度与广义坐标向量q的维度相同；Q_E(q)∈R⁷⁸为弹性广义力向量，其维度与广义坐标向量q的维度相同；

为非有势广义力向量，其维度与广义坐标向量q的维度相同；[]^T表示对矩阵进行转置；λ为拉格朗日乘子向量，其维度与几何约束方程向量Φ(q，t)的维度相同；q∈R⁷⁸为广义坐标向量，

为广义速度向量，

为广义加速度向量；τ∈R³为驱动力向量，包含了作用在点A_0i(i＝1，2，3)处的驱动关节的驱动力，其下限约束值与上限约束值分别为τ_min和τ_max，τ_max为三个驱动关节上驱动电机的最大驱动力向量，τ_min为三个驱动关节上驱动电机的最小驱动力向量；t为时间，Φ(q，t)为几何约束方程向量，其由21个几何约束条件组成，合计共包含六个刚性杆件的长度约束条件、A_2i(i＝1，2，3)处九个刚性转动关节轴线约束条件、A_4i(i＝1，2，3)处三个刚性虎克铰轴线正交约束条件和动平台A_4i(i＝1，2，3)处三个端点长度约束条件；在式(2)中，

为速度约束方程向量，是Φ(q，t)对t求全导数，包含速度约束条件，

为加速度约束方程向量，是Φ(q，t)对t求二阶全导数，包含加速度约束条件。

进而，得到完整约束条件下柔性多体机器人的动力学方程为：

在式(3)中，M(q)∈R^78×78为柔性多体机器人质量矩阵，其维度与广义坐标向量q的维度相匹配，由步骤S1中刚性构件和柔性构件分别构建的数学模型来确定。

步骤S4、预先设置柔性多体机器人的运动轨迹，根据柔性多体机器人在时刻t的已知状态参数，采用正动力学求解方法，并利用多维度重构校正法对完整约束条件下3-RRRU柔性多体机器人的力学方程进行迭代求解，以得到t时刻柔性多体机器人状态参数。

如图4所示，该正动力学求解方法的具体步骤为：

S401、根据驱动构件在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、q₀、

和

其中拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：

在式(4)中，||·||为对向量求模，n为迭代次数，初始值为0，几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度e_c、e_cv和e_ca均设为10^-8，驻留方程向量D_n的迭代求解精度e_d＝10^-6，驱动力向量下限约束值与上限约束值分别为τ_min＝[-20，-20，-20]^T，τ_max＝[20，20，20]^T，D_n包含两种情况：当柔性多体机器人处于静力平衡状态时D_n由式(5)表示，当柔性多体机器人处于动力平衡状态时D_n由式(6)表示：

S402、利用如下牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：

在式(8)中，n为迭代次数，初始值为0，每迭代一次n自动加1，Φ(q_n，t_n)为几何约束方程向量，D_n为柔性多体机器人驻留方程向量，根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，迭代变换矩阵H包含两个分块矩阵H₁和H₂，其表达式为：

在式(8)中，积分步长h＝0.002，()^-1为对矩阵求逆，()^-T为对矩阵求逆和转置，

为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时

由式(9)表示，当柔性多体机器人处于动力平衡状态时

由式(10)表示：

在式(9)和式(10)中，

为向量函数对广义加速度向量的偏导数，在式(7)中，迭代参数α_f、α_m和β采用如下广义-alpha隐式积分算法的参数：

在式(11)中，ρ为算法谱半径，ρ＝0.4；

按照上式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S403、设定N＝4，利用式(7)再迭代计算N-1次待求状态参数，记录第N次迭代后Φ_n和D_n的值，基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和

进行重构计算：

在式(12)中，迭代次数n的初始值为N，积分下限和上限分别t_n和t_n+1，且t_n+1＝t_n+h；

基于Adams法对q_n+1和

进行修正并重构计算

和λ_n+1：

按照上式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求：

在上式(14)～式(16)中，

和

分别为此刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量，三者各涉及15个分量，共计15个维度，，其中，

合计共包含六个刚性杆件的长度约束条件、A_4i(i＝1，2，3)点到给定平面的三个距离约束条件、A_4i(i＝1，2，3)处三个刚性虎克铰轴线正交约束条件和动平台A_4i(i＝1，2，3)处三个端点长度约束条件；在式(15)中，

为向量函数对广义速度向量的偏导数，矩阵

和

用Broyden拟牛顿法进行计算；

若||D_n||满足收敛条件要求，则λ_n、q_n、

就是t时刻系统的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤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构件的运动状态可由点A_0i(i＝1，2，3)处驱动关节角θ_1i(i＝1，2，3)的运动状态来描述，θ_1i的定义方式如图3所示，φ_i(i＝1，2，3)为坐标轴X_G绕Z_G轴旋转到与A_0i处坐标轴x_0i(i＝1，2，3)共线时的角度。已知三个驱动关节角的位移、速度和加速度如图5至图7所示，三个驱动关节所受的驱动力矩如图8所示，为验证算法的有效性和正确性，采用说明书中的方法对3-RRRU柔性多体机器人的正动力学模型进行数值迭代求解，求出末端执行器的运动轨迹和加速度如图9和图10所示，动平台与Z_G轴的夹角如图11所示，图12为迭代过程中动力学方程误差，其迭代求解精度满足e_d的设定值10^-6，图13为迭代过程中约束方程误差，它将所有几何约束方程向量、速度约束方程向量和加速度约束方程向量的模进行求和，其迭代精度均满足e_c、e_cv和e_ca的设定值10^-8，全部求解过程平稳并且收敛。

图14是使用广义-alpha方法对3-RRRU柔性多体机器人的正动力学模型迭代求解时约束方程的误差，通过对比发现，该误差值逐渐增加，超过了约束方程迭代精度的设定值，无法满足完整约束条件下柔性多体机器人模型求解的迭代收敛条件。

实施例2

一种基于多维度重构校正的柔性多体机器人建模与求解方法，其同样针对如图1所示的三自由度3-RRRU柔性多体机器人的特性进行描述。

与实施例1不同之处在于，在本实施例中的步骤S4中，根据柔性多体机器人在时刻t的已知状态参数，采用逆动力学求解方法，利用多维度重构校正法对完整约束条件下3-RRRU柔性多体机器人的力学方程进行迭代求解，以得到t时刻柔性多体机器人的状态参数。

如图15所示，该逆动力学求解方法的具体步骤为：

S401、根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、τ₀、q₀、

和

其中，拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

驱动力向量满足条件τ_min≤τ≤τ_max；按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件，此处条件设置方式与正动力学求解方法中步骤S401的设置方式使一致的；

S402、利用下式(17)的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：

在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1；由于驱动力向量未知，几何约束方程向量Φ(q_n，t_n)应包含Φ^a(q_n,t_n)∈R²¹和Φ^b(q_n,t_n)∈R³两组分量；其中，Φ^a(q_n,t_n)是正动力学模型求解中使用的21个几何约束条件，Φ^b(q_n,t_n)则由已知的末端执行器三个空间坐标值决定，D_n为柔性多体机器人的驻留方程向量，根据状态不同可分别由式(5)和式(6)表示，迭代变换矩阵H包含三个分块矩阵H₁、H₂和H₃，其表达式为：

利用式(17)、式(18)和式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S403、设定N＝4，利用式(17)再迭代计算N-1次待求状态参数，，记录第N次迭代后Φ_n和D_n的值，基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、

和λ进行重构计算，利用下式(19)对τ进行重构计算：

τ_n+1＝H₂[Φ^a(q_n+1,t_n+1) Φ^b(q_n+1,t_n+1) D_n+1]^T 式(19)

按照式(4)判断所求迭代结果是否满足迭代收敛条件，若满足则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求：若||D_n||和τ_n满足收敛条件要求，则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行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构件的运动状态，它们可由点A_0i(i＝1，2，3)处驱动关节角θ_1i(i＝1，2，3)的运动状态来描述，求出的驱动关节角的位移、速度和加速度如图19和图21所示，三个驱动关节所受的驱动力矩如图22所示，所有驱动力矩均满足驱动力向量τ的上限约束值τ_max＝[20，20，20]和下限约束值τ_min＝[-20，-20，-20]^T，图23为迭代过程中动力学方程误差，其迭代求解精度满足e_d的设定值10^-6，图24为迭代过程中约束方程误差，它将所有几何约束方程向量、速度约束方程向量和加速度约束方程向量的模进行求和，其迭代精度均满足e_c、e_cv和e_ca的设定值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为广义坐标向量，Q_G(q)为柔性多体机器人重力广义力向量，Q_E(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时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、q₀、

和

其中，拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

同时，设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：

在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；e_c、e_cv、e_ca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10^-15，10^-4]；e_d为驻留方程向量D_n的迭代求解精度，其取值范围为[10^-15，10^-5]；D_n包含两种情况：当柔性多体机器人处于静力平衡状态时，D_n由式(5)表示，而当柔性多体机器人处于动力平衡状态时，D_n由式(6)表示：

S402、利用牛顿法递推公式，如下式(7)所示，迭代计算柔性多体机器人的待求状态参数：

在式(7)中，n为迭代次数，初始值为0，每迭代一次n自动加1，D_n为柔性多体机器人驻留方程向量，其根据柔性多体机器人状态不同可分别由式(5)和式(6)表示，H为迭代变换矩阵，其包含两个分块矩阵H₁和H₂，对应表达式为：

在式(8)中，h为积分步长，[]^-1表示对矩阵求逆，[]^-T表示对矩阵求逆和转置；

为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时

由式(9)表示，当柔性多体机器人处于动力平衡状态时

由式(10)表示：

在式(9)和式(10)中，

为向量函数对广义加速度向量的偏导数，而上式(7)中的迭代参数为α_f、α_m和β采用如下广义-alpha隐式积分算法的参数：

在式(11)中，ρ为算法谱半径，ρ∈[0，1]；

根据式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S403、利用式(7)再迭代计算N-1次待求状态参数，其中，N为正整数且N≤50；记录第N次迭代后Φ_n和D_n的值，并基于拉格朗日多项式和Adams法利用待求状态参数的迭代初值和前N次迭代结果对q和

进行重构计算：

在式(12)中，迭代次数n的初始值为N；t_n和t_n+1分别为积分下限和上限，且t_n+1＝t_n+h；

基于Adams法对q_n+1和

进行修正并重构计算

和λ_n+1：

按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求：

在式(14)、式(15)和式(16)中，

和

分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；

为向量函数对广义速度向量的偏导数，而矩阵

和

则用Broyden拟牛顿法进行计算；

若||D_n||满足收敛条件要求，则λ_n、q_n、

就是t时刻的柔性多体机器人状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人末端执行器的状态参数；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、q_n、

设置为迭代初值，重复执行步骤S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代得到t时刻柔性多体机器人状态参数，并求出柔性多体机器人末端执行器的状态参数。

4.根据权利要求1所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，在所述步骤S4中，采用逆动力学求解方法得到t时刻柔性多体机器人状态参数的具体方法为：

S401、根据末端执行器在t时刻已知的状态参数，设置待求状态参数拉格朗日乘子向量、驱动力向量、广义坐标向量、广义速度向量和广义加速度向量的迭代初值分别为λ₀、τ₀、q₀、

和

其中，拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

驱动力向量满足条件τ_min≤τ≤τ_max；并按照式(4)设置完整约束条件下柔性多体机器人模型求解的迭代收敛条件为：

在式(4)中，||·||表示对向量求模；n为迭代次数，其初始值为0；e_c、e_cv、e_ca分别为几何约束方程向量、速度约束方程向量和加速度约束方程向量的迭代求解精度，它们的取值范围为[10^-15，10^-4]；e_d为驻留方程向量D_n的迭代求解精度，其取值范围为[10^-15，10^-5]；D_n包含两种情况：当柔性多体机器人处于静力平衡状态时，D_n由式(5)表示，而当柔性多体机器人处于动力平衡状态时，D_n由式(6)表示：

S402、利用式(17)的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数：

在式(17)中，n为迭代次数，初始值为0，每迭代一次n自动加1，柔性多体机器人的几何约束方程向量Φ(q_n，t_n)包含Φ^b(q_n,t_n)和Φ^a(q_n,t_n)两个分量，它们分别由末端执行器和运动构件的状态参数决定，D_n为柔性多体机器人驻留方程向量，根据柔性多体机器人的状态不同可分别由式(5)和式(6)表示；迭代变换矩阵H包含三个分块矩阵H₁、H₂和H₃，其表达式为：

利用式(17)、式(18)以及式(9)～式(11)描述的牛顿法递推公式迭代计算柔性多体机器人的待求状态参数，

其中，

为柔性多体机器人传递矩阵，当柔性多体机器人处于静力平衡状态时

由式(9)表示，当柔性多体机器人处于动力平衡状态时

由式(10)表示：在式(9)和式(10)中，

为向量函数对广义加速度向量的偏导数，而上式(17)中的迭代参数为α_f、α_m和β采用如下广义-alpha隐式积分算法的参数：

在式(11)中，ρ为算法谱半径，ρ∈[0，1]；

按照式(4)判断得到的迭代结果是否满足迭代收敛条件，若满足则λ_n、τ_n、q_n、

就是t时刻的柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S403、利用式(17)再迭代计算N-1次待求状态参数，N为正整数且N≤50，其中，记录第N次迭代后Φ_n和D_n的值，并基于拉格朗日多项式和Adams法利用式(12)和式(13)对q、

和λ进行重构计算，然后利用下式(19)对τ进行重构计算：

τ_n+1＝H₂[Φ^a(q_n+1,t_n+1) Φ^b(q_n+1,t_n+1) D_n+1]^T 式(19)

其中，在式(12)中，迭代次数n的初始值为N；t_n和t_n+1分别为积分下限和上限，且t_n+1＝t_n+h；按照式(4)判断所求迭代结果是否满足迭代收敛条件：若满足，则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的几何约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

S404、当收敛条件中||Φ(q_n，t_n)||、

和

不满足求解精度要求时，分别使用式(14)、式(15)和式(16)对q_n、

和

进行校正直到它们全部满足求解精度要求；若||D_n||和τ_n满足收敛条件要求，则λ_n、τ_n、q_n、

就是t时刻柔性多体机器人的状态参数，并根据柔性多体机器人的约束方程、速度约束方程和加速度约束方程求出柔性多体机器人驱动构件的状态参数；

在式(14)、式(15)和式(16)中，

和

分别为当前时刻柔性多体机器人的瞬时几何约束方程向量、瞬时速度约束方程向量和瞬时加速度约束方程向量；

为向量函数对广义速度向量的偏导数，而矩阵

和

则用Broyden拟牛顿法进行计算；

S405、若相邻两次迭代结果求出的||D_n||值是逐渐递减的，将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行S403～S404，直至迭代结果满足迭代收敛条件；若相邻两次迭代结果求出的||D_n||值是逐渐递增的，则将当前状态参数λ_n、τ_n、q_n、

设置为迭代初值，重复执行S402至S404，直至迭代结果满足迭代收敛条件；最终通过迭代计算得到t时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。

5.根据权利要求3所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，设置拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝0、广义速度向量

迭代初值满足条件

广义加速度向量的迭代初值满足条件

6.根据权利要求4所述的基于多维度重构校正的柔性多体机器人建模与求解方法，其特征在于，设置拉格朗日乘子向量迭代初值为λ₀＝0，广义坐标向量q₀迭代初值满足条件Q_E(q₀)＝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时刻柔性多体机器人的状态参数，并求出柔性多体机器人驱动构件的状态参数。

Images