jj1gujのブログ

アイコン画像は音速の奇行子 様よりいただきました

知的・機能工学システム応用実験軌道生成ツールの解説

工学 プログラミング

弊学では秋学期(10月~2月)にという各テーマごとに10人ほどに分かれ、3ヶ月間同じテーマに取り組む知的・機能工学システム応用実験という授業があります。
テーマはFPGAを使って何か実装したり跳躍ロボットを作ったりとハードからソフトまでかなり幅広いです。
僕はその中でロボットシステムという2軸マニピュレータを設計し、実際に動かすというテーマに取り組んでいます。
実際に動かしている様子はこちら↓

このテーマでは実際に2軸マニピュレータを動作させる際に時間ごとの角度情報をプログラムする必要があります。
一応便利ツールとしてExcelファイルが配布されるのですが編集するのに時間がかかりかなり不便なため、これをC++で実装しました。
実装したものはこちらにあります↓

github.com

今回は実装したものの中身というか原理というかを解説しようと思います。

目次

課題の概要

  今回の実験では指定されたコース(直線コースの往復とクランクコースの片道)をなるべく短い時間で動くよう、2軸のマニピュレータのリンク長を設計し、そのリンク長で最も早く動くプログラムを作成することとなっています。
リンク長は可動範囲がA4コピー用紙内全ての場所であればどのような長さ、幅でも問題ありません。
今回の軌道生成ツールはSolidWorksでのモーション解析や実機での動作用に作成したものとなっています。

手先速度の設計

  まず、手先の並進速度をどのようにするかを決定します。
  最初に思い浮かぶのは動作開始から動作終了まで終始一定速度で移動させることだと思います。
一定速度で終始動かすと下のグラフのように、動作開始時と動作終了時に加速度がとても大きくなります。

f:id:jj1guj:20191218162100j:plain
動作速度が常に一定の時の並進速度と加速度
回転体の運動方程式から、モータにかかるトルクは加速度に比例します。
そのため、これだとモータに大きな負荷がかかってしまい、モータのパフォーマンスを十分に生かせなくなってしまいます。
  これを解決するために動作開始時の加速度を小さくすることを考えます。
下のグラフのように加速度を小さくすると今度は加速時間を長くしなくてはならないことが分かります。
f:id:jj1guj:20191218162155j:plain
加速時間を長くし、加速度を小さくした時の速度曲線と加速度
そして、このときの速度曲線は上のグラフのように台形となることが分かります。
このような速度曲線を台形速度曲線といい、今回の手先起動の制御ではこれを使用します。

各時間ごとでの手先位置の設計

  並進速度の速度曲線を決定したら、次は各時間ごとのマニピュレーターの手先座標を考えていきます。
今回は指定されたコースをなるべく短い時間で動かすという目的であったため、加速度を調整するパラメータにしてしまうと直感的にどのくらい早くなるか見積もれません。
そこで、加速終了時刻と減速開始時刻を調整するパラメータとすることで簡単に動作時間を見積もれるようにします。
もし加速時の加速度の絶対値と減速時の加速度の絶対値が同じであれば動作時間は加速終了時刻と減速開始時刻の和になるためです。   この時、加速度と定速時の速度はどのように表すことができるか考えていきましょう。
下のグラフのように加速終了時刻を t _ {1}、減速開始時刻を t _ {2}とします。

f:id:jj1guj:20191218162314j:plain
考える速度曲線
また、加速時の加速度を a、減速時の加速度を -a、定速時の速度を vとします。
  この時、マニピュレータが動く道のり Lは上の図の台形の面積に等しいので
$L= vt _ {2}$ 、 v=at _ {1}から
$$L=at _ {1}t _ {2}$$ $$a=\frac{L}{t _ {1}t _ {2}}$$ よって a=\frac{L}{t _ {1}t _ {2}}、 v=\frac{L}{t _ {2}}となることがわかります。
  次に、加速終了時刻と減速開始時刻はそのままとし、原点 oから点P (p _ {x},p _ {y})まで台形速度曲線に従い動かすことを考えます。この時、 x軸方向に進む道のりは p _ {x}、 y軸方向に進む道のりは p _ {y}であるため、 x軸方向の加速度は $$\frac{p _ {x}}{t _ {1}t _ {2}}$$ y軸方向の加速度は $$\frac{p _ {y}}{t _ {1}t _ {2}}$$ となります。
加速終了時のマニピュレータの手先位置を (p _ {xt1},p _ {yt1})、減速開始時のマニピュレータの手先位置を (p _ {xt2},p _ {yt2})とすると各時間ごとのマニピュレータの手先位置 (x _ {t},y _ {t})は
$$ (x _ {t},y _ {t})= \begin{cases} (\frac{p _ {x}t^{2}}{2t _ {1}t _ {2}},\frac{p _ {y}t^{2}}{2t _ {1}t _ {2}}) & (0 \leqq t \leqq t _ {1}) \\ (p _ {xt1} + \frac{p _ {x}(t-t _ {1})}{t _ {2}},p _ {yt1} + \frac{p _ {y}(t-t _ {1})}{t _ {2}}) & (t _ {1} \leqq t \leqq t _ {2})\\ (p _ {xt2}+\frac{p _ {x}(t-t _ {2})}{t _ {2}}-\frac{p _ {x}(t-t _ {2})^{2}}{2t _ {1}t _ {2}},p _ {yt2}+\frac{p _ {y}(t-t _ {2})}{t _ {2}}-\frac{p _ {y}(t-t _ {2})^{2}}{2t _ {1}t _ {2}}) & (t _ {2} \leqq t \leqq t _ {1}+t _ {2}) \end{cases} $$ となります。

2軸マニピュレータの逆運動学を解く

  逆運動学を解くということは、簡単に言うと「与えられた座標から各モータのモータ角を求めること」です。
今回は座標とそれぞれのモータ角を下の図のようにとります。

f:id:jj1guj:20191218162338j:plain
座標とモータ角の定義
この時、 \theta _ {1}、 \theta _ {2}はそれぞれ
$$ \begin{cases} \ \theta _ {1}=\pi - \tan ^{-1} \frac{y}{x}- \cos ^{-1} \frac{x^{2}+y^{2}+L _ {1} ^{2}-L _ {2} ^{2}}{2L _ {1} \sqrt{x^{2}+y^{2}}}\\ \ \theta _ {2}=\sin ^{-1} \frac{x^{2}+y^{2}-L _ {1} ^{2}-L _ {2} ^{2}}{2L _ {1}L _ {2}} \end{cases} $$ となります。
実際にSolidWorksや実機で動かす際にはラジアンから度に変換しなくてはならないので注意してください。

プログラムへの実装

  ここまできたらあとは実装するだけです！
基本的なアルゴリズムとしては時間ごとの座標を求め、それに対応するモータ角を算出するという方法で問題ありません。
CSVへのファイル出力は、

#include<fstream>
#include<iostream>
using namespace std;

int main(){
    ofstream output;
    output.open("hoge.csv",ios::out);
    output<<0<<","<<"Hello World!"<<endl;
    return 0;
}

とすれば出力できるはずです。 なお、コンマと改行をつけ忘れると実機やSolidWorksに読み込ませることができなくなってしまうのでお忘れなきよう….

今後の展望

とりあえず時代はGUIなのでQtのお勉強してGUI化しようと思います。
あとは作成したプログラムだとクランク起動では各コーナーでいちいち停止してしまってるのでこれを解消するようにしようと思います。

参考文献

回転運動の運動方程式

追記

  • 2021/12/16 そういえばこのときに作成したリポジトリが南極に埋められました(今更)f:id:jj1guj:20211216134234p:plain

archiveprogram.github.com