(57)【要約】【課題】事前に障害物の位置についての情報を得る必要がない劣駆動型自律ロボットの制御方法を提供する。【解決手段】原点を目標到達位置とし障害物の位置を考慮せずに、価値関数を生成する。新たな障害物を検出するごとにその障害物の位置を求める。障害物にぶつからずに目標到達位置に到達するまでの、各時刻ステップにおける目標軌道位置を生成する。水中ロボットが各行動を取ったときに、上記目標軌道位置にどの程度近づくことができるのかを数値で評価し、その評価値により各行動に優先順位を付ける。水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算する。優先順位の高い行動の順番で、その行動を取ったときの障害物へのぶつかりやすさが一定の閾値よりも小さいかどうかを判定し、小さいと判定された場合にはその行動を選択する処理を、行動が選択されるまで繰り返す。上記選択された行動に従うように、水中ロボットを制御する。【選択図】図１
2008052473.tif 000002
【特許請求の範囲】
【請求項１】
原点を目標到達位置とし、水中ロボットが原点に到達するときの報酬＞その他の場合の報酬として、マルコフ状態遷移モデルにおける動作計画法に基づいて生成された価値関数が価値関数保存手段に保存されており、
環境モデル生成手段が、新たな障害物を検出するごとに、その障害物の位置を求めて、障害物情報保存手段に格納する環境モデル生成ステップと、
軌道生成手段が、上記障害物情報保存手段から読み出した障害物にぶつからずに目標到達位置に到達するまでの、各時刻ステップにおける目標軌道位置を生成して、目標軌道位置保存手段に格納する軌道生成ステップと、
軌道追従計算手段が、水中ロボットが各行動を取ったときに、上記目標軌道位置保存手段から読み出した目標軌道位置にどの程度近づくことができるのかを数値で評価し、その評価値により、各行動に優先順位を付ける軌道追従計算ステップと、
障害物回避計算手段が、水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算する障害物回避計算ステップと、
行動選択手段が、軌道追従計算ステップで付けられた優先順位が高い行動の順番で、上記障害物回避計算ステップで求められた、その行動を取ったときの障害物へのぶつかりやすさが一定の閾値よりも小さいかどうかを順次判定し、小さいと判定された場合にはその行動を選択する処理を、小さいと判定される行動が見つかるまで繰り返す行動選択ステップと、
フィードバック制御手段が、行動選択ステップで選択された行動に従って、水中ロボットが動作をするように制御するフィードバック制御ステップと、
を有する水中ロボットの動作制御方法。
【請求項２】
請求項１に記載の水中ロボットの動作制御方法において、
上記軌道生成ステップは、
初期値設定手段が、水中ロボットの現在位置が含まれる格子に水中ロボットが時刻ステップ０において存在する確率を１とし、その他の格子に存在する確率を０とする初期値設定ステップと、
存在確率計算手段が、水中ロボットが時刻ステップｔ−１において格子ｓの周りに存在する格子ｓ’に存在する確率に１／８を乗算したものを各格子ｓ’ごとに求め、この格子ｓ’ごとに求まった乗算結果を加算することによって、水中ロボットが時刻ステップｔにおいて格子ｓに存在する確率を計算する存在確率計算ステップと、
確率補正手段が、上記存在確率計算ステップで求まった、上記障害物情報保存手段から読み出した各障害物が存在している各格子についての存在確率を０とする確率補正ステップと、
制御手段が、上記存在確率計算ステップで求まった、目標到達位置を含む格子についての存在確率が０でない値になるまで、上記存在確率計算ステップと上記確率補正ステップの処理を繰り返すように制御する制御ステップと、
軌道決定手段が、上記存在確率計算ステップで求まった、目標到達位置を含む格子についての存在確率が０でない値になったときの時刻ステップをｔｄとし、時刻ステップｔｄにおける目標軌道位置τ（ｔｄ）とし、水中ロボットが時刻ステップｔ−１において目標到達位置τ（ｔ）の周りの格子に存在する確率を最も大きくする格子を目標軌道位置（τ−１）とする処理を繰り返すことによって、各時刻ステップにおける目標軌道位置を求めて目標軌道位置保存手段に格納する軌道決定ステップと、
から構成される、
ことを特徴とする水中ロボットの動作制御方法。
【請求項３】
請求項１又は２に記載の水中ロボットの動作制御方法において、
上記軌道追従計算ステップは、
相対位置決定手段が、上記目標軌道位置保存手段から読み出した各目標軌道位置に対する水中ロボットの相対位置を各目標軌道位置ごとに求める相対位置決定ステップと、
遷移先予測手段が、上記相対位置決定ステップで求まった各相対位置に位置する水中ロボットが、第一の行動を取ったときの遷移先の状態を第一の各行動ごとに求める遷移先予測ステップと、
最大値抽出手段が、価値関数保存手段を参照して、上記遷移先予測ステップで求まった遷移先の状態にある水中ロボットが、第二の各行動を取ったときに与えられる価値関数の値の最大値を求める最大値抽出ステップと、
加算手段が、上記最大値抽出ステップで求まった最大値の、上記各目標軌道位置についての和を取る加算ステップと、
順序決定手段が、上記加算ステップで求まった加算値が大きい順に、上記第一の各行動に優先順位を付ける順序決定ステップと、
から構成される、
ことを特徴とする水中ロボットの動作制御方法。
【請求項４】
請求項１又は２に記載の水中ロボットの動作制御方法において、
上記軌道追従計算ステップは、
相対位置決定手段が、上記目標軌道位置保存手段から読み出した各目標軌道位置に対する水中ロボットの相対位置を各目標軌道位置ごとに求める相対位置決定ステップと、
遷移先予測手段が、上記相対位置決定ステップで求まった各相対位置に位置する水中ロボットが、第一の行動を取ったときの遷移先の状態を第一の各行動ごとに求める遷移先予測ステップと、
平均値抽出手段が、価値関数保存手段を参照して、上記遷移先予測ステップで求まった遷移先の状態にある水中ロボットが、第二の各行動を取ったときに与えられる価値関数の値の平均値を求める平均値抽出ステップと、
加算手段が、上記平均値抽出ステップで求まった平均値の、上記各目標軌道位置についての和を取る加算ステップと、
順序決定手段が、上記加算ステップで求まった加算値が大きい順に、上記第一の各行動に優先順位を付ける順序決定ステップと、
から構成される、
ことを特徴とする水中ロボットの動作制御方法。
【請求項５】
請求項１から４の何れかに記載の水中ロボットの動作制御方法において、
上記障害物回避計算ステップは、
相対位置決定手段が、各障害物の位置に対する水中ロボットの相対位置を各障害物の位置ごとに求める相対位置決定ステップと、
遷移先予測手段が、上記相対位置決定ステップで求まった各相対位置に位置する水中ロボットが、第一の各行動を取ったときの遷移先の状態を第一の各行動ごとに求める遷移先予測ステップと、
最大値抽出手段が、価値関数保存手段を参照して、上記遷移先予測ステップで求まった遷移先の状態にある水中ロボットが、第二の各行動を取ったときに与えられる価値関数の値の最大値を求める最大値抽出ステップと、
最大値選択手段が、上記最大値抽出ステップで求まった最大値を最大にする第一の行動を各障害物の位置ごとに求める最大値選択ステップと、
リスク計算手段が、各障害物の位置を、最大値選択ステップで求まった第一の行動が同じもの同士でグループ分けし、上記各グループごとに最大値抽出ステップで求まった最大値の中で最も大きいものを選ぶことにより、水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算するリスク計算ステップと、
から構成される、
ことを特徴とする水中ロボットの動作制御方法。
【請求項６】
請求項１から４の何れかに記載の水中ロボットの動作制御方法において、
上記障害物回避計算ステップは、
相対位置決定手段が、各障害物の位置に対する水中ロボットの相対位置を各障害物の位置ごとに求める相対位置決定ステップと、
遷移先予測手段が、上記相対位置決定ステップで求まった各相対位置に位置する水中ロボットが、第一の各行動を取ったときの遷移先の状態を第一の各行動ごとに求める遷移先予測ステップと、
平均値抽出手段が、価値関数保存手段を参照して、上記遷移先予測ステップで求まった遷移先の状態にある水中ロボットが、第二の各行動を取ったときに与えられる価値関数の値の平均値を求める平均値抽出ステップと、
最大値選択手段が、上記平均値値抽出ステップで求まった平均値を最大にする第一の行動を各障害物の位置ごとに求める最大値選択ステップと、
リスク計算手段が、各障害物の位置を、最大値選択ステップで求まった第一の行動が同じもの同士でグループ分けし、上記各グループごとに最大値抽出ステップで求まった最大値の中で最も大きいものを選ぶことにより、水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算するリスク計算ステップと、
から構成される、
ことを特徴とする水中ロボットの動作制御方法。
【請求項７】
原点を目標到達位置とし、水中ロボットが原点に到達するときの報酬＞その他の場合の報酬として、マルコフ状態遷移モデルにおける動作計画法に基づいて生成された価値関数を保存する価値関数保存手段と、
新たな障害物を検出するごとに、その障害物の位置を求めて、障害物情報保存手段に格納する環境モデル生成手段と、
上記障害物情報保存手段から読み出した障害物にぶつからずに目標到達位置に到達するまでの、各時刻ステップにおける目標軌道位置を生成して、目標軌道位置保存手段に格納する軌道生成手段と、
水中ロボットが各行動を取ったときに、上記目標軌道位置保存手段から読み出した目標軌道位置にどの程度近づくことができるのかを数値で評価し、その評価値により、各行動に優先順位を付ける軌道追従計算手段と、
水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算する障害物回避計算手段と、
軌道追従計算手段で付けられた優先順位の高い行動の順番で、上記障害物回避計算手段で求められた、その行動を取ったときの障害物へのぶつかりやすさが一定の閾値よりも小さいかどうかを判定し、小さいと判定された場合にはその行動を選択する処理を、小さいと判定される行動が見つかるまで繰り返す行動選択手段と、
行動選択手段で選択された行動に従って、水中ロボットが動作をするように制御するフィードバック制御手段と、
を有する水中ロボットの動作制御装置。
【請求項８】
請求項１から６の何れかに記載の水中ロボットの動作制御方法の各ステップをコンピュータに実行させるための水中ロボットの動作制御プログラム。
【請求項９】
請求項８記載の水中ロボットの動作制御プログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、未知障害物の存在する環境において、劣駆動型自律ロボットに実時間での障害物回避の行動をさせる動作制御方法、装置、プログラム及びその記録媒体に関する。
【背景技術】
【０００２】
従来、以下に説明するマルコフ状態遷移モデルを利用した自律ロボットの動作計画法が提案されている（例えば、非特許文献１参照。）。
マルコフ状態遷移モデルを利用した自律ロボットの動作計画法を説明する前に、まず、前提知識となるマルコフ状態遷移モデル及びマルコフ状態遷移モデルを用いた動作計画法について説明する。
【０００３】
環境を以下のようにモデル化したものが、マルコフ状態遷移モデルである。環境のとりうる離散的な状態の集合をＳ＝｛ｓ １，ｓ２，…，ｓｎ｝、行動主体が取り得る行動の集合をＡ＝｛ａ１，ａ２，…ａｌ｝と表す。環境中のある状態 s ∈ Ｓにおいて、行動主体がある行動 a を 実行すると、環境は確率的に状態 s' ∈Ｓへ遷移する。その遷移確率を
Ｐａｓｓ’＝Ｐｒ｛ｓｔ＋１＝ｓ’｜ｓｔ＝ｓ，ａｔ＝ａ｝
により表す。このとき環境から行動主体へ報酬ｒが確率的に与えられるが、その期待値を
Ｒａｓｓ’＝Ｅ｛ｒt ｜ｓｔ=ｓ, ａｔ＝ａ，sｔ＋１＝s’｝
とする。行動主体の各時刻における意志決定は方策関数
π（ｓ，ａ）＝Ｐｒ｛ａｔ＝ａ｜ｓｔ＝ｓ｝
によって表される。π（ｓ，ａ）は、全状態ｓ，全行動ａにおいて定義される。方策関数π（ｓ，ａ）は、単に方策π とも呼ばれる。
【０００４】
ある時間ステップｔで選択した行動が、その後の報酬獲得にどの程度貢献したのかを評価するため、その後得られる報酬の時系列を考える。報酬の時系列評価は価値Ｑと呼ばれる。行動主体の目標は、価値Ｑを最大化することである。価値は、時間の経過とともに報酬を割引率γ（０≦γ＜１）で割引いて合計される。
すべての状態ｓ、行動ａ、遷移先の状態ｓ’の組み合わせについてのＰａｓｓ’とＲａｓｓ’の値が定まっていれば、ダイナミックプログラミング（Dynamic  Programminｇ）法により、価値関数Ｑ（ｓ，ａ）及び方策πを計算することができる（例えば、三上  貞芳、皆川  雅章  共訳、R.S.Sutton、A.G.Barto  原著「強化学習」森北出版、1998、pp.94-118参照）。
従来例による動作計画法の行動主体は、図１７に例示される水中ロボット１である。図１７は、上から見た水中ロボット１の概念図である。水中ロボット１は、舵８、主推進器３、海流差計測部６、位置計測部７を有する。この水中ロボット１は、真横方向に直接移動することができない。すなわち、搭載アクチュエータである舵８、主推進器３が制御可能な運動自由度よりも、水中ロボットの運動自由度が高いので、この水中ロボット１は劣駆動ロボットである。
【０００５】
以下、図１３を参照して、従来例による水中ロボットの動作制御方法を実現するための装置の機能構成例及びその処理例について説明する。図１３は、従来例による水中ロボットの動作制御装置の構成例を示す図である。
水中ロボットの動作計画装置１００は、最大加速度設定部１１２、目標速度計算部１０２、想定海流速度値入力部１０３、変位量計算部１０４、状態遷移確率計算部１０５、地形モデル保存部１０６、傾斜角差計算部１０７、登坂角度計算部１０８、報酬決定部１０９、動作計画部１１０、価値関数保存部１１１、海流差計測部６、位置計測部７、遷移先予測部２１３、価値関数値計算部２１４、動作選択部２１５、フィードバック制御部２１６から構成される。
目標速度計算部１０２が、マルコフ状態遷移モデルの各状態ｓにおいてある行動ａを取ったときの目標速度を決定する。この実施例では、水中ロボット目標速度は、対水機体座標に基づいて、かつ、飛行船の最大加速度を考慮して設定される。また、目標速度は、例えば、劣駆動水中ロボットの旋回速度ψ’τ（ｔ）と飛行船の機軸方向の速度ｖｘｗτ（ｔ）（以下、サージ速度とする）である。
【０００６】
水中ロボット１が航行する空間はマルコフ遷移状態モデルにより離散的にモデル化されており、その状態ｓは、水中ロボット１の水平面内位置のＸ座標，Ｙ座標，方位角ψ，旋回速度ψ’の４つのパラメータ・軸から構成される。各軸は、水中ロボットに搭載可能なセンサーの精度を考慮して離散化されている。例えば、状態ｓを構成する格子の一辺の長さが５〜１０ｍになるように、Ｘ軸方向，Ｙ軸方向に離散化される。また、格子の一辺の長さが、水中ロボットの大きさとほぼ同じになるように、Ｘ軸方向，Ｙ軸方向に離散化しても良い。方位角Ψは、５〜１０度ごとに離散化される。旋回速度Ψ’は、１度／秒ごとに離散化される。
例えば、目標速度計算部１０２は、最大加速度設定部１１２において予め設定された水中ロボット１の旋回加速度αを読み出し、各行動aについて、下記の式に従って水中ロボット１の旋回速度ψ’τ（ｔ）とサージ速度ｖｘｗτ（ｔ）を水中ロボット１の目標速度として定め、変位量計算部１０４に出力する。
【０００７】
【数１】
2008052473.tif 000003
ここで、ψ’τ（ｔ）は水中ロボット１の目標旋回速度、ａはマルコフ状態遷移モデルの各状態ｓにおける行動、αは水中ロボット１の旋回加速度、ｔは各行動aの開始時からの経過時間、ψ’τ０は各状態ｓの開始時における水中ロボットの旋回速度、ｖｘｗτ（ｔ）は水中ロボット１の目標対水機体速度（以下、目標サージ速度とする）、ｖｘ０は正の一定値とした場合である。ここで、αは、水中ロボットの最大加速度を超えないように設定される。また、この実施例では、計算量を少なくするためにｖｘ０を正の一定値として扱ったが、より精度の高い動作計画を行うためにｖｘ０の値を行動として選択することができるようにしても良い。
この実施例では、説明の簡略化のために、行動ａが−１，０，１の何れかの値を取るとし、行動ａが１の場合には、上記式（２）に従い、水中ロボットの旋回速度ψ’は一定加速度αで加速され、行動ａが０の場合には、水中ロボットの旋回速度ψ’は維持され、行動ａが−１の場合には、水中ロボットの旋回速度ψ’は一定加速度αで減速されるものとする。
【０００８】
例えば、時刻０の時の状態において行動として１を選択し、時刻Ｔの状態において行動として０を選択し、時刻２Ｔの時の状態において行動１、時刻３Ｔの時の状態において行動１、時刻４Ｔの時の状態において行動０、時刻５Ｔの時の状態において行動−１、時刻６Ｔの時の状態において行動−１、時刻７Ｔの時の状態において１を選択した場合には、水中ロボットの旋回速度ψ’は、図１２に示すように変化する。
なお、この行動ａの例は一例であることに注意する。すなわち、ａは、−１０，０，１０の値を取っても良いし、−１，―０．５，０，０．５，１の５つの値等を取っても良い。さらには、各行動時間中に水中ロボットの速度・加速度が変わるような行動を選択することができるようにしても良い。
このように、目標軌道や対地世界座標に基づく目標速度ではなく、対水機体座標に基づく目標速度の形で、目標となる行動を水中ロボット１に与えることにより、未知海流の外乱等により水中ロボットの水平面内の位置がずれた場合であっても、動作計画が破綻しないというメリットがある。すなわち、もし、目標軌道や対地世界座標で目標速度を設定した場合、任意環境において未知の強い海流の外乱があったときに、水中ロボットのアクチュエータ力では海流に対抗しきれず、目標軌道又は対地世界座標で表現された目標速度を大きく外れて航行してしまう可能性があった。本発明においては、上記のように、対水機体座標で目標速度を記述し、それを追従することで、海流による追従制御への影響を最小限とすることができる。また、これにより、海流外乱による影響を水中ロボット１の位置変化の誤差にのみ現れるようにすることができるのである。
【０００９】
また、水中ロボットは、その運動のイナーシャが高く、運動が海流による影響を受けやすいという性質があり、舵の切り角に対する旋回速度のステップ応答は遅く、舵を切って即、旋回速度が希望の値になることが望めないことがある。しかし、上記の式（２）により、水中ロボット１の加速度制限値を考慮した目標速度の設定をすることができるため、追従可能な動作計画を行うことができるのである。
変位量計算部１０４は、状態ｓにある水中ロボット１が、想定される海流下ｆｘ、ｆｙにおいて、各行動ａに従って航行した場合、水中ロボットの世界座標系における水平面内位置のＸ座標，Ｙ座標，方位角ψ，旋回速度ψ’がそれぞれどれくらい変位するのかを計算する。これらの変位を求めることにより、後述するように、状態ｓにいる水中ロボットが、行動ａを選択した場合に、次に、どの状態ｓ’にどのくらいの確率で遷移するのかを計算することができるのである。
水中ロボット１の水平面内位置のＸ座標の変位量をＤＸ（ψ０，ａ）、Ｙ座標の変位量をＤＹ（ψ０，ａ）、方位角ψの変位量をＤψ（ψ０，ａ）、旋回速度ψ’の変位量をＤψ’（ψ０，ａ）とすると、それぞれの変位量は、次式にように与えられる（図１４を参照のこと）。
【００１０】
【数２】
2008052473.tif 000004
ここで、ψ０は各状態ｓの開始時の方位角、Ｔは状態ｓから次の状態ｓ’に遷移するまでの時間（以下、行動単位時間とする）、ｆｘは想定される海流の速度の世界座標系におけるＸ軸方向の成分、ｆｙは想定される海流の速度の世界座標系におけるＹ軸方向の成分である。なお、方位角ψの変位量Ｄψ（ψ０，ａ）と、旋回速度ψ’の変位量Ｄψ’（ψ０，ａ）については、旋回速度ψ’の制御を行うことになるため、風の影響による補正は行わない。行動単位時間は例えば１５秒とすることができる。
変位量計算部１０４は、目標速度計算部が出力した各時刻ｔにおける水中ロボットの旋回速度ψ’τとサージ速度ｖｘｗτ、想定海流速度値入力部で入力されたｆｘとｆｙを用いて、水中ロボットの水平面内位置のＸ座標の変位量ＤＸ（ψ０，ａ）、Ｙ座標の変位量ＤＹ（ψ０，ａ）、方位角ψの変位量Ｄψ（ψ０，ａ）、旋回速度ψ’の変位量Ｄψ’（ψ０，ａ）を上記式に基づいてそれぞれ計算して、その計算結果を状態遷移確率計算部１０５に出力する。
【００１１】
状態遷移確率計算部１０５は、水中ロボット１の水平面内位置のＸ座標の変位量ＤＸ（ψ０，ａ）、Ｙ座標の変位量ＤＹ（ψ０，ａ）、方位角ψの変位量Ｄψ（ψ０，ａ）、旋回速度ψ’の変位量Ｄψ’（ψ０，ａ）に基づいて、状態遷移確率Ｐａｓｓ’を計算する。すなわち、これらの変位量を用いて、すべての状態ｓと状態ｓ’と行動ａの組み合わせについて、状態ｓにいる水中ロボットが行動ａを選択した場合に、状態ｓ’へ遷移する確率を計算する。
まず、状態ｓが、水中ロボット１の水平面内位置のＸ座標，Ｙ座標，方位角ψ，旋回速度ψ’の４つの軸から構成される４次元の格子で示されるとし、その格子をＲ（ｓ）と定義する（図１５を参照のこと）。そして、その格子Ｒ（ｓ）を、上記各変位量から構成される変位量ベクトル（ＤＸ（ψ０，ａ），ＤＹ（ψ０，ａ），Ｄψ（ψ０，ａ），Ｄψ’（ψ０，ａ））で、平行移動したものをＲｔ（ｓ）と定義する。
ここで、水中ロボット１が状態ｓにあるときは、水中ロボット１は、その状態ｓを表わす４次元の格子Ｒ（ｓ）の各点の何れかに、等しい確率で存在するものと仮定する。この仮定の下では、状態遷移確率Ｐａｓｓ’は、Ｒｔ（ｓ）と各Ｒ（ｓ’）の重なった部分の体積に比例してそれぞれ求めることができる。ここで、Ｒ（ｓ’）は、Ｒｔ（ｓ）と重なった格子である。すなわち、Ｒ（ｓ’）は、状態ｓにおいてある行動ａを取ったときの遷移先の候補の状態ｓ’に対応した４次元の格子である。Ｒｔ（ｓ）は最大で８つのＲ（ｓ’）と重なる可能性がある。
【００１２】
状態遷移確率Ｐａｓｓ’は、Ｒｔ（ｓ）とあるＲ（ｓ’）の重なった部分の体積をＶ０（ｓ，ｓ’，ａ）、Ｒｔ（ｓ）とすべてのＲ（ｓ’）との重なった部分の体積をΣｓ’Ｖ０（ｓ，ｓ’，ａ）とすると、次式により求めることができる。
【００１３】
【数３】
2008052473.tif 000005
このように状態遷移確率Ｐａｓｓ’を計算することにより、最大８つの遷移先の状態ｓ’についてのみ０でない状態遷移確率Ｐａｓｓ’を与え、他のすべての状態ｓ’については０の状態遷移確率Ｐａｓｓ’を与えることになるため、後述するダイナミックプログラミング（ＤＰ）法による計算量を削減することができる。
傾斜角差計算部１０７が、地形モデル保存部１０６に保存された地形モデルを参照して、状態ｓにおける地形の傾斜角θｓｔｅｅｐ（ｓ）と、その状態においてある行動ａを取ったときの遷移先の状態ｓ’における地形の傾斜角θｓｔｅｅｐ（ｓ’）の差の絶対値ｄθｓｔｅｅｐ（ｓ’，ｓ）を求める。計算されたｄθｓｔｅｅｐ（ｓ’，ｓ）は、報酬決定部１０９に出力される。
すなわち、状態間の傾斜角差ｄθｓｔｅｅｐ（ｓ’，ｓ）は下記の式により定義される（図１６を参照のこと）。
【００１４】
【数４】
2008052473.tif 000006
地形モデルは、位置（Ｘ，Ｙ）と方位角のすべての組み合わせについての傾斜角のデータと、障害物の位置が登録されているデータベースである。水中ロボットの動作計画を行う前に、実際に水中ロボットを航行させる予定の地形についての地形モデルを予め取得し、地形モデル保存部１０６に格納しておく必要がある。
登坂角度計算部１０８が、ある状態ｓから別の状態ｓ’に遷移するときの水中ロボットの最大登坂角度ｄθｍａｘ（ｓ，ｓ’）を計算する。計算された水中ロボットの最大登坂角度ｄθｍａｘ（ｓ，ｓ’）は、報酬決定部１０９に出力される。
ｖｚ（ｓ）を状態ｓにおけるピッチ角の変化速度、ａｈを水中ロボットのピッチ角変化の加速度の最大値、ｆｘｂを海流の機軸方向の対機体速度とし、登坂角度は限りなく小さく、上下方向には海流がないものとすると、水中ロボットの最大登坂角度ｄθｍａｘ（ｓ，ｓ’）は、以下のように定義される。
【００１５】
【数５】
2008052473.tif 000007
報酬決定部１０９が、状態ｓ、行動ａ、遷移先の状態ｓ’の各組み合わせについて、それぞれ、水中ロボットの最大登坂角度ｄθｍａｘ（ｓ，ｓ’）と状態間の傾斜角差ｄθｓｔｅｅｐ（ｓ’，ｓ）の大小関係を調べて、報酬を決定する。
具体的には、報酬決定部１０９は、状態間の傾斜角差ｄθｓｔｅｅｐ（ｓ’，ｓ）の方が大きい場合には、その状態ｓ、行動ａ、遷移先の状態ｓ’各組み合わせについての報酬Ｒａｓｓ’を−１に設定する。状態間の傾斜角差ｄθｓｔｅｅｐ（ｓ’，ｓ）の方が大きい場合には、水中ロボット１が、その遷移先の状態ｓ’に移行することは不可能であり、かかる遷移先の状態ｓは障害物とみなすことができるためである。
また、報酬決定部１０９は、遷移先の状態ｓ’が到達点を含む場合には報酬Ｒａｓｓ’を１に設定し、その他の場合には報酬Ｒａｓｓ’を０に設定する。
なお、報酬決定部１０９は、ｄθｍａｘ（ｓ，ｓ’）と状態間の傾斜角差ｄθｓｔｅｅｐ（ｓ’，ｓ）の比較をしないで、報酬を決定しても良い。具体的には、報酬決定部１０９は、地形モデル保存部１０６に保存された地形データを参照して、遷移先の状態ｓ’が到達点を含む場合には報酬Ｒａｓｓ’を１に設定し、遷移先の状態ｓ’が障害物を含む場合には報酬Ｒａｓｓ’を−１に設定し、その他の場合には報酬Ｒａｓｓ’を０に設定する。
【００１６】
また、先に述べたように、この｛１，０，−１｝の何れかの報酬を与えるという決定方法は一例に過ぎず、到達点を含む場合の報酬＞その他の場合の報酬＞障害物である場合の報酬という関係が成り立っている限り、報酬の値はどのような値でも良い。
動作計画部１１０は、状態遷移確率計算部１０５が計算した状態遷移確率Ｐａｓｓ’と、報酬決定部１０９が計算した報酬Ｒａｓｓ’を用いて、ダイナミックプログラミング法により、価値関数Ｑ（ｓ，ａ）を計算し、これを価値関数保存部１１１に格納する。
先に述べたように、すべての状態ｓ、行動ａ、遷移先の状態ｓ’の組み合わせについてのＰａｓｓ’とＲａｓｓ’の値がそれぞれ定まっていれば、ダイナミックプログラミング（Ｄｙｎａｍｉｃ  Ｐｒｏｇｒａｍｍｉｎｇ）法により、価値関数Ｑ（ｓ，ａ）を計算することができる。
Ｑ（ｓ，ａ）は、各状態ｓにおいて、行動ａを選択した結果、将来水中ロボットが受ける報酬の推定量を示すものであり、各状態ｓにおいて、Ｑ（ｓ，ａ）の値を最大化する行動ａを選択することが最適な行動方策となる。
【００１７】
以上に説明したＱ（ｓ，ａ）の値を利用して、実際の任務行動における自律水中ロボットの制御が行われる。すなわち、行動単位時間Ｔおきに、水中ロボットは、Ｑ（ｓ，ａ）の値を最大化する行動ａを選択する。ただし、この手法は、任務環境における海流速度ｆｘ、ｆｙが、動作計画時に想定していた場合と等しいときのみに利用することができる手法である。任意環境における海流速度が想定していた値と異なる場合には、以下に示す行動選択手法を利用する。
海流差計測部６が、各行動単位時間毎に、動作計画時に予測した海流の速度ｆｘ，ｆｙと、実際の海流の速度の実測値ｆｘａ，ｆｙａの差ｄｆｘ，ｄｆｙを計測する。ここで、ｄｆｘ＝ｆｘ−ｆｘａ，ｄｆｙ＝ｆｙ−ｆｙａである。計測された海流の速度差ｄｆｘ，ｄｆｙは、遷移先予測部２１３に出力される。
位置計測部７が、各行動単位時間毎に、水中ロボットの位置Ｘ，Ｙ、方位角ψ、旋回速度ψ’を計測する。計測結果は、遷移先予測部２１３に出力される。
遷移先予測部２１３が、海流差計測部６が出力した海流の速度差ｄｆｘ，ｄｆｙと、位置計測部７が出力した水中ロボットの位置Ｘ，Ｙ、方位角ψ、旋回速度ψ’を用いて、状態ｓにいる水中ロボットが、各行動ａを取った場合に、次にどの状態に遷移するのかを予測し、その予測される遷移先の状態ｓｅをそれぞれ求める。各行動ａごとに求められた予想される遷移先の状態ｓｅは、価値関数値計算部２１４に出力される。
具体的には、遷移先予測部２１３はまず、ある行動ａについて、
【００１８】
【数６】
2008052473.tif 000008
上記式（３）を用いて、海流の速度差ｄｆｘを考慮したときの水中ロボットのＸ軸方向の変位量ＤＸａ（ψ０，ａ）、海流の速度差ｄｆｙを考慮したときの水中ロボットのＹ軸方向の変位量ＤＹａ（ψ０，ａ）をそれぞれ求める。その後、
【数７】
2008052473.tif 000009
上記式（４）を用いて、その行動ａを取ったときの予想遷移先状態ｓｅを求める。ここで、Ｘｅ（ｓ，ａ），Ｙｅ（ｓ，ａ）、ψｅ（ｓ，ａ）、ψ’ｅ（ｓ，ａ）は、状態ｓにある水中ロボットがある行動ａを取ったときの予想される遷移先の状態ｓｅの位置、方位角、旋回速度である。これを、すべての行動ａについて行い、各行動ａごとにそれぞれ予想される遷移先の状態ｓｅを求める。各行動ａごとに求められた予想される遷移先の状態ｓｅは、価値関数値計算部２１４に出力される。
【００１９】
価値関数値計算部２１４が、価値関数保存部１１１を参照して、遷移後の状態ｓｅにおける行動ａ’を変化させたときのＱ（ｓｅ，ａ’）の最大値Ｑｍａｘ（ｓｅ（ａ））を、遷移前の状態ｓにおける各行動ａごとに求める。Ｑｍａｘ（ｓｅ（ａ））は、動作選択部２１５に出力される。
動作選択部２１５が、価値関数値計算部２１４が計算したＱｍａｘ（ｓｅ（ａ））の大小関係を比較して、Ｑｍａｘ（ｓｅ（ａ））を最大にする遷移前状態ｓでの行動ａを選択する。そして、その行動により決定される目標速度を式（２）から再計算する。再計算された目標速度は、フィードバック制御部２１６に出力される。
【００２０】
フィードバック制御部２１６は、再計算された目標速度を維持することができるように、主推進器力、舵角度を調整する。
以上が、従来例による自律ロボットの動作制御方法の説明である（例えば、非特許文献５参照。）。
【非特許文献１】T. Yamasaki and N. Goto:“identification of Blimp Dynamics by Flight Tests”,Transactions of JSASS,Vol.43,pp.195-205,2003.
【非特許文献２】T. Yamasaki and N. Goto:“identification of Blimp Dynamics by Flight Tests”,Transactions of JSASS,Vol.43,pp.195-205,2003.
【非特許文献３】中村仁彦「非ホロノミックロボットシステム  第２回  幾何学的な非ホロノミック拘束の下での運動計画」日本ロボット学会誌 Vol.11 No.5,pp.655〜662,1993
【非特許文献４】川野洋「未知不均一潮流中での航行を考慮した劣駆動水中ロボットの動作計画と制御」,JSAI2005,人工知能学会（第１９回）,1D1-04,2005年
【非特許文献５】川野洋「未知外乱中を航行する自律飛行船の三次元障害物回避」、日本機械学会ロボティクスメカトロニクス講演会２００６講演予稿集、2006年5月26日
【発明の開示】
【発明が解決しようとする課題】
【００２１】
従来技術では、自律ロボットが航行する環境についての障害物に関する情報を事前に知っておく必要があり、その情報が得られていない場合には、動作計画及びその動作計画を用いた動作制御を行うことができないという問題があった。また、マルコフ状態遷移モデルを利用した動作計画は時間がかかるため、自律ロボットが航行中に、障害物の情報を取得して、マルコフ状態遷移モデルを利用した動作計画及びその動作計画を用いた動作制御を実時間で行うことができないという問題があった。
【課題を解決するための手段】
【００２２】
本発明によれば、原点を目標到達位置とし、水中ロボットが原点に到達するときの報酬＞その他の場合の報酬として、マルコフ状態遷移モデルにおける動作計画法に基づいて生成された価値関数が価値関数保存手段に保存されており、環境モデル生成手段が、新たな障害物を検出するごとに、その障害物の位置を求めて、障害物情報保存手段に格納する。軌道生成手段が、上記障害物情報保存手段から読み出した障害物にぶつからずに目標到達位置に到達するまでの、各時刻ステップにおける目標軌道位置を生成して、目標軌道位置保存手段に格納する。軌道追従計算手段が、水中ロボットが各行動を取ったときに、上記目標軌道位置保存手段から読み出した目標軌道位置にどの程度近づくことができるのかを数値で評価し、その評価値により、各行動に優先順位を付ける。障害物回避計算手段が、水中ロボットが各行動を取ったときの障害物へのぶつかりやすさを計算する。行動選択手段が、軌道追従計算ステップで付けられた優先順位の高い行動の順番で、上記障害物回避計算ステップで求められた、その行動を取ったときの障害物へのぶつかりやすさが一定の閾値よりも小さいかどうかを順次判定し、小さいと判定された場合にはその行動を選択する処理を、小さいと判定される行動が見つかるまで繰り返す。フィードバック制御手段が、行動選択ステップで選択された行動に従って、水中ロボットが動作をするように制御する。
【発明の効果】
【００２３】
未知の障害物が存在する環境においても、自律ロボットが障害物に衝突せずに目標位置に到達することができるように制御することができる。
【発明を実施するための最良の形態】
【００２４】
本発明による制御の対象となる水中ロボット１’を、図１１に例示する。水中ロボット１’は、例えば、超音波測距センサ２、主推進器３、海流差計測部６、位置計測部７、舵８、動作制御部１０００を有する。
動作制御部１０００は、例えば、図１に示すように、環境モデル生成部１０、軌道生成部２０、実時間制御部９０、フィードバック制御部６０、価値関数保存部７０、ダイナミックプログラミング部８０から構成される。
ダイナミックプログラミング部８０は、例えば、図１８に示すように、最大加速度設定部１１２、目標速度計算部１０２、想定海流速度値入力部１０３、変位量計算部１０４、状態遷移確率計算部１０５、報酬決定部１０９’、動作計画部１１０から構成される。
軌道生成部２０は、例えば、図４に示すように、目標軌道位置保存部２０１、初期値設定部２０２、存在確率計算部２０３、確率補正部２０４、存在確率記憶部２０５、制御部２０６、軌道決定部２０７から構成される。
軌道追従計算部３０は、例えば、図８に示すように、相対位置決定部３０１、遷移先予測部２１３、最大値抽出部３０３、加算部３０４、順序決定部３０５、平均値抽出部３０６から構成される。
障害物回避計算部４０は、例えば、図９に示すように、相対位置決定部４０１、遷移先予測部２１３、最大値抽出部４０３、最大値選択部４０４、リスク計算部４０５、平均値抽出部４０６から構成される。
【００２５】
＜ステップＳ０＞
ダイナミックプログラミング部８０は、原点（０，０）を目標到達位置、水中ロボットが目標到達位置に到達するときの報酬を１、その他の場合の報酬を０として、障害物の位置を考慮しないで価値関数Ｑ（ｓ，ａ）を生成する。
ダイナミックプログラミング部８０は、最大加速度設定部１１２、目標速度計算部１０２、想定海流速度値入力部１０３、変位量計算部１０４、状態遷移確率計算部１０５、報酬決定部１０９’、動作計画部１１０を有する。最大加速度設定部１１２、目標速度計算部１０２、想定海流速度値入力部１０３、変位量計算部１０４、状態遷移確率計算部１０５、動作計画部１１０の処理は、従来技術と同じであるためその説明を省略する。
報酬決定部１０９’は、障害物の有無を考慮せずに、原点（０，０）を目標到達位置、水中ロボットが目標到達位置に到達するときの報酬＞その他の場合の報酬とする。例えば、水中ロボットが目標到達位置に到達するときの報酬を１、その他の場合の報酬決定された報酬を０とする。決定された報酬は、動作計画部１１０に出力される。この報酬決定部１０９’の処理のみが、従来技術とは異なる。ダイナミックプログラミング部８０の他の処理は、従来技術と同様である。
動作計画部１１０は、報酬決定部１０９’が決定した報酬に基づいて価値関数Ｑ（ｓ，
ａ）を求める。遷移前の状態ｓにおける行動ａ１（ｍ）（ｍ＝１，…，Ｍ）ごとに計算された価値関数Ｑ（ｓ，ａ）は、価値関数保存部７０に格納される。
Ｑ（ｓ，ａ）は、各状態ｓにおいて、行動ａを選択した結果、将来水中ロボットが受ける報酬の推定量を示すものであり、各状態ｓにおいて、Ｑの値を最大化するａを選択することにより最適な行動方策を選ぶことができる。
【００２６】
なお、上記式（２’）では、水流の速度（ｆｘ，ｆｙ）の方向や速さ及び旋回速度Ψ’が異なっても、水中ロボットの位置（Ｘ，Ｙ）は、水流の速度（ｆｘ，ｆｙ）を行動単位時間Ｔで積分した値だけが変化し、方位角Ψは、旋回速度Ψ’を行動単位時間Ｔだけ積分した値だけ変化すると仮定している。しかし、上記の仮定は、水中ロボットの形状やその他の要因を考慮すると、現実の世界においては必ずしも成り立つとは限らない。したがって、上記式（２’）を用いる代わりに、想定される水流を発生させた水槽の中に水中ロボットを入れて、Ｄｘ（Ψ０，ａ），Ｄｙ（Ψ０，ａ），ＤΨ，ＤΨ’の値を実際に測定しても良い。すなわち、いわゆる水槽試験等の手法により、Ｄｘ（Ψ０，ａ），Ｄｙ（Ψ０，ａ），ＤΨ，ＤΨ’を直接計測しても良い。
なお、本発明では、原点（０，０）を目標到達位置、水中ロボットが目標到達位置に到達するときの報酬＞その他の場合の報酬を０として、障害物の位置を考慮しないで生成された価値関数Ｑ（ｓ，ａ）に基づいて、下記に述べる処理を行う点が重要であり、価値関数Ｑ（ｓ，ａ）の生成手段であるダイナミックプログラミング部は、必須の構成要件ではない点に留意する。
【００２７】
＜ステップＳ１＞
環境モデル生成部１０は、新たな障害物を検出するごとに、その障害物の位置を求めて、環境モデル生成部１０の障害物情報保存部１０１に格納する。障害物の検出及び障害物の位置の計算には、例えば、超音波測距センサを用いる。例えば、超音波測距センサは、水中ロボットの進行方向に対して−ω〜＋ω度の範囲を等間隔に分けるように３〜５つ程度設けることができる。ここで、ω＝４５〜６０度である。図３は、超音波測距センサが、水中ロボット１の前方に５つ設けられている例である。
これらの超音波測距センサのうち、ｋ番目の超音波測距センサ２が検出した障害物９の位置（Ｘｏｋ，Ｙｏｋ）は、水中ロボット１の位置を（Ｘ，Ｙ）、水中ロボット１の方位角をΨ、ｋ番目の超音波測距センサとｋ番目の超音波測距センサが検出したその超音波測距センサと障害物の距離をｒｋ、水中ロボットの進行方向に対する超音波測距センサ２の角度をθｋとすると、方位角Ψが、Ｘ軸の正方向からの方位角として定義されている場合には、
Ｘｏ＝Ｘ＋ｒｋ×ｃｏｓ（Ψ＋θｋ）
Ｙｏ＝Ｙ＋ｒｋ×ｓｉｎ（Ψ＋θｋ）
として求めることができる。ここで、超音波測距センサが取り付けられた位置と水中ロボットの位置を定める際の基準となる点は、十分に近いものとする。
【００２８】
なお、障害物情報データの更新は、例えば、行動単位時間Ｔごとに行うことができるが、必ずしも行動単位時間Ｔごとに行う必要はない。例えば、行動単位時間Ｔよりも短い間隔で行っても良い。これにより、障害物の検知率が上がり、水中ロボットが障害物にぶつかる可能性を低くすることができる。
【００２９】
＜ステップＳ２＞
軌道生成部２０は、障害物情報保存部１０１から読み出した障害物の位置情報を基にして、水中ロボットの現在位置から目標位置に至るまでの、障害物と接触しない目標軌道位置を計算する。障害物の位置情報のみを考慮した目標軌道位置の計算方法としては既出の方法が多数ある。本実施形態では、例えば以下のようにして、目標軌道位置を求める。
図４は、軌道生成部２０の機能構成を例示する図である。図５は、軌道生成部２０の処理を例示する図である。
まず、現時点での水中ロボットの位置を含む格子をｓ０とする。そして、目標位置を含む格子をｓｄとする。１時刻ステップごとに、つまり、行動単位時間Ｔごと、水中ロボットは、隣り合う格子の何れかに等しい確率で移動するものとする。「隣り合う」とは、格子が辺を介して隣り合う場合だけではなく、格子が頂点を介して隣り合う場合をも意味する点に注意する。すなわち、水中ロボットの運動学的特性を考慮せずに、水中ロボットが自機を囲む８つのどの格子にも移動可能であると仮定する。
【００３０】
例えば、図６に示すように、ある時刻ステップで水中ロボットが黒丸が描かれた格子にいる場合には、次に時刻ステップにおいて水中ロボットは白丸で描かれた格子の何れかに等しい確率で移動するものとする。具体的には、図６の例では、水中ロボットは、黒丸が描かれた格子に１の確率で存在しているため、行動単位時間Ｔの経過後には、白丸が描かれた格子のそれぞれに１／８の確率で存在するものとする。
各時刻ステップｔで、水中ロボットが格子ｓに存在する確率をＰ（ｓ，ｔ）とする。そうすると、時刻ステップ０において、水中ロボットが格子ｓ０に位置する確率Ｐ（ｓ０，０）＝１であり、時刻ステップ０において、水中ロボットが格子ｓ０以外の格子に位置する確率Ｐ（ｓ（ｓ！＝ｓ０），０）＝０である。ここで、「ｓ！＝ｓ０」は、格子ｓ０以外の格子を意味する。
軌道生成部２０の初期値設定部２０２が、水中ロボットの現在位置が含まれる格子に水中ロボットが時刻ステップ０において存在する確率Ｐ（ｓ０，０）を１とし、その他の格子に存在する確率Ｐ（ｓ（ｓ！＝ｓ０），０）を０に設定する（ステップＳ２１）。
【００３１】
任意の時刻ステップｔにおけるＰ（ｓ，ｔ）は、下記の式で計算することができる。
Ｐ（ｓ，ｔ）＝Σｓ’（１／８）×Ｐ（ｓ’，ｔ−１）  …（５）
上記式中のｓ’は、格子ｓに隣り合う８つ格子、言い換えると、格子ｓの周りのｓを含まない８つの格子のことである。上記式中のΣは、このｓ’についての和を取るものである。例えば、図７に示すように、格子ｓの周りにｓ１’，…ｓ８’の８つの格子があり、水中ロボットは、時刻ステップｔ−１において格子ｓｐ’（ｐ＝１，…，８）に、それぞれＰ（ｓｐ’，ｔ−１）の確率で存在しているものとする。このとき、上記の仮定より、水中ロボットは、ｓｐ’（ｐ＝１，…，８）の各格子から１／８の確率で格子ｓに移動するため、水中ロボットが時刻ステップｔにおいて格子ｓに存在する確率Ｐ（ｓ，ｔ）は、
Ｐ（ｓ，ｔ）＝Σｐ＝１８（１／８）×Ｐ（ｓｐ’，ｔ−１）
となる。
軌道生成部２０の存在確率計算部２０３が、上記式（５）に基づいて各時刻ステップにおける水中ロボットの存在確率を計算する（ステップＳ２２）。
【００３２】
格子が障害物を含む場合には、水中ロボットはその格子に侵入することはできない。このため、軌道生成部２０の確率補正部２０４が、障害物の位置についての情報を障害物情報保存部１０１から読み出し、上記存在確率計算部２０３で求まった存在確率のうち、各障害物が存在している格子についての存在確率を０とする（ステップＳ２３）。
計算された各時刻ステップにおける水中ロボットが各格子に存在する確率Ｐ（ｓ，ｔ）は、存在確率記憶部２０５に保存される。また、存在確率記憶部２０５から読み出した一時刻ステップ前の確率Ｐ（ｓ，ｔ−１）に基づいて、次の時刻ステップにおける確率Ｐ（ｓ，ｔ）は計算される。
軌道生成部２０の制御部２０６は、水中ロボットが目標到達位置ｓｄを含む格子に存在する確率Ｐ（ｓｄ，ｔ）が０でない値になるまで、存在確率計算部２０３と確率補正部２０４の処理を繰り返すように制御する。制御部２０６は、Ｐ（ｓｄ，ｔ）が０でない値になった場合には、そのときの時刻ステップｔｄを、軌道生成部２０の軌道決定部２０７に出力する（ステップＳ２４）。
【００３３】
軌道生成部２０の軌道決定部２０７は、まず、時刻ステップｔｄに対応した目標軌道位置τ（ｔｄ）を、τ（ｔｄ）＝ｓｄとして、軌道生成部２０内の目標軌道位置保存部２０１に格納する（ステップＳ２５）。
軌道決定部２０７は、水中ロボットが時刻ステップｔ−１において、目標軌道位置τ（ｔ）の周りにある各格子に存在している確率を存在確率記憶部２０５から読み出す。そして、それらの確率を最も大きくする格子をτ（ｔ−１）として、目標軌道位置保存部２０１に格納する（ステップＳ２６）。
この処理をｔ＝ｔｄからｔ＝１まで繰り返すことによって、軌道決定部２０７は、最終的に、各時刻ステップにおける目標軌道位置τ（ｔｄ），τ（ｔｄ−１），…，τ（１），τ（０）を求める（ステップＳ２７）。
上記の計算方法は、既出の手法に比べて計算負担が少ない。このため、計算の実時間性を担保することができる。
【００３４】
なお、上記実施形態では、その格子が障害物を含むか含まないかを問わず、すべての格子について、水中ロボットがその格子に存在する確率を求めたのちに、その格子が障害物を含むかどうかを判断し、その格子が障害物を含む場合には、水中ロボットがその格子に存在する確率を０とした。しかし、障害物を含む格子については、水中ロボットがその格子に存在する確率を求めずに、常に、その格子に存在する確率を０としても良い。
また、軌道生成部２０は、障害物情報保存部１０１から読み出した障害物の位置情報を参照して、障害物の位置を構成する格子空間（Ｘ，Ｙで構成される）を利用して、いわゆる動的計画法により、目標軌道位置を求めても良い。
【００３５】
＜ステップＳ３＞
軌道追従計算部３０は、各行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を取ったときに、どの程度目標軌道位置τ（ｉ）（ｉ＝０，１，…，ｔｄ）に近づくことができるのかを値で評価し、その評価値により、各行動ａ１（ｍ）（ｍ＝１，…，Ｍ）に優先順位を付ける。図８は、軌道追従計算部３０の機能構成を例示した図である。
軌道追従計算部３０の相対位置決定部３０１は、目標軌道位置τ（ｉ）（ｉ＝０，１，…，ｔｄ）に対する現時点における水中ロボットの相対位置を（Ｘτ（ｉ），Ｙτ（ｉ））を求める。すなわち、目標軌道位置τ（ｉ）（ｉ＝０，１，…，ｔｄ）を原点としたときの、現時点における水中ロボットの位置を（Ｘτ（ｉ），Ｙτ（ｉ））を求める。
【００３６】
軌道追従計算部３０の遷移先予測部２１３は、（Ｘτ（ｉ），Ｙτ（ｉ））に位置する水中ロボットが、各行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を取ったときの遷移先の状態ｓ（τｉ，ａ１（ｍ））を求める。行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を第一の行動と呼ぶ。すなわち、時刻ｔ＝０における水中ロボットの方位角をΨ、旋回速度をΨ’とすると、位置（Ｘτ（ｉ）＋Ｄｘａ（ｓ，ａ），Ｙτ（ｉ）＋Ｄｙａ（ｓ，ａ），ＤΨ＋ＤΨ，ＤΨ’＋ＤΨ’）が含まれる状態ｓ（τｉ，ａ１（ｍ））を求める。遷移先予測部２１３、海流差計測部６、位置計測部７の処理は、従来例と同様であるため説明を省略する。
軌道追従計算部３０の最大値抽出部３０３は、遷移先の状態ｓ（τｉ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を取ったときのＱ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））の最大値Ｑｍａｘ（τｉ，ａ１（ｍ））を求める。
行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を第二の行動と呼ぶ。すなわち、遷移先の状況ｓ（τｉ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）を取ったときにＱ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））が定まるが、これらの各行動ａ２（ｎ）ごとに求まった報酬Ｑ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））のうち、最大のものＱｍａｘ（τｉ，ａ１（ｍ））を求める。
【００３７】
相対位置決定部３０１と遷移先予測部２１３と最大値抽出部３０３は、上記の処理を行うことにより、τｉとａ１（ｍ）のすべての組について、Ｑｍａｘ（τｉ，ａ１（ｍ））を求める。
軌道追従計算部３０の加算部３０４は、すべてのτｉについての、τｉとａ１（ｍ）の各組ごとに求まったＱｍａｘ（τｉ，ａ１（ｍ））の和を求める。すなわち、
Ｑｍａｘ（ａ１（ｍ））＝Σｉ＝０ｔｄＱｍａｘ（τｉ，ａ１（ｍ））
を計算する。
Ｑｍａｘ（ａ１（ｍ））は、行動ａ１（ｍ）を選択した水中ロボットが、将来的に、目標軌道上に乗っている可能性の大きさを示す指標となる。なぜなら、上記したように、価値関数Ｑ（ｓ，ａ）は、状態ｓにある自機が、価値関数Ｑ（ｓ，ａ）を最大にする行動ａを選択することにより、原点に最も効率良く近づくことができるように設計されており、かつ、上記相対位置決定部３０１の処理により、原点が目標軌道τｉに置き換えられているためである。
軌道追従計算部３０の順序決定部３０５は、行動ａ１（ｍ）を序列化する。具体的には、Ｑｍａｘ（ａ１（ｍ））の値が大きい順に、行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を並び替えて、ａ１’，ａ２’，…，ａＭ’とすることにより優先順位を付ける。ａｍ’（ｍ＝１，…，Ｍ）は、行動選択部５０に出力される。
【００３８】
なお、上記最大値抽出部３０３に替えて、以下に説明する平均値抽出部３０６を設けても良い。
軌道追従計算部３０の平均値抽出部３０６は、遷移先の状態ｓ（τｉ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を取ったときの報酬Ｑ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））の平均値Ｑａｖｅ（τｉ，ａ１（ｍ））を求める。すなわち、遷移先の状況ｓ（τｉ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）を取ったときに報酬Ｑ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））が定まるが、これらの各行動ａ２（ｎ）ごとに求まった報酬Ｑ（ｓ（τｉ，ａ１（ｍ）），ａ２（ｎ））の平均値Ｑａｖｅ（τｉ，ａ１（ｍ））を求める。
この場合には、加算部３０４は、Ｑｍａｘ（τｉ，ａ１（ｍ））ではなく、Ｑａｖｅ（τｉ，ａ１（ｍ））についての和を取る。すなわち、
Ｑａｖｅ（ａ１（ｍ））＝Σｉ＝０ｔｄＱａｖｅ（τｉ，ａ１（ｍ））
を計算する。
そして、順序決定部３０５は、Ｑａｖｅ（ａ１（ｍ））の値が大きい順に、行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を並び替えて、ａ１’，ａ２’，…，ａＭ’とする。
【００３９】
＜ステップＳ４＞
障害物回避計算部４０の相対位置決定部４０１は、環境モデル作成部の障害物情報記録部から読み出した障害物の位置ｏｂ（ｊ）（ｊ＝１，２，…，Ｊ）に対する現時点における水中ロボットの相対位置を（Ｘｏｂ（ｊ），Ｙｏｂ（ｊ））を求める。すなわち、障害物の位置ｏｂ（ｊ）（ｊ＝１，２，…，Ｊ）を原点としたときの、現時点における水中ロボットの位置を（Ｘｏｂ（ｊ），Ｙｏｂ（ｊ））を求める。
障害物回避計算部４０の遷移先予測部２１３は、（Ｘｏｂ（ｊ），Ｙｏｂ（ｊ））に位置する水中ロボットが、各行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を取ったときの遷移先の状態ｓ（ｏｂｊ，ａ１（ｍ））を求める。行動ａ１（ｍ）（ｍ＝１，…，Ｍ）を第一の行動と呼ぶ。すなわち、時刻ｔ＝０における水中ロボットの方位角をΨ、旋回速度をΨ’とすると、位置（Ｘｏｂ（ｊ）＋Ｄｘａ（ｓ，ａ），Ｙｏｂ（ｊ）＋Ｄｙａ（ｓ，ａ），ＤΨ＋ＤΨ，ＤΨ’＋ＤΨ’）が含まれる状態ｓ（ｏｂｊ，ａ１（ｍ））を求める。遷移先予測部２１３、海流差計測部６、位置計測部７の処理は、従来例と同様であるため説明を省略する。
【００４０】
障害物回避計算部４０の最大値抽出部４０３は、遷移先の状態ｓ（ｏｂｊ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を取ったときの報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））の最大値Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））を求める。行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を第二の行動と呼ぶ。すなわち、遷移先の状況ｓ（ｏｂｊ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）を取ったときに報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））が定まるが、これらの各行動ａ２（ｎ）ごとに求まった報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））のうち、最大のものＱｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））を求める。Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））は、最大値選択部４０４とリスク計算部４０５に出力される。
相対位置決定部４０１と遷移先予測部２１３と最大値抽出部４０３は、上記の処理を行うことにより、ｏｂｊとａ１（ｍ）のすべての組について、Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））を求める。
障害物回避計算部４０の最大値選択部４０４は、各障害物の位置ｏｂ（ｊ）ごとに、Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））を最大にする行動ａ１（ｍ）を選択する。選択されたａ１（ｍ）は、ａｍａｘ（ｊ）としてリスク計算部４０５に出力される。
【００４１】
リスク計算部４０５は、障害物の位置ｏｂ（ｊ）（ｊ＝１，２，…，Ｊ）を、ａｍａｘ（ｊ）の値が同じもの同士でグループ分けする。そして、それぞれのグループ内における、Ｑｏｂｍａｘ（ｏｂｊ，ａｍａｘ（ｊ））の最大値をＱｒｉｓｋ（ｓ，ａ１（ｎ））とする。すなわち、
リスク計算部４０５は、
Ｑｒｉｓｋ（ｓ，ａ１（ｍ））＝ｍａｘ｛Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））｜ａｍａｘ（ｊ）＝ａ１（ｍ）｝
を計算する。
Ｑｒｉｓｋ（ｓ，ａ１（ｍ））は、行動ａ１（ｍ）を選択した水中ロボットが、将来的に、障害物にぶつかる可能性の大きさを示す指標となる。なぜなら、上記したように、価値関数Ｑ（ｓ，ａ）は、状態ｓにある自機が、価値関数Ｑ（ｓ，ａ）を最大にする行動ａを選択することにより、原点に最も効率良く近づくことができるように設計されており、かつ、上記相対位置決定部４０１の処理により、原点が障害物の位置ｏｂ（ｊ）に置き換えられているためである。各行動ａ１（ｍ）ごとに求まったＱｒｉｓｋ（ｓ，ａ１（ｍ））は、行動選択部５０に出力される。
なお、最大値抽出部４０３に替えて、以下に説明する平均値抽出部４０６を設けても良い。
【００４２】
障害物回避計算部４０の平均値抽出部４０６は、遷移先の状態ｓｏｂ（ｊ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）（ｎ＝１，…，Ｎ）を取ったときの報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））の平均値Ｑａｖｅ（ｏｂｊ，ａ１（ｍ））を求める。すなわち、遷移先の状況ｓｏｂ（ｊ，ａ１（ｍ））において、水中ロボットが各行動ａ２（ｎ）を取ったときに報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））が定まるが、これらの各行動ａ２（ｎ）ごとに求まった報酬Ｑ（ｓ（ｏｂｊ，ａ１（ｍ）），ａ２（ｎ））の平均値Ｑａｖｅ（ｏｂｊ，ａ１（ｍ））を求める。計算された平均値Ｑａｖｅ（ｏｂｊ，ａ１（ｍ））は、最大値選択部４０４とリスク計算部４０５に出力される。
この場合には、最大値選択部４０４は、各障害物の位置ｏｂ（ｊ）ごとに、Ｑｏｂｍａｘ（ｏｂｊ，ａ１（ｍ））ではなく、Ｑａｖｅ（ｏｂｊ，ａ１（ｍ））を最大にする行動ａ１（ｍ）を選択する。選択されたａ１（ｍ）は、ａｍａｘ（ｊ）としてリスク計算部４０５に出力される。
リスク計算部４０５は、障害物の位置ｏｂ（ｊ）（ｊ＝１，２，…，Ｊ）を、ａｍａｘ（ｊ）の値が同じもの同士でグループ分けする。そして、それぞれのグループ内における、Ｑａｖｅ（ｏｂｊ，ａｍａｘ（ｊ））の最大値をＱｒｉｓｋ（ｓ，ａ１（ｎ））とする。すなわち、
リスク計算部４０５は、
Ｑｒｉｓｋ（ｓ，ａ１（ｍ））＝ｍａｘ｛Ｑｏｂａｖｅ（ｏｂｊ，ａ１（ｍ））｜ａｍａｘ（ｊ）＝ａ１（ｍ）｝
を計算する。
【００４３】
＜ステップＳ５＞
行動選択部５０は、軌道追従計算部３０が求めたａ１’，ａ２’，…，ａＭ’と、障害物回避計算部４０が求めたＱｒｉｓｋ（ｓ，ａ１（ｍ））を利用して、最適な行動を決定する。図１０は、行動選択部５０の処理を例示した図である。
ここで、Ｑｔｈｒｅｓｈを水中ロボットの安全性を保障する一定の閾値とする。すなわち、Ｑｒｉｓｋ（ｓ，ａ）＜Ｑｔｈｒｅｓｈであれば、その行動ａを取る水中ロボットが障害物にぶつからないことが保障される。例えば、ｒを水中ロボットが目標位置に着いたときに得られる報酬、γを割引率、ｎを行動ステップの数とすると、Ｑｔｈｒｅｓｈとしては、ｎ行動ステップ後にロボットが障害物にぶつかる場合のＱ値の値にすることができる。すなわち、Ｑｔｈｒｅｓｈ＝ｒ×γｎにすることができる。ここで、ｎは、ロボットの旋回半径を考慮して、２〜４の値にすると望ましい。
行動選択部５０は、まず、Ｑｒｉｓｋ（ｓ，ａ１’）とＱｔｈｒｅｓｈの大小関係を比較する（ステップＳ５１）。その結果、Ｑｒｉｓｋ（ｓ，ａ１’）＜Ｑｔｈｒｅｓｈであれば、行動ａ１’を最適な行動として選択する（ステップＳ５１’）。Ｑｒｉｓｋ（ｓ，ａ１’）＞Ｑｔｈｒｅｓｈであれば、Ｑｒｉｓｋ（ｓ，ａ２’）とＱｔｈｒｅｓｈの大小関係を比較する（ステップＳ５２）。その結果、Ｑｒｉｓｋ（ｓ，ａ２’）＜Ｑｔｈｒｅｓｈであれば、行動ａ２’を最適な行動として選択する（ステップＳ５２’）。Ｑｒｉｓｋ（ｓ，ａ２’）＞Ｑｔｈｒｅｓｈであれば、Ｑｒｉｓｋ（ｓ，ａ３’）とＱｔｈｒｅｓｈの大小関係を比較する（ステップＳ５３）。その結果、Ｑｒｉｓｋ（ｓ，ａ３’）＜Ｑｔｈｒｅｓｈであれば、行動ａ３’を最適な行動として選択する（ステップＳ５３’）。Ｑｒｉｓｋ（ｓ，ａ３’）＞Ｑｔｈｒｅｓｈであれば、Ｑｒｉｓｋ（ｓ，ａ４’）とＱｔｈｒｅｓｈの大小関係を比較する。
【００４４】
上記の処理を、行動が選択されるまで、又は、Ｑｒｉｓｋ（ｓ，ａＭ’）＞Ｑｔｈｒｅｓｈと判断されるまで繰り返す（ステップＳ５Ｍ）。
このように、目標軌道に到達する可能性が高い行動ａ１’，ａ２’，…，ａＭ’の順番で、その行動を取ったときの障害物へのぶつかりやすさＱｒｉｓｋ（ｓ，ａ）が、水中ロボットの安全性を保障する一定の閾値を下回っているかどうかを検証することにより、障害物にぶつからない行動の中で最も軌道追従性の高い行動を選択することができる。
【００４５】
＜ステップＳ６＞
フィードバック制御部６０は、水中ロボットが、行動選択部５０が選択した行動ａに従った動作をするように、舵の切り角δや、推進気力器力Ｍｔｈを制御する。
以上の処理を、行動単位時間Ｔ単位ごとに繰り返すことにより、未知海流外乱と未知障害物が存在する中での水中ロボットの障害物回避制御が可能となる。
以上が、本発明による水中ロボットの動作制御装置の概要である。
水中ロボットの動作制御装置の処理機能をコンピュータによって実現することができる。この場合、水中ロボットの動作制御装置の処理機能の内容はプログラムによって記述される。そして、このプログラムを、図１９に示すようなコンピュータで実行することにより、例えば、図１に示す水中ロボットの動作制御装置の各処理機能がコンピュータ上で実現される。
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。コンピュータで読み取り可能な記録媒体としては、例えば、磁気記録装置、光ディスク、光磁気記録媒体、半導体メモリ等どのようなものでもよい。具体的には、例えば、磁気記録装置として、ハードディスク装置、フレキシブルディスク、磁気テープ等を、光ディスクとして、ＤＶＤ（Digital Versatile Disc）、ＤＶＤ−ＲＡＭ（Random Access Memory）、ＣＤ−ＲＯＭ（Compact Disc Read Only Memory）、ＣＤ−Ｒ（Recordable）／ＲＷ（ReWritable）等を、光磁気記録媒体として、ＭＯ（Magneto-Optical disc）等を、半導体メモリとしてＥＥＰ−ＲＯＭ（Electronically Erasable and Programmable-Read Only Memory）等を用いることができる。
【００４６】
また、このプログラムの流通は、例えば、そのプログラムを記録したＤＶＤ、ＣＤ−ＲＯＭ等の可搬型記録媒体を販売、譲渡、貸与等することによって行う。さらに、このプログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することにより、このプログラムを流通させる構成としてもよい。
このようなプログラムを実行するコンピュータは、例えば、まず、可搬型記録媒体に記録されたプログラムもしくはサーバコンピュータから転送されたプログラムを、一旦、自己の記憶装置に格納する。そして、処理の実行時、このコンピュータは、自己の記録媒体に格納されたプログラムを読み取り、読み取ったプログラムに従った処理を実行する。また、このプログラムの別の実行形態として、コンピュータが可搬型記録媒体から直接プログラムを読み取り、そのプログラムに従った処理を実行することとしてもよく、さらに、このコンピュータにサーバコンピュータからプログラムが転送されるたびに、逐次、受け取ったプログラムに従った処理を実行することとしてもよい。また、サーバコンピュータから、このコンピュータへのプログラムの転送は行わず、その実行指示と結果取得のみによって処理機能を実現する、いわゆるＡＳＰ（Application Service Provider）型のサービスによって、上述の処理を実行する構成としてもよい。なお、本形態におけるプログラムには、電子計算機による処理用に供する情報であってプログラムに準ずるもの（コンピュータに対する直接の指令ではないがコンピュータの処理を規定する性質を有するデータ等）を含むものとする。
【００４７】
また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、水中ロボットの動作制御装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
以上の各実施形態の他、本発明である水中ロボットの動作制御方法、装置、プログラム及びその記録媒体は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【図面の簡単な説明】
【００４８】
【図１】本発明による水中ロボットの動作制御部１０００の機能構成を例示する図。
【図２】本発明による水中ロボットの動作制御部１０００の処理を例示した図。
【図３】障害物の位置の求め方の説明を補助する図。
【図４】軌道生成部２０の機能構成を例示する図。
【図５】軌道生成部２０の処理を例示する図。
【図６】目標軌道位置の求め方の説明を補助する図。
【図７】確率Ｐ（ｓ，ｔ）の計算の説明を補助する図。
【図８】軌道追従計算部３０の機能構成を例示した図。
【図９】障害物回避計算部４０の機能構成を例示した図。
【図１０】行動選択部５０の処理を例示した図。
【図１１】本発明による動作制御の対象となるロボットを例示した図。
【図１２】時間に応じて変化する目標旋回速度の様子を例示した図。
【図１３】従来例による自律ロボットの動作制御装置の機能構成を例示した図。
【図１４】ロボットの位置の変位量の説明を補助する図。
【図１５】状態遷移確率の計算の説明を補助する図。
【図１６】状態ｓと遷移先の状態ｓ’の傾斜角差を表わす図。
【図１７】従来例による動作制御の対象となるロボットを例示した図。
【図１８】ダイナミックプログラミング部８０の機能構成を例示する図。
【図１９】本発明による水中ロボットの動作制御装置をコンピュータで実施するときの機能構成を例示した図。
【図１】
2008052473.tif 000010
【図２】
2008052473.tif 000011
【図３】
2008052473.tif 000012
【図４】
2008052473.tif 000013
【図５】
2008052473.tif 000014
【図６】
2008052473.tif 000015
【図７】
2008052473.tif 000016
【図８】
2008052473.tif 000017
【図９】
2008052473.tif 000018
【図１０】
2008052473.tif 000019
【図１１】
2008052473.tif 000020
【図１２】
2008052473.tif 000021
【図１３】
2008052473.tif 000022
【図１４】
2008052473.tif 000023
【図１５】
2008052473.tif 000024
【図１６】
2008052473.tif 000025
【図１７】
2008052473.tif 000026
【図１８】
2008052473.tif 000027
【図１９】
2008052473.tif 000028