012 - 歩行制御

March 01, 2024

vnoidの解説

今回の内容

本チャレンジでは、初心者の方でも簡単に人形ロボットの運動計画ができるように、
vnoidというサンプルパッケージが用意されております。

今回はvnoidパッケージの機能の一つ、
歩行制御器(stepping_controller)について解説します。

歩行制御の概要

歩行制御では、一歩ずつ支持足や、離地足、着地足の位置・姿勢を更新します。
また、歩行開始時および終了時の目標 ZMP・DCMについても一歩ずつ更新します。

そのうえで、離地と着地の間をつなぐ浮遊足の目標軌道(遊脚軌道)を計算し、
人型ロボットにそれら全ての指令を送ります。

サンプルコードの解説

歩行ステップの更新

 1void SteppingController::Update(const Timer& timer, const Param& param, Footstep& footstep, Footstep& footstep_buffer, Centroid& centroid, Base& base, vector<Foot>& foot){
 2    if(CheckLanding(timer, footstep_buffer.steps[0], foot)){
 3        if(footstep.steps.size() > 1){
 4            // pop step just completed from the footsteps
 5            footstep.steps.pop_front();
 6            if(footstep.steps.size() == 1){
 7		        printf("end of footstep reached\n");
 8                return;
 9	        }
10        }
11
12        footstep_buffer.steps[1].dcm = footstep_buffer.steps[0].dcm;
13        footstep_buffer.steps.pop_front();
14        footstep_buffer.steps.push_back(Step());
15        footstep_buffer.steps[0].tbegin = timer.time;
16
17        buffer_ready = false;
18    }
19
20	...
21
22}

SteppingControllerでは、直近一歩の歩行制御を行います。
そのため、一歩が完了するたびに歩行ステップを更新します。
その際、footstep_bufferというバッファを用います。
これは、直近1歩分を表す2つの歩行ステップを格納するバッファです。

2行目のCheckLandingで、着地が完了したかを判定します。
着地が完了した場合は、バッファを次の1歩分の歩行ステップに更新します(3~17行目)。

まず、たった今終了した歩行ステップをfootstepから取り除きます。
このとき、歩行ステップが残り一つしかないときは歩行を終了します(6~9行目)。
まだ数歩残っているときには、終了した歩行ステップから目標 DCM を引継いだのちに、
footstep_bufferからも終了した歩行ステップを取りのぞきます。

