JPS61277010A - 移動体位置姿勢計測方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、移動台車等の移動体の位置進行方向制御に
おいて、移動体の位置、姿勢をカメラに自より検出する
計測方法に関するものである。

〔従来の技術〕

移動台車の位置、姿勢を検知する方法として、囚　床面
に付けられたマークなチンピカメラにより観測する方法
、床面に元反射板また・工白線を付け、元近接七ンサに
よって元の反射率の葦を計測する方法、床面に磁性似を
置き。

近接磁気センナにより磁性板の存在’Ｙ６ｉ１ｍするこ
とによる方法。

俤）　大きなマーク、例えば縦蔵マークを壁につげ、こ
のマークなチンピカメラで１１！ＩＩＩＩしてカメラ方
位を計測する方法。

等が用いられている。

〔発明が解決しようとする問題点〕

ｉａ記囚の従来の方法は１台車の位置を大雑把に知るこ
とはできるが、精度は大変悪い。また、台車が予定のコ
ースからはずれ一旦基準マークがチンピカメラまたはセ
ンナ等により観測できなくなった場合に、台車の位置姿
勢を知ることができない。さらに、また、マークや白線
が床面に付けられているので、汚れやすく、常時掃除を
する必要があり、保守上問題がある。

また、Ｈ「記０３１の場合にを工、カメラがマーク方向
ｔおよそ向いているかどうかの判定には使用できるが、
絶対座標系でのカメラ位置方位’ｌ’ｉＥ嬶に昶ること
は不可能である。

以上述べたように従来は、移動体に積載されたカメラに
よっては移動体の位置方位を正確に求めることができな
いという問題点があった。

この発明は、上記の問題点を解決するためＫなされたも
ので、簡単な構成で、高精＆九計測ができる移動体位置
計測方法を提供することを目的とする。

〔問題点を解決するための手段〕

この発明にかかる移動体位置姿勢計測方法は。

カメラにより３次元分布した複数マーク１ｋｗｉｌ測し
。

カメラ内部パターンの算出、誤差を表わす誤差パラメー
タな小さくしてカメラ位置姿勢パラメータを算出するこ
とにより、移動体の位置姿勢を計測するものである。

〔作用〕

この発明においてを工、窒間上または移動体のどちらか
一方に３次元分布した複数のマークｔ、他方に備えたカ
メラで観測することによりカメラの位置姿勢を算出し、
これに２！！！；ついて移動体の位置姿勢を計測する。

〔実施例１〕 第１図はこの発明の第１の実施例を説明する図であって
、１は移動台車、２はテレビカメラ、３は画像処理装置
、４はデータ処理装置、５は複数マークが付けられたマ
ーク板である。６は車輪制御装置、Ｔは車輪である。テ
レビカメラ２でマーク板１’観測し、マーク像は画像処
理装置３で２値化、ラベリング処理され、マーク位置が
識別される。データ処理装置４でマーク番号の識別、カ
メラ位置姿勢パラメータの算出７行い、さらＫ、移動台
車１の位置姿勢データに換算した後、目的地と移動台車
１の位置関係を求めて、目的地に至る経路に沿った車輪
方位の修正値と車輪の所要回ｆｔ数を算出する。これら
の値は車輪制御装置１６に送られ、車輪Ｔの方位が修正
され、所定の回転量が車輪Ｔに与えられる。

！２図はマーク板５におけるマーク配置の具体Ｐｌ？示
したものであり、第３図をエマーク板を斜めから見た図
である。さらに第４図をエマーク番号の配置例を示した
ものである。

第２図〜第４図において、円形部分が白丸マークであり
、奇数行のマークＹ：５Ａ、偶数行のマークｖ５Ｂで示
した。その他の部分は黒である。奇数行のマーク５への
サイズは１例えば４ｎφ、偶数行のマーク５Ｂは６ｆｉ
φである。但し、マーク番号４５には１０ｗ１φ、５５
には８Ｗφとしている。また、偶数行のマーク５Ｂは、
マーク板面に立てられた黒色円柱の上層につげられてい
る。この円柱の高さは実験例では５０Ｍとした。また。

マーク間隔は５９ｍとした。

第５図は画像処理装置１１３におけるマーク識別及びデ
ータ処理装置４におけるマーク番号識別の処理手順を示
したものである。第２図〜第４図に示したマーク板５′
Ｙ：対象として説明する。なお、（１）〜α３は各ステ
ップを示す。

カメラによるマーク画像入力ヲ行い（１）、このマーク
画像を２値化しく２）、マーク画像より小さい不要画像
？孤立点として除去した後（（至）、ラベリング処理を
行うｉ４＋。各ラベリング領域の円形夏ヲ調べ。

円形度の低いラベリング領域をマーク以外のものとして
除去する（５）。また、画像辺縁部にかかるラベリング
領域を除去する（６）。次いで、各２ベリング領域の面
積を求めるとともＫ、最大の面積をもつラベリング領域
とその面積を得る（７）。ステップ（７）で得た最大面
積Ｓを基準とし、マークの大きさに応じた４つの面積区
分を設定し、各ラベリング領域ｔ４つのクラスに分類す
る（８）。この実施例では、イ）最大面積Ｓ、ロン（０
，６４±０．１）ＸＳ、ハ）（Ｏ，３６±０．１）ＸＳ
、→（０，１６±０．１）ＸＳの４つのクラスに分類し
た。イ）に該当するマークにはマーク番号４５、口）に
該当するマークはマーク番号５５とし、また、・→に該
当するマークは偶数行番号をもつマーク、二）に該当す
るマークは奇数行番号として分類さする。

ステップ（９）では、奇数行番号をもつ各マーク５Ａに
ついて、マーク画像の縦方向座標及び確定マークすなわ
ち、マーク４５０座標を利用して行の番号の識別を行う
。ステップ（１（１では、各行のマ−夕の中から、番号
４５及び５５’＆基準として番号１５．２５．３５，６
５．７５，８５．９５のマーク？選別する。次に当該マ
ークすなわち１桁目（１の位）が５のマークを利用して
、他のマークの番号付けｔ行５゜ステップαＤでは各マ
ークの外接長方形または重心を用いて各マークの中心座
標を求める。最後にステップα力においてｉ−り番号、
マークの画像メモリ上の座標及びマークの実空間座標を
記録したマークテーブルを作成する。

なお、ここでは第４図のマーク配列を例にして説明した
が、マークを光ファイバ端、またはＬＥＤ、ノーザ、ラ
ンプ等発光体を使用することができる。この場合に各行
または各マークを選択発光させて１行番号識別またはマ
ーク番号識別Ｙ：簡略化することも可能である。

マーク板５は広い範囲からマーク配列の３次元分布が観
測できる構成であればよく、第２図〜第４図で示したマ
ーク構成と類似の種々の構成が考゛えられる。従って、
率−のマーク板の代わりに複数のマーク板から成るマー
ク抜群から１つのマークテーブルＹＭ成しても構わない
。

次に、マークテーブル（マークリスト）Ｙ：便ってカメ
ラ位置姿勢パラメータを求める。この処理は大別して２
つのステップからなる。第１ステツプでを工、カメラパ
ラメータの近似値を、誤差パラメータとともに解析的に
求める。第２ステツプは楕Ｉ！ｔＹ：高めるための補正
ルーチンであり、第１ステツプで得た誤差パラメータが
小さくなるようなカメラパラメータを解析的に求める。

以下、カメラパラメータを算出する処理の方法を第１ス
テツプ、第２ステツプに分けて詳細ＫａＳ、明する。

（第１ステツプ） カメラレンズ中心が物体座標系（Ｘ、７．Ｚ）の（ｅ１
＋　　ｅｌ＋　　ｃｌ　　）に位置しているものとする
。カメラ座標系（Ｘｅ　　＊　　Ｙｅ　＊　　ｚｅ　　
）と物体座標系（Ｘ・　ｙ、ｚ）との関係は一般に次式
で書き表わされる。なお、座標系は第６図に示すように
・右手系とし、７Ｊメラ光軸方向Ｙｙｅ　　軸とする。

また、５はマーク板で、に＋ｙ＊ｚｋ工空関座標、α。

