2/14 MITの安定化制御則(4)


今回も[1]の内容を見ていく(ほとんど日本語に訳しているだけです). ページで言うと, 14/47~18/47

3.2 Dynamic Motion Planning
自明でない環境で複雑な動きをするヒューマノイドロボットの場合, 運動学的制約と動的制約が一緒に現れることがよくある. すべての機械システムでは, システムの全線形および角運動量の割合は, システムの正味の外部レンチ(力/トルク)と等しくなければならない.  瞬間運動量は, 一般化された位置qと速度vの関数である. この実装では, 浮動基底位置と速度をオイラー角とその導関数を使って表すが, 特異点のない表現も使用できる.  重心運動量は, 瞬間重心(COM)位置で座標フレーム内に定義され, ワールドフレームと位置合わせされた総運動量である. OrinとGoswami [2]は, 重心運動量行列\(\mathbf A_G\)を使って簡単に計算できることを示した.

$$\mathbf{h}_G=\mathbf{A}_G(\mathbf q)\mathbf v\equiv \left[
\begin{array}{ccc}
\mathbf k \\
\mathbf l
\end{array}
\right]\\$$
ここで、klは, それぞれ重心角運動量と線形運動量である.
重心運動量の割合は上式を微分するか, あるいは外力の関数として書くことで表すことができる[3][4].

$$\dot{\mathbf k}=\sum_{j=1}^{N_c} (\mathbf c_j-\mathbf r)\times \mathbf {\lambda}_j$$
$$\dot{\mathbf l}=m\ddot{\mathbf r}=m\mathbf g+\sum_{j=1}^{N_c}\mathbf \lambda_j$$
ここで, rはロボットのCOM, \(m\)は総質量, gは重力による加速度, cj, λjはそれぞれ点jでの接触位置と力である.  Ncはアクティブな接点の数である.  以下で論じるように, それが最適化において単純な線形摩擦拘束の使用を可能にするので, 剛体に作用する総レンチの冗長な複数力記述を使用する. Kajitaらによって提案された解決された運動量制御フレームワークを含む, バランスと移動のためにヒューマノイドの運動量を制御することにおける説得力のある最近の研究がある[5][6][7][8]. この研究に触発されて, 計画アルゴリズムを次の概念を使って設計する.
ここでのアプローチは, 外力とモーメントを推論し, 重心ダイナミクスを使用しながら, 動的な実現可能性を確実にするため全身運動学を使用して軌道を計画することである.そうすることで, ロボットのn-6の内部自由度が完全に作動するという基本的な仮定をしている. そして動的制約はそれ故6つの浮動ベースDOFに関して定義されることができる. これは, 完全なダイナミクスを含む最適化と比較して, はるかに少ない決定変数および制約を伴う最適化問題をもたらす.
さらに, 制約条件の勾配(ほとんどの非線形最適化問題のソルバーで必要とされる)は計算がかなり簡単である. この計算量の減少の代償は, 内部トルクについて推論できないことである. しかし, アクチュエーターの強度があるロボットの場合, これはそれほど大きな制限ではない. アクチュエータの制限が非常に制限されているロボットであっても, この最適化の出力を軌道の最適化問題の初期シードとして使用して完全なダイナミクスを推論することには, 計算上の利点がいくらかある.
合理的な動的運動を発生させるためには, 接触力に対する摩擦の制約が満たされなければならない. アクティブ接触点cjごとに摩擦円錐の標準的な保守的な多面体近似を使用する.

$$\lambda_j=\sum_{i=1}^{N_d}\beta_{ij}\mathbf w_{ij},\;\; \beta_{ij}\geq0$$
生成ベクトルwijは, wij = nj + µjdijとして計算される. ここで, njdijは, j番目の接触に対する接触面の法線とi番目の接線ベクトルである. µjはクーロン摩擦係数, Ndは近似で使用される接ベクトルの数である[9]
.
軌道最適化アルゴリズムは, shooting methodsとtranscription methodsの2つの一般的なクラスに分類される[10]
. shooting methodsは決定変数として制御入力のみを含み, コスト関数を評価するためにシステムを前方にシミュレートする必要がある. 一方, transcription methodsは, 決定変数として軌道に沿った有限の状態のセットを含み, 状態変数および入力変数に対する制約としてシステムのダイナミクスを組み込んでいる. 軌跡に沿って状態と入力を同時に最適化することにより, transcription methodsはshooting methodsに存在する可能性がある数値的問題(軌跡の初期の入力の小さな変化がコストと最終状態の大きな変化につながる)を回避し,  最適化中のシステムダイナミクスをシミュレートを回避する.
\(k = 1, …, M\)で微分拘束を代数方程式として表すことによってdirect transcription 問題を定式化する. \(h[k]\)は点\(k\)と\(k + 1\)の間の時間間隔を表す.


