JP2007530978A - 反射光源を使用する位置推定方法および装置 - Google Patents

Abstract

本発明は、反射される光源（２０４、２０５）を用いて、ローカルまたはグローバル座標系に対する物体（２０１）の位置および姿勢の推定に概ね関する。本方法および装置の一般的な応用には、自律型移動ロボットの位置の推定および追跡が含まれる。他の応用には、位置認識、ユビキタスデバイスのための物体の推定および追跡が含まれる。別の応用には、室内環境における人またはペットの位置の追跡が含まれる。本方法および装置は、面（２０６）から反射した、１つまたはそれ以上の放射された光源（２０１、２０５）の信号を検出することに少なくとも部分的に基づいて、複数の光センサの少なくとも１つの光センサの位置および姿勢を測定するように、１つまたはそれ以上の光エミッタ（２０３）と、１つまたはそれ以上の光センサ（２０２）と、信号処理回路構成要素と、信号処理方法とを含む。

Description

関連出願
本願は、２００４年３月２９日付出願の米国仮出願第６０／５５７，２５２号の利益を主張し、その全体が、参照によりここに組み込まれる。

添付書類Ａ
添付書類Ａは、本開示の一部分を形成し、共通の所有者が所有する同時係属米国特許出願の一覧表である。添付書類Ａに挙げられた各同時係属出願は、その全体が、参照によりここに組み込まれる。

本発明は、ローカル座標系またはグローバル座標系に関連して物体の位置および姿勢の推定に概ね関する。特に、本発明は、位置および姿勢を推定し追跡し得る方法および装置に関する。本方法および装置は、移動ロボットなどの車両において使用し得る。

位置の推定は、自律型移動ロボット、ユビキタスコンピューティング、携帯型デバイス、貴重品の追跡、人の追跡、店内の顧客の位置追跡、ペットの追跡、臨時の無線ネットワークのノードの位置、車両の位置追跡、および携帯電話、携帯情報端末などの携帯型デバイスの位置追跡から幅広い様々な応用分野において関心を呼ぶテーマである。

ロボットは、社会においてますます珍しいものではなくなってきている。これらのロボットを、様々な形態で、例えば電気掃除機などの自動床手入れ製品において、実施し得ることが、理解されるだろう。限定はしないが、移動ロボットへの様々な応用が見つかり、それらの応用は、おもちゃのロボットなどのエンターテイメントへの応用、高齢者のお世話ロボットなどの健康管理への応用、パトロールおよび安全への応用、テレプレゼンスロボット、床掃除ロボットなどの掃除への応用、宇宙、深海、低温、放射能、化学物質への露出、バイオハザードなどの人間に有害な環境、爆発の恐れのある物を無害にするなどの危険な任務、崩壊したビルなどの閉じ込められた空間内における作業、および掃除などの人がやりたがらない仕事の実行における設備への応用などである。移動ロボット、一つの場所から別の場所へ移動し得るロボットは、その環境に対する自分の位置の情報を用いることが多い。

自己位置同定技術とは、ロボットが、基準座標系に対する位置および姿勢を測定するプロセスを指す。基準座標系は、（例えば、物体に対して）ローカル、またはグローバルのいずれかであり得る。位置の推定には、物体の三次元（３−Ｄ）六自由度の少なくともいくつかに関連する量を推定することを含み得る。これらの六自由度は、物体の（ｘ，ｙ，ｚ）位置、および３−Ｄ座標系の各軸線周りにおける回転角度として記述可能であって、その角度は、α、β、θと示され、それぞれ「ピッチ」、「ロール」、「ヨー」と呼ばれる。このような位置の推定は、様々なタスクおよび応用に有用となり得る。例えば、充電ステーションに対するロボットの方位は、ロボットが充電ステーションへサーボ制御され電池を自律的に再充電し得ることに役立ち得る。正面玄関からのペットの距離の推定を用いて、飼い主に起こり得る危険を警告し得る。屋内環境については、二次元の（２−Ｄ）床面における物体の（ｘ，ｙ）位置、および床面に垂直な軸線に対する姿勢θを追跡することが一般に望ましい。すなわち、ロボットのｚ座標と、ロボットのロールおよびピッチ角度とは一定であると仮定すると、好都合になり得る。物体の（ｘ，ｙ）位置および姿勢θを合わせて物体のポーズと呼ぶ。

位置推定のための多数のデバイス、プロセス、センサ、設備、および機構が、提案されてきた。これらの方法は、主に２つの種類に分類し得る。第１の種類は、位置推定を行い得るように環境においてビーコンを使用し、第２の種類は、環境において自然のランドマークを用いる。本明細書に記載の方法および装置は、ビーコンに基づく位置推定または自己位置同定の第１の種類に分類されるので、この項では、ビーコンに基づく自己位置同定方法に焦点を当てる。

ビーコンは、適切な検出装置により検出し得る、環境内の人工デバイスである。ビーコンは、受動または能動ビーコンであり得る。受動ビーコンの例には、逆反射材料がある。光源を逆反射材料に投射することによって、１つまたはそれ以上の適切な光センサを使用して容易に検出し得るシグネチャまたは信号が生成され得る。シグネチャまたは信号を使用して、１つまたはそれ以上のセンサが、ビーコンおよび／または環境に対する位置を測定し得る。

能動光ビーコンは、光センサにより検出され得る光を放射する。光センサは、（飛行時間を用いる）エミッタまでの距離、エミッタへの方位、信号強度などの、放射された光の種々の特性を測定し得る。このような特性を用いると、三角測量または三辺測量などの適切な技術を用いてセンサの位置を推定し得る。これらのアプローチは、光センサと対になった能動光ビーコンを使用しており、エミッタとセンサとの間の視線により束縛されることが不都合である。視線がなければ、センサは、エミッタを検出できなくなる。

本明細書に記載の実施形態は、グローバルまたはローカル基準座標系に対してロボットなどのデバイスの位置および姿勢を推定するための方法および装置に関する。本明細書に記載の装置は、光センサと、光エミッタと、関連の信号処理回路構成要素とを備える。センサのポーズは、未知であることが一般的であり、本明細書に記載の装置および方法を用いて、少なくとも１つのセンサのポーズと、エミッタの、ある面への投射位置とを測定または推定し得る。

本方法および装置の一般的な応用には、自律型移動ロボットの位置の推定および追跡が含まれる。他の応用は、位置認識、ユビキタスデバイスのための物体の推定および追跡を含む。別の応用には、室内環境における人またはペットの位置の追跡が含まれる。本方法および装置は、１つまたはそれ以上の光エミッタ、１つまたはそれ以上の光センサ、信号処理回路構成要素、信号処理方法を含み、ある面から反射する、１つまたはそれ以上の放射光源の信号の検出に少なくとも部分的に基づいて、複数の光センサの少なくとも１つの光センサの位置および姿勢を測定する。

ここで、以下に概要を述べる図面に関連して本発明のこれらおよび他の特徴を説明する。これらの図面（拡大・縮小していない）および関連の説明は、本発明の好ましい実施形態を示すために行い、本発明の範囲を限定するものではない。

用語集
ポーズ： ポーズとは、空間内における位置および姿勢のことである。三次元では、ポーズは、位置（ｘ，ｙ，z）と、三次元空間の軸線に対する姿勢（α、β、θ）を指し得る。二次元では、ポーズは、平面における位置（ｘ，y）と、その平面への垂直線に対する姿勢θとを指し得る。

光センサ： 光センサとは、光を使用して、状態を検知し、状態を定量的に記述するセンサである。一般に、光センサは、光源の１つまたはそれ以上の物理的特性を測定し得るセンサを指す。このような物理的特性は、光子の数、センサ上の光の位置、光の色などを含み得る。

位置検出素子： 位置検出素子は、位置検出器またはＰＳＤとしても知られており、一般に一次元または二次元で、入射した光源の重心を測定し得る光センサである。例えば、ＰＳＤは、入射した光スポットを比較的連続した位置データに変換し得る。

撮像装置： 撮像装置は、センサの作用領域の光を測定し、少なくとも１つの軸線または寸法に沿って光信号を測定し得る光センサを指す。例えば、フォトアレイは、一次元撮像装置と定義することができ、デュオ−ラテラルＰＳＤは、二次元撮像装置と定義することができる。

カメラ： カメラは、一般に、１つまたはそれ以上の撮像装置、１つまたはそれ以上のレンズ、および関連の補助回路構成要素を含む装置を指す。任意選択的に、カメラは、１つまたはそれ以上の光フィルタ、およびハウジングまたはケースを含む場合がある。

ＰＳＤカメラ：ＰＳＤカメラは、ＰＳＤを使用したカメラである。

プロジェクタ：プロジェクタは、光を投射する装置を指す。プロジェクタは、エミッタと、電源と、関連の補助回路構成要素とを含む。プロジェクタは、１つまたはそれ以上の光スポットをある面に投射し得る。

スポット：スポットは、ある面への光の投射を指す。スポットは、全投射に相当する場合があるし、または全投射の一部分のみに相当する場合もある。

光位置センサ：光位置センサは、１つまたはそれ以上のカメラと、信号処理装置と、電源と、補助回路構成要素とを含むデバイスであり、１つまたはそれ以上のスポットに対する位置、距離、角度、またはポーズを推定し得る。

これらの方法および装置を、ある好ましい実施形態に関して説明するが、本明細書に記載の利点および特徴の全てを提供するわけではない実施形態を含む、当業者に明らかな他の実施形態もまた、本発明の範囲内である。

実施形態では位置の推定に能動光ビーコンを使用することが好ましい。環境の比較的広い部分から、観察し得る面に光源を投射することによる通常の能動光ビーコンに基づく位置同定が、視線による制限を受けることを、本明細書に開示した技術によって最小限になるまたは減少することが好ましい。光源には、例えば、赤外線（ＩＲ）光源などの肉眼には見えない光源が含まれ得ることが理解されるだろう。例えば、屋内環境では、ビーコンから天井へ放射光を投射することが好ましい場合がある。多くの屋内環境では、部屋の天井は、その部屋の中の大部分の場所から観察され得る。

図のように、ロボット電気掃除機などの自律型移動ロボットに本方法および装置を応用することを考え得る。自己ドック入りし自己充電するための一般的なアプローチは、能動赤外線（ＩＲ）ビーコンを充電ステーションに配置することであって、ロボットは、この充電ステーションを光検出器で検知し、関連の知覚情報を用いてドックステーションを見つけ得る。このアプローチは、視線による制限を受ける。ロボットとドックステーションとの間に視線を遮るものがなければ、ロボットは、ドックステーションに対する自分の位置が分からない。

一実施形態では、ＩＲエミッタは、ドックステーションから上方へ天井に突出するように配置されることが好ましい場合があり、またロボットは、一般に天井を向いているかまたは天井を観察し得る光検出器を具え得る。ロボットは、ロボットとドックステーションとの間の視線が遮られない場合でも天井へのＩＲ投射を観察することが好ましい場合がある。比較的多くの場合、ロボットは、天井に視線をやり、これにより、ロボットは、ＩＲ投射を検出し、自己充電用のドックステーションに移動し得る。

本明細書に説明する方法および装置は、多数の変形を含み、これらの変形には、使用する能動ビーコンのタイプおよび数が異なる変形、反射光の検出に使用する光センサのタイプおよび数が異なる変形、および物体のポーズを測定するために使用する信号処理のタイプが異なる変形がある。本方法および装置の実施形態には、物体に対する別の物体の距離の推定、物体に対する別の物体の方位の推定、二次元の平面における物体の（ｘ，ｙ）位置の推定、三次元空間における物体の（ｘ，ｙ，ｚ）位置の推定、二次元または三次元における物体の位置および姿勢の推定、物体の直線または角速度の推定、および物体の直線または角加速度の推定のためのシステムが含まれる。

本方法および装置の実施形態は、グローバルまたはローカル座標系に関連して、移動ロボットなどのデバイスの位置および姿勢を推定することに関する。本装置は、１つまたはそれ以上の光センサと、１つまたはそれ以上の光エミッタと、信号処理回路構成要素とを含む。センサの初期の位置および姿勢は、知ることはできず、本装置および方法を用いて、複数のセンサの１つまたはそれ以上のセンサの位置および姿勢、およびある面上におけるエミッタの投射位置を測定または推定し得る。

