JP6409625B2 - 車両位置算出装置 - Google Patents

Description

本発明は、加速度計やジャイロセンサを用いて移動体の位置を求める技術に関する。

従来、複数軸の加速度および複数軸の各速度を検出するセンサユニットから得られる情報によって車両の姿勢（ロール角、ピッチ角）を推定し、その推定結果を車速と組み合わせた速度ベクトルを積算することで、車両の位置を推定する技術が知られている（特許文献１参照）。

特開２００９−１９８２７９号公報

ところで、センサユニットでの計測結果を表すための座標軸の一つを基準軸（例えばＺ軸）とし、この基準軸であるＺ軸が車両の位置する路面の法線ベクトルと一致するように設定される。このため、何等かの理由で車両の姿勢が傾くことにより、車両と一体に設けられたセンサユニットのＺ軸方向が路面の法線ベクトルに対して傾斜する、いわゆる軸ずれが生じると、位置の推定精度が劣化するという問題があった。

図３は、カーブ路を走行した時に、ＧＰＳによって求めた軌道と、加速度計やジャイロセンサ等からなるセンサユニットを利用し自律航法によって求めた軌道を表示したものである。カーブ路を走行することによって、自律航法による軌道は、ＧＰＳによる軌道から角速度が小さくなる方向のずれが生じ、車速が大きいほどそのずれが大きくなることがわかる。

本発明は、こうした問題に鑑みてなされたものであり、センサユニットの座標軸の軸ずれによる影響を軽減する技術の提供を目的とする。

本発明の車両位置算出装置は、センサユニットと、移動方向検出部と、測位部と、遠心力算出部と、対処実行部とを備える。
センサユニットは、移動体に加わる加速度および角速度を計測する複数のセンサを含む。移動方向検出部は、センサユニットからの検出信号に従って、移動体の移動方向を表す単位ベクトルを生成する。測位部は、移動方向検出部にて生成された単位ベクトルを利用して、少なくとも移動体の位置を求める。遠心力算出部は、移動体に加わる遠心力を求める。対処実行部は、遠心力算出部にて算出された遠心力が予め設定された閾値を超えた場合に、測位部での測位結果の劣化に対処する処理を実行する。

即ち、センサユニットは、移動体と一体に設けられているため、移動体に遠心力が加わることによって移動体の姿勢が傾斜すると、これに応じてセンサユニットの座標軸も傾斜する。つまり、遠心力とセンサユニットの座標軸の傾斜角度とは相関関係を有するため、遠心力から軸ずれの程度、ひいては測位結果の劣化の程度を判断することができる。

このような構成によれば、簡易に測定可能な遠心力を用いて、測位結果の劣化を判断してこれに対処するため、簡易な構成によって軸ずれによる影響を軽減することができる。
なお、測位結果の劣化への対処としては、アラームを発生させて利用者に通知するだけでもよいし、車載装置から得られる各種情報を利用して軸ずれを相殺するように単位ベクトルを補正する等してもよい。

なお、特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

車両位置算出装置の構成を示すブロック図である。 走行方向単位ベクトル生成部の構成を示すブロック図である。 カーブ路を走行時に、ＧＰＳによって求めた軌跡と自律航法によって求めた軌跡とを示すグラフである。

以下に本発明が適用された実施形態について、図面を用いて説明する。
［構成］
図１に示す車両位置算出装置１は、車両に設置され、６軸センサユニット２と、車速センサ３と、ＧＰＳ受信機４と、車載装置群５と、遠心力算出部６と、走行方向単位ベクトル生成部７と、アラーム駆動部８と、推測航法演算部９とを備える。

