CN108098777B - 一种冗余度机械臂力矩层重复运动控制方法 - Google Patents

Abstract

一种冗余度机械臂力矩层重复运动控制方法，设计力矩层的重复运动性能指标；基于所需要优化的重复运动性能指标，建立力矩层重复运动控制方案，该方案受约束于力矩的雅可比矩阵等式、关节角度极限、关节速度极限和关节力矩极限；将力矩层重复运动控制方案转化为一个标准的二次规划问题，并通过数值算法求解器对其进行求解；下位机控制器根据求解结果驱动机械臂动作使其完成给定的末端任务，并达到重复运动的目的。本发明基于动力学方程而设计的控制方案，是直接在力矩层上完成对机械臂的控制，使机械臂在完成给定末端任务的同时还能达到重复运动的目的。

Description

一种冗余度机械臂力矩层重复运动控制方法

技术领域

本发明涉及冗余度机械臂的运动规划及控制领域，特别是一种冗余度机械臂力矩层重复运动控制方法。

背景技术

冗余度机械臂是一种自身自由度多于执行末端任务所需自由度的机械装置；已广泛应用于国民经济生产活动中，如焊接组装、装备制造和产品加工等。冗余度机械臂的运动规划和控制中容易存在的一个问题是关节角偏差问题：当机械臂末端执行器的运动轨迹是闭合的，在完成给定的规划任务后，机械臂的关节角变量在其运动空间中的轨迹不是闭合的。简言之，闭合的末端执行器运动轨迹没有产生闭合的关节空间运动轨迹。

目前，为了解决关节角偏差问题(或称，非重复运动问题)，已有多种速度层和加速度层的重复运动方案被提出并实现了对机械臂的有效控制。然而，这些控制方案都是在不考虑机械臂动力学的情况下进行研究设计的，它们无法处理机械臂的关节力矩极限，也无法直接应用于与力控相关的机械臂，因而适用范围受到极大的限制。更重要的是，由于缺少对外力因素的考虑，这些控制方案在实际应用的时候，一旦机械臂存在扰动，那么它们将很容易失效，从而导致机械臂无法完成给定的末端任务。

发明内容

本发明的主要目的在于克服现有技术中的上述缺陷，提出一种冗余度机械臂力矩层重复运动控制方法。

本发明采用如下技术方案：

一种冗余度机械臂力矩层重复运动控制方法，其特征在于：设计力矩层的重复运动性能指标；基于所需要优化的重复运动性能指标，建立力矩层重复运动控制方案，该方案受约束于力矩的雅可比矩阵等式、关节角度极限、关节速度极限和关节力矩极限；将力矩层重复运动控制方案转化为一个标准的二次规划问题，并通过数值算法求解器对其进行求解；下位机控制器根据求解结果驱动机械臂动作使其完成给定的末端任务。

所述力矩层的重复运动性能指标为：

其中||·||₂表示向量的二范数，α>0和β>0为调节因子；θ表示机械臂关节角度，θ(0)表示机械臂关节角度的初始状态，

表示机械臂关节速度，τ表示机械臂关节力矩，H^-1表示机械臂惯性矩阵H的逆矩阵，c表示离心力变量，g表示重力变量。

力矩层重复运动控制方案设计为：

最小化

受约束于

θ^-≤θ≤θ⁺，

τ^-≤τ≤τ⁺，

其中，等式约束

对应于机械臂在力矩层的运动轨迹，κ₁>0和κ₂>0为误差反馈系数，J_H表示机械臂的惯性雅可比矩阵且J_H＝JH^-1，J表示机械臂的雅可比矩阵，

表示J的时间导数；e表示机械臂末端执行器的定位误差且e＝f(θ)-r_d，f表示一个非线性映射函数，r_d表示机械臂末端执行器期望的运动轨迹；ε表示机械臂末端执行器的速度误差且

表示r_d的时间导数；另外，

表示

的时间导数，θ^±、

和τ^±分别表示关节角度极限、关节速度极限和关节力矩极限。

将双端约束θ^-≤θ≤θ⁺和

转化为如下基于关节力矩τ的双端约束：

η^-+H^-1(c+g)≤H^-1t≤η⁺+H^-1(c+g)，

其中η^±表示联立关节角度θ和关节速度

后的上下极限，并且，

k_p>0和k_v>0为关节极限转换参数，

为关节极限转换裕量。

将力矩层重复运动控制方案转化为一个标准的二次规划问题，

最小化x^TQx/2+p^Tx，

约束条件为Cx＝d，Ax≤b，x^-≤x≤x⁺，

其中，x＝τ，Q＝(H^-1)²，

C＝J_H，

A＝[-(H^-1)^T(H^-1)^T]^T，b＝[-(η^-+H^-1(c+g))^T(η⁺+H^-1(c+g))^T]^T，x^±表示x的上下限，上标^T表示矩阵或向量的转置。

所述通过数值算法求解器对二次规划问题进行求解，具体为：将所述二次规划问题进一步变换为分段线性投影方程，从而构造相应的数值算法求解器进行求解。

由上述对本发明的描述可知，与现有技术相比，本发明具有如下有益效果：

本发明提供了一种能直接在力矩层上完成对机械臂的控制，使其在完成给定末端任务的同时还能达到重复运动的目的力矩层重复运动控制方法。

附图说明

图1为本发明的流程图。

具体实施方式

以下通过具体实施方式对本发明作进一步的描述。

参照图1，本发明的一种冗余度机械臂力矩层重复运动控制方法，主要包括设计力矩层重复运动性能指标1、建立力矩层重复运动控制方案2、转为标准的二次规划问题3、数值算法求解器4、下位机控制器5、冗余度机械臂6这六个部分组成。

对于本发明的力矩层重复运动控制方法，具体描述如下：首先根据机械臂动力学方程，通过引入调节因子，设计力矩层的重复运动性能指标；然后基于所需要优化的性能指标，建立力矩层重复运动控制方案，并将其转化为一个标准的二次规划问题，从而构造相应的数值算法求解器来求解该二次规划问题；最后将求解结果用于驱动机械臂的各个关节以使机械臂完成给定的末端任务，并达到重复运动的目的。

根据机械臂动力学方程，通过引入调节因子，可实现重复运动的力矩层性能指标设计为：

其中||·||₂表示向量的二范数，α>0和β>0为调节因子；θ表示机械臂关节角度，θ(0)表示机械臂关节角度的初始状态，

表示机械臂关节速度，τ表示机械臂关节力矩，H^-1表示机械臂惯性矩阵H的逆矩阵，c表示离心力变量，g表示重力变量。

基于上述所需要优化的性能指标(1)，结合机械臂的力矩控制，便可建立如下的力矩层重复运动控制方案：

最小化

约束条件：

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

τ^-≤τ≤τ⁺， (6)

其中，等式约束

对应于机械臂在力矩层的运动轨迹，κ₁>0和κ₂>0为误差反馈系数，J_H表示机械臂的惯性雅可比矩阵且J_H＝JH^-1，J表示机械臂的雅可比矩阵，

表示J的时间导数；e表示机械臂末端执行器的定位误差且e＝f(θ)-r_d，f表示一个非线性映射函数，r_d表示机械臂末端执行器期望的运动轨迹；ε表示机械臂末端执行器的速度误差且

表示r_d的时间导数；另外，

表示

的时间导数，θ^±、

和τ^±分别表示关节角度极限、关节速度极限和关节力矩极限。

对于双端约束(4)和(5)，其可转化为如下的基于关节力矩τ的双端约束：

η^-+H^-1(c+g)≤H^-1τ≤η⁺+H^-1(c+g)， (7)

其中η^±表示联立关节角度θ和关节速度

后的上下极限，并且，

k_p>0和k_v>0为关节极限转换参数，

为关节极限转换裕量。

基于双端约束(7)，上述力矩层的重复运动规划方案(2)-(6)便可转化为如下标准的二次规划问题：

最小化x^TQx/2+p^Tx， (8)

约束条件：Cx＝d， (9)

Ax≤b， (10)

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

其中，x＝τ，Q＝(H^-1)²，

C＝J_H，

A＝[-(H^-1)^T(H^-1)^T]^T，b＝[-(η^-+H^-1(c+g))^T(η⁺+H^-1(c+g))^T]^T，x^±表示x的上下限，上标^T表示矩阵或向量的转置。

并且，上述二次规划问题(8)-(11)的求解可等价于求解如下的分段线性投影方程：

P_Ω(y-(My+q))-y＝0， (12)

其中，P_Ω(·)表示分段线性投影算子。分段线性投影方程(9)中的原对偶决策变向量y，增广系数矩阵M和向量q分别定义如下：

其中，对偶决策变量u和v分别对应于等式约束(10)和不等式约束(11)。对于上述的分段线性投影方程(12)和二次规划问题(8)-(12)，可采用如下的数值算法来进行求解：

e(y^k)＝y^k-P_Ω(y^k-(My^k+q))，

y^k+1＝P_Ω(y^k-ρ(y^k)φ(y^k))，

φ(y^k)＝M^Te(y^k)+My^k+q，

其中迭代次数k＝0,1,2,…。给定一个初始值y⁰，通过该数值算法的不断迭代，便可得到分段线性投影方程(12)的解，从而得到二次规划问题(8)-(11)的最优解，也即前文所述的力矩层重复运动控制方案(2)-(6)的最优解。

通过数值算法求解器得到该二次规划问题的最优解之后，再将求解结果传递给下位机控制器驱动机械臂的运动，从而使得机械臂完成所给定的末端任务，并达到重复运动的目的。本发明可适用于常见的机械臂中，例如Epson机械臂，该工业机械臂为一个在空间作业，具有六自由度的Epson机械臂。该机械臂由三个连杆组成，包含了机械臂的6个关节(对应六个自由度)。

上述仅为本发明的具体实施方式，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。

Claims (3)

1.一种冗余度机械臂力矩层重复运动控制方法，其特征在于：设计力矩层的重复运动性能指标；基于所需要优化的重复运动性能指标，建立力矩层重复运动控制方案，该方案受约束于力矩的雅可比矩阵等式、关节角度极限、关节速度极限和关节力矩极限；将力矩层重复运动控制方案转化为一个标准的二次规划问题，并通过数值算法求解器对其进行求解；下位机控制器根据求解结果驱动机械臂动作使其完成给定的末端任务；

所述力矩层的重复运动性能指标为：

其中||·||₂表示向量的二范数，α>0和β>0为调节因子；θ表示机械臂关节角度，θ(0)表示机械臂关节角度的初始状态，

表示机械臂关节速度，τ表示机械臂关节力矩，H^-1表示机械臂惯性矩阵H的逆矩阵，c表示离心力变量，g表示重力变量；

力矩层重复运动控制方案设计为：

最小化

受约束于

θ^-≤θ≤θ⁺，

τ^-≤τ≤τ⁺，

其中，等式约束

对应于机械臂在力矩层的运动轨迹，κ₁>0和κ₂>0为误差反馈系数，J_H表示机械臂的惯性雅可比矩阵且J_H＝JH^-1，J表示机械臂的雅可比矩阵，

表示J的时间导数；e表示机械臂末端执行器的定位误差且e＝f(θ)-r_d，f表示一个非线性映射函数，r_d表示机械臂末端执行器期望的运动轨迹；ε表示机械臂末端执行器的速度误差且

表示r_d的时间导数；另外，

表示

的时间导数，θ^±、

和τ^±分别表示关节角度极限、关节速度极限和关节力矩极限；

将双端约束θ^-≤θ≤θ⁺和

转化为如下基于关节力矩τ的双端约束：

η^-+H^-1(c+g)≤H^-1τ≤η⁺+H^-1(c+g)，

其中η^±表示联立关节角度θ和关节速度

后的上下极限，并且，

k_p>0和k_v>0为关节极限转换参数，

为关节极限转换裕量。

2.如权利要求1所述的一种冗余度机械臂力矩层重复运动控制方法，其特征在于：将力矩层重复运动控制方案转化为一个标准的二次规划问题，

最小化x^TQx/2+p^Tx，约束条件为Cx＝d，Ax≤b，x^-≤x≤x⁺，

其中，x＝τ，Q＝(H^-1)²，

C＝J_H，

A＝[-(H^-1)^T(H^-1)^T]^T，b＝[-(η^-+H^-1(c+g))^T(η⁺+H^-1(c+g))^T]^T，x^±表示x的上下限，上标^T表示矩阵或向量的转置。

3.如权利要求1所述的一种冗余度机械臂力矩层重复运动控制方法，其特征在于，所述通过数值算法求解器对二次规划问题进行求解，具体为：将所述二次规划问题进一步变换为分段线性投影方程，从而构造相应的数值算法求解器进行求解。

Images