013 - 安定化制御
年度替わりに伴い、中の人が交代しています。文体等に違いがあるところがあるかと思いますが、ご了承いただければ幸いです。
今回の内容
本チャレンジでは、初心者の方でも簡単に人形ロボットの運動計画ができるように、
vnoidというサンプルパッケージが用意されております。
今回はvnoidパッケージの機能の一つ、
安定化制御器(Stabilizer)について解説します。
安定化制御の概要
これまでの記事で、人型ロボットを線形倒立振り子として目標運動を計画しましたが、
残念ながら完璧にその通りに動作することはありません。
計画運動からずれて不安定化してしまったときに安定化制御を行い、
転倒を回避しなければなりません。
安定化制御では、ロボットの回転運動を抑えるための転倒回復モーメントを実現させます。
そのために、目標着地位置を計画から修正します。
これは、例えばあなたが電車に乗っていて、
急ブレーキがかかると後ろに倒れそうになりますが、
倒れる方向に足を踏み出して踏ん張る現象と同じです。
本記事では、その着地位置修正がどのような仕組みで行われているかを説明します。
サンプルコードの解説
- 安定化制御全体の流れ
1void Stabilizer::Update(const Timer& timer, const Param& param, const Footstep& footstep_buffer, Centroid& centroid, Base& base, vector<Foot>& foot){
2 const Step& stb0 = footstep_buffer.steps[0];
3 const Step& stb1 = footstep_buffer.steps[1];
4 int sup = stb0.side;
5 int swg = !stb0.side;
6
7 // calc zmp from forces
8 CalcZmp(param, centroid, foot);
9
10 // error between desired and actual base link orientation
11 Vector3 theta = base.angle - base.angle_ref;
12 Vector3 omega = base.angvel - base.angvel_ref;
13
14 // calc base link tilt
15 CalcBaseTilt(timer, param, base, theta, omega);
16
17 // calc dcm and zmp
18 CalcDcmDynamics(timer, param, footstep_buffer, base, theta, omega, centroid);
19
20 // calc desired force applied to CoM
21 centroid.force_ref = param.total_mass*(centroid.com_acc_ref + Vector3(0.0, 0.0, param.gravity));
22 centroid.moment_ref = Vector3(0.0, 0.0, 0.0);
23
24 // calculate desired forces from desired zmp
25 CalcForceDistribution(param, centroid, foot);
26
27 for(int i = 0; i < 2; i++){
28 // ground reaction force control
29 if( foot[i].contact ){
30 for(int j = 0; j < 3; j++){
31 dpos[i][j] += (-force_ctrl_damping*dpos[i][j] + force_ctrl_gain*(foot[i].force_ref[j] - foot[i].force[j]))*timer.dt;
32 dpos[i][j] = std::min(std::max(-force_ctrl_limit, dpos[i][j]), force_ctrl_limit);
33
34 drot[i][j] += (-moment_ctrl_damping*drot[i][j] + moment_ctrl_gain*(foot[i].moment_ref[j] - foot[i].moment[j]))*timer.dt;
35 drot[i][j] = std::min(std::max(-moment_ctrl_limit, drot[i][j]), moment_ctrl_limit);
36 }
37
38 // feedback to desired foot pose
39 foot[i].pos_ref += -dpos[i];
40 foot[i].angle_ref += -drot[i];
41 foot[i].ori_ref = FromRollPitchYaw(foot[i].angle_ref);
42 }
43 }
44
45}
stabilizerでもfootstep_plannerで行ったのと同じように、参照変数としてst0、st1を定義します。st0やst1には、両足分の着地位置・姿勢情報を代入します。
これもfootstep_plannerと同じですね。
そのため、ここでも左右のうちどちらが支持足で、
どちらが振り足かを見分けておく必要があります。
そこでこれまたfootstep_plannerと同じようにsupとswg変数を用意します。supが支持足であることを意味し、swgが振り足であることを意味します。
次にCalcZmpという関数でZMPの位置を算出します。
ZMPについては009 - 目標DCM計画器で、
詳しく説明していますので、そちらをご覧ください。
関数の中身については、下に続く各項目にて解説しています!!
250行目と251行目ではベースリンクの角度と角速度を目標値との誤差を計算しています。
これらの誤差を用いて、254行目にあるCalcBaseTiltという関数を使ってベースリンクの傾きを計算しています。
257行目のCalcDcmDynamicsではDCMや重心の運動について計算されています。
260行目と261行目ではそれらの結果から重心に作用されるべき床反力とモーメントを計算しています。
デフォルトのままでは、目標モーメントの値が各軸で0になっているため、
現状の重心などの状態は反映された目標値になっていません。
264行目のCalcForceDistributionでは目標ZMPから作用する床反力を計算しています。
そのあとのfor文の中で目標とする床反力を制御するために、歩行計画で予定していた位置などからずれて
「どこに」、「どんな角度で」、「どのような姿勢で」足をつけばいいかを計算しています。
最後に278行目から280行目でその修正量を歩行計画で算出した値に
加えることで着地位置修正を考慮した新たな着地位置等をロボットに入力することになります。
では、次は各関数の中でどのようなことが行われているのかを見ていきましょう!!
- CalcZmp
1 void Stabilizer::CalcZmp(const Param& param, Centroid& centroid, vector<Foot>& foot){
2 // get actual force from the sensor
3 for(int i = 0; i < 2; i++){
4 // set contact state
5 foot[i].contact = (foot[i].force.z() >= min_contact_force);
6
7 // measure continuous contact duration
8 if(foot[i].contact){
9 foot[i].zmp = Vector3(-foot[i].moment.y()/foot[i].force.z(), foot[i].moment.x()/foot[i].force.z(), 0.0);
10 }
11 else{
12 foot[i].zmp = Vector3(0.0, 0.0, 0.0);
13 }
14 }
15 // both feet not in contact
16 if(!foot[0].contact && !foot[1].contact){
17 foot[0].balance = 0.5;
18 foot[1].balance = 0.5;
19 centroid.zmp = Vector3(0.0, 0.0, 0.0);
20 }
21 else{
22 double f0 = std::max(0.0, foot[0].force.z());
23 double f1 = std::max(0.0, foot[1].force.z());
24 foot[0].balance = f0/(f0 + f1);
25 foot[1].balance = f1/(f0 + f1);
26 centroid.zmp =
27 (foot[0].balance) * (foot[0].pos_ref + foot[0].ori_ref * foot[0].zmp)
28 + (foot[1].balance) * (foot[1].pos_ref + foot[1].ori_ref * foot[1].zmp);
29 }
30 }
まずはCalcZmpからです。
この関数内では全体の流れでも書いたようにZMPの位置を計算しています。
まず、43行目で足が設置しているかどうかをセンサーからの値で確認しています。
床反力のz軸方向が最小接触力$min_contact_force$よりも大きいとき、
$foot[i].contact$にはtrueが代入され、そうでないときはfalseが代入されます。
この$foot[i].contact$がtrueのとき、各軸回りのモーメントと床反力から
$$\boldsymbol{p} = \frac{\boldsymbol{M}}{\boldsymbol{F}}$$
を用いて、ZMPの位置を推定しています。
ただし、$\boldsymbol{p}$はZMP、$\boldsymbol{M}$はモーメント、$\boldsymbol{F}$は床反力を表しています。
49行目で0が代入されているのは、足が地面に接触していないときのZMPですね。
ここで、求めたのは各足のZMPですが、知りたいのは支持多角形全体でどの位置にZMPがあるかです。
そこで60行目からの計算が必要になってきます。
55行目から始まるif文は両足とも地面に接触していないとき、
つまり、両足ともが空中に浮いているときの話をしています。
さて、60行目からですが、ZMPは各足に作用している床反力の大きさの比によってその位置が決定されます。
そのため、2点間を内分する点を求めることでZMPを求めることができます。
61行目と62行目は各足が接触していれば、床反力の値が$f_0$と$f_1$にその値を代入します。
63行目と64行目で内分比を導出します。
もし仮に片側の足が接触していなかった場合、$f_0$か$f_1$のどちらかには0が代入されているため、foot[0].balanceかfoot[1].balanceのどちらかが0となるため、
もう一方の足で求めたZMPの位置がロボットのZMPの位置となります。
両方ともの足が接触している場合は、床反力の大きさに応じて内分比が求められるため、
最終的に65行目以降でロボットのZMPの位置が導出されます。
- CalcBaseTilt
1 void Stabilizer::CalcBaseTilt(const Timer& timer, const Param& param, Base& base, Vector3 theta, Vector3 omega){
2 // desired angular acceleration for regulating orientation (in local coordinate)
3 Vector3 omegadd_local(
4 -(orientation_ctrl_gain_p*theta.x() + orientation_ctrl_gain_d*omega.x()),
5 -(orientation_ctrl_gain_p*theta.y() + orientation_ctrl_gain_d*omega.y()),
6 0.0
7 );
8
9 //
10 Vector3 omegadd_base(
11 -base_tilt_rate*omegadd_local.x() - base_tilt_damping_p*base.angle_ref.x() - base_tilt_damping_d*base.angvel_ref.x(),
12 -base_tilt_rate*omegadd_local.y() - base_tilt_damping_p*base.angle_ref.y() - base_tilt_damping_d*base.angvel_ref.y(),
13 0.0
14 );
15
16 base.angle_ref += base.angvel_ref*timer.dt;
17 base.ori_ref = FromRollPitchYaw(base.angle_ref);
18 base.angvel_ref += omegadd_base*timer.dt;
19 }
次はCalcBaseTiltについてです。
ベースリンクの傾きを計算する部分です。
始めのomegadd_localでは、ローカル座標系における
ベースリンクの目標角加速度を計算しています。
そのあとのommegadd_baseでは、omegadd_localで
計算した各加速度をベースリンク基準に直しています。
165行目では、目標角速度を時間で積分することで目標角度を求め、
166行目でそれをFromRollPitchYawの関数に与えて、姿勢を計算します。
167行目では、先ほど計算した目標角加速度を利用して
次の目標角速度を計算しています。
- CalcDcmDynamics
1 void Stabilizer::CalcDcmDynamics(const Timer& timer, const Param& param, const Footstep& footstep_buffer, const Base& base, Vector3 theta, Vector3 omega, Centroid& centroid){
2 const Step& stb0 = footstep_buffer.steps[0];
3 const Step& stb1 = footstep_buffer.steps[1];
4 int sup = stb0.side;
5 int swg = !stb0.side;
6
7 double T = param.T;
8 double m = param.total_mass;
9 double h = param.com_height;
10 double T_mh = T/(m*h);
11 double T2_mh = T*T_mh;
12
13 Vector3 offset(0.0, 0.0, param.com_height);
14
15 // desired angular acceleration for regulating orientation (in local coordinate)
16 Vector3 omegadd_local(
17 -(orientation_ctrl_gain_p*theta.x() + orientation_ctrl_gain_d*omega.x()),
18 -(orientation_ctrl_gain_p*theta.y() + orientation_ctrl_gain_d*omega.y()),
19 //-(orientation_ctrl_gain_p*theta.z() + orientation_ctrl_gain_d*omega.z())
20 0.0
21 );
22 // desired moment (in local coordinate)
23 Vector3 Ld_local(
24 param.nominal_inertia.x()*omegadd_local.x(),
25 param.nominal_inertia.y()*omegadd_local.y(),
26 param.nominal_inertia.z()*omegadd_local.z()
27 );
28 // limit recovery moment for safety
29 for(int i = 0; i < 3; i++){
30 Ld_local[i] = std::min(std::max(-recovery_moment_limit, Ld_local[i]), recovery_moment_limit);
31 }
32 // desired moment (in global coordinate);
33 Vector3 Ld = base.ori_ref * Ld_local;
34
35 // virtual disturbance applied to DCM dynamics to generate desired recovery moment
36 Vector3 delta = Vector3(-T_mh*Ld.y(), T_mh*Ld.x(), 0.0);
37
38 // calc zmp for regulating dcm
39 const double rate = 1.0;
40 centroid.zmp_ref = stb0.zmp + dcm_ctrl_gain*(centroid.dcm_ref - stb0.dcm) + rate*T*delta;
41
42 // project zmp inside support region
43 if(stb0.stepping){
44 Vector3 zmp_local = stb0.foot_ori[sup].conjugate()*(centroid.zmp_ref - stb0.foot_pos[sup]);
45 for(int j = 0; j < 3; j++){
46 zmp_local[j] = std::min(std::max(param.zmp_min[j], zmp_local[j]), param.zmp_max[j]);
47 }
48 centroid.zmp_ref = stb0.foot_pos[sup] + stb0.foot_ori [sup]*zmp_local;
49 }
50
51 // calc DCM derivative
52 Vector3 dcm_d = (1/T)*(centroid.dcm_ref - (centroid.zmp_ref + Vector3(0.0, 0.0, h))) + delta;
53
54 // calc CoM acceleration
55 centroid.com_acc_ref = (1/T)*(dcm_d - centroid.com_vel_ref);
56
57 // update DCM
58 centroid.dcm_ref += dcm_d*timer.dt;
59 // limit deviation from reference dcm
60 for(int j = 0; j < 3; j++){
61 centroid.dcm_ref[j] = std::min(std::max(stb0.dcm[j] - dcm_deviation_limit, centroid.dcm_ref[j]), stb0.dcm[j] + dcm_deviation_limit);
62 }
63
64 // calc CoM velocity from dcm
65 centroid.com_vel_ref = (1/T)*(centroid.dcm_ref - centroid.com_pos_ref);
66
67 // update CoM position
68 centroid.com_pos_ref += centroid.com_vel_ref*timer.dt;
69 }
続いて，CalcDcmDynamicsについてです。
始めのomegadd_localはCalcBaseTiltと同じなので、省略しますね。
そのあとのLd_localは発生している角加速度に対して、
それを回復させるために必要なモーメントを計算しています。
しかし、そのモーメントはロボットが発生させることができる大きさに限界があります。
その限界を超えないようにしているのが、198行目からのfor文で書かれているところです。
限界値と算出した目標モーメントの大小を比較して、決定しています。
ここまで、計算していたのはローカル座標系での大きさになりますので、
202行目ではそれをワールド座標系に変換しています。
さて、ここまでの内容で目標となる回復モーメントを求めてきました。
ここからはそのモーメントを実現していく話になります。
205行目では目標となる回復モーメントに応じてDCMの運動を決定します。
この行では、どれだけ調整するかの調整量を計算しています。
そしてその208・209行目では、DCMを調整するためにZMPが
どの位置にあればいいかを計算しています。
009 - 目標ZMP・DCM計画の回でやったように、DCMはZMPから直線的に離れていきます。
イメージは下の図のような感じです。
さて、回復モーメントを考えなければ、
当初計画していたDCMの運動(黒色)でよかったとしましょう。
しかし、回復モーメントを必要分だけ発生させようと思うと、
水色のDCMのような運動をさせないといけなくなりました。
先ほども説明したようにDCMはZMPから直線的に離れますので、
黒色のZMPの位置では水色の捕点を実現することはできません。
そこで、それを実現するためにZMPの位置を水色の位置に変えようということです。
208・209行目ではその新たに修正することになったDCMの位置から、
ZMPの位置を求めているわけです。
212行目から218行目ではZMPが支持多角形内部に収まるようにしています。
009 - 目標ZMP・DCM計画の回に書いていますが、
ZMPは支持多角形の内部にしか存在できません。
いくら回復モーメントを実現するためとはいえ、その外に出ることは許されないのです。
そこで、本来あってほしい位置にできるだけ近い位置に
とるようにしているのがこの部分になります。
そこから先は修正したZMPを使ってDCMや重心の運動を考えていきます。
221行目はDCMの速度を
$$ \boldsymbol{\dot{\xi}} = \frac{1}{T}(\boldsymbol{\xi} - \boldsymbol{p}) $$
の式を使って更新します。
さらに224行目では更新したDCM速度を利用して
$$ \boldsymbol{\ddot{x}} = -\frac{1}{T}(\dot{\boldsymbol{\xi}} - \boldsymbol{x}) $$
から重心加速度を求めます。
ここまで求めたらあとはそれぞれを積分してDCMの位置、重心速度と位置を求めていくだけですね(227～237行目)。
- CalcForceDistribution
1 void Stabilizer::CalcForceDistribution(const Param& param, Centroid& centroid, vector<Foot>& foot){
2 // switch based on contact state
3 if(!foot[0].contact_ref && !foot[1].contact_ref){
4 foot[0].balance_ref = 0.5;
5 foot[1].balance_ref = 0.5;
6 foot[0].zmp_ref = Vector3(0.0, 0.0, 0.0);
7 foot[1].zmp_ref = Vector3(0.0, 0.0, 0.0);
8 }
9 if( foot[0].contact_ref && !foot[1].contact_ref){
10 foot[0].balance_ref = 1.0;
11 foot[1].balance_ref = 0.0;
12 foot[0].zmp_ref = foot[0].ori_ref.conjugate() * (centroid.zmp_ref - foot[0].pos_ref);
13 foot[1].zmp_ref = Vector3(0.0, 0.0, 0.0);
14 }
15 if(!foot[0].contact_ref && foot[1].contact_ref){
16 foot[0].balance_ref = 0.0;
17 foot[1].balance_ref = 1.0;
18 foot[0].zmp_ref = Vector3(0.0, 0.0, 0.0);
19 foot[1].zmp_ref = foot[1].ori_ref.conjugate() * (centroid.zmp_ref - foot[1].pos_ref);
20 }
21 if( foot[0].contact_ref && foot[1].contact_ref){
22 //
23 Vector2 b;
24 Vector3 pdiff = foot[1].pos_ref - foot[0].pos_ref;
25 double pdiff2 = pdiff.squaredNorm();
26 const double eps = 1.0e-10;
27 if(pdiff2 < eps){
28 b[0] = b[1] = 0.5;
29 }
30 else{
31 b[0] = (pdiff.dot(foot[1].pos_ref - centroid.zmp_ref))/pdiff2;
32 b[0] = std::min(std::max(0.0, b[0]), 1.0);
33 b[1] = 1.0 - b[0];
34 }
35
36 foot[0].balance_ref = b[0];
37 foot[1].balance_ref = b[1];
38
39 Vector3 zmp_proj = b[0]*foot[0].pos_ref + b[1]*foot[1].pos_ref;
40
41 double b2 = b.squaredNorm();
42 foot[0].zmp_ref = (b[0]/b2) * (foot[0].ori_ref.conjugate() * (centroid.zmp_ref - zmp_proj));
43 foot[1].zmp_ref = (b[1]/b2) * (foot[1].ori_ref.conjugate() * (centroid.zmp_ref - zmp_proj));
44 }
45
46 // limit zmp
47 for(int i = 0; i < 2; i++){
48 for(int j = 0; j < 3; j++){
49 foot[i].zmp_ref[j] = std::min(std::max(param.zmp_min[j], foot[i].zmp_ref[j]), param.zmp_max[j]);
50 }
51 }
52
53 for(int i = 0; i < 2; i++){
54 // force and moment to realize desired Zmp
55 foot[i].force_ref = foot[i].ori_ref.conjugate() * (foot[i].balance_ref * centroid.force_ref);
56 foot[i].moment_ref[0] = foot[i].force_ref.z() * foot[i].zmp_ref.y();
57 foot[i].moment_ref[1] = -foot[i].force_ref.z() * foot[i].zmp_ref.x();
58 foot[i].moment_ref[2] = foot[i].balance_ref * centroid.moment_ref.z();
59 }
60 }
さあ、この回も最後の解説です。最後はCalcForceDistributionですね。
さて、この関数の中ではif文がたくさん並んでいてややこしそうですが、
そんなに気にするほどではありません。
これはどちらの足が設置しているかを条件分けしているだけです。
最初のif文の条件では、両方ともの足が設置していないときを表しているので、
支持多角形なんてありませんから、ZMPはないことになっています。
そのあとの二つのif文は片足支持期の話です。
片足支持期のときは、その支持足だけを考えればいいですから、
82行目や89行目で支持足基準の座標系でZMPの目標位置を相対位置で表しています。
最後のif文は両足支持期のことを考えています。
このときは片足支持期のときとは異なり、
どちらの足にZMPが偏っているかを考える必要があります。
それを行っているのが、97行目から104行目の部分です。
この部分では、偏り具合を計算してそれを割合で表しています。
その割合に基づいて、支持多角形の中心を計算し、
112行目と113行目でZMPの相対位置を計算しています。
117行目から121行目ではそのZMPが支持多角形の内部で
存在するように計算しているところですが、
CalcDcmDynamicsの項目で説明した内容と同じですので、省略しますね。
ここまで、求めてきたZMPの目標位置から床反力やモーメントが
どのように作用すればいいのかを123行目から129行目で計算しています。
まとめ・次回予告
今回はvnoidパッケージの安定化制御
(Stabilizer) について解説しました。
ひとまずvnoidの標準機能に関する説明は以上となります！