一実施形態では、光センサは、ある面に投射された光を測定することにより、光エミッタにより生成される、センサの視野の範囲内の光信号を測定する。これとは対照的に、通常のシステムでは、距離または位置測定用のこのような光デバイスには、エミッタとセンサとの間の視線が必要であることが不都合である。本明細書に記載の実施形態では、屋内環境の天井などの面に投射された光信号を検出し得ることが好ましい。光エミッタは、その環境の比較的広い部分から、センサが観察し得る面へ、１つまたはそれ以上の光スポットを投射するように構成され得る。センサは、スポットを検出し、スポットに対するセンサの位置を推定する。センサは、センサの基準座標系におけるスポットの位置、およびスポットにより生成される信号の強度などの量を測定し、各スポットと、特有の識別子とを関連づけ得る。このような各測定または一連の測定によって、センサの位置とスポットの位置との間の関係が定まる。１つまたはそれ以上のセンサと１つまたはそれ以上のスポットとの間において定められたこのような複数の関係を用いると、信号処理回路構成要素は、複数のセンサの少なくとも１つのセンサのポーズ、および任意選択的に１つまたはそれ以上のスポットの位置を推定し得る。

本明細書に記載の本方法および装置の実施形態では、ここで使用する光センサの数およびタイプの変更、ここで使用する光エミッタの数およびタイプの変更、任意選択的に、１つまたはそれ以上のスポットを介したセンサへの光の投射の変更、単一のセンサまたは複数のセンサの距離、向き、位置、姿勢、速度、角速度、加速度、および角加速度の推定に使用する方法の変更を行い得る。例えば、光スポットは、ＩＲ光をある面に放射するＩＲセンサにより生成され、その面から反射した光を検出するために光検出器を使用し得る。エミッタを１台、センサを１台使用すると、投射された光までの距離と相対的な向きとを測定し得る。エミッタを２台、二次元センサを１台使用すると、平面におけるセンサの位置と、その平面に垂直な軸線の周りにおけるセンサの回転とを測定し得る。

本明細書に記載の本方法および装置の実施形態では、非常に様々な光センサを使用し得る。いくつかの実施形態では、ＣＭＯＳ撮像装置、ＣＣＤ撮像装置などの、デジタルまたはアナログ撮像あるいはビデオカメラを使用する。他の実施形態では、一次元ＰＳＤ、アンギュラ一次元ＰＳＤ、二次元ＰＳＤ、クアッドＰＳＤ、デュオ−ラテラルＰＳＤ、テトラ−ラテラルＰＳＤなどのＰＳＤを使用する。他の実施形態では、光検出器を使用する。

一実施形態では、光センサは、レンズおよび１つまたはそれ以上の光フィルタと組み合わされてカメラを形成する。例えば、ＰＳＤセンサは、入射光をフィルタ処理し周辺光の影響を低減させるためにレンズおよび光フィルタを取り付ける開口側を有するケースに封入され得る。

本明細書に記載の本方法および装置の実施形態はまた、幅広い様々の光エミッタを使用し得、この光エミッタには、可視光デバイス、不可視光デバイス、レーザー光デバイス、赤外光デバイス、偏光デバイス、発光ダイオード（ＬＥＤ）、レーザーダイオード、白熱電球、ハロゲン光、プロジェクタなどが含まれる。

本明細書に記載の本方法および装置の一実施形態では、１台の二次元ＰＳＤカメラと、複数の赤外線（ＩＲ）エミッタとを使用する。各ＩＲエミッタは、スポットを室内の天井に投射する。各エミッタは、特有のパターンまたは周波数で変調される。ＰＳＤカメラは、スポットが投射される天井を画定する平面の少なくとも一部分と視野とが交差するように、例えばロボットに装着され、天井に面する。ＰＳＤカメラは、カメラセンサの座標において、観察可能な各スポットの投射位置を示し得る。ここに示す実施形態では、観察された各スポットの位置は、その重心と定義される。

カメラが観察した各スポットの位置は、対応の透視変換を行うことにより定義される、カメラの像平面上へのスポットの投射位置に対応し得る。ＰＳＤカメラは、カメラの各スポットの位置を測定し得る。カメラが測定したスポットの位置と、スポット同士の距離に関する情報とを用いると、一平面におけるＰＳＤカメラの位置（ｘ，ｙ）と、その平面に垂直な軸線周りにおけるＰＳＤカメラの回転（θ）とが、測定され得る。（x，y，θ）により定められるカメラの位置および姿勢は、カメラのポーズとして知られる。

例えば、ＰＳＤカメラは、ロボットなどの移動デバイスに連結可能であり、デバイスのポーズは、２つまたはそれ以上のスポットで、室内において比較的精確に測定され得ることが、好ましい。ポーズの推定は、位置同定としても知られ、自動掃除、自動床掃除、テレプレゼンス、安全、および娯楽を含む多くの応用において重要な要素である。位置が精確に推定されなければ、通常のロボット内部の位置推定にはずれが生じやすく、通常のロボットは、ずれを測定または考慮することができないのが一般的なので、通常のロボットが、経路または軌道を作ることは比較的難しいかまたは不可能である。ロボット掃除機の計画的な床の掃除範囲について、例えば、位置同定をできない通常のロボットは、一般に、掃除が完了した領域と掃除が未了の領域とを認識し続けることができず、したがって、ロボットは、同じ領域を繰り返し非能率的に掃除してしまうことが比較的多く、また、十分な頻度で、他の領域を掃除することが比較的少ない。よって、多くの通常のロボット電気掃除機は、不規則な軌道を作る。それとは対照的に、比較的精確に位置同定を行い得る、一実施形態に係るロボット電気掃除機は、比較的高率よく計画された経路をたどり得る。一実施形態に係るロボット電気掃除機は、その経路をたどり、計画されて通過し得る経路を作り得るので、比較的効率よく部屋を掃除し得る。同様に、位置同定し得る移動ロボットは、望ましい場所へ進み、たどってきた経路の履歴を維持し得る。

