ICS変換基板の使用方法(6-2) 直線補間でサーボを滑らかに動かす

Posted on 2020.04.03 in

 

前回に引き続き、ICS変換基板とArduino MEGAを組み合わせて、KXR-L6のサーボを制御します。今回の記事では、KXR-L6の歩行制御をする前に、サーボを滑らかに動かすための簡単な直線補間について解説します。この記事で解説する直線補間は、非常に簡単な仕組みになっていますので、他のロボットにも応用できて非常に便利です。ぜひご利用ください。

この記事の内容を実行する場合の必要な準備は、前回の記事をご参照ください。

 

【関連記事】

   ICS変換基板の使用方法(1) 準備編 

   ICS変換基板の使用方法(2) サーボ制御編

   ICS変換基板の使用方法(3) 無線コントロール編

   ICS変換基板の使用方法(4) ID読み書き編

   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次式)で細かく分割して指示(ポジション)を送る方法です。これにより、勢いを抑えて滑らかな動作を実現することができます。本来は時間を基に分割していく方法が適切かもしれませんが、今回はサーボすべてに送る時間を1として何回の指示を送って目標位置に達するかで補間します。

また、KRSサーボの「スピード」パラメータは、モータ自体の出力を下げてしまうためトルクも下がりますが、補間制御によりゆっくり動かすことでトルクへの影響は抑えつつ、スピードを落とすことが可能になります。

 

【制御の流れ】

①指定したポジションをKRSサーボのニュートラル位置7500に合わせた数値に変換(目標値)

②現在値を取得し、目標値との距離(角度差)を求める

③目標値との距離を補間する回数で割り、1ステップのポジションを求める

④KRSサーボに対して、1ステップのポジションを補間回数送信する

 

直線補間のプログラムは下記のようになっています。これは、何度も使用しますので関数化(control_pos)しておきます。

 

引数として、各サーボのポジションデータと補間するときの間隔(delay)、補間回数を受け取ります。後述しますが、この補間間隔と補間回数でサーボの挙動を指定します。

 

補間するために必要な変数と配列を宣言しておきます。配列はサーボの個数分を用意します。

 

二足歩行ロボットの直立状態など、ホームポジションの姿勢がサーボのニュートラル位置(原点)と異なる場合は、予めここにポジションを入れておきます。こうすることにより、ホームポジションからどれくらい動くか=移動角のみを指定できるようになります。

今回は特にホームポジションのポジションデータを指定していません。

 

引数で受け取ったポジションデータを変数に代入しておきます。

 

ロボットのID分の処理を繰り返します。

まず、目標値としてホームポジションのデータと引数で受け取ったデータを加算します。さらにKRSサーボのニュートラル位置である7500を加算することで、ニュートラル位置を中心とした角度になります。これにより、この関数にポジションを指定するときに、ニュートラル位置からの純粋な移動値のみを指定することができるようになります。

下記の二例では同じ動作角を指定していますが、移動角でポジションを指定できたほうが、サーボの動作をイメージしやすく、対象的に動かすときにも楽です。

例)目標値を直接指定する場合、ID3 = 9500 / ID6 = 6500と指定する

  移動値のみ指定する場合、ID3 = 2000 / ID6 = -2000と指定する

 

下記の図は、KRSサーボのポジションと角度の関係についてです。”HTH4”とはモーション作成ソフト「HeartToHeart4」を指します。HTH4では、ニュートラル位置が0ですが、実際にKRSサーボに角度指定するときは7500になります。そこから-4000(3500)~+4000(11500)で角度を指定します。

 

ICS3.6のKRSサーボはgetPosにより現在値を取得することができます。ICS3.5より前のバージョンの場合は、現在値を取得するためのコマンドがありませんので、setFree(脱力する)で取得できる戻り値により現在値を取得します。

▼ICS3.5以前の場合

 pos[i] = krs1.setFree(i+1);
 pos[i+9] = krs2.setFree(i+1);

 

この処理で、現在のサーボ出力軸の位置から目標になる停止位置までの距離を求めます。

 

上記で求めた目標までの距離を補間する回数で割り、1ステップあたりの距離を求めます。

これでサーボにポジションを送るまでの準備は完了です。

 

あとは、1ステップあたりの角度を補間で割った回数分サーボに送信すれば完了です。

delayでサーボに角度を送信するタイミングを調整しています。delayが遅いほどゆっくりな動きになりますが、間隔を開けすぎるとカクカクとした動きになってしまいいます。

補間間隔と補間回数を調整することで適正なモーション再生が可能になります。

 

【補足】

実は、これだけでは目標位置に到達していない場合があります。tposやpos自体が整数であるため、特にtposに小数部分があった場合、目標値を補間回数で割った時の余りが切り捨てられて足されなくなります。結果として微少ではありますが、最終的に目標位置手前で停止する可能性があります。

正確性を求める場合は、最後にすべてのサーボに対して目標位置のポジションを送ることをお勧めします。

 

プログラム全文

 

■ホームポジションを再生して動作確認

関数化された直線補間を利用してホームポジションを再生する場合は、下記のように記述します。

サンプルプログラム<ダウンロード>

 

 

 

前回の記事では、最高速でシュッと動いましたが、今回はジワリとゆっくり動くようになりました。補間間隔と、補間回数を調整することでサーボの動きが変わります。プログラムを先へ進める前にお試しください。

また、この制御方法はヒューマノイド型、アーム型やカメ型など、他のロボットでも利用できます。ArduinoなどマイコンボードからKRSサーボを動かすときは、ぜひご活用ください。

次回の記事では、この直線補間でサーボを制御しつつ、前進歩行や旋回を再生したいと思います。

次回:『ICS変換基板の使用方法(6-3) Arduino MEGAからKXR-L6の歩行制御』