JP2002213920A

JP2002213920A - 移動体の位置認識方法および装置

Info

Publication number: JP2002213920A
Application number: JP2001014274A
Authority: JP
Inventors: Keiji Yoda; 圭司依田
Original assignee: Azbil Corp
Current assignee: Azbil Corp
Priority date: 2001-01-23
Filing date: 2001-01-23
Publication date: 2002-07-31

Abstract

(57)【要約】【課題】絶対空間における移動体の位置の算出に必要
な位置情報に時間的なズレが生じず、複雑な機構を必要
とせず、また、移動体周辺の障害物に影響されずに移動
体の絶対位置が算出可能な移動体の位置認識装置及び方
法を提供する。【解決手段】絶対空間に置かれた移動ロボット１の位
置認識を行なう位置認識装置であって、当該絶対空間に
おいて、床面から略同一の高さの位置に設けた、発光色
の異なる３つのＬＥＤ５ａ，５ｂ，５ｃを、移動ロボッ
ト１に搭載したＣＣＤカメラ３によって撮像し、その撮
像した画像をもとに、絶対空間の所定位置を原点とした
絶対座標系における移動ロボット１の絶対座標を求める
機能を備える。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、床面上を自律走行
するロボットなどの移動体の位置認識方法及び装置に関
する。

【０００２】

【従来の技術】自律走行を行うロボット等の移動体を動
作させるためには、その移動体に対し、絶対空間におけ
る自己の位置（絶対位置）を認識させる必要がある。そ
の位置を検知する装置としては、特開平１−２８７４１
５に開示された位置制御装置がある。これは、移動体と
しての自走車から光ビームを円周方向に走査することに
より、自走車の位置を検出する自走車の位置制御装置で
あり、入射光の方向に光を反射する反射器を自走車から
離れた少なくとも３箇所に設置し、その発光と受光の位
相差に基づいて自走車の位置を原点とした各反射器の座
標値を算出し、自走車の現在位置および進行方位を演算
するものである。

【０００３】

【発明が解決しようとする課題】しかし、上記位置制御
装置では、次のような問題点がある。

【０００４】（１）光ビームを円周方向に走査するため
に発光器および受光器を回転させながら発光および受光
を行なうため、反射光を受光する時間にずれが生じ、移
動体の絶対位置の算出結果に誤差が生じてしまう。

【０００５】（２）光ビームを円周方向に走査するため
には、光ビームの発光器を移動体に設置すると共に、当
該発光器を回転させるための可動機構を設ける必要があ
り、装置の構成が複雑で故障の可能性が増大する。

【０００６】（３）移動体の周辺（水平方向）に障害物
があると光ビームの反射および受光が正確に行えず、誤
動作してしまうおそれがある。

【０００７】本発明の目的は、絶対空間における移動体
の位置の算出に必要な位置情報に時間的なずれが生じ
ず、複雑な機構を必要とせず、また、移動体周辺の障害
物に影響されずに移動体の絶対位置を算出できる移動体
の位置認識装置及び方法を提供することである。

【０００８】

【課題を解決するための手段】本発明は、絶対空間に置
かれた移動体の位置認識を行なう位置認識方法であっ
て、移動体に設けられた検出手段により、基準面から略
同一の高さに設けられた光点からの光により光点の位置
を検出するステップと、移動体の現在位置を仮想原点と
する仮想座標系において、その仮想原点及び検出手段に
より検出された２つの光点の位置を結んで形成される三
角形の外接円の中心と該２つの光点の位置を結ぶ直線と
の間の距離を求めるステップ、求めた距離を用いて、絶
対空間の所定位置を原点とした絶対座標系における外接
円の中心座標を求めるステップ、２つの光点の位置を結
ぶ直線と外接円の中心点との間の距離を求めるステッ
プ、求めた距離を用いて、絶対空間の所定位置を原点と
した絶対座標系における外接円の中心座標を求めるステ
ップ、求めた中心座標を用いて、絶対座標系における移
動体の絶対座標を求めるステップとを含む移動体の位置
認識方法である。

【０００９】本発明の装置は、絶対空間に置かれた移動
体の位置認識を行なう位置認識装置であって、当該絶対
空間において、基準面から略同一の高さに設けられる、
位置を識別するための光を発生する光点と、移動体に設
けられ、光点からの光により光点の位置を検出する検出
手段と、移動体の現在位置を仮想原点とする仮想座標系
において、その仮想原点及び検出手段により検出された
２つの光点の位置を結んで形成される三角形の外接円の
中心と該２つの光点の位置を結ぶ直線との間の距離を求
める距離算出手段と、該距離算出手段での算出結果を用
いて、絶対空間の所定位置を原点とした絶対座標系にお
ける外接円の中心座標を求める外接円中心座標算出手段
と、該外接円中心座標算出手段での算出結果を用いて、
絶対座標系における移動体の絶対座標を求める移動体絶
対座標算出手段とを備えたことを特徴とする。

【００１０】本発明の装置の実施態様では、検出手段は
撮像手段で構成され、距離算出手段は、該撮像手段によ
って撮像した画像をもとに、画像の中心位置及び２つの
光点の位置を結んで形成される三角形の外接円の中心と
該２つの光点の位置を結ぶ直線との距離を求める。

【００１１】本発明の装置の別の実施態様では、距離算
出手段は、撮像手段によって撮像した画像上に、画像の
中心点を仮想原点とした仮想座標系を設定し、当該仮想
座標系において各光点の位置を極座標で表したときの隣
接する２つの光点の間の成す角度を求め、当該角度を用
いて、仮想原点及び２つの光点の位置を通る外接円の半
径を求め、そして、該２つの光点の位置を結ぶ直線と外
接円の中心点との距離を求める。

【００１２】本発明のさらに別の実施態様では、仮想座
標系におけるＸ軸の正方向と光点との成す角度、及び、
絶対座標系における移動体と該光点との相対的な角度を
求め、これら２つの角度の差により、絶対座標系におけ
る移動体の向きを求める移動体方向算出手段を備える。

【００１３】

【作用及び効果】本発明によれば、所定の高さ位置に設
けられた光点からの光を検出することで移動体の絶対座
標を求めるので、一度ですべての光が検出でき、光の検
出に時間的なずれが生じない。従って、上記位置制御装
置のような、移動体の絶対座標の算出結果に誤差が生じ
ることはない。

【００１４】また、基準面から略同一の高さ位置に光点
を設けるため、水平方向の障害物等に影響されることは
なく、正確な移動体の絶対座標が求められる。

【００１５】検出手段としては、光点からの全ての光を
同時に検出できるものであればよく、例えば、広角レン
ズを用いたＣＣＤカメラ等が用いられる。

【００１６】本発明の実施態様では、検出手段として撮
像装置が用いられ、ＣＣＤカメラ等の撮像装置で撮った
広範囲な画像をもとに絶対座標の検出が行える。また、
広範囲な画像を取り込むことで、全ての光点が検出で
き、光点の検出に時間的なずれが生じることはない。

【００１７】本発明の別の実施態様では、撮像装置で撮
った画像から得られる各光点の角度情報をもとに移動体
の絶対座標を求めるので、誤差のない移動体の絶対座標
が求められる。

【００１８】本発明のさらに別の実施態様では、絶対空
間において移動体が現在向いている方向を誤差なく求め
ることができる。

【００１９】

【発明の実施の形態】図１は、本発明の原理を利用した
移動体（例えばロボット）１が、建物の屋内のような床
面、壁面および天井面で形成された所定の空間に置かれ
ている状態を示す。移動体１は、本発明の方法により当
該空間における移動体自身の位置を認識することで、予
め定められた走行制御プログラムに基づいた当該空間内
での自律走行を行う。

【００２０】移動体１には、移動するための手段とし
て、前後左右に駆動輪２が取り付けられ、当該駆動輪２
が回転することで床面上での移動を可能としている。ま
た、移動体１の上部には、検出手段としてのＣＣＤカメ
ラ３が、そのレンズを垂直上方に向けて取り付けられ
る。

【００２１】また、移動体１が移動する空間の基準面と
なる床面から略同一の高さに形成された天井面に、それ
ぞれ位置を識別するための光を発生する光点を設ける。
本実施例では、天井面に３つのＬＥＤ（発光ダイオー
ド）を、その発光部を床面に向けて取り付けられる。こ
れら３つのＬＥＤ（第１ＬＥＤ５ａ，第２ＬＥＤ５ｂ，
第３ＬＥＤ５ｃ）は、それぞれ発光色が異なるもので構
成され、例えば、“赤色”、“緑色”、或いは“黄色”
のいずれかの色を発光するＬＥＤが用いられる。この実
施例では、各ＬＥＤの位置が光点となっている。また、
これら３つのＬＥＤは、常時発光しているものとする。

【００２２】３つのＬＥＤ５ａ〜５ｃの取り付け位置
は、移動体１が当該空間において自身の位置を認識する
ときに必要な基準位置（絶対位置）として天井面に予め
設定した位置を基準に定められる。具体的には、当該空
間を一つの座標系（後述の「グローバル座標系」）とし
て設定し、この座標系における上記絶対位置の座標
（「絶対座標」という）を基準とした座標位置に、各Ｌ
ＥＤが配置される。

【００２３】移動体１は、ＣＣＤカメラ３でこれら３つ
のＬＥＤ５ａ〜５ｃを撮像し、その撮影画像をもとに移
動体１の絶対座標を求める。従って、ＣＣＤカメラ３に
は、３つのＬＥＤ５ａ〜５ｃを一画像に収めることがで
きるように、広範囲に撮像可能な広角レンズ(魚眼レン
ズまたは焦点距離２０ｍｍ以下)が用いられる。

【００２４】なお、検出手段は、３つの光を判別できる
ものであればよく、上記のＣＣＤカメラ３に限らない。
例えば、２次元ＰＳＤ（Position Sensitive Detecto
r）センサと任意の波長のみを通過可能とするバンドパ
スフィルタとの組み合わせ等を用いてもよい。また、天
井に取り付ける光点は、上記の３色のＬＥＤに限らな
い。例えば、異なる波長を出力する赤外線ＬＥＤ等を用
いてもよい。

【００２５】図２は、移動体１の内部に設けられる制御
装置の一部を構成する位置認識装置の構成を示す。この
図２に示すように、位置認識装置は、制御部１１、ＣＣ
Ｄカメラ３で撮影された画像を読み込み、Ａ／Ｄ変換に
よりデジタルの画像データへ変換する画像処理部１２、
その画像データを格納する画像記憶部１３、読み込んだ
画像データから導き出される各ＬＥＤの位置に関するデ
ータ（各ＬＥＤ間の角度等）を記憶するＬＥＤ位置情報
記憶部１４、後述の「移動体の絶対座標及び隣接する２
つのＬＥＤの絶対座標を結んで形成される三角形の外接
円の中心点と、当該２つのＬＥＤの絶対座標を結んだ直
線との距離」を算出する距離算出手段１５、後述の外接
円中心座標を算出する外接円中心座標算出手段１６、移
動体１の絶対座標を算出する移動体絶対座標算出手段１
７、移動体の現在向いている方向を算出する移動体方向
算出手段１８、及び、この位置認識装置での算出結果を
移動体１の走行制御を行なう走行制御部（図示省略）に
伝送する算出結果出力部１９を備える。

【００２６】以下、上記構成の位置認識装置において、
ＣＣＤカメラ３により撮像した３つのＬＥＤ５ａ〜５ｃ
を含む画像から移動体１の絶対座標を求める一連の処理
手順について説明する。

【００２７】まず、制御部１１において、画像記憶部１
３に格納されている画像に基づき、３つのＬＥＤ５ａ〜
５ｃに関する各種情報を取得する処理（ＬＥＤ情報取得
処理）が行なわれる。

【００２８】図３は、ＬＥＤ５ａ〜５ｃを撮像した画像
の模式図である。「ＬＥＤ情報取得処理」では、まず、
当該画像を、画像の中心点（これは、移動体１の絶対位
置でもある）を原点（以下、「仮想原点」という）とし
たｘ−ｙ座標系（「仮想座標系」または「ローカル座標
系」という。）で表す。この座標系におけるｘ軸を「ｘ
_ｒ軸」と称し、ｙ軸を「ｙ_ｒ軸」と称する。以下の説明
では、この座標系を「ローカル座標系」と称する。ま
た、３つのＬＥＤ５ａ〜５ｃの中心点をそれぞれＡ，
Ｂ，Ｃとし、これら各中心点の「ローカル座標系」にお
ける座標を、それぞれ（ｘ_ｒ１，ｙ_ｒ１），（ｘ_ｒ２，
ｙ_ｒ２），（ｘ_ｒ２，ｙ_ｒ２）とする。

【００２９】上記ローカル座標系において、仮想原点か
ら各ＬＥＤ５ａ〜５ｃの中心点Ａ，Ｂ，Ｃまでの距離を
それぞれＤ_１，Ｄ_２，Ｄ_３とし、ｘ_ｒ軸の正方向を基準
とした各ＬＥＤ５ａ〜５ｃの成す角度、すなわち各ＬＥ
Ｄ５ａ〜５ｃの中心点を極座標で表したときの角度を、
それぞれα_１，α_２，α_３とする。これらは、それぞれ
画像から求められる。

【００３０】ここで、ＣＣＤカメラ３に魚眼レンズを用
いた場合、画像全体がゆがむため、各ＬＥＤの極座標
は、次式（１）で補正して求めるものとする。なお、ｈ
は撮像面から天井面までの距離、ｆは焦点距離である。

【００３１】

【数１】

【００３２】また、角度α_１，α_２，α_３は、次のとお
りである。

【００３３】

【数２】

【００３４】これより、魚眼レンズを用いても、角度情
報のみ用いる場合は、補正する必要がないことになる。

【００３５】以上のように、画像から求められたデータ
をもとに、次の情報（画像情報）が得られる。

【００３６】１．隣接する各ＬＥＤ間の角度（α_１２，
α_２３，α_３１）。

【００３７】２．仮想原点から各ＬＥＤまでの距離を長
い順に並べたときのＬＥＤの順番。

【００３８】３．ｘ_ｒ軸の正方向から反時計回りで見え
るＬＥＤの順番、すなわち各ＬＥＤの極座標を小さい順
に並べたときのＬＥＤの順番（図３の場合は、第１ＬＥ
Ｄ５ａ→第２ＬＥＤ５ｂ→第３ＬＥＤ５ｃの順番）。

【００３９】以上の画像情報は、上記ＬＥＤ位置情報記
憶部１４に格納される。

【００４０】図４は、移動体１の移動する空間の所定位
置を原点としたｘ−ｙ座標系（「絶対座標系」又は「グ
ローバル座標系」といい、以下の説明では「グローバル
座標系」と称する）における、移動体１の位置及び各Ｌ
ＥＤ５ａ〜５ｃの位置を示す。ここで、「グローバル座
標系」における座標を「絶対座標」と称する。この図４
に示す例では、グローバル座標系における移動体１の位
置を示す絶対座標はＲ（ｘ_Ｒ，ｙ_Ｒ）、第１ＬＥＤ５ａ
の位置を示す絶対座標はＡ（ｘ_１，ｙ_１）、第２ＬＥＤ
５ｂの位置を示す絶対座標はＢ（ｘ_２，ｙ_２）、第３Ｌ
ＥＤ５ｃの位置を示す絶対座標はＣ（ｘ_３，ｙ_３）で示
される。なお、各ＬＥＤ５ａ〜５ｃの絶対座標は既知と
する。

【００４１】次に、図４に示したグローバル座標系にお
ける各ＬＥＤ５ａ〜５ｃの絶対座標に基づいて、移動体
１の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）を算出する手順について
説明する。

【００４２】まず、移動体１の絶対座標Ｒ（ｘ_Ｒ，
ｙ_Ｒ）と隣接する２つのＬＥＤの絶対座標を直線で結ん
で構成される三角形を考え、この三角形の外接円の中心
座標を求める。

【００４３】この図４に示すように、Ｒ（ｘ_Ｒ，
ｙ_Ｒ）、Ａ（ｘ_１，ｙ_１）及びＢ（ｘ_２，ｙ_２）を直線
で結んで構成される三角形の外接円Ｔ_１の中心点座標を
Ｏ_１（ｘ_ｏ _１，ｙ_ｏ１）、Ｒ（ｘ_Ｒ，ｙ_Ｒ）、Ｂ
（ｘ_２，ｙ_２）及びＣ（ｘ_３，ｙ_３）を直線で結んで構
成される三角形の外接円Ｔ_２の中心点座標をＯ_２（ｘ
_ｏ２，ｙ_ｏ２）、そして、Ｒ（ｘ_Ｒ，ｙ_Ｒ）、Ｃ
（ｘ_３，ｙ_３）及びＡ（ｘ_１，ｙ_１）を直線で結んで構
成される三角形の外接円Ｔ_３の中心点座標をＯ_３（ｘ
_ｏ３，ｙ_ｏ３）とする。

【００４４】外接円の中心座標を求めるまでの手順につ
いて、外接円Ｔ_１の中心点座標Ｏ_１（ｘ_ｏ１，ｙ_ｏ１）
を求める場合を例に挙げて説明する。

【００４５】（i）Ａ（ｘ_１，ｙ_１）及びＢ（ｘ_２，ｙ
_２）を結ぶ直線Ｌ_１を示す式を求める。

【００４６】

【数３】

【００４７】（ii）直線Ｌ_１と外接円Ｔ_１の中心点Ｏ_１
との距離Ｓ_１の関係式を求める。

【００４８】

【数４】

【００４９】ここで、外接円Ｔ_１の半径をr_１とする
と、

【００５０】

【数５】

【００５１】さらに、外接円Ｔ_１の半径r_１は、正弦定
理より、

【００５２】

【数６】

【００５３】である。ここで、中心点Ｏ_１に隣接する２
つのＬＥＤ５ａ，５ｂの成す角度α_１ _２は、前述のよう
に、ＬＥＤ位置情報記憶部１４に格納されているので、
この角度α_１２を上記（６）式に代入することで、外接
円Ｔ_１の半径ｒ_１が求められる。そして、このr_１を上
記（５）式に代入することで、距離Ｓ_１が求まる。ここ
で、上記（i）〜（ii）の手順は、位置認識装置におけ
る距離算出手段１５において行なわれる。

【００５４】（iii）外接円Ｔ_１の中心点を通り、２つ
のＬＥＤ５ａ，５ｂの中心点Ａ，Ｂを結ぶ直線Ｌ_１と直
交する直線Ｌ_４を示す式を求める。すなわち、

【００５５】

【数７】

【００５６】（iv）上記（４）式及び上記（７）式よ
り、次の（８）式を導く。

【００５７】

【数８】

【００５８】ここで、Ｅ_１、Ｆ_１、Ｇ_１は、次の通りで
ある。

【００５９】

【数９】

【００６０】（v）上記（８）式より、外接円Ｔ_１の中
心点のｘ座標（ｘ_ｏ１）を求める。

【００６１】

【数１０】

【００６２】なお、このｘ_ｏ１は、上記（１２）式に示
すように、２つ求められるが、この時点では、どちらの
解が真値かどうか判別できない。

【００６３】（vi）（１２）式を（７）式に代入し、外
接円Ｔ_１の中心点のｙ座標ｙ_ｏ１を求める。

【００６４】（vii）外接円Ｔ_２の中心点座標Ｏ_２（ｘ
_ｏ２，ｙ_ｏ２）及び外接円Ｔ_３の中心点座標Ｏ_３（ｘ
_ｏ３，ｙ_ｏ３）についても、同様の手順で求める。

【００６５】ここで、上記（iii）〜（vii）の手順は、
図２の位置認識装置における外接円中心座標算出手段１
６において行なわれる。

【００６６】次に、上記で求められた外接円の中心座標
を用い、移動体１の絶対座標Ｒ（ｘ _Ｒ，ｙ_Ｒ）を求める
手順について説明する。以下の（I）〜（XII）の手順
は、位置認識装置における移動体絶対座標算出手段１７
において行なわれる。

【００６７】（I）求められた外接円の中心座標のうち
の２点を通る直線の方程式を求める。外接円Ｔ_１の中心
点Ｏ_１（ｘ_ｏ１，ｙ_ｏ１）と外接円Ｔ_２の中心点Ｏ
_２（ｘ_ｏ _２，ｙ_ｏ２）の２点を通る直線Ｌ_７の方程式
は、

【００６８】

【数１１】

【００６９】外接円Ｔ_１の中心点Ｏ_２（ｘ_ｏ２，
ｙ_ｏ２）と外接円Ｔ_３の中心点Ｏ_３（ｘ_ｏ３，ｙ_ｏ３）
の２点を通る直線Ｌ_８の方程式は、

【００７０】

【数１２】

【００７１】外接円Ｔ_３の中心点Ｏ_３（ｘ_ｏ３，
ｙ_ｏ３）と外接円Ｔ_１の中心点Ｏ_１（ｘ_ｏ１，ｙ_ｏ１）
の２点を通る直線Ｌ_９の方程式は、

【００７２】

【数１３】

【００７３】と表せる。

【００７４】（II）上記式（１３）〜（１５）で表され
る３つの直線Ｌ_７，Ｌ_８，Ｌ_９と各々関連するＬＥＤと
の距離Ｓ_７，Ｓ_８，Ｓ_９を求める。ここで、直線Ｌ_７と
関連するＬＥＤは第２ＬＥＤ５ｂ、直線Ｌ_８と関連する
ＬＥＤは第３ＬＥＤ５ｃ、そして、直線Ｌ_９と関連する
ＬＥＤは第１ＬＥＤ５ａである。

【００７５】

【数１４】

【００７６】（III）各ＬＥＤ５ａ〜５ｃと、移動体１
の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）を通る直線Ｌ_１０，
Ｌ_１１，Ｌ_１２を示す方程式を求める。直線Ｌ_１０を示
す方程式は下記（１９）式で表され、直線Ｌ_１１を示す
方程式は下記（２０）式で表され、直線Ｌ_１２を示す方
程式は下記（２１）式で表される。

【００７７】

【数１５】

【００７８】（IV）直線Ｌ_７と直線Ｌ_１０とが直交する
という関係より、移動体１の絶対座標Ｒのｙ座標ｙ_Ｒを
求める。

【００７９】

【数１６】

【００８０】（V）直線Ｌ_８と直線Ｌ_１１とが直交する
という関係より、移動体１の絶対座標Ｒのｙ座標ｙ_Ｒを
求める。

【００８１】

【数１７】

【００８２】（VI）直線Ｌ_９と直線Ｌ_１２とが直交する
という関係より、移動体１の絶対座標Ｒのｙ座標ｙ_Ｒを
求める。

【００８３】

【数１８】

【００８４】（VII）次のような置き換えを行い、上記
（２２）式を下記（２６）式のように再定義し、上記
（２３）を下記（２７）式のように再定義し、上記（２
４）を下記（２８）式のように再定義する。

【００８５】

【数１９】

【００８６】（VIII）直線Ｌ_７と移動体１の絶対座標Ｒ
（ｘ_Ｒ，ｙ_Ｒ）との距離Ｓ_１０、直線Ｌ_８と移動体１の
絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）との距離Ｓ_１１、及び、直線
Ｌ_９と移動体１の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）との距離Ｓ
_１２を求める。

【００８７】距離Ｓ_１０は、直線Ｌ_７が、第２ＬＥＤ５
ｂの中心点Ｂと移動体１の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）を
結ぶ線分の垂直２等分線であることから、次式のように
表せる。

【００８８】

【数２０】

【００８９】距離Ｓ_１１は、直線Ｌ_８が、第３ＬＥＤ５
ｃの中心点Ｃと移動体１の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）を
結ぶ線分の垂直２等分線であることから、次式のように
表せる。

【００９０】

【数２１】

【００９１】距離Ｓ_１２は、直線Ｌ_９が、第１ＬＥＤ５
ａの中心点Ａと移動体１の絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）を
結ぶ線分の垂直２等分線であることから、次式のように
表せる。

【００９２】

【数２２】

【００９３】なお、Ｖｉ，Ｗｉ（ｉ＝１，２，３）は、
前掲の式（１６）〜（１８）で定義された値である。

【００９４】（IX）上記（２９）式のｙ_Ｒに上記（２
６）式を代入して両辺二乗し、次式のようなｘ_Ｒの２次
方程式で表す。

【００９５】

【数２３】

【００９６】ここで、

【００９７】

【数２４】

【００９８】と置き換え、上記（３２）式の両辺を二乗
すると、次式のように表せる。

【００９９】

【数２５】

【０１００】（X）上記（３４）式を解いて、移動体１
の絶対座標のｘ軸の座標値ｘ_Ｒを求める。ｘ_Ｒは、解の
公式より下記のように２つ求められる。

【０１０１】

【数２６】

【０１０２】そして、求まったｘ_Ｒを上記（２２）式に
代入することにより、ｙ軸の座標値ｙ_Ｒも同様に２つ求
められる。

【０１０３】（XI）上記（３０）式のｙ_Ｒに上記（２
７）式を代入して両辺二乗し、

【０１０４】

【数２７】

【０１０５】と置き換えることで、次のようにｘ_Ｒが２
つ求められる。

【０１０６】

【数２８】

【０１０７】そして、求まったｘ_Ｒを上記（２３）式に
代入することにより、ｙ軸の座標値ｙ_Ｒも同様に２つ求
められる。

【０１０８】（XII）上記（３１）式のｙ_Ｒに上記（２
８）式を代入して両辺二乗し、

【０１０９】

【数２９】

【０１１０】と置き換えることで、次のようにｘ_Ｒが２
つ求められる。

【０１１１】

【数３０】

【０１１２】そして、求まったｘ_Ｒを上記（２３）式に
代入することにより、ｙ軸の座標値ｙ_Ｒも同様に２つ求
められる。

【０１１３】以上の（I）〜（XII）により、移動体１の
絶対座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）の解は６組求められるが、上
述の通り、３つの外接円Ｏ_１，Ｏ_２，Ｏ_３の中心座標の
解がそれぞれ２つ求められたことから、移動体１の絶対
座標Ｒ（ｘ_Ｒ，ｙ_Ｒ）の解は、合計１２個求められる。

【０１１４】ここで、本手法の場合、求められた１２個
の座標のうち、同一座標が３つ存在する座標が２組含ま
れる。そして、その２組のうちの片方が、求めたい座
標、すなわち、グローバル座標系における移動体１の現
在の座標を表すものとなる。

【０１１５】ここで、絞られた２組の座標から、移動体
１の実際の位置、すなわち移動体１の絶対座標の真値を
判別する方法として、各ＬＥＤ間の角度だけでは、２つ
のうちどちらが求めたい値なのか一意に決定することは
できない。そのため、本発明では、上記画像情報を用い
ることで、一意に移動体１の絶対座標の真値を決定す
る。

【０１１６】まず、絞られた２組の座標候補のそれぞれ
を原点とした２つの座標系において、各ＬＥＤの絶対座
標を極座標で表す。そして、画像情報としてＬＥＤ位置
情報記憶部１４に格納されている上述の「ｘ_ｒ軸の正方
向から反時計回りで見えるＬＥＤの順番」と一致するＬ
ＥＤの配置となっている方の原点を、移動体１の絶対座
標の真値とする。

【０１１７】また、真値と判定した移動体１の絶対座標
を原点とした座標系において、当該原点から各ＬＥＤま
での距離を長い順に並べたときのＬＥＤの順番を定め
る。そして、この順番が、画像情報としてＬＥＤ位置情
報記憶部１４に格納されている上述の「仮想原点から各
ＬＥＤまでの距離を長い順に並べたときのＬＥＤの順
番」と一致しているかどうかについても判定する。

【０１１８】移動体１の絶対座標Ｒ(ｘ_Ｒ，ｙ_Ｒ)の真値
が求まれば、続いて、グローバル座標系とローカル座標
系が成す角度を求める。この角度を求めることで、移動
体１の現在向いている方向が求められる。

【０１１９】一例として、図５に概念図を示す。移動体
１と１つのＬＥＤとの位置関係を考える。

【０１２０】ここで、ローカル座標系における当該ＬＥ
Ｄの中心点を極座標で表したときの角度α_Ｒは、当該Ｌ
ＥＤのローカル座標系における座標を（ｘ_r１，ｙ_r１）
とすると、次のように表される。

【０１２１】

【数３１】

【０１２２】次に、当該ＬＥＤの絶対座標（ｘ_１，
ｙ_１）と求められた移動体１の絶対座標Ｒ(ｘ_Ｒ，ｙ_Ｒ)
とから、移動体１と当該ＬＥＤとのグローバル座標系に
おける相対的な角度α_Ｇを求めると、以下のように表さ
れる。

【０１２３】

【数３２】

【０１２４】以上により、移動体１の角度θは次のよう
になる。ただし、ＬＥＤと移動体１の配置関係によりこ
の式は異なる。

【０１２５】

【数３３】

【０１２６】以上の角度θの算出は、位置認識装置にお
ける移動体方向算出手段１８において行われる。

【図面の簡単な説明】

【図１】本発明により位置認識を行う移動体とＬＥＤを
示す斜視図。

【図２】位置認識装置の構成を示すブロック図。

【図３】ＬＥＤを撮影した画像を示す模式図。

【図４】グローバル座標系における、移動体の位置及び
各ＬＥＤの位置を示す図。

【図５】移動体と１つのＬＥＤとの位置関係についての
概念図。

【符号の説明】

１…移動体、２…駆動輪、３…ＣＣＤカメラ、５ａ，５
ｂ，５ｃ…ＬＥＤ、１１…制御部、１２…画像処理部、
１３…画像記憶部、１４…ＬＥＤ位置情報記憶部、１５
…距離算出手段、１６…外接円中心座標算出手段、１７
…移動体絶対座標算出手段、１８…移動体方向算出手
段、１９…算出結果出力部。

フロントページの続きＦターム(参考） 2F065 AA03 AA37 BB05 BB15 BB29 CC00 EE08 FF09 FF61 GG07 GG13 JJ03 JJ26 LL10 QQ00 QQ03 QQ23 QQ24 QQ26 QQ28 3F059 AA11 BB07 DA02 DA05 DA08 DB04 DB09 DC08 DD11 3F060 CA12 5H301 AA01 AA10 BB14 FF09 GG09

Claims

【特許請求の範囲】

【請求項１】絶対空間に置かれた移動体の位置認識を行
なう位置認識方法であって、移動体に設けられた検出手段により、基準面から略同一
の高さに設けられた光点からの光により光点の位置を検
出するステップと、前記移動体の現在位置を仮想原点とする仮想座標系にお
いて、その仮想原点及び前記検出手段により検出された
２つの光点の位置を結んで形成される三角形の外接円の
中心と該２つの光点の位置を結ぶ直線との間の距離を求
めるステップ、求めた距離を用いて、絶対空間の所定位
置を原点とした絶対座標系における前記外接円の中心座
標を求めるステップ、前記２つの光点の位置を結ぶ直線と前記外接円の中心点
との間の距離を求めるステップ、求めた前記距離を用いて、前記絶対空間の所定位置を原
点とした絶対座標系における前記外接円の中心座標を求
めるステップ、求めた前記中心座標を用いて、前記絶対座標系における
移動体の絶対座標を求めるステップとを含む移動体の位
置認識方法。
【請求項２】絶対空間に置かれた移動体の位置認識を行
なう位置認識装置であって、当該絶対空間において、基準面から略同一の高さに設け
られる、位置を識別するための光を発生する光点と、前記移動体に設けられ、前記光点からの光により光点の
位置を検出する検出手段と、前記移動体の現在位置を仮想原点とする仮想座標系にお
いて、その仮想原点及び前記検出手段により検出された
２つの光点の位置を結んで形成される三角形の外接円の
中心と該２つの光点の位置を結ぶ直線との間の距離を求
める距離算出手段と、該距離算出手段での算出結果を用いて、前記絶対空間の
所定位置を原点とした絶対座標系における前記外接円の
中心座標を求める外接円中心座標算出手段と、該外接円中心座標算出手段での算出結果を用いて、前記
絶対座標系における前記移動体の絶対座標を求める移動
体絶対座標算出手段とを備えたことを特徴とする位置認
識装置。
【請求項３】請求項２記載の位置認識装置において、前
記検出手段は撮像手段で構成され、前記距離算出手段
は、該撮像手段によって撮像した画像をもとに、画像の
中心位置及び２つの光点の位置を結んで形成される三角
形の外接円の中心と該２つの光点の位置を結ぶ直線との
距離を求めることを特徴とする位置認識装置。
【請求項４】請求項３記載の位置認識装置において、前
記距離算出手段は、前記撮像手段によって撮像した画像
上に、画像の中心点を仮想原点とした仮想座標系を設定
し、当該仮想座標系において各光点の位置を極座標で表
したときの隣接する２つの光点の間の成す角度を求め、
当該角度を用いて、前記仮想原点及び２つの光点の位置
を通る外接円の半径を求め、そして、該２つの光点の位
置を結ぶ直線と前記外接円の中心点との距離を求めるこ
とを特徴とする位置認識装置。
【請求項５】請求項４記載の位置認識装置において、前
記仮想座標系におけるＸ軸の正方向と光点との成す角
度、及び、前記絶対座標系における前記移動体と該光点
との相対的な角度を求め、これら２つの角度の差によ
り、前記絶対座標系における前記移動体の向きを求める
移動体方向算出手段を備えたことを特徴とする位置認識
装置。