JP4664427B2 - 距離算出装置 - Google Patents

Description

本発明は、移動物体と固定対象物との間の距離算出方式に係り、さらに詳しくは例えば、走行車両と対象物の間の仰角や方位角、車両の速度やヨーレートなどの車両の状態量に基づいて、道路を走行している車両と対象物までの正確な距離を算出する距離算出装置、および算出プログラムに関する。

地図データベースやＧＰＳ（グローバル・ポジショニング・システム）を用いた車両の位置検出によって、車両の現在位置と目的地に応じてドライバに走行路を案内するカーナビゲーション技術が発達し、広範に普及している。そのようなシステムでは、ＧＰＳによって得られる緯度、経度の情報を基に、自車両の位置を地図上に重ねて表示し、交差点までの距離などを算出している。
このような従来技術の例として次の特許文献がある。

特許第３３８１３１２（Ｐ３３８１３１２）号 「ナビゲーション装置」

この特許文献には、地図情報によって位置が特定できる対象物を認識し、２つの時点の間の車両の走行距離と、各地点からの同一の対象物までの仰角を用いて対象物と自車両間の距離を算出し、自車両の位置を補正する技術が開示されている。

しかしながら、この従来技術では信号機までの直線距離しか計算することができず、曲線道路や信号機が自車の進行方向上に無い時など、実際の走行路上の距離を求める場合に誤差が生じるという問題点があった。

本発明の課題は上述の問題点に鑑み、対象物に対する車両からの仰角を含めて、方位角、速度あるいは加速度（ピッチング角）、ヨーレートなどを使い分けることによって、精度の高い距離計測機能を有する距離算出装置、および距離算出プログラムを提供することである。

図１は、本発明の距離算出装置の原理構成ブロック図である。同図は後述する距離算出装置の実施例１の原理構成ブロック図であり、実施例１の距離算出装置１は、少なくとも方位角算出手段２、移動距離算出手段３、角度変化量算出手段４、および直線距離算出手段５を備える。

方位角算出手段２は、移動物体、例えば走行車両と固定対象物、例えば信号機とを結ぶ方向と、移動物体との移動方向とが水平面上で成す角を方位角として算出するものであり、移動距離算出手段３は、２つの時点間、例えば直線距離算出の処理間隔としての１００ｍｓの間の移動物体の移動距離を算出するものである。

角度変化量算出手段４は、その２つの時点間の移動物体の重心を通る垂直軸周りの回転角度を、進行方向の角度変化量として算出するものであり、直線距離算出手段５は方位角算出手段２、移動距離算出手段３、角度変化量算出手段４の出力を用いて、移動物体と対象物の間の直線距離を算出するものである。

発明の実施の形態においては、移動物体の現在位置を検出する現在位置センサと、現在位置センサの出力に応じて現在位置周辺の道路を含む地図情報を出力するデータベース手段と、データベース手段、方位角算出手段２、および直線距離算出手段５の出力を用いて
、現在位置から対象物までの走行距離を算出する走行距離算出手段を、距離算出装置１はさらに備えることができる。

また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果、例えば対象物の座標を方位角算出手段２に与える対象物認識手段をさらに備えることもできる。この場合、対象物認識手段の出力があらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、その逸脱が検出された時、対象物認識手段の出力を補正して方位角算出手段に与える対象物認識補正手段をさらに備えることもできる。

あるいは移動物体の速度を移動距離算出手段３に与える速度センサと、前述の現在位置センサ、およびデータベース手段と、データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、ロール角と対象物の認識結果とを用いて対象物の認識結果、例えば座標を補正し、方位角算出手段２に与えるロール角補正手段をさらに備えることもできる。

さらに実施の形態においては、移動物体の鉛直軸周りの回転角速度を検出し、角度変化量算出手段４に与えるヨーレートセンサと、ヨーレートセンサと直線距離算出手段５との出力を用いて、移動物体から対象物までの距離を予測する距離予測手段をさらに備えることもでき、またヨーレートセンサの出力、および／または出力の変化範囲があらかじめ定められた出力の範囲、および／または出力の変化範囲を逸脱したか否かを監視するヨーレート監視手段と、逸脱が検出された時、過去のヨーレートや速度の値を用いてヨーレートセンサの出力を補正し、角度変化量算出手段５に与えるヨーレート補正手段とをさらに備えることもできる。

次に本発明の実施例３に対応する距離算出装置は、少なくとも仰角算出手段、移動距離算出手段、ピッチング角算出手段、および移動方向距離算出手段を備える。
仰角算出手段は移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するものであり、ピッチング角算出手段は移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するものであり、移動距離算出手段は図１に原理を説明した実施例１におけると同様に移動物体の２つの時点間の移動距離を算出するものである。そして移動方向距離算出手段は、仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて移動物体から対象物までの直線距離の（車両の）移動方向成分を算出する。

発明の実施の形態においては、実施例１に対すると同様に、前述の現在位置センサ、データベース手段、および移動距離算出手段をさらに備えることもできる。
また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果を仰角算出手段に与える対象物認識手段をさらに備えることもでき、また前述と同様の対象物認識監視手段、および対象物認識補正手段をさらに備えることもできる。

またこの場合、前述と同様の速度センサ、現在位置センサ、データベース手段、ロール角算出手段、およびロール角補正手段をさらに備えることもでき、ロール角補正手段は、対象物の認識結果を補正してその補正結果を仰角算出手段に与える。

さらに実施の形態においては、移動物体の移動方向の加速度を検出する加速度センサをさらに備え、ピッチング角算出手段が加速度センサの出力を用いてピッチング角を算出することもできる。

あるいは、前述と同様の現在位置センサ、データベース手段に加えて、データベース手段の出力を用いて移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段
をさらに備え、ピッチング角算出手段が、傾斜角算出手段の出力を用いてピッチング角の算出を行うこともでき、あるいはさらに加速度センサを備え、ピッチング角算出手段が傾斜角算出手段の出力に加えて、加速度センサの出力を用いてピッチング角の算出を行うこともできる。

以上のように本発明によれば、移動物体の方位角や、移動物体の進行方向の角度変化量などを用いて、あるいは移動物体の仰角や移動物体のピッチング角などを用いて、対象物までの直線距離や移動物体の移動方向距離などの算出が行われる。

