JP2020027443A - Autonomous mobile apparatus, soil detection method for lens of autonomous mobile device and program - Google Patents
Autonomous mobile apparatus, soil detection method for lens of autonomous mobile device and program Download PDFInfo
- Publication number
- JP2020027443A JP2020027443A JP2018152021A JP2018152021A JP2020027443A JP 2020027443 A JP2020027443 A JP 2020027443A JP 2018152021 A JP2018152021 A JP 2018152021A JP 2018152021 A JP2018152021 A JP 2018152021A JP 2020027443 A JP2020027443 A JP 2020027443A
- Authority
- JP
- Japan
- Prior art keywords
- image
- unit
- autonomous mobile
- lens
- mobile device
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 68
- 239000002689 soil Substances 0.000 title abstract 2
- 238000000034 method Methods 0.000 claims abstract description 166
- 238000003384 imaging method Methods 0.000 claims abstract description 97
- 238000003860 storage Methods 0.000 claims abstract description 64
- 230000008859 change Effects 0.000 claims abstract description 18
- 230000033001 locomotion Effects 0.000 claims description 54
- 238000012545 processing Methods 0.000 claims description 38
- 238000013519 translation Methods 0.000 claims description 35
- 230000000007 visual effect Effects 0.000 claims description 17
- 238000011109 contamination Methods 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000003708 edge detection Methods 0.000 claims description 6
- 238000001228 spectrum Methods 0.000 claims description 5
- 238000004458 analytical method Methods 0.000 claims description 4
- 238000004148 unit process Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 description 133
- 230000036544 posture Effects 0.000 description 31
- 239000011159 matrix material Substances 0.000 description 24
- 230000009471 action Effects 0.000 description 20
- 239000013598 vector Substances 0.000 description 20
- 238000004891 communication Methods 0.000 description 12
- 238000004140 cleaning Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 5
- 230000002452 interceptive effect Effects 0.000 description 4
- 230000010391 action planning Effects 0.000 description 3
- 230000006399 behavior Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 239000003550 marker Substances 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000008054 signal transmission Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000001994 activation Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 230000005674 electromagnetic induction Effects 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000003825 pressing Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
本発明は、自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラムに関する。 The present invention relates to an autonomous mobile device, a method for detecting dirt on a lens of the autonomous mobile device, and a program.
用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、自律的に移動して人とコミュニケーションを行う自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定を行い、人の検出や認識を行う必要がある。 Autonomous mobile devices that move autonomously according to their use have become widespread. For example, an autonomous mobile device that moves autonomously and communicates with a person is known. Generally, such an autonomous mobile device needs to create a map in the real space and estimate its own position in the real space to detect and recognize a person.
実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。ＳＬＡＭ法では、カメラを用いて自機の周囲の画像を撮影して環境地図を作成しながら自己位置を推定し、移動制御を行う。また、人の検出や認識を行うためにもカメラを使用する。コミュニケーションを行う自律移動装置は、人と接触する可能性が少なくないため、搭載しているカメラのレンズをユーザに指等で触られてレンズが汚れることにより、自己位置推定、人の検出や認識に支障を来すおそれがある。さらに、このような装置では、カメラで取得する画像をユーザが確認することが無いので、ユーザがレンズの汚れに気付くことができない。 As a technique for creating a map of a real space, for example, a SLAM (Simultaneous Localization And Mapping) method is known. In the SLAM method, a self-position is estimated while taking an image around the own device using a camera to create an environment map, and movement control is performed. Cameras are also used to detect and recognize people. Since the autonomous mobile device that performs communication is likely to come into contact with people, if the user touches the lens of the camera with a finger or the like and the lens becomes dirty, self-position estimation, detection and recognition of people May cause trouble. Furthermore, in such an apparatus, since the user does not check the image acquired by the camera, the user cannot notice the dirt on the lens.
特許文献１では、撮像部により撮像された画像データの明るさの変化の急峻度のうち、検出した最大の急峻度が所定の急峻度未満である場合又は所定の値以上の急峻度が検出できない場合には、撮像部のレンズが汚れている可能性があると判定する装置が開示されている。 In Patent Literature 1, when the detected maximum steepness is less than a predetermined steepness or a steepness equal to or more than a predetermined value cannot be detected among the steepnesses of changes in brightness of image data captured by an imaging unit. In this case, an apparatus that determines that the lens of the imaging unit may be dirty is disclosed.
しかしながら、上記の装置では、撮影位置や撮影方向は関係なくレンズの汚れを判断するものであって、レンズの一部に汚れがあったとしても、レンズの汚れの無い部分で検出された急峻度が所定の急峻度を満たしていれば、レンズに汚れがあると判定されないこととなる。 However, in the above-described apparatus, the dirt on the lens is determined regardless of the photographing position and the photographing direction. Even if a part of the lens is contaminated, the steepness detected in the part without the dirt on the lens is determined. Does not satisfy the predetermined steepness, it is not determined that the lens is dirty.
そこで、本発明は、上記問題を解決するためになされたものであり、撮像部のレンズの汚れを好適に検出する自律移動装置、撮像部のレンズの汚れを好適に検出する方法及びプログラムを提供することを目的とする。 In view of the above, the present invention has been made to solve the above problems, and provides an autonomous mobile device that preferably detects dirt on a lens of an imaging unit, a method and a program that preferably detects dirt on a lens of an imaging unit. The purpose is to do.
上記目的を達成するため、本発明の自律移動装置は、
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して撮像する撮像部と、
撮像された画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御し、前記画像又は前記画像に検出された物体の物体検出領域を構成する構造の細かさを示す画像先鋭度値を算定する制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第１の画像の第１の画像先鋭度値と、
前記第１の画像が撮像された時の前記自機位置及び前記自機の向きから所定の範囲内の自機位置及び自機の向きで撮像された第２の画像の第２の画像先鋭度値とを、
又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像の第２の画像先鋭度値とを、算定し、前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記レンズが汚れていると判定する、
ことを特徴とする。
In order to achieve the above object, the autonomous mobile device of the present invention,
A drive unit that moves and rotates the own machine to change the own machine position and the own machine direction,
An imaging unit for imaging via a lens;
An image storage unit that stores a captured image;
A control unit that controls the driving unit, the imaging unit and the image storage unit, and calculates an image sharpness value indicating the fineness of a structure constituting an object detection area of the image or an object detected in the image, With
The control unit includes:
A first image sharpness value of a first image stored in the image storage unit;
The second image sharpness of the second image captured at the own position and the own direction within a predetermined range from the own device position and the own device direction when the first image was captured. Value and
Or a second image sharpness value of a second image including a second object detection region of the object within the reference region from the first object detection region of the object detected by the first image. Calculating, if the second image sharpness value is lower than the first image sharpness value, determining that the lens is dirty;
It is characterized by the following.
本発明によれば、撮像部のレンズの汚れを好適に検出することができる。 ADVANTAGE OF THE INVENTION According to this invention, the dirt of the lens of an imaging part can be detected suitably.
以下、本発明の実施形態に係る自律移動装置について、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。 Hereinafter, an autonomous mobile device according to an embodiment of the present invention will be described with reference to the drawings. In the drawings, the same or corresponding parts have the same reference characters allotted.
本発明の実施形態に係る自律移動装置は、地図（環境地図）を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、人とコミュニケーションを行う事、警備監視、屋内掃除等である。また、本実施形態に係る自律移動装置は、後述する動作モードに応じて移動する。 The autonomous mobile device according to the embodiment of the present invention is a device that autonomously moves according to a use while creating a map (environmental map). This use is, for example, communication with a person, security monitoring, indoor cleaning, and the like. The autonomous mobile device according to the present embodiment moves according to an operation mode described later.
自律移動装置１００は、図１に示すように、外観上、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、を備える。なお、図１には示されていないが、自律移動装置１００は、周囲に存在する物体（障害物）を検出する障害物センサを備えている。また、自律移動装置１００の電池を充電するための充電設備である充電器２００は、図２に示すように、外観上、帰還信号送信部５１（５１ａ，５１ｂ）、電力供給部５２を備える。
As shown in FIG. 1, the autonomous
自律移動装置１００の充電接続部３５と充電器２００の電力供給部５２とが接続することにより、充電器２００から電力の供給を受けて、自律移動装置１００は内蔵している充電池を充電することができる。なお、充電接続部３５及び電力供給部５２は、それぞれがお互いと接続するための接続端子である。これらは、駆動部３２により自律移動装置１００が充電器２００の上に移動することによって、接続される。この接続は、充電接続部３５と電力供給部５２とが接触することによって接続してもよいし、充電接続部３５と電力供給部５２とが近接することによって電磁誘導等により接続してもよい。
When the
撮像部３３は、自律移動装置１００の前方から上部までの広範囲を撮像できる広角レンズを備える。このため、撮像部３３は天井の電灯が点灯しているか否かを判定可能な画像を撮像することができる。そして、自律移動装置１００は、撮像部３３が撮像した画像を利用して単眼ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）処理を行うことができる。
The
自律移動装置１００の帰還信号受信部３１は、充電器２００から送信される帰還信号（赤外線ビーコン）を受信するためのデバイスである。自律移動装置１００の正面に向かって左に帰還信号受信部３１ａ、右に帰還信号受信部３１ｂと、合計２つの帰還信号受信部３１が備えられている。また、充電器２００の帰還信号送信部５１は、自律移動装置１００に帰還信号を送信するためのデバイスである。充電器２００の正面に向かって右に帰還信号送信部５１ａが、左に帰還信号送信部５１ｂが、それぞれ備えられている。そして、帰還信号送信部５１ａから送信される帰還信号と、帰還信号送信部５１ｂから送信される帰還信号とは、異なる信号になっている。したがって、帰還信号受信部３１は、各々左右どちらの帰還信号送信部５１からの帰還信号を受信したのかを判別することができる。
The feedback
充電器２００の帰還信号送信部５１から送信される帰還信号の左右それぞれの受信可能範囲５３（５３ａ，５３ｂ）の一例を図３に示す。帰還信号送信部５１ａから送信される帰還信号は、受信可能範囲５３ａの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。そして、帰還信号送信部５１ｂから送信される帰還信号は、受信可能範囲５３ｂの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。したがって、自律移動装置１００は、受信可能範囲５３の内部に入ると充電器２００の存在する方向を把握することができる。そして、自律移動装置１００は、帰還信号送信部５１ａからの帰還信号を帰還信号受信部３１ａが受信し、帰還信号送信部５１ｂからの帰還信号を帰還信号受信部３１ｂが受信するように向きを調整しながら進むことによって、充電器２００の上に移動することができる。自律移動装置１００は、充電器２００の上に移動すると、充電接続部３５と電力供給部５２とが接続し、自律移動装置１００に内蔵されている電池を充電することができる。
FIG. 3 shows an example of the left and right receivable ranges 53 (53a, 53b) of the feedback signal transmitted from the feedback signal transmitting unit 51 of the
駆動部３２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲＲ、左車輪の回転数をＲＬとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（ＲＬ−ＲＲ）／（２×Ｉ）となる。この並進移動量及び回転量を逐次足し合わせていくことで、駆動部３２は、いわゆるオドメトリ（メカオドメトリ）として機能し、自機位置（移動開始時の位置及び向きを基準とした自律移動装置１００の位置及び向き）を計測することができる。駆動部３２は、撮像部３３によって物体の境界等が写るように画像を撮像できる自機の位置及び向きに自律移動装置１００を移動させる。
The
オドメトリから得られる自機の位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。ただし、オドメトリの回転成分（向きの情報）に関しては、後述する角速度センサ情報を用いることで精度を向上させることができる。また、地磁気を検出して方位を特定する方位センサ（図示せず）を用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することもできる。 The accuracy of the position and orientation of the aircraft obtained from odometry is often low due to wear or slippage of wheels. In particular, due to the accumulation of errors, accuracy deteriorates with time. However, the accuracy of the rotational component (direction information) of the odometry can be improved by using angular velocity sensor information described later. Also, by using an orientation sensor (not shown) for detecting the geomagnetism and specifying the orientation, it is also possible to acquire information on the absolute orientation using the geomagnetism irrespective of the value obtained from odometry.
なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。 Note that a crawler may be provided instead of the wheel, or a plurality of (for example, two) feet may be provided to move by walking with the feet. In these cases, the position and orientation of the own device can be measured based on the movement of the two crawlers and the movement of the foot, as in the case of the wheels.
図４に示すように、自律移動装置１００は、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、に加えて、制御部１０、記憶部２０、通信部３４、センサ部４０、を備える。
As shown in FIG. 4, the autonomous
制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（位置計測部１１、地図作成部１２、位置推定部１３、判定部１４、地図編集部１５、画像情報取扱部１６）の機能を実現する。
The
記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ)、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、画像記憶部２１や地図記憶部２２を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム（例えば、後述するＳＬＡＭ法の演算や自律移動制御処理に係るプログラム）や、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。
The
画像記憶部２１には、撮像部３３が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくても良く、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像（後述するキーフレーム）については、画像の情報と共に、上述したオドメトリで計測した自機位置（自機の位置及び向き）の情報並びに後述するＳＬＡＭ法で推定した自機位置（自機の位置及び向き）の情報が記憶される。
The
地図記憶部２２には、後述するＳＬＡＭ法や障害物センサ４３からの情報に基づいて地図作成部１２が作成した地図（特徴点や障害物の３次元位置の情報）が記憶される。
The
情報記憶部２３には、画像先鋭度値データベースＤＢ，ＤＢ２、画像先鋭度フラグデータベースＤＢＧ、顔領域先鋭度値データベースＤＢ３、顔領域先鋭度フラグデータベースＤＢ３Ｈ、等があり、画像情報取扱部１６が生成した後述する情報Ｇ（図５（ａ）），Ｇ１（図５（ｂ）），Ｈ（図５（ｃ）），Ｈ１（図５（ｄ））等が記憶される。
The
センサ部４０として、加速度センサ４１、角速度センサ４２、障害物センサ４３を備える。加速度センサ４１は、ＸＹＺ方向（３軸）の加速度を測定するセンサである。角速度センサ４２は角速度（単位時間あたりの角度移動量）を測定するセンサである。車輪の回転数に基づいて自機の向きを求めるよりも、角速度センサ４２を用いて向きを求める方が精度が向上することが知られている。障害物センサ（距離測定素子）４３は、走行中の障害物を検知するセンサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサを搭載せずに、撮像部３３を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。また、地磁気を検出して方位を特定する方位センサ（図示せず）を備えても良い。方位センサを用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することができる。
The
通信部３４は、充電器２００等の外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部３４は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部３４を用いることにより、自律移動装置１００は、外部装置とデータ受け渡し等を行うことができる。
The
次に、制御部１０の機能について説明する。制御部１０は、位置計測部１１、地図作成部１２、位置推定部１３、判定部１４、地図編集部１５、画像情報取扱部１６を含み、後述するＳＬＡＭ法の演算、自律移動装置１００の移動指示や画像先鋭度値の算定等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。
Next, the function of the
位置計測部１１は、駆動部３２の車輪とモータの動きに基づき、自機位置を計測する。具体的には、地面に高低差がなく、車輪がスリップしないという前提のもとで、各車輪の直径（Ｄ）及び回転数（Ｒ：ロータリエンコーダにより測定）に基づき、その車輪の接地部分での移動距離はπ・Ｄ・Ｒとして求められるので、これらと車輪間の距離とから並進移動量及び並進方向並びに向きの変化量（回転角度）を求めることができる。これを逐次加算していくことで、オドメトリとして、自機の位置及び向きを計測できる。地面に高低差がある場合は高さ方向を考慮した並進移動量を求める必要があるが、これは、加速度センサ４１により高度の変化量を把握することにより求められる。また車輪のスリップに関しては、駆動用の車輪以外に移動量計測用の車輪も搭載しておくことで誤差を小さくすることが可能である。
The
地図作成部１２は、画像記憶部２１に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてＳＬＡＭ法を用いて推定された特徴点の３次元位置（Ｍａｐ点）や、障害物センサ４３で障害物を検知したときの自機の位置及び向きの情報に基づいて得られた該障害物の３次元位置等を、地図の情報として地図記憶部２２に記憶する。
The map creator 12 uses the SLAM method to estimate the three-dimensional position of the feature point based on the image information stored in the
位置推定部１３は、後述するＳＬＡＭ法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。
The
判定部１４は、位置計測部１１で計測した位置及び向きと、位置推定部１３で推定した位置及び向きとの差が所定誤差内か否かを判定する。オドメトリ（位置計測部１１）で正しい位置及び向きが計測でき、しかも、ＳＬＡＭ法（ビジュアルオドメトリ：位置推定部１３）で正しい位置及び向きを推定できているなら、これらはほぼ一致するので、その場合は地図記憶部２２に記憶されている地図の情報も正しいと推測できる。しかし、位置及び向きの計測や推定に誤差が含まれていると、誤差の累積が生じ、計測値と推定値とは一致しなくなる。そのような場合は、地図記憶部２２に記憶されている地図の情報にも誤差が累積されてしまっていると推測できる。したがって、これら両者の差が所定誤差内か否かを判定することで、地図の情報が正しいかどうかを判定することができる。
The
地図編集部１５は、判定部１４によって地図記憶部２２に記憶されている地図の情報が正しくないと判定された場合は、該地図を編集する。地図の編集の最も簡単な処理は、該地図を消去する処理である。
The
画像情報取扱部１６は、撮像部３３によって撮像された画像（物体の境界等）に対してラプラシアンオペレータの畳み込み処理を行い、その後分散を取ることにより、画像（又は後述する画像内に検出された顔等の物体の検出領域）の画像先鋭度値（シャープネス）を算出する。画像先鋭度値は、画像全体又は画像内に検出された物体の物体検出領域全体に対して算出された１つの値（代表先鋭度値）である。画像先鋭度値は、画像又は物体検出領域を構成する構造の細かさを示す値である。ラプラシアンオペレータは、隣接する上下左右の４画素（４近傍）についての２次微分を行うものを使用するが、斜め方向に隣接する８画素（８近傍）について２次微分を行うものとしても良い。その他、画像から取得した後述の情報の各種算定等を行う。
The image
以上、自律移動装置１００の機能構成について説明した。次に、自律移動装置１００で起動される各種処理について説明する。自律移動装置１００は、電源オフ時は充電器２００に接続されて充電されており、電源が投入されると、充電器２００に接続された位置で、後述する起動時処理が実行され、各種アプリ及び各種スレッドが並行して実行される。では、自律移動装置１００の起動時の処理について、図６を参照して説明する。図６は、自律移動装置１００が起動した時に実行される起動時処理のフローチャートである。
The functional configuration of the autonomous
まず、自律移動装置１００の制御部１０は、ＳＬＡＭ処理用の各種スレッドを起動する（ステップＳ１０１）。ＳＬＡＭ処理用の各種スレッドとは、自律移動装置１００の姿勢（位置及び向き）を推定する自機位置推定処理及び環境地図を生成する処理を行う自機位置推定、環境地図生成スレッド、ループクロージング処理を行うループクロージングスレッド等である。なお、ループクロージング処理とは、自律移動装置１００が、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の自律移動装置１００の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの移動軌跡中のキーフレーム（の３Ｄ姿勢）や、関連するＭａｐＰｏｉｎｔ（の３Ｄ座標）を修正する処理をいう。自機位置推定、環境地図作成スレッドの詳細については後述する。
First, the
次に、制御部１０は、移動指示アプリ及び行動計画アプリを起動する（ステップＳ１０２）。行動計画アプリ（図示せず）は、自律移動装置１００の行動を制御するアプリである。行動計画アプリでは、自律移動装置１００の動作モード（充電器帰還モード、お散歩モード、対話モード）の設定を行うことができる。また、行動計画アプリは、環境地図内での自律移動装置１００の位置を理解し、自律移動装置１００の移動先を移動スレッドに指示することができる。さらに、行動計画アプリは、撮像部３３のレンズに汚れが有るとの通知を受けた際に、自律移動装置１００にユーザに対して撮像部３３のレンズの清掃を促す行動を指示する。移動指示アプリは、行動計画アプリの設定に応じた動作モード（例えば充電器帰還モード）に基づいて、次のステップで起動される自機位置推定、環境地図生成スレッドから自律移動装置１００の現在の環境地図内における姿勢（位置及び向き）の情報を受けた行動計画アプリの指示する移動先への移動を実行するアプリである。なお、移動指示アプリの詳細については後述する。
Next, the
次に、制御部１０は、移動スレッド、顔検出スレッドを起動し（ステップＳ１０３）、起動時処理を終了する。移動スレッドは、移動指示アプリから移動命令を受けて、制御部１０が駆動部３２を制御して自律移動装置１００を移動させる処理を行うスレッドである。そして、起動時処理を終了した後は、自律移動装置１００は、この起動時処理によって起動された各アプリ及びスレッドにより制御される。なお、移動スレッド、顔検出スレッドの詳細については後述する。
Next, the
ＳＬＡＭ処理用のスレッドのうち、自機位置推定、環境地図生成スレッドについて、図７を参照して説明する。自機位置推定、環境地図生成スレッドは、撮像部３３により撮像して取得した画像を利用したＶｉｓｕａｌ ＳＬＡＭ法による環境地図を用いて追跡処理（自機位置推定処理）を行うスレッドである。さらに、自機位置推定、環境地図生成のために取得した画像を利用して、撮像部３３のレンズの汚れの有無を判定するスレッドでもある。
Among the threads for SLAM processing, the self-position estimation and environment map generation threads will be described with reference to FIG. The own device position estimation and environment map generation thread is a thread that performs a tracking process (own device position estimation process) using an environment map based on the Visual SLAM method using an image captured and acquired by the
まず、制御部１０は、ＳＬＡＭ法に基づいて自機位置推定、環境地図の生成を行う（ステップＳ２０１）。これにより、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）が生成される。なお、自機位置推定処理及び環境地図作成処理の詳細については後述する。
First, the
次に、制御部１０は、生成した環境地図と推定した自機位置とを行動アプリに送信する（ステップＳ２０２）。これにより、行動計画アプリは、自律移動装置１００の周囲の環境を理解し、自律移動装置１００の移動先を指示することができる。
Next, the
そして、制御部１０は、画像情報取扱部１６に、ステップＳ２０１で取得した画像の画像先鋭度値を算出させることにより、画像先鋭度データベースＤＢを生成（ＤＢを生成１）する（ステップＳ２０３）。画像先鋭度データベースＤＢを生成する処理の詳細については後述する。
Then, the
そして、制御部１０は、画像の画像先鋭度値を用いて撮像部３３のレンズの汚れを判定（レンズ汚れ判定１）する（ステップＳ２０４）。撮像部３３のレンズの汚れを判定する処理については後述する。
Then, the
撮像部３３のレンズに汚れがあると判定された場合（ステップＳ２０５；Ｙｅｓ）、制御部１０は、その旨を行動計画アプリに通知する（ステップＳ２０６）。行動アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップＳ２０７に進む。撮像部３３のレンズに汚れが有ると判定されなかった場合（ステップＳ２０５；Ｎｏ）、レンズの汚れは不明であると判定し、ステップＳ２０７に進む。
When it is determined that the lens of the
ステップＳ２０７では、制御部１０は、各種アプリやユーザから終了指示があるか否かを判定する。終了指示があるなら（ステップＳ２０７；Ｙｅｓ）、自機位置推定、環境地図生成スレッドを終了する。終了指示がないなら（ステップＳ２０７；Ｎｏ）、ステップＳ２０１に戻る。
In step S207, the
以上、自機位置推定、環境地図生成スレッドの処理について説明した。次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０１で実行される自機位置推定処理について、図８を参照して説明する。この処理は、位置推定部１３が、最初に初期化処理を行い、その後自機位置推定（撮像部３３で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。
The processing of the own device position estimation and the environment map generation thread has been described above. Next, the self-location estimation process executed in step S201 of the self-location estimation and environment map generation thread (FIG. 7) will be described with reference to FIG. This process is a process in which the
これにより、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。 This makes it possible to appropriately update the map information while moving autonomously based on the map information.
典型的な例としては、自律移動装置１００は最初、充電器２００に置いてある状態で電源を投入すると、障害物センサ４３を頼りとして、家の各部屋をくまなく移動し、障害物センサ４３によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電器２００に戻ったり、効率的に部屋の掃除をしたりすることが可能になる。
As a typical example, when the autonomous
位置推定部１３は、まず、動作終了かを判定する（ステップＳ３０１）。動作終了なら（ステップＳ３０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ３０１；Ｎｏ）、初期化済であるか判定する（ステップＳ３０２）。初期化済なら（ステップＳ３０２；Ｙｅｓ）ステップＳ３２１以降の自機位置推定処理を行い、初期化済でないなら（ステップＳ３０２；Ｎｏ）ステップＳ３０３に進んで初期化処理を行う。まず初期化処理について説明する。
First, the
初期化処理では位置推定部１３はまずフレームカウンタＮに−１をセットし（ステップＳ３０３）、撮像部３３で画像を取得する（ステップＳ３０４）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。次に、取得した画像内から２Ｄ特徴点を取得する（ステップＳ３０５）。２Ｄ特徴点とは画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。なお、２Ｄ特徴点を取得するのに他のアルゴリズムを使っても良い。
In the initialization process, the
取得した２Ｄ特徴点の個数が少ないと、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法での計算ができないため、位置推定部１３はステップＳ３０６において、２Ｄ特徴点の取得数と基準値（例えば１０個）とを比較し、基準値未満だった場合（ステップＳ３０６；Ｎｏ）はステップＳ３０４に戻り、基準値以上の２Ｄ特徴点数が得られるまで、画像の取得と２Ｄ特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、障害物センサ４３を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と２Ｄ特徴点取得を繰り返していれば、移動しながら画像取得を繰り返すことになるので、様々な画像が取得でき、いずれは２Ｄ特徴点数が多い画像を取得できることが期待できる。
If the number of acquired 2D feature points is small, calculation by the Two-view Structure from Motion method described later cannot be performed, so the
２Ｄ特徴点の取得数が基準値以上だった場合は（ステップＳ３０６；Ｙｅｓ）、位置推定部１３はフレームカウンタＮをインクリメントする（ステップＳ３０７）。そして、フレームカウンタＮが０かどうかを判定する（ステップＳ３０８）。フレームカウンタＮが０なら（ステップＳ３０８；Ｙｅｓ）画像をまだ一つしか取得していないということになるので、２枚目の画像を取得するためにステップＳ３０４に戻る。なお図８のフローチャートでは記載していないが、１枚目の画像を取得した時の自律移動装置１００の位置と２枚目の画像を取得する時の自律移動装置１００の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ここでステップＳ３０８からステップＳ３０４に戻る際に、オドメトリによる並進距離が所定の距離（例えば１ｍ）以上となるまで待つ処理を追加しても良い。
If the number of acquired 2D feature points is equal to or greater than the reference value (step S306; Yes), the
フレームカウンタＮが０でないなら（ステップＳ３０８；Ｎｏ）二つの画像を取得したということがわかるので、位置推定部１３は、これら二つの画像間で２Ｄ特徴点の対応（実環境上の同一の点がそれぞれの画像中に存在し、その対応が取れるもの）を取得する（ステップＳ３０９）。ここで特徴点の対応数が５未満であれば、後述する二つの画像間の姿勢の推定ができないため、位置推定部１３は特徴点の対応数が５未満かどうかを判定する（ステップＳ３１０）。５未満であれば（ステップＳ３１０；Ｙｅｓ）初期画像を取得し直すために、ステップＳ３０３に戻る。特徴点の対応数が５点以上であれば（ステップＳ３１０；Ｎｏ）、Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法を用いることで、二つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができる（ステップＳ３１１）。
If the frame counter N is not 0 (step S308; No), it is known that two images have been acquired, and the
この推定は具体的には、対応する特徴点から基礎行列Ｅを求め、基礎行列Ｅを並進ベクトルｔと回転行列Ｒとに分解することによって得られるが、詳細はここでは割愛する。なお、ここで得られる並進ベクトルｔ（３次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、Ｘ，Ｙ，Ｚの３要素を持つ）の各要素の値は実環境上での値とは異なる（Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法では実環境上のスケール自体を得ることはできず、実環境と相似する空間上での値を得ることになる。）ため、これらをＳＬＡＭ空間上での値とみなし、以下ではＳＬＡＭ空間上での座標（ＳＬＡＭ座標）を用いて説明する。 Specifically, this estimation is obtained by obtaining a fundamental matrix E from corresponding feature points and decomposing the fundamental matrix E into a translation vector t and a rotation matrix R, but details are omitted here. Note that the value of each element of the translation vector t obtained here (assuming that it moves in a three-dimensional space, having three elements of X, Y, and Z with the origin at the position where the first image was obtained) is Since the value is different from the value in the real environment (the two-view structure from motion method cannot obtain the scale itself in the real environment, but obtains the value in a space similar to the real environment). These are regarded as values on the SLAM space, and the following description will be made using the coordinates on the SLAM space (SLAM coordinates).
二つの画像間の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まると、その値は、最初の画像を基準（最初の画像を取得した位置をＳＬＡＭ座標の原点、並進ベクトルは０ベクトル、回転行列は単位行列Ｉとする。）にした場合の、二枚目の画像の姿勢（二つ目の画像を取得した時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ））となる。ここで、二つの画像それぞれの姿勢（該画像（フレーム）撮影時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ）で、フレーム姿勢とも言う。）が求まっている場合、その二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）のＳＬＡＭ座標での３Ｄ位置を、以下の考え方に基づき、地図作成部１２が求める（ステップＳ３１２）。 Once the orientation between the two images (translation vector t and rotation matrix R) is determined, the values are based on the first image (the position where the first image was obtained is the origin of the SLAM coordinates, the translation vector is the zero vector, the rotation matrix Is the unit matrix I), the attitude of the second image (the position (translation vector t) and orientation (rotation matrix R) of the own device when the second image is obtained). . Here, when the posture of each of the two images (the position (translation vector t) and direction (rotation matrix R) of the own device at the time of photographing the image (frame) is also referred to as the frame posture) is obtained. The 3D position of the 2D feature point (corresponding feature point) corresponding to the two images in the SLAM coordinates is obtained by the map creating unit 12 based on the following concept (step S312).
２Ｄ特徴点の画像中の座標（フレーム座標：既知）を（ｕ，ｖ）とし、その２Ｄ特徴点のＳＬＡＭ座標での３Ｄ位置（未知）を（Ｘ，Ｙ，Ｚ）とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Ｐを用いて下記の式（１）で表される。ここで、「〜」記号は「非零の定数倍を除いて等しい」（つまり、等しいか又は定数（非零）倍になっている）ことを表し、「’」記号は「転置」を表す。
（ｕ ｖ １）’〜Ｐ（Ｘ Ｙ Ｚ １）’…（１）
Assuming that the coordinates (frame coordinates: known) of the 2D feature point in the image are (u, v) and the 3D position (unknown) of the 2D feature point in SLAM coordinates is (X, Y, Z), these are the same. These relationships when expressed by the next coordinate are expressed by the following equation (1) using the perspective projection matrix P. Here, the symbol "~" means "equal except for a non-constant multiple" (that is, it is equal or has been multiplied by a constant (non-zero)), and the "'" symbol means "transpose". .
(Uv1) '-P (XYZ1)' ... (1)
上記の式（１）において、Ｐは３×４の行列で、カメラの内部パラメータを示す３×３の行列Ａと、その画像の姿勢（フレーム姿勢）を示す外部パラメータＲ及びｔから以下の式（２）で表される。ここで、（Ｒ｜ｔ）は、回転行列Ｒの右に並進列ベクトルｔを並べた行列を表す。
Ｐ＝Ａ（Ｒ｜ｔ）…（２）
In the above equation (1), P is a 3 × 4 matrix, and the following equation is obtained from a 3 × 3 matrix A indicating internal parameters of the camera and external parameters R and t indicating the attitude (frame attitude) of the image. It is represented by (2). Here, (R | t) represents a matrix in which the translation column vector t is arranged on 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 attitude as described above. Further, since the internal parameter A of the camera is determined by the focal length and the size of the imaging element, it is a constant if the
二つの画像間で対応が取れている２Ｄ特徴点のうちの一つが、一つ目の画像のフレーム座標（ｕ１，ｖ１）と、二つ目の画像のフレーム座標（ｕ２，ｖ２）に写っているとすると、以下の式（３）及び式（４）ができる。ここで、Ｉは単位行列、０はゼロベクトル、（Ｌ｜ｒ）は、行列Ｌの右に列ベクトルｒを並べた行列を表す。
（ｕ１ ｖ１ １）’〜Ａ（Ｉ｜０）（Ｘ Ｙ Ｚ １）’…（３）
（ｕ２ ｖ２ １）’〜Ａ（Ｒ｜ｔ）（Ｘ Ｙ Ｚ １）’…（４）
One of the 2D feature points corresponding between the two images is a frame coordinate (u 1 , v 1 ) of the first image and a frame coordinate (u 2 , v 2 ) of the second image. ), The following equations (3) and (4) are 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 to the right of the matrix L.
(U 1 v 11 1) ′ to A (I | 0) (XYZ 1) ′ (3)
(U 2 v 2 1) ′ to A (R | t) (XYZ 1) ′ (4)
上記の式（３）及び式（４）において、ｕ１，ｖ１，ｕ２，ｖ２それぞれについての式ができるため、式は４つできるが、未知数はＸ，Ｙ，Ｚの３つなので、Ｘ，Ｙ，Ｚを求めることができ、これがその２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばｕ１，ｖ１，ｕ２で求めたＸ，Ｙ，Ｚとｕ１，ｖ１，ｖ２で求めたＸ，Ｙ，Ｚとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部１２は、最小二乗法を用いて、最も確からしいＸ，Ｙ，Ｚを求める。 In the above equations (3) and (4), equations can be formed for each of u 1 , v 1 , u 2 , and v 2, so that four equations can be obtained. However, since there are three unknowns, X, Y, and Z, , X, Y, and Z, which are the 3D positions of the 2D feature points in the SLAM coordinates. Since the number of equations is larger than the number of unknowns, for example, X, Y, Z determined by u 1 , v 1 , u 2 and X, Y, Z determined by u 1 , v 1 , v 2 May be different. In such a case, the system becomes a simultaneous linear equation with an excess condition, and generally there is no solution. However, the mapping unit 12 obtains the most probable X, Y, and Z using the least squares method.
２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１２がＭａｐ点データベース（Ｍａｐ点ＤＢとも言い、地図記憶部２２に格納される）に登録する（ステップＳ３１３）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置であるＸ，Ｙ，Ｚ」と、「その２Ｄ特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。「タイムスタンプ」（後述するキーフレームカウンタＮＫＦ（現在のキーフレーム番号を表す変数）のＭａｐ点データベースへの登録時点での値等）をＭａｐ点データベースへの登録要素に追加しておくと、Ｍａｐ点データベースを編集（過去の状態に戻す等）する際に便利である。 When the 3D position (X, Y, Z) of the 2D feature point in the SLAM coordinates is determined, the map creating unit 12 sets the 3D position (X, Y, Z) as a map point, and stores the map point database (also referred to as a map point DB and stored in the map storage unit 22). (Step S313). As elements to be registered in the Map point database, at least “X, Y, and Z, which are 3D positions of SLAM coordinates of 2D feature points” and “feature amounts of the 2D feature points” (for example, feature amounts obtained by SIFT or the like) )is necessary. If a “time stamp” (such as the value of a key frame counter NKF (variable representing the current key frame number) described later at the time of registration in the Map point database) is added to the elements registered in the Map point database, Map This is convenient when editing the point database (for example, returning to a past state).
そして、地図作成部１２は、二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）の全てをＭａｐ点データベースに登録したかを判定し（ステップＳ３１４）、まだ全ての登録ができていなかったら（ステップＳ３１４；Ｎｏ）ステップＳ３１２に戻り、全て登録できたら（ステップＳ３１４；Ｙｅｓ）ステップＳ３１５に進む。 Then, the map creating unit 12 determines whether all the 2D feature points (corresponding feature points) corresponding between the two images have been registered in the Map point database (step S314), and all the registrations are still possible. If not (step S314; No), the process returns to step S312, and if all registrations have been completed (step S314; Yes), the process proceeds to step S315.
次に位置推定部１３は、キーフレーム（後に続くスレッドでの処理対象となる画像を指す）のカウンタＮＫＦを０に初期化し（ステップＳ３１５）、二つ目の画像をキーフレームとしてフレームデータベース（フレームＤＢとも言い、画像記憶部２１に格納される）に登録する（ステップＳ３１６）。
Next, the
フレームデータベースに登録する要素は、「キーフレーム番号」（登録時点でのキーフレームカウンタＮＫＦの値）、「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「オドメトリで計測した実環境上での姿勢」（実環境での駆動部３２による移動距離に基づいて求められる自機の位置及び向き）、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。
The elements to be registered in the frame database are “key frame number” (the value of the key frame counter NKF at the time of registration), “posture” (position in the SLAM coordinates (translation vector t) of the own device at the time of capturing the image, and Orientation (rotation matrix R)), "posture in the real environment measured by odometry" (position and orientation of the own device obtained based on the moving distance by the
上記中、「オドメトリで計測した実環境上での姿勢」は、並進ベクトルｔと回転行列Ｒとで表すこともできるが、通常、本自律移動装置１００は２次元平面上を動くので、２次元データに単純化して、移動開始時の位置（原点）及び向きを基準にした２次元座標（Ｘ，Ｙ）及び向きφとして表しても良い。また、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の２Ｄ特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。
In the above, the “posture in the real environment measured by odometry” can be expressed by a translation vector t and a rotation matrix R. However, since the autonomous
次に、位置推定部１３は、キーフレームが生成された事を地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー（キュー（Ｑｕｅｕｅ）は、先入れ先出しのデータ構造になっている）に、キーフレームカウンタＮＫＦをセットする（ステップＳ３１７）。
Next, the
以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１３は、初期化済フラグをセットする（ステップＳ３１８）。
Since the initialization process of the own-device position estimation thread has been completed, the
そして、位置推定部１３は、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ３１９）。
Then, the
次に、位置推定部１３は、ステップＳ３０１、ステップＳ３０２を経由して、初期化済の場合の処理であるステップＳ３２１へ進む。
Next, the
初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、逐次現在の自律移動装置１００の位置及び向き（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒ）を推定する処理である。 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 is a process of sequentially estimating the current position and orientation of the autonomous mobile device 100 (the translation vector t and the rotation matrix R in the SLAM coordinates).
位置推定部１３は、撮像部３３で画像を撮影し（ステップＳ３２１）、フレームカウンタＮをインクリメントする（ステップＳ３２２）。そして、撮影した画像に含まれている２Ｄ特徴点を取得する（ステップＳ３２３）。次に、フレームデータベースに登録されている以前のキーフレーム（例えばキーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている２Ｄ特徴点のうち、３Ｄ位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）２Ｄ特徴点を取得し、今撮影した画像との間で対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ３２４）。
The
そして、位置推定部１３は、対応特徴点の個数が基準対応特徴点数（例えば１０）未満かどうかを判定し（ステップＳ３２５）、基準対応特徴点数未満の場合（ステップＳ３２５；Ｙｅｓ）はＳＬＡＭ法で推定する姿勢の精度が悪くなる（Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗となる）ので、位置の推定は行わずにステップＳ３２１での画像の取得に戻る。ここで、すぐにステップＳ３２１に戻るのではなく、ステップＳ３２４に戻って、フレームデータベースに登録されているキーフレームの中から対応特徴点の個数が基準対応特徴点数以上のものを検索するようにしても良い。この場合は、フレームデータベースに登録されているキーフレームの中に対応特徴点の個数が基準対応特徴点数以上のものが見つからなかった場合にステップＳ３２１に戻る。
Then, the
位置推定部１３は、基準対応特徴点数以上の対応特徴点が抽出できたら（ステップＳ３２５；Ｎｏ）、対応特徴点それぞれの３Ｄ位置（Ｘｉ，Ｙｉ，Ｚｉ）をＭａｐ点データベースから取得する（ステップＳ３２６）。今撮影した画像に含まれている対応特徴点のフレーム座標を（ｕｉ，ｖｉ）とし、その対応特徴点の３Ｄ位置を（Ｘｉ，Ｙｉ，Ｚｉ）とする（ｉは１から対応特徴点数までの値を取る）と、各対応特徴点の３Ｄ位置（Ｘｉ，Ｙｉ，Ｚｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘｉ，ｖｘｉ）とフレーム座標（ｕｉ，ｖｉ）とは理想的には一致するはずである。
（ｕｘｉ ｖｘｉ １）’〜Ａ（Ｒ｜ｔ）（Ｘｉ Ｙｉ Ｚｉ １）’…（５）
(Ux i vx i 1) ′-A (R | t) (X i Y i Z i 1) ′ (5)
実際には（Ｘｉ，Ｙｉ，Ｚｉ）にも（ｕｉ，ｖｉ）にも誤差が含まれているため、（ｕｘｉ，ｖｘｉ）と（ｕｉ，ｖｉ）とが一致することはめったにない。そして、未知数はＲとｔ（３次元空間ではそれぞれ３次元となり、３＋３＝６が未知数の個数である）だけなのに、数式は対応特徴点の個数の２倍存在する（対応特徴点一つに対して、フレーム座標のｕ，ｖそれぞれに対する式が存在するため）ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部１３は、以下の式（６）のコスト関数Ｅ１を最小化する姿勢（並進ベクトルｔ及び回転行列Ｒ）を求めることになる。これがＳＬＡＭ法で求めたＳＬＡＭ座標での自律移動装置１００の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自律移動装置１００の位置及び向き）となる。このようにして、位置推定部１３は自律移動装置１００の姿勢を推定する（ステップＳ３２７）。
In practice (X i, Y i, Z i) because they also contain error in (u i, v i), (ux i, vx i) and (u i, v i) and are consistent I rarely do. Although the unknowns are only R and t (three-dimensional in a three-dimensional space, and 3 + 3 = 6 is the number of unknowns), the mathematical expression is twice as many as the number of corresponding feature points (for one corresponding feature point). Therefore, since there is an equation for each of u and v of the frame coordinates), it becomes a simultaneous linear equation with excess conditions, and is obtained by the least square method as described above. Specifically, the
ＳＬＡＭ座標での現在の自律移動装置１００の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、位置推定部１３は、これにスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ３２８）。ＶＯは実環境での自律移動装置１００の位置及び向きとして利用できる。
Since the current attitude (translation vector t and rotation matrix R) of the autonomous
次に、位置推定部１３は、フレームＤＢに登録されている直前のキーフレーム（キーフレーム番号がＮＫＦである画像）を撮影した時の自律移動装置１００の位置から基準並進距離（例えば１ｍ）以上移動しているかを判定し（ステップＳ３２９）、移動しているなら（ステップＳ３２９；Ｙｅｓ）キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ３３０）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ３３１）。基準並進距離未満しか移動していないなら（ステップＳ３２９；Ｎｏ）ステップＳ３０１に戻る。
Next, the
ここで、基準並進距離と比較する自律移動装置１００の移動距離は、直前のキーフレームから現フレームまでの並進距離（両フレームの並進ベクトルの差のベクトルの絶対値（要素の二乗和の平方根））をオドメトリから取得しても良いし、上述したＶＯ（ビジュアルオドメトリ）から求めても良い。フレームＤＢに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「オドメトリで計測した実環境上での姿勢」、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。
Here, the movement distance of the autonomous
そして、位置推定部１３は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ３３２）。そして、ステップＳ３０１に戻る。なお、キーフレームカウンタＮＫＦ、スケールＳ、Ｍａｐ点ＤＢ、フレームＤＢはスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。
Then, the
なお、基準並進距離を自律移動装置１００が並進する間に、ステップＳ３２５のＶｉｓｕａｌ ＳＬＡＭ制御の失敗回数を、ステップＳ３２５に到達した回数で割ることによりＶｉｓｕａｌ ＳＬＡＭ制御の失敗率を算定し、この失敗率が基準失敗率（例えば５０パーセント）より高い場合、後述する撮像部３３のレンズが汚れている、と判定しても良い。レンズが汚れていることにより、対応特徴点を正しく取得できていないと考えられるからである。
While the autonomous
以上、自機位置推定処理について説明した。次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０１で実行される環境地図生成処理について、図９を用いて説明する。この処理では地図作成部１２が、キーフレーム中の対応特徴点の３Ｄ位置を計算して、地図の情報（Ｍａｐ点ＤＢ）を作成している。 The self-position estimation process has been described above. Next, the environment map generation process executed in step S201 of the own device position estimation and environment map generation thread (FIG. 7) will be described with reference to FIG. In this process, the map creator 12 calculates the 3D position of the corresponding feature point in the key frame and creates map information (Map point DB).
まず、地図作成部１２は、動作終了かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ４０１；Ｎｏ）、キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空だったら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、空でなければ（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＭＫＦ（地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。地図作成部１２は、ＭＫＦが０より大きいか判定し（ステップＳ４０４）、ＭＫＦが０である場合は（ステップＳ４０４；Ｎｏ）ステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。ＭＫＦが１以上の場合は（ステップＳ４０４；Ｙｅｓ）、以下の処理に進む。 First, the map creating unit 12 determines whether the operation has been completed (step S401). If the operation is completed (Step S401; Yes), the operation is completed. 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 process returns to step S401. If the key frame queue is not empty (step S402; No), the data is taken out from the key frame queue and the key frame of the key frame processed by the MKF (map creation thread). (A variable representing a number) (step S403). The map creation unit 12 determines whether MKF is greater than 0 (step S404), and if MKF is 0 (step S404; No), returns to step S401 and waits for data to enter the key frame queue. If the MKF is 1 or more (step S404; Yes), the process proceeds to the following process.
地図作成部１２は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＭＫＦ−１のキーフレーム）の２Ｄ特徴点と現キーフレーム（キーフレーム番号がＭＫＦのキーフレーム）の２Ｄ特徴点とで対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ４０５）。フレームＤＢにはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、自己位置推定処理の初期化時の処理の時と同様の方法で対応特徴点の３Ｄ位置を計算できる。地図作成部１２は、３Ｄ位置が計算できた対応特徴点をＭａｐ点としてＭａｐ点ＤＢに登録する（ステップＳ４０６）。地図作成部１２は、フレームＤＢに対しても今回３Ｄ位置を計算できた２Ｄ特徴点に対して３Ｄ位置を登録する（ステップＳ４０７）。 The map creating unit 12 refers to the frame DB, and determines the 2D feature point of the previous key frame (key frame number of which is MKF-1) and the 2D feature point of the current key frame (key frame number of which is MKF). Then, 2D feature points (corresponding feature points) that can be corresponded are extracted (step S405). Since the orientation (translation vector t and rotation matrix R) of each key frame is also registered in the frame DB, the 3D position of the corresponding feature point can be determined in the same manner as in the initialization process of the self-position estimation process. Can be calculated. The map creating unit 12 registers the corresponding feature point for which the 3D position has been calculated as a Map point in the Map point DB (Step S406). The map creating unit 12 registers the 3D position for the 2D feature point for which the 3D position was calculated this time also for the frame DB (step S407).
なお、地図作成部１２が抽出した対応特徴点がすでにＭａｐ点ＤＢに登録済だった場合は、３Ｄ位置計算をスキップして次の対応特徴点（Ｍａｐ点ＤＢに未登録のもの）に対する処理に進んでも良いし、改めて３Ｄ位置計算を行って、Ｍａｐ点ＤＢに登録済の３Ｄ位置や、フレームＤＢ中の対応特徴点に対する３Ｄ位置を更新するようにしても良い。 If the corresponding feature point extracted by the map creation unit 12 has already been registered in the Map point DB, the 3D position calculation is skipped and the processing for the next corresponding feature point (one not registered in the Map point DB) is performed. 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 with respect to the corresponding feature point in the frame DB.
次に、地図作成部１２は、キーフレームキューが空かどうかを判定する（ステップＳ４０８）。空であれば（ステップＳ４０８；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３Ｄ位置に対して、バンドルアジャストメント処理を行って、精度向上を図ってから（ステップＳ４０９）ステップＳ４０１に戻る。キーフレームキューが空でなければ（ステップＳ４０８；Ｎｏ）ステップＳ４０１に戻る。 Next, the map creation unit 12 determines whether the key frame queue is empty (Step S408). If it is empty (Step S408; Yes), the bundle adjustment process is performed on the postures of all key frames and the 3D positions of all Map points to improve the accuracy (Step S409), and the process returns to Step S401. If the key frame queue is not empty (step S408; No), the process returns to step S401.
なお、バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とＭａｐ点の３Ｄ位置とを同時に推定する非線形最適化法であり、Ｍａｐ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。 Note that the bundle adjustment process is a nonlinear optimization method for simultaneously estimating the camera posture (key frame posture) and the 3D position of the Map point, and an error generated when the Map point is projected on the key frame. This is to perform optimization so as to minimize it.
このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３Ｄ位置の精度向上を図ることができる。しかし、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、他の処理がない状態のとき（例えば、キーフレームキューが空の状態）であっても、この処理を毎回行う必要はない。 By performing the bundle adjustment process, it is possible to improve the accuracy of the key frame posture and the 3D position of the Map point. However, even if this process is not performed, the accuracy cannot be improved, but no special problem occurs. Therefore, even when there is no other processing (for example, the key frame queue is empty), it is not necessary to perform this processing every time.
また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいＭａｐ点が見つかることがある。このような誤差の大きなＭａｐ点については、ＳＬＡＭ推定に悪影響を及ぼすため、Ｍａｐ点ＤＢ及びフレームＤＢから削除したり、誤差の大きな注意を要するＭａｐ点であることを識別するためのフラグを立てておいたりしても良い。なお、バンドルアジャストメントの処理は本実施形態においてはオプション的な扱いになるため、ここでは処理の詳細については省略する。 In addition, when the bundle adjustment process is performed, a Map point where an error when projected onto a key frame is larger than a predetermined value may be found. Since such a Map point having a large error has a bad influence on the SLAM estimation, it is deleted from the Map point DB and the frame DB, or a flag is set for identifying a Map point that requires a great deal of attention. You may leave it. Note that the processing of the bundle adjustment is optional in the present embodiment, and thus the details of the processing are omitted here.
次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０３で実行されるＤＢを生成１の処理について、図１０を用いて説明する。この処理では撮像部３３で撮像した物体の境界等が写る画像から、画像情報取扱部１６が、画像先鋭度値を計算して、画像を撮像した時の自律移動装置１００の姿勢（３次元の位置及び３次元の回転）と、画像を撮像した時刻とともに１つの情報Ｇとして生成し、情報記憶部２３に保存されている情報Ｇの姿勢に近い姿勢で撮像された画像から得られた情報Ｇ０の画像先鋭度値と情報Ｇの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部２３に保存する。
Next, the process of generating the DB 1 executed in step S203 of the self-location estimation and environment map generation thread (FIG. 7) will be described with reference to FIG. In this process, the image
まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある画像先鋭度値データベースＤＢをクリアするように指示があるか否かを判定する（ステップＳ５０１）。クリアするように指示があるなら（ステップＳ５０１；Ｙｅｓ）画像先鋭度値データベースＤＢをクリアし（ステップＳ５０２）、クリアするように指示がないなら（ステップＳ５０１；Ｎｏ）、ステップＳ５０３に進む。画像先鋭度値データベースＤＢをクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。画像先鋭度値データベースＤＢをクリアする指示は、例えば、自律移動装置１００に備えられたスイッチ（図示せず）をレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話等によってレンズ清掃の確認ができたことであっても良い。
First, the image
ステップＳ５０３では、画像情報取扱部１６は、ステップＳ２０２で自機位置推定処理時等に取得した画像に対して以下の式（７）に示す４近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。
In step S503, the image
次に、画像情報取扱部１６は、ステップＳ２０２で自機位置推定処理時に計算した自律移動装置１００の姿勢を、画像先鋭度値が算出された画像が撮像された時の姿勢とし、この姿勢と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Ｇとする（ステップＳ５０４）。
Next, the image
画像情報取扱部１６は、画像先鋭度値データベースＤＢ内に、情報Ｇの姿勢と所定の誤差ＴＨ０の範囲内の姿勢である情報があるかどうかを検索する（ステップＳ５０５）。所定の誤差ＴＨ０の範囲内の姿勢である情報があった場合（ステップＳ５０５；Ｙｅｓ）、その情報を情報Ｇ０として取り出し、無かった場合（ステップＳ５０５；Ｎｏ）、ステップＳ５０８へ進む。なお、所定の誤差ＴＨ０の範囲内の姿勢の情報が複数ある場合には、１番画像先鋭度値が大きいものを選ぶ。姿勢間の距離は、ユークリッド距離を用いる。回転の３次元は、Ｅｘｐｏｎｅｎｔｉａｌ Ｍａｐ Ｑｕａｔｅｒｎｉｏｎを用いることによって、ユークリッド空間で扱うことができる。
The image
次に、画像情報取扱部１６は、情報Ｇの画像先鋭度値と情報Ｇ０の画像先鋭度値を比較する（ステップＳ５０６）。情報Ｇの画像先鋭度値が情報Ｇ０の画像先鋭度値よりも大きい場合（ステップＳ５０６；Ｙｅｓ）、画像先鋭度値データベースＤＢ内の情報Ｇ０を削除する（ステップＳ５０７）。情報Ｇの画像先鋭度値が情報Ｇ０の画像先鋭度値よりも大きくない場合（ステップＳ５０６；Ｎｏ）、そのまま終了する。
Next, the image
ステップＳ５０８では、画像情報取扱部１６は、画像先鋭度値データベースＤＢに情報Ｇを登録し、終了する。
In step S508, the image
次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０４で実行されるレンズ汚れ判定１の処理について、図１１を用いて説明する。この処理では、ＤＢを生成１の処理で算定した情報Ｇの画像先鋭度値と、現時刻から所定の時間Ｔ０以前に撮像された過去の画像であって情報Ｇが撮像された姿勢から所定の誤差ＴＨ１内で情報Ｇの姿勢に１番近い姿勢である情報Ｇ１の画像先鋭度値に所定の係数を掛けたものと、を比較することにより、自律移動装置１００の撮像部３３のレンズの汚れを判定する。
Next, the process of lens dirt determination 1 executed in step S204 of the self-location estimation and environment map generation thread (FIG. 7) will be described with reference to FIG. In this process, a predetermined value is calculated from the image sharpness value of the information G calculated in the process of generating the DB 1 and the posture in which the information G was captured before the predetermined time T0 from the current time. The dirt on the lens of the
まず、画像情報取扱部１６は、画像先鋭度値データベースＤＢ内に、現時刻より所定の時間Ｔ０以前に撮像された物体の境界等が写る画像の情報であって、情報Ｇの画像が撮像された姿勢からの姿勢距離が所定の誤差（範囲）ＴＨ１以下で１番近い姿勢である画像の情報（以下、この情報を情報Ｇ１とする）を検索する（ステップＳ６０１）。画像先鋭度値データベースＤＢ内に、情報Ｇ１が存在する場合（ステップＳ６０１；Ｙｅｓ）、ステップＳ６０２に進む。画像先鋭度値データベースＤＢ内に、情報Ｇ１が存在しない場合（ステップＳ６０１；Ｎｏ）、ステップＳ６１０に進む。
First, the image
ステップＳ６０２では、情報Ｇ１の画像先鋭度値に１未満の係数ＴＨＧ（先鋭度値が落ちたとみなすためのマージン）を掛けたものと情報Ｇの画像先鋭度値とを比較する。情報Ｇ１の画像先鋭度値に係数ＴＨＧを掛けたものより情報Ｇの画像先鋭度値が大きい場合（ステップＳ６０２；Ｙｅｓ）、画像先鋭度フラグを１とし、現時刻、情報Ｇの姿勢とともに画像先鋭度フラグデータベースＤＢＧ内に保存し（ステップＳ６０３）、ステップＳ６０４に進む。情報Ｇ１の画像先鋭度値に係数ＴＨＧを掛けたものより情報Ｇの画像先鋭度値が大きくない場合（ステップＳ６０２；Ｎｏ）、画像先鋭度フラグを０とし、現時刻、情報Ｇの姿勢とともに画像先鋭度フラグデータベースＤＢＧ内に保存し（ステップＳ６０９）、ステップＳ６０４に進む。 In step S602, the image sharpness value of the information G is compared with a value obtained by multiplying the image sharpness value of the information G1 by a coefficient THG (a margin for considering that the sharpness value has dropped). When the image sharpness value of the information G is larger than the value obtained by multiplying the image sharpness value of the information G1 by the coefficient THG (step S602; Yes), the image sharpness flag is set to 1, and the image sharpness is set together with the current time and the attitude of the information G. It is stored in the degree flag database DBG (step S603), and proceeds to step S604. If the image sharpness value of the information G is not greater than the value obtained by multiplying the image sharpness value of the information G1 by the coefficient THG (step S602; No), the image sharpness flag is set to 0, and the image is displayed together with the current time and the attitude of the information G. It is stored in the sharpness flag database DBG (step S609), and the process proceeds to step S604.
次に、画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内の、現時刻より所定の時間Ｔ１以上古い情報を削除する（ステップＳ６０４）。
Next, the image
画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしているかを判定する（ステップＳ６０５）。ここで、全回転姿勢がＴＨ３方向をカバーしている、について説明する。例えば、ＴＨ３＝６であるとする。本来、回転姿勢は３次元で持っているが、通常、自律移動装置１００は水平な床面上を移動するので、回転姿勢の移動はＹａｗ角だけの１次元とみなす。ＴＨ３＝６としているので、３６０度／６＝６０度となる。従って、画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の回転方向が、６０度毎に異なる６方向をカバーしている必要がある。これにより、太陽光や照明の方向によるレンズ汚れの検出精度への影響を小さくすることができる。画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしている場合（ステップＳ６０５；Ｙｅｓ）、ステップＳ６０６に進む。画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしてない場合（ステップＳ６０５；Ｎｏ）、ステップＳ６１０に進む。
The image
ステップＳ６０６では、画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内の、画像先鋭度フラグが０である情報の割合Ｘを算出する。
In step S606, the image
算出した割合Ｘが所定の割合ＴＨ２より大きい場合（ステップＳ６０７；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れがあると判定する（ステップＳ６０８）。算出した割合Ｘが所定の割合ＴＨ２より大きくない場合（ステップＳ６０７；Ｎｏ）、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定する（ステップＳ６１０）。なお、ステップＳ６０１でＮｏの場合、ステップＳ６０５でＮｏの場合も、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定する（ステップＳ６１０）。
If the calculated ratio X is larger than the predetermined ratio TH2 (Step S607; Yes), it is determined that the lens of the
次に自律移動装置１００の起動時処理（図６）のステップＳ１０２で起動される移動指示アプリについて、図１２を用いて説明する。移動指示アプリは、行動計画アプリに設定された動作モードに応じて、自律移動装置１００の移動先を移動スレッドに通知する。動作モードには、自律移動装置１００を充電器２００に移動させる充電器帰還モード、自律移動装置１００をランダムに移動させるお散歩モード及び人とコミュニケーションを取る対話モードがある。他の動作モードを設定しても良い。
Next, the movement instruction application started in step S102 of the startup processing (FIG. 6) of the autonomous
まず、制御部１０は、動作終了かを判定する（ステップＳ７０１）。動作終了なら（ステップＳ７０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ７０１；Ｎｏ）動作モードを確認する（ステップＳ７０２）。行動計画アプリによって設定された動作モードが、充電器帰還モードならステップＳ７０３に進み、お散歩モードならステップＳ７０６に進み、対話モードならステップＳ７０８に進む。
First, the
動作モードが充電器帰還モードの場合、制御部１０は、充電器２００を自律移動装置１００の目的地に設定するように移動スレッドに通知する（ステップＳ７０３）。充電器帰還モードでは、充電器２００からの赤外線ビーコンである帰還信号を頼りに自律移動装置１００を充電器２００に移動させる。ステップＳ７０４に進み、制御部１０は、レンズ汚れ判定２を行う。レンズ汚れ判定２の詳細については後述する。
When the operation mode is the charger return mode, the
その後、制御部１０は、自律移動装置１００が目的地である充電器２００に到着したかどうかを判定する（ステップＳ７０５）。自律移動装置１００が充電器２００に到着していれば（ステップＳ７０５；Ｙｅｓ）、ステップＳ７０１に戻る。自律移動装置１００が充電器２００に到着していなければ（ステップＳ７０５；Ｎｏ）、ステップＳ７０４に戻る。
Thereafter, the
動作モードがお散歩モードの場合、制御部１０は、ランダムに目的地を移動スレッドに通知する。なお、目的地は、環境地図内の空いている空間や他のスレッドからの移動要求（例えば、スマホからのマニュアル動作等）等から決定しても良い（ステップＳ７０６）。
When the operation mode is the stroll mode, the
所定の時間ｔ経過後、ステップＳ７０７に進み、後述する移動スレッドが終了していれば（ステップＳ７０７；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７０７；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７０７に戻る。 After the elapse of the predetermined time t, the process proceeds to step S707. If the moving thread described below has been completed (step S707; Yes), the process returns to step S701. If the moving thread has not ended (step S707; No), the process returns to step S707 after a predetermined time t has elapsed.
動作モードが対話モードの場合、制御部１０は、最新の画像内に顔領域を検出したかどうかを判定する（ステップＳ７０８）。画像内に顔領域を検出できれば（ステップＳ７０８；Ｙｅｓ）、制御部１０は、顔（検出）領域が撮像画像全体に行き渡るような方向に自律移動装置１００を移動させるように移動指示を移動スレッドに通知する（ステップＳ７０９）。画像内に顔領域を検出できない場合（ステップＳ７０８；Ｎｏ）、制御部１０は、ランダムに目的地を移動スレッドに通知する、或いは、環境地図内の空いている空間や他のスレッドからの移動要求（例えば、スマホからのマニュアル動作等）等から目的地を決定しても良い（ステップＳ７１１）。
If the operation mode is the interactive mode, the
ステップＳ７０９では、所定の時間ｔ経過後、ステップＳ７１０に進み、移動スレッドが終了していれば（ステップＳ７１０；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７１０；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７１０に戻る。 In step S709, after a predetermined time t has elapsed, the process proceeds to step S710. If the moving thread has ended (step S710; Yes), the process returns to step S701. If the moving thread has not ended (step S710; No), the process returns to step S710 after a predetermined time t has elapsed.
ステップＳ７１１では、所定の時間ｔ経過後、ステップＳ７１２に進み、移動スレッドが終了していれば（ステップＳ７１２；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７１２；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７１２に戻る。 In step S711, after a predetermined time t has elapsed, the process proceeds to step S712, and if the moving thread has ended (step S712; Yes), the process returns to step S701. If the moving thread has not ended (step S712; No), the process returns to step S712 after a predetermined time t has elapsed.
次にステップＳ７０４で実行されるレンズ汚れ判定処理２について図１３を用いて説明する。この処理では、充電器２００に戻る途中、充電器２００まで所定の距離ＤＩＳＴの地点付近で撮像した画像の画像先鋭度値と、画像先鋭度値データベースＤＢ２に保存されている充電器２００まで所定の距離ＤＩＳＴの地点付近で撮像した画像の画像先鋭度値とを比較することにより撮像部３３の汚れがあるかを判定する。
Next, the lens
まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある画像先鋭度値データベースＤＢ２をクリアするように指示があるか否かを判定する（ステップＳ８０１）。クリアするように指示がある場合（ステップＳ８０１；Ｙｅｓ）、画像先鋭度値データベースＤＢ２をクリアし（ステップＳ８０２）、クリアするように指示がない場合（ステップＳ８０１；Ｎｏ）、ステップＳ８０３に進む。画像先鋭度値データベースＤＢ２をクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。画像先鋭度値データベースＤＢ２をクリアする指示は、例えば、自律移動装置１００に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。
First, the image
ステップＳ８０３では、制御部１０は、自律移動装置１００が充電器２００から帰還信号を受信中であるか及び、自律移動装置１００が充電器２００からの距離（或いは自律移動装置１００の走行距離）が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内であるかを障害物センサ４３よって判断する。自律移動装置１００が充電器２００から帰還信号を受信中であり、かつ、自律移動装置１００が充電器２００からの距離が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内である場合（ステップＳ８０３；Ｙｅｓ）、制御部１０は、撮像部３３に物体の境界等が写る画像を撮像させ、画像情報取扱部１６に、撮像により取得した画像に対して４近傍のラプラシアンオペレータの畳み込み処理を行わせた後、分散を取らせ、画像先鋭度値を算定させる（ステップＳ８０４）。自律移動装置１００が充電器２００から帰還信号を受信中ではない場合、或いは、自律移動装置１００が充電器２００からの距離が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内でない場合（ステップＳ８０３；Ｎｏ）、戻る。
In step S803, the
ステップＳ８０５では、画像情報取扱部１６は、画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算出された画像先鋭度値が保存されているかどうかを判定する。画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算定された画像先鋭度値が保存されている場合（ステップＳ８０５；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値に１未満の所定の係数ＴＨＨ３を掛けたものとステップＳ８０４で算出された画像先鋭度値とを比較する（ステップＳ８０６）。画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算定された画像先鋭度値が保存されていない場合（ステップＳ８０５；Ｎｏ）、画像情報取扱部１６は、ステップＳ８０４で算出された画像先鋭度値を画像先鋭度値データベースＤＢ２に保存し（ステップＳ８０９）、戻る。
In step S805, the image
保存されている画像先鋭度値に所定の係数ＴＨＨ３を掛けたものよりステップＳ８０４で算出された画像先鋭度値が大きくない場合（ステップＳ８０６；Ｎｏ）、画像情報取扱部１６は、レンズの汚れがあると判定する。このとき、制御部１０は、レンズの汚れがあると行動計画アプリに通知する（ステップＳ８１０）。行動計画アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、戻る。保存されている画像先鋭度値に所定の係数ＴＨＨ３を掛けたものよりステップＳ８０４で算出された画像先鋭度値が大きい場合（ステップＳ８０６；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きいか否かを判定する（ステップＳ８０７）。
If the image sharpness value calculated in step S804 is not larger than the value obtained by multiplying the stored image sharpness value by a predetermined coefficient THH3 (step S806; No), the image
保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きい場合（ステップＳ８０７；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値をステップＳ８０４で算出された画像先鋭度値に入れ替えて画像先鋭度値データベースＤＢ２に保存し（ステップＳ８０８）、戻る。保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きくない場合（ステップＳ８０７；Ｎｏ）、戻る。
If the image sharpness value calculated in step S804 is larger than the stored image sharpness value (step S807; Yes), the image
以上、レンズ汚れ判定２の処理について説明した。次に、自律移動装置１００の起動時処理（図６）のステップＳ１０３で起動される移動スレッドについて、図１４を用いて説明する。このスレッドでは、移動指示アプリ等による移動指示に従って、自律移動装置１００を移動指示のあった方向や目的地まで移動させる。
The processing of the
制御部１０は、まず動作終了かどうかを判定し（ステップＳ９０１）、移動指示アプリ等から指示された方向や目的地に自律移動装置１００が移動済み等で動作終了なら（ステップＳ９０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ９０１；Ｎｏ）、移動指示アプリ等から移動指示があるかを判定する（ステップＳ９０２）。
The
移動指示アプリ等から移動指示がある場合（ステップＳ９０２；Ｙｅｓ）、制御部１０は、移動指示アプリ等から指示された方向や目的地に自律移動装置１００を移動させた後（ステップＳ９０３）、ステップＳ９０１に戻る。移動指示アプリ等から移動指示が無い場合（ステップＳ９０２；Ｎｏ）、ステップＳ９０１に戻る。
If there is a movement instruction from the movement instruction application or the like (Step S902; Yes), the
以上、移動スレッドについて説明した。次に、自律移動装置１００の起動時処理（図６）のステップＳ１０３で起動される顔検出スレッドについて、図１５を用いて説明する。このスレッドでは、取得した画像に顔領域を検出した場合、移動指示アプリに通知し、撮像部３３のレンズの汚れ判定を行う。
The moving thread has been described above. Next, the face detection thread that is started in step S103 of the startup process (FIG. 6) of the autonomous
制御部１０は、まず動作終了かどうかを判定し（ステップＳ１００１）、動作終了なら（ステップＳ１００１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ１００１；Ｎｏ）、撮像部３３に自律移動装置１００の前方を撮像させ、画像を取得する（ステップＳ１００２）。
The
ステップＳ１００３では、制御部１０は、画像内に人の顔の輪郭、目、鼻及び口等の形状に類似する形状を有する顔領域を検出したかを判定する。画像内に顔領域を検出した場合（ステップＳ１００３；Ｙｅｓ）、顔検出領域を移動指示アプリに通知する（ステップＳ１００４）。画像内に顔領域を検出しなかった場合（ステップＳ１００３；Ｎｏ）、ステップＳ１００１に戻る。
In step S1003, the
ステップＳ１００５では、制御部１０は、画像情報取扱部１６に、ステップＳ１００２で取得した画像の顔（検出）領域について画像先鋭度値を算出させることにより、顔領域先鋭度値データベースＤＢ３を生成（ＤＢを生成３）する。顔領域先鋭度値データベースＤＢ３の生成する処理の詳細については後述する。
In step S1005, the
そして、制御部１０は、画像先鋭度値を用いて撮像部３３のレンズの汚れを判定（レンズ汚れ判定３）する（ステップＳ１００６）。撮像部３３のレンズの汚れを判定する処理については後述する。
Then, the
撮像部３３のレンズに汚れが有ると判定された場合（ステップＳ１００７；Ｙｅｓ）、制御部１０は、その旨を行動計画アプリに通知する（ステップＳ１００８）。行動計画アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップＳ１００１に戻る。撮像部３３に汚れが有ると判定されなかった場合（ステップＳ１００７；Ｎｏ）、ステップＳ１００１に戻る。
When it is determined that the lens of the
次に、顔検出スレッド（図１５）のステップＳ１００５で実行されるＤＢを生成３の処理について、図１６を用いて説明する。この処理では撮像部３３で撮像した顔領域が検出された顔検出画像から、画像情報取扱部１６が、画像先鋭度値を計算して、顔検出領域と、画像を撮像した時刻とともに１つの情報Ｈとして生成し、情報記憶部２３に保存されている情報Ｈの顔検出領域に近い顔検出領域である画像から得られた情報Ｈ０の画像先鋭度値と情報Ｈの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部２３に保存する。
Next, the process of generating a
まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある顔領域先鋭度値データベースＤＢ３をクリアするように指示があるか否かを判定する（ステップＳ１１０１）。クリアするように指示があるなら（ステップＳ１１０１；Ｙｅｓ）顔領域先鋭度値データベースＤＢ３をクリアし（ステップＳ１１０２）、クリアするように指示がないなら（ステップＳ１１０１；Ｎｏ）、ステップＳ１１０３に進む。顔領域先鋭度値データベースＤＢ３をクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。顔領域先鋭度値データベースＤＢ３をクリアする指示は、画像先鋭度値データベースＤＢをクリアするときと同様に、自律移動装置１００に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。
First, the image
ステップＳ１１０３では、画像情報取扱部１６は、顔検出スレッドのステップＳ１００２で取得した画像に対して４近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。
In step S1103, the image
次に、画像情報取扱部１６は、ステップＳ１００２で取得した顔検出領域と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Ｈとする（ステップＳ１１０４）。
Next, the image
画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に情報Ｈの顔検出領域と所定の誤差ＴＨ２０の範囲内（基準領域内）の顔検出領域である過去の情報があるかどうかを検索する（ステップＳ１１０５）。所定の誤差ＴＨ２０の範囲内の顔検出領域である情報があった場合（ステップＳ１１０５；Ｙｅｓ）、その情報を情報Ｈ０として取り出し、無かった場合（ステップＳ１１０５；Ｎｏ）、ステップＳ１１０８へ進む。なお、所定の誤差ＴＨ２０の範囲内の顔検出領域の情報が複数ある場合には、１番顔検出領域が近いものを選ぶ。領域間の距離は、ユークリッド距離を用いる。領域の開始点（ｘ、ｙ）と、領域の大きさ（ｗ、ｈ）の４次元空間のユークリッド距離とすることができる。
The image
次に、画像情報取扱部１６は、情報Ｈの画像先鋭度値と情報Ｈ０の画像先鋭度値を比較する（ステップＳ１１０６）。情報Ｈの画像先鋭度値が情報Ｈ０の画像先鋭度値よりも大きい場合（ステップＳ１１０６；Ｙｅｓ）、顔領域先鋭度値データベースＤＢ３内の情報Ｈ０を削除する（ステップＳ１１０７）。情報Ｈの画像先鋭度値が情報Ｈ０の画像先鋭度値よりも大きくない場合（ステップＳ１１０６；Ｎｏ）、そのまま終了する。
Next, the image
ステップＳ１１０８では、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３に情報Ｈを登録し、終了する。
In step S1108, the image
次に、顔検出スレッド（図１５）のステップＳ１００６で実行されるレンズ汚れ判定３の処理について、図１７を用いて説明する。この処理では、ＤＢを生成３の処理で算定した情報Ｈの先鋭度値と、顔領域先鋭度値データベースＤＢ３内の情報の画像先鋭度値の平均値ＨＭ等を使用することにより、自律移動装置１００の撮像部３３のレンズの汚れを判定する。
Next, the process of
まず、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の数が所定の値Ｎ以上であるかを判定する（ステップＳ１２０１）。顔領域先鋭度値データベースＤＢ３内に保存されている情報の数がＮ以上である場合（ステップＳ１２０１；Ｙｅｓ）、ステップＳ１２０２に進む。顔領域先鋭度値データベースＤＢ３内に保存されている情報の数がＮ以上でない場合（ステップＳ１２０１；Ｎｏ）、ステップＳ１２１１に進む。
First, the image
ステップＳ１２０２では、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の画像先鋭度値の平均値ＨＭを算出する。
In step S1202, the image
画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の画像先鋭度値の平均値ＨＭに１未満の所定の係数ＴＨＨ２を掛けたものより情報Ｈの画像先鋭度値が大きいかを判定する（ステップＳ１２０３）。平均値ＨＭにＴＨＨ２を掛けたものより情報Ｈの画像先鋭度値が大きい場合（ステップＳ１２０３；Ｙｅｓ）、画像先鋭度フラグを１とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存し（ステップＳ１２０４）、ステップＳ１２０６に進む。平均値ＨＭにＴＨＨ２を掛けたものより情報Ｈの先鋭度値が大きくない場合（ステップＳ１２０３；Ｎｏ）、画像先鋭度フラグを０とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存し（ステップＳ１２０５）、ステップＳ１２０６に進む。
The image
次に、画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の、現時刻より所定の時間Ｔ１１以上古い情報を削除する（ステップＳ１２０６）。
Next, the image
画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存されている情報の数が所定の値Ｎ２以上であり、かつ、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしているかを判定する（ステップＳ１２０７）。顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存されている情報の数がＮ２以上であり、かつ、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしている場合（ステップＳ１２０７；Ｙｅｓ）、ステップＳ１２０８に進む。顔領域先鋭度フラグデータベースＤＢ２Ｈ内に保存されている情報の数が所定の値Ｎ２以上でない、又は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしていない場合（ステップＳ１２０７；Ｎｏ）、ステップＳ１２１１に進む。
The image
ステップＳ１２０８では、画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の、先鋭度フラグが０である情報の割合Ｘ２を算出する。
In step S1208, the image
算出した割合Ｘ２が所定の割合ＴＨ１２より大きい場合（ステップＳ１２０９；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れがあると判定し（ステップＳ１２１０）、終了する。算出した割合Ｘ２が所定の割合ＴＨ１２より大きくない場合（ステップＳ１２０９；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定し（ステップＳ１２１１）、終了する。また、ステップＳ１２０１でＮｏの場合、ステップＳ１２０７でＮｏの場合も、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定し（ステップＳ１２１１）、終了する。
If the calculated ratio X2 is larger than the predetermined ratio TH12 (step S1209; Yes), it is determined that the lens of the
以上説明したように、自律移動装置１００は、ＤＢを生成１・３の処理及びレンズ汚れ判定１〜３の処理において、画像先鋭度値をラプラシアンオペレータの畳み込み処理を行った後、分散を取るので、画像全体又は顔画像領域内のエッジ及び濃度を算定でき、撮像部のレンズの一部に汚れがあった場合でもレンズの汚れを好適に検出することができる。
As described above, the autonomous
そして、検出したレンズの汚れをユーザに知らせることにより、好ましいタイミングで、ユーザが汚れたレンズを清掃することができ、画像処理による自機位置推定及び環境地図生成のロバスト性と精度を維持することができる。 By notifying the user of the detected lens dirt, the user can clean the dirty lens at a preferable timing, and maintain robustness and accuracy in estimating the position of the own device by image processing and generating an environmental map. Can be.
上記実施形態では、画像先鋭度値の算出にラプラシアンオペレータと分散を使用したが、離散フーリエ変換（Ｄｉｓｃｒｅｔｅ Ｆｏｕｒｉｅｒ Ｔｒａｎｓｆｏｒｍ）や高速フーリエ変換（Ｆａｓｔ Ｆｏｕｒｉｅｒ Ｔｒａｎｓｆｏｒｍ）をはじめとするフーリエ変換等の周波数解析（法）を用いても良い。フーリエ変換を用いた場合、例えばフーリエ変換後のパワースペクトルのうち最もパワー（周波数成分の振幅の２乗）が大きい周波数を画像先鋭度値（代表先鋭度値）とする。ラプラシアンオペレータの代わりに、ロバーツ、プリビットやソーベル等のエッジ検出オペレータでエッジ検出しても良い。 In the above embodiment, the Laplacian operator and the variance are used to calculate the image sharpness value. ) May be used. When the Fourier transform is used, for example, a frequency having the largest power (square of the amplitude of the frequency component) in the power spectrum after the Fourier transform is set as an image sharpness value (representative sharpness value). Instead of the Laplacian operator, edge detection may be performed by an edge detection operator such as Roberts, Prebit or Sobel.
自律移動装置１００は、入力部として、自律移動装置１００を操作するための操作ボタンを備えていても良い。操作ボタンは、例えば、電源ボタン、モード切替ボタン（掃除モード、ペットモード等を切り替える）、初期化ボタン（地図の作成をやり直しさせる）などを含む。入力部として、音の入力を行うマイク（図示せず）と、自律移動装置１００への操作指示の音声を認識する音声認識部を備えても良い。
The autonomous
ＤＢを生成１及びレンズ汚れ判定１の処理では、撮像画像全体を使用して画像の判定を行っているが、自律移動装置１００の下方（床近辺）は物の配置の変化が起こりやすく画像が変わりやすいので、物の配置の変化等が起こりにくい一定の高さ以上の部分の画像を使用して、判定を行っても良い。 In the processing of DB generation 1 and lens dirt determination 1, image determination is performed using the entire captured image. However, the position of an object is likely to change below the autonomous mobile device 100 (near the floor), and the image is not displayed. Since it is easy to change, the determination may be made by using an image of a portion having a certain height or higher in which the change of the arrangement of the object does not easily occur.
レンズ汚れ判定１の処理では、回転姿勢を条件に用いたが、位置を用いても良い。或いは、環境地図をＱ個のブロックに分け、そのうちのいくつかのブロック内の画像を有していることを条件としても良いし、位置及び回転の両方を条件としても良い。条件を満たしていない場合、条件を満たすような移動指示を発するようにしても良い。 In the processing of the lens dirt determination 1, the rotation posture is used as a condition, but the position may be used. Alternatively, the environment map may be divided into Q blocks, and the condition may be that the images have images in some of the blocks, or both the position and the rotation may be the conditions. If the condition is not satisfied, a movement instruction that satisfies the condition may be issued.
顔検出スレッド、ＤＢを生成３及びレンズ汚れ判定３では、顔を検出対象としていたが、顔以外の物体、例えば、衣服や眼鏡、ペット等を検出対象としても良く、特定の個人の顔のみを検出対象としても良く、ＴＶ検出としても良い。
In the face detection thread, the
上記実施形態では、カメラを用いるＶｉｓｕａｌ ＳＬＡＭを用いたが、カメラの代わりにＬＲＦ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）を用いたＳＬＡＭであっても良い。この場合、物体認識用のカメラについての汚れ検出を行う。 In the above embodiment, a Visual SLAM using a camera is used, but a SLAM using an LRF (Laser Range Finder) instead of the camera may be used. In this case, dirt detection is performed on the camera for object recognition.
行動計画アプリの動作モードは、ユーザからのマニュアル操作で選択しても良く、所定のアルゴリズム、例えば、時間や人を認識した数などにより自動でモード選択をしても良い。 The operation mode of the action plan application may be selected by a manual operation from the user, or may be automatically selected according to a predetermined algorithm, for example, time or the number of recognized persons.
１つの画像先鋭度値は、１回の撮像により得られた画像又は物体検出領域から算定されるが、２回以上の撮像により得られた画像から１つの画像先鋭度値を算定しても良い。また、画像先鋭度値は、１つの画像又は物体検出領域から代表先鋭度値が１つ算定されるが、１つの画像又は物体検出領域のうちの一定の範囲毎に１つづつ算定されるようにしても良い。 One image sharpness value is calculated from an image or an object detection area obtained by one imaging, but one image sharpness value may be calculated from an image obtained by two or more imagings. . As for the image sharpness value, one representative sharpness value is calculated from one image or object detection area, but one representative sharpness value is calculated for each predetermined range in one image or object detection area. You may do it.
汚れ検査用のマーカーシートを撮像することにより得られる画像から画像先鋭度値を算定しても良い。マーカーシートは、自律移動装置１００の移動範囲内に、必要な時に配置すれば良い。マーカーシートは、汚れ検出が容易な模様、例えば市松模様等がデザインされていると良い。
The image sharpness value may be calculated from an image obtained by imaging the stain inspection marker sheet. The marker sheet may be arranged within the movement range of the autonomous
上記実施形態では、撮像部３３が単眼である単眼ＳＬＡＭの例を示したが、複数の撮像部を用いた複眼ＳＬＡＭでも同様の構成を採ることができる。例えば自律移動装置１００が二台の撮像部３３を備えている場合は、移動しなくても同じ位置から二つの画像を取得できるため、自機位置推定処理の初期化においてはステップＳ３０４の一回の動作で二つの画像が取得できる。それら二つの画像それぞれに基準値以上の特徴点が含まれていれば（ステップＳ３０６で判定する）、ステップＳ３０７では「Ｎ＝Ｎ＋２」を行い、ステップＳ３０８は常に「Ｎｏ」に進むことになる。そして、二つの画像の一方を前フレーム、もう一方を現フレームとして処理を行う場合は、ステップＳ３１９でスケールＳを計算する際の「オドメトリ並進距離」は、二つの撮像部間の距離を用いれば良い。これによりオドメトリからの取得する位置の精度が低い場合でも安定して初期化を行うことができる。
In the above-described embodiment, an example in which the
ただし、この場合の二つの撮像部間の距離は、通常用いられる基準並進距離（例えば１ｍ）よりも短い距離になってしまうことが多く、オドメトリから得られる並進距離の精度が高い場合は、むしろ単眼ＳＬＡＭと同様の初期化処理（例えば複数ある撮像部のうちの第一撮像部のみを使った初期化処理）を行った方が高い精度が得られることもある。そこで、単眼ＳＬＡＭと同様の初期化処理と、前段落で記載した二つの撮像部による初期化処理とを両方とも行って、誤差の小さいと思われる情報を採用するようにしても良い。 However, in this case, the distance between the two imaging units is often shorter than a standard translation distance (for example, 1 m) that is usually used, and when the accuracy of the translation distance obtained from odometry is high, A higher accuracy may be obtained by performing the same initialization processing as that of the monocular SLAM (for example, the initialization processing using only the first imaging unit of the plurality of imaging units). Therefore, both the initialization processing similar to the monocular SLAM and the initialization processing by the two imaging units described in the preceding paragraph may be performed to adopt information considered to have a small error.
この発明の自律移動装置１００の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）及びＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ Ｄｉｓｃ）等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。
Each function of the autonomous
以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。 As described above, the preferred embodiments of the present invention have been described, but the present invention is not limited to the specific embodiments, and the present invention includes the inventions described in the claims and equivalents thereof. It is. Hereinafter, the invention described in the claims of the present application is additionally described.
（付記１）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して撮像する撮像部と、
撮像された画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御し、前記画像又は前記画像に検出された物体の物体検出領域を構成する構造の細かさを示す画像先鋭度値を算定する制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第１の画像の第１の画像先鋭度値と、
前記第１の画像が撮像された時の前記自機位置及び前記自機の向きから所定の範囲内の自機位置及び自機の向きで撮像された第２の画像の第２の画像先鋭度値とを、
又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像の第２の画像先鋭度値とを、算定し、前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記レンズが汚れていると判定する、
自律移動装置。
(Appendix 1)
A drive unit that moves and rotates the own machine to change the own machine position and the own machine direction,
An imaging unit for imaging via a lens;
An image storage unit that stores a captured image;
A control unit that controls the driving unit, the imaging unit and the image storage unit, and calculates an image sharpness value indicating the fineness of a structure constituting an object detection area of the image or an object detected in the image, With
The control unit includes:
A first image sharpness value of a first image stored in the image storage unit;
The second image sharpness of the second image captured at the own position and the own direction within a predetermined range from the own device position and the own device direction when the first image was captured. Value and
Or a second image sharpness value of a second image including a second object detection region of the object within the reference region from the first object detection region of the object detected by the first image. Calculating, if the second image sharpness value is lower than the first image sharpness value, determining that the lens is dirty;
Autonomous mobile device.
（付記２）
前記画像先鋭度値は、前記画像全体又は前記物体検出領域全体に対して算出された代表先鋭度値である、
付記１に記載の自律移動装置。
(Appendix 2)
The image sharpness value is a representative sharpness value calculated for the entire image or the entire object detection area,
The autonomous mobile device according to supplementary note 1.
（付記３）
前記制御部は、前記画像又は前記検出領域をエッジ検出オペレータで処理し分散を取る、又は前記画像又は前記検出領域に対して周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、ことにより前記代表先鋭度値を算定する、
付記２に記載の自律移動装置。
(Appendix 3)
The control unit processes the image or the detection area with an edge detection operator and takes variance, or obtains the frequency of the maximum power from a power spectrum obtained by performing frequency analysis on the image or the detection area. Calculating the representative sharpness value by doing
An autonomous mobile device according to
（付記４）
前記撮像部は、物体の境界が写るように前記画像を撮像し、
前記制御部は、前記画像に写る前記物体の境界を前記エッジ検出オペレータで処理し分散を取る、又は前記周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、
付記３に記載の自律移動装置。
(Appendix 4)
The imaging unit captures the image so that a boundary of the object is captured,
The control unit acquires the frequency of the maximum power from the power spectrum obtained by processing the boundary of the object shown in the image by the edge detection operator and taking a variance, or by performing the frequency analysis.
An autonomous mobile device according to
（付記５）
前記制御部は、前記撮像部によって前記物体の境界が写るように前記画像を撮像できる自機位置及び自機の向きに移動するように前記駆動部を制御する、
付記４に記載の自律移動装置。
(Appendix 5)
The control unit controls the drive unit to move to its own position and the direction of its own device capable of capturing the image so that the boundary of the object is captured by the imaging unit,
An autonomous mobile device according to attachment 4.
（付記６）
前記物体は、顔である、
付記１から５のいずれか１つに記載の自律移動装置。
(Appendix 6)
The object is a face;
6. The autonomous mobile device according to any one of supplementary notes 1 to 5.
（付記７）
前記第１の画像及び前記第２の画像は、それぞれ２回以上の撮像により取得される、
付記１から６のいずれか１つに記載の自律移動装置。
(Appendix 7)
The first image and the second image are each obtained by imaging two or more times,
7. The autonomous mobile device according to any one of supplementary notes 1 to 6.
（付記８）
前記第２の画像は、充電設備に向かう際に撮像されることにより取得される、
付記１から７のいずれか１つに記載の自律移動装置。
(Appendix 8)
The second image is acquired by being imaged when heading to the charging facility,
8. The autonomous mobile device according to any one of supplementary notes 1 to 7.
（付記９）
距離測定素子をさらに備え、
前記自機の走行距離が所定の距離に達したと前記距離測定素子によって測定された時に前記第２の画像を撮像する、
付記１から８のいずれか１つに記載の自律移動装置。
(Appendix 9)
Further comprising a distance measuring element,
Capturing the second image when the travel distance of the own device reaches a predetermined distance and is measured by the distance measuring element;
The autonomous mobile device according to any one of supplementary notes 1 to 8.
（付記１０）
前記充電設備に向かう時に前記レンズの汚れを確認する、
付記８に記載の自律移動装置。
(Appendix 10)
Checking for dirt on the lens when heading to the charging facility,
An autonomous mobile device according to attachment 8.
（付記１１）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、前記撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
第１の画像が撮像された時の自機位置及び自機の向きを記憶しておき、自機を移動及び回転させて前記第１の画像を撮影した時の自機位置及び自機の向きと所定の範囲内にある時に、第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。
(Appendix 11)
An autonomous device including: a driving unit that moves and rotates the own device to change the position of the own device and the direction of the own device; an imaging unit that captures an image via a lens; and an image storage unit that stores the captured image. A method for detecting contamination of a lens of a moving device,
The position of the own device and the direction of the own device when the first image is captured are stored, and the position and the direction of the own device when the first image is captured by moving and rotating the own device. And when it is within a predetermined range, a second image is captured, and a first image, which is a past image, stored in the image storage unit is compared with the second image, which is a current image. Detecting the dirt on the lens of the autonomous mobile device.
（付記１２）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
自機を移動及び回転させて、前記画像記憶部に記憶されている第１の画像により検出された物体の物体検出領域から基準領域内にある前記物体の物体検出領域を含んだ第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である前記第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。
(Appendix 12)
An autonomous movement including a driving unit that moves and rotates the own device to change the position of the own device and the direction of the own device, an imaging unit that captures an image via a lens, and an image storage unit that stores the captured image. A method for detecting contamination of a lens of an apparatus, comprising:
A second image including the object detection area of the object located within the reference area from the object detection area of the object detected by the first image stored in the image storage unit by moving and rotating the own device; Autonomous movement to compare the first image, which is a past image, stored in the image storage unit, with the second image, which is a current image, to detect dirt on the lens. A method for detecting contamination of the lens of the apparatus.
（付記１３）
画像記憶部に記憶された第１の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第２の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を備える自律移動装置のレンズの汚れ検出方法。
(Appendix 13)
The position of the own device and the direction of the own device within a predetermined range from the position of the own device and the direction of the own device when the first image stored in the image storage unit is captured, or the first image is detected by the first image. The drive unit moves and moves the own device to the own device position and the own device direction that captures a second image including the second object detection region of the object that is within the reference region from the first object detection region of the object. An image capturing step of causing the image capturing unit to capture a second image by rotating the image capturing unit;
A control unit that controls the driving unit, the imaging unit, and the image storage unit calculates a first image sharpness value of the first image and a second image sharpness value of the second image. Calculation step;
When the second image sharpness value is lower than the first image sharpness value, the control unit determines that the lens of the imaging unit is dirty,
A method for detecting contamination of a lens of an autonomous mobile device, comprising:
（付記１４）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して画像を撮像する撮像部と、
撮像された前記画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、
を備えた自律移動装置のレンズの汚れ検出方法であって、
前記画像記憶部に記憶された第１の画像の特徴点と対応する前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗とし、
基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗率が基準失敗率より高い場合、レンズが汚れていると判定する、自律移動装置のレンズの汚れ検出方法。
(Appendix 14)
A drive unit that moves and rotates the own machine to change the own machine position and the own machine direction,
An imaging unit that captures an image via a lens;
An image storage unit that stores the captured image,
A control unit that controls the driving unit, the imaging unit, and the image storage unit to perform Visual SLAM control;
A method for detecting dirt on a lens of an autonomous mobile device comprising:
If the number of corresponding feature points of the second image captured by the imaging unit corresponding to the feature points of the first image stored in the image storage unit is less than the number of reference corresponding feature points, the visual SLAM control fails. age,
If the failure rate of the Visual SLAM control during the translation of the reference translation distance is higher than the reference failure rate, it is determined that the lens is dirty.
（付記１５）
コンピュータに、
画像記憶部に記憶された第１の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第２の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を実行させるためのプログラム。
(Appendix 15)
On the computer,
The position of the own device and the direction of the own device within a predetermined range from the position of the own device and the direction of the own device when the first image stored in the image storage unit is captured, or the first image is detected by the first image. The drive unit moves and moves the own device to the own device position and the own device direction that captures a second image including the second object detection region of the object that is within the reference region from the first object detection region of the object. An image capturing step of causing the image capturing unit to capture a second image by rotating the image capturing unit;
A control unit that controls the driving unit, the imaging unit, and the image storage unit calculates a first image sharpness value of the first image and a second image sharpness value of the second image. Calculation step;
When the second image sharpness value is lower than the first image sharpness value, the control unit determines that the lens of the imaging unit is dirty,
A program for executing
１０…制御部、１１…位置計測部、１２…地図作成部、１３…位置推定部、１４…判定部、１５…地図編集部、１６…画像情報取扱部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…情報記憶部、３１，３１ａ，３１ｂ…帰還信号受信部、３２，３２ａ，３２ｂ…駆動部、３３…撮像部、３４…通信部、３５…充電接続部、４０…センサ部、４１…加速度センサ、４２…角速度センサ、４３…障害物センサ、５１，５１ａ，５１ｂ…帰還信号送信部、５２…電力供給部、５３，５３ａ，５３ｂ…受信可能範囲、１００…自律移動装置、２００…充電器
DESCRIPTION OF
Claims (15)
レンズを介して撮像する撮像部と、
撮像された画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御し、前記画像又は前記画像に検出された物体の物体検出領域を構成する構造の細かさを示す画像先鋭度値を算定する制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第１の画像の第１の画像先鋭度値と、
前記第１の画像が撮像された時の前記自機位置及び前記自機の向きから所定の範囲内の自機位置及び自機の向きで撮像された第２の画像の第２の画像先鋭度値とを、
又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像の第２の画像先鋭度値とを、算定し、前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記レンズが汚れていると判定する、
自律移動装置。 A drive unit that moves and rotates the own machine to change the own machine position and the own machine direction,
An imaging unit for imaging via a lens;
An image storage unit that stores a captured image;
A control unit that controls the driving unit, the imaging unit and the image storage unit, and calculates an image sharpness value indicating the fineness of a structure constituting an object detection area of the image or an object detected in the image, With
The control unit includes:
A first image sharpness value of a first image stored in the image storage unit;
The second image sharpness of the second image captured at the own position and the own direction within a predetermined range from the own device position and the own device direction when the first image was captured. Value and
Or a second image sharpness value of a second image including a second object detection region of the object within the reference region from the first object detection region of the object detected by the first image. Calculating, if the second image sharpness value is lower than the first image sharpness value, determining that the lens is dirty;
Autonomous mobile device.
請求項１に記載の自律移動装置。 The image sharpness value is a representative sharpness value calculated for the entire image or the entire object detection area,
The autonomous mobile device according to claim 1.
請求項２に記載の自律移動装置。 The control unit processes the image or the detection area with an edge detection operator and takes variance, or obtains the frequency of the maximum power from a power spectrum obtained by performing frequency analysis on the image or the detection area. Calculating the representative sharpness value by doing
The autonomous mobile device according to claim 2.
前記制御部は、前記画像に写る前記物体の境界を前記エッジ検出オペレータで処理し分散を取る、又は前記周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、
請求項３に記載の自律移動装置。 The imaging unit captures the image so that a boundary of the object is captured,
The control unit acquires the frequency of the maximum power from the power spectrum obtained by processing the boundary of the object shown in the image by the edge detection operator and taking a variance, or by performing the frequency analysis.
The autonomous mobile device according to claim 3.
請求項４に記載の自律移動装置。 The control unit controls the drive unit to move to its own position and the direction of its own device capable of capturing the image so that the boundary of the object is captured by the imaging unit,
The autonomous mobile device according to claim 4.
請求項１から５のいずれか１項に記載の自律移動装置。 The object is a face;
The autonomous mobile device according to any one of claims 1 to 5.
請求項１から６のいずれか１項に記載の自律移動装置。 The first image and the second image are each obtained by imaging two or more times,
The autonomous mobile device according to any one of claims 1 to 6.
請求項１から７のいずれか１項に記載の自律移動装置。 The second image is acquired by being imaged when heading to the charging facility,
The autonomous mobile device according to any one of claims 1 to 7.
前記自機の走行距離が所定の距離に達したと前記距離測定素子によって測定された時に前記第２の画像を撮像する、
請求項１から８のいずれか１項に記載の自律移動装置。 Further comprising a distance measuring element,
Capturing the second image when the travel distance of the own device reaches a predetermined distance and is measured by the distance measuring element;
The autonomous mobile device according to claim 1.
請求項８に記載の自律移動装置。 Checking for dirt on the lens when heading to the charging facility,
An autonomous mobile device according to claim 8.
第１の画像が撮像された時の自機位置及び自機の向きを記憶しておき、自機を移動及び回転させて前記第１の画像を撮影した時の自機位置及び自機の向きと所定の範囲内にある時に、第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。 An autonomous device including: a driving unit that moves and rotates the own device to change the position of the own device and the direction of the own device; an imaging unit that captures an image via a lens; and an image storage unit that stores the captured image. A method for detecting contamination of a lens of a moving device,
The position of the own device and the direction of the own device when the first image is captured are stored, and the position and the direction of the own device when the first image is captured by moving and rotating the own device. And when it is within a predetermined range, a second image is captured, and a first image, which is a past image, stored in the image storage unit is compared with the second image, which is a current image. Detecting the dirt on the lens of the autonomous mobile device.
自機を移動及び回転させて、前記画像記憶部に記憶されている第１の画像により検出された物体の物体検出領域から基準領域内にある前記物体の物体検出領域を含んだ第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である前記第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。 An autonomous movement including a driving unit that moves and rotates the own device to change the position of the own device and the direction of the own device, an imaging unit that captures an image via a lens, and an image storage unit that stores the captured image. A method for detecting contamination of a lens of an apparatus, comprising:
A second image including the object detection area of the object located within the reference area from the object detection area of the object detected by the first image stored in the image storage unit by moving and rotating the own device; Autonomous movement to compare the first image, which is a past image, stored in the image storage unit, with the second image, which is a current image, to detect dirt on the lens. A method for detecting contamination of the lens of the apparatus.
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を備える自律移動装置のレンズの汚れ検出方法。 The position of the own device and the direction of the own device within a predetermined range from the position of the own device and the direction of the own device when the first image stored in the image storage unit is captured, or the first image is detected by the first image. The drive unit moves and moves the own device to the own device position and the own device direction that captures a second image including the second object detection region of the object that is within the reference region from the first object detection region of the object. An image capturing step of causing the image capturing unit to capture a second image by rotating the image capturing unit;
A control unit that controls the driving unit, the imaging unit, and the image storage unit calculates a first image sharpness value of the first image and a second image sharpness value of the second image. Calculation step;
When the second image sharpness value is lower than the first image sharpness value, the control unit determines that the lens of the imaging unit is dirty,
A method for detecting contamination of a lens of an autonomous mobile device, comprising:
レンズを介して画像を撮像する撮像部と、
撮像された前記画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、
を備えた自律移動装置のレンズの汚れ検出方法であって、
前記画像記憶部に記憶された第１の画像の特徴点と対応する前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗とし、
基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗率が基準失敗率より高い場合、レンズが汚れていると判定する、自律移動装置のレンズの汚れ検出方法。 A drive unit that moves and rotates the own machine to change the own machine position and the own machine direction,
An imaging unit that captures an image via a lens;
An image storage unit that stores the captured image,
A control unit that controls the driving unit, the imaging unit, and the image storage unit to perform Visual SLAM control;
A method for detecting dirt on a lens of an autonomous mobile device comprising:
If the number of corresponding feature points of the second image captured by the imaging unit corresponding to the feature points of the first image stored in the image storage unit is less than the number of reference corresponding feature points, the visual SLAM control fails. age,
If the failure rate of the Visual SLAM control during the translation of the reference translation distance is higher than the reference failure rate, it is determined that the lens is dirty.
画像記憶部に記憶された第１の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第２の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を実行させるためのプログラム。 On the computer,
The position of the own device and the direction of the own device within a predetermined range from the position of the own device and the direction of the own device when the first image stored in the image storage unit is captured, or the first image is detected by the first image. The drive unit moves and moves the own device to the own device position and the own device direction that captures a second image including the second object detection region of the object that is within the reference region from the first object detection region of the object. An image capturing step of causing the image capturing unit to capture a second image by rotating the image capturing unit;
A control unit that controls the driving unit, the imaging unit, and the image storage unit calculates a first image sharpness value of the first image and a second image sharpness value of the second image. Calculation step;
When the second image sharpness value is lower than the first image sharpness value, the control unit determines that the lens of the imaging unit is dirty,
A program for executing
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2018152021A JP7354528B2 (en) | 2018-08-10 | 2018-08-10 | Autonomous mobile device, method and program for detecting dirt on lenses of autonomous mobile device |
| JP2022110439A JP7327596B2 (en) | 2018-08-10 | 2022-07-08 | Autonomous mobile device, method and program for detecting dirt on lens of autonomous mobile device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2018152021A JP7354528B2 (en) | 2018-08-10 | 2018-08-10 | Autonomous mobile device, method and program for detecting dirt on lenses of autonomous mobile device |
Related Child Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2022110439A Division JP7327596B2 (en) | 2018-08-10 | 2022-07-08 | Autonomous mobile device, method and program for detecting dirt on lens of autonomous mobile device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2020027443A true JP2020027443A (en) | 2020-02-20 |
| JP7354528B2 JP7354528B2 (en) | 2023-10-03 |
Family
ID=69622190
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2018152021A Active JP7354528B2 (en) | 2018-08-10 | 2018-08-10 | Autonomous mobile device, method and program for detecting dirt on lenses of autonomous mobile device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7354528B2 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2022125319A (en) * | 2018-08-10 | 2022-08-26 | カシオ計算機株式会社 | Autonomous mobile device, and dirt detection method and program for lens of autonomous mobile device |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006230954A (en) * | 2005-02-28 | 2006-09-07 | Sharp Corp | Self-propelled type robot system |
| JP2011223181A (en) * | 2010-04-07 | 2011-11-04 | Sanyo Electric Co Ltd | Electronic camera |
| WO2012160901A1 (en) * | 2011-05-25 | 2012-11-29 | ソニー株式会社 | Robot device, control method for robot device, computer program, and program storage medium |
| US20130211659A1 (en) * | 2010-08-25 | 2013-08-15 | University Of Sydney | Sensor data processing |
| WO2013129361A1 (en) * | 2012-03-02 | 2013-09-06 | 日産自動車株式会社 | Three-dimensional object detection device |
| JP2015184903A (en) * | 2014-03-24 | 2015-10-22 | 日立建機株式会社 | Image processor of self-traveling industrial machine |
| JP2017538208A (en) * | 2014-11-26 | 2017-12-21 | アイロボット コーポレイション | System and method for performing simultaneous location mapping using a machine vision system |
| JP2019168938A (en) * | 2018-03-23 | 2019-10-03 | 東芝ライフスタイル株式会社 | Autonomous electric vacuum cleaner |
-
2018
- 2018-08-10 JP JP2018152021A patent/JP7354528B2/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006230954A (en) * | 2005-02-28 | 2006-09-07 | Sharp Corp | Self-propelled type robot system |
| JP2011223181A (en) * | 2010-04-07 | 2011-11-04 | Sanyo Electric Co Ltd | Electronic camera |
| US20130211659A1 (en) * | 2010-08-25 | 2013-08-15 | University Of Sydney | Sensor data processing |
| WO2012160901A1 (en) * | 2011-05-25 | 2012-11-29 | ソニー株式会社 | Robot device, control method for robot device, computer program, and program storage medium |
| WO2013129361A1 (en) * | 2012-03-02 | 2013-09-06 | 日産自動車株式会社 | Three-dimensional object detection device |
| JP2015184903A (en) * | 2014-03-24 | 2015-10-22 | 日立建機株式会社 | Image processor of self-traveling industrial machine |
| JP2017538208A (en) * | 2014-11-26 | 2017-12-21 | アイロボット コーポレイション | System and method for performing simultaneous location mapping using a machine vision system |
| JP2019168938A (en) * | 2018-03-23 | 2019-10-03 | 東芝ライフスタイル株式会社 | Autonomous electric vacuum cleaner |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2022125319A (en) * | 2018-08-10 | 2022-08-26 | カシオ計算機株式会社 | Autonomous mobile device, and dirt detection method and program for lens of autonomous mobile device |
| JP7327596B2 (en) | 2018-08-10 | 2023-08-16 | カシオ計算機株式会社 | Autonomous mobile device, method and program for detecting dirt on lens of autonomous mobile device |
Also Published As
| Publication number | Publication date |
|---|---|
| JP7354528B2 (en) | 2023-10-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6759307B2 (en) | Adaptive mapping using spatial aggregation of sensor data | |
| KR101776622B1 (en) | Apparatus for recognizing location mobile robot using edge based refinement and method thereof | |
| KR101776621B1 (en) | Apparatus for recognizing location mobile robot using edge based refinement and method thereof | |
| KR101725060B1 (en) | Apparatus for recognizing location mobile robot using key point based on gradient and method thereof | |
| KR101366860B1 (en) | Mobile robot and controlling method of the same | |
| Pradeep et al. | Robot vision for the visually impaired | |
| KR101784183B1 (en) | APPARATUS FOR RECOGNIZING LOCATION MOBILE ROBOT USING KEY POINT BASED ON ADoG AND METHOD THEREOF | |
| CN110968083B (en) | Method for constructing grid map, method, device and medium for avoiding obstacles | |
| JP6323439B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| JP6724439B2 (en) | Autonomous mobile device, autonomous mobile system, autonomous mobile method and program | |
| JP6187623B1 (en) | Autonomous mobile device, autonomous mobile method and program | |
| JP6311695B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| US11960297B2 (en) | Robot generating map based on multi sensors and artificial intelligence and moving based on map | |
| CN207488823U (en) | A kind of mobile electronic device | |
| JP2017107425A (en) | Autonomous mobile device, autonomous moving method and program | |
| Zhang et al. | An indoor navigation aid for the visually impaired | |
| CN108481327A (en) | A kind of positioning device, localization method and the robot of enhancing vision | |
| JP7354528B2 (en) | Autonomous mobile device, method and program for detecting dirt on lenses of autonomous mobile device | |
| US11774983B1 (en) | Autonomous platform guidance systems with unknown environment mapping | |
| JP7327596B2 (en) | Autonomous mobile device, method and program for detecting dirt on lens of autonomous mobile device | |
| JP2018156538A (en) | Autonomous mobile device, image processing method and program | |
| JP6638753B2 (en) | Autonomous mobile device, autonomous mobile method and program | |
| CN111971149A (en) | Recording medium, information processing apparatus, and information processing method | |
| Cumani et al. | Robot localisation and mapping with stereo vision. |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20210719 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20220428 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20220510 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20220708 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20221101 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20221222 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20230228 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230525 |
|
| A911 | Transfer to examiner for re-examination before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20230608 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20230704 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230725 |
|
| 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: 20230822 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230904 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7354528 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |