JP2004125689A

JP2004125689A - 自立航法用位置算出システム

Info

Publication number: JP2004125689A
Application number: JP2002292178A
Authority: JP
Inventors: Toru Ichikawa; 市川　徹; Akira Ishida; 石田　明
Original assignee: Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Holdings Corp
Priority date: 2002-10-04
Filing date: 2002-10-04
Publication date: 2004-04-22

Abstract

【課題】自立航法のナビゲーションシステムにおいて、移動体が傾斜路を走行したり筐体が傾いて取り付けられたり、角速度検出装置に傾きが生じた場合でも、正しい方位を算出する。
【解決手段】ヨー方向とピッチ方向の角速度を検出する角速度検出手段１と、互いに直交する少なくとも２軸の加速度を検出する加速度検出手段２と、移動距離を検出する距離検出手段３と、移動体の傾きを検出する傾斜検出手段１２と、前記傾きの検出結果と前記加速度検出手段で検出した加速度に基づいて筐体の取り付け角を検出する筐体取り付け角算出手段１３と、前記筐体取り付け角に基づいて前記検出された角速度と加速度の出力を補正し、補正後の角速度と補正後の加速度と移動距離に基づいて移動体の傾斜角を検出する傾斜角算出手段４と、検出した傾斜角を用いて角速度の方位補正を行う角速度補正手段５とを備える。
【選択図】　図４

Description

【０００１】
【発明の属する技術分野】
本発明は、自動車等の移動体に搭載されるナビゲーションシステムに関し、特に、角速度センサの検知軸のずれを補正することにより高精度に方位、位置を算出する自立航法用位置算出システムに関するものである。
【０００２】
【従来の技術】
近年、自動車等のナビゲーションシステムにおいては、細街路や複雑に交差した道路、または駐車場等での正確な経路案内や自車位置の表示に対する要求が高まっている。こうした要求に応えるためには、常に高精度な位置測位技術が求められる。現在のナビゲーションシステムにおける位置測位技術は、ＧＰＳ衛星等の情報を基に車の絶対位置を算出する電波航法システムと移動体に取り付けられた各種センサの情報を基に位置を算出する自立航法システムから成り立っている。このうち電波航法については新衛星の打ち上げや基準点データの利用によって、高精度化が図られている。しかし、電波航法の場合、屋内やトンネル、都心のビル街といった電波が受信できない場所では、移動体の正確な位置を算出することができなくなるという問題点がある。そのため、自立航法による位置算出システムの高精度化が必要になっている。
【０００３】
自立航法のためには、移動体の方位と移動速度を知る必要がある。角速度センサにより移動体のヨー方向角速度を検出して方位を算出するとき、移動体に設置された角速度センサが傾いている場合や、路面の勾配や走行状態により移動体が傾いている場合には、ヨー方向角速度センサの角速度検出面に傾きが生じ、その結果、算出される方位にズレが生じてしまう。この課題に対する従来技術では、３つの加速度センサを用いてヨー方向角速度センサの傾きを算出し、出力を補正しているものもある（例えば、特許文献１参照）。
【０００４】
この従来技術では、図１０に示すように、ヨー方向角速度センサと同じ平面状に設置された３つの加速度センサによりヨー方向角速度センサの鉛直方向からの傾きを求め、異なる２軸を中心に回転し補正するものである。
【０００５】
【特許文献１】
特開平６−３２４０６６号公報
【０００６】
【発明が解決しようとする課題】
しかし、従来の角速度センサの補正では加速度センサの出力のみを用いてヨー角速度の補正を行っているため、角速度検出面の瞬間的な変化に対する補正が不十分であり十分な精度が得られない。
【０００７】
また、他の問題として、カーナビゲーション等で筐体の取り付け時に傾きがある場合には各センサの出力をそのまま利用すると誤差を生じてしまう。現状のナビゲーションシステムでは、筐体の設置方法に細かな制限を設け、この問題を回避しているが、ユーザへの負担は大きくなっている。
【０００８】
本発明は、路面の勾配や移動体の傾き、また初期取り付け時のずれによってセンサの検出面が傾いた場合に生じる角速度の出力誤差を正確に補正することにより、移動体の方位、位置を精度良く検出すると共に、移動体への取り付け時の制約をなくすことができる自立航法用位置算出システムを提供することを目的とする。
【０００９】
【課題を解決するための手段】
上記の課題を解決するために、第一の発明は、移動体のヨー方向とピッチ方向の角速度を検出する角速度検出手段と、移動体の前後方向軸、左右方向軸、鉛直方向軸のうち少なくとも２軸の加速度を検出する加速度検出手段と、移動体の移動距離を検出する距離検出手段と、角速度検出手段で検出した角速度と加速度検出手段で検出した加速度と距離検出手段で検出した移動距離に基づいて、ある任意座標系に対する移動体の傾斜角を算出する傾斜角算出手段と、傾斜角算出手段で求めた傾斜角に基づいて角速度検出手段で検出した角速度の補正を行う角速度補正手段とを備えたことを特徴とする自立航法用位置算出システムである。
【００１０】
この第一の発明により、移動体が勾配路を走行した場合や運動により移動体に傾きが生じた場合でも移動体の正しい方位を算出することができ、その結果、移動体の正確な位置を知ることが可能となる。
【００１１】
第二の発明は、移動体のヨー方向とピッチ方向の角速度を検出する角速度検出手段と、移動体の前後方向軸、左右方向軸、鉛直方向軸のうち少なくとも２軸の加速度を検出する加速度検出手段と、角速度検出手段と加速度検出手段とを固定する筐体の移動体への取り付け角を加速度検出手段で検出した加速度に基づいて算出する筐体取り付け角算出手段と、移動体の移動距離を検出する距離検出手段と、筐体取り付け角算出手段で求めた筐体の取り付け角に基づいて、角速度検出手段で検出した角速度と加速度検出手段で検出した加速度を修正し、前記修正後の角速度と前記修正後の加速度と前記距離検出手段で検出した移動距離に基づいてある任意座標系に対する移動体の傾斜角を算出する傾斜角算出手段と、傾斜角算出手段で算出された傾斜角を用いて角速度検出手段で検出した角速度の補正を行う角速度補正手段とを備えたことを特徴とする自立航法用位置算出システムである。
【００１２】
この第二の発明により、筐体が移動体に対して斜めに取り付けられた場合でも、移動体の正確な位置を知ることが可能となる。その結果、設置に関する自由度も高まりユーザの負担を軽減することができる。
【００１３】
【発明の実施の形態】
（実施の形態１）
以下、第一の発明の具体的な実施の形態ついて図面を参照しながら説明する。図１は自立航法用位置算出システムのシステム構成を示し、１は移動体の角速度を検出する角速度検出手段、２は移動体の加速度を検出する加速度検出手段、３は移動体の走行距離を検出する距離検出手段、４は角速度検出手段１と加速度検出手段２と距離検出手段３の出力を基に移動体の傾斜角を算出する傾斜角算出手段、５は傾斜角算出手段４により得られる移動体の傾斜角に基づき角速度検出手段１で得られる角速度の値を補正する角速度補正手段である。
【００１４】
図２は自立航法用位置算出システムの各センサの構成を示す斜視図で、角速度検出手段１は、６，７に示される様にそれぞれ移動体のヨー方向、ピッチ方向の角速度を検出するための角速度センサで構成され、角速度の検出軸が互いに直交するように取り付けられる。角速度センサ６，７の例としては、カーナビゲーション用途で一般に用いられている水晶型の振動ジャイロを利用すればよい。加速度検出手段２は、８，９，１０に示される様にそれぞれ移動体の鉛直方向、左右方向、前後方向の加速度を検出するための加速度センサで構成される。加速度センサの例としては、小型の静電容量型加速度センサ等を利用すればよい。
【００１５】
距離検出手段３は、移動体の進行方向の移動距離を検出するように取り付けられる。距離検出手段には、移動体の対象を車とした場合には、カーナビゲーションシステムで一般的に使用されている車速パルスを利用すればよい。車速パルスは誤差も少なく、安定した出力が得られる。その構成は、パルス発生器とパルス検出器とから成り、車軸もしくは、車輪軸に取り付けられた発生器の出力するパルス数を検出器で読み取ることで移動体の移動距離を算出する。図２では、簡単のためパルス検出器１１のみを示している。
【００１６】
上記のように構成される自立航法用位置算出システムについて、全体の処理の流れを図３のフローチャートに示した。システムが起動すると、一定時間ごとに角速度センサ６，７、加速度センサ８，９，１０、パルス検出器１１から各データが取得される（処理１）。取得された角速度データ、加速度データ、距離データを用いて、移動体の傾斜角を算出する（処理２）。次に、算出された傾斜角を用いて、処理１で得られた角速度データのうち、ヨー方向の角速度を補正する（処理３）。そして、補正された角速度の値を時間積分することで移動体の方位を算出し、距離データと併せて、移動体の位置を算出する（処理４）。
【００１７】
以下では、上述の処理１から処理４について具体的に説明する。説明のために次の記号を定義する。
θ_１：移動体のヨー角
θ_２：移動体のピッチ角
θ_３：移動体のロール角
ｐ：移動体のロール方向の角速度
ｑ：ピッチ方向の角速度を検出する角速度センサ７の出力
ｒ：ヨー方向の角速度を検出する角速度センサ６の出力
ｇｘ：前後方向の加速度を検出する加速度センサ１０の出力
ｇｙ：左右方向の加速度を検出する加速度センサ９の出力
ｇｚ：鉛直方向の加速度を検出する加速度センサ８の出力
Ｄ：パルス検出器１１より得られる距離データ
Ａｘ：基準平面（ピッチ、ロールの値が０）での進行方向加速度
Ａｙ：遠心加速度
Ａｚ：鉛直方向の加速度
Ｇ：重力加速度
α：基準面におけるヨー方向角速度
処理１では、角速度センサ６，７、加速度センサ８，９，１０、パルス検出器１１で検出されるそれぞれのデータを、演算装置の処理能力に応じたサンプリング間隔で取得する。取得されたデータについて、ノイズ除去等の前処理を施し、距離データに関しては、距離係数を乗算することでパルス数データを距離データＤに変換する。
【００１８】
処理２では、処理１で得られた各データのうち距離データＤ、加速度データｇｘ，ｇｙ，ｇｚ、角速度データｐ，ｑを用いて、傾斜角の算出を行う。今、移動体がピッチ方向にθ_２、ロール方向にθ_３の傾きを持って移動している場合を考える。図６はピッチ方向の傾斜と移動体にかかる加速度の関係を示す図である。図６より、
（ｄ^２Ｄ／ｄｔ^２）＝ｇｘ＋Ｇｓｉｎθ_２　・・・（１）
が成り立つ。また、基準平面での加速度成分Ａｘは、
Ａｘ＝（ｄ^２Ｄ／ｄｔ^２）・ｃｏｓθ_２　・・・（２）
で与えられ、加速度の鉛直方向成分Ａｚは、
Ａｚ＝（ｄ^２Ｄ／ｄｔ^２）・ｓｉｎθ_２　・・・（３）
で与えられる。
【００１９】
次に、図７はロール方向の傾斜と移動体にかかる加速度の関係を示す図である。
【００２０】
図７より、
Ａｙ＝（ｇｙ−Ｇｃｏｓθ_２ｓｉｎθ_３）／ｃｏｓθ_３　・・・（４）
Ｇ＝（Ａｙｓｉｎθ_３−ｇｚ）／ｃｏｓθ_２ｃｏｓθ_３　・・・（５）
また、（４）式の遠心加速度Ａｙについて、基準面におけるヨー方向角速度αを用いて次の様に表される。
Ａｙ＝（ｄＤ／ｄｔ）・α　・・・（６）
ここで、処理２の移動体の傾斜角算出処理の前に、処理３におけるヨー方向角速度センサの補正方法について先に説明する。図８で、ｘ０−ｙ０−ｚ０座標系で表される平面を基準平面とし、基準平面をｚ０軸周りにθ_１回転してできる平面をｘ１−ｙ１−ｚ１平面とする。ｘ１−ｙ１−ｚ１平面をｙ１軸周りにθ_２回転してできる平面をｘ２−ｙ２−ｚ２平面、最後にｘ２−ｙ２−ｚ２平面をｘ２軸周りにθ_３回転してできる平面をｘ３−ｙ３−ｚ３平面とする。こうしてできた平面ｘ３−ｙ３−ｚ３が移動体の存在する平面であり、補正を行うためにはこの平面上で得られる角速度の値を基準平面での角速度に変換すればよい。上記のθ_ｎ（ｎ＝１，２，３）の回転変換を表す行列をＴ_１、Ｔ_２、Ｔ_３とし、基準平面におけるｚ０軸周りの回転角速度ベクトルを（０，０，α）、ｘ１−ｙ１−ｚ１平面におけるｙ１軸周りの回転角速度ベクトルを（０，β，０）、ｘ２−ｙ２−ｚ２平面におけるｘ２軸周りの回転角速度ベクトルを（γ，０，０）とすると、
（ｐ，ｑ，ｒ）^Ｔ＝Ｔ_３Ｔ_２Ｔ_１（０，０，α）^Ｔ＋Ｔ_３Ｔ_２（０，β，０）^Ｔ
＋Ｔ_３（γ，０，０）^Ｔ　・・・（７）
が成り立つ。ここで、算出すべき値はαであり、上式（７）を変形して、以下の補正式を得る。
α＝（−ｑｓｉｎθ_３＋ｒｃｏｓθ_３）／ｃｏｓθ_２　・・・（８）
次に、処理２の移動体の傾斜角算出方法であるが、例として移動体の前後方向と左右方向の２軸の加速度センサを用いる場合は、上記の式（１）、（４）、（６）、（８）を解くことにより、移動体の傾斜角θ_３、θ_２を得ることができる。また、別の例として、移動体の左右方向と鉛直方向の２軸の加速度センサを用いる場合は上記の（１）、（４）、（５）、（８）式よりθ_３、θ_２を求めることが可能である。
【００２１】
処理３では、こうして得られる傾斜角θ_３、θ_２を用いて（８）式よりαの値を算出する。処理４では、処理３で補正された移動体の角速度αと移動距離Ｄを用いて、移動体の位置（ｘ，ｙ，ｚ）を以下の様に算出する。ただし、移動前の位置を（ｘ０，ｙ０，ｚ０）とする。まず、移動体の基準面上の方位角は、
θ_１＝∫αｄｔ　・・・（９）
基準面上での移動距離Ｄ´は▲２▼式より、
Ｄ´＝∬Ａｘｄｔ＝Ｄ・ｃｏｓθ_２　・・・（１０）
よって、（３）、（９）、（１０）式より、
ｘ＝ｘ０＋Ｄ・ｃｏｓθ_１　・・・（１１）
ｙ＝ｙ０＋Ｄ・ｓｉｎθ_１　・・・（１２）
ｚ＝ｚ０＋∬Ａｚｄｔ＝ｚ０＋Ｄ・ｓｉｎθ_２　・・・（１３）
以上の式（１１）、（１２）、（１３）式により、基準面における移動体の正確な位置を知ることができる。
【００２２】
移動体の正確な位置情報が得られれば、例えば、カーナビゲーションシステムにおいて、複雑な勾配を持つらせん状の立体駐車場を走行した場合や、高低差のある道路を走行した場合にも自車の位置を正しく表示することができる。
【００２３】
（実施の形態２）
次に、第２の発明の具体的な実施の形態について図４、５を用いて説明する。図４は筐体取り付け角算出手段１３を備えた自立航法用位置算出システムの構成図であり、図４において、傾斜検出手段１２は、移動体の存在する路面に傾斜が存在するかどうかを判定する装置であり。例えば、車の左右車高センサや振り子式の傾斜角計、または、地図データ内の高度情報などの利用が考えられるが、ここでは例として車高センサを用いた場合について述べる。
【００２４】
図５は筐体取り付け角算出手段１３を備えた自立航法用位置算出システムでの処理の流れを示すフローチャートで、処理１では、前記の距離検出手段３、加速度検出手段２、角速度検出手段１から一定時間間隔でデータを取得する。同時に、車高センサで４輪にかかる荷重から移動体の傾きを計測する。路面に勾配がある場合や、カーブ走行などにより移動体に傾きがある場合には、処理２の筐体取り付け角算出処理を行わず、筐体のヨー方向、ピッチ方向、ロール方向の傾きを全て０ｄｅｇとして処理３に移る。移動体に傾きがない場合は、距離検出手段の出力を調べ処理２の筐体取り付け角算出処理を行う。以下、処理の詳細について述べる。
【００２５】
まず、距離検出手段の出力が０、つまり、移動体の停車中には下記の（１４）、（１５）、（１６）、（１７）式が成り立つ。ただし、図９に示すように移動体の進行方向、左右方向、鉛直方向に対する初期筐体取り付け角をそれぞれ、
Ｙ_０：ヨー方向への傾き角
Ｒ_０：ロール方向の傾き角
Ｐ_０：ピッチ方向の傾き角
と定義する。
【００２６】
Ｄ＝０のとき、
ｇｘ＝−ＧｓｉｎＰ_０・・・（１４）
ｇｙ＝ＧｃｏｓＰ_０ｓｉｎＲ_０・・・（１５）
ｇｚ＝−ＧｃｏｓＲ_０ｃｏｓＰ_０・・・（１６）
が成り立ち、このうち加速度センサの設置状況に応じた２式を利用してＲ_０、Ｒ_０を求める。
【００２７】
次に、Ｄが０でないとき、
ｇｘ＋ＧｓｉｎＰ_０＝（ｄ^２Ｄ／ｄｔ^２）ｃｏｓＹ_０・・・（１７）
が成り立ち、この式よりＹ_０を得る。
【００２８】
処理２の筐体取り付け角算出処理は、システム起動後、車高センサと距離センサの出力が最初に上記条件を満たした一度だけ処理を行い、以後は算出された取り付け角を利用する。
【００２９】
処理３では、角速度センサ、加速度センサから得られる入力に対し、初めに、上記筐体取り付け角算出手段で得られた筐体取り付け角を用いて、角速度センサ、加速度センサの出力を以下の式（１８）〜（２２）により補正する。ただし、補正後の出力を次の記号で定義する。
ｒ´：初期取り付け角による補正後のヨー方向角速度センサ６の出力
ｑ´：初期取り付け角による補正後のピッチ方向角速度センサ７の出力
ｇｘ´：初期取り付け角による補正後の前後方向の加速度センサ１０の出力
ｇｙ´：初期取り付け角による補正後の左右方向の加速度センサ９の出力
ｇｚ´：初期取り付け角による補正後の鉛直方向の加速度センサ８の出力
ｒ´＝ｒ／ｃｏｓＲ_０ｃｏｓＰ_０・・・（１８）
ｑ´＝ｑ／ｃｏｓＹ_０ｃｏｓＲ_０・・・（１９）
ｇｘ´＝（ｇｘ＋ＧｓｉｎＰ_０）／ｃｏｓＹ_０ｃｏｓＰ_０・・・（２０）
ｇｙ´＝（ｇｙ−ＧｃｏｓＰ_０ｓｉｎＲ_０）／ｃｏｓＹ_０ｃｏｓＰ_０・・・（２１）
ｇｚ´＝ｇｚ／ｃｏｓＲ_０ｃｏｓＰ_０・・・（２２）
上記の式で各センサの出力を補正後、修正されたセンサ出力を用いて、傾斜角の算出を行ない、ｑ´に対して方位補正の処理を行う。最後に、補正された角速度の値と上記距離検出手段３で得られた距離を用いて移動体の位置を算出する。
【００３０】
傾斜角算出、角速度補正、位置算出は上記実施の形態１と同様の方法で求めることができる。
【００３１】
以上、第２の発明によれば、初期の筐体取り付け角による走行時の位置算出の誤差をなくすことができる。そのため、本発明を利用したナビゲーションシステムでは、筐体取り付け時の制約を軽減することができ、たとえ、筐体がゆがんで設置された場合でも移動体の正確な位置を算出することが可能となり非常に有用である。
【００３２】
【発明の効果】
本発明による、自立航法用位置算出システムによれば、自立航法における移動体の方位を算出するための角速度センサに傾斜が生じた場合でも、その傾斜角に応じた補正を行うことで常に正確な角速度を保持することができ、その結果、移動体の正確な方位、位置を算出することが可能となる。そのため、本発明をカーナビゲーションの自立航法に利用すれば、らせん形状の道路や勾配路を走行した場合にも、方位誤差による位置ズレをなくすことが可能となる。
【図面の簡単な説明】
【図１】本発明の実施の形態１における自立航法用位置算出システムの構成図
【図２】本発明の実施の形態１における自立航法用位置算出システムの各センサの構成を示す斜視図
【図３】本発明の実施の形態１における自立航法用位置算出システムの処理の流れを示すフローチャート
【図４】本発明の実施の形態２における自立航法位置算出システムの構成を示すブロック図
【図５】本発明の実施の形態２における自立航法位置算出システムの処理の流れを示すフローチャート
【図６】本発明の実施の形態１における移動体のピッチ方向の傾斜と移動体にかかる加速度の関係を示す図
【図７】本発明の実施の形態１における移動体のロール方向の傾斜と移動体にかかる加速度の関係を示す図
【図８】本発明の実施の形態１におけるオイラー角を用いた座標変換を示す図
【図９】本発明の実施の形態２にかかる筐体初期取り付け角を示す図
【図１０】従来技術におけるヨー方向角速度補正システムの平面図
【符号の説明】
１　角速度検出手段
２　加速度検出手段
３　距離検出手段
４　傾斜角算出手段
５　角速度補正手段
６　ヨー方向の角速度を検出する角速度センサ
７　ピッチ方向の角速度を検出する角速度センサ
８　鉛直方向軸の加速度を検出する加速度センサ
９　左右方向軸の加速度を検出する加速度センサ
１０　前後方向軸の加速度を検出する加速度センサ
１１　パルス検出器
１２　傾斜検出手段
１３　筐体取り付け角算出手段

Claims

移動体のヨー方向とピッチ方向の角速度を検出する角速度検出手段と、
前記移動体の前後方向軸、左右方向軸、鉛直方向軸のうち少なくとも２軸の加速度を検出する加速度検出手段と、
前記移動体の移動距離を検出する距離検出手段と、
前記角速度検出手段で検出した角速度と前記加速度検出手段で検出した加速度と前記距離検出手段で検出した移動距離に基づいて、ある任意座標系に対する前記移動体の傾斜角を算出する傾斜角算出手段と、
前記傾斜角算出手段で求めた傾斜角に基づいて前記角速度検出手段で検出した角速度の補正を行う角速度補正手段とを備えたことを特徴とする自立航法用位置算出システム。
移動体のヨー方向とピッチ方向の角速度を検出する角速度検出手段と、
前記移動体の前後方向軸、左右方向軸、鉛直方向軸のうち少なくとも２軸の加速度を検出する加速度検出手段と、
前記角速度検出手段と前記加速度検出手段とを固定する筐体の前記移動体への取り付け角を前記加速度検出手段で検出した加速度に基づいて算出する筐体取り付け角算出手段と、
前記移動体の移動距離を検出する距離検出手段と、
前記筐体取り付け角算出手段で求めた前記筐体の取り付け角に基づいて、前記角速度検出手段で検出した角速度と前記加速度検出手段で検出した加速度を修正し、前記修正後の角速度と前記修正後の加速度と前記距離検出手段で検出した移動距離に基づいてある任意座標系に対する前記移動体の傾斜角を算出する傾斜角算出手段と、
前記傾斜角算出手段で算出された傾斜角を用いて前記角速度検出手段で検出した角速度の補正を行う角速度補正手段とを備えたことを特徴とする自立航法用位置算出システム。
前記任意座標系として、Ｚ軸を重力方向とし、Ｘ軸、Ｙ軸がそれぞれＺ軸と直交する右手座標系で定義されることを特徴とする請求項１または２記載の自立航法用位置算出システム。
前記筐体取り付け角算出手段は、前記移動体が傾斜しているか否かを検出する傾斜検出手段を備え、前記傾斜検出手段の検出結果によって前記移動体は傾斜しておらず、かつ前記距離算出手段の出力によって前記移動体が移動していないと判断した場合に、前記加速度検出手段の出力に基づいて前記筐体の取り付け角を算出することを特徴とする請求項２記載の自立航法用位置算出システム。
前記角速度補正手段は、前記傾斜角算出手段により検出された傾斜角に基づいて、前記角速度検出手段で検出されたヨー方向角速度の値を補正することを特徴とする請求項１または２記載の自立航法用位置算出システム。
前記角速度補正手段により補正された角速度と前記距離検出手段で検出された移動距離から前記移動体の位置を算出することを特徴とする請求項１または２記載の自立航法用位置算出システム。