本発明の距離算出装置の原理構成ブロック図である。 本発明の距離算出装置の実施例１の構成ブロック図である。 実施例１における距離算出の全体処理フローチャートである。 対象物認識処理の詳細フローチャートである。 方位角算出方法の説明図である。 直線距離算出方法の説明図である。 直線距離算出処理の詳細フローチャートである。 走行距離予測方法の説明図である。 距離算出装置の実施例２の構成ブロック図である。 走行距離算出方法の説明図である。 走行距離算出処理の詳細フローチャートである。 距離算出装置の実施例３の構成ブロック図である。 実施例３における進行方向距離算出処理の全体フローチャートである。 傾斜角算出を説明するための車両の走行状態の説明図である。 傾斜角算出方法の説明図である。 傾斜角算出処理の詳細フローチャートである。 進行方向距離算出を説明するための車両の走行状況を示す図である。 進行方向距離算出方法の説明図である。 進行方向距離算出処理の詳細フローチャートである。 距離算出装置の実施例４の構成ブロック図である。 距離算出装置の実施例５の構成ブロック図である。 距離算出装置の実施例６の構成ブロック図である。 距離算出装置の実施例７の構成ブロック図である。 距離算出装置の実施例８の構成ブロック図である。 ロール角補正方法の説明図である。 ロール角補正処理の詳細フローチャートである。 距離算出装置の実施例９の構成ブロック図である。 本発明のプログラムのコンピュータへのローディングを説明する図である。

以下本発明の実施の形態をいくつかの実施例に分けて詳細に説明する。
本発明において移動物体、例えば走行中の車両から、例えば直線距離が算出される固定対象物としては、信号機や道路標識をはじめとして、走行中の車両に設置されたカメラによって画像を捉えることができる看板、踏み切り、歩道橋、横断歩道、停止線などの各種の対象物までの距離が算出される。距離算出のために画像センサ、ヨーレートセンサ、車速センサ、および加速度センサなどからデータを、例えば１００ｍｓ毎に取り込んで、対象物認識や方位角算出などの各種の処理を１００ｍｓ毎に行い、最終的に例えば対象物までの直線距離を１００ｍｓ毎に算出する。以下の説明では、信号機を対象物の具体例として実施例を説明する。

図２は本発明の距離算出装置の実施例１の構成ブロック図である。同図において距離算出装置１０に対しては、画像センサ１１、車速センサ１２、ヨーレートセンサ１３の出力がそれぞれ与えられる。画像センサ１１としては、例えばＣＣＤカメラやＣＭＯＳカメラなどが用いられ、カメラは進行方向を撮影するために車両の前方に設置され、その光軸は基本的に車両の進行方向と同一となるように設置される。車速センサ１２は、車両の車輪の速度を検出するものであり、例えば４個の車輪にそれぞれ設置される４つのセンサによって構成される。ヨーレートセンサ１３は、車両の重心を通り、鉛直方向の軸の周りの車両の回転角速度を検出するものである。

距離算出装置１０の内部の対象物認識部１４は、画像センサ１１によって撮像された画像を用いて、対象物の認識、例えば対象物の座標の認識を行うものである。前述のように対象物としては信号機を例にとるが、信号機の認識方法については図４で後述する。方位角算出部１５は、対象物認識部１４によって認識された対象物、例えば信号機の方位角を算出するものである。すなわち車両の重心を通る水平面上で、車両の重心と認識された対象物とを結ぶ線分の投影線と、車両の進行方向とが成す角を方位角として算出するものである。

移動距離算出部１６は、車速センサ１２の出力を用いて、１００ｍｓあたりの車両の移動距離を算出するものである。角度変化量算出部１７は、ヨーレートセンサ１３の出力を用いて、処理間隔１００ｍｓの間の鉛直方向の回転軸周りの車両の回転角を角度変化量として算出するものである。ヨーレートセンサ１３の角速度がω（ｒａｄ／ｓ）の時に、角度変化量は０．１ω（ｒａｄ）となる。

直線距離算出部１８は、ここでは方位角算出部１５、移動距離算出部１６、および角度変化量算出部１７の出力を用いて車両と対象物の間の直線距離を算出するものである。この算出方法については図６、７で後述する。走行距離予測部１９は、直線距離算出部１８、ヨーレートセンサ１３の出力を用いて、車両の現在地点から対象物の地点までの走行距離を予測するものである。その算出方法については図８で後述する。

図３は、実施例１における距離算出の全体処理フローチャートである。同図において処
理が開始されると、まずステップＳ１で画像センサ１１の出力を用いて対象物の位置認識、例えば信号機の赤や青の信号の画像中の座標の出力が行われ、ステップＳ２で車両の進行方向と対象物への方向とが成す角、すなわち方位角が算出され、ステップＳ３で車速センサ１２の出力を用いてある２つの時点の間の車両の移動距離、すなわち処理間隔１００ｍｓの間の移動距離が算出され、ステップＳ４でヨーレートセンサ１３の出力を用いて車両の鉛直軸周りの角度変化量が算出される。

ただし、ここでステップＳ１からステップＳ４はそれぞれ独立に実行することができ、その順序を変更することも、また並行して処理することも可能である。続いてステップＳ５では、算出された方位角、移動距離、角度変化量を用いて対象物までの直線距離が算出され、ステップＳ６でその直線距離の算出結果とヨーレートセンサ１３の出力とを用いて、走行距離の予測が行われ、処理を終了する。

図４は、図２の対象物認識部１４による対象物認識処理の詳細フローチャートである。同図において処理が開始されると、ステップＳ１１で特定の色、ここでは信号の青、黄、赤の色をもつ領域の検出が行われる。灯火している信号の色の領域を抜き出すことを考えるが、どの色が灯火しているかわからないため、ここでは３つの色の領域の検出をそれぞれ試みるものとする。３つの色それぞれに対し、複数の領域が検出されることも、領域がひとつも検出されないことも考えられる。

ステップＳ１２で抽出された領域の大きさ、ここでは３つの色によって抽出されたすべての領域の大きさがそれぞれ取得され、ステップＳ１３で信号機までの距離にもよるが、正確な認識のためにあらかじめ設定された十分な範囲を持つ大きさによって領域の絞込みが行われる。

続いてステップＳ１４で各領域、ここではステップＳ１３で抽出されたすべての領域の周囲長がそれぞれ取得され、ステップＳ１５でその周囲長ｌと領域の面積Ｓとを用いて、各領域の円形度が算出される。円形度は４πＳ／ｌ^２によって算出され、ステップＳ１６
で円形度の大きさによる領域の絞込み、すなわち円形度の値があらかじめ定められた閾値、例えば０．８よりも大きい時にのみ、その領域が信号機の信号光の領域として認識される。そして処理結果として、画像中の信号機の信号光の領域の中心の座標が出力される。ここで円形度は完全な円に対しては１となるが、ここではステップＳ１３で抽出されたすべての領域の中で円形度が閾値より大きい領域の中心の座標が出力されるものとする。座標が複数出力されることも考えられるが、その場合出力された座標それぞれに関して、方位角等の算出を行うものとする。

図５は方位角算出部１５による方位角算出方法の説明図である。同図は後述する仰角の算出にも同様に用いられるが、方位角の算出においては画像上のｘ座標、すなわちカメラのレンズの位置Ｌを通る水平面上で、車両の進行方向に垂直な方向のｘ座標を用いて計算が行われる。

図５において信号機の水平面上の位置をＳ、Ｓからレンズの光軸方向に垂直に前述の平面に垂線をおろした時、その足をＯ、レンズの焦点距離をＦ、撮像面の位置をＯ’、画像中の信号機（信号光）のｘ座標をｘ（ピクセル単位）、ピクセルのｘ方向の寸法をｌとすると、求めるべき方位角αはα’と等しく、方位角はｔａｎ^−１（ｘｌ／Ｆ）によって求めることができる。

図６は、直線距離算出部１８による車両から対象物Ｓまでの距離Ｘの算出方法の説明図である。図６に示すように信号機の位置をＳとし、車両が時刻Δｔの間にＡ１地点からＡ２地点に移動したとする。Ａ１地点からの信号機の方位角をθ_１、Ａ２地点からの信号機
の方位角をθ_２とする。Ａ１のときの車両の進行方向にＡ２から垂線を下ろし、交点をＡ
２’とする。Ａ１−Ａ２’の距離をＤ’、Ａ２−Ａ２’の距離をｄとすると、移動距離算出部１６により算出された移動距離Ｄと角度変化量算出部１７により算出された角度変化量γにより、Ｄ’とｄを近似的に表すと、
Ｄ’＝Ｄｃｏｓγ
ｄ＝Ｄｓｉｎγ
Ａ１のときの進行方向にＳから垂線を下ろし、交点をＯとする。Ａ２’地点からＯまでの距離をＸ’とすると、下記の式が成り立つ。

ｄ＋Ｘ’ｔａｎ（θ_２＋γ）＝（Ｘ’＋Ｄ’）ｔａｎθ_１
これにＸ’について解くと
Ｘ’＝（Ｄ’ｔａｎθ_１−ｄ）／（ｔａｎ（θ_２＋γ）−ｔａｎθ_１）
＝（Ｄｃｏｓγｔａｎθ_１−Ｄｓｉｎγ）／（ｔａｎ（θ_２＋γ）−ｔａｎθ_１）
Ａ２からＳまでの距離Ｘは以下の式によって表される。

Ｘ＝Ｘ’／ｃｏｓ（θ_２＋γ）
図７は直線距離算出部１８による、車両から対象物までの直線距離算出処理のフローチャートである。同図において処理が開始されるとステップＳ２０で移動距離算出部１６によって移動距離Ｄが、ステップＳ２１で方位角算出部１５によって方位角θ_１、θ_２が、ステップＳ２２で角度変化量算出部１７によってγの値が設定され、ステップＳ２３でＸ’の算出式の分母の絶対値がある閾値ｔ_１が大きいか否かが判定される。この分母が０であ
る場合、Ｘ’の値を求めることができず、分母が０に近い場合にもＸ’を正確に求めることができないため、そのような場合には直ちに処理を終了する。

閾値ｔ_１より大きい場合には、ステップＳ２４でＸ’の値が算出され、ステップＳ２５
でＸの算出式の分母がある閾値ｔ_２より大きいか否かが判定され、大きくない場合には直
ちに処理を終了し、大きい場合にはステップＳ２６でＸの値が算出されて処理を終了する。

ステップＳ２３でＸ’の分母の絶対値が閾値ｔ_１以下でＸ’の値を算出できなかった場
合には、過去の車両と対象物までの距離、およびその後の車両の車速やヨーレートなどを用いて車両の現在位置を予測し、対象物までの直線距離を算出することができる。図６においてＡ１地点から信号機Ｓまでの距離をＸ”とすると、Ｘ’は次式によって与えられ、前述のＸ’をこの値で置き換えてＸを算出することも可能である。

Ｘ’＝Ｘ”ｃｏｓθ_１−Ｄ’
図７のステップＳ２５でＸの分母が閾値ｔ_２以下である時には、Ｘの算出を行うことが
できないが、速度が極端が遅く、移動距離があまりない時には、Ｘ”の値からＤの値を減算し、Ｘの値を近似的に求めることが可能である。

図８は、走行距離予測部１９による、現在位置から対象物までの走行距離の予測方法の説明図である。走行距離予測部１９は、車両の現在位置からの走行路に沿った対象物までの距離を、過去のヨーレートと車両の速度から予測するものである。この予測においては、現在位置Ａから対象物の位置Ｓまでの走行路上で、車両の重心を通る垂直軸回りの回転角速度としてのヨーレートの値が一定であり、また車の速度の絶対値も一定であるとして、微小時間Δｔおきに、図８に示すようにベクトルｌ_０、ｌ_１、ｌ_２・・を予測し、ベクト
ルの総和が対象物の位置Ｓに達するまで演算を繰り返すことによって、ベクトルの長さの総和を取り、対象物までの走行距離を予測する。

図８において、車両の現在地点Ａから現在の進行方向に対して対象物の位置から下ろし
た垂線の足をＳ’とすると、最初のベクトルｌ_０がＳ’の方向と成す角は、角速度ωと微
小時間Δｔの積となる。次のベクトルｌ_１とＳ’方向の成す角は、２ωと微小時間Δｔと
の積となる。同様にして各ベクトルの方向を求め、また各ベクトルの長さを現在の車両の速度ｖと微小時間Δｔとの積として計算することによって、各ベクトルの長さとその方向が決定され、対象物の位置Ｓに達するまでの走行距離の予測が行われる。

次に本発明の実施例２について説明する。図９は距離算出装置の実施例２の構成を示すブロック図である。同図を実施例１と比較すると、走行距離予測部１９に代わって走行距離算出部２３が備えられ、走行距離算出部２３に対しては、方位角算出部１５の出力と直線距離算出部１８の出力に加えて、距離算出装置１０の外側の現在位置センサ２１の出力を用いるデータベース２２の出力が与えられている。

現在位置センサ２１は車両の現在位置を検出するものであり、例えば一般的にカーナビゲーションシステムに用いられるＧＰＳ（グローバル・ポジショニング・システム）である。ＧＰＳによって車両の現在位置の緯度、経度、標高などの絶対位置を得ることが可能であるが、この位置には一般的にある程度の誤差が含まれているため、誤差を考慮してそれらの位置を参照することが必要である。

データベース２２は道路の形状（標高や曲率半径）、信号機や歩道橋、踏み切りのような対象物の位置などを示す地図情報であり、現在位置センサ２１によって検出された自車両の位置を用いて地図上の車両の位置を特定することができる。データベース２２は、例えば自車両付近の対象物のデータや、道路形状のデータなどを走行距離算出部２３に与える。

走行距離算出部２３は、データベース２２の地図情報によって得られる対象物の位置に基づき、直線距離算出部１８によって算出された対象物と自車両との距離を用いて対象物への方向と、自車両の進行方向との成す角が、方位角算出部１５によって算出された方位角と等しくなる点を自車両の位置として決定し、その位置から対象物までの道路上の距離を地図情報から算出するものである。この算出方法について図１０、および図１１を用いて説明する。

図１１は、走行距離算出処理のフローチャートである。まずステップＳ３１で、図１０に示すように対象物Ｓの位置を中心として、自車両と対象物との直線距離を半径とする円が地図上に描かれ、ステップＳ３２でその円の円周がかかる道路上の円弧、図１０では４つの円弧が検出され、ステップＳ３３で各円弧の長さｌが算出される。

続いてステップＳ３４で各円弧の端、すなわち図１０において円上で反時計方向に動く点を考えるとき、その点が最初に各円弧上に達する位置から各円弧上でｌ／４の長さの位置が車両の候補位置として検出される。すなわち、ここでは道路は上下各１車線とし、左側の車線の中心位置が車両の候補位置として検出される。

続いてステップＳ３５で、車両の候補位置における車両の進行方向を示す線、すなわち道路の接線方向と平行な線と、車両の候補位置と対象物とを結ぶ線との成す角がそれぞれの円弧について算出され、ステップＳ３６で算出された角度の値、図１０では４つの値のうちで、方位角算出部１５によって算出された方位角の値と最も近い車両の候補位置が車両の現在位置として決定され、ステップＳ３７で車両の現在位置から対象物までの距離が、道路形状に沿って算出されて処理を終了する。

次に本発明の実施例３を図１２のブロック図を用いて説明する。同図において距離算出装置１０は、図９で説明した実施例２と同様に、画像センサ１１の出力を用いる対象物認
識部１４、車速センサ１２の出力を用いる移動距離算出部１６、現在位置センサ２１の出力を用いるデータベース２２に加えて、対象物認識部１４の出力を用いて対象物方向の仰角を算出する仰角算出部２５、データベース２２の出力を用いて車両の傾斜角を算出する傾斜角算出部２６、傾斜角算出部の２６の出力を用いて車両のピッチング角を算出するピッチング角算出部２７を備え、直線距離算出部１８に代わる進行方向距離算出部２８は、仰角算出部２５、移動距離算出部１６、およびピッチング角算出部２７の出力を用いて対象物までの直線距離の車両の進行方向成分を算出する。

仰角算出部２５による仰角の算出方法は、図５で説明した方位角の算出方法と基本的に同様である。ただし、仰角に対しては図５の撮像面上で、車両の進行方向を含む水平面に垂直なｙ軸方向の座標に加えて、ｘ軸方向の座標とレンズの焦点距離Ｆを用いて計算が行われる。すなわち、図５で点Ｓ’の信号機（信号光）のｘ、ｙ座標をｘ、ｙとすると、ピクセルのｘ、ｙ方向の寸法ｌを用いて仰角の値は

によって求められる。
傾斜角算出部２６は、現在位置センサ２１により検出された現在位置周辺の地図データがデータベース２２から与えられることにより、現在位置周辺で標高値のわかっている２つの地点の標高の差に基づいて、自車両周辺位置の傾斜角を算出し、ピッチング角算出部２７に与えるものであり、傾斜角算出方法の詳細については後述する。

ピッチング角算出部２７は、傾斜角算出部２６によって算出された傾斜角、すなわち道路の進行方向の傾斜角を用いて車両のピッチング角、すなわち車両の前後を結ぶ軸と水平線との縦揺れを示す角度を算出するものである。車両のピッチング角が変化すると仰角が変化し、対象物までの正確な距離を求めることが不可能となる。このため、ピッチング角を算出し、仰角を補正する必要がある。この仰角の補正は、仰角算出部２５によって算出された仰角に対して進行方向距離算出部２８によって行われるものとする。

車両のピッチング角と道路の傾斜角との関係は、車両の前後左右のサスペンションの伸縮によって決まり、サスペンションの伸縮はバネ係数によって決まる。そのため、本実施例では傾斜角と車両のピッチング角との間の関係を実験的に求めるものとする。車両が水平な面上に停止している時と、傾斜角θの斜面に停止している時のピッチング角を測定する。傾斜角θを変化させてピッチング角を測定することにより、傾斜角とピッチング角との間の関係が実験的に求められる。傾斜角に対するピッチング角の測定結果がない場合には、測定結果を補間することによってピッチング角が算出される。

