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

三井住友信託銀行プログラミングコンテスト2019 参加

プログラミング 競技プログラミング

三井住友コン参加しました.
問題はこちら
結果はこちら

いつもとかわらず3完でした(4完したかった…) 今回はFortran、Python3、C++と全て違う言語で出しました~

A問題

Fortranで提出しました.
入力された2つの月が異なるとき末日になるのでこれで実装し提出しました.

B問題

Python3で提出しました.
ごちゃごちゃ考えるのが面倒だったのでとりあえず1円から N 円までループを回して、税込価格が N円と等しくなる iが存在するときにはこれを出力し、ないときは':('と出力しました.

C問題

C++で提出しました.
入力された価格 Xを100で割った商が買える品物の個数だと考えました. そのため、あとは Xの下二桁を作るには少なくとも何種類の品物を買えばよいか考え( X \equiv 0\ mod 5なら Xを5で割った商、それ以外なら Xを5で割った商に1を加えたもの)、これが買える品物の個数を超えていたときには'0'、超えていないときには'1'を出力するようにしました.

D問題

ずっと考えていたのですが、時間切れで最後までわかりませんでした.
とりあえずコンビネーション計算を使えば良いと考えていて、あとは重複する組み合わせをどうやって排除するかでずっと悩んでいました(最終的にわからずに諦めてずっとついったやってた).

D問題、解説に書いてあった全探索の解法が頭良すぎてずっと泣いてました.
どうやったらあんな天才解法思いつくんだろう...
あとはF問題が解説読んでみた感じ割と簡単そうだったのでDを捨ててFでずっと考えてたほうが良かったかもですね...
とりあえず今回レートがちょっと回復したので学年変わるまでに緑色になれるよう頑張りたいと思います.

ラズパイ4を買った

コンピュータ プログラミング

レポートの現実逃避で書いてます…
11/25に発売されたラズパイ4、買いました!

あきばお〜で購入すると8 GB MicroSDもついてくるとのことだったので、ここで購入しました.
あとは千石でMicroHDMI-HDMIの変換ケーブル、ケース、USB TypeC-USB TypeAの変換ケーブル、ヒートシンクを購入しました.
お家に帰ったらとりあえずRaspbianをインストールすることに.
公式からダウンロードしてきたのですが、ありえんほど重い…
結局ダウンロードが完了するまでに4時間程かかりました.

みんな、ミラーからダウンロードしような.
まぁそんなこんなあったものの、無事起動できました~.
とりあえず今は普段遣い的な感じで使ってます(Mなのかな？)↓

使い心地としてはちょっと重いかな？ってくらいで全然問題なく使えますね.
ただいかんせんCPU温度が高い(アイドル状態で50℃超える)のでお金たまったら専用ファン買ってつけます.

ABC145参加

プログラミング 競技プログラミング

ABC145参加しました~
結果はこちら
2完しかできず詰みました…
コンテスト中はすべてFortranで出しました.

A問題

  円の面積は \pi r^{2}なので r^{2}を出力して終わりでした.

B問題

  Nが偶数かつ文字列の前半分と後ろ半分が同じときのみYesと出力するようにしました.

C問題

  とりあえず3と4のときの順列を列挙して手で数えてみたところ、特定の隣り合う2つの組み合わせが 2^{N-1}回出てくると勘違いして、各点間の距離の総和に 2^{N-1}/N!をかければよいという結論に至り、実装して試してみたらサンプルケースで見事にコケてしまいました(それはそう). そのため、全探索するかぁってモチベーションになったものの、やりかたがよくわからず、時間がかかりそうだったのでしばらく迷ってから諦めました.

D問題

  与えられた条件から、到達できる点は下の図のように格子状に存在することがわかりました.

f:id:jj1guj:20191117010137j:plain
図1  与えられた条件で到達できる点の分布
ここから、入力された点 (X,Y)が到達できるかどうか判定して、あとはで求めれば良いことがわかったので到達できる点かどうかの判定方法および nと rの求め方を考えていくことにしました.
  まず、到達できる点かどうかの判定法です.
  図1から、
$$y= \frac{x}{2}+b \tag{1}$$ は必ず
$$(a, 2a) ただしa \in \boldsymbol{N} \tag{2}$$ を通ることがわかります. よって、ここから式(1)の bを求めると、 $$2a= \frac{a}{2}+b$$ $$b=\frac{3a}{2}$$ これと (X,Y)を式(1)に代入し、 aについて解くと、 $$Y= \frac{X+3a}{2}$$ $$a=\frac{2Y-X}{3} \tag{3}$$ よって、式(2)から 2Y-Xが3の倍数でないと到達できないことがわかります.
  次に、 nと rを求めます. 図1において、目的地まで原点から上方向にいくつ進まなくてはならないかは、入力された点 (X,Y)を通り傾きが \frac{1}{2}である直線と y=2xの交点の x座標に等しいことがわかります. これは、式(3)と等しいので、これを使います. 横方向には、今度は点 (X,Y)を通り傾きが 2である直線と y=\frac{x}{2}の交点の y座標と等しいことがわかります. よって、これを求めていきます. 点 (X,Y)を通り傾きが 2である直線を $$y=2x+c \tag{4}$$ と置き、 (X,Y)を代入して cを求めると、 $$c=Y-2X$$ よって、式(4)と y=\frac{x}{2}の交点の座標 (l,m)は $$2l+Y-2X=\frac{m}{2}$$ $$l=\frac{2(Y-2X)}{3}$$ よって、 y座標は $$m=\frac{l}{2}=\frac{Y-2X}{3}\tag{5}$$ であることがわかります.
よって、式(3)および式(5)から、 $$n=a+m=\frac{X+Y}{3}$$ $$r=min(a,m)$$ であると求められました.
ここまできたら後は実装するだけだったのですが、効率のよいコンビネーションの求め方がわからず、ひたすらググっていました…
結局、こちらのサイトに載っていたコードを使用させていただきました.
【Python】組み合わせ(nCr) 計算の高速化 - Qiita
ただ、これで実装できたはいいものの、終わったのがコンテスト終了直後という悲しい結末… しかも、提出してみたら普通に通ってしまったという悲しさ…

終了20秒後に出したのが通るとかなんなん…
終わってからずっとほえてました…

反省とか

よかったところ: Fortranの実装速度が上がった
悪かったところ: B問題に見切りをつけるのがおそすぎた
結局レートはちょっと下がってしまいました...(100くらい下がるかと思ってたけどそうでもなかった)

とりあえず来週もABCあるのでレートを戻せるようにがんばります.
あと4~5回くらいで緑色になりたいなぁ…

Tea Break 002参加

プログラミング 競技プログラミング

初めてKakeCoderにアカウント作ってTea Breakに参加しました!!
コンテストページはこちら
TLでコンテスト開催のお知らせが流れてきたのがきっかけです.


いろいろ調べてみたらAtCoder灰色~茶色向けとのことだったので参加してみることにしました.
問題はこちらです.
Fortranが使えないとのことだったのであまり使わないCですべて提出してみました.
なんとか3完できました.
以下試した解法です.
A問題: とりあえず2~N-1で割り切れる数があるかひたすらループ回して求めました.
B問題: 重さをすべて等しくする=平均値が整数=重さの和が4で割り切れるということに気づけたので重さの合計が4で割り切れるか判定するように実装し、ACしました.
C問題: 必要な総コストが3~N+1の総和に等しいことに気づいたのでこれで実装し、ACしました.
D問題: なんかよくわかんないけどDP使うんだろうなぁ…って眺めてたら時間が終わってしまいました...
解説読んでみたら、D問題は全探索でやるんですね…
絶対TLE食らうだろうなぁっていう先入観ガチガチで全然考えてなかった…
計算量を見積もれるようになってきた実感があったんですがまだまだですね…
ただ解いていてとても楽しかったです!!
またあったら参加します!!

第2回全国統一プログラミング王決定戦予選参加

プログラミング 競技プログラミング

日経コン予選参加しました〜.
コンテスト成績はこちら
まあ、爆死しました.
A問題のみの1完でした.
A問題は最初しらみつぶしにループかけて行けばいけるやろって思いながら実装している途中で2で割ればいいことに気づき、AC.
Aを解き終わってからBを読んでみたらなんか「木」とかいうワードが出てきてちょっと嫌な予感がしたので他の問題をのぞきに行くことに.
まあ、どれも難しそうだったのですが、その中でもDとEがいけそうな気がし、Eがこの前のABCで見たような問題(ABC143 D)と雰囲気が似ていたのでEを解いていくことに決めました.
とりあえずC++だし速いからまぁなんとかなるやろってことで を決めて を満たす最大の を見つけていく方針で実装していくことにしました.
最初はダメだったらダメで を見つけるところをにぶたんすればいいや〜って思ってたのですがその前に実装で時間がかかってしまいました…
1時間半ほど頑張って実装し、やっとの思いで提出しましたが、案の定TLE吐かれ、さらに追い打ちでWAも吐かれて一気にやる気がなくなりました…　
試しにいろいろと自分でテストケース作ってやってみたところ、下図のように出力されてしまい、その原因の究明でほとんど時間を使ってしまいました…

(どうしてこうなったのか原因は後日加筆します)
やはり途中で見切りをつけて、順位表を見て解きやすそうな問題にいくべきでしたね(それはそう)
後日AtCoder Problemsで難易度を見てみたところ、Eはかなり難しい部類(Difficulty 2281)でそれは無理だわ…ってなってました.

反省点
・精進が足りない(それはそう)
・Djikstraできない(できてたら多分Dが解けてたはず)
・順位表見なかった
今回の日経コンで後輩(4月から本格的にプログラミング始めた)がとうとう茶色になって、追い抜かれそうなので追い抜かれないように頑張ります.
f:id:jj1guj:20191110231551p:plain

6m&DOWNコンテスト結果発表

アマチュア無線

今年7月に行われた6m&DOWNコンテストの結果が発表されました〜
結果はこちらです
今年は個人局として初めて参加しました
電話部門シングルオペニューカマーで埼玉県新座市(JCC 1330)からの参加となりました
最近体力を使うからと電話をあまりやらず、ずっとCWばっかりだったので体力が持つか心配でしたが、結果的にはなんとかなりました(遠い目)
得点がなんとか1万点を超えたので入賞できるかと思ったのですが、結果は惜しくも1エリア2位で入賞とはなりませんでした
1位との差がわずか1500点だったために悔しいです
やっぱり50MHzが絶望的に飛ばなかったのが響きましたね…
幸いにも当日Eスポが出て6エリアと8エリアがよく受信できたので何度も呼んではみたのですが取ってもらえなくて泣いてました
やっぱ八木欲しいなぁ〜
でもベランダに八木はなぁ…
なんかいい方法あったら誰か教えてくださいorz