ここで, 決定変数\(\Gamma\)は次式となる.

\(\Gamma=\{\mathbf q[k], \mathbf v[k], h[k], \mathbf r[k], \dot{\mathbf r}[k], \ddot{\mathbf r}[k], \mathbf c_j[k], \mathbf \lambda[k], \beta[k], \mathbf k[k], \dot{\mathbf k}[k] |\:k=1, …,M\}\)
また, コスト関数\(L(\mathbf q[k], \mathbf v[k], \ddot{\mathbf r}[k], \mathbf \lambda[k], h[k])\)は, 次式に等しい.
$$h[k]\Bigr(||\mathbf q[k]-\mathbf q_{nom}[k]||^2_{\mathbf Q_q}+||\mathbf v[k]||^2_{\mathbf Q_v}+||\ddot{\mathbf r}[k]||^2+\sum_{j}||\mathbf\lambda_j||^2_{\mathbf Q_\lambda}\Bigr)$$
ここで, \(||\mathbf x||^2_\mathbf Q\)は2次コスト\(\mathbf{ x^TQx, Q}\geq 0\)の簡潔表現である. \(\mathbf A^{\mathbf k}_G\)は, \(\mathbf A_G\)の上位3行をとることによって形成される行列である.  コスト項\(||\mathbf q[k]-\mathbf q_{nom}[k]||^2_{\mathbf Q_q}\)は, ロボットの構成を公称姿勢(例えば, 典型的な直立姿勢)に正規化する.
時間積分の制約には, 一般化速度のbackward-Euler積分、角運動量とCOM加速度, およびCOM速度のmid-point積分が含まれる.

\(h[k]\mathbf v[k]=\mathbf q[k]-\mathbf q[k-1]\)
\(h[k]\dot {\mathbf r}[k]=\mathbf k[k]-\mathbf k[k-1]\)
\(h[k]\ddot {\mathbf r}[k]=\dot{\mathbf r}[k]-\dot{\mathbf r}[k-1]\)
\(\frac{h[k]}{2}(\dot{\mathbf r}[k]+\dot{\mathbf r}[k-1])=\mathbf r[k]-\mathbf r[k-1]\)

運動学的拘束は, 通常, 片側関節限界, 接触位置, および衝突のない拘束を含む. 全身の人型の動きを制御するために, 以前と同様の制約セットが採用されている[11][12]. [12]は, フルボディモデルと点質量動的モデルを使用して, 運動学的計画問題を個別に解決している. ここでのアプローチはこれら二つの問題をより記述的な動的モデルと一緒に組み合わせ, そしてこの種の二段階プランナーでは扱うことができない運動を生成することができる(例えばランニングやジャンプ).
制約条件のほとんどは1つまたは2つのノットポイントでの決定変数にのみ依存するので, この最適化問題の勾配はまばらである. 疎勾配をもつ非線形計画法は, SNOPTのような強力な逐次2次計画法(SQP)ソルバーを使って効率的に解くことができる[13].

【参考文献】
[1]https://dspace.mit.edu/openaccess-disseminate/1721.1/110533
[2]http://motionlab.kaist.ac.kr/wp-content/uploads/2017/06/2013_AURO_OGL.pdf
[3]https://pdfs.semanticscholar.org/cf32/008efe00f0748518a2c3321b3b1ff4d2acb8.pdf
[4]https://groups.csail.mit.edu/robotics-center/public_papers/Dai14.pdf
[5]https://www.researchgate.net/publication/224705467_A_Pattern_Generator_of_Humanoid_Robots_Walking_on_a_Rough_Terrain
[6]https://www.researchgate.net/publication/236661674_Momentum-based_Balance_Control_for_Torque-controlled_Humanoids
[7]https://www.researchgate.net/publication/256422371_Summary_of_team_IHMC’s_Virtual_Robotics_Challenge_entry
[8]https://www.researchgate.net/publication/4045805_Resolved_Momentum_Control_Humanoid_Motion_Planning_Based_on_the_Linear_and_Angular_Momentum
[9]http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.10.6519&rep=rep1&type=pdf
[10]http://www.appliedmathematicalanalysis.com/downloads/surveyppr.pdf
[11]https://www.researchgate.net/publication/221065673_Generic_dynamic_motion_generation_with_multiple_unilateral_constraints
[12]https://hal.archives-ouvertes.fr/hal-00654175v2/document
[13]http://www.ccom.ucsd.edu/~peg/papers/snpaper.pdf