CN111941421B - 一种基于多机器人协同操作的自适应模糊力跟踪控制方法 - Google Patents

Abstract

本发明提供了一种基于多机器人协同操作的自适应模糊力跟踪控制方法，包括：第一步通过目标操作物的位置信息，根据闭链形成的位置约束分别求取每一机器人末端协调位置的约束方程，以得到每一机器人的末端期望位姿；第二步根据每一机器人进行逆运动学求解，求取执行每一机器人协调操作所需的各关节转角控制指令，得到每一机器人自身当前各关节的期望角度；第三步建立导纳控制模型，进行多机器人的力跟踪控制；第四步在所建立的导纳控制模型中引入学习率η，通过模糊控制器根据目标操作物的动态变化对η进行自适应调节，进行多机器人的力跟踪控制。本发明的多机器人进行闭链约束下的协同操作时，能够实现机器人及时补偿跟踪误差，提高力跟踪精度。

Description

一种基于多机器人协同操作的自适应模糊力跟踪控制方法

技术领域

本发明属于多机器人控制系统技术领域，具体涉及一种基于多机器人协同操作的自适应模糊力跟踪控制方法。

背景技术

随着工业生产任务的复杂化和多样化，多机器人协作系统已显示出更强大的操作能力，更灵活的系统结构和更强大的协作能力。因此，多机器人协作已成为机器人控制的重要挑战。此外，随着力控技术的发展，对于环境信息确定或者已知的应用场合，经典的力控制策略已经得到了广泛的应用，如阻抗控制或者混合控制位置力协调控制等。

但对于非结构化环境而言，接触作业时的环境可能是不同的材料或者未知材料，也可能是未知形状的曲面，或者更为复杂多变的接触环境，对各种未知特性的环境要建立完善的环境非常困难，所以进行力控制的难度也大大增加。

因此研究能够面向复杂工况适应动态变化环境的自适应力控制方法具有重要意义，基于此，本发明特提出一种基于多机器人协同操作的自适应模糊力跟踪控制方法。

发明内容

针对现有技术的缺陷，本发明提供了一种基于多机器人协同操作的自适应模糊力跟踪控制方法，针对复杂未知的非结构环境，多机器人进行闭链约束下的协同操作时，能够实现机器人及时补偿跟踪误差，将每个机器人末端的操作力控制在期望的范围内，进而提高力跟踪精度。

本发明的具体技术方案是：

一种基于多机器人协同操作的自适应模糊力跟踪控制方法，其特征在于包括以下步骤：

第一步：通过目标操作物的位置信息，根据闭链形成的位置约束分别求取每一机器人末端协调位置的约束方程，以得到每一机器人的末端期望位姿；

第二步：根据每一机器人进行逆运动学求解，求取执行每一机器人协调操作所需的各关节转角控制指令，得到每一机器人自身当前各关节的期望角度；

第三步：建立导纳控制模型，将每个机器人末端的力等效为输入的位置、将每个机器人末端的力误差等效为姿态增量，进行多机器人的力跟踪控制；

第四步：在所建立的导纳控制模型中引入学习率η，通过模糊控制器根据目标操作物的动态变化对η进行自适应调节，进行多机器人的力跟踪控制。

根据本发明的另一种具体实施方式，第一步中多机器人所满足的约束方程为：

其中，n≥3；

其中，^BT_On为第n个机器人基坐标系相对于全局坐标系的位姿转换矩阵；

其中，^OnT_En为第n个机器人末端坐标系相对于基坐标系的位姿转换矩阵；

其中，^EnT_L为工件坐标系相对于第n个机器人末端坐标系的位姿转换矩阵。

根据本发明的另一种具体实施方式，第三步中建立导纳控制模型将机器人与力传感器构成的系统建模构建为“质量—阻尼—弹簧”二阶系统，将环境建模构建为一阶弹簧系统，并建立如下方程：

式中，m为机器人的理想惯量，b为机器人的理想阻尼，k为机器人的理想刚度；

式中，x为实际位置，x_d为期望位置；

式中，Δf＝f_e-f_d，其中，f_e为机器人末端与环境的接触力，f_d为期望跟踪力；