本明細書に記載の本方法および装置の別の実施形態では、二次元ＰＳＤカメラを一台と、ＩＲエミッタを一台とを使用する。ＩＲエミッタは、天井にスポットを投射し、ＰＳＤカメラは、スポットが投射された天井を定める平面の少なくとも一部分と視野とが交わるように天井を向く。ＰＳＤカメラは、カメラからスポットまでの距離と、その距離の測定により半径が定められる円の接線に対する、カメラからスポットへの向きとを測定するための表示を提供し得る。距離の測定によって、カメラの平面に投射されたスポットを中心とする円が定められる。一例では、ここに示す実施形態は、スポットに対してデバイスを位置決めすることが望ましい応用に用い得る。カメラが天井のスポットの下にあるとき、カメラの位置は、ＰＳＤカメラの中心にあることが好ましい。例えば、スポットが充電ステーションの上に投射される場合、移動デバイスは、充電ステーションに近づき、自律的に再充電し得る。ホイールエンコーダをさらに備える関連の実施形態では、ロボット電気掃除機は、不規則な掃除範囲の計画に比べて比較的効率のよい床掃除範囲の計画を実行するために、同一中心の円に沿って移動するか、または螺旋に沿って移動し得る。

自律型移動ロボットに関して、種々の実施形態を説明してきたし、さらに説明していくが、本明細書に記載の原理および利点は、位置推定から利益を得られる他の応用に適用し得ることを、当業者は理解するだろう。これらの他の応用も、本発明の範囲内である。

ここで、実施形態の例を説明する。図１に、装置の一実施形態の構成要素のブロック図を示す。本装置は、プロジェクタ１１１と、光位置センサ１１２とを含む。プロジェクタ１１１が、光のパターン１１３を面１１６に放射して、投射光のパターン１１９が生成される。一実施形態では、光のパターン１１３は、変調される。投射光のパターン１１９の反射１１４が、光位置センサ１１２に投射される。

プロジェクタ１１１は、光源１０２を含む。例として、光源１０２は、変調器１０１により変調され得るレーザーデバイス、赤外線デバイスなどのデバイスに相当し得る。任意選択的に、光源１０２からの光が、１つまたはそれ以上のレンズ１０３を通過して、光が面１１６に投射され得る。

光位置センサ１１２は、カメラ１１７と、処理装置１１８とを含む。カメラ１１７は、面１１６から反射した光１１４の強度および位置を検出し測定し、対応の信号を生成して、これらの信号は、光位置センサ１１２の投射光のパターン１１９に対する位置を推定するように、単一の処理装置１１８により処理される。光位置センサ１１２には、複数のカメラ１１７および／または複数の処理装置１１８が含まれる場合があることが、理解されるだろう。

カメラ１１７は、撮像装置１０４を含む。撮像装置１０４は、例えば、ＣＭＯＳ撮像装置、ＣＣＤ撮像装置、赤外線撮像装置などに相当し得る。カメラは、任意選択的に光フィルタ１０５を含み、また、任意選択的にレンズ１０６を含む場合がある。レンズ１０６は、標準的なレンズに相当する場合もあるし、あるいは広角レンズ、魚眼レンズ、全方位レンズなどの特別なレンズに相当する場合もある。さらに、レンズ１０６は、平坦な、放物面状または円錐形の鏡などの反射面を含み、この反射面を用いて、比較的広い視野または複数の視点を得られる。レンズ１０６は、反射光１１４を集め、撮像装置１０４に投射する。光フィルタ１０５は、レンズ１０６から撮像装置１０４へ通過する光の波長を抑制可能であって、このことが、周辺光の影響を低減させることに利用されることが好ましい場合があり、こうすることによって、プロジェクタ１１１から入射する光の波長を適合させるように光の範囲が狭められ、かつ／または撮像装置１０４に投射される光量が制限されて、露出過度または飽和度の影響が制限され得る。フィルタ１０５は、レンズ１０６の前にまたはレンズ１０６の後に配置され得る。カメラ１１７が、複数の撮像装置１０４、複数の光フィルタ１０５、および／または複数のレンズ１０６を含む場合があることが、理解されるだろう。

信号処理装置１１８は、アナログ素子を含む場合があり、また、カメラ１１７により生成される信号を処理するためのデジタル素子を含む場合がある。信号処理装置１１８の主な素子は、増幅器１０７、フィルタ１０８、アナログ−デジタル変換器１０９、ＰＩＣとしても知られる周囲のインターフェースコントローラなどのマイクロプロセッサ１１０を含むことが好ましい。信号処理装置１１８は、複数のフィルタ１０８および／または複数のマイクロプロセッサ１１０を含み得ることが、理解されるだろう。

本装置の実施形態は、本明細書に記載のプロジェクタ１１１または光位置センサ１１２の特定の実施に限定されない。本装置の真の精神および範囲から逸脱しない本装置の他の実施、実施形態、変形は、当業者に容易に理解されるだろう。

図２に、位置推定技術の利用法の一例を示す。環境には、天井２０６、床２０７、および１つまたはそれ以上の壁２０８が含まれる。ここに示す環境では、プロジェクタ２０３が、壁２０８に取り付けられている。プロジェクタ２０３は、内部電源を具え、壁のコンセントまたはその両方にプラグ接続され得ることが理解されるだろう。プロジェクタ２０３は、第１のスポット２０４および第２のスポット２０５を天井２０６に投射する。光位置センサ２０２が、ロボット２０１に取り付けられている。光位置センサ２０２は、天井２０６のスポット２０４、２０５を検出し、床平面上におけるロボット２０１の位置（ｘ，ｙ）と、ロボット２０１の、床平面に垂直な直線に対する姿勢θとを測定し得る。一実施形態では、ロボット２０１のポーズは、グローバル座標系に関連して測定される。

図３に、図２に関して既述の本方法および装置の一実施形態に関連する幾何学的モデルを図示する。天井２０６は、床２０７の上の高さｈのところにある。点ｗ_１３０１は、第１のスポット２０４の重心にあり、点ｗ_２３０２は、第２のスポット２０５の重心にある。ここに示す実施形態では、Ｘ軸、Ｙ軸、およびＺ軸を有するグローバル座標系が、定められ、グローバル基準座標系とも呼ばれる。

一実施形態では、Ｙ軸が点ｗ_１３０１を起点として点ｗ_２３０２を通るベクトルと平行となるように、Ｙ軸を定義すると都合がよい。さらに、Ｘ軸が、Ｙ軸に垂直に、床により画定される平面にあるように、Ｘ軸を定義すると都合がよい。さらに、正のＺ方向が、Ｘ方向の単位ベクトルとＹ方向の単位ベクトルとのクロス積により特定されるようにＺ軸を定義すると都合がよい。

したがって、ここに示す座標系では、Ｚ軸は、床平面に垂直であり、床から天井に向かっている。グローバル座標系では、原点Ｏは、座標（０，０，０）の点として定義される。さらに、グローバル座標系では、点ｗ_１３０１は、座標（ｘ_１，ｙ_１，ｈ）を有するものとして定義され、点ｗ_２３０２は、座標（ｘ_２，ｙ_２，ｈ）を有するものとして定義される。さらに、原点Ｏは、点ｗ_１３０１のすぐ下に配置されて、その結果、ｘ_１＝０、ｙ_１＝０となると仮定すると都合がよい。その上、Ｘ軸の定義は、同様にｘ_２＝０を意味する。よって、点ｗ_１３０１は、座標（０，０，ｈ）を有し、点ｗ_２３０２は、座標（０，ｙ_２，ｈ）を有する。一般的な原理を損なうことなく、前述の定義をし得ることが、理解されるだろう。

撮像装置に対する座標系は、ｕ軸、ｖ軸、およびｚ軸で定義され、カメラ座標系またはカメラ基準座標系と呼ばれる場合がある。ここに示す実施形態では、撮像装置は、二次元ＰＳＤセンサに相当する。一実施形態では、床平面からのＰＳＤセンサの高さは、天井の高さhに比べて比較的低く、よって、ＰＳＤセンサ、およびカメラ座標系の原点は、グローバル座標系の座標（ｘ，ｙ，０）および姿勢θを用いる。グローバル座標系の原点からカメラ座標系の原点への変位は、Ｓ、よって、

点ｃ_１３１１は、点ｗ_１３０１の撮像装置への投射を表し、点ｃ_２３１２は、点ｗ_２３０２の撮像装置への投射を表す。点ｃ_１３１１は、カメラの基準座標系において座標（ｕ_１，ｖ_１，０）を有し、点ｃ_２３１２は、カメラの基準座標系において座標（ｕ_２、ｖ_２、０）を有する。一般的な原理を損なうことなく前記定義を行い得ることが、理解されるだろう。

一実施形態では、スポット２０４、２０５は、特有の信号または特有のシグネチャを用いて識別され得る。例えば、スポット２０４、２０５を発生させるエミッタは、異なる周波数でオン−オフ変調させ得る。第１のスポット２０４を発生させるエミッタは、第1の周波数ｆ_１で変調され、第２のスポット２０５を発生させるエミッタは、第２の周波数ｆ_２で変調され得る。ここで、第１の周波数および第2の周波数は、異なっており、すなわち、ｆ_１≠ｆ_２である。

ここで、天井の高さｈ、および点ｗ_１３０１と点ｗ_２３０２との間の距離ｙ_２は、種々の方法で測定され得ることに、留意すべきである。例えば、光位置センサを使用する移動ロボット２０１が、車輪の移動距離の推定値を出し得る場合、ロボット２０１が、複数の位置からの、点ｗ_１３０１および点ｗ_２３０２の測定または観察を用いてｈおよびｙ_２を推定し得る。他の適切な技術が、当業者には容易に分かるだろう。

本方法および装置を使用した例示的な位置推定
図３に関連する前述の座標系、距離、角度、および点に関して、ＰＳＤカメラは、ｃ_１およびｃ_２を測定可能であり、これらのｃ_１およびｃ_２は、ＰＳＤセンサへのｗ_１およびｗ_２の投射に対応する。一実施形態では、本方法の目的は、グローバル基準座標系においてＰＳＤカメラの位置Ｓを測定することである。

一実施形態では、ＰＳＤは、光の重心の位置および強度に比例する電流を生成することにより、ＰＳＤに投射された光の重心の座標を測定する。アナログ回路構成要素、デジタル回路、ハードウェア、ソフトウェア、ファームウェア、およびそれらの組み合わせを含む、種々の幅広い方法で、関連の処理が行われ得る。例えば、マイクロコントローラ、マイクロプロセッサ、ＣＰＵ、多目的デジタルシグナルプロセッサ、専用のハードウェアなどを使用し得る。

複数のスポットの重心を測定するためには、いくつかの条件が好ましい。第一に、センサは、光、周辺光またはそれ以外の光で飽和しないことが好ましい。一実施形態では、これは、光フィルタを使用して、ＰＳＤセンサのアクティブ領域に投射する不要な光源を減少させるかまたは最低限にすることによって、また、ＰＳＤにバイアスをかけて、飽和する光のレベルを高くすることによって、実現される。第２に、特定の光源の位置を確実に測定するためには、その他の光源の影響を低減するまたは除くことにより、光源をその他の光源から分離することが好ましく、この他の光源には、周辺光、および他のスポットにより発生する光が含まれ得る。１つのアプローチは、１つの光源を分離してその光源を固有のパターンで変調することであって、その結果、その光源は、他の光源と識別される。ｉ番目のエミッタが、投射された光を周波数ｆ_ｉでオン−オフ変調する場合、ＰＳＤセンサは、帯域フィルタを使用して信号をフィルタ処理することにより発生しそれぞれ比較的低い周波数ｆ_ｉ-ｗおよび比較的高い周波数ｆ_ｉ＋ｗを有する信号を、引き出し得る。ここで、２ｗは、対応の帯域フィルタの幅に対応している。ＰＳＤの信号処理装置は、フィルタを使用して、帯域フィルタにより定められた周波数の範囲外の周波数の信号を抑制し得る。ＰＳＤが信号をフィルタ処理することは、ＰＳＤの電流が関連の重心位置に変換される前後のいずれかに行い得る。一実施形態では、第１のエミッタが周波数ｆ_ｉで変調され、第２のエミッタが周波数ｆ_２で変調され、ここでｆ_１≠ｆ_２である場合、信号処理装置が、ｆ_１により特定された信号をフィルタ処理して、第１のスポットの重心であるｃ_１を測定し、ｆ_２により特定された信号をフィルタ処理して、第２のスポットの重心であるｃ_２を測定する。

例示的なポーズ推定方法
一実施形態では、本装置は、Ｎ個の光スポットを投射するＮ台のエミッタと、Ｍ台のカメラとを含む。グローバル基準座標におけるｉ番目のカメラの位置は、本明細書では、Ｓ_ｉ＝（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）と示し、グローバル基準座標におけるｉ番目のカメラの回転姿勢は、本明細書では、Ｒ_ｉ＝（α_ｉ，β_ｉ，θ_ｉ）と示す。ｊ番目の光スポットの位置は、本明細書ではｗ_ｊと示し、ｉ番目のカメラへのｊ番目の光スポットの投射位置は、本明細書では、ｃ_ｉｊと示す。次に、以下の関係は、Ｓ_ｉ、ｗ_ｊ、ｃ_ｉｊに関する。

式１において、Ｒ_ｉは、３自由度の回転変換を表し、これは、一実施形態では、Ｒ_ｉ＝Ｒ_αＲ_βＲ_θなどの、３つの相互に直交する１自由度の回転変換を合成することにより行われる。さらに、式１において、Ｐ_ｉは、ｉ番目のカメラと関連する透視変換を表す。よって、式１は、６つの未知数（未知数はｘ_ｉ、ｙ_ｉ、ｚ_ｉ、α_ｉ、β_ｉ、θ_i）に対して３つの式を定義する。Ｎ個のスポットおよびＭ台のカメラを有するシステムでは、Ｎ×Ｍのこのような行列式を計算し得るが、必ずしも全てのこのような式が、一意的で、独立で、非退化であるとは限らない。したがって、スポットが２つ、カメラが一台で、ｘ、ｙ、およびθに対する値が決定され得る。６自由度全てを決定するためには、式１に類似した少なくとも６つの独立の非退化な式を生成する、スポットおよびカメラの構成にすることが好ましい。

エミッタを２台およびカメラを１台にした例示的システム
一実施形態では、本システムは、天井へ投射される２つのスポットと、１台のＰＳＤカメラを有する１台の光学位置センサとを含む。ＰＳＤカメラの基準座標系におけるスポットｗ_ｊと投射ｃ_ｊとの間の関係は、以下の式により得られる。

式２において、Ｓは、グローバル基準座標系におけるＰＳＤカメラの位置を表し、Ｐは、グローバル座標系における点（Ｘ，Ｙ，Ｚ）からＰＳＤカメラ基準座標系における点（ｕ，ｖ，ｚ）への変換を表す。さらに、この例の目的のために、カメラ座標系のｚ軸は、グローバル座標系のＺ軸と垂直方向に並べられる。このことは、Ｒ_αおよびＲ_βが、単位行列に相当し、よって、Ｒ_αおよびＲ_βは、式２から除外されたことを意味する。ピンホールカメラモデルの場合、Ｐは、スカラー値λ／（λ−Ｚ）に相当し、当該式において、λは、カメラの焦点距離を表す。スカラー倍の値はまた、適切な大きさにした、対応の複数の単位行列をかけることにより得られることが理解されるだろう。さらに式２において、Ｒ_θは、以下の単位行列で表し得る。

式２は、以下のように書き換えられる。

式４において、Ｐ^−１は、逆透視変換を表し、Ｒ_θ ^−１は、逆回転変換を表す。ｉ番目のスポットの位置が、ピンホールカメラモデルにおけるカメラの焦点距離などの、適切なカメラパラメータと関連づけられると、式４は、各測定ｃ_ｊのための３つの未知数ｘ、y、θで２つの非退化式を定義する。よって、３つの変数、ｘ、ｙおよびθが、ともにＰＳＤカメラのポーズを決定する。

３つの未知数での２つの式では、ＰＳＤカメラのポーズに対する唯一解が定められないので、未知数よりも多い独立方程式を用いることが好ましい。スポットを２つとＰＳＤカメラを１台で、以下のように３つの未知数で４つの式を生成し得る。

式５は、関連のＰＳＤカメラの位置がｃ_１であるスポットｗ_１に関連し、式６は、関連のＰＳＤカメラの位置がｃ_２であるスポットｗ_２に関連する。式５を式６から引くと、式７に示す以下の行列式が生成される。

式７は、以下の通り、展開し得る。

式８で与えられる行列式は、２つの非退化な１次方程式であることを表す。式８において、Δｙ＝ｄ＝ｙ_２−ｙ_１、Δｕ＝ｕ_２−ｕ_１、およびΔｖ＝ｖ_２−ｖ_１である。図３に関して前述したように、ｄは、２つのスポットを隔てる距離を表す。よって、式８により定義される２つの非退化な１次方程式の第１の非退化な１次方程式は、以下により与えられる。

式９においてθの解を求めると、θ＝ｔａｎ^−１（Δｕ／Δｖ）が得られ、この結果を式５に代入すると、グローバル基準座標系におけるＰＳＤカメラの位置であるＳに対する以下の解が得られる。

したがって、測定ｃ_１およびｃ_２の関数としてのＰＳＤカメラのポーズ（ｘ，ｙ，θ）が、式９および式１０を用いて決定され得る。

エミッタ１台およびカメラ１台の使用例
一実施形態では、本システムは、天井に投射される１つのスポットと、ＰＳＤカメラを１台有する１台の光位置センサとを含む。式２に関して上述した関係と同様に、ＰＳＤカメラ基準座標系におけるスポットｗとその投射ｃとの間の関係は、以下の式により与えられる。

グローバル座標系の原点は、一般的原理を損なうことなく、スポットｗが原点のすぐ上に配置されるように選択可能なので、式１１を再構成すると、以下のＳの解を得られる。

式１２は、以下の関係を得られるように座標記号の書き換えを行い得る。

よって、式１３は、２つの非退化な１次方程式を示す。Ｐ^−１が、単位行列のスカラーまたはスカラー倍に相当する場合、２つの非退化な１次方程式を二乗し合計して、その結果を簡単にすると、以下が得られる。

ｘ^２＋ｙ^２は‖Ｓ‖^２と等しく、ｕ^２＋ｖ^２は、‖ｃ‖^２と等しいので、式１４は、‖Ｓ‖^２＝（Ｐ^−１）^２‖ｃ‖^２または‖Ｓ‖＝Ｐ^−１‖ｃ‖と簡単にすることができる。

