CN115890735B

CN115890735B - 机械臂系统、机械臂及其控制方法、控制器和存储介质

Info

Publication number: CN115890735B
Application number: CN202310086513.8A
Authority: CN
Inventors: 李祥云; 陆奇; 李康
Original assignee: West China Hospital of Sichuan University
Current assignee: West China Hospital of Sichuan University
Priority date: 2023-02-09
Filing date: 2023-02-09
Publication date: 2023-05-05
Anticipated expiration: 2043-02-09
Also published as: CN115890735A

Abstract

本发明机器人控制技术领域，具体涉及一种机械臂系统、机械臂及其控制方法、控制器和存储介质。本发明的控制方法为：检测机械臂末端执行器的位置和姿态，并计算当前位姿同运动期望位姿之间的误差；通过计算得到的误差，代入不确定性和外部扰动估计器进行扰动估计；根据计算得到的误差及扰动估计构建控制输入，保证所述控制输入满足李雅普诺夫稳定条件；依据控制输入进一步对机械臂关节驱动电机的输出力矩进行控制。本发明还提供了实现上述控制方法的控制器。本发明在对偶四元数空间建立机械臂动力学模型，解决四元数的双平衡问题，并对模型的不确定性和外部干扰同时进行估计和补偿，实现对其位置和姿态的耦合实时控制，保证其鲁棒性。

Description

机械臂系统、机械臂及其控制方法、控制器和存储介质

技术领域

本发明机器人控制技术领域，具体涉及一种机械臂系统、机械臂及其控制方法、控制器和存储介质。

背景技术

随着工业4.0时代的到来，智能制造相关技术飞速发展。作为智能制造领域的重要工具之一，机器人凭借其高强度、长续航、流程自动化等优势替代了许多简单重复的体力劳动，提高了生产效率。机械臂是工业机器人中最常见的一类机械装置，它既可以实现自动化生产也可以随时切换成人为控制，具有高精度、多输入多输出、操作灵活性高、高度非线性及强耦合等特点，能够完成焊接、搬运、包装、检验、涂胶、抛光打磨等生产环节，在航空航天、工业装配、物流运输、船舶制造等多个领域得到了广泛的应用。

但许多机械臂应用场景不仅涉及简单的拾取和放置操作，还涉及对任何类型的物体的不同方式准确抓取等精细操作，这些需求对机械臂的位姿控制精度提出了更高要求。在经典的控制方法中，平移与旋转在控制律中是不耦合的，其中对于姿态的描述通常是利用基于极小表示来实现，如欧拉角。但用欧拉角来表述机械臂姿态存在万向节死锁问题，并且欧拉角是不可传递的，其旋转的顺序直接影响旋转的结果，而旋转顺序在不同的应用场景下无法统一。与传统方法相比，几何方法可以考虑平移与旋转之间的耦合，从而生成更加自然的运动轨迹。然而几何方法中用齐次变换矩阵来描述刚性变换会导致运算过程过于复杂，降低了控制器的运行效率和实时性，从而阻碍了现有的几何控制律在实际环境中的应用。

机械臂运动学研究了机械臂末端执行器位置与机械臂各个关节的转角、角速度及角加速度之间的关系，能够在动态响应要求不高、机器人系统不太庞大的情况下驱动机械臂按照指定轨迹移动。但由于重力、科里奥利和离心扭矩以及非线性摩擦等各种干扰的存在，机械臂控制是高度非线性和强耦合的，高速运动时摩擦力导致的电机转矩损失高达30%，而运动学无法有效应对实际应用中的这些非线性扰动，也无法控制机械臂与环境的交互力，无法实现柔顺控制、恒力控制等功能。因此需要通过动力学来补偿非线性误差，增强控制器的鲁棒性。机械臂动力学的问题是其模型的参数数量通常随着机械臂自由度（DOF）的增加而增加，因此大多数基于动力学的控制器复杂度高，并且对参数变化敏感，不能满足实时稳定、精确对末端执行器位置和姿态进行控制的要求。机械臂的动力学运动控制（包括平移和旋转）是一项极具挑战性的任务。

机械臂末端执行器的位置和姿态可以以不同的方式进行参数化，例如使用众所周知的4×4齐次变换矩阵、欧拉角、轴角或单位四元数、对偶四元数等。尽管对偶四元数是八维的，但已有许多学者（IEEE Transactions on Systems, Man, and Cybernetics, PartB: Cybernetics, 28(2):135–145, Apr 1998.）认为对偶四元数是耦合表达螺旋运动（包括机械臂运动中的平移和旋转）的最紧凑和最有效的方式，其性能在惯性导航、计算机视觉、四旋翼无人机等多个领域已被证明。

虽然基于对偶四元数的机械臂控制研究取得了一些进展，但现有技术的方法只停留在运动学层面，缺乏对重力、科里奥利和离心力矩、非线性摩擦等非线性扰动的考虑。这使得对偶四元数的机械臂控制方法的应用仍然受到了较大的限制。

发明内容

针对现有技术的问题，本发明提供一种位姿耦合的机械臂鲁棒控制方法和控制器，目的在于简化机械臂末端执行器位姿表述，在对偶四元数空间建立机械臂动力学模型，解决四元数的双平衡问题，并对模型的不确定性和外部干扰同时进行估计和补偿，实现对其位置和姿态的耦合实时控制，保证其鲁棒性。

一种机械臂控制方法，包括如下步骤：

步骤1：检测机械臂末端执行器的位置和姿态，并计算当前位姿同运动期望位姿之间的误差；

步骤2：通过步骤1中计算得到的误差，代入不确定性和外部扰动估计器进行扰动估计；

步骤3：根据步骤1中计算得到的误差及步骤2中得到的扰动估计构建控制输入，保证所述控制输入满足李雅普诺夫稳定条件；

步骤4：依据步骤3中取得的控制输入进一步对机械臂关节驱动电机的输出力矩进行控制。

优选的，步骤2中，所述扰动估计的表达式为：

其中为拉普拉斯逆算子，为自定义算子，定义为，其中，，为速度旋量误差的导数，为控制输入，满足的动力学方程，为系统集总不确定项，为自选滤波器，为体坐标系中的实际速度旋量，为体坐标系中的期望速度旋量，为体坐标系中的速度误差旋量，为四元数误差，为四元数误差的共轭，仿射变换定义为，代表四元数乘法运算。

优选的，所述和的表达式为：

其中，为一个幂零对偶单位，满足，，为单位对偶向量，为对偶向量形式的控制器增益， s为复参变量，称为复频率，为常数对偶向量，为系统状态误差，为对偶四元数误差的对数；

，代表机械臂惯性矩阵，为代表关节角速度与体坐标系中机械臂末端笛卡尔速度之间关系的雅可比矩阵，表示雅可比矩阵的逆矩阵或伪逆矩阵，表示雅可比矩阵伪逆的转置矩阵。

，；

其中为关节角度向量，为科氏力及离心力矩阵；，、、和分别为机械臂惯性矩阵的3*3分块矩阵，、、和分别为机械臂惯性矩阵的逆矩阵的3*3分块矩阵；

，其中，，代表重力力矩矢量；

为体坐标系中的机械臂末端坐标，为体坐标系中的机械臂末端角速度，为为体坐标系中的机械臂末端线速度，、、和分别为科氏力及离心力矩阵的3*3分块矩阵，和分别为重力力矩矢量的3*1分块矩阵。

优选的，所述系统状态误差及其导数构造为：

，

其中，为系统集总不确定项。

优选的，所述的表达式为：

，

其中为符号函数，为对偶四元数误差，为四元数误差。

优选的，步骤4中输出力矩的表达式为：

其中，，

