《机器人技术与应用》D-H参数标定实验程序

% 《机器人技术与应用》D-H参数标定实验程序

clear;

close all;

t1 = tic;

% 读取数据

EEvalue = importdata('./MeasureData/EndValue.txt');

EEbasematrix = importdata('./MeasureData/BaseValue.txt');

EEthetaValue = importdata('./MeasureData/AngleValue.txt');

% 去除含有NaN的行

EEvalue.data(any(isnan(EEvalue.data),2),:) = [];

EEbasematrix.data(any(isnan(EEbasematrix.data),2),:) = [];

% theta化为弧度

EEthetaValue = EEthetaValue * (pi / 180);

% 进行平面，圆，点的分裂处理

DataSize = size(EEthetaValue,1);

planeEE = zeros(DataSize,6);

circleEE = zeros(DataSize,6);

pointEE = zeros(DataSize,6);

for j = 1:DataSize

circleEE(j,:) = EEvalue.data(3*j-2,1:6);

planeEE(j,:) = EEvalue.data(3*j-1,1:6);

pointEE(j,:) = EEvalue.data(3*j,1:6);

end

% 测量坐标系下，机器人基坐标系Tm_b

pm_b = EEbasematrix.data(2,1:3)';

zm_b = EEbasematrix.data(1,4:6)';

ym_b = EEbasematrix.data(3,4:6)'; %因为不知道Y轴是怎么测的。所以需要尝试。xm_b = cross(ym_b,zm_b);

Tm_b = [xm_b, ym_b, zm_b, pm_b;

0, 0, 0, 1];

DH_nominal = [0, 338, -pi/2, 0;

0, 0, pi/2, 0;

0, 420, -pi/2, 0;

0, 0, pi/2, 0;

0, 380, -pi/2, 0;

0, 0, pi/2, 0;

0, 160, 0, 0];

dDH = zeros(7,4);

e = zeros(DataSize*6,1);

J = zeros(DataSize*6,28); % 系数矩阵

EP = 1e-10; % 迭代误差阈值

N = 0; % 迭代次数

% D-H参数矩阵通式

T_i = @(a_i,b_i,alpha_i,theta_i)([cos(theta_i),

-cos(alpha_i)*sin(theta_i), sin(alpha_i)*sin(theta_i),

a_i*cos(theta_i);

sin(theta_i), cos(alpha_i)*cos(theta_i),

-sin(alpha_i)*cos(theta_i), a_i*sin(theta_i);

0, sin(alpha_i),

cos(alpha_i), b_i;

0, 0, 0, 1;]);

% 关联矩阵通式

G_i = @(a_i,alpha_i)([0, 1, 0, 0;

0, 0, a_i*cos(alpha_i), sin(alpha_i);

0, 0, -a_i*sin(alpha_i), cos(alpha_i);

1, 0, 0, 0;

0, 0, sin(alpha_i), 0;

0, 0, cos(alpha_i), 0]);

% Ad矩阵

Ad = @(R,P)([R', -R'*P;

zeros(3,3), R']);

% 实时显示收敛情况

semilogy(0,1);

xlabel('Iteration N');

ylabel('DH error');

grid on;

hold on;

count=0;

% 进行迭代

while(1)

N = N + 1;

for j = 1:DataSize

DH = DH_nominal + dDH;

DH(:,4) = DH(:,4) + EEthetaValue(j,:)';

% 先求系数矩阵J

T01 = T_i(DH(1,1),DH(1,2),DH(1,3),DH(1,4));

T12 = T_i(DH(2,1),DH(2,2),DH(2,3),DH(2,4));

T23 = T_i(DH(3,1),DH(3,2),DH(3,3),DH(3,4));

T34 = T_i(DH(4,1),DH(4,2),DH(4,3),DH(4,4));

T45 = T_i(DH(5,1),DH(5,2),DH(5,3),DH(5,4));

T56 = T_i(DH(6,1),DH(6,2),DH(6,3),DH(6,4));

T67 = T_i(DH(7,1),DH(7,2),DH(7,3),DH(7,4));

T07 = T01 * T12 * T23 * T34 * T45 * T56 * T67; T17 = T12 * T23 * T34 * T45 * T56 * T67;

T27 = T23 * T34 * T45 * T56 * T67;

T37 = T34 * T45 * T56 * T67;

T47 = T45 * T56 * T67;

T57 = T56 * T67;

R17 = T17(1:3,1:3);

p17 = T17(1:3,4);

P17 = [0, -p17(3), p17(2);

p17(3), 0, -p17(1);

-p17(2), p17(1), 0];

Ad1 = Ad(R17,P17);

R27 = T27(1:3,1:3);

p27 = T27(1:3,4);

P27 = [0, -p27(3), p27(2);

p27(3), 0, -p27(1);

-p27(2), p27(1), 0];

Ad2 = Ad(R27,P27);

R37 = T37(1:3,1:3);

p37 = T37(1:3,4);

P37 = [0, -p37(3), p37(2);

p37(3), 0, -p37(1);

-p37(2), p37(1), 0];

Ad3 = Ad(R37,P37);

R47 = T47(1:3,1:3);

p47 = T47(1:3,4);

P47 = [0, -p47(3), p47(2);

p47(3), 0, -p47(1);