赞
踩
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个坐标系的变换,是有正负号的,这一点坑了我好久,谨记!
2021.8.9号,晚上画了一下单腿的运动空间
1、髋关节固定不动,膝关节和肘关节遍历180度,因为我用的舵机只有180转角
2、三个关节都遍历180度
附上matlab代码
function [x,y,z] = leg_fk(theta1,theta2,theta3)
% 连杆偏置,注意正负,跟建立的坐标系有关
a1=-9.57*0.001;
a2=-59.2*0.001;
a3=-77*0.001; d3=44.6*0.001;
x = a3*cos(theta1)*cos(theta2+theta3)+d3*sin(theta1)+a2*cos(theta1)*cos(theta2)+a1*cos(theta1);
y = a3*sin(theta1)*cos(theta2+theta3)-d3*cos(theta1)+a2*sin(theta1)*cos(theta2)+a1*sin(theta1);
z = a3*sin(theta2+theta3)+a2*sin(theta2);
end
% 绘制运动空间 (0-180) space_x=[]; space_y=[]; space_z=[]; for angle_1=0:10/180*pi:180/180*pi %这里是弧度 for angle_2=0:10/180*pi:180/180*pi for angle_3=0:10/180*pi:180/180*pi [x,y,z]=leg_fk(angle_1,angle_2,angle_3); space_x=[space_x,x]; space_y=[space_y,y]; space_z=[space_z,z]; end end end hold on scatter3(space_x,space_y,space_z,'filled');
后面有空了再补充吧
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。