JPH0661101B2 - 位置ずれ測定装置 - Google Patents

Description

【発明の詳細な説明】 ［発明の目的］ （産業上の利用分野） 本発明は、例えば移動ロボットに対して設定された走行
軌道からの位置ずれを該移動ロボットに搭載されたステ
レオカメラによる入力されるステレオ画像から効果的に
求め、その走行制御に用いることのできる実用性の高い
位置ずれ測定装置に関する。

（従来の技術） 近時、カメラの軽量・小形化と視覚情報処理技術の発展
に伴い、視覚機能を備えた移動ロボットの開発が盛んに
進められている。この種の視覚機能を備えた移動ロボッ
トは、従来の近接センサを備えた移動ロボットに比較し
て移動環境の認識やその走行経路の自動設定等、高度な
能力を持たせることが可能である。これ故、例えば原子
炉発電所における保守点検作業を行なうべく、その自律
走行が要求される分野への適用が期待されている。

さて視覚機能を備えた移動ロボットとしては、例えばカ
メラにより入力された画像と予め設定された移動環境の
特徴モデルとを比較照合してその走行経路を制御するも
のが報告されている。また上記移動環境の特徴モデルを
用いることなく、ステレオ視カメラを用いて入力された
ステレオ画像を３次元処理してその移動経路を設定して
いくもの等が報告されている。

然し乍ら、移動環境の特徴モデルを用いる前者の移動ロ
ボットにあっては、その特徴モデル化の容易な単純環境
に適用可能であるが、複雑な背景画像を含むような環境
下では、例えばその特徴モデル化や入力画像の比較照合
処理等が徒に複雑化する等の問題があった。

一方、ステレオ画像を入力して３次元処理する後者の移
動ロボットでは、予め環境モデルを作成しておく必要が
ないが、その反面、入力されたステレオ画像の各部に対
する対応付け等の複雑な計算処理を必要とする。しかも
様々な環境情報を入力するステレオ画像に対する３次元
計算には多大な処理時間を必要とする。この為、該移動
ロボットの高速、且つ円滑な走行制御が困難であると云
う問題があった。

（発明が解決しようとする問題点） 本発明は、上述したように従来の移動ロボットに組込ま
れる視覚機能が単純環境にしか適合せず、或いはその処
理に多大な時間を必要とする等、該移動ロボットの円滑
な走行制御を望み得ないことに鑑みて、また同時に移動
ロボットが完全なる自由環境を走行することが極めて希
であることに着目してなされたもので、移動ロボットの
視覚機能を用いてその走行制御に必要な情報、つまり設
定された走行軌道からの位置ずれを高速に検出し、その
走行制御に有効に利用することを可能とする位置ずれ測
定装置を提供することを目的としている。

［発明の構成］ （問題点を解決するための手段） 本発明は、移動ロボットに搭載されたステレオカメラに
より目標物を含む環境のステレオ画像を入力するステレ
オ画像入力手段と、このステレオ画像入力手段から入力
されたステレオ画像中から予め登録された複数の目標物
及び基準目標をそれぞれ抽出する抽出手段と、前記移動
ロボットの前記ステレオ画像を入力した地点からの、前
記抽出手段で抽出された前記複数の目標物の３次元位置
を予め３次元位置の判っている前記基準目標の校正デー
タに従ってそれぞれ求める３次元位置検出手段と、この
３次元位置検出手段で求められた前記ステレオ画像を入
力した地点からの前記複数の目標物の３次元位置と前記
移動ロボットに対して設定された走行軌道上の予め定め
られた地点からの前記複数の目標物の３次元位置とをそ
れぞれ比較してこれらの座標値間の関係を示す座標変換
行列を求める座標変換行列生成手段と、この座標変換行
列生成手段で生成された座標変換行列に従って前記移動
ロボットの前記ステレオ画像を入力した地点と前記予め
定められた地点との間の平行移動量及び前記ステレオ画
像を入力したときの前記ステレオカメラの回転角度から
なる位置ずれ量を計算する位置ずれ量計算手段と、この
位置ずれ量計算手段で計算された位置ずれ量に従って前
記移動ロボットの走行動作を制御する制御手段とを具備
したことを特徴とする位置ずれ測定装置。

（作用） 本発明によれば、移動ロボットの或る位置で入力された
ステレオ画像から求められる目標物の上記位置（現地
点）からの３次元位置と、上記移動ロボットの予め設定
された地点からの目標物の３次元位置とを比較してこれ
らの座標値間の関係を示す座標変換行列を求め、この座
標変換行列に従って計算することによって、その視点位
置の異なりによる目標物の３次元位置のずれ量が求めら
れ、この３次元位置のずれ量に対応して上記視点位置の
ずれ量、つまり予め設定された走行径路上の特定位置と
自走式走行ロボットの現在位置との間の平行移動量及び
現在位置で前記ステレオ画像を入力したときの前記ステ
レオカメラの回転角度とからなる位置ずれ量を直接的に
且つ高速に求めることが可能となる。従ってこの位置ず
れ量の情報に従って上記移動ロボットの走行径路に対す
る走行位置補正、つまりその走行制御を効果的に行なう
ことが可能となる。

（実施例） 以下、図面を参照して本発明の一実施例につき説明す
る。

第１図は実施例装置の概略構成図で、その処理機能は目
標物抽出部1 、３次元位置検出部2 、および位置ずれ量
計算部3 に大別される。

目標物抽出部1 は、移動ロボットが走行する環境の、例
えば作業室内の曲り角や隅部等の目標物を、該移動ロボ
ットに搭載されたステレオカメラにより撮像入力された
ステレオ画像中から抽出するものである。３次元位置検
出部2 は、上記目標物抽出部1 にて抽出された目標物に
ついて、前記ステレオ画像を入力した時点の前記移動ロ
ボットの位置からの３次元位置を前記ステレオ画像に従
ってステレオ計算処理、具体的には三角測量法の原理に
従って計算するものである。そして位置ずれ量計算部3
は、上記ステレオ画像から求められた目標物の３次元位
置と、予め設定された地点に対する目標物群の３次元位
置とをそれぞれ比較し、同一の目標物に関する上記３次
元位置のずれ量から前記ステレオ画像を入力した時点の
前記移動ロボットの位置の、予め設定された地点からの
位置ずれ量を計算するものとなっている。

この位置ずれ量計算部3 で求められた位置ずれ量が移動
ロボットの移動制御部に与えられ、その走行が制御され
る。

さて第２図は上記目標物抽出部1 の構成例を示す図であ
る。移動ロボットに搭載されるステレオカメラは、例え
ば水平方向に所定の距離を隔てて設けられた２台のカメ
ラ、つまり左カメラ5aと右カメラ5bとからなり、制御部
6 の制御を受けるスイッチ回路7 を介して１シーンづつ
画像入力部8 に入力されるようになっている。画像入力
部8 はこの撮像入力された左右２枚の画像からなるステ
レオ画像を、その内部に準備された画像メモリに格納し
て以下に説明する画像処理に供するものである。

しかして前処理部11は、上記画像メモリに格納されたメ
モリ画像に対して、例えば空間フィルタリングによるラ
ンダム雑音の除去やシェーディング補正処理等を行なっ
ている。特徴抽出部12はこの前処理が施されたステレオ
画像に対して、目標物の種類に応じた画像の特徴を抽出
している。

例えば入力されたステレオ画像が第３図（ａ）に例示す
る廊下（通路）を示す環境からなる場合、目標物Ａとし
ては壁と床との交点や柱の角と床との交点等として設定
される。このような場合には、画像の特徴として線分を
抽出すれば良いから、その入力画像を微分処理し、その
微分出力を２値化して第３図（ｂ）に示す如き線分画像
を得れば良い。

このようにして求められる画像の特徴（線分画像）か
ら、目標物位置検出部13にて予め設定された目標物を第
３図（ｃ）に示すように検出する。この目標物の検出
は、例えば第４図に示すような目標物に関する特徴パタ
ーンを準備し、前記第３図（ｂ）に示す線分画像に対し
てマッチング処理を行なうようにすれば良い。具体的に
は、第３図（ｂ）に示す線分画像中から部分画像を切出
し、この部分画像が第４図に示す特徴パターンに適合す
るか否かを調べる。この処理を上記部分画像の切出し位
置を順次変えながら繰返し行い、パターンがマッチング
したときこれを目標物の検出とし、且つそのときの部分
画像の切出し位置を目標物の検出位置とすれば良い。

このようにしてステレオ画像中から検出される目標物の
検出情報が目標物位置メモリ14に格納される。尚、目標
物は１つに限られるものではなく、複数種類あっても良
い。この場合には各目標物毎にその検出位置情報を目標
物位置メモリ14にそれぞれ格納するようにすれば良い。

一方、３次元位置検出部2 は、例えば第５図に示すよう
に構成される。この３次元位置検出部2 （第５図に示す
ブロック）は、予め３次元位置のわかっている基準目標
（マーク）を用いて校正データを作成し、この校正デー
タに従って前述した如く検出された目標物Ａの３次元位
置を計算するものとなっている。

即ち、マーク位置検出部21は前記画像入力部8 の画像メ
モリに格納された左右２枚のステレオ画像中から予め設
定された基準目標（マーク）をそれぞれ検出している。
このマークは、例えば入力画像中の識別容易な対象物と
して複数箇所に亙って定められる。そして上記左右の２
枚の画像における各マークの位置関係からそのマークの
対応付けを行い、校正計算部22にて後述する３次元位置
計算に必要なステレオパラメータを校正データとして求
めている。

つまり上記各マークのステレオ画像における対応関係か
ら、そのステレオ画像における対応点間の３次元位置を
特定するステレオパラメータが求められている。この校
正データ（パラメータ）が校正データメモリ23に格納さ
れる。

ステレオ計算部24は、このステレオパラメータを用いて
前記目標物メモリ14に格納された目標物の前記ステレオ
画像における位置から、画素目標物の３次元位置を計算
するものであり、これによって求められた目標物の３次
元位置が３次元座標メモリ25に格納される。従ってこの
３次元座標メモリ25に格納される前記目標物の３次元位
置は、前記ステレオ画像を入力した時点の移動ロボット
の位置（現地点）を基準とした３次元位置（ｘ，ｙ，
ｚ）となる。

さて前記位置ずれ計算部3 は、例えば第６図に示すよう
に構成される。

目標物モデル座標メモリ31には、予め設定された移動ロ
ボットの走行経路上の各地点から見た目標物の３次元位
置データが予め格納されている。

この目標物モデル座標メモリ31に格納される目標物の３
次元位置データは、例えば移動ロボットを上記走行経路
上を準備走行させ、その時に求められる目標物の３次元
位置データ（Ｘ，Ｙ，Ｚ）等として与えられるものであ
る。

変換行列計算部32は、上記目標物モデル座標メモリ31に
予め格納されている目標物の、予め定められた地点から
の３次元位置（Ｘ，Ｙ，Ｚ）と、前記３次元座標メモリ
25に格納された移動ロボットの現地点から見た目標物の
３次元位置（ｘ，ｙ，ｚ）とから、上記定められた地点
と現地点との座標変換行列Ｔを計算している。

即ち、第７図に示すように平面上を動く移動ロボットに
対して定められた走行経路Ｐ上の或る地点をＱとし、上
記移動ロボットが自走して前記ステレオ画像を得た現地
点をＲとした場合、それらの間の平行移動量はＶ（p,q,
r）として与えられる。また目標物を撮像入力するステ
レオ視カメラの向きが準備走行時と現地点とで異なるこ
とから、そのｚ軸回りの回転角度をθとする。

すると、同一の目標物に対して求められた上記３次元位
置の座標間には なる関係が成立する。但し、 α＝−ｐ cosθ−ｑ sinθ β＝ ｐ sinθ−ｑ cosθ である。これらの座標値間の関係を示す行列が前記座標
変換行列Ｔである。

変換行列計算部32は、前記ステレオ画像から求められる
最低２つの目標物に関する３次元位置情報から上述した
式を連立させ、これを解いて座標変換行列Ｔを求めてい
る。そして位置ずれ量出力部33は、上記座標変換行列Ｔ
から前記定められた地点Ｑに対する移動ロボットの現地
点Ｒの位置ずれ量（ｐ，ｑ，ｒ，θ）を計算し、これを
移動ロボットの動作制御部34に出力するものとなってい
る。

尚、ステレオ画像から２組以上の目標物が抽出される場
合には、例えば最小２乗法の原理を採用して座標変換行
列Ｔを求めることにより、その演算精度が向上する。従
って上述した如く求められる位置ずれ量の測定精度の向
上を図ることが可能となる。また位置ずれに起因して幾
つかの目標物がステレオカメラの視野から外れた場合で
あっても、少なくとも２組の目標物が上記視野内に残れ
ば、上述した位置ずれを求めることが可能なことは云う
までもない。

以上のように本発明によれば、移動ロボットに搭載され
たステレオカメラにより入力されたステレオ画像から、
該移動ロボットの本来の走行位置からの位置ずれ量を非
常に簡易に求めることができる。しかも、数個の目標物
に対するステレオ計算だけによって位置ずれを計算する
ので、従来装置のようにステレオ画像の各部の対応付け
や、これらの各部の全てに対するステレオ計算処理が不
要となる。この結果、その計算処理を高速に効率良く実
行することが可能となる。更に目標物として特徴抽出の
容易なものだけを選定することにより、その画像処理が
徒に複雑化することを防ぐことができ、その目標物に対
する計算精度の向上、位置ずれ検出に対する信頼性の向
上を図ること等が可能となる。

故に移動ロボットの走行制御に効果的に適用することが
可能となり、その視覚機能を十分に発揮させることが可
能となる。

尚、本発明は上述した実施例に限定されるものではな
い。例えば目標物としては、曲り角や段差、その他、境
界に固有でしかも特徴抽出の容易なものを適宜採用する
ことができる。その他、本発明はその要旨を逸脱しない
範囲で種々変形して実施することができる。

［発明の効果］ 以上説明したように本発明によれば、移動ロボットの本
走行時にステレオ画像から求められる目標物の３次元位
置と、予め設定された地点から求められる目標物の３次
元位置とを比較し、その位置座標変換行列を求めて上記
ステレオ画像を得た時点の移動ロボットの現地点の前記
設定された地点からの位置ずれを求めるので、移動ロボ
ットの走行制御に必要な情報を簡易に、且つ高速に精度
良く求めることができる。故に移動ロボットの視覚機能
として多大なる効果が奏せられる。

【図面の簡単な説明】

第１図は本発明の一実施例装置の概略構成図、第２図は
実施例装置における目標物抽出部の構成例を示す図、第
３図および第４図は目標物抽出処理の概念を示す図、第
５図は実施例装置における３次元位置検出部の構成例を
示す図、第６図は実施例装置における位置ずれ量計算部
の構成例を示す図、第７図は位置ずれの様子を説明する
ための図である。 1……目標物抽出部、 2……３次元位置検出部、 3……
位置ずれ量計算部、5a，5b……ステレオカメラ、 8……
画像入力部、11……前処理部、12……特徴抽出部、13…
…目標物位置検出部、14……目標物位置メモリ、21……
マーク位置検出部、22……構成計算部、23……校正デー
タメモリ、24……ステレオ計算部、25……３次元座標メ
モリ、31……目標物モデル座標メモリ、32……変換行列
計算部、33……位置ずれ量出力部。

Claims (1)

【特許請求の範囲】
1. 【請求項１】移動ロボットに搭載されたステレオカメラ
   により目標物を含む環境のステレオ画像を入力するステ
   レオ画像入力手段と、 このステレオ画像入力手段から入力されたステレオ画像
   中から予め登録された複数の目標物及び基準目標をそれ
   ぞれ抽出する抽出手段と、 前記移動ロボットの前記ステレオ画像を入力した地点か
   らの、前記抽出手段で抽出された前記複数の目標物の３
   次元位置を予め３次元位置の判っている前記基準目標の
   校正データに従ってそれぞれ求める３次元位置検出手段
   と、 この３次元位置検出手段で求められた前記ステレオ画像
   を入力した地点からの前記複数の目標物の３次元位置と
   前記移動ロボットに対して設定された走行軌道上の予め
   定められた地点からの前記複数の目標物の３次元位置と
   をそれぞれ比較してこれらの座標値間の関係を示す座標
   変換行列を求める座標変換行列生成手段と、 この座標変換行列生成手段で生成された座標変換行列に
   従って前記移動ロボットの前記ステレオ画像を入力した
   地点と前記予め定められた地点との間の平行移動量及び
   前記ステレオ画像を入力したときの前記ステレオカメラ
   の回転角度からなる位置ずれ量を計算する位置ずれ量計
   算手段と、 この位置ずれ量計算手段で計算された位置ずれ量に従っ
   て前記移動ロボットの走行動作を制御する制御手段と を具備したことを特徴とする位置ずれ測定装置。