次に、空の歩行ステップ情報をバッファの末尾に追加します。
次の歩行ステップの開始時刻tbeginを現在時刻にセットします。
バッファの1番目の要素がまだ空で、
歩行ステップ間の足の制御をする準備が整っていないので、
buffer_ready = falseとして、次のブロックに移ります。

 1void SteppingController::Update(const Timer& timer, const Param& param, Footstep& footstep, Footstep& footstep_buffer, Centroid& centroid, Base& base, vector<Foot>& foot){
 2
 3	...
 4
 5    Step& st0  = footstep.steps[0];
 6    Step& st1  = footstep.steps[1];
 7    Step& stb0 = footstep_buffer.steps[0];
 8    Step& stb1 = footstep_buffer.steps[1];
 9    int sup =  st0.side;
10    int swg = !st0.side;
11	
12    double T  = param.T;
13    Vector3 offset(0.0, 0.0, param.com_height);
14    
15    if(!buffer_ready){
16        // update support, lift-off, and landing positions
17        stb0.side = st0.side;
18        stb1.side = st1.side;
19
20        stb0.stepping = st0.stepping;
21        stb0.duration = st0.duration;
22
23        stb0.foot_pos  [sup] = foot[sup].pos_ref;
24        stb0.foot_angle[sup] = Vector3(0.0, 0.0, foot[sup].angle_ref.z());
25        stb0.foot_ori  [sup] = FromRollPitchYaw(stb0.foot_angle[sup]);
26        stb0.foot_pos  [swg] = foot[swg].pos_ref;
27        stb0.foot_angle[swg] = Vector3(0.0, 0.0, foot[swg].angle_ref.z());
28        stb0.foot_ori  [swg] = FromRollPitchYaw(stb0.foot_angle[swg]);
29        stb0.dcm = centroid.dcm_ref;
30    
31        // landing position relative to support foot, taken from footsteps
32        Quaternion ori_rel = st0.foot_ori[sup].conjugate()* st1.foot_ori[swg];
33        Vector3    pos_rel = st0.foot_ori[sup].conjugate()*(st1.foot_pos[swg] - st0.foot_pos[sup]);
34        Vector3    dcm_rel = st0.foot_ori[sup].conjugate()*(st1.dcm - st0.foot_pos[sup]);
35
36        // calc absolute landing position
37        stb1.foot_pos  [sup] = stb0.foot_pos  [sup];
38        stb1.foot_ori  [sup] = stb0.foot_ori  [sup];
39        stb1.foot_angle[sup] = stb0.foot_angle[sup];
40        stb1.foot_pos  [swg] = stb0.foot_pos[sup] + stb0.foot_ori[sup]*pos_rel;
41        stb1.foot_ori  [swg] = stb0.foot_ori[sup]*ori_rel;
42        stb1.foot_angle[swg] = ToRollPitchYaw(stb1.foot_ori[swg]);
43        stb1.dcm = stb0.foot_pos[sup] + stb0.foot_ori[sup]*dcm_rel;
44
45        // calc zmp
46        double alpha = exp(-stb0.duration/T);
47        stb0.zmp = (1.0/(1.0 - alpha))*(stb0.dcm - alpha*stb1.dcm) - offset;
48
49        buffer_ready = true;
50    }
51
52    if(footstep.steps.size() < 2){
53		return;
54	}
55
56	...
57
58}

このブロックでは、空の歩行ステップ情報を埋める処理が行われます。
この処理は、一歩につき最初の一度だけ行われます。

以降のブロックで用いる変数について説明します。
st0やst1は、footstepオブジェクトの0番目と1番目の要素です。
stb0やstb1は、footstep_bufferオブジェクトの0番目と1番目の要素です。
supはその足が支持側の足であることを、
swgはその足が遊脚側の足であることを示すフラグです。

0番目の歩行ステップのsupには、現在時刻における支持足の位置・姿勢を、
swgには、現在時刻における離地足の位置・姿勢を設定します(23~28行目)。

1番目の歩行ステップのsupは、0番目のものと一致するように設定し、
swgには、着地足の目標位置・姿勢を設定します(31~34, 36~39行目)。

また、0番目の歩行ステップの目標 DCM を、歩行安定化制御により修正された
現在時刻における目標 DCM centroid.dcm_refに設定します(29行目)。

45~47行目では、現在の歩行ステップにおける目標 ZMP を、
歩行開始時と終了時の目標 DCM に合わせて決めます。
ここで用いられている漸化式は、目標ZMP・DCM計画の記事で説明したものです。ここで、Tは LIPM の重心運動の時定数です。
offsetは、三次元空間上のDCM と水平面上の ZMP の次元を揃えるために用いる、
目標重心高さ分のオフセットです。

最後に、歩行ステップの情報がすべて埋まり、歩行制御の準備が整ったので、
buffer_ready = trueとします。

