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

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

An Approximate Time Optimal Trajectory Planning Method for Flexible Multibody Robots

【技术实现步骤摘要】
一种柔性多体机器人近似时间最优轨迹规划方法
本专利技术涉及柔性多体机器人轨迹规划领域，特别涉及一种柔性多体机器人近似时间最优轨迹规划方法。
技术介绍
随着工业对生产效率和降低能耗要求的不断提升，促使工业机器人不断向轻型化和小型化发展。运动轨迹规划是保证工业机器人精确、高效、稳定和健康工作的关键环节。从规划方式上讲，可分为离线轨迹规划和在线轨迹规划，两种方式均需要提前设计运动几何路径，然后利用优化算法对运动轨迹进行优化，同时遵循机构的运动学和动力学约束条件。目前常用的规划方法包含时间最优法、Jerk最优法和时间-能量最优法，其中时间最优算法研究较多。由于约束条件下柔性多体机器人的动力学模型求解十分复杂，因此合理构建轨迹规划策略对于柔性多体机器人工作性能和效率的优化都显得十分重要。常用的时间最优轨迹规划算法有降阶优化法和智能数值优化算法，前者通过引入路径参数将多维时间优化问题转变为低维度优化问题，后者通过引入智能化的搜索算法或者算子进行求解。然而时间最优法的缺点在于机器人在规划的运动过程中至少有一个驱动装置处于饱和状态，所以无法对系统扰动进行有效的补偿和抑制。
技术实现思路
本专利技术的目的是提供一种解决现有约束条件下柔性多体机器人的运动轨迹规划复杂问题的柔性多体机器人近似时间最优轨迹规划方法。为此，本专利技术技术方案如下：一种柔性多体机器人近似时间最优轨迹规划方法，步骤如下：S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。进一步地，在步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：其中，式(1)中，M(q)为质量矩阵；QG(q)为重力广义力向量；QE(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；ΦqT为对Φ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)中，tend为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；为伪速度，是一维路径坐标s的导数；为伪加速度，是一维路径坐标的二阶导数；为广义速度向量中驱动关节对应的速度向量；和分别为驱动关节速度向量的下限约束值与上限约束值；为广义加速度向量中驱动关节对应的加速度向量；和分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τmin和τmax分别为驱动力向量的下限约束值与上限约束值；为的初始值；为的终止值；为的初始值；为的终止值；Ω为可行域；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、伪速度和伪加速度的当前值sk、和S402、根据由步骤S401得到的路径坐标s、伪速度和伪加速度的当前值sk、和并利用广义-alpha隐式积分算法求解如式(1)所述的逆动力学方程，得到柔性多体机器人各个驱动关节的驱动力，各个运动关节的角位移、角速度、角加速度，以及各刚性运动杆件及柔性运动杆件的位置状态；S403、建立柔性多体机器人的瞬态刚性逆动力学模型，利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件：式(5)中，Ms为瞬态刚性逆动力学模型中的质量矩阵M对s求一阶导数；Cs为瞬态刚性逆动力学模型中的离心力和哥氏力项矩阵C对s求一阶导数；Gs为瞬态刚性逆动力学模型中的重力向量G对s求一阶导数；将式(5)整理为式(6)中，mi为向量Ms中的元素；ci和gi分别为Cs和Gs中的元素；l为柔性多体机器人驱动力向量τ的维数；根据当前已知的路径坐标s即可求出伪速度的最大允许值S404、建立当前柔性多体机器人的瞬态刚性逆运动学模型，设置路径坐标区间为[sk,sk+1]并在其中均匀提取j个采样点，然后根据式(6)计算此区间段的伪速度约束曲线并确定可行域，在可行域内利用五次样条函数在路径坐标s和伪速度构成的平面上对[sk,sk+1]区间段规划出如下运动轨迹：式(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柔性多体机器人各构件建模时的广义坐标向本文档来自技高网...

【技术保护点】
1.一种柔性多体机器人近似时间最优轨迹规划方法，其特征在于，步骤如下：S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。

【技术特征摘要】
1.一种柔性多体机器人近似时间最优轨迹规划方法，其特征在于，步骤如下：S1、利用自然坐标法或相对坐标法描述构建柔性多体机器人的各刚性构件的数学模型；利用绝对节点坐标法或浮动坐标系法或几何精确单元法描述构建柔性多体机器人的各柔性构件的数学模型；获取各刚性构件和柔性构件的几何参数和材料物理性能参数并确定该柔性多体机器人中的驱动构件；S2、基于拉格朗日方程，利用步骤S1所建立的各刚性构件的数学模型和各柔性构件的数学模型建立各刚性构件和各柔性构件的广义坐标向量、质量矩阵、以及作用在各刚性构件和各柔性构件上的有势广义力向量和非有势广义力向量；进而得到约束条件下柔性多体机器人的逆动力学方程；S3、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；S4、建立柔性多体机器人的瞬态刚性逆动力学模型和瞬态刚性逆运动学模型，并利用路径坐标s及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。2.根据权利要求1所述的柔性多体机器人近似时间最优轨迹规划方法，其特征在于，在所述步骤S2中，约束条件下柔性多体机器人的逆动力学方程为：其中，式(1)中，M(q)为质量矩阵；QG(q)为重力广义力向量；QE(q)为弹性广义力向量；Q(q,τ,t)为驱动广义力向量；ΦqT为对Φ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)中，tend为最终规划总时间；J为运动时间；s为一维路径坐标，其定义域为[0,1]；为伪速度，是一维路径坐标s的导数；为伪加速度，是一维路径坐标的二阶导数；为广义速度向量中驱动关节对应的速度向量；和分别为驱动关节速度向量的下限约束值与上限约束值；为广义加速度向量中驱动关节对应的加速度向量；和分别为驱动关节加速度向量的下限约束值与上限约束值；τ为驱动力向量，τmin和τmax分别为驱动力向量的下限约束值与上限约束值；...

【专利技术属性】
技术研发人员：刘凉，苏鑫，肖兰齐，赵新华，解志峰，李建玲，李培鑫，
申请(专利权)人：天津理工大学，
类型：发明
国别省市：天津,12