JP2017151499A - 障害物回避方法および装置 - Google Patents

Abstract

【課題】 水中航走体が障害物を回避しながら目標位置まで移動できるようにする。【解決手段】 水中航走体の現在位置から目標位置に向かう目標位置方向を求める。水中航走体の進行方向を前記目標位置方向に定める。進行方向の設定された領域内に回避対象となる障害物が存在する場合は、障害物回避処理を開始する。障害物回避処理では、先ず、水中航走体の進行方向の左右いずれか一側が障害物に沿う姿勢となるように、水中航走体の進行方向を障害物に沿う方向に定めて水中航走体を移動させる。その後、水中航走体の現在位置から目標位置方向の設定された判定領域内に障害物が存在する間は、障害物に沿う方向の移動を継続する。判定領域内に障害物が存在しなくなると、障害物回避処理を終了し、水中航走体を現在位置を基準とする目標位置方向に移動させる。【選択図】図３

Description

本発明は、自律移動体が障害物を回避して移動するための障害物回避方法および装置に関するものである。

自律移動体のうち、たとえば、水中を移動する自律型水中ロボットを自律移動させるときの制御手法としては、出発点と、到達目標点と、出発点から到達目標点までの航走海域周辺の地理情報とを基に、燃料消費量や所要時間などの評価値を最適化する手法を用いて、出発点から到達目標点までの全体経路（道順）を決定させる手法が従来提案されている。

更に、前記自律型水中ロボットでは、全体経路上にて局所的な出発点と目標点とを定め、その間の局所経路を水中ロボット自身の運動性能に合わせて決める機能を備えること、および、自律型水中ロボットに搭載されたセンサにより障害物が検出された場合は、前記目標点に向かうための方位と、障害物を回避するための方位とを適当な重み付けを行って合成することによって障害物を回避しつつ前記目標点へ向かうための方位を決定すること、が提案されている（たとえば、特許文献１参照）。

特開平１１−２４９７３４号公報

ところが、特許文献１に示された手法は、出発点から到達目標点までの全体経路を決定する際に燃料消費量や所要時間などの評価値を最適化しているが、この評価値を得るためには、出発点から到達目標点までの航走海域周辺の地理情報が既知である必要がある。

更に、特許文献１に示された手法では、自律型水中ロボットが、局所的な出発点と目標点との間の局所経路を順次定め、この局所経路に障害物が存在する場合は、障害物を回避して目標点に向かう方位が決定されるものとされている。しかし、この手法を実施するためには、自律型水中ロボットが自身で局所経路を設定するときに、目標点には障害物がないという地理情報が既知である必要がある。しかも、局所経路に存在する障害物を回避して目標点に向かう方位の具体的な決定手法については示されていない。

したがって、特許文献１に示された手法では、自律移動体としての自律型水中ロボットを、障害物の有無などの地理情報が未知の領域で、設定された目標位置まで移動させる場合に適用することができるものではない。

そこで、本発明は、自律移動体が、地理情報が未知の領域にて、障害物を回避しながら設定された目標位置まで移動できるようにするための障害物回避方法および装置を提供しようとするものである。

本発明は、前記課題を解決するために、自律移動体の現在位置から目標位置に向かう目標位置方向を求める処理と、前記自律移動体の進行方向を前記目標位置方向に定める処理と、前記自律移動体の現在位置から前記進行方向の設定された領域内に回避対象となる障害物が存在する場合に、前記自律移動体の左右いずれか一側が前記障害物に沿う姿勢となるように前記自律移動体の進行方向を該障害物に沿う方向に定める処理と、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合は、前記自律移動体の進行方向を目標位置方向に設定する処理と、を行う障害物回避方法とする。

更に、前記自律移動体の回避対象となる障害物をマップに記録する処理と、前記マップ上で、前記自律移動体の進行方向を障害物に沿う方向に定める際に基準とした前記障害物についてマークを付する処理と、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合に、前記自律移動体の進行方向を目標位置方向に設定する処理に代えて、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合であって、該自律移動体の現在位置から前記目標位置方向に前記マップ上で前記マークが付された障害物が存在するときには、前記自律移動体の左右いずれか一側が前記障害物に沿う姿勢となるように前記自律移動体の進行方向を該障害物に沿う方向に定めた状態を継続する処理と、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合であって、該自律移動体の現在位置から前記目標位置方向に前記マップ上で前記マークが付された障害物が存在しないときには、前記自律移動体の進行方向を目標位置方向に設定する処理と、を行う方法としてある。

また、自律移動体の自身の位置を検出する手段と、前記自律移動体の回避対象となる障害物を検出する手段と、演算装置とを備え、前記演算装置は、前記した障害物回避方法を実施する機能を備える障害物回避装置とする。

本発明の障害物回避方法および装置によれば、自律移動体は、地理情報が未知の領域にて、障害物を回避しながら設定された目標位置まで移動することができる。

障害物回避装置の一例を示す概要図である。 回避対象となる障害物を示す図である 障害物回避方法の第１実施形態のフローを示す図である。 障害物回避処理による自律移動体の移動経路を示す図である。 複数の障害物をまとめて回避する手法を示す図である。 障害物回避装置の別の例を示す図である。 障害物回避方法の第２実施形態のフローを示す図である。 障害物回避処理による自律移動体の移動経路を示す図である。

以下、本発明を実施するための形態を図面を参照して説明する。

［障害物回避方法の第１実施形態］
図１は、障害物回避方法の第１実施形態の実施に用いる障害物回避装置を示す概要図である。図２は、回避の対象となる障害物を説明するための図である。図３は、障害物回避方法の第１実施形態における処理手順を示すフロー図である。図４は、図３の障害物回避方法によって定められる自律移動体の移動経路の例を示すもので、図４（ａ）は自律移動体が進行予定経路上に検出された１つの障害物を回避する場合の移動経路を示す概要図、図４（ｂ）は１つの障害物を回避した後の進行予定経路上に検出された別の障害物を回避する場合の移動経路を示す概要図である。図５は、複数の障害物をまとめて回避する手法を説明するための図である。

本実施形態は、障害物回避方法を、自律移動体として自律型の水中航走体を適用して行う場合について説明する。

本実施形態の障害物回避方法では、水中航走体の進行方向を左右に変化させることにより障害物の回避を行う。この障害物の回避パターンは、水中航走体の浅海域での運用時に水中航走体が障害物を回避する場合に用いられる。また、この回避パターンは、水中航走体が上昇しても障害物の上方へ回避できない場合に、水中航走体が障害物を回避する場合にも用いられる。

先ず、図１を用いて、本実施形態の障害物回避方法の実施に用いる障害物回避装置１の構成について説明する。

障害物回避装置１は、水中航走体Ａに装備されるもので、水中航走体Ａ自身の位置と姿勢（向き）を検出する手段２（以下、自己位置検出手段２という）と、水中航走体Ａの周囲の障害物ｂ（図２参照）を検出する手段３（以下、障害物検出手段３という）と、アクチュエータ４と、演算装置５とを備えた構成とされている。

自己位置検出手段２は、水中航走体Ａ自身の位置の情報を、水中航走体Ａが移動する領域にある地点を基準として設定される絶対座標に関して検出する機能を備えている。なお、この絶対座標としては、経度と緯度を用いることが好適であるが、それ以外の任意の地点を原点とする絶対座標を用いるようにしてもよい。

また、自己位置検出手段２は、水中航走体Ａの姿勢として、水中航走体Ａがどの方向に向いているかという情報を検出する機能を備えている。

更に、自己位置検出手段２は、水中航走体Ａが移動するときの移動速度に関する情報を検出する機能を備えることが好ましい。

水中航走体Ａに装備する自己位置検出手段２としては、たとえば、慣性航法装置（ＩＮＳ）が用いられる。

障害物検出手段３は、水中航走体Ａの周囲の障害物ｂを、水中航走体Ａを基準とする相対位置で検知する機能を備えている。水中の障害物ｂを検知する障害物検出手段３としては、たとえば、ソーナーが用いられる。

なお、障害物検出手段３は、少なくとも、図２に示すように、水中航走体Ａの進行方向（矢印方向）の前方を中心として、その左右両側に以下で述べる離隔距離ｃの分、離れた位置までの矩形状（帯状）の領域ｄに存在する障害物ｂを検出する機能を備えている。

この領域ｄについて、水中航走体Ａの進行方向に沿う方向の長さは、障害物検出手段３の検知範囲（探知範囲）により定まっている。

前記離隔距離ｃは、水中航走体Ａが障害物ｂに接触することを防止するために、水中航走体Ａのサイズや運用状況に応じて水中航走体Ａと障害物ｂとの間に最低限保持することが必要な寸法として設定される距離である。あるいは、離隔距離ｃは、水中航走体Ａを障害物ｂからどの程度離れた状態で回避させるかを、水中航走体Ａの使用者が水中航走体Ａの運用状況などに応じて設定するようにしてもよい。

したがって、障害物検出手段３により前記領域ｄ内に障害物ｂが検出されない状態のときには、水中航走体Ａは、その時点での進行方向に移動しても、障害物ｂに対して離隔距離ｃ以内に接近する虞はない。

一方、障害物検出手段３により前記領域ｄ内に、たとえば、図２に二点鎖線で示すような障害物ｂが検出された場合は、水中航走体Ａは、その時点での進行方向に移動すると、検出された障害物ｂとの間に離隔距離ｃを保持することができなくなる。したがって、この場合は、水中航走体Ａは進行方向を左右方向に変えて障害物ｂを回避する必要が生じる。

水中航走体Ａの進行方向前方の所定の領域ｄで障害物ｂを検出するための障害物検出手段３としては、たとえば、水中航走体Ａにおける進行方向の前端側となる個所に、前方検知ソーナー（ＦＬＳ：Forward-Looking SONAR）を備えるようにすればよい。

なお、障害物検出手段３は、前記所定の領域ｄに存在する障害物ｂを検出可能なものであればよく、障害物検出手段３により実際に障害物ｂの検出が可能となる検知範囲は、前記矩形の領域ｄよりも広い幅を有していてよいことは勿論である。

更に、後述するように、本実施形態の障害物回避方法で障害物ｂの回避を行うときには、水中航走体Ａは、回避対象となる障害物ｂとの間に離隔距離ｃを保持しながら該障害物ｂに沿って移動する。そのため、障害物検出手段３は、水中航走体Ａの進行方向に対して側方に位置する障害物ｂも検出できることが好ましい。

このように、水中航走体Ａの側方に位置している障害物ｂを検出するための障害物検出手段３としては、たとえば、水中航走体Ａの進行方向に対して左右両側となる両側部に、側方検知用のソーナーを備えるようにすればよい。

あるいは、水中航走体Ａの進行方向の前端側に設ける障害物検出手段３が、水中航走体Ａの側方に位置している障害物ｂを検出するための障害物検出手段３を兼ねる構成としてもよい。この場合は、たとえば、水中航走体Ａの進行方向の前端側に設ける障害物検出手段３を、超音波のビームを水中航走体Ａの進行方向の前方を中心に左右方向に走査する形式の前方検知ソーナーとする。更に、この前方検知ソーナーは、超音波のビームを左右方向に走査するときの中心を、水中航走体Ａの進行方向の前方から、検出対象の障害物ｂが存在している左右いずれかの側に傾ける機能を備えるようにすればよい。これにより、前方検知ソーナーでは、進行方向前方の障害物に加えて、水中航走体Ａの側方に位置する障害物ｂ、あるいは、水中航走体Ａの移動に伴って水中航走体Ａの側方に位置するようになる障害物ｂの検出を行うことができる。

アクチュエータ４は、水中航走体Ａの移動を行うための移動手段であり、水中航走体Ａの形式に応じてスラスタや操舵装置などを備えた構成とされている。このアクチュエータ４に依存して、水中航走体Ａの加速、推進、制動などに関する性能と、水中航走体Ａの姿勢や進行方向を変える性能が定まる。

演算装置５は、図１に示すように、情報処理部６と、マップ記憶部７と、移動方向算出処理部８と、アクチュエータ指令処理部９とを備えた構成とされている。

情報処理部６には、自己位置検出手段２により検出された絶対座標系における水中航走体Ａ自身の位置の情報と、障害物検出手段３により検出された水中航走体Ａを基準とする相対座標系における障害物ｂの位置情報とが入力される。

情報処理部６は、入力された前記各情報を基に、ある時点での障害物ｂの位置情報を示す相対座標系における原点の位置に、同時点での絶対座標系における水中航走体Ａ自身の位置を代入して、障害物ｂの絶対座標系に関する位置の情報を算出する機能を備えている。更に、情報処理部６は、その算出結果を基に、障害物検出手段３で検出された障害物ｂの位置を絶対座標系で示すマップｅを作成する機能を備えている。

情報処理部６で作成されたマップｅは、マップ記憶部７に記憶される。情報処理部６は、障害物検出手段３より新たな障害物ｂの位置情報が入力されると、その障害物ｂの絶対座標系に関する位置の情報を算出して、それを前記マップｅに順次追加する機能も備えている。

なお、水中航走体Ａの障害物検出手段３によって実際に障害物ｂの位置が検出されるのは、障害物ｂにおける水中航走体Ａの現在位置に近い側の面（表面）である。水中航走体Ａの移動に伴って水中航走体Ａの現在位置が変化すると、障害物ｂにおいて障害物検出手段３によって実際に位置が検出される面の範囲も徐々にずれる。したがって、情報処理部６は、マップｅに、水中航走体Ａが移動するときに障害物検出手段３によって検出される障害物ｂの面の位置の情報を順次追加するようにしてある。

これにより、マップ記憶部７に記憶されているマップｅによれば、水中航走体Ａの周辺に存在している障害物ｂの位置の情報が分かる。なお、図２では、図示する便宜上、障害物ｂは全体の平面形状で示してある（後述する図４（ａ）（ｂ）、図５、図８も同様である）。

図１に示すように、移動方向算出処理部８には、前記のように設定された離隔距離ｃと、別途設定された目標位置ｆ（図４（ａ）参照）の情報が与えられている。

この目標位置ｆは、水中航走体Ａを任意の方式で自律移動させるときの到達目標として設定された位置である。なお、目標位置ｆは、水中航走体Ａの最終的な到達目標であってもよいし、ウェイポイント方式で設定されるウェイポイントのように、水中航走体Ａを自律移動させるときに経路の途中で経由させるために水中航走体Ａの一時的な到達を目的とする到達目標であってもよい。

移動方向算出処理部８に与えられる目標位置ｆの情報は、前記マップｅと同じ絶対座標系で設定されていることが好ましい。なお、換算などによってマップｅと同じ絶対座標系にて目標位置ｆを特定することができれば、移動方向算出処理部８に与えられる目標位置ｆの情報は、いかなる形式の情報であってもよい。この場合は、移動方向算出処理部８が、与えられた目標位置ｆの情報を基に、マップｅと同じ絶対座標系における目標位置ｆを特定する機能を備えるようにすればよい。

移動方向算出処理部８には、自己位置検出手段２から、水中航走体Ａ自身の位置と姿勢と移動速度の情報ｉが入力される。更に、移動方向算出処理部８には、マップ記憶部７からマップｅに記録されている障害物ｂの位置の情報ｊが入力される。

移動方向算出処理部８は、離隔距離ｃと目標位置ｆと前記各情報ｉ，ｊが入力されると、図３にフローを示すルールに従って、水中航走体Ａの移動方向を算出する機能を備えている。このルールについては後述する。

アクチュエータ指令処理部９は、移動方向算出処理部８で算出された移動方向に水中航走体Ａが移動するように、アクチュエータ４に指令を与える機能を有している。

また、アクチュエータ指令処理部９は、水中航走体Ａの移動方向に関する指令に加えて、移動速度の制御に関する指令をアクチュエータ４に与える機能を有している。たとえば、アクチュエータ指令処理部９には、図示しないが、水中航走体Ａの使用目的などに応じて、障害物ｂの回避が必要ない状態のときの水中航走体Ａの移動速度の目標値が、平常時移動速度ｖ１（図４参照）として設定されている。更に、アクチュエータ指令処理部９には、水中航走体Ａの運動性能や、設定された離隔距離ｃ、更には想定される外乱などに応じて、水中航走体Ａが障害物ｂを回避する状態のとき移動速度の目標値が回避用移動速度ｖ２（図４参照）として設定されている。これにより、アクチュエータ指令処理部９は、水中航走体Ａが障害物ｂの回避処理を行っていないときには、水中航走体Ａの移動速度が設定された平常時移動速度ｖ１に一致するように、アクチュエータ４に指令を与える機能を備えている。また、アクチュエータ指令処理部９は、水中航走体Ａが障害物ｂの回避処理を行っているときには、水中航走体Ａの移動速度が設定された回避用移動速度ｖ２に一致するように、アクチュエータ４に指令を与える機能を備えている。

更に、水中航走体Ａについて、その使用目的に応じて目標姿勢が設定されている場合は、アクチュエータ指令処理部９は、水中航走体Ａの姿勢が設定された目標姿勢に一致するように、アクチュエータ４に指令を与える機能を備えるようにすればよい。

次に、図３に示したルールについて説明すると共に、このルールに基づく水中航走体Ａの移動について説明する。

移動方向算出処理部８は、ステップＳ１で、図４（ａ）に示すように、水中航走体Ａの現在位置から目標位置ｆに向かう方向ｇ（以下、目標位置方向ｇという）を求める。図４（ａ）には、水中航走体Ａが位置ｘ１、位置ｘ２、位置ｘ３、位置ｘ４に存在している時点での目標位置方向ｇがそれぞれ破線で示してある。

次に、移動方向算出処理部８は、目標位置方向ｇを水中航走体Ａの進行方向に設定する（ステップＳ２）。

このように、ステップＳ２で水中航走体Ａの進行方向が目標位置方向ｇに設定されるのは、図４に位置ｘ１で示すように、水中航走体Ａの目標位置方向ｇに回避対象となる障害物ｂが検出されていない状態のときである。このため、図示しないが、移動方向算出処理部８は、ステップＳ２に伴って、アクチュエータ指令処理部９（図１参照）に対して、水中航走体Ａの移動が障害物回避処理ではなく、平常に行われているという情報を与える機能も備えることが好ましい。

これにより、アクチュエータ指令処理部９は、移動方向算出処理部８で進行方向として設定された目標位置方向ｇに向けて水中航走体Ａが移動するようにアクチュエータ４を制御するための指令を、アクチュエータ４に与える。また、アクチュエータ指令処理部９は、水中航走体Ａの移動速度が平常時移動速度ｖ１となるようにするための指令を、アクチュエータ４に与える。

したがって、水中航走体Ａは、図４における位置ｘ１では、目標位置方向ｇに向けて平常時移動速度ｖ１で移動するようになる。

次いで、移動方向算出処理部８は、設定された進行方向（目標位置方向ｇ）の前方における図２で説明した所定の領域ｄに、回避対象となる障害物ｂが存在するか否かを判断する（ステップＳ３）。

ステップＳ３にて、進行方向の前方に回避対象となる障害物ｂが存在しないと判断された場合は、移動方向算出処理部８は、ステップＳ１に戻る。

これにより、移動方向算出処理部８では、水中航走体Ａの進行方向の前方に回避対象となる障害物ｂが存在しない状態では、ステップＳ１からステップＳ３の処理を順次繰り返す。 この際、移動する水中航走体Ａは、時間の経過と共に現在位置が順次変化し、外乱などの影響により進行方向にずれが生じることもあるので、移動方向算出処理部８では、ステップＳ１に戻るごとに、その時点での水中航走体Ａの現在位置を順次更新し、更新後の現在位置から目標位置ｆに向かう方向の目標位置方向ｇを再度求める。その後のステップＳ２では、水中航走体Ａの進行方向を、更新された目標位置方向ｇに設定する。

したがって、水中航走体Ａは、移動方向算出処理部８でステップＳ１からステップＳ３の処理が繰り返される間は、目標位置方向ｇに向かう平常時移動速度ｖ１での移動を継続して行うようになる。

一方、図３に示すように、ステップＳ３にて、水中航走体Ａの進行方向の前方に回避対象となる障害物ｂが存在すると判断される場合は、移動方向算出処理部８は、ステップＳ４に進んで、障害物回避処理を開始する。

障害物回避処理を開始すると、移動方向算出処理部８は、先ず、水中航走体Ａが、図４（ａ）に位置ｘ２で示すように、回避対象となる障害物ｂに設定された距離ｈまで近づくと、水中航走体Ａの進行方向を、目標位置方向ｇから、目標位置方向ｇに対して左方向または右方向で且つ回避対象の障害物ｂに沿って移動する方向まで変化させる（ステップＳ５）。

たとえば、図４（ａ）における位置ｘ２で目標位置方向ｇに平常時移動速度ｖ１で移動している水中航走体Ａが、位置ｘ３で障害物ｂに沿う方向に回避用移動速度ｖ２で移動するようにするためには、水中航走体Ａを平常時移動速度ｖ１を回避用移動速度ｖ２まで減速するための制動距離が必要になると共に、水中航走体Ａの進行方向を変えるための旋回半径および旋回時間が必要とされる。しかも、水中航走体Ａは、位置ｘ３に到達した時点でも、障害物ｂとの間に離隔距離ｃが保持されている必要がある。

したがって、前記距離ｈは、以下のようにして定められる。すなわち、水中航走体Ａが進行方向を目標位置方向ｇから障害物ｂに沿う方向に変える処理を完了して位置ｘ３に到達した時点で、障害物ｂからの離隔距離ｃが保持されるように、水中航走体Ａの制動時間、旋回半径および旋回時間を考慮して定められている。

図３のステップＳ５において、水中航走体Ａの進行方向を目標位置方向ｇから右方向へ変化させるか、あるいは、左方向へ変化させるかは、たとえば、マップ記憶部７（図１参照）のマップｅに記録されている回避対象となる障害物ｂの位置情報を用いて、障害物ｂの目標位置方向ｇよりも左側の分布状況と、右側の分布状況とを比較し、障害物ｂの分布量がより小さくなっている側を、水中航走体Ａの進行方向を変化させる方向として選択するようにすればよい。

なお、水中航走体Ａの進行方向を左方向と右方向のいずれに変化させるかは、前記分布状況以外の何らかのパラメータや障害物ｂに関する情報を用いて、障害物ｂの回避に要する距離がより短くなる方向を推定し、その推定結果に従って定めるようにしてもよいことは勿論である。また、水中航走体Ａが障害物ｂを回避するために進行方向を変える方向は、左方向または右方向のいずれか一方に予め決めておくようにしてもよい。

