本发明涉及双冗余机械臂的运动学技术领域，具体涉及一种求解双冗余机械臂互相碰撞的动力学方法。
背景技术：
近年来，机械臂在很多领域得到了广泛的应用，如医疗仪器和工业制造。它吸引如此之多关注机器人操纵器的主要原因是能够携带重物，做重复的事情，而且可以在危险和恶劣的环境中工作。为了满足所需任务的复杂要求，机械臂需要更灵活的配置和可选的优化。因此，与非冗余机械臂相比，配备有更多自由度的冗余机械臂迎合了更复杂任务的需求。随着自由度的增加，机械臂可以更好地完成基本任务，更好地完成可选优化任务，同时也可以满足一些额外但必要的目标，如联合限制回避和奇异回避。
另外，双冗余机械臂变得越来越受欢迎，因为双冗余机械臂可以完成单冗余机械臂不可以完成的协作任务。因此，在自动化订单拣选系统，帮助老年人和残疾人穿衣等各种应用中，它们可以被广泛应用。
然而，在操作双冗余机械臂的领域，一直存在一些具有挑战性的问题：
1)冗余度解析问题。逆运动学问题是机械臂运动学的核心问题之一。由于运动学方程的非线性，逆运动学问题求解往往比较困难。另外，由于自由度的数量大于工作空间的数量，所以存在无数个解的情况。传统方法是应用伪逆的算法。然而，伪逆法存在明显的缺点，即计算矩阵的逆矩阵需要花费大量的时间。此外，伪逆法不能考虑不等式约束。最近，基于优化的方案被广泛研究，以便于将最优标准设计为最优目标和等式/不等式约束条件。
2)双冗余度机械臂协作任务的执行。通常期望双冗余机械臂协同完成末端执行器任务。应该考虑如何同时控制双冗余度机械臂。以前的研究表明，基于优化的方法是可行的。
3)实时避免相互碰撞算法。当双冗余机械臂在执行末端执行器任务时，不可忽视的问题是相互碰撞问题。一旦相互碰撞发生，双冗余机械臂将被迫偏离期望的轨迹并导致任务失败。更有甚者，可能会严重损害双冗余度机械臂。为避免这种严重的情况发生，相互避免碰撞的方案是必要的。
为了克服机器人和机械臂与障碍物碰撞的潜在问题，需要提出避免碰撞的方案。
技术实现要素：
本发明的目的是为了解决现有技术中的上述缺陷，提供一种求解双冗余机械臂互相碰撞的动力学方法，该避免相互碰撞方案，以满足获得包含关节限位避免，避免碰撞和完成末端执行器任务的最优解的要求。
本发明的目的可以通过采取如下技术方案达到：
一种求解双冗余机械臂之间相互躲避的运动学方法，包括如下步骤：
s1、将实际的物理系统形式化，以左边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，实现在保持机械臂精准完成末端追踪轨迹任务的同时进行速度层面的最优化处理；
具有应用中，先以左边机械臂还是先以右边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，不构成对本发明技术方案的限制。
s2、根据避免双冗余度机械臂之间碰撞的约束，对步骤s1中的二次规划问题进行扩充，考虑避免互相碰撞的不等式约束；
s3、根据避免双冗余机械臂关节角和关节角速度的约束，在步骤s2的基础上将角度以及角速度的约束加入二次规划问题；
s4、将步骤s3中的左边机械臂的二次规划问题类似地应用到右边机械臂上；
s5、将步骤s4中的双冗余度机械臂的二次规划问题统一到一个二次规划框架下；
s6、基于步骤s5中统一后的标准二次规划问题，利用线性变分不等式-原对偶神经网络进行求解，所求得的解即为能实现双冗余机械臂互相躲避的结果。
进一步地，所述的步骤s1中将实际物理系统形式化，以左边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，其模型如下：
在实数域中，定义
通过本发明中所述的运动规划方法，可以寻找满足二次规划问题(1)-(2)的最优解
进一步地，所述的步骤s2中双冗余机械臂之间避免碰撞的约束条件具体如下：
首先需要运用空间几何的知识计算出双机械臂之间的最短距离以及临界点，在这个基础上，关于双冗余机械臂之间避免碰撞的约束条件便可以转化为与最短距离有关的临界点的约束条件。假设双冗余机械臂之间的最短距离为d，左臂上的临界点为p，右臂上的临界点为e。如此，可初步得到以下约束：
其中，
为了进一步优化机械臂的性能，减少机械臂在避障时速度突变带来的影响以及进入安全距离之前的速度损失，在本发明中，将速度的突变转化为暂变的过程。因此，定义一个βl，并令βl的定义如下所示：
其中，函数s(·)的定义如下：
其中，d1和d2分别表示绝对安全距离以及相对安全距离。当双冗余机械臂之间的最短距离小于绝对安全距离时则可认为机械臂之间发生了碰撞，当双冗余机械臂之间的最短距离小于相对安全距离时则可认为此时必须采取措施以防发生碰撞。因此需要机械臂在进入相对安全距离之前保持预定的运行轨迹，并且禁止进入绝对安全距离而发生碰撞，所定义的βl则可实现如上设想功能。此时，避免机械臂之间的互相碰撞的约束可以转化为以下的平稳过程：
并且，令
进一步地，所述的步骤s3过程如下：
机械臂关节角度和关节角速度的约束都需在关节角速度层上解决。根据双冗余机械臂本身的参数，可得到以下不等式：
由于需要在速度层上求解二次规划问题，所以需要将关节角度的约束转化为关节角速度层的约束，于是，提出以下的转换方式代替(10)中的前部分的式子：
其中，α为与接机械臂本身有关的参数。同时，再令
进一步地，根据上述的二次规划问题公式(7)-(9)以及关节角度以及角速度的约束(12)，可以得到左臂完整的标准二次规划问题，类似得出右臂的标准二次规划问题，具体如下：根据公式(7)-(9)以及(12)可以类比出一个如下的右臂二次规划问题：
其中，定义
进一步地，所述的步骤s5、将步骤s4中的双冗余度机械臂的二次规划问题统一到一个二次规划框架下具体如下：
根据左右臂两个子二次规划问题整合成一个统一的二次规划问题，便于神经网路求解器求解。根据矩阵知识，整合后的二次规划问题如下所示：
subjecttogz＝r(14-1)
dz≤b(15-1)
z-≤z≤z+(16-1)
式中各矩阵的定义如下：
至此，标准的二次规划问题以及被完整提出。因此，求解这个问题相当于同时求解双冗余机械臂的轨迹追踪，互相躲避以及关节角度和关节角速度约束的问题。
进一步地，所述的步骤s6过程如下：
将标准二次规划问题通过基于线性变分不等式的原对偶神经网络求解，二次规划问题转化为线性变分不等式，并且进一步可以转换为如下的分段线性投影方程：
pω(y-(my+q))-y＝0(27)
其中，pω(·)是一个分段线性投影函数并定义如下：
同时，
最后，上述的分段线性投影方程通过下列的原对偶神经网络解决：
其中，λ是用来调节网络收敛速率的一个参数。
所得的结果即为双冗余机械臂在互相躲避碰撞和关节角度以及关节角速度极限的基础上完成末端任务的最优解。
本发明相对于现有技术具有如下的优点及效果：
本发明基于二次规划方法，不同于传统的伪逆方法，提出的求解双冗余机械臂之间相互躲避的运动学方法在实现控制双冗余机械臂的基础上，实现了寻求速度层面的最优解，同时，可以通过约束条件使得双冗余机械臂之间实现互相的躲避，除此之外，还可以有效地避免关节角度以及关节角速度的约束。利用二次规划问题求解运动规划的问题可以避免计算伪逆矩阵等带来的繁杂运算量，同时可以加入多组等式以及不等式的约束，充分利用冗余的自由度，实现机械臂更完善的功能。同时，双冗余机械臂在成功完成互相躲避的任务下，末端轨迹的误差达到了一个极小的范围，这样更进一步确保了方法的准确性，有效性以及可利用性。
附图说明
图1是本发明公开的一种求解双冗余机械臂之间相互躲避的运动学方法的流程图；
图2是应用本发明方法前后双冗余机械臂在协同合作过程中的路径和状态示意图；
其中，图2(a)是任务1未应用本发明方法时的路径示意图；
其中，图2(b)是任务1应用本发明方法时的路径示意图；
其中，图2(c)是任务2未应用本发明方法时的路径示意图；
其中，图2(d)是任务2应用本发明方法时的路径示意图；
图3是应用本发明方法前后各个时间段双冗余机械臂之间的最短距离示意图；
其中，图3(a)是任务1应用本发明方法前的最短距离示意图；
其中，图3(b)是任务1应用本发明方法后的最短距离示意图；
其中，图3(c)是任务2应用本发明方法前的最短距离示意图；
其中，图3(d)是任务2应用本发明方法前的最短距离示意图；
图4是在给出标准的二次规划问题之后，基于线性变分不等式的原对偶神经网络求解器求解时的流程图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例
图1所示为本发明实例的基于标准二次规划问题解决双冗余机械臂之间相互躲避问题的流程图；基于标准二次规划问题的求解双冗余机械臂之间相互躲避的运动学方法，包括如下步骤：
s1、将实际的物理系统形式化，以左边机械臂为基础建立该系统基于二次规划问题的最小速度二范数模型，实现在保持机械臂精准完成末端追踪轨迹任务的同时进行速度层面的最优化处理；
s2、根据避免双冗余度机械臂之间碰撞的约束，对步骤s1中的二次规划问题进行扩充，考虑避免互相碰撞的不等式约束；
s3、根据避免双冗余机械臂关节角和关节角速度的约束，在步骤s2的基础上将角度以及角速度的约束加入二次规划问题；
s4、将步骤s3中的左边机械臂的二次规划问题类似地应用到右边机械臂上；
s5、将步骤s4中的双冗余度机械臂的二次规划问题统一到一个二次规划框架下；
s6、基于步骤s5中统一后的标准二次规划问题，利用线性变分不等式-原对偶神经网络进行求解，所求得的解即为能实现双冗余机械臂互相躲避的结果。
图2所示为双冗余机械臂在应用本申请内容前后的路径，通过图片的对比可以看出，无论在完成描画交叉的圆或者汉字的‘回’，本方法都有着比较好的效果，可以在整体上看出双冗余机械臂在协同合作的过程中互相躲避的状态。若如图2(a)、图2(c)中所示，在双冗余机械臂协同合作的过程中，倘若没有考虑机械臂之间的碰撞问题，容易导致任务的失败，甚至是机械臂的损坏。应用本申请的内容后，如图2(b)、图2(d)中所示，双冗余机械臂完整、安全地完成了整个设定的任务。
图3所示为通过应用本申请解决双冗余度机械臂之间相互躲避问题的方法后，双冗余度机械臂之间的最短距离如图三所示。在应用本申请的内容之前，双冗余机械臂之间的最短距离如图3(a)、图3(c)中所示，分别在t＝4.1s和t＝64.3s内进入了碰撞的安全绝对距离，这也意味着双冗余机械臂在这个时刻发生了碰撞并且影响了任务的完成，除此之外，从图中可以看出双冗余机械臂在应用本专利的内容后，分别在t＝3.12s和t＝59.89s的时候与为应用方法的路径发生了改变，而这些改变正是使得双冗余机械臂之间实现互相避免的主要体现。同时，得以实现如此功能的方式如下所示：
以左边为例：首先需要运用空间几何的知识计算出双机械臂之间的最短距离以及临界点，在这个基础上，关于双冗余机械臂之间避免碰撞的约束条件便可以转化为与最短距离有关的临界点的约束条件。假设双冗余机械臂之间的最短距离为d，左臂上的临界点为p，右臂上的临界点为e。如此，可初步得到以下约束：
其中
其中的函数s(·)的定义如下：
其中，d1和d2分别表示绝对安全距离以及相对安全距离。当双冗余机械臂之间的最短距离小于绝对安全距离时则可认为机械臂之间发生了碰撞，当双冗余机械臂之间的最短距离小于相对安全距离时则可认为此时必须采取措施以防发生碰撞。因此我们需要机械臂在进入相对安全距离之前保持预定的运行轨迹，并且禁止进入绝对安全距离而发生碰撞，所定义的βl则可实现如上设想功能。此时，避免机械臂之间的互相碰撞的约束可以转化为以下的平稳过程：
并且，令
这样，就可以得到左边机械臂的避免碰撞的约束并且以标准二次规划问题表示出来。
所述的步骤s3过程如下：
机械臂关节角度和关节角速度的约束都需在关节角速度层上解决。根据双冗余机械臂本身的参数，可得到以下不等式：
由于需要在速度层上求解二次规划问题，所以需要将关节角度的约束转化为关节角速度层的约束，于是，提出以下的转换方式代替(10)中的前部分的式子：
其中，α为与接机械臂本身有关的参数。同时，再令
根据双冗余度机械臂的对偶性，可以得出右边机械臂的标准二次规划问题：
其中，定义
如此，双冗余机械臂的左边和右边都以标准二次规划问题的形式表示出来，下一步的工作则是根据矩阵的知识将左臂和右臂的二次规划问题统一到一个标准的二次规划问题上面，如下所示：
subjecttogz＝r(14-1)
dz≤b(15-1)
z-≤z≤z+(16-1)
式中各矩阵的定义如下：
至此，标准的二次规划问题以及被完整提出。因此，求解这个问题相当于同时求解双冗余机械臂的轨迹追踪，互相躲避以及关节角度和关节角速度约束的问题。
最后，通过神经网络求解上述的标准二次规划问题便可得到所需的关节角度的解决方案，可得出图3中的结果。
图4为在给出标准的二次规划问题之后，基于线性变分不等式的原对偶神经网络求解器求解时的流程图，且其求解过程如下所诉：
标准二次规划问题可以由基于线性变分不等式的原对偶神经网络求解。二次规划问题可以转化为线性变分不等式，并且进一步可以转换为如下的分段线性投影方程：
pω(y-(my+q))-y＝0(27)
其中，pω(·)是一个分段线性投影函数并定义如下：
同时，
最后，上述的分段线性投影方程可以通过下列的原对偶神经网络解决：
其中，λ是用来调节网络收敛速率的一个参数。
所得的结果即为双冗余机械臂在互相躲避碰撞和关节角度以及关节角速度极限的基础上完成末端任务的最优解。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。