用matlab实现机械臂正逆运动学控制

设计要求：
1.建立一个三自由度的机器人
2.建立坐标系，给出 D-H 参数表；
3.推导正运动学，并写出机器人的齐次变换矩阵；
4.推导逆运动学，并让机器人完成按要求绘制给定图形。
5.MATLAB 程序源代码；

一、设计三轴机器人

设计出如上图的三轴机器人，第一个和第三个轴是旋转的，第二个是伸长的。第一个轴到第二个轴的距离是100cm，第二个轴的伸长量是0~100cm，第三个轴到手持器的距离是100cm。因此可以得出D-H参数表。

二、建立坐标系，给出 D-H 参数表
建立坐标系如下所示，由下图得出下面的D-H参数表

三、推导正运动学，并写出三个齐次变换矩阵
假设现在位于本地参考坐标系xn-zn,那么通过以下4步标准运动即可到达下一个本地参考坐标系xn+1-zn+1。

绕z_n轴旋转θ_(n+1)，使得x_n 和x_(n+1)互相平行；
沿z_n 轴平移d_(n+1) 距离，使得x_n 和x_(n+1) 共线；
沿已经旋转过的x_n轴平移a_(n+1)的距离，使得x_n 和x_(n+1)的原点重合；
将z_n轴绕x_(n+1)轴旋转α_(n+1)，使得z_n轴与z_(n+1)轴对准；
根据矩阵右乘可得到以下结果

根据3dof_robot D-H表表以及上面公式，可得机器人的正运动学方程如下

四、推导逆运动学，并让机器人完成按要求绘制给定图形

U1与A3对应颜色相比较得如下三个式子：
100 – pz=100cos(zeta3)
pycos(zeta1) - d2 - pxsin(zeta1)= 100sin(zeta3)
pxcos(zeta1) + pysin(zeta1)=0

三式联合求解得出
zeta1=-arctan(px/py)
zata3=arccos((100-pz)/100)
D2=pycos(zeta1)-pxsin(zeta1)-100*sin(zata3)

主要matlab代码
1.建机器人
zq_3dof_robot.m

ToDeg = 180/pi;
ToRad = pi/180;
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';Link= struct('name','Body' , 'th',  0, 'dz', 0,  'dy', 0, 'dx', 0, 'alf',90*ToRad,'az',UZ);     % az
Link(1)= struct('name','Base' , 'th',  0*ToRad, 'dz', 0, 'dy', 0,'dx', 0, 'alf',0*ToRad,'az',UZ);        %Base To 1
Link(2) = struct('name','J1' , 'th',   0*ToRad, 'dz', 100, 'dy', 0, 'dx', 0, 'alf',-90*ToRad,'az',UZ);       %1 TO 2
Link(3) = struct('name','J2' , 'th',  90*ToRad, 'dz', 200, 'dy', 0,  'dx', 0, 'alf',90*ToRad,'az',UZ);    %2 TO 3
Link(4) = struct('name','J3' , 'th',  0*ToRad, 'dz', 0, 'dy', 0, 'dx', 100, 'alf',0*ToRad,'az',UZ);          %3 TO E

2.画工作空间
draw_6DOF_Workplace.m

close all;    %删除其句柄未隐藏的所有图窗。
clear;        %清除工作空间ToDeg = 180/pi;   %转化为度数
ToRad = pi/180;   %转化为弧度point1=[];    %设为矩阵
point2=[];
point3=[];
th_interval = 40;    %弧间隔
z_interval = 4;      %线间隔th1=0;   %为th1至th6设定初始值
th2=0;
th3=0;global Linknum = 1;for theta1=-180:th_interval:180   %循环画工作空间for dt2=00:z_interval:100for theta3=-180:th_interval:180zq_robot_dh(th1+theta1,th2+dt2,th3+theta3,1);  %,d4+dz4,th5+theta5,th6+theta6point1(num) = Link(4).p(1);    %用这个矩阵来存数据，这里共存三行数据point2(num) = Link(4).p(2);point3(num) = Link(4).p(3);num = num + 1;plot3(point1,point2,point3,'r*');hold on;   %这里用了point1,point2,point3，而不用 point1(num)，point2(num), point3(num)?  endend
endcla;       %cla 从当前坐标区删除包含可见句柄的所有图形对象，把上面的图形清除。
plot3(point1,point2,point3,'r*');      %这里再画一个图形
axis([-400,400,-400,400,-400,400]);    %设置轴范围和纵横比
grid on;                %显示 gca 命令返回的当前坐标区或图的主网格线。主网格线从每个刻度线延伸。grid off 删除当前坐标区或图上的所有网格线。

3.根据逆运动学方程求关节角度
zq_robot_qiunijie.m

function [ th1,d2,th3] = zq_robot_qiunijie( px,py,pz )
ToDeg = 180/pi;
ToRad = pi/180;th1=-atan2(px,py);        %逆运动学方程
th3=acos((100-pz)/100);
d2=py*cos(th1)-px*sin(th1)-100*sin(th3);fprintf('th1=%4.2f \n',th1*ToDeg);   %观察输出结果
fprintf('d2=%4.2f \n',d2);
fprintf('th3=%4.2f \n',th3*ToDeg);end

4.画正方体（此处用到正、逆运动学）
draw_cube.m

close all;
clear;ToDeg = 180/pi;
ToRad = pi/180;
point1=[];    %设为矩阵
point2=[];
point3=[];num=1;
global Linkfor z=0:5:50for y=-25:5:25for x=50:5:100[th1,d2,th3]= zq_robot_qiunijie(x,y,z); %逆运动学th1=th1*ToDeg;th3=th3*ToDeg;move=zq_robot_dh(th1,d2,th3,1);  %正运动学point1(num) = Link(4).p(1);    %用这个矩阵来存数据，这里共存三行数据point2(num) = Link(4).p(2);point3(num) = Link(4).p(3);plot3(point1,point2,point3,'r.');hold on;   %这里用了point1,point2,point3，而不用 point1(num)，point2(num), point3(num)?  fprintf('point1=%4.2f \n',point1(num));     %观察输出点的情况fprintf('point2=%4.2f \n',point2(num));fprintf('point3=%4.2f \n',point3(num));num = num + 1;endend
end
grid on;

5.写“志”字（此处用到正、逆运动学）
draw_writing.m

close all;
clear;ToDeg = 180/pi;
ToRad = pi/180;
point1=zeros(100,1);    %设为矩阵
point2=zeros(100,1);
point3=zeros(100,1);  num=1;
global Linkxx=50;
yy=[20;20;12;14;16;18;20;22;24;26;28;20;18;20;22;14;12;16;18;20;22;24;26;26;18;20;22];
zz=[50;48;46;46;46;46;46;46;46;46;46;44;42;42;42;38;36;36;34;34;34;34;35;34;38;37;38];for i=1:1:27[th1,d2,th3]= zq_robot_qiunijie(xx,yy(i),zz(i));    %求逆解th1=th1*ToDeg;th3=th3*ToDeg;move=zq_robot_dh(th1,d2,th3,1);point1(num) = Link(4).p(1);    %用这个矩阵来存数据，这里共存三行数据point2(num) = Link(4).p(2);point3(num) = Link(4).p(3);plot3(point1,point2,point3,'r*');hold on;   %这里用了point1,point2,point3，而不用 point1(num)，point2(num), point3(num)?  fprintf('point1=%4.2f \n',point1(num));     %观察输出点的情况fprintf('point2=%4.2f \n',point2(num));fprintf('point3=%4.2f \n',point3(num));num = num + 1;end
grid on;

6.连杆
Connect3D.m

function Connect3D(p1,p2,option,pt)        %这是连接两个关节成一条杆的函数，Link(i).p表示第i个关节的空间位置。h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);    %画p1点到p2点的直线，p1,p2两点都是四行一列的矩阵，不过这里取前三行的值。option是线条颜色值。
set(h,'LineWidth',pt)    %这里pt为线宽，即机器人杆的宽度。

7.画工作空间（封装成函数而已，无他）
DHfk6Dof_Workplace.m

function pic=DHfk6Dof_Workplace(th1,th2,th3,fcla,fplot)
%这是用来画工作空间的函数内核  d4,th5,th6,
% close all
global Link% zq_3dof_robot;
%Build_3DOFRobot_Lnya;
radius    = 10;
len       =   20;
joint_col = 0;plot3(0,0,0,'ro'); Link(2).th=th1*pi/180;           %变成弧度，th1取至draw_6DOF_Workplace。
Link(3).th=th2*pi/180;
Link(4).th=th3*pi/180;% p0=[0,0,0]';for i=1:4
Matrix_DH_Ln(i);  %生成关节链接的D-H矩阵。
endfor i=2:4Link(i).A=Link(i-1).A*Link(i).A;     %第i+1个矩阵乘第i个矩阵，矩阵右乘，把所有矩阵相乘。 Link(i)函数取至Matrix_DH_Ln(i)。Link(i).p= Link(i).A(:,4);     %取Link(i).A中所有行的第4列放到Link(i).p，把第i个关节的位置存在Link(i).p中。Link(i).n= Link(i).A(:,1);     %同上Link(i).o= Link(i).A(:,2);     %同上Link(i).a= Link(i).A(:,3);     %同上Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];  %把第i个关节的姿态存在 Link(i).R中if fplot   %当fplot为1时执行下面两个函数Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;             %'b'，是指线条为蓝色，Link(i)函数取至Matrix_DH_Ln(i)。画杆  ，hold on 保留当前坐标区中的绘图，从而使新添加到坐标区中的绘图不会删除现有绘图。DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;   %画圆筒end
endgrid on;
% view(134,12);
axis([-500,500,-500,500,-500,500]);    %指定范围
xlabel('x');
ylabel('y');
zlabel('z');
drawnow;
pic=getframe;if(fcla)cla;
end

8.D-H矩阵的算法函数
Matrix_DH_Ln.m

function Matrix_DH_Ln(i)
%这个是D-H矩阵的算法函数
% Caculate the D-H Matrix
global LinkToDeg = 180/pi;
ToRad = pi/180;C=cos(Link(i).th);
S=sin(Link(i).th);
Ca=cos(Link(i).alf);
Sa=sin(Link(i).alf);
a=Link(i).dx;    %distance between zi and zi-1
d=Link(i).dz;    %distance between xi and xi-1
y=Link(i).dy;Link(i).n=[C,S,0,0]';
Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';
Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
Link(i).p=[a*C-y*S,a*S+y*C,d,1]';    %书本第57页的D-H矩阵Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];%把上面D-H矩阵前面的3*3矩阵存起来
Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];%把第i到i+1的D-H矩阵存进Link(i).A

9.画关节
DrawCylinder.m

function h = DrawCylinder(pos, az, radius,len, col)        %这是个画圆筒（关节）的函数
% draw closed cylinder
%
%******** rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps rot = eye(3);
elseax = ax/ax_n;ay = cross(az,ax);ay = ay/norm(ay);rot = [ax ay az];
end%********** make cylinder
% col = [0 0.5 0];  % cylinder colora = 20;    % number of side faces
theta = (0:a)/a * 2*pi;x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));for n=1:size(x,1)xyz = [x(n,:);y(n,:);z(n,:)];xyz2 = rot * xyz;x2(n,:) = xyz2(1,:);y2(n,:) = xyz2(2,:);z2(n,:) = xyz2(3,:);
end%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);for n=1:2patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end

10.用于显示的
zq_robot_dh.m

function pic = zq_robot_dh( th1,distance,th3,fcla )
%UNTITLED4 此处显示有关此函数的摘要
%   此处显示详细说明global Linkzq_3dof_robot;
radius = 10;
len = 30;
joint_col = 0;plot3(0,0,0,'ro');Link(2).th=th1*pi/180;
Link(3).dz=distance;
Link(4).th=th3*pi/180;p0=[0,0,0]';for i=1:4
Matrix_DH_Ln(i);
endfor i=2:4
Link(i).A=Link(i-1).A*Link(i).A;
Link(i).p= Link(i).A(:,4);
Link(i).n= Link(i).A(:,1);
Link(i).o= Link(i).A(:,2);
Link(i).a= Link(i).A(:,3);
Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az,radius,len, joint_col); hold on;
endgrid on;
% view(134,12);
axis([-200,200,-200,200,-100,200]);
xlabel('x');
ylabel('y');
zlabel('z');
drawnow;
pic=getframe;
if(fcla)
cla;
endend

11.正运动学(内核函数)
zhengyundongxue.m

function [ result ] = zhengyundongxue( A )
%dh矩阵函数
zeta=A(1);
d=A(2);
a=A(3);
alf=A(4);result=[cos(zeta)  -sin(zeta)*cos(alf)  sin(zeta)*sin(alf)  a*cos(zeta);sin(zeta)   cos(zeta)*cos(alf)    -cos(zeta)*sin(alf)  a*sin(zeta);0          sin(alf)             cos(alf)             d;0           0                    0                  1];end

