JP5910180B2 - Moving object position and orientation estimation apparatus and method - Google Patents
Moving object position and orientation estimation apparatus and method Download PDFInfo
- Publication number
- JP5910180B2 JP5910180B2 JP2012049382A JP2012049382A JP5910180B2 JP 5910180 B2 JP5910180 B2 JP 5910180B2 JP 2012049382 A JP2012049382 A JP 2012049382A JP 2012049382 A JP2012049382 A JP 2012049382A JP 5910180 B2 JP5910180 B2 JP 5910180B2
- Authority
- JP
- Japan
- Prior art keywords
- image
- moving object
- captured image
- estimation
- posture
- 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.)
- Active
Links
Images
Landscapes
- Image Processing (AREA)
- Traffic Control Systems (AREA)
- Image Analysis (AREA)
- Navigation (AREA)
Description
本発明は、移動物体の位置を推定する移動物***置姿勢推定装置及び方法に関する。 The present invention relates to a moving object position / posture estimation apparatus and method for estimating the position of a moving object.
３次元地図とカメラの撮像画像を比較することによって移動物体の位置を算出する技術として、例えば下記の特許文献１に記載された技術が知られている。この特許文献１には、車両に備えられた車載カメラにより得られる現実の映像から抽出したエッジ画像と、周囲環境のエッジの位置や形状を３次元で記録した３次元地図を車載カメラの位置姿勢に投影したときの仮想映像とが一致するように車載カメラの位置と姿勢角を調整する。これによって、車載カメラの３次元空間での位置と姿勢角を推定している。 As a technique for calculating the position of a moving object by comparing a three- dimensional map and a captured image of a camera, for example, a technique described in Patent Document 1 below is known. The Patent Document 1, the edge image extracted from the real image obtained by the vehicle-mounted camera provided in the vehicle, the position and orientation of the three-dimensional map of the vehicle camera recorded in three dimensions the position and shape of the edge of the surrounding environment The position and posture angle of the in-vehicle camera are adjusted so that the virtual image when projected onto the camera matches the virtual image. As a result, the position and posture angle of the in-vehicle camera in the three- dimensional space are estimated.
しかしながら、特許文献１において、現実の映像と仮想映像が一致していても、この一致した場所が車載カメラから遠い場合、車載カメラの位置の誤差が大きい可能性がある。逆に、一致した位置が車載カメラから近い場合、車載カメラの姿勢角の誤差が大きい可能性がある。 However, in Patent Document 1, even if the actual video and the virtual video match, if the matching location is far from the in-vehicle camera, there may be a large error in the position of the in-vehicle camera. Conversely, if the matched position is close to the in-vehicle camera, the error in the attitude angle of the in-vehicle camera may be large.
そこで、本発明は、上述した実情に鑑みて提案されたものであり、精度良く移動物体の位置及び姿勢角を推定することができる移動物***置姿勢推定装置及び方法を提供することを目的とする。 Therefore, the present invention has been proposed in view of the above-described circumstances, and an object thereof is to provide a moving object position / posture estimation apparatus and method that can accurately estimate the position and posture angle of a moving object. .
本発明は、前方撮像画像の姿勢角推定用画像尤度を高め、側方撮像画像の位置推定用画像尤度を高めて、位置推定用画像尤度が所定値以上の撮像画像と仮想画像との一致度に基づいて位置を推定すると共に、姿勢角推定用画像尤度が所定値以上の撮像画像と仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定する。 The present invention increases the image likelihood for posture angle estimation of the front captured image, increases the image likelihood for position estimation of the side captured image, and captures the captured image and the virtual image whose position estimation image likelihood is a predetermined value or more. The position is estimated based on the degree of coincidence, and the attitude angle of the actual moving object is estimated based on the degree of coincidence between the captured image whose attitude angle estimation image likelihood is equal to or greater than a predetermined value and the virtual image.
本発明によれば、自車両の姿勢角を前方撮像画像を用いて推定でき、自車両の位置を側方撮像画像を用いて推定できるので、精度良く移動物体の位置及び姿勢角を推定することができる。 According to the present invention, the posture angle of the host vehicle can be estimated using the front captured image, and the position of the host vehicle can be estimated using the side captured image. Therefore, the position and posture angle of the moving object can be estimated with high accuracy. Can do.
以下、本発明の実施の形態について図面を参照して説明する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.
本発明の実施形態として示す移動物***置姿勢推定装置は、例えば図１に示すように構成される。この移動物***置姿勢推定装置は、ＥＣＵ１、カメラ群２、３次元地図データベース３、及び、車両センサ群４を含む。車両センサ群４は、ＧＰＳ受信機４１、アクセルセンサ４２、ステアリングセンサ４３、ブレーキセンサ４４、車速センサ４５、加速度センサ４６、車輪速センサ４７、及び、ヨーレートセンサ等のその他センサ４８を含む。なお、ＥＣＵ１は、実際にはＲＯＭ、ＲＡＭ、演算回路等にて構成されている。ＥＣＵ１がＲＯＭに格納された移動物***置姿勢推定用のプログラムに従って処理をすることによって、後述する機能（仮想画像取得手段、画像尤度設定手段、移動物***置姿勢推定手段）を実現する。
A moving object position and orientation estimation apparatus shown as an embodiment of the present invention is configured as shown in FIG. This moving object position / posture estimation apparatus includes an ECU 1, a
カメラ群２は、複数の撮像手段（カメラ）を含む。カメラ群２の各カメラは例えばＣＣＤ等の固体撮像素子を用いたものである。カメラ群２は、少なくとも車両の前方カメラ（前方撮像手段）と、左側又は左側の何れか一方を撮像する側方カメラ（側方撮像手段）とを含む。
The
ここで、図２に示すように、自車両Ｖの前方向とは自車両Ｖの主たる進行方向である。自車両Ｖの後方向とは前方向とは逆の方向である。自車両Ｖの右方向とは前方向を見た場合の右手方向である。自車両Ｖの左方向とは前方向を見た場合の左手方向である。また、カメラ群２に含まれるカメラは、図３に示すように、所定の撮像画角θであり、カメラの撮像方向を中心とした撮像範囲２１となっている。
Here, as shown in FIG. 2, the forward direction of the host vehicle V is the main traveling direction of the host vehicle V. The rear direction of the host vehicle V is the direction opposite to the front direction. The right direction of the host vehicle V is the right-hand direction when viewing the front direction. The left direction of the host vehicle V is the left hand direction when the front direction is viewed. Further, as shown in FIG. 3, the cameras included in the
カメラ群２の車載例を図３に示す。図４（ａ）、（ｂ）は本実施形態における最小構成を示す。図４（ａ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの左方向が撮像範囲２１ｌとされた左側方カメラ２ｌとを含む。図４（ｂ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの右方向が撮像範囲２１ｒとされた右側方カメラ２ｒとを含む。
An in-vehicle example of the
また、カメラ群２は、図５乃至図７の何れかの構成であってもよい。図５（ａ）、（ｂ）に示したカメラ群２は、図４（ａ）、（ｂ）の構成に加えて、自車両Ｖの後方向が撮像範囲２１ｂとされた後方カメラ２ｂを含む。図６に示したカメラ群２は、前方カメラ２ｆと、自車両Ｖの右方向及び左方向の双方を撮像可能なように左側方カメラ２ｌ及び右側方カメラ２ｒの双方を含む。図７（ａ）、（ｂ）に示したカメラ群２は、自車両Ｖの全方向を撮像範囲とするものである。図７（ａ）に示したカメラ群２は、自車両Ｖの前方向、右方向、左方向、後方向のすべてが撮像可能なように前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ、及び、後方カメラ２ｂを含む。図７（ｂ）に示すカメラ群２は、自車両Ｖの全方向を切れ目なく撮像範囲２１ａとされた全方位カメラ２ａのみを含む。なお、図４乃至図７に示したカメラ群２ではなく、自車両Ｖの前方や側方等を撮像できるような画角がカバーできていれば、例えば自車両Ｖに対して斜めにカメラを設置してもよい。
The
カメラ群２に含まれるカメラは、所定時間毎に撮像して撮像画像を取得し、ＥＣＵ１に供給する。
Cameras included in the
３次元地図データベース３は、例えば路面表示を含む周囲環境のエッジ等の３次元位置情報が記憶されている。本実施形態において、３次元地図データベース３には、白線、停止線、横断歩道、路面マーク等の路面表示の他に、縁石、建物等の構造物のエッジ情報も含まれる。白線などの各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。
The three-
車両センサ群（走行状態検出手段）４は、ＥＣＵ１に接続される。車両センサ群４は、各センサ４１〜４８により検出した各種のセンサ値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることで、自車両Ｖの概位置の算出、単位時間に自車両Ｖが進んだ移動量を示すオドメトリを算出する。特に、車両センサ群４は、自車両Ｖに搭載されたカメラ群２により取得する撮像画像が遮蔽される遮蔽状態を検出する（遮蔽状態検出手段）。
The vehicle sensor group (running state detection means) 4 is connected to the ECU 1. The vehicle sensor group 4 supplies various sensor values detected by the
また、ＥＣＵ１は、カメラ群２により撮像された撮像画像と３次元地図データベース３に記憶された３次元位置情報とを用いて自車両Ｖの位置及び姿勢角の推定を行う電子制御ユニットである。特に、ＥＣＵ１は、車両センサ群４により検出された撮像画像の遮蔽状態に基づいて、自車両Ｖの位置及び姿勢角の推定を行う。なお、ＥＣＵ１は、他の制御に用いるＥＣＵと兼用しても良い。
特に、この移動物***置姿勢推定装置は、カメラ群２により撮像した撮像画像と、３次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換した仮想画像とを比較して、自車両Ｖの位置及び姿勢角を推定する。ここで、図８（ａ）に示すような撮像画像が得られ、図８（ｂ）のようなエッジ画像が得られたとする。一方、３次元位置情報をカメラ群２の位置及び姿勢角に投影した仮想画像が図８（ｃ）のようになったとする。図８（ｂ）の撮像画像と図８（ｃ）の仮想画像とを比較すると、遠方位置（Ａ）及び近傍位置（Ｂ）の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両Ｖの位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図８（ｄ）に示すようになる。この場合、図８（ａ）の撮像画像と図８（ｄ）の撮像画像とを比較すると、遠方位置（Ａ）は一致しているが、近傍位置（Ｂ）は大きくずれてしまう。逆に、図８（ｃ）の仮想画像の仮想姿勢角をずらし（図示せず）、図８（ａ）の撮像画像と比較すると、近傍位置（Ｂ）は一致するが、遠方位置（Ａ）は大きくずれる。
The ECU 1 is an electronic control unit that estimates the position and posture angle of the host vehicle V using the captured image captured by the
In particular, the moving object position / orientation estimation apparatus compares the captured image captured by the
このような現象に着目して、移動物***置姿勢推定装置は、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の仮想位置が尤もらしいと判断する。逆に、移動物***置姿勢推定装置は、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の仮想姿勢角が尤もらしいと判断する。 Focusing on such a phenomenon, the moving object position / posture estimation apparatus determines that the virtual position of the virtual image is likely when the neighboring position pixel in the captured image matches the neighboring position pixel in the virtual image. . Conversely, the moving object position / posture estimation apparatus determines that the virtual posture angle of the virtual image is likely when the far position pixel in the captured image matches the far position pixel in the virtual image.
以下、この移動物***置姿勢推定装置の動作について、図９の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、自車両Ｖの６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を推定するものとする。また、この図９の位置姿勢推定アルゴリズムは、ＥＣＵ１によって、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。 Hereinafter, the operation of this moving object position / orientation estimation apparatus will be described with reference to the position / orientation estimation algorithm of FIG. In the present embodiment, it is assumed that the position (front-rear direction, horizontal direction, vertical direction) and posture angle (roll, pitch, yaw) of the host vehicle V are estimated. Further, the position / orientation estimation algorithm of FIG. 9 is continuously performed by the ECU 1 at intervals of, for example, about 100 msec.
ステップＳ１において、ＥＣＵ１は、車両センサ群４から得られるセンサ値の情報から車速Ｖ（単位：ｍ／ｓ）とヨーレートγ（単位：ｒａｄ／ｓ）を検出する。ＥＣＵ１は、旋回半径ρ（単位：ｍ）を、ρ＝Ｖ／γなる演算によって取得する。更に、１ループ前の位置姿勢推定アルゴリズムにおけるステップＳ１を実行した時から自車両Ｖの移動量をオドメトリとして算出する。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。また、ヨーレートγが十分に小さい場合には、旋回半径ρは例えば１０００［ｍ］に設定して、車速を略ゼロで除算することを回避する。 In step S1, the ECU 1 detects the vehicle speed V (unit: m / s) and the yaw rate γ (unit: rad / s) from the sensor value information obtained from the vehicle sensor group 4. The ECU 1 obtains the turning radius ρ (unit: m) by calculation of ρ = V / γ. Furthermore, the movement amount of the host vehicle V is calculated as odometry from the time when step S1 in the position and orientation estimation algorithm before one loop is executed. In the case of the first loop after starting the position / orientation estimation algorithm, the odometry is calculated as zero. When the yaw rate γ is sufficiently small, the turning radius ρ is set to 1000 [m], for example, to avoid dividing the vehicle speed by substantially zero.
このオドメトリ算出方法としては、例えば、車両運動を平面上に限定した上で、各車輪の車輪速とヨーレートセンサから、単位時間での移動量と回転量を算出すれば良い。また、ＥＣＵ１は、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートセンサを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリが算出できればどの手法を用いても良い。
As the odometry calculation method, for example, the movement amount and the rotation amount per unit time may be calculated from the wheel speed of each wheel and the yaw rate sensor after limiting the vehicle motion to a plane. Further, the ECU 1 may substitute the wheel speed by the difference between the vehicle speed and the positioning value of the
次のステップＳ２において、ＥＣＵ１は、ステップＳ１にて検出した車速Ｖに基づいて、自車両Ｖの前後方向の撮像画像の取得範囲を設定する。本実施形態では、撮像範囲の取得範囲を自車両Ｖの前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂ（前後方向カメラ）の水平画角αf（単位：deg）とする。この前後方向カメラの画角は、図１０に示すように、車速Ｖに応じて設定される。車速Ｖが高いほど前後方向カメラの画角は狭くなる。なお、本実施形態は、自車両Ｖに車載されて前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂは、水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。
In the next step S2, the ECU 1 sets a captured image acquisition range in the front-rear direction of the host vehicle V based on the vehicle speed V detected in step S1. In the present embodiment, the acquisition range of the imaging range is set to the horizontal field angle αf (unit: deg) of the
なお、自車両Ｖの前後左右方向の撮像画像のうち、前後方向の撮像画像については車速Ｖが高くなるほど撮像範囲が狭くなる画像とする。これは、前後方向の撮像画像は、自車両Ｖの速度が低くなるほど横方向の運動の影響を受けやすくなるため、より広い範囲で撮像して３次元地図データと一致するように調整する必要があるためである。 Of the captured images in the front-rear and left-right directions of the host vehicle V, the captured image in the front-rear direction is an image in which the imaging range becomes narrower as the vehicle speed V increases. This is because captured images in the front-rear direction are more susceptible to lateral movement as the speed of the host vehicle V decreases, so it is necessary to adjust the image to be captured in a wider range and match the three-dimensional map data. Because there is.
次のステップＳ３において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、前方向の画像である前方撮像画像を抽出する。また、ＥＣＵ１は、撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、後方撮像画像を抽出する。これにより、ＥＣＵ１は、前後撮像画像群を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として使用する画像の撮像方向に応じて、前後一方の撮像画像を抽出してもよく、双方の撮像画像を抽出してもよい。後述する処理において後方撮像画像を使用しない場合には、後方撮像画像を抽出する必要はない。なお、抽出する画像は、図３のように、自車両Ｖの前方方向から左右にそれぞれ水平画角αf／２の角度θの範囲内で撮像された部分である。
In the next step S3, the ECU 1 extracts a forward captured image that is a forward image over the acquisition range of the captured image in the front-rear direction set in step S2 from the captured images captured by the
ステップＳ４において、ＥＣＵ１は、ステップＳ１で算出した旋回半径ρに基づいて左方向及び右方向の画像の取得範囲を設定する。本実施形態において、左方向及び右方向の画像取得範囲を自車両Ｖの左右横方向を撮像する左側方カメラ２ｌ又は右側方カメラ２ｒの水平画角αs（単位：deg）とする。この左方向及び右方向カメラの画角は、図１１に示すように、旋回半径ρを参照して設定される。旋回半径ρが大きいほど左方向及び右方向カメラの画角は狭くなる。なお、左側方カメラ２ｌ及び右側方カメラ２ｒは、例えば水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。
In step S4, the ECU 1 sets the left and right image acquisition ranges based on the turning radius ρ calculated in step S1. In the present embodiment, the left and right image acquisition ranges are set to the horizontal angle of view αs (unit: deg) of the left-
次のステップＳ５において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ４にて設定した左方向及び右方向の撮像画像の取得範囲に亘って、左方向又は右方向の何れかの画像（側方撮像画像）を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として選択する画像の撮像方向に応じて、左右一方の側方撮像画像を抽出してもよく、双方の側方撮像画像を抽出してもよい。これにより、ＥＣＵ１は、側方撮像画像群を抽出する。なお、抽出する画像は、図３のように、自車両Ｖの左右横方向から左右にそれぞれ水平画像αs／２の角度θの範囲内で撮像された部分である。
In the next step S5, the ECU 1 performs either the left direction or the right direction over the acquisition range of the left direction and right direction captured images set in step S4 among the captured images captured by the
ステップＳ６において、ＥＣＵ１は、車両センサ群４により検出された撮像画像の遮蔽状態に基づいて、ステップＳ３及びステップＳ５にて取得された撮像画像のそれぞれについて、位置推定用画像尤度及び姿勢角推定用画像尤度を設定する（画像尤度設定手段）。位置推定用画像尤度は、自車両Ｖの位置を推定するための画像として適している度合いを表す。姿勢角推定用画像尤度は、自車両Ｖの姿勢角を推定するための画像として適している度合いを表す。この位置推定用画像尤度及び姿勢角推定用画像尤度は、後述するように位置推定用尤度と姿勢角推定用尤度を求める際に参照される。 In step S6, the ECU 1 estimates the position likelihood image likelihood and the posture angle for each of the captured images acquired in step S3 and step S5 based on the shielding state of the captured image detected by the vehicle sensor group 4. The image likelihood is set (image likelihood setting means). The image likelihood for position estimation represents a degree suitable for an image for estimating the position of the host vehicle V. The posture angle estimation image likelihood represents a degree suitable for an image for estimating the posture angle of the host vehicle V. The position estimation image likelihood and the posture angle estimation image likelihood are referred to when obtaining the position estimation likelihood and the posture angle estimation likelihood, as will be described later.
ＥＣＵ１は、初期設定において、前後撮像画像群のうち前方撮像画像についての姿勢角推定用画像尤度を高めている。また、ＥＣＵ１は、初期設定において、側方撮像画像群についての位置推定用画像尤度を高めている。ここで、初期設定とは、前方撮像画像の遮蔽状態があまりない状態を想定している。また、初期設定において、側方撮像画像は、自車両Ｖの前方よりも側方の方が壁等の構造物で遮蔽されていることを想定している。 In the initial setting, the ECU 1 increases the posture angle estimation image likelihood for the front captured image in the front and rear captured image group. Moreover, ECU1 is raising the image likelihood for position estimation about a side captured image group in the initial setting. Here, the initial setting assumes a state in which there is not much shielding of the front captured image. In the initial setting, it is assumed that the side captured image is shielded by a structure such as a wall on the side rather than the front of the host vehicle V.
自車両Ｖの前方は横方向に比べて見通しが良いために、前方撮像画像は自車両Ｖから遠くを撮像していることが多い。一方、側方撮像画像は自車両Ｖから近くを撮像していることが多い。したがって、移動物***置姿勢推定装置は、上述した図８を参照して説明したように、自車両Ｖから遠くを撮像している前方撮像画像を姿勢角推定用画像として使用するために姿勢角推定用画像尤度を高める。一方、移動物***置姿勢推定装置は、自車両Ｖから近くを撮像している側方撮像画像を位置推定用画像として使用するために位置推定用画像尤度を高める。 Since the front of the host vehicle V has better visibility than in the lateral direction, the forward captured image is often taken far from the host vehicle V. On the other hand, the side-captured image is often taken near the host vehicle V. Therefore, as described with reference to FIG. 8 described above, the moving object position / posture estimation apparatus uses posture angle estimation in order to use a front captured image capturing a distance from the host vehicle V as a posture angle estimation image. Increase image likelihood. On the other hand, the moving object position / posture estimation apparatus increases the position estimation image likelihood in order to use the side captured image capturing the vicinity from the host vehicle V as the position estimation image.
また、ステップＳ６において、ＥＣＵ１は、自車両Ｖが走行する走行車線が規制されている場合には、当該規制された車線に応じて姿勢角推定用画像尤度を設定してもよい。ＥＣＵ１は、車両センサ群４からの出力信号に基づいて、自車両Ｖが進行方向の左側を走行している場合には左側側方画像が遮蔽された状態であることを検出する。また、ＥＣＵ１は、自車両Ｖが進行方向の右側を走行している場合には右側側方画像が遮蔽された状態であることを検出する。 In step S6, when the travel lane in which the host vehicle V travels is restricted, the ECU 1 may set the posture angle estimation image likelihood according to the restricted lane. Based on the output signal from the vehicle sensor group 4, the ECU 1 detects that the left side image is shielded when the host vehicle V is traveling on the left side in the traveling direction. Further, the ECU 1 detects that the right side image is shielded when the host vehicle V is traveling on the right side in the traveling direction.
例えば、左側通行に規制されている場合には自車両Ｖの左側が遠くまで見通せない、遮蔽された状態にあると判断する。ＥＣＵ１は、遮蔽された側方を撮像した側方撮像画像の姿勢角推定用画像尤度を低く設定する。これにより、ＥＣＵ１は、左方向の画像を姿勢角を推定するために選択しにくくする。一方、ＥＣＵ１は、遮蔽された側方を撮像した側方撮像画像の位置推定用画像尤度を高く設定する。これにより、ＥＣＵ１は、左方向の画像を位置を推定するために選択しやすくする。また、ＥＣＵ１は、遮蔽されていない前方を撮像した前方撮像画像の位置推定用画像尤度を低く設定する。これにより、ＥＣＵ１は、前方の画像を位置を推定するために選択しにくくする。一方、ＥＣＵ１は、遮蔽されていない前方を撮像した前方撮像画像の姿勢角推定用画像尤度を高く設定する。これにより、ＥＣＵ１は、前方の画像を姿勢角を推定するために選択しやすくする。このように姿勢角推定用画像尤度及び位置推定用画像尤度を設定する場合、カメラ群２は、最低限でも図４（ａ）に示した前方カメラ２ｆと左側方カメラ２ｌを含む構成であればよい。
For example, when left-hand traffic is restricted, it is determined that the left side of the host vehicle V cannot be seen far and is in a shielded state. The ECU 1 sets the attitude likelihood estimation image likelihood of the side captured image obtained by capturing the shielded side. Thereby, ECU1 makes it difficult to select the image of the left direction in order to estimate the posture angle. On the other hand, ECU1 sets the image likelihood for position estimation of the side captured image which imaged the shielded side side highly. Thereby, ECU1 makes it easy to select the image of the left direction in order to estimate a position. Moreover, ECU1 sets the image likelihood for position estimation of the front captured image which imaged the front which is not shielded low. Thereby, ECU1 makes it difficult to select a front image in order to estimate a position. On the other hand, the ECU 1 sets the posture likelihood estimation image likelihood of the front captured image obtained by capturing the front that is not shielded high. Thereby, ECU1 makes it easy to select a front image in order to estimate a posture angle. When setting the posture angle estimation image likelihood and the position estimation image likelihood in this way, the
例えば、ＥＣＵ１は、側方撮像画像の位置推定用画像尤度を「１」に設定する。また、ＥＣＵ１は、前方撮像画像の姿勢角推定用画像尤度を「１」に設定する。すなわち、尤度が最も高い場合には値を「１」に設定し、尤度が低いほど値を高くする。例えば図７（ａ）のように４方向を撮像するカメラ群２の場合、側方撮像画像以外の位置推定用画像尤度を「２」に設定し、前方撮像画像以外の姿勢角推定用画像尤度を「２」に設定する。
For example, the ECU 1 sets the position estimation image likelihood of the side captured image to “1”. Further, the ECU 1 sets the posture likelihood estimation image likelihood of the forward captured image to “1”. That is, when the likelihood is the highest, the value is set to “1”, and the value is increased as the likelihood is lower. For example, in the case of the
逆に、右側通行に規制されている場合には自車両Ｖの右側が遠くまで見通せない、遮蔽された状態にあると判断する。これにより、ＥＣＵ１は、右方向の画像を姿勢角を推定するために選択しにくくする。一方、ＥＣＵ１は、右方向の画像を位置を推定するために選択しやすくする。このように姿勢角推定用画像尤度及び位置推定用画像尤度を設定する場合、カメラ群２は、最低限でも図４（ｂ）に示した前方カメラ２ｆと右側方カメラ２ｒを含む構成であればよい。
On the other hand, when the right traffic is restricted, it is determined that the right side of the host vehicle V cannot be seen far and is in a shielded state. Thereby, ECU1 makes it difficult to select the image of the right direction in order to estimate the posture angle. On the other hand, the ECU 1 makes it easy to select an image in the right direction for estimating the position. When setting the posture angle estimation image likelihood and the position estimation image likelihood in this way, the
ＥＣＵ１は、ステップＳ３及びステップＳ５にて抽出した撮像画像それぞれについて、その全画素の輝度平均を求める。ＥＣＵ１は、輝度平均値が閾値ＴＨｍａｘ（単位：なし）以上、又は、閾値ＴＨｍｉｎ（単位：なし）以下であれば、当該撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を下げる。ＥＣＵ１は、例えば、姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に設定する。すなわち、撮像画像の輝度平均が極端に大きい場合は、カメラ群２によって太陽光を直接撮像しているといった明るすぎる撮像画像となってしまう。逆に、撮像画像の輝度平均が極端に小さい場合には、カメラ群２によって暗がりを撮像している所謂遮蔽された状態にある撮像画像となる。このような撮像画像は、後述するステップＳ７で行うエッジ抽出などの画像の特徴点を抽出し難い画像であり、投影画像と一致するよう調整することが難しい。このため、自車両Ｖの位置や姿勢角の推定が不安定化する恐れがあるので、姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。例えば、各撮像画像をＥＣＵ１に記録するときの輝度数値は、０〜２５５（０：最も暗い，２５５：最も明るい）の範囲内であり、閾値ＴＨｍａｘには２４０、閾値ＴＨｍｉｎには１５が設定される。
ECU1 calculates | requires the brightness | luminance average of all the pixels about each of the captured image extracted in step S3 and step S5. If the luminance average value is equal to or greater than the threshold value THmax (unit: none) or equal to or less than the threshold value THmin (unit: none), the ECU 1 determines the posture angle estimation image likelihood and the position estimation image likelihood for the captured image. Lower. For example, the ECU 1 sets the posture angle estimation image likelihood and the position estimation image likelihood to “4”. That is, when the average brightness of the captured image is extremely large, the captured image is too bright such that sunlight is directly captured by the
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖから上方に勾配がある方向よりも遠い範囲の撮像画像が遮蔽された状態であることを検出してもよい。このとき、ＥＣＵ１は、３次元地図データベース３を参照して自車両Ｖの周囲の勾配を求める（勾配検出手段）。 Further, in step S <b> 6, the ECU 1 may detect that the captured image in a range farther from the host vehicle V than the direction having a gradient upward is shielded. At this time, the ECU 1 obtains a gradient around the host vehicle V with reference to the three-dimensional map database 3 (gradient detection means).
具体的には、ＥＣＵ１は、３次元地図データベース３と前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、自車両Ｖの周囲において道路又は地平面の勾配が上方に凸となっている方向を抽出する。ＥＣＵ１は、抽出した方向において、当該凸となっている場所以遠は遮蔽されていると判断する。なお、勾配が上方に凸となっていると判断する条件は、自車両Ｖから離れる方向に単位長さ（１ｍ）辺りの勾配変化量が２［ｄｅｇ］以上の場合とする。また、上方に凸となっている場所が、カメラ群２の各カメラから３０［ｍ］以内の距離であれば遠方が見渡せないとして、姿勢角推定用画像尤度を低下させる。例えば、ＥＣＵ１は、姿勢角推定用画像尤度を「５」に設定する。更に、上方に凸となっている場所が、各カメラから５［ｍ］以内の距離であれば近場が撮像できないので、更に姿勢角推定用画像尤度を低下させる。例えば、ＥＣＵ１は、姿勢角推定用画像尤度「７」、位置推定用画像尤度を「５」に設定する。なお、ＥＣＵ１は、自車両Ｖの近傍の勾配が撮像できる場合には、当該撮像画像の位置推定用画像尤度を高めてもよい。
Specifically, the ECU 1 determines that the gradient of the road or the ground plane is convex upward around the host vehicle V from the position and posture angle of the host vehicle V estimated by the three-
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖに対する構造物の位置及び形状によって撮像画像の遮蔽状態を検出してもよい。このとき、ＥＣＵ１は、３次元地図データベース３を参照して自車両Ｖの周囲に存在する構造物の位置及び形状を求める（構造物検出手段）。
Furthermore, in step S6, the ECU 1 may detect the shielding state of the captured image based on the position and shape of the structure relative to the host vehicle V. At this time, the ECU 1 refers to the three-
具体的には、ＥＣＵ１は、自車両Ｖの周囲にある建物や塀などの構造物の位置及び形状を抽出する。ＥＣＵ１は、当該構造物が存在する方向の撮像画像が、当該構造物以遠で遮蔽されているか否かを判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。例えば、ＥＣＵ１は、構造物の位置及び形状と、前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、ステップＳ３及びステップＳ５で抽出した各撮像画像の画素のうち、どの程度の割合で構造物が映りこんでいるかを推定する。例えば、撮像画像のうち５０％以上構造物が映りこんでいると推定された撮像画像は、遠くが見渡せないので、姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「３」に設定する。更に、最も近い構造物までの距離が２０［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「５」に設定する。更に、最も近い構造物までの距離が２［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「７」に設定し、位置推定用画像尤度を「５」に設定する。なお、ＥＣＵ１は、自車両Ｖの近傍の構造物が撮像できる場合には、当該撮像画像の位置推定用画像尤度を高めてもよい。 Specifically, the ECU 1 extracts the position and shape of structures such as buildings and fences around the host vehicle V. The ECU 1 determines whether the captured image in the direction in which the structure exists is shielded beyond the structure, and sets the posture angle estimation image likelihood and the position estimation image likelihood. For example, the ECU 1 determines how much of the pixels of each captured image extracted in step S3 and step S5 from the position and shape of the structure and the position and posture angle of the host vehicle V estimated in the previous position and posture estimation algorithm. It is estimated whether the structure is reflected at the rate of. For example, a captured image that is estimated to include 50% or more of the captured image in the captured image cannot be viewed far away, and thus the posture angle estimation image likelihood is reduced. For example, the posture angle estimation image likelihood is set to “3”. Further, if the distance to the nearest structure is within 20 [m], the posture likelihood estimation image likelihood is further reduced. For example, the posture angle estimation image likelihood is set to “5”. Furthermore, if the distance to the nearest structure is within 2 [m], the posture likelihood estimation image likelihood is further reduced. For example, the posture angle estimation image likelihood is set to “7”, and the position estimation image likelihood is set to “5”. When the structure in the vicinity of the host vehicle V can be imaged, the ECU 1 may increase the image likelihood for position estimation of the captured image.
更に、ステップＳ６において、ＥＣＵ１は、降雨又は降雪がない場合と比べて前方撮像画像が遮蔽されていることを検出してもよい。このとき、ＥＣＵ１は、降雨又は降雪があることを検出する（降雨降雪検出手段）。 Further, in step S6, the ECU 1 may detect that the front captured image is shielded compared to a case where there is no rain or snow. At this time, the ECU 1 detects that there is rain or snow (rain / snow detection means).
具体的には、ＥＣＵ１は、車両センサ群４から、降雨や降雪があることをワイパーの動作状態、又はインターネット等の天気予報を取得する。ＥＣＵ１は、当該車両センサ群４からの情報に基づいて各撮像画像の遮蔽状態を判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。 Specifically, the ECU 1 acquires, from the vehicle sensor group 4, an operating state of the wiper or a weather forecast such as the Internet that there is rainfall or snowfall. The ECU 1 determines the shielding state of each captured image based on information from the vehicle sensor group 4, and sets the posture angle estimation image likelihood and the position estimation image likelihood.
例えば、降雨や降雪が時間当たり１［ｍｍ］を超えた場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くする。例えば、姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に設定する。一方、それ以外の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を高め、例えば、「２」に設定する。更に、降雨や降雪が時間当たり３［ｍｍ］を超えた場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くし、例えば「６」に設定する。一方、それ以外の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に低下させる。更に、自車両Ｖの車速Ｖが１０［ｍ／ｓ］以上となった場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を更に低くして「８」に設定する。 For example, when the rainfall or snowfall exceeds 1 [mm] per hour, the posture likelihood estimation image likelihood for a captured image including a portion capturing within 30 [deg] from the traveling direction of the host vehicle V And the image likelihood for position estimation is lowered. For example, the posture angle estimation image likelihood and the position estimation image likelihood are set to “4”. On the other hand, the posture angle estimation image likelihood and the position estimation image likelihood for other captured images are increased and set to “2”, for example. Further, when the rainfall or snowfall exceeds 3 [mm] per hour, the posture likelihood estimation image likelihood for the captured image including the portion capturing within 30 [deg] from the traveling direction of the host vehicle V And the image likelihood for position estimation is lowered and set to, for example, “6”. On the other hand, the posture angle estimation image likelihood and the position estimation image likelihood for other captured images are reduced to “4”. Further, when the vehicle speed V of the host vehicle V is 10 [m / s] or more, the posture angle estimation for the captured image including a portion capturing within 30 [deg] from the traveling direction of the host vehicle V is performed. The image likelihood and the position estimation image likelihood are further lowered and set to “8”.
更に、ステップＳ６において、ＥＣＵ１は、太陽光によって遮蔽される撮像画像を検出してもよい。このとき、ＥＣＵ１は、現在時刻を検出して太陽の方位角を算出する（太陽位置算出手段）。ＥＣＵ１は、前回の位置姿勢推定アルゴリズムにより推定された自車両Ｖの姿勢角と太陽の方位角とから、太陽光によって遮蔽される撮像画像を検出する。具体的には、ＥＣＵ１は、ステップＳ３及びステップＳ５にて抽出した各撮像画像を撮像した方向内に太陽が存在するか判定する。ＥＣＵ１は、太陽が存在する方向の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させ、例えば「６」に設定する。なお、ＥＣＵ１は、現在時刻だけでなく月日も検出できる時計を用いて太陽の高度を検出し、前回の位置姿勢推定アルゴリズムにて推定した自車両Ｖの位置及び姿勢角を参照して、各撮像画像を撮像した方向内に太陽が存在するか判定するとなお良い。 Further, in step S6, the ECU 1 may detect a captured image shielded by sunlight. At this time, the ECU 1 detects the current time and calculates the azimuth angle of the sun (solar position calculation means). The ECU 1 detects a captured image shielded by sunlight from the posture angle of the host vehicle V and the azimuth angle of the sun estimated by the previous position and orientation estimation algorithm. Specifically, ECU1 determines whether the sun exists in the direction which imaged each captured image extracted in step S3 and step S5. The ECU 1 decreases the posture angle estimation image likelihood and the position estimation image likelihood for the captured image in the direction in which the sun exists, and sets it to “6”, for example. The ECU 1 detects the altitude of the sun using a clock that can detect not only the current time but also the date, and refers to the position and posture angle of the host vehicle V estimated by the previous position and posture estimation algorithm. It is even better to determine whether the sun is present in the direction in which the captured image is captured.
なお、ＥＣＵ１は、例えばインターネット等の天気情報を参照し、天候が雨や曇りの場合には、当該太陽光に応じた姿勢角推定用画像尤度及び位置推定用画像尤度の設定をキャンセルすることが望ましい。また、ＥＣＵ１は、上述したように自車両Ｖの周辺の構造物を検出して、当該構造物によって太陽光が直接カメラ群２に届かない、すなわち遮蔽されていることを検出してもよい。この場合には、太陽光に応じた姿勢角推定用画像尤度及び位置推定用画像尤度の設定をキャンセルするとなお良い。
The ECU 1 refers to weather information such as the Internet, and cancels the setting of the posture angle estimation image likelihood and the position estimation image likelihood according to the sunlight when the weather is rainy or cloudy. It is desirable. Further, the ECU 1 may detect a structure around the host vehicle V as described above and detect that sunlight does not directly reach the
更に、ステップＳ６において、ＥＣＵ１は、日照量が少ない場合には、自車両Ｖに搭載された照明機器によって照射されていない方向の撮像画像が、照明機器により照射されている方向の撮像画像に比べて遮蔽されていると検出してもよい。このとき、ＥＣＵ１は、現在時刻を検出して現在の日照量を推定する（日照量推定手段）。 Further, in step S6, when the amount of sunshine is small, the ECU 1 compares the captured image in the direction not illuminated by the illumination device mounted on the host vehicle V with the captured image in the direction illuminated by the illumination device. May be detected as being shielded. At this time, the ECU 1 detects the current time and estimates the current amount of sunshine (sunshine amount estimation means).
具体的には、ＥＣＵ１は、８時〜１６時までは日照量が十分であり、姿勢角推定用画像尤度及び位置推定用画像尤度を再設定せず、高い値に維持する。一方、７時〜８時及び１６時〜１７時は、それぞれ夜明けと夕方なので日照量が不足する。したがって、ＥＣＵ１は、各撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。このとき、ＥＣＵ１は、自車両Ｖのヘッドライトによる照射が可能な前方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度の低下度合いを抑制して、例えば「３」に設定する。一方、ヘッドライトによる照射が不可能な側方撮像画像及び後方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度は大きく低下させ、例えば「６」に設定する。更に、１７時〜７時の夜間においては、ヘッドライトによる照射が可能な前方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させて、例えば「４」に設定する。一方、ヘッドライトによる照射が不可能な側方撮像画像及び後方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を大きく低下させて、例えば「９」に設定する。 Specifically, the ECU 1 has a sufficient amount of sunshine from 8:00 to 16:00, and does not reset the posture angle estimation image likelihood and the position estimation image likelihood, and maintains them at a high value. On the other hand, from 7 o'clock to 8 o'clock and from 16 o'clock to 17 o'clock, the amount of sunlight is insufficient because it is dawn and evening. Therefore, the ECU 1 reduces the posture angle estimation image likelihood and the position estimation image likelihood for each captured image. At this time, the ECU 1 suppresses the degree of decrease in the posture angle estimation image likelihood and the position estimation image likelihood for the front captured image that can be irradiated by the headlight of the host vehicle V, and sets it to “3”, for example. To do. On the other hand, the posture angle estimation image likelihood and the position estimation image likelihood for the side captured image and the rear captured image that cannot be irradiated by the headlight are greatly reduced and set to, for example, “6”. Further, at night from 17:00 to 7:00, the posture angle estimation image likelihood and the position estimation image likelihood of the front captured image that can be irradiated by the headlight are reduced and set to “4”, for example. . On the other hand, the posture angle estimation image likelihood and the position estimation image likelihood for the side captured image and the rear captured image that cannot be irradiated by the headlight are greatly reduced and set to “9”, for example.
なお、３次元地図データベース３に現在走行している道路に設置された街灯等の照明機器による明るさ情報をあらかじめ記録しておき、ＥＣＵ１は、当該３次元地図データベース３を参照して自車両Ｖ周囲の明るさに応じて姿勢角推定用画像尤度及び位置推定用画像尤度を設定するとなお良い。
In addition, brightness information by lighting devices such as street lamps installed on the currently running road is recorded in advance in the three-
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖ周辺の障害物に基づいて、当該障害物によって前方又は側方の撮像方向が遮られているかを判定してもよい。このために、ＥＣＵ１は、自車両Ｖの周辺に存在する障害物を検出する（障害物検出手段）。例えば、カメラ群２を用いたモーションステレオ又は自車両Ｖに別途レーザレンジファインダ等を搭載することによって、自車両Ｖの周囲にある他車両や歩行者等の障害物の位置及び形状を抽出する。ＥＣＵ１は、当該障害物が存在する方向の撮像画像が、当該障害物以遠で遮蔽されているか否かを判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。
Furthermore, in step S6, the ECU 1 may determine whether the front or side imaging direction is obstructed by the obstacle based on the obstacle around the host vehicle V. For this purpose, the ECU 1 detects an obstacle existing around the host vehicle V (obstacle detection means). For example, the position and shape of obstacles such as other vehicles and pedestrians around the own vehicle V are extracted by mounting a laser range finder or the like separately on the motion stereo using the
具体的には、自車両Ｖ周辺の障害物の位置及び形状と、前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、ステップＳ３及びステップＳ５で抽出した各撮像画像の画素のうち、どの程度の割合で障害物が映りこんでいるかを推定する。例えば、撮像画像のうち５０％以上障害物が映りこんでいると推定された撮像画像は、遠くが見渡せないので、姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「３」に設定する。更に、最も近い障害物までの距離が２０［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「５」に設定する。更に、最も近い障害物までの距離が２［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「７」に設定し、位置推定用画像尤度を「５」に設定する。 Specifically, the pixel of each captured image extracted in step S3 and step S5 from the position and shape of the obstacle around the host vehicle V and the position and posture angle of the host vehicle V estimated by the previous position and orientation estimation algorithm. It is estimated how much of the obstacles are reflected. For example, a captured image that is estimated to include 50% or more obstacles in the captured image cannot be viewed far away, and thus the posture angle estimation image likelihood is reduced. For example, the posture angle estimation image likelihood is set to “3”. Furthermore, if the distance to the nearest obstacle is a distance within 20 [m], the image likelihood for posture angle estimation is further reduced. For example, the posture angle estimation image likelihood is set to “5”. Furthermore, if the distance to the nearest obstacle is within 2 [m], the posture likelihood estimation image likelihood is further reduced. For example, the posture angle estimation image likelihood is set to “7”, and the position estimation image likelihood is set to “5”.
次のステップＳ７において、ＥＣＵ１は、ステップＳ６にて位置推定用画像尤度及び姿勢角推定用画像尤度が設定された撮像画像のうち、所定値を超える（又は所定値以上）位置推定用画像尤度又は姿勢角推定用画像尤度の撮像画像からエッジ画像を作成する。この所定値は、適切な撮像画像を使用して位置及び姿勢角を推定するための任意に設定した値であってもよい。これにより、ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐを作成できる。 In the next step S7, the ECU 1 is a position estimation image that exceeds a predetermined value (or more than a predetermined value) among the captured images in which the position estimation image likelihood and the posture angle estimation image likelihood are set in step S6. An edge image is created from a captured image of likelihood or posture angle estimation image likelihood. This predetermined value may be an arbitrarily set value for estimating the position and the posture angle using an appropriate captured image. Thereby, the ECU 1 can create the posture angle estimation edge image Ea and the position estimation edge image Ep.
例えば初期設定においては、前方撮像画像についての姿勢角推定用画像尤度を高めており、側方撮像画像群についての位置推定用画像尤度を高めているので、前方撮像画像から姿勢推定用エッジ画像Ｅａを作成し、側方撮像画像から位置推定用エッジ画像Ｅｐを作成する。他の例を挙げると、自車両Ｖが左側通行の道路を走行している場合には、左側方撮像画像の位置推定用画像尤度が高く設定され、前方撮像画像の姿勢角推定用画像尤度が高く設定されている。この場合、ＥＣＵ１は、左側方撮像画像から位置推定用エッジ画像Ｅｐを作成し、前方撮像画像から姿勢推定用エッジ画像Ｅａを作成する。 For example, in the initial setting, the posture angle estimation image likelihood for the front captured image is increased, and the position estimation image likelihood for the side captured image group is increased. An image Ea is created, and a position estimation edge image Ep is created from the side captured image. As another example, when the host vehicle V is traveling on a left-handed road, the position estimation image likelihood of the left side captured image is set high, and the posture angle estimation image likelihood of the front captured image is set. The degree is set high. In this case, the ECU 1 creates a position estimation edge image Ep from the left-side captured image, and creates a posture estimation edge image Ea from the front captured image.
なお、このステップＳ７においては、上述した勾配、構造物、降雨又は降雪、又は、太陽光の有無や、日射量、障害物の有無に応じて設定された姿勢角推定用画像尤度及び位置推定用画像尤度に応じて、位置及び姿勢角の推定に適切な方向を撮像した撮像画像からエッジ画像を作成することが望ましい。 In step S7, the posture angle estimation image likelihood and the position estimation set according to the above-described gradient, structure, rain or snow, or the presence or absence of sunlight, the amount of solar radiation, and the presence or absence of an obstacle. It is desirable to create an edge image from a captured image obtained by capturing an appropriate direction for estimating the position and posture angle according to the image likelihood.
本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。また、ＥＣＵ１は、カメラ群２の撮像画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップＳ５及びステップＳ６において、３次元地図データベース３にも記録しておいた、これらエッジ以外の情報も用いて位置推定用尤度、姿勢角推定用尤度を設定して、自車両の位置及び姿勢角を推定してもよい。
An edge in the present embodiment refers to a location where the luminance of a pixel changes sharply. For example, the Canny method can be used as the edge detection method. The edge detection method is not limited to this, and various other methods such as differential edge detection may be used. Further, it is desirable that the ECU 1 extracts the brightness change direction of the edge, the color near the edge, and the like from the captured image of the
ステップＳ１の次のステップＳ８において、ＥＣＵ１は、１ループ前のステップＳ６にて推定された車両位置から、今回のステップＳ１で算出したオドメトリ分だけ移動させる。ＥＣＵ１は、移動させた車両位置の近傍で、複数の仮想（予測）位置及び仮想（予測）姿勢角の候補を算出する。ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、ＥＣＵ１は前回の車両位置情報を持っていない。このため、ＥＣＵ１は、車両センサ群４に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。又は、ＥＣＵ１は、前回に停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報にしてもよい。
In step S8 following step S1, the ECU 1 moves the odometry calculated in step S1 from the vehicle position estimated in step S6 one loop before. The ECU 1 calculates a plurality of virtual (predicted) position and virtual (predicted) attitude angle candidates in the vicinity of the moved vehicle position. However, in the case of the first loop after starting the position / orientation estimation algorithm, the ECU 1 does not have the previous vehicle position information. Therefore, the ECU 1 uses the data from the
ＥＣＵ１は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置や姿勢角の真値が取り得る可能性がある複数の仮想位置及び仮想姿勢角の候補を複数個生成する。この仮想位置及び仮想姿勢角の候補は、位置及び姿勢角の６自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。 The ECU 1 takes into account measurement errors of the vehicle sensor group 4, odometry errors caused by communication delays, and vehicle dynamic characteristics that cannot be taken into account by the odometry, and there is a possibility that the true value of the vehicle position and attitude angle can be obtained A plurality of virtual position and virtual attitude angle candidates are generated. The candidates for the virtual position and the virtual attitude angle set the upper and lower limits of the error with respect to the 6-degree-of-freedom parameters of the position and the attitude angle, and randomly use a random number table or the like within the upper and lower limits of the error. Set.
なお、本実施形態では、仮想位置及び仮想姿勢角の候補を５００個作成する。また、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。この仮想位置及び仮想姿勢角の候補を作成する数や、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、自車両Ｖの運転状態や路面の状況を検出又は推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向（前後方向，横方向，ヨー）の誤差が大きくなる可能性が高いので、この３つのパラメータの誤差の上下限を大きくし、且つ仮想位置及び仮想姿勢角の候補の作成数を増やすことが望ましい。 In the present embodiment, 500 candidates for the virtual position and the virtual posture angle are created. The upper and lower limits of the error for the 6-degree-of-freedom parameters of position and posture angle are ± 0.05 [m], ± 0.05 [m], ± 0.05 [0.05] in the order of front / rear direction, lateral direction, vertical direction, roll, pitch, yaw. m], ± 0.5 [deg], ± 0.5 [deg], ± 0.5 [deg]. The number of virtual position and virtual attitude angle candidates to be created and the upper and lower limits of the error with respect to the 6-degree-of-freedom parameters of position and attitude angle are determined by detecting or estimating the driving state of the host vehicle V and the road surface condition. It is desirable to change. For example, when a sudden turn or slip occurs, the error in the plane direction (front / rear direction, lateral direction, yaw) is likely to increase, so the upper and lower limits of the error of these three parameters are increased. In addition, it is desirable to increase the number of virtual position and virtual attitude angle candidates created.
次のステップＳ９において、ＥＣＵ１は、ステップＳ８で作成した全ての仮想位置及び仮想姿勢角の候補のそれぞれについて仮想（投影）画像を作成する。このとき、ＥＣＵ１は、例えば３次元地図データベース３に記憶されたエッジ等の３次元位置情報を、仮想位置及び仮想姿勢角から撮像した撮像画像となるよう投影変換して、仮想画像を作成する。この仮想画像は、各仮想位置及び仮想姿勢角の候補が実際の自車両Ｖの位置及び姿勢角と合致しているかを評価する評価用画像である。投影変換では、カメラ群２の位置を示す外部パラメータと、カメラ２の内部パラメータが必要となる。外部パラメータは、車両位置（例えば中心位置）からカメラ群２までの相対位置を予め計測しておくことで、仮想位置及び仮想姿勢角の候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。
In the next step S9, the ECU 1 creates a virtual (projection) image for each of all the virtual positions and virtual attitude angle candidates created in step S8. At this time, ECU 1, for example a three-dimensional position information, such as stored edge in the three-
ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補について、自車両Ｖに搭載されたカメラ群２から撮像した撮像画像となるような仮想画像を作成する。例えば図４（ａ）に示すようにカメラ群２が前方カメラ２ｆ及び左側方カメラ２ｌを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像及び仮想左側方画像を作成する。また、図７（ａ）に示すようにカメラ群２が前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ及び後方カメラ２ｂを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像、仮想右側方画像、仮想左側方画像及び仮想後方画像を作成する。これにより、ＥＣＵ１は、姿勢推定用エッジ画像Ｅａの撮像方向と同じ方向に投影した姿勢推定用仮想画像Ａａを作成し、位置推定用エッジ画像Ｅｐの撮像方向と同じ方向に投影した位置推定用仮想画像Ａｐを作成する。なお、この仮想画像は、ステップＳ２及びステップＳ４で設定されたαf、αsの画角の範囲のみ算出すればよい。
The ECU 1 creates a virtual image that becomes a captured image captured from the
なお、ステップＳ１においてカメラ２により撮像した撮像画像からエッジの輝度変化の方向や、エッジ近辺のカラーなどについても撮像画像から抽出できる場合には、それらについても３次元位置情報を３次元地図データベース３に記録しておき、仮想画像を作成するとなお良い。
Incidentally, and direction of change in luminance of the edge from the captured image captured by the
ステップＳ７及びステップＳ９の処理の後に、ステップＳ１０を行う。ステップＳ１０において、ＥＣＵ１は、ステップＳ８にて設定した全ての仮想位置及び仮想姿勢角の候補それぞれについて、ステップＳ７にて作成したエッジ画像Ｅａ，Ｅｐと、エッジ画像の撮像方向と同じステップＳ９にて作成した仮想画像Ａａ，Ａｐとをそれぞれ比較する。ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとの一致度に応じた値の姿勢角推定用尤度（単位：なし）を設定する。また、ＥＣＵ１は、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとの一致度に応じた値の位置推定用尤度（単位：なし）を設定する。これにより、ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補点について、姿勢角推定用尤度及び位置推定用尤度を設定できる。 Step S10 is performed after the process of step S7 and step S9. In step S10, the ECU 1 performs the edge image Ea, Ep created in step S7 and the imaging direction of the edge image for each of the virtual position and virtual attitude angle candidates set in step S8, in the same step S9. The created virtual images Aa and Ap are respectively compared. The ECU 1 sets a posture angle estimation likelihood (unit: none) having a value corresponding to the degree of coincidence between the posture angle estimation edge image Ea and the posture estimation virtual image Aa. Further, the ECU 1 sets a position estimation likelihood (unit: none) having a value corresponding to the degree of coincidence between the position estimation edge image Ep and the position estimation virtual image Ap. Thus, the ECU 1 can set the posture angle estimation likelihood and the position estimation likelihood for each virtual position and virtual posture angle candidate point.
具体的には、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素がある場合には、姿勢角推定用尤度及び位置推定用尤度を加算する。逆に、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素が無い場合には、姿勢角推定用尤度及び位置推定用尤度を加算しない。これにより、各仮想位置及び仮想姿勢角の候補点の撮像方向ごとに、姿勢角推定用尤度及び位置推定用尤度を設定できる。 Specifically, when there are pixels of the posture angle estimation edge image Ea and the position estimation edge image Ep at the pixel position in the virtual image, the posture angle estimation likelihood and the position estimation likelihood are added. . Conversely, when there is no pixel of the posture angle estimation edge image Ea and the position estimation edge image Ep at the pixel position in the virtual image, the posture angle estimation likelihood and the position estimation likelihood are not added. Thereby, the posture angle estimation likelihood and the position estimation likelihood can be set for each imaging direction of each virtual position and virtual posture angle candidate point.
また、姿勢推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に１．０を加算し、位置推定用尤度に０．２を加算してもよい。一方、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に０．２を加算し、位置推定用尤度に１．０を加算してもよい。 In addition, when the posture estimation edge image Ea and the posture estimation virtual image Aa are compared, and there is a pixel matching the same position, 1.0 is added to the posture estimation likelihood and the position estimation likelihood is increased. You may add 0.2 to the degree. On the other hand, when the position estimation edge image Ep and the position estimation virtual image Ap are compared and there is a pixel that matches the same position, 0.2 is added to the posture estimation likelihood, and the position estimation likelihood is increased. You may add 1.0 to the degree.
次のステップＳ１１において、ＥＣＵ１は、ステップＳ１０にて位置推定用尤度及び姿勢角推定用尤度が求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両Ｖの位置及び姿勢角を算出する。ＥＣＵ１は、仮想画像の姿勢角推定用尤度に基づいて当該仮想画像の仮想姿勢角に対する実際の自車両Ｖの姿勢角を推定する（移動物***置姿勢推定手段）。ＥＣＵ１は、仮想画像の位置推定用尤度に基づいて当該仮想画像の仮想位置に対する実際の自車両Ｖの位置を推定する（移動物***置姿勢推定手段）。 In the next step S11, the ECU 1 uses the plurality of virtual position and virtual attitude angle candidates for which the position estimation likelihood and the attitude angle estimation likelihood are obtained in step S10, to determine the final vehicle V. The position and posture angle are calculated. The ECU 1 estimates the actual posture angle of the host vehicle V with respect to the virtual posture angle of the virtual image based on the likelihood for posture angle estimation of the virtual image (moving object position and posture estimation means). The ECU 1 estimates the actual position of the host vehicle V with respect to the virtual position of the virtual image based on the position estimation likelihood of the virtual image (moving object position / posture estimation means).
このとき、ＥＣＵ１は、例えば、位置推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、姿勢角推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、位置推定用尤度と姿勢角推定用尤度との和が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の位置推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の姿勢角推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、各仮想画像の位置推定用尤度と姿勢角推定用尤度の和に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。 At this time, the ECU 1 calculates the virtual position and the virtual attitude angle as the actual position and attitude angle of the host vehicle V using, for example, the virtual position and virtual attitude angle candidate having the highest position estimation likelihood. May be. Alternatively, the ECU 1 may calculate the virtual position and the virtual posture angle as the actual position and posture angle of the host vehicle V using the virtual position and virtual posture angle candidate having the highest posture angle estimation likelihood. Good. Alternatively, the ECU 1 uses the virtual position and the virtual attitude angle candidate having the highest sum of the position estimation likelihood and the attitude angle estimation likelihood, and uses the virtual position and the virtual attitude angle as the actual position of the host vehicle V. You may calculate as a position and a posture angle. Alternatively, the ECU 1 calculates the actual position and attitude angle of the host vehicle V by taking a weighted average according to the position estimation likelihood of each virtual image for a plurality of virtual position and virtual attitude angle candidates. May be. Alternatively, the ECU 1 calculates the actual position and posture angle of the host vehicle V by taking a weighted average according to the posture angle estimation likelihood of each virtual image for a plurality of virtual position and virtual posture angle candidates. May be. Alternatively, the ECU 1 may calculate the actual position and posture angle of the host vehicle V by taking a weighted average corresponding to the sum of the position estimation likelihood and the posture angle estimation likelihood of each virtual image.
ＥＣＵ１は、以上のようなステップＳ１乃至ステップＳ１１を繰り返して行うことによって、逐次、自車両Ｖの位置と姿勢角を算出できる。 The ECU 1 can sequentially calculate the position and posture angle of the host vehicle V by repeatedly performing the above steps S1 to S11.
以上詳細に説明したように、本実施形態として示した移動物***置姿勢推定装置によれば、撮像方向が遮蔽されている遮蔽状態に応じて、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。これにより、移動物***置姿勢推定装置は、遮蔽状態に基づいて最適な撮像画像と当該撮像方向と同じ方向に仮想的に投影した仮想画像とを比較して、実際の自車両Ｖの位置及び姿勢角を推定できる。したがって、移動物***置姿勢推定装置は、精度良く自車両Ｖの位置及び姿勢角を推定することができる。 As described above in detail, according to the moving object position / posture estimation apparatus shown as the present embodiment, the posture angle estimation image likelihood and the position estimation image likelihood according to the shielding state in which the imaging direction is shielded. Set the degree. Accordingly, the moving object position / posture estimation apparatus compares the optimum captured image based on the shielding state with the virtual image virtually projected in the same direction as the imaging direction, and compares the actual position and posture of the host vehicle V with each other. The angle can be estimated. Therefore, the moving object position / posture estimation apparatus can accurately estimate the position and posture angle of the host vehicle V.
特に、移動物***置姿勢推定装置は、前方撮像画像の姿勢角推定用画像尤度を高めるので、側方に比べて遮蔽されずに遠くまで撮像した前方撮像画像を用いて自車両Ｖの姿勢を推定できる。また、移動物***置姿勢推定装置は、側方撮像画像の位置推定用画像尤度を高めるので、前方に比べて遮蔽されて近くを撮像した側方撮像画像を用いて自車両Ｖの位置を推定できる。 In particular, since the moving object position / posture estimation apparatus increases the posture likelihood estimation image likelihood of the front captured image, the posture of the host vehicle V is determined using the front captured image captured far away without being shielded compared to the side. Can be estimated. In addition, the moving object position / posture estimation apparatus increases the image likelihood for position estimation of the side captured image, so the position of the host vehicle V is estimated using the side captured image that is shielded and imaged near the front. it can.
２つの映像が一致した場所が自車両Ｖから遠い場合には位置の誤差が大きい場合があり、逆に、２つの映像が一致した位置が自車両Ｖから近い場合には姿勢角の誤差が大きい場合があるが、この移動物***置姿勢推定装置によれば、精度良く移動物体の位置及び姿勢角を推定することができる。 When the place where the two images match is far from the host vehicle V, the position error may be large. Conversely, when the position where the two images match is close to the host vehicle V, the posture angle error is large. In some cases, the moving object position / posture estimation apparatus can accurately estimate the position and the posture angle of the moving object.
また、この移動物***置姿勢推定装置によれば、自車両Ｖの位置及び姿勢角を求めるために不要な撮像画像については仮想画像と一致する処理を省くことができるため、演算負荷の低減が実現できる。 Further, according to this moving object position / posture estimation apparatus, it is possible to omit processing that matches a virtual image for a captured image that is unnecessary for obtaining the position and posture angle of the host vehicle V, thereby realizing a reduction in calculation load. it can.
また、この移動物***置姿勢推定装置によれば、自車両Ｖが進行方向の左側を走行している場合には左側側方画像が遮蔽された状態であり、自車両Ｖが進行方向の右側を走行している場合には右側側方画像が遮蔽された状態であることを検出できる。これにより、この移動物***置姿勢推定装置によれば、遮蔽された左側方又は右側方の撮像画像を姿勢角を推定するのに適切な撮像画像とは選択しないようにすることができる。また、移動物***置姿勢推定装置は、自車両Ｖの前方に比べて側方は近くを撮像できることを利用して位置推定用画像尤度を高めることもできる。これにより、移動物***置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置及び姿勢角の推定精度を向上させることができる。
更に、この移動物***置姿勢推定装置によれば、自車両Ｖから上方に勾配がある方向よりも遠い範囲の撮像画像が遮蔽された状態であることを検出できる。これにより、例えば坂道の頂上手前では前方が遮蔽され、頂上を越えたら後方が遮蔽された状態にあることが判断できる。これにより、ＥＣＵ１は、遮蔽された方向の撮像画像を使用して自車両Ｖの位置及び姿勢角を推定しないよう姿勢角推定用画像尤度及び位置推定用画像尤度を低下できる。又は、ＥＣＵ１は、遮蔽された状態によっては構造物が撮像できる状況であり、この場合には、自車両Ｖの位置を推定しやすくするので位置推定用画像尤度を高めることができる。これにより、移動物***置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置及び姿勢角の推定精度を向上させることができる。
Further, according to this moving object position and orientation estimation device, when the host vehicle V is traveling on the left side in the traveling direction, the left side image is shielded, and the host vehicle V is positioned on the right side in the traveling direction. When traveling, it can be detected that the right side image is shielded. Thus, according to the moving object position / posture estimation apparatus, it is possible to prevent the shielded left-side or right-side captured image from being selected as an appropriate captured image for estimating the posture angle. Further, the moving object position / posture estimation apparatus can also increase the image likelihood for position estimation by utilizing the fact that the side can image nearer than the front of the host vehicle V. As a result, the moving object position / posture estimation apparatus can set appropriate posture angle estimation image likelihood and position estimation image likelihood to improve the accuracy of position and posture angle estimation.
Furthermore, according to this moving object position / posture estimation apparatus, it is possible to detect that a captured image in a range farther than a direction having a gradient upward from the host vehicle V is shielded. Thereby, for example, it can be determined that the front is shielded before the top of the hill and the rear is shielded when the top is exceeded. Thereby, the ECU 1 can reduce the posture angle estimation image likelihood and the position estimation image likelihood so as not to estimate the position and posture angle of the host vehicle V using the captured image in the shielded direction. Alternatively, the ECU 1 is in a situation where a structure can be imaged depending on the shielded state. In this case, the position of the host vehicle V can be easily estimated, so that the position estimation image likelihood can be increased. As a result, the moving object position / posture estimation apparatus can set appropriate posture angle estimation image likelihood and position estimation image likelihood to improve the accuracy of position and posture angle estimation.
更に、この移動物***置姿勢推定装置によれば、自車両Ｖに対する構造物の位置及び形状によって撮像画像の遮蔽状態を検出できる。この移動物***置姿勢推定装置によれば、例えばトンネルや建物などの構造物情報を参照して遮蔽状態を判断するので、構造物が遠くあり遮蔽されていない場合には、当該方向の撮像画像の姿勢角推定用画像尤度を高めて、当該撮像画像を姿勢角の推定に使用しやすくすることができる。また、構造物が近くにあって遮蔽されている場合には、当該方向の撮像画像を位置及び姿勢角の推定に使用しないよう位置推定用画像尤度及び姿勢角推定用画像尤度を低下させることができる。更に、自車両Ｖの近くの構造物と投影画像が一致させることができる場合には、当該方向の撮像画像により位置の推定ができるよう位置推定用画像尤度を高めることができる。 Furthermore, according to this moving object position / posture estimation apparatus, the shielding state of the captured image can be detected based on the position and shape of the structure relative to the host vehicle V. According to this moving object position / orientation estimation apparatus, for example, the shielding state is determined with reference to structure information such as a tunnel or a building. Therefore, when the structure is far away and is not shielded, the captured image of the direction is displayed. By increasing the posture angle estimation image likelihood, the captured image can be easily used for posture angle estimation. Further, when the structure is close and shielded, the position estimation image likelihood and the posture angle estimation image likelihood are reduced so that the captured image in the direction is not used for the position and posture angle estimation. be able to. Further, when the structure near the host vehicle V and the projected image can be matched, the position estimation image likelihood can be increased so that the position can be estimated from the captured image in the direction.
更に、この移動物***置姿勢推定装置によれば、降雨又は降雪がある場合には、降雨又は降雪がない場合と比べて前方撮像画像が遮蔽されていることを検出できる。この移動物***置姿勢推定装置によれば、降雨や降雪によって遮蔽されている場合には、当該降雨や降雪による視界悪化が著しい移動方向を撮像した撮像画像の姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。これにより、移動物***置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。 Furthermore, according to this moving object position / posture estimation apparatus, it is possible to detect that the front captured image is shielded when there is rain or snow compared to when there is no rain or snow. According to this moving object position / posture estimation apparatus, when it is blocked by rain or snow, the image likelihood and position estimation for posture angle estimation of a captured image obtained by capturing a moving direction in which the visibility is greatly deteriorated due to the rain or snow The image likelihood is reduced. Thereby, the moving object position / posture estimation apparatus can set the appropriate posture angle estimation image likelihood and the position estimation image likelihood, and improve the estimation accuracy of the position and posture angle.
更に、この移動物***置姿勢推定装置によれば、自車両Ｖの姿勢角と、太陽の方位角とから、太陽光によって遮蔽される撮像画像を検出できる。この移動物***置姿勢推定装置によれば、カメラ群２のうちの特定のカメラが太陽を含む範囲を撮像している場合には太陽光の影響によって撮像し難いので、当該方向の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くする。これにより、移動物***置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
更に、この移動物***置姿勢推定装置によれば、日照量が少ない場合には照明機器によって照射されていない方向の撮像画像が、照明機器により照射されている方向の撮像画像に比べて遮蔽されていると検出できる。この移動物***置姿勢推定装置によれば、日照量が高い場合には姿勢角推定用画像尤度及び位置推定用画像尤度を高めることができる。また、日照量が少ない場合には撮像画像が遮蔽されており、姿勢角推定用画像尤度及び位置推定用画像尤度を低下させることができる。また、日照量が少ないがヘッドライト等で照射される前方撮像画像が遮蔽されていないとして適切な姿勢角推定用画像尤度を設定することができる。これにより移動物***置姿勢推定装置によれば、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
Furthermore, according to the moving object position / posture estimation apparatus, a captured image shielded by sunlight can be detected from the posture angle of the host vehicle V and the azimuth angle of the sun. According to this moving object position / posture estimation apparatus, when a specific camera in the
Further, according to this moving object position / posture estimation apparatus, when the amount of sunlight is small, a captured image in a direction not illuminated by the illumination device is shielded compared to a captured image in the direction illuminated by the illumination device. Can be detected. According to this moving object position / posture estimation apparatus, when the amount of sunlight is high, the posture angle estimation image likelihood and the position estimation image likelihood can be increased. Further, when the amount of sunshine is small, the captured image is shielded, and the posture angle estimation image likelihood and the position estimation image likelihood can be reduced. Further, it is possible to set an appropriate posture angle estimation image likelihood assuming that the front captured image irradiated with a headlight or the like is not blocked although the amount of sunlight is small. Thereby, according to the moving object position / posture estimation apparatus, it is possible to set appropriate posture angle estimation image likelihood and position estimation image likelihood to improve the estimation accuracy of the position and posture angle.
更に、この移動物***置姿勢推定装置によれば、自車両Ｖの周囲の障害物を検出して、当該障害物が存在する方向の撮像画像が当該障害物以遠が遮蔽されていると判定できる。例えば、カメラ群２の視界は、大きな障害物であっても自車両Ｖから遠くにあれば遮蔽されないが、小さな障害物であっても自車両Ｖの間近にあれば遮蔽されてしまう。したがって、この移動物***置姿勢推定装置は、障害物が遠くにあって撮像画像が遮蔽されていない場合には当該撮像画像についての姿勢角推定用画像尤度を高めることができる。また、移動物***置姿勢推定装置は、障害物が近くにあって撮像画像が遮蔽されている場合には当該撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させることができる。これにより移動物***置姿勢推定装置によれば、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
Furthermore, according to this moving object position / posture estimation apparatus, it is possible to detect an obstacle around the host vehicle V and determine that the captured image in the direction in which the obstacle exists is shielded from the obstacle. For example, the field of view of the
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。 The above-described embodiment is an example of the present invention. For this reason, the present invention is not limited to the above-described embodiment, and various modifications can be made depending on the design and the like as long as the technical idea according to the present invention is not deviated from this embodiment. Of course, it is possible to change.
上述した実施形態では車両を例にしたが、少なくとも１台以上のカメラを搭載した移動体であれば、航空機や船舶などにも適用可能である。 In the above-described embodiment, a vehicle is taken as an example. However, any vehicle that has at least one camera mounted thereon can be applied to an aircraft, a ship, and the like.
また、上述した実施形態では車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めているが、これに限らない。例えば、サスペンション等を有さない、工場などで用いられる無人搬送車などのように３自由度での位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合にも、本実施形態が適用可能である。具体的には、このような車両であれば、上下方向の位置と、ロールおよびピッチといった姿勢角は固定であるので、予めこれらのパラメータを計測しておいたり、３次元地図データベース３を参照して求めるようにすればよい。
In the above-described embodiment, the position (front-rear direction, lateral direction, vertical direction) and posture angle (roll, pitch, yaw) of the six degrees of freedom of the vehicle are obtained, but the present invention is not limited to this. For example, this embodiment is also used when estimating a position (front-rear direction, lateral direction) and posture angle (yaw) with three degrees of freedom, such as an automatic guided vehicle used in a factory without a suspension or the like. Is applicable. Specifically, in such a vehicle, the vertical position and the posture angle such as roll and pitch are fixed. Therefore, these parameters are measured in advance or the
１ ＥＣＵ
２ カメラ群
２ａ 全方位カメラ
２ｂ 後方カメラ
２ｆ 前方カメラ
２ｌ 左側方カメラ
２ｒ 右側方カメラ
３ ３次元地図データベース
４ 車両センサ群
２１ 撮像範囲
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ その他センサ
1 ECU
2
Claims (10)
移動物体の前方を撮像して前方撮像画像を取得する前方撮像手段と、
移動物体の右側又は左側を撮像して側方撮像画像を取得する側方撮像手段と、
３次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、３次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得する仮想画像取得手段と、
撮像方向が遮蔽されている状態を表す遮蔽状態を検出する遮蔽状態検出手段と、
前記遮蔽状態検出手段により検出された遮蔽状態に基づいて、前記前方撮像手段により取得された前方撮像画像及び前記側方撮像手段により取得された側方撮像画像より、移動物体の位置を推定するために適した撮像画像及び移動物体の姿勢角を推定するために適した撮像画像を選択し、
前記移動物体の位置を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の位置を推定すると共に、前記移動物体の姿勢角を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定する移動物***置姿勢推定手段と
を有することを特徴とする移動物***置姿勢推定装置。 A moving object position and orientation estimation apparatus for estimating a position and an attitude angle of a moving object,
Forward imaging means for capturing the front of the moving object and obtaining a forward captured image;
A side imaging means for capturing a right side or a left side of a moving object to obtain a side captured image;
The three- dimensional map data is converted into an image virtually captured by the front imaging means to obtain a virtual front image, and the three- dimensional map data is converted into an image virtually captured by the side imaging means to be virtual. Virtual image acquisition means for acquiring side images;
Shielding state detecting means for detecting a shielding state representing a state where the imaging direction is shielded;
Based on the shielding state detected by the shielding state detection unit, the position of the moving object is estimated from the front captured image acquired by the front imaging unit and the side captured image acquired by the side imaging unit. select the captured image suitable for estimating the attitude angle of the captured image and the moving object which is suitable for,
Based on the degree of coincidence between a captured image suitable for estimating the position of the moving object and a virtual image projected in the same direction as the captured image, the position of the actual moving object is estimated , and A moving object position / posture estimation means for estimating a posture angle of an actual moving object based on a degree of coincidence between a captured image suitable for estimating the posture angle and a virtual image projected in the same direction as the captured image; A moving object position / posture estimation apparatus characterized by comprising:
前記遮蔽状態検出手段により検出された遮蔽状態に基づいて、
前記前方撮像手段により取得された前方撮像画像及び前記側方撮像手段により取得された側方撮像画像のそれぞれについて、移動物体の位置を推定するための画像として適している度合いを表す位置推定用画像尤度と、移動物体の姿勢角を推定するための画像として適している度合いを表す姿勢角推定用画像尤度とを設定するものであり、前記前方撮像画像の姿勢角推定用画像尤度を高め、前記側方撮像画像の位置推定用画像尤度を高める画像尤度設定手段を有し、
前記移動物***置姿勢推定手段は、前記前方撮像画像及び前記側方撮像画像のうち、前記画像尤度設定手段により設定された位置推定用画像尤度が所定値以上の撮像画像を移動物体の位置を推定するために適した撮像画像として選択し、
前記前方撮像画像及び側方撮像画像のうち、前記画像尤度設定手段により設定された姿勢角推定用画像尤度が所定値以上の撮像画像を前記移動物体の姿勢角を推定するために適した撮像画像として選択すること
を特徴とする請求項１に記載の移動物***置姿勢推定装置。 The moving object position / posture estimation means includes:
Based on the shielding state detected by the shielding state detection means,
A position estimation image representing the degree of suitability as an image for estimating the position of the moving object for each of the front captured image acquired by the front imaging unit and the side captured image acquired by the side imaging unit. A likelihood and an image likelihood for posture angle estimation representing a degree suitable for an image for estimating the posture angle of the moving object, and the image likelihood for posture angle estimation of the forward captured image is set. Image likelihood setting means for increasing and increasing the image likelihood for position estimation of the side captured image,
The moving object position / orientation estimation unit is configured to select a captured image having a position estimation image likelihood set by the image likelihood setting unit of the front captured image and the side captured image having a predetermined value or more as a moving object position. Is selected as a captured image suitable for estimating
Of the forward captured image and the lateral captured image, the captured image having the posture angle estimation image likelihood set by the image likelihood setting means is suitable for estimating the posture angle of the moving object. The moving object position / posture estimation apparatus according to claim 1, wherein the moving object position / posture estimation apparatus is selected as a captured image .
前記移動物体の前方を撮像して前方撮像画像を取得し、右側又は左側を撮像して側方撮像画像を取得するステップと、Capturing the front of the moving object to acquire a front captured image, capturing the right or left side to acquire a side captured image;
３次元地図データから仮想前方画像、及び仮想側方画像を取得するステップと、Obtaining a virtual front image and a virtual side image from the three-dimensional map data;
撮像方向が遮蔽されている状態を表す遮蔽状態を検出するステップと、Detecting a shielding state representing a state where the imaging direction is shielded;
前記遮蔽状態に基づいて、前記前方撮像画像及び前記側方撮像画像より、前記移動物体の位置を推定するために適した撮像画像及び移動物体の姿勢角を推定するために適した撮像画像を選択し、Based on the shielding state, a captured image suitable for estimating the position of the moving object and a captured image suitable for estimating the attitude angle of the moving object are selected from the front captured image and the side captured image. And
前記移動物体の位置を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の位置を推定すると共に、前記移動物体の姿勢角を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定するステップとBased on the degree of coincidence between a captured image suitable for estimating the position of the moving object and a virtual image projected in the same direction as the captured image, the position of the actual moving object is estimated, and Estimating a posture angle of an actual moving object based on a degree of coincidence between a captured image suitable for estimating a posture angle and a virtual image projected in the same direction as the captured image;
を有することを特徴とする移動物***置姿勢推定方法。A moving object position / posture estimation method characterized by comprising:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012049382A JP5910180B2 (en) | 2012-03-06 | 2012-03-06 | Moving object position and orientation estimation apparatus and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012049382A JP5910180B2 (en) | 2012-03-06 | 2012-03-06 | Moving object position and orientation estimation apparatus and method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2013185871A JP2013185871A (en) | 2013-09-19 |
| JP5910180B2 true JP5910180B2 (en) | 2016-04-27 |
Family
ID=49387432
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2012049382A Active JP5910180B2 (en) | 2012-03-06 | 2012-03-06 | Moving object position and orientation estimation apparatus and method |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5910180B2 (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10249056B2 (en) | 2014-04-11 | 2019-04-02 | Nissan North America, Inc. | Vehicle position estimation system |
| US9151626B1 (en) | 2014-04-11 | 2015-10-06 | Nissan North America, Inc. | Vehicle position estimation system |
| US9727793B2 (en) * | 2015-12-15 | 2017-08-08 | Honda Motor Co., Ltd. | System and method for image based vehicle localization |
| JP2017116363A (en) * | 2015-12-24 | 2017-06-29 | アイシン・エィ・ダブリュ株式会社 | Vehicle position estimation system, method, and program |
| JP6336010B2 (en) | 2016-11-16 | 2018-06-06 | 三菱電機株式会社 | Road lane marking recognition device, road lane marking recognition method, driving assistance device provided with road lane marking recognition device, and driving assistance method provided with road lane marking recognition method |
| JP2021082181A (en) * | 2019-11-22 | 2021-05-27 | パナソニックＩｐマネジメント株式会社 | Position estimation device, vehicle, position estimation method and position estimation program |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003263104A (en) * | 2002-03-11 | 2003-09-19 | Mitsubishi Electric Corp | Imaging information recognition system |
| WO2006035755A1 (en) * | 2004-09-28 | 2006-04-06 | National University Corporation Kumamoto University | Method for displaying movable-body navigation information and device for displaying movable-body navigation information |
| JP2006177818A (en) * | 2004-12-22 | 2006-07-06 | Denso Corp | Navigation apparatus |
| JP2006260011A (en) * | 2005-03-16 | 2006-09-28 | Denso Corp | Display device for vehicle |
| JP4737523B2 (en) * | 2005-08-03 | 2011-08-03 | 三菱電機株式会社 | Car navigation system |
| JP5227065B2 (en) * | 2008-01-25 | 2013-07-03 | 株式会社岩根研究所 | 3D machine map, 3D machine map generation device, navigation device and automatic driving device |
| US20110109745A1 (en) * | 2008-07-07 | 2011-05-12 | Yuzuru Nakatani | Vehicle traveling environment detection device |
| JP5361421B2 (en) * | 2009-01-30 | 2013-12-04 | 三菱電機株式会社 | Measuring device, laser position / orientation value correction method and laser position / orientation value correction program for measuring device |
| JP2012043308A (en) * | 2010-08-20 | 2012-03-01 | Canon Inc | Position and attitude determination method, position and attitude determination device, object model generation method, object model generation device and program |
-
2012
- 2012-03-06 JP JP2012049382A patent/JP5910180B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2013185871A (en) | 2013-09-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9740942B2 (en) | Moving object location/attitude angle estimation device and moving object location/attitude angle estimation method | |
| US10696227B2 (en) | Determining a road surface characteristic | |
| CN108572663B (en) | Target tracking | |
| KR102493878B1 (en) | controlling host vehicle based on detected spacing between stationary vehicles | |
| US10832061B2 (en) | Traveling road boundary estimation apparatus and traveling assistance system using same | |
| JP5441549B2 (en) | Road shape recognition device | |
| JP4676373B2 (en) | Peripheral recognition device, peripheral recognition method, and program | |
| US10964217B2 (en) | Travel control method and travel control apparatus | |
| JP5966747B2 (en) | Vehicle travel control apparatus and method | |
| JP5910180B2 (en) | Moving object position and orientation estimation apparatus and method | |
| JP6567659B2 (en) | Lane detection device and lane detection method | |
| US20150332101A1 (en) | Lane boundary line recognition apparatus and program for recognizing lane boundary line on roadway | |
| US20130027511A1 (en) | Onboard Environment Recognition System | |
| US20090122136A1 (en) | Object detection device | |
| JP6299517B2 (en) | Signal recognition device and signal recognition method | |
| US20040061712A1 (en) | Stereoscopic image processing apparatus and the method of processing stereoscopic images | |
| JP7190261B2 (en) | position estimator | |
| TWI754808B (en) | Vehicle, vehicle positioning system, and vehicle positioning method | |
| JP6834401B2 (en) | Self-position estimation method and self-position estimation device | |
| JP6044084B2 (en) | Moving object position and orientation estimation apparatus and method | |
| JP6620175B2 (en) | Distance calculation device and vehicle control device | |
| JP2017117105A (en) | Visibility determination device | |
| JP2016037098A (en) | Vehicular imaging system | |
| JP7020957B2 (en) | Road structure estimation method and road structure estimation device | |
| JP2010086266A (en) | Image processing apparatus for vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20150203 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20150916 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20151006 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20151119 |
|
| 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: 20160301 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20160314 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 5910180 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |