CN117021109A - 基于六维力传感器的机械臂末端负载动力学参数辨识方法 - Google Patents

Abstract

本发明公开了一种基于六维力传感器的机械臂末端负载动力学参数辨识方法，涉及机械臂负载辨识技术领域，包括：基于牛顿‑欧拉动力学方程构建机械臂末端负载动力学模型，并转化为线性形式并加入六维力传感器误差项；设计机械臂初始激励轨迹，对该轨迹进行优化求解，控制机械臂运行最优激励轨迹，并采集实验数据；对采集到的实验数据进行滤波和中心差分处理，通过机械臂雅克比矩阵获得待辨识方程中所需数据；将获得的待辨识方程中所需数据输入加权和声算法中，求解出负载的动力学参数，实现负载动力学参数的辨识。本发明具有实时性好，受空间约束小和精度高等优点，适用于多种非结构环境下的机械臂负载动力学参数辨识。

Description

基于六维力传感器的机械臂末端负载动力学参数辨识方法

技术领域

本发明涉及机械臂负载辨识相关技术领域，特别涉及一种基于六维力传感器的机械臂末端负载动力学参数辨识方法。

背景技术

装载机这种复杂装备涉及机、电、液等多个领域，存在着多学科信息间的交叉，制造这种复杂装备实体物理样机周期长、成本大，实验研究进行相关实机实验也即为不便，但随着物联网、大数据、云计算、人工智能等新信息技术广泛应用，以及数字孪生技术的提出，能很好解决该问题，其通过数字化建模的方式建立物理世界和数字世界之间精准映射关系、实时反馈机制，可以构建起虚拟世界对物理世界描述、诊断、预测和决策的新体系。将数字孪生技术应用到装载机自主铲掘过程中，从而实现物理模型和数字孪生模型的双向映射，并在可视化的仿真环境中积极测试模拟，在测试中不断提升模型质量。利用数字孪生体开展大量装载机自主化、智能化、无人化工作实验论证，能够有效提高工作效率，极大地减少了对实际样机的数据测试采集难度，缩短了实验周期，有效地节约了实际测试带来的金钱、人工成本。

然而，目前装载机数字孪生模型大多都停留在模型只能够可视化阶段，具体模运行控制逻辑一致性，数据交互等关键并不能精确把握，所构建的模型对于实际物理实体的映射准确度仍然有待提高，且目前尚未有一种能够针对装载机自主铲掘过程的数字孪生模型构建后进行有效测试验证的方法，从而保证所构建的装载机数字孪生模型各方面准确合理，满足实际工程运用需要。

发明内容

针对现有技术存在的效率低、精度差的缺陷，本发明提出一种基于六维力传感器的机械臂末端负载动力学参数辨识方法。

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

一种基于六维力传感器的机械臂末端负载动力学参数辨识方法，包括：

S1，基于牛顿-欧拉动力学公式，建立相对六维力传感器坐标系的负载动力学方程；

S2，将负载动力学方程线性化得到负载动力学参数辨识模型，分离出观测矩阵和待辨识参数集；

S3，在观测矩阵和待辨识参数集中加入传感器测量值误差项，获得加入六维力传感器误差项的线性化负载动力学模型；

S4，结合傅里叶级数和五次多项式设计用于实验数据采集的初始激励轨迹，以观测矩阵条件数最小为优化目标，根据机械臂关节角度、角加速度限值、始末速度和加速度为零和机械臂末端TCP的运动空间约束，优化求解得到最优激励轨迹；

S5，控制机械臂运行最优激励轨迹，采集机械臂的关节角度、角速度值以及六维力传感器测量数据，采用巴特沃斯低通滤波器对采集的数据进行滤波；基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度、线速度以及重力加速度；基于中心差分算法计算机械臂末端负载相对六维力传感器坐标系的角加速度和线加速度；

S6，将角速度、重力加速度、角加速度、线加速度以及六维力传感器测量数据输入到加权和声算法中，计算出负载的动力学参数集，实现负载动力学参数的辨识。