図１３は、実施例３における進行方向距離算出処理の全体フローチャートである。同図において処理が開始されると、まず対象物認識部１４によって図４におけると同様に対象物の位置が認識され、ステップＳ４２で対象物への方向の仰角が算出され、ステップＳ４３で移動距離が算出され、ステップＳ４４で傾斜角算出部２６の出力を用いてピッチング角が算出され、ステップＳ４５で車両の現在位置から対象物までの距離を表すベクトルの車両の進行方向と平行な成分として、進行方向距離が算出されて処理を終了する。

傾斜角の算出について説明する。図１４、および図１５は、傾斜角の算出方法の説明図であり、図１６は、傾斜角算出処理のフローチャートである。図１４は、データベース２２から出力される地図上での、車両の走行状態の説明図である。車両の現在位置周辺に、
標高の値がわかっている２つの地点Ｐ１とＰ２が存在するものとする。

図１５は傾斜角θの算出方法の説明図である。図１４で説明したように、車両が点Ｐ１からＰ２まで走行する間の標高差をｈ２−ｈ１とし、地図上での点Ｐ１とＰ２の間の距離をｄとすると傾斜角θは次式によって与えられる。

θ＝ｔａｎ^−１｛（ｈ２−ｈ１）／ｄ｝
図１６の傾斜角算出処理のフローチャートにおいて処理が開始されると、まずステップＳ５１、Ｓ５２で自車両の現在の位置から最も距離が近く、標高の値の既知な２つの地点Ｐ１、Ｐ２が探索され、ステップＳ５３で２つの地点の間の地図上での距離ｄが算出され、ステップＳ５４で傾斜角θの算出が行われて処理を終了する。

次に進行方向距離の算出について、図１７−図１９を用いて説明する。図１７は、車両の進行状態の説明図である。同図において車両は処理時間、例えば１００ｍｓの間に点Ａ１から点Ａ２まで距離Ｄだけ移動したものとする。車両は直線方向に進行を続けるものと仮定し、対象物の位置Ｓから進行方向に下ろした垂線の足をＳ’とし、点Ａ２からＳ’までの距離Ｘを進行方向距離として算出するものとする。なお車両は実際には点Ａ２からはカーブする道路上を対象物Ｓに近づいていくものとする。

図１８は、車両の進行状態を真横から見た状態を示す図である。図１８に示すように車両が時刻Δｔの間にＡ１地点からＡ２地点に移動したとする。Ａ１地点からの信号機の仰角をφ_１、Ａ２地点からの信号機の仰角をφ_２とする。Ａ１地点からＡ２地点までの移動距離算出部１６により算出した移動距離をＤ、Ａ２地点から信号機までの距離の車両の進行方向と平行な成分をＸとすると、下記の式が成り立つ。

（Ｘ＋Ｄ）ｔａｎφ_１＝Ｘｔａｎφ_２
Ｘについて解くと、
Ｘ＝Ｄｔａｎφ_１／（ｔａｎφ_２−ｔａｎφ_１）
となり、対象物までの距離が算出できる。カメラの光軸方向が車両の進行方向に比べ仰角ρ_ｃだけ上を向いている（カメラの設置時に設定）場合でも、距離Ｘは以下の式によって
表される。

Ｘ＝Ｄｔａｎ（φ_１＋ρ_ｃ）／（ｔａｎ（φ_２＋ρ_ｃ）−ｔａｎ（φ_１＋ρ_ｃ））
また、Ａ１地点においてピッチング角算出部２７により算出したピッチング角をρ_１、
Ａ２地点においてピッチング角算出部２７により算出したピッチング角をρ_２とする。こ
のとき、距離Ｘは以下の式によって表される。

Ｘ＝Ｄｔａｎ（φ_１＋ρ_ｃ＋ρ_１）／（ｔａｎ（φ_２＋ρ_ｃ＋ρ_２）−ｔａｎ（φ_１＋ρ_ｃ＋ρ_１））
このように、進行方向距離算出部２８は、仰角算出部２５により算出された仰角、移動距離算出部１６により算出した移動距離およびピッチング角算出部２７により算出したピッチング角に基づき、車両と対象物の距離の車両の進行方向と平行な成分を算出する。

図１９の進行方向距離算出処理が開始されるとステップＳ５６で移動距離算出部１６によって移動距離Ｄが、ステップＳ５７で仰角算出部２５によって２つの仰角が、ステップＳ５８でピッチング角算出部２７によって２つのピッチング角の値が設定され、ステップＳ５９でＸの値を算出する式の分母の絶対値がある閾値ｔ_１より大きいか否かが判定され
、ｔ_１以下の値であるときには直ちに処理を終了し、閾値を超えている場合にはステップ
Ｓ６０でＸの値が算出されて処理を終了する。ステップＳ５９でＸを算出する式の分母の絶対値が０、または０に近い時、Ｘを正確に算出することはできないが、車両の速度が極
端に遅く、移動距離Ｄが小さい時には１つ前の時点で算出した距離Ｘから移動距離Ｄを減算して、進行方向距離Ｘを近似的に求めることもできる。

続いて本発明の実施例４について説明する。図２０は実施例４の距離算出装置の構成ブロック図である。同図を図１２の実施例３と比較すると、距離算出装置１０の外部に現在位置センサ２１に代わって、車両の前後方向、すなわち進行方向の加速度を検出する加速度センサ３０が備えられ、ピッチング角算出部２７が加速度センサの出力を用いてピッチング角を算出して、進行方向距離算出部２８に与えることが異なっている。図２０において加速度センサ３０は、例えば車両の中央部分に設置され、圧電素子などによって車の前後方向の加速度成分を検出する。ピッチング角算出部２７は加速度センサ３０の出力としての加速度と、車両のピッチング角との間の既知の関係からピッチング角を算出するものとする。

車両のピッチング角と加速度との関係は、前述の傾斜角との関係のようにサスペンションの伸縮、すなわちバネ係数によって決まる。このため、加速度とピッチング角の関係を実験的に求めるものとする。

すなわち、車両が水平な面上に停止している時と、水平な面上をある一定の加速度で走行している時のピッチング角の測定を行う。加速度の値を変化させ、ピッチング角を測定することによって、加速度とピッチング角の間の関係が実験的に求められる。加速度センサ３０によって検出された加速度の値に対するピッチング角が測定されていなかった場合には、測定結果を補間することによってピッチング角が求められる。

続いて本発明の実施例５について説明する。図２１は、距離算出装置の実施例５の構成を示すブロック図である。同図を実施例３を示す図１２、実施例４を示す図２０と比較すると、これらの実施例のすべての構成要素を備えており、ピッチング角算出部２７が傾斜角算出部２６の出力に加えて、加速度センサ３０の出力も用いてピッチング角を算出し、進行方向距離算出部２８に与えることだけが異なっている。

図２１のピッチング角算出部２７は、ピッチング角と道路の傾斜角、および加速度との間の実験的関係に基づいてピッチング角を算出する。ある一定の傾斜角の斜面を一定加速度で車両が走行している時のピッチング角を測定し、傾斜角、および加速度を変えて測定を繰り返すことによって、傾斜角と加速度が与えられた時のピッチング角を算出することが可能となる。あるいは実施例３において述べたピッチング角と傾斜角との間の実験的関係と、実施例４において述べたピッチング角と加速度との間の関係を用いて、それぞれピッチング角を求め、それらの和を計算することによって、傾斜角と加速度に対応するピッチング角を算出することも可能である。

図２２は距離算出装置の実施例６の構成ブロック図である。同図を図１２の実施例３と比較すると、方位角算出部１５と走行距離算出部２３がさらに備えられている点が異なっている。方位角算出部１５は、図２の実施例１におけると同様に車両の方位角を算出し、走行距離算出部２３に与える。

走行距離算出部２３は、方位角算出部１５の出力、データベース２２の出力、進行方向距離算出部２８の出力を用いて走行距離を算出する。図９で説明した実施例２では走行距離算出部２３は、直線距離算出部１８の出力、すなわち車両の位置から対象物までの直線距離を用いて、対象物までの道路に沿った走行距離を算出している。その様子は図１０に説明されている。一方、図２２の進行方向距離算出部２８は、図１７で説明したように車両の進行方向の距離Ｘを算出しており、車両の現在位置と対象物までの直線距離は方位角算出部１５の出力、すなわち∠ＳＡ_２Ｓ’を用いることによって容易に算出される。従っ
てこの直線距離を用いることによって、走行距離算出部２３は対象物までの走行距離を算出することができる。

図２３は距離算出装置の実施例７の構成を示すブロック図である。同図を図１２で説明した実施例３と比較すると、図２の実施例１などで説明したヨーレートセンサ１３と、走行距離予測部１９をさらに備えている点が異なっている。ここで走行距離予測部１９は、図２では直線距離算出部１８の算出結果、すなわち車両の現在位置から対象物までの直線距離に代わって、進行方向距離算出部２８の算出結果、すなわち図１７で説明した距離Ｘを用いて、図８で説明した方法と同様にして対象物までの走行距離を予測するものである。

図２４は本発明の距離算出装置の実施例８の構成ブロック図である。同図を図２の実施例１と比較すると、距離算出装置１０の外部に現在位置センサ２１が備えられ、装置１０の内部に現在位置センサ２１の出力を受け取るデータベース２２と、データベース２２の出力と車速センサ１２の出力とを用いて車両のロール角、すなわち車両が進行方向の軸の周りに回転する角度としてのロール角を算出するロール角算出部３２と、対象物認識部１４と方位角算出部１５との間に備えられ、対象物認識部１４の出力とロール角算出部３２の出力とを用いて、ロール角の発生によって生じる対象物認識部１４による認識結果の誤差を補正して、補正結果の対象物の座標を方位角算出部１５に与えるロール角補正部３３を備えている点が異なっている。

図２４のロール角算出部３２は、一般的に車両がカーブを走行している時に発生するロール角を算出するものである。このロール角が発生すると、対象物認識部１４による認識結果が不正確となる、すなわち画像センサ１１の光軸が回転することにより、仰角、方位角ともに変化し、対象物までの正確な距離を求めることが不可能となる。そこでロール角を算出し、対象物認識部１４による認識結果を補正し、方位角を正しく算出することが必要となる。

ロール角は車両が走行している道路の曲率半径とサスペンションの伸縮によって決まる。ロール角と道路の曲率半径の関係を実験的に算出するものとし、一定曲率半径の道路を一定速度で走行した時のロール角を測定する。曲率半径と速度の値を様々に変化させ、ロール角の値を測定することによって、ロール角と道路の曲率半径、および車両の速度との間の関係を実験的に求めることができる。

ロール角補正部３３は、ロール角算出部３２によって算出されたロール角を用いて対象物認識部１４の認識結果、例えば信号機の信号光の中心の画像上の座標位置の補正を行う。図２５はロール角補正方法の説明図である。ロール角θによる座標軸の傾きを補正するために座標（ｘ’、ｙ’）から座標（ｘ、ｙ）を算出する。

図２５に示すようにロール角をθ、対象物の座標（ｘ’、ｙ’）とし、車両の重心からカメラの中心までの高さをＬ、カメラの画素サイズをｌ、補正後の対象物の座標を（ｘ、ｙ）とする。ｌを用いてｘ’、ｙ’を画像の中心からの距離Ｘ’、Ｙ’に置き換える。

Ｘ’＝ｘ’・ｌ、Ｙ’＝ｙ’・ｌ、
Ｘ、ＹとＸ’、Ｙ’の関係により以下の２式が成立する。
（Ｌ＋Ｙ）_２＋Ｘ_２＝（Ｌ＋Ｙ’）_２＋Ｘ’_２

これをＸおよびＹについて解くと以下の式となる。

最後に、カメラの画素サイズｌを用いて下式のように画像上の座標系に戻す。
ｘ＝Ｘ／ｌ、ｙ＝Ｙ／ｌ
図２６は、ロール角補正処理のフローチャートである。同図において、ステップＳ６５でロール角θが生じた場合の対象物の座標（ｘ’、ｙ’）から、対象物の画像の中心からの距離Ｘ’、Ｙ’が算出され、ステップＳ６６でθ’の値が算出され、ステップＳ６７で補正後の対象物の画像の中心からの距離Ｘ、ステップＳ６８でＹが算出され、ステップＳ６９で補正後の画像上の座標（ｘ、ｙ）が算出されて処理を終了する。

