JP2020125960A - Moving object position estimating device and moving object position estimating program - Google Patents
Moving object position estimating device and moving object position estimating program Download PDFInfo
- Publication number
- JP2020125960A JP2020125960A JP2019018117A JP2019018117A JP2020125960A JP 2020125960 A JP2020125960 A JP 2020125960A JP 2019018117 A JP2019018117 A JP 2019018117A JP 2019018117 A JP2019018117 A JP 2019018117A JP 2020125960 A JP2020125960 A JP 2020125960A
- Authority
- JP
- Japan
- Prior art keywords
- map
- coordinate system
- feature points
- points
- point group
- 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.)
- Pending
Links
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Navigation (AREA)
Abstract
Description
本発明は、移動***置推定装置、及び移動***置推定プログラムに関する。 The present invention relates to a moving body position estimating device and a moving body position estimating program.
移動体に搭載されたカメラ等の撮影部によって撮影された撮影画像に基づいて、３次元空間上における移動体の位置を推定する技術が提案されている。 There has been proposed a technique for estimating the position of a mobile body in a three-dimensional space based on a captured image captured by a capturing unit such as a camera mounted on the mobile body.
例えば、移動体の位置を推定する技術として、撮影画像から取得した構造物の特徴を地図に登録し、運転時に取得した画像の構造物の特徴点と登録した地図の特徴点と、を照合して自車の位置を推定する技術がある（特許文献１参照）。この技術では、事前に走行した際の画像から構造物の特徴点を抽出し、その位置を推定して地図に登録し、運転時に画像から構造物の特徴点を抽出し、地図に登録された構造物の特徴点と照合することにより、移動体の位置情報を算出している。 For example, as a technique for estimating the position of a moving body, the features of a structure acquired from a captured image are registered in a map, and the feature points of the structure of the image acquired during driving are compared with the registered feature points of the map. There is a technique for estimating the position of the own vehicle (see Patent Document 1). In this technology, the feature points of the structure are extracted from the image when the vehicle was driven in advance, their positions are estimated and registered in the map, and the feature points of the structure are extracted from the image during driving and registered in the map. The position information of the moving body is calculated by collating with the feature points of the structure.
しかしながら、特許文献１に記載の技術では、樹木等の構造物では、特徴点が密集して検出されてしまう。この場合、撮影画像の樹木の特徴点と、地図上の樹木の特徴点との照合において局所解となってしまう場合があり、誤認識することがある。そのため、特許文献１の技術では、移動体の位置を必ずしも精度よく推定できるとは限らなかった。 However, in the technique described in Patent Document 1, feature points are densely detected in a structure such as a tree. In this case, there may be a local solution in matching the feature points of the tree of the captured image with the feature points of the tree on the map, which may result in erroneous recognition. Therefore, the technique of Patent Document 1 cannot always accurately estimate the position of the moving body.
本開示は、以上の事情を鑑みて成されたものであり、構造物から密集した特徴点が検出される場合であっても、精度良く移動体の位置を推定することができる移動***置推定装置、及び移動***置推定プログラムを提供することを目的とする。 The present disclosure has been made in view of the above circumstances, and is capable of accurately estimating the position of a moving body even when dense feature points are detected from a structure, the moving body position estimation An object is to provide an apparatus and a moving body position estimation program.
上記目的を達成するために、請求項１に記載の移動***置推定装置は、移動体から画像を撮影する撮影部と、撮影部により撮影された画像から複数の特徴点を検出する検出部と、検出部により検出された複数の特徴点を含む測定点群と、画像を撮影した際の移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する取得部と、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点に置換し、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図上の移動体の位置を推定する推定部と、を備えている。 In order to achieve the above-mentioned object, the moving body position estimating device according to claim 1 includes a photographing unit that photographs an image from the moving body, and a detector that detects a plurality of feature points from the image photographed by the photographing unit. , A creation unit that creates a measurement coordinate system having a measurement point group including a plurality of feature points detected by the detection unit, and a moving body point that indicates the position of the moving body when an image is captured; Of a plurality of feature points included in the measurement point group and an acquisition unit that acquires a map coordinate system having a map point group, including a plurality of feature points detected in advance from each of a plurality of locations on a map that can move A plurality of related feature points are replaced with representative points given a first weight value according to the density, and related feature points among the plurality of feature points included in the map point group are changed according to the density. The first weight value and the second weight value, the characteristic points and representative points included in the measurement point group, and the characteristic points and representative points included in the map point group are replaced with the representative points given the second weight value. From the mutual distance between and, derive a homogeneous transformation matrix for matching the measurement coordinate system to the map coordinate system, apply the moving body point of the measurement coordinate system to the map coordinate system by the derived homogeneous transformation matrix, And an estimation unit that estimates the position of the moving body on the map.
また、請求項２に記載の発明は、請求項１に記載の発明において、推定部が、測定点群及び地図点群間の任意の対応する特徴点の組又は代表点の組において、特徴点又は代表点が整合する同次変換行列を導出することを、導出した同次変換行列を測定点群及び地図点群間の対応する特徴点の組及び代表点の組に適用した時の特徴点間の距離及び代表点間の距離と前記第一の重み値及び前記第二の重み値とを用いた評価関数が所定の条件を満たすまで、繰り返し処理を行う。 Further, in the invention described in claim 2, in the invention described in claim 1, the estimation unit is a feature point in a set of arbitrary corresponding feature points or a set of representative points between the measurement point group and the map point group. Alternatively, deriving a homogeneous transformation matrix with which the representative points match is applied to the feature points when the derived homogeneous transformation matrix is applied to the set of corresponding feature points and the set of representative points between the measurement point group and the map point group. The iterative process is repeated until the evaluation function using the distance between the points and the distance between the representative points and the first weight value and the second weight value satisfies a predetermined condition.
また、請求項３に記載の発明は、請求項１又は２の発明において、推定部が、作成部で作成した測定座標系に含まれる特徴点と取得部から取得した地図座標系に含まれる特徴点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列の初期値を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図座標系上の移動体の初期位置を推定する。 The invention according to claim 3 is the invention according to claim 1 or 2, wherein the estimating unit includes a feature point included in the measurement coordinate system created by the creating unit and a feature point included in the map coordinate system acquired from the acquiring unit. The initial value of the homogeneous transformation matrix for matching the measurement coordinate system to the map coordinate system is derived from the mutual distance with the point, and the moving body point of the measurement coordinate system is set to the map coordinate system by the derived homogeneous transformation matrix. It is applied to estimate the initial position of the moving body on the map coordinate system.
また、請求項４に記載の発明は、請求項１から３の何れか一項に記載の発明において、推定部が、代表点の重み値が閾値を超えた場合、前述の同次変換行列の導出に代えて、測定点群に含まれる特徴点及び重み値が閾値以下の代表点と前記地図点群に含まれる特徴点及び重み値が閾値以下の代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出する。 In the invention according to claim 4, in the invention according to any one of claims 1 to 3, when the weighting value of the representative point exceeds a threshold value, Instead of derivation, from the mutual distance between the characteristic points included in the measurement point group and the representative points whose weight value is less than or equal to a threshold and the characteristic points included in the map point group and the representative points whose weight value is less than or equal to the threshold value, the measurement Deriving a homogeneous transformation matrix for matching the coordinate system with the map coordinate system.
また、請求項５に記載の発明は、請求項１から４の何れか一項に記載の発明において、測定座標系及び地図座標系が、２次元平面に表される座標系である。 In the invention described in claim 5, in the invention described in any one of claims 1 to 4, the measurement coordinate system and the map coordinate system are coordinate systems represented on a two-dimensional plane.
一方、上記目的を達成するために、請求項６に記載の移動***置推定プログラムは、移動体に搭載された撮影部により画像を撮影するステップと、撮影部により撮影された画像から複数の特徴点を検出するステップと、検出部により検出された複数の特徴点を含む測定点群と、画像を撮影した際の移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得するステップと、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点を置換し、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図上の移動体の位置を推定するステップと、をコンピュータに実行させる。 On the other hand, in order to achieve the above-mentioned object, the moving body position estimation program according to claim 6 has a step of photographing an image by a photographing unit mounted on the moving body, and a plurality of characteristics from the images photographed by the photographing unit. Creating a measurement coordinate system having a step of detecting points, a measurement point group including a plurality of feature points detected by the detection unit, and a moving body point indicating the position of the moving body when the image is captured Part and a step of acquiring a map coordinate system having a map point group including a plurality of feature points detected in advance from each of a plurality of locations on the map where the mobile body can move; Of the plurality of feature points related to each other are replaced with the representative point given the first weight value according to the density, and the related feature points among the plurality of feature points included in the map point group are , A representative point given a second weight value according to the density, and a first weight value and a second weight value, and characteristic points and representative points included in the measurement point group and included in the map point group. From the mutual distance between the feature point and the representative point, derive a homogeneous transformation matrix for matching the measurement coordinate system with the map coordinate system, and use the derived homogeneous transformation matrix to determine the moving body point of the measurement coordinate system as the map coordinate system. And estimating the position of the mobile body on the map, and causing the computer to execute the following steps.
本開示によれば、構造物の特徴点が密集している場合であっても、精度良く移動体の位置を推定することができる。 According to the present disclosure, it is possible to accurately estimate the position of a moving body even when feature points of a structure are densely arranged.
以下、図面を参照して、本開示の技術を実施するための形態例を詳細に説明する。なお、本実施形態は、移動体が車両である場合を例示する。しかし、移動体は、車両に限定されない。移動体は、移動する物体であれば、飛行する物体であってもよい。以下、車両に搭載された移動***置推定装置について詳細に説明する。 Hereinafter, an example of a mode for carrying out the technology of the present disclosure will be described in detail with reference to the drawings. In addition, this embodiment exemplifies the case where the moving body is a vehicle. However, the moving body is not limited to the vehicle. The moving body may be a flying object as long as it is a moving object. Hereinafter, the moving body position estimation device mounted on the vehicle will be described in detail.
図１は、本実施形態に係る車両１０の一例を示す側面図である。図１に示すように、車両１０は、カメラ１１を備えている。 FIG. 1 is a side view showing an example of a vehicle 10 according to the present embodiment. As shown in FIG. 1, the vehicle 10 includes a camera 11.
カメラ１１は、車両１０の後部のトランク等に設けられ、車両後方を撮影する。カメラ１１は、例えば、車幅方向の略中央部付近に設けられ、かつカメラ１１の光軸が水平方向より若干下側を向くように配置されている。なお、本実施形態では、カメラ１１を車両１０の後部に設ける形態を例示して説明する。しかし、これに限られず、環境等に応じて例えば前部又は側部に設けてもよい。また、本実施形態では、カメラ１１として単眼カメラを例示して説明する。しかし、これに限らず他の形態のカメラ、例えばステレオカメラ等であってもよい。 The camera 11 is provided in the trunk or the like at the rear of the vehicle 10 and photographs the rear of the vehicle. The camera 11 is provided, for example, in the vicinity of a substantially central portion in the vehicle width direction, and is arranged such that the optical axis of the camera 11 faces slightly below the horizontal direction. In addition, in this embodiment, the form which provides the camera 11 in the rear part of the vehicle 10 is illustrated and demonstrated. However, the present invention is not limited to this, and may be provided in the front portion or the side portion depending on the environment and the like. In addition, in the present embodiment, a monocular camera is exemplified as the camera 11 for description. However, the present invention is not limited to this, and other types of cameras such as a stereo camera may be used.
図２は、本実施形態に係る移動***置推定装置２０のハードウェア構成の一例を示すブロック図である。図２に示すように、本実施形態に係る移動***置推定装置２０は、車両１０に搭載され、カメラ１１、表示部１２、ＣＰＵ（Central Processing Unit）１３、ＲＡＭ（Random Access Memory）１４、ＲＯＭ（Read Only Memory）１５、及びストレージ１６を含んで構成されている。カメラ１１、表示部１２、ＣＰＵ１３、ＲＡＭ１４、ＲＯＭ１５、及びストレージ１６の各々はバス１７により相互に接続されている。 FIG. 2 is a block diagram showing an example of the hardware configuration of the moving body position estimation device 20 according to this embodiment. As shown in FIG. 2, the moving body position estimation apparatus 20 according to the present embodiment is mounted on a vehicle 10, and includes a camera 11, a display unit 12, a CPU (Central Processing Unit) 13, a RAM (Random Access Memory) 14, and a ROM. (Read Only Memory) 15 and a storage 16 are included. The camera 11, the display unit 12, the CPU 13, the RAM 14, the ROM 15, and the storage 16 are connected to each other by a bus 17.
表示部１２は、カメラ１１が撮影した画像等を表示する。表示部１２は、画像を表示できる限りいかなる装置であってもよい。表示部１２は、例えば、液晶モニタ、ＣＲＴ（Cathode Ray Tube）モニタ、ＦＰＤ（Flat Panel Display）モニタ等である。 The display unit 12 displays an image or the like taken by the camera 11. The display unit 12 may be any device as long as it can display an image. The display unit 12 is, for example, a liquid crystal monitor, a CRT (Cathode Ray Tube) monitor, an FPD (Flat Panel Display) monitor, or the like.
ＣＰＵ１３は、移動***置推定装置２０の全体を統括、制御する。ＲＡＭ１４は、各種プログラムの実行時のワークエリアとして用いられるメモリである。ＲＯＭ１５は、本実施形態で用いる移動体の位置を推定する移動***置推定プログラム１５Ａを含む各種プログラムやデータ等を記憶している。ＣＰＵ１３は、ＲＯＭ１５に記憶されたプログラムをＲＡＭ１４に展開して実行することにより、地図の生成や、移動体の位置の推定を行う。 The CPU 13 controls and controls the entire mobile body position estimation device 20. The RAM 14 is a memory used as a work area when various programs are executed. The ROM 15 stores various programs including the moving body position estimation program 15A for estimating the position of the moving body used in this embodiment, data, and the like. The CPU 13 develops a program stored in the ROM 15 in the RAM 14 and executes the program to generate a map and estimate the position of the moving body.
ストレージ１６には、本実施形態に係る移動体の位置を推定する処理に用いる地図情報１６Ａ等を格納している。ストレージ１６は、一例としてＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、フラッシュメモリ等である。なお、ストレージ１６にはＣＰＵ１３によって生成した地図情報１６Ａの他に、移動***置推定プログラム１５Ａ等を記憶してもよい。 The storage 16 stores map information 16A and the like used for the process of estimating the position of the moving body according to the present embodiment. The storage 16 is, for example, a HDD (Hard Disk Drive), an SSD (Solid State Drive), a flash memory, or the like. In addition to the map information 16A generated by the CPU 13, the storage 16 may store a moving body position estimation program 15A and the like.
なお、本実施形態では、カメラ１１及び移動***置推定装置２０が車両１０に搭載されている形態について説明したが、これに限定されない。車両１０に搭載されたカメラ１１によって撮影された画像を、通信回線等を通して別の場所に設置されている移動***置推定装置２０に送信して移動体の位置を推定する処理を行ってもよい。 In addition, in this embodiment, although the camera 11 and the moving body position estimation apparatus 20 were mounted in the vehicle 10, the present invention is not limited to this. An image captured by the camera 11 mounted on the vehicle 10 may be transmitted to a moving body position estimating device 20 installed at another place through a communication line or the like to perform a process of estimating the position of the moving body. ..
次に、移動体値推定装置２０の機能構成について説明する。図３は、本実施形態に係る移動***置推定装置２０の機能的な構成の一例を示すブロック図である。図４は、本実施形態に係る座標系の一例を示す図である。図４（ａ）は、測定座標系の一例を模式的に示す図であり、図４（ｂ）は、地図座標系の一例を模式的に示す図である。 Next, the functional configuration of the moving body value estimation device 20 will be described. FIG. 3 is a block diagram showing an example of a functional configuration of the moving body position estimation device 20 according to this embodiment. FIG. 4 is a diagram showing an example of the coordinate system according to the present embodiment. FIG. 4A is a diagram schematically showing an example of the measurement coordinate system, and FIG. 4B is a diagram schematically showing an example of the map coordinate system.
図３に示すように、移動***置推定装置２０は、撮影部２１、検出部２２、作成部２３、取得部２４、及び推定部２５を有する。ＣＰＵ１３が移動***置推定プログラム１５Ａを実行することで、撮影部２１、検出部２２、作成部２３、取得部２４、及び推定部２５として機能する。 As shown in FIG. 3, the moving body position estimation device 20 includes a photographing unit 21, a detection unit 22, a creation unit 23, an acquisition unit 24, and an estimation unit 25. When the CPU 13 executes the moving body position estimation program 15A, the CPU 13 functions as the imaging unit 21, the detection unit 22, the creation unit 23, the acquisition unit 24, and the estimation unit 25.
撮影部２１は、車両１０から画像を撮影する。具体的には、撮影部２１は、車両１０に搭載されたカメラ１１を制御して、カメラ１１により車両１０から画像を撮影する。 The image capturing unit 21 captures an image from the vehicle 10. Specifically, the image capturing unit 21 controls the camera 11 mounted on the vehicle 10 so that the camera 11 captures an image from the vehicle 10.
検出部２２は、撮影部２１により撮影された画像から複数の特徴点を検出する。検出部２２は、一例として、建物の凹凸に基づく陰影、壁面の模様等、撮影画像において輝度の濃淡差が所定値よりも大きい点を、特徴点として検出する。 The detection unit 22 detects a plurality of feature points from the image captured by the image capturing unit 21. As an example, the detection unit 22 detects, as a feature point, a point in the photographed image in which the difference in brightness is larger than a predetermined value, such as a shadow based on the unevenness of the building or a pattern on the wall surface.
作成部２３は、検出部２２により検出された複数の特徴点を含む測定点群と、画像を撮影した際の車両１０の位置を示す移動体点と、を有する測定座標系を作成する。測定座標系とは、例えば、図４（ａ）に示すような座標系Σｓであり、Ｘｓ軸、Ｙｓ軸、及びＺｓ軸の３次元により表される系である。 The creation unit 23 creates a measurement coordinate system having a measurement point group including a plurality of feature points detected by the detection unit 22 and a moving body point indicating the position of the vehicle 10 when the image is captured. The measurement coordinate system is, for example, a coordinate system Σs as shown in FIG. 4A, which is a system represented by three dimensions of an Xs axis, a Ys axis, and a Zs axis.
なお、本実施形態では、一例として、座標系は右手系の同次座標系を用いて表される形態について説明する。同次座標系とは、Ｘ軸、Ｙ軸、及びＺ軸の３次元ベクトルに１又は−1を付加して表す形式である。右手系の座標系の場合は、１を付加して（Ｘ、Ｙ、Ｚ、１）で表され、左手系の座標系の場合は、−1を付加して（Ｘ、Ｙ、Ｚ、−1）で表される。 In the present embodiment, as an example, a form in which the coordinate system is represented by using a right-handed homogeneous coordinate system will be described. The homogeneous coordinate system is a format in which 1 or −1 is added to a three-dimensional vector of X axis, Y axis, and Z axis. In the case of the right-handed coordinate system, 1 is added to be represented by (X, Y, Z, 1), and in the case of the left-handed coordinate system, -1 is added (X, Y, Z, -). It is represented by 1).
取得部２４は、車両１０が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する。取得部２４は、予めストレージ１６から地図座標系を取得してもよいし、車両１０の外部から通信により地図座標系を取得してもよい。地図座標系とは、例えば、図４（ｂ）に示すような座標系Σｍであり、Ｘｍ軸、Ｙｍ軸及びＺｍ軸の３次元により表される系である。地図座標系は、例えば、次のように作成される。 The acquisition unit 24 acquires a map coordinate system having a map point group including a plurality of feature points detected in advance from each of a plurality of locations on the map where the vehicle 10 can move. The acquisition unit 24 may acquire the map coordinate system from the storage 16 in advance, or may acquire the map coordinate system by communication from outside the vehicle 10. The map coordinate system is, for example, a coordinate system Σm as shown in FIG. 4B, and is a system represented by three dimensions of an Xm axis, a Ym axis, and a Zm axis. The map coordinate system is created as follows, for example.
車両１０が走行しうる経路を別の車両又は車両１０が予め走行する。以下では、車両１０とは別の車両が走行する場合について、説明する。ここで、別の車両は、車両１０と同様にカメラを搭載している。車両に搭載されたカメラにより経路上の画像を撮像する。カメラが撮影した画像から特徴点の座標（３次元位置）と画像を撮影したカメラの位置を推定する。これらの推定は、例えばＶｉｓｕａｌ ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて行うことができる。 Another vehicle or the vehicle 10 travels in advance on a route on which the vehicle 10 can travel. Hereinafter, a case where a vehicle different from the vehicle 10 travels will be described. Here, another vehicle is equipped with a camera, like the vehicle 10. An image on the route is captured by a camera mounted on the vehicle. From the image captured by the camera, the coordinates (three-dimensional position) of the feature point and the position of the camera capturing the image are estimated. These estimations can be performed using, for example, Visual SLAM (Simultaneous Localization and Mapping).
またオドメトリー法を適用してカメラにより撮影された画像から計測した車両の位置及び移動量、構造物までの距離及び大きさ等の情報が座標系に反映される。オドメトリー法とは、車両の車輪回転数や加速度等を計測するセンサーから取得した情報、及びカメラ１１により撮影された画像等を用いて、移動体の位置及び移動量、構造物までの距離及び大きさ等を計測する手法である。 In addition, information such as the position and movement amount of the vehicle, the distance to the structure, and the size measured from the image captured by the camera by applying the odometry method is reflected in the coordinate system. The odometry method uses the information acquired from the sensor that measures the vehicle wheel rotation speed, acceleration, etc., and the image captured by the camera 11, etc., to determine the position and amount of movement of the moving body, and the distance and size to the structure. This is a method of measuring the size.
すなわち、撮影した画像から、Ｖｉｓｕａｌ ＳＬＡＭを用いて特徴点の３次元位置と撮影したカメラ１１の位置とが推定され、これらの推定された位置と推定された位置における特徴点を地図座標系に登録する。また、オドメトリー法を用いて地図座標系に構造物の大きさ及び車両から構造物までの距離等の情報が反映される。ストレージ１６に地図座標系を地図情報１６Ａとして予め記憶される。 That is, from the photographed image, the three-dimensional position of the feature point and the position of the photographed camera 11 are estimated using the Visual SLAM, and these estimated position and the feature point at the estimated position are registered in the map coordinate system. To do. In addition, information such as the size of the structure and the distance from the vehicle to the structure is reflected in the map coordinate system using the odometry method. The map coordinate system is stored in the storage 16 as map information 16A in advance.
なお、本実施形態では、地図座標系は、予め作成されてストレージ１６に記憶されている形態として説明した。しかし、これに限定されない。地図座標系は、地図情報１６Ａとしてストレージ１６に記憶された画像を用いて測定座標系を作成する際に同時に作成される形態としてもよい。 In the present embodiment, the map coordinate system has been described as being created in advance and stored in the storage 16. However, it is not limited to this. The map coordinate system may be simultaneously created when the measurement coordinate system is created using the image stored in the storage 16 as the map information 16A.
また、本実施形態では、特徴点の３次元位置と撮影したカメラ１１の位置との推定にＳＬＡＭを適用した例について説明した。しかし、これに限定されない。特徴点の検出方法として、ＳｆＭ（Structure from Motion）を適用してもよい。ＳｆＭは、カメラ１１で撮影した画像を用いて、周囲の構造物の幾何学的形状及びカメラ１１の移動の軌跡を同時に復元する技術である。 Further, in the present embodiment, the example in which the SLAM is applied to the estimation of the three-dimensional position of the feature point and the position of the camera 11 that has captured the image has been described. However, it is not limited to this. As a feature point detection method, SfM (Structure from Motion) may be applied. The SfM is a technique that simultaneously restores the geometrical shape of the surrounding structure and the movement trajectory of the camera 11 using the image captured by the camera 11.
推定部２５は、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を密度に応じた第一の重み値を付与した代表点に置換する。推定部２５は、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換する。そして、推定部２５は、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出する。推定部２５は、導出した同次変換行列を測定座標系の移動体点に適用して、地図座標系の移動体の位置を推定する。 The estimation unit 25 replaces the plurality of related feature points among the plurality of feature points included in the measurement point group with the representative point to which the first weight value according to the density is assigned. The estimation unit 25 replaces the related feature points among the plurality of feature points included in the map point group with the representative point to which the second weight value according to the density is given. Then, the estimation unit 25 calculates the first weight value and the second weight value, and the mutual distances between the feature points and representative points included in the measurement point group and the feature points and representative points included in the map point group, Derive a homogeneous transformation matrix to match the measurement coordinate system to the map coordinate system. The estimating unit 25 applies the derived homogeneous transformation matrix to the moving body point in the measurement coordinate system to estimate the position of the moving body in the map coordinate system.
次に、移動***置推定装置２０の作用について説明する前に、移動体の位置を推定するための手法を説明する。本実施形態では、一例として、ＩＣＰ（Interactive Closest Point）法を適用して、車両１０の位置の推定を行う手法について説明する。 Next, before describing the operation of the moving body position estimating device 20, a method for estimating the position of the moving body will be described. In the present embodiment, as an example, a method of applying the ICP (Interactive Closest Point) method to estimate the position of the vehicle 10 will be described.
ＩＣＰ法とは、画像から取得した測定点群上のある点ｐについて、地図から取得した地図点群上のある点ｑに対する距離を求める処理を点群上の全ての点ｐ及び全て点ｑについて行い、移動体の移動位置を推定する手法である。ＩＣＰ法は、点群上の全ての点ｐ及び全て点ｑの距離を算出して距離の総和が最小となるｐ及びｑの組み合わせを求める（以下、「位置合わせ」という。）ことで、測定点群の点ｐと、点ｐに対応する地図点群の点ｑとを整合させるための同次変換行列を導出する。同次変換行列とは、点ｐに対して並進変換と回転変換を施すことで、点ｑにより近い座標になると推測される行列である。また、ＩＣＰ法は、位置合わせを行うことで同次変換行列及び距離の総和を求め、求めた同次変換行列を適用して位置合わせを行う処理を繰り返し行う。繰り返し求めた距離の総和の差分が所定の閾値を下回る収束条件を満たした場合、同次変換行列及び地図点群上における測定点群の点ｐの位置が確定する。 The ICP method is a process for obtaining a distance to a point q on a map point group acquired from a map for a point p on a measurement point group acquired from an image for all points p and all points q on the point group. This is a method of performing and estimating the moving position of the moving body. In the ICP method, measurement is performed by calculating the distances of all points p and all points q on the point group, and obtaining the combination of p and q that minimizes the total sum of distances (hereinafter referred to as “registration”). A homogeneous transformation matrix for matching the point p of the point group and the point q of the map point group corresponding to the point p is derived. The homogeneous transformation matrix is a matrix that is supposed to have coordinates closer to the point q by performing translational transformation and rotational transformation on the point p. In addition, the ICP method repeats the process of performing the alignment to obtain the total of the homogeneous transformation matrix and the distance, and applying the obtained homogeneous transformation matrix to perform the alignment. When the difference of the total sum of the distances repeatedly obtained satisfies the convergence condition that is less than a predetermined threshold value, the position of the point p of the measurement point group on the homogeneous transformation matrix and the map point group is determined.
つまり、ＩＣＰ法は、画像から取得した測定点群及び地図から取得した地図点群の位置合わせと、同次変換行列及び距離の総和を導出する処理と、を繰り返し行うことで、地図座標系上における移動体点の位置を推定する処理の精度を上げる手法である。 That is, the ICP method repeats the alignment of the measurement point group acquired from the image and the map point group acquired from the map, and the process of deriving the total of the homogeneous transformation matrix and the distance, thereby making This is a method for improving the accuracy of the process of estimating the position of the moving body point in.
なお、本実施形態では、関連する複数の特徴点同士を密度に応じた重み値を付与した代表点に置換する（以下、「リサンプリング」という。）処理を行い、測定座標系の特徴点及び代表点と、地図座標系の特徴点及び代表点との位置合わせを行う。更に、本実施形態では、一例として、測定座標系の特徴点及び代表点と、地図座標系の特徴点及び代表点との相互距離に、測定点群の代表点の密度及び地図点群の代表点の密度を適用して、同次変換行列及び距離の総和を導出する。 In the present embodiment, a process of replacing a plurality of related feature points with a representative point having a weight value according to the density (hereinafter referred to as “resampling”) is performed, and the feature points of the measurement coordinate system and The representative point is aligned with the characteristic point and representative point of the map coordinate system. Further, in the present embodiment, as an example, the density of the representative points of the measurement point group and the representative point of the map point group are set to the mutual distance between the characteristic point and the representative point of the measurement coordinate system and the characteristic point and the representative point of the map coordinate system. The density of points is applied to derive the homogeneous transformation matrix and the sum of the distances.
次に、図５を参照して、リサンプリング処理について説明する。図５は、リサンプリング処理前後の構造物の概念を示す図である。図５（ａ）は、本実施形態に係る構造物の特徴点を模式的に示す図である。図５（ｂ）は、本実施形態に係る構造物に配置された代表点を模式に示す図である。 Next, the resampling process will be described with reference to FIG. FIG. 5 is a diagram showing the concept of the structure before and after the resampling process. FIG. 5A is a diagram schematically showing characteristic points of the structure according to the present embodiment. FIG. 5B is a diagram schematically showing representative points arranged on the structure according to the present embodiment.
図５（ａ）に示すように、特徴点が密集した構造物は、一例として、特徴点が隣接した状態で表される。図５（ｂ）に示すような代表点は、例えば、次のような処理を行うことで置換することができる。 As shown in FIG. 5A, a structure in which feature points are densely shown is, for example, in a state where feature points are adjacent to each other. The representative points as shown in FIG. 5B can be replaced by performing the following processing, for example.
推定部２４は、測定座標系及び地図座標系上から任意の特徴点を選択し、選択した特徴点に隣接する座標を参照する。参照した座標に特徴点が配置されている場合、推定部２４は、特徴点が配置されている座標を取得する。また、推定部２４は、参照した座標に特徴点が配置されていた場合、特徴点が配置されていた座標に隣接する座標に特徴点が配置されているか参照する処理を測定座標系及び地図座標系上の特徴点を全て抽出するまで行う。また、推定部２４は、取得した隣接する特徴点の座標を用いて、隣接する特徴点の代表点及び重み値を導出する。 The estimation unit 24 selects an arbitrary feature point from the measurement coordinate system and the map coordinate system, and refers to the coordinates adjacent to the selected feature point. When the feature point is located at the referred coordinate, the estimation unit 24 acquires the coordinate at which the feature point is located. In addition, when the feature point is located at the referred coordinate, the estimating unit 24 performs a process of referring to whether the feature point is located at the coordinate adjacent to the coordinate where the feature point was located at the measurement coordinate system and the map coordinate system. The process is repeated until all feature points on the system are extracted. Further, the estimation unit 24 derives the representative point and the weight value of the adjacent feature points by using the acquired coordinates of the adjacent feature points.
なお、本実施形態では、一例として、隣接する特徴点の重心位置に代表点を導出する形態について説明する。また、本実施形態では、一例として、隣接する特徴点の密度を重み値として導出する形態について説明する。また、隣接する特徴点の重心位置Ｇｉ、及び隣接する特徴点の密度Ｄｉは次の数式によって求めることができる。 In the present embodiment, as an example, a form in which the representative point is derived at the barycentric position of the adjacent feature points will be described. Further, in the present embodiment, as an example, a form in which the density of adjacent feature points is derived as a weight value will be described. Further, the barycentric position Gi of the adjacent feature points and the density Di of the adjacent feature points can be obtained by the following mathematical expressions.
ここで、ｉは代表点ごとに付与される正の整数である。また、ｍは、特徴点ごとに付与される正の整数である。また、Ｎｉは、代表点に関連する特徴点の個数である。また、ｐｍは、測定点群上のｍ番目の特徴点の座標である。また、Ｋｉは、特徴点を算出する際に参照した座標の数である。 Here, i is a positive integer given to each representative point. Further, m is a positive integer given to each feature point. Further, Ni is the number of feature points related to the representative point. Further, pm is the coordinates of the m-th feature point on the measurement point group. Ki is the number of coordinates referred to when calculating the feature points.
推定部２４は、算出した重心位置に代表点を配置し、リサンプリング後の測定座標系及びリサンプリング後の地図座標系を作成する。 The estimating unit 24 arranges the representative point at the calculated center of gravity and creates a measurement coordinate system after resampling and a map coordinate system after resampling.
図５（ａ）に示すような特徴点が密集する構造物に対してリサンプリング処理を行うことで、図５（ｂ）に示すように構造物の特徴点を簡略化して表示できる。 By performing the resampling process on the structure in which the feature points are dense as shown in FIG. 5A, the feature points of the structure can be displayed in a simplified manner as shown in FIG. 5B.
なお、本実施形態では、一例として、隣接している複数の特徴点を代表点に置換する形態について説明した。しかし、これに限定されない。所定の範囲に配置されている複数の特徴点を代表点に置換する形態としてもよい。また、本実施形態では、一例として、関連する複数の特徴点の重心位置を代表点の位置とする形態について説明した。しかし、これに限定されない。関連する複数の特徴点の任意の特徴点の位置を代表点としてもよいし、関連する複数の特徴点のうち最上部及び最下部に位置する特徴点の位置を代表点としてもよい。また、本実施形態では、重み値として、密度を用いた形態について説明した。しかし、これに限定されない。重み値として、関連する特徴点の個数を適用してもよい。 In the present embodiment, as an example, the form in which a plurality of adjacent feature points are replaced with representative points has been described. However, it is not limited to this. It is also possible to replace a plurality of feature points arranged in a predetermined range with representative points. Further, in the present embodiment, as an example, the form in which the barycentric position of a plurality of related feature points is set as the position of the representative point has been described. However, it is not limited to this. The positions of arbitrary feature points of a plurality of related feature points may be used as the representative points, or the positions of the feature points located at the top and bottom of the plurality of related feature points may be used as the representative points. Further, in the present embodiment, the form in which the density is used as the weight value has been described. However, it is not limited to this. The number of related feature points may be applied as the weight value.
次に、図６を参照して、本実施形態に係る測定点群及び地図点群の位置合わせ処理について説明する。図６（ａ）は、本実施形態に係るリサンプリング前の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。また、図６（ｂ）は、本実施形態に係るリサンプリング後の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。 Next, with reference to FIG. 6, the alignment processing of the measurement point group and the map point group according to the present embodiment will be described. FIG. 6A is a diagram schematically showing an example of alignment of the characteristic points of the measurement point group and the map point group before resampling according to the present embodiment. Further, FIG. 6B is a diagram schematically showing an example of the alignment of the characteristic points of the measurement point cloud and the map point cloud after resampling according to the present embodiment.
図６（ａ）に示すように特徴点が密集した構造物同士を位置合わせすると、誤った特徴点の組み合わせであっても距離の総和が最小となり、正常な組み合わせと誤認識してしまい、特徴点の位置合わせが精度よく行われないことがある。 As shown in FIG. 6A, when the structures having dense feature points are aligned with each other, the total sum of the distances is minimized even if the combination of wrong feature points is minimized, and the combination is erroneously recognized as a normal combination. The points may not be aligned accurately.
本実施形態では図６（ｂ）に示すように、測定点群及び地図点群の密集する特徴点に代えて代表点を導入し、位置合わせ処理を行う。つまり、リサンプリング後の測定点群上のｉ個目の点ｐｉとリサンプリング後のｉ個目の地図点群上の点ｑｉとの組を（ｐｉ，ｑｉ）とし、点ｐｉ及び点ｑｉの距離に代表点の密度を掛けた値の総和が最小となる点ｐｉ及びｑｉの組み合わせを求める。なお、代表点の密度に点ｐｉ及びｑｉの距離を掛けた値の総和（以下、「距離の総和」という。）ｄは、次の数式によって求めることができる。 In the present embodiment, as shown in FIG. 6B, representative points are introduced instead of dense feature points of the measurement point group and the map point group, and alignment processing is performed. That is, the set of the i-th point pi on the re-sampled measurement point group and the point qi on the i-th map point group after resampling is defined as (pi, qi), and the points pi and qi are The combination of the points pi and qi that gives the minimum sum of the values obtained by multiplying the distance by the density of the representative points is obtained. The sum of the values obtained by multiplying the density of the representative points by the distances of the points pi and qi (hereinafter referred to as the “sum of distances”) d can be obtained by the following mathematical expression.
ここで、ｑは地図点群上のある１つの点であり、Ｑは、地図点群上の全ての点の集合である。また、Ｄｉ（ｐｉ）は、測定点群ｉ番目の代表点の密度、Ｄｉ（ｑｉ）は、地図点群上のｉ番目の代表点の密度である。また、ｐｉは、測定点群上のｉ番目の特徴点及び代表点であり、ｑｉは、地図点群上のｉ番目の特徴点及び代表点である。また、Ｍｉｃｐは、測定座標系から地図座標系に整合するための同次変換行列である。同次変換行列Ｍｉｃｐは、次の数式で表される。 Here, q is a certain point on the map point group, and Q is a set of all points on the map point group. Further, Di(pi) is the density of the i-th representative point of the measurement point group, and Di(qi) is the density of the i-th representative point on the map point group. Further, pi is the i-th feature point and representative point on the measurement point group, and qi is the i-th feature point and representative point on the map point group. Micp is a homogeneous transformation matrix for matching the measurement coordinate system to the map coordinate system. The homogeneous transformation matrix Micp is represented by the following mathematical formula.
ここで、Ｒｉｃｐは、回転変換を表す３×３行列である。また、ｔｉｃｐは、並進変換を表す３次元ベクトルである。なお、本実施形態では、同次変換行列Ｍｉｃｐを次の数式によって求めることができる。 Here, Ricp is a 3×3 matrix representing a rotation transformation. Also, tipp is a three-dimensional vector representing translational conversion. In this embodiment, the homogeneous transformation matrix Micp can be obtained by the following mathematical formula.
ここで、Ｍは、予め設定された同次変換行列の初期値、又は繰り返し行われた処理うち、直近の処理で求められた同次変換行列である。 Here, M is a preset initial value of a homogeneous transformation matrix or a homogeneous transformation matrix obtained by the most recent processing among the repeated processing.
本実施形態では、次の数式で表される収束条件を満たす場合、測定点群及び地図点群の位置合わせを終了する。 In this embodiment, when the convergence condition represented by the following mathematical formula is satisfied, the positioning of the measurement point group and the map point group is finished.
なお、ｋは、測定点群及び地図点群の位置合わせを行った回数である。また、δは、予め決められた所定の閾値である。なお、本実施形態では、収束条件が所定の閾値を下回った場合、測定点群及び地図点群の位置合わせを終了する形態について説明したが、これに限定されない。位置合わせを所定の回数行った場合、処理を終了する形態としてもよいし、距離の総和が所定の値を下回った場合、位置合わせを終了する形態としてもよい。 Note that k is the number of times the measurement point group and the map point group are aligned. Further, δ is a predetermined threshold value determined in advance. It should be noted that, in the present embodiment, the mode in which the alignment of the measurement point group and the map point group is terminated when the convergence condition falls below a predetermined threshold has been described, but the present invention is not limited to this. The process may be terminated when the alignment is performed a predetermined number of times, or the alignment may be terminated when the total distance is less than a predetermined value.
次に、図７及び図８を参照して、本実施形態に係る移動***置推定プログラムの作用について説明する。まず、図７は、本実施形態に係る移動***置推定処理の一例を示すフローチャートである。ＣＰＵ１３が移動***置推定プログラム１５Ａを実行することによって、図７に示す移動***置推定処理が実行される。図７に示す移動***置推定処理は、例えば、ユーザにより移動***置推定装置２０に移動***置推定処理の実行指示が入力された場合に実行される。 Next, the operation of the moving body position estimation program according to this embodiment will be described with reference to FIGS. 7 and 8. First, FIG. 7 is a flowchart showing an example of the moving body position estimation processing according to the present embodiment. The CPU 13 executes the moving body position estimating program 15A, whereby the moving body position estimating process shown in FIG. 7 is executed. The moving body position estimating process shown in FIG. 7 is executed, for example, when the user inputs an instruction to execute the moving body position estimating process to the moving body position estimating device 20.
ステップＳ１００で、検出部２２は、撮影部２１で撮影した画像を取得する。 In step S100, the detection unit 22 acquires the image captured by the image capturing unit 21.
ステップＳ１０２で、検出部２２は、取得した画像からＦＡＳＴ等のコーナー検出アルゴリズムを用いて構造物の特徴点を検出する。 In step S102, the detection unit 22 detects feature points of the structure from the acquired image using a corner detection algorithm such as FAST.
ステップＳ１０４で、作成部２３は、ＳＬＡＭを用いて検出部２２で検出した構造物の特徴点を座標系に配置した無次元測定点群を作成する。 In step S104, the creation unit 23 creates a dimensionless measurement point group in which the feature points of the structure detected by the detection unit 22 are arranged in the coordinate system using SLAM.
ステップＳ１０６で、作成部２３は、オドメトリー法を用いて撮影部２１により撮影された画像から計測した車両１０の移動量、及び構造物までの距離及び大きさ等の情報を無次元測定点群に適用した有次元測定点群をさらに作成する。また、作成部２３は、作成した有次元測定点群及び移動体点を設定した測定座標系を作成する。また、取得部２４は、ストレージ１６に予め記憶していた地図座標系を取得する。 In step S106, the creating unit 23 sets information such as the amount of movement of the vehicle 10 measured from the image captured by the image capturing unit 21 using the odometry method and the distance and size to the structure in the dimensionless measurement point group. The applied dimensional measurement point cloud is further created. Further, the creation unit 23 creates a measurement coordinate system in which the created dimensional measurement point group and the moving body points are set. The acquisition unit 24 also acquires the map coordinate system stored in advance in the storage 16.
ステップＳ１０８で、推定部２５は、作成した測定座標系及び取得した地図座標系に隣接する特徴点を代表点に置換し、代表点に重み値を付与するリサンプリング処理を行う。 In step S108, the estimation unit 25 replaces the characteristic points adjacent to the created measurement coordinate system and the acquired map coordinate system with representative points, and performs resampling processing for giving a weight value to the representative points.
ステップＳ１１０で、推定部２５は、リサンプリング処理を施した測定座標系及び地図座標系を作成する。 In step S110, the estimation unit 25 creates a measurement coordinate system and a map coordinate system that have undergone resampling processing.
ステップＳ１１２で、推定部２５は、リサンプリング処理を施した代表点の密度を取得する。 In step S112, the estimation unit 25 acquires the density of the representative points subjected to the resampling process.
ステップＳ１１４で、推定部２５は、密度が所定の閾値を超えた代表点を特定する。ステップＳ１１６で、推定部２５は、特定した代表点に関連する特徴点を除いたリサンプリング前の測定座標系及び地図座標系を用いて位置合わせ処理を行い、距離の総和が最小となる特徴点及び代表点の組み合わせと、距離の総和の値とを算出する。 In step S114, the estimation unit 25 identifies a representative point whose density exceeds a predetermined threshold value. In step S116, the estimation unit 25 performs the alignment process using the measurement coordinate system before resampling and the map coordinate system excluding the feature points related to the identified representative point, and the feature points that minimize the total sum of distances. And the combination of representative points and the value of the sum of distances are calculated.
ステップＳ１１８で、推定部２５は、算出した特徴点及び代表点の組み合わせを用いて同次変換行列の初期値を導出する。ステップＳ１２０で、推定部２５は、リサンプリング前の測定座標系に含まれる移動体点に同次変換行列の初期値を適用して、車両１０の初期位置を推定する。 In step S118, the estimation unit 25 derives the initial value of the homogeneous transformation matrix using the calculated combination of feature points and representative points. In step S120, the estimation unit 25 estimates the initial position of the vehicle 10 by applying the initial value of the homogeneous transformation matrix to the moving body points included in the measurement coordinate system before resampling.
ステップＳ１２２で、推定部２５は、リサンプリング後の測定点群及び地図点群に含まれる特徴点及び代表点と、同次変換行列の初期値とを用いて位置合わせを行い、距離の総和が最小となる特徴点及び代表点の組み合わせと、距離の総和の値とを算出する。 In step S122, the estimation unit 25 performs alignment using the feature points and representative points included in the resampled measurement point group and map point group, and the initial value of the homogeneous transformation matrix, and the total distance is calculated. The minimum combination of feature points and representative points and the value of the total sum of distances are calculated.
ステップＳ１２４で、推定部２５は、算出した特徴点及び代表点の組み合わせを用いて同次変換行列を導出する。ステップＳ１２６で、推定部２５は、測定座標系に含まれる移動体点に同次変換行列を適用して、車両１０の位置を推定する。 In step S124, the estimation unit 25 derives a homogeneous transformation matrix using the calculated combination of feature points and representative points. In step S126, the estimation unit 25 estimates the position of the vehicle 10 by applying the homogeneous transformation matrix to the moving body points included in the measurement coordinate system.
ステップＳ１２８で、推定部２５は収束条件を満たしているか否かの判定を行う。ここで、収束条件を満たす場合（ステップＳ１２８：Ｙ）、ステップＳ１３０に移行する。ステップＳ１３０で、推定部２５は、推定した車両１０の位置を車両１０の現在位置としてみなす。一方、収束条件を満たさない場合（ステップＳ１２８：Ｎ）、ステップＳ１２２に移行する。 In step S128, the estimation unit 25 determines whether or not the convergence condition is satisfied. Here, if the convergence condition is satisfied (step S128: Y), the process proceeds to step S130. In step S130, the estimation unit 25 regards the estimated position of the vehicle 10 as the current position of the vehicle 10. On the other hand, when the convergence condition is not satisfied (step S128: N), the process proceeds to step S122.
ステップＳ１３２で、推定部２５は、目的位置に到達し、撮影を終了するか否かの判定を行う。撮影を終了する場合（ステップＳ１３２：Ｙ）、移動***置推定処理を終了する。一方、撮影を終了しない場合（ステップＳ１３２：Ｎ）、ステップＳ１００に移行する。 In step S132, the estimation unit 25 determines whether or not to reach the target position and end shooting. If the photographing is to be ended (step S132: Y), the moving body position estimation processing is ended. On the other hand, when the shooting is not completed (step S132: N), the process proceeds to step S100.
次に図８を参照して、本実施形態に係るリサンプリング処理の説明をする。図８は本実施形態に係るリサンプリング処理の一例を示すフローチャートである。図７に示すステップＳ１０６が終了した場合、一例として、図８に示すリサンプリング処理が実行される。 Next, the resampling processing according to the present embodiment will be described with reference to FIG. FIG. 8 is a flowchart showing an example of resampling processing according to the present embodiment. When step S106 shown in FIG. 7 ends, as an example, the resampling process shown in FIG. 8 is executed.
ステップＳ２００で、推定部２５は、測定座標系及び地図座標系を取得する。 In step S200, the estimation unit 25 acquires the measurement coordinate system and the map coordinate system.
ステップＳ２０２で、推定部２４は、測定座標系及び地図座標系の任意の特徴点を検出する。ステップＳ２０４で、推定部２４は、検出した特徴点の座標及び参照した座標をＲＡＭ１４に記憶する。 In step S202, the estimation unit 24 detects arbitrary feature points in the measurement coordinate system and the map coordinate system. In step S204, the estimation unit 24 stores the detected feature point coordinates and the referred coordinates in the RAM 14.
ステップＳ２０６で、推定部２４は、検出した特徴点に隣接する座標を参照して、特徴点が設定されているか否かの判定を行う。隣接する特徴点がない場合（ステップＳ２０６：Ｙ）、ステップＳ２０８に移行する。一方、隣接する特徴点がある場合（ステップＳ２０６：Ｎ）、ステップＳ２０２に移行して、検出した特徴点に隣接する特徴点を検出する。 In step S206, the estimation unit 24 refers to the coordinates adjacent to the detected feature point and determines whether the feature point is set. When there is no adjacent feature point (step S206: Y), the process proceeds to step S208. On the other hand, if there is an adjacent feature point (step S206: N), the process moves to step S202 to detect a feature point adjacent to the detected feature point.
ステップＳ２０８で、推定部２５は、記憶した特徴点の数及び参照した座標の数を算出する。ステップＳ２１０で、推定部２５は、記憶した特徴点の座標を用いて、関連する複数の特徴点の代表点として重心座標を算出する。 In step S208, the estimation unit 25 calculates the number of stored feature points and the number of referenced coordinates. In step S210, the estimation unit 25 uses the stored coordinates of the feature points to calculate the barycentric coordinates as a representative point of the plurality of related feature points.
ステップＳ２１２で、推定部２５は、隣接する特徴点の数、及び参照した座標の数、を用いて重心位置に付与する密度を算出する。 In step S212, the estimation unit 25 calculates the density to be applied to the position of the center of gravity using the number of adjacent feature points and the number of referenced coordinates.
ステップＳ２１４で、推定部２５は、測定座標系及び地図座標系上に設定されている全ての特徴点を検出したか否かの判定を行う。全ての特徴点を検出した場合（ステップＳ２１４：Ｙ）、ステップＳ２１６に移行する。一方、全ての特徴点を検出していない場合（ステップＳ２１４：Ｎ）、ステップＳ２０２に移行して、特徴点の検出を行う。 In step S214, the estimation unit 25 determines whether or not all the feature points set on the measurement coordinate system and the map coordinate system have been detected. When all the feature points have been detected (step S214: Y), the process proceeds to step S216. On the other hand, when all the feature points have not been detected (step S214: N), the process moves to step S202 to detect the feature points.
ステップＳ２１６で、推定部２５は、特徴点及び算出した密度を付与した重心位置を測定座標系及び地図座標系に設定し、リサンプリング処理を終了する。 In step S216, the estimation unit 25 sets the feature point and the position of the center of gravity to which the calculated density is added in the measurement coordinate system and the map coordinate system, and ends the resampling process.
以上説明したように、本実施形態によれば、特徴点が密集している構造物の特徴点の代表点及び密度を算出し、構造物の代表点及び密度を用いて、測定点群及び地図点群の位置合わせを行い、同次変換行列を導出することができる。従って、特徴点が密集した構造物があった場合でも、精度よく移動体の位置を推定することができる。 As described above, according to the present embodiment, the representative point and the density of the characteristic points of the structure in which the characteristic points are dense are calculated, and the representative point and the density of the structure are used to measure the measurement point group and the map. The point cloud can be aligned to derive the homogeneous transformation matrix. Therefore, even if there is a structure in which feature points are densely located, it is possible to accurately estimate the position of the moving body.
なお、本実施形態では、同次変換行列の初期値及び移動体の初期位置は、測定点群及び地図点群に含まれる特徴点及び密度が所定の閾値以下の代表点を位置合わせして導出する形態として説明したが、これに限定されない。所定の閾値を超えた代表点を用いて、同次変換行列の初期値及び移動体の初期位置を導出する形態としてもよいし、同次変換行列の初期値及び移動体の初期位置は予め初期値を設定する形態としてもよい。 In the present embodiment, the initial value of the homogeneous transformation matrix and the initial position of the moving body are derived by aligning the characteristic points included in the measurement point group and the map point group and the representative points whose density is equal to or less than a predetermined threshold value. However, the present invention is not limited to this. The representative value that exceeds a predetermined threshold may be used to derive the initial value of the homogeneous transformation matrix and the initial position of the moving body, and the initial value of the homogeneous transformation matrix and the initial position of the moving body may be initialized in advance. The value may be set.
また、本実施形態に係る測定座標系及び地図座標系は、三次元空間座標上に特徴点を配置する形態として、説明したがこれに限定されない。二次元平面座標上に特徴点を配置する形態としてもよい。 Further, the measurement coordinate system and the map coordinate system according to the present embodiment have been described as the form in which the feature points are arranged on the three-dimensional space coordinates, but the present invention is not limited to this. The feature points may be arranged on the two-dimensional plane coordinates.
その他、上記実施形態で説明した移動***置推定装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。 In addition, the configuration of the moving body position estimation device described in the above embodiment is an example, and may be changed according to the situation without departing from the spirit of the invention.
また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。 The flow of processing of the program described in the above embodiment is also an example, and unnecessary steps may be deleted, new steps may be added, or the processing order may be changed without departing from the spirit of the invention. Good.
なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した移動***置推定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Field Programmable Gate Array）等の製造後に回路構成を変更可能なＰＬＤ（Programmable Logic Device）、及びＡＳＩＣ（Application Specific Integrated Circuit）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、リサンプリング処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。
また、上記各実施形態では、移動***置推定処理、及びリサンプリング処理のプログラムがストレージ１６に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ−ＲＯＭ（Compact Disk Read Only Memory）、ＤＶＤ−ＲＯＭ（Digital Versatile Disk Read Only Memory）、及びＵＳＢ（Universal Serial Bus）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。
Note that the CPU may read the software (program) and execute the moving body position estimation processing in each of the above embodiments by various processors other than the CPU. In this case, the processor is dedicated to execute a specific process such as PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacturing of FPGA (Field Programmable Gate Array) and ASIC (Application Specific Integrated Circuit). An example is a dedicated electric circuit which is a processor having a circuit configuration designed for. Further, the resampling process may be executed by one of these various processors, or a combination of two or more processors of the same type or different types (for example, a plurality of FPGAs and a combination of a CPU and an FPGA). Etc.). The hardware structure of these various processors is, more specifically, an electric circuit in which circuit elements such as semiconductor elements are combined.
Further, in each of the above-described embodiments, the mode in which the programs for the moving body position estimation process and the resampling process are stored (installed) in the storage 16 in advance has been described, but the invention is not limited to this. The program may be provided in a form recorded in a recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), and a USB (Universal Serial Bus) memory. Further, the program may be downloaded from an external device via a network.
１０ 車両
１１ カメラ
１２ 表示部
１３ ＣＰＵ
１４ ＲＡＭ
１５ ＲＯＭ
１５Ａ 移動***置推定プログラム
１６ ストレージ
１６Ａ 地図情報
２０ 移動***置推定装置
２１ 撮影部
２２ 検出部
２３ 作成部
２４ 取得部
２５ 推定部
10 Vehicle 11 Camera 12 Display 13 CPU
14 RAM
15 ROM
15A mobile body position estimation program 16 storage 16A map information 20 mobile body position estimation device 21 imaging unit 22 detection unit 23 creation unit 24 acquisition unit 25 estimation unit
Claims (6)
前記撮影部により撮影された画像から複数の特徴点を検出する検出部と、
前記検出部により検出された前記複数の特徴点を含む測定点群と、前記画像を撮影した際の前記移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、
前記移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する取得部と、
前記測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点に置換し、前記地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、前記第一の重み値及び前記第二の重み値と、前記測定点群に含まれる特徴点及び代表点と前記地図点群に含まれる特徴点及び代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出し、導出した前記同次変換行列により前記測定座標系の前記移動体点を前記地図座標系に適用して、前記地図上の前記移動体の位置を推定する推定部と、
を備える移動***置推定装置。 A shooting unit that shoots images from a moving object,
A detection unit that detects a plurality of feature points from the image captured by the imaging unit,
A measurement unit including a measurement point group including the plurality of characteristic points detected by the detection unit, and a moving body point indicating a position of the moving body when the image is captured, and a creating unit that creates a measurement coordinate system.
An acquisition unit that acquires a map coordinate system having a map point group, including a plurality of feature points detected in advance from each of a plurality of locations on the map where the mobile body can move,
Among the plurality of feature points included in the measurement point group, a plurality of related feature points are replaced with representative points given a first weight value according to the density, and a plurality of features included in the map point group Among the points, related feature points are replaced with representative points given a second weight value according to the density, and the first weight value and the second weight value are included in the measurement point group. From the mutual distances between the feature points and the representative points and the feature points and the representative points included in the map point group, a homogeneous transformation matrix for matching the measurement coordinate system with the map coordinate system is derived, and the derived transformation matrix is derived. An estimation unit that estimates the position of the mobile object on the map by applying the mobile object point of the measurement coordinate system to the map coordinate system by a homogeneous transformation matrix,
A mobile body position estimation device including.
前記測定点群及び前記地図点群間の任意の対応する特徴点の組又は代表点の組において、特徴点又は代表点が整合する前記同次変換行列を導出することを、導出した同次変換行列を測定点群及び前記地図点群間の対応する特徴点の組及び代表点の組に適用した時の特徴点間の距離及び代表点間の距離と前記第一の重み値及び前記第二の重み値とを用いた評価関数が所定の条件を満たすまで、繰り返す請求項１記載の移動***置推定装置。 The estimation unit is
Derivation of the homogeneous transformation matrix in which the feature points or the representative points match in any corresponding set of feature points or set of representative points between the measurement point group and the map point group is derived. When the matrix is applied to the set of corresponding feature points and the set of representative points between the measurement point group and the map point group, the distance between the characteristic points and the distance between the representative points, the first weight value, and the second The mobile body position estimating apparatus according to claim 1, wherein the evaluation function using the weight value of is repeated until a predetermined condition is satisfied.
撮影部により撮影された画像から複数の特徴点を検出するステップと、
検出部により検出された前記複数の特徴点を含む測定点群と、前記画像を撮影した際の前記移動体の位置を示す移動体点と、を有する測定座標系を作成するステップと、
前記移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得するステップと、
前記測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点を置換し、前記地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、前記第一の重み値及び前記第二の重み値と、前記測定点群に含まれる特徴点及び代表点と前記地図点群に含まれる特徴点及び代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出し、導出した前記同次変換行列により前記測定座標系の前記移動体点を前記地図座標系に適用して、前記地図上の前記移動体の位置を推定するステップと、
をコンピュータに実行させる移動***置推定プログラム。 A step of shooting an image by a shooting unit mounted on the moving body,
Detecting a plurality of feature points from the image captured by the image capturing unit,
A step of creating a measurement coordinate system having a measurement point group including the plurality of characteristic points detected by the detection unit, and a moving body point indicating the position of the moving body when the image is captured;
Acquiring a map coordinate system having a map point group, including a plurality of feature points previously detected from each of a plurality of locations on the map where the mobile body can move,
Among the plurality of feature points included in the measurement point group, a plurality of related feature points are replaced with a representative point given a first weight value according to the density, and a plurality of features included in the map point group. Among the points, related feature points are replaced with representative points given a second weight value according to the density, and the first weight value and the second weight value are included in the measurement point group. From the mutual distances between the feature points and the representative points and the feature points and the representative points included in the map point group, a homogeneous transformation matrix for matching the measurement coordinate system with the map coordinate system is derived, and the derived transformation matrix is derived. Applying the moving body point of the measurement coordinate system to the map coordinate system by a homogeneous transformation matrix to estimate the position of the moving body on the map;
A moving body position estimation program that causes a computer to execute.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019018117A JP2020125960A (en) | 2019-02-04 | 2019-02-04 | Moving object position estimating device and moving object position estimating program |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019018117A JP2020125960A (en) | 2019-02-04 | 2019-02-04 | Moving object position estimating device and moving object position estimating program |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2020125960A true JP2020125960A (en) | 2020-08-20 |
Family
ID=72083855
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2019018117A Pending JP2020125960A (en) | 2019-02-04 | 2019-02-04 | Moving object position estimating device and moving object position estimating program |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2020125960A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20220075794A (en) * | 2020-11-30 | 2022-06-08 | (주)심스리얼리티 | Method for producing three-dimensional map |
| WO2022181138A1 (en) * | 2021-02-25 | 2022-09-01 | 日野自動車株式会社 | Self-position estimation device |
| WO2024027634A1 (en) * | 2022-08-01 | 2024-02-08 | 京东方科技集团股份有限公司 | Running distance estimation method and apparatus, electronic device, and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008250906A (en) * | 2007-03-30 | 2008-10-16 | Sogo Keibi Hosho Co Ltd | Mobile robot, and self-location correction method and program |
| JP2009294233A (en) * | 2009-09-24 | 2009-12-17 | Aisin Aw Co Ltd | Navigation device |
| JP2011227888A (en) * | 2010-03-31 | 2011-11-10 | Aisin Aw Co Ltd | Image processing system and location positioning system |
| JP2016109650A (en) * | 2014-12-10 | 2016-06-20 | 株式会社デンソー | Position estimation system, position estimation method, and position estimation program |
-
2019
- 2019-02-04 JP JP2019018117A patent/JP2020125960A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008250906A (en) * | 2007-03-30 | 2008-10-16 | Sogo Keibi Hosho Co Ltd | Mobile robot, and self-location correction method and program |
| JP2009294233A (en) * | 2009-09-24 | 2009-12-17 | Aisin Aw Co Ltd | Navigation device |
| JP2011227888A (en) * | 2010-03-31 | 2011-11-10 | Aisin Aw Co Ltd | Image processing system and location positioning system |
| JP2016109650A (en) * | 2014-12-10 | 2016-06-20 | 株式会社デンソー | Position estimation system, position estimation method, and position estimation program |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20220075794A (en) * | 2020-11-30 | 2022-06-08 | (주)심스리얼리티 | Method for producing three-dimensional map |
| KR102461980B1 (en) * | 2020-11-30 | 2022-11-03 | (주)심스리얼리티 | Method for producing three-dimensional map |
| WO2022181138A1 (en) * | 2021-02-25 | 2022-09-01 | 日野自動車株式会社 | Self-position estimation device |
| WO2024027634A1 (en) * | 2022-08-01 | 2024-02-08 | 京东方科技集团股份有限公司 | Running distance estimation method and apparatus, electronic device, and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10825198B2 (en) | 3 dimensional coordinates calculating apparatus, 3 dimensional coordinates calculating method, 3 dimensional distance measuring apparatus and 3 dimensional distance measuring method using images | |
| US20220036574A1 (en) | System and method for obstacle avoidance | |
| JP5671281B2 (en) | Position / orientation measuring apparatus, control method and program for position / orientation measuring apparatus | |
| JP5624394B2 (en) | Position / orientation measurement apparatus, measurement processing method thereof, and program | |
| JP5548482B2 (en) | Position / orientation measuring apparatus, position / orientation measuring method, program, and storage medium | |
| JP5297403B2 (en) | Position / orientation measuring apparatus, position / orientation measuring method, program, and storage medium | |
| JP5430456B2 (en) | Geometric feature extraction device, geometric feature extraction method, program, three-dimensional measurement device, object recognition device | |
| CN110702111A (en) | Simultaneous localization and map creation (SLAM) using dual event cameras | |
| JP6456141B2 (en) | Generating map data | |
| JP2011175477A (en) | Three-dimensional measurement apparatus, processing method and program | |
| JP5615055B2 (en) | Information processing apparatus and processing method thereof | |
| JP6589636B2 (en) | 3D shape measuring apparatus, 3D shape measuring method, and 3D shape measuring program | |
| JP2011174879A (en) | Apparatus and method of estimating position and orientation | |
| JP2020125960A (en) | Moving object position estimating device and moving object position estimating program | |
| JP2013178656A (en) | Image processing device, image processing method, and image processing program | |
| EP3155369B1 (en) | System and method for measuring a displacement of a mobile platform | |
| US11222433B2 (en) | 3 dimensional coordinates calculating apparatus and 3 dimensional coordinates calculating method using photo images | |
| JP6817742B2 (en) | Information processing device and its control method | |
| JP5976089B2 (en) | Position / orientation measuring apparatus, position / orientation measuring method, and program | |
| JP5698815B2 (en) | Information processing apparatus, information processing apparatus control method, and program | |
| JP6040264B2 (en) | Information processing apparatus, information processing apparatus control method, and program | |
| JP4935769B2 (en) | Plane region estimation apparatus and program | |
| Furukawa et al. | Interactive shape acquisition using marker attached laser projector | |
| CN113295089B (en) | Carriage volume rate measuring method based on visual inertia SLAM | |
| JP2024501731A (en) | Speed measurement method and speed measurement device using multiple cameras |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20211214 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20221104 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20221122 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20230516 |