CN112372630B - 一种多机械臂协同打磨力柔顺控制方法和系统 - Google Patents

Abstract

本发明提供了一种多机械臂协同打磨力柔顺控制方法和多机械臂协同打磨力柔顺控制系统，控制方法包括1)给定目标物体的参考位置、期望抓取内力和期望打磨力；2)对两个抓取机械臂协作所形成的闭链进行位置分解，建立闭链动力学位置约束方程；3)通过力传感器获取两个抓取机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理；4)建立动态自适应阻抗控制方程，根据真实内力和期望内力得到两个抓取机械臂末端的位置补偿，根据打磨机械臂末端对目标物体的作用力信号和期望打磨力得到打磨机械臂末端的位置补偿。本发明能够在确保多机械臂协同操作的同时，将机械臂末端夹持力控制在期望值的范围，并且实现恒定的打磨力控制。

Description

一种多机械臂协同打磨力柔顺控制方法和系统

技术领域

本发明属于多机械臂协同控制领域，具体涉及一种多机械臂协同打磨力柔顺控制方法和多机械臂协同打磨力柔顺控制系统。

背景技术

近年来，为推动智能制造的发展，越来越多的企业将机器人引入到打磨作业中，相比原始的手工打磨，大大提高了生产效率，而且机器人具有良好的力控制性能和很高的重复定位精度，相比一个熟练的工人，可以保证待打磨工件获得更高的表面精度。

传统的机器人打磨方式是用一个机械臂去打磨固定在工作台上的工件，由于机器人的工作空间有限，一般只能打磨单个面，对于回转体或多个面需要打磨的工件来说，要将工件调整位姿，涉及到工件的重新装夹和坐标标定等问题，会影响加工效率。

目前出现多机械臂协同打磨的方案，用两个机械臂将待打磨的工件夹持，另一个机械臂作为打磨机器人，当打磨完一个面后，通过双臂调整工件的位姿，继续打磨其他面。这种多机器人协作的打磨方案相比单臂打磨，可以提高生产效率，对于回转体或多个面需要打磨的工件更有效，对于一些形状复杂的大型工件，可以通过双臂协作调整工件位姿，使得打磨系统具有更佳的灵巧性和更大的工作空间。

在多机械臂协同打磨作业中，双臂与被夹持工件形成一个闭链系统，双臂系统存在位置及内力的强耦合约束，传统的双臂协同控制通常采用位置级的协同控制方法或主从式控制方法，纯位置控制仅仅考虑了双臂系统的位置约束，无法控制内力，在工件运动的过程中，会因为机械臂协同的位置误差，产生很大的内力，损坏机械臂或工件。而主从式控制方法要求从臂具有很高的响应速度，否则难以控制内力在期望值附近。

在机器人打磨作业的力控中，通常采用力位混合控制和阻抗控制，在传统的力柔顺控制方法中，没有考虑环境的不确定性所带来的建模误差，因此在进行机器人力跟踪时，会存在稳态误差。打磨机器人在进行打磨作业时，由于电主轴高速旋转，会带来很大的随机噪声，使得机器人的力控算法不稳定。为解决打磨中的随机噪声干扰问题，必须对力传感器采集的数据滤波，而常用的均值滤波得到的数据往往有很多较小的尖峰，难以得到平滑的曲线，影响力跟踪效果。

因此，如何解决噪声干扰，并实现不确定接触环境下的精确力跟踪，是一个亟待解决的问题。

发明内容

针对现有技术的缺陷，本发明提供了一种多机械臂协同打磨力柔顺控制方法和多机械臂协同打磨力柔顺控制系统，确保多机械臂协同操作的同时，将机械臂末端夹持力控制在期望值的附近，并且实现恒定的打磨力控制。

为了实现上述目的，一方面本发明提供了一种多机械臂协同打磨力柔顺控制方法，利用两个抓取机械臂进行协同搬运，利用打磨机械臂进行打磨，其包括以下步骤：

1)给定目标物体的参考位置、期望抓取内力和期望打磨力；

2)对两个抓取机械臂协作所形成的闭链进行位置分解，建立闭链动力学位置约束方程，将目标物体的参考位置分解为两个抓取机械臂末端的期望位置；

3)通过力传感器获取两个抓取机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理，将卡尔曼滤波处理后的作用力信号进行分解得到真实内力和真实外力；通过力传感器获取打磨机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理；

4)建立动态自适应阻抗控制方程；

抓取机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_Idi(i＝1,2)表示两个机械臂末端期望的抓取内力，f_Iri(i＝1,2)表示两个机械臂末端真实的内力，M_I表示惯性矩阵，B_I表示阻尼矩阵，K_I表示刚度矩阵，X_mci(i＝1,2)表示两个机械臂末端期望位置，X_ci(i＝1,2)表示经过阻抗控制器修正后的两个机械臂末端实际位置，

分别表示X_ci的一阶导数、二阶导数，

分别表示X_mci的一阶导数、二阶导数；

根据真实内力和期望内力得到两个抓取机械臂末端的位置补偿，以修正两个抓取机械臂的末端位置，通过运动学逆解得到关节角进而控制抓取机械臂末端达到期望接触力；

打磨机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_d表示期望的打磨力，f_e表示真实的打磨力，m,b,k分别表示惯性系数、阻尼系数、刚度系数，x_c表示打磨机械臂末端真实位置，x_e表示打磨机械臂末端参考位置，

分别表示x_c的一阶导数、二阶导数，

分别表示x_e的一阶导数、二阶导数；

根据打磨机械臂末端对目标物体的真实作用力和期望打磨力得到打磨机械臂末端的位置补偿，以修正打磨机械臂的末端位置，保持恒定的打磨力大小。

根据本发明的另一种具体实施方式，在步骤2)中首先根据给定的目标物体参考轨迹，得到物体质心坐标系在世界坐标系中的位姿，再根据闭链运动学约束，得到两个抓取机械臂末端坐标系相对于其基坐标系的坐标变换。

根据本发明的另一种具体实施方式，步骤3)中对力信号进行卡尔曼滤波处理过程包括建立力信号预测方程和力信号观测方程；

建立的力信号预测方程：

其中，F_k表示第k时刻的力信号值，

表示第k时刻的力信号的变化率，T表示采样周期，u₁和u₂表示模型的扰动，也即过程噪声；

离散状态方程为：

X_k+1＝AX_k+U_k

其中，X_k为第k时刻的状态变量，A为状态转移矩阵，U_k为过程噪声，状态变量为六维力和六维力的变化率，即

则状态转移矩阵为：

建立的力信号观测方程为：

Y_k+1＝HX_k+V_k

其中，Y_k为第k时刻的观测向量，H为观测矩阵，V_k为系统的观测噪声，观测向量为六维力信息，即：Y_k＝F_k，观测矩阵为：H＝[I_6×60_6×6]；

则，步骤3)中卡尔曼滤波处理的迭代方程为：

其中，

为第k时刻的力感知信息预测值，X_k为第k时刻的力感知信息最优估计值，

为第k时刻预测的误差协方差矩阵，P_k为第k时刻的后验误差协方差矩阵，K为卡尔曼增益矩阵，Q为过程噪声，R为观测噪声。

根据本发明的另一种具体实施方式，步骤4)中的α通过力跟踪误差动态调整，调整方法为：

α＝Ue^-βΔf

其中，U为自适应更新率的上限值，e为自然指数，β为自适应更新率的调整系数，Δf＝f_d-f_e代表力跟踪误差。

另一方面，本发明提供了一种实现前述多机械臂协同打磨力柔顺控制方法的多机械臂协同打磨力柔顺控制系统，其包括：

工控机；

两个抓取机械臂，在抓取机械臂的执行末端装有抓取用的夹手；

一个打磨机械臂，在打磨机械臂的执行末端装有打磨头；

以及三个六维力传感器；

两个抓取机械臂、打磨机械臂与工控机之间通信连接，三个六维力传感器分别设置于抓取机械臂和打磨机械臂的执行末端，并且六维力传感器与工控机之间通信连接。

根据本发明的另一种具体实施方式，打磨机械臂的执行末端装有电主轴，打磨头安装在电主轴上并由电主轴进行驱动。

根据本发明的另一种具体实施方式，电主轴与工控机之间控制连接，并且所述电主轴采用速度闭环的PID控制。

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

本发明将卡尔曼滤波用于力信号的滤波处理，相比均值滤波，可以得到更为平滑的力信息曲线，有利于打磨力的稳定控制，同时将自适应控制与阻抗控制结合，通过力跟踪误差对阻抗模型进行补偿，以消除因为环境位置的估计不准确所带来的力跟踪误差。

此外本发明采用基于位置的阻抗控制，仅通过末端力的检测及位置闭环就可实现力的闭环控制，适用于现有的工业机器人，具有实时性强，可操作性高的优点。

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

附图说明

图1是本发明控制方法中双机械臂协作所形成闭链约束的示意图；

图2是本发明双机械臂协作所夹取的目标物体的受力示意图；

图3是本发明动态自适应阻抗控制的框架示意图；

图4是本发明两个抓取机械臂的动态自适应阻抗控制的示意图；

图5是本发明控制系统的架构示意图。

具体实施方式

如图1-4所示，一种多机械臂协同打磨力柔顺控制方法，利用两个抓取机械臂进行协同搬运，利用打磨机械臂进行打磨，其包括以下步骤：

1)给定目标物体的参考位置、期望内力(f_Id1、f_Id2)和期望打磨力；

其中根据目标物体的参考位置得到其运动轨迹X_md，选取两个抓取机械臂阻抗控制器的阻抗参数M_I、B_I、K_I，以及自适应更新率上限值U，调整系数β；

其中设置恒定的打磨力f_d，选取打磨机械臂阻抗控制器的阻抗参数m,b,k，以及自适应更新率上限值U，调整系数β；

其中设置卡尔曼滤波器的过程噪声Q和观测噪声R。

2)对两个抓取机械臂协作所形成的闭链进行位置分解，将目标物体的参考位置分解为两个抓取机械臂末端的期望位置：

如图1所示，由两个抓取机械臂的运动学可得到目标物体质心坐标系在世界坐标系中的位姿：

^WT_L＝^WT_Oa ^OaT_Ea ^EaT_L

^WT_L＝^WT_Ob ^ObT_Eb ^EbT_L

当给定目标物体的期望位姿^WT_L后，可得两个抓取机械臂末端相对于其基坐标系的位姿：

^OaT_Ea＝(^WT_Oa)^-1·^WT_L·(^EaT_L)^-1

^ObT_Eb＝(^WT_Ob)^-1·^WT_L·(^EbT_L)^-1

通过闭链动力学约束将目标物体位姿分解到两个抓取机械臂末端，便可实现两个抓取机械臂的位置级的协同控制。

同时，对两个抓取机械臂末端作用力进行分解：

如图2所示的目标物体的受力情况，图中f_L、τ_L为外界环境作用在所夹持的目物体上的外力；f_ea、τ_ea为抓取机械臂a末端施加给目标物体的作用力；f_eb、τ_eb为抓取机械臂b末端施加给目标物体的作用力，r_L、r_a、r_b分别为目标物体质心到力f_L、f_a、f_eb作用点的位置矢量；v_L、ω_L为目标物体质心的线速度和角速度，m_L为目标物体的质量，I_L为目标物体质心转动惯量，G_L为目标物体所受的重力。

由牛顿-欧拉法建立目标物体的动力学方程：

进一步简化为

其中，F_o表示抓取机械臂作用于抓取目标质心的等效广义力，M_o表示目标物体的惯性矩阵，

表示被抓取目标的质心加速度，C_o表示目标物体的离心力、科氏力和重力的合力矢量，F_e表示外界环境作用于抓取目标质心的等效广义力；

