倒立振子のモデル予測制御(MPC)

参考

目標

モデル予測制御(Model Predictive Control)を用いて倒立振子の制御を行う。
制御対象は左右の脚に関節が付いたもので、下記を制御達成の目安とする。

立つこと
コントローラの操作に合わせて前後左右に移動すること
関節を用いて左右間の段差(高低差)に対応できること
高所から落ちて着地できること。
ジャンプできること。

手段

M5Stack Core2を用いて制御する。
下記センサをフュージョンして状態の推定を行う。
- M5Stack Core2に乗っている6軸IMU(MPU6886)を使用。
- モーター駆動軸に磁気式ロータリ位置センサ(AS5047D)を使用。
モーター駆動軸にM2006を使用。データシート

手順

倒立振子の物理モデルを立てる。(状態方程式に変換。)
MPCを実装。
シミュレータを実装。
MPCを用いてシミュレータ内で倒立振子を立たせる。
立たせる実機実験。
MPCを用いてシミュレータ内で追従を行う。
追従実機実験。
関節の制御で段差対応。
ジャンプ。

台車型倒立振子の物理モデル

モデルを単純化するため、はじめは関節なしの台車型倒立振子型ロボットを $x$ 方向にのみ移動させるものと仮定して数式を立てる。制御入力は台車に加える力 $F$ とする。
鉛直上向きに対する $x$ 軸方向への振り子の傾きを $\theta$ 、台車のx変位を $x$ 、状態量を $\boldsymbol{x} = [x, \dot{x}, \theta, \dot{\theta}]^\mathsf{T}$ として考える。

台車に力 $F$ を加えるとき、台車の質量を $m$ 、振り子からの水平方向反力を $H$ として水平方向の運動方程式は

\begin{aligned} m \ddot{x} &= F - H \\ \end{aligned}

ここで水平方向の反力 $H$ を振り子の質量 $M$ による慣性力と近似して $H \simeq M\ddot{x}$ より

\begin{aligned} m \ddot{x} &\simeq F - M\ddot{x} \\ (m + M) \ddot{x}&\simeq F \\ \end{aligned}

振り子の質量を $M$ 、垂直抗力を $V$ 、慣性モーメントを $J$ 、重心までの長さを $l$ としてモーメントを考える。モデルを線形にするために $\theta \ll 1$ と仮定して $\sin\theta \simeq \theta, \cos\theta \simeq 1$ 。垂直抗力 $V \simeq Mg$ と近似。

\begin{aligned} J \ddot{\theta} &= Vl \sin\theta - Hl \cos\theta \\ &\simeq Mg l \sin\theta - M \ddot{x} l \cos\theta \\ &\simeq Mg l \sin\theta - M\frac{F}{m + M} l \cos\theta \\ &\simeq M g l \theta - \frac{M}{m + M} lF \\ \end{aligned}

以上より式を整理して

\begin{aligned} \ddot{x} &= \frac{F}{m + M} \\ \ddot{\theta} &= \frac{Mgl}{J} \theta - \frac{M}{m + M} \frac{l}{J} F \\ \end{aligned}

\begin{bmatrix} \dot{x} \\ \ddot{x} \\ \dot{\theta} \\ \ddot{\theta} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{Mgl}{J} & 0 \\ \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \theta \\ \dot{\theta} \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{m + M} \\ 0 \\ - \frac{M}{m + M} \frac{l}{J} \\ \end{bmatrix} F

 反力H、Vを近似しない場合台車の変位xと振り子の変位x_p, z_pの関係より

\begin{aligned}
x_p &= x + l\sin\theta \\
z_p &= l\cos\theta \\
\end{aligned}
上式を2階微分して

\begin{aligned}
\ddot{x}_p &= \ddot{x} + (l\sin\theta)'' \\
    &= \ddot{x} + (l\dot{\theta}\cos\theta)' \\
    &= \ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta \\
\ddot{z}_p &= (l\cos\theta)'' \\
    &= (-l\dot{\theta}\sin\theta)' \\
    &= -l\ddot{\theta} \sin\theta - l\dot{\theta}^2\cos\theta \\
\end{aligned}
また振り子の運動方程式は

\begin{aligned}
M \ddot{x_p} &= H \\
M \ddot{z_p} &= V - Mg \\
\end{aligned}
ここにx_p,z_pを代入して整理すると

\begin{aligned}
H &= M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
V &= Mg + M(-l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) \\
    &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) \\
\end{aligned}
これを台車の運動方程式に代入し、

\begin{aligned}
m\ddot{x} &= F - H \\
    &= F - M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
(m+M)\ddot{x} +Ml\ddot{\theta}\cos\theta &= Ml\dot{\theta}^2\sin\theta + F \\
(m+M)\ddot{x} +Ml\ddot{\theta} &\simeq Ml\dot{\theta}^2\theta + F \\
(m+M)\ddot{x} +Ml\ddot{\theta} &\simeq F \\
\end{aligned}
同様にモーメントの式に代入し

\begin{aligned}
J \ddot{\theta}
    &= V l \sin\theta - H l \cos\theta \\
    &= M(g -l\ddot{\theta}\sin\theta - \cancel{l\dot{\theta}^2\cos\theta}) l \sin\theta \\
    &\phantom{{}={}} - M (\ddot{x} + l\ddot{\theta}\cos\theta - \cancel{l\dot{\theta}^2\sin\theta}) l \cos\theta \\
    &= Mgl\sin\theta - Ml\ddot{x}\cos\theta - Ml^2(\cancel{\sin^2\theta+\cos^2\theta})\ddot{\theta} \\
    &= Mgl\sin\theta - Ml\ddot{x}\cos\theta - Ml^2\ddot{\theta}
\end{aligned}

\begin{aligned}
Ml\ddot{x}\cos\theta + (Ml^2+J) \ddot{\theta} &= Mgl\sin\theta \\
Ml\ddot{x} + (Ml^2+J) \ddot{\theta}
    &\simeq Mgl\theta \\
\end{aligned}
これを整理して\ddot{x},\ddot{\theta}の表現を得る。上の2式を連立して

\begin{cases}
(m+M)\ddot{x} +Ml\ddot{\theta} &=& F & (*)\\
Ml\ddot{x} + (Ml^2+J) \ddot{\theta} &=& Mgl\theta & (**) \\
\end{cases}
(Ml^2+J)(*)-Ml(**)より

\begin{aligned}
\{ (m+M)(Ml^2+J) - M^2l^2 \}\ddot{x} &= - M^2gl^2\theta + (Ml^2+J)F \\
\ddot{x} &= -\frac{M^2gl^2}{D}\theta + \frac{Ml^2+J}{D}F \\
\end{aligned} \\
(\because D:=(m+M)(Ml^2+J) - M^2l^2 )
Ml(*)-(m-M)(**)より

\begin{aligned}
\{ M^2l^2 - (m+M)(Ml^2+J) \}\ddot{\theta} &= -(m+M)Mgl\theta+MlF \\
-D\ddot{\theta} &= -(m+M)Mgl\theta+MlF\\
\ddot{\theta} &= \frac{(m+M)Mgl}{D}\theta - \frac{Ml}{D}F \\
\end{aligned} \\
(\because D:=(m+M)(Ml^2+J) - M^2l^2 )
以上より状態方程式表現を得る。

\begin{bmatrix}
    \dot{x} \\ \ddot{x} \\ \dot{\theta} \\ \ddot{\theta} \\
\end{bmatrix} =  \begin{bmatrix}
    0 & 1 & 0 & 0 \\
    0 & 0 & -\frac{M^2gl^2}{D} & 0 \\
    0 & 0 & 0 & 1 \\
    0 & 0 & \frac{(m+M)Mgl}{D} & 0 \\
\end{bmatrix}
\begin{bmatrix}
    x \\ \dot{x} \\ \theta \\ \dot{\theta} \\
\end{bmatrix}
+
\begin{bmatrix}
    0 \\
    \frac{Ml^2+J}{D} \\
    0 \\
    -\frac{Ml}{D} \\
\end{bmatrix}
F

 車輪型倒立振子状態変数を\boldsymbol{x} = [x, \dot{x}, \theta, \dot{\theta}]^\mathsf{T}として、左右のモーターそれぞれに発生させるトルクをTとする。

ただしx：車輪の変位、\theta：鉛直上向きに対するx軸方向への振り子の傾き。
地面・タイヤ間の摩擦力をfとすると、タイヤの水平方向の運動方程式は（m：タイヤ1輪の質量、H：振り子からの水平反力）

\begin{align}
m \ddot{x} &= f - H \\
\end{align}
ホイールの角変位を\theta_wとしてホイールの角運動方程式は（J_w：ホイール1輪の慣性モーメント、r：半径）

\begin{align}
J_w \ddot{\theta}_w &= T - fr \\
\end{align}
物理的制約より、加速度と角加速度の関係式

\begin{align}
\ddot{x} &= r\ddot{\theta}_w \\
\end{align}
以上3式を整理して、f,\ddot{\theta_w}を消す。

\begin{align}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} - H \\
\end{align}
上式の導出(2)式に\frac{1}{r}をかけて、(3)式を代入。

\begin{aligned}
\frac{J_w}{r} \ddot{\theta}_w &= \frac{T}{r} - f \\
\frac{J_w}{r^2} \ddot{x} &= \frac{T}{r} - f \\
\end{aligned}
これを(1)式と比較して、(1)+(2)'より

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} - H \\
\end{aligned}
振り子のモーメントの式（M、振り子の質量、J：振り子の慣性モーメント、l：重心までの長さ）

\begin{align}
J \ddot{\theta} &= Vl\sin\theta - Hl\cos\theta \\
\end{align}
ここで車輪の変位xと振り子の変位x_p,z_pの関係式より、台車の場合と同様に以下を得る。

\begin{aligned}
H &= M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
V &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) \\
\end{aligned}
以上を代入して整理することで\ddot{x}, \ddot{\theta}の表現を得る。

\begin{aligned}
\ddot{x} &= -\frac{M^2gl^2}{D}\theta + \frac{Ml^2+J}{Dr}T \\
\ddot{\theta} &= \frac{m+M+\frac{J_w}{r^2}}{D}Mgl\theta - \frac{Ml}{Dr}T \\
\end{aligned} \\
(\because D:=(m+M+\frac{J_w}{r^2})(Ml^2+J) - M^2l^2 )
上式の導出(4)式にHを代入。\theta \ll 1より近似。

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} - H \\
    &= \frac{T}{r} - M(\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
(m+M+\frac{J_w}{r^2})\ddot{x} + Ml\ddot{\theta}\cos\theta &= \frac{T}{r} + \frac{Ml}{2}\dot{\theta}^2\sin\theta \\
(m+M+\frac{J_w}{r^2})\ddot{x} + Ml\ddot{\theta} &\simeq \frac{T}{r} \\
\end{aligned}
V,Hを(5)式に代入。これは台車型と同様なので省略。

\begin{aligned}
Ml\ddot{x}\cos\theta + (Ml^2+J) \ddot{\theta} &= Mgl\sin\theta \\
Ml\ddot{x} + (Ml^2+J) \ddot{\theta} &\simeq Mgl\theta \\
\end{aligned}
上2式を連立して整理する。

\begin{cases}
(m+M+\frac{J_w}{r^2})\ddot{x} + Ml\ddot{\theta} &=& \frac{T}{r} & (*) \\
Ml\ddot{x} + (Ml^2+J) \ddot{\theta} &=& Mgl\theta & (**) \\
\end{cases}
(Ml^2+J)(*)-Ml(**)より

\begin{aligned}
\{ (m+M+\frac{J_w}{r^2})(Ml^2+J) - M^2l^2 \} \ddot{x} &= -M^2gl^2\theta + (Ml^2+J)\frac{T}{r} \\
\ddot{x} &= -\frac{M^2gl^2}{D}\theta + \frac{Ml^2+J}{Dr}T \\
\end{aligned} \\
(\because D:=(m+M+\frac{J_w}{r^2})(Ml^2+J) - M^2l^2 )
Ml(*) - (m+M+\frac{J_w}{r^2})(**)より

\begin{aligned}
\{ M^2l^2 - (m+M+\frac{J_w}{r^2})(Ml^2+J) \}\ddot{\theta} &= -(m+M+\frac{J_w}{r^2})Mgl\theta + \frac{Ml}{r}T \\
-D \ddot{\theta} &= -(m+M+\frac{J_w}{r^2})Mgl\theta + \frac{Ml}{r}T \\
\ddot{\theta} &= \frac{m+M+\frac{J_w}{r^2}}{D}Mgl\theta - \frac{Ml}{Dr}T \\
\end{aligned} \\
(\because D:=(m+M+\frac{J_w}{r^2})(Ml^2+J) - M^2l^2 )
以上より状態方程式表現を得る。

\begin{bmatrix}
    \dot{x} \\ \ddot{x} \\ \dot{\theta} \\ \ddot{\theta} \\
\end{bmatrix} =  \begin{bmatrix}
    0 & 1 & 0 & 0 \\
    0 & 0 & -\frac{M^2gl^2}{D} & 0 \\
    0 & 0 & 0 & 1 \\
    0 & 0 & \frac{(m+M+\frac{J_w}{r^2})Mgl}{D} & 0 \\
\end{bmatrix}
\begin{bmatrix}
    x \\ \dot{x} \\ \theta \\ \dot{\theta} \\
\end{bmatrix}
+
\begin{bmatrix}
    0 \\
    \frac{Ml^2+J}{Dr} \\
    0 \\
    -\frac{Ml}{Dr} \\
\end{bmatrix}
T

A_c = \begin{bmatrix}
    0 & 1 & 0 & 0 \\
    0 & 0 & -\frac{M^2gl^2}{D} & 0 \\
    0 & 0 & 0 & 1 \\
    0 & 0 & \frac{(m+M+\frac{J_w}{r^2})Mgl}{D} & 0 \\
\end{bmatrix},
B_c = \begin{bmatrix}
    0 \\
    \frac{Ml^2+J}{Dr} \\
    0 \\
    -\frac{Ml}{Dr} \\
\end{bmatrix},
\boldsymbol{u} = \begin{bmatrix} T \end{bmatrix}
\text{として}

\begin{aligned}
\dot{\boldsymbol{x}} &= A_c\boldsymbol{x} + B_c\boldsymbol{u} \\
\end{aligned}

また出力を $\boldsymbol{x} = [x, \theta]^\mathsf{T}$ としたときの出力方程式は

\begin{bmatrix} x \\ \theta \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \theta \\ \dot{\theta} \\ \end{bmatrix}

モータに電流 $I$ を制御入力として与える場合、 $T$ に以下を代入する。トルク定数を $K_T$ として

T = K_T I

可制御性

可制御性行列 $M_c$ を以下の通り定義する。

M_c = \begin{bmatrix} B & AB & A^2B & A^3B \end{bmatrix}

このとき $\mathrm{rank}{M_c} = 4$ ならば可制御。

可観測性

可観測行列 $M_o$ を以下の通り定義する。

M_o = \begin{bmatrix} C \\ CA \\ CA^2 \\ CA^3 \end{bmatrix}

このとき $M_o=4$ ならば可観測。

最終的な式はこちら

MPCの実装

状態方程式の離散化
最適化問題に落とし込む。
凸最適化問題を解く。

 状態方程式をdt間隔で離散化時刻kでの状態量\boldsymbol{x}_kから単位ステップ時間dt経過した時刻k+1での状態量\boldsymbol{x}_{k+1}を求める。

\begin{aligned}
\frac{d\boldsymbol{x}}{dt} &= A\boldsymbol{x} + B\boldsymbol{u} \\
    &= \frac{\boldsymbol{x}_{k+1} - \boldsymbol{x}_k}{dt} \\
\boldsymbol{x}_{k+1} - \boldsymbol{x}_k &= A\boldsymbol{x}_k dt + B\boldsymbol{u}_k dt \\
\boldsymbol{x}_{k+1} &= \boldsymbol{x}_k + A\boldsymbol{x}_k dt + B\boldsymbol{u}_k dt \\
    &= (I + Adt) \boldsymbol{x}_k + B dt\boldsymbol{u}_k \\
\end{aligned}
または\dot{x} = Axの解より

\begin{aligned}
\dot{x} &= Ax \\
\int_k^{k+1} \frac{1}{x} \,\mathrm{d}x &= \int_k^{k+1} A \,\mathrm{d}t \\
x_{k+1} &= e^{A \Delta t} x_k \\
    &= (I + A \Delta t + \frac{(A \Delta t)^2}{2!} + \frac{(A \Delta t)^3}{3!} + \dots) x_k \\
    &= (I + A \Delta t) x_k \\
&\because\; (A \Delta t)^2 \simeq 0
\end{aligned}

 最適化問題に落とし込む時刻k=0時点での初期値を\boldsymbol{x}_0とする。

A:=(I + A_cdt), B:=B_cdtと置き直して
k=0のとき

\boldsymbol{x}_1 = A\boldsymbol{x}_0 + B \boldsymbol{u}_0
k=1のとき

\begin{aligned}
\boldsymbol{x}_2 &= A\boldsymbol{x}_1 + B \boldsymbol{u}_1 \\
    &= A(A\boldsymbol{x}_0 + B \boldsymbol{u}_0) + B \boldsymbol{u}_1 \\
    &= A^2\boldsymbol{x}_0 + AB\boldsymbol{u}_0 + B \boldsymbol{u}_1 \\
    &= A^2\boldsymbol{x}_0 + \begin{bmatrix} AB & B \end{bmatrix}
        \begin{bmatrix} \boldsymbol{u}_0 \\ \boldsymbol{u}_1 \end{bmatrix} \\
\end{aligned}
k=2のとき

\begin{aligned}
\boldsymbol{x}_3 &= A\boldsymbol{x}_2 + B \boldsymbol{u}_2 \\
    &= A(A^2\boldsymbol{x}_0 + AB\boldsymbol{u}_0 + B \boldsymbol{u}_1) + B \boldsymbol{u}_2 \\
    &= A^3\boldsymbol{x}_0 + \begin{bmatrix} A^2B & AB & B \end{bmatrix}
        \begin{bmatrix} \boldsymbol{u}_0 \\ \boldsymbol{u}_1 \\ \boldsymbol{u}_2 \end{bmatrix} \\
\end{aligned}
以降同様にk=n-1のとき

\begin{aligned}
\boldsymbol{x}_{n} &= A\boldsymbol{x}_{n-1} + B \boldsymbol{u}_{n-1} \\
    &= A^n\boldsymbol{x}_0 + \begin{bmatrix} A^{n-1}B & A^{n-2}B & \cdots & B \end{bmatrix}
        \begin{bmatrix} \boldsymbol{u}_0 \\ \boldsymbol{u}_1 \\ \vdots \\ \boldsymbol{u}_{n-1} \end{bmatrix} \\
\end{aligned}
これらをすべてまとめて以下の行列とする。

\begin{bmatrix}
    \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \\ \boldsymbol{x}_3 \\ \vdots \\ \boldsymbol{x}_n
\end{bmatrix} = \begin{bmatrix}
    A \\ A^2 \\ A^3 \\ \vdots \\ A^n
\end{bmatrix}
\boldsymbol{x}_0 + 
\begin{bmatrix}
    B & 0 & 0 & \cdots & 0 \\
    AB & B & 0 & \cdots & 0 \\
    A^2B & AB & B & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    A^{n-1}B & A^{n-2}B & A^{n-3}B & \cdots & B \\
\end{bmatrix} \begin{bmatrix}
    \boldsymbol{u}_0 \\ \boldsymbol{u}_1 \\ \boldsymbol{u}_2 \\ \vdots \\ \boldsymbol{u}_{n-1}
\end{bmatrix}
また出力の方程式は単にy_k=Cx_kなので

\begin{bmatrix}
    y_1 \\ y_2 \\ y_3 \\ \vdots \\ y_n
\end{bmatrix} = \begin{bmatrix}
    C & 0 & 0 & \cdots & 0 \\
    0 & C & 0 & \cdots & 0 \\
    0 & 0 & C & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    0 & 0 & 0 & \cdots & C \\
\end{bmatrix}
\begin{bmatrix}
    \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \\ \boldsymbol{x}_3 \\ \vdots \\ \boldsymbol{x}_n
\end{bmatrix}
これらをまとめて以下のように表現する。

\begin{aligned}
X &= F\boldsymbol{x}_0 + GU \\
Y &= HX \\
\end{aligned}
これはnステップ先までの出力Yが入力列Uによって表現できたことを意味する。

\begin{aligned}
Y &= H(F\boldsymbol{x}_0 + GU) \\
    &= HF\boldsymbol{x}_0 + HGU \\
\end{aligned}

評価関数

最適な入力列 $U$ を決めることによって、目標となる軌道（参照軌道） $Y_\mathrm{ref}$ 　に追従させたい。

評価関数(目的関数) $J$ は一般に以下の二次形式で表現できる。

J = (Y - Y_\mathrm{ref})^\mathsf{T}Q(Y - Y_\mathrm{ref}) + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref})

ここで $Y - Y_\mathrm{ref},U - U_\mathrm{ref}$ はそれぞれ参照軌道 $Y_\mathrm{ref}$ あるいは参照入力 $U_\mathrm{ref}$ に対する追従誤差を意味する。出力 $Y$ や入力 $U$ が参照軌道,参照入力から離れすぎないようにする（過大とならないようにする）ものとなる。
(例：制御入力を最小にする場合、 $U_\mathrm{ref}=0$ )
また $Q,R$ は軌道追従誤差と制御入力の誤差それぞれの重み付けを行う半正定値対称行列とする。
( $A$ が半正定値対称行列である. $\Leftrightarrow$ $A \geq 0$ . $\Leftrightarrow$ $A$ のすべての固有値 $\lambda$ が非負.)

評価関数 $J$ が最小となるような $U$ を決定することで最適な制御入力が決定できる。

入力 $U$ による関数として評価関数 $J$ を表現する。 $Y,X$ を代入して

\begin{aligned} J(U) &= (HX - Y_\mathrm{ref})^\mathsf{T}Q(HX - Y_\mathrm{ref}) + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref}) \\ &= \{ H(F\boldsymbol{x}_0 + GU) - Y_\mathrm{ref} \}^\mathsf{T}Q\{H(F\boldsymbol{x}_0 + GU) - Y_\mathrm{ref}\} + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref}) \\ &= U^\mathsf{T}(G^\mathsf{T}H^\mathsf{T}QHG + R)U + 2\{(HF\boldsymbol{x}_0 - Y_\mathrm{ref})^\mathsf{T}QHG -U_\mathrm{ref}^\mathsf{T}R \}U + \text{(定数項)} \\ \end{aligned}

詳細な変換

$Q,R$ は対称行列なため、

\begin{aligned} Q = Q^T \\ R = R^T \\ \end{aligned}

また転置の性質より

\begin{aligned} C &= A^T B \\ C^T &= (A^T B)^T \\ &= B^T (A^T)^T \\ &= B^T A \\ \end{aligned} \\

ここで評価関数 $J(U)$ はスカラーなので、 $C=C^T \Leftrightarrow A^T B = B^T A$

\begin{aligned} \therefore \ U^TRU_\mathrm{ref} &= U^TR^TU_\mathrm{ref} \\ &= (RU)^TU_\mathrm{ref} \\ &= U_\mathrm{ref}^TRU \\ \end{aligned} \\

\begin{aligned} U^T G^T H^T Q(HF x_0 - Y_\mathrm{ref}) &= U^T G^T H^T Q^T(HF x_0 - Y_\mathrm{ref}) \\ &= (QHGU)^T(HF x_0 - Y_\mathrm{ref}) \\ &= (HF x_0 - Y_\mathrm{ref})^T QHGU \end{aligned}

\begin{aligned} J(U) &= (HX - Y_\mathrm{ref})^\mathsf{T}Q(HX - Y_\mathrm{ref}) + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref}) \\ &= \{ H(F\boldsymbol{x}_0 + GU) - Y_\mathrm{ref} \}^\mathsf{T}Q\{H(F\boldsymbol{x}_0 + GU) - Y_\mathrm{ref}\} + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref}) \\ &= (HF\boldsymbol{x}_0 + HGU - Y_\mathrm{ref})^\mathsf{T}Q(HF\boldsymbol{x}_0 + HGU - Y_\mathrm{ref}) + (U - U_\mathrm{ref})^\mathsf{T}R(U - U_\mathrm{ref}) \\ &= (\boldsymbol{x}_0^\mathsf{T}F^\mathsf{T}H^\mathsf{T} + U^\mathsf{T}G^\mathsf{T}H^\mathsf{T} - Y_\mathrm{ref}^\mathsf{T})Q(HF\boldsymbol{x}_0 + HGU - Y_\mathrm{ref}) + (U^\mathsf{T} - U_\mathrm{ref}^\mathsf{T})R(U - U_\mathrm{ref}) \\ &= U^\mathsf{T}G^\mathsf{T}H^\mathsf{T}QHGU + (\boldsymbol{x}_0^\mathsf{T}F^\mathsf{T}H^\mathsf{T}-Y_\mathrm{ref}^\mathsf{T})QHGU + U^\mathsf{T}G^\mathsf{T}H^\mathsf{T}Q(HF\boldsymbol{x}_0 -Y_\mathrm{ref}) + U^\mathsf{T}RU - U^\mathsf{T}RU_\mathrm{ref} -U_\mathrm{ref}^\mathsf{T}RU + \text{(定数項)} \\ &= U^\mathsf{T}(G^\mathsf{T}H^\mathsf{T}QHG + R)U + \{(\boldsymbol{x}_0^\mathsf{T}F^\mathsf{T}H^\mathsf{T}-Y_\mathrm{ref}^\mathsf{T})QHG -U_\mathrm{ref}^\mathsf{T}R \}U + U^\mathsf{T}\{G^\mathsf{T}H^\mathsf{T}Q(HF\boldsymbol{x}_0 - Y_\mathrm{ref}) - RU_\mathrm{ref}\} + \text{(定数項)} \\ &= U^\mathsf{T}(G^\mathsf{T}H^\mathsf{T}QHG + R)U + 2\{(HF\boldsymbol{x}_0 - Y_\mathrm{ref})^\mathsf{T}QHG -U_\mathrm{ref}^\mathsf{T}R \}U + \text{(定数項)} \\ \end{aligned}

これを微分して $\frac{dJ}{dU}=0$ となる最適な入力 $U^*$ を求める。

\begin{aligned} J(U) &= U^\mathsf{T}(G^\mathsf{T}H^\mathsf{T}QHG + R)U + 2\{(HF\boldsymbol{x}_0 - Y_\mathrm{ref})^\mathsf{T}QHG -U_\mathrm{ref}^\mathsf{T}R \}U + \text{(定数項)} \\ &=U^\mathsf{T}MU + 2NU + \text{(定数項)} \\ &\phantom{={}}(\because M:=G^\mathsf{T}H^\mathsf{T}QHG + R, N:= (HF\boldsymbol{x}_0 - Y_\mathrm{ref})^\mathsf{T}QHG -U_\mathrm{ref}^\mathsf{T}R \ ) \\ \frac{dJ}{dU} &= U^T(M+M^T) + 2N \\ &= 2U^TM + 2N \\ &\phantom{={}}(\because Mは対称行列) \\ &= 0 \\ U^* &= -M^{-1}N^T \\ &= -(G^TH^TQHG+R)^{-1}\{(x_0FH-Y_\mathrm{ref})^TQHG -U_\mathrm{ref}^TR\}^T \end{aligned}

これによって $n$ ステップ先までの最適な制御入力列 $U^*$ が定まった。
MPCではこの入力のうちの最初の要素のみを毎回使用するため、今使用する制御入力は

u = U^*(0)

なおこの制御入力列を適用したときの状態の遷移は以下の通り予測できる。

X = F\boldsymbol{x}_0 + GU^* \\

 最適化問題のソルバに勾配を入力する場合

X := \begin{bmatrix}
    x_0 \\ x_1 \\ x_2 \\ \vdots \\ x_{n-1}
\end{bmatrix}, 
F := \begin{bmatrix}
    A \\ A^2 \\ A^3 \\ \vdots \\ A^n
\end{bmatrix}, 
U := \begin{bmatrix}
    u_0 \\ u_1 \\ u_2 \\ \vdots \\ u_{n-1}
\end{bmatrix}, \\
Q := \begin{bmatrix}
    R & 0 & 0 & \cdots 0 \\
    0 & R & 0 & \cdots 0 \\
    0 & 0 & R & \cdots 0 \\
    \vdots & \vdots & \vdots & \ddots \vdots \\
    0 & 0 & 0 & \cdots R \\
\end{bmatrix}, \\

G := \begin{bmatrix}
    B & 0 & 0 & \cdots & 0 \\
    AB & B & 0 & \cdots & 0 \\
    A^2B & AB & B & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    A^{n-1}B & A^{n-2}B & A^{n-3}B & \cdots & B \\
\end{bmatrix}, \\


\begin{aligned}
x_{\tau+1} &= A x_\tau + B u_\tau \\
X &= F x_0 + GU \\
c(x_\tau) &= {x_\tau}^T R x_\tau\\
J(U) &= \sum_{\tau=0}^{T-1} c(x_\tau) \\
    &= X^T Q X \\
    &= (F x_0 + GU)^T Q (F x_0 + GU) \\
    &= ({x_0}^TF^T + U^TG^T) Q (F x_0 + GU) \\
    &= U^TG^TQGU + 2 {x_0}^TF^TQGU + \text{(定数項)} \\
    &= U^TMU + 2NU + \text{(定数項)} \\
