3A2 SY0004/14/0000-0215 © 2014 SICE ๏ʹΑΔ༏ઌ͖ٯӡಈֶͷϩόετղ๏ ਿ ݪಓ ∗1 Robust Solution of Prioritized Inverse Kinematics Based On Multiplier Method Tomomichi Sugihara∗1 ∗1 Department of Adaptive Machine Systems, Graduate School of Engineering, Osaka University 2-1 Yamadaoka, Suita, Osaka 565-0871, Japan A robust numerical solution of prioritized inverse kinematics is proposed. It is based on the augmented Lagrangian function and the multiplier method, which only requires an increment of residual with respect to the highly-prioritized constraint at each step of iteration of an ordinary inverse kinematics with weights. Hence, it is preferable because of its easy implementation to the conventional method which utilizes kernel space of the robot configuration and thus requires complex matrix computations. More than that, the proposed method is robust even in cases where the highly-prioritized constraint is unsatisfiable. Key Words : Prioritized Inverse kinematics, Numerical solution, Multiplier method 1. ͡ Ί ʹ ૾ۭؒʹ͓͍ͯຬͨ͢ʣͱ͍͏ߟ͑ํͰ͋Δɽ ٯӡಈֶɼ࡞͍͓ͯʹۭؒۀϩϘοτʹཁ͞ٻΕ ΔӡಈɼతʹޮՌثͷҐஔɾ͔࢟ΒɼϩϘοτ ͜ͷߟ͑ํʹɼ࣍ͷ 3 ͭͷ͕͋Δɽ I) Ұൠతͳٯӡಈֶͱಉ༷ʹɼߴ༏ઌ߆ଋ݅Λ ຬ͢Δղͷଘࡏอূ͞Εͳ͍ɽ·ͨɼͦͷΑ ͷؔઅมҐΛ͢ࢉٯΔ͋ͰࢉܭΔɽӡಈͷཁٻɼ ͏ͳղ͕ଘࡏ͢Δ͔൱͔Λࣄલʹผ͢Δ͜ͱ ϩϘοτ͕࣮͖ͰݱΔӡಈͱແؔʹ͍͔Α͏ʹ༩ (8) ༰қͰͳ͍ɽਿࢦ͕ ݪఠͨ͠Α͏ʹɼํఔ͕ࣜ ͑ΒΕಘΔͷͰɼͲͷΑ͏ͳؔઅมҐͷΛͬͯ͠ ෆՄղͳ߹ɼ࠷ࠩޡখղඞͣಛҟʹ͔͏ ͯɼͦΕΒͷཁٻΛશͯಉ࣌ʹຬͨ͢͜ͱ͕ෆՄೳ ͷͰɼ࠷ѱͷ߹͕ࢉܭഁ͢Δɽ͜ΕϩϘο ͳ߹͕͋ΔɽͦͷΑ͏ͳ߹ʹɼ࡞ۀతཧత τͷӡ༻ʹ͓͍ͯɼղͷີݫੑΛࣦ͏ΑΓݥة ੍ʹ͍ͯͮجɼཁٻΛʹີݫຬͨ͢͜ͱ͕ٻΊΒΕ Δͷʢߴ༏ઌ߆ଋ݅ʣͱͦ͏Ͱͳ͍ͷʢ༏ ઌ߆ଋ݅ʣͱʹྨ͠ɼऀޙྗඪͱͯ͠ͷ Ͱ͋Δɽ II) ۭ֩ؒΛར༻ͨ͠࠷ࠩޡখԽͷࢉܭɼߦྻಉ࢜ ͷੵྻߦٯΛͭزٻΊͳ͚ΕͳΒͳ͍ॏ͍ Έ༩͑ࠩޡΛ࠷খԽ͢Δ͜ͱͰղΛٻΊΔํ๏͕͋Δɽ ͋ͰࢉܭΔɽ (1) ͜Ε༏ઌ͖ٯӡಈֶ ͱݺΕΔɽ ٯӡಈֶෳࡶͳඇઢܗ࿈ཱํఔࣜͷٻղͰ͋ (2)∼(5) Γɼಛघͳߏػ III) ͦͦղ͖͍ͨͷݩͷඇઢܗ࿈ཱํఔࣜͰ ͋ͬͯɼ෮͚͓ʹࢉܭΔຖճͷࢉܭεςοϓͰ Λআ͚ɼղੳతʹղΛٻΊΔ͜ ͦͷฏ໘ͷ૾ۭؒɾۭ֩ؒΛʹີݫӠʑ͢Δ ͱҰൠతʹෆՄೳͰ͋Δɽ͕ͨͬͯ͠ଟ͘ͷ߹ɼ ͜ͱͦΕ΄ͲॏཁͰͳ͍ɽࢉܭͷෳࡶ͞ʹޮ ඍٯӡಈֶΛҰͭͷج൫ͱ͢Δղ๏ʹཔΔ͜ͱ Ռ͕߹ݟΘͳ͍ɽ ʹͳΔɽ༏ઌ͖ٯӡಈֶɼैདྷͬͺΒඍ (1) (6) ٯӡಈֶͷจ຺Ͱٞ͞Ε͖ͯͨ ɽΑ͘ΒΕͯ ͍ΔΑ͏ʹɼ͋Δ࢟ʹ͓͚ΔޮՌثͷແݶখมҐͱ (7) ؔઅͷແݶখมҐઢ͚ͭͼ݁ͰؔͳܗΒΕΔ ɽ ͜ͷؔʹ͍ͯͮجɼؔઅมҐۭؒʹۭ͕֩ؒଘࡏ͢ Δɼ͢ͳΘͪޮՌثͷӡಈʹӨڹΛ͓Α΅͞ͳ͍ؔઅ มҐͷू߹͕ۭͰͳ͍ͳΒɼ͜ΕΛར༻ͯ͠༏ઌ ߆ଋ݅ͷࠩޡΛ࠷খԽ͢Δʢߴ༏ઌ߆ଋ݅ ຊߘͰɼҰൠతͳ੍͖ࣜ࠷খೋͷղ ๏Ͱ͋Δ๏ (9) (10) ΛԠ༻ͨ͠ɼ༏ઌ͖ٯӡಈ ֶͷ৽ͨͳղ๏ΛఏҊ͢Δɽ͜ΕɼؔઅมҐͱ ಉ࣌ʹͦͷਵมʢLagrange ʣ෮తʹٻΊ (8) Δํ๏Ͱ͋Δɽ࣮࣭తʹਿ͕ ݪఏҊͨ͠ Levenberg- Marquardt ๏ʢLM ๏ʣʹΑΔղ๏ΛΘ͔ͣʹमਖ਼͢ Δ͚ͩͰ࣮Ͱ͖ɼຖճͷࢉܭεςοϓͰۭ֩ؒΛཅ ʹٻΊΔ͜ͱͳ͘ɼߴ༏ઌ߆ଋ݅Λຬ͢Δղ͕ ଘࡏ͢Δ߹ͦΕʹߴʹऩଋ͢ΔɽؔઅมҐ ∗1 େࡕେֶେֶӃֶڀݚՊೳɾػೳֶઐ߈ʢ˟ 5650871 େࡕਧాࢢࢁాٰ 2-1ʣ[email protected] 第 19 回ロボティクスシンポジア（2014 年 3 月 13 日 -14 日・兵庫） － 215 － 1 ࣍ऩଋ͢ΔҰํɼLagrange 1 ࣍ऩଋ͢Δղ๏ ͨͩ͠ɼλ Lagrange Ͱ͋Δɽ͜ͷͱ͖ɼ ʢQP1ʣͷ࠷దղʹ͓͍͕ͯ࣍Γཱͭɽ T ղ͕ଘࡏ͠ͳ͍߹ͷऩଋੑ͕ѱ͍͜ͱ͕ͱͯ͠ ∂L T = −JJ W W W eW − J TS λ = 0 ͍ͬͯΔ͕ɼϩόετʹղΛٻΊΔ͜ͱ͕Ͱ͖Δɽ ∂q ࣮ࡍɼ͔ͭزͷैདྷํ๏ͱൺֱͨ͠ͱ͜Ζɼޭ ∂L T = eS = 0 །Ұ 100%Ͱ͋ͬͨɽ ∂λ Ͱ͋ΔͨΊɼݪཧతʹɼߴ༏ઌ߆ଋ݅Λຬ͢Δ 2. ϨϏϡʔɿ༏ઌ͖ٯӡಈֶ (5) (6) ͨͩ͠ɼ ∂ eW −JJ W ∂q ∂ eS −JJ S ∂q ࡞͍͓ͯʹۭؒۀཁ͞ٻΕΔӡಈ͕ɼෳͷϦϯΫ ͷҐஔ·ͨ࢟ͷඪͰ༩͑ΒΕΔͱ͠Α͏ɽ͢ ͳΘͪؔઅมҐϕΫτϧ q = [q1 q2 · · · qn ]T ∈ Rn ʹର ͠ɼମ্ͷ i ൪ͷؔ৺ͷҐஔͱͦͷඪҐஔΛ (7) (8) ͦΕͧΕ pi (qq) ∈ R3 ͓Αͼ d pi (qq) ∈ R3 ɼi ൪ͷؔ৺ Λ༻͍ͨɽJ W ͓Αͼ J S ͦΕͧΕݩͷରԠ͢Δମ ͓Αͼ d R ہɼ্هͷ࿈ཱํఔࣜΛղ͘͜ͱʹؼண͢Δɽ ϦϯΫͷ࢟ͱͦͷඪ࢟ΛͦΕͧΕ R i (qq) ∈ SO(3) q) ∈ SO(3) ͱఆٛ͢ΔͱɼϩϘοτͷٯӡ i (q ಈֶ࣍ͷΑ͏ͳඇઢํܗఔࣜΛղ͘͜ͱʹؼண͢Δɽ e (qq) = 0 ͨͩ͠ɼҐஔɾ࢟ʹؔ͢ΔࠩϕΫτϧΛ d p − p (qq) ʢҐஔ߆ଋʣ i i e i (qq) ≡ a (d R R (qq)T ) ʢ࢟߆ଋʣ (1) i (3) (10) e Sk = e S (qqk ) ͱͦΕͧΕ͓͍ͨɽ͜ΕΛɼ T T T JW JW ∆qqk k W W eW k kW W J W k J Sk = J Sk O λk e Sk (11) ͱ·ͱΊͯղ͘͜ͱʹΑΓ ∆qqk ͕ಘΒΕΕɼq k+1 = ͓Αͼ d Ri ɼϩϘοτͷӡಈֶత੍ͱແؔ ʹ͍͔Α͏ʹ༩͑ΒΕಘΔͷͰɼํఔࣜʢ1ʣͷՄ ղੑҰൠతʹอূ͞Εͳ͍ɽͦ͜Ͱ e (qq) Λɼʹີݫ 0 ͱ͢Δ͜ͱ͕·ΕΔͷʢߴ༏ઌ߆ଋ݅ʣͱɼ 0 ͱͰ͖ͳ͍·ͰՄೳͳൣғͰ࠷খԽ͢Δ͜ͱ͕ ·ΕΔͷʢ༏ઌ߆ଋ݅ʣͷೋछྨʹྨ͠ɼ લऀΛ e S (qq)ɼऀޙΛ eW (qq) ͱͦΕͧΕ͓͘ͱɼ༏ઌ ͖ٯӡಈֶɼ࣍ͷΑ͏ͳ੍͖ࣜ݅ 2 ࣍ ܭըͱͯ͠ఆࣜԽ͞ΕΔɽ 1 T E ≡ eW W W eW → min. subject to e S = 0 2 J Sk ∆qqk = e Sk (9) ͨͩ͠ɼJ W k = J W (qqk )ɼJ Sk = J S (qqk )ɼeW k = eW (qqk )ɼ τϧΛҙຯ͢Δʣͱఆٛ͠ɼ dp ࣜʢ5ʣ͓Αͼʢ6ʣΛɼ෮͚͓ʹࢉܭΔ k εςοϓ ͷ q = q k ʹ͍ͭͯͦΕͧΕ࣍ͷΑ͏ʹઢܗԽ͢Δɽ (2) R) ∈ R3 Ձ֯࣠ϕΫ ʢҙͷ R ∈ SO(3) ʹର͠ a (R ͱͨ͠ɽ Newton-Raphson ๏Λ͢ͱܗݪΔޯ๏Ͱɼ·ͣ T T JW qk + J TSk λ k = J W k W W J W k ∆q k W W eW k i i T e (qq) ≡ e T1 (qq) e T2 (qq) · · · e TN (qq) ҐஔɾϦϯΫ࢟ʹؔ͢ΔૅجϠίϏߦྻͰ͋Δɽ݁ q k + ∆qqk ͱߋ৽͢Δ͜ͱͰɼ࿈ཱํఔࣜʢ5ʣʢ6ʣͷ ղʹۙऩଋ͢Δ͜ͱ͕ظ͞ΕΔɽλ k ຖճͷܭ ࢉͰ·ٻΔ͜ͱʹҙ͞Ε͍ͨɽ ͜͜ʹ͓͍ͯɼJ Sk ͓Αͼ J W k ͷαΠζ͓Αͼ ϥϯΫʹ͍ͭͯԿอূ͞Εͣɼ͕ͨͬͯࣜ͠ʢ11ʣ ͷࠨลͷߦྻͷਖ਼ଇੑɾਖ਼ఆੑͱอূ͞Εͳ ͍͜ͱͰ͋ΔɽMoore-Penrose ͷྻߦٯʢMP ྻߦٯʣ Λ༻͍Ε͜ΕΛ ∆qqk ʹ͍ͭͯࣜܗతʹղ͘͜ͱͰ ͖Δ͕ɼಛҟۙͰతʹෆ҆ఆԽ͢Δ͜ͱ͕ආ ͚ΒΕͳ͍ɽ༏ઌ͖ٯӡಈֶੜདྷɼෆՄղͳํ (QP1) ఔࣜͷٻղͳͷͰɼͦͷղ͠͠ಛҟۙ ʹू·Δɽ͕ͨͬͯ͜͠ͷํ๏Ͱɼଟ͘ͷ߹෮ ͨͩ͠ɼeW (qq)ɼe S (qq) ΛͦΕͧΕ eW ɼe S ͱུͨ͠هɽ ·ͨ W W ਖ਼ఆॏΈߦྻͰ͋Γɼ௨ৗର֯ߦྻͱ ͕ࢉܭऩଋ͠ͳ͍ɽ ͱ͜ΖͰɼλ ΛٻΊΔ͜ͱͳࣜ͘ʢ11ʣΛղ͘ ͯ͠༩͑Δɽ͜Εʹର͠ɼLagrange ؔΛ࣍ͷΑ͏ʹ ͜ͱՄೳͰ͋ΔɽҎ߱ɼҙͷߦྻ A ʹରͯ͠ A # ఆٛ͢Δɽ Λ A ͷॏΈ͖ MP ͱྻߦٯఆٛ͢Δɽࣜʢ10ʣͷҰ 1 T L ≡ eW W W eW + λ T e S 2 (4) ൠղɼ࣍ͷΑ͏ʹද͞ΕΔɽ ∆qqk = J #Sk e Sk + N Sk y － 216 － (12) ͨͩ͠ɼN S ≡ J #S J S − 1 J S ͷۭ֩ؒجఈߦྻͰ͋Γɼ ͖ͳ͕Βɼ੍ࣜ݅ʹରԠ͢Δଟ༷ମ্ʹ͋ͬͯ N Sk ≡ N S (qqk ) ͱ͓͍ͨɽ·ͨɼy ҙͷ n × 1 ϕΫτ ϧͰ͋Δɽy Λ৽ͨͳઃܭมͱ͢ΔͱɼʢQP1ʣ తؔΛ࠷খԽ͢Δʹۙ͢Δɽ ͷ࠷దղʹ͓͍͕ͯ࣍Γཱͭɽ ∂E T T T = 0 ⇔ J˜ W W W J˜ W y = J˜ W W W (eeW − J W J #S e S ) ∂y (13) ؔΛ༻͍Δɽ ͜ͷํ๏Ͱɼࣜʢ4ʣͷΘΓʹ࣍ͷ֦ு Lagrange 1 L ≡ e TW E e + λ T e S 2 ͨͩ͠ɼ eS , e≡ eW ͨͩ͠ɼJ˜ W = J W N S ͱ͓͍ͨɽ͜ΕΑΓ y= # J˜ W k (eeW k − J W k J #Sk e Sk ) (14) ͱ͢Δɽͨͩ͠ J˜ W k ≡ J˜ W (qqk ) Ͱ͋Δɽ͜ΕΛࣜʢ12ʣ ∆qqk = 1 O WE ≡ O WW (17) ͱ͓͍ͨɽ͜Εɼ2 ࣍ܭըʢQP1ʣΛ࣍ͷΑ͏ ʹมͨ͠ܗͷͱಉٛͰ͋Δɽ ʹೖ͢Εɼ # J #Sk e Sk + N Sk J˜ W k (eeW k − J W k J #Sk e Sk ) (16) 1 1 T E ≡ e TS e S + eW W W eW → min. 2 2 subject to e S = 0 (15) (1) (QP2) ΛಘΔɽ͜ͷํ๏Ֆɾ٢ɾதଜ ʹΑͬͯఏҊ ͞Εͨɽ ্هͷํ๏ɼҰྻߦͯ͠ݟಉ࢜ͷੵ MP ྻߦٯ ΛٻΊΔॏ͍͋ͰࢉܭΔ͜ͱ͕͔Δɽࣜʢ14ʣͷ Α͏ͳ࿈ཱํఔࣜͷ MP ྻߦٯղͷٻղɼҰൠతʹ MP ྻߦٯΛٻΊΔඞཁͳ͘ɼ్தࢉܭΛ ͢Δ͜ͱʹΑͬͯྔࢉܭΛ O(n2 ) ͱͰ͖Δɽ͔ࣜ͠͠ ʢ12ʣʹ͓͚Δ N Sk ͷͰࢉܭɼMP ྻߦٯΛٻΊ ͳ͚ΕͳΒͳ͍ɽ͜Ε ͕ྔࢉܭO(n3 ) ͱͳΔɽ· ͨɼMP ྻߦٯղͷಛҟۙʹ͓͚Δ੬ऑͬ͞ ͨ··Ͱ͋Δɽ͞Βʹ͕ࢉܭෳࡶͰ͋Δ͜ͱɼ࣮ ࣌ʹᘥᙪ͕ࠞೖ͢Δ֬ΛߴΊΔ͜ͱʹͭͳ͕ΔɽҎ ্ͷΑ͏ʹ͜ͷํ๏ɼඍٯӡಈֶʹ͓͚Δಉ ͷղ๏ͱֶͯ͠తʹཧʹ͔ͳ͍ͬͯΔ͕ɼҰൠత ͳٯӡಈֶͷղ๏ͱͯ͠ίετʹ߹ݟΘͳ͍ɽ (11) தଜɾՖ ɼࣜʢ14ʣʹ͓͍ͯ MP ྻߦٯͷ ʢQP2ʣͷ࠷దղʹ͓͍͕ͯ࣍Γཱͭɽ T ∂L T = −JJ W W W eW − J TS (eeS + λ ) = 0 (18) ∂q T ∂L = eS = 0 (19) ∂λ ࣜʢ19ʣʹҙ͢Εɼ͜Εࣜʢ5ʣʢ6ʣͱಉʹ ͳ͍ͬͯΔɼ͢ͳΘͪʢQP2ʣͷ࠷దղݩͷ ʢQP1ʣͷ࠷దղͰ͋Δ͜ͱ͕͔Δɽ͞Βʹࣜ ʢ5ʣͱࣜʢ18ʣΛൺֱ͢Εɼ͋Δ λ ͕༩͑ΒΕͨԼ Ͱ L Λ࠷খԽ͢Δ q ΛٻΊͨͱ͖ɼͦͷͱ͖ͷ e S + λ ࠷దղʹ͓͚Δ Lagrange ʹΑΓ͍͍ۙͮͯΔ͜ ͱ͕ظ͞ΕΔɽৄࡉͳٞݪจΛࢀর͞Ε͍ͨɽ Ҏ্ͷߟ͑ํʹ͖ͮجɼLM ๏Λجຊͱͨ͠߹ͷ ๏ͷߋ৽ଇɼ࣍ͷ௨ΓͰ͋Δɽ −1 T W Nk q k+1 = q k + J Tk W E J k +W J k W E e k ΘΓʹಛҟྻߦٯײʢSR ྻߦٯʣΛ༻͍Δ͜ ͱΛఏҊ͍ͯ͠Δɽࣜʢ12ʣʹ͓͍ͯ SR ྻߦٯΛ༻ λ k+1 = λ k + e Sk ͍Δ͜ͱɼࣜܗతʹՄೳͰ͋Δɽ͜ΕʹΑΓɼಛҟ ۙͰͷෆ҆ఆԽ͖ͰݮΔ͕ɼࢉܭͷෳࡶ͞ͷ J Sk Jk ≡ , JW k ߴ༏ઌ߆ଋ݅Λຬͨؔ͢અมҐ q ͷू߹ɼଟ ༷ମΛܗ͢Δɽࠓ͍ٞͯ͠Δͷɼͦͷଟ༷ମ্ ໘্ʹ߆ଋ͞Εͨ q ͷৼΔ͍Ͱͳ͍͜ͱʹҙ͢ (21) ͨͩ͠ ͔؍ΒґવෆརͰ͋Δɽ ʹ߆ଋ͞Εͨ q ͷৼΔ͍Ͱ͋ͬͯɼଟ༷ମͷฏ (20) e Sk + λ k ek ≡ eW k (22) ͱ͓͍ͨɽ·ͨɼW Nk LM ๏ʹ͓͚ΔݮਰҼࢠͰ ͋Δɽͳ͓ݪจͷํ๏Ͱɼ༩͑ΒΕͨ λk ʹର͢ ͖Ͱ͋Ζ͏ɽ͢ͳΘͪɼฏ໘ͷ૾ۭؒɾۭ֩ؒ Δ L ͷແ੍࠷খԽͱɼλk ͷߋ৽Λަʹޓ෮͢Δ ΛӠʑ͢Δ͜ͱຊ࣭తͰͳ͍ɽ ೋஈ֊࠷దԽΛߦ͍ͬͯΔ͕ɼຊߘͰհ͢Δํ๏Ͱ ɼq k ͱ λ k Λຖεςοϓಉ࣌ʹߋ৽͍ͯ͠Δɽ 3. ๏ͱ LM ๏Λซ༻ͨ͠ϩόετղ๏ (9) (10) (8) ্هͷߋ৽ଇɼLM ๏ʹΑΔॏΈ͖ٯӡಈֶ ʹΑͬͯఏҊ ͷͦΕʹରͯ͠ɼࣜʢ21ʣʹΑΓߴ༏ઌ߆ଋ݅ͷ ͞Εͨɼ੍͖ࣜ݅ 2 ࣍ܭըͷ෮ղ๏Ͱ ࠩޡΛྦྷੵ͠ɼ͜ΕΛ༻͍ͯ e k Λ e k ʹஔ͖͑Δͩ ͋Δɽઃܭมͱ Lagrange Λಉ࣌ʹߋ৽͍ͯ͠ ͚ͷҧ͍Ͱ͋Δɽ͢ͳΘͪॏΈ͖ٯӡಈֶͱɼ࣮ ๏ɼHestenes ͓Αͼ Powell － 217 － 0.15 0.05 Initial pose of each iteration 0.15 0.15 Point 2 Fig. 1 Point 1 Kinematics model of the tested redundant manipulator comprising five links and four spherical joints Fig. 2 Test 1: prioritized IK with solvable constraint, where the target of Point 1 (highly-prioritized constraint) is fixed while that of Point 2 (lowlyprioritized constraint) moves upward as to be out of reach. ίετ͓Αͼຖεςοϓͷࢉܭίετ͕΄ͱΜͲมΘ Βͳ͍ɽ·੍ͨࣜ݅Λຬͨ͢ q ͕ଘࡏ͠ͳ͍ɼ ͢ͳΘͪෆՄղͳ߹Ͱ͋ͬͯɼϩόετʹ e S ͷ Initial pose of each iteration ϊϧϜΛ࠷খԽ͢Δ͜ͱظ͞ΕΔɽͨͩ͠ɼq k ͕ 1 ࣍ऩଋ͢Δͷʹର͠ɼλ k 1 ࣍ऩଋ͢Δ͜ͱ͕ ͱͯ͛͠ڍΒΕΔɽ Point 2 ͳ͓ɼ࢟߆ଋʹؔͯࠩ͠ޡϕΫτϧ֯࣠දݱ Λ༻͍͍ͯΔͨΊɼՃࢉ͢Δ͜ͱదͰͳ͍ɽ Point 1 චऀͷ࣮Ͱɼྫ͑ߴ༏ઌ߆ଋ݅ʹ·ؚΕΔ e i ͕࢟߆ଋͳΒɼ࣍ͷΑ͏ʹࢉܭΛஔ͖ͯ͑ ͍Δɽ ε i = ε (eei ) λ i,k + ε i λ i,k+1 = λ i,k + ε i e i = e (λ i,k ) (23) Fig. 3 (24) Test 2: prioritized IK with unsolvable constraint, where the target of Point 1 (highlyprioritized constraint) moves forward as to be out of reach, while that of Point 2 (lowly- (25) prioritized constraint) is fixed. ͨͩ͠ɼλ i,k ei ʹରԠ͢Δਵมͷ෮ ࢉܭk ε ςοϓʹ͓͚Δɼε (ee) ֯࣠ϕΫτϧ e ΛΦΠ ϥʔύϥϝʔλʢ୯ҐΫΥʔλχΦϯʣʹม͢Δؔ ʢPoint 2 ͱ͢ΔʣΛͰ͖Δ͚ͩҐஔ (0, 0.2, 0.005i) ɼe (ε ) ΦΠϥʔύϥϝʔλΛ֯࣠ϕΫτϧʹม ʹ͚ۙͮΔɽͨͩ͠ i 0 ͔Β 100 ·Ͱͷ ࢼ ݧ2: ਤ 3 ͷ Α ͏ ʹ ɼPoint 2 ͷ ඪ Ґ ஔ Λ ͢ΔؔͰ͋Δɽ 4. ධ (0, 0.2, 0.5) ͱͨ͠··ɼPoint 1 ͷඪҐஔΛ Ձ (0, 0.4 + 0.004i, 0) ͱ͢Δɽͨͩ͠ i 0 ͔Β 100 ·Ͱͷ ਤ 1 ʹࣔ͢Α͏ͳϚχϐϡϨʔλϞσϧΛ༻͍ ͯɼϩόετੑ͓Αͼؒ࣌ࢉܭͷ͔؍ΒఏҊํ๏Λ (8) ධՁͨ͠ɽ͜ͷϞσϧจ͍༻ʹ ݙΒΕͨͷͱಉҰ ͷͷͰɼ5 ͭͷϦϯΫ͕ 4 ͭͷٿ໘ؔઅʹΑͬͯ ঢ়ʹ࿈݁͞Εɼ12 ࣗ༝Λ࣋ͭɽ֤ؔઅؒͷ͞ ͯ͢ 0.15[m]ɼΤϑΣΫλؔઅத৺͔ΒΤϯυϙΠ ϯτ·Ͱͷ͞ 0.05[m] Ͱ͋ΔɽҎԼʹ͓͍ͯɼΤ ϑΣΫλҐஔͱΤϯυϙΠϯτҐஔΛҙຯ͢Δɽ ࣮ࢪͨ͠ࢼݧ࣍ͷ 2 छͰ͋Δɽ ࢼ ݧ1: ਤ 2 ͷΑ͏ʹɼखઌ 1ʢPoint 1 ͱ͢Δʣ ΛҐஔ (0, 0.4, 0) ʹ߆ଋͨ͠··ɼୈ 3 ؔઅத৺ ൺֱͷͨΊʹɼ࣍ͷ 6 छྨͷํ๏ͰࢉܭΛߦͬͨɽ • w = 1.0:ॏΈ͖ٯӡಈֶɼจ(ݙ8)ͷํ๏ɼͨͩ ͠ߴ༏ઌ߆ଋ݅ͷॏΈΛ 1.0 ͱͨ͠ͷ • w = 10.0:ಉ্ɼߴ༏ઌ߆ଋ݅ͷॏΈΛ 10.0 ͱͨ͠ͷ • w = 100.0:ಉ্ɼߴ༏ઌ߆ଋ݅ͷॏΈΛ 100.0 ͱͨ͠ͷ • HYN-MP:༏ઌ͖ٯӡಈֶɼՖɾ٢ɾதଜ (1) ͷํ๏ɼͨͩࣜ͠ʢ14ʣʹ͓͍ͯ SR ྻߦٯΛ － 218 － ༻͍Δ ଋੑ͕ѱԽ͍ͯ͠Δͱ͍ͯͬͯ͠Δɽ • HYN-SR:ಉ্ɼࣜʢ12ʣ͓Αͼࣜʢ14ʣʹ͓͍ͯ SR ྻߦٯΛ༻͍Δ • Proposed:ఏҊํ๏ HYN-MP ͰඞཁͱͳΔ MP ͓ྻߦٯΑͼۭ֩ؒجఈߦ ྻͷํࢉܭ๏ɼʹ͢هɽࢀߟ·ͰʹɼHYN-MP 5. ͓ Θ Γ ʹ ༏ઌ͖ٯӡಈֶͷɼ๏Λར༻ͨ͠৽ͨͳղ ๏Λࣔͨ͠ɽͦͷಛ࣍ͷΑ͏ʹ·ͱΊΒΕΔɽ I) ۭ֩ؒΛར༻ͨ͠ैདྷํ๏͕ɼྻߦٯΛٻΊΔ (12) ʹ͓͍ͯ࠾༻͞Ε͍ͯΔɽ·ͨɼ༏ ॏ͍ࢉܭΛ͍ͬͯͨͷʹର͠ɼఏҊํ๏ɼॏ ઌ߆ଋ݅ͷॏΈશͯ 1.0 ͱͨ͠ɽॳظຖճ Έ͖ٯӡಈֶʹߴ༏ઌ߆ଋ݅ͷͱੵྦྷࠩޡ Yamane Β Lagrange ਪఆΛՃ͚ͨͩ͠Ͱ͋Γɼ࣮ί q 0 = 0 ʹϦηοτͨ͠ɽ͜Ε͕ಛҟͰ͋Δ͜ͱʹ ҙ͞Ε͍ͨɽ෮ࢉܭͯ͢ɼ࣍ͷ݅ͷ͏͍ͪͣ ετ͓Αͼຖεςοϓͷࢉܭίετ΄ͱΜͲม Ε͔͕ຬͨ͞Εͨ࣌Ͱऴྃͱͨ͠ɽ ΘΒͳ͍ɽ i) ∆qqk ͷͯ͢ͷ͕ ε = ະຬ ii) eek ͷલճ͔ΒͷมԽ͕ δ = 1.0 × 10−12 ະຬ iii) ෮ճ͕ 10,000 ճΛ͑ͨ II) Ұൠతͳٯӡಈֶͱಉ༷ʹ༏ઌٯӡಈֶʹ͓͍ ࢼ ݧ1 ͷ݁ՌΛਤ 4 ʹࣔ͢ɽಉਤ (a) ɼߴ༏ઌ߆ Δ͜ͱ༰қͰͳ͍ɽ͜ͷʹର͠ैདྷํ๏ ଋ݅ͷ͋ͰࠩޡΔɽॏΈ͖ٯӡಈֶղ๏Ͱɼߴ ੬ऑͰ͋Γɼࣦ͠͠ʹࢉܭഊ͍ͯͨ͠ɽҰํ ༏ઌ߆ଋ݅ͷॏΈ͕ 10 ഒɼ100 ഒͱ૿͑Δʹैͬ ͰఏҊํ๏ɼͳͱܕݪΔॏΈ͖ٯӡಈֶ͕ϩ ͯɼࠩޡঃʑʹ͕͍ͯ͘͠ݮɼͲΜͳʹॏΈΛ૿ όετͰ͋Εɼಉ༷ʹϩόετͰ͋Δɽ࣮ࡍɼ ͯ͠ɼ༏ઌ߆ଋ݅ͷඪ͕ΕΔʹैͬ ࢼʹݧΑͬͯఏҊํ๏ͷΈ͕།ҰɼෆՄղͳ߹ ͯࠩޡ૿Ճ͢Δɼ͢ͳΘͪ༩͑ΒΕͨҙͷඪ ͖ͳ͍ͱ͔Δɽ͜ͷ͜ͱ͔Βେલఏͱͯ͠ɼॏΈ ΛؚΉશͯͷ݅Ͱʹࢉܭޭͨ͠ɽ III) ఏҊํ๏ɼؔઅมҐΛٻΊΔࢉܭ 1 ࣍ऩଋ ͢Δ͕ɼLagrange ΛٻΊΔࢉܭ 1 ࣍ऩଋ ͖ٯӡಈֶ͚ͩͰෆेͰ͋Γɼ༏ઌ͖ٯӡಈ Ͱ͋Δɽ͢ͳΘ͍ͪɽ͜ͷͨΊɼঢ়ʹگΑͬͯ ֶͷղ๏ඞཁͰ͋Δͱ͑ݴΔɽఏҊํ๏ɼैདྷͷ τʔλϧࢉܭίετͷͰैདྷํ๏ʹྼΔɽऩ Ֆɾ٢ɾதଜͷํ๏ͱಉ༷ɼߴ༏ઌ߆ଋ݅ͷ ଋੑͷ্͕ࠓޙͷ՝Ͱ͋Δɽ 1.0 × 10−12 ͷʹରࠩ͠ޡΛ࣮֬ʹᮢҎԼʹ͢Δ͜ͱอূͰ ࠩޡΛશͯᮢҎԼʹͰ͖͍ͯΔɽಉਤ (b) ɼ༏ ͯɼߴ༏ઌ߆ଋ݅Λຬ͢ΔղͷଘࡏʢՄ ղੑʣอূ͞Εͣɼ·ͨՄղੑΛࣄલʹผ͢ ࠷ʹޙɼ༏ઌ͖ٯӡಈֶɼӡಈʹଟͷ߆ଋ ઌ߆ଋ݅ͷ͋ͰࠩޡΔɽॏΈ͖ٯӡಈֶͰɼ ݅ɼ͔ͦ͠ͷதʹดϦϯΫߏػ֎քͱͷ৮ ߴ༏ઌ߆ଋ݅ͷີݫੑ͕٘ਜ਼ʹͳΔ෭࣍తޮ༻ͱ ҡ࣋Λҙຯ͢Δ͍͠ݫ͕݅՝ͤΒΕͳ͕Βɼେࣗ ͯ͠ɼͪ͜Β͕͞ݮΕΔ͜ͱ͕͔Δɽ·ͨɼॏΈ ༝Λͯ͠༻׆ෳࡶۀ࡞ͳߴͰڥΛߦ͏Α͏ͳ Λ૿ͤ༏ઌ͖ٯӡಈֶͷ߹ͷʹࠩޡۙ͢Δ ϩϘοτʢྫ͑ਓܕϩϘοτʣʹ͓͍ͯಛʹॏཁͳ ͜ͱ༧͞ΕΔɽ͍ͯͭʹؒ࣌ࢉܭɼಉਤ (c) Α ٕज़ͱߟ͑ΒΕΔɽචऀΒ Γɼ༏ઌ߆ଋ݅ͷ͕ࠩޡখ͍͞͏ͪɼఏҊํ ৳లͷؔઅՄಈݶքΛ͍ͳ͕Βࢧ࣋ྖҬΛϩό ๏ͷํ͕͕ࢉܭ୯७ͳ͍͕ɼ͜Ε͕େ͖͘ͳΔͱɼ ετʹҡ࣋͠ɼͦͷதͰྗΛૢ࡞͠ྗֶత߆ଋ݅ ఏҊํ๏ 1 ࣍ऩଋͰ͋ΔͨΊ෮ճ͕૿͑ɼෆར Λຬͨ͢৳ͼ͔ͳಈ࡞Λઃ͖ͰܭΔΑ͏ͳํࢉܭ ʹͳΔ͜ͱ͕͔Δɽ ๏Λ։ൃ͍ͯ͠Δɽ (13) ఏҊํ๏ΛԠ༻͠ɼක ࢼ ݧ2 ͷ݁ՌΛਤ 5 ʹࣔ͢ɽಉਤ (a)(b) ΑΓɼߴ༏ ँ ࣙ ຊڀݚɼެӹࡒஂ๏ਓךՊֶٕज़ৼڵ ઌ߆ଋ͕݅ಛҟۙͰ༩͑ΒΕΔɼͳ͍࣮͠ݱ ࡒஂॿڀݚۚʢަ൪߸ɿK23 ݚXVI ୈ 355 ߸ʣͷ ෆՄೳʹͳΔͱɼՖɾ٢ɾதଜͷํ๏తʹ ࢧԉΛड͚ͨɽ ෆ҆ఆʹͳΔ͜ͱ͕͔ΔɽHYN-MP ͱ HYN-SR Ͱ ࢀ ߟ จ ݙ ڍಈʹ΄ͱΜͲ͕ࠩͳ͍͜ͱ͔ΒɼࢉܭաఔͰҰ෦ ͷ MP ྻߦٯΛ SR ʹྻߦٯஔ͖͑Δ͚ͩͰղܾ ʹͳΒͳ͍͜ͱ͔ΔɽҰํͰఏҊํ๏།Ұɼશ ͯͷ߹ʹ͓͍ͯʹࢉܭޭ͍ͯ͠Δɽ͢ͳΘͪɼϩ όετੑͷ͔؍ΒఏҊํ๏ͷ༏Ґੑ໌֬Ͱ͋Δɽ ·ͨ͜ͷঢ়͍͓ͯʹگɼ͍ͯͭʹؒ࣌ࢉܭಉਤ (c) ΑΓఏҊํ๏ͷํ͕༏ҐͰ͋Δͱ͑ݴΔɽͨͩ͠ɼऩ (1) Ֆल, ٢߃, தଜਔ. ؔઅܗϩϘοτΞʔϜ ͷੑͷղੳͱͦͷ༏ઌॱҐΛ༗͢Δ࡞ۀͷԠ༻ . ܭଌࣗಈ੍ֶޚձจू, Vol. 19, No. 5, pp. 421–426, 1983. (2) D. L. Pieper. The Kinematics of Manipulators under Computer Control. PhD thesis, Stanford University, 1968. (3) M. Raghavan and B. Roth. Kinematic Analysis of the 6R Manipulator of General Geometry. In Proceedings of The － 219 － 0.01 0.04 w=10.0 Error [m] Error [m] 0.03 w=1.0 0.02 HYN(-MP,-SR) Proposed 0.01 0.2 w=100.0 w=10.0 0.1 w=100.0 0 Computation time [s] HYN(-MP,-SR) Proposed 0.3 0 0.1 0.2 0.3 HYN(-MP,-SR) w=100.0 w=10.0 0.005 w=1.0 Proposed w=1.0 0.4 0 0.5 0 Desired position of point 2 in z-axis [m] 0.1 0.2 0.3 0.4 0 0.5 0 0.1 (a) Error at Point 1 0.2 0.3 0.4 0.5 Desired position of point 2 in z-axis [m] Desired position of point 2 in z-axis [m] (c) Computation time (b) Error at Point 2 Fig. 4 Result of Test 1 0.3 0.6 HYN-SR Proposed HYN-SR HYN-MP 0.1 w=10.0 Error [m] 0.4 Error [m] 0.2 Computation time [s] 0.5 w=1.0 w=100.0 w=100.0 0.3 w=10.0 0.2 0.5 0.6 0.7 0.1 Proposed w=1.0 w=100.0 w=10.0 w=1.0 Proposed 0 0.4 HYN-MP HYN-SR HYN-MP 0.1 0.2 0.8 0 0.4 0.5 0.6 0.7 0.8 Desired position of point 1 in y-axis [m] Desired position of point 1 in y-axis [m] (a) Error at Point 1 (b) Error at Point 2 0 0.4 0.5 0.6 0.7 0.8 Desired position of point 1 in y-axis [m] (c) Computation time Fig. 5 Result of Test 2 Fifth International Symposium on Robotics Research, pp. 263–269, 1991. (4) M. Raghavan and B. Roth. Solving Polynomial Systems for the Kinematic Analysis and Synthesis of Mechanisms and Robot Manipulators. Transaction of the ASME, Special 50th Anniversary Design Issue, Vol. 117, pp. 71– 79, 1995. (12) K. Yamane and Y. Nakamura. Natural Motion Animation through Constraining and Deconstraining at will. IEEE Transactions on Visualization and Computer Graphics, Vol. 9, No. 3, pp. 352–360, 2003. (13) ాத݈, ਿݪಓ. ӡಈֶత੍ͷݶքͰྗֶత੍ Λຬͨ͢ਓܕϩϘοτͷӡಈઃܭ. ୈ 19 ճϩϘςΟ ΫεγϯϙδΞ༧ߘूʢ࠾ܾఆʣ, 2014. (5) S. Sasaki. Feasibility Studies of Kinematic Problems in the Case of a Class of Redundant Manipulators. Robotica, Vol. 13, pp. 233–241, 1995. (6) A. A. Maciejewski and C. A. Klein. Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments. International Journal of Robotic Research, Vol. 4, No. 3, pp. 109–117, 1985. (7) D. E. Whitney. Resolved Motion Rate Control of Manipulators and Human Prostheses. IEEE Transactions on Man-Machine Systems, Vol. 10, No. 2, pp. 47–53, 1969. :LQ ղʹΑΔ MP ͓ྻߦٯΑͼۭ֩ؒ جఈߦྻͷࢉܭ ߦྻ A Λ࣍ͷΑ͏ʹ LQ ղ͢Δɽ A = LQ (26) ͨͩ͠ɼL ྻϑϧϥϯΫɼQ ߦϑϧϥϯΫͰ͋Δ ͜ͱ͕อূ͞ΕΔɽ·ͨ Q ਖ਼نަߦྻͰ͋ΔɽA ͷ MP ྻߦٯʢਖ਼֬ʹॏΈ͖ MP ྻߦٯʣA # ɼ (8) T. Sugihara. Solvability-Unconcerned Inverse Kinematics by the Levenberg-Marquardt Method. IEEE Transaction on Robotics, Vol. 27, No. 5, pp. 984–991, 2011. ࣍ͷΑ͏ʹ·ٻΔɽ (9) M. R. Hestenes. Multiplier and Gradient Methods. Journal of Optimization Theory and Applications, Vol. 4, pp. 303–320, 1969. ͨͩ͠ɼW ਖ਼ఆॏΈߦྻͰ͋Δɽ·ͨɼۭ֩ؒج (10) M. J. D. Powell. A Method for Nonlinear Constraints in Minimization Problems. In R. Fletcher, editor, Optimization, pp. 283–298. Academic Press, 1969. LTW L )−1 L TW L Q − 1 N = A # A − 1 = Q T (L (11) தଜਔ, Ֆल. ؔઅܕϩϘοτΞʔϜͷಛҟ ײӡಈղ. ܭଌࣗಈ੍ֶޚձจू, Vol. 20, No. 5, pp. 453–459, 1984. LTW L )−1 L TW A # = Q T (L (27) ఈߦྻ N ࣍ͷΑ͏ʹ·ٻΔɽ = QTQ − 1 (28) Q ͕ϑϧϥϯΫͰ͋Εɼਖ਼نަੑΑΓ N = O ͱ ͳΔɽ － 220 －
© Copyright 2025 ExpyDoc
