Robotyzacja.com

Rozwiązanie analityczne manipulatora stanfordzkiego

Schemat kinematyczny manipulatora stanfordzkiego :



Stopień ruchliwości manipulatora stanfordzkiego
    N = 7
    p₅ = 6
    ω = 5*6 = 30
    R = 36 – 30 = 6

Stopień manewrowości manipulatora stanfordzkiego
    M = 6(7 – 2) – 5*6 = 0

Przydział układu współrzędnych do manipulatora stanfordzkiego zgodnie z notacją Denavita – Hartenberga

   

T₆ –układ związany z nadgarstkiem

T = T₆TOOL
T₆ = TTOOL^-1

Manipulator posiada nadgarstek sferyczny
T₆ = (A₁A₂A₃)(A₄A₅A₆)
T₆ = T_ARMT_WRIST
Zauważamy właściwość geometryczną :
p₃ = p₆ – d₆z₆

A₁A₂A₃ = T₃
³T₆ = T_WRIST
p₃ = p₆ – d₆z₆
p₃² = d₂² + d₃²

d₃c₂ = ( p₆ – d₆z₆)_Z


Stosując wzory Cramera

Θ₁ = arctg2( s₁, c₁ )
³T₆ = A₄A₅A₆


Przyrównuję elementy a₃₃
(³a₆)_Z = c₅
Θ₅ =±arccos((³a₆)_Z )
Θ₄ = arctg2(s₄, c₄)
Θ₆ = arctg2(s₆, c₆)
Jeżeli s₅ = 0 to arbitralnie ustalamy jeden z kątów Θ₄,Θ₆ przyjmując drugi za zero.
Dla Θ₅ = 0