2/21 Quadratic Program


逆運動学を解くことで分かるロボットの関節を動かすのに必要なトルクを\(\mathbf τ\)とする. また, 関節角度ベクトルを\(\mathbf θ\inℝ^n\), 角速度ベクトルを\(\dot{\mathbf θ}\)とおく. ただし,\(n\)は関節自由度のトータル数.

※角度はエンコーダ, 角速度は ジャイロセンサなどから得られる.

逆運動学を解くとは, 位置, 速度, 加速度から関節トルクを算出することである[1]. つまり, 逆運動学を解くためには目標軌道が与えられなければならない. Humの場合, 安定した歩行を実現させることが目的である. そうなると目標軌道として考えるべきはCOM軌道や足跡軌道といったところであろう.
一般的なロボットの運動方程式は次式のようになる[1].
$$\mathbf M(\mathbf θ)\ddot{\mathbf θ}+\mathbf h(\mathbf θ,\dot{\mathbf θ})+\mathbf D\dot{\mathbf θ}+\mathbf f(\dot{\mathbf θ})+\mathbf g(\mathbf θ)=\mathbf τ$$
※逆運動学については[234]
[5]を参考に, State Estimatorから得られるロボットの状態ベクトルを\(\mathbf q,\dot{\mathbf q}\)とおく.
floating base ヒューマノイドロボットの運動方程式および拘束方程式は, 次のように記述できる[5].
$$M(q)\ddot q+h(q,\dot q)=S\tau+J^T(q)F$$
$$J(q)\ddot q+\dot J(q,\dot q)\dot q=\ddot x$$
\(q,\dot q)\)はfloating baseでの6自由度関節を含むシステムの全ての状態である.
※floating baseについては, [67]

Low level controlのために, 与えられた目標運動を最もよく満足させるであろうトルクについて解くために逆動力学アプローチを使用する. 全身floating base(浮動基底?)逆動力学問題を二次計画法(QP)問題として定式化し, 各時間ステップで標準QPソルバーを適用してtorque commandsのセットを生成する[5].

Quadratic program(以下QP)を考えるにあたりKuindersmaらによる[8]を参考にする.
$$V(\bar{\mathbf x},\bar{\mathbf u},t)=\bar{\mathbf y}(t)^T\mathbf Q\bar{\mathbf y}(t)+\frac{∂J^*}{∂\bar{\mathbf x}}\bigr |_{\bar{\mathbf x}}\dot{\bar{\mathbf x}}\hspace{20mm}(6)$$
ZMPダイナミクスの安定化ソリューションを考えると, 瞬間拘束の下で歩行するための式(6)と2次運動コストを最小化する完全なロボットダイナミクスの制御入力を解くためのQPを設計する.
Quadratic Programは次のようになる.

\({min}_{\ddot{\mathbf q},\mathbf β, \mathbf λ, \mathbf η}V(\bar{\mathbf x},\bar{\mathbf u},t)+\omega_{\ddot{\mathbf q}}||\ddot{\mathbf q}_{des}-\ddot{\mathbf q}||^2+\epsilon \sum_{ij}\beta^2_{ij}+||\mathbf η||^2\)
subject to
$$\mathbf H_f\ddot{\mathbf q}+\mathbf C_f=\mathbf Φ^T_f\mathbf λ$$
$$\mathbf J\ddot{\mathbf q}+\dot{\mathbf J}\dot{\mathbf q}=-\alpha\mathbf J\dot{\mathbf q}+\mathbf η$$
$$\mathbf B^{-1}_a(\mathbf H_a\ddot{\mathbf q}+\mathbf C_a-\mathbf Φ^T_a\mathbf λ)\in[\mathbf τ_{min},\mathbf τ_{max}]$$
$$∀_{j=\{1…N_c\}}\;\mathbf λ_j=\sum_{i=1}^{N_d}\beta_{ij}\mathbf v_{ij}$$
$$∀_{i,j}\beta_{ij}\geq 0$$
$$\mathbf η\in [\mathbf η_{min},\mathbf η_{max}]$$

