JP6724439B2 - Autonomous mobile device, autonomous mobile system, autonomous mobile method and program - Google Patents
Autonomous mobile device, autonomous mobile system, autonomous mobile method and program Download PDFInfo
- Publication number
- JP6724439B2 JP6724439B2 JP2016049544A JP2016049544A JP6724439B2 JP 6724439 B2 JP6724439 B2 JP 6724439B2 JP 2016049544 A JP2016049544 A JP 2016049544A JP 2016049544 A JP2016049544 A JP 2016049544A JP 6724439 B2 JP6724439 B2 JP 6724439B2
- Authority
- JP
- Japan
- Prior art keywords
- communication
- autonomous mobile
- mobile device
- map
- route
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 105
- 238000004891 communication Methods 0.000 claims description 286
- 230000033001 locomotion Effects 0.000 claims description 40
- 238000003384 imaging method Methods 0.000 claims description 25
- 238000005259 measurement Methods 0.000 claims description 15
- 230000002950 deficient Effects 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 description 77
- 239000011159 matrix material Substances 0.000 description 25
- 239000013598 vector Substances 0.000 description 24
- 238000013519 translation Methods 0.000 description 22
- 230000036544 posture Effects 0.000 description 18
- 238000012545 processing Methods 0.000 description 17
- 230000006870 function Effects 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 7
- 230000000007 visual effect Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 4
- 238000004140 cleaning Methods 0.000 description 3
- 230000005684 electric field Effects 0.000 description 3
- 238000011156 evaluation Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000001816 cooling Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013213 extrapolation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
- 239000013585 weight reducing agent Substances 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Mobile Radio Communication Systems (AREA)
Description
本発明は、自律移動装置、自律移動システム、自律移動方法及びプログラムに関する。 The present invention relates to an autonomous mobile device, an autonomous mobile system, an autonomous mobile method, and a program.
用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、屋内の掃除のために自律的に移動する自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定を行う必要がある。実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、非特許文献１及び非特許文献２に記載されており、カメラの撮影する動画像の複数フレームから、同一の特徴点を追跡することで、自機の３次元位置（カメラ位置）と特徴点の３次元位置（これが集まって地図の情報を構成する）とを交互または同時に推定する処理を行っている。
2. Description of the Related Art Autonomous mobile devices that move autonomously according to their use have become popular. For example, an autonomous mobile device that autonomously moves for indoor cleaning is known. Generally, such an autonomous mobile device needs to create a map of the real space and estimate its own position in the real space. A SLAM (Simultaneous Localization And Mapping) method is known as a method for creating a map of a real space. The basic principle of SLAM technology using a monocular camera is described in Non-Patent
この自律移動装置の画像認識・音声認識等の計算処理を、ネットワーク接続した外部の計算装置で処理させると、ネットワーク通信機能が必要となる代わりに、自律移動装置側の計算パワーや記憶容量の節約（それらに付随して冷却機構の省略やバッテリ等の軽量化）、データベースの大規模化とリアルタイム共有化、アップデートの容易さ等、多くのメリットが得られる。 When the calculation processing such as image recognition and voice recognition of the autonomous mobile device is processed by an external computing device connected to the network, the network communication function is required, but the calculation power and storage capacity of the autonomous mobile device are saved. (Along with them, the omission of the cooling mechanism and the weight reduction of the battery etc.), the large scale of the database, the real time sharing, the ease of updating, and many other advantages can be obtained.
このような無線通信を行う移動ロボットの先行技術として、特許文献１には、無線通信が切断される場所に移動した場合であっても、自律的に無線通信を復旧できる場所に移動することが可能な移動ロボットが開示されている。
As a prior art of a mobile robot that performs such wireless communication,
しかし、特許文献１に開示された技術では、通信が切断されると、本来の目的地は無視して通信可能な場所に移動するため、通信が切断されないように目的地に移動することはできなかった。
However, in the technique disclosed in
本発明は、上記問題を解決するためになされたものであり、通信が切断されないように目的地に移動することができる自律移動装置等を提供することを目的する。 The present invention has been made to solve the above problem, and an object of the present invention is to provide an autonomous mobile device or the like that can move to a destination so that communication is not disconnected.
上記目的を達成するため、本発明の自律移動装置は、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した前記自律移動装置の位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記地図作成部が作成した地図と、前記通信品質地図作成部が作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索部と、
前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部と、を備え、
前記経路探索部は、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得部が取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索部は、前記通信不良領域を通らない経路を探索する、
ことを特徴とする。
In order to achieve the above object, the autonomous mobile device of the present invention,
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit;
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the position of the autonomous mobile device estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
A route search unit that searches for a route to a destination using the map created by the map creation unit and the communication quality map created by the communication quality map creation unit,
A communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched by the path search unit,
The route search unit searches for a route through the communication failure area when the communication failure time is equal to or less than the allowable latency,
Furthermore, a speed setting unit for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route search unit searches for a route passing through the defective communication area, the speed setting unit sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition unit, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The search unit searches for a route that does not pass through the poor communication area.
And wherein a call.
本発明によれば、通信が切断されないように目的地に移動することができる。 According to the present invention, it is possible to move to a destination without disconnecting communication.
以下、図１を参照しながら、本発明の実施形態に係る自律移動装置について説明する。自律移動装置１００は、用途に応じて自律的に移動する。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用などである。
Hereinafter, an autonomous mobile device according to an embodiment of the present invention will be described with reference to FIG. The autonomous
自律移動装置１００は、外観上、撮像部４１、駆動部４２を備える。
The autonomous
撮像部４１は、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓで画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、自機位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。
The
駆動部４２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。
The
例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲＲ、左車輪の回転数をＲＬとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（ＲＬ−ＲＲ）／（２×Ｉ）となる。この並進移動量や回転量を逐次足し合わせていくことで、駆動部４２は、いわゆるオドメトリとして機能し、自機位置（移動開始時の位置及び向きを基準とした位置及び向き）を計測することができる。
For example, if the diameter of the wheel is D and the number of rotations is R (measured by a rotary encoder), the translational movement amount of the wheel at the ground contact portion is π·D·R. Further, if the diameter of the wheels is D, the distance between the wheels is I, the rotation speed of the right wheel is RR , and the rotation speed of the left wheel is RL , the rotation amount of the direction change (when the right rotation is positive) is 360. It becomes (degree)*D*( RL- RR )/(2*I). By sequentially adding the translational movement amount and the rotation amount, the
なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。 Note that a crawler may be provided instead of the wheels, or a plurality of (eg, two) feet may be provided to move by walking with the feet. In these cases as well, the position and orientation of the vehicle itself can be measured based on the movements of the two crawlers and the movements of the legs, as in the case of the wheels.
図２に示すように、自律移動装置１００は、撮像部４１、駆動部４２に加えて、制御部１０、記憶部２０、センサ部３０、入力部４３、通信部４４、電源４５、を備える。
As shown in FIG. 2, the autonomous
制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（地図作成部１１、位置推定部１２、通信品質測定部１３、通信品質地図作成部１４、経路探索部１５）の機能を実現する。
The control unit 10 is configured by a CPU (Central Processing Unit) or the like, and executes a program stored in the storage unit 20 so that each unit (a map creating unit 11, a
記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ)、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、画像記憶部２１、地図記憶部２２、通信品質地図記憶部２３、経路探索用地図記憶部２４を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム（例えば、後述する通信品質地図の作成や自律移動制御処理に係るプログラム）や、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。
The storage unit 20 includes a ROM (Read Only Memory), a RAM (Random Access Memory), and the like, and includes an
画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。
The
地図記憶部２２には、後述するＳＬＡＭ法や距離センサ３１からの情報に基づいて地図作成部１１が作成した地図（壁や障害物の情報）が記憶される。
The
通信品質地図記憶部２３には、後述する通信品質地図作成部１４が作成した通信品質地図が記憶される。
The communication quality
経路探索用地図記憶部２４には、地図記憶部２２と通信品質地図記憶部２３の情報から作成した、通信品質を考慮した経路を探索するために用いる経路探索用地図が記憶される。
The route search
センサ部３０として、距離センサ３１を備える。距離センサ３１は、周囲の壁や障害物までの距離を測定するセンサであって、例えば、赤外線や超音波を用いたセンサである。なお、独立した距離センサ３１を搭載せずに、撮像部４１を用いて壁や障害物までの距離を測定するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。
A
入力部４３として、自律移動装置１００を操作するための操作ボタンを備える。操作ボタンは、例えば、電源ボタン、モード切替ボタン（掃除モード、ペットモード等を切り替える）、初期化ボタン（地図の作成をやり直しさせる）等を含む。入力部４３として、音の入力を行うマイク（図示せず）と、自律移動装置１００への操作指示の音声を認識する音声認識部を備えても良い。
As the
通信部４４は、外部の通信機２００と無線通信するための無線モジュールである。一般的には電波を用いた無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）モジュールであるが、赤外線や可視光を用いたものであっても良い。なお、外部の通信機２００との通信においては、図３（ａ）に示すように、ネットワーク経由で通信しても良いし、図３（ｂ）に示すように直接通信しても良い。図３に示すように、自律移動装置１００だけでなく通信機２００も通信部４４を備える。
The
電源４５は、自律移動装置１００を動作させる電源であり、一般的には内蔵された充電池であるが、太陽電池であっても良いし、床面から無線で電力供給されるシステムであっても良い。電源４５が充電池の場合は、充電ステーション（ホームベース）に自律移動装置１００がドッキングすることで充電される。
The
次に、制御部１０の機能について説明する。制御部１０は、地図作成部１１、位置推定部１２、通信品質測定部１３、通信品質地図作成部１４、経路探索部１５を含み、後述する通信品質地図の作成や自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。
Next, the function of the control unit 10 will be described. The control unit 10 includes a map creation unit 11, a
地図作成部１１は、画像記憶部２１に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてＳＬＡＭ法を用いて推定された特徴点（Ｍａｐ点）の位置や、距離センサ３１で壁や障害物を検知したときの自機の位置及び向きの情報に基づいて得られた壁や障害物の位置等を、地図の情報として地図記憶部２２に記憶する。
The map creation unit 11 determines the characteristic points (Map points) estimated by using the SLAM method based on the information of the image stored in the
位置推定部１２は、後述するＳＬＡＭ法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。
The
通信品質測定部１３は、通信部４４で外部の通信機２００と通信する際の通信品質を測定する。
The communication
通信品質地図作成部１４は、自律移動装置１００の周囲の各地点において、通信品質測定部１３で外部の通信機２００と通信する際の通信品質を測定し、通信品質地図（各地点での通信品質が記録された地図）を作成する。
The communication quality map creation unit 14 measures the communication quality when the communication
経路探索部１５は、経路探索用地図記憶部２４に記憶された地図の情報に基づいて経路を探索する。
The
自律移動装置１００の自律移動制御処理のメインフローを図４を参照して説明する。制御部１０は、まず、目的地を示す変数（目的地）に初期値である「未設定」をセットする（ステップＳ１０１）。次に、制御部１０は、自機位置推定スレッドを起動する（ステップＳ１０２）。そして、地図作成スレッドを起動する（ステップＳ１０３）。次に、ループクロージングスレッドを起動する（ステップＳ１０４）。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）、距離センサ３１とビジュアルオドメトリを利用して地図の更新を行う（ステップＳ１０６）。
The main flow of the autonomous movement control process of the autonomous
次に制御部１０は、目的地への移動指示があるか否かを判定する（ステップＳ１０７）。移動指示があれば（ステップＳ１０７；Ｙｅｓ）、指示された目的地を目的地を示す変数（目的地）に設定し（ステップＳ１１０）、目的地への経路計画を行う（ステップＳ１１１）。目的地への経路計画の詳細は後述する。そして、制御部１０は、経路計画により設定された経路に沿って移動を行うために駆動部４２に動作指示を出し（ステップＳ１１２）、ステップＳ１０５に戻る。
Next, the control unit 10 determines whether or not there is an instruction to move to the destination (step S107). If there is a movement instruction (step S107; Yes), the instructed destination is set as a variable indicating the destination (destination) (step S110), and a route to the destination is planned (step S111). Details of route planning to the destination will be described later. Then, the control unit 10 issues an operation instruction to the
目的地への移動指示がなければ（ステップＳ１０７；Ｎｏ）、制御部１０は、目的地が未設定であるか否かを判定する（ステップＳ１０８）。目的地が未設定の場合は（ステップＳ１０８；Ｙｅｓ）、自由な自律移動を行い（ステップＳ１０９）、ステップＳ１０５に戻る。自由な自律移動とは、ランダムに動き回るような移動でも良いし、地図を作成するために部屋の中をくまなく動き回るような移動でも良い。 If there is no instruction to move to the destination (step S107; No), the control unit 10 determines whether the destination has not been set (step S108). When the destination is not set (step S108; Yes), free autonomous movement is performed (step S109), and the process returns to step S105. The free autonomous movement may be movement that randomly moves around, or movement that moves around the room to create a map.
目的地が設定されている場合は（ステップＳ１０８；Ｎｏ）、経路計画により経路が設定されているので、その経路に沿って移動を行うために制御部１０は駆動部４２に動作指示を出し（ステップＳ１１２）、ステップＳ１０５に戻る。なお、目的地を示す変数（目的地）はスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。
If the destination is set (step S108; No), the route is set according to the route plan, and therefore the control unit 10 issues an operation instruction to the
このメインフローの処理により、地図を更新しながら自律的に移動を行いつつ、移動指示を受けた目的地に効率の良い移動経路を経て移動することができる。典型的な例としては、自律移動装置１００は最初、充電ステーションに置いてある状態で電源を投入すると、距離センサ３１を頼りとして、家の各部屋をくまなく移動し、距離センサ３１によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電ステーションに戻ったり、効率的に部屋の掃除をしたりすることが可能になる。
By the processing of this main flow, it is possible to autonomously move while updating the map and move to the destination instructed to move through an efficient moving route. As a typical example, when the autonomous
次に、自律移動装置１００のメインフロー（図４）のステップＳ１０２で起動される自機位置推定スレッドについて、図５を参照して説明する。このスレッドは、位置推定部１２が、最初に初期化処理を行い、その後自機位置推定（撮像部４１で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。
Next, the self-position estimation thread started in step S102 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. This thread is a process in which the
位置推定部１２は、まず、推定状態変数に「初期化中」をセットする（ステップＳ２０１）。推定状態変数は、その時点の自機位置推定処理の状態を示し、「初期化中」「追跡成功」「追跡失敗」の三つの値を取る。次に、動作終了か否かを判定する（ステップＳ２０２）。動作終了なら（ステップＳ２０２；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０２；Ｎｏ）、推定状態変数が「初期化中」であるか否かを判定する（ステップＳ２０３）。初期化中でないなら（ステップＳ２０３；Ｎｏ）ステップＳ２２１以降の自機位置推定処理を行い、初期化中なら（ステップＳ２０３；Ｙｅｓ）ステップＳ２０４に進んで初期化処理を行う。まず初期化処理について説明する。
The
初期化処理では位置推定部１２はまずフレームカウンタＮに−１をセットし（ステップＳ２０４）、撮像部４１で画像を取得する（ステップＳ２０５）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。次に、位置推定部１２は、撮像部４１で取得した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２０６）。
In the initialization process, the
２Ｄ特徴点とは、画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。 A 2D feature point is a characteristic part in an image, such as an edge part in the image, and may be acquired using an algorithm such as SIFT (Scale-Invariant Future Transform) or SURF (Speed-Up Robust Features). it can.
取得した２Ｄ特徴点の個数が少ないと、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法での計算ができないため、位置推定部１２は、２Ｄ特徴点の取得数と基準値（例えば１０個）とを比較し（ステップＳ２０７）、基準値未満だった場合（ステップＳ２０７；Ｎｏ）はステップＳ２０５に戻り、基準値以上の２Ｄ特徴点数が得られるまで、画像の取得と２Ｄ特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、距離センサ３１を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と２Ｄ特徴点取得を繰り返していれば、移動しながら画像取得を繰り返すことになる。従って、様々な画像が取得でき、いずれは２Ｄ特徴点数が多い画像を取得できることが期待できる。
If the number of acquired 2D feature points is small, calculation by the two-view Structure from motion method described later cannot be performed, and thus the
２Ｄ特徴点の取得数が基準値以上だった場合は（ステップＳ２０７；Ｙｅｓ）、位置推定部１２はフレームカウンタＮをインクリメントする（ステップＳ２０８）。そして、フレームカウンタＮが０かどうかを判定する（ステップＳ２０９）。フレームカウンタＮが０なら（ステップＳ２０９；Ｙｅｓ）画像をまだ一つしか取得していないということになるので、２枚目の画像を取得するためにステップＳ２０５に戻る。なお図５のフローチャートでは記載していないが、１枚目の画像を取得した時の自機の位置と２枚目の画像を取得する時の自機の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ステップＳ２０９からステップＳ２０５に戻る際に、メインフロー（図４）のステップＳ１０９での自由な自律移動により、オドメトリによる並進距離が所定の距離（例えば１ｍ）以上となるまで待ってからステップＳ２０５に戻るようにしても良い。
When the number of acquired 2D feature points is equal to or greater than the reference value (step S207; Yes), the
フレームカウンタＮが０でないなら（ステップＳ２０９；Ｎｏ）二つの画像を取得したことになるので、位置推定部１２は、これら二つの画像間で２Ｄ特徴点の対応（実環境上の同一の点がそれぞれの画像中に存在し、その対応が取れるもの）を取得する（ステップＳ２１０）。ここで特徴点の対応数が５未満であれば、後述する二つの画像間の姿勢の推定ができないため、位置推定部１２は特徴点の対応数が５未満かどうかを判定する（ステップＳ２１１）。５未満であれば（ステップＳ２１１；Ｙｅｓ）初期画像を取得し直すために、ステップＳ２０４に戻る。特徴点の対応数が５点以上であれば（ステップＳ２１１；Ｎｏ）、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法を用いることで、二つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができる（ステップＳ２１２）。
If the frame counter N is not 0 (step S209; No), it means that two images have been acquired. Therefore, the
この、Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法による姿勢の推定は、具体的には、対応する特徴点から基礎行列Ｅを求め、基礎行列Ｅを並進ベクトルｔと回転行列Ｒとに分解することによって行う（詳細は非特許文献２を参照）。なお、ここで得られる並進ベクトルｔ（３次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、Ｘ，Ｙ，Ｚの３要素を持つ）の各要素の値は実環境上での値とは異なる（Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法では実環境上の値自体を得ることはできず、実環境と相似する空間上での値を得ることになる。）ため、これらをＳＬＡＭ空間上での値とみなし、以下ではＳＬＡＭ空間上での座標（ＳＬＡＭ座標）を用いて説明する。 The pose estimation by the Two-view Structure from Motion method is specifically performed by obtaining a basic matrix E from the corresponding feature points and decomposing the basic matrix E into a translation vector t and a rotation matrix R ( See Non-Patent Document 2 for details). The value of each element of the translation vector t obtained here (assuming movement in a three-dimensional space has three elements of X, Y, and Z with the position where the first image is acquired as the origin) Since it is different from the value in the real environment (the Two-view Structure from Motion method cannot obtain the value itself in the real environment, but the value in the space similar to the real environment). These are regarded as values in the SLAM space, and will be described below using coordinates in the SLAM space (SLAM coordinates).
二つの画像間の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まると、その値は、最初の画像を基準（最初の画像を取得した位置をＳＬＡＭ座標の原点、並進ベクトルは０ベクトル、回転行列は単位行列Ｉとする。）にした場合の、二枚目の画像の姿勢（二つ目の画像を取得した時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ））となる。ここで、二つの画像それぞれの姿勢（該画像（フレーム）撮影時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ）で、フレーム姿勢とも言う。）が求まっている場合、その二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）のＳＬＡＭ座標での３Ｄ位置を、以下の考え方に基づき、地図作成部１１が求める（ステップＳ２１３）。 When the orientation (translation vector t and rotation matrix R) between two images is obtained, its value is based on the first image (the position where the first image is acquired is the origin of SLAM coordinates, the translation vector is 0 vector, and the rotation matrix is Is the unit matrix I.) is the posture of the second image (the position (translation vector t) and the orientation (rotation matrix R) of the own device when the second image is acquired). .. Here, when the orientations of the two images (the position (translation vector t) and the orientation (rotation matrix R) of the own device at the time of capturing the image (frame), also referred to as the frame orientation) are obtained, the two The map creating unit 11 obtains the 3D position in SLAM coordinates of the 2D feature points (corresponding feature points) that are in correspondence between the two images (step S213).
２Ｄ特徴点の画像中の座標（フレーム座標：既知）を（ｕ，ｖ）とし、その２Ｄ特徴点のＳＬＡＭ座標での３Ｄ位置（未知）を（Ｘ，Ｙ，Ｚ）とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Ｐを用いて下記の式（１）で表される。ここで、「〜」記号は「非零の定数倍を除いて等しい」（つまり、等しいか又は定数（非零）倍になっている）ことを表し、「’」記号は「転置」を表す。
（ｕ ｖ １）’〜Ｐ（Ｘ Ｙ Ｚ １）’…（１）
If the coordinates (frame coordinates: known) of the 2D feature point in the image are (u, v) and the 3D position (unknown) in SLAM coordinates of the 2D feature point is (X, Y, Z), these are the same. These relationships when expressed by the secondary coordinates are expressed by the following equation (1) using the perspective projection matrix P. Here, the symbol "~" means "equal except constant multiples of non-zero" (that is, equal or multiplied by a constant (non-zero) times), and the symbol "'" represents "transposition". ..
(Uv1)'-P(XYZ1)'... (1)
上記の式（１）において、Ｐは３×４の行列で、カメラの内部パラメータを示す３×３の行列Ａと、その画像の姿勢（フレーム姿勢）を示す外部パラメータＲ及びｔから以下の式（２）で表される。ここで、（Ｒ｜ｔ）は、回転行列Ｒの右に並進列ベクトルｔを並べた行列を表す。
Ｐ＝Ａ（Ｒ｜ｔ）…（２）
In the above equation (1), P is a 3×4 matrix, and the following equation is derived from the 3×3 matrix A indicating the internal parameters of the camera and the external parameters R and t indicating the orientation (frame orientation) of the image. It is represented by (2). Here, (R|t) represents a matrix in which the translation column vector t is arranged to the right of the rotation matrix R.
P=A(R|t)...(2)
上記の式（２）において、Ｒ及びｔは上述したようにそのフレーム姿勢として求められている。また、カメラの内部パラメータＡは、焦点距離と撮像素子サイズにより決まるので、撮像部４１を決めておけば定数となる。
In the above equation (2), R and t are obtained as the frame postures thereof as described above. Further, since the internal parameter A of the camera is determined by the focal length and the size of the image pickup element, it becomes a constant if the
二つの画像間で対応が取れている２Ｄ特徴点のうちの一つが、一つ目の画像のフレーム座標（ｕ１，ｖ１）と、二つ目の画像のフレーム座標（ｕ２，ｖ２）に写っているとすると、以下の式（３）及び式（４）ができる。ここで、Ｉは単位行列、０はゼロベクトル、（Ｌ｜ｒ）は、行列Ｌの右に列ベクトルｒを並べた行列を表す。
（ｕ１ ｖ１ １）’〜Ａ（Ｉ｜０）（Ｘ Ｙ Ｚ １）’…（３）
（ｕ２ ｖ２ １）’〜Ａ（Ｒ｜ｔ）（Ｘ Ｙ Ｚ １）’…（４）
One of the 2D feature points that are associated between the two images is one of the frame coordinates (u 1 , v 1 ) of the first image and the frame coordinates (u 2 , v 2 of the second image. ), the following equations (3) and (4) can be obtained. Here, I is a unit matrix, 0 is a zero vector, and (L|r) represents a matrix in which a column vector r is arranged on the right side of the matrix L.
(U 1 v 1 1)′ to A(I|0)(XYZ 1)′... (3)
(U 2 v 2 1) ' ~A (R | t) (X Y Z 1)' ... (4)
上記の式（３）及び式（４）において、ｕ１，ｖ１，ｕ２，ｖ２それぞれについての式ができるため、式は４つできるが、未知数はＸ，Ｙ，Ｚの３つなので、Ｘ，Ｙ，Ｚを求めることができ、これがその２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばｕ１，ｖ１，ｕ２で求めたＸ，Ｙ，Ｚとｕ１，ｖ１，ｖ２で求めたＸ，Ｙ，Ｚとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部１１は、最小二乗法を用いて、最も確からしいＸ，Ｙ，Ｚを求める。 In the above equations (3) and (4), since there are equations for u 1 , v 1 , u 2 , and v 2, respectively, there are four equations, but there are three unknowns, X, Y, and Z. , X, Y, Z can be obtained, and this is the 3D position of the 2D feature point in SLAM coordinates. Since the number of equations is larger than the number of unknowns, for example, X, Y, Z obtained by u 1 , v 1 , u 2 and X, Y, Z obtained by u 1 , v 1 , v 2. Can be different from. In such a case, the simultaneous linear equations with excess conditions are present, and there is generally no solution, but the map creation unit 11 uses the least squares method to find the most probable X, Y, and Z.
２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１１がＭａｐ点データベース（Ｍａｐ点ＤＢ（Ｄａｔａｂａｓｅ）とも言い、地図記憶部２２に格納される）に登録する（ステップＳ２１４）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置であるＸ，Ｙ，Ｚ」と、「その２Ｄ特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。
When the 3D position (X, Y, Z) in the SLAM coordinates of the 2D feature point is obtained, the map creation unit 11 stores it in the
そして、地図作成部１１は、二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）の全てをＭａｐ点データベースに登録したかを判定し（ステップＳ２１５）、まだ全ての登録ができていなかったら（ステップＳ２１５；Ｎｏ）ステップＳ２１３に戻り、全て登録できたら（ステップＳ２１５；Ｙｅｓ）ステップＳ２１６に進む。 Then, the map creation unit 11 determines whether or not all of the 2D feature points (corresponding feature points) that are associated with each other between the two images are registered in the Map point database (step S215), and all can be registered. If not registered (step S215; No), the process returns to step S213, and if all are registered (step S215; Yes), the process proceeds to step S216.
そして、位置推定部１２は、キーフレーム（後に続くスレッドでの処理対象となる画像を指す）のカウンタＮＫＦを０に初期化し（ステップＳ２１６）、二つ目の画像をキーフレームとしてフレームデータベース（フレームＤＢとも言い、画像記憶部２１に格納される）に登録する（ステップＳ２１７）。
Then, the
フレームデータベースに登録する要素は、「キーフレーム番号」（登録時点でのキーフレームカウンタＮＫＦの値）、「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、であるが、これらに加えて「オドメトリで計測した実環境上での姿勢」（実環境での駆動部４２による移動距離に基づいて求められる自機の位置及び向き）も登録しても良い。
The elements to be registered in the frame database are the "key frame number" (the value of the key frame counter NKF at the time of registration), the "attitude" (the position in the SLAM coordinate of the device at the time of capturing the image (translation vector t), and Direction (rotation matrix R), “all extracted 2D feature points”, “point of which 3D position is known as Map point among all 2D feature points”, “feature of key frame itself”, In addition to these, the “posture in the actual environment measured by odometry” (the position and orientation of the own device, which is obtained based on the moving distance by the
上記中、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の２Ｄ特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。また、「オドメトリで計測した実環境上での姿勢」は、並進ベクトルｔと回転行列Ｒとで表すこともできるが、通常、本自律移動装置１００は２次元平面上を動くので、２次元データに単純化して、移動開始時の位置（原点）及び向きを基準にした２次元座標（Ｘ，Ｙ）及び向きφとして表しても良い。
In the above description, the “feature of the key frame itself” is data for improving the efficiency of the process of obtaining the image similarity between the key frames, and normally a histogram of 2D feature points in the image or the like should be used. The image itself may be the “feature of the key frame itself”. Further, the “posture in the real environment measured by odometry” can be represented by the translation vector t and the rotation matrix R, but since the autonomous
次に、位置推定部１２は、キーフレームが生成された事を地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー（キュー（Ｑｕｅｕｅ）は、先入れ先出しのデータ構造になっている）に、キーフレームカウンタＮＫＦをセットする（ステップＳ２１８）。
Next, the
以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１２は、推定状態変数に「追跡成功」をセットし（ステップＳ２１９）、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ２２０）。
Since the initialization process of the own device position estimation thread is completed as described above, the
そして、ステップＳ２０２、ステップＳ２０３を経由して、初期化済の場合の処理であるステップＳ２２１へ進む。 Then, through steps S202 and S203, the process proceeds to step S221, which is the process in the case of initialization.
初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、位置推定部１２が、逐次、現在の自機の位置及び向き（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒ）を推定する処理である。
The processing when the initialization has been completed will be described. This process is a normal process of the own device position estimation thread, and the
まず、位置推定部１２は、撮像部４１で画像を撮影し（ステップＳ２２１）、フレームカウンタＮをインクリメントする（ステップＳ２２２）。そして、特徴点取得部１１は、撮像部４１で撮影した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２２３）。
First, the
次に、位置推定部１２は、推定状態変数が「追跡成功」か否かを判定する（ステップＳ２２４）。「追跡成功」なら（ステップＳ２２４；Ｙｅｓ）、位置推定部１２は、フレームデータベースに登録されている一つ前のキーフレーム（キーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている２Ｄ特徴点のうち、３Ｄ位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）２Ｄ特徴点を取得して、その中でステップＳ２２３で取得した特徴点と対応が取れる２Ｄ特徴点（対応特徴点）の個数（特徴点対応数）を取得する（ステップＳ２２５）。
Next, the
推定状態変数の値が「追跡成功」でないなら（ステップＳ２２４；Ｎｏ）、位置推定部１２は、フレームデータベースに登録されている二つ以上前のキーフレーム（キーフレーム番号がＮＫＦ未満である画像）の中から、Ｍａｐ点とステップＳ２２３で取得した特徴点との対応数が最大となるキーフレームと、その特徴点対応数を取得する（ステップＳ２２６）。
If the value of the estimated state variable is not “successful tracking” (step S224; No), the
そして、位置推定部１２は、特徴点対応数が閾値（例えば３０。以下「基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２７）、基準対応特徴点数以下の場合（ステップＳ２２７；Ｎｏ）はＳＬＡＭ法で推定する姿勢の精度が悪くなるので、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。
Then, the
特徴点対応数が基準対応特徴点数より大きい場合は（ステップＳ２２７；Ｙｅｓ）、ステップＳ２２５又はステップＳ２２６において特徴点対応数を取得したキーフレームの近傍のキーフレーム（特徴点対応数を取得したキーフレームと、所定の割合以上（例えば３０％以上）特徴点の重なりが存在するキーフレーム）に含まれるＭａｐ点と対応が取れるステップＳ２２３で取得した特徴点（対応特徴点）の個数（特徴点対応数）を位置推定部１２が取得する（ステップＳ２２８）。 If the number of corresponding feature points is larger than the number of corresponding corresponding feature points (step S227; Yes), a key frame in the vicinity of the key frame for which the corresponding number of corresponding feature points is acquired in step S225 or step S226 (key frame for which the corresponding number of corresponding feature points is acquired) And the number of feature points (corresponding feature points) acquired in step S223 (corresponding feature point correspondence number) that can be associated with Map points included in a key frame in which feature points overlap at a predetermined ratio or more (for example, 30% or more) ) Is acquired by the position estimation unit 12 (step S228).
そして、位置推定部１２は、特徴点対応数が閾値（例えば５０。以下「第２の基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２９）、第２の基準対応特徴点数以下なら（ステップＳ２２９；Ｎｏ）、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。特徴点対応数が第２の基準対応特徴点数よりも大きければ（ステップＳ２２９；Ｙｅｓ）、ステップＳ２３１に進む。
Then, the
ステップＳ２３１では、位置推定部１２は、推定状態変数に追跡成功をセットする（ステップＳ２３１）。位置推定部１２は、ステップＳ２２８で取得した対応特徴点それぞれの３Ｄ位置（Ｘｉ，Ｙｉ，Ｚｉ）をＭａｐ点データベースから取得し、ステップＳ２２１で取得した画像に含まれている対応特徴点との対応関係を用いて、現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）を推定する（ステップＳ２３２）。
In step S231, the
この自機の姿勢の推定方法について、補足説明する。ステップＳ２２１で取得した画像に含まれている対応特徴点のフレーム座標を（ｕｉ，ｖｉ）とし、その対応特徴点の３Ｄ位置を（Ｘｉ，Ｙｉ，Ｚｉ）とする（ｉは１から特徴点対応数までの値を取る）と、各対応特徴点の３Ｄ位置（Ｘｉ，Ｙｉ，Ｚｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘｉ，ｖｘｉ）とフレーム座標（ｕｉ，ｖｉ）とは理想的には一致するはずである。
（ｕｘｉ ｖｘｉ １）’〜Ａ（Ｒ｜ｔ）（Ｘｉ Ｙｉ Ｚｉ １）’…（５）
A supplementary description will be given of the method of estimating the attitude of the own device. Let (u i , v i ) be the frame coordinates of the corresponding feature points included in the image acquired in step S221, and let the 3D position of the corresponding feature points be (X i , Y i , Z i ) (where i is 1) to the number of corresponding feature points) and the 3D position (X i , Y i , Z i ) of each corresponding feature point projected onto the frame coordinate system by the following equation (5) (ux i , Ideally, vx i ) and the frame coordinates (u i , v i ) should match.
(Ux i vx i 1)' to A(R|t)(X i Y i Z i 1)'... (5)
しかし、実際には（Ｘｉ，Ｙｉ，Ｚｉ）にも（ｕｉ，ｖｉ）にも誤差が含まれているため、（ｕｘｉ，ｖｘｉ）と（ｕｉ，ｖｉ）とが一致することはめったにない。そして、未知数はＲとｔ（３次元空間ではそれぞれ３次元となり、３＋３＝６が未知数の個数である）だけなのに、数式は対応特徴点の個数の２倍存在する（対応特徴点一つに対して、フレーム座標のｕ，ｖそれぞれに対する式が存在するため）ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部１２は、以下の式（６）のコスト関数Ｅ１を最小化する姿勢（並進ベクトルｔ及び回転行列Ｒ）を求めることになる。これがＳＬＡＭ法で求めたＳＬＡＭ座標での自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）となる。このようにして、位置推定部１２は自機の姿勢を推定する（ステップＳ２３２）。
ＳＬＡＭ座標での現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、位置推定部１２は、これにスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ２３３）。ＶＯは実環境での自機の位置及び向きとして利用できる。
Since the current attitude (translation vector t and rotation matrix R) in SLAM coordinates has been obtained, the
次に、位置推定部１２は、フレームＤＢに登録されている直前のキーフレーム（キーフレーム番号がＮＫＦである画像）を撮影した時の自機の位置から所定の距離（例えば１ｍ。以下「基準並進距離」という。）以上移動しているかを判定し（ステップＳ２３４）、移動しているなら（ステップＳ２３４；Ｙｅｓ）キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ２３５）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ２３６）。基準並進距離未満しか移動していないなら（ステップＳ２３４；Ｎｏ）ステップＳ２０２に戻る。
Next, the
ここで、基準並進距離と比較する自機の移動距離は、直前のキーフレームから現フレームまでの並進距離（両フレームの並進ベクトルの差のベクトルの絶対値（要素の二乗和の平方根））をオドメトリから取得しても良いし、上述したＶＯから求めても良い。フレームＤＢに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。 Here, the movement distance of the own machine compared with the reference translation distance is the translation distance from the immediately preceding key frame to the current frame (the absolute value of the vector of the difference between the translation vectors of both frames (the square root of the sum of squares of elements)). It may be obtained from odometry or may be obtained from VO described above. As described above, the contents to be registered in the frame DB are “key frame number”, “posture”, “all extracted 2D feature points”, and “point of which 3D position is known as Map point among all 2D feature points”. ", "Characteristics of the key frame itself".
そして、位置推定部１２は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ２３７）。そして、ステップＳ２０２に戻る。なお、キーフレームカウンタＮＫＦ、スケールＳ、Ｍａｐ点ＤＢ、フレームＤＢ、推定状態変数、ＶＯ（ビジュアルオドメトリ）はスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。
Then, the
次に、自律移動装置１００のメインフロー（図４）のステップＳ１０３で起動される地図作成スレッドについて、図６を参照して説明する。このスレッドは地図作成部１１が、キーフレーム中の対応特徴点の３Ｄ位置を計算して取得するＭａｐ点の情報と、距離センサ３１を用いて取得する壁や障害物までの距離の情報と、から地図の情報を作成し、地図記憶部２２に記憶する。そして、通信品質地図作成部１４が、通信品質測定部１３から取得する通信品質の情報を用いて通信品質地図を作成し、通信品質地図記憶部２３に記憶する。
Next, the map creation thread started in step S103 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. In this thread, the map creation unit 11 calculates the 3D position of the corresponding feature point in the key frame and acquires the information about the Map point, and the
まず、地図作成部１１は、動作終了かを判定する（ステップＳ３０１）。動作終了なら（ステップＳ３０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ３０１；Ｎｏ）、キーフレームキューが空かどうかを判定する（ステップＳ３０２）。キーフレームキューが空だったら（ステップＳ３０２；Ｙｅｓ）ステップＳ３０１に戻り、空でなければ（ステップＳ３０２；Ｎｏ）、キーフレームキューからデータを取り出してＭＫＦ（地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ３０３）。地図作成部１１は、ＭＫＦが０より大きいか判定し（ステップＳ３０４）、ＭＫＦが０である場合は（ステップＳ３０４；Ｎｏ）ステップＳ３０１に戻ってキーフレームキューにデータが入るのを待つ。ＭＫＦが１以上の場合は（ステップＳ３０４；Ｙｅｓ）、以下の処理に進む。 First, the map creation unit 11 determines whether the operation is completed (step S301). If the operation is completed (step S301; Yes), the operation is ended, and if the operation is not ended (step S301; No), it is determined whether or not the key frame queue is empty (step S302). If the key frame queue is empty (step S302; Yes), the process returns to step S301. If not empty (step S302; No), data is extracted from the key frame queue and MKF (key frame of key frame processed by map creation thread) (A variable representing a number) is set (step S303). The map creation unit 11 determines whether MKF is larger than 0 (step S304), and when MKF is 0 (step S304; No), returns to step S301 and waits for data to enter the key frame queue. When MKF is 1 or more (step S304; Yes), the process proceeds to the following process.
地図作成部１１は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＭＫＦ−１のキーフレーム）の２Ｄ特徴点と現キーフレーム（キーフレーム番号がＭＫＦのキーフレーム）の２Ｄ特徴点とで対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ３０５）。フレームＤＢにはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、自機位置推定スレッドの初期化時の処理の時と同様の方法で対応特徴点の３Ｄ位置を計算できる。地図作成部１１は、３Ｄ位置が計算できた対応特徴点をＭａｐ点としてＭａｐ点ＤＢに登録する（ステップＳ３０６）。地図作成部１１は、フレームＤＢ中の他のキーフレームに対しても今回３Ｄ位置を計算できた２Ｄ特徴点に対して３Ｄ位置を登録する（ステップＳ３０７）。 The map creation unit 11 refers to the frame DB and identifies 2D feature points of the previous key frame (key frame whose key frame number is MKF-1) and 2D feature points of the current key frame (key frame whose key frame number is MKF). A 2D feature point (corresponding feature point) that can be corresponded with is extracted (step S305). Since the attitudes (translation vector t and rotation matrix R) of each key frame are also registered in the frame DB, the 3D position of the corresponding feature point is processed in the same manner as in the processing at the time of initialization of the own position estimation thread. Can be calculated. The map creation unit 11 registers the corresponding feature points for which the 3D position has been calculated as Map points in the Map point DB (step S306). The map creation unit 11 also registers the 3D position for the 2D feature point for which the 3D position could be calculated this time for other key frames in the frame DB (step S307).
なお、地図作成部１１が抽出した対応特徴点がすでにＭａｐ点ＤＢに登録済だった場合は、３Ｄ位置計算をスキップして次の対応特徴点（Ｍａｐ点ＤＢに未登録のもの）に対する処理に進んでも良いし、改めて３Ｄ位置計算を行って、Ｍａｐ点ＤＢに登録済の３Ｄ位置や、フレームＤＢ中の対応特徴点に対する３Ｄ位置を更新するようにしても良い。 If the corresponding feature point extracted by the map creation unit 11 has already been registered in the Map point DB, the 3D position calculation is skipped and the next corresponding feature point (not registered in the Map point DB) is processed. The process may proceed, or the 3D position calculation may be performed again to update the 3D position registered in the Map point DB or the 3D position for the corresponding feature point in the frame DB.
次に地図作成部１１は、自機位置推定スレッドの推定状態変数が「追跡成功」か否かを判定する（ステップＳ３０８）。「追跡成功」でなければ（ステップＳ３０８；Ｎｏ）、自機位置を推定できていないということなので、障害物や通信品質の測定をスキップしてステップＳ３１１へ進む。 Next, the map creation unit 11 determines whether or not the estimated state variable of the own device position estimation thread is “successful tracking” (step S308). If it is not “successful tracking” (step S308; No), it means that the position of the own device has not been estimated, and therefore the measurement of obstacles and communication quality is skipped and the process proceeds to step S311.
推定状態変数が「追跡成功」であれば（ステップＳ３０８；Ｙｅｓ）、地図作成部１１は、距離センサ３１で壁や障害物までの距離を測定し、ＶＯで示される自機の位置及び向きと測定した距離とに基づき、壁や障害物の位置や、自機位置から壁や障害物までの距離を地図記憶部２２に登録する（ステップＳ３０９）。
If the estimated state variable is “successful tracking” (step S308; Yes), the map creation unit 11 measures the distance to the wall or obstacle with the
そして、通信品質地図作成部１４は、通信品質測定部１３により通信品質を測定し、測定して得られた通信品質の値を、ＶＯで示される自機位置での通信品質の値として、通信品質地図記憶部２３に登録する（ステップＳ３１０）。
Then, the communication quality map creating unit 14 measures the communication quality by the communication
通信品質は、通信部４４で観測できる電界強度や受信電力を用いたり、実際に行っている通信のＲＴＴ（Ｒｏｕｎｄ−Ｔｒｉｐ Ｔｉｍｅ）や、実効速度、エラー率、遅延を用いたりして取得できる。これら様々な評価値を複合した評価値を用いても良い。また、赤外線や可視光の通信リンクを用いる場合などは、信号が到達したか否か（間に遮蔽物があるか否か）という二値の情報を用いても良い。既にその位置で通信品質を測定済みだった場合は、それまでの測定結果の平均値やメディアンを採用したり、最悪値を採用したりしても良い。また、平均値、メディアン、最良値、最悪値等を別々に保存しておき、自律移動装置１００の移動目的に応じて使い分けても良い。
The communication quality can be acquired by using the electric field strength and the received power that can be observed by the
通信品質地図記憶部２３に登録する値は、通信品質の良いほど小さい正の数（コスト値）になるように変換してから登録する。例えば、電界強度や実効速度は通信品質が良いほど大きくなるので、逆数を登録する等する。
The value registered in the communication quality
次に、地図作成部１１は、キーフレームキューが空かどうかを判定する（ステップＳ３１１）。空であれば（ステップＳ３１１；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３Ｄ位置に対して、バンドルアジャストメント処理を行って、精度向上を図る（ステップＳ３１２）。そして、バンドルアジャストメント処理を行った結果、誤差が大きいＭａｐ点が見つかったら、それをＭａｐ点ＤＢから削除し（ステップＳ３１３）、ステップＳ３１４に進む。キーフレームキューが空でなければ（ステップＳ３１１；Ｎｏ）、何もせずにステップＳ３１４に進む。 Next, the map creation unit 11 determines whether the key frame queue is empty (step S311). If it is empty (step S311; Yes), the bundle adjustment process is performed on the postures of all the key frames and the 3D positions of all the map points to improve the accuracy (step S312). When a Map point having a large error is found as a result of the bundle adjustment process, the Map point is deleted from the Map point DB (step S313), and the process proceeds to step S314. If the key frame queue is not empty (step S311; No), the process proceeds to step S314 without doing anything.
ステップＳ３１４では、地図作成部１１は、地図記憶部２２に記憶されている情報に基づき、地図兼経路コストマップを配列Ｓ（ｘ，ｙ）に格納する（ステップＳ３１４）。地図兼経路コストマップとは、座標（ｘ，ｙ）が移動不能領域（壁や障害物）なら「Ｓ（ｘ，ｙ）＝∞」、移動不能領域でなければ「Ｓ（ｘ，ｙ）＝座標（ｘ，ｙ）から移動不能領域までの最短距離が大きいほど小さい正数」となるように値が格納された配列である。例えば、座標（ｘ，ｙ）が移動不能領域でない場合、座標（ｘ，ｙ）から移動不能領域までの最短距離の逆数をＳ（ｘ，ｙ）に格納することができる。なお、配列Ｓは、地図記憶部２２に記憶される。
In step S314, the map creation unit 11 stores the map/route cost map in the array S(x, y) based on the information stored in the map storage unit 22 (step S314). The map/route cost map is “S(x,y)=∞” if the coordinates (x,y) are immovable areas (walls or obstacles), and “S(x,y)=” if they are not immovable areas. It is an array in which values are stored such that the larger the shortest distance from the coordinates (x, y) to the immovable area, the smaller the positive number. For example, when the coordinate (x, y) is not the immovable area, the reciprocal of the shortest distance from the coordinate (x, y) to the immovable area can be stored in S(x, y). The array S is stored in the
地図兼経路コストマップの配列Ｓ（ｘ，ｙ）の具体例を図７に示す。図７で、黒の領域は移動不能領域（壁や障害物）を示しており、この領域内はＳ（ｘ，ｙ）＝∞である。また白い領域は移動不能領域までの最短距離が大きい領域を示しており、この領域内のＳ（ｘ，ｙ）の値は小さい（例えば１未満）。そして、灰色の領域は移動不能領域までの最短距離が小さい領域を示しており、この領域内のＳ（ｘ，ｙ）の値は大きい（例えば１以上）。 FIG. 7 shows a specific example of the array S(x, y) of the map/route cost map. In FIG. 7, a black area indicates an immovable area (a wall or an obstacle), and S(x, y)=∞ in this area. The white area indicates an area where the shortest distance to the immovable area is large, and the value of S(x, y) in this area is small (for example, less than 1). The gray area indicates an area where the shortest distance to the immovable area is small, and the value of S(x, y) in this area is large (for example, 1 or more).
次に、通信品質地図作成部１４は、通信品質地図記憶部２３に記憶されている情報に基づき、通信品質コストマップを配列Ｃ（ｘ，ｙ）に格納する（ステップＳ３１５）。通信品質コストマップとは、座標（ｘ，ｙ）での通信品質が良いほど小さい正数がＣ（ｘ，ｙ）に格納された配列である。例えば、座標（ｘ，ｙ）での電界強度や実効速度の逆数をＣ（ｘ，ｙ）に格納することができる。なお、配列Ｃは通信品質地図記憶部２３に記憶される。
Next, the communication quality map creation unit 14 stores the communication quality cost map in the array C(x, y) based on the information stored in the communication quality map storage unit 23 (step S315). The communication quality cost map is an array in which a smaller positive number is stored in C(x, y) as the communication quality at the coordinates (x, y) is better. For example, the electric field strength at the coordinates (x, y) and the reciprocal of the effective velocity can be stored in C(x, y). The array C is stored in the communication quality
通信品質コストマップを配列Ｃ（ｘ，ｙ）の具体例を図８に示す。図８の左端に外部の通信機２００（図示せず）が存在している。図８で、白の領域は通信品質が良好な領域を示しており、この領域内のＣ（ｘ，ｙ）は小さい（例えば１未満）。そして、薄い灰色の領域は通信品質が悪い領域を示しており、この領域内のＣ（ｘ，ｙ）の値は比較的大きい（例えば１以上３未満）。濃い灰色の領域は通信できないほど通信品質が悪い領域を示しており、この領域内のＣ（ｘ，ｙ）の値はとても大きい（例えば３以上）。 A concrete example of the communication quality cost map of the array C(x, y) is shown in FIG. An external communication device 200 (not shown) is present at the left end of FIG. In FIG. 8, a white area indicates an area with good communication quality, and C(x, y) in this area is small (for example, less than 1). A light gray area indicates an area with poor communication quality, and the value of C(x, y) in this area is relatively large (for example, 1 or more and less than 3). The dark gray area indicates an area in which the communication quality is so bad that communication cannot be performed, and the value of C(x, y) in this area is very large (for example, 3 or more).
自律移動装置１００が通過する座標（ｘ，ｙ）は一般に疎なので、Ｓ（ｘ，ｙ）もＣ（ｘ，ｙ）も、まばらにしか値が設定されていない状態になることが多い。そこで、内挿・外挿を行って、各座標（ｘ，ｙ）に密に値を設定する。
Since the coordinates (x, y) that the autonomous
次に、地図作成部１１は、ループクロージングスレッドのキーフレームキューにＭＫＦをセットして（ステップＳ３１６）、ステップＳ３０１に戻る。 Next, the map creation unit 11 sets MKF in the key frame queue of the loop closing thread (step S316), and returns to step S301.
以上の処理で作成した地図兼経路コストマップの配列Ｓ（ｘ，ｙ）及び通信品質コストマップの配列Ｃ（ｘ，ｙ）は、いずれもスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。 The map/route cost map array S(x, y) and the communication quality cost map array C(x, y) created by the above processing are both stored so that the values can be referred to across threads. It is stored in the unit 20.
なお、バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とＭａｐ点の３Ｄ位置とを同時に推定する非線形最適化法であり、Ｍａｐ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。 Note that the bundle adjustment process is a non-linear optimization method that simultaneously estimates the camera posture (key frame posture) and the 3D position of the Map point, and the error that occurs when the Map point is projected on the key frame. The optimization is performed so as to minimize it.
このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３Ｄ位置の精度向上を図ることができる。しかし、この処理は計算量が多いことと、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、ここでは、キーフレームが空の状態の時（他の処理が比較的軽い状態の時）のみバンドルアジャストメント処理を行うようにしている。通信品質が良い場合は、自律移動装置１００自身でバンドルアジャストメント処理をするのではなく、画像記憶部２１や地図記憶部２２の情報を通信部４４で外部の通信機２００に送信し、外部の通信機２００でバンドルアジャストメント処理した結果を自律移動装置１００に送り返してもらうようにしても良い。
By performing the process of the bundle adjustment, it is possible to improve the accuracy of the key frame posture and the 3D position of the Map point. However, this process does not cause a special problem because the amount of calculation is large and the accuracy cannot be improved without this process. Therefore, here, the bundle adjustment process is performed only when the key frame is empty (when other processes are relatively light). If the communication quality is good, the autonomous
また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいＭａｐ点が見つかることがある。このような誤差の大きなＭａｐ点については、ＳＬＡＭ推定に悪影響を及ぼすため、ステップＳ３１３ではＭａｐ点ＤＢから削除している。なお、Ｍａｐ点ＤＢから削除するのではなく、Ｍａｐ点ＤＢにおいて、誤差の大きな注意を要するＭａｐ点であることを識別するためのフラグを立てておいても良い。 Further, when the bundle adjustment process is performed, a Map point having an error when projected on the key frame is larger than a predetermined value may be found. Map points having such a large error have an adverse effect on SLAM estimation, and are therefore deleted from the map point DB in step S313. It should be noted that instead of deleting from the Map point DB, a flag may be set in the Map point DB to identify a Map point requiring a great deal of error.
次に自律移動装置１００のメインフロー（図４）のステップＳ１０４で起動されるループクロージングスレッドについて、図９を参照して説明する。このスレッドでは制御部１０は、ループクロージング処理ができるかどうかをチェックし続け、ループクロージング処理ができる場合にはループクロージング処理を行っている。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの軌跡中のキーフレームや、関連するＭａｐ点の３Ｄ位置を修正することをいう。 Next, the loop closing thread started in step S104 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. In this thread, the control unit 10 continues to check whether the loop closing process can be performed, and when the loop closing process can be performed, the control unit 10 performs the loop closing process. Note that the loop closing process uses the deviation between the posture value when the user was at this same place before and the value of the current posture when he/she recognizes that he has returned to the same place where he has come before. , It means to correct the 3D position of the key frame and the related Map point in the locus from the time when it came before until now.
まず、制御部１０は、動作終了かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ４０１；Ｎｏ）キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空なら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、キーフレームキューが空でないなら（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＬＫＦ（ループクロージングスレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。次に制御部１０は、ＬＫＦが１より大きいかを判定する（ステップＳ４０４）。ＬＫＦが０又は１である場合（ステップＳ４０４；Ｎｏ）はステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。そして、ＬＫＦが２以上の場合（ステップＳ４０４；Ｙｅｓ）は、以下の処理を行う。 First, the control unit 10 determines whether the operation is completed (step S401). If the operation ends (step S401; Yes), the operation ends. If the operation is not completed (step S401; No), it is determined whether the key frame queue is empty (step S402). If the key frame queue is empty (step S402; Yes), the procedure returns to step S401. If the key frame queue is not empty (step S402; No), data is extracted from the key frame queue and the LKF (loop closing thread processes the key frame It is set to a variable representing the key frame number) (step S403). Next, the control unit 10 determines whether LKF is larger than 1 (step S404). If LKF is 0 or 1 (step S404; No), the process returns to step S401 and waits for data to enter the key frame queue. When LKF is 2 or more (step S404; Yes), the following processing is performed.
制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が所定の類似度（例えば０．９。以下「基準画像類似度」という。）以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。ここで、この類似度は、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、二つの画像の特徴ベクトルの絶対値（要素の二乗和の平方根）を１に正規化したもの同士の内積を、その二つの画像の類似度とすることができる。また、二つの画像の特徴ベクトル（絶対値を１に正規化したもの）の距離（各要素の差の二乗和の平方根）の逆数を類似度としても良い。 The control unit 10 refers to the frame DB, and the similarity between the current key frame (key frame whose key frame number is LKF) and the “feature of the key frame itself” is a predetermined similarity (for example, 0.9. Hereinafter, “reference image”). A key frame having the above degree is searched from the frame DB (step S405). Here, when the feature of an image (key frame) is represented by a feature vector, this similarity is obtained by normalizing the absolute values of the feature vectors of two images (square root of sum of squares of elements) to 1. The inner product of can be the similarity of the two images. Alternatively, the reciprocal of the distance (square root of the sum of squares of the differences between the elements) between the feature vectors (absolute values normalized to 1) of the two images may be used as the similarity.
制御部１０は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたかどうかを判定し（ステップＳ４０６）、発見されなければ（ステップＳ４０６；Ｎｏ）ステップＳ４０１へ戻り、発見されたら（ステップＳ４０６；Ｙｅｓ）、発見されたキーフレームから現キーフレームまでの軌跡中のキーフレームの姿勢と、軌跡中のキーフレームに含まれるＭａｐ点の３Ｄ位置を修正する（ステップＳ４０７）。例えば、制御部１０は、現キーフレームの姿勢を、発見されたキーフレームの姿勢と同じ姿勢として修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるＭａｐ点の３Ｄ位置についても各キーフレームの姿勢の補正量に応じて修正する。そしてステップＳ４０１に戻る。 The control unit 10 determines whether or not a key frame in which the similarity of the “feature of the key frame itself” is equal to or higher than the reference image similarity is found (step S406), and if not found (step S406; No) step S401. Returning to step S406, if found (step S406; Yes), the posture of the key frame in the locus from the found key frame to the current key frame and the 3D position of the Map point included in the key frame in the locus are corrected ( Step S407). For example, the control unit 10 corrects the attitude of the current key frame to be the same as the attitude of the discovered key frame. Then, using the difference between the discovered attitude of the key frame and the attitude of the current key frame, the attitude of each key frame in the locus from the discovered key frame to the current key frame is linearly corrected. Furthermore, the 3D position of the Map point included in each of these key frames is also corrected according to the correction amount of the posture of each key frame. Then, the process returns to step S401.
以上のループクロージング処理は、計算量が多いため、通信品質が良い場合は、自律移動装置１００自身でループクロージング処理をするのではなく、画像記憶部２１や地図記憶部２２の情報を通信部４４で外部の通信機２００に送信し、外部の通信機２００でループクロージング処理した結果を自律移動装置１００に送り返してもらうようにしても良い。
Since the above loop closing process requires a large amount of calculation, when the communication quality is good, the autonomous
次に自律移動装置１００のメインフロー（図４）のステップＳ１１１の処理である目的地への経路計画について説明する。従来の経路計画は、地図兼経路コストマップである配列Ｓ（ｘ，ｙ）、現在位置及び目的地位置を与えると、現在位置から目的地位置への最小累積コスト経路を選択するものである。具体的には、各座標をノード、隣接関係をエッジとし、ノード乃至エッジに配列Ｓ（ｘ，ｙ）に従った重みをつけたグラフを構成すれば、Ｄｉｊｋｓｔｒａアルゴリズムなどグラフ理論の最短経路探索のアルゴリズムを用いて効率良く最小累積コスト経路を選択することができる。本発明では、配列Ｓ（ｘ，ｙ）だけでなく、通信品質マップである配列Ｃ（ｘ，ｙ）も用いることで、通信品質を保つような経路を選択できる。 Next, the route planning to the destination, which is the process of step S111 of the main flow of the autonomous mobile device 100 (FIG. 4), will be described. In the conventional route planning, when an array S(x, y) which is a map and route cost map, a current position and a destination position are given, the minimum accumulated cost route from the current position to the destination position is selected. Specifically, if each graph is constructed by assigning each coordinate as a node, an adjacency relation as an edge, and weighting nodes or edges according to the array S(x, y), the shortest path search of the graph theory such as Dijkstra algorithm can be performed. An algorithm can be used to efficiently select the minimum cumulative cost path. In the present invention, not only the array S(x, y) but also the array C(x, y) which is a communication quality map is used, so that a route that maintains the communication quality can be selected.
（第１の実施形態）
では、目的地への経路計画の第１の実施形態について、図１０を参照して説明する。まず経路探索部１５は、閾値の値を通信品質が良好と考えられる場合の値に設定する（ステップＳ５０１）。次に、各配列の全ての座標について値を更新するために、ｘとｙを０に初期化する（ステップＳ５０２）。
(First embodiment)
Now, a first embodiment of the route planning to the destination will be described with reference to FIG. First, the
次に、経路探索部１５は、地図兼経路コストマップである配列Ｓの（ｘ，ｙ）の値を、経路探索用コストマップである配列Ｔ（ｘ，ｙ）に代入する（ステップＳ５０３）。配列Ｔは、経路を探索する際に、従来の地図兼経路コストマップＳの代わりに用いる配列で、経路探索用地図記憶部２４に記憶される。
Next, the
次に経路探索部１５は、通信品質マップである配列Ｃの（ｘ，ｙ）の値が閾値より大きいか否かを判定する（ステップＳ５０４）。閾値よりも大きければ（ステップＳ５０４；Ｙｅｓ）、座標（ｘ，ｙ）での通信品質が良好でないことを意味するので、その場所を通らないようにするために、Ｔ（ｘ，ｙ）に∞を設定し（ステップＳ５０５）、ステップＳ５０６に進む。閾値以下なら（ステップＳ５０４；Ｎｏ）、その場所（ｘ，ｙ）での通信品質は良好ということになるので、Ｔ（ｘ，ｙ）の値はＳ（ｘ，ｙ）のまま変更せずにステップＳ５０６へ進む。
Next, the
ステップＳ５０６では、経路探索部１５は、全てのｘ，ｙについて、ステップＳ５０３〜ステップＳ５０５の処理が完了したか否かを判定する（ステップＳ５０６）。完了していなければ（ステップＳ５０６；Ｎｏ）、ステップＳ５０３に戻る。
In step S506, the
全てのｘ，ｙについて、ステップＳ５０３〜ステップＳ５０５の処理が完了したら（ステップＳ５０６；Ｙｅｓ）、経路探索用コストマップである配列Ｔ（ｘ，ｙ）の設定が完了したことになるので、経路探索部１５は、Ｔに従来の経路計画のアルゴリズムを適用して、経路探索を行う（ステップＳ５０７）。
When the processes of steps S503 to S505 have been completed for all x and y (step S506; Yes), it means that the setting of the array T(x, y) which is the route search cost map has been completed. The
そして、経路探索部１５は、ステップＳ５０７で経路を見つけることができたか否かを判定する（ステップＳ５０８）。経路が見つかったら（ステップＳ５０８；Ｙｅｓ）、終了する。経路が存在しない場合は（ステップＳ５０８；Ｎｏ）、経路探索部１５は閾値を上げて（ステップＳ５０９）、ステップＳ５０２に戻り、リトライを繰り返す。
Then, the
なお、ステップＳ５０９で閾値の値が、通信品質が「通信不可能」を示すほど大きな値になった場合は、経路探索をあきらめて終了する。フローチャートが煩雑になるため、図には示していないが、この場合は、メインフロー（図４）のステップＳ１１０での目的地の設定からやり直す必要がある。 In addition, when the value of the threshold value is so large as to indicate that the communication quality is “incommunicable” in step S509, the route search is abandoned and terminated. Although not shown in the figure because the flow chart becomes complicated, in this case, it is necessary to start over from the setting of the destination in step S110 of the main flow (FIG. 4).
なお、この処理を行う前に、通信品質マップである配列Ｃ（ｘ，ｙ）が示す通信品質が悪い領域（図８で薄い灰色や濃い灰色の領域）を膨張、収縮等させておいてもよい。より確実に通信を行いたい場合は通信品質が悪い領域を膨張させておく。また、移動中の通信の瞬断を許容して可動域を広く取りたい場合は通信品質が悪い領域を収縮させておく。 It should be noted that, before performing this processing, it is possible to expand, contract, etc. the area of poor communication quality (area of light gray or dark gray in FIG. 8) indicated by the array C(x, y) which is the communication quality map. Good. If you want to communicate more reliably, expand the area with poor communication quality. In addition, when it is desired to allow a wide range of motion by allowing momentary interruption of communication during movement, a region with poor communication quality is contracted.
経路探索の具体例を図７、図８及び図１１を参照して説明する。例えば、図７で示される地図兼経路コストマップＳ（ｘ，ｙ）を用いて、図１１に示す現在位置から目的位置までの経路を従来の方法で探索すると、図１１（ａ）の矢印で示す経路が選択される。これに対し、図８で示される通信品質コストマップＣ（ｘ，ｙ）を上述の第１の実施形態に適用すると、図８の白い部分以外は通信品質が悪いので、図１１（ｂ）に示す経路探索用コストマップＴ（ｘ，ｙ）が得られ、この図で現在位置から目的位置までの経路を探索すれば、図１１（ｂ）の点線の矢印で示す経路が選択される。このように、通信品質を考慮した経路探索用コストマップを用いることで、通信が切断されない経路をたどって目的地に移動することができる。 A specific example of the route search will be described with reference to FIGS. 7, 8 and 11. For example, when the route from the current position to the destination position shown in FIG. 11 is searched by the conventional method using the map/route cost map S(x, y) shown in FIG. 7, the arrow in FIG. The route shown is selected. On the other hand, if the communication quality cost map C(x, y) shown in FIG. 8 is applied to the above-described first embodiment, the communication quality is poor except for the white part in FIG. The route search cost map T(x, y) shown is obtained, and if the route from the current position to the destination position is searched in this figure, the route shown by the dotted arrow in FIG. 11B is selected. As described above, by using the route search cost map in consideration of the communication quality, it is possible to follow the route in which the communication is not disconnected and move to the destination.
（第２の実施形態）
次に、一定時間（許容されるレイテンシ（ｌａｔｅｎｃｙ））通信ができない状態を許容する第２の実施形態について、説明する。例えば、自律移動装置１００が、その実行するタスクとして、人間との自然言語会話を行っていて、自然言語会話を行うために必要な音声認識処理を外部の通信機２００に処理させているとすると、許容されるレイテンシは、その自然言語会話中の沈黙期間の延長が許される時間である。例えば、この場合、レイテンシを２秒と設定することができる。また、自律移動装置１００が実行するタスクの内容によって、外部の通信機２００と通信するデータ量が変わるが、このデータ量の通信に必要な帯域を満たす通信品質を閾値に設定する。
(Second embodiment)
Next, a second embodiment will be described in which a state in which communication cannot be performed for a fixed time (allowable latency) is allowed. For example, suppose that the autonomous
では、第２の実施形態について、図１２を参照して説明する。まず、経路探索部１５は、自律移動装置１００が実行するタスクの内容に基づき、許容レイテンシを変数ＴＬに設定する（ステップＳ６０１）。このステップにおいて、経路探索部１５は許容レイテンシ設定部に相当する。次に、経路探索部１５は、自律移動装置１００が実行するタスクの内容に基づき、通信品質の閾値を設定する（ステップＳ６０２）。そして、経路探索部１５は、地図兼経路コストマップである配列Ｓの（ｘ，ｙ）の値を、経路探索用コストマップである配列Ｔ（ｘ，ｙ）に代入する（ステップＳ６０３）。
Now, the second embodiment will be described with reference to FIG. First, the
次に、経路探索部１５は、経路探索用コストマップＴに従来の経路計画のアルゴリズムを適用して経路探索を行い、得られた経路を仮経路Ｌとする（ステップＳ６０４）。この処理において、経路探索部１５は経路候補探索部に相当する。もしここで経路が得られなかった場合は、経路探索をあきらめて終了する。フローチャートが煩雑になるため、図には示していないが、この場合は、メインフロー（図４）のステップＳ１１０での目的地の設定からやり直す必要がある。
Next, the
次に、経路探索部１５は、仮経路Ｌを現在位置から目的位置まで順にたどって、Ｃ（ｘ，ｙ）＞閾値となる区間（仮経路Ｌをたどる際に通信品質が足りない区間）の長さに対して、その長さを通過するのに必要な通過所要時間（以下、ＴＴで表す）を算出し、ＴＬ＜ＴＴか否かを判定する（ステップＳ６０５）。ＴＴの算出は、簡単には、仮経路Ｌの経路を一つたどるたびに、その一つ分の経路の長さを自律移動装置１００の移動速度で割った値を足していけば良い。ステップＳ６０５において、経路探索部１５は通信不良時間取得部に相当する。
Next, the
仮経路Ｌを現在位置からたどった際に、ＴＬ＜ＴＴとなるような区間無しに目的位置までたどり着けたら（ステップ６０５；Ｎｏ）、仮経路Ｌを本経路に採用し（ステップＳ６０７）、終了する。 When the provisional route L is reached from the current position to the target position without a section where TL<TT (step 605; No), the provisional route L is adopted as the main route (step S607), and the process ends. ..
仮経路Ｌをたどった際に、ＴＬ＜ＴＴとなるような区間が見つかったら（ステップ６０５；Ｙｅｓ）、仮経路Ｌは採用できないということになるので、仮経路Ｌを現在位置からＴＬ＜ＴＴとなる地点までたどる区間上の点の周辺（当該区間を所定のマージン幅だけ拡大した領域）のＴ（ｘ，ｙ）に∞を代入し（ステップＳ６０６）、ステップＳ６０４に戻る。当該区間の周辺領域のＴ（ｘ，ｙ）に∞を代入することで、次回以降の経路探索において、その区間を経路に選択しないようにしている。 If a section that satisfies TL<TT is found when tracing the tentative route L (step 605; Yes), it means that the tentative route L cannot be adopted. Therefore, the tentative route L is set to TL<TT from the current position. ∞ is substituted for T(x, y) around the point on the section that traces to the point (area in which the section is enlarged by a predetermined margin width) (step S606), and the process returns to step S604. By substituting ∞ for T(x, y) in the peripheral region of the section, the section is not selected as a route in the route search from the next time onward.
この第２の実施形態での経路探索の具体例を図７、図８、図１３及び図１４を参照して説明する。例えば、図７で示される地図兼経路コストマップＳ（ｘ，ｙ）と、図８で示される通信品質コストマップを用いて図１２で示される経路計画の処理を行うと、ステップＳ６０４において、図１３（ａ）の矢印で示される仮経路Ｌが得られる。そして、ステップＳ６０５で、図１３（ｂ）の薄い灰色部分を矢印に沿って通過する時間ＴＴが、許容レイテンシＴＬ以下であったなら、仮経路Ｌが本経路として採用される。 A specific example of the route search in the second embodiment will be described with reference to FIGS. 7, 8, 13 and 14. For example, when the route planning process shown in FIG. 12 is performed using the map/route cost map S(x, y) shown in FIG. 7 and the communication quality cost map shown in FIG. A temporary route L indicated by the arrow 13(a) is obtained. Then, in step S605, if the time TT for passing through the light gray portion of FIG. 13B along the arrow is less than or equal to the allowable latency TL, the provisional route L is adopted as the main route.
図１３（ｂ）の薄い灰色部分を矢印に沿って通過する途中で時間ＴＴが、許容レイテンシＴＬを超えてしまったら、ステップＳ６０６で、図１４（ａ）に示されるように、仮経路Ｌの矢印の途中まで（ＴＴがＴＬを超えた位置まで）のＴ（ｘ，ｙ）の周辺の領域が∞（図では黒塗り）に設定される。そして、ステップＳ６０４に戻って、再度新たな仮経路Ｌを求める。この新たな仮経路Ｌは、図１４（ａ）の点線矢印で表している。 If the time TT exceeds the allowable latency TL while passing through the light gray portion of FIG. 13B along the arrow, in step S606, as shown in FIG. The area around T(x, y) up to the middle of the arrow (to the position where TT exceeds TL) is set to ∞ (black in the figure). Then, returning to step S604, a new temporary route L is obtained again. This new temporary route L is indicated by a dotted arrow in FIG.
この点線矢印の経路をたどっても、途中で時間ＴＴが、許容レイテンシＴＬを超えてしまうので、ステップＳ６０６で、図１４（ｂ）に示されるように、仮経路Ｌの点線矢印の途中まで（ＴＴがＴＬを超えた位置まで）のＴ（ｘ，ｙ）の周辺の領域が∞（図では黒塗り）に設定される。そして、ステップＳ６０４に戻って、再度新たな仮経路Ｌを求める。この新たな仮経路Ｌは、図１４（ｂ）の点線矢印で表している。 Even if the path of the dotted arrow is followed, the time TT exceeds the allowable latency TL on the way, so in step S606, as shown in FIG. The area around T(x, y) at the position where TT exceeds TL) is set to ∞ (painted black in the figure). Then, returning to step S604, a new temporary route L is obtained again. This new provisional route L is indicated by a dotted arrow in FIG.
この点線矢印の経路は、通信品質が良好な部分のみを通るので、この経路が本経路として採用される。 The route indicated by the dotted arrow passes only the portion with good communication quality, and thus this route is adopted as the main route.
以上、図１２のような処理を行うことで、通信品質の悪い領域があっても、そこを通過する時間が許容レイテンシ以下であれば、そこを通過する経路をたどることができるようになる。目的地への移動に、通信品質の悪い領域を通らざるを得ない場合、第１の実施形態では目的地への移動が不可能だったが、第２の実施形態では許容レイテンシ以下の時間でその領域を通過できる場合は、目的地の移動が可能になる。 As described above, by performing the processing shown in FIG. 12, even if there is an area with poor communication quality, it is possible to follow a path that passes therethrough if the time for passing therethrough is less than the allowable latency. In the case where there is no choice but to pass through a region with poor communication quality to move to the destination, the move to the destination was impossible in the first embodiment, but in the second embodiment it took less time than the allowable latency. If the area can be passed, the destination can be moved.
例えば、音声認識のもとになる音声データは、通信が途絶えている間はバッファしておき、通信品質が回復した後に外部機器に送信して処理を行うようにすれば、許容レイテンシを守って処理を継続できる。また、自律移動装置１００のタスクが自然言語会話の場合、自然言語会話において沈黙期間の延長が許される時間（例えば２秒）を許容レイテンシとして設定しておけば、通信品質の悪い領域を通過せざるを得ない場合でも自然言語会話を継続できる。
For example, voice data, which is the basis of voice recognition, should be buffered while communication is interrupted and transmitted to an external device for processing after communication quality is restored, in order to protect the allowable latency. Processing can continue. Further, when the task of the autonomous
なお、自律移動装置１００の速度を現在速度より上げることができる場合、ステップＳ６０５でＴＴ＜ＴＬとなった場合でも、すぐにはステップＳ６０６に進まず、その環境での最大速度で移動した場合のＴＴを計算し直し、計算し直したＴＴならＴＬ以下になる場合は（ステップＳ６０５；Ｎｏ）、経路探索部１５は、仮経路Ｌを本経路とし（ステップＳ６０７）、制御部１０は駆動部４２に最大速度を設定して移動を指示するようにしても良い。この処理において、制御部１０は、速度設定部に相当する。このような処理をすることにより、通常の速度では許容レイテンシ以下の時間で通信品質の悪い領域を通過できない場合でも、移動速度を上げて通過させることができる。したがって、通常速度では目的地に到達できない状況下でも、速度を上げることにより、許容レイテンシを守って目的地へ到達できるようになる。
If the speed of the autonomous
また、上述した実施形態では、目的地への経路計画を行う時のみ通信品質を考慮したが、メインフロー（図４）のステップＳ１０９での自由な自律移動の際も、制御部１０が通信品質コストマップＣ（ｘ，ｙ）を参照して、通信品質が悪いところには長時間滞在しないように移動制御をしても良い。このようにすることによって、目的地の位置や目的地の設定の有無によらず、外部の通信機との通信を良好に保つことができる。 Further, in the above-described embodiment, the communication quality is considered only when the route planning to the destination is performed, but the control unit 10 controls the communication quality during the free autonomous movement in step S109 of the main flow (FIG. 4). By referring to the cost map C(x, y), movement control may be performed so as not to stay for a long time in a place where communication quality is poor. By doing so, it is possible to maintain good communication with an external communication device regardless of the position of the destination or whether or not the destination is set.
なお、この発明の自律移動装置１００の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）及びＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ Ｄｉｓｃ）等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。
Each function of the autonomous
以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。 Although the preferred embodiment of the present invention has been described above, the present invention is not limited to the specific embodiment, and the present invention includes the invention described in the claims and an equivalent range thereof. Be done. The inventions described in the initial claims of the present application will be additionally described below.
（付記１）
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した自機位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記通信品質地図作成部が作成した通信品質地図を用いて、目的地までの経路を探索する経路探索部と、
を備える自律移動装置。
(Appendix 1)
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the own device using information of a plurality of images captured by the imaging unit,
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the own position estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
Using the communication quality map created by the communication quality map creation unit, a route search unit that searches for a route to a destination,
An autonomous mobile device comprising.
（付記２）
前記経路探索部は、前記通信品質地図作成部が作成した通信品質地図において、通信品質が所定の閾値より低い領域である通信不良領域を通らない経路を探索する、
付記１に記載の自律移動装置。
(Appendix 2)
In the communication quality map created by the communication quality map creation unit, the route search unit searches for a route that does not pass through a poor communication area where the communication quality is lower than a predetermined threshold value.
The autonomous mobile device according to
（付記３）
さらに、前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部を備え、
前記経路探索部は、前記通信不良時間取得部が取得した通信不良時間が許容レイテンシを超える場合には前記通信不良領域を通らない経路を探索し、前記通信不良時間が前記許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索する、
付記１に記載の自律移動装置。
(Appendix 3)
Furthermore, a communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the route searched by the route search unit,
The route search unit searches for a route that does not pass through the communication failure area when the communication failure time acquired by the communication failure time acquisition unit exceeds the allowable latency, and when the communication failure time is less than or equal to the allowable latency. Searches for a route through the poor communication area,
The autonomous mobile device according to
（付記４）
さらに、自機のタスク内容に基づいて前記許容レイテンシを設定する許容レイテンシ設定部を備える、
付記３に記載の自律移動装置。
(Appendix 4)
Furthermore, an allowable latency setting unit that sets the allowable latency based on the task content of the own device is provided.
The autonomous mobile device according to
（付記５）
さらに、自機の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、自律移動装置が最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が最大速度で移動した場合の前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は自機の速度を最大速度に設定する、
付記３又は４に記載の自律移動装置。
(Appendix 5)
Furthermore, it is equipped with a speed setting unit that sets the speed of its own machine,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at the maximum speed,
When the communication failure time when the autonomous mobile device moves at the maximum speed is less than or equal to the allowable latency, the route search unit searches for a route passing through the communication failure region, and the speed setting unit Set speed to maximum speed,
The autonomous mobile device according to
（付記６）
前記経路探索部は、前記目的地が設定されていない場合でも、前記通信品質地図作成部が作成した通信品質地図において、通信品質が所定の閾値より低い領域である通信不良領域を避ける経路を設定する、
付記１に記載の自律移動装置。
(Appendix 6)
Even if the destination is not set, the route search unit sets a route that avoids a poor communication area where the communication quality is lower than a predetermined threshold value in the communication quality map created by the communication quality map creation unit. To do
The autonomous mobile device according to
（付記７）
付記１から６のいずれか一つに記載の自律移動装置と、
前記自律移動装置と通信する外部の通信機と、
を備える自律移動システム。
(Appendix 7)
The autonomous mobile device according to any one of
An external communication device that communicates with the autonomous mobile device;
An autonomous mobile system equipped with.
（付記８）
外部の通信機と通信を行いながら自律的に移動するための自律移動方法であって、
前記通信機と通信する際の通信品質に応じて経路を探索する、
自律移動方法。
(Appendix 8)
An autonomous moving method for moving autonomously while communicating with an external communication device,
A route is searched according to the communication quality when communicating with the communication device,
Autonomous movement method.
（付記９）
前記通信機と通信する際の通信品質が所定の閾値より低い領域である通信不良領域を通らない経路を探索する、
付記８に記載の自律移動方法。
(Appendix 9)
Search for a route that does not pass through a poor communication area, which is an area in which communication quality when communicating with the communication device is lower than a predetermined threshold value,
The autonomous moving method according to attachment 8.
（付記１０）
前記通信機と通信する際の通信品質が所定の閾値より低い通信不良領域を許容レイテンシ以下の時間しか通らない経路を探索する、
付記８に記載の自律移動方法。
(Appendix 10)
Searching for a route in which the communication quality when communicating with the communication device passes through a communication failure area lower than a predetermined threshold value only for a time equal to or less than the allowable latency,
The autonomous moving method according to attachment 8.
（付記１１）
目的地が設定されていない場合でも、前記通信機と通信する際の通信品質が所定の閾値より低い領域である通信不良領域を避ける経路を設定する、
付記８に記載の自律移動方法。
(Appendix 11)
Even if the destination is not set, set a route to avoid a poor communication area, which is an area where the communication quality when communicating with the communication device is lower than a predetermined threshold value,
The autonomous moving method according to attachment 8.
（付記１２）
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定ステップと、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップと、
前記位置推定ステップで推定した自機位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップと、
前記通信品質地図作成ステップで作成した通信品質地図を用いて、目的地までの経路を探索する経路探索ステップと、
を備える自律移動方法。
(Appendix 12)
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimating step of estimating the own position using information of a plurality of images captured by the image capturing unit;
A communication quality measuring step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the own position estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
Using the communication quality map created in the communication quality map creating step, a route searching step of searching for a route to a destination,
An autonomous moving method comprising.
（付記１３）
コンピュータに、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップ、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定ステップ、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップ、
前記位置推定ステップで推定した自機位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップ、
前記通信品質地図作成ステップで作成した通信品質地図を用いて、目的地までの経路を探索する経路探索ステップ、
を実行させるためのプログラム。
(Appendix 13)
On the computer,
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the own position using information of a plurality of images captured by the imaging unit,
Communication quality measurement step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the own position estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
Using the communication quality map created in the communication quality map creating step, a route searching step for searching for a route to a destination,
A program to execute.
１００…自律移動装置、１０…制御部、１１…地図作成部、１２…位置推定部、１３…通信品質測定部、１４…通信品質地図作成部、１５…経路探索部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…通信品質地図記憶部、２４…経路探索用地図記憶部、３０…センサ部、３１…距離センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源、２００…通信機 100... Autonomous mobile device, 10... Control part, 11... Map creation part, 12... Position estimation part, 13... Communication quality measurement part, 14... Communication quality map creation part, 15... Route search part, 20... Storage part, 21 ... image storage unit, 22... map storage unit, 23... communication quality map storage unit, 24... route search map storage unit, 30... sensor unit, 31... distance sensor, 41... imaging unit, 42... drive unit, 43... Input unit, 44... Communication unit, 45... Power supply, 200... Communication device
Claims (6)
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した前記自律移動装置の位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記地図作成部が作成した地図と、前記通信品質地図作成部が作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索部と、
前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部と、を備え、
前記経路探索部は、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得部が取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索部は、前記通信不良領域を通らない経路を探索する、
自律移動装置。 An autonomous mobile device,
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit;
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the position of the autonomous mobile device estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
A route search unit that searches for a route to a destination using the map created by the map creation unit and the communication quality map created by the communication quality map creation unit,
A communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched by the path search unit,
The route search unit searches for a route through the communication failure area when the communication failure time is equal to or less than the allowable latency,
Furthermore, a speed setting unit for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route search unit searches for a route passing through the defective communication area, the speed setting unit sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition unit, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The search unit searches for a route that does not pass through the poor communication area.
Autonomous mobile device.
請求項１に記載の自律移動装置。 Furthermore, an allowable latency setting unit that sets the allowable latency based on the task content of the autonomous mobile device is provided.
The autonomous mobile device according to claim 1 .
請求項１又は２に記載の自律移動装置。 The route search unit, even if the destination has not been set, the communication quality map the communication quality mapping unit created, it sets the path to avoid the communication failure region,
Autonomous mobile apparatus according to claim 1 or 2.
前記自律移動装置と通信する外部の通信機と、
を備える自律移動システム。 The autonomous mobile device according to any one of claims 1 to 3 ,
An external communication device that communicates with the autonomous mobile device;
An autonomous mobile system equipped with.
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定ステップと、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップと、
前記位置推定ステップで推定した前記自律移動装置の位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップと、
前記地図作成ステップで作成した地図と、前記通信品質地図作成ステップで作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索ステップと、
前記経路探索ステップで探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得ステップと、を備え、
前記経路探索ステップは、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定ステップを備え、
前記通信不良時間取得ステップは、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索ステップは前記通信不良領域を通る経路を探索し、前記速度設定ステップは前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得ステップが取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索ステップは、前記通信不良領域を通らない経路を探索する、
自律移動方法。 An autonomous moving method of an autonomous moving device, comprising:
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit,
A communication quality measuring step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the position of the autonomous mobile device estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
A route search step for searching a route to a destination using the map created in the map creating step and the communication quality map created in the communication quality map creating step;
A communication failure time acquisition step of acquiring a communication failure time which is a time required to pass through a communication failure area which is an area in which communication quality is lower than a predetermined threshold in the path searched in the path searching step,
The route search step searches for a route passing through the communication failure area when the communication failure time is equal to or less than an allowable latency,
Furthermore, a speed setting step for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition step acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route searching step searches for a route passing through the communication defective area, the speed setting step sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition step, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The searching step searches for a route that does not pass through the poor communication area.
Autonomous movement method.
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップ、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定ステップ、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップ、
前記位置推定ステップで推定した前記自律移動装置の位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップ、
前記地図作成ステップで作成した地図と、前記通信品質地図作成ステップで作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索ステップ、
前記経路探索ステップで探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得ステップ、を実行させ、
前記経路探索ステップは、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定ステップを前記コンピュータに実行させ、
前記通信不良時間取得ステップは、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索ステップは前記通信不良領域を通る経路を探索し、前記速度設定ステップは前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得ステップが取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索ステップは、前記通信不良領域を通らない経路を探索する、
プログラム。 In the computer of the autonomous mobile device ,
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit,
Communication quality measurement step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the position of the autonomous mobile device estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
A route search step of searching for a route to a destination using the map created in the map creation step and the communication quality map created in the communication quality map creation step;
A communication failure time acquisition step of acquiring a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched in the path search step,
The route searching step searches for a route passing through the communication failure area when the communication failure time is equal to or less than an allowable latency,
Further, causing the computer to execute a speed setting step for setting the speed of the autonomous mobile device,
The communication failure time acquisition step acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route searching step searches for a route passing through the communication defective area, the speed setting step sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition step, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The searching step searches for a route that does not pass through the poor communication area.
Program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016049544A JP6724439B2 (en) | 2016-03-14 | 2016-03-14 | Autonomous mobile device, autonomous mobile system, autonomous mobile method and program |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016049544A JP6724439B2 (en) | 2016-03-14 | 2016-03-14 | Autonomous mobile device, autonomous mobile system, autonomous mobile method and program |
Publications (3)
| Publication Number | Publication Date |
|---|---|
| JP2017167625A JP2017167625A (en) | 2017-09-21 |
| JP2017167625A5 JP2017167625A5 (en) | 2019-03-14 |
| JP6724439B2 true JP6724439B2 (en) | 2020-07-15 |
Family
ID=59913826
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2016049544A Active JP6724439B2 (en) | 2016-03-14 | 2016-03-14 | Autonomous mobile device, autonomous mobile system, autonomous mobile method and program |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6724439B2 (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3703913A1 (en) | 2017-10-30 | 2020-09-09 | Telefonaktiebolaget LM Ericsson (publ) | Robotic method and system |
| JP2019192030A (en) * | 2018-04-26 | 2019-10-31 | 株式会社日立ビルシステム | Apparatus and method for determining moving path of autonomously movable information processing apparatus |
| JP6841263B2 (en) * | 2018-05-24 | 2021-03-10 | 株式会社デンソー | Travel plan generator, travel plan generation method, and control program |
| JP7139762B2 (en) * | 2018-07-31 | 2022-09-21 | カシオ計算機株式会社 | AUTONOMOUS MOBILE DEVICE, AUTONOMOUS MOVEMENT METHOD AND PROGRAM |
| JP7267543B2 (en) * | 2018-09-28 | 2023-05-02 | ニデックエレシス株式会社 | How to adjust the current detection circuit |
| JP7225647B2 (en) * | 2018-10-02 | 2023-02-21 | カシオ計算機株式会社 | AUTONOMOUS MOBILE DEVICE, AUTONOMOUS MOVEMENT METHOD AND PROGRAM |
| JP7326720B2 (en) * | 2018-10-26 | 2023-08-16 | 富士通株式会社 | Mobile position estimation system and mobile position estimation method |
| JP7236310B2 (en) | 2019-03-29 | 2023-03-09 | 本田技研工業株式会社 | Route setting device, route setting method and program |
| CN110220524A (en) * | 2019-04-23 | 2019-09-10 | 炬星科技（深圳）有限公司 | Paths planning method, electronic equipment, robot and computer readable storage medium |
| JP7276596B2 (en) | 2020-03-11 | 2023-05-18 | 日本電気株式会社 | Mobile object control system, control device, control method and recording medium |
| JP7435736B2 (en) * | 2020-03-17 | 2024-02-21 | 日本電気株式会社 | Mobile control system, control device, control method and recording medium |
| JP7288888B2 (en) * | 2020-09-29 | 2023-06-08 | Kddi株式会社 | Wireless quality information providing device, navigation system, wireless quality information providing method and computer program |
| WO2023146443A1 (en) * | 2022-01-27 | 2023-08-03 | Telefonaktiebolaget Lm Ericsson (Publ) | Optimization of planned movement of industrial devices |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3972461B2 (en) * | 1998-05-21 | 2007-09-05 | ソニー株式会社 | Navigation method, navigation device, and automobile |
| JP2003005833A (en) * | 2001-06-25 | 2003-01-08 | Yaskawa Electric Corp | Radio controller for movable cart |
| JP4658892B2 (en) * | 2006-10-02 | 2011-03-23 | 本田技研工業株式会社 | Mobile robot, mobile robot control device, mobile robot control method, and mobile robot control program |
| JP4608472B2 (en) * | 2006-10-02 | 2011-01-12 | 本田技研工業株式会社 | Mobile robot and mobile robot controller |
| JP4996453B2 (en) * | 2007-12-28 | 2012-08-08 | 株式会社Ｉｈｉ | Sniper detection device |
| JP2012247290A (en) * | 2011-05-27 | 2012-12-13 | Kyocera Corp | Radio communication device, radio communication system and route search method |
| US9668146B2 (en) * | 2014-04-25 | 2017-05-30 | The Hong Kong University Of Science And Technology | Autonomous robot-assisted indoor wireless coverage characterization platform |
-
2016
- 2016-03-14 JP JP2016049544A patent/JP6724439B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2017167625A (en) | 2017-09-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6724439B2 (en) | Autonomous mobile device, autonomous mobile system, autonomous mobile method and program | |
| JP6288060B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| JP6323439B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| US20210109537A1 (en) | Autonomous exploration framework for indoor mobile robotics using reduced approximated generalized voronoi graph | |
| JP6187623B1 (en) | Autonomous mobile device, autonomous mobile method and program | |
| Zhang et al. | An indoor wayfinding system based on geometric features aided graph SLAM for the visually impaired | |
| US10424320B2 (en) | Voice detection, apparatus, voice detection method, and non-transitory computer-readable storage medium | |
| US20180161986A1 (en) | System and method for semantic simultaneous localization and mapping of static and dynamic objects | |
| US10296009B2 (en) | Autonomous movement device, autonomous movement method and non-transitory recording medium | |
| US9224043B2 (en) | Map generation apparatus, map generation method, moving method for moving body, and robot apparatus | |
| Westman et al. | Feature-based SLAM for imaging sonar with under-constrained landmarks | |
| JP2002048513A (en) | Position detector, method of detecting position, and program for detecting position | |
| Zhang et al. | An indoor navigation aid for the visually impaired | |
| JP2003271975A (en) | Method of extracting plane, extractor therefor, program therefor, recording medium therefor, and robot system mounted with plane extractor | |
| Mattamala et al. | Learning camera performance models for active multi-camera visual teach and repeat | |
| Karrer et al. | Real-time dense surface reconstruction for aerial manipulation | |
| JP2009178782A (en) | Mobile object, and apparatus and method for creating environmental map | |
| JP6638753B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| JP2018156538A (en) | Autonomous mobile device, image processing method and program | |
| JP7354528B2 (en) | Autonomous mobile device, method and program for detecting dirt on lenses of autonomous mobile device | |
| JP2022138037A (en) | Information processor, information processing method, and program | |
| CN111971149A (en) | Recording medium, information processing apparatus, and information processing method | |
| JP7327596B2 (en) | Autonomous mobile device, method and program for detecting dirt on lens of autonomous mobile device | |
| JP2019159354A (en) | Autonomous mobile device, memory defragmentation method and program | |
| Spournias et al. | Enchancing slam method for mapping and tracking using a low cost laser scanner |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190201 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20190201 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20191212 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20191217 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20200123 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20200526 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20200608 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6724439 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |