1 正运动学

正向运动学已知条件为各个关节的角度，通过各个关节的角度来求解机械手末端的位姿。

1.1 DH法参数表

按如上步骤对六自由度机械手建立坐标系，得到如图：

根据所建立的坐标系采用Denavit-Hartenbery法进行运动学求解，其连杆参数如下表所示。

令l1=50cm，l2=l3=40cm，l4=20cm，l5=l6=0cm。

1.2 建立运动学方程

变换矩阵的运算可以利用matlab中的符号变量syms，以坐标系4对坐标系3的坐标变换为例：

%坐标系4对坐标系3的坐标变换
syms c4 s4 l4;
T1=[1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];
T2=[c4 -s4 0 0;s4 c4 0 0 ;0 0 1 0;0 0 0 1];
T3=[1 0 0 0;0 1 0 0;0 0 1 l4;0 0 0 1];
T34=T1*T2*T3%得到答案
T34 =[ c4, -s4,  0,   0]
[  0,   0, -1, -l4]
[ s4,  c4,  0,   0]
[  0,   0,  0,   1]

可以得出各个变换矩阵，也可通过手动计算的方式，得到变换矩阵：

由此可得：

计算可得：

1.3 Matlab建模

根据所得的DH参数表，再利用matlab机器人工具箱robotic toolbox，可以很简便的得到matlab模型，matlab程序如下：

clear all
clc
%The parameter of DH
l=[0.5 0.4 0.4 0.2 0.2 0.2 0.2]
a=pi/2
%MDH coordinate
%         θ    d       a    α       offset
L(1)=Link([0   0        0    0          0  ],'modified');
L(2)=Link([0   0        0    a          0  ],'modified');
L(3)=Link([0   0       l(2)    0          0  ],'modified');
L(4)=Link([0   l(3)+l(4)    0    a          0  ],'modified');
L(5)=Link([0   0        0   -a          0  ],'modified');
L(6)=Link([0   l(5)       0    a          0  ],'modified');
%L(7)=Link([0   l(6)       0    0          0  ],'modified');qr=[0 0 pi/2 0 0 0 ] ;  % ready
qu=[0 pi/3 -pi/6 0 pi/3 0 ];
%qu=[0 0 0 0 0 0 0];% standup robot=SerialLink(L,'name','robot6R','manufacturer','Unimation','comment','AK&B');
robot.display();  %display MDH tablerobot.fkine(qr) %zero
robot.plot(qr); %readyrobot.fkine(qu);
robot.plot(qu); %standupt=0:0.01:1;
[q,qd,qdd]=jtraj(qr,qu,t);
plot(t,qd,t,qdd);teach(robot)

得到结果：

l =0.5000    0.4000    0.4000    0.2000    0.2000    0.2000    0.2000a =1.5708robot = robot6R [Unimation]:: 6 axis, RRRRRR, modDH, slowRNE             - AK&B;
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|          0|          0|     1.5708|          0|
|  3|         q3|          0|        0.4|          0|          0|
|  4|         q4|        0.6|          0|     1.5708|          0|
|  5|         q5|          0|          0|    -1.5708|          0|
|  6|         q6|        0.2|          0|     1.5708|          0|
+---+-----------+-----------+-----------+-----------+-----------+ans = 0         0         1       1.20        -1         0         01         0         0         00         0         0         1

以及：

再最后的teach——示教图中，可以通过改变关节角度来实现对机器人运动的控制。

2 逆运动学

逆运动学分析与正运动学分析计算过程完全相反,即给出驱动机器人末端位姿,求解各个连杆关节的变量值。

可以得到

可以算出各个关节的选择角度。

3 工作空间

机械手的工作空间，可通过一下matlab程序得出：

clc
clear all
%DH参数
l=[0.5 0.4 0.4 0.2 0.2 0.2 0.2]
a=pi/2
%       theta    d        a        alpha     offset
L1=Link([0   0        0    0          0  ],'modified');
L2=Link([0   0        0    a          0  ],'modified');
L3=Link([0   0       l(2)    0          0  ],'modified');
L4=Link([0   l(3)+l(4)    0    a          0  ],'modified');
L5=Link([0   0        0   -a          0  ],'modified');
L6=Link([0   l(5)       0    a          0  ],'modified');
robot=SerialLink([L1 L2 L3 L4 L5 L6 ],'name','manman');
A=unifrnd(-pi,pi/2,[1,30000]);
B=unifrnd(-pi/2,pi/2,[1,30000]);
C=unifrnd(-pi,pi,[1,30000]);
D=unifrnd(-pi,pi/2,[1,30000]);
E=unifrnd(-pi/2,pi/2,[1,30000]);
F=unifrnd(-pi,pi,[1,30000]);G= cell(30000, 3);        %建立元胞数组
for n = 1:30000G{n} =[A(n) B(n) C(n) D(n) E(n) F(n)];
end                         %产生3000组随机点
H1=cell2mat(G);              %将元胞数组转化为矩阵
T=double(robot.fkine(H1));      %机械臂正解
figure(1)
scatter3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)))
robot.plot([pi/2 pi/4 0],'workspace',[-5 5 -5 5 -5 5 ],'tilesize',2)        %机械臂图

得到下图：

六自由度机械手正逆运动学相关推荐

六自由度机器人正逆运动学
简介本文主要是对传统六自由度机器人进行正逆运动学求解,选取大族机器人Elfin05 为分析的对象,开发语言是C++.(完善中) 机器人正运动学机器人正运动学推导过程各关节坐标系确定的通用方法: ...
三自由度求解正逆运动学
(空间机器人运动学刚结课,最后的大作业是根据一个3r机械臂求解正.逆运动学:雅可比矩阵:利用拉格朗日方程建立动力学方程:设置控制器:matlab仿真运动.搞了好久,在这就简单地记录一下吧,以及一些知识 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
三轴机械臂/三自由度四足单腿DH正逆运动学及matlab验证
实物模型 DH建立坐标系以及正逆运动学推导 Matlab验证 clear; clc; a1=-9.57*0.001;alpha1=pi/2; a2=-59.2*0.001; a3=-77*0.001; ...
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵
MATLAB机器人工具箱[1]-- 机械臂建模+正逆运动学+雅克比矩阵 1. 二维空间位姿描述 2. 三维空间位姿描述 3. 建立机器人模型 3.1 Link 类 3.2 SerialLink 类 3 ...
Matlab机器人工具箱（1）——机器人的建立、绘制与正逆运动学
Matlab机器人工具箱(1)--机器人的建立.绘制与正逆运动学前言 rtbdemo 机器人的建立代码解析单个Link的解释建立机器人整体的解释绘制正运动学逆运动学微分运动学(求雅克比 ...
MATLAB仿真Gough-Stewart并联机器人斯图尔特6自由度并联机器人逆运动学仿真动力学控制pid控制
MATLAB仿真Gough-Stewart并联机器人斯图尔特6自由度并联机器人逆运动学仿真动力学控制pid控制 1.搭建了六自由度Stewart并联机器人simulink simscape仿真模型 ...
10000多套机械手毕业设计课程设计毕业论文图纸分享/上下料机械手、搬运机械手、苹果采摘机械手、液压机械手、三自由度机械手、水果采摘机械手、六自由度机械手、焊接机械手、码垛机械手、四自由……
10000多套机械手毕业设计课程设计毕业论文图纸分享/部分清单如下: 车床上下料机械手毕业设计全套(设计说明书(论文)+CAD图纸+PLC接线图+液压系统图+外文翻译) 搬运机械手及其控制系统 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...

六自由度机械手正逆运动学