[发明专利]一种柔性多体机器人近似时间最优轨迹规划方法

权利要求书

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

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

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

在所述步骤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、采用一维路径坐标描述柔性多体机器人的运动轨迹，并建立规划轨迹的目标函数；

在步骤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及其导数项描述动力学约束条件和运动学约束条件，进而根据约束条件计算约束曲线，以得到轨迹规划可行域；进一步利用样条函数在可行域内规划出柔性多体机器人驱动关节的角位移曲线、角速度曲线、角加速度曲线和驱动力曲线。