CN106426164B - 一种冗余度双机械臂的多指标协调运动规划方法 - Google Patents

Abstract

本发明公开了一种冗余度双机械臂的多指标协调运动规划方法，包括步骤：1)基于目标问题，通过上位机采用二次型优化方案在速度层上分别对双机械臂进行逆运动学解析，设计的性能指标为最小速度二范数、重复运动和最小速度无穷范数这三个指标通过加权组合而成的指标，受约束于双机械臂各自的运动学方程、关节角极限和关节角速度极限；2)将步骤1)中双机械臂各自的二次型优化方案转化为标准的二次规划问题；3)将步骤2)中双机械臂各自的二次规划问题统一成一个二次规划问题；4)将步骤3)中统一的二次规划问题用简化的基于线性变分不等式的原对偶神经网络求解器进行求解；5)将步骤4)的求解结果传递给下位机控制器驱动双机械臂的运动。

Description

一种冗余度双机械臂的多指标协调运动规划方法

技术领域

本发明属于冗余度机械臂和多机械臂的运动规划方法，特别是涉及一种基于关节角速度层控制，多指标优化，使两个机械臂协调工作的冗余度双机械臂的多指标协调运动规划方法。

背景技术

冗余度机械臂是一种自由度大于完成任务所需最少自由度的末端能动机械装置。冗余度机械臂在完成末端主要任务的同时还可以完成一些次级任务，例如躲避障碍物、躲避关节极限、躲避奇异点和优化其它目标函数等。其可以完成包括焊接、油漆、组装、挖掘和绘图等任务，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。传统的冗余度解析方法以及工业机械臂控制方法主要是基于伪逆的方法。而近几年，计算量小、实时性强的基于二次型优化的冗余度解析方法被提出和应用。然而，目前的二次型优化方法仅仅只考虑单个指标优化或双指标优化，并不能在特定情况下满足需求。

同时，相对于单个机械臂工作，双机械臂或多机械臂协调工作不仅能够提高工作效率，还可以完成一些单个机械臂无法完成的任务，例如搬运物体、组装元件和炒菜做饭等。因此双机械臂协调控制一直被广泛研究。

发明内容

本发明要解决的技术问题是提供一种通过加权而考虑三种指标的冗余度双机械臂多指标协调运动规划方法。

为了实现上述发明目的，采用的技术方案如下：

一种冗余度双机械臂的多指标协调运动规划方法，包括如下步骤：

1)基于目标问题，通过上位机采用二次型优化方案在速度层上分别对双机械臂进行逆运动学解析，设计的优化性能指标为最小速度二范数、重复运动和最小速度无穷范数，这三个指标者通过两个加权因子加权组合而成，受约束于双机械臂各自的运动学方程、关节角极限和关节角速度极限；

2)将步骤1)中双机械臂各自的二次型优化方案转化为标准的二次规划问题；

3)将步骤2)中双机械臂各自的关节角、雅克比矩阵、末端轨迹、关节角极限和关节角速度极限进行组合，生成组合的关节角、组合雅克比矩阵、组合末端轨迹、组合关节角极限和组合关节角速度极限，从而将两个二次规划问题统一成一个二次规划问题；

4)将步骤3)中统一的二次规划问题用简化的基于线性变分不等式的原对偶神经网络求解器进行求解；

5)将步骤4)的求解结果传递给下位机控制器驱动双机械臂的运动。

本发明通过两个加权因子对三个指标进行权重分配，可以通过调节加权因子的大小进而形成不同的性能指标组合，以满足不同任务需求。同时两个机械臂的协调运动规划不仅可以提高工作效率，还可以完成一些单个机械臂无法完成的任务。

附图说明

图1为本发明的流程示意图；

图2为本发明的双机械臂模型示意图。

图中所示为：1-左机械臂；2-右机械臂；3-第一关节；4-第二关节；5第三关节；6-第四关节；7-第五关节；8-第六关节。

具体实施方式

下面结合附图对本发明做进一步的说明。

图1所示的冗余度双机械臂多指标协调运动规划方法主要由目标问题、左机械臂的多指标二次型优化方案和右机械臂的多指标二次型优化方案、左机械臂的二次规划问题和右机械臂的二次规划问题、统一的二次规划问题、简化的基于线性变分不等式原对偶神经网络求解器、下位机控制器和双(左右)机械臂组成。

首先基于目标问题，采用二次型优化方案，先将左右机械臂各自的逆运动学求解在速度层上设计为最小化和其中受约束于 受约束于 欲优化的性能指标和为通过加权因子α_L/R和β_L/R对最小速度范数指标重复运动指标(其中c_L/R＝λ_L/R(θ_L/R-θ_L/R(0))，λ_L/R>0是用来控制关节位移幅值的正设计参数)和最小速度无穷范数指标这三个指标进行加权组合而成，其数学表达式如下：

接着，将上述的左右机械臂各自的二次型优化方案转化为各自的二次规划问题，再将左右机械臂各自的二次规划问题转化为统一的二次规划问题。然后使用简化的基于线性变分不等式原对偶神经网络求解器求解，并将求解结果传递给下位机控制器驱动双(左右)机械臂协调运动。

图2所示的双机械臂模型主要由左机械臂1和右机械臂2组成，每个机械臂均有6个关节，即第一关节3、第二关节4、第三关节5、第四关节6、第五关节7和第六关节8，两个机械臂可以同时且协调地执行某一任务。

首先基于目标问题，左机械臂多指标二次型优化方案的逆运动学求解在速度层上可设计为：

其中，代表左机械臂运动欲优化的性能指标,其数学表达式为其中，C_L＝λ_L(θ_L-θ_L(0))，λ_L>0是用来控制关节位移幅值的正设计参数，α_L、β_L和(1-α_L-β_L)分别为最小速度范数指标、重复运动指标和最小速度无穷范数指标的加权参数；等式约束表示左机械臂的运动学方程，J_L表示左机械臂的雅可比矩阵，表示左机械臂关节速度，表示左机械臂末端执行器的速度矢量；不等式约束和分别表示左机械臂的关节角度极限和关节角速度极限。

右机械臂多指标二次型优化方案的逆运动学求解在速度层上可设计为：

其中，代表右机械臂运动欲优化的性能指标,其数学表达式为其中，c_R＝λ_R(θ_R-θ_R(0))，λ_R>0是用来控制关节位移幅值的正设计参数，α_R、β_R和(1-α_R-β_R)分别为最小速度范数指标、重复运动指标和最小速度无穷范数指标的加权参数；等式约束表示右机械臂的运动学方程，J_R表示右机械臂的雅可比矩阵，表示右机械臂关节速度，表示右机械臂末端执行器的速度矢量；不等式约束分别表示右机械臂的关节角度极限和关节角速度极限。

接着，将上述左右机械臂的多指标二次型优化方案转化为各自的标准二次规划问题。左机械臂的二次规划问题可写为如下形式：

s.t.A_Lx_L＝b_L， (10)

C_Lx_L≤d_L， (11)

令则决策变量x_L可以被定义为而系数矩阵或向量为

I为单位矩阵， 元素都为1的相应维数向量)，d_L＝0；不等式约束(3)转换到速度层上的表达式为其中系数μ_L>0是用来调节关节角速度的可行域，令和可以得到新的双端约束(12)，其中是足够大的常数，用于数值上替代无穷大+∞)。

右机械臂的二次规划问题类似地可写为如下形式：

s.t.A_Rx_R＝b_R， (14)

C_Rx_R≤d_R， (15)

令则决策变量x_R可以被定义为而系数矩阵或向量为

I为单位矩阵， 元素都为1的相应维数向量)，d_R＝0；不等式约束(3)转换到速度层上的表达式为其中系数μ_R>0是用来调节关节角速度的可行域，令可以得到新的双端约束(12)，其中是足够大的常数，用于数值上替代无穷大+∞)。

然后，将上述左右机械臂各自的二次规划问题转化为一个统一的二次规划问题。统一的二次规划问题可以写成如下形式：

min.z^TKz/2+w^Tz， (17)

s.t.Gz＝h， (18)

Dz≤e， (19)

z^-≤z≤z⁺； (20)

其中决策变量系数矩阵或向量

得到上述的统一二次规划问题(17)-(20)式后，本发明的求解方法是采用简化的基于线性变分不等式的原对偶神经网络算法来实时求解此二次规划问题。

首先，将统一的二次规划问题转化为一个线性投影方程P_Ω[y-(My+q)]-y＝0的求解，其中P_Ω[·]为空间R^2(3n+m+1)到集合 的分段线性投影算子，P_Ω(y)的第i个计算单元定义为：

m为单个机械臂的笛卡尔空间维数，n为单个机械臂的关节空间维数，y∈R^2(3n+m+1)表示原对偶变量，y^-表示原对偶变量下极限，y⁺表示原对偶变量上极限，原对偶变量y及其上下限定义如下：

u∈R^2m是对应于等式约束Gz＝h的对偶决策向量，v∈R⁴ⁿ是对应于不等式约束Dz≤e的对偶决策向量，而扩展矩阵M、q分别定义如下：

接着，用简化的基于线性变分不等式的原对偶神经网络来求解上述线性变分不等式问题及二次规划问题：

最后将上述计算得到的角速度传送给左右机械臂各自的下位机控制器从而控制左右机械臂同时协调地运动，实现本发明的方法。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (5)

1.一种冗余度双机械臂的多指标协调运动规划方法，其特征在于，包括如下步骤：

1)基于目标问题，通过上位机采用二次型优化方案在速度层上分别对双机械臂进行逆运动学解析，设计的优化性能指标为最小速度二范数、重复运动和最小速度无穷范数这三个指标，通过两个加权因子加权组合而成新的指标，受约束于双机械臂各自的运动学方程、关节角极限和关节角速度极限；

2)将步骤1)中双机械臂各自的二次型优化方案转化为标准的二次规划问题；

3)将步骤2)中双机械臂各自的关节角、雅克比矩阵、末端轨迹、关节角极限和关节角速度极限进行组合，生成组合的关节角、组合雅克比矩阵、组合末端轨迹、组合关节角极限和组合关节角速度极限，从而将两个二次规划问题统一成一个二次规划问题；

4)将步骤3)中统一的二次规划问题用简化的基于线性变分不等式的原对偶神经网络求解器进行求解；

5)将步骤4)的求解结果传递给下位机控制器驱动双机械臂的运动。

2.根据权利要求1所述的冗余度双机械臂的多指标协调运动规划方法，其特征在于，所述步骤1)中双机械臂各自的二次型优化冗余度解析方案将机械臂的逆运动学求解在速度层上设计为：

左机械臂：最小化受约束于 其中代表左机械臂欲优化的性能指标函数，θ_L和分别表示左机械臂的关节角度和关节角速度，等式约束表示左机械臂的运动学方程，J_L表示左机械臂末端运动轨迹r_L关于关节角度θ_L的雅可比矩阵，表示机械臂末端的速度矢量，分别表示左机械臂的关节角度极限、关节角速度极限，和分别表示左机械臂关节角度的上下限，和分别表示左机械臂关节角速度的上下限；

右机械臂：最小化受约束于 其中表示右机械臂的优化指标函数、运动学方程、关节角极限和关节角速度极限，J_R表示右机械臂末端运动轨迹r_R关于关节角度θ_R的雅可比矩阵，表示右机械臂末端的速度矢量，和分别表示右机械臂关节角度的上下限，和分别表示右机械臂关节角速度的上下限；

所述欲优化的性能指标和为双机械臂冗余度解析方案的优化判据，通过加权因子α_L/R和β_L/R对最小速度范数指标重复运动指标和最小速度无穷范数指标这三个指标进行加权组合而成，其中c_L/R＝λ_L/R(θ_L/R-θ_L/R(0))，λ_L/R＞0是用来控制关节位移幅值的正设计参数，θ_L/R(0)表示机械臂关节角度的初始值，其数学表达式如下：

。

3.根据权利要求2所述的冗余度双机械臂的多指标协调运动规划方法，其特征在于，所述步骤2)中双机械臂各自的二次型优化转化为标准二次规划问题具体为：

左机械臂的二次规划问题为如下：

s.t.A_Lx_L＝b_L，

C_Lx_L≤d_L，

其中表示左机械臂的决策变量，表示左机械臂的关节角度向量的无穷范数，表示左机械臂的二次系数矩阵，I为单位矩阵，表示左机械臂的线性系数向量，Λ_L＝2λ_L，A_L＝[J_L 0]表示左机械臂的等式约束的线性系数矩阵，表示左机械臂的等式约束的常向量，表示左机械臂的不等式约束矩阵的线性系数矩阵，1_v＝[1，…1]^T元素都为1的相应维数向量，d_L＝0表示左机械臂的不等式约束的常向量，和表示左机械臂的决策变量的上下限；

右机械臂的二次规划问题为如下：

s.t.A_Rx_R＝b_R，

C_Rx_R≤d_R，

其中表示右机械臂的决策变量，表示右机械臂的关节角度向量的无穷范数，表示右机械臂的二次系数矩阵，I为单位矩阵，表示右机械臂的线性系数向量，Λ_R＝2λ_R，A_R＝[J_R 0]表示右机械臂的等式约束的线性系数矩阵，表示右机械臂的等式约束的常向量，表示右机械臂的不等式约束矩阵的线性系数矩阵，1_v＝[1，…1]^T元素都为1的相应维数向量，d_R＝0表示右机械臂的不等式约束的常向量，和表示右机械臂的决策变量的上下限。

4.根据权利要求3所述的冗余度双机械臂的多指标协调运动规划方法，其特征在于，所述步骤3)中将两个二次规划问题统一成一个二次规划问题具体包括：

双机械臂二次规划问题统一为一个二次规划问题如下：

min.z^TKz/2+w^Tz，

s.t.Gz＝h，

Dz≤e，

z^-≤z≤z⁺，

其中 表示统一的决策变量，表示统一的二次规划问题的二次系数矩阵，表示统一的二次规划问题的线性系数向量，表示统一的等式约束的线性系数矩阵，表示统一的等式约束的常向量，表示统一的不等式约束的线性系数矩阵，e＝0表示统一的不等式约束的常向量，表示统一的决策变量上限，表示统一的决策变量下限。

5.根据权利要求4所述的冗余度双机械臂的多指标协调运动规划方法，其特征在于，所述步骤4)将统一后标准的二次规划问题用简化的基于线性变分不等式的原对偶神经网络求解器进行求解具体包括：

41)将统一的二次规划问题转化为一个线性投影方程P_Ω[y-(My+q)]-y＝0的求解，其中P_Ω[·]为空间R^2(3n+m+1)到集合 的分段线性投影算子，P_Ω(y)的第i个计算单元定义为：

其中m为单个机械臂的笛卡尔空间维数，n为单个机械臂的关节空间维数，y∈R^2(3n+m+1)表示原对偶变量，y^-表示原对偶变量下极限，y⁺表示原对偶变量上极限，原对偶变量y及其上下限定义如下：

u∈R^2m是对应于等式约束G_z＝h的对偶决策向量，v∈R⁴ⁿ是对应于不等式约束Dz≤e的对偶决策向量，而扩展矩阵M、q分别定义如下：

42)用简化的基于线性变分不等式的原对偶神经网络来求解线性变分不等式问题及二次规划问题：

。