更に、ステップＳ５で水中航走体Ａの進行方向が目標位置方向ｇから変更されるのは、障害物ｂを回避するためである。このため、図示しないが、移動方向算出処理部８は、ステップＳ５に伴って、アクチュエータ指令処理部９に対して、水中航走体Ａの障害物回避処理を開始したという情報を与える機能も備えることが好ましい。

これにより、アクチュエータ指令処理部９は、水中航走体Ａの進行方向を移動方向算出処理部８で決定された左方向または右方向に変えるための指令を、アクチュエータ４に与える。更に、アクチュエータ指令処理部９は、水中航走体Ａの移動速度を平常時移動速度ｖ１から回避用移動速度ｖ２に減速するための指令を、アクチュエータ４に与える。

したがって、水中航走体Ａは、アクチュエータ４による制動を行って、移動速度を平常時移動速度ｖ１から回避用移動速度ｖ２まで減速すると共に、水中航走体Ａの進行方向を目標位置方向ｇから移動方向算出処理部８で定められた左方向または右方向の障害物ｂに沿う方向へ変えるよう旋回する。図４（ａ）では、旋回する水中航走体Ａの進行方向が目標位置方向ｇから左方向に定められた場合を示している。

水中航走体Ａは、左方向への旋回が完了して位置ｘ３に達した時点で、障害物ｂとの間に離隔距離ｃを保持した状態で、水中航走体Ａの進行方向の右側が障害物ｂに沿う姿勢となる。

移動方向算出処理部８は、図３に示すように、ステップＳ５の後はステップＳ６に進む。

ステップＳ６では、移動方向算出処理部８は、障害物ｂとの間に離隔距離ｃを保持した状態で、水中航走体Ａの進行方向の右側が障害物ｂに沿う姿勢となるように、水中航走体Ａの進行方向を、障害物ｂに沿う方向に定める。

これにより、アクチュエータ指令処理部９は、水中航走体Ａの進行方向を移動方向算出処理部８で決定された障害物ｂに沿う方向とするための指令を、アクチュエータ４に与える。また、アクチュエータ指令処理部９は、水中航走体Ａの移動速度を回避用移動速度ｖ２に保持する指令を、アクチュエータ４に与える。

したがって、水中航走体Ａは、図４（ａ）における位置ｘ３に達した後は、進行方向の右側の障害物ｂとの間に離隔距離ｃを保ちながら、障害物ｂに沿って移動するようになる。

図３に示すように、移動方向算出処理部８は、ステップＳ６の後はステップＳ７に進む。

ステップＳ７では、移動方向算出処理部８は、設定された時間間隔で、水中航走体Ａの現在位置を基準として、目標位置方向ｇを中心とする左右に離隔距離ｃの幅で且つ目標位置方向ｇに設定された距離の矩形状（帯状）の判定領域内に障害物ｂがあるか否かを判断する。この判定領域内の障害物ｂの有無の判断は、水中航走体Ａの移動に伴って障害物検出手段３により検出されてマップｅに順次追加登録されている障害物ｂの位置を参照して行うようにすればよい。

判定領域内に存在する障害物ｂは、水中航走体Ａが現在位置から目標位置ｆに向けて移動しようとすると、すぐに離隔距離ｃを保つことができなくなる障害物ｂである。したがって、この判定領域内に存在する障害物ｂについては、障害物回避処理を継続して行う必要がある。

なお、この場合、障害物回避処理を継続して行う必要がある障害物ｂは、ステップＳ７の処理を行う時点の以前に回避処理の対象とされていた障害物ｂに繋っている部分に限られるものではなく、その時点以前に回避処理の対象とされていた障害物ｂに対して、離隔距離ｃの２倍未満の間隔（より具体的には、離隔距離ｃの２倍と水中航走体Ａの幅寸法との和よりも小さい間隔）で配置されている別の障害物ｂも含まれる。これは、複数の障害物ｂが離隔距離ｃの２倍よりも小さい間隔で隣接している場合は、その間を水中航走体Ａが双方の障害物ｂに対して離隔距離ｃを保持しながら通過することはできないので、このような配置の複数の障害物ｂは、まとめて回避することが妥当なためである。

移動方向算出処理部８は、ステップＳ７において判定領域内に障害物ｂが存在すると判断された場合は、ステップＳ６に戻る。

これにより、水中航走体Ａが移動して現在位置が変化しても、その現在位置を基準として定まる目標位置方向ｇの判定領域内に障害物ｂが存在している間は、移動方向算出処理部８では、ステップＳ６とステップＳ７の処理ループが繰り返し実施される。

したがって、図４（ａ）に位置ｘ３から位置ｘ４の間の移動経路で示すように、水中航走体Ａは、現在位置から目標位置方向ｇの判定領域内に障害物ｂが存在している間は、障害物ｂとの間に離隔距離ｃを保持しつつ、障害物ｂに沿う移動を継続して行うようになる。

一方、図３のステップＳ７で、水中航走体Ａの現在位置から目標位置方向ｇの判定領域内に障害物ｂが存在しないと判断される場合は、移動方向算出処理部８は、ステップＳ８に進む。

この場合は、水中航走体Ａが現在位置から目標位置方向ｇに向けて移動しても、障害物ｂは存在していない状態になっているので、移動方向算出処理部８は、ステップＳ８で、障害物回避処理を終了してから、ステップＳ１に戻る処理を行う。

これにより、移動方向算出処理部８では、障害物ｂを回避した後の水中航走体Ａの現在位置を基準とする目標位置方向ｇがステップＳ１で求められ、ステップＳ２にて、その目標位置方向ｇが水中航走体Ａの進行方向に設定される。

