More Related Content Similar to つくばチャレンジ２０２３シンポジウム 発表資料 (18) 3. X. About Me
• お仕事
➢ コンピュータビジョン，ロボティクスなどの研究開発業務．
• 画像処理技術＆ロボティクスの知識を深めたい
• ものづくり大好き
➢ 過去にもロボコン参加歴あり．
• ETロボコン2013
• つくばチャレンジ 2015 ~ 2022
ETロボコンのロボット つくばチャレンジ第二ステージ つくばチャレンジ第三ステージ
4. X. 年表でみる，つくばチャレンジと僕
2015 2016 2017 2018 2019 2020 2021 2022
会社設立
2015年
• つくばチャレンジ初参加
• ハードウェアを作成
• このときは３年で完走するつもり
• 自己位置推定できず，“曲がれないロボット”
• 本走行距離：７０ｍ
• 転職（笑）
2017年
• カメラを使った自己位置推定がおぼろげな
がら実装でき，“曲がるロボット” に進化！
• 本走行：２００ｍ
2018年
• ひとまず先人のキャッチアップに方向転換
• i-cart mini と ROS を使った構成に
• 確認走行を始めてクリア！
• 本走行距離：２００ｍ
• 自己位置推定がずれで，コースアウト．
2019年
• ベース SW に Autoware を使用
• 3D Lidar を採用
• 出発時インタビューで「完走します！」と高ら
かに宣言．直後に自己位置推定破綻
• 本走行距離：２ｍ
• 独立（笑）
2021年
• 横断歩道のルールを勘違い，途中リタイア(/ω＼)
• 本走行としては，初めて確認走行区間を走破
• 本走行：540m
2022年
• 確認走行区間で自己位置推定破綻
• 本走行：140m
未完走のまま，８年目に
突入です．( ﾟДﾟ)
11. X. システム構成 ~ Hardware
ハードウェア構成は昨年から大きく変わらず，i-Cart mini をベースとして，2-D & 3-D Lidar と IMU + Encoder の構成．
X．GPS デバイス（ZED-F9P）
位置の初期化にあった方が便利．
X．RTK位置補正サービス（Ichimill）
ただし，本年度は GPS 未使用．
X．6軸 IMU
(RT USB Output 9-axis
IMU)
X．2D Lidar
（UTM-30LX-EW）
※今回は近接検知（10cm
程度まで）に使用．
X．3D Lidar（VLP16）
メインセンサとして利用．但し，
90cm 程度までしか検知でき
ない．
X．ノートPC
（G-Tune P6-I7G60BK-A）
画像処理等を鑑みると，ゲーミングPCが便
利．バッテリー駆動ではパフォーマンスが落ち
る可能性があるので，要電源供給．
X．モバイルバッテリ
（Jackery ポータブル電源 708）
瞬間最大電力に要注意．小さければ，PC
への供給ができない．
X．駆動用電源
（LONG WP12-12 x 2）
モータ用の電源．
Components Qty Model
Platform 1 i-Cart mini (T-frog Project)
Motor/
Controller
1 TF-2MD3-R6 (T-frog Project)
2D Lidar 1 UTM-30LX-EW
3D Lidar 1 VLP16
IMU 1
RT USB Output
9-Axis IMU Sensor Module
GPS 1 ZED-F9P (未使用)
PC Battery 1 Jackery ポータブル電源 708
Lead-acid
Battery
2 WP12-12
PC 1 G-Tune P6-I7G60BK-A
12. X. システム構成 ~ Software
本年度，ソフトウェアについては刷新．内容としては，，，
１．ROS1 から ROS2 への移行．
２．“ちゃんと” 作ったステートマシン．
３．“ロストしない！” 自己位置推定．
４．といいつつも．．．“万が一のため” の自己位置復帰．
５．“ちょっとだけ柔軟な” 局所的経路計画（ローカルプランニング）．
６．OSS（Teb Local Planner）を使った “華麗な” 障害物回避．
13. X. システム構成 ~ Software 構成図
黄色ブロックが自作モジュール，灰色ブロックが 3rd party のモジュールになります．
（この後の発表で触れる部分は赤色になっています．）
2. “ちゃんと”作ったステートマシン．
3. “ロストしない！”自己位置推定．
4. “万が一のため”の自己位置復帰．
５. “ちょっとだけ柔軟な” 局所的経路計画．
6. OSS を使った，“華麗な” 障害物回避．
15. X. 環境地図作成 ~ 概要
【背景＆動機】
地図作成については，昨年度と同様 Google Cartographer を使用．
３D Lidar (VLP-16) を用いて地図を作製．
昨年度，Autoware の ndt_scan_matcher との組み合わせで上手く動くことを確認済．
※地図作成に関しては特に自作した部分がないため，使用上のコツやハマった点をいくつか紹介．
【使用上のコツ】
コツ１：Cartographer 使用する場合，地図作成の成否は “ループ閉じの成否” にほぼ依存．
コツ２：オドメトリの精度が低いと，パラメータチューニングが難しくなる．オドメトリの精度はある程度担保したい．
初めて使う場合，ループ閉じの感覚をつかむことがとても重要で，下記の順番でやるのがおススメ．
１．GLOBAL SLAM を OFF にして，LOCAL SLAM だけで地図を作る．
２．GLOBAL SLAM を ON にして，ループ閉じのパラメータを大きく緩和する．（誤った閉ループができるくらいに．）
３．おそらく誤った閉ループができているので，ここからループ閉じのパラメータを調整する．
16. X. 環境地図作成 ~ 一連の流れ
【データの取得方法】
１．データの早い段階で，ループ閉じが発生するようにデータを取る．
（今回の場合だと，あえて市役所をもう一周する．）
【パラメータ調整】
１．Lidar のレンジを最大まで使わない．
（今回は 60m (最大100m) で設定．）
２．オドメトリの精度がある程度出ていることを確認．
（市役所一周して，始点と終点が離れすぎていない．）
３．GLOBAL SLAM を OFF にして，
市役所一周分に対してある程度マップができるか確認．
４．ループ閉じの閾値を大きく下げ，ひとまずループを閉じてみる．
５．市役所一周分の地図が正しくできたことを確認し，全コースの地図を作成．
【データ確認】
１．xy 平面だけでなく，xz 平面，yz 平面も確認する．（xy 平面が正しくても，他の平面でずれているとロストする！！）
ここでループが閉じるようにデータを取得し，
チューニングを繰り返す．
17. X. 環境地図作成 ~ 成功例 vs 失敗例
xy 平面で見るとできているが，
xz, yz 平面で路面が反っている．
Lidar から一定半径離れた点
にゴーストが発生したため，使用
距離を短くした．
・成功例 ・失敗例
18. X. ステートマシン ~ 概要
【背景＆動機】
昨年度まで，if 分を使った簡易的な実装で状態遷移をごまかしていた．結果として，，，
モジュールが複雑になり，バグが入りやすくなって開発効率が低下．
翌年度，コードの解読が大変で，再利用性が低下する．
【方針】
ステートマシンを “ちゃんと” 作ってみようと一念発起．(; ･`д･´)
【設計】
状態遷移の定義：紙に状態遷移の定義をきちんと起こす．
フレームワークの使用：Boost の Meta State Machine をフレームワークとして使う．
ただし，ステートマシンを利用しても，状態／遷移条件が多くなると組み合わせが増え，結局複雑になってしまう．
これを避けるため，モジュール毎にステートマシンを分ける設計とした．
19. X. ステートマシン ~ ステートマシンと役割分担
ステートマシンの実装複雑化を避けるため，モジュール毎に分けた． 1. メインステートマシン
役割：
• ユーザとのやり取り．（ボタン押下）
• ステートマシン間の状態仲介
2. プランニングステートマシン
役割：
• 障害物検知．
• 障害物前停止．
• 障害物回避．
• 目標停止点到達
4. ルート管理ステートマシン
役割：
• ルート供給．
• 最終ゴール到達判定．
3. 自己位置管理ステートマシン
役割：
• 自己位置ロスト判定．
• 自己位置復帰．
実装的には，ある程度の量に収まった．
ただ，モジュール間の結合が本当に疎にできるかは要検討．みなさん，どうやって実装してます？？
20. X. 自己位置推定 ~ 概要
【背景＆動機】
昨年度まで，自己位置推定には Autoware の ndt_scan_matcher を利用．走行回数／距離が増えると，どうしても走行中に
ロストする場合が出てくる．昨年の本走行では確認走行区間で自己位置推定をロストし，リタイア．
【方針】
“ロストしない！” 自己位置推定の実現 ＆ “万が一のため” の自己位置復帰ロジック．
【設計】
・オドメトリによる補完：スキャンマッチングの精度悪化をオドメトリによって補完する形にしたい．
・ロスト復帰の仕組み： Autoware の pose_initializer を参考に，自己位置復帰ロジックを作成．
21. X. 自己位置推定 ~ Ndt と Odom の統合
０．はじめに
マップ座標系（ndt の座標系）と odom 座標系において，変換を
T_map_to_odom とする．
x
y
z
z
y
x
Map
Odom
1. スイッチを入れた段階で，map 座標系と odom 座標系の関係（変換）きまる．
2-a. スキャンマッチングとオドメトリが完璧なら，Map 座標系と Odom 座標系の変換は一定．
2-b. 実際には，短い時間間隔ではスキャンマッチングの誤差で変換が変動し，長い時間間隔で
はオドメトリのドリフトが乗ってきて変換がずれてくる．
x
y
z
Map
z
y
x
Odom
z
y
x
z
y
x
x
y
z
Map
x
Odom
z
y
短い周期では，スキャンマッチングの誤差
で，真値近辺で振動するイメージ．
長い周期では，オドメトリのドリフトでズレが
広がっていくイメージ．
x
y
z
z
y
x
Map
Odom
スキャンマッチングとオドメトリの良いとこどりしたい！つまり，，，
“短い周期ではオドメトリを使って，長い周期ではスキャンマッチングを使いたい．”
22. X. 自己位置推定 ~ Ndt と Odom の統合
１．バッファリングされている要素（前提条件）
直近一定時間内における有効な Ndt 位置（Map 座標系），Odom 位置
（Odom 座標系），Ndt と Odom の変換がバッファリングされている．
バッファリングされている Ndt 位置と Odom 位置で時間的に近いものをとって，変換を計算．
t
t
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
x
y
z
t
T_map_to_odom
Ndt 位置
Odom 位置
２．バッファリングされている変換の平均をとって，現在のシステ
ムにおける T_map_to_odom とする．
+ + +
３．新たな Ndt 位置の有効／無効判定
直近の Odometry 位置との変換を計算し，現在のシステムの変換（２で計
算）と比較．差分が一定以内にある場合，この Ndt 位置を有効であるとしてバッ
ファリング．
Ndt の短期的な振動は平均化で緩和し，オドメトリのドリフトはバッファサイズを決め
ることで良いとこどりできた．（EKF を使おうとしたが，ロバストにできなかった．）
23. X. 自己位置推定 ~ 自己位置推定ロスト時の復帰
１．ロスト判定
下記の指標をロストの判定に用いる．
• 最適解までの収束回数．（ndt_scan_matcher）
• 最適解のスコア．（ndt_scan_matcher）
• Ndt 位置の有効／無効判定結果．（Ndt と Odometry の統合）
上記指標をモニタリングし，発生回数が一定以上になった段階でロストと判定する．
※１．上記事象が発生した場合には，Ndt 位置と Odometry 位置の変換（前
述）は更新されていない．
※２．最適解のスコアは環境によって変動するので，直近の収束解に対する値を比
較対象としてバッファリングしておき，この値に対する比の形で閾値を設けた．
２．現在の推定位置を中心として，正規乱数で粒子を生成．
（Autoware の pose_initializer の実装を参考にさせていただいた．）
z
y
x
粒子一つ一つに対してスキャンマッチング．推
定位置と真値の乖離が大きい場合なかなか収
束しないので，ロストしている間にオドメトリでど
こまで位置精度を保てるかが重要．
３．スキャンマッチングと復帰位置の選択
（ndt は Autoware 実装を使わせていただきました．）
スキャンマッチングのスコアが最もよかった粒子に対して，下記のチェックを実施．
• 現在のシステム位置と比較し，解離が一定範囲内．
• ロスト前の最適解のスコアと復帰位置候補のスコアとの比較．
上記の条件が満たされた場合，復帰位置が確定する．
【注意事項】
位置がロストした際にロボットの動きを止めてしまうと，Ndt が毎回同じ
局所解に陥ってしまう．この場合，解が局所解に陥っているのか正しい解
が求まっているのか判断ができなくなるため，位置がロストしてもロボットを
動かす必要がある．
本システムでは，走行速度を落とす仕様とした．
幸いなことに，全走行を通して自己位置ロストは一度も起こらず．
24. X. 局所的経路計画 ~ 概要
【背景＆動機】
PurePursuit Controller による固定ウェイポイントの単純追従を実装していたが，問題が発覚．
１．自己位置推定のズレにより，ウェイポイントと静止物体が重なった場合．
２．植生等の変化で，ウェイポイントが衝突判定された場合．
上記のようなケースで走行を中断してしまう．
【方針】
“ちょっとだけ柔軟な” 局所的経路計画（ローカルプランニング）を実現する．
【設計】
完走への不安要素となるため，事前に与えたパスから大きく外れたくない．これを実現するため，事前に与えたパスの近傍でのみ経路変
更を実施する方法をとった．
25. X. 局所的経路計画 ~ 簡易経路生成
１．レファレンスパスに対するウェイポイント水増し．
データ取得時に取得したパス（ウェイポイント）をレファレンスとして，左右方向にそ
れぞれ n(本) のパスを一定間隔 dy (m) で生成する．
２．全ウェイポイントに対する衝突チェック
水増ししたウェイポイントすべてに対して，衝突チェックを実施．下図では，赤丸が
衝突判定されたウェイポイント．
３．全ウェイポイントに対するコスト計算
個々のウェイポイントに対して，コストを計算する．主に，障害物との距離や横方向
の移動量などを用いて計算．
26. X. 局所的経路計画 ~ 簡易経路生成
４．動的計画法による最適パスの探索
ステップ３にて計算したコスト総和が最小になるパスを動的計画法で計算する．
※実際の様子
27. X. 障害物回避 ~ 概要
【背景＆動機】
簡易経路生成を用いても小さい障害物を回避することはできるが，一定以上の迂回が必要なケースでは対応できない．ただし迂回が大
きくなると経路生成が難しく，時間的に実装が間に合わない．．．
【方針】
OSS（Teb Local Planner）を使った “華麗な” 障害物回避を実現する．
【設計】
障害物回避を下記２通りの方法で実装する．
１．レファレンスパスからの小さな移動量で回避できる場合．
ー＞前述した簡易経路生成によって，停止することなく回避．
２．レファレンスパスからの移動量が一定以上必要な場合．
ー＞障害物前で一定時間停止し，Teb Local Planner に切り替える．
32. X. 実験結果 ~ 本走行の結果
８度目の正直，達成しました！
【結果】
本走行においては，
１．静止障害物（コーン）回避
２．動的障害物との対峙・回避（他チームロボット）
が発生しましたが，問題なくクリアすることができました．
歓喜するアラフォーのおじさん
