CN110561441B - 一种冗余度机械臂位姿控制的单94lvi迭代算法 - Google Patents

Abstract

本发明提供一种冗余度机械臂位姿控制的单94LVI迭代算法，该算法实现了对于机械臂末端执行器位置和姿态的同时控制，有效地消除了机械臂超越角度极限和角速度极限从而造成物理损坏的可能性，同时本发明的算法只需要一次迭代，能有效地减少计算时间，在同样的时间内能够实现更多次和更精确的控制，因此具有较强的适用性、安全性和实用性。

Description

一种冗余度机械臂位姿控制的单94LVI迭代算法

技术领域

本发明涉及机械臂运动规划及控制技术领域，更具体地，涉及一种冗余度机械臂位姿控制的单94LVI迭代算法。

背景技术

随着科技的发展，机器人将会在未来生产生活中扮演重要角色。作为机器人的重要组成部分，冗余度机械臂是自由度大于任务空间所需最少自由度的一种类手臂装置，也被视为一种简化的机器人，已经受到了国内外众多研究者的关注。目前，冗余度机械臂已经在许多工业生产中被使用来代替人类完成一些重要的工作。现有的冗余度机械臂控制算法主要针对末端执行器的位置控制，而忽略了对其姿态的控制，但是姿态控制在许多场景下都是必要的。例如，在焊接和钻孔任务中，末端执行器的位置固然重要，但是其姿态(方向)也同样重要，否则也是不能成功完成任务的。同时，考虑到机械臂物理极限，才能够保证有效和安全地进行机械臂控制。而实际的机械臂操作过程中，离散的算法才能保证实时的控制，而目前许多算法都是多次迭代的算法，每个时刻的控制信号都需要经过多次迭代才能获得，计算时间比较长。

发明内容

本发明提供一种冗余度机械臂位姿控制的单94LVI迭代算法，该算法实现了对于机械臂末端执行器位置和姿态的同时控制。

为了达到上述技术效果，本发明的技术方案如下：

一种冗余度机械臂位姿控制的单94LVI迭代算法，包括如下步骤：

S1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；

S2：将二次型位姿控制解析方案转化为标准的二次规划；

S3：运用单94LVI迭代算法求解；

S4：将计算结果传给下位机控制器驱动机械臂运动。

进一步地，所述步骤S1中的二次型位姿控制解析方案为：

对于自由度为n，工作空间为m的冗余度机械臂，设计其性能指标为最小化

受约束于

θ^-≤θ≤θ⁺和

其中

A＝J₁(θ)，

θ表示机械臂的关节角度向量，

表示机械臂的关节角速度向量，J₁(θ)表示机械臂末端执行器位置雅克比矩阵，J₂(θ)表示机械臂末端执行器姿态雅克比矩阵，o_a表示末端执行器实际姿态向量，o_d表示末端执行器期望姿态向量，

表示末端执行器期望姿态速度向量，r_a表示末端执行器实际位置向量，r_d表示末端执行器期望位置向量，

表示末端执行器期望位置速度向量，θ^±表示关节角度上下限，

表示关节角速度上下限，上标^T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。

进一步地，所述步骤S2中的的二次型位姿控制解析方案转化为标准的二次规划，即性能指标为最小化x^TQx/2+p^Tx，受约束于Ax＝b和x^-≤x≤x⁺，其中

正的常数η用来调节关节角速度的可行域。

进一步地，所述步骤S3的标准二次规划用如下单94LVI迭代算法求解：

e(z_k)＝z_k-P_Ω(z_k-(M_kz_k+c_k))，

z_k+1＝z_k-ρ(z_k)d(z_k)，

θ_k+1＝θ_k+τx_k+1

其中，(·)_k代表在t_k＝kτ时刻矩阵或者向量的值，τ为采样间隔，z＝[x；y]，y∈R^m为对应于等式约束Ax＝b的对偶决策向量，M＝[Q,-A^T；A,0]，P_Ω(z)的第i个元素定义为：

(·)_i代表向量的第i个元素，

1_v是长度为m的全1向量，正的常数

应该被设置得足够大来代表+∞。

进一步地，步骤S4中的单94LVI迭代算法计算结果传给下位机控制器，驱动机械臂运动。

与现有技术相比，本发明技术方案的有益效果是：

本发明专利提出一种冗余度机械臂位姿控制的单94LVI迭代算法，实现了对于机械臂末端执行器位置和姿态的同时控制，有效地消除了机械臂超越角度极限和角速度极限从而造成物理损坏的可能性，同时本发明的算法只需要一次迭代，能有效地减少计算时间，在同样的时间内能够实现更多次和更精确的控制，因此具有较强的适用性、安全性和实用性。

附图说明

图1为本发明的流程图；

图2为实现本发明的机械臂三维模型图；

图3为实现本发明的机械臂运动轨迹示意图；

图4为实现本发明的机械臂关节角度示意图；

图5为实现本发明的机械臂关节角速度示意图；

图6为实现本发明的机械臂末端执行器姿态示意图；

图7为实现本发明的机械臂末端执行器姿态误差示意图；

图8为实现本发明的机械臂末端执行器位置误差示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

如图1所示，冗余度机械臂位姿控制的单94LVI迭代算法首先提出位姿控制的性能指标与约束；然后将其转化为标准的二次规划；接着应用单94LVI迭代算法求解；最后下位机控制器根据求解结果控制机械臂的运动。

图2是实现本发明的机械臂——UR5机械臂。该机械臂由六个连杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成，其工作空间是三维空间。

图3是UR5机械臂运动轨迹示意图。给定末端任务为画出一个平面圆形，且末端执行器的姿态垂直于该平面。

