同以IP為初解，比較DLS與SDLS的差異

為了要了解 damped least square 與 selectively damped least square 的差別，因此同以 IP 為初解，分別使用 DLS 與 SDLS 的方法做 IK iteration。

實驗結果如下：

Selective Damped Least Square：

Initial Solution 時各關節角度：

Lheel x-rot -1.54662 Lshouder x-rot 129.336 Lshouder y-rot 13.0242 Lshouder z-rot 8.68211 Lelbow -rot 0.0223306 Rshouder x-rot 88.026 Rshouder y-rot 17.2408 Rshouder z-rot 15.5562 Relbow -rot 4.60507

LeftWrist Position Error = 2.37451 Head Position Error = 5.74403 RightWrist Position Error = 2.81116 RightHeel Position Error = 8.30422

Damped Least Square：

Initial Solution 時各關節角度：

Lheel x-rot -1.54662 Lshouder x-rot 129.336 Lshouder y-rot 13.0242 Lshouder z-rot 8.68211 Lelbow -rot 0.0223306 Rshouder x-rot 88.026 Rshouder y-rot 17.2408 Rshouder z-rot 15.5562 Relbow -rot 4.60507

LeftWrist Position Error = 2.37451 Head Position Error = 5.74403 RightWrist Position Error = 2.81116 RightHeel Position Error = 8.30422

Iteration No.1 Lshouder x-rot -0.427835 Lshouder y-rot -0.0507532 Lshouder z-rot -0.479033 Lelbow -rot 3.10601e-018 Rshouder x-rot 0.314963 Rshouder y-rot 0.0158025 Rshouder z-rot 0.569135 Relbow -rot 0.0967705 Rhip x-rot -0.123619 Rhip y-rot 0.00499919 Rhip z-rot -0.827953 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.75829 Head Position Error = 2.37155 RightWrist Position Error = 2.21502 RightHeel Position Error = 2.13941 Total Position Error = 9.48426

Iteration No.1 Lshouder x-rot -0.532279 Lshouder y-rot -0.0812514 Lshouder z-rot -0.0612277 Lelbow -rot 0

Rshouder x-rot 0.144434 Rshouder y-rot 0.00374821 Rshouder z-rot 0.299319 Relbow -rot 0.0621731 Rhip x-rot 0.139752 Rhip y-rot 0.0630137 Rhip z-rot -2.39379 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.38809 Head Position Error = 2.20685 RightWrist Position Error = 2.14207 RightHeel Position Error = 3.59462 Total Position Error = 10.3316

Iteration No.2 Lshouder x-rot -0.537025 Lshouder y-rot -0.0718573 Lshouder z-rot -0.258374 Lelbow -rot 2.48481e-017 Rshouder x-rot 0.0835114 Rshouder y-rot -0.0141025 Rshouder z-rot 0.294645 Relbow -rot 0.01897 Rhip x-rot -0.034664 Rhip y-rot 0.00210709 Rhip z-rot -0.454104 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.60359 Head Position Error = 1.88747 RightWrist Position Error = 2.14104 RightHeel Position Error = 1.07848

Iteration No.2 Lshouder x-rot -0.528455 Lshouder y-rot -0.0762212 Lshouder z-rot -0.170826 Lelbow -rot 0

Rshouder x-rot 0.046264 Rshouder y-rot 0.0162505 Rshouder z-rot -0.0190939 Relbow -rot 0.0611072 Rhip x-rot 0.134836 Rhip y-rot 0.0110969 Rhip z-rot -0.984978 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.48343 Head Position Error = 1.84696 RightWrist Position Error = 2.05734 RightHeel Position Error = 1.41781

Iteration No.3 Lshouder x-rot -0.547212 Lshouder y-rot -0.0761672 Lshouder z-rot -0.103018 Lelbow -rot 0

Rshouder x-rot -0.0276229 Rshouder y-rot -0.0123013 Rshouder z-rot 0.0322107 Relbow -rot 0.000612739 Rhip x-rot 0.0225875 Rhip y-rot 0.00156348 Rhip z-rot -0.405163 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.33823 Head Position Error = 1.6847 RightWrist Position Error = 2.07555 RightHeel Position Error = 0.822963 Total Position Error = 6.92145

