h0
L31
θ1
θ2
θ3
φ0
手臂座標系初始化
L1
L2
h0
(X0 , Y0 , Z0)
(X1 , Y1 , Z1)
X0＝0
Y0＝0
Z0＝h0＝22.4cm
θ1
θ2
X1＝L1COSθ1COSψ
Y1＝L1COSθ1SINψ
Z1＝L1SINθ1＋h0
第1軸與第2軸關節點座標
θ2
L2
(X1 , Y1 , Z1)
(X2 , Y2 , Z2)
X2＝X1＋L2COS (θ1＋θ2) COSψ
Y2＝Y1 ＋L2COS (θ1＋θ2) SINψ
Z2＝
Z1 ＋L2SIN (θ1＋θ2)
第3軸關節點座標
(X2 , Y2 , Z2)
L30
L31
h1