JP7047576B2

JP7047576B2 - Cartography device

Info

Publication number: JP7047576B2
Application number: JP2018087110A
Authority: JP
Inventors: 渚小山; 亮暢藤井
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2018-04-27
Filing date: 2018-04-27
Publication date: 2022-04-05
Anticipated expiration: 2038-04-27
Also published as: JP2019191498A

Description

本明細書に開示する技術は、対象領域の環境地図を作成する地図作成装置に関する。 The technique disclosed in the present specification relates to a cartographic device for creating an environmental map of a target area.

対象領域内を移動体に自律的に移動させる場合、対象領域内の障害物（例えば、壁等）を記憶する環境地図が必要となる。対象領域内の環境地図を作成する技術としては、例えば、Ｇｒａｐｈ－ｂａｓｅｄＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｌｉｚａｔｉｏｎａｎｄＭａｐｐｉｎｇ）技術が知られている。Ｇｒａｐｈ－ｂａｓｅｄＳＬＡＭ技術では、通常、レーザレンジファインダ（ＬＲＦ）等のセンサを搭載した移動体を対象領域内で移動させ、移動経路上に設けた複数の計測点においてセンサにより計測情報を取得する。そして、地図作成装置を用いて、複数の計測点で取得した計測情報に基づいて環境地図を作成する。すなわち、地図作成装置には、複数の計測点のそれぞれについて、センサによって計測された計測情報と、各計測点における計測時のセンサの位置を規定するセンサ位置情報が入力される。次に、入力された計測情報とセンサ位置情報に基づいて、地図作成装置は、計測点のセンサ位置やセンサによって計測された計測物（対象領域内の障害物等）の位置を規定するノードと、関連する２つのノード間の相対的位置関係を規定するエッジから構成されるグラフ構造データを生成する。地図作成装置は、複数のエッジが表す想定誤差を最小化することによってグラフ構造データを最適化する。Ｇｒａｐｈ－ｂａｓｅｄＳＬＡＭ技術を用いることによって、地図作成装置は、誤差の小さい環境地図を作成することができる。また特許文献１には、地図作成装置に関する技術が開示されている。 When autonomously moving the target area to a moving object, an environmental map that stores obstacles (for example, a wall, etc.) 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 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 measurement information is acquired by sensors at a plurality of measurement points provided on the movement path. Then, using the map creation device, an environmental map is created based on the measurement information acquired at a plurality of measurement points. That is, for each of the plurality of measurement points, the measurement information measured by the sensor and the sensor position information defining the position of the sensor at the time of measurement at each measurement point are input to the map creating device. Next, based on the input measurement 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 measurement object (obstacle, etc. in the target area) measured by the sensor. , Generates graph structure data consisting of edges that define the relative positional relationship between two related nodes. The cartographic 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 cartographic device can create an environmental map with a small error. Further, Patent Document 1 discloses a technique relating to a cartographic device.

特許第５６４７２９４号公報Japanese Patent No. 5647294

環境が変化する場合がある。例えば、工場や倉庫などにおいて、駐車されているトラックや床に仮置きされる荷物が存在する場合である。このような環境下では、グラフ構造データに誤ったエッジが追加されることで，そのエッジを用いた最適化により地図の精度が低下する。精度の低下した地図を参照した移動体は、自己位置の推定に失敗してしまう。その結果、移動体を自律的に移動させることが困難になる。本明細書は、環境が変化する場合においても、正確な地図を作成可能な技術を開示する。 The environment may change. For example, in a factory or a warehouse, there is a parked truck or luggage temporarily placed on the floor. In such an environment, the addition of an erroneous edge to the graph structure data reduces the accuracy of the map due to optimization using that edge. A moving object that refers to a map with reduced accuracy fails to estimate its own position. As a result, it becomes difficult to move the moving body autonomously. The present specification discloses a technique capable of creating an accurate map even when the environment changes.

本明細書に開示する地図作成装置の一実施形態は、対象領域の地図を作成する地図作成装置である。移動体に搭載された環境計測センサによって対象領域内の複数の計測点のそれぞれについて測定された複数の計測情報を取得する計測情報取得部であって、計測情報は、計測点の周囲に存在する複数の計測物の計測点からの相対位置または距離を示すデータである計測情報取得部を、地図作成装置は備える。複数の計測情報に基づいて、複数の部分地図を生成する部分地図生成部であって、部分地図は計測点の周囲における計測物の状態を示す局所的な地図である部分地図生成部を、地図作成装置は備える。グラフ構造データを生成するグラフ構造データ生成部を、地図作成装置は備える。グラフ構造データは、複数の計測点のそれぞれにおける環境計測センサの位置および姿勢を示すセンサノードと、部分地図の位置および姿勢を規定する環境ノードと、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する第１エッジと、を備える。複数の部分地図の各々の信頼度を算出する信頼度算出部であって、信頼度は、部分地図の確かさが高いほど高くなる指標である信頼度算出部を、地図作成装置は備える。複数の部分地図の各々について、部分地図と複数の計測情報との一致判定を行い、一致度が所定のしきい値を超えた部分地図である特定部分地図を抽出する一致判定部であって、部分地図の信頼度が高いほど所定のしきい値が小さくされる一致判定部を、地図作成装置は備える。特定部分地図に対応する環境ノードと、一致判定部にてその特定部分地図と一致判定された計測情報に対応するセンサノードとの相対的位置関係を規定する第２エッジを生成し、生成した第２エッジをグラフ構造データに追加するエッジ追加部であって、第２エッジの元になる特定部分地図の信頼度が高いほど追加する際の重み付けを大きくして第２エッジを追加するエッジ追加部を、地図作成装置は備える。第２エッジが追加されたグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化するグラフ構造データ最適化部を、地図作成装置は備える。 One embodiment of the cartographic device disclosed in the present specification is a cartographic device that creates a map of a target area. It is a measurement information acquisition unit that acquires a plurality of measurement information measured for each of a plurality of measurement points in the target area by an environment measurement sensor mounted on a moving body, and the measurement information exists around the measurement points. The map creation device includes a measurement information acquisition unit that is data indicating a relative position or distance from a measurement point of a plurality of measurement objects. A partial map generator that generates a plurality of partial maps based on a plurality of measurement information, and the partial map is a local map showing the state of the measured object around the measurement point. A creation device is provided. The map creation device includes a graph structure data generation unit that generates graph structure data. The graph structure data includes a sensor node that indicates the position and orientation of the environmental measurement sensor at each of the multiple measurement points, an environment node that defines the position and orientation of the partial map, and the environment node and the sensor node corresponding to the environment node. It includes a first edge that defines a relative positional relationship. The map creation device includes a reliability calculation unit that calculates the reliability of each of the plurality of partial maps, and the reliability is an index that increases as the certainty of the partial map increases. A match determination unit that determines a match between a partial map and a plurality of measurement information for each of a plurality of partial maps, and extracts a specific partial map that is a partial map whose degree of matching exceeds a predetermined threshold value. The map creation device includes a match determination unit in which a predetermined threshold value is reduced as the reliability of the partial map is higher. The second edge that defines the relative positional relationship between the environment node corresponding to the specific partial map and the sensor node corresponding to the measurement information that is determined to match the specific partial map by the match determination unit is generated and generated. An edge addition part that adds two edges to the graph structure data, and the higher the reliability of the specific part map that is the source of the second edge, the greater the weight when adding, and the edge addition part that adds the second edge. The map making device is provided. The map creation device includes a graph structure data optimization unit that optimizes the graph structure data so that the sum of the error functions calculated from each edge is minimized for the graph structure data to which the second edge is added.

上記の地図作成装置では、複数の部分地図の各々の信頼度を算出することができる。そして、移動体で測定された計測情報に一致する特定部分地図を、信頼度に基づいて抽出することができる。抽出された特定部分地図について作成したエッジを、グラフ構造データに追加することができる。このとき、エッジの元になる特定部分地図の信頼度が高いほど、追加する際の重み付けを大きくすることができる。これにより、環境が変化する場合においても、移動体によって測定された計測情報に基づいて正確な地図を生成することができる。 In the above-mentioned cartography apparatus, the reliability of each of a plurality of partial maps can be calculated. Then, a specific partial map that matches the measurement information measured by the moving body can be extracted based on the reliability. The edges created for the extracted specific partial map can be added to the graph structure data. At this time, the higher the reliability of the specific partial map that is the source of the edge, the larger the weight at the time of addition can be increased. As a result, even when the environment changes, an accurate map can be generated based on the measurement information measured by the moving body.

信頼度算出部が算出する信頼度は、部分地図生成部によって部分地図が生成された時刻から、当該部分地図と一致判定される計測情報が計測情報取得部によって取得された時刻までの経過時間の絶対値が大きくなるほど単調減少してもよい。効果の詳細は実施例で説明する。 The reliability calculated by the reliability calculation unit is the elapsed time from the time when the partial map is generated by the partial map generation unit to the time when the measurement information determined to match the partial map is acquired by the measurement information acquisition unit. It may decrease monotonically as the absolute value increases. Details of the effect will be described in Examples.

部分地図は、複数のグリッドで表されていてもよい。グリッドが物体で占有されているか否かが、複数のグリッドの各々について占有確率で示されていてもよい。信頼度算出部が算出する信頼度は、予め定められた２値化しきい値と占有確率との差の絶対値を複数のグリッドの各々について求めた場合に、差の絶対値の総和が大きくなるほど高くなってもよい。２値化しきい値は、占有確率から、物体がグリッドで占有されているか否かに２値化するための値であってもよい。効果の詳細は実施例で説明する。 The partial map may be represented by a plurality of grids. Whether or not the grid is occupied by an object may be indicated by the occupation probability for each of the plurality of grids. The reliability calculated by the reliability calculation unit increases as the sum of the absolute values of the differences increases when the absolute value of the difference between the predetermined binarization threshold and the occupancy probability is obtained for each of the plurality of grids. It may be higher. The binarization threshold value may be a value for binarizing from the occupancy probability to whether or not the object is occupied by the grid. Details of the effect will be described in Examples.

対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれていてもよい。信頼度算出部は、定常領域に対応している部分地図の信頼度を、定常領域に対応していない部分地図の信頼度よりも高くしてもよい。効果の詳細は実施例で説明する。 The target region may include a stationary region in which the number, position, and shape of the measured objects do not change over time. The reliability calculation unit may make the reliability of the partial map corresponding to the stationary region higher than the reliability of the partial map not corresponding to the stationary region. Details of the effect will be described in Examples.

対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれていてもよい。部分地図は、複数のグリッドで表されていてもよい。グリッドが物体で占有されているか否かが、複数のグリッドの各々について占有確率で示されていてもよい。信頼度算出部が定常領域に対応している部分地図の信頼度を算出する場合には、定常領域に対応している部分地図の信頼度を、定常領域に対応していない部分地図の信頼度よりも高くし、予め定められた２値化しきい値と占有確率との差の絶対値が小さくなるほど信頼度を低くしてもよい。２値化しきい値は、占有確率から、物体がグリッドで占有されているか否かに２値化するための値であってもよい。差の絶対値は、複数のグリッドの各々について求められてもよい。信頼度算出部が定常領域に対応していない部分地図の信頼度を算出する場合には、部分地図生成部によって部分地図が生成された時刻から、計測情報取得部によって当該部分地図と一致判定を行う計測情報が取得された時刻までの経過時間の絶対値が大きくなるほど信頼度を単調減少させ、予め定められた２値化しきい値と占有確率との差の絶対値が小さくなるほど信頼度を低くしてもよい。効果の詳細は実施例で説明する。 The target region may include a stationary region in which the number, position, and shape of the measured objects do not change over time. The partial map may be represented by a plurality of grids. Whether or not the grid is occupied by an object may be indicated by the occupation probability for each of the plurality of grids. When the reliability calculation unit calculates the reliability of the partial map corresponding to the stationary area, the reliability of the partial map corresponding to the stationary area is used as the reliability of the partial map not corresponding to the stationary area. The reliability may be lowered as the absolute value of the difference between the predetermined binarization threshold value and the occupancy probability becomes smaller. The binarization threshold value may be a value for binarizing from the occupancy probability to whether or not the object is occupied by the grid. The absolute value of the difference may be determined for each of the plurality of grids. When the reliability calculation unit calculates the reliability of a partial map that does not correspond to the stationary region, the measurement information acquisition unit determines the match with the partial map from the time when the partial map is generated by the partial map generation unit. The reliability decreases monotonically as the absolute value of the elapsed time until the time when the measurement information to be performed is acquired increases, and the reliability decreases as the absolute value of the difference between the predetermined binarization threshold and the occupancy probability decreases. You may. Details of the effect will be described in Examples.

第１エッジおよび第２エッジは、環境ノードとその環境ノードに対応するセンサノードとの相対位置・姿勢および計測誤差の共分散行列を表していてもよい。エッジ追加部は、第２エッジが表す計測誤差の共分散行列を信頼度が大きいほど小さく設定してもよい。効果の詳細は実施例で説明する。 The first edge and the second edge may represent a covariance matrix of relative positions / attitudes and measurement errors between the environment node and the sensor node corresponding to the environment node. The edge addition unit may set the covariance matrix of the measurement error represented by the second edge to be smaller as the reliability is larger. Details of the effect will be described in Examples.

地図作成装置はネットワーク上に配置されたサーバであってもよい。地図作成装置は、移動体からネットワークを介して複数の計測情報を受信可能であってもよい。効果の詳細は実施例で説明する。 The cartographic device may be a server located on the network. The cartographic device may be able to receive a plurality of measurement information from the mobile body via the network. Details of the effect will be described in Examples.

計測情報取得部は、時刻ｔにおいてｉ番目（ｉは２以上の自然数）の計測点で測定された計測物の位置と、時刻ｔ－１においてｉ－１番目の計測点で測定された計測物の位置とを比較した場合に、時刻ｔにおける位置と時刻ｔ－１における位置とが一致している計測物の位置のみを示す情報を、時刻ｔにおける計測情報として受信してもよい。効果の詳細は実施例で説明する。 The measurement information acquisition unit is the position of the measured object measured at the i-th (i is a natural number of 2 or more) measurement point at time t and the measured object measured at the i-1th measurement point at time t-1. Information indicating only the position of the measured object whose position at time t and the position at time t-1 coincide with each other when compared with the position at time t may be received as the measurement information at time t. Details of the effect will be described in Examples.

実施例１に係る地図生成システムの構成を示す図。The figure which shows the structure of the map generation system which concerns on Example 1. 全体地図の具体例を示す図。The figure which shows the concrete example of the whole map. 部分地図の具体例を示す図。The figure which shows the concrete example of a partial map. グラフ構造データの具体例を示す図。The figure which shows the specific example of the graph structure data. 初期地図生成モードの具体的な処理を示すフローチャート。A flowchart showing the specific processing of the initial map generation mode. 地図生成・更新処理の具体的な処理を示すフローチャート。A flowchart showing a specific process of map generation / update process. グラフ構造データの具体例を示す図。The figure which shows the specific example of the graph structure data. 単調減少する関数の例を示す図。The figure which shows the example of the function which decreases monotonically. 部分地図の内部状態を示す図。The figure which shows the internal state of a partial map. 信頼度の算出のための第３の方法の具体例を示す図。The figure which shows the specific example of the 3rd method for the calculation of reliability. グラフ構造データの具体例を示す図。The figure which shows the specific example of the graph structure data. 地図生成処理の具体的な処理を示すフローチャート。A flowchart showing a specific process of map generation processing. 実施例２に係る地図生成システムの構成を示す図。The figure which shows the structure of the map generation system which concerns on Example 2. フィルタ処理の具体的な内容を示すフローチャート。A flowchart showing the specific contents of the filtering process. フィルタ処理の一例を説明する図。The figure explaining an example of the filtering process.

（地図生成システム１の構成）
図１のブロック図を用いて、地図生成システム１の構成を説明する。地図生成システム１は、地図作成装置１０、ロボット１１および１２を備えている。地図生成システム１は、「初期地図生成モード」で用いるシステムである。「初期地図生成モード」では、ロボット１１および１２を操作し、ロボット１１および１２から得る計測情報に基づき、地図作成装置１０を用いて全体地図を生成する。なお、「初期地図生成モード」で生成された全体地図を用いてロボットの自律制御を行う、「運用モード」も存在する。しかし本明細書では、「運用モード」の説明については省略する。 (Configuration of map generation system 1)
The configuration of the map generation system 1 will be described with reference to the block diagram of FIG. The map generation system 1 includes a map creation device 10, robots 11 and 12. The map generation system 1 is a system used in the "initial map generation mode". In the "initial map generation mode", the robots 11 and 12 are operated, and the entire map is generated by using the map creation device 10 based on the measurement information obtained from the robots 11 and 12. There is also an "operation mode" in which the robot is autonomously controlled using the entire map generated in the "initial map generation mode". However, in this specification, the description of the "operation mode" is omitted.

（ロボット１１および１２の構成）
ロボット１１および１２は、センサ４０、計測情報取得部４１、データ送信部４２、移動制御部４３を備える。ロボット１１および１２は対象領域内を移動しながら、センサ４０によって対象領域内の環境（例えば、壁等の計測物）を計測する。これにより計測情報取得部４１は、センサ４０から計測情報を取得する。センサ４０の一例としては、LiDAR、カメラ、RADAR、超音波センサ等が挙げられる。計測情報の一例としては、センサ４０で測定される相対位置情報・距離情報や、ＩＭＵ（Inertial Measurement Unit）やエンコーダ等により得られるロボットの速度情報・角速度情報等が挙げられる。また、ロボット１１および１２は、車輪の回転角を検出する不図示のセンサ（例えば、エンコーダ）を備えている。このセンサによって車輪の回転角を検出することで、ロボット１１および１２の移動方向及び移動量を算出することができる。よって、各計測地点におけるロボット１１および１２の姿勢及び位置を算出することができる。 (Structure of robots 11 and 12)
The robots 11 and 12 include a sensor 40, a measurement information acquisition unit 41, a data transmission unit 42, and a movement control unit 43. The robots 11 and 12 measure the environment (for example, a measurement object such as a wall) in the target area by the sensor 40 while moving in the target area. As a result, the measurement information acquisition unit 41 acquires measurement information from the sensor 40. Examples of the sensor 40 include LiDAR, a camera, RADAR, an ultrasonic sensor, and the like. Examples of the measurement information include relative position information / distance information measured by the sensor 40, speed information / angular velocity information of the robot obtained by an IMU (Inertial Measurement Unit), an encoder, or the like. Further, the robots 11 and 12 include a sensor (for example, an encoder) (not shown) that detects the rotation angle of the wheel. By detecting the rotation angle of the wheel by this sensor, the moving direction and the moving amount of the robots 11 and 12 can be calculated. Therefore, the postures and positions of the robots 11 and 12 at each measurement point can be calculated.

