屋外ROS 2ロボットのための車輪速を用いないGNSS・IMU車速推定

Transcript

1. 屋外ROS 2ロボットのための車輪速を用いない GNSS/IMU車速推定 〇元制御系エンジニア( X(旧Twitter):@KariControl) FA設備技術勉強会 2025/05/31 ※所属先企業とは一切関係ありません。個人としての発表です。
2. ロボットにおける運動量推定 2 自車運動量推定は認知・判断・操作のいずれにおいても不可欠 【認 知】 ▪地図＆ 自己位置推定 ▪物体・ 環境認識 【判 断】 （運転行動計画） ▪軌道生成 ▪障害物回避 ▪先行車追従 【操作】 (運 動制 御) ▪速度制御 ▪経路追従制御 ブレーキ系 ステアリングﾞ系 駆動系 【アクチュエーター】 ▪自車運動量推定：車速・移動量等
3. 地図・自己位置推定ための車速推定の重要性  車載したLiDARによる外界のスキャニングを実施  自己位置推定情報と点群を用いて地図を生成  地図をもとに制御や地図を介した認識を実施  車体が動いているとLiDARで計測した点群に歪みが発生  点群歪みにより地図生成や自己位置推定の精度劣化が発生 3 ロボットにおけるLiDARでの地図・自己位置推定 スキャン開始 スキャン終了 LiDARにおける点群の歪み LiDARの点群歪みを解消するため、自車の運動量(車速・ヨーレートの推定)が必要 認知の物体追跡や車速制御にも車速情報が必要
4. 車速推定手法  大まかに3つのアプローチが存在 – 1. 車輪速センサの情報を用いた車速推定 – 2. IMUの加速度情報を用いた車速推定 – 3. 対地車速計(例:レーザードップラセンサ等)による推定  車輪速センサによる推定 – センサ分解能/路面スリップにより使用困難な場合有 – レトロフィット形式だとセキュリティ問題で使用困難  IMUの加速度情報を用いた推定 – 加速度をそのまま積分すると推定値がドリフト – 振動・温度変化により推定精度が著しく劣化  対地車速計による推定 – レーザードップラ等 – センサコスト大、設置が困難等  機能安全 – 対故障のための冗⾧な車速推定も必要 4 移動ロボットの主流な方法 屋外ロボット向けに車輪速を用いずロボットの車速を推定する方法を紹介 GNSSとIMUの統合により外付けセンサのみで車速推定可能 課題
5. アプローチ  GNSSは緯度経度以外にドップラ速度を測定可能 – 車体の北向き速度/東向き速度をGNSSから測定可能 – 速度ベクトルから車速の絶対値を推定可能 – 後退走行や停止中の速度推定は困難  GNSSドップラとIMUの統合による車速推定  IMUとGNSSの併用により前進/後退/停止時の正確な推定を実現  屋外ロボットではGNSSとIMUはほぼついているのでセンサ追加無しで推定 5 GNSSドップラ 屋外ロボットのための車速推定アルゴリズムを家で開発 GNSS GNSS/IMUの統合による 車速推定 IMU East North 𝒗𝒚 𝒗𝒙 Heading angle 𝒗𝒂 = 𝒗𝒚 𝟐 + 𝒗𝒚 𝟐 提案手法
6. 提案手法  アルゴリズムの構成 – 1)GNSSドップラの前処理、2)車体姿勢推定、3)運動状態判定、4)速度推定 6 GNSSとIMUの統合 時間の関係上、運動状態判定と速度推定に関して説明 Heading angle estimation based on Kalman filter Heading angle estimation based on dead- reckoning Motion state classification Velocity estimator 𝒗𝒂 𝜽𝒂 𝝎 𝒂𝒙 𝒗𝒙 , 𝒗𝒚 𝜽𝒅 𝒗𝒆 𝜽𝒌𝒇 Pre-processing For GNSS GNSS doppler IMU Heading angle estimation
7. 運動状態判定  アルゴリズムの構成 – 車体の運転状態を1)前進走行、2)後退走行、3)停止、のいずれかとして判定 7 アルゴリズムの概要 時間の関係上、運動状態判定と速度推定に関して説明 Heading angle estimation based on Kalman filter Heading angle estimation based on dead- reckoning Motion state classification Velocity estimator 𝒗𝒂 𝜽𝒂 𝝎 𝒂𝒙 𝒗𝒙 , 𝒗𝒚 𝜽𝒅 𝒗𝒆 𝜽𝒌𝒇 Pre-processing For GNSS GNSS doppler IMU Heading angle estimation
8. 前進・後退判別  車体前進時ではGNSSドップラ速度ベクトルと車体姿勢ベクトルが同じ向き  車体後退時ではGNSSドップラ速度ベクトルと車体姿勢ベクトルが反対向き 8 アルゴリズムの概要 GNSSドップラの速度ベクトルと車体姿勢ベクトルを比較することで 車体の進行方向の判別が可能 East North 𝒗𝒙 , 𝒗𝒚 𝐜𝐨𝐬 𝜽𝒅 , 𝐬𝐢𝐧 𝜽𝒅 East North 𝒗𝒙 , 𝒗𝒚 𝐜𝐨𝐬 𝜽𝒅 , 𝐬𝐢𝐧 𝜽𝒅 (a) Forward (b) Backward
9. 停止判定 9 アルゴリズムの概要 GNSSドップラの絶対速度とIMU観測値の標準偏差に基づいて停止判定を処理 𝝎 ヨーレート𝜔 𝜎 = 1 𝑁 𝜔 − 𝜇 判定処理 𝜎 = 1 𝑁 𝑎 − 𝜇 前後加速度𝒂𝒙 GNSSドップラの絶対速度𝒗𝒂 標準偏差計算 標準偏差計算 停止判定
10. 運動状態判定  アルゴリズムの構成 – 車体の運転状態を1)前進走行、2)後退走行、3)停止、のいずれかとして判定 10 アルゴリズムの概要 時間の関係上、運動状態判定と速度推定に関して説明 Heading angle estimation based on Kalman filter Heading angle estimation based on dead- reckoning Motion state classification Velocity estimator 𝒗𝒂 𝜽𝒂 𝝎 𝒂𝒙 𝒗𝒙 , 𝒗𝒚 𝜽𝒅 𝒗𝒆 𝜽𝒌𝒇 Pre-processing For GNSS GNSS doppler IMU Heading angle estimation
11. カルマンフィルタによる速度推定  運動状態判定の結果をもとにGNSSドップラの車速絶対値に符号を付 – 停止時の観測ノイズの影響抑制と後退時における車速推定実現  符号付与済み車速とIMUの加速度をカルマンフィルタでフュージョン – 誤差共分散に基づいた統合による高精度な車速推定を実現 11 アルゴリズムの概要 符号情報の付与とカルマンフィルタにより所望の車速推定を実現 Sensor Fusion (Kalman filter) 符号情報 付与 符号情報 付与 IMUの前後加速度𝒂𝒙 符号付与済み車速𝒛𝒗 GNSSドップラの車速絶対値𝒗𝒂 運動状態判定結果 車速推定値 𝑧 = 𝑣 , 𝑖𝑓 前進 0.0 , 𝑖𝑓 停止 −𝑣 , 𝑖𝑓 後退 𝒗𝒂 = 𝒗𝒚 𝟐 + 𝒗𝒚 𝟐
12. 実験データによる検証  U-blox社のチップ(ZED-f9p)が搭載されたボードタイプのGNSSレシーバー  NEO D9Cのシールド基板を付けるとCLAS方式の高精度測位が可能  Ubuntu PCにてGNSSとIMUのデータをROS bag形式でロギング 12 ROS 2によるデータ収集システム Ubuntu PCとGNSS・IMUにてロギングした実験データを用いて推定精度を検証 ZED f9p搭載基板 NEO D9C搭載基板 Ubuntu GNSSアンテナ GNSSレシーバー Ubuntu PC IMUユニット
13. 実験例  IMUの積分で得た推定値(紫線)とGNSSドップラのみの推定値(黄線)を記載  提案手法の方が前進・後退・停止のいずれにおいても所望の推定を実現 13 GNSS/IMUでの実験結果 GNSSドップラの速度ベクトルとIMUの加速度情報を用いた車速推定を実現
14. まとめ  背景 – 車速推定はロボットには重要不可欠な技術 – 外界認識/自己位置/地図/機能安全の面で車速推定は重要  内容 – GNSS/IMUの情報を統合した車速推定を開発 – 前進/後退/停止においてGNSS単独とIMU単独よりも精度良好  屋内での利用に向けたステレオカメラ or 3D LiDARでの車速推定 – GNSSの測位精度が悪化しやすい屋内向けにステレオカメラ or 3D LiDARで推定 – Scan to Scanでの車速推定や画像とIMUの統合による推定をやりたい(実機でも) 14 まとめ 今後
15. ご清聴ありがとうございました 15