こんばんは、また随分ご無沙汰してしまいました。
二脚はいろいろ難しいので四足歩行ロボットを作っていました。
サーボモーターを使っていないので自由には動かないですけど・・・。
こんばんは。着地ロス計算に歩速を見込みました、トップページのC++ソースコードを修正してあります。
ロール方向のロスも見直しました、計算ではなくシュミュレーションの結果です。 シュミュレーション上ですがジャイロなしで足サイズ25㎜×20㎜まで小さくできました。始めた頃に比べると完成度がとても上がりました。まだ誤差はありますがフラットな床上での歩行は問題ないと思います。
leg2 = 0.366 * pow(sin(leg1 * 2.35), 0.85)*(1 - 0.17 * (Vxs * Vxs) / Lx);//前脚の角度(着地ロスからの近似式)
現在ココログリニューアル中で不具合があるようです、コードが正しく表示されません。
ロール方向の着地ロス分の補正を簡易ですが入れてみました。シュミレーション上ですが、ジャイロなしで足サイズ３０㎜×２０㎜までいけました。 かなり良くなった感じです。
ロール方向の着地ロスは歩隔によってかわりますが、KHR-3HVのシュミレーション上では１０％位が良いようです。
みっともないロボットになっちゃいましたが、外で試してみました。
少ししか歩けません、腕の振りも逆でした。次回はまともなロボットにしたいと思います。
ロボットは単にKHR-3HVを大きくしただけです。
・コントロールボード RCB-4HV ・サーボモーター KRS-2552×8 KRS-4034HV×4
ジャイロを使ってはいますが、モーションの調整のみでここまではいけます。
またご無沙汰しております。
ずっと挑戦していた膝の曲げ伸ばしによる上体の安定化制御なんですが、ようやく何とかなりました。
ロボットは、Go Simulation!上のKHR-3HVです。
上体の姿勢により膝の曲げ角をフィードバック制御しました。
凸凹に対応できるよう、足裏の面積をできるだけ小さくしています。（20㎜四方）
・床反力制御 、なし（固定角度）
・目標ZMP制御、有り（膝曲げ角のフィードバック制御）
・着地位置制御、なし（固定角度）
凸凹に対応できるよう、足裏の面積をできるだけ小さくしています。（20㎜四方）
・床反力制御 、なし（固定角度）
・目標ZMP制御、有り（膝曲げ角のフィードバック制御）
・着地位置制御、なし（固定角度）
歩行モーションのデータ打ち込みどうしてますか。
なるべく楽に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)
簡単にするために、ロール方向のロスを見込んでいません。（ＩＫもロール方向は無視して計算） そのせいか、まだロール方向の乗りが今一な気もします。
こんにちは、またまた随分とご無沙汰しております。
二足歩行ロボットのシュミュレーション用C++ソースコード「自然な歩き方をする」タイプです。
・Microsoft Visual C++ 2017上でビルドしてGo simulation ver1.5.0上のDLLで動かしています。
・ロボットはKHR-3HVの設定になっていて、Go Simulation!上で安定して歩行します。（足を小さくするには、足首のKp･Kdを下げた方がよいです）
・「人間のように自然な歩き方をする」タイプです。着地時のエネルギーロスを回復する膝の曲げ角を近似式を作り計算しています。しかし、タイミングは正確に計算できないのでリニアなままですが、「膝を曲げて歩く」モーションよりも安定している感じなのでこの方が断然いいです。
・・・ずいぶん苦労しました、アホですよね（ ＾ω＾）・・・
void Walking();
int gImsec = 0;
CRobot robo;
double Hsin(double);
double Hcos(double);
double G = 9.8;
int phase = 0; //歩行周期カウンタ（一歩行周期４回）
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;
}
今晩は、足首サーボにジャイロ入れてみました。
シュミュレーション上ですが、結構いける感じです。
今日は、またずいぶんご無沙汰しております。
人並みの足サイズで歩けるように歩行モーションを極めました。 足着地ロスの計算精度を上げたのが大きいです。
足のサイズは３０ミリ×２５ミリで、設定はかなり微妙です。さすがに実ロボットでこのサイズを試すのはやめておきます。
携帯URL
最近のコメント