β、ψは方位角、２はテレビカメラで、Ｘ＠＊７□Ｚ、
はその軸である。

ただし１Ｍは物体座標系からカメラ座標系への変換マド
ＩＪクスである。Ｍの行列要素７次式で示すＯ Ｍ＝（ａｋｊ）　　　（ｋ、ｌ−１，２，３）　・−・
・（２１一方、カメラ中心からカメラ画面へ垂線を下し
たときの距離（光軸長）ンＬとし、その垂線と画面との
交点（光軸点）の座標′４ｔ（ｉｏ、ｊｏ）とし。

また、画面上の像と画像メモリ上の像との間のスケール
ファクタ（縮小率）′％：画像のｉ方向（横方向）につ
いてη＋−ｊ方向（縦方向）についてηｊとする。基準
マークのカメラ座標系における座標とその画像（１，ｊ
）とは次の関係を有している。

以後の計算で、変換マトリクスが直交すること、すなわ
ち １と；’ａ２ｋＪ”１（ｋ＝＝＝１＋２＋３２−−−−
−−−−−＋５ノ上Ｊｌｋ７１Ｌｖｍｌ　−０・・・・
・・・・・・・・・・・・・・・・・・・・・・・・・
・・・・（６）（ｋ←鴎） を保証させるため、第１３１式、第（４）式を次のよう
に書き直す。

η１・ｊ−（Ｌ＋δ）　Ｚ　＠／Ｆ　ｅ＋η３　　Ｊ６
＋ηｊεｊ　δと（ｇ＋、εｊ）は光軸長および元軸点
座標の誤差ヲ我わす＠差バクメータである。第（υ式、
第（２）式１を第（７）式に代入すれば次式が得られる
。

ａｊｌ　ｉｘ＋　＊’２２　ｉ　ｙ　＋ａ’２１　ｉ　
ｚ＋Ｂ’ｉ　ｘ＋Ｂ’ｊｙ＋Ｂ′ｌ’ｚ　−Ｂ’ｉ　ｅ
ｌ−Ｂ７　ｃ２−　Ｂ’３　ｃ３＝　ｉ　−・（８）た
だし、 Ｂｌ＝−Ｌａｌｌ−δａＩ７−η１１０１Ｌｔｊ−η山
ａｔｊ（Ｊ＝１．２．３）Ｂ”Ｊ　＝ｘＨＪ　／Ａ２η
ｔ　　（ｌ＝ｔ、　２．３）ａ’ｔｚ−＆ｔｔ／Ａｔ　
Ｃ１−１，２ｅ　３　）同様にして。

ａ’２．　ｊ　ｘ＋ａ’ｌ、　ｊｙ十ａ’２３　ｊｚ十
〇’；　ｘ＋１）％ｙ＋Ｉ）Ｂ　ｚ−Ｄ’；　ｃｌ−Ｄ
”２０ｘ　−Ｌ”ｓ　ｅａ　＝　ｊ　　　−−’（１１
ただし、 ＤＩ＝−Ｌａ３４−δ＆５ｉ−１１Ｊｏ＆ｔｔ−１ｊε
ｊ％ｉ　（Ｊ＝１．２．３）ｏ”７＝Ｄｚ／４η３　（
ｌ−ｔ、　２ｔ　３）　　　　　”””、＠　（８１式
、第顛式を用い、物体座標系での複数（ｎ個）の基準マ
ーク座標’ｉ’　（ＸＩｅ　７１　＋　Ｊ　）、・・・
。

（Ｘｌｌ＋　７Ｂｒ　　Ｚ＊　）とし、それらに対応す
る＊Ｖ（Ｆｊ＋）ｅ　・・・＊　　（１’＠＋　ｊｍ）
とすると。

ＧＮ−Ｗ・・・・・・・・・・・・・・・・・・・・・
・・・・旧・・・・・・・・・・・・・・ａａただし、
Ｇは多次元パラメータベクトルでありＱｍ　（ＪＬＦ　
＋　４６’２３＋　Ｂ’：　＋　ｇ＊　Ｂ’３　＊ＴＸ
＋Ｄ；　＋　ＤＭ＋　ｕ’Ｈ＋　４　）”’　Ｑｌｌ点
点ベクト ル＝　（”’　　　”’　　””　　　”ｌ＋　　＋　
　ｂ　　＋　　・・・＊　　　ｊＪ”’ａｌ基準マーク
行列 ・・・・・・・・・ａ・ ただし、 に４＝（１＊　・・・・・・、１）・・・・・・αＤま
た、０Ｉｌｌｌｌはｍ行ｎ列の０マトリクス、１．ｌ’
１工それぞれ３行３列およびｎ行ｎ列の単位行列である
。

次Ｋ、第０式を用いて最小自乗法によりＧを求める。す
なわち誤差Ｅ Ｅ−Σ（ΣＧ、、　Ｎｌｌｋ−Ｎ１１ｋ−ｋ　　　ｎ を最小とする０１に：求める。結果は次式で与えられる
Ｏ Ｇ”Ｗ”ＮＴ（ＮＮＴ）−’　　・・・・・・・・・・
・・・・・・・・・・・・・・Ｑ８ただし、ｐ４Ｔは基
準マーク行列Ｎの転置行列である。

さて、第（９）式により次の関係が成立する。

ａ’２１　ｃｌ＋ａ’、、　ｃ４＋ａ’ｇ　Ｃ３■１　
・・・・・・・・・・・・・・・・・・α９このことと
、第０４１式からカメラレンズ中心座像（ｃｌ・Ｃ２・
Ｃ３）ｅ１次式で与えられる。

また、変換マトリクスＭが１文すること、すなわち第（
５１式、第（６１式の条件と第（９）式とを用いて変形
すると、誤差パラメータεしε１　を算出することがで
きる。

ｌ　１−−Ｎ　（＆’２１　Ｂ’ｌ＋ａ’、２Ｂ’２＋
＆’２３　Ｂ’ｌ　　）−１６”’　・・’　　ＣＱＩ
ｔ　ｊ−Ａｉ　　（ａ　’２１　Ｄ７　＋ａ’２．Ｄ’
２＋ａ’２３　Ｄ−二　）−ｊｏ　　・＝　−ａａ。

ここで、第（５）式、第（θ弐を考慮すると、Ａｉとし
て次式で計算される値を使５ことができる。

Ａｒ　＝　（＆’２１２＋　ａ’！！”＋　ａ’２３”
　）　−’　　−−−−・、、　、旧、団、（Ｂ次に誤
差パラメータδを求める。第（９）式から（Ｌ＋δ）”
　Ｃ２，ｊ（１ｍＬ　２＊　３　）を求め、第（５）式
、第（６）式を用いて変形すれば１次式が得られる。

（Ｌ＋δ）”／ｆ＋−Ａｉ　（Ｂ’ｌ　”＋Ｂ、２＋Ｂ
′、２）＋（ｉ６＋εｌ）”＋２　（ｉ０＋ｇθ（Ｂ；
　ａ’、＋Ｂ’；　ａ’、＋　Ｂ′ｓａ；３）ｋ茎・・
・・・・− 同様に （Ｌ＋ｔＦ　Ｖｆ、　＝　Ａ萎（Ｄ７”　十Ｄ；　２　
＋偽り＋Ｃｊｏ＋ｇｊ〕２＋　２　（ＪＯ＋　ｇ　ｊ）
　（Ｄ　’Ｈａ　’２１　＋Ｄ’、ａ’２．　＋Ｄ’１
ａ’２３　）　Ａ　星・・・・・・・・・― 次に第（９）式から　　　− ａ７＋ｊ＝（−Ｂ”ｌ１７＋　　’７＋（ｉｏ＋ｆｆｔ
）ａ’Ｂ〆（Ｌ十δ）、　（ｌ−１，２，３）であるか
ら ａ’１　ｔ　＝　（−Ｂ”１−　（ｉ　６　＋ｔＩ）＆
’２ｔ　）／　（（Ｌ＋Ｊ　）／’７＋’）・・・・・
・・・・（至） 同様に。

