WO2006121091A1

WO2006121091A1 - 経路計画装置

Info

Publication number: WO2006121091A1
Application number: PCT/JP2006/309436
Authority: WO
Inventors: Shintaro Yoshizawa
Original assignee: Toyota Jidosha Kabushiki Kaisha
Priority date: 2005-05-13
Filing date: 2006-05-01
Publication date: 2006-11-16
Also published as: EP1862876B1; US20090019410A1; DE602006012943D1; EP1862876A1; US8055383B2; EP1862876A4

Description

糸田 »

経路計画装置

技術分野

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

背景技術

ロボットを移動させる際や、口ポットアームを自,動操作する際には、その経路を計画する必要が生じる。「R a n d om i z e d K i n o d y n am i c P 1 a n n i n g ： S . M. L a V a 1 1 e a n d J. J. K u f f n e r , J r. ： P r o c o f I EEE I n t. C o n f . o n R o b o t i c s a n d Au t oma t i o n, 1 999」（以下「非特許文献 1」という）には、確率機構を用いた経路計画について記載されている。非特許文献 1に記載された方法では、 1つのスタート位置（初期位置：初期姿勢）に対して 1つのゴール（最終位置：最終姿勢）が設定され、この間に一つ以上の中間点（中間位置：中間姿勢）を経由する経路が設定される。このとき、中間点に基づいて経路を計画する際の関節空間などが探索空間であり、中間点は一度に一つのみ設定される。

なお、関節空間などの探索空間を用いた経路設定については、「口ポテイクス一機構 '力学'制御一（第 7章)、. J o hn J. C r a i g 著、三浦宏文'下山勲訳、共立出版株式会社、 109641号公報」（以下「非特許文献 2」という）に記載がある。非特許文献 2には、空間内でのロボットの望ましい経路を決定する方法として、空間的 ·時間的な経路を関節角の関数として記述する関節空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一つ以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。

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

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

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

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

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

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

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

本発明に係る経路計画装置においては、拘束楕円体を定義することによって中間点を生成している。このような拘束楕円体を生成することにより、経路を計算することができる。拘束楕円体を用いて定義した確率分布に基づいて、可動空間における中間点を生成することにより、拘束条件を近似的に満たす経路を生成することができるので、計算量をより少なくすることができる。また、 '拘束条件に基づいて、生成した経路を補正する経路補正手段をさらに備える態様とすることができる。このように、生成した経路を拘束条件に基づいて補正することにより移動対象体の姿勢をより確実に維持した状態での経路を生成することができる。

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

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

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

さらに、拘束条件設定手段は、可動空間の変数を作業空間へ写像したベクトルと、作業空間の基底ベクトルとの内積が定める方程式に基づいて、拘束条件を設定する態様とすることができる。

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

本発明のさらなる応用範囲は、以下の詳細な発明か.ら明らかになるだろう。しかしながら、詳細な説明および特定の事例は本発明の好適な実施形態を示すものではあるが、例示のためにのみ示されているものであって、本発明の思想および範囲における様々な変形および改良はこの詳細な説明から当業者には自明であることは明らかである。

図面の簡単な説明

図 1は、本発明に係る経路計画装置のプロック構成図である。

図 2は、アームの自由度の構成を示す説明図である。

図 3は、アームの各リンクでの座標系の関係を示す説明図である。

図 4は、ハンドの座標系を示す図である。

図 5は、経路計画方法の手順を示すフローチャートである。

図 6は、図 5に続く手順を示すフローチャートである。

図 7は、図 6に続く手順を示すフローチャートである。

図 8は、関節角度空間のイメージを示す図である。

図 9は、関節角度空間での補間姿勢を示す図である。

図 1 0は、関節角度空間での補間姿勢を作成する手順を説明するための図である。

図 1 1は、第二の実施形態に係る経路計画装置のプロック構成図である。発明を実施するための最良の形態

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

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

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

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

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

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

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

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

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

データベース ₂ 0は、具体的にはハードディスクや R OMなどによって構成されており、知識としての環境データ 2 1 (すでに知識として保持している情報）および画像入力手段 3によって入力されたデータとを記憶している。データべ一ス 2 0において新たに入力された環境情報は、その後は知識としての環境データ 2 1として蓄積される。また、体格データ 2 2には、アーム 5のリンクやモータなどの各部材の情報が記憶されている。

アーム 5は、図 2に示すように、第一リンク 3 1〜第七リンク 3. 7の 7つのリンク（関節）を備えており、アーム 5は 7自由度である。各リンク 3 1〜 3 7には、それぞれァクチユエータ（モータ）が内蔵されており、その各リンクの角度を調整可能とされている。 '

このアーム 5における各リンク 3 1〜3 7に対して、 'アーム 5が存在する作業空間の中にリンク座標系をそれぞれ設定する。リンク座標系の設定方法としては、各リンク 3 1〜3 7の回転軸方向に Z軸をとり、次に適当な X軸を決め、右手系をなすように Y軸を決める。図 3に、上記のようにして決めたリンク座標系を示す。図 3に示すリンク座標系では、添え字の数字で関節番号を示している。第一リンク 3 1〜第七リンク 37は、ベース 38上に設けられ、第七リンク 37の先端部には、ハンド 39が取り付けられている。また、リンク長を L_nで示している。

各リンク座標系間の変換は、次のようにして行うことができる。いま、第 1リンク 3 1のリンク座標系をアーム 5の^座座標系に関連付ける場合、その変換はェ丁？で与えられる。また、第七リンク 37に取り付けられたハンド 39の位置姿勢を表す座標系が第七リンク 37の座標系に対して⁷ T_Eで表される。さらに、ァーム 5の台座座標系が、基準座標系に対して ^KT。で表されるものとする。この場合、ハンド 39は、基準座標系に対して下記（1) 式で表される位置姿勢を有する。

^RT_E = ^RT₁ ¹T₇ ⁷T_E · · ■ (1)

この ^RT_Eによって、ハンド 39の位置べクトノレ rが決まり、丄丁ァが関節変換べクトルの関数であることから、リンクパラメータの値を与えることにより、 7自由度アームの関節変換べクトルとベース座標系からみた手先位置のベタトル rとの関係式 r = f (q) を求めることができる。

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

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

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

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

こうして環境'認識を行い、初期姿勢および最終姿勢を関節空間の点としてそれぞれ表した後、アーム 5の姿勢を決定するアーム 5の自由度に基づいて定義される可動空間、本実施形態では関節角度空間上に、アーム 5の中間姿勢をランダムに作成する（S 3 )。図 8に示すように、関節角度空間 Qには、ステップ S 1で入力された初期姿勢および最終姿勢を示す所定のベタトルを有する初期姿勢 q _sおよび最終姿勢 (！^が示されている。これらの初期姿勢 q _sおよび最終姿勢 ci c;の間で、確率機構を用いた公知の手法（上記 [非特許文献 1 ]などを参照）によって、ベクトルを有する中間姿勢 q (q ₁； q₂， ···）をランダムに生成する。

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

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

ここで、 ^ΚΘ_Εは、アーム 5の姿勢を定義する回転行列であり、 ^R;p_Eは基準座標系から見たアームの手首位置べクトルである。 ^ΚΘ_Εは、正規直交基底 {eい e ₂， e ₃}によって、 ^ΚΘ_Ε= [e！ e ₂ e ₃] と表すことができる。

いま、図 4に示すハンド 3 9のハンド姿勢を定義するべクトルを、鉛直方向に沿ったべクトルであるべクトル e 3とし、このべクトル e 3に直交するとともに、互いに直交するべクトルをそれぞれべクトル e 1， e 2とする。また、基準座標系の正規直交基底を {^Re _x， ^Re_y, ^Re _z}とすると、コップに入った液体をこぼさないハンド拘束条件は、下記（3) 式および（4) 式によって与えられる。 h ₁ (q) ≡<^R e _x， e 3〉= 0 · ■ · (3)

h₂ (q) ≡<^Re _y, e 3 >=0 ■ · ■ (4)

ここで、く，〉はユークリッド内積を表している。

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

V= { q eQCR⁷ I h_x (q) = 0， h₂ (q) =0 } ■ - · (5) さらに、ベクトル値関数 h (q) を、下記（6) 式のように定義する。

h (q) = ( , )， h₂ )) ^T · ■ ■ (6)

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

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

d (q) ={h^T (q) h (q) }^{1 z} · · ■ (A)

所定の射影中間姿勢を q_kER⁷とし、 q_kでの h (q) のグラフの接空間は、下記（8) 式で表される。

p-h (q_k) = (3 h/d q) (q_k) - (q-q_k) · ■ - (8)

ここで、記号「 ·」は、行列の積を表す。この（8) 式を変形して、（9)式を得ることができる。

q-q_k- (3 h/d q) ^† (q_k) ■ h (q_k) ■ · - (9)

ここで、行列（3 h/3 q) ^† (q_k) は、行列（3 hZ3 q) (q_k) の一般逆行列を表す。具体的には、下記（10) 式で表される。

(3 h/3 q) ^† (q_k) = (3 h/d q) ^T (q_k) [3 h/3 q (q_k) (d h /3 q) ^T (q_k)] - ¹ ■ ■ · (10)

この射影中間姿勢 q _kを更新姿勢（点）と考え、射影中間姿勢 q_kに対する次の射影中間姿勢 q_{k + 1}を下記（1 1) 式により更新する。

q_k+1 = q_k一 (3 h/a q) ^† (q_k) · h (q_k) · · - (1 1)

この（1 1) 式がニュートン法における更新公式となる。このようにして、中間姿勢 _{q i}， q₂， …を拘束曲面 Vに射影し、射影中間姿勢 q_kを得ることができる。

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

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

この判断の結果、中間姿勢が環境物体と干渉すると判断した場合には、ステツプ S 3に戻って、関節角度空間に中間姿勢をランダムに生成する。一方、中間姿勢が環境物体と干渉しないと判断した場合には、ランダムに作成された射影中間姿勢 q_2> …における任意の射影中間姿勢 q_kに着目し、関節角度空間 Qにおいて、射影中間姿勢 q_kと最も近い最終姿勢 ic側の射影中間姿勢 q_{k + 1} (中間姿勢が 1つしか作成されていない状態では、最終姿勢 q_G) とを直線で結び、結んだ直線上に直線上射影中間姿勢 c を作成する（S 6)。ここでの直線上射影中間姿勢は一つのみ作成してもよいし、複数作成してもよいが、ここでは 1つのみの直線上射影中間姿勢 q _Lを作成する。

直線上射影中間姿勢を作成したら、図 6に示すように、作成された直線上射影中間姿勢 q_{k l}が拘束条件を満たすか否かを判断する（S 7)。拘束条件を満たすか否かの判断は、直線上射影中間姿勢が拘束曲面 V上にあるか否かによって行われる。その結果、拘束条件を満たさないと判断した場合には、新たに補間姿勢を作成するが、補間姿勢の作成については後述する。

一方、拘束条件を満たすと判断した場合には、射影中間姿勢 q_kと次の射影中間姿勢 q_{k + 1}との間の姿勢が環境物体と干渉するか否かを判断する（S 8)。射影中間姿勢 q _kと次の射影中間姿勢 q _{k +1}との間の姿勢が環境物体と干渉するか否かの判断は、次の干渉判定によって行われる。

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

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

その結果、干渉があると判断された場合には、枝の生成を中止し（S 9)、ステップ S 3に戻り、改めて関節角度空間に中間姿勢を作成する。一方、干渉がないと判断された場合には、枝を生成する（S 10)。ここでの枝の作成とは、射影中間姿勢 q _kと次の射影中間姿勢 (！ _{k +1}との間の直線を経路として確定することを意味する。

枝を作成したら、続いてステップ S 3で任意に着目された射影中間姿勢 q_kと最終姿勢 q_cとが枝で接続されたか否かを判断する（S l l)。その結果、中間姿勢と最終姿勢とが枝で接続されていないと判断した場合には、ステップ S 8に戻り、次の射影中間姿勢 q_{k + 1}について射影中間姿勢 q_kと同様の処理を行い、枝をさらに生成する。また、射影中間姿勢 q_kと最終姿勢とが枝で接続されたと判断した場合には、射影中間姿勢 q_kと最終姿勢側との間の処理を、射影中間姿勢 q _kと初期姿勢 q _s側との間で行い、射影中間姿勢 q _kと初期姿勢 q _sとを枝で結ぶ処理を行う（S 12)。

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

続いて、ステップ S 7で拘束条件を満たさないと判断し、補間姿勢を作成する場合について説明する。射影中間姿勢 q _kと次の射影中間姿勢 q _{k +} iとの間の直線が拘束条件を満たしていない場合、射影中間姿勢 q_kと次の射影中間姿勢 q_{k + 1} との間で、アーム 5の姿勢が崩れ、コップに入った水をこぼす可能性があることになる。このように、コップに入った水をこぼす事態を防止するため、射影中間姿勢 q _kと次の射影中間姿勢 q _{k +} iとの間に補間姿勢を作成する。

補間姿勢の作成にあたっては、射影中間姿勢 q _kと次の射影中間姿勢 q _k + iとの間における直線上射影中間姿勢を拘束曲面 Vに射影し、図 9に示すように、この射影された姿勢を補間姿勢 q_rとして作成する（S 1 5)。この補間姿勢 q_rを作成することにより、コップに入った水がこぼれるのを防止することができる。補間姿勢は、次のようにして作成される。いま、拘束すべきすべての中間姿勢を拘束曲面 Vに射影した後、図 10に示すように、点 q_{k + 1}と点 (l_kとを直線で結び、線分 (l_{k + 1} q_kを作成する。この線分 q_{k + 1} q_kを予め与えられた単位長さに基づいて分割し、分割点 q_{k l}を生成する。この分割点 q_{k l}において、拘束曲面 Vとの近さを上記（A) 式で表される d (q) が予め設定されたしきい値よりも大きいか否かによって評価する。その結果、ある分割点 q_{k l}が上記（A) 式による評価の結果所定のしきい値を超える場合には、射影中間姿勢 q_kと次の射影中間姿勢 q_{k + 1}との間の直線が拘束条件を満たしていないと判断し、この分割点 q_{k l}をニュートン法によって拘束曲面 Vに十分近い位置に射影し、補間姿勢 q_r を生成する。補間姿勢 q_rを作成したら、射影中間姿勢 q _kと補間姿勢 q _rとの間の姿勢が環境物体と千渉するか否かを判断する（S 16)。射影中間姿勢 q_kと補間姿勢との間の姿勢が環境物体と干渉するか否かの判断は、ステップ S 8における射影中間姿勢 c と次の射影中間姿勢 q_{k + 1}との間の姿勢が環境物体と干渉するか否かの判断と同様に行われる。

その結果、射影中間姿勢 q_kと補間姿勢との間の姿勢が環境物体と干渉すると判断した場合には、枝の生成を中止し（S 17)、ステップ S 3に戻る。一方、射影中間姿勢 q _kと補間姿勢との間の姿勢が環境物体と干渉しないと判断した場合には、射影中間姿勢 q_kと補間姿勢 q_rとの間、および補間姿勢 q_rと次の射影中間姿勢 q_{k + 1}との間に、それぞれ枝を生成する（S 18)。

それから、続いてステップ S 3で任意に着目された射影中間姿勢 q_kと最終姿勢 q_Qとが枝で接続されだか否かを判断する（S 19)。その結果、枝で接続されていないと判断した場合には、ステップ S 16に戻り、次の射影中間姿勢 q_{k + 1} について射影中間姿勢

q_kと同様の処理を行い、枝をさらに生成する。また、射影中間姿勢 q_kと最終姿勢 q _cとが枝で接続されたと判断した場合には、射影中間姿勢 q _kと最終姿勢側 q • _eとの間の処理を、射影中間姿勢 q_kと初期姿勢 (1₅側との間で行い、射影中間姿勢 q_kと初期姿勢 q_sとを枝で結ぶ処理を行う（S 20)。

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

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

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

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

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

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

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

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

