説明

距離算出装置および算出プログラム

移動物体と対象物との間の方位角や、速度、ヨーレートなどの各種の状態量に基づいて、移動物体と対象物との間の距離を正確に算出することを目的とする。算出装置1は、移動物体と対象物との間の方位角を算出する手段2と、2つの時点間の移動物体の移動距離を算出する手段3と、移動方向の角度変化量を算出する手段4と、手段2、手段3、手段4の出力を用いて移動物体から対象物までの直線距離を算出する手段5とを備える。

【発明の詳細な説明】
【技術分野】
本発明は、移動物体と固定対象物との間の距離算出方式に係り、さらに詳しくは例えば、走行車両と対象物の間の仰角や方位角、車両の速度やヨーレートなどの車両の状態量に基づいて、道路を走行している車両と対象物までの正確な距離を算出する距離算出装置、および算出プログラムに関する。
【背景技術】
地図データベースやGPS(グローバル・ポジショニング・システム)を用いた車両の位置検出によって、車両の現在位置と目的地に応じてドライバに走行路を案内するカーナビゲーション技術が発達し、広範に普及している。そのようなシステムでは、GPSによって得られる緯度、経度の情報を基に、自車両の位置を地図上に重ねて表示し、交差点までの距離などを算出している。
このような従来技術の例として次の特許文献がある。
【特許文献1】 特許第3381312(P3381312)号 「ナビゲーション装置」
この特許文献には、地図情報によって位置が特定できる対象物を認識し、2つの時点の間の車両の走行距離と、各地点からの同一の対象物までの仰角を用いて対象物と自車両間の距離を算出し、自車両の位置を補正する技術が開示されている。
しかしながら、この従来技術では信号機までの直線距離しか計算することができず、曲線道路や信号機が自車の進行方向上に無い時など、実際の走行路上の距離を求める場合に誤差が生じるという問題点があった。
【発明の開示】
本発明の課題は上述の問題点に鑑み、対象物に対する車両からの仰角を含めて、方位角、速度あるいは加速度(ピッチング角)、ヨーレートなどを使い分けることによって、精度の高い距離計測機能を有する距離算出装置、および距離算出プログラムを提供することである。
図1は、本発明の距離算出装置の原理構成ブロック図である。同図は後述する距離算出装置の実施例1の原理構成ブロック図であり、実施例1の距離算出装置1は、少なくとも方位角算出手段2、移動距離算出手段3、角度変化量算出手段4、および直線距離算出手段5を備える。
方位角算出手段2は、移動物体、例えば走行車両と固定対象物、例えば信号機とを結ぶ方向と、移動物体との移動方向とが水平面上で成す角を方位角として算出するものであり、移動距離算出手段3は、2つの時点間、例えば直線距離算出の処理間隔としての100msの間の移動物体の移動距離を算出するものである。
角度変化量算出手段4は、その2つの時点間の移動物体の重心を通る垂直軸周りの回転角度を、進行方向の角度変化量として算出するものであり、直線距離算出手段5は方位角算出手段2、移動距離算出手段3、角度変化量算出手段4の出力を用いて、移動物体と対象物の間の直線距離を算出するものである。
発明の実施の形態においては、移動物体の現在位置を検出する現在位置センサと、現在位置センサの出力に応じて現在位置周辺の道路を含む地図情報を出力するデータベース手段と、データベース手段、方位角算出手段2、および直線距離算出手段5の出力を用いて、現在位置から対象物までの走行距離を算出する走行距離算出手段を、距離算出装置1はさらに備えることができる。
また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果、例えば対象物の座標を方位角算出手段2に与える対象物認識手段をさらに備えることもできる。この場合、対象物認識手段の出力があらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、その逸脱が検出された時、対象物認識手段の出力を補正して方位角算出手段に与える対象物認識補正手段をさらに備えることもできる。
あるいは移動物体の速度を移動距離算出手段3に与える速度センサと、前述の現在位置センサ、およびデータベース手段と、データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、ロール角と対象物の認識結果とを用いて対象物の認識結果、例えば座標を補正し、方位角算出手段2に与えるロール角補正手段をさらに備えることもできる。
さらに実施の形態においては、移動物体の鉛直軸周りの回転角速度を検出し、角度変化量算出手段4に与えるヨーレートセンサと、ヨーレートセンサと直線距離算出手段5との出力を用いて、移動物体から対象物までの距離を予測する距離予測手段をさらに備えることもでき、またヨーレートセンサの出力、および/または出力の変化範囲があらかじめ定められた出力の範囲、および/または出力の変化範囲を逸脱したか否かを監視するヨーレート監視手段と、逸脱が検出された時、過去のヨーレートや速度の値を用いてヨーレートセンサの出力を補正し、角度変化量算出手段5に与えるヨーレート補正手段とをさらに備えることもできる。
次に本発明の実施例3に対応する距離算出装置は、少なくとも仰角算出手段、移動距離算出手段、ピッチング角算出手段、および移動方向距離算出手段を備える。
仰角算出手段は移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するものであり、ピッチング角算出手段は移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するものであり、移動距離算出手段は図1に原理を説明した実施例1におけると同様に移動物体の2つの時点間の移動距離を算出するものである。そして移動方向距離算出手段は、仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて移動物体から対象物までの直線距離の(車両の)移動方向成分を算出する。
発明の実施の形態においては、実施例1に対すると同様に、前述の現在位置センサ、データベース手段、および移動距離算出手段をさらに備えることもできる。
また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果を仰角算出手段に与える対象物認識手段をさらに備えることもでき、また前述と同様の対象物認識監視手段、および対象物認識補正手段をさらに備えることもできる。
またこの場合、前述と同様の速度センサ、現在位置センサ、データベース手段、ロール角算出手段、およびロール角補正手段をさらに備えることもでき、ロール角補正手段は、対象物の認識結果を補正してその補正結果を仰角算出手段に与える。
さらに実施の形態においては、移動物体の移動方向の加速度を検出する加速度センサをさらに備え、ピッチング角算出手段が加速度センサの出力を用いてピッチング角を算出することもできる。
あるいは、前述と同様の現在位置センサ、データベース手段に加えて、データベース手段の出力を用いて移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段をさらに備え、ピッチング角算出手段が、傾斜角算出手段の出力を用いてピッチング角の算出を行うこともでき、あるいはさらに加速度センサを備え、ピッチング角算出手段が傾斜角算出手段の出力に加えて、加速度センサの出力を用いてピッチング角の算出を行うこともできる。
以上のように本発明によれば、移動物体の方位角や、移動物体の進行方向の角度変化量などを用いて、あるいは移動物体の仰角や移動物体のピッチング角などを用いて、対象物までの直線距離や移動物体の移動方向距離などの算出が行われる。
【図面の簡単な説明】
図1は、本発明の距離算出装置の原理構成ブロック図である。
図2は、本発明の距離算出装置の実施例1の構成ブロック図である。
図3は、実施例1における距離算出の全体処理フローチャートである。
図4は、対象物認識処理の詳細フローチャートである。
図5は、方位角算出方法の説明図である。
図6は、直線距離算出方法の説明図である。
図7は、直線距離算出処理の詳細フローチャートである。
図8は、走行距離予測方法の説明図である。
図9は、距離算出装置の実施例2の構成ブロック図である。
図10は、走行距離算出方法の説明図である。
図11は、走行距離算出処理の詳細フローチャートである。
図12は、距離算出装置の実施例3の構成ブロック図である。
図13は、実施例3における進行方向距離算出処理の全体フローチャートである。
図14は、傾斜角算出を説明するための車両の走行状態の説明図である。
図15は、傾斜角算出方法の説明図である。
図16は、傾斜角算出処理の詳細フローチャートである。
図17は、進行方向距離算出を説明するための車両の走行状況を示す図である。
図18は、進行方向距離算出方法の説明図である。
図19は、進行方向距離算出処理の詳細フローチャートである。
図20は、距離算出装置の実施例4の構成ブロック図である。
図21は、距離算出装置の実施例5の構成ブロック図である。
図22は、距離算出装置の実施例6の構成ブロック図である。
図23は、距離算出装置の実施例7の構成ブロック図である。
図24は、距離算出装置の実施例8の構成ブロック図である。
図25は、ロール角補正方法の説明図である。
図26は、ロール角補正処理の詳細フローチャートである。
図27は、距離算出装置の実施例9の構成ブロック図である。
図28は、本発明のプログラムのコンピュータへのローディングを説明する図である。
【発明を実施するための最良の形態】
以下本発明の実施の形態をいくつかの実施例に分けて詳細に説明する。
本発明において移動物体、例えば走行中の車両から、例えば直線距離が算出される固定対象物としては、信号機や道路標識をはじめとして、走行中の車両に設置されたカメラによって画像を捉えることができる看板、踏み切り、歩道橋、横断歩道、停止線などの各種の対象物までの距離が算出される。距離算出のために画像センサ、ヨーレートセンサ、車速センサ、および加速度センサなどからデータを、例えば100ms毎に取り込んで、対象物認識や方位角算出などの各種の処理を100ms毎に行い、最終的に例えば対象物までの直線距離を100ms毎に算出する。以下の説明では、信号機を対象物の具体例として実施例を説明する。
図2は本発明の距離算出装置の実施例1の構成ブロック図である。同図において距離算出装置10に対しては、画像センサ11、車速センサ12、ヨーレートセンサ13の出力がそれぞれ与えられる。画像センサ11としては、例えばCCDカメラやCMOSカメラなどが用いられ、カメラは進行方向を撮影するために車両の前方に設置され、その光軸は基本的に車両の進行方向と同一となるように設置される。車速センサ12は、車両の車輪の速度を検出するものであり、例えば4個の車輪にそれぞれ設置される4つのセンサによって構成される。ヨーレートセンサ13は、車両の重心を通り、鉛直方向の軸の周りの車両の回転角速度を検出するものである。
距離算出装置10の内部の対象物認識部14は、画像センサ11によって撮像された画像を用いて、対象物の認識、例えば対象物の座標の認識を行うものである。前述のように対象物としては信号機を例にとるが、信号機の認識方法については図4で後述する。方位角算出部15は、対象物認識部14によって認識された対象物、例えば信号機の方位角を算出するものである。すなわち車両の重心を通る水平面上で、車両の重心と認識された対象物とを結ぶ線分の投影線と、車両の進行方向とが成す角を方位角として算出するものである。
移動距離算出部16は、車速センサ12の出力を用いて、100msあたりの車両の移動距離を算出するものである。角度変化量算出部17は、ヨーレートセンサ13の出力を用いて、処理間隔100msの間の鉛直方向の回転軸周りの車両の回転角を角度変化量として算出するものである。ヨーレートセンサ13の角速度がω(rad/s)の時に、角度変化量は0.1ω(rad)となる。
直線距離算出部18は、ここでは方位角算出部15、移動距離算出部16、および角度変化量算出部17の出力を用いて車両と対象物の間の直線距離を算出するものである。この算出方法については図6、7で後述する。走行距離予測部19は、直線距離算出部18、ヨーレートセンサ13の出力を用いて、車両の現在地点から対象物の地点までの走行距離を予測するものである。その算出方法については図8で後述する。
図3は、実施例1における距離算出の全体処理フローチャートである。同図において処理が開始されると、まずステップS1で画像センサ11の出力を用いて対象物の位置認識、例えば信号機の赤や青の信号の画像中の座標の出力が行われ、ステップS2で車両の進行方向と対象物への方向とが成す角、すなわち方位角が算出され、ステップS3で車速センサ12の出力を用いてある2つの時点の間の車両の移動距離、すなわち処理間隔100msの間の移動距離が算出され、ステップS4でヨーレートセンサ13の出力を用いて車両の鉛直軸周りの角度変化量が算出される。
ただし、ここでステップS1からステップS4はそれぞれ独立に実行することができ、その順序を変更することも、また並行して処理することも可能である。続いてステップS5では、算出された方位角、移動距離、角度変化量を用いて対象物までの直線距離が算出され、ステップS6でその直線距離の算出結果とヨーレートセンサ13の出力とを用いて、走行距離の予測が行われ、処理を終了する。
図4は、図2の対象物認識部14による対象物認識処理の詳細フローチャートである。同図において処理が開始されると、ステップS11で特定の色、ここでは信号の青、黄、赤の色をもつ領域の検出が行われる。灯火している信号の色の領域を抜き出すことを考えるが、どの色が灯火しているかわからないため、ここでは3つの色の領域の検出をそれぞれ試みるものとする。3つの色それぞれに対し、複数の領域が検出されることも、領域がひとつも検出されないことも考えられる。
ステップS12で抽出された領域の大きさ、ここでは3つの色によって抽出されたすべての領域の大きさがそれぞれ取得され、ステップS13で信号機までの距離にもよるが、正確な認識のためにあらかじめ設定された十分な範囲を持つ大きさによって領域の絞込みが行われる。
続いてステップS14で各領域、ここではステップS13で抽出されたすべての領域の周囲長がそれぞれ取得され、ステップS15でその周囲長lと領域の面積Sとを用いて、各領域の円形度が算出される。円形度は4πS/lによって算出され、ステップS16で円形度の大きさによる領域の絞込み、すなわち円形度の値があらかじめ定められた閾値、例えば0.8よりも大きい時にのみ、その領域が信号機の信号光の領域として認識される。そして処理結果として、画像中の信号機の信号光の領域の中心の座標が出力される。ここで円形度は完全な円に対しては1となるが、ここではステップS13で抽出されたすべての領域の中で円形度が閾値より大きい領域の中心の座標が出力されるものとする。座標が複数出力されることも考えられるが、その場合出力された座標それぞれに関して、方位角等の算出を行うものとする。
図5は方位角算出部15による方位角算出方法の説明図である。同図は後述する仰角の算出にも同様に用いられるが、方位角の算出においては画像上のx座標、すなわちカメラのレンズの位置Lを通る水平面上で、車両の進行方向に垂直な方向のx座標を用いて計算が行われる。
図5において信号機の水平面上の位置をS、Sからレンズの光軸方向に垂直に前述の平面に垂線をおろした時、その足をO、レンズの焦点距離をF、撮像面の位置をO’、画像中の信号機(信号光)のx座標をx(ピクセル単位)、ピクセルのx方向の寸法をlとすると、求めるべき方位角αはα’と等しく、方位角はtan−1(xl/F)によって求めることができる。
図6は、直線距離算出部18による車両から対象物Sまでの距離Xの算出方法の説明図である。図6に示すように信号機の位置をSとし、車両が時刻Δtの間にA1地点からA2地点に移動したとする。A1地点からの信号機の方位角をθ、A2地点からの信号機の方位角をθとする。A1のときの車両の進行方向にA2から垂線を下ろし、交点をA2’とする。A1−A2’の距離をD’、A2−A2’の距離をdとすると、移動距離算出部16により算出された移動距離Dと角度変化量算出部17により算出された角度変化量γにより、D’とdを近似的に表すと、
D’=Dcosγ
d=Dsinγ
A1のときの進行方向にSから垂線を下ろし、交点をOとする。A2’地点からOまでの距離をX’とすると、下記の式が成り立つ。
d+X’tan(θ+γ)=(X’+D’)tanθ
これにX’について解くと
X’=(D’tanθ−d)/(tan(θ+γ)−tanθ
=(Dcosγtanθ−Dsinγ)/(tan(θ+γ)−tanθ
A2からSまでの距離Xは以下の式によって表される。
X=X’/cos(θ+γ)
図7は直線距離算出部18による、車両から対象物までの直線距離算出処理のフローチャートである。同図において処理が開始されるとステップS20で移動距離算出部16によって移動距離Dが、ステップS21で方位角算出部15によって方位角θ、θが、ステップS22で角度変化量算出部17によってγの値が設定され、ステップS23でX’の算出式の分母の絶対値がある閾値tが大きいか否かが判定される。この分母が0である場合、X’の値を求めることができず、分母が0に近い場合にもX’を正確に求めることができないため、そのような場合には直ちに処理を終了する。
閾値tより大きい場合には、ステップS24でX’の値が算出され、ステップS25でXの算出式の分母がある閾値tより大きいか否かが判定され、大きくない場合には直ちに処理を終了し、大きい場合にはステップS26でXの値が算出されて処理を終了する。
ステップS23でX’の分母の絶対値が閾値t以下でX’の値を算出できなかった場合には、過去の車両と対象物までの距離、およびその後の車両の車速やヨーレートなどを用いて車両の現在位置を予測し、対象物までの直線距離を算出することができる。図6においてA1地点から信号機Sまでの距離をX”とすると、X’は次式によって与えられ、前述のX’をこの値で置き換えてXを算出することも可能である。
X’=X”cosθ−D’
図7のステップS25でXの分母が閾値t以下である時には、Xの算出を行うことができないが、速度が極端が遅く、移動距離があまりない時には、X”の値からDの値を減算し、Xの値を近似的に求めることが可能である。
図8は、走行距離予測部19による、現在位置から対象物までの走行距離の予測方法の説明図である。走行距離予測部19は、車両の現在位置からの走行路に沿った対象物までの距離を、過去のヨーレートと車両の速度から予測するものである。この予測においては、現在位置Aから対象物の位置Sまでの走行路上で、車両の重心を通る垂直軸回りの回転角速度としてのヨーレートの値が一定であり、また車の速度の絶対値も一定であるとして、微小時間Δtおきに、図8に示すようにベクトルl、l、l・・を予測し、ベクトルの総和が対象物の位置Sに達するまで演算を繰り返すことによって、ベクトルの長さの総和を取り、対象物までの走行距離を予測する。
図8において、車両の現在地点Aから現在の進行方向に対して対象物の位置から下ろした垂線の足をS’とすると、最初のベクトルlがS’の方向と成す角は、角速度ωと微小時間Δtの積となる。次のベクトルlとS’方向の成す角は、2ωと微小時間Δtとの積となる。同様にして各ベクトルの方向を求め、また各ベクトルの長さを現在の車両の速度vと微小時間Δtとの積として計算することによって、各ベクトルの長さとその方向が決定され、対象物の位置Sに達するまでの走行距離の予測が行われる。
次に本発明の実施例2について説明する。図9は距離算出装置の実施例2の構成を示すブロック図である。同図を実施例1と比較すると、走行距離予測部19に代わって走行距離算出部23が備えられ、走行距離算出部23に対しては、方位角算出部15の出力と直線距離算出部18の出力に加えて、距離算出装置10の外側の現在位置センサ21の出力を用いるデータベース22の出力が与えられている。
現在位置センサ21は車両の現在位置を検出するものであり、例えば一般的にカーナビゲーションシステムに用いられるGPS(グローバル・ポジショニング・システム)である。GPSによって車両の現在位置の緯度、経度、標高などの絶対位置を得ることが可能であるが、この位置には一般的にある程度の誤差が含まれているため、誤差を考慮してそれらの位置を参照することが必要である。
データベース22は道路の形状(標高や曲率半径)、信号機や歩道橋、踏み切りのような対象物の位置などを示す地図情報であり、現在位置センサ21によって検出された自車両の位置を用いて地図上の車両の位置を特定することができる。データベース22は、例えば自車両付近の対象物のデータや、道路形状のデータなどを走行距離算出部23に与える。
走行距離算出部23は、データベース22の地図情報によって得られる対象物の位置に基づき、直線距離算出部18によって算出された対象物と自車両との距離を用いて対象物への方向と、自車両の進行方向との成す角が、方位角算出部15によって算出された方位角と等しくなる点を自車両の位置として決定し、その位置から対象物までの道路上の距離を地図情報から算出するものである。この算出方法について図10、および図11を用いて説明する。
図11は、走行距離算出処理のフローチャートである。まずステップS31で、図10に示すように対象物Sの位置を中心として、自車両と対象物との直線距離を半径とする円が地図上に描かれ、ステップS32でその円の円周がかかる道路上の円弧、図10では4つの円弧が検出され、ステップS33で各円弧の長さlが算出される。
続いてステップS34で各円弧の端、すなわち図10において円上で反時計方向に動く点を考えるとき、その点が最初に各円弧上に達する位置から各円弧上でl/4の長さの位置が車両の候補位置として検出される。すなわち、ここでは道路は上下各1車線とし、左側の車線の中心位置が車両の候補位置として検出される。
続いてステップS35で、車両の候補位置における車両の進行方向を示す線、すなわち道路の接線方向と平行な線と、車両の候補位置と対象物とを結ぶ線との成す角がそれぞれの円弧について算出され、ステップS36で算出された角度の値、図10では4つの値のうちで、方位角算出部15によって算出された方位角の値と最も近い車両の候補位置が車両の現在位置として決定され、ステップS37で車両の現在位置から対象物までの距離が、道路形状に沿って算出されて処理を終了する。
次に本発明の実施例3を図12のブロック図を用いて説明する。同図において距離算出装置10は、図9で説明した実施例2と同様に、画像センサ11の出力を用いる対象物認識部14、車速センサ12の出力を用いる移動距離算出部16、現在位置センサ21の出力を用いるデータベース22に加えて、対象物認識部14の出力を用いて対象物方向の仰角を算出する仰角算出部25、データベース22の出力を用いて車両の傾斜角を算出する傾斜角算出部26、傾斜角算出部の26の出力を用いて車両のピッチング角を算出するピッチング角算出部27を備え、直線距離算出部18に代わる進行方向距離算出部28は、仰角算出部25、移動距離算出部16、およびピッチング角算出部27の出力を用いて対象物までの直線距離の車両の進行方向成分を算出する。
仰角算出部25による仰角の算出方法は、図5で説明した方位角の算出方法と基本的に同様である。ただし、仰角に対しては図5の撮像面上で、車両の進行方向を含む水平面に垂直なy軸方向の座標に加えて、x軸方向の座標とレンズの焦点距離Fを用いて計算が行われる。すなわち、図5で点S’の信号機(信号光)のx、y座標をx、yとすると、ピクセルのx、y方向の寸法lを用い

傾斜角算出部26は、現在位置センサ21により検出された現在位置周辺の地図データがデータベース22から与えられることにより、現在位置周辺で標高値のわかっている2つの地点の標高の差に基づいて、自車両周辺位置の傾斜角を算出し、ピッチング角算出部27に与えるものであり、傾斜角算出方法の詳細については後述する。
ピッチング角算出部27は、傾斜角算出部26によって算出された傾斜角、すなわち道路の進行方向の傾斜角を用いて車両のピッチング角、すなわち車両の前後を結ぶ軸と水平線との縦揺れを示す角度を算出するものである。車両のピッチング角が変化すると仰角が変化し、対象物までの正確な距離を求めることが不可能となる。このため、ピッチング角を算出し、仰角を補正する必要がある。この仰角の補正は、仰角算出部25によって算出された仰角に対して進行方向距離算出部28によって行われるものとする。
車両のピッチング角と道路の傾斜角との関係は、車両の前後左右のサスペンションの伸縮によって決まり、サスペンションの伸縮はバネ係数によって決まる。そのため、本実施例では傾斜角と車両のピッチング角との間の関係を実験的に求めるものとする。車両が水平な面上に停止している時と、傾斜角θの斜面に停止している時のピッチング角を測定する。傾斜角θを変化させてピッチング角を測定することにより、傾斜角とピッチング角との間の関係が実験的に求められる。傾斜角に対するピッチング角の測定結果がない場合には、測定結果を補間することによってピッチング角が算出される。
図13は、実施例3における進行方向距離算出処理の全体フローチャートである。同図において処理が開始されると、まず対象物認識部14によって図4におけると同様に対象物の位置が認識され、ステップS42で対象物への方向の仰角が算出され、ステップS43で移動距離が算出され、ステップS44で傾斜角算出部26の出力を用いてピッチング角が算出され、ステップS45で車両の現在位置から対象物までの距離を表すベクトルの車両の進行方向と平行な成分として、進行方向距離が算出されて処理を終了する。
傾斜角の算出について説明する。図14、および図15は、傾斜角の算出方法の説明図であり、図16は、傾斜角算出処理のフローチャートである。図14は、データベース22から出力される地図上での、車両の走行状態の説明図である。車両の現在位置周辺に、標高の値がわかっている2つの地点P1とP2が存在するものとする。
図15は傾斜角θの算出方法の説明図である。図14で説明したように、車両が点P1からP2まで走行する間の標高差をh2−h1とし、地図上での点P1とP2の間の距離をdとすると傾斜角θは次式によって与えられる。
θ=tan−1{(h2−h1)/d}
図16の傾斜角算出処理のフローチャートにおいて処理が開始されると、まずステップS51、S52で自車両の現在の位置から最も距離が近く、標高の値の既知な2つの地点P1、P2が探索され、ステップS53で2つの地点の間の地図上での距離dが算出され、ステップS54で傾斜角θの算出が行われて処理を終了する。
次に進行方向距離の算出について、図17−図19を用いて説明する。図17は、車両の進行状態の説明図である。同図において車両は処理時間、例えば100msの間に点A1から点A2まで距離Dだけ移動したものとする。車両は直線方向に進行を続けるものと仮定し、対象物の位置Sから進行方向に下ろした垂線の足をS’とし、点A2からS’までの距離Xを進行方向距離として算出するものとする。なお車両は実際には点A2からはカーブする道路上を対象物Sに近づいていくものとする。
図18は、車両の進行状態を真横から見た状態を示す図である。図18に示すように車両が時刻Δtの間にA1地点からA2地点に移動したとする。A1地点からの信号機の仰角をφ、A2地点からの信号機の仰角をφとする。A1地点からA2地点までの移動距離算出部16により算出した移動距離をD、A2地点から信号機までの距離の車両の進行方向と平行な成分をXとすると、下記の式が成り立つ。
(X+D)tanφ=Xtanφ
Xについて解くと、
X=Dtanφ/(tanφ−tanφ
となり、対象物までの距離が算出できる。カメラの光軸方向が車両の進行方向に比べ仰角ρだけ上を向いている(カメラの設置時に設定)場合でも、距離Xは以下の式によって表される。
X=Dtan(φ+ρ)/(tan(φ+ρ)−tan(φ+ρ))
また、A1地点においてピッチング角算出部27により算出したピッチング角をρ、A2地点においてピッチング角算出部27により算出したピッチング角をρとする。このとき、距離Xは以下の式によって表される。
X=Dtan(φ+ρ+ρ)/(tan(φ+ρ+ρ)−tan(φ+ρ+ρ))
このように、進行方向距離算出部28は、仰角算出部25により算出された仰角、移動距離算出部16により算出した移動距離およびピッチング角算出部27により算出したピッチング角に基づき、車両と対象物の距離の車両の進行方向と平行な成分を算出する。
図19の進行方向距離算出処理が開始されるとステップS56で移動距離算出部16によって移動距離Dが、ステップS57で仰角算出部25によって2つの仰角が、ステップS58でピッチング角算出部27によって2つのピッチング角の値が設定され、ステップS59でXの値を算出する式の分母の絶対値がある閾値tより大きいか否かが判定され、t以下の値であるときには直ちに処理を終了し、閾値を超えている場合にはステップS60でXの値が算出されて処理を終了する。ステップS59でXを算出する式の分母の絶対値が0、または0に近い時、Xを正確に算出することはできないが、車両の速度が極端に遅く、移動距離Dが小さい時には1つ前の時点で算出した距離Xから移動距離Dを減算して、進行方向距離Xを近似的に求めることもできる。
続いて本発明の実施例4について説明する。図20は実施例4の距離算出装置の構成ブロック図である。同図を図12の実施例3と比較すると、距離算出装置10の外部に現在位置センサ21に代わって、車両の前後方向、すなわち進行方向の加速度を検出する加速度センサ30が備えられ、ピッチング角算出部27が加速度センサの出力を用いてピッチング角を算出して、進行方向距離算出部28に与えることが異なっている。図20において加速度センサ30は、例えば車両の中央部分に設置され、圧電素子などによって車の前後方向の加速度成分を検出する。ピッチング角算出部27は加速度センサ30の出力としての加速度と、車両のピッチング角との間の既知の関係からピッチング角を算出するものとする。
車両のピッチング角と加速度との関係は、前述の傾斜角との関係のようにサスペンションの伸縮、すなわちバネ係数によって決まる。このため、加速度とピッチング角の関係を実験的に求めるものとする。
すなわち、車両が水平な面上に停止している時と、水平な面上をある一定の加速度で走行している時のピッチング角の測定を行う。加速度の値を変化させ、ピッチング角を測定することによって、加速度とピッチング角の間の関係が実験的に求められる。加速度センサ30によって検出された加速度の値に対するピッチング角が測定されていなかった場合には、測定結果を補間することによってピッチング角が求められる。
続いて本発明の実施例5について説明する。図21は、距離算出装置の実施例5の構成を示すブロック図である。同図を実施例3を示す図12、実施例4を示す図20と比較すると、これらの実施例のすべての構成要素を備えており、ピッチング角算出部27が傾斜角算出部26の出力に加えて、加速度センサ30の出力も用いてピッチング角を算出し、進行方向距離算出部28に与えることだけが異なっている。
図21のピッチング角算出部27は、ピッチング角と道路の傾斜角、および加速度との間の実験的関係に基づいてピッチング角を算出する。ある一定の傾斜角の斜面を一定加速度で車両が走行している時のピッチング角を測定し、傾斜角、および加速度を変えて測定を繰り返すことによって、傾斜角と加速度が与えられた時のピッチング角を算出することが可能となる。あるいは実施例3において述べたピッチング角と傾斜角との間の実験的関係と、実施例4において述べたピッチング角と加速度との間の関係を用いて、それぞれピッチング角を求め、それらの和を計算することによって、傾斜角と加速度に対応するピッチング角を算出することも可能である。
図22は距離算出装置の実施例6の構成ブロック図である。同図を図12の実施例3と比較すると、方位角算出部15と走行距離算出部23がさらに備えられている点が異なっている。方位角算出部15は、図2の実施例1におけると同様に車両の方位角を算出し、走行距離算出部23に与える。
走行距離算出部23は、方位角算出部15の出力、データベース22の出力、進行方向距離算出部28の出力を用いて走行距離を算出する。図9で説明した実施例2では走行距離算出部23は、直線距離算出部18の出力、すなわち車両の位置から対象物までの直線距離を用いて、対象物までの道路に沿った走行距離を算出している。その様子は図10に説明されている。一方、図22の進行方向距離算出部28は、図17で説明したように車両の進行方向の距離Xを算出しており、車両の現在位置と対象物までの直線距離は方位角算出部15の出力、すなわち∠SAS’を用いることによって容易に算出される。従ってこの直線距離を用いることによって、走行距離算出部23は対象物までの走行距離を算出することができる。
図23は距離算出装置の実施例7の構成を示すブロック図である。同図を図12で説明した実施例3と比較すると、図2の実施例1などで説明したヨーレートセンサ13と、走行距離予測部19をさらに備えている点が異なっている。ここで走行距離予測部19は、図2では直線距離算出部18の算出結果、すなわち車両の現在位置から対象物までの直線距離に代わって、進行方向距離算出部28の算出結果、すなわち図17で説明した距離Xを用いて、図8で説明した方法と同様にして対象物までの走行距離を予測するものである。
図24は本発明の距離算出装置の実施例8の構成ブロック図である。同図を図2の実施例1と比較すると、距離算出装置10の外部に現在位置センサ21が備えられ、装置10の内部に現在位置センサ21の出力を受け取るデータベース22と、データベース22の出力と車速センサ12の出力とを用いて車両のロール角、すなわち車両が進行方向の軸の周りに回転する角度としてのロール角を算出するロール角算出部32と、対象物認識部14と方位角算出部15との間に備えられ、対象物認識部14の出力とロール角算出部32の出力とを用いて、ロール角の発生によって生じる対象物認識部14による認識結果の誤差を補正して、補正結果の対象物の座標を方位角算出部15に与えるロール角補正部33を備えている点が異なっている。
図24のロール角算出部32は、一般的に車両がカーブを走行している時に発生するロール角を算出するものである。このロール角が発生すると、対象物認識部14による認識結果が不正確となる、すなわち画像センサ11の光軸が回転することにより、仰角、方位角ともに変化し、対象物までの正確な距離を求めることが不可能となる。そこでロール角を算出し、対象物認識部14による認識結果を補正し、方位角を正しく算出することが必要となる。
ロール角は車両が走行している道路の曲率半径とサスペンションの伸縮によって決まる。ロール角と道路の曲率半径の関係を実験的に算出するものとし、一定曲率半径の道路を一定速度で走行した時のロール角を測定する。曲率半径と速度の値を様々に変化させ、ロール角の値を測定することによって、ロール角と道路の曲率半径、および車両の速度との間の関係を実験的に求めることができる。
ロール角補正部33は、ロール角算出部32によって算出されたロール角を用いて対象物認識部14の認識結果、例えば信号機の信号光の中心の画像上の座標位置の補正を行う。図25はロール角補正方法の説明図である。ロール角θによる座標軸の傾きを補正するために座標(x’、y’)から座標(x、y)を算出する。
図25に示すようにロール角をθ、対象物の座標(x’、y’)とし、車両の重心からカメラの中心までの高さをL、カメラの画素サイズをl、補正後の対象物の座標を(x、y)とする。lを用いてx’、y’を画像の中心からの距離X’、Y’に置き換える。
X’=x’・l、Y’=y’・l、
X、YとX’、Y’の関係により以下の2式が成立する。
(L+Y)+X=(L+Y’)+X’

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

最後に、カメラの画素サイズlを用いて下式のように画像上の座標系に戻す。
x=X/l、y=Y/l
図26は、ロール角補正処理のフローチャートである。同図において、ステップS65でロール角θが生じた場合の対象物の座標(x’、y’)から、対象物の画像の中心からの距離X’、Y’が算出され、ステップS66でθ’の値が算出され、ステップS67で補正後の対象物の画像の中心からの距離X、ステップS68でYが算出され、ステップS69で補正後の画像上の座標(x、y)が算出されて処理を終了する。
最後に本発明の実施例9について説明する。図27は距離算出装置の実施例9の構成ブロック図である。同図を例えば図2の実施例1と比較すると、走行距離予測部19に代わって走行距離算出部23が設けられ、走行距離算出部23に対しては直線距離算出部18の出力に加えて、方位角算出部15の出力と現在位置センサ21の出力に基づくデータベース22の出力とが与えられ、対象物認識部14に対してその認識結果に異常がないか否かを監視する対象物認識監視部35と、異常があった場合に対象物認識部14の認識結果を補正して方位角算出部15に与える対象物認識補正部36、同様にヨーレートセンサ13の出力(角速度)、または出力の変化率(角加速度)を監視するヨーレートセンサ監視部38と、出力、または出力の変化率に異常があった時、ヨーレートセンサの出力を補正して角度変化量算出部17に与えるヨーレート補正部39を備えている点が異なっている。
図27において対象物認識部14は、画像センサ11によって撮像された画像に基づいて対象物を認識するものであるが、様々な原因によって認識結果に誤りが生じる可能性がある。そのため過去の認識結果と比較して、例えば対象物としての信号機の位置が処理間隔100msの間に著しくずれている場合など、対象物の認識結果に異常があると判断される場合に、対象物認識監視部35はその異常を対象物認識補正部36に通知する。
対象物認識補正部36は、対象物認識部14の認識結果に異常があると通知された時に、過去の対象物認識結果に基づき、線形予測やカルマンフィルタを用いた予測を行った結果を、対象物認識の補正結果として方位角算出部15に与える。
ヨーレートセンサ監視部38は、ヨーレートセンサ13の出力、すなわちヨーレートセンサが出力する車両の鉛直軸周りの回転角速度の値が急変した場合など、車両の正常な動きとしては考えにくい角速度の変化などを検出した時に、それをヨーレート補正部39に通知する。
ヨーレート補正部39は、ヨーレートセンサ監視部38からヨーレートセンサ13の出力やその変化率の異常が通知された時に、ヨーレートセンサの出力値に代わって、過去のヨーレートや速度の値などに基づき線形予測やカルマンフィルタによる予測の結果を利用して補正されたヨーレートの値を角度変化量算出部17に与える。
なお、図27では、例えば図9の実施例2と比較して対象物認識監視部35、対象物認識補正部36、ヨーレートセンサ監視部38、およびヨーレート補正部39が追加された構成となっているが、例えば図23の実施例7を基本として、これらの各部が追加され、対象物認識補正部36が認識結果を補正して仰角算出部25に与える構成も当然可能である。
以上において、本発明の距離算出装置、および算出プログラムについてその詳細を説明したが、この距離算出装置は当然一般的なコンピュータシステムとして構成することが可能である。図28はそのようなコンピュータシステム、すなわちハードウェア環境の構成ブロック図である。
図28においてコンピュータシステムは中央処理装置(CPU)50、リードオンリメモリ(ROM)51、ランダムアクセスメモリ(RAM)52、通信インタフェース53、記憶装置54、入出力装置55、可搬型記憶媒体の読取り装置56、およびこれらの全てが接続されたバス57によって構成されている。
記憶装置54としてはハードディスク、磁気ディスクなど様々な形式の記憶装置を使用することができ、このような記憶装置54、またはROM51に図3、図4、図7、図11、図13、図16、図19、および図26などのフローチャートに示されたプログラムや、本発明の特許請求の範囲の請求項16〜19のプログラムなどが格納され、そのようなプログラムがCPU50によって実行されることにより、本実施形態における画像センサ、車速センサ、現在位置センサ、加速度センサ、ヨーレートセンサなどを用いた距離算出が可能となる。
このようなプログラムは、プログラム提供者58側からネットワーク59、および通信インタフェース53を介して、例えば記憶装置54に格納されることも、また市販され、流通している可搬型記憶媒体60に格納され、読取り装置56にセットされて、CPU50によって実行されることも可能である。可搬型記憶媒体60としてはCD−ROM、フレキシブルディスク、光ディスク、光磁気ディスク、DVDなど様々な形式の記憶媒体を使用することができ、このような記憶媒体に格納されたプログラムが読取り装置56によって読み取られることにより、本実施形態における走行車両から対象物までの正確な距離の算出が可能となる。
以上詳細に説明したように本発明によれば、例えば信号機などの対象物を認識し、自車両からその対象物までの方位角、あるいは仰角を算出し、必要に応じて車両のヨーレート、進行方向の加速度、進行方向の軸周りの回転角や、路面の傾斜角によるピッチング角などの影響を補正することによって、自車両と対象物までの距離を正確に算出することが可能となる。また、車両の走行状態や地図データを用いることによって、対象物までの走行距離を算出することも可能となる。さらに目的が車両の自動停止の場合には停止線を対象物として、停止線までの距離を正確に算出することもでき、目的に応じて各種の対象物までの距離算出を行うことができる。
【産業上の利用可能性】
本発明は、自動車などに搭載されるナビゲーション装置の製造産業は当然のこととして、対象物までの正確な距離の算出技術を必要とするすべての産業において利用可能である。
【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】

【図23】

【図24】

【図25】

【図26】

【図27】

【図28】


【特許請求の範囲】
【請求項1】
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該移動物体の2つの時点間の移動距離を算出する移動距離算出手段と、
該2つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する角度変化量算出手段と、
該方位角算出手段、移動距離算出手段、および角度変化量算出手段の出力を用いて、移動物体と対象物との間の直線距離を算出する直線距離算出手段とを備えることを特徴とする距離算出装置。
【請求項2】
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段、前記方位角算出手段、および直線距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする請求項1記載の距離算出装置。
【請求項3】
前記対象物を含む画像から対象物を認識し、該認識結果を前記方位角算出手段に与える対象物認識手段をさらに備えることを特徴とする請求項1記載の距離算出装置。
【請求項4】
前記対象物認識手段の出力が、あらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、
該対象物認識監視手段が該逸脱を検出した時、該対象物認識手段の出力を補正して前記方位角算出手段に与える対象物認識補正手段をさらに備えることを特徴とする請求項3記載の距離算出装置。
【請求項5】
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて、移動物体の移動方向軸の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて、対象物の認識結果としての座標を補正し、前記方位角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする請求項3記載の距離算出装置。
【請求項6】
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
該ヨーレートセンサと前記直線距離算出手段との出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする請求項1記載の距離算出装置。
【請求項7】
前記ヨーレートセンサの出力値、および/または出力値の変化率があらかじめ定められた出力値の範囲、および/または出力値の変化率の範囲を逸脱したか否かを監視するヨーレートセンサ監視手段と、
ヨーレートセンサ監視手段が該逸脱を検出した時、過去のヨーレートセンサの出力と移動物体の速度の値に基づいて、ヨーレートセンサの出力値を補正して前記角度変化量算出手段に与えるヨーレート補正手段をさらに備えることを特徴とする請求項6記載の距離算出装置。
【請求項8】
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する仰角算出手段と、
移動物体の2つの時点間の移動距離を算出する移動距離算出手段と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するピッチング角算出手段と、
該仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する移動方向距離算出手段とを備えることを特徴とする距離算出装置。
【請求項9】
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該データベース手段、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする請求項8記載の距離算出装置。
【請求項10】
前記対象物を含む画像から対象物を認識し、該認識結果を前記仰角算出手段に与える対象物認識手段をさらに備えることを特徴とする請求項8記載の距離算出装置。
【請求項11】
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて対象物の認識結果としての座標を補正し、前記仰角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする請求項10記載の距離算出装置。
【請求項12】
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段の出力を用いて、該移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段とをさらに備え、
前記ピッチング角算出手段が、該傾斜角算出手段の出力を用いてピッチング角の算出を行うことを特徴とする請求項8記載の距離算出装置。
【請求項13】
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、前記傾斜角算出手段の出力に加えて、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする請求項12記載の距離算出装置。
【請求項14】
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする請求項8記載の距離算出装置。
【請求項15】
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面で成す角を方位角として算出する方位角算出手段と、
該ヨーレートセンサ、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする請求項8記載の距離算出装置。
【請求項16】
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する手順と、
該移動物体の2つの時点間の移動距離を算出する手順と、
該2つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する手順と、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出する手順とを計算機に実行させるための距離算出プログラム。
【請求項17】
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出するステップと、
該移動物体の2つの時点間の移動距離を算出するステップと、
該2つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出するステップと、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。
【請求項18】
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する手順と、
移動物体の2つの時点間の移動距離を算出する手順と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出する手順と、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する手順とを計算機に実行させるための距離算出プログラム。
【請求項19】
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するステップと、
移動物体の2つの時点間の移動距離を算出するステップと、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するステップと、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。

【国際公開番号】WO2005/043081
【国際公開日】平成17年5月12日(2005.5.12)
【発行日】平成19年5月10日(2007.5.10)
【国際特許分類】
【出願番号】特願2005−510146(P2005−510146)
【国際出願番号】PCT/JP2003/014029
【国際出願日】平成15年10月31日(2003.10.31)
【出願人】(000005223)富士通株式会社 (25,993)
【Fターム(参考)】