JPH0418603B2 - - Google Patents

Description

【発明の詳細な説明】 本発明は３次元空間における物体の位置計測シ
ステムに係り、特に複数の簡単な受動型センサを
用いて分散処理により物体位置を精密に計測する
のに好適なシステムに関する。 ３次元空間における物体の位置（ｘ，ｙ，ｚ）
を計測する受動的手段として従来光学的距離計が
用いられている。距離計から得られる、観測点か
ら対象物体までの距離情報と、別途光学的センサ
を用いて対象の像を撮り、その像のセンサ面上で
の位置情報から算出できる、観測点から見た該対
象物体の方位情報とにより、該物体の観測点を基
準とした位置が得られる。しかしこの方法では遠
距離にある物体は、距離計の距離計測精度が落ち
るため、その空間での位置の計測精度が低くなる
という欠点があつた。 したがつて、本発明の目的は遠距離にある物体
の３次元空間における位置を精度良く計測するシ
ステムを提供することにある。 この目的を達成するため本発明では、相異なる
位置に分散して設置した複数のセンサによる対象
物体の方位情報にもとづいて、立体幾何学計算を
行なうことにより上記センサごとに物体の位置を
算出するようにした点に特徴がある。 ここで本発明で用いる立体幾何学の原理を説明
する。初めに２つの観測点からの方位観測情報か
ら対象物体の位置を算出する方法を述べる。第１
図の１，２を観測点、３を対象物体点とする。観
測点１に固定された座標系１０（x₁，y₁，z₁）を
基準座標系とする。この座標系で点２の位置ベク
トルをｒ ₂₁、点１から３への視線ベクトル（単位
長さ）４をｕ ₁₁、点２から３への単位長さの視線
ベクトル５をｕ ₂₁、未知の点３の位置ベクトルｒ
³ ₁と表わすものとする。このとき点３の位置ベク
トルｒ ³ ₁は、点１から３への視線と、点２から３
への視線との交点として次式で求まる。 ｒ ³ ₁＝t₁ ｕ ₁₁ ＝ｒ ₂₁＋t₂ ｕ ₂₁ (1) ただし、t₁，t₂はパラメータ（スカラー量）で
ある。 視線ベクトルは例えば次のようにして得られ
る。第２図の観測点６に中心を持つ光学センサを
考える。点６を原点とする座標系８のｙ軸上で原
点６から上記センサの焦点距離ｆだけ離れたとこ
ろにある焦点面７に対象物体ｉのセンサ像９があ
るとする。像９の座標を（x_f，ｆ，z_f）とすれば
点６と像９を結ぶ視線に沿つた視線ベクトルは ｕ＝（x_f／ｄ，ｆ／ｄ，z_f／ｄ）^T (2) で与えられる。 但し、 ｄ＝（x_f ²＋f²＋z_f ²）^1/2 (3) Ｔ：転置行列を示す記号 である。第１図の点１における座標系１０で定義
される視線ベクトルｕ ₁₁はこの方法で求まる。同
様にして点２における視線ベクトルとして点２に
固定された座標系１１（x₂，y₂，z₂）で定義され
たｕ ₂₂が(2)式で得られる。ｕ ₂₂は座標系１０で定
義したｕ ₂₁に次の式で変換できる。 ｕ ₂₁＝G₁₂ ｕ ₂₂ (4) ここにG₁₂は座標系１１の各軸方向を座標系１
０の各軸方向に合致させるための回転マトリツク
スである。 以上まとめると、回転マトリツクスG₁₂、座標
系１０で定義した点２の位置ベクトルｒ ₂₁が既知
であれば、点１および点２のセンサで(2)式に従つ
て観測された視線ベクトルｕ ₁₁，ｕ ₂₂から、(4)
式、(1)式により未知の点３にある物体の座標が求
まる。 次に２つの観測点におけるセンサ像の相対位置
関係を算出する方法を述べる。これにもとづき第
１図の２つの観測点１，２について(1)式中の位置
ベクトルｒ ₂₁、(4)式中の回転マトリツクスG₁₂を
求める。いま位置が未知のＮ個の点に物体ｉ（ｉ
＝１，……，Ｎ）があり、点１および点２のセン
サでその像が得られているとする。物体ｉの座標
系１０および１１での位置ベクトルをそれぞれｒ
_１ ⁱ，ｒ ₂ ⁱとすると、両者の間には ｒ ₂ ⁱ＝G₁₂ ^-1（ｒ ₁ ⁱ−ｒ ₂₁） (5) （G₁₂ ^-1：G₁₂の逆行列） なる関係がある。観測センサが第２図の観測点６
に中心を持つごとき光学センサであるとすれば、
観測点１にあるセンサ（S₁とする）で観測した物
体ｉの像の位置は x_f1 ⁱ＝f₁x₁ ⁱ／y₁ ⁱ，z_f1 ⁱ＝f₁z₁ ⁱ／y₁ ⁱ (6) また観測点２にあるセンサ（S₂とする）での像の
位置は x_f2 ⁱ＝f₂x₂ ⁱ／y₂ ⁱ，z_f2 ⁱ＝f₂z₂ ⁱ／y₂ ⁱ (7) で与えられる。ただし、f₁，f₂はセンサS₁，S₂の
焦点距離で既知、ｒ ₁ ⁱ＝（x₁ ⁱy₁ ⁱz₁ ⁱ）^T，ｒ ₂ ⁱ＝（x₂ ⁱy
₂ ⁱ
z₂ ⁱ）^Tとする。そこでＮ個の物体の両センサ面上で
の像の位置が測定できれば、(5)，(6)，(7)の関係式
から最小２乗法を用いて回転マトリツクスG₁₂と
単位ベクトルｒ ₂₁′が求まる。ここに ｒ ₂₁′＝r₂₁／r₂₁，（r₂₁：観測点１，２間の距離）(
8) である。解法は例えば次の文献で述べられてい
る。 D.Gennery，Stereo−Camera Calibration，Proc.of ARPA Image
Understonding Workshop，101−107，
Nov.1978。この方法では観測点２の１に対する
位置は方向を示す単位ベクトルｒ ₂₁′のみが求ま
り、その距離r₂₁は不足である。このr₂₁は次の方
法で得る。すなわちＮ個の点物体のうち１組２点
についてその距離が既知とする。この２点をｋ，
ｌとし距離をr^klとする。上記方法で得た回転マト
リツクスG₁₂、単位ベクトルｒ ₂₁′と、点ｋにある
物体の観測点１および２の両センサにおける像の
位置から(2)式に従つて求めた視線ベクトルｕ ₁₁ ^k，
ｕ₂₂ ^kから(4)式，(1)式により点ｋの位置ベクトル
ｒ^k′が得られる。但し(1)式でｒ ₂₁の代りにｒ ₂₁′を
用いる。点ｌについても同様に位置ベクトルｒ
^ｌ′が得られる。そこで既知の距離r^klを用いて r₂₁＝r^kl／｜r^k′−r^l′｜ (9) なる計算で２つの観測点１，２間の距離r₂₁が与
えられる。これから(8)式に従つて未知の位置ベク
トルｒ ₂₁が求まる。以上の方法でG₁₂とｒ ₂₁が決
定できる。ここでマトリツクスG₁₂と、ベクトル
r₂₁とを合わせて相対位置情報と呼ぶ。 以下、本発明を実施例にもとづき詳細に説明す
る。第３図は本発明による３次元空間における物
体位置計測システムの全体構成図である。観測点
ごとに観測装置１２（・点で示す）が設置され、
それらは相互に回線１３で結ばれている。位置が
未知の対象物体を１４、較正用物体を１５とす
る。第４図は観測装置１２の構成図である。該装
置は第ｉ観測点に設置されているとする。観測装
置１２は、検出手段２００と、処理装置１００
と、通信手段４０とから成る。検出手段２００
は、センサ１７と、検出回路１８とから成る。通
信手段４０は、送信器２６と、受信器２７とから
成る。センサ１７は物体１４からの光線１６をと
りこむ。検出回路１８はセンサ面上の物体の像位
置を検出し、(2)式の計算を行ない物体への単位視
線ベクトルｕ _iiを算出する。ｕ _iiの値は送信器２６
により回線１３を通して他の観測装置へ送られ
る。他の観測装置から回線１３を通して送られて
きた単位視線ベクトルｕ _jj（ｊ≠ｉ）は受信器２
７により受け取られ位置算出回路１９に送られ
る。メモリ２０には該観測点ｉと他の観測点ｊと
の相対位置関係を示す相対位置姿勢情報すなわち
回転マトリツクスG_ijと位置ベクトルr_ijがすべて
のｊについて蓄えられている。位置算出回路１９
は受信器２７の出力である観測点ｊでの単位視線
ベクトルｕ _ijと、メモリ２０から読み出したマト
リツクスG_ijおよびベクトルｒ _jiを用いて、(2)式お
よび(1)式の演算を行なつて物体１４の位置ｒを算
出する。こうして物体の３次元空間における未知
の位置が計測できる。この結果は操作装置２１に
送られ、この値にもとづいて所定の制御を行なわ
せることができる。以上が本発明による位置計測
システムの基本動作である。なお、メモリ２０に
は、外部入力装置２５（たとえば、キーボード入
力装置）を介してあらかじめ計算に必要なマトリ
ツクスG_ijおよびベクトルｒ _jiの値が収納されてい
るものとする。これは各観測点の位置とセンサの
とりつけ方向（センサ面の方向が、ｘ，ｙ，ｚ軸
となす角度で表わされる）があらかじめ正確に判
つている場合である。 次にこの各観測点の位置とセンサのとりつけ方
向があらかじめ判つていない場合の動作を説明す
る。このとき以下に述べる方法によりマトリツク
スG_ij，ベクトルｒ _jiを較正演算回路２２で計算し
結果を相対位置情報としてメモリ２０に蓄える。
この較正動作が完了した後の未知の物体位置計測
の動作は上述のとおりである。第３図のごとく較
正用の物体１５を任意の位置に複数設置する（そ
の数をＮとする）。各観測装置１２ではこれら較
正用物体１５の像を次々撮影する。第４図の観測
装置１２の設置位置をｉとする。前述の未知の物
体位置計測の時と同様にセンサ１７、検出回路１
８により物体１５への視線ベクトルが得られる。
第ｋ番目の較正用物体への視線ベクトルをｕ _ii ^kと
する。すべてのｋについてｕ _ii ^kを送信器２６、回
線１３を介して他の観測装置１２に送る。他のｊ
地点の観測装置１２から回線１３を通して送られ
た視線ベクトルｕ _jj ^k（ｋ＝１，……，Ｎ）は受信
器２７で受信したのち較正演算回路２２に送られ
る。回路２２は、検出回路１８の出力であるｕ _ii ^k
と、受信器２７の出力であるｕ _jj ^k（ｋ＝１，…
…，Ｎ）にもとづき、(5)，(6)，(7)式を前述の最小
２乗法により解いて回転マトリツクスG_ijと単位
ベクトルｒ _ji′を求める。一方、較正用物体１５の
うちの２個（ｋとｌとする）についてその距離r^kl
が既知で外部入力装置２４（たとえば、キーボー
ド入力装置）から与えられるとする。これを用い
て回路２２は(9)式により観測点ｉとｊとの距離r_ji
を算出し、(8)式により所望の位置ベクトルｒ _jiを
得る。上記の演算をおこなう較正演算回路２２と
しては通常のマイクロプロセツサを用いればよ
い。こうして算出したマトリツクスG_ijとベクト
ルｒ _jiはメモリ２０に蓄えられる。以上の較正演
算を、該観測点をｉとするとき、すべての観測点
ｊ（ｊ≠ｉ）について行なつて結果をメモリ２０
に蓄えておく。 以上述べた複数の観測装置を結んで物体位置を
計測する方式では観測装置が２個あれば原理的に
動作可能である。しかし観測装置数を３以上にす
ることにより精度の向上、信頼性の向上、協調操
作の実現が可能となる。まず複数の観測装置があ
れば各観測装置と他の装置との組合せのそれぞれ
について相互の位置および方向の較正演算と、未
知物体の位置計測が可能であり、この結果を平均
する等の処理により較正精度、物体位置計測精度
を高めることができる。 次に複数の観測装置があればそのうち１つまた
はそれ以上が故障して動作しなくなつても、残り
装置が２つ以上あれば機能するので全体としての
信頼性が向上する。 また複数観測装置の協調動作が可能である。例
えば観測装置がその観測点から対象物体までの距
離を測定し、距離が最小の装置のみに所定の操作
をほどこしたい場合には次のようにすればよい。
第ｉ番目の観測装置で前述の動作で物体までの位
置ベクトルｒを算出したとする。第４図の判定回
路２３は、メモリ２０から他の観測装置ｊの位置
ベクトルｒ _jiをすべてのｊについて読み出し、｜ｒ
−ｒ _ji｜の最小のものと｜ｒ｜とを比較し、その
最小値を与える装置を、該対象物体に最も近い装
置と判定する。もし判定結果として該装置ｉが最
も近いと判定されたならば、操作装置２１に所定
の操作を行なわせる。 以上述べた較正すなわちセンサの相互位置関係
算出の演算、および未知物体の位置測定の演算
は、複数のセンサからの信号を中央処理装置（図
示せず）に取りこんで、まとめて行なうことも可
能であるが、本実施例のように各観測装置ごとに
行なう分散処理方式にすれば全体の信頼性を高め
る効果がある。 以上述べたごとく本発明によれば、複数の観測
装置で相違なる位置から測定した方位情報から、
立体幾何学演算により物体の未知の位置を、推定
できる。これにより従来の光学的距離計を用いた
測定よりも、遠距離物体の位置測定精度を高める
ことができる。また方位測定は距離測定よりも簡
単な装置で高精度でおこなうことができる。また
本発明の、較正用物体を用いた較正方式によれば
各観測装置の位置と方向が算出できるので、較正
のための厳密な測量が不要となり、観測装置を任
意の位置に移動設置することが可能となる。さら
に複数の観測装置を組合せて使うことにより位置
測定精度を装置数に応じて高めることが可能であ
る。また位置測定を短い周期でくり返すことによ
り移動物体の位置測定も可能である。

【図面の簡単な説明】

第１図は方位観測から物体位置を計測する原理
を示す図、第２図はセンサ面上の像位置と視線の
関係を示す図、第３図は本発明の位置計測システ
ムを用いた装置の全体図、第４図は第３図の構成
要素である観測装置の構成図である。 １，２……観測点、３……対象物体、７……セ
ンサ面、９……センサ上の像、１２……観測装
置、１３……回線、１４……対象物体、１５……
較正用物体、１７……センサ、１８……検出回
路、１９……位置算出回路、２０……メモリ、２
１……操作装置、２２……較正演算回路、２３…
…判定回路、２６……送信器、２７……受信器。

Claims (1)

1. 【特許請求の範囲】 １ 観測装置１２と、回線１３とを有する物体位
   置計測システムであつて、 観測装置１２は、 処理装置１００と、検出手段２００と、通信手
   段４０とを有し、任意の複数位置に配置され、回
   線１３を介して相互に交信可能であり、 検出手段２００により計測対象物体の該観測装
   置１２からみた方位を検出し、また、検出した方
   位を通信手段４０および回線１３を介して他の各
   観測装置１２に送信し、 かつ、回線１３および通信手段４０を介して他
   の各観測装置１２から、該各観測装置からみた該
   計測対象物体の方位を受信し、 処理装置１００は、該観測装置１２からみた方
   位と、他の各観測装置１２からみた方位と、あら
   かじめメモリ２０に蓄えた該観測装置１２と他の
   各観測装置１２との既知の相対位置姿勢情報とを
   用いて、該計測対象物体の位置を算出する手段を
   有する 物体位置計測システム。 ２ 上記観測装置１２は、 処理装置１００が算出した計測対象物体の位置
   と、上記相対位置姿勢情報とを用いて、該観測装
   置１２のとるべき操作を判定する手段を有する 特許請求の範囲第１項に記載の物体位置計測シ
   ステム。 ３ 観測装置１２と、回線１３とを有する物体位
   置計測システムであつて、 観測装置１２は、 処理装置１００と、検出手段２００と、通信手
   段４０とを有し、任意の複数位置に配置され、回
   線１３を介して相互に交信可能であり、 少なくとも１組２個の較正用物体についてその
   間の距離が既知である、任意の位置に置いた各較
   正用物体１５の、該観測装置１２からみた方位を
   検出手段２００により検出し、また、検出した各
   方位を通信手段４０および回線１３を介して他の
   各観測装置１２に送信し、 かつ、回線１３および通信手段４０を介して他
   の各観測装置１２から、該各観測装置からみた該
   各較正用物体の方位を受信し、 処理装置１００は、該観測装置１２からみた各
   方位と、他の各観測装置１２からみた各方位とを
   用いて、該観測装置１２と他の各観測装置１２と
   の相対位置姿勢情報を算出し、該情報をメモリ２
   ０に蓄える手段を有する 物体位置計測システム。