こんにちは、まえけんです。
在宅でアーム研修をしているとロボットの動作が迫力満点です。
今回は軌道生成を行い、アームにゆっくり動いてもらおうと思います。
アームの軌道生成
軌道を計画する箇所の選択
アームの軌道生成と言っても様々なやり方があります。
まずどこで軌道を生成するかを決めます。
次のどちらかで軌道を生成しますが、それぞれ特徴があるので目的に応じて選べると嬉しいです。
- 作業座標(手先座標)
- 手先がどこを通るかを計画が可能
- 関節がどのように動くかわからない
- 計算は少し複雑
- 関節空間
- 関節がどのように動くかを計画が可能
- 手先がどのように動くかがわからない
- 計算はある程度簡単
今回は手先の軌道は問わないので関節空間で軌道生成を行います。
軌道の計算式の選択
次にどのような式で関節角度の軌道を計算するかを選択します。
代表的な式は以下の2つです。
- 速度台形則
- 計算が簡単
- 速度が切り替わるタイミングで加加速度が無限大になる
- モータの加速度の最大値がわかっていると移動時間最短の軌道を計算出来る
- n次の時間多項式
- 次数が増えると計算が複雑
- 次数が増えると軌道のつなぎ目が滑らかになる
- どのような軌道が生成されるかは計算するまで予想するのは難しい
今回はシンプルに実装したいので、マイクロマウスでもおなじみの速度台形則を用います。
詳細は省きますが、5次の時間多項式もマイクロマウスなど様々なロボットでよく使われます。
加速度の次元が2次の多項式になるためロボットが追従しやすい軌道になりやすいという理由もあります。
関節の速度台形則
速度台形の式は各関節毎に計算します。
各関節の速度は以下の3式で与えられます。
関節変位量Sと経過時間tは与えられます。
今回は一定時間で動作することを大切にしたいので、TとTaを与えてa、ωmaxを求めます。
aとωmaxを求められればωも求めることが出来ます。
式から明らかな通り、加速時間Taや軌道の計画時間Tを小さくするとωmaxとaは増大します。
結果としてサーボが追従不可能な軌道が生成される恐れがあるため
パラメータは注意して決定する必要があります。
実装
実装は以下のように行いました。
最初に最大角速度と加速度を求めます。
その後時刻を比較して加速区間、一定角速度区間、減速区間を判別して使う式を変えます。
軌道計画を始めたときの角度に軌道計画で移動すべき分の角度を足した角度がサーボに指令する値です。
double angle = 0;
double diff_angle = target_angle - start_angle;
double omega_max = diff_angle / (move_time*(1.0-accel_time_rate));
double accel_time = move_time*accel_time_rate;
double accel = omega_max/accel_time;
//台形加減速時の関節角度位置を計算
if(now_time <accel_time){
//加速区間
angle = start_angle;
angle += omega_max*now_time*now_time/(2.0*accel_time);
}else if(accel_time <= now_time && now_time <= (move_time - accel_time)){
//定速区間
angle = start_angle + (omega_max*accel_time/2.0);
angle += omega_max*(now_time - accel_time);
}else{
//減速区間
double deceleration_time = (now_time-(move_time-accel_time));
angle = start_angle + (omega_max*accel_time/2.0) + omega_max*(move_time - 2.0*accel_time);
angle += (2*omega_max - omega_max*deceleration_time/accel_time)*deceleration_time/2.0;
}これを制御したい関節の数だけfor文などで回してあげれば、アーム全体の軌道が計算可能です。
動かしてみた
実際に2つの姿勢で軌道生成をしてみました。
以前の記事と比べてゆっくり、オーバーシュートも少なく動作しています！
次回
今回手先はホームポジションへ向けて軌道生成をしました。
次回は手先を一定の姿勢になるようにしてみたいと思います。
