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

由前文介绍的构型判定方法，选取逆解中符合构型参数的那一组，即为正确的逆解.