微分動的計画法の近年の動向:制約付き軌道計画問題へのアプローチ / A Survey of Constrained Differential Dynamics Programming

1b491df018c8b671a4b97ee5a167e85c?s=47 yuki
May 15, 2020

微分動的計画法の近年の動向:制約付き軌道計画問題へのアプローチ / A Survey of Constrained Differential Dynamics Programming

1b491df018c8b671a4b97ee5a167e85c?s=128

yuki

May 15, 2020
Tweet

Transcript

  1. 微分動的計画法の近年の動向: 制約付き軌道計画問題へのアプローチ yuki (@blessingyuki) 第 1 回ロボティクス勉強会 2020 年 5

    月 15 日(金) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  2. 目次 1 はじめに 2 微分動的計画法 3 制約の付加 4 まとめ yuki

    @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  3. 1 はじめに 2 微分動的計画法 3 制約の付加 4 まとめ yuki @

    Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  4. 微分動的計画法 Differential Dynamic Programming (Jacobson & Mayne, 1970) DDP と略される.

    有限時間最適制御問題の数値解法 制御入力の列を求める 最適性の 1 次の必要条件: 評価関数の勾配がゼロとなる解を求める1 二次収束性を有する ロボットの歩行や自動運転の駐車のプランニング, モデル予測制御 (MPC) などで利用されている. 1indirect な方法と呼ばれる. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  5. この資料の目的 微分動的計画法の基礎的な事項についてまとめる 最近の研究動向を紹介する 公開されているソフトウェアを紹介する yuki @ Tokyo Tech SSR A

    Survey of Constrained Differential Dynamics Programming
  6. 1 はじめに 2 微分動的計画法 3 制約の付加 4 まとめ yuki @

    Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  7. ダイナミクスモデル 次の一般的な時間発展の N ステップ離散システムを考える. xi+1 = f(xi , ui )

    (1) i ∈ {0, 1, . . . , N} はインデックス,xi , f ∈ Rn, ui ∈ Rm. 状態軌道を X = {x0 , x1 , . . . , xN } ∈ Rn×(N+1), 入力軌道を U = {u0 , u1 , . . . , uN−1 } ∈ Rn×N と定義. ⇒ 初期状態 x0 と入力列 U から状態軌道 X が決まる. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  8. 評価関数 スカラ関数 J を評価関数とする. J(X0 , U) = N−1 i=0

    l(xi , ui ) + lf (xN ) l(xi , ui ) はステップ毎のコスト,lf (xN ) は終端コスト. 軌道計画問題は最適化問題として書くと U∗ = arg min U J(x0 , U) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  9. 最適性の原理 最適性の原理 (Bellman, 1957) 「最適軌道の部分軌道もまた最適軌道である」 N ステップの最適入力軌道を U∗ = {u∗

    0 , u∗ 1 , . . . , uN−1 } とする. 任意の i ∈ {0, 1, . . . , N} に対して, U∗ の部分軌道 U∗ i = {u∗ i , . . . , u∗ N } は, x0 と {u∗ 0 , . . . , u∗ i−1 } から得られる xi を初期状態とする 部分問題の最適解となる. この原理を用いて,終端を開始点として, 時間を遡る方向に方向に 1 ステップずつに解くのが 動的計画法である. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  10. 動的計画法 i ステップから終端までの部分コスト (cost-to-go) Ji : Ji (x, Ui )

    = N−1 j=i l(xj , uj ) + lf (xN ) i ステップ時点の価値関数 Vi を次のように定義する. Vi (x) = min Ui Ji (x, Ui ) 導かれるベルマン方程式: Vi (xi ) = min ui [l(xi , ui ) + Vi+1 (f(x, u))] 以降では,インデックス i を省略し,Vi+1 = V と書く. V (x) = min u [l(x, u) + V (f(x, u))] (2) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  11. 微分動的計画法の概略 アルゴリズムの概略 1. 最適性の原理に従い, 価値関数 (2) を二次で近似して textcolorred 逆方向パスを計算する. 2.

    ダイナミクス (1) に従って, 順方向パスでダイナミクスを計算. 3. 上記を繰り返す. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  12. 価値関数の二次形式近似 (1/3) 最適な逆方向パスの計算のため, ある点 (x, u) における (2) の右辺の微小変化分 Q

    を考える. Q(δx, δu) = l(x + δx, u + δu) + V (f(x + δx, u + δu) (3) Q は疑似ハミルトニアンと呼ばれる. 二次までの偏微分を計算する. Qx = lx + fT x Vx (4a) Qu = lu + fT u Vx (4b) Qxx = lxx + fT x Vxx fx + Vx · fxx (4c) Qux = lux + fT u Vxx fx + Vx · fux (4d) Quu = luu + fT u Vxx fu + Vx · fuu (4e) (4c)〜(4e) の 2 項目はテンソル積を表す. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  13. 価値関数の二次形式近似 (2/3) 二次形式に近似された Q は次の通り.minδu Q を解きたい. Q 1 2

      1 δx δu   T   0 QT x QT u Qx Qxx QT ux Qu Qux Quu     1 δx δu   (5) 局所的に最適な微小入力 δu∗ は停留点23と見て, フィードフォワード項 k と, 摂動 δx の状態フィードバック項 Kδx の和として求まる4. δu∗(δx) = arg min δu Q(δx, δu) = k + Kδx (6a) k = −Q−1 uu Qu , K = −Q−1 uu Qux (6b) 2二次形式なので凸と仮定している. 3 ∂Q ∂(δu) = 0. 4一般には Quu の正則化が必要 yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  14. 価値関数の二次形式近似 (3/3) 最適微小入力 δu∗ を u ← u + δu

    として適用すると, 現在ステップの価値関数 V が(近似的に)計算できる. V 1 2 δxT(Qxx + KTQuu K − 2Qxu K)δx + δxT(Qx + KTQu ) − 1 2 kTQuu k δx = 0 のときを考えると,次を得る. ∆V = − 1 2 kTQuu k (7a) Vx = Qx + KTQu (7b) Vxx = Qxx + KTQux (7c) 価値関数の初期値は終端コスト. VN = lf (xN ) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  15. 直線探索 逆方向パスが計算されたところで,順方向パスを計算する. 先の計算で求まったのはあくまでも局所的な最適入力で, 大域的な最適性は保証されていないことに注意する. 更新規則は次の通り. ˆ x0 =x0 (8a) ˆ

    ui =ui + αki + Ki (ˆ xi − xi ) (8b) ˆ xi+1 =f(ˆ xi , ˆ ui ) (8c) ここで α は更新パラメータ. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  16. アルゴリズムまとめ 1. 初期入力軌道 U で終端コスト VN を計算 2. 逆方向にステップ毎の局所最適入力 u∗

    i を計算 3. u∗ i を用いて入力列を補正,順方向にモデルを計算 4. 収束するまで 2, 3 を繰り返す(イテレーション) 1 イテレーションの計算量は, 入力次元 m とステップ数 N に対して O(Nm3). DDP はm 次元の問題を N 回解く5. アルゴリズムとしては,Newton-Raphson 法に似ている. Newton-Raphson 法は Nm 次元の問題を解くため, 計算量が O(N3m3). 5m 次元の逆行列の計算量は O(m3) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  17. 1 はじめに 2 微分動的計画法 3 制約の付加 4 まとめ yuki @

    Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  18. 最適化に対する制約 実用上の軌道計画において,制約がないことは稀. 実際の制約の例: 状態制約 関節の可動域(トルク入力,速度入力の場合) 障害物との接触・干渉 入力制約 関節の可動域(位置入力の場合, 逆運動学) トルクや速度の飽和

    これらの問題を考慮しないと, 「最適であっても実現は不可能な軌道」が生成されてしまう. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  19. 制約を考慮した微分動的計画法 (1/2) いろいろ研究されている. 入力に対する矩形制約:b ≤ u ≤ b Y. Tassa,

    et al.: Control-Limited Differential Dynamic Programming, ICRA 2014. 一般不等式制約:g(x, u) ≤ 0 Z. Xie, et al.: Differential Dynamic Programming with Nonlinear Constraints, ICRA 2017. ホロノミック拘束:φ(x) = 0 C. Mastalli, et al.: Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control, ICRA 2020. yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  20. 制約を考慮した微分動的計画法 (2/2) いろいろ研究されているが,方向性は大体同じ. 制約の組み込み方 最適性の原理によって分割された 部分問題それぞれに制約を組み込む. 具体的には次の操作が必要: 1. 逆方向パスにおけるゲイン k,

    K の修正(入力制約) 2. 順方向パスにおける実行可能性の担保(状態制約) yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  21. 逆方向パスへの制約の付与 入力制約: 逆方向パスにおけるゲイン k, K の修正が必要. つまり,次を解く. min δu Q(δx,

    δu) s.t. . . . やること自体は実質制約付き QP なので, 制約に適した方法を使えば良さそう. 矩形制約 Projected-Newton 法が決定的な模様 一般の制約 一般化ラグランジアンと KKT 条件 yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  22. 順方向パスへの制約の付与 (1/2) 状態制約: 順方向パスにおける実行可能性 (feasibility) の担保が必要. 状態制約は難しい! 直接操作できるのは入力 u で,

    その結果生じるのが状態 x xi はその時刻までの入力列 {u0 , . . . , ui−1 } に依存 逆方向パスほど簡単ではない…… yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  23. 順方向パスへの制約の付与 (2/2) 状態制約: 順方向パスにおける実行可能性 (feasibility) の担保が必要. 次の方法で解決できている: 制約でダイナミクスを縛る 例えば,関節可動域を超えたらその関節をロック 次のイテレーションで改善されることを期待する

    ※ 期待するだけで何も保証されないことに注意 順方向パスでも部分問題 (QP) を解き直す 制約を満たしつつ,最適な入力 u を順方向パスで再探索 計算量は増えるが,少なくとも制約は満たされる 現状の最も有望な方法だと思われる yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  24. 動画・ソフトウェア 動画: Control-Limited Differential Dynamic Programming https://www.youtube.com/watch?v=tCQSSkBH2NI Crocoddyl: An Efficient

    and Versatile Framework for Multi-Contact Optimal Control https://www.youtube.com/watch?v=wHy8YAHwj-M OSS: iLQG/DDP trajectory optimization (MATLAB) https://jp.mathworks.com/matlabcentral/ fileexchange/ 52069-ilqg-ddp-trajectory-optimization Crocoddyl (C++) https://github.com/loco-3d/crocoddyl yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  25. 1 はじめに 2 微分動的計画法 3 制約の付加 4 まとめ yuki @

    Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming
  26. まとめ まとめ DDP の本質は部分問題 QP への細分化 入力制約については,既存の QP 解法を適用すれば良い 状態制約については,計算力で押すしかない?

    備考 OSS の DDP ソルバ Crocoddyl は C++実装なのでめちゃ速い! yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming