自作二足歩行パターン生成

自作の歩行パターン生成を説明してみたいと思う。一番簡単な2次元capture pointによる手法を用いる。
先に断っておくけど、hrpsys-baseのAutoBalancerがずっとよく出来ていると思う。
自分は勉強用で作ってみた。まあ暇な方、あるいは雰囲気を感じたい方は読んでみてください。
指摘的なコメントも大歓迎。むしろこの記事のメイン目的はそれ。
なお、自分のプログラムはchoreonoidのライブラリを使用するので、最後にchoreonoidの使用したソースコードも混ざって説明する。このパターン生成プログラムはRTCとして実装した。

歩行生成の大まかな流れ

1. 遊脚の着地位置姿勢を指定
2. 遊脚の軌道を計画する
3. capture point(歩行動作の終点におけるロボットの重心位置)が遊脚の着地位置に到達するように計画する。
4. capture pointから目標重心軌道を計算する。
5. 両足(片足6自由度、または6自由度以上)を用いて以下の12個目標値を満たすように逆運動学を解く

• 重心位置(3DOF)
• 腰姿勢(3DOF)。支持脚と遊脚の中間姿勢を腰の姿勢にしている。
• 遊脚位置姿勢(6DOF)

1. 逆運動学の解が目標関節角度となり、サーボの目標値へ出力。

重心軌道の生成について、自分も昔は梶田さんの予見制御を用いたが、capture pointの方法はより簡潔に実装でき、かつ倒立振子モデルのダイナミックもちゃんと考慮しているのでcapture pointの方法に移行した。
hrpsys-baseのAutoBalancerはまだ予見制御を使っているので、予見制御の実装について知りたい方はそっちのソースを参照されたい。

一歩の歩行動作の定義

歩行時の逆運動学計算は支持脚座標系基準で解いている。支持脚は一歩踏むごとに交換するので、支持脚を交換してから、次に支持脚を交換するまでの歩行動作を"一歩"と定義する。
図にしてみると以下の通り。これが"一歩"である。

時間軸から一歩の歩行動作をみると、両足支持期前半(時間長さ$\rm T_{dbl}$)、片足支持期(時間長さ$\rm T_{sup}$)、両足支持期後半 (時間長さ$\rm T_{dbl}$)で構成されている。黒い点がエンドエフェクタ位置。逆運動学はこの位置について解いている。エンドエフェクタ位置が片足支持期につま先から踵へ間に移動する。
一歩の歩行動作は以下のものを計算する：

• 遊脚のエンドエフェクタ位置が以下通りで計画する。
  • 両足支持期前半がつま先に固定
  • 片足支持期がつま先から踵に移動する
  • 両足支持期後半が踵に固定
• 遊脚のエンドエフェクタのx、y、z、yaw軸方向の軌道
• つま先離陸、踵着地するように遊脚のエンドエフェクタのpitch方向回転角
• 目標重心軌道

重心位置以外は適当な補間(5次補間とかスプライン補間とか)で計算する。ただし、起点と終点における速度加速度ともに0。

capture pointと目標重心軌道の計画

"Biped walking control based on Capture Point dynamics(IROS 2011)"この論文の一部を説明する。

なお、この方法はロボットを重心一個のみの線形倒立振子としてモデル化する方法である。より実際のロボットに近い簡易モデルとして、hondaが重心位置をフライホイールにし、プラス両足の重心で計3重心のモデル使用している。hondaのモデルだと遊脚が振り出し時の慣性も考慮されより安定している。こちらもいつか実装してみたいね...とりあえず本文は重心一個のみの線形倒立振子で説明する。
まず線形倒立振子の運動方程式を召喚する(梶田さんの教科書p130で詳しく導出されているのでここで割愛)

\ddot{x}_c = \omega^2 (x_c - p_x), ~\omega\equiv\sqrt{g/z_c} \tag{1}

ここで変数の定義は以下の通り：

• $p_x$: zmp x方向水平位置
• $x_c$: 重心x位置
• $g$: 重力
• $z_c$: 重心高さ

y方向についても同じ解析行えるので以下に$x$方向の解析のみ説明する。
次に$\sigma \equiv [x_c,~\dot{x_c}]^{\rm T}$と定義し、上の式を状態方程式に直す。

\dot{\sigma}= \begin{bmatrix} 0 & 1 \\ \omega ^2 & 0 \end{bmatrix} \sigma + \begin{bmatrix} 0 \\ -\omega ^2 \end{bmatrix} p_x \tag{2}

