質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

ただいまの
回答率

88.06%

二足歩行ロボットのプログラムで、歩行関数の部分が理解できません。

受付中

回答 0

投稿 編集

  • 評価
  • クリップ 0
  • VIEW 1,941

score 10

 前提・実現したいこと

二足歩行ロボットの歩行のプログラムを作っています。

しかし、歩行関数の部分の計算がまったく意味が分かりません。
歩行関数から座標が求まることはなんとなくわかるのですが、if文もどういう意味か分からず、その下の計算も全く理解できません。これはどういうのでという感じで、詳しく教えてください。

また、この歩行関数を用いて横歩行を行いたいのですが、その場合逆運動学が使えない(※1)ので、どうやってプログラムを組めばいいのかわかりません。そこについても教えてください。
※1横歩行はロール軸、前後歩行はピッチ軸のため横歩行で使える角度が一つしかない。
(歩行プログラムでwhile文でdelayを使って行うと、変な動きをする。例)ボタンを離しても動くという誤動作が起こる。)

プログラムの説明

このプログラムは、歩行プログラムに歩行関数(TLLD)から得られた座標を逆運動学(LF)の計算式に代入して、座標からサーボモータを動かすための角度を求めて、サーボモータを動かして歩行を実現させています。

 該当のソースコード

/////////////////歩行関数(左足のx軸)////////////////////
void TLLD(float tL, float v_b) {
//前後方向 left(左)
if (tL < eps_X * T / 2.0) {
v_xL = -v_b;
xL   = -v_b * tL;
}
else if (tL < (1.0 - eps_X / 2.0)*T) {
v_xL = (v_b / (1.0 - eps_X)) * (1.0 - cos(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - v_b;
xL   = (v_b / (1.0 - eps_X)) * (tL - (eps_X * T / 2.0) - (T * (1.0 - eps_X) / (2.0 * pi)) * sin(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - (v_b * tL);
}
else {
v_xL = -v_b;
xL   = -v_b * (tL - T);
}
}
///////////////////////////////////////////////

/////////////////歩行関数(左足のz軸)////////////////////
void TLH(float tL, float h) {
//足上げ高さ
if (tL < eps_Z * T / 2.0) {
v_zL = 0.0;
zL   = -H;
}
else if (tL < (1.0 - eps_Z / 2.0)*T) {
v_zL = pi * h / ((1.0 - eps_Z) * T) * sin(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T));
zL   = -h / 2.0 * cos(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T)) + h / 2.0 - H;
}
else {
v_zL = 0.0;
zL   = -H;
}
}
///////////////////////////////////////////////

/////////////////逆運動学//////////////////////
void LF(float x, float y, float z) {
uint16_t val4, val5, val6;
/*逆運動学**/
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = (90.0 - A);
B = degrees( PI - acos(((L1 * L1) + (L2 * L2) - (L3 * L3)) / (2 * L1 * L2)) ); //角度2
C = -(A + B) + 90.0;
val4 = map(AA, -135, 135, 3500, 11500);   //サーボモータは3500~11500の間で動く
val5 = map(B, -135, 135, 3500, 11500);
val6 = map(C, -135, 135, 3500, 11500);
/*ピッチ軸*/
servoset(12, val6 + 400);     //ID_12番をval3の角度に +で↓ 左足首(手動で治す)
servoset(10, val5 - 80); //ID_10番をval2の角度に     -で後ろ   (手動で治す)
servoset(8, val4 - 120);  //ID_8番をval1の角度に      +で後ろ    (手動で治す)
}
////////////////////////////////////////////

////////////////歩行プログラム////////////////////
uint16_t val7,val8;
float dt = 0.04,
v_b = 20.0,
h   = -25.0;
if(t > T){
t -= T;
}
if(tR > T){
tR -= T;
}
if(tL > T){
tL -= T;
}
/* 右足の進行方向平面の動き */
TRLD(tR,v_b);     //歩行関数(TRLD)でx座標を求める
TRH(tR,h);        //歩行関数(RF)でz座標を求める
RF(xR, yR, zR);   //逆運動学よりサーボモータを動かす
/* 左足の進行方向平面の動き */
TLLD(tL,v_b);    //歩行関数(TLLD)でx座標を求める
TLH(tL,h);       //歩行関数(LF)でz座標を求める
LF(xL, yL, zL);  //逆運動学よりサーボモータを動かす
servoset(15, 6500);  //腕
servoset(13, 9000);  //腕
/* 時間を進める */
tR += dt;
tL += dt;
}
////////////////////////////////////////////

 補足情報(初期設定)

/////////////////////初期設定/////////////////////////
v_b=20.0  //歩行速度[mm/s]
eps_x=0.7  //水平方向デューティ比
tL  //時間[s]?
v_xL  //左の水平方向速度[mm/s]
xL  //左の水平方向位置[mm]
dt=0.04  //サンプリングタイム
T=2.0  //歩行周期[s]
tL=3.0*T/4.0;
pi=3.1415926535
/////////////////////////////////////////////////////

  • 気になる質問をクリップする

    クリップした質問は、後からいつでもマイページで確認できます。

    またクリップした質問に回答があった際、通知やメールを受け取ることができます。

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 過去に投稿した質問と同じ内容の質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

質問への追記・修正の依頼

  • coco_bauer

    2018/11/25 11:41

    「ボタンを離しても動くという誤動作が起こる」と捉えるのではなく、「プログラムに誤りがあるため、ボタンを離しても動くという動作が起こる」と考えないとデバッグなんて出来ないですよ。

    キャンセル

  • coco_bauer

    2018/11/25 11:49

    逆運動学の関数 LF の14行目 "servoset(12, val6 + 400); //ID_12番をval3の角度に +で↓ 左足首(手動で治す)" のように、コードでは val6が使われていて、コメントでは val3を使うように書かれている部分はコードのコピーミスなどの可能性が高いので、信じ込まないほうが良いです。

    キャンセル

  • 退会済みユーザー

    2018/11/28 12:52

    複数のユーザーから「やってほしいことだけを記載した丸投げの質問」という意見がありました
    「質問を編集する」ボタンから編集を行い、調査したこと・試したことを記入していただくと、回答が得られやすくなります。

まだ回答がついていません

15分調べてもわからないことは、teratailで質問しよう!

  • ただいまの回答率 88.06%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

関連した質問

同じタグがついた質問を見る