JPH0680403B2

JPH0680403B2 - 移動体位置姿勢計測方法

Info

Publication number: JPH0680403B2
Application number: JP60118754A
Authority: JP
Inventors: 稔伊藤; 明石井
Original assignee: Nippon Telegraph and Telephone Corp
Current assignee: NTT Inc
Priority date: 1985-06-03
Filing date: 1985-06-03
Publication date: 1994-10-12
Anticipated expiration: 2009-10-12
Also published as: JPS61277010A

Description

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

〔従来の技術〕移動台車の位置，姿勢を検知する方法として、（Ａ）床面に付けられたマークをテレビカメラにより観
測する方法、床面に光反射板または白線を付け、光近接
センサによつて光の反射率の差を計測する方法、床面に
磁性板を置き、近接磁気センサにより磁性板の存在を確
認することによる方法。

（Ｂ）大きなマーク、例えば縦線マークを壁につけ、こ
のマークをテレビカメラで観測してカメラ方位を計測す
る方法。

等が用いられている。

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

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

また、前記（Ｂ）の場合には、カメラがマーク方向をお
よそ向いているかどうかの判定には使用できるが、絶対
座標系でのカメラ位置方位を正確に知ることは不可能で
ある。

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

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

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

この発明にかかる移動体位置姿勢計測方法は、カメラに
より３次元分布した複数マークを観測し、カメラの内部
パラメータの算出誤差を表わす誤差パラメータを小さく
してカメラ位置姿勢パラメータを算出することにより、
移動体の位置姿勢を計測するものである。

〔作用〕

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

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

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

第２図〜第４図において、円形部分が白丸マークであ
り、奇数行のマークを5A、偶数行のマークを5Bで示し
た。その他の部分は黒である。奇数行のマーク5Aのサイ
ズは、例えば4mmφ、偶数行のマーク5Bは6mmφである。
但し、マーク番号45には10mmφ、55には8mmφとしてい
る。また、偶数行のマーク5Bは、マーク板面に立てられ
た黒色円柱の上端につけられている。この円柱の高さは
実験例では50mmとした。また、マーク間隔は50mmとし
た。

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

カメラによるマーク画像入力を行い（１）、このマーク
画像を２値化し（２）、マーク画像より小さい不要画像
を孤立点として除去した後（３）、ラベリング処理を行
う（４）。各ラベリング領域の円形度を調べ、円形度の
低いラベリング領域をマーク以外のものとして除去する
（５）。また、画像辺縁部にかかるラベリング領域を除
去する（６）。次いで、各ラベリング領域の面積を求め
るとともに、最大の面積をもつラベリング領域とその面
積を得る（７）。ステツプ（７）で得た最大面積Ｓを基
準とし、マークの大きさに応じた４つの面積区分を設定
し、各ラベリング領域を４つのクラスに分類する
（８）。この実施例では、イ）最大面積S,ロ）（0.64±
0.1）×S,ハ）（0.36±0.1）×S,ニ）（0.16±0.1）×
Ｓの４つのクラスに分類した。イ）に該当するマークに
はマーク番号45、ロ）に該当するマークはマーク番号55
とし、また、ハ）に該当するマークは偶数行番号をもつ
マーク、ニ）に該当するマークは奇数行番号として分類
される。

ステツプ（９）では、奇数行番号をもつ各マーク5Aにつ
いて、マーク画像の縦方向座標及び確定マークすなわ
ち、マーク45の座標を利用して行の番号の識別を行う。
ステツプ（10）では、各行のマークの中から、番号45及
び55を基準として番号15,25,35,65,75,85,95のマークを
選別する。次に当該マークすなわち１桁目（１の位）が
５のマークを利用して、他のマークの番号付けを行う。
ステツプ（11）では各マークの外接長方形または重心を
用いて各マークの中心座標を求める。最後にステツプ
（12）においてマーク番号、マークの画像メモリ上の座
標及びマークの実空間座標を記録したマークテーブルを
作成する。

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

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

次に、マークテーブル（マークリスト）を使つてカメラ
位置姿勢パラメータを求める。この処理は大別して２つ
のステツプからなる。第１ステツプでは、カメラパラメ
ータの近似値を、誤差パラメータとともに解析的に求め
る。第２ステツプは精度を高めるための補正ルーチンで
あり、第１ステツプで得た誤差パラメータが小さくなる
ようなカメラパラメータを解析的に求める。以下、カメ
ラパラメータを算出する処理の方法を第１ステツプ、第
２ステツプに分けて詳細に説明する。

（第１ステツプ）カメラレンズ中心が物体座標系（x,y,z）の（c₁，c₂，c
₃）に位置しているものとする。カメラ座標系（x_c，
y_c，z_c）と物体座標系（x,y,z）との関係は一般に次式
で書き表わされる。なお、座標系は第６図に示すように
右手系とし、カメラ光軸方向をy_c軸とする。また、５は
マーク板で、x,y,zは空間座標、α，β，は方位角、
２はテレビカメラで、x_c，y_c，z_cはその軸である。

ただし、Ｍは物体座標系からカメラ座標系への変換マト
リクスである。Ｍの行列要素を次式で示す。

Ｍ＝（a_kl）（k,l＝1,2,3） ……（２）一方、カメラの内部パラメータとして、カメラの中心か
らカメラ画面へ垂線を下したときの距離（光軸長）をＬ
とし、その垂線と画面との交点（光軸点）の座標を
（i₀，j₀）とし、また、画面上の像と画像メモリ上の像
との間のスケールフアクタ（縮小率）を画像のｉ方向
（横方向）についてη_ｉ、ｊ方向（縦方向）についてη
_ｊとする。基準マークのカメラ座標系における座標とそ
の画像（i,j）とは次の関係を有している。

以後の計算で、変換マトリクスが直交すること、すなわ
ちを保証させるため、第（３）式，第（４）式を次のよう
に書き直す。

δと（ε_ｉ，ε_ｊ）は内部パラメータである光軸長Ｌお
よび光軸点座標（i_o，j_o）の誤差を表わす誤差パラメー
タである。第（１）式，第（２）式を第（７）式に代入
すれば次式が得られる。

ａ′₂₁ix＋ａ′₂₂iy＋ａ′₂₃iz＋Ｂ″₁ｘ＋Ｂ″₂ｙ＋
Ｂ″₃ｚ−Ｂ″₁c₁−Ｂ″₂c₂−Ｂ″₃c₃＝ｉ …（８）ただし、 B_l≡−La_１−δａ_１−η_ii₀a_2l−η_iε_ia_2l（ｌ＝
1,2,3） A₂≡a₂₁c₁＋a₂₂c₂＋a₂₃c₃ ……（９）Ｂ″_l≡B_l／A₂η_i（ｌ＝1,2,3）ａ′_2l≡a_2l／A₂（ｌ＝1,2,3）同様にして、ａ′₂₁jx＋ａ′₂₂jy＋ａ′₂₃jz＋Ｄ″₁ｘ＋Ｄ″₂ｙ＋
Ｄ″₃ｚ−Ｄ″₁c₁−Ｄ″₂c₂−Ｄ″₃c₃＝ｊ ……（10）ただし、第（８）式，第（10）式を用い、物体座標系での複数
（ｎ個）の基準マーク座標を（x₁，y₁，z₁），…，
（x_n，y_n，z_n）とし、それらに対応する像を（i₁，
j₁），…，（i_n，j_n）とすると、 GN＝Ｗ ……………………………………（12）ただし、Ｇは多次元パラメータベクトルでありＧ≡（ａ′₂₁,a′₂₂,a′₂₃,B″₁,B″₂,B″₃,D″₁,D″₂,
D″₃,u′₁,u′₂） …（13）像点ベクトルＷ＝（i₁，i₂，…i_n，j₁，j₂，…，j_n） …（15）基準マーク行列ただし、また、O_mnはｍ行ｎ列のＯマトリクス,I,I′はそれぞれ
３行３列およびｎ行ｎ列の単位行列である。

次に、第（12）式を用いて最小自乗法によりＧを求め
る。すなわち誤差Ｅを最小とするＧを求める。結果は次式で与えられる。

Ｇ＝Ｗ・N^T(NN^T)^-1 ……………………（18）ただし、N^Tは基準マーク行列Ｎの転置行列である。

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

ａ′₂₁c₁＋ａ′₂₂c₂＋ａ′₂₃c₃＝１………………（19）このことと、第（14）式からカメラレンズ中心座標
（c₁，c₂，c₃）は次式で与えられる。

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

ε_i＝−▲A² ₂▼（ａ′₂₁Ｂ″₁＋ａ′₂₂Ｂ″₂＋ａ′
₂₃Ｂ″₃）−i₀ ……（21） ε_j＝−▲A² ₂▼（ａ′₂₁Ｄ″₁＋ａ′₂₂Ｄ″₂＋ａ′
₂₃Ｄ″₃）−j₀ ……（22）ここで、第（５）式，第（６）式を考慮すると、▲A² ₂
▼として次式で計算される値を使うことができる。

▲A² ₂▼＝（ａ′₂₁ ²＋ａ′₂₂ ²＋ａ′₂₃ ²）^-1……………
…（23）次に誤差パラメータδを求める。第（９）式から（Ｌ＋δ）²a² _il（ｌ＝1,2,3）を求め、第（５）式，第（６）式を用いて変形すれば、
次式が得られる。

（Ｌ＋δ）²／η² _i＝▲A² ₂▼（Ｂ″₁ ²＋Ｂ″₂ ²＋
Ｂ″₃ ²）＋（i₀＋ε_i）²＋２（i₀＋ε_i）（Ｂ″₁ａ′₂₁
＋Ｂ″₂ａ′₂₂＋Ｂ″₃ａ′₂₃）▲A² ₂▼ ……（24）同様に（Ｌ＋δ²）／η² _j＝▲A² ₂▼（Ｄ″₁ ²＋Ｄ″₂ ²＋
Ｄ″₃ ²）＋（j₀＋ε_j）²＋２（j₀＋ε_j）（Ｄ″₁ａ′₂₁
＋Ｄ″₂ａ′₂₂＋Ｄ″₃ａ′₂₃）▲A² ₂▼ ………（25）次に第（９）式からａ′_１＝｛−Ｂ″_lη_i−η_i（i₀＋ε_i）ａ′_2l｝／
（Ｌ＋δ）、（ｌ＝1,2,3）であるからａ′_１＝｛−Ｂ″_l−（i₀＋ε_i）ａ′_2l｝／｛（Ｌ＋
δ）／η_i｝ ………（26）同様に、ａ′_3l＝｛−Ｄ″_l−（j₀＋ε_j）ａ′_2l｝／｛（Ｌ＋
δ）／η_j｝ ………（27）ここで、上記で求める変換マトリクスＭ（a_kl）は直交
していることが保証されている。みかけの光軸長を第
（24）式または第（25）式により求まるＬ＋δとし、ま
た、みかけの光軸点を第（21）式，第（22）式により算
出される（i₀＋ε_i，j₀＋ε_j）とすれば、x,y,z軸回り
のカメラの回転角α，β，は α＝sin^-1（−ａ′₁₂tan/a′₃₂） β＝tan^-1（ａ′₃₁/a′₃₃）＝tan^-1（−ａ′₃₂sinβ/a′₃₁）により求めることとなる。

カメラとマークの傾きがなく、マークの構成あるいは照
明条件が最良に調整されている場合で、かつマークの奥
行き方向分布が広い範囲にわたつている場合には、誤差
パラメータが無視できる程小さい値となる可能性が高
い。このときには、求まつたi₀＋ε_i，j₀＋ε_j,L＋δの
値をそれぞれ内部パラメータi_o，j_o,Lとして使用でき
る。しかし、通常はカメラとマークとの位置関係や照明
条件が変動する。さらに、マークの中心座標の読取り誤
差も実際の計測では生じる。このため、算出されるみか
けの光軸点（i₀＋ε_i，j₀＋ε_j），みかけの光軸長（Ｌ
＋δ）は真の値、（i₀，j₀）,Lと差が生じ、このため、
カメラ位置姿勢パラメータの誤差が生じる。実際に、こ
の誤差がしばしば著しく大きくなることがある。高精度
なカメラ位置姿勢パラメータ校正を行うには、みかけの
光軸点，みかけの光軸長が真の値に一致すること、すな
わち誤差パラメータε_i，ε_j，δを０とする第２ステツ
プが必要である。

（第２ステツプ）一般には、上記方法によつてはみかけの光軸点から
ε_i，ε_jを分離することはできないが、通常のレンズ特
性の計測手段によつて別途光軸点（i₀，j₀）を求めるこ
とができる。また、光軸長Ｌについては基準マーク配列
面がカメラの光軸にほぼ直角におかれ、かつ基準マーク
数が多いときには、この発明による前記第１ステツプの
パラメータ校正により精度よく得られる。これにより、
レンズくり出し位置とＬとの関係を予め校正しておく。
このように、カメラ固有のパラメータである（i₀，j₀）
とＬを予め求めておくことにより、ε_i，ε_j，δの値を
分離することができる。次は、ε_i，ε_j，δ＝０とな
り、かつ変換マトリクスが直交するようにパラメータ補
正を行う。

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

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

