CN109991847A - 一种柔性多体机器人近似时间最优轨迹规划方法 - Google Patents

Abstract

本发明公开了一种柔性多体机器人近似时间最优轨迹规划方法，通过1)分别构建柔性多体机器人的各刚性构件和各柔性构件的数学模型、2)建立约束条件下柔性多体机器人的逆动力学方程、3)采用一维路径坐标描述柔性多体机器人的运动轨迹并建立规划轨迹的目标函数、4)建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型以利用利用路径坐标s得到轨迹规划可行域，进而利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线这四个步骤完成；该方法避免了时间最优方法中存在的至少一个驱动装置处于饱和状态的现象发生，保证驱动装置有能力对系统外界扰动进行补偿和抑制，提高了规划的效率。

Description

一种柔性多体机器人近似时间最优轨迹规划方法

技术领域

本发明涉及柔性多体机器人轨迹规划领域，特别涉及一种柔性多体机器人近似时间最优轨迹规划方法。

背景技术

随着工业对生产效率和降低能耗要求的不断提升，促使工业机器人不断向轻型化和小型化发展。运动轨迹规划是保证工业机器人精确、高效、稳定和健康工作的关键环节。从规划方式上讲，可分为离线轨迹规划和在线轨迹规划，两种方式均需要提前设计运动几何路径，然后利用优化算法对运动轨迹进行优化，同时遵循机构的运动学和动力学约束条件。目前常用的规划方法包含时间最优法、Jerk最优法和时间-能量最优法，其中时间最优算法研究较多。由于约束条件下柔性多体机器人的动力学模型求解十分复杂，因此合理构建轨迹规划策略对于柔性多体机器人工作性能和效率的优化都显得十分重要。

常用的时间最优轨迹规划算法有降阶优化法和智能数值优化算法，前者通过引入路径参数将多维时间优化问题转变为低维度优化问题，后者通过引入智能化的搜索算法或者算子进行求解。然而时间最优法的缺点在于机器人在规划的运动过程中至少有一个驱动装置处于饱和状态，所以无法对系统扰动进行有效的补偿和抑制。

发明内容

本发明的目的是提供一种解决现有约束条件下柔性多体机器人的运动轨迹规划复杂问题的柔性多体机器人近似时间最优轨迹规划方法。

为此，本发明技术方案如下：

一种柔性多体机器人近似时间最优轨迹规划方法，步骤如下：

S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；

S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；

S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；

S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。

进一步地，在步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：

其中，