其中，为代表关节角速度与体坐标系中机械臂末端笛卡尔速度之间关系的雅可比矩阵，为雅可比矩阵的转置。

为控制输入的实部，为控制输入的对偶部。

本发明还提供一种机械臂，它的运动通过上述控制方法进行控制。

本发明还提供一种控制器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述控制方法。

本发明还提供一种机械臂系统，它包括通过信号输送装置连接的机械臂和上述控制器。

本发明还提供一种计算机可读存储介质，其上存储有用于实现上述控制方法的计算机程序。

采用本发明的技术方案后，具有如下有益的效果：

1、采用对偶四元数实现了对机械臂末端执行器的位姿表述，具有位姿耦合一体化表达、计算效率高、方便轨迹插值优化等优点。相较传统笛卡尔坐标及欧拉角、转换矩阵等位姿表述方法具有控制运算量小、无万向节死锁、表述非奇异等优势。通过引入独特的控制参数有效解决四元数的双平衡问题。

2、在对偶四元数空间建立了七自由度冗余机械臂动力学模型，在链式结构上具有足够的扩展性和代表意义。动力学控制器在精细控制、非线性控制及柔顺控制中比运动学控制器有更大的发展空间和更好的性能表现。

3、开发了基于UDE的控制器，能够同时估计和补偿模型的不确定性和外部干扰，减少时间延迟和振荡，应对控制任务中的非线性挑战，保证控制鲁棒性和精度。

4、通过仿真验证了所提方法的可行性和有效性，包括位姿耦合控制、动力学模型不确定性和外部干扰抑制及轨迹跟踪。

显然，根据本发明的上述内容，按照本领域的普通技术知识和惯用手段，在不脱离本发明上述基本技术思想前提下，还可以做出其它多种形式的修改、替换或变更。

以下通过实施例形式的具体实施方式，对本发明的上述内容再作进一步的详细说明。但不应将此理解为本发明上述主题的范围仅限于以下的实例。凡基于本发明上述内容所实现的技术均属于本发明的范围。

附图说明

图1为实验例1和实施例2中的坐标系几何与螺旋运动示意图；

图2为实验例1中的初始位姿，目标位姿及运动轨迹；

图3为实验例1中的末端执行器位置坐标及姿态欧拉角；

图4为实验例1中的位置坐标误差及姿态欧拉角误差；

图5为实验例2中的期望轨迹和实际运动轨迹；

图6为实验例2中的末端执行器位置坐标及姿态欧拉角；

图7为实验例2中的位置坐标误差及姿态欧拉角误差。

具体实施方式

需要特别说明的是，实施例中未具体说明的数据采集、传输、储存和处理等步骤的算法，以及未具体说明的硬件结构、电路连接等均可通过现有技术已公开的内容实现。

实施例1 位姿耦合的机械臂鲁棒控制方法

本实施例的方法包括如下步骤：

在本实施例中，机械臂的关节力矩控制输入为：

，。

下面对上述关节力矩控制输入的推导过程进行进一步说明：

步骤1，检测机械臂末端执行器的位置和姿态，并计算当前位姿同运动期望位姿之间的误差：

1.机械臂动力学

设表示机械臂末端执行器的速度旋量，其中代表末端平移速度，代表末端旋转速度。速度旋量与关节角速度向量之间的关系可以描述为

其中代表雅可比矩阵。关节空间机械臂动力学可以描述为：

其中代表机械臂惯性矩阵，表示科氏力及离心力矩阵，代表重力力矩矢量，表示控制力矩，而是包含了摩擦、外部扰动和模型不确定性等因素的有界扰动项。结合正运动学(1)和关节空间动力学(2)可以得到任务空间动力学：

其中代表机械臂末端执行器的力旋量，为有界扰动项。在体坐标系中的机械臂末端执行器速度旋量还可以用对偶四元数重写成。