优选的，所述S1中，负载动力学方程表示如下：

其中，^sf＝[^sf_x，^sf_y，^sf_z]^T，为负载对机械臂末端六维力传感器的力矢量；^sτ＝[^sτ_x，^sτ_y，^sτ_z]^T，为负载对机械臂末端六维力传感器的力矩矢量；m为负载质量；^sa、^sg、^sα和^sω分别为负载相对六维力传感器坐标系的线加速度、重力加速度、角加速度和角速度；^sc＝[^sc_x，^sc_y，^sc_z]^T，为负载质心在六维力传感器坐标系中的位置坐标；^sI为负载相对六维力传感器坐标系的惯性张量；

^sI表示如下：

其中，I_xx为负载对x轴的惯性矩；I_yy为负载对y轴的惯性矩；I_zz为负载对z轴的惯性矩；I_xy为负载对x轴和y轴的惯性积；I_xz为负载对x轴和z轴的惯性积；I_yz为负载对y轴和z轴的惯性积。

优选的，所述S2中，负载动力学参数辨识模型F′表示如下：

F′＝Y(^sa，^sα，^sω，^sg)^sP

其中，Y(^sa，^sα，^sω，^sg)为观测矩阵；待辨识参数集为^sP＝[m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_xy，I_wz，I_yz]^T。

优选的，所述S3中，加入六维力传感器误差项的线性化负载动力学模型F表示如下：

F＝Y_ext(^sa，^sα，^sω，^sg)·^sP_ext

其中，F＝[^sf，^sτ]^T为六维力传感器的测量值；Y_ext(^sa，^sα，^sω，^sg)为观测矩阵；待辨识参数集为^sP_ext＝[f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，^sP]^T＝

f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_wy，I_xz，I_yz]^T；

其中，f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz分别表示六维力传感器的六个分量上的初始误差。

优选的，所述S4，具体包括：

S41，设计结合傅里叶级数与五次多项式的初始激励轨迹，表示如下：

其中，q_i(t)为机械臂关节i在时刻t时的角度；N为傅里叶级数轨迹谐波次数；k为谐波的编号，ω_f为基频；a_i,k和b_i,k分别为傅里叶级数的正弦和余弦分量系数；c_i,m为五次多项式第m项的系数；t为轨迹运行时间；T＝2π/ω_f；表示向下取整；其中a_i,k，b_i,k和c_i,k为待优化参数，i＝4,5,6；

S42，以观测矩阵条件数最小为优化目标求解激励轨迹，设置约束条件如下：

其中，表示负载动力学辨识方程的数值型观测矩阵，/>为观测矩阵条件数；n为激励轨迹上选择的采样点数量；q_i,min为关节i角度最小限值；q_i,max为关节i角度最大限值；/>表示预设的最大关节角速度；/>表示预设的最大关节角加速度；t₀和t_f分别为激励轨迹的初始时刻和终止时刻；w(q(t))表示t时刻机械臂的笛卡尔空间位置；W₀表示受工作环境约束的机械臂工作空间；

S43，给定激励轨迹的谐波次数N＝5和基频ω_f＝0.2π，采用遗传算法求解激励轨迹参数a_i,k，b_i,k和c_i,k，作为最优激励轨迹的参数。

优选的，所述S5，具体包括：

S51，接收输入的最优激励轨迹，控制机械臂按照最优激励轨迹做周期性运动；

S52，实时采集机械臂的各个关节角度、角速度和六维力传感器测量数据；

S53，基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度^sω和线速度s_v以及重力加速度^sg，计算公式为：

其中，J＝[J₁ J₂ J₃ J₄ J₅ J₆]为机械臂的速度雅克比矩阵；q为机械臂关节角度；为机械臂关节角速度；R_s表示D-H法建立的机械臂六维力传感器坐标系s相对于基坐标系0的变换矩阵A_s的前3列的前3行；g＝[009.81]^T；

具体的：

其中，z_i-1＝[001]^T、p_n和p_i-1分别表示D-H法建立的机械臂末端坐标系n和关节坐标系i-1相对于基坐标系0的变换矩阵A_n和A_i-1的第4列的前3行；^i-1A_i表示从关节坐标系i-1到坐标系i的齐次变换矩阵；q_i表示第i个关节的角度；θ_i表示关节坐标系Z_i-1轴到Z_i轴的转角；l_i表示从Z_i-1轴到Z_i轴的距离；d_i表示从X_i-1轴到X_i轴的距离；

S54，采用中心差分算法计算末端加速度数据，计算公式如下：

其中，^sa_i表示激励轨迹第i个采样点时刻的机械臂末端负载的线加速度矩阵；^sα_i表示激励轨迹第i个采样点时刻的机械臂末端负载的角加速度矩阵；^sv_i+1表示激励轨迹第i+1个采样点时刻的机械臂末端负载线速度矩阵；^sω_i+1表示激励轨迹第i+1个采样点时刻的机械臂末端负载角速度矩阵；^sv_i-1表示激励轨迹第i-1个采样点时刻的机械臂末端负载线速度矩阵；^sω_i-1表示激励轨迹第i-1个采样点时刻的机械臂末端角速度矩阵；Δt表示激励轨迹采样点的时间间隔。

优选的，所述S6，具体包括：

S61，设置辨识模型误差绝对值函数为加权和声算法的优化目标函数，如下：

其中，其中，error表示动力学模型估计六维力的误差矩阵；norm()表示矩阵范数；W＝1/|std(error)|，表示权重向量，std()表示计算标准差；表示由激励轨迹中采集并计算得到的n组^sa，^sα，^sω，^sg数据输入Y_ext(^sa，^sα，^sω，^sg)中得到的数值观测矩阵，^sP_ext表示负载的动力学参数集，即和声变量；/>表示激励轨迹中采集的n组六维力传感器测量数据；

S62，根据优化目标函数，设置加权和声算法的计算参数，根据负载类型设置和声变量约束区间，随机生成若干组初始和声变量，组成和声记忆库；

S63，复制和声记忆库中的第一组和声变量，生成随机数r，比较随机数与设定阈值的大小，根据规则生成新的和声变量；规则表述如下：

当r＜hmcr时，选择一个随机的和声并将其对应变量的值复制到新和声中；

当r＜par_min时，使用另一个随机的和声来更新该变量的值；

如果r＜par_max，则使用新和声和当前和声之间的差异来更新该变量的值。

其中，hmcr表示和声记忆库响应率；par_mmin表示最小参数值；par_max表示最大参数值；

S64，按照优化目标函数计算和声记忆库中各组和声变量的评价值，根据评价值从小到大的顺序对和声记忆库中的和声变量进行排序；

S65，用新生成的和声变量替换和声记忆库中排序最后的旧和声变量，即评价值最大的和声变量；

S66，判断加权和声算法是否达到最大迭代次数，若未达到，返回S63；若达到，执行S67；

S67，返回和声记忆库中排序第一的和声变量作为加权和声算法的最终解，即负载的动力学参数集^sP_ext。

优选的，所述S6之后，还包括：

采用另一条不同的激励轨迹实验数据，对负载动力学参数的辨识精度进行验证，根据负载动力学模型计算末端所受六维力，与六维力传感器采集到的真实值比较，采用六维力的均方根评价负载动力学模型的准确性；

准确性的评价标准如下：

其中，△F_RMS为六维力预测均方根误差；N为验证激励轨迹的采样点数；F_i是第i个采样点的真实六维力；Y_ext(^sa_i，^sα_i，^sω_i，^sg_i)是第i个采样点的观测矩阵；^sP_ext表示负载的动力学参数集。

与现有技术相比，本发明的有益效果如下：