データ送信部４２は、計測情報取得部４１で取得した計測情報に、ロボットを識別するためのロボットＩＤを付与して地図作成装置１０に送信する。移動制御部４３は、外部コントローラの操作に基づき、自身の移動を制御する。 The data transmission unit 42 assigns a robot ID for identifying the robot to the measurement information acquired by the measurement information acquisition unit 41 and transmits the measurement information to the map creation device 10. The movement control unit 43 controls its own movement based on the operation of the external controller.

（地図作成装置１０の構成）
地図作成装置１０は、対象領域の地図を作成する装置である。地図作成装置１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、地図作成装置１０は、図１に示す計測情報取得部２１～初期地図構成要素群書出部３２等として機能する。 (Configuration of map creation device 10)
The map creation device 10 is a device that creates a map of the target area. The map creating 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 creating device 10 functions as the measurement information acquisition unit 21 to the initial map component group writing unit 32 and the like shown in FIG.

計測情報取得部２１は、ロボットＩＤの付与された計測情報を受信する。計測情報取得部２１は、ロボットＩＤの付与された計測情報をロボット１１および１２から受信する。そして、センサ４０の位置と共に、計測情報を記憶する。 The measurement information acquisition unit 21 receives the measurement information to which the robot ID is assigned. The measurement information acquisition unit 21 receives the measurement information to which the robot ID is assigned from the robots 11 and 12. Then, the measurement information is stored together with the position of the sensor 40.

部分地図生成部２２は、計測情報をロボットＩＤ毎に所定の間隔で蓄積して部分地図を生成する。また、蓄積の過程で得られる部分地図とロボットとの相対位置・姿勢を、グラフ構造データ生成部２７へ出力する。部分地図記憶部２３は、部分地図生成部２２が作成した部分地図を記憶する。信頼度算出部２４は、部分地図記憶部２３中の部分地図について、その信頼度を算出する。 The partial map generation unit 22 accumulates measurement information for each robot ID at predetermined intervals to generate a partial map. Further, the relative position / posture between the partial map obtained in the accumulation process and the robot is output to the graph structure data generation unit 27. The partial map storage unit 23 stores the partial map created by the partial map generation unit 22. The reliability calculation unit 24 calculates the reliability of the partial map in the partial map storage unit 23.

一致判定部２５は、部分地図記憶部中の部分地図と計測情報との一致判定を行う。そして、一致度が所定のしきい値を超えた部分地図を抽出する。エッジ追加部２６は、一致判定部２５で抽出された部分地図に関するエッジを生成し追加する。 The match determination unit 25 determines a match between the partial map in the partial map storage unit and the measurement information. Then, a partial map whose degree of matching exceeds a predetermined threshold value is extracted. The edge addition unit 26 generates and adds an edge related to the partial map extracted by the match determination unit 25.

グラフ構造データ生成部２７は、ノード（頂点）とエッジ（枝）で構成されるグラフ構造データを生成する。ノードは、ロボット１１および１２の位置・姿勢、部分地図の位置・姿勢を表す情報である。エッジは、部分地図とロボット１１および１２との相対位置・姿勢と、その計測誤差を表す情報である。 The graph structure data generation unit 27 generates graph structure data composed of nodes (vertices) and edges (branches). The node is information representing the position / posture of the robots 11 and 12 and the position / posture of the partial map. The edge is information representing the relative positions / postures of the partial map and the robots 11 and 12 and their measurement errors.

グラフ最適化部２８は、グラフ構造データを最適化する。グラフ記憶部２９は、グラフ構造データを保存する。地図生成部３０は、部分地図および信頼度に基づき、全体地図を生成する。全体地図記憶部３１は、地図生成部３０で生成された全体地図を保存する。初期地図構成要素群書出部３２は、部分地図記憶部２３、グラフ記憶部２９、全体地図記憶部３１の内容を、読込可能な形式で書き出す。 The graph optimization unit 28 optimizes the graph structure data. The graph storage unit 29 stores graph structure data. The map generation unit 30 generates an entire map based on the partial map and the reliability. The overall map storage unit 31 stores the entire map generated by the map generation unit 30. The initial map component group writing unit 32 writes out the contents of the partial map storage unit 23, the graph storage unit 29, and the entire map storage unit 31 in a readable format.

（地図の構成要素の説明）
地図の構成要素には、全体地図、部分地図、グラフ構造データが含まれる。これらの構成要素について説明する。全体地図は、地図作成装置１０にて最終的に出力する情報である。出力された全体地図は、ロボットの経路計画や、ロボットの自己位置推定に利用される。全体地図の具体例を図２に示す。全体地図はグリッド表現されている。すなわち、複数のグリッドに区分けされている。各グリッドは、「１」または「０」によって、そのグリッドが物体に占有されているか否かを表現している。図２では、ハッチングで示されているグリッドが「１」であり、物体に占有されていることを示している。また白色で示されているグリッドが「０」であり、物体に占有されていないことを示している。全体地図には、１つの固定の座標系である、全体地図座標系ＥＣ１が設定されている。全体地図の生成方法は後述する。 (Explanation of map components)
Map components include full maps, partial maps, and graph structure data. These components will be described. The whole map is information finally output by the map creating device 10. The output overall map is used for robot route planning and robot self-position estimation. A specific example of the whole map is shown in FIG. The whole map is represented by a grid. That is, it is divided into a plurality of grids. Each grid is represented by "1" or "0" whether or not the grid is occupied by an object. In FIG. 2, the grid shown by hatching is "1", indicating that it is occupied by an object. Further, the grid shown in white is "0", indicating that the grid is not occupied by an object. In the whole map, one fixed coordinate system, the whole map coordinate system EC1, is set. The method of generating the entire map will be described later.

部分地図は、地図作成装置１０にて中間情報として生成される情報である。部分地図は、環境の計測情報から取得した特徴量として用いられる。部分地図の具体例を図３に示す。部分地図は、ロボットが一定間隔（例えば、一定移動距離）で生成する局所的な地図である。すなわち部分地図は、複数の計測点の各々について、計測点の周囲における計測物の状態を示している。部分地図は、全体地図と同様にグリッド表現されている。グリッドの数や部分地図の形状は、部分地図により異なっていてもよい。部分地図は、それぞれ固有の座標系である部分地図座標系ＰＣ１～ＰＣ４を備えている。部分地図座標系の方向は、部分地図により異なっている。部分地図の生成方法は後述する。 The partial map is information generated as intermediate information by the map creating device 10. The partial map is used as a feature amount acquired from the measurement information of the environment. A specific example of the partial map is shown in FIG. A partial map is a local map generated by a robot at regular intervals (for example, a constant travel distance). That is, the partial map shows the state of the measured object around the measurement points for each of the plurality of measurement points. The partial map is represented by a grid like the whole map. The number of grids and the shape of the partial map may differ depending on the partial map. The partial map includes partial map coordinate systems PC1 to PC4, which are unique coordinate systems. The direction of the partial map coordinate system differs depending on the partial map. The method of generating the partial map will be described later.

グラフ構造データは、地図作成装置１０にて中間情報として生成される情報である。グラフ構造データは、ロボットの走行軌跡の誤差を抑制するために用いられる。グラフ構造データの具体例を図４に示す。グラフ構造データは、センサノードＳＮ１～ＳＮ４、環境ノードＥＮ１～ＥＮ３、エッジＥ１～Ｅ６を備えている。センサノードＳＮ１～ＳＮ４は、ロボット１１が備えるセンサ４０の、複数の計測点のそれぞれにおける位置および姿勢を示す情報である。環境ノードＥＮ１～ＥＮ３は、複数の計測点のそれぞれにおける、センサ４０から得られた計測情報を蓄積して生成した部分地図の位置および姿勢を規定する情報である。エッジＥ１～Ｅ６は、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する情報である。具体的には、エッジＥ１～Ｅ６は、環境ノードとその環境ノードに対応するセンサノードとの相対位置・姿勢、および、計測誤差の共分散行列を表している。センサノードと環境ノードは、全結合するわけではない。また、処理の過程でマッチングされたロボットと部分地図との関係が、グラフ構造データに逐次追加されていく。グラフ構造データの生成方法は後述する。 The graph structure data is information generated as intermediate information by the map creating device 10. The graph structure data is used to suppress an error in the traveling locus of the robot. A specific example of the graph structure data is shown in FIG. The graph structure data includes sensor nodes SN1 to SN4, environment nodes EN1 to EN3, and edges E1 to E6. The sensor nodes SN1 to SN4 are information indicating the positions and postures of the sensors 40 included in the robot 11 at each of the plurality of measurement points. The environment nodes EN1 to EN3 are information that defines the position and orientation of the partial map generated by accumulating the measurement information obtained from the sensor 40 at each of the plurality of measurement points. The edges E1 to E6 are information that defines the relative positional relationship between the environment node and the sensor node corresponding to the environment node. Specifically, the edges E1 to E6 represent the relative position / orientation of the environment node and the sensor node corresponding to the environment node, and the covariance matrix of the measurement error. The sensor node and the environment node are not fully connected. In addition, the relationship between the matched robot and the partial map in the process is sequentially added to the graph structure data. The method of generating graph structure data will be described later.

（初期地図生成モードの動作）
図５のフローチャートを基に、「初期地図生成モード」の具体的な処理について説明する。Ｓ１０において、ロボット１１および１２が送信する計測情報を受信する、計測情報受信処理を行う。この処理は、計測情報取得部２１にて実施する。 (Operation of initial map generation mode)
The specific processing of the "initial map generation mode" will be described with reference to the flowchart of FIG. In S10, the measurement information reception process for receiving the measurement information transmitted by the robots 11 and 12 is performed. This process is performed by the measurement information acquisition unit 21.

Ｓ１５において、地図生成・更新処理を行う。処理の詳細は、図６を用いて後述する。Ｓ２０において、地図生成・更新処理の終了指示が入力されたか否かを判断する。否定判断される場合（Ｓ２０：ＮＯ）にはＳ１０へ戻り、処理を続行する。一方、肯定判断される場合（Ｓ２０：ＹＥＳ）にはＳ２５へ進む。 In S15, map generation / update processing is performed. Details of the processing will be described later with reference to FIG. In S20, it is determined whether or not the end instruction of the map generation / update process has been input. If a negative judgment is made (S20: NO), the process returns to S10 and the process is continued. On the other hand, if affirmative judgment is made (S20: YES), the process proceeds to S25.

Ｓ２５において、初期地図構成要素群の書出処理を行う。この処理は、初期地図構成要素群書出部３２にて実施する。具体的には、部分地図記憶部２３、グラフ記憶部２９、全体地図記憶部３１の記憶内容を保存する。 In S25, the initial map component group is written out. This process is performed by the initial map component group writing unit 32. Specifically, the stored contents of the partial map storage unit 23, the graph storage unit 29, and the entire map storage unit 31 are stored.

（地図生成・更新処理の内容（Ｓ１５））
Ｓ１５の地図生成・更新処理の詳細内容について、図６のフローチャートを用いて説明する。Ｓ１００において、ロボットの現在位置および現在姿勢の概算処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。概算したロボットの現在位置および現在姿勢は、Ｓ１０５のスキャンマッチ処理の初期値として用いられる。ロボットの位置・姿勢の概算には、過去２地点の位置・姿勢から外挿しても良い。またロボットの速度や各情報を参照し、概算しても良い。 (Details of map generation / update processing (S15))
The detailed contents of the map generation / update process of S15 will be described with reference to the flowchart of FIG. In S100, the robot's current position and current posture are estimated. This process is performed by the partial map generation unit 22 for each of the robots 11 and 12. The estimated current position and current posture of the robot are used as initial values of the scan match process of S105. For the estimation of the position / posture of the robot, the position / posture of the past two points may be extrapolated. You may also refer to the speed of the robot and each information to make an approximation.

Ｓ１０５において、スキャンマッチ処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。スキャンマッチ処理では、ロボットが時刻ｔ－１にて生成中の部分地図と、時刻ｔの計測情報とのマッチングを行い、時刻tでのロボットの位置・姿勢を出力する。具体的には、スキャンマッチ初期値算出処理（Ｓ１００）により得たロボットの位置・姿勢を初期値として、ロボットの計測情報と生成中の部分地図との誤差の総和が最小となるようなロボットの位置・姿勢を求める。また、マッチングの過程で算出する部分地図とロボットとの相対位置・姿勢と、その計測誤差共分散を出力する。 In S105, scan match processing is performed. This process is performed by the partial map generation unit 22 for each of the robots 11 and 12. In the scan match process, the partial map generated by the robot at time t-1 is matched with the measurement information at time t, and the position and posture of the robot at time t are output. Specifically, the robot's position / posture obtained by the scan match initial value calculation process (S100) is used as the initial value, and the total error between the robot's measurement information and the generated partial map is minimized. Find the position and posture. In addition, the relative position / attitude between the partial map and the robot calculated in the matching process and the measurement error covariance are output.

Ｓ１１０において、部分地図生成処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。部分地図生成処理では、スキャンマッチ処理（Ｓ１０５）により得られた部分地図座標系でのロボットの位置・姿勢を用いて時刻tの計測情報を部分地図に蓄積することで、生成中の部分地図を更新する。一定数の計測情報を蓄積すると，部分地図の生成を完了し、新規に部分地図の生成を開始する。新規に生成を開始する部分地図の位置・姿勢は、時刻tでのロボットの位置・姿勢としてもよい。生成を完了した部分地図は、部分地図記憶部２３に記憶される。 In S110, the partial map generation process is performed. This process is performed by the partial map generation unit 22 for each of the robots 11 and 12. In the partial map generation process, the measured information at time t is accumulated in the partial map using the position and orientation of the robot in the partial map coordinate system obtained by the scan match process (S105), and the partial map being generated is stored. Update. When a certain amount of measurement information is accumulated, the generation of the partial map is completed and the generation of a new partial map is started. The position / posture of the partial map to be newly generated may be the position / posture of the robot at time t. The partially generated partial map is stored in the partial map storage unit 23.

Ｓ１１５において、グラフ構造データ生成処理を行う。具体的には、スキャンマッチ処理（Ｓ１０５）の結果と、部分地図生成処理（Ｓ１１０）の結果に基づき、グラフ構造データの更新を行う。この処理は、グラフ構造データ生成部２７にて、ロボット１１および１２の各々について実施する。処理の具体例を、図４および図７を用いて説明する。なお、図４および図７では，重なりを避けるために環境ノードとセンサノードをずらして描画している。図４のグラフ構造データに対して、新規に生成した部分地図の位置および姿勢を環境ノードＥＮ４として追加するとともに、ロボットの最新の位置および姿勢を、センサノードＳＮ５として追加する。これにより、グラフ構造データは図７の状態になる。また、ロボットが生成中の部分地図に対する、ロボットの相対位置・姿勢をエッジＥ７として追加する。エッジＥ７には、スキャンマッチにより得た相対位置・姿勢・計測誤差共分散を加える。 In S115, graph structure data generation processing is performed. Specifically, the graph structure data is updated based on the result of the scan match process (S105) and the result of the partial map generation process (S110). This process is performed by the graph structure data generation unit 27 for each of the robots 11 and 12. Specific examples of the processing will be described with reference to FIGS. 4 and 7. In FIGS. 4 and 7, the environment node and the sensor node are shifted and drawn in order to avoid overlap. To the graph structure data of FIG. 4, the position and posture of the newly generated partial map are added as the environment node EN4, and the latest position and posture of the robot are added as the sensor node SN5. As a result, the graph structure data is in the state shown in FIG. Further, the relative position / posture of the robot with respect to the partial map being generated by the robot is added as the edge E7. The relative position / orientation / measurement error covariance obtained by the scan match is added to the edge E7.

Ｓ１２０において、部分地図の信頼度算出処理を行う。この処理は、信頼度算出部２４にて、ロボット１１および１２の各々について、また、部分地図記憶部２３内の複数の部分地図の各々について実施する。信頼度は、部分地図の確からしさを示す指標である。信頼度は、部分地図の確からしさが高くなるほど高くなってもよい。 In S120, the reliability calculation process of the partial map is performed. This process is performed by the reliability calculation unit 24 for each of the robots 11 and 12, and for each of the plurality of partial maps in the partial map storage unit 23. Reliability is an index showing the certainty of a partial map. The reliability may increase as the certainty of the partial map increases.

信頼度の算出方法の具体例としては、以下の３つの方法が挙げられる。第１の方法は、部分地図生成処理（Ｓ１１０）によって部分地図が生成された時刻から、計測情報受信処理（Ｓ１０）によって当該部分地図との一致判定を試みる計測情報が取得された時刻までの経過時間の絶対値が大きくなるほど、信頼度を単調減少させる方法である。具体的には、時間変数の増加に伴い単調減少する関数を用いることで、信頼度を算出することができる。図８に、単調減少する関数の例を示す。横軸（経過時間ｔ）は、部分地図が生成された時刻から計測情報が取得された時刻までの経過時間の絶対値である。信頼度ｃは、例えば、「ｃ＝－ａｔ＋ｂ」という、係数ａが負である１次関数Ｆ１で表すことができる。また例えば、「ｃ＝－ｄｔ^２＋ｂ」という、係数ｄが負である二次関数Ｆ２で表すことができる。また例えば、「ｃ＝ｆ^ｔ＋ｂ－１」という、係数を正とする指数関数Ｆ３で表すことができる。これにより、部分地図が生成された時刻から、その部分地図と一致判定を試みる計測情報が取得された時刻までの経過時間の絶対値が増加するに伴って、部分地図の信頼度ｃを低下させることができる。 Specific examples of the reliability calculation method include the following three methods. The first method is the elapsed time from the time when the partial map is generated by the partial map generation process (S110) to the time when the measurement information for attempting to determine the match with the partial map is acquired by the measurement information reception process (S10). The larger the absolute value of time, the more monotonously the reliability is reduced. Specifically, the reliability can be calculated by using a function that monotonically decreases as the time variable increases. FIG. 8 shows an example of a monotonically decreasing function. The horizontal axis (elapsed time t) is the absolute value of the elapsed time from the time when the partial map is generated to the time when the measurement information is acquired. The reliability c can be expressed by, for example, "c = -at + b", which is a linear function F1 in which the coefficient a is negative. Further, for example, it can be expressed by a quadratic function F2 in which the coefficient d is negative, that is, “c = −dt ² + b”. Further, for example, it can be expressed by an exponential function F3 having a positive coefficient, that is, "c ^{= ft} + b-1". As a result, the reliability c of the partial map decreases as the absolute value of the elapsed time from the time when the partial map is generated to the time when the measurement information that attempts to match the partial map is acquired increases. be able to.

信頼度の算出方法の第２の方法は、部分地図の確率的な確からしさに基づいて算出する方法である。具体例を図９（Ａ）および図９（Ｂ）に示す。部分地図の内部状態は、複数のグリッドで表されている。複数のグリッドの各々が物体で占有されているか否かが、占有確率で示されている。占有確率は、占有されている状態を「１」、占有されていない状態を「０」とし、その間の状態を数値で確率的に示したものである。図９（Ａ）および図９（Ｂ）では、占有確率は、ハッチングの密度が高いほど大きいとする。図９（Ａ）の部分地図ＰＭ１は、図９（Ｂ）の部分地図ＰＭ２に比して、占有確率を示すハッチング密度のコントラスト差が、各グリッド間で大きい。すなわち、部分地図ＰＭ１の方が、部分地図ＰＭ２よりも「確からしさ」が高いと言える。第２の方法では、このような「確からしさ」の大きい部分地図ほど信頼度を上げる。 The second method of calculating the reliability is a method of calculating based on the probabilistic certainty of the partial map. Specific examples are shown in FIGS. 9 (A) and 9 (B). The internal state of the partial map is represented by multiple grids. Whether or not each of the plurality of grids is occupied by an object is indicated by the occupation probability. The occupancy probability is a state in which the occupied state is "1", the unoccupied state is "0", and the state in between is shown stochastically numerically. In FIGS. 9 (A) and 9 (B), it is assumed that the higher the hatch density, the higher the occupancy probability. The partial map PM1 of FIG. 9A has a larger contrast difference of the hatching densities indicating the occupancy probability between the grids than the partial map PM2 of FIG. 9B. That is, it can be said that the partial map PM1 has a higher "certainty" than the partial map PM2. In the second method, the reliability of such a partial map with a large "certainty" is increased.

信頼度の算出方法の一例を説明する。「２値化しきい値」を予め定める。２値化しきい値は、０～１の間の数値をとる占有確率を、物体がグリッドで占有されているか否かを示す「１」または「０」に２値化するための値である。２値化しきい値は、例えば「０．５」に設定してもよい。予め定められた２値化しきい値と占有確率との「差の絶対値」を、複数のグリッドの各々について求める。信頼度は、この「差の絶対値」が大きくなるほど高くなる。
具体的には、下式（１）によって求めることができる。

ここで、ｃは部分地図の信頼度である。ｌ_ｉｊはグリッド（ｉ，ｊ）の内部状態である。ｌ_ｔｈは２値化しきい値である。ｎは部分地図内のグリッドの総数である。αは係数である。 An example of the reliability calculation method will be described. A "binarization threshold" is set in advance. The binarization threshold is a value for binarizing the occupancy probability, which takes a numerical value between 0 and 1, to "1" or "0" indicating whether or not the object is occupied by the grid. The binarization threshold may be set to, for example, "0.5". The "absolute value of the difference" between the predetermined binarization threshold and the occupancy probability is obtained for each of the plurality of grids. The reliability increases as this "absolute value of difference" increases.
Specifically, it can be obtained by the following equation (1).

Here, c is the reliability of the partial map. l _ij is the internal state of the grid (i, j). _lth is a binarization threshold. n is the total number of grids in the partial map. α is a coefficient.

信頼度の算出のための第３の方法は、信頼度が高くなる領域を予め認識しておく方法である。例えば、時間が経過しても計測物の数、位置、形状が変化しない定常領域に対応する部分地図の信頼度を、定常領域に対応しない部分地図よりも高く設定する。定常領域の具体例としては、物体設置が禁止された領域が挙げられる。具体例を図１０に示す。図１０において、定常領域Ｒ１内の部分地図ＰＭ１２およびＰＭ１３の信頼度を、定常領域Ｒ１に含まれない部分地図ＰＭ１１、ＰＭ１４、ＰＭ１５の信頼度よりも高くする。 The third method for calculating the reliability is a method of recognizing in advance a region where the reliability is high. For example, the reliability of the partial map corresponding to the stationary region in which the number, position, and shape of the measured objects do not change over time is set higher than that of the partial map not corresponding to the stationary region. Specific examples of the stationary region include regions where object installation is prohibited. A specific example is shown in FIG. In FIG. 10, the reliability of the partial maps PM12 and PM13 in the constant region R1 is made higher than the reliability of the partial maps PM11, PM14, and PM15 not included in the constant region R1.

信頼度の算出のための第１～第３の方法の組み合わせ例を説明する。例えば、定常領域に少なくとも一部が含まれている部分地図の信頼度を算出する場合には、第２および第３の方法を組み合わせて使用してもよい。一方、定常領域に含まれていない部分地図の信頼度を算出する場合には、第１および第２の方法を組み合わせて使用してもよい。なお、１つの部分地図に対して複数の信頼度が算出された場合には、最小値や最大値を選択してもよいし、複数の信頼度の積や平均値を算出してもよい。 An example of a combination of the first to third methods for calculating the reliability will be described. For example, when calculating the reliability of a partial map in which at least a part is included in the constant region, the second and third methods may be used in combination. On the other hand, when calculating the reliability of the partial map not included in the stationary region, the first and second methods may be used in combination. When a plurality of reliabilitys are calculated for one partial map, the minimum value or the maximum value may be selected, or the product or the average value of the plurality of reliabilitys may be calculated.

Ｓ１２５において、部分地図と計測情報との一致判定処理を行う。この処理は、一致判定部２５にて、ロボット１１および１２の各々について実施する。また、部分地図記憶部２３内の部分地図ごとに実施する。 In S125, a match determination process between the partial map and the measurement information is performed. This process is performed by the match determination unit 25 for each of the robots 11 and 12. In addition, it is carried out for each partial map in the partial map storage unit 23.

Ｓ１２５の一致判定処理の具体的内容は、Ｓ１０５のスキャンマッチ処理と同様である。すなわち、複数の部分地図の各々について、部分地図と計測情報との一致判定（スキャンマッチ）を行う。そして、マッチング度合を示す指標（スコア）が所定のしきい値を超えた部分地図である特定部分地図を抽出する。このとき、所定のしきい値は、部分地図の信頼度が高いほど小さくする。これにより、信頼度の低い部分地図が計測情報と一致すると判断されてしまい、抽出されてしまう事態を防止することができる。 The specific content of the match determination process of S125 is the same as that of the scan match process of S105. That is, for each of the plurality of partial maps, a match determination (scan match) between the partial map and the measurement information is performed. Then, a specific partial map, which is a partial map whose index (score) indicating the degree of matching exceeds a predetermined threshold value, is extracted. At this time, the predetermined threshold value is reduced as the reliability of the partial map is higher. As a result, it is possible to prevent a situation in which a partial map with low reliability is determined to match the measurement information and is extracted.

式（２）に、一致判定処理で用いられる数式の具体例を示す。
τ_ｌ＝η／Ｃ_ｌ・・・式（２）
ここで、τ_ｌは部分地図ｌの判定閾値、Ｃ_ｌは部分地図ｌの信頼度、ηは定数である。 Equation (2) shows a specific example of the mathematical expression used in the match determination process.
τ _l = η / C _l ... Equation (2)
Here, τ _l is the determination threshold value of the partial map _l , Cl is the reliability of the partial map l, and η is a constant.

Ｓ１３０において、一致判定に成功したか否かが判断される。否定判断される場合（Ｓ１３０：ＮＯ）にはＳ１４０へ進み、肯定判断される場合（Ｓ１３０：ＹＥＳ）にはＳ１３５へ進む。 In S130, it is determined whether or not the match determination is successful. If a negative judgment is made (S130: NO), the process proceeds to S140, and if a positive judgment is made (S130: YES), the process proceeds to S135.

Ｓ１３５において、エッジ追加処理を行う。この処理は、一致判定処理（Ｓ１２５）の結果に基づいてグラフ構造データの更新を行うための処理である。具体的には、Ｓ１２５で抽出された特定部分地図に対応する環境ノードと、その特定部分地図と一致判定された計測情報に対応するセンサノードとの間にエッジ（制約）を生成する。この処理は、エッジ追加部２６にて、ロボット１１および１２の各々について実施する。また、部分地図記憶部２３内の部分地図ごとに実施する。 In S135, edge addition processing is performed. This process is a process for updating the graph structure data based on the result of the match determination process (S125). Specifically, an edge (constraint) is generated between the environment node corresponding to the specific partial map extracted in S125 and the sensor node corresponding to the measurement information determined to match the specific partial map. This process is performed for each of the robots 11 and 12 by the edge addition unit 26. In addition, it is carried out for each partial map in the partial map storage unit 23.

図７および図１１を用いて、エッジ追加処理を具体的に説明する。図１１に示すように、過去の部分地図である環境ノードＥＮ３と、最新のロボット位置・姿勢であるセンサノードＳＮ５の相対的位置関係を規定するエッジＥ８を追加生成する。これにより、グラフ構造データを図７の状態から図１１の状態にすることができる。追加生成したエッジＥ８には、エッジＥ８の元となる特定部分地図に対する当該ロボットの相対位置・姿勢と、計測誤差の共分散行列を設定する。このとき、エッジＥ８が表す計測誤差の共分散行列を、部分地図の信頼度が大きいほど小さく設定する。これにより、エッジＥ８の元になる特定部分地図の信頼度が高いほど、エッジＥ８をグラフ構造データに追加する際の重み付けを大きくすることができる。換言すると、特定部分地図の信頼度が高いほど、エッジＥ８の寄与度が大きくなるように調整することができる。 The edge addition process will be specifically described with reference to FIGS. 7 and 11. As shown in FIG. 11, an edge E8 that defines the relative positional relationship between the environment node EN3, which is a partial map of the past, and the sensor node SN5, which is the latest robot position / attitude, is additionally generated. As a result, the graph structure data can be changed from the state of FIG. 7 to the state of FIG. In the additionally generated edge E8, the relative position / posture of the robot with respect to the specific partial map that is the source of the edge E8 and the covariance matrix of the measurement error are set. At this time, the covariance matrix of the measurement error represented by the edge E8 is set smaller as the reliability of the partial map is larger. As a result, the higher the reliability of the specific partial map that is the source of the edge E8, the greater the weighting when adding the edge E8 to the graph structure data. In other words, the higher the reliability of the specific partial map, the larger the contribution of the edge E8 can be adjusted.

式（３）に、共分散行列の具体例を示す。

ここで、左辺のΣはロボット位置ｓと過去の部分地図ｌの間に制約として設定する計測誤差共分散である。右辺のΣはロボット位置ｓと部分地図ｌとのスキャンマッチによる誤差共分散である。ｃ_ｌは部分地図ｓの信頼度である。εは定数である。 Equation (3) shows a specific example of the covariance matrix.

Here, Σ on the left side is a measurement error covariance set as a constraint between the robot position s and the past partial map l. Σ on the right side is the error covariance due to the scan match between the robot position s and the partial map l. _cl is the reliability of the partial map s. ε is a constant.

Ｓ１４０において、全ての部分地図を処理したか否かを判断する。否定判断される場合（Ｓ１４０：ＮＯ）にはＳ１４５へ進み、次の部分地図が選択される。そしてＳ１２０へ戻る。一方、肯定判断される場合（Ｓ１４０：ＹＥＳ）には、Ｓ１５０へ進む。 In S140, it is determined whether or not all the partial maps have been processed. If a negative judgment is made (S140: NO), the process proceeds to S145, and the next partial map is selected. Then, it returns to S120. On the other hand, if a positive judgment is made (S140: YES), the process proceeds to S150.

Ｓ１５０において、全てのロボットについて処理を実行したか否かを判断する。否定判断される場合（Ｓ１５０：ＮＯ）にはＳ１５５へ進み、次のロボットが選択される。そしてＳ１００へ戻る。一方、肯定判断される場合（Ｓ１５０：ＹＥＳ）にはＳ１６０へ進む。 In S150, it is determined whether or not the processing has been executed for all the robots. If a negative judgment is made (S150: NO), the process proceeds to S155, and the next robot is selected. Then return to S100. On the other hand, if an affirmative judgment is made (S150: YES), the process proceeds to S160.

Ｓ１６０において、グラフ最適化処理を行う。この処理は、グラフ最適化部２８にて実施する。グラフ最適化では、グラフ構造データ全体で、エッジが表す制約（すなわち、相対位置・姿勢と誤差共分散）から誤差量を表す誤差関数を作成する。そして、各エッジの誤差関数の和が最小となるように、各ノードの位置・姿勢を補正する。そして、最適化後のグラフ構造データをグラフ最適化部２８記憶部に保存する。誤差関数としては、各エッジに対する計測誤差の総和を用いることができる。計測誤差の一例としては、エッジ両端のノード間の相対位置・姿勢と、エッジに設定された相対位置・姿勢との誤差をエッジの計測誤差共分散の逆行列で重み付けしたもの（マハラノビス距離）が挙げられる。 In S160, graph optimization processing is performed. This process is performed by the graph optimization unit 28. In graph optimization, an error function representing the amount of error is created from the constraints represented by the edges (that is, relative position / orientation and error covariance) in the entire graph structure data. Then, the position / orientation of each node is corrected so that the sum of the error functions of each edge is minimized. Then, the graph structure data after optimization is stored in the graph optimization unit 28 storage unit. As the error function, the sum of the measurement errors for each edge can be used. As an example of measurement error, the error between the relative position / orientation between the nodes at both ends of the edge and the relative position / orientation set on the edge is weighted by the inverse matrix of the edge measurement error covariance (Mahalanobis distance). Can be mentioned.

Ｓ１６５において、地図生成処理を行う。この処理は、地図生成部３０にて実施する。図１２のフローチャートを用いて、地図生成処理の具体的な内容を説明する。Ｓ２００において、部分地図記憶部２３のすべての部分地図を、その位置・姿勢に基づき配置する。また、全ての部分地図の外接矩形から、全体地図のサイズを調整する。Ｓ２０５において、全体地図の各グリッドのうちのｉ＝１番目のグリッドを選択する。Ｓ２１０において、選択されたｉ番目のグリッドと重なる部分地図の数を算出する。Ｓ２１５において、選択されたｉ番目のグリッドの内部状態を、部分地図の信頼度の重み付けによって算出する。例えば下式（４）によって算出できる。

ここで、Ｌ_ｉは全体地図のｉ番目のグリッドの内部状態である。Ｎ_ｉは全体地図のｉ番目のグリッドと重なる部分地図の数である。ｃ_ｊはｊ番目の部分地図の信頼度である。ｌ_ｊはｊ番目の部分地図の内部状態である。 In S165, a map generation process is performed. This process is performed by the map generation unit 30. The specific contents of the map generation process will be described with reference to the flowchart of FIG. In S200, all the partial maps of the partial map storage unit 23 are arranged based on their positions and postures. Also, adjust the size of the entire map from the circumscribed rectangles of all the partial maps. In S205, i = 1st grid in each grid of the whole map is selected. In S210, the number of partial maps overlapping the selected i-th grid is calculated. In S215, the internal state of the selected i-th grid is calculated by weighting the reliability of the partial map. For example, it can be calculated by the following equation (4).

Here, Li is the internal state of the _i -th grid of the entire map. Ni is the number of partial maps that overlap with the _i -th grid of the entire map. c _j is the reliability of the j-th partial map. l _j is the internal state of the j-th partial map.

Ｓ２２０の内部状態の閾値処理において、全体地図のｉ番目のグリッドの占有状態を「０」または「１」で出力する。Ｓ２２５において、全てのグリッドについて処理を実行したか否かが判断される。肯定判断される場合（Ｓ２２５：ＹＥＳ）にはフローを終了する。否定判断される場合（Ｓ２２５：ＮＯ）にはＳ２０３へ進み、次のｉ＋１番目のグリッドを選択した上で、Ｓ２１０へ戻る。 In the threshold processing of the internal state of S220, the occupied state of the i-th grid of the entire map is output as "0" or "1". In S225, it is determined whether or not the processing has been executed for all the grids. If affirmative judgment is made (S225: YES), the flow is terminated. If a negative judgment is made (S225: NO), the process proceeds to S203, the next i + 1th grid is selected, and then the process returns to S210.

