UR5机械臂正向运动学

翻滚的石子

已于 2022-06-23 17:03:06 修改

阅读量1w

点赞数 8

分类专栏：机械臂文章标签：线性代数矩阵 matlab

于 2022-06-23 17:02:32 首次发布

本文链接：https://blog.csdn.net/qq_38962956/article/details/125430624

版权

机械臂专栏收录该内容

6 篇文章 8 订阅

订阅专栏

一、UR机械臂的DH参数

1.1 UR机械臂连杆坐标系图

图中红色为x轴，橘色为z轴。

在这里插入图片描述

1.2 UR机械臂的DH参数表

上图中的尺寸并不是完全精准尺寸，因此下面DH参数表中尺寸是从三维模型中测量的。

	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	0	$\pi/2$	89.15	$\theta_1$
2	-425	0	0	$\theta_2$
3	-392.25	0	0	$\theta_3$
4	0	$\pi/2$	109.15	$\theta_4$
5	0	$-\pi/2$	94.65	$\theta_5$
6	0	0	82.30	$\theta_6$

二、UR机械臂正运动学分析

2.1 正运动学建模

从坐标系 $i$ 到坐标系 $i - 1$ 的通用变换矩阵公式为：
$_{i-1}^{i}\textrm{T}=Rot_{z_{i-1}}(\theta_i)Trans_{z_{i-1}}(d_i)Rot_{x_i}(\alpha_i)Trans_{x_i}(a_i)\\ =\begin{bmatrix} cos\theta_i & -sin\theta_i & 0 & 0\\ sin\theta_i & cos\theta_i & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\cdot \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & d_i\\ 0 & 0 & 1 & 1 \end{bmatrix}\cdot \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & cos\alpha_i & -sin\alpha_i & 0\\ 0 & sin\alpha_i & cos\alpha_i & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\cdot \begin{bmatrix} 1 & 0 & 0 & a_i\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\\ =\begin{bmatrix} cos\theta_i & -sin\theta_i \cdot cos\alpha_i & sin\theta_i \cdot sin\alpha_i & a_i cos\theta_i\\ sin\theta_i & cos\theta_i \cdot cos\alpha_i & -cos\theta_i \cdot sin\alpha_i & a_i sin\theta_i\\ 0 & sin\alpha_i & cos\alpha_i & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix}$
将上述DH参数代入到上述公式，可以得到变换矩阵：
$_{1}^{0}\textrm{T}=\begin{bmatrix} cos\theta_1 & 0 & sin\theta_1 & 0\\ sin\theta_1 & 0 & -cos\theta_1 & 0\\ 0 & 1 & 0 & d_1\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{1}\textrm{T}=\begin{bmatrix} cos\theta_2 & -sin\theta_2 & 0 & a_2 cos\theta_2\\ sin\theta_2 & cos\theta_2 & 0 & a_2 sin\theta_2\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{3}^{2}\textrm{T}=\begin{bmatrix} cos\theta_3 & -sin\theta_3 & 0 & a_3 cos\theta_3\\ sin\theta_3 & cos\theta_3 & 0 & a_3 sin\theta_3\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{4}^{3}\textrm{T}=\begin{bmatrix} cos\theta_4 & 0 & sin\theta_4 & 0\\ sin\theta_4 & 0 & -cos\theta_4 & 0\\ 0 & 1 & 0 & d_4\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{5}^{4}\textrm{T}=\begin{bmatrix} cos\theta_5 & 0 & -sin\theta_5 & 0\\ sin\theta_5 & 0 & cos\theta_5 & 0\\ 0 & -1 & 0 & d_5\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{5}^{6}\textrm{T}=\begin{bmatrix} cos\theta_6 & -sin\theta_6 & 0 & 0\\ sin\theta_6 & cos\theta_6 & 0 & 0\\ 0 & 0 & 1 & d_6\\ 0 & 0 & 0 & 1 \end{bmatrix}$

正运动学的最终变换矩阵为：
$\textrm{T}=_{1}^{0}\textrm{T} \cdot _{2}^{1}\textrm{T} \cdot _{3}^{2}\textrm{T} \cdot _{4}^{3}\textrm{T} \cdot _{5}^{4}\textrm{T} \cdot _{6}^{5}\textrm{T}$

2.2 正运动学MATLAB编程

T_param.m：用于获取坐标系 $i$ 到坐标系 $i - 1$ 的通用变换矩阵

function T = T_param(theta, d, a, alpha)
% 输入：theta，d，a和alpha为变量，两个相邻连杆坐标系之间DH参数
% 输出：机械臂两个相邻连杆坐标系的正运动学变换矩阵

    T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha) , a*cos(theta);
         sin(theta), cos(theta)*cos(alpha) , -cos(theta)*sin(alpha), a*sin(theta);
         0         , sin(alpha)            , cos(alpha)            , d           ;
         0         , 0                     , 0                     , 1            ];
     
end

myfkine.m：用于获取UR机械臂的坐标变换矩阵

function T = myfkine(theta, d, a, alpha)
% 输入：theta，d，a和alpha为数组，存储机械臂DH参数
% 输出：机械臂正运动学变换矩阵

    T01 = T_param(theta(1), d(1), a(1), alpha(1));
    T12 = T_param(theta(2), d(2), a(2), alpha(2));
    T23 = T_param(theta(3), d(3), a(3), alpha(3));
    T34 = T_param(theta(4), d(4), a(4), alpha(4));
    T45 = T_param(theta(5), d(5), a(5), alpha(5));
    T56 = T_param(theta(6), d(6), a(6), alpha(6));
    
    T = T01*T12*T23*T34*T45*T56;
    
end

main.m：比较机器人工具箱建立的机器人模型和自编代码输出结果

%% 启动机器人工具箱
% startup_rvc 

%% 使用机器人工具箱建立机器人模型
% UR5机械臂参数
a = [0, -0.42500, -0.39225, 0, 0, 0];
d = [0.089159, 0, 0, 0.10915, 0.09465, 0.08230];
alpha = [pi/2, 0, 0, pi/2, -pi/2, 0];

% 建立UR5机械臂模型
L1 = Link('d', d(1),  'a', a(1), 'alpha', alpha(1),  'standard');
L2 = Link('d', d(2),  'a', a(2), 'alpha', alpha(2),  'standard');
L3 = Link('d', d(3),  'a', a(3), 'alpha', alpha(3),  'standard');
L4 = Link('d', d(4),  'a', a(4), 'alpha', alpha(4),  'standard');
L5 = Link('d', d(5),  'a', a(5), 'alpha', alpha(5),  'standard');
L6 = Link('d', d(6),  'a', a(6), 'alpha', alpha(6),  'standard');
tool_robot = SerialLink([L1,L2,L3,L4,L5,L6], 'name', 'UR5');
% tool_robot.display();
% view(3); % 必须添加，否则报错，错误是由版本引起
% tool_robot.teach();

%% 验证机器人正运动学模型
theta = [1,1/2,1,1,1,1];
T_tool = tool_robot.fkine(theta)    %机器人工具箱建立模型输出位姿信息

TT = myfkine(theta, d, a, alpha)    %编写代码输出位姿信息

输出结果：

在这里插入图片描述