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

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

Info

Publication number
CN109991847B
CN109991847B CN201910026172.9A CN201910026172A CN109991847B CN 109991847 B CN109991847 B CN 109991847B CN 201910026172 A CN201910026172 A CN 201910026172A CN 109991847 B CN109991847 B CN 109991847B
Authority
CN
China
Prior art keywords
flexible multi
vector
body robot
flexible
rigid
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.)
Active
Application number
CN201910026172.9A
Other languages
English (en)
Other versions
CN109991847A (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 CN201910026172.9A priority Critical patent/CN109991847B/zh
Publication of CN109991847A publication Critical patent/CN109991847A/zh
Application granted granted Critical
Publication of CN109991847B publication Critical patent/CN109991847B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种柔性多体机器人近似时间最优轨迹规划方法
技术领域
本发明涉及柔性多体机器人轨迹规划领域，特别涉及一种柔性多体机器人近似时间最优轨迹规划方法。
背景技术
随着工业对生产效率和降低能耗要求的不断提升，促使工业机器人不断向轻型化和小型化发展。运动轨迹规划是保证工业机器人精确、高效、稳定和健康工作的关键环节。从规划方式上讲，可分为离线轨迹规划和在线轨迹规划，两种方式均需要提前设计运动几何路径，然后利用优化算法对运动轨迹进行优化，同时遵循机构的运动学和动力学约束条件。目前常用的规划方法包含时间最优法、Jerk最优法和时间-能量最优法，其中时间最优算法研究较多。由于约束条件下柔性多体机器人的动力学模型求解十分复杂，因此合理构建轨迹规划策略对于柔性多体机器人工作性能和效率的优化都显得十分重要。
常用的时间最优轨迹规划算法有降阶优化法和智能数值优化算法，前者通过引入路径参数将多维时间优化问题转变为低维度优化问题，后者通过引入智能化的搜索算法或者算子进行求解。然而时间最优法的缺点在于机器人在规划的运动过程中至少有一个驱动装置处于饱和状态，所以无法对系统扰动进行有效的补偿和抑制。
发明内容
本发明的目的是提供一种解决现有约束条件下柔性多体机器人的运动轨迹规划复杂问题的柔性多体机器人近似时间最优轨迹规划方法。
为此，本发明技术方案如下：
一种柔性多体机器人近似时间最优轨迹规划方法，步骤如下：
S1、利用自然坐标法或相对坐标法构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；
S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；
S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；
S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。
进一步地，在步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：
Figure GDA0003529519130000021
其中，
Figure GDA0003529519130000022
式(1)中，M(q)为质量矩阵；QG(q)为重力广义力向量；QE(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φq T为对Φq的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；
Figure GDA0003529519130000023
为广义速度向量；
Figure GDA0003529519130000024
为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；
Figure GDA0003529519130000025
为速度约束方程向量，其为Φ(q,t)对t求全导数；
Figure GDA0003529519130000026
为加速度约束方程向量，其为Φ(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)描述柔性多体机器人的运动轨迹，其所建立规划轨迹的目标函数为：
Figure GDA0003529519130000031
其中，
Figure GDA0003529519130000032
式(3)中，tend为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；
Figure GDA0003529519130000033
为伪速度，是一维路径坐标s的导数；
Figure GDA0003529519130000034
为伪加速度，是一维路径坐标的二阶导数；
Figure GDA0003529519130000035
为广义速度向量中驱动关节对应的速度向量；
Figure GDA0003529519130000036
和
Figure GDA0003529519130000037
分别为驱动关节速度向量的下限约束值与上限约束值；
Figure GDA0003529519130000038
为广义加速度向量中驱动关节对应的加速度向量；
Figure GDA0003529519130000039
和
Figure GDA00035295191300000310
分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τmin和τmax分别为驱动力向量的下限约束值与上限约束值；
Figure GDA00035295191300000311
为
Figure GDA00035295191300000312
的初始值；
Figure GDA00035295191300000313
为
Figure GDA00035295191300000314
的终止值；
Figure GDA00035295191300000315
为
Figure GDA00035295191300000316
的初始值；
Figure GDA00035295191300000317
为
Figure GDA00035295191300000318
的终止值；Ω为可行域；T为运动总时间；式(4)中，qds是驱动关节坐标向量qd对s求一阶导数；qdss是驱动关节坐标向量qd对s求二阶导数。
进一步地，所述步骤S4的具体实施步骤如下：
S401、将路径坐标s的定义域均匀离散为由n个元素组成的序列：{s0，…sk，…,sn}，其中，s0＝s(0)＝0，sn＝s(T)＝1，令k＝0，得到路径坐标s、伪速度
Figure GDA00035295191300000319
和伪加速度
Figure GDA00035295191300000320
的当前值sk、
Figure GDA00035295191300000321
和
Figure GDA00035295191300000322
S402、根据由步骤S401得到的路径坐标s、伪速度
Figure GDA00035295191300000326
和伪加速度
Figure GDA00035295191300000323
的当前值sk、
Figure GDA00035295191300000324
和
Figure GDA00035295191300000325
并利用广义-alpha隐式积分算法求解如式(1)所述的逆动力学方程，得到柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度、角加速度，以及各刚性运动杆件及柔性运动杆件的位置状态；
S403、建立柔性多体机器人的瞬态刚性逆动力学模型，利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件：
Figure GDA0003529519130000041
式(5)中，Ms为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；Cs为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；Gs为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；
将式(5)整理为
Figure GDA0003529519130000042
式(6)中，mi为向量Ms中的元素；ci和gi分别为Cs和Gs中的元素；l为柔性多体机器人驱动力向量τ的维数；
根据当前已知的路径坐标s即可求出伪速度
Figure GDA0003529519130000043
的最大允许值
Figure GDA0003529519130000044
S404、建立当前柔性多体机器人的瞬态刚性逆运动学模型，设置路径坐标区间为[sk,sk+1]并在其中均匀提取j个采样点，然后根据式(6)计算此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度
Figure GDA0003529519130000045
构成的平面上对[sk,sk+1]区间段规划出如下运动轨迹：
Figure GDA0003529519130000046
式(7)中，系数cl1和cl2由端点值(sk,f(sk))和(sk+1,f(sk+1))确定；
S405、根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中柔性多体机器人的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度、角加速度，并根据式(3)进行校核：若不满足要求，设置比例因子σ∈(0,1)，修正当前五次样条函数末端点的纵坐标为f(sk+1)＝σf(sk+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个运动副，其中点A0i、A1i、A2i(i＝1，2，3)处为刚性转动关节，点A0i(i＝1，2，3)处转动关节为驱动关节，点A4i(i＝1，2，3)处为刚性虎克铰。
对此，如图3所示，针对该三自由度3-RRRU柔性多体机器人的近似时间最优轨迹规划方法的具体步骤如下：
S1、构建柔性多体机器人各构件的数学模型并设置参数：
获取参与运动的构件的质量、尺寸和质心分布情况，不参与运动的构件的尺寸，以及柔性构件的相关参数；具体为获取该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；由于所有构件的质量分布都是均匀的，其质心均位于构件的几何中心处。
将该柔性多体机器人的末端执行器设置在动平台的几何中心点处，即如图1和图2所示的动平台上的P点；对该柔性多体机器人中的静平台、动平台和六根刚性杆使用自然坐标法进行建模，而三个柔性杆则使用绝对节点坐标法进行建模；在上述建模过程中，选取静平台的中心点作为所建立的坐标系的原点。
S2、根据步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；根据步骤S1所设置的各刚性构件和柔性构件的几何参数和材料物理特性建立柔性多体机器人的完整约束方程，包括运动学约束方程和动力学约束方程；接着，基于拉格朗日方程，组建柔性多体机器人的广义坐标向量、质量矩阵、有势广义力向量、非有势广义力向量和约束方程的雅克比矩阵，进而建立柔性多体机器人的运动学约束条件，得到约束条件下柔性多体机器人的逆动力学方程。
该步骤S2的具体实施过程如下：
如图2所示，每个支链中的两根刚性杆分别用广义坐标向量q1＝(x1，z1)T、q2＝(x4，z4)T和q3＝(x7，z7)T表示，其中，(x1，z1)为A11点的全局坐标，(x4，z4)为A12点的全局坐标，(x7，z7)为A13点的全局坐标；每个支链中的柔性杆分别用广义坐标向量
Figure GDA0003529519130000071
和
Figure GDA0003529519130000072
表示；动平台用广义坐标向量
Figure GDA0003529519130000073
表示，其中，[]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依次表示6根刚性杆、3根柔性杆和1个动平台；
根据各刚性杆、柔性杆、转动副和动平台的运动学约束关系建立柔性多体机器人的运动学约束条件和动力学约束条件；具体地，运动学约束条件包括：几何约束方程向量Φ(q，t)、速度约束方程向量
Figure GDA0003529519130000081
和加速度约束方程向量
Figure GDA0003529519130000082
接着，基于拉格朗日方程，组建该柔性多体机器人的广义坐标向量为q＝[q1 T,e1T,q2 T,e2T,q3 T,e3T]T、质量矩阵为M、重力有势广义力向量为QG，弹性有势广义力向量为QE，非有势广义力向量为Q，以及约束方程的雅克比矩阵，进而得到约束条件下柔性多体机器人的逆动力学方程为；
Figure GDA0003529519130000083
其中，
Figure GDA0003529519130000084
式(1)中，M(q)为质量矩阵；QG(q)为重力广义力向量；QE(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φq T为对Φq的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；
Figure GDA0003529519130000085
为广义速度向量；
Figure GDA0003529519130000086
为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；
Figure GDA0003529519130000087
为速度约束方程向量，其为Φ(q,t)对t求全导数；
Figure GDA0003529519130000088
为加速度约束方程向量，其为Φ(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)描述该柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数为：
Figure GDA0003529519130000091
其中，
Figure GDA0003529519130000092
式(3)中，tend为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；
Figure GDA0003529519130000093
为伪速度，是一维路径坐标s的导数；
Figure GDA0003529519130000094
为伪加速度，是一维路径坐标的二阶导数；
Figure GDA0003529519130000095
为广义速度向量中驱动关节对应的速度向量；
Figure GDA0003529519130000096
和
Figure GDA0003529519130000097
分别为驱动关节速度向量的下限约束值与上限约束值；
Figure GDA0003529519130000098
为广义加速度向量中驱动关节对应的加速度向量；
Figure GDA0003529519130000099
和
Figure GDA00035295191300000910
分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τmin和τmax分别为驱动力向量的下限约束值与上限约束值；
Figure GDA00035295191300000911
为
Figure GDA00035295191300000912
的初始值；
Figure GDA00035295191300000913
为
Figure GDA00035295191300000914
的终止值；
Figure GDA00035295191300000915
为
Figure GDA00035295191300000916
的初始值；
Figure GDA00035295191300000917
为
Figure GDA00035295191300000918
的终止值；Ω为可行域；T为运动总时间；式(4)中，qds是驱动关节坐标向量qd对s求一阶导数；qdss是驱动关节坐标向量qd对s求二阶导数。
步骤S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，根据约束条件计算约束曲线，得到轨迹规划可行域，再利用样条函数在可行域内规划出该柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线；具体实施步骤如下：
步骤S401：将路径坐标s的定义域均匀离散为由10个元素组成的序列{s0，…sk，…,sn}，其中，s0＝s(0)＝0，sn＝s(T)＝1，令k＝0，得到路径坐标s、伪速度
Figure GDA00035295191300000919
和伪加速度
Figure GDA00035295191300000920
的当前值sk、
Figure GDA00035295191300000921
和
Figure GDA00035295191300000922
步骤S402：根据路径坐标s、伪速度
Figure GDA00035295191300000923
和伪加速度
Figure GDA00035295191300000924
的当前值sk、
Figure GDA00035295191300000925
和
Figure GDA00035295191300000926
利用广义-alpha隐式积分算法求解上述式(1)的逆动力学方程，得到该柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度和角加速度，以及各刚性运动杆件及柔性运动杆件的位置状态；
步骤S403：建立柔性多体机器人的瞬态刚性逆动力学模型，即利用路径坐标s及其导数项将动力学约束条件和运动学约束条件描述为：
Figure GDA0003529519130000101
式(5)中，Ms为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；Cs为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；Gs为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；
进一步，为了便于求出伪速度约束曲线，将上述式(5)整理为：
Figure GDA0003529519130000102
式(6)中，mi为向量Ms中的元素；ci和gi分别为Cs和Gs中的元素；l为柔性多体机器人驱动力向量τ的维数；
将当前已知的路径坐标s代入式(6)中，求出伪速度
Figure GDA0003529519130000105
的最大允许值
Figure GDA0003529519130000106
步骤S404：建立当前柔性多体机器人的瞬态刚性逆运动学模型，即设置路径坐标区间为[sk,sk+1]，并在其中均匀提取10个采样点，根据式(6)计算出此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度
Figure GDA0003529519130000103
构成的平面上对[sk,sk+1]区间段规划出如下运动轨迹：
Figure GDA0003529519130000104
式(7)中，系数cl1和cl2由端点值(sk,f(sk))和(sk+1,f(sk+1))确定；
步骤S405：根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中如式(1)所示的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度和角加速度；
将上述得到的计算结果带入式(3)中，对计算结果是否满足式(3)中各项参数的设定范围进行校核：
I、若不满足要求，则设置比例因子σ＝0.05，并修正当前步骤S404中的五次样条函数的末端点的纵坐标为f(sk+1)＝σf(sk+1)，返回步骤S404，直至满足校核要求；
II、若满足要求，则令k＝k+1，返回步骤S403，直至k＝n，完成全部轨迹规划，得到该3-RRRU柔性多体机器人的最优运行轨迹。
以下为该3-RRRU柔性多体机器人近似时间最优轨迹规划仿真算例：
第1、第4和第7构件的运动状态可由点A0i(i＝1,2,3)处的驱动关节角θ1i(i＝1,2,3)的运动状态来描述，其定义方式如图4所示；φi(i＝1,2,3)为坐标轴XG绕ZG轴旋转到与A0i处坐标轴x0i(i＝1,2,3)共线时的角度；
如图5所示为已知3-RRRU柔性多体机器人末端执行器的空间摆线运动轨迹，其表达式为：
Figure GDA0003529519130000111
式(8)中，0≤s≤1，
Figure GDA0003529519130000112
在本次轨迹规划过程中，该3-RRRU柔性多体机器人的约束条件如表1所示。
表1：
Figure GDA0003529519130000113
进一步地，为验证规划方法的有效性和正确性，采用上述轨迹规划方法对该3-RRRU柔性多体机器人进行近似时间最优轨迹，求出第1、第4和第7构件的运动状态，它们由点A0i(i＝1,2,3)处驱动关节角θ1i(i＝1,2,3)的运动状态来描述，进而求出如图6～图9所示的驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线；空间摆线的规划出的运动总时间T为1.608秒。
通过对比表1，以及图7～图9中驱动关节的规划运动曲线可知，该方法得到的驱动关节的角速度曲线未超过其约束值
Figure GDA0003529519130000121
和
Figure GDA0003529519130000122
得到的驱动关节的角加速度曲线未超过其约束值
Figure GDA0003529519130000123
和
Figure GDA0003529519130000124
得到的驱动关节的驱动力曲线未超过其约束值τmin和τmax，而且第三支链驱动关节的驱动力曲线非常接近τmin。
综上所述，该规划方法能够充分利用了驱动力向量约束条件的最大允许值，缩短了运行时间，提高了柔性多体机器人的工作效率，而且规划出的驱动关节的运动曲线比较平滑，没有出现大幅度振荡的现象，实现了柔性多体机器人的平稳运动，验证了本算法的有效性。

Claims (5)

1.一种柔性多体机器人近似时间最优轨迹规划方法，其特征在于，步骤如下：
S1、利用自然坐标法或相对坐标法构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；
S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；
在所述步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：
Figure RE-FDA0003529519120000011
其中，
Figure RE-FDA0003529519120000012
式(1)中，M(q)为质量矩阵；QG(q)为重力广义力向量；QE(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；Φq T为对Φq的矩阵转置；λ为拉格朗日乘子向量；q为广义坐标向量；
Figure RE-FDA0003529519120000013
为广义速度向量；
Figure RE-FDA0003529519120000014
为广义加速度向量；τ为驱动力向量；t为时间；Φ(q,t)为几何约束方程向量，其包含各构件的几何尺寸约束条件、关节约束条件、平面约束条件、曲面约束条件和位移约束条件；
Figure RE-FDA0003529519120000015
为速度约束方程向量，其为Φ(q,t)对t求全导数；
Figure RE-FDA0003529519120000016
为加速度约束方程向量，其为Φ(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、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；
在步骤S3中，采用一维路径坐标s(0≤s≤1)描述柔性多体机器人的运动轨迹，其所建立规划轨迹的目标函数为：
Figure RE-FDA0003529519120000021
其中，
Figure RE-FDA0003529519120000022
式(3)中，tend为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；
Figure RE-FDA0003529519120000023
为伪速度，是一维路径坐标s的导数；
Figure RE-FDA0003529519120000024
为伪加速度，是一维路径坐标的二阶导数；
Figure RE-FDA0003529519120000025
为广义速度向量中驱动关节对应的速度向量；
Figure RE-FDA0003529519120000026
和
Figure RE-FDA0003529519120000027
分别为驱动关节速度向量的下限约束值与上限约束值；
Figure RE-FDA0003529519120000028
为广义加速度向量中驱动关节对应的加速度向量；
Figure RE-FDA0003529519120000029
和
Figure RE-FDA00035295191200000210
分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τmin和τmax分别为驱动力向量的下限约束值与上限约束值；
Figure RE-FDA00035295191200000211
为
Figure RE-FDA00035295191200000212
的初始值；
Figure RE-FDA00035295191200000213
为
Figure RE-FDA00035295191200000214
的终止值；
Figure RE-FDA00035295191200000215
为
Figure RE-FDA00035295191200000216
的初始值；
Figure RE-FDA00035295191200000217
为
Figure RE-FDA00035295191200000218
的终止值；Ω为可行域；T为运动总时间；式(4)中，qds是驱动关节坐标向量qd对s求一阶导数；qdss是驱动关节坐标向量qd对s求二阶导数；
S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。
2.根据权利要求1所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S4的具体实施步骤如下：
S401、将路径坐标s的定义域均匀离散为由n个元素组成的序列：
{s0，…sk，…,sn}，其中，s0＝s(0)＝0，sn＝s(T)＝1，令k＝0，得到路径坐标s、伪速度
Figure RE-FDA00035295191200000219
和伪加速度
Figure RE-FDA00035295191200000220
的当前值sk、
Figure RE-FDA00035295191200000221
和
Figure RE-FDA00035295191200000222
S402、根据由步骤S401得到的路径坐标s、伪速度
Figure RE-FDA0003529519120000031
和伪加速度
Figure RE-FDA0003529519120000032
的当前值sk、
Figure RE-FDA0003529519120000033
和
Figure RE-FDA0003529519120000034
并利用广义-alpha隐式积分算法求解如式(1)所述的逆动力学方程，得到柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度、角加速度，以及各刚性运动杆件及各柔性运动杆件的位置状态；
S403、建立柔性多体机器人的瞬态刚性逆动力学模型，利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件：
Figure RE-FDA0003529519120000035
式(5)中，Ms为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；Cs为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；Gs为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；
将式(5)整理为
Figure RE-FDA0003529519120000036
式(6)中，mi为向量Ms中的元素；ci和gi分别为Cs和Gs中的元素；l为柔性多体机器人驱动力向量τ的维数；
根据当前已知的路径坐标s即可求出伪速度
Figure RE-FDA0003529519120000037
的最大允许值
Figure RE-FDA0003529519120000038
S404、建立当前柔性多体机器人的瞬态刚性逆运动学模型，设置路径坐标区间为[sk,sk+1]并在其中均匀提取j个采样点，然后根据式(6)计算此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度
Figure RE-FDA0003529519120000039
构成的平面上对[sk,sk+1]区间段规划出如下运动轨迹：
Figure RE-FDA00035295191200000310
式(7)中，系数c l1 和c l2 由端点值(sk,f(sk))和(sk+1,f(sk+1))确定；
S405、根据当前规划出的运动轨迹，利用广义-alpha隐式积分算法求解步骤S2中柔性多体机器人的逆动力学方程，得到各个驱动关节的驱动力以及各个运动关节的角位移、角速度、角加速度，并根据式(3)进行校核：若不满足要求，设置比例因子σ∈(0,1)，修正当前五次样条函数末端点的纵坐标为f(sk+1)＝σf(sk+1)，返回步骤S404，直至满足校核要求；若满足要求，则令k＝k+1，返回步骤S403，直至k＝n，完成全部轨迹规划。
3.根据权利要求2所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S401中，元素的个数n取10。
4.根据权利要求2所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S404中，采样点j的个数为10。
5.根据权利要求2所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，所述步骤S405中，比例因子σ为0.05。
CN201910026172.9A 2019-01-11 2019-01-11 一种柔性多体机器人近似时间最优轨迹规划方法 Active CN109991847B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910026172.9A CN109991847B (zh) 2019-01-11 2019-01-11 一种柔性多体机器人近似时间最优轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910026172.9A CN109991847B (zh) 2019-01-11 2019-01-11 一种柔性多体机器人近似时间最优轨迹规划方法

Publications (2)

Publication Number Publication Date
CN109991847A CN109991847A (zh) 2019-07-09
CN109991847B true CN109991847B (zh) 2022-04-22

Family

ID=67129178

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910026172.9A Active CN109991847B (zh) 2019-01-11 2019-01-11 一种柔性多体机器人近似时间最优轨迹规划方法

Country Status (1)

Country Link
CN (1) CN109991847B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110561419B (zh) * 2019-08-09 2021-01-12 哈尔滨工业大学（深圳） 臂型线约束柔性机器人轨迹规划方法及装置
CN111123943B (zh) * 2019-12-30 2023-09-01 汕头大学 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
CN112818482B (zh) * 2021-01-19 2023-07-21 中国科学院沈阳自动化研究所 弹性光滑曲面约束的细长软体机器人的建模与控制方法
CN113771046B (zh) * 2021-10-25 2023-06-30 中国北方车辆研究所 一种最小化Jerk指标摆动轨迹规划方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1989008890A1 (en) * 1988-03-09 1989-09-21 N.V. Philips' Gloeilampenfabrieken Differential budding: method and apparatus for path planning with moving obstacles and goals
JPH0756617A (ja) * 1993-08-17 1995-03-03 Nishijima Yoshitaro マニピュレータの最適軌道計画法
CN107220421A (zh) * 2017-05-18 2017-09-29 北京理工大学 一种空间复杂柔性结构多体系统动力学建模与计算方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN108544493A (zh) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 一种针对机器人操作空间的时间最优轨迹规划方法和装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO1989008890A1 (en) * 1988-03-09 1989-09-21 N.V. Philips' Gloeilampenfabrieken Differential budding: method and apparatus for path planning with moving obstacles and goals
JPH0756617A (ja) * 1993-08-17 1995-03-03 Nishijima Yoshitaro マニピュレータの最適軌道計画法
CN107220421A (zh) * 2017-05-18 2017-09-29 北京理工大学 一种空间复杂柔性结构多体系统动力学建模与计算方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN108544493A (zh) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 一种针对机器人操作空间的时间最优轨迹规划方法和装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Improving the efficiency of time-optimal path-following algorithms;J.-J.E. Slotine，et al.;《IEEE Transactions on Robotics and Automation》;19891231;第118-124页 *
一种含柔性杆件的空间并联机器人控制策略的研究;刘凉;《中国优秀硕士学位论文全文数据库信息科技辑》;20171231;第1-13页，第48-103页，第109-116页 *
基于最优时间的超声检测轨迹规划;郑慧峰等;《浙江大学学报(工学版)》;20100115;第44卷(第01期);第29-33页 *

Also Published As

Publication number Publication date
CN109991847A (zh) 2019-07-09

Similar Documents

Publication Publication Date Title
CN109991847B (zh) 一种柔性多体机器人近似时间最优轨迹规划方法
CN109543264B (zh) 一种基于多维度重构校正的柔性多体机器人建模与求解方法
Walker et al. Efficient dynamic computer simulation of robotic mechanisms
CN107160401B (zh) 一种解决冗余度机械臂关节角偏移问题的方法
CN110405762A (zh) 一种基于空间二阶倒立摆模型的双足机器人姿态控制方法
CN104076685B (zh) 一种减少基座姿态扰动的空间机械臂路径规划方法
CN113043278B (zh) 一种基于改进型鲸鱼搜索方法的机械臂轨迹规划方法
Valsamos et al. Index based optimal anatomy of a metamorphic manipulator for a given task
Zheng et al. An efficient dynamic modeling and simulation method of a cable-constrained synchronous rotating mechanism for continuum space manipulator
Khatamian Solving kinematics problems of a 6-dof robot manipulator
CN111123943A (zh) 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
Wiese et al. Kinematic modeling of a soft pneumatic actuator using cubic hermite splines
Zhao et al. Trajectory planning for 6-DOF robotic arm based on quintic polynormial
CN113721626A (zh) 一种制动盘补偿累计误差的机器人轨迹规划方法
Mishra et al. Dynamic modelling of an elephant trunk like flexible bionic manipulator
Xie et al. Manipulator calibration based on PSO-RBF neural network error model
Li et al. Simulation and comparison research of Lagrange and Kane dynamics modeling for the 4-DOF modular industrial robot
CN114888812A (zh) 一种基于刚度性能优化的移动机械臂站位规划方法
CN107944135A (zh) 三维长管的支撑位置优化方法
Zhang et al. Lightweight long-reach 5-dof robot arm for farm application
CN113434982A (zh) 一种电力智能仿生攀爬机器人的运动学逆解算法
Yang et al. Research on gravity compensation in motion control of multi-joint robot
Petr et al. Investigation of the Stewart Platform Workspace Using MATLAB-Simulink and Simscape Multibody Library
Jiang Vision-based Cartesian space motion control for flexible robotic manipulators
Duan et al. Energy optimal trajectory algorithm for a robotic manipulator

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