（効果）
ロボットの正しい位置・姿勢（走行軌跡）が分かれば、その位置・姿勢に対応する計測情報を重ねてグリッド化することで、正確な地図をロボットを用いて生成することができる。しかし実際には、ロボットの走行軌跡の推定値には誤差が生じるため、ロボットの走行軌跡を最適化する必要がある。最適化の方法として、部分地図を特徴量（環境の局所的な特徴を表現する情報）としたGraph-based SLAMがある。しかし、特徴量である部分地図は、時間経過に伴う環境の変化（例：倉庫の床に置かれる荷物の位置や数の変化）に従ってその信頼度が変化する。そのため、信頼度の低い部分地図を用いた最適化処理が発生してしまい、ロボットの走行軌跡の誤差が増大してしまう場合があった。そこで本実施形態の地図作成装置１０では、複数の部分地図の各々の信頼度を算出することができる（Ｓ１２０）。そして、移動体に搭載された環境計測センサで測定された計測情報に一致する特定部分地図を、信頼度に基づいて抽出することができる（Ｓ１２５）。抽出された特定部分地図について作成したエッジを、グラフ構造データに追加することができる（Ｓ１３５）。このとき、エッジの元になる特定部分地図の信頼度が高いほど、追加する際の重み付けを大きくすることができる。これにより、状態が変化する環境下においても、移動体によって測定された計測情報に基づいて正確な地図を生成することが可能となる。 (effect)
Once the correct position / posture (traveling locus) of the robot is known, an accurate map can be generated using the robot by superimposing the measurement information corresponding to the position / posture into a grid. However, in reality, since an error occurs in the estimated value of the traveling locus of the robot, it is necessary to optimize the traveling locus of the robot. As an optimization method, there is Graph-based SLAM using a partial map as a feature quantity (information expressing local features of the environment). However, the reliability of the partial map, which is a feature quantity, changes according to changes in the environment over time (eg, changes in the position and number of luggage placed on the floor of the warehouse). Therefore, the optimization process using the partial map with low reliability may occur, and the error of the traveling locus of the robot may increase. Therefore, in the map creating device 10 of the present embodiment, the reliability of each of the plurality of partial maps can be calculated (S120). Then, a specific partial map that matches the measurement information measured by the environmental measurement sensor mounted on the moving body can be extracted based on the reliability (S125). The edges created for the extracted specific partial map can be added to the graph structure data (S135). At this time, the higher the reliability of the specific partial map that is the source of the edge, the larger the weight at the time of addition can be increased. This makes it possible to generate an accurate map based on the measurement information measured by the moving object even in an environment where the state changes.

実施例１の地図生成システム１は、ロボットによって作成された部分地図の良否を、信頼度を用いて判断するシステムであった。実施例２の地図生成システム１ａは、計測情報のうちから不要な情報を取り除くことで、精度よく部分地図を作成する機能をさらに備えたシステムである。図１３に、実施例２の地図生成システム１ａのブロック図を示す。実施例２の地図生成システム１ａ（図１３）と、実施例１の地図生成システム１（図１）とで、共通する部位には共通の符号を付している。また実施例２に特有の部位には、符号の末尾に「ａ」を付している。 The map generation system 1 of the first embodiment was a system for judging the quality of a partial map created by a robot by using the reliability. The map generation system 1a of the second embodiment is a system further provided with a function of accurately creating a partial map by removing unnecessary information from the measurement information. FIG. 13 shows a block diagram of the map generation system 1a of the second embodiment. The map generation system 1a (FIG. 13) of the second embodiment and the map generation system 1 (FIG. 1) of the first embodiment have a common reference numeral. Further, "a" is added to the end of the reference numeral to the portion peculiar to the second embodiment.

実施例２の地図生成システム１ａは、ロボット１１ａおよび１２ａを備えている。ロボット１１ａおよび１２ａは、フィルタ部４４ａをさらに備えている。計測情報取得部４１から出力された計測情報は、フィルタ部４４ａを介してデータ送信部４２へ入力される。 The map generation system 1a of the second embodiment includes robots 11a and 12a. The robots 11a and 12a further include a filter unit 44a. The measurement information output from the measurement information acquisition unit 41 is input to the data transmission unit 42 via the filter unit 44a.

図１４のフローチャートを用いて、フィルタ部４４ａで行われるフィルタ処理を説明する。Ｓ３００において、時刻ｔ－１と時刻ｔの計測情報を位置合わせする。Ｓ３０３において、ｉ＝１番目の計測情報を選択する。Ｓ３０５において、時刻ｔ－１の計測情報と時刻ｔの計測情報の位置が一致するか否かを判断する。肯定判断される場合（Ｓ３０５：ＹＥＳ）にはＳ３１５へ進み、選択された計測情報をデータ送信部４２へ送信する。そしてＳ３２０へ進む。一方、否定判断される場合（Ｓ３０５：ＮＯ）には、Ｓ３１５をスキップしてＳ３２０へ進む。 The filter processing performed by the filter unit 44a will be described with reference to the flowchart of FIG. In S300, the measurement information at time t-1 and time t are aligned. In S303, i = first measurement information is selected. In S305, it is determined whether or not the positions of the measurement information at time t-1 and the measurement information at time t match. If affirmative judgment is made (S305: YES), the process proceeds to S315, and the selected measurement information is transmitted to the data transmission unit 42. Then proceed to S320. On the other hand, if a negative determination is made (S305: NO), S315 is skipped and the process proceeds to S320.

Ｓ３２０において、全ての計測情報について処理を実行したか否かが判断される。肯定判断される場合（Ｓ３２０：ＹＥＳ）にはフローを終了する。否定判断される場合（Ｓ３２０：ＮＯ）にはＳ３２５へ進み、次のｉ＋１番目の計測情報を選択した上で、Ｓ３０５へ戻る。 In S320, it is determined whether or not the processing has been executed for all the measurement information. If affirmative judgment is made (S320: YES), the flow is terminated. If a negative judgment is made (S320: NO), the process proceeds to S325, the next i + 1th measurement information is selected, and then the process returns to S305.

図１５の計測情報の例を用いて、フィルタ処理の内容を説明する。図１５では、時刻ｔ－１および時刻ｔの各々におけるロボット１１ａの位置を、位置Ｐ１１およびＰ１２とする。時刻ｔ－１および時刻ｔの各々における壁の計測情報を、ＭＤ１１およびＭＤ１２とする。時刻ｔ－１および時刻ｔの各々における歩行者の脚の計測情報を、ＭＤ２１およびＭＤ２２とする。時刻ｔにおいて検出開始された走行中のフォークリフトの計測情報を、ＭＤ３２とする。図１５において、時刻ｔ－１の計測情報は破線で示し、時刻ｔの計測情報は実線で示している。計測情報ＭＤ１１およびＭＤ１２は壁の計測情報であるため、時刻ｔ－１と時刻ｔとで位置が一致する（Ｓ３０５：ＹＥＳ）。よって、計測情報ＭＤ１２はデータ送信部４２へ送信される（Ｓ３１５）。一方、計測情報ＭＤ２１とＭＤ２２は、歩行者の脚の計測情報であるため、時刻ｔ－１と時刻ｔとで位置が一致しない（Ｓ３０５：ＮＯ）。また計測情報ＭＤ３２は、時刻ｔで検出開始されている。従って、計測情報ＭＤ２２およびＭＤ３２は、データ送信部４２へ送信されない。 The content of the filter processing will be described with reference to the example of the measurement information of FIG. In FIG. 15, the positions of the robot 11a at each of time t-1 and time t are defined as positions P11 and P12. The measurement information of the wall at each of the time t-1 and the time t is MD11 and MD12. The measurement information of the pedestrian's leg at each of the time t-1 and the time t is MD21 and MD22. The measurement information of the running forklift that started detection at time t is set to MD32. In FIG. 15, the measurement information at time t-1 is shown by a broken line, and the measurement information at time t is shown by a solid line. Since the measurement information MD11 and MD12 are wall measurement information, the positions match at time t-1 and time t (S305: YES). Therefore, the measurement information MD 12 is transmitted to the data transmission unit 42 (S315). On the other hand, since the measurement information MD21 and MD22 are the measurement information of the pedestrian's leg, the positions do not match between the time t-1 and the time t (S305: NO). Further, the measurement information MD 32 is started to be detected at time t. Therefore, the measurement information MD22 and MD32 are not transmitted to the data transmission unit 42.

