JP2009300457A - Distance calculator and calculation program - Google Patents
Distance calculator and calculation program Download PDFInfo
- Publication number
- JP2009300457A JP2009300457A JP2009214522A JP2009214522A JP2009300457A JP 2009300457 A JP2009300457 A JP 2009300457A JP 2009214522 A JP2009214522 A JP 2009214522A JP 2009214522 A JP2009214522 A JP 2009214522A JP 2009300457 A JP2009300457 A JP 2009300457A
- Authority
- JP
- Japan
- Prior art keywords
- angle
- distance
- moving
- calculating
- moving object
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Measurement Of Optical Distance (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
Description
本発明は、移動物体と固定対象物との間の距離算出方式に係り、さらに詳しくは例えば、走行車両と対象物の間の仰角や方位角、車両の速度やヨーレートなどの車両の状態量に基づいて、道路を走行している車両と対象物までの正確な距離を算出する距離算出装置、および算出プログラムに関する。 The present invention relates to a method for calculating a distance between a moving object and a fixed object, and more specifically, for example, a vehicle state quantity such as an elevation angle and an azimuth angle between a traveling vehicle and an object, a vehicle speed, and a yaw rate. The present invention relates to a distance calculation device and a calculation program for calculating an accurate distance between a vehicle traveling on a road and an object.
地図データベースやＧＰＳ（グローバル・ポジショニング・システム）を用いた車両の位置検出によって、車両の現在位置と目的地に応じてドライバに走行路を案内するカーナビゲーション技術が発達し、広範に普及している。そのようなシステムでは、ＧＰＳによって得られる緯度、経度の情報を基に、自車両の位置を地図上に重ねて表示し、交差点までの距離などを算出している。
このような従来技術の例として次の特許文献がある。
Car navigation technology that guides the driving path to the driver according to the current position of the vehicle and the destination by developing the position of the vehicle using a map database or GPS (Global Positioning System) has been developed and widely spread. . In such a system, based on the latitude and longitude information obtained by GPS, the position of the host vehicle is displayed superimposed on a map, and the distance to the intersection is calculated.
As an example of such a prior art, there is the following patent document.
この特許文献には、地図情報によって位置が特定できる対象物を認識し、２つの時点の間の車両の走行距離と、各地点からの同一の対象物までの仰角を用いて対象物と自車両間の距離を算出し、自車両の位置を補正する技術が開示されている。 This patent document recognizes an object whose position can be specified by map information, and uses the travel distance of the vehicle between two points in time and the elevation angle from each point to the same object to determine the object and the subject vehicle. A technique for calculating the distance between the vehicles and correcting the position of the host vehicle is disclosed.
しかしながら、この従来技術では信号機までの直線距離しか計算することができず、曲線道路や信号機が自車の進行方向上に無い時など、実際の走行路上の距離を求める場合に誤差が生じるという問題点があった。 However, this conventional technique can only calculate the straight distance to the traffic light, and there is an error when calculating the actual distance on the road, such as when there is no curved road or traffic light in the traveling direction of the vehicle. There was a point.
本発明の課題は上述の問題点に鑑み、対象物に対する車両からの仰角を含めて、方位角、速度あるいは加速度（ピッチング角）、ヨーレートなどを使い分けることによって、精度の高い距離計測機能を有する距離算出装置、および距離算出プログラムを提供することである。 In view of the above problems, the object of the present invention is a distance having a highly accurate distance measurement function by properly using an azimuth angle, a speed or acceleration (pitching angle), a yaw rate, etc., including an elevation angle from a vehicle with respect to an object. To provide a calculation device and a distance calculation program.
図１は、本発明の距離算出装置の原理構成ブロック図である。同図は後述する距離算出装置の実施例１の原理構成ブロック図であり、実施例１の距離算出装置１は、少なくとも方位角算出手段２、移動距離算出手段３、角度変化量算出手段４、および直線距離算出手段５を備える。
FIG. 1 is a block diagram showing the principle configuration of a distance calculation apparatus according to the present invention. The figure is a block diagram of the principle configuration of a distance calculating device according to a first embodiment described later. The
方位角算出手段２は、移動物体、例えば走行車両と固定対象物、例えば信号機とを結ぶ方向と、移動物体との移動方向とが水平面上で成す角を方位角として算出するものであり、移動距離算出手段３は、２つの時点間、例えば直線距離算出の処理間隔としての１００ｍｓの間の移動物体の移動距離を算出するものである。 The azimuth calculating means 2 calculates an angle formed by a moving object, for example, a direction connecting a traveling vehicle and a fixed object, for example, a traffic light, and a moving direction of the moving object on a horizontal plane as an azimuth. The distance calculating means 3 calculates the moving distance of the moving object between two time points, for example, 100 ms as the processing interval for calculating the linear distance.
角度変化量算出手段４は、その２つの時点間の移動物体の重心を通る垂直軸周りの回転角度を、進行方向の角度変化量として算出するものであり、直線距離算出手段５は方位角算出手段２、移動距離算出手段３、角度変化量算出手段４の出力を用いて、移動物体と対象物の間の直線距離を算出するものである。
The angle change amount calculation means 4 calculates a rotation angle around the vertical axis passing through the center of gravity of the moving object between the two time points as an angle change amount in the traveling direction, and the linear distance calculation means 5 calculates the azimuth angle. The linear distance between the moving object and the object is calculated using the outputs of the
発明の実施の形態においては、移動物体の現在位置を検出する現在位置センサと、現在位置センサの出力に応じて現在位置周辺の道路を含む地図情報を出力するデータベース手段と、データベース手段、方位角算出手段２、および直線距離算出手段５の出力を用いて
、現在位置から対象物までの走行距離を算出する走行距離算出手段を、距離算出装置１はさらに備えることができる。
In an embodiment of the invention, a current position sensor for detecting a current position of a moving object, a database means for outputting map information including roads around the current position according to an output of the current position sensor, a database means, an azimuth angle The
また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果、例えば対象物の座標を方位角算出手段２に与える対象物認識手段をさらに備えることもできる。この場合、対象物認識手段の出力があらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、その逸脱が検出された時、対象物認識手段の出力を補正して方位角算出手段に与える対象物認識補正手段をさらに備えることもできる。
In the embodiment, an object recognition unit that recognizes an object from an image including the object and gives the recognition result, for example, the coordinates of the object to the azimuth
あるいは移動物体の速度を移動距離算出手段３に与える速度センサと、前述の現在位置センサ、およびデータベース手段と、データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、ロール角と対象物の認識結果とを用いて対象物の認識結果、例えば座標を補正し、方位角算出手段２に与えるロール角補正手段をさらに備えることもできる。
Alternatively, the rotational angle around the moving direction of the moving object can be determined by using the output of the speed sensor that gives the speed of the moving object to the moving distance calculating
さらに実施の形態においては、移動物体の鉛直軸周りの回転角速度を検出し、角度変化量算出手段４に与えるヨーレートセンサと、ヨーレートセンサと直線距離算出手段５との出力を用いて、移動物体から対象物までの距離を予測する距離予測手段をさらに備えることもでき、またヨーレートセンサの出力、および／または出力の変化範囲があらかじめ定められた出力の範囲、および／または出力の変化範囲を逸脱したか否かを監視するヨーレート監視手段と、逸脱が検出された時、過去のヨーレートや速度の値を用いてヨーレートセンサの出力を補正し、角度変化量算出手段５に与えるヨーレート補正手段とをさらに備えることもできる。
Further, in the embodiment, the rotational angular velocity around the vertical axis of the moving object is detected, and the yaw rate sensor given to the angle change amount calculating means 4 and the outputs of the yaw rate sensor and the linear distance calculating
次に本発明の実施例３に対応する距離算出装置は、少なくとも仰角算出手段、移動距離算出手段、ピッチング角算出手段、および移動方向距離算出手段を備える。
仰角算出手段は移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するものであり、ピッチング角算出手段は移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するものであり、移動距離算出手段は図１に原理を説明した実施例１におけると同様に移動物体の２つの時点間の移動距離を算出するものである。そして移動方向距離算出手段は、仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて移動物体から対象物までの直線距離の（車両の）移動方向成分を算出する。
Next, a distance calculation apparatus corresponding to Example 3 of the present invention includes at least an elevation angle calculation means, a movement distance calculation means, a pitching angle calculation means, and a movement direction distance calculation means.
The elevation angle calculation means calculates the angle formed by the direction connecting the moving object and the target with the horizontal plane as the elevation angle, and the pitching angle calculation means calculates the moving object on the plane determined by the moving direction of the moving object and the vertical direction. The rotation angle is calculated as the pitching angle, and the movement distance calculation means calculates the movement distance between the two time points of the moving object as in the first embodiment whose principle is explained in FIG. Then, the moving direction distance calculating means calculates the moving direction component (of the vehicle) of the linear distance from the moving object to the target using the outputs of the elevation angle calculating means, the moving distance calculating means, and the pitching angle calculating means.
発明の実施の形態においては、実施例１に対すると同様に、前述の現在位置センサ、データベース手段、および移動距離算出手段をさらに備えることもできる。
また実施の形態においては、対象物を含む画像から対象物を認識し、その認識結果を仰角算出手段に与える対象物認識手段をさらに備えることもでき、また前述と同様の対象物認識監視手段、および対象物認識補正手段をさらに備えることもできる。
In the embodiment of the invention, similarly to the first embodiment, the present position sensor, the database means, and the movement distance calculation means can be further provided.
Further, in the embodiment, it is possible to further include an object recognition unit that recognizes the object from an image including the object and gives the recognition result to the elevation angle calculation unit. And an object recognition correcting means.
またこの場合、前述と同様の速度センサ、現在位置センサ、データベース手段、ロール角算出手段、およびロール角補正手段をさらに備えることもでき、ロール角補正手段は、対象物の認識結果を補正してその補正結果を仰角算出手段に与える。 In this case, a speed sensor, a current position sensor, a database unit, a roll angle calculation unit, and a roll angle correction unit similar to those described above may be further provided. The roll angle correction unit corrects the recognition result of the object. The correction result is given to the elevation angle calculation means.
さらに実施の形態においては、移動物体の移動方向の加速度を検出する加速度センサをさらに備え、ピッチング角算出手段が加速度センサの出力を用いてピッチング角を算出することもできる。 Furthermore, in the embodiment, an acceleration sensor that detects acceleration in the moving direction of the moving object is further provided, and the pitching angle calculation means can also calculate the pitching angle using the output of the acceleration sensor.
あるいは、前述と同様の現在位置センサ、データベース手段に加えて、データベース手段の出力を用いて移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段
をさらに備え、ピッチング角算出手段が、傾斜角算出手段の出力を用いてピッチング角の算出を行うこともでき、あるいはさらに加速度センサを備え、ピッチング角算出手段が傾斜角算出手段の出力に加えて、加速度センサの出力を用いてピッチング角の算出を行うこともできる。
Alternatively, in addition to the current position sensor and database means similar to those described above, the apparatus further comprises an inclination angle calculating means for calculating an inclination angle in the moving direction near the current position of the moving object using the output of the database means, and a pitching angle calculating means However, the pitching angle can be calculated using the output of the tilt angle calculating means, or further provided with an acceleration sensor, and the pitching angle calculating means uses the output of the acceleration sensor in addition to the output of the tilt angle calculating means. The pitching angle can also be calculated.
以上のように本発明によれば、移動物体の方位角や、移動物体の進行方向の角度変化量などを用いて、あるいは移動物体の仰角や移動物体のピッチング角などを用いて、対象物までの直線距離や移動物体の移動方向距離などの算出が行われる。 As described above, according to the present invention, the azimuth of the moving object, the amount of change in the traveling direction of the moving object, or the like, or the elevation angle of the moving object, the pitching angle of the moving object, etc. The straight line distance and the moving direction distance of the moving object are calculated.
以下本発明の実施の形態をいくつかの実施例に分けて詳細に説明する。
本発明において移動物体、例えば走行中の車両から、例えば直線距離が算出される固定対象物としては、信号機や道路標識をはじめとして、走行中の車両に設置されたカメラによって画像を捉えることができる看板、踏み切り、歩道橋、横断歩道、停止線などの各種の対象物までの距離が算出される。距離算出のために画像センサ、ヨーレートセンサ、車速センサ、および加速度センサなどからデータを、例えば１００ｍｓ毎に取り込んで、対象物認識や方位角算出などの各種の処理を１００ｍｓ毎に行い、最終的に例えば対象物までの直線距離を１００ｍｓ毎に算出する。以下の説明では、信号機を対象物の具体例として実施例を説明する。
The embodiment of the present invention will be described in detail below by dividing it into several examples.
In the present invention, as a fixed object for which, for example, a linear distance is calculated from a moving object, for example, a traveling vehicle, an image can be captured by a camera installed in the traveling vehicle, including a traffic light and a road sign. Distances to various objects such as signs, railroad crossings, footbridges, pedestrian crossings, and stop lines are calculated. In order to calculate the distance, data is taken from the image sensor, the yaw rate sensor, the vehicle speed sensor, the acceleration sensor, etc., for example, every 100 ms, and various processes such as object recognition and azimuth calculation are performed every 100 ms. For example, the linear distance to the object is calculated every 100 ms. In the following description, an embodiment will be described using a traffic light as a specific example of an object.
図２は本発明の距離算出装置の実施例１の構成ブロック図である。同図において距離算出装置１０に対しては、画像センサ１１、車速センサ１２、ヨーレートセンサ１３の出力がそれぞれ与えられる。画像センサ１１としては、例えばＣＣＤカメラやＣＭＯＳカメラなどが用いられ、カメラは進行方向を撮影するために車両の前方に設置され、その光軸は基本的に車両の進行方向と同一となるように設置される。車速センサ１２は、車両の車輪の速度を検出するものであり、例えば４個の車輪にそれぞれ設置される４つのセンサによって構成される。ヨーレートセンサ１３は、車両の重心を通り、鉛直方向の軸の周りの車両の回転角速度を検出するものである。
FIG. 2 is a block diagram of the configuration of the first embodiment of the distance calculation apparatus of the present invention. In the figure, the output of the
距離算出装置１０の内部の対象物認識部１４は、画像センサ１１によって撮像された画像を用いて、対象物の認識、例えば対象物の座標の認識を行うものである。前述のように対象物としては信号機を例にとるが、信号機の認識方法については図４で後述する。方位角算出部１５は、対象物認識部１４によって認識された対象物、例えば信号機の方位角を算出するものである。すなわち車両の重心を通る水平面上で、車両の重心と認識された対象物とを結ぶ線分の投影線と、車両の進行方向とが成す角を方位角として算出するものである。
The
移動距離算出部１６は、車速センサ１２の出力を用いて、１００ｍｓあたりの車両の移動距離を算出するものである。角度変化量算出部１７は、ヨーレートセンサ１３の出力を用いて、処理間隔１００ｍｓの間の鉛直方向の回転軸周りの車両の回転角を角度変化量として算出するものである。ヨーレートセンサ１３の角速度がω（ｒａｄ／ｓ）の時に、角度変化量は０．１ω（ｒａｄ）となる。
The travel
直線距離算出部１８は、ここでは方位角算出部１５、移動距離算出部１６、および角度変化量算出部１７の出力を用いて車両と対象物の間の直線距離を算出するものである。この算出方法については図６、７で後述する。走行距離予測部１９は、直線距離算出部１８、ヨーレートセンサ１３の出力を用いて、車両の現在地点から対象物の地点までの走行距離を予測するものである。その算出方法については図８で後述する。
Here, the straight line
図３は、実施例１における距離算出の全体処理フローチャートである。同図において処
理が開始されると、まずステップＳ１で画像センサ１１の出力を用いて対象物の位置認識、例えば信号機の赤や青の信号の画像中の座標の出力が行われ、ステップＳ２で車両の進行方向と対象物への方向とが成す角、すなわち方位角が算出され、ステップＳ３で車速センサ１２の出力を用いてある２つの時点の間の車両の移動距離、すなわち処理間隔１００ｍｓの間の移動距離が算出され、ステップＳ４でヨーレートセンサ１３の出力を用いて車両の鉛直軸周りの角度変化量が算出される。
FIG. 3 is an overall process flowchart of distance calculation in the first embodiment. When the processing is started in the figure, first, in step S1, the position of the object is recognized using the output of the
ただし、ここでステップＳ１からステップＳ４はそれぞれ独立に実行することができ、その順序を変更することも、また並行して処理することも可能である。続いてステップＳ５では、算出された方位角、移動距離、角度変化量を用いて対象物までの直線距離が算出され、ステップＳ６でその直線距離の算出結果とヨーレートセンサ１３の出力とを用いて、走行距離の予測が行われ、処理を終了する。
However, step S1 to step S4 can be executed independently of each other, and the order thereof can be changed or processed in parallel. Subsequently, in step S5, a linear distance to the object is calculated using the calculated azimuth angle, moving distance, and angle change amount, and in step S6, the calculation result of the linear distance and the output of the
図４は、図２の対象物認識部１４による対象物認識処理の詳細フローチャートである。同図において処理が開始されると、ステップＳ１１で特定の色、ここでは信号の青、黄、赤の色をもつ領域の検出が行われる。灯火している信号の色の領域を抜き出すことを考えるが、どの色が灯火しているかわからないため、ここでは３つの色の領域の検出をそれぞれ試みるものとする。３つの色それぞれに対し、複数の領域が検出されることも、領域がひとつも検出されないことも考えられる。
FIG. 4 is a detailed flowchart of the object recognition processing by the
ステップＳ１２で抽出された領域の大きさ、ここでは３つの色によって抽出されたすべての領域の大きさがそれぞれ取得され、ステップＳ１３で信号機までの距離にもよるが、正確な認識のためにあらかじめ設定された十分な範囲を持つ大きさによって領域の絞込みが行われる。 The size of the region extracted in step S12, here, the size of all the regions extracted by the three colors, is acquired, respectively, and depending on the distance to the traffic light in step S13, The area is narrowed down by a size having a set sufficient range.
続いてステップＳ１４で各領域、ここではステップＳ１３で抽出されたすべての領域の周囲長がそれぞれ取得され、ステップＳ１５でその周囲長ｌと領域の面積Ｓとを用いて、各領域の円形度が算出される。円形度は４πＳ／ｌ２によって算出され、ステップＳ１６
で円形度の大きさによる領域の絞込み、すなわち円形度の値があらかじめ定められた閾値、例えば０．８よりも大きい時にのみ、その領域が信号機の信号光の領域として認識される。そして処理結果として、画像中の信号機の信号光の領域の中心の座標が出力される。ここで円形度は完全な円に対しては１となるが、ここではステップＳ１３で抽出されたすべての領域の中で円形度が閾値より大きい領域の中心の座標が出力されるものとする。座標が複数出力されることも考えられるが、その場合出力された座標それぞれに関して、方位角等の算出を行うものとする。
Subsequently, in step S14, the perimeter of each region, here, all the regions extracted in step S13, is acquired. In step S15, the circularity of each region is determined using the perimeter length l and the area S of the region. Calculated. The circularity is calculated by 4πS / l 2 , and step S16
Thus, only when the area is narrowed down by the size of the circularity, that is, when the value of the circularity is larger than a predetermined threshold, for example, 0.8, the area is recognized as the signal light area of the traffic light. Then, as the processing result, the coordinates of the center of the signal light area of the traffic light in the image are output. Here, the circularity is 1 for a complete circle, but here, the coordinates of the center of the region where the circularity is larger than the threshold among all the regions extracted in step S13 are output. It is conceivable that a plurality of coordinates are output. In this case, the azimuth angle is calculated for each of the output coordinates.
図５は方位角算出部１５による方位角算出方法の説明図である。同図は後述する仰角の算出にも同様に用いられるが、方位角の算出においては画像上のｘ座標、すなわちカメラのレンズの位置Ｌを通る水平面上で、車両の進行方向に垂直な方向のｘ座標を用いて計算が行われる。
FIG. 5 is an explanatory diagram of an azimuth calculation method by the
図５において信号機の水平面上の位置をＳ、Ｓからレンズの光軸方向に垂直に前述の平面に垂線をおろした時、その足をＯ、レンズの焦点距離をＦ、撮像面の位置をＯ’、画像中の信号機（信号光）のｘ座標をｘ（ピクセル単位）、ピクセルのｘ方向の寸法をｌとすると、求めるべき方位角αはα’と等しく、方位角はｔａｎ−１（ｘｌ／Ｆ）によって求めることができる。 In FIG. 5, the position of the traffic light on the horizontal plane is S, and when a perpendicular line is dropped from S to the plane perpendicular to the optical axis direction of the lens, the foot is O, the focal length of the lens is F, and the position of the imaging surface is O. ', Where the x coordinate of the traffic light (signal light) in the image is x (pixel unit) and the size of the pixel in the x direction is l, the azimuth angle α to be obtained is equal to α', and the azimuth angle is tan −1 (xl / F).
図６は、直線距離算出部１８による車両から対象物Ｓまでの距離Ｘの算出方法の説明図である。図６に示すように信号機の位置をＳとし、車両が時刻Δｔの間にＡ１地点からＡ２地点に移動したとする。Ａ１地点からの信号機の方位角をθ１、Ａ２地点からの信号機
の方位角をθ２とする。Ａ１のときの車両の進行方向にＡ２から垂線を下ろし、交点をＡ
２’とする。Ａ１−Ａ２’の距離をＤ’、Ａ２−Ａ２’の距離をｄとすると、移動距離算出部１６により算出された移動距離Ｄと角度変化量算出部１７により算出された角度変化量γにより、Ｄ’とｄを近似的に表すと、
Ｄ’＝Ｄｃｏｓγ
ｄ＝Ｄｓｉｎγ
Ａ１のときの進行方向にＳから垂線を下ろし、交点をＯとする。Ａ２’地点からＯまでの距離をＸ’とすると、下記の式が成り立つ。
FIG. 6 is an explanatory diagram of a method for calculating the distance X from the vehicle to the object S by the linear
2 ′. If the distance of A1-A2 ′ is D ′ and the distance of A2-A2 ′ is d, the movement distance D calculated by the movement
D ′ = D cos γ
d = Dsinγ
A perpendicular line is drawn from S in the traveling direction at A1, and the intersection is defined as O. If the distance from the point A2 ′ to O is X ′, the following equation is established.
ｄ＋Ｘ’ｔａｎ（θ２＋γ）＝（Ｘ’＋Ｄ’）ｔａｎθ１
これにＸ’について解くと
Ｘ’＝（Ｄ’ｔａｎθ１−ｄ）／（ｔａｎ（θ２＋γ）−ｔａｎθ１）
＝（Ｄｃｏｓγｔａｎθ１−Ｄｓｉｎγ）／（ｔａｎ（θ２＋γ）−ｔａｎθ１）
Ａ２からＳまでの距離Ｘは以下の式によって表される。
d + X ′ tan (θ 2 + γ) = (X ′ + D ′) tan θ 1
Solving for X ′, X ′ = (D′ tan θ 1 −d) / (tan (θ 2 + γ) −tan θ 1 )
= (Dcos γ tan θ 1 −D sin γ) / (tan (θ 2 + γ) −tan θ 1 )
The distance X from A2 to S is represented by the following equation.
Ｘ＝Ｘ’／ｃｏｓ（θ２＋γ）
図７は直線距離算出部１８による、車両から対象物までの直線距離算出処理のフローチャートである。同図において処理が開始されるとステップＳ２０で移動距離算出部１６によって移動距離Ｄが、ステップＳ２１で方位角算出部１５によって方位角θ１、θ２が、ステップＳ２２で角度変化量算出部１７によってγの値が設定され、ステップＳ２３でＸ’の算出式の分母の絶対値がある閾値ｔ１が大きいか否かが判定される。この分母が０であ
る場合、Ｘ’の値を求めることができず、分母が０に近い場合にもＸ’を正確に求めることができないため、そのような場合には直ちに処理を終了する。
X = X ′ / cos (θ 2 + γ)
FIG. 7 is a flowchart of a straight line distance calculation process from the vehicle to the target by the straight line
閾値ｔ１より大きい場合には、ステップＳ２４でＸ’の値が算出され、ステップＳ２５
でＸの算出式の分母がある閾値ｔ２より大きいか否かが判定され、大きくない場合には直
ちに処理を終了し、大きい場合にはステップＳ２６でＸの値が算出されて処理を終了する。
If the threshold t 1 greater than the value of X 'is calculated in step S24, step S25
In is determined whether the threshold t 2 is greater than or not there is a denominator calculation formula of X is immediately terminated process is not greater, the value of X and the process is terminated calculated in step S26 in greater .
ステップＳ２３でＸ’の分母の絶対値が閾値ｔ１以下でＸ’の値を算出できなかった場
合には、過去の車両と対象物までの距離、およびその後の車両の車速やヨーレートなどを用いて車両の現在位置を予測し、対象物までの直線距離を算出することができる。図６においてＡ１地点から信号機Ｓまでの距離をＸ”とすると、Ｘ’は次式によって与えられ、前述のＸ’をこの値で置き換えてＸを算出することも可能である。
If the absolute value of the denominator of X ′ is less than or equal to the threshold t 1 and the value of X ′ cannot be calculated in step S23, the past distance between the vehicle and the object, the subsequent vehicle speed, yaw rate, etc. are used. Thus, the current position of the vehicle can be predicted, and the linear distance to the object can be calculated. In FIG. 6, when the distance from the point A1 to the traffic light S is X ″, X ′ is given by the following equation, and it is also possible to calculate X by replacing the aforementioned X ′ with this value.
Ｘ’＝Ｘ”ｃｏｓθ１−Ｄ’
図７のステップＳ２５でＸの分母が閾値ｔ２以下である時には、Ｘの算出を行うことが
できないが、速度が極端が遅く、移動距離があまりない時には、Ｘ”の値からＤの値を減算し、Ｘの値を近似的に求めることが可能である。
X ′ = X ″ cos θ 1 −D ′
When the denominator of X in the step S25 of FIG. 7 is the threshold value t 2 less is not able to calculate the X, are slow extreme, when there is not much movement distance, a value of D from the value of X " It is possible to subtract and obtain the value of X approximately.
図８は、走行距離予測部１９による、現在位置から対象物までの走行距離の予測方法の説明図である。走行距離予測部１９は、車両の現在位置からの走行路に沿った対象物までの距離を、過去のヨーレートと車両の速度から予測するものである。この予測においては、現在位置Ａから対象物の位置Ｓまでの走行路上で、車両の重心を通る垂直軸回りの回転角速度としてのヨーレートの値が一定であり、また車の速度の絶対値も一定であるとして、微小時間Δｔおきに、図８に示すようにベクトルｌ０、ｌ１、ｌ２・・を予測し、ベクト
ルの総和が対象物の位置Ｓに達するまで演算を繰り返すことによって、ベクトルの長さの総和を取り、対象物までの走行距離を予測する。
FIG. 8 is an explanatory diagram of a method for predicting the travel distance from the current position to the object by the travel
図８において、車両の現在地点Ａから現在の進行方向に対して対象物の位置から下ろし
た垂線の足をＳ’とすると、最初のベクトルｌ０がＳ’の方向と成す角は、角速度ωと微
小時間Δｔの積となる。次のベクトルｌ１とＳ’方向の成す角は、２ωと微小時間Δｔと
の積となる。同様にして各ベクトルの方向を求め、また各ベクトルの長さを現在の車両の速度ｖと微小時間Δｔとの積として計算することによって、各ベクトルの長さとその方向が決定され、対象物の位置Ｓに達するまでの走行距離の予測が行われる。
In FIG. 8, when the foot of the perpendicular line from the position of the object with respect to the current traveling direction from the current point A of the vehicle is S ′, the angle formed by the first vector l 0 with the direction of S ′ is the angular velocity ω And the minute time Δt. The angle formed by the next vector l 1 and the S ′ direction is the product of 2ω and the minute time Δt. Similarly, the direction of each vector is obtained, and the length of each vector and its direction are determined by calculating the length of each vector as the product of the current vehicle speed v and the minute time Δt. The travel distance until the position S is reached is predicted.
次に本発明の実施例２について説明する。図９は距離算出装置の実施例２の構成を示すブロック図である。同図を実施例１と比較すると、走行距離予測部１９に代わって走行距離算出部２３が備えられ、走行距離算出部２３に対しては、方位角算出部１５の出力と直線距離算出部１８の出力に加えて、距離算出装置１０の外側の現在位置センサ２１の出力を用いるデータベース２２の出力が与えられている。
Next, a second embodiment of the present invention will be described. FIG. 9 is a block diagram illustrating a configuration of the distance calculating device according to the second embodiment. Comparing this figure with the first embodiment, a travel
現在位置センサ２１は車両の現在位置を検出するものであり、例えば一般的にカーナビゲーションシステムに用いられるＧＰＳ（グローバル・ポジショニング・システム）である。ＧＰＳによって車両の現在位置の緯度、経度、標高などの絶対位置を得ることが可能であるが、この位置には一般的にある程度の誤差が含まれているため、誤差を考慮してそれらの位置を参照することが必要である。
The
データベース２２は道路の形状（標高や曲率半径）、信号機や歩道橋、踏み切りのような対象物の位置などを示す地図情報であり、現在位置センサ２１によって検出された自車両の位置を用いて地図上の車両の位置を特定することができる。データベース２２は、例えば自車両付近の対象物のデータや、道路形状のデータなどを走行距離算出部２３に与える。
The
走行距離算出部２３は、データベース２２の地図情報によって得られる対象物の位置に基づき、直線距離算出部１８によって算出された対象物と自車両との距離を用いて対象物への方向と、自車両の進行方向との成す角が、方位角算出部１５によって算出された方位角と等しくなる点を自車両の位置として決定し、その位置から対象物までの道路上の距離を地図情報から算出するものである。この算出方法について図１０、および図１１を用いて説明する。
Based on the position of the object obtained from the map information in the
図１１は、走行距離算出処理のフローチャートである。まずステップＳ３１で、図１０に示すように対象物Ｓの位置を中心として、自車両と対象物との直線距離を半径とする円が地図上に描かれ、ステップＳ３２でその円の円周がかかる道路上の円弧、図１０では４つの円弧が検出され、ステップＳ３３で各円弧の長さｌが算出される。 FIG. 11 is a flowchart of the travel distance calculation process. First, in step S31, as shown in FIG. 10, a circle whose radius is the linear distance between the host vehicle and the object is drawn on the map with the position of the object S as the center. In step S32, the circumference of the circle is changed. Such arcs on the road, four arcs in FIG. 10, are detected, and the length l of each arc is calculated in step S33.
続いてステップＳ３４で各円弧の端、すなわち図１０において円上で反時計方向に動く点を考えるとき、その点が最初に各円弧上に達する位置から各円弧上でｌ／４の長さの位置が車両の候補位置として検出される。すなわち、ここでは道路は上下各１車線とし、左側の車線の中心位置が車両の候補位置として検出される。 Subsequently, in step S34, when considering the end of each arc, that is, the point moving in the counterclockwise direction on the circle in FIG. 10, from the position where the point first reaches each arc, the length of ¼ on each arc is reached. The position is detected as a candidate position of the vehicle. That is, here, the road is one lane above and below, and the center position of the left lane is detected as a candidate vehicle position.
続いてステップＳ３５で、車両の候補位置における車両の進行方向を示す線、すなわち道路の接線方向と平行な線と、車両の候補位置と対象物とを結ぶ線との成す角がそれぞれの円弧について算出され、ステップＳ３６で算出された角度の値、図１０では４つの値のうちで、方位角算出部１５によって算出された方位角の値と最も近い車両の候補位置が車両の現在位置として決定され、ステップＳ３７で車両の現在位置から対象物までの距離が、道路形状に沿って算出されて処理を終了する。
Subsequently, in step S35, an angle formed by a line indicating the traveling direction of the vehicle at the candidate position of the vehicle, that is, a line parallel to the tangential direction of the road, and a line connecting the candidate position of the vehicle and the object is set for each arc. The candidate position of the vehicle that is calculated and is the closest to the azimuth value calculated by the azimuth
次に本発明の実施例３を図１２のブロック図を用いて説明する。同図において距離算出装置１０は、図９で説明した実施例２と同様に、画像センサ１１の出力を用いる対象物認
識部１４、車速センサ１２の出力を用いる移動距離算出部１６、現在位置センサ２１の出力を用いるデータベース２２に加えて、対象物認識部１４の出力を用いて対象物方向の仰角を算出する仰角算出部２５、データベース２２の出力を用いて車両の傾斜角を算出する傾斜角算出部２６、傾斜角算出部の２６の出力を用いて車両のピッチング角を算出するピッチング角算出部２７を備え、直線距離算出部１８に代わる進行方向距離算出部２８は、仰角算出部２５、移動距離算出部１６、およびピッチング角算出部２７の出力を用いて対象物までの直線距離の車両の進行方向成分を算出する。
Next,
仰角算出部２５による仰角の算出方法は、図５で説明した方位角の算出方法と基本的に同様である。ただし、仰角に対しては図５の撮像面上で、車両の進行方向を含む水平面に垂直なｙ軸方向の座標に加えて、ｘ軸方向の座標とレンズの焦点距離Ｆを用いて計算が行われる。すなわち、図５で点Ｓ’の信号機（信号光）のｘ、ｙ座標をｘ、ｙとすると、ピクセルのｘ、ｙ方向の寸法ｌを用いて仰角の値は
The elevation angle calculation method by the elevation
によって求められる。
傾斜角算出部２６は、現在位置センサ２１により検出された現在位置周辺の地図データがデータベース２２から与えられることにより、現在位置周辺で標高値のわかっている２つの地点の標高の差に基づいて、自車両周辺位置の傾斜角を算出し、ピッチング角算出部２７に与えるものであり、傾斜角算出方法の詳細については後述する。
Sought by.
The inclination
ピッチング角算出部２７は、傾斜角算出部２６によって算出された傾斜角、すなわち道路の進行方向の傾斜角を用いて車両のピッチング角、すなわち車両の前後を結ぶ軸と水平線との縦揺れを示す角度を算出するものである。車両のピッチング角が変化すると仰角が変化し、対象物までの正確な距離を求めることが不可能となる。このため、ピッチング角を算出し、仰角を補正する必要がある。この仰角の補正は、仰角算出部２５によって算出された仰角に対して進行方向距離算出部２８によって行われるものとする。
The pitching
車両のピッチング角と道路の傾斜角との関係は、車両の前後左右のサスペンションの伸縮によって決まり、サスペンションの伸縮はバネ係数によって決まる。そのため、本実施例では傾斜角と車両のピッチング角との間の関係を実験的に求めるものとする。車両が水平な面上に停止している時と、傾斜角θの斜面に停止している時のピッチング角を測定する。傾斜角θを変化させてピッチング角を測定することにより、傾斜角とピッチング角との間の関係が実験的に求められる。傾斜角に対するピッチング角の測定結果がない場合には、測定結果を補間することによってピッチング角が算出される。 The relationship between the vehicle pitching angle and the road inclination angle is determined by the expansion and contraction of the front, rear, left and right suspensions of the vehicle, and the expansion and contraction of the suspension is determined by the spring coefficient. Therefore, in this embodiment, the relationship between the tilt angle and the vehicle pitching angle is experimentally obtained. The pitching angle is measured when the vehicle is stopped on a horizontal surface and when the vehicle is stopped on a slope having an inclination angle θ. By measuring the pitching angle by changing the tilt angle θ, the relationship between the tilt angle and the pitching angle can be experimentally determined. When there is no measurement result of the pitching angle with respect to the tilt angle, the pitching angle is calculated by interpolating the measurement result.
図１３は、実施例３における進行方向距離算出処理の全体フローチャートである。同図において処理が開始されると、まず対象物認識部１４によって図４におけると同様に対象物の位置が認識され、ステップＳ４２で対象物への方向の仰角が算出され、ステップＳ４３で移動距離が算出され、ステップＳ４４で傾斜角算出部２６の出力を用いてピッチング角が算出され、ステップＳ４５で車両の現在位置から対象物までの距離を表すベクトルの車両の進行方向と平行な成分として、進行方向距離が算出されて処理を終了する。
FIG. 13 is an overall flowchart of the traveling direction distance calculation process according to the third embodiment. When the process is started in the figure, the
傾斜角の算出について説明する。図１４、および図１５は、傾斜角の算出方法の説明図であり、図１６は、傾斜角算出処理のフローチャートである。図１４は、データベース２２から出力される地図上での、車両の走行状態の説明図である。車両の現在位置周辺に、
標高の値がわかっている２つの地点Ｐ１とＰ２が存在するものとする。
The calculation of the tilt angle will be described. FIG. 14 and FIG. 15 are explanatory diagrams of a tilt angle calculation method, and FIG. 16 is a flowchart of the tilt angle calculation process. FIG. 14 is an explanatory diagram of the running state of the vehicle on the map output from the
It is assumed that there are two points P1 and P2 whose altitude values are known.
図１５は傾斜角θの算出方法の説明図である。図１４で説明したように、車両が点Ｐ１からＰ２まで走行する間の標高差をｈ２−ｈ１とし、地図上での点Ｐ１とＰ２の間の距離をｄとすると傾斜角θは次式によって与えられる。 FIG. 15 is an explanatory diagram of a method for calculating the tilt angle θ. As described with reference to FIG. 14, if the difference in elevation while the vehicle travels from point P1 to P2 is h2−h1 and the distance between points P1 and P2 on the map is d, the inclination angle θ is expressed by the following equation: Given.
θ＝ｔａｎ−１｛（ｈ２−ｈ１）／ｄ｝
図１６の傾斜角算出処理のフローチャートにおいて処理が開始されると、まずステップＳ５１、Ｓ５２で自車両の現在の位置から最も距離が近く、標高の値の既知な２つの地点Ｐ１、Ｐ２が探索され、ステップＳ５３で２つの地点の間の地図上での距離ｄが算出され、ステップＳ５４で傾斜角θの算出が行われて処理を終了する。
θ = tan −1 {(h2−h1) / d}
When the process is started in the flowchart of the inclination angle calculation process of FIG. 16, first, in steps S51 and S52, two points P1 and P2 that are closest to the current position of the host vehicle and have known altitude values are searched. In step S53, the distance d on the map between the two points is calculated. In step S54, the inclination angle θ is calculated, and the process ends.
次に進行方向距離の算出について、図１７−図１９を用いて説明する。図１７は、車両の進行状態の説明図である。同図において車両は処理時間、例えば１００ｍｓの間に点Ａ１から点Ａ２まで距離Ｄだけ移動したものとする。車両は直線方向に進行を続けるものと仮定し、対象物の位置Ｓから進行方向に下ろした垂線の足をＳ’とし、点Ａ２からＳ’までの距離Ｘを進行方向距離として算出するものとする。なお車両は実際には点Ａ２からはカーブする道路上を対象物Ｓに近づいていくものとする。 Next, calculation of the traveling direction distance will be described with reference to FIGS. FIG. 17 is an explanatory diagram of a traveling state of the vehicle. In the figure, it is assumed that the vehicle has moved from point A1 to point A2 by a distance D during a processing time, for example, 100 ms. Assuming that the vehicle continues to travel in a straight line direction, the perpendicular foot drawn from the position S of the object in the traveling direction is S ′, and the distance X from point A2 to S ′ is calculated as the traveling direction distance. To do. It is assumed that the vehicle actually approaches the object S on a curved road from the point A2.
図１８は、車両の進行状態を真横から見た状態を示す図である。図１８に示すように車両が時刻Δｔの間にＡ１地点からＡ２地点に移動したとする。Ａ１地点からの信号機の仰角をφ１、Ａ２地点からの信号機の仰角をφ２とする。Ａ１地点からＡ２地点までの移動距離算出部１６により算出した移動距離をＤ、Ａ２地点から信号機までの距離の車両の進行方向と平行な成分をＸとすると、下記の式が成り立つ。
FIG. 18 is a diagram illustrating a state in which the traveling state of the vehicle is viewed from the side. As shown in FIG. 18, it is assumed that the vehicle has moved from the A1 point to the A2 point during the time Δt. The elevation angle of the traffic light from the point A1 is φ 1 , and the elevation angle of the traffic light from the point A2 is φ 2 . When the movement distance calculated by the movement
（Ｘ＋Ｄ）ｔａｎφ１＝Ｘｔａｎφ２
Ｘについて解くと、
Ｘ＝Ｄｔａｎφ１／（ｔａｎφ２−ｔａｎφ１）
となり、対象物までの距離が算出できる。カメラの光軸方向が車両の進行方向に比べ仰角ρｃだけ上を向いている（カメラの設置時に設定）場合でも、距離Ｘは以下の式によって
表される。
(X + D) tanφ 1 = Xtanφ 2
Solving for X,
X = Dtanφ 1 / (tanφ 2 −tanφ 1 )
Thus, the distance to the object can be calculated. Even if the optical axis of the camera is facing up only elevation [rho c than in the traveling direction of the vehicle (set at the time of camera installation), the distance X is expressed by the following equation.
Ｘ＝Ｄｔａｎ（φ１＋ρｃ）／（ｔａｎ（φ２＋ρｃ）−ｔａｎ（φ１＋ρｃ））
また、Ａ１地点においてピッチング角算出部２７により算出したピッチング角をρ１、
Ａ２地点においてピッチング角算出部２７により算出したピッチング角をρ２とする。こ
のとき、距離Ｘは以下の式によって表される。
X = Dtan (φ 1 + ρ c ) / (tan (φ 2 + ρ c ) −tan (φ 1 + ρ c ))
Further, the pitching angle calculated by the pitching
The pitching angle calculated by the pitching
Ｘ＝Ｄｔａｎ（φ１＋ρｃ＋ρ１）／（ｔａｎ（φ２＋ρｃ＋ρ２）−ｔａｎ（φ１＋ρｃ＋ρ１））
このように、進行方向距離算出部２８は、仰角算出部２５により算出された仰角、移動距離算出部１６により算出した移動距離およびピッチング角算出部２７により算出したピッチング角に基づき、車両と対象物の距離の車両の進行方向と平行な成分を算出する。
X = Dtan (φ 1 + ρ c + ρ 1 ) / (tan (φ 2 + ρ c + ρ 2 ) −tan (φ 1 + ρ c + ρ 1 ))
As described above, the traveling direction
図１９の進行方向距離算出処理が開始されるとステップＳ５６で移動距離算出部１６によって移動距離Ｄが、ステップＳ５７で仰角算出部２５によって２つの仰角が、ステップＳ５８でピッチング角算出部２７によって２つのピッチング角の値が設定され、ステップＳ５９でＸの値を算出する式の分母の絶対値がある閾値ｔ１より大きいか否かが判定され
、ｔ１以下の値であるときには直ちに処理を終了し、閾値を超えている場合にはステップ
Ｓ６０でＸの値が算出されて処理を終了する。ステップＳ５９でＸを算出する式の分母の絶対値が０、または０に近い時、Ｘを正確に算出することはできないが、車両の速度が極
端に遅く、移動距離Ｄが小さい時には１つ前の時点で算出した距離Ｘから移動距離Ｄを減算して、進行方向距離Ｘを近似的に求めることもできる。
When the advancing direction distance calculation process in FIG. 19 is started, the movement distance D is calculated by the movement
続いて本発明の実施例４について説明する。図２０は実施例４の距離算出装置の構成ブロック図である。同図を図１２の実施例３と比較すると、距離算出装置１０の外部に現在位置センサ２１に代わって、車両の前後方向、すなわち進行方向の加速度を検出する加速度センサ３０が備えられ、ピッチング角算出部２７が加速度センサの出力を用いてピッチング角を算出して、進行方向距離算出部２８に与えることが異なっている。図２０において加速度センサ３０は、例えば車両の中央部分に設置され、圧電素子などによって車の前後方向の加速度成分を検出する。ピッチング角算出部２７は加速度センサ３０の出力としての加速度と、車両のピッチング角との間の既知の関係からピッチング角を算出するものとする。
Next, a fourth embodiment of the present invention will be described. FIG. 20 is a configuration block diagram of a distance calculation apparatus according to the fourth embodiment. Compared to the third embodiment of FIG. 12, the
車両のピッチング角と加速度との関係は、前述の傾斜角との関係のようにサスペンションの伸縮、すなわちバネ係数によって決まる。このため、加速度とピッチング角の関係を実験的に求めるものとする。 The relationship between the vehicle pitching angle and the acceleration depends on the expansion and contraction of the suspension, that is, the spring coefficient, as in the relationship with the tilt angle. For this reason, the relationship between the acceleration and the pitching angle is obtained experimentally.
すなわち、車両が水平な面上に停止している時と、水平な面上をある一定の加速度で走行している時のピッチング角の測定を行う。加速度の値を変化させ、ピッチング角を測定することによって、加速度とピッチング角の間の関係が実験的に求められる。加速度センサ３０によって検出された加速度の値に対するピッチング角が測定されていなかった場合には、測定結果を補間することによってピッチング角が求められる。
That is, the pitching angle is measured when the vehicle is stopped on a horizontal surface and when the vehicle is traveling on the horizontal surface at a certain acceleration. By changing the value of acceleration and measuring the pitching angle, the relationship between the acceleration and the pitching angle is experimentally determined. When the pitching angle with respect to the acceleration value detected by the
続いて本発明の実施例５について説明する。図２１は、距離算出装置の実施例５の構成を示すブロック図である。同図を実施例３を示す図１２、実施例４を示す図２０と比較すると、これらの実施例のすべての構成要素を備えており、ピッチング角算出部２７が傾斜角算出部２６の出力に加えて、加速度センサ３０の出力も用いてピッチング角を算出し、進行方向距離算出部２８に与えることだけが異なっている。
Next, Example 5 of the present invention will be described. FIG. 21 is a block diagram illustrating the configuration of the distance calculation device according to the fifth embodiment. Comparing this figure with FIG. 12 showing the third embodiment and FIG. 20 showing the fourth embodiment, all the components of these embodiments are provided, and the pitching
図２１のピッチング角算出部２７は、ピッチング角と道路の傾斜角、および加速度との間の実験的関係に基づいてピッチング角を算出する。ある一定の傾斜角の斜面を一定加速度で車両が走行している時のピッチング角を測定し、傾斜角、および加速度を変えて測定を繰り返すことによって、傾斜角と加速度が与えられた時のピッチング角を算出することが可能となる。あるいは実施例３において述べたピッチング角と傾斜角との間の実験的関係と、実施例４において述べたピッチング角と加速度との間の関係を用いて、それぞれピッチング角を求め、それらの和を計算することによって、傾斜角と加速度に対応するピッチング角を算出することも可能である。
The pitching
図２２は距離算出装置の実施例６の構成ブロック図である。同図を図１２の実施例３と比較すると、方位角算出部１５と走行距離算出部２３がさらに備えられている点が異なっている。方位角算出部１５は、図２の実施例１におけると同様に車両の方位角を算出し、走行距離算出部２３に与える。
FIG. 22 is a configuration block diagram of
走行距離算出部２３は、方位角算出部１５の出力、データベース２２の出力、進行方向距離算出部２８の出力を用いて走行距離を算出する。図９で説明した実施例２では走行距離算出部２３は、直線距離算出部１８の出力、すなわち車両の位置から対象物までの直線距離を用いて、対象物までの道路に沿った走行距離を算出している。その様子は図１０に説明されている。一方、図２２の進行方向距離算出部２８は、図１７で説明したように車両の進行方向の距離Ｘを算出しており、車両の現在位置と対象物までの直線距離は方位角算出部１５の出力、すなわち∠ＳＡ２Ｓ’を用いることによって容易に算出される。従っ
てこの直線距離を用いることによって、走行距離算出部２３は対象物までの走行距離を算出することができる。
The travel
図２３は距離算出装置の実施例７の構成を示すブロック図である。同図を図１２で説明した実施例３と比較すると、図２の実施例１などで説明したヨーレートセンサ１３と、走行距離予測部１９をさらに備えている点が異なっている。ここで走行距離予測部１９は、図２では直線距離算出部１８の算出結果、すなわち車両の現在位置から対象物までの直線距離に代わって、進行方向距離算出部２８の算出結果、すなわち図１７で説明した距離Ｘを用いて、図８で説明した方法と同様にして対象物までの走行距離を予測するものである。
FIG. 23 is a block diagram illustrating a configuration of the distance calculating device according to the seventh embodiment. 12 is different from the third embodiment described in FIG. 12 in that the
図２４は本発明の距離算出装置の実施例８の構成ブロック図である。同図を図２の実施例１と比較すると、距離算出装置１０の外部に現在位置センサ２１が備えられ、装置１０の内部に現在位置センサ２１の出力を受け取るデータベース２２と、データベース２２の出力と車速センサ１２の出力とを用いて車両のロール角、すなわち車両が進行方向の軸の周りに回転する角度としてのロール角を算出するロール角算出部３２と、対象物認識部１４と方位角算出部１５との間に備えられ、対象物認識部１４の出力とロール角算出部３２の出力とを用いて、ロール角の発生によって生じる対象物認識部１４による認識結果の誤差を補正して、補正結果の対象物の座標を方位角算出部１５に与えるロール角補正部３３を備えている点が異なっている。
FIG. 24 is a block diagram showing the configuration of the eighth embodiment of the distance calculating apparatus according to the present invention. 2 is compared with the first embodiment of FIG. 2, a
図２４のロール角算出部３２は、一般的に車両がカーブを走行している時に発生するロール角を算出するものである。このロール角が発生すると、対象物認識部１４による認識結果が不正確となる、すなわち画像センサ１１の光軸が回転することにより、仰角、方位角ともに変化し、対象物までの正確な距離を求めることが不可能となる。そこでロール角を算出し、対象物認識部１４による認識結果を補正し、方位角を正しく算出することが必要となる。
The roll
ロール角は車両が走行している道路の曲率半径とサスペンションの伸縮によって決まる。ロール角と道路の曲率半径の関係を実験的に算出するものとし、一定曲率半径の道路を一定速度で走行した時のロール角を測定する。曲率半径と速度の値を様々に変化させ、ロール角の値を測定することによって、ロール角と道路の曲率半径、および車両の速度との間の関係を実験的に求めることができる。 The roll angle is determined by the curvature radius of the road on which the vehicle is traveling and the expansion and contraction of the suspension. The relationship between the roll angle and the radius of curvature of the road is calculated experimentally, and the roll angle when a road with a constant radius of curvature travels at a constant speed is measured. By varying the value of the radius of curvature and the speed and measuring the value of the roll angle, the relationship between the roll angle and the radius of curvature of the road and the speed of the vehicle can be determined experimentally.
ロール角補正部３３は、ロール角算出部３２によって算出されたロール角を用いて対象物認識部１４の認識結果、例えば信号機の信号光の中心の画像上の座標位置の補正を行う。図２５はロール角補正方法の説明図である。ロール角θによる座標軸の傾きを補正するために座標（ｘ’、ｙ’）から座標（ｘ、ｙ）を算出する。
The roll
図２５に示すようにロール角をθ、対象物の座標（ｘ’、ｙ’）とし、車両の重心からカメラの中心までの高さをＬ、カメラの画素サイズをｌ、補正後の対象物の座標を（ｘ、ｙ）とする。ｌを用いてｘ’、ｙ’を画像の中心からの距離Ｘ’、Ｙ’に置き換える。 As shown in FIG. 25, the roll angle is θ, the coordinates of the object (x ′, y ′), the height from the center of gravity of the vehicle to the center of the camera is L, the pixel size of the camera is l, and the corrected object Let (x, y) be the coordinates of. l 'is used to replace x' and y 'with distances X' and Y 'from the center of the image.
Ｘ’＝ｘ’・ｌ、Ｙ’＝ｙ’・ｌ、
Ｘ、ＹとＸ’、Ｙ’の関係により以下の２式が成立する。
（Ｌ＋Ｙ）２＋Ｘ２＝（Ｌ＋Ｙ’）２＋Ｘ’２
X ′ = x ′ · l, Y ′ = y ′ · l,
The following two formulas are established according to the relationship between X and Y and X ′ and Y ′.
(L + Y) 2 + X 2 = (L + Y ′) 2 + X ′ 2
これをＸおよびＹについて解くと以下の式となる。 When this is solved for X and Y, the following equation is obtained.
最後に、カメラの画素サイズｌを用いて下式のように画像上の座標系に戻す。
ｘ＝Ｘ／ｌ、ｙ＝Ｙ／ｌ
図２６は、ロール角補正処理のフローチャートである。同図において、ステップＳ６５でロール角θが生じた場合の対象物の座標（ｘ’、ｙ’）から、対象物の画像の中心からの距離Ｘ’、Ｙ’が算出され、ステップＳ６６でθ’の値が算出され、ステップＳ６７で補正後の対象物の画像の中心からの距離Ｘ、ステップＳ６８でＹが算出され、ステップＳ６９で補正後の画像上の座標（ｘ、ｙ）が算出されて処理を終了する。
Finally, the camera pixel size l is used to return to the coordinate system on the image as in the following equation.
x = X / l, y = Y / l
FIG. 26 is a flowchart of the roll angle correction process. In the figure, distances X ′ and Y ′ from the center of the image of the object are calculated from the coordinates (x ′, y ′) of the object when the roll angle θ occurs in step S65, and θ in step S66. 'Is calculated, the distance X from the center of the image of the corrected object is calculated in step S67, Y is calculated in step S68, and the coordinates (x, y) on the corrected image are calculated in step S69. To finish the process.
最後に本発明の実施例９について説明する。図２７は距離算出装置の実施例９の構成ブロック図である。同図を例えば図２の実施例１と比較すると、走行距離予測部１９に代わって走行距離算出部２３が設けられ、走行距離算出部２３に対しては直線距離算出部１８の出力に加えて、方位角算出部１５の出力と現在位置センサ２１の出力に基づくデータベース２２の出力とが与えられ、対象物認識部１４に対してその認識結果に異常がないか否かを監視する対象物認識監視部３５と、異常があった場合に対象物認識部１４の認識結果を補正して方位角算出部１５に与える対象物認識補正部３６、同様にヨーレートセンサ１３の出力（角速度）、または出力の変化率（角加速度）を監視するヨーレートセンサ監視部３８と、出力、または出力の変化率に異常があった時、ヨーレートセンサの出力を補正して角度変化量算出部１７に与えるヨーレート補正部３９を備えている点が異なっている。
Finally, Example 9 of the present invention will be described. FIG. 27 is a configuration block diagram of Embodiment 9 of the distance calculation device. Compared to FIG. 2, for example, the first embodiment of FIG. 2, a travel
図２７において対象物認識部１４は、画像センサ１１によって撮像された画像に基づいて対象物を認識するものであるが、様々な原因によって認識結果に誤りが生じる可能性がある。そのため過去の認識結果と比較して、例えば対象物としての信号機の位置が処理間隔１００ｍｓの間に著しくずれている場合など、対象物の認識結果に異常があると判断される場合に、対象物認識監視部３５はその異常を対象物認識補正部３６に通知する。
In FIG. 27, the
対象物認識補正部３６は、対象物認識部１４の認識結果に異常があると通知された時に、過去の対象物認識結果に基づき、線形予測やカルマンフィルタを用いた予測を行った結果を、対象物認識の補正結果として方位角算出部１５に与える。
When the object
ヨーレートセンサ監視部３８は、ヨーレートセンサ１３の出力、すなわちヨーレートセンサが出力する車両の鉛直軸周りの回転角速度の値が急変した場合など、車両の正常な動きとしては考えにくい角速度の変化などを検出した時に、それをヨーレート補正部３９に通知する。
The yaw rate
ヨーレート補正部３９は、ヨーレートセンサ監視部３８からヨーレートセンサ１３の出
力やその変化率の異常が通知された時に、ヨーレートセンサの出力値に代わって、過去のヨーレートや速度の値などに基づき線形予測やカルマンフィルタによる予測の結果を利用して補正されたヨーレートの値を角度変化量算出部１７に与える。
The yaw
なお、図２７では、例えば図９の実施例２と比較して対象物認識監視部３５、対象物認識補正部３６、ヨーレートセンサ監視部３８、およびヨーレート補正部３９が追加された構成となっているが、例えば図２３の実施例７を基本として、これらの各部が追加され、対象物認識補正部３６が認識結果を補正して仰角算出部２５に与える構成も当然可能である。
In FIG. 27, for example, an object
以上において、本発明の距離算出装置、および算出プログラムについてその詳細を説明したが、この距離算出装置は当然一般的なコンピュータシステムとして構成することが可能である。図２８はそのようなコンピュータシステム、すなわちハードウェア環境の構成ブロック図である。 Although the details of the distance calculation apparatus and calculation program of the present invention have been described above, the distance calculation apparatus can naturally be configured as a general computer system. FIG. 28 is a block diagram showing the configuration of such a computer system, that is, a hardware environment.
図２８においてコンピュータシステムは中央処理装置（ＣＰＵ）５０、リードオンリメモリ（ＲＯＭ）５１、ランダムアクセスメモリ（ＲＡＭ）５２、通信インタフェース５３、記憶装置５４、入出力装置５５、可搬型記憶媒体の読取り装置５６、およびこれらの全てが接続されたバス５７によって構成されている。
In FIG. 28, the computer system includes a central processing unit (CPU) 50, a read only memory (ROM) 51, a random access memory (RAM) 52, a
記憶装置５４としてはハードディスク、磁気ディスクなど様々な形式の記憶装置を使用することができ、このような記憶装置５４、またはＲＯＭ５１に図３、図４、図７、図１１、図１３、図１６、図１９、および図２６などのフローチャートに示されたプログラムや、本発明の特許請求の範囲の請求項１６〜１９のプログラムなどが格納され、そのようなプログラムがＣＰＵ５０によって実行されることにより、本実施形態における画像センサ、車速センサ、現在位置センサ、加速度センサ、ヨーレートセンサなどを用いた距離算出が可能となる。
As the
このようなプログラムは、プログラム提供者５８側からネットワーク５９、および通信インタフェース５３を介して、例えば記憶装置５４に格納されることも、また市販され、流通している可搬型記憶媒体６０に格納され、読取り装置５６にセットされて、ＣＰＵ５０によって実行されることも可能である。可搬型記憶媒体６０としてはＣＤ−ＲＯＭ、フレキシブルディスク、光ディスク、光磁気ディスク、ＤＶＤなど様々な形式の記憶媒体を使用することができ、このような記憶媒体に格納されたプログラムが読取り装置５６によって読み取られることにより、本実施形態における走行車両から対象物までの正確な距離の算出が可能となる。
Such a program is stored in, for example, the
以上詳細に説明したように本発明によれば、例えば信号機などの対象物を認識し、自車両からその対象物までの方位角、あるいは仰角を算出し、必要に応じて車両のヨーレート、進行方向の加速度、進行方向の軸周りの回転角や、路面の傾斜角によるピッチング角などの影響を補正することによって、自車両と対象物までの距離を正確に算出することが可能となる。また、車両の走行状態や地図データを用いることによって、対象物までの走行距離を算出することも可能となる。さらに目的が車両の自動停止の場合には停止線を対象物として、停止線までの距離を正確に算出することもでき、目的に応じて各種の対象物までの距離算出を行うことができる。
（付記１）
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該移動物体の２つの時点間の移動距離を算出する移動距離算出手段と、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する角度変化量算出手段と、
該方位角算出手段、移動距離算出手段、および角度変化量算出手段の出力を用いて、移動物体と対象物との間の直線距離を算出する直線距離算出手段とを備えることを特徴とする距離算出装置。
（付記２）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段、前記方位角算出手段、および直線距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記３）
前記対象物を含む画像から対象物を認識し、該認識結果を前記方位角算出手段に与える対象物認識手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記４）
前記対象物認識手段の出力が、あらかじめ定められた範囲を逸脱したか否かを監視する対象物認識監視手段と、
該対象物認識監視手段が該逸脱を検出した時、該対象物認識手段の出力を補正して前記方位角算出手段に与える対象物認識補正手段をさらに備えることを特徴とする付記３記載の距離算出装置。
（付記５）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて、移動物体の移動方向軸の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて、対象物の認識結果としての座標を補正し、前記方位角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする付記３記載の距離算出装置。
（付記６）
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
該ヨーレートセンサと前記直線距離算出手段との出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする付記１記載の距離算出装置。
（付記７）
前記ヨーレートセンサの出力値、および／または出力値の変化率があらかじめ定められた出力値の範囲、および／または出力値の変化率の範囲を逸脱したか否かを監視するヨーレートセンサ監視手段と、
ヨーレートセンサ監視手段が該逸脱を検出した時、過去のヨーレートセンサの出力と移動物体の速度の値に基づいて、ヨーレートセンサの出力値を補正して前記角度変化量算出手段に与えるヨーレート補正手段をさらに備えることを特徴とする付記６記載の距離算
出装置。
（付記８）
移動物体と固定対象物との間の距離を算出する装置において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する仰角算出手段と、
移動物体の２つの時点間の移動距離を算出する移動距離算出手段と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するピッチング角算出手段と、
該仰角算出手段、移動距離算出手段、およびピッチング角算出手段の出力を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する移動方向距離算出手段とを備えることを特徴とする距離算出装置。
（付記９）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面上で成す角を方位角として算出する方位角算出手段と、
該データベース手段、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、現在位置から前記対象物までの移動軌道上の距離を算出する走行距離算出手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１０）
前記対象物を含む画像から対象物を認識し、該認識結果を前記仰角算出手段に与える対象物認識手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１１）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
移動物体の速度を前記移動距離算出手段に与える速度センサと、
該データベース手段と速度センサとの出力を用いて移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて対象物の認識結果としての座標を補正し、前記仰角算出手段に与えるロール角補正手段とをさらに備えることを特徴とする付記１０記載の距離算出装置。
（付記１２）
前記移動物体の現在位置を検出する現在位置センサと、
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
該データベース手段の出力を用いて、該移動物体の現在位置付近の移動方向の傾斜角を算出する傾斜角算出手段とをさらに備え、
前記ピッチング角算出手段が、該傾斜角算出手段の出力を用いてピッチング角の算出を行うことを特徴とする付記８記載の距離算出装置。
（付記１３）
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、前記傾斜角算出手段の出力に加えて、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする付記１２記載の距離算出装置。
（付記１４）
前記移動物体の移動方向の加速度を検出する加速度センサをさらに備え、
前記ピッチング角算出手段が、該加速度センサの出力を用いてピッチング角の算出を行うことを特徴とする付記８記載の距離算出装置。
（付記１５）
前記移動物体の鉛直軸回りの回転角速度を検出し、前記角度変化量算出手段に与えるヨーレートセンサと、
移動物体と対象物とを結ぶ方向と移動物体の移動方向とが水平面で成す角を方位角として算出する方位角算出手段と、
該ヨーレートセンサ、方位角算出手段、および前記移動方向距離算出手段の出力を用いて、移動物体から前記対象物までの移動軌道上の距離を予測する距離予測手段をさらに備えることを特徴とする付記８記載の距離算出装置。
（付記１６）
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出する手順と、
該移動物体の２つの時点間の移動距離を算出する手順と、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出する手順と、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出する手順とを計算機に実行させるための距離算出プログラム。
（付記１７）
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向と該移動物体の移動方向とが水平面上で成す角を方位角として算出するステップと、
該移動物体の２つの時点間の移動距離を算出するステップと、
該２つの時点間の移動物体の重心を通る垂直軸回りの移動物体の回転角度を、移動方向の角度変化量として算出するステップと、
該算出された方位角、移動距離、および角度変化量を用いて、移動物体と対象物との間の直線距離を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。
（付記１８）
移動物体と固定対象物との間の距離を算出する計算機によって使用されるプログラムにおいて、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出する手順と、
移動物体の２つの時点間の移動距離を算出する手順と、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出する手順と、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出する手順とを計算機に実行させるための距離算出プログラム。
（付記１９）
移動物体と固定対象物との間の距離を算出する計算機によって使用される記憶媒体において、
移動物体と対象物とを結ぶ方向が水平面と成す角を仰角として算出するステップと、
移動物体の２つの時点間の移動距離を算出するステップと、
移動物体の移動方向と鉛直方向とで決まる平面上での移動物体の回転角度をピッチング角として算出するステップと、
該算出された仰角、移動距離、およびピッチング角を用いて、移動物体から対象物までの直線距離の移動方向成分を算出するステップとを計算機に実行させるためのプログラムを格納した計算機読出し可能可搬型記憶媒体。
As described above in detail, according to the present invention, for example, an object such as a traffic light is recognized, the azimuth angle or elevation angle from the own vehicle to the object is calculated, and the yaw rate and traveling direction of the vehicle are calculated as necessary. It is possible to accurately calculate the distance between the host vehicle and the object by correcting the influence of the acceleration, the rotation angle around the axis in the traveling direction, and the pitching angle due to the inclination angle of the road surface. Further, the travel distance to the object can be calculated by using the travel state of the vehicle and the map data. Further, when the purpose is to automatically stop the vehicle, the distance to the stop line can be accurately calculated using the stop line as an object, and the distance to various objects can be calculated according to the purpose.
(Appendix 1)
In an apparatus for calculating the distance between a moving object and a fixed object,
Azimuth angle calculating means for calculating an angle formed by a direction connecting the moving object and the target object and a moving direction of the moving object on a horizontal plane as an azimuth angle;
A moving distance calculating means for calculating a moving distance between two time points of the moving object;
Angle change amount calculating means for calculating a rotation angle of the moving object around the vertical axis passing through the center of gravity of the moving object between the two time points as an angle change amount in the moving direction;
A linear distance calculating means for calculating a linear distance between the moving object and the object using outputs of the azimuth calculating means, the moving distance calculating means, and the angle change amount calculating means; Calculation device.
(Appendix 2)
A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
The apparatus further comprises travel distance calculating means for calculating a distance on the moving trajectory from the current position to the object using outputs of the database means, the azimuth angle calculating means, and the straight line distance calculating means. The distance calculation apparatus according to 1.
(Appendix 3)
The distance calculation apparatus according to
(Appendix 4)
Object recognition monitoring means for monitoring whether the output of the object recognition means deviates from a predetermined range;
The distance according to
(Appendix 5)
A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
A speed sensor that gives the speed of the moving object to the moving distance calculating means;
Roll angle calculation means for calculating a rotation angle about the moving direction axis of the moving object as a roll angle using outputs of the database means and the speed sensor;
The apparatus further comprises roll angle correction means that corrects the coordinates as the recognition result of the object using the outputs of the roll angle calculation means and the object recognition means, and gives them to the azimuth angle calculation means. The distance calculation device according to
(Appendix 6)
A yaw rate sensor that detects a rotational angular velocity around the vertical axis of the moving object and provides the angular change amount calculating means;
The distance calculation apparatus according to
(Appendix 7)
A yaw rate sensor monitoring means for monitoring whether the output value of the yaw rate sensor and / or the rate of change of the output value deviates from a predetermined range of output values and / or a range of the rate of change of output values;
When the yaw rate sensor monitoring means detects the deviation, yaw rate correction means for correcting the output value of the yaw rate sensor based on the past output of the yaw rate sensor and the value of the speed of the moving object and giving the output to the angle change amount calculation means. The distance calculation apparatus according to
(Appendix 8)
In an apparatus for calculating the distance between a moving object and a fixed object,
An elevation angle calculating means for calculating, as an elevation angle, an angle formed by a direction connecting the moving object and the object with the horizontal plane;
A moving distance calculating means for calculating a moving distance between two time points of the moving object;
A pitching angle calculating means for calculating the rotation angle of the moving object on the plane determined by the moving direction and the vertical direction of the moving object as a pitching angle;
And a moving direction distance calculating unit that calculates a moving direction component of a linear distance from the moving object to the target using outputs of the elevation angle calculating unit, the moving distance calculating unit, and the pitching angle calculating unit. Distance calculation device.
(Appendix 9)
A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
An azimuth angle calculating means for calculating an angle formed by a direction connecting the moving object and the target object and a moving direction of the moving object on a horizontal plane as an azimuth angle;
It further comprises travel distance calculation means for calculating a distance on a moving trajectory from the current position to the object using outputs of the database means, azimuth angle calculation means, and movement direction distance calculation means. The distance calculation apparatus according to
(Appendix 10)
9. The distance calculating apparatus according to
(Appendix 11)
A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
A speed sensor that gives the speed of the moving object to the moving distance calculating means;
Roll angle calculating means for calculating a rotation angle around the moving direction of the moving object as a roll angle using outputs of the database means and the speed sensor;
(Appendix 12)
A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
An inclination angle calculating means for calculating an inclination angle of the moving direction near the current position of the moving object using the output of the database means;
9. The distance calculation apparatus according to
(Appendix 13)
An acceleration sensor for detecting acceleration in the moving direction of the moving object;
13. The distance calculating apparatus according to
(Appendix 14)
An acceleration sensor for detecting acceleration in the moving direction of the moving object;
The distance calculating apparatus according to
(Appendix 15)
A yaw rate sensor that detects a rotational angular velocity around the vertical axis of the moving object and provides the angular change amount calculating means;
An azimuth angle calculating means for calculating an angle formed by a horizontal plane between a direction connecting the moving object and the target object and a moving direction of the moving object as an azimuth angle;
The apparatus further includes a distance predicting unit that predicts a distance on the moving trajectory from the moving object to the target object using outputs of the yaw rate sensor, the azimuth calculating unit, and the moving direction distance calculating unit. 9. The distance calculation device according to 8.
(Appendix 16)
In a program used by a computer that calculates the distance between a moving object and a fixed object,
A procedure for calculating, as an azimuth angle, an angle formed on a horizontal plane between a direction connecting the moving object and the target and the moving direction of the moving object;
A procedure for calculating a moving distance between two time points of the moving object;
A procedure for calculating a rotation angle of the moving object around the vertical axis passing through the center of gravity of the moving object between the two time points as an angle change amount in the moving direction;
A distance calculation program for causing a computer to execute a procedure for calculating a linear distance between a moving object and an object using the calculated azimuth angle, moving distance, and angle change amount.
(Appendix 17)
In a storage medium used by a computer that calculates the distance between a moving object and a fixed object,
Calculating an angle formed on a horizontal plane between a direction connecting the moving object and the target and the moving direction of the moving object as an azimuth;
Calculating a moving distance between two time points of the moving object;
Calculating a rotation angle of the moving object about the vertical axis passing through the center of gravity of the moving object between the two time points as an angle change amount in the moving direction;
A computer-readable portable type storing a program for causing a computer to execute a step of calculating a linear distance between the moving object and the object using the calculated azimuth angle, moving distance, and angle change amount Storage medium.
(Appendix 18)
In a program used by a computer that calculates the distance between a moving object and a fixed object,
A procedure for calculating an angle formed by the direction connecting the moving object and the target and the horizontal plane as an elevation angle;
A procedure for calculating a moving distance between two time points of the moving object;
A procedure for calculating the rotation angle of the moving object on the plane determined by the moving direction and the vertical direction of the moving object as a pitching angle;
A distance calculation program for causing a computer to execute a procedure for calculating a moving direction component of a linear distance from a moving object to a target object using the calculated elevation angle, moving distance, and pitching angle.
(Appendix 19)
In a storage medium used by a computer that calculates the distance between a moving object and a fixed object,
Calculating an angle formed by the direction connecting the moving object and the target and the horizontal plane as an elevation angle;
Calculating a moving distance between two time points of the moving object;
Calculating a rotation angle of the moving object on a plane determined by the moving direction and the vertical direction of the moving object as a pitching angle;
A computer readable portable type storing a program for causing a computer to execute a step of calculating a moving direction component of a linear distance from a moving object to a target using the calculated elevation angle, moving distance, and pitching angle Storage medium.
本発明は、自動車などに搭載されるナビゲーション装置の製造産業は当然のこととして、対象物までの正確な距離の算出技術を必要とするすべての産業において利用可能である。
The present invention can be used in all industries that require a technique for calculating an accurate distance to an object, as a matter of course, in the manufacturing industry of navigation devices mounted on automobiles and the like.
Claims (2)
前記対象物を含む画像から前記対象物を認識する対象物認識手段と、
前記画像の撮影に用いられたレンズの位置を通る水平面上で前記移動物体の進行方向に垂直な方向であるｘ方向のピクセル単位の座標であって前記対象物認識手段による認識結果として得られる前記画像中の前記対象物の座標ｘと、前記水平面に垂直なｙ方向のピクセル単位の座標であって前記対象物認識手段による認識結果として得られる前記画像中の前記対象物の座標ｙに基づいて、前記画像の撮像面上での前記ｘ方向および前記ｙ方向のピクセルの寸法ｓと、前記レンズの焦点距離Ｆとを用いて、前記移動物体と前記対象物とを結ぶ方向が水平面と成す角を仰角ｔａｎ−１（｜ｙｓ｜／（（ｘｓ）２＋Ｆ２）１／２）として算出する仰角算出手段と、
前記移動物体の２つの時点間の移動距離Ｄを算出する移動距離算出手段と、
前記移動物体の移動方向と鉛直方向とで決まる平面上での前記移動物体の回転角度をピッチング角として算出するピッチング角算出手段と、
前記移動物体の進行方向に対する前記レンズの光軸方向の仰角ρｃ、前記仰角算出手段が前記２つの時点それぞれについて算出した仰角φ１およびφ２、前記移動距離算出手段が算出した前記移動距離Ｄ、ならびに前記ピッチング角算出手段が前記２つの時点それぞれについて算出したピッチング角ρ１およびρ２を用いて、
Ｘ＝Ｄｔａｎ（φ１＋ρｃ＋ρ１）／（ｔａｎ（φ２＋ρｃ＋ρ２）−ｔａｎ（φ１＋ρｃ＋ρ１））
なる式により前記移動物体から前記対象物までの直線距離の移動方向成分Ｘを算出する移動方向距離算出手段とを備え、
前記ピッチング角算出手段は、
前記移動物体が水平面上を一定の加速度で走行している時のピッチング角を、複数の加速度について予め測定した測定結果から得られる、加速度とピッチング角との第１の関係に基づいて、前記移動物体の前記進行方向の加速度から前記ピッチング角を算出するか、
前記移動物体が一定の傾斜角の斜面を一定の加速度で走行している時のピッチング角を、複数の傾斜角と複数の加速度について予め測定した測定結果から得られる、傾斜角と加速度の組とピッチング角との第２の関係に基づいて、前記移動物体が走行している面の傾斜角と前記移動物体の前記進行方向の加速度から前記ピッチング角を算出するか、または、
前記移動物体が一定の傾斜角の斜面に停止している時のピッチング角を、複数の傾斜角について予め測定した測定結果から得られる、傾斜角とピッチング角との第３の関係に基づいて、前記移動物体が走行している面の傾斜角から前記ピッチング角の第１の成分を算出し、前記第１の関係に基づいて、前記移動物体の前記進行方向の前記加速度から前記ピッチング角の第２の成分を算出し、前記第１の成分と前記第２の成分から前記ピッチング角を算出する、
ことを特徴とする距離算出装置。 In an apparatus for calculating the distance between a moving object and a fixed object,
Object recognition means for recognizing the object from an image including the object;
The coordinates in pixel units in the x direction, which is the direction perpendicular to the traveling direction of the moving object, on the horizontal plane passing through the position of the lens used for taking the image, and obtained as a recognition result by the object recognition means Based on the coordinates x of the object in the image and the coordinates in pixel units in the y direction perpendicular to the horizontal plane and obtained as a recognition result by the object recognition means, based on the coordinates y of the object in the image The angle formed by the horizontal plane with the direction connecting the moving object and the object using the pixel dimensions s in the x and y directions on the imaging plane of the image and the focal length F of the lens Elevation angle calculating means for calculating the angle as the elevation angle tan −1 (| ys | / ((xs) 2 + F 2 ) 1/2 ),
A moving distance calculating means for calculating a moving distance D between two time points of the moving object;
Pitching angle calculation means for calculating a rotation angle of the moving object on a plane determined by a moving direction and a vertical direction of the moving object as a pitching angle;
The elevation angle ρ c in the optical axis direction of the lens with respect to the moving direction of the moving object, the elevation angles φ 1 and φ 2 calculated by the elevation angle calculation means for each of the two time points, and the movement distance D calculated by the movement distance calculation means And the pitching angles ρ 1 and ρ 2 calculated by the pitching angle calculation means for each of the two time points,
X = Dtan (φ 1 + ρ c + ρ 1 ) / (tan (φ 2 + ρ c + ρ 2 ) −tan (φ 1 + ρ c + ρ 1 ))
A moving direction distance calculating means for calculating a moving direction component X of a linear distance from the moving object to the target object according to the following formula:
The pitching angle calculation means includes
Based on the first relationship between the acceleration and the pitching angle obtained from the measurement result obtained by measuring the pitching angle when the moving object is traveling at a constant acceleration on a horizontal plane in advance for a plurality of accelerations, the movement is performed. The pitching angle is calculated from the acceleration in the traveling direction of the object,
A set of a tilt angle and an acceleration obtained from a measurement result obtained by measuring a pitching angle when the moving object is traveling on a slope with a constant tilt angle at a constant acceleration in advance for a plurality of tilt angles and a plurality of accelerations. Based on the second relationship with the pitching angle, the pitching angle is calculated from the inclination angle of the surface on which the moving object is traveling and the acceleration in the traveling direction of the moving object, or
Based on the third relationship between the inclination angle and the pitching angle obtained from the measurement results obtained by measuring the pitching angle when the moving object is stopped on the slope having a constant inclination angle in advance for a plurality of inclination angles, A first component of the pitching angle is calculated from an inclination angle of a surface on which the moving object is traveling, and based on the first relationship, a first component of the pitching angle is calculated from the acceleration of the moving object in the traveling direction. 2 components are calculated, and the pitching angle is calculated from the first component and the second component.
A distance calculation device characterized by that.
該現在位置センサの出力に応じて、該現在位置周辺の道路を含む地図情報を出力するデータベース手段と、
前記移動物体の速度を前記移動距離算出手段に与える速度センサと、
前記データベース手段と前記速度センサとの出力を用いて前記移動物体の移動方向の回りの回転角度をロール角として算出するロール角算出手段と、
該ロール角算出手段と前記対象物認識手段との出力を用いて前記対象物の認識結果としての座標を補正し、前記仰角算出手段に前記座標ｘおよび前記座標ｙとして与えるロール角補正手段と、
をさらに備えることを特徴とする請求項１記載の距離算出装置。 A current position sensor for detecting a current position of the moving object;
Database means for outputting map information including roads around the current position in response to the output of the current position sensor;
A speed sensor that gives the speed of the moving object to the moving distance calculating means;
Roll angle calculating means for calculating a rotation angle around the moving direction of the moving object as a roll angle using outputs of the database means and the speed sensor;
Roll angle correction means that corrects the coordinates as the recognition result of the object using the outputs of the roll angle calculation means and the object recognition means, and gives the coordinates x and the coordinates y to the elevation angle calculation means;
The distance calculation apparatus according to claim 1, further comprising:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2009214522A JP4664427B2 (en) | 2009-09-16 | 2009-09-16 | Distance calculation device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2009214522A JP4664427B2 (en) | 2009-09-16 | 2009-09-16 | Distance calculation device |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2005510146A Division JP4398430B2 (en) | 2003-10-31 | 2003-10-31 | Distance calculation device and calculation program |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2009300457A true JP2009300457A (en) | 2009-12-24 |
| JP4664427B2 JP4664427B2 (en) | 2011-04-06 |
Family
ID=41547465
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2009214522A Expired - Fee Related JP4664427B2 (en) | 2009-09-16 | 2009-09-16 | Distance calculation device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4664427B2 (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012098968A (en) * | 2010-11-02 | 2012-05-24 | Aisin Seiki Co Ltd | Distance estimation apparatus, distance estimation method and program |
| WO2012086401A1 (en) * | 2010-12-24 | 2012-06-28 | 株式会社日立製作所 | Driving assist device |
| JP2013145168A (en) * | 2012-01-13 | 2013-07-25 | Denso Corp | Angular velocity error correction device of gyro for vehicle |
| CN105416290A (en) * | 2015-11-30 | 2016-03-23 | 奇瑞汽车股份有限公司 | Method and device for detecting distances between vehicles |
| KR20170014916A (en) * | 2015-07-31 | 2017-02-08 | 부경대학교 산학협력단 | Method for calculating an object's coordinates in an image using single camera and gps |
| JP2017075846A (en) * | 2015-10-14 | 2017-04-20 | 富士通株式会社 | Acceleration correction program, road surface condition evaluation program, acceleration correction method and acceleration correction device |
| US9651386B2 (en) | 2012-09-07 | 2017-05-16 | Mando Corporation | Apparatus and method for calculating distance between vehicles |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2557808C1 (en) * | 2014-04-09 | 2015-07-27 | Федеральное государственное образовательное бюджетное учреждение высшего профессионального образования "Санкт-Петербургский государственный университет телекоммуникаций им. проф. М.А. Бонч-Бруевича" | Method of determining inclined range to moving target using passive monostatic direction-finder |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05196437A (en) * | 1992-01-20 | 1993-08-06 | Nippon Telegr & Teleph Corp <Ntt> | Input device for three-dimensional information |
| JPH05314243A (en) * | 1992-04-03 | 1993-11-26 | Sony Corp | Three-dimensional shape restoring method |
| JPH0735560A (en) * | 1993-07-23 | 1995-02-07 | Nippondenso Co Ltd | Navigation device |
| JPH10341458A (en) * | 1997-06-10 | 1998-12-22 | Toyota Motor Corp | Method for correcting on-vehicle stereo camera and onvehicle stereo camera applied with the method |
| JPH1163949A (en) * | 1997-08-20 | 1999-03-05 | Ricoh Co Ltd | Device and method for restoring threedimensional shape |
| JP2000161915A (en) * | 1998-11-26 | 2000-06-16 | Matsushita Electric Ind Co Ltd | On-vehicle single-camera stereoscopic vision system |
| JP2001187553A (en) * | 1999-10-21 | 2001-07-10 | Matsushita Electric Ind Co Ltd | Parking support system |
| JP2002501349A (en) * | 1998-01-06 | 2002-01-15 | インテル・コーポレーション | Method for determining relative camera orientation to create a 3-D visual image |
| JP4398430B2 (en) * | 2003-10-31 | 2010-01-13 | 富士通株式会社 | Distance calculation device and calculation program |
-
2009
- 2009-09-16 JP JP2009214522A patent/JP4664427B2/en not_active Expired - Fee Related
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05196437A (en) * | 1992-01-20 | 1993-08-06 | Nippon Telegr & Teleph Corp <Ntt> | Input device for three-dimensional information |
| JPH05314243A (en) * | 1992-04-03 | 1993-11-26 | Sony Corp | Three-dimensional shape restoring method |
| JPH0735560A (en) * | 1993-07-23 | 1995-02-07 | Nippondenso Co Ltd | Navigation device |
| JPH10341458A (en) * | 1997-06-10 | 1998-12-22 | Toyota Motor Corp | Method for correcting on-vehicle stereo camera and onvehicle stereo camera applied with the method |
| JPH1163949A (en) * | 1997-08-20 | 1999-03-05 | Ricoh Co Ltd | Device and method for restoring threedimensional shape |
| JP2002501349A (en) * | 1998-01-06 | 2002-01-15 | インテル・コーポレーション | Method for determining relative camera orientation to create a 3-D visual image |
| JP2000161915A (en) * | 1998-11-26 | 2000-06-16 | Matsushita Electric Ind Co Ltd | On-vehicle single-camera stereoscopic vision system |
| JP2001187553A (en) * | 1999-10-21 | 2001-07-10 | Matsushita Electric Ind Co Ltd | Parking support system |
| JP4398430B2 (en) * | 2003-10-31 | 2010-01-13 | 富士通株式会社 | Distance calculation device and calculation program |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012098968A (en) * | 2010-11-02 | 2012-05-24 | Aisin Seiki Co Ltd | Distance estimation apparatus, distance estimation method and program |
| WO2012086401A1 (en) * | 2010-12-24 | 2012-06-28 | 株式会社日立製作所 | Driving assist device |
| CN103270543A (en) * | 2010-12-24 | 2013-08-28 | 株式会社日立制作所 | Driving assist device |
| US9097541B2 (en) | 2010-12-24 | 2015-08-04 | Hitachi, Ltd. | Driving support device |
| JP2013145168A (en) * | 2012-01-13 | 2013-07-25 | Denso Corp | Angular velocity error correction device of gyro for vehicle |
| US9651386B2 (en) | 2012-09-07 | 2017-05-16 | Mando Corporation | Apparatus and method for calculating distance between vehicles |
| KR20170014916A (en) * | 2015-07-31 | 2017-02-08 | 부경대학교 산학협력단 | Method for calculating an object's coordinates in an image using single camera and gps |
| KR101709317B1 (en) * | 2015-07-31 | 2017-02-22 | 부경대학교 산학협력단 | Method for calculating an object's coordinates in an image using single camera and gps |
| JP2017075846A (en) * | 2015-10-14 | 2017-04-20 | 富士通株式会社 | Acceleration correction program, road surface condition evaluation program, acceleration correction method and acceleration correction device |
| CN105416290A (en) * | 2015-11-30 | 2016-03-23 | 奇瑞汽车股份有限公司 | Method and device for detecting distances between vehicles |
Also Published As
| Publication number | Publication date |
|---|---|
| JP4664427B2 (en) | 2011-04-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4398430B2 (en) | Distance calculation device and calculation program | |
| JP4664427B2 (en) | Distance calculation device | |
| US11113544B2 (en) | Method and apparatus providing information for driving vehicle | |
| US8239131B2 (en) | Navigation device, navigation method, and navigation program | |
| US6753902B1 (en) | Image processing apparatus, image processing method, navigation apparatus, program storage device and computer data signal embodied in carrier wave | |
| CN103718224B (en) | Three-dimensional body pick-up unit and three-dimensional body detection method | |
| RU2668459C1 (en) | Position evaluation device and method | |
| US20220270358A1 (en) | Vehicular sensor system calibration | |
| WO2012086401A1 (en) | Driving assist device | |
| JP6881369B2 (en) | Vehicle position estimation device | |
| KR102441073B1 (en) | Apparatus for compensating sensing value of gyroscope sensor, system having the same and method thereof | |
| JP5088401B2 (en) | Road structure measuring method and road surface measuring device | |
| EP3499187B1 (en) | Method and apparatus for determining object position | |
| US20220155075A1 (en) | Method and apparatus with localization | |
| US11908206B2 (en) | Compensation for vertical road curvature in road geometry estimation | |
| JP7234840B2 (en) | position estimator | |
| JP6422431B2 (en) | Improvement of inertial sensor | |
| JP4375549B2 (en) | Vehicle positioning device | |
| JP7411108B2 (en) | Vehicle posture estimation system and vehicle posture estimation method | |
| JP6522270B1 (en) | SUPPORT IMAGE DISPLAY DEVICE, SUPPORT IMAGE DISPLAY METHOD, AND SUPPORT IMAGE DISPLAY PROGRAM | |
| JP6037351B2 (en) | Vehicle image processing apparatus and vehicle image processing method | |
| US20230101472A1 (en) | Methods and Systems for Estimating Lanes for a Vehicle | |
| EP4160154A1 (en) | Methods and systems for estimating lanes for a vehicle | |
| US20240020868A1 (en) | Vehicle and method of controlling the same | |
| CN116659491A (en) | Vehicle initialization positioning method and system by means of road boundary line constraint |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20101228 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20110106 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 4664427 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140114 Year of fee payment: 3 |
|
| LAPS | Cancellation because of no payment of annual fees |