、、和分别为机械臂惯性矩阵的3*3分块矩阵，、、和分别为机械臂惯性矩阵的逆矩阵的3*3分块矩阵，、、和分别为科氏力及离心力矩阵的3*3分块矩阵，和分别为重力力矩矢量的3*1分块矩阵。

则任务空间机械臂动力学可重写成：

其中。

2.误差动力学

体坐标系中的速度旋量及其导数可以被重写成以下形式：

其中，为体坐标系下的机械臂末端角速度，为体坐标系中的机械臂末端坐标。

两个对偶四元数之间的误差可以描述为：

其中代表期望位姿而代表当前位姿，为四元数误差。此处代表对偶四元数乘法运算，为体坐标系下的机械臂末端位置误差。对于的三维向量需要转化成形式的对偶向量再进行计算。结合四元数运动学可以推出对偶四元数误差的导数：

其中代表体坐标系相对于惯性坐标系I的角速度向量，为速度旋量误差，仿射变换定义为。结合(4)(5)(7)可以推导出速度旋量误差动力学：

其中

步骤2，通过步骤1中计算得到的误差，代入不确定性和外部扰动估计器进行扰动估计：

考虑(3)中的集总不确定项D，任务空间速度旋量的时间导数可改写成：

结合(8)及上述推导，在存在模型不确定性和外部干扰的情况下，非线性对偶四元数误差动力学可以重写为：

为处理误差动态的非线性结构，滤波后的跟踪误差及其导数构造为：

其中是常数对偶向量，,。是一种算子，定义为。参数的引入可以用于处理四元数的双平衡问题，控制器将根据机械臂的当前位姿自动选择最短路线收敛到期望位姿。期望误差动力学可指定为：

其中是以对偶向量形式表示的控制器增益。结合反馈线性化技术，控制输入可以设计为：

利用UDE技术对集总不确定项进行估计：

步骤3，根据步骤1中计算得到的误差及步骤2中得到的扰动估计构建控制输入，保证所述控制输入满足李雅普诺夫稳定条件：

推出基于UDE的轨迹跟踪控制器控制输入：

其中为单位对偶向量。再结合(9)及上述推导可推出任务空间动力学力矩旋量控制输入：

最终推出关节力矩控制输入：

，。

实施例2 机械臂系统

本实施例包括机械臂和用于控制机械臂运动的计算机设备。机械臂和计算机设备通过信号输送装置连接，所述信号输送装置可以是信号线、信号接口和无线信号连接设备等。

机械臂的运动通过关节驱动电机实现。计算机设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例1的控制方法。计算机设备用于按照实施例1的方法控制驱动电机控制力矩。

下面通过仿真实验对实施例1的控制方法的可行性和有效性进行验证。

实验例1定点收敛实验

一、实验方法

如图1、2所示：实验例1任务为移动机械臂末端执行器，使其位姿收敛到期望位姿。同时通过设置相反期望姿态模拟四元数双平衡问题。目的为评估控制器的收敛性和稳态误差以及是否能以较短路径进行运动。

仿真软件使用Simulink及Robotic toolbox工具包， 7自由度机械臂模型的D-H参数如表1所示。

控制器参数设置为，，

本实验例中控制器滤波器的表达式为：

T为滤波器截止频率相关参数， s为为复参变量，称为复频率。

滤波器中 T=0.1。

初始姿态，

对应初始姿态欧拉角，

初始位置，

期望姿态，

对应期望姿态欧拉角，

期望位置，

欧拉角旋转顺序为“ZYX”，单位为度。

表1 仿真机械臂模型D-H参数

二、实验结果

为方便理解，将实验结果转换成位置坐标和姿态欧拉角来进行分析。图2 中，无论期望姿态指定为正或为负，机械臂均能沿较短路径运动，说明控制器能够正确应对四元数的双平衡问题。如图3、4 所示，机械臂从零初速度开始运动，其末端执行器位姿从初始位姿逐渐收敛到期望位姿，经过一段时间后位置和姿态的跟踪误差均趋近于0，表明所提出的控制器具有良好的稳态性能。