Iteration No.3 Lshouder x-rot -0.474158 Lshouder y-rot -0.0667774 Lshouder z-rot -0.160493 Lelbow -rot 0

Rshouder x-rot -0.0216946 Rshouder y-rot 0.0096029 Rshouder z-rot -0.122448 Relbow -rot 0.0403235 Rhip x-rot 0.116654 Rhip y-rot 0.00168476 Rhip z-rot -0.483852 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.27563 Head Position Error = 1.68083 RightWrist Position Error = 1.96487 RightHeel Position Error = 0.934999 Total Position Error = 6.85633

Iteration No.4 Lshouder x-rot -0.488109 Lshouder y-rot -0.06849 Lshouder z-rot -0.0471322 Lelbow -rot 0

Rshouder x-rot -0.0859496 Rshouder y-rot -0.0131154 Rshouder z-rot -0.0921078 Relbow -rot -0.0109465 Rhip x-rot 0.0586141 Rhip y-rot 0.000470071 Rhip z-rot -0.379437 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.09197 Head Position Error = 1.52975 RightWrist Position Error = 1.9898 RightHeel Position Error = 0.685835

Iteration No.4 Lshouder x-rot -0.407434 Lshouder y-rot -0.0550601 Lshouder z-rot -0.174632 Lelbow -rot 0

Rshouder x-rot -0.05302 Rshouder y-rot 0.00803783 Rshouder z-rot -0.180983 Relbow -rot 0.0312902 Rhip x-rot 0.0943268 Rhip y-rot -0.000583632 Rhip z-rot -0.349017 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 2.05501 Head Position Error = 1.56947 RightWrist Position Error = 1.84836 RightHeel Position Error = 0.715575

Iteration No.5 Lshouder x-rot -0.443478 Lshouder y-rot -0.0614706 Lshouder z-rot -0.0546444 Lelbow -rot 0

Rshouder x-rot -0.105731 Rshouder y-rot -0.013086 Rshouder z-rot -0.13582 Relbow -rot -0.0145444 Rhip x-rot 0.0751613 Rhip y-rot -0.000848261 Rhip z-rot -0.351829 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 1.87381 Head Position Error = 1.40063 RightWrist Position Error = 1.89672 RightHeel Position Error = 0.575107 Total Position Error = 5.74627

Iteration No.5 Lshouder x-rot -0.347644 Lshouder y-rot -0.0448695 Lshouder z-rot -0.183439 Lelbow -rot 0

Rshouder x-rot -0.0613108 Rshouder y-rot 0.00934922 Rshouder z-rot -0.209268 Relbow -rot 0.0296903 Rhip x-rot 0.0785884 Rhip y-rot -0.00159838 Rhip z-rot -0.288466 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 1.86022 Head Position Error = 1.47943 RightWrist Position Error = 1.73228 RightHeel Position Error = 0.573228 Total Position Error = 5.64515

Iteration No.8 Lshouder x-rot -0.323609 Lshouder y-rot -0.0408487 Lshouder z-rot -0.127709 Lelbow -rot 0

Rshouder x-rot -0.0657563 Rshouder y-rot 0.00613432 Rshouder z-rot -0.19391 Relbow -rot 0.0172803 Rhip x-rot 0.0768016 Rhip y-rot -0.00373486 Rhip z-rot -0.271162 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 1.34158 Head Position Error = 1.1538 RightWrist Position Error = 1.62699 RightHeel Position Error = 0.326843

Iteration No.8 Lshouder x-rot -0.218295 Lshouder y-rot -0.0242559 Lshouder z-rot -0.181458 Lelbow -rot 0

Rshouder x-rot -0.0383825 Rshouder y-rot 0.016765 Rshouder z-rot -0.215992 Relbow -rot 0.0386279 Rhip x-rot 0.0553607 Rhip y-rot -0.00282875 Rhip z-rot -0.201816 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 1.41099 Head Position Error = 1.2786 RightWrist Position Error = 1.42631 RightHeel Position Error = 0.328519

Iteration No.13 Lshouder x-rot -0.0948053 Lshouder y-rot -0.00600287 Lshouder z-rot -0.210881 Lelbow -rot 0

Rshouder x-rot 0.0279569 Rshouder y-rot 0.0366744 Rshouder z-rot -0.238374 Relbow -rot 0.0792107 Rhip x-rot 0.0557655 Rhip y-rot -0.00483026 Rhip z-rot -0.172403 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 0.776377 Head Position Error = 0.984377 RightWrist Position Error = 1.18822 RightHeel Position Error = 0.180079 Total Position Error = 3.12905

Iteration No.13 Lshouder x-rot -0.103274 Lshouder y-rot -0.00858616 Lshouder z-rot -0.146695 Lelbow -rot 0

Rshouder x-rot 0.00163217 Rshouder y-rot 0.021179 Rshouder z-rot -0.167972 Relbow -rot 0.0470786 Rhip x-rot 0.0418492 Rhip y-rot -0.00342875 Rhip z-rot -0.139103 Rknee -rot 0

Iteration 後的 Error：

LeftWrist Position Error = 0.920239 Head Position Error = 1.09497 RightWrist Position Error = 1.06348 RightHeel Position Error = 0.18831 Total Position Error = 3.267

Iteration No.17 Lshouder x-rot -0.00558947 Lshouder y-rot 0.0043127 Lshouder z-rot -0.210064 Lelbow -rot 0.0203126 Rshouder x-rot 0.04591 Rshouder y-rot 0.0387597 Rshouder z-rot -0.227476 Relbow -rot 0.0900932

LeftWrist Position Error = 0.432185 Head Position Error = 0.933754 RightWrist Position Error = 0.840133 RightHeel Position Error = 0.147608

Iteration No.17 Lshouder x-rot -0.0546906 Lshouder y-rot -0.00321115 Lshouder z-rot -0.118482 Lelbow -rot 0

Rshouder x-rot 0.0130837 Rshouder y-rot 0.0192942 Rshouder z-rot -0.131688 Relbow -rot 0.0443873

LeftWrist Position Error = 0.654775 Head Position Error = 1.02182 RightWrist Position Error = 0.865641 RightHeel Position Error = 0.155281

修正後：(iteration 25 次) Lshouder x-rot 124.796 Lshouder y-rot 12.4884 Lshouder z-rot 4.36112 Lelbow -rot 0.251014 Rshouder x-rot 88.3514 Rshouder y-rot 17.8342 Rshouder z-rot 11.303 Relbow -rot 6.10711

LeftWrist Position Error = 0.144516 Head Position Error = 0.916711 RightWrist Position Error = 0.293559 RightHeel Position Error = 0.0845181 Total Position Error = 1.4393

修正後：(iteration 17 次) Lshouder x-rot 125.279 Lshouder y-rot 12.5213 Lshouder z-rot 6.056

Lelbow -rot 0.00223581 Rshouder x-rot 87.9161 Rshouder y-rot 17.5131 Rshouder z-rot 13.1468 Relbow -rot 5.34377

LeftWrist Position Error = 0.654775 Head Position Error = 1.02182 RightWrist Position Error = 0.865641 RightHeel Position Error = 0.155281 Total Position Error = 2.69752

頭部位置誤差收斂比較圖

(藍色實線：SDLS Method；紅色虛線：DLS Method)

右手腕位置誤差收斂比較圖

(藍色實線：SDLS Method；紅色虛線：DLS Method)

右腳跟位置誤差收斂比較圖

End-Effectors 總和位置誤差收斂比較圖 (藍色實線：SDLS Method；紅色虛線：DLS Method)

SDLS 的Δθ 收斂情形：

DLS 方法的Δθ 收斂情形：

SDLS 方法的θ 收斂情形：

DLS 方法的θ 收斂情形：

View 1 Data Image

View 2 Data Image

實驗結論：

兩種方法在實驗的結果中，不論是在 iteration 次數上或是 position error 上都差不多，

然而在速度上我們只考慮 iteration 的次數卻沒有考慮單次 iteration 所花的時間；然而 SDLS 的演算法比 DLS 明顯複雜的多，因此在下一節中我們將分析這兩個方法的演算 法，並實際量測時間判斷 DLS 與 SDLS 的執行效率如何。