JP2009032189A - ロボットの動作経路生成装置 - Google Patents

Abstract

【課題】拘束条件を満たしかつ様々な評価条件の最適化を図ることが可能な関節ロボットの動作経路を生成することができるロボットの動作経路生成装置を提供することを課題とする。
【解決手段】力学的拘束を伴う関節ロボットの動作経路を生成するロボットの動作経路生成装置１であって、ロボットの拘束条件を取得する拘束条件取得手段３と、ロボットの評価条件を取得する評価条件取得手段３と、拘束条件を満たすロボットの姿勢を複数生成する姿勢生成手段５と、評価条件に基づいて姿勢生成手段５で生成した複数の姿勢をそれぞれ評価する姿勢評価手段６と、姿勢生成手段５で生成した複数の姿勢の中から姿勢評価手段６での評価結果に基づいて姿勢を選択する姿勢選択手段６と、姿勢選択手段６で選択した姿勢を用いてロボットの動作経路を生成する動作経路生成手段７とを備えることを特徴とする。
【選択図】図１

Description

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

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

ロボットの評価条件に対する最適化問題には、１次関数を扱う線形計画問題と１次関数以外（２次関数、３次関数、・・・、任意の非線形関数）を扱う非線形計画問題がある。しかし、特許文献１に記載の運動制御装置は、評価関数が２次関数である最適化問題を扱っており、２次計画問題の範囲で表現できる場合にしか適用できない。したがって、２次関数以外のより複雑な関数を評価関数とするロボットについての動作経路を生成することができない。

そこで、本発明は、拘束条件を満たしかつ様々な評価条件の最適化を図ることが可能な関節ロボットの動作経路を生成することができるロボットの動作経路生成装置を提供することを課題とする。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

そのために、動作経路生成装置１は、データベース２、入力部３、記憶部４、姿勢生成部５、姿勢評価部６、角度接続部７、出力部８を備えている。動作経路生成装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、姿勢生成部５、姿勢評価部６、角度接続部７はハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。

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

まず、本実施の形態に適用されるロボットについて説明する。図２にはロボットの一例を示している。ロボットＲ１は、ｎ個の関節Ｊ_１，・・・，Ｊ_ｎを備えており、関節間がリンクＬ_１，・・・，Ｌ_ｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ_１の一端が固定され、先端のリンクＬ_ｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ_１，・・・，Ｊ_ｎは、アクチュエータが内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度ｑ_１，・・・，ｑ_ｎをそれぞれ変更する。

このように、ロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ個の角度に対して座標軸を持つｎ次元座標空間（関節空間、コンフィグレーション空間）における一点（ｑ_１，・・・，ｑ_ｎ）で表される。また、ロボットＲ１の実際の位置姿勢は、３次元空間（作業空間）におけるロボットＲ１の先端部Ｔ（リンクＬ_ｎ＋１とハンドＨとの取付部）の座標位置（Ｙ１，Ｙ２，Ｙ３）とハンドＨの姿勢で表される。

ｎ個の関節角度ｑ_１，・・・，ｑ_ｎによって、ｑ＝（ｑ_１，・・・，ｑ_ｎ）^Ｔを定義し、これを関節ベクトルと呼ぶ。関節ベクトルｑは、時間の関数であり、時系列で一定時間毎のｑ（１），・・・ｑ（ｋ−１），ｑ（ｋ），ｑ（ｋ＋１），・・・で表される。したがって、時間ｔにおける関節ベクトルｑは、ｑ（ｔ）＝（ｑ_１（ｔ），・・・，ｑ_ｎ（ｔ））^Ｔとなる。

また、図３にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、１０個の関節Ｊ_１，・・・，Ｊ_１０を備えており、１０個の自由度を持つ。ロボットＲ２は、自由度が関節空間における座標系（ｑ_１，・・・，ｑ_１０）で表され、実際の位置姿勢が作業空間における各先端部Ｔ１，Ｔ２の座標位置（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）と各ハンドＨ１，Ｈ２の姿勢で表される。

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

式（１）において、ｄ^２ｑ／ｄｔ^２が関節ベクトルｑの２階時間微分であり、ｄｑ／ｄｔが関節ベクトルｑの１階時間微分である。また、Ｈ（ｑ）がロボットに作用する慣性力を表す行列であり、Ｃ（ｄｑ／ｄｔ，ｑ）がロボットに作用する遠心力とコリオリ力を表す行列であり、Ｇ（ｑ）がロボットに作用する重力を表すベクトルである。

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

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

例えば、時間ｔからΔｔ／２前の時間ｔ−Δｔ／２における関節ｉの角度ｑ_ｉ（ｔ−Δｔ／２）と時間ｔからΔｔ／２後の時間ｔ＋Δｔ／２における関節ｉの角度ｑ_ｉ（ｔ＋Δｔ／２）とすると、関節ｉの角度ｑ_ｉの１階時間微分は、Δｔが極短い時間であると、式（５）で示すようにｑ_ｉ（ｔ−Δｔ／２）とｑ_ｉ（ｔ＋Δｔ／２）によって近似できる。

そこで、関節ｉの角度ｑ_ｉの時間微分を、一般化し、式（６）のように定義する。式（６）において上付き文字（ｍ）はｍ階時間微分であることを示し、（ｍ−１）はｍ−１階時間微分であることを示す。

関節ｉの角度ｑ_ｉの１階時間微分を、関節ｉの角度ｑ_ｉの０回時間微分で表すと、式（７）で示すようになる。ここで、式（７）のｑ_ｉ ^（０）は、関節の角度ｑ_ｉの０回時間微分なので、ｑ_ｉ ^（０）＝ｑ_ｉとする。また、ｑ_ｉ（ｋ）＝ｑ_ｉ（ｔ−Δｔ／２，Δｔ）／Δｔ、ｑ_ｉ（ｋ＋１）＝ｑ_ｉ（ｔ＋Δｔ／２，Δｔ）／Δｔとおく。これによって、時間ｔ＝ｋでの関節ｉの角度ｑ_ｉの１階時間微分は、式（８）に示すように、時間ｋの関節ｉの角度ｑ_ｉ（ｋ）とその次の時間ｋ＋１の関節ｉの角度ｑ_ｉ（ｋ＋１）の２項間での差分式で表すことができる。

このように、任意の関節ｉの角度ｑ_ｉのｍ階時間微分を、角度ｑ_ｉのｍ＋１項間の差分式で近似することができる。したがって、拘束条件も各関節ｉの角度ｑ_ｉのｍ＋１項間の差分式で近似することができる。これによって、拘束条件を、各関節ｉの角度ｑ_ｉのｍ＋１項間の変化に基づいて判定することができる。例えば、式（３）の場合、拘束条件式が関節ベクトルｑ（各関節ｉの角度ｑ_ｉ）の１階時間微分を変数とするので、関節ベクトルｑ（ｋ）、ｑ（ｋ＋１）（各関節ｉの角度ｑ_ｉ（ｋ）、ｑ_ｉ（ｋ＋１））の２項間の関係式で表すことができる。また、式（４）の場合、拘束条件式が関節ベクトルｑの２階時間微分を変数とするので、関節ベクトルｑ（ｋ−１）、ｑ（ｋ）、ｑ（ｋ＋１）（各関節ｉの角度ｑ_ｉ（ｋ−１）、ｑ_ｉ（ｋ）、ｑ_ｉ（ｋ＋１））の３項間の関係式で表すことができる。このように、本実施の形態では、時系列で連続する関節ベクトルｑの差分式で近似して拘束条件を取り扱う。

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

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

本実施の形態では、ｐ＋１個の連続した関節ベクトルｑ（ｋ−ｐ＋１），・・・，ｑ（ｋ），ｑ（ｋ＋１）の関数Ｆ（ｑ（ｋ−ｐ＋１），・・・，ｑ（ｋ），ｑ（ｋ＋１））を評価関数とする。そして、ｐ個の関節ベクトルｑ（ｋ−ｐ＋１），・・・，ｑ（ｋ）を既知として、関節ベクトルｑ（ｋ＋１）の複数の候補（拘束条件を満たす候補）の中から、評価関数の値を最小（あるいは、最大）とする関節ベクトルを選択し、ｋ＋１番目の関節ベクトルｑ（ｋ＋１）を決定する。

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

式（９）において、τ_ｉは、関節ｉ（すなわち、アクチュエータ）で発生するトルクである。ここで、説明を簡単にするために、図４に示すような、２つの関節Ｊ_１，Ｊ_２と重力バランサＧ_１，Ｇ_２を持つロボットを考える。このようなロボットの場合、重力バランサＧ_１，Ｇ_２があるので、遠心力とコリオリ力が０となり、重力が０となり、慣性力が一定値となる。したがって、式（１）に示すロボットの運動方程式から、関節ｉのトルクτ_ｉを式（１０）で表すことができる。式（１０）において、Ｈは、時定数の慣性行列である。

したがって、この場合の評価関数Ｃは、関節角度ｑ_ｉを３階時間微分した値の２乗和で表すことができる。上記したように、関節角度ｑ_ｉの３階時間微分は、時系列で連続した４項間ｑ_ｉ（ｋ−２）、ｑ_ｉ（ｋ−１）、ｑ_ｉ（ｋ）、ｑ_ｉ（ｋ＋１）の差分式で近似することができる。したがって、評価関数Ｃは、２次元の関節ベクトルｑ（ｋ−２）、ｑ（ｋ−１）、ｑ（ｋ）、ｑ（ｋ＋１）の非線形関数として表すことができる。ちなみに、ｎ個の関数を持つ場合、ｎ次元の関節ベクトルの非線形関数となる。

２つ目の例は、評価関数を各アクチュエータで消費する電気エネルギの総和を求めるための関数とし、この評価関数の値を最小とする関節ベクトルを選択する。このように、アクチュエータで消費する電気エネルギの総和を最小とすることにより、ロボットの消費電力を抑制することができる。その評価関数Ｊを、式（１１）に示す。

Ｉ_ｉ（ｔ）は、各関節ｉを駆動するアクチュエータのモータで消費する電流である。したがって、式（１１）の電流ベクトルＩ（ｔ）＝（Ｉ_１（ｔ），・・・，Ｉ_ｎ（ｔ））^Ｔである。Ｒ_ｉは、各関節ｉを駆動するモータ及びそれを制御する電力変換回路での損失などを考慮した各関節ｉのモータでの抵抗値である。したがって、式（１１）の抵抗ベクトルＲ＝ｄｉａｇ（Ｒ_１，・・・，Ｒ_ｎ）である。

トルクと電流の関係式は、式（１２）で表される。式（１２）のＫは、トルク定数である。ここでも、上記した２つの関節Ｊ_１，Ｊ_２と重力バランサＧ_１，Ｇ_２を持つロボットを考えた場合、式（１０）と式（１２）から、電流ベクトルＩ（ｔ）は関節ベクトルｑの２階時間微分で表されることになる。したがって、この場合の評価関数Ｊは、２次元の関節ベクトルｑ（ｋ−１），ｑ（ｋ），ｑ（ｋ＋１）の非線形関数として表すことができる。

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

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

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

記憶部４は、ＲＡＭの所定の領域に構成される。記憶部４には、姿勢生成部５、姿勢評価部６、角度接続部７における各処理結果などが一時記憶される。

姿勢生成部５は、拘束条件を満たす次の時間ｋ＋１での関節ベクトルｑ（ｋ＋１）の候補をＮ個以上生成する。ここでは、説明を簡単にするために、拘束条件として、式（３）の関節ベクトルの１階時間微分を２項間ｑ（ｋ），ｑ（ｋ＋１）で近似した拘束条件（式（１３））が入力された場合について説明する。前回の処理までで関節ベクトルｑ（ｋ）まで決定されているので、姿勢生成部５では、今回の処理で新たな関節ベクトルｑ（ｋ＋１）の候補をＮ個以上生成する。図５には、関節空間を示しており、関節ベクトルｑ（ｋ）を中心としている。

まず、姿勢生成部５では、関節ベクトルｑ（ｋ）を始点とした多数のベクトルｑ_{ｒａｎｄ１}，ｑ_{ｒａｎｄ２}，・・・を乱数を用いてランダムに生成する。具体的には、乱数によって、各ベクトルｑ_ｒａｎｄの各関節ｉの角度ｑ_ｉをランダムにそれぞれ生成する。図５に示す例では、１００個のベクトルｑ_{ｒａｎｄ１}，ｑ_{ｒａｎｄ２}，・・・，ｑ_{ｒａｎｄ１００}が生成される。そして、姿勢生成部５では、関節ベクトルｑ（ｋ）からの距離がステップサイズεとなる位置にベクトルｑ_{ｒａｎｄ１}，ｑ_{ｒａｎｄ２}，・・・をそれぞれ射影し、候補となるベクトルｑ_ｐ１，ｑ_ｐ２，・・・を生成する。例えば、ベクトルｑ_{ｒａｎｄｊ}の場合、候補ベクトルｑ_ｐｊは、式（１４）によって表される。

式（１４）で示すように、ｑ（ｋ）からｑ_{ｒａｎｄｊ}への単位ベクトルにステップサイズεを乗算したベクトルが候補ベクトルｑ_ｐｊとなる。言い換えれば、ベクトル（ｑ_{ｒａｎｄｊ}−ｑ（ｋ））に（ε／（ｑ_{ｒａｎｄｊ}−ｑ（ｋ））のノルム）をスカラ倍することによってベクトルｑ_ｐｊを生成する。具体的には、（ランダムに生成したベクトルｑ_{ｒａｎｄｊ}の各関節ｉの角度ｑ_ｉ−ｑ（ｋ）の各関節ｉの角度ｑ_ｉ）にスカラ倍する。

次に、生成した候補のベクトルｑ_ｐ１，ｑ_ｐ２，・・・毎に、姿勢生成部５では、関節ベクトルｑ（ｋ）と候補のベクトルｑ_ｐｊ（具体的には、ｑ（ｋ）の各関節ｉの角度ｑ_ｉとｑ_ｐｊの各関節ｉの角度ｑ_ｉ）を式（１３）に代入し、ｈ_２（ｑ（ｋ＋１），ｑ（ｋ））の値を算出する。そして、姿勢生成部５では、ｈ_２（ｑ（ｋ＋１），ｑ（ｋ））の値が閾値δ以下か否かを判定する。

ちなみに、拘束条件を厳密に満たす（すなわち、ｈ_２（ｑ（ｋ＋１），ｑ（ｋ））の値が０）候補ベクトルを探索するのは、処理負荷が大きく、時間を要する。そこで、閾値δによって拘束条件を必要十分に満たす候補ベクトルを探索する。閾値δは、ロボットの形状や構造、ロボットの作業精度、処理負荷などを考慮して作業者によって設定される。

姿勢生成部５では、ランダムに生成した候補ベクトルｑ_ｐ１，ｑ_ｐ２，・・・のうち、ｈ_２（ｑ（ｋ＋１），ｑ（ｋ））の値が閾値δ以下となった候補ベクトルの数がＮ個以上か否かを判定する。Ｎ個未満の場合、姿勢生成部５では、上記と同様の方法により、前回までと異なる候補ベクトルｑ_ｐ１，ｑ_ｐ２，・・・を生成し、その中から拘束条件を満たすものを選択する。このようにして、姿勢生成部５では、拘束条件を満たすＮ個以上の関節ベクトルｑ（ｋ＋１）の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭを決定するまで上記の処理を行う。

ちなみに、姿勢評価部６での評価数をＮ個以上とするのは、評価ができるだけ高い関節ベクトルｑ（ｋ＋１）を決定するためであり、Ｎの数が大きくするほど評価の高い関節ベクトルｑ（ｋ＋１）を決定できる確率が高くなる。しかし、Ｎの数を大きくするほど、処理負荷も増大する。したがって、Ｎは、ロボットの評価レベルや精度、処理負荷などを考慮して作業者によって設定される。

このように、姿勢生成部５では、δを閾値として拘束条件を満たすＮ個以上の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭを決定する。なお、例えば、拘束条件が２階時間微分を３項間ｑ（ｋ−１），ｑ（ｋ），ｑ（ｋ＋１）の差分で近似した拘束条件の場合には既に決定されているｑ（ｋ−１），ｑ（ｋ）を用いてｑ（ｋ＋１）についてのＮ個以上の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭを決定し、拘束条件が３階時間微分を４項間ｑ（ｋ−２），ｑ（ｋ−１），ｑ（ｋ），ｑ（ｋ＋１）の差分で近似した拘束条件の場合には既に決定されているｑ（ｋ−２），ｑ（ｋ―１），ｑ（ｋ）を用いてｑ（ｋ＋１）についてのＮ個以上の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭを決定する。

姿勢評価部６は、姿勢生成部５で生成された拘束条件を満たす関節ベクトルｑ（ｋ＋１）の候補ｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの中から、評価関数を使って評価が高くかつ障害物と干渉しない関節ベクトルｑ（ｋ＋１）を１つ決定する。ここでは、説明を簡単にするために、評価関数をＦ（ｑ（ｋ），ｑ（ｋ＋１））とした場合について説明する。前回の処理までで関節ベクトルｑ（ｋ）まで決定されているので、姿勢評価部６では、今回の処理で関節ベクトルｑ（ｋ＋１）の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの中から１つの関節ベクトルｑ（ｋ＋１）を決定する。

まず、候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭ毎に、姿勢評価部６では、関節ベクトルｑ（ｋ）と候補ベクトルｑ_ｐｐｊ（具体的には、ｑ（ｋ）の各関節ｉの角度ｑ_ｉとｑ_ｐｐｊの各関節ｉの角度ｑ_ｉ）を評価関数Ｆに代入し、評価関数Ｆの値を算出する。そして、姿勢評価部６では、全ての候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの評価関数Ｆの値を比較し、評価関数Ｆの値が最小（すなわち、最も評価の高い）となる候補ベクトルｑ_ｏｐｔ１を選択する。但し、評価関数Ｆによっては、評価関数Ｆの値が最大のものが最も評価が高くなる場合もある。

次に、姿勢評価部６では、関節ベクトルｑ（ｋ）と選択した候補ベクトルｑ_ｏｐｔ１とを接続し、線分（枝）を生成する。そして、姿勢評価部６では、作業する環境において生成した線分上の各関節ベクトルによって決まる姿勢のロボットの各部が障害物と干渉するか否かを判定する。障害物と干渉する場合（つまり、ロボットが障害物とぶつかる場合）、姿勢評価部６では、全ての候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの評価関数Ｆの値を再度比較し、評価関数Ｆの値が次に小さい値の候補ベクトルｑ_ｏｐｔ２を選択する。そして、姿勢評価部６では、上記と同様に、関節ベクトルｑ（ｋ）と候補ベクトルｑ_ｏｐｔ２との線分について障害物との干渉判定を行う。このようにして、姿勢評価部６では、障害物と干渉しない候補ベクトルｑ_ｏｐｔを決定するまで上記の処理を行う。

障害物と干渉しない候補ベクトルｑ_ｏｐｔを決定すると、姿勢評価部６では、その候補ベクトルｑ_ｏｐｔを時間ｋ＋１での関節ベクトルｑ（ｋ＋１）とする。つまり、候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの中から、評価関数Ｆでの評価ができるだけ高くかつロボットが障害物とぶつからない関節ベクトルｑ（ｋ＋１）が１つ決定される。なお、例えば、評価関数がＦ（ｑ（ｋ−１），ｑ（ｋ），ｑ（ｋ＋１））の場合には既に決定されているｑ（ｋ−１），ｑ（ｋ）を用いて候補ベクトルの中から関節ベクトルｑ（ｋ＋１）が１つ決定され、評価関数がＦ（ｑ（ｋ−２），ｑ（ｋ―１），ｑ（ｋ），ｑ（ｋ＋１））の場合には既に決定されているｑ（ｋ−２），ｑ（ｋ―１），ｑ（ｋ）を用いて候補ベクトルの中から関節ベクトルｑ（ｋ＋１）が１つ決定される。

角度接続部７は、姿勢評価部６で決定された時系列で連続する関節ベクトルｑを接続し、ロボットのスタートからゴールまでの動作経路を生成する。具体的には、姿勢評価部６で関節ベクトルｑ（ｋ＋１）が決定されると、角度接続部７では、既に決定されている関節ベクトルｑ（ｋ）と関節ベクトルｑ（ｋ＋１）を接続し（具体的には、関節ベクトルｑ（ｋ）の各関節ｉの角度ｑ_ｉと関節ベクトルｑ（ｋ＋１）の各関節ｉの角度ｑ_ｉを接続し）、その接続した線分上に関節ベクトル（各関節ｉの角度ｑ_ｉ）を補間する。このようにして、角度接続部７では、スタートからゴールまでの時系列で連続する関節ベクトルによって動作経路を生成する。ちなみに、動作経路を生成する際に、スタートからゴールに向けて関節ベクトルを延ばしていってもよいし、ゴールからスタートに向けて関節ベクトルを延ばしていってもよいし、スタートとゴールの両側から関節ベクトルを延ばしていってもよい。

出力部８は、角度接続部７で作成した動作経路を出力する手段である。出力部８は、例えば、モニタ、プリンタ、ロボットを動作させる制御部との通信を行う通信装置である。また、出力部８は、ロボットを動作させる制御部としての機能を有する場合、動作経路における各関節ベクトルに従ってロボットの各関節のアクチュエータを駆動制御する。

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

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

姿勢生成部５では、乱数を用いて候補ベクトルｑ_ｐ１，ｑ_ｐ２，・・・をランダムに生成し、候補ベクトルｑ_ｐ１，ｑ_ｐ２，・・・の中からδを閾値として拘束条件を満たす次の関節ベクトルｑ（ｋ＋１）の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭをＮ個以上選択する（Ｓ２）。姿勢評価部６では、次の関節ベクトルｑ（ｋ＋１）の候補ベクトルｑ_ｐｐ１，ｑ_ｐｐ２，・・・，ｑ_ｐｐＭの中から、評価関数Ｆで評価ができるだけ高くかつ障害物と干渉しない関節ベクトルｑ（ｋ＋１）を１つ選択する（Ｓ３）。角度接続部７では、選択された関節ベクトルｑ（ｋ＋１）と前回の関節ベクトルｑ（ｋ）とを接続し、その間を補間する（Ｓ４）。

そして、角度接続部７では、スタートからゴールまでの関節ベクトルｑの時系列からなる動作経路が完成したか否かを判定する（Ｓ５）。Ｓ５にて動作経路が完成していないと判定した場合、動作経路生成装置１では、Ｓ２の処理に戻って、再度、Ｓ２からＳ４の動作を行う。Ｓ５にて動作経路が完成したと判定した場合、動作経路生成装置１では、出力部８によって、動作経路を出力する。

この動作経路生成装置１によれば、ロボットが拘束条件を満たしかつ障害物とぶつからず、評価条件を考慮した動作経路を自動的に生成できる。特に、動作経路生成装置１では、評価関数として線形関数や各種非線形関数を適用でき、あらゆる評価条件についての最適化を図ることができる。例えば、式（９）や式（１１）で示すような非常に複雑な非線形関数を評価関数とした場合でも、各評価関数による評価条件を最適化した動作経路を生成することができる。これによって、ロボットの動作経路についてのあらゆる最適化問題に対応することができる。

動作経路生成装置１では、候補となる関節ベクトル（各関節の角度）を乱数によってランダムに生成することにより、関節の個数に関係なく候補ベクトルを簡単かつ効率良く生成することができる。また、動作経路生成装置１では、関節ベクトル間の変化（各関節の角度間の変化）をスカラ倍することによって候補となる関節ベクトルを生成することにより、候補ベクトルを簡単に生成することができ、拘束条件を満たす姿勢の探索効率を高めることができる。

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

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

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

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

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

本実施の形態に係る動作経路生成装置の構成図である。 本実施の形態で適用されるロボットの一例である。 本実施の形態で適用されるロボットの他の例である。 ２つの関節と重力バランサを持つロボットの一例である。 図１の姿勢生成部で生成する関節ベクトルの候補の一例である。 本実施の形態に係る動作経路生成装置での動作の流れを示すフローチャートである。

符号の説明

１…動作経路生成装置、２…データベース、３…入力部、４…記憶部、５…姿勢生成部、６…姿勢評価部、７…角度接続部、８…出力部

Claims (6)

1. 力学的拘束を伴う関節ロボットの動作経路を生成するロボットの動作経路生成装置であって、
   ロボットの動作を拘束する拘束条件を取得する拘束条件取得手段と、
   ロボットの動作を評価する評価条件を取得する評価条件取得手段と、
   前記拘束条件取得手段で取得した拘束条件を満たすロボットの姿勢を複数生成する姿勢生成手段と、
   前記評価条件取得手段で取得した評価条件に基づいて前記姿勢生成手段で生成した複数の姿勢をそれぞれ評価する姿勢評価手段と、
   前記姿勢生成手段で生成した複数の姿勢の中から前記姿勢評価手段での評価結果に基づいて姿勢を選択する姿勢選択手段と、
   前記姿勢選択手段で選択した姿勢を用いてロボットの動作経路を生成する動作経路生成手段と
   を備えることを特徴とするロボットの動作経路生成装置。
2. 前記姿勢生成手段は、ロボットの各関節の角度をランダムに生成してロボットの姿勢を生成し、当該生成したロボットの姿勢における各関節の角度の変化に基づいて拘束条件を満たすか否かを判定することを特徴とする請求項１に記載するロボットの動作経路生成装置。
3. 前記姿勢生成手段は、ロボットの前回の姿勢に対する各関節の角度の変化をスカラ倍することによってロボットの姿勢を生成することを特徴とする請求項１又は請求項２に記載するロボットの動作経路生成装置。
4. 評価条件は、ロボットの姿勢の各関節の角度を変数とする評価関数を用いた条件であり、
   前記姿勢評価手段は、前記姿勢生成手段で生成した姿勢の各関節の角度を評価関数に入力し、当該評価関数の出力値に基づいて姿勢を評価することを特徴とする請求項１〜請求項３のいずれか１項に記載するロボットの動作経路生成装置。
5. 評価条件は、複数個の条件があることを特徴とする請求項１〜請求項４のいずれか１項に記載するロボットの動作経路生成装置。
6. 評価条件は、ロボットの姿勢が障害物と干渉しない姿勢であるという条件を含むことを特徴とする請求項１〜請求項５のいずれか１項に記載するロボットの動作経路生成装置。

Images