JP6235213B2

JP6235213B2 - 自律飛行ロボット

Info

Publication number: JP6235213B2
Application number: JP2013017247A
Authority: JP
Inventors: 遠藤　孝行; 孝行遠藤
Original assignee: Secom Co Ltd
Current assignee: Secom Co Ltd
Priority date: 2013-01-31
Filing date: 2013-01-31
Publication date: 2017-11-22
Anticipated expiration: 2033-01-31
Also published as: JP2014149622A

Description

本発明は、自己位置を推定できないときであっても自己位置を推定できるよう自律飛行する自律飛行ロボットに関する。

従来、予め記憶した地図情報および自身に備わる各種センサからのセンサ情報に基づいて自律的に移動可能な自律移動ロボットが提案されている。例えば、特許文献１には、レーザレンジファインダなどの距離検出センサを用いて自律移動ロボットの周囲の障害物等からの距離を測定し、予め記憶した建物等の障害物を含む地図情報（障害物マップ）と距離検出センサの出力値（距離データ）とを用いて移動空間における自己位置を推定して、自律的に移動可能な自律移動ロボットが開示されている。

特開２０１２−１３７９０９号公報

上記の従来技術のように距離検出センサを用いて自己位置を推定する方法を用いた場合、例えば、トラックなどの大きめの障害物が出現することによって障害物の設置環境が大きく変化した場合、予め記憶した障害物マップと実際の障害物のレイアウトとが大きく乖離することになるため、適切に自己位置を推定できないことがある。しかしながら、上記の従来技術では、このような自己位置を推定できない場合について考慮されていないため、自己位置が推定できない場合に適切に自律移動することができない。ところで、近年、移動経路上に存在する物体や段差等の障害物に移動を制限されることなく空間内を自由に移動でき、侵入者等から攻撃され難い位置に移動できるといった面から、小型の飛行ロボットを自律的に移動させて侵入者を監視するといった自律飛行ロボットの利用が検討されている。このような自律飛行ロボットにおいては自律飛行が困難となった場合、侵入者に攻撃され易くなるだけでなく、建物や人物への衝突や墜落等による事故を発生させる恐れがあった。そこで、本発明は、自己位置を推定できなかった場合であっても、自己位置を推定できるよう自律飛行することにより、自律飛行ロボットが安定的に自律飛行できるようにすることを目的とする。

かかる課題を解決するために、飛行空間の障害物マップを記憶した記憶部と、自律飛行ロボットの周囲に存在する障害物までの距離を測定する距離検出センサと、前記距離検出センサの出力及び前記障害物マップを用いて自律飛行ロボットの前記飛行空間における水平方向の自己位置である水平自己位置と高さ方向の自己位置である飛行高度とを推定する処理を行う位置推定手段と、前記位置推定手段にて前記水平自己位置を推定できるとき、前記自己位置から所定の移動目標位置に向かうよう移動制御する通常飛行制御処理と、前記位置推定手段にて前記水平自己位置を推定できないとき、前記飛行高度が高くなるよう移動制御する自己位置探索制御処理とを行う飛行制御手段と、を有し、前記各処理を逐次繰り返すことによって自律的に飛行することを特徴とする自律飛行ロボットを提供する。

かかる構成により、本発明の自律飛行ロボットは、位置推定手段にて水平自己位置を推定できなくなったとき、障害物の存在し易い低高度の位置から、比較的障害物の存在しない高高度の位置へと上昇することにより、水平自己位置を推定し易い位置へと自律飛行することができる。

また、本発明の好ましい態様として、前記自己位置探索制御処理は、更に、水平方向へ移動しないよう移動制御するものとする。

かかる構成により、水平自己位置が推定できなくなったとき、その位置から真上に向かって上昇することにより、周囲に存在する障害物や人物等への衝突の危険性を回避しつつ、水平自己位置を推定し易い位置へと自律飛行することができる。

また、本発明の好ましい態様として、前記飛行制御手段は、更に、前記位置推定手段にて前記水平自己位置を推定できないときであって、かつ、前記飛行高度が所定の閾値以上であれば、前記自己位置探索制御処理を行わずに前記飛行高度を徐々に下げるよう移動制御する着陸制御処理を行うものとする。