したがって、距離の測定‖ｃ‖、および対応の距離の測定‖Ｓ‖は、ｘ−ｙ平面に、原点（０，０）を中心とし半径を‖Ｓ‖とする円を定義し得る。センサの位置（すなわち、Ｓ）の円への接線は、ベクトルｓ＝（ｘｙ）^Ｔと直交し、当該ベクトルの肩字「Ｔ」は、ベクトルまたは行列の転置演算を示す。

したがって、この実施形態では、‖Ｓ‖およびφが決定され、ロボット、人の追跡、物体の追跡などの応用を援助し得ることが好ましい。一例では、スポットが、ドックステーションのすぐ上の天井に投射され、ＰＳＤカメラ１台を有する光位置センサが、ロボットに取り付けられている。‖Ｓ‖およびφの推定を用いると、ロボットは、スポットの方を向いて、スポットに近づくように自らを誘導し得る。このようにして、ロボットは、ドックステーションに近づき、再充電を行い得る。代替例では、プロジェクタは、ハンドヘルド型プロジェクタに相当する場合があり、プロジェクタを使用して、ユーザーが選択した物体または対象となる場所の上方を向いて、ロボットを物体または対象となる場所に導き得る。この代替実施形態によって、ロボットの相互作用のための効果的なインターフェースを得られる。

本発明の他の実施形態の例
本方法および装置の一実施形態は、ＣＣＤカメラ、ＣＭＯＳカメラなどのカメラと、天井などの投射面にパターンを生成するプロジェクタとを含む。本実施形態は、複数のカメラおよび／または複数のプロジェクタを含み得ることが理解されるだろう。例として、プロジェクタは、スライドプロジェクタに相当する場合があり、パターンは、スライドにおいて符号化され得る。一実施形態では、少なくとも１つのパターンは円形であり、また、別の実施形態では、少なくとも１つのパターンは、四角形である。各カメラは、グレースケールまたはカラー画像を生成する。信号処理装置は、カメラの像を処理し、特有のパターンを引き出し、カメラセンサ座標系におけるパターンの位置を推定する。パターンの位置は、パターンの重心として定義され得る。グローバル基準座標系におけるｊ番目のパターンの位置は、本明細書ではｗ_ｊと示し、ｉ番目のカメラ基準座標系におけるｊ番目のパターンの位置は、ｃ_ｉｊと示し得る。次に、ｊ番目のパターンとｉ番目のカメラへのパターンの投射との間の関係は、式１により定義される。信号処理装置は、カメラの像を捕らえ、１つまたはそれ以上の像分析技術を用いてその像を処理して既知のパターンの位置を検出し引き出し得る。像分析技術には、例として、直線および角の検出（例えば、四角形のパターンの検出）、ハフ変換（例えば、円の検出）などが含まれ得る。カメラの基準座標系におけるパターンの位置を測定した後、信号処理装置は、既述の方法を用いてグローバル基準座標系に対するカメラの位置を測定し得る。任意選択的に、プロジェクタの１つまたはそれ以上のプロジェクタは、周辺光の影響を低減させるためにオン−オフ変調し得る。周波数の変調は、各パターンと特有の識別子とを関連づけるために用い得ることが好ましい。一実施形態では、パターンの識別子は、パターン自体の中において符号化されることが好ましい。一例として、パターン形状は、各パターンに別個の形状が使用される場合、特有の識別子を定め得る。例えば、本システムは、四角形のパターンと円形のパターンとを区別し、各パターンと異なる識別子とを関連づけ得る。

一実施形態では、プロジェクタの変調周波数は、カメラおよび信号処理装置により検出され引き出されたメッセージを送るために、ビットパターンなどの情報を符号化し得る。ビットパターンは、パルス幅変調、空間幅変調、および位相変調などの様々な一般的な変調技術を用いて信号が変調され得る。

別の実施形態では、ビットパターンは、スポットのもとの「キャリヤ」周波数の上部において変調される。プロジェクタおよび光位置センサは、光無線通信に使用し得ることが好ましい。本実施形態では、プロジェクタは、光のパターンを反射面に投射し、光センサが、反射面を調べることにより信号を検出することで、エミッタとセンサとの間の視線が不要となる。投射された光において変調された信号は、遠隔制御装置により変調された光がコマンドを器具に送り得る方法と同様に、ロボットへのコマンドなどのコマンドを送り得る。一例では、ドックステーションのすぐ上方の天井にスポットを投射することによって、ロボットは、ドックステーションを見つけ、自動充電を行い得る。さらに、ドックステーションの、ボタンなどのドックステーションとのインターフェースは、充電ステーションに戻るようにロボットへのコマンドを生成し得る。

本方法および装置のさらに別の実施形態は、別個の各領域用のプロジェクタなどの、ある環境の１つまたはそれ以上の別個の領域用のプロジェクタを含む。本実施形態は、比較的広い領域全体にわたって、または、複数の部屋などの、比較的制限された複数の領域全体にわたって、位置同定の範囲を広げることが好ましい。１台のプロジェクタと関連づけられてカバーされる領域は、カメラの視野、プロジェクタから反射面への距離、およびカメラがスポットを見る障害となる物体および壁の存在の制約を受ける場合がある。光のパターンの数が増すと、適用範囲も広がり得る。一実施形態では、複数の部屋を横切る適用範囲については、１つまたはそれ以上のプロジェクタが、適用範囲の望ましい各部屋用に設けられて、その結果、例えば、各部屋には専用のプロジェクタを具えられる。例えば、各プロジェクタは、その部屋に特有の識別子を有する１つまたはそれ以上のスポットを投射する。スポットに関連づけられた識別子は、そのスポットの変調周波数、スポットの形状、スポットの色、またはカメラセンサにより検出可能な別の適切な特性に基づく場合があることが理解されるだろう。

１つの実施において、個々のスポットの識別子を部屋と組み合わせることによって、部屋に特有の識別子が定められる。例として、第１の部屋は、特有の識別子「Ａ」および「Ｂ」と関連づけられた特有の２つのスポットを有し、第２の部屋は、特有の識別子「Ａ」および「Ｃ」と関連づけられた２つのスポットを有する場合がある。各部屋に特有の識別子は、部屋の形態および部屋同士のつながりをマッピングするために、ロボットによるなど、システムにより使用されることが好ましい。各部屋または領域のための特有の識別子がなければ、本システムは、不明瞭な位置情報を生成する場合があり、都合が悪い。一例として、各部屋に特有の識別子がなければ、第１の部屋の（ｘ，ｙ）座標と関連づけられた位置は、第２の部屋の（ｘ，ｙ）座標と関連づけられた位置と区別できないのが、一般的である。

