JP5569472B2

JP5569472B2 - 傾斜角検出装置、傾斜角検出方法、およびプログラム

Info

Publication number: JP5569472B2
Application number: JP2011122773A
Authority: JP
Inventors: 裕章 ▲高▼梨
Original assignee: JVCKenwood Corp
Current assignee: JVCKenwood Corp
Priority date: 2011-05-31
Filing date: 2011-05-31
Publication date: 2014-08-13
Anticipated expiration: 2031-05-31
Also published as: JP2012251798A

Description

本発明は、傾斜角検出技術に関し、特に車両の傾斜角を検出する傾斜角検出装置、傾斜角検出方法、およびプログラムに関する。

従来、カーナビゲーション装置は、絶対位置を得ることが可能なＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）と、ジャイロや速度パルス等のセンサから計算される相対位置を得る自立航法を組合わせて、車両位置や進行方位を算出する。自立航法では、道路傾斜を含む車両の姿勢角が考慮されることによって、より高精度な位置を算出することが可能になる。一般的に、車両の姿勢角を算出するために、加速度センサから出力された加速度信号が使用される。加速度センサは、車両に取り付けられる場合に傾斜して取り付けられることが多く、車両は水平であっても、加速度センサ自身の傾斜のために、重力加速度の成分が加速度信号に重畳される。また、温度変化により車両水平時の出力電圧（以下、「オフセット」という）がドリフトすることがある。車両の姿勢角の算出精度を向上させるために、加速度センサのオフセットに対して補正が必要になる。

これまでは、車両等の移動体が停止状態または等速度走行状態にある場合に加速度センサの出力の平均値が導出され、当該平均値をもとに加速度センサ出力のオフセット値が更新されている（例えば、特許文献１参照）。また、ＧＰＳレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および自律航法部が出力する緯度、経度、高さ方向の車両位置、車両速度を用いて、加速度信号オフセットを含んだ１０のパラメータが補正されることもある（例えば、特許文献２参照）。

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

しかしながら、停止状態または等速度走行状態にある場合に、車両が必ずしも水平であるとは限らない。したがって、上りまたは下りが連続する傾斜路を走行するときに加速度センサに重力成分が連続して重畳される。その結果、真のオフセットに対して生じる偏りが大きくなりやすい。また、補正値を決めるもとになるＧＰＳレシーバおよび自律航法部の出力には、加速度オフセットに直接関係する出力がなく、かつ出力の数に比べて補正すべきパラメータ数が多い。その結果、パラメータ間での補正配分決定が困難であり、補正精度が悪化してしまうことがある。

本発明はこうした状況に鑑みてなされたものであり、その目的は、傾斜角を高精度に検出する技術を提供することにある。

上記課題を解決するために、本発明のある態様の傾斜角検出装置は、移動体の加速度に関する加速度情報を移動体に搭載された加速度センサから取得する第１取得部と、移動体の高度に関する高度情報を取得する第２取得部と、移動体の移動速度に関する速度情報を取得する第３取得部と、第２取得部において取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、第３取得部において取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定する判定部と、判定部において第１条件と第２条件のいずれも満たしている場合、加速度センサの観測誤差として第１値を設定し、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定部と、設定部において設定した観測誤差と、第１取得部において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサのオフセットと移動体の傾斜角を導出する処理部と、を備える導出部と、を備える。

この態様によると、高度情報の変化が小さく、かつ速度情報の変化が小さいか否かに応じて、処理内容を変更して移動体の傾斜角を導出するので、移動体の状況に応じて傾斜角を高精度に検出できる。この場合、第１値よりも第２値を大きくするので、移動体が低速で上りまたは下りが連続する傾斜路を走行するときに、加速度センサに重力成分が連続して重畳され、真のオフセットに対して偏りが生ずることによる傾斜角の検出精度の悪化を抑制できる。

移動体の移動距離に関する距離情報を取得する第４取得部をさらに備えてもよい。判定部は、第１条件として、第２取得部において取得した高度情報の変化が第１のしきい値よりも小さく、かつ第４取得部において取得した距離情報の積算値が第３のしきい値よりも大きいことを判定してもよい。この場合、移動体が水平に近い状態であるかを判定するために、距離情報の積算値が大きいかを判定するので、移動体が静止している状態を除外できる。

第２取得部は、全地球測位システムの衛星から受信した信号をもとに高度情報を取得し、判定部は、第１条件として、所定の期間にわたって、ＶＤＯＰ（ＶｅｒｔｉｃａｌＤｉｌｕｔｉｏｎｏｆＰｒｅｃｉｓｉｏｎ）が所定値以下であり、かつ信号を受信した衛星数が所定数以上の一定数であることを判定してもよい。この場合、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定するので、移動体が水平に近い状態であるかの判定精度を向上できる。

第３取得部において取得した速度情報と所定値とを比較する比較部をさらに備えてもよい。処理部は、比較部における比較の結果、第３取得部において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において加速度センサに対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定してもよい。この場合、速度情報が小さければ、加速度センサに対するカルマンゲインを所定の値に設定するので、精度の悪化を抑制しながら、処理量を低減できる。

本発明の別の態様は、傾斜角検出方法である。この方法は、移動体の加速度に関する加速度情報を移動体に搭載された加速度センサから取得するステップと、移動体の高度に関する高度情報を取得するステップと、移動体の移動速度に関する速度情報を取得するステップと、取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、第１条件と第２条件のいずれも満たしている場合、加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、設定ステップにおいて設定した観測誤差と、加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサのオフセットと移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、を備える。

なお、以上の構成要素の任意の組合せ、本発明の表現を方法、装置、システム、記録媒体、コンピュータプログラムなどの間で変換したものもまた、本発明の態様として有効である。

本発明によれば、傾斜角を高精度に検出できる。

本発明の実施例１に係る傾斜角検出装置の構成を示す図である。図１の傾斜角検出装置が搭載された車両の動作状態を示す図である。図１のリングバッファに記憶される情報が取得される状況を示す図である。図４（ａ）−（ｂ）は、図１のリングバッファに記憶されるデータの構造を示す図である。図１の傾斜角検出装置による傾斜角の出力手順を示すフローチャートである。図１の傾斜角検出装置による第１条件の判定手順を示すフローチャートである。本発明の実施例２に係る傾斜角検出装置の構成を示す図である。図７の傾斜角検出装置による傾斜角の出力手順を示すフローチャートである。

（実施例１）
本発明を具体的に説明する前に、まず概要を述べる。本発明の実施例１は、カーナビゲーション装置での自立航法において、車両の傾斜角を検出する傾斜角検出装置に関する。実施例１に係る傾斜角検出装置は、加速度センサのオフセットを高精度に検出して、傾斜角も高精度に検出するために、次の処理を実行する。傾斜角検出装置は、高度計から高度情報を取得し、距離センサから距離情報を取得し、速度センサから速度情報を取得し、加速度センサから加速度情報を取得する。傾斜角検出装置は、高度情報と距離情報とをもとに水平に近い状態であるかという第１条件を判定するとともに、速度情報をもとに等速度に近い状態であるかという第２条件を判定する。

第１条件と第２条件とのいずれもが満たされていれば、傾斜角検出装置は、加速度情報をもとに加速度センサのオフセットを更新してから、加速度情報とオフセットとをもとに車両の傾斜角を算出する。一方、第１条件と第２条件との少なくともいずれかが満たされていなければ、傾斜角検出装置は、オフセットを新たに更新せずに、加速度情報とすでに更新したオフセットとをもとに車両の傾斜角を算出する。このように水平に近く、かつ運動加速度が小さいタイミングにおいて、加速度センサのオフセットを補正するので、傾斜路でも加速度センサのオフセットの偏りが小さくされる。このように加速センサのオフセットの偏りが小さくなるので、傾斜角が高精度に補正される。

図１は、本発明の実施例１に係る傾斜角検出装置１００の構成を示す。傾斜角検出装置１００は、高度計１０、距離センサ１２、速度センサ１４、加速度センサ１６、第２取得部１８、第４取得部２０、第３取得部２２、第１取得部２４、リングバッファ２６、判定部２８、導出部３０、制御部３２を含む。また、判定部２８は、第１条件判定部３４、第２条件判定部３６を含み、導出部３０は、オフセット補正部３８、傾斜角演算部４０を含む。傾斜角検出装置１００は、図示しない車両に搭載される。

高度計１０は、気圧をもとに車両の高度を検出する。また、高度計１０は、ＧＰＳ衛星から受信される信号をもとに車両の高度を検出してもよい。高度計１０は、検出した高度に関する情報（以下、「高度情報」という）を第２取得部１８へ出力する。距離センサ１２は、車両の移動距離を検出する。例えば、距離センサ１２は、所定の期間の速度パルスを加算することによって移動距離を導出する。距離センサ１２は、検出した距離に関する情報（以下、「距離情報」という）を第４取得部２０へ出力する。

速度センサ１４は、車両の移動速度を検出する。例えば、移動速度は、単位時間あたりの速度パルスを加算することによって導出される。具体的に説明すると、速度センサ１４は、ドライブシャフトの回転に対応して回転するスピードメータケーブルの中間に設置され、ドライブシャフトの回転に伴った速度パルスを検出した後、車速パルスの単位時間あたりのパルス数を計測することにより移動速度を取得する。ここで、車速パルスは、所定の距離ごとに発生させたパルスに相当する。なお、速度センサ１４は、ＧＰＳから移動速度を直接取得してもよい。速度センサ１４は、検出した移動速度に関する情報（以下、「速度情報」という）を第３取得部２２へ出力する。

加速度センサ１６は、車両の主たる移動方向、例えば、車両の前後方向に生じる加速度を検出する。ここでは、加速度を説明するために図２を使用する。図２は、傾斜角検出装置１００が搭載された車両５０の動作状態を示す。車両５０は、図１の傾斜角検出装置１００を搭載する。また、車両５０は、道路面５４上を移動する。ここで、車両５０に搭載された加速度センサ１６（図示せず）の感応軸５６は、道路面５４に平行であり、かつ車両５０の前後方向と一致するものとする。また、感応軸５６および道路面５４は、水平面５２に対して傾斜角θだけ傾いている。加速度においては、車両５０の移動加速度と、感応軸５６が水平面５２となす傾斜角θに応じた重力成分とが重畳されている。図１に戻る。加速度センサ１６は、検出した加速度に関する情報（以下、「加速度情報」という）を第１取得部２４へ出力する。

第２取得部１８は、高度計１０に接続され、高度計１０からの高度情報を取得する。なお、高度計１０がＧＰＳ衛星から受信した信号をもとに高度を取得している場合、第２取得部１８は、高度情報に加えて、ＶＤＯＰ（ＶｅｒｔｉｃａｌＤｉｌｕｔｉｏｎｏｆＰｒｅｃｉｓｉｏｎ）や受信衛星数を取得してもよい。ＶＤＯＰは、ＧＰＳの高度測位精度の指標である。なお、ＨＤＯＰ（ＨｏｒｉｚｏｎｔａｌＤＯＰ）等のＤＯＰが含まれていてもよい。第４取得部２０は、距離センサ１２に接続され、距離センサ１２からの距離情報を取得する。第３取得部２２は、速度センサ１４に接続され、速度センサ１４からの速度情報を取得する。第１取得部２４は、加速度センサ１６に接続され、加速度センサ１６からの加速度情報を取得する。

リングバッファ２６は、第２取得部１８、第４取得部２０、第３取得部２２、第１取得部２４のそれぞれにおいて取得した各情報を一時的に記憶する。なお、リングバッファ２６は、各情報を定期的に取得しているが、その際、同一タイミングに相当する高度情報、距離情報、速度情報、加速度情報とが組み合わされて記憶される。ここで、リングバッファ２６は、所定数の組合せを記憶しており、最も古いタイミングの情報に最新の情報を上書きして記憶する。図３は、リングバッファ２６に記憶される情報が取得される状況を示す。図３は、図２の車両５０が移動している軌跡を上空から見下ろしている場合を示す。ここで、車両５０は、Ｓ地点から移動を開始して、Ａ地点、Ｂ地点、Ｃ地点、Ｄ地点、Ｅ地点の順に移動している。現在、車両５０は、Ｅ地点にいる。

図４（ａ）−（ｂ）は、リングバッファ２６に記憶されるデータの構造を示す。図４（ａ）のごとく、地点名欄２００、アドレス欄２０２、高度欄２０４、距離欄２０６、速度欄２０８、加速度欄２１０が含まれる。地点名欄２００は、図３に示された地点との対応を明確にするために示されている。実際には地点名欄２００が含まれてていなくてもよい。前述の組合せは、高度欄２０４、距離欄２０６、速度欄２０８、加速度欄２１０に示された情報のうち、ひとつの行に含まれるものに相当する。ここで、リングバッファ２６に記憶される組合せの所定数は「５」であるとされる。アドレス欄２０２には、情報を記憶すべきアドレスが示されている。図４（ｂ）には、書込みポインタと読出しポインタとが示されており、これらは、情報を書き込むべきアドレスと、情報を読み出すアドレスとに対応する。図１に戻る。

第１条件判定部３４は、リングバッファ２６から高度情報を取得するとともに、距離情報を取得する。第１条件判定部３４は、リングバッファ２６に記憶された高度情報の最大値から最小値を減算することによって、高度の範囲Ｒｈを導出する。これは、第２取得部１８において取得した高度情報の変化に相当する。具体的には、次のように、５地点の高度ｈ１〜ｈ５の最大値と最小値とを選択し、最大値から最小値が減算される。

第１条件判定部３４は、リングバッファ２６に記憶された距離情報を加算することによって移動距離Ｓを導出する。これは、第４取得部２０において取得した距離情報の積算値に相当する。具体的には、次のように、Ａ地点からＥ地点までの距離情報、ｓ２からｓ５が加算される。

第１条件判定部３４は、高度の範囲Ｒｈが第１のしきい値よりも小さくなり、かつ移動距離Ｓが第３のしきい値よりも大きくなる場合に、第１条件を満たすと判定する。これは、車両５０が水平に近い状態であることを検出することに相当する。このように、高度の範囲とともに移動距離を第１条件に加えることによって、停止や非常に速度の遅い状態を水平に近いと判定することが抑制される。なお、処理を簡易にするために、第１条件として、高度の範囲Ｒｈに対する判定のみがなされてもよい。

ここで、ＧＰＳ衛星から取得される高度情報を使用する場合、第１条件に対して以下の判定が追加されてもよい。第１条件判定部３４は、図４（ｂ）の読出しポインタを「１」に設定し、読出しポインタで指定されたアドレスの情報を読み出す。第１条件判定部３４は、ＧＰＳのＶＤＯＰが所定値以下であり、ＧＰＳの受信衛星数が所定の値以上であり、ＧＰＳの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じであるかを確認する。これらの条件をすべて満たす場合、第１条件判定部３４は、読出しポインタを加算し、最後の情報になるまで以上の処理を繰り返し実行する。

これらの条件を満たす場合に、第１条件判定部３４は、前述の高度の範囲Ｒｈ、移動距離Ｓに対する判定を実行する。これらの条件を満たさない場合、第１条件判定部３４は、第１条件を満たさないと判定する。つまり、第１条件判定部３４は、第１条件として、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定してもよい。

第２条件判定部３６は、リングバッファ２６から速度情報を取得する。第２条件判定部３６は、隣接した地点間での速度変化ΔＶｉを導出する。ここで、ｉは２〜５である。

第２条件判定部３６は、速度変化ΔＶｉのすべてが第２のしきい値よりも小さくなる場合に、第２条件を満たすと判定する。これは、車両５０が等速走行に近い状態であることを検出することに相当する。

判定部２８は、第１条件と第２条件とのいずれも満たしているか否かを判定する。いずれも満たしている場合、判定部２８は、オフセット補正部３８に対してオフセットを更新させてから、傾斜角演算部４０に傾斜角を導出させる。一方、少なくともいずれか一方を満たしていない場合、判定部２８は、オフセット補正部３８に対してオフセットを更新させずに、傾斜角演算部４０に傾斜角を導出させる。

オフセット補正部３８は、判定部２８において第１条件と第２条件のいずれも満たしている場合、リングバッファ２６に記憶された加速度情報をもとに加速度センサ１６のオフセットを更新する。具体的に説明すると、オフセット補正部３８は、リングバッファ２６に記憶された領域の中心、つまり図４（ｂ）の読出しポインタとして設定されているアドレスに含まれた加速度情報をもとに、加速度オフセットを更新する。ここで、図４（ｂ）の読出しポインタとして設定されているアドレスに含まれた加速度情報は、図３のＣ地点の加速度情報ａ３に相当する。

なお、ａ_ＯＦＳ１は更新後のオフセット、ａ_ＯＦＳ０は更新前のオフセット、Ｋは０〜１の値をとる係数である。Ｋは、例えば０．８である。Ｋを大きくすれば更新前のオフセットの重みが大きく、Ｋを小さくすれば加速度情報ａ３の重みが大きくなる。そのため、Ｋの調節によって、更新の度合いが調節される。一方、オフセット補正部３８は、第１条件と第２条件の少なくともいずれかを満たしていない場合、オフセット補正部３８は、加速度センサ１６のオフセットの更新をスキップする。

傾斜角演算部４０は、判定部２８において第１条件と第２条件のいずれも満たしている場合、速度センサ１４からの速度情報、加速度センサ１６からの加速度情報、オフセット補正部３８にて更新したオフセットとを入力する。傾斜角演算部４０は、速度情報、加速度情報、更新したオフセットとをもとに、車両５０の傾斜角θを次のように導出する。

ここで、ｇは重力加速度であり、ａｍは移動加速度であり、ａ_ＯＵＴは加速度情報、ａ_ＯＦＳ１は更新後のオフセット、ｋ_ａは加速度情報の単位を電圧Ｖｏｌｔから加速度ｍ／ｓ^２へ変換するための係数である。また、移動加速度ａｍは、速度情報の単位時間の変化として導出される。なお、θの単位はラジアンであり、θの値が小さい場合の式である。

傾斜角演算部４０は、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、オフセット補正部３８において更新したオフセットの代わりに、すでに更新されていたオフセットを使用して、前述のごとく車両５０の傾斜角θを導出する。このように、導出部３０は、判定部２８において第１条件と第２条件のいずれも満たしているか否かに応じて、処理内容を変更しながら、第１取得部２４において取得した加速度情報をもとに、移動体の傾斜角θを導出する。制御部３２は、傾斜角検出装置１００での動作タイミングを制御する。

この構成は、ハードウエア的には、任意のコンピュータのＣＰＵ、メモリ、その他のＬＳＩで実現でき、ソフトウエア的にはメモリにロードされたプログラムなどによって実現されるが、ここではそれらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックがハードウエアのみ、ソフトウエアのみ、またはそれらの組合せによっていろいろな形で実現できることは、当業者には理解されるところである。

以上の構成による傾斜角検出装置１００の動作を説明する。図５は、傾斜角検出装置１００による傾斜角の出力手順を示すフローチャートである。第２取得部１８は、高度計１０から高度情報を取得する（Ｓ１０）。第４取得部２０は、距離センサ１２から距離情報を取得する（Ｓ１２）。第３取得部２２は、速度センサ１４から速度情報を取得する（Ｓ１４）。第１取得部２４は、加速度センサ１６から加速度情報を取得する（Ｓ１６）。リングバッファ２６は、これらの情報を記憶する（Ｓ１８）。第１条件判定部３４は、高度の範囲を計算し（Ｓ２０）、移動距離を計算する（Ｓ２２）。第１条件判定部３４において第１条件を満たしている場合（Ｓ２４のＹ）、第２条件判定部３６は、速度変化を計算する（Ｓ２６）。

第２条件判定部３６において第２条件を満たしている場合（Ｓ２８のＹ）、オフセット補正部３８は、加速度情報からオフセットを更新する（Ｓ３０）。一方、第１条件判定部３４において第１条件を満たしていない場合（Ｓ２４のＮ）、あるいは第２条件判定部３６において第２条件を満たしていない場合（Ｓ２８のＮ）、ステップ３２へスキップされる。傾斜角演算部４０は、傾斜角を計算し（Ｓ３２）、傾斜角を出力する（Ｓ３４）。処理を継続する場合（Ｓ３６のＹ）、リングバッファ２６の書込みポインタと読出しポインタを進め（Ｓ３８）、ステップ１０に戻る。書込みポインタが５、読出しポインタが３の場合、ポインタを進めるとそれぞれ１と４となる。処理を継続しない場合（Ｓ３６のＮ）、処理は終了される。

図６は、傾斜角検出装置１００による第１条件の判定手順を示すフローチャートである。これは、図５のステップ２４の処理に相当する。第１条件判定部３４は、読出しポインタを１に設定する（Ｓ５０）。第１条件判定部３４は、読出しポインタで指定されたアドレスの情報を読み出す（Ｓ５２）。ＧＰＳのＶＤＯＰが所定の値以下であり（Ｓ５４のＹ）、ＧＳＰの受信衛星数が所定の数以上であり（Ｓ５６のＹ）、ＧＳＰの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じであれば（Ｓ５８のＹ）、最後の情報であるかが確認される（Ｓ６０）。最後の情報でなければ（Ｓ６０のＮ）、読出しポインタに１が加算されて（Ｓ６２）、ステップ５２に戻る。

最後の情報である場合（Ｓ６０のＹ）、高度の範囲が第１のしきい値よりも小さく（Ｓ６４のＹ）、移動距離の範囲が第３のしきい値よりも小さければ（Ｓ６６のＹ）、第１条件判定部３４は、第１条件を満たすと判定する（Ｓ６８）。一方、ＧＰＳのＶＤＯＰが所定の値以下でない場合（Ｓ５４のＮ）、あるいはＧＳＰの受信衛星数が所定の数以上でない場合（Ｓ５６のＮ）、あるいはＧＳＰの受信衛星数がアドレス１のＧＰＳ受信衛星数と同じでない場合（Ｓ５８のＮ）、あるいは高度の範囲が第１のしきい値よりも小さくない場合（Ｓ６４のＮ）、あるいは移動距離の範囲が第３のしきい値よりも小さくない場合（Ｓ６６のＮ）、第１条件判定部３４は、第１条件を満たさないと判定する（Ｓ７０）。

本発明の実施例によれば、水平に近く、かつ等速度移動に近いか否かに応じて、処理内容を変更して車両の傾斜角を導出するので、車両の状況に応じて傾斜角を高精度に検出できる。また、水平に近く、かつ等速度移動に近い場合、オフセット値を更新するので、オフセット値の精度を向上できる。また、オフセット値の精度が向上するので、傾斜角の検出精度を向上できる。また、水平に近くない場合や、等速度移動に近くない場合に、オフセット値を更新しないので、オフセット値の精度を維持できる。また、水平に近くない場合や、等速度移動に近くない場合に、オフセット値を更新せずに、傾斜角を導出するので、最新の傾斜角を検出できる。また、車両が水平に近い状態であるかを判定するために、距離情報の積算値が大きいかを判定するので、車両が静止している状態を除外できる。また、車両が静止している状態が除外されるので、車両が水平に近い状態であるかの判定精度を向上できる。

