JPS60162902A - 移動物体の位置計測方式 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は移動物体の閉空間内の位置計測方式に係り、特
に搭載装置を用いて該物体が自己の位置を高精度に推定
するのに好適な方式に関する。

〔発明の背景〕

工場内等閉窒間を移動する物体、例えば走行ロボットを
制御するには、その刻々の位置および姿勢角を把握する
必要がある。従来この目的には次のよ、うな手段が試み
られてきた。

その１は、移動物体にジャイロ全搭載しそれが検出した
加速度全積分して刻々の位置および姿勢角をめる方式で
ある。この慣性航法方式は積分により誤差が発散するた
め、誤差を抑えるたぬに高精度ジャイロ全必要とし、搭
載装置が高価になるという欠点があった。

その２は、移動物体に光学センサとデータ処理装置全搭
載し、センサで撮影した画像から自己の位置および姿勢
角全推定する方法である。これには（１）データ処理装
置に環境の記述（モデル）を蓄えておき、センサのとら
えた画像とこの記述とを照合する。（２）時々刻々順次
とられた画像内の対応点を照合する。という２つの方法
があるが、いずれもほう大な演算量となり、一般に照合
精度が上らず、結果として該物体の位置、姿勢角の推定
精度が低くなるという欠点があった。

〔発明の目的〕

本発明の目的に建物等閉空間内を移動する物体が、自己
の３次元窒間における位置と姿勢角を精度良く計測する
手段を提供することにある。

〔発明の概要〕

この目的を達成するたぬ本発明では、閉空間内の既知の
相異なる位置に複数の点光源を設置し、これらを順次点
灯させ、移動物体に設けたセンサでその像をとらえ、そ
のセンサ面内の位置情報から該移動物体が自己の位置と
姿勢角を刻々算出するようにした点に特徴がある。

ここで本発明に用いる立体幾何学および推定法の原理′
ｆｔ説明する。初めにセンサによる点光源観測の幾何学
的関係を述べる。第１図の１は全開に固定された座標系
（Ｘ、Ｙ、Ｚ）で、これを基準座標系とする。移動座標
系２（Ｘｔ　Ｙ’＋　Ｚ）は移動物体に固定されたセン
サの焦点面３に固定されたものとする。ある瞬間にＶン
ズ４全通して焦点面３上に像を作る点光源を５−ｉ　（
ｉ＝１．２゜・・・、Ｎ）とする。その座標を、基準座
標系でｒ４＝　（Ｘｔ、Ｙ＋、Ｚｌ　）Ｔ１移動座標系
でｒ！”’（”＋。

ｙｌ、Ｚｌ）Ｔとする。ここにＴは行列の転置記号であ
る。

このｒ！とｒ５の関係は次のように与えられる。

ｒ　！　＝　Ｇ　（ｒ　！Ｉ　−ｒ　ｍｏ　）　・・−
−−−−−−（１）式ここにｒｌ。””　（Ｘｏ、Ｙ、
、Ｚｏ）Ｔは移動座標系２の原点７の座標全基準座標系
で表わしたものである。行列Ｇは座標系１から座標系２
への回転行列で第２図のごとく、Ｘ、Ｙ、Ｚの各軸まわ
りの回転全表わすオイラ角θ８．θア、θ２を用いて次
式で与えられる。

一方点光源５−ｉの焦点面３上の像６−ｉの位置（ｘＬ
ｙ：）は次式で与えられる。

ここにｆばレンズ４の焦点距離で、レンズ４の収差は無
視できるものとする。

以上の幾何学的関係に従い、センサにより観測した点光
源の像の位置から、移動物体の位置と姿勢角を推定する
方法を述べる。ある瞬間に観測される点光源５−ｉ　（
ｉ＝１．２．・・・、Ｎ）について、基準座標系での位
置ｒ４は既知である。また観測によりセンサ面３上の像
位置（Ｘ１＋ｙｔ）が与えられる。このとき（１）　、
　：（２）　、　（３）式の関係を用いて、移動物体の
未知の位置ｒ□。および、姿勢角を示すオイラ角θオ、
θア、θ７をめることが課題である。この関係は次の最
小２乗推定問題として定式化できる。

