IMUと車速に基づく横滑り角を考慮した自己位置推定

はじめに

自動車やロボットの技術の中で「自己位置推定」は非常に重要な技術です。自己位置推定にも色々な方法がありますが，内界センシング結果に基づく横滑り角を考慮した位置推定が古くから使われています。横滑り角とは自動車の運動制御をする上で必ず出てくる重要な物理量であり，自動車工学の研究者・エンジニアが長年取り組み続けている技術テーマの一つであります。これから自動車工学に入門する方や自動運転開発等で車側の知識に触れたい方に向けて説明すると，横滑りとは車体特性によって車の向きと進行方向にズレが生じる物理現象を指します。自動車制御系の論文を読むと，例えば下記のような図を目にすることが多いと思います。

図1：自動車の横滑り

この図を見て，何か違和感を感じないでしょうか？ よくみると，車速の矢印と車の向いている向きが一致していません。これは図の間違いではなく，横滑りという現象によって生じているものです。メカニズムを説明すると，自動車のステアリングを変化させると車の向きとタイヤの向きにズレが生じます。車体の姿勢変化・向心加速度・タイヤの滑りによって車の向きと進行方向にわずかなズレが生じます。このズレが横滑りという現象を指します。車の向きと進行方向の角度ズレ量を「横滑り角」と呼びます。つまり，「車が曲がる」という現象は車体の横滑りと姿勢角の変化の両方によって成り立つ現象なのです。

車体の横滑り角を直接センサで推定することはかなり難しく，非常に高級なセンサを搭載しなければりません。自動車工学では横滑り角を高級センサを使わずに推定することが長年の研究テーマとなっております。低価格なセンサのみで横滑り角を推定できれば，市販車でも横滑り角推定の機能を実装することが容易となります。

ここまで読んでいただいた読者の中には，「横滑り角を推定して何がうれしいの？」と思った方がいると思います。結論から述べると，横滑り角推定は先進安全や自動運転等の様々な分野に恩恵をもたらす技術です。例えば，自動運転システムではIMUと車速情報から車体の横滑り角を推定することがあります。車体の横滑り角を推定することで

・横滑り角を用いることにより，LiDARやレーダーを使わずにある程度精度のよい横速度や移動量/自己位置を計算できる。
・車体の滑り角と車体モデルを使うと，タイヤの滑り角も推定できる。
・LiDARやカメラの自己位置推定結果とフュージョンすることでロバストな推定が可能となる。

が可能となります。自動運転システムでは車体の位置・移動量計算に基づいて認識結果の補正や経路計画・車体制御をします。LiDARやカメラのセンシング結果を期待できない場合でも，内界センサの情報を用いた自己位置推定を用いることで自動運転を持続することが可能となります。

以上の背景のもと，本記事では横滑り角推定を考慮した自己位置推定に関して解説していきます。今回の方法はIMUのヨーレート情報と車速情報を用いることにより，内界センシング結果のみに基づいて自己位置を推定できます。また，一部の既存文献に関する考察に関しても述べていきます。簡易的なサンプルプログラムもgithub上に準備しましたので，本記事が皆様の技術開発や勉学に少しでも参考になれば幸いです。

問題設定

本記事の問題設定を述べます。図2に自動車のシステム構成を示します。$v$が車両の進行方向速度，$\beta$が横滑り角，$\theta$が座標系における姿勢角を示します。$\gamma$が車体に発生するヨーレートです。本システムではIMU・車速センサのような内界センサを用いて$v$と$\gamma$を直接測定できるとします。車両の初期位置・初期姿勢が既知とした際に，内界センサの情報のみを用いて，車の横滑り角$\beta$を推定することを目指します。また，推定した滑り角情報を用いて車体の位置$x_{p}$と$y_{p}$の推定を目指します。

図2：自動車システム構成

横滑り角と自己位置推定

さっそく，横滑り角推定の方法に関して解説していきます。ここでは横滑り角推定の方法に関して述べた後，それを活用したシンプルな位置推定方法を解説していきます。

横滑り角推定

横滑り角の推定方法に関して説明します。横滑り角の推定方法自体は何十年も前から先進安全技術向けに自動車メーカーで研究されており，様々な方法が提案されています。ここでは，自己位置推定で用いられるIMUと車速情報に基づく方法について解説します。

まず，車体の運動方程式として等価二輪モデル：

$$
\frac{d\beta(t)}{dt} = \frac{a_{11}}{v(t)} \beta(t) + \left(-1 + \frac{a_{12}}{v(t)^2} \right)\gamma(t) + \frac{a_{13}}{v(t)} \delta(t) \tag{1}
$$

$$
\frac{d\gamma(t)}{dt} = a_{21} \beta(t) + \frac{a_{22}}{v(t)} \gamma(t) + a_{23} \delta(t) \tag{2}
$$

を定義します。「等価二輪モデルって何？」という方は自動車業界で古くから活用されている半デファクトスタンダードな運動方程式だと理解しておけば問題ありません。ここで，$a_{ij}$ は定数係数であり、以下のように定義します．

$$
a_{11} = -\frac{K_f + K_r}{m} \tag{3}
$$

$$
a_{12} = \frac{-l_f K_f + l_r K_r}{m} \tag{4}
$$

$$
a_{13} = \frac{K_f}{m} \tag{5}
$$

$$
a_{21} = \frac{-l_f K_f + l_r K_r}{I} \tag{6}
$$

$$
a_{22} = -\frac{l_f^2 K_f + l_r^2 K_r}{I} \tag{7}
$$

$$
a_{23} = \frac{l_f K_f}{I} \tag{8}
$$

本運動方程式では$\delta$が前輪の操舵角(タイヤの切れ角)，$\gamma$がヨーレート，$\beta$が横滑り角となります。$K_{f}$と$K_{r}$がタイヤのコーナリングパワーとなります。$l_{f}$と$l_{r}$が車両重心に対するホイールベースです。等価二輪モデルは車速を時変パラメータとする線形な状態空間モデルとして利用できます。横滑り角$\beta$を状態変数に設定すれば，カルマンフィルタのような状態推定手法にて横滑り角$\beta$を推定できます。しかしながら，等価二輪モデルの運動方程式に$\delta$が含まれているため，何らかの方法で前輪の操舵角を測定しなければなりません。実際の自動車システムでは前輪の操舵角を測定できないことが珍しくありません。ハンドルの角度情報から前輪の操舵角を推定することもできますが，ハンドル角センサの分解能やハンドル・前輪間のメカ特性により，前輪操舵角を正確に推定ができないケースも存在します。以上の理由から，前輪操舵角の情報を用いずに横滑り角を推定することが望ましいとされています。

ここで，実際の乗用車の自動運転システムにおける車両挙動を考察すると，以下の仮定が成り立つとします。

1. 自動運転システムでは搭乗者の乗り心地を維持するため，横加速度と横ジャークを極力抑制するような運動計画を計算する
2. タイヤのスピンを防止するため，操舵角や横滑り角が急激に変動するような危険走行が生じないように走行エリア定義や走行計画が立てられる

以上の仮定より，安全運転の範疇では車体の横加速度と横ジャークを抑制するような走行となり，操舵角や横滑り角が急変しないような挙動となることが期待できます。操舵角の変化速度が小さければ，車体のヨーレートと滑り角の変化速度も十分小さくなることを期待できます。つまり，$d\beta(t)/dt=0$と$d\gamma(t)/dt=0$が成り立つと期待でき，ヨーレートと横滑り角の方程式に関して
$$
\frac{a_{11}}{v(t)} \beta(t) + \left(-1 + \frac{a_{12}}{v(t)^2} \right)\gamma(t) + \frac{a_{13}}{v(t)} \delta(t) \tag{9}=0
$$

$$
a_{21} \beta(t) + \frac{a_{22}}{v(t)} \gamma(t) + a_{23} \delta(t) \tag{10}=0
$$

が成り立つことを期待できます。式(9)を操舵角$\delta$について解いて式(10)に代入して整理することにより，車体の運動方程式を

