JP6227950B2

JP6227950B2 - センサの指向制御方法と装置

Info

Publication number: JP6227950B2
Application number: JP2013194153A
Authority: JP
Inventors: 周平江本; 肇坂野
Original assignee: IHI Corp; IHI Aerospace Co Ltd
Current assignee: IHI Corp; IHI Aerospace Co Ltd
Priority date: 2013-09-19
Filing date: 2013-09-19
Publication date: 2017-11-08
Anticipated expiration: 2033-09-19
Also published as: JP2015060430A

Description

本発明は、計測体のカメラやレーザセンサなどの外界センサの指向方向を制御する方法と装置に関する。

複数の物体（例えば移動体）が、互いに独立してエリア内を移動する場合に、複数の移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御などを行うことがある。
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。

移動ロボットの位置を推定する手段として、特許文献１、２が既に開示されている。
また、ロボットのセンサの指向制御手段として、特許文献３〜５が既に開示されている。

特許文献１は、移動ロボット上にカメラやレーザセンサなどの外界センサを搭載する。外界センサによって周囲を計測した結果から、特徴点を抽出し、特徴点の位置と自己位置を同時に推定するものである。
特許文献２は、外界センサを搭載した計測体を複数用いて、各計測体が互いの方位、距離を計測した結果を統合することで、全ての計測体の位置と姿勢を推定するものである。
特許文献３は、既定の写像関数を用いて、アクチュエータ制御座標へ、動作前と動作後のポジションの偏差を写像し、アクチュエータ手段用のコマンドを形成するものである。
特許文献４は、音波を受信する少なくとも２つの音波センサを備え、２つの音波センサ間の音波受信時間差を利用して、ロボットへの音波入射角を計算するものである。
特許文献５は、ロボット経路追従装置のセンサの指向方向をエンドエフェクタ中心軸まわりに逐次制御し、センサの方向を適正に維持するものである。

特開２０１０−２８８１１２号公報特開２０１３−６１３０７号公報特開２００５−３０５６３３号公報特開２００４−２１２４００号公報特開平７−１４１００８号公報

いずれの方法も、外界センサを使って、複数の物体の位置を同時に推定する。推定の計算は、以下のような方法である。
ステップ１：あらかじめ、各物体の位置と姿勢の推定値として適切な初期値を与えておく。また、初期値の誤差の大きさを表す誤差分散行列も設定する。
ステップ２：外界センサを使って、物体間の相対関係（方位と距離）を計測する。
ステップ３：計測結果に基づいて、各物体の推定値を修正し、誤差分散行列を小さくする。この時、誤差分散が大きい推定値の方が修正量が大きくなる。
事前に一部の物体の位置と姿勢が分かっていれば、それを基準に各物体の位置と姿勢が推定され、全体的に誤差分散が小さくなる。

上述の位置推定において、対象物の特徴を正確に抽出するには、外界センサによって、高い分解能で対象を計測する必要がある。しかし、一般的に外界センサは、分解能と視野範囲がトレードオフの関係にある。例えばカメラの場合、レンズの画角が狭いほど分解能が高くなる。レーザセンサの場合、レーザを走査する範囲を狭めた方が高い密度で空間を計測できる。
したがって、位置推定に用いる外界センサは、視野角が９０度以下のカメラやレーザセンサを、パン角とチルト角を制御できる雲台上に搭載する構成となる。

しかし、一般的な外界センサの構成では、全ての物体を視野範囲内に入れることはできない。視野範囲に推定値が収束している物体が無く、自己の推定値も収束していない場合は，上述のステップ２、３を繰り返しても、推定が収束しない。
特許文献１には、以前計測した特徴点の少なくとも一部が視野に残るように、センサの向きを制御する方法が記載されている。しかし、この方法は、移動体の周囲が全て静止体であり、「以前計測した点は誤差分散が小さい」と言える場合にのみ有効である。

周囲に移動体がある場合、過去に計測していても、移動することによって誤差分散が増加している場合がある。このような移動体を計測しても、自己位置の推定には有効ではない。一方で、移動体の位置誤差は徐々に増加していくため、定期的に観測しないと、位置が不明になり、推定値を使う制御動作にも影響を与える。
また、複数の計測体が存在する場合は、他の計測体が観測することで、自分（自己）とは無関係に、周囲の物体の誤差分散が小さくなることもある。このように、自己と、周囲の物体の誤差分散が動的に増減する状況下で、効果的にセンサの向きを制御することが要望されていた。

また、例えば、各物体にＧＰＳを搭載すれば、各物体の正確な位置が取得できるが、ＧＰＳで正確な位置が取得できないような環境（屋内、市街地、森林等）においても、各物体の位置を推定する必要がある。

本発明は上述した問題点を解決するために創案されたものである。すなわち本発明は、カメラやレーザセンサなどの外界センサを備えた計測体の周囲に位置する物体までの距離が離れており、かつ計測体又は物体が移動体であり、計測体と周囲の物体の誤差分散が動的に増減する状況下を想定している。この状況下において本発明の目的は、計測体の外界センサの指向方向を効果的に制御できるセンサの指向制御方法と装置を提供することにある。

本発明によれば、周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体におけるセンサの指向制御方法であって、
前記物体又は前記計測体は、移動体であり、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部により、
（Ａ）自己の位置又は姿勢の推定値を、前記状態推定部に記憶された直前のデータと内界センサの出力から計算し、
自己の周囲の物体の位置又は姿勢の推定値を、予め前記状態推定部に記憶し、又は、自己の周囲の物体の位置又は姿勢の推定値として、前記通信部を介して、その物体の状態推定部に記憶された直前のデータを取得し、
（Ｂ）周囲の物体の前記推定値の推定誤差値を算出し、
（Ｂ１）前記推定誤差値が最も大きい物体、又は、（Ｂ２）前記推定誤差値が最も小さい物体、を注視物体として選択し、
（Ｃ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御方法が提供される。

前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
前記推定誤差値は、位置誤差分散、姿勢誤差分散又は観測誤差範囲である。

前記（Ｂ）において、前記（Ｂ１）と（Ｂ２）を、交互に切り替える。

前記（Ｂ）において、自己の推定値の推定誤差値の大きさを、推定誤差値として許容できる閾値αと比較し、
前記大きさが前記閾値αより小さい場合、周囲の物体のうち前記推定誤差値が最も大きい物体を選択し、
前記大きさが前記閾値αより大きい場合、周囲の物体のうち前記推定誤差値が最も小さい物体を選択する。

また、本発明によれば、周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体に設けられたセンサの指向制御装置であって、
前記物体又は前記計測体は、移動体であり、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部は、
（Ａ）自己の位置又は姿勢の推定値を、前記状態推定部に記憶された直前のデータと内界センサの出力から計算し、
自己の周囲の物体の位置又は姿勢の推定値を、予め前記状態推定部に記憶し、又は、自己の周囲の物体の位置又は姿勢の推定値として、前記通信部を介して、その物体の状態推定部に記憶された直前のデータを取得し、
（Ｂ）周囲の物体の前記推定値の推定誤差値を算出し、
（Ｂ１）前記推定誤差値が最も大きい物体、又は、（Ｂ２）前記推定誤差値が最も小さい物体、を注視物体として選択し、
（Ｃ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御装置が提供される。

上述した本発明の方法と装置によれば、状態推定部により、周囲の物体の位置又は姿勢の推定値の推定誤差値を算出する。次いで、推定誤差値が最も大きい物体（誤差最大物体）、又は、推定誤差値が最も小さい物体（誤差最小物体）を注視物体として選択し、選択した注視物体の方向に雲台のパン角又はチルト角を制御する。
この制御で注視物体が観測されることで、自己又は注視物体の位置又は姿勢が修正される。

誤差最大物体を注視物体として選択することにより、計測体が、推定誤差値（例えば位置誤差分散）が大きくなりつつある物体を優先的に計測するため、位置推定誤差の増加を防げる。
また、誤差最小物体を注視物体として選択することにより、計測体自身の位置を効率よく推定できる。

従って、カメラやレーザセンサなどの外界センサを備えた計測体の周囲に位置する物体までの距離が離れており、かつ計測体又は物体が移動体であり、計測体と周囲の物体の誤差分散が動的に増減する状況下で、計測体の外界センサの指向方向を効果的に制御できる。

本発明の使用例を示す図である。計測体（移動体）の構成図である。移動体（例えば移動ロボット）の構成図である。本発明によるセンサの指向制御方法の全体フロー図である。推定誤差値のイメージ図である。本発明の方法を示す模式図である。

本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明の使用例を示す図である。
この図において、計測体１２は、移動可能な移動体１３又は移動不能な固定体１４である。
図１の例では、所定のエリア内に移動体１３（例えば移動ロボット）や固定体１４（例えば固定デバイス）を配置し、移動体１３と固定体１４の位置を求める。なお、以下、計測体１２（移動体１３又は固定体１４）と計測体１２以外の固定体（例えばランドマーク１５）を、区別が必要な場合を除き単に物体１０と呼ぶ。得られた物体１０の位置は、移動体１３の制御やエリアの地図作成、監視などに用いる。

図２は、計測体１２（この例では移動体１３）の構成図である。
計測体１２は、外界センサ１６、雲台１８、雲台制御装置２０を備える。

外界センサ１６は、他の物体１０の方位又は距離を計測可能なセンサである。
この例において、外界センサ１６は、カメラ１６ａとＬＲＦ１６ｂ（レーザセンサ）からなる。なおカメラ１６ａとＬＲＦ１６ｂの両方を計測体１２に搭載することが好ましいが、一方のみでもよい。

カメラ１６ａにより、周囲の物体１０のある方位を方位角と仰角として計測することができる。なお方位角と仰角は一般的な極座標表現と同じ定義である。物体１０には、カメラ１６ａで検出するためのマーカ１０ａを設置するのが好ましい。
ＬＲＦ１６ｂにより、周囲の物体１０までの距離を計測することができる。また、ＬＲＦ１６ｂにより、距離だけでなく他の物体１０の方位も計測できる。

雲台１８は、外界センサ１６を載せ、水平方向のパン角と鉛直方向のチルト角の２軸を調節できるようになっている。

雲台制御装置２０は、雲台１８のパン角又はチルト角を制御する。

この図に示すように、計測体１２（移動体１３又は固定体１４）は、外界センサ１６を載せた雲台１８を有する。

上述した構成により、パン角を動かすことで外界センサ１６（カメラ１６ａとＬＲＦ１６ｂ）の向きを水平方向に変えられる。またチルト角を動かすことで、カメラ１６ａとＬＲＦ１６ｂの向きを鉛直方向に変えられる。なお、カメラ１６ａとＬＲＦ１６ｂを別々の雲台１８に搭載し、独立して動かしても良い。

図２において、計測体１２は移動体１３（例えば移動ロボット）であるが、移動不能な固定体１４であってもよい。

図３は、移動体１３（例えば移動ロボット）の構成図である。
この図において、上述した雲台制御装置２０は、雲台駆動部２０ａと雲台制御部２０ｂからなる。また、移動体１３は、上述した外界センサ１６、雲台制御装置２０の他に、内界センサ２２、ロボット駆動部２４、及び車両制御部２６を有する。

内界センサ２２は、この例では、ジャイロセンサ２２ａ（移動体１３の加速度、角速度を計測するセンサ）と車輪エンコーダ２２ｂ（移動体１３の速度、角速度を計測するセンサ）である。
外界センサ１６及び内界センサ２２の検出データは、それぞれの計測部を介して状態推定部３４（後述する）に入力される。

本発明のセンサの指向制御装置３０は、計測体１２（この例では移動体１３）に設けられており、通信部３２と状態推定部３４とを備える。
通信部３２は、その周囲の物体１０と通信可能であり、好ましくはリアルタイムに無線ＬＡＮで通信可能に構成されている。
状態推定部３４は、外界センサ１６及び内界センサ２２で計測されたセンサ情報を使って、自己（その計測体１２）及びその周囲の物体１０の推定値Ａ（後述する）を取得する。
指向制御装置３０は、例えばコンピュータ（ＰＣ）で構成されている。また、上述した雲台制御部２０ｂ、及び車両制御部２６を含めて同一のコンピュータ（ＰＣ）で構成してもよい。

状態推定部３４は、車両制御部２６及び雲台制御部２０ｂにそれぞれ制御信号を出力する。車両制御部２６は指令値を算出してロボット駆動部２４に出力し、ロボット駆動部２４は車輪エンコーダ２２ｂを制御する。同様に雲台制御部２０ｂは指令値を算出して雲台駆動部２０ａに出力し、雲台駆動部２０ａは雲台１８のパン角又はチルト角を制御するようになっている。
また、状態推定部３４は、後述するセンサの指向制御方法を実行する。

図４は、本発明によるセンサの指向制御方法の全体フロー図である。
この図に示すように本発明の指向制御方法は、Ｓ１〜Ｓ６の各ステップ（工程）からなる。
以下、特に必要な場合を除き、自己（観測元の計測体１２）を「デバイスｉ」又は「観測元ｉ」と呼び、周囲の物体１０（観測対象）を「デバイスｊ」又は「観測対象ｊ」と呼ぶ。
なお必要に応じて、観測対象の別の物体１０を「デバイスｋ」と呼ぶ。ｉ、ｊ、ｋは、１，２，３・・・の正数であり、デバイスｉ、デバイスｊ、デバイスｋは異なる物体１０を意味する。

ステップＳ１では、自己及びその周囲の物体１０の位置又は姿勢の推定値Ａを取得する。この手段は、特許文献１、２などによる。
通常、各物体１０がエリア内に配置された時点で大まかな位置又は姿勢は分かっている。そのため、自己及びその周囲の物体１０の初期値は、例えば位置誤差２ｍ、姿勢誤差２度程度の精度で設定する。
自己（観測元の計測体１２）の位置又は姿勢の推定値Ａは、状態推定部３４に記憶された直前のデータと内界センサ２２の出力から計算される。その周囲の物体１０（計測体１２）の位置又は姿勢の推定値Ａは、通信部３２を介して、その物体の状態推定部３４に記憶された直前のデータを取得する。また、周囲の物体１０が移動不能な固定体１４又は計測体１２以外の固定体（例えばランドマーク１５）の場合には、その位置又は姿勢を予め状態推定部３４に記憶しておく。

周囲の物体１０とは、状態推定部３４にある推定結果のうち、外界センサ１６で計測できる距離範囲内にいると予測される物体１０とする。
推定値Ａとは、位置と姿勢を示す状態量Ｘｉと、状態量Ｘｉの誤差分散行列ＣＸｉｉである。推定値Ａは、時間経過と共に常に更新されている。
また、移動体１３の場合、状態量Ｘｉに速度や角速度などの運動成分を含めてもよい。

デバイスｉの現在の推定値Ａ（状態量Ｘｉと誤差分散行列ＣＸｉｉ）は、数１の式（１）（２）で求められる。また、デバイスｉの状態量Ｘｉとデバイスｊの状態量Ｘｊとの共分散行列ＣＸｉｊも状態推定部３４から取得できる。
式（１）において、（ｘ，ｙ，ｚ）は位置を表し、（ｒｘ，ｒｙ，ｒｚ）は姿勢を表す。

ステップＳ２では、周囲の物体１０の推定値Ａの推定誤差値Ｂを算出する。
推定誤差値Ｂは、位置誤差分散Ｂ１、姿勢誤差分散Ｂ２又は観測誤差範囲Ｂ３である。また、誤差推定値ＢをＢ１，Ｂ２，Ｂ３から唯一つを選ぶのではなく、重みパラメータＷ１，Ｗ２，Ｗ３で重み付けをしてＷ１×Ｂ１＋Ｗ２×Ｂ２＋Ｗ３×Ｂ３など複数の推定誤差値を併用して決定してもよい。

図５は、推定誤差値Ｂのイメージ図である。
この図において、デバイスｉ、デバイスｊ、デバイスｋは、３つの異なる物体１０である。３つの異なる物体１０のうち少なくとも１つは、計測体１２である。計測体１２は移動体１３であっても固定体１４であってもよい。
図中の楕円（破線で示す）は、デバイスｉ、デバイスｊ、デバイスｋの位置誤差範囲を示している。また、図中の角度βは、観測誤差範囲を示している。

位置誤差分散Ｂ１は、誤差分散行列ＣＸｉｉの位置成分の対角項の和（数２の式（３））又は、誤差分散行列ＣＸｉｉの位置成分のみの行列式（４）で求められる。

姿勢誤差分散Ｂ２は、誤差分散行列ＣＸｉｉの姿勢成分の対角項の和（数３の式（５））又は、誤差分散行列ＣＸｉｉの姿勢成分のみの行列式（６）で求められる。

観測誤差範囲Ｂ３は、数４の式（７）の観測誤差分散Ｖｉｊの対角項の和、又は行列式で求められる。
ここで、Ｂｎは、観測方程式ｇを、観測元ｉ（自分）の状態量Ｘｉ、観測対象ｊの状態量Ｘｊで偏微分した行列であり、式（８）で与えられる。
また、観測方程式ｇは、式（９）〜（１１）で求められる。
ここで、Ｃ_ｎ ^ｂｉは、全体座標系からデバイスｉのローカル座標系に変換する行列であり、デバイスｉの状態量Ｘｉの姿勢成分から式（１２）により算出する。
なお、式（１２）では、ｃｏｓ（ｘ）＝ｃ（ｘ）、ｓｉｎ（ｘ）＝ｓ（ｘ）と略記している。

ステップＳ３では、注視物体１１を選択する。注視物体１１は、周囲の物体１０のなかから選択される。
注視物体１１として、推定誤差値Ｂが最も大きい物体１０（「誤差最大物体１１Ａ」と呼ぶ）、又は、推定誤差値Ｂが最も小さい物体１０（「誤差最小物体１１Ｂ」と呼ぶ）を選択する。この選択時に用いる推定誤差値Ｂは、位置誤差分散Ｂ１、姿勢誤差分散Ｂ２、又は観測誤差範囲Ｂ３であるのがよい。

また、誤差最大物体１１Ａと誤差最小物体１１Ｂを一定時間毎に切り替えて注視物体１１として選択してもよい。
また、自己の推定値Ａの推定誤差値Ｂの大きさＣを、推定誤差値Ｂとして許容できる閾値αと比較し、大きさＣが閾値αより小さい場合、周囲の物体１０のうち推定誤差値Ｂが最も大きい物体１０（誤差最大物体１１Ａ）を選択し、大きさＣが閾値αより大きい場合、周囲の物体１０のうち推定誤差値Ｂが最も小さい物体１０（誤差最小物体１１Ｂ）を選択してもよい。

ステップＳ４では、パン角指令値θ_ｏｐｅとチルト角指令値φ_ｏｐｅを算出する。
自分（自己）から見た注視物体１１の方向（θ，φ）を、前述の観測方程式ｇの第２，第３行で求め、これをパン角指令値θ_ｏｐｅとチルト角指令値φ_ｏｐｅとする。

ステップＳ５では、雲台１８のパン角又はチルト角を制御する。
パン角指令値θ_ｏｐｅとチルト角指令値φ_ｏｐｅに基づいて、以下のように雲台１８を制御する。
θ軸は、数５の式（１３）の範囲で動かす。
φ軸は式（１４）の範囲で動かす。
ここで、Ｖｔｈ，Ｖｐｈは、式（１５）で示す観測誤差分散Ｖｉｊの対角項である。観測誤差分散Ｖｉｊは式（７）で求める。式（１５）は、観測誤差分散Ｖｉｊの対角項に記号を定義したものである。
また、ｎは観測対象ｊの探索範囲を決める係数である。ｎを０にすると、予測した方向の一点を向く。

式（１５）において、ＶＬは「距離の観測誤差範囲」を意味する。
式（７）を定義どおりに計算すると、観測誤差分散Ｖｉｊは３×３の行列となる。このうち、ＶｔｈやＶｐｈは雲台の往復動作の幅を決めるのに用いる。また、ＶＬは、式（８）の観測誤差範囲Ｂ３を使う時に用いる。

ステップＳ６では、ステップＳ３で選択した注視物体１１が観測された後に、或いは一定時間経過後に、ステップＳ１に戻る。
ステップＳ３で選択した注視物体１１が観測された後にステップＳ１に戻ることが好ましい。注視物体１１が観測されることで、注視物体１１の位置又は姿勢が修正される。
しかし、ステップＳ３の選択にて、注視物体１１が毎回変わるような状況では、目標指令値θ_ｏｐｅ，φ_ｏｐｅがループ毎に変わり制御が不安定になる場合がある。この場合には、一定時間の間、ステップＳ５の制御を継続した後にステップＳ１に戻る。
なお、一定時間経過の代わりに、注視物体１１に関する推定誤差値Ｂが、ステップＳ３で選択した時より減る（又は増える）まで、ステップＳ５の制御を継続してもよい。

また、ステップＳ５の制御を一定時間継続しても、注視物体１１が観測されない場合は、推定誤差が大きいことで予測した方向に対象が存在しない可能性がある。このような場合は、全周囲に渡って視線方向を回すような動作を加えても良い。

図６は、本発明の方法を示す模式図である。
この図において、デバイスｉが、誤差最大物体１１Ａとしてデバイスｊを検出し、誤差最小物体１１Ｂとしてランドマーク１５を検出している。
上述した本発明の方法により以下の効果が得られる。なお、以下、推定誤差値Ｂが位置誤差分散Ｂ１の場合を説明するが、推定誤差値Ｂが、姿勢誤差分散Ｂ２又は観測誤差範囲Ｂ３である場合も同様である。

従来技術に示すとおり、観測されない物体１０（特に移動体１３）は、推定誤差値Ｂ（位置誤差分散Ｂ１）が徐々に増加する。
ステップＳ３で、注視物体１１として、推定誤差値Ｂが最も大きい物体１０（誤差最大物体１１Ａ）を選択した場合、ステップＳ４〜Ｓ６において、選択した注視物体１１（誤差最大物体１１Ａ）が観測されるように自己の雲台１８を制御する。
したがって、自己（計測体１２）が、位置誤差分散Ｂ１が大きくなりつつある物体１０を優先的に計測し修正するため、誤差最大物体１１Ａの位置誤差分散Ｂ１の増加を防ぐことができる。

計測体１２自身の位置と姿勢を推定する場合、推定誤差値Ｂ（位置誤差分散Ｂ１）が小さい物体１０を観測した方が効率的である。ステップＳ３で、注視物体１１として、推定誤差値Ｂが最も小さい物体１０（誤差最小物体１１Ｂ）を選択した場合、ステップＳ４〜Ｓ６において、選択した注視物体１１（誤差最小物体１１Ｂ）が観測されるように自己の雲台１８を制御する。したがって、計測体１２自身の位置と姿勢を効率よく推定できる。
特許文献１では，以前計測した点が視野に残るように制御している。これに対し本発明では、注視物体１１として、推定誤差値Ｂが最も小さい物体１０（誤差最小物体１１Ｂ）を選択することにより、周囲の物体１０の誤差分散が動的に変わっている状況下で、最も適した物体１０を選択できる。

ステップＳ３で、注視物体１１として、誤差最大物体１１Ａと誤差最小物体１１Ｂを組み合わせることで、特定の物体１０の位置誤差が過大になる状態を防ぎつつ，各計測体１２の位置と姿勢を効率よく推定できる。

またステップＳ３で、注視物体１１として、推定誤差値Ｂの大きさＣを、推定誤差値Ｂとして許容できる閾値αと比較し、大きさＣが閾値αより小さい場合、誤差最大物体１１Ａを選択する。また、大きさＣが閾値αより大きい場合、誤差最小物体１１Ｂを選択する。これにより、計測体１２の姿勢（または位置）の推定誤差の大きさに応じて、誤差最大物体１１Ａと誤差最小物体１１Ｂを切り替えられる。例えば，移動していない計測体１２は、推定値Ａの誤差分散が増加しにくい。したがって、この場合は、誤差最小物体１１Ｂの選択は不要となる場合が多くなり、適切に動作を切り替えるので、効率よく推定できる。

上述した本発明の方法によれば、周囲の物体１０の推定値Ａの推定誤差値Ｂを算出する。次いで、推定誤差値Ｂが最も大きい物体１０（誤差最大物体１１Ａ）、又は、推定誤差値Ｂが最も小さい物体１０（誤差最小物体１１Ｂ）を注視物体１１として選択し、選択した注視物体１１の方向に雲台１８のパン角又はチルト角を制御する。

誤差最大物体１１Ａを注視物体１１として選択することにより、計測体１２が、位置誤差分散Ｂ１が大きくなりつつある物体１０を優先的に計測するため、位置誤差分散Ｂ１の増加を防げる。
また、誤差最小物体１１Ｂを注視物体１１として選択することにより、計測体１２の位置と姿勢を効率よく推定できる。