未知パラメータをベクトルＷ−（Ｘ、Ｙ、Ｚ。

θア　θア　θア）Ｔで、また（３）式の観測モデルを
ｙｔ＝ｇｌ（ｘ）　・・・・・・・・・・・・（４）式
で表わす。但しｙ＋＝（Ｘ：　ｙ：）Ｔ、ｇｌ（ｘ）＝
を 、ＨＸ）＝（Ｘ−Ｘ、）ＴＷ、（Ｘ−Ｘ、）で定義する
。このとき問題は（１）、　（２）式の制約のもとに、
（５）式のＪ　（Ｘ）　’に最小にする未知パラメータ
Ｘの値をめることである。

ここに（５）式において、Ｘ６ばＸＶｃついての先験情
報としての推定値、Ｗ、Ｕその誤差共分散行列の逆行列
、またＷＩは観測量ｙ＋の誤差共分散行列の逆行列であ
る。

この最小２乗推定問題を例えば数値解法により解けば解
として得られたベクトルＸが、所望の移動体の対象時点
での位置および姿勢角を与える。

以上の議論では、ある瞬間に焦点面３上に作られた像６
＝　（ｉ＝：ｌ、２．・・・、Ｎ）が識別できσ点光源
５−ｉと正しく対応づけられること全前提としている。

しかし一般に、移動体の位置および姿勢角が未知の場合
には、焦点面上にできる点光源の像の位置を正確に予測
することは困難である。

また、移動物体の位置、姿勢角によっては複数の点光源
の像がセンサ面上で近接あるいは重畳し、識別が困難に
なる場合もある。

そこで本発明では、点光源を一定の時間間隔に従い順次
点灯し、センサ面上にできる点光源の像が高々１となる
ようにして、像の識別すなわち点光源との対応が正確か
つ容易に行なえるようにした。このとき移動物体の位置
、姿勢角の推定用に必要な一定個数の像を入力する間に
該物体が移動してしまうという問題が生ずるが、そ７″
Ｌ′ｆ：次のように、移動モデルを導入することにより
解決した。

移動物体は、あらかじめ外部から与えられた点光源の点
灯時刻に関する情報と、内蔵した時計とにより各時刻に
点灯された点光源の識別ができるものとする。また位置
、姿勢角の推的に必要な個数の像を得るσ〕に必要な時
間内の移動物体の移動は等速直線運動であり、その間に
該物体は姿勢を一定に保つものとみなす。物体が走行ロ
ボットであればこの仮定は十分妥当と考えらｊる。この
とき物体の位置ｒｍ０＝（ＸｏＹｏＺｏ）′ｒ　は時刻
ｔの関数として次式で与えられる。

ここにＸ。ｏ　Ｉ　Ｙｏ。ＩＺｏａばｔ＝Ｑにおける位
置を、［１，Ｖ、Ｗは速度を表わす。

いま一定時間内にＮ個の像を観測するものとする。第１
番目の像の観測時刻ｋ　ｔＩとすれば、その時の物体の
位置は、（６）式の１＝１．とおいてまるＸ。（ｔＩ　
）　、　Ｙｏ　（ｙυ、Ｚ。（’＋）ｋ用いてｒ、ｎｏ
−（Ｘｏ（ｔｌ）Ｙｏ（ｔｌ）Ｚｏ（ｔＩ））Ｔで与え
られる。この時像を作った点光源５−ｉの基準座標系で
の座標ｒ！と、移動座標系での座標ｒ１　との関係は（
１）式の代りに次の（１）７式で与えられる。

ｒ五二〇（ｒ！ｎ−ｒ４）　・・・・・・・・・（１γ
式ここにＧは（２）式で与えられる回転行列である。

この時移動物体の位置および姿勢角に関する推定問題は
次のように定式化される。

未知パラメータベクトルＵＸ＝（Ｘ、。ＹｏｏＺｏ。

ＴＪＶＷθ８θアθ、）Ｔ　で与えられる。この時問題
は（１）’、　（２）式の制約のもとで、（５）式のＪ
　（Ｘ）全最小にする未知パラメータＸの値をめること
である。

この最小２乗推定問題は非線形であるので、伝えば次の
二−ユートンーラフソン法を用いて解ｘ２求ぬる。第３
図にその計算フローを示す。

Ｘの初期推定値ｘ０から出発し、逐次その修正量Δχを
算出し、ＸをＸ十ΔＸに修正するという過程（ｒ　Ｊ　
（Ｘ）が変化しなくなるまで繰返す。この修正量は次式
で与えられる。

ここにＸＩ　＋　Ｘ２・、・・・、Ｘ、Ｖｉ未知ベクト
ルＸの要素である。

計算フローは次のとおりである。

まず初期化として、 Ｘ　：　＝Ｘ０ Ｊｎ、Ｗ：＝閃（あるいは十分大きな正数）ΔＸ：＝０ と設定する。次に以下の■・・・■を、十分率さな正数
εに対し、ＩＪｏ、ｄ−ＪｎａＷ］〈εとなるまで繰返
す。

（９） ■　Ｊ　ｏａｄ　：　”　Ｊ　ｎｅｗ ■　）に：＝Ｘ＋ΔＸ ■　Δχを（６）式によりめる。

■　Ｊ　（ｘ）　−ｉ　（５）式によりめ、Ｊ　ｌｏｗ
　Ｈ二Ｊ　（ｘ）とする。

この逐次計算により得られ、たＸが、所望の移動物体の
位置、速度および姿勢角を与える。

〔発明の実施例〕

以下、本発明全実施例にもとづき詳細に説明する。

第４図は本発明による移動物体の位１に計測方式の全体
構成を示す図である。１５は移動物体、１６はセンサで
ある。点光源５−１．・・・、５−Ｎ。

は、制御装置１７で点灯が制御される。点灯回路１８は
、タイマー１９の与える時刻と、テーブルメモリ２０に
蓄えられた各光源の点灯時刻に応じて、点光源５−ｉ’
１ｌｌｉ＋次点灯する。

第５図は、移動物体１５に搭載さｊた、位置・姿勢角推
定装置の構成図である１、センサ１６内蔵のＶンズ４を
通して外部からの入党がセンサ面３（１０） 上に像を結ぶ。センサ面上の像は光′ＮＬ変換装置２３
全通してディジタル信号に変換され、画像メモリ２４に
蓄えられる。点像検出回路２５はメモリ２４中の画像デ
ータの中から点光源の像を検出し、その位置座標”Ｉ＋
Ｙ：’ｆｃ出力する。一方読出回路２８はタイマー３０
の与える現在時刻にしたがいテーブルメモリ２９に蓄え
られた情報の中から、該時刻に点灯した光源の座標ｒ！
全読み出す。この光源傷付１’　ｘｔ　＋　Ｙ　ｔ　の
検出と、光源座標ｒ、、！の読み出しの動作を、順次点
灯する一定個数Ｎの光源について繰返す。演算装＠２６
１−１：こうして得られたｌ＋　Ｙ：、ｒよ＜１＝１．
２．・・・。

Ｎ）を入力データとし第３図に示した手順に性い、移動
物体の位置、姿勢角に関する未知パラメータｘ−（ｘｏ
ｏｙｏｏｚｌｌｏｌＪ　ｖｗ　ｖｘＯｙＵ、　）Ｔｖ推
定する。この推定結果は、例えば移動物体駆動制御装置
２７に送られ、該物体の行動の制御に利用できる。

次に第４図に示したテーブルメモリ２０および第５図に
示したテーブルメモリ２９の内容につい（１１） て述べる。第６図はメモリ２０の内容を示すＩン１であ
る。メモリ２０には周ＭＴと、各光源について識別扁と
時刻ｔ１が蓄えられている。、タイマー１９の起動時刻
音ｔ−Ｏとすると、第１番目の光源　５　−　ｉ　に丁
　ｔ　−ｔ　１　＋　ｎ　Ｔ　（世　Ｌｎ＝０．１，２
゜・・・）なる時刻に点灯さハる。この点灯制御全点灯
回路１８が行なう、。

一方テーブルメモリ２９には第７図に示す如くテーブル
メモリ２０と同一の周期、各光源についての識別扁と時
刻ｔ１の他に、各光源についての位１に座標Ｘ＋、Ｙ＋
、Ｚ＋　とが蓄えられている。読出回路２８は、タイマ
ー出力時刻ｔ　ｙ、定期的に読取り、テーブルメモリ２
９の中の各光源の時刻ｔ　、　［対し、ｔ＝ｔｌ−１−
ｎＴ（ｎ＝ｏ、１．”’）なるｉがあるか否かを判定し
、もしあればそのｉに対応する位置座標Ｘ＋、Ｙ＋、Ｚ
＋　全メモリ２９から胱出す。

テーブルメモリ２０および２９の内容は第４図。

第５図に示すようにそｊぞれ外部２２および３２から与
えらねる。またタイマー１９と３０とは同（１２） 期・全とる。すなわち起動時刻ｔ＝Ｑを一致させる必要
がある。これも外部２１および３１から与えられるもの
とする。

なおセンサ面３上の像の光電変換装＋ｔ２３によるテイ
ジタル化は、センサ固有の周期、例えばテ７ビ信号周期
で行なうが、読出回路２Ｂがテーブルメモリ２９から位
置座標を読み出す際、同期信号を変換装置２３に送るこ
とによって動作させる方法をとってもよい。

以上述べた如く本実施例によれば、点光源を順次所定の
時刻に点灯することにより、その像のセンサによる検出
が正確、容易になる。この像の位置情報から移動物体搭
載の演算装置で、該物体の位置、姿勢を正確に推定する
ことが可能となる。

本実施例では、移動物体は基準座標糸に対し６自由度の
運動をする、すなわちＸ、Ｙ、Ｚの移動およびθ８．θ
ア、θ７　の回転を許すものと仮定している。しかし運
動に制約がある場合には推定問題を簡単化できる。例え
ば床面全走行する物体で、Ｚ方向の移動がなく、Ｚ軸ま
わりの回転（ヨーイＣ１１） ング）以外の姿勢変動がない、すなわちθ８−θア＝０
とみなせる場合には、推定対象である未知ベクトルＸは
ｘ　＝　（Ｘｏ。Ｙｏ。ＵＶ　θ７）Ｔになる。

また本実施例でに、テーブルメモ１２０，２９の内容全
外部から設定できるので、対象とする閉空間の変化に適
応させることが可能である。

〔発明の効果〕

本発明によれば、複数の点光源という簡単な地上設備と
、通常の光学センサとマイクログロスツサ等による演算
装置という簡単な搭載装置との組合せにより、移動物体
の位置の高精な推定が可能となる。本発明による位置の
推定を周期的に行なうことにより、移動物体の閉伊間内
の追跡が可能となる。追跡結果は該物体搭載の制御装置
での自律的な制御、あるいに伝送手段によりセンタの管
制装置へ送ることによりセンタでの該物体の管制・制御
にそれぞれ利用可能であり、走行ロボットの有効活用を
可能にするという効果がある。

【図面の簡単な説明】

第１図に９間固定座標系と物体固定座標系との（１４） 定義を表わす図、第２図は＃２座標系の間の回転を示す
図、第３図は物体の位置、姿勢角を推定するたぬの最小
２乗法の解の計算手順を示す図、第４図は本発明による
位置計測方式の全体構成図、第５図は車載装置の構成図
、第６図はう１０源の制御装置内のテーブルメモリの内
容図、第７図は上記車載装置内のテーブルメモリの内容
図である。 １・・・借問固定座標系、２・・・物体固定座標系、３
・・・センサ面、４・・・Ｖンズ、５・・・点光源、６
・・・点光源の像、１５・・・移動物体、１６・・・セ
ンサ、１７・・・光源制御装置、１９・・・タイマー、
２０・・・テーブルメモリ、２３・・・光電変換装置、
２４・・・画像メモリ、２５・・・点像検出回路、２６
・・・演算装置、２８・・・読（１５） Ｙ１図 冨　２　図 猪４図 員　５　図 ローーー］ お　ｌ　図 第　７　図

Claims (1)

【特許請求の範囲】
1. １、閉空間内の複数地点に点光源を設置し、センサとデ
   ータ処理装置から成る搭載装置を移動物体に搭載し、順
   次点灯される点光源の上記センサ面上での像位置から、
   該物体の位置、速度および姿勢角全推定すること全特徴
   とする移動物体の位置計測方式。