双臂末端作用力的分解；

两个抓取机械臂作用于目标物体上的合力可以表述为：

F_o＝WF_i

其中，W表示抓取矩阵，

E为三阶单位矩阵，O为三阶零矩阵，

F_i表示两个抓取机械臂末端对目标物体的广义力，可由两个抓取机械臂末端的力传感器采集得到；

作用于物体上的广义抓取力可以分解为相互正交的内力和外力两部分，利用伪逆得到一种分解形式如下：

其中，W⁺是W的广义逆矩阵。

3)通过力传感器获取两个抓取机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理，将卡尔曼滤波处理后的作用力信号进行分解得到真实内力和真实外力；通过力传感器获取打磨机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理；

力信号的预处理：

将卡曼尔滤波器的初始状态设为零，通过模型预测得到力信息预测值，读取力传感器的数据，作为观测量，再通过卡尔曼系数加权，对预测值进行修正，得到后验估计值。多次迭代得到作用力信息的最优估计值，具体包括建立力信号预测方式和力信号观测方程：

建立力信号预测方程(二阶模型)：

其中，F_k表示第k时刻的力信号值，

表示第k时刻的力信号的变化率，T表示采样周期，u₁和u₂表示模型的扰动，也即过程噪声；

离散状态方程为：

X_k+1＝AX_k+U_k

其中，X_k为第k时刻的状态变量，A为状态转移矩阵，U_k为过程噪声，状态变量为六维力和六维力的变化率，即

状态转移矩阵为：

建立力信号观测方程：

Y_k+1＝HX_k+V_k

其中，Y_k为第k时刻的观测向量，H为观测矩阵，V_k为系统的观测噪声，其中观测向量为六维力信息，即：Y_k＝F_k，则观测矩阵为H＝[I_6×60_6×6]；

因此，可以得到卡尔曼滤波处理的迭代方程为：

其中，

为第k时刻的力感知信息预测值，X_k为第k时刻的力感知信息最优估计值，

为第k时刻预测的误差协方差矩阵，P_k为第k时刻的后验误差协方差矩阵，K为卡尔曼增益矩阵，Q为过程噪声，R为观测噪声。

4)建立动态自适应阻抗控制方程；

抓取机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_Idi(i＝1,2)表示两个机械臂末端期望的抓取内力，f_Iri(i＝1,2)表示两个机械臂末端真实的内力，M_I表示惯性矩阵，B_I表示阻尼矩阵，K_I表示刚度矩阵，X_mci(i＝1,2)表示两个机械臂末端期望位置，X_ci(i＝1,2)表示经过阻抗控制器修正后的两个机械臂末端实际位置，

分别表示X_ci的一阶导数、二阶导数，

分别表示X_mci的一阶导数、二阶导数。

根据真实内力和期望内力得到两个抓取机械臂末端的位置补偿，以修正两个抓取机械臂的末端位置，通过运动学逆解得到关节角进而控制抓取机械臂末端达到期望接触力，具体如图4所示。

在图4中，X_md表示目标物体的参考轨迹，X_mc1、X_mc2分别表示抓取机械臂a和抓取机械臂b末端执行器的参考轨迹；X_c1、X_c2分别表示抓取机械臂a和抓取机械臂b末端执行器的真实轨迹；ΔX_i(i＝1，2)表示阻抗控制器的位置补偿，f_Idi(i＝1，2)表示期望的内力，f_Iri(i＝1，2)表示两个抓取机械臂末端执行器施加在目标物体上的内力。

首先，通过闭链运动学约束方程，将工件的参考位置X_md分解为两个抓取机械臂末端执行器的期望位置(X_mc1、X_mc2)，通过对抓取机械臂末端力分解，将力传感器采集的数据分解为内力(f_Ir1、f_Ir2)和外力(f_Er1、f_Er2)两部分，此处只考虑抓取机械臂对目标物体的夹持内力，将期望内力f_Idi和真实内力f_Iri作为动态自适应阻抗控制器的输入，利用阻抗控制器得到两个抓取机械臂末端的位置补偿(ΔX₁、ΔX₂)，再修正两个抓取机械臂的末端位置，通过运动学逆解得到关节角，进而控制两个抓取机械臂末端达到期望接触力。