$$
\beta(t) = -\frac{ a_{22} - \frac{a_{23} a_{12}}{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}} \frac{\gamma(t)}{v(t)}- \frac{\frac{a_{23} }{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}} v(t)\gamma(t)　\tag{11}
$$

と表現できます。本式では操舵角が式中に現れないため，IMUのヨーレートと車速の情報のみを測定できれば横滑り角を推定できます。車体のヨーレートと車速をセンサで測定して式(11)に入力すれば，車体の横滑り角を推定できます。以上が横滑り角の推定方法となります。

自己位置推定

横滑り角の情報を用いて車体の自己位置を推定します。まず，車体のヨー角を推定します。IMUで観測したヨーレートを積分することでヨー角を推定できます。しかしながら，IMUの観測値にはバイアス誤差が生じており，観測値をそのまま積分すると推定値がドリフトしてしまいます。バイアス誤差は温度や衝撃によって変動してしまいます。走行開始前にバイアス誤差を補正しても徐々にバイアス誤差が変動してしまい，推定値がドリフトしてしまいます。何らかの方法でリアルタイムでのバイアス誤差補正をしなければなりません。バイアス誤差補正にあたり，以下の仮定が成り立つとします。

・自己位置推定中に停止線前停止や荷物の積み下ろし等により，車体が長時間停止することがある。
・IMUのヨーレートに関するバイアス誤差の変化速度が十分遅く，走行中におけるバイアス誤差の変化量が微小である。

以上の仮定のもと，ヨーレートのバイアス誤差を推定します。車体が完全に停止していればヨーレートが発生しないため，ヨーレートの真値が0となる筈です。もし停止中にヨーレート$\gamma$の観測値$\gamma_{m}$が0でないのであれば，ヨーレートにバイアス誤差が重畳しているとみなすことができます。つまり，車体停止中のヨーレート$\gamma$の観測値$\gamma_{m}$の平均値を計算すれば，それがそのままヨーレートのバイアス誤差となります。つまり，バイアス誤差$\gamma_{b}$を

$$
\gamma_{b}(t)=\frac{1}{t_{2}-t_{1}}\int_{t_{1}}^{t_{2}} \gamma_{m}(t)dt \tag{12}
$$

と推定できます。$t_{1}$が車体停止開始時の時刻，$t_{2}$が車体停止終了時の時刻となります。車体が停止するたびにヨーレートのバイアス誤差を更新することにより，正確なバイアス誤差推定を実現できます。バイアス誤差の推定値を用いることにより，観測値$\gamma_{m}(t)$を

$$
\gamma_{est}(t)=\gamma_{m}(t)-\gamma_{b}(t) \tag{13}
$$

と補正できます。$\gamma_{est}(t)$がバイアス補正後のヨーレートとなります。車体のヨーレートの観測誤差が完全に除去されていれば，車体のヨーレートの真値$\gamma(t)$と$\gamma_{est}(t)$が一致するため，$\gamma_{est}(t)$を数値積分することで車体の姿勢角を

$$
\theta(t)=\theta_{0}+\int_{0}^{t} \gamma_{est}(\tau) d\tau \tag{14}
$$

と推定できます。$\theta_{0}$は初期の姿勢角です。ここで，車体に発生する$x$方向速度$v_{x}$と$y$方向速度$v_{y}$の2軸速度を図3のように図示できます。車体の進行速度$v$を三角関数にて分解すれば，$v_{x}$と$v_{y}$を得られることがわかります。

図3: 座標系におけるx-y軸の2軸速度

つまり，$\theta$と$\beta$を用いることにより，各方向の車速を

$$
v_{x}(t) = v(t)\cos(\beta(t)+\theta(t))　\tag{15}
$$

$$
v_{y}(t) = v(t)\sin(\beta(t)+\theta(t))　\tag{16}
$$

と推定できます。各方向の車速の感度誤差とオフセット誤差が完全に除去されていれば，推定した各方向の速度を数値積分することで車体の位置を

$$
x_{p}(t)=x_{0}+\int_{0}^{t} v_{x}(\tau) d\tau \tag{17}
$$

$$
y_{p}(t)=y_{0}+\int_{0}^{t} v_{y}(\tau) d\tau \tag{18}
$$

と推定できます。以上が位置推定の方法となります。ここまでの手順をまとめると，自己位置推定の処理は下記のブロック線図で表すことができます。車体の横滑り角とヨーレートのバイアス誤差を推定することにより，内界センサの情報を用いて車体の位置と姿勢を推定することができます。

図4: 自己位置推定のブロック線図

注意事項

ここではいくつかの注意事項に関して解説していきます。

先行文献における自己位置推定のための横滑り角推定

先行文献において，横滑り角を考慮した自己位置推定技術が多数提案されています。例えば自動車技術会論文集では，下記の論文が提案されています。

・高野瀬碧輝, 渥美善規, 滝川叶夢, & 目黒淳一. (2020). 都市部環境下で適用可能な GNSS/IMU に関する研究-横すべり角を考慮した相対位置性能と絶対位置性能の評価. 自動車技術会論文集, 51(4), 721-726.

参考文献でも車体の自己位置推定のための横滑り角推定の方法が述べられています。ただし，本qiita記事と異なる推定式にて横滑りを推定しています。論文中では，ヨーレートと車速の乗算値を用いることで

$$
\beta(t)= -\frac{l_{r}m}{K_{r}L} \gamma(t)v(t)　\tag{19}
$$

にて横滑り角を推定できると述べられています。ここで，$L$はホイールベース$L=l_{r}+l_{f}$となります。なお，本qiita記事の変数定義に表記を合わせていますが，式(19)は論文集で述べられている推定式と同じ式です。

著者の計算違いでなければ，等価二輪モデルを用いた横滑り角の推定式は式(11)になる筈です。では，参考文献では何故式(19)のように横滑り角を推定しているのでしょうか？ 本節では，著者なりの考察を述べていきます。結論から述べると，式(19)の推定式と式(11)の推定式は特定条件下において等価となります。ここでは，読者が雰囲気をつかめるように大まかな原理に関して述べていきます。なお，本内容はあくまで著者の個人見解であり，実際の背景や理由等に関して論文の著者の方からお伺いしているわけではありません。本考察に関しては，あくまで参考内容として読んでいただけると幸いです。

それでは，論文の推定式に関して考察していきます。式(11)を改めて分析すると，第1項が車速$v(t)$に対して反比例し，第2項が車速$v(t)$に対して比例することがわかります。言い換えると，車速が高い条件下では第2項が支配的になり，車速が低い条件下では第1項が支配的になります。車速が十分に大きければ$\gamma(t)$/$v(t)<<1$が成り立つため，式(11)の第1項を無視しても横滑り角推定の精度に悪影響を及ぼさなくなります。詳細な計算は割愛しますが，一般的な自動車の車両パラメータであれば，車速20 km/h以上速度での旋回時に第1項の値がほぼゼロになります。したがって，渋滞区間を除く都市部の走行では車速が十分高くなるため，第1項を無視して

$$
\beta(t) = -\frac{ a_{22} - \frac{a_{23} a_{12}}{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}} \frac{\gamma(t)}{v(t)}- \frac{\frac{a_{23} }{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}} v(t)\gamma(t)\approx-\frac{\frac{a_{23} }{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}}v(t)\gamma(t)　\tag{20}
$$

と表現しても実用上問題ないと考えられます。式(20)に定数係数のパラメータを代入すると，

$$
\beta(t) = -\frac{\frac{a_{23} }{a_{13}}}{a_{21} - \frac{a_{23} a_{11}}{a_{13}}}v(t)\gamma(t)=-\frac{l_{r}m}{K_{r}L} \gamma(t)v(t)　\tag{20}
$$

