一、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$的通用变换矩阵公式为：
$$\begin{gathered} {}_{i-1}^i\text{T}=Ro{t}_{z_{i-1}}({\theta }_i)Tran{s}_{z_{i-1}}(d_i)Ro{t}_{x_i}({\alpha }_i)Tran{s}_{x_i}(a_i) \\ =\left[\begin{array}{cccc}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{array}\right]\cdot \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& d_i\\ 0& 0& 1& 1\end{array}\right]\cdot \left[\begin{array}{cccc}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{array}\right]\cdot \left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \\ =\left[\begin{array}{cccc}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{array}\right] \end{gathered}$$
将上述DH参数代入到上述公式，可以得到变换矩阵：
$${}_1^0\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

$${}_2^1\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

$${}_3^2\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

$${}_4^3\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

$${}_5^4\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

$${}_5^6\text{T}=\left[\begin{array}{cccc}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{array}\right]$$

正运动学的最终变换矩阵为：
$$\text{T}{=}_1^0\text{T}{\cdot }_2^1\text{T}{\cdot }_3^2\text{T}{\cdot }_4^3\text{T}{\cdot }_5^4\text{T}{\cdot }_6^5\text{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)    %编写代码输出位姿信息

输出结果：