Comments
Description
Transcript
多関節三叉ヘビ型移動ロボットの 解析と制御
多関節三叉ヘビ型移動ロボットの 解析と制御 東京大学 石川将人 [email protected] ２００３年５月２８日 第３回制御部門大会 第３回制御部門大会 – p.1/27 発表概要 1. 背景 2. 1 リンクモデル 可制御構造の解析 回転制御・推進制御 3. 2 リンクモデル 可制御構造の解析 回転制御・推進制御 4. まとめ 第３回制御部門大会 – p.2/27 発表概要 1. 背景 2. 1 リンクモデル 可制御構造の解析 回転制御・推進制御 3. 2 リンクモデル 可制御構造の解析 回転制御・推進制御 4. まとめ 第３回制御部門大会 – p.2/27 ヘビ型移動ロボット 駆動関節 受動車輪 (Nonholonomic constraints) 広瀬ら (1972 –): Active Cord Mechanism Titech COE/Super-Mechano Systems (1997–2001) Serpenoid 曲線に基づく推進制御 (蛇行の原理) 受動車輪が非ホロノミックな拘束を受けるシステム 運動学モデルは 2 入力の対称アフィンシステム 第３回制御部門大会 – p.3/27 可制御性とジェネレータ ヘビ型ロボットの運動学を状態方程式で表すと… 対称アフィンシステム ξ̇ = g1 (ξ)u1 + g2 (ξ)u2 , ξ ∈ Rn , gi (x) : Rn → Rn Ḡ = { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · } dim Ḡ = n となるので可制御 2 ジェネレータ・高次の可制御構造を持つ 構造が複雑になるほど難しい・面白い 第３回制御部門大会 – p.4/27 対称アフィン系の分類 [石川’01 非ホロノミック系の位相構造] 対称アフィン系の分類と典型的な Example は… 1gen.・1 次 : { g1 , g2 , [g1 , g2 ]} ⇒ 車両 1gen.・高次 : { g1 , g2 , [g1 , g2 ]}, [g1 , [g1 , g2 ]], [g1 , [g1 , [g1 , g2 ]]], · · · } ⇒ トレーラ 2gen.・1 次 : { g1 , g2 , g3 , [g1 , g2 ], [g1 , g3 ], [g2 , g3 ]} ⇒ [浦久保ほか] 宇宙ロボット 2gen.・高次 : { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · } ⇒ ヘビ 第３回制御部門大会 – p.5/27 対称アフィン系の分類 [石川’01 非ホロノミック系の位相構造] 対称アフィン系の分類と典型的な Example は… 1gen.・1 次 : { g1 , g2 , [g1 , g2 ]} ⇒ 車両 1gen.・高次 : { g1 , g2 , [g1 , g2 ]}, [g1 , [g1 , g2 ]], [g1 , [g1 , [g1 , g2 ]]], · · · } ⇒ トレーラ 2gen.・1 次 : { g1 , g2 , g3 , [g1 , g2 ], [g1 , g3 ], [g2 , g3 ]} ⇒ 三叉ヘビ 2gen.・高次 : { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · } ⇒ ヘビ 2gen・1 次の構造を持つ移動ロボットを考えていたら思いついた 第３回制御部門大会 – p.5/27 これまでの研究 [岩谷・石川・原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (ある種の正準形) に近似し，時間軸状態 制御形を拡張した切り替えフィードバック制御 第３回制御部門大会 – p.6/27 これまでの研究 [岩谷・石川・原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (ある種の正準形) に近似し，時間軸状態 制御形を拡張した切り替えフィードバック制御 問題点 第３回制御部門大会 – p.6/27 これまでの研究 [岩谷・石川・原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (ある種の正準形) に近似し，時間軸状態 制御形を拡張した切り替えフィードバック制御 問題点 近似の有効範囲が狭い ⇒ 微調整にはよいが移動には向かない 第３回制御部門大会 – p.6/27 これまでの研究 [岩谷・石川・原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (ある種の正準形) に近似し，時間軸状態 制御形を拡張した切り替えフィードバック制御 問題点 近似の有効範囲が狭い ⇒ 微調整にはよいが移動には向かない 移動原理がよく分からない Serpenoid のようなシンプルな移動原理はないのだろうか？ 第３回制御部門大会 – p.6/27 これまでの研究 [岩谷・石川・原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (ある種の正準形) に近似し，時間軸状態 制御形を拡張した切り替えフィードバック制御 問題点 近似の有効範囲が狭い ⇒ 微調整にはよいが移動には向かない 移動原理がよく分からない Serpenoid のようなシンプルな移動原理はないのだろうか？ 特異点に引っ掛かりやすい 第３回制御部門大会 – p.6/27 アウトライン 1. 背景 2. 1 リンクモデル 可制御構造の解析 回転制御・推進制御 3. 2 リンクモデル 可制御構造の解析 回転制御・推進制御 4. まとめ 第３回制御部門大会 – p.7/27 1 リンクの場合（運動学モデル） 各車輪は滑らない NT FRO ベースの配置 w = (x, y, θ0 )T 関節角ベクトル φ Ā(φ)RθT0 ẇ = B̄(φ)φ̇ Ā(φ), Rθ0 は正則, B̄(φ) = I θ0 P (x, y) 1 1 長さは適当に正規化 第３回制御部門大会 – p.8/27 1 リンクの場合（運動学モデル） 各車輪は滑らない NT FRO ベースの配置 w = (x, y, θ0 )T 関節角ベクトル φ Ā(φ)RθT0 ẇ = B̄(φ)φ̇ Ā(φ), Rθ0 は正則, B̄(φ) = I θ0 P (x, y) 1 1 長さは適当に正規化 本来の (物理的な) 入力は φ̇ だが、入力変換を前提として v = RθT0 ẇ を制御入力と考える 先にベースの運動を決め、逆運動学を解いて関節角速度を決定 第３回制御部門大会 – p.8/27 1 リンクの場合（状態方程式） d dt " φ w # = " Ā(φ) Rθ 0 # v 第３回制御部門大会 – p.9/27 1 リンクの場合（状態方程式） Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1 Sφ2 +α2 − Cφ2 +α2 − 1 − Cφ2 " # S − C − 1 − C d φ φ +α φ3 +α3 φ3 = 3 3 v Cθ0 dt w −S 0 θ 0 Cθ0 0 Sθ 0 0 0 1 第３回制御部門大会 – p.9/27 1 リンクの場合（状態方程式） Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1 Sφ2 +α2 − Cφ2 +α2 − 1 − Cφ2 " # S − C − 1 − C d φ φ +α φ3 +α3 φ3 = 3 3 v Cθ0 dt w −S 0 θ 0 Cθ0 0 Sθ 0 0 0 1 ξ̇ = g1 (ξ)v1 + g2 (ξ)v2 + g3 (ξ)v3 第３回制御部門大会 – p.9/27 1 リンク: 可制御構造の解析 Ḡ = span{g1 , g2 , g3 , [g1 , g2 ], [g2 , g3 ], [g3 , g1 ]} = R6 (可制御) Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1 1 Sα1 + Sφ1 +α1 −Cα1 − Cφ1 +α1 S φ2 +α2 − Cφ2 +α2 − 1 − Cφ2 S φ3 +α3 − Cφ3 +α3 − 1 − Cφ3 Cθ −Sθ0 0 0 Sθ Cθ 0 0 0 0 0 1 1 Sα2 + Sφ2 +α2 1 Sα3 + Sφ3 +α3 0 Cθ 0 0 Sθ 0 0 0 −Cα2 − Cφ2 +α2 −Cα3 − Cφ3 +α3 −Sθ0 Cθ 0 0 第３回制御部門大会 – p.10/27 1 リンク: 回転制御 θ0 だけを変化させたい。 √ √ 3 1 −2 1 − 3 1 − 2 2 0 −1 −2 1 0 − 2 √ √ 3 1 − 2 1 3 1 2 Ḡ(0) = span 2 1 0 0 0 1 0 0 1 0 0 0 1 0 0 1 0 0 0 第３回制御部門大会 – p.11/27 1 リンク: 回転制御 θ0 だけを変化させたい。 √ √ 3 1 −2 1 − 3 1 − 2 2 0 −1 −2 1 0 − 2 √ √ 3 1 − 2 1 3 1 2 Ḡ(0) = span 2 1 0 0 0 1 0 0 1 0 0 0 1 0 0 1 0 0 0 [g1 , g2 ] + 2g3 = ∆θ0 !! 初めに [g1 , g2 ] の方向に動かし、次いで g3 の方向に動かせば良い。 第３回制御部門大会 – p.11/27 ホロノミー Lie 括弧積 [g1 , g2 ] の効果 −Aω sin ωt v(t) = Aω cos ωt 0 第３回制御部門大会 – p.12/27 ホロノミー Lie 括弧積 [g1 , g2 ] の効果 −Aω sin ωt v(t) = Aω cos ωt 0 第３回制御部門大会 – p.12/27 1 リンク: 回転制御の手順 h i 1. 入力 v(t) = −Aω sin ωt, Aω cos ωt, 0 を何周期か加える。 Step 1 h 2. 入力 v(t) = 0 0 1 iT を加えて関節角 φ を 0 まで戻す。 戻す Step 2 第３回制御部門大会 – p.13/27 1 リンク: 回転制御 (simulation) Powered by MATX & Gtk+ 第３回制御部門大会 – p.14/27 1 リンク: 推進制御 x 座標だけを変化させたい。 √ √ 3 1 − 2 −2 1 − 3 1 2 0 −1 −2 1 0 − 2 √ √ 3 1 − 2 1 3 1 2 Ḡ(0) = span 2 1 0 0 0 1 0 0 1 0 0 0 1 0 0 1 0 0 0 第３回制御部門大会 – p.15/27 1 リンク: 推進制御 x 座標だけを変化させたい。 √ √ 3 1 − 2 −2 1 − 3 1 2 0 −1 −2 1 0 − 2 √ √ 3 1 − 2 1 3 1 2 Ḡ(0) = span 2 1 0 0 0 1 0 0 1 0 0 0 1 0 0 1 0 0 0 −[g2 , g3 ] + 2g1 = ∆x !! -[g2 , g3 ] の方向に動かし、次いで g1 の方向に動かせば良い。 第３回制御部門大会 – p.15/27 1 リンク: 推進制御の手順 h i 1. 入力 v(t) = 0, −Aω sin ωt, Aω cos ωt を何周期か加える。 Step 1 h 2. 入力 v(t) = 1 0 0 iT を加えて関節角 φ を 0 まで戻す。 戻す 戻す Step 2 第３回制御部門大会 – p.16/27 1 リンク: 推進制御 (simulation) 第３回制御部門大会 – p.17/27 これまでの研究 [岩谷，石川，原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (双線形) に近似し，時間軸状態制御系を 適用した切り替えフィードバック制御 問題点 近似の有効範囲が狭い ⇒ 微調整にはよいが移動には向かない 移動原理がよく分からない Serpenoid のようなシンプルな移動原理はないのだろうか？ 第３回制御部門大会 – p.18/27 これまでの研究 [岩谷，石川，原 理論シンポ’01] 1 リンクの三叉ヘビのモデルを提案 First-order system (双線形) に近似し，時間軸状態制御系を 適用した切り替えフィードバック制御 問題点 近似の有効範囲が狭い ⇒ 微調整にはよいが移動には向かない 移動原理がよく分からない Serpenoid のようなシンプルな移動原理はないのだろうか？ 特異点に引っ掛かりやすい 第３回制御部門大会 – p.18/27 アウトライン 1. 背景 2. 1 リンクモデル 可制御構造の解析 回転制御・推進制御 3. 2 リンクモデル 可制御構造の解析 回転制御・推進制御 4. まとめ 第３回制御部門大会 – p.19/27 2 リンクの場合（運動学モデル） φ22 Ā(φ)RθT0 ẇ = B̄(φ)φ̇ w ∈ R3 , φ32 φ31 φ21 φ ∈ R6 φ12 φ11 B̄(φ), Rθ0 は正則, Ā(φ) は列フルランク 各関節の角速度には拘束が存在し、実質 3 自由度 v = RθT0 ẇ を制御入力と考える 先にベースの運動を決め、逆運動学を解いて得られる関節角速 度が拘束を満たす入力である 第３回制御部門大会 – p.20/27 2 リンクの場合（状態方程式） d dt " φ w # = " B̄(φ)−1 Ā(φ) Rθ 0 # v 第３回制御部門大会 – p.21/27 2 リンクの場合（状態方程式） 1 S1+α 1 C1+α 1 1 −S 1 − S 1 C + C 1+α 1−2+α 1+α 1−2+α 2 2 S C 1+α 1+α 2 2 2 2 −S1+α − S C + C 1−2+α 1+α 1−2+α d φ 3 3 = S1+α C1+α dt w 3 3 3 3 − S1−2+α C1+α + C1−2+α −S1+α C θ0 −Sθ0 C θ0 Sθ 0 0 0 ξ̇ = g1 (ξ)v1 + g2 (ξ)v2 + g3 (ξ)v3 −1 − C11 + 2 −1 − C1 2 2 C1 + C1−2 3 −1 − C1 v 3 3 C1 + C1−2 0 0 1 C11 1 C1−2 第３回制御部門大会 – p.21/27 2 リンク: 可制御構造の解析 Ḡ = span{g1 , g2 , g3 ,[g1 , g2 ], [g2 , g3 ], [g3 , g1 ], [g1 , [g1 , g2 ]], [g2 , [g1 , g2 ]], [g1 , [g3 , g1 ]]} = R9 第３回制御部門大会 – p.22/27 2 リンク: 可制御構造の解析 Ḡ = span{g1 , g2 , g3 ,[g1 , g2 ], [g2 , g3 ], [g3 , g1 ], [g1 , [g1 , g2 ]], [g2 , [g1 , g2 ]], [g1 , [g3 , g1 ]]} = R9 messy 第３回制御部門大会 – p.22/27 2 リンク: 回転制御 θ0 だけを変化させたい φ∗r = ( π2 , β, π2 , β, π2 , β), (ただし β ; 127◦ ) のとき [g1 , g2 ] − g3 = ∆θ0 !! 1 −0.8 1 −0.8 g3 = 1 , −0.8 0 0 1 1 −0.8 1 −0.8 [g1 , g2 ] = 1 −0.8 0 0 0 [g1 , g2 ] の方向に動かし、次いで -g3 の方向に動かせば良い。 第３回制御部門大会 – p.23/27 2 リンク: 回転制御 (simulation) 第３回制御部門大会 – p.24/27 2 リンク: 推進制御 x だけを変化させたい φ∗t = (0, − 23 π, 0, 0, 0, 23 π) のとき −[g2 , g3 ] + g1 = ∆x !! √ − 3 √ 3 0 0 √ , g1 = 3 √ − 3 2 0 0 √ − 3 √ 3 0 0 √ [g2 , g3 ] = 3 √ − 3 1 0 0 -[g2 , g3 ] の方向に動かし、次いで g1 の方向に動かせば良い。 第３回制御部門大会 – p.25/27 2 リンク: 推進制御 (simulation) 第３回制御部門大会 – p.26/27 まとめ 三叉ヘビ型移動ロボット (1 リンク・2 リンク) について，可制 御性構造を解析し、周期制御入力による回転・推進制御方法を 提案した。特に 1 リンクの場合の Locomotion の原理はほぼ明 らかになった。 今後の展開 動力学モデルへの拡張 実機製作 第３回制御部門大会 – p.27/27