vector map navigation

[kyo0221]

概要

• e2e学習用のデータ収集の自動化
• 今後のルールベース制御の流用可能性
• map推論用の教師データにできる

という観点から，vector mapに基づくナビゲーションを行うパッケージ群を開発する

ToDo

• パッケージ群の作成
• シミュレータでの検証
• 実環境での検証
• 検証結果に基づく修正

[kyo0221]

mapping

• map serverのような役割
  • vectormapを保持し，rosのtopicとして出版
  • vectormapはlanelet2をベースにした情報を持つ

localization

• gnssから得た位置をベースに，白線に基づくマップマッチングで補正

planning

• vectormapと自己位置情報を受け取る
• 事前に走行ルートをyamlに記載（各laneletはidを持つ）し，global pathを生成
• 障害物回避経路はfrenet座標系に置き換えて導出

control

• planningで生成した経路に対して追従制御を行う

tf

• 複数の座標系を使い分ける必要があるので，tfを利用
• earth→map→odom→base_link

[kyo0221]

localizationではICPによるmapmathingを行っている．
ここで，vector mapはmap座標系であり，カメラから認識する白線情報はスクリーン座標系であるため，座標変換が必要となる．

座標変換に関してカメラの内部パラメータ等が重要になるので，以下に値を示す．

cameraの取り付け位置

• x : 0.055
• y : 0.0
• z : 0.54
• roll : -1.53938040026
• pitch : -0.00349065850
• yaw : -1.57428698530

画像サイズ

• 640x360

内部パラメータ

• fx : 246.55866667
• fy : 246.389
• cx : 318.345
• cy : 186.59033333

[kyo0221]

上記パラメータでのicpをシミュレータ環境で検証した時の動画
https://youtu.be/Fzy1kMlIpZo

赤ベクトル：補正前自己位置
黄色点：補正前自己位置から見た白線情報
covaliance付き赤ベクトル：補正後自己位置
水色点：補正後自己位置から見た白線情報

なので，水色の点群がマップと一致していれば良いということになる．
微妙な位置のズレはicpアルゴリズム側のパラメータチューニングでどうにかなりそうだが，位置がガタガタしているのが問題となるため，そこの対処が必要と考える．

[kyo0221]

localizationについて，自車の速度状態を利用し，ekfを実装したら良い挙動になった
https://youtu.be/ZEG7mcGyEJQ

[kyo0221]

pure pursuitベースの追従器も開発したので，自律で動作させることができた
https://youtu.be/cX0ziSqDYLA

[kyo0221]

レーン切り替え機能を実装しました．
https://youtu.be/njymSvqE_XI

std_msgs/msg/Emptyの/flagトピックをトリガーにして，vectormapに含まれるlanelet情報から隣接laneletを辿り，そこのレーン中心線をglobal pathに射影して移動しています．

マップには2車線しか含まれないことを前提に作成しているので，レーン切り替えの左右指定はありません．

[kyo0221]

レーンの切り替えに加え，経路選択機能も実装しました．
https://youtu.be/-qdTL6iAGgo

コントローラ操作での操作をトリガーにしています．
また，global pathの更新条件をlanelet到達毎に変更しました．

自己位置推定と追従側をもう少し詰める必要がありそう

[kyo0221]

navigation2のnav2_controllerの実装を参考に，control側でpluginlibベースの実装をしてみました．

現状はpure pursuitの実装だけですが，ControllerPluginを継承したクラスを実装し，plugins.xmlに登録してcontroller_pluginのパラメータで名前を指定することで切り替えができます．

[kyo0221]

障害物回避の検証が完了しました．
従来通り，点群データから障害物情報を求める構成です．

https://youtu.be/ROfWI_mSw-Y

[kyo0221]

前回の実験走行で検証したところ，白線マップマッチングの際に，座標変換のズレが生じていたことでマップとの正確なマッチングが成立していなかった
また，自己位置が隣のレーンまでズレた際に，ズレた先の経路を通るような実装になっているため，そちらを廃止する．（自己位置が信頼できない場合に，分岐路で動作が不安定になる原因になりそう）

よって，以下の修正を行う

• 実機でのカメラ行列調整
• 自己位置に応じて経路を変える機能の廃止

[kyo0221]

自己位置に応じて経路を変える機能の廃止

• lanelet接続情報から，接続lane以外には遷移しないように修正
• 車線変更をglobal path再計画の形で行うように修正

動作確認済み

[kyo0221]

実機でのカメラ行列調査

zed sdkからカメラ行列の値を調査
あとは取り付け位置の誤差（外部パラメータ）が影響しそう

image_width:  640
image_height: 360
distortion_model: plumb_bob

# D: [k1, k2, p1, p2, k3]
D: [0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000]

# K: 3x3 camera matrix
K: [249.122009, 0.000000, 325.983643,
    0.000000, 249.122009, 187.070150,
    0.000000, 0.000000, 1.000000]

# R: 3x3 rectification matrix
R: [1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0]

# P: 3x4 projection matrix
P: [249.122009, 0.000000, 325.983643, 0.000000,
    0.000000, 249.122009, 187.070150, 0.000000,
    0.000000, 0.000000, 1.000000, 0.000000]