JP6874343B2

JP6874343B2 - Map making device

Info

Publication number: JP6874343B2
Application number: JP2016228278A
Authority: JP
Inventors: 祐司津坂; 亮暢藤井
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2016-11-24
Filing date: 2016-11-24
Publication date: 2021-05-19
Anticipated expiration: 2036-11-24
Also published as: JP2018084699A

Description

本明細書に開示する技術は、対象領域の環境地図を作成する地図作成装置に関する。 The technique disclosed herein relates to a mapping device that creates an environmental map of a target area.

例えば、移動体に対象領域内を自律的に移動させる場合、対象領域内の障害物（例えば、壁等）を記憶する環境地図が必要となる。対象領域内の環境地図を作成する技術としては、例えば、Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｌｉｚａｔｉｏｎａｎｄＭａｐｐｉｎｇ）技術が知られている。Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術では、通常、レーザレンジファインダ（ＬＲＦ）等のセンサを搭載した移動体を対象領域内で移動させ、移動経路上に設けた複数の計測点においてセンサにより環境情報を取得する。そして、地図作成装置を用いて、複数の計測点で取得した環境情報に基づいて環境地図を作成する。すなわち、地図作成装置には、複数の計測点のそれぞれについて、センサによって計測された環境情報と、各計測点における計測時のセンサの位置を規定するセンサ位置情報が入力される。次に、入力された環境情報とセンサ位置情報に基づいて、地図作成装置は、計測点のセンサ位置やセンサによって計測された計測物（対象領域内の障害物等）の位置を規定するノードと、関連する２つのノード間の相対的位置関係を規定するエッジから構成されるグラフ構造データを生成する。地図作成装置は、複数のエッジが表す想定誤差を最小化することによってグラフ構造データを最適化する。Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術を用いることによって、地図作成装置は、誤差の小さい環境地図を作成することができる。 For example, when a moving body autonomously moves in a target area, an environment map for storing obstacles (for example, a wall or the like) in the target area is required. As a technique for creating an environmental map in a target area, for example, a Graph-based SLAM (Simultaneus Localization and Mapping) technique is known. In the Graph-based SLAM technology, a moving body equipped with a sensor such as a laser range finder (LRF) is usually moved within a target region, and environmental information is acquired by the sensors at a plurality of measurement points provided on the moving path. Then, using the map creation device, an environmental map is created based on the environmental information acquired at a plurality of measurement points. That is, for each of the plurality of measurement points, the environment information measured by the sensor and the sensor position information that defines the position of the sensor at the time of measurement at each measurement point are input to the map creation device. Next, based on the input environmental information and sensor position information, the map creation device is a node that defines the sensor position of the measurement point and the position of the measured object (obstacle, etc. in the target area) measured by the sensor. , Generates graph structure data composed of edges that define the relative positional relationship between two related nodes. The mapping device optimizes the graph structure data by minimizing the assumption error represented by the plurality of edges. By using the Graph-based SLAM technology, the mapping apparatus can create an environmental map with a small error.

ＬＲＦ等のセンサによって取得した環境情報に基づいて環境地図を作成する場合、センサの計測誤差等によって、作成された環境地図に誤差が生じる。このため、作成された環境地図を、真値情報を用いて修正する技術が知られている。例えば、特許文献１には、ＧＰＳ測位機能によって取得した真値情報を用いて地図の精度を向上させる技術が開示されている。特許文献１の技術では、まず、ＧＰＳ測位機能によって基準点（真値情報）を取得する。次に、地図作成装置は、３点の基準点によって囲まれた三角形に基づいて、元の地図において基準点に対応する３点によって囲まれる三角形を補正する。この補正は、３点の基準点によって囲まれた三角形ごとに行われる。３点の基準点によって囲まれた三角形ごとに元の地図を補正することによって、元の地図は部分ごとに精度の高い地図に変換される。 When an environmental map is created based on the environmental information acquired by a sensor such as an LRF, an error occurs in the created environmental map due to a measurement error of the sensor or the like. For this reason, there is known a technique for modifying the created environment map using true value information. For example, Patent Document 1 discloses a technique for improving the accuracy of a map by using true value information acquired by a GPS positioning function. In the technique of Patent Document 1, first, a reference point (true value information) is acquired by a GPS positioning function. Next, the map-making device corrects the triangle surrounded by the three points corresponding to the reference points in the original map based on the triangle surrounded by the three reference points. This correction is made for each triangle surrounded by three reference points. By correcting the original map for each triangle surrounded by three reference points, the original map is converted into a highly accurate map for each part.

特開２００２−３４１７５７号公報JP-A-2002-341757

精度の高い環境地図を作成するためには、例えば、対象領域内に配置される計測物を測量機器で計測し、その計測データに基づいて地図を作成する方法がある。しかしながら、対象領域内に配置される多数の計測物を全て測量機器で計測することは、時間や手間がかかるという問題があった。 In order to create a highly accurate environmental map, for example, there is a method of measuring a measurement object placed in a target area with a surveying instrument and creating a map based on the measurement data. However, there is a problem that it takes time and labor to measure all of a large number of measurement objects arranged in the target area with a surveying instrument.

Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術では、対象領域内の計測物を測量機器で計測していないため、簡易に環境地図を作成できる。しかしながら、センサによって複数の計測点で計測物を計測するため、複数の計測点ごとに計測誤差が生じる。このため、Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術を用いた地図作成装置は、グラフ構造データの最適化によって比較的誤差の小さい環境地図を作成することができるものの、測量機器で計測したデータに基づいて作成した地図と比較すると作成された環境地図に歪みが生じるという問題があった。 With the Graph-based SLAM technology, since the measured object in the target area is not measured by the surveying instrument, it is possible to easily create an environmental map. However, since the sensor measures the measured object at a plurality of measurement points, a measurement error occurs at each of the plurality of measurement points. Therefore, although the map creation device using the Graph-based SLAM technology can create an environment map with relatively small error by optimizing the graph structure data, the map created based on the data measured by the surveying instrument. There was a problem that the created environmental map was distorted when compared with.

また、特許文献１の技術では、ＧＰＳ測位機能によって取得した３点の基準点に基づいて元の地図を補正しているため、補正した部分については地図の精度を高くすることができる。しかしながら、３点の基準点によって囲まれた三角形ごとに元の地図を補正するため、部分ごとの地図の精度については高くすることができる一方、地図全体としては歪みが生じ易いという問題があった。 Further, in the technique of Patent Document 1, since the original map is corrected based on the three reference points acquired by the GPS positioning function, the accuracy of the map can be improved for the corrected portion. However, since the original map is corrected for each triangle surrounded by three reference points, the accuracy of the map for each part can be improved, but there is a problem that the map as a whole is easily distorted. ..

本明細書は、Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術を用いながら、比較的に精度の高い地図を作成する技術を開示する。 The present specification discloses a technique for creating a map with relatively high accuracy while using the Graph-based SLAM technique.

本明細書に開示する地図作成装置は、対象領域の環境地図を作成する。地図作成装置は、対象領域内の複数の計測点のそれぞれについて、センサによって計測された環境情報を記憶する環境情報記憶部と、複数の計測点のそれぞれについて、計測時のセンサの位置に関するセンサ位置情報を記憶するセンサ位置情報記憶部と、環境情報及びセンサ位置情報に基づいて、複数の計測点のそれぞれにおけるセンサの位置を規定するセンサノードと、隣接するセンサノード間におけるセンサの相対的位置関係を規定するセンサエッジと、複数の計測点のそれぞれにおいてセンサで計測された計測物の位置を規定する環境ノードと、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する環境エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、対象領域内に位置する計測物の中から選択された少なくとも１つの計測物の位置に関する真値情報を記憶する真値情報記憶部と、真値情報と、その真値情報に対応する環境ノードに基づいて、真値情報が規定する計測物の位置と対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジをグラフ構造データに追加する計測物エッジ追加部と、計測物エッジが追加されたグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化するグラフ構造データ最適化部と、を備えている。 The map-creating device disclosed in the present specification creates an environmental map of the target area. The map creation device has an environmental information storage unit that stores environmental information measured by a sensor for each of a plurality of measurement points in the target area, and a sensor position regarding the position of the sensor at the time of measurement for each of the plurality of measurement points. A sensor position information storage unit that stores information, a sensor node that defines the position of a sensor at each of a plurality of measurement points based on environmental information and sensor position information, and a relative positional relationship of sensors between adjacent sensor nodes. An environment that defines the relative positional relationship between the environment node and the sensor node corresponding to the environment node, the environment node that defines the position of the measured object measured by the sensor at each of the multiple measurement points, and the sensor edge that defines True value information that stores true value information about the position of at least one measurement object selected from the measurement objects located in the target area and the graph structure data generation unit that generates graph structure data composed of edges. Based on the storage unit, the true value information, and the environment node corresponding to the true value information, the deviation between the position of the measured object specified by the true value information and the position of the measured object specified by the corresponding environment node is specified. Optimizing the graph structure data so that the sum of the error functions calculated from each edge is minimized for the measurement object edge addition part that adds the measurement object edge to the graph structure data and the graph structure data to which the measurement object edge is added. It is equipped with a graph structure data optimization unit to be converted.

上記の地図作成装置では、真値情報と環境情報とのずれを規定する計測物エッジをグラフ構造データに追加することによって、グラフ構造データに真値情報が取込まれる。そして、真値情報が取込まれたグラフ構造データに含まれる誤差情報から算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。これにより、従来のＧｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術を用いて作成した地図と比較して地図の精度を高くできる。また、計測物エッジを導入したグラフ構造データを全体として最適化するため、真値情報を用いて地図を部分的に補正する場合と比較して地図全体の精度を高めることができる。 In the above-mentioned map creating device, the true value information is taken into the graph structure data by adding the measurement object edge that defines the deviation between the true value information and the environmental information to the graph structure data. Then, the graph structure data is optimized so that the sum of the error functions calculated from the error information included in the graph structure data in which the true value information is taken is minimized. As a result, the accuracy of the map can be improved as compared with the map created by using the conventional Graph-based SLAM technique. Further, since the graph structure data in which the measurement object edge is introduced is optimized as a whole, the accuracy of the entire map can be improved as compared with the case where the map is partially corrected by using the true value information.

実施例１に係る地図作成装置のシステム構成を示す図。The figure which shows the system configuration of the map making apparatus which concerns on Example 1. FIG. 地図作成処理を示すフローチャート。A flowchart showing a map creation process. 移動体に搭載されるセンサが対象領域内の計測物を計測する構成を模式的に示す図。The figure which shows typically the structure which the sensor mounted on the moving body measures the measurement object in a target area. センサノード及びセンサエッジを模式的に示す図。The figure which shows the sensor node and the sensor edge schematically. グラフ構造データを模式的に示す図。The figure which shows the graph structure data schematically. 測量器が対象領域内の計測物の特徴的な形状を示す地点を計測する構成を模式的に示す図。The figure which shows typically the structure which the surveying instrument measures the point which shows the characteristic shape of the measurement object in a target area. 実施例１に係る地図作成装置において、真値情報及び計測物エッジが追加されたグラフ構造データを模式的に示す図。FIG. 5 is a diagram schematically showing graph structure data to which true value information and measurement object edges are added in the map creation device according to the first embodiment. 測量用レーザスキャナが対象領域内の計測物の特徴的な形状部分（マーカー）を計測する構成を模式的に示す図。The figure which shows typically the structure which the laser scanner for survey measures the characteristic shape part (marker) of the measurement object in a target area. 実施例２に係る地図作成装置において、真値情報及び計測物エッジを追加したグラフ構造データを模式的に示す図。The figure which shows typically the graph structure data which added true value information and measurement object edge in the map making apparatus which concerns on Example 2. FIG.

以下に説明する実施例の主要な特徴を列記しておく。なお、以下に記載する技術要素は、それぞれ独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。 The main features of the examples described below are listed. The technical elements described below are independent technical elements and exhibit technical usefulness alone or in various combinations, and are limited to the combinations described in the claims at the time of filing. It's not a thing.

（特徴１）本明細書に開示する地図作成装置は、最適化されたグラフ構造データ内の環境ノードを用いて環境地図を作成する環境地図作成部をさらに備えていてもよい。このような構成によると、真値情報が取込まれ、最適化されたグラフ構造データ内の環境ノードに基づいて、精度の高い地図を作成することができる。 (Feature 1) The map creation device disclosed in the present specification may further include an environment map creation unit that creates an environment map using the environment nodes in the optimized graph structure data. With such a configuration, true value information is captured and a highly accurate map can be created based on the environment nodes in the optimized graph structure data.

（特徴２）本明細書に開示する地図作成装置では、真値情報は、測量、測定及び図面の少なくとも１つから取得されてもよい。 (Feature 2) In the mapping apparatus disclosed in the present specification, true value information may be obtained from at least one of surveying, measurement, and drawing.

（特徴３）本明細書が開示する地図作成装置では、真値情報記憶部は、少なくとも３つの座標で示される真値情報を記憶していてもよい。このような構成によると、少なくとも３点の真値情報に基づいて計測物ノードを生成することができる。このため、より精度の高い環境地図を作成することができる。 (Feature 3) In the map creation device disclosed in the present specification, the true value information storage unit may store true value information indicated by at least three coordinates. According to such a configuration, the measurement object node can be generated based on the true value information of at least three points. Therefore, it is possible to create a more accurate environmental map.

（特徴４）本明細書が開示する地図作成装置では、真値情報記憶部は、位置、姿勢及び形状で示される真値情報を記憶していてもよい。このような構成によると、位置、姿勢及び形状を備える真値情報に基づいて計測物ノードを生成することができる。このため、より精度の高い環境地図を作成することができる。 (Feature 4) In the map creation device disclosed in the present specification, the true value information storage unit may store true value information indicated by a position, a posture, and a shape. According to such a configuration, the measurement object node can be generated based on the true value information having the position, the posture, and the shape. Therefore, it is possible to create a more accurate environmental map.

以下、実施例１に係る地図作成装置１０について説明する。地図作成装置１０は、対象領域の環境地図を作成する。地図作成装置１０は、例えば、ＣＰＵ，ＲＯＭ，ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、地図作成装置１０は、図１に示すグラフ構造データ生成部１４、計測物エッジ追加部１８、グラフ構造データ最適化部２０、環境地図作成部２２等として機能する。地図作成装置１０の各部１４、１８、２０、２２の処理については、後で詳述する。 Hereinafter, the map creating device 10 according to the first embodiment will be described. The map creation device 10 creates an environmental map of the target area. The map creation device 10 can be configured by, for example, a computer equipped with a CPU, ROM, RAM, and the like. When the computer executes the program, the map creation device 10 functions as the graph structure data generation unit 14, the measurement object edge addition unit 18, the graph structure data optimization unit 20, the environment map creation unit 22, and the like shown in FIG. .. The processing of each part 14, 18, 20, and 22 of the map creating device 10 will be described in detail later.

また、図１に示すように、地図作成装置１０は、環境計測データ記憶部１２と、真値情報記憶部１６を備えている。環境計測データ記憶部１２は、センサ５２によって計測された対象領域内の環境情報及びセンサ位置情報を記憶する。後述するようにセンサ５２は移動体５０に搭載される。移動体５０は対象領域内を移動しながら、センサ５２によって対象領域内の環境（例えば、壁等の計測物）を計測する。センサ５２によって取得された環境情報は、その環境情報が取得されたときのセンサ５２の位置と共に環境計測データ記憶部１２に記憶される。真値情報記憶部１６は、測量器５６によって計測された対象領域内の真値情報を記憶する。環境計測データ記憶部１２に記憶される環境情報及びセンサ位置情報と、真値情報記憶部１６に記憶される真値情報については、後で詳述する。 Further, as shown in FIG. 1, the map creation device 10 includes an environment measurement data storage unit 12 and a true value information storage unit 16. The environment measurement data storage unit 12 stores the environment information and the sensor position information in the target area measured by the sensor 52. As will be described later, the sensor 52 is mounted on the moving body 50. While moving in the target area, the moving body 50 measures the environment (for example, a measured object such as a wall) in the target area by the sensor 52. The environmental information acquired by the sensor 52 is stored in the environmental measurement data storage unit 12 together with the position of the sensor 52 when the environmental information is acquired. The true value information storage unit 16 stores true value information in the target area measured by the surveying instrument 56. The environmental information and sensor position information stored in the environmental measurement data storage unit 12 and the true value information stored in the true value information storage unit 16 will be described in detail later.

図２〜図７を参照して、地図作成装置１０が行う処理について説明する。図２は、地図作成装置１０が行う地図作成処理を示すフローチャートである。図２に示すように、まず、地図作成装置１０は、移動体５０から対象領域内の複数の計測地点Ａ〜Ｈ（図３参照）において計測された環境情報及びセンサ位置情報を取得し、環境計測データ記憶部１２に記憶する（Ｓ１０）。ここで、移動体５０によって環境情報を計測する手順について説明する。 The processing performed by the map creating device 10 will be described with reference to FIGS. 2 to 7. FIG. 2 is a flowchart showing a map creation process performed by the map creation device 10. As shown in FIG. 2, first, the map creating device 10 acquires the environmental information and the sensor position information measured at a plurality of measurement points A to H (see FIG. 3) in the target area from the moving body 50, and then obtains the environment. It is stored in the measurement data storage unit 12 (S10). Here, the procedure for measuring the environmental information by the moving body 50 will be described.

