PUMA 560 机器人构型
PUMA560 六轴机器人构型的判定
一般定义机器人三种构型:
face:朝向前后
eblow:肘部上下
wrist:腕部翻转.
三个变量共 2 *2 *2=8 种姿态.
face:朝向前后
影响因素:
- 一关节所处区间
- 手腕部x坐标值所处区间
例如:
θ 1 = 0 , X w r i s t > = 0 , 则 f a c e = 1 θ 1 = 0 , X w r i s t < 0 , 则 f a c e = 0. 反 之 亦 然 \theta_1=0,X_{wrist} >=0,则face=1\\ \theta_1=0,X_{wrist} <0,则face=0. 反之亦然 θ1=0,Xwrist>=0,则face=1θ1=0,Xwrist<0,则face=0.反之亦然
face:肘部上下
影响因素:
1.计算二三关节共线时的角度:
θ
e
b
l
o
w
\theta_{eblow}
θeblow
2.判断三关节角度与共线角度的关系
例如:
θ
3
>
=
θ
e
b
l
o
w
,
则
e
b
l
o
w
=
1
θ
3
<
θ
e
b
l
o
w
,
则
e
b
l
o
w
=
0
\begin{aligned}&\theta_3 >=\theta_{eblow},则eblow=1\\ &\theta_3<\theta_{eblow},则eblow=0 \end{aligned}
θ3>=θeblow,则eblow=1θ3<θeblow,则eblow=0
wrist:腕部翻转
影响因素:
五关节所处区间
例如:
θ
5
>
=
0
,
则
w
r
i
s
t
=
1
θ
5
<
0
,
则
w
r
i
s
t
=
0
\begin{aligned}&\theta_5 >=0,则wrist=1\\ &\theta_5<0,则wrist=0 \end{aligned}
θ5>=0,则wrist=1θ5<0,则wrist=0
PUMA560 六轴机器人解得选取
按照二维范数距离进行选取
参数:
参
考
关
节
值
,
θ
p
r
e
1
,
θ
p
r
e
2
,
θ
p
r
e
3
,
θ
p
r
e
4
,
θ
p
r
e
5
,
θ
p
r
e
6
,
机
器
人
末
端
位
姿
:
c
a
r
t
P
o
s
=
M
a
t
r
i
x
(
4
,
4
)
\begin{aligned} &参考关节值,\theta_{pre1},\theta_{pre2},\theta_{pre3},\theta_{pre4},\theta_{pre5},\theta_{pre6},\\ &机器人末端位姿:cartPos = Matrix(4,4) \end{aligned}
参考关节值,θpre1,θpre2,θpre3,θpre4,θpre5,θpre6,机器人末端位姿:cartPos=Matrix(4,4)
按照本博客中讲解的方法,可以计算出8组关节值:
θ
1
,
θ
2
,
θ
3
,
θ
4
,
θ
5
,
θ
6
,
\begin{aligned} &\theta_{1},\theta_{2},\theta_{3},\theta_{4},\theta_{5},\theta_{6},\\ \end{aligned}
θ1,θ2,θ3,θ4,θ5,θ6,
计算二维范数:
D
=
(
θ
1
−
θ
p
r
e
1
)
2
+
(
θ
2
−
θ
p
r
e
2
)
2
+
(
θ
3
−
θ
p
r
e
3
)
2
+
(
θ
4
−
θ
p
r
e
4
)
2
+
(
θ
5
−
θ
p
r
e
5
)
2
+
(
θ
6
−
θ
p
r
e
6
)
2
\begin{aligned} D = \sqrt{(\theta_{1}-\theta_{pre1})^2+(\theta_{2}-\theta_{pre2})^2+(\theta_{3}-\theta_{pre3})^2+(\theta_{4}-\theta_{pre4})^2+(\theta_{5}-\theta_{pre5})^2+(\theta_{6}-\theta_{pre6})^2} \end{aligned}
D=(θ1−θpre1)2+(θ2−θpre2)2+(θ3−θpre3)2+(θ4−θpre4)2+(θ5−θpre5)2+(θ6−θpre6)2
取八组解中,计算得到D最小的那组作为最终逆解结果.
按照指定机器人构型进行选取
参数:
参
考
关
节
值
,
θ
p
r
e
1
,
θ
p
r
e
2
,
θ
p
r
e
3
,
θ
p
r
e
4
,
θ
p
r
e
5
,
θ
p
r
e
6
,
机
器
人
末
端
位
姿
:
c
a
r
t
P
o
s
=
M
a
t
r
i
x
(
4
,
4
)
机
器
人
末
端
构
型
:
f
a
c
e
、
e
l
b
o
w
、
w
r
i
s
t
\begin{aligned} &参考关节值,\theta_{pre1},\theta_{pre2},\theta_{pre3},\theta_{pre4},\theta_{pre5},\theta_{pre6},\\ &机器人末端位姿:cartPos = Matrix(4,4)\\ &机器人末端构型:face、elbow、wrist \end{aligned}
参考关节值,θpre1,θpre2,θpre3,θpre4,θpre5,θpre6,机器人末端位姿:cartPos=Matrix(4,4)机器人末端构型:face、elbow、wrist
按照本博客中讲解的方法,可以计算出8组关节值:
θ
1
,
θ
2
,
θ
3
,
θ
4
,
θ
5
,
θ
6
,
\begin{aligned} &\theta_{1},\theta_{2},\theta_{3},\theta_{4},\theta_{5},\theta_{6},\\ \end{aligned}
θ1,θ2,θ3,θ4,θ5,θ6,
由前文介绍的构型判定方法,选取逆解中符合构型参数的那一组,即为正确的逆解.