JP2008165275A

JP2008165275A - Mobile body with self-position identification device

Info

Publication number: JP2008165275A
Application number: JP2006350740A
Authority: JP
Inventors: Yasuo Kishi; 泰生岸
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 2006-12-27
Filing date: 2006-12-27
Publication date: 2008-07-17

Abstract

<P>PROBLEM TO BE SOLVED: To provide a self-position identification device of a mobile body capable of reducing a processing cost of landmark information necessary for self-position identification even a target environment is in a large scale and also flexibly dealing with changes in the environment. <P>SOLUTION: The mobile body with the self-position identification device includes a local map and a global map, and stores a landmark candidate and a landmark collation result in the local map to use for the next landmark collation. Further, while newly registering a landmark candidate without a corresponding landmark in the collation process, the mobile body deletes an already registered landmark that has not been observed for a long term. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

本発明は、環境を観測しながら移動する移動体が環境中における自己位置を逐次同定するための自己位置同定装置に関する。 The present invention relates to a self-position identification device for a mobile body that moves while observing the environment to sequentially identify the self-position in the environment.

本発明における自己位置同定装置とは、移動体に備えた外界センサおよび内界センサの観測結果にもとづき、環境中に定めた基準点に対する移動体の絶対的な位置を逐次同定するものである。 The self-position identification device according to the present invention sequentially identifies the absolute position of the moving body with respect to a reference point determined in the environment based on the observation results of the external sensor and the internal sensor provided in the moving body.

特許文献１には、移動体の自己位置同定のための基本的な構成が示されている。図３はその自己位置同定システムの全体構成を示すブロック図であって、図中３０１はカメラで得られる画像などからランドマークを抽出するランドマーク認識部、３０２はランドマーク認識部３０１から随時得られるランドマーク認識結果を用いてランドマークの列をランドマーク間の位置関係とともに求め、前記移動体の移動経緯を示す観測地図を作成・更新する観測地図作成部、３０３は観測地図情報を保存する観測地図情報保存部、３０４は移動体の概略の移動量を内界センサで獲得する移動量算出部、３０５は走行距離等により走行経路中の移動体概略位置を求める大局位置算出部、３０６は実際の環境中のランドマークを予め情報として収集してデータ化した地図データを保持する地図データ記憶部、３０７は地図データ記憶部３０６から大局位置算出部３０５により得られた概略位置周辺のデータを切り出す地図データ管理部、３０８は地図データ管理部より送られてきた地図データと観測地図とを照合することにより走行経路中の移動体の自己位置を同定する照合部である。特許文献１の自己位置同定システムは複数のランドマーク列をつなぎ合わせた観測地図と地図データとを照合することで自己位置を同定するものであって、この点が特徴として主張されている。 Patent Document 1 shows a basic configuration for self-position identification of a moving body. FIG. 3 is a block diagram showing the overall configuration of the self-position identification system, in which 301 is a landmark recognition unit that extracts a landmark from an image obtained by a camera, and 302 is obtained from the landmark recognition unit 301 as needed. An observation map creation unit 303 obtains a row of landmarks together with a positional relationship between the landmarks using a landmark recognition result, and creates / updates an observation map indicating the movement history of the moving object, and 303 stores observation map information Observation map information storage unit 304, a movement amount calculation unit 304 that obtains an approximate movement amount of the moving object with an internal sensor, 305 a global position calculation unit that obtains the approximate position of the moving object in the travel route based on the travel distance, etc. 306 A map data storage unit that stores map data obtained by collecting landmarks in the actual environment as information in advance, and 307 is a map data storage unit A map data management unit that extracts data around the approximate position obtained by the global position calculation unit 305 from 06, and 308 is a movement in the travel route by comparing the map data sent from the map data management unit with the observation map It is a collation part which identifies the self position of a body. The self-position identification system of Patent Document 1 identifies a self-position by collating an observation map obtained by connecting a plurality of landmark rows with map data, and this point is claimed as a feature.

特許文献２では、ランドマークの存在する環境中の移動ロボット装置が複数の登録済みの環境から現在の環境を同定するための環境同定装置が開示されている。図４はそのブロック図であって、図中４０１は前記ロボット装置が現在おかれている環境のランドマーク情報を有する環境地図を構築する環境地図構築部、４０２は環境内のランドマークの位置情報とその環境に一意に割り振られる環境ＩＤを一つの組として、複数の環境地図及びその環境ＩＤからなるデータベースを有する環境地図記憶部、４０３は環境地図構築部４０１によって構築される現在前記ロボット装置が置かれている環境の環境地図と、環境地図記憶部４０２に記憶された既知の環境地図との類似度を比較することによって前記ロボット装置が現在置かれている環境を特定する環境同定部、４０４は新しい環境を認識するために前記ロボット装置を歩き回らせる環境探索部である。特許文献２の環境同定装置が特許文献１と異なるのは、前記ロボット装置が電源を切られた状態で別の環境に運ばれてから再び稼動させられる場合などであっても、環境を正しく同定することを目的としている点である。 Patent Document 2 discloses an environment identification device for a mobile robot device in an environment where a landmark exists to identify a current environment from a plurality of registered environments. FIG. 4 is a block diagram thereof, in which 401 is an environment map construction unit that constructs an environment map having landmark information of the environment in which the robot apparatus is currently located, and 402 is location information of landmarks in the environment. And an environment map storage unit having a database composed of a plurality of environment maps and the environment IDs, and 403 is an environment map construction unit 401 that is currently constructed by the robot apparatus. An environment identification unit 404 that identifies the environment in which the robot apparatus is currently placed by comparing the degree of similarity between the environment map of the placed environment and a known environment map stored in the environment map storage unit 402; Is an environment search unit for walking the robot apparatus to recognize a new environment. The environment identification device in Patent Document 2 is different from Patent Document 1 in that the robot device is correctly identified even when the robot device is turned off and then moved to another environment and then restarted. The purpose is to do.

特許文献１と特許文献２は、予め記憶しておいたランドマーク配置とランドマーク観測結果とを照合して対応付けることで自己位置を同定する点で、従来の多くの自己位置同定装置と共通している。 Patent Document 1 and Patent Document 2 are common to many conventional self-position identification devices in that the self-position is identified by collating and matching the landmark arrangement stored in advance with the landmark observation result. ing.

特許文献３には、照合に際して発生する計算コストや類似環境の存在による同定位置に誤りを生じる可能性に注目した自己位置同定機能を有する自律移動装置が開示されている。特許文献３において、自己位置の推定及び修正を行う自己位置認識手段は、環境情報取得手段で得た環境情報と地図情報に含まれる環境情報との照合に際し、推定自己位置にもとづいて、予め設定されている所定範囲内で照合を行い、この照合で修正情報を得られない時、前記所定範囲を広げて再度照合を行う、前記所定範囲をずらして再度照合を行う、などの措置をとる。照合する範囲を所定範囲に限定することで、照合のための計算コストを削減できる。また、所定範囲を推定自己位置付近とすることで、地図情報中の類似環境と誤って対応付けてしまう可能性を減らすことができる。 Patent Document 3 discloses an autonomous mobile device having a self-position identification function that pays attention to the possibility of causing an error in the identification position due to the calculation cost generated during matching and the presence of a similar environment. In Patent Document 3, the self-position recognition means for estimating and correcting the self-position is preset based on the estimated self-position when collating the environment information obtained by the environment information acquisition means with the environment information included in the map information. The collation is performed within a predetermined range, and when correction information cannot be obtained by this collation, measures are taken such that the predetermined range is expanded and collation is performed again, or the predetermined range is shifted and collation is performed again. By limiting the collation range to a predetermined range, the calculation cost for collation can be reduced. Further, by setting the predetermined range in the vicinity of the estimated self-position, it is possible to reduce the possibility of erroneous association with the similar environment in the map information.