移動体５０は、車輪駆動型の移動体である。移動体５０は、図示しない操作装置を備えており、例えば、ジョイスティック等のリモコンで操縦される。なお、本実施例では、移動体５０はリモコンで操縦されるが、このような構成に限定されない。例えば、移動体５０は、作業者がハンドル操作によって運転することによって移動してもよいし、作業者が手で押すことによって移動してもよい。また、移動体５０は飛翔体であってもよい。 The moving body 50 is a wheel-driven moving body. The moving body 50 includes an operating device (not shown), and is operated by a remote controller such as a joystick. In this embodiment, the moving body 50 is operated by the remote controller, but the mobile body 50 is not limited to such a configuration. For example, the moving body 50 may be moved by the operator driving by operating the steering wheel, or may be moved by being pushed by the operator by hand. Further, the moving body 50 may be a flying body.

移動体５０は、センサ５２と記憶部５４を備えている。本実施例では、移動体５０が搭載するセンサ５２は、レーザレンジファインダ（ＬＲＦ）である。ＬＲＦは、レーザ光を射出し、射出したレーザ光が物体で反射して返ってくるまでの時間を計測する。ＬＲＦで計測された時間から、ＬＲＦから物体までの距離が計測される。また、ＬＲＦからレーザ光を射出した方向（すなわち、物体から反射されるレーザ光の入射角度）は既知であるため、ＬＲＦに対する物体の方位を決定することができる。ＬＲＦは、計測地点から見た壁や柱等（以下、「計測物６２」という）の位置、姿勢及び形状等の環境情報を計測することができる。ＬＲＦで計測された環境情報は記憶部５４に記憶される。また、移動体５０は、車輪の回転角を検出するセンサ（例えば、エンコーダ）を備えている。このセンサによって車輪の回転角を検出することで、移動体５０の移動方向及び移動量を算出することができる。このため、移動体５０は、算出された移動体５０の移動方向及び移動量に基づいて、各計測地点における移動体５０の姿勢及び位置を算出することができる。移動体５０へのセンサ（ＬＲＦ）５２の設置状態は既知であるため、算出された各計測地点における移動体５０の姿勢及び位置に基づいて、各計測地点におけるセンサ５２の姿勢及び位置を算出することができる。記憶部５４には、各計測点においてセンサ５２で計測された環境情報に対応付けて、その計測点におけるセンサ５２の姿勢及び位置が記憶される。本実施例では、センサ５２が計測する環境情報及び算出されるセンサ位置情報を合わせて「環境計測データ」と称することがある。なお、センサ５２はＬＲＦに限定されるものではない。センサ５２は、環境情報を計測できればよく、例えば、センサ５２は画像情報によって計測物６２を計測するものであってもよい。センサ５２によって計測された計測物６２の環境情報は、移動体５０の記憶部５４に時系列で記憶される。また、センサ５２の位置及び姿勢も上記の方法（いわゆる、オドメトリ）に限られず、例えば、ＧＰＳセンサ等を用いて検出するようにしてもよい。 The mobile body 50 includes a sensor 52 and a storage unit 54. In this embodiment, the sensor 52 mounted on the moving body 50 is a laser range finder (LRF). The LRF emits a laser beam and measures the time until the emitted laser beam is reflected by an object and returned. From the time measured by the LRF, the distance from the LRF to the object is measured. Further, since the direction in which the laser beam is emitted from the LRF (that is, the incident angle of the laser beam reflected from the object) is known, the direction of the object with respect to the LRF can be determined. The LRF can measure environmental information such as the position, posture, and shape of a wall, a pillar, or the like (hereinafter referred to as “measurement object 62”) as seen from a measurement point. The environmental information measured by the LRF is stored in the storage unit 54. Further, the moving body 50 includes a sensor (for example, an encoder) that detects the rotation angle of the wheel. By detecting the rotation angle of the wheel with this sensor, the moving direction and the moving amount of the moving body 50 can be calculated. Therefore, the moving body 50 can calculate the posture and position of the moving body 50 at each measurement point based on the calculated moving direction and moving amount of the moving body 50. Since the installation state of the sensor (LRF) 52 on the moving body 50 is known, the posture and position of the sensor 52 at each measuring point are calculated based on the calculated posture and position of the moving body 50 at each measuring point. be able to. The storage unit 54 stores the posture and position of the sensor 52 at each measurement point in association with the environmental information measured by the sensor 52 at each measurement point. In this embodiment, the environmental information measured by the sensor 52 and the calculated sensor position information may be collectively referred to as "environmental measurement data". The sensor 52 is not limited to the LRF. The sensor 52 may be capable of measuring environmental information, and for example, the sensor 52 may measure the measured object 62 based on image information. The environmental information of the measurement object 62 measured by the sensor 52 is stored in the storage unit 54 of the moving body 50 in time series. Further, the position and orientation of the sensor 52 are not limited to the above method (so-called odometry), and may be detected by using, for example, a GPS sensor or the like.

図３は、移動体５０に搭載されるセンサ５２が、対象領域内の計測物６２を計測する状態を模式的に示している。移動体５０は、対象領域内に設定される移動経路６０上を移動する。対象領域内には、壁や柱等（すなわち、計測物６２）が配置されており、センサ５２は、計測地点から計測物６２までの距離及び計測物の形状等を計測する。図３に示される矢印は、移動体５０が計測地点Ｂに移動したときにセンサ５２から計測物６２ａに射出されるレーザと、移動体５０が計測地点Ｄに移動したときにセンサ５２から計測物６２ｂ、６２ｃに射出されるレーザを示している。なお、以下の明細書において、計測物６２を区別する必要があるときは計測物６２ａ、６２ｂ、６２ｃのように沿字のアルファベットを用いて記載し、区別する必要のないときは単に計測物６２と記載する場合がある。また、他の構成要素についても同一構成のものについて区別する必要がないときは、上記と同様に沿字のアルファベットを省略して単に数字で記載することがある。 FIG. 3 schematically shows a state in which the sensor 52 mounted on the moving body 50 measures the measured object 62 in the target area. The moving body 50 moves on the moving path 60 set in the target area. A wall, a pillar, or the like (that is, a measurement object 62) is arranged in the target area, and the sensor 52 measures the distance from the measurement point to the measurement object 62, the shape of the measurement object, and the like. The arrows shown in FIG. 3 indicate a laser emitted from the sensor 52 to the measurement object 62a when the moving body 50 moves to the measurement point B, and a measurement object from the sensor 52 when the moving body 50 moves to the measurement point D. The lasers emitted at 62b and 62c are shown. In the following specification, when it is necessary to distinguish the measurement object 62, it is described by using alphabets along the characters such as the measurement objects 62a, 62b, 62c, and when it is not necessary to distinguish them, the measurement object 62 is simply described. May be described as. In addition, when it is not necessary to distinguish other components having the same structure, the alphabets along the characters may be omitted and simply numbers may be used as described above.

図３に示すように、例えば、センサ５２は、地点Ｂにおいて計測可能な計測物６２ａの環境情報を計測し、また、移動体５０（詳細には、移動体５０に搭載された演算装置）は地点Ｂにおける計測時のセンサ位置情報を算出する。計測された環境情報と算出されたセンサ位置情報は記憶部５４に記憶される。また、移動体５０が地点Ｄに移動すると、センサ５２は、地点Ｄにおいて計測可能な計測物６２ｂの環境情報及び計測物６２ｃの環境情報を計測し、また、地点Ｄにおける計測時のセンサ位置情報が算出される。このようにして、移動体５０は、移動経路６０上を移動し、センサ５２は、対象領域内の計測物６２を計測し、その計測物６２を計測した地点のセンサ位置情報が算出される。計測された環境情報と算出されたセンサ位置情報は、時系列順に記憶部５４に蓄積される。記憶部５４に蓄積された環境情報とセンサ位置情報は、地図作成装置１０に入力され、環境計測データ記憶部１２に記憶される。なお、環境計測データ記憶部１２は、「環境情報記憶部」及び「センサ位置情報記憶部」の一例である。 As shown in FIG. 3, for example, the sensor 52 measures the environmental information of the measurement object 62a that can be measured at the point B, and the moving body 50 (specifically, the arithmetic unit mounted on the moving body 50) is The sensor position information at the time of measurement at the point B is calculated. The measured environmental information and the calculated sensor position information are stored in the storage unit 54. Further, when the moving body 50 moves to the point D, the sensor 52 measures the environmental information of the measurement object 62b and the environmental information of the measurement object 62c that can be measured at the point D, and the sensor position information at the time of measurement at the point D. Is calculated. In this way, the moving body 50 moves on the moving path 60, the sensor 52 measures the measured object 62 in the target region, and the sensor position information of the point where the measured object 62 is measured is calculated. The measured environmental information and the calculated sensor position information are stored in the storage unit 54 in chronological order. The environmental information and the sensor position information stored in the storage unit 54 are input to the map creation device 10 and stored in the environment measurement data storage unit 12. The environmental measurement data storage unit 12 is an example of the “environmental information storage unit” and the “sensor position information storage unit”.

次に、地図作成装置１０では、環境計測データ記憶部１２に記憶された環境情報及びセンサ位置情報に基づいて、グラフ構造データ生成部１４が対象領域内のグラフ構造データを生成する（Ｓ１２）。グラフ構造データは、センサノード３０、センサエッジ３２、環境ノード３４及び環境エッジ３６によって構成される。グラフ構造データ生成部１４は、環境情報及びセンサ位置情報に基づいてセンサノード３０、センサエッジ３２、環境ノード３４及び環境エッジ３６を生成し、生成したセンサノード３０及び環境ノード３４がセンサエッジ３２及び環境エッジ３６により関連付けられたグラフ構造データを生成する。 Next, in the map creating device 10, the graph structure data generation unit 14 generates the graph structure data in the target area based on the environment information and the sensor position information stored in the environment measurement data storage unit 12 (S12). The graph structure data is composed of a sensor node 30, a sensor edge 32, an environment node 34, and an environment edge 36. The graph structure data generation unit 14 generates the sensor node 30, the sensor edge 32, the environment node 34 and the environment edge 36 based on the environment information and the sensor position information, and the generated sensor node 30 and the environment node 34 generate the sensor node 32 and the sensor edge 32 and the environment node 34. Generate graph structure data associated with the environment edge 36.

すなわち、図４に示すように、グラフ構造データ生成部１４は、まず、環境計測データ記憶部１２が記憶するセンサ位置情報に基づいて、各計測地点Ａ〜Ｈにおける計測時のセンサ５２の位置や姿勢を規定するセンサノード３０ａ〜３０ｈをそれぞれ生成する。なお、隣接する計測地点の間隔は、対象領域内に設置されている複数の計測物６２の環境情報を計測できる間隔であればよい。隣接する計測地点の間隔が狭すぎると、計測地点の数が多くなり、後述するように各計測地点の姿勢誤差の蓄積が大きくなる。このため、隣接する計測地点の間隔は、対象領域内に設置される複数の計測物６２を計測することができる範囲で広くすることができる。 That is, as shown in FIG. 4, the graph structure data generation unit 14 first determines the position of the sensor 52 at the time of measurement at each measurement point A to H based on the sensor position information stored in the environment measurement data storage unit 12. Sensor nodes 30a to 30h that define the posture are generated respectively. The interval between adjacent measurement points may be any interval as long as it can measure the environmental information of a plurality of measurement objects 62 installed in the target area. If the distance between adjacent measurement points is too narrow, the number of measurement points will increase, and the accumulation of attitude errors at each measurement point will increase, as will be described later. Therefore, the distance between adjacent measurement points can be widened within a range in which a plurality of measurement objects 62 installed in the target area can be measured.

次に、グラフ構造データ生成部１４は、環境計測データ記憶部１２に記憶される時系列に蓄積されたセンサ位置情報及び環境情報に基づいて、隣接するセンサノード３０間におけるセンサ５２の相対的位置関係を規定するセンサエッジ３２を生成する。センサエッジ３２は、センサエッジ３２の両端にある２つのセンサノード３０の各姿勢についての制約を表しており、具体的には、運動誤差の共分散行列と相対位置・姿勢変換行列から表される公知の方法を用いて表すことができる。 Next, the graph structure data generation unit 14 determines the relative position of the sensor 52 between the adjacent sensor nodes 30 based on the sensor position information and the environment information stored in the environment measurement data storage unit 12 in the time series. Generate a sensor edge 32 that defines the relationship. The sensor edge 32 represents a constraint on each posture of the two sensor nodes 30 at both ends of the sensor edge 32, and is specifically represented by a covariance matrix of motion errors and a relative position / posture conversion matrix. It can be expressed using a known method.

例えば、グラフ構造データ生成部１４は、環境計測データ記憶部１２に記憶される時系列に蓄積された計測地点Ａ、Ｂの環境情報及びセンサ位置情報から、計測地点Ａ、Ｂ間のセンサ５２の移動量（すなわち、移動体５０が地点Ａ、Ｂ間を移動したことによって移動したセンサ５２の移動量）を算出する。この移動量と計測地点Ａ、Ｂのセンサ位置情報から、グラフ構造データ生成部１４は、センサノード３０ａとセンサノード３０ｂとを連結するセンサエッジ３２ａを生成する。センサエッジ３２ａは、センサノード３０ａ、３０ｂ間におけるセンサ５２の相対的位置関係を規定する。すなわち、センサエッジ３２ａは、センサ５２が計測地点Ａから計測地点Ｂまで移動したという事象を表している。同様にして、グラフ構造データ生成部１４は、時系列に蓄積された計測地点Ｂ、Ｃの環境情報及びセンサ位置情報から、センサノード３０ｂとセンサノード３０ｃとを連結するセンサエッジ３２ｂを生成する。このようにして、グラフ構造データ生成部１４は、全ての隣接するセンサノード３０間に、センサエッジ３２を生成ずる。 For example, the graph structure data generation unit 14 uses the environmental information and sensor position information of the measurement points A and B stored in the environmental measurement data storage unit 12 in time series to obtain the sensor 52 between the measurement points A and B. The movement amount (that is, the movement amount of the sensor 52 moved by the moving body 50 moving between the points A and B) is calculated. From the movement amount and the sensor position information of the measurement points A and B, the graph structure data generation unit 14 generates a sensor edge 32a that connects the sensor node 30a and the sensor node 30b. The sensor edge 32a defines the relative positional relationship of the sensor 52 between the sensor nodes 30a and 30b. That is, the sensor edge 32a represents an event in which the sensor 52 has moved from the measurement point A to the measurement point B. Similarly, the graph structure data generation unit 14 generates a sensor edge 32b that connects the sensor node 30b and the sensor node 30c from the environmental information and the sensor position information of the measurement points B and C accumulated in time series. In this way, the graph structure data generation unit 14 generates the sensor edge 32 between all the adjacent sensor nodes 30.

次に、グラフ構造データ生成部１４は、環境情報に基づいて、各計測地点Ａ〜Ｈにおいて計測された計測物６２の特徴（例えば、位置、姿勢及び形状等）を規定する環境ノード３４を生成する。また、グラフ構造データ生成部１４は、環境情報及びセンサ位置情報に基づいて、環境ノード３４とその環境ノード３４を規定する環境情報を計測した位置のセンサノード３０との相対的位置関係を規定する環境エッジ３６を生成する。環境エッジ３６は、センサノード３０とそのセンサノード３０の位置から計測された環境情報によって規定された環境ノード３４との間の姿勢の制約を表している。具体的には、環境エッジ３６は、計測誤差の共分散行列と相対位置・姿勢変換行列から表される公知の方法を用いて表すことができる。 Next, the graph structure data generation unit 14 generates an environment node 34 that defines the characteristics (for example, position, posture, shape, etc.) of the measurement object 62 measured at each measurement point A to H based on the environment information. To do. Further, the graph structure data generation unit 14 defines the relative positional relationship between the environment node 34 and the sensor node 30 at the position where the environment information that defines the environment node 34 is measured, based on the environment information and the sensor position information. Generate the environment edge 36. The environment edge 36 represents a posture constraint between the sensor node 30 and the environment node 34 defined by the environmental information measured from the position of the sensor node 30. Specifically, the environment edge 36 can be represented by using a known method represented by a covariance matrix of measurement errors and a relative position / orientation conversion matrix.

具体的には、図５に示すように、グラフ構造データ生成部１４は、計測地点Ｂにおいてセンサ５２によって計測された環境情報から、環境ノード３４ａを生成する。環境ノード３４ａは、計測地点Ｂから計測された計測物６２ａの特徴（例えば、位置、姿勢及び形状等）を規定している。また、グラフ構造データ生成部１４は、計測地点Ｂにおける環境情報及びセンサ位置情報から、センサノード３０ｂと環境ノード３４ａの間に環境エッジ３６ａを生成する。環境エッジ３６ａは、環境ノード３４ａとセンサノード３０ｂの相対的位置関係を規定している。すなわち、環境エッジ３６ａは、計測地点Ｂから環境ノード３４ａを計測したという事象を表している。同様にして、グラフ構造データ生成部１４は、計測地点Ｃにおいてセンサ５２によって計測された環境情報から、環境ノード３４ｂを生成する。また、グラフ構造データ生成部１４は、計測地点Ｃにおける環境情報及びセンサ位置情報から、センサノード３０ｃと環境ノード３４ｂの間に環境エッジ３６ｂを生成する。環境エッジ３６ｂは、環境ノード３４ｂとセンサノード３０ｃとの相対的位置関係を規定しており、計測地点Ｃから環境ノード３４ｂを計測したという事象を表している。 Specifically, as shown in FIG. 5, the graph structure data generation unit 14 generates the environment node 34a from the environment information measured by the sensor 52 at the measurement point B. The environment node 34a defines the characteristics (for example, position, posture, shape, etc.) of the measurement object 62a measured from the measurement point B. Further, the graph structure data generation unit 14 generates an environment edge 36a between the sensor node 30b and the environment node 34a from the environment information and the sensor position information at the measurement point B. The environment edge 36a defines the relative positional relationship between the environment node 34a and the sensor node 30b. That is, the environment edge 36a represents an event in which the environment node 34a is measured from the measurement point B. Similarly, the graph structure data generation unit 14 generates the environment node 34b from the environment information measured by the sensor 52 at the measurement point C. Further, the graph structure data generation unit 14 generates an environment edge 36b between the sensor node 30c and the environment node 34b from the environment information and the sensor position information at the measurement point C. The environment edge 36b defines the relative positional relationship between the environment node 34b and the sensor node 30c, and represents an event in which the environment node 34b is measured from the measurement point C.

上記の説明から明らかなように、計測物６２ａは、計測地点Ｂから計測されると共に、計測地点Ｃからも計測される。しかしながら、図５に示すように、計測地点Ｂから計測した計測物６２ａの特徴を規定する環境ノード３４ａと、計測地点Ｃから計測した計測物６２ａの特徴を規定する環境ノード３４ｂが一致しないことがある。これは、計測地点Ｂからセンサ５２によって計測された環境情報の姿勢誤差等と、計測地点Ｃからセンサ５２によって計測された環境情報の姿勢誤差等の蓄積によるものである。 As is clear from the above description, the measurement object 62a is measured from the measurement point B as well as from the measurement point C. However, as shown in FIG. 5, the environment node 34a that defines the characteristics of the measurement object 62a measured from the measurement point B and the environment node 34b that defines the characteristics of the measurement object 62a measured from the measurement point C may not match. is there. This is due to the accumulation of the attitude error of the environmental information measured by the sensor 52 from the measurement point B and the attitude error of the environmental information measured by the sensor 52 from the measurement point C.

同様にして、グラフ構造データ生成部１４は、計測地点Ａから計測した計測物６２ｅの特徴を規定する環境ノード３４ｃを生成し、環境ノード３４ｃとセンサノード３０ａの相対的位置関係を規定する環境エッジ３６ｃを生成する。また、グラフ構造データ生成部１４は、計測地点Ｄから計測した計測物６２ｂの特徴を規定する環境ノード３４ｄを生成し、環境ノード３４ｄとセンサノード３０ｄの相対的位置関係を規定する環境エッジ３６ｄを生成する。さらに、グラフ構造データ生成部１４は、計測地点Ｄから計測した計測物６２ｃの特徴を規定する環境ノード３４ｅを生成し、環境ノード３４ｅとセンサノード３０ｄの相対的位置関係を規定する環境エッジ３６ｅを生成する。このようにして、グラフ構造データ生成部１４は、各計測地点から計測した計測物６２の特徴を規定する環境ノード３４を生成し、環境ノード３４とその環境ノード３４とを関連付ける環境エッジ３６を生成する。センサノード、センサエッジ、環境ノード及び環境エッジを生成する際には、グラフ構造データ内に仮の原点を設定し、仮の原点を基準に環境情報及びセンサ位置情報を座標変換すればよい。 Similarly, the graph structure data generation unit 14 generates the environment node 34c that defines the characteristics of the measurement object 62e measured from the measurement point A, and the environment edge that defines the relative positional relationship between the environment node 34c and the sensor node 30a. Generate 36c. Further, the graph structure data generation unit 14 generates an environment node 34d that defines the characteristics of the measured object 62b measured from the measurement point D, and generates an environment edge 36d that defines the relative positional relationship between the environment node 34d and the sensor node 30d. Generate. Further, the graph structure data generation unit 14 generates an environment node 34e that defines the characteristics of the measured object 62c measured from the measurement point D, and creates an environment edge 36e that defines the relative positional relationship between the environment node 34e and the sensor node 30d. Generate. In this way, the graph structure data generation unit 14 generates the environment node 34 that defines the characteristics of the measurement object 62 measured from each measurement point, and generates the environment edge 36 that associates the environment node 34 with the environment node 34. To do. When generating the sensor node, the sensor edge, the environment node, and the environment edge, a temporary origin may be set in the graph structure data, and the environment information and the sensor position information may be coordinate-converted based on the temporary origin.

グラフ構造データ生成部１４は、生成したセンサノード３０、センサエッジ３２、環境ノード３４及び環境エッジ３６から、対象領域内のグラフ構造データを生成する。また、グラフ構造データ生成部１４は、グラフ構造データ内の地点Ｉ〜Ｋに対応する地点（以下、「グラフ構造データ内の地点Ｉ〜Ｋ」ともいう）について、仮の原点を基準とする座標を算出する。地点Ｉ〜Ｋは、グラフ構造データを構成する環境ノード３４のうち、凹凸がある等の特徴的な形状となっている部分に対応する計測物６２の形状を示す地点である。すなわち、地点Ｉ〜Ｋは、計測物６２の特徴的な形状を示す地点となる。例えば、地点Ｉは、計測物６２ｅの角部と一致する地点を示しており、地点Ｊは、計測物６２ｂの角部と一致する地点を示しており、地点Ｋは、計測物６２ｃの角部と一致する地点を示している。図５に示す実施例においては、仮の原点（０，０）を基準にすると、グラフ構造データ内の地点Ｉの座標は（９，１０）となり、グラフ構造データ内の地点Ｊの座標は（５３，１０）となり、グラフ構造データ内の地点Ｋの座標は（５０，−５）となっている。 The graph structure data generation unit 14 generates graph structure data in the target area from the generated sensor node 30, sensor edge 32, environment node 34, and environment edge 36. Further, the graph structure data generation unit 14 coordinates the points corresponding to the points I to K in the graph structure data (hereinafter, also referred to as “points I to K in the graph structure data”) with reference to the temporary origin. Is calculated. Points I to K are points indicating the shape of the measurement object 62 corresponding to the portion of the environment node 34 constituting the graph structure data that has a characteristic shape such as unevenness. That is, the points I to K are points showing the characteristic shape of the measurement object 62. For example, point I indicates a point that coincides with the corner of the object 62e, point J indicates a point that coincides with the corner of the object 62b, and point K indicates the corner of the object 62c. Indicates a point that matches. In the embodiment shown in FIG. 5, the coordinates of the point I in the graph structure data are (9,10) and the coordinates of the point J in the graph structure data are (9,10) with reference to the tentative origin (0,0). 53,10), and the coordinates of the point K in the graph structure data are (50, -5).

次に、地図作成装置１０は、真値情報記憶部１６に、対象領域内の計測物６２の特徴的な形状を示す地点Ｉ〜Ｋの真値情報を記憶させる（Ｓ１４）。真値情報記憶部１６は、真値情報を測量器５６から取得する。 Next, the map creation device 10 causes the true value information storage unit 16 to store true value information of points I to K indicating the characteristic shape of the measurement object 62 in the target area (S14). The true value information storage unit 16 acquires true value information from the surveying instrument 56.

真値情報は、基本測量や公共測量に用いられる測量器５６によって計測される。測量器５６は、図示しないレーザセンサを備えており、反射板までの距離を計測することによって、反射板が設置される位置を精度良く測量する。なお、本実施例では、真値情報は、測量器５６を用いて計測しているが、このような構成に限定されない。真値情報を計測する精度が高い方法であればよく、例えば、ＡＲマーカーやＱＲコード（登録商標）等をカメラで計測してもよいし、ＧＰＳ測量や航空測量を用いてもよい。また、メジャーやコンベックス等を用いて測定してもよいし、ＣＡＤ図等の座標値を示す図面を用いてもよい。 The true value information is measured by a surveying instrument 56 used for basic surveying and public surveying. The surveying instrument 56 includes a laser sensor (not shown), and measures the position where the reflector is installed with high accuracy by measuring the distance to the reflector. In this embodiment, the true value information is measured by using the surveying instrument 56, but the true value information is not limited to such a configuration. Any method may be used as long as the accuracy of measuring the true value information is high. For example, an AR marker, a QR code (registered trademark), or the like may be measured with a camera, or GPS survey or aerial survey may be used. Further, the measurement may be performed using a measure, a convex or the like, or a drawing showing coordinate values such as a CAD diagram may be used.

測量器５６を用いて計測する際は、まず、図６に示すように、対象領域内の地点Ｉ〜Ｋに図示しない反射板をそれぞれ設置する。次いで、測量器５６を用いて、地点Ｉ〜Ｋに設置される反射板の位置（すなわち、測量器５６からの距離と方位）を計測する。そして、計測結果を用いて、グラフ構造データの仮の原点の位置と対応する位置を真値の原点として、計測地点Ｉ〜Ｋの座標を算出する。例えば、図６では、測量器５６の計測結果に基づいて算出される地点Ｉの座標は（１０，８）となり、測量器５６の計測結果に基づいて算出される地点Ｊの座標は（５５，８）となり、測量器５６の計測結果に基づいて算出される地点Ｉの座標は（５０．４，−７．３）となる。なお、本実施例では、測量器５６によって３つの地点Ｉ〜Ｋを計測しているが、測量器５６によって計測する地点の数はこれに限定されない。例えば、測量器５６を用いて３つより多くの地点を計測してもよい。真値情報記憶部１６は、測量器５６から地点Ｉ〜Ｋの真値情報を取得し記憶する。また、本実施例では、ステップＳ１４はステップＳ１２の後に行われているが、これに限定されない。ステップＳ１４は，ステップＳ１０の前に行われてもよいし、ステップＳ１０とステップＳ１２の間に行われてもよい。 When measuring using the surveying instrument 56, first, as shown in FIG. 6, reflectors (not shown) are installed at points I to K in the target area. The surveyor 56 is then used to measure the position of the reflectors installed at points I-K (ie, the distance and orientation from the surveyor 56). Then, using the measurement result, the coordinates of the measurement points I to K are calculated with the position corresponding to the position of the temporary origin of the graph structure data as the origin of the true value. For example, in FIG. 6, the coordinates of the point I calculated based on the measurement result of the surveying instrument 56 are (10, 8), and the coordinates of the point J calculated based on the measurement result of the surveying instrument 56 are (55, 8), and the coordinates of the point I calculated based on the measurement result of the surveying instrument 56 are (50.4, -7.3). In this embodiment, the surveying instrument 56 measures the three points I to K, but the number of points measured by the surveying instrument 56 is not limited to this. For example, the surveyor 56 may be used to measure more than three points. The true value information storage unit 16 acquires and stores true value information of points I to K from the surveying instrument 56. Further, in this embodiment, step S14 is performed after step S12, but is not limited thereto. Step S14 may be performed before step S10 or between steps S10 and S12.

次に、地図作成装置１０は、計測物エッジ追加部１８によって、計測物エッジ３８をグラフ構造データ内に追加する（Ｓ１６）。計測物エッジ３８は、真値情報による計測物６２の位置と、その位置に対応するグラフ構造データ内の位置とのずれを規定する。図７に示すように、計測物エッジ追加部１８は、まず、真値情報記憶部１６に記憶される地点Ｉ〜Ｋの真値の座標を、グラフ構造データ生成部１４によって生成されたグラフ構造データに追加する。次いで、計測物エッジ追加部１８は、追加された地点Ｉ〜Ｋの真値の座標と、グラフ構造データ内の地点Ｉ〜Ｋの座標に基づいて、両座標のずれを規定する計測物エッジ３８を生成する。計測物エッジ追加部１８は、生成した計測物エッジ３８をグラフ構造データに追加する。 Next, the map creation device 10 adds the measured object edge 38 to the graph structure data by the measured object edge adding unit 18 (S16). The measurement object edge 38 defines the deviation between the position of the measurement object 62 based on the true value information and the position in the graph structure data corresponding to the position. As shown in FIG. 7, the measurement object edge addition unit 18 first obtains the coordinates of the true values of points I to K stored in the true value information storage unit 16 in a graph structure generated by the graph structure data generation unit 14. Add to the data. Next, the measurement object edge addition unit 18 defines the deviation of both coordinates based on the coordinates of the true values of the added points I to K and the coordinates of the points I to K in the graph structure data. To generate. The measurement object edge addition unit 18 adds the generated measurement object edge 38 to the graph structure data.

図７では、グラフ構造データ内の地点Ｉの座標は（９，１０）となっている一方、追加した真値情報による地点Ｉの座標は（１０，８）となっている。計測物エッジ３８ａは、この両座標のずれを規定している。同様にして、計測物エッジ３８ｂは、グラフ構造データ内の地点Ｊの座標（５３，１０）と、追加した真値情報による地点Ｊの座標（５５，８）のずれを規定している。計測物エッジ３８ｃは、グラフ構造データ内の地点Ｋの座標（５０，−５）と、追加した真値情報による地点Ｋの座標（５０．４，−７．３）のずれを規定している。計測物エッジ３８を追加することによって、グラフ構造データに真値情報を取り込むことができる。 In FIG. 7, the coordinates of the point I in the graph structure data are (9,10), while the coordinates of the point I based on the added true value information are (10,8). The measurement object edge 38a defines the deviation between the two coordinates. Similarly, the measurement object edge 38b defines the deviation between the coordinates (53,10) of the point J in the graph structure data and the coordinates (55,8) of the point J due to the added true value information. The measurement object edge 38c defines the deviation between the coordinates of the point K (50, -5) in the graph structure data and the coordinates of the point K (50.4, -7.3) due to the added true value information. .. By adding the measurement object edge 38, true value information can be incorporated into the graph structure data.

次に、地図作成装置１０は、グラフ構造データ最適化部２０によって、真値情報及び計測物エッジ３８が追加されたグラフ構造データを最適化する（Ｓ１８）。具体的には、グラフ構造データ最適化部２０は、グラフ構造データ内のセンサエッジ３２、環境エッジ３６及び計測物エッジ３８に基づいて算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。誤差関数は、Ｇｒａｐｈ−ｂａｓｅｄＳＬＡＭ技術で用いられている、公知の方法を用いて生成することができ、また、最適化方法についても、最急降下法や共役勾配法等の最小二乗法等の公知の方法を用いることができる。グラフ構造データ最適化部２０は、まず、グラフ構造データ内の全てのエッジ（センサエッジ３２、環境エッジ３６及び計測物エッジ３８）から誤差関数を算出し、これら算出された誤差関数を加算し、次いで、この誤差関数の和が最小となるように最適化する。最適化されたグラフ構造データでは、真値の原点がグラフ構造データの原点となる。センサエッジ３２及び環境エッジ３６に加え、計測物エッジ３８を用いて最適化することによって、真値情報を取り込んだグラフ構造データを最適化することができる。 Next, the map creation device 10 optimizes the graph structure data to which the true value information and the measurement object edge 38 are added by the graph structure data optimization unit 20 (S18). Specifically, the graph structure data optimization unit 20 minimizes the sum of the error functions calculated based on the sensor edge 32, the environment edge 36, and the measurement object edge 38 in the graph structure data. Optimize. The error function can be generated by using a known method used in the Graph-based SLAM technique, and the optimization method is also known as a least squares method such as the steepest descent method or the conjugate gradient method. Method can be used. The graph structure data optimization unit 20 first calculates an error function from all the edges (sensor edge 32, environment edge 36, and measurement object edge 38) in the graph structure data, and adds these calculated error functions. Then, the sum of the error functions is optimized to be the minimum. In the optimized graph structure data, the origin of the true value is the origin of the graph structure data. By optimizing using the measurement object edge 38 in addition to the sensor edge 32 and the environment edge 36, the graph structure data incorporating the true value information can be optimized.

最後に、地図作成装置１０は、環境地図作成部２２によって、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を作成させる（Ｓ２０）。具体的には、環境地図作成部２２は、グラフ構造データ最適化部２０によって最適化されたグラフ構造データを構成する環境ノード３４を用いて環境地図を作成する。グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を作成することによって、環境地図全体の精度を高めることができる。 Finally, the map creation device 10 causes the environment map creation unit 22 to create an environment map based on the graph structure data optimized by the graph structure data optimization unit 20 (S20). Specifically, the environment map creation unit 22 creates an environment map using the environment nodes 34 that configure the graph structure data optimized by the graph structure data optimization unit 20. By creating an environment map based on the graph structure data optimized by the graph structure data optimization unit 20, the accuracy of the entire environment map can be improved.

本実施例の地図作成装置１０は、真値情報が規定する計測物６２の位置と、その位置に対応するグラフ構造データ内の位置とのずれを規定する計測物エッジ３８をグラフ構造データに追加する。これによって、グラフ構造データに真値情報を取込むことができる。また、真値情報が取り込まれたグラフ構造データの各エッジ３２、２６、２８に基づいて算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。これによって、環境地図全体の精度を高めることができる。 The map creation device 10 of this embodiment adds to the graph structure data the measurement object edge 38 that defines the deviation between the position of the measurement object 62 defined by the true value information and the position in the graph structure data corresponding to the position. To do. As a result, true value information can be incorporated into the graph structure data. Further, the graph structure data is optimized so that the sum of the error functions calculated based on the edges 32, 26, and 28 of the graph structure data in which the true value information is taken is minimized. This makes it possible to improve the accuracy of the entire environmental map.

上述の実施例１では、真値情報記憶部１６が記憶する真値情報は、測量器５６によって計測された対象領域内の地点Ｉ〜Ｋの座標であったが、このような構成に限定されない。例えば、真値情報記憶部１６が記憶する真値情報は、位置及び姿勢等に関する情報を含む、対象領域内の特徴的な形状部分の情報であってもよい。以下、本実施例では、真値情報として用いる位置及び姿勢等に関する情報を含む特徴的な形状部分を「マーカー」と称することがある。図２、図８及び図９を参照して、本実施例において地図作成装置１０が行う処理について説明する。なお、本実施例では、図２のステップＳ１０とステップＳ１２については実施例１と同様であるため、詳細な説明は省略する。 In the first embodiment described above, the true value information stored in the true value information storage unit 16 is the coordinates of points I to K in the target area measured by the surveying instrument 56, but is not limited to such a configuration. .. For example, the true value information stored in the true value information storage unit 16 may be information on a characteristic shape portion in the target area, including information on a position, a posture, and the like. Hereinafter, in this embodiment, a characteristic shape portion including information on a position, a posture, etc. used as true value information may be referred to as a “marker”. The processing performed by the map creating device 10 in this embodiment will be described with reference to FIGS. 2, 8 and 9. In this embodiment, steps S10 and S12 in FIG. 2 are the same as those in the first embodiment, and thus detailed description thereof will be omitted.

地図作成装置１０は、真値情報記憶部１６に、対象領域内の計測物６２が備える特徴的な形状部分（すなわち、マーカー）の真値情報を記憶させる（Ｓ１４）。真値情報記憶部１６は、真値情報を測量用レーザスキャナ５８（基本測量や公共測量に用いられる機器の一種）から取得することができる。 The map creation device 10 causes the true value information storage unit 16 to store true value information of a characteristic shape portion (that is, a marker) included in the measurement object 62 in the target area (S14). The true value information storage unit 16 can acquire true value information from a surveying laser scanner 58 (a type of equipment used for basic surveying and public surveying).

測量用レーザスキャナ５８は、図示しないレーザセンサを備えており、レーザセンサから計測物６２までの距離を計測する。測量用レーザスキャナ５８のレーザセンサが計測物６２を測量する高さは、移動体５０のセンサ５２が計測物６２を計測する高さと同一となっている。測量用レーザスキャナ５８は、計測物６２の形状を計測することができる。なお、本実施例では、真値情報は、測量用レーザスキャナ５８を用いて計測しているが、このような構成に限定されない。真値情報を座標だけでなく位置及び姿勢に関する情報を含む形状として計測することができる計測機器であれば用いることができる。 The surveying laser scanner 58 includes a laser sensor (not shown), and measures the distance from the laser sensor to the object to be measured 62. The height at which the laser sensor of the surveying laser scanner 58 measures the measured object 62 is the same as the height at which the sensor 52 of the moving body 50 measures the measured object 62. The surveying laser scanner 58 can measure the shape of the object to be measured 62. In this embodiment, the true value information is measured by using the surveying laser scanner 58, but the true value information is not limited to such a configuration. Any measuring device that can measure true value information as a shape that includes not only coordinates but also information on position and posture can be used.

図８に示すように、測量用レーザスキャナ５８によって計測された情報から、対象領域内の地点Ｉを含む計測物６２ｅの一部分の形状６４ａに関する情報（以下、マーカー６４ａともいう）、地点Ｊを含む計測物６２ｅの一部分の形状６４ｂに関する情報（以下、マーカー６４ｂともいう）、地点Ｋを含む計測物６２ｃの一部分の形状６４ｃに関する情報（以下、マーカー６４ｃともいう）を抽出する。このとき、対象領域内において、グラフ構造データの仮の原点の位置と対応する位置を真値の原点として、マーカー６４ａ〜６４ｃについて算出する。マーカー６４ａは、地点Ｉを含む計測物６２ｅの一部分の位置、姿勢及び形状等を規定しており、マーカー６４ｂは、地点Ｊを含む計測物６２ｂの一部分の位置、姿勢及び形状等を規定しており、マーカー６４ｃは、地点Ｋを含む計測物６２ｃの一部分の位置、姿勢及び形状を規定している。真値情報記憶部１６は、測量用レーザスキャナ５８からマーカー６４ａ〜６４ｃに関する情報を取得して記憶する。なお、本実施例では、測量用レーザスキャナ５８によって計測された情報から３つのマーカー６４ａ〜６４ｃを抽出しているが、これに限定されない。例えば、測量用レーザスキャナ５８によって計測された情報から３つより多くのマーカーを抽出してもよい。 As shown in FIG. 8, from the information measured by the surveying laser scanner 58, the information regarding the shape 64a of a part of the measurement object 62e including the point I in the target area (hereinafter, also referred to as the marker 64a) and the point J are included. Information on the shape 64b of a part of the measurement object 62e (hereinafter, also referred to as a marker 64b) and information on the shape 64c of a part of the measurement object 62c including the point K (hereinafter, also referred to as a marker 64c) are extracted. At this time, the markers 64a to 64c are calculated with the position corresponding to the position of the temporary origin of the graph structure data as the origin of the true value in the target area. The marker 64a defines the position, posture, shape, etc. of a part of the measurement object 62e including the point I, and the marker 64b defines the position, posture, shape, etc. of a part of the measurement object 62b including the point J. The marker 64c defines the position, orientation, and shape of a part of the measurement object 62c including the point K. The true value information storage unit 16 acquires and stores information related to the markers 64a to 64c from the surveying laser scanner 58. In this embodiment, the three markers 64a to 64c are extracted from the information measured by the surveying laser scanner 58, but the present invention is not limited to this. For example, more than three markers may be extracted from the information measured by the surveying laser scanner 58.

次に、地図作成装置１０は、計測物エッジ追加部１８によって、計測物エッジ３８をグラフ構造データ内に追加する（Ｓ１６）。計測物エッジ３８は、真値情報による計測物６２の位置と、その位置に対応するグラフ構造データ内の位置とのずれを規定する。 Next, the map creation device 10 adds the measured object edge 38 to the graph structure data by the measured object edge adding unit 18 (S16). The measurement object edge 38 defines the deviation between the position of the measurement object 62 based on the true value information and the position in the graph structure data corresponding to the position.