$\sigma$について解析解解くと

\sigma(t) = \begin{bmatrix} x_c (t) \\ \dot{x}_c (t) \end{bmatrix} =  \begin{bmatrix} \cosh(t) & \frac{1}{\omega}\sinh(\omega t) \\ \omega \sinh(\omega t) & \cosh(\omega t) \end{bmatrix} \sigma _0 + \begin{bmatrix} 1-\cosh(\omega t) \\ -\omega \sinh(\omega t) \end{bmatrix} p_x \\ \tag{3}

\sigma _0 \equiv \begin{bmatrix} x_{c0},~\dot{x}_{c0}\end{bmatrix}^{\rm T}

ここで、$\sigma_0$ が初期値と定義する。
この式3において、ある特殊な$p_x$を代入すると時間が無限大の時の$x_c$が$p_x$と一致する。この特殊の$p_x$をcapture point ($\xi _x$)と定義する。式3の第一行を時間＝無限大で展開すると：

x_c (t)_ {t \rightarrow \infty} = p_x = x_{c0} \cosh(\omega t) + \frac{\dot{x}_ {c0}}{\omega}\sinh(\omega t) + p_x - p_x \cosh(\omega t) \tag{4}

$t$が無限大の時に双曲線関数$\sinh(\omega t)$が0になるので、式4が以下に書き直す:

p_x = x_{c0} + \frac{\dot{x}_ {c0}}{\omega}\tanh(\omega t) \tag{5}

また$t$が無限大の時に双曲線関数$\tanh(\omega t)$が1になるので、先ほど述べた"特殊な$p_x$"が以下式で得る。

p_x = x_{c0} + \frac{\dot{x}_ {c0}}{\omega} \equiv \xi _x \tag{6}

これが"capture point"だ！この式から時間$t$における重心速度が以下の式で計算：

\dot{x}_c(t) = -\omega (x_c(t) - \xi _x(t)) \tag{7}

この式はcapture pointを入力として、重心位置を計算する式となる。
時間が無限大にすると重心がcapture pointに収束するのをこの微分方程式から分かった。

またcapture pointの計算式を微分すると、

\dot{\xi} _x = \dot{x}_c + \frac{\ddot{x}_c}{\omega} \tag{8}

倒立振子の運動方程式(式1)と重心速度の計算式(式7)をこの式に代入すると以下の式を得る。

\dot{\xi} _x = \dot{x}_c + \frac{\ddot{x}_c}{\omega}= \omega(\xi _x - p_x) \tag{9}

この式はzmpを入力として、capture pointを計算する式になる。
$p_x$が定数の時に、$\xi _x$について微分方程式を解くと

\xi _x(t) = e ^{\omega t}\xi _{x0} + (1-e^{\omega t})p_x \tag{10}

$\xi _{x0}$に現在のcapture point$\xi _x$を代入し、$\xi _x(t)$ に目標時間$dT$後の目標capture point$\xi _{xd}$を代入したら、$\xi _x$が$dT$秒後に$\xi _d$になるためのzmp($\equiv p _{xr}$) が以下の通りで計算できる。

p_{xr} = \frac{\xi _{xd}-e^{\omega dT}\xi_x}{1-e^{\omega dT}} \tag{11}

この$p _{xr}$が定数です。

capture pointによる重心軌道計画法のまとめ

以上の説明を整理すると、

1. 遊脚の着地位置姿勢を目標capture point($\xi _{xd}$)指定。
2. 現在の重心位置をcapture pointの初期位置$\xi _{x0}$にし、$\xi _{x0}$が目標時間($dT$)後に目標capture point($\xi _{xd}$)に到達するような目標zmp($p _{xr}$)を計算する(式11)。
3. 目標zmpから目標capture point($\xi _x(t)$)軌道を計算する(式10)
4. 目標capture point軌道から各時刻の目標重心速度を計算する(式7)。目標重心速度を積分して目標重心軌道を得る。重心位置はひたすらcapture pointへ追従する。

図で説明するたため、以上の変数を全部x,y二次元に拡張して、以下にに再度に定義する：
$\boldsymbol \xi \equiv \begin{bmatrix} \xi _x,~\xi _y \end{bmatrix}^{\rm T}$ : 現在capture point
$\boldsymbol \xi _d\equiv \begin{bmatrix} \xi _{xd},~\xi _{yd} \end{bmatrix}^{\rm T}$ : 目標capture point
${\boldsymbol x} _c \equiv \begin{bmatrix} x _c ,~y _c \end{bmatrix}^{\rm T}$ : 重心
${\boldsymbol p} _r \equiv \begin{bmatrix} p _{xr} ,~p _{yr} \end{bmatrix}^{\rm T}$ :目標ZMP

