ICS変換基板の使用方法（6-3） Arduino MEGAからKXR-L6の歩行制御
前回に引き続き、ICS変換基板とArduino MEGAを組み合わせて、KXR-L6のサーボを制御します。今回の記事では、KXR-L6の歩行制御について解説します。
この記事の内容を実行する場合の必要な準備は、前々回の記事をご参照ください。
※弊社では他社マインボード、及びプログラムに関するお問い合わせついて対応致しかねます。以下の内容は、仕様事例として参考にご一読ください。
【これまでの関連記事】
ICS変換基板の使用方法（4-2） M5StackでID読み書き
ICS変換基板の使用方法（5） Arduino Nano EveryでKRSサーボを制御
ICS変換基板の使用方法（6-1） Arduino MEGAから18個のサーボを制御する
ICS変換基板の使用方法（6-2） 直線補間でサーボを滑らかに動かす
■概要
この記事での歩行制御は、基本的にHeartToHeart4のKXR-L6用サンプルモーションと同じポジションデータを各サーボに送る方法で実行します。また、プログラミングの強みを活かし、一部のポジションデータを変数にしておくことで、歩行時の姿勢の高さや、旋回するときの角度を指定できるようにします。
完成すると以下のように歩行することができます。
【ICS変換基板の使用方法（6-1） Arduino MEGAで18個のサーボを制御する】を公開しました！https://t.co/d565J5RkAb
今回は、KXR-L6を例にArduino MEGAからICS変換基板を経由し、18個のKRSサーボを制御する方法をご紹介します。
完成するとこの動画のようにシャキシャキ歩きますよ！#Arduino pic.twitter.com/KIncyL4Ray
— KONDO_ROBOT (@KONDO_ROBOT) April 2, 2020
■setStrcによるストレッチの設定
KRSサーボのストレッチとは、サーボの出力軸の固さを調整する機能です。数値が小さいほど柔らかくなります。出力軸が固い状態では、ロボットが踏ん張った時にバランスが崩れにくい効果などがありますが、このまま歩行しますとガタガタとした動きになりがちです。人間も力を入れたままうまく歩行できないのと同じ状態になります。ストレッチを適正な柔らかさに変更することで、ガタツキの少ないスムーズな歩行が可能になります。今回の歩行では、ストレッチを50に設定しました。
簡単な設定で効果は非常に高いです。KRSサーボを搭載する場合は、ぜひ設定変更しながらご利用ください。
|
1 2 3 4 5 |
//ストレッチ設定 for(int i = 1; i <= 9; i++){ krs1.setStrc(i, 50); krs2.setStrc(i, 50); } |
■ホームポジションと角度指定
前回の記事で作成したホームポジションを関数化しました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
//脚を上げた時のポジション（MAX：600） int UP_pos2 = 600; //ID2, 5, 8 int UP_pos3 = UP_pos2 + 2600; //ID3, 6, 9 //脚を下げた時のポジション（MIN：-2800） int DW_pos2 = -300; //ID2, 5, 8 int DW_pos3 = DW_pos2 + 2600; //ID3, 6, 9 void Home_Pos(int d_time, int hokan){ int F_MID = 1200; //前脚-中心位置 int M_MID = 0; //中脚-中心位置 int B_MID = 1200; //後脚-中心位置 control_pos(F_MID * (-1), DW_pos2, DW_pos3, M_MID * (-1), DW_pos2, DW_pos3, B_MID, DW_pos2, DW_pos3, //左 F_MID, DW_pos2, DW_pos3, M_MID, DW_pos2, DW_pos3, B_MID * (-1), DW_pos2, DW_pos3, d_time, hokan); //右 } |
各関節のポジションデータを変数に代入してから、補間制御の関数を使用して角度を指定しています。
18個のサーボ全てに対して個別に角度を指定するのは手間がかかりますが、同じ角度のサーボに対しては、ポジションデータを変数化してしまうことで作業が楽になります。また、姿勢を変えるときに変数の数値を変えるだけ処理できますので非常に楽になります。
角度が複数あるとどの状態の角度か分からなくなる可能性がありますが、分かりやすい変数名にすることで脚を上げているのか、下げているのかなどわかりやすくなるメリットもあります。
脚を上下するときの角度指定
UP_pos2、DW_pos2は、胴体寄りのロール軸ID2、5、8の角度です。脚部を上げた位置をUP_pos2、下げた位置をDW_pos2の数値で変更できます。UP_pos2、DW_pos2に連動してその先のサーボID3、6、9のサーボに2666を加算することで、足先がだいたい床を向くようになります。
UP_pos2、DW_pos2変数の数値を変えるだけで、脚を上げる高さと、ボディと床の距離を変えることが可能です。
脚部の上下は、前進歩行や旋回にも影響がありますので、グローバル変数として宣言しています。
ポジション”2666”は、90°です。KRSサーボは、動作角270°に対して8000のポジションで指定しますので、90°は2666になります。
足先が床を向く理由は下記の図を参照してください。
この図は、KXR-L6の脚部を抜粋したものです。pos3のサーボから延びるフレームを床から90°にしたい場合、pos2の動作角θに対して、対角θを90°から引くことでpos3の動作角が求められます。実際は、pos2に対してpos3のサーボは反転していますので、
動作角 ＝ 90° - θ(-1)となります。
これにサーボのポジションを割り当て、θを-300とすると
動作角 ＝ 2666 - (-300)(-1)となり、2366がpos3のポジションとなります。
サンプルプログラムでは、-1の掛け算を省いて予め足し算にしています。
int UP_pos3 = UP_pos2 + 2666;
脚を前後に振るときの角度指定
変数F_MID / M_MID / B_MIDは、胴体のヨー軸それぞれの角度を格納してあります。下記の写真は、ホームポジションの状態です。前脚、後ろ足に1200を指定すると、だいたいこのような姿勢になります。
ゆっくり動かす場合は補間間隔(d_time):30、補間回数(hokan):60程度をお勧めしますが、数値を変えながらお好みのスピードに調整してください。
■前進モーション
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
void Walk_Forward(int c, int d_time, int hokan){ int i; int F_BFR = 2600; //前脚-前に出す int F_MID = 1200; //前脚-中央位置 int F_BAK = 2200; //前脚-後ろに下げる int M_BFR = 400; //中脚-前に出す int M_MID = 0; //中脚-中央位置 int M_BAK = 0; //中脚-後ろに下げる int B_BFR = 1800; //後脚-前に出す int B_MID = 1200; //後脚-中央位置 int B_BAK = 2200; //後脚-後ろに下げる // 歩行ポーズに移行 // control_pos(左半身:ID1, ID2, ID3, ID4, ID5, ID6, ID7, ID8, ID9 // 右半身:ID1, ID2, ID3, ID4, ID5, ID6, ID7, ID8, ID9, 補間間隔, 補間回数); control_pos(F_MID * (-1), UP_pos2, UP_pos3, M_MID * (-1), DW_pos2, DW_pos3, B_MID, UP_pos2, UP_pos3, //左 F_MID, DW_pos2, DW_pos3, M_MID, UP_pos2, UP_pos3, B_MID * (-1), DW_pos2, DW_pos3, d_time, hokan); //右 |
ホームポジションと同じように、予め角度を変数に格納しておきます。前進するときのヨー軸のパターンは、それぞれ3種類しかありません。これを左右順番に送っていくことで、6脚ロボットは歩きます。
それぞれの数値を変更することで、歩幅の調整ができます。左右別にすればスラロームもできますね！
脚を上下に上げる角度は、ホームポジションの時に宣言してありますので、上げるときはUP_pos、下げるときはDW_posを指定してください。
補間間隔は3、補間回数は20程度がお勧めです。
歩行モーションは長いので、全文はサンプルプログラムをご参照ください。
■旋回モーション
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
void Walk_Turn(int c, int dig, int d_time, int hokan){ //引数のint cは旋回する回数です。 int i; int FB_MID = 1200; //前後脚-中央位置 int F_TRN = FB_MID + dig; //前脚-動作角 int B_TRN = FB_MID - dig; //後脚-動作角 int M_MID = 0; //中脚-中央位置 int M_TRN = M_MID + dig; //中脚-動作角 // 指定した回数旋回 // control_pos(左半身:ID1, ID2, ID3, ID4, ID5, ID6, ID7, ID8, ID9 // 右半身:ID1, ID2, ID3, ID4, ID5, ID6, ID7, ID8, ID9, 補間間隔, 補間回数); for(i = 0; i < c; i++) { control_pos(F_TRN * (-1), UP_pos2, UP_pos3, M_TRN, DW_pos2, DW_pos3, B_TRN, UP_pos2, UP_pos3, //左 F_TRN, DW_pos2, DW_pos3, M_TRN * (-1), UP_pos2, UP_pos3, B_TRN * (-1), DW_pos2, DW_pos3, d_time, hokan); //右 |
旋回モーションは、前進モーションに比べて非常に簡単です。ヨー軸のパターンも2種類しかありません。簡単に解説しますと、脚を上げて旋回したい方向に振り、降ろすを繰り返すだけです。サンプルではさらに簡略化し、旋回したい角度（ポジションデータ）を渡せば、ロボットが旋回する角度と方向を指定できるようにしました。
引数digに自然数を指定すると左旋回、マイナスを指定すると右旋回します。脚部同士の干渉からdigに指定できるポジションデータは±1000までです。
■モーション再生
|
1 2 3 4 5 6 7 8 9 10 11 |
void loop() { //ゆっくりとホームポジションへ Home_Pos(30, 60); //前進する Walk_Forward(5, 3, 20); //(歩行回数, 補間間隔, 補間回数) //旋回する Walk_Turn(3, 1000, 3, 20); //(歩行回数, 旋回角度, 補間間隔, 補間回数) } |
Loop内で関数化したモーションを再生します。前進モーション内で繰り返し文を使用することで歩行回数を可変できるようにし、引数で歩行回数を設定できるようにしています。また、旋回モーションは、前述した通り旋回の角度と方向を指定できます。モーションごとに補間間隔、補間回数を指定できますので、モーションにあったスピードで再生することが可能です。
この記事でご紹介した内容のサンプルプログラムは、下記よりダウンロードできます。
サンプルプログラム＜ダウンロード＞
HeartToHeart4のサンプルモーションを参照すれば、多彩なモーションの角度を知ることができます。サーボが多いので面倒ではありますが、6脚ロボットの歩行はパターン化できますので二足歩行ロボットより簡単かもしれません。
また、プログラムを応用していけばセンサに応じた割り込み処理やジェスチャによる動作指示など、RCB-4では実現が難しい処理も可能になります。Arduinoの豊富なオプションを利用して高度なロボット制御にチャレンジしてはいかがでしょうか。ぜひお試しください！
KXR アドバンスセットA Ver.2の詳細をみる KXR アドバンスセットB Ver.2の詳細をみる