\frac{dJ}{dU} &= (M + M^T)U + 2N^T \\
    &= 2MU + 2N^T \\
    &= 2G^TQGU + 2 ({x_0}^TF^TQG)^T \\
    &= 2G^TQGU + 2 G^T Q F {x_0} \\
    &= 2G^TQ(GU + F {x_0}) \\
\end{aligned}


\begin{aligned}
\begin{bmatrix}
    \frac{\partial J}{\partial u_0} \\
    \frac{\partial J}{\partial u_1} \\
    \frac{\partial J}{\partial u_2} \\
    \vdots \\
    \frac{\partial J}{\partial u_{n-1}} \\
\end{bmatrix} &= 2
\begin{bmatrix}
    B & AB & A^2B & \cdots & A^{n-1}B \\
    0 & B & AB & \cdots &  A^{n-2}B \\
    0 & 0 & B & \cdots & A^{n-3}B \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    0 &0 & 0 & \cdots & B \\
\end{bmatrix}
\begin{bmatrix}
    R & 0 & 0 & \cdots & 0 \\
    0 & R & 0 & \cdots & 0 \\
    0 & 0 & R & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    0 & 0 & 0 & \cdots & R \\
\end{bmatrix}
\left(
\begin{bmatrix}
    B & 0 & 0 & \cdots & 0 \\
    AB & B & 0 & \cdots & 0 \\
    A^2B & AB & B & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    A^{n-1}B & A^{n-2}B & A^{n-3}B & \cdots & B \\
\end{bmatrix}
\begin{bmatrix}
    u_0 \\ u_1 \\ u_2 \\ \vdots \\ u_{n-1}
\end{bmatrix}
+
\begin{bmatrix}
    A \\ A^2 \\ A^3 \\ \vdots \\ A^n
\end{bmatrix} x_0
\right)
\end{aligned}

 上記に参照軌道を追加

X_\mathrm{ref} := \begin{bmatrix}
    x_{\mathrm{ref} \mid 0} \\ x_{\mathrm{ref} \mid 1} \\ x_{\mathrm{ref} \mid 2} \\ \vdots \\ x_{\mathrm{ref} \mid n-1}
\end{bmatrix}


\begin{aligned}
c(x_\tau) &= (x_\tau - x_\mathrm{ref})^T R (x_\tau - x_\mathrm{ref}) \\
J(U) &= \sum_{\tau=0}^{T-1} c(x_\tau) \\
    &= (X - X_\mathrm{ref})^T Q (X - X_\mathrm{ref}) \\
    &= (F x_0 + GU - X_\mathrm{ref})^T Q (F x_0 + GU - X_\mathrm{ref}) \\
    &= ({x_0}^TF^T + U^TG^T - {X_\mathrm{ref}}^T) Q (F x_0 + GU - X_\mathrm{ref}) \\
    &= U^TG^TQGU + 2 ({x_0}^TF^T - {X_\mathrm{ref}}^T)QGU + \text{(定数項)} \\
    &= U^TMU + 2NU + \text{(定数項)} \\
\frac{dJ}{dU} &= (M + M^T)U + 2N^T \\
    &= 2MU + 2N^T \\
    &= 2G^TQGU + 2 \{ ({x_0}^TF^T - {X_\mathrm{ref}}^T)QG \}^T \\
    &= 2G^TQGU + 2 G^T Q F {x_0} - 2 G^T Q X_\mathrm{ref} \\
    &= 2G^TQ(GU + F {x_0} - X_\mathrm{ref}) \\
\end{aligned}


\begin{aligned}
\begin{bmatrix}
    \frac{\partial J}{\partial u_0} \\
    \frac{\partial J}{\partial u_1} \\
    \frac{\partial J}{\partial u_2} \\
    \vdots \\
    \frac{\partial J}{\partial u_{n-1}} \\
\end{bmatrix} &= 2
\begin{bmatrix}
    B & AB & A^2B & \cdots & A^{n-1}B \\
    0 & B & AB & \cdots &  A^{n-2}B \\
    0 & 0 & B & \cdots & A^{n-3}B \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    0 &0 & 0 & \cdots & B \\
\end{bmatrix}
\begin{bmatrix}
    R & 0 & 0 & \cdots & 0 \\
    0 & R & 0 & \cdots & 0 \\
    0 & 0 & R & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    0 & 0 & 0 & \cdots & R \\
\end{bmatrix}
\left(
\begin{bmatrix}
    B & 0 & 0 & \cdots & 0 \\
    AB & B & 0 & \cdots & 0 \\
    A^2B & AB & B & \cdots & 0 \\
    \vdots & \vdots & \vdots & \ddots & \vdots \\
    A^{n-1}B & A^{n-2}B & A^{n-3}B & \cdots & B \\
\end{bmatrix}
\begin{bmatrix}
    u_0 \\ u_1 \\ u_2 \\ \vdots \\ u_{n-1}
\end{bmatrix}
+
\begin{bmatrix}
    A \\ A^2 \\ A^3 \\ \vdots \\ A^n
\end{bmatrix} x_0
-
\begin{bmatrix}
    x_{\mathrm{ref} \mid 0} \\ x_{\mathrm{ref} \mid 1} \\ x_{\mathrm{ref} \mid 2} \\ \vdots \\ x_{\mathrm{ref} \mid n-1}
\end{bmatrix}
\right)
\end{aligned}

Rustで数値計算を行うライブラリはいくつかある模様。
https://crates.io/crates/ndarray

RustでMPCを実装してPythonでシミュレーション結果をplotしたい。

 車輪型倒立振子を差動二輪として動かす状態変数を\boldsymbol{x} = [x, \dot{x}, y, \dot{y}, \phi, \dot{\phi}, \theta, \dot{\theta}]^\mathsf{T}とする。

x：車輪のx変位、y：車輪のy変位、\phi：yaw角、\theta：鉛直上向きに対するx軸方向への振り子の傾き（pitch角）。

左右のモーターそれぞれに発生させるトルクをT_1,T_2とする。
地面・タイヤ間の摩擦力をそれぞれf_1, f_2として、各ホイールの角運動方程式

\begin{aligned}
J_{w1} \ddot{\theta}_{w1} &= T_1 - f_1 r \\
J_{w2} \ddot{\theta}_{w2} &= T_2 - f_2 r \\
\end{aligned}
ホイールベースをdとして

d\phi = r\theta_{w1} - r\theta_{w2}  

 坂道対応坂の角度を\alphaとして、状態変数は\boldsymbol{x} = [x, \dot{x}, \theta, \dot{\theta}]^\mathsf{T}。（\alphaの推定が必要となる。）

Rustで行列演算を行う

Rustには行列演算を行う有名なcrateが2つある。
ndarrayはヒープに値を持つため組み込み向きではない。

よってNalgebraを使用する。

Cargo.toml

[dependencies]
nalgebra = "0.32.5"

公式の推奨より以下のエイリアスを定義。

extern crate nalgebra as na;

行列は以下で与えられている。ここでTは要素の型。Rは行数、Cは列数を表す。

na::MatrixMN<T, R, C>

エイリアスが定義されているので通常使用する分にはこれを使えばよさそう。

let m = Matrix3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
println!("A = {}", m);

let m = Matrix3::from_fn(|r, c| 10 * r + c);
println!("B = {}", m);

let m = m.fixed_view::<2, 2>(1, 0).clone_owned();
println!("C = {}", m);

マクロを使用すればさらに簡単に構築できる。

use nalgebra::matrix;

// Produces a Matrix3<_> == SMatrix<_, 3, 3>
let a = matrix![1, 2, 3;
                4, 5, 6;
                7, 8, 9];

一般的な行列演算を行える。v1,v2をベクトル、m1,m2を行列とする。

let _ = 2.0 * v1 + 3.0 * v2;
let _ = 4.0 * m1 + 5.0 * m2;
let _ = m1 * v1 + m2 * v2;

print!も可能。

println!("{}", v1);
println!("{}", m1);

制約付き最適化問題はopt_engineで解けそう。^[1]

使用手順

最適化したい関数fとその関数の勾配dfを用意する。
制約を用意する。
関数・勾配・制約をもとに最適化問題を生成。
solve()で最適化問題を解き、最適な入力を取り出す。

雑に $f(u_1, u_2) = u_1^2 + u_2^2$ の最適化問題を解いてみる。

use optimization_engine::{panoc::*, *};

fn f(u: &[f64], c: &mut f64) -> Result<(), SolverError> {
    *c = u[0].powi(2) + u[1].powi(2);
    Ok(())
}

fn df(u: &[f64], grad: &mut [f64]) -> Result<(), SolverError> {
    grad[0] = 2.0 * u[0];
    grad[1] = 2.0 * u[1];
    Ok(())
}

fn main() {
    let tolerance = 1e-6;
    let n = 2;
    let lbfgs_memory = 10;
    let max_iters = 200;
    let mut u = [0.0, 0.0];
    let radius = 1.0;

    // the cache is created only ONCE
    let mut panoc_cache = PANOCCache::new(n, tolerance, lbfgs_memory);

    // define the bounds at every iteration
    let bounds = constraints::Ball2::new(None, radius);

    // the problem definition is updated at every iteration
    let problem = Problem::new(&bounds, df, f);

    // updated instance of the solver
    let mut panoc = PANOCOptimizer::new(problem, &mut panoc_cache).with_max_iter(max_iters);

    let status = panoc.solve(&mut u).unwrap();

    // print useful information
    println!(
        "parameters: (r={:.4}), iters = {}",
        radius,
        status.iterations()
    );
    println!("u = {:#.6?}", u);
}

脚注

https://alphaville.github.io/optimization-engine/docs/openrust-basic ↩︎

OpEnでMPCを動かしてみる

OpEnを使用して最適化問題を解くには目的関数とその勾配が必要。
勾配は数値微分によって求めることとする。

系ダイナミクスの数値積分の誤差が気になる場合はルンゲクッタ。

mppiも使ってみたい

 線形近似せずに状態遷移関数を求める以下を式①から式④とする。

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} - H \\
J \ddot{\theta} &= Vl \sin\theta - Hl \cos\theta \\
H &= M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
V &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) \\
\end{aligned}
式①に式③を代入し、式⑤を得る。

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} - M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
(m+M+\frac{J_w}{r^2})\ddot{x} + Ml\ddot{\theta}\cos\theta &= \frac{T}{r} + Ml\dot{\theta}^2\sin\theta \\
\end{aligned}
式②に式③,④を代入し、式⑥を得る。

\begin{aligned}
J \ddot{\theta} &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta)l \sin\theta - M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta)l \cos\theta \\
M\ddot{x}l\cos\theta +J \ddot{\theta} &= Mgl\sin\theta + Ml^2\sin\theta(-\ddot{\theta}\sin\theta - \dot{\theta}^2\cos\theta) + Ml^2\cos\theta(-\ddot{\theta}\cos\theta + \dot{\theta}^2\sin\theta) \\
    &= Mgl\sin\theta - Ml^2(\ddot{\theta}\sin^2\theta + \ddot{\theta}\cos^2\theta) + Ml^2(-\dot{\theta}^2\sin\theta\cos\theta + \dot{\theta}^2\sin\theta\cos\theta) \\
    &= Mgl\sin\theta - Ml^2\ddot{\theta} \\
M\ddot{x}l\cos\theta +(J + Ml^2)\ddot{\theta} &= Mgl\sin\theta \\
\end{aligned}
すなわち下記が式⑤、式⑥である。

\begin{aligned}
(m+M+\frac{J_w}{r^2})\ddot{x} + Ml\ddot{\theta}\cos\theta &= \frac{T}{r} + Ml\dot{\theta}^2\sin\theta \\
M\ddot{x}l\cos\theta +(J + Ml^2)\ddot{\theta} &= Mgl\sin\theta \\
\end{aligned}
(J + Ml^2)⑤ - Ml\cos\theta⑥より\ddot{x}を示す。