＆’ａａ−（Ｄ”＊−（ｊｏ＋ｇ３）ａ’ｔり／（（Ｌ
＋δ）／７７Ｊ）・・・・・・・・・節 ここで、上記で求める変換マトリクスＭ（ａｉｖ）は直
交していることが保証されている。みかけの光軸長を第
０式または第（ハ）弐により求まるＬ＋δとし、また、
みかけの光軸点ｖ　ｇ　Ｃ１１１式、第翰式により算出
される（　ｉ６＋４ｔ　＊　ｊｏ＋’ｊ）とすれば、Ｘ
。

ｌｒＺ軸回りのカメラの回転角α、β、ψはα＝ａｉｎ
−’　（−ａ’１２　ｔａｎ　ｇ／／ａ’Ｒ）β−ｔａ
ｎ−１（ＪＬ’３１／Ｌ′３３　）ψ”　ｔａｎ−’　
（−ａ’！！　ａ　ｉ　ｎβ／ａ’３１）により求める
こととなる。

カメラとマークの傾きがなく、マークの樽成あるいは照
明条件が最良に調姫されている場合で、かつマークの奥
行き方向分布が広い範囲にわたっている場合には、誤走
パラメータか無視できる程小さい１ＶＬとなる可能性が
調い。このときにＶ工、求まった１０＋εｚｊｏ＋εｊ
、Ｌ＋δの＋ｌｋｖそれぞれｉ。＋ｊｏ、Ｌの円部パラ
メータとして使用できる。しかし、通常はカメラとマー
クとの位置関係や照明条件が変動する。さらに、マーク
の中心座標の読取り誤差も実際の計測では生じる。この
ため、算出されるみかげの光軸点（ｉｏ＋εし　ｊ０＋
εｊ）、みかげの光軸長（Ｌ＋δ）は真の値、　　（ｔ
、、ｊｏ）＊Ｌと差が生じ、このため、カメラ位置姿勢
パラメータの誤差が生じる。実際に、この誤差がしばし
ば著しく大きくなることがある。礒ａ反なカメラ位置姿
勢パラメータ校正を行５には、みかげの元軸点、みかげ
の大軸長が真の愼に一致すること、すなわち誤差パラメ
ータεしεｊ、δＹ：０とする第２ステツプが必要であ
る。

（第２ステツプ〕 一般には、上記方法によってはみかげの光軸点からεし
εＪＹ分離することはできないが１通常のＶンズ特性の
計測子ＮＫよって別途元軸点（ｉ（、。

ｊｏ　）を求めることができる。また、光軸長ＬＫつい
ては基準マーク配列面がカメラの元軸にほぼ直角におか
れ、かつ基準マーク数が多いときには、この分明による
前記第１ステツプのパラメータ校正により積置よく得ら
れる。これにより、ンンズくり出し位置とＬとの関係を
予め校正しておく。

このように、カメラ固有のパラメータである（五〇。

ｊｏ）とＬｌｋ予め求めておくことにより、Ｃ１，εｊ
。

δの値を分離することができる。次は、ｇｌ＋εｊ。

δ−０となり、かつ変換マトリクスが直交するよ５にパ
ラメータ補正を行う。

まず、画像面と物体座標系の水平面（ｘ−ｙ面）の交線
と１画像面のｉ軸とのなす角度θＭを求める。

θにとして次式が導出できる。

０Ｍ　−ｃＱ　＄１−凰　（ｂ、　　ｌ　　ａ’、、　
十す、　　ａ’１２　　）ただし、　　ｂ、＝±（１＋
　（１１’２１／’Ｌ’２２　）２］−；　　１０．・
９・四ｂ２　””−”２１　ｂ、　／ａ’２２上記の交
線と、（ｉｏ、ｊｏ　）、　（ｉｏ＋εｌ、ｊｏ＋εｊ
）１【ド＠嶽とのなす角度θは、第（２）式の０Ｍを言
む次式で与えられる。

ただし、１ｇ−６２Ｉ十ε２ｊ カメラ方位補正量は、θを用いて で与えられる。

またｅｌ　、（！２　＊　　Ｃ３はδを用いて（ｃｌ、
Ｃ２，Ｃ８）；苗（Ｃ１，Ｃ！１Ｃ３）・・・・・・（
２）で与えられる。

次に、第６υ式により補正した方位を使って、Ｃ２１＋
　＆２２＋　Ｃ２３’に求める。この値と、第Ｕａ式に
より補正したＪ　ｌ　（Ｘ２＋　Ｃ３Ｙ使って、次式に
よりａ′２□。

ａ′２□１　ａ’ｔｓを求める。

１”２１＝　Ｃ２１／’Ａ２１　ａ’２２＝ａｚ２／Ａ
Ｈ＊　ａ’２３＝ａ２３／Ａ２−　關ただし、Ａ２町１
２１　ｃｌ＋ａ２２　Ｃ２＋ａ２３ｃ３・・・・・・・
・・・・・曽ここまでが予備補正である。

さて、ａ′２１・＆’Ｈ＋　ｌｋ’２３　’！’既知数
とすると、第（８１式は未知係数が左辺にのみ含まれる
次式に書き直すことができる。　　　　゛ Ｂ’；　ｘ＋Ｂ’；　Ｙ＋Ｂ’Ａ　Ｚ　−Ｂ’、　ｅｌ
　−Ｂ’２　Ｃ２−Ｂ”、　ｃ。

ｘｉ　−ａ’２１　ｉ　ｘ−ａ’２１　ｉｙ−”’２３
　ｉｚ　・・−−ＧＳ同様に第Ｑ１式の代わりに次式が
得られる。

Ｄ’、ｘ　＋　Ｄ”２　ｙ　＋Ｄ”３　ｚ−Ｄ″Ｉｃ　
１　Ｄ’２　Ｃ２−Ｄ’３　Ｃ３”ｊ　　ａ’ｚｔ　ｊ
　Ｘ−ａ２′２ｊ　ｙ−ａ’ｚｓ　ｊ　Ｚ・・・・・・
圓物体座標系での複数（ｎ個）の基準マーク座標を （Ｘ１＋　７１　＊　Ｚｌ　Ｌ・・・・・・、（Ｘｎ　
ｌ　）’Ｉｌｌ　Ｚ、ｌ）とし、それらに対応する像ｔ ＜ｔ、　、　ｊ、　）、・・・・・・、（１ｆｉ、ｊｆ
ｉ）とすると、第（至）式、第（至）式を用いて次の関
係式か得られる。

Ｐ−Ｑ編Ｗ　　　・・・・・・・・・・・・・・・・・
・・・・・・・・・・・・・・・・Ｃ１７まただし。

Ｖ　＝　（Ｉ　Ｊ　）　−１”’Ａ’　Ｋ、に、−１”
Ａ’に、　Ｋ３−・・・・−−−・ｔ４１ｊである。第
０８式を導出したときと同様にして、Ｐは次式 ％式％ により求められ、Ｐから Ｂ１；、　ＢｌＩ、　Ｂ１１．　、Ｄ”、Ｄ’、Ｄ’、
　ｕ’＋　ｕ／。

が求められる。これＹ用い、第（至）、＠、＠式から補
正後の角度α、β、ψが求まる。得られた変換マトリク
スは直交性が保証されている。これらの値を使って第一
代によりｅｌ　＊　Ｃ２＊　Ｃ３’ｋｓ　　また第ａ１
１．■、（２）、（ハ）式から６１．εｊ、δを求める
。

ここまでの処理で、通常の基準マーク条件の場合にはε
しεｊ、δは十分小さくなる。しかし、基準マークが少
なく、かつ画像上１カ所にかたまっている等基準マーク
の条件が悪いときなどには、εしεｊ、δが無視できな
い程度となることがある。その場合には、第（支）式か
ら第ｉ１υ式の処理を再実行する。このようにして、カ
メラ位置・姿勢のパラメータヲ誦精度に知ることができ
る。

第７図は処理の流れ図を示したものである。なお、ｒ２
１１−（２）は各ステップを示す。

ステップＣ１１ｌで基準マークの座ｓを入力した後。

ステップのにおいて、第ａｓ、ａｅ式により、Ｎ、　Ｗ
Ｙ上セツト、ステップ＠において、第Ｏ８式によりＧ、
すなわち ａ’２１　＊　ａ’Ｈ＋　ａ’２Ｂ　＊　Ｂ’ｌ　＊　
Ｂ’２　＊　Ｂ”３　ＩＩ）”、　ｌ　Ｄｓ、　＋Ｄ’
３　＊　ｕ’１　＊　ｕ’２を求め、ステップ（２４）
において、第翰式〜第（イ）式により ０１＊　ｅｌ＋　Ｃ３＋εＩｌεｊ−δトδｊｔ求める
。次にステップ（ハ）において、第（２）式、第（財）
式により ａ’１１　・＆’Ｉ２１ａ’１３　、　ａ’３１＋　＆
’３２−８３ｇを求め、ステップ（至）により、方位角
α、β、ψを求める。ステップ額から（工補正ルーチン
である。

ｊｌ、εｊ、δが無視できないとぎはステップ（至）に
進む。ステップ（２）では、第Ｇυ式、第一式により、
方位角の補正とＣＩ＋　０２　ｒ　１７３の補正を行っ
た後・ステップ（２）で、第Ｑ式、ＩＪ４（至）式によ
り”２ｇ　＋　＆’Ｈ１”２３　Ｙ：求め、ステップ−
において、第■式、第＠Ｑ式により、Ｑ、ＶＹ上セツト
、ステップ０υにおいて第Ｏｎ式によりＰ、すなわち Ｂｌ　、　Ｂ／纂、　ＢＩＳ　、　ＤＩＮ　、　ＤＩ；
　、　ＤＩＮ　、　ｕＪ、、　ｕｌ。

を算出する。ステップ（至）では、第一、　Ｃ１１１，
ｃｉ、（至）。

（ハ）式により ６２．Ｃ１，δ を求め、ステップ＠に戻る。ステップ鰭でＹＥＳの場合
、すなわち Ｃ１，εｊ、δ が無視できる程小さい場合には、確認ルーチンのステッ
プ（ハ）に移る。ステップ（ハ）ではカメラ方位角α、
β、ψとカメラ位ｔｅｌ　ｌ　Ｃ２＋　Ｃ３Ｙ用いて基
準マーク像を算出し、ステップ−で、マーク実画像の座
標と、算出したマーク像座標とのずれを算出する。

次に、この発明による移動台車の位１１姿努の計測精度
と処理時間Ｙ：ａ明する。

カメラにｉ工ＣＣＤカメラン用い、ンンズの焦点距ｌＩ
！　５５　ｍとした。カメラの光軸がマーク板の法線と
０．４ラジアンの角度を有し、またカメラとマーク板と
°が９００ｍ１ｌれ−ているような位置を移動台車が通
過したとき、計測される移動台車の位置の誤差は０．９
９寵（〜０．１％）、方位誤差は最大０．００１ラジア
ンであった。マーク画像入力からｓｌａ台車の位置・姿
勢パラメータが出力されるまでの全所要時間は、市販の
汎用画像処理装置とミニコンピユータを用いて約６秒で
あった。

〔実施例２〕 第８図は第２の実施例を示したものである。この図で、
８は前記移動台車１上の無線送受信器、９はアンテナで
あり、１０は固定された無線送受信器、１１は前記無線
送受信器１０に接続された７ンテナである。

テノビカメラ２でマーク板５を観測し、画像処理装置３
でマーク画像からマークの位１ｉ１′％：繊別し。

データ処理装置４でマーク番号の識別およびカメラ位置
・姿勢を算出し１台車の位置・方位に換算する。さらに
、台車の方向修正ｉｔを算出する。算出された修正量を
工、無線送受信器１０およびアンテナ１１で移動台車１
に伝送される。アンテナ９で無Ｍｆ！号を検知し、無線
送受信器８で、送られたデータゼデジタルな電気信号に
変換し、車輪制御装置６で車輪７の方向修正をする。な
お、マーク板５の構成およびマーク板５の識別、カメラ
位置・姿勢を算出する処理は、実施ｐＨと同じである。

