【机器人学】1-1.六自由度机器人运动学正解【附MATLAB代码】

鱼会上树HY

已于 2024-08-19 19:30:38 修改

阅读量3.4k

点赞数 19

分类专栏：机器人学文章标签：机器人

于 2024-06-12 15:23:23 首次发布

本文链接：https://blog.youkuaiyun.com/y12345655/article/details/139620689

版权

机器人学专栏收录该内容

23 篇文章

订阅专栏

D-H参数

标准型D-H

对于标准型，X轴的方向以当前的Z轴和前一个关节的Z轴（Zi-1轴）的叉乘方向，右手定则：由Zi-1轴转向Z轴，大拇指方向即为X轴方向。由Zi-1轴指向Z轴，公垂线方向即是X轴的方向。

建模步骤：

通过每个旋转和平移得到旋转矩阵：

可以得到变换矩阵：

改进型D-H

对于改进型来说，X轴的方向以当前的Z轴和后一个关节的Z轴的叉乘方向，由Z轴转向

轴，大拇指方向即为X轴方向。

建模步骤：

运动学模型建立

假设有六轴机器人的D-H参数如下所示：

	关节1	关节2	关节3	关节4	关节5	关节6
$\alpha$	0	90	0	0	-90	90
a	0	0	425	393	0	0
d	160.7	0	0	113.3	99	93.6
$\theta$	0	90	0	-90	0	0
$\beta$	0	0	0	0	0	0

选用改进型的D-H参数，各矩阵分别如下所示：

对该六个齐次变换矩阵按顺序相乘后得到六自由度机器人的总变换：

当给定关节角一与关节角6的值时即可确定机器人末端相对于极坐标系的位姿矩阵。但4*4的姿态矩阵并不能直观感受机器人的末端姿态，通常用轴线方式表示。

姿态矩阵转换为轴线表示：

MATLAB仿真验证

% 求齐次变换矩阵
function T = DHTrans(alpha, a, d, theta)
T= [cos(theta)             -sin(theta)            0            a;
    sin(theta)*cos(alpha)  cos(theta)*cos(alpha)  -sin(alpha)  -sin(alpha)*d;
    sin(theta)*sin(alpha)  cos(theta)*sin(alpha)  cos(alpha)   cos(alpha)*d;
    0                      0                      0            1];
end

function Rxyz=RotMat_AxisAngle(R)
    theta = acos((R(1,1)+R(2,2)+R(3,3)-1)/2);
    r = 1/2/sin(theta)*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)];
    Rxyz=theta*r;
end

% 输入机器人q1,q2,q3,q4,q5,q6的角度
% 输出末端位姿的轴线表示
function position = modelRobot(theta)
th(1) = theta(1); d(1) = 0.1607; a(1) = 0; alp(1) = 0;
th(2) = theta(2); d(2) = 0; a(2) = 0; alp(2) = pi/2;   
th(3) = theta(3); d(3) = 0; a(3) = 0.425; alp(3) = 0;
th(4) = theta(4); d(4) = 0.1133; a(4) = 0.393; alp(4) = 0;
th(5) = theta(5); d(5) = 0.099; a(5) = 0; alp(5) = -pi/2;
th(6) = theta(6); d(6) = 0.0936; a(6) = 0; alp(6) = pi/2;

T01 = DHTrans(alp(1), a(1), d(1), th(1));
T12 = DHTrans(alp(2), a(2), d(2), th(2)+pi/2);
T23 = DHTrans(alp(3), a(3), d(3), th(3));
T34 = DHTrans(alp(4), a(4), d(4), th(4)-pi/2);
T45 = DHTrans(alp(5), a(5), d(5), th(5));
T56 = DHTrans(alp(6), a(6), d(6), th(6));

T06=T01*T12*T23*T34*T45*T56;
axis = RotMat_AxisAngle(T06)/pi*180;
position = [T06(1:3,4:4),axis];
end

机器人工具箱

机器人工具箱的安装一报错解决请看我的博客MATLAB机器人工具箱的安装与问题处理

clc
clear;
% 输入关节角
q(1)=150.1/180*pi;
q(2)=-25.19/180*pi;
q(3)=-35.07/180*pi;
q(4)=-155.82/180*pi;
q(5)=256.86/180*pi;
q(6)=-1.51/180*pi;
%机器人工具箱建立机器人模型
%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       0.1607       0        0       ],'modified');
L2=Link([  0       0        0       pi/2    ],'modified');L2.offset = pi/2;
L3=Link([  0       0        0.425       0       ],'modified');
L4=Link([  0       0.1133       0.393       0    ],'modified');L4.offset = -pi/2;
L5=Link([  0       0.099        0       -pi/2    ],'modified');
L6=Link([  0       0.0936        0        pi/2    ],'modified');
My_Robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','My_Robot');
%----------
%输出结果
data = My_Robot.fkine(q);
result = [data.t,RotMat_AxisAngle([data.n,data.o,data.a])/pi*180];
[result,modelRobot(q)]

结果：