θ_M＝cos^-1（b₁,a′₁₁＋b₂ａ′₁₂）ただし、上記の交線と、（i₀，j₀），（i₀＋ε_i，j₀＋ε_j）を結
ぶ直線とのなす角度θは、第（29）式のθ_Mを含む次式
で与えられる。

ただし、▲l² ₀▼＝ε² _i＋ε² _j カメラ方位補正量は、θを用いてで与えられる。

またc₁，c₂，c₃はδを用いてで与えられる。

次に、第（31）式により補正した方位を使つて、a₂₁，a
₂₂，a₂₃を求める。この値と、第（32）式により補正し
たc₁，c₂，c₃を使つて、次式によりａ′₂₁,a′₂₂,a′₂₃
を求める。

ａ′₂₁＝a₂₁／A₂,a′₂₂＝a₂₂／A₂,a′₂₃＝a₂₃／A₂…（3
3）ただし、A₂＝a₂₁c₁＋a₂₂c₂＋a₂₃c₃ …………（34）ここまでが予備補正である。

さて、ａ′₂₁,a′₂₂,a′₂₃を既知数とすると、第（８）
式は未知係数が左辺にのみ含まれる次式に書き直すこと
ができる。

Ｂ″₁ｘ＋Ｂ″₂ｙ＋Ｂ″₃ｚ−Ｂ″₁c₁−Ｂ″₂c₂−Ｂ″₃
c₃＝ｉ−ａ′₂₁ix−ａ′₂₂iy−ａ′₂₃iz ……（35）同様に第（10）式の代わりに次式が得られる。

Ｄ″₁ｘ＋Ｄ″₂ｙ＋Ｄ″₃ｚ−Ｄ″₁c₁−Ｄ″₂c₂−Ｄ″₃
c₃＝ｊ−ａ′₂₁jx−ａ′₂₂jy−ａ′₂₃jz ……（36）物体座標系での複数（ｎ個）の基準マーク座標を（x₁，y₁，z₁），……，（x_n，y_n，z_n）とし、それらに対応する像を（i₁，j₁），……，（i_n，j_n）とすると、第（35）式，第（36）式を用いて次の関係式
が得られる。

Ｐ・Ｑ＝Ｗ ……………………………（37）ただし、である。第（18）式を導出したときと同様にして、Ｐは
次式Ｐ＝Ｖ・Q^T（Ｑ・Q^T）^-1 …………………（41）により求められ、ＰからＢ″₁,B″₂,B″₃,D″₁,D″₂,D″₃,u′₁,u′₂ が求められる。これを用い、第（26），（27），（28）
式から補正後の角度α，β，が求まる。得られた変換
マトリクスは直交性が保証されている。これらの値を使
つて第（20）式によりc₁，c₂，c₃を、また第（21），
（22），（24），（25）式からε_i，ε_j，δを求める。

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

第７図は処理の流れ図を示したものである。なお、（2
1）〜（34）は各ステツプを示す。

ステツプ（21）で基準マークの座標を入力した後、ステ
ツプ（22）において、第（15），（16）式により、N,W
をセツトし、ステツプ（23）において、第（18）式によ
りＧ、すなわちａ′₂₁,a′₂₂,a′₂₃,B″₁,B″₂,B″₃,D″₁,D″₂,D″₃,
u′₁,u′₂ を求め、ステツプ（24）において、第（20）式〜第（2
5）式により c₁，c₂，c₃，ε_i，ε_j，δ₁，δ_j を求める。次にステツプ（25）において、第（26）式，
第（27）式によりａ′₁₁,a′₁₂,a′₁₃,a′₃₁,a′₃₂,a′₃₃ を求め、ステツプ（26）により、方位角α，β，を求
める。ステツプ（27）からは補正ルーチンである。
ε_i，ε_j，δが無視できないときはステツプ（28）に進
む。ステツプ（28）では、第（31）式，第（32）式によ
り、方位角の補正とc₁，c₂，c₃の補正を行つた後、ステ
ツプ（29）で、第（33）式，第（34）式によりａ′₂₁,
a′₂₂,a′₂₃を求め、ステツプ（30）において、第（3
9）式，第（40）式により、Q,Vをセツトし、ステツプ
（31）において第（37）式にＰ、すなわちＢ″₁,B″₂,B″₃,D″₁,D″₂,D″₃,u′₁,u′₂ を算出する。ステツプ（32）では、第（20），（21），
（22），（24），（25）式により ε_i，ε_j，δ を求め、ステツプ（27）に戻る。ステツプ（27）でYES
の場合、すなわち ε_i，ε_j，δ が無視できる程小さい場合には、確認ルーチンのステツ
プ（23）に移る。ステツプ（23）ではカメラ方位角α，
β，とカメラ位置c₁，c₂，c₃を用いて基準マーク像を
算出し、ステツプ（34）で、マーク実画像の座標と、算
出したマーク像座標とのずれを算出する。

