CN104991448B - 一种基于构型平面的水下机械臂运动学的求解方法 - Google Patents

Abstract

本发明提供的是一种基于构型平面的水下机械臂运动学的求解方法。本发明包括：输入水下机械臂关节参数：输入水下机械臂关节参数，进行机械臂关节分析，将已知的机器人工作构形按照关节模块的形式进行分解，对机器人关节根据运动形式分解成相应的基本关节的形式输入机器人目标点的位置矩阵；基本关节建模：对组成空间机械臂进行基本运动关节进行运动学建模，并进行归一化处理，形成统一形式的建模方法。本发明既可以克服传统解析方法求解的复杂性和多解难以剔除的缺点，又克服了通用迭代方法实时性差和精度不高的问题，能够快速、准确的实现机器人逆运动学的求解，满足水下机械臂实际应用需要。

Description

一种基于构型平面的水下机械臂运动学的求解方法

技术领域

本发明提供的是一种基于构型平面的水下机械臂运动学的求解方法。

背景技术

随着工业机器人技术的广泛的应用，空间6R串联机构的应用具有重要的意义。串联机构运动学逆解是串联机器人控制计算的先决条件，它直接关系到机器人离线编程、轨迹规划、实时控制等工作，在机器人学中占有重要地位，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机器人末端执行器按空间位姿进行编程控制(如直线轨迹和圆弧轨迹等)。

串联机构运动学中，空间6R串联机构的运动学逆解是最困难的，该问题与空间机构学中的单环7R机构运动学逆解属于同一问题，曾被喻为空间机构运动分析中的珠穆朗玛峰。各国学者对此开展很多有益的探索和研究。空间6R串联机构的运动学逆解求解方法分为解析形式和数值形式。一般6R串联操作臂运动学逆解因涉及结构参数多、解的非线性和耦合性以及需要求解代数方程等问题而变得难于得到解析解。解析解法适用于具有特殊几何结构参数的6R串联操作臂，可以应用矢量、螺旋或李代数方法得到理论解，这种方法具有计算结果准确、能够得到全部解等优点，但需要进行大量的代数和矩阵运算，推导过程比较复杂，并且有解的条件是操作臂的位置和姿态具有解耦特征或其特征多项式的次数小于等于4。廖启征将倍四元数引入空间串联机器人运动学研究当中，解决了一个经典的6R机器人的逆运动学问题。2006年，有学者提出把串联运动链拆成几个简单部分的组合，但该方法只适于某些解耦的特殊情况。以往用于串联机构位置逆解数学建模的方法主要有D-H矩阵法、球面三角法、实矩阵法、对偶数法等，得到了各不相同的逆解算法，不具有通用性。Raghavan和Roth通过矢量运算由6个逆运动学等式构造14个基础方程，消元运算后得到一元24次方程，求出最多16组逆运动学解，但存在8个增根.Manocha采用24阶矩阵特征分解方法对Raghavan的算法进行改进，提高了逆运动学解算的稳定性和精度。为解决空间7R机构的位移分析难题，分别采用复数方法和矩阵运算构造10个基础方程，进而得到一元16次方程，消除了增根。借鉴前期学者研究成果，将6R串联型机器人逆运动学求解问题分为两类：封闭解法求解满足Pieper准则的6R机器人的逆运动学问题；矢量计算和符号运算将Manocha得到的目标矩阵从24阶降低到16阶，并以矩阵特征分解方法提高一般6R机器人逆运动学求解的效率和稳定性，并组合牛顿-拉夫森迭代算法解决非Pieper准则的6R机器人的逆运动学问题。

而对于6R串联型机器人实际运动作业下，仅仅需要一种能够实时快速找到满足一定工作要求(如避障和动力学要求)和末端工作点位姿要求逆运动学解。为此产生了很多种数值形式的串联机器人逆运动学求解方法。一个常用的数值方法是将6R串联操作臂各关节的D-H参数中的径向参数ai、αi和轴向参数si、θi分离，运用双四元数方法或李代数方法将6R串联操作臂运动学正解矩阵构造成两个独立的齐次变换线性方程组，通过将两个方程组联立逐次迭代或消元而得到关于各关节转角的16组运动学逆解。如QIAO等运用双四元数理论得到了一般6R串联操作臂运动学逆解的数值解；ROCCO等运用李群、李代数等方法也得到了该问题的数值解。另一个比较常用的数值方法是将遗传算法和神经网络等工具引入6R操作臂的运动学逆解问题中，通过设定关节转角进给值等约束条件，以运动学正解和目标值之间的差值最小化为目标函数，采用上述算法求解最佳拟合的关节转角进给值。如CHIDDARWAR等比较了预测型与常规型神经网络算法对求解效率的影响；等提出了一种考虑关节速度和加速度的3自由度机器人运动学逆解神经网络算法；KALRA等提出了一种基于遗传算法的6自由度工业机器人运动学逆解算法；HAMMOUR等采用连续传算法规划了6R操作臂的运动轨迹；ZHA利用末端执行器位置和姿态矢量构成的曲面特征，通过遗传算法搜寻该曲面最小特征值而获得最优轨迹规划等。