最後に本発明の実施例９について説明する。図２７は距離算出装置の実施例９の構成ブロック図である。同図を例えば図２の実施例１と比較すると、走行距離予測部１９に代わって走行距離算出部２３が設けられ、走行距離算出部２３に対しては直線距離算出部１８の出力に加えて、方位角算出部１５の出力と現在位置センサ２１の出力に基づくデータベース２２の出力とが与えられ、対象物認識部１４に対してその認識結果に異常がないか否かを監視する対象物認識監視部３５と、異常があった場合に対象物認識部１４の認識結果を補正して方位角算出部１５に与える対象物認識補正部３６、同様にヨーレートセンサ１３の出力（角速度）、または出力の変化率（角加速度）を監視するヨーレートセンサ監視部３８と、出力、または出力の変化率に異常があった時、ヨーレートセンサの出力を補正して角度変化量算出部１７に与えるヨーレート補正部３９を備えている点が異なっている。

図２７において対象物認識部１４は、画像センサ１１によって撮像された画像に基づいて対象物を認識するものであるが、様々な原因によって認識結果に誤りが生じる可能性がある。そのため過去の認識結果と比較して、例えば対象物としての信号機の位置が処理間隔１００ｍｓの間に著しくずれている場合など、対象物の認識結果に異常があると判断される場合に、対象物認識監視部３５はその異常を対象物認識補正部３６に通知する。

対象物認識補正部３６は、対象物認識部１４の認識結果に異常があると通知された時に、過去の対象物認識結果に基づき、線形予測やカルマンフィルタを用いた予測を行った結果を、対象物認識の補正結果として方位角算出部１５に与える。

ヨーレートセンサ監視部３８は、ヨーレートセンサ１３の出力、すなわちヨーレートセンサが出力する車両の鉛直軸周りの回転角速度の値が急変した場合など、車両の正常な動きとしては考えにくい角速度の変化などを検出した時に、それをヨーレート補正部３９に通知する。

ヨーレート補正部３９は、ヨーレートセンサ監視部３８からヨーレートセンサ１３の出
力やその変化率の異常が通知された時に、ヨーレートセンサの出力値に代わって、過去のヨーレートや速度の値などに基づき線形予測やカルマンフィルタによる予測の結果を利用して補正されたヨーレートの値を角度変化量算出部１７に与える。

なお、図２７では、例えば図９の実施例２と比較して対象物認識監視部３５、対象物認識補正部３６、ヨーレートセンサ監視部３８、およびヨーレート補正部３９が追加された構成となっているが、例えば図２３の実施例７を基本として、これらの各部が追加され、対象物認識補正部３６が認識結果を補正して仰角算出部２５に与える構成も当然可能である。

以上において、本発明の距離算出装置、および算出プログラムについてその詳細を説明したが、この距離算出装置は当然一般的なコンピュータシステムとして構成することが可能である。図２８はそのようなコンピュータシステム、すなわちハードウェア環境の構成ブロック図である。

図２８においてコンピュータシステムは中央処理装置（ＣＰＵ）５０、リードオンリメモリ（ＲＯＭ）５１、ランダムアクセスメモリ（ＲＡＭ）５２、通信インタフェース５３、記憶装置５４、入出力装置５５、可搬型記憶媒体の読取り装置５６、およびこれらの全てが接続されたバス５７によって構成されている。

記憶装置５４としてはハードディスク、磁気ディスクなど様々な形式の記憶装置を使用することができ、このような記憶装置５４、またはＲＯＭ５１に図３、図４、図７、図１１、図１３、図１６、図１９、および図２６などのフローチャートに示されたプログラムや、本発明の特許請求の範囲の請求項１６〜１９のプログラムなどが格納され、そのようなプログラムがＣＰＵ５０によって実行されることにより、本実施形態における画像センサ、車速センサ、現在位置センサ、加速度センサ、ヨーレートセンサなどを用いた距離算出が可能となる。

このようなプログラムは、プログラム提供者５８側からネットワーク５９、および通信インタフェース５３を介して、例えば記憶装置５４に格納されることも、また市販され、流通している可搬型記憶媒体６０に格納され、読取り装置５６にセットされて、ＣＰＵ５０によって実行されることも可能である。可搬型記憶媒体６０としてはＣＤ−ＲＯＭ、フレキシブルディスク、光ディスク、光磁気ディスク、ＤＶＤなど様々な形式の記憶媒体を使用することができ、このような記憶媒体に格納されたプログラムが読取り装置５６によって読み取られることにより、本実施形態における走行車両から対象物までの正確な距離の算出が可能となる。

以上詳細に説明したように本発明によれば、例えば信号機などの対象物を認識し、自車両からその対象物までの方位角、あるいは仰角を算出し、必要に応じて車両のヨーレート、進行方向の加速度、進行方向の軸周りの回転角や、路面の傾斜角によるピッチング角などの影響を補正することによって、自車両と対象物までの距離を正確に算出することが可能となる。また、車両の走行状態や地図データを用いることによって、対象物までの走行距離を算出することも可能となる。さらに目的が車両の自動停止の場合には停止線を対象物として、停止線までの距離を正確に算出することもでき、目的に応じて各種の対象物までの距離算出を行うことができる。
（付記１）
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該移動物体の２つの時点間の移動距離を算出する移動距離算出手段と、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する角度変化量算出手段と、
該方位角算出手段、移動距離算出手段、および角度変化量算出手段の出力を用いて、移動物体と対象物との間の直線距離を算出する直線距離算出手段とを備えることを特徴とする距離算出装置。
（付記２）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段、前記方位角算出手段、および直線距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記３）
前記対象物を含む画像から対象物を認識し、該認識結果を前記方位角算出手段に与える対象物認識手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記４）
前記対象物認識手段の出力が、あらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、
該対象物認識監視手段が該逸脱を検出した時、該対象物認識手段の出力を補正して前記方位角算出手段に与える対象物認識補正手段をさらに備えることを特徴とする付記３記載の距離算出装置。
（付記５）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて、移動物体の移動方向軸の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて、対象物の認識結果としての座標を補正し、前記方位角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする付記３記載の距離算出装置。
（付記６）
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
該ヨーレートセンサと前記直線距離算出手段との出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記７）
前記ヨーレートセンサの出力値、および／または出力値の変化率があらかじめ定められた出力値の範囲、および／または出力値の変化率の範囲を逸脱したか否かを監視するヨーレートセンサ監視手段と、
ヨーレートセンサ監視手段が該逸脱を検出した時、過去のヨーレートセンサの出力と移動物体の速度の値に基づいて、ヨーレートセンサの出力値を補正して前記角度変化量算出手段に与えるヨーレート補正手段をさらに備えることを特徴とする付記６記載の距離算
出装置。
（付記８）
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する仰角算出手段と、
移動物体の２つの時点間の移動距離を算出する移動距離算出手段と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するピッチング角算出手段と、
該仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する移動方向距離算出手段とを備えることを特徴とする距離算出装置。
（付記９）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該データベース手段、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１０）
前記対象物を含む画像から対象物を認識し、該認識結果を前記仰角算出手段に与える対象物認識手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１１）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて対象物の認識結果としての座標を補正し、前記仰角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする付記１０記載の距離算出装置。
（付記１２）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段の出力を用いて、該移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段とをさらに備え、
前記ピッチング角算出手段が、該傾斜角算出手段の出力を用いてピッチング角の算出を行うことを特徴とする付記８記載の距離算出装置。
（付記１３）
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、前記傾斜角算出手段の出力に加えて、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする付記１２記載の距離算出装置。
（付記１４）
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする付記８記載の距離算出装置。
（付記１５）
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面で成す角を方位角として算出する方位角算出手段と、
該ヨーレートセンサ、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１６）
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する手順と、
該移動物体の２つの時点間の移動距離を算出する手順と、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する手順と、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出する手順とを計算機に実行させるための距離算出プログラム。
（付記１７）
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出するステップと、
該移動物体の２つの時点間の移動距離を算出するステップと、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出するステップと、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。
（付記１８）
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する手順と、
移動物体の２つの時点間の移動距離を算出する手順と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出する手順と、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する手順とを計算機に実行させるための距離算出プログラム。
（付記１９）
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するステップと、
移動物体の２つの時点間の移動距離を算出するステップと、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するステップと、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。

本発明は、自動車などに搭載されるナビゲーション装置の製造産業は当然のこととして、対象物までの正確な距離の算出技術を必要とするすべての産業において利用可能である。

Claims (2)

1. 移動物体と固定対象物との間の距離を算出する装置において、
   前記対象物を含む画像から前記対象物を認識する対象物認識手段と、
   前記画像の撮影に用いられたレンズの位置を通る水平面上で前記移動物体の進行方向に垂直な方向であるｘ方向のピクセル単位の座標であって前記対象物認識手段による認識結果として得られる前記画像中の前記対象物の座標ｘと、前記水平面に垂直なｙ方向のピクセル単位の座標であって前記対象物認識手段による認識結果として得られる前記画像中の前記対象物の座標ｙに基づいて、前記画像の撮像面上での前記ｘ方向および前記ｙ方向のピクセルの寸法ｓと、前記レンズの焦点距離Ｆとを用いて、前記移動物体と前記対象物とを結ぶ方向が水平面と成す角を仰角ｔａｎ^−１（｜ｙｓ｜／（（ｘｓ）^２＋Ｆ^２）^１／２）として算出する仰角算出手段と、
   前記移動物体の２つの時点間の移動距離Ｄを算出する移動距離算出手段と、
   前記移動物体の移動方向と鉛直方向とで決まる平面上での前記移動物体の回転角度をピッチング角として算出するピッチング角算出手段と、
   前記移動物体の進行方向に対する前記レンズの光軸方向の仰角ρ_ｃ、前記仰角算出手段が前記２つの時点それぞれについて算出した仰角φ_１およびφ_２、前記移動距離算出手段が算出した前記移動距離Ｄ、ならびに前記ピッチング角算出手段が前記２つの時点それぞれについて算出したピッチング角ρ_１およびρ_２を用いて、
   Ｘ＝Ｄｔａｎ（φ_１＋ρ_ｃ＋ρ_１）／（ｔａｎ（φ_２＋ρ_ｃ＋ρ_２）−ｔａｎ（φ_１＋ρ_ｃ＋ρ_１））
   なる式により前記移動物体から前記対象物までの直線距離の移動方向成分Ｘを算出する移動方向距離算出手段とを備え、
   前記ピッチング角算出手段は、
   前記移動物体が水平面上を一定の加速度で走行している時のピッチング角を、複数の加速度について予め測定した測定結果から得られる、加速度とピッチング角との第１の関係に基づいて、前記移動物体の前記進行方向の加速度から前記ピッチング角を算出するか、
   前記移動物体が一定の傾斜角の斜面を一定の加速度で走行している時のピッチング角を、複数の傾斜角と複数の加速度について予め測定した測定結果から得られる、傾斜角と加速度の組とピッチング角との第２の関係に基づいて、前記移動物体が走行している面の傾斜角と前記移動物体の前記進行方向の加速度から前記ピッチング角を算出するか、または、
   前記移動物体が一定の傾斜角の斜面に停止している時のピッチング角を、複数の傾斜角について予め測定した測定結果から得られる、傾斜角とピッチング角との第３の関係に基づいて、前記移動物体が走行している面の傾斜角から前記ピッチング角の第１の成分を算出し、前記第１の関係に基づいて、前記移動物体の前記進行方向の前記加速度から前記ピッチング角の第２の成分を算出し、前記第１の成分と前記第２の成分から前記ピッチング角を算出する、
   ことを特徴とする距離算出装置。
2. 前記移動物体の現在位置を検出する現在位置センサと、
   該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
   前記移動物体の速度を前記移動距離算出手段に与える速度センサと、
   前記データベース手段と前記速度センサとの出力を用いて前記移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、
   該ロール角算出手段と前記対象物認識手段との出力を用いて前記対象物の認識結果としての座標を補正し、前記仰角算出手段に前記座標ｘおよび前記座標ｙとして与えるロール角補正手段と、
   をさらに備えることを特徴とする請求項１記載の距離算出装置。

Images