したがって、図４（ａ）の位置ｘ４に到達して目標位置方向ｇの判定領域内に障害物ｂが存在しなくなると、水中航走体Ａは、障害物ｂの回避処理を終了して目標位置方向ｇを進行方向とする移動を再開するようになる。

また、図示しないが、移動方向算出処理部８は、ステップＳ８の障害物回避処理終了に伴って、アクチュエータ指令処理部９に対して、水中航走体Ａの障害物回避処理を終了したという情報を与える機能も備えることが好ましい。

これにより、アクチュエータ指令処理部９は、水中航走体Ａの移動速度を平常時移動速度ｖ１にするための指令を、アクチュエータ４に与える。

したがって、水中航走体Ａは、目標位置方向ｇを進行方向とする移動を開始すると、移動速度が回避用移動速度ｖ２から平常時移動速度ｖ１に復帰される。

なお、図４（ｂ）に示すように、水中航走体Ａが、図４（ａ）の場合と同様に、位置ｘ４に到達して目標位置方向ｇに向かう移動を再開した後、進行方向となる目標位置方向ｇに新たな障害物ｂ１が存在していた場合は、移動方向算出処理部８にて、図３におけるステップＳ３の処理が再び開始される。

なお、この場合、水中航走体Ａが新たな障害物ｂ１の回避処理を開始するときには、水中航走体Ａの移動速度が平常時移動速度ｖ１まで復帰していない場合がある。この場合は、水中航走体Ａが目標位置方向ｇから向きを変える位置の障害物ｂ１からの距離ｈ１は、障害物回避処理を開始する時点での水中航走体Ａの移動速度に応じて適宜調整すればよい。

これにより、水中航走体Ａは、この新たに検出された障害物ｂ１についても、離隔距離ｃを保持しながら回避し、その後、目標位置方向ｇに向かう移動を再開するようになる。

その後、水中航走体Ａが目標位置方向ｇに移動しているときに、進行方向の前方に回避対象となる障害物ｂが検出される場合は、その障害物ｂについても水中航走体Ａは前記したと同様の回避処理を行ってから、目標位置方向ｇへの移動を再開するようになる。

なお、図示しないが、水中航走体Ａが目標位置ｆに到達すると、移動方向算出処理部８は、その目標位置ｆに到達するための処理を終了する。

また、水中航走体Ａが到達した目標位置ｆが、水中航走体Ａを自律移動させるときに経路の途中で経由させるために一時的な到達を目的とする到達目標であった場合は、移動方向算出処理部８は、水中航走体Ａが現在位置から次に到達すべき到達目標を目標位置ｆとして、図３に示したルールによる処理を再開するようにすればよい。

このように、本実施形態の障害物回避方法および装置によれば、水中航走体Ａは、障害物検出手段３で検出される範囲の障害物ｂの位置情報を用いて、障害物ｂを回避しながら設定された目標位置ｆに到達することができる。

したがって、本実施形態の障害物回避方法および装置では、水中航走体Ａは、地理情報が未知の領域にて、障害物ｂを回避しながら設定された目標位置ｆまで移動することができる。

ところで、障害物検出手段３で検出された複数の障害物ｂが離隔距離ｃの２倍の値と水中航走体Ａの幅寸法とを足した算出値よりも大となる間隔を隔てた配置となっている場合は、その複数の障害物ｂ同士の間を、双方の各障害物ｂと離隔距離ｃを保持した状態で水中航走体Ａが通過することは可能である。しかし、検出された障害物ｂ同士の間隔が前記算出値よりも大となっていても、その差があまり大きくない場合は、障害物ｂ同士の隙間が奥の方で狭くなっていたり、検出された障害物ｂ同士が隙間の奥で繋がっていたりする可能性がある。

そこで、移動方向算出処理部８は、図５に示すように、障害物検出手段３で検出された複数の障害物ｂ同士の間隔ｋが、たとえば、離隔距離ｃの２．５倍未満など、設定されたしきい値未満の場合は、これら複数の障害物ｂを図５に二点鎖線で示すように繋がった１つの障害物ｂとみなして、まとめて回避する回避処理を行う機能を備えることが好ましい。

［障害物回避方法の第２実施形態］
図６は、障害物回避方法の第２実施形態の実施に用いる障害物回避装置を示す概要図である。図７は、障害物回避方法の第２実施形態における処理手順を示すフロー図である。図８は、図７の障害物回避方法によって定められる自律移動体の移動経路の例を示すものである。

なお、図６、図８において、第１実施形態に示したものと同一のものには、同一符号を付して、その説明を省略する。

本実施形態の障害物回避方法の実施に用いる障害物回避装置１は、図６に示すように、図１に示した障害物回避装置１と同様の構成において、演算装置５の移動方向算出処理部８が、マップ記憶部７に記憶されたマップｅに対して障害物ｂに関連する情報を追加する機能を備えた構成を有している。このマップｅに対し移動方向算出処理部８が追加する情報については後述する。

更に、移動方向算出処理部８は、図７にフローを示すルールに従って、水中航走体Ａの移動方向を算出する機能を備えている。

次に、図７に示したルールについて説明すると共に、このルールに基づく水中航走体Ａの移動について説明する。

図７において、ステップＳ１〜ステップＳ６までの処理は、図３に示したと同様である。

本実施形態では、移動方向算出処理部８は、ステップＳ６で、障害物ｂとの間に離隔距離ｃを保持した状態で、水中航走体Ａの進行方向の右側が障害物ｂに沿う姿勢となるように、水中航走体Ａの進行方向を、障害物ｂに沿う方向に定め、その後、ステップＳａに進む。

ステップＳａでは、移動方向算出処理部８は、マップ記憶部７に記憶されたマップｅ上で、ステップＳ６において水中航走体Ａの進行方向を障害物ｂに沿う方向に定める際に基準とした障害物ｂについて、マークを付す処理を行う。このマークは、ステップＳ６において水中航走体Ａの進行方向を定める際に基準とした障害物ｂであることが分かるものであれば、任意のマークを使用してよい。

移動方向算出処理部８は、ステップＳａの後はステップＳ７に進む。

このステップＳ７は、図３に示したステップＳ７と同様である。すなわち、移動方向算出処理部８は、設定された時間間隔で、水中航走体Ａの現在位置を基準として、目標位置方向ｇの判定領域内に障害物ｂがあるか否かを判断する。

移動方向算出処理部８は、ステップＳ７において判定領域内に障害物ｂが存在すると判断された場合は、ステップＳ６に戻る。

これにより、水中航走体Ａの現在位置から目標位置方向ｇの判定領域内に障害物ｂが存在している間は、移動方向算出処理部８では、ステップＳ６、ステップＳａ、ステップＳ７の処理ループが繰り返し実施される。

したがって、水中航走体Ａは、現在位置から目標位置方向ｇの判定領域内に障害物ｂが存在している間は、障害物ｂとの間に離隔距離ｃを保持しながら障害物ｂに沿う移動を継続して行うようになる。この間、水中航走体Ａの進行方向を定める際に基準とした障害物ｂには、マップｅ上で順次マークが付される。

一方、ステップＳ７で、水中航走体Ａの現在位置から目標位置方向ｇの判定領域内に障害物ｂが存在しないと判断される場合は、移動方向算出処理部８は、ステップＳｂに進む。

ステップＳｂでは、移動方向算出処理部８は、目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在しているか否かを判断する。

ここで、ステップＳｂにて目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在していると判断される状況について説明する。

たとえば、図８に示すような配置で障害物ｂ２，ｂ３，ｂ４，ｂ５が存在している領域に水中航走体Ａが進入した場合について考える。障害物ｂ２と障害物ｂ３との間隔、障害物ｂ３と障害物ｂ４との間隔、障害物ｂ４と障害物ｂ５との間隔は、いずれも水中航走体Ａの通過が可能となる幅よりも狭いものとする。

水中航走体Ａは、位置ｘ５では、目標位置方向ｇを進行方向として移動している。

水中航走体Ａは、位置ｘ６に達すると、移動方向算出処理部８で図７のステップＳ４による障害物回避処理が開始される。

図７のステップＳ４に続くステップＳ５〜ステップＳ７のうち、ステップＳ５、ステップＳ６、ステップＳ７の処理内容は、第１実施形態の場合における図３のステップＳ５、ステップＳ６、ステップＳ７と同様である。

したがって、水中航走体Ａは、図８の位置ｘ６で向きを変更した後、位置ｘ７から位置ｘ８まで、障害物ｂ２との間に離隔距離ｃを保持しつつ、進行方向の右側を障害物ｂ２に沿わせた姿勢で、障害物ｂ２に沿う移動を行うようになる。

次に、水中航走体Ａが位置ｘ８に達すると、水中航走体Ａの現在位置から目標位置方向ｇの判定領域内に障害物ｂ２は存在しなくなるが、判定領域内に障害物ｂ３が存在するようになる。

このため、移動方向算出処理部８では、図７のステップＳ５〜ステップＳ７の処理ループが継続される。したがって、水中航走体Ａは、図８の位置ｘ８から位置ｘ９まで、障害物ｂ３との間に離隔距離ｃを保持しつつ、進行方向の右側を障害物ｂ３に沿わせた姿勢で、障害物ｂ３に沿う移動を行うようになる。

次いで、水中航走体Ａが図８の位置ｘ９に達すると、水中航走体Ａは障害物ｂ３と障害物ｂ４との間を通過できないために、移動方向算出処理部８では、障害物ｂ４が障害物ｂ３と一体のものと見なされて、図７のステップＳ５〜ステップＳ７の処理ループが継続される。したがって、水中航走体Ａは、図８の位置ｘ９からは、障害物ｂ４との間に離隔距離ｃを保持しつつ、進行方向の右側を障害物ｂ４に沿わせた姿勢で、障害物ｂ４に沿う移動を開始するようになる。

図８に示すように、水中航走体Ａが障害物ｂ４に沿って移動する方向が、位置ｘ５における当初の進行方向からほぼ反転した方向になっていると、障害物ｂ４に沿って水中航走体Ａがある距離移動して図８の位置ｘ１０に到達した時点で、現在位置である位置ｘ１０を基準とする目標位置方向ｇの判定領域内には、障害物が存在しない状態になる。

しかし、この状態のときに水中航走体Ａの進行方向をその時点での目標位置方向ｇに設定して水中航走体Ａを移動させると、その移動方向の先には、以前回避した障害物ｂ３が存在しているため、障害物ｂ３と障害物ｂ４を対象とする回避処理が繰り返されるようになる。

そこで、本実施形態では、移動方向算出処理部８は、ステップＳａにおいて、水中航走体Ａの移動方向を障害物に沿う方向に定めるために基準とした障害物ｂ２，ｂ３，ｂ４に対し、マップｅ上で順次マークを付す。水中航走体Ａが図８の位置ｘ１０に達するまでには、図８にハッチングで示すように障害物ｂ２，ｂ３，ｂ４にマークが付される。

よって、水中航走体Ａが図８の位置ｘ１０を移動している場合と同様に、水中航走体Ａの現在位置から目標位置方向ｇの判定領域内には障害物ｂが存在していないが、目標位置方向ｇの先には水中航走体Ａの進行方向を定める際に以前基準とした障害物ｂ３が存在しているという状況が、ステップＳｂにて目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在していると判断される状況になる。

この点に鑑みて、移動方向算出処理部８は、図７のステップＳｂで、目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在していると判断される場合は、ステップＳ６に戻り、その時点までの処理を継続する。

これにより、水中航走体Ａは、図８の位置ｘ１０以降は、それまで水中航走体Ａの進行方向を定めるために基準とされていた障害物ｂ４との間に離隔距離ｃを保持しつつ、水中航走体Ａの進行方向の右側が障害物ｂ４に沿う姿勢で、水中航走体Ａの進行方向が障害物ｂ４に沿う方向に定められて移動する。

更に、水中航走体Ａは、障害物ｂ４と一体とみなされる障害物ｂ５についても、障害物ｂ５との間に離隔距離ｃを保持した状態で、水中航走体Ａの進行方向の右側が障害物ｂ５に沿う姿勢となるように、水中航走体Ａの進行方向が障害物ｂ５に沿う方向に定められて移動するようになる。

一方、図７のステップＳｂで、目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在していないと判断される場合は、水中航走体Ａの現在位置から目標位置方向ｇの判定領域に障害物ｂが存在せず、更に判定領域の先の目標位置方向ｇには、以前水中航走体Ａの進行方向を定めるために基準とした障害物ｂが存在していない状況である。よって、この場合は、水中航走体Ａが目標位置方向ｇを進行方向に定めて移動しても、以前水中航走体Ａの進行方向を定めるために基準とした障害物ｂに再び到達する虞はない。

よって、図７に示すように、移動方向算出処理部８は、ステップＳｂにて、目標位置方向ｇにマップｅ上でマークが付された障害物ｂが存在していないと判断される場合は、ステップＳ８に進み、障害物回避処理を終了してから、ステップＳ１に戻る処理を行う。

この場合の処理は第１実施形態と同様であるため、説明は省略する。

このように、本実施形態の障害物回避方法および装置によれば、第１実施形態と同様の効果に加えて、水中航走体Ａが一旦回避した障害物ｂに向けて再び移動することを防止することができる。よって、図８に示した袋小路のような領域に水中航走体Ａが進入しても、その領域から水中航走体Ａを脱出させて目標位置ｆまで到達させることができる。

なお、前記各実施形態では、水中航走体Ａは、進行方向の右側が障害物ｂ，ｂ１，ｂ２，ｂ３，ｂ４，ｂ５に沿う姿勢となるように、水中航走体Ａの進行方向が障害物ｂ，ｂ１，ｂ２，ｂ３，ｂ４，ｂ５に沿う方向に定められる場合について説明したが、進行方向の左側が障害物に沿う姿勢となるように、水中航走体Ａの進行方向が障害物に沿う方向に定められてもよいことは勿論である。

また、複数の障害物の回避を順次行う場合に、すべての障害物に対して、水中航走体Ａが進行方向の左右両側のうちの常に同じ側が沿う姿勢をとってもよいし、とらなくてもよい。

水中航走体Ａが障害物ｂを回避するときの方向を左右方向のいずれかに定めるときに、水中航走体Ａが存在している環境における潮流の影響を考慮するようにしてもよい。

自律移動体としては、水中航走体Ａを例示したが、水中航走体Ａ以外の水面航走体や、陸上を移動する移動体を自律移動体として、本発明を適用してもよい。

水面航走体や陸上を移動する移動体の場合は、自己位置検出手段２として、慣性航法装置のほかに、ＧＰＳなどの航法衛星システムを用いるようにすればよく、障害物検出手段３として、ソーナーの他に、カメラと画像処理装置とを備えた障害物の検出システムや、レーダーなどを用いるようにすればよい。

その他本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。

２ 自己位置検出手段（手段）、３ 障害物検出手段（手段）、５ 演算装置、Ａ 水中航走体（自律移動体）、ｂ，ｂ１〜ｂ５ 障害物、ｄ 領域、ｅ マップ、ｆ 目標位置、ｇ 目標位置方向

Claims (3)

1. 自律移動体の現在位置から目標位置に向かう目標位置方向を求める処理と、
   前記自律移動体の進行方向を前記目標位置方向に定める処理と、
   前記自律移動体の現在位置から前記進行方向の設定された領域内に回避対象となる障害物が存在する場合に、前記自律移動体の左右いずれか一側が前記障害物に沿う姿勢となるように前記自律移動体の進行方向を該障害物に沿う方向に定める処理と、
   自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合は、前記自律移動体の進行方向を目標位置方向に設定する処理とを行うこと
   を特徴とする障害物回避方法。
2. 前記自律移動体の回避対象となる障害物をマップに記録する処理と、
   前記マップ上で、前記自律移動体の進行方向を障害物に沿う方向に定める際に基準とした前記障害物についてマークを付する処理と、
   且つ、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合に、前記自律移動体の進行方向を目標位置方向に設定する処理に代えて、自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合であって、該自律移動体の現在位置から前記目標位置方向に前記マップ上で前記マークが付された障害物が存在するときには、前記自律移動体の左右いずれか一側が前記障害物に沿う姿勢となるように前記自律移動体の進行方向を該障害物に沿う方向に定めた状態を継続する処理と、
   自律移動体の現在位置から目標位置方向の設定された判定領域内に障害物が存在しない場合であって、該自律移動体の現在位置から前記目標位置方向に前記マップ上で前記マークが付された障害物が存在しないときには、前記自律移動体の進行方向を目標位置方向に設定する処理とを行う
   請求項１記載の障害物回避方法。
3. 自律移動体の自身の位置を検出する手段と、
   前記自律移動体の回避対象となる障害物を検出する手段と、
   演算装置とを備え、
   前記演算装置は、請求項１又は請求項２記載の障害物回避方法を実施する機能を備えること
   を特徴とする障害物回避装置。

Images