かかる構成により、自己位置探索制御処理によって所定の閾値以上の位置に上昇しても、まだ水平自己位置を推定できない場合については、着陸制御処理によってその水平方向における位置で着陸するように移動制御することにより、水平自己位置が推定できないままの状態を不必要に継続しないようにして、より安全性を高めることが可能となる。

また、本発明の好ましい態様として、前記位置推定手段は、更に、前記飛行制御手段にて前記自己位置探索制御処理が行われた後に前記水平自己位置を推定できたとき、水平自己位置を推定できなかった高度における前記距離検出センサの出力値を用いて前記障害物マップを更新するマップ更新処理を行うものとする。

かかる構成により、水平自己位置を推定できなかった飛行空間における位置の周辺に、再び自律飛行した場合であっても、更新された障害物マップを用いて水平自己位置を推定することができると共に、更新された障害物マップにより新たな障害物の存在を認識することができるため当該障害物への衝突の危険性を低減することができる。

上記のように、本発明の自律飛行ロボットは、自己位置（水平自己位置）を推定できなかった場合であっても、自己位置（水平自己位置）を推定できるよう飛行することにより、安定的に移動目標位置へ自律飛行できる。

自律飛行ロボットの概観図自律飛行ロボットの機能ブロック図飛行制御処理の概要を説明する概略図２Ｄポイント情報と距離検出センサの出力値とを重畳表示した説明図飛行制御処理のフローチャート移動経路の生成におけるグラフ構造の説明図ローカル目標算出の説明図

以下、監視対象の飛行空間において所定の移動目標位置に向かって自律的に飛行しながら、飛行中における飛行空間の監視画像を取得する自律飛行ロボットについての実施形態について添付した図面を参照しながら詳細に説明する。

図１に本実施形態で利用する自律飛行ロボット１の概観図を表す。また、図２に本実施形態で利用する自律飛行ロボット１の機能ブロック図を表す。図１に表すように、本実施形態で利用する自律飛行ロボット１は、符号２ａ〜２ｄに示す４枚のロータ２（プロペラ）が一平面上に存在し、各ロータ２が図示しないバッテリ（二次電池）により駆動するモータ６によって回転することによって飛行するクアッドロータ型の小型無人ヘリコプタである。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、本実施形態で利用する自律飛行ロボット１のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ２を用いることで反トルクの相殺を行っている。そして、例えば、機体をヨー方向に回転させたいときは、符号ｆａ〜ｆｄの矢印で示すように前後のロータ２ａ、２ｃと左右ロータ２ｄ、２ｂの回転数に差を与える。このように、各ロータ２の回転数を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。

撮像部３は、例えばレンズなどの光学系および所定画素（例えば６４０×４８０画素）のＣＣＤやＣＭＯＳなどの２次元アレイ素子を有する二次元イメージセンサで構成され、飛行空間の撮像画像を所定の時間間隔で取得するいわゆるカラーカメラである。本実施形態では、撮像部３は、その光軸が自律飛行ロボット１の正面方向を撮像するよう筐体部分に設置され、かつ、水平面（ＸＹ平面）から予め定めた俯角θにより斜め下方の空間を撮像するよう設置されている。取得した撮像画像は後述する制御部７に出力され、制御部７により記憶部８に記憶されたり、後述する通信部９を介して図示しない外部装置に送信されたりする。
通信部９は外部装置との間で、例えば無線ＬＡＮや携帯電話回線等により無線通信するための通信モジュールである。本実施形態では、撮像部３によって取得した撮像画像を図示しない警備センタに設置されたＰＣに送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。

距離検出センサ４は、自律飛行ロボット１の周囲に存在する障害物と距離検出センサ４との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施形態では、距離検出センサ４としてレーザスキャナを備えている。レーザスキャナは、一定の角度サンプル間隔の角度毎に二次元スキャンすることによって、地面（又は床面）から一定の高さの水平面における周囲に存在する物体（障害物）との距離情報を極座標値として取得することができるものである。ここで、レーザスキャナにおける二次元スキャンとは、予め設定された検知エリアを走査するように、放射状にレーザ光である探査信号を送信し、検知エリア内の物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、その探査信号を送信した方向と算出した距離を求めることをいう。本実施形態では、角度サンプル間隔を０．２５°、検出角度範囲を３６０°、センサ検出範囲を３０ｍとして自律飛行ロボットの周囲の障害物との距離を測定可能なレーザセンサを用いている。なお、距離検出センサ４は、レーザスキャナから照射されるレーザの一部をミラー５で地面方向に反射させて計測された距離情報を利用して飛行高度を推測することにも利用している。

記憶部８は、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）等の情報記憶装置である。記憶部８は、各種プログラムや各種データを記憶し、制御部との間でこれらの情報を入出力する。各種データには、本発明の「障害物マップ」に対応する２Ｄポイント情報８１及びボクセル情報８２、移動目標位置８３の他、制御部７の各処理に用いられる設定値、閾値等の各種パラメータ８４や、各センサ等からの出力値、撮像画像等が含まれる。

２Ｄポイント情報８１は、後述する位置推定手段７１にて自律飛行ロボットの現在の飛行位置（以下、「自己位置」という）を推定するために利用する情報であり、グローバル座標と呼ばれる水平面における二次元の絶対座標上に表され、飛行空間における建造物等の障害物の外形を表す点集合の座標情報である。本実施形態では、飛行高度毎に設定された複数の点集合を２Ｄポイント情報として予め記憶部８に記憶していることとし、自律飛行ロボットの飛行高度によって対応する飛行高度の点集合を記憶部８から読み出して利用するものとする。

ボクセル情報８２は、飛行空間をボクセル空間として複数のボクセルに分割して飛行空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部８に記憶される情報である。本実施形態では、飛行空間を所定の大きさ（例えば１５ｃｍ×１５ｃｍ）に分割し、建造物等の障害物に位置するボクセルを「占有ボクセル」と定義して、自律飛行ロボット１が移動できない空間とした。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」として定義した。そして、各ボクセルには、後述する移動経路生成手段にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値を持たせた。占有ボクセルのボクセルコスト値は最大値をとり、距離が大きくなるほどボクセルコスト値が小さくなるように、（ボクセルコスト値）＝ｅｘｐ｛−λ・（占有ボクセルからの距離）｝の計算式からボクセルコスト値を算出した。ここでλは実験によって求めたパラメータである。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とした。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を０と再設定した。なお、自律飛行ロボット１が飛行空間における予め定めた移動可能空間の外に出ないようにするため、移動可能空間とその外部との境界となるボクセルを占有ボクセルと設定した。

移動目標位置８３は、自律飛行ロボット１が移動する目標となる飛行空間における位置情報であり、所定の位置（点）を示す座標情報として、予め管理者等によって設定され記憶部８に記憶される情報である。なお、本実施形態では、移動目標位置８３として一つの座標情報を記憶していることを想定しているが、これに限らず、複数の座標情報を記憶して時刻や所定条件（例えば、図示しないセンサの感知状況）に応じて制御部７にて利用する移動目標位置８３を切り替えるように利用してもよい。

制御部７は、ＣＰＵ等を備えたコンピュータで構成され、自己位置推定処理、速度推定処理、飛行制御処理を行う一連の処理として、位置推定手段７１、速度推定手段７２、飛行制御手段７３を含んでいる。

位置推定手段７１は、距離検出センサ４及び撮像部３の出力に基づいて、飛行空間における自律飛行ロボット１の現在の飛行位置（自己位置）を推定する自己位置推定処理を行う。自己位置推定処理では、距離検出センサ４の出力と記憶部８に記憶された２Ｄポイント情報８１とを用いて、自己位置として、飛行空間における水平方向（ＸＹ平面方向）の自己位置ｘ，ｙ（以下、「水平自己位置」という）と、高さ方向（Ｚ軸方向）の自己位置である飛行高度ｚと、Ｚ軸に対する回転角であるヨー角ψとを推定する処理を行う。

自己位置推定処理では、まず、レーザスキャナから照射されるレーザ光の一部をミラー５で地面方向に反射させて計測された距離情報を利用して、自己位置の飛行高度ｚを算出する。レーザスキャナによって計測された距離情報は、地面には建造物以外の物体（例えば荷物や乗り物など）が存在するため、必ずしも地面までの距離として正確に計測されるとは限らない。したがって、本実施形態では、これらの建造物以外の障害物の影響を受け難いようレーザスキャナによって計測された距離情報に対して拡張カルマンフィルタを適用することにより飛行高度を推定する。

次に、自己位置推定処理では、自己位置のｘ，ｙ，ヨー角ψを推定する。自己位置のｘ，ｙ，ヨー角ψを推定するにあたり、まず、記憶部に記憶された２Ｄポイント情報８１から飛行高度ｚに対応した飛行空間の二次元地図としての点集合を読み出す。そして、求めた点集合とレーザスキャナの出力とを用いて、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）アルゴリズムを利用した既知のスキャンマッチングにより自己位置のｘ，ｙ，ヨー角ψを推定する。ＩＣＰアルゴリズムは、２つの点集合Ａ，Ｂにおいてユークリッド距離が最小となる組合せを求めることによりマッチング処理するものである。すなわち、２Ｄポイント情報８１の点集合と、自律飛行ロボット１に搭載されているレーザスキャナから取得した出力値（スキャンデータ）である入力点集合とをマッチングさせ位置誤差を修正することによって、グローバル座標における自己位置のｘ，ｙ，ヨー角ψを推定することができる。

速度推定手段７２は、後述する飛行制御手段７３における経路追従制御で利用するため、自律飛行ロボット１の現在の飛行速度（ｖ_x，ｖ_y，ｖ_z，ｖ_yaw）を推定する処理を行う。本実施形態では、位置推定手段７１にて推定した自己位置（ｘ，ｙ，ｚ，ｙａｗ）の時間変化から飛行速度を求めた。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定した。

飛行制御手段７３は、位置推定手段７１にて推定した自己位置（水平自己位置、飛行高度）と速度推定手段７２で推定した飛行速度とを用いて、各時刻での飛行制御値である速度指令値を求め、当該速度指令値に基づいてモータ６を制御し、ロータ２の回転数を制御することにより、自律飛行ロボット１が記憶部８に記憶した移動目標位置８３へ向かって飛行させる飛行制御処理を行う。図３は、本発明の要点を示した概略図であり、同図を用いて飛行制御処理の概要について概略説明する。図３において、符号１ａ、１ｂ、１ｃはそれぞれ時刻ｔ、ｔ＋１、ｔ＋２における自律飛行ロボット１の位置に対応しており、符号Ａ１、Ａ２は飛行空間に存在する障害物である建造物に対応している。同図では、時刻ｔ及び時刻ｔ＋２においては自律飛行ロボット１が水平自己位置を推定できており、この場合、飛行制御処理によって、建造物Ａ１、Ａ２に衝突しないよう符号Ｇで表す移動目標位置８３に向かって移動制御されることを表している。その一方で、時刻ｔ＋１においては、本来、飛行空間内に存在しなかったはずのバスＣが存在していることに起因して、自律飛行ロボット１ｂが水平自己位置を推定できず、この場合、飛行制御処理によって飛行高度を上昇するよう移動制御されることを表している。

図４（ａ）（ｂ）は、それぞれ時刻ｔ＋１、ｔ＋２の位置における２Ｄポイント情報８１と距離検出センサ４の出力値によって算出された障害物までの距離情報とを重畳表示した図である。図４において、２Ｄポイント情報８１の点集合の座標値を符号Ｐで示す“○印”で表し、距離検出センサ４のレーザ光を符号Ｒで示す点線で表し、距離検出センサ４の出力値によって算出された障害物までの距離情報（座標値）を符号Ｐ’で示す“×印”で表している。図４（ａ）に表すように、時刻ｔ＋１では、レーザ光Ｒが一点鎖線で示す障害物のバスＣによって遮られることから、自律飛行ロボット１ｂの飛行高度における２Ｄポイント情報Ｐと距離検出センサ４の出力値から算出された障害物までの距離情報Ｐ’とが適切にマッチングできないことから水平自己位置を推定することができない。しかし、飛行制御処理により、自律飛行ロボット１が、図３の符号１ｂの位置から符号１ｃの位置へと飛行高度を上昇するよう移動制御されることによって、バスＣの高さよりも高い位置に移動することができ、当該バスＣによってレーザ光Ｒが遮られることがなくなる。したがって、図４（ｂ）に表すように、時刻ｔ＋２では、自律飛行ロボット１ｃの飛行高度における２Ｄポイント情報Ｐと距離検出センサ４の出力値から算出された障害物までの距離情報Ｐ’とが適切にマッチングでき、水平自己位置を推定することができるようになる。図５は、飛行制御処理を示すフローチャートであり、以下、同図を用いて飛行制御処理の詳細について説明する。

飛行制御処理では、まず、記憶部８に各種パラメータ８４として記憶された着陸フラグがＯＮとなっているか否かを判定する（ＳＴ１０）。着陸フラグは、後述する着陸制御処理が実施されたときにＳＴ１８にてＯＮとされるパラメータであり、デフォルト値はＯＦＦとなっている。ＳＴ１０にて着陸フラグがＯＮとなっていない（ＯＦＦ）と判定されたとき（ＳＴ１０−Ｎｏ）、位置推定手段７１における自己位置推定処理にて水平自己位置が推定できているか否かを判定する（ＳＴ１１）。前述したように本実施形態における自己位置推定処理では、距離検出センサ４（レーザスキャナ）の出力である入力点集合と２Ｄポイント情報８１の点集合とのマッチングによって水平自己位置を推定しているが、このマッチングの際に、２つの点集合間のユークリッド距離に応じたスコアを求め（点集合間の距離が大きくなるほど低スコアとなるように求める）、当該スコアが予め定めた閾値よりも小さいとき水平自己位置が推定できないとみなす。例えば、図４（ａ）に表すように、自律飛行ロボット１ｂの飛行高度における２Ｄポイント情報Ｐの点集合と、距離検出センサ４の出力値から算出された障害物までの距離情報Ｐ’の入力点集合の間でユークリッド距離が大きい場合、低スコアとなって水平自己位置が推定できないとみなされる。なお、本実施形態では、スコアによって水平自己位置が推定できているか否かを判定しているが、これに限らない。例えば、２Ｄポイント情報８１の点集合と大きく位置が異なる入力点集合の点を「外れ値」としてマッチング処理から除外しているが、この外れ値の数が予め定めた閾値以上であるとき水平自己位置が推定できないとみなす方法を用いてもよい。

ＳＴ１１にて水平自己位置が推定できていると判定されたとき（ＳＴ１１−Ｙｅｓ）、飛行制御手段７３は通常時（水平自己位置が推定できている時）における飛行制御処理として、通常飛行制御処理を行う（ＳＴ１２〜ＳＴ１４）。通常飛行制御処理では、まず、記憶部８に記憶されたボクセル情報８２及び移動目標位置８３と、位置推定手段７１にて算出した自己位置とを用いて自律飛行ロボットの移動経路を算出する移動経路生成処理を行う（ＳＴ１２）。具体的には、移動経路生成処理では、まず、記憶部からボクセル情報８２を読出し、分割した移動可能な空間である自由ボクセル及び近接ボクセルの各ボクセルの中心をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしてグラフ構造を生成する。図６はグラフ構造を説明する図であり、飛行空間における移動可能なボクセル（自由ボクセル又は近接ボクセル）の一部（２７個）を切り欠いたものである。図６において符号Ｂで表す個々の立方体は自由ボクセル又は近接ボクセルを表すものである。また、これらのボクセルＢの中心にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。このようにボクセル情報８２に基づいて全ての移動可能なボクセルについてグラフ構造を生成した後、自律飛行ロボット１が存在しているボクセルのノードから移動目標位置８３が存在しているボクセルのノードまでの移動経路を生成する。移動経路の生成方法については、さまざまな経路生成方法が適用可能であるが、本実施形態ではＡ＊（エースター）経路探索法を用いて移動経路を探索する。この際、ボクセル情報８２として記憶されたボクセルコストをＡ＊経路探索法における移動コストとして利用する。飛行制御手段７３で生成された生成された移動経路のデータは、経由点（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。

次に、通常飛行制御処理として、現在時刻での自律飛行ロボットが目標とすべき直近の位置（以下、「ローカル目標」と呼ぶ）を算出する処理を行う（ＳＴ１３）。図７はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、飛行制御手段７３は、ＳＴ１３にて生成された移動経路を記憶部８から読出し、自律飛行ロボット１が現在時刻で目指している経由点Ｗｐ１と前回通過済みである経由点Ｗｐ０との２点間を繋げた直線Ｗを求める。そして、飛行制御手段７３は、求めた直線Ｗと自律飛行ロボット１の自己位置を中心とした球Ｓとの交点Ｌｐ’、Ｌｐを算出し、目指している経由点Ｗｐ１に近い交点Ｌｐをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律飛行ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置８３に向かって移動していき、自律飛行ロボット１は移動経路に沿って飛行していくことになる。

次に、通常飛行制御処理として、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角ψの各方向毎に速度指令値ｕ_x，ｕ_y，ｕ_z，ｕ_ψを算出する処理を行う（ＳＴ１４）。この際、現在の自己位置とローカル目標の位置との差異が小さくなるような速度指令値を求める。具体的には、ＸＹＺ軸方向の速度指令値ｕ＝（ｕ_x，ｕ_y，ｕ_z）は、位置推定手段で求めた自己位置ｒ＝（ｘ，ｙ，ｚ）と速度推定手段で推定した速度ｖ＝（ｖ_x，ｖ_y，ｖ_z）とを利用し、ＰＩＤ制御により求める。ＸＹＺ軸方向の各速度指令値をｕ＝（ｕ_x，ｕ_y，ｕ_z）、ローカル目標をｒ’＝（ｘ，ｙ，ｚ）としたとき、速度指令値は、ｕ＝Ｋ_p（ｒ’−ｒ）＋Ｋ_d・ｖ＋Ｋ_i・ｅの式で算出される。ここで、Ｋ_p、Ｋ_d、Ｋ_iはそれぞれＰＩＤ制御のゲインのことであり、ｅ＝（ｅ_x，ｅ_y，ｅ_z）は誤差の積分値である。一方、ヨー角方向の速度指令値ｕ_ψは、ψ'を目標角度、ψを位置推定手段７１にて推定した自律飛行ロボット１の姿勢（角度）、ｖ_yawを速度推定手段７２で推定した角速度とすると、ｕ_ψ＝Ｋ_p（ψ’−ψ）＋Ｋ_d・ｖ_ψの式のようなＰＤ制御により求める。なお、本実施形態では、目標角度ψ'を移動目標位置８３（Ｇ）の方向を向く角度とした。

続いて、ＳＴ１１にて水平自己位置が推定できていないと判定されたとき（ＳＴ１１−Ｎｏ）、現在の飛行高度が予め記憶部８に記憶された閾値（本実施形態では８ｍとする）以下であるか否かを判定する（ＳＴ１５）。ＳＴ１５にて現在の飛行高度が８ｍ以下であると判定されたとき（ＳＴ１５−Ｙｅｓ）、飛行制御手段７３は水平自己位置を推定するための飛行制御処理として、自己位置探索制御処理を行う（ＳＴ１６）。自己位置探索制御処理では、飛行制御手段７３は、自律飛行ロボット１の飛行高度を徐々に上げるような速度指令値ｕ＝（ｕ_x，ｕ_y，ｕ_z）を求め、当該速度指令値ｕによりロータ２の回転数が制御される。本実施形態では、自律飛行ロボット１が水平方向へ移動しないように、Ｘ、Ｙ軸方向の速度指令値ｕ_x、ｕ_yはそれぞれ０とし、Ｚ軸方向の速度指令値ｕzを実験によって求めた所定値としている。

