(57)【特許請求の範囲】
【請求項１】
移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得する第１取得部と、
前記移動体の高度に関する高度情報を取得する第２取得部と、
前記移動体の移動速度に関する速度情報を取得する第３取得部と、
前記第２取得部において取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、前記第３取得部において取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定する判定部と、
前記判定部において第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、前記判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定部と、前記設定部において設定した観測誤差と、前記第１取得部において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理部と、を備える導出部と、
を備えることを特徴とする傾斜角検出装置。
【請求項２】
前記移動体の移動距離に関する距離情報を取得する第４取得部をさらに備え、
前記判定部は、第１条件として、前記第２取得部において取得した高度情報の変化が第１のしきい値よりも小さく、かつ前記第４取得部において取得した距離情報の積算値が第３のしきい値よりも大きいことを判定することを特徴とする請求項１に記載の傾斜角検出装置。
【請求項３】
前記第２取得部は、全地球測位システムの衛星から受信した信号をもとに高度情報を取得し、
前記判定部は、第１条件として、所定の期間にわたって、ＶＤＯＰが所定値以下であり、かつ信号を受信した衛星数が所定数以上の一定数であることを判定することを特徴とする請求項１または２に記載の傾斜角検出装置。
【請求項４】
前記第３取得部において取得した速度情報と所定値とを比較する比較部をさらに備え、
前記処理部は、前記比較部における比較の結果、前記第３取得部において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において前記加速度センサに対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定することを特徴とする請求項１に記載の傾斜角検出装置。
【請求項５】
移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得するステップと、
前記移動体の高度に関する高度情報を取得するステップと、
前記移動体の移動速度に関する速度情報を取得するステップと、
取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、
第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、前記設定ステップにおいて設定した観測誤差と、前記加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、
を備えることを特徴とする傾斜角検出方法。
【請求項６】
移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得するステップと、
前記移動体の高度に関する高度情報を取得するステップと、
前記移動体の移動速度に関する速度情報を取得するステップと、
取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、
第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、前記設定ステップにおいて設定した観測誤差と、前記加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、
を、傾斜角検出装置が備えるコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、傾斜角検出技術に関し、特に車両の傾斜角を検出する傾斜角検出装置、傾斜角検出方法、およびプログラムに関する。
【背景技術】
【０００２】
従来、カーナビゲーション装置は、絶対位置を得ることが可能なＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）と、ジャイロや速度パルス等のセンサから計算される相対位置を得る自立航法を組合わせて、車両位置や進行方位を算出する。自立航法では、道路傾斜を含む車両の姿勢角が考慮されることによって、より高精度な位置を算出することが可能になる。一般的に、車両の姿勢角を算出するために、加速度センサから出力された加速度信号が使用される。加速度センサは、車両に取り付けられる場合に傾斜して取り付けられることが多く、車両は水平であっても、加速度センサ自身の傾斜のために、重力加速度の成分が加速度信号に重畳される。また、温度変化により車両水平時の出力電圧（以下、「オフセット」という）がドリフトすることがある。車両の姿勢角の算出精度を向上させるために、加速度センサのオフセットに対して補正が必要になる。
【０００３】
これまでは、車両等の移動体が停止状態または等速度走行状態にある場合に加速度センサの出力の平均値が導出され、当該平均値をもとに加速度センサ出力のオフセット値が更新されている（例えば、特許文献１参照）。また、ＧＰＳレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および自律航法部が出力する緯度、経度、高さ方向の車両位置、車両速度を用いて、加速度信号オフセットを含んだ１０のパラメータが補正されることもある（例えば、特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１０−３０７０３２号公報
【特許文献２】特開２００８−２１５９１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、停止状態または等速度走行状態にある場合に、車両が必ずしも水平であるとは限らない。したがって、上りまたは下りが連続する傾斜路を走行するときに加速度センサに重力成分が連続して重畳される。その結果、真のオフセットに対して生じる偏りが大きくなりやすい。また、補正値を決めるもとになるＧＰＳレシーバおよび自律航法部の出力には、加速度オフセットに直接関係する出力がなく、かつ出力の数に比べて補正すべきパラメータ数が多い。その結果、パラメータ間での補正配分決定が困難であり、補正精度が悪化してしまうことがある。
【０００６】
本発明はこうした状況に鑑みてなされたものであり、その目的は、傾斜角を高精度に検出する技術を提供することにある。
【課題を解決するための手段】
【０００７】
上記課題を解決するために、本発明のある態様の傾斜角検出装置は、移動体の加速度に関する加速度情報を移動体に搭載された加速度センサから取得する第１取得部と、移動体の高度に関する高度情報を取得する第２取得部と、移動体の移動速度に関する速度情報を取得する第３取得部と、第２取得部において取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、第３取得部において取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定する判定部と、判定部において第１条件と第２条件のいずれも満たしている場合、加速度センサの観測誤差として第１値を設定し、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定部と、設定部において設定した観測誤差と、第１取得部において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサのオフセットと移動体の傾斜角を導出する処理部と、を備える導出部と、を備える。
【０００８】
この態様によると、高度情報の変化が小さく、かつ速度情報の変化が小さいか否かに応じて、処理内容を変更して移動体の傾斜角を導出するので、移動体の状況に応じて傾斜角を高精度に検出できる。この場合、第１値よりも第２値を大きくするので、移動体が低速で上りまたは下りが連続する傾斜路を走行するときに、加速度センサに重力成分が連続して重畳され、真のオフセットに対して偏りが生ずることによる傾斜角の検出精度の悪化を抑制できる。
【０００９】
移動体の移動距離に関する距離情報を取得する第４取得部をさらに備えてもよい。判定部は、第１条件として、第２取得部において取得した高度情報の変化が第１のしきい値よりも小さく、かつ第４取得部において取得した距離情報の積算値が第３のしきい値よりも大きいことを判定してもよい。この場合、移動体が水平に近い状態であるかを判定するために、距離情報の積算値が大きいかを判定するので、移動体が静止している状態を除外できる。
【００１０】
第２取得部は、全地球測位システムの衛星から受信した信号をもとに高度情報を取得し、判定部は、第１条件として、所定の期間にわたって、ＶＤＯＰ（Ｖｅｒｔｉｃａｌ  Ｄｉｌｕｔｉｏｎ  ｏｆ  Ｐｒｅｃｉｓｉｏｎ）が所定値以下であり、かつ信号を受信した衛星数が所定数以上の一定数であることを判定してもよい。この場合、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定するので、移動体が水平に近い状態であるかの判定精度を向上できる。
【００１３】
第３取得部において取得した速度情報と所定値とを比較する比較部をさらに備えてもよい。処理部は、比較部における比較の結果、第３取得部において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において加速度センサに対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定してもよい。この場合、速度情報が小さければ、加速度センサに対するカルマンゲインを所定の値に設定するので、精度の悪化を抑制しながら、処理量を低減できる。
【００１４】
本発明の別の態様は、傾斜角検出方法である。この方法は、移動体の加速度に関する加速度情報を移動体に搭載された加速度センサから取得するステップと、移動体の高度に関する高度情報を取得するステップと、移動体の移動速度に関する速度情報を取得するステップと、取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、第１条件と第２条件のいずれも満たしている場合、加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、設定ステップにおいて設定した観測誤差と、加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサのオフセットと移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、を備える。
【００１５】
なお、以上の構成要素の任意の組合せ、本発明の表現を方法、装置、システム、記録媒体、コンピュータプログラムなどの間で変換したものもまた、本発明の態様として有効である。
【発明の効果】
【００１６】
本発明によれば、傾斜角を高精度に検出できる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施例１に係る傾斜角検出装置の構成を示す図である。
【図２】図１の傾斜角検出装置が搭載された車両の動作状態を示す図である。
【図３】図１のリングバッファに記憶される情報が取得される状況を示す図である。
【図４】図４（ａ）−（ｂ）は、図１のリングバッファに記憶されるデータの構造を示す図である。
【図５】図１の傾斜角検出装置による傾斜角の出力手順を示すフローチャートである。
【図６】図１の傾斜角検出装置による第１条件の判定手順を示すフローチャートである。
【図７】本発明の実施例２に係る傾斜角検出装置の構成を示す図である。
【図８】図７の傾斜角検出装置による傾斜角の出力手順を示すフローチャートである。
【発明を実施するための形態】
【００１８】
（実施例１）
本発明を具体的に説明する前に、まず概要を述べる。本発明の実施例１は、カーナビゲーション装置での自立航法において、車両の傾斜角を検出する傾斜角検出装置に関する。実施例１に係る傾斜角検出装置は、加速度センサのオフセットを高精度に検出して、傾斜角も高精度に検出するために、次の処理を実行する。傾斜角検出装置は、高度計から高度情報を取得し、距離センサから距離情報を取得し、速度センサから速度情報を取得し、加速度センサから加速度情報を取得する。傾斜角検出装置は、高度情報と距離情報とをもとに水平に近い状態であるかという第１条件を判定するとともに、速度情報をもとに等速度に近い状態であるかという第２条件を判定する。
【００１９】
第１条件と第２条件とのいずれもが満たされていれば、傾斜角検出装置は、加速度情報をもとに加速度センサのオフセットを更新してから、加速度情報とオフセットとをもとに車両の傾斜角を算出する。一方、第１条件と第２条件との少なくともいずれかが満たされていなければ、傾斜角検出装置は、オフセットを新たに更新せずに、加速度情報とすでに更新したオフセットとをもとに車両の傾斜角を算出する。このように水平に近く、かつ運動加速度が小さいタイミングにおいて、加速度センサのオフセットを補正するので、傾斜路でも加速度センサのオフセットの偏りが小さくされる。このように加速センサのオフセットの偏りが小さくなるので、傾斜角が高精度に補正される。
【００２０】
図１は、本発明の実施例１に係る傾斜角検出装置１００の構成を示す。傾斜角検出装置１００は、高度計１０、距離センサ１２、速度センサ１４、加速度センサ１６、第２取得部１８、第４取得部２０、第３取得部２２、第１取得部２４、リングバッファ２６、判定部２８、導出部３０、制御部３２を含む。また、判定部２８は、第１条件判定部３４、第２条件判定部３６を含み、導出部３０は、オフセット補正部３８、傾斜角演算部４０を含む。傾斜角検出装置１００は、図示しない車両に搭載される。
【００２１】
高度計１０は、気圧をもとに車両の高度を検出する。また、高度計１０は、ＧＰＳ衛星から受信される信号をもとに車両の高度を検出してもよい。高度計１０は、検出した高度に関する情報（以下、「高度情報」という）を第２取得部１８へ出力する。距離センサ１２は、車両の移動距離を検出する。例えば、距離センサ１２は、所定の期間の速度パルスを加算することによって移動距離を導出する。距離センサ１２は、検出した距離に関する情報（以下、「距離情報」という）を第４取得部２０へ出力する。
【００２２】
速度センサ１４は、車両の移動速度を検出する。例えば、移動速度は、単位時間あたりの速度パルスを加算することによって導出される。具体的に説明すると、速度センサ１４は、ドライブシャフトの回転に対応して回転するスピードメータケーブルの中間に設置され、ドライブシャフトの回転に伴った速度パルスを検出した後、車速パルスの単位時間あたりのパルス数を計測することにより移動速度を取得する。ここで、車速パルスは、所定の距離ごとに発生させたパルスに相当する。なお、速度センサ１４は、ＧＰＳから移動速度を直接取得してもよい。速度センサ１４は、検出した移動速度に関する情報（以下、「速度情報」という）を第３取得部２２へ出力する。
【００２３】
加速度センサ１６は、車両の主たる移動方向、例えば、車両の前後方向に生じる加速度を検出する。ここでは、加速度を説明するために図２を使用する。図２は、傾斜角検出装置１００が搭載された車両５０の動作状態を示す。車両５０は、図１の傾斜角検出装置１００を搭載する。また、車両５０は、道路面５４上を移動する。ここで、車両５０に搭載された加速度センサ１６（図示せず）の感応軸５６は、道路面５４に平行であり、かつ車両５０の前後方向と一致するものとする。また、感応軸５６および道路面５４は、水平面５２に対して傾斜角θだけ傾いている。加速度においては、車両５０の移動加速度と、感応軸５６が水平面５２となす傾斜角θに応じた重力成分とが重畳されている。図１に戻る。加速度センサ１６は、検出した加速度に関する情報（以下、「加速度情報」という）を第１取得部２４へ出力する。
【００２４】
第２取得部１８は、高度計１０に接続され、高度計１０からの高度情報を取得する。なお、高度計１０がＧＰＳ衛星から受信した信号をもとに高度を取得している場合、第２取得部１８は、高度情報に加えて、ＶＤＯＰ（Ｖｅｒｔｉｃａｌ  Ｄｉｌｕｔｉｏｎ  ｏｆ  Ｐｒｅｃｉｓｉｏｎ）や受信衛星数を取得してもよい。ＶＤＯＰは、ＧＰＳの高度測位精度の指標である。なお、ＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ  ＤＯＰ）等のＤＯＰが含まれていてもよい。第４取得部２０は、距離センサ１２に接続され、距離センサ１２からの距離情報を取得する。第３取得部２２は、速度センサ１４に接続され、速度センサ１４からの速度情報を取得する。第１取得部２４は、加速度センサ１６に接続され、加速度センサ１６からの加速度情報を取得する。
【００２５】
リングバッファ２６は、第２取得部１８、第４取得部２０、第３取得部２２、第１取得部２４のそれぞれにおいて取得した各情報を一時的に記憶する。なお、リングバッファ２６は、各情報を定期的に取得しているが、その際、同一タイミングに相当する高度情報、距離情報、速度情報、加速度情報とが組み合わされて記憶される。ここで、リングバッファ２６は、所定数の組合せを記憶しており、最も古いタイミングの情報に最新の情報を上書きして記憶する。図３は、リングバッファ２６に記憶される情報が取得される状況を示す。図３は、図２の車両５０が移動している軌跡を上空から見下ろしている場合を示す。ここで、車両５０は、Ｓ地点から移動を開始して、Ａ地点、Ｂ地点、Ｃ地点、Ｄ地点、Ｅ地点の順に移動している。現在、車両５０は、Ｅ地点にいる。
【００２６】
図４（ａ）−（ｂ）は、リングバッファ２６に記憶されるデータの構造を示す。図４（ａ）のごとく、地点名欄２００、アドレス欄２０２、高度欄２０４、距離欄２０６、速度欄２０８、加速度欄２１０が含まれる。地点名欄２００は、図３に示された地点との対応を明確にするために示されている。実際には地点名欄２００が含まれてていなくてもよい。前述の組合せは、高度欄２０４、距離欄２０６、速度欄２０８、加速度欄２１０に示された情報のうち、ひとつの行に含まれるものに相当する。ここで、リングバッファ２６に記憶される組合せの所定数は「５」であるとされる。アドレス欄２０２には、情報を記憶すべきアドレスが示されている。図４（ｂ）には、書込みポインタと読出しポインタとが示されており、これらは、情報を書き込むべきアドレスと、情報を読み出すアドレスとに対応する。図１に戻る。
【００２７】
第１条件判定部３４は、リングバッファ２６から高度情報を取得するとともに、距離情報を取得する。第１条件判定部３４は、リングバッファ２６に記憶された高度情報の最大値から最小値を減算することによって、高度の範囲Ｒｈを導出する。これは、第２取得部１８において取得した高度情報の変化に相当する。具体的には、次のように、５地点の高度ｈ１〜ｈ５の最大値と最小値とを選択し、最大値から最小値が減算される。
【数１】
0005569472.tif 000002
第１条件判定部３４は、リングバッファ２６に記憶された距離情報を加算することによって移動距離Ｓを導出する。これは、第４取得部２０において取得した距離情報の積算値に相当する。具体的には、次のように、Ａ地点からＥ地点までの距離情報、ｓ２からｓ５が加算される。
【００２８】
【数２】
0005569472.tif 000003
第１条件判定部３４は、高度の範囲Ｒｈが第１のしきい値よりも小さくなり、かつ移動距離Ｓが第３のしきい値よりも大きくなる場合に、第１条件を満たすと判定する。これは、車両５０が水平に近い状態であることを検出することに相当する。このように、高度の範囲とともに移動距離を第１条件に加えることによって、停止や非常に速度の遅い状態を水平に近いと判定することが抑制される。なお、処理を簡易にするために、第１条件として、高度の範囲Ｒｈに対する判定のみがなされてもよい。
【００２９】
ここで、ＧＰＳ衛星から取得される高度情報を使用する場合、第１条件に対して以下の判定が追加されてもよい。第１条件判定部３４は、図４（ｂ）の読出しポインタを「１」に設定し、読出しポインタで指定されたアドレスの情報を読み出す。第１条件判定部３４は、ＧＰＳのＶＤＯＰが所定値以下であり、ＧＰＳの受信衛星数が所定の値以上であり、ＧＰＳの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じであるかを確認する。これらの条件をすべて満たす場合、第１条件判定部３４は、読出しポインタを加算し、最後の情報になるまで以上の処理を繰り返し実行する。
【００３０】
これらの条件を満たす場合に、第１条件判定部３４は、前述の高度の範囲Ｒｈ、移動距離Ｓに対する判定を実行する。これらの条件を満たさない場合、第１条件判定部３４は、第１条件を満たさないと判定する。つまり、第１条件判定部３４は、第１条件として、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定してもよい。
【００３１】
第２条件判定部３６は、リングバッファ２６から速度情報を取得する。第２条件判定部３６は、隣接した地点間での速度変化ΔＶｉを導出する。ここで、ｉは２〜５である。
【数３】
0005569472.tif 000004
第２条件判定部３６は、速度変化ΔＶｉのすべてが第２のしきい値よりも小さくなる場合に、第２条件を満たすと判定する。これは、車両５０が等速走行に近い状態であることを検出することに相当する。
【００３２】
判定部２８は、第１条件と第２条件とのいずれも満たしているか否かを判定する。いずれも満たしている場合、判定部２８は、オフセット補正部３８に対してオフセットを更新させてから、傾斜角演算部４０に傾斜角を導出させる。一方、少なくともいずれか一方を満たしていない場合、判定部２８は、オフセット補正部３８に対してオフセットを更新させずに、傾斜角演算部４０に傾斜角を導出させる。
【００３３】
オフセット補正部３８は、判定部２８において第１条件と第２条件のいずれも満たしている場合、リングバッファ２６に記憶された加速度情報をもとに加速度センサ１６のオフセットを更新する。具体的に説明すると、オフセット補正部３８は、リングバッファ２６に記憶された領域の中心、つまり図４（ｂ）の読出しポインタとして設定されているアドレスに含まれた加速度情報をもとに、加速度オフセットを更新する。ここで、図４（ｂ）の読出しポインタとして設定されているアドレスに含まれた加速度情報は、図３のＣ地点の加速度情報ａ３に相当する。
【数４】
0005569472.tif 000005
【００３４】
なお、ａＯＦＳ１は更新後のオフセット、ａＯＦＳ０は更新前のオフセット、Ｋは０〜１の値をとる係数である。Ｋは、例えば０．８である。Ｋを大きくすれば更新前のオフセットの重みが大きく、Ｋを小さくすれば加速度情報ａ３の重みが大きくなる。そのため、Ｋの調節によって、更新の度合いが調節される。一方、オフセット補正部３８は、第１条件と第２条件の少なくともいずれかを満たしていない場合、オフセット補正部３８は、加速度センサ１６のオフセットの更新をスキップする。
【００３５】
傾斜角演算部４０は、判定部２８において第１条件と第２条件のいずれも満たしている場合、速度センサ１４からの速度情報、加速度センサ１６からの加速度情報、オフセット補正部３８にて更新したオフセットとを入力する。傾斜角演算部４０は、速度情報、加速度情報、更新したオフセットとをもとに、車両５０の傾斜角θを次のように導出する。
【数５】
0005569472.tif 000006
ここで、ｇは重力加速度であり、ａｍは移動加速度であり、ａＯＵＴは加速度情報、ａＯＦＳ１は更新後のオフセット、ｋａは加速度情報の単位を電圧Ｖｏｌｔから加速度ｍ／ｓ２へ変換するための係数である。また、移動加速度ａｍは、速度情報の単位時間の変化として導出される。なお、θの単位はラジアンであり、θの値が小さい場合の式である。
【００３６】
傾斜角演算部４０は、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、オフセット補正部３８において更新したオフセットの代わりに、すでに更新されていたオフセットを使用して、前述のごとく車両５０の傾斜角θを導出する。このように、導出部３０は、判定部２８において第１条件と第２条件のいずれも満たしているか否かに応じて、処理内容を変更しながら、第１取得部２４において取得した加速度情報をもとに、移動体の傾斜角θを導出する。制御部３２は、傾斜角検出装置１００での動作タイミングを制御する。
【００３７】
この構成は、ハードウエア的には、任意のコンピュータのＣＰＵ、メモリ、その他のＬＳＩで実現でき、ソフトウエア的にはメモリにロードされたプログラムなどによって実現されるが、ここではそれらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックがハードウエアのみ、ソフトウエアのみ、またはそれらの組合せによっていろいろな形で実現できることは、当業者には理解されるところである。
【００３８】
以上の構成による傾斜角検出装置１００の動作を説明する。図５は、傾斜角検出装置１００による傾斜角の出力手順を示すフローチャートである。第２取得部１８は、高度計１０から高度情報を取得する（Ｓ１０）。第４取得部２０は、距離センサ１２から距離情報を取得する（Ｓ１２）。第３取得部２２は、速度センサ１４から速度情報を取得する（Ｓ１４）。第１取得部２４は、加速度センサ１６から加速度情報を取得する（Ｓ１６）。リングバッファ２６は、これらの情報を記憶する（Ｓ１８）。第１条件判定部３４は、高度の範囲を計算し（Ｓ２０）、移動距離を計算する（Ｓ２２）。第１条件判定部３４において第１条件を満たしている場合（Ｓ２４のＹ）、第２条件判定部３６は、速度変化を計算する（Ｓ２６）。
【００３９】
第２条件判定部３６において第２条件を満たしている場合（Ｓ２８のＹ）、オフセット補正部３８は、加速度情報からオフセットを更新する（Ｓ３０）。一方、第１条件判定部３４において第１条件を満たしていない場合（Ｓ２４のＮ）、あるいは第２条件判定部３６において第２条件を満たしていない場合（Ｓ２８のＮ）、ステップ３２へスキップされる。傾斜角演算部４０は、傾斜角を計算し（Ｓ３２）、傾斜角を出力する（Ｓ３４）。処理を継続する場合（Ｓ３６のＹ）、リングバッファ２６の書込みポインタと読出しポインタを進め（Ｓ３８）、ステップ１０に戻る。書込みポインタが５、読出しポインタが３の場合、ポインタを進めるとそれぞれ１と４となる。処理を継続しない場合（Ｓ３６のＮ）、処理は終了される。
【００４０】
図６は、傾斜角検出装置１００による第１条件の判定手順を示すフローチャートである。これは、図５のステップ２４の処理に相当する。第１条件判定部３４は、読出しポインタを１に設定する（Ｓ５０）。第１条件判定部３４は、読出しポインタで指定されたアドレスの情報を読み出す（Ｓ５２）。ＧＰＳのＶＤＯＰが所定の値以下であり（Ｓ５４のＹ）、ＧＳＰの受信衛星数が所定の数以上であり（Ｓ５６のＹ）、ＧＳＰの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じであれば（Ｓ５８のＹ）、最後の情報であるかが確認される（Ｓ６０）。最後の情報でなければ（Ｓ６０のＮ）、読出しポインタに１が加算されて（Ｓ６２）、ステップ５２に戻る。
【００４１】
最後の情報である場合（Ｓ６０のＹ）、高度の範囲が第１のしきい値よりも小さく（Ｓ６４のＹ）、移動距離の範囲が第３のしきい値よりも小さければ（Ｓ６６のＹ）、第１条件判定部３４は、第１条件を満たすと判定する（Ｓ６８）。一方、ＧＰＳのＶＤＯＰが所定の値以下でない場合（Ｓ５４のＮ）、あるいはＧＳＰの受信衛星数が所定の数以上でない場合（Ｓ５６のＮ）、あるいはＧＳＰの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じでない場合（Ｓ５８のＮ）、あるいは高度の範囲が第１のしきい値よりも小さくない場合（Ｓ６４のＮ）、あるいは移動距離の範囲が第３のしきい値よりも小さくない場合（Ｓ６６のＮ）、第１条件判定部３４は、第１条件を満たさないと判定する（Ｓ７０）。
【００４２】
本発明の実施例によれば、水平に近く、かつ等速度移動に近いか否かに応じて、処理内容を変更して車両の傾斜角を導出するので、車両の状況に応じて傾斜角を高精度に検出できる。また、水平に近く、かつ等速度移動に近い場合、オフセット値を更新するので、オフセット値の精度を向上できる。また、オフセット値の精度が向上するので、傾斜角の検出精度を向上できる。また、水平に近くない場合や、等速度移動に近くない場合に、オフセット値を更新しないので、オフセット値の精度を維持できる。また、水平に近くない場合や、等速度移動に近くない場合に、オフセット値を更新せずに、傾斜角を導出するので、最新の傾斜角を検出できる。また、車両が水平に近い状態であるかを判定するために、距離情報の積算値が大きいかを判定するので、車両が静止している状態を除外できる。また、車両が静止している状態が除外されるので、車両が水平に近い状態であるかの判定精度を向上できる。
【００４３】
また、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定するので、移動体が水平に近い状態であるかの判定精度を向上できる。また、水平に近いことを検出すればよいので、カーナビゲーションシステムならば標準に搭載されているＧＰＳ等を高度計として使用することができる。また、カーナビゲーションシステムならば標準に搭載されているＧＰＳ等が高度計として使用されるので、高価なセンサを新規に装備することを不要にできる。また、高価なセンサを新規に装備することなく、カーナビゲーションシステムならば標準に搭載されているセンサのみから、加速度センサのオフセットを容易に推定できるので、精度の高い傾斜角検出装置を容易に実現できる。
【００４４】
（実施例２）
本発明の実施例２は、実施例１と同様に、傾斜角を検出するための傾斜角検出装置に関する。実施例１における傾斜角検出装置は、加速度センサのオフセットおよび傾斜角を状態変数として設定し、カルマンフィルタ処理を実行することによって、オフセットおよび傾斜角を更新する。ここで、傾斜角検出装置は、第１条件と第２条件とのいずれも満たしている場合、観測誤差として第１値を設定して、カルマンフィルタ処理に使用し、第１条件と第２条件との少なくともいずれかを満たしていない場合、観測誤差として第２を設定して、カルマンフィルタ処理を実行する。ここで、第２値は第１値よりも大きな値として規定されている。
【００４５】
図７は、本発明の実施例２に係る傾斜角検出装置１００の構成を示す。傾斜角検出装置１００は、図１の傾斜角検出装置１００と比較して、導出部３０を含まず、導出部７０を含む。導出部７０は、設定部７２、処理部７４を含む。ここでは差異を中心に説明する。
【００４６】
設定部７２は、判定部２８において第１条件と第２条件のいずれも満たしている場合、加速度センサ１６の観測誤差δａｎとして第１値を設定する。一方、設定部７２は、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサ１６の観測誤差δａｎとして第２値を設定する。前述のごとく、第２値は、大きな値として設定する。設定部７２は、設定した観測誤差を処理部７４に使用させる。
【００４７】
処理部７４は、第１取得部２４からの加速度情報ａＯＵＴを入力する。なお、タイミングｎでの加速度情報は、ａＯＵＴｎと示されるものとする。ここでは、タイミングｎでの状態変数ｘｎを定義する。状態変数ｘｎは、次のように、速度ｖｎ、傾斜角θｎ、加速度のオフセットａＯＦＳｎの３要素を含む。
【数６】
0005569472.tif 000007
ここで、上付きの添え字Ｔは、ベクトルまたは行列の転置を示す。前述のごとく、下付きの添え字ｎはタイミングｎを示す。
【００４８】
処理部７４は、入力ｕｎと、１回前の状態変数、つまりタイミングｎ−１での状態変数ｘｎ−１とをもとに、次に示された状態方程式を計算することによって、タイミングｎでの推定値ｘｎ’を演算する。なお、状態変数ｘｎ−１は、すでに導出された後、保持されている。
【数７】
0005569472.tif 000008
ここで、入力ｕｎは、加速度情報ａＯＵＴｎである。
【数８】
0005569472.tif 000009
【００４９】
また、式（７）におけるシステム行列Ａと入力行列Ｂとは、式（９）および式（１０）のように示される。
【数９】
0005569472.tif 000010
【数１０】
0005569472.tif 000011
ここで、ｇは重力加速度、ｋは加速度情報から加速度単位（例えばｍ／ｓ２）への変換係数、ΔＴはサンプリング間隔を示す。前述のごとく、加速度センサの感応軸は、車両５０の前後方向と一致するものとする。
【００５０】
処理部７４は、速度情報ｖｎ、加速度情報ａＯＵＴｎとを構成要素として、観測値ｚｎを規定する。処理部７４は、次のように、推定値ｘｎ’から抜き出された状態変数の速度と加速度のオフセットとに対して、観測値ｚｎとの観測残差ｙｎを導出する。
【数１１】
0005569472.tif 000012
ここで、出力行列Ｃは次のように示される。
【数１２】
0005569472.tif 000013
【００５１】
処理部７４は、設定部７２で設定された観測誤差δａｎ、速度センサ１４の観測誤差δｖｎとを入力し、これらの観測誤差が標準偏差を表すものとして、共分散行列Ｒｎを次のように導出する。
【数１３】
0005569472.tif 000014
なお、速度センサ１４の観測誤差δｖｎは一定の値として設定される。処理部７４は、共分散行列Ｒｎとシステム誤差とをもとに、カルマンフィルタのゲインＫｎを演算する。ここで、共分散行列Ｒ等からカルマンフィルタのゲインの導出には、公知の技術が使用されればよい。公知の技術は、例えば、ＭＯＨＩＮＤＥＲ  Ｓ．  ＧＲＥＷＡＬ、ＡＮＧＵＳ  Ｐ．  ＡＮＤＲＥＷＳ著「Ｋａｌｍａｎ  Ｆｉｌｔｅｒｉｎｇ：Ｔｈｅｏｒｙ  ａｎｄ  Ｐｒａｃｔｉｃｅ  Ｕｓｉｎｇ  ＭＡＴＬＡＢ」、Ａ  Ｗｉｌｅｙ−Ｉｎｔｅｒｓｃｉｅｎｃｅ  Ｐｕｂｌｉｃａｔｉｏｎの１１６頁から１２０頁に示されている。処理部７４は、次のように、６つの構成要素によって形成されるゲインを導出する。
【数１４】
0005569472.tif 000015
【００５２】
処理部７４は、推定値ｘｎ’、ゲインＫｎ、観測残差ｙｎをもとに、式（１５）を演算することによって、状態変数ｘｎを更新する。
【数１５】
0005569472.tif 000016
ゲインＫｎが小さければ、推定値ｘｎ’の重みが大きく、ゲインＫｎが大きければ、観測残差ｙｎの重みが大きい。なお、式（１５）は、ｘｎ’＋Ｋｎｙｎを新たにｘｎに代入することを表している。以上の処理は、設定部７２において設定した観測誤差と、第１取得部２４において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサ１６のオフセットと車両５０の傾斜角θを導出することに相当する。
【００５３】
図８は、傾斜角検出装置１００による傾斜角の出力手順を示すフローチャートである。第２取得部１８は、高度計１０から高度情報を取得する（Ｓ１００）。第４取得部２０は、距離センサ１２から距離情報を取得する（Ｓ１０２）。第３取得部２２は、速度センサ１４から速度情報を取得する（Ｓ１０４）。第１取得部２４は、加速度センサ１６から加速度情報を取得する（Ｓ１０６）。リングバッファ２６は、これらの情報を記憶する（Ｓ１０８）。第１条件判定部３４は、高度の範囲を計算し（Ｓ１１０）、移動距離を計算する（Ｓ１１２）。第１条件判定部３４において第１条件を満たしている場合（Ｓ１１４のＹ）、第２条件判定部３６は、速度変化を計算する（Ｓ１１６）。
【００５４】
第２条件判定部３６において第２条件を満たしている場合（Ｓ１１８のＹ）、設定部７２は、観測誤差として第１値を設定する（Ｓ１２０）。一方、第１条件判定部３４において第１条件を満たしていない場合（Ｓ１１４のＮ）、あるいは第２条件判定部３６において第２条件を満たしていない場合（Ｓ１１８のＮ）、設定部７２は、観測誤差として第２値を設定する（Ｓ１２２）。処理部７４は、カルマンフィルタ処理を実行し（Ｓ１２４）、傾斜角を出力する（Ｓ１２６）。処理を継続する場合（Ｓ１２８のＹ）、リングバッファ２６の書込みポインタと読出しポインタを進め（Ｓ１３０）、ステップ１００に戻る。処理を継続しない場合（Ｓ１２８のＮ）、処理は終了される。
【００５５】
本発明の実施例によれば、カルマンフィルタを使用するので、加速度センサのオフセットと傾斜角とを同時に推定できる。また、観測誤差として第１値よりも第２値を大きくするので、車両が低速で上りまたは下りが連続する傾斜路を走行するときに、加速度センサに重力成分が連続して重畳され、真のオフセットに対して偏りが生ずることによる傾斜角の検出精度の悪化を抑制できる。また、速度情報が小さければ、加速度センサに対するカルマンゲインを所定の値に設定するので、精度の悪化を抑制しながら、処理量を低減できる。
【００５６】
以上、本発明を実施例をもとに説明した。この実施例は例示であり、それらの各構成要素や各処理プロセスの組合せにいろいろな変形例が可能なこと、またそうした変形例も本発明の範囲にあることは当業者に理解されるところである。
【００５７】
本発明の実施例１または２において、傾斜角検出装置１００は車両５０に搭載されているとしている。しかしながらこれに限らず例えば、傾斜角検出装置１００は歩行者に携帯されてもよい。本変形例によれば、傾斜角検出装置１００をさまざまな製品に適用できる。
【００５８】
本発明の実施例２において、傾斜角検出装置は、速度情報の値にかかわらず、カルマンフィルタのゲインを導出し、カルマンフィルタ処理を実行する。速度情報の値が小さい場合、一部のゲインはある程度小さな値になる。そのようなゲインの値が小さければ、ゲインの乗算結果も小さくなるので、ゲインの精度があまり重要ではなくなる。そのため、処理量を低減することを目的として、処理部７４は、第３取得部２２において取得した速度情報と所定値とを比較してもよい。
【００５９】
その後、処理部７４は、比較の結果、第３取得部２２において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において加速度センサ１６に対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定する。予め定めた値は、例えば所定の固定値である。加速度情報に対するゲインは、式（１４）におけるゲインＫ３２に相当する。ゲインＫ３２はゼロまたは小さな値に設定される。本変形例によれば、速度情報が小さければ、加速度情報に対するゲインを所定の値に設定するので、処理量を低減できる。また、速度情報が小さければ、加速度情報に対するゲインがゼロに近くなるべきなので、所定の値にゲインを設定しても、傾斜角の検出精度の悪化を抑制できる。
【００６０】
本発明の実施例２において、設定部７２が設定すべき観測誤差として、第１値と第２値の２種類が規定されている。しかしながらこれに限らず例えば、観測誤差として３種類以上の値が規定されていてもよい。その場合、設定部７２は、水平への近さの程度や等速度移動への近さの程度に応じて、観測誤差の値を設定する。具体的には、水平や等速度移動に近くなるほど、観測誤差が小さくなるように設定される。本変形例によれば、観測誤差を詳細に設定できる。
【符号の説明】
【００６１】
１０  高度計、  １２  距離センサ、  １４  速度センサ、  １６  加速度センサ、  １８  第２取得部、  ２０  第４取得部、  ２２  第３取得部、  ２４  第１取得部、  ２６  リングバッファ、  ２８  判定部、  ３０  導出部、  ３２  制御部、  ３４  第１条件判定部、  ３６  第２条件判定部、  ３８  オフセット補正部、  ４０  傾斜角演算部、  ５０  車両、  ５２  水平面、  ５４  道路面、  ５６  感応軸、  ７０  導出部、  ７２  設定部、  ７４  処理部、  １００  傾斜角検出装置。
【図１】
0005569472.tif 000017
【図２】
0005569472.tif 000018
【図３】
0005569472.tif 000019
【図４】
0005569472.tif 000020
【図５】
0005569472.tif 000021
【図６】
0005569472.tif 000022
【図７】
0005569472.tif 000023
【図８】
0005569472.tif 000024