\begin{aligned}
(m+M+\frac{J_w}{r^2})(J + Ml^2)\ddot{x} + Ml(J + Ml^2)\ddot{\theta}\cos\theta &= (J + Ml^2)(\frac{T}{r} + Ml\dot{\theta}^2\sin\theta) \\
M^2l^2\ddot{x}\cos^2\theta +Ml(J + Ml^2)\ddot{\theta}\cos\theta &= M^2gl^2\sin\theta\cos\theta \\
\{ (m+M+\frac{J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta \}\ddot{x} &= (J + Ml^2)(\frac{T}{r} + Ml\dot{\theta}^2\sin\theta) - M^2gl^2\sin\theta\cos\theta \\
\end{aligned} \\

\ddot{x} = \frac{ (J + Ml^2)(\frac{T}{r} + Ml\dot{\theta}^2\sin\theta) - M^2gl^2\sin\theta\cos\theta }{ (m+M+\frac{J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta } \\
Ml\cos\theta⑤ - (m+M+\frac{J_w}{r^2})⑥より\ddot{\theta}を示す。

\begin{aligned}
(m+M+\frac{J_w}{r^2})Ml\cos\theta\ddot{x} + M^2l^2\cos^2\theta\ddot{\theta} &= (\frac{T}{r} + Ml\dot{\theta}^2\sin\theta)Ml\cos\theta \\
(m+M+\frac{J_w}{r^2})Ml\cos\theta\ddot{x} +(m+M+\frac{J_w}{r^2})(J + Ml^2)\ddot{\theta} &= (m+M+\frac{J_w}{r^2})Mgl\sin\theta \\
\{ M^2l^2\cos^2\theta - (m+M+\frac{J_w}{r^2})(J + Ml^2) \} \ddot{\theta} &= (\frac{T}{r} + Ml\dot{\theta}^2\sin\theta)Ml\cos\theta - (m+M+\frac{J_w}{r^2})Mgl\sin\theta \\
\end{aligned} \\

\ddot{\theta} = \frac{ (m+M+\frac{J_w}{r^2})Mgl\sin\theta - (\frac{T}{r} + Ml\dot{\theta}^2\sin\theta)Ml\cos\theta }{ (m+M+\frac{J_w}{r^2})(J + Ml^2) - M^2l^2\cos^2\theta }
すなわち

\begin{aligned}
\ddot{x} &= \frac{ (J + Ml^2)(\frac{T}{r} + Ml\dot{\theta}^2\sin\theta) - M^2gl^2\sin\theta\cos\theta }{ (m+M+\frac{J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta } \\

\ddot{\theta} &= \frac{ (m+M+\frac{J_w}{r^2})Mgl\sin\theta - (\frac{T}{r} + Ml\dot{\theta}^2\sin\theta)Ml\cos\theta }{ (m+M+\frac{J_w}{r^2})(J + Ml^2) - M^2l^2\cos^2\theta }
\end{aligned}

 反力Hが左右の車輪で等分される場合振り子が受ける力をH,Vとして以下を式①から式④とする。

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} -  \frac{H}{2} \\
J \ddot{\theta} &= Vl \sin\theta - Hl \cos\theta \\
H &= M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
V &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) \\
\end{aligned}
式①に式③を代入し、式⑤を得る。

(m+\frac{J_w}{r^2})\ddot{x} = \frac{T}{r} - \frac{M}{2} (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) \\
(m+\frac{M}{2}+\frac{J_w}{r^2})\ddot{x} + \frac{Ml}{2}\ddot{\theta}\cos\theta = \frac{T}{r} + \frac{Ml}{2}\dot{\theta}^2\sin\theta \\
\therefore\; (2m+M+\frac{2J_w}{r^2})\ddot{x} +Ml\ddot{\theta}\cos\theta = \frac{2T}{r} +Ml\dot{\theta}^2\sin\theta
式②に式③,④を代入し、式⑥を得る。

\begin{aligned}
J \ddot{\theta} &= M(g -l\ddot{\theta}\sin\theta - \cancel{l\dot{\theta}^2\cos\theta})l \sin\theta \\
    &\phantom{={}} -M(\ddot{x} + l\ddot{\theta}\cos\theta - \cancel{l\dot{\theta}^2\sin\theta})l \cos\theta \\
    &= Mgl\sin\theta -Ml\ddot{x}\cos\theta -Ml^2(\cancel{\sin^2\theta+\cos^2\theta}) \\
    &= Mgl\sin\theta -Ml\ddot{x}\cos\theta -Ml^2\ddot{\theta} \\
\end{aligned}
\\
\therefore\; Ml\ddot{x}\cos\theta +(J + Ml^2)\ddot{\theta} = Mgl\sin\theta
すなわち下記が式⑤、式⑥である。

\begin{aligned}
(2m+M+\frac{2J_w}{r^2})\ddot{x} +Ml\ddot{\theta}\cos\theta &= Ml\dot{\theta}^2\sin\theta +\frac{2T}{r} \\
Ml\ddot{x}\cos\theta +(J + Ml^2)\ddot{\theta} &= Mgl\sin\theta
\end{aligned}
(J+Ml^2)⑤ - Ml\cos\theta⑥より\ddot{\theta}を消去して\ddot{x}を示す。

\begin{aligned}
(2m+M+\frac{2J_w}{r^2})(J + Ml^2)\ddot{x} + \cancel{Ml(J + Ml^2)\ddot{\theta}\cos\theta} &= (J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) \\
M^2l^2\ddot{x}\cos^2\theta +\cancel{Ml(J +Ml^2)\ddot{\theta}\cos\theta} &= M^2gl^2\sin\theta\cos\theta \\
\{ (2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta \}\ddot{x} &= (J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta \\
\end{aligned} 
\\
\therefore\; \ddot{x} = \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
Ml\cos\theta⑤ - (2m+M+\frac{2J_w}{r^2})⑥より\ddot{x}を消去して\ddot{\theta}を示す。

\begin{aligned}
\cancel{(2m+M+\frac{2J_w}{r^2})Ml\cos\theta\ddot{x}} +M^2l^2\cos^2\theta\ddot{\theta} &= (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta \\
\cancel{(2m+M+\frac{2J_w}{r^2})Ml\cos\theta\ddot{x}} +(2m+M+\frac{2J_w}{r^2})(J + Ml^2)\ddot{\theta} &= (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta \\
\{ M^2l^2\cos^2\theta -(2m+M+\frac{2J_w}{r^2})(J +Ml^2) \} \ddot{\theta} &= (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta - (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta \\
\end{aligned}
\\
\therefore\; \ddot{\theta} = \frac{ (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta - (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
すなわち

\begin{aligned}
\ddot{x} &= \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
\\
\ddot{\theta} &= \frac{ (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta - (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
\end{aligned}

 振り子に外乱を加える振り子に外乱f_dを加えたときの\ddot{x}, \ddot{\theta}を求める。

\begin{aligned}
(m+\frac{J_w}{r^2})\ddot{x} &= \frac{T}{r} -  \frac{H}{2} \\
J \ddot{\theta} &= Vl \sin\theta - Hl \cos\theta - f_dl \\
M\ddot{x}_p &= H - f_d\cos\theta \\
M\ddot{z}_p &= V - Mg + f_d\sin\theta \\
\end{aligned}

\begin{aligned}
H &= M (\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) + f_d\cos\theta \\
V &= M(g -l\ddot{\theta}\sin\theta - l\dot{\theta}^2\cos\theta) - f_d\sin\theta \\
\end{aligned}

\begin{aligned}
(2m+\frac{2J_w}{r^2})\ddot{x} &= \frac{2T}{r} -  H \\
    &= \frac{2T}{r} -  M(\ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) - f_d\cos\theta
\end{aligned}

\begin{aligned}
J \ddot{\theta} &= Vl \sin\theta - Hl \cos\theta - f_dl \\
    &= Mgl\sin\theta -Ml\ddot{x}\cos\theta -Ml^2\ddot{\theta} - 2f_dl \\
\end{aligned}

\begin{aligned}
(2m+M+\frac{2J_w}{r^2})\ddot{x} +Ml\ddot{\theta}\cos\theta &= Ml\dot{\theta}^2\sin\theta +\frac{2T}{r} - f_d\cos\theta \\
Ml\ddot{x}\cos\theta +(J + Ml^2)\ddot{\theta} &= Mgl\sin\theta - 2f_dl
\end{aligned}
(J+Ml^2)⑤ - Ml\cos\theta⑥より\ddot{\theta}を消去して\ddot{x}を示す。

\begin{aligned}
(2m+M+\frac{2J_w}{r^2})(J + Ml^2)\ddot{x} + \cancel{Ml(J + Ml^2)\ddot{\theta}\cos\theta} &= (J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}-f_d\cos\theta) \\
M^2l^2\ddot{x}\cos^2\theta +\cancel{Ml(J +Ml^2)\ddot{\theta}\cos\theta} &= M^2gl^2\sin\theta\cos\theta - 2Ml^2f_d\cos\theta \\
\{ (2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta \}\ddot{x} &= (J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}+f_d\cos\theta) -M^2gl^2\sin\theta\cos\theta \\
\end{aligned} 
\\
\therefore\; \ddot{x} = \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}+f_d\cos\theta) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
Ml\cos\theta⑤ - (2m+M+\frac{2J_w}{r^2})⑥より\ddot{x}を消去して\ddot{\theta}を示す。

\begin{aligned}
\cancel{(2m+M+\frac{2J_w}{r^2})Ml\cos\theta\ddot{x}} +M^2l^2\cos^2\theta\ddot{\theta} &= (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r} - f_d\cos\theta)Ml\cos\theta \\
\cancel{(2m+M+\frac{2J_w}{r^2})Ml\cos\theta\ddot{x}} +(2m+M+\frac{2J_w}{r^2})(J + Ml^2)\ddot{\theta} &= (2m+M+\frac{2J_w}{r^2})(Mgl\sin\theta-2f_dl) \\
\{ M^2l^2\cos^2\theta -(2m+M+\frac{2J_w}{r^2})(J +Ml^2) \} \ddot{\theta} &= (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}-f_d\cos\theta)Ml\cos\theta - (2m+M+\frac{2J_w}{r^2})(Mgl\sin\theta-2f_dl) \\
\end{aligned}
\\
\therefore\; \ddot{\theta} = \frac{ (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}-f_d\cos\theta)Ml\cos\theta - (2m+M+\frac{2J_w}{r^2})(Mgl\sin\theta-2f_dl) }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
すなわち

\begin{aligned}
\ddot{x} &= \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}+f_d\cos\theta) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
\\
\ddot{\theta} &= \frac{ (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}-f_d\cos\theta)Ml\cos\theta - (2m+M+\frac{2J_w}{r^2})(Mgl\sin\theta-2f_dl) }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
\end{aligned}

 ジャイロセンサを状態推定に用いる#反力H、Vを近似しない場合 より振り子の加速度\ddot{x}_p, \ddot{z}_pは以下の通り。

\begin{aligned}
\ddot{x}_p &= \ddot{x} + l\ddot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta \\
\ddot{z}_p &= -l\ddot{\theta} \sin\theta - l\dot{\theta}^2\cos\theta \\
\end{aligned}
重力加速度gと加速度\ddot{x}_p, \ddot{z}_pがジャイロセンサに観測される。

ジャイロセンサの観測値を(a_x, a_z)としてジャイロセンサの座標系に変換する。

\begin{pmatrix}
    a_x \\ a_z
\end{pmatrix} = \begin{pmatrix}
    \cos\theta & \sin\theta \\
    -\sin\theta & \cos\theta \\
\end{pmatrix} \begin{pmatrix}
    0 \\ g
\end{pmatrix} +
\begin{pmatrix}
    \cos\theta & -\sin\theta \\
    -\sin\theta & -\cos\theta \\
\end{pmatrix} \begin{pmatrix}
    \ddot{x}_p \\ \ddot{z}_p
\end{pmatrix}
すなわち

\begin{aligned}
a_x &= g\sin\theta + \ddot{x}_p\cos\theta - \ddot{z}_p\sin\theta \\
    &= g\sin\theta + (\ddot{x} + l\ddot{\theta}\cos\theta - \cancel{ l\dot{\theta}^2\sin\theta})\cos\theta - (-l\ddot{\theta} \sin\theta - \cancel{l\dot{\theta}^2\cos\theta})\sin\theta \\
    &= g\sin\theta + \ddot{x}\cos\theta + l\ddot{\theta}(\cancel{\cos^2\theta + \sin^2\theta}) \\
    &= g\sin\theta + \ddot{x}\cos\theta + l\ddot{\theta} \\
a_z &= g\cos\theta -\ddot{x}_p\sin\theta - \ddot{z}_p\cos\theta \\
    &= g\cos\theta -(\ddot{x} + \cancel{l\ddot{\theta}\cos\theta} - l\dot{\theta}^2\sin\theta)\sin\theta -(-\cancel{l\ddot{\theta}\sin\theta} - l\dot{\theta}^2\cos\theta)\cos\theta \\
    &= g\cos\theta - \ddot{x}\sin\theta + l{\dot{\theta}}^2(\cancel{\sin^2\theta + \cos^2\theta}) \\
    &= g\cos\theta - \ddot{x}\sin\theta + l{\dot{\theta}}^2 \\
\end{aligned}

 状態遷移関数をオイラー法で定める
\begin{aligned}
\ddot{x} &= \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
\\
\ddot{\theta} &= \frac{ (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta - (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
\end{aligned}

\begin{aligned}
x_{k+1} &= x_k + \dot{x}_k \Delta t \\
\dot{x}_{k+1} &= \dot{x}_k + \ddot{x}_k \Delta t \\
\ddot{x}_{k+1} &= \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta}
\\
\theta_{k+1} &= \theta_k + \dot{\theta}_k \Delta t \\
\dot{\theta}_{k+1} &= \dot{\theta}_k + \ddot{\theta}_k \Delta t \\
\ddot{\theta}_{k+1} &= \frac{ (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta - (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta }
\end{aligned}

 ルンゲ=クッタ法変数xの一階微分が関数f(x)で表されるとき、d_1, d_2, d_3, d_4を以下の通り定めて重み付き平均することにより、x_k:=x(t)から単位ステップ時間\Delta t\mathrm[s]経過後のx_{k+1}:=x(t + \Delta t)を計算する。[1][2][3][4][5][6]

\dot{x}(t) = f(x(t))

x_{k+1} = x_k + \frac{d_1 + 2d_2 + 2d_3 + d_4}{6}

\begin{aligned}
d_1 &= f(x_k) \Delta t \\
d_2 &= f(x_k + d_1 / 2) \Delta t \\
d_3 &= f(x_k + d_2 / 2) \Delta t \\
d_4 &= f(x_k + d_3) \Delta t
\end{aligned}

 状態方程式をルンゲ=クッタ法で数値計算する\bm{x} = (x, \dot{x}, \theta, \dot{\theta})^\mathsf{T}とすると\dot{\bm{x}} = f(\bm{x}, u, \Delta t)は

\begin{aligned}
\bm{x}_k^0 = \dot{x}_{k+1} &= \dot{x}_k +  \ddot{x}_k\Delta t \\
\bm{x}_k^1 = \ddot{x}_{k+1} &= \frac{(J + Ml^2)(Ml\dot{\theta}^2\sin\theta +\frac{2T}{r}) -M^2gl^2\sin\theta\cos\theta}{(2m+M+\frac{2J_w}{r^2})(J +Ml^2) -M^2l^2\cos^2\theta} \\
\bm{x}_k^2 = \dot{\theta}_{k+1} &= \dot{\theta}_k + \ddot{\theta}_k \Delta t \\
\bm{x}_k^3 = \ddot{\theta}_{k+1} &= \frac{ (2m+M+\frac{2J_w}{r^2})Mgl\sin\theta - (Ml\dot{\theta}^2\sin\theta +\frac{2T}{r})Ml\cos\theta }{ (2m+M+\frac{2J_w}{r^2})(J + Ml^2) -M^2l^2\cos^2\theta } \\
\end{aligned}
これを代入して

\bm{x}_{k+1} = \bm{x}_k + \frac{\bm{d_1} + 2\bm{d_2} + 2\bm{d_3} + \bm{d_4}}{6}

\begin{aligned}
\bm{d_1} &= f(\bm{x_k}, 0) \Delta t \\
\bm{d_2} &= f(\bm{x_k} + d_1 / 2, \Delta t / 2) \Delta t \\
\bm{d_3} &= f(\bm{x_k} + d_2 / 2, \Delta t / 2) \Delta t \\
\bm{d_4} &= f(\bm{x_k} + d_3, \Delta t) \Delta t
\end{aligned}

脚注
https://mech-eng-uni.com/matlabで学ぶ-オイラー法とルンゲクッタ法/ ↩︎
https://sites.google.com/site/dueyama/home-jp/python_simulation/4 ↩︎
https://ja.wikipedia.org/wiki/ルンゲ＝クッタ法 ↩︎
http://nalab.mind.meiji.ac.jp/~mk/labo/text/num-ode.pdf ↩︎
https://sites.google.com/site/dueyama/home-jp/python_simulation/4 ↩︎
https://vigne-cla.com/5-8/ ↩︎

このスクラップは4ヶ月前にクローズされました