[发明专利]一种冗余度机械臂力矩层重复运动控制方法

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

其中||·||₂表示向量的二范数，α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的时间导数；另外，表示的时间导数，θ^±、和τ^±分别表示关节角度极限、关节速度极限和关节力矩极限。