% 《机器人技术与应用》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);
评论1