JP2017003363A - Position estimation method, position estimation device, and position estimation program - Google Patents
Position estimation method, position estimation device, and position estimation program Download PDFInfo
- Publication number
- JP2017003363A JP2017003363A JP2015116095A JP2015116095A JP2017003363A JP 2017003363 A JP2017003363 A JP 2017003363A JP 2015116095 A JP2015116095 A JP 2015116095A JP 2015116095 A JP2015116095 A JP 2015116095A JP 2017003363 A JP2017003363 A JP 2017003363A
- Authority
- JP
- Japan
- Prior art keywords
- plane
- frame
- point group
- position estimation
- sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Navigation (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
Description
本発明は、位置推定方法、位置推定装置及び位置推定プログラムに関する。 The present invention relates to a position estimation method, a position estimation apparatus, and a position estimation program.
位置推定装置として、レーザセンサ又は撮像装置等のセンサを用いて周囲の地物の位置を測定し、測定された周囲の地物の位置に基づいて自己位置を推定するものが知られている。このような位置推定装置では、測定中における各センサの位置及び姿勢が推定され、推定された位置及び姿勢と周囲の地物の位置とに基づいて自己位置が推定されるため、低精度の測定結果の除去が必要となる。 As a position estimation device, a device that measures the position of a surrounding feature using a sensor such as a laser sensor or an imaging device and estimates the self-position based on the measured position of the surrounding feature is known. In such a position estimation device, the position and posture of each sensor during measurement are estimated, and the self-position is estimated based on the estimated position and posture and the positions of surrounding features, so that low-precision measurement is performed. It is necessary to remove the result.
例えば、非特許文献１には、移動ロボットに搭載されたカメラによって撮影された複数の画像に含まれる複数の特徴点座標に基づいてカメラの位置姿勢が推定され、推定された位置姿勢から移動ロボットの自己位置が推定される位置推定方法が記載されている。この位置推定方法では、撮影された最初の画像から特徴点が抽出され、特徴点追跡手法によって移動後の画像に含まれる特徴点が追跡され、各画像における特徴点に基づいて並進ベクトル及び回転行列が算出されることによってカメラの位置姿勢が推定される。 For example, in Non-Patent Document 1, the position and orientation of a camera are estimated based on a plurality of feature point coordinates included in a plurality of images captured by a camera mounted on the mobile robot, and the mobile robot is calculated from the estimated position and orientation. Describes a position estimation method for estimating the self-position. In this position estimation method, feature points are extracted from the first captured image, feature points included in the image after movement are tracked by a feature point tracking method, and a translation vector and a rotation matrix are based on the feature points in each image. Is calculated to estimate the position and orientation of the camera.
従来の位置推定方法においては、レーザセンサ又は撮像装置等によって測定されて得られた複数の情報から抽出される特徴点として、その追跡に誤対応が生じないように、コーナー点、エッジ交点等が抽出されていた。しかしながら、このような位置推定方法によって屋外における自己位置推定が実施される場合、植栽や樹木の揺れ並びに移動する車両及び人等に対応する特徴点が抽出されることがあった。この場合、取得された複数の情報毎に、特徴点の現実空間における位置が異なることがあったため、特徴点の抽出又は追跡の精度が低下するという問題が生じていた。 In the conventional position estimation method, corner points, edge intersections, etc. are used as feature points extracted from a plurality of pieces of information obtained by measurement with a laser sensor or an imaging device so as not to cause miscorrespondence in tracking. It was extracted. However, when the self-position estimation is performed outdoors by such a position estimation method, feature points corresponding to planting, tree shaking, moving vehicles, people, and the like may be extracted. In this case, since the position of the feature point in the real space may be different for each of a plurality of pieces of acquired information, there has been a problem that the accuracy of extraction or tracking of the feature point is lowered.
本発明は、このような課題を解決すべくなされたものであり、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることを可能とする位置推定方法、位置推定装置及び位置推定プログラムを提供することを目的とする。 The present invention has been made to solve such problems, and position estimation that can improve the accuracy of self-position estimation based on information acquired by a sensor that measures the position of surrounding features. It is an object to provide a method, a position estimation device, and a position estimation program.
本発明に係る位置推定方法は、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定するコンピュータによる位置推定方法であって、センサから出力された点群をフレーム毎に取得し、取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、平面点群により形成される平面の位置をフレーム毎に特定し、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出し、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定することを含む。 According to the position estimation method of the present invention, a trajectory of a moving body equipped with a sensor that measures the positions of a plurality of features at a predetermined measurement time and outputs a point cloud indicating the position of the measured feature sensor coordinate system. Is a computer-based position estimation method for acquiring a point cloud output from a sensor for each frame, and from the acquired point cloud, a point cloud forming a plane is extracted for each frame as a plane point cloud, The position of the plane formed by the plane point group is specified for each frame, and the translation of the moving body between the frames is performed based on the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame. This includes calculating a vector and a rotation matrix, sequentially calculating a translation vector and a rotation matrix of the moving body between frames for each frame, and estimating a trajectory of the moving body.
また、本発明に係る位置推定方法において、センサは、パルス状に発光するレーザを照射して地物の位置を測定するレーザ照射部を有し、出力された点群に含まれる各点は、センサが搭載された位置を中心にレーザ照射部を回転させることにより順次測定された地物のセンサ座標系の位置を示し、検出することは、フレーム毎に、点群に含まれる各点のうちの所定の点と次の点との距離を算出し、算出した各距離に基づいて点群をセグメントに分類し、セグメントに分類された点群に含まれる点の数が所定数以上であり且つセグメントに分類された点群によって形成される形状が直線状であるセグメントを特定し、複数の特定されたセグメントの中から、各特定されたセグメントの法線ベクトル及び各特定されたセグメントの位置に応じて平面を形成するセグメントを検出し、検出されたセグメントに分類された点群を平面点群として抽出することを含むことが好ましい。 Further, in the position estimation method according to the present invention, the sensor has a laser irradiation unit that measures the position of the feature by irradiating a laser emitting light in a pulse shape, and each point included in the output point group is: The position of the sensor coordinate system of the features sequentially measured by rotating the laser irradiating unit around the position where the sensor is mounted is detected and detected from each point included in the point cloud for each frame. The distance between the predetermined point and the next point is calculated, the point cloud is classified into segments based on the calculated distances, and the number of points included in the point cloud classified into the segment is equal to or greater than a predetermined number; The segment formed by the point group classified into segments is identified as a straight line, and the normal vector of each identified segment and the position of each identified segment are selected from the plurality of identified segments. Flat Detecting the segments forming a preferably comprises extracting the classified point groups detected segment as a flat point group.
また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の位置と、所定のフレームにおける特定された平面の位置に最も近い次のフレームにおける特定された平面の位置とによって、移動体の並進ベクトル及び回転行列を算出することを含むことが好ましい。 In the position estimation method according to the present invention, the calculation includes the position of the specified plane in the predetermined frame and the specified plane in the next frame closest to the position of the specified plane in the predetermined frame. Preferably, the method includes calculating a translation vector and a rotation matrix of the moving body according to the position.
また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の法線方向と次のフレームにおける特定された平面の法線方向とが所定の角度以内である場合、所定のフレームにおける特定された平面の位置と次のフレームにおける特定された平面の位置とによって、移動体の移動体の並進ベクトル及び回転行列を算出することを含むことが好ましい。 In the position estimation method according to the present invention, the calculation is performed when the normal direction of the specified plane in a predetermined frame and the normal direction of the specified plane in the next frame are within a predetermined angle. Preferably, the method includes calculating a translation vector and a rotation matrix of the moving body of the moving body according to the position of the specified plane in the predetermined frame and the position of the specified plane in the next frame.
また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の法線ベクトルと次のフレームにおける特定された平面の法線ベクトルとによって、センサの並進ベクトル及び回転行列を算出することを含むことが好ましい。 In the position estimation method according to the present invention, the calculation may include calculating a translation vector and a rotation of the sensor based on the normal vector of the specified plane in a predetermined frame and the normal vector of the specified plane in the next frame. It preferably includes calculating a matrix.
また、本発明に係る位置推定方法において、算出することは、算出された並進ベクトル及び回転行列を、所定のフレームにおける特定された平面と次のフレームにおける特定された平面とによって最適化し、最適化された並進ベクトル及び回転行列を、所定のフレームと次のフレームにおけるフレーム間の移動体の並進ベクトル及び回転行列とすることを含むことが好ましい。 Further, in the position estimation method according to the present invention, calculating includes optimizing the calculated translation vector and rotation matrix by the specified plane in the predetermined frame and the specified plane in the next frame, and It is preferable that the translation vector and the rotation matrix obtained include the translation vector and the rotation matrix of the moving body between the frames in the predetermined frame and the next frame.
また、本発明に係る位置推定方法において、最適化することは、所定のフレームにおける平面点群を、算出された並進ベクトル及び回転行列によって移動させ、移動した所定のフレームにおける平面点群と次のフレームにおける平面点群とに対する、移動した平面点群により形成される平面の法線方向の分散が最小となるように並進ベクトル及び回転行列を最適化することを含むことが好ましい。 Further, in the position estimation method according to the present invention, the optimization includes moving the plane point group in the predetermined frame by the calculated translation vector and rotation matrix, and the plane point group in the moved predetermined frame and the following Preferably, the method includes optimizing the translation vector and the rotation matrix so that the normal dispersion of the plane formed by the moved plane point group with respect to the plane point group in the frame is minimized.
本発明に係る位置推定装置は、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定装置であって、センサから出力された点群をフレーム毎に取得する取得部と、取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出する抽出部と、平面点群により形成される平面の位置をフレーム毎に特定する特定部と、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出する算出部と、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定する推定部とを備える。 A position estimation device according to the present invention is a mobile object equipped with a sensor that measures the positions of a plurality of features at a predetermined measurement time and outputs a point cloud indicating the position of the measured sensor coordinate system of the features. A position estimation device for estimating a trajectory, which acquires a point group output from a sensor for each frame, and extracts a point group forming a plane from the acquired point group for each frame as a plane point group From the relationship between the extraction unit, the specifying unit for specifying the position of the plane formed by the plane point group for each frame, the position of the plane specified in the predetermined frame, and the position of the plane specified in the next frame A calculating unit that calculates a translation vector and a rotation matrix of a moving body between frames, and an estimation unit that sequentially calculates the translation vector and the rotation matrix of the moving body between frames for each frame to estimate a trajectory of the moving body. .
本発明に係る位置推定プログラムは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定プログラムであって、センサから出力された点群をフレーム毎に取得し、取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、平面点群により形成される平面の位置をフレーム毎に特定し、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出し、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定することをコンピュータに実行させる。 The position estimation program according to the present invention measures a position of a plurality of features at a predetermined measurement time and outputs a point cloud indicating a position of the measured feature in a sensor coordinate system. A position estimation program for estimating a trajectory, which acquires a point cloud output from a sensor for each frame, extracts a point cloud forming a plane from the acquired point cloud as a plane point cloud for each frame, and obtains a plane point The position of the plane formed by the group is specified for each frame, and the translation vector of the moving body between the frames based on the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame, and A rotation matrix is calculated, a translation vector and a rotation matrix of the moving body between frames are sequentially calculated for each frame, and the computer is caused to estimate the trajectory of the moving body.
本発明に係る位置推定方法、位置推定装置及び位置推定プログラムでは、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることが可能となる。 With the position estimation method, position estimation apparatus, and position estimation program according to the present invention, it is possible to improve the accuracy of self-position estimation based on information acquired by a sensor that measures the position of surrounding features.
以下、図面を参照しつつ、本発明の様々な実施形態について説明する。ただし、本発明の技術的範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶ点に留意されたい。 Hereinafter, various embodiments of the present invention will be described with reference to the drawings. However, it should be noted that the technical scope of the present invention is not limited to these embodiments, and extends to the invention described in the claims and equivalents thereof.
１．本実施形態の概略
図１は、本実施形態の位置推定システムの概要を説明するための模式図である。本実施形態の位置推定システムでは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物の複数のセンサ座標系の位置を示す点群ＰＴを出力するセンサを搭載した移動体の軌跡が推定される。位置推定装置は、コンピュータであるが、本発明が適用可能であればどのような形態でもよい。例えば、位置推定装置は、パーソナルコンピュータ、サーバ、ノートＰＣ、多機能携帯電話機（所謂「スマートフォン」）、携帯情報端末（Personal Digital Assistant, PDA）、タブレット端末、タブレットＰＣ、携帯型ゲーム機等でもよい。移動体は、自動車、原動機付自転車、又は軽車両（自転車等）等である。
1. Outline of the Present Embodiment FIG. 1 is a schematic diagram for explaining the outline of the position estimation system of the present embodiment. In the position estimation system of the present embodiment, a movement equipped with a sensor that measures the positions of a plurality of features every predetermined measurement time and outputs a point cloud PT indicating the positions of the plurality of sensor coordinate systems of the measured features. The trajectory of the body is estimated. The position estimation device is a computer, but may take any form as long as the present invention is applicable. For example, the position estimation device may be a personal computer, a server, a notebook PC, a multi-function mobile phone (so-called “smart phone”), a personal digital assistant (PDA), a tablet terminal, a tablet PC, a portable game machine, or the like. . The moving body is an automobile, a motorbike, a light vehicle (such as a bicycle), or the like.
センサは、周囲にある地物等の物体までの距離及び角度を連続して測定し、測定された距離及び角度に基づいてセンサ座標系の３次元位置を出力する。センサは、ＬｉＤＡＲ（Light Detection and Ranging）又はレーザレンジファインダ等のレーザ測距装置である。レーザ測距装置のレーザ照射部分は、パルス状に発光するレーザを照射して散乱光を測定し、照射から検出までの時間に基づいて反射した位置を測定する。レーザ照射部分は、垂直視野角約４０度の範囲を測定できるように３２個のレーザ送受信センサを備え、接地面と平行面上に３６０度回転する。このレーザ測距装置では、垂直視野角約４０度の範囲内において３２本のレーザが同時に照射され、レーザ照射部分の回転により、周囲３６０度の地物の位置が測定される。なお、センサとして、レーダ測距装置、撮像装置等が用いられてもよい。センサとして撮像装置が用いられる場合、例えば、２地点において、撮像装置がセンサの周囲にある同一物体を撮像することにより、三角測量によって物体の位置が測定される。 The sensor continuously measures a distance and an angle to an object such as a surrounding feature, and outputs a three-dimensional position of the sensor coordinate system based on the measured distance and angle. The sensor is a laser range finder such as LiDAR (Light Detection and Ranging) or a laser range finder. The laser irradiation portion of the laser distance measuring device measures the scattered light by irradiating a laser emitting light in a pulse shape, and measures the reflected position based on the time from irradiation to detection. The laser irradiation portion includes 32 laser transmission / reception sensors so that a range of a vertical viewing angle of about 40 degrees can be measured, and rotates 360 degrees on a plane parallel to the ground plane. In this laser distance measuring device, 32 laser beams are simultaneously irradiated within a range of a vertical viewing angle of about 40 degrees, and the position of a feature of 360 degrees around is measured by rotating the laser irradiation portion. Note that a radar distance measuring device, an imaging device, or the like may be used as the sensor. When an imaging device is used as a sensor, for example, at two points, the imaging device images the same object around the sensor, whereby the position of the object is measured by triangulation.
位置推定装置は、以下に説明する、取得工程、抽出工程、平面特定工程、算出工程、及び推定工程を実行する。図１（１）に示すとおり、位置推定装置が取得工程を実行する前に、センサによる測定が行われる。まず、センサが移動体に搭載され、移動体が移動を開始する。そして、移動体が移動を続けながら、センサは、所定の測定時間毎（例えば、０．１秒毎）にレーザ照射部分を１回転させながら、移動体周囲の地物までの距離及び角度を測定し、地物のセンサ座標系の３次元位置を複数出力する。以上により、センサによる測定が終了する。 The position estimation device executes an acquisition process, an extraction process, a plane identification process, a calculation process, and an estimation process, which will be described below. As shown in FIG. 1 (1), measurement by a sensor is performed before the position estimation device executes the acquisition process. First, the sensor is mounted on the moving body, and the moving body starts moving. While the moving body continues to move, the sensor measures the distance and angle to the features around the moving body while rotating the laser irradiation part once every predetermined measurement time (for example, every 0.1 second). A plurality of three-dimensional positions of the feature sensor coordinate system are output. Thus, the measurement by the sensor is completed.
取得工程として、位置推定装置は、センサから出力された点群をフレーム毎に取得する。すなわち、位置推定装置は、出力された複数の地物の３次元位置を示す点群ＰＴを、レーザ照射部分が１回転するたびに１フレームずつ順次取得する。以下、センサのレーザ照射部分が１回転されることによって取得された点群を１フレームの点群と称する。なお、点群ＰＴに含まれる各点は、各フレームにおいてセンサによって測定された測距値及び角度に基づく複数のセンサ座標系の３次元位置（ｘ、ｙ、ｚ）で表される。図１（２）に示すとおり、特定のフレームにおいて点群ＰＴによって示される位置は、センサ周囲の地物の表面上の位置である。 As the acquisition step, the position estimation device acquires the point group output from the sensor for each frame. That is, the position estimation device sequentially acquires the point group PT indicating the three-dimensional positions of the plurality of outputted features one frame at a time each time the laser irradiation portion makes one rotation. Hereinafter, a point group acquired by rotating the laser irradiation part of the sensor once is referred to as a point group of one frame. Each point included in the point group PT is represented by a three-dimensional position (x, y, z) of a plurality of sensor coordinate systems based on distance measurement values and angles measured by the sensors in each frame. As shown in FIG. 1 (2), the position indicated by the point group PT in a specific frame is a position on the surface of the feature around the sensor.
抽出工程として、位置推定装置は、取得した点群ＰＴから、平面を形成する点群を平面点群としてフレーム毎に抽出する。図１（３）に示すとおり、平面を形成する点群ＰＴとは、一辺ａが他の二辺ｂ，ｃよりも極めて短い直方体の内部に分布される点群ＰＴである。一辺ａが他の二辺ｂ，ｃよりも極めて短い場合とは、例えば、一辺ａが他の二辺ｂ，ｃの１／４００の長さよりも短い場合などである。位置推定装置は、点群に対して後述する主成分分析を行い、辺ａ，ｂ，ｃの軸を求めて平面点群を抽出する。なお、平面点群によって示される位置は、例えば、建築物等の地物の同一壁面上の位置を示していると推定される。 As an extraction step, the position estimation device extracts a point group forming a plane from the acquired point group PT as a plane point group for each frame. As shown in FIG. 1 (3), the point group PT forming a plane is a point group PT in which one side a is distributed inside a rectangular parallelepiped that is extremely shorter than the other two sides b and c. The case where one side a is extremely shorter than the other two sides b and c is, for example, the case where one side a is shorter than 1/400 of the other two sides b and c. The position estimation device performs principal component analysis described later on the point group, obtains axes of sides a, b, and c, and extracts a plane point group. In addition, it is estimated that the position shown by a plane point group has shown the position on the same wall surface of features, such as a building, for example.
平面特定工程として、位置推定装置は、平面点群により形成される平面の位置をフレーム毎に特定する。図１（４）に示すとおり、位置推定装置は、各フレームにおいて、平面点群に対して後述する主成分分析を行い、２次元平面ＰＬを特定する。 As the plane specifying step, the position estimation device specifies the position of the plane formed by the plane point group for each frame. As shown in FIG. 1 (4), the position estimation device performs a principal component analysis described later on the plane point group in each frame to identify the two-dimensional plane PL.
算出工程として、位置推定装置は、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトルＴ及び回転行列Ｒを算出する。まず、位置推定装置は、複数のフレーム毎に、各フレームにおける平面と次のフレームにおける平面とのマッチングを行う。次に、図１（５）に示すとおり、位置推定装置は、マッチングが成立したフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する。 As a calculation step, the position estimation device calculates the translation vector T and the rotation matrix R of the moving body between frames from the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame. To do. First, the position estimation apparatus performs matching between a plane in each frame and a plane in the next frame for each of a plurality of frames. Next, as illustrated in FIG. 1 (5), the position estimation apparatus calculates a translation vector T and a rotation matrix R between frames in which matching is established.
推定工程として、位置推定装置は、フレーム間の移動体の並進ベクトルＴ及び回転行列Ｒをフレーム毎に順次算出して、移動体の軌跡を推定する。すなわち、位置推定装置は、各フレームにおいて算出された移動体の並進ベクトルＴ及び回転行列Ｒを、フレームの順番に従って、累積加算していき移動体の移動軌跡を推定する。例えば図１（６）に示すとおり、位置推定装置は、測定開始位置の３次元位置（Ｘ，Ｙ，Ｚ）を（０，０，０）として、フレーム間の並進ベクトルＴ及び回転行列Ｒに基づいて移動体の軌跡を推定する。 As the estimation step, the position estimation apparatus sequentially calculates the translation vector T and the rotation matrix R of the moving body between frames for each frame, and estimates the trajectory of the moving body. That is, the position estimation apparatus accumulates and adds the translation vector T and the rotation matrix R of the moving body calculated in each frame according to the order of the frames, and estimates the moving locus of the moving body. For example, as shown in FIG. 1 (6), the position estimation apparatus sets the three-dimensional position (X, Y, Z) of the measurement start position as (0, 0, 0), and translates the translation vector T between frames and the rotation matrix R. Based on this, the trajectory of the moving object is estimated.
これにより、本実施形態の位置推定装置は、移動や揺れのない建築物等の地物の壁面に位置する平面点群を、位置推定の対象として抽出することができるため、自己位置推定の精度を向上させることが可能となる。 Thereby, since the position estimation apparatus of this embodiment can extract a plane point group located on the wall surface of a feature such as a building that does not move or shake as a target of position estimation, the accuracy of self-position estimation Can be improved.
２．位置推定システム１の構成
図２は、位置推定システム１の概略構成の一例を示す図である。
2. Configuration of Position Estimation System 1 FIG. 2 is a diagram illustrating an example of a schematic configuration of the position estimation system 1.
位置推定システム１は、位置推定装置２、センサ３、センサ３が搭載された移動体４を有する。センサ３は、測定された３次元位置の情報を出力するための出力インターフェース（図示せず）を備える。位置推定装置２は、センサ３と有線又は無線接続され、センサ３の出力インターフェース経由でセンサ３から出力された点群の情報を取得する。 The position estimation system 1 includes a position estimation device 2, a sensor 3, and a moving body 4 on which the sensor 3 is mounted. The sensor 3 includes an output interface (not shown) for outputting information on the measured three-dimensional position. The position estimation device 2 is wired or wirelessly connected to the sensor 3, and acquires the point cloud information output from the sensor 3 via the output interface of the sensor 3.
なお、センサ３は、出力インターフェースを介して、ＣＤ−ＲＯＭ（compact disk read only memory）、ＤＶＤ−ＲＯＭ（digital versatile disk read only memory）等のコンピュータ読み取り可能な可搬型記録媒体の記録装置と接続されてもよい。この場合、可搬型記録媒体の記録装置は、センサ３の出力インターフェースを介して入力された点群の置の情報を、可搬型記録媒体に記録する。そして、位置推定装置２は、点群の情報が記録された可搬型記録媒体を読み込むことにより、当該点群の情報を取得する。 The sensor 3 is connected to a computer-readable recording device such as a CD-ROM (compact disk read only memory) or a DVD-ROM (digital versatile disk read only memory) via an output interface. May be. In this case, the recording device of the portable recording medium records the information on the position of the point group input via the output interface of the sensor 3 on the portable recording medium. And the position estimation apparatus 2 acquires the information of the said point group by reading the portable recording medium with which the information of the point group was recorded.
２．１．位置推定装置２の構成
図３は、位置推定装置２の概略構成の一例を示す図である。また、図４は、記憶部２１が記憶する点群テーブルのデータ構造の一例を示す図である。
2.1. Configuration of Position Estimation Device 2 FIG. 3 is a diagram illustrating an example of a schematic configuration of the position estimation device 2. FIG. 4 is a diagram illustrating an example of a data structure of a point cloud table stored in the storage unit 21.
図３に示すように、位置推定装置２は、記憶部２１と、入力部２２と、出力部２３と、処理部２４とを備える。位置推定装置２は、センサから出力された点群を取得し、取得された点群に基づいてセンサを搭載した移動体の軌跡を推定する。 As illustrated in FIG. 3, the position estimation device 2 includes a storage unit 21, an input unit 22, an output unit 23, and a processing unit 24. The position estimation device 2 acquires the point cloud output from the sensor, and estimates the trajectory of the moving object on which the sensor is mounted based on the acquired point cloud.
記憶部２１は、例えば、磁気テープ装置、磁気ディスク装置、又は光ディスク装置のうちの少なくとも一つを備える。記憶部２１は、処理部２４での処理に用いられるオペレーティングシステムプログラム、ドライバプログラム、アプリケーションプログラム、データ等を記憶する。例えば、記憶部２１は、アプリケーションプログラムとして、点群を取得する処理、取得された点群に基づく移動体の位置推定処理、及び位置推定処理の結果に係る表示データを作成する処理等を、処理部２４に実行させるための位置推定プログラム等を記憶する。位置推定プログラムは、例えばＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ等のコンピュータ読み取り可能な可搬型記録媒体から、公知のセットアッププログラム等を用いて記憶部２１にインストールされてもよい。 The storage unit 21 includes, for example, at least one of a magnetic tape device, a magnetic disk device, and an optical disk device. The storage unit 21 stores an operating system program, a driver program, an application program, data, and the like used for processing in the processing unit 24. For example, the storage unit 21 performs, as an application program, a process for acquiring a point cloud, a position estimation process for a moving object based on the acquired point cloud, a process for creating display data related to the result of the position estimation process, and the like. A position estimation program to be executed by the unit 24 is stored. The position estimation program may be installed in the storage unit 21 from a computer-readable portable recording medium such as a CD-ROM or DVD-ROM using a known setup program or the like.
また、記憶部２１は、データとして、取得した点群を管理するための点群テーブル等を記憶する。さらに、記憶部２１は、所定の処理に係る一時的なデータを一時的に記憶してもよい。 In addition, the storage unit 21 stores a point cloud table and the like for managing the acquired point cloud as data. Furthermore, the storage unit 21 may temporarily store temporary data related to a predetermined process.
図４は、点群テーブルのデータ構造の一例を示す図である。 FIG. 4 is a diagram illustrating an example of the data structure of the point cloud table.
点群テーブルには、垂直視野角約４０度の範囲内において３２本のレーザを同時に照射し、レーザ照射部分の回転により周囲３６０度の地物の位置を測定するセンサによる測定結果のデータが格納される。図４に示すように、点群テーブルには、レーザＩＤ毎に、フレーム番号と、レーザの照射によって測定された測距値及び各レーザに固有の垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）とが、測定時間に従って順次関連付けられて記憶される。なお、１回のレーザの照射によって測定された測距値及び垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）が、１レコードとして記憶され、各レコードは、測定された順番に、レコード１、レコード２、レコード３、・・・として記憶される。レーザＩＤは、３２本のレーザをそれぞれ識別するための識別情報である。フレーム番号は、フレームを識別するための番号であり、レーザ照射部分が１回転するたびに時系列順に付与される。 The point cloud table stores data of measurement results from sensors that simultaneously irradiate 32 lasers within a vertical viewing angle of about 40 degrees and measure the position of features around 360 degrees by rotating the laser irradiation part. Is done. As shown in FIG. 4, the point cloud table includes, for each laser ID, a three-dimensional position of a sensor coordinate system based on a frame number, a distance measurement value measured by laser irradiation, and a vertical viewing angle unique to each laser. (X, y, z) are sequentially associated and stored according to the measurement time. The three-dimensional position (x, y, z) of the sensor coordinate system based on the distance measurement value and the vertical viewing angle measured by one laser irradiation is stored as one record, and each record is measured. In order, they are stored as record 1, record 2, record 3,. The laser ID is identification information for identifying each of the 32 lasers. The frame number is a number for identifying a frame, and is assigned in chronological order every time the laser irradiation portion rotates once.
図３に戻り、入力部２２は、データの入力が可能であればどのようなデバイスでもよく、例えば、タッチパネル、キーボタン等である。操作者は、入力部２２を用いて、文字、数字、記号等を入力することができる。入力部２２は、操作者により操作されると、その操作に対応する信号を生成する。そして、生成された信号は、操作者の指示として、処理部２４に供給される。 Returning to FIG. 3, the input unit 22 may be any device that can input data, such as a touch panel, a key button, or the like. The operator can input characters, numbers, symbols, and the like using the input unit 22. When operated by the operator, the input unit 22 generates a signal corresponding to the operation. The generated signal is supplied to the processing unit 24 as an instruction from the operator.
出力部２３も、映像や画像等の表示が可能であればどのようなデバイスでもよく、例えば、液晶ディスプレイや有機ＥＬ（Electro−Luminescence）ディスプレイ等である。出力部２３は、処理部２４から供給された映像データに応じた映像や、画像データに応じた画像等を表示する。また、出力部２３は、紙などの表示媒体に、映像、画像又は文字等を印刷する出力装置であってもよい。 The output unit 23 may be any device as long as it can display images, images, and the like, such as a liquid crystal display or an organic EL (Electro-Luminescence) display. The output unit 23 displays a video corresponding to the video data supplied from the processing unit 24, an image corresponding to the image data, and the like. The output unit 23 may be an output device that prints video, images, characters, or the like on a display medium such as paper.
処理部２４は、一又は複数個のプロセッサ及びその周辺回路を有する。処理部２４は、位置推定装置２の全体的な動作を統括的に制御するものであり、例えば、ＣＰＵである。処理部２４は、記憶部２１に記憶されているプログラム（ドライバプログラム、オペレーティングシステムプログラム、アプリケーションプログラム等）に基づいて処理を実行する。また、処理部２４は、複数のプログラム（アプリケーションプログラム等）を並列に実行できる。 The processing unit 24 includes one or a plurality of processors and their peripheral circuits. The processing unit 24 comprehensively controls the overall operation of the position estimation device 2 and is, for example, a CPU. The processing unit 24 executes processing based on programs (driver program, operating system program, application program, etc.) stored in the storage unit 21. The processing unit 24 can execute a plurality of programs (such as application programs) in parallel.
２．２．処理部２４の構成
処理部２４は、取得部２４１、抽出部２４２、平面特定部２４３、算出部２４４、最適化部２４５、及び推定部２４６を備える。これらの各部は、処理部２４が備えるプロセッサで実行されるプログラムにより実現される機能モジュールである。あるいは、これらの各部は、ファームウェアとして位置推定装置２に実装されてもよい。
2.2. Configuration of Processing Unit 24 The processing unit 24 includes an acquisition unit 241, an extraction unit 242, a plane identification unit 243, a calculation unit 244, an optimization unit 245, and an estimation unit 246. Each of these units is a functional module realized by a program executed by a processor included in the processing unit 24. Or these each part may be mounted in the position estimation apparatus 2 as firmware.
図５は、位置推定処理の動作フローの一例を示す図である。この動作フローは、予め記憶部２１に記憶されているプログラムに基づいて、主に処理部２４により、位置推定装置２の各要素と協働して実行される。 FIG. 5 is a diagram illustrating an example of an operation flow of the position estimation process. This operation flow is executed mainly by the processing unit 24 in cooperation with each element of the position estimation device 2 based on a program stored in the storage unit 21 in advance.
まず、センサが、移動体が移動している間に回転されるレーザ照射部分によって測定された地物の位置を示す点群ＰＴを出力し、位置推定装置２の取得部２４１は、出力された点群ＰＴを１回転毎に１フレームずつ順次取得する（ステップＳ１０１）。取得部２４１は、取得した点群ＰＴを記憶部２１の点群テーブルに記憶する。 First, the sensor outputs a point cloud PT indicating the position of the feature measured by the laser irradiation portion rotated while the moving body is moving, and the acquisition unit 241 of the position estimation device 2 outputs the point cloud PT. The point cloud PT is acquired sequentially for each frame per rotation (step S101). The acquisition unit 241 stores the acquired point group PT in the point group table of the storage unit 21.
図６は、１フレームの点群をマッピングした３次元空間の一例を示す模式図である。図６に示すように、１フレームの点群は、３２本のレーザを同時に照射するセンサのレーザ照射部分の回転によって測定された、周囲３６０度の複数の地物のセンサ座標系の３次元位置である。以下、１フレームにおけるセンサ１本分の点群を１レイヤーの点群と称する。 FIG. 6 is a schematic diagram illustrating an example of a three-dimensional space in which point frames of one frame are mapped. As shown in FIG. 6, a point group of one frame is a three-dimensional position of a sensor coordinate system of a plurality of features around 360 degrees measured by rotation of a laser irradiation portion of a sensor that simultaneously irradiates 32 lasers. It is. Hereinafter, a point group for one sensor in one frame is referred to as a point group of one layer.
図５に戻り、次に、位置推定装置２の抽出部２４２及び平面特定部２４３は、取得部２４１によって点群ＰＴが取得されると、平面を抽出するための平面抽出処理を実行する（ステップＳ１０２）。平面抽出処理の詳細については後述する。 Returning to FIG. 5, when the point group PT is acquired by the acquisition unit 241, the extraction unit 242 and the plane identification unit 243 of the position estimation device 2 next execute a plane extraction process for extracting a plane (step). S102). Details of the plane extraction process will be described later.
次に、位置推定装置２の算出部２４４は、抽出部２４２及び平面特定部２４３によって平面抽出処理が行われると、並進ベクトル・回転行列算出処理を実行する（ステップＳ１０３）。並進ベクトル・回転行列算出処理の詳細については後述する。 Next, when the extraction unit 242 and the plane specifying unit 243 perform the plane extraction process, the calculation unit 244 of the position estimation apparatus 2 executes a translation vector / rotation matrix calculation process (step S103). Details of the translation vector / rotation matrix calculation process will be described later.
次に、位置推定装置２の最適化部２４５は、算出部２４４によって並進ベクトル・回転行列算出処理が実行されると、最適化処理を実行する（ステップＳ１０４）。最適化処理の詳細については後述する。 Next, when the translation unit / rotation matrix calculation process is executed by the calculation unit 244, the optimization unit 245 of the position estimation apparatus 2 executes the optimization process (step S104). Details of the optimization process will be described later.
そして、位置推定装置２の推定部２４６は、最適化部２４５によって最適化処理が実行されると、推定処理を実行し（ステップＳ１０５）、一連のステップを終了する。なお、位置推定処理の一連のステップが終了すると、操作者の指示に応じて、位置推定装置２の処理部２４は、出力部２３に、位置推定処理の結果（移動体の移動軌跡等）を表示する。 Then, when the optimization process is executed by the optimization unit 245, the estimation unit 246 of the position estimation apparatus 2 executes the estimation process (step S105) and ends a series of steps. When a series of steps of the position estimation process is completed, the processing unit 24 of the position estimation apparatus 2 sends the result of the position estimation process (such as a moving locus of the moving body) to the output unit 23 in accordance with an instruction from the operator. indicate.
図７は、位置推定装置２の抽出部２４２及び平面特定部２４３による平面抽出処理の動作フローの一例を示す図である。図７に示す平面抽出処理は、図５のステップＳ１０２において実行される。 FIG. 7 is a diagram illustrating an example of an operation flow of plane extraction processing by the extraction unit 242 and the plane identification unit 243 of the position estimation device 2. The plane extraction process shown in FIG. 7 is executed in step S102 of FIG.
まず、抽出部２４２は、点群テーブルに記憶された複数のフレームの点群のうち、１つのフレームの点群を取得する（ステップＳ２０１）。以下のステップＳ２０２〜Ｓ２０７は、取得された１つのフレームの点群に対して実行される。 First, the extraction unit 242 acquires a point group of one frame among the point groups of a plurality of frames stored in the point group table (step S201). The following steps S202 to S207 are performed on the acquired point group of one frame.
次に、抽出部２４２は、レイヤー毎に、取得した点群のうちの所定の点と次の点との間の距離を算出する（ステップＳ２０２）。すなわち、抽出部２４２は、レーザＩＤ毎に、所定のレコードの３次元位置（ｘ、ｙ、ｚ）と、当該レコードの次のレコードの３次元位置（ｘ、ｙ、ｚ）とを特定し、特定した両位置を端点とする線分の距離を算出する。以下、算出された距離を点間距離と称する。 Next, the extraction unit 242 calculates a distance between a predetermined point in the acquired point group and the next point for each layer (step S202). That is, the extraction unit 242 specifies, for each laser ID, the three-dimensional position (x, y, z) of a predetermined record and the three-dimensional position (x, y, z) of the next record of the record, The distance of the line segment with the specified positions as end points is calculated. Hereinafter, the calculated distance is referred to as a point-to-point distance.
次に、抽出部２４２は、各レイヤーの点群を点間距離に基づいてセグメントに分類する（ステップＳ２０３）。すなわち、抽出部２４２は、レーザＩＤ毎に、レコードの順番に従って各点間距離を累積加算した値が閾値を超えた場合、当該閾値を超える前までに累積加算した全ての点間距離における端点を示す各点を１つのセグメントとする。なお、以下、セグメントに含まれる全ての点を、セグメントに含まれる点群と称する場合がある。図８は、特定の１つのレイヤーにおける全てのセグメントをマッピングした２次元空間の一例を示す模式図である。 Next, the extraction unit 242 classifies the point group of each layer into segments based on the inter-point distance (step S203). That is, for each laser ID, if the value obtained by cumulatively adding the distance between points in accordance with the order of records exceeds the threshold value, the extraction unit 242 calculates the end points at all the distances between points that have been cumulatively added before the threshold value is exceeded. Each point shown is a segment. Hereinafter, all points included in the segment may be referred to as a point group included in the segment. FIG. 8 is a schematic diagram showing an example of a two-dimensional space in which all segments in one specific layer are mapped.
次に、抽出部２４２は、Ｓ２０３において分割したセグメントのうち、セグメントに含まれる点群の数が所定数以上であり且つセグメントに含まれる点群の形状が直線状であるセグメントを検出する（ステップＳ２０４）。ステップＳ２０４では、まず、抽出部２４２は、セグメントに含まれる点群の数が所定数以上であるか否か判断する。次に、抽出部２４２は、点群の数が所定数以上であるセグメントに含まれる全て点群に対して、主成分分析を行う。次に、抽出部２４２は、第１主成分の分散が第２主成分の分散及び第３主成分の分散よりも所定比率以上大きい場合，主成分分析を行った点群を含むセグメントを直線状のセグメントとして検出する。これにより、セグメントに含まれる点群によって形成される形状が、第１主成分を軸とする直線状であると判定される。 Next, the extraction unit 242 detects a segment in which the number of point groups included in the segment is equal to or greater than a predetermined number and the shape of the point group included in the segment is a straight line among the segments divided in S203 (step S202 S204). In step S204, first, the extraction unit 242 determines whether or not the number of point groups included in the segment is a predetermined number or more. Next, the extraction unit 242 performs principal component analysis on all point groups included in the segment in which the number of point groups is a predetermined number or more. Next, when the variance of the first principal component is larger than the variance of the second principal component and the variance of the third principal component by a predetermined ratio or more, the extraction unit 242 linearly segments the segment including the point group subjected to the principal component analysis. Detect as a segment. Thereby, it is determined that the shape formed by the point group included in the segment is a straight line having the first main component as an axis.
図９は、特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図であり、図１０は、特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。 FIG. 9 is a schematic diagram illustrating an example of a two-dimensional space in which each linear segment in a specific layer is mapped, and FIG. 10 is a three-dimensional diagram in which each linear segment in a specific frame is mapped. It is a schematic diagram which shows an example of space.
次に、抽出部２４２は、Ｓ２０４において特定された直線状のセグメントの法線ベクトルを算出する（ステップＳ２０５）。ステップＳ２０５では、まず、抽出部２４２は、Ｓ２０４において特定された全ての直線状のセグメントを取得する。次に、抽出部２４２は、取得した全ての直線状のセグメントのそれぞれに対して、各直線状のセグメントを中心とした所定範囲内の他の直線状のセグメントを検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された他の直線状のセグメントとの内積のなす角が所定閾値以下の他の直線状のセグメントを近傍セグメントとして検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された近傍セグメントとに含まれる点群に対して主成分分析を行う。そして、抽出部２４２は、第１主成分の分散及び第２主成分の分散が第３主成分の分散よりも一定比率以上（例えば、４００倍以上）大きい場合、第１主成分及び第２主成分を軸とした平面に対する法線ベクトルを、各直線状のセグメントの法線ベクトルとして算出する。以下、図１１（ａ）及び（ｂ）を参照して、ステップＳ２０５の処理について具体的に説明する。 Next, the extraction unit 242 calculates a normal vector of the linear segment specified in S204 (step S205). In step S205, first, the extraction unit 242 acquires all the linear segments specified in S204. Next, the extraction unit 242 detects, for each of all acquired linear segments, other linear segments within a predetermined range centered on each linear segment. Next, the extraction unit 242 nearby another linear segment whose angle formed by the inner product of each linear segment and the other linear segments detected for each linear segment is equal to or less than a predetermined threshold value. Detect as a segment. Next, the extraction unit 242 performs principal component analysis on a point group included in each linear segment and a neighboring segment detected for each linear segment. When the variance of the first principal component and the variance of the second principal component are larger than the variance of the third principal component by a certain ratio or more (for example, 400 times or more), the extraction unit 242 A normal vector with respect to a plane with the component as an axis is calculated as a normal vector of each linear segment. Hereinafter, with reference to FIGS. 11A and 11B, the process of step S205 will be described in detail.
図１（ａ）は、直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥ毎に、ステップＳ２０４において検出された直線状のセグメントが特定されている。例えば、セグメントＳ１５は、レイヤーＡにおける直線状のセグメントである。また、セグメントＳ１１、Ｓ１２、Ｓ１６は、レイヤーＢにおける直線状のセグメントである。また、セグメントＳ１３、Ｓ１４は、レイヤーＣにおける直線状のセグメントである。なお、図１１（ａ）及び（ｂ）において示される全てのセグメントは、直線状のセグメントである。図１１（ａ）に示すように、抽出部２４２は、セグメントＳ１１を中心とした所定範囲内にある他のセグメントＳ１２〜Ｓ１６を検出する。次に、抽出部２４２は、セグメントＳ１１とセグメントＳ１２〜Ｓ１６のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ａ）では、セグメントＳ１２〜Ｓ１６の全てにおいて、セグメントＳ１１との内積のなす角度が所定閾値以下であるので、抽出部２４２は、セグメントＳ１１〜Ｓ１６に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ１１の法線ベクトルとして特定する。 Fig.1 (a) is a schematic diagram for demonstrating an example of the normal vector in a linear segment. For each of the layers A, B, C, D, and E, the linear segment detected in step S204 is specified. For example, the segment S 15 is a linear segment in the layer A. Segments S 11 , S 12 , and S 16 are linear segments in the layer B. The segments S 13 and S 14 are linear segments in the layer C. Note that all segments shown in FIGS. 11A and 11B are linear segments. As illustrated in FIG. 11A, the extraction unit 242 detects other segments S 12 to S 16 that are within a predetermined range centered on the segment S 11 . Next, the extraction unit 242, the angle of the inner product of the respective segments S 11 and segment S 12 to S 16 is a predetermined threshold value (e.g., 60 degrees) to determine whether or less. In FIG. 11A, in all of the segments S 12 to S 16 , the angle formed by the inner product with the segment S 11 is equal to or less than a predetermined threshold value, so that the extraction unit 242 includes the point group included in the segments S 11 to S 16. The normal vector is calculated based on the above, and the calculated normal vector is specified as the normal vector in S11.
図１１（ｂ）は、直線状のセグメントにおける法線ベクトルの他の一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥは、図１１（ａ）と同様である。セグメントＳ２４、Ｓ２５は、レイヤーＣにおける直線状のセグメントである。また、セグメントＳ２１、Ｓ２３、Ｓ２６は、レイヤーＤにおける直線状のセグメントである。また、セグメントＳ２２は、レイヤーＥにおける直線状のセグメントである。図１１（ｂ）に示すように、抽出部２４２は、セグメントＳ２１を中心とした所定範囲内にある他のセグメントＳ２２〜Ｓ２６を検出する。次に、抽出部２４２は、セグメントＳ２１とセグメントＳ２２〜Ｓ２６のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ｂ）では、セグメントＳ２２〜Ｓ２５のそれぞれとセグメントＳ２１との内積のなす角度は、それぞれ所定閾値以下であり、セグメントＳ２６とセグメントＳ２１との内積のなす角度は所定閾値よりも大きい。したがって、抽出部２４２は、セグメントＳ２１〜Ｓ２５に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ２１の法線ベクトルとして特定する。 FIG. 11B is a schematic diagram for explaining another example of a normal vector in a linear segment. Layers A, B, C, D, and E are the same as in FIG. The segments S 24 and S 25 are linear segments in the layer C. Segments S 21 , S 23 , and S 26 are linear segments in the layer D. The segment S 22 is a linear segment in the layer E. As illustrated in FIG. 11B, the extraction unit 242 detects other segments S 22 to S 26 that are within a predetermined range with the segment S 21 as the center. Next, the extraction unit 242, the angle of the inner product of the respective segments S 21 and the segment S 22 to S 26 is a predetermined threshold value (e.g., 60 degrees) to determine whether or less. In FIG. 11B, the angle formed by the inner product of each of the segments S 22 to S 25 and the segment S 21 is not more than a predetermined threshold value, and the angle formed by the inner product of the segment S 26 and the segment S 21 is equal to the predetermined threshold value. Bigger than. Therefore, the extraction unit 242 calculates a normal vector based on the point group included in the segments S 21 to S 25 and specifies the calculated normal vector as the normal vector of S 21 .
図１２は、特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。 FIG. 12 is a schematic diagram illustrating an example of a three-dimensional space in which normal vectors of each linear segment in one specific frame are mapped.
図７に戻り、次に、抽出部２４２及び平面特定部２４３は、平面を形成する点群を平面点群として抽出する（ステップＳ２０６）。ステップＳ２０６では、まず、抽出部２４２は、各直線状のセグメントを無向グラフの節点ｖkとして扱い、法線ベクトル間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖi、ｖjに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、２つの直線状のセグメントの法線ベクトルの内積が所定閾値（例えば、１０度）以内である場合、法線ベクトル間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ａij＝１とし、無向辺が存在しない場合、ａij＝０とする隣接行列Ａ＝［ａij］を作成する。なお、各直線状のセグメントの数がｎである場合、ｉ＝１，２，３，・・・，ｎであり、ｊ＝１，２，３，・・・，ｎであり、隣接行列Ａ＝［ａij］は、ｎ×ｎの正方行列である。このようにして、各直線状のセグメントの法線ベクトル間の類似度を示す隣接行列Ａ＝［ａij］が作成される。 Returning to FIG. 7, next, the extraction unit 242 and the plane identification unit 243 extract a point group forming a plane as a plane point group (step S <b> 206). In step S206, the extraction unit 242 first treats each linear segment as a node v k of the undirected graph, and two nodes v i corresponding to two linear segments having high similarity between normal vectors. , V j , it is determined that an undirected side exists. For example, when the inner product of the normal vectors of two linear segments is within a predetermined threshold (for example, 10 degrees), the extraction unit 242 determines that the similarity between the normal vectors is high. The extraction unit 242 creates an adjacency matrix A = [a ij ] with a ij = 1 when there is an undirected side and a ij = 0 when there is no undirected side. When the number of each linear segment is n, i = 1, 2, 3,..., N, j = 1, 2, 3,. = [A ij ] is an n × n square matrix. In this way, the adjacency matrix A = [a ij ] indicating the similarity between the normal vectors of each linear segment is created.
次に、抽出部２４２は、隣接行列Ａの作成処理と同様に、各直線状のセグメントを無向グラフの節点ｖkとして扱い、各直線状のセグメントの位置間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖi、ｖjに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、特定の直線状のセグメントと他の直線状のセグメントとの間の距離を算出し、特定の直線状のセグメントとの距離が短い所定数（例えば、５つ）の他の直線状のセグメントを特定する。そして、特定の直線状のセグメントと、特定の直線状のセグメントとの距離が短い所定数の他の直線状のセグメントとの間において、位置間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ｂij＝１とし、無向辺が存在しない場合、ｂij＝０とする隣接行列Ｂ＝［ｂij］を作成する。このようにして、各直線状のセグメントの位置間の類似度を示す隣接行列Ｂ＝［ｂij］が作成される。 Next, as in the process of creating the adjacency matrix A, the extraction unit 242 treats each linear segment as a node v k of the undirected graph, and two straight lines with high similarity between the positions of the linear segments. It is determined that an undirected side exists for the two nodes v i and v j corresponding to the segment. For example, the extraction unit 242 calculates a distance between a specific linear segment and another linear segment, and other than a predetermined number (for example, five) having a short distance from the specific linear segment. Identify linear segments of. Then, it is determined that the degree of similarity between the positions is high between the specific linear segment and a predetermined number of other linear segments with a short distance between the specific linear segment. The extraction unit 242 creates an adjacency matrix B = [b ij ] with b ij = 1 when there is an undirected side and b ij = 0 when there is no undirected side. In this way, the adjacency matrix B = [b ij ] indicating the similarity between the positions of the respective linear segments is created.
次に、抽出部２４２は、次の式（１）によって、隣接行列Ａ及び隣接行列Ｂの要素積Ｃを算出する。 Next, the extraction unit 242 calculates an element product C of the adjacency matrix A and the adjacency matrix B by the following equation (1).
次に、抽出部２４２は、次の式（２）によって、ベキ乗行列のブール和を算出する。 Next, the extraction unit 242 calculates the Boolean sum of the power matrix by the following equation (2).
次に、抽出部２４２は、次の式（３）に示すとおり、ベキ乗行列のブール和ＣkとＣk+1とが変わらなくなる場合におけるＣk（又はＣk+1）を可到達行列Ｄとして算出する。 Next, as shown in the following equation (3), the extraction unit 242 calculates C k (or C k + 1 ) when the Boolean sum C k and C k + 1 of the power matrix remain unchanged as the reachable matrix. Calculate as D.
次に、抽出部２４２は、可到達行列Ｄに対して、ガウスの消去法の前進消去を行うことにより重複する行を削除し、Ｄの上三角行列を算出する。次に、抽出部２４２は、Ｄの上三角行列において、要素が０でない行を１つの平面点群であると判定し、列に各平面点群に属する直線状のセグメントを抽出する。なお、行列のランクは、当該フレームにおける平面点群の総数である。 Next, the extraction unit 242 deletes duplicate rows by performing forward erasure using Gaussian elimination on the reachable matrix D, and calculates an upper triangular matrix of D. Next, the extraction unit 242 determines that a row whose elements are not 0 in the upper triangular matrix of D is one plane point group, and extracts a linear segment belonging to each plane point group in a column. Note that the rank of the matrix is the total number of plane point groups in the frame.
そして、平面特定部２４３は、抽出された各平面点群に属する直線状のセグメントの点群に対し主成分分析を行う。平面特定部２４３は、主成分分析によって算出された第１主成分及び第２主成分に基づき各平面点群の平面の式を算出することにより、平面の位置を特定する。以上により、ステップＳ２０６の処理が終了する。図１３は、特定の１つのフレームにおいて、同一の平面を形成する平面点群をマッピングした３次元空間の一例を示す模式図である。 Then, the plane specifying unit 243 performs principal component analysis on the point group of the linear segments belonging to each extracted plane point group. The plane specifying unit 243 specifies the plane position by calculating the plane expression of each plane point group based on the first principal component and the second principal component calculated by the principal component analysis. Thus, the process of step S206 ends. FIG. 13 is a schematic diagram showing an example of a three-dimensional space in which plane point groups forming the same plane are mapped in a specific one frame.
図７に戻り、次に、平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームであるか否かを判定する（ステップＳ２０７）。平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームでない場合（ステップＳ２０７−Ｎｏ）、ステップＳ２０１に処理を戻す。 Returning to FIG. 7, next, the plane specifying unit 243 determines whether or not the frame executed in steps S201 to S206 is the last frame stored in the point cloud table (step S207). If the frame executed in steps S201 to S206 is not the last frame stored in the point cloud table (step S207—No), the plane specifying unit 243 returns the process to step S201.
平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームである場合（ステップＳ２０７−Ｙｅｓ）、一連のステップを終了する。 If the frame executed in steps S201 to S206 is the last frame stored in the point cloud table (step S207—Yes), the plane specifying unit 243 ends the series of steps.
図１４は、位置推定装置２の算出部２４４による並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。図１４に示す並進ベクトル・回転行列算出処理は、図５のステップＳ１０３において実行される。 FIG. 14 is a diagram illustrating an example of an operation flow of translation vector / rotation matrix calculation processing by the calculation unit 244 of the position estimation device 2. The translation vector / rotation matrix calculation process shown in FIG. 14 is executed in step S103 of FIG.
まず、算出部２４４は、抽出部２４２がステップＳ２０６において抽出した複数のフレームの平面点群のうち、特定のフレームの平面点群及び特定のフレームの次のフレームの平面点群を取得する（ステップＳ３０１）。以下、特定のフレームの次のフレームを、単に次のフレームと称する場合がある。 First, the calculation unit 244 acquires a plane point group of a specific frame and a plane point group of the next frame after the specific frame among the plane point groups of the plurality of frames extracted by the extraction unit 242 in Step S206 (Step S206). S301). Hereinafter, the frame next to a specific frame may be simply referred to as the next frame.
まず、算出部２４４は、ステップＳ３０１で取得した特定のフレームの平面点群と次のフレームの平面点群とのマッチング候補を検出する（ステップＳ３０２）。ステップＳ３０２では、まず、算出部２４４は、特定のフレームの各平面点群の重心位置及び次のフレームの各平面点群の重心位置を算出し、特定のフレームの各平面点群の重心位置と次のフレームの各平面点群の重心位置との重心間距離を算出する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトル及び次のフレームの各平面点群の法線ベクトルを算出する。なお、算出部２４４が実行する法線ベクトルの算出処理は、抽出部２４２がステップＳ２０５において実行する法線ベクトルの算出処理と同様の処理である。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、特定のフレームの各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であると判定された場合、当該特定のフレームの平面点群と当該次のフレームの平面点群とを互いにマッチング候補として関連付ける。以上により、ステップＳ３０２の処理が終了する。 First, the calculation unit 244 detects matching candidates between the plane point group of the specific frame acquired in step S301 and the plane point group of the next frame (step S302). In step S302, the calculation unit 244 first calculates the centroid position of each plane point group of a specific frame and the centroid position of each plane point group of the next frame, and calculates the centroid position of each plane point group of the specific frame. The distance between the centroids with the centroid position of each plane point group of the next frame is calculated. Next, the calculation unit 244 calculates a normal vector of each plane point group of a specific frame and a normal vector of each plane point group of the next frame. The normal vector calculation process executed by the calculation unit 244 is similar to the normal vector calculation process executed by the extraction unit 242 in step S205. Next, the calculation unit 244 calculates the normal vector of the plane point group of the next frame with the shortest distance between the center of gravity between each plane point group of the specific frame and each plane point group of the specific frame. It is determined whether or not the angle formed by is less than or equal to a threshold value. Next, when the calculation unit 244 determines that the angle formed between the normal vector of each plane point group of a specific frame and the normal vector of the plane point group of the next frame is equal to or less than the threshold value, the calculation unit 244 The plane point group of the next frame and the plane point group of the next frame are associated with each other as matching candidates. Thus, the process of step S302 ends.
算出部２４４が実行するステップＳ３０２の処理により、特定のフレームの平面点群と次のフレームの平面点群との誤マッチングを防止することができる。以下、図１５を参照して、隣り合う２つのフレームの平面点群のマッチングについて具体的に説明する。 By the process of step S302 executed by the calculation unit 244, it is possible to prevent erroneous matching between the plane point group of a specific frame and the plane point group of the next frame. Hereinafter, with reference to FIG. 15, the matching of the plane point group of two adjacent frames will be described in detail.
図１５は、隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。図１５では、平面点群Ａ１及びＡ２は、特定のフレームの平面点群であり、平面点群Ｂ１及びＢ２は、次のフレームの平面点群である。センサ３を搭載した移動体４の移動方向は、図１５に示す矢印のとおりであり、特定のフレームの平面点群Ａ１が位置する実際の平面は、次のフレームの平面点群Ｂ１が位置する平面であり。特定のフレームの平面点群Ａ２が位置する実際の平面は、次のフレームの平面点群Ｂ２が位置する平面である。 FIG. 15 is a schematic diagram for explaining the matching of plane point groups of two adjacent frames. In FIG. 15, plane point groups A1 and A2 are plane point groups of a specific frame, and plane point groups B1 and B2 are plane point groups of the next frame. The moving direction of the moving body 4 on which the sensor 3 is mounted is as shown by the arrow in FIG. 15, and the plane point group B1 of the next frame is positioned on the actual plane where the plane point group A1 of the specific frame is positioned. It is a plane. The actual plane where the plane point group A2 of a specific frame is located is the plane where the plane point group B2 of the next frame is located.
特定のフレームの各平面点群に対して、各平面点群の法線ベクトルとの重心間距離が一番短い次のフレームの平面点群をマッチング候補とした場合、図１５に示す模式図では、平面点群Ｂ２が平面点群Ａ１のマッチング候補となる。このような誤マッチングを防止するため、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。これにより、平面点群Ａ１の法線ベクトルと平面点群Ｂ２の法線ベクトルとの誤マッチングを防止することができる。 When the plane point group of the next frame with the shortest distance between the center of gravity with the normal vector of each plane point group is set as a matching candidate for each plane point group of a specific frame, the schematic diagram shown in FIG. The plane point group B2 is a matching candidate for the plane point group A1. In order to prevent such false matching, the calculation unit 244 calculates the normal point vector of each plane point group of a specific frame and the plane point group of the next frame with the shortest distance between the centroids of each plane point group. It is determined whether the angle formed with the normal vector is equal to or less than a threshold value. Thereby, erroneous matching between the normal vector of the plane point group A1 and the normal vector of the plane point group B2 can be prevented.
図１４に戻り、次に、算出部２４４は、特定のフレームのマッチング候補の平面点群と次のフレームのマッチング候補の平面点群とのマッチングを判定する（ステップＳ３０３）。 Returning to FIG. 14, the calculation unit 244 next determines matching between the plane point group of the matching candidate of the specific frame and the plane point group of the matching candidate of the next frame (step S <b> 303).
ステップＳ３０３では、まず、算出部２４４は、マッチング候補として互いに関連付けられた特定のフレームの平面点群及び次のフレームの平面点群を取得し、特定のフレームの平面点群に対して主成分分析を行う。算出部２４４は、図１６（ａ）に示すように、主成分分析によって算出された第１主成分及び第２主成分を軸とした２次元平面を算出し、算出された２次元平面に、特定のフレームのマッチング候補の平面点群を射影することにより次元圧縮を行う。次に、算出部２４４は、図１６（ｂ）に示すように、特定のフレームのマッチング候補の平面点群に対して算出された２次元平面に、次のフレームのマッチング候補の平面点群を射影する。次に、算出部２４４は、図１７に示すように、次元圧縮された特定のフレームのマッチング候補の平面点群に対してドロネー三角形分割を求め、求められたドロネー三角形分割に基づいてＣｏｎｖｅｘ ｈｕｌｌを作成する。次に、算出部２４４は、作成したＣｏｎｖｅｘ ｈｕｌｌの内部に、射影された次のフレームのマッチング候補の平面点群が包含される包含率を算出し、算出された包含率が所定の閾値（例えば、６０％）以上である場合は、マッチングが成立したと判定する。以上により、ステップＳ３０３の処理が終了する。 In step S303, first, the calculation unit 244 acquires a plane point group of a specific frame and a plane point group of the next frame that are associated with each other as matching candidates, and performs principal component analysis on the plane point group of the specific frame. I do. As illustrated in FIG. 16A, the calculation unit 244 calculates a two-dimensional plane with the first principal component and the second principal component calculated by principal component analysis as axes, and the calculated two-dimensional plane is Dimensional compression is performed by projecting a plane point group of matching candidates of a specific frame. Next, as shown in FIG. 16B, the calculation unit 244 adds the plane point group of the matching candidate for the next frame to the two-dimensional plane calculated for the plane point group of the matching candidate for the specific frame. Project. Next, as illustrated in FIG. 17, the calculation unit 244 obtains a Delaunay triangulation for the plane point group of the matching candidate of the dimension-compressed specific frame, and calculates a Convex hull based on the obtained Delaunay triangulation. create. Next, the calculation unit 244 calculates an inclusion rate in which the plane point group of the matching candidate of the next frame projected is included in the created Convex hull, and the calculated inclusion rate is a predetermined threshold (for example, , 60%) or more, it is determined that matching has been established. Thus, the process of step S303 ends.
図１４に戻り、次に、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群に基づいてフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する（ステップＳ３０４）。 Returning to FIG. 14, the calculation unit 244 then calculates a translation vector T and a rotation matrix R between the frames based on the plane point group of the specific frame for which matching is established and the plane point group of the next frame (step S304).
ステップＳ３０４では、まず、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群のそれぞれに対する平面の式を算出する。以下、前者を、特定のフレームのマッチング平面と称し、後者を、次のフレームのマッチング平面と称する。なお、算出部２４４が実行する平面の式の算出処理は、平面特定部２４３がステップＳ２０６において実行する平面の式の算出処理と同様である。次に、算出部２４４は、特定のフレームのマッチング平面の法線ベクトルＮ1と次のフレームのマッチング平面の法線ベクトルＮ2とにより次の式（４）に示す目的関数を最小二乗法を用いて最小化する行列Ｒ1を算出する。 In step S304, the calculation unit 244 first calculates a plane expression for each of the plane point group of the specific frame and the plane point group of the next frame for which matching has been established. Hereinafter, the former is referred to as a matching plane of a specific frame, and the latter is referred to as a matching plane of the next frame. The plane formula calculation process executed by the calculation unit 244 is the same as the plane formula calculation process executed by the plane specifying unit 243 in step S206. Next, the calculation unit 244 calculates the objective function shown in the following equation (4) by the least square method using the normal vector N 1 of the matching plane of the specific frame and the normal vector N 2 of the matching plane of the next frame. A matrix R 1 to be minimized is calculated.
次に、算出部２４４は、行列のｐｏｌａｒ ｄｅｃｏｍｐｏｓｉｔｉｏｎを利用して、次の式（５）及び（６）によってフレーム間の回転行列Ｒを算出する。 Next, the calculation unit 244 calculates a rotation matrix R between frames using the following equations (5) and (6), using the matrix polar decomposition.
次に、算出部２４４は、特定のフレームのマッチング平面の垂線の足からなるベクトルｄ1と次のフレームのマッチング平面の垂線の足からなるベクトルｄ2とから、次の式（７）によってフレーム間の並進ベクトルＴを算出する。以上により、ステップＳ３０４の処理が終了する。 Next, the calculation unit 244 calculates the frame from the vector d 1 consisting of the perpendicular foot of the matching plane of the specific frame and the vector d 2 consisting of the perpendicular foot of the matching plane of the next frame by the following equation (7). A translation vector T between them is calculated. Thus, the process of step S304 ends.
そして、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームでない場合（ステップＳ３０５−Ｎｏ）、ステップＳ３０１に処理を戻す。また、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームである場合（ステップＳ３０５−Ｙｅｓ）、一連のステップを終了する。 If the next frame executed in steps S301 to S304 is not the last frame (step S305-No), the calculation unit 244 returns the process to step S301. Moreover, the calculation part 244 complete | finishes a series of steps, when the next frame performed in step S301-S304 is the last frame (step S305-Yes).
図１８は、位置推定装置２の最適化部２４５による最適化処理の動作フローの一例を示す図である。図１８に示す最適化処理は、図５のステップＳ１０４において実行される。以下、図１９（ａ）及び（ｂ）を参照して、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群について説明する。 FIG. 18 is a diagram illustrating an example of an operation flow of optimization processing by the optimization unit 245 of the position estimation device 2. The optimization process shown in FIG. 18 is executed in step S104 of FIG. Hereinafter, with reference to FIGS. 19A and 19B, a plane point group of a specific frame and a plane point group of the next frame for which matching has been established will be described.
図１９（ａ）は、平面点群の真の平面及びマッチング平面を説明するための模式図である。図１９（ａ）に示すように、特定のフレームの平面点群の真の平面（実際の平面）と特定のフレームのマッチング平面との間に誤差が生じている場合がある。同様に、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との間に誤差が生じている場合がある。 FIG. 19A is a schematic diagram for explaining a true plane and a matching plane of a plane point group. As shown in FIG. 19A, there may be an error between the true plane (actual plane) of the plane point group of the specific frame and the matching plane of the specific frame. Similarly, there may be an error between the true plane (actual plane) of the plane point group of the next frame and the matching plane of the next frame.
図１９（ｂ）は、平面点群の最適化を説明するための模式図である。特定のフレームの平面点群の真の平面と特定のフレームのマッチング平面との間に誤差が生じている場合や、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との誤差が生じている場合、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させると、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じる。 FIG. 19B is a schematic diagram for explaining the optimization of the plane point group. If there is an error between the true plane of the plane point group of a specific frame and the matching plane of the specific frame, or the true plane (actual plane) of the plane point group of the next frame and the next frame When an error with the matching plane occurs, the plane vector of the specific frame for which the matching is established is moved using the translation vector T and the rotation matrix R between the frames. A blur occurs between the matching planes of the other frames.
最適化部２４５は、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じることが無いように、フレーム間の並進ベクトルＴ及び回転行列Ｒを最適化する。 The optimization unit 245 optimizes the translation vector T and the rotation matrix R between frames so that no blurring occurs between the matching plane of a specific frame and the matching plane of the next frame.
まず、最適化部２４５は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群、並びに、特定のフレーム及び次のフレームのフレーム間の並進ベクトルＴ及び回転行列Ｒを取得する（ステップＳ４０１）。 First, the optimization unit 245 obtains a plane point group of a specific frame and a plane point group of the next frame that are matched, and a translation vector T and a rotation matrix R between the frames of the specific frame and the next frame. (Step S401).
次に、最適化部２４５は、フレーム間の並進ベクトルＴ及び回転行列Ｒの最適化処理を行う（ステップＳ４０２）。ステップ４０２では、最適化部２４５は、まず、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させ、マッチングが成立した特定のフレームの平面点群とマッチングが成立した次のフレームの平面点群とを重ねる。次に、最適化部２４５は、マッチングが成立した全ての特定のフレームの平面点群及び次のフレームの平面点群の法線方向の分散が最小となるような目的関数を設定し、レーベンバーグ・マルカート法によって、並進ベクトルＴ及び回転行列Ｒの最適化を行う。なお、レーベンバーグ・マルカート法による最適化は、並進ベクトルＴ及び回転行列Ｒを交互に最適化し、並進ベクトルＴに対して最適化を行っている場合は、回転行列Ｒを固定し、回転行列Ｒに対して最適化を行っている場合は、並進ベクトルＴを固定する。以上により、ステップＳ４０２の処理が終了する。 Next, the optimization unit 245 performs an optimization process on the translation vector T and the rotation matrix R between the frames (step S402). In step 402, the optimization unit 245 first uses the translation vector T and the rotation matrix R between the frames to move the plane point group of the specific frame in which the matching is established, and the plane of the specific frame in which the matching is established. The point group and the plane point group of the next frame for which matching has been established are overlapped. Next, the optimization unit 245 sets an objective function that minimizes the variance in the normal direction of the plane point group of all the specific frames for which matching is established and the plane point group of the next frame. -The translation vector T and the rotation matrix R are optimized by the Marquardt method. In the optimization by the Levenberg-Marquardt method, the translation vector T and the rotation matrix R are alternately optimized. When the translation vector T is optimized, the rotation matrix R is fixed, and the rotation matrix R When the optimization is performed on the translation vector T, the translation vector T is fixed. Thus, the process of step S402 ends.
そして、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームでない場合（ステップＳ４０３−Ｎｏ）、ステップＳ４０１に処理を戻す。また、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームである場合（ステップＳ４０３−Ｙｅｓ）、一連のステップを終了する。 If the next frame executed in steps S401 to S402 is not the last frame (No in step S403), the optimization unit 245 returns the process to step S401. In addition, when the next frame executed in steps S401 to S402 is the last frame (step S403-Yes), the optimization unit 245 ends a series of steps.
図２０は、移動体の軌跡を説明するための模式図である。位置推定処理のステップＳ１０５で示したとおり、位置推定装置２の推定部２４６は、最適化部２４５によって最適化された並進ベクトルＴ及び回転行列Ｒに基づいて、移動体の軌跡を算出する。推定部２４６は、最初のフレームを取得した移動体の位置をワールド座標系の座標（Ｘ，Ｙ，Ｚ）＝（０，０，０）とし、フレーム間の並進ベクトルＴ及び回転行列Ｒを、フレームの順番に従って順次累積加算し、各フレームを取得した移動体の位置のワールド座標系の座標を算出する。 FIG. 20 is a schematic diagram for explaining the trajectory of the moving body. As shown in step S <b> 105 of the position estimation process, the estimation unit 246 of the position estimation device 2 calculates the trajectory of the moving body based on the translation vector T and the rotation matrix R optimized by the optimization unit 245. The estimation unit 246 sets the position of the moving body that acquired the first frame as coordinates (X, Y, Z) = (0, 0, 0) in the world coordinate system, and calculates the translation vector T and the rotation matrix R between the frames. The cumulative addition is performed sequentially according to the order of the frames, and the coordinates of the world coordinate system of the position of the moving body that acquired each frame are calculated.
以上説明してきたように、位置推定装置２は、移動や揺れのない建築物等の地物の壁面に位置する平面点群を、位置推定の対象として抽出するができるため、自己位置推定の精度を向上させることが可能となる。したがって、位置推定装置２は、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることが可能となる。 As described above, the position estimation device 2 can extract a plane point group located on the wall surface of a feature such as a building that does not move or shake as a position estimation target. Can be improved. Therefore, the position estimation apparatus 2 can improve the accuracy of self-position estimation based on information acquired by a sensor that measures the positions of surrounding features.
当業者は、本発明の精神及び範囲から外れることなく、様々な変更、置換、及び修正をこれに加えることが可能であることを理解されたい。 It should be understood by those skilled in the art that various changes, substitutions, and modifications can be made thereto without departing from the spirit and scope of the present invention.
１ 位置推定システム
２ 位置推定装置
２１ 記憶部
２２ 入力部
２３ 出力部
２４ 処理部
２４１ 取得部
２４２ 抽出部
２４３ 平面特定部
２４４ 算出部
２４５ 最適化部
２４６ 推定部
３ センサ
４ 移動体
DESCRIPTION OF SYMBOLS 1 Position estimation system 2 Position estimation apparatus 21 Storage part 22 Input part 23 Output part 24 Processing part 241 Acquisition part 242 Extraction part 243 Plane specific part 244 Calculation part 245 Optimization part 246 Estimation part 3 Sensor 4 Mobile body
Claims (9)
前記センサから出力された点群をフレーム毎に取得し、
前記取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
前記平面点群により形成される平面の位置をフレーム毎に特定し、
所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出し、
前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する
ことを含むことを特徴とする位置推定方法。 A computer-based position estimation method for measuring the position of a plurality of features at a predetermined measurement time and estimating the trajectory of a moving object equipped with a sensor that outputs a point cloud indicating the position of the measured feature sensor coordinate system. There,
The point cloud output from the sensor is acquired for each frame,
From the acquired point group, a point group forming a plane is extracted for each frame as a plane point group,
Specify the position of the plane formed by the plane point group for each frame,
A translation vector and a rotation matrix of the moving body between the frames are calculated from the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame;
A position estimation method comprising: sequentially calculating a translation vector and a rotation matrix of a moving body between the frames for each frame to estimate a locus of the moving body.
前記出力された点群に含まれる各点は、前記センサが搭載された位置を中心に前記レーザ照射部を回転させることにより順次測定された地物のセンサ座標系の位置を示し、
前記検出することは、フレーム毎に、
前記点群に含まれる各点のうちの所定の点と次の点との距離を算出し、前記算出した各距離に基づいて前記点群をセグメントに分類し、
前記セグメントに分類された点群に含まれる点の数が所定数以上であり且つ前記セグメントに分類された点群によって形成される形状が直線状であるセグメントを特定し、
複数の前記特定されたセグメントの中から、各前記特定されたセグメントの法線ベクトル及び各前記特定されたセグメントの位置に応じて平面を形成するセグメントを検出し、検出されたセグメントに分類された点群を前記平面点群として抽出することを含む、請求項１に記載の位置推定方法。 The sensor has a laser irradiation unit that measures the position of a feature by irradiating a laser that emits light in pulses,
Each point included in the output point group indicates the position of the sensor coordinate system of the feature sequentially measured by rotating the laser irradiation unit around the position where the sensor is mounted,
The detection is performed for each frame,
Calculating a distance between a predetermined point of each point included in the point cloud and the next point, classifying the point cloud into segments based on the calculated distances,
Identifying a segment in which the number of points included in the point group classified into the segment is a predetermined number or more and the shape formed by the point group classified into the segment is linear;
A segment forming a plane according to the normal vector of each identified segment and the position of each identified segment is detected from among the plurality of identified segments, and is classified into the detected segments. The position estimation method according to claim 1, comprising extracting a point group as the plane point group.
前記所定のフレームにおける前記特定された平面の位置と、前記所定のフレームにおける前記特定された平面の位置に最も近い前記次のフレームにおける前記特定された平面の位置とによって、前記移動体の並進ベクトル及び回転行列を算出することを含む、請求項１又は２に記載の位置推定方法。 Said calculating is
The translation vector of the moving body by the position of the specified plane in the predetermined frame and the position of the specified plane in the next frame closest to the position of the specified plane in the predetermined frame The position estimation method according to claim 1, further comprising calculating a rotation matrix.
前記所定のフレームにおける前記特定された平面の法線方向と前記次のフレームにおける前記特定された平面の法線方向とが所定の角度以内である場合、前記所定のフレームにおける前記特定された平面の位置と次のフレームにおける前記特定された平面の位置とによって、前記移動体の前記移動体の並進ベクトル及び回転行列を算出することを含む、請求項１〜３のいずれか一項に記載の位置推定方法。 Said calculating is
If the normal direction of the specified plane in the predetermined frame and the normal direction of the specified plane in the next frame are within a predetermined angle, the specified plane of the predetermined frame The position according to claim 1, comprising calculating a translation vector and a rotation matrix of the moving body of the moving body according to the position and the position of the specified plane in the next frame. Estimation method.
前記算出された前記並進ベクトル及び前記回転行列を、前記所定のフレームにおける前記特定された平面と前記次のフレームにおける前記特定された平面とによって最適化し、
前記最適化された並進ベクトル及び回転行列を、前記所定のフレームと次のフレームにおける前記フレーム間の移動体の並進ベクトル及び回転行列とすることを含む、請求項５に記載の位置推定方法。 Said calculating is
Optimizing the calculated translation vector and the rotation matrix by the identified plane in the predetermined frame and the identified plane in the next frame;
The position estimation method according to claim 5, wherein the optimized translation vector and rotation matrix are set as a translation vector and a rotation matrix of a moving body between the frames in the predetermined frame and the next frame.
前記所定のフレームにおける平面点群を、前記算出された並進ベクトル及び回転行列によって移動させ、
移動した所定のフレームにおける平面点群と次のフレームにおける平面点群とに対する、移動した平面点群により形成される平面の法線方向の分散が最小となるように並進ベクトル及び回転行列を最適化することを含む、請求項６に記載の位置推定方法。 The optimization includes
The plane point group in the predetermined frame is moved by the calculated translation vector and rotation matrix,
Optimize translation vector and rotation matrix so that the normal dispersion of the plane formed by the moved plane point group is minimized with respect to the plane point group in the given frame and the plane point group in the next frame The position estimation method according to claim 6, further comprising:
前記センサから出力された点群をフレーム毎に取得する取得部と、
前記取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出する抽出部と、
前記平面点群により形成される平面の位置をフレーム毎に特定する特定部と、
所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出する算出部と、
前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する推定部と
を備えることを特徴とする位置推定装置。 This is a position estimation device that estimates the trajectory of a mobile object equipped with a sensor that measures the position of a plurality of features at a predetermined measurement time and outputs a point cloud indicating the position of the measured feature in the sensor coordinate system. And
An acquisition unit for acquiring the point cloud output from the sensor for each frame;
An extraction unit that extracts a point group forming a plane as a plane point group for each frame from the acquired point group;
A specifying unit for specifying the position of a plane formed by the plane point group for each frame;
A calculation unit that calculates a translation vector and a rotation matrix of the moving body between frames from the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame;
A position estimation apparatus comprising: an estimation unit that sequentially calculates a translation vector and a rotation matrix of a moving body between the frames for each frame and estimates a locus of the moving body.
前記センサから出力された点群をフレーム毎に取得し、
前記取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
前記平面点群により形成される平面の位置をフレーム毎に特定し、
所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出し、
前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する
ことをコンピュータに実行させることを特徴とする位置推定プログラム。 This is a position estimation program that estimates the trajectory of a moving object equipped with a sensor that measures the position of multiple features at predetermined measurement times and outputs a point cloud that indicates the position of the measured feature in the sensor coordinate system. And
The point cloud output from the sensor is acquired for each frame,
From the acquired point group, a point group forming a plane is extracted for each frame as a plane point group,
Specify the position of the plane formed by the plane point group for each frame,
A translation vector and a rotation matrix of the moving body between the frames are calculated from the relationship between the position of the plane specified in the predetermined frame and the position of the plane specified in the next frame;
A position estimation program for causing a computer to sequentially calculate a translation vector and a rotation matrix of a moving body between frames for each frame and estimate a locus of the moving body.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015116095A JP6601613B2 (en) | 2015-06-08 | 2015-06-08 | POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND POSITION ESTIMATION PROGRAM |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015116095A JP6601613B2 (en) | 2015-06-08 | 2015-06-08 | POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND POSITION ESTIMATION PROGRAM |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2017003363A true JP2017003363A (en) | 2017-01-05 |
| JP6601613B2 JP6601613B2 (en) | 2019-11-06 |
Family
ID=57754087
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2015116095A Active JP6601613B2 (en) | 2015-06-08 | 2015-06-08 | POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND POSITION ESTIMATION PROGRAM |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6601613B2 (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017181401A (en) * | 2016-03-31 | 2017-10-05 | セコム株式会社 | Object detection sensor |
| CN109901202A (en) * | 2019-03-18 | 2019-06-18 | 成都希德瑞光科技有限公司 | A kind of airborne system position correcting method based on point cloud data |
| KR102063845B1 (en) * | 2019-06-21 | 2020-01-08 | 국방과학연구소 | Apparatus and method for measurement based on images |
| JP2020532734A (en) * | 2017-09-04 | 2020-11-12 | コモンウェルス サイエンティフィック アンド インダストリアル リサーチ オーガナイゼーション | Methods and systems to use when performing localization |
| CN113532472A (en) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | Method and system for detecting laser mapping odometer and integrated navigation positioning deviation |
| WO2023281810A1 (en) * | 2021-07-07 | 2023-01-12 | ソニーセミコンダクタソリューションズ株式会社 | Distance measurement device and distance measurement method |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007069726A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Self-position identifying method and device, and three-dimensional shape measurement method and device |
| JP2009193240A (en) * | 2008-02-13 | 2009-08-27 | Toyota Motor Corp | Mobile robot and method for generating environment map |
| US20140098094A1 (en) * | 2012-10-05 | 2014-04-10 | Ulrich Neumann | Three-dimensional point processing and model generation |
-
2015
- 2015-06-08 JP JP2015116095A patent/JP6601613B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007069726A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Self-position identifying method and device, and three-dimensional shape measurement method and device |
| JP2009193240A (en) * | 2008-02-13 | 2009-08-27 | Toyota Motor Corp | Mobile robot and method for generating environment map |
| US20140098094A1 (en) * | 2012-10-05 | 2014-04-10 | Ulrich Neumann | Three-dimensional point processing and model generation |
Non-Patent Citations (2)
| Title |
|---|
| 塩澤秀門 外３名: ""Velodyneレーザスキャナを用いた上空からの三次元再構成"", 日本ロボット学会誌, vol. Volume 31，Number 10, JPN6018048132, 15 December 2013 (2013-12-15), JP, pages 992 - 1000, ISSN: 0003934323 * |
| 木村孝広 外４名: ""移動ロボットを用いたユーザの姿勢情報分類及びマッピング"", ロボティクス・メカトロニクス講演会２０１３ 講演論文集, vol. Number 13-2, JPN6018048133, 21 May 2013 (2013-05-21), pages 1 - 1, ISSN: 0003934324 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017181401A (en) * | 2016-03-31 | 2017-10-05 | セコム株式会社 | Object detection sensor |
| JP2020532734A (en) * | 2017-09-04 | 2020-11-12 | コモンウェルス サイエンティフィック アンド インダストリアル リサーチ オーガナイゼーション | Methods and systems to use when performing localization |
| JP7278263B2 (en) | 2017-09-04 | 2023-05-19 | コモンウェルス サイエンティフィック アンド インダストリアル リサーチ オーガナイゼーション | Method and system for use in performing localization |
| CN109901202A (en) * | 2019-03-18 | 2019-06-18 | 成都希德瑞光科技有限公司 | A kind of airborne system position correcting method based on point cloud data |
| KR102063845B1 (en) * | 2019-06-21 | 2020-01-08 | 국방과학연구소 | Apparatus and method for measurement based on images |
| CN113532472A (en) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | Method and system for detecting laser mapping odometer and integrated navigation positioning deviation |
| WO2023281810A1 (en) * | 2021-07-07 | 2023-01-12 | ソニーセミコンダクタソリューションズ株式会社 | Distance measurement device and distance measurement method |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6601613B2 (en) | 2019-11-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6601613B2 (en) | POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND POSITION ESTIMATION PROGRAM | |
| TWI467494B (en) | Mobile camera localization using depth maps | |
| JP5950122B2 (en) | Calibration apparatus, calibration method, and calibration program | |
| US9242171B2 (en) | Real-time camera tracking using depth maps | |
| JP6525148B2 (en) | Trajectory estimation method, trajectory estimation apparatus and trajectory estimation program | |
| JP6658001B2 (en) | Position estimation device, program, position estimation method | |
| JP2002024807A (en) | Object movement tracking technique and recording medium | |
| JPWO2017051480A1 (en) | Image processing apparatus and image processing method | |
| JP2017117386A (en) | Self-motion estimation system, control method and program of self-motion estimation system | |
| WO2022217988A1 (en) | Sensor configuration scheme determination method and apparatus, computer device, storage medium, and program | |
| JP2018092636A (en) | Multi-component corresponder for multiple cameras | |
| JP2017096813A (en) | Calibration device, calibration method, and calibration program | |
| CN116091431A (en) | Case Liang Binghai detection method, apparatus, computer device, and storage medium | |
| WO2018148219A1 (en) | Systems and methods for user input device tracking in a spatial operating environment | |
| CN112381873A (en) | Data labeling method and device | |
| JP2009216480A (en) | Three-dimensional position and attitude measuring method and system | |
| JP7024405B2 (en) | Information processing equipment, programs and information processing methods | |
| JP2012225752A (en) | Mobile body tracking apparatus and mobile body tracking method | |
| CN115082520A (en) | Positioning tracking method and device, terminal equipment and computer readable storage medium | |
| WO2018134866A1 (en) | Camera calibration device | |
| JP2016136099A (en) | Positioning system and positioning method | |
| US20240119620A1 (en) | Posture estimation apparatus, posture estimation method, and computer-readable recording medium | |
| CN117649619B (en) | Unmanned aerial vehicle visual navigation positioning recovery method, system, device and readable storage medium | |
| JP2018044769A (en) | Self position estimation device, mobile entity, and self position estimation method | |
| JP7152103B2 (en) | Estimation device, estimation method, and estimation program |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180215 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A821 Effective date: 20180215 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20181130 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20181211 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190207 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20190618 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190807 |
|
| 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: 20190827 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20190924 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6601613 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| S531 | Written request for registration of change of domicile |
Free format text: JAPANESE INTERMEDIATE CODE: R313531 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |