線形倒立振子における両足支持期間の補完関数導出

「自律二足歩行に向けた知識のまとめ」はこちら

梶田先生著書の「ヒューマノイドロボット」に登場する、2足歩行モデルである線形倒立振子において「両足支持期間」という考え方が登場します。

本ではさらっと触れるに留まっており、式の導出は記載されていないため、ここに自分なりに導出した際のメモを残します。

(この記事はまだ書きかけですが、とりあえず公開します。グラフを書くとうまく補間できたようには見えるのですが、間違ってたらごめんなさい)

補間関数の導出

補間関数として、重心の「位置」と「速度」のそれぞれについて関数を導出する。

先に「速度」についての関数を導出した後、これを時間積分することで「位置」の関数を導出する。

また、以下では\( x \) 方向について考えるが、\( y \)方向についても全く同様の方法で計算できる。

重心速度関数の導出

両足支持期間において、前回の歩行素片と次回の歩行素片を滑らかにつなぐ「速度」の補間関数として、以下のような3次関数を用いる。

$$
v(t) = pt^3 + qt^2 + rt + s \tag{1}
$$

ここでの目的は、\(p~s\)の各係数を求める事である。

まず、両足支持の開始時刻を \(t_1\), 終了時刻を \(t_2\) とする。

計算簡略化のため、両足支持期間を \(T_{dbl}\) とし、\(t_1=0\), \(t_2=T_{dbl}\)として\(p~s\)を算出する。

これは、時間方向に\(-t_1\)だけシフトした関数を考えることに等しい。実際に使う場合は、以下で導出する関数を使って補間値を計算したのち、時刻を \(+t_1\) 分だけ戻して扱う(例えば求めた関節角を時系列の配列へ代入する際など)。

また、以下の情報が既知であるとする。
----
前回の歩行素片の終了端 (\(t=0\)) における速度: \( v_c \)、加速度: \( a_1 \)
次回の歩行素片の開始端 (\(t=T_{dbl}\)) における速度: \( v_c \)、加速度: \( a_2 \)
※歩行素片の整合性から、前回終了端と次回開始端の速度はともに同じ速度\( v_c \)
----

では、係数を導出してみる。

まず、式(1) を時間微分することで、加速度についての関数を得る。

$$
\dot{v}(t) = 3pt^2 + 2qt + r
$$

これに \( \dot{v}(0) = a_1 \) を代入し、\( r \) が求まる。

$$
\dot{v}(0) = r = a_1
$$

また、\( \dot{v}(T_{dbl}) = a_2 \) を代入。

$$
\begin{eqnarray}
\dot{v}(T_{dbl}) & = & 3pT_{dbl}^2 + 2qT_{dbl} + r = a_2 \\
\Leftrightarrow q & = & \frac{a_2 - a_1}{2 T_{dbl}} - \frac{3}{2}p T_{dbl} \tag{2}
\end{eqnarray}
$$



次に、 式(1) に \( v(0) = v_c \) を代入し、\( s \) が求まる。

$$
v(0) = s = v_c
$$

また、 \( v(T_{dbl}) = v_c \) を代入。

$$
v(T_{dbl}) = pT_{dbl}^3 + qT_{dbl}^2 + a_1 T_{dbl} + v_c = v_c \\
\Leftrightarrow pT_{dbl}^2 + qT_{dbl} + a_1 = 0
$$

これに 式(2) を代入し、\( p \) が求まる。

$$
\begin{eqnarray}
pT_{dbl}^2 + ( \frac{a_2 - a_1}{2 T_{dbl}} - \frac{3}{2}p T_{dbl} ) T_{dbl} + a_1 & = & 0 \\
\Leftrightarrow p = \frac{a_2 + a_1}{T_{dbl}^2}
\end{eqnarray}
$$

これを 式(2) に代入し、\( q \) が求まる。

$$
\begin{eqnarray}
q & = & \frac{a_2 - a_1}{2 T_{dbl}} - \frac{3}{2} \frac{a_2 + a_1}{T_{dbl}^2} T_{dbl} \\
& = & \frac{-(a_2+2a_1)}{T_{dbl}}
\end{eqnarray}
$$

以上より、重心速度の補間関数は以下のようになる。

$$
v(t) = \frac{a_2 + a_1}{T_{dbl}^2}t^3 + \frac{-(a_2+2a_1)}{T_{dbl}}t^2 + a_1t + v_c \tag{3}
$$

重心位置関数の導出

まず、既知情報として、前回の歩行素片の終了端 (\(t=0\)) における位置を \( x_1 \) とする。

次に、式(3)を時間積分し、以下の式を得る。

$$
x(t) = \int v(t) dt = \frac{a_2 + a_1}{4T_{dbl}^2}t^4 + \frac{-(a_2+2a_1)}{3T_{dbl}}t^3 + \frac{a_1}{2}t^2 + v_ct + C \tag{4}
$$

ここで、\( x(0) = x_1 \) であるため、\( C = x_1 \) である。

なお、補間関数の終了端(= 次の歩行素片の開始端)における重心位置は、\( t=T_{dbl} \) の時であるため、 式(4) に代入して以下のようになる。

$$
\begin{eqnarray}
x(T_{tbl}) & = & \frac{a_2 + a_1}{4T_{dbl}^2}T_{dbl}^4 + \frac{-(a_2+2a_1)}{3T_{dbl}}T_{dbl}^3 + \frac{a_1}{2}T_{dbl}^2 + v_cT_{dbl} + x_1 \\
& = & \frac{a_1-a_2}{12}T_{dbl}^2 + v_c T_{dbl} + x_1 \tag{5}
\end{eqnarray}
$$

Todo: 両足期間を入れる前と後の速度・加速度の変化の様子のグラフ(とりあえず、Twitterで上げたグラフを張っときます)