Tanuki_Bayashin’s diary

電子工作を趣味としています。最近はラズベリーパイPicoというマイコンを使って楽しんでいます

運動方程式の性質　その２

力学・ロボット関連 倒立振子

※なにか気になる点がありましたらコメント欄にご記入ください。
【目次】

１．はじめに

学生時代 Slotine と言う人の論文を読み（読まされ？）、その中に以下のような定理（？）が現れました。

（定理）
機械系の運動方程式において、その機械のパラメータは運動方程式に関して線形である。

「線形である」というのは、ここではその機械のパラメータを要素とするベクトルを考えたとき、上でいう運動方程式を、数式で表現された行列とパラメータのベクトルの積で表せるという意味です。
（本文にて解説します）

今回は、今自分が取り組んでいる倒立振子の運動方程式を例として、上の定理の説明をしていきたいと思います。

２．定理の解説と運動方程式による実例

２．１　１リンクロボットによる解説

まず、１リンクロボット（？）にて解説します。

図１　１リンクロボット（イメージ）

１リンクなので、運動方程式は


\begin{aligned} &J \, \ddot \theta + d \, \dot \theta + mgl \,\mathrm {sin} \theta = \tau \quad \quad \quad (1) \\ 　\\ ただし、\\ J&：リンクの慣性モーメント [ \mathrm {Kg m^2} ] \\ d&：モーター軸の粘性摩擦係数 [ \mathrm {Nm \, sec} ] \\ m&：リンクの質量 [ \mathrm {Kg} ] \\ g&：重力加速度 [ \mathrm {m/s^2} ] \\ l&：モーター軸からリンクの重心までの距離 [ \mathrm {m} ] \\ \theta &：リンクの回転角度 [ \mathrm {rad} ] \\ \tau&：モーターからリンクに働くトルク [ \mathrm {Nm} ] \\ \end{aligned}

となります。（変数の正負は適当です）このとき、行列 \textbf {Y}(\ddot \theta , \dot \theta, \theta )を


\begin{aligned} \textbf{Y}(\ddot \theta, \dot \theta, \theta ) = \left [ \begin{array}{l} \ddot \theta \quad \dot \theta \quad \mathrm {sin}\theta \end{array} \right ] \quad \quad \quad (2) \\ \end{aligned}

とおき、またパラメータを要素とするベクトル \textbf {a}を

\begin{aligned} \textbf{a} &= \left [ \begin{array}{l} a_0 \\ a_1 \\ a_2 \end{array} \right ] \\ &= \left [ \begin{array}{l} \,m \\ \,d \\ mgl \end{array} \right ] \quad \quad \quad (3) \end{aligned}

としたとき、式（１）の左辺は以下のように表されます。

\begin{aligned} \quad \quad \quad \mathrm{(式（１）の左辺)}= \textbf {Y} \textbf {a} \quad \quad \quad (4) \end{aligned}

１章の定理が示す内容としては、運動方程式において、（４）式のようにパラメータのベクトル \textbf {a}が、うまく各要素 a_i を取ることにより、行列 \textbf {Y}との積で表されるということです。

要は運動方程式中に出てくる各項にて、変数に当たる部分とパラメータとしてくくってしまう部分で、別々に分けて表現すると言ったイメージです。
（ここで出てきたパラメータのベクトルを推定して、適応制御に活かそうというのが、Slotine氏の論文の要となってくる部分だと筆者は解釈しています）

２．２　倒立振子の場合

まず運動方程式を以下に示します。


\begin{aligned} &M( \theta ) \ddot \theta + C(\dot \theta , \theta) \dot \theta + D( \dot \theta ) + G( \theta ) = \tau \quad \quad (5) \\ \\ &但し、\\ &M(\theta) ：慣性行列 　　　　　　　C(\dot \theta , \theta) ： 遠心力、コリオリ力に関する行列 \\ &D( \dot \theta )：摩擦に関する項（静止摩擦力はここでは除外）　　G( \theta )： 重力項 \\ &\theta ：角度　 \dot \theta：角速度　 \ddot \theta ：角加速度\\ & \tau ：トルク\\ \end{aligned}

このとき、１輪車型の倒立振子（写真１）の運動方程式では、

写真１　自作した倒立振子（注：現時点で倒立できていません）


\begin{aligned} M( \theta ) &= \left [ \begin{array}{l} \alpha + 2 \beta \cos \theta _p \quad \gamma + &\beta \cos \theta _p \\ \gamma + \beta \cos \theta _p \quad &\gamma \end{array} \right ] \\ C(\dot \theta , \theta) &= \left [ \begin{array}{l} {-} \beta \sin \theta _p \cdot \dot \theta _p &0 \\ {-} \beta \sin \theta _p \cdot \dot \theta _p &0 \end{array} \right ] \\ 但し、\\ \alpha &= (m_w + m_p) r_w^2 + m_p r_p^2 + I_p + I_w \\ \beta &= m_p r_w r_p \\ \gamma &= (m_w + m_p)r_w^2 + I_w \\ \\ ここで、\\ m_w &：車輪の質量 [ \mathrm {Kg} ] \\ m_p &：本体の質量 [ \mathrm {Kg} ] \\ r_w &：車輪の回転軸から車輪の重心までの長さ [ \mathrm m ] \\ r_p &：車輪の回転軸から本体の重心までの長さ [ \mathrm m ] \\ I_w &：車輪の回転軸まわりの車輪の慣性モーメント [ \mathrm {Kg m^2} ] \\ I_p &：車輪の回転軸まわりの本体の慣性モーメント [ \mathrm {Kg m^2} ] \\ \theta _p &：本体の傾斜角 \left [ \mathrm rad \right ] \\ \dot \theta _p &： 本体の角速度 \left [ \mathrm rad \right ] \\ \end{aligned}

であることが導かれています。（トランジスタ技術２０１９年７月号）

（この辺りまでは、私のブログ記事「運動方程式の性質　その１」の2章のはじまりと同じです）

また、摩擦に関する項 D( \dot \theta )は


\begin{aligned} D( \dot \theta ) &= \left [ \begin{array}{l} 0 \\ d_1 \dot \theta_w + d_2 \,\mathrm {sgn} ( \dot \theta_w ) \\ \end{array} \right ] \\ 但し、\\ d1&：粘性摩擦係数 [ \mathrm{Nm \, sec}] \\ d2&：動摩擦力 \mathrm {[Nm]} \\ また、\\ &\begin{eqnarray} \mathrm {sgn} (x) = \left\{ \begin{array}{l} 1 \quad x > 0 \\ 0 \quad x = 0 \\ -1 \, \quad x < 0 \end{array} \right. \end{eqnarray} \end{aligned}

として表されます。（静止摩擦の項は、記述しにくいので省略しました）

続いて重力項 G( \theta ) は、


\begin{aligned} G( \theta ) &= \left [ \begin{array}{l} - \delta \, \mathrm {sin} \, \theta_p　\\ 　0 \end{array} \right ] \\ 　\\ ただし、 \delta &= m_p \,g \,r_p とする\\ \end{aligned}

と表されます。

以上をまとめると、


\begin{aligned} \textbf {a} &= \left [ \begin{array}{l} \alpha \\ \beta \\ \gamma \\ \delta \\d_1 \\d_2 \end{array} \right ] \\ 　\\ \textbf {Y}( \ddot \theta , \dot \theta ,\theta ) &= \left [ \begin{array}{l} \ddot \theta_p &(2 \ddot \theta_p + \ddot \theta_w)\,\mathrm {cos}\,\theta_p - {\dot \theta_p}^2\, \mathrm {sin} \, \theta_p &　\ddot \theta_w & - \mathrm {sin} \, \theta_p &0 &　0　\\ 0 &　　\ddot \theta_p\, \mathrm {cos}\,\theta_p - {\dot \theta_p}^2\, \mathrm {sin} \, \theta_p, &\ddot \theta_p + \ddot \theta_w &　0 &\dot \theta_w &\mathrm {sgn}(\dot \theta_w) \end{array} \right ] \\ \end{aligned}

とした場合、（５）式の運動方程式は、

\begin{aligned} \quad \quad \quad \mathrm{(式（５）の左辺)}= \textbf {Y} \textbf {a} \quad \quad \quad (6) \end{aligned}

と記述することができます。具体的にこの式を使って適応制御の適応則がどのように記述されるのかは、機会があったら述べたいと思います。

（終わり）