６軸センサユニット２は、Ｘ軸ジャイロセンサ２１、Ｙ軸ジャイロセンサ２２、Ｚ軸ジャイロセンサ２３、Ｘ軸加速度センサ２４、Ｙ軸加速度センサ２５、Ｚ軸加速度センサ２６を備える。なお、Ｘ軸、Ｙ軸、Ｚ軸は、互いに直交する３軸であり、６軸センサユニット２を車両に搭載した時に、車両の前後方向がＸ軸、車両の左右方向がＹ軸、車両の上下方向がＺ軸と一致するように設置される。Ｘ軸ジャイロセンサ２１は、Ｘ軸を中心とする角速度Ｗｘを検出し、Ｙ軸ジャイロセンサ２２は、Ｙ軸を中心とする角速度Ｗｙを検出し、Ｚ軸ジャイロセンサ２３は、Ｚ軸を中心とする角速度Ｗｚを検出する。Ｘ軸加速度センサ２４は、Ｘ軸に沿った方向の加速度Ａｘを検出し、Ｙ軸加速度センサ２５は、Ｙ軸に沿った方向の加速度Ａｙを検出し、Ｚ軸加速度センサ２６は、Ｚ軸に沿った方向の加速度Ａｚを検出する。

車速センサ３は、車両の車速Ｖまたは車速の算出に要な信号（例えば車速パルス）を出力する。
ＧＰＳ受信機４は、ＧＰＳ衛星からの信号を受信して、現在位置や移動速度、移動方向等を求める周知のものである。

車載装置群５は、６軸センサユニット２，車速センサ３，ＧＰＳ受信機４を含む車両に搭載された各種車載装置からなる。具体的には、上述した６軸センサユニット２，車速センサ３，ＧＰＳ受信機４の他、車両の周囲を撮像する画像センサ、車両の周囲に存在する物標を検出する各種レーダセンサ、車両の傾きを検出する傾斜センサ、ステアリングの操作角度（ステア角）を検出するステア角センサ、車両の現在位置の高度を計測する高度計、ナビゲーション等に使用する地図情報、推測航法演算部９で求められた走行軌跡の情報などが車載装置群５に含まれる。

遠心力算出部６は、６軸センサユニット２で検出される角速度Ｗｘ，Ｗｙ，Ｗｚと、車速センサ３で検出される車速Ｖとから遠心力を算出し、その算出された遠心力が予め設定された閾値を超えた場合にアクティブレベルとなる補正許可信号ＥＮを出力する。なお、遠心力の計算は、Ｚ軸の角速度Ｗｚと車速Ｖとで簡易的に求めてもよいし、走行方向単位ベクトル生成部７で生成される単位ベクトルＰも利用して精密に求めてもよい。また、６軸センサユニット２で検出される加速度Ａｘ，Ａｙ，Ａｚから求めてもよい。

アラーム駆動部８は、遠心力算出部６からの補正許可信号ＥＮがアクティブレベルに変化すると、自律航法による測位精度が劣化していることを示すアラームの出力を、図示しない車載音響機器に対して指示する。

走行方向単位ベクトル生成部７は、ジャイロセンサ２１〜２３の出力Ｗｘ，Ｗｙ，Ｗｚに従って走行方向を示す走行方向単位ベクトルＰを生成し、補正許可信号ＥＮがアクティブレベルである間、車載装置群５からの情報を利用して、遠心力による車両の傾斜による精度劣化を補う処理を実行する。その詳細については後述する。

推測航法演算部９は、走行方向単位ベクトルＰと車速Ｖを組み合わせた車速ベクトルを累積することで自律航法による位置推定を行うと共に、ＧＰＳ受信機４から得られる情報との差に基づき、カルマンフィルタ等を利用して自律航法での位置推定に使用する状態量の補正等を行う周知のものである。

なお、遠心力算出部６、走行方向単位ベクトル生成部７、推測航法演算部９の機能は、ＣＰＵ，ＲＯＭ，ＲＡＭを中心に構成されたマイクロコンピュータが実行する処理によって実現してもよいし、その機能全体または一部を、ロジック回路等のハードウェアによって実現してもよい。

［走行方向単位ベクトル生成部］
走行方向単位ベクトル生成部７は、図２に示すように、変換行列生成部７１と、変換行列補正部７２と、乗算器７３と、単位ベクトル記憶部７４と、補正値算出部７５とを備える。

変換行列生成部７１は、ジャイロセンサ２１〜２３にて検出された各軸の角速度Ｗｘ，Ｗｙ，Ｗｚに基づき、車両の走行方向の変化を表現する変換行列を生成する。なお、変換行列は、オイラー角を用いて表現される周知の回転行列を用いる。

変換行列補正部７２は、補正値算出部７５で求められた補正値に従って変換行列を補正する。
乗算器７３は、変換行列補正部７２から供給される変換行列に、１サイクル前の走行方向単位ベクトルＰｎ−１を乗じて今サイクルの走行方向単位ベクトルＰｎを算出する。

単位ベクトル記憶部７４は、乗算器７３で算出された走行方向単位ベクトルＰｎを記憶し、次サイクルで１サイクル前の走行方向単位ベクトルＰｎ−１として、乗算器７３に供給する。

補正値算出部７５は、補正許可信号ＥＮがアクティブレベルである間、車載装置群５からの信号に基づいて補正値となる角度差を求めるものであり、車両傾き角算出部７５１と、路面傾き角算出部７５２と、センサ傾き角算出部７５３とを備える。

車両傾き角算出部７５１は、車載装置群５から取得する各種情報に基づいて、鉛直方向（道路面の法線ベクトルが示す方向）に対する６軸センサユニット２で定義されるＺ軸の傾き角、即ち、道路面に対する車両の傾き角（以下「車両傾き角」という）を求める。具体的には、例えば、車載装置群５から傾斜センサでの検出結果を取得し、その検出結果から算出してもよい。また、車載装置群５から、車速や角速度、ステア角等、車両に加わる遠心力の算出に必要な情報を取得し、その取得結果から算出した遠心力を用いて傾き角を算出してもよい。

路面傾き角算出部７５２は、車載装置群５から取得する各種情報に基づいて、水平面の法線ベクトルに対する路面の法線ベクトルの傾き角、即ち、水平面に対する路面の傾き角（以下「路面傾き角」という）を算出する。具体的には、例えば、ＧＰＳ受信機４または推測航法演算部９で推定された位置情報に基づき、地図情報から現在位置の路面の傾きに関する情報を取得し、これを使用する。また、車両の周囲を撮像する画像センサからの情報を取得して、撮像された物体の状況等から推定してもよい。

センサ傾き角算出部７５３は、車両傾き角算出部７５１で算出された車両傾き角と路面傾き角算出部７５２で算出された路面傾き角とに基づき、水平面に対する６軸センサユニット２の傾き角を算出する。

つまり、走行方向単位ベクトル生成部７では、遠心力が閾値より小さく、６軸センサユニット２が傾くことによって生じる測位誤差が許容範囲内である場合は、ジャイロセンサの検出結果から生成された変換行列を用いて、測位に使用する走行方向単位ベクトルＰを生成する。一方、遠心力が閾値以上であり、６軸センサユニット２が傾くことによって生じる測位誤差が許容範囲を超える場合は、変換行列を、６軸センサユニット２の傾き角が相殺されるように、補正した変換行列を用いて走行方向単位ベクトルＰを生成する。

［効果］
以上説明したように、車両位置算出装置１は、水平面に対する６軸センサユニット２の傾きによる測位結果の劣化を、角速度Ｗｘ，Ｗｙ，Ｗｚと車速Ｖから求めた遠心力によって判断し、劣化が許容範囲を超える場合には、アラームによる乗員への通知を行うと共に、走行方向単位ベクトル生成部７にて生成される走行方向単位ベクトルＰを、軸ずれの影響が相殺されるように補正している。

このような構成によれば、軸ずれによる測位結果の劣化への対処（アラーム、走行方向単位ベクトルの補正）を、簡易な構成で実現することができる。
なお、対処する処理としては、単にアラームを発生させて測位情報の利用者に通知するだけでもよいし、その軸ずれを相殺するように単位ベクトルを補正する等してもよい。

［他の実施形態］
以上、本発明の実施形態について説明したが、本発明は、上記実施形態に限定されることなく、種々の形態を採り得る。

（１）上記実施形態では、６軸センサユニット２の傾きによる測位結果の劣化への対処として、アラームと走行方向単位ベクトルの補正をいずれも実行しているが、いずれか一方だけを実行するように構成してもよい。

（２）上記実施形態では、走行方向単位ベクトルＰの算出に、回転行列を用いているが、これに限定されるものではない。例えば、四元数によって三次元回転を表現した関数等を用いて算出するように構成してもよい。

（３）上記実施形態における一つの構成要素が有する機能を複数の構成要素に分散させたり、複数の構成要素が有する機能を一つの構成要素に統合させたりしてもよい。また、上記実施形態の構成の少なくとも一部を、同様の機能を有する公知の構成に置き換えてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加または置換等してもよい。なお、特許請求の範囲に記載した文言のみによって特定される技術思想に含まれるあらゆる態様が本発明の実施形態である。

（４）本発明は、上述した車両位置算出装置の他、当該車両位置算出装置を構成要素とするシステム、当該車両位置算出装置としてコンピュータを機能させるためのプログラム、このプログラムを記録した媒体、車両位置算出方法など、種々の形態で実現することもできる。

１…車両位置算出装置 ２…６軸センサユニット ３…車速センサ ４…ＧＰＳ受信機 ５…車載装置群 ６…遠心力算出部 ７…走行方向単位ベクトル生成部 ８…アラーム駆動部 ９…推測航法演算部 ２１〜２３…ジャイロセンサ ２４〜２６…加速度センサ ７１…変換行列生成部 ７２…変換行列補正部 ７３…乗算器 ７４…単位ベクトル記憶部 ７５…補正値算出部 ７５１…車両傾き角算出部 ７５２…路面傾き角算出部 ７５３…センサ傾き角算出部

Claims (3)

1. 移動体に加わる加速度および角速度を計測する複数のセンサを含んだセンサユニット（２）と、
   前記センサユニットからの検出信号に従って、前記移動体の移動方向を表す単位ベクトルを生成する移動方向検出部（７１，７３，７４）と、
   前記移動方向検出部にて生成された単位ベクトルを利用して、少なくとも移動体の位置を求める測位部（９）と、
   前記移動体に加わる遠心力を求める遠心力算出部（６）と、
   前記遠心力算出部にて算出された遠心力が予め設定された閾値を超えた場合に、前記測位部での測位結果の劣化に対処する処理を実行する対処実行部（７２，７５，８）と、
   を備え、
   前記対処実行部は、
   前記移動体が位置する路面の法線ベクトルが示す方向を路面法線方向とし、前記移動体が非傾斜状態にある時に、前記路面法線方向と一致するように設定された前記センサユニットで使用する座標系において基準となる基準軸の方向を基準軸方向とし、前記路面法線方向に対する前記基準軸方向の傾斜角を前記センサユニットの傾き角として算出する傾斜角算出部（７５）と、
   前記移動方向検出部が前記単位ベクトルの生成に使用する関数のパラメータを、前記傾斜角度算出部にて算出された傾斜角が相殺されるように補正する関数補正部（７２）と、
   を備えることを特徴とする車両位置算出装置。
2. 前記関数補正部で使用する関数は、オイラー角によって三次元回転を表現した回転行列であることを特徴とする請求項１に記載の車両位置算出装置。
3. 前記対処実行部は、
   前記移動体に設けられたアラームを作動させるための指令を出力するアラーム指令出力部（８）を備えることを特徴とする請求項１または請求項２に記載の車両
   位置算出装置。