はじめに
こんにちは！はしもとかずさです
ROS 2アドカレ20日目はロボコンで使用した経路探索＆追従について解説しようと思います
今年は大量の大きなオブジェクトがフィールド上に存在しているため、オンラインで経路を探索し追従していました
計算量や計算時間などを考慮し色々工夫しました
もくじ
経路探索法について
自動運転では自己位置とゴールや障害物情報などから適切な経路を選び走行する必要があります
Navigation2では様々な経路探索アルゴリズムが実装されています
今回はロボコン以外でもよく使われるA*とHybridA*について軽く説明します
A*
A*は離散的な2D座標(平面)で行われます
スタートからゴールまでの最短経路を求めます
スタートから4方位や8方位に進んだとしてその時のゴールまでの距離をコストとします
進んだ先で更に進み、コストを追加する動作を繰り返すことでコストが一番小さい進み方が最短経路となります
進むときに障害物を考慮することで障害物を避けながら最短経路を求められます
しかし、ロボットの向きを考慮しないのでロボコンでそのまま使うことは難しいです
Hybrid A*
Hybrid A*はロボットの向きも考慮して探索を行います
A*と違いロボットが移動可能な動き方でゴールを目指します
ただし実装次第では3Dの探索となり計算量が爆発的に増えてしまいます
ロボコンで使うことはできますが、競技中に計算するのは現実的ではありません
今回作成したA*2について
従来の手法(特に実装が多くあり、すぐ使えるもの)では以下の問題がありました
- 複雑な当たり判定(長方形など)を考慮した経路を出せない
- 角度を考慮した経路を出せない
- 横移動や回転移動を同時に行う経路を出せない
- (x, y, yaw)の3Dになってしまうので計算量が多すぎる
ここで私は以下のようなA*、通称A*2を作成しました
- 事前にユーザーが決めた旋回したら当たる範囲に拡張したコストマップ上でxyの探索
- 経路がなめらかになるようにスムージング
- xyが確定した経路に対して複雑な当たり判定で実現可能なyaw各の探索
- yaw各のスムージング
これは従来の手法と違い、A*を2回行うだけでyaw各の探索も当たり判定を守ることもできます
ここで重要な点は以下のとおりです
- xyの探索は壁から離れるようにする
- yawの探索ではindexを戻さないようにする
- yawの探索では+-180度がつながるようにする
こうすることで角度情報がない経路に壁に当たらない姿勢情報を付け加えることができます
yaw角の探索(angular_astar)ではindex, thetaの平面で通常通りA*を行います
これによりxyはある程度最短で到達し、yawは壁に当たらず最短で到達可能となります
ソースコード
全方位移動ロボット向け経路追従について
世の中にはいろいろな経路追従アルゴリズムがあります
PIDもそのうちの一つです
有名なものにPurePursuitとMPCというものがあります
今回はそれらについて軽く説明します
PurePursuit
PurePursuit(以降PP)は超有名な手法です
かんたんに言うと、経路において自分と一番近い点＋1mくらいの点を目標に走る というものです
＋１mくらいのところをlookahead点といい1mのことをlookahead distanceと言ったりします
しかしその方法だとカーブ時に内側に入ってしまったり、速度によって安定性が変わったりしてしまいます
そこで考案されたのがRPPです
これは速度に応じてlookahead distanceを変更することで、高速時は振動しにくいように遠くを見て、低速時は追従精度を上げるために近くをみる というものです
また曲率を計算し減速をすることで高速時でも追従率を上げることができます
また近年DWPPというロボットの加速度、速度制約から経路を完璧に追従するものも存在します
しかしこれらの手法は全方位移動向けではなく、全方位移動に拡張したとしてもyaw角の制御次第で安定しなくなってしまいます
MPC
MPCは未来の軌跡を予測し、コスト関数を最小化する入力を計算することで経路を追従します
障害物回避や速度制約を同時に考慮可能なことが大きなメリットですが、計算量がロボコン向けではないです
またパラメーター調整や実装、理解が難しく、全方位移動に拡張したものもありますが実機で動かすのはとても難しいです
MPC以外にもMPPIなどが存在しますが未来を予測して最適っぽい制御司令を出すというやり方 という認識で(高専ロボコンでは)問題ないと思います
本気でモデル予測制御をやりたい方は以下のリポジトリが参考になると思います
今回作成したHolonomicPurePursuitについて
従来の手法では以下の問題がありました
- ロボットの制約を無視する
- 横移動ができない
- yaw角の制御は完全に無視(自由)
- 計算量が多い
- ゴールで止まりきれない
そこで僕はPurePursuitを改造し、以下を実現しました
- ロボットの加速度、速度制約を守った司令
- lookaheadを速度で変化させる
- yaw角の変化が許容範囲外(yawの加速度、速度制約を満たせない)場合はxy速度を落とす
- yaw角もlookaheadで制御
- ゴール到達時の減速度パラメーターから減速開始点を特定し確実に減速させる
一番のこだわりはlookaheadがlookahead_time後にはx,y,yawともに実現可能な姿勢である点です
せっかくyaw角で当たり判定を厳密に確認し計画した経路もxyが早すぎてyawが追いつかなければ意味がありません
もちろんyawの変化量を減らすようにA*2でスムージングしていますが間に合わないことが多いです
そこで制約から実現可能なlookaheadに更新するようにしました
実際にその更新を入れた場合と入れない場合でどのように挙動が変わるか、シミュレーターですが動画を作成したので貼っておきます
左の動画では旋回が間に合わず、壁に激突(貫通)している様子がよくわかりますが、右ではそれが起こっていないです
ソースコード
論よりRUN!
以下コマンドでNHKロボコンのフィールドで遊ぶことができます
git clone https://github.com/NITIC-Robot-Club/natto_library.git
cd natto_library
./colcon_build.sh
source install/setup.bash
ros2 launch nhk2025_sample nhk2025_sample.launch.xml
RViz2が起動したらゴールを設定すると経路生成、追従を行います
ROS 2 Humbleで走査確認を行っています
最後に
今年のロボコンでは自動運転を結構頑張ったので色々共有、意見交換を今後もやっていきたいです
またROS 2アドカレ24日ではMetaQuest3Sを使って自動運転の監視、可視化などを行った話があるそうです
明日は@KariControlさんによる自作速度プランナー(仮)です