机械臂运动学求解---MATLAB

正运动学求解过程

Step1：为了用D-H表示法队机器人建模，第一件事就是为每个关节指定一个本地的参考坐标系。因此，对于每个关节都必须都必须指定一个Z轴和X轴。指定原则如下：

指定Z轴，如果关节是旋转的，Z轴位于按右手规则旋转的方向。绕Z轴的旋转角是关节变量;如果关节是滑动的，Z轴为沿直线运动的方向。 沿Z轴的连杆长度d是关节变量。

指定X轴，当两关节不平行或相交时，Z轴通常是斜线，但总有一条距离最短的公垂线，它正交于任意两条斜线。在公垂线方向上定义本地参考坐标系的X轴。如果an,表示Zn-1与Zn之间的公垂线，则Xn的方向将沿an 。

若两关节Z轴平行，就会有无数条公垂线，此时可挑选与前一关节的公垂线共线的一条，可简化模型;两关节Z轴相交，它们之间没有公垂线(或者说公垂线距离为零)。这时可将垂直于两条轴线构成的平面的直线定义为X轴(相当于选取两条Z轴的又积方向作为X轴)，可化模型。

MATLAB实现代码:

clc;
clear;
%% 机械臂建模
% 定义各个连杆以及关节类型，默认为转动关节
% theta d a alpha
L1 = Link([0 0 0 pi/2], 'standard'); % [四个DH参数], options
L2 = Link([0 0.7405 0.260 0], 'standard');
L3 = Link([0 0 0.945 0], 'standard');
L4 = Link([0 0 1.245 0], 'standard');
robot = SerialLink([L1,L2,L3,L4],'name','4DOF Robotic Arm'); % 将四个连杆组成机械臂
robot.name = '新时达SP-120机器人';
robot.display();
init_ang = [0 0 0 0];
targ_ang = [0, pi/6, pi/9, pi/6];
step = 200;
[q,qd,qdd] = jtraj(init_ang,targ_ang,step); % 关节空间规划轨迹，得到机器人末端运动的[位置，速度，加速度]
T0 = robot.fkine(init_ang); % 正运动学解算
Tf = robot.fkine(targ_ang);
disp('正运动学的解：');
disp(Tf);
Tc = ctraj(T0,Tf,step); % 笛卡尔空间规划轨迹，得到机器人末端运动的变换矩阵
Tjtraj = transl(Tc);
hold on;
view(3); 
qq = robot.ikine(Tc, 'mask', [1 1 1 0 0 0], 'q0', init_ang);
final_pose = robot.fkine(qq(end,:)); % 取qq中的最后一行，即最终关节角度
final_position = final_pose(1:3, 4);
disp('逆运动学（x, y, z）为：');
disp(final_position');
% 从变换矩阵中提取最终方向的旋转矩阵
final_orientation = final_pose(1:3, 1:3);
disp('逆运动学：');
disp(final_orientation);
robot.plot(qq);
view([-39.2 19.3])

逆运动学求解过程

几何法---MATLAB实现代码：

clear;
clc;
syms thetal theta2 theta3 alpha l0 l1 l2 x y m n k a b c;
%连杆长度
l0 = 0.7450
l1 =0.945
l2 =0.1245
%目标值
x=10
y=11+12
alpha=pi/2;
m=12*cos(alpha)-x
n=12*sin(alpha)-y
k=(11^2-10^2-m^2-n^2)/2/10
a=m^2+n^2
b=-2*n*k
c=k^2-m^2
(-b-sqrt(b^2-4*a*c))/2/a
k=(10^2-11^2-m^2-n^2)/2/11;
a-m^2+n^2;
b=-2*n*k;
c=k^2-m^2
(-b+sqrt(b^2-4*a*c))/2/a ;

代数法--MATLAB实现代码

clc;
clear all;
close all;
%关节长度设置
a1_0=12;
a2_0=20;
T03_03 = forward(61.5,180,-20,a1_0,a2_0);
disp("正解：")
disp(T03_03);
theta = inverse(T03_03,a1_0,a2_0);
disp("逆解：")
disp(theta);
[R C] =size(theta);
if R==1
T03_verify = zeros(4,4);
T03_verify =forward(theta(1,1),theta(1,2),theta(1,3),a1_0,a2_0);
disp("逆解为无数组解情况下：")
disp(T03_verify);
elseif R==2
T03_verify =zeros(4,4,2);
for i = 1:2
T03_verify(:,:,i) = forward(theta(i,1),theta(i,2),theta(i,3),a1_0,a2_0);
disp(T03_verify(:,:,i));
end
end

function T03 = forward(theta1,theta2,theta3,a1,a2)
T01 =[cosd(theta1),-sind(theta1),0,0;
sind(theta1),cosd(theta1),0,0;
0,0,1,a1;
0,0,0,1
];
T12 = [cosd(theta2),-sind(theta2),0,0;
0,0,1,0;
-sind(theta2),-cosd(theta2),0,0;
0,0,0,1
];
T23 =[cosd(theta3),-sind(theta3),0,0;
0,0,-1,-a2;
sind(theta3),cosd(theta3),0,0;
0,0,0,1
];
T03=T01*T12*T23;
end

function theta =inverse(T03,a1,a2)
T=T03;
const =45;
tol =1e-4;
if(T(1,3)==0&&T(2,3)==0)
theta =zeros(1,3);
if abs(T(3,4)-(a1+a2))<tol
inver_theta2_1=0;
inver_theta1_1=const;
inver_theta3_1=atan2d(-T(1,2),T(1,1))-inver_theta1_1;
theta(1,:) =[inver_theta1_1,inver_theta2_1,inver_theta3_1];
elseif abs(T(3,4)-(a1-a2))<tol
inver_theta2_2=180;
inver_theta1_2=const;
inver_theta3_2=atan2d(T(1,2),-T(1,1))+inver_theta1_2;
theta(1,:)=[inver_theta1_2,inver_theta2_2,inver_theta3_2];
end
else 
s2 =(1-T(3,3)^2)^(0.5);
inver_theta2_1 = atan2d(s2,T(3,3));
inver_theta2_2 = atan2d(-s2,T3(3,3));
inver_theta1_1 = atan2d(T(2,3),T(1,3));
inver_theta1_2 = atan2d(-T(2,3),-T(1,3));
inver_theta3_1 = atan2d(T(3,2)*cscd(inver_theta2_1),-T(3,1)*cscd(inver_theta2_1));
inver_theta3_2 = atan2d(T(3,2)*cscd(inver_theta2_2),-T(3,1)*cscd(inver_theta2_2));
theta = zeros(2,3);
theta(1,:)=[inver_theta1_1,inver_theta2_1,inver_theta3_1];
theta(2,:)=[inver_theta1_2,inver_theta2_2,inver_theta3_2];
end
end