UR5机械臂正向运动学

一、UR机械臂的DH参数

1.1 UR机械臂连杆坐标系图

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

1.2 UR机械臂的DH参数表

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

	a i a_i ai	α i \alpha_i αi	d i d_i di	θ i \theta_i θi
1	0	π / 2 \pi/2 π/2	89.15	θ 1 \theta_1 θ1
2	-425	0	0	θ 2 \theta_2 θ2
3	-392.25	0	0	θ 3 \theta_3 θ3
4	0	π / 2 \pi/2 π/2	109.15	θ 4 \theta_4 θ4
5	0	− π / 2 -\pi/2 −π/2	94.65	θ 5 \theta_5 θ5
6	0	0	82.30	θ 6 \theta_6 θ6

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

2.1 正运动学建模

从坐标系 i i i到坐标系 i − 1 i-1 i−1的通用变换矩阵公式为：
i − 1 i T = R o t z i − 1 ( θ i ) T r a n s z i − 1 ( d i ) R o t x i ( α i ) T r a n s x i ( a i ) = [ c o s θ i − s i n θ i 0 0 s i n θ i c o s θ i 0 0 0 0 1 0 0 0 0 1 ] ⋅ [ 1 0 0 0 0 1 0 0 0 0 0 d i 0 0 1 1 ] ⋅ [ 1 0 0 0 0 c o s α i − s i n α i 0 0 s i n α i c o s α i 0 0 0 0 1 ] ⋅ [ 1 0 0 a i 0 1 0 0 0 0 1 0 0 0 0 1 ] = [ c o s θ i − s i n θ i ⋅ c o s α i s i n θ i ⋅ s i n α i a i c o s θ i s i n θ i c o s θ i ⋅ c o s α i − c o s θ i ⋅ s i n α i a i s i n θ i 0 s i n α i c o s α i d i 0 0 0 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} i−1iT=Rotzi−1(θi)Transzi−1(di)Rotxi(αi)Transxi(ai)=⎣⎢⎢⎡cosθisinθi00−sinθicosθi0000100001⎦⎥⎥⎤⋅⎣⎢⎢⎡10000100000100di1⎦⎥⎥⎤⋅⎣⎢⎢⎡10000cosαisinαi00−sinαicosαi00001⎦⎥⎥⎤⋅⎣⎢⎢⎡100001000010ai001⎦⎥⎥⎤=⎣⎢⎢⎡cosθisinθi00−sinθi⋅cosαicosθi⋅cosαisinαi0sinθi⋅sinαi−cosθi⋅sinαicosαi0aicosθiaisinθidi1⎦⎥⎥⎤
将上述DH参数代入到上述公式，可以得到变换矩阵：
1 0 T = [ c o s θ 1 0 s i n θ 1 0 s i n θ 1 0 − c o s θ 1 0 0 1 0 d 1 0 0 0 1 ] _{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} 10T=⎣⎢⎢⎡cosθ1sinθ1000010sinθ1−cosθ10000d11⎦⎥⎥⎤

2 1 T = [ c o s θ 2 − s i n θ 2 0 a 2 c o s θ 2 s i n θ 2 c o s θ 2 0 a 2 s i n θ 2 0 0 1 0 0 0 0 1 ] _{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} 21T=⎣⎢⎢⎡cosθ2sinθ200−sinθ2cosθ2000010a2cosθ2a2sinθ201⎦⎥⎥⎤

3 2 T = [ c o s θ 3 − s i n θ 3 0 a 3 c o s θ 3 s i n θ 3 c o s θ 3 0 a 3 s i n θ 3 0 0 1 0 0 0 0 1 ] _{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} 32T=⎣⎢⎢⎡cosθ3sinθ300−sinθ3cosθ3000010a3cosθ3a3sinθ301⎦⎥⎥⎤

4 3 T = [ c o s θ 4 0 s i n θ 4 0 s i n θ 4 0 − c o s θ 4 0 0 1 0 d 4 0 0 0 1 ] _{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} 43T=⎣⎢⎢⎡cosθ4sinθ4000010sinθ4−cosθ40000d41⎦⎥⎥⎤

5 4 T = [ c o s θ 5 0 − s i n θ 5 0 s i n θ 5 0 c o s θ 5 0 0 − 1 0 d 5 0 0 0 1 ] _{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} 54T=⎣⎢⎢⎡cosθ5sinθ50000−10−sinθ5cosθ50000d51⎦⎥⎥⎤

5 6 T = [ c o s θ 6 − s i n θ 6 0 0 s i n θ 6 c o s θ 6 0 0 0 0 1 d 6 0 0 0 1 ] _{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} 56T=⎣⎢⎢⎡cosθ6sinθ600−sinθ6cosθ600001000d61⎦⎥⎥⎤

正运动学的最终变换矩阵为：
T = 1 0 T ⋅ 2 1 T ⋅ 3 2 T ⋅ 4 3 T ⋅ 5 4 T ⋅ 6 5 T \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} T=10T⋅21T⋅32T⋅43T⋅54T⋅65T

2.2 正运动学MATLAB编程

T_param.m：用于获取坐标系 i i i到坐标系 i − 1 i-1 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)    %编写代码输出位姿信息

输出结果：