本発明は、カメラ１６ａやレーザセンサ１６ｂなどの外界センサ１６を備えた計測体１２の周囲に位置する物体１０までの距離が離れており、かつ計測体１２又は物体１０が移動体１３であり、計測体１２と周囲の物体１０の誤差分散が動的に増減する状況下を想定している。
本発明によれば、この状況下において、計測体１２に設けられたカメラ１６ａやレーザセンサ１６ｂなどの外界センサ１６の指向方向を効果的に制御できる。

なお、１台の計測体１２が、２組の外界センサ１６を独立して動かす２台の雲台１８を有している場合、一方の雲台１８を誤差最大物体１１Ａに対して制御し、他方の雲台１８を誤差最小物体１１Ｂに対して制御してもよい。このように２組の外界センサ１６と雲台１８の役割を分担させることにより、周囲の物体１０の観測と、自己の位置と姿勢の推定を両立させることができる。

なお本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。

Ａ推定値、Ｂ推定誤差値、Ｂ１位置誤差分散、Ｂ２姿勢誤差分散、
Ｂ３観測誤差範囲、ｇ観測方程式、Ｘｉ、Ｘｊ状態量、
ＣＸｉｉ誤差分散行列、ＣＸｉｊ共分散行列、Ｖｉｊ観測誤差分散、
θ_ｏｐｅパン角指令値、φ_ｏｐｅチルト角指令値、１０物体、
１０ａマーカ、１１注視物体、１１Ａ誤差最大物体、
１１Ｂ誤差最小物体、１２計測体、１３移動体、１４固定体、
１５ランドマーク、１６外界センサ、１６ａカメラ、
１６ｂＬＲＦ（レーザセンサ）、１８雲台、２０雲台制御装置、
２０ａ雲台駆動部、２０ｂ雲台制御部、２２内界センサ、
２２ａジャイロセンサ、２２ｂ車輪エンコーダ、２４ロボット駆動部、
２６車両制御部、３０指向制御装置、３２通信部、３４状態推定部

Claims

周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体におけるセンサの指向制御方法であって、
前記物体又は前記計測体は、移動体であり、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部により、
（Ａ）自己の位置又は姿勢の推定値を、前記状態推定部に記憶された直前のデータと内界センサの出力から計算し、
自己の周囲の物体の位置又は姿勢の推定値を、予め前記状態推定部に記憶し、又は、自己の周囲の物体の位置又は姿勢の推定値として、前記通信部を介して、その物体の状態推定部に記憶された直前のデータを取得し、
（Ｂ）周囲の物体の前記推定値の推定誤差値を算出し、
（Ｂ１）前記推定誤差値が最も大きい物体、又は、（Ｂ２）前記推定誤差値が最も小さい物体、を注視物体として選択し、
（Ｃ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御方法。
前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
前記推定誤差値は、位置誤差分散、姿勢誤差分散又は観測誤差範囲である、ことを特徴とする請求項１に記載のセンサの指向制御方法。
前記（Ｂ）において、前記（Ｂ１）と（Ｂ２）を、交互に切り替える、ことを特徴とする請求項１又は２に記載のセンサの指向制御方法。
前記（Ｂ）において、自己の推定値の推定誤差値の大きさを、推定誤差値として許容できる閾値αと比較し、
前記大きさが前記閾値αより小さい場合、周囲の物体のうち前記推定誤差値が最も大きい物体を選択し、
前記大きさが前記閾値αより大きい場合、周囲の物体のうち前記推定誤差値が最も小さい物体を選択する、ことを特徴とする請求項１又は２に記載のセンサの指向制御方法。
周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体に設けられたセンサの指向制御装置であって、
前記物体又は前記計測体は、移動体であり、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部は、
（Ａ）自己の位置又は姿勢の推定値を、前記状態推定部に記憶された直前のデータと内界センサの出力から計算し、
自己の周囲の物体の位置又は姿勢の推定値を、予め前記状態推定部に記憶し、又は、自己の周囲の物体の位置又は姿勢の推定値として、前記通信部を介して、その物体の状態推定部に記憶された直前のデータを取得し、
（Ｂ）周囲の物体の前記推定値の推定誤差値を算出し、
（Ｂ１）前記推定誤差値が最も大きい物体、又は、（Ｂ２）前記推定誤差値が最も小さい物体、を注視物体として選択し、
（Ｃ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御装置。