方位角推定装置、方法及びプログラム
【課題】方位角計測センサを用いずに、位置計測センサの計測値のみから、方位角を推定する。
【解決手段】時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計測更新部１８が計算する。
【解決手段】時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計測更新部１８が計算する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、対象物の方位角を推定する技術に関する。
【背景技術】
【０００２】
近年、屋内外で活動可能な自律移動ロボットの研究が活発に行われており、それらの応用先が広がりつつある。自律移動ロボットの種類としては、車輪を使用して地表を移動するタイプのものや、空を飛行して移動する自律型空中ロボット、水中を航行する水中型ロボットなどがある。自律移動ロボットの行動を高精度で制御するアルゴリズムを確立する上では、自律移動ロボットの位置と姿勢角を実時間で高精度に計測するための技術が必須である。
【０００３】
自律移動ロボットの位置を計測する技術としてＧＰＳ方式、無線ＬＡＮ方式による位置計測技術が、姿勢角、方位角を計測する技術としてジャイロセンサ等を使用する技術が知られている。しかし、これらの位置・角度計測技術の計測精度は、自律移動ロボットを制御する上で不十分であることがある。そこで、これらの位置計測技術により求まった計測値を用いて、より精度が高い位置を推定するために、フィルタ技術が導入されている（例えば、非特許文献１参照。）。これらのフィルタ技術のほとんどがベイズフィルタの応用によるものである。ベイズフィルタを用いて高精度の位置の推定を行うためには、自動移動ロボットの高精度な移動モデルが必要である。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】上田隆一、新井民夫、浅沼和範、梅田和昇、大隅久、「パーティクルフィルタを利用した自己位置推定に生じる致命的な推定誤りからの回復法」、日本ロボット学会誌Vol. 23、No. 4、pp.466〜473、2005
【発明の概要】
【発明が解決しようとする課題】
【０００５】
角加速度を計測するジャイロセンサには小型のものが存在するが、絶対角を正確に計測するためのレーザージャイロなどは重量が大きいものが多く、それらを小型の自律飛行船ロボットに搭載するのは難しい。また、自律移動ロボットの中には、自律飛行船型ロボットのようにそのペイロードの重量が限られている場合や、ロボット自身の大きさの制限から必要な性能を満たすセンサをロボットに搭載できない場合がある。
【０００６】
非特許文献１の技術では、ロボットの方位角を計測するためのセンサが搭載されていることを前提としているため、このようなセンサが搭載されていない状況において方位角の計測及び推定を行うことはできないという課題がある。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、この発明では、対象物に搭載された位置を計測するセンサの計測値を使用して、その対象物の方位角を推定する。
【発明の効果】
【０００８】
方位角を計測するためのセンサがなくても、対象物の方位角を推定することができる。
【図面の簡単な説明】
【０００９】
【図１】方位角推定装置の例の機能ブロック図。
【図２】方位角推定方法の例の流れ図。
【図３】方位角の推定の対象となる対象物を例示する図。
【図４】アクチュエータへの指示値を例示する図。
【図５】図４の指示値に対応する方位角速度Φ’の変化を例示する図。
【図６】計算方法２を説明するための図。
【発明を実施するための形態】
【００１０】
［対象物］
方位角の推定の対象となる対象物は、例えば図３に例示する自律飛行船等の自律移動ロボットである。この自律飛行船は、舵２、主推進器３、上下方向推進器４を装備している。また、図示していないが、横方向（方位角方向）推進器を備える。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、対象物が自律移動ロボットである場合を例に挙げて説明する。なお、状態の推定の対象となる対象物は何でもよく、自律移動ロボットに限られない。
【００１１】
図１の自律移動ロボットのゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。この例では、これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面（ＸＹ平面）内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されているものとする。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。
【００１２】
時刻ステップｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。
【００１３】
Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））
すると、これらの平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））は、以下のように計算できる。
【００１４】
Ｚｘａ（ｔ）＝［Ｚｘ１（ｔ）＋Ｚｘ２（ｔ）＋Ｚｘ３（ｔ）＋Ｚｘ４（ｔ）＋Ｚｘ５（ｔ）］／５ …（ａ）
Ｚｙａ（ｔ）＝［Ｚｙ１（ｔ）＋Ｚｙ２（ｔ）＋Ｚｙ３（ｔ）＋Ｚｙ４（ｔ）＋Ｚｙ５（ｔ）］／５ …（ｂ）
Ｚｚａ（ｔ）＝［Ｚｚ１（ｔ）＋Ｚｚ２（ｔ）＋Ｚｚ３（ｔ）＋Ｚｚ４（ｔ）＋Ｚｚ５（ｔ）］／５
【００１５】
センサが点対象に配置されていることから、上記平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として扱うことができる。以後、平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として説明する。
【００１６】
なお、自律飛行船は船尾に装備された垂直尾翼の効果により、方位角方向の回転運動については安定であるとする。
【００１７】
[自律飛行船の制御]
この発明では、自律飛行船のアクチュエータ（例えば、横方向推進器）に与える指示値は、各行動選択時刻における方位角速度が一定値０となるよう制御されるものとする。また、前進速度は常に一定の値Ｖｘであるとの仮定がなりたつように制御を行う。つまり、各時刻ステップにおいて、自律飛行船のアクチュエータに与える指示値は図４のように制御される。ここで、Ｔａは加速方向に一定値（方位角方向トルクＭ）を与える時間、Ｔｄは減速方向に一定値（方位角方向トルクＭ）を与える時間、Ｔｓは自律飛行船自身の持つ方位角方向の速度の安定特性を利用して、方位角速度が０になるよう調整する時間である。
【００１８】
図４の指示値を自律飛行船のアクチュエータに与えると、自律飛行船の方位角速度Φ’は図５のようなステップ応答となる。１時刻ステップに相当する時間は、Ｔ＝Ｔａ＋Ｔｄ＋Ｔｓである。ＴａとＴｄの時間の長さの比率は、アクチュエータに与える指示値と方位角速度との関係を別途計測し、その関係をもとに決める。また、ＴａとＴｄの時間の長さは、Ｔｄ終了時点でなるべく自律飛行船の方位角速度が０に近くなるように設定される。また、前進速度は一定とするために、前進方向のアクチュエータ力は一定値を指示値とする。
【００１９】
［ベイズフィルタ］
以下、ベイズフィルタについて簡単に説明をする。ベイズフィルタの詳細については、参考文献１を参照のこと。
〔参考文献１〕スラン・セバスチャン（著）、バーガード・ウルフラム（著）、フォックス・ディーター（著）、上田隆一（訳）、「確率ロボティクス」、毎日コミュニケーションズ、2007/10/25
【００２０】
ベイズフィルタは、以下に記述される（１）予測と（２）計測更新とを繰り返すアルゴリズムである。以下、方位角Φ(t)の推定を例にとって説明する。
【００２１】
（１）予測
bel0(Φ(t)) = ∫p(Φ(t)| u(t),Φ(t-1))・bel(Φ(t-1)) dΦ(t-1)
（２）計測更新
bel(Φ(t))=ηp( Zφ(t)| Φ(t) )・bel0(Φ(t))
【００２２】
ｂｅｌ（Φ（ｔ））は、時刻ステップｔにおいて自律飛行船の方位角がΦ（ｔ）である確率である。時刻ステップｔのｂｅｌ（Φ（ｔ））を求めるために、まず、（１）予測のステップにおいて、一時刻ステップ前のｂｅｌ（Φ（ｔ−１））に、方位角遷移モデルにより推定した自律移動ロボットの状態遷移確率ｐ（Φ（ｔ）｜ｕ（ｔ），Φ（ｔ−１））をかけて、時刻ステップｔのｂｅｌ（Φ（ｔ））の推定値ｂｅｌ０（Φ（ｔ））を求める。ここで、ｕ（ｔ）は時刻ステップｔでの自律移動ロボットの制御入力である。
【００２３】
次に、（２）計測更新のステップにおいて、時刻ステップｔで実際に計測された方位角センサの計測値Ｚφ（ｔ）を用いて、推定値ｂｅｌ０（Φ（ｔ））の補正を行い、時刻ステップｔのｂｅｌ（Φ（ｔ））を求める。具体的には、自律移動ロボットの状態がΦ（ｔ）である場合に位置計測センサにより計測値Ｚφ（ｔ）が得られる確率値を用いて、ｂｅｌ０の補正を行っている。なお、ηは、正の定数である。
【００２４】
［バイナリベイズフィルタ］
上述のベイズフィルタは数学的概念を記したものにすぎないので、計算機の上でこれを実装するためには別の表現が必要である。ベイズフィルタの実装形態としては、パーティクルフィルタやモンテカルロ・ローカリゼーション、バイナリベイズフィルタなどが知られている。以下では、この発明の手法に関連の深いバイナリベイズフィルタについて簡単に説明する。
【００２５】
バイナリベイズフィルタは、ベイズフィルタを計算機での利用に即した離散表現で書き下したものである。バイナリベイズフィルタにおける（１）予測、（２）計算更新、の各ステップは、例えば以下のように記述される。
【００２６】
（１−１）予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t),Φs(t-1))・Bel(Φs(t-1))
（１−２）計測更新
Bel(Φｓ(t))=ηP( Zφs(t)| Φs(t) )・Bel0(Φs(t))
【００２７】
ここで、Φｓ（ｔ）は、時刻ステップｔにおける自律飛行船の方位角を離散値で表現したものである。たとえば、０度から３６０度までの一回転の角度を３６分割した場合、Φｓ（ｔ）の値は３６通りの離散値のうちのいずれかをとることになる。例えば、３６個のΦｓ（ｔ）にそれぞれ１から３６までの整数が割り当てられる。
【００２８】
Ｂｅｌ（Φｓ（ｔ））は、時刻ステップｔにおいて、自律飛行船の方位角がΦｓ（ｔ）である確率である。時刻ステップｔのＢｅｌ（Φｓ（ｔ））を求めるために、まず、（１−１）予測のステップにおいて、一時刻ステップ前のＢｅｌ（Φｓ（ｔ−１））に、方位角遷移モデルにより推定した自律移動ロボットの状態遷移確率をかけて、時刻ステップｔのＢｅｌ（Φｓ（ｔ））の推定値Ｂｅｌ０（Φｓ（ｔ））を求める。ここで、方位角遷移モデルは、方位角遷移確率ｐ（Φｓ（ｔ）｜ａ（ｔ），Φｓ（ｔ−１））で表される。ここでａ（ｔ）は時刻ステップｔにてロボットがとる行動の離散表現であり、その時刻ステップにおいて自律移動ロボットへ与える制御入力値を識別するための識別子である。すなわち、ロボットは正の整数個の行動のうち、各時刻ステップにおいて１つの行動を選択し、その行動と対応づけられた制御入力値を自律移動ロボットのアクチュエータに与える。
【００２９】
次に、（１−２）計測更新のステップにおいて、時刻ステップｔで実際に計測された方位角センサの計測値の離散値Ｚφｓ（ｔ）を用いて、推定値Ｂｅｌ０（Φｓ（ｔ））の補正を行い、時刻ステップｔのＢｅｌ（Φｓ（ｔ））を求める。ここで、Ｐ（Ｚφｓ（ｔ）｜Φｓ（ｔ））は、時刻ステップｔで自律飛行船の方位角の真値がΦｓ（ｔ）であった場合に、方位角センサの計測値の離散値としてＺφｓ（ｔ）が計測される確率値である。
【００３０】
以上に述べたバイナリベイズフィルタを用いれば、ベイズフィルタを計算機上で実装することが可能である。しかしながら、この発明は方位角を計測するための方位角センサを搭載できない状況を前提としているため、上記の計算方法で必要となる「方位角センサの計測値の離散値Ｚφｓ（ｔ）」を得ることができない。そのため、そのままこの手法を用いることはできない。そこで、この発明では、時刻ステップｔ及びｔ−１における位置センサによる計測値（５つのセンサの計測値の平均値）Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）を利用して、（１−２）計測更新の処理を行う。
【００３１】
［基本的な考え方］
この発明では、時刻ステップｔ及びｔ−１における位置センサによる計測値（５つのセンサの計測値の平均値）をＺｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）とし、上記Ｐ（Ｚφｓ（ｔ）｜Φｓ（ｔ））の代わりに、確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を使用する。つまり、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が、行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置するという計測結果が得られる確率を使用する。この確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））の計算方法は後述する。
【００３２】
各時刻における自律飛行船の方位角と制御入力によって、それぞれの時刻での自律飛行船の位置変位量は一意に決まるので、この置き換えは妥当であるといえる。この置き換えを行うとバイナリベイズフィルタは、以下のようになる。
【００３３】
（２−１）１時刻ステップ前の予測
Bel0(Φs(t-1)) = Σp(Φs(t-1)| a(t-2),Φs(t-2))・Bel(Φs(t-2))
（２−２）計測更新
Bel(Φｓ(t-1))=ηP( Zxa(t), Zya(t), Zxa(t-1), Zya(t-1)| Φs(t-1), a(t-1) )・Bel0(Φs(t-1))
（２−３）現在時刻の予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t-1),Φs(t-1))・Bel(Φs(t-1))
【００３４】
ここで重要なのは、上記（２−２）計測更新ステップを終了した時点で得られるのは、時刻ステップｔにおいて自律飛行船の方位角がΦｓ（ｔ）である確率Ｂｅｌ（Φｓ（ｔ））ではなく、時刻ステップｔ−１において自律飛行船の方位角がΦｓ（ｔ−１）である確率Ｂｅｌ（Φｓ（ｔ−１））となる点である。これは、Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を使用した更新が、一時刻ステップ前（時刻ステップｔ−１）の方位角Φｓ（ｔ−１）について行われていることにより生じるものである。
【００３５】
そこで、現在時刻ステップｔにおいて自律飛行船の方位角がΦｓ（ｔ）である確率Ｂｅｌ（Φｓ（ｔ））を得るために、（２−２）計測更新ステップの後に、さらに（２−３）現在時刻の予測ステップの計算を行っている。そして、（２−３）現在時刻の予測ステップで得られるＢｅｌ０（Φｓ（ｔ））を方位角の推定値の算出に使用する。
【００３６】
ここで、以上の計算を計算機上に実装して、各時刻について繰り返すことを考えると、隣り合う時刻において（２−１）と（２−３）の計算は冗長となる。冗長部分を整理すると、以下のようになる。
【００３７】
（３−１）計測更新
Bel(Φｓ(t-1))=ηP( Zxa(t), Zya(t), Zxa(t-1), Zya(t-1)| Φs(t-1), a(t-1) )・Bel0(Φs(t-1))
（３−２）現在時刻の予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t-1),Φs(t-1))・Bel(Φs(t-1))
【００３８】
以下に説明する方位角推定装置の一実施形態では、上記の（３−１）及び（３−２）のステップに従って、自律飛行船の方位角の推定を行う。
【００３９】
［実施形態］
図１は、この発明による方位角推定装置の例の機能ブロック図である。図２は、この発明による方位角推定方法の例の流れ図である。
【００４０】
方位角推定装置は、図１に例示するように、記憶部１１、行動入力部１２、初期値設定部１３、時刻設定部１４、位置計測確率計算部１５、推定変位量計算部１６、比較部１７、計測更新部１８、制御部１９、予測部２０、方位角推定部２１、正規化部２２、位置計測センサ６１，６２，６３，６４，６５、重心計算部６６及び格子位置取得部６７を例えば含む。
【００４１】
記憶部１１には、各時刻ステップｔにおいて対象物のアクチュエータに与えられた行動ａ（ｔ）が格納される。また、方位角推定部２１により計算される、過去の各時刻ステップｔにおける対象物の方位角の推定値が格納されてもよい。
【００４２】
各時刻ステップｔにおける対象物の行動ａ（ｔ）は、例えば行動入力部１２がその各時刻ステップｔにおける対象物のＸＹ平面内の位置及び推定された方位角に基づいて自動的に決定してもよいし（例えば、参考文献２参照。）、対象物を遠隔操作する者がその操作により決定してもよい。
〔参考文献２〕特開２００７−３１７１６５号明細書
【００４３】
＜ステップＳ１＞
初期値設定部１３は、時刻ステップｔを初期値０に設定し、時刻ステップｔ＝０におけるＢｅｌ０（Φｓ（０））の初期値を設定する（ステップＳ１）。事前に対象物の方位角を定めておき、対称物をその方位角で配置した場合には、例えば、その方位角に対応するΦｓのＢｅｌ０（Φｓ（０））＝１とし、他のΦｓについてのＢｅｌ０（Φｓ（０））＝０とする。
【００４４】
＜ステップＳ２＞
時刻設定部１４は、ｔ＝ｔ＋１とする。すなわち時刻ステップｔを１だけインクリメントする（ステップＳ２）。
【００４５】
＜ステップＳ３＞
位置計測確率計算部１５は、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が、行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置するという計測結果が得られる確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する（ステップＳ３）。後述するように、ステップＳ３及びステップＳ４の処理は、全ての方位角の離散値Φｓ（ｔ−１）について行う。計算された確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））は、計測更新部１８に送られる。
【００４６】
位置計測確率計算部１５は、上記計算に用いる、行動ａ（ｔ−１）については記憶部１１から読み込み、Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）については重心計算部６６から取得する。なお、重心計算部６６は、位置計測センサ６１，６２，６３，６４，６５からそれぞれ取得したＺ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ），Ｚ１（ｔ−１），Ｚ２（ｔ−１），Ｚ３（ｔ−１），Ｚ４（ｔ−１），Ｚ５（ｔ−１）から、上記（ａ）及び（ｂ）式に基づいてＺｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）を計算し、位置計測確率計算部１５に送る。Ｚｘａ（ｔ），Ｚｙａ（ｔ）は、必要に応じて格子位置取得部６７及び比較部１７にも送る。
【００４７】
確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））は、例えば以下の≪計算方法１≫〜≪計算方法４≫の何れかにより計算することができる。
【００４８】
なお、対象物が位置するＸＹ平面は面積Δｓの格子で分割されているとし、重心計算部６６が計算した重心（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に基づいて、その重心に対応する格子ｓ（ｔ）とその格子ｓ（ｔ）の座標（Ｘｓ（ｔ），Ｙｓ（ｔ））を格子位置取得部６７が取得する。重心に対応する格子とは、例えばその重心を含む格子のことである。座標（Ｘｓ（ｔ），Ｙｓ（ｔ））は、位置計測確率計算部１５に出力される。また、必要に応じて、格子ｓ（ｔ）、格子ｓ（ｔ−１）、格子ｓ（ｔ−１）の座標（Ｘｓ（ｔ−１），Ｙｓ（ｔ−１））も、位置計測確率計算部１５に送られる。格子のサイズは、位置計測センサ６１，６２，６３，６４，６５の計測精度を考慮して定める。計測精度が高ければ高いほど格子のサイズを小さくすることができる。
【００４９】
また、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））とし、方位角Φｓ（ｔ−１）と行動ａ（ｔ−１）に基づいて、推定変位量計算部１６が推定変位量（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））を計算する。例えば、以下の式に基づいて、推定変位量を計算する。推定変位量は、位置計測確率計算部１５に出力される。
【００５０】
【数１】
【００５１】
上記式において、Ｖｘは対象物の前進速度、Φｓ（ｔ−１）ｒは方位角（の離散値）Φｓ（ｔ−１）の代表値であり例えばΦｓ（ｔ−１）の中央値、ｄΦ（τ）は対象物が行動ａ（ｔ−１）を取ったときの時刻τにおける方位角の変化量とする。なお、格子Φｓ（ｔ−１）の任意の点をΦｓ（ｔ−１）の代表値としてもよい。τは各時刻ステップｔ内の経過時刻を表わす。すなわち、各時刻ステップｔが始まるときにτ＝０とされその後時間の経過と共に増加しτ＝Ｔとなると時刻ステップｔ＋１となり再度τ＝０となる。行動ａ（ｔ−１）に基づいて対象物のアクチュエータに与える指示値が例えば図４のように定まり、この指示値に基づいて例えば図５のように方位角速度の変化量ｄΦ（τ）が定まる。
【００５２】
≪計算方法１≫
計算方法１は、正規分布を用いて位置計測確率を求めるものである。
【００５３】
位置計測確率計算部１５は、下記式の値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５４】
【数２】
【００５５】
ここで、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）であり、分散σ２は予め定められた定数である。
【００５６】
≪計算方法２≫
位置計測確率計算部１５は、時刻ｔ−１における対象物の重心の計測値（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に対応する格子ｓ（ｔ−１）を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））だけＸＹ平面を平行移動させた格子ｓ’（ｔ−１）が、時刻ｔにおける対象物の重心の計測値（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に対応する格子ｓ（ｔ）と重なる面積（図６の斜線部分の面積）をΔｓで割った値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５７】
計算方法２は、格子ｓ’（ｔ−１）が他の格子と重なる面積に比例した確率で、対象物が重なった先の格子内に存在する、という仮定に基づくものである。 ≪計算方法３≫
位置計測確率計算部１５は、下記式の値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５８】
【数３】
【００５９】
上記式において、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））であり、Φｓ（ｔ−１）ｒをΦｓ（ｔ−１）の代表値とし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量として、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））であり、分散σを予め定められた定数である。
【００６０】
対象物が自律飛行船である場合には、その方位角は進行方向と一致している場合が多い。特に、自律飛行船の進行方向の速度が十分に大きい場合には、それが特に顕著である。計算方法３は、この性質に基づくものである。
【００６１】
≪計算方法４≫
自律飛行船の変位ベクトル（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１）），（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））の大きさが小さくなると（例えば飛行船がその場で回頭しているとき等）、計算方法３の精度は急激に低下する。
【００６２】
そこで、計算方法４では、変位ベクトル（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１）），（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））が、小さい場合には計算方法１により計算を行い、大きい場合には計算方法３により計算を行う。
【００６３】
具体的には、比較部１７が、変位ベクトルの大きさ（（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１））２＋（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））２）（１／２）と所定の閾値とを比較して、所定の閾値の方が大きい場合には、計算方法１により確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１）の計算を行い、所定の閾値の方が小さい場合には、計算方法３により確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１）の計算を行う。
【００６４】
＜ステップＳ４＞
計測更新部１８は、位置計測確率計算部１５により計算された確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を用いて、上記（３−１）に対応する計測更新を行う（ステップＳ４）。
【００６５】
具体的には、計測更新部１８は、確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する。更新前確率Ｂｅｌ０（Φｓ（ｔ−１））は、記憶部１１から読み込まれる。計算された更新後確率Ｂｅｌ（Φｓ（ｔ−１））は、予測部２０に送られる。
【００６６】
＜ステップＳ５＞
制御部１９は、全ての方位角の離散値Φｓ（ｔ−１）について、ステップＳ３及びステップＳ４の処理を行ったかどうかを判定する（ステップＳ５）。全ての方位角の離散値Φｓ（ｔ−１）について処理を行っていないと判定された場合には、まだ処理を行っていない方位角の離散値Φｓ（ｔ−１）を選択して、ステップＳ３に戻る。全ての方位角の離散値Φｓ（ｔ−１）について処理を行ったと判定された場合には、ステップＳ６に進む。
【００６７】
＜ステップＳ６＞
予測部２０は、計測更新部１８により計算された更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、上記（３−２）に対応する現在時刻の予測の処理を行う（ステップＳ６）。
【００６８】
具体的には、予測部２０は、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する。更新前確率Ｂｅｌ０（Φｓ（ｔ））は、方位角推定部２１に送られると共に、記憶部１１に格納される。
【００６９】
確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））は、例えば下記式により計算することができる。
【００７０】
【数４】
【００７１】
上記式においてＲ３＝（Φｓ（ｔ）−（ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）））２である。ＤΦ（ａ（ｔ−１））は、対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量である。τを各時刻ステップｔ内の経過時刻とし、Ｔを１時刻ステップの長さとし、Φ’（τ）を対象物が行動ａ（ｔ−１）を取ったときの各時刻τにおける方位角速度とすると、ＤΦ（ａ（ｔ−１））は下式により計算することができる。
【００７２】
【数５】
【００７３】
＜ステップＳ６１＞
制御部１９は、全ての方位角の離散値Φｓ（ｔ）について、ステップＳ６の処理を行ったかどうかを判定する（ステップＳ６１）。全ての方位角の離散値Φｓ（ｔ）について処理を行っていないと判定された場合には、まだ処理を行っていない方位角の離散値Φｓ（ｔ）を選択して、ステップＳ６に戻る。全ての方位角の離散値Φｓ（ｔ）について処理を行ったと判定された場合には、ステップＳ７に進む。
【００７４】
これにより、全ての方位角の離散値Φｓ（ｔ）について、ステップＳ６の処理を行う。
【００７５】
＜ステップＳ７＞
方位角推定部２１は、更新前確率Ｂｅｌ０（Φｓ（ｔ））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））を計算して、その計算結果を時刻ステップｔにおける対象物の方位角の推定値とする（ステップＳ７）。
【００７６】
なお、方位角推定部２１は、時刻ステップｔ−１の更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ０（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ−１における対象物の方位角の推定値としてもよい。
【００７７】
＜ステップＳ８＞
制御部１９は次の時刻ステップについても継続して予測を行うかどうかを判断し（ステップＳ８）、次の時刻ステップについても予測を行う場合にはステップＳ２に戻る。これ以上予測を行わない場合は処理を終了する。
【００７８】
このように、位置計測センサ６１，６２，６３，６４，６５の計測値を用いて上記（３−１）に対応する計測更新を行うことにより、方位角計測用のセンサを用いずに方位角を推定することができる。
【００７９】
［変形例等］
正規化部２２が、ステップＳ５の後に、ΣΦｓ（ｔ−１）Ｂｅｌ（Φｓ（ｔ−１））＝１となるように、Ｂｅｌ（Φｓ（ｔ−１））を正規化してもよい。すなわち、各Ｂｅｌ（Φｓ（ｔ−１））を、Ｂｅｌ（Φｓ（ｔ−１））／ΣΦｓ（ｔ−１）Ｂｅｌ（Φｓ（ｔ−１））に置き換えてもよい。同様に、正規化部２２が、ステップＳ６１の後に、ΣΦｓ（ｔ）Ｂｅｌ０（Φｓ（ｔ））＝１となるように、Ｂｅｌ０（Φｓ（ｔ））を正規化してもよい。すなわち、各Ｂｅｌ０（Φｓ（ｔ））を、Ｂｅｌ０（Φｓ（ｔ））／ΣΦｓ（ｔ）Ｂｅｌ０（Φｓ（ｔ））に置き換えてもよい。正規化を行うことにより、方位角の推定精度が増す。
【００８０】
方位角推定装置の各部間のデータのやり取りは直接行われてもよいし、記憶部１１を介して行われてもよい。
【００８１】
方位角推定装置は、コンピュータによって実現することができる。この場合、この装置が有すべき各機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。
【００８２】
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【００８３】
この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【符号の説明】
【００８４】
１５ 位置計測確率計算部
１７ 比較部
１８ 計測更新部
２０ 予測部
２１ 方位角推定部
６１，６２，６３，６４，６５ 位置計測センサ
【技術分野】
【０００１】
この発明は、対象物の方位角を推定する技術に関する。
【背景技術】
【０００２】
近年、屋内外で活動可能な自律移動ロボットの研究が活発に行われており、それらの応用先が広がりつつある。自律移動ロボットの種類としては、車輪を使用して地表を移動するタイプのものや、空を飛行して移動する自律型空中ロボット、水中を航行する水中型ロボットなどがある。自律移動ロボットの行動を高精度で制御するアルゴリズムを確立する上では、自律移動ロボットの位置と姿勢角を実時間で高精度に計測するための技術が必須である。
【０００３】
自律移動ロボットの位置を計測する技術としてＧＰＳ方式、無線ＬＡＮ方式による位置計測技術が、姿勢角、方位角を計測する技術としてジャイロセンサ等を使用する技術が知られている。しかし、これらの位置・角度計測技術の計測精度は、自律移動ロボットを制御する上で不十分であることがある。そこで、これらの位置計測技術により求まった計測値を用いて、より精度が高い位置を推定するために、フィルタ技術が導入されている（例えば、非特許文献１参照。）。これらのフィルタ技術のほとんどがベイズフィルタの応用によるものである。ベイズフィルタを用いて高精度の位置の推定を行うためには、自動移動ロボットの高精度な移動モデルが必要である。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】上田隆一、新井民夫、浅沼和範、梅田和昇、大隅久、「パーティクルフィルタを利用した自己位置推定に生じる致命的な推定誤りからの回復法」、日本ロボット学会誌Vol. 23、No. 4、pp.466〜473、2005
【発明の概要】
【発明が解決しようとする課題】
【０００５】
角加速度を計測するジャイロセンサには小型のものが存在するが、絶対角を正確に計測するためのレーザージャイロなどは重量が大きいものが多く、それらを小型の自律飛行船ロボットに搭載するのは難しい。また、自律移動ロボットの中には、自律飛行船型ロボットのようにそのペイロードの重量が限られている場合や、ロボット自身の大きさの制限から必要な性能を満たすセンサをロボットに搭載できない場合がある。
【０００６】
非特許文献１の技術では、ロボットの方位角を計測するためのセンサが搭載されていることを前提としているため、このようなセンサが搭載されていない状況において方位角の計測及び推定を行うことはできないという課題がある。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、この発明では、対象物に搭載された位置を計測するセンサの計測値を使用して、その対象物の方位角を推定する。
【発明の効果】
【０００８】
方位角を計測するためのセンサがなくても、対象物の方位角を推定することができる。
【図面の簡単な説明】
【０００９】
【図１】方位角推定装置の例の機能ブロック図。
【図２】方位角推定方法の例の流れ図。
【図３】方位角の推定の対象となる対象物を例示する図。
【図４】アクチュエータへの指示値を例示する図。
【図５】図４の指示値に対応する方位角速度Φ’の変化を例示する図。
【図６】計算方法２を説明するための図。
【発明を実施するための形態】
【００１０】
［対象物］
方位角の推定の対象となる対象物は、例えば図３に例示する自律飛行船等の自律移動ロボットである。この自律飛行船は、舵２、主推進器３、上下方向推進器４を装備している。また、図示していないが、横方向（方位角方向）推進器を備える。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、対象物が自律移動ロボットである場合を例に挙げて説明する。なお、状態の推定の対象となる対象物は何でもよく、自律移動ロボットに限られない。
【００１１】
図１の自律移動ロボットのゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。この例では、これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面（ＸＹ平面）内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されているものとする。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。
【００１２】
時刻ステップｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。
【００１３】
Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））
すると、これらの平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））は、以下のように計算できる。
【００１４】
Ｚｘａ（ｔ）＝［Ｚｘ１（ｔ）＋Ｚｘ２（ｔ）＋Ｚｘ３（ｔ）＋Ｚｘ４（ｔ）＋Ｚｘ５（ｔ）］／５ …（ａ）
Ｚｙａ（ｔ）＝［Ｚｙ１（ｔ）＋Ｚｙ２（ｔ）＋Ｚｙ３（ｔ）＋Ｚｙ４（ｔ）＋Ｚｙ５（ｔ）］／５ …（ｂ）
Ｚｚａ（ｔ）＝［Ｚｚ１（ｔ）＋Ｚｚ２（ｔ）＋Ｚｚ３（ｔ）＋Ｚｚ４（ｔ）＋Ｚｚ５（ｔ）］／５
【００１５】
センサが点対象に配置されていることから、上記平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として扱うことができる。以後、平均値（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｚａ（ｔ））を自律飛行船の重心位置（Ｘｇ，Ｙｇ，Ｚｇ）の計測値として説明する。
【００１６】
なお、自律飛行船は船尾に装備された垂直尾翼の効果により、方位角方向の回転運動については安定であるとする。
【００１７】
[自律飛行船の制御]
この発明では、自律飛行船のアクチュエータ（例えば、横方向推進器）に与える指示値は、各行動選択時刻における方位角速度が一定値０となるよう制御されるものとする。また、前進速度は常に一定の値Ｖｘであるとの仮定がなりたつように制御を行う。つまり、各時刻ステップにおいて、自律飛行船のアクチュエータに与える指示値は図４のように制御される。ここで、Ｔａは加速方向に一定値（方位角方向トルクＭ）を与える時間、Ｔｄは減速方向に一定値（方位角方向トルクＭ）を与える時間、Ｔｓは自律飛行船自身の持つ方位角方向の速度の安定特性を利用して、方位角速度が０になるよう調整する時間である。
【００１８】
図４の指示値を自律飛行船のアクチュエータに与えると、自律飛行船の方位角速度Φ’は図５のようなステップ応答となる。１時刻ステップに相当する時間は、Ｔ＝Ｔａ＋Ｔｄ＋Ｔｓである。ＴａとＴｄの時間の長さの比率は、アクチュエータに与える指示値と方位角速度との関係を別途計測し、その関係をもとに決める。また、ＴａとＴｄの時間の長さは、Ｔｄ終了時点でなるべく自律飛行船の方位角速度が０に近くなるように設定される。また、前進速度は一定とするために、前進方向のアクチュエータ力は一定値を指示値とする。
【００１９】
［ベイズフィルタ］
以下、ベイズフィルタについて簡単に説明をする。ベイズフィルタの詳細については、参考文献１を参照のこと。
〔参考文献１〕スラン・セバスチャン（著）、バーガード・ウルフラム（著）、フォックス・ディーター（著）、上田隆一（訳）、「確率ロボティクス」、毎日コミュニケーションズ、2007/10/25
【００２０】
ベイズフィルタは、以下に記述される（１）予測と（２）計測更新とを繰り返すアルゴリズムである。以下、方位角Φ(t)の推定を例にとって説明する。
【００２１】
（１）予測
bel0(Φ(t)) = ∫p(Φ(t)| u(t),Φ(t-1))・bel(Φ(t-1)) dΦ(t-1)
（２）計測更新
bel(Φ(t))=ηp( Zφ(t)| Φ(t) )・bel0(Φ(t))
【００２２】
ｂｅｌ（Φ（ｔ））は、時刻ステップｔにおいて自律飛行船の方位角がΦ（ｔ）である確率である。時刻ステップｔのｂｅｌ（Φ（ｔ））を求めるために、まず、（１）予測のステップにおいて、一時刻ステップ前のｂｅｌ（Φ（ｔ−１））に、方位角遷移モデルにより推定した自律移動ロボットの状態遷移確率ｐ（Φ（ｔ）｜ｕ（ｔ），Φ（ｔ−１））をかけて、時刻ステップｔのｂｅｌ（Φ（ｔ））の推定値ｂｅｌ０（Φ（ｔ））を求める。ここで、ｕ（ｔ）は時刻ステップｔでの自律移動ロボットの制御入力である。
【００２３】
次に、（２）計測更新のステップにおいて、時刻ステップｔで実際に計測された方位角センサの計測値Ｚφ（ｔ）を用いて、推定値ｂｅｌ０（Φ（ｔ））の補正を行い、時刻ステップｔのｂｅｌ（Φ（ｔ））を求める。具体的には、自律移動ロボットの状態がΦ（ｔ）である場合に位置計測センサにより計測値Ｚφ（ｔ）が得られる確率値を用いて、ｂｅｌ０の補正を行っている。なお、ηは、正の定数である。
【００２４】
［バイナリベイズフィルタ］
上述のベイズフィルタは数学的概念を記したものにすぎないので、計算機の上でこれを実装するためには別の表現が必要である。ベイズフィルタの実装形態としては、パーティクルフィルタやモンテカルロ・ローカリゼーション、バイナリベイズフィルタなどが知られている。以下では、この発明の手法に関連の深いバイナリベイズフィルタについて簡単に説明する。
【００２５】
バイナリベイズフィルタは、ベイズフィルタを計算機での利用に即した離散表現で書き下したものである。バイナリベイズフィルタにおける（１）予測、（２）計算更新、の各ステップは、例えば以下のように記述される。
【００２６】
（１−１）予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t),Φs(t-1))・Bel(Φs(t-1))
（１−２）計測更新
Bel(Φｓ(t))=ηP( Zφs(t)| Φs(t) )・Bel0(Φs(t))
【００２７】
ここで、Φｓ（ｔ）は、時刻ステップｔにおける自律飛行船の方位角を離散値で表現したものである。たとえば、０度から３６０度までの一回転の角度を３６分割した場合、Φｓ（ｔ）の値は３６通りの離散値のうちのいずれかをとることになる。例えば、３６個のΦｓ（ｔ）にそれぞれ１から３６までの整数が割り当てられる。
【００２８】
Ｂｅｌ（Φｓ（ｔ））は、時刻ステップｔにおいて、自律飛行船の方位角がΦｓ（ｔ）である確率である。時刻ステップｔのＢｅｌ（Φｓ（ｔ））を求めるために、まず、（１−１）予測のステップにおいて、一時刻ステップ前のＢｅｌ（Φｓ（ｔ−１））に、方位角遷移モデルにより推定した自律移動ロボットの状態遷移確率をかけて、時刻ステップｔのＢｅｌ（Φｓ（ｔ））の推定値Ｂｅｌ０（Φｓ（ｔ））を求める。ここで、方位角遷移モデルは、方位角遷移確率ｐ（Φｓ（ｔ）｜ａ（ｔ），Φｓ（ｔ−１））で表される。ここでａ（ｔ）は時刻ステップｔにてロボットがとる行動の離散表現であり、その時刻ステップにおいて自律移動ロボットへ与える制御入力値を識別するための識別子である。すなわち、ロボットは正の整数個の行動のうち、各時刻ステップにおいて１つの行動を選択し、その行動と対応づけられた制御入力値を自律移動ロボットのアクチュエータに与える。
【００２９】
次に、（１−２）計測更新のステップにおいて、時刻ステップｔで実際に計測された方位角センサの計測値の離散値Ｚφｓ（ｔ）を用いて、推定値Ｂｅｌ０（Φｓ（ｔ））の補正を行い、時刻ステップｔのＢｅｌ（Φｓ（ｔ））を求める。ここで、Ｐ（Ｚφｓ（ｔ）｜Φｓ（ｔ））は、時刻ステップｔで自律飛行船の方位角の真値がΦｓ（ｔ）であった場合に、方位角センサの計測値の離散値としてＺφｓ（ｔ）が計測される確率値である。
【００３０】
以上に述べたバイナリベイズフィルタを用いれば、ベイズフィルタを計算機上で実装することが可能である。しかしながら、この発明は方位角を計測するための方位角センサを搭載できない状況を前提としているため、上記の計算方法で必要となる「方位角センサの計測値の離散値Ｚφｓ（ｔ）」を得ることができない。そのため、そのままこの手法を用いることはできない。そこで、この発明では、時刻ステップｔ及びｔ−１における位置センサによる計測値（５つのセンサの計測値の平均値）Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）を利用して、（１−２）計測更新の処理を行う。
【００３１】
［基本的な考え方］
この発明では、時刻ステップｔ及びｔ−１における位置センサによる計測値（５つのセンサの計測値の平均値）をＺｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）とし、上記Ｐ（Ｚφｓ（ｔ）｜Φｓ（ｔ））の代わりに、確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を使用する。つまり、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が、行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置するという計測結果が得られる確率を使用する。この確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））の計算方法は後述する。
【００３２】
各時刻における自律飛行船の方位角と制御入力によって、それぞれの時刻での自律飛行船の位置変位量は一意に決まるので、この置き換えは妥当であるといえる。この置き換えを行うとバイナリベイズフィルタは、以下のようになる。
【００３３】
（２−１）１時刻ステップ前の予測
Bel0(Φs(t-1)) = Σp(Φs(t-1)| a(t-2),Φs(t-2))・Bel(Φs(t-2))
（２−２）計測更新
Bel(Φｓ(t-1))=ηP( Zxa(t), Zya(t), Zxa(t-1), Zya(t-1)| Φs(t-1), a(t-1) )・Bel0(Φs(t-1))
（２−３）現在時刻の予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t-1),Φs(t-1))・Bel(Φs(t-1))
【００３４】
ここで重要なのは、上記（２−２）計測更新ステップを終了した時点で得られるのは、時刻ステップｔにおいて自律飛行船の方位角がΦｓ（ｔ）である確率Ｂｅｌ（Φｓ（ｔ））ではなく、時刻ステップｔ−１において自律飛行船の方位角がΦｓ（ｔ−１）である確率Ｂｅｌ（Φｓ（ｔ−１））となる点である。これは、Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を使用した更新が、一時刻ステップ前（時刻ステップｔ−１）の方位角Φｓ（ｔ−１）について行われていることにより生じるものである。
【００３５】
そこで、現在時刻ステップｔにおいて自律飛行船の方位角がΦｓ（ｔ）である確率Ｂｅｌ（Φｓ（ｔ））を得るために、（２−２）計測更新ステップの後に、さらに（２−３）現在時刻の予測ステップの計算を行っている。そして、（２−３）現在時刻の予測ステップで得られるＢｅｌ０（Φｓ（ｔ））を方位角の推定値の算出に使用する。
【００３６】
ここで、以上の計算を計算機上に実装して、各時刻について繰り返すことを考えると、隣り合う時刻において（２−１）と（２−３）の計算は冗長となる。冗長部分を整理すると、以下のようになる。
【００３７】
（３−１）計測更新
Bel(Φｓ(t-1))=ηP( Zxa(t), Zya(t), Zxa(t-1), Zya(t-1)| Φs(t-1), a(t-1) )・Bel0(Φs(t-1))
（３−２）現在時刻の予測
Bel0(Φs(t)) = Σp(Φs(t)| a(t-1),Φs(t-1))・Bel(Φs(t-1))
【００３８】
以下に説明する方位角推定装置の一実施形態では、上記の（３−１）及び（３−２）のステップに従って、自律飛行船の方位角の推定を行う。
【００３９】
［実施形態］
図１は、この発明による方位角推定装置の例の機能ブロック図である。図２は、この発明による方位角推定方法の例の流れ図である。
【００４０】
方位角推定装置は、図１に例示するように、記憶部１１、行動入力部１２、初期値設定部１３、時刻設定部１４、位置計測確率計算部１５、推定変位量計算部１６、比較部１７、計測更新部１８、制御部１９、予測部２０、方位角推定部２１、正規化部２２、位置計測センサ６１，６２，６３，６４，６５、重心計算部６６及び格子位置取得部６７を例えば含む。
【００４１】
記憶部１１には、各時刻ステップｔにおいて対象物のアクチュエータに与えられた行動ａ（ｔ）が格納される。また、方位角推定部２１により計算される、過去の各時刻ステップｔにおける対象物の方位角の推定値が格納されてもよい。
【００４２】
各時刻ステップｔにおける対象物の行動ａ（ｔ）は、例えば行動入力部１２がその各時刻ステップｔにおける対象物のＸＹ平面内の位置及び推定された方位角に基づいて自動的に決定してもよいし（例えば、参考文献２参照。）、対象物を遠隔操作する者がその操作により決定してもよい。
〔参考文献２〕特開２００７−３１７１６５号明細書
【００４３】
＜ステップＳ１＞
初期値設定部１３は、時刻ステップｔを初期値０に設定し、時刻ステップｔ＝０におけるＢｅｌ０（Φｓ（０））の初期値を設定する（ステップＳ１）。事前に対象物の方位角を定めておき、対称物をその方位角で配置した場合には、例えば、その方位角に対応するΦｓのＢｅｌ０（Φｓ（０））＝１とし、他のΦｓについてのＢｅｌ０（Φｓ（０））＝０とする。
【００４４】
＜ステップＳ２＞
時刻設定部１４は、ｔ＝ｔ＋１とする。すなわち時刻ステップｔを１だけインクリメントする（ステップＳ２）。
【００４５】
＜ステップＳ３＞
位置計測確率計算部１５は、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が、行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置するという計測結果が得られる確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する（ステップＳ３）。後述するように、ステップＳ３及びステップＳ４の処理は、全ての方位角の離散値Φｓ（ｔ−１）について行う。計算された確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））は、計測更新部１８に送られる。
【００４６】
位置計測確率計算部１５は、上記計算に用いる、行動ａ（ｔ−１）については記憶部１１から読み込み、Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）については重心計算部６６から取得する。なお、重心計算部６６は、位置計測センサ６１，６２，６３，６４，６５からそれぞれ取得したＺ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ），Ｚ１（ｔ−１），Ｚ２（ｔ−１），Ｚ３（ｔ−１），Ｚ４（ｔ−１），Ｚ５（ｔ−１）から、上記（ａ）及び（ｂ）式に基づいてＺｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）を計算し、位置計測確率計算部１５に送る。Ｚｘａ（ｔ），Ｚｙａ（ｔ）は、必要に応じて格子位置取得部６７及び比較部１７にも送る。
【００４７】
確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））は、例えば以下の≪計算方法１≫〜≪計算方法４≫の何れかにより計算することができる。
【００４８】
なお、対象物が位置するＸＹ平面は面積Δｓの格子で分割されているとし、重心計算部６６が計算した重心（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に基づいて、その重心に対応する格子ｓ（ｔ）とその格子ｓ（ｔ）の座標（Ｘｓ（ｔ），Ｙｓ（ｔ））を格子位置取得部６７が取得する。重心に対応する格子とは、例えばその重心を含む格子のことである。座標（Ｘｓ（ｔ），Ｙｓ（ｔ））は、位置計測確率計算部１５に出力される。また、必要に応じて、格子ｓ（ｔ）、格子ｓ（ｔ−１）、格子ｓ（ｔ−１）の座標（Ｘｓ（ｔ−１），Ｙｓ（ｔ−１））も、位置計測確率計算部１５に送られる。格子のサイズは、位置計測センサ６１，６２，６３，６４，６５の計測精度を考慮して定める。計測精度が高ければ高いほど格子のサイズを小さくすることができる。
【００４９】
また、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））とし、方位角Φｓ（ｔ−１）と行動ａ（ｔ−１）に基づいて、推定変位量計算部１６が推定変位量（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））を計算する。例えば、以下の式に基づいて、推定変位量を計算する。推定変位量は、位置計測確率計算部１５に出力される。
【００５０】
【数１】
【００５１】
上記式において、Ｖｘは対象物の前進速度、Φｓ（ｔ−１）ｒは方位角（の離散値）Φｓ（ｔ−１）の代表値であり例えばΦｓ（ｔ−１）の中央値、ｄΦ（τ）は対象物が行動ａ（ｔ−１）を取ったときの時刻τにおける方位角の変化量とする。なお、格子Φｓ（ｔ−１）の任意の点をΦｓ（ｔ−１）の代表値としてもよい。τは各時刻ステップｔ内の経過時刻を表わす。すなわち、各時刻ステップｔが始まるときにτ＝０とされその後時間の経過と共に増加しτ＝Ｔとなると時刻ステップｔ＋１となり再度τ＝０となる。行動ａ（ｔ−１）に基づいて対象物のアクチュエータに与える指示値が例えば図４のように定まり、この指示値に基づいて例えば図５のように方位角速度の変化量ｄΦ（τ）が定まる。
【００５２】
≪計算方法１≫
計算方法１は、正規分布を用いて位置計測確率を求めるものである。
【００５３】
位置計測確率計算部１５は、下記式の値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５４】
【数２】
【００５５】
ここで、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）であり、分散σ２は予め定められた定数である。
【００５６】
≪計算方法２≫
位置計測確率計算部１５は、時刻ｔ−１における対象物の重心の計測値（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に対応する格子ｓ（ｔ−１）を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））だけＸＹ平面を平行移動させた格子ｓ’（ｔ−１）が、時刻ｔにおける対象物の重心の計測値（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に対応する格子ｓ（ｔ）と重なる面積（図６の斜線部分の面積）をΔｓで割った値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５７】
計算方法２は、格子ｓ’（ｔ−１）が他の格子と重なる面積に比例した確率で、対象物が重なった先の格子内に存在する、という仮定に基づくものである。 ≪計算方法３≫
位置計測確率計算部１５は、下記式の値を計算して、その計算結果を確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする。
【００５８】
【数３】
【００５９】
上記式において、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））であり、Φｓ（ｔ−１）ｒをΦｓ（ｔ−１）の代表値とし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量として、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））であり、分散σを予め定められた定数である。
【００６０】
対象物が自律飛行船である場合には、その方位角は進行方向と一致している場合が多い。特に、自律飛行船の進行方向の速度が十分に大きい場合には、それが特に顕著である。計算方法３は、この性質に基づくものである。
【００６１】
≪計算方法４≫
自律飛行船の変位ベクトル（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１）），（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））の大きさが小さくなると（例えば飛行船がその場で回頭しているとき等）、計算方法３の精度は急激に低下する。
【００６２】
そこで、計算方法４では、変位ベクトル（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１）），（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））が、小さい場合には計算方法１により計算を行い、大きい場合には計算方法３により計算を行う。
【００６３】
具体的には、比較部１７が、変位ベクトルの大きさ（（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１））２＋（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））２）（１／２）と所定の閾値とを比較して、所定の閾値の方が大きい場合には、計算方法１により確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１）の計算を行い、所定の閾値の方が小さい場合には、計算方法３により確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１）の計算を行う。
【００６４】
＜ステップＳ４＞
計測更新部１８は、位置計測確率計算部１５により計算された確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を用いて、上記（３−１）に対応する計測更新を行う（ステップＳ４）。
【００６５】
具体的には、計測更新部１８は、確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する。更新前確率Ｂｅｌ０（Φｓ（ｔ−１））は、記憶部１１から読み込まれる。計算された更新後確率Ｂｅｌ（Φｓ（ｔ−１））は、予測部２０に送られる。
【００６６】
＜ステップＳ５＞
制御部１９は、全ての方位角の離散値Φｓ（ｔ−１）について、ステップＳ３及びステップＳ４の処理を行ったかどうかを判定する（ステップＳ５）。全ての方位角の離散値Φｓ（ｔ−１）について処理を行っていないと判定された場合には、まだ処理を行っていない方位角の離散値Φｓ（ｔ−１）を選択して、ステップＳ３に戻る。全ての方位角の離散値Φｓ（ｔ−１）について処理を行ったと判定された場合には、ステップＳ６に進む。
【００６７】
＜ステップＳ６＞
予測部２０は、計測更新部１８により計算された更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、上記（３−２）に対応する現在時刻の予測の処理を行う（ステップＳ６）。
【００６８】
具体的には、予測部２０は、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する。更新前確率Ｂｅｌ０（Φｓ（ｔ））は、方位角推定部２１に送られると共に、記憶部１１に格納される。
【００６９】
確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））は、例えば下記式により計算することができる。
【００７０】
【数４】
【００７１】
上記式においてＲ３＝（Φｓ（ｔ）−（ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）））２である。ＤΦ（ａ（ｔ−１））は、対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量である。τを各時刻ステップｔ内の経過時刻とし、Ｔを１時刻ステップの長さとし、Φ’（τ）を対象物が行動ａ（ｔ−１）を取ったときの各時刻τにおける方位角速度とすると、ＤΦ（ａ（ｔ−１））は下式により計算することができる。
【００７２】
【数５】
【００７３】
＜ステップＳ６１＞
制御部１９は、全ての方位角の離散値Φｓ（ｔ）について、ステップＳ６の処理を行ったかどうかを判定する（ステップＳ６１）。全ての方位角の離散値Φｓ（ｔ）について処理を行っていないと判定された場合には、まだ処理を行っていない方位角の離散値Φｓ（ｔ）を選択して、ステップＳ６に戻る。全ての方位角の離散値Φｓ（ｔ）について処理を行ったと判定された場合には、ステップＳ７に進む。
【００７４】
これにより、全ての方位角の離散値Φｓ（ｔ）について、ステップＳ６の処理を行う。
【００７５】
＜ステップＳ７＞
方位角推定部２１は、更新前確率Ｂｅｌ０（Φｓ（ｔ））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））を計算して、その計算結果を時刻ステップｔにおける対象物の方位角の推定値とする（ステップＳ７）。
【００７６】
なお、方位角推定部２１は、時刻ステップｔ−１の更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ０（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ−１における対象物の方位角の推定値としてもよい。
【００７７】
＜ステップＳ８＞
制御部１９は次の時刻ステップについても継続して予測を行うかどうかを判断し（ステップＳ８）、次の時刻ステップについても予測を行う場合にはステップＳ２に戻る。これ以上予測を行わない場合は処理を終了する。
【００７８】
このように、位置計測センサ６１，６２，６３，６４，６５の計測値を用いて上記（３−１）に対応する計測更新を行うことにより、方位角計測用のセンサを用いずに方位角を推定することができる。
【００７９】
［変形例等］
正規化部２２が、ステップＳ５の後に、ΣΦｓ（ｔ−１）Ｂｅｌ（Φｓ（ｔ−１））＝１となるように、Ｂｅｌ（Φｓ（ｔ−１））を正規化してもよい。すなわち、各Ｂｅｌ（Φｓ（ｔ−１））を、Ｂｅｌ（Φｓ（ｔ−１））／ΣΦｓ（ｔ−１）Ｂｅｌ（Φｓ（ｔ−１））に置き換えてもよい。同様に、正規化部２２が、ステップＳ６１の後に、ΣΦｓ（ｔ）Ｂｅｌ０（Φｓ（ｔ））＝１となるように、Ｂｅｌ０（Φｓ（ｔ））を正規化してもよい。すなわち、各Ｂｅｌ０（Φｓ（ｔ））を、Ｂｅｌ０（Φｓ（ｔ））／ΣΦｓ（ｔ）Ｂｅｌ０（Φｓ（ｔ））に置き換えてもよい。正規化を行うことにより、方位角の推定精度が増す。
【００８０】
方位角推定装置の各部間のデータのやり取りは直接行われてもよいし、記憶部１１を介して行われてもよい。
【００８１】
方位角推定装置は、コンピュータによって実現することができる。この場合、この装置が有すべき各機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。
【００８２】
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【００８３】
この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【符号の説明】
【００８４】
１５ 位置計測確率計算部
１７ 比較部
１８ 計測更新部
２０ 予測部
２１ 方位角推定部
６１，６２，６３，６４，６５ 位置計測センサ
【特許請求の範囲】
【請求項１】
時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する位置するという計測結果が得られる確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する位置計測確率計算部と、
上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する計測更新部と、
時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する予測部と、
上記更新前確率Ｂｅｌ０（Φｓ（ｔ））又は上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））又はΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ又は時刻ｔ−１における対象物の方位角の推定値とする方位角推定部と、
を含む方位角推定装置。
【請求項２】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子ｓ（ｔ）の座標を（Ｘｓ（ｔ），Ｙｓ（ｔ））とし、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）），）とし、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）とし、分散σを予め定められた定数として、
【数６】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項３】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子をｓ（ｔ）とし、（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））を時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量として、
格子ｓ（ｔ−１）を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））だけＸＹ平面を平行移動させた格子ｓ’（ｔ−１）が、格子（ｓ）と重なる面積をΔｓで割った値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項４】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおける対象物の位置を（Ｚｘａ（ｔ），Ｚｙａ（ｔ））とし、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））とし、Φｓ（ｔ−１）の中央値をΦｓ（ｔ−１）ｒとし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量とし、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））とし、分散σを予め定められた定数として、
【数７】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘｓ（ｔ），Ｚｙｓ（ｔ），Ｚｘｓ（ｔ−１），Ｚｙｓ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項５】
請求項１の方位角推定装置において、
変位ベクトル（（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１））２＋（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））２）（１／２）と所定の閾値とを比較する比較部と、
上記変位ベクトルの方が小さければ、対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子ｓ（ｔ）の座標を（Ｘｓ（ｔ），Ｙｓ（ｔ））とし、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）），）とし、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）とし、分散σを予め定められた定数として、
【数８】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とし、
上記変位ベクトルの方が大きければ、対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおける対象物の位置を（Ｚｘａ（ｔ），Ｚｙａ（ｔ））とし、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））とし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量とし、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））とし、分散σを予め定められた定数として、
【数９】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘｓ（ｔ），Ｚｙｓ（ｔ），Ｚｘｓ（ｔ−１），Ｚｙｓ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項６】
位置計測確率計算部が、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する位置計測確率計算ステップと、
計測更新部が、上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する計測更新ステップと、
予測部が、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する予測ステップと、
方位角推定部が、上記更新前確率Ｂｅｌ０（Φｓ（ｔ））又は上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））又はΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ又は時刻ｔ−１における対象物の方位角の推定値とする方位角推定ステップと、
を含む方位角推定方法。
【請求項７】
請求項１から５の何れかの方位角推定装置の各部としてコンピュータを機能させるための方位角推定プログラム。
【請求項１】
時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する位置するという計測結果が得られる確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する位置計測確率計算部と、
上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する計測更新部と、
時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する予測部と、
上記更新前確率Ｂｅｌ０（Φｓ（ｔ））又は上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））又はΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ又は時刻ｔ−１における対象物の方位角の推定値とする方位角推定部と、
を含む方位角推定装置。
【請求項２】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子ｓ（ｔ）の座標を（Ｘｓ（ｔ），Ｙｓ（ｔ））とし、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）），）とし、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）とし、分散σを予め定められた定数として、
【数６】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項３】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子をｓ（ｔ）とし、（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））を時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量として、
格子ｓ（ｔ−１）を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）））だけＸＹ平面を平行移動させた格子ｓ’（ｔ−１）が、格子（ｓ）と重なる面積をΔｓで割った値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項４】
請求項１の方位角推定装置において、
対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおける対象物の位置を（Ｚｘａ（ｔ），Ｚｙａ（ｔ））とし、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））とし、Φｓ（ｔ−１）の中央値をΦｓ（ｔ−１）ｒとし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量とし、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））とし、分散σを予め定められた定数として、
【数７】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘｓ（ｔ），Ｚｙｓ（ｔ），Ｚｘｓ（ｔ−１），Ｚｙｓ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項５】
請求項１の方位角推定装置において、
変位ベクトル（（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１））２＋（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））２）（１／２）と所定の閾値とを比較する比較部と、
上記変位ベクトルの方が小さければ、対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおいて対象物が位置する格子ｓ（ｔ）の座標を（Ｘｓ（ｔ），Ｙｓ（ｔ））とし、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときのＸＹ平面における推定変位量を（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１）），Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１）），）とし、Ｒ１＝（Ｘｓ（ｔ）−（Ｄｘ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｘｓ（ｔ−１））２＋（Ｙｓ（ｔ）−（Ｄｙ（Φｓ（ｔ−１），ａ（ｔ−１））＋Ｙｓ（ｔ−１））２）とし、分散σを予め定められた定数として、
【数８】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とし、
上記変位ベクトルの方が大きければ、対象物が位置するＸＹ平面が面積Δｓの格子で分割されているとし、時刻ステップｔにおける対象物の位置を（Ｚｘａ（ｔ），Ｚｙａ（ｔ））とし、Φｔａｎ＝ａｒｃｔａｎ（（Ｚｙａ（ｔ）−Ｚｙａ（ｔ−１））／（Ｚｘａ（ｔ）−Ｚｘａ（ｔ−１）））とし、ＤΦ（ａ（ｔ−１））を対象物が行動ａ（ｔ−１）を取ったときの方位角の変位量とし、Ｒ２＝（Φｔａｎ−(ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））、Ｒ２＝（Φｔａｎ−Φｓ（ｔ−１）ｒ）又はＲ２＝（Φｔａｎ−（0.5×ＤΦ（ａ（ｔ−１））＋Φｓ（ｔ−１）ｒ））とし、分散σを予め定められた定数として、
【数９】
上記位置計測確率計算部は、上記式の値を計算して、その計算結果を上記確率Ｐ（Ｚｘｓ（ｔ），Ｚｙｓ（ｔ），Ｚｘｓ（ｔ−１），Ｚｙｓ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））とする、
ことを特徴とする方位角推定装置。
【請求項６】
位置計測確率計算部が、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において（Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１））に位置し、時刻ステップｔにおいて（Ｚｘａ（ｔ），Ｚｙａ（ｔ））に位置する確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））を計算する位置計測確率計算ステップと、
計測更新部が、上記確率Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））と、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新前確率Ｂｅｌ０（Φｓ（ｔ−１））とを用いて、Ｂｅｌ（Φｓ（ｔ−１））＝Ｐ（Ｚｘａ（ｔ），Ｚｙａ（ｔ），Ｚｘａ（ｔ−１），Ｚｙａ（ｔ−１）｜Φｓ（ｔ−１），ａ（ｔ−１））・Ｂｅｌ０（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ−１）である更新後確率Ｂｅｌ（Φｓ（ｔ−１））を計算する計測更新ステップと、
予測部が、時刻ステップｔ−１において方位角Φｓ（ｔ−１）の対象物が行動ａ（ｔ−１）を取ったときに、時刻ステップｔ−１において対象物の方位角がΦｓ（ｔ）となる確率ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））と、上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））とを用いて、Ｂｅｌ０（Φｓ（ｔ））＝ΣΦｓ（ｔ−１）ｐ（Φｓ（ｔ）｜ａ（ｔ−１），Φｓ（ｔ−１））・Ｂｅｌ（Φｓ（ｔ−１））の関係を満たす、時刻ステップｔにおいて対象物の方位角がΦｓ（ｔ）である更新前確率Ｂｅｌ０（Φｓ（ｔ））を計算する予測ステップと、
方位角推定部が、上記更新前確率Ｂｅｌ０（Φｓ（ｔ））又は上記更新後確率Ｂｅｌ（Φｓ（ｔ−１））を用いて、ΣΦｓ（ｔ）Φｓ（ｔ）・Ｂｅｌ０（Φｓ（ｔ））又はΣΦｓ（ｔ−１）Φｓ（ｔ−１）・Ｂｅｌ（Φｓ（ｔ−１））を計算して、その計算結果を時刻ステップｔ又は時刻ｔ−１における対象物の方位角の推定値とする方位角推定ステップと、
を含む方位角推定方法。
【請求項７】
請求項１から５の何れかの方位角推定装置の各部としてコンピュータを機能させるための方位角推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−１２８１１５（Ｐ２０１１−１２８１１５Ａ）
【公開日】平成２３年６月３０日（２０１１．６．３０）
【国際特許分類】
【出願番号】特願２００９−２８９３９３（Ｐ２００９−２８９３９３）
【出願日】平成２１年１２月２１日（２００９．１２．２１）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
【公開日】平成２３年６月３０日（２０１１．６．３０）
【国際特許分類】
【出願日】平成２１年１２月２１日（２００９．１２．２１）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
[ Back to top ]