实验例2轨迹跟踪实验

一、实验方法

如图1、5所示：实验例2任务为移动机械臂末端执行器，使其位姿在无扰动及有5N单方向扰动力两种情况下跟随期望轨迹。目的为评估控制器的抗干扰能力及动态性能。

仿真软件、机械臂模型及控制器参数设置同实验例1。

初始关节角设置为，

对应末端执行器初始姿态，

对应初始姿态欧拉角，

初始位置，

期望轨迹姿态，，，

对应期望轨迹姿态欧拉角，

期望轨迹位置，

仿真时间为20秒。

二、实验结果

如图5、6、7所示：外部干扰在运动开始时对控制器有较大影响。然而控制器能够迅速估计并补偿模型的不确定性和外部干扰，并在后续仿真过程中展示出良好的跟踪性能。无论有无干扰，仿真过程中跟踪误差均可以保持在0 附近的较小范围内，表明控制器具有良好的动态性能和鲁棒性。

通过上述实施例可见，本发明提供了一种新的机械臂控制方法和机械臂系统。通过本发明的机械臂控制方法，能够对模型的不确定性和外部干扰同时进行估计和补偿，实现对其位置和姿态的耦合实时控制，保证其鲁棒性，具有很好的应用前景。

Claims

1.一种机械臂控制方法，其特征在于，包括如下步骤：

步骤4：依据步骤3中取得的控制输入进一步对机械臂关节驱动电机的输出力矩进行控制；

步骤2中，所述扰动估计的表达式为：

其中为拉普拉斯逆算子，为自定义算子，定义为，其中，，为速度旋量误差的导数，为控制输入，满足的动力学方程，为系统集总不确定项，为自选滤波器，为体坐标系中的实际速度旋量，为体坐标系中的期望速度旋量，为体坐标系中的速度误差旋量，为四元数误差，为四元数误差的共轭，仿射变换定义为，代表四元数乘法运算；

所述和的表达式为：

其中，为一个幂零对偶单位，满足，，为单位对偶向量，为对偶向量形式的控制器增益，s为复参变量，称为复频率，为常数对偶向量，为系统状态误差，为对偶四元数误差的对数；

，代表机械臂惯性矩阵，为代表关节角速度与体坐标系中机械臂末端笛卡尔速度之间关系的雅可比矩阵，表示雅可比矩阵的逆矩阵或伪逆矩阵，表示雅可比矩阵伪逆的转置矩阵；

，；

其中为关节角度向量，为科氏力及离心力矩阵，、、和分别为机械臂惯性矩阵的3*3分块矩阵，、、和分别为机械臂惯性矩阵的逆矩阵的3*3分块矩阵；

，其中，，代表重力力矩矢量；

为体坐标系中的机械臂末端坐标，为体坐标系中的机械臂末端角速度，为为体坐标系中的机械臂末端线速度，、、和分别为科氏力及离心力矩阵的3*3分块矩阵，和分别为重力力矩矢量的3*1分块矩阵；

所述系统状态误差及其导数构造为：

，

其中，为系统集总不确定项；

所述的表达式为：

，

其中为符号函数，为对偶四元数误差，为四元数误差；

步骤4中输出力矩的表达式为：

其中，，

其中，为代表关节角速度与体坐标系中机械臂末端笛卡尔速度之间关系的雅可比矩阵，为雅可比矩阵的转置，

为控制输入的实部，为控制输入的对偶部，、、和分别为机械臂惯性矩阵的逆矩阵的3*3分块矩阵。

2.一种控制器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1所述的控制方法。

3.一种机械臂系统，其特征在于：它包括通过信号输送装置连接的机械臂和权利要求2所述的控制器。

4.一种计算机可读存储介质，其特征在于：其上存储有用于实现权利要求1所述的控制方法的计算机程序。