腕部三根軸線相交的機器人,其末端執行器的位置和姿態可轉換爲腕部關節中心點的位置和末端執行器的姿態分別求解,其運行結果如下;
[J,T] = TX90_jacobian([90 0 90 45 30 60])
T =
-0.9186 0.1768 -0.3536 -85.3553
-0.2500 0.4330 0.8660 561.6025
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.9186 0.1767 -0.3536 -85.3600
-0.2501 0.4329 0.8660 561.6038
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.9186 0.1769 -0.3534 -85.3449
-0.2499 0.4329 0.8661 561.6100
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.9184 0.1772 -0.3538 -85.3933
-0.2501 0.4331 0.8660 561.5792
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.9185 0.1768 -0.3536 -85.3739
-0.2501 0.4330 0.8660 561.5836
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.9186 0.1768 -0.3535 -85.3651
-0.2499 0.4331 0.8660 561.5812
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.9185 0.1768 -0.3536 -85.3764
-0.2501 0.4331 0.8660 561.5768
0.3062 0.8838 -0.3536 389.6838
0 0 0 1.0000
上述結果表明,用逆運動學可得出8組逆解,逆解算出關節角求出的的末端姿態與正運動學求出的末端姿態基本一致。注:theta5爲零時,機器人處於奇異位型,此時theta4與theta6有無數組解,儘量避免theta5爲零
推導過程如下:
注:此處theta爲與初始角的夾角,具體建模和正運動學過程