JPS59193309A - 自立走行装置の位置計測方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 この発明は、ロボット等の自立走行装置の位置計測方法
に関するものであり、特に、特定形状の幾何学図形を自
立走行装置に搭載したＴＶ左カメラ１ｇｌ視して得た映
像に基づいて自己の位置を計算する自立走行装置の位置
計測方法に関するものである。

従来、自立走行するロボットや無人車等の自立走行装置
に於ける走行位置を求める１つの方法として、ある目標
物体を該走行体に搭載したＴＶ左カメラで２眼視して得
られる２つの画像の形状の相異から現在の位置を計算で
求めることが行なわれていた。　この場合、上記２眼視
の手段として、ＴＶ左カメラ２台用いるか、あるいは、
２台の受光器から導かれた光を１台のカメラで時分割的
に切換える等の方法を採用していたので、コストが高く
つき、また、計測自体に時間ががかり、更に計算が複雑
になる難点があった。

この煩雑さを避けるために、電磁誘導方法や光学的誘導
方法が実用化されている。　　しかしなか呟これ等方法
は誘導のきめを細がくしようとすればするほど、誘導ラ
インの施設が大がかりで複′　雑となりコスト高となる
難点があった。

この発明は上記従来の事情に鑑みて、簡単かつ安価な方
法を得ることを目的とするものであって、その主たる特
徴は、自立走行装置に搭載した撮像手段の視界内に配置
した特定形状の幾何学図形を、該撮像手段で撮像上この
図形の撮像面上の座標値から自立走行装置の位置を計算
する点にある。

以下、この発明を図面に基づき詳細に説明する。

第１図において、１は自立走行装置、２はその自立走行
装置１の走行平面、３はその自立走行装置１に搭載した
撮像手段、４はその自立走行装置１に搭載された図形処
理装置及び動力装置、５はその撮像手段２の視界、６は
その視界５内に配置した特定形状の幾何学図形をそれぞ
れ示している。

この幾何学図形６は、撮像した映像から３次元的位置を
算出するのを容易にするために、２等辺３角形ＡＢＣの
底辺ＢＣに中点Ｏを設け、垂線ＡＯを境にして複数色で
色分けしである。

撮像手段３はＴＶカメラなどで構成され、その本体３１
は自立走行装置ｊの定められた１点を中心にして縦軸心
周りに回転させられる。　　また、撮像手段３のレンズ
３２は結像面と共に上丁に揺動させることができ、走行
平面２上の特定点に設けた幾何学図形６をその視界５内
に納めるために俯角？の斜め下向きに向けである。　従
って、この撮像手段３の視界５は、円錐形になり、走行
平面ンにおいては円環形になる。

なお、撮像手段３の撮像管は２次元のものに限らず、１
次元ＣＣｌ）カメラのようなもので゛光学的スキャンニ
ングを行ない２次元映像を得るようにしてもよい。

自立走行装置〕と目標とされる幾何学図形６との相対位
置を測定するためには、まず、上記撮像手段３で該幾何
学図形６を撮像し、映像画面に映し出す。　この映像画
面に写った図形の映像画面での位置と、」１記撮像手段
３に対する走行平面２の幾ｔｉｒ学図形６の位置との関
係に従って映像画面」二での位置を撮像手段３の座標系
の位置へ変換すれば、目標とする図形６と撮像手段３と
の位置関係が求まる。

第２図は、この映像画面に写った図形の映像画面での位
置と、上記撮像手段３に対する走行平面２の幾何学図形
６の位置との関係を示す図である。

即ち、第２図に示すように、撮像手段３の座標系（カメ
ラ座標系）は、撮像手段３のレンズ３２を原点とする直
交座標系で、レンズ３２から視て光軸に直交する横方向
右側を「正」にＸ軸を、縦方向」二側を「正」にｙ袖を
、レンズ３２の光軸上奥行ぎ方向を「正」１こＺ軸をと
る。　映像画面は、レンズ３２の焦点距離をＦとすると
、カメラ座標系で点（０，０，Ｆ）でＺ軸に直交する平
面上に位置する。　映像画面の座標系（映像座標系）は
、Ｘ軸に平行でＸの「正」の側と同じ側を「正」にξ軸
を、ｙ軸と平行でｙのＶ正」と同じ側を「正」にη軸を
とる。

目標とする幾何学図形６は、映像座標に第３図のように
投写される。　第４図に示すように、目標とする幾何学
図形６の回転角θは３角形の垂線ＡＯとｙｚ一平面とが
なす角で右ネジ方向の回転を「正」とする。　幾何学図
形６の回転角ψは垂線Ａ○とＸＺ一平面とが゛なす角で
右ネジ方向の回転を「正」とする。　なお、第５図に示
すように、幾何学図形６の垂線Ａ○がＸＺ一平面のＺ軸
との間でＸ軸周りにグの角をなすことは、光軸７が？だ
け下方（または上方）に回転していることと等価である
。中点Ｏかレンズ３２の位置から（α、−β。

ρ）だけ離れ、図形６が０点を中心にθだけ回転してお
り、この図形６を撮像するために、レンズ３２の光軸が
り・だけ下を向いている場合、幾何学図形６の各頂点Ａ
　Ｂ　Ｃと底辺の中点０のカメラ座標系での位置は次の
ようになる。

Ａ点の座標：（α−ＪＩ　Ｓ　；　ＩＩθ［β＋ａｃ、
ｏｓθＳ　Ｉ　ＩＩ　？ｒＱ　＋　ａｃｏｓ０ｃｏｓｓ
／）） Ｂ点の座標：（α−１）ＣＯ３θ、−β−１）　Ｓ　ｉ
　ｎθ５ｉｎｙ）。

Ｉ２−１）ｓｉｎθｃｏｓｓ’） ０点の座標：　（Ｑ　＋　ｌ）　ＣＯＳθ、−β＋ｂｓ
ｉｎθ５ｉｌｌｙ’＋４＋ｌ：＋ｓｉｎ　θｃｏｓｙ’
） （）点の座標：（α、−β、ρ）。

一方、カメラ座標系での位置（Ｘ　ｒ　Ｙ　＋　２　）
は、映像画像座標では、 ξ＝）Ｐ矛　−−−（１） η二ｒＬ　　−−−＜２） となり、目標とする幾何学図形６の各頂点Ａ　Ｂ　Ｃ及
び底辺ＢＣの中点Ｏの座標は映像座標系では以下の各式
て゛与えられる。

撮像手段３で既知の幾何学図形６をとらえ、その映像を
電気信号に変換し、その電気信号を基にして映像座標系
でのＡ点（乙、η、）、Ｂ点（ξ８゜ηＢ）、０点（ξ
。、η。）、０点（ξ０．η。）の値を実測値として求
める。　これらの実測値から実際の３次元空間内での既
知の幾何学図形６と撮像手段３との位置関係を表わす（
α、β、ρ、θ）、Ｓ／）を（１）〜（１０）式を用い
て求めることができる。

すなわち、（１）〜（１０）式より、 θ＝ｃｏｓ−’（Ｋρ）　　　　　　　　　−−−−−
（１１）−−−−−（１４） α−，！−Ｌρ　　　　　　　　−−−−−（１５）β
−」恋ρ　　　　　　　　−−−−−（１，６）これら
の値の組（α、β、＋２．θ）、ｙ）により既知幾何学
図形６に対する撮像手段３の位置か一意的に定まる。

なお、光軸Ｚの回転角２は既知幾何学図形が撮像手段３
が置かれている面でその面に平行に置かれておれば、レ
ンズ３３を上下に動かすことにより検出できる。

実際の（ξ、η）の読み取りは、例えば、第６図に示す
ように、既知幾何学図形６を撮像手段３で撮像しくＦｌ
）、この画像を背景から切出し、第７図（ａ）に示す原
画像に微分処理を行って、同図（１））に示すようにエ
ツジ検出をする（Ｆ２）。　エツジ検出された画像は多
値レベルの電気信号で構成されているので、スレッシュ
ホールド値により２値化させ（Ｆ３）、第７図（ｃ）に
示すように画像を細線化する（Ｆ４）。　この細線化さ
れた図形をもとに、線列を接続して細線化された幾何学
図形の各辺及び垂線を直線化してモデル化を行なって、
第７図（ｄ）に示すような正確な図形を得る。　そして
、各辺の交点から抽出点Ａ、Ｂ、Ｃ，Ｏの映像画面座標
（ξ、η）を検出している。

なお、本発明方法に用いる幾何学図形は、上述の２等辺
３角形に限られることはない。　すなわち、各頂点Ａ、
Ｂ、Ｃ１垂線ＡＯ１底辺ＢＣが検出できＡＯ＝８、Ｂ○
＝ｏｃ＝ｂで上下非対称の幾何学図形であればよい。　
例えば、第８図（ａ）〜（ｄ）に示す各幾何学図形はこ
れらの条件を満たしているので、本発明に用いることが
できる。

ところで、本発明に係る自立走行装置の位置計測方法は
、自立走行装置１の軌道制御に応用することができる。

　すなわち、この計測方法によって、（α、β、ρ、θ
）、２が求まり、これらの値から撮像手段３のレンズ直
下の走行平面２の１点と既知幾何学図形の中点０との距
離り及び自立走行装置１の向きと幾何学図形６の垂線Ａ
Ｏとがなす相対角度β′が分かる。　従って、自立走行
装置１を、現在見ている幾何学図形６に対して指定され
た相対角度、指定された距離を保つこと、あるいは指定
された相対角度で、あるいは指定された相対角度になる
ように制御しながら現在見ている幾何学図形に近づいて
行くように制御できる。　また、逆に現在見ている幾何
学図形から遠ざかるような制御も行い得る。

例えば、第９図に示すように、走行平面２上に既知の幾
何学図形６１〜６′１６を基盤目状に配ダル１、これら
個々の幾何学図形６１−．６１６を順に目標にして、目
的地まで追随させる。　この場合、走行平面２の幾何学
図形６１〜６１６の配置の地図を自立走行装置１に記＠
！、させる。　例えば、その幾何学図形の番号、向た、
隣接する幾何学図形の番号とそこまでの距離及び方向等
のデータを各幾何学図形について記憶させる。　そして
、次の各式に従って自立走行装置１と当該幾何学図形と
の走行平面２での位置関係が分かるので、幾何学図形６
１〜６１６を目標として自立走行装置１の位置・軌跡の
制御をイーない得る。

ｄ＝ｐｃｏｓ　９ンーβｓｉｎ　Ｓ／）　　　　　　　
　　　　　　−−−−−（１７）Ｄ　＝ｖ１フＷ−−−
−−（１８） ７′＝γ十ｔａｎ１−９−−−　　　　　　　　−−−
−−（１９）θ′＝γ′−〇 ＝γ＋ｊａｎしく一θ　　　　−−−−−（２０）〔ま ただし、７′：　自立走行装置の方向とレンズ中心・中
点○を結 ぶ・線分とがなす角度 γ：　自立走行装置の方向と レンズの光軸とかなす角 度 θ′：　自立走行装置と幾何学 図形の垂線Ａ　Ｏとかなす 相対角度 Ｄ：　レンズ中心と幾何学図 形の中点Ｏとの走行平面 上での距離 例えば、第１Ｏ図に示すように、レンズの光軸と自立走
行装置１の進行方向とを一致させた後、はぼ映像画面の
中心で幾何学図形６１をとらえながらその幾何学図形６
１に近づいたとき、撮像手段３は第１図で示された探索
範囲を持っているので、幾何学図形６１は撮像手段３の
視界５から消える。

しかし、その前に自立走行装置］の進行方向を変えずに
撮像手段：（をωだげ回転させて次の幾何学図形６Ｊを
撮像手段３の映像画面のほぼ用］心でとらえるように構
成される。　幾何学図形６Ｉに向う進行方向の符号と幾
何学図面６ｊに向う進行方向の符号とが異なると５には
、進行方向の符号も撮像手段３の方向の切換えと同時に
切換えてしまう。

そして、レンズが幾何学図形６Ｊをほぼ中心にとらえる
ようにレンズを回転さぜながら幾何学図形６１に向って
直進を続ける。　自立走行装置１の方向レンズの光軸と
かなす角か図−εになったときに自立走行装置１をその
場で幾何学図形６Ｊに向って！−−εのターンを行わせ
る。　撮像手段３はこれと逆に回転させられる。　この
ようにして、自立走行装置１か′直角に曲ったと外でも
次の目標である幾何学図形６）を撮像手段３のほぼ中心
でとらえることができ、そのまま幾何学図形６Ｊをとら
えながら幾何学図形６Ｊに向って自立走行装置１を進め
ることかでトる。

幾何学図形の配列は、種々考えられるが、地図の記憶の
容易化、目的地への通路の算出の容易化等の観点から基
盤目状に配列するのが最良である。

幾何学図形の配列が基盤目状でないときには、幾何学図
形間の距離ρｉｊは走行平面２内でベクトルとして、ま
た、幾何学図形の向きはｙ軸、ｙ軸に対する符号付きの
角度で表現される。　　　また、スタート地点の幾何学
図形６１は、池の幾何学図形と区別でとるようにする。

　これにより、スタート地点を間違うことをなくせる。

なお、幾何学図形６は走行平面の天井面に設けてもよく
、また、天井面や走行平面から浮外出した高さに設けて
もよい。

上述のように本発明に係る自立走行装置の位置計測方法
によれば、１台の撮像手段を用いて自立走行装置の位置
を計測するので、設備コストが少ないうえランニングコ
ストも安価になるうえ、計測白木を短時間で行なえる。

　　また、自立走行装置の誘導のだめの施設は、幾何学
図形を設けるだけで済み、幾何学図形の密度を大とくす
るだけできめ細かな誘導を行なえるので、電磁誘導力法
や光学的誘導力法に比へて簡単で安価になる笠の効果を
奏する。

【図面の簡単な説明】

第１図は本発明の一実施例の要部の斜視図、第２図はそ
のカメラ座標系」二の幾何学図形の位置と映像画面に写
ったその映像の位置との関係を示す斜視図、第３図はそ
の映像画面座標への幾何学図形の投影図、第４図は幾何
学図形の回転角θを説明する斜視図、第５図は幾１１１
ｆ学図形の回転角ｙ）を説明する図、第６図は図形処理
のフローチャート、第７図（ａ）〜（（１）はその各段
階における幾何学図形の態様を示す図、第８図（ａ）〜
（〔１）はそれぞれ幾何学図形の変形例の正面図、第９
図は幾何学図形の配置を示す平面図、第１０図は幾何学
図形を基盤目状に配列した場合の自立走行装置の走行制
御の手順を説明する平面図である。 １・・・自立走行装置、３・・・撮像手段、６・・・幾
何学図ノ］気

Claims (2)

【特許請求の範囲】
1. （１）　　ロボット等の自立走行装置に搭載した撮像手
   段の視界内に配置した特定形状の幾何学図形を、上記撮
   像手段で撮像し、前記幾何学図形の撮像面上の座標値か
   ら自立走行装置の位置を算出することを特徴とする自立
   走行装置の位置計測方法。
2. （２）幾何学図形として２等辺３角形又はそれと等価な
   図形を用いた特許請求の範囲第１項に記載の自立走行装
   置の位置計測方法。