将机器人末端与环境的接触力模型构建为弹簧模型，则机器人末端与环境的接触力与机器人嵌入环境之中的距离满足下述正比关系：

f_e＝k_e(x_e-x)

式中，k_e为环境刚度，x_e为环境位置，x为机器人末端的实际位置。

根据本发明的另一种具体实施方式，预先给定机器人末端的参考位置Xr，则

机器人末端的实际位置为：

机器人末端的实际位置与期望位置的偏差为：

结合Δf＝f_e-f_d可得：

求取二次导数计算得到：

又因为

进一步计算得到如下方程：

当多机器人系统趋于稳定时，期望跟踪力f_d维持不变，上式简化为：

k·k_e(x_e-x_r)＝k(Δf+f_d)+Δf·k_e＝(k_e+k)Δf+k·f_d

进而可以得到机器人在稳定时的稳态力误差：

使

求取f_ss为：

在多机器人系统稳定后，满足期望力要求，此时力偏差△f_ss为零，得到：

根据本发明的另一种具体实施方式，设定初始的环境位置xe代替参考位置x_r，结合公式

可知：f_e＝k_e(x_e-x)

进而得到基准公式：

式中，e＝x-x_e；

进一步设定

表示环境的估计值，定义：

将

代入基准公式中，并引入自适应项Δb(t)与Δk(t)，得到力跟踪方程：

其中，Δb(t)与Δk(t)的表达式分别为：

式中，λ为控制器采样周期，η为学习率，根据劳斯判据系统稳定时，满足

根据本发明的另一种具体实施方式，模糊调整器的输入为力跟踪误差υf＝f_e-f_d和力跟踪误差的变化率dυf＝(f_e-f_d)/λ，输出为学习率η。

传统机器人力控制方法，虽然可以将机器人的末端力控制在一个允许的范围内，但必须要求建立机器人的精确的动力学模型，然而这在实际工程中是很难做到的，另外它可以将力控制在一个合适的范围内，却不能稳定到期望值上，本发明提出的控制方法解决了上述控制算法的弊端，该方法的优势在于不需要建立机器人精确的动力学模型，只需要建立一个估计模型即可，这大大降低算法应用的条件，更加贴合实际的工程需要，对环境具有一定的鲁棒性，即变化的环境刚度算法依然可以将接触力控制在期望的值。

本发明具备以下有益效果：

本发明提供的基于多机器人协同操作的自适应模糊力跟踪控制方法，针对多机器人协同焊接、打磨、抛光等典型任务，提出了目标负载操作的闭链多机器人协同力跟踪控制方法，可以确保多机器人在对工件进行闭链约束下协同操作的同时，将机器人末端操作力控制在期望的范围。

本发明能够根据传感器获取机器人的力信息和位置信息，识别操作的运动意图，及时调整控制系统的导纳模型参数，缩短机器人的响应时间，提高机器人的响应速度，进而提高机器人在环境等效阻尼、刚度未知或变化条件下与环境接触作用中接触力、位置跟踪控制的稳定性和鲁棒性。

下面结合附图对本发明作进一步的详细说明。

附图说明

图1是本发明实施例1的多机器人系统操作系统的示意图；

图2是本发明实施例1多机器人闭链位置约束的示意图；

图3是本发明实施例1的原理框架示意图。

具体实施方式

实施例1

一种基于多机器人协同操作的自适应模糊力跟踪控制方法，如图1-3所示，包括以下步骤：

第一步：通过目标操作物的位置信息，根据闭链形成的位置约束分别求取每一机器人末端协调位置的约束方程，以得到每一机器人的末端期望位姿；

第二步：根据每一机器人进行逆运动学求解，求取执行每一机器人协调操作所需的各关节转角控制指令，得到每一机器人自身当前各关节的期望角度；

第三步：建立导纳控制模型，将每个机器人末端的力等效为输入的位置、将每个机器人末端的力误差等效为姿态增量，进行多机器人的力跟踪控制；

第四步：在所建立的导纳控制模型中引入学习率，通过模糊控制器根据目标操作物的动态变化对进行自适应调节，进行多机器人的力跟踪控制。

如图1-3所示，下面以两个串联机器人和一个并联机器人所组成的多机器人系统为例进行详细介绍：

如图1所示，多机器人进行紧协同操作时，假设每一机器人的末端执行器与操作物的接触点固连在一起或只存在很小的滑移，这样就可以近似的认为多机器人形成一条闭链。

根据目标操作物的位姿信息，考虑闭链形成的位置约束，即可分别得到多机器人末端的期望位姿，通过分别对多机器人进行逆运动学求解便可得到执行多机器人协调操作所需的各关节转角控制指令。

设定被操作工件的位姿用坐标系{E_L}或{X_LY_LZ_L}表示，其他坐标系含义如图2所示，具体为：

{X_BY_BZ_B}：多机器人的全局参考系；

{X_0aY_0aZ_0a}：第一机器人的基坐标系；

{X_eaY_eaZ_ea}：第一机器人的末端坐标系；

{X_0bY_0bZ_0b}：第二机器人的基坐标系；

{X_ebY_ebZ_eb}：第二机器人的末端坐标系；

{X_0cY_0cZ_0c}：第三机器人的基坐标系；

{X_ecY_ecZ_ec}：第三机器人的末端坐标系；

根据图2所示出的几何关系可知，可通过第一机器人的运动学计算工件坐标系在全局坐标系中的位姿，即：

^BT_L＝^BT_Oa ^OaT_Ea ^EaT_L (1)

其中，^BT_Oa为第一机器人基坐标系相对于全局坐标系的位姿转换矩阵；

其中，^OaT_Ea为第一机器人末端坐标系相对于基坐标系的位姿转换矩阵；

其中，^EaT_L为工件坐标系相对于第一机器人末端坐标系的位姿转换矩阵。

同理，也可通过第二机器人的运动学计算工件坐标系在本体坐标系中的位姿，即：

^BT_L＝^BT_Ob ^ObT_Eb ^EbT_L (2)

其中，^BT_Ob为第二机器人基坐标系相对于全局坐标系的位姿转换矩阵；

其中，^ObT_Eb为第二机器人末端坐标系相对于基坐标系的位姿转换矩阵；

其中，^EbT_L为工件坐标系相对于第二机器人末端坐标系的位姿转换矩阵。

同理，还可以通过第三机器人的运动学计算工件坐标系在本体坐标系中的位姿，即：

^BT_L＝^BT_Oc ^OcT_Ec ^EcT_L (3)

其中，^BT_Oc为第三机器人基坐标系相对于全局坐标系的位姿转换矩阵；

其中，^OcT_Ec为第三机器人末端坐标系相对于基坐标系的位姿转换矩阵；

其中，^EcT_L为工件坐标系相对于第三机器人末端坐标系的位姿转换矩阵。

在上述公式(1)(2)(3)中，矩阵^BT_Oa、^BT_Ob、^BT_Oc只与机器人的安装位置有关，为常值矩阵且可预先确定，而^EaT_L、^EbT_L、^EcT_L只与接触点有关，对于同样的工件，在操作精度得到保证的前提下，^EaT_L、^EbT_L、^EcT_L也为常值矩阵且可预先确定，而^OaT_Ea、^ObT_Eb、^ObT_Ec由第一机器人、第二机器人和第三机器人的正运动学方程确定，分别为：

^OcT_Ec＝fkine(Θ_c) (6)

其中，公式(4)(5)(6)中的n代表各个机器人的关节数。

当给定末端工件的期望位姿^BT_L后，由公式(1)、(2)、(3)可得：

综上，第一机器人、第二机器人和第三机器人之间需要满足的位置约束方程为：

^BT_Oa ^OaT_Ea ^EaT_L＝^BT_Ob ^ObT_Eb ^EbT_L＝^BT_Oc ^OcT_Ec ^EcT_L (10)

也即：

^BT_Oa·fkine(Θa)·^EaT_L＝^BT_Ob·fkine(Θ_b)·^EbT_L＝^BT_Oc·fkine(Θ_c)·^EcT_L (11)

此时，分别对单个机器人进行逆运动学求解便可得到执行多机器人协调操作所需的各关节转角控制指令。

根据传统的导纳控制的定义，需要预先给定一个参考位置X_r，然后才能在控制器上进行力的跟踪，X_r值的确定将会影响末端期望力的实现，确定其具体的数值是阻抗控制系统的一个重点。

将机器人和力传感器构成的系统建模为“质量-阻尼-弹簧”二阶系统，将环境建模为一阶弹簧系统，满足如下关系式：

式中，m为机器人的理想惯量，b为机器人的理想阻尼，k为机器人的理想刚度；

式中，x为实际位置，x_d为期望位置；

式中，Δf＝f_e-f_d，其中，f_e为机器人末端与环境的接触力，f_d为期望跟踪力；

同时，环境建模可看作一阶弹簧系统，将机器人末端与环境的接触力模型构建为弹簧模型，则机器人末端与环境的接触力与机器人嵌入环境之中的距离满足下述正比关系：

f_e＝k_e(x_e-x) (13)

式中，k_e为环境刚度，x_e为环境位置，x为机器人末端的实际位置。

根据公式(13)，能够得到机器人末端实际位置与受力之间的函数关系，机器人末端的实际位置可以用下式来表示：

机器人末端的实际位置与期望位置的偏差为：

此外：

Δf＝f_e-f_d (16)

则：

f_e＝Δf+f_d (17)

结合式(16)、(17)可得：

求取公式(18)的导数得到：

对公式(19)求取二次导数得：

又因为：

将公式(18)、(19)、(20)代入公式(21)进一步计算得到如下方程：

当多机器人系统趋于稳定时，期望跟踪力f_d维持不变，上式简化为：

k·k_e(x_e-x_r)＝k(Δf+f_d)+Δf·k_e＝(k_e+k)Δf+k·f_d (23)

进而可以得到机器人在稳定时的稳态力误差：

使

则此式代表了目标刚度与环境刚度之间的等效刚度，求取，求取机器人与环境实际接触力fss为：

在多机器人系统稳定后，能满足期望力要求，则力偏差△fss为零，此时：

通过公式(26)可知，当力偏差为0时，期望位置和期望力之间是一一对应关系，期望位置由初始接触位置、期望力、环境刚度确定，即需要己知精确的环境位置和环境刚度求得期望轨迹才可以使得实际接触力无误差的跟踪期望力。

然而，在大多数情况下，无法精确获得甚至是不可能获得精确的环境位置和环境刚度，并且通常环境刚度非常大，微小的位置偏差就可以导致非常大的接触力偏差。当在环境位置和刚度未知的情况时，即使对环境位置和环境刚度进行补偿，也很难使力跟踪误差为0，进而无法实现精确的力跟踪控制。

所以，发明人对导纳模型进行修正，具体为：

设定初始的环境位置x_e代替参考位置X_r，结合公式(15)可知：

f_e＝k_e(x_e-x) (27)

进而得到基准公式：

式中，e＝x-x_e；

由于环境位置未知，进一步设定

表示环境的估计值，定义：

将

代入基准公式(28)中，并引入自适应项Δb(t)与Δk(t)，得到力跟踪方程：

其中，Δb(t)与Δk(t)的表达式分别为：

式中，λ为控制器采样周期，η为学习率，根据劳斯判据系统稳定时，满足

学习率η的实时调整是优化动态响应的关键。模糊控制具有构造容易、鲁棒性好等特点，因此采用模糊调整器来实时调整学习率η。

模糊调整器的输入为力跟踪误差υf＝f_e-f_d和力跟踪误差的变化率dυf＝(f_e-f_d)/λ，输出为学习率η。选取论域，输入变量υf＝{NB,NM,NS,ZO,PS,PM,PB}，dυf＝{NB,NS,ZO,PS,PB}，输出变量η＝{SS,SB,M,BS,BB}，均采用高斯型。

模糊推理采用Mam-dani规则，学习率调整原理为：

当力跟踪误差较大时，应加大η的值，以加快系统的响应速度；

当力跟踪误差与变换化率为中等大小时，为保证较小的超调量与一定的响应速度，应让η维持在较小值；

当力跟踪误差较小时，需要根据变化率来决定输出值，进而获取更好的稳态性能；

当力误差变化率较小时，η取中等值；

当力误差变换速率较大时，η取较小值。

其中，模糊规则表见下述附表1：

附表1

虽然本发明以较佳实施例揭露如上，但并非用以限定本发明实施的范围。任何本领域的普通技术人员，在不脱离本发明的发明范围内，当可作些许的改进，即凡依照本发明所做的同等改进，应为本发明的范围所涵盖。

Claims (2)

1.一种基于多机器人协同操作的自适应模糊力跟踪控制方法，其特征在于包括以下步骤：

第一步：通过目标操作物的位置信息，根据闭链形成的位置约束分别求取每一机器人末端协调位置的约束方程，以得到每一机器人的末端期望位姿；

第二步：根据每一机器人进行逆运动学求解，求取执行每一机器人协调操作所需的各关节转角控制指令，得到每一机器人自身当前各关节的期望角度；

第三步：建立导纳控制模型，将每个机器人末端的力等效为输入的位置、将每个机器人末端的力误差等效为姿态增量，进行多机器人的力跟踪控制；

第四步：在所建立的导纳控制模型中引入学习率η，通过模糊控制器根据目标操作物的动态变化对η进行自适应调节，进行多机器人的力跟踪控制；

第三步中建立导纳控制模型将机器人与力传感器构成的系统建模构建为“质量—阻尼—弹簧”二阶系统，将环境建模构建为一阶弹簧系统，并建立如下方程：

式中，m为机器人的理想惯量，b为机器人的理想阻尼，k为机器人的理想刚度；

式中，x为实际位置，x_d为期望位置；

式中，Δf＝f_e-f_d，其中，f_e为机器人末端与环境的接触力，f_d为期望跟踪力；

将机器人末端与环境的接触力模型构建为弹簧模型，则机器人末端与环境的接触力与机器人嵌入环境之中的距离满足下述正比关系：

f_e＝k_e(x_e-x)

式中，k_e为环境刚度，x_e为环境位置，x为机器人末端的实际位置；

预先给定机器人末端的参考位置X_r，则：

机器人末端的实际位置为：

机器人末端的实际位置与期望位置的偏差为：

结合Δf＝f_e-f_d可得：

求取二次导数计算得到：

又因为

进一步计算得到如下方程：

当多机器人系统趋于稳定时，期望跟踪力f_d维持不变，上式简化为：

k·k_e(x_e-x_r)＝k(Δf+f_d)+Δf·k_e＝(k_e+k)Δf+k·f_d

进而可以得到机器人在稳定时的稳态力误差：

使

此式代表了目标刚度与环境刚度之间的等效刚度；

求取f_ss为：

在多机器人系统稳定后，满足期望力要求，此时力偏差△f_ss为零，得到：

设定初始的环境位置x_e代替参考位置X_r，结合公式

可知：f_e＝k_e(x_e-x)

进而得到基准公式：

式中，e＝x-x_e；

进一步设定

表示环境的估计值，定义：

将

代入基准公式中，并引入自适应项Δb(t)与Δk(t)，得到力跟踪方程：

其中，Δb(t)与Δk(t)的表达式分别为：

式中，λ为控制器采样周期，η为学习率，根据劳斯判据系统稳定时，满足

第四步中模糊调整器的输入为力跟踪误差Δf＝f_e-f_d和力跟踪误差的变化率dΔf＝(f_e-f_d)/λ，输出为学习率η，其中模糊推理采用Mam-dani规则，学习率η调整原理为：

当力跟踪误差较大时，应加大η的值，以加快系统的响应速度；

当力跟踪误差与变换化率为中等大小时，为保证较小的超调量与一定的响应速度，应让η维持在较小值；

当力跟踪误差较小时，需要根据变化率来决定输出值，进而获取更好的稳态性能；

当力误差变化率较小时，η取中等值；

当力误差变换速率较大时，η取较小值。

2.如权利要求1所述的基于多机器人协同操作的自适应模糊力跟踪控制方法，其特征在于，第一步中多机器人所满足的约束方程为：

其中，n≥3；

其中，

为第n个机器人基坐标系相对于全局坐标系的位姿转换矩阵；

其中，

为第n个机器人末端坐标系相对于基坐标系的位姿转换矩阵；

其中，^EnT_L为工件坐标系相对于第n个机器人末端坐标系的位姿转换矩阵。

Images