JP7845985B2 - Self-localization device, self-localization method, and self-localization program - Google Patents
Self-localization device, self-localization method, and self-localization programInfo
- Publication number
- JP7845985B2 JP7845985B2 JP2022169676A JP2022169676A JP7845985B2 JP 7845985 B2 JP7845985 B2 JP 7845985B2 JP 2022169676 A JP2022169676 A JP 2022169676A JP 2022169676 A JP2022169676 A JP 2022169676A JP 7845985 B2 JP7845985 B2 JP 7845985B2
- Authority
- JP
- Japan
- Prior art keywords
- image data
- flow
- self
- feature points
- unit
- 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
Landscapes
- Navigation (AREA)
- Image Analysis (AREA)
Description
本開示は、自己位置推定装置、自己位置推定方法、および自己位置推定プログラムに関する。 This disclosure relates to a self-localization device, a self-localization method, and a self-localization program.
従来、例えば特許文献１に示すように、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）を用いて自己の位置を推定する方法が知られている。ＳＬＡＭは、カメラやセンサを搭載した移動体によって、ＧＰＳなどの衛星システムに依存せずに、自己位置の推定と３次元地図である環境地図の作成とを同時に実行する技術である。 Conventionally, as shown in Patent Document 1, for example, a method for estimating one's own position using SLAM (Simultaneous Localization and Mapping) is known. SLAM is a technology that simultaneously performs self-position estimation and the creation of a three-dimensional environmental map using a mobile device equipped with cameras and sensors, without relying on satellite systems such as GPS.
しかし、上記従来技術においては、３次元地図の作成を行うため、自己位置推定に要する処理量が多く処理時間が長いという問題があった。本開示は、上記のような点に鑑みて創作されたものであり、その目的は、処理時間を短縮することが可能な自己位置推定装置、自己位置推定方法、および自己位置推定プログラムを提供することにある。 However, the conventional technology described above suffers from the problem of requiring a large amount of processing power and long processing time for self-localization in order to create a 3D map. This disclosure was created in view of the above-mentioned issues, and its purpose is to provide a self-localization device, a self-localization method, and a self-localization program that can shorten processing time.
本開示は、以下の形態として実現することが可能である。 This disclosure can be implemented in the following forms:
本開示の一形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含む。前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する。前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される。
本開示の他の形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含む。前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する。前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される。
本開示の他の形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記選別部は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する。
According to one embodiment of the present disclosure, a self-position estimation device is provided. This self-position estimation device is a self-position estimation device for estimating the self-position of a moving object (MC), and includes an image data acquisition unit (11) that acquires image data output from a multi-camera (20) consisting of a plurality of cameras (21, 22, 23, 24) mounted on the moving object at set intervals, a feature point extraction unit (12) that extracts feature points from the image data acquired by the image data acquisition unit, a storage unit (13) that stores the position information of each feature point in each of the image data, and the current feature point which is a feature point in the latest image data and the feature point in past image data. The system includes: a flow creation unit (14) that associates a point, which is a pre-feature point, with a flow, which is a set of corresponding feature points in the image data that differs for each time series; a selection unit (15) that selects from the flows created by the flow creation unit the flow that satisfies predetermined constraints, including an epipolar constraint calculated based on the predicted motion of the moving body, and calculates the position and orientation of the moving body using the information of the correct flow selected by the selection unit; and a self-position calculation unit (16) that calculates the position and orientation of the moving body using the information of the correct flow selected by the selection unit. The constraints include a reference range of flow length, which is the length of the flow in the image data that is continuous in the time series, as an indicator for selecting the flow. The selection unit selects from the created flows the flow whose flow length is within the reference range. The reference range of flow length is set individually for each of the multiple cameras.
In other forms of this disclosure, a self-localization device is provided. This self-localization device estimates the self-localization of a moving object (MC), and includes an image data acquisition unit (11) that acquires image data output from a multi-camera (20) consisting of a plurality of cameras (21, 22, 23, 24) mounted on the moving object at set intervals, a feature point extraction unit (12) that extracts feature points from the image data acquired by the image data acquisition unit, a storage unit (13) that stores the position information of each feature point in each of the image data, and the current feature point which is a feature point in the latest image data and the feature point in past image data. The system includes: a flow creation unit (14) that associates a point with a pre-feature point and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series; a selection unit (15) that selects from the flows created by the flow creation unit a flow that satisfies predetermined constraints, including an epipolar constraint calculated based on the predicted motion of the moving body, as the correct flow; and a self-position calculation unit (16) that calculates the position and orientation of the moving body using the information of the correct flow selected by the selection unit. The constraints include a reference range of flow length, which is the length of the flow in the image data that is continuous in the time series, as an indicator for selecting the flow. The selection unit selects from the created flows the ones whose flow length is within the reference range. The reference range of flow length is set according to the motion state of the moving body.
In other forms of this disclosure, a self-localization device is provided. This self-localization device estimates the self-localization of a moving object (MC), and includes an image data acquisition unit (11) that acquires image data output from a multi-camera (20) consisting of a plurality of cameras (21, 22, 23, 24) mounted on the moving object at set intervals, a feature point extraction unit (12) that extracts feature points from the image data acquired by the image data acquisition unit, a storage unit (13) that stores the position information of each feature point in each of the image data, and the current feature point which is a feature point in the latest image data and the feature point in past image data. The system includes: a flow creation unit (14) that associates a point with a pre-feature point and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series; a selection unit (15) that selects from the flows created by the flow creation unit the flows that satisfy predetermined constraints, including an epipolar constraint calculated based on the predicted motion of the moving body, as correct flows; and a self-position calculation unit (16) that calculates the position and orientation of the moving body using the information of the correct flows selected by the selection unit. If the number of correct flows selected by the selection unit is less than a predetermined standard, the selection unit performs the flow selection again using constraints that have been changed from the previously used constraints until the number of correct flows is equal to or greater than the standard.
この形態の自己位置推定装置によれば、画像データ取得部により取得されたマルチカメラから出力された画像データを用いて、３次元マップを作成することなく車両の自己位置を推定できる。そして、選別部により、エピポーラ拘束を含む予め定められた制約条件を用いて、制約条件を満たすフローを正しいフローとして選別する。このように制約条件を用いることで、最新の画像データ内の適切な現特徴点を特定する際に、適切な探索範囲を限定することができるため、現特徴点の予測が可能となり探索処理時間を少なくできる。以上により、自己位置推定装置における自己位置推定の処理時間を短縮できる。 This type of self-localization device allows for the estimation of a vehicle's position without creating a 3D map, using image data output from multiple cameras acquired by the image data acquisition unit. The selection unit then uses predetermined constraints, including epipolar constraints, to select flows that satisfy these constraints as the correct flows. By using these constraints, the appropriate search range can be limited when identifying appropriate current feature points within the latest image data, enabling prediction of current feature points and reducing search processing time. As a result, the processing time for self-localization in the self-localization device can be shortened.
以下、実施形態について、図１～図１６に基づいて説明する。
Ａ．第１実施形態：
Ａ１．自己位置推定装置１の構成：
第１実施形態の自己位置推定装置１の構成について、図１、図２を参照しつつ説明する。第１実施形態の自己位置推定装置１は、移動体としての自動運転車両ＭＣ（以下、単に「車両ＭＣ」という）に搭載され、車両ＭＣの自己位置を推定する。車両ＭＣは、自己位置推定装置１により推定された自己位置に基づいて自動運転による移動を行うことができる。なお、「自己位置」とは、移動体の位置および向きを含むものであり、位置姿勢を意味する。
The embodiments will be described below with reference to Figures 1 to 16.
A. First embodiment:
A1. Configuration of the self-position estimation device 1:
The configuration of the self-position estimation device 1 of the first embodiment will be described with reference to Figures 1 and 2. The self-position estimation device 1 of the first embodiment is mounted on an autonomous driving vehicle MC (hereinafter simply referred to as "vehicle MC") as a moving object, and estimates the self-position of the vehicle MC. The vehicle MC can move by autonomous driving based on the self-position estimated by the self-position estimation device 1. Note that "self-position" includes the position and orientation of the moving object, and means position and attitude.
図１に示すように、車両ＭＣは、自己位置推定装置１のほか、車両制御部１０と、カメラ２０と、を備える。車両制御部１０は、車両ＭＣの制御を行う機能部であり、車両制御部１０には、カメラ２０が電気的に接続されており、カメラ２０によって撮像された画像データ（以下、「フレーム」ともいう。）が通知される。その他、車両制御部１０には、図示しないＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）センサ等が電気的に接続されている。 As shown in Figure 1, the vehicle MC includes a self-position estimation device 1, a vehicle control unit 10, and a camera 20. The vehicle control unit 10 is a functional unit that controls the vehicle MC. The camera 20 is electrically connected to the vehicle control unit 10, and image data (hereinafter also referred to as "frames") captured by the camera 20 is notified to the vehicle control unit 10. In addition, GNSS (Global Navigation Satellite System) sensors and other devices (not shown) are electrically connected to the vehicle control unit 10.
図２に示すように、カメラ２０は、前カメラ２１と、後カメラ２２と、右カメラ２３と、左カメラ２４と、の合計４つのカメラを有した複眼のマルチカメラとして構成されている。前カメラ２１は、車両ＭＣの前方における周辺画像を取得する。後カメラ２２は、車両ＭＣの後方における周辺画像を取得する。 As shown in Figure 2, the camera 20 is configured as a compound-eye multi-camera with a total of four cameras: a front camera 21, a rear camera 22, a right camera 23, and a left camera 24. The front camera 21 acquires surrounding images in front of the vehicle MC. The rear camera 22 acquires surrounding images behind the vehicle MC.
右カメラ２３は、車両ＭＣの進行方向に見たときの右方における周辺画像を取得する。左カメラ２４は、車両ＭＣの進行方向に見たとき左方における周辺画像を取得する。以下、各カメラ２１，２２，２３，２４を特に区別しないときは、単に「カメラ２０」という。カメラ２０は、魚眼を代表する広角カメラであり、４つのカメラの合計の画角は３６０度の視野をカバーする。 The right camera 23 acquires peripheral images to the right when viewed in the direction of travel of the vehicle MC. The left camera 24 acquires peripheral images to the left when viewed in the direction of travel of the vehicle MC. Hereafter, unless otherwise specified, cameras 21, 22, 23, and 24 will simply be referred to as "camera 20". Camera 20 is a wide-angle camera, typically a fisheye lens, and the combined field of view of the four cameras covers a 360-degree field of view.
再び、図１を参照する。本実施形態の自己位置推定装置１は、車両ＭＣの車両制御部１０と電気的に接続されている。具体的には、本実施形態の自己位置推定装置１は、車両ＭＣのＯＢＤ２（Ｏｎ Ｂｏａｒｄ Ｄｉａｇｎｏｓｔｉｃｓ ２）に接続されているため、自己位置推定装置１は、車両制御部１０から車両ＭＣの各種の情報の受信が可能である。 Refer to Figure 1 again. The self-position estimation device 1 in this embodiment is electrically connected to the vehicle control unit 10 of the vehicle motor control unit (MC). Specifically, since the self-position estimation device 1 in this embodiment is connected to the OBD2 (On-Board Diagnostics 2) of the vehicle MC, the self-position estimation device 1 can receive various types of information from the vehicle control unit 10 of the vehicle MC.
なお、自己位置推定装置１と車両ＭＣとの接続はこれに限られず、例えば、無線通信による接続を用いてもよい。また、自己位置推定装置１と車両ＭＣとは接続されておらず、自己位置推定装置１が、カメラ２０を別途備えてもよい。本実施形態では、ＯＢＤ２から自己位置推定装置１へ電力が供給されるが、これに限られず、自己位置推定装置１は別途バッテリーを備えてもよく、車両ＭＣのシガーソケットから自己位置推定装置１の電力を供給してもよい。 The connection between the self-position estimation device 1 and the vehicle MC is not limited to this; for example, wireless communication may be used. Alternatively, the self-position estimation device 1 and the vehicle MC may not be connected, and the self-position estimation device 1 may be equipped with a separate camera 20. In this embodiment, power is supplied to the self-position estimation device 1 from the OBD2, but this is not limited; the self-position estimation device 1 may be equipped with a separate battery, or power may be supplied to the self-position estimation device 1 from the vehicle MC's cigarette lighter socket.
自己位置推定装置１は、画像データ取得部１１と、特徴点抽出部１２と、記憶部１３と、フロー作成部１４と、選別部１５と、自己位置算出部１６と、の各機能モジュールを備える。自己位置推定装置１は、ＣＰＵと、ＲＯＭやＲＡＭなどのメモリと、を備える周知のコンピュータとして構成されている。各機能モジュール１１～１６は、メモリに記憶されたプログラムをＣＰＵによって実行することで実現される。 The self-localization device 1 comprises the following functional modules: an image data acquisition unit 11, a feature point extraction unit 12, a storage unit 13, a flow creation unit 14, a sorting unit 15, and a self-localization calculation unit 16. The self-localization device 1 is configured as a well-known computer, comprising a CPU and memory such as ROM or RAM. Each functional module 11 to 16 is implemented by the CPU executing programs stored in memory.
画像データ取得部１１は、車両ＭＣの周辺環境の画像データを取得する。具体的には、画像データ取得部１１は、上記カメラ２０から出力された画像データを設定間隔毎に車両ＭＣから取得する工程を実行する。 The image data acquisition unit 11 acquires image data of the surrounding environment of the vehicle's monitor center (MC). Specifically, the image data acquisition unit 11 performs the process of acquiring image data output from the camera 20 from the vehicle's MC at set intervals.
特徴点抽出部１２は、画像取得部１１により取得された画像データから特徴点を抽出する工程を実行する。特徴点は、例えば、エッジ、コーナー、色分布などを含む。特徴点抽出の手法は、ＯＲＢ，ＳＩＦＴ，ＳＵＲＦを代表するインダイレクト法であってもよいし、ＤＳＯ（Ｄｉｒｅｃｔ Ｓｐａｒｓｅ Ｏｄｏｍｅｔｒｙ）を代表するダイレクト法であってもよい。 The feature point extraction unit 12 performs the process of extracting feature points from image data acquired by the image acquisition unit 11. Feature points include, for example, edges, corners, and color distributions. The feature point extraction method may be an indirect method such as ORB, SIFT, or SURF, or a direct method such as DSO (Direct Spare Odometry).
記憶部１３は、各画像データ内の各特徴点の位置情報として３次元位置座標を記憶する工程を実行する。その他、記憶部１３は、車両ＭＣの自己位置推定の一連の処理において用いられる各種データや、演算結果を記憶する。 The memory unit 13 performs the process of storing the three-dimensional position coordinates as positional information for each feature point within each image data. In addition, the memory unit 13 stores various data and calculation results used in the series of processes for estimating the vehicle MC's self-position.
フロー作成部１４は、撮影された時間毎に異なる画像データにおいて対応する特徴点同士の組であるフローを作成する工程を実行する。フロー作成部１４は、フローの作成に際して、記憶部１３に記憶された過去の画像データにおける特徴点である前特徴点と、最新の画像データにおける特徴点である現特徴点と、の対応付けを行う。具体的には、最新より一つ前の過去の画像データにおける前特徴点と、現特徴点との対応付けが、フレーム毎に実行される。 The flow creation unit 14 executes the process of creating a flow, which is a set of corresponding feature points in different image data captured at different times. When creating the flow, the flow creation unit 14 associates the previous feature points (feature points in past image data stored in the storage unit 13) with the current feature points (feature points in the latest image data). Specifically, the association between the previous feature points in the image data immediately preceding the latest image and the current feature points is performed for each frame.
選別部１５は、予め定められた制約条件を用いて、フロー作成部１４により作成されたフローのうち、上記制約条件を満たすフローを正しいフローとして選別する工程を実行する。制約条件は、車両ＭＣの予測される運動に基づいて計算されたエピポーラ拘束を含む。なお、制約条件の詳細については、後述する自己位置推定方法において、制御フローチャートと併せて詳細に説明する。 The selection unit 15 performs a process of selecting the correct flow from among the flows created by the flow creation unit 14, using predetermined constraints, that satisfy the above constraints. The constraints include epipolar constraints calculated based on the predicted motion of the vehicle MC. Details of the constraints will be explained in detail later in the self-position estimation method section, along with the control flowchart.
自己位置算出部１６は、選別部１５により選別されたフローの情報を用いて、車両ＭＣの位置および向きを算出する工程を実行する。算出された車両ＭＣの位置および向き、すなわち位置姿勢が、車両ＭＣの現在位置の推定結果となる。ここで、自己位置算出に際しては、ＳＬＡＭ処理を用いる。ＳＬＡＭとは、Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇの頭字語であり、自己位置推定と環境地図作成とを同時に行う手法のことである。なお、本実施形態では、３次元マップとしての環境地図の作成は行わず、カメラ２０からの画像のみを用いて車両ＭＣの位置推定を行う。 The self-position calculation unit 16 performs a process of calculating the position and orientation of the vehicle MC using the flow information selected by the selection unit 15. The calculated position and orientation of the vehicle MC, i.e., the position and attitude, become the estimated current position of the vehicle MC. Here, SLAM processing is used for self-position calculation. SLAM is an acronym for Simultanate Localization And Mapping, and is a method that simultaneously performs self-position estimation and environmental map creation. In this embodiment, however, an environmental map as a 3D map is not created; the vehicle MC's position is estimated using only images from the camera 20.
Ａ２．自己位置推定方法：
次に、上記自己位置推定装置１による自己位置推定方法について説明する。図３は、現在（すなわち最新）の自己位置算出に用いるための、現在フレームの特徴点リストを得る手順を示すフローチャートである。「現在フレーム」は最新のフレームであり、以下、「前回フレーム」とは、現在フレームより一つ前のフレームであり、「前々回フレーム」とは、前回フレームよりもさらに一つ前のフレームである。「前回フレーム」および「前々回フレーム」は、いずれも過去の画像データに相当する。
A2. Self-location estimation method:
Next, the self-position estimation method using the self-position estimation device 1 described above will be explained. Figure 3 is a flowchart showing the procedure for obtaining a list of feature points of the current frame for use in calculating the current (i.e., latest) self-position. "Current frame" is the latest frame, and so on, "previous frame" is the frame immediately preceding the current frame, and "frame before that" is the frame immediately preceding the previous frame. Both "previous frame" and "frame before that" correspond to past image data.
特徴点リストには、上記フローの情報が含まれる。図３に示す処理は、上記自己位置推定装置１により、設定間隔毎に繰り返し実行される。ここでの設定間隔は、カメラ２０による撮影の設定間隔と同じである。 The feature point list contains the information from the above flow. The process shown in Figure 3 is repeatedly executed by the self-localization device 1 at set intervals. The set interval here is the same as the set interval for camera 20's image capture.
図３に示すように、Ｓ１０１において、車両ＭＣの角度変化量および移動量の前回推定値が読み込まれる。「前回推定値」は、前々回フレームと前回フレーム間における車両ＭＣの角度変化量および移動量であり、事前に計算されて記憶部１３に記憶されている。次に、Ｓ１０２において、Ｓ１０１において読み込んだ前回推定値を用いて、等速運動を前提として、前回フレームと現在フレームとの間の基本行列が作成される。基本行列は、エピポーラ拘束を数式で表すときに、用いられる行列である。 As shown in Figure 3, in S101, the previously estimated values of the vehicle MC's angular change and displacement are read. The "previously estimated values" are the angular change and displacement of the vehicle MC between the frame before last and the previous frame, and are calculated in advance and stored in the memory unit 13. Next, in S102, using the previously estimated values read in S101, a basic matrix is created between the previous frame and the current frame, assuming constant velocity motion. This basic matrix is used when expressing the epipolar constraint mathematically.
次に、Ｓ１０３において、エピポーラ線が作成される。エピポーラ線は、Ｓ１０２において作成された基本行列と、現在フレームにおける特徴点の３次元座標およびカメラ２０の焦点距離と、前回フレームにおける特徴点の３次元座標およびカメラ２０の焦点距離と、を用いて公知の方法により作成される。 Next, in S103, the epipolar line is created. The epipolar line is created using a known method, employing the fundamental matrix created in S102, the three-dimensional coordinates of the feature points in the current frame and the focal length of the camera 20, and the three-dimensional coordinates of the feature points in the previous frame and the focal length of the camera 20.
そして、Ｓ１０４において、エピポーラ拘束による上限閾値と、フロー長の基準範囲が決定される。このＳ１０４の処理は、選別部１５により実行される。なお、Ｓ１０４の処理は、選別部１５に限らず、他の機能部が実行してもよい。なお、「フロー長」とは、前特徴点と、前特徴点と対応する現特徴点と、を結んだ一つのフローの長さである。「フロー長」は、時系列において連続する画像データ間におけるフローの長さであり、換言すると、１フレーム間のフローの長さのことをいう。Ｓ１０４において決定される、エピポーラ拘束による上限閾値と、フロー長の基準範囲を、併せて「制約条件」という。また、エピポーラ拘束による上限閾値を満たすとの制約条件を「第１条件」とし、フロー長の基準範囲を満たすとの制約条件を「第２条件」とする。 Then, in S104, the upper threshold value due to the epipolar constraint and the reference range for the flow length are determined. This S104 process is performed by the sorting unit 15. Note that the S104 process is not limited to the sorting unit 15; other functional units may also perform it. "Flow length" refers to the length of a single flow connecting a previous feature point and the current feature point corresponding to that previous feature point. "Flow length" is the length of the flow between consecutive image data in a time series; in other words, it refers to the length of the flow between one frame. The upper threshold value due to the epipolar constraint and the reference range for the flow length determined in S104 are collectively called "constraint conditions." Furthermore, the constraint condition of satisfying the upper threshold value due to the epipolar constraint is referred to as the "first condition," and the constraint condition of satisfying the reference range for the flow length is referred to as the "second condition."
Ｓ１０４において制約条件が決定された後は、Ｓ１０５において、制約条件による判定が行われる。すなわち、上述の第１条件および第２条件を満たすか否かが判定される。具体的には、現特徴点の座標が、エピポーラ線からの所定距離の上限閾値内にあり、かつ、現特徴点と前特徴点とを結んだフロー長が、基準範囲内にあれば、制約条件を満たすと判定する。なお、第１条件と第２条件のうち、いずれか一つでも満たさない場合には、制約条件を満たさないと判定する。この判定は、フレーム内の全ての特徴点に対して実行される。なお、Ｓ１０４，Ｓ１０５における、制約条件の決定および判定についての詳細は、後述する。 After the constraints are determined in S104, a determination is made in S105 based on those constraints. Specifically, it is determined whether the first and second conditions described above are met. More precisely, if the coordinates of the current feature point are within the upper limit threshold of a predetermined distance from the epipolar line, and the flow length connecting the current feature point and the previous feature point is within the reference range, then the constraints are deemed met. If either the first or second condition is not met, then the constraints are deemed not met. This determination is performed for all feature points within the frame. Further details regarding the determination and evaluation of the constraints in S104 and S105 will be described later.
次に、Ｓ１０６において、制約条件を満たす特徴点が「特徴点リスト」に追加される。「特徴点リスト」とは、本制御ルーチンの終了後に、自己位置推定の算出に用いられる特徴点およびフローの情報をまとめて格納するリストである。次に、Ｓ１０７において、特徴点リストに追加されたフローの数が基準以上であるか否かが判断される。フローの数の基準値は、適切な自己位置推定の算出に際して必要とされる数として、事前の試験により予め定められる。そして、Ｓ１０７において、フローの数が基準以上である場合には（Ｓ１０７：Ｙｅｓ）、Ｓ１０９に進み、特徴点リストが出力される。ここで出力される特徴点リストは、制約条件を満たさない特徴点およびフローの情報が間引きされたものである。Ｓ１０９の処理の後、本処理ルーチンは終了する。その後、出力された特徴点リストの情報を用いて、例えば２点法等により、自己位置推定が行われる。 Next, in S106, feature points that satisfy the constraints are added to the "feature point list." The "feature point list" is a list that stores information on feature points and flows used for calculating self-localization after the completion of this control routine. Next, in S107, it is determined whether the number of flows added to the feature point list is above a certain threshold. The threshold number of flows is predetermined through prior testing as the number required for appropriate self-localization calculation. If the number of flows is above the threshold in S107 (S107: Yes), the process proceeds to S109, and the feature point list is output. The feature point list output here is a filtered version with information on feature points and flows that do not satisfy the constraints removed. After processing in S109, this processing routine terminates. Subsequently, self-localization is performed using the information in the output feature point list, for example, by the two-point method.
一方、Ｓ１０７において、特徴点リストに追加されたフローの数が基準以上ではない場合には（Ｓ１０７：Ｎｏ）、Ｓ１０８に進み、制約条件が緩和される。フローの数があまりに少なく基準より少ない場合には、現在の自己位置推定の算出が行えないため、もしくは、精度が著しく低下するため、制約条件を緩和することで、特徴点リストのフローの数を増やすためである。 On the other hand, if the number of flows added to the feature point list in S107 is not equal to or greater than the threshold (S107: No), the process proceeds to S108, and the constraint is relaxed. This is because if the number of flows is too small and below the threshold, the current self-localization calculation cannot be performed, or the accuracy is significantly reduced. Therefore, the constraint is relaxed to increase the number of flows in the feature point list.
「制約条件を緩和する」とは、具体的には、エピポーラ線からの所定距離の上限閾値を大きくする、または、フロー長の基準範囲を大きくすることである。制約条件が緩和された後は、再びＳ１０５における判定処理に戻り、以下、Ｓ１０７においてフローの数が基準以上となるまで、Ｓ１０８、Ｓ１０５、Ｓ１０６の処理が繰り返される。 "Relaxing the constraints" specifically means increasing the upper limit threshold for the predetermined distance from the epipolar line, or increasing the reference range for the flow length. After the constraints are relaxed, the process returns to the determination process in S105, and then the processes in S108, S105, and S106 are repeated until the number of flows exceeds the reference value in S107.
Ａ３．制約条件の詳細：
次に、上記Ｓ１０４，Ｓ１０５における、制約条件の決定および判定に関する詳細について説明する。まず、エピポーラ拘束による上限閾値について図４を用いて説明する。図４は、エピポーラ拘束を説明するための図であり、３次元空間を異なる位置のカメラから撮影した幾何を示す図である。図４において、点ｅＬおよび点ｅＲは、エピポール（Ｅｐｉｐｏｌｅ）であり、一方のカメラから他方のカメラが写る位置のことである。また、エピポーラ線Ｅは、「カメラＯＬから見てＸＬに物体が写っている」とき、「カメラＯＲから見てどこに物体が写るか」の候補となる線である。
A3. Details of the constraints:
Next, we will explain in detail the determination and judgment of the constraints in S104 and S105 above. First, we will explain the upper threshold due to the epipolar constraint using Figure 4. Figure 4 is a diagram for explaining the epipolar constraint, and it shows the geometry of a three-dimensional space as captured by cameras at different positions. In Figure 4, points eL and eR are epipoles, which are the positions from which one camera captures the other camera. The epipolar line E is a line that represents a candidate for "where the object will be captured from the perspective of camera OR " when "the object is captured at XL as seen from camera OL ".
図４に示すように、ＸＰにある物体は、前回フレームにおけるカメラＯＬから見るとＸＬに写り、現在フレームにおけるカメラＯＲから見るとＸＲに写る。「カメラＯＬから見てＸＬに物体が写っている」ことしか分かっていない時点では、カメラＯＲでその物体が写る（ＸＲがある）位置は、「エピポーラ線Ｅ上のどこか」に限定される。このことをエピポーラ拘束（Ｅｐｉｐｏｌａｒ Ｃｏｎｓｔｒａｉｎｔ）と呼ぶ。 As shown in Figure 4, an object at XP appears as XL from the perspective of camera OL in the previous frame, and as XR from the perspective of camera OR in the current frame. At a point where we only know that "the object appears as XL from the perspective of camera OL ," the position where the object appears (where XR is) in camera OR is limited to "somewhere on the epipolar line E." This is called the epipolar constraint.
従来の３次元マップを作成する手法の場合には、２枚のフレームから、Ｘｐの奥行き方向におけるおおまかな位置が分かるため、現在フレーム内で特徴点を探す際の探索範囲を、比較的小さい範囲に２次元的に定めることができる。しかし、本実施形態では、３次元マップを持たないため、ＯＬとＸＬを結ぶ直線であるＸ線の延長上にあることは分かるものの、Ｘ線上における奥行き方向の位置が不明である。すなわち、図４において、Ｘ２やＸ１やＸ０にあるかも知れない。また、ＸＲの近くにあることは分かるが、エピポーラ線Ｅの近傍といっても、特徴点の探索範囲は広く、定まらない。 In conventional methods for creating 3D maps, the approximate position of Xp in the depth direction can be determined from two frames, allowing the search range for feature points within the current frame to be defined in a relatively small, two-dimensional area. However, in this embodiment, since there is no 3D map, although it is known that it lies on the extension of the X-ray line connecting OL and XL , its position in the depth direction on the X-ray line is unknown. That is, in Figure 4, it may be at X2 , X1 , or X0 . Also, although it is known to be near XR , even if it is in the vicinity of the epipolar ray E, the search range for feature points is wide and not fixed.
そこで、本実施形態では、現在フレーム内で現特徴点を探す際において、エピポーラ線Ｅから所定の距離以内の範囲を探索範囲Ｓとする。言い換えると、探索範囲Ｓは、現在フレーム内においてエピポーラ線Ｅから両側に所定距離をとった２本の平行な直線Ｌ１，Ｌ２に挟まれた領域である。「エピポーラ線Ｅからの距離」は、図４において、２次元平面としての現在フレーム内における、エピポーラ線Ｅと直線Ｌ１，Ｌ２との１次元距離である。つまり、Ｓ１０４では、作成されたエピポーラ線と、予め設定された距離とに基づいて、エピポーラ拘束による上限閾値、すなわち上記探索範囲Ｓが決定される。 Therefore, in this embodiment, when searching for the current feature point within the current frame, the search range S is defined as the range within a predetermined distance from the epipolar line E. In other words, the search range S is the region within the current frame that is sandwiched between two parallel lines L1 and L2, which are set at predetermined distances on both sides from the epipolar line E. The "distance from the epipolar line E" is the one-dimensional distance between the epipolar line E and lines L1 and L2 within the current frame, which is a two-dimensional plane in Figure 4. That is, in S104, the upper threshold due to the epipolar constraint, i.e., the search range S, is determined based on the created epipolar line and the pre-set distance.
そして、Ｓ１０５において、選別部１５は、作成されたフローのうち、現在フレーム内の現特徴点の位置座標が、探索範囲Ｓ内にあるものを正しいフローとして選別する。エピポーラ拘束による上限閾値、すなわち、エピポーラ線Ｅからの所定距離を、フローの選別の指標としている。なお、距離の上限閾値は、探索処理時間や、特徴点の位置精度等を鑑みて、事前の試験データにより探索範囲Ｓがあまり広くなりすぎない程度に予め適宜設定される。 Then, in S105, the selection unit 15 selects from the created flows those in which the position coordinates of the current feature points within the current frame are within the search range S. The upper threshold due to the epipolar constraint, i.e., a predetermined distance from the epipolar line E, is used as an indicator for flow selection. The upper threshold for distance is set appropriately in advance based on prior test data, taking into account the search processing time, the positional accuracy of the feature points, etc., to ensure that the search range S does not become too wide.
次に、フロー長の基準範囲の決定について、図５～図１４を参照して説明する。図５は、車両ＭＣの運動情報を動的に示し、各カメラ２１，２２，２３，２４の位置変化を示す図であり、任意の環境における左折時の形態を示している。図６～図９は、フロー長の度数分布を示すヒストグラムである。図６は、前カメラ２１により取得されたデータに基づいたヒストグラムであり、図７は、後カメラ２２により取得されたデータに基づいたヒストグラムである。図８は、右カメラ２３により取得されたデータに基づいたヒストグラムであり、図９は、左カメラ２４により取得されたデータに基づいたヒストグラムである。図６～図９において、横軸にフロー長を取り、縦軸に出現値を取っている。なお、フロー長の目盛は、フロー長の大小を見るためのデータ処理上の便宜上の単位のない数値である。以下、後述する図１１～図１４についても同様である。 Next, the determination of the reference range for flow length will be explained with reference to Figures 5 to 14. Figure 5 dynamically shows the motion information of the vehicle MC, illustrating the positional changes of each camera 21, 22, 23, and 24, and showing the form during a left turn in an arbitrary environment. Figures 6 to 9 are histograms showing the frequency distribution of flow length. Figure 6 is a histogram based on data acquired by the front camera 21, and Figure 7 is a histogram based on data acquired by the rear camera 22. Figure 8 is a histogram based on data acquired by the right camera 23, and Figure 9 is a histogram based on data acquired by the left camera 24. In Figures 6 to 9, the horizontal axis represents flow length, and the vertical axis represents the occurrence value. Note that the flow length scale is a unitless numerical value for convenience in data processing to observe the magnitude of the flow length. The same applies to Figures 11 to 14, which will be described later.
図５～図９に示すように、車両ＭＣが左折するとき、各カメラ２１，２２，２３，２４により取得されるフロー長のヒストグラムはそれぞれ異なる。図６に示すように、前カメラ２１によるヒストグラムでは、フロー長が３０の出現値が約９０であり突出して大きく、次にフロー長が４０の出現値が約６０であり大きく、その他は小さくなっている。図７に示すように、後カメラ２２によるヒストグラムでは、フロー長が３０の出現値が６０であり突出して大きく、フロー長が２０，４０，５０の出現値が２０近傍であり、フロー長が３０の出現値に対して半数以下である。 As shown in Figures 5 to 9, when the vehicle MC turns left, the histograms of flow lengths acquired by each camera 21, 22, 23, and 24 are different. As shown in Figure 6, in the histogram from the front camera 21, the frequency of flow lengths of 30 is exceptionally high at approximately 90, followed by flow lengths of 40 at approximately 60, while other values are low. As shown in Figure 7, in the histogram from the rear camera 22, the frequency of flow lengths of 30 is exceptionally high at 60, while the frequency of flow lengths of 20, 40, and 50 is around 20, less than half the frequency of flow lengths of 30.
また、図８に示すように、右カメラ２３によるヒストグラムでは、フロー長が５０および６０の出現値が突出して大きい。図９に示すように、左カメラ２４によるヒストグラムでは、フロー長が１０～４０と大きくなるにつれて出現値も大きくなり、フロー長が４０の出現値が６０であり最も大きい。一方、フロー長が５０，６０の出現値は１０以下であり小さくなっている。 Furthermore, as shown in Figure 8, the histogram from the right camera 23 shows exceptionally large occurrences for flow lengths of 50 and 60. As shown in Figure 9, the histogram from the left camera 24 shows that the occurrences increase as the flow length increases from 10 to 40, with the largest occurrence being 60 at a flow length of 40. On the other hand, the occurrences for flow lengths of 50 and 60 are small, below 10.
以上のように、各カメラ２１～２４において、フロー長の統計分布の系統はそれぞれ異なっていることが分かった。また、出現値の大きいフロー、すなわち長いフローを用いて推定を行った方が、精度が良くなることが分かった。そこで、本実施形態では、これらの性質を利用し、フロー長の基準範囲を、各カメラ２１～２４において適宜設定する。また、フロー長の基準範囲を、右折や左折等の運転状況に応じて設定する。なお、右折や左折等の運転状況は、「移動体の移動状態」の一例に相当する。 As described above, it was found that the statistical distribution of flow length differs for each camera 21-24. Furthermore, it was found that using flows with larger occurrence values, i.e., longer flows, improves accuracy. Therefore, in this embodiment, these properties are utilized to appropriately set the reference range of flow length for each camera 21-24. The reference range of flow length is also set according to driving conditions such as right turns and left turns. Note that driving conditions such as right turns and left turns correspond to examples of "movement states of the moving object."
本実施形態では、例えば、前カメラ２１については（図６参照）、フロー長の基準範囲を、最頻値である３０，４０近傍の「２５～４５」とする。後カメラ２２については（図７参照）、フロー長の基準範囲を、最頻値である３０近傍の「２５～３５」とする。右カメラ２３については（図８参照）、フロー長の基準範囲を、最頻値である５０，６０近傍の「４５～６０」とする。 In this embodiment, for example, for the front camera 21 (see Figure 6), the reference range for the flow length is set to "25 to 45," near the mode of 30 and 40. For the rear camera 22 (see Figure 7), the reference range for the flow length is set to "25 to 35," near the mode of 30. For the right camera 23 (see Figure 8), the reference range for the flow length is set to "45 to 60," near the mode of 50 and 60.
すなわち、前カメラ２１および後カメラ２２における左折時のフロー長の基準範囲は、フロー長の最頻値を基準に設定される。「最頻値を基準に」とは、上記例示のように最頻値を中央値として、経験則に基づく任意の幅を上下にとる形態であってもよいし、最頻値から上または下のみに予め試験により定めた値をとる形態であってもよい。つまり、「最頻値」を指標として、基準範囲を設定する任意の形態のことを意味する。以下、「平均値を基準に」についても同義である。 In other words, the reference range for the flow length during a left turn in the front camera 21 and rear camera 22 is set based on the mode of the flow length. "Based on the mode" means that, as in the example above, the mode may be used as the median, with an arbitrary range above and below it based on empirical rules, or it may be a form where values predetermined by testing are taken only above or below the mode. In short, it means any form in which the reference range is set using the "mode" as an indicator. The same applies to "based on the mean" below.
左カメラ２４については（図９参照）、フロー長の基準範囲を、平均値である４０近傍の「２５～４５」とする。つまり、左カメラ２４におけるフロー長の基準範囲は、フロー長の平均値を基準に設定される。基準範囲として、最頻値を指標とするか、平均値を指標とするかについては、ヒストグラムの形態に応じてカメラ２１，２２，２３，２４ごとに適宜選択できる。また、最頻値や平均値からどのくらいの幅を許容範囲とするかは、事前の試験データ等から予め設定される。よって、上記基準範囲の具体的数値については適宜変更可能である。 For the left camera 24 (see Figure 9), the reference range for flow length is set to "25 to 45," near the average value of 40. In other words, the reference range for flow length in the left camera 24 is set based on the average flow length. Whether to use the mode or the average value as the reference range can be appropriately selected for each camera 21, 22, 23, and 24 depending on the histogram configuration. Furthermore, the acceptable range from the mode or average value is predetermined based on prior test data, etc. Therefore, the specific numerical values of the above reference range can be changed as appropriate.
以下、右折時の一例についても説明する。図１０は、車両ＭＣの運動情報を動的に示し、各カメラ２１，２２，２３，２４の位置変化を示す図であり、任意の環境における右折時の形態を示している。図１１～図１４は、フロー長の統計分布を示すヒストグラムである。図１１は、前カメラ２１により取得されたデータに基づいたヒストグラムであり、図１２は、後カメラ２２により取得されたデータに基づいたヒストグラムである。図１３は、右カメラ２３により取得されたデータに基づいたヒストグラムであり、図１４は、左カメラ２４により取得されたデータに基づいたヒストグラムである。 The following describes an example of a right turn. Figure 10 dynamically shows the motion information of the vehicle MC and illustrates the positional changes of each camera 21, 22, 23, and 24, illustrating the form of a right turn in an arbitrary environment. Figures 11 to 14 are histograms showing the statistical distribution of flow length. Figure 11 is a histogram based on data acquired by the front camera 21, and Figure 12 is a histogram based on data acquired by the rear camera 22. Figure 13 is a histogram based on data acquired by the right camera 23, and Figure 14 is a histogram based on data acquired by the left camera 24.
図６～図１４を適宜比較すると分かるように、各カメラ２１～２４に写る物体の見え方は、右折時と左折時においては異なるため、当然、フロー長のヒストグラムはそれぞれ異なる。図１１に示すように、右折時の前カメラ２１によるヒストグラムでは、出現値が５０以下と小さいものが分散している。ヒストグラムがこのような形態の場合には、最頻値を取るよりも、平均値を取る方が好ましい。この場合、例えば、前カメラ２１については、フロー長の基準範囲を、平均値である２０近傍の「１５～２５」とする。 As can be seen by comparing Figures 6 to 14 as appropriate, the appearance of objects captured by each camera 21 to 24 differs between right turns and left turns, and therefore, the histograms of flow length naturally differ. As shown in Figure 11, in the histogram from the front camera 21 during a right turn, values below 50 are dispersed. When the histogram has this form, it is preferable to take the mean rather than the mode. In this case, for example, for the front camera 21, the reference range of flow length is set to "15 to 25," which is close to the mean value of 20.
一方、図１２に示すように、後カメラ２２によるヒストグラムでは、フロー長が２０の出現値が約２６０であり極めて突出して大きく、その他は小さくなっている。ヒストグラムがこのような形態の場合には、フロー長の基準範囲を、最頻値である２０近傍の「１５～２５」とするのが好ましい。同様にして、図１３、図１４に示すように、右カメラ２３および左カメラ２４によるヒストグラムでは、フロー長の基準範囲を、例えば平均値近傍に決定するのが好ましい。 On the other hand, as shown in Figure 12, the histogram from the rear camera 22 shows that the frequency of flow length 20 is approximately 260, which is extremely high, while other values are small. When the histogram has this shape, it is preferable to set the reference range for flow length to "15 to 25," near the mode of 20. Similarly, as shown in Figures 13 and 14, for the histograms from the right camera 23 and left camera 24, it is preferable to determine the reference range for flow length to be, for example, near the mean value.
以上のように、現在フレームにおける、各カメラ２１～２４によるヒストグラムを取得し、自己位置推定に際して、随時、フロー長の基準範囲を決定する。Ｓ１０５において、選別部１５は、作成されたフローのうち、現在フレームと前回フレーム間のフロー長が、基準範囲内にあるものを正しいフローとして選別する。すなわち、フロー長の統計であるヒストグラムから得た基準範囲を、フローの選別の指標としている。上記フロー分布は、車両ＭＣの移動速度および旋回方向に応じて変化する。なお、一般に、上記基準範囲は、車両ＭＣの移動速度が大きいほど大きくなる。また、車両ＭＣの旋回方向と逆に設けられたカメラにおける値の方が大きくなる傾向がある。 As described above, histograms from each camera 21-24 are acquired in the current frame, and a reference range for flow length is determined as needed during self-position estimation. In S105, the selection unit 15 selects from the created flows those whose flow length between the current frame and the previous frame falls within the reference range as correct flows. In other words, the reference range obtained from the histogram, which is a statistical representation of flow length, is used as an indicator for flow selection. The above flow distribution changes according to the vehicle MC's movement speed and turning direction. Generally, the above reference range increases as the vehicle MC's movement speed increases. Also, the value tends to be larger for cameras positioned opposite the vehicle MC's turning direction.
図１５、図１６は、任意の現在フレームの一例を示しており、フレーム内に特徴点およびフローを図示した図である。図１５は、左折中の前カメラ２１により得られたフレームを示している。図１６は、左折中の右カメラ２３により得られたフレームを示している。各図において、複数の白い丸で示すのは現特徴点Ｃであり、特徴点から引き出された黒色の線で示すのは誤フローＦｅであり、特徴点から引き出されたグレーの線で示すのは正フローＦｃである。「誤フロー」とは、Ｓ１０５における制約条件による判定により、制約条件を満たさないと判定されたフローである。「正フロー」とは、Ｓ１０５における制約条件による判定により、制約条件を満たすと判定されたフローである。 Figures 15 and 16 show examples of arbitrary current frames, illustrating feature points and flows within the frame. Figure 15 shows a frame obtained by the front camera 21 during a left turn. Figure 16 shows a frame obtained by the right camera 23 during a left turn. In each figure, the multiple white circles represent current feature points C, the black lines drawn from the feature points represent erroneous flows Fe, and the gray lines drawn from the feature points represent correct flows Fc. An "erroneous flow" is a flow determined not to satisfy the constraint conditions in the S105 constraint condition determination. A "correct flow" is a flow determined to satisfy the constraint conditions in the S105 constraint condition determination.
このように、正フローＦｃと誤フローＦｅとでは相違がある。第１実施形態では、抽出された現特徴点を用いて引いたフローの全てを自己位置推定に用いるのではなく、図１５、図１６に示される正フローＦｃの情報を用いて、車両ＭＣの自己位置推定が算出される。 Thus, there are differences between the positive flow Fc and the erroneous flow Fe. In the first embodiment, instead of using all of the flow drawn using the extracted current feature points for self-localization, the self-localization of the vehicle MC is calculated using the information of the positive flow Fc shown in Figures 15 and 16.
［効果］
（１）上記第１実施形態の自己位置推定装置１によれば、センサ等によらず、マルチカメラとしてのカメラ２０によって撮影した画像のみから、３次元マップを作成することなく車両ＭＣの自己位置を推定できる。このため、自己位置推定装置１における処理時間を短縮できる。また、３次元マップを作成しないため、自己位置推定装置１におけるメモリリソースを削減できる。
[effect]
(1) According to the self-position estimation device 1 of the first embodiment described above, the self-position of the vehicle MC can be estimated from images captured by the camera 20 as a multi-camera, without the use of sensors or the like, and without the creation of a 3D map. Therefore, the processing time in the self-position estimation device 1 can be shortened. In addition, since a 3D map is not created, the memory resources in the self-position estimation device 1 can be reduced.
（２）また、上記第１実施形態の自己位置推定装置１では、３次元マップを作成しないため、画像に対する奥行き方向の情報を現在フレームに反映させることができない。しかし、上記第１実施形態の自己位置推定装置１では、特徴点があると予想される範囲を、エピポーラ線からの所定距離として設定しており、エピポーラ線を基準とする２次元的な予測範囲を探索範囲Ｓとして設定できるため、特徴点の存在する範囲の予測ができ、探索処理時間を短縮することができる。 (2) Furthermore, in the self-localization device 1 of the first embodiment described above, since a three-dimensional map is not created, information in the depth direction of the image cannot be reflected in the current frame. However, in the self-localization device 1 of the first embodiment described above, the range in which feature points are expected to exist is set as a predetermined distance from the epipolar line, and a two-dimensional prediction range based on the epipolar line can be set as the search range S. Therefore, the range in which feature points exist can be predicted, and the search processing time can be shortened.
（３）上記第１実施形態の自己位置推定装置１によれば、車両ＭＣの予測される運動に基づいて計算されたエピポーラ線からの所定距離として予め定められた上限閾値を、フローの選別の指標として含んでいる。そして、選別部１５により、作成されたフローのうち、現特徴点の位置座標が、エピポーラ線からの所定距離として予め定められた上限閾値の範囲内にあるものが、正しいフローとして選別される。このため、車両ＭＣの自己位置推定の精度を向上させることができる。 (3) According to the self-position estimation device 1 of the first embodiment described above, a predetermined upper threshold value, calculated as a predetermined distance from the epipolar line based on the predicted motion of the vehicle MC, is included as an indicator for flow selection. The selection unit 15 then selects from the created flows those in which the position coordinates of the current feature points fall within the predetermined upper threshold value range from the epipolar line as the correct flow. Therefore, the accuracy of the vehicle MC's self-position estimation can be improved.
（４）さらに、上記第１実施形態の自己位置推定装置１によれば、フロー長の統計としてのヒストグラムから得た基準範囲を、フローの選別の指標として含んでいる。そして、選別部１５により、作成されたフローのうち、フロー長が基準範囲内にあるものが、正しいフローとして選別される。このため、車両ＭＣの自己位置推定の精度を向上させることができる。 (4) Furthermore, according to the self-position estimation device 1 of the first embodiment described above, a reference range obtained from a histogram as a statistical representation of flow length is included as an indicator for flow selection. The selection unit 15 then selects from the created flows those whose flow length falls within the reference range as correct flows. Therefore, the accuracy of the vehicle MC's self-position estimation can be improved.
また、上記第１実施形態では、第１条件と第２条件を共に満たすフローのみ正しいフローとして選別し、自己位置推定に用いるため、さらに車両ＭＣの自己位置推定の精度を向上させることができる。 Furthermore, in the first embodiment described above, only flows that satisfy both the first and second conditions are selected as correct flows and used for self-position estimation, thereby further improving the accuracy of the vehicle's MC's self-position estimation.
（５）上記第１実施形態の自己位置推定装置１によれば、第２条件は、前後左右の各カメラ２１～２４において取得されたデータに基づくヒストグラムの形態によって、各カメラ２１～２４において適宜個別に決定される。すなわち、ヒストグラムの形態から、最頻値近傍や平均値近傍など、適切な基準範囲が決定される。このため、より正確にフローの抽出を行うことができる。 (5) According to the self-position estimation device 1 of the first embodiment described above, the second condition is determined individually for each camera 21-24 based on the shape of the histogram obtained from the data acquired by each of the front, rear, left, and right cameras 21-24. That is, an appropriate reference range, such as the vicinity of the mode or the vicinity of the mean, is determined from the shape of the histogram. Therefore, flow extraction can be performed more accurately.
（６）上記第１実施形態の自己位置推定装置１によれば、Ｓ１０２において、等速運動を前提として基本行列を作成しているため、基本行列の算出を容易に行うことができる。 (6) According to the self-position estimation device 1 of the first embodiment described above, since the basic matrix is created in S102 based on the assumption of uniform velocity motion, the calculation of the basic matrix can be easily performed.
（７）上記第１実施形態の自己位置推定装置１によれば、制約条件の判定（Ｓ１０５）を行ったのちに、判定条件をクリアしたフローの数が基準より少ない場合には（Ｓ１０７）、従前の制約条件を緩和して再度判定するようにしている（Ｓ１０８）。このため、自己位置推定に用いるフローの数が過度に少なくなることを回避できる。 (7) According to the self-position estimation device 1 of the first embodiment described above, after determining the constraint conditions (S105), if the number of flows that meet the determination conditions is less than the standard (S107), the previous constraint conditions are relaxed and the determination is made again (S108). Therefore, it is possible to avoid the number of flows used for self-position estimation becoming excessively small.
Ｂ．他の実施形態：
（Ｂ１）上記第１実施形態の自己位置推定装置１は、車両ＭＣに搭載されるものとしたが、その他、船舶やドローン、自律移動型ロボットなどの物体や、人等の移動体に搭載されていてもよい。
B. Other embodiments:
(B1) Although the self-position estimation device 1 of the first embodiment described above is mounted on a vehicle MC, it may also be mounted on other objects such as ships, drones, autonomous mobile robots, or moving objects such as people.
（Ｂ２）上記第１実施形態の自己位置推定装置１において、カメラ２０は、前後左右の４つのカメラで構成したが、２つ以上の複眼カメラであればよい。 (B2) In the self-position estimation device 1 of the first embodiment described above, the camera 20 was composed of four cameras (front, back, left, and right), but any two or more compound cameras will suffice.
（Ｂ３）上記第１実施形態の自己位置推定装置１では、Ｓ１０２において、等速運動を前提として、前回フレームと現在フレームとの間の基本行列を作成したが、動作の予想は、前回計算値の利用の他、車両ＭＣのＣＡＮデータからのステアリング情報等を用いてもよい。 (B3) In the self-position estimation device 1 of the first embodiment described above, in S102, a basic matrix was created between the previous frame and the current frame, assuming constant velocity motion. However, in addition to using the previously calculated values, steering information from the vehicle MC's CAN data may also be used to predict the movement.
（Ｂ４）上記第１実施形態の自己位置推定装置１では、フロー長の基準範囲の決定に際して、ヒストグラム中の最頻値または平均値を利用したが、中央値を利用してもよい。例えば、統計において、フロー長が著しく大きく外れたものが存在するような場合には、平均値ではなく、中央値を適用するなど、適宜対応できる。 (B4) In the self-position estimation device 1 of the first embodiment described above, the mode or mean value in the histogram was used to determine the reference range of the flow length, but the median value may also be used. For example, in statistics, if there are cases where the flow length deviates significantly, the median value may be applied instead of the mean value, or other appropriate measures may be taken.
（Ｂ５）本開示に記載の自己位置推定装置１及びそれら手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の自己位置推定装置１及びそれら手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより実現されてもよい。もしくは、本開示に記載の自己位置推定装置１及びそれら手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組合わせにより構成された一つ以上の専用コンピュータにより実現されてもよい。また、コンピュータプログラム（自己位置推定プログラム）は、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されていてもよい。 (B5) The self-localization device 1 and its methods described in this disclosure may be implemented by a dedicated computer provided by configuring a processor and memory programmed to perform one or more functions embodied by a computer program. Alternatively, the self-localization device 1 and its methods described in this disclosure may be implemented by a dedicated computer provided by configuring a processor by one or more dedicated hardware logic circuits. Alternatively, the self-localization device 1 and its methods described in this disclosure may be implemented by one or more dedicated computers configured by a combination of a processor and memory programmed to perform one or more functions and a processor configured by one or more hardware logic circuits. Furthermore, the computer program (self-localization program) may be stored as instructions executed by the computer on a computer-readable non-transitional tangible recording medium.
本開示は、上述の実施形態に限られるものではなく、その趣旨を逸脱しない範囲において種々の構成で実現することができる。例えば、発明の概要の欄に記載した形態中の技術的特徴に対応する各実施形態中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。 This disclosure is not limited to the embodiments described above, and can be implemented in various configurations without departing from its spirit. For example, the technical features in each embodiment corresponding to the technical features in the embodiments described in the summary of the invention can be replaced or combined as appropriate to solve some or all of the above-described problems, or to achieve some or all of the above-described effects. Furthermore, if a technical feature is not described as essential in this specification, it can be deleted as appropriate.
１…自己位置推定装置、１０…車両制御部、１１…画像データ取得部、１２…特徴点抽出部、１３…記憶部、１４…フロー作成部、１５…選別部、１６…自己位置算出部、２０…カメラ、２１…前カメラ、２２…後カメラ、２３…右カメラ、２４…左カメラ、ＭＣ…車両 1…Self-position estimation device, 10…Vehicle control unit, 11…Image data acquisition unit, 12…Feature point extraction unit, 13…Storage unit, 14…Flow creation unit, 15…Selection unit, 16…Self-position calculation unit, 20…Camera, 21…Front camera, 22…Rear camera, 23…Right camera, 24…Left camera, MC…Vehicle
Claims (14)
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別し、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定装置。 A self-position estimation device for estimating the self-position of a moving object (MC),
An image data acquisition unit (11) acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A feature point extraction unit (12) extracts feature points from the image data acquired by the image data acquisition unit,
A storage unit (13) that stores the positional information of each feature point in each of the aforementioned image data,
A flow creation unit (14) associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A selection unit (15) selects from the flows created by the flow creation unit a flow that satisfies predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, as the correct flow.
The system includes a self-position calculation unit (16) that calculates the position and orientation of the moving body using the correct flow information selected by the sorting unit ,
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The sorting unit selects from the created flows those whose flow length falls within the reference range.
The reference range of the flow length is set individually for each of the multiple cameras in the self-position estimation device.
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別し、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定装置。 A self-position estimation device for estimating the self-position of a moving object (MC),
An image data acquisition unit (11) acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A feature point extraction unit (12) extracts feature points from the image data acquired by the image data acquisition unit,
A storage unit (13) that stores the positional information of each feature point in each of the aforementioned image data,
A flow creation unit (14) associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A selection unit (15) selects from the flows created by the flow creation unit a flow that satisfies predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, as the correct flow.
The system includes a self-position calculation unit (16) that calculates the position and orientation of the moving body using the correct flow information selected by the sorting unit ,
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The sorting unit selects from the created flows those whose flow length falls within the reference range.
The reference range of the flow length is set according to the movement state of the moving body in the self-position estimation device.
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記選別部は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する、自己位置推定装置。 A self-position estimation device for estimating the self-position of a moving object (MC),
An image data acquisition unit (11) acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A feature point extraction unit (12) extracts feature points from the image data acquired by the image data acquisition unit,
A storage unit (13) that stores the positional information of each feature point in each of the aforementioned image data,
A flow creation unit (14) associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A selection unit (15) selects from the flows created by the flow creation unit a flow that satisfies predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, as the correct flow.
The system includes a self-position calculation unit (16) that calculates the position and orientation of the moving body using the correct flow information selected by the sorting unit ,
The sorting unit, if the number of selected correct flows is less than a predetermined standard, performs the sorting of flows again using the constraints changed from the previously used constraints until the number of correct flows is equal to or greater than the standard, in a self-localization device.
前記選別部は、作成された前記フローのうち、前記現特徴点の位置座標が、前記最新の画像データ内において前記エピポーラ線から前記上限閾値の範囲内にあるものを選別する、請求項１から請求項３のいずれか一項に記載の自己位置推定装置。 The aforementioned constraints include a predetermined upper threshold value, which is calculated as the distance from the epipolar line based on the predicted motion of the moving object, as an indicator for selecting the flow.
The self-localization device according to any one of claims 1 to 3, wherein the selection unit selects from the created flow the one in which the position coordinates of the current feature point are within the range of the epipolar line to the upper threshold in the latest image data.
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する工程は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する工程を含み、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定方法。 A self-position estimation method for estimating the self-position of a moving object (MC) using a self-position estimation device,
The self-localization device comprises an image data acquisition unit (11), a feature point extraction unit (12), a storage unit (13), a flow creation unit (14), a sorting unit (15), and a self-localization calculation unit (16).
The image data acquisition unit acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the mobile body at set intervals.
The feature point extraction unit performs the step of extracting feature points from the image data acquired by the image data acquisition unit,
The memory unit performs the step of storing the positional information of each feature point in each of the image data,
The process involves the flow creation unit associating current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, to create a flow, which is a set of corresponding feature points in different image data for each time series.
The selection unit selects from the flows created by the flow creation unit the flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, and identifies them as correct flows.
The self-position calculation unit calculates the position and orientation of the moving body using the correct flow information selected by the selection unit ,
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The selection process includes selecting from the created flows those whose flow length falls within the reference range.
A self-position estimation method in which the reference range of the flow length is set individually for each of the multiple cameras .
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する工程は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する工程を含み、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定方法。 A self-position estimation method for estimating the self-position of a moving object (MC) using a self-position estimation device,
The self-localization device comprises an image data acquisition unit (11), a feature point extraction unit (12), a storage unit (13), a flow creation unit (14), a sorting unit (15), and a self-localization calculation unit (16).
The image data acquisition unit acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the mobile body at set intervals.
The feature point extraction unit performs the step of extracting feature points from the image data acquired by the image data acquisition unit,
The memory unit performs the step of storing the positional information of each feature point in each of the image data,
The process involves the flow creation unit associating current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, to create a flow, which is a set of corresponding feature points in different image data for each time series.
The selection unit selects from the flows created by the flow creation unit the flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, and identifies them as correct flows.
The self-position calculation unit calculates the position and orientation of the moving body using the correct flow information selected by the selection unit ,
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The selection process includes selecting from the created flows those whose flow length falls within the reference range.
A self-position estimation method in which the reference range of the flow length is set according to the movement state of the moving body .
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記選別する工程は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する工程を含む、自己位置推定方法。 A self-position estimation method for estimating the self-position of a moving object (MC) using a self-position estimation device,
The self-localization device comprises an image data acquisition unit (11), a feature point extraction unit (12), a storage unit (13), a flow creation unit (14), a sorting unit (15), and a self-localization calculation unit (16).
The image data acquisition unit acquires image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the mobile body at set intervals.
The feature point extraction unit performs the step of extracting feature points from the image data acquired by the image data acquisition unit,
The memory unit performs the step of storing the positional information of each feature point in each of the image data,
The process involves the flow creation unit associating current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, to create a flow, which is a set of corresponding feature points in different image data for each time series.
The selection unit selects from the flows created by the flow creation unit the flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, and identifies them as correct flows.
The process includes a step of using the self-position calculation unit to calculate the position and orientation of the moving body using the correct flow information selected by the selection unit ,
A self-localization method comprising the selection step, which, if the number of selected correct flows is less than a predetermined standard, repeats the selection of flows using constraints modified from the previously used constraints until the number of correct flows equals or exceeds the standard .
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する機能は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する機能を含み、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定プログラム。 A self-position estimation program for estimating the self-position of a moving object (MC),
The function includes acquiring image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A function to extract feature points from the aforementioned image data,
A function for storing the positional information of each feature point in each of the aforementioned image data,
A function that associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A function to select from the created flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving object, as the correct flows,
The computer is provided with a function to calculate the position and orientation of the moving object using the selected correct flow information .
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The aforementioned selection function includes a function for selecting from the created flows those whose flow length falls within the aforementioned reference range.
The reference range of the flow length is set individually for each of the multiple cameras by a self-localization program.
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する機能は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する機能を含み、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定プログラム。 A self-position estimation program for estimating the self-position of a moving object (MC),
The function includes acquiring image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A function to extract feature points from the aforementioned image data,
A function to store the positional information of each feature point in each of the aforementioned image data,
A function that associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A function to select from the created flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, as the correct flows,
The computer is provided with a function to calculate the position and orientation of the moving object using the selected correct flow information .
The aforementioned constraints include a reference range for flow length, which is the length of the flow in the continuous image data in the time series, as an indicator for selecting the flow.
The aforementioned selection function includes a function for selecting from the created flows those whose flow length falls within the aforementioned reference range.
The reference range of the flow length is set according to the movement state of the moving object , and is a self-position estimation program.
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記選別する機能は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する機能を含む、自己位置推定プログラム。 A self-position estimation program for estimating the self-position of a moving object (MC),
The function includes acquiring image data output from a multi-camera (20) consisting of multiple cameras (21, 22, 23, 24) mounted on the aforementioned mobile body at set intervals,
A function to extract feature points from the aforementioned image data,
A function to store the positional information of each feature point in each of the aforementioned image data,
A function that associates current feature points, which are feature points in the latest image data, with previous feature points, which are feature points in past image data, and creates a flow, which is a set of corresponding feature points in the image data that differs for each time series.
A function to select from the created flows that satisfy predetermined constraints, including epipolar constraints calculated based on the predicted motion of the moving body, as the correct flows,
The computer is provided with a function to calculate the position and orientation of the moving object using the selected correct flow information .
The self-localization program includes a function that, if the number of selected correct flows is less than a predetermined standard, repeats the selection of flows using modified constraints from those previously used until the number of correct flows equals or exceeds the standard .
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2022169676A JP7845985B2 (en) | 2022-10-24 | 2022-10-24 | Self-localization device, self-localization method, and self-localization program |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2022169676A JP7845985B2 (en) | 2022-10-24 | 2022-10-24 | Self-localization device, self-localization method, and self-localization program |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2024061982A JP2024061982A (en) | 2024-05-09 |
| JP7845985B2 true JP7845985B2 (en) | 2026-04-14 |
Family
ID=90970479
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2022169676A Active JP7845985B2 (en) | 2022-10-24 | 2022-10-24 | Self-localization device, self-localization method, and self-localization program |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7845985B2 (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010085240A (en) | 2008-09-30 | 2010-04-15 | Mazda Motor Corp | Image processing device for vehicle |
| JP2021193340A (en) | 2020-06-08 | 2021-12-23 | フォルシアクラリオン・エレクトロニクス株式会社 | Self-position estimation device |
| JP2022064506A (en) | 2020-10-14 | 2022-04-26 | Ｎｅｃソリューションイノベータ株式会社 | Image processing device, image processing method, and program |
-
2022
- 2022-10-24 JP JP2022169676A patent/JP7845985B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010085240A (en) | 2008-09-30 | 2010-04-15 | Mazda Motor Corp | Image processing device for vehicle |
| JP2021193340A (en) | 2020-06-08 | 2021-12-23 | フォルシアクラリオン・エレクトロニクス株式会社 | Self-position estimation device |
| JP2022064506A (en) | 2020-10-14 | 2022-04-26 | Ｎｅｃソリューションイノベータ株式会社 | Image processing device, image processing method, and program |
Non-Patent Citations (1)
| Title |
|---|
| 高橋 健大 他，画像処理を用いた屋内駐車場における自車両の高精度方位変化推定，情報処理学会 研究報告 高度交通システムとスマートコミュニティ（ＩＴＳ） ２０１９－ＩＴＳ－０７６ ［ｏｎｌｉｎｅ］，情報処理学会，2019年02月21日 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2024061982A (en) | 2024-05-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112292711B (en) | Correlating LIDAR and image data | |
| CN110325818B (en) | Joint 3D object detection and orientation estimation via multimodal fusion | |
| CN111081064B (en) | Automatic parking system and automatic passenger-replacing parking method of vehicle-mounted Ethernet | |
| US9443309B2 (en) | System and method for image based mapping, localization, and pose correction of a vehicle with landmark transform estimation | |
| CN111223135A (en) | System and method for enhancing range estimation by monocular camera using radar and motion data | |
| US20230109473A1 (en) | Vehicle, electronic apparatus, and control method thereof | |
| WO2017051480A1 (en) | Image processing device and image processing method | |
| CN114283401B (en) | Obstacle identification method and device, and driving method and device | |
| DE102021111536A1 (en) | METHOD AND SYSTEM FOR GROUND SURFACE PROJECTION FOR AUTONOMOUS DRIVING | |
| WO2023017624A1 (en) | Drive device, vehicle, and method for automated driving and/or assisted driving | |
| CN114648639A (en) | Target vehicle detection method, system and device | |
| EP4174777B1 (en) | Apparatus for accelerating simultaneous localization and mapping and electronic device including the same | |
| CN116740681A (en) | Target detection method, device, vehicle and storage medium | |
| JP7845985B2 (en) | Self-localization device, self-localization method, and self-localization program | |
| US12080013B2 (en) | Multi-view depth estimation leveraging offline structure-from-motion | |
| US11210535B1 (en) | Sensor fusion | |
| US20240329655A1 (en) | Information processing device, movable apparatus, information processing method, and storage medium | |
| US20240078749A1 (en) | Method and apparatus for modeling object, storage medium, and vehicle control method | |
| CN116645663B (en) | Monocular three-dimensional target real-time detection method applied to intelligent driving | |
| US12608843B2 (en) | Self-position estimation device | |
| JP7574780B2 (en) | Autonomous vehicle and control device for autonomous vehicle | |
| CN117818587B (en) | Vehicle control method, apparatus, computer-readable storage medium, and electronic device | |
| JP7564742B2 (en) | Information processing device and information processing method | |
| CN114973182B (en) | Method and system for determining distance of object | |
| EP4718030A1 (en) | Propagation of uncertainties in map-based agent pose estimation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20250113 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20250917 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20251111 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20251216 |
|
| 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: 20260310 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20260402 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7845985 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |