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

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

A single 94lvi iterative algorithm for redundant manipulator pose control

【技术实现步骤摘要】
一种冗余度机械臂位姿控制的单94LVI迭代算法
本专利技术涉及机械臂运动规划及控制
，更具体地，涉及一种冗余度机械臂位姿控制的单94LVI迭代算法。
技术介绍
随着科技的发展，机器人将会在未来生产生活中扮演重要角色。作为机器人的重要组成部分，冗余度机械臂是自由度大于任务空间所需最少自由度的一种类手臂装置，也被视为一种简化的机器人，已经受到了国内外众多研究者的关注。目前，冗余度机械臂已经在许多工业生产中被使用来代替人类完成一些重要的工作。现有的冗余度机械臂控制算法主要针对末端执行器的位置控制，而忽略了对其姿态的控制，但是姿态控制在许多场景下都是必要的。例如，在焊接和钻孔任务中，末端执行器的位置固然重要，但是其姿态(方向)也同样重要，否则也是不能成功完成任务的。同时，考虑到机械臂物理极限，才能够保证有效和安全地进行机械臂控制。而实际的机械臂操作过程中，离散的算法才能保证实时的控制，而目前许多算法都是多次迭代的算法，每个时刻的控制信号都需要经过多次迭代才能获得，计算时间比较长。
技术实现思路
本专利技术提供一种冗余度机械臂位姿控制的单94LVI迭代算法，该算法实现了对于机械臂末端执行器位置和姿态的同时控制。为了达到上述技术效果，本专利技术的技术方案如下：一种冗余度机械臂位姿控制的单94LVI迭代算法，包括如下步骤：S1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；S2：将二次型位姿控制解析方案转化为标准的二次规划；S3：运用单94LVI迭代算法求解；S4：将计算结果传给下位机控制器驱动机械臂运动。进一步地，所述步骤S1中的二次型位姿控制解析方案为：对于自由度为n，工作空间为m的冗余度机械臂，设计其性能指标为最小化受约束于θ-≤θ≤θ+和其中A＝J1(θ)，θ表示机械臂的关节角度向量，表示机械臂的关节角速度向量，J1(θ)表示机械臂末端执行器位置雅克比矩阵，J2(θ)表示机械臂末端执行器姿态雅克比矩阵，oa表示末端执行器实际姿态向量，od表示末端执行器期望姿态向量，表示末端执行器期望姿态速度向量，ra表示末端执行器实际位置向量，rd表示末端执行器期望位置向量，表示末端执行器期望位置速度向量，θ±表示关节角度上下限，表示关节角速度上下限，上标T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。进一步地，所述步骤S2中的的二次型位姿控制解析方案转化为标准的二次规划，即性能指标为最小化xTQx/2+pTx，受约束于Ax＝b和x-≤x≤x+，其中正的常数η用来调节关节角速度的可行域。进一步地，所述步骤S3的标准二次规划用如下单94LVI迭代算法求解：e(zk)＝zk-PΩ(zk-(Mkzk+ck))，zk+1＝zk-ρ(zk)d(zk)，θk+1＝θk+τxk+1其中，(·)k代表在tk＝kτ时刻矩阵或者向量的值，τ为采样间隔，z＝[x；y]，y∈Rm为对应于等式约束Ax＝b的对偶决策向量，M＝[Q,-AT；A,0]，PΩ(z)的第i个元素定义为：(·)i代表向量的第i个元素，1v是长度为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的角度分别是θ1、θ2、θ3、θ4、θ5和θ6。图5是UR5机械臂的六个关节的角速度示意图。关节1、关节2、关节3、关节4、关节5和关节6的角速度分别是和图6是UR5机械臂的末端执行器姿态示意图。末端执行器的姿态相对X轴、Y轴和Z轴的分量分别是ox、oy和oz。图7是UR5机械臂的末端执行器姿态误差示意图。末端执行器的姿态误差2范数是||oa-od||2。图8是UR5机械臂的末端执行器位置误差示意图。末端执行器的位置误差2范数是||ra-rd||2。本专利技术设计的二次型位姿控制解析方案为：最小化：约束条件：θ-≤θ≤θ+，(3)其中A＝J1(θ)，θ表示机械臂的关节角度向量，表示机械臂的关节角速度向量，J1(θ)表示机械臂末端执行器位置雅克比矩阵，J2(θ)表示机械臂末端执行器姿态雅克比矩阵，oa表示末端执行器实际姿态向量，od表示末端执行器期望姿态向量，表示末端执行器期望姿态速度向量，ra表示末端执行器实际位置向量，rd表示末端执行器期望位置向量，表示末端执行器期望位置速度向量，θ±表示关节角度上下限，表示关节角速度上下限，上标T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。用x表示机械臂的关节加速度二次型位姿控制解析方案(1)-(4)便可描述为如下标准的二次规划方案：最小化：xΤQx/2+pΤx，(5)约束条件：Ax＝b，(6)x-≤x≤x+，(7)其中正的常数η用来调节关节角速度的可本文档来自技高网...

【技术保护点】
1.一种冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，包括如下步骤：/nS1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；/nS2：将二次型位姿控制解析方案转化为标准的二次规划；/nS3：运用单94LVI迭代算法求解；/nS4：将计算结果传给下位机控制器驱动机械臂运动。/n

【技术特征摘要】
1.一种冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，包括如下步骤：
S1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；
S2：将二次型位姿控制解析方案转化为标准的二次规划；
S3：运用单94LVI迭代算法求解；
S4：将计算结果传给下位机控制器驱动机械臂运动。


2.根据权利要求1所述的冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，所述步骤S1中的二次型位姿控制解析方案为：
对于自由度为n，工作空间为m的冗余度机械臂，设计其性能指标为最小化受约束于θ-≤θ≤θ+和其中A＝J1(θ)，θ表示机械臂的关节角度向量，表示机械臂的关节角速度向量，J1(θ)表示机械臂末端执行器位置雅克比矩阵，J2(θ)表示机械臂末端执行器姿态雅克比矩阵，oa表示末端执行器实际姿态向量，od表示末端执行器期望姿态向量，表示末端执行器期望姿态速度向量，ra表示末端执行器实际位置向量，rd表示末端执行器期望位置向量，表示末端执行器期望位置速度向量，θ±表示关节角度上下限，表示关节角速度上下限，上标T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。
<...

【专利技术属性】
技术研发人员：胡海峰，杨敏，张雨浓，凌一宏，陈建荣，
申请(专利权)人：中山大学，
类型：发明
国别省市：广东;44