以上の説明を図にすると以下通りに：

1. [最初の$\rm T_{sup}$]最初の一歩は少し特殊で、現在重心位置を現在capture point ($\boldsymbol \xi$ )にして、
   式11で$\rm T _{sup}$ 秒かけて目標capture point(${\boldsymbol \xi} _d$ 支持脚の中心)へ移動するための目標zmp(${\boldsymbol p} _r$)を計算する。次に式10で$\rm T _{sup}$秒間のcapture pointを計画し、式7で$\rm T _{sup}$秒間の重心軌道を計画する。
2. [両足支持期前半$\rm T_{dbl}$]この期間にcapture pointを固定にし、同じく式7で$\rm T_{dbl}$秒間の重心軌道を計画する。また、目標ZMPを両足の中間から支持脚中心へ移動するように補間する(この区間の目標ZMPの計算は適当。あまりよくないかも)。また、遊脚をつま先離陸するようエンドエフェクタ周りpitch方向回転させる。
3. [片足支持期$\rm T_{sup}$ + 両足支持期後半$\rm T_{dbl}$]現在capture point ($\boldsymbol \xi$ )を$\rm T _{sup} + T _{dbl}$ 秒かけて目標capture point(${\boldsymbol \xi} _d$ 遊脚着位置の中心位置)へ移動するための目標zmp(${\boldsymbol p} _r$)を計算する。次に式10で$\rm T _{sup} + T _{dbl}$秒間のcapture pointを計画し、式7で$\rm T _{sup} + T _{dbl}$秒間の重心軌道を計画する。なお、踵にあるエンドエフェクタが$T _{sup}$秒後に着地するよう遊脚軌道を計画する。着地後$\rm T _{dbl}$秒間かけて回転pitch軸まわり回転させる。
4. 新しい一歩のはじめり、手順2に戻る。以後手順2~3を繰り返す。

実装について

恥ずかしだが、自分の実装を晒す。あくまで参考だが、PR大歓迎。ソースリポジトリ。choreonoidのシミュレーション環境の使い方についてまた次の記事で紹介する。
以下にいくつかのテーマをピックアップして解説する。なお、以前書いた記事choreonoid便利ライブラリも合わせて参考にしてください。

目標遊脚着地位置(エンドエフェクタ)

自分は

1. 初期状態の両足中間点に仮想な参照ポイントを設定し、
2. 参照ポイントから両足エンドエフェクタへのベクトルを記録し,
3. 以後はこの参照ポイントの速度を与えて、参照ポイントを移動させるのと同時に
4. 先ほど記録した両足エンドエフェクタへのベクトルを用いて目標遊脚着地位置を計算する。
   遊脚の着地位置は支持脚座標系を切り替えるタイミングに参照され、そこから一歩分の歩行パターンを生成する。
   TODO:service portから目標遊脚着地位置のqueueを設定できるように。

end effectorの設定

末端リンクの根元(リンク座標系原点)位置について逆運動学を解くのではなく、エンドエフェクタの位置について解きたい場合がよくある。この場合は末端のリンクにジョイントタイプがfixedのリンクをappendして、このリンク位置について逆運動学を解けばよい。このリンクの座標系原点をエンドエフェクタ位置とする。つま先離陸、踵着陸するときに使うテクニック。

cnoid::Link* ee;
ee = new cnoid::Link();
// エンドエフェクタの親リンクとの相対位置姿勢の設定
cnoid::Position T;
T.linear() = Eigen::MatrixXd::Identity(3,3); // 相対姿勢
T.translation() = Vector3(0.0, 0.0, -1.0); //　相対位置
ee -> setOffsetPosition(T);
ee -> setName("end_effector");
ee -> setJointType(cnoid::Link::FIXED_JOINT);
m_robot -> link("追加された親リンク名") -> appendChild(ee);
m_robot -> updateLinkTree();
m_robot -> calcForwardKinematics(); // エンドエフェクタ位置姿勢更新

エンドエフェクタ追加後、根元リンクからエンドエフェクタまでのJointPathクラスの参照を取得し、逆運動学解けば良い。

capture pointと重心軌道計画

一歩分の時系列capture pointを計画し、その後はひたすら式(7)を用いて重心の速度を計算し積分する。

void patternPlanner::getNextCom(Vector3 &cm_ref)
{
  //if(cp_deque.empty())
  //  return;
  Vector2 cp_cur, cm_vel;
 
  if(!cp_deque.empty()){
    cp_cur = cp_deque.at(0);
    cp_deque.pop_front();
  }
  else{ 
    cp_cur = cp;
  }
  cm_vel = w * (cp_cur - cm_ref.head<2>());
  cm_ref.head<2>() += cm_vel * dt;
}

全身の順運動学計算について

choronoidのbodyクラスの順運動学関数calcForwardKinematics()を使うとロボットのrootlinkから末端のリンクまで全部計算してくれる。

m_robot-> calcForwardKinematics();

この計算は、あたかもrootlinkが空間中に固定されて全身のリンクの位置姿勢を計算する。しかし2足歩行ロボットの場合、膝を曲げるように関節角度をモデルに設定し、この順運動学計算を行うと計算された足位置が床から浮いてしまう。この問題を防ぐため、自分が

1. 支持脚から遊脚までのJointPathで一回順運動学計算し、rootlinkの位置姿勢を更新してから
2. rootLinkから順運動学計算
   で全身の順運動学計算を行う。

JointPathPtr supLeg2swingLeg = getCustomJointPath(m_robot, m_robot->link(支持脚末端リンク名), m_robot->link(遊脚末端リンク名);
supLeg2swingLeg -> calcForwardKinematics();
m_robot -> calcForwardKinematics();

12自由度の逆運動学計算

歩行中は常に両足(片足6自由度、または6自由度以上)を用いて以下の12個目標値を満たすように逆運動学を解いている

• 重心位置(3DOF)
• 腰姿勢(3DOF)
• 遊脚位置姿勢(6DOF)

12自由度のヤコビ計算

JointPathPtr  SupLeg2SwLeg,SupLeg2W;
MatrixXd out_J, JSupLeg2SwLeg, JSupLeg2W, Jcom;

SupLeg2SwLeg = getCustomJointPath(m_robot, 支持脚リンクポインタ, 遊脚リンクポインタ);
SupLeg2W = getCustomJointPath(m_robot, 支持脚リンクポインタ, 腰リンクポインタ);
m_robot -> calcCenterOfMass();
calcCMJacobian(m_robot, 支持脚ポインタ, Jcom);//jcom size (3 x dof)
SupLeg2SwLeg -> calcJacobian(JSupLeg2SwLeg);
SupLeg2W -> calcJacobian(JSupLeg2W);

// 自分のロボットの足jointのindex 0~11なのでそれに合わせて重心ヤコビがJcomから足の寄与する成分を抽出
out_J.block<3,12>(0,0) = Jcom.block<3,12>(0,0);
  
// 支持脚から腰姿勢へのヤコビ計算
for(int i = 0; i < SupLeg2W->numJoints(); i++) {
    int id = SupLeg2W->joint(i)->jointId();
    for(int j=3; j<6; j++){
      out_J(j,id) = JSupLeg2W(j,i);
  }
}
  
//支持脚から遊脚へのヤコビ計算
for(int i = 0; i < SupLeg2SwLeg->numJoints(); i++) {
    int id = SupLeg2SwLeg->joint(i)->jointId();
    for(int j = 6; j<12; j++){
      out_J(j,id) = JSupLeg2SwLeg(j-6,i);
    }
}

後はこれを用いて逆運動学数値計算すればよい！実際にこれ用いて逆運動学解くとこ逆運動学関数を参照されたい。

Stabilizerへの他の情報を出力

hrpsys-baseのStabilizerを拝借した。歩行パターン生成RTCが歩行パターン以外に以下の追加情報をStabilizer RTCに出力する。

• rzmp:ロボット座標系(rootLink座標系)からみた目標ZMP
• contactStates:二足の場合長さ2のデータ。それぞれ右足、左足の接地状態を表す。空中にある場合は0で、それ以外の場合は1。
• toeheelRatio: 二足の場合長さ2のデータ。それぞれ右足、左足の足の回転状態を表す。つま先かかと接地期の足には0，それ以外の場合は１。
• controlSwingSupportTime：二足の場合長さ2のデータ。それぞれ右足、左足の支持脚->遊脚もしくは遊脚->支持脚　のように接地状態が変わるまでの時間。

#おわりに
説明しきれない細かいとこはソースコードを参照されたい。またコメントも対応するよう頑張ります。

次回を待て！！