本文借鉴其他方法的研究经验，以6R转动关节机器人为研究对象，运用构型平面的方法，解决机器人逆运动学通用快速求解问题，为机器人关节控制提供运动参数。

发明内容

本发明的目的在于提供一种克服传统解析方法的求解机器人构形的局限性和专一性，也克服了通用的迭代方法非实时性和精度问题，能够解决6R机器人逆运动学求解问题的基于构型平面的水下机械臂运动学的求解方法。

本发明的主要目的是这样实现的：

一种基于构型平面的水下机械臂运动学的求解方法包括如下步骤：

(1)输入水下机械臂关节参数：输入水下机械臂关节参数，进行机械臂关节分析，将已知的机器人工作构形按照关节模块的形式进行分解，对机器人关节根据运动形式分解成相应的基本关节的形式输入机器人目标点的位置矩阵；

(2)基本关节建模：对组成空间机械臂进行基本运动关节进行运动学建模，并进行归一化处理，形成统一形式的建模方法：

(2.1)摇摆模块建模

摇摆模块运动模型矩阵：

式中β_yb—摇摆模块两部分旋转相对角度；h_yb—摇摆中心到下一个模块连接面的长度；

(2.2)移动模块建模

移动模块运动模型矩阵：

式中H_yd—移动模块中心到下一模块中心的长度；h_yd—移动模块的移动量；

(2.3)回转模块建模

回转模块运动模型矩阵：

式中θ_hz—回转模块两部分旋转相对角度；l_hz—回转中心到下一个模块中心的长度；

(2.4)连接模块建模

连接模块运动模型矩阵：

式中H_lj—模块长度；

(2.5)模块的统一表达形式

基于前面各模块的建模方法，建立可重构机器人模块的统一表达方式：

式中θ为该模块是回转模块时的回转角度，若为其他模块时为零；β为该模块是摇摆模块时的摇摆角度，若为其他模块时为零；h为该模块是摇摆模块时的连接长度，若为其他模块时为零；l为该模块是回转模块或连接模块时的连接长度，若为其他模块时为零；w为该模块是移动模块时的移动量，若为其他模块时为零；

(2.6)通过基本模块的统一表达方式，在已知模块类型的情况下，建立整个机器人运动学形式：

T(总)＝T₁*T₂…T_n；

(3)构形平面分解：

对水下机械臂进行构形平面分解，对多角度连接模块进行处理，形成合理分解，将机械臂的位置和姿态进行有效分解；

机械臂关键关节中心位置求解、通过位置求解其中三个关节运动量、通过姿态求解剩余三个关节运动量、运动量校核、输入控制器进行控制；

求解关键关节中心的三维空间位置坐标，以这些位置坐标和分解的两个构形平面求解组成构形平面的关节广义运动量；

求解关键关节中心点：

目标点位姿矩阵为：

2关节和5关节的中心位置

TS2＝[0 0 d1+d2]

将2关节的中心设为新的原点，5关节的中心设为目标点，则新的目标点的位置表示为

分解的两个构形平面模型表达式

构形平面简化的转化矩阵为：

利用位置关系求解几个关节的广义运动量

设p'_x＝p_x-d₆*a_x

p'_y＝p_y-d₆*a_y

p'_z＝p_z-d₆*a_z-d₁-d₂

