JP6900248B2 - 現在位置算出装置、ナビゲーションシステム及びピッチ角誤差の算出方法 - Google Patents

Description

本発明は、主として、現在位置の算出に用いられるピッチ角を補正する技術に関するものである。

現在位置を算出する技術としては、自動車に搭載されたシステムにおいて、加速度センサ、ジャイロセンサ、速度センサを用いて、自動車の姿勢を考慮した当該自動車の三次元の移動量を算定することにより、三次元の現在位置を自律航法で算出する技術が知られている（たとえば、特許文献１）。

また、このようなシステムにおいて、GPS等のGNSSの測位データを用いて、ピッチ角等の算定誤差を補正する技術も知られている（特許文献２、３）。

特開２００９-１９８２７９号公報 特開２０１０-３２３９８号公報 特開２０１２-１７３１９０号公報

上述した技術によれば、GPS等のGNSSの良好な測位データを利用できない屋内施設などの環境下ではピッチ角の算定誤差を補正することができなくなる。そして、このために、加速度センサやジャイロセンサのドリフト誤差等がある場合、正しいピッチ角を考慮した三次元の移動量を算定することができなくなり、現在位置の標高が誤って算出されてしまうこととなる。

そこで、本発明は、GNSSの測位データを良好に利用できない屋内施設において、ピッチ角の算定誤差を正しく算出して、より精度良い現在位置の標高の算定等を行えるようにすすることを課題とする。

前記課題達成のために、本発明は、自動車に搭載される、現在位置を算出する現在位置算出装置に、自律航法によって標高を含む現在位置を算出する自律航法手段と、参照標高値を記憶する記憶手段と、前記自動車が、自走可能な多階層の施設内の各階の水平な平地を走行している際に、少なくとも当該走行している平地の階の参照標高値が前記記憶手段に記憶されていない場合に、前記自律航法手段が算定した標高値を、当該走行している平地の階の参照標高値として、前記記憶手段に記憶する参照値保存手段と、前記自動車が、前記施設内の異なる階の平地に移動した際に、当該移動した平地の階の参照標高値が前記記憶手段に記憶されている場合に、前記自律航法手段が算定した標高値と当該記憶されている参照標高値との差である標高誤差が、当該記憶されていた参照標高値が前記参照値保存手段によって記憶された時点から現時点までの走行距離である移動距離を進む間に生じたものとして、前記自律航法手段が前記標高の算出に用いたピッチ角の算定誤差を算定するピッチ角誤差算定手段と、前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差に基づいて、前記自律航法手段が算定している現在位置の標高を補正する補正手段とを備えたものである。

ここで、このような現在位置算出装置は、前記記憶手段を、前記参照標高値と共に、参照走行距離を記憶するものとし、前記参照値保存手段において、前記参照標高値と共に、現時点における所定の時点からの走行距離を、当該走行している平地の階の参照走行距離として前記記憶手段に記憶し、前記ピッチ角誤差算定手段において、前記施設内の異なる階の平地に移動した際に、当該移動した平地の階の参照標高値と参照走行距離とが前記記憶手段に記憶されている場合に、前記移動距離を、現時点における前記所定の時点からの走行距離と当該記憶されている参照走行距離との差によって求めるように構成してもよい。

また、以上のような現在位置算出装置では、前記ピッチ角誤差算定手段において、ピッチ角の算定誤差＝arctan(標高誤差/移動距離）によって、前記ピッチ角の算定誤差を算定するように構成してもよい。

また、以上の現在位置算出装置は、前記補正手段において、前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差が、所定の範囲内にない場合には、前記標高の補正を行わないように構成してもよい。

また、以上の現在位置算出装置は、当該現在位置算出装置に、前記施設に進入する前に衛星測位を利用して算出した標高に、前記自律航法手段が算定している現在位置の標高を補正する衛星測位利用補正手段を設け、前記補正手段において、前記自律航法手段が衛星測位を利用して標高を算出した時点から現時点までの走行距離をＬ、前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差をθ、前記自律航法手段が現時点で算出している標高をＨとして、Ｈ-Ｌ×sinθに、前記自律航法手段が算定している現在位置の標高を補正するように構成してもよい。

また、本発明は、併せて、現在位置算出装置と、当該現在位置算出装置が算出した現在位置を地図上に表した案内画面を表示する案内手段と備えたナビゲーションシステムも提供する。

また、前記課題達成のために、本発明は、自動車に搭載されるピッチ角誤差算定装置において、自律航法によって標高を含む前記自動車の現在位置を算出する自律航法手段が、前記標高の算出に用いたピッチ角の算定誤差を算出するピッチ角誤差の算出方法として、前記ピッチ角誤差算定装置において、前記自動車が、自走可能な多階層の施設内の各階の水平な平地を走行している際に、少なくとも当該走行している平地の参照標高値が記憶されていない場合に、前記自律航法手段が算定した標高値を、当該走行している平地の参照標高値として記憶する参照値保存ステップと、前記ピッチ角誤差算定装置において、前記自動車が、前記施設内の平地に移動した際に、当該移動した平地の参照標高値が記憶されている場合に、前記自律航法手段が算定した標高値と当該記憶されている参照標高値との差である標高誤差が、当該記憶されていた参照標高値が記憶された時点から現時点までの走行距離である移動距離を進む間に生じたものとして、前記自律航法手段が前記標高の算出に用いたピッチ角の算定誤差を算定するピッチ角誤差算定ステップとを有するとを備えたピッチ角誤差の算出方法も提供する。

ここで、より具体的には、このようなピッチ角誤差の算出方法は、前記ピッチ角誤差算定ステップにおいて、ピッチ角の算定誤差＝arctan(標高誤差/移動距離）によって、前記ピッチ角の算定誤差を算定するようにしてよい。

以上のような本発明によれば、自走式の立体駐車状などの自走可能な多階層の施設内の各階に水平な平地が存在することを利用して、異なる時点で同じ平地について自律航法手段が算定した標高値の差と、両時点間の走行距離より自律航法手段が現在位置の算出に用いるピッチ角の誤差を正しく算定することができる。

よって、GNSSの測位データを良好に利用できない自走可能な多階層の施設内において、ピッチ角の算定誤差を正しく算出し、より精度良く現在位置の標高を算出することができるようになる。

以上のように、本発明によれば、GNSSの測位データを良好に利用できない屋内施設において、ピッチ角の算定誤差を正しく算出して、より精度良い現在位置の標高の算定等を行うことができる。

本発明の実施形態に係るナビゲーションシステムの構成を示すブロック図である。 本発明の実施形態に係るピッチ角誤差補正処理を示すフローチャートである。 本発明の実施形態に係るピッチ角誤差補正処理において設定する参照エリアを示す図である。 本発明の実施形態に係るピッチ角誤差補正処理の処理例を示す図である。

以下、本発明の実施形態について説明する。
図１に、本実施形態に係るナビゲーションシステムの構成を示す。
ナビゲーションシステムは自動車に搭載される装置であり、図示するように、加速度センサ１、ジャイロセンサ２、車速センサ３、ＧＮＮＳ受信器４、自律航法処理部５、現在位置算出部６、地図データ記憶部７、ルート探索部８、制御部９、表示装置１０、入力装置１１を備えている。

ここで、加速度センサ１は自動車の加速度を検出し、ジャイロセンサ２は自動車の角速度を検出し、車速センサ３は自動車の速度を検出する。また、ＧＮＮＳ受信器４は、たとえば、ＧＰＳ受信器などであり、衛星測位により自動車の三次元の位置や移動ベクトルを計測する。

また、地図データ記憶部７には、道路網の三次元地図を表す地図データと、自走式立体駐車場（地下に設けられるものを含む）などの自動車が走行する可能性のある多階層施設の施設内の三次元の地図を表す施設地図データとが格納されている。

また、自律航法処理部５は、加速度センサ１、ジャイロセンサ２、車速センサ３の出力に基づいて、三次元の現在位置や移動ベクトルなどの自動車の現在の状態を、所定の演算式に従って算定する。また、自律航法処理部５は、ＧＮＮＳ受信器４における衛星測位が良好に行えている期間中には、ＧＮＮＳ受信器４が計測した移動ベクトル等に応じて、算定した自動車の現在の状態を補正したり、加速度センサ１、ジャイロセンサ２、車速センサ３を校正したりする ＧＮＮＳ利用補正機能も備えている。

次に、現在位置算出部６は、ＧＮＮＳ受信器４が計測した現在位置、移動ベクトルや、自律航法処理部５が算定した現在位置、移動ベクトルと、地図データ記憶部７に記憶されている地図データや施設地図データとのマップマッチングを行って、尤も現在位置や現在の進行方向として確からしい位置や方向を、現在位置や現在の進行方向として算出する。

ただし、現在位置算出部６は、多階層施設内を走行している期間中などの、ＧＮＮＳ受信器４が衛星からの信号を正常に受信できないために良好に衛星測位を行えない期間中は、自律航法処理部５が算定した現在位置、移動ベクトルのみを用いて、地図データ記憶部７に記憶されている地図データや施設地図データとのマップマッチングを行って現在位置や現在の進行方向を算出する。

また、制御部９は、ユーザから入力装置１１を介して目的地の設定を受付け、目的地に到る誘導ルートをルート探索部８に探索させる。ルート探索部８は、地図データや施設地図データに基づいて、現在位置算出部６が算出している現在位置から目的地までの最小コストの経路を、距離最小などの所定のコストモデルに基づいて算出し誘導ルートとして設定する。

そして、制御部９は、現在位置算出部６が算出している現在位置や、ルート探索部８が設定した誘導ルートを地図データが表す三次元もしくは二次元の地図や、施設地図データが表す三次元の施設内地図の上に表した案内画像を生成し、表示装置１０に表示する。

なお、以上のナビゲーションシステムは、ハードウエア的には、マイクロプロセッサや、メモリや、その他のグラフィックプロセッサやジオメトリックプロセッサ等の周辺デバイスを有する一般的な構成を備えたＣＰＵ回路を用いて構成されるであって良く、この場合、自律航法処理部５、現在位置算出部６、ルート探索部８、制御部９などは、マイクロプロセッサが予め用意されたプログラムを実行することにより実現されるものであって良い。

さて、このような構成において、制御部９は、現在位置算出部６が算出している現在位置が、自走式立体駐車場などの多階層施設内にあるときに、ＧＮＮＳ受信器４の衛星測位が良好に行えなくなったならば、図２に示すピッチ角誤差補正処理を開始する。

ここで、ピッチ角誤差補正処理は、ＧＮＮＳ受信器４の衛星測位が良好に行えないために自律航法処理部５がＧＮＮＳ利用補正機能を利用できない期間中、加速度センサ１やジャイロセンサドリフト（バイアス）によって、自律航法処理部５の現在位置や移動ベクトルの算定において生じる、ピッチ角に関する誤差であるピッチ角誤差を補正する処理である
さて、図示するように、ピッチ角誤差補正処理において、制御部９は、走行距離の計測を開始する（ステップ２０２）。なお、この時点までは、ＧＮＮＳ受信器４の衛星測位が良好に行えていた期間であるので、ステップ２０２の実行時点において現在位置算出部６において現在位置の標高値として正しい標高値が算出されている。また、この時点まではＧＮＮＳ受信器４の衛星測位が良好に行えていたことよりＧＮＮＳ利用補正機能によってピッチ角誤差が正しく補正されていた期間であるので、ステップ２０２の実行時点において、自律航法処理部５も、現在位置の標高値として正しい標高値を認識している。

そして、次に、施設地図データに基づいて多階層施設内に参照エリアを設定する（ステップ２０４）。
ここで、ステップ２０４では、図３に３階層の自走式立体駐車場について示したように各階の水平な平地部分のそれぞれを参照エリアに設定する。なお、図３において、灰色で示したエリアが、それぞれ参照エリアに該当し、この例では、各階につき一つの標高が異なる３つの参照エリアが設定されている。

そして、参照エリアを設定したならば（ステップ２０４）、自律航法処理部５が算出している現在位置の自動車の参照エリアへの到達の発生を監視する（ステップ２０６）。ただし、ステップ２０８の実行開始時点で、現在位置が参照エリア内にあるときには、現在位置の参照エリアへの到達が発生したものとする。

次に、参照エリアへの到達が発生したならば（ステップ２０６）、到達した参照エリアと同じ階の参照エリアに対して参照標高値と参照走行距離が登録されているかどうかを調べる（ステップ２０８）。

ここで、到達した参照エリアの階の識別は、たとえば、自律航法処理部５が算出した現在位置の経緯度を含む参照エリアであって、自律航法処理部５が算出した現在位置の標高値の増減パターンによって示される、走行距離の計測を開始した時点の現在位置が位置していた階からの上昇回数／下降回数から求まる、現在位置が位置している階を、参照エリアの階として識別することにより行う。

すなわち、たとえば、標高値の変化量の増減パターンが、所定の走行距離以上にわたる標高値の単位走行距離あたりの変化が小さい期間と、所定の走行距離以上にわたる標高値の単位走行距離あたりの変化が大きい期間があることを示している増減パターンであれば、所定の走行距離以上にわたる標高値の単位走行距離あたりの変化が大きい期間のうちの、標高値が増大している期間を一つ上の階への上昇を行っている期間とし、標高値が減少している期間を一つ下の階への下降を行っている期間として、走行距離の計測を開始した時点の現在位置が位置していた階からの上昇回数／下降回数を求め、上昇回数／下降回数より定まる現在位置が位置している階を、参照エリアの階として識別する。

または、単に、所定の走行距離以上にわたる標高値の単位走行距離あたりの変化の絶対値が所定のしきい値より大きい期間のうちの、標高値が増大している期間を一つ上の階への上昇を行っている期間とし、標高値が減少している期間を一つ下の階への下降を行っている期間として、走行距離の計測を開始した時点の現在位置が位置していた階からの上昇回数／下降回数を求め、上昇回数／下降回数より定まる現在位置が位置している階を、参照エリアの階として識別する。なお、この場合には、しきい値は、たとえば、予め見積もったピッチ角誤差がとりえる範囲中の絶対値が最大となるピッチ角誤差をθMAXとして、｜単位走行距離×sin(θMAX)｜に設定する。

そして、到達した参照エリアと同じ階の参照エリアに対して参照標高値と参照走行距離が登録されていなければ（ステップ２０８）、ステップ２０６で到達を検出した参照エリアに対して、自律航法処理部５が算出している現在位置の標高値を参照標高値として登録し、計測している走行距離を参照走行距離として登録する（ステップ２１８）。

そして、現在位置算出部６が算出している現在位置の参照エリアの通過を待ち（ステップ２２０）、ステップ２０６の監視に戻る。なお、ステップ２２０では、自律航法処理部５が算出した現在位置の経緯度が、ステップ２０６で到達を検出した参照エリアを逸脱したならば、参照エリアを通過したものと判定する。または、ステップ２２０では、所定の走行距離以上にわたって、自律航法処理部５が算出した現在位置の標高値の単位走行距離あたりの変化の絶対値が所定のしきい値以上大きくなったならば、参照エリアを通過したものと判定する。なお、この場合も、しきい値は、予め見積もったピッチ角誤差がとりえる範囲中の絶対値が最大となるピッチ角誤差をθMAXとして、｜単位走行距離×sin(θMAX) ｜に設定する。

一方、到達した参照エリアと同じ階の参照エリアに対して参照標高値が登録されている場合には（ステップ２０８）、到達した参照エリアと同じ階の参照エリアに対して登録されている参照標高値と自律航法処理部５が算出している現在位置の標高値との差と、計測している走行距離と到達した参照エリアに対して登録されている参照走行距離との差からピッチ角誤差を算出する（ステップ２１０）。

ここで、このステップ２１０におけるピッチ角誤差の算出は、自律航法処理部５が算出している現在位置の標高値と、到達した参照エリアと同じ階の参照エリアに対して登録されている参照標高値との差をdH、計測している走行距離と到達した参照エリアと同じ階の参照エリアに対して登録されている参照走行距離との差をdD、ピッチ角誤差をθeとして、
θe＝arctan(dH/ dD)
により算出する。

また、次に、算出したピッチ角誤差θeの絶対値が、しきい値Th以下であるかどうかを調べる（ステップ２１２）。ここで、しきい値Thは、予め見積もったピッチ角誤差がとりえる範囲中の絶対値が最大となるピッチ角誤差をθMAXとして、｜θMAX｜とする。

そして、ピッチ角誤差θeの絶対値が、しきい値Th以下でなければ（ステップ２１２）、ステップ２０６の監視に戻る。
一方、ピッチ角誤差θeの絶対値が、しきい値Th以下であれば（ステップ２１２）、算出したピッチ角誤差θeを反映した標高値に、自律航法処理部５が現在位置としている位置の標高値や、現在位置算出部６が現在位置としている位置の標高値を補正する（ステップ２１４）。

ここで、このステップ２１４における補正は、計測している走行距離をＤとして、補正前の標高値から、D ×sinθeを減ずることにより行う。
また、次に、算出したピッチ角誤差θeが、以降の現在位置の標高の算出からキャンセルされるように補正値に設定する（ステップ２１６）。
ここで、このステップ２１６の補正値の設定は、補正後の標高値を標高値の真値として、自律航法処理部５が算定する標高値が、真値に近づくように、自律航法処理部５が自動車の現在の状態の算定に用いる演算式を更新(学習）することにより行うようにしてもよいし、ピッチ角誤差θeをドリフト量として、加速度センサ１やジャイロセンサ２をドリフト量がキャンセルされるように校正することにより行うようにしてもよいし、
そして、ピッチ角誤差補正処理を終了する。

以上、制御部９が行うピッチ角誤差補正処理について説明した。
さて、制御部９は、このようなピッチ角誤差補正処理を、ＧＮＮＳ受信器４の衛星測位が良好に行えるようになった時点で強制的に終了する処理も行う。
さて、加速度センサ１やジャイロセンサドリフト（バイアス）によって、自律航法処理部５の現在位置や移動ベクトルの算定におけるピッチ角誤差がθeである場合、自律航法処理部５が算定している現在位置の標高値の、真の標高値に対する誤差Heは、図４ａに示すように、正しく標高値を算定していた時点からの走行距離をDとして、He＝D×sinθeとなる。

したがって、走行距離Ｄと標高Ｈの真の関係が図４ｂに示す関係となるように、自走式立体駐車状場に１階の入口から侵入し、２階、３階へと進んで駐車し、２階、１階と戻って、１階の出口で自走式立体駐車状場で退出した場合において、ピッチ角誤差θeが存在する場合、図４ｃの４３０に示すように、自律航法処理部５において算出される現在位置の標高値Ｈの誤差Heは、He＝D×sinθeの関係に従って、走行に伴って一様に増大していくものとなる。

一方、ピッチ角誤差θeは、図４ａの関係に従って、走行距離と、その走行距離を走行した期間中における標高誤差の増大量が分かれば、θe=arctan(標高誤差の増大量/走行距離)として求めることができる。

そして、図４ｄにおいて、４４０が自律航法処理部５で算定された現在位置の標高値を表し、黒丸が各参照エリア進入時に当該最小エリアに対して登録された参照標高値と参照走行距離を示すものとして、上述したピッチ角誤差補正処理のステップ２１０において算出した、到達した参照エリア(図では２Ｆの参照エリア）と同じ階の参照エリアに対して登録されている参照標高値と自律航法処理部５が算出している現在位置の標高値との差dHは、同じ階の水平な平地、すなわち、同じ標高に対して算出された標高値の差であるので、到達した参照エリアと同じ階の参照エリアに対して過去に参照標高値を登録した時点からステップ２１０の実行時点までの間の標高値の誤差の増大量である。

そして、ステップ２１０において算出した計測している走行距離と到達した参照エリアと同じ階の参照エリアに対して登録されている参照走行距離との差dDは、到達した参照エリアに対して過去に参照標高値を登録した時点からステップ２１０の実行時点までの間の走行距離を表す。

よって、上述のようにθe＝arctan(dH/ dD)によってピッチ角誤差θeを算定するピッチ角誤差補正処理によって、自律航法処理部５の現在位置や移動ベクトルの算定において生じる、ピッチ角誤差θeを正しく算出して、現在位置の標高値を正しい値に補正したり、以降の自律航法処理部５の現在位置や移動ベクトルの算定においてピッチ角や標高値が正しく算定されるように補正値を設定することができるようになる。

以上、本発明の実施形態について説明した。
ところで、以上の実施形態のピッチ角誤差補正処理では、多階層施設の各階の水平な平地部分のそれぞれを参照エリアに設定したが、これは、多階層施設の各階の水平な平地部分の一部または平地部分中の１または複数の地点を参照エリアに設定するようにしてもよい。

すなわち、たとえば、多階層施設の各階の水平な平地部分の、上層階や下層階からの入口となる地点を参照エリアに設定するようにしてもよい。
または、以上の実施形態のピッチ角誤差補正処理では、ステップ２０４において参照エリアを設定し、ステップ２０６における参照エリアへの到達の検出と、ステップ２２０における参照エリアの通過の検出を行うようにしたが、これは、ステップ２０６において、単に、所定の走行距離以上にわたり標高値の単位走行距離あたりの変化の絶対値が所定のしきい値より以下となったならば、参照エリアへの到達を検出するようにしてもよい。

なお、この場合にも、しきい値は、予め見積もったピッチ角誤差がとりえる範囲中の絶対値が最大となるピッチ角誤差をθMAXとして、｜単位走行距離×sin(θMAX) ｜に設定する。また、この場合における、参照エリアが何階の参照エリアであるかの識別は、上述したものと同様である。

また、この場合におけるステップ２２０での参照エリアの通過の検出は、所定の走行距離以上にわたって、自律航法処理部５が算出した現在位置の標高値の単位走行距離あたりの変化が所定のしきい値より大きくなったならば、参照エリアを通過したものと判定することにより行う。

１…加速度センサ、２…ジャイロセンサ、３…車速センサ、４…ＧＮＮＳ受信器、５…自律航法処理部、６…現在位置算出部、７…地図データ記憶部、８…ルート探索部、９…制御部、１０…表示装置、１１…入力装置。

Claims (8)

1. 自動車に搭載される、現在位置を算出する現在位置算出装置であって、
   自律航法によって標高を含む現在位置を算出する自律航法手段と、
   参照標高値を記憶する記憶手段と、
   前記自動車が、自走可能な多階層の施設内の各階の水平な平地を走行している際に、少なくとも当該走行している平地の階の参照標高値が前記記憶手段に記憶されていない場合に、前記自律航法手段が算定した標高値を、当該走行している平地の階の参照標高値として、前記記憶手段に記憶する参照値保存手段と、
   前記自動車が、前記施設内の異なる階の平地に移動した際に、当該移動した平地の階の参照標高値が前記記憶手段に記憶されている場合に、前記自律航法手段が算定した標高値と当該記憶されている参照標高値との差である標高誤差が、当該記憶されていた参照標高値が前記参照値保存手段によって記憶された時点から現時点までの走行距離である移動距離を進む間に生じたものとして、前記自律航法手段が前記標高の算出に用いたピッチ角の算定誤差を算定するピッチ角誤差算定手段と、
   前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差に基づいて、前記自律航法手段が算定している現在位置の標高を補正する補正手段とを有することを特徴とする現在位置算出装置。
2. 請求項１記載の現在位置算出装置であって、
   前記記憶手段は、前記参照標高値と共に、参照走行距離を記憶し、
   前記参照値保存手段は、前記参照標高値と共に、現時点における所定の時点からの走行距離を、当該走行している平地の階の参照走行距離として前記記憶手段に記憶し、
   前記ピッチ角誤差算定手段は、前記施設内の異なる階の平地に移動した際に、当該移動した平地の階の参照標高値と参照走行距離とが前記記憶手段に記憶されている場合に、前記移動距離を、現時点における前記所定の時点からの走行距離と当該記憶されている参照走行距離との差によって求めることを特徴とする現在位置算出装置。
3. 請求項１または２記載の現在位置算出装置であって、
   前記ピッチ角誤差算定手段は、
   ピッチ角の算定誤差＝arctan(標高誤差/移動距離）
   によって、前記ピッチ角の算定誤差を算定することを特徴とする現在位置算出装置。
4. 請求項１、２または３記載の現在位置算出装置であって、
   前記補正手段は、前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差が、所定の範囲内にない場合には、前記標高の補正を行わないことを特徴とする現在位置算出装置。
5. 請求項１、２または３記載の現在位置算出装置であって、
   前記施設に進入する前に衛星測位を利用して算出した標高に、前記自律航法手段が算定している現在位置の標高を補正する衛星測位利用補正手段を有し、
   前記補正手段は、前記自律航法手段が衛星測位を利用して標高を算出した時点から現時点までの走行距離をＬ、前記ピッチ角誤差算定手段が算定したピッチ角の算定誤差をθ、前記自律航法手段が現時点で算出している標高をＨとして、
   Ｈ-Ｌ×sinθ
   に、前記自律航法手段が算定している現在位置の標高を補正することを特徴とする現在位置算出装置。
6. 請求項１、２、３、４または５記載の現在位置算出装置と、
   当該現在位置算出装置が算出した現在位置を地図上に表した案内画面を表示する案内手段とを有することを特徴とするナビゲーションシステム。
7. 自動車に搭載されるピッチ角誤差算定装置において、自律航法によって標高を含む前記自動車の現在位置を算出する自律航法手段が、前記標高の算出に用いたピッチ角の算定誤差を算出するピッチ角誤差の算出方法であって、
   前記ピッチ角誤差算定装置において、前記自動車が、自走可能な多階層の施設内の各階の水平な平地を走行している際に、少なくとも当該走行している平地の参照標高値が記憶されていない場合に、前記自律航法手段が算定した標高値を、当該走行している平地の参照標高値として記憶する参照値保存ステップと、
   前記ピッチ角誤差算定装置において、前記自動車が、前記施設内の平地に移動した際に、当該移動した平地の参照標高値が記憶されている場合に、前記自律航法手段が算定した標高値と当該記憶されている参照標高値との差である標高誤差が、当該記憶されていた参照標高値が記憶された時点から現時点までの走行距離である移動距離を進む間に生じたものとして、前記自律航法手段が前記標高の算出に用いたピッチ角の算定誤差を算定するピッチ角誤差算定ステップとを有することを特徴とするピッチ角誤差の算出方法。
8. 請求項７記載のピッチ角誤差の算出方法であって、
   前記ピッチ角誤差算定装置は、前記ピッチ角誤差算定ステップにおいて、
   ピッチ角の算定誤差＝arctan(標高誤差/移動距離）
   によって、前記ピッチ角の算定誤差を算定することを特徴とするピッチ角誤差の算出方法。

Images