matlab实现六维力传感器参数辨识

通过采集数据，通过最小二乘算法，辨识出重力，漂移力，以及相对于末端的旋转角度。详情请看博客。

%author:            Wang junhe
%time:               20201013 10:55（completed）
%describe:         estimate parameter of force transducer
%algorithm source: 《基于力控制的工业机器人精密装配研究》
clear;
clc;
close;

%define parameter
number =11;
one = ones（number1）;
R3 = zeros（number3）;
A1 = zeros（number2）;
B1 = zeros（number1）;
A2 = zeros（number*24）;
B2 = zeros（number*21）;
A3 = zeros（number*36）;
B3 = zeros（number*31）;

%robot forward kinematics（verified）
q = load（‘C:\Users\shumiaogeek\Documents\MATLAB\bianshi\data\q.txt‘）;
%       theta    d           a        alpha     offset
SL0=link（[0      0.3113-0.08946  0     0        0]‘standard‘）;
SL1=link（[0       0.089459          0    pi/2        0     ]‘standard‘）; 
SL2=link（[0       0          -0.425     0           0     ]‘standard‘）;
SL3=link（[0      0          -0.39225   0        0     ]‘standard‘）;
SL4=link（[0      0.10915       0        pi/2        0     ]‘standard‘）;
SL5=link（[0      0.09465       0       -pi/2        0     ]‘standard‘）;
SL6=link（[0      0.0823         0        0             0     ]‘standard‘）;
starobot=Seriallink（[SL0 SL1 SL2 SL3 SL4 SL5 SL6]‘name‘‘standard‘）;

%record data
filepath = ‘C:\Users\shumiaogeek\Documents\MATLAB\bianshi\data\‘;
for i = 1:number
n = load（[filepath num2str（i） ‘.txt‘]）;
fx = median（n（:3））/1000000;
fy = median（n（:4））/1000000;
fz = median（n（:5））/1000000;
tx = median（n（:6））/1000000;
ty = median（n（:7））/1000000;
tz = median（n（:8））/1000000;
qq = q（i:）*pi/180;
stat06=starobot.fkine（qq）;
R3（i1） = stat06.n（3 1）;
R3（i2） = stat06.o（3 1）;
R3（i3） = stat06.a（3 1）;
A1 = [-R3（:3） one ];
B1（i） =fz;
B2（i*2-1:） = fx;
B2（i*2:） = fy;
B3（i*3-2:）=tx;
B3（i*3-1:）=ty;
B（i*3:）=tz;
end
%estimate Gfzdrift
P1=A1‘ * A1\ A1‘ * B1;
G = P1（1）;
fzdrift = P1（2）;

%estimate cosasinafxdriftfydrift
for i = 1:number
A2（i*2-1:）=[-G*R3（i1）-G*R3（i2）10];
A2（i*2:） =[-G*R3（i2）G*R3（i1）01];
end
P2 = A2‘ * A2\A2‘ * B2;
cosa = P2（1）;
sina = P2（2）;
fxdrift = P2（3）;
fydrift = P2（4）;

%estimate pxpypztxdrifttydrifttzdrift
for i = 1:number
A3（i*3-2:）=[0-G*R3（i3）-G*sina*R3（i1）+G*cosa*R3（i2）100];
A3（i*3-1:）=[G*R3（i3）0-G*cosa*R3（i1）-G*sina*R3（i2）010];
A3（i*3:）=[G*sina*R3（i1）-G*cosa*R3（i2）G*cosa*R3（i1）+G*sina*R3（i2）0001];
end
P3 = A3‘*A3\A3‘*B3;
px = P3（1）;
py = P3（2）;
pz = P3（3）;
txdrift = P3（4）;
tydrift = P3（5）;
tzdrift = P3（6）;

%display
value = {numberGfxdriftfydriftfzdrifttxdrifttydrifttzdriftcosasinapxpypz};
title = {‘number of simple‘‘G‘‘fxdrift‘‘fydrift‘‘fzdrift‘‘txdrift‘‘tydrift‘‘tzdrift‘‘cosa‘‘sina‘‘px‘‘py‘‘pz‘};
result=[title;value]‘;
xlswrite（‘Parameter Estimation.xls‘result）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件     1789996  2020-10-12 20:07  data\1.txt
     文件     1313296  2020-10-12 20:27  data\10.txt
     文件     1811696  2020-10-12 20:27  data\11.txt
     文件     2008068  2020-10-12 20:06  data\2.txt
     文件     2659604  2020-10-12 20:06  data\3.txt
     文件     1309096  2020-10-12 20:06  data\4.txt
     文件     2801496  2020-10-12 20:05  data\5.txt
     文件     1588289  2020-10-12 20:05  data\6.txt
     文件      802495  2020-10-12 20:05  data\7.txt
     文件     2105696  2020-10-12 20:04  data\8.txt
     文件     1714396  2020-10-12 20:26  data\9.txt
     文件         467  2020-10-12 20:24  data\q.txt
     文件        2773  2020-10-20 02:36  sensorF_T_PE.m
     文件         794  2020-10-20 02:36  verify.m