はじめに
ロボットの姿勢推定では,IMUがよく用いられますが,長時間使用するとジャイロのドリフト誤差が蓄積する問題があります.そこで,GNSS座標を用いて,t秒前からの移動量に基づき姿勢角度を推定する方法を紹介します.
動作環境
| 項目 | バージョン |
|---|---|
| ubuntu | 22.04 |
| ROS | ROS2 humble |
| GNSSアンテナ | ANN-MB-00 |
| ZED-F9P | AS-RTK2B-F9P-L1L2-NH-02 |
| IMU | ADIS16475-2 |
GNSSを用いた姿勢推定
RTKを使用したGNSSは,fix解を得られている場合誤差数センチ単位と高精度で緯度経度を測定することが可能です.GNSSの測定には,ZED-F9Pの評価ボードとu-blox社のGNSSアンテナ(２周波)を使用しました.この測定で取得できた緯度経度の移動量に基づきロボットの姿勢角を推定します.
上図のように,$(t-1)$時点と$t$時点の２点の座標が測定できれば,以下の式でロボットの進行方向$\theta_t$が求められます.
\begin{align}
\theta _{t}=\tan ^{-1}\left( \dfrac{\Delta y}{\Delta x}\right)
=\tan ^{-1}\left( \dfrac{y_t-y_{t-1}}{x_t-x_{t-1}}\right)
\end{align}
GNSSとIMUを併用した姿勢推定
GNSSでロボットの姿勢角を推定できる条件は,以下に限られます.
・RTK測位でfix解を得ていること
・ロボットがある程度の速度で走行していること
しかし,実際の走行環境では,これらの条件が常に満たされるとは限りません.そのため,条件を満たさない場合には,IMUやホイールオドメトリなど,他の手法を用いて姿勢を推定する必要があります.本記事では,短時間の測定では精度良く姿勢角を推定できるIMUを採用し,ロボットの状況に応じてGNSSとIMUを切り替える手法を紹介します.切替条件は,下図のフローチャートの通りです.
また,fix解およびロボットの速度に加え,前回とのGNSS座標の差分や,GNSSで推定した姿勢角の急激な変動を考慮することで,安定した推定を実現できました.
実装したソースコード
実験走行
つくばチャレンジの実験走行で撮影した,GNSSとIMUを組み合わせた姿勢推定による自律走行の動画です.
走行の約9割はGNSSでの姿勢角を採用していました.以前はホイールオドメトリのみで推定していたため200m程度で大きくズレが生じていましたが,今回の手法でその課題を解消し,安定した走行を実現できました.
今後は,カルマンフィルタを導入し,GNSSとIMUの誤差を推定しながら補正を行うことで,精度をさらに高めることができればと考えています.
参考文献
中村勇太, GNSSと2D 地図の位置推定切替機構を備えた クローラロボットナビゲーションシステム, つくばチャレンジ2023参加レポート集, 2023.
謝辞
この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました.
また,アナログ・デバイセズ株式会社様から,IMU-ADIS16475-2をご貸与いただきました.
心より感謝申し上げます.