q eC£Rⁿ · · · 9)

. いま、図 3に示すアームの各リンクでの座標系において、 3つのベクトルの組

(_{X i}， y _{i ;} z _;) で決まる座標系を∑iと表記する。ここで、 ∑_Eはハンド位置姿勢座標系を表し、 ∑_Rは台座座標系を表す。また、ハンド 39の位置姿勢は、上記第一の実施形態で示した（1) 式で表される。この（1) 式で求められる ^R T_Eによって、ハンド 39の手首位置ベクトル pが下記（20) 式で表される。

p= (x, y, z) ^τ ' · · (20)

ここで、ェ丁ァが関節変数ベクトル qの関数であるので、リンクパラメータの値が与えられれば、 7自由度アームの関節変数べクトル qとベース座標系から見た手首位置のベタトル pとの関係式が下記（2 1) 式として決まる。

P= f (q, L) ■ · · (21)

ここで L= (Lい L₂， L₃， L₄， L₅， L₆， L₇) ^Tである。

続いて、（x， y， z) ^Tで示される作業空間内を動く 7自由度のロボットについて説明する。

関節角度空間における関節変数ベクトル qは、下記（22) 式で表される。 q = ( q 1 , q₂， q₃， q₄， ¾ s> 1₆. a ₇) * - - ' (22)

このとき、 x， y, zは、それぞれ下記（23— 1) 式〜（23— 3)·式で表すことができる。

x= f ！ (q, L) - - - (23- 1)

y = f ₂ (q, L) · · · (23— 2)

z = f 3 (q, L) - ■ ■ (23 - 3)

ここで、作業空間においては、下記（24— 1) 式〜（24—m) 式で表される手首位置べクトル pの拘束条件があるとする。

■(24-1)

•(24-2)

•(24-3)

■(24-w) このとき、上記（23) 式（（23- 1) 式〜（23— 3) 式）を（24) 式（2 4- 1) 式〜（24—m) 式）に代入して、関節変数ベクトル qの拘束条件を得る。この式を略記すると、下記（25) 式と表すことができる。

h (q) =0 · ■ ■ (25)

ただし、 h (q) = (_{g l} (p (q))， · ■ · , g_m (p ))) ^Tである。いま、各リンク 3 1〜37のリンク長は定数としているので、（25) 式は、関節変数ベクトル qの式と考えることができる。この（25) 式を満たす関節変数ベタトル qの自由度は関節角度空間の中で、高々、 7— m次元（m^ 6)である。このとき、微分可能な基準関節変数ベクトル q。において微分可能な（25) 式のグラフの接空間は、下記（26) 式で表すことができる。

J - d q = 0 - - - (26)

ここで、 d q = q— q。であり、 Jは関数 hのヤコビ行列である。

続いて、下記 (27) 式に示す 2次形式を考える。

d q^T■ U^T · (J^T J) · U■ d q-0 · · ■ (27)

行列 J^T Jを対角化する直列行列を Uとすると、（27) 式は、下記（28) 式のように表現することができる。

で、〉 = diag(a_l,a₂,a₃,---,a,. )>0 "C3¾)¾_c また、上記（27) 式から、下記（29) 式を得ることができる ₍

で、

'ひ" ひ ₁₂

U二

—ひ 21 ひ 22」

ひ _uはん X 行列、ひ ₁₂は x(7—ん) ί亍列

さらに、下記（30) 式は 3 X 7の行列であるとすると、上記（29) 式により、下記（31) 式を得ることができる。この（31) 式は、（32) 式に書き換えることができる。

=(U_uU_n) . -(30)

。 g^TU^T∑U_q = _qo ^TU^T∑Uq₀ -(31)

q^TU^T∑ /q₀ ^TU^T∑Uq₀)uq = l ...(32) 上記（32) 式における左辺の q^Tと qとで挟まれている部分が半正定値対称行列となっている。このため、（3 2 )式は、楕円体を表す式となる。上記（3 2 ) 式で表される楕円体が、本発明の拘束楕円体となる。このような拘束楕円体を用いることにより、経路計算を行う。

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

