(57)【要約】【課題】傾斜角を算出するための遅延を低減する技術を提供する。【解決手段】第１取得部１４は、移動体の加速度を取得する。第２取得部１６は、移動体の移動距離を取得する。推定部２２は、移動距離と加速度とをもとに、移動体の速度を推定する。速度差演算部２４は、速度の変化を演算することによって、第１の速度差を導出する。加速度積算部２０は、加速度を積算することによって、第２の速度差を導出する。傾斜角演算部２６は、第２の速度差と第１の速度差とをもとに、移動体の傾斜角を導出する。【選択図】図１
2014119393.tif 000002
【特許請求の範囲】
【請求項１】
移動体の加速度を取得する第１取得部と、
前記移動体の移動距離を取得する第２取得部と、
前記第２取得部において取得した移動距離と、前記第１取得部において取得した加速度とをもとに、前記移動体の速度を推定する推定部と、
前記推定部において推定した速度の変化を演算することによって、第１の速度差を導出する速度差演算部と、
前記第１取得部において取得した加速度を積算することによって、第２の速度差を導出する加速度積算部と、
前記加速度積算部において導出した第２の速度差と、前記速度差演算部において導出した第１の速度差とをもとに、前記移動体の傾斜角を導出する傾斜角演算部と、
を備えることを特徴とする傾斜角検出装置。
【請求項２】
前記第２取得部は、
車速パルスをもとに、第１時間間隔での距離を順次算出する距離計と、
前記距離計において順次算出した第１時間間隔での距離を低域通過フィルタ処理することによって、移動距離を導出するフィルタ部とを備え、
前記第１取得部は、第１時間間隔以下の第２時間間隔での出力であって、かつ加速度センサからの出力をもとに、加速度を算出することを特徴とする請求項１に記載の傾斜角検出装置。
【請求項３】
前記推定部は、前記第１取得部において取得した加速度を入力値とし、前記第２取得部において取得した移動距離を観測値として、カルマンフィルタ処理を実行することによって速度を推定することを特徴とする請求項１または２に記載の傾斜角検出装置。
【請求項４】
移動体の加速度を取得するステップと、
前記移動体の移動距離を取得するステップと、
取得した移動距離と、取得した加速度とをもとに、前記移動体の速度を推定するステップと、
推定した速度の変化を演算することによって、第１の速度差を導出するステップと、
取得した加速度を積算することによって、第２の速度差を導出するステップと、
導出した第２の速度差と、導出した第１の速度差とをもとに、前記移動体の傾斜角を導出するステップと、
を備えることを特徴とする傾斜角検出方法。
【請求項５】
移動体の加速度を取得するステップと、
前記移動体の移動距離を取得するステップと、
取得した移動距離と、取得した加速度とをもとに、前記移動体の速度を推定するステップと、
推定した速度の変化を演算することによって、第１の速度差を導出するステップと、
取得した加速度を積算することによって、第２の速度差を導出するステップと、
導出した第２の速度差と、導出した第１の速度差とをもとに、前記移動体の傾斜角を導出するステップと、
をコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、傾斜角検出技術に関し、特に車両の傾斜角を検出する傾斜角検出装置、傾斜角検出方法、およびプログラムに関する。
【背景技術】
【０００２】
従来、カーナビゲーション装置は、絶対位置を得ることが可能なＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）と、ジャイロや速度パルス等のセンサから計算される相対位置を得る自立航法を組合わせて、車両位置や進行方位を算出する。自立航法では、道路傾斜を含む車両の姿勢角が考慮されることによって、より高精度な位置を算出することが可能になる。車両の姿勢角である傾斜角は、例えば、傾斜角が単位時間において変化しないというモデルから導出された状態方程式をもとにカルマンフィルタ処理にて導出される（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−１０２７８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
傾斜角が変化する場合においても、傾斜角を導出することを可能とするために、傾斜角を算出するための遅延を低減することが望まれる。
【０００５】
本発明はこうした状況に鑑みてなされたものであり、その目的は、傾斜角を算出するための遅延を低減する技術を提供することにある。
【課題を解決するための手段】
【０００６】
上記課題を解決するために、本発明のある態様の傾斜角検出装置は、移動体の加速度を取得する第１取得部と、移動体の移動距離を取得する第２取得部と、第２取得部において取得した移動距離と、第１取得部において取得した加速度とをもとに、移動体の速度を推定する推定部と、推定部において推定した速度の変化を演算することによって、第１の速度差を導出する速度差演算部と、第１取得部において取得した加速度を積算することによって、第２の速度差を導出する加速度積算部と、加速度積算部において導出した第２の速度差と、速度差演算部において導出した第１の速度差とをもとに、移動体の傾斜角を導出する傾斜角演算部と、を備える。
【０００７】
本発明の別の態様は、傾斜角検出方法である。この方法は、移動体の加速度を取得するステップと、移動体の移動距離を取得するステップと、取得した移動距離と、取得した加速度とをもとに、移動体の速度を推定するステップと、推定した速度の変化を演算することによって、第１の速度差を導出するステップと、取得した加速度を積算することによって、第２の速度差を導出するステップと、導出した第２の速度差と、導出した第１の速度差とをもとに、移動体の傾斜角を導出するステップと、を備える。
【０００８】
なお、以上の構成要素の任意の組合せ、本発明の表現を方法、装置、システム、記録媒体、コンピュータプログラムなどの間で変換したものもまた、本発明の態様として有効である。
【発明の効果】
【０００９】
本発明によれば、傾斜角を算出するための遅延を低減できる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施例に係る傾斜角検出装置の構成を示す図である。
【図２】図１の傾斜角検出装置が搭載された車両の動作状態を示す図である。
【図３】図１の傾斜角検出装置による傾斜角の出力手順を示すフローチャートである。
【発明を実施するための形態】
【００１１】
本発明を具体的に説明する前に、まず概要を述べる。本発明の実施例は、カーナビゲーション装置での自立航法において、車両の傾斜角を検出する傾斜角検出装置に関する。実施例に係る傾斜角検出装置は、傾斜角を算出するための遅延を低減し、傾斜角を高精度に検出するために、次の処理を実行する。本実施例に係る傾斜角検出装置は、車速パルスの移動平均による遅延時間に合うように、加速度センサ出力の遅延を調整する。また、傾斜角検出装置は、移動平均をした車速パルスを観測値とし、遅延調整した加速度センサ出力をもとに算出した加速度を入力値としたカルマンフィルタ処理を実行することによって、速度を推定する。さらに、傾斜角検出装置は、所定時間間隔の２時点の推定速度の差（以下、「第１の速度差」という）と、該所定時間間隔における加速度の積分値（以下、「第２の速度差」という）とをもとに、傾斜角を算出する。なお、カルマンフィルタ処理において使用される状態変数にも傾斜角が含まれているが、これとは別にふたつの速度差から傾斜角が算出されるので、遅延が低減される。また、車速パルスが移動平均されるので、傾斜角の精度が向上する。
【００１２】
図１は、本発明の実施例に係る傾斜角検出装置１００の構成を示す。傾斜角検出装置１００は、第１取得部１４、第２取得部１６、リングバッファ１８、加速度積算部２０、推定部２２、速度差演算部２４、傾斜角演算部２６、制御部２８を含み、第２取得部１６は、距離計３０、フィルタ部３２を含む。また、傾斜角検出装置１００は、加速度センサ１０、距離センサ１２に接続される。さらに、傾斜角検出装置１００は、図示しない車両に搭載される。
【００１３】
まず、処理時間の記号を定義する。ここでは、車速パルスを計数する際の時間間隔をΔＴ１し、加速度センサ１０がサンプリングする時間間隔をΔＴ２とし、傾斜角の演算の時間間隔をΔＴ３とする。なお、ΔＴ１が第１時間間隔とよばれ、ΔＴ２が第２時間間隔とよばれることもある。また、各時間間隔は、ΔＴ２≦ΔＴ１≦ΔＴ３の関係である。以下の説明では、ｍΔＴ２＝ｍΔＴ１＝ΔＴ３とする。ただし、ｍは正の整数である。
【００１４】
加速度センサ１０は、車両の主たる移動方向、例えば、車両の前後方向に生じる加速度を検出する。ここでは、加速度を説明するために図２を使用する。図２は、傾斜角検出装置１００が搭載された車両５０の動作状態を示す。車両５０は、図１の傾斜角検出装置１００を搭載する。また、車両５０は、道路面５４上を移動する。ここで、車両５０に搭載された加速度センサ１０（図示せず）の感応軸５６は、道路面５４に平行であり、かつ車両５０の前後方向と一致するものとする。また、感応軸５６および道路面５４は、水平面５２に対して傾斜角θだけ傾いている。加速度においては、車両５０の移動加速度と、感応軸５６が水平面５２となす傾斜角θに応じた重力成分とが重畳されている。図１に戻る。なお、加速度センサ１０における検出は、第２の時間間隔ごとに順次なされる。加速度センサ１０は、検出結果を第１取得部１４へ出力する。距離センサ１２は、車両５０の移動距離を検出するための車速パルスを検出し、車速パルスを距離計３０へ出力する。
【００１５】
第１取得部１４は、加速度センサ１０に接続され、加速度センサ１０からの出力、つまり加速度センサ１０での検出結果を受けつける。検出結果は、第１時間間隔以下の第２時間間隔で受けつけられる。第１取得部１４は、検出結果をもとに加速度を算出する。具体的に説明すると、第１取得部１４は、加速度センサ１０の検出結果を加速度の単位に変換し、後述のフィルタ部３２の遅延時間に等しくなるように変換結果を遅延させる。以下では、変換結果、あるいは遅延させた変換結果を加速度とよぶこともある。なお、第１取得部１４では、遅延のためにフィルタが使用されてもよいし、フィルタと遅延手段との組合わせが使用されてもよい。第１取得部１４は、加速度をリングバッファ１８、推定部２２に出力する。
【００１６】
距離計３０は、距離センサ１２に接続され、距離センサ１２からの車速パルスを受けつける。距離計３０は、車速パルスをもとに、第１時間間隔での距離を順次算出する。具体的に説明すると、距離計３０は、ΔＴ１内に計数された車速パルスの数をもとに、ΔＴ１内における移動距離を算出する。フィルタ部３２は、距離計３０において順次算出した第１時間間隔での距離を低域通過フィルタ処理することによって、車両５０の移動距離を導出する。例えば、フィルタ部３２は、距離計３０の出力を移動平均する。その結果、距離の分解能が、１パルス単位から移動平均の数の逆数まで向上される。フィルタ部３２は、移動距離を推定部２２に出力する。
【００１７】
推定部２２は、第２取得部１６において取得した移動距離と、第１取得部１４において取得した加速度とをもとに、車両５０の速度を推定する。例えば、推定部２２は、第１取得部１４において取得した加速度を入力値とし、第２取得部１６において取得した移動距離を観測値として、カルマンフィルタ処理を実行することによって速度を推定する。ここでは、カルマンフィルタ処理の内容を具体的に説明する。状態変数ｘｎは、次のように、距離ｓｎ、速度ｖｎ、傾斜角θｎの３つの要素とする。
【数１】
2014119393.tif 000003
ここで、上付きの添え字Ｔは、ベクトルまたは行列の転置を示す。また、下付きの添え字ｎはタイミングｎを示す。
【００１８】
推定部２２は、入力ｕｎと、１回前の状態変数、つまりタイミングｎ−１での状態変数ｘｎ−１とをもとに、次に示された状態方程式を計算することによって、タイミングｎでの推定値ｘｎ’を演算する。なお、状態変数ｘｎ−１は、導出された後、保持されている。
【数２】
2014119393.tif 000004
ここで、入力ｕｎは、加速度センサ出力ａＯＵＴｎと加速度センサオフセットａＯＦＳｎである。
【数３】
2014119393.tif 000005
なお、加速度センサ出力ａＯＵＴｎは、第１取得部１４からの加速度に相当し、加速度センサオフセットａＯＦＳｎは、公知の技術等によって導出されている。
【００１９】
また、式（２）におけるシステム行列Ａと入力行列Ｂとは、式（４）および式（５）のように示される。
【数４】
2014119393.tif 000006
【数５】
2014119393.tif 000007
ここで、ｇは重力加速度、ｋは加速度単位（例えばｍ／ｓ２）への変換係数、ΔＴはサンプリング間隔を示す。サンプリング間隔は、ΔＴ３に相当する。前述のごとく、加速度センサの感応軸は、車両５０の前後方向と一致するものとする。
【００２０】
推定部２２は、フィルタ部３２からの移動距離を観測値ｚｎとして、式（２）にて計算された推定値ｘｎ’から抜き出された状態変数の速度と加速度のオフセットとのそれぞれの差を次のように求める。
【数６】
2014119393.tif 000008
ここで、出力行列Ｃは次のように示される。
【数７】
2014119393.tif 000009
【００２１】
推定部２２は、距離計３０の観測誤差δｓｎおよびシステム誤差よりカルマンゲインＫｎを演算する。観測誤差は、標準偏差を表すものとして、共分散行列Ｒｎは下式となる。
【数８】
2014119393.tif 000010
ここで、共分散行列Ｒ等からカルマンフィルタのゲインの導出には、公知の技術が使用されればよい。公知の技術は、例えば、ＭＯＨＩＮＤＥＲ  Ｓ．  ＧＲＥＷＡＬ、ＡＮＧＵＳ  Ｐ．  ＡＮＤＲＥＷＳ著「Ｋａｌｍａｎ  Ｆｉｌｔｅｒｉｎｇ：Ｔｈｅｏｒｙ  ａｎｄ  Ｐｒａｃｔｉｃｅ  Ｕｓｉｎｇ  ＭＡＴＬＡＢ」、Ａ  Ｗｉｌｅｙ−Ｉｎｔｅｒｓｃｉｅｎｃｅ  Ｐｕｂｌｉｃａｔｉｏｎの１１６頁から１２０頁に示されている。本実施例の場合、カルマンゲインＫｎは下式のように３個得られる。
【数９】
2014119393.tif 000011
【００２２】
推定部２２は、推定値ｘｎ’、カルマンゲインＫｎと観測残差ｙｎとから推定値ｘｎを次のように更新する。
【数１０】
2014119393.tif 000012
カルマンゲインＫｎが小さければ推定値ｘｎ’の重みが大きく、一方、カルマンゲインＫｎが大きければ観測値Ｚｎの重みが大きい。式（１０）により求められた推定値ｘｎの速度成分が、求める速度になる。ここで、推定値ｘｎには傾斜角も成分として含まれるが、この傾斜角は遅延が大きいため外部では使用しない。推定部２２は、カルマンフィルタ処理によって推定した速度を速度差演算部２４に出力する。
【００２３】
速度差演算部２４は、推定部２２において推定した現在の速度と、ΔＴ３前の速度との差を演算する。演算結果である速度差が、前述の第１の速度差に相当する。つまり、速度差演算部２４は、推定部２２において推定した速度の変化を演算することによって、第１の速度差を導出する。速度差演算部２４は、第１の速度差を傾斜角演算部２６に出力する。リングバッファ１８は、第１取得部１４から加速度を受けつける。リングバッファ１８は、ΔＴ３の加速度を記憶する。ΔＴ３過ぎた加速度は、順次最新の加速度に書き換えられる。リングバッファ１８は、加速度積算部２０に加速度を出力する。
【００２４】
加速度積算部２０は、リングバッファ１８からの加速度を受けつける。加速度積算部２０は、ΔＴ３の加速度を積算して、ΔＴ３離れた２時点の速度差を導出する。積算結果である速度差が、前述の第２の速度差に相当する。つまり、加速度積算部２０は、第１取得部１４において取得した加速度を積算することによって、第２の速度差を導出する。加速度積算部２０は、第２の速度差を傾斜角演算部２６に出力する。
【００２５】
傾斜角演算部２６は、速度差演算部２４から第１の速度差を受けつけ、加速度積算部２０から第２の速度差を受けつける。傾斜角演算部２６は、第１の速度差と第２の速度差とをもとに、車両５０の傾斜角を導出する。加速度に関して、次の式が成立する。
【数１１】
2014119393.tif 000013
式（１１）の上式では、運動加速度ａｉと車両５０の傾斜角θｐに応じた重力加速度成分ｇｓｉｎ(θｐ)を加えた加速度（左辺）が、加速度センサ１０で検出される（右辺）ことを表している。ここで、ｋａは加速度センサ出力の単位から加速度単位への変換係数である。加速度センサ出力の単位は、例えば電圧Ｖｏｌｔであり、加速度単位は、例えばｍ／ｓ２である。また、ａＯＵＴは加速度センサ出力、ａＯＦＳは加速度センサ１０のオフセットである。加速度センサ１０のオフセットは、車両５０が水平の場合の加速度センサ出力に相当する。なお、加速度センサ１０は、その感応軸が道路面に対して平行になるように車両５０に取付けられているものとする。
【００２６】
式（１１）の下式では、ｎ−１時点の速度Ｖｎ−１に傾斜角の演算の間隔時間ΔＴ３内の運動加速度ａｉとΔＴ２との乗算の総和を加えたものが、ｎ時点の速度Ｖｎとなることを表している。ｎ時点とｎ−１時点の間隔時間はΔＴ３に等しい。式（１１）を傾斜角θｐに関して整理すると、次のようになる。
【数１２】
2014119393.tif 000014
傾斜角θｐが小さい場合は、式（１２）の下式に変形される。ここで、傾斜角θｐの単位はラジアンである。制御部２８は、傾斜角検出装置１００の動作を制御する。
【００２７】
この構成は、ハードウエア的には、任意のコンピュータのＣＰＵ、メモリ、その他のＬＳＩで実現でき、ソフトウエア的にはメモリにロードされたプログラムなどによって実現されるが、ここではそれらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックがハードウエアのみ、ソフトウエアのみ、またはそれらの組合せによっていろいろな形で実現できることは、当業者には理解されるところである。
【００２８】
以上の構成による傾斜角検出装置１００の動作を説明する。図３は、傾斜角検出装置１００による傾斜角の出力手順を示すフローチャートである。第１取得部１４は、加速度センサ１０からの出力を取得する（Ｓ１０）。第１取得部１４は、加速度センサ出力と加速度センサ１０のオフセットとの差に変換係数ｋａを乗算することによって、加速度を算出する（Ｓ１２）。ここで、距離計３０からフィルタ部３２までの遅延時間と同じになるようにＦＩＦＯ（Ｆｏｒｓｔ Ｉｎ  Ｆｉｒｓｔ  Ｏｕｔ）メモリ等にて、加速度の出力タイミングが調整される。リングバッファ１８は、第１取得部１４から出力された加速度を記憶する（Ｓ１４）。加速度積算部２０は、リングバッファ１８に記憶されているΔＴ３の間の加速度とΔＴ２とを乗算してから積算することによって、第２の速度差を導出する（Ｓ１６）。第３の速度差は、ΔＴ３時間内の速度差に相当する。ただし、第２の速度差には、傾斜角に応じて重力加速度の影響を受けている。
【００２９】
距離計３０は、ΔＴ１の間計数した車速パルスの値に距離係数を乗算することによって、第１時間間隔での距離を算出する（Ｓ１８）。ここで、距離係数は、１パルスあたりの移動距離に相当する。フィルタ部３２は、低域通過フィルタ処理によって移動距離を導出する（Ｓ２０）。推定部２２は、フィルタ部３２から出力されるΔＴ１あたりの移動距離と、第１取得部１４から出力される加速度から速度を推定する（Ｓ２２）。速度差演算部２４は、現在の速度とΔＴ３時間前の速度との差、つまり第１の速度差を導出する（Ｓ２４）。傾斜角演算部２６は、第１の速度差と第２の速度差とをもとに、傾斜角を導出する（Ｓ２６）。
【００３０】
本発明の実施例によれば、車両の速度を推定する処理とは別に傾斜角を導出するので、傾斜角を算出するための遅延を低減できる。また、傾斜角を算出するための遅延が低減されるので、傾斜角が変化する場合においても傾斜角を導出できる。また、距離計において順次算出した第１時間間隔での距離を低域通過フィルタ処理するので、傾斜角の導出精度を向上できる。また、加速度と速度のタイミングを合わせて推定するので、傾斜角の算出精度を向上できる。また、高価な気圧センサを新規に装備することなく、カーナビゲーションシステムならば標準に搭載されているセンサのみから、傾斜角を容易にかつ少ない遅延で導出できる。このことによってコストの増加を抑制できる。また、カルマンフィルタを使用するので、速度を精度よく推定できる。
【００３１】
以上、本発明を実施例をもとに説明した。この実施例は例示であり、それらの各構成要素や各処理プロセスの組合せにいろいろな変形例が可能なこと、またそうした変形例も本発明の範囲にあることは当業者に理解されるところである。
【００３２】
本発明の実施例において、傾斜角検出装置１００は車両５０に搭載されているとしている。しかしながらこれに限らず例えば、傾斜角検出装置１００は歩行者に携帯されてもよい。本変形例によれば、傾斜角検出装置１００をさまざまな製品に適用できる。
【００３３】
本発明の実施例において、推定部２２は、カルマンフィルタ処理を実行することによって、速度を推定している。しかしながらこれに限らず例えば、推定部２２は、カルマンフィルタ処理以外の処理を実行することによって速度を推定してもよい。具体的には、Ｈインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムが使用されればよい。本変形例によれば、推定部２２の構成の自由度を向上できる。
【符号の説明】
【００３４】
１０  加速度センサ、  １２  距離センサ、  １４  第１取得部、  １６  第２取得部、  １８  リングバッファ、  ２０  加速度積算部、  ２２  推定部、  ２４  速度差演算部、  ２６  傾斜角演算部、  ２８  制御部、  ３０  距離計、  ３２  フィルタ部、  １００  傾斜角検出装置。
【図１】
2014119393.tif 000015
【図２】
2014119393.tif 000016
【図３】
2014119393.tif 000017