2/22 Linear quadratic regulator:線形2次レギュレータ



ロボットの現在の状態を使用して, LQRソリューションをオンラインで(通常は別のスレッドで)再計算して, nominal歩行軌跡からの偏差に対するシステムの感度を下げることができる[1].
LQRソリューションはロボットの全身の動きを捉えないため, 足や骨盤などのロボットの身体に取り付けられた一連のフレームの目標加速度として追加の動きの目標を指定する.  時間\(t\)におけるボディフレームの望ましい加速度\(\ddot{\mathbf p}(t)\)は, PD規則を使用して,
$$\ddot{\mathbf p}_{ref}(t)=\mathbf K_p(\mathbf p_d(t)-\mathbf p(t))+\mathbf K_d(\dot{\mathbf p}_d(t)-\dot{\mathbf p}_(t))+\ddot{\mathbf p}_d(t)$$
と計算される. ここで, \(\mathbf p_d(t)\)は滑らかな2階微分可能なフレームの目標軌道である.

[1]で紹介されているLQR問題の定式化を示す.

\({minimize}_{\mathbf u_{CM}(t)}\;\int_0^∞g(\mathbf x_{CM}(t),\mathbf u_{CM}(t),t)dt\)
subject to  \(\dot{\bar{\mathbf x}}_{CM}(t)=\mathbf A\bar{\mathbf x}_{CM}(t)+\mathbf B\bar{\mathbf u}_{CM}(t)\)
\(\mathbf c(t)=[\mathbf I_{2\times 2}\;0_{2\times 4}]\mathbf x_{CM}(t)-\frac{r_z}{g}\mathbf {Iu}_{CM}(t)\)
\(\mathbf c_d(t)=\mathbf c_d(t_f),\;∀t\geq t_f\)
\(\mathbf Q=\mathbf Q^T≻0\)
\(\mathbf R=\mathbf R^T≻0\)

ここで, \(\bar{\mathbf x}_{CM}=\mathbf x_{CM}(t)-[\mathbf c_d^T(t_f),0,0]^T\)また\(\bar{\mathbf u}_{CM}(t)=\mathbf u_{CM}(t)\)である. ここでコストは\(\bar{\mathbf c}(t)=\mathbf c(t)-\mathbf c_d(t)\)に関して次のように定義される.
\(g(\mathbf x_{CM}(t),\mathbf u_{CM}(t),t)=\bar{\mathbf c}^T(t)\mathbf Q_c\bar{\mathbf c}(t)+\bar{\mathbf u}_{CM}^T(t)\mathbf R\bar{\mathbf u}_{CM}(t)\)
ここで, 右辺は, 線形出力方程式に代入することによって\(x_{CM}(t),\bar{\mathbf u}_{CM}(t)\)および\(c_d(t)\)で表すことができる.  この問題についてRiccati方程式を導出すると, Sには時間依存項がないことがわかる( continuous algebraic Riccati equation:連続代数Riccati方程式に相当).
次に、線形および affine cost-to-go係数は, 軌跡に沿った単一のbackward passで(すなわち, 数値的に積分する必要なしに)解くことができる線形微分方程式になる. これは実際には\( l(\bar{\mathbf x}_{CM},\bar{\mathbf u}_{CM},t)\)はreceding-horizon(後退地平線?)の足跡計画に適した時間スケールで非常に効率的に計算できることを意味する.

運動量とCoG(center of gravity)の制御は, 本質的に運動と力の両方の課題である. 実際, 重心運動量行列[2]を使用して, ロボット全体の運動量とロボット関節および姿勢速度の間のlinear mapping(線形写像)を見つけることができる[3]
最適なフィードバックゲインを計算するために次式のモデルを使用する.
$$\dot{\mathbf x}_{cog}=\frac{1}{m}\mathbf h_{lin}$$
$$\dot{\mathbf h}=\begin{bmatrix}\mathbf I_{3\times 3}&\mathbf  0_{3\times 3}…\\
[\mathbf x_i-\mathbf x_{cog}]_\times&\mathbf I_{3\times 3}…\end{bmatrix}\mathbf λ+\begin{bmatrix}m\mathbf g\\ \mathbf 0\end{bmatrix}$$
ここで, \(\mathbf h=\mathbf H_{\mathbf G}(\mathbf q)\dot{\mathbf q}\)であり, \(\mathbf h=[\mathbf h^T_{lin}\; \mathbf h^T_{ang}]^T\)はCoGにおける線形および角運動量である.
ダイナミクスを線形化し, 目標パフォーマンスコストを選択してLQRコントローラーを計算する.  次の形式の制御則を得る[3].

$$\mathbf λ=-\mathbf K\begin{bmatrix}\mathbf x_{cog}\\ \mathbf h\end{bmatrix}+\mathbf k(\mathbf x_{ref},\mathbf h_{ref})$$

その他のよさげなLQR[5

【参考文献】
[1]https://dspace.mit.edu/openaccess-disseminate/1721.1/110533
[2]https://www.researchgate.net/publication/224339669_Centroidal_Momentum_Matrix_of_a_Humanoid_Robot_Structure_and_Properties
[3]https://arxiv.org/pdf/1410.7284.pdf
[4]http://agile.seas.harvard.edu/files/agile/files/dircon.pdf
[5]https://groups.csail.mit.edu/robotics-center/public_papers/Kuindersma13.pdf
[6]
[7]