（効果）
実施例１において部分地図を生成するために行うスキャンマッチ処理（Ｓ１０５）は、静的な環境を前提にした技術である。移動する物体（例：作業者やフォークリフトなど）が存在する動的な環境では、過去の計測情報とのマッチングが適切に一致しない場合がある。すると、特徴量とする部分地図が正確に生成されなくなる結果、全体地図の精度が悪化してしまう場合がある。そこで実施例２に係る地図生成システム１ａでは、時刻ｔおよび時刻ｔ－１において測定された計測情報が示す計測物の位置を比較し（Ｓ３０５）、両者が一致している計測情報のみを地図作成装置１０へ送信する（Ｓ３１５）。これにより、複数の計測情報の中から移動する物体（動的物体）を計測したデータを除き、静的物体（例：壁、柱など）および準静的物体（例：停止中のトラックなど）を計測したデータのみを地図作成装置１０に送信することができる。全体地図の精度を向上させることが可能となる。 (effect)
The scan match process (S105) performed to generate a partial map in the first embodiment is a technique premised on a static environment. In a dynamic environment with moving objects (eg workers, forklifts, etc.), matching with past measurement information may not match properly. Then, as a result of not being able to accurately generate the partial map as the feature amount, the accuracy of the entire map may deteriorate. Therefore, in the map generation system 1a according to the second embodiment, the positions of the measured objects indicated by the measurement information measured at the time t and the time t-1 are compared (S305), and only the measurement information in which the two match is created as a map. It is transmitted to the device 10 (S315). As a result, static objects (eg walls, pillars, etc.) and quasi-static objects (eg, stopped trucks, etc.) are excluded from the data obtained by measuring moving objects (dynamic objects) from multiple measurement information. Only the measured data can be transmitted to the map creating device 10. It is possible to improve the accuracy of the entire map.

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。 Although the examples of the techniques disclosed in the present specification have been described in detail above, these are merely examples and do not limit the scope of claims. The techniques described in the claims include various modifications and modifications of the specific examples exemplified above.

（変形例）
地図作成装置１０はコンピュータに限られず、例えば、インターネットなどのネットワーク上に配置されたサーバであってもよい。地図作成装置１０は、ネットワークを介して、ロボット１１および１２から計測情報を受信してもよい。地図作成装置１０の汎用性を高めることができる。 (Modification example)
The map creating device 10 is not limited to a computer, and may be, for example, a server arranged on a network such as the Internet. The cartographic device 10 may receive measurement information from the robots 11 and 12 via a network. The versatility of the map creating device 10 can be increased.

地図生成システム１は、ロボット１１および１２の２台の移動体を備える場合を説明したが、この形態に限られない。３台以上のロボットを備える場合においても、本明細書の技術を適用可能である。 The case where the map generation system 1 includes two mobile bodies of the robots 11 and 12 has been described, but the map generation system 1 is not limited to this form. The techniques of this specification can be applied even when three or more robots are provided.

式（１）～式（４）は一例である。本実施例の技術では、様々なタイプの数式を使用することが可能である。 Equations (1) to (4) are examples. In the technique of this embodiment, it is possible to use various types of mathematical formulas.

実施例２の地図生成システム１ａにおいて、フィルタ部４４ａの配置位置は自由に設定できる。例えば、地図作成装置１０において、計測情報取得部２１の前後に配置してもよい。また、実施例２の地図生成システム１ａでは、全体地図の精度を向上させることができるため、信頼度を用いなくてもよい。すなわち実施例２では、信頼度算出部２４を省略してもよい。また、一致判定部２５およびエッジ追加部２６では、信頼度を用いた処理を実行しなくてもよい。 In the map generation system 1a of the second embodiment, the arrangement position of the filter unit 44a can be freely set. For example, in the map creating device 10, it may be arranged before and after the measurement information acquisition unit 21. Further, in the map generation system 1a of the second embodiment, since the accuracy of the entire map can be improved, it is not necessary to use the reliability. That is, in the second embodiment, the reliability calculation unit 24 may be omitted. Further, the match determination unit 25 and the edge addition unit 26 do not have to execute the process using the reliability.

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。 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 exemplified in the present specification or the drawings achieve a plurality of purposes at the same time, and achieving one of the purposes itself has technical usefulness.

ロボット１１、１２は、移動体の一例である。センサ４０は、環境計測センサの一例である。グラフ最適化部２８は、グラフ構造データ最適化部の一例である。 Robots 11 and 12 are examples of moving objects. The sensor 40 is an example of an environment measurement sensor. The graph optimization unit 28 is an example of a graph structure data optimization unit.

１：地図生成システム１０：地図作成装置１１、１２：ロボット２１：計測情報取得部２２：部分地図生成部２４：信頼度算出部２５：一致判定部２６：エッジ追加部２７：グラフ構造データ生成部２８：グラフ最適化部４０：センサ
1: Map generation system 10: Map creation device 11, 12: Robot 21: Measurement information acquisition unit 22: Partial map generation unit 24: Reliability calculation unit 25: Match determination unit 26: Edge addition unit 27: Graph structure data generation unit 28: Graph optimization unit 40: Sensor

Claims

It is a cartography device that creates an entire map of the target area.
It is a measurement information acquisition unit that acquires a plurality of measurement information measured for each of a plurality of measurement points in the target area by an environment measurement sensor mounted on a moving body, and the measurement information is around the measurement point. The measurement information acquisition unit, which is data indicating the relative position or distance from the measurement point of a plurality of measurement objects existing in the above.
A partial map generation unit that generates a plurality of partial maps based on the plurality of the measurement information, wherein the partial map is a local map showing the state of the measurement object around the measurement point. Map generator and
A graph structure data generation unit that generates graph structure data, wherein the graph structure data includes a sensor node indicating the position and orientation of the environment measurement sensor at each of the plurality of measurement points, and the position and orientation of the partial map. The graph structure data generation unit comprising the environment node that defines the environment node and the first edge that defines the relative positional relationship between the environment node and the sensor node corresponding to the environment node.
A reliability calculation unit that calculates the reliability of each of the plurality of partial maps, and the reliability is an index that increases as the certainty of the partial map increases.
For each of the plurality of the partial maps, a match determination unit that determines a match between the partial map and the plurality of measurement information and extracts a specific partial map that is a partial map whose degree of matching exceeds a predetermined threshold value. Therefore, the higher the reliability of the partial map, the smaller the predetermined threshold value.
A second edge that defines the relative positional relationship between the environment node corresponding to the specific partial map and the sensor node corresponding to the measurement information matched with the specific partial map by the matching determination unit is generated. , The edge addition part that adds the generated second edge to the graph structure data, and the higher the reliability of the specific partial map that is the source of the second edge, the larger the weight when adding the second edge. With the edge addition part that adds two edges,
A graph structure data optimization unit that optimizes the graph structure data so that the sum of the error functions calculated from each edge is minimized for the graph structure data to which the second edge is added.
Generate the whole map by arranging each of the plurality of the partial maps based on the position and orientation of the partial map defined by the environment node provided in the optimized graph structure data. Map generator and
A cartography device equipped with.

The reliability calculated by the reliability calculation unit is such that the measurement information determined to match the partial map is acquired by the measurement information acquisition unit from the time when the partial map is generated by the partial map generation unit. The cartographic device according to claim 1, wherein the map monotonically decreases as the absolute value of the elapsed time until the time increases.

The partial map is represented by multiple grids.
Whether or not the grid is occupied by an object is indicated by the occupation probability for each of the plurality of grids.
The reliability calculated by the reliability calculation unit is the absolute value of the difference when the absolute value of the difference between the predetermined binarization threshold value and the occupancy probability is obtained for each of the plurality of grids. The larger the total of, the higher it becomes.
The cartographic device according to claim 1, wherein the binarization threshold value is a value for binarizing from the occupancy probability to whether or not the object is occupied by the grid.

The target region includes a stationary region in which the number, position, and shape of the measured objects do not change over time.
The cartographic device according to claim 1, wherein the reliability calculation unit makes the reliability of the partial map corresponding to the stationary region higher than the reliability of the partial map not corresponding to the stationary region.

The target region includes a stationary region in which the number, position, and shape of the measured objects do not change over time.
The partial map is represented by multiple grids.
Whether or not the grid is occupied by an object is indicated by the occupation probability for each of the plurality of grids.
When the reliability calculation unit calculates the reliability of the partial map corresponding to the stationary region,
The reliability of the partial map corresponding to the constant region is made higher than the reliability of the partial map not corresponding to the constant region.
The smaller the absolute value of the difference between the predetermined binarization threshold and the occupancy probability, the lower the reliability.
The binarization threshold value is a value for binarizing from the occupancy probability to whether or not the object is occupied by the grid.
The absolute value of the difference is determined for each of the plurality of grids.
When the reliability calculation unit calculates the reliability of a partial map that does not correspond to the stationary region,
The reliability becomes monotonous as the elapsed time from the time when the partial map is generated by the partial map generation unit to the time when the measurement information for which the measurement information acquisition unit performs a match determination with the partial map is acquired increases. Reduce,
The cartographic device according to claim 1, wherein the reliability is lowered as the absolute value of the difference between the predetermined binarization threshold value and the occupancy probability becomes smaller.

The first edge and the second edge represent a covariance matrix of relative positions / attitudes and measurement errors between the environment node and the sensor node corresponding to the environment node.
The cartographic device according to any one of claims 1 to 5, wherein the edge addition unit sets the covariance matrix of the measurement error represented by the second edge to be smaller as the reliability is larger.

The cartographic device is a server located on the network.
The cartographic device according to any one of claims 1 to 6, wherein a plurality of the measurement information can be received from the mobile body via the network.

The measurement information acquisition unit was measured at the position of the measurement object measured at the i-th measurement point (i is a natural number of 2 or more) at time t and at the i-1th measurement point at time t-1. When the position of the measured object is compared, information indicating only the position of the measured object whose position at the time t and the position at the time t-1 coincides is received as the measurement information at the time t. The map making device according to any one of claims 1 to 7.