WO2009017242A2

WO2009017242A2 - ロボットの動作経路生成装置

Info

Publication number: WO2009017242A2
Application number: PCT/JP2008/063934
Authority: WO
Inventors: Shintaro Yoshizawa; Yutaka Hirano
Original assignee: Toyota Jidosha Kabushiki Kaisha
Priority date: 2007-07-30
Filing date: 2008-07-29
Publication date: 2009-02-05
Also published as: JP2009032189A; US20100204828A1

Abstract

pct-art17.2a

Description

明糸田書

ロボットの動作経路生成装置

技術分野

本発明は、力学的拘束を伴う関節ロボットの動作経路を生成するロボットの動作経路生成装置に関する。

背景技術

近年、各種産業用ロボットゃ人型ロボットなどの様々なロボットが開発されている。例えば、ロボットには、多数の関節を有し、関節間がリンクで連結され、各関節の動作によって多数の自由度を持つロボットがある。このようなロボットを動作させる場合、力学的な拘束条件があり、この拘束条件を満たすような動作経路を生成する必要がある。特許文献 (日本国特開 2 0 0 4— 3 0 6 2 3 1号公報）に記載のロボットの運動制御装置では、脚式口ポットに課されるタスクや運動状態に応じて課される拘束条件を現在状態からの変化量に関する等式及び不等式で与えるとともに、冗長自由度の駆動ストラテジをエネルギ関数で規定する。これによつて、拘束条件の変化に関しては、拘束条件毎に特化した制御系を構成する必要がなく、行列及ぴベクトルの変更のみで対応することができるので、多様かつ動的な拘束条件を扱い易い。また、冗長自由度の利用方法についても、行列及びべクトルの変更のみで対応できる。なお、日本国特開 2 0 0 6— 4 8 3 7 2号公報には、ロボット動作の経路を計画する方法について開示されている。発明の開示

ロボットの評価条件に対する最適化問題には、 1次関数を扱う線形計画問題と 1次関数以外（2次関数、 3次関数、 · · ·、任意の非線形関数）を扱う非線形計画問題がある。しかし、特許文献 1に記載の運動制御装置は、評価関数が 2次関数である最適化問題を扱つており、 2次計画問題の範囲で表現できる場合にしか適用できない。したがって、 2次関数以外のより複雑な関数を評価関数とするロボットについての動作経路を生成することができない。そこで、本発明は、拘束条件を満たしかつ様々な評価条件の最適化を図ることが可能な関節ロボットの動作経路を生成することができるロボットの動作経路生成装置を提供することを課題とする。

本発明に係るロボットの動作経路生成装置は、力学的拘束を伴う関節ロボットの動作経路を生成する口ポットの動作経路生成装置であって、ロボットの動作を拘束する拘束条件を取得する拘束条件取得手段と、ロポットの動作を評価する評価条件を取得する評価条件取得手段と、拘束条件取得手段で取得した拘束条件を満たすロボットの姿勢を複数生成する姿勢生成手段と、評価条件取得手段で取得した評価条件に基づいて姿勢生成手段で生成した複数の姿勢をそれぞれ評価する姿勢評価手段と、姿勢生成手段で生成した複数の姿勢の中から姿勢評価手段での評価結果に基づいて姿勢を選択する姿勢選択手段と、姿勢選択手段で選択した姿勢を用いてロボットの動作経路を生成する動作経路生成手段とを備えることを特徴とする。

このロボットの動作経路生成装置では、拘束条件取得手段により口ポットの拘束条件を取得するとともに、評価条件取得手段によりロボットの評価条件を取得する。拘束条件は、ロボットを動作させる上で動きを拘束する力学的な条件であり、例えば、ロボットの関節の角度についての拘束条件、関節の角度の速度や加速度についての拘束条件がある。評価条件は、ロボットを動作させる際の評価条件であり、例えば、ロボットの関節で発生するトルクについての評価条件、関節のァクチユエータで消費する電気工ネルギについての評価条件、ロボットの姿勢と障害物との干渉についての評価条件がある。評価条件としては、様々な条件を適用可能であり、例えば、評価条件として評価関数が用いられた場合、線形関数、様々な非線形関数が適用である。そして、動作経路生成装置では、姿勢生成手段により、拘束条件を満たすようなロボットの姿勢を複数個生成する。ここで、口ボッ.トの時系列での次の姿勢になる候補が複数個生成され、全ての候補が拘束条件を満たしている。ロボットの姿勢を複数個生成する毎に、動作経路生成装置では、姿勢評価手段により、評価条件に基づいて複数の姿勢についてそれぞれ評価する。さらに、動作経路生成装置では、姿勢選択手段により、その複数の姿勢についての各評価結果に基づいて複数の姿勢の中から評価の高い姿勢を選択する。ここで、ロボットの時系列での次の姿勢になる複数の候補の中から、評価の高い姿勢が選択される。そして、動作経路生成装置では、動作経路生成手段により、選択した姿勢を用いてロボットの動作経路を生成してゆく。これによつて、動作経路生成装置では、拘束条件を満たしつつ評価条件を考慮した動作経路を自動的に生成でき、各種非線形関数などを適用したあらゆる評価条件についての最適化を図ることができる。したがって、 1次計画問題、 2次計画問題のみならず、より複雑な計画問題について適用することができる。

本発明の上記ロボットの動作経路生成装置では、姿勢生成手段は、ロボットの各関節の角度をランダムに生成してロボットの姿勢を生成し、当該生成したロボットの姿勢における各関節の角度の変化に基づいて拘束条件を満たすか否かを判定する構成としてもよい。

この動作経路生成装置の姿勢生成手段では、ロボットの各関節の角度をランダムに生成し、各関節のランダムな角度からなるロボットの姿勢を複数生成する。生成した姿勢毎に、姿勢生成手段では、その生成した姿勢における各関節の角度の前回の姿勢における各関節の角度からの変化に基づいて拘束条件を満たすか否力を判定する。この拘束条件を満たす姿勢だけが姿勢評価手段で評価される。これによって、関節の個数に関係'なく拘束条件を満たす姿勢の候補を簡単かつ効率良く生成することができる。

本発明の上記ロボットの動作経路生成装置では、姿勢生成手段は、ロボットの前回の姿勢に対する各関節の角度の変化をスカラ倍することによってロボットの姿勢を生成する構成としてもよい。

この動作経路生成装置の姿勢生成手段では、口ポットの各関節の角度からなる姿勢を生成すると、その生成した姿勢における各関節の角度と前回の姿勢における各関節の角度との変化をスカラ倍することにより、ロボットの姿勢を生成する。これによつて、拘束条件を満たす姿勢の探索効率を高めることができる。

本発明の上記ロボットの動作経路生成装置では、評価条件は、ロボットの姿勢の各関節の角度を変数とする評価関数を用いた条件であり、姿勢評価手段は、姿勢生成手段で生成した姿勢の各関節の角度を評価関数に入力し、当該評価関数の出力値に基づいて姿勢を評価する構成としてもよい。

このロボットの動作経路生成装置では、評価条件としてロボットの姿勢の各関節角度を変数とする評価関数を用いる。この評価関数としては、線形関数である 1次関数及び非線形関数である 2次以上の n次関数や任意の非線形関数がある。姿勢生成手段で生成した姿勢毎に、姿勢評価手段では、生成した姿勢の各関節角度を評価関数に入力し、評価関数の出力値に基づいて姿勢を評価する。これによつて、複数の姿勢について評価関数によって簡単に評価でき、複数の姿勢の中から評価関数を考慮して効率良く姿勢を選択することができる。

本発明の上記ロボットの動作経路生成装置では、評価条件は、複数個の条件がある構成としてもよい。このように評価条件を複数設定することにより、様々な評価条件（例えば、ァクチユエータに負荷の少ない、消費電力が少ない、動作範囲が狭い、障害物と干渉しない）を複数考慮した動作経路を生成することができる。

本発明の上記ロボットの動作経路生成装置では、評価条件は、ロボットの姿勢が障害物と干渉しない姿勢であるという条件を含む構成としてもよい。このようにロボットの姿勢が障害物と干渉しないことを評価条件とすることにより、動作中にロボットが障害物とぶつからなレ、動作経路を生成することができる。

図面の簡単な説明

図 1は、本実施の形態に係る動作経路生成装置の構成図である。

図 2は、本実施の形態で適用される口ポットの一例である。

図 3は、 '本実施の形態で適用されるロボットの他の例である。図 4は、 2つの関節と重カバランサを持つロボットの一例である。

図 5は、図 1の姿勢生成部で生成する関節べクトルの候補の一例である。

図 6は、本実施の形態に係る動作経路生成装置での動作の流れを示すフローチヤートである。

発明を実施するための最良の形態

以下、図面を参照して、本発明に係るロボットの動作経路生成装置の実施の形態を説明する。

本実施の形態では、本発明に係る口ポットの動作経路生成装置を、多自由度リンク系のロボットの動作経路を作成する動作経路生成装置に適用する。本実施の形態に係る動作経路生成装置は、力学的な（運動学的な）拘束条件を満たしつつ評価条件についての最適化を図ったロボットのスタートの位置姿勢からゴールの位置姿勢まで動作するための動作経路を生成する。本実施の形態では、評価条件が複数あり、評価条件の 1つがロボットの姿勢が障害物と干渉しないことであり、他の評価条件がロボットの各関節の角度（関節ベクトル）を変数とする評価関数が適用される。

図 1〜図 5を参照して、本実施の形態に係る動作経路生成装置 1について説明する。図 1は、本実施の形態に係る動作経路生成装置の構成図である。図 2は、本実施の形態で適用されるロボットの一例である。図 3は、本実施の形態で適用されるロボットの他の例である。図 4は、 2つの関節と重カバランサを持つロボットの一例である。図 5は、図 1の姿勢生成部で生成する関節べクトルの候補の一例である。

動作経路生成装置 1は、時系列で連続する一定時間毎のロボットの姿勢（各関節の角度からなる関節ベクトルによって決まる姿勢）を順次求め、この時系列で連続する姿勢を接続していくことによつて動作経路を自動的に作成する。特に、動作経路生成装置 1は、様々な力学的拘束条件や評価関数を適用可能とするために、力学的拘束条件を満たす口ポットの姿勢の候補を複数個生成し、この姿勢の候捕の中から評価関数での評価が高くかつ障害物と干渉しない姿勢を 1つ選択する。

そのために、動作経路生成装置 1は、データベース 2、入力部 3、記憶部 4、姿勢生成部 5、姿勢評価部 6、角度接続部 7、出力部 8を備えている。動作経路生成装置 1の主要部はコンピュータ上あるいはロボット内の電子制御ュニットなどに構成され、特に、姿勢生成部 5、姿勢評価部 6、角度接続部 7はハードディスクあるいは ROM内に格納された各アプリケーションプログラムを RAMに口ードし、 CPUで実行することによって構成される。

なお、本実施の形態では、入力部 3が特許請求の範囲に記载する拘束条件取得手段及び評価条件取得手段に相当し、姿勢生成部 5が特許請求の範囲に記載する姿勢生成手段に相当し、姿勢評価部 6が特許請求の範囲に記載する姿勢評価手段及び姿勢選択手段に相当し、角度接続部 7が特許請求の範囲に記載する動作経路生成手段に相当する。

まず、本実施の形態に適用される口ポットについて説明する。図 2にはロボットの一例を示している。ロボット R 1は、 n個の関節 J · · ·， J _nを備えており、関節間がリンク Lい · · ·， L_{n + 1}で接続されている。また、ロボット R 1は、末端のリンク 1^の一端が固定され、先端のリンク L_{n + 1}の一端にハンド Hが取り付けられている。各関節 Jい · · ·， J _nは、ァクチユエータが内蔵されており、回転動作をそれぞれ行い、接続される 2本のリンク間の角度 q い · · ■， q_nをそれぞれ変更する。

このように、ロボット R 1は、 n個の自由度を持つ。この自由度は、 n個の角度に対して座標軸を持つ n次元座標空間（関節空間、コンフィグレーション空間）における一点（qい · · ·， q _n) で表される。また、ロボット R 1の実際の位置姿勢は、 3次元空間（作業空間）におけるロボット R 1の先端部 T (リンク L_n+1とハンド Hとの取付部）の座標位置（Y l, Y 2, Y 3) とハンド H の姿勢で表される。 n個の関節角度 qい · · ·， q_nによって、 q= ( qい · · ·， q J ^Tを定義し、これを関節ベクトルと呼ぶ。関節ベクトル qは、時間の関数であり、時系列で一定時間毎の q ( 1 ) ， ' · · （ΐ (ΐ£— 1 ) ， (1 , (1ί + 1) , · · 'で表される。したがって、時間 tにおける関節ベクトル qは、 q (t) = (_{α ι} (t) , · · · , q_n (t) ) ^Tとなる。

また、図 3にはロボットの他の例を示している。口ポット R2は、人型のロボットであり、左右一対のアーム部 A 1， A2とハンド HI, H2を有している。ロボット R 2は、 10個の関節 Jい · · ·， J i。を備えており、 10個の自由度を持つ。ロボット R 2は、自由度が関節空間における座標系（ ^, · · · , q ₁₀) で表され、実際の位置姿勢が作業空間における各先端部 T 1， T2の座標位置（Y l l， Y 1 2, Υ 1 3) 、 (Υ 2 1 , Υ 22, Υ 23) と各ハンド Η 1, Η 2の姿勢で表される。

次に、ロボットの運動方程式について説明する。ロボットの運動方程式は、式 (1) で表される。式（1) において、左辺の第 1項が関節ベクトルの加速度の項であり、第 2項が関節ベクトルの速度の項であり、第 3項が重力の項であり、右辺が η個の関節にかかるトルクである。

式（1) において、 d ² qZd t ²が関節ベクトル qの 2階時間微分であり、 d qZd tが関節ベクトル qの 1階時間微分である。また、 H (q) がロボットに作用する慣性力を表す行列であり、 C (d q/d t, q) が口ポットに作用する遠心力とコリオリカを表す行列であり、 G (q) がロボットに作用する重力を表すべクトルである。

次に、拘束条件について説明する。拘束条件は、ロボットを動作させる上で口ボットの動きを拘束する力学的な（運動学的な）条件である。拘束条件の例を、式（2) 、式（3) 、式（4) で示す。式（2) は、ロボットの位置姿勢についての拘束条件である。式（3) は、ロボットの位置姿勢と速度についての拘束条件であり、例えば、一定速度で動作させるという条件がある。式（4) は、ロボットの位置姿勢と速度と加速度についての拘束条件であり、例えば、一定加速度で動作させるという条件がある。

m—(2)

R (3)

拘束条件は、上記の条件以外にも様々な条件が適用可能である。例えば、不等式を含む条件式でもよいし、 3階以上の時間微分を含む条件式でもよい。

例えば、時間 tから Δ tZ2前の時間 t一 Δ t Z 2における関節 iの角度 q i ( t - Δ t/2) と時間 tから Δ tノ 2後の時間 t + Δ t / 2における関節 iの角度 d i ( t + Δ t / 2) とすると、関節 iの角度 q iの 1階時間微分は、 A t が極短い時間であると、式（5) で示すように q； ( t - Δ t /2) と d i ( t +厶 t 2) によって近似できる。

そこで、関節 iの角度 q iの時間微分を、一般化し、式（6 ) のように定; る。式（6) において上付き文字（m) は m階時間微分であることを示し、一 1 は m_ 1階時間微分であることを示す。

関節 iの角度 q iの 1階時間微分を、関節 iの角度 q iの 0回時間微分で表すと、式（7) で示すようになる。ここで、式（7) の q _; ⁽⁰⁾は、関節の角度 q i の 0回時間微分なので、 q； (⁰) = q iとする。また、 q i (k) = q ₅ ( t一 Δ t / 2 , A t ) /Δ t q； ( k + 1 ) = q ； ( t + Δ t / 2 , Δ t ) ΖΔ tとおく。これによつて、時間 t =kでの関節 iの角度 q iの 1階時間微分は、式 (8) に示すように、時間 kの関節 iの角度 q i (k) とその次の時間 k+ 1の関節 iの角度 _{Q i} (k+ 1) の 2項間での差分式で表すことができる。

1) (t) = lim_Ai→。 (。)( △り… (7)

= ( +1) - W'"(8)

このように、任意の関節 iの角度 q iの m階時間微分を、角度の m+ 1項間の差分式で近似することができる。したがって、拘束条件も各関節 iの角度 q iの m+ 1項間の差分式で近似することができる。これによつて、拘束条件を、各関節 iの角度 q ₅の _m+ 1項間の変化に基づいて判定することができる。例えば、式（3) の場合、拘束条件式が関節ベクトル q (各関節 iの角度 d i) の 1 階時間微分を変数とするので、関節ベクトル q (k) 、 q (k+ 1) (各関節 i の角度 ci i (k) 、 q j (k+ 1) ) の 2項間の関係式で表すことができる。また、式（4) の場合、拘束条件式が関節ベクトル qの 2階時間微分を変数とするので、関節ベクトル q (k— 1) 、 q (k) 、 q (k + 1) (各関節 iの角度 q i (k一 1) 、 q； (k) 、 q ₅ (k+ 1) ) の 3項間の関係式で表すことができる。このように、本実施の形態では、時系列で連続する関節ベクトル qの差分式で近似して拘束条件を取り扱う。

次に、評価関数（評価指標）について説明する。評価関数は、ロボットを動作させる際の評価条件を表す関数である。評価条件としては、例えば、ロボットの関節で発生するトルクをできるだけ小さくするという条件、ァクチユエ一タで消費する電気工ネルギを少なくするという条件がある。評価関数としては、線形関数である 1次関数及び非線形関数である 2次以上の n次関数や任意の非線形関数が適用でき、あらゆる関数が適用可能である。

ロボットをスタートの位置姿勢からゴールの位置姿勢まで動作させる場合、口ボットの動きは無数に存在する。そこで、時系列で連続した P個の姿勢をそれぞれ定義する P個の関節ベクトル q (1) , q (2) , · · ·， q (p) まで決定 3934 されていたとすると、この p個の関節ベクトルに対して、評価関数の値を最小 (あるいは、最大）にするように（すなわち、評価が最も高くなるように）、 P + 1番目の関節ベクトル q (p + 1) を決定する。

本実施の形態では、 p + 1個の連続した関節ベクトル q ( k - p + 1) , · · · , q (k) , q (k + 1) の関数 F (q (k-p + 1) , · · ·， q (k) , q (k+ 1) ) を評価関数とする。そして、 p個の関節ベクトル q (k-p + 1) , · · · , q (k) を既知として、関節ベクトル q (k + 1) の複数の候補（拘束条件を満たす候補）の中から、評価関数の値を最小（あるいは、最大）とする関節ベクトルを選択し、 k+ 1番目の関節ベクトル q (k+ 1) を決定する。

以下に、評価関数について 2つの例を示す。 1つ目の例は、評価関数を各関節で発生するトルクの総和を求めるための関数とし、この評価関数の値を最小とする関節ベクトルを選択する。このように、各関節で発生するトルクの総和を最小とすることにより、各ァクチユエータにかかる負荷の総和を最小とすることができ、ァクチユエータにかかる負荷を抑制できる。その評価関数 Cを、式（9) に示す。

C =∑ … ）

i ^at J

式（9) において、 _{T i}は、関節 i (すなわち、ァクチユエータ）で発生するトルクである。ここで、説明を簡単にするために、図 4に示すような、 2つの関節 Jい J ₂と重カバランサ Gい G₂を持つロボットを考える。このようなロボットの場合、重カバランサ G_1; G₂があるので、遠心力とコリオリカが 0となり、重力が 0となり、慣性力が一定値となる。したがって、式（1) に示すロボットの運動方程式から、関節 iのトルクを式（1 0) で表すことができる。式（1 0) において、 Hは、時定数の慣性行列である。 3934

dt² '

