JP6319736B2

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

Info

Publication number: JP6319736B2
Application number: JP2013194154A
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: 2018-05-09
Anticipated expiration: 2033-09-19
Also published as: JP2015060431A

Description

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

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

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

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

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

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

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

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

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

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

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

本発明によれば、周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体におけるセンサの指向制御方法であって、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部により、
（Ａ）自己及びその周囲の物体の位置又は姿勢の推定値を取得し、
前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
（Ｂ）周囲の物体の前記推定値のカルマンゲインを算出し、
前記カルマンゲインは、観測対象の位置に関する第１修正ゲイン、観測元の位置に関する第２修正ゲイン、観測元の姿勢に関する第３修正ゲイン、又は、これらを統合した統合修正ゲインであり、
前記第１修正ゲインは、観測対象ｊのカルマンゲインＫｊの位置成分の二乗和であり、
前記第２修正ゲインは観測元ｉのカルマンゲインＫｉの位置成分の二乗和であり、
前記第３修正ゲインは、観測元ｉのカルマンゲインＫｉの姿勢成分の二乗和であり、
前記統合修正ゲインはカルマンゲインＫｉ，Ｋｊの各成分の二乗和であり、
カルマンゲインＫｉ，Ｋｊは、数９の式（１０）（１１）で算出され、

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御方法が提供される。

前記統合修正ゲインは、第１修正ゲインと第２修正ゲインの和に、前記計測体と周囲の物体の距離、第３修正ゲイン、及び姿勢成分の重み係数βの積を加算した値である。

前記（Ｃ）において、カルマンゲインとして第１修正ゲイン、第２修正ゲイン、及び第３修正ゲインを、交互に切り替える。

また、本発明によれば、周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体に設けられたセンサの指向制御装置であって、
周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部は、
（Ａ）自己及びその周囲の物体の位置又は姿勢の推定値を取得し、
前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
（Ｂ）周囲の物体の前記推定値のカルマンゲインを算出し、
前記カルマンゲインは、観測対象の位置に関する第１修正ゲイン、観測元の位置に関する第２修正ゲイン、観測元の姿勢に関する第３修正ゲイン、又は、これらを統合した統合修正ゲインであり、
前記第１修正ゲインは、観測対象ｊのカルマンゲインＫｊの位置成分の二乗和であり、
前記第２修正ゲインは観測元ｉのカルマンゲインＫｉの位置成分の二乗和であり、
前記第３修正ゲインは、観測元ｉのカルマンゲインＫｉの姿勢成分の二乗和であり、
前記統合修正ゲインはカルマンゲインＫｉ，Ｋｊの各成分の二乗和であり、
カルマンゲインＫｉ，Ｋｊは、数９の式（１０）（１１）で算出され、

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御装置が提供される。

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

カルマンゲインは、推定値の修正量に比例する係数であり、この値が大きい方が推定値の修正量が大きくなる。したがって、カルマンゲインが最も大きい物体（ゲイン最大物体）を注視物体として選択することで、推定値を効率よく収束させることができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

計測体が物体を観測するたびに、状態量Ｘｉと分散を更新する。
状態量Ｘｉの修正式は、［数２］の（４）で示すことができる。また、分散の更新式は、［数２］の（５）で示すことができる。

ＸやＣｏｖＸの定義は、数３の式（６）〜（９）で示すことができる。したがって、各物体の誤差分散行列ＣＸｉｉと共分散行列ＣＸｉｊは、式（５）で更新される。なお物体の数に応じて、数３の次数は変化する。

ステップＳ２では、周囲の物体１０の推定値ＡのカルマンゲインＫを計算する。

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

観測元ｉのカルマンゲインＫｉと観測対象ｊのカルマンゲインＫｊは数４の式（１０）（１１）で算出される。
ここで、Ｒは、距離、方位角、仰角の計測誤差として想定される観測誤差分散値である。
また、カルマンゲインＫｉの各成分を式（１２）のように表記する（カルマンゲインＫｊも同様である）。
なお、以下、「カルマンゲインＫ」は、カルマンゲインＫｉ又はカルマンゲインＫｊを意味する。

例えば、観測元ｉで観測された距離（Ｌ）、方位（θ，φ）と、現在の推定値Ａとの差がΔＬ，Δθ，Δφだった時、カルマンフィルタ理論では、状態量Ｘｉの修正量を数５の式（１３）で算出する。
式（１３）から、カルマンゲインＫｉの各成分が大きいほど、状態量Ｘｉの修正量が大きくなり、状態量Ｘｉが修正されやすいことがわかる。

本発明では、周囲の物体１０の推定値ＡのカルマンゲインＫは、観測対象ｊの位置に関する第１修正ゲインＥ１、観測元ｉの位置に関する第２修正ゲインＥ２、観測元ｉの姿勢に関する第３修正ゲインＥ３、又は、これらを統合した統合修正ゲインＥ_ａｌｌである。
統合修正ゲインＥ_ａｌｌは、第１修正ゲインＥ１と第２修正ゲインＥ２の和に、計測体１２と周囲の物体１０の距離、第３修正ゲインＥ３、及び姿勢成分の重み係数βの積を加算した値である。

第１修正ゲインＥ１、第２修正ゲインＥ２、及び第３修正ゲインＥ３は、数６の式（１４）で与えられる。
また統合修正ゲインＥ_ａｌｌは、式（１５）で与えられる。
ただし、式（１６）の関係がある。
その他ｙｊ〜ｒｚｉも同様に、カルマンゲイン行列（式（１２））の対応する一行の和である。
なおＥ_ａｌｌの式で、より正確には、ＣはＣ_ｎ ^ｂｉを指す。
またＰ_ｉｊは、「観測元ｉから見た観測対象ｊの位置ベクトル」を意味する。

式（１０）（１１）から明らかなように、カルマンゲインＫ（Ｋｉ又はＫｊ）は、推定値Ａの誤差分散ＣＸを、観測誤差分散Ｖｉｊと観測誤差分散値Ｒの和で割った値である。
観測誤差分散Ｖｉｊは、「観測元ｉと観測対象ｊの誤差分散の和」を意味する。
したがって、カルマンゲインＫは数７の式（１７）（１８）のように近似できる。なお、この式は、観測行列Ｂｎや、共分散行列ＣＸｉｊを無視した近似だが、近似値が増減する傾向は、カルマンゲインＫと同じである。

したがって、上述した修正ゲインＥ１〜Ｅ３、Ｅ_ａｌｌは、推定値Ａの誤差分散行列ＣＸｉｉの大きさに応じて、以下のように増減する。

（１）観測対象ｊの位置に関する第１修正ゲインＥ１
式（１４）から、第１修正ゲインＥ１は、観測対象ｊのカルマンゲインＫｊ（式（１２）と同様）の位置（ｘ，ｙ，ｚ）成分の二乗和である。したがって、観測元ｉの誤差分散行列ＣＸｉｉが小さく、観測対象ｊの誤差分散行列ＣＸｊｊが大きい時に、大きな値となる。
逆に、観測元ｉの誤差分散行列ＣＸｉｉが大きく、観測対象ｊの誤差分散行列ＣＸｊｊが小さい時は、小さな値となる。

（２）観測元ｉの位置に関する第２修正ゲインＥ２と、観測元ｉの姿勢に関する第３修正ゲインＥ３
式（１４）から、第２修正ゲインＥ２は観測元ｉのカルマンゲインＫｉ（式（１２）と同様）の位置（ｘ，ｙ，ｚ）成分の二乗和である。
同様に式（１４）から、第３修正ゲインＥ３は、観測元ｉのカルマンゲインＫｉの姿勢（ｒｘ，ｒｙ，ｒｚ）成分の二乗和である。
したがって、観測元ｉの誤差分散行列ＣＸｉｉが大きく、観測対象ｊの誤差分散行列ＣＸｊｊが小さい時に、大きな値となる。
逆に、観測元ｉの誤差分散行列ＣＸｉｉが小さく、観測対象ｊの誤差分散行列ＣＸｊｊが大きい時は、小さな値となる。

（３）これらを統合した統合修正ゲインＥ_ａｌｌ
統合修正ゲインＥ_ａｌｌは、観測元ｉと観測対象ｊの全てのカルマンゲインＫ（Ｅ１，Ｅ２，Ｅ３）の重み付き二乗和である。すなわち、統合修正ゲインＥ_ａｌｌはＫｉ，Ｋｊの各成分の二乗和である。したがって、表１のような傾向となる。

したがって、統合修正ゲインＥ_ａｌｌが最も大きくなる注視物体１１を選ぶことで、ケース１、２に該当する対象が優先的に選ばれる。第１修正ゲインＥ１、第２修正ゲインＥ２を組み合わせた効果が期待できる。

図４に戻り、ステップＳ３では、カルマンゲインＫが最も大きい物体１０を注視物体１１として選択する。注視物体１１は、周囲の物体１０から選択される。
カルマンゲインＫは、第１修正ゲインＥ１、第２修正ゲインＥ２、第３修正ゲインＥ３、又は統合修正ゲインＥ_ａｌｌであるのがよい。

また、カルマンゲインＫとして第１修正ゲインＥ１、第２修正ゲインＥ２、及び第３修正ゲインＥ３を、一定時間毎に切り替えてもよい。

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

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

式（２１）において、ＶＬは「距離の観測誤差範囲」を意味する。
観測誤差分散Ｖｉｊは３×３の行列となる。このうち、ＶｔｈやＶｐｈは雲台の往復動作の幅を決めるのに用いる。

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

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

図６は、本発明の方法を示す模式図である。
この図において、デバイスｉが、第１修正ゲインＥ１のゲイン最大物体としてデバイスｊを検出し、第２修正ゲインＥ２のゲイン最大物体としてランドマーク１５を検出している。

カルマンゲインＫは、推定値Ａの修正量に比例する係数であり、この値が大きい方が推定値Ａの修正量が大きくなる。したがって、カルマンゲインＫが大きくなるように対象を選択することで、推定値Ａを効率よく収束させることができる。本発明では、カルマンゲインＫの特定の成分を検証することで、以下のような効果がある。

従来技術に示すとおり、観測されない物体１０（特に移動体１３）は、推定誤差値Ｂ（位置誤差分散Ｂ１）が徐々に増加する。このような物体１０は、観測対象ｊとなった時の位置のカルマンゲインＫ（第１修正ゲインＥ１）が大きくなる。
ステップＳ３で、注視物体１１として、観測対象ｊの位置に関する第１修正ゲインＥ１を選択した場合、ステップＳ４〜Ｓ６において、計測体１２が、位置誤差分散Ｂ１が大きくなりつつある物体１０（観測対象ｊ）を優先的に計測する。このため、位置誤差分散Ｂ１の増加を防げる。

ステップＳ３で、注視物体１１として、観測元ｉの位置に関する第２修正ゲインＥ２又は観測元ｉの姿勢に関する第３修正ゲインＥ３を選択した場合、計測体１２（観測元ｉ）の推定値Ａの修正量が大きくなるような対象（観測対象ｊ）を選択する。このため、計測体１２自身（観測元ｉ）の位置と姿勢を効率よく推定できる。
特許文献１では、以前計測した点が視野に残るように制御している。これに対し本発明では、注視物体１１として、第２修正ゲインＥ２又は第３修正ゲインＥ３を選択することにより、周囲の物体１０の誤差分散が動的に変わっている状況下で、最も適した物体１０を選択できる。

ステップＳ３で、カルマンゲインＫとして第１修正ゲインＥ１、第２修正ゲインＥ２、及び第３修正ゲインＥ３を、交互に切り替える。これにより、特定の物体１０（観測対象ｊ）の位置誤差が過大になる状態を防ぎつつ、各計測体１２（観測元ｉ）の位置と姿勢を効率よく推定できる。

統合修正ゲインＥ_ａｌｌは、第１修正ゲインＥ１、第２修正ゲインＥ２で使っている指標値を足し合わせたものである。位置誤差分散Ｂ１が増加している物体１０（観測対象ｊ）が周囲にあれば、第１修正ゲインＥ１が増加することで、注視物体１１に選択される。それ以外は、自己（観測元ｉ）の位置と姿勢を効率よく推定できる対象（観測対象ｊ）が選ばれる。したがって、周囲の物体１０の誤差分散と、計測体１２自身の誤差分散の大きさを総合的に考慮した注視物体１１を選べる。

上述した本発明の方法によれば、状態推定部３４により、周囲の物体１０の位置又は姿勢の推定値Ａの推定誤差値ＢとカルマンゲインＫを算出する。次いで、カルマンゲインＫが最も大きい物体１０（ゲイン最大物体）を注視物体１１として選択し、選択した注視物体１１の方向に雲台１８のパン角又はチルト角を制御する。

カルマンゲインＫは、推定値Ａの修正量に比例する係数であり、この値が大きい方が推定値Ａの修正量が大きくなる。したがって、カルマンゲインＫが最も大きい物体１０（ゲイン最大物体）を注視物体１１として選択することで、推定値Ａを効率よく収束させることができる。

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

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

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

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

Claims

周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体におけるセンサの指向制御方法であって、
前記計測体は、その周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部により、
（Ａ）自己及びその周囲の物体の位置又は姿勢の推定値を取得し、
前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
（Ｂ）周囲の物体の前記推定値のカルマンゲインを算出し、
前記カルマンゲインは、観測対象の位置に関する第１修正ゲイン、観測元の位置に関する第２修正ゲイン、観測元の姿勢に関する第３修正ゲイン、又は、これらを統合した統合修正ゲインであり、
前記第１修正ゲインは、観測対象ｊのカルマンゲインＫｊの位置成分の二乗和であり、
前記第２修正ゲインは観測元ｉのカルマンゲインＫｉの位置成分の二乗和であり、
前記第３修正ゲインは、観測元ｉのカルマンゲインＫｉの姿勢成分の二乗和であり、
前記統合修正ゲインはカルマンゲインＫｉ，Ｋｊの各成分の二乗和であり、
カルマンゲインＫｉ，Ｋｊは、数９の式（１０）（１１）で算出され、

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御方法。
前記統合修正ゲインは、第１修正ゲインと第２修正ゲインの和に、前記計測体と周囲の物体の距離、第３修正ゲイン、及び姿勢成分の重み係数βの積を加算した値である、ことを特徴とする請求項１に記載のセンサの指向制御方法。
前記（Ｃ）において、カルマンゲインとして第１修正ゲイン、第２修正ゲイン、及び第３修正ゲインを、交互に切り替える、ことを特徴とする請求項１に記載のセンサの指向制御方法。
周囲に位置する物体の方位又は距離を計測可能な外界センサと、該外界センサを載せた雲台と、該雲台のパン角又はチルト角を制御する雲台制御装置と、を有する計測体に設けられたセンサの指向制御装置であって、
周囲の物体と通信可能な通信部と、前記雲台制御装置に指令値を出力する状態推定部とを備え、前記状態推定部は、
（Ａ）自己及びその周囲の物体の位置又は姿勢の推定値を取得し、
前記推定値は、位置と姿勢を示す状態量と、該状態量の誤差分散行列であり、
（Ｂ）周囲の物体の前記推定値のカルマンゲインを算出し、
前記カルマンゲインは、観測対象の位置に関する第１修正ゲイン、観測元の位置に関する第２修正ゲイン、観測元の姿勢に関する第３修正ゲイン、又は、これらを統合した統合修正ゲインであり、
前記第１修正ゲインは、観測対象ｊのカルマンゲインＫｊの位置成分の二乗和であり、
前記第２修正ゲインは観測元ｉのカルマンゲインＫｉの位置成分の二乗和であり、
前記第３修正ゲインは、観測元ｉのカルマンゲインＫｉの姿勢成分の二乗和であり、
前記統合修正ゲインはカルマンゲインＫｉ，Ｋｊの各成分の二乗和であり、
カルマンゲインＫｉ，Ｋｊは、数９の式（１０）（１１）で算出され、

ここで、ＣＸｉｉは状態量Ｘｉの誤差分散行列であり、ＣＸｉｊは共分散行列であり、Ｖｉｊは、観測誤差分散であり、Ｒは観測誤差分散値であり、Ｂｉ，Ｂｊは観測行列であり、
（Ｃ）前記カルマンゲインが最も大きい物体を注視物体として選択し、
（Ｄ）選択した注視物体の方向に前記雲台のパン角又はチルト角を制御する、ことを特徴とするセンサの指向制御装置。