歩行制御

 1void SteppingController::Update(const Timer& timer, const Param& param, Footstep& footstep, Footstep& footstep_buffer, Centroid& centroid, Base& base, vector<Foot>& foot){
 2
 3	...
 4
 5    // time to landing
 6    double ttl = stb0.tbegin + stb0.duration - timer.time;
 7	double alpha = exp(-ttl/T);
 8    
 9	stb0.dcm = (1.0-alpha)*(stb0.zmp + offset) + alpha*stb1.dcm;
10    
11    // landing adjustment based on dcm
12    Vector3 land_rel = (st1.foot_pos[swg] - st0.foot_pos[sup]) - (stb0.dcm - stb0.foot_pos[sup]) + 0.0*(stb0.zmp - stb0.foot_pos[sup]);
13	stb1.foot_pos[swg].x() = centroid.dcm_ref.x() + land_rel.x();
14	stb1.foot_pos[swg].y() = centroid.dcm_ref.y() + land_rel.y();
15
16    // reference base orientation is set as the middle of feet orientation
17    double angle_diff = foot[1].angle_ref.z() - foot[0].angle_ref.z();
18    while(angle_diff >  pi) angle_diff -= 2.0*pi;
19    while(angle_diff < -pi) angle_diff += 2.0*pi;
20	base.angle_ref.z() = foot[0].angle_ref.z() + angle_diff/2.0;
21
22    base.ori_ref   = FromRollPitchYaw(base.angle_ref);
23
24    // set support foot position
25    foot[sup].pos_ref     = stb0.foot_pos[sup];
26    foot[sup].angle_ref   = stb0.foot_angle[sup];
27    foot[sup].ori_ref     = FromRollPitchYaw(foot[sup].angle_ref);
28    foot[sup].contact_ref = true;
29
30    // set swing foot position
31    if(!stb0.stepping || (timer.time - stb0.tbegin) < dsp_duration)
32    {
33        foot[swg].pos_ref     = stb0.foot_pos  [swg];
34        foot[swg].angle_ref   = stb0.foot_angle[swg];
35        foot[swg].ori_ref     = stb0.foot_ori  [swg];
36        foot[swg].contact_ref = true;
37    }
38    else{
39        double ts   = timer.time - (stb0.tbegin + dsp_duration);            //< time elapsed in ssp
40        double tauv = stb0.duration - dsp_duration; //< duration of vertical movement
41        double tauh = tauv - descend_duration;     //< duration of horizontal movement
42
43        // cycloid swing profile
44        double sv     = ts/tauv;
45        double sh     = ts/tauh;
46        double thetav = 2.0*pi*sv;
47        double thetah = 2.0*pi*sh;
48        double ch     = (sh < 1.0 ? (thetah - sin(thetah))/(2.0*pi) : 1.0);
49        double cv     = (1.0 - cos(thetav))/2.0;
50        double cv2    = (1.0 - cos(thetav/2.0))/2.0;
51        double cw     = sin(thetah);
52
53        // foot turning
54        Vector3 turn = stb1.foot_angle[swg] - stb0.foot_angle[swg];
55        while(turn.z() >  pi) turn.z() -= 2.0*pi;
56        while(turn.z() < -pi) turn.z() += 2.0*pi;
57
58        // foot tilting
59        Vector3 tilt = stb0.foot_ori[swg]*Vector3(0.0, swing_tilt, 0.0);
60
61        foot[swg].pos_ref      = (1.0 - ch)*stb0.foot_pos[swg] + ch*stb1.foot_pos[swg];
62        foot[swg].pos_ref.z() += (cv*(swing_height + 0.5*descend_depth) - cv2*descend_depth);
63        foot[swg].angle_ref    = stb0.foot_angle[swg] + ch*turn + cw*tilt;
64        foot[swg].ori_ref      = FromRollPitchYaw(foot[swg].angle_ref);
65        foot[swg].contact_ref  = false;
66
67        // adjust swing foot considering base link inclination
68        Quaternion qrel = 
69            FromRollPitchYaw(Vector3(base.angle_ref.x(), base.angle_ref.y(), base.angle_ref.z()))*
70            FromRollPitchYaw(Vector3(base.angle    .x(), base.angle    .y(), base.angle_ref.z())).conjugate();
71        Vector3 pivot   = centroid.zmp_ref;
72        foot[swg].pos_ref   = qrel*(foot[swg].pos_ref - pivot) + pivot;
73        foot[swg].ori_ref   = qrel* foot[swg].ori_ref;
74        foot[swg].angle_ref = ToRollPitchYaw(foot[swg].ori_ref);
75
76    }

このブロックから、本格的に歩行制御が行われます。

歩行は、両足支持期から片足支持期へ移行する流れを繰り返すものとします。
浮遊足の離地から着地までの期間が片足支持期です(下図)。

両足支持期の場合は、支持足はそのまま変化しないように(24~28行目)、
もう一方の支持足は離地の位置・姿勢を保持するように(31~37行目)制御します。
片足支持期の場合は、支持足はそのまま変化しないように(24~28行目)、
浮遊足は、サイクロイド曲線をベースに更新されるように(61~62行目)制御します。

以下、片足支持期における浮遊足の位置・姿勢の更新方法について説明します。
離地足と着地足をつなぐ軌道を、遊脚軌道と言います。

離地、着地瞬間の浮遊足の状態は、それぞれ
stb0.foot_pos[swg]とstb1.foot_pos[swg]にあらかじめ計画されています。
そのうち着地に関しては、歩行間にロボットがバランスを崩した際に、
安定化のために修正しなければならない場合があります。
そこで、11~14行目でDCM力学に基づいた着地位置修正を実装しています。
この安定化方法については、012 - 安定化制御の記事で取り扱います。

安定化制御に伴い目標DCMは修正されます。
修正前の現在時刻における目標DCMstb0.dcmは、
事前に計画しておいた着地瞬間の目標DCMに安定収束するように計算されます(9行目)。
ここで、alphaは、現在時刻から着地予定時刻までの区間で、
DCMの運動方程式を離散化して得られる係数です。
修正後の目標DCMはcentroid.dcm_refです。

遊脚軌道$\boldsymbol{p^{swg}}$は、離地位置を$\boldsymbol{p^{lift}}$、着地位置を$\boldsymbol{p^{land}}$として、
次式のように表される(61~62行目, 下図)。
$$ \boldsymbol{p^{swg}} = \boldsymbol{p^{lift}} + c_h (\phi(t_{ssp})) (\boldsymbol{p^{land}} - \boldsymbol{p^{lift}}) + c_v (\phi(t_{ssp})) \boldsymbol{h_{swg}} $$

ここで、$\boldsymbol{h_{swg}} = [0.0, 0.0, h_{swg}]^T$は足を上げる高さです。
また、$c_h$、$c_v$はそれぞれ、正規化されたサイクロイドの横変位と縦変位で、
$\phi$はサイクロイドの回転角とすると、次式で表されます(39~48行目)。

$$ c_h(\phi) = \frac{C_h(\phi) - C_h(\phi_0)}{C_h(\phi_1) - C_h(\phi_0)} = \frac{\phi - \mathrm{sin}\phi}{2\pi} $$ $$ c_v(\phi) = \frac{C_v(\phi) - C_v(\phi_0)}{C_v(\phi_1) - C_v(\phi_0)} = \frac{1 - \mathrm{cos}\phi}{2} $$ $$ C_h(\phi) = \phi - \mathrm{sin}\phi $$ $$ C_v(\phi) = 1 - \mathrm{cos}\phi $$ $$ \phi = 2\pi s $$ $$ \phi_0 = 0 $$ $$ \phi_1 = 2\pi $$ $$ s = \frac{t_{ssp}}{\tau_{ssp}} \in [0, 1] $$

他にも歩行制御では、
ベースリンクのヨー方向姿勢を両足の中間角度に設定したり(16~22行目)、
浮遊足の離地から着地までの姿勢を滑らかにつないだり(53~59, 63~64行目)、
ベースリンク姿勢の目標誤差に合わせて着地修正したりします(67~74行目)。

まとめ・次回予告

今回はvnoidパッケージの歩行制御
(SteppingController) について解説しました。

次回は安定化制御(Stabilizer)について解説しようと思います。

次回： 013 - 安定化制御