特開平８−２４７７７５号公報（第１６頁、図１）JP-A-8-247775 (page 16, FIG. 1) 特開２００４−１１０８０２号公報（第４０頁、図５）JP 2004-110802 A (page 40, FIG. 5) 特開２００４−５５９３号公報（第４−５頁）Japanese Patent Laying-Open No. 2004-5593 (page 4-5)

以上に示したように、予め記憶しておいたランドマーク配置とランドマーク観測結果とを照合して対応付けることで移動体の自己位置を同定する方式がある。しかしながら特許文献１や特許文献２では、特許文献３で指摘されているように照合に際して発生する計算のコストや類似環境の存在による同定位置に誤りを生じる可能性が問題となっていた。特許文献３はこれに対する一対処法を与えるものである。しかし、環境情報取得手段で得た環境情報と地図情報に含まれる環境情報とを照合する際に過去の照合結果を利用しない構成であるため、不要な計算処理が発生する可能性があった。また、走行中に環境状態が変化した場合や正確な観測ができなかった場合、実際のランドマーク観測結果と予め記憶しておいたランドマーク配置が相異なるので、照合が失敗するという問題があった。すなわち、予め記憶しておいたランドマークの一部が撤去された場合、予め記憶しておいたランドマーク以外のランドマークが新たに設置された場合、予め記憶しておいたランドマークの一部が他の移動物体の陰に入るなどして観測できなかった場合などには、実際のランドマーク観測結果と予め記憶しておいたランドマーク配置が相異なり、照合が失敗してしまう。この問題は、ランドマークは環境中の所定位置に常に存在し、安定して観測可能であるとの仮定に起因するものである。 As described above, there is a method for identifying the self-position of the moving object by collating and matching the landmark arrangement stored in advance with the landmark observation result. However, in Patent Document 1 and Patent Document 2, as pointed out in Patent Document 3, there is a problem that an error may occur in the identification position due to the cost of calculation occurring at the time of collation or the presence of a similar environment. Japanese Patent Application Laid-Open No. H10-228561 provides a countermeasure for this. However, since the configuration is such that past verification results are not used when the environmental information obtained by the environmental information acquisition means and the environmental information included in the map information are collated, unnecessary calculation processing may occur. In addition, when environmental conditions change during driving or when accurate observation is not possible, the actual landmark observation result and the landmark arrangement stored in advance differ from each other. It was. That is, when a part of a previously memorized landmark is removed, when a landmark other than a memorized landmark is newly installed, a part of a landmark memorized in advance If the observation cannot be performed due to the shadow of another moving object or the like, the actual landmark observation result and the landmark arrangement stored in advance are different, and collation fails. This problem is caused by the assumption that the landmark is always present at a predetermined position in the environment and can be observed stably.

本発明は以上のような問題を念頭に、少ない計算量で自己位置の同定が可能で、なおかつ環境の変化に大しても柔軟に適応しながら自己位置を同定できる移動体の自己位置同定装置を提供することを目的としている。
In consideration of the above problems, the present invention provides a self-position identification device for a mobile object that can identify a self-position with a small amount of calculation and that can identify the self-position while flexibly adapting to changes in the environment. The purpose is to do.

上記問題を解決するため、本発明は、次のような移動体の自己位置同定装置を構成するものである。 In order to solve the above problem, the present invention constitutes the following mobile body self-position identification device.

請求項１に記載の発明は、環境内のランドマークを観測して自己の位置を同定する自己位置同定装置を備え、前記自己位置同定装置によって前記環境内での自己位置を同定しながら移動する移動体において、前記自己位置同定装置が、前記移動体周辺の前記ランドマークを観測する外界センサの観測結果からランドマーク候補を抽出するランドマーク候補抽出部と、前記ランドマーク候補に関する情報を収めた局所地図と、前記環境内の全域における前記ランドマークに関する情報を収めた大域地図とを構築する地図構築部と、前記局所地図を保存する局所地図保存部と、前記大域地図を保存する大域地図保存部と、前記外界センサの観測結果と前記大域地図の情報とから前記移動体の最新の移動***置推定値を算出する推定位置補正部と、前記移動体の内界センサから得る情報と前記移動***置推定値とから、前記環境内における自己位置同定結果を出力する位置推定部と、を備え、前記地図構築部は、前記大域地図を構築するにあたって、前記ランドマーク候補と前記大域地図の登録済みの前記ランドマークとの一致関係を判断し、一致しないと判断したとき、前記ランドマーク候補を新たな前記ランドマークとして前記大域地図に追加登録する処理を行う自己位置同定装置を備えた移動体とした。
請求項２に記載の発明は、前記地図構築部は、前記大域地図を構築するにあたって、前記大域地図を複数の小領域毎に分けて構築するとともに、前記ランドマーク候補の情報をランドマーク観測結果として前記小領域毎に保存する請求項１記載の自己位置同定装置を備えた移動体とした。
請求項３に記載の発明は、前記地図構築部は、前記判断によって前記ランドマーク候補が前記登録済みのランドマークと一致すると判断したとき、前記局所地図中に、これら前記ランドマーク候補と前記大域地図の登録済みのランドマークとの対応情報も含めて前記ランドマーク候補を保存する処理を行う請求項１または２記載の自己位置同定装置を備えた移動体とした。
請求項４に記載の発明は、前記地図構築部は、前記判断を行う前に、まず前記局所地図において、前記ランドマーク候補と、前回の処理で前記局所地図に保存された前ランドマーク候補との一致関係を照合し、前記照合において前記前ランドマーク候補と一致関係が得られた前記ランドマーク候補についてのみ、前記判断を行う請求項３記載の自己位置同定装置を備えた移動体とした。
請求項５に記載の発明は、前記地図構築部が前記判断を行うにあたって、前記照合において前記ランドマーク候補と一致関係の得られた前記前ランドマーク候補の前記対応情報を参照し、該前ランドマーク候補と一致関係がある前記登録済みのランドマークを、前記ランドマーク候補と一致するものと判断する請求項４記載の自己位置同定装置を備えた移動体とした。
請求項６に記載の発明は、前記地図構築部は、前記照合において前記ランドマーク候補が前記前ランドマーク候補との前記一致関係が得られたが、前記参照において、前記前ランドマーク候補が前記登録済みのランドマークとの前記対応情報を所有していなかったとき、前記照合一致回数を加算し、該照合一致回数が閾値以上となった前記ランドマーク候補と、前記登録済みのランドマークとが一致するものがないか判別する請求項５記載の自己位置同定装置を備えた移動体とした。
請求項７に記載の発明は、前記地図構築部は、前記判別において、前記ランドマーク候補が存在する前記小領域を特定し、該小領域中に存在する前記登録済みのランドマークの情報を順次読み出し、前記ランドマーク候補との距離をそれぞれ計算し、該計算した距離が閾値未満である前記登録済みのランドマークのうち、最も近い前記登録済みのランドマークが前記ランドマーク候補と一致しているものと判別する請求項６記載の自己位置同定装置を備えた移動体とした。
請求項８に記載の発明は、前記地図構築部は、前記ランドマーク観測結果とあわせて観測時刻を前記大域地図に保存し、現在時刻から指定時間以上経過している前記登録済みのランドマークを前記大域地図から削除する請求項２から７いずれかに記載の自己位置同定装置を備えた移動体とした。
請求項９に記載の発明は、前記指定時間は、前記移動体が前記環境内を周回する場合、前記周回の１周に要する時間よりも大きな値に設定されている請求項８記載の自己位置同定装置を備えた移動体とした。 The invention according to claim 1 includes a self-position identification device that identifies a position of the self by observing a landmark in the environment, and moves while identifying the self-position in the environment by the self-position identification device. In a mobile object, the self-position identification device stores a landmark candidate extraction unit that extracts a landmark candidate from an observation result of an external sensor that observes the landmark around the mobile object, and information related to the landmark candidate A map construction unit that constructs a local map and a global map that contains information about the landmarks in the entire area of the environment, a local map storage unit that stores the local map, and a global map storage that stores the global map An estimated position correction unit that calculates the latest estimated position of the moving object from the observation result of the external sensor and the information of the global map, A position estimation unit that outputs a self-position identification result in the environment from information obtained from an internal sensor of the mobile body and the mobile body position estimation value, and the map construction unit constructs the global map In determining whether the landmark candidate matches the registered landmark of the global map, and when it is determined that they do not match, the landmark candidate is additionally registered in the global map as the new landmark. The mobile body is equipped with a self-position identification device that performs the processing.
According to a second aspect of the present invention, in constructing the global map, the map constructing unit constructs the global map separately for each of a plurality of small regions, and uses the landmark observation result as the landmark candidate information. As a moving object, the self-position identification device according to claim 1 is stored for each small area.
According to a third aspect of the present invention, when the map construction unit determines that the landmark candidate matches the registered landmark by the determination, the landmark candidate and the global area are included in the local map. 3. A mobile body comprising the self-position identification device according to claim 1 or 2, wherein processing for storing the landmark candidates including the correspondence information with the registered landmarks on the map is performed.
According to a fourth aspect of the present invention, the map construction unit first determines the landmark candidate in the local map and the previous landmark candidate saved in the local map in the previous process before making the determination. The mobile body having the self-position identification device according to claim 3, wherein the determination is performed only for the landmark candidate for which a matching relationship with the previous landmark candidate is obtained in the verification.
The invention according to claim 5 refers to the correspondence information of the previous landmark candidate obtained in the collation with the landmark candidate when the map construction unit makes the determination, and 5. The mobile body including the self-position identification device according to claim 4, wherein the registered landmark having a coincidence relationship with a mark candidate is judged to coincide with the landmark candidate.
In the invention according to claim 6, the map construction unit obtains the coincidence relationship between the landmark candidate and the previous landmark candidate in the collation, but in the reference, the previous landmark candidate is the When the correspondence information with the registered landmark is not possessed, the number of matching matches is added, and the landmark candidate whose matching matching count is equal to or greater than a threshold value and the registered landmark are 6. A moving body provided with the self-position identification device according to claim 5 for determining whether there is a match.
In the invention according to claim 7, in the determination, the map construction unit identifies the small area where the landmark candidate exists, and sequentially stores information on the registered landmarks existing in the small area. Read, calculate the distance to each landmark candidate, and among the registered landmarks whose calculated distance is less than the threshold, the nearest registered landmark matches the landmark candidate. A mobile body provided with the self-position identification device according to claim 6 for discriminating from an object.
In the invention according to claim 8, the map construction unit stores the observation time in the global map together with the landmark observation result, and the registered landmarks that have passed a specified time or more from the current time. It was set as the mobile body provided with the self-position identification apparatus in any one of Claim 2 to 7 deleted from the said global map.
The invention according to claim 9 is the self-position according to claim 8, wherein the specified time is set to a value larger than a time required for one turn of the lap when the moving body circulates in the environment. It was set as the mobile body provided with the identification apparatus.

