安定全振りソフトウェアのお話
ご無沙汰しております。たのロボ！迷路探索アルゴリズム担当のお餅にゃむにゃむです。全国大会優勝記事として、前年度との相関も交えながら解説していきたいと思います。前年度は黒歴史すぎるので、何してたかは書くけど、詳細は語りません…
前提
日本大会では「安定」に全振りしてるため、効率的だとか革新的だとかはあんまりないと思います。でもそこまでバグfix詰めてるんだ。ってとこを見てほしいです。安定してるやつが勝つんです(勝ちました)
大枠のアルゴリズム
マスを進んでいかないとレスキューメイズはお話にならないのですが、進路をどう決定していくのか、2024(今シーズン)は拡張右手法、2023(昨シーズン)は計画的な乱数(笑)で探索してました。昨シーズンは、3:2の比率で右、左を選択していました。壁追従は完璧にできる状態で、フローティングウォールにたどり着く苦肉の策でした。運ゲーしてました実は。実はというか、全部運で勝ちました。
今年度の拡張右手法は、よくある探索方法なのですが、他チームがおそらくしてるであろう「仮想壁」の概念を考えたのですが、見えないところに壁を作るというのはプログラム上で四方を壁に囲まれている状況になって詰みかねないので避けたくて、進路決定を数値的な重み付けですることにしました。
重みとは
大まかな流れを書いていきます↓
右,前,左の順番で1,2,3の数値をあらかじめ振っておく
進路のマスの到達回数×5を足す
壁がある場合はすごく大きい数字に上書きするex.1000
最も小さい数値の方向に進む
まじでこれを無限にしてるだけです。重み付けの強いところは、万が一座標がずれても、そこから重みがより小さいほうに進んでいくだけから、プログラム上で走行不能の判定が絶対に出ないことです。ただ、周囲の到達回数が均一になるまで同じ場所をぐるぐる回る羽目になる可能性を秘めていますが、シンプルに１マス進むのにかかる時間を短縮するというハードウェア性能で解決しました。
switch (exploring.weighting()) {
case 0: // right
// uart1.println("CASE A: right");
servo.suspend = true;
servo.velocity = 0;
movement.turnRight();
break;
case 1: // front
// uart1.println("CASE B: front");
break;
case 2: // left
servo.suspend = true;
servo.velocity = 0;
movement.turnLeft();
// uart1.println("CASE C: left");
break;
}
movement.move_1tile();
exploring.reachedCount[location.x + FIELD_ORIGIN]
[location.y + FIELD_ORIGIN]++;
exploring.updateMap();実際のコードなんですが、探索のメインコードはこんだけでおさまってます。裏の重みの計算も
int Exploring::rightWeight(void) {
int x = location.x + FIELD_ORIGIN;
int y = location.y + FIELD_ORIGIN;
int weight = 0;
if (gyro.direction == WEST && tof.wallExists[NORTH] == false) {
weight = reachedCount[x][y + 1];
} else if (gyro.direction == NORTH && tof.wallExists[EAST] == false) {
weight = reachedCount[x + 1][y];
} else if (gyro.direction == EAST && tof.wallExists[SOUTH] == false) {
weight = reachedCount[x][y - 1];
} else if (gyro.direction == SOUTH && tof.wallExists[WEST] == false) {
weight = reachedCount[x - 1][y];
}
if (weight > 1000) {
weight = 1000;
}
return weight * PASSED_WEIGHT;
}こんな感じで何をしてるかはわかるんじゃないかなーって思います。これは右の到達回数分の重みを加算してる部分で、前と左も同じことをして、それを集約して進路決定をするのがswitch文に突っ込まれてるやつです(語彙力)てか今思えばconstrainで1000頭打ちにすればよかった。歴の浅さ出てる()しかもreturnするときにPASSED_WEIGHTかけてるから1000で頭打ちにできてないわ。でも、まぁどんだけ到達回数多くてもせいぜい重みは100くらいにしかならないから大丈夫ではある
探索アルゴリズムの「安定」ポイント
これが詰んだら元も子もないので、かなり詰めました。2月入るころには、全探索が一応できるようになってはいましたが、３月のバグFix祭で確実に探索できるようにしました。その中でも特に、
・黒タイルの重み
・壁の有無の判定
にはかなりこだわりました。
黒タイルの重み
黒タイルの重みは、壁と同じ重みにしたい。でも、もし座標がずれて黒タイルであろう場所(プログラム上ではそうだけど、実際には違う)をもう一回通った時に、壁よりも重みが大きくなってしまい、いけないはずの壁に突っ込もうとしてLoP、座標ずれで、通れるはずの通路に蓋をしてしまうなどのバグがあったため、
・すごく大きい数値を頭打ちにしてちゃんと統一
・任意の時間で黒タイルの情報をリセット
をするようにしました。単純に地図情報を上書きすればいいという意見もあると思うんですが、チーム内の思想として、探索が地図情報に依存しすぎるのは好ましくないという考えがあったため、このような感じになりました。あと、黒だと思われる場所を通れずに、一生壁だと認識されてしまう可能性があるからよくないなって感じです。
ちなみに昨年度は、黒タイルを踏んだら後退して左に曲がるだけでした()
壁の有無判定
壁の有無判定は、もともと１マス進んで止まった先で、ToFの値を見て決めていたのですが、進んだ先での点としての情報のみの判断だと、行くことができるのに、障害物をたまたまToFが読み取って壁判定を出しかねないので、移動中28-30cm区間に壁がない瞬間が一度でもあったら壁が無い判定を出すようにしてました。この機能が役に立ったかは実際にトラブらないとわかんないので悪魔の証明みたいになっていますが、まぁ障害物を壁として認識する事故は起きていないのでこの機能は良かったのでしょう。昨年度は壁の有無を点のデータで判断してました()
日本大会のフィールドの状態が素晴らしいという前提があっての機能でした。綺麗なコートの提供ありがとうございます。
"1マス進む""90°旋回"に搭載した機能
1マス前進の定義にもかなりこだわりました。見かけ上1マス進んでいるだけですが、裏では
・前詰め補正
・衝突回避
・登坂での減速
・壁と並行補正
をしています。前詰め補正は、自己位置推定では30cm進んだはずなのに、タイルの中央に居ないことをToFで検知し、自己位置推定のタスクを切って、前に詰めることで補正をしています。坂道や段差などで生じる誤差をこれでなしにすることができます。
壁との並行補正は第3Roundでジャイロが10°ほどずれてしまったのですが、それでも最低限の走行ができたことが全てを語っていたでしょう。備えあれば憂いなし。
進路が右or左なら90°旋回をするのですが、その時にも、裏で自動スタック検知の機能があり、練習走行でバンプにハマったときに人間がちょんって押してあげる動作を搭載しました。判断方法としては、プログラム上での今向いている角度と、ジャイロの実測値との不整合を読み取ってあげてる感じです。大鯰の足回りはバンプ乗り越え性能に関しては絶大な信頼があったため、斜め配置のバンプがタイヤにがっちりホールドする状態じゃなければ旋回できるのでスタックを検知したら少し前進してから旋回するようにしてました。得点走行でお披露目することはできなかったのですが、バンプにおびえながら得点走行を見守る羽目にはならなかったです。昨年度は足回りも弱くお祈りするだけでした()
実際の動画はたのロボ！の投稿にある通りです。マジで頭いい。
帰還アルゴリズム
帰還アルゴリズムはダイクストラ法を用いました。当初は重み付けの概念を応用したA*もどきを実装していましたが、重みが同列になった際、進む方向によっては、到達済みの重みで蓋をしてしまい、帰還はできるけれどタイムロスになりかねなかったのでダイクストラ法の最短経路帰還としました。
座標がずれても帰還ができる!?
帰還をするには座標ずれをゼロにしなきゃいけないと皆さん思っているのではないでしょうか。しかし、今回の帰還アルゴリズムでは、
(0,0)だと思われる場所に行く
スタート時に記録した壁情報と一致したら帰還判定
そうでないならば、周囲をうろうろして、誤差±1の範囲で壁情報が一致する場所を探して帰還判定
とすることによって、万が一座標がずれてもリカバリーが効くようになってました。(0,0)でしか帰還判定を出さない座標厳密モードと、座標曖昧壁情報モードの２種類用意してたって感じです。第1Roundでは座標が1ずれてしまいましたが、無事帰還できました。
昨年度も実は似たような仕組みがあったのですが、探索と帰還を分離していなかったので、そもそも乱数探索でスタート地点付近に来なきゃいけないっていう暴挙に出てました…
絶対方位と相対方位
プログラムを書いていく中で、ロボットから見た前後左右と、コートを真上から見た時の前後左右で話を進めたい瞬間があり、絶対方位と相対方位という考え方を取り入れました。相対方位はロボットから見た前後左右で、絶対方位は、ロボット起動時の正面方向を北に固定し、4方位で考える概念です。メインコードでロボットが向く方向などを指定するときは、相対方位だと考えやすいし、裏で地図情報をもとに計算をする際は、絶対方位で俯瞰して考えた方がわかりやすいです。この考え方が何か直接的な効果をもたらしたかはわかんないですが、デバック性能が向上したことは間違いなく、安定に振るためにすごく大事な概念だと思います。
最後に
とまぁこんな感じです。言いたかったこと全部言えたかな。勝てば美学とか言ってた前年度があほらしく思えるくらいには成長したのではないかなと思います。
本番での急なジャイロずれや、なぜか検出できない被災者など、いわゆる「魔物」が現れましたがそれを上回るバグFixで勝ったって感じです。満点取りたかったなぁ～。この目標は世界大会で達成するとしますか。世界大会基準で考えるのであれば、もっと対応しなくてはいけない部分はあると思うので、４か月ちゃんと詰めたいと思います。ハード解決が一番楽ではあるけれども、ハードウェア製作に時間をかけすぎてしまったのが昨年度の反省でもあるので、様々な視点からLoPする可能性を考えて、ロボット自身に判断させれるようにして、世界大会でもLoPゼロを達成したいです。ソフトウェアでLoP検知して回避してます。ってどや顔で言いたい。ｱﾗｼﾀｰ
この記事が気に入ったらサポートをしてみませんか？