JP4304512B2 - 経路計画装置 - Google Patents

Description

本発明は、自動機械の全体または一部分などの動作経路を計画する経路計画装置に関する。

ロボットを移動させる際や、ロボットアームを自動操作する際には、その経路を計画する必要が生じる。下記非特許文献１には、確率機構を用いた経路計画について記載されている。非特許文献１に記載された方法では、１つのスタート位置（初期位置：初期姿勢）に対して１つのゴール（最終位置：最終姿勢）が設定され、この間に一つ以上の中間点（中間位置：中間姿勢）を経由する経路が設定される。このとき、中間点に基づいて経路を計画する際の関節空間などが探索空間であり、中間点は一度に一つのみ設定される。

なお、関節空間などの探索空間を用いた経路設定については、下記非特許文献２に記載がある。非特許文献２には、空間内でのロボットの望ましい経路を決定する方法として、空間的・時間的な経路を関節角の関数として記述する関節空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一つ以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。
Ｒａｎｄｏｍｉｚｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ： Ｓ．Ｍ．ＬａＶａｌｌｅ ａｎｄ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ． ： Ｐｒｏｃ ｏｆ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， １９９９ ロボティクス−機構・力学・制御−（第７章）、Ｊｏｈｎ Ｊ．Ｃｒａｉｇ 著、三浦宏文・下山勲 訳、共立出版株式会社、１０９６４１号公報

しかし、上記非特許文献１に記載された経路計画方法においては、中間姿勢をランダムネス（確率）を用いて確率的に作成するため、経路を移動しているロボットなどの姿勢については、まったく考慮されていない。このため、ロボットの姿勢もランダムとなり、たとえば液体が入ったグラスをロボットが移動させようとするにあたり、液体をこぼさないようにロボットの姿勢を制御することはできない。

ここで、たとえば関節の数に応じた自由度を持つ多関節ロボットの場合、液体をこぼすことなく最終位置まで移動させるロボットの姿勢と経路とを求めるには、拘束条件を満たしながらロボットの初期姿勢と最終姿勢とを接続する中間姿勢を作成する必要がある。

このような中間姿勢を作成すると、拘束条件を満たす中間姿勢と拘束条件を満たす中間姿勢とを接続する姿勢が、拘束条件からずれることがある。このずれが大きい場合、ずれを補正するように経路を修正する必要が生じるが、この補正のための計算量が多くなる問題がある。

そこで、本発明の課題は、初期位置から最終位置までの移動対象体を移動させるにあたり、その移動対象体の姿勢が拘束条件を満たすような経路を効率よく計画することができる経路計画装置を提供することにある。

上記課題を解決した本発明に係る経路計画装置は、移動対象体を初期位置から最終位置まで移動させるにあたり、初期位置と最終位置との間に中間点を確率的に生成させ、中間点を用いて初期位置から最終位置までの間の経路を計画する経路計画装置において、移動対象体の姿勢を決定する移動対象体の自由度に基づいて定義される可動空間の中に、移動対象体が存在する作業空間における移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、可動空間において確率的に生成された中間点を拘束部分空間に射影することにより、拘束中間点を生成する中間点生成手段と、拘束中間点を用いて、初期位置と最終位置とを接続する経路を生成する経路生成手段と、を備えるものである。

本発明に係る経路計画装置においては、可動空間に、移動対象体の姿勢を維持するための拘束条件を設定している。この拘束条件に基づいて、拘束中間点を生成することにより、移動対象体の姿勢を維持しながらも、効率よく経路を計画することができる。なお、本明細書中、ロボットの関節空間など動作の自由度によって決まる空間を可動空間ということとする。また、可動空間と探索空間は同義であるものとする。さらに、拘束条件を満たす中間点を確率的に生成することは困難であるので、本発明では、確率的に生成した中間点を拘束部分空間に射影して拘束中間点を生成している。このため、拘束条件を近似的に満たす経路を生成することができ、計算量をより少なくすることができる。

また、上記課題を解決した本発明に係る経路計画装置は、移動対象体を初期位置から最終位置まで移動させるにあたり、初期位置と最終位置との間に中間点を確率的に生成させ、中間点を用いて初期位置から最終位置までの間の経路を計画する経路計画装置において、移動対象体の姿勢を決定する移動対象体の自由度に基づいて定義される可動空間の中に、移動対象体が存在する作業空間における移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、可動空間の中に、拘束条件を満たす任意の位置とその近傍点とを満たす拘束楕円体を設定し、拘束楕円体を用いて確率分布を定義し、確率分布に基づいて、拘束条件を満たす拘束中間点を生成する中間点生成手段と、拘束中間点を用いて、初期位置と最終位置とを接続する経路を生成する経路生成手段と、を備えるものである。

本発明に係る経路計画装置においては、拘束楕円体を定義することによって中間点を生成している。このような拘束楕円体を生成することにより、経路を計算することができる。拘束楕円体を用いて定義した確率分布に基づいて、可動空間における中間点を生成することにより、拘束条件を近似的に満たす経路を生成することができるので、計算量をより少なくすることができる。

また、拘束条件に基づいて、生成した拘束中間点を補正する経路補正手段をさらに備え、拘束条件設定手段は、拘束条件として、可動空間において拘束条件を満たす拘束部分空間を定義し、経路補正手段は、経路生成手段で生成された経路を拘束部分空間に射影することにより、拘束中間点を補正する態様とすることができる。

このように、経路生成手段で生成された経路を拘束部分空間に射影することで、経路を容易に補正することができる。

さらに、経路補正手段は、経路生成手段で生成された経路と拘束曲面との距離が所定値以上の場合に、拘束曲面への射影を行って、拘束中間点を補正する態様とすることもできる。このように、経路と拘束曲面との距離が所定値以上の場合に拘束中間点を補正することにより、計算量の削減に寄与することができる。

また、経路補正手段は、経路生成手段で生成された経路における所定間隔ごとに、射影を行う態様とすることもできる。このように、所定間隔ごとに射影を行うことにより、演算量の削減に寄与することができる。ここでの所定間隔としては、可動空間における距離を基準とするのが望ましい。

さらに、経路は、移動対象体の所定部位が、所定の初期姿勢にある初期位置から、所定の最終姿勢となる最終位置まで移動する経路であり、拘束条件設定手段は、可動空間の変数を作業空間へ写像したベクトルと、作業空間の基底ベクトルとの内積が定める方程式に基づいて、拘束条件を設定する態様とすることができる。

このように、直交空間を張るベクトル（作業空間の基底ベクトル）と、姿勢を定義したベクトル（可動空間の変数を作業空間へ写像したベクトル）との内積を利用することにより、好適に経路を計算することができる。

本発明に係る経路計画装置によれば、初期位置から最終位置までの移動対象体を移動させるにあたり、その移動対象体の姿勢が拘束条件を満たすような経路を効率よく計画することができる。

以下、図面を参照して本発明の実施形態について説明する。なお、各実施形態において、同一の機能を有する部分については同一の符号を付し、重複する説明は省略することがある。まず、本発明の第一の実施形態について説明する。図１は、第一の実施形態に係る経路計画装置のブロック構成図である。

本実施形態に係る経路計画装置は、図１に示すように、経路生成手段１を備えている。経路生成手段１は、拘束経路作成手段１０およびデータベース２０を有しており、経路生成手段１には、初期姿勢・最終姿勢入力手段２および画像入力手段３が接続されている。また、経路生成手段１には、経路出力手段４が接続されている。また。経路生成手段１は、具体的にはコンピュータによって構成されている。経路出力手段４には、図２に模式的に示す本発明の移動対象体であるロボットアーム（自動機械、以下「アーム」という）５が接続されており、アーム５の経路出力手段からアーム５に対して、動作経路および姿勢を出力する。

初期姿勢・最終姿勢入力手段２は、アーム５の現状位置および現状姿勢を初期姿勢として設定し、初期姿勢の情報および予め指定された目標姿勢の情報を拘束経路作成手段１０に出力する。また、画像入力手段３は、アーム５の周辺情報、特に障害物を外部環境として取得する手段であり、画像情報取得装置として、カメラなどを備えている、画像入力手段３は、取得した画像情報を加工し、データベース２０に出力している。この画像入力手段３としては、カメラのほか、各種センサなどの情報取得デバイスやキーボード、光ディスクドライブなどを用いることもできる。

経路出力手段４は、経路生成手段１によって生成された経路に沿ってアーム５を動作させる。この経路出力手段４としては、アーム５に接続されてアーム５に動作経路および姿勢を出力するもののほか、モニタやプリンタ、記憶可能な媒体を扱うドライブに接続されて動作経路および姿勢を表示し、印刷し、または記憶させるものなどを用いることもできる。

経路生成手段１おける拘束経路作成手段１０には、本発明の中間点生成手段および拘束条件設定手段である拘束中間姿勢作成部１１、拘束経路作成部１２、および本発明の経路補正手段である姿勢補間部１３が設けられている。また、データベース２０は、環境データ２１および体格データ２２が記憶されている。環境データ２１には、アーム５が載置されている環境に関するデータが記憶されている。体格データ２２には、アーム５のリンク３１〜３７間の長さや太さ、各リンク３１〜３７の可動範囲などの諸元が記憶されている。

拘束中間姿勢作成部１１は、アーム５の姿勢に関する拘束条件を満足するような初期姿勢と最終姿勢との間を接続する中間姿勢を複数作成する。なお、本明細書において、「姿勢」と示す場合には、その姿勢を取る位置をも含む意味とする。したがって、初期姿勢、最終姿勢は、本発明の初期位置および最終位置における姿勢を意味し、中間姿勢は、中間位置における姿勢を意味する。

拘束中間姿勢作成部１１は、作成した中間姿勢を拘束経路作成部１２に出力する。拘束経路作成部１２では、出力された複数の中間姿勢を利用し、これらの中間姿勢を接続することによって、初期姿勢・最終姿勢入力手段２から出力された初期姿勢と最終姿勢とを繋ぐ経路を作成する。

拘束経路作成部１２は、作成した経路を姿勢補間部１３に出力する。姿勢補間部１３では、拘束経路作成部１２から出力された経路において、中間姿勢と中間姿勢との間における姿勢を補間するとき、補間姿勢が拘束条件を近似的に満たすように補間姿勢を作成する。

こうして、拘束経路作成手段１０で作成された経路および姿勢は、拘束経路作成手段１０から経路出力手段４に出力される。

データベース２０は、具体的にはハードディスクやＲＯＭなどによって構成されており、知識としての環境データ２１（すでに知識として保持している情報）および画像入力手段３によって入力されたデータとを記憶している。データベース２０において新たに入力された環境情報は、その後は知識としての環境データ２１として蓄積される。また、体格データ２２には、アーム５のリンクやモータなどの各部材の情報が記憶されている。

アーム５は、図２に示すように、第一リンク３１〜第七リンク３７の７つのリンク（関節）を備えており、アーム５は７自由度である。各リンク３１〜３７には、それぞれアクチュエータ（モータ）が内蔵されており、その各リンクの角度を調整可能とされている。

このアーム５における各リンク３１〜３７に対して、アーム５が存在する作業空間の中にリンク座標系をそれぞれ設定する。リンク座標系の設定方法としては、各リンク３１〜３７の回転軸方向にＺ軸をとり、次に適当なＸ軸を決め、右手系をなすようにＹ軸を決める。図３に、上記のようにして決めたリンク座標系を示す。図３に示すリンク座標系では、添え字の数字で関節番号を示している。第一リンク３１〜第七リンク３７は、ベース３８上に設けられ、第七リンク３７の先端部には、ハンド３９が取り付けられている。また、リンク長をＬ_ｎで示している。

各リンク座標系間の変換は、次のようにして行うことができる。いま、第１リンク３１のリンク座標系をアーム５の台座座標系に関連付ける場合、その変換は^１Ｔ_７で与えられる。また、第七リンク３７に取り付けられたハンド３９の位置姿勢を表す座標系が第七リンク３７の座標系に対して^７Ｔ_Ｅで表される。さらに、アーム５の台座座標系が、基準座標系に対して^ＲＴ_０で表されるものとする。この場合、ハンド３９は、基準座標系に対して下記（１）式で表される位置姿勢を有する。

^ＲＴ_Ｅ＝^ＲＴ_１ ^１Ｔ_７ ^７Ｔ_Ｅ ・・・（１）
この^ＲＴ_Ｅによって、ハンド３９の位置ベクトルｒが決まり、^１Ｔ_７が関節変換ベクトルの関数であることから、リンクパラメータの値を与えることにより、７自由度アームの関節変換ベクトルとベース座標系からみた手先位置のベクトルｒとの関係式ｒ＝ｆ（ｑ）を求めることができる。

次に、本実施形態に係る経路計画方法について説明する。本実施形態に係る経路計画方法では、所定の拘束条件下、具体的に、所定の姿勢を維持した状態でのアーム５の制御について説明する。ここでは、所定の姿勢の維持として、アーム５の先端に取り付けられたハンド３９によってコップを把持して所定の載置面にまで運んで載置する際、コップに入った液体をこぼさないようにしてコップを移動させる場合について説明する。

以下、経路計画方法について説明する。図５は、本実施形態に係る経路計画の手順を示すフローチャート、図６は、図５に続く手順を示すフローチャート、図７は、図６の途中過程を示すフローチャートである。

図５に示すように、本実施形態に係る経路計画においては、最初に初期姿勢・最終姿勢入力手段２から初期姿勢および最終姿勢が入力される（Ｓ１）。アーム５の初期姿勢は、各リンク３１〜３７がなす角度によって知ることができ、アーム５の最終姿勢は、アーム５が把持するコップを載置面に載置する状態によって知ることができる。これらの初期姿勢および最終姿勢は、アーム５の各リンク３１〜３７におけるアクチュエータの角度と、データベース２０を参照して得られる体格データ２２で示されるアーム５の諸元に基づいて求められる。またアーム５の初期姿勢および最終姿勢は、作業空間上にオペレータが適宜設定して入力される。これらの位置が予めプリセットされている際には、この入力は必要ではない。なお、初期姿勢は現状姿勢で定義し、最終姿勢はオペレータあるいは指示者の簡単な指示に基づいて自動設定されるようにしてもよい。

初期姿勢および最終姿勢が入力されたら、環境認識を行う（Ｓ２）。環境認識においては、まず、画像入力手段３によってデータベース２０に外部環境データが入力される。データベース２０は、予め記憶している環境データ２１と、画像入力手段から入力された外部環境データとを、拘束経路作成手段１０に出力する。環境データ２１には、予めアーム５の周囲に存在する障害物の情報が含まれており、外部環境データには、画像入力手段３のカメラに撮像された障害物のデータが含まれている。拘束経路作成手段１０では、これらの環境データ２１および外部環境データを入力することによって、環境認識を行う。作業空間において決定されたアームの初期姿勢および最終姿勢は、本発明の可動空間である関節角度空間上に変換され、関節角度空間上の点としてそれぞれ表される。

こうして環境認識を行い、初期姿勢および最終姿勢を関節空間の点としてそれぞれ表した後、アーム５の姿勢を決定するアーム５の自由度に基づいて定義される可動空間、本実施形態では関節角度空間上に、アーム５の中間姿勢をランダムに作成する（Ｓ３）。図８に示すように、関節角度空間Ｑには、ステップＳ１で入力された初期姿勢および最終姿勢を示す所定のベクトルを有する初期姿勢ｑ_Ｓおよび最終姿勢ｑ_Ｇが示されている。これらの初期姿勢ｑ_Ｓおよび最終姿勢ｑ_Ｇの間で、確率機構を用いた公知の手法（上記［非特許文献１］などを参照）によって、ベクトルを有する中間姿勢ｑ（ｑ_１，ｑ_２，…）をランダムに生成する。

中間姿勢を作成したら、中間姿勢を本発明の拘束条件として定義される拘束曲面Ｖに射影する（Ｓ４）。ここで、拘束曲面Ｖについて説明する。

上述したように、アーム５は、作業空間内における基準座標系に対して、^ＲＴ_Ｅで表される位置姿勢にある。^ＲＴ_Ｅは、具体的には下記（２）式で表される。

ここで、^ＲΘ_Ｅは、アーム５の姿勢を定義する回転行列であり、^Ｒｐ_Ｅは基準座標系から見たアームの手首位置ベクトルである。^ＲΘ_Ｅは、正規直交基底{ｅ_１，ｅ_２，ｅ_３}によって、^ＲΘ_Ｅ＝［ｅ_１ｅ_２ｅ_３］と表すことができる。

