[发明专利]一种面向双臂机械手协同重复运动的有限时间规划方法

权利要求书

1.一种面向双臂机械手协同重复运动的有限时间规划方法，其特征在于，所述方法包括以下步骤：

1)确定双臂机械手的左右末端执行器的期望目标轨迹和以及双臂机械手各关节的角期望回拢角度和

2)通过设计二次优化指标，形成双臂机械手协同重复运动规划方案，其中可以任意指定双臂机械手实际运动时的初始关节角，末端执行器可以不处于期望轨迹上；记实际运动时左右两臂的关节角度为θ_L和θ_R,且分别以θ_L(0)和θ_R(0)为左右机械臂的运动起始点，双臂机械手的协同重复运动解析方案转化为统一的二次规划问题：

其中，P(t)＝κ(Θ(t)-Θ(0))，κ＞0，Θ(t)＝[θ_L；θ_R]，表示双臂机械手的各运动关节角速度，表示机械手左右末端执行器期望的运动轨迹，表示左右末端执行器期望的速度向量；由于机械手的左右初始位置可能不在期望的轨迹上，分别通过减小左右臂末端执行器期望位置与实际运动轨迹位置间的误差(Υ-f(Θ(t)))，改变末端执行器的运动方向，β_Υ＞0表示位置的参数增益，用来调节左右双臂的末端执行器运动时的速率，J(Θ(t))为双臂机械手雅可比矩阵，f(Θ(t))是双臂机械手的实际运动轨迹；

3)引入初始修正因子，构建有限时间收敛的终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝Θ(t)-Θ(0)，η_E是一个非正定的设计参数，σ_E的值与η_E的取值有关，为一初始修正向量，使误差能在有限时间内收敛到零；ξ(t)满足以下条件：

a.当t∈(-∞,0]∪[Δ,+∞)时，ξ(t)＝0；

b.当t∈(0,Δ)时，

其中，t₀为起始时间，Δ为时间间隔，当二次规划问题获得最优解时，双臂机械手的各个关节角可以回拢到期望的目标轨迹上；

通过对拉格朗日函数各个变量求导，并令其为零，得如下时变矩阵方程

W(t)Y(t)＝v(t) (3)

其中，

I为单位矩阵

记时变矩阵方程误差E(t)＝W(t)Y(t)-ν(t)，根据终态网络动态特性方程(2)，构建网络模型，得到终态网络模型的求解方程如下：

4)将步骤3)中求解得到的结果用于控制各关节电机，驱动机械臂执行任务。