が成立します。以上の結果から，参考文献の横滑り角推定は本qiita記事の式(11)の特殊ケースに相当すると考えられます。参考文献では都市部環境での自己位置推定を目指しています。車速が十分に大きい走行ケースが対象のため，式(19)の横滑り角推定の式を採用しても推定精度に悪影響が無いと考えられます。また，参考文献ではGNSSのドップラー速度から式(19)の車体パラメータを同定する方法に関しても述べられており，より簡易的な推定式を採用することでパラメータ同定をしやすくするといった実用上の理由もあったのかもしれません。

車速センサの誤差に関して

実際の自己位置推定では車速推定の処理も必要となります。大半の自動車やロボットでは車体に車輪速センサを取り付けて車体の速度を計算します。実際に車輪速の波形を見たことのある方はご存じかもしれませんが，車輪速と実車速は必ずしも一致するわけではありません。車輪速センサではパルスエンコーダー方式で速度を計算するため，極低速走行時にパルス分解能の問題で車速推定の誤差が増加してしまいます。また，車輪と地面は完全に粘着しているわけではありません。大なり小なり地面とのスリップが生じるため，スリップの影響による誤差も生じてしまいます。さらに，タイヤの径が常に設計値通りの値であることも多くなく，タイヤの周径の誤差による精度劣化も起きてしまいます。

したがって，実際にはヨーレートのバイアス誤差だけでなく，車速を正確に推定することも自己位置推定の観点で非常に重要となります。車速の推定に関しても横滑り角推定と同様に自動車業界で古くから取り組まれている技術テーマのため，もし興味があれば調べていただくとよいかもしれません。例えば下記の文献ではタイヤと実車速の関係に関して言及しております。

・前田健太, 篠原尚希, 金子聡, 園田大樹, & 鈴木圭介. (2023). 車輪速度オブザーバに基づく駆動伝達系の共振を低減する制振制御手法の検討. 自動車技術会論文集, 54(5), 842-849.

・前田健太, 藤本博志, & 堀洋一. (2014). タイヤ横すべりを考慮した限界スリップ率推定に基づく電気自動車の駆動力制御. 計測自動制御学会論文集, 50(3), 259-265.

・高野瀬碧輝. 車両軌跡推定に基づく都市環境における GNSS/INS 統合の高精度化に関する研究. 名古屋大学博士論文.

オドメトリベースの自己位置推定の利点と欠点

今回の記事における自己位置推定手法は，一般的には「オドメトリ」という区分の方法となります。様々な自己位置推定方法がありますが，今回のようなオドメトリベースの自己位置推定には下記のメリットがあるとされています。

1. 車体の自己位置情報を連続的な軌跡情報として保存できる。
2. IMUや車速センサの観測周期が高いため，高サンプリングレートの位置情報として推定できる。
3. 外界センサが破損したときでも最低限のナビゲーションができる。

GNSSやLiDARのスキャンマッチングの方法では自己位置推定結果が不連続に変動することもあります。オドメトリの自己位置推定では，車体の自己位置を積分計算で導出するため，連続的に変化する自己位置情報を得ることができます。自己位置情報が不連続に変化しにくいため，車体の連続的な軌跡情報を必要とするようなパスプランや車体制御に有効な方法と言えます。また，IMUや車速センサの観測周期がカメラやLiDARよりも早いため，高いサンプリングレートの自己位置推定結果を期待できます。アクチュエータへの指令信号を計算するような軌道追従制御では，高いサンプリングレートの位置情報が必要になることがあります。パスプランや車体制御の仕様次第ではオドメトリベースの自己位置推定が必要となることがあります。

一方，オドメトリベースの方法では，センサの誤差を積分計算で累積してしまうため，時間経過とともに推定結果がドリフトしてしまう欠点があります。時間が経過するほど推定精度が劣化してしまうため，実用上はGNSSやLiDARによるマップマッチング手法と併用することが望ましい方法と言えます。なお，オドメトリの利点と欠点に関しては名古屋大学の赤井先生が非常にわかりやすい記事を公開しているため，こちらも読んでいただくと理解が深まると思います。

自己位置推定用IMU

