CN107378952A

CN107378952A - 一种冗余度机械臂末端执行器姿态保持的解决方法

Info

Publication number: CN107378952A
Application number: CN201710700402.6A
Authority: CN
Inventors: 张智军; 朱徐鹏
Original assignee: South China University of Technology SCUT
Current assignee: South China University of Technology SCUT
Priority date: 2017-08-16
Filing date: 2017-08-16
Publication date: 2017-11-24
Anticipated expiration: 2037-08-16
Also published as: CN107378952B

Abstract

本发明公开了一种冗余度机械臂末端执行器姿态保持的解决方法，包括如下步骤：S1、给机械臂建立D‑H模型获得正运动学模型，并对其求导，转化为速度层，并建立目标轨迹等式约束指标；S2、测量机械臂任务平面的法向量在机械臂基坐标系中的数值；S3、通过建立的D‑H模型以及法向量建立两种末端执行器姿态保持等式约束指标；S4、将建立的目标轨迹等式约束指标以及两种末端执行器姿态保持等式约束指标，写为统一形式的二次规划问题；S5、将二次规划问题转化为线性变分不等式；S6、使用原对偶神经网络求解器求解线性变分不等式；S7、将原对偶神经网络求解出来的机械臂关节角控制量输出到机械以实现控制冗余度机械臂。

Description

一种冗余度机械臂末端执行器姿态保持的解决方法

技术领域

本发明涉及冗余度机器人的运动规划技术领域，具体涉及一种冗余度机械臂末端执行器姿态保持的解决方法。

背景技术

近年来，机械臂在复杂的环境中有着越来越广泛的应用：工业上的装配线，家庭和餐厅中的服务机器人，医疗机器人，灾后的救援机器人，等等。其中的种种应用都对机械臂或移动机械臂的灵巧性、各种指标的优化能力有着较高的要求。冗余度机械臂有着冗余的自由度，因而能在完成主任务的同时完成很多子任务。因此，相对于非冗余机械臂，冗余度机械臂有着更广阔的应用前景和研究价值。

对于冗余度机械臂，最基础的问题就是其逆运动学问题。逆运动学问题就是在笛卡尔坐标系下给定目标轨迹时，如何获得对应的各控制变量(移动平台驱动车轮的角速度及机械臂各关节角度)。对于冗余度机械臂，其逆运动学问题的解析解通常难以获得，因为其正运动学方程式是非线性的。一种较为常用的线性化简化方法是在速度层计算逆运动学方程。尽管已经线性化简化，但是由于冗余度机械臂的冗余特性，逆运动学需要求解的变量(控制变量)多于方程数量，所以最终会有无数的解。为了获得一个目标解，常用的方法是采用在一定指标下的最优化控制方法。

在机械臂的画图任务中，末端执行器姿态变形问题是十分影响画图效果的问题。即，当夹持在末端执行器的笔在整个画图过程中无法保持始终垂直于画图平面。在很多时候，末端执行器姿态变形将导致机械臂画图任务的失败。传统的解决末端执行器姿态变形问题的方式是基于欧拉角的算法。然而基于欧拉角的算法有很多缺点：如需要确保末端执行器与画笔是同时垂直于任务平面的，或者如果末端执行器夹持画笔时存在一个夹角，该方法需要测量并换算这个夹角在欧拉角空间中的表达式，以及计算任务平面的姿态。总的来说，传统的姿态保持算法十分复杂而且不便于实际使用。为了有效解决机械臂末端执行器姿态变形这一问题，目前亟待提出一种末端执行器姿态保持算法，能十分有效的解决末端执行器姿态变形问题。

发明内容

本发明的目的是为了解决现有技术中的上述缺陷，提供一种冗余度机械臂末端执行器姿态保持的解决方法，在明确已知任务平面的法向量时，末端执行器的姿态可能会沿着法向量旋转但不会发生倾斜变形，即实现末端执行器始终垂直于任务平面的技术效果。

本发明的目的可以通过采取如下技术方案达到：

一种冗余度机械臂末端执行器姿态保持的解决方法，所述的解决方法包括下列步骤：

S1、给机械臂建立D-H模型，并对其求导，转化为速度层，并建立目标轨迹等式约束指标；

S2、测量机械臂任务平面的法向量在机械臂基坐标系中的数值；

S3、将步骤S1中建立的D-H模型以及步骤S2中的法向量的数值按照本发明提出的算法的公式建立两种末端执行器姿态保持等式约束指标，选择其中一种约束指标使用；

S4、将步骤S1建立的目标轨迹等式约束指标，以及步骤S3建立的两种末端执行器姿态保持等式约束指标，写为统一形式的二次规划问题；

