JP2016045330A - Method and device for aligning three-dimensional point group data and mobile body system thereof - Google Patents
Method and device for aligning three-dimensional point group data and mobile body system thereof Download PDFInfo
- Publication number
- JP2016045330A JP2016045330A JP2014168996A JP2014168996A JP2016045330A JP 2016045330 A JP2016045330 A JP 2016045330A JP 2014168996 A JP2014168996 A JP 2014168996A JP 2014168996 A JP2014168996 A JP 2014168996A JP 2016045330 A JP2016045330 A JP 2016045330A
- Authority
- JP
- Japan
- Prior art keywords
- data
- current
- point cloud
- dimensional point
- immediately preceding
- 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
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000012545 processing Methods 0.000 claims description 21
- 238000012937 correction Methods 0.000 claims description 20
- 230000009466 transformation Effects 0.000 claims description 12
- 239000002245 particle Substances 0.000 claims description 8
- 238000013075 data extraction Methods 0.000 claims description 7
- 238000004891 communication Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 20
- 238000004364 calculation method Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 11
- 238000000605 extraction Methods 0.000 description 6
- 238000006243 chemical reaction Methods 0.000 description 5
- 238000007781 pre-processing Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Instructional Devices (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Processing Or Creating Images (AREA)
Abstract
Description
本発明は、移動体により取得した３次元点群データの位置合わせ方法と装置及びその移動体システムに関する。 The present invention relates to a method and apparatus for aligning three-dimensional point cloud data acquired by a moving body, and a moving body system thereof.
自律走行又は半自律走行を行う移動体にレーザ装置（例えば、レーザレンジファインダ：ＬＲＦ）を搭載し、前方の路面や障害物を検出して、周辺の地形を把握し、経路計画や速度制御などの車両制御を行うための地図情報を構築する手段が既に開示されている（例えば、特許文献１）。 A laser device (for example, laser range finder: LRF) is mounted on a moving body that performs autonomous or semi-autonomous traveling, detects the road surface and obstacles ahead, grasps the surrounding terrain, route planning, speed control, etc. A means for constructing map information for performing vehicle control has already been disclosed (for example, Patent Document 1).
上記地図情報として、構造格子あるいは非構造格子の地図上に、各点の高さ情報を与えたモデルをＤＥＭ（Ｄｉｇｉｔａｌ Ｅｌｅｖａｔｉｏｎ Ｍｏｄｅｌ：数値標高モデル）と呼ぶ。
レーザレンジファインダ（ＬＲＦ）の計測密度は、計測位置から離れるほど疎になり、例えば移動体から４０〜５０ｍ離れた位置では、計測間隔は２〜３ｍ以上となる。そのため、数値標高モデル（ＤＥＭ）のデータ密度を高めるためには移動体が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。同様に、広域の数値標高モデルを生成する場合にも、移動体が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。
複数の点群データを位置合わせする点群位置合わせ手段は、例えば特許文献２〜４に開示されている。
As the map information, a model that gives height information of each point on a structured or unstructured map is called a DEM (Digital Elevation Model).
The measurement density of the laser range finder (LRF) becomes sparser as it moves away from the measurement position. For example, at a position 40 to 50 m away from the moving body, the measurement interval is 2 to 3 m or more. Therefore, in order to increase the data density of the digital elevation model (DEM), it is necessary to repeat the terrain measurement while moving the moving body and integrate a plurality of data. Similarly, when generating a digital elevation model in a wide area, it is necessary to repeat terrain measurement while moving the moving body and integrate a plurality of data.
For example,
一方、移動体の位置と姿勢の情報（位置姿勢情報）は、加速度センサ、重力センサ、地磁気センサ、ジャイロ、ＧＰＳなど各種センサ類の計測値を統合することで推定できる。かかる推定方法は、一般にデッドレコニングと呼ばれる。 On the other hand, the position and orientation information (position and orientation information) of the moving body can be estimated by integrating the measurement values of various sensors such as an acceleration sensor, a gravity sensor, a geomagnetic sensor, a gyroscope, and a GPS. Such an estimation method is generally called dead reckoning.
上記デッドレコニングによっても、移動体の位置情報又は姿勢情報には誤差が発生する。この誤差により、レーザレンジファインダで取得した複数の点群データを位置合わせする際に、位置ずれが生じる。この位置ずれに起因し、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）が誤差に埋もれて早期に検出できず、直前になって検出されるため、高速走行（例えば時速３０〜１００ｋｍ）が困難となる問題が発生し得る。 Even with the dead reckoning, an error occurs in the position information or posture information of the moving body. Due to this error, misalignment occurs when aligning a plurality of point cloud data acquired by the laser range finder. Due to this misalignment, a small obstacle (for example, 2 to 30 cm in height) away from the moving body is buried in the error and cannot be detected at an early stage, but is detected immediately before. 30-100 km) may be difficult.
また、レーザレンジファインダによる３次元点群データは、毎秒約２０フレーム（例えば５〜５０Ｈｚ）の頻度で計測される。従って前回と今回のフレーム（前フレームと現フレーム）の点群位置合わせは、１／２０秒以内（１／５０〜１／５秒以内）に完了する必要がある。
しかし、特許文献２〜４の点群位置合わせ手段では、複数の点群データから位置合わせの基準となるデータとして、地面、特徴部分、建物のコーナーなどを抽出する前処理が不可欠である。この前処理によって位置合わせに用いる点数を削減できるが、前処理に要する計算コストが大きい、あるいは点数の削減効果が薄いなどの理由により、高速処理が困難又は不可能であった。
Further, the three-dimensional point cloud data by the laser range finder is measured at a frequency of about 20 frames (for example, 5 to 50 Hz) per second. Therefore, the point cloud positioning between the previous and current frames (previous frame and current frame) needs to be completed within 1/20 second (within 1/50 to 1/5 second).
However, in the point group alignment means of
本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、移動体の位置情報又は姿勢情報に誤差が発生しても、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）を検出でき、かつ短時間（例えば１／２０秒以内）に点群位置合わせを完了できる３次元点群データの位置合わせ方法と装置及びその移動体システムを提供することにある。これにより、移動体の位置情報又は姿勢情報を修正でき、かつ移動体の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。 The present invention has been developed to solve the above-described problems. That is, an object of the present invention is to detect a small obstacle (for example, a height of 2 to 30 cm) at a position far from the moving body even if an error occurs in the position information or posture information of the moving body, and for a short time (for example, An object of the present invention is to provide a method and apparatus for aligning three-dimensional point cloud data and a moving body system thereof that can complete point cloud alignment within 1/20 second). Thereby, the position information or posture information of the moving body can be corrected, and the moving body can run at high speed (for example, 30 to 100 km / h).
本発明によれば、移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ方法であって、
（Ａ）直前の３次元点群データのうち、直前の移動体の進行方向である第１方向に位置する鉛直平面上の直前データと、
現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データと、を抽出するデータ抽出ステップと、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置修正ステップと、
（Ｃ）修正した現在の３次元点群データを直前までの３次元点群データに加算して、移動体周辺の高さ情報を含む地図データを更新する地図更新ステップと、を有する、ことを特徴とする３次元点群データの位置合わせ方法が提供される。
According to the present invention, there is provided a method for aligning three-dimensional point cloud data acquired by a laser range finder mounted on a moving body,
(A) Of the immediately preceding three-dimensional point cloud data, immediately preceding data on a vertical plane located in the first direction that is the traveling direction of the immediately preceding moving body;
A data extraction step of extracting current data on a vertical plane located in the first direction from the current three-dimensional point cloud data;
(B) a position correcting step for correcting the position of the current three-dimensional point cloud data so that the difference between the immediately preceding data and the current data is minimized;
(C) adding the corrected current three-dimensional point cloud data to the previous three-dimensional point cloud data to update the map data including the height information around the moving body, and a map updating step. A method for aligning characteristic 3D point cloud data is provided.
前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置合わせ量を算出する。 In the position correction step, an alignment amount for correcting the position of the current three-dimensional point cloud data is calculated so that the difference between the immediately preceding data and the current data is minimized.
前記位置修正ステップにおいて、前記第１方向に位置する鉛直平面上に、鉛直平面近傍の直前又は現在の３次元点群データの一部を投影する。 In the position correcting step, a part of the three-dimensional point cloud data immediately before or near the vertical plane is projected onto the vertical plane located in the first direction.
前記位置修正ステップにおいて、鉛直平面上の直前データ又は現在データから前記鉛直平面上の地表面に相当する形状パラメータを求める。 In the position correction step, a shape parameter corresponding to the ground surface on the vertical plane is obtained from immediately preceding data or current data on the vertical plane.
前記位置修正ステップにおいて、現在の３次元点群データの幾何変換を行う。 In the position correction step, the current three-dimensional point cloud data is geometrically converted.
前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、移動体の現在の位置又は姿勢を修正する。 In the position correcting step, the current position or posture of the moving body is corrected so that the difference between the immediately preceding data and the current data is minimized.
前記位置修正ステップにおいて、粒子フィルタを用いる。 In the position correction step, a particle filter is used.
前記位置修正ステップにおいて、状態推定方法を適用する。 In the position correction step, a state estimation method is applied.
前記位置修正ステップにおいて、路面推定処理を含む。 The position correction step includes road surface estimation processing.
また、本発明によれば、移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ装置であって、
移動体周辺の高さ情報を含む地図データを記憶する地図メモリと、
直前の３次元点群データのうち、直前の移動体の進行方向である第１方向と、該第１方向に位置する鉛直平面上の直前データとを記憶する直前メモリと、
現在の３次元点群データを記憶する現在メモリと、
データ処理装置と、を備え、該データ処理装置は、
（Ａ）現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データを抽出し、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正し、
（Ｃ）修正した現在の３次元点群データを直前までの地図メモリに加算する、ことを特徴とする３次元点群データの位置合わせ装置が提供される。
Further, according to the present invention, there is provided an alignment apparatus for three-dimensional point cloud data acquired by a laser range finder mounted on a moving body,
A map memory for storing map data including height information around the moving body;
Of the immediately preceding three-dimensional point cloud data, the immediately preceding memory for storing the first direction, which is the traveling direction of the immediately preceding moving body, and the immediately preceding data on the vertical plane located in the first direction;
A current memory for storing current 3D point cloud data;
A data processing device, the data processing device comprising:
(A) Extracting current data on a vertical plane located in the first direction from the current three-dimensional point cloud data,
(B) Correct the position of the current three-dimensional point cloud data so that the difference in distance between the immediately preceding data and the current data is minimized.
(C) A three-dimensional point cloud data alignment device is provided, wherein the corrected current three-dimensional point cloud data is added to the map memory until immediately before.
前記データ処理装置は、前記（Ｃ）の後、
前記直前メモリの前記第１方向を、現在の移動体の進行方向に置換し、
前記直前メモリの直前データを、現在の移動体の進行方向に位置する鉛直平面上のデータに置換する。
The data processing apparatus, after the (C),
Replacing the first direction of the immediately preceding memory with the current traveling direction of the moving body;
The immediately preceding data in the immediately preceding memory is replaced with data on a vertical plane located in the current traveling direction of the moving body.
さらに、本発明によれば、前記移動体は、上述した３次元点群データの位置合わせ装置を搭載する、ことを特徴とする移動体システムが提供される。 Furthermore, according to the present invention, there is provided a mobile system characterized in that the mobile body is equipped with the above-described 3D point cloud data alignment device.
さらに、本発明によれば、前記移動体とは別の場所に設置され前記移動体と通信する通信装置を備え、前記通信装置は上述した３次元点群データの位置合わせ装置の一部を搭載する、ことを特徴とする移動体システムが提供される。 Furthermore, according to the present invention, the communication device is installed at a location different from the mobile body and communicates with the mobile body, and the communication device is equipped with a part of the above-described 3D point cloud data alignment device. A mobile system is provided.
上記本発明の方法と装置及びその移動体システムによれば、データ抽出ステップにおいて、直前データと現在データを抽出する。直前データは、直前の移動体の進行方向である第１方向に位置する鉛直平面上のデータであり、直前の３次元点群データの一部である。また、現在データは、前記第１方向に位置する鉛直平面上のデータであり、現在の３次元点群データの一部である。
直前データと現在データは取得時がわずか（例えば１／２０秒以内）に異なるが、同一の第１方向（直前の移動体の進行方向）のデータなので、実質的にほとんど一致する。しかし、移動体の位置又は姿勢に誤差がある場合、その誤差により、直前データと現在データとの間に距離の差が生じる。
According to the method and apparatus of the present invention and the mobile system thereof, the previous data and the current data are extracted in the data extraction step. The immediately preceding data is data on the vertical plane located in the first direction which is the traveling direction of the immediately preceding moving body, and is a part of the immediately preceding three-dimensional point group data. The current data is data on the vertical plane located in the first direction, and is a part of the current three-dimensional point group data.
The immediately preceding data and the current data are slightly different at the time of acquisition (for example, within 1/20 second), but are substantially the same because they are data in the same first direction (the moving direction of the immediately preceding moving body). However, if there is an error in the position or orientation of the moving body, a difference in distance occurs between the immediately preceding data and the current data due to the error.
本発明では、位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する。この修正により、移動体の位置又は姿勢の誤差の影響を排除することができる。
従って、本発明によれば、移動体の位置情報又は姿勢情報に誤差が発生しても、誤差の影響を排除した地図データを作成することができ、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）を検出できる。
In the present invention, in the position correction step, the current position of the three-dimensional point cloud data is corrected so that the difference between the immediately preceding data and the current data is minimized. By this correction, it is possible to eliminate the influence of the position or posture error of the moving body.
Therefore, according to the present invention, even if an error occurs in the position information or posture information of the moving object, map data that eliminates the influence of the error can be created, and an obstacle (such as a small obstacle away from the moving object) For example, a height of 2 to 30 cm) can be detected.
また、直前データと現在データは、レーザレンジファインダで取得した３次元点群データのうち、直前の移動体の進行方向である第１方向のデータであり、それぞれ抽出が容易であり、かつその点数は少ない（例えば、１０〜１００点）。
従って、追加の処理なしに短時間（例えば１／２０秒以内）に点群位置合わせを完了でき、これにより、移動体の位置情報又は姿勢情報を修正でき、かつ移動体の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。
The immediately preceding data and the current data are data in the first direction, which is the traveling direction of the immediately preceding moving body, among the three-dimensional point cloud data acquired by the laser range finder. Is small (for example, 10 to 100 points).
Therefore, the point cloud positioning can be completed in a short time (for example, within 1/20 second) without additional processing, whereby the position information or posture information of the moving body can be corrected, and the mobile body can be moved at high speed (for example, speed per hour). 30-100 km) is possible.
言い換えれば、本発明によれば、少ない計算コストで、デッドレコニングによる自己位置姿勢を補正して数値標高モデルの精度を向上することにより、進行方向に存在する背の低い（回避すべき）障害物を発見しやすくなる。 In other words, according to the present invention, a low obstacle (to be avoided) existing in the traveling direction by correcting the self-position and posture by dead reckoning and improving the accuracy of the digital elevation model with low calculation cost. Makes it easier to discover.
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。 Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings. In addition, the same code | symbol is attached | subjected to the common part in each figure, and the overlapping description is abbreviate | omitted.
図１は、本発明の位置合わせ装置１０を備えた移動体１の構成図である。この図において、（Ａ）は側面図、（Ｂ）は平面図である。また図中の○印は、レーザレンジファインダ２による計測点を示している。
FIG. 1 is a configuration diagram of a moving
移動体１は車両であり、レーザレンジファインダ２を備える。
レーザレンジファインダ２（ＬＲＦ）は、移動体１の前方あるいは上方に設置されている。レーザレンジファインダ２の設置高さは地面から３０ｃｍ以上、例えば１．５〜４ｍの範囲である。
レーザレンジファインダ２は、レーザ光３の入射光の投光から反射光の受光までの時間差から距離を検出する。
レーザレンジファインダ２は、この例では、上下に等ピッチ（例えば０．１〜２度）のチルト角で配列されたレーザ光３を水平旋回軸Ｚを中心に水平に（水平面内で）旋回させ、１回転で移動体１の周囲の３次元点群データ４（図２参照）を取得する。回転方向の計測ピッチは例えば０．１〜２度である。
水平旋回軸Ｚの向きは移動体１の姿勢に追随する（移動体１に対して固定されている）。すなわち、移動体１の姿勢が水平であるとき、水平旋回軸Ｚの向きは鉛直になる。換言すれば、水平旋回軸Ｚは、移動体１が水平に位置するときの鉛直軸である。
The moving
The laser range finder 2 (LRF) is installed in front of or above the moving
The
In this example, the
The direction of the horizontal turning axis Z follows the posture of the moving body 1 (fixed with respect to the moving body 1). That is, when the posture of the moving
上述したレーザレンジファインダ２は、１秒間に約２０回（５〜５０回）旋回し、毎秒約２０フレーム（例えば５〜５０フレーム）の３次元点群データ４を取得する。
各３次元点群データ４は、例えばレーザ光３が５０であり、回転方向の計測ピッチが１度の場合、５０×３６０＝１８０００点の点群データを含む。
従って、本発明の位置合わせ装置１０は、この例では、２万点程度（１８０００点）の点群データを扱い、１／２０秒以内（１／５０〜１／５秒以内）に位置合わせを完了する必要がある。
なお、レーザレンジファインダ２は、上述したものに限定されず、回転することで点群データを取得するレーザレンジファインダであればよい。また本発明はレーザレンジファインダに限定されず、レーダ、あるいは距離画像センサを用いてもよい。
The
Each 3D
Therefore, in this example, the
The
図１において、説明の都合上、ｎ点（例えば５０点）のレーザ光３を移動体１に近いほうから、１，２，３，・・・，ｋ，・・・ｎ−１，ｎと番号を付ける。番号ｎのレーザ光３は、移動体１から例えば３０〜１００ｍの走行路面、番号１のレーザ光３は、移動体１から１〜１０ｍの走行路面を照射するように設定されている。
ｎ＝５０かつチルト方向の角度ピッチが１度の場合、番号１，２の水平間隔は例えば数ｃｍ程度であるが、番号ｎ−１，ｎの水平間隔は数ｍにも及ぶため、遠方の小さな障害物を早期に発見することは困難である。
そのため、上述したように、数値標高モデル（ＤＥＭ）のデータ密度を高めるためには移動体１が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。
In FIG. 1, for convenience of explanation, n points (for example, 50 points) of
When n = 50 and the angle pitch in the tilt direction is 1 degree, the horizontal interval of
Therefore, as described above, in order to increase the data density of the digital elevation model (DEM), it is necessary to repeat terrain measurement while moving the moving
図２は、本発明の位置合わせ装置１０の全体構成図である。
この図において、本発明の位置合わせ装置１０は、地図メモリ１２、直前メモリ１４、現在メモリ１６、及びデータ処理装置１８を備える。
FIG. 2 is an overall configuration diagram of the
In this figure, the
地図メモリ１２は、移動体１の周辺の高さ情報を含む地図データ５を記憶する。地図データ５は、上述した数値標高モデル（ＤＥＭ）に相当し、後述する地図更新ステップＳ４により、移動体周辺の高さ情報が繰り返し蓄積され、データ密度を高めるようになっている。なお、例えば不要になったときに、地図データ５の一部又は全部を消去することもできる。
The map memory 12 stores map
直前メモリ１４は、直前の３次元点群データ４Ａのうち、直前の移動体１の進行方向である第１方向Ｆ（図５参照）と、第１方向Ｆに位置する鉛直平面上の直前データ６とを記憶する。「第１方向Ｆに位置する鉛直平面」とは、第１方向Ｆと水平旋回軸Ｚの張る平面を意味する。
直前データ６は、レーザレンジファインダ２により計測された進行方向である第１方向Ｆに位置する鉛直平面の直前のデータであり、上述の例では５０点のデータ数を有する。以下、この鉛直平面を「直前基準平面Ｐ１」と呼ぶ。
直前メモリ１４は、直前の３次元点群データ４Ａのすべては記憶せず、少なくとも第１方向Ｆと、第１方向Ｆに位置する鉛直平面の直前のデータとを記憶すればよい。従って、直前メモリ１４の記憶容量を小さく設定することができる。
The immediately preceding memory 14 includes, in the immediately preceding three-dimensional
The immediately preceding
The immediately preceding memory 14 does not store all the immediately preceding three-dimensional
現在メモリ１６は、現在の３次元点群データ４Ｂを記憶する。現在メモリ１６は、現在の３次元点群データ４Ｂのみを記憶する。また、後述する地図更新ステップＳ４の後に、現在の３次元点群データ４Ｂの一部のみを直前メモリ１４に置換した後、現在の３次元点群データ４Ｂは消去される。従って、現在メモリ１６は、１フレームの３次元点群データ４Ｂのみを記憶できればよく、その記憶容量を小さく設定することができる。
The current memory 16 stores the current three-dimensional
データ処理装置１８は、例えばコンピュータ（ＰＣ）であり、以下の演算を実施する。
（Ａ）初めに、データ処理装置１８は、現在メモリ１６から、現在の３次元点群データ４Ｂのうち、上述した第１方向Ｆ（図５参照）に位置する鉛直平面上の現在データ７を抽出する。現在データ７は、直前データ６と同一の第１方向Ｆ（方位）に位置する鉛直平面上の現在のデータであり、上述の例では５０点のデータ数を有する。以下、この鉛直平面を「現在基準平面Ｐ２」と呼ぶ。
なお、第１方向Ｆは、「直前の移動体１の進行方向」であり、「現在の移動体１の進行方向」ではない。現在の移動体１の進行方向は、第１方向Ｆと一致しても相違してもよい。
また、レーザレンジファインダのレーザ配向、あるいは計測誤差によって、第１方向Ｆ（方位）に位置する鉛直平面、すなわち現在基準平面Ｐ２上にデータが整列しない場合が有り得る。そのため、現在基準平面Ｐ２の近傍に位置し、同時に計測したとみなせるｎ点（例えば５０点）のデータを現在基準平面Ｐ２上に投影して現在データ７として用いることが好ましい。この近傍は、第１方向に対して３０度以内の範囲であることが好ましい。
The data processing device 18 is a computer (PC), for example, and performs the following calculations.
(A) First, the data processing device 18 obtains the
The first direction F is the “advance direction of the immediately preceding moving
Further, there may be a case where data is not aligned on the vertical plane located in the first direction F (orientation), that is, the current reference plane P2, due to the laser orientation of the laser range finder or measurement error. For this reason, it is preferable to project data on n points (for example, 50 points) that are located in the vicinity of the current reference plane P2 and can be regarded as simultaneously measured on the current reference plane P2 and use them as the
直前基準平面Ｐ１と現在基準平面Ｐ２は、「直前の移動体１の進行方向である第１方向Ｆに位置する鉛直平面」である点で共通する。すなわち、両平面とも、直前の移動体１の第１方向Ｆ（方位）を基準としている。そのため上述した水平旋回軸Ｚが互いに平行である限り、移動体１の位置と向きが異なっても、直前基準平面Ｐ１と現在基準平面Ｐ２は互いに平行である。
従って、直前データ６と現在データ７は取得時がわずか（例えば１／２０秒程度）に異なるが、同一の第１方向Ｆ（方位）の３次元点群データ４Ａ、４Ｂの一部なので、実質的にはほとんど一致する。しかし、移動体１の位置又は姿勢に誤差がある場合、その誤差により、直前データ６と現在データ７との間に距離の差が生じる。
The immediately preceding reference plane P1 and the current reference plane P2 are common in that they are “vertical planes located in the first direction F that is the traveling direction of the immediately preceding moving
Therefore, although the immediately preceding
（Ｂ）次に、データ処理装置１８は、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。
（Ｃ）次いで、データ処理装置１８は、修正した現在の３次元点群データ４Ｃを直前までの地図メモリ１２に加算する。ここで「加算」とは、情報を付け加える意味である。
(B) Next, the data processing device 18 corrects the position of the current three-dimensional
(C) Next, the data processing device 18 adds the corrected current three-dimensional point cloud data 4C to the map memory 12 until just before. Here, “addition” means adding information.
またデータ処理装置１８は、（Ｃ）の後、直前データ６の第１方向Ｆを現在の移動体１の進行方向に置換する。さらに、直前メモリ１４の直前データ６を現在の移動体１の進行方向に位置する鉛直平面上のデータに置換する。これらの進行方向とデータは、現在の３次元点群データ４Ｂの一部であり、追加の処理なしに置換することができる。
この場合、現在の移動体１の進行方向（方位）は、直前の移動体１の進行方向（方位）とは通常相違する。
Further, after (C), the data processing device 18 replaces the first direction F of the immediately preceding
In this case, the current traveling direction (orientation) of the moving
図３は、本発明による位置合わせ方法の全体フロー図である。この図において、本発明の位置合わせ方法は、Ｓ１〜Ｓ４の各ステップ（工程）からなる。
また、図４〜図９は、本発明による位置合わせ方法の説明図である。
以下、図３〜図９を用いて、本発明の方法を説明する。
FIG. 3 is an overall flowchart of the alignment method according to the present invention. In this figure, the alignment method of the present invention comprises steps (steps) S1 to S4.
4 to 9 are explanatory diagrams of the alignment method according to the present invention.
Hereinafter, the method of the present invention will be described with reference to FIGS.
ステップＳ１は、周囲環境計測ステップであり、移動体１の現在の位置姿勢情報と、現在の３次元点群データ４Ｂを取得する。
Step S1 is an ambient environment measurement step, in which the current position and orientation information of the moving
移動体１の現在の位置姿勢情報は、移動体１に搭載された加速度センサ、重力センサ、地磁気センサ、ジャイロ、ＧＰＳなど各種センサ類の計測値を統合して推定する。従って、移動体１の進行方向（図１参照）は、位置姿勢情報として常に得られている。しかしこの位置姿勢情報（移動体１の位置又は姿勢の情報）には誤差が含まれる。
The current position and orientation information of the moving
図４は、ステップＳ１（周囲環境計測ステップ）の説明図であり、（Ａ）は側面図、（Ｂ）は平面図である。また、図中において、Ｇは、移動体１の進行方向真正面に位置する障害物である。
FIG. 4 is an explanatory diagram of step S1 (ambient environment measurement step), where (A) is a side view and (B) is a plan view. In the figure, G is an obstacle located in front of the moving
現在の３次元点群データ４Ｂは、例えば上述した２万点程度（１８０００点）の点群データである。３次元点群データ４の各点データは、位置データと、移動体１の進行方向真正面に対する水平旋回軸Ｚまわりの角度データと、を含んでいる。
The current three-dimensional
ステップＳ２は、データ抽出ステップであり、直前データ６と現在データ７を抽出する。図３において、ステップＳ２は、直前データ６の抽出ステップＳ２１と、現在データ７の抽出ステップＳ２２とからなる。抽出ステップＳ２１、Ｓ２２は順序が逆であってもよい。
Step S2 is a data extraction step, in which the immediately preceding
図５は、ステップＳ２（データ抽出ステップ）の説明図である。
図５（Ａ）は、図４（Ｂ）と同様の平面図である。この図において、移動体１は、実線は現在、破線は直前のものを示している。また、図５（Ｂ）は、移動体１の姿勢変化を（１）（２）（３）の順で示している。
FIG. 5 is an explanatory diagram of step S2 (data extraction step).
FIG. 5A is a plan view similar to FIG. In this figure, the moving
直前データ６は、直前の３次元点群データ４Ａのうち、直前メモリ１４に記憶された直前の移動体１の進行方向である第１方向Ｆと、この第１方向Ｆに位置する直前基準平面Ｐ１上の５０点のデータである。直前データ６は、直前メモリ１４から追加の処理なしに抽出することができる。この直前データ６は、図５（Ａ）の直前基準平面Ｐ１（破線の矢印）上のデータに相当する。
なお図５（Ａ）には、直前データ６は示されていない。
The immediately preceding
In FIG. 5A, the immediately preceding
現在データ７は、現在の３次元点群データ４Ｂのうち、直前データ６と同一の第１方向Ｆに位置する５０点のデータである。このデータは、ステップＳ１で取得した３次元点群データ４の角度データと、移動体１の現在の位置姿勢情報の進行方向から、抽出することができる。この現在データ７は、図５（Ａ）の現在基準平面Ｐ２（実線の矢印）上のデータ（●印）に相当する。
The
図５（Ｂ）において、（１）が直前の姿勢（２）が現在の姿勢の場合、（２）の実線の矢印は現在の進行方向、破線の矢印は直前の進行方向である。
同様に、（２）が直前の姿勢（３）が現在の姿勢の場合、（３）の実線の矢印は現在の進行方向、破線の矢印は直前の進行方向である。
In FIG. 5B, when (1) is the previous posture (2) is the current posture, the solid line arrow in (2) is the current traveling direction, and the dashed arrow is the previous traveling direction.
Similarly, when (2) is the previous posture (3) is the current posture, the solid arrow in (3) is the current traveling direction, and the dashed arrow is the previous traveling direction.
図３において、ステップＳ３は位置修正ステップであり、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。
この際、直前データ６と現在データ７の距離の差が最小となる位置合わせ量に従い、現在の３次元点群データ４Ｂに幾何変換を実施する。なお、本発明における幾何変換とは、点群全体に対して均一な回転ないし並進移動を行う、いわゆる剛体変換を意図している。
In FIG. 3, step S3 is a position correction step, in which the position of the current three-dimensional
At this time, geometric conversion is performed on the current three-dimensional
図３において、ステップＳ３（位置修正ステップ）は、Ｓ３１〜Ｓ３６の各ステップ（工程）からなる。 In FIG. 3, step S3 (position correction step) includes steps S31 to S36.
ステップＳ３１は、平面投影ステップである。図６は、平面投影ステップの説明図である。
ステップＳ３１において、図６に示すように、第１方向Ｆに位置する鉛直平面である直前基準平面Ｐ１上に、この鉛直平面近傍の直前又は現在の３次元点群データ４Ａ、４Ｂの一部を投影する。
直前データ６と現在データ７は、レーザレンジファインダ２による前回と今回のフレーム（前フレームと現フレーム）のデータであるため、例えば１／２０秒以内（１／５０〜１／５秒以内）の時間差がある。そのため、レーザレンジファインダのレーザ配向、わずかな計測時間のずれ、あるいは計測誤差により、直前データ６と現在データ７のデータが同一方向に整列していない場合がある。この場合、直前データ６と現在データ７を直前基準平面Ｐ１上に投影して同一平面上で扱うことが好ましい。
Step S31 is a plane projection step. FIG. 6 is an explanatory diagram of the planar projection step.
In step S31, as shown in FIG. 6, on the immediately preceding reference plane P1, which is a vertical plane located in the first direction F, a part of the immediately preceding or current three-dimensional
Since immediately preceding
なおステップＳ３において、点群の位置合わせを行う際、位置合わせ不良の原因となる点群を事前に除去しておくことで、位置合わせ計算を安定化できる。 In step S3, when performing point group alignment, alignment calculation can be stabilized by previously removing point groups that cause alignment failure.
図３において、ステップＳ３２は、直前データ６の法線推定ステップである。図７は、法線推定ステップの説明図である。
各点における法線は、例えば前後の点との３点による接続角度をもとに推定することができる。各点における法線を推定することは、言い換えれば地表面の形状を各点近傍において平面近似することに他ならない。
また法線推定に限らず、各点を結ぶ折れ線、あるいは折れ線を近似する曲線を用いて、地表面形状を推定してもかまわない。
なお、地表面形状の推定は、現在データ７に適用してもよい。
すなわちこのステップでは、鉛直平面上の直前データ又は現在データから鉛直平面上の地表面に相当する形状パラメータを求める。「形状パラメータ」には、曲線、折れ線、法線などが含まれる。
In FIG. 3, step S <b> 32 is a normal estimation step for the immediately preceding
The normal line at each point can be estimated based on, for example, the connection angle of three points with the front and rear points. In other words, estimating the normal line at each point is nothing but a plane approximation of the shape of the ground surface in the vicinity of each point.
In addition, the ground surface shape may be estimated using not only the normal line estimation but also a broken line connecting points or a curve approximating the broken line.
The ground surface shape estimation may be applied to the
That is, in this step, a shape parameter corresponding to the ground surface on the vertical plane is obtained from immediately preceding data on the vertical plane or current data. The “shape parameter” includes a curve, a broken line, a normal line, and the like.
図３において、ステップＳ３３は、近傍点探索ステップである。直前データ６の各点について、現在データ７の各点のうちもっとも距離が近いと推定される点を探索し、対応付けを行う。上述した推定した地表面形状に対し、現在データ７の各点を投影し、近傍点探索を行うことが好ましい。なお、直前データ６と現在データ７の親子関係は入れ替えてもよい。
In FIG. 3, step S33 is a neighborhood point searching step. For each point in the immediately preceding
図３において、ステップＳ３４は、位置合わせ量算出ステップである。図８は、位置合わせ量算出ステップの説明図である。
この図に示すように、ステップＳ３４において、直前データ６と現在データ７の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する位置合わせ量を算出する。
ここで「直前データ６と現在データ７の差が最小となる」とは、「直前データ６と現在データ７の差、例えば図８では直前データ６をもとに推定あるいは近似した地表面に対する現在データ７の垂直距離Ｌの二乗和が最小となる」ことを意味する。
位置合わせ量は、現在の３次元点群データ４Ｂを幾何変換するための、位置と姿勢の変換量である。
In FIG. 3, step S34 is a registration amount calculation step. FIG. 8 is an explanatory diagram of the alignment amount calculation step.
As shown in this figure, in step S34, an alignment amount for correcting the position of the current three-dimensional
Here, “the difference between the immediately preceding
The alignment amount is a position and orientation conversion amount for geometrically converting the current three-dimensional
図３において、ステップＳ３５は、幾何変換ステップである。図９は、幾何変換ステップの説明図である。
図９において、（Ａ）は変換前、（Ｂ）は変換後を示している。また各図において、破線は直前データ６、実線は現在データ７である。
図９（Ａ）は、前方の地形が同じであるが、移動体１が上向きにチルトしたため、現在データ７と直前データ６の距離の差が大きい状態を示している。この場合、図９（Ｂ）に示すように、ステップＳ３４で求めた位置合わせ量に基づき、現在データ７を直前データ６の位置まで幾何変換することで、直前データ６との誤差を最小化することができる。
なお、この幾何変換は、現在データ７のみではなく、現在の３次元点群データ４Ｂの全体を幾何変換する。
In FIG. 3, step S35 is a geometric transformation step. FIG. 9 is an explanatory diagram of the geometric conversion step.
In FIG. 9, (A) shows before conversion and (B) shows after conversion. In each figure, the broken line is the immediately preceding
FIG. 9A shows a state where the frontal terrain is the same, but the distance between the
In this geometric transformation, not only the
図３において、ステップＳ３６は、自己位置姿勢情報の修正ステップである。
ステップＳ３６において、直前データ６と現在データ７の距離の差が最小となるように、移動体１の自己位置姿勢情報（現在の位置情報と姿勢情報）を修正することで、計測誤差の影響を低減し、より妥当な自己位置姿勢情報を得ることができる。
このステップＳ３６により、現在の移動体１が有する位置姿勢情報（移動体１の位置又は姿勢の情報）の誤差が修正される。また、このステップを常に繰り返すことから、直前の移動体１の位置姿勢情報の誤差が低減され、同様に数値標高モデル（ＤＥＭ）である地図データ５は、移動体１の位置姿勢情報の誤差に起因する進行方向の路面上におけるずれが低減されたものとなる。
In FIG. 3, step S36 is a step for correcting the self-position / posture information.
In step S36, the self-position / posture information (current position information and posture information) of the moving
By this step S36, the error of the position / orientation information (position or orientation information of the moving body 1) of the current moving
ステップＳ３４では、所望の最適化手法、例えば最小二乗法を適用できる。あるいは、粒子フィルタ（パーティクルフィルタ、モンテカルロ法などとも呼ばれる）を用いて解探索を行うことができる。
また、ステップＳ３６で自己位置姿勢を修正する際は、各センサの出力値および上述した最適化手法により得られた推定量を統合するため、カルマンフィルタなどの状態推定方法を用いることができる。
In step S34, a desired optimization method, for example, a least square method can be applied. Alternatively, a solution search can be performed using a particle filter (also referred to as a particle filter or a Monte Carlo method).
Further, when correcting the self-position / posture in step S36, a state estimation method such as a Kalman filter can be used to integrate the output values of the sensors and the estimation amount obtained by the optimization method described above.
さらに、直前データ６ないし現在データ７に対して路面推定処理を行い、ステップＳ３の位置合わせ量算出への寄与度が小さい点をあらかじめ除去しておくことが好ましい。
例えば最小二乗法やＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）などのランダムサンプリング手法を用いて、路面推定すなわち路面を構成する点群の抽出を行い、推定後の点群を用いて点群位置合わせ方法を適用することができる。
上述した路面推定処理により障害物Ｇを除去した状態の地図同士を位置合わせすることができるため、障害物Ｇに起因するミスマッチを低減できる。
Further, it is preferable to perform a road surface estimation process on the immediately preceding
For example, using a random sampling method such as the least square method or RANSAC (RANdom Sample Consensus), the road surface is estimated, that is, the point cloud constituting the road surface is extracted, and the point cloud registration method is applied using the estimated point cloud. be able to.
Since maps in a state where the obstacle G is removed by the road surface estimation process described above can be aligned, mismatches caused by the obstacle G can be reduced.
図３において、ステップＳ４は、地図データ５の地図更新ステップであり、修正した現在の３次元点群データ４Ｃを直前までの３次元点群データ４に加算して、移動体周辺の高さ情報を含む地図データ５を更新する。
すなわち、ステップＳ３で現在の３次元点群データ４Ｂを幾何変換した３次元点群データ４Ｃを、直前までの３次元点群データ４に重ね合わせることで、ずれの少ない地図データ５を得ることができる。
In FIG. 3, step S4 is a map update step of the
That is, the
なお、上述した第１方向Ｆは、直前の移動体１の進行方向に限定されず、その他の方向であってもよい。また、直前データ６と現在データ７は、上述したそれぞれ直前基準平面Ｐ１と現在基準平面Ｐ２上のデータに限定されず、その他の任意の部分データであってもよい。以下、この場合の直前データ６と現在データ７をそれぞれ単に、「第１の部分点群」、「第２の部分点群」と呼ぶ。
In addition, the 1st direction F mentioned above is not limited to the advancing direction of the
この場合、上述したステップＳ２では、位置合わせ対象となる前後の点群データについて、前フレームの点群から前フレームの進行方向中央付近すなわち第１方向Ｆに沿った第１の部分点群を抽出し、後フレームの点群から同一の第１方向Ｆに沿った方向の第２の部分点群を抽出する。 In this case, in step S2 described above, the first partial point group in the vicinity of the center in the traveling direction of the previous frame, that is, the first direction point group is extracted from the point group of the previous frame from the point cloud data before and after the alignment target. Then, the second partial point group in the direction along the same first direction F is extracted from the point group of the rear frame.
＜進行方向の部分地形抽出、前フレームの進行方向に沿った部分地形抽出＞
位置合わせ対象となる前後の点群について、前フレームの点群から前フレームの進行方向中央付近すなわち第１方向Ｆに沿った第１の部分点群を抽出し、後フレームの点群から第１方向Ｆに沿った方向の第２の部分点群を抽出する。以下、第１の部分点群と第２の部分点群に点群位置合わせ方法を適用し、双方のずれを減少する位置合わせ量を得る。
<Partial terrain extraction in the direction of travel, partial terrain extraction along the travel direction of the previous frame>
For the front and rear point groups to be aligned, a first partial point group is extracted from the point group of the previous frame near the center in the traveling direction of the previous frame, that is, along the first direction F, and the first point group is extracted from the point group of the rear frame. A second partial point group in the direction along the direction F is extracted. Hereinafter, the point group alignment method is applied to the first partial point group and the second partial point group to obtain an alignment amount that reduces the deviation between the two.
本発明の目的においては、少なくとも重力方向の併進ずれ、およびピッチ角方向の回転ずれ量を得る必要がある。また、一般的な車道においては進行方向に障害物Ｇが少なく、点群位置合わせのみでは進行方向のずれ量が推測できない場合があるため、進行方向の移動量はＧＰＳによる位置情報、あるいは加速度センサや車速パルスなどを統合して得られる移動量などを用いることができる。点群位置合わせ方法として、例えばＩＣＰ法（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ法）が利用できる。ＩＣＰ法は、対応点同士の距離を繰り返し計算により最小化し、点群の位置合わせを行う解析手段である。 In the object of the present invention, it is necessary to obtain at least a translational deviation in the gravitational direction and a rotational deviation in the pitch angle direction. Further, since there are few obstacles G in the traveling direction on a general roadway, and the amount of deviation in the traveling direction may not be estimated only by point cloud alignment, the amount of movement in the traveling direction is determined by GPS position information or an acceleration sensor. The amount of movement obtained by integrating the vehicle speed pulse and the like can be used. As a point group alignment method, for example, an ICP method (Iterative Closest Point method) can be used. The ICP method is an analysis means for minimizing the distance between corresponding points by repeated calculation and aligning point groups.
＜平面投影、近傍点探索＞
上述した第１の部分点群あるいは第２の部分点群を得る際、レーザレンジファインダ２の仕様によって同一方向に点群が整列していない場合がある。また移動体１の姿勢変化や計測誤差などの要因により、第１の部分点群と第２の部分点群の向きは厳密には一致しない。そこで重力方向軸と上述した第１方向Ｆが張る平面に点群を投影することで、部分点群同士の向きを揃え、３次元点群の位置合わせを２次元的に行うことができる。
第１の部分点群と第２の部分点群の間には上述した平面に対し垂直方向のオフセットが生じてしまう場合があるが、上述した投影により同一平面上で点群位置合わせを行い、上述したオフセットの影響は少ないものと見なす。
<Planar projection, neighborhood search>
When obtaining the first partial point group or the second partial point group described above, the point groups may not be aligned in the same direction depending on the specifications of the
An offset in the direction perpendicular to the plane described above may occur between the first partial point group and the second partial point group, but the point group alignment is performed on the same plane by the above-described projection, The effect of the offset described above is considered to be small.
ＩＣＰ法のような近傍点探索を必要とする点群位置合わせ方法において、近傍点探索による計算コストの増大が課題となる場合があるが、上述した投影により点群が第１方向Ｆに沿って配置されるため、近傍点探索が容易になる。点群の位置合わせを行う際、位置合わせ不良の原因となる点群を事前に除去しておくことで、位置合わせ計算を安定化できる。例えば路面推定により路面のみを含む点群同士で位置合わせを行うのが好適である。 In a point cloud positioning method that requires a search for neighboring points such as the ICP method, an increase in calculation cost due to the neighboring point search may be a problem. However, the point cloud is moved along the first direction F by the above-described projection. Since it is arranged, it is easy to search for neighboring points. When the point cloud is aligned, the alignment calculation can be stabilized by removing in advance the point cloud that causes the alignment failure. For example, it is preferable to perform alignment between point groups including only the road surface by road surface estimation.
＜前フレームの法線推定、位置合わせ量算出＞
ＩＣＰ法などの点群位置合わせ方法は、対応点同士の結びつきが強く、局所解に陥りやすい。そのため、レーザレンジファインダ２のような疎な点群同士の位置合わせには向いていない。本発明では、上述した局所解への解析手段として、一方あるいは双方の点群を折れ線や曲線で補間する、あるいは一方の点群の法線を推定し上述した法線による疑似平面を仮定することで位置合わせ量を得る。
<Estimation of normal of previous frame, calculation of alignment amount>
The point cloud positioning method such as the ICP method has strong connection between corresponding points, and easily falls into a local solution. Therefore, it is not suitable for alignment of sparse point groups like the
例えば、点群対疑似平面の垂直距離をもとに位置合わせを行うよう拡張されたＩＣＰ法は「Ｐｏｉｎｔ‐ｔｏ‐Ｐｌａｎｅ型ＩＣＰ法」などと呼ばれ、本発明に適用可能な解析手段の１つである。ＩＣＰ法を適用する際、初期姿勢の決定方法が課題となる場合がある。例えば、パーティクルフィルタを用いて初期値探索を行うことができる。パーティクルフィルタは粒子フィルタ、あるいはモンテカルロフィルタなどと呼ばれることもあるが、本発明に適用可能な解析手段の別称にすぎない。また、本発明はパーティクルフィルタ以外の初期値探索の可能性を拒絶するものではなく、他の初期値探索方法を適用することも可能である。 For example, an ICP method extended to perform alignment based on a vertical distance between a point group and a pseudo-plane is called a “Point-to-Plane ICP method”, and is one of analysis means applicable to the present invention. One. When applying the ICP method, a method for determining an initial posture may be a problem. For example, an initial value search can be performed using a particle filter. The particle filter is sometimes referred to as a particle filter or a Monte Carlo filter, but is merely another name for analysis means applicable to the present invention. Further, the present invention does not reject the possibility of initial value search other than the particle filter, and other initial value search methods can be applied.
＜地形情報の幾何変換、自己位置姿勢情報の修正＞
上述した位置合わせ量に従い、各部分点群の親である３次元点群に幾何変換、例えば回転変換および並進変換を行う。上述した幾何変換により、ずれの少ない合成地形を得ることができる。このとき、上述した位置合わせ量に応じて移動体１の自己位置姿勢情報を修正することで、計測誤差の影響を低減し、より妥当な自己位置姿勢情報を得ることができる。センサ情報や自己位置姿勢情報を統合する際、カルマンフィルタなどの状態推定方法を用いることができる。
<Geometric transformation of terrain information, correction of position and orientation information>
In accordance with the alignment amount described above, geometric transformation, for example, rotation transformation and translation transformation, is performed on the three-dimensional point group that is the parent of each partial point group. By the above-described geometric transformation, it is possible to obtain a composite terrain with little deviation. At this time, by correcting the self-position / posture information of the moving
＜前フレームの部分地形の上書き＞
複数の点群を連続して位置合わせしていく際、不要になった点群を極力早期に削除ないし削減したい場合がある。例えば部分点群を得る際、上述した第１の部分点群および第２の部分点群に加え、後フレームの進行方向すなわち第２の方向に沿った第３の部分点群さえ確保できれば、上述した位置合わせを連続して行うことができる。また、第１の部分点群を毎回更新せず、特定の時間保持し続けてもよい。
<Overwrite partial terrain of previous frame>
When sequentially aligning a plurality of point clouds, there are cases where it is desired to delete or reduce unnecessary point clouds as early as possible. For example, when obtaining the partial point group, in addition to the first partial point group and the second partial point group described above, if the third partial point group along the traveling direction of the rear frame, that is, the second direction can be secured, The alignment can be performed continuously. Further, the first partial point group may not be updated every time and may be held for a specific time.
＜計算時間が低減できることに関する具体的説明＞
レーザレンジファインダ２で取得した２組以上の点群の位置ずれ量を求め位置合わせを行う場合、計測点数の増大に伴い計算時間も増大することが課題とされてきた。
上述した課題に対し、従来方法では例えば点間の起伏などをもとに点群の特徴部分を抽出し位置合わせを行う方法が提案されている。しかし、特徴部分の抽出のための前処理コストが生じ、抽出方法によっては十分に点数を削減することができない、あるいは適切に特徴部分を抽出することができないなどの課題があった。
本発明は位置合わせに必要な点を取得する際に全点群のうち所望の方位に分布する点群を選択するだけなので、追加の処理が不要である。また、位置合わせのために抽出する点数も高々レーザレンジファインダ２のレイヤ数（進行方向のレーザ照射点数）程度なので、位置合わせ計算のコストを容易に低減できる。
<Specific explanation regarding reduction of calculation time>
When positioning is performed by obtaining positional deviation amounts of two or more sets of point groups acquired by the
In order to solve the above-described problems, a conventional method has been proposed in which, for example, a feature part of a point group is extracted and aligned based on undulations between points. However, preprocessing costs for extracting feature portions are generated, and there are problems that the number of points cannot be sufficiently reduced depending on the extraction method, or feature portions cannot be extracted appropriately.
Since the present invention only selects a point group distributed in a desired direction from all point groups when acquiring points necessary for alignment, no additional processing is required. Further, since the number of points extracted for alignment is at most about the number of layers of the laser range finder 2 (the number of laser irradiation points in the traveling direction), the cost of alignment calculation can be easily reduced.
本発明の移動体システムは、移動体が３次元点群データの位置合わせ装置を搭載する。 In the mobile body system of the present invention, the mobile body is equipped with a three-dimensional point cloud data alignment device.
また、移動体とは別の場所に設置され移動体と通信する通信装置を備え、この通信装置が３次元点群データの位置合わせ装置の一部を搭載してもよい。 In addition, a communication device that is installed in a place different from the mobile body and communicates with the mobile body may be provided, and the communication device may be mounted with a part of the alignment device for the three-dimensional point cloud data.
上述した本発明の方法と装置及びその移動体システムによれば、データ抽出ステップＳ２において、直前データ６と現在データ７を抽出する。直前データ６は、直前の移動体１の進行方向である第１方向Ｆに位置する鉛直平面上のデータであり、直前の３次元点群データ４Ａの一部である。また、現在データ７は、第１方向Ｆに位置する鉛直平面上のデータであり、現在の３次元点群データ４Ｂの一部である。
直前データ６と現在データ７は取得時がわずか（例えば１／２０秒程度）に異なるが、同一の第１方向Ｆ（直前の移動体１の進行方向）のデータなので、実質的にほとんど一致する。しかし、移動体１の位置又は姿勢に誤差がある場合、その誤差により、直前データ６と現在データ７との間に距離の差が生じる。
According to the above-described method and apparatus of the present invention and the mobile system thereof, the
The immediately preceding
本発明では、位置修正ステップＳ３において、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。この修正により、移動体１の位置又は姿勢の誤差の影響を排除することができる。
従って、本発明によれば、移動体１の位置情報又は姿勢情報に誤差が発生しても、誤差の影響を排除した地図データ５を作成することができ、移動体１から離れた位置の小さい障害物Ｇ（例えば高さ２〜３０ｃｍ）を検出できる。
In the present invention, in the position correction step S3, the current position of the three-dimensional
Therefore, according to the present invention, even if an error occurs in the position information or posture information of the moving
また、直前データ６と現在データ７は、レーザレンジファインダ２で取得した３次元点群データ４Ａ、４Ｂのうち、直前の移動体１の進行方向である第１方向Ｆのデータであり、それぞれ３次元点群データ４Ａ、４Ｂから追加の処理なしに抽出でき、かつその点数は少ない（例えば５０点）。
従って、追加の処理なしに短時間（例えば１／２０秒以内）に点群位置合わせを完了でき、これにより、移動体１の位置情報又は姿勢情報を修正でき、かつ移動体１の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。
Further, the immediately preceding
Therefore, the point cloud positioning can be completed in a short time (for example, within 1/20 second) without additional processing, whereby the position information or posture information of the moving
言い換えれば、本発明によれば、少ない計算コストで、デッドレコニングによる自己位置姿勢を補正し数値標高モデルの精度を向上することにより、進行方向に存在する背の低い（回避すべき）障害物Ｇを発見しやすくなる。 In other words, according to the present invention, the short obstacle (which should be avoided) existing in the traveling direction is corrected by correcting the self-position and posture by dead reckoning and improving the accuracy of the digital elevation model with low calculation cost. Makes it easier to discover.
なお本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。 Note that the present invention is not limited to the above-described embodiment, and it is needless to say that various modifications can be made without departing from the gist of the present invention.
Ｆ 第１方向、Ｇ 障害物、Ｌ 垂直距離、
Ｐ１ 直前基準平面、Ｐ２ 現在基準平面、Ｚ 水平旋回軸、
１ 移動体、２ レーザレンジファインダ（ＬＲＦ）、
３ レーザ光、４ ３次元点群データ、４Ａ 直前の３次元点群データ、
４Ｂ 現在の３次元点群データ、４Ｃ 修正した現在の３次元点群データ、
５ 地図データ、６ 直前データ、７ 現在データ、１０ 位置合わせ装置、
１２ 地図メモリ、１４ 直前メモリ、１６ 現在メモリ、１８ データ処理装置
F first direction, G obstacle, L vertical distance,
P1 immediately preceding reference plane, P2 current reference plane, Z horizontal pivot axis,
1 mobile body, 2 laser range finder (LRF),
3 Laser light, 4 3D point cloud data, 4A 3D point cloud data immediately before,
4B current 3D point cloud data, 4C modified current 3D point cloud data,
5 Map data, 6 Last data, 7 Current data, 10 Positioning device,
12 Map memory, 14 Previous memory, 16 Current memory, 18 Data processing device
Claims (13)
（Ａ）直前の３次元点群データのうち、直前の移動体の進行方向である第１方向に位置する鉛直平面上の直前データと、
現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データと、を抽出するデータ抽出ステップと、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置修正ステップと、
（Ｃ）修正した現在の３次元点群データを直前までの３次元点群データに加算して、移動体周辺の高さ情報を含む地図データを更新する地図更新ステップと、を有する、ことを特徴とする３次元点群データの位置合わせ方法。 A method for aligning 3D point cloud data acquired with a laser range finder mounted on a moving object,
(A) Of the immediately preceding three-dimensional point cloud data, immediately preceding data on a vertical plane located in the first direction that is the traveling direction of the immediately preceding moving body;
A data extraction step of extracting current data on a vertical plane located in the first direction from the current three-dimensional point cloud data;
(B) a position correcting step for correcting the position of the current three-dimensional point cloud data so that the difference between the immediately preceding data and the current data is minimized;
(C) adding the corrected current three-dimensional point cloud data to the previous three-dimensional point cloud data to update the map data including the height information around the moving body, and a map updating step. A feature 3D point cloud data alignment method.
移動体周辺の高さ情報を含む地図データを記憶する地図メモリと、
直前の３次元点群データのうち、直前の移動体の進行方向である第１方向と、該第１方向に位置する鉛直平面上の直前データとを記憶する直前メモリと、
現在の３次元点群データを記憶する現在メモリと、
データ処理装置と、を備え、該データ処理装置は、
（Ａ）現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データを抽出し、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正し、
（Ｃ）修正した現在の３次元点群データを直前までの地図メモリに加算する、ことを特徴とする３次元点群データの位置合わせ装置。 An apparatus for aligning 3D point cloud data acquired with a laser range finder mounted on a moving body,
A map memory for storing map data including height information around the moving body;
Of the immediately preceding three-dimensional point cloud data, the immediately preceding memory for storing the first direction, which is the traveling direction of the immediately preceding moving body, and the immediately preceding data on the vertical plane located in the first direction;
A current memory for storing current 3D point cloud data;
A data processing device, the data processing device comprising:
(A) Extracting current data on a vertical plane located in the first direction from the current three-dimensional point cloud data,
(B) Correct the position of the current three-dimensional point cloud data so that the difference in distance between the immediately preceding data and the current data is minimized.
(C) A three-dimensional point cloud data alignment device characterized in that the corrected current three-dimensional point cloud data is added to the previous map memory.
前記直前メモリの前記第１方向を、現在の移動体の進行方向に置換し、
前記直前メモリの直前データを、現在の移動体の進行方向に位置する鉛直平面上のデータに置換する、ことを特徴とする請求項１０に記載の３次元点群データの位置合わせ装置。 The data processing apparatus, after the (C),
Replacing the first direction of the immediately preceding memory with the current traveling direction of the moving body;
11. The three-dimensional point cloud data alignment apparatus according to claim 10, wherein the immediately preceding data in the immediately preceding memory is replaced with data on a vertical plane located in the traveling direction of the current moving body.
A communication device that is installed at a location different from the mobile body and communicates with the mobile body, and the communication device is equipped with a part of the alignment device for 3D point cloud data according to claim 10 or 11, A mobile system characterized by that.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2014168996A JP6432825B2 (en) | 2014-08-22 | 2014-08-22 | Method and apparatus for aligning three-dimensional point cloud data and moving body system thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2014168996A JP6432825B2 (en) | 2014-08-22 | 2014-08-22 | Method and apparatus for aligning three-dimensional point cloud data and moving body system thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2016045330A true JP2016045330A (en) | 2016-04-04 |
| JP6432825B2 JP6432825B2 (en) | 2018-12-05 |
Family
ID=55635955
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2014168996A Expired - Fee Related JP6432825B2 (en) | 2014-08-22 | 2014-08-22 | Method and apparatus for aligning three-dimensional point cloud data and moving body system thereof |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6432825B2 (en) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017207438A (en) * | 2016-05-20 | 2017-11-24 | アジア航測株式会社 | Topographic change analysis method |
| JP2018025553A (en) * | 2016-07-27 | 2018-02-15 | 株式会社エムアールサポート | Shape measuring method, device and program for three dimensional measurement object |
| JP2018044769A (en) * | 2016-09-12 | 2018-03-22 | 株式会社日立製作所 | Self position estimation device, mobile entity, and self position estimation method |
| WO2018101455A1 (en) * | 2016-12-02 | 2018-06-07 | 東日本旅客鉄道株式会社 | Facility patrol system and facility patrol method |
| JP2020020694A (en) * | 2018-08-01 | 2020-02-06 | トヨタ自動車株式会社 | Axis deviation detector and vehicle |
| JP2020034559A (en) * | 2018-08-30 | 2020-03-05 | バイドゥ オンライン ネットワーク テクノロジー （ベイジン） カンパニー リミテッド | Method, apparatus, device, and storage medium for calibrating posture of dynamic obstacle |
| JP2021066423A (en) * | 2020-07-30 | 2021-04-30 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| JP2021075263A (en) * | 2020-06-02 | 2021-05-20 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| JP2021075262A (en) * | 2020-06-02 | 2021-05-20 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| WO2022064665A1 (en) * | 2020-09-28 | 2022-03-31 | 日本電気株式会社 | Measurement device, information processing device, position adjustment method, and computer-readable medium |
| KR102470705B1 (en) * | 2022-03-15 | 2022-11-25 | (주)뷰런테크놀로지 | Method and system for calibrating Lidar point data |
| JP2023519624A (en) * | 2020-04-03 | 2023-05-11 | メルセデス・ベンツ グループ アクチェンゲゼルシャフト | Method for recognizing decalibration of a vehicle's lidar sensor |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008020370A (en) * | 2006-07-13 | 2008-01-31 | Toyota Motor Corp | Autonomous moving apparatus |
| US20090125175A1 (en) * | 2007-11-09 | 2009-05-14 | Samsung Electronics Co., Ltd. | Apparatus and method for generating three-dimensional map using structured light |
| JP2009193240A (en) * | 2008-02-13 | 2009-08-27 | Toyota Motor Corp | Mobile robot and method for generating environment map |
| JP2011221631A (en) * | 2010-04-06 | 2011-11-04 | Advanced Telecommunication Research Institute International | Robot self-position identification system |
| JP2012089174A (en) * | 2012-02-08 | 2012-05-10 | Fuji Soft Inc | Robot and program of information processor |
| JP2012133696A (en) * | 2010-12-24 | 2012-07-12 | Hitachi Ltd | Running assist device |
| JP2013205130A (en) * | 2012-03-28 | 2013-10-07 | Zenrin Co Ltd | Method for generating road surface shape data, device for generating road surface shape data and computer program |
| JP2013225336A (en) * | 2008-08-29 | 2013-10-31 | Mitsubishi Electric Corp | Overhead view image generation device, overhead view image generation method and overhead view image generation program |
-
2014
- 2014-08-22 JP JP2014168996A patent/JP6432825B2/en not_active Expired - Fee Related
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008020370A (en) * | 2006-07-13 | 2008-01-31 | Toyota Motor Corp | Autonomous moving apparatus |
| US20090125175A1 (en) * | 2007-11-09 | 2009-05-14 | Samsung Electronics Co., Ltd. | Apparatus and method for generating three-dimensional map using structured light |
| JP2009193240A (en) * | 2008-02-13 | 2009-08-27 | Toyota Motor Corp | Mobile robot and method for generating environment map |
| JP2013225336A (en) * | 2008-08-29 | 2013-10-31 | Mitsubishi Electric Corp | Overhead view image generation device, overhead view image generation method and overhead view image generation program |
| JP2011221631A (en) * | 2010-04-06 | 2011-11-04 | Advanced Telecommunication Research Institute International | Robot self-position identification system |
| JP2012133696A (en) * | 2010-12-24 | 2012-07-12 | Hitachi Ltd | Running assist device |
| JP2012089174A (en) * | 2012-02-08 | 2012-05-10 | Fuji Soft Inc | Robot and program of information processor |
| JP2013205130A (en) * | 2012-03-28 | 2013-10-07 | Zenrin Co Ltd | Method for generating road surface shape data, device for generating road surface shape data and computer program |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2017207438A (en) * | 2016-05-20 | 2017-11-24 | アジア航測株式会社 | Topographic change analysis method |
| JP2018025553A (en) * | 2016-07-27 | 2018-02-15 | 株式会社エムアールサポート | Shape measuring method, device and program for three dimensional measurement object |
| JP7208708B2 (en) | 2016-07-27 | 2023-01-19 | 株式会社エムアールサポート | Shape measurement method, apparatus, and program for three-dimensional measurement object |
| JP2018044769A (en) * | 2016-09-12 | 2018-03-22 | 株式会社日立製作所 | Self position estimation device, mobile entity, and self position estimation method |
| WO2018101455A1 (en) * | 2016-12-02 | 2018-06-07 | 東日本旅客鉄道株式会社 | Facility patrol system and facility patrol method |
| JP7119724B2 (en) | 2018-08-01 | 2022-08-17 | トヨタ自動車株式会社 | Shaft deviation detector and vehicle |
| JP2020020694A (en) * | 2018-08-01 | 2020-02-06 | トヨタ自動車株式会社 | Axis deviation detector and vehicle |
| JP2020034559A (en) * | 2018-08-30 | 2020-03-05 | バイドゥ オンライン ネットワーク テクノロジー （ベイジン） カンパニー リミテッド | Method, apparatus, device, and storage medium for calibrating posture of dynamic obstacle |
| US11087474B2 (en) | 2018-08-30 | 2021-08-10 | Baidu Online Network Technology (Beijing) Co., Ltd. | Method, apparatus, device, and storage medium for calibrating posture of moving obstacle |
| JP7356604B2 (en) | 2020-04-03 | 2023-10-04 | メルセデス・ベンツ グループ アクチェンゲゼルシャフト | Method for recognizing decalibration of a vehicle's lidar sensor |
| JP2023519624A (en) * | 2020-04-03 | 2023-05-11 | メルセデス・ベンツ グループ アクチェンゲゼルシャフト | Method for recognizing decalibration of a vehicle's lidar sensor |
| JP2021075262A (en) * | 2020-06-02 | 2021-05-20 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| JP2021075263A (en) * | 2020-06-02 | 2021-05-20 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| JP2021066423A (en) * | 2020-07-30 | 2021-04-30 | 株式会社センシンロボティクス | Aerial vehicle, inspection method and inspection system |
| WO2022064665A1 (en) * | 2020-09-28 | 2022-03-31 | 日本電気株式会社 | Measurement device, information processing device, position adjustment method, and computer-readable medium |
| JP7452681B2 (en) | 2020-09-28 | 2024-03-19 | 日本電気株式会社 | Measuring device, information processing device, alignment method, and program |
| KR102470705B1 (en) * | 2022-03-15 | 2022-11-25 | (주)뷰런테크놀로지 | Method and system for calibrating Lidar point data |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6432825B2 (en) | 2018-12-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6432825B2 (en) | Method and apparatus for aligning three-dimensional point cloud data and moving body system thereof | |
| Stöcker et al. | Quality assessment of combined IMU/GNSS data for direct georeferencing in the context of UAV-based mapping | |
| JP6827627B2 (en) | Methods and systems for generating and updating vehicle environment maps | |
| JP6694395B2 (en) | Method and system for determining position relative to a digital map | |
| JP6354556B2 (en) | POSITION ESTIMATION DEVICE, POSITION ESTIMATION METHOD, POSITION ESTIMATION PROGRAM | |
| KR101625486B1 (en) | Map-based positioning system and method thereof | |
| CN102506824B (en) | Method for generating digital orthophoto map (DOM) by urban low altitude unmanned aerial vehicle | |
| CN110703268B (en) | Air route planning method and device for autonomous positioning navigation | |
| Di et al. | Photogrammetric processing of rover imagery of the 2003 Mars Exploration Rover mission | |
| Carle et al. | Global rover localization by matching lidar and orbital 3d maps | |
| JP2013187862A (en) | Image data processing device, image data processing method, and program for image data processing | |
| Ab Rahman et al. | Volumetric calculation using low cost unmanned aerial vehicle (UAV) approach | |
| CN113706702A (en) | Mining area three-dimensional map construction system and method | |
| WO2018142057A1 (en) | Method and device for calibrating a perception system including a set of lidar rangefinders | |
| Li et al. | MER Spirit rover localization: Comparison of ground image–and orbital image–based methods and science applications | |
| CN110986888A (en) | Aerial photography integrated method | |
| JP2023127588A (en) | Information processing device | |
| Li et al. | Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system | |
| Hussnain et al. | Enhanced trajectory estimation of mobile laser scanners using aerial images | |
| KR101409802B1 (en) | System for analysis space information using three dimensions 3d scanner | |
| Previtali et al. | Accurate 3D surface measurement of mountain slopes through a fully automated image-based technique | |
| Toth et al. | Terrain-based navigation: Trajectory recovery from LiDAR data | |
| CN115574816A (en) | Bionic vision multi-source information intelligent perception unmanned platform | |
| Furgale et al. | A comparison of global localization algorithms for planetary exploration | |
| Li et al. | Integration of orbital and ground images for enhanced topographic mapping in Mars landed missions |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20170628 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20180427 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20180601 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20180620 |
|
| 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: 20181012 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20181025 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 6432825 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
| LAPS | Cancellation because of no payment of annual fees |