こんばんは、また随分ご無沙汰してしまいました。
二脚はいろいろ難しいので四足歩行ロボットを作っていました。
サーボモーターを使っていないので自由には動かないですけど・・・。
こんばんは、また随分ご無沙汰してしまいました。
二脚はいろいろ難しいので四足歩行ロボットを作っていました。
サーボモーターを使っていないので自由には動かないですけど・・・。
こんばんは。着地ロス計算に歩速を見込みました、トップページのC++ソースコードを修正してあります。
ロール方向のロスも見直しました、計算ではなくシュミュレーションの結果です。 シュミュレーション上ですがジャイロなしで足サイズ25㎜×20㎜まで小さくできました。始めた頃に比べると完成度がとても上がりました。まだ誤差はありますがフラットな床上での歩行は問題ないと思います。
leg2 = 0.366 * pow(sin(leg1 * 2.35), 0.85)*(1 - 0.17 * (Vxs * Vxs) / Lx);//前脚の角度(着地ロスからの近似式)
現在ココログリニューアル中で不具合があるようです、コードが正しく表示されません。
少ししか歩けません、腕の振りも逆でした。次回はまともなロボットにしたいと思います。
ロボットは単にKHR-3HVを大きくしただけです。
・コントロールボード RCB-4HV ・サーボモーター KRS-2552×8 KRS-4034HV×4
ジャイロを使ってはいますが、モーションの調整のみでここまではいけます。
またご無沙汰しております。
ずっと挑戦していた膝の曲げ伸ばしによる上体の安定化制御なんですが、ようやく何とかなりました。
歩行モーションのデータ打ち込みどうしてますか。
なるべく楽にKHR-3HV用に変換したかったのでエクセルで編集できないかと色々試しましたがエクセルでは読み込むことは出来てもうまく保存できませんでした。
私のやり方ですが、モーション毎にコピペする必要はありますが角度をすべて打ち込むよりははるかに楽なので紹介します。
元になるモーションデータはHeartToHear4で作っておいてください。角度データの部分のみ修正します。
①変換データのシートを選択してテキスト(スペース区切り)形式で保存しnotepadで開く
②あらかじめ作成した元のモーションをXML Notepad 2007で開く
③当該箇所に変換データを一行毎にコピペする
変換データの作成を追加したシートです・・・ダウンロード WalkExcel_2.1.xls (357.5K)
モーションデータの詳細はKondo KagakuさんのRCB-4のリファレンスセットにありました。
下線の部分です
<Description>ポジション設定</Description>
<Group>Position</Group>
<ConnectedGuids>093c52f2-2845-450f-9fd4-6c6994ef213d</ConnectedGuids>
<ConnectTypes>BeginConnect</ConnectTypes>
<ProgramCode>
<anyType xsi:type="xsd:string">2B 10 3D F3 3F 00 00 1E 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D 4C 1D C1</anyType>
エクセルで、ロボットが自然に歩くモーションを作ることができます。 マクロを使っていますが中身をご覧いただけます。 C++版と同じにしてあります。
ダウンロード WalkExcel_2.0.xls (184.0K)
簡単にするために、ロール方向のロスを見込んでいません。(IKもロール方向は無視して計算) そのせいか、まだロール方向の乗りが今一な気もします。
void Walking(); int gImsec = 0; CRobot robo; double Hsin(double); double Hcos(double); double G = 9.8; int phase = 0; //歩行周期カウンタ(一歩行周期4回) int n = 0; //モーション分割カウンタ //------------モーション時間設定--------------------------------- int dmt = 15; //モーション間隔(ms) int nc = 20; //モーション分割数 int Max_step = 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;//ロール側軸オフセット //-----歩き始めの歩幅設定例-----// if (phase <= 6) { St = 0.035; } if (phase <= 4) { St = 0.015; } if (phase == 2) { St = 0; Sw = Bsw / 4; } if (phase == 1) { St = 0; Sw = Bsw / 4; Sw_offset = 0.0; } //-----止まる時の歩幅設定例-----// if (phase >= (Max_step - 5)) { St = 0.035; } if (phase >= (Max_step - 3)) { St = 0.015; } if (phase == (Max_step - 1)) { St = 0; Sw = Bsw / 4; } if (phase == (Max_step - 0)) { St = 0; Sw = Bsw / 4; Sw_offset = 0.0; } Uh = St * 0.20; //遊脚相の足上げ高さの設定(歩幅に対する比率) Turning = 0.0 * DEG2RAD;//旋回角度の設定 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; } //----------------着地時のロスによる補正------------------------- double Pxs = St; //後脚の位置 double Vxs = Pxs / (Tc * S); //着地前速度 double leg1 = asin(Pxs / Lx); //後脚の角度 double leg2 = 0.36 * pow(sin(leg1 * 2.4), 0.9);//前脚の角度(着地ロスからの近似式) 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 = Vxs * Tc * Hsin(t); Py = Sw / C * Hcos(t); Ux = sin(a[n] * Tmp) / sin(Tmp) * -Pxe; Uy = -Py; Uz = sin(pow((a[n] / 2 + 0.50), 0.75) * M_PI); toe = asin(Uz * Uh / 0.120) * a[n];//踵から着地(足高÷足長) Pratio = 1.0; Uratio = 1 - ratio * a[n] - Uz * Uh / Lx; q0 = Turning * a[n]; } else { t = -(double)Ts / Tc * b[n]; Px = Vxe * Tc * Hsin(t); Py = Sw / C * Hcos(t); Ux = sin(b[n] * Tmp) / sin(Tmp) * Pxs; Uy = -Py; Uz = sin(pow((a[n] / 2), 0.75) * M_PI); toe = -asin(Uz * Uh / 0.120) * b[n];//爪先で蹴り(足高÷足長) Pratio = 1 - ratio * b[n]; //ロス分曲げる Uratio = 1.0 - Uz * Uh / Lx; q0 = -Turning * b[n]; } //-------------------関節角の計算(簡易IK)-------------------------------- //----------立脚相の関節角---------- if (Pratio > 1) { Pratio = 1; } c1 = acos(Pratio); if (Px > Lx) { Px = Lx; } cf = -asin(Px / Lx); q1 = atan((Py - Sw_offset) / Ly * Pratio); q2 = cf + c1; q3 = c1 * 2; q4 = c1 - cf; q5 = q1; q6 = asin(Px / Lx); 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 = -atan((Uy + Sw_offset) / (Ly * Uratio)); q2 = cf + c1; q3 = c1 * 2; q4 = c1 - cf + toe; q5 = q1; q6 = -asin(Px / Lx); 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; }
今晩は、足首サーボにジャイロ入れてみました。
シュミュレーション上ですが、結構いける感じです。
今日は、またずいぶんご無沙汰しております。
人並みの足サイズで歩けるように歩行モーションを極めました。 足着地ロスの計算精度を上げたのが大きいです。
足のサイズは30ミリ×25ミリで、設定はかなり微妙です。さすがに実ロボットでこのサイズを試すのはやめておきます。
最近のコメント