打磨机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_d表示期望的打磨力，f_e表示真实的打磨力，m,b,k分别表示惯性系数、阻尼系数、刚度系数，x_c表示打磨机械臂末端真实位置，x_e表示打磨机械臂末端参考位置，

分别表示x_c的一阶导数、二阶导数，

分别表示x_e的一阶导数、二阶导数。

根据打磨机械臂末端对目标物体的真实作用力和期望打磨力得到打磨机械臂末端的位置补偿，以修正打磨机械臂的末端位置，保持恒定的打磨力大小。

动态自适应阻抗控制的具体实现方法如下：

具体如图3所示的动态自适应阻抗控制的框架示意图，将动态自适应阻抗控制算法离散化为以下形式：

给定估计的参考轨迹

通过该方程迭代得到机械臂末端位置x_c后，调用机械臂逆运动学解即可得到对应的关节角，进而控制机械臂的运动。

另一方面同时提供了一种实现前述多机械臂协同打磨力柔顺控制方法的多机械臂协同打磨力柔顺控制系统，如图5所示，包括工控机、两个抓取机械臂、一个打磨机械臂和三个六维力传感器，在抓取机械臂的执行末端装有夹手，在打磨机械臂的执行末端装有砂纸打磨头，两个抓取机械臂、打磨机械臂与工控机例如通过交换机建立通信连接，三个六维力传感器分别设置于抓取机械臂和打磨机械臂的执行末端，并且三个六维力传感器与工控机也通过例如通过交换机建立通信连接。

其中打磨机械臂的执行末端装有电主轴，打磨头装在电主轴上并由电主轴进行驱动，并且电主轴通过STM32开发板，采用速度闭环的PID控制。

其中每个机械臂的控制器，都优选通过交换机与工控机之间采用以太网集中通讯。

力传感器采集的信号经过滤波和重力补偿后，反馈给工控机(上位机)控制软件；工控机将计算出的关节角发送给机械臂控制器，机械臂控制器根据规划的期望关节角度，驱动机械臂完成任务轨迹，并实现内力的控制；而打磨机器人根据规划的刀具路径点运动，并通过力控保持恒定的打磨力。

具体的打磨作业流程为：两个抓取机械臂从起点运动到指定位置，将目标物体抓起，再把目标物体搬运到设定的打磨工位，打磨机械臂开始打磨工件，打磨完一个面后，通过两个抓取机械臂调整目标物体的姿态，再打磨另一个面，最后将目标物体送回指定位置，两个抓取机械臂和打磨机械臂回到初始位置，单周期打磨完成。

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

Claims (6)

1.一种多机械臂协同打磨力柔顺控制方法，利用两个抓取机械臂进行协同搬运，利用打磨机械臂进行打磨，其包括以下步骤：

1)给定目标物体的参考位置、期望抓取内力和期望打磨力；

2)对两个抓取机械臂协作所形成的闭链进行位置分解，建立闭链动力学位置约束方程，将目标物体的参考位置分解为两个抓取机械臂末端的期望位置；

3)通过力传感器获取两个抓取机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理，将卡尔曼滤波处理后的作用力信号进行分解得到真实内力和真实外力；通过力传感器获取打磨机械臂末端对目标物体的作用力信号，对该作用力信号进行卡尔曼滤波处理；

其中，对力信号进行卡尔曼滤波处理过程包括建立力信号预测方程和力信号观测方程；

建立的力信号预测方程：

其中，F_k表示第k时刻的力信号值，

表示第k时刻的力信号的变化率，T表示采样周期，u₁和u₂表示模型的扰动，也即过程噪声；

离散状态方程为：

X_k+1＝AX_k+U_k