いま、図４に示すハンド３９のハンド姿勢を定義するベクトルを、鉛直方向に沿ったベクトルであるベクトルｅ３とし、このベクトルｅ３に直交するとともに、互いに直交するベクトルをそれぞれベクトルｅ１，ｅ２とする。また、基準座標系の正規直交基底を{^Ｒｅ_ｘ，^Ｒｅ_ｙ，^Ｒｅ_ｚ}とすると、コップに入った液体をこぼさないハンド拘束条件は、下記（３）式および（４）式によって与えられる。

ｈ_１（ｑ）≡＜^Ｒｅ_ｘ，ｅ３＞＝０ ・・・（３）
ｈ_２（ｑ）≡＜^Ｒｅ_ｙ，ｅ３＞＝０ ・・・（４）
ここで、＜，＞はユークリッド内積を表している。

いま、関節角度空間をＱにおける拘束曲面（拘束部分空間）Ｖは、下記（５）式で与えられる。なお、上述のとおりアーム５は７次元自由度を有しているのに対して、拘束曲面Ｖは５次元空間である。

Ｖ＝｛ｑ∈Ｑ⊂Ｒ^７｜ｈ_１（ｑ）＝０，ｈ_２（ｑ）＝０｝ ・・・（５）
さらに、ベクトル値関数ｈ（ｑ）を、下記（６）式のように定義する。

ｈ（ｑ）＝（ｈ_１（ｑ），ｈ_２（ｑ））^Ｔ ・・・（６）
（６）式において、Ｔは転置を表している。

ここで、関節角度空間Ｑにおいて中間姿勢ｑはランダムに作成されるため、中間姿勢ｑによって定められるアーム５の姿勢もランダムとなり、コップに入った液体がこぼれる姿勢である可能性がある。これに対して、中間姿勢ｑが拘束曲面Ｖ上にある場合、中間姿勢ｑによって定められるアーム５の姿勢は、アーム５のハンド３９に把持されるコップの表面が水平となる姿勢となる。したがって、中間姿勢を拘束曲面Ｖに射影した射影中間姿勢を用いて経路を計画することにより、ハンド３９に把持されたコップに入った液体をこぼすことなく、コップを移動させる経路を計画することができる。

そこで、中間姿勢ｑを生成したら、中間姿勢ｑを、上記（５）式で表される拘束部分空間を示す拘束曲面Ｖ上に射影する（Ｓ４）。ただし、中間姿勢がある基準に基づいて拘束曲面Ｖの十分近くにある場合には、射影を行う必要がないので、射影は行わない。射影中間姿勢ｑ_ｋを拘束曲面Ｖに射影するためには、ニュートン法を利用する。ここでは、中間姿勢ｑと拘束曲面Ｖとの関係が所定基準を満たす場合にニュートン法を実行し、所定基準を満たさない場合には再度中間姿勢を生成する。本実施形態では、下記（Ａ）式で表されるｄ（ｑ）が予め設定されたしきい値よりも小さい場合に、所定条件を満たすと判断する。

ｄ（ｑ）＝{ｈ^Ｔ（ｑ）ｈ（ｑ）}^１／２ ・・・（Ａ）
所定の射影中間姿勢をｑ_ｋ∈Ｒ^７とし、ｑ_ｋでのｈ（ｑ）のグラフの接空間は、下記（８）式で表される。

ｐ−ｈ（ｑ_ｋ）＝（∂ｈ／∂ｑ）（ｑ_ｋ）・（ｑ−ｑ_ｋ） ・・・（８）
ここで、記号「・」は、行列の積を表す。この（８）式を変形して、(９)式を得ることができる。

ｑ＝ｑ_ｋ−（∂ｈ／∂ｑ）^†（ｑ_ｋ）・ｈ（ｑ_ｋ） ・・・（９）
ここで、行列（∂ｈ／∂ｑ）^†（ｑ_ｋ）は、行列（∂ｈ／∂ｑ）（ｑ_ｋ）の一般逆行列を表す。具体的には、下記（１０）式で表される。

（∂ｈ／∂ｑ）^†（ｑ_ｋ）＝（∂ｈ／∂ｑ）^Ｔ（ｑ_ｋ）［∂ｈ／∂ｑ（ｑ_ｋ）（∂ｈ／∂ｑ）^Ｔ（ｑ_ｋ）］^−１ ・・・（１０）
この射影中間姿勢ｑ_ｋを更新姿勢（点）と考え、射影中間姿勢ｑ_ｋに対する次の射影中間姿勢ｑ_ｋ＋１を下記（１１）式により更新する。

ｑ_ｋ＋１＝ｑ_ｋ−（∂ｈ／∂ｑ）^†（ｑ_ｋ）・ｈ（ｑ_ｋ） ・・・（１１）
この（１１）式がニュートン法における更新公式となる。このようにして、中間姿勢ｑ_１，ｑ_２，…を拘束曲面Ｖに射影し、射影中間姿勢ｑ_ｋを得ることができる。

拘束条件を近似的に満たすか否かは、上記（Ａ）式によって表されるｄ（ｑ）が予め設定されたしきい値よりも大きいか否かによって判定する。なお、ここでのしきい値は、ニュートン法を実行するか否かの判断を行う場合に用いられるしきい値よりも十分に小さい値に設定されている。

次に、拘束曲面Ｖに中間姿勢を射影したら、中間姿勢と環境物体とが干渉するか否かを判断する（Ｓ５）。中間姿勢と環境物体とが干渉するか否かの判断を行う際には、関節角度空間Ｑに作成された中間姿勢を、それぞれ作業空間に変換する。作業空間において、中間姿勢と障害物情報とが比較され、中間姿勢が障害物と接触する場合には、中間姿勢と環境物体とが干渉すると判断する。

この判断の結果、中間姿勢が環境物体と干渉すると判断した場合には、ステップＳ３に戻って、関節角度空間に中間姿勢をランダムに生成する。一方、中間姿勢が環境物体と干渉しないと判断した場合には、ランダムに作成された射影中間姿勢ｑ_１，ｑ_２，…における任意の射影中間姿勢ｑ_ｋに着目し、関節角度空間Ｑにおいて、射影中間姿勢ｑ_ｋと最も近い最終姿勢ｑ_Ｇ側の射影中間姿勢ｑ_ｋ＋１（中間姿勢が１つしか作成されていない状態では、最終姿勢ｑ_Ｇ）とを直線で結び、結んだ直線上に直線上射影中間姿勢ｑ_ｌを作成する（Ｓ６）。ここでの直線上射影中間姿勢は一つのみ作成してもよいし、複数作成してもよいが、ここでは１つのみの直線上射影中間姿勢ｑ_ｌを作成する。

直線上射影中間姿勢ｑ_ｌを作成したら、図６に示すように、作成された直線上射影中間姿勢ｑ_ｋｌが拘束条件を満たすか否かを判断する（Ｓ７）。拘束条件を満たすか否かの判断は、直線上射影中間姿勢が拘束曲面Ｖ上にあるか否かによって行われる。その結果、拘束条件を満たさないと判断した場合には、新たに補間姿勢を作成するが、補間姿勢の作成については後述する。

一方、拘束条件を満たすと判断した場合には、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の姿勢が環境物体と干渉するか否かを判断する（Ｓ８）。射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の姿勢が環境物体と干渉するか否かの判断は、次の干渉判定によって行われる。

干渉判定では、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１とを結ぶ線分をさらに微小な区間に分割し、その微小区間の境界部毎に干渉判定を行う。この微小区間での判定は、次の射影中間姿勢ｑ_ｋ＋１側から射影中間姿勢ｑ_ｋ側に向けて順次行われる。なお、ここでの「所定長さ」や「微小区間」は、間接角度空間の距離に基づいている。

このようにすることにより、演算負荷を軽減することができる。実際の干渉判定では、関節角度空間Ｑにおける上述した微小区間の境界部の位置座標が実際の作業空間に変換され、予め入力されている環境物体（障害物）の情報と比較され、作業空間上で干渉の有無が判定される。

その結果、干渉があると判断された場合には、枝の生成を中止し（Ｓ９）、ステップＳ３に戻り、改めて関節角度空間に中間姿勢を作成する。一方、干渉がないと判断された場合には、枝を生成する（Ｓ１０）。ここでの枝の作成とは、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の直線を経路として確定することを意味する。

枝を作成したら、続いてステップＳ３で任意に着目された射影中間姿勢ｑ_ｋと最終姿勢ｑ_Ｇとが枝で接続されたか否かを判断する（Ｓ１１）。その結果、中間姿勢と最終姿勢とが枝で接続されていないと判断した場合には、ステップＳ８に戻り、次の射影中間姿勢ｑ_ｋ＋１について射影中間姿勢ｑ_ｋと同様の処理を行い、枝をさらに生成する。また、射影中間姿勢ｑ_ｋと最終姿勢ｑ_Ｇとが枝で接続されたと判断した場合には、射影中間姿勢ｑ_ｋと最終姿勢側ｑ_Ｇとの間の処理を、射影中間姿勢ｑ_ｋと初期姿勢ｑ_Ｓ側との間で行い、射影中間姿勢ｑ_ｋと初期姿勢ｑ_Ｓとを枝で結ぶ処理を行う（Ｓ１２）。

こうして、初期姿勢と最終姿勢とが枝で結ばれたら、その枝からなる経路を出力する（Ｓ１３）。こうして、経路計画が終了する。

続いて、ステップＳ７で拘束条件を満たさないと判断し、補間姿勢を作成する場合について説明する。射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の直線が拘束条件を満たしていない場合、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間で、アーム５の姿勢が崩れ、コップに入った水をこぼす可能性があることになる。このように、コップに入った水をこぼす事態を防止するため、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間に補間姿勢を作成する。

補間姿勢の作成にあたっては、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間における直線上射影中間姿勢を拘束曲面Ｖに射影し、図９に示すように、この射影された姿勢を補間姿勢ｑ_ｒとして作成する（Ｓ１５）。この補間姿勢ｑ_ｒを作成することにより、コップに入った水がこぼれるのを防止することができる。

補間姿勢ｑ_ｒは、次のようにして作成される。いま、拘束すべきすべての中間姿勢を拘束曲面Ｖに射影した後、図１０に示すように、点ｑ_ｋ＋１と点ｑ_ｋとを直線で結び、線分ｑ_ｋ＋１ｑ_ｋを作成する。この線分ｑ_ｋ＋１ｑ_ｋを予め与えられた単位長さに基づいて分割し、分割点ｑ_ｋｌを生成する。この分割点ｑ_ｋｌにおいて、拘束曲面Ｖとの近さを上記（Ａ）式で表されるｄ（ｑ）が予め設定されたしきい値よりも大きいか否かによって評価する。その結果、ある分割点ｑ_ｋｌが上記（Ａ）式による評価の結果所定のしきい値を超える場合には、射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の直線が拘束条件を満たしていないと判断し、この分割点ｑ_ｋｌをニュートン法によって拘束曲面Ｖに十分近い位置に射影し、補間姿勢ｑ_ｒを生成する。

補間姿勢ｑ_ｒを作成したら、射影中間姿勢ｑ_ｋと補間姿勢ｑ_ｒとの間の姿勢が環境物体と干渉するか否かを判断する（Ｓ１６）。射影中間姿勢ｑ_ｋと補間姿勢ｑ_ｒとの間の姿勢が環境物体と干渉するか否かの判断は、ステップＳ８における射影中間姿勢ｑ_ｋと次の射影中間姿勢ｑ_ｋ＋１との間の姿勢が環境物体と干渉するか否かの判断と同様に行われる。

その結果、射影中間姿勢ｑ_ｋと補間姿勢ｑ_ｒとの間の姿勢が環境物体と干渉すると判断した場合には、枝の生成を中止し（Ｓ１７）、ステップＳ３に戻る。一方、射影中間姿勢ｑ_ｋと補間姿勢ｑ_ｒとの間の姿勢が環境物体と干渉しないと判断した場合には、射影中間姿勢ｑ_ｋと補間姿勢ｑ_ｒとの間、および補間姿勢ｑ_ｒと次の射影中間姿勢ｑ_ｋ＋１との間に、それぞれ枝を生成する（Ｓ１８）。

それから、続いてステップＳ３で任意に着目された射影中間姿勢ｑ_ｋと最終姿勢ｑ_Ｇとが枝で接続されたか否かを判断する（Ｓ１９）。その結果、枝で接続されていないと判断した場合には、ステップＳ１６に戻り、次の射影中間姿勢ｑ_ｋ＋１について射影中間姿勢
ｑ_ｋと同様の処理を行い、枝をさらに生成する。また、射影中間姿勢ｑ_ｋと最終姿勢ｑ_Ｇとが枝で接続されたと判断した場合には、射影中間姿勢ｑ_ｋと最終姿勢側ｑ_Ｇとの間の処理を、射影中間姿勢ｑ_ｋと初期姿勢ｑ_Ｓ側との間で行い、射影中間姿勢ｑ_ｋと初期姿勢ｑ_Ｓとを枝で結ぶ処理を行う（Ｓ２０）。

こうして、初期姿勢と最終姿勢とが枝で結ばれたら、その枝からなる経路を出力する（Ｓ１３）。こうして、経路計画が終了する。

このように、関節角度空間Ｑに形成した拘束曲面Ｖ上に中間姿勢を順次作成してアーム５の経路を作成することにより、アーム５が水の入ったコップを把持して移動させる際、コップに入った水をこぼすことなく、コップを移動させることができる。

次に、本発明の第二の実施形態について説明する。本実施形態では、上記第一の実施形態と比較して、中間点の生成手順が主に異なっている。図１１は、本実施形態に係る経路計画装置のブロック構成図である。ここでは、中間点の生成手順を中心に説明する。

図１１に示すように、本実施形態に係る経路計画装置は、上記第一の実施形態と比較して、経路作成手段３０の構成が主に異なっており、他の構成は同様とされている。経路作成手段３０は、上記第一の実施形態と同様のデータベース２０を備えており、その他、拘束楕円体算出部３１および動作経路生成部３２を有している。また、動作経路生成部３２は、主成分空間算出部３３、乱数生成部３４、および経路作成部３５を備えている。本実施形態では、拘束楕円体算出部３１および動作経路生成部３３が本発明の中間点生成手段となる。

拘束楕円体算出部３１は、初期姿勢・最終姿勢入力手段２から出力された初期姿勢と最終姿勢とを繋ぐ経路を作成するために利用する拘束楕円体を算出する。拘束楕円体を算出するにあたっては、関節角度空間の変数と作業空間の変数の関数で表示された拘束条件の接空間によって関節角度空間に拘束すべき自由度変化量（または自由度変化率の拘束量）を楕円体として算出する。この楕円体の軸長に基づいて大きい自由度変数の主成分が抽出される。また、拘束楕円体算出部３１は、算出した拘束楕円体の体積によって拘束度を定義する。

拘束楕円体算出部３１は、算出した拘束楕円体を動作経路生成部３２に出力する。動作経路生成部３２では、拘束楕円体算出部３１から出力された拘束楕円体およびデータベース２１から出力された環境データおよびロボットの体格データを用いて、ロボットの動作経路を作成する。動作経路生成部３２は、作成した動作経路を経路出力手段４に出力する。

動作経路生成部３２の機能をさらに説明すると、動作経路生成部３２における主成分空間算出部３３は、拘束楕円体算出部３１から出力された拘束楕円体を定義する半正定値対称行列の主成分分析を行い、拘束楕円体の主成分を分析する。乱数生成部３４は、主成分空間算出部３３で分析された主成分によって確率分布を決定し、この確率分布に基づいて、動作経路を生成するための候補となる乱数を生成する。この乱数は、ロボットの姿勢を示すものである。

乱数生成部３４は、生成した乱数を経路作成部３５に出力する。経路作成部３５では、データベース２１から出力される環境データと、乱数生成部で生成された乱数とが干渉するか否かを判定し、干渉しないと判定された乱数を複数接続することによって、一連の動作経路を生成する。