請求項１に記載の発明によると、ランドマーク環境が変化した場合でも自己位置同定装置で保持する大域地図を柔軟に適応させ、自己位置同定を継続することができる。
請求項３、請求項４，請求項５記載の発明により、前サイクルのランドマーク照合結果を利用することでランドマークの照合に要する計算量の大幅な削減が可能となる。
請求項６に記載の発明により、安定して観測できるランドマーク候補のみを大域地図のランドマークと一致するか判別するため、計算量を削減できる。更には、ランドマーク候補の観測結果に含まれる外界センサのノイズの影響を排除できる。
請求項６記載の発明により、観測されたランドマークと登録済みのランドマークとの一致の判断をより確実にすることができる。
請求項２、請求項７記載の発明により、互いに対応している可能性が高いランドマーク候補と登録済みランドマークとの照合を優先でき、照合にかかる計算量を削減できる。
請求項８記載の発明により、環境中から除去されたランドマーク情報を大域地図から消去し、記憶領域を節約することができる。
請求項９記載の発明により、特に同じ軌道を周回するような移動体について、環境中から除去されたランドマーク情報を大域地図から消去し、記憶領域を節約することができる。
According to the first aspect of the present invention, even when the landmark environment changes, the global map held by the self-position identification device can be flexibly adapted and self-position identification can be continued.
According to the third, fourth, and fifth aspects of the present invention, it is possible to significantly reduce the amount of calculation required for landmark matching by using the landmark matching result of the previous cycle.
According to the invention described in claim 6, since only the landmark candidates that can be observed stably coincide with the landmarks of the global map, the amount of calculation can be reduced. Furthermore, it is possible to eliminate the influence of external sensor noise included in the observation results of the landmark candidates.
According to the sixth aspect of the present invention, it is possible to more reliably determine the coincidence between the observed landmark and the registered landmark.
According to the second and seventh aspects of the present invention, priority can be given to collation between landmark candidates that have a high possibility of corresponding to each other and registered landmarks, and the amount of calculation required for collation can be reduced.
According to the eighth aspect of the present invention, the landmark information removed from the environment can be erased from the global map, and the storage area can be saved.
According to the ninth aspect of the present invention, it is possible to delete the landmark information removed from the environment from the global map and save the storage area, particularly for a moving body that orbits the same orbit.

以下、本発明の実施の形態について図を参照して説明する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.

図１は、本発明における移動体の自己位置同定装置の基本構成を示したブロック図である。図において、点線内１００が自己位置同定装置、１０１が位置推定部、１０２がランドマーク候補抽出部、１０３が地図構築部、１０４が局所地図保存部、１０５が大域地図保存部、１０６が推定位置補正部であり、信号１２１と１２２は自己位置同定装置１００の入力信号であってそれぞれ外界センサ計測結果、内界センサ計測結果であり、信号１２３は自己位置同定装置１００の出力信号であって自己位置同定結果である。実施例１における自己位置同定装置１００は外部入出力インターフェィスを備えた計算機とその内部で動作する複数のタスクであって、各タスクはそれぞれ以降に述べる処理を周期的に実行する。各構成要素について以下で説明する。 FIG. 1 is a block diagram showing a basic configuration of a mobile unit self-position identification apparatus according to the present invention. In the figure, a dotted line 100 is a self-position identification device, 101 is a position estimation unit, 102 is a landmark candidate extraction unit, 103 is a map construction unit, 104 is a local map storage unit, 105 is a global map storage unit, and 106 is an estimated position. The correction unit is a signal 121 and 122 which are input signals of the self-position identification device 100, and are external sensor measurement results and internal sensor measurement results, respectively, and a signal 123 is an output signal of the self-position identification device 100 and self It is a position identification result. The self-location identification apparatus 100 according to the first embodiment includes a computer having an external input / output interface and a plurality of tasks that operate inside the computer. Each task periodically executes the processes described below. Each component will be described below.

位置推定部１０１は、移動体の動作状態を計測する内界センサの観測結果１２２にもとづき、前記移動体の環境中における推定位置を計算し自己位置同定結果として出力する処理を毎サイクル実行する。ここで内界センサとは、例えば姿勢角度センサや車輪回転速度センサといった移動体内部の情報を計測するためのセンサを指す。実施例１における内界センサは、前記移動体車輪の回転量を計測できるように設置された回転型エンコーダである。内界センサによる推定位置はセンサノイズや計測不可能な車輪の滑りなどの影響で真値から徐々に乖離していく。そこで位置推定部１０１は、後述の推定位置補正部１０６から修正自己位置同定結果を受け取った場合、自己位置同定結果を前記修正自己位置同定結果に置き換える。
実施例１における位置推定部１０１は、前記計算機内部で前記エンコーダの信号と移動体固有の運動学関係式を用いて移動***置を推定(オドメトリ)するオドメトリ位置推定タスクである。 The position estimation unit 101 executes a process of calculating an estimated position in the environment of the moving body and outputting the result as a self-position identification result based on the observation result 122 of the internal sensor that measures the operating state of the moving body every cycle. Here, the internal sensor refers to a sensor for measuring information inside the moving body, such as a posture angle sensor or a wheel rotation speed sensor. The internal sensor in the first embodiment is a rotary encoder installed so as to be able to measure the rotation amount of the moving body wheel. The estimated position by the internal sensor gradually deviates from the true value due to sensor noise and wheel slipping that cannot be measured. Therefore, when the position estimation unit 101 receives a corrected self-position identification result from an estimated position correction unit 106 described later, the position estimation unit 101 replaces the self-position identification result with the corrected self-position identification result.
The position estimation unit 101 according to the first embodiment is an odometry position estimation task for estimating (odometry) a moving body position using the encoder signal and a kinematic relational expression specific to the moving body in the computer.

ランドマーク候補抽出部１０２は、外界センサ観測結果１２２を用いて環境中のランドマーク候補を抽出してその移動体に対する位置を特定し、地図構築部１０３へ送る処理を毎サイクル実行する。
ここで外界センサとは、例えば画像センサやレーザーレンジファインダといった移動体外部の環境情報を計測するためのセンサを指す。実施例１における外界センサは、二次元レーザーレンジファインダである。
ランドマーク候補とは、特徴的な模様を描いた板や反射板など人工的に設置した目印、あるいは物体の角や平板など特徴的な形状を有する環境中の物体や点を、現サイクルの外界センサ観測結果１２２から抽出したものである。実施例１におけるランドマーク候補は、環境中の物体に存在する所定範囲以内の角度を有する角(コーナー)である。
実施例１におけるランドマーク候補抽出部は、前記計算機内部で前記二次元レーザーレンジファインダの観測結果から所定範囲内の角度を有する角を抽出し、移動体に対する位置を特定する角抽出タスクである。ここで角とは、前記二次元レーザーレンジファインダの計測面において所定値以上の長さを有する２線分が交わることで構成される角を指し、例えば角柱の頂点や通路交差点の角などである。本実施例では、各サイクルで抽出された角をランドマーク候補としてその位置情報および角度情報を利用する。
ランドマークとは、自己位置同定に使用するため大域地図保存部１０５にその情報を登録・保存されたランドマーク候補を指す。ランドマークとして保存するランドマーク候補の選択方法は後述する。
なお、ランドマーク候補抽出部１０２の処理サイクルは位置推定部１０１の処理サイクルと同期している必要はない。 The landmark candidate extraction unit 102 executes a process of extracting a landmark candidate in the environment using the external sensor observation result 122, specifying a position with respect to the moving object, and sending the candidate to the map construction unit 103 every cycle.
Here, the external sensor refers to a sensor for measuring environmental information outside the moving body, such as an image sensor or a laser range finder. The external sensor in the first embodiment is a two-dimensional laser range finder.
A landmark candidate is an artificially installed mark such as a plate or reflector with a characteristic pattern, or an object or point in the environment that has a characteristic shape such as a corner or a plate of the object. This is extracted from the sensor observation result 122. The landmark candidate in the first embodiment is a corner (corner) having an angle within a predetermined range existing in an object in the environment.
The landmark candidate extraction unit according to the first embodiment is an angle extraction task for extracting a corner having an angle within a predetermined range from the observation result of the two-dimensional laser range finder in the computer and specifying a position with respect to the moving object. Here, the angle refers to an angle formed by the intersection of two line segments having a length equal to or greater than a predetermined value on the measurement surface of the two-dimensional laser range finder, such as an apex of a prism or a corner of a passage intersection. . In the present embodiment, the position information and the angle information are used by using the corner extracted in each cycle as a landmark candidate.
A landmark refers to a landmark candidate whose information is registered and stored in the global map storage unit 105 for use in self-location identification. A method for selecting landmark candidates to be stored as landmarks will be described later.
Note that the processing cycle of the landmark candidate extraction unit 102 does not need to be synchronized with the processing cycle of the position estimation unit 101.

地図構築部１０３は、ランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である現ランドマーク候補情報と、前サイクルまでの局所地図とから新たな局所地図を構築し保存する処理、新たに得られた局所地図と前サイクルまでの大域地図と最近の位置推定結果とから新たな大域地図を構築し保存する処理、を毎サイクル実行する。地図構築部１０３の該サイクルにおける処理については後述する。
ここで局所地図とは、前記外界センサで計測される移動体周囲環境の局所的な地図であって、
・前サイクルにおいてランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である前ランドマーク候補情報、
・現サイクルにおいてランドマーク候補抽出部１０２で抽出されたランドマーク候補の情報である現ランドマーク候補情報、
・前ランドマーク候補と現ランドマーク候補の照合が連続して一致した回数である照合一致回数、
・どの前ランドマーク候補がどのランドマークに対応しているかを示す前ランドマーク候補−ランドマーク対応関係、
・どの現ランドマーク候補がどのランドマークに対応しているかを示す現ランドマーク候補−ランドマーク対応関係、
を含む。
また大域地図とは、推定位置補正部１０６で利用するための移動体の移動環境全体にわたる大域的な地図であって、前記ランドマーク情報の集合体である。大域地図は
・移動***置同定結果、
・ランドマーク位置同定結果、
・ランドマーク観測結果、
を含む。なお、大域地図は環境を複数に分割してなる小領域毎に分けて構築され、該領域内に存在するランドマークのランドマーク情報(ランドマーク位置同定結果、ランドマーク観測結果)は該領域毎にまとめて保存される。
地図構築部１０３の処理サイクルはランドマーク候補抽出部１０２の処理サイクル終了と同時に開始される。
実施例１における地図構築部１０３は、前記計算機内部の地図構築タスクである。地図構築タスクは前記角抽出タスク完了と同時にその処理を開始する、 The map construction unit 103 constructs and stores a new local map from the current landmark candidate information, which is the landmark candidate information extracted by the landmark candidate extraction unit 102, and the local map up to the previous cycle. The process of constructing and saving a new global map from the local map obtained in the previous step, the global map up to the previous cycle and the latest position estimation result is executed every cycle. Processing in the cycle of the map construction unit 103 will be described later.
Here, the local map is a local map of the surrounding environment measured by the external sensor,
-Previous landmark candidate information which is information of landmark candidates extracted by the landmark candidate extraction unit 102 in the previous cycle,
Current landmark candidate information, which is information of landmark candidates extracted by the landmark candidate extraction unit 102 in the current cycle,
The number of matching matches, which is the number of times that the previous landmark candidate and current landmark candidate match continuously,
-Previous Landmark Candidate-Landmark Correspondence Relationship that indicates which Landmark Candidate corresponds to which Landmark,
-Current landmark candidate-landmark correspondence relationship indicating which current landmark candidate corresponds to which landmark,
including.
The global map is a global map that covers the entire moving environment of the moving object to be used by the estimated position correction unit 106, and is an aggregate of the landmark information. The global map is the result of mobile location identification,
・ Landmark position identification result,
・ Landmark observation results,
including. The global map is constructed by dividing the environment into a plurality of small areas, and landmark information (landmark position identification results, landmark observation results) of the landmarks existing in the areas is Are saved together.
The processing cycle of the map construction unit 103 is started simultaneously with the end of the processing cycle of the landmark candidate extraction unit 102.
The map construction unit 103 in the first embodiment is a map construction task inside the computer. The map construction task starts its processing upon completion of the corner extraction task.

地図構築部１０３における前記サイクルの処理の流れを、図２を用いて説明する。処理の詳細については後述する。図２はランドマーク候補抽出部１０２で抽出された最新のランドマーク候補である現ランドマーク候補１つ１つについて実行する処理である。
図２において、
Ｓ２０１は、処理を開始するステップ、
Ｓ２０２は、現ランドマーク候補情報を局所地図に保存するステップ、
Ｓ２０３は、現ランドマーク候補を局所地図に保存してある前ランドマーク候補と照合した結果によって以降の処理を分岐するステップ、
Ｓ２０４は、現ランドマーク候補に対応する前ランドマーク候補の前ランドマーク候補−ランドマーク対応関係を参照した結果によって以降の処理を分岐するステップ、
Ｓ２０５は、前ランドマーク候補に対応していたランドマークを、現ランドマーク候補にも対応すると判断して、現ランドマーク候補−ランドマーク対応関係を更新し、局所地図に保存するステップ、
Ｓ２０６は、現ランドマーク候補情報を、対応する既存ランドマークの観測結果として大域地図に保存するステップ、
Ｓ２０７は、現ランドマーク候補が前ランドマーク候補との照合結果が連続して一致した回数である照合一致回数を前ランドマーク候補の照合一致回数に１を加算した値とするステップ、
Ｓ２０８は、現ランドマーク候補情報の一部として前記照合一致回数を局所地図に追加保存するステップ、
Ｓ２０９は、前記照合一致回数と予め設定した閾値とを比較した結果により、以降の処理を分岐させるステップ、
Ｓ２１０は、現ランドマークを、大域地図に登録されている既存ランドマークと照合した結果により、以降の処理を分岐させるステップ、
Ｓ２１１は、現ランドマーク候補情報を、新ランドマークの観測結果として大域地図に追加登録するステップ、
Ｓ２１２は、現ランドマーク候補の照合一致回数を零に初期化するステップ、
Ｓ２１３は、地図構築処理を完了するステップである。 The process flow of the cycle in the map construction unit 103 will be described with reference to FIG. Details of the processing will be described later. FIG. 2 shows processing executed for each current landmark candidate, which is the latest landmark candidate extracted by the landmark candidate extraction unit 102.
In FIG.
S201 is a step of starting processing;
S202 stores the current landmark candidate information in a local map;
S203 branches the subsequent processing depending on the result of collating the current landmark candidate with the previous landmark candidate stored in the local map;
S204 is a step of branching the subsequent processing depending on the result of referring to the previous landmark candidate-landmark correspondence relationship of the previous landmark candidate corresponding to the current landmark candidate;
S205 determines that the landmark corresponding to the previous landmark candidate also corresponds to the current landmark candidate, updates the current landmark candidate-landmark correspondence relationship, and stores it in the local map;
S206 is a step of storing the current landmark candidate information in the global map as an observation result of the corresponding existing landmark;
S207 is a step of setting the number of matching matches, which is the number of times the current landmark candidate matches the matching result with the previous landmark candidate, as a value obtained by adding 1 to the number of matching matches of the previous landmark candidate;
S208 is a step of additionally storing the number of matching matches as part of the current landmark candidate information in a local map;
S209 is a step of branching the subsequent processing according to a result of comparing the number of matching matches with a preset threshold value;
S210 is a step of branching the subsequent processing according to the result of collating the current landmark with the existing landmark registered in the global map,
S211 is a step of additionally registering the current landmark candidate information in the global map as an observation result of the new landmark;
S212 is a step of initializing the number of matching matches of the current landmark candidate to zero,
S213 is a step of completing the map construction process.

以上で説明した地図構築処理の１サイクルの各ステップについて、さらに詳細に説明する。
ランドマーク候補抽出部１０２で抽出した現ランドマーク候補の情報は、ステップＳ２０２で局所地図に保存する。このとき、大域地図中の移動***置推定値と現ランドマーク候補情報とから、現ランドマーク候補の大域的な位置の推定値を計算し、現ランドマーク候補推定位置として、あわせて局所地図に保存する。 Each step of one cycle of the map construction process described above will be described in more detail.
The information on the current landmark candidate extracted by the landmark candidate extraction unit 102 is stored in the local map in step S202. At this time, the estimated global position of the current landmark candidate is calculated from the estimated position of the moving object in the global map and the current landmark candidate information, and is also added to the local map as the current landmark candidate estimated position. save.

次いでステップＳ２０３では、現ランドマーク候補と前ランドマーク候補の一致関係を照合する。照合は、以下の方法による。すなわち、前のサイクルにおけるステップＳ２０３で局所地図中に保存してあった前ランドマーク候補推定位置と、現ランドマーク候補推定位置とを比較し、両者の距離が予め定めた閾値未満である前ランドマーク候補のうち最も近い推定位置を有するものを該現ランドマーク候補と一致しているものとする。本ステップでは、現ランドマーク候補について、大域地図内の全てのランドマークとの照合をいきなり行うのではなく、まず、局所地図内において、現ランドマーク候補が、前のサイクルで保存された前ランドマーク候補のうち、どれと一致するかを照合する。こうすることによって、照合対象が前サイクルで抽出された前ランドマーク候補に限定されるので、大域地図中の全ランドマークと直接照合するよりもはるかに少ない計算量で処理を完了できる。照合の結果、現ランドマーク候補に一致する前ランドマーク候補が存在した場合はステップＳ２０４に、存在しなかった場合はステップＳ２１２に進む。 Next, in step S203, the matching relationship between the current landmark candidate and the previous landmark candidate is collated. The collation is performed by the following method. That is, the previous landmark candidate estimated position stored in the local map in step S203 in the previous cycle is compared with the current landmark candidate estimated position, and the distance between the two is less than a predetermined threshold. It is assumed that the mark candidate having the closest estimated position matches the current landmark candidate. In this step, the current landmark candidate is not suddenly checked against all the landmarks in the global map. First, in the local map, the current landmark candidate is the previous landmark saved in the previous cycle. Check which of the mark candidates matches. By doing so, the target of collation is limited to the previous landmark candidates extracted in the previous cycle, so that the processing can be completed with a much smaller calculation amount than when collating directly with all the landmarks in the global map. As a result of the collation, if there is a previous landmark candidate that matches the current landmark candidate, the process proceeds to step S204, and if not, the process proceeds to step S212.

ステップＳ２０４は、局所地図中に保存されている前ランドマーク候補−ランドマーク対応関係を利用して、現ランドマーク候補と大域地図中のランドマークの一致あるいは不一致を調べるステップである。ここで前ランドマーク候補−ランドマーク対応関係とは、前述のように、どの前ランドマーク候補がどのランドマークに一致しているかを示すものであって、前サイクルで局所地図中に保存されている対応関係のことである。ステップＳ２０４に進んだ現ランドマーク候補は、一致する前ランドマーク候補を有しているので、該前ランドマーク候補のランドマークとの対応関係を参照することで現ランドマーク候補−ランドマーク対応関係を調べる探索処理を一部省略することができる。すなわち、対応する前ランドマーク候補が大域地図中のいずれかのランドマークに対応していた場合は、現ランドマーク候補も該ランドマークに対応していると判断して、探索処理を省略してステップＳ２０５に進む。対応する前ランドマーク候補がいずれのランドマークにも対応していなかった場合は探索処理のためにステップＳ２０７に進む。 Step S204 is a step of checking the match or mismatch between the current landmark candidate and the landmark in the global map using the previous landmark candidate-landmark correspondence relationship stored in the local map. Here, the previous landmark candidate-landmark correspondence relationship indicates which previous landmark candidate matches which landmark as described above, and is stored in the local map in the previous cycle. It is a corresponding relationship. Since the current landmark candidate that has proceeded to step S204 has a matching previous landmark candidate, the current landmark candidate-landmark correspondence relationship is referred to by referring to the correspondence relationship with the landmark of the previous landmark candidate. A part of the search process for checking That is, if the corresponding previous landmark candidate corresponds to any landmark in the global map, it is determined that the current landmark candidate also corresponds to the landmark, and the search process is omitted. Proceed to step S205. If the corresponding previous landmark candidate does not correspond to any landmark, the process proceeds to step S207 for search processing.

ステップＳ２０５では、ステップＳ２０４にて一致すると判断した現ランドマーク候補−ランドマーク対応関係を局所地図に保存してステップＳ２０６に進む。 In step S205, the current landmark candidate-landmark correspondence relationship determined to match in step S204 is stored in the local map, and the process proceeds to step S206.

ステップＳ２０６では、まず現ランドマーク候補情報を、対応するランドマーク観測結果として大域地図に保存する。次いで、最近の（最も新しい）移動***置同定結果と、ランドマーク位置同定結果（現ランドマーク候補とそれに対応するランドマークとを同定した結果）とを用いて、上記ランドマーク観測結果を、計算上で予想される観測結果推定値として算出し、この予想された観測結果推定値と実際に計測された上記ランドマーク観測結果とを比較して、それらの乖離量を計算し、これを小さくするように移動***置同定結果とランドマーク位置同定結果とを修正する。この際、移動***置とランドマーク位置とを状態方程式の状態量、ランドマーク観測結果を出力ととらえ、観測結果の推定値と実際の観測結果との偏差を用いて推定状態量を毎サイクルの計算によって真値に近付けるのであるが、具体的な修正計算の方法は、例えば下記文献などにより公知であるためここでは述べない。
M.W.M.Gamini Dissanayake et al、 "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem"、 IEEE Trans. R&A、 Vol.17、 No.3、 2001
最後に、修正された移動***置同定結果とランドマーク位置同定結果とを大域地図に保存し処理を完了する。なお、同定結果の修正にあたって使用するランドマーク観測結果はできるだけ多い方が良い。個々の観測結果にはセンサノイズなどに起因する誤差が含まれているため、複数のランドマーク情報を用いることで誤差の影響を薄めることができるからである。 In step S206, first, the current landmark candidate information is stored in the global map as the corresponding landmark observation result. Next, the landmark observation result is calculated using the recent (newest) moving object position identification result and the landmark position identification result (the result of identifying the current landmark candidate and the corresponding landmark). Calculated as the estimated observation result estimated above, and compares this predicted observation result estimated value with the actually measured landmark observation result to calculate the amount of divergence between them and reduce this Thus, the moving body position identification result and the landmark position identification result are corrected. At this time, the position of the moving object and the landmark position are regarded as the state quantities in the state equation, and the landmark observation results are regarded as outputs, and the estimated state quantities are calculated for each cycle by using the deviation between the estimated observation results and the actual observation results. Although it is close to the true value by calculation, a specific correction calculation method is not described here because it is known from the following document, for example.
MWMGamini Dissanayake et al, "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem", IEEE Trans. R & A, Vol. 17, No. 3, 2001
Finally, the corrected moving body position identification result and landmark position identification result are stored in the global map, and the process is completed. It should be noted that the landmark observation results used for correcting the identification results should be as large as possible. This is because each observation result includes an error due to sensor noise or the like, and therefore the influence of the error can be reduced by using a plurality of landmark information.

ステップＳ２０７では、現ランドマーク候補が、前ランドマーク候補に対しては一致する候補が見つかったものの、その前ランドマーク候補が大域地図のランドマークと一致するものがなかったとき（前ランドマーク候補−ランドマーク対応関係を参照して対応関係が無かったとき）、現ランドマーク候補の照合一致回数を加算し、ステップＳ２０８に進む。 In step S207, when the current landmark candidate matches the previous landmark candidate, but the previous landmark candidate does not match the landmark of the global map (previous landmark candidate). -When there is no correspondence with reference to the landmark correspondence), the number of matching matches of the current landmark candidate is added, and the process proceeds to step S208.

ステップＳ２０８では、ステップＳ２０７で得た照合一致回数を局所地図に保存しステップＳ２０９に進む。 In step S208, the number of matching matches obtained in step S207 is stored in the local map, and the process proceeds to step S209.

ステップＳ２０９では、ステップＳ２０７で得た照合一致回数と前記閾値とを比較し、閾値以上であればステップＳ２１０に進み、閾値未満であれば処理を完了する。
ステップＳ２１０に進む現ランドマーク候補は、前記閾値回数以上連続して観測されたものである。前記閾値を設定することによって、ランドマーク候補抽出部１０２がセンサノイズなどの影響で偶然出力した、誤ったランドマーク候補情報を無視することができる。その結果、ステップＳ２１０でランドマークと照合するランドマーク候補の数を必要最小限に抑え、計算量を節約することができる。 In step S209, the number of matching matches obtained in step S207 is compared with the threshold value. If the threshold value is equal to or greater than the threshold value, the process proceeds to step S210.
The current landmark candidate that proceeds to step S210 has been continuously observed for the threshold number of times or more. By setting the threshold value, it is possible to ignore erroneous landmark candidate information that the landmark candidate extraction unit 102 accidentally outputs due to the influence of sensor noise or the like. As a result, the number of landmark candidates to be collated with the landmarks in step S210 can be minimized and the amount of calculation can be saved.

ステップＳ２１０は、現ランドマーク候補が、前ランドマーク候補に対しては一致する候補が見つかったものの、その前ランドマーク候補が大域地図のランドマークと一致する対応情報がなかったとき、現ランドマーク候補と大域地図のランドマークとが一致するかどうかを判別するステップである。
このステップは、閾値回数以上連続して観測された現ランドマーク候補に対して、大域地図中の既存のランドマークと一致するものが無いかどうかの確認を行うことを目的としている。
一致不一致の照合は以下の手順による。まず、ステップＳ２０２で局所地図に保存した現ランドマーク候補位置推定値を読み出す。次いで、前記現ランドマーク候補位置推定値が存在する大域地図中の前記小領域を特定する。小領域特定後は、該小領域中に存在するランドマークのランドマーク位置推定値を順次読み出し、前記ランドマーク候補推定位置との距離をそれぞれ計算する。そして、計算した距離が予め定めた閾値未満であるランドマークのうち最も近い位置推定値を有するランドマークが該現ランドマーク候補と一致しているものとする。照合の結果、現ランドマーク候補に一致するランドマークが存在した場合はステップＳ２０５に、存在しなかった場合はステップＳ２１１に進む。 In step S210, when the current landmark candidate matches the previous landmark candidate, but there is no corresponding information that matches the previous landmark candidate with the landmark of the global map, In this step, it is determined whether or not the candidate matches the landmark of the global map.
The purpose of this step is to confirm whether or not there is a match with an existing landmark in the global map with respect to the current landmark candidate observed continuously for the threshold number of times.
Matching / non-matching is performed according to the following procedure. First, the current landmark candidate position estimated value stored in the local map in step S202 is read. Next, the small area in the global map where the current landmark candidate position estimated value exists is specified. After specifying the small area, the landmark position estimated values of the landmarks existing in the small area are sequentially read, and the distance from the landmark candidate estimated position is calculated. Then, it is assumed that the landmark having the closest position estimation value among the landmarks whose calculated distance is less than the predetermined threshold matches the current landmark candidate. As a result of collation, if there is a landmark that matches the current landmark candidate, the process proceeds to step S205, and if not, the process proceeds to step S211.