拘束楕円体の主成分空間を定義したら、乱数発生部 3 4において、拘束楕円体の主成分空間上に確率分布を定義する。この確率分布は、たとえば一般化ガウス分布で生成して定義することができる。具体的には、自己共分散行列∑' を下記

( 3 3 ) 式で定義する。なお、自己共分散行列は、正則でない（逆行列を持たない）行列である。

∑'≡^∑ひ/ ∑ひ -(33) この自己共分散行列∑' および基準関節変数べクトル q。に基づいて乱数を発生させる。ここで発生された乱数のうち任意の 1つを、関節変数ベクトル qの候補として生成する。関節変数ベクトル qの候補は、基準関節変数ベクトル q。を平均値とした自己共分散行列∑' の一般化ガウス分布を用いて作成する。

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

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

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

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

産業上の利用可能性

本発明は、本発明は、自動機械の全体または一部分などの動作経路を計画する経路計画装置に利用することができる。

Claims

請求の範圏

1 . 移動対象体を初期位置から最終位置まで移動させるにあたり、前記初期位置と前記最終位置との間に中間点を確率的に生成させ、前記中間点を用いて前記初期位置から前記最終位置までの間の経路を計画する経路計画装置において、前記移動対象体の姿勢を決定する前記移動対象体の自由度に基づいて定義される可動空間の中に、前記移動対象体が存在する作業空間における前記移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、

前記可動空間において確率的に生成された中間点を前記拘束部分空間に射影することにより、拘束中間点を生成する中間点生成手段と、

前記拘束中間点を用いて、前記初期位置と前記最終位置とを接続する経路を生成する経路生成手段と、

を備えることを特徴とする経路計画装置。

2 . 移動対象体を初期位置から最終位置まで移動させるにあたり、前記初期位置と前記最終位置との間に中間点を確率的に生成させ、前記中間点.を用いて前記初期位置から前記最終位置までの間の経路を計画する経路計画装置において、前記移動対象体の姿勢を決定する前記移動対象体の自由度に基づいて定義される可動空間の中に、前記移動対象体が存在する作業空間における前記移動対象体のとりうる姿勢を制約するための拘束条件を満たす拘束部分空間を設定する拘束条件設定手段と、

前記可動空間の中に、前記拘束条件を満たす任意の位置とその近傍点とを満たす拘束楕円体を設定し、前記拘束楕円体を用いて確率分布を定義し、前記確率分布に基づいて、前記拘束条件を満たす拘束中間点を生成する中間点生成手段と、前記拘束中間点を用いて、前記初期位置と前記最終位置とを接続する経路を生成する経路生成手段と、

を備えることを特徴とする経路計画装置。

3 . 前記拘束条件に基づいて、生成した経路を補正する経路補正手段をさらに備える請求の範囲第 1項に記載の経路計画装置。

4 . 前記拘束条件設定手段は、前記拘束条件として、前記可動空間において前記拘束条件を満たす拘束部分空間を定義し、

前記経路補正手段は、前記経路生成手段で生成された経路を前記拘束部分空間に射影することにより、前記経路を補正する請求の範囲第 3項に記載に記載の経路計画装置。 '

5 . 前記経路補正手段は、前記経路生成手段で生成された経路と拘束曲面との距離が所定値以上の場合に、前記拘束曲面への射影を行って、前記経路を補正する請求の範囲第 4項に記載の経路計画装置。

6 . 前記経路補正手段は、前記経路生成手段で生成された経路における所定間隔ごとに、前記射影を行う請求の範囲第 4項に記載の経路計画装置。

7 . 前記経路補正手段は、前記経路生成手段で生成された経路における所定間隔ごとに、前記射影を行う'請求の範囲第 5項に記載の経路計画装置。

8 . 前記拘束条件設定手段は、前記可動空間の変数を前記作業空間へ写像したベクトノレと、前記作業空間の基底べクトルとの内積が定める方程式に基づいて、前記拘束条件を設定する請求の範囲第 1項に記載の経路計画装置。'