JPH06229771A - 経路算出装置 - Google Patents

Abstract

(57)【要約】 【目的】 高い信頼性をもって移動経路を適確に算出す
ることができる経路算出装置を提供する。 【構成】 レンジセンサ２０１で対象物までの距離を計
測するとともに、この距離データとともに出力される付
加データから距離データの信頼度を算出し、この信頼度
の付加された距離データに基づき対象物の形状特徴点を
特徴点列算出回路２０４で抽出するとともに該特徴点の
信頼度を算出し、この抽出された複数の特徴点からなる
特徴点列およびその信頼度から移動体が移動する経路を
経路算出回路２０５で算出する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、レンジセンサにより対
象物の局所形状を計測しながら対象物の形状特徴点に沿
ってロボットまたはアクチュエータ等の移動体を移動さ
せるための経路算出装置に関する。

【０００２】

【従来の技術】この種の経路算出装置は、従来、センサ
を用いて経路を算出するために、移動しながら対象物の
表面までの距離をスリット光により逐次的に計測するレ
ンジセンサと、スリット光により照射されて得られた曲
線上の位置に対応する距離データから対象物の特徴点を
抽出する特徴抽出手段と、逐次的に得られる特徴点列か
らロボットまたはアクチュエータが移動するための経路
を算出する手段とを縦列的に接続し処理していたが、こ
のように複数の手段が独立に構成されるため、１つの手
段の誤差が次段の処理に大きな誤差を発生していた。

【０００３】図２は、従来の経路算出装置の構成を示す
ブロック図である。図２において、１０１はレンジセン
サ、１０２は雑音除去回路、１０３は特徴点列算出回
路、１０４は経路算出回路である。レンジセンサ１０１
は、例えば文献（小関、中野、山本：「光切断法を用い
た実時間距離検出装置」、電子情報通信論文誌、Vol,J6
8-D,No.5,pp.1141-1148,1985）に示されるように三角測
量に基づき対象物にスリット光を照射し、２次元センサ
によりスリットの位置を観測して対象物までの距離を測
定する。２次元センサにはスリット光が対象物に反射し
た結果、曲線として受光される。特に対象物が多面体の
場合には折れ線となる。以下では、対象物を多面体と仮
定して説明する。折れ線データから三角測量により２次
元センサのｘ−ｙ座標に対する奥行きｚを算出すること
ができる。

【０００４】雑音除去回路１０２は、平均操作などによ
り雑音を除去することにより上記計測誤差の影響を低減
するものである。特にランダム雑音を含む信号には雑音
除去効果が大きい。雑音を低減した距離情報は続いて特
徴点列算出回路１０３に入力される。特徴点列算出回路
１０３ではスリット光により得られた折れ線（多面体を
仮定）から特徴点を抽出する。例えば３次元物体の稜
線、谷線を検出する場合には特徴点として折れ線の角を
検出すればよい。この方法として例えばHough 変換によ
り折れ線を構成する２本の直線を検出して、その交点を
角として推定する方法が知られている。

【０００５】このようにして逐次的に得られるこの特徴
点列を次に経路算出回路１０４に入力し、ロボットまた
はアクチュエータが移動する経路を算出する。経路算出
回路１０４では、特徴点列から得られる経路上の位置お
よび姿勢データに基づいて、３次元空間での連続的な軌
道データ、あるいは予め教示された軌道の修正量を算出
し、ロボットコントローラへ出力する。この結果、ロボ
ットまたはアクチュエータは対象物体上の目標経路に追
従することができる。

【０００６】

【発明が解決しようとする課題】上述した従来の経路算
出装置においては、レンジセンサから得られた２次元セ
ンサのｘ−ｙ座標に対する奥行きｚには計測誤差を伴
う。例えば対象物の表面の材質、色、形状、表面の法線
方向などに応じて反射の特性が変動する。そのため、例
えば反射レベルの低いところに対しては一般に計測誤差
が大きい。即ち、データの信頼度は測定点毎に異なるこ
とになる。雑音除去回路は、平均操作などにより雑音を
除去することにより上記計測誤差を低減するものであ
る。上記レンジセンサのように反射レベルに応じて誤差
のパワーが測定点毎に変化する場合（例えば、表面の反
射率、形状により反射レベルは大きく異なる）、平均操
作によって得られた結果のデータの信頼度も測定点毎に
異なることになる。即ち、センサのデータそのものおよ
びセンサデータを雑音除去したデータどちらも信頼度は
測定点毎に異なる。

【０００７】従来は上記の方法によって得られた距離デ
ータをそのまま用いて次の特徴点抽出、経路算出を行っ
ていた。しかしながら、信頼度の異なるデータに同一の
処理を行っても最適な特徴点抽出、経路算出を行うこと
はできない。これはデータの信頼度を各処理過程で一定
と見なしていることに起因する。

【０００８】本発明は、上記に鑑みてなされたもので、
その目的とするところは、高い信頼性をもって移動経路
を適確に算出することができる経路算出装置を提供する
ことにある。

【０００９】

【課題を解決するための手段】上記目的を達成するた
め、本発明の経路算出装置は、対象物までの距離を計測
する距離計測手段と、該距離計測手段から前記距離デー
タとともに出力される付加データから前記距離データの
信頼度を算出する信頼度算出手段と、該信頼度算出手段
で算出された信頼度の付加された前記距離データに基づ
き対象物の形状特徴点を抽出するとともに該特徴点の信
頼度を算出する特徴抽出手段と、該特徴抽出手段で抽出
された複数の特徴点からなる特徴点列および該特徴点列
の信頼度から移動体が移動する経路を算出する移動経路
算出手段とを有することを要旨とする。

【００１０】

【作用】本発明の経路算出装置では、対象物までの距離
を計測するとともに、この距離データとともに出力され
る付加データから距離データの信頼度を算出し、この信
頼度の付加された距離データに基づき対象物の形状特徴
点を抽出するとともに該特徴点の信頼度を算出し、この
抽出された複数の特徴点からなる特徴点列およびその信
頼度から移動体が移動する経路を算出する。

【００１１】

【実施例】以下、図面を用いて本発明の実施例を説明す
る。

【００１２】図１は、本発明の一実施例に係わる経路算
出装置の構成を示すブロック図である。同図において、
２０１はレンジセンサ、２０２は分散算出回路、２０３
は雑音除去回路、２０４は特徴点列算出回路、２０５は
経路算出回路である。

【００１３】レンジセンサ２０１は、三角測量に基づき
対象物にスリット光を照射し、２次元センサによりスリ
ットの位置を観測して対象物の距離を測定する。また、
各点の反射光の受信レベル、反射スリット光の巾も同時
に出力する。このとき対象物の表面の材質、色、形状、
表面の法線方向などに応じて反射の特性が変動する。そ
のため、計測した結果には計測誤差を伴う。

【００１４】分散算出回路２０２はレンジセンサ２０１
より各点の反射光の受信レベル、反射スリット光の巾を
入力し、計測対象物の距離における誤差の分散の推定値
を出力する。一般に反射レベルが小さいほど計測された
距離データの信頼度が低く、また反射されたスリットの
巾が広いほど信頼性が低い。表１は反射レベルＬおよび
反射スリット巾Ｗに対応して計測距離データの誤差の分
散を評価した一例である。

【００１５】

【表１】 更に詳しくは、表１は分散算出回路２０２において参照
される誤差分散のテーブルの一例であり、横軸は反射レ
ベルＬを８段階に量子化したもの、縦軸はスリット巾を
Ｗを６段階に量子化したものである。分散値の単位は１
／１００mm、数値が零の箇所は計測データが無いところ
である。

【００１６】雑音除去回路２０３は入力される近傍数点
の重み付け平均を算出する。その際、近傍各点の信頼度
に基づき重みを変更する。信頼度は例えば、分散σ
_(i,j) で表現される。但し、ｉ，ｊはセンサ上のｘ−ｙ
座標を表す。雑音除去回路２０３は以下の式に基づいて
行う。

【００１７】出力値Ｘ＊は、

【数１】 Ｅ＝Σ［（Ｘ_(i,j) −Ｘ＊_(i,j) ）∧２／σ_(i,j) ＾２］ （１） を最小化することにより得られる。

【００１８】また平均値に対する分散は、

【数２】 σ＊_(i,j) ＝ｓｑｒｔ［（Σσ_(i,j) ＾２）／Ｎ］ （２） により与えられる。但し、Ｎは演算に用いる近傍画素数
である。

【００１９】次に、特徴点列算出回路２０４では、特徴
点として折れ線の角を検出する。そのためにまず、Houg
h 変換により折れ線を構成する２本の直線を検出する。
Hough 変換は以下の処理により行う。

【００２０】幾つかの点が直線 ｙ＝ａｘ＋ｂ （３） の上に存在していると仮定すれば、

【数３】 ｙ_i ＝ａｘ_i ＋ｂ， ｉ＝１，２，・・・，ｎ （４） となるが、これらの式を、（ａ，ｂ）を変数とみなして
（ａ，ｂ）平面を考えると、そこでの直線の式は、 ｂ＝−ｘ_i ａ＋ｙ_i （５） となる。これらの直線は、（ｘ_i ，ｙ_i ）によって全て
異なるが、（ｘ_i ，ｙ_i）は全て同一直線上に乗ってい
ると仮定したのであるから、この直線を ｙ＝ａ_o ｘ＋ｂ_o （６） であるとすると、式（５）は（ｘ_i ，ｙ_i ）のいかんに
かかわらず（ａ，ｂ）平面上の点（ａ_o ，ｂ_o ）を通ら
なければならない。そこで（ａ，ｂ）平面を十分細かい
升目に区切って、（ｘ_i ，ｙ_i ）に対応する直線がある
升目を通過するものであれば、その升目の計数を１増
す。これを全ての（ｘ_i ，ｙ_i ）について行えば、当然
のことながら（ａ_o ，ｂ_o ）に対応する升目の計数値が
最大になるはずである。そこで（ａ，ｂ）平面上で升目
の計数値最大のものを求めれば、それによって（ａ_o ，
ｂ_o ）が決まったことになる。そして直線の方程式は式
（６）となる。このようにして２本の直線をHough 変換
した場合、標本空間上では２つのある最大値を持つ山と
して表される。

【００２１】通常のHough 変換では単純に升目の計数を
加算するが、ここでは雑音除去されたデータの分散の評
価値に応じて加算する値を変化させる。例えば、１／
（σ＊＾２）で重み付けした値を投票する。このように
して元々のデータの信頼度に応じて、より妥当な直線検
出が可能となる。また投票の結果得られたこの標本空間
上の点の広がりを分散として表せば、この直線の信頼度
が得られる。直線の交点の信頼度はそれらの信頼度の平
均値として評価できる。具体的には２本の直線それぞれ
について、直線と直交する方向の分散を算出し、この平
均を交点の信頼度とする。

【００２２】このようにして算出された特徴点列とその
信頼度を用いてロボットの経路を生成するには、例えば
スプライン関数を用いた方法（特願平４−１２４９９
５）などを併用すると効果的である。この場合、具体的
な手順は次のようになる。

【００２３】まず、経路生成回路２０５では、上記特徴
点列算出回路２０４からの折れ線情報から求まる経路上
の注目点位置および姿勢を基に、経路関数を表すｎ次多
項式を逐次的に生成する。この場合、センサのスキャン
毎に新たな区間経路関数が生成される。

【００２４】具体的には例えば以下のようにする。これ
から生成しようとする第ｉ番目の区間の経路関数を表す
ｎ次多項式（例えば３次のスプライン関数など）の未定
係数のうち１ないし２個を既に生成されている前区間の
経路関数との接続条件から決定する。この時の接続条件
としては経路位置の１次ないしは２次微分の連続性など
と対応する。残りの未定係数は前記特徴点列生成回路か
ら出力されてくる有限個の信頼度付き特徴点列データに
重み付け最小２乗法を適用して決定する。こうして決定
された多項式が第ｉ番目の区間の経路関数を表すことに
なる。この結果得られた経路関数は、分散算出回路２０
２、雑音除去回路２０３、さらにセンサ特徴点列の信頼
度を反映したものである。

【００２５】上述したように、従来、雑音除去、特徴点
抽出、経路算出においてデータの信頼性を一定と見なし
ていたのに対して、本発明の経路算出装置では、センサ
入力からの計測データの信頼度に応じて適応的に雑音除
去処理を行い雑音除去データを出力し、また同時に処理
結果の信頼度を算出する。雑音除去されたデータおよび
そのデータの信頼度は次の特徴点抽出処理部２０４に入
力され、ここで特徴点の抽出およびその信頼度を算出す
る。最後に経路算出において特徴点列およびその信頼度
を用いて経路を算出する。このようにデータとともに信
頼度を再評価、更新しながら次段の処理に持ち回るとこ
ろに特徴がある。

【００２６】従って、本発明の経路算出装置では、何等
かの理由でセンサが誤った情報を出力したり、また測定
対象物の表面形状の局所的変化などによってセンサ情報
が異常となるような場合でも信頼性の高い経路生成が可
能である。

【００２７】

【発明の効果】以上説明したように、本発明によれば、
対象物までの距離を計測するとともに、この距離データ
とともに出力される付加データから距離データの信頼度
を算出し、この信頼度の付加された距離データに基づき
対象物の形状特徴点を抽出するとともに該特徴点の信頼
度を算出し、この抽出された複数の特徴点からなる特徴
点列およびその信頼度から移動体が移動する経路を算出
するので、高い信頼性をもって移動経路を算出すること
ができ、ロボットまたはアクチュエータ等の移動体を対
象物の形状特徴点に沿って適確に移動させることができ
る。

【図面の簡単な説明】

【図１】本発明の一実施例に係わる経路算出装置の構成
を示すブロック図である。

【図２】従来の経路算出装置の構成を示すブロック図で
ある。

【符号の説明】

２０１ レンジセンサ ２０２ 分散算出回路 ２０３ 雑音除去回路 ２０４ 特徴点列算出回路 ２０５ 経路算出回路

───────────────────────────────────────────────────── フロントページの続き (72)発明者 柿崎 隆夫 東京都千代田区内幸町１丁目１番６号 日 本電信電話株式会社内

Claims (1)

【特許請求の範囲】
1. 【請求項１】 対象物までの距離を計測する距離計測手
   段と、該距離計測手段から前記距離データとともに出力
   される付加データから前記距離データの信頼度を算出す
   る信頼度算出手段と、該信頼度算出手段で算出された信
   頼度の付加された前記距離データに基づき対象物の形状
   特徴点を抽出するとともに該特徴点の信頼度を算出する
   特徴抽出手段と、該特徴抽出手段で抽出された複数の特
   徴点からなる特徴点列および該特徴点列の信頼度から移
   動体が移動する経路を算出する移動経路算出手段とを有
   することを特徴とする経路算出装置。