GNSSの位置情報を用いたナビゲーションシステムの開発

[taishiyamamoto]

454 でGNSSの動作確認を行えたため，ソフトウェアの開発を行う．

このissueではGNSSの位置情報を利用したナビゲーションシステムを開発する．

[taishiyamamoto]

具体的な内容は

• [x] rvizにある"2D Pose Estimate"による初期位置合わせを位置情報を利用して行う．

• [x] 初期位置から最も近い距離にあるwaypointを参照し，そこから自律移動を行えるようにする．

とする．

[taishiyamamoto]

orne_navigationでは以下のパッケージを用いてwaypointを管理している． https://github.com/open-rdc/fulanghua_navigation/tree/indigo-devel/fulanghua_waypoints_nav

基本的にfulangha_waypoints_navに機能を追加する形になると思います．

[taishiyamamoto]

vincenty法という緯度から2点間の距離と方位角を計算する手法があるそうです． https://ja.wikipedia.org/wiki/Vincenty%E6%B3%95

実装してみたところ，国土地理院の計算と大差ない精度で収束していると思われます． https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2stf.html

出発点をmap_frame, 到着点をbase_linkにして計算することを想定しています．事前情報としてmap_frameの緯度経度と方位角が必要になります．

[taishiyamamoto]

初期位置から最も近い距離にあるwaypointを参照し，そこから自律移動を行えるようにした．開発の過程で4つのサービスサーバーを追加した． https://github.com/taishiyamamoto/fulanghua_navigation/pull/1/commits/47bd67c3f3a8f2299dc74ee7e315697b8e444ef7

1. /pause_wp_nav ナビゲーションを中断する
2. /unpause_wp_nav ナビゲーションを再開する
3. /stop_wp_nav ナビゲーションを終了する
4. /near_wp_nav tfを参照してbase_linkから最も近い距離にあるwaypointからナビゲーションを行う

[taishiyamamoto]

GNSSを扱う上でPROJというライブラリがあるため，使い方の調査を行う． https://proj.org/usage/quickstart.html

[taishiyamamoto]

コマンドラインで実行できるようになっていたので実行した． PROJのバージョンは4.9.3で行った． 以下のコマンドを入力して座標変換を行った．

$ echo 140.0202 35.6886 | proj +init=epsg:2451 出力結果 16912.89 -34532.19

$ echo 140.0203 35.6891 | proj +init=epsg:2451 出力結果 16921.84 -34476.70

echoでパイプしているのがそれぞれ経度と緯度の数値になっていて+init=epsg:2451でGISに用いるパラメータを入力しています． 2451番は平面直角座標系の第9系に相当しています． https://epsg.io/2451

[taishiyamamoto]

Cで開発できるapiが提供されていて，サンプルプログラムがそのまま使えそうです． https://proj.org/development/migration.html#version-4-to-5-api-migration

とりあえずROSと連携したサンプルプログラムを作成しようと思います．

[taishiyamamoto]

以下のリポジトリで開発しています． https://github.com/taishiyamamoto/gnss_initpose_ros

[taishiyamamoto]

GNSSを座標系に変換するテストを行いました。 https://www.youtube.com/watch?v=x8mpO4A3YRg&feature=youtu.be

rosでは緯度経度を座標に変換する際にはgps_commonというパッケージがよく用いられているようなので、それを利用しています。 http://wiki.ros.org/gps_common

パラメータはマップ原点のUTM座標と向きを調整する必要があります。

[yasuohayashibara]

分かりやすい動画をありがとう． TFで示されている座標がGNSS(u-blox F9)で求めた位置ですね．

結果としては，以下となりますね． LiDARとオドメトリから求めたロボットの自己位置と比較して

• 計測した位置は大きく外れることはない． 10m以上ずれることはなさそうですね．

• 十分に一致しているとはいえない． 道路を外れていたり，屋内に入っていたりすることが多いので，単体で自律移動に使用できる程度ではない．

あと，別件ですが，膨張リセット法のパラメータが適正になったのか，必要なところでリセットがかかる様子が見られますね．（ただし，復帰できていませんが．．．）

[taishiyamamoto]

リポジトリにドキュメントを書いたら閉じる．

[taishiyamamoto]

README書きました． https://github.com/taishiyamamoto/gnss_initpose_ros/blob/master/README.md

[taishiyamamoto]

openrdcのリポジトリにフォークしました． このissueは一旦closeし，問題が発生したらreopenします．