図９に示すように、計測物エッジ追加部１８は、真値情報記憶部１６に記憶されるマーカー６４ａ〜６４ｃに関する情報を、グラフ構造データ生成部１４によって生成されたグラフ構造データに追加する。計測物エッジ追加部１８は、追加されたマーカー６４と、グラフ構造データ内のマーカー６４に対応する部分を含む環境ノード３４に基づいて、マーカー６４と対応する環境ノード３４とのずれ（すなわち、位置と姿勢のずれ）を規定する計測物エッジ３８を生成する。具体的には、マーカー６４ａと地点Ｉを含む計測物６２ｅの特徴を規定する環境ノード３４ｃに基づいて、両者のずれを規定する計測物エッジ３８ｄを生成する。マーカー６４ｂと地点Ｊを含む計測物６２ｂの特徴を規定する環境ノード３４ｄに基づいて、両者のずれを規定する計測物エッジ３８ｅを生成する。マーカー６４ｃと地点Ｋを含む計測物６２ｃの特徴を規定する環境ノード３４ｅに基づいて、両者のずれを規定する計測物エッジ３８ｆを生成する。計測物エッジ３８は、追加されたマーカー６４と対応する環境ノード３４との間の姿勢の制約を表す情報行列が含まれる。この情報行列の算出には、パターンマッチングやＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）等の公知の方法を用いることができる。計測物エッジ追加部１８は、生成した計測物エッジ３８をグラフ構造データに追加する。計測物エッジ３８を追加することによって、グラフ構造データに真値情報を取り込むことができる。 As shown in FIG. 9, the measurement object edge addition unit 18 adds information regarding markers 64a to 64c stored in the true value information storage unit 16 to the graph structure data generated by the graph structure data generation unit 14. The measurement object edge addition unit 18 is based on the added marker 64 and the environment node 34 including the portion corresponding to the marker 64 in the graph structure data, and the deviation (that is, the position) between the marker 64 and the corresponding environment node 34. The measurement object edge 38 that defines the deviation of the posture) is generated. Specifically, the measurement object edge 38d that defines the deviation between the two is generated based on the environment node 34c that defines the characteristics of the measurement object 62e including the marker 64a and the point I. Based on the environment node 34d that defines the characteristics of the measurement object 62b including the marker 64b and the point J, the measurement object edge 38e that defines the deviation between the two is generated. Based on the environment node 34e that defines the characteristics of the measurement object 62c including the marker 64c and the point K, the measurement object edge 38f that defines the deviation between the two is generated. The measuring object edge 38 includes an information matrix representing a posture constraint between the added marker 64 and the corresponding environment node 34. For the calculation of this information matrix, a known method such as pattern matching or ICP (Iterative Closet Point) can be used. The measurement object edge addition unit 18 adds the generated measurement object edge 38 to the graph structure data. By adding the measurement object edge 38, true value information can be incorporated into the graph structure data.

次に、地図作成装置１０は、グラフ構造データ最適化部２０に、真値情報及び計測物エッジ３８が追加されたグラフ構造データを最適化させる（Ｓ１８）。そして、地図作成装置１０は、環境地図作成部２２に、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を作成させる（Ｓ２０）。なお、本実施例では、ステップＳ１８とステップＳ２０の処理については実施例１と同様であるため、詳細な説明は省略する。 Next, the map creation device 10 optimizes the graph structure data to which the true value information and the measurement object edge 38 are added to the graph structure data optimization unit 20 (S18). Then, the map creation device 10 causes the environment map creation unit 22 to create an environment map based on the graph structure data optimized by the graph structure data optimization unit 20 (S20). In this embodiment, the processes of steps S18 and S20 are the same as those in the first embodiment, and thus detailed description thereof will be omitted.

本実施例においても、地図作成装置１０は、真値情報が規定する計測物６２の一部を示すマーカー６４と、そのマーカー６４に対応する環境ノード３４とのずれを規定する計測物エッジ３８をグラフ構造データに追加することによって、グラフ構造データに真値情報を取込むことができる。また、真値情報が取り込まれたグラフ構造データの各エッジ３２、２６、２８に基づいて算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。これによって、環境地図全体の精度を高めることができる。 Also in this embodiment, the map creating device 10 has a measuring object edge 38 that defines a deviation between the marker 64 indicating a part of the measuring object 62 defined by the true value information and the environment node 34 corresponding to the marker 64. By adding to the graph structure data, the true value information can be incorporated into the graph structure data. Further, the graph structure data is optimized so that the sum of the error functions calculated based on the edges 32, 26, and 28 of the graph structure data in which the true value information is taken is minimized. This makes it possible to improve the accuracy of the entire environmental map.

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。 The technical elements described herein or in the drawings exhibit their technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the techniques illustrated in this specification or drawings achieve a plurality of objectives at the same time, and achieving one of the objectives itself has technical usefulness.

１０：地図作成装置
１２：環境計測データ記憶部
１４：グラフ構造データ生成部
１６：真値情報記憶部
１８：計測物エッジ追加部
２０：グラフ構造データ最適化部
２２：環境地図作成部
３０：センサノード
３２：センサエッジ
３４：環境ノード
３６：環境エッジ
３８：計測物エッジ
５０：移動体
５２：センサ
５４：移動体の記憶部
５６：測量器
５８：測量用レーザスキャナ
６０：移動体の移動経路
６２：計測物
６４：マーカー 10: Map creation device 12: Environmental measurement data storage unit 14: Graph structure data generation unit 16: True value information storage unit 18: Measured object edge addition unit 20: Graph structure data optimization unit 22: Environmental map creation unit 30: Sensor Node 32: Sensor edge 34: Environment node 36: Environment edge 38: Measured object edge 50: Moving object 52: Sensor 54: Moving object storage unit 56: Surveying instrument 58: Surveying laser scanner 60: Moving object moving path 62 : Measured object 64: Marker

Claims

対象領域の環境地図を作成する装置であって、
前記対象領域内の複数の計測点のそれぞれについて、センサによって計測された環境情報を記憶する環境情報記憶部と、
前記複数の計測点のそれぞれについて、計測時の前記センサの位置に関するセンサ位置情報を記憶するセンサ位置情報記憶部と、
前記環境情報及び前記センサ位置情報に基づいて、前記複数の計測点のそれぞれにおける前記センサの位置を規定するセンサノードと、隣接するセンサノード間における前記センサの相対的位置関係を規定するセンサエッジと、前記複数の計測点のそれぞれにおいて前記センサで計測された計測物の位置を規定する環境ノードと、前記環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する環境エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、
前記対象領域内に位置する前記計測物の中から選択された少なくとも１つの計測物の位置に関する真値情報を記憶する真値情報記憶部と、
前記真値情報と、その真値情報に対応する環境ノードに基づいて、前記真値情報が規定する計測物の位置と前記対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジを前記グラフ構造データに追加する計測物エッジ追加部と、
前記計測物エッジが追加された前記グラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように前記グラフ構造データを最適化するグラフ構造データ最適化部と、を備えている、地図作成装置。 A device that creates an environmental map of the target area.
An environmental information storage unit that stores environmental information measured by a sensor for each of a plurality of measurement points in the target area.
For each of the plurality of measurement points, a sensor position information storage unit that stores sensor position information regarding the position of the sensor at the time of measurement, and a sensor position information storage unit.
Based on the environmental information and the sensor position information, a sensor node that defines the position of the sensor at each of the plurality of measurement points and a sensor edge that defines the relative positional relationship of the sensor between adjacent sensor nodes. From the environment node that defines the position of the measured object measured by the sensor at each of the plurality of measurement points, and the environment edge that defines the relative positional relationship between the environment node and the sensor node corresponding to the environment node. A graph structure data generator that generates structured graph structure data, and a graph structure data generator
A true value information storage unit that stores true value information regarding the position of at least one measured object selected from the measured objects located in the target area.
Based on the true value information and the environment node corresponding to the true value information, the measurement that defines the deviation between the position of the measured object specified by the true value information and the position of the measured object specified by the corresponding environment node. A measurement object edge addition part that adds an object edge to the graph structure data,
For the graph structure data to which the measurement object edge is added, a graph structure data optimization unit that optimizes the graph structure data so that the sum of error functions calculated from each edge is minimized is provided. , Mapping device.

最適化された前記グラフ構造データ内の環境ノードを用いて環境地図を作成する環境地図作成部をさらに備える、請求項１に記載の地図作成装置。 The map creation device according to claim 1, further comprising an environment map creation unit that creates an environment map using the environment nodes in the optimized graph structure data.

前記真値情報は、測量、測定及び図面の少なくとも１つから取得される、請求項１又は２に記載の地図作成装置。 The mapping apparatus according to claim 1 or 2, wherein the true value information is obtained from at least one of surveying, measurement, and drawing.

前記真値情報記憶部は、少なくとも３つの座標で示される真値情報を記憶している、請求項１〜３にいずれか一項に記載の地図作成装置。 The map creation device according to any one of claims 1 to 3, wherein the true value information storage unit stores true value information indicated by at least three coordinates.

前記真値情報記憶部は、前記グラフ構造データ内に設定した原点に対する位置、又は、前記位置及び姿勢を含む形状で示される真値情報を記憶している、請求項１〜３にいずれか一項に記載の地図作成装置。 Any one of claims 1 to 3, wherein the true value information storage unit stores true value information indicated by a position with respect to an origin set in the graph structure data or a shape including the position and posture. The mapping device described in the section.