したがって、この場合の評価関数 Cは、関節角度 q iを 3階時間微分した値の 2乗和で表すことができる。上記したように、関節角度 q iの 3階時間微分は、時系列で連続した 4項間 q _; (k- 2) 、 q _{ (k一 1) 、 q , (k) 、 q _; (k + 1) の差分式で近似することができる。したがって、評価関数 Cは、 2次元の関節ベクトル q (k— 2) 、 q (k一 1) 、 q (k) 、 q (k+ 1) の非線形関数として表すことができる。ちなみに、 n個の関数を持つ場合、 n次元の関節べクトルの非線形関数となる。

2つ目の例は、評価関数を各ァクチユエータで消費する電気工ネルギの総和を求めるための関数とし、この評価関数の値を最小とする関節べクトルを選択する。このように、ァクチユエータで消費する電気工ネルギの総和を最小とすることにより、ロボットの消費電力を抑制することができる。その評価関数 Jを、式（1 1) に示す。

J = /(t)^rxi?xJ( ---(ll)

I i (t) は、各関節 iを駆動するァクチユエータのモータで消費する電流である。したがって、式（1 1) の電流べクトル I ( t) = (I i (t) ， · · ·， I _n (t) ) ^τである。 Riは、各関節 iを駆動するモータ及びそれを制御する電力変換回路での損失などを考慮した各関節 iのモータでの抵抗値である。したがつて、式（1 1) の抵抗べクトル R= d i a g (Rい · · ·， R_n) である。

トルクと電流の関係式は、式（1 2) で表される。式（1 2) の Kは、トルク定数である。ここでも、上記した 2つの関節 jい j ₂と重カバランサ G₂ を持つロボットを考えた場合、式（1 0) と式（1 2) から、電流べクトル I (t) は関節ベクトル qの 2階時間微分で表されることになる。したがって、この場合の評価関数 jは、 2次元の関節ベクトル q (k— 1) ， q (k) , q (k + 1) の非線形関数として表すことができる。 τ(ή = Κ ΐ(ί - · (\2)

このように、本実施の形態では、評価関数 Fについても、時系列で連続する関節ベクトル qの差分式で取り扱う。なお、評価関数 Fが連続であるならば、評価関数 Fがどのような非線形関数であってもロボットの動作経路を生成することができる。したがって、評価関数 Fとしては、連続であることが条件となる。

それでは、動作経路生成装置 1の各部について説明する。データベース 2は、ハードディスクあるいは R AMの所定の領域に構成される。データベース 2には、ロボットの形状データ（ロボットの各部の形状、大きさなど）、構造データ（リンク長、関節の最大回転角範囲など）、口ポットが作業を行う環境データ（障害物情報、口ポットの作業対象物体情報など）などが格納される。障害物情報としては、障害物の位置、形状、大きさなどである。なお、環境データについては、データベース ₂に予め格納しておくのではなく、ロボットに備えられる各種センサ (ミリ波センサ、超音波センサ、レーザセンサ、レンジファインダ、カメラセンサなど) によって取得してもよい。この場合、取得した環境データは、記憶部 4に記憶される。センサについては、例えば、図 3に示す人型ロボットの場合、顔部の目に相当する部分にカメラなどが取り付けされる。

入力部 3は、作業者が入力や選択するための手段であり、例えば、マウス、キ一、タツチ式である。作業者が入力部 3から入力や選択するものとしては、ロボットのスタートとゴールの位置姿勢（関節ベクトル qで規定される位置姿勢）、評価関数及ぴその評価方法、拘束条件及びその判定方法、拘束条件を満たす候補を探すステップサイズ ε (時期列で連続する関節べクトル間のステップサイズに相当）、拘束条件を満たすか否かを判定する閾値 δ、拘束条件を満たす候補数の下限値 Ν (姿勢評価部 6での評価数の下限値に相当）である。

記憶部 4は、 R AMの所定の領域に構成される。記憶部 4には、姿勢生成部 5、姿勢評価部 6、角度接続部 7における各処理結果などが一時記憶される。

姿勢生成部 5は、拘束条件を満たす次の時間 k + 1での関節ベクトル q ( k + 1) の候補を N個以上生成する。ここでは、説明を簡単にするために、拘束条件として、式（3) の関節ベクトルの 1階時間微分を 2項間 q (k) ， q (k + 1) で近似した拘束条件（式（1 3) ) が入力された場合について説明する。前回の処理までで関節ベクトル q (k) まで決定されているので、姿勢生成部 5では、今回の処理で新たな関節ベクトル q (k+ 1) の候補を N個以上生成する。図 5には、関節空間を示しており、関節ベクトル q (k) を中心としている。 h₂(q{k + l),q(k))=0--- i3)

まず、姿勢生成部 5では、関節ベクトル q (k) を始点とした多数のベクトル q _{r andlJ} q _rand2J · ■ 'を乱数を用いてランダムに生成する。具体的には、乱数によって、各ベクトル q _{r and}の各関節 iの角度 q；をランダムにそれぞれ生成する。図 5に示す例では、 1 00個のベクトル q _{r and}い q _{r and2}, · · · , q _{r andl}。。が生成される。そして、姿勢生成部⁵では、関節ベクトル q (k) からの距離がステップサイズ ε となる位置にベタトノレ q _{r a n d l}， q _{r a n d} ₂， · · 'をそれぞれ射影し、候補となるベクトル q_{p l}, q_p2) · · 'を生成する。例えば、ベクトル q _{r an d j}の場合、候補ベクトル q は、式（14) によつて表される。

U ( ） _{n /1}、式（14) で示すように、 q (k) から q _{r andj}への単位ベクトルにステップサイズ εを乗算したべクトルが候補べクトル q_Pjとなる。言い換えれば、ベタトル _{r and j}— q (k) ) に（εΖ (q _{r and j} - q (k) ) のノルム）をスカラ倍することによってベクトル q_{p j}を生成する。具体的には、（ランダムに生成したベクトル q _{r a n d j}の各関節 iの角度 q i— q (k) の各関節 iの角度 q ；) にスカラ倍する。

次に、生成した候補のベクトル q_{p l}, q_{p 2}, · · '毎に、姿勢生成部 5では、関節ベクトル q (k) と候補のベクトル q_{p j} (具体的には、 q (k) の各関節 iの角度と q_{p j}の各関節 iの角度 d i) を式（13) に代入し、 h₂ (q (k +1) , q (k) ) の値を算出する。そして、姿勢生成部 5では、 h₂ (q (k +1) , q (k) ) の値が閾値 δ以下か否かを判定する。

ちなみに、拘束条件を厳密に満たす（すなわち、 h ₂ (q (k + 1) ， q (k) ) の値が 0) 候補べクトルを探索するのは、処理負荷が大きく、時間を要する。そこで、閾値 δによって拘束条件を必要十分に満たす候補ベクトルを探索する。閾値 δは、ロボットの形状や構造、口ポットの作業精度、処理負荷などを考慮して作業者によつて設定される。

姿勢生成部 5では、ランダムに生成した候補ベクトル q _{p l}， _ρ2, · · ·のうち、 h₂ (q (k + 1) ， q (k) ) の値が閾値 δ以下となった候補ベクトルの数が Ν個以上か否かを判定する。 Ν個未満の場合、姿勢生成部 5では、上記と同様の方法により、前回までと異なる候補ベクトル q_{p l}， q_p2, · · 'を生成し、その中から拘束条件を満たすものを選択する。このようにして、姿勢生成部 5では、拘束条件を満たす N個以上の関節ベクトル q (k + 1) の候補ベクトル q_ppl, q_pp2， ' ■ · , q_ppMを決定するまで上記の処理を行う。

ちなみに、姿勢評価部 6での評価数を N個以上とするのは、評価ができるだけ高い関節ベクトル q (k+ 1) を決定するためであり、 Nの数が大きくするほど評価の高い関節ベクトル q (k + 1) を決定できる確率が高くなる。しかし、 N の数を大きくするほど、処理負荷も増大する。したがって、 Nは、口ポットの評価レベルや精度、処理負荷などを考慮して作業者によって設定される。

このように、姿勢生成部 5では、 δを閾値として拘束条件を満たす Ν個以上の候補ベクトル q_ppい q_pp2) · · ' , q_ppMを決定する。なお、例えば、拘束条件が 2階時間微分を 3項間 q (k-1) ， q (k) , q (k+1) の差分で近似した拘束条件の場合には既に決定されている q (k- 1) , q (k) を用いて q (k + 1) についての N個以上の候補ベクトル q _ppl， q_pp2, · · · , q _pp

Mを決定し、拘束条件が 3階時間微分を 4項間 q (k-2) ， q (k- 1) ， q (k) ， q (k+ 1) の差分で近似した拘束条件の場合には既に決定されている q (k- 2) ， q (k一 1) ， q (k) を用いて q (k+ 1) についての N個以上の候補ベクトル q _{p p}い q _{p p 2}, · · ·， q _{p pM}を決定する。

姿勢評価部 6は、姿勢生成部 5で生成された拘束条件を満たす関節べクトル q (k+ 1) の候補 q _{p p}い q _{p p 2!} · · · , q _{p pM}の中から、評価関数を使って評価が高くかつ障害物と干渉しない関節ベクトル q (k+ 1) を 1つ決定する。ここでは、説明を簡単にするために、評価関数を F (q (k) , q (k+ 1) ) とした場合について説明する。前回の処理までで関節ベクトル q (k) まで決定されているので、姿勢評価部 6では、今回の処理で関節ベクトル q (k + 1) の候補ベクトル q _{p p l}, q _{p p 2)} · · ·， q _{p pM}の中から 1つの関節ベクトル q (k+ 1) を決定する。

まず、候補ベクトル q _{p p l}， q _{p p 2)} · · · , q _{p pM}毎に、姿勢評価部 6では、関節ベクトル q (k) と候補ベクトル q _{p p j} (具体的には、 q (k) の各関節 i の角度 q iと q _{p p j}の各関節 iの角度 q J を評価関数 Fに代入し、評価関数 Fの値を算出する。そして、姿勢評価部 6では、全ての候補ベクトル q _{p p l}, q _{p p} ₂， · · ·， q _{p pM}の評価関数 Fの値を比較し、評価関数 Fの値が最小（すなわち、最も評価の高い）となる候補ベクトル。_{p t l}を選択する。但し、評価関数 Fによっては、評価関数 Fの値が最大のものが最も評価が高くなる場合もある。次に、姿勢評価部 6では、関節ベクトル q (k) と選択した候補ベクトル q。 _{pt l}とを接続し、線分（枝）を生成する。そして、姿勢評価部 6では、作業する環境において生成した線分上の各関節べクトルによって決まる姿勢のロボットの各部が障害物と干渉するか否かを判定する。障害物と干渉する場合（つまり、口ポットが障害物とぶっかる場合）、姿勢評価部 6では、全ての候補ベクトル q _p _{p l}, q _{p p 2)} · · ·， q _{p pM}の評価関数 Fの値を再度比較し、評価関数 Fの値が次に小さい値の候補ベクトル q。_{p t2}を選択する。そして、姿勢評価部 6では、上記と同様に、関節ベクトル q (k) と候補ベクトル q。_{p t 2}との線分について障害物との干渉判定を行う。このようにして、姿勢評価部 6では、障害物と干渉しない候補べクトル q。_{p t}を決定するまで上記の処理を行う。

障害物と干渉しない侯補ベクトル q。_{p t}を決定すると、姿勢評価部 6では、その候補ベクトル q。_{p t}を時間 k + 1での関節ベクトル q (k + 1) とする。つまり、候補ベクトル q_{pp l}, q_{p p 2J} · · · , q_{p pM}の中から、評価関数 Fでの評価ができるだけ高くかつロボットが障害物とぶっからない関節べクトル q (k + 1) が 1つ決定される。なお、例えば、評価関数が F (q (k- 1) , q (k) q (k+ 1) ) の場合には既に決定されている q (k- 1) , q (k) を用いて候補ベクトルの中から関節ベクトル q (k+ 1) が 1つ決定され、評価関数が F (q (k-2) , q (k— 1) , q (k) , q (k+ 1) ) の場合には既に決定されている q (k-2) , q (k一 1) ， q (k) を用いて候補ベクトルの中から関節ベクトル q (k + 1) が 1つ決定される。

角度接続部 7は、姿勢評価部 6で決定された時系列で連続する関節べクトル q を接続し、ロボットのスタートからゴールまでの動作経路を生成する。具体的には、姿勢評価部 6で関節ベクトル q (k+ 1) が決定されると、角度接続部 7では、既に決定されている関節ベクトル q (k) と関節ベクトル q (k + 1) を接続し（具体的には、関節べクトル q (k) の各関節 iの角度 q iと関節べクトル q (k + 1) の各関節 iの角度 q；を接続し）、その接続した線分上に関節べクトル（各関節 iの角度 d i) を補間する。このようにして、角度接続部 7では、スタートからゴールまでの時系列で連続する関節べクトルによって動作経路を生成する。ちなみに、動作経路を生成する際に、スタートからゴールに向けて関節ベタトルを延ばしていってもよいし、ゴールからスタートに向けて関節べクトルを延ばしていってもよいし、スタートとゴールの両側から関節べクトルを延ばしていってもよレヽ。

出力部 8は、角度接続部 7で作成した動作経路を出力する手段である。出力部

8は、例えば、モニタ、プリンタ、ロボットを動作させる制御部との通信を行う通信装置である。また、出力部 8は、ロボットを動作させる制御部としての機能を有する場合、動作経路における各関節べクトルに従ってロボットの各関節のァクチユエータを駆動制御する。

図 1を参照して、動作経路生成装置 1の動作を、図 6のフローチャートに沿つて説明する。図 6は、本実施の形態に係る動作経路生成装置での動作の流れを示すフローチヤ一トである。

動作経路生成装置 1のデータベース 2には、ロボットの形状データや構造データ、環境データが予め格納される。動作経路生成装置 1では、作業者によって入力部 3から、ロボットのスタートとゴールの位置姿勢（関節ベクトル）、評価関数及ぴその評価方法、拘束条件及びその判定方法、ステップサイズ ε、閾値 δ、候補の数 Νが入力される（S 1) 。なお、例えば、評価関数、拘束条件において 3項間の関節ベクトルを用いる場合には関節ベクトル q (1) , q (2) を入力する必要があり、 4項間の関節ベクトルを用いる場合には関節ベクトル q (1) ， q (2) , q (3) を入力する必要がある。

姿勢生成部 5では、乱数を用いて候捕ベクトル q_pい q_{p 2}， · · 'をランダムに生成し、候補ベクトル q_pい q_{p 2}， · · 'の中から δを閾値として拘束条件を満たす次の関節ベクトル q (k + 1 ) の候捕ベクトル q _{p p l}, q _{p p} ₂, · · · , q_ppMを N個以上選択する（S 2) 。姿勢評価部 6では、次の関節ベクトル q (k + 1) の候補ベクトル q_ppい q_{pp 2}, · · ·， q_ppMの中から、評価関数 Fで評価ができるだけ高くかつ障害物と干渉しない関節ベクトル q (k + 1) を 1つ選択する（S 3) 。角度接続部 7では、選択された関節ベクトル q (k+ 1) と前回の関節ベクトル q (k) とを接続し、その間を補間する（S 4) 。

そして、角度接続部 7では、スタートからゴールまでの関節ベクトル qの時系列からなる動作経路が完成したか否かを判定する（S 5) 。 S 5にて動作経路が完成していないと判定した場合、動作経路生成装置 1では、 S 2の処理に戻って、再度、 S 2から S 4の動作を行う。 S 5にて動作経路が完成したと判定した場合、動作経路生成装置 1では、出力部 8によって、動作経路を出力する。

この動作経路生成装釁 1によれば、ロボットが拘束条件を満たしかつ障害物とぶっからず、評価条件を考慮した動作経路を自動的に生成できる。特に、動作経路生成装置 1では、評価関数として線形関数や各種非線形関数を適用でき、あらゆる評価条件についての最適化を図ることができる。例えば、式（9 ) や式（1 1 ) で示すような非常に複雑な非線形関数を評価関数とした場合でも、各評価関数による評価条件を最適化した動作経路を生成することができる。これによつて、ロボットの動作経路についてのあらゆる最適化問題に対応することができる。動作経路生成装置 1では、候補となる関節べクトル（各関節の角度）を乱数によってランダムに生成することにより、関節の個数に関係なく候補べクトルを簡単かつ効率良く生成することができる。また、動作経路生成装置 1では、関節べタトル間の変化（各関節の角度間の変化）をスカラ倍することによって候補となる関節べクトルを生成することにより、候捕べクトルを簡単に生成することができ、拘束条件を満たす姿勢の探索効率を高めることができる。

動作経路生成装置 1では、拘束条件を時系列で連続する関節べクトルの差分で近似することにより、拘束条件を簡単化でき、拘束条件を効率良く判定することができる。また、動作経路生成装置 1では、評価関数を時系列で連続する関節べクトルの差分で近似することにより、評価関数を簡単化でき、複数の候補べクトルの中から評価関数を考慮して効率良く 1つの関節べクトルを選択することができる。

以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。

例えば、本実施の形態では多数の関節を有し、関節が回転動作する口ポットに適用したが、関節が伸縮動作などの他の動作を行うものでも適用可能であり、また、ロボット全体が 1次元上、 2次元平面内、 3次元空間内を移動可能なロボットでも適用可能である。

また、本実施の形態では評価条件として障害物と干渉しないという条件と評価関数を用いた条件との 2つの条件としたが、評価条件が 1つだけでもよいし、あるいは、 3つ以上の条件でもよい。

また、本実施の形態では拘束条件及び評価条件が入力部によって入力される構成としたが、データベースなどの記憶手段に予め格納しておくなど、他の手段でこれらの条件を取得する構成としてもよい。

産業上の利用可能性

本発明に係るロボットの動作経路生成装置によれば、拘束条件を満たしかつ様々な評価条件の最適化を図ったロボットの動作経路を生成することができる。

Claims

請求の範囲

1 . 力学的拘束を伴う関節ロボットの動作経路を生成するロボットの動作経路生成装置であって、

ロボットの動作を拘束する拘束条件を取得する拘束条件取得手段と、ロボットの動作を評価する評価条件を取得する評価条件取得手段と、前記拘束条件取得手段で取得した拘束条件を満たす口ボットの姿勢を複数生成する姿勢生成手段と、

前記評価条件取得手段で取得した評価条件に基づいて前記姿勢生成手段で生成した複数の姿勢をそれぞれ評価する姿勢評価手段と、

前記姿勢生成手段で生成した複数の姿勢の中から前記姿勢評価手段での評価結果に基づいて姿勢を選択する姿勢選択手段と、

前記姿勢選択手段で選択した姿勢を用いてロボットの動作経路を生成する動作経路生成手段と

を備えることを特徴とするロボットの動作経路生成装置。

2 . 前記姿勢生成手段は、ロボットの各関節の角度をランダムに生成してロボットの姿勢を生成し、当該生成したロボットの姿勢における各関節の角度の変化に基づいて拘束条件を満たすか否かを判定することを特徴とする請求項 1 に記載するロボットの動作経路生成装置。

3 . 前記姿勢生成手段は、口ポットの前回の姿勢に対する各関節の角度の変化をスカラ倍することによってロボットの姿勢を生成することを特徴とする請求項 1又は請求項 2に記載するロボットの動作経路生成装置。

4 . 評価条件は、ロボットの姿勢の各関節の角度を変数とする評価関数を用いた条件であり、

前記姿勢評価手段は、前記姿勢生成手段で生成した姿勢の各関節の角度を評価関数に入力し、当該評価関数の出力値に基づいて姿勢を評価することを特徴とする請求項 1〜請求項 3のいずれか 1項に記載するロボットの動作経路生成装置。

5 . 評価条件は、複数個の条件があることを特徴とする請求項 1〜請求項 4のいずれか 1項に記載するロボットの動作経路生成装置。

6 . 評価条件は、ロボットの姿勢が障害物と干渉しない姿勢であるという条件を含むことを特徴とする請求項 1〜請求項 5のいずれか 1項に記载する口ボットの動作経路生成装置。