实物模型

DH建立坐标系以及正逆运动学推导

Matlab验证

clear;
clc;
a1=-9.57*0.001;alpha1=pi/2;
a2=-59.2*0.001;
a3=-77*0.001;d3=44.6*0.001;
% 建立连杆系
%  theta    关节角度
%  d        连杆偏移量
%  a        连杆长度
%  alpha    连杆扭角
%  sigma    旋转关节为0，移动关节为1
%  mdh      标准的D&H为0，否则为1
%  offset   关节变量偏移量
%  qlim     关节变量范围[min max]
L1=Link([0 0 a1 alpha1 0]);
L2=Link([0 0 a2 0 0]);
L3=Link([0 d3 a3 0 0 0]);

% 机器人模型对象建立
Leg=SerialLink([L1 L2 L3],'name','single_leg');

xlim([-40,40])
ylim([-40,40])
zlim([0,60])
[theta1,theta2,theta3]=leg_ik(-0.023577,-0.0446,0.1); %单位米
% Leg.display;
theta=[theta1 theta2 theta3];
Leg.plot(theta);
% Leg.teach();

function [theta1,theta2,theta3] = leg_ik(end_x,end_y,end_z)
% 连杆偏置，注意正负，跟建立的坐标系有关
a1=-9.57*0.001;
a2=-59.2*0.001;
a3=-77*0.001;  d3=44.6*0.001;

L1 = sqrt(end_x^2+end_y^2);
Phi_1 = atan2(end_y,end_x);
% fprintf('Phi1=%f\n',Phi_1/pi*180);
% fprintf('atan2(d3/L1,sqrt(1-(d3/L1)^2)) = %f\n',atan2(d3/L1,sqrt(1-(d3/L1)^2))/pi*180);
theta1_1 = atan2(d3/L1,sqrt(1-(d3/L1)^2))+Phi_1 ;
theta1_2 = atan2(d3/L1,-sqrt(1-(d3/L1)^2))+Phi_1 ;
% fprintf('theat1_1=%f\t,theat1_2=%f\n',theta1_1/pi*180,theta1_2/pi*180);
theta1=theta1_2;
fprintf('theat1=%f\n',theta1/pi*180);

m=end_x*cos(theta1)+end_y*sin(theta1)-a1;
n=end_z;
% fprintf('m=%f,n=%f\n',m,n);
k=0.5*(m*m+n*n+a2*a2-a3*a3);
% fprintf('k=%f\n',k);
is_big =((m*a2)^2+(n*a2)^2) - k^2;
% fprintf('is_big =%f\n',is_big);
L2 = sqrt((n*a2)^2+(m*a2)^2);
% fprintf('L2=%f\n',L2);
Phi_2 = atan2(m*a2,n*a2);
% fprintf('Phi_2=%f\n',Phi_2/pi*180);
theta2_1 = atan2(k/L2,sqrt(1-(k/L2)^2))-Phi_2 ;
theta2_2 = atan2(k/L2,-sqrt(1-(k/L2)^2))-Phi_2;
% fprintf('theat2_1=%f,theat2_2=%f\n',theta2_1/pi*180,theta2_2/pi*180);
theta2 = theta2_1;
fprintf('theat2=%f\n',theta2/pi*180);

theta3= asin((end_z-a2*sin(theta2))/a3 ) - theta2;
fprintf('theat3=%f\n',theta3/pi*180);

% 运动学正解验算
x_test = a3*cos(theta1)*cos(theta2+theta3)+d3*sin(theta1)+a2*cos(theta1)*cos(theta2)+a1*cos(theta1);
y_test = a3*sin(theta1)*cos(theta2+theta3)-d3*cos(theta1)+a2*sin(theta1)*cos(theta2)+a1*sin(theta1);
z_test = a3*sin(theta2+theta3)+a2*sin(theta2);
fprintf('x_test=%f\t,y_test=%f\t,z_test=%f\n',x_test,y_test,z_test);
end

结果

通过正解验算逆解结果，发现跟输入完全吻合。

注意

这里有一个小坑，就是偏置a，开始的时候我以为是距离量，默认是正值，结果就一直逆解不正确，最后发现a（d也是一样）跟坐标系有关，要看第i-1坐标系到第i个坐标系的变换，是有正负号的，这一点坑了我好久，谨记！

标签：四足,theta2,单腿,theta1,end,DH,a2,fprintf,d3
来源： https://blog.csdn.net/blue_skyLZW/article/details/119304321