なおまた、前後左右から常に少な（とも１つのマーク板
５が見えるように、複数のマーク板を塔載することがで
きる。

〔実施例３〕 第９図は第３の実施例を示したものである。この図で、
１２は室内の壁、１３は他の壁で、室内の壁１２とは方
向が異なる。５１〜５４はマーク板であり、各マーク板
５１〜５４に瞥エマーク板番号に対応する特別なマーク
か付けられている。１は移動台車であり、テレビカメラ
と広帯域無線送信器が積まれている。１４は訂記移動台
阜１からの無ｍ伝送された画像を受信し、移動台車１の
位置進行方向を決定する画像データ処理装置である。

点Ａ、Ｂ、Ｃ，Ｄｔ工予定通過地点であり、点ａ。

ｂ　＋　　ｃ　＊　　ｄは実際に３ｊ！ｌ遇した地点、
１５は前記移動台車１の軌跡である。床の凹凸が大きい
ため進行方向が一定にならないため、複数地点においた
マーク板ｔテＶビカメラで観測し移動台車１の位置姿勢
ン計測し、進行方向が修正される。なお、マーク板の構
成、カメラ位置姿勢パラメータを求める処理手順は、実
施例１で説明したとおりである。

〔実施例４〕 第１０図は、第４−＠の実施例を説明する図である。３
ａはポート競技場の水面、３１はテレビカメラと広帯域
無線送受信器を積んだポートである。

５１．５２はマーク板である。テレビカメラは、マーク
板５１．５２Ｙ視野に入れるため１回転できるようにな
っており、回転角は画像と共に画像データ処理装置１４
に送信される。画像データ処理装置１４はポート３１の
軌跡、速度を算出し。

記録すると共に無厭伝送によりポート３１に位置姿勢？
知らせる。

このよプに、水上のように従来１工、軌跡、速度の計測
が困難なところにもこの発明？利用できる。

〔発明の効果〕

この発明は以上説明したようＫ、複数のマークが配置さ
れたマーク板を空間上に配置し、移動体に積載したカメ
ラでこれらのマークを観測するか。

または空間にカメラを配置し、移動体に付けられたマー
クラ観測することによってカメラとマークとの相対的な
位置方位を求め、移動体の位置姿勢を計測する方法であ
るから、次の利点がある、（１）　　移動体の位置姿勢
を短時間に８度よく計測できる。このため、移動体の進
行方向の誘導を正確に行うことができる。

（２）マークを＃動体の極く近くに置く必要がない。し
たがって、天井や側壁あるいは立て札につけることがで
きる。このため、環境に応じて任意にマークを配置でき
る。

（３）移動体は必ずしも床面上になくてもよい。

到んば、水上、空中であってもよい。

【図面の簡単な説明】

第１図を二〇の発明の第１の実施例を説明するための図
、第２図はこの発明に用いるマーク板の正面図、第３図
は同じく斜視図、第４図をエマーク板のマーク番号の配
置例を示す図、第５図はマーク識別およびマーク番号識
別の処理手１１１１を示す流れ図、第６図はカメラ座標
系と物体座標系のとり方ケ示す図、第７図（二カメラ位
置姿勢パラメータを算出する処理の流れ図、第８図はこ
の発明の第２の実施例を説明するための図、第９図を工
同じく第３の実施例を説明するための図、第１０図は同
じく第４の実施例を説明するための図である。 図中、１は移動台車、２（エテレビカメラ、３は画像処
理装置、４はデータ処理装置、５はマーク板、６は車輪
制御装置、Ｔは車輪である。 第１図 第２図 第３図 第４図 穴子 第５図 第６図 ７Ｃ 第８図 第７図

Claims (1)

【特許請求の範囲】
1. 空間上または移動体上に固定され、かつ３次元分布した
   複数のマークを既知の位置に配置し、これらのマークを
   前記移動体または空間上に備えたカメラによつて観測し
   、前記カメラの内部パターンの算出誤差を表わす誤差パ
   ラメータを小さくしてカメラ位置姿勢パラメータを算出
   することにより前記移動体の位置姿勢を計測することを
   特徴とする移動体位置姿勢計測方法。