CN114872046B - 一种基于激励轨迹的机器人末端负载快速辨识方法 - Google Patents

Abstract

本发明公开一种基于激励轨迹的机器人末端负载快速辨识方法，属于机器人控制领域，包括建立机器人和机器人末端负载的空间坐标系，获得重力和惯性力的综合表达式；设计机器人末端各关节激励轨迹，以此改变机器人末端姿态；采集机器人在不同姿态下的数据，通过最小二乘法得到机器人末端负载的各项参数。本发明可快速辨识机器人末端的负载参数，弥补了辨识效率低和精度差的问题，使之能适应当前高速与高精度的力控工业场景中。

Description

一种基于激励轨迹的机器人末端负载快速辨识方法

技术领域

本发明属于机器人控制技术领域，具体是涉及一种基于激励轨迹的机器人末端负载快速辨识方法。

背景技术

为了实现各种柔顺/力控时，在机器人机械臂末端加装6维力传感器是比较典型的做法，其可以感知其后方工具重力以及真实施加的有效负载力，然后作为反馈可以实现各种力控，比如力控笛卡尔空间拖动示教、末端力控等。在机器人接触作业中，依靠安装在末端多维力传感器感知外部的接触力。但当机械臂的有效载荷和速度较大时，其产生的重力/惯性力对力传感器的数据输出将产生不可忽视的影响，从而严重影响力控的精度和效果。

针对这些问题，目前有不少的文献提出了重力补偿算法，但目前的大部分算法都只采集了空间有限点进行重力补偿的计算，补偿的精度相对较差且运行的效率也非常低，已经无法满足现有的工业现场的高精度高速率的力控要求，将很大程度地影响力控作业的效率。

发明内容

发明目的：本发明目的在于针对现有技术的不足，提供一种基于激励轨迹的机器人末端负载快速辨识方法，设计基于激励轨迹的最优空间辨识位姿，各关节的激励轨迹采用有限项的傅里叶级数表示，采用最小二乘法计算得到负载的辨识参数，解决机器人末端负载辨识效率低和精度差的问题。

技术方案：本发明所述基于激励轨迹的机器人末端负载快速辨识方法，包括如下步骤：

S10、建立机器人和机器人末端负载的空间坐标系，并根据建立的坐标系关系，分别推导负载辨识计算公式和惯性力影响公式，从而获得重力和惯性力的综合表达式；

S20、基于优化目标函数及各限制条件，设计机器人末端各关节激励轨迹，以此改变机器人末端姿态；

S30、在采集机器人末端姿态基于激励轨迹改变时，采集至少12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，最后基于重力和惯性力的综合表达式通过最小二乘法得到机器人末端负载的各项参数。

本发明进一步优选地技术方案为，步骤S10获得重力和惯性力的综合表达式的具体方法为：

S1001、首先建立参考坐标系，设定世界坐标系为W，机器人基坐标系为B，机器人末端坐标系为E，六维力传感器自身的坐标系为FT：其中六维力传感器坐标系的原点为O，坐标系的方向和传感器厂家给定的坐标系方向一致；

然后确定坐标系之间的变换矩阵，通过所建立的坐标系，得到六维力传感器坐标系相对于机器人末端坐标系的齐次变换矩阵为对于任意给定的末端位子/>末端负载的重力在FT坐标系中的各个轴上均有分量；

S1002、假设六维力传感器的三个力的初始值为和/>三个力矩的初始值为/>和/>负载重力为G以及负载重心在力传感器坐标系中的位置为x，y和z，则负载重力在传感器坐标系下的作用力分量为F_gx、F_gy和F_gz，作用力矩分量为T_gx、T_gy和T_gz，六维力传感器实测的作用力反馈值为F_x、F_y和F_g，作用力矩为T_x、T_y和T_z；

S1003、由力与力矩的关系以及右手定则可得公式(1)：

其中：

通过以上公式，并对多组数据采样，最终通过最小二乘法可以得到负载质心坐标：