12.解算正运动学
jisuanzhengyundong.m

syms pi d2 zeta1 zeta2 zeta3 A4  a1 a2 a3   nx ny nz ox oy oz ax ay az px py pz  U1;
%这是用来求正运动学中各个dh矩阵的
%p1=[0 d2 0 0];
p1=[zeta1 100 0 -pi/2];
A1=zhengyundongxue(p1);
A1=simplify(A1)%p2=[zeta2 0 100 pi/2];
p2=[pi/2 d2 0 pi/2];
A2=zhengyundongxue(p2);
A2=simplify(A2)%p3=[zeta3 0 100 0];
p3=[zeta3 0 100 0];
A3=zhengyundongxue(p3);
A3=simplify(A3)A4=A1*A2*A3;
A4=simplify(A4)a1=inv(A1);     %求逆
a2=inv(A2);
a3=inv(A3);
a1=simplify(a1)
a2=simplify(a2)
a3=simplify(a3)U=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1];%设出要求的矩阵UU1=a2*a1*U;
U1=simplify(U1)

13.坐标变换，测试用的
Computer_T.m

close all;
clear;ToDeg = 180/pi;
ToRad = pi/180;syms theta d a alpha y;
T1 = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0; 0 0 0 1];
T2 = [1 0 0 0; 0 1 0 0; 0 0 1 d; 0 0 0 1];
T3 = [1 0 0 a; 0 1 0 0; 0 0 1 0; 0 0 0 1];
T4 = [1 0 0 0; 0 cos(alpha) -sin(alpha) 0; 0 sin(alpha) cos(alpha) 0; 0 0 0 1];
Ty = [1 0 0 0; 0 1 0 y; 0 0 1 0; 0 0 0 1];
T = T1 * T2 * Ty * T3 * T4syms theta1 theta2 theta3 d4 theta5 theta6
theta=theta1;d=100;y=0;a=0;alpha = -90*ToRad;
A1 = subs(T)
theta=-90*ToRad+theta2;d=0;y=0;a=100;alpha = 0;
A2 = subs(T)
theta=theta3;d=0;y=50;a=0;alpha = -90*ToRad;
A3 = subs(T)
% theta=0;d=50+d4;y=0;a=0;alpha = 0;
% A4 = subs(T)
% theta=theta5;d=50;y=0;a=0;alpha = 90*ToRad;
% A5 = subs(T)
% theta=90*ToRad+theta6;d=0;y=0;a=50;alpha = 0;
% A6 = subs(T)
A = A1 * A2 * A3   %* A4 * A5 * A6

上面代码比较多，逐一放到matlab中运行一下，看看各个代码的效果如何。

2022-4-27：根据评论区的一些问题更新了一下，大家把最新代码文件(.m文件)拷贝到自己的工程目录下后，运行draw_cube.m和draw_writing.m即可看到运动效果，也可以运行draw_6DOF_Workplace.m来查看工作空间绘制情况。回头看自己四年前的代码，写得有点粗糙，不想大改了，大家谅解一下，哈哈。

用matlab实现机械臂正逆运动学控制相关推荐

修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
实验一机械臂正逆运动学
实验一机械臂正逆运动学一.实验目的 1.巩固正逆运动学基础概念. 2.了解正逆运动学在机械臂控制中的实际用途. 二.实验内容 1.机械臂模型DH参数的计算. 2.机械臂正运动学的计算. 3.机械臂 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
ROS机械臂正逆运动学
这里做一个六轴机械臂用于正逆运动学实验. 这里其实一共只有3轴,只有3轴位置没有姿态.所以urdf文件里我在末端做了3个虚拟关节,以便将kdl的frame能够填满,使得齐次坐标变换是规则的. 1.ur ...
【机器人原理与实践（三）】六轴机械臂正逆解控制
文章目录 3.1 空间转换矩阵的理解 3.1.1平移变换 3.1.2旋转变换 3.2 D-H参数法 3.3 建立机械臂模型 3.3.1 机械臂模型介绍 3.3.2 使用Matlab进行示教仿真 3.4 ...
matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...

用matlab实现机械臂正逆运动学控制

用matlab实现机械臂正逆运动学控制相关推荐

最新文章

热门文章