次に、本実施形態に係る経路計画方法について説明する。経路計画を行うにあたり、上記第一の実施形態で説明したロボットを用いた場合を例に説明する。本実施形態では、中間点を作成するにあたり、拘束条件として、拘束楕円体を定義する点が、上記第一の実施形態と主に異なる。以下、拘束楕円体を定義する手順について説明する。拘束楕円体の定義は、拘束楕円体算出部３１で行われる。また、本実施形態では、生成する拘束中間点を自動機械の関節角度空間の変数として関節変数ベクトルｑとして求め、関節変数ベクトルｑについて、下記（１９）式を用いて表す。さらに、関節変数ベクトルｑを求めるにあたり、基準となる点を基準関節変数ベクトルｑ_０として表す。すなわち、拘束条件を満たすｑ_０を中心として楕円体を作成し、これを用いて拘束条件を十分に満たす点ｑ_１を作成する。この操作を繰り返すことによって拘束条件を満たす姿勢を生成していく。なお、下記（１９）式において、ｎは自由度の総数である。ここで、Ｃは関節角度空間を表す。

ｑ∈Ｃ⊆Ｒ^ｎ ・・・（１９）
いま、図３に示すアームの各リンクでの座標系において、３つのベクトルの組（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）で決まる座標系をΣ_ｉと表記する。ここで、Σ_Ｅはハンド位置姿勢座標系を表し、Σ_Ｒは台座座標系を表す。また、ハンド３９の位置姿勢は、上記第一の実施形態で示した（１）式で表される。この（１）式で求められる^ＲＴ_Ｅによって、ハンド３９の手首位置ベクトルｐが下記（２０）式で表される。

ｐ＝（ｘ，ｙ，ｚ）^Ｔ ・・・（２０）
ここで、^１Ｔ_７が関節変数ベクトルｑの関数であるので、リンクパラメータの値が与えられれば、７自由度アームの関節変数ベクトルｑとベース座標系から見た手首位置のベクトルｐとの関係式が下記（２１）式として決まる。

ｐ＝ｆ（ｑ，Ｌ） ・・・（２１）
ここでＬ＝（Ｌ_１，Ｌ_２，Ｌ_３，Ｌ_４，Ｌ_５，Ｌ_６，Ｌ_７）^Ｔである。

続いて、（ｘ，ｙ，ｚ）^Ｔで示される作業空間内を動く７自由度のロボットについて説明する。

関節角度空間における関節変数ベクトルｑは、下記（２２）式で表される。

ｑ＝（ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６，ｑ_７）^Ｔ ・・・（２２）
このとき、ｘ，ｙ，ｚは、それぞれ下記（２３−１）式〜（２３−３）式で表すことができる。

ｘ＝ｆ_１（ｑ，Ｌ） ・・・（２３−１）
ｙ＝ｆ_２（ｑ，Ｌ） ・・・（２３−２）
ｚ＝ｆ_３（ｑ，Ｌ） ・・・（２３−３）
ここで、作業空間においては、下記（２４−１）式〜（２４−ｍ）式で表される手首位置ベクトルｐの拘束条件があるとする。

このとき、上記（２３）式（（２３−１）式〜（２３−３）式）を（２４）式（２４−１）式〜（２４−ｍ）式）に代入して、関節変数ベクトルｑの拘束条件を得る。この式を略記すると、下記（２５）式と表すことができる。

ｈ（ｑ）＝０ ・・・（２５）
ただし、ｈ（ｑ）＝（ｇ_１（ｐ（ｑ）），・・・，ｇ_ｍ（ｐ（ｑ）））^Ｔである。

いま、各リンク３１〜３７のリンク長は定数としているので、（２５）式は、関節変数ベクトルｑの式と考えることができる。この（２５）式を満たす関節変数ベクトルｑの自由度は関節角度空間の中で、高々、７−ｍ次元（ｍ≦６）である。

このとき、微分可能な基準関節変数ベクトルｑ_０において微分可能な（２５）式のグラフの接空間は、下記（２６）式で表すことができる。

Ｊ・ｄｑ＝０ ・・・（２６）
ここで、ｄｑ＝ｑ−ｑ_０であり、Ｊは関数ｈのヤコビ行列である。

続いて、下記（２７）式に示す２次形式を考える。

ｄｑ^Ｔ・Ｕ^Ｔ・（Ｊ^ＴＪ）・Ｕ・ｄｑ＝０ ・・・（２７）
行列Ｊ^ＴＪを対角化する直列行列をＵとすると、（２７）式は、下記（２８）式のように表現することができる。

また、上記（２７）式から、下記（２９）式を得ることができる。

さらに、下記（３０）式は３×７の行列であるとすると、上記（２９）式により、下記（３１）式を得ることができる。この（３１）式は、（３２）式に書き換えることができる。

上記（３２）式における左辺のｑ^Ｔとｑとで挟まれている部分が半正定値対称行列となっている。このため、（３２）式は、楕円体を表す式となる。上記（３２）式で表される楕円体が、本発明の拘束楕円体となる。このような拘束楕円体を用いることにより、経路計算を行う。

こうして、拘束楕円体算出部３１において拘束楕円体を設定したら、動作経路生成部３２における主成分空間算出部３３において、この拘束楕円体の主成分空間を定義する。（３２）式で示される拘束楕円体の主成分空間は、この拘束楕円体を定義する半正定値対称行列の対応する部分空間となる。この部分空間を主成分空間として定義する。

拘束楕円体の主成分空間を定義したら、乱数発生部３４において、拘束楕円体の主成分空間上に確率分布を定義する。この確率分布は、たとえば一般化ガウス分布で生成して定義することができる。具体的には、自己共分散行列Σ′を下記（３３）式で定義する。なお、自己共分散行列Σ′は、正則でない（逆行列を持たない）行列である。

この自己共分散行列Σ′および基準関節変数ベクトルｑ_０に基づいて乱数を発生させる。ここで発生された乱数のうち任意の１つを、関節変数ベクトルｑの候補として生成する。関節変数ベクトルｑの候補は、基準関節変数ベクトルｑ_０を平均値とした自己共分散行列Σ′の一般化ガウス分布を用いて作成する。