重みパラメータ\(\omega_{\ddot{\mathbf q}}\)は, 目標の動きコストとZMP追跡コストとの相対的寄与のバランスをとるために使用される.
※参考までに[9]で紹介されたQuadratic Programを示す.
———————————————————–
\({minimize}_{\dot{\mathbf v},\mathbf λ,\mathbf β,\mathbf ε}\;l(\bar{\mathbf x}_{CM},\bar{\mathbf u}_{CM},t)+\sum_{i∉C}\omega_i||\ddot{\mathbf p}_i(\dot{\mathbf v})-\ddot{\mathbf p}_{i,ref}(t)||^2+\omega_z||\mathbf ε||^2\)
subject to  \(\mathbf H_f\dot{\mathbf v}-\mathbf J^T_f\mathbf λ=-\mathbf C_f\hspace{20mm}(dynamics)\)
\(∀_{i\in C}\;\ddot{\mathbf p}_i(\dot{\mathbf v})=-\eta\dot{\mathbf p}(t)+\mathbf ε\\
|\mathbf ε|\leq\epsilon_{max}\hspace{20mm}(no slip)\)
\(∀_{j=\{1…N_c\}}\;\mathbf λ_j=\sum_{i=1}^{N_d}\beta_{ij}\mathbf w_{ij}\hspace{20mm}(friction)\\
∀_{i,j}\;\beta_{ij}\geq 0\)
\(\mathbf B^{-1}_a(\mathbf H_a\dot{\mathbf v}+\mathbf C_a-\mathbf J^T_a\mathbf λ)\in[\mathbf τ_{min},\mathbf τ_{max}]\hspace{20mm}(input)\)

\(\bar{\mathbf x}_{CM}=\begin{bmatrix}{COM}(\mathbf q)\\
\mathbf J_{CM}(\mathbf q)\mathbf v\end{bmatrix}-\bar{\mathbf x}_{CM,des}\)
\(\bar{\mathbf u}_{CM}=\dot{\mathbf J}_{CM}(\mathbf q)\mathbf v+\mathbf J_{CM}(\mathbf q)\dot{\mathbf v}-\bar{\mathbf u}_{CM,des}\)
——————————————————-
上のQuadratic Programを解くには, robot state,\(\mathbf q,\dot{\mathbf q}\)が必要である.(下図の赤矢印)
Quadratic programと逆運動学を1つにまとめると,

[9]を参考に以前書いたブロック線図を示す.

QPを解くための目標軌道を生成するBody Motion Controllers(その前にplannersが存在)が必要である.
ここで, 目標軌道としてはFoot Trajectory(足跡軌道)が大事である. それを実現するためには, ZMPやDCM,  CoM, さらにバランスを保つためにはPelvis Rotation(骨盤回転)も必要だ.
[10]によれば, DCMの固有振動数を変えることで, ステッピング中に一般的なCoMの高さの軌跡を達成することができる.

[11]をもとに考えると, 回転のためのrotation controllers(&planner), 足跡軌道のためのswing foot controller(&planner), さらに運動量制御のためのmomentum controllerが必要である.

【参考文献】
[1]松日楽信人, 大明準治, ロボットシステム入門, 株式会社オーム社, 2017
[2]https://shingi.jst.go.jp/past_abst/abst/p/12/1221/kumamoto06.pdf
[3]http://web.tbgu.ac.jp/ait/fei/robotics/Robotics_13_review_1short.pdf
[4]http://www.cqpub.co.jp/interface/sample/200610/I0610119.pdf
[5]http://www.cs.cmu.edu/afs/cs/user/sfeng/www/sf_hum13.pdf
[6]https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/6-dynamics2_ex.pdf
[7]http://ipvs.informatik.uni-stuttgart.de/mlr/wp-content/uploads/2014/04/advanced_robotics_ss14_lecture3_notes.pdf
[8]https://groups.csail.mit.edu/robotics-center/public_papers/Kuindersma13.pdf
[9]https://dspace.mit.edu/openaccess-disseminate/1721.1/110533
[10]http://www.romela.org/wp-content/uploads/2015/05/2014_humanoid_locomotion_on_uneven_terrain_using_the_time-varying_divergent_component_of_motion.pdf
[11]https://www.researchgate.net/profile/Robert_Griffin17/publication/287208188_Design_of_a_Compliant_Bipedal_Walking_Controller_for_the_DARPA_Robotics_Challenge/links/58207a5608ae40da2cb4e7bb/Design-of-a-Compliant-Bipedal-Walking-Controller-for-the-DARPA-Robotics-Challenge.pdf
[12]
[13]
[14]
[15]