次に、この発明による移動台車の位置姿勢の計測精度と
処理時間を説明する。

カメラにはCCDカメラを用い、レンズの焦点距離55mmと
した。カメラの光軸がマーク板の法線と0.4ラジアンの
角度を有し、またカメラとマーク板とが900mm離れてい
るような位置を移動台車が通過したとき、計測される移
動台車の位置の誤差は0.99mm（〜0.1％），方位誤差は
最大0.001ラジアンであつた。マーク画像入力から移動
台車の位置・姿勢パラメータが出力されるまでの全所要
時間は、市販の汎用画像処理装置とミニコンピユータを
用いて約６秒であつた。

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

テレビカメラ２でマーク板５を観測し、画像処理装置３
でマーク画像からマークの位置を識別し、データ処理装
置４でマーク番号の識別およびカメラ位置・姿勢を算出
し、台車の位置・方位に換算する。さらに、台車の方向
修正量を算出する。算出された修正量は、無線送受信器
10およびアンテナ11で移動台車１に伝送される。アンテ
ナ９で無線信号を検知し、無線送受信器８で、送られた
データをデジタルな電気信号に変換し、車輪制御装置６
で車輪７の方向修正をする。なお、マーク板５の構成お
よびマーク板５の識別，カメラ位置・姿勢を算出する処
理は、実施例１と同じである。なおまた、前後左右から
常に少なくとも１つのマーク板５が見えるように、複数
のマーク板を塔載することができる。

〔実施例３〕第９図は第３の実施例を示したものである。この図で、
12は室内の壁、13は他の壁で、室内の壁12とは方向が異
なる。51〜54はマーク板であり、各マーク板51〜54には
マーク板番号に対応する特別なマークが付けられてい
る。１は移動台車であり、テレビカメラと広帯域無線送
信器が積まれている。14は前記移動台車１からの無線伝
送された画像を受信し、移動台車１の位置進行方向を決
定する画像データ処理装置である。点A,B,C,Dは予定通
過地点であり、点a,b,c,dは実際に通過した地点、15は
前記移動台車１の軌跡である。床の凹凸が大きいため進
行方向が一定にならないため、複数地点においたマーク
板をテレビカメラで観測し移動台車１の位置姿勢を計測
し、進行方向が修正される。なお、マーク板の構成、カ
メラ位置姿勢パラメータを求める処理手順は、実施例１
で説明したとおりである。

〔実施例４〕第10図は、第４の実施例を説明する図である。30はボー
ト競技場の水面、31はテレビカメラと広帯域無線送受信
器を積んだボートである。51,52はマーク板である。テ
レビカメラは、マーク板51,52を視野に入れるため、回
転できるようになつており、回転角は画像と共に画像デ
ータ処理装置14に送信される。画像データ処理装置14は
ボート31の軌跡，速度を算出し、記録すると共に無線伝
送によりボート31に位置姿勢を知らせる。

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

〔発明の効果〕

この発明は以上説明したように、複数のマークが配置さ
れたマーク板を静止体上に配置し、移動体に積載したカ
メラでこれらのマークを観測するか、または静止体上に
カメラを配置し、移動体に付けられたマークを観測する
ことによつてカメラとマークとの相対的な位置方位を求
め、移動体の位置姿勢を計測する方法であるから、次の
利点がある。

（１）移動体の位置姿勢を短時間に精度よく計測でき
る。このため、移動体の進行方向の誘導を正確に行うこ
とができる。

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

（３）移動体は必ずしも床面上になくてもよい。例え
ば、水上，空中であつてもよい。

【図面の簡単な説明】

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

Claims

【特許請求の範囲】

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