S5、将步骤S4中的二次规划问题转化为线性变分不等式；

S6、使用原对偶神经网络求解器求解步骤S5中的线性变分不等式；

S7、将步骤S6中原对偶神经网络求解出来的机械臂关节角控制变量输出到机械以实现控制冗余度机械臂。

进一步地，所述的步骤S1中建立的目标轨迹等式约束指标为：其中J_E为末端执行器的雅克比矩阵，为机械臂关节角的角速度，为速度层的目标轨迹。

进一步地，所述的步骤S3中的两种末端执行器姿态保持等式约束指标包括：三元素姿态保持指标和两元素姿态保持指标，

其中，所述的三元素姿态保持指标的建立过程具体如下：为保持末端执行器始终垂直于任务平面，应保证在任何时间t中，任务平面的法向量在末端执行器坐标系中的数值保持不变，即：

式中，向量和向量代表在t和0时刻的向量而向量是在基坐标系中的任务平面的法向量在末端执行器坐标系中的投影，其中，可由以下公式得到：

式中，为从末端执行器坐标系转化到机械臂基坐标系的旋转变化矩阵，为了便于写成规范的二次规划形式，两端对时间求导，得：

其中也就是如下等式：

其中J_ω表示向量的雅克比矩阵；

其中，所述的两元素姿态保持指标的建立过程具体如下：由旋转矩阵的性质因此中的任意两个元素固定，其中的另一个元素也同时固定，所以，只固定中的任意两个元素，通过定义为中的任意两个元素，可以得到如下简化形式：

其中，J_υ为向量的雅克比矩阵，表示向量[0 0]^T。

进一步地，所述的步骤S4中写为统一形式的二次规划问题具体为：

取最小二范数的二次规划指标，即：

在这里，代表欧拉二范数，即

其中，针对所述的三元素机械臂末端姿态保持指标，将其写为标准的QP形式，即：

其中，针对所述的二元素机械臂末端姿态保持指标，将其写为标准的QP形式，即：

进一步地，所述的步骤S5中将二次规划问题转化为线性变分不等式具体为：

为了解决二次规划问题，设定原对偶矢量u^*，满足以下条件：

其中是原对偶决定变量矢量，其中，矢量g代表等式的原对偶决定变量矢量，Ω为原对偶决定变量矢量的取值范围，其为一凸集，其中的各符号定义如下：

Ω＝{u|u^-≤u≤u⁺}

u⁺＝+∞,and u^-＝-∞

W＝E,and C＝0

上式中E为单位矩阵，其它变量与前文的定义一致；

分段线性变分不等式可以转化为以下分段线性投影方程：

P_Ω(u-(Mu+q))-u＝0

上式中函数P_Ω为一分段线性投影算子，将自变量投影到Ω。

另外矩阵M，矢量u与前文定义一致。

进一步地，所述的步骤S6中使用原对偶神经网络求解器求解线性变分不等式具体为：

采用基于线性变分不等式的原对偶神经网络：

其中，参数β被设置为正数，并且其值要设置得尽可能的大，这样将增加基于线性变分不等式的原对偶神经网络的收敛率。

本发明相对于现有技术具有如下的优点及效果：

本发明公开的冗余度机械臂末端执行器姿态保持的解决方法，通过设计两种末端执行器姿态保持等式约束指标，能十分有效的实现末端执行姿态保持，使得末端执行器在执行任务过程中保证末端执行器(画笔)始终垂直于固定的任意姿态的任务平面。

附图说明

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

图2为本发明中使用的冗余度机械臂及其末端执行器。

图中所示为：1-冗余度机械臂；2-冗余度机械臂末端执行器。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

图1所示的一种冗余度机械臂末端执行器姿态保持的解决方法，主要由建立的目标轨迹等式约束指标、测量机械臂任务平面的法向量在机械臂基坐标系中的数值、建立两种末端执行器姿态保持等式约束指标、将建立的目标轨迹等式约束指标，以及建立的两种末端执行器姿态保持等式约束指标，写为统一形式的二次规划问题、将二次规划问题转化为线性变分不等式具体包括、使用原对偶神经网络求解器求解线性变分不等式几个步骤组成。

建立的目标轨迹等式约束指标为：其中，J_E为末端执行器的雅克比矩阵，为机械臂关节角的角速度，为速度层的目标轨迹。

测量机械臂任务平面的法向量在机械臂基坐标系中的数值，即测量基坐标系中的任务平面的法向量的数值。

按照本发明提出的算法的公式建立两种末端执行器姿态保持等式约束指标的具体方法如下：三元素姿态保持指标：为保持画笔(即末端执行器)始终垂直于任务平面，应保证在任何时间t中，任务平面的法向量在末端执行器坐标系中的数值保持不变。即：

其中，向量和向量代表在t和0时刻的向量而向量是在基坐标系中的任务平面的法向量在末端执行器坐标系中的投影。其中，可由以下公式得到：

其中，为从末端执行器坐标系转化到机械臂基坐标系的旋转变化矩阵。为了便于写成规范的二次规划形式，对两端对时间求导，得：

其中也就是如下等式：

其中J_ω表示向量的雅克比矩阵。

两元素姿态保持指标：由旋转矩阵的性质因此中的任意两个元素固定，其中的另一个元素也同时固定。所以，只固定中的任意两个元素即可达到姿态保持效果。通过定义为中的任意两个元素，可以得到如下简化形式：

其中，J_υ为向量的雅克比矩阵。表示向量[0 0]^T。

将机械臂末端姿态保持指标、最小二范数的二次规划指标写为统一形式的二次规划问题具体为：最小二范数的二次规划指标，即：

在这里，代表欧拉二范数，即

第一种算法为三元素机械臂末端姿态保持指标，将其写为标准的QP形式，即：

第二种算法为二元素机械臂末端姿态保持指标，将其写为标准的QP形式，即：

将二次规划问题转化为线性变分不等式具体包括：为了解决二次规划问题，设定原对偶矢量u^*，满足以下条件：

其中是原对偶决定变量矢量，其中的矢量g代表了等式的原对偶决定变量矢量。Ω为原对偶决定变量矢量的取值范围，其为一凸集。其中的各符号定义如下：

Ω＝{u|u^-≤u≤u⁺}

u⁺＝+∞,and u^-＝-∞

W＝E,and C＝0

式中，E为单位矩阵。其它变量与前文的定义一致。

分段线性变分不等式可以转化为以下分段线性投影方程：

P_Ω(u-(Mu+q))-u＝0

这里，函数P_Ω为一分段线性投影算子，将自变量投影到Ω。

另外矩阵M，矢量u与前文定义一致。

使用原对偶神经网络求解器求解线性变分不等式具体包括：我们采用基于线性变分不等式的原对偶神经网络：

这里的参数β应被设置为正数，并且其值要设置得尽可能的大，这样将增加基于线性变分不等式的原对偶神经网络的收敛率。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims

1.一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的解决方法包括下列步骤：

S5、将步骤S4中的二次规划问题转化为线性变分不等式；

2.根据权利要求1所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S1中建立的目标轨迹等式约束指标为：其中J_E为末端执行器的雅克比矩阵，为机械臂关节角的角速度，为速度层的目标轨迹。

3.根据权利要求1所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S3中的两种末端执行器姿态保持等式约束指标包括：三元素姿态保持指标和两元素姿态保持指标，

其中也就是如下等式：

其中J_ω表示向量的雅克比矩阵；

其中，所述的两元素姿态保持指标的建立过程具体如下：由旋转矩阵的性质因此中的任意两个元素固定，其中的另一个元素也同时固定，所以，只固定中的任意两个元素即可达到姿态保持效果，通过定义为中的任意两个元素，可以得到如下简化形式：

其中，J_υ为向量的雅克比矩阵，表示向量[0 0]^T。

4.根据权利要求3所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S4中写为统一形式的二次规划问题具体为：

取最小二范数的二次规划指标，即：

在这里，代表欧拉二范数，即

。

5.根据权利要求4所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S5中将二次规划问题转化为线性变分不等式具体为：

Ω＝{u|u^-≤u≤u⁺}

u⁺＝+∞,and u^-＝-∞

W＝E,and C＝0

上式中E为单位矩阵，其它变量与前文的定义一致；

分段线性变分不等式可以转化为以下分段线性投影方程：

P_Ω(u-(Mu+q))-u＝0

上式中函数P_Ω为一分段线性投影算子，将自变量投影到Ω，

其中，

另外矩阵M，矢量u与前文定义一致。

6.根据权利要求5所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S6中使用原对偶神经网络求解器求解线性变分不等式具体为：

采用基于线性变分不等式的原对偶神经网络：

其中，参数β被设置为正数，并且其值要设置得尽可能的大，这样将增加基于线性变分不等式的原对偶神经网络的收敛率，按照该公式，原对偶神经网络将实现全局收敛，其原对偶决定变量矢量将收敛到期望的值，将原对偶决定变量矢量做积分处理后即可得到冗余度机械臂关节控制向量θ。