p＝(F^TF)^-1·F^Tτ (3)；

其中：p＝[x y zk₁ k₂ k₃]^T

在机器人基坐标系的z轴和重力的方向不一致，存在安装倾角时，假设机器人基座与世界坐标系的x轴和y轴方向的偏角即安装倾角分别为U和V，且重力在世界坐标系中的方向向量为g_W＝[0 0 -1]^T，设负载重力为G，得到：

S1004、机器人在快速运动中，在负载的作用下，机器人末端产生惯性F_inert和惯性力矩T_inert，则传感器的实测值F_meas与T_meas由期望跟踪力与力矩F_des与T_des以及惯性力与力矩组成，即：

S1005、通过公式(4)和公式(5)得到重力和惯性力的综合表达式：

优选地，步骤S20中设计机器人末端各关节激励轨迹的具体方法为：

S2001、找到与关节参数相关的方程组，将其作为激励方程，即考虑惯性力下的力矩方程组：

由于负载的加速度为负载绕末端坐标系旋转产生的加速度与坐标系本身加速度的矢量和，则有：

其中α是机器人末端坐标系的角加速度，是末端坐标系的加速度；得到：

T_a＝a·p (9)；

其中：

a是关于θ₄、θ₅、θ₆的函数，记为a＝f(q₄，q₅，q₆)；

S2002、各个关节的激励轨迹用有限项的傅里叶级数表示，则机器人第i关节的角位移q_i、角速度以及角加速度/>表示为：

式中的N表示正弦和余弦的项数，w_f＝2πf_f是基波角频率，a_i，l和b_i，l是待确定的参数；

为使机器人末端尽可能的充满整个空间，设优化的目标函数为minf(a)；由于重力补偿只涉及机械臂末端的末三轴，因此共有6N个参数待优化；为满足机器人关节限制条件，给出如下关节约束表达式：

启停约束：

S2003、考虑辨识点姿态“不具有代表性”及传感器数据浮动等因素，M_a、a和p均受到扰动，则扰动后的方程组为：

T_a+δT_a＝(a+δa)·(p+δp) (15)；

由此得到：

δp＝a^-(δT_a-δa·p-δa·δp) (16)；

由||δp||≤||a^-||(||δT_a||+||δa||·||p||+||δa||·||δp||)，解出：

由于T_a＝a·p，则||T_a||≤||a||·||p||，因此式(16)写成：

则辨识参数b的相对误差为：

由于是相对扰动项，辨识参数矩阵p的误差主要由||a^-||·||a||引起，在此引入条件数为激励轨迹的优化目标函数：

Cond(a)＝||a^-||·||a|| (20)；

因此得到：

则优化目标函数为：

minf(a)＝||a^-||·||a||(22)；

在满足机器人各关节位置、速度、加速度条件下，当优化目标函数minf(a)值最小时即表示外界扰动对辨识参数矩阵p的扰动最小，对应的参数a_i，l、b_i，l即激励轨迹对应参数；先假定工件质心在机器人末端坐标系中的位置矢量为一已知量p，并将其代入式(9)，从而激励出具有代表性的机器人位姿。

优选地，步骤S30获得机器人末端负载的各项参数的具体方法为：

通过重力和惯性力的综合表达式，可以得到如下公式：

F＝RL (23)；

其中：

通过最小二乘法可以得到：

L＝(R^TR)^-1·R^TF (24)；

H＝(R^TR)^-1·R^TT (25)；

方程(25)中：

采集至少12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，带入公式(24)和公式(25)，求出负载重力G，基座倾角U和V，传感器零点值 和/>负载质心x、y和z共12个参数的数值。

有益效果：本发明通过设计一条尽可能代表空间预期姿态的激励轨迹，并采用有限项的傅里叶级数表示各个关节的激励轨迹，然后通过最小二乘法拟合N组机器人末端不同姿态下的数据并结合参数化模型对机器人末端负载进行快速辨识。本发明可快速辨识机器人末端的负载参数，弥补了辨识效率低和精度差的问题，使之能适应当前高速与高精度的力控工业场景中。

附图说明

图1为本发明实施例中坐标系和负载重力示意图；

图2为本发明实施例中末三轴的关节角度运动轨迹；

图3为本发明实施例中末端负载位置曲线；

图4为本发明实施例中末端负载速度曲线；

图5为本发明实施例中末端负载加速度曲线。

具体实施方式

下面通过附图对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

实施例：一种基于激励轨迹的机器人末端负载快速辨识方法，包括如下步骤：

S10、选定一款六维力传感器安装在机器人末端，建立机器人和机器人末端负载的空间坐标系，并根据建立的坐标系关系，分别推导负载辨识计算公式和惯性力影响公式，从而获得重力和惯性力的综合表达式；

S1001、首先建立参考坐标系，设定世界坐标系为W，机器人基坐标系为B，机器人末端坐标系为E，六维力传感器自身的坐标系为FT；其中六维力传感器坐标系的原点为O，坐标系的方向和传感器厂家给定的坐标系方向一致；

然后确定坐标系之间的变换矩阵，通过所建立的坐标系，得到六维力传感器坐标系相对于机器人末端坐标系的齐次变换矩阵为对于任意给定的末端位子/>末端负载的重力在FT坐标系中的各个轴上均有分量；

S1002、假设六维力传感器的三个力的初始值为和/>三个力矩的初始值为/>和/>负载重力为G以及负载重心在力传感器坐标系中的位置为x，y和z，则负载重力在传感器坐标系下的作用力分量为F_gx、F_gy和F_gz，作用力矩分量为T_gx、T_gy和T_gz，六维力传感器实测的作用力反馈值为F_x、F_y和F_g，作用力矩为T_x、T_y和T_z；

S1003、由力与力矩的关系以及右手定则可得公式(1)：

其中：

通过以上公式，并对多组数据采样，最终通过最小二乘法可以得到负载质心坐标：

p＝(F^TF)^-1·F^Tτ (3)；

其中：p＝[x y zk₁ k₂ k₃]^T

在机器人基坐标系的z轴和重力的方向不一致，存在安装倾角时，假设机器人基座与世界坐标系的x轴和y轴方向的偏角即安装倾角分别为U和V，且重力在世界坐标系中的方向向量为g_W＝[0 0 -1]^T，设负载重力为G，得到：

S1004、机器人在快速运动中，在负载的作用下，机器人末端产生惯性F_igert和惯性力矩T_igert，则传感器的实测值F_meas与T_meas由期望跟踪力与力矩F_des与T_des以及惯性力与力矩组成，即：

S1005、通过公式(4)和公式(5)得到重力和惯性力的综合表达式：

S20、重力和惯性力的各个未知参数通过改变末端的姿态可辨识得到(即与末三轴有关)，为使得选取的姿态能尽可能代表辨识空间，需要设计一条最优的关节参考轨迹。将各关节的激励轨迹用有限项的傅里叶级数表示，设计一条最优关节参考轨迹，以此改变末端姿态辨识得到重力的各个未知参数。将傅里叶级数中的正余弦项数取为N＝5，基频f_f选定为0.1Hz，采样频率取100Hz，总共1000次采样来计算矩阵a。

S2001、找到与关节参数相关的方程组，将其作为激励方程，即考虑惯性力下的力矩方程组：

由于负载的加速度为负载绕末端坐标系旋转产生的加速度与坐标系本身加速度的矢量和，则有：

其中α是机器人末端坐标系的角加速度，是末端坐标系的加速度；得到：

T_a＝a·p (9)；

其中：

a是关于θ₄、θ₅、θ₆的函数，记为a＝f(q₄,q₅,q₆)；

S2002、各个关节的激励轨迹用有限项的傅里叶级数表示，则机器人第i关节的角位移q_i、角速度以及角加速度/>表示为：

式中的N表示正弦和余弦的项数，w_f＝2πf_f是基波角频率，a_i,l和b_i,l是待确定的参数；

为使机器人末端尽可能的充满整个空间，设优化的目标函数为minf(a)；由于重力补偿只涉及机械臂末端的末三轴，因此共有6N个参数待优化；为满足机器人关节限制条件，给出如下关节约束表达式：

启停约束：

S2003、考虑辨识点姿态“不具有代表性”及传感器数据浮动等因素，M_a、a和p均受到扰动，则扰动后的方程组为：

T_a+δT_a＝(a+δa)·(p+δp) (15)；

由此得到：

δp＝a^-(δT_a-δa·p-δa·δp) (16)；

由||δp||≤||a^-||(||δT_a||+||δa||·||p||+||δa||·||δp||)，解出：

由于T_a＝a·p，则||T_a||≤||a||·||p||，因此式(16)写成：

则辨识参数b的相对误差为：

由于是相对扰动项，辨识参数矩阵p的误差主要由||a-||·||a||引起，在此引入条件数为激励轨迹的优化目标函数：

Cond(a)＝||a-||·||a|| (20)；

因此得到：

则优化目标函数为：

minf(a)＝‖a^-‖·‖a‖ (22)；

在满足机器人各关节位置、速度、加速度条件下，当优化目标函数minf(a)值最小时即表示外界扰动对辨识参数矩阵p的扰动最小，对应的参数a_i,l、b_i,l即激励轨迹对应参数；先假定工件质心在机器人末端坐标系中的位置矢量为一已知量p，并将其代入式(9)，从而激励出具有代表性的机器人位姿。

S30、在采集机器人末端姿态基于激励轨迹改变时，采集12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，最后基于重力和惯性力的综合表达式通过最小二乘法得到机器人末端负载的各项参数。

通过重力和惯性力的综合表达式，可以得到如下公式：

F＝RL (23)；

其中：

通过最小二乘法可以得到：

L＝(R^TR)^-1·R^TF (24)；

H＝(R^TR)^-1·R^TT (25)；

方程(25)中：

/>

采集至少12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，带入公式(24)和公式(25)，求出负载重力G，基座倾角U和V，传感器零点值 和/>负载质心x、y和z共12个参数的数值。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (1)

1.一种基于激励轨迹的机器人末端负载快速辨识方法，其特征在于，包括如下步骤：

S10、建立机器人和机器人末端负载的空间坐标系，并根据建立的坐标系关系，分别推导负载辨识计算公式和惯性力影响公式，从而获得末端负载重力和惯性力的综合表达式；

获得末端负载重力和惯性力的综合表达式的具体方法为：

S1001、首先建立参考坐标系，设定世界坐标系为W，机器人基坐标系为B，机器人末端坐标系为E，六维力传感器坐标系为FT；其中六维力传感器坐标系的原点为O，坐标系的方向和传感器厂家给定的坐标系方向一致；

然后确定坐标系之间的变换矩阵，通过所建立的坐标系，得到六维力传感器坐标系相对于机器人末端坐标系的齐次变换矩阵为对于任意给定的末端位置/>末端负载的重力在六维力传感器坐标系中的各个轴上均有分量；

S1002、假设六维力传感器的三个力的初始值为和/>三个力矩的初始值为和/>负载重力为G以及负载质心在六维力传感器坐标系中的位置为x，y和z，则负载重力在六维力传感器坐标系的作用力分量为F_gx、F_gy和F_gz，作用力矩分量为T_gx、T_gy和T_gz，六维力传感器实测的作用力反馈值为F_x、F_y和F_z，作用力矩反馈值为T_x、T_y和T_z；

S1003、由力与力矩的关系以及右手定则可得公式(1)：

其中：

通过以上公式，并对多组数据采样，最终通过最小二乘法可以得到负载质心坐标：

p＝(F^TF)^-1·F^Tτ (3)；

其中：p＝[x y z k₁ k₂ k₃]^T

在机器人基坐标系的z轴和重力的方向不一致，存在安装倾角时，假设机器人基座与世界坐标系的x轴和y轴方向的偏角即安装倾角分别为U和V，且重力在世界坐标系中的方向向量为g_W＝[0 0 -1]^T，得到：

S1004、机器人在快速运动中，在负载的作用下，机器人末端产生惯性力F_inert和惯性力矩T_inert，则传感器的实测力F_meas与实测力矩T_meas由期望跟踪力与力矩F_des与期望力矩T_des以及惯性力与惯性力矩组成，即：

S1005、通过公式(4)和公式(5)得到重力和惯性力的综合表达式：

S20、基于优化目标函数及各限制条件，设计机器人末端各关节激励轨迹，以此改变机器人末端姿态；

设计机器人末端各关节激励轨迹的具体方法为：

S2001、找到与关节参数相关的方程组，将其作为激励方程，即考虑惯性力下的力矩方程组：

由于负载的加速度为负载绕机器人末端坐标系旋转产生的加速度与机器人末端坐标系本身加速度的矢量和，则有：

其中α是机器人末端坐标系的角加速度，是机器人末端坐标系的加速度；得到：

T_a＝a·p (9)；

其中：

a是关于θ₄、θ₅、θ₆的函数，记为a＝f(q₄，q₅，q₆)；

S2002、各个关节的激励轨迹用有限项的傅里叶级数表示，则机器人第i关节的角位移q_i、角速度以及角加速度/>表示为：

式中的N表示正弦和余弦的项数，w_f＝2πf_f是基波角频率，a_i，l和b_i，l是待确定的参数；

为使机器人末端尽可能的充满整个空间，设优化的目标函数为minf(a)；由于重力补偿只涉及机械臂末端的末三轴，因此共有6N个参数待优化；为满足机器人关节限制条件，给出如下关节约束表达式：

启停约束：

S2003、考虑辨识点姿态不具有代表性及传感器数据浮动因素，T_a、a和p均受到扰动，则扰动后的方程组为：

T_a+δT_a＝(a+δa)·(p+δp) (15)；

由此得到：

δp＝a^-(δT_a-δa·p-δa·δp) (16)；

由||δp||≤||a^-||(||δT_a||+||δa||·||p||+||δa||·||δp||)，解出：

由于T_a＝a·p，则||T_a||≤||a||·||p||，因此式(16)写成：

则辨识参数负载质心坐标p的相对误差为：

由于是相对扰动项，辨识参数负载质心坐标p的误差主要由||a^-||·||a||引起，在此引入条件数为激励轨迹的优化目标函数：

Cond(a)＝||a^-||·||a|| (20)；

因此得到：

则优化目标函数为：

minf(a)＝||a^-||·||a|| (22)；

在满足机器人各关节位置、速度、加速度条件下，当优化目标函数minf(a)值最小时即表示外界扰动对辨识参数负载质心坐标p的扰动最小，对应的参数a_i，l、b_i，l即激励轨迹对应参数；先假定负载质心在机器人末端坐标系中的位置矢量为一已知量p，并将其代入式(9)，从而激励出具有代表性的机器人位姿；

S30、在基于激励轨迹采集机器人的末端姿态时，采集至少12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，最后基于重力和惯性力的综合表达式通过最小二乘法得到机器人末端负载的各项参数；

获得机器人末端负载的各项参数的具体方法为：

通过重力和惯性力的综合表达式，可以得到如下公式：

F＝RL (23)；

其中：

通过最小二乘法可以得到：

L＝(R^TR)^-1·R^TF (24)；

H＝(R^TR)^-1·R^TT (25)；

公式(25)中：

采集至少12组机器人在不同姿态下安装在机器人末端的六维力传感器的数据，带入公式(24)和公式(25)，求出负载重力G，安装倾角U和V，六维力传感器坐标系中力与力矩的初始值和/>负载质心在六维力传感器坐标系中的位置x、y和z，共12个参数的数值。