章节导览
前言
机器人DH参数有两种，标准DH参数和改进DH参数（Craig），UR默认使用的是标准DH参数，见UR官网说明。此外，DH参数表的建立也与X/Z方向的选择有关系，下述改进DH参数的坐标系与标准DH有所不同，所以最终姿态角与标准DH参数不一致（结果会影响机械臂的Offset和末端姿态角）。
一、UR机器人正运动学
1. UR5的标准D-H参数描述（MATLAB）
UR5的标准D-H图片描述
标准D-H参数表
标准D-H参数表（官方）
i-1 轴到 i 轴的变换矩阵为
i
−
1
T
i
=
[
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}T_i = \begin{bmatrix} cos\theta_i & -sin\theta_icos\alpha_i & sin\theta_isin\alpha_i & a_icos\theta_i\\ sin\theta_i & cos\theta_i cos\alpha_i & -cos\theta_isin\alpha_i & a_isin\theta_i \\ 0 & sin\alpha_i & cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
i−1Ti=
cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1
代入DH参数
0
T
1
=
[
c
o
s
q
1
0
s
i
n
q
1
0
s
i
n
q
1
0
−
c
o
s
q
1
0
0
1
0
d
1
0
0
0
1
]
1
T
2
=
[
c
o
s
q
2
−
s
i
n
q
2
0
a
2
c
o
s
q
2
s
i
n
q
2
c
o
s
q
2
0
a
2
s
i
n
q
2
0
0
1
0
0
0
0
1
]
^0T_1 = \begin{bmatrix} cosq_1 & 0 & sinq_1 & 0 \\ sinq_1 & 0 & -cosq_1 & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix} ^1T_2 = \begin{bmatrix} cosq_2 & -sinq_2 & 0& a_2cosq_2 \\ sinq_2 & cosq_2 & 0& a_2sinq_2 \\ 0 & 0& 1& 0 \\ 0 & 0& 0& 1 \end{bmatrix}
0T1=
cosq1sinq1000010sinq1−cosq10000d11
1T2=
cosq2sinq200−sinq2cosq2000010a2cosq2a2sinq201
2
T
3
=
[
c
o
s
q
3
−
s
i
n
q
3
0
a
3
c
o
s
q
3
s
i
n
q
3
c
o
s
q
3
0
a
3
s
i
n
q
3
0
0
1
0
0
0
0
1
]
3
T
4
=
[
c
o
s
q
4
0
s
i
n
q
4
0
s
i
n
q
4
0
−
c
o
s
q
4
0
0
1
0
d
4
0
0
0
1
]
^2T_3 = \begin{bmatrix} cosq_3 & -sinq_3 & 0& a_3cosq_3 \\ sinq_3 & cosq_3 & 0& a_3sinq_3 \\ 0& 0& 1& 0 \\ 0& 0& 0& 1 \end{bmatrix} ^3T_4 = \begin{bmatrix} cosq_4 & 0& sinq_4 & 0 \\ sinq_4 & 0& -cosq_4 & 0 \\ 0 & 1& 0& d_4 \\ 0 & 0& 0& 1 \end{bmatrix}
2T3=
cosq3sinq300−sinq3cosq3000010a3cosq3a3sinq301
3T4=
cosq4sinq4000010sinq4−cosq40000d41
4
T
5
=
[
c
o
s
q
5
0
−
s
i
n
q
5
0
s
i
n
q
5
0
c
o
s
q
5
0
0
−
1
0
d
5
0
0
0
1
]
5
T
6
=
[
c
o
s
q
6
−
s
i
n
q
6
0
0
s
i
n
q
6
c
o
s
q
6
0
0
0
0
1
d
6
0
0
0
1
]
^4T_5 = \begin{bmatrix} cosq_5 & 0& -sinq_5 & 0 \\ sinq_5 & 0& cosq_5 & 0 \\ 0 & -1 & 0 & d_5 \\ 0 & 0 & 0 & 1 \end{bmatrix} ^5T_6 = \begin{bmatrix} cosq_6 & -sinq_6 & 0& 0 \\ sinq_6 & cosq_6 & 0& 0 \\ 0 & 0 & 1& d_6 \\ 0 & 0 & 0& 1 \end{bmatrix}
4T5=
cosq5sinq50000−10−sinq5cosq50000d51
5T6=
cosq6sinq600−sinq6cosq600001000d61
确定根据末端位置向对于基座标的变换矩阵为
0
T
6
=
0
T
1
1
T
2
2
T
3
3
T
4
4
T
5
5
T
6
^0T_6 = {^0T_1}{^1T_2}{^2T_3}{^3T_4}{^4T_5}{^5T_6}
0T6=0T11T22T33T44T55T6
末端执行器位姿推导
工具矩阵为
T
t
o
o
l
=
0
T
6
=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
T_{tool} = ^0T_6 = \begin{bmatrix} n_x&o_x&a_x&p_x \\ n_y&o_y&a_y&p_y \\ n_z&o_z&a_z&p_z \\ 0&0&0&1 \end{bmatrix}
Ttool=0T6=
nxnynz0oxoyoz0axayaz0pxpypz1
式中， n x , n y , n z n_x , n_y, n_z nx,ny,nz 表示机械臂末端坐标系的x轴在基坐标系中的方向矢量； o x , o y , o z o_x , o_y, o_z ox,oy,oz 表示机械臂末端坐标系的y轴在基坐标系中的方向矢量； a x , a y , a z a_x , a_y, a_z ax,ay,az 表示机械臂末端坐标系的z轴在基坐标系中的方向矢量； p x , p y , p z p_x , p_y, p_z px,py,pz 表示机械臂末端在基坐标系中的坐标。
末端执行器的位置是 [ p x p y p z ] \begin{bmatrix} p_x & p_y & p_z\end{bmatrix} [pxpypz]
绕定轴X-Y-Z旋转（RPY角）
RPY角用于描述船舶、飞机和车辆的姿态时非常直观。横滚、俯仰和偏航是指分别绕x、y、z轴的旋转。（通常定义x轴为前进的方向、z轴垂直向下，y轴指向右手方向）
R
x
y
z
(
γ
,
β
,
α
)
=
R
z
(
α
)
∗
R
y
(
β
)
∗
R
x
(
γ
)
R_{xyz}(\gamma, \beta, \alpha) = R_z(\alpha)*R_y(\beta)*R_x(\gamma)
Rxyz(γ,β,α)=Rz(α)∗Ry(β)∗Rx(γ)
=
[
c
α
−
s
α
0
s
α
c
α
0
0
0
1
]
[
c
β
0
s
β
0
1
0
−
s
β
0
c
β
]
[
1
0
0
0
c
γ
−
s
γ
0
s
γ
c
γ
]
= \begin{bmatrix} c\alpha& -s\alpha& 0\\ s\alpha& c\alpha& 0\\ 0& 0& 1 \end{bmatrix} \begin{bmatrix} c\beta& 0& s\beta\\ 0& 1& 0\\ -s\beta& 0& c\beta \end{bmatrix} \begin{bmatrix} 1& 0& 0\\ 0& c\gamma& -s\gamma\\ 0& s\gamma& c\gamma \end{bmatrix}
=
cαsα0−sαcα0001
cβ0−sβ010sβ0cβ
1000cγsγ0−sγcγ
=
[
c
α
c
β
c
α
s
β
s
γ
−
s
α
c
γ
c
α
s
β
c
γ
+
s
α
s
γ
s
α
c
β
s
α
s
β
s
γ
+
c
α
c
γ
s
α
s
β
c
γ
−
c
α
s
γ
−
s
β
c
β
s
γ
c
β
c
γ
]
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
= \begin{bmatrix} c\alpha c\beta & c\alpha s\beta s\gamma - s\alpha c\gamma & c\alpha s\beta c\gamma + s\alpha s\gamma\\ s\alpha c\beta & s\alpha s\beta s\gamma + c\alpha c\gamma & s\alpha s\beta c\gamma - c\alpha s\gamma\\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{bmatrix}=\begin{bmatrix}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}
=
cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ
=
r11r21r31r12r22r32r13r23r33
末端执行器姿态是
I f β ≠ 90 ° β = A t a n 2 ( − r 31 , r 11 2 + r 21 2 ) α = A t a n 2 ( r 21 / c β , r 11 / c β ) γ = A t a n 2 ( r 32 / c β , r 33 / c β ) If \beta \ne 90° \\ \beta=Atan2(-r_{31}, \sqrt{r_{11}^2+r_{21}^2})\\ \alpha=Atan2(r_{21}/c\beta, r_{11}/c\beta)\\ \gamma=Atan2(r_{32}/c\beta, r_{33}/c\beta) Ifβ=90°β=Atan2(−r31,r112+r212)α=Atan2(r21/cβ,r11/cβ)γ=Atan2(r32/cβ,r33/cβ)，其中 − 90 ° ≤ β ≤ 90 ° -90°\le\beta\le90° −90°≤β≤90°
I f β = 90 ° : α = 0 ° γ = A t a n 2 ( r 12 , r 22 ) If \beta = 90°:\\ \alpha=0°\\ \gamma=Atan2(r_{12}, r_{22}) Ifβ=90°:α=0°γ=Atan2(r12,r22)
I f β = − 90 ° : α = 0 ° γ = − A t a n 2 ( r 12 , r 22 ) If \beta = -90°:\\ \alpha=0°\\ \gamma=-Atan2(r_{12}, r_{22}) Ifβ=−90°:α=0°γ=−Atan2(r12,r22)
机器人工具箱求解末端执行器的姿态角命令：
rpy = tr2rpy(T06, ‘xyz’)
标准D-H机器人工具箱代码如下：
clear,clc,close all;
d1 = 0.089159;
a2 = -0.42500;
a3 = -0.39225;
d4 = 0.10915;
d5 = 0.09465;
d6 = 0.0823;
%% Standard DH 注意a2 a3为负值
L(1) = Link('d',d1, 'a', 0, 'alpha', pi/2);
L(2) = Link('d',0, 'a', a2, 'alpha', 0);
L(3) = Link('d',0, 'a', a3, 'alpha', 0);
L(4) = Link('d',d4, 'a', 0, 'alpha', pi/2);
L(5) = Link('d',d5, 'a', 0, 'alpha', -pi/2);
L(6) = Link('d',d6, 'a', 0, 'alpha', 0);
robot = SerialLink(L, 'name', 'UR5');
target=[0,-pi/2,0,-pi/2,0,0];
fdata = robot.fkine(target) %求正解的齐次变换矩阵
robot.plot(target);
robot.teach(fdata,'rpy' )
效果展示：
2. UR5的改进D-H（Craig）参数描述（MATLAB）（DH坐标选择与官方有所不同）
改进D-H参数表（这里的偏移量与标准D-H参数表一致）
Craig D-H机器人工具箱代码如下：
clear,clc,close all;
d1 = 0.089159;
a2 = 0.42500;
a3 = 0.39225;
d4 = 0.10915;
d5 = 0.09465;
d6 = 0.0823;
%% Craig DH
L(1) = Link('alpha', 0, 'a', 0, 'd',d1, 'modified' );
L(2) = Link('alpha', -pi/2, 'a', 0, 'd',0, 'modified');
L(3) = Link('alpha', 0, 'a', a2, 'd',0, 'modified' );
L(4) = Link('alpha', 0, 'a', a3, 'd',d4, 'modified');
L(5) = Link('alpha', -pi/2, 'a', 0, 'd',d5, 'modified');
L(6) = Link('alpha', pi/2, 'a', 0, 'd',d6, 'modified' );
robot = SerialLink(L, 'name', 'UR5')
target=[0,-pi/2,0,-pi/2,0,0];
fdata = robot.fkine(target)
robot.plot(target)
robot.teach(fdata,'rpy' )
效果展示：
3. C++验证（参考官方Github代码）
输入double q[6] = { 0, -PI/2, 0, -PI/2, 0, 0 };
结果为：
上述C++计算结果与MDH建立坐标系的位姿一致。
二、UR机械臂逆运动学
逆运动学问题是指给定期望的末端执行器位姿，如何求取所需的关节坐标。
根据运动学正解求出的齐次变换矩阵T06，反解算各个关节的角度值
首先，还是以标准D-H参数得到的变换矩阵为例，已知
0
T
6
=
0
T
1
1
T
2
2
T
3
3
T
4
4
T
5
5
T
6
^0T_6 = {^0T_1}{^1T_2}{^2T_3}{^3T_4}{^4T_5}{^5T_6}
0T6=0T11T22T33T44T55T6
将上式两边同时左乘
(
1
0
T
)
−
1
(^0_1T)^{-1}
(10T)−1
1
T
5
=
(
1
0
T
)
−
1
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
(
6
5
T
)
−
1
{^1T_5}=(^0_1T)^{-1}\begin{bmatrix} n_x&o_x&a_x&p_x \\ n_y&o_y&a_y&p_y \\ n_z&o_z&a_z&p_z \\ 0&0&0&1 \end{bmatrix}(^5_6T)^{-1}
1T5=(10T)−1
nxnynz0oxoyoz0axayaz0pxpypz1
(65T)−1
等式左侧展开为
1
T
5
=
[
c
234
c
5
−
s
234
−
c
234
s
5
a
3
c
23
+
a
2
c
2
+
d
5
s
234
s
234
c
5
c
234
−
s
234
s
5
a
3
s
23
+
a
2
s
2
−
d
5
c
234
s
5
0
c
5
d
4
0
0
0
1
]
{^1T_5}=\begin{bmatrix} c_{234}c_5 & -s_{234} & -c_{234}s_5 & a_3c_{23} + a_2c_2 + d_5s_{234}\\ s_{234}c_5 & c_{234} & -s_{234}s_5 & a_3s_{23} + a_2s_2 - d_5c_{234} \\ s_5 & 0 & c_5 & d_4 \\ 0 & 0 & 0 & 1 \end{bmatrix}
1T5=
c234c5s234c5s50−s234c23400−c234s5−s234s5c50a3c23+a2c2+d5s234a3s23+a2s2−d5c234d41
等式右侧展开为
[
c
6
(
n
x
c
1
+
n
y
s
1
)
−
s
6
(
o
x
c
1
+
o
y
s
1
)
s
6
(
n
x
c
1
+
n
y
s
1
)
+
c
6
(
o
x
c
1
+
o
y
s
1
)
a
x
c
1
+
a
y
s
1
s
1
(
p
y
−
d
6
a
y
)
+
c
1
(
p
x
−
d
6
a
x
)
n
z
c
6
−
o
z
s
6
o
z
c
6
+
n
z
s
6
a
z
p
z
−
d
1
−
a
z
d
6
c
6
(
n
x
s
1
−
n
y
c
1
)
+
s
6
(
o
y
c
1
−
o
x
s
1
)
s
6
(
n
x
s
1
−
n
y
c
1
)
+
c
6
(
o
x
s
1
−
o
y
c
1
)
a
x
s
1
−
a
y
c
1
s
1
(
p
x
−
d
6
a
x
)
+
c
1
(
d
6
a
y
−
p
y
)
0
0
0
1
]
\begin{bmatrix} c_6(n_xc_1+n_ys_1)-s_6(o_xc_1+o_ys_1) & s_6(n_xc_1+n_ys_1)+c_6(o_xc_1+o_ys_1) & a_xc_1 + a_ys_1 & s_1(p_y-d_6a_y)+c_1(p_x-d_6a_x)\\ n_zc_6- o_zs_6 & o_zc_6+ n_zs_6 & a_z & p_z - d_1 - a_zd_6\\ c_6(n_xs_1-n_yc_1)+ s_6(o_yc_1-o_xs_1) & s_6(n_xs_1-n_yc_1)+c_6(o_xs_1-o_yc_1) & a_xs_1 - a_yc_1 & s_1(p_x-d_6a_x)+c_1(d_6a_y-p_y)\\ 0 & 0 & 0 & 1 \end{bmatrix}
c6(nxc1+nys1)−s6(oxc1+oys1)nzc6−ozs6c6(nxs1−nyc1)+s6(oyc1−oxs1)0s6(nxc1+nys1)+c6(oxc1+oys1)ozc6+nzs6s6(nxs1−nyc1)+c6(oxs1−oyc1)0axc1+ays1azaxs1−ayc10s1(py−d6ay)+c1(px−d6ax)pz−d1−azd6s1(px−d6ax)+c1(d6ay−py)1
利用等式两侧矩阵第3行，第4列相等求关节角1
−
s
1
(
d
6
a
x
−
p
x
)
+
c
1
(
d
6
a
y
−
p
y
)
=
d
4
-s_1(d_6a_x-p_x)+c_1(d_6a_y-p_y)=d_4
−s1(d6ax−px)+c1(d6ay−py)=d4
令
m
1
=
d
6
a
x
−
p
x
,
n
1
=
d
6
a
y
−
p
y
m_1=d_6a_x-p_x, n_1=d_6a_y-p_y
m1=d6ax−px,n1=d6ay−py
θ 1 = a t a n 2 ( n 1 , m 1 ) − a t a n 2 ( d 4 , ± m 1 2 + n 1 2 − d 4 2 ) ，其中 m 1 2 + n 1 2 − d 4 2 ≥ 0 \theta_1 = atan2(n_1, m_1)-atan2(d_4, \pm \sqrt{m_1^2+n_1^2-d_4^2})， 其中 m_1^2+n_1^2-d_4^2\ge 0 θ1=atan2(n1,m1)−atan2(d4,±m12+n12−d42)，其中m12+n12−d42≥0
至此共有2组解
利用等式左右两边第3行，第3列对应相等求关节角5
a
x
s
1
−
a
y
c
1
=
c
5
a_xs_1 - a_yc_1 =c_5
axs1−ayc1=c5
解得
θ 5 = ± a r c c o s ( a x s 1 − a y c 1 ) ，其中 a x s 1 − a y c 1 ≤ 1 \theta_5 = \pm arccos(a_xs_1 - a_yc_1)，其中 a_xs_1 - a_yc_1 \le1 θ5=±arccos(axs1−ayc1)，其中axs1−ayc1≤1
至此共有4组解
利用等式左右两边第3行，第1列对应相等求关节角6
−
s
6
(
o
x
s
1
−
o
y
c
1
)
+
c
6
(
n
x
s
1
−
n
y
c
1
)
=
s
5
-s_6(o_xs_1 - o_yc_1)+c_6(n_xs_1-n_yc_1) = s_5
−s6(oxs1−oyc1)+c6(nxs1−nyc1)=s5
令
m
6
=
o
x
s
1
−
o
y
c
1
,
n
6
=
n
x
s
1
−
n
y
c
1
m_6=o_xs_1 - o_yc_1, n_6=n_xs_1-n_yc_1
m6=oxs1−oyc1,n6=nxs1−nyc1
θ
6
=
a
t
a
n
2
(
n
6
,
m
6
)
−
a
t
a
n
2
(
s
5
,
±
m
6
2
+
n
6
2
−
s
5
2
)
\theta_6 = atan2(n_6, m_6)-atan2(s_5, \pm \sqrt{m_6^2+n_6^2-s_5^2})
θ6=atan2(n6,m6)−atan2(s5,±m62+n62−s52)
对第3行第1列、第3行第2列两项作平方和，得到
m
6
2
+
n
6
2
=
s
5
2
{m_6^2}+{n_6^2}={s_5^2}
m62+n62=s52，即
m
6
2
+
n
6
2
−
s
5
2
=
0
{m_6^2}+{n_6^2}-{s_5^2} =0
m62+n62−s52=0，
θ 6 = a t a n 2 ( n 6 , m 6 ) − a t a n 2 ( s 5 , 0 ) \theta_6=atan2(n_6, m_6)-atan2(s_5, 0) θ6=atan2(n6,m6)−atan2(s5,0)
至此共有4组解
1 T 5 ( 4 T 5 ) − 1 = ( 1 0 T ) − 1 [ n x o x a x p x n y o y a y p y n z o z a z p z 0 0 0 1 ] ( 6 5 T ) − 1 ( 5 4 T ) − 1 {^1T_5}{(^4T_5)^{-1}}=(^0_1T)^{-1}\begin{bmatrix} n_x&o_x&a_x&p_x \\ n_y&o_y&a_y&p_y \\ n_z&o_z&a_z&p_z \\ 0&0&0&1 \end{bmatrix}(^5_6T)^{-1}{(^4_5T)^{-1}} 1T5(4T5)−1=(10T)−1 nxnynz0oxoyoz0axayaz0pxpypz1 (65T)−1(54T)−1
等式左侧展开为
1
T
4
=
[
c
234
0
s
234
a
3
c
23
+
a
2
c
2
s
234
0
−
c
234
a
3
s
23
+
a
2
s
2
0
1
0
d
4
0
0
0
1
]
{^1T_4}=\begin{bmatrix} c_{234} & 0 & s_{234} & a_3c_{23} + a_2c_2 \\ s_{234} & 0 & -c_{234} & a_3s_{23} + a_2s_2 \\ 0 & 1 & 0 & d_4 \\ 0 & 0 & 0 & 1 \end{bmatrix}
1T4=
c234s234000010s234−c23400a3c23+a2c2a3s23+a2s2d41
等式右侧展开为
[
n
x
c
1
c
5
c
6
−
a
y
s
1
s
5
−
a
x
c
1
s
5
+
n
y
c
5
c
6
s
1
−
o
x
c
1
c
5
s
6
−
o
y
c
5
s
1
s
6
a
x
c
1
c
5
+
a
y
c
5
s
1
+
n
x
c
1
c
6
s
5
+
n
y
c
6
s
1
s
5
−
o
x
c
1
s
5
s
6
−
o
y
s
1
s
5
s
6
−
o
x
c
1
c
6
−
n
x
c
1
s
6
−
o
y
c
6
s
1
−
n
y
s
1
s
6
p
x
c
1
+
p
y
s
1
−
a
y
d
6
s
1
−
a
x
d
6
c
1
+
d
5
o
x
c
1
c
6
+
d
5
n
x
c
1
s
6
+
d
5
o
y
c
6
s
1
+
d
5
n
y
s
1
s
6
n
z
c
5
c
6
−
a
z
s
5
−
o
z
c
5
s
6
a
z
c
5
+
n
z
c
6
s
5
−
o
z
s
5
s
6
−
o
z
c
6
−
n
z
s
6
p
z
−
d
1
−
a
z
d
6
+
d
5
o
z
c
6
+
d
5
n
z
s
6
a
y
c
1
s
5
−
a
x
s
1
s
5
−
n
y
c
1
c
5
c
6
+
n
x
c
5
c
6
s
1
+
o
y
c
1
c
5
s
6
−
o
x
c
5
s
1
s
6
a
x
c
5
s
1
−
a
y
c
1
c
5
−
n
y
c
1
c
6
s
5
+
n
x
c
6
s
1
s
5
+
o
y
c
1
s
5
s
6
−
o
x
s
1
s
5
s
6
o
y
c
1
c
6
+
n
y
c
1
s
6
−
o
x
c
6
s
1
−
n
x
s
1
s
6
p
x
s
1
−
p
y
c
1
−
a
x
d
6
s
1
+
a
y
d
6
c
1
−
d
5
o
y
c
1
c
6
−
d
5
n
y
c
1
s
6
+
d
5
o
x
c
6
s
1
+
d
5
n
x
s
1
s
6
0
0
0
1
]
\begin{bmatrix} n_xc_1c_5c_6-a_ys_1s_5-a_xc_1s_5+n_yc_5c_6s_1-o_xc_1c_5s_6-o_yc_5s_1s_6 & a_xc_1c_5+a_yc_5s_1+n_xc_1c_6s_5+n_yc_6s_1s_5-o_xc_1s_5s_6-o_ys_1s_5s_6 & -o_xc_1c_6-n_xc_1s_6-o_yc_6s_1-n_ys_1s_6 & p_xc_1+p_ys_1-a_yd_6s_1-a_xd_6c_1+d_5o_xc_1c_6+d_5n_xc_1s_6+d_5o_yc_6s_1+d_5n_ys_1s_6\\ n_zc_5c_6-a_zs_5-o_zc_5s_6 & a_zc_5+n_zc_6s_5-o_zs_5s_6 & -o_zc_6-n_zs_6 & p_z-d_1-a_zd_6+d_5o_zc_6+d_5n_zs_6\\ a_yc_1s_5-a_xs_1s_5-n_yc_1c_5c_6+n_xc_5c_6s_1+o_yc_1c_5s_6-o_xc_5s_1s_6 & a_xc_5s_1-a_yc_1c_5-n_yc_1c_6s_5+n_xc_6s_1s_5+o_yc_1s_5s_6-o_xs_1s_5s_6 & o_yc_1c_6+n_yc_1s_6-o_xc_6s_1-n_xs_1s_6 & p_xs_1-p_yc_1-a_xd_6s_1+a_yd_6c_1-d_5o_yc_1c_6-d_5n_yc_1s_6+d_5o_xc_6s_1+d_5n_xs_1s_6\\ 0 & 0 & 0 & 1 \end{bmatrix}
nxc1c5c6−ays1s5−axc1s5+nyc5c6s1−oxc1c5s6−oyc5s1s6nzc5c6−azs5−ozc5s6ayc1s5−axs1s5−nyc1c5c6+nxc5c6s1+oyc1c5s6−oxc5s1s60axc1c5+ayc5s1+nxc1c6s5+nyc6s1s5−oxc1s5s6−oys1s5s6azc5+nzc6s5−ozs5s6axc5s1−ayc1c5−nyc1c6s5+nxc6s1s5+oyc1s5s6−oxs1s5s60−oxc1c6−nxc1s6−oyc6s1−nys1s6−ozc6−nzs6oyc1c6+nyc1s6−oxc6s1−nxs1s60pxc1+pys1−ayd6s1−axd6c1+d5oxc1c6+d5nxc1s6+d5oyc6s1+d5nys1s6pz−d1−azd6+d5ozc6+d5nzs6pxs1−pyc1−axd6s1+ayd6c1−d5oyc1c6−d5nyc1s6+d5oxc6s1+d5nxs1s61
利用等式左右两边第1行，第4列对应相等，第2行，第4列对应相等，求关节角3。
p
x
c
1
+
p
y
s
1
−
a
y
d
6
s
1
−
a
x
d
6
c
1
+
d
5
o
x
c
1
c
6
+
d
5
n
x
c
1
s
6
+
d
5
o
y
c
6
s
1
+
d
5
n
y
s
1
s
6
=
a
3
c
23
+
a
2
c
2
p
z
−
d
1
−
a
z
d
6
+
d
5
o
z
c
6
+
d
5
n
z
s
6
=
a
3
s
23
+
a
2
s
2
p_xc_1+p_ys_1-a_yd_6s_1-a_xd_6c_1+d_5o_xc_1c_6+d_5n_xc_1s_6+d_5o_yc_6s_1+d_5n_ys_1s_6=a_3c_{23} + a_2c_2\\ p_z-d_1-a_zd_6+d_5o_zc_6+d_5n_zs_6=a_3s_{23} + a_2s_2
pxc1+pys1−ayd6s1−axd6c1+d5oxc1c6+d5nxc1s6+d5oyc6s1+d5nys1s6=a3c23+a2c2pz−d1−azd6+d5ozc6+d5nzs6=a3s23+a2s2
令
m
3
=
p
x
c
1
+
p
y
s
1
−
a
y
d
6
s
1
−
a
x
d
6
c
1
+
d
5
o
x
c
1
c
6
+
d
5
n
x
c
1
s
6
+
d
5
o
y
c
6
s
1
+
d
5
n
y
s
1
s
6
n
3
=
p
z
−
d
1
−
a
z
d
6
+
d
5
o
z
c
6
+
d
5
n
z
s
6
m_3=p_xc_1+p_ys_1-a_yd_6s_1-a_xd_6c_1+d_5o_xc_1c_6+d_5n_xc_1s_6+d_5o_yc_6s_1+d_5n_ys_1s_6\\ n_3=p_z-d_1-a_zd_6+d_5o_zc_6+d_5n_zs_6
m3=pxc1+pys1−ayd6s1−axd6c1+d5oxc1c6+d5nxc1s6+d5oyc6s1+d5nys1s6n3=pz−d1−azd6+d5ozc6+d5nzs6
则
a
3
c
23
+
a
2
c
2
=
m
3
a
3
s
23
+
a
2
s
2
=
n
3
a_3c_{23} + a_2c_2 = m_3 \\ a_3s_{23} + a_2s_2 = n_3
a3c23+a2c2=m3a3s23+a2s2=n3
两式取平方和
m
3
2
+
n
3
2
=
a
2
2
+
a
3
2
+
2
a
2
a
3
(
s
23
s
2
+
c
23
c
2
)
=
a
2
2
+
a
3
2
+
2
a
2
a
3
c
3
m_3^2+n_3^2=a_2^2+a_3^2+2a_2a_3(s_{23}s_2+c_{23}c_2)=a_2^2+a_3^2+2a_2a_3c_3
m32+n32=a22+a32+2a2a3(s23s2+c23c2)=a22+a32+2a2a3c3
解得
θ 3 = ± a r c c o s ( m 3 2 + n 3 2 − a 2 2 − a 3 2 2 a 2 a 3 ) ，其中 m 3 2 + n 3 2 ≤ ( a 2 + a 3 ) 2 \theta_3=\pm arccos(\frac{m_3^2+n_3^2-a_2^2-a_3^2}{2a_2a_3})， 其中m_3^2+n_3^2\le(a_2+a_3)^2 θ3=±arccos(2a2a3m32+n32−a22−a32)，其中m32+n32≤(a2+a3)2
至此共有8组解
求解关节角2，m_3和n_3展开得
c
2
(
a
2
+
a
3
c
3
)
−
a
3
s
3
s
2
=
m
3
s
2
(
a
2
+
a
3
c
3
)
+
a
3
s
3
c
2
=
n
3
c_2(a_2+a_3c_3)-a_3s_3s_2 = m_3 \\ s_2(a_2+a_3c_3)+a_3s_3c_2=n_3
c2(a2+a3c3)−a3s3s2=m3s2(a2+a3c3)+a3s3c2=n3
解得##
s
2
=
(
a
2
+
a
3
c
3
)
n
3
−
a
3
s
3
m
3
a
2
2
+
a
3
2
+
2
a
2
a
3
c
3
,
c
2
=
(
a
2
+
a
3
c
3
)
m
3
+
a
3
s
3
n
3
a
2
2
+
a
3
2
+
2
a
2
a
3
c
3
s_2=\frac{(a_2+a_3c_3)n_3-a_3s_3m_3}{a_2^2+a_3^2+2a_2a_3c_3}, c_2=\frac{(a_2+a_3c_3)m_3+a_3s_3n_3}{a_2^2+a_3^2+2a_2a_3c_3}
s2=a22+a32+2a2a3c3(a2+a3c3)n3−a3s3m3,c2=a22+a32+2a2a3c3(a2+a3c3)m3+a3s3n3
θ 2 = a t a n 2 ( s 2 , c 2 ) \theta_2 = atan2(s_2,c_2) θ2=atan2(s2,c2)
至此共有8组解
求解关节角4，根据
1
T
5
^1T_5
1T5等式展开式有
−
s
234
=
s
6
(
n
x
c
1
+
n
y
s
1
)
+
c
6
(
o
x
c
1
+
o
y
s
1
)
c
234
=
o
z
c
6
+
n
z
s
6
-s_{234}=s_6(n_xc_1+n_ys_1)+c_6(o_xc_1+o_ys_1) \\ c_{234}=o_zc_6+ n_zs_6
−s234=s6(nxc1+nys1)+c6(oxc1+oys1)c234=ozc6+nzs6
则
θ 4 = a t a n 2 ( − s 6 ( n x c 1 + n y s 1 ) − c 6 ( o x c 1 + o y s 1 ) , o z c 6 + n z s 6 ) − θ 2 − θ 3 \theta_4 = atan2(-s_6(n_xc_1+n_ys_1)-c_6(o_xc_1+o_ys_1), o_zc_6+ n_zs_6)-\theta_2-\theta_3 θ4=atan2(−s6(nxc1+nys1)−c6(oxc1+oys1),ozc6+nzs6)−θ2−θ3
至此共8组解
总结
本文记录了UR机器人的运动学求解过程，运用C++和Matlab进行求解。
扫一扫
1577
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
