機器人逆運動學求解（三軸相交）

腕部三根軸線相交的機器人，其末端執行器的位置和姿态可轉換為腕部關節中心點的位置和末端執行器的姿态分别求解，其運作結果如下;

[J,T] = TX90_jacobian([90 0 90 45 30 60])

T =

   -.    .   -.  -
   -.    .    .  
    0.3062    0.8839   -0.3536  389.6447
         0         0         0    1.0000


theta_1 =

[ 90.0, 90.0, -90.0, 22.21, 110.7, 109.1]

T =

   -.    .   -.  -
   -.    .    .  
    0.3061    0.8839   -0.3535  389.6525
         0         0         0    1.0000


theta_2 =

[ 90.0, 90.0, -90.0, -157.8, -110.7, -70.89]

T =

   -.    .   -.  -
   -.    .    .  
    0.3062    0.8839   -0.3535  389.6525
         0         0         0    1.0000


theta_3 =

[ 90.0, 0.0002703, 90.0, 45.0, 30.0, 60.0]


theta_4 =

[ 90.0, 0.0002703, 90.0, -135.0, -30.0, -120.0]


theta_5 =

[ -77.98, -13.63, -74.75, -156.4, 24.45, 83.81]

T =

   -.    .   -.  -
   -.    .    .  
    0.3067    0.8838   -0.3534  389.7066
         0         0         0    1.0000

theta_6 =

[ -77.98, -13.63, -74.75, 23.57, -24.45, -96.19]

T =

   -.    .   -.  -
   -.    .    .  
    0.3062    0.8839   -0.3535  389.6979
         0         0         0    1.0000

theta_7 =

[ -77.98, -88.38, 74.75, -170.4, 97.28, 106.7]

T =

   -.    .   -.  -
   -.    .    .  
    0.3062    0.8838   -0.3536  389.6833
         0         0         0    1.0000

theta_8 =

[ -77.98, -88.38, 74.75, 9.607, -97.28, -73.3]

T =

   -.    .   -.  -
   -.    .    .  
    0.3062    0.8838   -0.3536  389.6838
         0         0         0    1.0000
           

上述結果表明，用逆運動學可得出8組逆解，逆解算出關節角求出的的末端姿态與正運動學求出的末端姿态基本一緻。注：theta5為零時，機器人處于奇異位型，此時theta4與theta6有無數組解，盡量避免theta5為零

推導過程如下：

注：此處theta為與初始角的夾角，具體模組化和正運動學過程