其中，X_k为第k时刻的状态变量，A为状态转移矩阵，U_k为过程噪声，状态变量为六维力和六维力的变化率，即

则状态转移矩阵为：

建立的力信号观测方程为：

Y_k+1＝HX_k+V_k

其中，Y_k为第k时刻的观测向量，H为观测矩阵，V_k为系统的观测噪声，观测向量为六维力信息，即：Y_k＝F_k，观测矩阵为：H＝[I_6×6 0_6×6]；

则，步骤3)中卡尔曼滤波处理的迭代方程为：

其中，

为第k时刻的力感知信息预测值，X_k为第k时刻的力感知信息最优估计值，

为第k时刻预测的误差协方差矩阵，P_k为第k时刻的后验误差协方差矩阵，K为卡尔曼增益矩阵，Q为过程噪声，R为观测噪声；

4)建立动态自适应阻抗控制方程；

抓取机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_Idi(i＝1,2)表示两个机械臂末端期望的抓取内力，f_Iri(i＝1,2)表示两个机械臂末端真实的内力，M_I表示惯性矩阵，B_I表示阻尼矩阵，K_I表示刚度矩阵，X_mci (i＝1,2)表示两个机械臂末端期望位置，X_ci(i＝1,2)表示经过阻抗控制器修正后的两个机械臂末端实际位置，

分别表示X_ci的一阶导数、二阶导数，

分别表示X_mci的一阶导数、二阶导数；

根据真实内力和期望内力得到两个抓取机械臂末端的位置补偿，以修正两个抓取机械臂的末端位置，通过运动学逆解得到关节角进而控制抓取机械臂末端达到期望接触力；

打磨机械臂的动态自适应阻抗控制方程为：

其中，λ为采样周期，α为更新率，ψ表示自适应补偿，f_d表示期望的打磨力，f_e表示真实的打磨力，m,b,k分别表示惯性系数、阻尼系数、刚度系数，x_c表示打磨机械臂末端真实位置，x_e表示打磨机械臂末端参考位置，

分别表示x_c的一阶导数、二阶导数，

分别表示x_e的一阶导数、二阶导数；

根据打磨机械臂末端对目标物体的真实作用力和期望打磨力得到打磨机械臂末端的位置补偿，以修正打磨机械臂的末端位置，保持恒定的打磨力大小。

2.如权利要求1所述的多机械臂协同打磨力柔顺控制方法，其中在步骤2)中首先根据给定的目标物体参考轨迹，得到物体质心坐标系在世界坐标系中的位姿，再根据闭链运动学约束，得到两个抓取机械臂末端坐标系相对于其基坐标系的坐标变换。

3.如权利要求1所述的多机械臂协同打磨力柔顺控制方法，其中步骤4)中的α通过力跟踪误差动态调整，调整方法为：

α＝Ue^-β|Δf|

其中，U为自适应更新率的上限值，e为自然指数，β为自适应更新率的调整系数，Δf＝f_d-f_e代表力跟踪误差。

4.一种实现如权利要求1-3任一项所述的多机械臂协同打磨力柔顺控制方法的多机械臂协同打磨力柔顺控制系统，其包括：

工控机；

两个抓取机械臂，在所述抓取机械臂的执行末端装有抓取用的夹手；

一个打磨机械臂，在所述打磨机械臂的执行末端装有打磨头；

以及三个六维力传感器；

两个所述抓取机械臂、所述打磨机械臂与所述工控机之间通信连接，三个所述六维力传感器分别设置于所述抓取机械臂和所述打磨机械臂的执行末端，并且所述六维力传感器与所述工控机之间通信连接。

5.如权利要求4所述的多机械臂协同打磨力柔顺控制系统，其中所述打磨机械臂的执行末端装有电主轴，所述打磨头安装在所述电主轴上并由所述电主轴进行驱动。

6.如权利要求5所述的多机械臂协同打磨力柔顺控制系统，其中所述电主轴与所述工控机之间控制连接，并且所述电主轴采用速度闭环的PID控制。

Images