はじめに
自動車や移動ロボット等の自律移動システムの位置を推定する身近な手段として，Global Navigation Satellite System(GNSS)による測位が挙げられます。一部の専門家を除くと，GNSSよりもGPSという言葉の方がなじみがあるでしょうか。GPSとはアメリカの衛星システムを指し，GNSSとはGPSを含む各国の衛星システムの総称です。世の中に存在する様々なシステムは，GNSSを利用して各国の衛星からの信号情報を利用して位置を推定します。
昨今はLiDARやカメラのセンシングが主流と言われていますが，GNSSによるセンシング手法は依然重要と考えられます。著者個人の主観として，GNSSが利用される主なメリットとして
- 受信機とアンテナを設置すれば高精度地図を事前準備せずに位置を推定可能
- 屋内では使用できないが，特徴点の少ない屋外空間で位置情報を取得可能
- 地図上の絶対位置を取得できるため，スキャンマッチングや相対位置推定手法の初期位置情報として利用可能
- GNSSドップラによる自律移動システムの速度・姿勢推定に利用可能
が挙げられます。受信機とアンテナを移動システムに設置すれば比較的簡単に位置推定できます。高精度地図を準備せずに位置推定できるため，比較的導入が容易と考えられます。また，LiDARやカメラのスキャンマッチングは屋内環境で有効な方法ですが，特徴点の少ない屋外空間で精度が劣化しやすいことが知られています。GNSSの位置推定は衛星信号の届かない屋内だと利用できませんが，屋外空間ではマルチパスさえなければ正確な位置情報を取得できます。GNSSによる位置推定は地図上の絶対位置を推定できる点が特徴です。LiDARやカメラのスキャンマッチングや相対位置推定を利用する場合，地図上での自車の初期位置が必要となります。現状外界センサのみで自車の初期位置を推定することは難しく，GNSSのような方法で地図上の絶対位置を推定する必要があります。
ここまでのメリットは主にGNSSの絶対位置に関するものですが，実はGNSSは緯度経度の測位以外の用途での利用も可能です。4番目のメリットとして，GNSSドップラによる移動システムの速度・姿勢推定に利用できることが挙げられます。GNSSは緯度経度情報だけでなく，GNSSドップラ周波数から求めた移動体の地図座標系での速度も取得できます。GNSSで求めた速度ベクトルに基づく，自律移動システムの姿勢角も推定できます。自律移動システムにおいて，自車の姿勢角を推定することは非常に重要です。自車の姿勢角がわからなければ外界センサで取得した物体がフレーム間でどの程度移動したかわからなくなります。また，車体のパス計算や軌道追従制御の実現も困難となります。IMUのヨーレートを積分することでGNSSを使わずに姿勢推定することもできます。しかしながら，バイアス誤差の累積により時間経過で精度が悪化してしまうことが知られています。IMUのみでは原理的に短時間の相対姿勢しか推定できません。したがって，世の中の自律移動システムではGNSSで絶対座標系の姿勢を取得し，それをIMUや車輪速の情報で補完する方式がよく用いられています。屋外の自律移動システムを作るのであれば，GNSSを上手に使いこなすことが求められることも多いでしょう。
以上の背景のもと，本記事では自己位置推定技術の一つであるGNSSでの姿勢角推定について解説していきます。今回はもっとも簡単な方法として，「複合航法」と呼ばれるGNSSとIMU・車輪速センサの情報を統合して絶対座標系の姿勢角を推定する方法について解説します。今回解説する方法では，車輪速とIMUで車体の停止判定を実施し，その結果に基づいてカルマンフィルタにて車体姿勢を推定します。「なぜ，停止判定をする必要があるのか？」という部分に関しても解説していきます。今回はサンプルプログラムとして，Githubに下記のサンプルコードも公開しておきました。本記事が何かしらの役に立つことをねがっております。
表記に関して
今回の記事は式がややこしいので，表記に関する注意事項を記載しておきます。本記事では，任意の時間信号 $x$ と $y$ の時刻 $t$ における値を $x(t)$ と $y(t)$ に対して，離散信号 $x[k]$ と $y[m]$ をそれぞれサンプリング周期$T_1$，$T_2$ で更新された値とします。各離散インデックスは時刻 $t$ に対して，$x[k] = x(t = kT_1)$，$y[m] = y(t = mT_2)$ となるように対応するものとします。また，$x[k]$ に関して $t = mT_2 = k_m T_1$ が成り立つ場合，$x[k_m]$ と表記することがあります。具体的にはIMUと車輪速で取得した任意の時間信号に関して，サンプリング周期が$T_1$となり，添え字インデックス$k$を用いて表記します。GNSSで取得した任意の時間信号に関して，サンプリング周期が$T_2$となり，添え字インデックス$m$を用いて表記します。なお，本記事では文脈上明らかであれば文章の可読性向上のために変数の明示的な時間表現を省略することがあるためご注意ください。
問題設定
まず，問題設定について解説します。Fig.1に自律移動システムの概要を示します。本システムでは，自動車や車輪型ロボットのような車輪での自律移動システムを想定します。車体にGNSSアンテナ・GNSSレシーバーとIMUと車輪速センサが搭載されているとします。GNSSでは車体の緯度・経度情報だけでなく，測位ステータス・DOP値・GNSSドップラの速度ベクトルを測定できるとします。なお，GNSSはシングルアンテナであり，問題設定簡単化のためアンテナの取り付け角が補正済みとします。また，車体の横滑りに関しては本記事内では無視できる前提で議論を進めます。
Fig.1 自律移動システム
今回はROS 2でGNSSの測位をする都合上，測位ステータス$G$を下記のように4値の値をとる変数として定義します。
G[m]:=
\begin{cases}
-1, \text{if unable to fix position} \\
0 , \text{if unaugmented fix} \\
1, \text{if with satellite-based augmentation} \\
2, \text{if with ground-based augmentation} \\
\end{cases}
詳細は割愛しますが，ステータスが-1の場合は正常な受信ができていないと考えていただいて問題ありません。GNSSにあまり詳しくない方は「測位ステータスの値が大きいほど高精度な位置や速度を得られている可能性が高い」と考えていただいて問題ありません。自律移動システムでGNSSを有効とみなせる測位ステータスとしては，ステータスが1以上となるケースであることが多いです。GNSS測位における補正の詳細に関して，下記の文献等で解説されています。ご興味あればこちらもご参照ください。
また，測位ステータスとは別にDOP値という測位指標に関する情報も受信できるとします。DOP値とは上空にある衛星の配置バランスの度合いを指します。衛星の測位を利用する都合上，アンテナの上空に人工衛星がなるべく広い範囲にまんべんなく配置されているとGNSSの精度が向上することが知られています。逆に，アンテナの上空に衛星が少なければ精度が悪化してしまいます。「DOP値が小さいほど衛星の配置バランスが良好で高精度な位置や速度を測定できている可能性が高い」と考えていただいて問題ありません。DOP値にもいくつか種類がありますが，本記事ではPDOPと呼ばれる指標のみを扱います。もしDOP値に関する詳細に興味あれば，下記の文献等をご参照してください。
IMUでは車体の前後方向加速度とヨーレートを検出できるとします。今回は問題設定を簡単化するため，IMUの取り付け角補正済みでIMUの前後方向加速度が車両座標系の前後方向の向きと一致するとします。また，車輪速センサではスケールファクタで補正された車体の車両座標系における車速を推定できるとします。GNSSドップラでは絶対座標系におけるGNSSアンテナの北向き速度$v_{y}$と東向き速度$v_{x}$を測定できます。
以上の問題設定のもと，GNSS・IMU・車輪速センサの情報を用いて姿勢角を推定することを目指します。
複合航法による姿勢推定
ここでは，GNSSドップラ・IMU・車輪速センサの複合利用による姿勢角推定の方法について述べます。今回は複合航法の中でも最もポピュラーなカルマンフィルタを主体とする推定方式について解説します。
GNSSドップラでの姿勢角
GNSSドップラでは車体の北向き速度 $v_y$ と東向き速度 $v_x$ を速度ベクトルとして測定できます。簡単な三角関数の計算から，速度ベクトル情報から車体姿勢として姿勢角を
\theta_a [m] = \tan^{-1} \left( \frac{v_y [m]}{v_x [m]} \right)
と推定できます。以上の手順にてGNSSの受信情報に基づく姿勢角を計算できます。しかしながら，GNSSドップラで求めた姿勢角をそのまま使うことはできません。GNSSドップラで得た速度にはノイズが重畳しています。車体停止中・極低速付近の走行時ではS/N比が著しく悪化してしまいます。ノイズの影響で車体停止中でも速度がゼロとならず，停止中に姿勢角の推定値が揺れ動くことも珍しくありません。極低速での走行時も本来の速度がノイズに埋もれてしまい，姿勢角の推定精度が著しく悪化してしまいます。また，GNSSの受信周期は5～10 Hz程度が一般的です。リアルタイムでの運動制御やパス計算の計算周期が20～50 Hz程度であることを考えると，GNSSで計算可能な姿勢角のレートが低いと考えられます。したがって，後述の停止判定とカルマンフィルタを施すことで正確かつ高レートな姿勢推定を目指します。
停止判定
後述のカルマンフィルタと出力制限のため，車体停止の判定を実施します。車体停止判定のもっとも簡単な方法は車輪速センサの値をモニタリングすることです。もし車輪と地面間のスリップが一切なく，エンコーダーの測定誤差がなければ，車輪速センサの値が0となる状況を「停止」とみなすことができます。しかしながら，実際には車輪速センサの値のみをモニタリングしても，正確な停止判定ができないことがあります。理由としては，以下の理由により車輪速センサの精度が極低速付近で著しく悪化してしまうためです。
・発進/停止間際ではタイヤと路面のスリップにより，実速度と車輪速センサから得た車速推定値が乖離しやすい。
・ホイールエンコーダーの分解能により，極低速での走行時にエンコーダパルスのカウント数が減って車輪速センサの推定精度が悪くなる。
・振動や量子化等のそのほかの様々な理由によるS/N比が悪化してノイズに埋もれてしまう。
以上の理由から，車輪速センサの原理的な問題により，車体が停止していても車輪速センサの車速推定値が0でないことがあります。したがって，車輪速での推定値に対して閾値を設定して停止判定しなければなりません。しかしながら，保守的な閾値設定になると車体がまだわずかに動いているのに停止判定をしてしまいかねません。攻めた閾値にしてしまうと誤判定を引き起こしてしまいます。車速真値がわからないため，車輪速センサで得た推定値が閾値以下であっても確実に止まっているかどうかわからないのです。
一般的には車輪速センサの値だけでなく，IMUの値も用いて停止判定を施すこと方法が広く用いられています。車体が停止していれば，ヨーレートと加速度の値が0付近に収束します。また，停止に伴い路面や車体の影響によるIMUの振動が低減します。以上の点から，車体停止していれば走行中と比べるとヨーレートと前後加速度の標準偏差が著しく小さくなると考えられます。したがって，車輪速での車速推定値とヨーレート/前後加速度の標準偏差を用いて停止判定を施すことにより，極低速での車輪速の精度悪化の影響を緩和することを目指します。
IMUのヨーレート/前後加速度と車輪速センサにて推定した車速 $v_a$ を用いて車体停止しているかどうかを判定します。前後加速度とヨーレートの観測値を受信するたびにデータ長 $N$ のヨーレートのデータベクトル
\omega_r = [\omega_{r1}, \omega_{r2}, \cdots, \omega_{rN}]
を更新します。$\omega_{ri}$ $(i=1,\cdots,N)$ が測定されたヨーレートです。また，データ長 $N$ の前後加速度のデータベクトル
a_r = [a_{x1}, a_{x2}, \cdots, a_{xN}]
を更新します。$a_{xi}$ $(i=1,\cdots,N)$ が測定された前後加速度です。本データベクトルではリングバッファ方式にてIMUでのヨーレート・前後加速度測定のたびに値を更新します。所定時間におけるヨーレートの標準偏差 $\sigma_y$ と前後加速度の標準偏差 $\sigma_a$ を
\sigma_y = \sqrt{\frac{1}{N} \sum_{i=1}^N \left( \omega_{ri} - \mu_{\omega} \right)^2}
\sigma_a = \sqrt{\frac{1}{N} \sum_{i=1}^N \left( a_{xi} - \mu_{a} \right)^2}
をリアルタイムで計算します。$\mu_{\omega}$ と $\mu_a$ がデータベクトルの平均値です。本方法では速度 $v_a$ と各標準偏差が閾値以下であれば車体を停止したとみなします。すなわち，判定結果 $S$ を
S[k] =
\begin{cases}
1, & \sigma_y[k] < \sigma_{1} \ \wedge\ \sigma_a[k] < \sigma_{2} \ \wedge\ v_a[k] < v_{stop} \\
0, & \text{otherwise}
\end{cases}
と計算すればよいことになります． $S=1$ が停止，$S=0$ が否停止を指します。$\sigma_{i}$ $(i=1,2)$ が閾値です。$v_{stop}$は停止判定用の車速閾値です。本手順により，車輪速とIMUの情報に基づいて車体停止を判定します。車輪速とIMUの標準偏差に閾値を設定することにより，確実な車体停止の判定が可能となります。なお余談となりますが，自動車であればより停止判定の精度を向上させるため，車両CANに含まれているギアシフト情報を利用することがあります。ギアがパーキングとなっていれば車輪速の値に関係なく車体が停止しているとみなすことができるためです。このように，車輪速以外の情報を利用することで車体停止判定の精度向上することが一般的なアプローチとなります。
カルマンフィルタでの姿勢推定
GNSSとIMUに基づいて車体の姿勢角を推定してみましょう。姿勢推定では，線形カルマンフィルタを適用します。既に偉大なる先人の方々により非常にわかりやすいカルマンフィルタの原理に関する記事が存在するため，本記事ではカルマンフィルタの原理に関する説明を省略します。例えば，下記のような有益な記事が存在するため，ぜひこちらをご参照ください。ただし，本記事ではGNSSの測定原理の都合上，やや特殊な形態のカルマンフィルタを扱います。
さっそく，カルマンフィルタでの推定に関して解説します。ここでは離散時間状態方程式
x[k]=Ax[k-1]+B\omega[k-1]
にて状態変数を予測します。状態方程式の状態行列と各ベクトルを
A=\begin{bmatrix}1 & -T_1 \\ 0 & 1\end{bmatrix}
B=\begin{bmatrix}T_1 \\ 0\end{bmatrix}
C=\begin{bmatrix}1 & 0\end{bmatrix}
x[k]=\begin{bmatrix}\theta_{kf}[k] \\ \omega_{b2}[k]\end{bmatrix}
と定義します。$\omega_{b2}$は状態方程式におけるヨーレートのバイアス値です。$θ_{kf}$は姿勢角です。前回の記事で解説した通り，IMUの観測値にはバイアス誤差が含まれます。カルマンフィルタでIMUのバイアス成分を考慮するため，上記のような拡大系の方程式を利用します。これらの式により，IMUにてヨーレートを測定するたびに車体の姿勢角とヨーレートのバイアス値を
x[k|k-1]=Ax[k-1|k-1]+B\omega[k-1]
と予測します。線形カルマンフィルタでは状態方程式による予測だけでなく，誤差共分散$P$も予測しなければなりません。カルマンフィルタでは，誤差共分散を
P[k|k-1]=AP[k-1]A^T+Q
と予測します。$Q$はユーザーによって定義されるプロセスノイズの共分散行列となります。
次に，カルマンゲインを計算し，観測値と予測値を統合することでカルマンフィルタの推定値を計算します。本手法ではGNSSドップラにて推定した車体姿勢をカルマンフィルタの観測値として利用します。つまり，観測値を
z[m] =\theta_a [m]
と設定します。ここで，GNSSドップラで得た姿勢角はGNSSドップラの速度ベクトルのS/N比により，速度が低くなるほど精度が劣化してしまうとが予想されます。低速域での走行になってしまうとGNSSの観測値のノイズが増大してしまうことが予想されます。そこで，車輪速$v_a$に対して閾値を設定し，観測ノイズの共分散$R$を
R[m]=
\begin{cases}
R_1, & \text{if } v_a[k_{m}] \le v_m \\
R_2, & \text{else}
\end{cases}
と設定します。$R_1$と$R_2$はユーザーの可調整パラメータであり，$R_1 \gg R_2$となるように設定される。$v_m$は車速の閾値となります。本記事では3つの車速閾値を利用するため，$v_{m}$は$v_{stop}$や後述の$v_{gate}$とは別のパラメータとなります。観測値の誤差が大きいと推察できるときのみ共分散$R$が増大させることにより，観測ノイズの影響を受けにくい推定が可能となります。
最後に，カルマンフィルタの更新ステップにて状態変数を更新します。カルマンフィルタではカルマンゲインと観測値に基づいて状態方程式での予測値を修正することにより，正確な状態推定を実施します。したがって，原理上ある程度正確な観測値が必要となります。しかしながら，GNSSドップラでの姿勢角は特定条件において精度が著しく悪化してしまいます。具体的には，下記のいずれかの条件下においてGNSSドップラでの姿勢角をカルマンフィルタで利用することが難しくなってしまいます。
[1]
GNSSのDOP値が所定値以上であり，GNSSアンテナ上空の衛星配置の影響でGNSSドップラの速度に過大な誤差が生じてしまう時
[2]
車体が極低速で走行することにより，GNSSドップラの速度が完全にノイズに埋もれてしまう時
[3]
車体が停止したにも関わらず，GNSSドップラの速度がノイズの影響で0とならない時
[4]
GNSSの測位ステータス$G$が0以下の値となっており，基準静止衛星や地上基準局の補正情報を利用できない時
[5]
GNSSドップラでの姿勢角と前周期でのカルマンフィルタの推定値に過大な差分が生じており，マルチパス等の影響が疑わしい時
上記の[1]-[5]のいずれかにあてはまるケースではGNSSドップラで得た姿勢角に過大な推定誤差が重畳しているとみなせます。カルマンフィルタで利用してしまうと，カルマンフィルタの推定値が著しく劣化してしまうことが予想されます。したがって，GNSSドップラでの姿勢角を観測値として利用できないとみなせるケースでは，カルマンフィルタによる更新をスキップする必要があります。そこで，カルマンフィルタの更新可否を決定するための変数
\delta[m]:=
\begin{cases}
1, & \text{if } v_a[k_{m}] > v_{gate} \wedge G[m] > 0 \wedge D[m] < D_m \wedge S[k_{m}]=0 \wedge |z[m]-Cx[k_m|k_m-1]| < z_m \\
0 & \text{otherwise}
\end{cases}
を定義します。$D$がDOP値を示します。$D_m$・$z_m$・$v_{gate}$は閾値です。本式では[1]-[5]のいずれかに該当するケースでは，$\delta=0$の値となります。言い換えると，GNSSドップラでの姿勢角推定値の精度が著しく劣化する際に$\delta=0$が成立します。GNSSドップラでの姿勢角推定値をカルマンフィルタで利用可能であれば，$\delta=1$が成立します。$\delta$を後述の更新ステップで利用することにより，GNSSの精度劣化の影響を抑制することを目指します。
線形カルマンフィルタの枠組みで更新ステップでの状態変数更新をします。カルマンゲイン$K$は観測ノイズの分散と誤差共分散により，
K[k_m]=\frac{P[k_m|k_m-1]C^T}{CP[k_m|k_m-1]C^T+R[m]}
と計算できます。本記事では$\delta$を用いてカルマンゲイン更新のたびに状態変数と事後共分散を
x[k_m|k_m]=x[k_m|k_m-1]+\delta[m]K[k_m]\left(z[m]-Cx[k_m|k_m-1]\right)
P[k_m|k_m]=P[k_m|k_m-1]-\delta[m]K[k_m]CP[k_m|k_m-1]
と計算します。本手法では$\delta=0$となる状況において，$x[k_m|k_m]=x[k_m|k_m-1]$と$P[k_m|k_m]=P[k_m|k_m-1]$となります。つまり，上記の[1]-[5]のいずれかにあてはまる状況下ではカルマンフィルタの事後更新がスキップされます。上記の[1]-[5]のいずれにも当てはまらなければ$\delta=1$となり，事後更新式が通常の線形カルマンフィルタと等価となります。以上の手順により，走行状況に合わせてカルマンフィルタの事後更新をスキップすることで推定値の精度劣化を抑制します。
GNSSとIMU・車輪速の測定周期が異なるため，GNSSドップラの観測値更新があれば状態変数を更新し，更新後の推定値を出力します。GNSSドップラの観測値更新がなければ状態方程式の予測値をそのまま出力します。すなわち，姿勢角とヨーレートのバイアス値を
x[k|k]=
\begin{cases}
x[k|k-1], & \text{if } k \neq k_m \\
x[k_m|k_m], & \text{if } k = k_m
\end{cases}
と出力します。本手順により，カルマンフィルタにて姿勢推定を実現できます。
出力制限
カルマンフィルタは白色雑音のみを想定した手法のため，実際のセンサデータに含まれるノイズを完全に除去できないことがあります。したがって，除去しきれなかったはずれ値等の悪影響により，推定値が振動することがあります。また，車体停止中にも関わらずIMUのヨーレートのノイズの影響により，車体停止時においても推定値が振動することがあります。GNSSの測位状況次第では観測値がジャンプすることにより，推定値の急変も懸念されます。したがって，カルマンフィルタの推定値 $\theta_{kf}$ に出力制限を施し，推定誤差の影響を抑制します。ここではレートリミットと停止判定に基づく出力制限を施します。まず，変化率速度に関するレートリミット
\theta_l [k] =
\begin{cases}
\theta_{kf}[k|k], & \text{if} \frac{|\theta_{kf}[k|k]-\theta_l[k-1]|}{T_1}<\omega_{max} \\
\theta_l [k-1]+sgn(\theta_{kf}[k|k]-\theta_l[k-1]) T_1\omega_{max}, & \text{otherwise } \\
\end{cases}
を施します。sgn(・)は符号関数である。$\omega_{max}$がレートリミットの最大変化速度となります。カルマンフィルタの推定値が最大変化速度を超えた場合のみ推定値を制限します。レートリミットの出力に対して，停止判定に基づく出力制限
\theta_e [k] =
\begin{cases}
\theta_e [k-1], & \text{if} S[k]=1 \\
\theta_l [k], & \text{otherwise } \\
\end{cases}
を施します。$\theta_e$が出力制限における姿勢角推定値となります。車体が停止しているのであれば，姿勢角変化が生じないハズなので前時刻の値を出力します。車体が動いていればレートリミットの出力を推定値を$\theta_e$として出力します。
以上の手順により，GNSSとIMUと車輪速の情報を用いて車体の姿勢角を推定できます。ここまでの手順をまとめると，姿勢推定の処理は下記のブロック線図で表すことができます。
Fig.2 姿勢推定のブロック線図
注意事項
ここではいくつかの注意事項に関して解説していきます。
先行事例におけるGNSSとIMU・車輪速の利用事例
産業界やアカデミアにてGNSSを利用した事例は数多く存在します。本節にて自律移動分野を中心にどのような取り組みがあるかを簡単に解説します。
先行文献等
国内のアカデミアにおいて，GNSSドップラ・IMU・車輪速を用いた姿勢推定技術が多数提案されています。特に，名城大学の目黒先生は国内のロボティクス向けのGNSSの権威で知られています。例えば下記のようなGNSSのノウハウ・利用方法に関する有益な記事を執筆しております。
・目黒淳一, 竹内栄二朗, & 鈴木太郎. (2019). ロボティクスにおける GNSS 失敗学. 日本ロボット学会誌, 37(7), 585-592.
他にも様々な文献が発表されているため，もし興味あれば他の文献も調べてみると，GNSSについて理解が深まると思います。
代表的な製品に関して
GNSSによる自己位置推定は運転支援や自動運転の必須技術のため，アカデミアだけでなく長年企業でも製品開発されてきました。現在もGNSS・IMU・車輪速の複合航法を用いた様々な測位システムが企業で販売されています。例えば，ニコン・トリンブル社ではPosLvシリーズのような精度保証された高精度測位ユニットを販売しています。PosLvシリーズは車載での製品評価のための位置や姿勢の真値測定のために利用されており，様々なメーカーで利用されている商品となります。
車載向けの製品として，三菱電機ではGNSS・IMU・車両CAN情報を用いたロケータが販売されています。
過去の三菱電機の技報等を読むと，スバルのレヴォーグ等での採用実績があり，実際の市販車での先進安全機能として活用されているようです。自動車では高度な運転支援のために走行レーンの識別や地図情報照合のためにGNSSを用いた位置・姿勢推定向けのデバイスが採用されています。昨今の先進安全では高度な運転アシストのために「自車がどのレーンを走行しているか」を判別する必要があり，高精度な位置と姿勢を求める必要があります。他の自動車メーカでも多くの先進安全機能として，GNSS・IMU車輪速を用いたデバイスが採用されています。
End2Endの自律移動への活用に関して
昨今大流行しているEnd2Endの自律移動との関係についても言及しておきます。End2Endのアプローチの一部ではカメラオンリーでの自律移動システムの研究が推進されています。昨今の学会や展示会を除くと，一部のスタートアップや研究者が「End2Endのアプローチであれば位置や姿勢情報不要で走行できる」と説明していることも珍しくありません。一方，実際にはいまだ多くの自律移動で位置・姿勢推定が重要視されております。特に，良質なデータを収集するという観点では，位置・姿勢推定の技術は必要不可欠です。
AI主体の自律移動では賢い運転行動を学習させるため，適正なデータセットを準備することが不可欠と言われています。ここからは著者の個人見解・理解となりますが，運転行動を適切にAIに学習させるには外界センサのデータだけでなく，「どのレーンを走行中にどのような運転行動をしているのか」「自分はレーン内のどのあたりを走行していて，車の向きをどう変えて走っているか」「どのような走行軌跡を描いて目的地までたどり着いたか」等の付加情報を加えることが必要な場合もあります。すなわち，アプローチによっては「走行軌跡や運転行動の解釈を付与した質の良い教師データ」をAIに学習させる必要があると考えられます。データセットに付加情報を加えるにはある程度精度の良い自己位置・姿勢情報が不可欠のため，データセット作成の観点でもGNSSを用いたデバイスが活用されていくと考えられます。
車体の姿勢推定に関しては，データセットの付加情報以外の観点でも重要性に関して分析されています。例えば，UniADやVAD等の代表的なEnd2Endの自律移動のアプローチではマルチカメラで得た画像をBEV特徴マップという鳥瞰図形式の空間情報に落とし込みます。
Jiang, B., Chen, S., Xu, Q., Liao, B., Chen, J., Zhou, H., ... & Wang, X. (2023). Vad: Vectorized scene representation for efficient autonomous driving. In Proceedings of the IEEE/CVF International Conference on Computer Vision (pp. 8340-8350).
Hu, Y., Yang, J., Chen, L., Li, K., Sima, C., Zhu, X., ... & Li, H. (2023). Planning-oriented autonomous driving. In Proceedings of the IEEE/CVF conference on computer vision and pattern recognition (pp. 17853-17862).
ここで，過去にマップに蓄積された特徴マップの位置や角度を現在のフレームに整列させるために自車の姿勢情報を必要とします。言い換えると，自車がフレーム間にどれぐらい動いたかの補正が必要となるわけです。また，プランニングにおける軌跡計算においても自車の情報が必要となります。もし，自車の姿勢情報を用いないと経路計画の性能が大幅に劣化することが下記の文献で知られています。したがって，GNSS等の複合利用により姿勢推定が必要になると考えられます。
Li, Z., Yu, Z., Lan, S., Li, J., Kautz, J., Lu, T., & Alvarez, J. M. (2024). Is ego status all you need for open-loop end-to-end autonomous driving?. In Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (pp. 14864-14873).
以上のように，アプローチによってはいまだ位置や姿勢の推定情報は必要とされています。
横滑り角に関して
本記事では問題設定・議論を簡単化するため，あえて横滑り角について無視して議論を進めてきました。しかしながら，本来であれば車輪付きの自律移動の高精度な姿勢を推定するのであれば横滑り角の影響に関しても議論すべきです。ここでは，横滑り角の影響に関して解説していきます。GNSSドップラで得られる速度とは「衛星とGNSSアンテナ間の相対速度」となります。言い換えると，GNSSドップラで得られる姿勢角とは車体の向きではなく，進行方向の角度となります。したがって，GNSSドップラで得られる角度とは厳密には「車体の姿勢角と横滑り角の複合値」となります。このあたりの議論は例えば下記のような文献で議論されています。
・高野瀬碧輝, 渥美善規, 滝川叶夢, & 目黒淳一. (2020). 都市部環境下で適用可能な GNSS/IMU に関する研究-横すべり角を考慮した相対位置性能と絶対位置性能の評価. 自動車技術会論文集, 51(4), 721-726.
ここで，横滑り角が推定値に与える影響に関して考えてみます。車体が直線や緩やかなカーブを走行しているのであれば横滑り角がほぼ生じないため，姿勢角推定に与える影響は少なそうです。横滑り角は車体がある程度大きい曲率のカーブを走行した際に生じます。ただし，本記事の手法ではGNSSドップラで得た姿勢角をそのまま用いずにIMUのヨーレートの情報と統合することで姿勢を推定しています。横滑り角の影響を受けないIMUのヨーレートをカルマンフィルタで統合したことにより，カルマンゲインの大きさ次第では横滑り角の影響がある程度低減されていると考えられます。また，車体の横滑り角は車体前後の重心により大きさが変わることを考えると全ての車体で影響が出るわけでもありません。
以上の点から，横滑り角が姿勢角の推定精度にただちに悪影響を与えるとは断定できません。しかしながら，より高精度な姿勢を推定するのであれば，やはり横滑り角を考慮して推定すべきです。なお，前回の記事で横滑り角に関してある程度解説をしてありますので，もし気になる方はこちらもご覧ください。
時刻同期に関して
本記事で紹介したようにGNSSを自律移動に用いる場合，IMUや車輪速センサの情報と統合して利用することが必要となります。本記事では省略しましたが，実際には複数のセンサを組み合わせる都合上「時刻同期」の処理が必要となります。各センサの物理量は異なる原理・デバイスで測定されています。したがって，測定タイミングや通信遅延等がバラバラであり，各センサの値をそのままカルマンフィルタで統合すると時刻の誤差の影響による精度劣化が発生します。したがって，実際には時刻同期の処理を施し，ある基準時刻に対して受信したセンサ信号の時刻を同期させる必要があります。後述のサンプルプログラムでは省略していますが，実機で位置や姿勢を推定するには時刻同期の必要があることを注意した方がよいでしょう。
カルマンフィルタの設計に関して
本記事では車輪速の値に応じてカルマンフィルタの$R$を時変パラメータとして切り替えることで良好な推定を実現する方法について述べました。しかしながら，実際に産業現場等で利用するのであれば，より複雑な条件分岐にて$R$の値を変動させる必要があります。より精度向上を目指すのであれば，本記事のように2値の条件分岐ではなく，Look up Tableにて$R$の値を決めることも考えられます。$R$だけではなく，$Q$の値を変動させる必要もあるでしょう。また，産業製品ではセンサ異常時の安全性も考慮しなければならないため，特定のセンサ故障・通信異常時のためのパラメータ設計も必要です。例えば，何らかの理由でIMUが劣化してヨーレートの測定精度が低下したのであれば，$Q$の値を増加させてIMUの劣化の影響を抑制するような手法も求められます。
本記事で紹介した方法は最も簡単な事例のため，実際に製品現場で使用するにはカルマンフィルタのパラメータ設計に関して様々なノウハウが必要なことは注意した方がよいでしょう。
ROS 2サンプルプログラムでの例
それではサンプルプログラムを用いて本記事の方法で姿勢推定ができるかを試してみます。本記事では下記の記事を参考にしてデータセットを準備しました。u-blox ZED-F9Pを準備して,ROS 2 humbleでIMUと車輪速の値をROSbagデータとしてロギングしました。
ロギングした実験データを用いて実際に姿勢推定できるかを確認します。サンプルとしてGithubに下記のパッケージを準備しました。動作環境の詳細はReadmeに記載しておきましたが，Ubuntu22.04とROS 2humble環境での動作を確認しています。なお，サンプルコード内では本記事で解説した手法の一部を条件分岐を用いた等価な処理に置き換えています。また，ラジアン角度における±πの値跨ぎが起きない運用・前処理を実装しています。アルゴリズムとしては動作も含めて等価になっていますが，実装の都合により数式とコードの記載が一致していない部分があることにご注意ください。
実際にROSbagデータを用いて姿勢推定した結果が下記となります。赤線がGNSSドップラ・IMU・車輪速センサの複合利用による推定結果，青線が前回の記事の方法を基にしたIMU単独利用による推定結果です。なお，今回の図に関しては可読性向上のために角度をアンラップさせて表示しています。実際のROSのメッセージでは角度が$-\pi～\pi$のラジアン角になることにご注意ください。
Fig.3 推定結果
GNSSとIMUの統合によって姿勢が正しく推定されております。10-70 s間ではどちらの波形もおおむね似た傾向となっているように見えます。しかしながら，144–154 s間では二つの推定値間が乖離していることがわかります。前回の記事で解説したようにIMU単独での姿勢推定ではIMUに含まれるバイアス誤差の影響で時間経過でドリフトが生じてしまいます。144–154 s間では，IMUによる姿勢推定値に過大な推定誤差が生じたことで二つの推定値間が乖離したと考えられます。本記事の手法ではGNSSドップラを用いたことにより，長時間経過してもドリフトが生じず姿勢推定を継続できます。今回はレートリミットの最大変化速度を緩めに設定したことで一部値が大きく変化している箇所もありますが，このあたりもパラメータをファインチューニングすれば解消できそうです。したがって，推定値ドリフトが抑制され，所望の推定を実現できました。以上の点から，姿勢推定できることを実験にて確認できました。
おわりに
本記事ではGNSSドップラ・IMU・車輪速に基づく姿勢推定に関して解説しました。今回の記事の内容はすでに色々な業界で活用されている手法を一部簡略化版の解説となっております。また，githubに公開しているサンプルソフトも必要最低限の機能を実装したものとなっており，実際に活用する場合は適用対象に応じたカスタマイズが必要です。
注意事項で述べたように正確な自己位置情報を必要としない自律移動システムが普及したとしても，ナビゲーションや運転行動の評価・データセット作成等の何らかの形で利用されると考えられます。また，自律移動システムでは機能安全のためのルールベースでの縮退運転機能が必要であり，それには自己位置推定や姿勢推定が必要不可欠です。少なくとも安全性を保証した上で世の中に製品を出すには位置や姿勢の推定はいまだ重要といえるでしょう。本記事が読者の方に少しでも役立つと幸いです。