ICS変換基板の使用方法（6-2） 直線補間でサーボを滑らかに動かす
前回に引き続き、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-3） Arduino MEGAからKXR-L6の歩行制御
■直線補間
KXR-L6の全身のサーボを動かす場合は、setPos関数でポジションを指定すればその通り動作しますが、単純に送信しただけでは常に最高速で動こうとするのでうまく歩かせることができません。これを解消するために、簡単な直線補間で制御することをお勧めします。
直線補間とは、始点(現在位置)から終点(目標位置)を直線(1次式)で細かく分割して指示(ポジション)を送る方法です。これにより、勢いを抑えて滑らかな動作を実現することができます。本来は時間を基に分割していく方法が適切かもしれませんが、今回はサーボすべてに送る時間を１として何回の指示を送って目標位置に達するかで補間します。
また、KRSサーボの「スピード」パラメータは、モータ自体の出力を下げてしまうためトルクも下がりますが、補間制御によりゆっくり動かすことでトルクへの影響は抑えつつ、スピードを落とすことが可能になります。
【制御の流れ】
①指定したポジションをKRSサーボのニュートラル位置7500に合わせた数値に変換（目標値）
②現在値を取得し、目標値との距離（角度差）を求める
③目標値との距離を補間する回数で割り、1ステップのポジションを求める
④KRSサーボに対して、1ステップのポジションを補間回数送信する
直線補間のプログラムは下記のようになっています。これは、何度も使用しますので関数化（control_pos）しておきます。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
void control_pos( int dpos0, //左半身-ID1 int dpos1, //左半身-ID2 int dpos2, //左半身-ID3 int dpos3, //左半身-ID4 int dpos4, //左半身-ID5 int dpos5, //左半身-ID6 int dpos6, //左半身-ID7 int dpos7, //左半身-ID8 int dpos8, //左半身-ID9 int dpos9, //右半身-ID1 int dpos10, //右半身-ID2 int dpos11, //右半身-ID3 int dpos12, //右半身-ID4 int dpos13, //右半身-ID5 int dpos14, //右半身-ID6 int dpos15, //右半身-ID7 int dpos16, //右半身-ID8 int dpos17, //右半身-ID9 int d_time, //補間間隔 int hokan) //補間回数 { |
引数として、各サーボのポジションデータと補間するときの間隔（delay）、補間回数を受け取ります。後述しますが、この補間間隔と補間回数でサーボの挙動を指定します。
|
1 2 3 4 5 6 7 |
int i = 0; // カウント1 int c = 0; // カウント2 int pos[18]; // ポジション int tpos[18]; // 目標値 int hpos[18]; // ホームポジション int dpos[18]; // 指定値 |
補間するために必要な変数と配列を宣言しておきます。配列はサーボの個数分を用意します。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
// ホームポジション hpos[0] = 0; //左半身-ID1 hpos[1] = 0; //左半身-ID2 hpos[2] = 0; //左半身-ID3 hpos[3] = 0; //左半身-ID4 hpos[4] = 0; //左半身-ID5 hpos[5] = 0; //左半身-ID6 hpos[6] = 0; //左半身-ID7 hpos[7] = 0; //左半身-ID8 hpos[8] = 0; //左半身-ID9 hpos[9] = 0; //右半身-ID1 hpos[10] = 0; //右半身-ID2 hpos[11] = 0; //右半身-ID3 hpos[12] = 0; //右半身-ID4 hpos[13] = 0; //右半身-ID5 hpos[14] = 0; //右半身-ID6 hpos[15] = 0; //右半身-ID7 hpos[16] = 0; //右半身-ID8 hpos[17] = 0; //右半身-ID9 |
二足歩行ロボットの直立状態など、ホームポジションの姿勢がサーボのニュートラル位置（原点）と異なる場合は、予めここにポジションを入れておきます。こうすることにより、ホームポジションからどれくらい動くか＝移動角のみを指定できるようになります。
今回は特にホームポジションのポジションデータを指定していません。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
// 指定値を配列に代入 dpos[0] = dpos0; dpos[1] = dpos1; dpos[2] = dpos2; dpos[3] = dpos3; dpos[4] = dpos4; dpos[5] = dpos5; dpos[6] = dpos6; dpos[7] = dpos7; dpos[8] = dpos8; dpos[9] = dpos9; dpos[10] = dpos10; dpos[11] = dpos11; dpos[12] = dpos12; dpos[13] = dpos13; dpos[14] = dpos14; dpos[15] = dpos15; dpos[16] = dpos16; dpos[17] = dpos17; |
引数で受け取ったポジションデータを変数に代入しておきます。
|
1 2 3 4 5 |
for(i = 0; i < 9; i++) { // tpos[]に目標値を代入 tpos[i] = 7500 + hpos[i] + dpos[i]; tpos[i+9] = 7500 + hpos[i+9] + dpos[i+9]; |
ロボットのID分の処理を繰り返します。
まず、目標値としてホームポジションのデータと引数で受け取ったデータを加算します。さらにKRSサーボのニュートラル位置である7500を加算することで、ニュートラル位置を中心とした角度になります。これにより、この関数にポジションを指定するときに、ニュートラル位置からの純粋な移動値のみを指定することができるようになります。
下記の二例では同じ動作角を指定していますが、移動角でポジションを指定できたほうが、サーボの動作をイメージしやすく、対象的に動かすときにも楽です。
例）目標値を直接指定する場合、ID3 ＝ 9500 / ID6 ＝ 6500と指定する
移動値のみ指定する場合、ID3 ＝ 2000 / ID6 ＝ -2000と指定する
下記の図は、KRSサーボのポジションと角度の関係についてです。”HTH4”とはモーション作成ソフト「HeartToHeart4」を指します。HTH4では、ニュートラル位置が0ですが、実際にKRSサーボに角度指定するときは7500になります。そこから-4000（3500）～+4000（11500）で角度を指定します。
|
1 2 3 |
// サーボの現在値を取得(ICS3.6) pos[i] = krs1.getPos(i+1); pos[i+9] = krs2.getPos(i+1); |
ICS3.6のKRSサーボはgetPosにより現在値を取得することができます。ICS3.5より前のバージョンの場合は、現在値を取得するためのコマンドがありませんので、setFree（脱力する）で取得できる戻り値により現在値を取得します。
▼ICS3.5以前の場合
pos[i] = krs1.setFree(i+1);
pos[i+9] = krs2.setFree(i+1);
|
1 2 3 4 |
// 現在値から目標値へ動作するため // 目標値と現在値の距離を求める tpos[i] -= pos[i]; tpos[i+9] -= pos[i+9]; |
この処理で、現在のサーボ出力軸の位置から目標になる停止位置までの距離を求めます。
|
1 2 3 4 5 |
// 目標値までの距離を補間回数で割り // 1回あたりのポジションを求める tpos[i] /= hokan; tpos[i+9] /= hokan; } |
上記で求めた目標までの距離を補間する回数で割り、1ステップあたりの距離を求めます。
これでサーボにポジションを送るまでの準備は完了です。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
// 補間動作で全身のサーボモータを動かす for(i = 0; i < hokan; i++) { for(c = 0; c < 9; c++) { pos[c] = pos[c] + tpos[c]; krs2.setPos(c+1,pos[c]); // 各サーボにポジションデータを送信する pos[c+9] = pos[c+9] + tpos[c+9]; krs3.setPos(c+1,pos[c+9]); // 各サーボにポジションデータを送信する } // サーボを滑らかに動かすように指定した間隔を空る delay(d_time); } |
あとは、1ステップあたりの角度を補間で割った回数分サーボに送信すれば完了です。
delayでサーボに角度を送信するタイミングを調整しています。delayが遅いほどゆっくりな動きになりますが、間隔を開けすぎるとカクカクとした動きになってしまいいます。
補間間隔と補間回数を調整することで適正なモーション再生が可能になります。
【補足】
実は、これだけでは目標位置に到達していない場合があります。tposやpos自体が整数であるため、特にtposに小数部分があった場合、目標値を補間回数で割った時の余りが切り捨てられて足されなくなります。結果として微少ではありますが、最終的に目標位置手前で停止する可能性があります。
正確性を求める場合は、最後にすべてのサーボに対して目標位置のポジションを送ることをお勧めします。
プログラム全文
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 |
void control_pos( int dpos0, //左半身-ID1 int dpos1, //左半身-ID2 int dpos2, //左半身-ID3 int dpos3, //左半身-ID4 int dpos4, //左半身-ID5 int dpos5, //左半身-ID6 int dpos6, //左半身-ID7 int dpos7, //左半身-ID8 int dpos8, //左半身-ID9 int dpos9, //右半身-ID1 int dpos10, //右半身-ID2 int dpos11, //右半身-ID3 int dpos12, //右半身-ID4 int dpos13, //右半身-ID5 int dpos14, //右半身-ID6 int dpos15, //右半身-ID7 int dpos16, //右半身-ID8 int dpos17, //右半身-ID9 int d_time, //補間間隔 int hokan) //補間回数 { int i = 0; // カウント1 int c = 0; // カウント2 int pos[18]; // ポジション int tpos[18]; // 目標値 int hpos[18]; // ホームポジション int dpos[18]; // 指定値 // ホームポジション // 二足歩行ロボットの直立した状態などホームポジションが // サーボのニュートラルでない場合は、ここにポジションを入れておきます。 hpos[0] = 0; //左半身-ID1 hpos[1] = 0; //左半身-ID2 hpos[2] = 0; //左半身-ID3 hpos[3] = 0; //左半身-ID4 hpos[4] = 0; //左半身-ID5 hpos[5] = 0; //左半身-ID6 hpos[6] = 0; //左半身-ID7 hpos[7] = 0; //左半身-ID8 hpos[8] = 0; //左半身-ID9 hpos[9] = 0; //右半身-ID1 hpos[10] = 0; //右半身-ID2 hpos[11] = 0; //右半身-ID3 hpos[12] = 0; //右半身-ID4 hpos[13] = 0; //右半身-ID5 hpos[14] = 0; //右半身-ID6 hpos[15] = 0; //右半身-ID7 hpos[16] = 0; //右半身-ID8 hpos[17] = 0; //右半身-ID9 // 指定値を配列に代入 dpos[0] = dpos0; dpos[1] = dpos1; dpos[2] = dpos2; dpos[3] = dpos3; dpos[4] = dpos4; dpos[5] = dpos5; dpos[6] = dpos6; dpos[7] = dpos7; dpos[8] = dpos8; dpos[9] = dpos9; dpos[10] = dpos10; dpos[11] = dpos11; dpos[12] = dpos12; dpos[13] = dpos13; dpos[14] = dpos14; dpos[15] = dpos15; dpos[16] = dpos16; dpos[17] = dpos17; for(i = 0; i < 9; i++) { // tpos[]に目標値を代入 tpos[i] = 7500 + hpos[i] + dpos[i]; tpos[i+9] = 7500 + hpos[i+9] + dpos[i+9]; // サーボの現在値を取得(ICS3.6) pos[i] = krs1.getPos(i+1); pos[i+9] = krs2.getPos(i+1); /* // サーボの現在値を取得(ICS3.5) // ICS3.5は現在値を取得するコマンドがありませんので // setFreeコマンドの戻り値で角度を読み込みます。 pos[i] = krs1.setFree(i+1); pos[i+9] = krs2.setFree(i+1); */ // 現在値から目標値へ動作するため // 目標値と現在値の距離を求める tpos[i] -= pos[i]; tpos[i+9] -= pos[i+9]; // 目標値までの距離を補間回数で割り // 1回あたりのポジションを求める tpos[i] /= hokan; tpos[i+9] /= hokan; } // 補間動作で全身のサーボモータを動かす for(i = 0; i < hokan; i++) { for(c = 0; c < 9; c++) { //現在値に補間回数で割った距離を順次加算し目標値に移動する pos[c] = pos[c] + tpos[c]; krs1.setPos(c+1,pos[c]); // 左半身の各サーボにポジションデータを送信する pos[c+9] = pos[c+9] + tpos[c+9]; krs2.setPos(c+1,pos[c+9]); // 右半身の各サーボにポジションデータを送信する } // サーボを滑らかに動かすように指定した間隔を空る delay(d_time); } } |
■ホームポジションを再生して動作確認
関数化された直線補間を利用してホームポジションを再生する場合は、下記のように記述します。
|
1 2 3 4 5 6 7 |
void loop() { //ホームポジションへ移動 control_pos(-1200, -300, 2366, 0, -300, 2366, 1200, -300, 2366, //左 1200, -300, 2366, 0, -300, 2366, -1200, -300, 2366, 20, 60); //右 } |
サンプルプログラム＜ダウンロード＞
【ICS変換基板の使用方法（6-2） 直線補間でサーボを滑らかに動かす】を公開しました！https://t.co/unx8TMwc3R
今回は、ロボットを歩行させる前に簡単な直線補間でKRSサーボを滑らかに動かす方法を解説しています。
他のロボットでも利用できますので、ぜひご一読ください！#Arduino pic.twitter.com/JYCp7VW8e7
— KONDO_ROBOT (@KONDO_ROBOT) April 3, 2020
前回の記事では、最高速でシュッと動いましたが、今回はジワリとゆっくり動くようになりました。補間間隔と、補間回数を調整することでサーボの動きが変わります。プログラムを先へ進める前にお試しください。
また、この制御方法はヒューマノイド型、アーム型やカメ型など、他のロボットでも利用できます。ArduinoなどマイコンボードからKRSサーボを動かすときは、ぜひご活用ください。
次回の記事では、この直線補間でサーボを制御しつつ、前進歩行や旋回を再生したいと思います。
次回：『ICS変換基板の使用方法（6-3） Arduino MEGAからKXR-L6の歩行制御』
KXR アドバンスセットA Ver.2の詳細をみる KXR アドバンスセットB Ver.2の詳細をみる