(57)【要約】【課題】適切な逆座標変換計算を行えない際、特異点やその近辺に特異点回避軌道を再生成する計算を行うことなく、計算済みの目標点を再利用可能な多関節型マニピュレータの手先動作軌道生成方法を提供する。【解決手段】関節部９，１１，１３がアーム１０，１２を介して連結され、一端に手先８が設けられた多関節型マニピュレータ７と、多関節型マニピュレータ７に設けられ、関節部９，１１，１３を駆動する駆動装置と、直線軌道上の目標点に対応する関節角座標への逆座標変換によって求めた関節角座標系における目標値に関節部９，１１，１３が順次到達するようにこの駆動装置を駆動することにより、手先８を直線軌道上に沿って移動させる制御装置４とを備え、制御装置４は、直線軌道上又はこの近傍に逆座標変換により適切な解が得られない特異点が存在する場合、この特異点が存在する又はその近傍の目標点にこれを中心とする円錐状の回避軌道を生成する。【選択図】図１
2010076058.tif 000002
【特許請求の範囲】
【請求項１】
複数の駆動可能な関節がアームを介して連結され、一端に手先が設けられた多関節型マニピュレータと、
この多関節型マニピュレータに設けられ、前記関節を駆動する駆動装置と、
直線軌道上の目標点に対応する関節角座標への逆座標変換によって求めた関節角座標系における目標値に前記関節が順次到達するように前記駆動装置を駆動することにより、前記手先をこの直線軌道上に沿って移動させる制御装置と、を備え、
この制御装置は、
前記直線軌道上またはこの近傍に前記逆座標変換により唯一の解が得られない特異点が存在する場合、この特異点が存在する目標点にこれを中心とする円錐状の回避軌道を生成することを特徴とする、多関節型マニピュレータの制御装置。
【請求項２】
前記制御装置は、前記直線軌道に直交する底面を有し各底面が接触している２つの円錐を、これらの円錐の各円錐高さと各母線の長さとの比が整数比となるように求めることを特徴とする、請求項１記載の多関節型マニピュレータの制御装置。
【請求項３】
前記制御装置は、前記直線軌道に直交する底面を有し各底面が接触している２つの円錐を、これらの円錐の各母線が前記底面の外周上で連続するようにして求め、前記回避軌道上に、複数の目標点を、前記各サンプル時刻におけるこれらの目標点の間隔が等間隔になるように設定することを特徴とする、請求項１記載の多関節型マニピュレータの制御装置。
【請求項４】
前記制御装置は、一つの円錐について互いに異なる底面半径および円錐高さの組を複数求めた場合、これらの組のうち、前記底面半径が最も小さい組を選び、この組の底面半径および円錐高さを有する円錐により規定される回避軌道を生成することを特徴とする、請求項３記載の多関節型マニピュレータの制御装置。
【請求項５】
前記制御装置は、一つの円錐について前記回避軌道を複数生成した場合、これらの回避軌道のうち、より多くの関節の各軸の動作量が最大となる回避軌道を選択することを特徴とする、請求項３又は請求項４記載の多関節型マニピュレータの制御装置。
【請求項６】
複数の駆動可能な関節がアームを介して連結され、一端に手先が設けられた多関節型マニピュレータと、この多関節型マニピュレータに設けられ、前記関節を駆動する駆動装置と、前記多関節型マニピュレータに設けられ、前記関節に含まれる関節軸の角度値を検出する関節角度センサと、この関節角度センサからのセンサ出力を用いて前記駆動装置を制御することにより、前記手先を現在位置から最終目標位置まで移動させる手先移動直線軌道を生成する制御装置と、を備えた多関節型マニピュレータの制御装置に用いられる多関節型マニピュレータの手先動作軌道生成方法において、
前記制御装置が、前記関節角度センサが出力する現在位置における前記角度値と指示された前記最終目標位置に基づき、前記現在位置及び前記最終目標位置を結ぶ直線軌道を生成し、この直線軌道上に各サンプル時刻における目標点をこれらの目標点が等間隔になるように設定し、
各目標点において目標関節角座標へ逆座標変換を行い、
この逆座標変換の結果に基づいて、前記直線軌道上またはこの近傍に特異点が存在しているかどうかを検出判定し、
この特異点が存在すると判定した場合、それぞれ前記直線軌道に直交する底面を有し各底面が接触している２つの円錐を求め、これらの円錐の各円錐面上に回避軌道を生成し、
この回避軌道により前記直線軌道中の特異点近辺の軌道を置換えし、置換えした軌道に沿って前記多関節型マニピュレータを移動させることを特徴とする、多関節型マニピュレータの手先動作軌道生成方法。
【請求項７】
前記２つの円錐はそれぞれ円錐高さと母線の長さとの比が整数比となるように選定されることを特徴とする、請求項６記載の多関節型マニピュレータの手先動作軌道生成方法。
【請求項８】
前記２つの円錐は底面半径、円錐高さ、およびこれらの底面半径及び円錐高さによって決まる母線を有し、これらの母線が前記底面の外周上で連続することにより前記回避軌道が形成され、
複数の目標点は、この回避軌道上に、前記各サンプル時刻におけるこれらの目標点の間隔が等間隔になるように設定されることを特徴とする、請求項６記載の多関節型マニピュレータの手先動作軌道生成方法。
【請求項９】
一つの円錐について底面半径および円錐高さの組が複数存在する場合、これらの組のうち、前記底面半径が最も小さい組が選ばれて、この組の持つ底面半径および円錐高さを有する円錐により規定される回避軌道が生成されることを特徴とする、請求項８記載の多関節型マニピュレータの手先動作軌道生成方法。
【請求項１０】
一つの円錐について前記回避軌道が複数存在する場合、これらの回避軌道のうち、前記各関節のより多くの軸の動作量が最大となる回避軌道が選択されることを特徴とする、請求項８又は請求項９記載の多関節型マニピュレータの手先動作軌道生成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は多関節型マニピュレータの制御装置及び多関節型マニピュレータの手先動作軌道生成方法に関し、特に生成された軌道上またはその軌道の近傍に特異点が存在し、各目標手先位置姿勢を実現するための関節角座標系目標値が逆座標変換計算により適切に得られない場合の回避軌道の生成計算を含む装置及び方法に関する。
【背景技術】
【０００２】
一般に、マニピュレータの手先動作軌道を生成する場合は、現在の手先位置姿勢から最終目標点の手先位置姿勢までを直線で結んだ、直線動作軌道を生成することが多い。そして、多関節型マニピュレータの軌道を生成する場合は、その直線動作軌道上に、各サンプル時刻における目標点を設定し、設定した各サンプル時刻における目標点の目標手先位置姿勢を実現するための関節角座標系目標値を予め逆座標変換計算により求めておく。マニピュレータを動作させる時は、各関節を各サンプル時刻における目標関節角度値に順次到達させることで、結果としてマニピュレータの手先が直線動作軌道に沿って動作することになる。この時の逆座標変換計算には、解析解が得られている６自由度の多関節型マニピュレータでは、解析的な解法が用いられるが、７自由度以上の冗長マニピュレータの場合は、通常、目標手先位置姿勢から関節角座標系目標値を解析的に求めることは困難であり、多くの場合、繰り返し収束演算による数値解法が用いられる。
【０００３】
多関節型マニピュレータの手先を直線軌道に沿って動作させる際に、その軌道上またはその軌道近傍にマニピュレータの特異点が存在する場合、その点における目標手先位置姿勢を実現するための関節角座標系目標値が解析的に解けない、あるいは数値解法の繰り返し計算が目標値の手前で収束してしまい、正しい逆座標変換計算が行えない、といった問題が発生することがあった。この問題を回避するための方法としては、特異点近傍において、直線軌道への追従性を多少犠牲にし、特異点から少し離れた領域を通過することで、目標手先位置姿勢を実現するための関節角座標系目標値を求める方法が知られている（非特許文献１参照）。
【０００４】
マニピュレータの軌道生成方法に関しては、少なくとも６関節軸を有し、先端側の３関節軸にて姿勢が決定されるマニピュレータを用い、マニピュレータがいかなる姿勢であってもマニピュレータが障害物と干渉しない空間を求めてこの空間内にてマニピュレータの姿勢を変化させるようにマニピュレータを駆動するマニピュレータの軌道生成方法が知られている（特許文献１参照）。特許文献１は、マニピュレータが外部環境との干渉を避けるための回避軌道を生成するものであり、空間が、マニピュレータによって移動される物体の移動開始前と移動終了後の位置によっても変化しないように設定されている点を開示している。
【特許文献１】特開平２−２７７１０１号公報
【非特許文献１】中村仁彦、外１名、「関節形ロボットアームの特異点低感度運動分解」、計測自動制御学会論文集、１９８４年、Ｖｏｌ．２０，Ｎｏ．５，ｐｐ．４５３-４５９
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、非特許文献１に記載の特異点回避軌道の生成方法では、特異点から少し離れた領域を関節形ロボットアームの手先が通過するため、特異点の近辺において、マニピュレータの動作速度が変化してしまい、このマニピュレータを一定の速度で動作させられない、またはこのマニピュレータを一定の速度で動作させるために、その後の軌道を全て再生成しなければならないという問題点がある。
【０００６】
本発明は、上記問題を解決するためになされたものであり、その目的は、多関節型マニピュレータの手先を直線軌道に沿って一定速度で動作させるための手先動作軌道をマニピュレータの制御装置が生成する際、それぞれ直線軌道に沿って生成された複数の目標点の中に特異点が存在する場合、制御装置が特異点から少し離れた領域を通過する特異点回避軌道を生成することにより、特異点又は特異点の近辺にその後の軌道を全て再生成する計算を行うことなく、特異点以降に位置する計算済みの目標点を手先の動作軌道として再利用可能とすることである。
【課題を解決するための手段】
【０００７】
このような課題を解決するため、本発明の一態様によれば、複数の駆動可能な関節がアームを介して連結され、一端に手先が設けられた多関節型マニピュレータと、この多関節型マニピュレータに設けられ、前記関節を駆動する駆動装置と、直線軌道上の目標点に対応する関節角座標への逆座標変換によって求めた関節角座標系における目標値に前記関節が順次到達するように前記駆動装置を駆動することにより、前記手先をこの直線軌道上に沿って移動させる制御装置と、を備え、この制御装置は、前記直線軌道上またはこの近傍に前記逆座標変換により唯一の解が得られない特異点が存在する場合、この特異点が存在する目標点にこれを中心とする円錐状の回避軌道を生成することを特徴とする、多関節型マニピュレータの制御装置が提供される。
【０００８】
また、本発明の別の一態様によれば、複数の駆動可能な関節がアームを介して連結され、一端に手先が設けられた多関節型マニピュレータと、この多関節型マニピュレータに設けられ、前記関節を駆動する駆動装置と、前記多関節型マニピュレータに設けられ、前記関節に含まれる関節軸の角度値を検出する関節角度センサと、この関節角度センサからのセンサ出力を用いて前記駆動装置を制御することにより、前記手先を現在位置から最終目標位置まで移動させる手先移動直線軌道を生成する制御装置と、を備えた多関節型マニピュレータの制御装置に用いられる多関節型マニピュレータの手先動作軌道生成方法において、前記制御装置が、前記関節角度センサが出力する現在位置における前記角度値と指示された前記最終目標位置に基づき、前記現在位置及び前記最終目標位置を結ぶ直線軌道を生成し、この直線軌道上に各サンプル時刻における目標点をこれらの目標点が等間隔になるように設定し、各目標点において目標関節角座標へ逆座標変換を行い、この逆座標変換の結果に基づいて、前記直線軌道上またはこの近傍に特異点が存在しているかどうかを検出判定し、この特異点が存在すると判定した場合、それぞれ前記直線軌道に直交する底面を有し各底面が接触している２つの円錐を求め、これらの円錐の各円錐面上に回避軌道を生成し、この回避軌道により前記直線軌道中の特異点近辺の軌道を置換えし、置換えした軌道に沿って前記多関節型マニピュレータを移動させることを特徴とする、多関節型マニピュレータの手先動作軌道生成方法が提供される。
【発明の効果】
【０００９】
本発明によれば、適切な関節角座標系目標値が逆座標変換計算により得られない場合でも、各サンプル時刻における目標点の間隔が等間隔になるような特異点回避軌道を生成することができ、特異点以降に位置する目標点を手先の動作軌道として再利用でき、また、特異点又は特異点の近辺にその後の軌道を全て再生成する計算を行うことなく、多関節型マニピュレータを一定の速度で動作させることができるようになる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の実施の形態に係る多関節型マニピュレータの制御装置及び多関節型マニピュレータの手先動作軌道生成方法について、図１乃至図８を参照しながら説明する。尚、各図において同一箇所については同一の符号を付すとともに、重複した説明は省略する。
【００１１】
図１は本実施形態に係る多関節型マニピュレータの制御装置が搭載されたロボットの外観斜視図である。ロボット１は、２個の車輪２及び車輪駆動用モータを有する移動機構３と、この移動機構３に対して旋回可能に設けられ、制御装置４（本実施形態に係る多関節型マニピュレータの制御装置）を有する胴部５と、この胴部５に対して回転可能且つ揺動可能に結合された頭部６と、一端が胴部５に回転自在に連結された３つの関節部を持つマニピュレータ７とを備えている。関節部は３箇所であるが、肩関節部および手首関節部はそれぞれ３軸の駆動部を持っており、マニピュレータ７は合計で７軸のマニピュレータである。このマニピュレータ７はロボット１の上肢部として機能するものであり、その他端には各種手先装置であるエンドエフェクタ８が装着されている。
【００１２】
胴部５は、制御装置４を含みこのロボット１の各部の制御を司るシステム制御部を有し、このシステム制御部はカバーによって覆われている。頭部６は、胴部５に対して水平及び垂直の各軸周りに回動自在に連結された図示しない駆動機構と、この駆動機構に支持されたカメラとを有する。
【００１３】
マニピュレータ７は、手首関節部９、この手首関節部９に連結した第１アーム１０、この第１アーム１０に連結した肘関節部１１、この肘関節部１１に連結した第２アーム１２、及びこの第２アーム１２に連結した肩関節部１３と、これらの手首関節部９、肘関節部１１及び肩関節部１３を各別に回転駆動するこれらの関節内に設けられた各軸のモータ（駆動装置）と、手首関節部９、肘関節部１１及び肩関節部１３にそれぞれ取付けられた各軸の関節角度センサ１４、１５及び１６とを有する。各モータは制御装置４に接続されており、この制御装置４から回転角速度の指令値を入力される。関節角度センサ１４、１５及び１６はそれぞれ手首関節部９、肘関節部１１及び肩関節部１３の各関節軸の軸周りの位置（回転角度）を検出するものである。関節角度センサ１４、１５及び１６はいずれも制御装置４に接続されており、これらの関節角度センサ１４、１５及び１６からのセンサ値は制御装置４に入力される。関節角度センサ１４、１５及び１６には、ロータリエンコーダやポテンショメータが用いられる。
【００１４】
図２はロボット１の制御システムのブロック図である。同図に示す符号のうち上述した符号と同じ符号を有する要素はそれらと同じ要素を表す。制御システム１７は、ロボット１自身の動作や処理を決定するためのシステム制御部１８と、上肢部サブシステムとして機能しこのシステム制御部１８が出力する制御情報に基づいて、上肢部であるマニピュレータ７の各関節軸の関節角度及び回転角速度などの目標動作量を生成して出力する制御装置４とを備えている。この制御装置４は、センサ出力に基づきマニピュレータ７の各関節内のモータを駆動することにより手先を現在位置から最終目標位置まで直線軌道に沿って移動させる各軸の目標軌道を生成する関節動作軌道生成部１９ａと、特異点が存在する場合、特異点近傍に後述する２つの円錐を求め、これらの円錐の各円錐面上に回避軌道を生成する特異点回避軌道生成部１９ｂと、生成済みの直線軌道中の特異点近辺の軌道をこの回避軌道で置換えし、置換えた軌道に沿ってマニピュレータ７を移動させる制御部１９ｃとを有する。
【００１５】
関節動作軌道生成部１９ａは、アーム手先の現在位置から最終目標位置に至る直線軌道を生成し、この直線軌道上に各サンプル時刻における等間隔の目標点を設定する図示しない目標点設定手段と、各目標点において目標関節角座標系への逆座標変換を行う図示しない逆座標変換計算手段と、逆座標変換の結果に基づいて直線軌道上またはこの直線軌道の近傍に特異点の存在を検出する図示しない特異点検出手段とを有する。目標点設定手段、逆座標変換計算手段及び特異点検出手段の機能は、ソフトウェアがＣＰＵにプログラムを実行させることによって実現される。
【００１６】
制御システム１７は更に、システム制御部１８からの制御情報に基づき、移動機構３に対する胴部５の旋回角度及び回転角速度などの目標動作量を生成して出力する胴回転サブシステム２０と、システム制御部１８からの制御情報に基づき、胴部５に対する頭部６の回転角度及び回転角速度などの目標動作量を生成して出力する頭部回転サブシステム２１と、システム制御部１８からの制御情報に基づき、移動機構３の車輪２の回転角度及び回転角速度などの目標動作量を生成して出力する移動サブシステム２２とを備えている。これらのシステム制御部１８、制御装置４、胴回転サブシステム２０、頭部回転サブシステム２１及び移動サブシステム２２の機能はいずれもＣＰＵ、ＲＯＭ及びＲＡＭによって実現されている。
【００１７】
本実施形態に係る多関節型マニピュレータの制御装置４は、各軸の関節角度センサ１４、１５及び１６からのセンサ値に基づき手先の現在位置姿勢を取得し、この現在位置姿勢および与えられた最終目標位置姿勢に基づいて特異点を回避するための手先動作軌道を生成し、マニピュレータ７の先端の動作軌道が等速軌道になるように各関節のモータへ角速度の指令値を出力するものである。
【００１８】
本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法は、この制御装置４が、マニピュレータ７先端の手先の現在位置及び最終目標位置間で各目標点間隔が一定間隔になるよう各目標点を予め設定し、これらの目標点において手先の位置姿勢を実現するための各関節軸角座標への逆座標変換を行い、いずれかの目標点に特異点が存在すること又はいずれかの目標点の近傍に特異点が存在することを検出した場合、この特異点上の目標点又は特異点近傍の目標点をよけるための回避軌道を直線軌道上の一部に生成する。制御装置４は、特異点の手前の直線軌道上の目標点を始点とし、この特異点以降の直線軌道上の目標点を終点とする特別な軌道を別途生成し、設定済みの直線軌道のうち特異点近辺の直線軌道を、この特別な軌道により置換えて、置換えした軌道に沿ってマニピュレータ７を移動させるように、各関節部を各別の目標角度に駆動制御する。
【００１９】
換言すれば、制御装置４は、手先を、現在位置から最終目標位置へ真っ直ぐ動かす直線軌道上に目標点を生成した後、この直線軌道上に特異点が存在する場合、この特異点又は特異点近辺に直線軌道から少し離れた領域に等間隔の目標点間隔を有する別軌道を生成し、生成済みの直線軌道上の特異点以降の目標点にこの別軌道が連続するように制御を行う。従来例も特異点を回避する軌道を生成すること自体は行っているが、本実施形態に係る多関節型マニピュレータの制御装置４では、現在位置から特異点に至るまでの軌道上の目標点の間隔と、回避軌道上の目標点の間隔と、回避軌道から最終目標位置までの軌道上の目標点の間隔とがいずれも一定間隔になるように回避軌道を生成し、且つこれらの目標点を手先が等速に移動するよう各関節軸を制御するようにしている。制御装置４は、最初に生成した直線軌道上の目標点のうち、手先が真っ直ぐに進める目標点についてはなるべく残しそのまま用いるとともに、この直線軌道上の目標点のうち、特異点又はその近傍の目標点については手先に回避軌道を通過させるようにしている。
【００２０】
制御装置４が有する目標点設定機能と、手先動作軌道生成機能とについて述べる。制御装置４は、手先の３次元的な位置（ｘ、ｙ、ｚ）およびその位置における手先の姿勢（ロール角、ピッチ角、ヨー角）を示す６つの要素からなる手先位置姿勢の値を使って制御を行う。制御装置４は、手先位置姿勢の値を微分してまたは差分を取って得た手先位置姿勢の速度値を計算し、さらに、この手先位置姿勢の速度値から、手首関節部９、肘関節部１１及び肩関節部１３の各軸角速度の値を求める。制御装置４は、求めた各軸角速度の値を各サンプル時刻における各軸のモータへの指令値として出力している。ここで、各関節の独立な動きと手先の位置及び姿勢との間には非線形の関数関係が存在している。マニピュレータ７の与えられた位置及び姿勢を実現させる関節角を決定する処理は連立非線形方程式の根を計算する処理に対応する。制御装置４は、この根計算のために逆座標変換計算処理をＣＰＵに行わせる。また、マニピュレータの制御装置４は手先位置姿勢を逆座標変換計算し、微分または差分によって各軸角速度を求める。制御装置４は、３次元空間で、例えば１／１００秒毎、１／１０００秒毎のポイントのそれぞれに対して関節がどう動くかを計算する。各ポイントで逆座標変換を行う段階で、目標点が特異点上に存在するために唯一の解を求めることができず、複数の解が存在するという状況や、目標点が特異点近傍に存在するために関節角に過大な動作を要求する状況がでてくる。そこで目標点が特異点を通るかどうかを判断するチェックのルーチンを制御装置４はＣＰＵに行わせる。制御装置４は特異点またはその近傍を通ると判断した場合、特異点回避軌道生成部１９ａは特異点をよけるための回避軌道の生成処理をＣＰＵに行わせる。
【００２１】
また、特異点は解が無い点ではなく、解が複数あるので解が一つに決まらない点である。特異点の近傍では、解が一つ決まるが、この解は、それぞれ複数存在する特異点上の解の異なるものに近い解となる。このため、特異点の近傍を通過する時に関節角レベルで目標点が大きく変化することになり、実際には動作できない（軌道に追従するのに過大な速度が必要になる）ため、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法により特異点近傍の目標点を特異点から離すようにしている。
【００２２】
図１に示すように、このロボット１が対象物のハンドリング作業等を行う際には、本体下部に設けられた車輪２を移動サブシステム２２が駆動させてロボット１の胴部５を作業実施場所に移動させ、本体に搭載されたマニピュレータ７を制御装置４が駆動して目的の作業を実施する。マニピュレータ７の手先には、実際にハンドリング等の作業を行うハンド等のエンドエフェクタ８が設けられている。また、ロボット頭部６には作業対象物の位置及び姿勢の認識と計測とを行うためのカメラが取付けられている。多くの場合、移動ロボット１に搭載したマニピュレータ７は、人間の腕を模した多関節型マニピュレータであるため、作業空間内にいくつかの特異点を有している。作業空間とは、マニピュレータ先端の並進と回転を表すための位置の３自由度及び姿勢の３自由度からなる６次元の空間である。制御装置４が、手先を、指示された軌道に沿ってマニピュレータ７を動作させる際に、手先の軌道上または手先の軌道の近傍に特異点が存在すると、その特異点における目標手先位置姿勢を実現するための関節角座標系目標値への逆座標変換計算を適切に行えなくなるため、特異点回避軌道を生成する必要が生じる。
【００２３】
ここで、「関節角座標系目標値への逆座標変換計算を適切に行えない、」とは、特異点上の場合は唯一の解が得られるが、特異点近傍の場合は唯一の解が得られても、その解を実現するために関節角を短時間で過大に動作させるような動作を要求することになる、ということを指す。制御装置４は、特異点回避軌道生成部１９ａに以下に述べる２つの円錐の円錐面上の母線を回避軌道とする回避軌道生成を行わせるようにしている。
【００２４】
図３は、マニピュレータ７が手先動作軌道を生成する際に、生成した直線軌道上またはその近傍に特異点が存在する場合の回避軌道生成方法を説明するための図である。特異点回避軌道生成部１９ａは、マニピュレータ７の現在手先位置Ｐと最終目標手先位置Ｇとを結んだ直線軌道上に各サンプル時刻における目標点を設定する。記号「×」は設定された手先の目標点である。
【００２５】
通常、直線軌道上の各サンプル時刻における目標点は、現在手先位置Ｐと最終目標手先位置Ｇ近傍の加減速区間とを除いては、手先が等速移動するために等間隔に設定される。Ｓ０は特異点の近傍の点であり、この軌道上に設定された各サンプル時刻における目標点の１つを表す。このＳ０の上またはＳ０の近傍に特異点が存在する場合、このＳ０又はこの近傍点の目標手先位置姿勢を実現するための関節角座標系目標値が逆座標変換計算により適切に得られない場合が生じる。そこで、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法では、特異点回避軌道生成部１９ａが、それぞれＳ０を中心とした底面の半径Ｌ、高さ（円錐高さ）Ｈの２つの円錐２３、２４を設ける処理を行って、これらの円錐２３、２４に共通する一底面の円周上の点Ｓ１を設定する処理を行う。特異点回避軌道生成部１９ａはこの点Ｓ１を経由し、円錐２３、２４の各円錐面に沿った回避軌道を生成する。これにより、生成軌道が特異点近傍を回避することができる。
【００２６】
しかし、ここで生成される円錐２３、２４の各底面の半径Ｌ、高さＨが任意の値に設定されると、これらの円錐２３、２４の各円錐面に沿って設定された回避軌道上の各サンプル時刻における目標点の間隔を、回避軌道部分以外の直線軌道部と同じにすることができず、全体として一定間隔の等速軌道とすることができない。また、円錐２３、２４の各円錐面に沿った回避軌道上の各目標点の間隔が円錐２３の頂点よりも手前の直線軌道部分の目標点間隔と同じに設定されると、今度は回避軌道とこの回避軌道よりも後段の直線軌道部分との接続部で目標点が一定間隔をもって接続できなくなるため、制御装置４は回避軌道以降に位置する直線軌道上の各目標点を再度設定しなおす必要が生じる。
【００２７】
そこで、特異点回避軌道生成部１９ａは、回避軌道として生成する円錐の底面半径Ｌ、高さＨを、各サンプル時刻における目標点の間隔が等間隔になるように設定するようにしている。図４は、２つの円錐の各円錐面の一母線に沿って設定された回避軌道についての底面半径Ｌ、高さＨの第１の例を示す図である。具体的には、同図に示すように、直線軌道部分の目標点の間隔を１とした時、回避軌道として生成された円錐２５、２６の各底面の半径をＬ＝√３とし（√とそれに続く数字は、この数字の平方根を表す）、高さをＨ＝１と設定すると、円錐２５、２６の各円錐面に沿った回避軌道部分の距離についてＳ＝２が得られる。各目標点がこれらの目標点の前後の直線軌道部分と円錐面に沿った回避軌道部分でスムーズに繋げることができる。
【００２８】
特異点回避軌道生成部１９ａは、このような２つの円錐２５、２６の各底面半径Ｌと各高さＨの組合せを複数設定することができるようにされている。図５は、２つの円錐の各円錐面の一母線に沿って設定された回避軌道についての底面半径Ｌ、高さＨの第２の例を示す図である。同図には２つの円錐２７、２８の各底面の半径をＬ＝√５とし、高さをＨ＝２とした例が示されている。この時の円錐２７、２８の各円錐面の一母線に沿った回避軌道部分の距離Ｓ＝３である。図６に第３の例を示す。同図には、円錐２９、３０の各底面の半径をＬ＝√７、高さＨ＝３として例が示されている（この時の円錐２９、３０の各円錐面の一母線に沿った回避軌道部分の距離Ｓ＝４である）。図４から図６に示すように回避軌道が生成可能である。
【００２９】
このような、各サンプル時刻における目標点の間隔が等間隔になるような回避軌道が生成できる２つの円錐についてそれぞれ、底面半径Ｌ、高さＨ、および回避軌道部分の距離Ｓの組合せを図７に示す。図７は生成される２つの円錐の円錐面の一母線に沿った回避軌道についての底面半径Ｌ、高さＨの設定順位を表す表図である。ここで、２つの円錐の底面半径Ｌ、高さＨの組は、各円錐の底面半径Ｌの値が小さい順に並べて表示してある。多くの場合、マニピュレータの動作軌道に特異点回避軌道を付加する際には、元の動作軌道からのズレを極力小さくすることが望ましいため、特異点回避軌道生成部１９ａは、この表図に示された２つの円錐の底面半径Ｌ、高さＨの組の順に回避軌道を設定していくようにしている。すなわち、特異点回避軌道生成部１９ａは、２つの円錐の底面半径Ｌが小さい順に回避軌道を設定し、回避軌道上の各目標手先位置姿勢を実現するための関節角座標系目標値の逆座標変換計算が適切に行えない場合、この計算を行った底面半径の順位の次の順位を持つ底面半径Ｌの値を選択するようにしている。
【００３０】
また、同一の底面半径Ｌ、高さＨの２つの円錐の円錐面の一母線に沿った回避軌道を特異点回避軌道生成部１９ａが複数生成することが可能である場合、特異点回避軌道生成部１９ａは回避軌道上の各目標手先位置姿勢を実現するための関節角座標系目標値の逆座標変換計算を実施した後、これらの回避軌道のうち、より多くの関節の各軸の動作量が制限範囲内で最大となる回避軌道を選択することとしている。これは、より多くの関節の各軸の動作量が大きい回避軌道のほうが、特異点からの回避程度が高いと判断されるためである。「より多くの関節の各軸の動作量が最大となる」とは、１つの軸だけが沢山動くのではなく、なるべく多くの複数の軸が沢山動く解を選ぶようにすることを指す。
【００３１】
以上説明した方法により、多関節型のマニピュレータ７が手先動作軌道を生成する際に、マニピュレータ７の現在手先位置姿勢と最終目標手先位置姿勢とを結んだ直線軌道上またはその近傍に特異点が存在する場合、制御装置４が回避軌道を生成することで、元の生成した直線軌道からのズレが小さく、かつ各サンプル時刻における目標点の間隔が等間隔で、その近辺において、その後の軌道を全て再生成せずに、マニピュレータ７を一定の速度で動作させられる動作軌道を得られるようになる。
【００３２】
以下、図８を用いて制御装置４がＣＰＵに実行させるアーム制御の処理を述べる。図８は特異点回避軌道生成部１９ａが行うアーム制御処理を説明するためのフローチャートである。
【００３３】
まず、ステップ１０１において、特異点回避軌道生成部１９ａはアーム手先の目標位置姿勢及び移動時間（または移動速度）を入力される。ステップ１０１において、特異点回避軌道生成部１９ａはアーム各軸の現在位置（現在の回転角度）情報を、各関節の軸位置検出センサから読み込み、順座標変換計算を行ってアーム手先の現在位置姿勢を計算する。その後、ステップ１０３において、特異点回避軌道生成部１９ａは、アーム手先の現在位置姿勢からアーム手先目標位置姿勢に指定移動時間または移動速度で至るアーム手先位置姿勢の目標軌道を計算する。この時、多くの場合は手先現在位置から手先目標位置へ至る直線軌道を制御装置４は生成し、姿勢については手先現在姿勢から手先目標姿勢へ一様に変化させる。
【００３４】
アーム手先位置姿勢の目標軌道が生成されると、ステップ１０４において、特異点回避軌道生成部１９ａは、この目標軌道上の各点（各時刻における目標位置姿勢）を実現するためのアーム各軸の目標位置を、逆座標変換計算により求める。そして、ステップ１０５において、ステップ１０４で計算されたアーム各軸の目標位置が過大に変化していたり、あるいは唯一の目標位置が計算できないなど特異点近傍にあるかどうかのチェックをこの特異点回避軌道生成部１９ａは行う。
【００３５】
ステップ１０５において、特異点近傍にあると判断されると、“近傍”と付されたルートを通り、ステップ１０６において特異点回避軌道生成部１９ａは特異点の回避軌道を生成し、ステップ１０７において、生成された特異点回避軌道を実現するための各点の各時刻における各軸目標位置を計算する。本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法は、この特異点回避軌道生成部１９ａにおいて、特異点が存在する位置よりも後の軌道を全て再生成せずに、マニピュレータ７を一定の速度で動作させることを目的としたものである。
【００３６】
以上の処理により、特異点近傍を通らない軌道が準備されると、ステップ１０８において、制御装置４は各時刻におけるアーム各軸の目標値を達成するようにアームを制御する。これにより、アームは手先目標位置姿勢軌道に沿って動作することになる。そして、ステップ１０９において、特異点回避軌道生成部１９ａは、アーム手先が目標位置姿勢に到達したかどうかの判断を行い、アーム手先が目標位置姿勢に到達していない間は“未達”と付されたルートを通り、ステップ１０８に進む。アームの動作は目標位置姿勢に到達するまで続けられる。最終的に、アーム手先が目標位置姿勢に到達したところでアーム動作は終了する。ステップ１０９において、アーム手先が目標位置姿勢に到達したと判定されると、“到達”と付されたルートを通り、特異点回避軌道生成部１９ａは処理を終える。なお、ステップ１０５において、特異点近傍ではないと判断されると、“近傍でない”と付されたルートを通り、ステップ１０８に進む。
【００３７】
このようにして、本実施形態に係る多関節型マニピュレータの制御装置及び多関節型マニピュレータの手先動作軌道生成方法によれば、それぞれ直線軌道に沿って並ぶ複数の目標点からなる手先の動作軌道を生成した後、特異点やその近傍への手先の接近を検出してこの手先の回避軌道を生成したときに、手先の動作軌道に含まれる複数の目標点のうち、この特異点以降に位置する目標点を手先の動作軌道としてそのまま利用することができるようになる。
【００３８】
直線軌道上を例えば３秒間あるいは５秒間で手先を動かそうとする状況で、１／１００秒に１回制御する場合、制御装置４は１／１００秒毎の目標位置を生成する。各１／１００秒は制御周期であり、これらの制御周期毎に計算して得た目標点の間隔が等しくなるようにされている。つまり、手先が１／１００秒毎にこの手先が等速で目標位置に行くようにして、マニピュレータ７は直線軌道の上を通って各目標地点に行くよう指令されている。例えば３秒の移動時間が与えられた場合、制御装置４は３００個の目標値を生成する。制御装置４は、各関節の関節軸の回転角がこれらの目標値になるように制御する。制御装置４が特異点の存在を検知した場合、非特許文献１に記載の従来例では、特異点近傍において、多少直線に沿っていく追従性を犠牲にして、特異点から少し離れたところを関節形ロボットアームが通るように制御している。
【００３９】
本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法では、直線軌道からやや曲がった別の軌道をマニピュレータ７が通るようにしている。なるべく、マニピュレータ７が真っ直ぐに進めるところはそのまま行かせ、特異点が存在する箇所だけをよけるようにしている。よけ方の一つとして、制御装置４は円錐軌道を生成するのである。制御装置４は、特異点またはその近傍の目標点を中心に、半径Ｌの円をとり、この円の円周面を底面とする２つの円錐を生成している。マニピュレータ７は２つの円錐の円錐面上の回避軌道を進み、予め生成されておいた次の目標値まで進められる。
【００４０】
従来技術では、マニピュレータが各目標点を一定の速度で動くものと仮定すると、これらの目標点が並ぶ間隔は等間隔であるため、マニピュレータは、一定速度で各目標点を辿っていくことになる。最初に制御装置が等間隔に作成した点が目標点として残っているため、マニピュレータが２つの円錐状の軌道を通って元の直線軌道に戻ったときに、マニピュレータは特異点がない状況で作成されて後、残っている目標点を手先が通るため通過経路が長くなり、あるいは短くなり、その点において等速動作ができなくなることがある。このため、従来技術では、特異点回避軌道後に目標点の再生成を行わなければならない。この理由は、開始地点から特異点の手前までは直線軌道を等間隔で進み、特異点の付近だけ、空間的及び時間的に間延びすること、あるいは縮まることが起きるからである。
【００４１】
これに対して、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法では、マニピュレータ７がよけたときの円錐面上の目標点の間隔が特異点に至るまでの目標点間隔と等しくされている。つまり、回避軌道は、この回避軌道の前後の各直線軌道の延長線になっている。そうすると、マニピュレータ７が特異点を避けて、母線上の経路を通った後、元の直線軌道に戻ってきたときに、最初に計算したときに得られた目標点をマニピュレータ７はそのまま使える。特異点よりも手前に設定された目標点と、特異点よりも以降に設定された目標点とが、円錐軌道を介してつながっている。従って、制御装置４では、円錐軌道を含む開始地点から最終地点までの各目標点を、マニピュレータ７は空間的及び時間的に等間隔で進むことができる。
【００４２】
円錐の形状について述べると、直線軌道の軸線を含む平面で円錐を切断したときの円錐断面の直角三角形の辺の比は、母線の長さＳと、円錐高さＨとが整数比の関係になるようにした中の一番小さい円錐を制御装置４は採るようにしている。母線の長さＳが円錐高さＨの２倍になると、２本の母線上に目標点が２つづつ増える。底面半径の整数倍の長さを母線長に設定することで、同じ目標点間隔をとれる円錐の形状は多数あり、その組合せは図７で説明したとおりである。円錐高さＨと母線の長さＳが整数比の関係になっていると、回避軌道から計算済みの直線軌道へ手先が戻ってきたときに回避軌道の終点と直線軌道の始点とがつながる。
【００４３】
このように、元々特異点がない状況で作成された目標点を利用してマニピュレータ７を途中で進ませて、特異点が出た場合、特異点よりも後に位置する目標点は最初に計算して得たものをそのまま利用することができる。直線軌道全体のうち、マニピュレータ７が通りにくい部分だけ回避軌道が作成される。具体的には、制御装置４は、ｎ番目、ｎ＋１番目、ｎ＋２番目、ｎ＋３番目、ｎ＋４番目…の各目標点を１／１００秒毎にマニピュレータ７へ与えている。これらのうちｎ番目の目標点が特異点またはその近傍であるとした場合、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法では、制御装置４がｎ番目の目標点の存在する部分の前後ｍ個分の行程だけ、０番目からｎ−ｍ番目の目標点へ至るまでの間隔の２倍あるいは整数倍の回避軌道を生成している。直線軌道の途中に追加された目標点が入るが、ｎ＋ｍ番目以後の目標点は、最初に計算した計算済みの値をマニピュレータ７はそのまま使える。
【００４４】
仮に生成する回避軌道が０番目からｎ−ｍ番目の目標点へ至るまでの間隔の２倍あるいは整数倍となっておらず、目標点間の距離が長い場所が存在すると、等間隔に進行していたマニピュレータの軌道が急に間延びしてしまう。仮にこれが設定された場合、マニピュレータは同じ時間間隔で特異点近傍の手前まで進行しているため、同じ時間間隔で急に長い距離を進むことになる。これでは手先の動作軌道を等速軌道にするという制御を行うことができない。また、仮に生成する回避軌道の目標点間の距離が０番目からｎ−ｍ番目の目標点へ至るまでの間隔より短い場所が存在すると、与えられた時間では手先はそれまでより少ししか進めない。仮にこれが設定された場合、手先は０番目からｎ−ｍ番目までずっと一律に来ていたのに、ｎ番目の近辺だけ縮まることとなる。やはりこれでは手先を等速軌道上で動かせない。等速軌道上で動かせない場合、全行程を通じて目標点間隔を一定間隔に保つためには、目標値の再作成を要する。後段の目標点を全部作り直さなければならず、計算上不利である。
【００４５】
本実施形態に係る多関節型マニピュレータの制御装置４が、特異点や特異点の近傍だけ回避軌道を使って軌道を閉じることによって、全体の軌道は等間隔な目標点間隔を得られるようになる。円錐面を通る点が数点動作軌道に追加されるだけで、後段には予め計算しておいた目標点と一緒の目標点を使える。移動時間は予め与えられて決まっているため、手先は追加された数点を通過することによる時間ロスはあるものの、制御装置４は最初に作成した目標点を使えるため、再計算に要する計算量の節約になる。
【００４６】
また、特許文献１に記載の従来例は特異点回避ではなく障害物回避のための軌道生成方法であり、従来例によるマニピュレータの軌道生成方法と、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法とは、回避軌道を生成するという点では同じである。しかし、障害物回避はマニピュレータの外部環境の問題に対処するものであり、特異点回避はマニピュレータの内部の問題に対処するために行うものである。特許文献１に記載のマニピュレータの軌道生成方法と、本実施形態に係る多関節型マニピュレータの手先動作軌道生成方法とは、根本的に目的が異なる。
【００４７】
なお、上記実施形態の説明は、本発明を説明するための例示であって、特許請求の範囲に記載の発明を限定するものではない。又、本発明の各部構成は上記実施形態に限らず、特許請求の範囲に記載の技術的範囲内で種々の変形が可能である。上記の実施形態では、多関節型マニピュレータはロボット１に適用されたが、本発明は、生産ラインで物体を運ぶロボットや、グラインダなど産業用ロボット全般に適用可能である。上記の実施形態では、２つの円錐は同じ円錐角を持つものであったが、本発明は、互いに異なる円錐角を持ち、且つ底面共通とする２つの円錐を回避軌道として設定してもよい。上記実施形態では、動作軌道の対象はマニピュレータ７先端の手先であったが、本発明は手先以外のマニピュレータ７の動作点について回避軌道を生成することもできる。駆動装置にはモータの代わりに他のアクチュエータを用いても良い。本発明は、これらの変更を加えて実施した発明に対しても、何ら優位性を損なうものではない。
【００４８】
また、上記実施形態では、直線軌道の取得計算をロボット１内部の制御装置４が行い制御装置４が回避軌道を生成していたが、本発明の実施形態に係る多関節型マニピュレータの手先動作軌道生成方法は、直線軌道の取得計算をロボット１外部の別の装置が行って、この装置から直線軌道データをロボット１が読み込み、制御装置４が回避軌道の計算だけを行うようにしてもよい。
【００４９】
また、上記実施形態に開示されている複数の構成要素の適宜な組合せにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。
【００５０】
上記ロボット１では胴部５と移動機構３との間に胴部旋回軸が設けられていたが、胴部５と移動機構３との間に回転軸を設けずに、胴部５の回転を、車輪２の回転で実現するようにしてもよい。
【図面の簡単な説明】
【００５１】
【図１】本発明の実施の形態に係る多関節型マニピュレータを搭載したロボットの一例を示す外観斜視図である。
【図２】図１のロボットの制御システムのブロック図である。
【図３】生成した直線軌道上に特異点またはその近傍が存在する場合の回避軌道生成方法を説明するための図である。
【図４】２つの円錐の各円錐面の一母線に沿って設定された回避軌道についての底面半径Ｌ、高さＨの第１の例を示す図である。
【図５】２つの円錐の各円錐面の一母線に沿って設定された回避軌道についての底面半径Ｌ、高さＨの第２の例を示す図である。
【図６】２つの円錐の各円錐面の一母線に沿って設定された回避軌道についての底面半径Ｌ、高さＨの第３の例を示す図である。
【図７】生成される２つの円錐の円錐面の一母線に沿った回避軌道についての底面半径Ｌ、高さＨの設定順位を表す表図である。
【図８】特異点回避軌道生成部が行うアーム制御処理を説明するためのフローチャートである。
【符号の説明】
【００５２】
１…ロボット（多関節マニピュレータを搭載した移動ロボット本体）、２…移動ロボット車輪、３…移動機構、４…制御装置（多関節型マニピュレータの制御装置）、５…胴部、６…頭部、７…マニピュレータ（多関節型マニピュレータ）、８…エンドエフェクタ（手先）、９…手首関節部、１０…第１アーム、１１…肘関節部、１２…第２アーム、１３…肩関節部、１４〜１６…関節角度センサ、１７…制御システム、１８…システム制御部、１９ａ…関節動作軌道生成部、１９ｂ…特異点回避軌道生成部、１９ｃ…制御部、２０…胴回転サブシステム、２１…頭部回転サブシステム、２２…移動サブシステム、２３〜３０…円錐（回避する軌道として生成する２つの円錐）。
【図１】
2010076058.tif 000003
【図２】
2010076058.tif 000004
【図３】
2010076058.tif 000005
【図４】
2010076058.tif 000006
【図５】
2010076058.tif 000007
【図６】
2010076058.tif 000008
【図７】
2010076058.tif 000009
【図８】
2010076058.tif 000010