微分動的計画法の近年の動向： 制約付き軌道計画問題へのアプローチ Yuki Onishi 第 0 回ロボティクス勉強会 2020 年 5 月 15 日（金） yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

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

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

微分動的計画法 Differential Dynamic Programming (Jacobson & Mayne, 1970) DDP と略される． 有限時間最適制御問題の数値解法 制御入力の列を求める 最適性の 1 次の必要条件： 評価関数の勾配がゼロとなる解を求める1 二次収束性を有する ロボットの歩行や自動運転の駐車のプランニング， モデル予測制御 (MPC) などで利用されている． 1indirect な方法と呼ばれる． yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

この資料の目的 微分動的計画法の基礎的な事項についてまとめる 最近の研究動向を紹介する 公開されているソフトウェアを紹介する yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

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

ダイナミクスモデル 次の一般的な時間発展の 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

評価関数 スカラ関数 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

最適性の原理 最適性の原理 (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

動的計画法 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

微分動的計画法の概略 アルゴリズムの概略 1. 最適性の原理に従い， 価値関数 (2) を二次で近似して 逆方向パスを計算する． 2. ダイナミクス (1) に従って， 順方向パスでダイナミクスを計算． 3. 上記を繰り返す． yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

価値関数の二次形式近似 (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

価値関数の二次形式近似 (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

価値関数の二次形式近似 (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

直線探索 逆方向パスが計算されたところで，順方向パスを計算する． 先の計算で求まったのはあくまでも局所的な最適入力で， 大域的な最適性は保証されていないことに注意する． 更新規則は次の通り． ˆ 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

アルゴリズムまとめ 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

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

最適化に対する制約 実用上の軌道計画において，制約がないことは稀． 実際の制約の例： 状態制約 関節の可動域（トルク入力，速度入力の場合） 障害物との接触・干渉 入力制約 関節の可動域（位置入力の場合， 逆運動学） トルクや速度の飽和 これらの問題を考慮しないと， 「最適であっても実現は不可能な軌道」が生成されてしまう． yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

制約を考慮した微分動的計画法 (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

制約を考慮した微分動的計画法 (2/2) いろいろ研究されているが，方向性は大体同じ． 制約の組み込み方 最適性の原理によって分割された 部分問題それぞれに制約を組み込む． 具体的には次の操作が必要： 1. 逆方向パスにおけるゲイン k, K の修正（入力制約） 2. 順方向パスにおける実行可能性の担保（状態制約） yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

逆方向パスへの制約の付与 入力制約： 逆方向パスにおけるゲイン k, K の修正が必要． つまり，次を解く． min δu Q(δx, δu) s.t. . . . やること自体は実質制約付き QP なので， 制約に適した方法を使えば良さそう． 矩形制約 Projected-Newton 法が決定的な模様 一般の制約 一般化ラグランジアンと KKT 条件 yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

順方向パスへの制約の付与 (1/2) 状態制約： 順方向パスにおける実行可能性 (feasibility) の担保が必要． 状態制約は難しい！ 直接操作できるのは入力 u で， その結果生じるのが状態 x xi はその時刻までの入力列 {u0 , . . . , ui−1 } に依存 逆方向パスほど簡単ではない…… yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

順方向パスへの制約の付与 (2/2) 状態制約： 順方向パスにおける実行可能性 (feasibility) の担保が必要． 次の方法で解決できている： 制約でダイナミクスを縛る 例えば，関節可動域を超えたらその関節をロック 次のイテレーションで改善されることを期待する ※ 期待するだけで何も保証されないことに注意 順方向パスでも部分問題 (QP) を解き直す 制約を満たしつつ，最適な入力 u を順方向パスで再探索 計算量は増えるが，少なくとも制約は満たされる 現状の最も有望な方法だと思われる yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming

動画・ソフトウェア 動画： 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

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

まとめ まとめ DDP の本質は部分問題 QP への細分化 入力制約については，既存の QP 解法を適用すれば良い 状態制約については，計算力で押すしかない？ 備考 OSS の DDP ソルバ Crocoddyl は C++実装なのでめちゃ速い！ yuki @ Tokyo Tech SSR A Survey of Constrained Differential Dynamics Programming