式(1)中，M(q)为质量矩阵；Q_G(q)为重力广义力向量；Q_E(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φ_q ^T为对Φ_q的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；为广义速度向量；为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；为速度约束方程向量，其为Φ(q,t)对t求全导数；为加速度约束方程向量，其为Φ(q,t)对t求二阶全导数；式(2)中，Φ_q为Φ(q,t)对q求偏导数，Φ_qq为Φ(q,t)对q求二阶偏导数，Φ_t为Φ(q,t)对t求偏导数，Φ_qt为Φ(q,t)依次对q和t求一阶偏导数，Φ_tt为Φ(q,t)对时间t求二阶偏导数。

进一步地，在步骤S3中，采用一维路径坐标s(0≤s≤1)描述柔性多体机器人的运动轨迹，其所建立规划轨迹的目标函数为：

其中，

式(3)中，t_end为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；为伪速度，是一维路径坐标s的导数；为伪加速度，是一维路径坐标的二阶导数；为广义速度向量中驱动关节对应的速度向量；和分别为驱动关节速度向量的下限约束值与上限约束值；为广义加速度向量中驱动关节对应的加速度向量；和分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τ_min和τ_max分别为驱动力向量的下限约束值与上限约束值；为的初始值；为的终止值；为的初始值；为的终止值；Ω为可行域；T为运动总时间；式(4)中，q_ds是驱动关节坐标向量q_d对s求一阶导数；q_dss是驱动关节坐标向量q_d对s求二阶导数。

进一步地，所述步骤S4的具体实施步骤如下：

S401、将路径坐标s的定义域均匀离散为由n个元素组成的序列：{s₀，…s_k，…,s_n}，其中，s₀＝s(0)＝0，s_n＝s(T)＝1，令k＝0，得到路径坐标s、伪速度和伪加速度的当前值s_k、和

S402、根据由步骤S401得到的路径坐标s、伪速度和伪加速度的当前值s_k、和并利用广义-alpha隐式积分算法求解如式(1)所述的逆动力学方程，得到柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度、角加速度，以及各刚性运动杆件及柔性运动杆件的位置状态；

S403、建立柔性多体机器人的瞬态刚性逆动力学模型，利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件：

式(5)中，M_s为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；C_s为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；G_s为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；

将式(5)整理为

式(6)中，m_i为向量M_s中的元素；c_i和g_i分别为C_s和G_s中的元素；l为柔性多体机器人驱动力向量τ的维数；

根据当前已知的路径坐标s即可求出伪速度的最大允许值

S404、建立当前柔性多体机器人的瞬态刚性逆运动学模型，设置路径坐标区间为[s_k,s_k+1]并在其中均匀提取j个采样点，然后根据式(6)计算此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度构成的平面上对[s_k,s_k+1]区间段规划出如下运动轨迹：

式(7)中，系数c_l1和c_l2由端点值(s_k,f(s_k))和(s_k+1,f(s_k+1))确定；

S405、根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中柔性多体机器人的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度、角加速度，并根据式(3)进行校核：若不满足要求，设置比例因子σ∈(0,1)，修正当前五次样条函数末端点的纵坐标为f(s_k+1)＝σf(s_k+1)，返回步骤S404，直至满足校核要求；若满足要求，则令k＝k+1，返回步骤S403，直至k＝n，完成全部轨迹规划。

优选，所述步骤S401中，元素的个数n取10。

优选，所述步骤S404中，采样点j的个数为10。

优选，所述步骤S405中，比例因子σ为0.05。

与现有技术相比，该柔性多体机器人近似时间最优轨迹规划方法使用了近似时间最优轨迹规划方法，避免了时间最优方法中存在的至少一个驱动装置处于饱和状态的现象发生，保证了驱动装置有能力对系统外界扰动进行补偿和抑制；另外，该方法基于柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性运动学模型提出了一种预估-修正规划算法，避免了直接使用柔性多体机器人模型进行求解规划，提高了规划的效率。

附图说明

图1为本发明的实施例中的三自由度3-RRRU柔性多体机器人的结构示意图；

图2为本发明的实施例中的三自由度3-RRRU柔性多体机器人各构件建模时的广义坐标向量示意图；

图3为本发明的实施例中的柔性多体机器人近似时间最优轨迹规划方法的步骤流程图；

图4为本发明的实施例中的三自由度3-RRRU柔性多体机器人使用相对坐标法对驱动关节角进行定义的示意图；

图5为本发明的实施例中的三自由度3-RRRU柔性多体机器人的末端执行器的空间摆线运动轨迹的示意图；

图6为本发明的实施例中的三自由度3-RRRU柔性多体机器人的驱动关节角位移曲线；

图7为本发明的实施例中的三自由度3-RRRU柔性多体机器人的驱动关节角速度曲线；

图8为本发明的实施例中的三自由度3-RRRU柔性多体机器人的驱动关节角加速度曲线；

图9为本发明的实施例中的三自由度3-RRRU柔性多体机器人的驱动关节驱动力曲线。

具体实施方式

下面结合附图及具体实施例，以如图1所示的三自由度3-RRRU柔性多体机器人为例，对本发明的近似时间最优轨迹规划方法做进一步的说明，但下述实施例绝非对本发明有任何限制。

如图1所示，该三自由度3-RRRU柔性多体机器人包括1个静平台、1个动平台、6根刚性杆和3根柔性杆，其中，1根柔性杆和与之相连接的2根串联的刚性杆构成一条支链，每条支链的两端分别与动平台和静平台相连接；柔性杆与刚性杆之间、刚性杆与静平台之间通过刚性转动关节形成活动链接，柔性杆与动平台之间均通过刚性虎克铰形成活动链接；刚性杆与静平台之间通过刚性驱动关节形成活动链接。具体地，序号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所示，针对该三自由度3-RRRU柔性多体机器人的近似时间最优轨迹规划方法的具体步骤如下：

S1、构建柔性多体机器人各构件的数学模型并设置参数：

获取参与运动的构件的质量、尺寸和质心分布情况，不参与运动的构件的尺寸，以及柔性构件的相关参数；具体为获取该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；由于所有构件的质量分布都是均匀的，其质心均位于构件的几何中心处。

将该柔性多体机器人的末端执行器设置在动平台的几何中心点处，即如图1和图2所示的动平台上的P点；对该柔性多体机器人中的静平台、动平台和六根刚性杆使用自然坐标法进行建模，而三个柔性杆则使用绝对节点坐标法进行建模；在上述建模过程中，选取静平台的中心点作为所建立的坐标系的原点。

S2、根据步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；根据步骤S1所设置的各刚性构件和柔性构件的几何参数和材料物理特性建立柔性多体机器人的完整约束方程，包括运动学约束方程和动力学约束方程；接着，基于拉格朗日方程，组建柔性多体机器人的广义坐标向量、质量矩阵、有势广义力向量、非有势广义力向量和约束方程的雅克比矩阵，进而建立柔性多体机器人的运动学约束条件，得到约束条件下柔性多体机器人的逆动力学方程。

该步骤S2的具体实施过程如下：

如图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依次表示6根刚性杆、3根柔性杆和1个动平台；

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

接着，基于拉格朗日方程，组建该柔性多体机器人的广义坐标向量为q＝[q₁ ^T,e^1T,q₂ ^T,e^2T,q₃ ^T,e^3T]^T、质量矩阵为M、重力有势广义力向量为Q_G，弹性有势广义力向量为Q_E，非有势广义力向量为Q，以及约束方程的雅克比矩阵，进而得到约束条件下柔性多体机器人的逆动力学方程为；

其中，

式(1)中，M(q)为质量矩阵；Q_G(q)为重力广义力向量；Q_E(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φ_q ^T为对Φ_q的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；为广义速度向量；为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；为速度约束方程向量，其为Φ(q,t)对t求全导数；为加速度约束方程向量，其为Φ(q,t)对t求二阶全导数；式(2)中，Φ_q为Φ(q,t)对q求偏导数，Φ_qq为Φ(q,t)对q求二阶偏导数，Φ_t为Φ(q,t)对t求偏导数，Φ_qt为Φ(q,t)依次对q和t求一阶偏导数，Φ_tt为Φ(q,t)对时间t求二阶偏导数。

步骤S3、利用一维路径坐标s(0≤s≤1)描述该柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数为：

其中，

式(3)中，t_end为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；为伪速度，是一维路径坐标s的导数；为伪加速度，是一维路径坐标的二阶导数；为广义速度向量中驱动关节对应的速度向量；和分别为驱动关节速度向量的下限约束值与上限约束值；为广义加速度向量中驱动关节对应的加速度向量；和分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τ_min和τ_max分别为驱动力向量的下限约束值与上限约束值；为的初始值；为的终止值；为的初始值；为的终止值；Ω为可行域；T为运动总时间；式(4)中，q_ds是驱动关节坐标向量q_d对s求一阶导数；q_dss是驱动关节坐标向量q_d对s求二阶导数。

步骤S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，根据约束条件计算约束曲线，得到轨迹规划可行域，再利用样条函数在可行域内规划出该柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线；具体实施步骤如下：

步骤S401：将路径坐标s的定义域均匀离散为由10个元素组成的序列{s₀，…s_k，…,s_n}，其中，s₀＝s(0)＝0，s_n＝s(T)＝1，令k＝0，得到路径坐标s、伪速度和伪加速度的当前值s_k、和

步骤S402：根据路径坐标s、伪速度和伪加速度的当前值s_k、和利用广义-alpha隐式积分算法求解上述式(1)的逆动力学方程，得到该柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度和角加速度，以及各刚性运动杆件及柔性运动杆件的位置状态；

步骤S403：建立柔性多体机器人的瞬态刚性逆动力学模型，即利用路径坐标s及其导数项将动力学约束条件和运动学约束条件描述为：

式(5)中，M_s为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；C_s为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；G_s为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；

进一步，为了便于求出伪速度约束曲线，将上述式(5)整理为：

式(6)中，m_i为向量M_s中的元素；c_i和g_i分别为C_s和G_s中的元素；l为柔性多体机器人驱动力向量τ的维数；

将当前已知的路径坐标s代入式(6)中，求出伪速度的最大允许值

步骤S404：建立当前柔性多体机器人的瞬态刚性逆运动学模型，即设置路径坐标区间为[s_k,s_k+1]，并在其中均匀提取10个采样点，根据式(6)计算出此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度构成的平面上对[s_k,s_k+1]区间段规划出如下运动轨迹：

式(7)中，系数c_l1和c_l2由端点值(s_k,f(s_k))和(s_k+1,f(s_k+1))确定；

步骤S405：根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中如式(1)所示的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度和角加速度；

将上述得到的计算结果带入式(3)中，对计算结果是否满足式(3)中各项参数的设定范围进行校核：

I、若不满足要求，则设置比例因子σ＝0.05，并修正当前步骤S404中的五次样条函数的末端点的纵坐标为f(s_k+1)＝σf(s_k+1)，返回步骤S404，直至满足校核要求；

II、若满足要求，则令k＝k+1，返回步骤S403，直至k＝n，完成全部轨迹规划，得到该3-RRRU柔性多体机器人的最优运行轨迹。

以下为该3-RRRU柔性多体机器人近似时间最优轨迹规划仿真算例：

第1、第4和第7构件的运动状态可由点A_0i(i＝1,2,3)处的驱动关节角θ_1i(i＝1,2,3)的运动状态来描述，其定义方式如图4所示；φ_i(i＝1,2,3)为坐标轴X_G绕Z_G轴旋转到与A_0i处坐标轴x_0i(i＝1,2,3)共线时的角度；

如图5所示为已知3-RRRU柔性多体机器人末端执行器的空间摆线运动轨迹，其表达式为：

式(8)中，0≤s≤1，t∈[0,T]；

在本次轨迹规划过程中，该3-RRRU柔性多体机器人的约束条件如表1所示。

表1：

进一步地，为验证规划方法的有效性和正确性，采用上述轨迹规划方法对该3-RRRU柔性多体机器人进行近似时间最优轨迹，求出第1、第4和第7构件的运动状态，它们由点A_0i(i＝1,2,3)处驱动关节角θ_1i(i＝1,2,3)的运动状态来描述，进而求出如图6～图9所示的驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线；空间摆线的规划出的运动总时间T为1.608秒。

通过对比表2，以及图7～图9中驱动关节的规划运动曲线可知，该方法得到的驱动关节的角速度曲线未超过其约束值和得到的驱动关节的角加速度曲线未超过其约束值和得到的驱动关节的驱动力曲线未超过其约束值τ_min和τ_max，而且第三支链驱动关节的驱动力曲线非常接近τ_min。

综上所述，该规划方法能够充分利用了驱动力向量约束条件的最大允许值，缩短了运行时间，提高了柔性多体机器人的工作效率，而且规划出的驱动关节的运动曲线比较平滑，没有出现大幅度振荡的现象，实现了柔性多体机器人的平稳运动，验证了本算法的有效性。

Claims (7)

1.一种柔性多体机器人近似时间最优轨迹规划方法，其特征在于，步骤如下：

S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；

S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；

S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；

S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。

2.根据权利要求1所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，在所述步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：

其中，

式(1)中，M(q)为质量矩阵；Q_G(q)为重力广义力向量；Q_E(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φ_q ^T为对Φ_q的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；为广义速度向量；为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；为速度约束方程向量，其为Φ(q,t)对t求全导数；为加速度约束方程向量，其为Φ(q,t)对t求二阶全导数；式(2)中，Φ_q为Φ(q,t)对q求偏导数，Φ_qq为Φ(q,t)对q求二阶偏导数，Φ_t为Φ(q,t)对t求偏导数，Φ_qt为Φ(q,t)依次对q和t求一阶偏导数，Φ_tt为Φ(q,t)对时间t求二阶偏导数。

3.根据权利要求1所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，在步骤S3中，采用一维路径坐标s(0≤s≤1)描述柔性多体机器人的运动轨迹，其所建立规划轨迹的目标函数为：

其中，

式(3)中，t_end为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；为伪速度，是一维路径坐标s的导数；为伪加速度，是一维路径坐标的二阶导数；为广义速度向量中驱动关节对应的速度向量；和分别为驱动关节速度向量的下限约束值与上限约束值；为广义加速度向量中驱动关节对应的加速度向量；和分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τ_min和τ_max分别为驱动力向量的下限约束值与上限约束值；为的初始值；为的终止值；为的初始值；为的终止值；Ω为可行域；T为运动总时间；式(4)中，q_ds是驱动关节坐标向量q_d对s求一阶导数；q_dss是驱动关节坐标向量q_d对s求二阶导数。

4.根据权利要求1所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S4的具体实施步骤如下：

S401、将路径坐标s的定义域均匀离散为由n个元素组成的序列：{s₀，…s_k，…,s_n}，其中，s₀＝s(0)＝0，s_n＝s(T)＝1，令k＝0，得到路径坐标s、伪速度和伪加速度的当前值s_k、和

S402、根据由步骤S401得到的路径坐标s、伪速度和伪加速度的当前值s_k、和并利用广义-alpha隐式积分算法求解如式(1)所述的逆动力学方程，得到柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度、角加速度，以及各刚性运动杆件及各柔性运动杆件的位置状态；

S403、建立柔性多体机器人的瞬态刚性逆动力学模型，利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件：

式(5)中，M_s为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；C_s为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；G_s为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；

将式(5)整理为

式(6)中，m_i为向量M_s中的元素；c_i和g_i分别为C_s和G_s中的元素；l为柔性多体机器人驱动力向量τ的维数；

根据当前已知的路径坐标s即可求出伪速度的最大允许值

S404、建立当前柔性多体机器人的瞬态刚性逆运动学模型，设置路径坐标区间为[s_k,s_k+1]并在其中均匀提取j个采样点，然后根据式(6)计算此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度构成的平面上对[s_k,s_k+1]区间段规划出如下运动轨迹：

式(7)中，系数c_l1和c_l2由端点值(s_k,f(s_k))和(s_k+1,f(s_k+1))确定；

S405、根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中柔性多体机器人的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度、角加速度，并根据式(3)进行校核：若不满足要求，设置比例因子σ∈(0,1)，修正当前五次样条函数末端点的纵坐标为f(s_k+1)＝σf(s_k+1)，返回步骤S404，直至满足校核要求；若满足要求，则令k＝k+1，返回步骤S403，直至k＝n，完成全部轨迹规划。

5.根据权利要求4所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S401中，元素的个数n取10。

6.根据权利要求4所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S404中，采样点j的个数为10。

7.根据权利要求4所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S405中，比例因子σ为0.05。