続いて、ＳＴ１５にて現在の飛行高度が８ｍ以下ではない判定されたとき（ＳＴ１５−Ｎｏ）、飛行制御手段７３は着陸制御処理を行う（ＳＴ１７）。着陸制御処理では、飛行制御手段７３は、自律飛行ロボット１の飛行高度を徐々に下げるような速度指令値ｕ＝（ｕ_x，ｕ_y，ｕ_z）を求め、当該速度指令値ｕによりロータ２の回転数が制御される。なお、本実施形態では、自律飛行ロボット１が水平方向へ移動しないように、Ｘ、Ｙ軸方向の速度指令値ｕ_x、ｕ_yはそれぞれ０とし、Ｚ軸方向の速度指令値ｕ_zを実験によって求めた所定値としている。ＳＴ１７の着陸制御処理が行われると、飛行制御手段７３は、記憶部８の着陸フラグをＯＮに変更する処理を行う（ＳＴ１８）。このように、着陸フラグをＯＮに変更することにより、ＳＴ１０の判定にて、次回からの飛行制御処理において速度指令値が変更されること無く、飛行高度を下げ続けることができる。なお、本実施形態では、飛行制御処理は自律飛行ロボット１が地上に着陸するまで処理されることとし、また、自律飛行ロボット１が着陸した場合（飛行高度が０と推定された場合）、ロータ２の回転を停止するよう制御されることとする。

このように、制御部７は、上述した位置推定手段７１、速度推定手段７２、飛行制御手段７３における各処理を所定時間おきに逐次繰り返す。これにより、本実施形態の自律飛行ロボット１は、水平自己位置が推定できるときは、通常飛行制御によって移動制御することにより、移動目標位置８３に向かって自律的に移動することができる。また、水平自己位置が推定できないときは、自己位置探索制御によって飛行高度を上げるよう移動制御することにより、障害物が存在し易い低高度の位置から、比較的障害物が存在し難い高高度の位置へと移動することができる。この際、水平方向へは移動せずに、水平自己位置から真上に上昇することにより、周囲に存在する障害物や人物等への衝突の危険性を回避することができる。このように、自律飛行ロボット１は、水平自己位置を推定できなくなった場合であっても、安全性を保ちつつ水平自己位置を推定し易い位置へと移動することが可能となる。

また、自己位置探索制御処理によって、所定の閾値（例えば８ｍ）以上の位置に上昇しても、まだ水平自己位置を推定できない場合については、着陸制御処理によってその水平方向における位置で着陸するように移動制御することにより、水平自己位置が推定できないままの状態を不必要に継続しないようにして、より安全性を高めることが可能となる。

ところで、本発明は、上記実施形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施形態で実施されてもよいものである。また、実施形態に記載した効果は、これに限定されるものではない。

上記実施形態では、自己位置探索制御処理によって自己位置が推定できたとき、通常飛行制御処理に移行することによって、再び移動目標位置８３に向かって自律移動している。しかし、これだけに限らず、自己位置探索制御処理によって自己位置が推定できたとき、位置推定手段７１は、水平自己位置を推定できなかった飛行高度における距離検出センサの出力値を用いて２Ｄポイント情報とボクセル情報８２とを更新するマップ更新処理を行ってもよい。すなわち、水平自己位置を推定できなかったときの飛行高度と距離検出センサの出力値（入力点集合の座標値）とを対応付けて一時的に記憶部８に記憶しておき、自己位置探索制御処理によって真上に上昇して水平自己位置が推定できたとき、一時的に記憶した距離検出センサの出力値を真として、当該出力値を用いて記憶部８に記憶した２Ｄポイント情報８１における対応する飛行高度（水平自己位置を推定できなかった飛行高度）の値を変更する。同じように、記憶部８に記憶されたボクセル情報８２についても、水平自己位置を推定できなかった飛行高度のボクセルであって、占有ボクセルの位置とは水平位置が大きく異なる位置にある入力点集合を特定し、当該入力点集合と干渉するボクセルを占有ボクセルとして更新する。そして、当該占有ボクセルの近くに存在するボクセルを近接ボクセルとして更新する。これにより、再びこの周辺の位置に自律飛行した場合であっても、水平自己位置を推定することができると共に、更新した位置に存在する障害物への衝突の危険性を低減することができる。

上記実施形態では、距離検出センサ４としてレーザスキャナを用いているが、これに限らず、例えば、赤外線距離センサ、ミリ波レーダ（マイクロ波レーダ）、超音波距離センサなどを用いてもよい。

上記実施形態では、飛行制御手段７３にて速度指令値を算出するために、速度推定手段７２にて自己速度を算出している。しかし、これに限らず、速度推定手段７２を省略して自己速度を算出せずに速度指令値を算出してもよい。また、上記実施形態では、速度推定手段７２にて自己位置に基づいて自己速度を算出している。しかし、これに限らず、ＩＭＵ（ＩｎｅｒｔｉａｌＭｅａｓｕｒｅｍｅｎｔＵｎｉｔ：慣性計測ユニット）などのセンサを利用したり、自律飛行ロボット１の真下方向を撮像するカメラを設置し、カメラからの撮像画像に基づいて既知のオプティカルフロー法を用いて自己速度を算出してもよい。

上記実施形態では、制御部７において位置推定処理（自己位置推定処理、移動物体位置推定処理）、速度推定処理、経路探索処理、経路追従制御処理の一連の処理を行っている。しかし、これに限らず、図示しない制御用のＰＣを用意し、当該ＰＣにこれらの一連の処理を実施させてもよい。すなわち、自律飛行ロボット１は、距離検出センサ４のレーザスキャナの出力及び撮像部３で取得した撮像画像を通信部９を介して無線通信により当該ＰＣに送信する。そして、ＰＣは、位置推定処理、速度推定処理、飛行制御処理を実施し、飛行制御処理にて求めた速度指令値を無線通信により自律飛行ロボット１に送信する。そして、自律飛行ロボット１は、通信部９を介して受信した速度指令値に基づいてモータ６の回転数を制御することにより、目的の位置に飛行する。このように、上記の一連の処理を外部ＰＣを用いて分担することにより、自律飛行ロボット１のＣＰＵ処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。

１・・・自律飛行ロボット
２・・・ロータ
３・・・撮像部
４・・・距離検出センサ
５・・・ミラー
６・・・モータ
７・・・制御部
８・・・記憶部
９・・・通信部
８１・・・２Ｄポイント情報
８２・・・ボクセル情報
８３、Ｇ・・・移動目標位置
８４・・・各種パラメータ
７１・・・位置推定手段
７２・・・速度推定手段
７３・・・飛行制御手段
Ａ１、Ａ２・・・建造物
Ｃ・・・バス
Ｂ・・・ボクセル
Ｒ・・・レーザ光
Ｐ・・・２Ｄポイント情報の座標値
Ｐ’・・・距離検出センサ出力値の座標値
Ｌｐ・・・ローカル目標

Claims

飛行空間の障害物マップを記憶した記憶部と、
自律飛行ロボットの周囲に存在する障害物までの距離を測定する距離検出センサと、
前記距離検出センサの出力及び前記障害物マップを用いて自律飛行ロボットの前記飛行空間における水平方向の自己位置である水平自己位置と高さ方向の自己位置である飛行高度とを推定する処理を行う位置推定手段と、
前記位置推定手段にて前記水平自己位置を推定できるとき、前記自己位置から所定の移動目標位置に向かうよう移動制御する通常飛行制御処理と、前記位置推定手段にて前記水平自己位置を推定できないとき、前記飛行高度が高くなるよう移動制御する自己位置探索制御処理とを行う飛行制御手段と、を有し、前記各処理を逐次繰り返すことによって自律的に飛行することを特徴とする自律飛行ロボット。
前記自己位置探索制御処理は、更に、水平方向へ移動しないよう移動制御する請求項１に記載の自律飛行ロボット。
前記飛行制御手段は、更に、前記位置推定手段にて前記水平自己位置を推定できないときであって、かつ、前記飛行高度が所定の閾値以上であれば、前記自己位置探索制御処理を行わずに前記飛行高度を徐々に下げるよう移動制御する着陸制御処理を行う請求項２に記載の自律飛行ロボット。
前記位置推定手段は、更に、前記飛行制御手段にて前記自己位置探索制御処理が行われた後に前記水平自己位置を推定できたとき、水平自己位置を推定できなかった高度における前記距離検出センサの出力値を用いて前記障害物マップを更新するマップ更新処理を行う請求項２又は請求項３に記載の自律飛行ロボット