(1)本发明不需要多次重复运动，只需运行一次求解到的最优激励轨迹即可辨识得到负载的动力学参数，辨识效率明显提高；

(2)本发明辨识负载动力学参数过程中不涉及机械臂本体参数，没有累计误差，辨识精度更高；

(3)本发明采用加权和声算法辨识负载的动力学参数，相比传统的最小二乘法能够达到更高的辨识精度；

(4)本发明提出的辨识方法通用性好，针对配置各类末端六维力传感器的机械臂均适用。

附图说明

图1为本发明实施例的基于六维力传感器的机械臂末端负载动力学参数辨识方法的流程图；

图2是本发明实施例的机械臂最优激励轨迹对应的关节运动角度变化示意图；

图3是本发明实施例的基于D-H法建立的机械臂连杆坐标系示意图；

图4是本发明实施例的基于和声加权算法的负载动力学参数求解过程示意图。

具体实施方式

下面结合具体实施例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

参见图1所示，本实施例一种基于六维力传感器的机械臂末端负载动力学参数辨识方法，包括：

S1，基于牛顿-欧拉动力学公式，建立相对六维力传感器坐标系的负载动力学方程；

S2，将负载动力学方程线性化得到负载动力学参数辨识模型，分离出观测矩阵和待辨识参数集；

S3，在观测矩阵和待辨识参数集中加入传感器测量值误差项，获得加入六维力传感器误差项的线性化负载动力学模型；

S4，结合傅里叶级数和五次多项式设计用于实验数据采集的初始激励轨迹，以观测矩阵条件数最小为优化目标，根据机械臂关节角度、角加速度限值、始末速度和加速度为零和机械臂末端TCP的运动空间约束，优化求解得到最优激励轨迹；

S5，控制机械臂运行最优激励轨迹，采集机械臂的关节角度、角速度值以及六维力传感器测量数据，采用巴特沃斯低通滤波器对采集的数据进行滤波；基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度、线速度以及重力加速度；基于中心差分算法计算机械臂末端负载相对六维力传感器坐标系的角加速度和线加速度；

S6，将角速度、重力加速度、角加速度、线加速度以及六维力传感器测量数据输入到加权和声算法中，计算出负载的动力学参数集，实现负载动力学参数的辨识。

S7，采用另一条不同的激励轨迹实验数据，对负载动力学参数的辨识精度进行验证，根据负载动力学模型计算末端所受六维力，与六维力传感器采集到的真实值比较，采用六维力的均方根评价负载动力学模型的准确性。

本实施例中，所述S1中，负载动力学方程表示如下：

其中，^sf＝[^sf_x，^sf_y，^sf_z]^T，为负载对机械臂末端六维力传感器的力矢量；^sτ＝[^sτ_x，^sτ_y，^sτ_z]^T，为负载对机械臂末端六维力传感器的力矩矢量；m为负载质量；^sa、^sg、^sα和^sω分别为负载相对六维力传感器坐标系的线加速度、重力加速度、角加速度和角速度；^sc＝[^sc_x，^sc_y，^sc_z]^T，为负载质心在六维力传感器坐标系中的位置坐标；^sI为负载相对六维力传感器坐标系的惯性张量；

^sI表示如下：

其中，I_xx为负载对x轴的惯性矩；I_yy为负载对y轴的惯性矩；I_zz为负载对z轴的惯性矩；I_xy为负载对x轴和y轴的惯性积；I_xz为负载对x轴和z轴的惯性积；I_yz为负载对y轴和z轴的惯性积。

本实施例中，所述S2中，负载动力学参数辨识模型F′表示如下：

F′＝Y(^sa，^sα，^sω，^sg)^sP

其中，Y(^sa，^sα，^sω，^sg)为符号形式观测矩阵；待辨识参数集为^sP＝[m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_xy，I_xz，I_yz]^T。

本实施例中，所述S3中，加入六维力传感器误差项的线性化负载动力学模型F表示如下：

F＝Y_ext(^sa，^sα，^sω，^sg)·^sP_ext

其中，F＝[^sf，^sτ]^T为六维力传感器的测量值；Y_ext(^sa，^sα，^sω，^sg)为观测矩阵；待辨识参数集为^sP_ext＝[f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，^sP]^T＝

f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_xy，I_xz，I_yz]^T。

其中，f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz分别表示六维力传感器的六个分量上的初始误差。

本实施例中，所述S4包括如下步骤。

S41，设计结合傅里叶级数与五次多项式的初始激励估计表达式为：

其中，q_i(t)为机械臂关节i在时刻t时的角度，N为傅里叶级数轨迹谐波次数，k为谐波的编号，ω_f为基频，a_i,k和b_i,k分别为傅里叶级数的正弦和余弦分量系数，c_i,m为五次多项式第m项的系数，t为轨迹运行时间，T＝2π/ω_f，表示向下取整。其中a_i,k，b_i,k和c_i,k为待优化参数，i＝4,5,6。

S42，以观测矩阵条件数最小为优化目标求解激励轨迹，设置约束条件如下：

其中，表示负载动力学辨识方程的数值型观测矩阵，/>为观测矩阵条件数；n为激励轨迹上选择的采样点数量；q_i,min为关节i角度最小限值；q_i,max为关节i角度最大限值；/>表示预设的最大关节角速度；/>表示预设的最大关节角加速度；t₀和t_f分别为激励轨迹的初始时刻和终止时刻；w(q(t))表示t时刻机械臂的笛卡尔空间位置；W₀表示受工作环境约束的机械臂工作空间。

S43，给定激励轨迹的谐波次数N＝5和基频ω_f＝0.2π，采用遗传算法求解激励轨迹参数a_i,k，b_i,k和c_i,k。参见图2所示，即为基于求解出的激励轨迹参数的最优激励轨迹。

本实施例中，所述S5包括如下步骤。

S51，基于机械臂操作系统ROS建立上位机与机械臂之间的通讯，通过python调用ROS库的控制指令，输入激励轨迹，使机械臂按照激励轨迹做周期性运动。

S52，通过python编写脚本，调用ROS的/joint_states话题并建立与六维力传感器Onrobot HEX_E的IP通讯，在机械臂运行激励轨迹时，实时采集机械臂的各个关节角度、角速度和六维力传感器数据。

S53，基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度^sω和线速度^sv以及重力加速度^sg，计算公式为：

其中，J＝[J₁ J₂ J₃ J₄ J₅ J₆]为机械臂的速度雅克比矩阵，q为机械臂关节角度，为机械臂关节角速度；R_s表示D-H法建立的机械臂六维力传感器坐标系s相对于基坐标系0的变换矩阵A_s的前3列的前3行，g＝[0 0 9.81]^T。

具体的：

其中，z_i-1＝[0 0 1]^T，p_n和p_i-1分别表示D-H法建立的机械臂末端坐标系n和关节坐标系i-1相对于基坐标系0的变换矩阵A_n和A_i-1的第4列的前3行；^i-1A_i表示从关节坐标系i-1到坐标系i的齐次变换矩阵，q_i表示第i个关节的角度，θ_i表示关节坐标系Z_i-1轴到Z_i轴的转角，l_i表示从Z_i-1轴到Z_i轴的距离，d_i表示从X_i-1轴到X_i轴的距离，具体参见图3所示。

S54，采用中心差分算法计算末端加速度数据，计算公式如下：

其中，^sa_i，^sα_i分别表示激励轨迹第i个采样点时刻的机械臂末端负载的线加速度矩阵与角加速度矩阵，^sv_i+1，^sω_i+1为激励轨迹第i+1个采样点时刻的机械臂末端负载线速度矩阵与角速度矩阵，^sv_i-1，sω_i-1为激励轨迹第i-1个采样点时刻的机械臂末端负载线速度矩阵与角速度矩阵；Δt是激励轨迹采样点的时间间隔。

参见图4所示，本实施例中，所述S6包括如下步骤。

S61，设置辨识模型误差绝对值函数为加权和声算法的优化目标函数，如下：

其中，error表示动力学模型估计六维力的误差矩阵；norm()表示矩阵范数；W＝1/|std(error)|，表示权重向量，std()表示计算标准差；表示由激励轨迹中采集并计算得到的n组^sa，^sα，^sω，^sg数据输入Y_ext(^sa，^sα，^sω，^sg)中得到的数值观测矩阵，^sP_ext表示负载的动力学参数集，即和声变量；/>表示激励轨迹中采集的n组六维力传感器测量数据。

S62，根据优化目标函数，设置加权和声算法的计算参数，根据负载类型设置和声变量约束区间，随机生成若干组初始和声变量，组成和声记忆库。

S63，复制和声记忆库中的第一组和声变量，生成随机数r，比较随机数与设定阈值的大小，根据规则生成新的和声变量。具体规则表述如下：

(1)当r＜hmcr时，选择一个随机的和声并将其对应变量的值复制到新和声中；

(2)当r＜par_min时，使用另一个随机的和声来更新该变量的值；

(3)如果r＜par_max，则使用新和声和当前和声之间的差异来更新该变量的值。

其中，hmcr表示和声记忆库响应率，par_mmin表示最小参数值，par_max表示最大参数值。

S64，按照所述加权和声算法的目标函数计算和声记忆库中各组和声变量的评价值，根据评价值从小到大的顺序对和声记忆库中的和声变量进行排序。

S65，用新生成的和声变量替换和声记忆库中排序最后的旧和声变量，即评价值最大的和声变量。

S66，判断加权和声算法是否达到最大迭代次数，若未达到，返回S63；若达到，执行S67。

S67，返回和声记忆库中排序第一的和声变量作为加权和声算法的最终解，也就是S5中所述负载的动力学参数^sP_ext。

本实施例中，S7选择一条不同的激励轨迹验证所得负载动力学参数的准确性，准确性的评价标准为：

其中，△F_RMS为六维力预测均方根误差；N为验证激励轨迹的采样点数；F_i是第i个采样点的真实六维力；Y_ext(^sa_i，^sα_i，^sω_i，^sg_i)是第i个采样点的观测矩阵；^sP_ext表示负载的动力学参数集。

上述仅为本发明的具体实施方式，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。

Claims (8)

1.一种基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，包括：

S1，基于牛顿-欧拉动力学公式，建立相对六维力传感器坐标系的负载动力学方程；

S2，将负载动力学方程线性化得到负载动力学参数辨识模型，分离出观测矩阵和待辨识参数集；

S3，在观测矩阵和待辨识参数集中加入传感器测量值误差项，获得加入六维力传感器误差项的线性化负载动力学模型；

S4，结合傅里叶级数和五次多项式设计用于实验数据采集的初始激励轨迹，以观测矩阵条件数最小为优化目标，根据机械臂关节角度、角加速度限值、始末速度和加速度为零和机械臂末端TCP的运动空间约束，优化求解得到最优激励轨迹；

S5，控制机械臂运行最优激励轨迹，采集机械臂的关节角度、角速度值以及六维力传感器测量数据，采用巴特沃斯低通滤波器对采集的数据进行滤波；基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度、线速度以及重力加速度；基于中心差分算法计算机械臂末端负载相对六维力传感器坐标系的角加速度和线加速度；

S6，将角速度、重力加速度、角加速度、线加速度以及六维力传感器测量数据输入到加权和声算法中，计算出负载的动力学参数集，实现负载动力学参数的辨识。

2.根据权利要求1所述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S1中，负载动力学方程表示如下：

其中，^sf＝[^sf_x，^sf_y，^sf_z]^T，为负载对机械臂末端六维力传感器的力矢量；^sτ＝[^sτ_x，^sτ_y，^sτ_z]^T，为负载对机械臂末端六维力传感器的力矩矢量；m为负载质量；^sa、^sg、^sα和^sω分别为负载相对六维力传感器坐标系的线加速度、重力加速度、角加速度和角速度；^sc＝[^sc_x，^sc_y，^sc_z]^T，为负载质心在六维力传感器坐标系中的位置坐标；^sI为负载相对六维力传感器坐标系的惯性张量；

^s，表示如下：

其中，I_xx为负载对x轴的惯性矩；I_yy为负载对y轴的惯性矩；I_zz为负载对z轴的惯性矩；I_xy为负载对x轴和y轴的惯性积；I_xz为负载对x轴和z轴的惯性积；I_yz为负载对y轴和z轴的惯性积。

3.根据权利要求2所述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S2中，负载动力学参数辨识模型F′表示如下：

F′＝Y(^sα，^sα，^sω，^sg)^sP

其中，Y(^sa，^sα，^sω，^sg)为观测矩阵；待辨识参数集为^sP＝[m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_xy，I_xz，I_yz]^T。

4.根据权利要求3所述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S3中，加入六维力传感器误差项的线性化负载动力学模型F表示如下：

F＝Y_ext(^sa，^sα，^sω，^sg)·^sP_ext

其中，F＝[^sf，^sτ]^T为六维力传感器的测量值；Y_ext(^sa，^sα，^sω，^sg)为观测矩阵；待辨识参数集为^sP_ext＝[f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，^sP]^T＝

f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz，m，m^sc_x，m^sc_y，m^sc_z，I_xx，I_yy，I_zz，I_xy，I_xz，I_yz]^T；

其中，f_ox，f_oy，f_oz，τ_ox，τ_oy，τ_oz分别表示六维力传感器的六个分量上的初始误差。

5.据权利要求4述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S4，具体包括：

S41，设计结合傅里叶级数与五次多项式的初始激励轨迹，表示如下：

其中，q_i(t)为机械臂关节i在时刻t时的角度；N为傅里叶级数轨迹谐波次数；k为谐波的编号，ω_f为基频；a_i，k和b_i，k分别为傅里叶级数的正弦和余弦分量系数；c_i，m为五次多项式第m项的系数；t为轨迹运行时间；T＝2π/ω_f；表示向下取整；其中a_i，k，b_i，k和c_i，k为待优化参数，i＝4，5，6；

S42，以观测矩阵条件数最小为优化目标求解激励轨迹，设置约束条件如下：

其中，表示负载动力学辨识方程的数值型观测矩阵，为观测矩阵条件数；n为激励轨迹上选择的采样点数量；q_i,min为关节i角度最小限值；q_i,max为关节i角度最大限值；/>表示预设的最大关节角速度；/>表示预设的最大关节角加速度；t₀和t_f分别为激励轨迹的初始时刻和终止时刻；w(q(t))表示t时刻机械臂的笛卡尔空间位置；W₀表示受工作环境约束的机械臂工作空间；

S43，给定激励轨迹的谐波次数N＝5和基频ω_f＝0.2π，采用遗传算法求解激励轨迹参数a_i,k，b_i,k和c_i,k，作为最优激励轨迹的参数。

6.据权利要求5述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S5，具体包括：

S5 1，接收输入的最优激励轨迹，控制机械臂按照最优激励轨迹做周期性运动；

S52，实时采集机械臂的各个关节角度、角速度和六维力传感器测量数据；

S53，基于速度雅克比矩阵计算机械臂末端负载相对六维力传感器坐标系的角速度^sω和线速度^sv以及重力加速度^sg，计算公式为：

其中，J＝[J₁ J₂ J₃ J₄ J₅ J₆]为机械臂的速度雅克比矩阵；q为机械臂关节角度；为机械臂关节角速度；R_s表示D-H法建立的机械臂六维力传感器坐标系s相对于基坐标系0的变换矩阵A_s的前3列的前3行；g＝[0 0 9.81]^T；

具体的：

其中，z_i-1＝[0 0 1]^T、p_n和p_i-1分别表示D-H法建立的机械臂末端坐标系n和关节坐标系i-1相对于基坐标系0的变换矩阵A_n和A_i-1的第4列的前3行；^i-1A_i表示从关节坐标系i-1到坐标系i的齐次变换矩阵；q_i表示第i个关节的角度；θ_i表示关节坐标系Z_i-1轴到Z_i轴的转角；l_i表示从Z_i-1轴到Z_i轴的距离；d_i表示从X_i-1轴到X_i轴的距离；

S54，采用中心差分算法计算末端加速度数据，计算公式如下：

其中，^sa_i表示激励轨迹第i个采样点时刻的机械臂末端负载的线加速度矩阵；^sα_i表示激励轨迹第i个采样点时刻的机械臂末端负载的角加速度矩阵；^sv_i+1表示激励轨迹第i+1个采样点时刻的机械臂末端负载线速度矩阵；^sω_i+1表示激励轨迹第i+1个采样点时刻的机械臂末端负载角速度矩阵；^sv_i-1表示激励轨迹第i-1个采样点时刻的机械臂末端负载线速度矩阵；^sω_i-1表示激励轨迹第i-1个采样点时刻的机械臂末端角速度矩阵；Δt表示激励轨迹采样点的时间间隔。

7.据权利要求6述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S6，具体包括：

S61，设置辨识模型误差绝对值函数为加权和声算法的优化目标函数，如下：

其中，error表示动力学模型估计六维力的误差矩阵；norm()表示矩阵范数；W＝1/|std(error)|，表示权重向量，std()表示计算标准差；表示由激励轨迹中采集并计算得到的n组^sa，^sα，^sω，^sg数据输入Y_ext(^sa，^sα，^sω，^sg)中得到的数值观测矩阵，^sP_ext表示负载的动力学参数集，即和声变量；/>表示激励轨迹中采集的n组六维力传感器测量数据；

S62，根据优化目标函数，设置加权和声算法的计算参数，根据负载类型设置和声变量约束区间，随机生成若干组初始和声变量，组成和声记忆库；

S63，复制和声记忆库中的第一组和声变量，生成随机数r，比较随机数与设定阈值的大小，根据规则生成新的和声变量；规则表述如下：

当r＜hmcr时，选择一个随机的和声并将其对应变量的值复制到新和声中；

当r＜par_min时，使用另一个随机的和声来更新该变量的值；

如果r＜par_max，则使用新和声和当前和声之间的差异来更新该变量的值。

其中，hmcr表示和声记忆库响应率；par_min表示最小参数值；par_max表示最大参数值；

S64，按照优化目标函数计算和声记忆库中各组和声变量的评价值，根据评价值从小到大的顺序对和声记忆库中的和声变量进行排序；

S65，用新生成的和声变量替换和声记忆库中排序最后的旧和声变量，即评价值最大的和声变量；

S66，判断加权和声算法是否达到最大迭代次数，若未达到，返回S63；若达到，执行S67；

S67，返回和声记忆库中排序第一的和声变量作为加权和声算法的最终解，即负载的动力学参数集^sP_ext。

8.据权利要求1述的基于六维力传感器的机械臂末端负载动力学参数辨识方法，其特征在于，所述S6之后，还包括：

采用另一条不同的激励轨迹实验数据，对负载动力学参数的辨识精度进行验证，根据负载动力学模型计算末端所受六维力，与六维力传感器采集到的真实值比较，采用六维力的均方根评价负载动力学模型的准确性；

准确性的评价标准如下：

其中，△F_RMS为六维力预测均方根误差；N为验证激励轨迹的采样点数；F_i是第i个采样点的真实六维力；Y_ext(^sa_i，^sα_i，^sω_i，^sg_i)是第i个采样点的观测矩阵；^sP_ext表示负载的动力学参数集。