ステップＳ２１１では、現ランドマーク候補を新ランドマークとして大域地図に登録しステップＳ２０５に進む。新ランドマークの情報の初期値は、最近の移動***置推定値と現ランドマーク候補情報にもとづいて計算された新ランドマーク大域的位置推定結果として登録する。 In step S211, the current landmark candidate is registered in the global map as a new landmark, and the process proceeds to step S205. The initial value of the new landmark information is registered as a new landmark global position estimation result calculated based on the latest moving object position estimated value and the current landmark candidate information.

ステップＳ２１２では、現ランドマーク候補の照合一致回数を零に初期化して局所地図に保存し処理を完了する。 In step S212, the number of matching matches of the current landmark candidate is initialized to zero and stored in the local map, and the process is completed.

以上の手順によれば新たなランドマーク候補が出現した場合に、大域地図にランドマークを追加登録することができ、環境の実情をより正確に反映した大域地図を構築できる。自己位置同定に利用できるランドマークが増えるため、後述の推定位置補正部１０６においてより正確に自己位置同定値を得ることができるようになる。 According to the above procedure, when a new landmark candidate appears, a landmark can be additionally registered in the global map, and a global map that more accurately reflects the actual situation of the environment can be constructed. Since landmarks that can be used for self-position identification increase, the self-position identification value can be obtained more accurately in the estimated position correction unit 106 described later.

一方、当初は存在していたランドマークが環境の変化によって観測されなくなることもあり得る。この場合は既に存在しないランドマーク情報を大域地図に保持し続けることになり、記憶領域を消費するばかりか照合に要する計算量が増大してしまう。これを防ぐために以下のような処理を追加しても良い。
まずステップＳ２０６において、ランドマーク観測結果とあわせて観測時刻をも大域地図に保存する。更に、地図構築部１０３の処理の後段に以下のような処理を追加する。すなわち、大域地図中のランドマークの観測時刻を調べ、現在時刻から指定時間以上経過しているランドマークは削除する。前記指定時間は必要なランドマーク情報を除去することがないように十分大きな値を設定する。移動体が同一環境内を周回する場合は、前記指定時間を１周に要する時間よりも大きな値とすればよい。 On the other hand, landmarks that originally existed may not be observed due to environmental changes. In this case, landmark information that does not already exist is kept in the global map, which not only consumes a storage area but also increases the amount of calculation required for collation. In order to prevent this, the following processing may be added.
First, in step S206, the observation time is stored in the global map together with the landmark observation result. Further, the following processing is added to the subsequent stage of the processing of the map construction unit 103. That is, the observation time of the landmark in the global map is checked, and the landmark that has passed the specified time from the current time is deleted. The specified time is set to a sufficiently large value so that necessary landmark information is not removed. When the moving body circulates in the same environment, the specified time may be set to a value larger than the time required for one lap.

推定位置補正部１０６は、位置推定部１０１が出力する移動***置同定結果を補正するものである。位置推定部１０１でオドメトリによって得られる値は、使用する運動学関係式と実際との乖離によって誤差を生じるため、これを外界センサによるランドマーク観測結果にもとづく自己位置同定値で置き換えることで誤差を打ち消すのである。
本実施例における推定位置補正部１０６は、地図構築部１０３の処理の後段で呼び出される推定位置補正ルーチンであって、大域地図から最近の移動***置同定結果を読み出して位置推定部１０１に出力する。位置推定部１０１は移動***置同定結果を受け取ると同時にそれまでの値を廃棄し、受け取った値に置き換える。 The estimated position correcting unit 106 corrects the moving body position identification result output from the position estimating unit 101. The value obtained by odometry in the position estimation unit 101 causes an error due to the difference between the kinematic relational expression to be used and the actual value. It cancels out.
The estimated position correction unit 106 in the present embodiment is an estimated position correction routine that is called later in the process of the map construction unit 103, reads the latest mobile body position identification result from the global map, and outputs the result to the position estimation unit 101. . The position estimation unit 101 receives the moving body position identification result and at the same time discards the previous value and replaces it with the received value.

以上のような構成としたため、本発明による自己位置同定装置の出力である自己位置同定結果１２３は、外界センサ観測結果１２１にもとづいて補正されるため、内界センサ観測結果１２２のみを用いて推定される値よりも真値に近くなる。本発明は従来の自己位置同定装置(例えば特許文献１乃至３)とは異なり、ランドマーク候補抽出部１０２において連続して観測されるランドマーク候補を、ランドマークとして新たに登録する一方で、指定時間以上観測されないランドマーク情報を除去することにより、環境の変化に大域地図を柔軟に対応させることができる。また、前ランドマーク候補の照合結果を利用することで、現ランドマーク候補と大域地図のランドマークとを直接照合する数を極力減らし、計算量を大幅に削減している。 Since the above configuration is adopted, the self-position identification result 123, which is the output of the self-position identification device according to the present invention, is corrected based on the external sensor observation result 121, and therefore is estimated using only the internal sensor observation result 122. Will be closer to the true value than Unlike the conventional self-position identification apparatus (for example, Patent Documents 1 to 3), the present invention newly registers landmark candidates that are continuously observed in the landmark candidate extraction unit 102 as specified landmarks. By removing landmark information that is not observed over time, the global map can be flexibly adapted to changes in the environment. Also, by using the matching result of the previous landmark candidate, the number of direct matching between the current landmark candidate and the landmark of the global map is reduced as much as possible, and the calculation amount is greatly reduced.

大域地図を環境の変化に応じて柔軟に適応させることができ、更には自己位置同定のためのランドマーク照合に要する計算量を大幅に削減できるため、時間とともに変化する環境中を、ランドマークを利用して自己位置を同定しながら移動する移動体全般に広く適用可能である。 The global map can be flexibly adapted to changes in the environment, and the amount of computation required for landmark matching for self-location identification can be greatly reduced. It can be widely applied to all moving objects that move while identifying their own positions.

本発明による自己位置同定装置の構成図Configuration diagram of self-position identification device according to the present invention 本発明による自己位置同定装置の地図構築部における処理の流れ図Flow chart of processing in map construction unit of self-position identification device according to the present invention 従来技術における自己位置同定システムの構成を示したブロック図Block diagram showing the configuration of a self-localization system in the prior art 従来技術における環境同定装置の構成を示したブロック図Block diagram showing the configuration of the environment identification device in the prior art

符号の説明Explanation of symbols

１００自己位置同定装置
１０１位置推定部
１０２ランドマーク候補抽出部
１０３地図構築部
１０４局所地図保存部
１０５大域地図保存部
１０６推定位置補正部
１２１外界センサ計測結果
１２２内界センサ計測結果
１２３自己位置同定結果
３０１ランドマーク認識部
３０２観測地図作成部
３０３移動量算出部
３０５地図データ記憶部
３０７地図データ管理部
３０８照合部
４０１環境地図構築部
４０２環境地図記憶部
４０３環境同定部
４０４環境探索部 DESCRIPTION OF SYMBOLS 100 Self-position identification apparatus 101 Position estimation part 102 Landmark candidate extraction part 103 Map construction part 104 Local map preservation | save part 105 Global map preservation | save part 106 Estimated position correction | amendment part 121 External world sensor measurement result 122 Internal world sensor measurement result 123 Self-position identification result 301 Landmark recognition unit 302 Observation map creation unit 303 Travel amount calculation unit 305 Map data storage unit 307 Map data management unit 308 Verification unit 401 Environmental map construction unit 402 Environmental map storage unit 403 Environment identification unit 404 Environment search unit

Claims

環境内のランドマークを観測して自己の位置を同定する自己位置同定装置を備え、前記自己位置同定装置によって前記環境内での自己位置を同定しながら移動する移動体において、
前記自己位置同定装置が、
前記移動体周辺の前記ランドマークを観測する外界センサの観測結果からランドマーク候補を抽出するランドマーク候補抽出部と、
前記ランドマーク候補に関する情報を収めた局所地図と、前記環境内の全域における前記ランドマークに関する情報を収めた大域地図とを構築する地図構築部と、
前記局所地図を保存する局所地図保存部と、
前記大域地図を保存する大域地図保存部と、
前記外界センサの観測結果と前記大域地図の情報とから前記移動体の最新の移動***置推定値を算出する推定位置補正部と、
前記移動体の内界センサから得る情報と前記移動***置推定値とから、前記環境内における自己位置同定結果を出力する位置推定部と、を備え、
前記地図構築部が、前記大域地図を構築するにあたって、前記ランドマーク候補と前記大域地図の登録済みの前記ランドマークとの一致関係を判断し、一致しないと判断したとき、前記ランドマーク候補を新たな前記ランドマークとして前記大域地図に追加登録する処理を行うことを特徴とする自己位置同定装置を備えた移動体。 In a moving body that comprises a self-position identification device that observes landmarks in the environment and identifies its own position, and moves while identifying the self-position in the environment by the self-position identification device,
The self-position identification device is
A landmark candidate extraction unit that extracts landmark candidates from observation results of an external sensor that observes the landmarks around the moving body;
A map construction unit for constructing a local map containing information on the landmark candidates and a global map containing information on the landmarks in the entire area in the environment;
A local map storage unit for storing the local map;
A global map storage unit for storing the global map;
An estimated position correction unit that calculates the latest moving object position estimated value of the moving object from the observation result of the external sensor and the information of the global map;
A position estimation unit that outputs a self-position identification result in the environment from the information obtained from the internal sensor of the mobile object and the mobile object position estimation value;
When the map construction unit constructs the global map, it judges the coincidence relationship between the landmark candidates and the registered landmarks of the global map. A mobile object equipped with a self-position identification device that performs a process of additionally registering as a landmark in the global map.

前記地図構築部は、前記大域地図を構築するにあたって、前記大域地図を複数の小領域毎に分けて構築するとともに、前記ランドマーク候補の情報をランドマーク観測結果として前記小領域毎に保存することを特徴とする請求項１記載の自己位置同定装置を備えた移動体。 In constructing the global map, the map construction unit constructs the global map by dividing it into a plurality of small areas, and stores the landmark candidate information as landmark observation results for each of the small areas. A moving body comprising the self-position identification device according to claim 1.

前記地図構築部は、前記判断によって前記ランドマーク候補が前記登録済みのランドマークと一致すると判断したとき、前記局所地図中に、これら前記ランドマーク候補と前記大域地図の登録済みのランドマークとの対応情報も含めて前記ランドマーク候補を保存する処理を行うことを特徴とする請求項１または２記載の自己位置同定装置を備えた移動体。 When the map construction unit determines that the landmark candidate matches the registered landmark according to the determination, the map construction unit includes the landmark candidate and the registered landmark of the global map in the local map. The mobile object having the self-position identification device according to claim 1, wherein a process of storing the landmark candidates including correspondence information is performed.

前記地図構築部は、前記判断を行う前に、まず前記局所地図において、前記ランドマーク候補と、前回の処理で前記局所地図に保存された前ランドマーク候補との一致関係を照合し、前記照合において前記前ランドマーク候補と一致関係が得られた前記ランドマーク候補についてのみ、前記判断を行うことを特徴とする請求項３記載の自己位置同定装置を備えた移動体。 Before making the determination, the map construction unit first collates the landmark candidate with the previous landmark candidate saved in the local map in the previous process in the local map, and the collation The mobile object having the self-position identification device according to claim 3, wherein the determination is performed only for the landmark candidate that has a matching relationship with the previous landmark candidate.

前記地図構築部が前記判断を行うにあたって、前記照合において前記ランドマーク候補と一致関係の得られた前記前ランドマーク候補の前記対応情報を参照し、該前ランドマーク候補と一致関係がある前記登録済みのランドマークを、前記ランドマーク候補と一致するものと判断することを特徴とする請求項４記載の自己位置同定装置を備えた移動体。 In making the determination, the map construction unit refers to the correspondence information of the previous landmark candidate obtained in the matching with the landmark candidate, and the registration having the matching relationship with the previous landmark candidate 5. The moving body having a self-position identification device according to claim 4, wherein a completed landmark is determined to coincide with the landmark candidate.

前記地図構築部は、前記照合において前記ランドマーク候補が前記前ランドマーク候補との前記一致関係が得られたが、前記参照において、前記前ランドマーク候補が前記登録済みのランドマークとの前記対応情報を所有していなかったとき、前記照合一致回数を加算し、該照合一致回数が閾値以上となった前記ランドマーク候補と、前記登録済みのランドマークとが一致するものがないか判別することを特徴とする請求項５記載の自己位置同定装置を備えた移動体。 In the collation, the map construction unit obtains the matching relationship between the landmark candidate and the previous landmark candidate in the collation, but in the reference, the previous landmark candidate corresponds to the registered landmark. When the information is not owned, the number of matching matches is added, and it is determined whether there is a match between the landmark candidate whose registered matching count is equal to or greater than a threshold and the registered landmark. A moving body comprising the self-position identification device according to claim 5.

前記地図構築部は、前記判別において、前記ランドマーク候補が存在する前記小領域を特定し、該小領域中に存在する前記登録済みのランドマークの情報を順次読み出し、前記ランドマーク候補との距離をそれぞれ計算し、該計算した距離が閾値未満である前記登録済みのランドマークのうち、最も近い前記登録済みのランドマークが前記ランドマーク候補と一致しているものと判別することを特徴とする請求項６記載の自己位置同定装置を備えた移動体。 In the determination, the map construction unit identifies the small area where the landmark candidate exists, sequentially reads information on the registered landmarks existing in the small area, and distance from the landmark candidate Are calculated, and among the registered landmarks whose calculated distance is less than a threshold, it is determined that the nearest registered landmark coincides with the landmark candidate. A moving body comprising the self-position identification device according to claim 6.

前記地図構築部は、前記ランドマーク観測結果とあわせて観測時刻を前記大域地図に保存し、現在時刻から指定時間以上経過している前記登録済みのランドマークを前記大域地図から削除することを特徴とする請求項２から７いずれかに記載の自己位置同定装置を備えた移動体。 The map construction unit stores the observation time together with the landmark observation result in the global map, and deletes the registered landmark that has passed a specified time from the current time from the global map. A moving body comprising the self-position identification device according to claim 2.

前記指定時間は、前記移動体が前記環境内を周回する場合、前記周回の１周に要する時間よりも大きな値に設定されていることを特徴とする請求項８記載の自己位置同定装置を備えた移動体。 9. The self-position identification device according to claim 8, wherein the specified time is set to a value larger than a time required for one round of the lap when the moving body circulates in the environment. Moving body.