ロボットを自然に歩くようにする方法を紹介しています

エクセルを使ってホビーロボットKHR-3HV用の自然に歩くモーションを作る方法と、シミュレーションソフトのGo Simulation!上で歩かせるC++ソースプログラムを紹介しています。 設定を変更すれば他のロボット用のモーションデータも作ることが可能です。

 サンプルモーション (近藤科学株式会社 KHR-3HVのRCB-4HV用)

 調整のためダウンロード一時停止中

   足踏モーション 「foot2.0.xml」をダウンロード

   歩行モーション 「walk2.0.xml」をダウンロード 右クリックして「対象をファイルに保存」を選択してください。

 実際の歩行の様子(足の改造のみです)

 

 調整の手順

  ①直立姿勢で足がまっすぐになるように脚部のトリムを調整する。
  ②直立姿勢で前後・左右に揺らしてみて、ジャイロの効果により安定しているか確認する。
  ③足踏モーションが安定するように足首のトリムを調整する。

  ・フレーム周期は15msです。ジャイロはKHR-3のマニュアル通り足首のみに効かせます。
  ・荷重が足裏全体に掛かるように足首トリムを調整します。(ZMPを足裏中心にする)
  ・動画の足裏のサイズは60㎜×46㎜です、厚さ5㎜の木片を付けてあります。(足裏接地面はフラットです)
  ・無改造の状態では試していませんが、ソールに厚さ数ミリの板を張り付けるか、足首用サーボモーターのトルクを下げれば良いかなと思います。

 

 

自然に歩くモーションをエクセルで作る「WalkExcel_2.2.xls」をダウンロード

 モーション計算に着地ロスを見込むことで、自然で安定した歩行ができます。

   ・できるだけ関節の動きがスムーズになるようにしています。

   ・タイミングが多少ずれてもバランスがくずれないように考慮しています。

   ・歩き方が自然になるようにしています。

 設定画面

Photo_3

 作成されるモーションデータの例

Photo_2

 (注)KHR-3HVのRCB-4HV用のデータにするには手作業でXMLデータの編集が必要です。
 マクロを使っていますが、保護はかけてないので開いてソースコードをご覧いただけます。


◎C++ソースコード
・Microsoft Visual C++ 2017上でビルドしてGo simulation ver1.5.0上のDLLで動かしています。
・ロボットはKHR-3HVの設定になっていて、Go Simulation!上で安定して歩行します。(足首サーボモーターのKp・Kdを下げた方が安定します)

 

void Walking();
double Hsin(double);
double Hcos(double);
double G = 9.8;
int phase = 0; //歩行周期カウンタ(歩行周期4回)
int n = 0; //モーション分割カウンタ
int Max_step = 15 * 2;//歩数(偶数)
//------------モーション時間設定---------------------------------
int dmt = 15; //モーション間隔(ms)
int nc = 20; //モーション分割数
double Ts = (double)dmt * (double)nc / 1000;//(一歩の時間 = dmt × nc × 2 )
//---------------ロボット設定------------------------------------
double Hc = 0.245;//重心高さ(m)
double Lx = 0.130;//ピッチ側の関節間隔(m)
double Ly = 0.207;//ロール側の関節間隔(m)
double Bsw = 0.056;//歩隔=両足の間隔(m)
//---------------------------------------------------------------
double St = 0;
double Sw = 0;
double Uh = 0;
double Sw_offset;
double Turning;
void __stdcall Dll_Run() {
//Time count up
int dt = (int)(robo.GetDt() / 0.001);
gImsec += dt;
if ((gImsec % dmt) == 0) {
phase = gImsec / (dmt * nc) + 1;
n =((gImsec / dmt) % nc) + 1;
St = 0.100 / 2; //歩幅の設定(m)(足踏みモーションではゼロにする)
Sw = Bsw / 2; //歩隔は両足の中心間隔に設定(固定)
Sw_offset = Sw; //ロール側軸オフセット(股関節の間隔)
Uh = 0.05; //遊脚相の足上げ高さの設定(Lxに対する比率)
Turning = 0.0 * DEG2RAD;//旋回角度の設定
//-----歩き始める歩幅設定例-----//
if (phase <= 6) { St = St * 0.70; }
if (phase <= 4) { St = St * 0.35; }
if (phase == 2) { St = 0 ;Uh = 0;Sw = Sw / 2; }
if (phase == 1) { St = 0 ;Uh = 0;Sw = Sw / 2; Sw_offset = 0.0;}
//-----止まる時の歩幅設定例-----//
if (phase >= (Max_step - 5)) { St = St * 0.70; }
if (phase >= (Max_step - 3)) { St = St * 0.35; }
if (phase == (Max_step - 1)) { St = 0 ;Uh = 0;Sw = Sw / 2; }
if (phase == (Max_step - 0)) { St = 0 ;Uh = 0;Sw = Sw / 2; Sw_offset = 0.0;}
Walking();//歩行モーションへ→
//DLL終了
if (phase == Max_step && n == nc) {
PrintMsg("Walking is finished\n");
robo.FinishDll();//finish DLL
}
}
return;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-----------------------------------------------------------------------
// 歩行モーション
//-----------------------------------------------------------------------
void Walking() {
int i;
double a[100]; //計算用係数
double b[100];
for (i = 0; i <= nc; i++) {
a[i] = (double)i / (double)nc;
b[i] = ((double)nc - (double)i) / (double)nc;
}
int fp = phase & 1; //前半・後半フラグ
int fl = (phase >> 1) & 1;//左足・右足フラグ
double t;
double Tc = sqrt(Hc / G);
double C = Hcos(Ts / Tc);
double S = Hsin(Ts / Tc);
double Px; //立脚足位置ピッチ
double Py; //  〃  ロール
double Ux; //遊脚足位置ピッチ
double Uy; //  〃  ロール
double Uz; //遊脚高さ軌跡
double Pratio; //立脚脚長比率
double Uratio; //遊脚脚長比率
double toe; //足首角
double Tmp = M_PI * 3 / 4;
double c1;
double cf;
double q0; //股ヨー
double q1; //股ロール
double q2; //股ピッチ
double q3; //膝
double q4; //足ピッチ
double q5; //足ロール
double q6; //腕
double servoref[20]; //制御角
for (i = 1; i <= 18; i++) { servoref[i] = 0; }
//----------------着地時のロスによる補正(original)---------------
double Pxs = St; //後脚の位置
double Vxs = Pxs / (Tc * S); //着地前速度
double leg1 = asin(Pxs / Lx); //後脚の角度
double leg2 = 0.366 * pow(sin(leg1 * 2.35), 0.85)*(1 - 0.17 * (Vxs * Vxs) / Lx);//前脚の角度(着地ロスからの近似式)
double Pxe = sin(leg2) * Lx; //前脚の位置
double Vxe = Pxe / (Tc * S); //着地後速度
double ratio = 1 - (cos(leg1) / cos(leg2)); //前後脚長比率
//------------------足位置の計算(誤差有)-------------------------
if (fp == 1) {
t = (double)Ts / Tc * a[n]; //立脚後半
Px = Tc * Vxs * Hsin(t);
Py = Sw * Hcos(t * 1.15) / C; //(着地ロス+0.15)
Ux = sin(a[n] * Tmp) / sin(Tmp) * -Pxe; //(original)
Uy = -Py;
Uz = sin(pow((a[n] / 2 + 0.50), 0.75) * M_PI);//遊脚徐々に下げる(original)
toe = Uz * Uh * 4.3 * a[n]; //踵から着地(係数4.3 は脚長÷足部長)
Pratio = 1.0;
Uratio = 1.0 - ratio * a[n] - Uz * Uh;
q0 = Turning * a[n];
}
else {
t = -(double)Ts / Tc * b[n]; //立脚前半
Px = Tc * Vxe * Hsin(t);
Py = Sw * Hcos(t * 0.85) / C; //ロール(着地ロス-0.15)
Ux = sin(b[n] * Tmp) / sin(Tmp) * Pxs; //(original)
Uy = -Py;
Uz = sin(pow((a[n] / 2), 0.75) * M_PI); //遊脚早めに上げる(original)
toe = -Uz * Uh * 4.3 * b[n]; //爪先で蹴り(係数4.3 は脚長÷足部長)
Pratio = 1.0 - ratio * b[n]; //ロス分曲げる
Uratio = 1.0 - Uz * Uh;
q0 = -Turning * b[n];
}
//-------------------関節角の計算(簡易IK)--------------------------------
//----------立脚相の関節角----------
if (Pratio > 1) { Pratio = 1; }
c1 = acos(Pratio);
if (Px > Lx) { Px = Lx; }
cf = -asin(Px / Lx);
q1 = asin((Py - Sw_offset) / (Ly * Pratio));
q2 = cf + c1;
q3 = c1 * 2;
q4 = c1 - cf;
q5 = q1;
q6 = Px / Lx * 1.2;
if (fl == 0){
//-----Rleg-----
servoref[ 1] = q0;
servoref[ 3] = q1;
servoref[ 5] = q2;
servoref[ 7] = q3;
servoref[ 9] = q4;
servoref[11] = q5;
servoref[17] = q6;
}
else {
//-----Lleg-----
servoref[ 2] = q0;
servoref[ 4] = -q1;
servoref[ 6] = q2;
servoref[ 8] = q3;
servoref[10] = q4;
servoref[12] = -q5;
servoref[18] = q6;
}
//----------游脚相の関節角----------
if (Uratio > 1) { Uratio = 1; }
c1 = acos(Uratio);
if (Ux > Lx) { Ux = Lx; }
cf = -asin(Ux / Lx);
q1 = -asin((Uy + Sw_offset) / (Ly * Uratio));
q2 = cf + c1;
q3 = c1 * 2;
q4 = c1 - cf + toe;
q5 = q1;
q6 = -Px / Lx * 1.2;
if (fl == 0){
//-----Lleg-----
servoref[ 2] = -q0;
servoref[ 4] = q1;
servoref[ 6] = q2;
servoref[ 8] = q3;
servoref[10] = q4;
servoref[12] = q5;
servoref[18] = q6;
}
else {
//-----Rleg-----
servoref[ 1] = -q0;
servoref[ 3] = -q1;
servoref[ 5] = q2;
servoref[ 7] = q3;
servoref[ 9] = q4;
servoref[11] = -q5;
servoref[17] = q6;
}
//----------------------角度制御---------------------------------
for (i = 1; i <= 18; i++) { robo.motor.SetRefPos(i, servoref[i]); }
//---------------------------------------------------------------
PrintMsg("phase=%02d, n=%02d, Px=%+5.3lf, Py=%+5.3lf\n", phase, n, Px, Py);
}
double Hsin(double x)
{
return (exp(x) - exp(-x)) / 2;
}
double Hcos(double x)
{
return (exp(x) + exp(-x)) / 2;
}

〇着地ロスからの近似式について

Photo_4

試作の大き目なロボット(身長90㎝)

 

コメント

すみません、しばらくブログをチェックしてませんでした。
「一歩踏み出した瞬間に転倒します」とのことですが、簡単のためにテンポを一定にしている関係上足裏サイズが小さい場合は少しづつ歩幅を広げないとZMPが足裏内から外れ転倒します、VCのソースコードを参考にしてモーションを作成して下さい。あとは足首サーボのパラメータ設定と遊脚の着地直前の位置や角度も重要です。それと逆キネ計算にも誤差があります。 私もかなりの労力を費やしてここまでにまとめております、素人なのでアドバイス出来ることはこの程度です。

 はじめまして、お世話になります。

 当方KHR-3HVの改造モデル(上記URL参照)を歩かせようとして、こちらの記事を参考とさせてもらっております。

 WalkExcel_2.2に必要なモデル情報を登録して、マクロ計算で得られたデータをGo Simulation!のrmoファイルに登録してみたのですが一歩踏み出した瞬間に転倒します(泣)

 ろぼG-様が身長90cmのロボ歩行に成功されていますので、おそらく私のExcel利用方法、rmoファイルへのデータ移植方法が根本的に間違っているのではないかと思います。

 大変恐縮なのですが、歩行成功のためにアドバイスを頂けませんでしょうか。もちろん可能な範囲で構いません。ご教示いただけるならば私のメアドにご一報いただければ、より詳細な情報をお伝えします。

 よろしくご検討ください。

コメントを書く

コメントは記事投稿者が公開するまで表示されません。

(ウェブ上には掲載しません)

無料ブログはココログ