TGX1*TGX2*[0 0 0 1]^T＝[p'_x p'_y p'_z 1]^T

可得：

利用三角函数关系，进行消元处理，得到如下公式：

令

得到仅有θ₄的等式：

经过处理可得到θ₃表达式。

进一步可计算得到θ₂，

则

步骤4：利用姿态关系求解剩余关节的广义运动量

根据此式可求出θ₅，n'_zcθ₅-a'_zsθ₅＝a_z。

根据此式可求出θ₁，n'_xcθ₅-a'_xsθ₅＝a_xcθ₁+a_ysθ₁。

-n'_xsθ₅cθ₆+o'_xsθ₆-a'_xcθ₅cθ₆＝n_xcθ₁+n_ysθ₁，

θ₆，(-n'_xsθ₅-a'_xcθ₅)cθ₆+o'_xsθ₆＝n_xcθ₁+n_ysθ₁

根据已求解的三个关节广义运动量，利用机械臂末端姿态要求，建立姿态关系等式，求解剩余运动关节的广义运动量，并对该运动量进行校核；

将得到的水下机械臂的广义运动量输给到机器人控制器，进而实现机器人关节控制。

本发明的有益效果在于：

本发明将水下6R串联形式的机械臂分解成依次连接的构形平面的方法，通过构形平面的组合和分解实现了机械臂的位置和姿态的耦合性分解。该方法既可以克服传统解析方法求解的复杂性和多解难以剔除的缺点，又克服了通用迭代方法实时性差和精度不高的问题，能够快速、准确的实现机器人逆运动学的求解，满足水下机械臂实际应用需要。

附图说明

图1是机器人基本运动关节。

图2是摇摆模块运动学模型。

图3是移动模块运动学模型。

图4是回转模块运动学模型。

图5是水下机械臂结构简图。

图6是水下机械臂建模图。

图7是求解过程示图。

具体实施方式

下面结合附图对本发明做更详细的描述。

包括输入水下机械臂关节参数、基本关节建模、构形平面分解、机械臂关键关节中心位置求解、通过位置求解其中三个关节运动量、通过姿态求解剩余三个关节运动量、运动量校核、输入控制器进行控制；

输入水下机械臂关节参数，进行机械臂关节分析，将已知的机器人工作构形按照关节模块的形式进行分解，对机器人关节根据运动形式分解成相应的基本关节的形式输入机器人目标点的位置矩阵

对组成空间机械臂进行基本运动关节进行运动学建模，并进行归一化处理，形成统一形式的建模方法。

对该水下机械臂进行构形平面分解，对多角度连接模块进行处理，形成合理分解，将机械臂的位置和姿态进行有效分解。

求解关键关节中心的三维空间位置坐标，以这些位置坐标和分解的两个构形平面求解组成构形平面的关节广义运动量。

根据已求解的三个关节广义运动量，利用机械臂末端姿态要求，建立姿态关系等式，求解剩余运动关节的广义运动量，并对该运动量进行校核。

最后将得到的水下机械臂的广义运动量输给到机器人控制器，进而实现机器人关节控制。

本发明还可以包括：对空间的基本关节进行建模分析整理

(1)摇摆模块建模

摇摆模块运动数学模型矩阵：

式中β_yb—摇摆模块两部分旋转相对角度；h_yb—摇摆中心到下一个模块连接面的长度。

(2)移动模块建模

移动模块运动数学模型矩阵：

式中H_yd—移动模块中心到下一模块中心的长度；h_yd—移动模块的移动量。

(3)回转模块建模

回转模块运动数学模型矩阵：

式中θ_hz—回转模块两部分旋转相对角度；l_hz—回转中心到下一个模块中心的长度。

(4)连接模块建模

连接模块运动数学模型矩阵：

式中H_lj—模块长度。

(5)模块的统一表达形式

基于前面各模块的建模方法，建立可重构机器人模块的统一表达方式：

式中θ为该模块是回转模块时的回转角度，若为其他模块时为零；β为该模块是摇摆模块时的摇摆角度，若为其他模块时为零；h为该模块是摇摆模块时的连接长度，若为其他模块时为零；l为该模块是回转模块或连接模块时的连接长度，若为其他模块时为零；w为该模块是移动模块时的移动量，若为其他模块时为零。

通过基本模块的统一表达方式，在已知模块类型的情况下，就可建立整个机器人运动学形式：

T(总)＝T₁*T₂…T_n (6)

本发明还可以包括：

步骤1：求解关键关节中心点：

假设目标点位姿矩阵为：

关节2和关节5的中心位置

TS2＝[0 0 d1+d2]

将运动关节2的中心设为新的原点，运动关节5的中心设为目标点，则新的目标点的位置可表示为，

步骤2：分解的两个构形平面模型表达式

构形平面1简化的转化矩阵为：

步骤3：利用位置关系求解几个关节的广义运动量

设p'_x＝p_x-d₆*a_x

p'_y＝p_y-d₆*a_y

p'_z＝p_z-d₆*a_z-d₁-d₂

TGX1*TGX2*[0 0 0 1]^T＝[p'_x p'_y p'_z 1]^T

可得：

利用三角函数关系，进行消元处理，得到如下公式：

令

得到仅有θ₄的等式：

经过处理可得到θ₃表达式。

进一步可计算得到θ₂，

令

则

步骤4：利用姿态关系求解剩余关节的广义运动量

根据此式可求出θ₅，n'_zcθ₅-a'_zsθ₅＝a_z。

根据此式可求出θ₁，n'_xcθ₅-a'_xsθ₅＝a_xcθ₁+a_ysθ₁。

-n'_xsθ₅cθ₆+o'_xsθ₆-a'_xcθ₅cθ₆＝n_xcθ₁+n_ysθ₁，

此式可化简成下式：

根据此式可求出θ₆，(-n'_xsθ₅-a'_xcθ₅)cθ₆+o'_xsθ₆＝n_xcθ₁+n_ysθ₁。

实施1，结合图1，机器人的空间机构主要由低副机构组成。低副机构主要有转动副(R)、移动副(P)、螺旋副(H)、圆柱副(C)、平面副(E)、球面副(S)以及虎克铰(HO)或通用关节(U)。其中转动副、移动副和螺旋副的自由度为1，圆柱副(PR)和通用关节(RR)自由度为2，平面副(PPR)与球面副(RRR)自由度为3。

实施2，结合图2，摇摆模块建模：将模块的坐标系建立在模块的摇摆中心处，模块转动时，模块的坐标系相对于上一个模块的连接坐标系不变，而与下一个模块的连接坐标系相对模块的坐标系发生变化。摇摆模块运动数学模型矩阵：

式中β_yb—摇摆模块两部分旋转相对角度；

h_yb—摇摆中心到下一个模块连接面的长度。

实施3，结合图3，移动模块建模：将模块的坐标系建立在移动模块的固定部分和移动部分连接面，模块移动时，模块的坐标系相对于上一个模块的连接坐标系不变，而与下一个模块的连接坐标系相对模块的坐标系发生变化。而为了模块间的运动学重构，两个连接面上的坐标矩阵采用标准形式。模块的运动学模型为式(2)。

移动模块运动数学模型矩阵：

式中H_yd—移动模块中心到下一模块中心的长度；

h_yd—移动模块的移动量。

实施4，结合图4，回转模块的坐标系建立在模块相对转动面处，模块转动时，模块的坐标系相对于上一个模块的连接坐标系不变，而与下一个模块的连接坐标系相对模块的坐标系发生变化。回转模块运动数学模型矩阵：

式中θ_hz—回转模块两部分旋转相对角度；

l_hz—回转中心到下一个模块中心的长度。

实施5，结合图5，利用构型平面的方法对一个6R水下机械臂进行运动学解算，水下机械臂模型图见附图5，机械臂是由手爪(1)、腕(2)、关节5(3)、关节4(4)、关节3(5)、关节2(6)、关节1(7)和底座(8)组成。所选的水下机械臂是一个液压驱动形式的机械手，前端安装有一个手爪来完成夹持、操作物体的功能。水下机械臂由六个自由度组成，从基座开始依次为回转、上下摆动、左右摆动、上下摆动、左右摆动、回转。框架材料为铝合金，工作环境为水下3000米，伸长距离为1.5m，各关节运动范围如表1所示。

表1各关节运动范围及结构参数

运动形式	执行器	动作范围	连接关节	长度(cm)
					肩关节左右摆动	油缸	120°	d1	80
大臂上下摆动	油缸	120°	d2	100
					肘部左右摆动	油缸	120°	d3	390
小臂上下摆动	油缸	120°	d4	180
					手腕左右摆动	油缸	120°	d5	305

腕部回转

旋转马达

360°

d6

185

实施6，结合图6，对水下机械臂运动学建模如附图6所示，对机械臂进行运动学建模，各个关节建模如下：

实施7，结合图7，具体求解过程。

假设目标点位姿矩阵为：

关节2和关节5的中心位置

TS2＝[0 0 d1+d2]

将运动关节2的中心设为新的原点，运动关节5的中心设为目标点，则新的目标点的位置可表示为，

由所设计的两个构形平面形成的新的坐标

构形平面1简化的转化矩阵为：

设p'_x＝p_x-d₆*a_x

p'_y＝p_y-d₆*a_y

p'_z＝p_z-d₆*a_z-d₁-d₂

TGX1*TGX2*[0 0 0 1]^T＝[p'_x p'_y p'_z 1]^T

可得：

利用三角函数关系，进行消元处理，得到如下公式：

令

得到仅有θ₄的等式：

经过处理可得到θ₃表达式。

进一步可计算得到θ₂，

令

则

根据此式可求出θ₅，n'_zcθ₅-a'_zsθ₅＝a_z。

根据此式可求出θ₁，n'_xcθ₅-a'_xsθ₅＝a_xcθ₁+a_ysθ₁。

-n'_xsθ₅cθ₆+o'_xsθ₆-a'_xcθ₅cθ₆＝n_xcθ₁+n_ysθ₁，

此式可化简成下式：

根据此式可求出θ₆，(-n'_xsθ₅-a'_xcθ₅)cθ₆+o'_xsθ₆＝n_xcθ₁+n_ysθ₁。

Claims (1)

1.一种基于构型平面的水下机械臂运动学的求解方法，其特征是，包括如下步骤：

(1)输入水下机械臂关节参数：输入水下机械臂关节参数，进行机械臂关节分析，将已知的机器人工作构形按照关节模块的形式进行分解，对机器人关节根据运动形式分解成相应的基本关节的形式输入机器人目标点的位置矩阵；

(2)基本关节建模：对组成空间机械臂进行基本运动关节进行运动学建模，并进行归一化处理，形成统一形式的建模方法：

(2.1)摇摆模块建模

摇摆模块运动模型矩阵：

式中β_yb—摇摆模块两部分旋转相对角度；h_yb—摇摆中心到下一个模块连接面的长度；

(2.2)移动模块建模

移动模块运动模型矩阵：

式中H_yd—移动模块中心到下一模块中心的长度；h_yd—移动模块的移动量；

(2.3)回转模块建模

回转模块运动模型矩阵：

式中θ_hz—回转模块两部分旋转相对角度；l_hz—回转中心到下一个模块中心的长度；

(2.4)连接模块建模

连接模块运动模型矩阵：

式中H_lj—模块长度；

(2.5)模块的统一表达形式

基于前面各模块的建模方法，建立可重构机器人模块的统一表达方式：

式中θ为该模块是回转模块时的回转角度，若为其他模块时为零；β为该模块是摇摆模块时的摇摆角度，若为其他模块时为零；h为该模块是摇摆模块时的连接长度，若为其他模块时为零；l为该模块是回转模块或连接模块时的连接长度，若为其他模块时为零；w为该模块是移动模块时的移动量，若为其他模块时为零；

(2.6)通过基本模块的统一表达方式，在已知模块类型的情况下，建立整个机器人运动学形式：

T(总)＝T₁*T₂…T_n；

(3)构形平面分解：

对水下机械臂进行构形平面分解，对多角度连接模块进行处理，形成合理分解，将机械臂的位置和姿态进行有效分解；

机械臂关键关节中心位置求解、通过位置求解其中三个关节运动量、通过姿态求解剩余三个关节运动量、运动量校核、输入控制器进行控制；

求解关键关节中心的三维空间位置坐标，以这些位置坐标和分解的两个构形平面求解组成构形平面的关节广义运动量；

求解关键关节中心点：

目标点位姿矩阵为：

2关节和5关节的中心位置

TS2＝[0 0 d1+d2]

将2关节的中心设为新的原点，5关节的中心设为目标点，则新的目标点的位置表示为TD1＝[p_x-d6*a_x p_y-d6*a_y p_z-d6*a_z-d1-d2]

分解的两个构形平面模型表达式

构形平面简化的转化矩阵为：

利用位置关系求解几个关节的广义运动量

设p'_x＝p_x-d₆*a_x

p'_y＝p_y-d₆*a_y

p'_z＝p_z-d₆*a_z-d₁-d₂

TGX1*TGX2*[0 0 0 1]^T＝[p'_x p'_y p'_z 1]^T

可得：

利用三角函数关系，进行消元处理，得到如下公式：

令

得到仅有θ₄的等式：

经过处理可得到θ₃表达式；

进一步可计算得到θ₂，

则

(4)利用姿态关系求解剩余关节的广义运动量

根据此式可求出θ₅，n'_zcθ₅-a'_zsθ₅＝a_z；

根据此式可求出θ₁，n'_xcθ₅-a'_xsθ₅＝a_xcθ₁+a_ysθ₁；

-n'_xsθ₅cθ₆+o'_xsθ₆-a'_xcθ₅cθ₆＝n_xcθ₁+n_ysθ₁，

θ₆，(-n'_xsθ₅-a'_xcθ₅)cθ₆+o'_xsθ₆＝n_xcθ₁+n_ysθ₁

根据已求解的三个关节广义运动量，利用机械臂末端姿态要求，建立姿态关系等式，求解剩余运动关节的广义运动量，并对该运动量进行校核；

将得到的水下机械臂的广义运动量输给到机器人控制器，进而实现机器人关节控制。