JPWO2015072217A1 - 移動体位置推定装置および移動体位置推定方法 - Google Patents

移動体位置推定装置および移動体位置推定方法 Download PDF

Info

Publication number
JPWO2015072217A1
JPWO2015072217A1 JP2015547675A JP2015547675A JPWO2015072217A1 JP WO2015072217 A1 JPWO2015072217 A1 JP WO2015072217A1 JP 2015547675 A JP2015547675 A JP 2015547675A JP 2015547675 A JP2015547675 A JP 2015547675A JP WO2015072217 A1 JPWO2015072217 A1 JP WO2015072217A1
Authority
JP
Japan
Prior art keywords
edge
evaluation value
image
edges
distance
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
Application number
JP2015547675A
Other languages
English (en)
Other versions
JP6112221B2 (ja
Inventor
田中　慎也
慎也 田中
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nissan Motor Co Ltd
Original Assignee
Nissan Motor Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nissan Motor Co Ltd filed Critical Nissan Motor Co Ltd
Publication of JPWO2015072217A1 publication Critical patent/JPWO2015072217A1/ja
Application granted granted Critical
Publication of JP6112221B2 publication Critical patent/JP6112221B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F1/00—Details not covered by groups G06F3/00 - G06F13/00 and G06F21/00
    • G06F1/16—Constructional details or arrangements
    • G06F1/1613—Constructional details or arrangements for portable computers
    • G06F1/163—Wearable computers, e.g. on a belt
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F3/00—Input arrangements for transferring data to be processed into a form capable of being handled by the computer; Output arrangements for transferring data from processing unit to output unit, e.g. interface arrangements
    • G06F3/01—Input arrangements or combined input and output arrangements for interaction between user and computer
    • G06F3/011—Arrangements for interaction with the human body, e.g. for user immersion in virtual reality
    • G06F3/012—Head tracking input arrangements
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T1/00—General purpose image data processing
    • G06T1/0007—Image acquisition
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/20—Scenes; Scene-specific elements in augmented reality scenes
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/123—Traffic control systems for road vehicles indicating the position of vehicles, e.g. scheduled vehicles; Managing passenger vehicles circulating according to a fixed timetable, e.g. buses, trains, trams

Abstract

評価値算出部15は、パーティクル毎にエッジ画像と仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであってエッジ画像中のエッジと仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与える。位置・姿勢推定部16は、各パーティクルの評価値に基づいて自車両の位置を推定する。

Description

本発明は、移動体位置推定装置および移動体位置推定方法に関する。
特許文献１には、カメラの撮像画像から生成したエッジ画像とパーティクルフィルタを用いて既知の三次元地図から生成した仮想画像とのエッジのマッチングをパーティクル毎に行い、エッジの重なりが多いほど尤度を高くし、エッジの重なりが少ないほど尤度を低くすることで、尤度分布から確率的に移動体の位置を推定している。
特開2010-60451号公報
しかしながら、上記従来技術にあっては、エッジの重なり度合いから尤度を算出しているため、パーティクルのパラメータが移動体の真の位置に近い場合であっても、パーティクルの位置のパラメータが真の位置から僅かでもずれているときには、当該パーティクルの尤度が極端に低下することで、推定が不安定になるという問題があった。
本発明の目的は、移動体の位置を安定して推定できる移動体位置推定装置および移動体位置推定方法を提供することにある。
本発明では、パーティクル毎にエッジ画像と仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであってエッジ画像中のエッジと仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与える。
よって、移動体の位置を安定して推定できる。
実施例１の移動体位置推定装置の構成図である。 実施例１の計算機3における自己位置推定の制御ブロック図である。 実施例１の移動体位置推定装置によるパーティクルの散布方法を示す説明図である。 実施例１の画素数に応じた評価補正値の算出方法を示す説明図である。 実施例１の実距離に応じた評価補正値の算出方法を示す説明図である。 実施例１の計算機3の全体処理の流れを示すフローチャートである。 実施例１の自己位置推定処理の流れを示すフローチャートである。 実施例１の自己位置推定作用を示す図である。 実施例２のエッジ密度に応じた評価補正値の算出方法を示す説明図である。
1 車両
2 カメラ（撮像手段）
3 計算機
4 記憶部（記憶手段）
11 特徴抽出部（エッジ画像生成手段）
12 初期化処理部
13 位置・姿勢候補生成部
14 仮想画像生成部（仮想画像生成手段）
15 評価値算出部（評価値算出手段）
15a 評価補正値算出部
16 位置・姿勢推定部（位置推定手段）
17 初期化判定部
〔実施例１〕
図１は、実施例１の移動体位置推定装置の構成図である。
車両1は、カメラ（撮像手段）2と、計算機3と、記憶部（記憶手段）4とを備える。
カメラ2は、車両1の前端部であって、高さh、水平から下向きに角度θで取り付けられ、車両1の前方領域を撮像する。
計算機3は、記憶部4に格納された地図データとカメラ2で撮影された画像とのマッチング処理を実施し、車両1の位置・姿勢を推定する。
記憶部4は、車両1の周囲に存在する構造物のエッジ情報と位置情報とを有する三次元の地図データが記録されている。
図２は、実施例１の計算機3における自己位置推定の制御ブロック図である。
カメラ2のレンズは、一般的な歪みの無いレンズとするが、例えば広範囲を撮影するために魚眼レンズとしてもよい。この場合、後述する仮想画像生成部14でのカメラモデルを魚眼のものとすれば同じ枠組みで車両の自己位置を推定することができる。また、車両周辺をより広範囲に撮影するために魚眼レンズを装着した場合カメラを複数車載してもよい。
特徴抽出部（エッジ画像生成手段）11は、カメラ2で撮影した画像からエッジを抽出してエッジ画像を生成する。このエッジを抽出するためにはCanny法など既存のエッジ検出手法を用いればよい。もちろん問題に合わせて、Sobelフィルタなどの方法でエッジを検出できればよく、要は地図データから、ある与えられた位置・姿勢のパラメータとカメラ2のモデルから生成する仮想画像とのマッチングに十分な精度のエッジを画像中から観測できればよい。
初期化処理部12は、車両の位置・姿勢を推定するために利用するパーティクルフィルタの初期化処理を行う。実施例１では、車両の想定される位置・姿勢の範囲でパーティクル群の（位置・姿勢）パラメータの初期化を行う。今回は限られた区間の中で車両の位置・姿勢を推定する問題であるので、車両位置を表す3自由度（x,y,z）、および車両姿勢を表す3自由度（yaw,pitch,roll）の計6自由度とする（図３参照）。地図データを用いて自己位置を推定する実施例１の場合、初期化の範囲を設定するためにGPSを用いてラフな位置範囲を設定してもよい。また、このときパーティクルの数を設定する必要があるが、問題に合わせて適切な数を設定すればよい。パーティクルフィルタ自身は既知の手法であるので詳細は割愛する。
位置・姿勢候補生成部13は、あらかじめ設定された車両のシステムモデルを用いて、一時刻前のパーティクル群の位置・姿勢パラメータから現時刻のパーティクル群のパラメータを設定する（初期化処理部を抜けた直後は、その初期化された値から現時刻のパラメータを設定する）。この枠組みもパーティクルフィルタの範疇であり、詳細は割愛するが、実施例１では、システムモデルをランダムウォーク（車両運動を所定範囲内でランダムに想定する。）とする。もちろん、問題に合わせて等速直線運動などのモデルをシステムモデルとしてもよい。例えば、図３に示すように、１ループ前に推定されていた車両V(t1)の位置および姿勢角のパーティクルPと周囲のパーティクルP1〜P5をオドメトリ分だけ移動させ、パーティクルの存在分布範囲を設定して補正する。そして、新たな車両V(t2)の位置および姿勢角を推定するためにパーティクルP10〜P15を設定する。
仮想画像生成部（仮想画像生成手段）14は、記憶部4を参照して、前述した位置・姿勢候補生成部13であるパーティクルに設定された位置・姿勢のパラメータとカメラ2のカメラモデルを用いて仮想画像を生成する。一般に車両の位置・姿勢がパラメータとして与えられ、かつカメラ2のカメラモデルが既知であり（事前に計測すればよい。あるいは設計値でもよい）、さらに記憶部4の三次元データが既知であれば、三次元の地図データを二次元の画像（これを仮想画像と呼ぶ）に変換できる。
実施例１では、後述する評価値算出部15に仮想画像生成部14で生成される仮想画像のエッジと、カメラ2で撮影された画像から特徴抽出部11で抽出された画像中のエッジとのマッチングを行う。したがって、仮想画像生成部14では、特徴抽出部11で撮影した画像からエッジとして抽出される成分に対応した、三次元の地図データのエッジ情報のみを、仮想画像に投影すればよい。具体的には、地図中の建物のエッジ部分や、路面上の白線などさえ投影すればよく、建物や路面のテクスチャ・色などの情報の投影は不要である。
前述した位置・姿勢候補生成部13でパーティクルの数は問題に合わせて設定すればよいと記載したが、仮に100個のパーティクルを生成する場合、この処理を100回繰り返すことになる。
評価値算出部（評価値算出手段）15は、特徴抽出部11から出力されたエッジ画像と、仮想画像生成部14から出力された仮想画像のエッジ成分とのマッチングを行って両者のエッジの重なり度合を評価し、重なっているほど高い評価値（尤度）eを算出する。従来の評価値算出方法では、二つの画像を走査して注目画素（xi,yi）に共にエッジがあれば、評価値を加算し(Eval(xi,yi)=1)、そうでない場合は加算しないで(Eval(xi,yi)=0)、重なっているほど評価値が高くなるように設定するのに対し、実施例１では、パーティクルフィルタを用いた位置・姿勢推定手法の精度および安定化の向上を狙いとし、評価値を算出する際、エッジが重なっていなくても、最も近接するエッジ同士の距離（エッジ間距離）が近い（所定距離以下）場合には、両エッジの近さに応じた評価補正値を加算するようにした。評価補正値の算出は、評価値算出部15内の評価補正値算出部15aにより行なわれる。
評価補正値は、仮想画像中のエッジからエッジ画像中のエッジまでの画素数（ピクセル数）に応じて設定することができる。例えば、仮想画像中のエッジに対し、エッジ画像中のエッジが1または2画素ずれている画素（xi,yi）の画素数に応じた評価補正値ep(xi,yi)=0.5とし、3または4画素ずれている画素（xi,yi）の画素数に応じた評価補正値ep(xi,yi)=0.25とし、5画素以上ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.0とする。すなわち、図４(a)のような仮想画像において、エッジの両側に4画素分のエッジ近傍領域を設けることで、図４(b)に示すように、仮想画像中のエッジの幅を仮想的に太くし、このエッジ近傍領域にエッジ画像中のエッジが重なる場合には、エッジ間距離に応じて評価補正値ep(xi,yi)を付与する。
別の例として、単に画素数ではなく、実空間の三次元の距離を利用して評価補正値を算出してもよい。上記のように画素数から所定距離を定義した場合、車両からの奥行きの距離に応じて、1画素に対応する実空間の距離が異なる、すなわち、手前の立体物と奥の立体物の位置では1画素当たりの縦、横の分解能が異なるからである。例えば、図５(a)に示す仮想画像において、破線で囲んだ立体物は、実空間では手前の面と奥の面で同じ大きさであるが、奥行きに応じて仮想画像に投影される大きさが変化する。よって、実空間を反映させる場合、仮想画像生成部14での方法と同様に、三次元情報が既知の地図データを用いて、図５(b)に示すように、実距離を考慮したエッジ近傍領域を設け、実距離に応じた評価補正値ed(xi,yi)を求める。
実距離に応じた評価補正値ed(xi,yi)は、以下の式から求める。
ed(xi,yi) = 1/(1 + alpha × d(xi,yi))
ここで、alphaは調整パラメータ、d(xi,yi)は座標(xi,yi)を三次元投影したときのエッジとの最短距離である。この距離が所定値以上である（エッジまで遠い）場合はed(xi,yi)全体をゼロとしてよい。
評価補正値算出部15aでは、上記の処理を位置・姿勢候補生成部13で設定したパーティクルの数だけ繰り返す。
評価値算出部15は、各画素の評価値を、以下の式から算出する。
(i) 画素数に応じた評価補正値ep(xi,yi)を設定した場合
e = Σ(Eval(xi,yi) +ep(xi,yi))
Eval(xi,yi) = 1 （エッジが重なっている）
Eval(xi,yi) = 0 （エッジが重なっていない(otherwise)）
(ii) 実距離に応じた評価補正値ed(xi,yi)を設定した場合
e = Σ(Eval(xi,yi) +ed(xi,yi))
Eval(xi,yi) = 1 （エッジが重なっている）
Eval(xi,yi) = 0 （エッジが重なっていない(otherwise)）
これにより、画素数に応じた評価補正値ep(xi,yi)を設定した場合、仮想画像中のエッジからエッジ画像中のエッジまでの画素数が2画素までの部分なら元の点数の1/2を加算し、4画素までの部分なら元の点数の1/4を加算する。一方、実距離に応じた評価補正値ed(xi,yi)を設定した場合、加算の際に距離の逆数を加算すれば仮想画像中のエッジからエッジ画像中のエッジまでの距離が近いほど高い値が加算されるようになる。
評価値算出部15でも同様に、この処理を位置・姿勢候補生成部13で設定したパーティクルの数だけ、繰り返せばよい。
なお、上記の例では、仮想画像中のエッジを基準とし、仮想画像中のエッジからエッジ画像中のエッジまでの画素数または実距離に応じて評価補正値を算出したが、エッジ画像中のエッジを基準とし、エッジ画像中のエッジから仮想画像中のエッジまでの画素数または実距離に応じて評価補正値を算出しても同じ結果が得られる。
位置・姿勢推定部（位置推定手段）16は、評価値算出部15で設定された各パーティクルの評価値に基づいて、車両の位置・姿勢を推定する。この処理自体はパーティクルフィルタの枠組みであるが、評価値が大きいパーティクルほど大きな重みが設定される。例えば、尤度が最も高い予測位置および姿勢角候補を、車両の実際の位置および姿勢角として算出する。また、各予測位置および姿勢角候補の尤度を用いて、予測位置および姿勢角の重み付き平均を求め、その値を最終的な車両の位置および姿勢角としてもよい。
位置・姿勢推定部16では、パーティクルフィルタの枠組みで位置・姿勢を推定し、その推定したパラメータ（位置・姿勢）での評価値を計算する（処理としては位置・姿勢候補生成部13および評価値算出部15を実行する。これまで述べた処理ではこの一連の流れをパーティクルの個数分実行する必要があるが、ここでは推定結果の一パラメータ分だけ実行する）。この評価値を後段の初期化判定部17に出力し、推定が誤っている場合の初期化処理に利用する。
初期化判定部17は、推定した位置・姿勢が誤検出であるか否かの判定を行う。誤検出判定された場合は初期化処理部12に初期化の指令をだし、処理を再度実行する。判定の方法として、位置・姿勢推定部16より出力された推定結果での評価値が事前に設定された閾値よりも低い場合は、誤検出と判定する。
［全体処理］
図６は、実施例１の計算機3の全体処理の流れを示すフローチャートである。
ステップS1では、初期化処理部12において、パーティクルフィルタの初期化処理を行う。この処理はシステム起動時に一回実行すればよい。ただし、ステップS5において、初期化判定部17で再度の初期化が必要であると判定された場合には、再度パーティクル群を初期化する。
ステップS2では、カメラ2により車両前方の画像を撮影する。
ステップS3では、特徴抽出部11において、カメラ2により取得した画像からエッジを抽出する。
ステップS4では、自己位置推定処理を実施する。詳細は後述する。
ステップS5では、初期化判定部17において、推定した位置・姿勢が誤検出であり、再度の初期化が必要であるか否かを判定する。YESの場合はステップS1へ進み、NOの場合はステップS6へ進む。
ステップS6では、ユーザーの手でシステムオフが入力されたか否かを判定し、YESの場合は処理を終了し、NOの場合はステップS2へ進む。
［自己位置推定処理］
図７は、実施例１の自己位置推定処理の流れを示すフローチャートである。
ステップS41では、位置・姿勢候補生成部13において、あらかじめ設定された車両のシステムモデルを用いて、一時刻前のパーティクル群の位置・姿勢パラメータから現時刻のパーティクル群のパラメータ（位置・姿勢の6自由度）を設定する。
ステップS42では、仮想画像生成部14において、パーティクルに設定された位置・姿勢のパラメータに基づき、仮想画像を生成する。
ステップS43では、評価値算出部15の評価補正値算出部15aにおいて、エッジ画像中のエッジと仮想画像中のエッジとのずれ量（画素または実距離）に応じて評価補正値（epまたはed）を算出する。
ステップS44では、評価値算出部15において、エッジ画像と仮想画像とのエッジの重なり度合いを評価する。このとき、評価補正値（epまたはed）を加味して評価値を算出する。
ステップS45では、評価値算出部15において、初期化処理部12で生成したすべてのパーティクルに対して評価値算出が終了したか否かを判定し、YESの場合はステップS46へ進み、NOの場合はステップS41へ進む。
ステップS46では、位置・姿勢推定部16において、各パーティクルの評価値に対して重みを算出し、現時刻での位置・姿勢を推定する。
次に、作用を説明する。
従来の移動体位置推定装置では、パーティクルのパラメータが自車両の真の位置・姿勢に近い場合であっても、位置のパラメータが真の位置・姿勢から僅かでもずれているときには、そのパーティクルの尤度が極端に低下するため、図８(a)に示すように、自車両の位置・姿勢の推定が不安定になるという問題があった。
これに対し、実施例１では、エッジ画像中のエッジと仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下となる画素が多いほど尤度（評価値）を高くする。これにより、図８(b)に示すように、自車両の真の位置・姿勢に対する位置・姿勢のパラメータのずれが小さなパーティクルについて、尤度が極端に低下するのを抑制できる。換言すると、パーティクル群の位置・姿勢パラメータが自車両の真の位置・姿勢から小さなずれが生じた場合でも、尤度が与えられるので、自車両の真の位置・姿勢に近い位置・姿勢が推定できるようになる。よって、自車両の位置・姿勢を安定して推定できる。
実施例１では、エッジ間距離が所定距離以下である場合、エッジ間距離が近いほど尤度を高くする。これにより、自車両の真の位置・姿勢に対する位置・姿勢のパラメータのずれがより小さいパーティクルほど尤度が高くなるため、パラメータ空間の尤度分布において真の位置・姿勢に近い領域を密にすることができる。よって、より適正な尤度を求めることができ、位置・姿勢推定の精度および安定性を向上できる。
実施例１では、所定距離を所定画素数とした。これにより、評価値を高くする範囲をあらかじめ設計者が定めた画素数とすることができる。よって、問題に応じてパーティクルの尤度が高く評価されるエッジからの画素数の範囲を設定することができる。
実施例１では、所定距離を実空間上の三次元の距離とした。これにより、評価値を高くする範囲をあらかじめ設計者が定めた実空間の距離とすることができる。よって、問題に応じてパーティクルの尤度が高く評価されるエッジからの実空間上の距離の範囲を設定することができる。
実施例１にあっては以下に列挙する効果を奏する。
(1) 自車両前方を撮像し撮像画像を取得するカメラ2と、撮像画像からエッジを抽出したエッジ画像を生成する特徴抽出部11と、自車両周囲に存在する構造物のエッジ情報と位置情報とを有する地図データを記憶する記憶部4と、自車両の想定される位置および姿勢であるパーティクルを複数設定し、パーティクル毎に地図データのエッジ情報を想定される位置および姿勢から撮影された仮想画像に変換する仮想画像生成部14と、パーティクル毎にエッジ画像と仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであってエッジ画像中のエッジと仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与える評価値算出部15と、各パーティクルの評価値に基づいて自車両の位置を推定する位置・姿勢推定部16とを備えた。
よって、自車両の位置・姿勢を安定して推定できる。
(2) 評価値算出部15は、エッジ間距離が所定距離以下である場合、当該エッジ間距離が近いほど評価値を高くする。
よって、より適正な尤度を求めることができ、自車両の位置・姿勢の推定の精度および安定性を向上できる。
(3) 所定距離を所定画素数とした。
よって、問題に応じてパーティクルの尤度が高く評価されるエッジからの画素数の範囲を設定することができる。
(4) 所定距離を実空間上の三次元の距離とした。
よって、問題に応じてパーティクルの尤度が高く評価されるエッジからの実空間上の距離の範囲を設定することができる。
(5) 自車両前方を撮像し撮像画像を取得し、撮像画像からエッジを抽出したエッジ画像を生成し、自車両の想定される位置および姿勢であるパーティクルを複数設定し、パーティクル毎に自車両周囲に存在する構造物のエッジ情報と位置情報とを有する地図データのエッジ情報を想定される位置および姿勢から撮影された仮想画像に変換し、パーティクル毎にエッジ画像と仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであってエッジ画像中のエッジと仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与え、各パーティクルの評価値に基づいて自車両の位置を推定する。
よって、自車両の位置・姿勢を安定して推定できる。
〔実施例２〕
次に、実施例２に係る移動体位置推定装置について図面を参照して説明する。
図９は、エッジ密度に応じた評価補正値の算出方法を示す説明図である。
なお、その他の構成は、実施例１と同一であるため、同一の符号を付して詳細な説明は省略する。
図９(a)は、特徴抽出部（エッジ画像生成手段）11で、カメラ2で撮影した画像からエッジを抽出して生成したエッジ画像である。図９(a)に示すエッジ画像では、レーン9aに並行して、路肩9bが近傍に存在しているため、パーティクルの位置・姿勢によって、仮想画像のレーンのエッジと、エッジ画像中の路肩9bのエッジとが近い状態（エッジ密度が高い状態）となり、部分的に評価値が高くなってしまう。
そこで、実施例２では、最も近接するエッジ同士の距離（エッジ間距離）が近い（所定距離以下）場合で、エッジ画像のエッジ密度が高い領域では、判定する所定距離を小さくする。
例えば、エッジ画像のエッジ密度が低い領域では、実施例１のように、仮想画像中のエッジに対し、エッジ画像中のエッジが1または2画素ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.5とし、3または4画素ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.25とし、5画素以上ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.0とする。
一方、エッジ画像のエッジ密度が高い領域では、仮想画像中のエッジに対し、エッジ画像中のエッジが1画素ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.5とし、2画素ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.25とし、3画素以上ずれている画素(xi,yi)の画素数に応じた評価補正値ep(xi,yi)=0.0とする。
すなわち、図９(a)のようなエッジ画像において、エッジ画像のエッジ密度が高い領域であるレーン9aのエッジの両側では、2画素分のエッジ近傍領域を設けることで、図９(b)に示すように、他の領域に比較して、エッジ近傍領域を狭くし、路肩9bのエッジによって評価補正値が高くなるのを抑制できる。
実施例２にあっては、実施例１の効果(1)〜(5)に加え、以下の効果を奏する。
(6) 評価値算出部15は、エッジ画像のエッジ密度が高いほど、所定距離を小さくする。
よって、エッジ密度が高い領域の評価補正値が誤って高くなるのを抑制でき、自車両の位置・姿勢の推定の精度および安定性を向上できる。
（他の実施例）
以上、本発明を実施するための形態を、実施例に基づいて説明したが、本発明の具体的な構成は、実施例に限定されるものではなく、発明の要旨を逸脱しない範囲の設計変更等があっても本発明に含まれる。
例えば、実施例では、エッジ画像と仮想画像のエッジが重なっているほど評価値が高くなるように評価関数を設定した（最大化問題）が、エッジが重なっているほど誤差が少なるように評価関数を設定してもよい（最小化問題）。
本発明は、走行路の左右白線に対する車両の相対位置（横位置）の推定にも適用可能である。この場合、位置・姿勢パラメータは、位置パラメータとして横位置の1自由度、姿勢パラメータとしてpitch,yawの2自由度の計3自由度となる。なお、白線の幅も場所によって異なるため、白線の幅も推定パラメータとして追加してもよい。

Claims (6)

  1. 移動体周囲を撮像し撮像画像を取得する撮像手段と、
    前記撮像画像からエッジを抽出したエッジ画像を生成するエッジ画像生成手段と、
    前記移動体周囲に存在する構造物のエッジ情報と位置情報とを有する地図データを記憶する記憶手段と、
    前記移動体の想定される位置および姿勢であるパーティクルを複数設定し、パーティクル毎に前記地図データのエッジ情報を前記想定される位置および姿勢から撮影された仮想画像に変換する仮想画像生成手段と、
    前記パーティクル毎に前記エッジ画像と前記仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであって前記エッジ画像中のエッジと前記仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与える評価値算出手段と、
    前記各パーティクルの評価値に基づいて前記移動体の位置を推定する位置推定手段と、
    を備えたことを特徴とする移動体位置推定装置。
  2. 請求項１に記載の移動体位置推定装置において、
    前記評価値算出手段は、前記エッジ間距離が所定距離以下である場合、当該エッジ間距離が近いほど前記評価値を高くすることを特徴とする移動体位置推定装置。
  3. 請求項１または２に記載の移動体位置推定装置において、
    前記評価値算出手段は、前記エッジ画像のエッジ密度が高いほど、前記所定距離を小さくすることを特徴とする移動体位置推定装置。
  4. 請求項２または３に記載の移動体位置推定装置において、
    前記所定距離を所定画素数としたことを特徴とする移動体位置推定装置。
  5. 請求項２または３に記載の移動体位置推定装置において、
    前記所定距離を実空間上の三次元の距離としたことを特徴とする移動体位置推定装置。
  6. 移動体周囲を撮像し撮像画像を取得し、
    前記撮像画像からエッジを抽出したエッジ画像を生成し、
    前記移動体の想定される位置および姿勢であるパーティクルを複数設定し、パーティクル毎に前記移動体周囲に存在する構造物のエッジ情報と位置情報とを有する地図データのエッジ情報を前記記憶される位置および姿勢から撮影された仮想画像に変換し、
    前記パーティクル毎に前記エッジ画像と前記仮想画像とを比較し、エッジが重なっている数が多いほど高い評価値を与え、かつ、重なっていないエッジであって前記エッジ画像中のエッジと前記仮想画像中のエッジとの距離であるエッジ間距離が所定距離以下であるエッジが多いほど高い評価値を与え、
    前記各パーティクルの評価値に基づいて前記移動体の位置を推定することを特徴とする移動体位置推定方法。
JP2015547675A 2013-11-13 2014-09-05 移動体位置推定装置および移動体位置推定方法 Active JP6112221B2 (ja)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2013234516 2013-11-13
JP2013234516 2013-11-13
PCT/JP2014/073432 WO2015072217A1 (ja) 2013-11-13 2014-09-05 移動体位置推定装置および移動体位置推定方法

Publications (2)

Publication Number Publication Date
JPWO2015072217A1 true JPWO2015072217A1 (ja) 2017-03-16
JP6112221B2 JP6112221B2 (ja) 2017-04-12

Family

ID=53057162

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2015547675A Active JP6112221B2 (ja) 2013-11-13 2014-09-05 移動体位置推定装置および移動体位置推定方法

Country Status (8)

Country Link
US (1) US9424649B1 (ja)
EP (1) EP3070430B1 (ja)
JP (1) JP6112221B2 (ja)
CN (1) CN105723180B (ja)
BR (1) BR112016010089B1 (ja)
MX (1) MX355313B (ja)
RU (1) RU2621480C1 (ja)
WO (1) WO2015072217A1 (ja)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9849591B2 (en) * 2015-10-02 2017-12-26 X Development Llc Localization of a robot in an environment using detected edges of a camera image from a camera of the robot and detected edges derived from a three-dimensional model of the environment
EP3360023A4 (en) * 2015-10-09 2018-10-10 SZ DJI Technology Co., Ltd. Salient feature based vehicle positioning
US10013617B2 (en) * 2015-12-03 2018-07-03 Gm Global Technology Operations Snow covered path of travel surface condition detection
US9727793B2 (en) * 2015-12-15 2017-08-08 Honda Motor Co., Ltd. System and method for image based vehicle localization
JP6782903B2 (ja) * 2015-12-25 2020-11-11 学校法人千葉工業大学 自己運動推定システム、自己運動推定システムの制御方法及びプログラム
JP6795379B2 (ja) * 2016-03-10 2020-12-02 パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカＰａｎａｓｏｎｉｃ Ｉｎｔｅｌｌｅｃｔｕａｌ Ｐｒｏｐｅｒｔｙ Ｃｏｒｐｏｒａｔｉｏｎ ｏｆ Ａｍｅｒｉｃａ 運転制御装置、運転制御方法及び運転制御プログラム
JP6552448B2 (ja) * 2016-03-31 2019-07-31 株式会社デンソーアイティーラボラトリ 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム
JP6929183B2 (ja) * 2017-09-29 2021-09-01 株式会社デンソーテン レーダ装置および物標検出方法
GB2568286B (en) * 2017-11-10 2020-06-10 Horiba Mira Ltd Method of computer vision based localisation and navigation and system for performing the same
US11531354B2 (en) * 2017-12-05 2022-12-20 Sony Corporation Image processing apparatus and image processing method
CN109935108A (zh) * 2017-12-18 2019-06-25 姜鹏飞 一种基于准确位置的交通安全预警方法及装置
US20200311455A1 (en) * 2019-03-27 2020-10-01 GM Global Technology Operations LLC Methods and systems for correcting sensor information
RU2769918C1 (ru) 2021-05-18 2022-04-08 Общество с ограниченной ответственностью "ЭвоКарго" Способ позиционирования наземного транспортного средства

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009252112A (ja) * 2008-04-09 2009-10-29 Canon Inc 画像処理装置、画像処理方法
JP2013186551A (ja) * 2012-03-06 2013-09-19 Nissan Motor Co Ltd 移動物体位置姿勢推定装置及び方法
JP2014056535A (ja) * 2012-09-14 2014-03-27 Oki Electric Ind Co Ltd 物体追跡装置及び物体追跡方法

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8350850B2 (en) * 2008-03-31 2013-01-08 Microsoft Corporation Using photo collections for three dimensional modeling
JP5297727B2 (ja) 2008-09-04 2013-09-25 トヨタ自動車株式会社 ロボット装置及び物体の位置姿勢推定方法
US8385591B1 (en) * 2009-04-28 2013-02-26 Google Inc. System and method of using images to determine correspondence between locations
US8164543B2 (en) * 2009-05-18 2012-04-24 GM Global Technology Operations LLC Night vision on full windshield head-up display
JP5512258B2 (ja) * 2009-12-25 2014-06-04 本田技研工業株式会社 向き計測装置、向き計測システム、向き計測方法、及び向き計測プログラム
EP2606472A2 (en) * 2010-06-11 2013-06-26 Estill, James A. System and method for manipulating data having spatial coordinates
JP5703801B2 (ja) * 2011-02-04 2015-04-22 富士通株式会社 ロボット、位置推定方法及びプログラム
JP2012243051A (ja) * 2011-05-19 2012-12-10 Fuji Heavy Ind Ltd 環境認識装置および環境認識方法
US9046364B2 (en) * 2011-06-14 2015-06-02 Nissan Motor Co., Ltd. Distance measurement device and environment map generation apparatus
US8704882B2 (en) * 2011-11-18 2014-04-22 L-3 Communications Corporation Simulated head mounted display system and method
JP5804185B2 (ja) * 2012-03-06 2015-11-04 日産自動車株式会社 移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法
EP2662828B1 (en) * 2012-05-11 2020-05-06 Veoneer Sweden AB A vision system and method for a motor vehicle
JP6197388B2 (ja) * 2013-06-11 2017-09-20 富士通株式会社 距離測定装置、距離測定方法、およびプログラム
JPWO2015159547A1 (ja) * 2014-04-18 2017-04-13 Ｎｅｃソリューションイノベータ株式会社 情報処理システム、制御方法、及びプログラム記録媒体

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009252112A (ja) * 2008-04-09 2009-10-29 Canon Inc 画像処理装置、画像処理方法
JP2013186551A (ja) * 2012-03-06 2013-09-19 Nissan Motor Co Ltd 移動物体位置姿勢推定装置及び方法
JP2014056535A (ja) * 2012-09-14 2014-03-27 Oki Electric Ind Co Ltd 物体追跡装置及び物体追跡方法

Also Published As

Publication number Publication date
WO2015072217A1 (ja) 2015-05-21
EP3070430A1 (en) 2016-09-21
MX2016005904A (es) 2016-07-13
RU2621480C1 (ru) 2017-06-06
BR112016010089A2 (pt) 2020-11-10
JP6112221B2 (ja) 2017-04-12
EP3070430B1 (en) 2019-08-14
US20160239973A1 (en) 2016-08-18
CN105723180A (zh) 2016-06-29
CN105723180B (zh) 2017-08-15
BR112016010089B1 (pt) 2021-06-08
US9424649B1 (en) 2016-08-23
EP3070430A4 (en) 2017-01-11
MX355313B (es) 2018-04-16

Similar Documents

Publication Publication Date Title
JP6112221B2 (ja) 移動体位置推定装置および移動体位置推定方法
KR101725060B1 (ko) 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
KR101776622B1 (ko) 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
KR101708659B1 (ko) 이동 로봇의 맵을 업데이트하기 위한 장치 및 그 방법
KR101776621B1 (ko) 에지 기반 재조정을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
JP5926228B2 (ja) 自律車両用の奥行き検知方法及びシステム
JP4341564B2 (ja) 対象物判定装置
JP2018522348A (ja) センサーの３次元姿勢を推定する方法及びシステム
CN105551020B (zh) 一种检测目标物尺寸的方法及装置
WO2020228694A1 (zh) 一种相机姿态信息检测方法、装置以及相应的智能驾驶设备
JP2016029564A (ja) 対象検出方法及び対象検出装置
KR20150144730A (ko) ADoG 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
KR101207535B1 (ko) 이동 로봇의 이미지 기반 동시적 위치 인식 및 지도 작성 방법
WO2017033422A1 (ja) 画像処理装置および画像処理方法
JP6410231B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
CN111105467A (zh) 一种图像标定方法、装置及电子设备
JP2021085855A (ja) 補正距離算出装置、補正距離算出用プログラムおよび補正距離算出方法
CN111260709B (zh) 一种面向动态环境的地面辅助的视觉里程计方法
KR101531313B1 (ko) 차량 하부의 물체 탐지장치 및 방법
KR101980899B1 (ko) 직교 소실점을 이용한 단일 영상의 내벽 구조 검출 장치 및 방법
Brunken et al. Incorporating Plane-Sweep in Convolutional Neural Network Stereo Imaging for Road Surface Reconstruction.
US9818180B2 (en) Image processing device, method, and program
WO2021109068A1 (zh) 手势控制方法及可移动平台
CN116934980A (zh) 单体建筑模型的构建方法、设备及计算机可读存储介质
JP5614100B2 (ja) 画像処理装置及び移動体位置推定方法

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: 20170214

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20170227

R151 Written notification of patent or utility model registration

Ref document number: 6112221

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151