これらの特定の実施形態に関して本発明を説明してきたが、本説明は、本発明の例示的なものであって、本発明を限定するものではない。本発明の真の精神および範囲から逸脱せずに種々の変形および応用を、当業者は着想し得る。

位置推定装置の一実施形態を示すブロック図である。 位置推定技術の一利用例を示す図である。 一実施形態に関連する幾何学的モデルを示す図である。

Claims (27)

1. ある面を有する壁により画定される領域内において物体の位置を推定するための位置推定システムであって、
   前記面の実質的に固定された場所の光スポットと、
   前記光スポットからの反射光を検出するように前記物体に装着された検出器と、
   前記検出器により検出された反射光を用いて前記物体の位置を推定するように、前記物体に装着されたデータ処理装置と、
   を備えるシステム。
2. 前記光スポットが投射される前記面を有する壁には、天井が含まれる請求項１に記載の位置推定システム。
3. 前記光スポットは、光源の投射である請求項１に記載の位置推定システム。
4. 前記検出器には、位置検出素子（ＰＳＤ）が含まれ、前記ＰＳＤへ入射する前記反射光の重心を用いて、前記物体の位置を推定する請求項１に記載の位置推定システム。
5. 前記光スポットには、２つの光スポットが含まれ、かつ、前記反射光には、前記２つの光スポットからそれぞれ反射した２つの光が含まれ、前記データ処理装置は、前記２つの反射光がそれぞれ前記検出器に入射する場所同士の間の距離を用いて、前記物体の位置を推定する請求項１に記載の位置推定システム。
6. 前記データ処理装置はまた、前記物体の角姿勢を推定する請求項１に記載の位置推定システム。
7. 前記物体は、前記光源により投射された前記光スポットの波長と異なる波長の光をフィルタ処理するための光フィルタをさらに含む請求項３に記載の位置推定システム。
8. 前記光源には２つの光源が含まれ、前記光スポットには２つの光スポットが含まれ、前記光源は、相互に異なる特性の光を生成して、前記２つの光スポットが、前記検出器および／または前記データ処理装置により相互に区別され得る請求項２に記載の位置推定システム。
9. 前記異なる特性には、異なる光強度パターンが含まれる請求項８に記載の位置推定システム。
10. 前記異なる特性には、異なる変調が含まれる請求項８に記載の位置推定システム。
11. 前記光スポットは、前記データ処理装置により引き出され得る情報を含むように、ビットパターンで符号化される請求項１に記載の位置推定システム。
12. 前記面は、投射された前記光スポットの少なくとも一部分を反射する反射面を含む請求項１に記載の位置推定システム。
13. ある面を有する壁により画定された領域において物体の位置を推定する方法であって、
    前記面の実質的に固定された場所に光スポットを投射するステップと、
    前記物体に装着された検出器を使用して前記光スポットからの反射光を検出するステップと、
    前記検出器により検出された前記反射光を用いて、前記物体に装着されたデータ処理装置で、前記物体の位置を推定するステップと、
    を含む方法。
14. 前記光スポット投射ステップは、天井を含む前記壁の面に前記光スポットを投射するステップを含む請求項１３に記載の方法。
15. 前記光スポット投射ステップは、光源により行われる請求項１３に記載の方法。
16. 前記検出器は、位置検出素子（ＰＳＤ）を含み、前記位置推定ステップは、前記ＰＳＤへ入射する前記反射光の重心を用いて位置を推定するステップを含む請求項１３に記載の方法。
17. 前記光スポットには、２つの光スポットが含まれ、前記反射光には、前記２つの光スポットからそれぞれ反射した２つの光が含まれ、前記位置推定ステップは、前記２つの反射光がそれぞれ前記検出器に入射する場所同士の間の距離を用いて、前記物体の位置を推定するステップを含む請求項１３に記載の方法。
18. 前記物体の角姿勢を推定するステップをさらに含む請求項１３に記載の方法。
19. 前記光源により投射される前記光スポットの波長と異なる波長の光をフィルタ処理するステップをさらに含む請求項１５に記載の方法。
20. 前記光源には、２つの光源が含まれ、前記光スポットには、２つの光スポットが含まれ、前記２つの光源は、相互に異なる特性の光を生成して、前記２つの光スポットが、前記検出器および／または前記データ処理装置により相互に区別され得る請求項１５に記載の方法。
21. 前記異なる特性には、異なる光強度パターンが含まれる請求項２０に記載の方法。
22. 前記異なる特性には、異なる変調が含まれる請求項２０に記載の方法。
23. 前記光スポットは、前記データ処理装置により引き出し得る情報を含むようにビットパターンで符号化され得る請求項１３に記載の方法。
24. 前記面は、投射された前記光スポットの少なくとも一部分を反射する反射面を含む請求項１６に記載の方法。
25. ある面を有する壁により画定された領域において物体の位置を推定するために、実体的媒体として実施されたコンピュータプログラムであって、
    前記面の実質的に固定された場所にある光スポットの場所を計算するように構成された命令を有するモジュールと、
    前記物体に装着された検出器により記録された反射光の場所を計算するように構成された命令を有するモジュールと、
    前記光源の場所と前記反射光の場所とを用いて、前記物体の位置を推定するように構成された命令を有するモジュールと、
    を含むコンピュータプログラム。
26. 前記光スポットの場所には、２つの光スポットの２つの場所が含まれ、前記反射光の場所には、前記２つの光スポットからそれぞれ反射した２つの光の２つの場所が含まれ、前記物体の位置を推定するように構成された命令を有する前記モジュールは、前記２つの反射光がそれぞれ前記検出器に入射する場所同士の間の距離を用いて前記物体の位置を推定する請求項２５に記載のコンピュータプログラム。
27. 前記物体の位置を推定するように構成された命令を有する前記モジュールはまた、前記物体の角姿勢を推定する請求項２５に記載のコンピュータプログラム。