１１７．ラジコンサーボのＳ字加減速制御

2005/04/03 【ソフトウエア編ＴＯＰに戻る】

突然ですが、ここで「ロボットのマネをして下さい。」と言われたら、あなたはどんな動きをして見せますか？多分、１０人中の１０人までが、直線的に腕や腰を動かし、ガクガクっと言った動きをしてみせるでしょう。また、芸が細かい人なら目的の位置まで動かした後に急停止させ、その衝撃で発生する小刻みな振動まで再現するでしょうね。(^^; このように、ガクガクっとした動きはロボットの代名詞になっています。
実際のところはどうでしょうか。企業が作る高価なロボットについてはさすがに滑らかな動きをしていますが、私たちが作るホビーロボットの世界では、まだまだガクガクっという動きが一般的でしょう。現在市販されているホビーロボット用のモーションコントローラについても、速度の指定は出来ても、その動きは急発進／急停止が基本だと思います。

そこで今回は、ラジコンサーボを使って自然で滑らかな動きを行うＳ字加減速制御についてご紹介しましょう。Ｓ字加減速制御では急発進／急停止をするのではなく、一定の加速度で徐々に速度を上げ、続いて一定の負の加速度で徐々に減速して停止します。つまり、滑らかに動いて始動と停止の際に無理な反動による衝撃が発生しづらくなります。Ｓ字加減速制御を行うことによって、ガクガクっと言った動きから開放され、一歩進んだモーションへの足掛かりになるでしょう。

■１１７−１．Ｓ字加減速制御とは

先ず、加速や減速とは何か、またなぜ必要なのか、などについて２０４．加減速に必要なトルクのページで簡単にご紹介していますので、理解を深めるためにも是非ご覧下さい。ここでは、改めてＳ字加減速とは何かついて考えて見ましょう。

さて、Ｓ字加減速制御を使ってラジコンサーボをどのように動かしたら良いのでしょうか。それは、以下に示す２個のパラメータで決定します。

　１．現在地から移動させたい角度　Ｒ（度）、もしくは距離　Ｌ（ｍ）
　２．移動に掛ける時間　Ｔ（秒）

サーボモータで扱う偏移は回転角なので角度Ｒについて考えますが、直感的に分かりやすい距離Ｌに置き換えて説明を進めることにしましょう。最終的には距離Ｌは角度Ｒに容易に置き換えて戻すことができます。


■１１７−１−１．Ｓ字加速曲線

一定の加速度ａ(m/s²）で加速し続けた場合の経過時間ｔ（ｓ）と移動距離ｌ（ｍ）の関係は、ｌ ＝ ａ ｔ ²　で表されます。また、この関係をグラフで表すと以下の通りになります。

	ｌ は ａ を係数とした ｔ の２次曲線となり、時間が経過するごとにその移動距離もうなぎのぼりに大きくなっていきます。これは、常に一定の加速度で速度を増やし続けるためです。ｔ軸でグラフを反転させると、高いところから物を投げた時の放物線と相似になります。物を投げると常に引力で加速され続けるため、空気抵抗を無視すれば落下速度もうなぎのぼりに速くなるのと同じです。 今回のラジコンサーボの制御では、Ｔの時間でＬの距離だけ移動して停止させたいのでした。つまり、どこかで加速をやめて減速に転じるタイミングがあるはずです。 今回のＳ字加減速制御では、そのタイミングを「目的の移動距離の半分まで来た時点」もしくは「目標の移動時間の半分が経過した時点」としてみます。つまり、移動の前半で加速を行い、後半で減速して目的の位置で停止するというシナリオです。
	さて、目的の移動距離の半分はL/2、目標の移動時間の半分はT/2で表されますが、T/2秒後にL/2の距離を移動するためにはどうしたら良いのでしょうか。左のグラフでは点線の位置ということになりますが、これを上の式 ｌ ＝ ａ ｔ 2 に当てはめると、次の式になります。 L/2 = a (T/2)2  　 さらにこれを変形して加速度 a について整理すると、以下の式となります。 a = 2L/T2 つまりこれは、「T/2秒後にL/2の距離まで移動するには、上記の式で導かれる加速度aで加速すれば良い」ということを示しています。そしてこの地点を境にして今度は同じ絶対値を持つ加速度-aで減速すれば、次のT/2秒後には、目標とする移動距離Ｌで自然に停止する計算になります。その様子を示したのが、次のＳ字グラフです。

左のＳ字グラフのうち、左下のエリアが加速度ａによる加速領域で、全行程の半分を過ぎた以降の右上のエリアが加速度−ａによる減速領域になります。 同じ大きさの加速度で減速するため、加速領域の曲線と減速領域の曲線は全く同じ形になります。つまり、T/2秒後にL/2の距離を移動する加速度aで加速し、T/2秒を経過した後すぐに同じ加速度−ａで減速すれば、もともとの「Ｔ秒間でＬの距離を移動して停止する」という目標を達成することができます。そして、グラフはＳ字型をして、始動時にはゆっくり動き出し、徐々に加速して次に減速に転じ、停止前には十分に速度を落として静かに停止する、という特性を示しています。なので、この加減速制御をＳ字加減速制御と呼んでいます。 Ｓ字型ではなく直線型で動作させても、同じ「Ｔ秒間でＬの距離を移動して停止する」という目標を達成することができます。しかし、始動時と停止時には無理な加減速による衝撃が発生し、安定的な動作は望めません。

【先頭に戻る】

■１１７−１−２．Ｓ字加減速制御の具体化

H8のITUを利用してラジコンサーボを動作させるためのPWM波形を出力させる場合、GRAのコンペアマッチをTCNTカウンタクリア要因とするならば、GRAの数値がPWMパルスの周期を決定し、GRBがPWMのパルス幅を決定します。つまり、GRBの数値がラジコンサーボの首振り角度を決定することになります。詳しくは、１１２．ラジコンサーボモータを動かすのページをご覧下さい。

さて、以上のことを踏まえると、ラジコンサーボの首振りを行わせようとした場合、あるGRBの値からあるGRBの値まで連続的にその数値を変化させていけば、その通りに首を振ってくれるということになります。そこで、GRBの首振りの開始点をＬ１、終了点をＬ２とした時、下記のグラフに示すように時間の経過とともにGRBの値をＳ字型に連続的に変化させれあげれば、Ｓ字加減速制御が実現できます。


では、具体的にマイコン上でどのような処理をさせれば良いのか考えてみましょう。大きく分けて、４つのエリアについて処理を分けることとします。

①：正転方向での加速エリア
②：正転方向での減速エリア
③：逆転方向での加速エリア
④：逆転方向での減速エリア

以下に、それぞれのエリアについて、時間の経過とともに変化させるGRBの計算の仕方について具体的に考えて見ましょう。

■加速度aの算出

先ず最初に、事例となる動作での加速度を求めて見ましょう。例えば加減速の目標を以下の通りとします。

正転加減速については「GRBの値をL1=2000からL2=4000までT=1000m秒で変化させて停止する」
逆転加減速については「GRBの値をL1=4000からL2=2000までT=1000m秒で変化させて停止する」

この場合の加速度は以下のように求めます。

a = 2L/T²
  = 2(4000 - 2000)/(1000)²
  = 0.004


■①：正転方向での加速エリア

全移動時間の前半を占める加速エリアです。0秒〜T/2秒の間の任意の時間をｔとすると、GRBの値は以下の式で表されます。

GRB = at² + L1

これは最も基本的な式です。例えば、動作開始後から100m秒後のGRBの値は以下の数値となります。

GRB = at² + L1
      = 0.004 * 100² + 2000
      = 2040


■②：正転方向での減速エリア

次に全移動時間の後半を占める減速エリアです。T/2秒〜T秒の間の任意の時間をtとすると、GRBは以下の式で表されます。

GRB = -a(T - t)² + L2

このエリアでは時間の経過とともに目標とするL2に収束する形としています。例えば、動作開始後から600m秒後のGRBの値は以下の数値となります。

GRB = -a(T-t)² + L2
      = -0.004 * (1000 - 600)² + 4000
      = 3360


■③：逆転方向での加速エリア

次に、逆転方向へ移動する際の前半の加速エリアです。0秒〜T/2秒の間の任意の時間をｔとすると、GRBの値は以下の式で表されます。

GRB = - at^{2 +} L1

これはグラフの通り、初期値であるL1から２次曲線の値を差し引いて算出します。例えば、動作開始後から100m秒後のGRBの値は以下の数値となります。

GRB = - at^{2 +} L1
      = - 0.004 * 100^{2 +} 4000
      = 3960


■④：逆転方向での減速エリア

最後に、逆転方向の後半を占める減速エリアです。T/2秒〜T秒の間の任意の時間をtとすると、GRBは以下の式で表されます。

GRB = a(T - t)² + L2

このエリアでも②と同様に時間の経過とともに目標とするL2に収束する形としています。例えば、動作開始後から600m秒後のGRBの値は以下の数値となります。

GRB = a(T-t)² + L2
      = 0.004 * (1000 - 600)² + 2000
      = 2640


■GRBを更新する周期

それでは、いったいどのタイミングでGRBを算出し、その値を更新すれば良いのでしょうか。それは、「発生させる全てのPWMパルスごとに計算する」ということになります。例えばPWM波形の周期を20m秒とした場合、20m秒ごとにGRBを計算し、その値を更新してあげます。逆に、それ以上の短い周期での更新は意味がありません。ただし、勝手に20m秒ごとに更新するのではなく、PWMパルスのタイミングに合わせて更新しないと、GRBの更新タイミングとコンペアマッチの発生タイミングが合わずに、スムーズな動きが阻害される場合があります。ここでは、PWMパルスの終了タイミング、つまりGRBのコンペアマッチが発生した直後にGRBの値を更新してあげることにします。


■PWM波形のパルス幅は？

ところで、今までお話してきたGRBの値では、実際にどれくらいのパルス幅が出力されるのでしょう。それについては、１１２．ラジコンサーボモータを動かすのページなどを見て頂けると分かりますが、もう一度簡単におさらいしてみましょう。実際に出力されるパルス幅は動作するハードウエアやソフト設定で異なりますが、標準的なAKI-H8で言うと、クロック周波数１６ＭＨｚで、ITUのTCNTカウンタのクロックをφ／８に設定した場合、GRBに設定した値によって、以下のパルス幅を得ることができます。

パルス幅　＝　GRB * 0.5 * 10^-6

例えば、GRBを2000に設定すると、出力されるパルス幅は1m秒、3000なら1.5m秒、4000ならm秒ということになります。

【先頭に戻る】

■１１７−２．Ｓ字加減速関数　srv_ctrl1.c

次に、具体的なプログラム例をご紹介しましょう。H8/3048FのITU0チャンネルを利用したＳ字加減速関数 srv_ctrl( );によりPWM波形を発生させます。この関数は以下の引数を持ち、サーボモータの動きを制御します。

int srv_ctrl(long start,long stop,long m_time)

start : Ｓ字加減速制御を開始するサーボの位置（始点）を、GRBの値で指定します。
stop : Ｓ字加減速制御を終了するサーボの位置（終点）を、GRBの値で指定します。
m_time: 始点から終点までの移動時間を mS で指定します。

なお、関数の終了時には、以下の値を返します。

  1 : 正常終了 、　-1 : 入力値エラー

プログラムはここをクリックするとダウンロードできます。　srv_ctrl1.c  今回のプログラムは、HEW3にKPIT-GNUを組み込んで開発してみました。ただし、H8のヘッダファイルは、AKI-H8コンパイラ純正の3048F.hを利用しています。また、今回の実験に利用しているラジコンサーボはKO PROPOのPDS-2144FETです。

■プログラムの説明

PWM波形を発生させるための各レジスタ設定や、サーボモータを制御するための基本プログラムなどについては、１０６．ＰＷＭでモータ制御　や　１１２．ラジコンサーボモータを動かす　のページをご参照下さい。ここでは細かな説明は省略させて頂き、要点だけをお話することにします。

■main関数のなかでの、srv_ctrl関数の使い方

     while(1){         // Ｓ字加減速動作の実行
          while(srv_ctrl(1500,4500,1500) != 1){};
          while(srv_ctrl(4500,2500,750) != 1){};
          while(srv_ctrl(2500,1500,500) != 1){};
     }

上の記述が、具体的にラジコンサーボを動作させるためのsrv_ctrl関数の使い方です。srv_ctrl(1500,4500,1500)は、GRBが1500から4500の値まで、1500m秒の時間を掛けて移動するという動作を行います。　Ｓ字加減速動作をスムーズに連続して行うためには、連続するsrv_ctrl関数の終点と始点を同じGRBに設定する必要がありますので注意して下さい。
ところで、srv_ctrl関数は正常終了すると1の値を返します。なので、上記のようにwhile( )文の中に入れることによって、サーボの動作が終了するまで次の処理を待機状態にすることができます。


■srv_ctrl関数の処理


     // 引数チェック
     if (m_time <= 0) return -1;

先ず引数のチェックとして、移動時間の指定が0秒以下でないかどうかを確認します。0秒以下だった場合は戻り値に-1を返して関数を以上終了します。

     // 加速度計算
     a = 2*((float)stop-(float)start)/((float)m_time*(float)m_time);

     // 絶対値化（符号クリア）
     if(a < 0){ a *= -1; }

次に、指定の移動量と移動時間に基づいて加速度ａを計算します。計算式は始めにご紹介したa = 2L/T²を利用します。計算結果は始めにご紹介した計算事例の通り１以下になるため、変数をfloat型にします。また、計算に利用しているstop や start や m_timeなどの引数を小数点計算に利用するため、(float)で一時的にfloat型に変換（キャスト）しています。更に計算結果はstart と stop の大小関係によって負になることがあるため、正の値になるように整理しています。

     // 正方向移動
     if((stop-start) > 0){ ......... }
     // 負方向移動
     else if((stop-start) < 0){ .........}

次に、引数で指定されたGRBの値によって、正方向回転か、負（逆）方向回転かを判定します。これにより、

①：正転方向での加速エリア
②：正転方向での減速エリア
③：逆転方向での加速エリア
④：逆転方向での減速エリア

でご紹介しているそれぞれの計算を実行します。

     // 正方向加速制御
          do{
               with = a * time * time + start;
               ITU0.GRB = with;
               pwm_wait();
               time += PWM_PITCH;
          }while(time < (m_time/2));

上記は「①：正転方向での加速エリア」で示される正方向加速制御の処理部分です。
このdo{ }while文は、PWMのパルスを１個発生させるたびにループで１回ずつ実行されます。そして利用されている変数timeは、動作開始からの経過時間を示しています。timeはtime += PWM_PITCH;によって毎回その値が更新されます。PWM_PITCHは#define PWM_PITCH 20;によって定義していて、その値はPWMの周期に合わせておきます。ここでは20に定義していますので、ループごとに20m秒ずつ加算されていきます。従って、with = a * time * time + start;の計算結果はループを繰り返すごとに経過した時間に対応したパルス幅を算出することになります。

ここで、pwm_wait();という関数を利用していますが、それは以下の内容になっています。

/* PWM終了タイミング検出 ********************************/
void pwm_wait(void){
     do{ /* TCNT = GRBなるまで待つ*/
     }while(ITU0.TSR.BIT.IMFB == 0);
     ITU0.TSR.BIT.IMFB = 0; /* 検知フラグを戻して再開 */
return;
}

ここではGRBのコンペアマッチが発生するまで待機状態とする動作を行っています。つまり、PWMのパルス出力が終了するタイミングを検出しているということになります。この関数を利用することで、GRBを更新するタイミングをパルス出力の終了後のタイミングに同期させています。
そして、do{  }while(time < (m_time/2));によって、開始からの経過時間が目標とする移動時間を半分を経過するまでループが繰り返されます。


     // 正方向減速制御
          do{
               with = -1 * a * (m_time-time) * (m_time-time) + stop;
               ITU0.GRB = with;
               pwm_wait();
               time += PWM_PITCH;
          }while(time <= m_time);
     return 1;

続いて、「②：正転方向での減速エリア」で示される正方向減速制御の処理部分に移ります。
ここでも先ほどの正方向加速制御と同様に、減速処理の計算が実行されます。do{  }while(time <= m_time);によって、目標とする移動時間が経過するまでループが繰り返されて終了します。終了後はreturn 1;によって戻り値１を返します。

逆転方向の加速制御と減速制御についても、同様の処理によってGRBの値を連続的に更新し、サーボを動作させます。

【先頭に戻る】

実際にラジコンサーボを動作させてみると、確かにスムーズに加減速動作が実行されます。個人的にちょっと気になるのが、本来の加減速動作とは一定の力を加え続けた結果の加減速であるのに対し、今回の動作は、経過時間に対する移動量のシミュレーションであるという点です。もしかしたらその辺りが根本的に違うのかもしれませんが、始動時と停止時の衝撃は見事に緩和されることと、動作自体が柔らかくやさしい動きになることで、その効果は大きいのではないかと思います。
今回のプログラムでは、ＣＰＵ１個に対してサーボ１個分の制御にしか対応していません。プログラミングによっては複数のサーボに対応させることも可能とは思いますが、ちょっと煩雑になりそうです。加速度計算やそれを使った時刻ごとの位置計算などがあるため、処理内容としてはCPLDなどを使ったモーションコントローラには荷が重そうです。この点では、ＣＰＵを使ったコントローラが優勢かも知れません。
今後はひとつのＣＰＵで複数のサーボに対応させるという方針よりも、ＰＩＣなどに移植して、ひとつのサーボにひとつずつ専用にコントローラを持たせてネットワークで接続する分散制御という方向に進みたいと思っています。ＰＩＣのページでご紹介できるようになるまで、もうちょっとお待ち下さいね。(^^;;

【ソフトウエア編ＴＯＰに戻る】