图4是UR5机械臂的六个关节的角度示意图。关节1、关节2、关节3、关节4、关节5和关节6的角度分别是θ₁、θ₂、θ₃、θ₄、θ₅和θ₆。

图5是UR5机械臂的六个关节的角速度示意图。关节1、关节2、关节3、关节4、关节5和关节6的角速度分别是

和

图6是UR5机械臂的末端执行器姿态示意图。末端执行器的姿态相对X轴、Y轴和Z轴的分量分别是o_x、o_y和o_z。

图7是UR5机械臂的末端执行器姿态误差示意图。末端执行器的姿态误差2范数是||o_a-o_d||₂。

图8是UR5机械臂的末端执行器位置误差示意图。末端执行器的位置误差2范数是||r_a-r_d||₂。

本发明设计的二次型位姿控制解析方案为：

最小化：

约束条件：

θ^-≤θ≤θ⁺， (3)

其中

A＝J₁(θ)，

θ表示机械臂的关节角度向量，

表示机械臂的关节角速度向量，J₁(θ)表示机械臂末端执行器位置雅克比矩阵，J₂(θ)表示机械臂末端执行器姿态雅克比矩阵，o_a表示末端执行器实际姿态向量，o_d表示末端执行器期望姿态向量，

表示末端执行器期望姿态速度向量，r_a表示末端执行器实际位置向量，r_d表示末端执行器期望位置向量，

表示末端执行器期望位置速度向量，θ^±表示关节角度上下限，

表示关节角速度上下限，上标^T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。

用x表示机械臂的关节加速度

二次型位姿控制解析方案(1)-(4)便可描述为如下标准的二次规划方案：

最小化：x^ΤQx/2+p^Τx，(5)

约束条件：Ax＝b， (6)

x^-≤x≤x⁺， (7)

其中

正的常数η用来调节关节角速度的可行域。

将标准二次规划用如下单94LVI迭代算法求解：

e(z_k)＝z_k-P_Ω(z_k-(M_kz_k+c_k))，

z_k+1＝z_k-ρ(z_k)d(z_k)，

θ_k+1＝θ_k+τx_k+1。

其中，(·)_k代表在t_k＝kτ时刻矩阵或者向量的值，τ为采样间隔，z＝[x；y]，y∈R^m为对应于等式约束Ax＝b的对偶决策向量，M＝[Q,-A^T；A,0]，P_Ω(z)的第i个元素定义为：

(·)_i代表向量的第i个元素，

1_v是长度为m的全1向量，正的常数

应该被设置得足够大来代表+∞。

接下来，将单94LVI迭代算法计算结果传给下位机控制器，驱动机械臂运动。

现结合一个具体的实例操作对本发明的工作流程进行如下说明。

在满足物理约束限制下，实现末端执行器位置和姿态的同时控制。

机械臂初始位置关节1至6的角度设置为[0,-2π/3,-2π/3,-π/6,2π/3,0]^Τ弧度，关节角度上限设置为θ⁺＝[π/2,0,0,π/2,π,π/2]^Τ弧度，关节角度下限设置为θ^-＝[-π/2,-π,-π,-π/2,0,-π/2]^Τ弧度，关节角速度上限设置为

弧度/秒，关节角速度下限设置为

弧度/秒。任务执行时间为20秒，采样间隔取1毫秒。末端执行器位置期望跟踪一个平面圆，末端执行器姿态期望保持与该平面垂直。

实验结果显示，机械臂的末端执行器最终成功画出一个圆，且保持竖直状态，机械臂末端执行器位置和姿态都得到了准确的控制。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用于仅用于示例性说明，不能理解为对本专利的限制；

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

Claims (3)

1.一种冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，包括如下步骤：

S1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；

S2：将二次型位姿控制解析方案转化为标准的二次规划；

S3：运用单94LVI迭代算法求解；

S4：将计算结果传给下位机控制器驱动机械臂运动；

所述步骤S1中的二次型位姿控制解析方案为：

对于自由度为n，工作空间为m的冗余度机械臂，设计其性能指标为最小化

受约束于

θ^-≤θ≤θ⁺和

其中

A＝J₁(θ)，

θ表示机械臂的关节角度向量，

表示机械臂的关节角速度向量，J₁(θ)表示机械臂末端执行器位置雅克比矩阵，J₂(θ)表示机械臂末端执行器姿态雅克比矩阵，o_a表示末端执行器实际姿态向量，o_d表示末端执行器期望姿态向量，

表示末端执行器期望姿态速度向量，r_a表示末端执行器实际位置向量，r_d表示末端执行器期望位置向量，

表示末端执行器期望位置速度向量，θ^±表示关节角度上下限，

表示关节角速度上下限，上标^T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。

2.根据权利要求1所述的冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，所述步骤S2中的的二次型位姿控制解析方案转化为标准的二次规划，即性能指标为最小化x^TQx/2+p^Tx，受约束于Ax＝b和x^-≤x≤x⁺，其中

正的常数η用来调节关节角速度的可行域。

3.根据权利要求2所述的冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，所述步骤S3的标准二次规划用如下单94LVI迭代算法求解：

e(z_k)＝z_k-P_Ω(z_k-(M_kz_k+c_k))，

z_k+1＝z_k-ρ(z_k)d(z_k)，

θ_k+1＝θ_k+τx_k+1

其中，(·)_k代表在t_k＝kτ时刻矩阵或者向量的值，τ为采样间隔，z＝[x；y]，y∈R^m为对应于等式约束Ax＝b的对偶决策向量，M＝[Q,-A^T；A,0]，P_Ω(z)的第i个元素定义为：

(·)_i代表向量的第i个元素，

1_v是长度为m的全1向量，正的常数

应该被设置得足够大来代表+∞。

Images