IMUに基づく横滑り角や自己位置推定ではIMUの選定が非常に重要となります。昨今ではセンサ技術の発展により，安価な値段でバイアス変動の起きにくいIMUもいくつか世の中にでてきました。著者の知る範囲では，IMUと車速センサのみで30分程度自己位置推定精度を持続させるような製品も登場しております。最新事例として，32個のmems IMUを用いたマルチIMUボードも登場しております。こちらのボードは自動運転分野で用いる光ファイバージャイロに匹敵する精度を実現しており，IMU単体でも十分な自己位置推定精度を実現できるそうです。

また，国内の産業界でよく使われる製品として，多摩川精機社の光ファイバージャイロとMEMS IMUを併用したマルチタイプ型IMUも販売されています。多摩川精機社の光ファイバージャイロは比較的安価な値段でありながらバイアス変動が起きにくい仕様のため，国内の様々な自動運転開発で活用されています。

IMU自体は自己位置推定だけでなく車両制御や認識結果の補正に必要となります。安価で高精度なIMUの需要が依然高く，今後も継続的に新製品が登場すると考えられます。他にも海外のメーカーから様々なIMUが販売されているため，もし気になるものがあれば探してみるとよいかもしれません。なお，3年以上前の記事ではありますが，本界隈の大ベテランである名城大学の目黒先生がIMU選定のための有益な記事を執筆されているので，こちらもご参考にしていただけるとよいと思います。

検証例

サンプルプログラムを用いて，ヨーレートと車速情報を用いて自己位置推定ができるかを検証してみましょう。Githubにサンプルプログラムをアップロードしましたので，このプログラムを用いて自己位置推定ができるかを検証していきます。なお，本ソフトは必要最低限の機能のみを実装したものであり，実際のシステムで活用する場合には色々なカスタマイズが必要となります。もし本ソフトをご参考にしていただく場合はご注意いただけると幸いです。

動作環境

本ソフトウェアでは下記のROS 2 humble 環境での動作を想定しています。

PC環境
・Ubuntu 22.04
・ROS 2 humble

センサ入力(2入力)
・IMUの観測値：Sensor_msgs/Imu型
・車速の観測値：geometry_msgs/TwistStamped型

出力(1出力)
・オドメトリ座標系での自己位置と2軸速度：nav_msgs/Odometry型

入力信号として，IMUの観測値と車速の観測値の2つのトピックを必要としています。IMUと車速センサの時刻がほぼ一致している前提でのソフトとなっています。もし各センサの時刻が一致していないのであれば，message filter等の時刻同期処理を自分で追加する必要があるのでご注意ください。

検証結果

サンプルデータを用いて自己位置推定結果を確認してみます。本サンプルデータでは車体が数秒間停止してからコースを旋回するようなデータとなっています。4輪車のセンサデータを用いて位置推定した結果が図5の結果となります。自己位置推定の結果と真値が大まかに一致していることがわかります。時間経過とともに誤差が発生していますが，これはヨーレートのバイアス誤差を完全に推定できていないことが主因だと考えられます。車体を数分間停止させてヨーレートのバイアス誤差を推定するような運用に変更すれば，もう少し精度を改善できると考えられます。また，今回のソフトでは車速センサの誤差を補正するような方法を適用していないため，IMUの情報を用いて車速センサの観測値を補正すればさらに精度改善できると考えられます。しかしながら，車両の軌跡を連続的な波形として推定できており，他の自己位置推定手法と併用すればパスプランや車体制御で活用できると考えられます。

図5　サンプルデータによる位置推定結果

おわりに

本記事では横滑り角を考慮した自己位置推定に関して解説しました。今回の記事の内容はすでに色々な業界で活用されている手法の解説となっております。また，githubに公開しているサンプルソフトも必要最低限の機能を実装したものとなっており，実際に活用する場合は適用対象に応じたカスタマイズが必要です。一方，LiDARやカメラを用いた高精度な自己位置推定システムや正確な自己位置情報を必要としないシステムが普及したとしても，高いサンプリングレートで車体の滑らかな軌跡情報を推定可能なオドメトリベースの手法は何らかの形で利用されると考えられます。本記事が読者の方に少しでも役立つと幸いです。