Recommended
Recommended
More Related Content
What's hot
What's hot (20)
Similar to Introduction of Trajopt
Similar to Introduction of Trajopt (20)
Recently uploaded
Recently uploaded (20)
Introduction of Trajopt
- 1. 論文解説 Motion Planning with Sequential Convex Optimization and Convex Collision Checking s. matoba
- 2. 目的 • MoveItを構成する軌道最適化アルゴリズムのひとつであるTrajOptを解説 する • 論文は Motion Planning with Sequential Convex Optimization and Convex Collision Checking John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, Pieter Abbeel International Journal of Robotics Research(2014)
- 3. 要旨 • 軌道最適化問題は，制約付き非凸最適化問題 • TrajOptは，非凸最適化問題を近似した複数の凸最適化問題に分割する逐次 凸最適化手法 • 近似が有効な領域で分割するために信頼領域法を使用 • 制約を扱いやすくするために，制約違反するとペナルティを与えるℓ𝟏ペナル ティを使用 • (連続時間)非干渉制約でのℓ𝟏ペナルティを説明するために，(離散時間)非干 渉制約を説明
- 6. 非凸最適化 • 非凸最適化問題 • 軌道最適化では，タイムステップ数 𝑇 の軌道𝑥":$ = (𝑥", 𝑥%, … , 𝑥$)を最小化 • 不等式制約 𝑔 : 障害物回避，関節の角度制限と関節の角速度制限を含む
- 7. ℓ!ペナルティ • 制約付き最適化問題の制約をペナルティ関数に変換するため，計算しやすい • 最適化が収束すると，ペナルティ関数付き最適化の解は制約付き最適化の解 と一致 • ℓ"ペナルティ：ペナルティ関数を ℓ"ノルムの形で設計 minimize 𝑓 x + ) !"# $!"#$ | max 𝑔! x , 0 | + ) !"# $#$ |ℎ! x | ペナルティ関数
- 9. 逐次凸最適化 • タイムステップ数 𝑇 で分割した，𝑇 個の凸最適化問題を考える • タイムステップ𝑡 ∈ 1, … , 𝑇 の時の，1つの凸最適化問題(2次計画問題)に着 目して解説する • 特殊ユークリッド群 𝑆𝐸(3)上で定義された軌道最適化問題を検討する
- 10. 信頼領域法 - アルゴリズム全体の流れ - True Improve: 実際の目的関数の向上 Model Improve: モデル化した(凸近似)目的関数 の向上 • 実際の目的関数の向上が良い場合 信頼領域に伸び代がある → 信頼区間を拡大 • モデルの向上の方が良い場合 信頼領域が大き過ぎる → 信頼領域を縮小
- 11. 凸最適化問題(2次計画問題) - 変数の定義 - • ロボットの6Dコンフィギュレーション空間: p ∈ ℝ1 : 位置， r ∈ ℝ1 : 姿勢 • 空間の対応関係 ℝ" 𝑠𝑒(3) 𝑆𝐸(3) ∧ ∨ exp log 9 𝑋 ∈ 𝑆𝐸(3) * : ノミナル + : インクリメンタルな増分
- 12. 凸最適化問題(2次計画問題) - 軌道の更新 - • ノミナルな位置姿勢 3 𝑋 をインクリメンタルな ̅ 𝑥 で増分した局所近傍 𝑋 • 軌道の更新 / 逐次2次計画の 𝑖 番目の反復計算: where 𝑖 番目の軌道: , 𝑖 番目のインクリメンタル増分:
- 13. TOC (離散時間)非干渉制約 (連続時間)非干渉制約 逐次凸最適化 非凸最適化 ℓ!ペナルティ化 [NOTE] • ここから非干渉制約の話 • ある時刻 𝑡 ∈ 1, … , 𝑇 の2次計画問題を考える． • ここから，𝑇 は平行移動量を表す(記号の扱いが雑なのは元の論文との対応関係を維持したいため)
- 14. (離散時間)非干渉制約 - 距離の定義 - • 離散時間の干渉: ロボット 𝐴(𝑡), 𝐴(𝑡 + 1) v.s. 障害物 𝐵 • 連続時間の干渉: ロボット 𝐴(𝑡)と𝐴(𝑡 + 1)の間 v.s. 障害物 𝐵 • オブジェクトのラベル: (ロボット)𝐴，(何か)𝐵, (障害物)𝑂 • オブジェクトに含まれる点の集合: (ローカル座標)𝑝A ∈ 𝒜A，(ワールド)𝒜B • 座標変換(ローカル→ワールド): 𝐹A B • 符号付き距離: where 𝑇は平行移動量
- 15. (離散時間)非干渉制約 - 符号付き距離の線形化 - • 最近傍点𝑝A, 𝑝D: (以下の式を最適化すると，法線 > 𝑛 ともども求まる) • 最小な平行移動方向 𝑇 は，法線 > 𝑛 と一致． • 𝐴を𝑇だけ平行移動すると， 𝑝Aと𝑝Dは接する． • ノミナルな位置姿勢𝑥Eに関する距離sd の線形化: • 最近傍点𝑝A, 𝑝Dに関する距離sd の線形化: • ノミナルな位置姿勢𝑥Eに関する距離sd の線形化: ワールド座標系でのA,B間の距離
- 16. (離散時間)非干渉制約 - 非干渉の制約 - • 障害物干渉，自己干渉の制約 𝑑%&'(: TrajOptの近似誤差用の安全マージン
- 18. (離散時間)非干渉制約 - ℓ!ペナルティ化 - • ℓ"ペナルティ化 • ペナルティ関数の単一項 : 𝑑#$%&: TrajOptの近似誤差用のマージン 𝑑'(&'): ユーザー定義の干渉マージン [NOTE] 実装では，処理高速化のために 𝑑'(&')よりも短いsdの場合のみ，ペナルティ計算を実施．
- 20. (連続時間)非干渉制約 - 非干渉の制約 - • 離散時間の干渉: ロボット 𝐴(𝑡), 𝐴(𝑡 + 1) v.s. 障害物 𝐵 • 連続時間の干渉: ロボット 𝐴(𝑡)と𝐴(𝑡 + 1)の間 v.s. 障害物 𝐵 • 𝐴(𝑡)と𝐴(𝑡 + 1) で構成した凸包と障害物𝐵で，非干渉制約 • 回転移動の安全マージン 𝑑GHI = JK! L 𝑟 : 回転軸と⋃*∈[-,!] 𝐴(𝑡)の最大距離 𝜙 : 回転移動量 [NOTE] 経験的に，飛び出し量は1cm以下だったので 実装では，𝑑$0'を無視 𝑇は平行移動量 𝐴(𝑡)と𝐴(𝑡 + 1) で構成した凸包 swept outされた時の過小評価に対する 安全マージン 回転移動の影響で凸包から飛び出てる swept out
- 21. (連続時間)非干渉制約 - サポートマッピング - • サポートマッピング：凸-凸符号付き距離計算を効率的に実行 • サポートマッピングを使ってベクトル v 方向にもっとも遠い凸包上の点を 計算 • この表現により，表面の明示的な多面体表現を考慮せずに，凸形状を暗黙的 に記述することができる． ベクトル v 方向にもっとも遠い 𝒜 上の点 p を計算
- 22. (連続時間)非干渉制約 - 凸包の符号付き距離の線形化 - • 凸包上の最近傍点 pRSTUV : 1. 法線 > n を使って， −> nに沿ってサポートマップすることでpE ∈ 𝒜(𝑡), p" ∈ 𝒜(𝑡 + 1)を見つける 2. 係数𝛼 ∈ [0, 1]で固定した pE，p"の凸結合で，最近傍点 pRSTUV を近似 • 時刻 𝑡 , 𝑡 + 1での位置 x で線形近似したpRSTUV，pD 間の符号付き距離 • 2Dだと勾配近似は正しいが，3Dは経験上問題ないとしか言えてない where, Jacobian:
- 23. Recap • TrajOptは，制約付き非凸最適化問題をℓ"ペナルティ関数を使った逐次凸最 適化問題に置き換えている • ロボット軌道を連続区間に分割し，各区間の始点/終点から凸包を形成 • 凸包と障害物との非干渉制約を符号付き距離で表現 • 符号付き距離 sd を線形化し，サポートマッピングを使って効率的に計算 -- 間違ってるところなどあれば，ご教授ください． よろしくお願いします．
- 24. Appendix
- 25. TrajOpt ‒ アルゴリズムの全体像 - 第1ループ：すべての制約が満たされるまでペナル ティ係数 𝜇 を増加 第2ループ：問題の凸近似を繰り返し作成し，それ らを最適化 第3ループ：信頼領域法で凸近似可能な領域の更新
- 26. 符号付き距離を線形化 • Face-Faceの接触を伴うような退化する場合 符号付き距離は，オブジェクトの位置姿勢 x に関して微分不可能 • 経験的に，このようなケースに出くわしていないが， Future workとしてよりよい局所近似を提案する