(57)【要約】【課題】障害物を回避する際に移動距離をより短距離にする。【解決手段】実施形態において、切替制御手段は、移動体と進行方向にある障害物との距離が回避開始距離となると、基本移動制御から回避制御に制御を切替える。距離算出手段は、前方距離と前方安全距離とから、移動体が移動方向に対して進行可能な移動可能距離を算出する。回避方向算出手段は、側方安全距離と移動可能距離とから、回避方向を算出する。回避動作制御手段は、距離算出手段が算出した移動可能距離と、回避方向算出手段が算出した回避方向とから、回避経路上の目標地点を決定する。【選択図】図１
2017220057.tif 000002
【特許請求の範囲】
【請求項１】
移動体の進行方向にある障害物を検出し、当該障害物と前記移動体との距離を測定する前方測定手段と、
前記距離が障害物に対する回避動作を開始する回避開始距離となると、目的地の位置情報に基づいて決定された進行方向に移動体を移動させる基本移動制御から、障害物を回避する回避制御に制御を切替える切替制御手段と、
前記距離と、前記移動体と前記進行方向にある障害物との間で安全性を保つために予め設定された前方安全距離とから、前記移動体が前記進行方向に対して進行可能な移動可能距離を算出する距離算出手段と、
前記進行方向と異なる方向である側方において、前記移動体と前記側方にある障害物との間で安全性を保つために予め設定された側方安全距離と、前記距離算出手段が算出した前記移動可能距離とから、前記回避動作において前記移動体が移動する方向である回避方向を算出する回避方向算出手段と、
前記距離算出手段が算出した前記移動可能距離と、前記回避方向算出手段が算出した前記回避方向とから、回避経路上の目標地点を決定する回避動作制御手段と、
を備えた障害物回避装置。
【請求項２】
前記回避動作制御手段は、前記進行方向にある障害物との距離が前記前方安全距離を下回ると、前記進行方向以外の方向を前記回避方向として前記進行方向への移動を中止し、前記障害物が前記進行方向から消えた後も、前記移動体の側方において前記側方にある障害物との間に前記側方安全距離が確保されるまで、前記回避方向への回避動作を行う、請求項１に記載の障害物回避装置。
【請求項３】
前記距離算出手段は、段階的に設けられた複数の前記前方安全距離を順次用いて、前記移動可能距離を算出する、請求項１に記載の障害物回避装置。
【請求項４】
前記前方測定手段は、パルスレーザの入射光と反射光との時間差を測定することにより、前記移動体と前記障害物との距離を測定する、請求項１ないし３のいずれか１つに記載の障害物回避装置。
【請求項５】
前記パルスレーザの入射光を前記障害物上で走査するための走査ミラーと、
前記走査ミラーを走査する走査制御手段と、をさらに備えた請求項４に記載の障害物回避装置。
【請求項６】
前記前方測定手段は、超音波の入射波と反射波との時間差を測定することにより、前記移動体と前記障害物との距離を測定する、請求項１ないし３のいずれか１つに記載の障害物回避装置。
【請求項７】
移動体の進行方向にある障害物を検出し、当該障害物と前記移動体との距離を測定する前方測定手段と、
前記距離が障害物に対する回避動作を開始する回避開始距離となると、目的地の位置情報に基づいて決定された進行方向に移動体を移動させる基本移動制御から、障害物を回避する回避制御に制御を切替える切替制御手段と、
前記距離と、前記移動体と前記進行方向にある障害物との間で安全性を保つために予め設定された前方安全距離とから、前記移動体が前記進行方向に対して進行可能な移動可能距離を算出する距離算出手段と、
前記進行方向と異なる方向である側方において、前記移動体と前記側方にある障害物との間で安全性を保つために予め設定された側方安全距離と、前記距離算出手段が算出した前記移動可能距離とから、前記回避動作において前記移動体が移動する方向である回避方向を算出する回避方向算出手段と、
前記距離算出手段が算出した前記移動可能距離と、前記回避方向算出手段が算出した前記回避方向とから、回避経路上の目標地点を決定する回避動作制御手段と、
を備えた障害物回避システム。
【請求項８】
障害物回避装置を制御するコンピュータを、
移動体の進行方向にある障害物を検出し、当該障害物と前記移動体との距離を測定する前方測定手段と、
前記距離が障害物に対する回避動作を開始する回避開始距離となると、目的地の位置情報に基づいて決定された進行方向に移動体を移動させる基本移動制御から、障害物を回避する回避制御に制御を切替える切替制御手段と、
前記距離と、前記移動体と前記進行方向にある障害物との間で安全性を保つために予め設定された前方安全距離とから、前記移動体が前記進行方向に対して進行可能な移動可能距離を算出する距離算出手段と、
前記進行方向と異なる方向である側方において、前記移動体と前記側方にある障害物との間で安全性を保つために予め設定された側方安全距離と、前記距離算出手段が算出した前記移動可能距離とから、前記回避動作において前記移動体が移動する方向である回避方向を算出する回避方向算出手段と、
前記距離算出手段が算出した前記移動可能距離と、前記回避方向算出手段が算出した前記回避方向とから、回避経路上の目標地点を決定する回避動作制御手段として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、障害物回避装置、障害物回避システムおよびプログラムに関する。
【背景技術】
【０００２】
従来、指定された目的地まで障害物を避けて対象物を移動させる技術が開示されている。
【０００３】
例えば、特許文献１には、前後上下左右における障害物までの距離を超音波で検知し、検知した距離と予め設定された危険距離との比較により、障害物を回避する方向を決定する技術が開示されている。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上述した従来技術においては、目的地までの移動距離をより短距離にするという視点に立った技術に関して改良の余地があった。
【０００５】
本発明は、上記に鑑みてなされたものであって、障害物を回避する際に移動距離をより短距離にすることができる障害物回避装置、障害物回避システムおよびプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上述した課題を解決し、目的を達成するために、本発明は、移動体の進行方向にある障害物を検出し、当該障害物と前記移動体との距離を測定する前方測定手段と、前記距離が障害物に対する回避動作を開始する回避開始距離となると、目的地の位置情報に基づいて決定された進行方向に移動体を移動させる基本移動制御から、障害物を回避する回避制御に制御を切替える切替制御手段と、前記距離と、前記移動体と前記進行方向にある障害物との間で安全性を保つために予め設定された前方安全距離とから、前記移動体が前記進行方向に対して進行可能な移動可能距離を算出する距離算出手段と、前記進行方向と異なる方向である側方において、前記移動体と前記側方にある障害物との間で安全性を保つために予め設定された側方安全距離と、前記距離算出手段が算出した前記移動可能距離とから、前記回避動作において前記移動体が移動する方向である回避方向を算出する回避方向算出手段と、前記距離算出手段が算出した前記移動可能距離と、前記回避方向算出手段が算出した前記回避方向とから、回避経路上の目標地点を決定する回避動作制御手段と、を備えることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、障害物を回避する際に移動距離をより短距離にすることができるという効果を奏する。
【図面の簡単な説明】
【０００８】
【図１】図１は、本実施形態の障害物回避装置を搭載した飛行移動体が、崖などの障害物を回避して目的地に向かう場合の動作例を示した模式図である。
【図２】図２は、本実施形態の障害物回避装置の機能構成を示した機能ブロック図である。
【図３】図３は、回避動作の制御例を示した説明図である。
【図４】図４は、障害物回避装置が実行する回避動作の一手順を示したフローチャートである。
【図５】図５は、障害物との距離が安全距離を下回っている場合における制御例を示した説明図である。
【図６】図６は、安全距離を複数設定した場合の制御例を示した説明図である。
【図７】図７は、側方安全距離を複数設定した場合の制御例を示した説明図である。
【図８】図８は、前方距離測定部において、パルスレーザを用いる場合の構成例を示した構成図である。
【図９】図９は、前方距離測定部において、超音波を用いる場合の構成例を示した構成図である。
【図１０】図１０は、前方距離測定部がレーザ光を走査する構成を有する場合の構成図である。
【図１１】図１１は、ピエゾ振動子の駆動電圧の一例を示した説明図である。
【図１２】図１２は、測定対象物上におけるレーザ光の走査例を示した説明図である。
【図１３】図１３は、ピエゾ振動子の駆動電圧の他の例を示した説明図である。
【図１４】図１４は、図１３に示す駆動電圧を用いた際のレーザ光の走査パターン例を示した模式図である。
【発明を実施するための形態】
【０００９】
以下に添付図面を参照して、障害物回避装置、障害物回避システムおよびプログラムの実施形態を詳細に説明する。以下では、本実施形態にかかる障害物回避装置を、飛行移動体（以下では単に移動体という）に搭載した例について説明する。移動体の例としては、例えば、自律飛行を行う無人航空機などがある。但し、以下に示す実施形態は一例であり、これにより本発明が限定されるものではない。
【００１０】
図１は、本実施形態の障害物回避装置１を搭載した移動体１０１が、崖などの障害物を回避して目的地に向かう場合の動作例を示した模式図である。尚、図１は、移動体１０１を上から見た図である。
【００１１】
概略的には、移動体１０１に搭載された障害物回避装置１は、障害物１０２までの距離Ｄを測定し、当該距離Ｄが回避開始距離Ｄａとなる地点Ｐ０に至ると、回避動作を開始する。即ち、障害物回避装置１は、従来用いられていたような安全距離Ｄｓよりも大きい値である回避開始距離Ｄａを判定基準に用いて、回避動作を障害物のより手前側で開始する。
【００１２】
回避動作において障害物回避装置１は、障害物１０２までの距離Ｄと、予め設定された安全距離Ｄｓを用いて、進行方向Ｑに移動できる距離である移動可能距離Ｄｍを算出する。そして、障害物回避装置１は、移動可能距離Ｄｍと、機体側方の障害物との間で保持し続ける距離である側方安全距離Ｄｈとから、回避動作における機体の進行方向である、回避角度θａを算出する。そして、障害物回避装置１は機体を回避角度θａに方向転換し、当初の進行方向Ｑに移動可能距離Ｄｍ分だけ対応する距離を移動することにより回避動作をとる。これにより移動体１０１は、進行方向の障害物１０２に対しては安全距離Ｄｓ分の距離を確保し、機体側方の障害物１０２に対しては側方安全距離Ｄｈ分の距離を確保しながら障害物１０２を回避し、回避動作後に再び進行方向Ｑに向かって飛行を続ける。
【００１３】
すなわち、Ｄ＝Ｄｓとなる地点Ｐ９において右に９０°方向転換をして障害物１０２を回避するのではなく、地点Ｐ９よりも手前の地点Ｐ０（Ｄ＝Ｄａ）で回避角度θａに回避動作をとる。これにより、障害物回避装置１は、回避動作中の方向転換をなだらかにして、回避動作に伴う移動距離をより短くする。
【００１４】
図２は、本実施形態の障害物回避装置１の機能構成を示した機能ブロック図である。以下、図１および図２を用いて障害物回避装置１の構成について説明する。図２に示すように、障害物回避装置１は、機体制御部７０１、進行方向制御部７０２、データ格納部７０３、前方距離測定部７０４、位置情報取得部７０５、高度情報取得部７０６、回避動作制御部７０７、無線通信制御部７０８、側方距離測定部７０９を主に備えている。
【００１５】
機体制御部７０１は、上昇、前進、旋回など、移動体１０１の機体の基本的動作を制御する。
【００１６】
進行方向制御部７０２は、出発地点と目的地（最終目標地点）の位置情報から、移動体１０１の移動経路（飛行ルート）と進行方向Ｑ（飛行方向）とを決定する。位置情報としては、緯度、経度、高度等を用いればよい。何らかの影響で移動体１０１が当初の飛行ルートから外れた場合、進行方向制御部７０２は、目的地への進行方向を再度決定する。以下では、当初決定された移動経路および当初決定された進行方向に乗って、回避動作を含まずに進行方向Ｑへ単純に移動するための制御を、基本移動制御という。
【００１７】
データ格納部７０３は、回避動作を開始する距離である回避開始距離Ｄａ、安全距離Ｄｓ、側方安全距離Ｄｈ、出発地点と目的地の位置情報などを格納する。また、データ格納部７０３は、障害物との距離Ｄを測定する時間間隔である測距時間間隔を格納する。ユーザは、従来一般的に用いられている入力インタフェースを用いて、データ格納部７０３にこれらデータを入力し設定することができる。また、データ格納部７０３は、移動体１０１が撮影した映像のデータや、移動体１０１が測定したデータなどを格納する。
【００１８】
前方距離測定部７０４（前方測定手段）は、進行方向Ｑにある障害物１０２を検出し、当該障害物１０２と移動体１０１との間の距離Ｄを測定する。進行方向Ｑにある障害物１０２とは、移動体１０１が進行方向Ｑにそのまま飛行（進行）を続けた場合に移動体１０１と衝突する可能性のある物体や地形のことである。障害物は例えば、建物等の構造物、崖、樹木、その他の地形全般、あるいは、他の飛行体などである。
【００１９】
尚、前方距離測定部７０４は、進行方向Ｑに対して所定の角度範囲を持つ領域について、障害物を検出し、障害物を検知した角度の情報とともに、当該障害物との距離を測定してもよい。尚、この場合に、従来一般的に用いられている入力インタフェース等を用いて、当該角度範囲を適宜変更できるようにしてもよい。
【００２０】
前方距離測定部７０４のより詳細な構成例については、図８ないし図１４とともに後述する。
【００２１】
位置情報取得部７０５は、移動体１０１の現在位置を示す緯度および経度の情報を取得し、位置情報としてデータ格納部７０３に格納する。この機能としては、例えば、従来一般的に用いられているＧＰＳ（Global  Positioning  System）機能などを利用すればよい。また、位置情報取得部７０５は、ジャイロセンサ等を用いて移動体１０１の機体の姿勢を示す姿勢情報を取得して、データ格納部７０３に格納する。
【００２２】
高度情報取得部７０６は、移動体１０１の地表面からの距離を測定し、測定値を高度情報としてデータ格納部７０３に格納する。
【００２３】
回避動作制御部７０７（切替制御手段）は、上述した基本移動制御と、障害物を回避するための回避制御とを切替える。即ち、回避動作制御部７０７は、前方距離測定部７０４が測定した距離Ｄが回避開始距離Ｄａ以下となると、回避動作にかかる制御（回避制御）を開始する。また、回避動作制御部７０７は、距離Ｄが回避開始距離Ｄａを上回ると回避動作を終了し、基本移動制御を再開する。尚、制御を切り替えるタイミングは上述の例に限定されず、距離Ｄが回避開始距離Ｄａを下回ると回避制御を開始し、距離Ｄが回避開始距離Ｄａ以上となると回避動作を終了するとしてもよい。
【００２４】
回避動作制御部７０７（回避制御手段）は、回避動作中の移動体１０１の目標地点（即ち、回避経路上の目標地点）を決定し、当該目標地点を示す情報を機体制御部７０１に出力することで、回避動作を制御する。
【００２５】
即ち、回避動作制御部７０７（距離算出手段、回避方向算出手段）は、回避動作における移動体１０１の回避方向Ｒに対応する回避角度θａと、移動可能距離Ｄｍとを算出する。
【００２６】
まず、回避動作制御部７０７は、障害物１０２までの距離Ｄと、データ格納部７０３に設定された安全距離Ｄｓとから、進行方向Ｑの移動可能距離Ｄｍを算出する。
Ｄｍ＝Ｄ−Ｄｓ                                                （式１）    
【００２７】
そして、回避動作制御部７０７は、式１で算出した移動可能距離Ｄｍと、データ格納部７０３に格納されている側方安全距離Ｄｈとから、回避動作中の進行方向Ｒを決定する回避角度θａを算出する。
ｔａｎ（θａ）＝Ｄｈ／Ｄｍ                                    （式２）    
【００２８】
そして、回避動作制御部７０７は、算出した回避角度θａおよび移動可能距離Ｄｍを機体制御部７０１に出力することで、回避動作を指示する。
【００２９】
無線通信制御部７０８は外部機器と無線通信を行う。例えば、無線通信制御部７０８は、外部機器から移動体１０１の機体制御を行うためのコマンド（機体制御コマンド）を受信する。また、無線通信制御部７０８は、位置情報取得部７０５が取得した位置情報や姿勢情報、高度情報取得部７０６が取得した高度情報を外部機器に送信する。また、無線通信制御部７０８は、その他の情報や、データ格納部７０３に格納されているその他のデータを外部機器に送信する。
【００３０】
側方距離測定部７０９は、移動体１０１の機体の側方に存在する外部物体（障害物）と、移動体１０１との距離を測定する。測距対象とする外部物体は特に限定されず、飛行体などの移動体であってもよいし、地表面、建物や電柱などの構造物、樹木、崖などの地形などの静止した物体であってもよい。
【００３１】
ここで側方とは、機体の上側に向かう方向（上方向）、または、機体の下側に向かう方向（下方向）、または、機体の左側に向かう方向（左方向）、または、機体の右側に向かう方向（右方向）の、少なくともいずれか１つの方向のことである。または、上方向、下方向、左方向、右方向のうち複数の方向を組み合わせた方向のことである。
【００３２】
なお、側方は進行方向と異なる方向であればよく、進行方向制御部７０２が決定した進行方向Ｑ、あるいは、上述した角度範囲で定められる進行方向を除くすべての方向を側方としてもよい。ユーザが、側方をいずれの方向とし、どのような範囲とするかを適宜設定できるとしてもよい。
【００３３】
尚、進行方向、側方は測定手段の形態に応じて適宜設定可能であり、本実施形態では各方向について何ら限定するものではない。例えば、レーザや超音波等、指向性を持って所定方向あるいは所定範囲について距離を測定するような測定手段であれば、測定機構の取り付け位置や測定波の出射方向によってその方向は決定される。一方、広角の撮像素子と画像認識技術等とを用いて例えば全方位的に距離を測定するような測定手段であれば、ユーザが適宜測定する方向や範囲を設定すればよい。
【００３４】
次に、回避動作の制御例について、図３を用いて説明する。
【００３５】
図３は、回避動作の制御例を示した説明図である。障害物回避装置１は、図１と同様に、地点Ｐ０から回避動作を開始する。前方距離測定部７０４は、回避動作中に一定の時間間隔、例えば１００ｍ秒等ごとに障害物までの距離Ｄｔ（ｔ＝１、２、３、…）を順次測定する。そして回避動作制御部７０７は、進行方向Ｑに移動可能な距離Ｄｍｔ（ｔ＝１、２、３、…）と、データ格納部７０３が記憶する側方安全距離Ｄｈとから、回避角度θａｔを順次算出する。
Ｄｍｔ＝Ｄｔ−Ｄｓ                                            （式３）    
ｔａｎ（θａｔ）＝Ｄｈ／Ｄｍｔ  （ｔ＝１、２、３、…）        （式４）    
【００３６】
これにより回避動作制御部７０７は、地点Ｐ０において進行方向Ｑに移動可能な距離Ｄｍ１と回避角度θａ１とを算出する。そして回避動作制御部７０７は、算出した値に基づいて回避動作の目標地点を地点Ｐ１に設定する。
【００３７】
そして、地点Ｐ１へ向かう回避経路上の地点Ｐ２において、前方距離測定部７０４は障害物１０２までの距離Ｄ２を測定する。そして、回避動作制御部７０７は、地点Ｐ２において進行方向Ｑに移動可能な距離Ｄｍ２と回避角度θａ２とを算出し、これら値に基づいて回避動作の目標地点を地点Ｐ３に設定する。
【００３８】
同様に、地点Ｐ３に向かう回避経路上の地点Ｐ４において、前方距離測定部７０４は障害物１０２との距離Ｄ３を測定する。そして、回避動作制御部７０７は、地点Ｐ４において進行方向Ｑに移動可能な距離Ｄｍ３と、回避角度θａ３とを算出し、これら値に基づいて回避動作の目標地点を地点Ｐ５に設定する。
【００３９】
障害物回避装置１は、前方距離測定部７０４が測定した障害物１０２までの距離Ｄが回避開始距離Ｄａ（図１参照）を上回るまで、障害物１０２までの距離Ｄの測定値に基づいて移動可能距離Ｄｍｔと回避角度θａｔの算出を繰り返し、回避動作の目標地点を更新し続ける。
【００４０】
このように、本実施形態の障害物回避装置１は、回避動作中に障害物１０２までの距離Ｄを測定し続けて、当該測定値を用いて回避動作の目標地点を更新し続ける。従って、機体の前方と側方との双方について安全距離を保ち続けることができる。また、崖などの障害物１０２を回避する際に、前方の障害物および側方の障害物と機体との安全距離を保ち続けつつも、障害物の形状に沿うように回避経路を更新することができる。従って、回避動作時に急激な方向転換を避けることができ、回避経路の最適化を図ることができる。
【００４１】
図４は、障害物回避装置１が実行する回避動作の一手順を示したフローチャートである。
【００４２】
障害物回避装置１は、飛行開始地点および最終目標地点の緯度、経度、対障害物回避動作を開始する距離である回避開始距離Ｄａ、安全距離Ｄｓ、側方安全距離Ｄｈの入力を受付けて、データ格納部７０３に設定する（ステップＳ１）。尚、各値の設定方法は特に限定されず、ユーザによる入力でもよいし、ＧＰＳ機能等による自動入力や記憶手段からの読み込みと連携させた入力でもよい。各値の設定後、機体制御部７０１は最終目標地点へ向けて飛行を開始し（ステップＳ２）、回避動作制御部７０７は基本動作制御を開始する（ステップＳ３）。
【００４３】
前方距離測定部７０４は、一定周期、例えば１００ｍｓ周期で、進行方向にある障害物までの距離Ｄを測定する（ステップＳ４）。
【００４４】
回避動作制御部７０７は、ステップＳ４で測定された距離Ｄと、回避開始距離Ｄａとを比較する（ステップＳ５）。距離Ｄが回避開始距離Ｄａより大きい場合（ステップＳ５：Ｎｏ）には、ステップＳ３に戻る。距離Ｄが回避開始距離Ｄａ以下であれば（ステップＳ５：Ｙｅｓ）、回避動作制御部７０７は基本動作制御から回避動作制御へと制御を切替える（ステップＳ６）。
【００４５】
回避動作制御部７０７は、距離Ｄと安全距離Ｄｓとから、進行方向への移動可能距離Ｄｍを算出する（ステップＳ７）。また、回避動作制御部７０７は、移動可能距離Ｄｍと、側方安全距離Ｄｈとから、回避角度θａを算出する（ステップＳ８）。回避動作制御部７０７は、ステップＳ７、Ｓ８で算出した移動可能距離Ｄｍおよび回避角度θａを機体制御部７０１に出力し、回避動作を指示する（ステップＳ９）。
【００４６】
回避動作制御部７０７は、移動体１０１がステップＳ１で設定された最終目標地点に到達したか判定し（ステップＳ１０）、最終目標地点に到達していなければ（ステップＳ１０：Ｎｏ）、ステップＳ３に戻ってステップＳ３〜Ｓ１０の手順を繰り返す。その間、障害物までの距離Ｄが回避開始距離Ｄａより大きくなれば（ステップＳ５：Ｎｏ）、回避動作制御部７０７は回避動作制御から基本動作制御に制御を切替える（ステップＳ３）。最終目標地点に到達すると（ステップＳ１０：Ｙｅｓ）、障害物回避装置１は基本動作制御および回避動作制御を終了する。
【００４７】
尚、機体制御部７０１による回避動作の実施には数１００ｍ秒から数秒程度の時間がかかる。そこで、回避動作制御部７０７は、ステップＳ６で回避動作を開始した後はステップＳ３に戻り、進行方向の障害物１０２が回避開始距離Ｄａ以内に検出されなくなるまで、ステップＳ３〜Ｓ９の手順を繰り返す。これにより、回避動作の目標地点の更新により迅速に取り掛かれるため、回避動作をより滑らかにし易くできる。
【００４８】
次に、回避動作におけるその他の制御例について説明する。
【００４９】
＜制御例１＞距離Ｄが安全距離Ｄｓを下回っている場合の制御例
図５は、障害物１０２との距離Ｄが安全距離Ｄｓを下回っている場合の制御例を示した説明図である。
【００５０】
例えば、地点Ｐ５１から飛行を開始する際において、進行方向Ｑにビルなどの障害物１０２がある場合、あるいは、飛行中に進行方向が変更された場合など、進行方向Ｑにある障害物１０２までの距離Ｄが安全距離Ｄｓを下回る状況が起こりうる。このような状況では、上述した式１でＤｍ＝Ｄ−Ｄｓ＜０となり移動可能距離Ｄｍが負の値となってしまう。
【００５１】
そこでこのような場合、即ちＤ＜Ｄｓの場合に回避動作制御部７０７は、回避方向Ｒ５１を回避角度θａが９０°となるような方向として方向転換をする。即ち、回避動作制御部７０７は、進行方向Ｑへの移動を中止（中断）し、進行方向Ｑ以外の方向に機体を回避させて回避動作を行う。尚、方向転換を行うタイミングは、Ｄ＜Ｄｓではなく、Ｄ≦Ｄｓである場合として判定してもよい。
【００５２】
そして、回避動作制御部７０７は、地点Ｐ５２において障害物１０２が進行方向Ｑから消えた後も、移動体１０１の側方において、障害物１０２との間に側方安全距離Ｄｈを確保できるまで、回避方向Ｒ５１への回避動作を続行する。その後、地点Ｐ５３において移動体１０１と障害物１０２との距離が側方安全距離Ｄｈ以上となると、障害物回避装置１は回避動作を終了し、進行方向Ｑへの飛行制御（基本移動制御）へと移行する。
【００５３】
このように制御することで、進行方向Ｑに障害物１０２がなくなり、回避方向Ｒ５１から９０°方向転換して元の進行方向Ｑに戻した場合に、側方安全距離Ｄｈを確保することができる。
【００５４】
＜制御例２＞安全距離Ｄｓを複数設定した場合の制御例
図６は、安全距離Ｄｓを複数設定した場合の制御例を示した説明図である。進行方向の安全距離Ｄｓは段階的に複数設定してもよい。図６の例では、第１の安全距離Ｄｓ１、第２の安全距離Ｄｓ２、第３の安全距離Ｄｓ３を、Ｄｓ１＞Ｄｓ２＞Ｄｓ３として段階的に設定した場合の回避動作の制御例について説明する。
【００５５】
回避動作制御部７０７は回避動作を開始すると、安全距離Ｄｓを大きい値から小さい値へと段階的に変更して用い、回避動作中の目標地点Ｐ６１〜Ｐ６３を順次決定する。まず回避動作制御部７０７は、進行方向Ｑにある障害物１０２までの距離Ｄが回避開始距離Ｄａとなると、測定した距離Ｄと第１の安全距離Ｄｓ１とから、第１の移動可能距離Ｄｍ１を算出する。
Ｄｍ１＝Ｄ−Ｄｓ１                                            （式５）    
そして、第１の移動可能距離Ｄｍ１と、側方安全距離Ｄｈとから、第１の回避角度θａ１を算出する。
ｔａｎ（θａ１）＝Ｄｈ／Ｄｍ１                                （式６）    
【００５６】
そして、回避動作制御部７０７は、第１の回避角度θａ１と、第１の移動可能距離Ｄｍ１とにより決定される地点Ｐ６１を第１の目標地点として、回避動作の第１段階を行う。
【００５７】
移動体１０１が回避方向Ｒ６１の向きに沿って地点Ｐ６１まで飛行すると、障害物１０２までの距離Ｄは第１の安全距離Ｄｓ１となる。
【００５８】
回避動作制御部７０７は、障害物までの距離Ｄ（即ち、安全距離Ｄｓ１）と、第２の安全距離Ｄｓ２とから、第２の移動可能距離Ｄｍ２、第２の回避角度θａ２を式５、式６と同様の計算により算出する。そして、回避動作制御部７０７は、第２の回避角度θａ２と、第２の移動可能距離Ｄｍ２とにより決定される地点Ｐ６２を第２の目標地点として、回避動作の第２段階へ移行する。
【００５９】
移動体１０１が回避方向Ｒ６２の向きに沿って地点Ｐ６２まで飛行すると、次は回避動作の第３段階へと移行する。即ち、回避動作制御部７０７は、式５、式６と同様の計算により、第３の移動可能距離Ｄｍ３、第３の回避角度θａ３を算出し、これら値により決定される地点Ｐ６３を第３の目標地点とする。そして、回避方向Ｒ６３の向きに沿って地点Ｐ６３に到達し、障害物１０２までの距離Ｄが回避開始距離Ｄａを上回る（即ち、Ｄ＞Ｄａ）と、回避動作制御部７０７は回避動作制御を終了し、基本移動制御に制御を切替える。そして、移動体１０１は、当初の進行方向Ｑに向かって飛行する。
【００６０】
尚、障害物１０２との距離Ｄが、複数の安全距離Ｄｓのうち最も短い距離（図６の例では安全距離Ｄｓ３）となった場合には、回避動作制御部７０７は図５で説明したように、回避角度θａ＝９０°などとして方向転換する回避動作を行えばよい。
【００６１】
尚、上述では、一の回避動作の中で、より大きい安全距離Ｄｓ１から段階的に小さい安全距離Ｄｓ２、Ｄｓ３へと切り替えて、次の目標地点を決定したが、複数の安全距離Ｄｓを用いる順序はこれに限定されない。回避方向Ｒの状況や、側方の物体との距離、あるいは、撮像手段により得た周辺状況の画像等から、複数の安全距離Ｄｓを選択して、目標位置の算定に用いてもよい。
【００６２】
以上のように、安全距離Ｄｓを複数設定して回避動作中に随時用いることで、回避動作に伴う移動距離の増加を抑えることができる。また、回避動作における方向転換も滑らかになるので、エネルギー消費量を抑えやすいという利点がある。
【００６３】
＜制御例３＞側方安全距離Ｄｈを複数設定した場合の制御例
側方安全距離Ｄｈは複数設定してもよい。例えば、移動体１０１が左右両側方について障害物の検出を行う場合、左右両側方に対する側方安全距離Ｄｈをそれぞれ設けてもよい。あるいは、移動体１０１が機体の上下両側方について障害物の検出を行う場合には、上下両側方に対して側方安全距離Ｄｈをそれぞれ設けてもよい。側方安全距離Ｄｈはこのように互いに反対方向となる向きの双方に対してそれぞれ設定してもよいし、３つ以上の方向について障害物の検出を行う場合には、各方向についてそれぞれ側方安全距離Ｄｈを設定してもよい。以下、図７とともに説明する例では、左右両側方について側方安全距離Ｄｈを設定する例を挙げて説明する。
【００６４】
図７は、側方安全距離Ｄｈを複数設定した場合の制御例を示した説明図である。ここでは、機体左側方について第１の側方安全距離Ｄｈ１、機体右側方について第２の側方安全距離Ｄｈ２をそれぞれ設定した場合について説明する。なお、側方距離測定部７０９は進行方向Ｑに対して右側に９０°の右方向、そして、進行方向Ｑに対して左側に９０°の左方向に対して障害物を検出し、それぞれ障害物と機体との側方距離を測定する。
【００６５】
図７において、移動体１０１が地点Ｐ０に達して、進行方向Ｑにある障害物１０２までの距離Ｄが回避開始距離Ｄａとなると、回避動作制御部７０７は図３とともに上述したような回避動作を開始し、当該回避動作の目標地点を地点Ｐ１に設定する。また、地点Ｐ１への移動中に、地点Ｐ２で障害物１０２との距離Ｄが変化すると、回避動作制御部７０７は図３で上述したように、目標地点を地点Ｐ３に変更する。
【００６６】
そして、地点Ｐ３への移動中に地点Ｐ７１において、側方距離Ｄｓｓ２が側方安全距離Ｄｈ２を下回ると、回避動作制御部７０７は障害物１０３を回避するために、回避動作の目標地点を地点Ｐ３から地点Ｐ７２に変更する。次に、地点Ｐ７２の導出方法の一例について説明する。
【００６７】
なお、図７の例では、地点Ｐ２の位置情報を基準として、機体左側の側方距離Ｄｓｓ１と、機体右側の側方距離Ｄｓｓ２を算出し、回避経路更新の処理に用いるものとする。地点Ｐ２は、前方距離測定部７０４が前方の障害物１０２を最後に検出した地点である。即ち、地点Ｐ２は、先に回避対象としていた左側の障害物１０２について、移動体１０１が地点Ｐ２に到達した後は左側方の安全は確保される、という地点である。地点Ｐ２に到達した後は、右側方の障害物１０３に対する回避を優先して回避経路を決定すればよい。
【００６８】
回避動作制御部７０７は、複数の側方距離のうちいずれか一つが、対応する側方安全距離を下回った場合には、側方距離の平均値に基づいて回避経路上の目標地点を設定する。これにより、回避経路上の目標地点を、複数の障害物間の中央寄りに寄せて、機体と複数の障害物との距離がより均等となるよう図ることができる。
【００６９】
具体的には、回避動作制御部７０７は、側方距離Ｄｓｓ１が側方安全距離Ｄｈ１を下回った場合、あるいは、側方距離Ｄｓｓ２が側方安全距離Ｄｈ２を下回った場合に、側方安全距離Ｄｈ１、Ｄｈ２を用いた回避動作を中止する。そして、回避動作制御部７０７は、新たに算出した安全確保距離Ｄｒを用いて回避経路上の目標地点を設定する。
【００７０】
回避動作制御部７０７は、側方距離Ｄｓｓ１と側方距離Ｄｓｓ２との合計距離を等分した安全確保距離Ｄｒを算出する。
Ｄｒ＝（Ｄｓｓ１＋Ｄｓｓ２）／２                                  （式７）    
そして回避動作制御部７０７は、後から接近した障害物、即ち、右側の障害物１０３からの距離が安全確保距離Ｄｒとなる地点Ｐ７２を、地点Ｐ３に替わって新たな目標地点として設定しなおす。
【００７１】
即ち、回避動作制御部７０７は、側方安全距離Ｄｈ１、Ｄｈ２の代わりに、側方安全距離として安全確保距離Ｄｒを用いて回避経路上の目標地点を算出する。
【００７２】
その後、回避動作制御部７０７は、側方距離Ｄｓｓ１が側方安全距離Ｄｈ１を超え、かつ、側方距離Ｄｓｓ２が側方安全距離Ｄｈ２を超えるまでは、障害物１０３からの距離が上述した安全確保距離Ｄｒとなるよう、回避経路の目標地点を設定し続ける（例えば、地点Ｐ７３）。
【００７３】
そして地点Ｐ７４に到達し、側方距離Ｄｓｓ１が側方安全距離Ｄｈ１を超え、かつ、側方距離Ｄｓｓ２が側方安全距離Ｄｈ２を超えると、回避動作制御部７０７は回避動作制御を終了し、当初の進行方向Ｑへの基本移動制御を再開する。尚、基本移動制御に移行した後において回避動作制御部７０７は、側方安全距離Ｄｈ１、Ｄｈ２を用いて、回避動作を開始するか否かの判定を行う。
【００７４】
＜前方距離測定部の構成例＞
次に、前方距離測定部７０４の構成例について、図８ないし図１４を用いて説明する。尚、以下で説明する前方距離測定部７０４の構成および測定方法を、側方距離測定部７０９の構成および測定方法に利用してもよい。
【００７５】
図８は、前方距離測定部７０４において、パルスレーザを用いる場合の構成例を示した構成図である。図８に示すように、前方距離測定部７０４は、制御ユニット８０１、パルス発生器８０２、レーザダイオード８０３、半透過ミラー８０４、参照波検出器８０６、測定波検出器８０７を主に備えている。制御ユニット８０１は、ＡＤコンバータ８０５ａ、８０５ｂを備えている。
【００７６】
制御ユニット８０１は、パルス発生器８０２にパルス幅、パルス周期、振幅など、パルス発生の指示を出力する。ＡＤコンバータ８０５ａは、参照波検出器８０６が検出した信号をＡＤ変換する。ＡＤコンバータ８０５ｂは、測定波検出器８０７が検出した信号をＡＤ変換する。制御ユニット８０１は、ＡＤ変換後の信号を用いて、移動体１０１と測定対象物８１０との距離を測定する。
【００７７】
パルス発生器８０２は、制御ユニット８０１からの指示を受けて、単発の電気パルス、あるいは、連続した電気パルス列を発生させる。レーザダイオード８０３は、パルス発生器８０２から受信した電気パルスに応じたタイミングでパルスレーザ光を出射する。レーザダイオード８０３から出射したレーザ光は半透過ミラー８０４に入射する。
【００７８】
半透過ミラー８０４に入射したレーザ光の一部は半透過ミラー８０４で反射し、反射したレーザ光は参照波検出器８０６で検出されて電気信号に変換される。
【００７９】
また、半透過ミラー８０４に入射したレーザ光の一部は、半透過ミラー８０４を通過して測定対象物８１０の表面で反射する。反射光（反射波）の少なくとも一部は測定波検出器８０７で検出されて電気信号に変換される。
【００８０】
制御ユニット８０１は、参照波検出器８０６の電気信号と、測定波検出器８０７の電気信号とに基づき、移動体１０１と測定対象物８１０との距離を測定する。一例として、制御ユニット８０１は、参照波検出器８０６が検出したパルス到達のタイミングと、測定波検出器８０７が検出したパルス到達のタイミングとの時間差Δｔ１を算出する。そして、制御ユニット８０１は、光速３．０×１０８ｍ／秒と、光路往復分を考慮した１／２とを時間差Δｔ１に乗ずることで、移動体１０１と測定対象物８１０との距離を算出する。
【００８１】
図９は、前方距離測定部７０４において、超音波を用いる場合の構成例を示した構成図である。この場合、前方距離測定部７０４は、図９に示すように、制御ユニット９０１、パルス発生器９０２、超音波発生器９０３、測定波検出器９０４を主に備えている。制御ユニット９０１は、ＡＤコンバータ９０５を備えている。
【００８２】
制御ユニット９０１は、パルス発生器９０２にパルス幅、パルス周期、振幅など、パルス発生の指示を出力する。ＡＤコンバータ９０５は、測定波検出器９０４が検出した信号をＡＤ変換する。制御ユニット９０１は、ＡＤ変換後の信号を用いて、移動体１０１と測定対象物９１０との距離を測定する。
【００８３】
パルス発生器９０２は、制御ユニット９０１からの指示を受けて、単発の電気パルス、あるいは、連続した電気パルス列を発生させる。超音波発生器９０３は、パルス発生器９０２から受信した電気パルスに応じたタイミングで超音波を出射する。パルス発生器９０２から出射した超音波は、測定対象物９１０の表面で反射し、反射した超音波の一部は測定波検出器９０４で検出されて電気信号に変換される。
【００８４】
制御ユニット９０１は、パルス発生器９０２におけるパルス出力のタイミングと、測定波検出器９０４が検出したパルス到達のタイミングとの時間差Δｔ２を算出する。そして、制御ユニット９０１は、音速３４０ｍ／秒と超音波往復分を考慮した１／２とを時間差Δｔ２に乗ずることで、移動体１０１と測定対象物９１０との距離を算出する。
【００８５】
尚、図８、図９で説明した距離測定方法は一例にすぎず、その他の方法で移動体１０１と測定対象物との距離を測定してもよい。例えば、移動体１０１に搭載した撮像部で測定対象物を撮像し、撮像画像を画像処理するなどして移動体１０１と測定対象物との距離を測定してもよい。
【００８６】
図８では、前方距離測定部７０４にパルスレーザを用いた構成例を示したが、さらに、レーザ光を所定の範囲で走査する構成としてもよい。つまり、レーザ光は直進性が高いため、図９のように超音波を用いる場合と比べて対象物を検出する範囲（レーザ光の照射領域）が限定されることとなる。移動体の大きさは数１０ｃｍから数ｍ程度と想定されるので、照射範囲をより拡大してより広い範囲で障害物の有無を検出する際には、レーザ光を適宜設定した範囲で走査する構成とすればよい。
【００８７】
図１０は、前方距離測定部７０４がレーザ光を走査する構成を有する場合の構成図である。図１０に示す構成例では、前方距離測定部７０４において、レーザ光の光路上に走査ミラー１１０５を配置した例を示す。その他の構成は、図８で示した構成と同様である。走査ミラー１１０５の光軸は、ピエゾ振動子１１０４によりＸ軸およびＹ軸の２軸方向に駆動可能である。
【００８８】
図１１は、ピエゾ振動子１１０４の駆動電圧の一例を示した説明図である。制御ユニット１１０１は、ピエゾ振動子１１０４をＸ方向に駆動する電極、および、ピエゾ振動子１１０４をＹ方向に駆動する電極に対して、図１１に示すような駆動電圧を印加する。これにより、走査ミラー１１０５の光軸は、Ｘ軸方向およびＹ軸方向に周期的に変動する。
【００８９】
図１２は、測定対象物８１０上におけるレーザ光の走査例を示した説明図である。図１１に示すような駆動電圧がピエゾ振動子１１０４に印加されると、レーザ光は、測定対象物８１０上において図１２に示すように走査される。即ち、レーザ光は、Ｘ方向およびＹ方向にそれぞれ所定の範囲を持った領域内を走査することとなる。
【００９０】
このようにレーザ光を走査することにより、直進性の高いレーザ光を用いた場合でも、レーザ光の照射範囲を拡大して障害物の有無を検出することができる。また、測定対象物８１０との距離を測定する際に精度の向上を図ることができる。
【００９１】
尚、制御ユニット１１０１は、ピエゾ振動子１１０４に印加する駆動電圧の振幅を増減させることで、測定対象物８１０上でのレーザ照射領域を増減することができる。
【００９２】
また、制御ユニット１１０１は、ピエゾ振動子１１０４に印加する駆動電圧の波形（周波数、位相）を変化させることで、測定対象物８１０上でのレーザ光の走査周期や走査パターン、走査パターンの粗密などを変化させることができる。
【００９３】
図１３は、ピエゾ振動子１１０４の駆動電圧の他の例を示した説明図である。また、図１４は、図１３に示す駆動電圧を用いた際のレーザ光の走査パターン例を示した模式図である。制御ユニット１１０１は、図１３に例示するように、Ｘ方向の駆動電極に印加する電圧の周波数、あるいは、Ｙ方向の駆動電極に印加する電圧の周波数を変化させることにより、レーザ光の走査パターンを図１４に示すようなパターンに変えることができる。
【００９４】
なお、障害物回避装置１は、ＣＰＵなどの制御装置と、ＲＯＭ（Read  Only  Memory）やＲＡＭなどの記憶装置を備えている。本実施の形態の障害物回避装置１で実行されるプログラムは、ＲＯＭ等に予め組み込まれて提供される。
【００９５】
本実施の形態の障害物回避装置１で実行されるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フレキシブルディスク（ＦＤ）、ＣＤ−Ｒ、ＤＶＤ（Digital  Versatile  Disk）等のコンピュータで読み取り可能な記録媒体に記録して提供するように構成してもよい。
【００９６】
さらに、本実施の形態の障害物回避装置１で実行されるプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成しても良い。また、本実施の形態の障害物回避装置１で実行されるプログラムをインターネット等のネットワーク経由で提供または配布するように構成しても良い。
【００９７】
本実施形態の障害物回避装置１で実行されるプログラムは、図２で示したように、機体制御部７０１、進行方向制御部７０２、前方距離測定部７０４、位置情報取得部７０５、高度情報取得部７０６、回避動作制御部７０７、無線通信制御部７０８、側方距離測定部７０９を含むモジュール構成となっている。また、障害物回避装置１が有する記憶装置のメモリ領域内には、データ格納部７０３が設けられている。上記ＣＰＵ（プロセッサ）は、上記ＲＯＭから本実施形態のプログラムを読みだして実行する。これにより、図２に示した各部が主記憶装置上にロードされ、主記憶装置上に、機体制御部７０１、進行方向制御部７０２、前方距離測定部７０４、位置情報取得部７０５、高度情報取得部７０６、回避動作制御部７０７、無線通信制御部７０８、側方距離測定部７０９が生成される。
【００９８】
尚、本実施形態では、障害物回避装置１がこれら各部を備えた構成について示しているが、これら構成の一部を他の装置が有してもよい。そして、移動体１０１が当該他の装置との連携により上述した障害物の回避動作を行うとしてもよい。例えば、無線通信制御部７０８が無線通信により他の装置とのデータ送受信を行って、回避動作を行ってもよい。また、上記構成を他の装置が有する場合、複数のサーバ装置等に分散させて備える形態としてもよい。
【００９９】
以上説明した通り、上記実施形態では、目的地（最終目標地点）に向かう当初の進行方向Ｑと、障害物までの距離Ｄと、障害物との間で確保すべき安全距離Ｄｓとから回避方向Ｒを算出している。従って、当初の経路により近い回避経路をとって目的地に接近できるので、回避動作による移動距離の増加を抑えて、目的地までの移動距離をより短距離にすることができる。また、上記実施形態では、左右いずれかに回避するような単純な回避動作に比べて、より緩やかに障害物の形状に対応した方向変換を行うことができるので、より効率的な回避経路をとることができる。
【符号の説明】
【０１００】
１  障害物回避装置
１０１  移動体
７０１  機体制御部
７０２  進行方向制御部
７０３  データ格納部
７０４  前方距離測定部
７０５  位置情報取得部
７０６  高度情報取得部
７０７  回避動作制御部
７０８  無線通信制御部
７０９  側方距離測定部
【先行技術文献】
【特許文献】
【０１０１】
【特許文献１】特開平６−２４７３９３号公報
【図１】
2017220057.tif 000003
【図２】
2017220057.tif 000004
【図３】
2017220057.tif 000005
【図４】
2017220057.tif 000006
【図５】
2017220057.tif 000007
【図６】
2017220057.tif 000008
【図７】
2017220057.tif 000009
【図８】
2017220057.tif 000010
【図９】
2017220057.tif 000011
【図１０】
2017220057.tif 000012
【図１１】
2017220057.tif 000013
【図１２】
2017220057.tif 000014
【図１３】
2017220057.tif 000015
【図１４】
2017220057.tif 000016