こうして、主成分空間算出部３３において関節変数ベクトルｑの候補を算出したら、経路作成部３５は、基準関節変数ベクトルｑ_０と関節変数ベクトルｑとを結ぶ線分を生成する。この線分をデータベース２１から出力される環境データを参照して、線分が障害物と衝突するか否かを判定する。

その結果、障害物と干渉しないと判定した場合には、生成した関節変数ベクトルを中間姿勢と決定する。続いて、この関節変数ベクトルｑを基準関節変数ベクトルｑ_０とし、同様にして中間姿勢を決定していく。一方、線分が障害物と干渉すると判定した場合には、乱数生成部３４で異なる乱数を生成させ、同様の処理を繰り返す。このようにして、一連の動作経路が生成される。

なお、本実施形態では、乱数を生成するにあたって、一般化ガウス分布を用いたが、他の確率分布を用いて乱数を生成する態様とすることもできる。また、乱数を生成するにあたり、１つの乱数のみを生成するようにしたが、たとえば複数の乱数を同時に生成し、その中の１つ、たとえば拘束条件に近い乱数を用いる態様とすることもできる。

以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。たとえば、上記実施形態では、中間姿勢を拘束曲面に射影する際、拘束曲面上にない中間姿勢を拘束曲面に射影しているが、拘束曲面から所定範囲内にある中間姿勢については、射影を行わず、そのまま経路として利用する態様とすることができる。また、補間姿勢の作成は必須ではなく、補間姿勢を作成することなく、経路を作成することもできる。

本発明に係る経路計画装置のブロック構成図である。 アームの自由度の構成を示す説明図である。 アームの各リンクでの座標系の関係を示す説明図である。 ハンドの座標系を示す図である。 経路計画方法の手順を示すフローチャートである。 図５に続く手順を示すフローチャートである。 図６に続く手順を示すフローチャートである。 関節角度空間のイメージを示す図である。 関節角度空間での補間姿勢を示す図である。 関節角度空間での補間姿勢を作成する手順を説明するための図である。 第二の実施形態に係る経路計画装置のブロック構成図である。

符号の説明

１…経路生成手段、２…初期姿勢・最終姿勢入力手段、３…画像入力手段、４…経路出力手段、５…アーム、１０…拘束経路作成手段、１１…拘束中間姿勢作成部、１２…拘束経路作成部、１３…姿勢補間部、２０…データベース、２１…環境データ、２２…体格データ、３１〜３７…リンク、３８…ベース、３９…ハンド、ｑ…中間姿勢、ｑ_ｋ…射影中間姿勢、ｑ_ｋ＋１…次の射影中間姿勢、ｑ_ｋｌ…直線上射影中間姿勢、ｑ_ｒ…補間姿勢、Ｑ…関節角度空間、Ｖ…拘束曲面（拘束部分空間）。

Claims (7)

1. 移動対象体を初期位置から最終位置まで移動させるにあたり、前記初期位置と前記最終位置との間に中間点を確率的に生成させ、前記中間点を用いて前記初期位置から前記最終位置までの間の経路を計画する経路計画装置において、
   前記移動対象体の姿勢を決定する前記移動対象体の自由度に基づいて定義される可動空間の中に、前記移動対象体が存在する作業空間における前記移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、
   前記可動空間において確率的に生成された中間点を前記拘束部分空間に射影することにより、拘束中間点を生成する中間点生成手段と、
   前記拘束中間点を用いて、前記初期位置と前記最終位置とを接続する経路を生成する経路生成手段と、
   を備えることを特徴とする経路計画装置。
2. 移動対象体を初期位置から最終位置まで移動させるにあたり、前記初期位置と前記最終位置との間に中間点を確率的に生成させ、前記中間点を用いて前記初期位置から前記最終位置までの間の経路を計画する経路計画装置において、
   前記移動対象体の姿勢を決定する前記移動対象体の自由度に基づいて定義される可動空間の中に、前記移動対象体が存在する作業空間における前記移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、
   前記可動空間の中に、前記拘束条件を満たす任意の位置とその近傍点とを満たす拘束楕円体を設定し、前記拘束楕円体を用いて確率分布を定義し、前記確率分布に基づいて、前記拘束条件を満たす拘束中間点を生成する中間点生成手段と、
   前記拘束中間点を用いて、前記初期位置と前記最終位置とを接続する経路を生成する経路生成手段と、
   を備えることを特徴とする経路計画装置。
3. 前記拘束条件に基づいて、生成した前記拘束中間点を補正する経路補正手段をさらに備える請求項１または請求項２に記載の経路計画装置。
4. 前記拘束条件設定手段は、前記拘束条件として、前記可動空間において前記拘束条件を満たす拘束部分空間を定義し、
   前記経路補正手段は、前記経路生成手段で生成された経路を前記拘束部分空間に射影することにより、前記拘束中間点を補正する請求項３に記載に記載の経路計画装置。
5. 前記経路補正手段は、前記経路生成手段で生成された経路と拘束曲面との距離が所定値以上の場合に、前記拘束曲面への射影を行って、前記拘束中間点を補正する請求項４に記載の経路計画装置。
6. 前記経路補正手段は、前記経路生成手段で生成された経路における所定間隔ごとに、前記射影を行う請求項４または請求項５に記載の経路計画装置。
7. 前記拘束条件設定手段は、前記可動空間の変数を前記作業空間へ写像したベクトルと、前記作業空間の基底ベクトルとの内積が定める方程式に基づいて、前記拘束条件を設定する請求項１〜請求項６のうちのいずれか１項に記載の経路計画装置。

Images