また、所定の期間にわたって、ＶＤＯＰが所定値以下でありつつ、信号を受信した衛星数が一定数でかつ所定数以上であることも判定するので、移動体が水平に近い状態であるかの判定精度を向上できる。また、水平に近いことを検出すればよいので、カーナビゲーションシステムならば標準に搭載されているＧＰＳ等を高度計として使用することができる。また、カーナビゲーションシステムならば標準に搭載されているＧＰＳ等が高度計として使用されるので、高価なセンサを新規に装備することを不要にできる。また、高価なセンサを新規に装備することなく、カーナビゲーションシステムならば標準に搭載されているセンサのみから、加速度センサのオフセットを容易に推定できるので、精度の高い傾斜角検出装置を容易に実現できる。

（実施例２）
本発明の実施例２は、実施例１と同様に、傾斜角を検出するための傾斜角検出装置に関する。実施例１における傾斜角検出装置は、加速度センサのオフセットおよび傾斜角を状態変数として設定し、カルマンフィルタ処理を実行することによって、オフセットおよび傾斜角を更新する。ここで、傾斜角検出装置は、第１条件と第２条件とのいずれも満たしている場合、観測誤差として第１値を設定して、カルマンフィルタ処理に使用し、第１条件と第２条件との少なくともいずれかを満たしていない場合、観測誤差として第２を設定して、カルマンフィルタ処理を実行する。ここで、第２値は第１値よりも大きな値として規定されている。

図７は、本発明の実施例２に係る傾斜角検出装置１００の構成を示す。傾斜角検出装置１００は、図１の傾斜角検出装置１００と比較して、導出部３０を含まず、導出部７０を含む。導出部７０は、設定部７２、処理部７４を含む。ここでは差異を中心に説明する。

設定部７２は、判定部２８において第１条件と第２条件のいずれも満たしている場合、加速度センサ１６の観測誤差δａ_ｎとして第１値を設定する。一方、設定部７２は、判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、加速度センサ１６の観測誤差δａ_ｎとして第２値を設定する。前述のごとく、第２値は、大きな値として設定する。設定部７２は、設定した観測誤差を処理部７４に使用させる。

処理部７４は、第１取得部２４からの加速度情報ａ_ＯＵＴを入力する。なお、タイミングｎでの加速度情報は、ａ_ＯＵＴｎと示されるものとする。ここでは、タイミングｎでの状態変数ｘ_ｎを定義する。状態変数ｘ_ｎは、次のように、速度ｖ_ｎ、傾斜角θ_ｎ、加速度のオフセットａ_ＯＦＳｎの３要素を含む。

ここで、上付きの添え字Ｔは、ベクトルまたは行列の転置を示す。前述のごとく、下付きの添え字ｎはタイミングｎを示す。

処理部７４は、入力ｕ_ｎと、１回前の状態変数、つまりタイミングｎ−１での状態変数ｘ_ｎ−１とをもとに、次に示された状態方程式を計算することによって、タイミングｎでの推定値ｘ_ｎ’を演算する。なお、状態変数ｘ_ｎ−１は、すでに導出された後、保持されている。

ここで、入力ｕ_ｎは、加速度情報ａ_ＯＵＴｎである。

また、式（７）におけるシステム行列Ａと入力行列Ｂとは、式（９）および式（１０）のように示される。

ここで、ｇは重力加速度、ｋは加速度情報から加速度単位（例えばｍ／ｓ^２）への変換係数、ΔＴはサンプリング間隔を示す。前述のごとく、加速度センサの感応軸は、車両５０の前後方向と一致するものとする。

処理部７４は、速度情報ｖ_ｎ、加速度情報ａ_ＯＵＴｎとを構成要素として、観測値ｚ_ｎを規定する。処理部７４は、次のように、推定値ｘ_ｎ’から抜き出された状態変数の速度と加速度のオフセットとに対して、観測値ｚ_ｎとの観測残差ｙ_ｎを導出する。

ここで、出力行列Ｃは次のように示される。

処理部７４は、設定部７２で設定された観測誤差δａ_ｎ、速度センサ１４の観測誤差δｖ_ｎとを入力し、これらの観測誤差が標準偏差を表すものとして、共分散行列Ｒ_ｎを次のように導出する。

なお、速度センサ１４の観測誤差δｖ_ｎは一定の値として設定される。処理部７４は、共分散行列Ｒｎとシステム誤差とをもとに、カルマンフィルタのゲインＫ_ｎを演算する。ここで、共分散行列Ｒ等からカルマンフィルタのゲインの導出には、公知の技術が使用されればよい。公知の技術は、例えば、ＭＯＨＩＮＤＥＲＳ．ＧＲＥＷＡＬ、ＡＮＧＵＳＰ．ＡＮＤＲＥＷＳ著「ＫａｌｍａｎＦｉｌｔｅｒｉｎｇ：ＴｈｅｏｒｙａｎｄＰｒａｃｔｉｃｅＵｓｉｎｇＭＡＴＬＡＢ」、ＡＷｉｌｅｙ−ＩｎｔｅｒｓｃｉｅｎｃｅＰｕｂｌｉｃａｔｉｏｎの１１６頁から１２０頁に示されている。処理部７４は、次のように、６つの構成要素によって形成されるゲインを導出する。

処理部７４は、推定値ｘ_ｎ’、ゲインＫ_ｎ、観測残差ｙ_ｎをもとに、式（１５）を演算することによって、状態変数ｘ_ｎを更新する。

ゲインＫ_ｎが小さければ、推定値ｘ_ｎ’の重みが大きく、ゲインＫ_ｎが大きければ、観測残差ｙ_ｎの重みが大きい。なお、式（１５）は、ｘ_ｎ’＋Ｋ_ｎｙ_ｎを新たにｘ_ｎに代入することを表している。以上の処理は、設定部７２において設定した観測誤差と、第１取得部２４において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、加速度センサ１６のオフセットと車両５０の傾斜角θを導出することに相当する。

図８は、傾斜角検出装置１００による傾斜角の出力手順を示すフローチャートである。第２取得部１８は、高度計１０から高度情報を取得する（Ｓ１００）。第４取得部２０は、距離センサ１２から距離情報を取得する（Ｓ１０２）。第３取得部２２は、速度センサ１４から速度情報を取得する（Ｓ１０４）。第１取得部２４は、加速度センサ１６から加速度情報を取得する（Ｓ１０６）。リングバッファ２６は、これらの情報を記憶する（Ｓ１０８）。第１条件判定部３４は、高度の範囲を計算し（Ｓ１１０）、移動距離を計算する（Ｓ１１２）。第１条件判定部３４において第１条件を満たしている場合（Ｓ１１４のＹ）、第２条件判定部３６は、速度変化を計算する（Ｓ１１６）。

第２条件判定部３６において第２条件を満たしている場合（Ｓ１１８のＹ）、設定部７２は、観測誤差として第１値を設定する（Ｓ１２０）。一方、第１条件判定部３４において第１条件を満たしていない場合（Ｓ１１４のＮ）、あるいは第２条件判定部３６において第２条件を満たしていない場合（Ｓ１１８のＮ）、設定部７２は、観測誤差として第２値を設定する（Ｓ１２２）。処理部７４は、カルマンフィルタ処理を実行し（Ｓ１２４）、傾斜角を出力する（Ｓ１２６）。処理を継続する場合（Ｓ１２８のＹ）、リングバッファ２６の書込みポインタと読出しポインタを進め（Ｓ１３０）、ステップ１００に戻る。処理を継続しない場合（Ｓ１２８のＮ）、処理は終了される。

本発明の実施例によれば、カルマンフィルタを使用するので、加速度センサのオフセットと傾斜角とを同時に推定できる。また、観測誤差として第１値よりも第２値を大きくするので、車両が低速で上りまたは下りが連続する傾斜路を走行するときに、加速度センサに重力成分が連続して重畳され、真のオフセットに対して偏りが生ずることによる傾斜角の検出精度の悪化を抑制できる。また、速度情報が小さければ、加速度センサに対するカルマンゲインを所定の値に設定するので、精度の悪化を抑制しながら、処理量を低減できる。

以上、本発明を実施例をもとに説明した。この実施例は例示であり、それらの各構成要素や各処理プロセスの組合せにいろいろな変形例が可能なこと、またそうした変形例も本発明の範囲にあることは当業者に理解されるところである。

本発明の実施例１または２において、傾斜角検出装置１００は車両５０に搭載されているとしている。しかしながらこれに限らず例えば、傾斜角検出装置１００は歩行者に携帯されてもよい。本変形例によれば、傾斜角検出装置１００をさまざまな製品に適用できる。

本発明の実施例２において、傾斜角検出装置は、速度情報の値にかかわらず、カルマンフィルタのゲインを導出し、カルマンフィルタ処理を実行する。速度情報の値が小さい場合、一部のゲインはある程度小さな値になる。そのようなゲインの値が小さければ、ゲインの乗算結果も小さくなるので、ゲインの精度があまり重要ではなくなる。そのため、処理量を低減することを目的として、処理部７４は、第３取得部２２において取得した速度情報と所定値とを比較してもよい。

その後、処理部７４は、比較の結果、第３取得部２２において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において加速度センサ１６に対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定する。予め定めた値は、例えば所定の固定値である。加速度情報に対するゲインは、式（１４）におけるゲインＫ３２に相当する。ゲインＫ３２はゼロまたは小さな値に設定される。本変形例によれば、速度情報が小さければ、加速度情報に対するゲインを所定の値に設定するので、処理量を低減できる。また、速度情報が小さければ、加速度情報に対するゲインがゼロに近くなるべきなので、所定の値にゲインを設定しても、傾斜角の検出精度の悪化を抑制できる。

本発明の実施例２において、設定部７２が設定すべき観測誤差として、第１値と第２値の２種類が規定されている。しかしながらこれに限らず例えば、観測誤差として３種類以上の値が規定されていてもよい。その場合、設定部７２は、水平への近さの程度や等速度移動への近さの程度に応じて、観測誤差の値を設定する。具体的には、水平や等速度移動に近くなるほど、観測誤差が小さくなるように設定される。本変形例によれば、観測誤差を詳細に設定できる。

１０高度計、１２距離センサ、１４速度センサ、１６加速度センサ、１８第２取得部、２０第４取得部、２２第３取得部、２４第１取得部、２６リングバッファ、２８判定部、３０導出部、３２制御部、３４第１条件判定部、３６第２条件判定部、３８オフセット補正部、４０傾斜角演算部、５０車両、５２水平面、５４道路面、５６感応軸、７０導出部、７２設定部、７４処理部、１００傾斜角検出装置。

Claims

移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得する第１取得部と、
前記移動体の高度に関する高度情報を取得する第２取得部と、
前記移動体の移動速度に関する速度情報を取得する第３取得部と、
前記第２取得部において取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、前記第３取得部において取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定する判定部と、
前記判定部において第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、前記判定部において第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定部と、前記設定部において設定した観測誤差と、前記第１取得部において取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理部と、を備える導出部と、
を備えることを特徴とする傾斜角検出装置。
前記移動体の移動距離に関する距離情報を取得する第４取得部をさらに備え、
前記判定部は、第１条件として、前記第２取得部において取得した高度情報の変化が第１のしきい値よりも小さく、かつ前記第４取得部において取得した距離情報の積算値が第３のしきい値よりも大きいことを判定することを特徴とする請求項１に記載の傾斜角検出装置。
前記第２取得部は、全地球測位システムの衛星から受信した信号をもとに高度情報を取得し、
前記判定部は、第１条件として、所定の期間にわたって、ＶＤＯＰが所定値以下であり、かつ信号を受信した衛星数が所定数以上の一定数であることを判定することを特徴とする請求項１または２に記載の傾斜角検出装置。
前記第３取得部において取得した速度情報と所定値とを比較する比較部をさらに備え、
前記処理部は、前記比較部における比較の結果、前記第３取得部において取得した速度情報が所定値よりも小さい場合、カルマンフィルタ処理において前記加速度センサに対するカルマンゲインを導出する代わりに、予め定めた値をカルマンゲインとして設定することを特徴とする請求項１に記載の傾斜角検出装置。
移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得するステップと、
前記移動体の高度に関する高度情報を取得するステップと、
前記移動体の移動速度に関する速度情報を取得するステップと、
取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、
第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、前記設定ステップにおいて設定した観測誤差と、前記加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、
を備えることを特徴とする傾斜角検出方法。
移動体の加速度に関する加速度情報を前記移動体に搭載された加速度センサから取得するステップと、
前記移動体の高度に関する高度情報を取得するステップと、
前記移動体の移動速度に関する速度情報を取得するステップと、
取得した高度情報の変化が第１のしきい値よりも小さい第１条件と、取得した速度情報の変化が第２のしきい値よりも小さい第２条件とのいずれも満たしているか否かを判定するステップと、
第１条件と第２条件のいずれも満たしている場合、前記加速度センサの観測誤差として第１値を設定し、第１条件と第２条件の少なくともいずれかを満たしていない場合、前記加速度センサの観測誤差として第１値よりも大きな第２値を設定する設定ステップと、前記設定ステップにおいて設定した観測誤差と、前記加速度センサから取得するステップにおいて取得した加速度情報とをもとに、カルマンフィルタ処理を実行することによって、前記加速度センサのオフセットと前記移動体の傾斜角を導出する処理ステップと、を備える導出ステップと、
を、傾斜角検出装置が備えるコンピュータに実行させるためのプログラム。