見出し画像

A*実装までの道のり

お餅にゃむにゃむ

こんばんは。お餅にゃむにゃむです。
今回は僕が自作した「A*アルゴリズム」的な帰還アルゴリズムについて解説していこうと思います。

前提条件

まず、電源を入れた地点を座標(0,0)とする。
電源を入れた際の正面を北として、方位を定める。車体が回転しても方位はズレないので便宜上「絶対方位」と以後記す。

最短経路の算出

ゴールまでの直線距離を計算。三平方の定理使って√(x^2+y^2)するのがベストではあるけど、計算量の増加とrootの処理が面倒な気がしたので、x^2 + y^2で互換した。

実装してみた

現在地から直線的に進める4方位はそれぞれ、((x+1),y) ((x-1),y) (x,(y+1)) (x,(y-1))
ですよね？この値に前述した計算をしてあげます。そして、それらを比較して最も値の小さい座標に進みます。最小値が同じになってしまった場合は、y方向を優先するようにしました。
しかし、この処理だけだと壁があっても突進してしまうので、壁があった場合はその方位の値は無効とします。そうすることで、だだっ広い平面空間においては、確実に帰還することが可能となりました。

脆弱性その1

それは、壁の存在です。
例えば(0,1)の位置にいる時に(0,0)との間に壁があったとしましょう。そうすると、壁を避けて(1,1)の位置に移動したあと再び(0,1)の位置に戻ってしまいます(前述した通り最小値が等しい時y方向を優先するため)
この問題を解決するために行ったのは、直前にいたマスに仮装の壁を作ることです。
そうすることで、同じ場所を反復することがなくなるので、先ほどのループにハマらなくなりました。(制御周期を壁の情報を1回読み取ってる間に仮装の壁判定を10回出せるような周期にしたおかげで実現できました。RTOS様々)

脆弱性その2

その2は、自己位置推定の信頼性です。
現時点での自己位置推定方法(しろくま実装)にはオドメトリを採用しており、車輪の回転数から距離を算出しています。他にも、距離センサの情報と比較し、信頼度の高い方を選び補正をかけたりしてますが、オドメトリはいわば、積分なのでどうしても積分定数Cが出るのと、補正をかけていてもドリフトなどで誤差は蓄積してしまうことです。
長時間走らせると、データ上は(0,0)でも実際は±1程度の座標の誤差が出てしまいます。
そこで、電源を入れた際に壁のある方位を記憶しておき、座標が(0,0)かつ、壁の情報も正しければ、帰還判定とみなしました。そうしたことで精度も多少上がりました。
今後の自己位置推定の精度にもよりますが、逆に考えれば壁の情報が合致していれば±1の座標の誤差は許容する。みたいなこともできそうですよね。abs関数とやらを使って実装してみようかしら。

今後の課題

指定された座標に移動する際に、確実な制御方法が確立されておらず平面で30cm進む時間を求めて走らせるだけの制御をしています。これだと帰還ルートに坂道があったら詰むし、バンプなどで狂う可能性も大いにあります。しかし、距離センサの値を使うにも壁を追従して走るわけではないので、だだっ広い空間に出されたら精度が落ちてしまいます。
(時間制御の方は　l cosθ でうまいことできそうだから希望あるかも？)
まだまだ世界大会に向けて改善の余地は無限にありますね…💦

最後に

個人的に最近の1番大きな進捗は、絶対方位の導入です。これによって本当に扱いやすくなった。直前のマスに仮想の壁作るのも、壁情報との合致を判断するのも、全部それ。神。
そして、全国大会前に比べ、プログラムに慣れてきたのか、機械的な考え方が身についてきたのかわかんないけど、自分の思っている動きをちゃんと実装できている気がします(妥協してる場面がかなり減った)そして、開発時間も平日でありながら毎日3時間は確保し、長い時は10時間してました()

改めてblendに引き抜いてくれてありがとう。
世界で通用するようなプログラム書いてみます。あと、もう少し書き味綺麗にしたい。普通に汚コードなんだよな…慣れてないとはいえ良くない。
ここまで読んでくれてありがとうございました。今後の活動の励みになるので是非いいね押してください！！！

いいなと思ったら応援しよう！