L1=Femur=AB

L2=Tibia=BP

L3=Coxa=OA

α = góc Coxa
β = góc Femur
γ = góc Tibia
Mục tiêu: biết tọa độ của P(x, y, z) tìm α, β, γ

1. L= √ x 2+ y 2 B
y
2. ∝=arctan
x L1
β1
γ
2
3. z= ( L−L3 ) + y 2
√ L2
L−L3 β
4. z=arctan
y O
A2
2 2 2 A
5. L2=L1 + A P −2 L1 . z . cos A2 A1
L3
L21 + z 2−L22
⟹ A2=arccos ⁡
2. L1 . z P(x, y ,z)

6. β= A1 + A 2−90 ° L3

2 2 2
L + L −z
1 2
7. B1=arccos
2 L1 . L2
8. γ =90° −B 1

O tâm robot y2 y1

P (end point) Offset x

P1(x1, y1)

Tọa độ P so với O1: P1 (x1, y1)

Tọa độ P so với O2:
O1
x2 = x1 + offset x x1
y2 = y2 + offset y
Offset y

⟹Khoảng cách giữa end point với tâm robot: θ

O2 x2
D= √ x 22 + y 22