角速度補正装置及びその補正方法並びにナビゲーション装置
【目的】センサーの取り付けピッチ角、車両のピッチ角、ジャイロセンサーの感度誤差の3つの誤差要因に対処できる「角速度補正装置及びその補正方法並びにナビゲーション装置」を提供することである。
【構成】角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にし、該第1のセンサー感度補正方法により得られたセンサー感度と第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出し、該感度誤差を用いて角速度検出センサーが出力する角速度を補正する。
【構成】角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にし、該第1のセンサー感度補正方法により得られたセンサー感度と第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出し、該感度誤差を用いて角速度検出センサーが出力する角速度を補正する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は角速度補正装置及びその補正方法並びにナビゲーション装置に関わり、特に、ジャイロセンサーが出力する角速度信号の誤差(センサー取り付けピッチ角、車両ピッチ角、センサーの感度誤差等に起因する角速度誤差)を補正する角速度補正装置及びその補正方法及び並びにナビゲーション装置に関する。
【背景技術】
【0002】
一般に、車載用のナビゲーション装置では、自立航法センサーやGPS(Global Positioning System)受信機などを用いて車両の現在位置を検出し、その近傍の地図データを記録媒体から読み出して画面上に表示する。そして、画面上の所定箇所に自車位置を示す自車位置マークを重ね合わせて表示することにより、車両が現在どこを走行しているのかを一目で分かるようにしている。
自立航法センサーは、所定走行距離毎に1個のパルスを出力して車両の移動距離を検出する車速センサー(距離センサー)と、車両の回転角度(移動方位)を検出する振動ジャイロ等の角速度センサーとを含んでいる。自立航法センサーは、これらの車速センサーおよび角速度センサーによって車両の相対位置および相対方位を検出するようになっている。ここで、角速度センサーは、角速度を検出しようとする平面が、実際に角速度センサーが回転する平面と平行である必要がある。両者の平面が互いに平行でなくなると、角速度センサーの出力感度が本来の感度からずれてしまう。
一般的に、角速度センサーは、これを水平に設置したときの出力感度で正しく進行方向が判断できるように調整してある。そのため、角速度センサーの角度が水平でなくなった場合には、角速度センサーに加わる角速度と出力感度との関係が変化して、車両の進行方向を正しく検出することができなくなってしまう。そこで、角速度センサーの水平方向に対する傾斜分の感度補正を行うことが必要になる。
【0003】
従来、この感度補正は、以下のようにして行っていた。すなわち、GPS衛星から電波を良好に受信できているときに、GPS受信機により絶対方位として得られるGPS方位と、角速度センサーにより得られる角速度から計算されるジャイロ方位とを比較し、角速度
がGPS方位の変化量と一致するように、角速度センサーの感度を補正し、該補正された感度を用いて角速度補正をしている(特許文献1)。
図14はかかる従来技術の角速度補正装置の構成図であり、自立航法センサー1、位置・方位演算部2、GPS受信機3、メモリコントローラ4、RAM(走行履歴記憶部)5及びプロセッサ6を備えて構成されている。
自立航法センサー1は、車両の現在位置を測定するためのものであり、距離センサー(車速センサー)1aと振動ジャイロ等の角速度センサー(相対方位センサー)1bとを含んでいる。距離センサー1aは、一定時間ごとに1個のパルスを出力して車両の移動距離を検出する。角速度センサー1bは、車両の走行に伴って変化する角速度を検出し、検出した角速度を積分して一定時間ごとに車両の回転角度(移動方位)を出力する。自立航法センサー1は、これらの距離センサー1a及び角速度センサー1bによって車両の相対位置および相対方位を一定時間ごとに検出し、その情報を逐次出力する。
位置・方位演算部2は、自立航法センサー1から出力される車両の相対位置および相対方位の情報に基づいて、絶対的な車両位置(推定車両位置)および車両方位を計算し、一定時間ごとに出力する。以下では、このように自立航法センサー1の出力に基づき計算した絶対位置および絶対方位をそれぞれジャイロ位置、ジャイロ方位と呼ぶ。なお、角速度センサー1bおよび位置・方位演算部2によって方位センサーが構成される。
【0004】
GPS受信機3も車両の現在位置を測定するためのものであり、図示しない複数のGPS衛星から送られてくる電波を図示しないGPSアンテナで受信し、3次元測位処理あるいは2次元測位処理を行って車両の絶対位置および絶対方位を自律航法センサー1と同じ一定時間ごとに計算する(車両方位は、現時点における自車位置と1サンプリング時間ΔT前の自車位置とに基づいて計算する)。そして、これらの計算した車両の絶対位置および絶対方位の情報を、測位時刻と共に逐次出力する。以下では、GPS受信機3の出力に基づき計算した絶対位置および絶対方位をそれぞれGPS位置、GPS方位と呼ぶ。
メモリコントローラ4は、位置・方位演算部2により求められたジャイロ位置およびジャイロ方位を、自立航法センサー1による走行履歴のデータとしてRAM5に順次格納する。また、GPS受信機3により求められたGPS位置及びGPS方位を、GPS受信機3による走行履歴のデータとしてRAM5に順次格納する。このメモリコントローラ4は又、プロセッサ6からの要求に応じて、RAM5から各走行履歴のデータを読み出してプロセッサ6に供給する。
【0005】
図15は、RAM5に格納される走行履歴データの例を示すイメージ図であり、図15(A)はGPS受信機3による走行履歴を、図15(B)は自立航法センサー1による走行履歴を示している。図15(A)において、黒丸は一定時間ごとに(例えば1秒おき)に出力されるGPS位置を示し、矢印は一定時間ごとに(例えば1秒おき)に出力されるGPS方位を示す。GPS方位はθgpsn (nは1,2,・・・Nで、Nは任意の整数)の記号で表している。
図15(B)において、白丸は一定時間ごとに(例えば1秒おき)に出力されるジャイロ位置を示し、矢印は一定時間ごとに(例えば1秒おき)に出力されるジャイロ方位を示す。ジャイロ方位はθgyrn (nは1,2,・・・Nで、Nは任意の整数)の記号で表している。
図15は車両が交差点を左折しているときに、GPS方位は比較的正確な値を示しているのに対し、角速度センサー1bがセンサー感度に誤差が含まれているためにジャイロ方位が正確な値からずれている様子を示している。
プロセッサ6は、その機能構成として、一次サンプリング部6a、二次サンプリング部6b、感度サンプル演算部6c及び統計処理部6dを備えている。一次サンプリング部6aは、一定時間ごとにRAM5に格納されたジャイロ方位θgyrn(第1の方位信号)およびGPS方位θgpsn(第2の方位信号)の中から、以下の第1条件(1)〜(3)を満たすときに出力された方位信号のみを選択的に抽出する。
第1条件は、
(1)角速度センサー1bを搭載している車両の速度がしきい値(例えば、10[km/h])以上であること、
(2)GPSの信頼度が高いこと、
(3)曲率(または方位変化率)が低いこと,
である。
(1)の条件は、車速が低いときはGPS方位の誤差が大きくなることが多いためである。車速は、距離センサー1aの出力から求めることが可能である。すなわち、RAM5に走行履歴として格納されている連続するジャイロ位置間の距離をその間の時間(1秒)で割ることによって求めることができる。
(2)の条件は、GPSの信頼度が低いとGPS方位の誤差が大きくなることが多いためである。自立航法センサー1により得られるある区間の走行距離(自立航法走行距離)と、GPS受信機3により得られる上記ある区間に相当する区間の距離(GPS測位点間距離)との差がしきい値以下であるときに、GPS信頼度は高いと判定することができる。
(3)の条件は、曲率(方位変化率)が高いときはGPS方位の誤差が大きくなることが多いためである。曲率に関しては、例えば、角速度センサー1により得られるジャイロ方位の過去一定期間(例えば過去30m)における変化量がしきい値(例えば5度)以下であることが条件となる。
以上の条件に従って抽出されたデータを、以下では「一次サンプル」と呼ぶ。以上により、位置・方位演算部2により一定時間毎に求められるジャイロ方位とGPS受信機3により一定時間毎に求められるGPS方位とを全てRAM5に一旦格納し、その中から上記条件に合致するものが一次サンプルとして抽出される。
【0006】
二次サンプリング部6bは、一次サンプリング部6aによって抽出されたジャイロ方位及びGPS方位の中から、以下に述べる第2の条件(4)〜(5)を満たすもののみを選択的に抽出する。
(4)後述する第1の方位変化角度及び第2の方位変化角度を求める際に対象とする2つの一次サンプルの時間差がしきい値以下であること。すなわち、現在の一次サンプルNから対象とする一次サンプルi(iはN−1,N−2,・・・)までの時間がしきい値(例えば、60秒)以下であること。演算対象とするサンプル間の時間間隔が長いと、角速度センサー1bのゼロ点誤差に起因するジャイロ方位の誤差が大きくなってしまうからである。ゼロ点誤差とは、角速度センサー1bの出力電圧値が周辺温度の影響によって変動し、基準電圧値がゼロ点の真の電圧値から外れてしまうことを言う。
(5)現在の一次サンプルNと対象とする一次サンプルi(iはN−1,N−2,・・・)とのGPS変化角度がしきい値範囲内であること。ここで、GPS変化角度は以下の次式
GPS変化角度=θgpsN−θgpsi (1)
により計算する。GPS変化角度が小さすぎたり、大きすぎたりすると、角速度センサー1bの感度誤差が顕在化されず、当該感度誤差を演算によって正しく求めることが困難になるからである。
以上の条件に従って抽出されたデータを、以下では「二次サンプル」と呼ぶ。感度サンプル演算部6cは、二次サンプリング部6bによって抽出されたジャイロ方位同士の差分を表す第1の方位変化角度(ジャイロ変化角度)と、二次サンプリング部6bによって抽出されたGPS方位同士の差分を表す第2の方位変化角度(GPS変化角度)とから複数の感度サンプル(感度の瞬時値)を算出する。
【0007】
上述したように、GPS変化角度は(1)式によって求める。一方、ジャイロ変化角度は、次式
ジャイロ変化角度=θgyrN−θgyri (2)
により計算する。さらに、角速度センサーの感度サンプルは、次式
感度サンプル=GPS変化角度/ジャイロ変化角度 (3)
により計算する。ここでは、ジャイロ変化角度及びGPS変化角度がそれぞれ複数求められており、それぞれについて(3)式の演算を行うことにより、複数の感度サンプルを求める。
統計処理部6dは、感度サンプル演算部6cによって求められた複数の感度サンプルを統計処理することによって、角速度センサー1bのセンサー感度を算出する。例えば、抽出された複数の感度サンプルの移動平均値をセンサー感度Aとして出力する。なお、平均値からの差が大きい感度サンプル(例えば差が15%以上の感度サンプル)を異常値とみなして除外し、再度平均値を計算することにより、演算精度を上げることが可能である。なお、(3)式より、センサー感度Aは、該センサー感度Aによりを角速度検出センサーから得られたセンサー方位を補正したとき、補正されたセンサー方位がGPS電波より得られるGPS方位に一致するように決定される。
以上によりセンサー感度Aを求め、角速度補正部7は該センサー感度Aを用いて次式
ωgyro′=ωgyro×A
により、角速度センサー1bより出力する角速度ωgyroを補正する。
【0008】
又、本願出願人が平成19年3月8日付けで日本特許庁に出願済みの角速度補正装置(特許文献2)では、以下の(1)〜(5)のステップによりセンサー感度Bを算出して角速度を補正する。すなわち、
(1)速度センサー(距離センサー)と加速度センサーとの出力信号を用いて水平面に対するジャイロ/加速度センサーの傾斜角(加速度傾斜角)θaccと、GPS受信信号を用いて水平線に対する車両の傾斜角(GPS傾斜角)θgpsをそれぞれ測定し、
(2)これら加速度傾斜角θaccとGPS傾斜角θgpsよりセンサー傾斜角θsenser(=θacc−θgps)を求め、また
(3)加速度傾斜角θaccとセンサー傾斜角θsenserを用いて車両傾斜角θcar(=θacc−θsenser)を求め、
(4)センサー傾斜角θsenserと車両傾斜角θcarを用いてセンサー感度Bを次式
1/cos(θsensor+θcar)
により決定し、
(5)該センサー感度Bを用いて角速度補正する。
【特許文献1】特開2006−71474号公報
【特許文献2】特願2007−58131号
【発明の開示】
【発明が解決しようとする課題】
【0009】
ジャイロセンサーが出力する角速度は、以下の(1)〜(3)の誤差要因があり、
(1) 車両に対するセンサーの取り付けピッチ角
(2) 水平面に対する、車両のピッチ角(道路の傾斜角)
(3) ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)
それぞれを考慮した補正を行う必要がある。
図14の従来技術は、GPS電波を受信できないとき、あるいは、車両ピッチ角(車両傾斜角)の変化が大きい立体駐車場を走行しているとき、ジャイロセンサーの感度を計算できず、結果的にジャイロ角速度の補正ができない(上記の誤差要因(2)に対処できない)。このため、方位ズレが発生し、立体駐車場内での自車位置の精度悪化を来たし、また立体駐車場脱出後のマップマッチング処理による実際の走行道路への復帰が遅れる問題がある。
また、特許文献2で提案の技術は、ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)を吸収できない。このため、センサー感度が標準値から大きく外れて、スペックぎりぎりの場合、交差点右左折時などにおいて、曲がりが足りない、または曲がり過ぎとなり、自車位置・方位精度が悪化する。特に、短時間で大きく方位が変化する立体駐車場内で、位置精度の悪化が大きい。例えば、感度の標準値は25mV/(deg/s)で、スペック上の許容範囲は23.25〜26.75 mV/(deg/s)である。これは誤差に換算すると、+7.5〜−6.5%の範囲であり、ジャイロ角速度誤差が生じる。したがって、例えば5階建ての立体駐車場を上って降りてきたとすると、脱出時の自車方位最大誤差θerrorは、
θerror=360deg×5×2×7.5÷100=270deg
となり、脱出後の道路への復帰遅れにつながる。なお、上式中5は5回転、2は上り/下りによるものである。
【0010】
以上から本発明の目的は上記(1)〜(3)の誤差要因に対処できる角速度補正装置及びその補正方法並びにナビゲーション装置を提供することである。
本発明の別の目的は、GPS電波を受信できないときでも、あるいは、車両のピッチ角変化が大きい立体駐車場を走行しているときでも、ジャイロ角速度の補正ができるようにすることである。
本発明の別の目的は、立体駐車場内での自車位置精度を維持でき、また立体駐車場脱出後に速やかに実際の走行道路への復帰を可能にすることである。
【課題を解決するための手段】
【0011】
・角速度補正方法
本発明の第1は、車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正方法であり、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にするステップ、前記第1のセンサー感度補正方法により得られたセンサー感度と前記第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出するステップ、該感度誤差を用いて前記センサーが出力する角速度を補正するステップを有している。
上記の角速度補正方法は、前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正するステップ、該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正するステップを有している。
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、前記第2のセンサー感度補正方法は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算し、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算し、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算し、前記加速度センサーの水平面に対する傾斜角θaccと前記傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算し、
1/cos(θsensor+θcar)
をセンサー感度として出力する。
また、前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、前記第2のセンサー感度補正方法は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして求め、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力する。
前記感度誤差を算出するステップは、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いか監視するステップ、該条件が成立したとき前記感度誤差の算出を行なうステップ、を有している。
【0012】
・角速度補正装置
本発明の第2は、車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正装置であり、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部を有している。
角速度補正装置は、更に、前記算出された感度誤差を前記第2のセンサー感度算出部で求めたセンサー感度に乗算してセンサー感度を補正する感度補正部を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する。
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、前記第2のセンサー感度算出部は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算するセンサー傾斜角計算部、前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
1/cos(θsensor+θcar)
をセンサー感度として出力する感度算出部、を備えている。
また、前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、前記第2のセンサー感度算出部は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして取得する車両傾斜角取得部、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力するセンサー感度算出部を備えている。
角速度補正装置は、更に、GPSの信頼度を判定するGPS信頼度判定部、車両の水平面に対する傾斜角度が水平に近いか監視する監視部を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう。
【0013】
・ナビゲーション装置
本発明の第3は、GPS受信機と角速度センサーと加速度センサーと車速センサーとを備え、これらの出力値を用いて車両の位置および方位を検出する機能を備えたナビゲーション装置であり、前記角速度補正装置と、角速度補正装置により補正された角速度と車速センサーを用いて車両位置を計算する車両位置計算部を備えている
【発明の効果】
【0014】
本発明によれば、
(1) 車両に対するセンサーの取り付けピッチ角
(2) 水平面に対する車両のピッチ角(道路の傾斜角)
(3) ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)
のそれぞれを考慮した感度補正をすることができ、該補正された感度により角速度を正確に補正することができる。
また、本発明によれば、上記(1)〜(3)の誤差要因を考慮して補正された角速度を用いて自動車位置を正確に推測するナビゲーション装置を提供することができる。
また、本発明によれば、GPS電波を受信できないときでも、あるいは、車両のピッチ角変化が大きい立体駐車場を走行しているときでも、ジャイロ角速度の補正ができ、立体駐車場内での自車位置精度を維持することができ、また、立体駐車場脱出後に速やかに実際の走行道路への復帰ができる。
更に、ジャイロセンサーの感度誤差を自動的に補正できるため、センサーを回転台等に載せて、手動で感度を補正する工程が不要になり、これにより、コスト削減につなげることができる。
【発明を実施するための最良の形態】
【0015】
(A)角速度補正装置の全体構成
図1は角速度補正装置の全体構成図である。角速度補正装置は、車両に取り付けられて該車両の角速度を検出する角速度検出センサー(ジャイロセンサー)の感度を補正し、該補正された感度を用いて角速度を補正する。
角速度補正装置において、第1感度算出部11は、ジャイロセンサーの水平面に対する傾斜角(車両ピッチ角+センサー取り付けピッチ角)に起因するセンサー感度の変化およびジャイロセンサーの感度誤差の両方を考慮して該センサー感度を補正し、補正後の感度Aを出力する。
第2感度算出部12は、ジャイロセンサーの感度誤差を考慮せず、ジャイロセンサーの水平面に対する傾斜角に起因するセンサー感度の変化のみを考慮して該センサー感度を補正し、補正後の感度Bを出力する。
感度誤差算出部13は、第1センサー感度算出部11により得られたセンサー感度Aと第2センサー感度算出部12により得られたセンサー感度Bの比η(=A/B)をセンサーの感度誤差として計算して出力する。第1感度算出部11により補正算出される角速度センサーの感度Aはセンサー取り付け面と水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)およびジャイロセンサーの感度誤差Serrorに依存し、以下の関係式
A∝g(θsensor+θcar)・Serror
が成立する。但し、g( )は傾斜角(θsensor+θcar)より感度を算出する関数である。また、第2感度算出部12により補正算出される角速度センサーの感度Bはセンサー取り付け面と水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)のみに依存し、以下の関係式
B∝g(θsensor+θcar)
が成立する。したがって、センサー感度Aとセンサー感度Bの比η(=A/B)を計算することにより、ηは
η∝Serror
となり、感度誤差Serrorを計算することができる。なお、精度のよい感度誤差Serrorを計算するには、GPSの信頼度が高く、かつ車両ピッチ角(車両傾斜角)が0deg(=0度)付近のときに計算する必要がある。
感度補正部14は、該感度誤差Serrorを第2感度算出部12より求めたセンサー感度Bに乗算してセンサー感度を補正し、角速度補正部15は該補正したセンサー感度をジャイロセンサーが出力する角速度ωgyroに乗算して角速度を補正し、補正された角速度ωgyro′を出力する。
なお、第1感度算出部11の補正方法により算出するセンサー感度は車両のピッチ角度が大きいと誤差が大きくなるため、感度誤差Serrorを第2感度算出部12より求めたセンサー感度Bに乗算してセンサー感度を補正し、この補正したセンサー感度を使用して角速度を補正する。
【0016】
(B)第1感度算出部
第1感度算出部11は、図14に示す従来と同一の構成を採用することができる。すなわち、第1感度算出部11は、自律航法センサー1、位置・方位算出部2、GPS受信機3、メモリコントローラ4、RAM 5、プロセッサ6により構成され、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、補正されたセンサー感度Aを出力する。
【0017】
(C)第2感度算出部
図2は第2感度算出部12の構成図であり、加速度センサー21、車速センサー(距離センサー)22、GPS受信機23および感度算出本体部24を備えている。この感度算出本体部24は加速度センサー21、車速センサー22、GPS受信機23の出力値を用いて、車両に搭載される角速度センサー(図示せず)の感度Bを補正して出力する。ただし、加速度センサー21および角速度センサーは図3にGYACとして示すように一体に同一基板上に搭載され共に同一傾斜角で車両CARの内部に設置されている。
【0018】
(a)加速度傾斜角算出部
感度算出本体部24において、加速度傾斜角算出部31は加速度センサー21の出力信号と車速センサー22から出力する車速パルスを用いて加速度センサー(角速度センサー)の水平面に対する加速度傾斜角θaccを計算するとともに、加速度傾斜角θaccの信頼性の有無を示す加速度傾斜角信頼フラグFaccをフラグ保持部31aに保存するようになっている。
すなわち、加速度傾斜角算出部31は、加速度センサー21より出力される加速度Aacc、車速センサー22より出力される車速Vcarの単位時間当たりの変化量から求められる加速度Acar、および重力加速度Gに基づいて、水平方向に対する加速度センサー21の傾斜角を表した加速度傾斜角θaccを算出する。図4(A),(B)に示すように、加速度センサー21より出力される加速度Aaccは、加速度センサー21が水平方向に対して傾斜している分だけ重力加速度Gの影響を受け、重力加速度Gの正弦成分G・sin(θacc)の分だけ、車速変化による加速度Acarよりも値が小さくなる。つまり、
G・sin(θacc)=Acar−Aacc
の関係が成り立つ。これにより、加速度傾斜角θaccは、図3の(1)に示すように
θacc=sin-1 [(Acar−Aacc)/G] (4)
なる演算によって求めることができる。
また、加速度傾斜角算出部31は車速センサー22より出力される車速Vcarに基づいて加速度傾斜角θaccの信頼性を判定する。車速Vcarが遅い状況では路面の振動の影響を受けやすいので、算出される加速度傾斜角θaccの信頼性が低下する。そこで、加速度傾斜角算出部31は、例えば車速Vcarが10km/h以上かどうかを判定し、10km/h以上のときは加速度傾斜角θaccの信頼性があると判断し、加速度傾斜角信頼フラグFaccをオンし、10km/hより遅いときは加速度傾斜角θaccの信頼性がないと判断し、加速度傾斜角信頼フラグFaccをオフする。加速度傾斜角算出部31は、加速度傾斜角信頼フラグFaccがオンのときにのみ、加速度傾斜角θaccを算出して出力する。
図5は加速度傾斜角算出部31の加速度傾斜角算出の処理フローであり、ステップS11〜S14により加速度傾斜角θaccが計算されて出力される。
【0019】
(b)GPS傾斜角算出部
GPS傾斜角算出部32は、GPS受信機23で複数のGPS衛星(図示せず)から受信する電波のドップラー効果による周波数の変化に基づいて、水平面に対する車両の傾斜角を表したGPS傾斜角θgpsを算出する。具体的には、GPS傾斜角算出部32は、三次元測位が可能であるか判断し、可能であれば図3の(2)に示すように、緯度方向(X方向)、経度方向(Y方向)および高さ方向(Z方向)の3次元測位処理を行って、各方向の速度成分Xv,Yv,Zvに基づいて次式
θgps=tan-1[Zv/√(Xv2+Yv2)] (5)
によりGPS傾斜角θgpsを求める。また、GPS傾斜角算出部32は、GPS傾斜角θgpsの信頼性の判断を行い、信頼性があると判断すればGPS傾斜角フラグFgpsをオンし、信頼性がないと判断すれば該フラグFgpsをオフして保持部32aに保存する。GPS傾斜角θgpsの信頼性の判断は以下のように行なう。
車速が遅い状況では、算出されるGPS傾斜角θgpsの精度が上がらない。また、GPS傾斜角θgpsを求める演算式から分かるように、GPS傾斜角θgpsを求める際には垂直速度Zvを水平速度√(Xv2+Yv2)により除算する必要があるので、車速がゼロの停車時はGPS傾斜角θgpsを求めることができない。そこで、GPS傾斜角算出部32は、例えばGPS速度Vgpsが10km/h以上かどうかを判定し、10km/h以上のときはGPS傾斜角θgpsの信頼性があると判断し、10km/hより遅いときはGPS傾斜角θgpsの信頼性がないと判断する。
また、GPS傾斜角算出部32は、実際に算出したGPS傾斜角θgpsを取得し、当該GPS傾斜角θgpsの絶対値が所定値以下か否かでGPS傾斜角θgpsの信頼性を判定する。日本の道路には10度より大きい傾斜は存在しない。そこで、GPS傾斜角算出部32は、例えばGPS傾斜角θgpsが10度以下かどうかを判定し、10度以下のときはGPS傾斜角θgpsの信頼性があると判断し、10度より大きいときはGPS傾斜角θgpsの信頼性がないと判断する。
さらに、GPS傾斜角算出部32は、実際に算出したGPS傾斜角θgpsを取得し、当該GPS傾斜角θgpsの標準偏差が所定値以下か否かでGPS傾斜角θgpsの信頼性を判定する。例えば、GPS傾斜角θgpsの標準偏差が3度以下であれば、算出したGPS傾斜角θgpsの値にバラツキは少ないと考えられるので、GPS傾斜角θgpsの信頼性があると判断する。一方、標準偏差が3度より大きいときはGPS傾斜角θgpsの信頼性がないと判断する。GPS傾斜角算出部32は、少なくとも1つの条件を満たさないときはGPS傾斜θgpsの信頼性がないと判断する。
GPS傾斜角算出部32は、GPS傾斜角θgpsの信頼性があると判断したときは、内部に持つGPS傾斜角信頼フラグFgpsをオンし、GPS傾斜角θgpsの信頼性がないと判断したときは該フラグFgpsをオフする。
図6はGPS傾斜角算出部32のGPS傾斜角算出処理フローであり、ステップS21〜S26によりGPS傾斜角加速度傾斜角θgpsの計算及びその信頼性が判定される。
【0020】
(c)センサー傾斜角算出部
センサー傾斜角算出部33は、加速度傾斜角信頼度フラグFaccがオンであり、かつ、GPS傾斜角信頼度フラグFgpsが共にオンのとき、すなわち、加速度傾斜角θaccとGPS傾斜角θgpsが共に信頼性があるとき、該加速度傾斜角θacc及びGPS傾斜角θgpsに基づいて、車両の走行方向に対する角速度センサーの傾斜角を表したセンサー傾斜角θsensorを算出する。具体的には、センサー傾斜角算出部33は、図3の(3)に示すように、次式
θsensor=θacc−θgps (6)
によりセンサー傾斜角θsensorを算出し、記憶部33bに記憶すると共に車両傾斜角算出部34に出力する。
また、センサー傾斜角算出部33は、加速度傾斜角θaccおよびGPS傾斜角θgpsの少なくとも一方が信頼性なしであれば、センサー傾斜角記憶部33bにセンサー傾斜角θsensorの過去の算出値が記憶されているか調べ、記憶されていれば、当該センサー傾斜角記憶部33bに記憶されているセンサー傾斜角θsensorを読み出して車両傾斜角算出部34に出力する。
更に、センサー傾斜角算出部33は、センサー傾斜角θsensorを出力したときはセンサー傾斜角θsensorの信頼性があると判断してセンサー傾斜角信頼フラグFsensorをオンし、センサー傾斜角θsensorが出力されないときはセンサー傾斜角θsensorの信頼性がないと判断して該フラグをオフしてフラグ保存部33aに保存する。
図7はセンサー傾斜角算出部33のセンサー傾斜角算出処理フローであり、ステップS31〜S37によりセンサー傾斜角θsensorの計算及びその信頼性が判定される。
【0021】
(d)車両傾斜角算出部
車両傾斜角算出部34は、加速度傾斜角信頼度フラグFaccがオンであり、かつ、センサー傾斜角信頼度フラグFsensorがオンのとき、すなわち、加速度傾斜角θaccとセンサー傾斜角θsensorが共に信頼性があるとき、該加速度傾斜角θacc及びセンサー傾斜角θsensorに基づいて、車両の水平面に対する傾斜角(車両傾斜角)θcarを算出する。具体的には、車両傾斜角算出部34は、図3の(4)に示すように、次式
θcar=θacc−θsensor (7)
によって車両傾斜角θcarを算出して出力する。車両傾斜角算出部34は、加速度傾斜角θaccとセンサー傾斜角θsensorの一方の信頼性がないときには車両傾斜角θcarを算出しない。又、車両傾斜角算出部34は、車両傾斜角θcarを算出したか否かで車両傾斜角θcarの信頼性を判定する。すなわち、車両傾斜角算出部34は、車両傾斜角θcarを算出して出力すれば該車両傾斜角θcarに信頼性があると判定して車両傾斜角信頼フラグFcarをオンし、車両傾斜角θcarを出力しないときには、信頼性がないと判定して車両傾斜角信頼フラグFcarをオフしてフラグ保存部34aに保存する。
図8は車両傾斜角算出部34の車両傾斜角算出処理フローであり、ステップS41〜S44により車両傾斜角θcarの計算及びその信頼性が判定される。
【0022】
(e)感度算出部
感度算出部35は、センサー傾斜角θsensorと車両の水平面に対する傾斜角である車両傾斜角θcarを用いて次式、
B=1/cos(θsensor+θcar) (8)
によりセンサー感度Bを計算して出力する。
図9は感度算出部35の感度算出処理フローである。感度算出部35は、ステップS61,S62においてセンサー傾斜角θsensorの信頼性があり(センサー傾斜角信頼フラグFsensorがオン)、かつ、車両傾斜角θcarの信頼性がある(車両傾斜角信頼フラグFcarがオン)と判定したとき、(8)式によりセンサー感度Bを計算して出力する(ステップS63)。一方、センサー傾斜角θsensorの信頼性はあるが、車両傾斜角θcarの信頼性がなければ、感度算出部35はセンサー傾斜角θsensorに基づいて次式
B=1/cos(θsensor) (8)′
によりセンサー感度Bを計算して出力する(ステップS64)。なお、センサー傾斜角θsensorおよび車両傾斜角θcarの両方とも信頼性がなければ、B=1として角速度を補正しない(ステップS65)。
以上より、第2感度算出部12 は、(8)式より、車両傾斜角(車両ピッチ角)及びセンサー傾斜角(センサー取り付けピッチ角)に起因する角速度センサーの感度を補正することができる。また、車両傾斜角(車両ピッチ角)に信頼性がない場合であっても(8)′式により、角速度センサーの感度を補正することができる。ただし、第2感度算出部12は、角速度センサーの感度誤差に起因する角速度センサーの感度を補正することができない。
【0023】
(D)角速度補正装置の詳細な構成
図10は角速度補正装置の詳細な構成図であり、図1と同一部分には同一符号を付している。図1と異なる点は、(1)車両の加速度ωgyroを検出して角速度補正部15に入力する加速度センサー(ジャイロセンサー)25を設けた点、(2)第2誤差算出部12より感度誤差算出部13へ感度Bに加えて、車両傾斜角(車両のピッチ角)θcar、加速度傾斜角信頼フラグFaccとGPS傾斜角信頼フラグFgpsを入力している点、(3)第2誤差算出部12より感度補正部14へ感度Bを入力している点である。
図11は図10の角速度補正装置の角速度補正処理フローである。
第1感度算出部11はジャイロセンサー25の感度Aを第1の方法により算出して感度誤差算出部13に入力する(ステップ101)。このジャイロセンサーの感度Aはセンサー取り付け面の水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)およびジャイロセンサーの感度誤差Serrorに依存し、以下の関係式
A∝g(θsensor+θcar)・Serror (9)
が成立する。
また、第2感度算出部12は、ジャイロセンサー25の感度Bを第2の方法により算出して感度誤差算出部13に入力すると共に、車両傾斜角(車両のピッチ角)θcar、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsを感度誤差算出部13に入力する(ステップ102)。センサー感度Bはセンサー取り付け面の水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)のみに依存し、以下の関係式
B∝g(θsensor+θcar) (10)
が成立する。
感度誤差算出部13は、車両傾斜角(車両のピッチ角)θcarの絶対値が1deg(1度)以下であるかチェックし、1degより大きければ誤差感度を計算せずステップ101に戻る。これは、車両ピッチ角θcarが0度あるいはそれに近いとき、ジャイロセンサー25の誤差要因から該車両ピッチ角を除くことができるからである。例えば、車両ピッチ角θcarが0度であれば、(9)、(10)式におけるθcarを無視でき、第1、第2の方法における車両ピッチ角の相違に起因する計算誤差をなくすことができるからである。
車両傾斜角(車両のピッチ角)θcarの絶対値が1deg(1度)以下であれば、感度誤差算出部13は、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsが共にオンしているか判断し(ステップ104)、いずれかがオフしていれば、センサー感度A、Bの信頼度がないものとして誤差感度を計算せずステップ101に戻る。
一方、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsが共にオンしていれば、感度誤差算出部13は感度A、Bの信頼度が共に高いと判断し、センサー感度Aとセンサー感度Bの比η(=A/B)を計算し、ηを感度誤差として感度補正部14に入力する(ステップ105)。ηは(9)、(10)式より明らかなように感度誤差Serrorに応じた値である。したがって、感度が標準値の25mv/(deg/s)と一致する場合に感度補正値ηが1になるように調整することにより感度誤差Serrorを計算することができる。
感度補正部14は感度誤差ηが入力されると、該感度誤差Serrorを第2感度算出部12により求めたセンサー感度Bに乗算してセンサー感度を補正する(ステップ106)。なお、センサー感度Bは(8)、(8)′式より求まる。すなわち、感度補正部14は次式によりジャイロセンサー25のセンサー感度を補正する。
センサー感度={1/(θsensor+θcar)}×Serror (11)
しかる後、角速度補正部15はジャイロセンサー25より入力する角速度ωgyroを次式
ωgyro′=ωgyro×{1/(θsensor+θcar)}×Serror (12)
により補正し、該補正した角速度ωgyro′を出力する(ステップ107)。
【0024】
(E)道路傾斜角度が地図に含まれている場合の変形例
(a) 第1変形例
ナビゲーション装置の地図情報に道路傾斜角度θroadが含まれている場合には、(7)式により車両傾斜角θcarを算出する必要はない。すなわち、車両が位置する地点の道路傾斜角度θroadを車両傾斜角θcarとして採用することができ、次式
センサー感度={1/(θsensor+θroad)}×Serror (13)
によりセンサー感度を補正することができ、又、次式
ωgyro′=ωgyro×{1/(θsensor+θroad)}×Serror (14)
により角速度センサーの角速度を補正することができる。
第1変形例を採用する場合には、図2の車両傾斜角算出部34に替えて、自車位置に基づいてナビゲーション装置の地図データベースから道路傾斜角度θroadを取得する道路傾斜角度取得部を感度算出本体部24に設ける。
(b) 第2変形例
また、第1感度算出部11が算出するセンサー感度Aと道路傾斜角度θroadを用いることにより、センサー傾斜角θsensorを求めることができ、GPSが受信できない場所でも角速度センサーの角速度の補正が可能になる。
すなわち、(11)式により求まるセンサー感度は第1感度算出部11が算出するセンサー感度Aと等しいから、次式
A={1/(θsensor+θroad)}×Serror (15)
が成立する。なお、(3)式よりA= GPS方位差/ジャイロ方位差である。したがって、2つのセンサー感度A1、A2を用いて、以下の連立方程式が得られる。
A1= {1/cos(θsensor+θroad1)}×Serror1 (16)
A2= {1/cos(θsensor+θroad2)}×Serror2 (17)
ここで、A1とA2の計算間隔が短時間であれば、Serror1=Serror2とみなせる。したがって、(17)式を(16)式に代入して整理すると、以下の式が得られる。
A1×cos(θsensor+θroad1)=A2×cos(θsensor+θroad2) (18)
この(18)式をθsensorについて解くことによりセンサー取り付け角が得られる。さらに、θsensorを(16)式又は(18)式に代入して、ジャイロセンサーの感度補正値が得られる。そして、計算したセンサー取り付け角、ジャイロセンサー感度誤差を保存することで、地図データベースの道路傾斜角を利用して、車両ピッチ角を考慮した、ジャイロ角速度の補正が可能となる。
第2の変形例を採用する場合には、上記の手順でθsensorを算出するθsensor算出部を感度算出本体部24に更に設け、GPSの受信可否に基づいてセンサー傾斜角算出部33の出力とθsensor算出部の出力を切り換えて感度算出部35に入力するように構成する。
【0025】
(F) ナビゲーション装置
図12は図10の角速度補正装置を適用したナビゲーション装置の構成図である。地図記録媒体(CD-ROM、DVDなど)51には地図データが記録されており、必要に応じて読み取られるようになっている。操作部52はナビゲーション装置本体50を操作するものでリモコン、操作用のハードキーなどを有している。GPS受信機53はGPS衛星から送られてくる位置情報を受信して車両の絶対的現在位置を測定する。自律航法センサー54は、加速度センサー54a、角速度センサー54b、車速センサー54cを備え、ナビゲーション装置本体50はこれら出力信号を用いて自律的に車両の現在位置を推定できるようになっている。ナビゲーション装置50はGPS受信機と自律航法センサーを併用し、適宜マップマッチングを行なって車両現在位置を推定する。
タッチパネル式ディスプレイ装置55はナビゲーション装置50からの指示に従って車両周辺地図、交差点拡大図、その他の案内情報、メニュー等を表示する。また、タッチパネル式ディスプレイ装置55は、スクリーンに表示したソフトキーが押下されたとき所定のコマンドをナビゲーション装置50に入力するようになっている。オーディオ部56は交差点に接近したとき、交差点での案内音声を出力する。
ナビゲーション装置50において、角速度補正装置61は図10の構成を備え、補正された角速度ωgyro′を出力し、走行距離補正部62は次式
L′=L×cosθcar
により車両走行距離を補正して出力する。地図バッファ71は地図記録媒体から読み取った地図データを保存し、制御部72は各種情報、コマンドに基づいて、(1)車両の現在位置の計算制御、(2)車両周辺の地図データをバッファに読み出す地図読み出し制御、(3)誘導経路探索制御、(4)マップマッチング制御、(5)交差点案内制御等を実行する。地図描画部73は地図バッファ71から読み出された地図データを用いて地図画像を生成してVRAM 74に書込み、画像読み出し部75は制御部72からの指示に従ってVRAM 74から所定の画像部分を切り取って画像合成部76に入力する。
誘導経路メモリ77は、制御部72により探索された目的地までの誘導経路情報、すなわち、誘導経路を構成する全ノードの位置データを出発地から目的地まで通行順に記録する。誘導経路描画部78は誘導経路情報を用いて誘導経路画像を発生して画像合成部76に入力し、描画地図上に強調表示する。操作画面発生部79は各種メニュー画面(操作画面)を生成して画像合成部76に入力し、マーク発生部80は車両位置マークやカーソル等の各種マークを生成して画像合成部76に入力する。交差点案内部81は接近中の交差点における案内をディスプレイ画像及び音声で行う。すなわち、実際の経路誘導時に、自車が接近中の交差点より所定距離内に接近した時、該交差点案内図(交差点拡大図、進行方向矢印等)をディスプレイ画面に表示すると共に進行方向を音声で案内する。画像合成部76は、VRAM 74から読み出した地図画像に各種マークや誘導経路画像、交差点拡大図を適宜重ね合わせてディスプレイ画面に表示する。
制御部72の位置計算機能部は、走行距離が一定値L0になる毎に角速度補正装置60から入力する角速度ωgyro′を用いて車両の相対回転角度θiを計算し、次式
Xi=L0×cosθi
Yi=L0×sinθi
により各軸方向の相対走行距離Xi、Yiを計算する。しかる後、これらθi、Xi、Yiを用いて次式
θ=θ+θi
X =X+Xi
Y =Y+Yi
により、それまでの絶対車両方位θ、各軸位置X、Yを更新する。
【0026】
(G)本発明の効果
図13は、立体駐車場内および立体駐車場脱出後の自車位置を示す図であり、(A)は第2感度算出部12から出力される感度補正値Bのみを用いて角速度補正した場合の例、(B)は本実施形態を適用した場合の例を示している。図13(A)に示す例において、ジャイロセンサーの感度は標準値の25mV/(deg/s)からかなりずれており、約23.5mV/(deg/s)である。この23.5mV/(deg/s)は標準値の25mV/(deg/s)に対して+6%のジャイロ角速度誤差に相当する。このため、駐車場内で自車位置精度が明らかに悪化し、脱出時の方位ズレにもつながっている。図の例では脱出時に40degの方位ズレが生じている。
これに対しては、本実施例によれば、ジャイロ感度の感度誤差を補正し、しかも、駐車場内でも角速度補正及び距離補正を行うため、駐車場内の自車位置精度が大幅に改善し、脱出時の方位ズレがほぼ0となる。すなわち、自車位置精度の大幅な改善が可能になる。
以上実施例に従って説明したが、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその精神、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。
【図面の簡単な説明】
【0027】
【図1】角速度補正装置の全体構成図である。
【図2】第2感度算出部の構成図である。
【図3】車両傾斜状態及び各傾斜角演算式の説明図である。
【図4】加速度傾斜角θaccの演算方法説明図である。
【図5】加速度傾斜角算出の処理フローである。
【図6】GPS傾斜角算出処理フローである。
【図7】センサー傾斜角算出処理フローである。
【図8】車両傾斜角算出処理フローである。
【図9】感度補正処理フローである。
【図10】角速度補正装置の詳細な構成図である。
【図11】角速度補正装置の角速度補正処理フローである。
【図12】角速度補正装置を適用したナビゲーション装置の構成図である。
【図13】立体駐車場内および立体駐車場脱出後の自車位置を示す図である。
【図14】従来技術の角速度補正装置の構成図である。
【図15】走行履歴データの例を示すイメージ図である。
【符号の説明】
【0028】
11 第1感度算出部
12 第2感度算出部
13 感度誤差算出部
14 感度補正部
15 角速度補正部
25 ジャイロセンサー
【技術分野】
【0001】
本発明は角速度補正装置及びその補正方法並びにナビゲーション装置に関わり、特に、ジャイロセンサーが出力する角速度信号の誤差(センサー取り付けピッチ角、車両ピッチ角、センサーの感度誤差等に起因する角速度誤差)を補正する角速度補正装置及びその補正方法及び並びにナビゲーション装置に関する。
【背景技術】
【0002】
一般に、車載用のナビゲーション装置では、自立航法センサーやGPS(Global Positioning System)受信機などを用いて車両の現在位置を検出し、その近傍の地図データを記録媒体から読み出して画面上に表示する。そして、画面上の所定箇所に自車位置を示す自車位置マークを重ね合わせて表示することにより、車両が現在どこを走行しているのかを一目で分かるようにしている。
自立航法センサーは、所定走行距離毎に1個のパルスを出力して車両の移動距離を検出する車速センサー(距離センサー)と、車両の回転角度(移動方位)を検出する振動ジャイロ等の角速度センサーとを含んでいる。自立航法センサーは、これらの車速センサーおよび角速度センサーによって車両の相対位置および相対方位を検出するようになっている。ここで、角速度センサーは、角速度を検出しようとする平面が、実際に角速度センサーが回転する平面と平行である必要がある。両者の平面が互いに平行でなくなると、角速度センサーの出力感度が本来の感度からずれてしまう。
一般的に、角速度センサーは、これを水平に設置したときの出力感度で正しく進行方向が判断できるように調整してある。そのため、角速度センサーの角度が水平でなくなった場合には、角速度センサーに加わる角速度と出力感度との関係が変化して、車両の進行方向を正しく検出することができなくなってしまう。そこで、角速度センサーの水平方向に対する傾斜分の感度補正を行うことが必要になる。
【0003】
従来、この感度補正は、以下のようにして行っていた。すなわち、GPS衛星から電波を良好に受信できているときに、GPS受信機により絶対方位として得られるGPS方位と、角速度センサーにより得られる角速度から計算されるジャイロ方位とを比較し、角速度
がGPS方位の変化量と一致するように、角速度センサーの感度を補正し、該補正された感度を用いて角速度補正をしている(特許文献1)。
図14はかかる従来技術の角速度補正装置の構成図であり、自立航法センサー1、位置・方位演算部2、GPS受信機3、メモリコントローラ4、RAM(走行履歴記憶部)5及びプロセッサ6を備えて構成されている。
自立航法センサー1は、車両の現在位置を測定するためのものであり、距離センサー(車速センサー)1aと振動ジャイロ等の角速度センサー(相対方位センサー)1bとを含んでいる。距離センサー1aは、一定時間ごとに1個のパルスを出力して車両の移動距離を検出する。角速度センサー1bは、車両の走行に伴って変化する角速度を検出し、検出した角速度を積分して一定時間ごとに車両の回転角度(移動方位)を出力する。自立航法センサー1は、これらの距離センサー1a及び角速度センサー1bによって車両の相対位置および相対方位を一定時間ごとに検出し、その情報を逐次出力する。
位置・方位演算部2は、自立航法センサー1から出力される車両の相対位置および相対方位の情報に基づいて、絶対的な車両位置(推定車両位置)および車両方位を計算し、一定時間ごとに出力する。以下では、このように自立航法センサー1の出力に基づき計算した絶対位置および絶対方位をそれぞれジャイロ位置、ジャイロ方位と呼ぶ。なお、角速度センサー1bおよび位置・方位演算部2によって方位センサーが構成される。
【0004】
GPS受信機3も車両の現在位置を測定するためのものであり、図示しない複数のGPS衛星から送られてくる電波を図示しないGPSアンテナで受信し、3次元測位処理あるいは2次元測位処理を行って車両の絶対位置および絶対方位を自律航法センサー1と同じ一定時間ごとに計算する(車両方位は、現時点における自車位置と1サンプリング時間ΔT前の自車位置とに基づいて計算する)。そして、これらの計算した車両の絶対位置および絶対方位の情報を、測位時刻と共に逐次出力する。以下では、GPS受信機3の出力に基づき計算した絶対位置および絶対方位をそれぞれGPS位置、GPS方位と呼ぶ。
メモリコントローラ4は、位置・方位演算部2により求められたジャイロ位置およびジャイロ方位を、自立航法センサー1による走行履歴のデータとしてRAM5に順次格納する。また、GPS受信機3により求められたGPS位置及びGPS方位を、GPS受信機3による走行履歴のデータとしてRAM5に順次格納する。このメモリコントローラ4は又、プロセッサ6からの要求に応じて、RAM5から各走行履歴のデータを読み出してプロセッサ6に供給する。
【0005】
図15は、RAM5に格納される走行履歴データの例を示すイメージ図であり、図15(A)はGPS受信機3による走行履歴を、図15(B)は自立航法センサー1による走行履歴を示している。図15(A)において、黒丸は一定時間ごとに(例えば1秒おき)に出力されるGPS位置を示し、矢印は一定時間ごとに(例えば1秒おき)に出力されるGPS方位を示す。GPS方位はθgpsn (nは1,2,・・・Nで、Nは任意の整数)の記号で表している。
図15(B)において、白丸は一定時間ごとに(例えば1秒おき)に出力されるジャイロ位置を示し、矢印は一定時間ごとに(例えば1秒おき)に出力されるジャイロ方位を示す。ジャイロ方位はθgyrn (nは1,2,・・・Nで、Nは任意の整数)の記号で表している。
図15は車両が交差点を左折しているときに、GPS方位は比較的正確な値を示しているのに対し、角速度センサー1bがセンサー感度に誤差が含まれているためにジャイロ方位が正確な値からずれている様子を示している。
プロセッサ6は、その機能構成として、一次サンプリング部6a、二次サンプリング部6b、感度サンプル演算部6c及び統計処理部6dを備えている。一次サンプリング部6aは、一定時間ごとにRAM5に格納されたジャイロ方位θgyrn(第1の方位信号)およびGPS方位θgpsn(第2の方位信号)の中から、以下の第1条件(1)〜(3)を満たすときに出力された方位信号のみを選択的に抽出する。
第1条件は、
(1)角速度センサー1bを搭載している車両の速度がしきい値(例えば、10[km/h])以上であること、
(2)GPSの信頼度が高いこと、
(3)曲率(または方位変化率)が低いこと,
である。
(1)の条件は、車速が低いときはGPS方位の誤差が大きくなることが多いためである。車速は、距離センサー1aの出力から求めることが可能である。すなわち、RAM5に走行履歴として格納されている連続するジャイロ位置間の距離をその間の時間(1秒)で割ることによって求めることができる。
(2)の条件は、GPSの信頼度が低いとGPS方位の誤差が大きくなることが多いためである。自立航法センサー1により得られるある区間の走行距離(自立航法走行距離)と、GPS受信機3により得られる上記ある区間に相当する区間の距離(GPS測位点間距離)との差がしきい値以下であるときに、GPS信頼度は高いと判定することができる。
(3)の条件は、曲率(方位変化率)が高いときはGPS方位の誤差が大きくなることが多いためである。曲率に関しては、例えば、角速度センサー1により得られるジャイロ方位の過去一定期間(例えば過去30m)における変化量がしきい値(例えば5度)以下であることが条件となる。
以上の条件に従って抽出されたデータを、以下では「一次サンプル」と呼ぶ。以上により、位置・方位演算部2により一定時間毎に求められるジャイロ方位とGPS受信機3により一定時間毎に求められるGPS方位とを全てRAM5に一旦格納し、その中から上記条件に合致するものが一次サンプルとして抽出される。
【0006】
二次サンプリング部6bは、一次サンプリング部6aによって抽出されたジャイロ方位及びGPS方位の中から、以下に述べる第2の条件(4)〜(5)を満たすもののみを選択的に抽出する。
(4)後述する第1の方位変化角度及び第2の方位変化角度を求める際に対象とする2つの一次サンプルの時間差がしきい値以下であること。すなわち、現在の一次サンプルNから対象とする一次サンプルi(iはN−1,N−2,・・・)までの時間がしきい値(例えば、60秒)以下であること。演算対象とするサンプル間の時間間隔が長いと、角速度センサー1bのゼロ点誤差に起因するジャイロ方位の誤差が大きくなってしまうからである。ゼロ点誤差とは、角速度センサー1bの出力電圧値が周辺温度の影響によって変動し、基準電圧値がゼロ点の真の電圧値から外れてしまうことを言う。
(5)現在の一次サンプルNと対象とする一次サンプルi(iはN−1,N−2,・・・)とのGPS変化角度がしきい値範囲内であること。ここで、GPS変化角度は以下の次式
GPS変化角度=θgpsN−θgpsi (1)
により計算する。GPS変化角度が小さすぎたり、大きすぎたりすると、角速度センサー1bの感度誤差が顕在化されず、当該感度誤差を演算によって正しく求めることが困難になるからである。
以上の条件に従って抽出されたデータを、以下では「二次サンプル」と呼ぶ。感度サンプル演算部6cは、二次サンプリング部6bによって抽出されたジャイロ方位同士の差分を表す第1の方位変化角度(ジャイロ変化角度)と、二次サンプリング部6bによって抽出されたGPS方位同士の差分を表す第2の方位変化角度(GPS変化角度)とから複数の感度サンプル(感度の瞬時値)を算出する。
【0007】
上述したように、GPS変化角度は(1)式によって求める。一方、ジャイロ変化角度は、次式
ジャイロ変化角度=θgyrN−θgyri (2)
により計算する。さらに、角速度センサーの感度サンプルは、次式
感度サンプル=GPS変化角度/ジャイロ変化角度 (3)
により計算する。ここでは、ジャイロ変化角度及びGPS変化角度がそれぞれ複数求められており、それぞれについて(3)式の演算を行うことにより、複数の感度サンプルを求める。
統計処理部6dは、感度サンプル演算部6cによって求められた複数の感度サンプルを統計処理することによって、角速度センサー1bのセンサー感度を算出する。例えば、抽出された複数の感度サンプルの移動平均値をセンサー感度Aとして出力する。なお、平均値からの差が大きい感度サンプル(例えば差が15%以上の感度サンプル)を異常値とみなして除外し、再度平均値を計算することにより、演算精度を上げることが可能である。なお、(3)式より、センサー感度Aは、該センサー感度Aによりを角速度検出センサーから得られたセンサー方位を補正したとき、補正されたセンサー方位がGPS電波より得られるGPS方位に一致するように決定される。
以上によりセンサー感度Aを求め、角速度補正部7は該センサー感度Aを用いて次式
ωgyro′=ωgyro×A
により、角速度センサー1bより出力する角速度ωgyroを補正する。
【0008】
又、本願出願人が平成19年3月8日付けで日本特許庁に出願済みの角速度補正装置(特許文献2)では、以下の(1)〜(5)のステップによりセンサー感度Bを算出して角速度を補正する。すなわち、
(1)速度センサー(距離センサー)と加速度センサーとの出力信号を用いて水平面に対するジャイロ/加速度センサーの傾斜角(加速度傾斜角)θaccと、GPS受信信号を用いて水平線に対する車両の傾斜角(GPS傾斜角)θgpsをそれぞれ測定し、
(2)これら加速度傾斜角θaccとGPS傾斜角θgpsよりセンサー傾斜角θsenser(=θacc−θgps)を求め、また
(3)加速度傾斜角θaccとセンサー傾斜角θsenserを用いて車両傾斜角θcar(=θacc−θsenser)を求め、
(4)センサー傾斜角θsenserと車両傾斜角θcarを用いてセンサー感度Bを次式
1/cos(θsensor+θcar)
により決定し、
(5)該センサー感度Bを用いて角速度補正する。
【特許文献1】特開2006−71474号公報
【特許文献2】特願2007−58131号
【発明の開示】
【発明が解決しようとする課題】
【0009】
ジャイロセンサーが出力する角速度は、以下の(1)〜(3)の誤差要因があり、
(1) 車両に対するセンサーの取り付けピッチ角
(2) 水平面に対する、車両のピッチ角(道路の傾斜角)
(3) ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)
それぞれを考慮した補正を行う必要がある。
図14の従来技術は、GPS電波を受信できないとき、あるいは、車両ピッチ角(車両傾斜角)の変化が大きい立体駐車場を走行しているとき、ジャイロセンサーの感度を計算できず、結果的にジャイロ角速度の補正ができない(上記の誤差要因(2)に対処できない)。このため、方位ズレが発生し、立体駐車場内での自車位置の精度悪化を来たし、また立体駐車場脱出後のマップマッチング処理による実際の走行道路への復帰が遅れる問題がある。
また、特許文献2で提案の技術は、ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)を吸収できない。このため、センサー感度が標準値から大きく外れて、スペックぎりぎりの場合、交差点右左折時などにおいて、曲がりが足りない、または曲がり過ぎとなり、自車位置・方位精度が悪化する。特に、短時間で大きく方位が変化する立体駐車場内で、位置精度の悪化が大きい。例えば、感度の標準値は25mV/(deg/s)で、スペック上の許容範囲は23.25〜26.75 mV/(deg/s)である。これは誤差に換算すると、+7.5〜−6.5%の範囲であり、ジャイロ角速度誤差が生じる。したがって、例えば5階建ての立体駐車場を上って降りてきたとすると、脱出時の自車方位最大誤差θerrorは、
θerror=360deg×5×2×7.5÷100=270deg
となり、脱出後の道路への復帰遅れにつながる。なお、上式中5は5回転、2は上り/下りによるものである。
【0010】
以上から本発明の目的は上記(1)〜(3)の誤差要因に対処できる角速度補正装置及びその補正方法並びにナビゲーション装置を提供することである。
本発明の別の目的は、GPS電波を受信できないときでも、あるいは、車両のピッチ角変化が大きい立体駐車場を走行しているときでも、ジャイロ角速度の補正ができるようにすることである。
本発明の別の目的は、立体駐車場内での自車位置精度を維持でき、また立体駐車場脱出後に速やかに実際の走行道路への復帰を可能にすることである。
【課題を解決するための手段】
【0011】
・角速度補正方法
本発明の第1は、車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正方法であり、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にするステップ、前記第1のセンサー感度補正方法により得られたセンサー感度と前記第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出するステップ、該感度誤差を用いて前記センサーが出力する角速度を補正するステップを有している。
上記の角速度補正方法は、前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正するステップ、該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正するステップを有している。
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、前記第2のセンサー感度補正方法は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算し、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算し、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算し、前記加速度センサーの水平面に対する傾斜角θaccと前記傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算し、
1/cos(θsensor+θcar)
をセンサー感度として出力する。
また、前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、前記第2のセンサー感度補正方法は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして求め、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力する。
前記感度誤差を算出するステップは、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いか監視するステップ、該条件が成立したとき前記感度誤差の算出を行なうステップ、を有している。
【0012】
・角速度補正装置
本発明の第2は、車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正装置であり、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部を有している。
角速度補正装置は、更に、前記算出された感度誤差を前記第2のセンサー感度算出部で求めたセンサー感度に乗算してセンサー感度を補正する感度補正部を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する。
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、前記第2のセンサー感度算出部は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算するセンサー傾斜角計算部、前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
1/cos(θsensor+θcar)
をセンサー感度として出力する感度算出部、を備えている。
また、前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、前記第2のセンサー感度算出部は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして取得する車両傾斜角取得部、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力するセンサー感度算出部を備えている。
角速度補正装置は、更に、GPSの信頼度を判定するGPS信頼度判定部、車両の水平面に対する傾斜角度が水平に近いか監視する監視部を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう。
【0013】
・ナビゲーション装置
本発明の第3は、GPS受信機と角速度センサーと加速度センサーと車速センサーとを備え、これらの出力値を用いて車両の位置および方位を検出する機能を備えたナビゲーション装置であり、前記角速度補正装置と、角速度補正装置により補正された角速度と車速センサーを用いて車両位置を計算する車両位置計算部を備えている
【発明の効果】
【0014】
本発明によれば、
(1) 車両に対するセンサーの取り付けピッチ角
(2) 水平面に対する車両のピッチ角(道路の傾斜角)
(3) ジャイロセンサーの感度誤差(ハードウェア要因等による特性値の個体差)
のそれぞれを考慮した感度補正をすることができ、該補正された感度により角速度を正確に補正することができる。
また、本発明によれば、上記(1)〜(3)の誤差要因を考慮して補正された角速度を用いて自動車位置を正確に推測するナビゲーション装置を提供することができる。
また、本発明によれば、GPS電波を受信できないときでも、あるいは、車両のピッチ角変化が大きい立体駐車場を走行しているときでも、ジャイロ角速度の補正ができ、立体駐車場内での自車位置精度を維持することができ、また、立体駐車場脱出後に速やかに実際の走行道路への復帰ができる。
更に、ジャイロセンサーの感度誤差を自動的に補正できるため、センサーを回転台等に載せて、手動で感度を補正する工程が不要になり、これにより、コスト削減につなげることができる。
【発明を実施するための最良の形態】
【0015】
(A)角速度補正装置の全体構成
図1は角速度補正装置の全体構成図である。角速度補正装置は、車両に取り付けられて該車両の角速度を検出する角速度検出センサー(ジャイロセンサー)の感度を補正し、該補正された感度を用いて角速度を補正する。
角速度補正装置において、第1感度算出部11は、ジャイロセンサーの水平面に対する傾斜角(車両ピッチ角+センサー取り付けピッチ角)に起因するセンサー感度の変化およびジャイロセンサーの感度誤差の両方を考慮して該センサー感度を補正し、補正後の感度Aを出力する。
第2感度算出部12は、ジャイロセンサーの感度誤差を考慮せず、ジャイロセンサーの水平面に対する傾斜角に起因するセンサー感度の変化のみを考慮して該センサー感度を補正し、補正後の感度Bを出力する。
感度誤差算出部13は、第1センサー感度算出部11により得られたセンサー感度Aと第2センサー感度算出部12により得られたセンサー感度Bの比η(=A/B)をセンサーの感度誤差として計算して出力する。第1感度算出部11により補正算出される角速度センサーの感度Aはセンサー取り付け面と水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)およびジャイロセンサーの感度誤差Serrorに依存し、以下の関係式
A∝g(θsensor+θcar)・Serror
が成立する。但し、g( )は傾斜角(θsensor+θcar)より感度を算出する関数である。また、第2感度算出部12により補正算出される角速度センサーの感度Bはセンサー取り付け面と水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)のみに依存し、以下の関係式
B∝g(θsensor+θcar)
が成立する。したがって、センサー感度Aとセンサー感度Bの比η(=A/B)を計算することにより、ηは
η∝Serror
となり、感度誤差Serrorを計算することができる。なお、精度のよい感度誤差Serrorを計算するには、GPSの信頼度が高く、かつ車両ピッチ角(車両傾斜角)が0deg(=0度)付近のときに計算する必要がある。
感度補正部14は、該感度誤差Serrorを第2感度算出部12より求めたセンサー感度Bに乗算してセンサー感度を補正し、角速度補正部15は該補正したセンサー感度をジャイロセンサーが出力する角速度ωgyroに乗算して角速度を補正し、補正された角速度ωgyro′を出力する。
なお、第1感度算出部11の補正方法により算出するセンサー感度は車両のピッチ角度が大きいと誤差が大きくなるため、感度誤差Serrorを第2感度算出部12より求めたセンサー感度Bに乗算してセンサー感度を補正し、この補正したセンサー感度を使用して角速度を補正する。
【0016】
(B)第1感度算出部
第1感度算出部11は、図14に示す従来と同一の構成を採用することができる。すなわち、第1感度算出部11は、自律航法センサー1、位置・方位算出部2、GPS受信機3、メモリコントローラ4、RAM 5、プロセッサ6により構成され、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、補正されたセンサー感度Aを出力する。
【0017】
(C)第2感度算出部
図2は第2感度算出部12の構成図であり、加速度センサー21、車速センサー(距離センサー)22、GPS受信機23および感度算出本体部24を備えている。この感度算出本体部24は加速度センサー21、車速センサー22、GPS受信機23の出力値を用いて、車両に搭載される角速度センサー(図示せず)の感度Bを補正して出力する。ただし、加速度センサー21および角速度センサーは図3にGYACとして示すように一体に同一基板上に搭載され共に同一傾斜角で車両CARの内部に設置されている。
【0018】
(a)加速度傾斜角算出部
感度算出本体部24において、加速度傾斜角算出部31は加速度センサー21の出力信号と車速センサー22から出力する車速パルスを用いて加速度センサー(角速度センサー)の水平面に対する加速度傾斜角θaccを計算するとともに、加速度傾斜角θaccの信頼性の有無を示す加速度傾斜角信頼フラグFaccをフラグ保持部31aに保存するようになっている。
すなわち、加速度傾斜角算出部31は、加速度センサー21より出力される加速度Aacc、車速センサー22より出力される車速Vcarの単位時間当たりの変化量から求められる加速度Acar、および重力加速度Gに基づいて、水平方向に対する加速度センサー21の傾斜角を表した加速度傾斜角θaccを算出する。図4(A),(B)に示すように、加速度センサー21より出力される加速度Aaccは、加速度センサー21が水平方向に対して傾斜している分だけ重力加速度Gの影響を受け、重力加速度Gの正弦成分G・sin(θacc)の分だけ、車速変化による加速度Acarよりも値が小さくなる。つまり、
G・sin(θacc)=Acar−Aacc
の関係が成り立つ。これにより、加速度傾斜角θaccは、図3の(1)に示すように
θacc=sin-1 [(Acar−Aacc)/G] (4)
なる演算によって求めることができる。
また、加速度傾斜角算出部31は車速センサー22より出力される車速Vcarに基づいて加速度傾斜角θaccの信頼性を判定する。車速Vcarが遅い状況では路面の振動の影響を受けやすいので、算出される加速度傾斜角θaccの信頼性が低下する。そこで、加速度傾斜角算出部31は、例えば車速Vcarが10km/h以上かどうかを判定し、10km/h以上のときは加速度傾斜角θaccの信頼性があると判断し、加速度傾斜角信頼フラグFaccをオンし、10km/hより遅いときは加速度傾斜角θaccの信頼性がないと判断し、加速度傾斜角信頼フラグFaccをオフする。加速度傾斜角算出部31は、加速度傾斜角信頼フラグFaccがオンのときにのみ、加速度傾斜角θaccを算出して出力する。
図5は加速度傾斜角算出部31の加速度傾斜角算出の処理フローであり、ステップS11〜S14により加速度傾斜角θaccが計算されて出力される。
【0019】
(b)GPS傾斜角算出部
GPS傾斜角算出部32は、GPS受信機23で複数のGPS衛星(図示せず)から受信する電波のドップラー効果による周波数の変化に基づいて、水平面に対する車両の傾斜角を表したGPS傾斜角θgpsを算出する。具体的には、GPS傾斜角算出部32は、三次元測位が可能であるか判断し、可能であれば図3の(2)に示すように、緯度方向(X方向)、経度方向(Y方向)および高さ方向(Z方向)の3次元測位処理を行って、各方向の速度成分Xv,Yv,Zvに基づいて次式
θgps=tan-1[Zv/√(Xv2+Yv2)] (5)
によりGPS傾斜角θgpsを求める。また、GPS傾斜角算出部32は、GPS傾斜角θgpsの信頼性の判断を行い、信頼性があると判断すればGPS傾斜角フラグFgpsをオンし、信頼性がないと判断すれば該フラグFgpsをオフして保持部32aに保存する。GPS傾斜角θgpsの信頼性の判断は以下のように行なう。
車速が遅い状況では、算出されるGPS傾斜角θgpsの精度が上がらない。また、GPS傾斜角θgpsを求める演算式から分かるように、GPS傾斜角θgpsを求める際には垂直速度Zvを水平速度√(Xv2+Yv2)により除算する必要があるので、車速がゼロの停車時はGPS傾斜角θgpsを求めることができない。そこで、GPS傾斜角算出部32は、例えばGPS速度Vgpsが10km/h以上かどうかを判定し、10km/h以上のときはGPS傾斜角θgpsの信頼性があると判断し、10km/hより遅いときはGPS傾斜角θgpsの信頼性がないと判断する。
また、GPS傾斜角算出部32は、実際に算出したGPS傾斜角θgpsを取得し、当該GPS傾斜角θgpsの絶対値が所定値以下か否かでGPS傾斜角θgpsの信頼性を判定する。日本の道路には10度より大きい傾斜は存在しない。そこで、GPS傾斜角算出部32は、例えばGPS傾斜角θgpsが10度以下かどうかを判定し、10度以下のときはGPS傾斜角θgpsの信頼性があると判断し、10度より大きいときはGPS傾斜角θgpsの信頼性がないと判断する。
さらに、GPS傾斜角算出部32は、実際に算出したGPS傾斜角θgpsを取得し、当該GPS傾斜角θgpsの標準偏差が所定値以下か否かでGPS傾斜角θgpsの信頼性を判定する。例えば、GPS傾斜角θgpsの標準偏差が3度以下であれば、算出したGPS傾斜角θgpsの値にバラツキは少ないと考えられるので、GPS傾斜角θgpsの信頼性があると判断する。一方、標準偏差が3度より大きいときはGPS傾斜角θgpsの信頼性がないと判断する。GPS傾斜角算出部32は、少なくとも1つの条件を満たさないときはGPS傾斜θgpsの信頼性がないと判断する。
GPS傾斜角算出部32は、GPS傾斜角θgpsの信頼性があると判断したときは、内部に持つGPS傾斜角信頼フラグFgpsをオンし、GPS傾斜角θgpsの信頼性がないと判断したときは該フラグFgpsをオフする。
図6はGPS傾斜角算出部32のGPS傾斜角算出処理フローであり、ステップS21〜S26によりGPS傾斜角加速度傾斜角θgpsの計算及びその信頼性が判定される。
【0020】
(c)センサー傾斜角算出部
センサー傾斜角算出部33は、加速度傾斜角信頼度フラグFaccがオンであり、かつ、GPS傾斜角信頼度フラグFgpsが共にオンのとき、すなわち、加速度傾斜角θaccとGPS傾斜角θgpsが共に信頼性があるとき、該加速度傾斜角θacc及びGPS傾斜角θgpsに基づいて、車両の走行方向に対する角速度センサーの傾斜角を表したセンサー傾斜角θsensorを算出する。具体的には、センサー傾斜角算出部33は、図3の(3)に示すように、次式
θsensor=θacc−θgps (6)
によりセンサー傾斜角θsensorを算出し、記憶部33bに記憶すると共に車両傾斜角算出部34に出力する。
また、センサー傾斜角算出部33は、加速度傾斜角θaccおよびGPS傾斜角θgpsの少なくとも一方が信頼性なしであれば、センサー傾斜角記憶部33bにセンサー傾斜角θsensorの過去の算出値が記憶されているか調べ、記憶されていれば、当該センサー傾斜角記憶部33bに記憶されているセンサー傾斜角θsensorを読み出して車両傾斜角算出部34に出力する。
更に、センサー傾斜角算出部33は、センサー傾斜角θsensorを出力したときはセンサー傾斜角θsensorの信頼性があると判断してセンサー傾斜角信頼フラグFsensorをオンし、センサー傾斜角θsensorが出力されないときはセンサー傾斜角θsensorの信頼性がないと判断して該フラグをオフしてフラグ保存部33aに保存する。
図7はセンサー傾斜角算出部33のセンサー傾斜角算出処理フローであり、ステップS31〜S37によりセンサー傾斜角θsensorの計算及びその信頼性が判定される。
【0021】
(d)車両傾斜角算出部
車両傾斜角算出部34は、加速度傾斜角信頼度フラグFaccがオンであり、かつ、センサー傾斜角信頼度フラグFsensorがオンのとき、すなわち、加速度傾斜角θaccとセンサー傾斜角θsensorが共に信頼性があるとき、該加速度傾斜角θacc及びセンサー傾斜角θsensorに基づいて、車両の水平面に対する傾斜角(車両傾斜角)θcarを算出する。具体的には、車両傾斜角算出部34は、図3の(4)に示すように、次式
θcar=θacc−θsensor (7)
によって車両傾斜角θcarを算出して出力する。車両傾斜角算出部34は、加速度傾斜角θaccとセンサー傾斜角θsensorの一方の信頼性がないときには車両傾斜角θcarを算出しない。又、車両傾斜角算出部34は、車両傾斜角θcarを算出したか否かで車両傾斜角θcarの信頼性を判定する。すなわち、車両傾斜角算出部34は、車両傾斜角θcarを算出して出力すれば該車両傾斜角θcarに信頼性があると判定して車両傾斜角信頼フラグFcarをオンし、車両傾斜角θcarを出力しないときには、信頼性がないと判定して車両傾斜角信頼フラグFcarをオフしてフラグ保存部34aに保存する。
図8は車両傾斜角算出部34の車両傾斜角算出処理フローであり、ステップS41〜S44により車両傾斜角θcarの計算及びその信頼性が判定される。
【0022】
(e)感度算出部
感度算出部35は、センサー傾斜角θsensorと車両の水平面に対する傾斜角である車両傾斜角θcarを用いて次式、
B=1/cos(θsensor+θcar) (8)
によりセンサー感度Bを計算して出力する。
図9は感度算出部35の感度算出処理フローである。感度算出部35は、ステップS61,S62においてセンサー傾斜角θsensorの信頼性があり(センサー傾斜角信頼フラグFsensorがオン)、かつ、車両傾斜角θcarの信頼性がある(車両傾斜角信頼フラグFcarがオン)と判定したとき、(8)式によりセンサー感度Bを計算して出力する(ステップS63)。一方、センサー傾斜角θsensorの信頼性はあるが、車両傾斜角θcarの信頼性がなければ、感度算出部35はセンサー傾斜角θsensorに基づいて次式
B=1/cos(θsensor) (8)′
によりセンサー感度Bを計算して出力する(ステップS64)。なお、センサー傾斜角θsensorおよび車両傾斜角θcarの両方とも信頼性がなければ、B=1として角速度を補正しない(ステップS65)。
以上より、第2感度算出部12 は、(8)式より、車両傾斜角(車両ピッチ角)及びセンサー傾斜角(センサー取り付けピッチ角)に起因する角速度センサーの感度を補正することができる。また、車両傾斜角(車両ピッチ角)に信頼性がない場合であっても(8)′式により、角速度センサーの感度を補正することができる。ただし、第2感度算出部12は、角速度センサーの感度誤差に起因する角速度センサーの感度を補正することができない。
【0023】
(D)角速度補正装置の詳細な構成
図10は角速度補正装置の詳細な構成図であり、図1と同一部分には同一符号を付している。図1と異なる点は、(1)車両の加速度ωgyroを検出して角速度補正部15に入力する加速度センサー(ジャイロセンサー)25を設けた点、(2)第2誤差算出部12より感度誤差算出部13へ感度Bに加えて、車両傾斜角(車両のピッチ角)θcar、加速度傾斜角信頼フラグFaccとGPS傾斜角信頼フラグFgpsを入力している点、(3)第2誤差算出部12より感度補正部14へ感度Bを入力している点である。
図11は図10の角速度補正装置の角速度補正処理フローである。
第1感度算出部11はジャイロセンサー25の感度Aを第1の方法により算出して感度誤差算出部13に入力する(ステップ101)。このジャイロセンサーの感度Aはセンサー取り付け面の水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)およびジャイロセンサーの感度誤差Serrorに依存し、以下の関係式
A∝g(θsensor+θcar)・Serror (9)
が成立する。
また、第2感度算出部12は、ジャイロセンサー25の感度Bを第2の方法により算出して感度誤差算出部13に入力すると共に、車両傾斜角(車両のピッチ角)θcar、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsを感度誤差算出部13に入力する(ステップ102)。センサー感度Bはセンサー取り付け面の水平面に対する傾斜角(センサー取り付けピッチ角θsensor+車両のピッチ角θcar)のみに依存し、以下の関係式
B∝g(θsensor+θcar) (10)
が成立する。
感度誤差算出部13は、車両傾斜角(車両のピッチ角)θcarの絶対値が1deg(1度)以下であるかチェックし、1degより大きければ誤差感度を計算せずステップ101に戻る。これは、車両ピッチ角θcarが0度あるいはそれに近いとき、ジャイロセンサー25の誤差要因から該車両ピッチ角を除くことができるからである。例えば、車両ピッチ角θcarが0度であれば、(9)、(10)式におけるθcarを無視でき、第1、第2の方法における車両ピッチ角の相違に起因する計算誤差をなくすことができるからである。
車両傾斜角(車両のピッチ角)θcarの絶対値が1deg(1度)以下であれば、感度誤差算出部13は、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsが共にオンしているか判断し(ステップ104)、いずれかがオフしていれば、センサー感度A、Bの信頼度がないものとして誤差感度を計算せずステップ101に戻る。
一方、加速度傾斜角信頼フラグFacc、GPS傾斜角信頼フラグFgpsが共にオンしていれば、感度誤差算出部13は感度A、Bの信頼度が共に高いと判断し、センサー感度Aとセンサー感度Bの比η(=A/B)を計算し、ηを感度誤差として感度補正部14に入力する(ステップ105)。ηは(9)、(10)式より明らかなように感度誤差Serrorに応じた値である。したがって、感度が標準値の25mv/(deg/s)と一致する場合に感度補正値ηが1になるように調整することにより感度誤差Serrorを計算することができる。
感度補正部14は感度誤差ηが入力されると、該感度誤差Serrorを第2感度算出部12により求めたセンサー感度Bに乗算してセンサー感度を補正する(ステップ106)。なお、センサー感度Bは(8)、(8)′式より求まる。すなわち、感度補正部14は次式によりジャイロセンサー25のセンサー感度を補正する。
センサー感度={1/(θsensor+θcar)}×Serror (11)
しかる後、角速度補正部15はジャイロセンサー25より入力する角速度ωgyroを次式
ωgyro′=ωgyro×{1/(θsensor+θcar)}×Serror (12)
により補正し、該補正した角速度ωgyro′を出力する(ステップ107)。
【0024】
(E)道路傾斜角度が地図に含まれている場合の変形例
(a) 第1変形例
ナビゲーション装置の地図情報に道路傾斜角度θroadが含まれている場合には、(7)式により車両傾斜角θcarを算出する必要はない。すなわち、車両が位置する地点の道路傾斜角度θroadを車両傾斜角θcarとして採用することができ、次式
センサー感度={1/(θsensor+θroad)}×Serror (13)
によりセンサー感度を補正することができ、又、次式
ωgyro′=ωgyro×{1/(θsensor+θroad)}×Serror (14)
により角速度センサーの角速度を補正することができる。
第1変形例を採用する場合には、図2の車両傾斜角算出部34に替えて、自車位置に基づいてナビゲーション装置の地図データベースから道路傾斜角度θroadを取得する道路傾斜角度取得部を感度算出本体部24に設ける。
(b) 第2変形例
また、第1感度算出部11が算出するセンサー感度Aと道路傾斜角度θroadを用いることにより、センサー傾斜角θsensorを求めることができ、GPSが受信できない場所でも角速度センサーの角速度の補正が可能になる。
すなわち、(11)式により求まるセンサー感度は第1感度算出部11が算出するセンサー感度Aと等しいから、次式
A={1/(θsensor+θroad)}×Serror (15)
が成立する。なお、(3)式よりA= GPS方位差/ジャイロ方位差である。したがって、2つのセンサー感度A1、A2を用いて、以下の連立方程式が得られる。
A1= {1/cos(θsensor+θroad1)}×Serror1 (16)
A2= {1/cos(θsensor+θroad2)}×Serror2 (17)
ここで、A1とA2の計算間隔が短時間であれば、Serror1=Serror2とみなせる。したがって、(17)式を(16)式に代入して整理すると、以下の式が得られる。
A1×cos(θsensor+θroad1)=A2×cos(θsensor+θroad2) (18)
この(18)式をθsensorについて解くことによりセンサー取り付け角が得られる。さらに、θsensorを(16)式又は(18)式に代入して、ジャイロセンサーの感度補正値が得られる。そして、計算したセンサー取り付け角、ジャイロセンサー感度誤差を保存することで、地図データベースの道路傾斜角を利用して、車両ピッチ角を考慮した、ジャイロ角速度の補正が可能となる。
第2の変形例を採用する場合には、上記の手順でθsensorを算出するθsensor算出部を感度算出本体部24に更に設け、GPSの受信可否に基づいてセンサー傾斜角算出部33の出力とθsensor算出部の出力を切り換えて感度算出部35に入力するように構成する。
【0025】
(F) ナビゲーション装置
図12は図10の角速度補正装置を適用したナビゲーション装置の構成図である。地図記録媒体(CD-ROM、DVDなど)51には地図データが記録されており、必要に応じて読み取られるようになっている。操作部52はナビゲーション装置本体50を操作するものでリモコン、操作用のハードキーなどを有している。GPS受信機53はGPS衛星から送られてくる位置情報を受信して車両の絶対的現在位置を測定する。自律航法センサー54は、加速度センサー54a、角速度センサー54b、車速センサー54cを備え、ナビゲーション装置本体50はこれら出力信号を用いて自律的に車両の現在位置を推定できるようになっている。ナビゲーション装置50はGPS受信機と自律航法センサーを併用し、適宜マップマッチングを行なって車両現在位置を推定する。
タッチパネル式ディスプレイ装置55はナビゲーション装置50からの指示に従って車両周辺地図、交差点拡大図、その他の案内情報、メニュー等を表示する。また、タッチパネル式ディスプレイ装置55は、スクリーンに表示したソフトキーが押下されたとき所定のコマンドをナビゲーション装置50に入力するようになっている。オーディオ部56は交差点に接近したとき、交差点での案内音声を出力する。
ナビゲーション装置50において、角速度補正装置61は図10の構成を備え、補正された角速度ωgyro′を出力し、走行距離補正部62は次式
L′=L×cosθcar
により車両走行距離を補正して出力する。地図バッファ71は地図記録媒体から読み取った地図データを保存し、制御部72は各種情報、コマンドに基づいて、(1)車両の現在位置の計算制御、(2)車両周辺の地図データをバッファに読み出す地図読み出し制御、(3)誘導経路探索制御、(4)マップマッチング制御、(5)交差点案内制御等を実行する。地図描画部73は地図バッファ71から読み出された地図データを用いて地図画像を生成してVRAM 74に書込み、画像読み出し部75は制御部72からの指示に従ってVRAM 74から所定の画像部分を切り取って画像合成部76に入力する。
誘導経路メモリ77は、制御部72により探索された目的地までの誘導経路情報、すなわち、誘導経路を構成する全ノードの位置データを出発地から目的地まで通行順に記録する。誘導経路描画部78は誘導経路情報を用いて誘導経路画像を発生して画像合成部76に入力し、描画地図上に強調表示する。操作画面発生部79は各種メニュー画面(操作画面)を生成して画像合成部76に入力し、マーク発生部80は車両位置マークやカーソル等の各種マークを生成して画像合成部76に入力する。交差点案内部81は接近中の交差点における案内をディスプレイ画像及び音声で行う。すなわち、実際の経路誘導時に、自車が接近中の交差点より所定距離内に接近した時、該交差点案内図(交差点拡大図、進行方向矢印等)をディスプレイ画面に表示すると共に進行方向を音声で案内する。画像合成部76は、VRAM 74から読み出した地図画像に各種マークや誘導経路画像、交差点拡大図を適宜重ね合わせてディスプレイ画面に表示する。
制御部72の位置計算機能部は、走行距離が一定値L0になる毎に角速度補正装置60から入力する角速度ωgyro′を用いて車両の相対回転角度θiを計算し、次式
Xi=L0×cosθi
Yi=L0×sinθi
により各軸方向の相対走行距離Xi、Yiを計算する。しかる後、これらθi、Xi、Yiを用いて次式
θ=θ+θi
X =X+Xi
Y =Y+Yi
により、それまでの絶対車両方位θ、各軸位置X、Yを更新する。
【0026】
(G)本発明の効果
図13は、立体駐車場内および立体駐車場脱出後の自車位置を示す図であり、(A)は第2感度算出部12から出力される感度補正値Bのみを用いて角速度補正した場合の例、(B)は本実施形態を適用した場合の例を示している。図13(A)に示す例において、ジャイロセンサーの感度は標準値の25mV/(deg/s)からかなりずれており、約23.5mV/(deg/s)である。この23.5mV/(deg/s)は標準値の25mV/(deg/s)に対して+6%のジャイロ角速度誤差に相当する。このため、駐車場内で自車位置精度が明らかに悪化し、脱出時の方位ズレにもつながっている。図の例では脱出時に40degの方位ズレが生じている。
これに対しては、本実施例によれば、ジャイロ感度の感度誤差を補正し、しかも、駐車場内でも角速度補正及び距離補正を行うため、駐車場内の自車位置精度が大幅に改善し、脱出時の方位ズレがほぼ0となる。すなわち、自車位置精度の大幅な改善が可能になる。
以上実施例に従って説明したが、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその精神、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。
【図面の簡単な説明】
【0027】
【図1】角速度補正装置の全体構成図である。
【図2】第2感度算出部の構成図である。
【図3】車両傾斜状態及び各傾斜角演算式の説明図である。
【図4】加速度傾斜角θaccの演算方法説明図である。
【図5】加速度傾斜角算出の処理フローである。
【図6】GPS傾斜角算出処理フローである。
【図7】センサー傾斜角算出処理フローである。
【図8】車両傾斜角算出処理フローである。
【図9】感度補正処理フローである。
【図10】角速度補正装置の詳細な構成図である。
【図11】角速度補正装置の角速度補正処理フローである。
【図12】角速度補正装置を適用したナビゲーション装置の構成図である。
【図13】立体駐車場内および立体駐車場脱出後の自車位置を示す図である。
【図14】従来技術の角速度補正装置の構成図である。
【図15】走行履歴データの例を示すイメージ図である。
【符号の説明】
【0028】
11 第1感度算出部
12 第2感度算出部
13 感度誤差算出部
14 感度補正部
15 角速度補正部
25 ジャイロセンサー
【特許請求の範囲】
【請求項1】
車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正方法において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にし、
前記第1のセンサー感度補正方法により得られたセンサー感度と前記第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出し、
該感度誤差を用いて前記センサーが出力する角速度を補正する、
ことを特徴とする角速度補正方法。
【請求項2】
前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正し、
該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項1記載の角速度補正方法。
【請求項3】
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、
前記第2のセンサー感度補正方法は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算し、該傾斜角を用いて角速度センサーの水平面に対する傾斜角に起因するセンサー感度を補正する、
ことを特徴とする請求項1または2記載の角速度補正方法。
【請求項4】
前記第2のセンサー感度補正方法は、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算し、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角θgpsとから角速度センサーの車両進行方向に対する傾斜角θsensorを計算し、前記加速度センサーの水平面に対する傾斜角θaccと前記傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算し、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項3記載の角速度補正方法。
【請求項5】
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、
前記第2のセンサー感度補正方法は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして求め、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項1または2記載の角速度補正方法。
【請求項6】
GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いか監視し、
該条件が成立したとき前記感度誤差の算出を行なう、
ことを特徴とする請求項1乃至4記載の角速度補正方法。
【請求項7】
車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正装置において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、
角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、
前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、
該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部、
を有することを特徴とする角速度補正装置。
【請求項8】
前記算出された感度誤差を前記第2のセンサー感度算出部で求めたセンサー感度に乗算してセンサー感度を補正する感度補正部、
を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項7記載の角速度補正装置。
【請求項9】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、
該傾斜角を用いて角速度センサーの水平面に対する傾斜角に起因するセンサー感度を補正するセンサー誤差算出部、
を備えることを特徴とする請求項7または8記載の角速度補正装置。
【請求項10】
前記第2のセンサー感度算出部は、更に、
GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対するに対する傾斜角θsensorを計算するセンサー傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
を備え、前記センサー感度算出部は、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項9記載の角速度補正装置。
【請求項11】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして取得する車両傾斜角取得部、
該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力するセンサー感度算出部、
を備えることを特徴とする請求項7または8記載の角速度補正装置。
【請求項12】
GPSの信頼度を判定するGPS信頼度判定部、
車両の水平面に対する傾斜角度が水平に近いか監視する監視部、
を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう、
ことを特徴とする請求項7乃至または10記載の角速度補正装置。
【請求項13】
GPS受信機と角速度センサーと加速度センサーと車速センサーとを備え、これらの出力値を用いて車両の位置および方位を検出する機能を備えたナビゲーション装置において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、
角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、
前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、
該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部、
補正された前記角速度と車速センサー出力を用いて車両位置を計算する車両位置計算部、
を有することを特徴とするナビゲーション装置。
【請求項14】
前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正する感度補正部、
を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項13記載のナビゲーション装置。
【請求項15】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、
該傾斜角を用いて該センサーの水平面に対する傾斜角に起因するセンサー感度を補正するセンサー感度補正部、
を備えることを特徴とする請求項13または14載のナビゲーション装置。
【請求項16】
前記第2のセンサー感度算出部は、更に、
GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算するセンサー傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
を備え、前記センサー感度補正部は、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項15記載のナビゲーション装置。
【請求項17】
GPSの信頼度を判定するGPS信頼度判定部、
車両の水平面に対する傾斜角度が水平に近いか監視する監視部、
を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう、
ことを特徴とする請求項13または16記載のナビゲーション装置。
【請求項1】
車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正方法において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度補正方法と、角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度補正方法を実施可能にし、
前記第1のセンサー感度補正方法により得られたセンサー感度と前記第2のセンサー感度補正方法により得られたセンサー感度の比によりセンサーの感度誤差を算出し、
該感度誤差を用いて前記センサーが出力する角速度を補正する、
ことを特徴とする角速度補正方法。
【請求項2】
前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正し、
該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項1記載の角速度補正方法。
【請求項3】
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、
前記第2のセンサー感度補正方法は、角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算し、該傾斜角を用いて角速度センサーの水平面に対する傾斜角に起因するセンサー感度を補正する、
ことを特徴とする請求項1または2記載の角速度補正方法。
【請求項4】
前記第2のセンサー感度補正方法は、GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算し、前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角θgpsとから角速度センサーの車両進行方向に対する傾斜角θsensorを計算し、前記加速度センサーの水平面に対する傾斜角θaccと前記傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算し、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項3記載の角速度補正方法。
【請求項5】
前記第1のセンサー感度補正方法は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正し、
前記第2のセンサー感度補正方法は、地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして求め、該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項1または2記載の角速度補正方法。
【請求項6】
GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いか監視し、
該条件が成立したとき前記感度誤差の算出を行なう、
ことを特徴とする請求項1乃至4記載の角速度補正方法。
【請求項7】
車両に取り付けられて該車両の角速度を検出するセンサーの感度を補正し、該補正された感度を用いて該角速度検出センサーが出力する角速度を補正する角速度補正装置において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、
角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、
前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、
該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部、
を有することを特徴とする角速度補正装置。
【請求項8】
前記算出された感度誤差を前記第2のセンサー感度算出部で求めたセンサー感度に乗算してセンサー感度を補正する感度補正部、
を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項7記載の角速度補正装置。
【請求項9】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、
該傾斜角を用いて角速度センサーの水平面に対する傾斜角に起因するセンサー感度を補正するセンサー誤差算出部、
を備えることを特徴とする請求項7または8記載の角速度補正装置。
【請求項10】
前記第2のセンサー感度算出部は、更に、
GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対するに対する傾斜角θsensorを計算するセンサー傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
を備え、前記センサー感度算出部は、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項9記載の角速度補正装置。
【請求項11】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
地図データベースに含まれる道路傾斜角情報より自車位置の道路傾斜角を車両傾斜角θcarとして取得する車両傾斜角取得部、
該車両傾斜角θcarとセンサーの車両進行方向に対する傾斜角θsensorを用いて
1/cos(θsensor+θcar)
をセンサー感度として出力するセンサー感度算出部、
を備えることを特徴とする請求項7または8記載の角速度補正装置。
【請求項12】
GPSの信頼度を判定するGPS信頼度判定部、
車両の水平面に対する傾斜角度が水平に近いか監視する監視部、
を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう、
ことを特徴とする請求項7乃至または10記載の角速度補正装置。
【請求項13】
GPS受信機と角速度センサーと加速度センサーと車速センサーとを備え、これらの出力値を用いて車両の位置および方位を検出する機能を備えたナビゲーション装置において、
角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化および角速度検出センサーの感度誤差を考慮して該センサー感度を補正する第1のセンサー感度算出部、
角速度検出センサーの感度誤差を考慮せず、角速度検出センサーの水平面に対する傾斜角に起因するセンサー感度の変化を考慮して該センサー感度を補正する第2のセンサー感度算出部、
前記第1のセンサー感度算出部により得られたセンサー感度と前記第2のセンサー感度算出部により得られたセンサー感度の比をセンサーの感度誤差として計算して出力する感度誤差計算部、
該感度誤差を用いて前記センサーが出力する角速度を補正する角速度補正部、
補正された前記角速度と車速センサー出力を用いて車両位置を計算する車両位置計算部、
を有することを特徴とするナビゲーション装置。
【請求項14】
前記算出された感度誤差を前記第2のセンサー感度補正方法により求めたセンサー感度に乗算してセンサー感度を補正する感度補正部、
を備え、前記角速度補正部は該補正したセンサー感度を前記センサーが出力する角速度に乗算して角速度を補正する、
ことを特徴とする請求項13記載のナビゲーション装置。
【請求項15】
前記第1のセンサー感度算出部は、角速度検出センサーから得られたセンサー方位がGPS電波より得られるGPS方位に一致するようにセンサー感度を補正する手段を備え、
前記第2のセンサー感度算出部は、
角速度検出センサーと加速度センサーが同一平面に取り付けられている場合、該加速度センサーの出力信号と距離センサーから出力する車速パルスを用いて前記加速度センサーの水平面に対する傾斜角θaccを計算する傾斜角計算部、
該傾斜角を用いて該センサーの水平面に対する傾斜角に起因するセンサー感度を補正するセンサー感度補正部、
を備えることを特徴とする請求項13または14載のナビゲーション装置。
【請求項16】
前記第2のセンサー感度算出部は、更に、
GPS電波より得られた車両の水平面に対する傾斜角であるGPS傾斜角θgpsを計算するGPS傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと該GPS傾斜角とからセンサーの車両進行方向に対する傾斜角θsensorを計算するセンサー傾斜角計算部、
前記加速度センサーの水平面に対する傾斜角θaccと前記センサー傾斜角θsensorとから車両の水平面に対する傾斜角である車両傾斜角θcarを計算する車両傾斜角計算部、
を備え、前記センサー感度補正部は、
1/cos(θsensor+θcar)
をセンサー感度として出力する、
ことを特徴とする請求項15記載のナビゲーション装置。
【請求項17】
GPSの信頼度を判定するGPS信頼度判定部、
車両の水平面に対する傾斜角度が水平に近いか監視する監視部、
を備え、前記感度誤差計算部は、GPSの信頼度が高く、かつ、車両の水平面に対する傾斜角度が水平に近いとき前記感度誤差の算出を行なう、
ことを特徴とする請求項13または16記載のナビゲーション装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【公開番号】特開2009−139227(P2009−139227A)
【公開日】平成21年6月25日(2009.6.25)
【国際特許分類】
【出願番号】特願2007−315918(P2007−315918)
【出願日】平成19年12月6日(2007.12.6)
【出願人】(000101732)アルパイン株式会社 (2,424)
【Fターム(参考)】
【公開日】平成21年6月25日(2009.6.25)
【国際特許分類】
【出願日】平成19年12月6日(2007.12.6)
【出願人】(000101732)アルパイン株式会社 (2,424)
【Fターム(参考)】
[ Back to top ]