[发明专利]一种求解双冗余机械臂互相碰撞的动力学方法

权利要求书

1.一种求解双冗余机械臂互相碰撞的动力学方法，其特征在于，所述的动力学方法包括如下步骤：

S1、将实际的物理系统形式化，以左边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，实现在保持机械臂精准完成末端追踪轨迹任务的同时进行速度层面的最优化处理；

S2、根据避免双冗余度机械臂之间碰撞的约束，对步骤S1中的二次规划问题进行扩充，考虑避免互相碰撞的不等式约束；

S3、根据避免双冗余机械臂关节角和关节角速度的约束，在步骤S2的基础上将角度以及角速度的约束加入二次规划问题；

S4、将步骤S3中的左边机械臂的二次规划问题应用到右边机械臂上；

S5、将步骤S4中的双冗余度机械臂的二次规划问题统一到一个二次规划框架下；

S6、基于步骤S5中统一后的标准二次规划问题，利用线性变分不等式-原对偶神经网络进行求解，所求得的解即为能实现双冗余机械臂互相躲避的结果。

2.根据权利要求1所述的一种求解双冗余机械臂互相碰撞的动力学方法，其特征在于，所述的步骤S1中将实际物理系统形式化，以左边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，其模型如下：

在实数域中，定义为左边机械臂关节角速度矩阵，为雅可比矩阵，为机械臂末端目标轨迹，||·||表示欧几里得二范数；

寻找满足二次规划问题(1)-(2)的最优解同时，n表示机械臂的自由度，m表示机械臂所处的工作空间维度。

3.根据权利要求2所述的一种求解双冗余机械臂互相碰撞的动力学方法，其特征在于，所述的步骤S2中双冗余机械臂之间避免碰撞的约束条件具体如下：

首先运用空间几何的知识计算出双机械臂之间的最短距离以及临界点，然后将关于双冗余机械臂之间避免碰撞的约束条件转化为与最短距离有关的临界点的约束条件，假设双冗余机械臂之间的最短距离为d，左臂上的临界点为P，右臂上的临界点为E，如此，初步得到以下约束：

其中，为从临界点P到点E方向上的向量，为临界点P的雅可比矩阵，为左边机械臂关节角速度矩阵，此外，符号定义为矩阵的每一行之间的乘积；

将速度的突变转化为暂变的过程，定义一个β_L，并令β_L的定义如下所示：

其中，函数S(·)的定义如下：

其中，d₁和d₂分别表示绝对安全距离以及相对安全距离，当双冗余机械臂之间的最短距离小于绝对安全距离时则认为机械臂之间发生了碰撞，当双冗余机械臂之间的最短距离小于相对安全距离时则认为此时必须采取措施以防发生碰撞；

此时，避免机械臂之间的互相碰撞的约束、转化为以下的平稳过程：

并且，令二次规划问题更新为如下形式：

4.根据权利要求3所述的一种求解双冗余机械臂互相碰撞的动力学方法，其特征在于，所述的步骤S3过程如下：

将机械臂关节角度和关节角速度的约束转换到关节角速度层上解决，根据双冗余机械臂本身的参数，得到以下不等式：

将关节角度的约束转化为关节角速度层的约束，于是，提出以下的转换方式代替(10)中的前部分的式子：

其中，α为与接机械臂本身有关的参数，同时，再令和于是，机械臂关节角度以及关节角速度的约束转化为以下的形式：

5.根据权利要求4所述的一种求解双冗余机械臂互相碰撞的动力学方法，其特征在于，所述的步骤S4过程如下：

根据公式(7)-(9)以及(12)类比出一个如下的右臂二次规划问题：

其中，定义为左边机械臂关节角速度矩阵，为雅可比矩阵，为右机械臂末端目标轨迹，||·||表示欧几里得二范数；J_NR为右机械臂上临界点的雅可比矩阵，β_R，的定义与左臂上的定义如下：