(57)【特許請求の範囲】
【請求項１】
複数の関節により複数のリンクが手先まで連結された冗長マニピュレータの制御方法であって、
複数の前記関節を複数の非冗長関節と複数の冗長関節とに切り分けて、前記複数の冗長関節に対応する複数の冗長関節角であるθａとθｂとを選択する工程と、
前記関節の関節角の適性を数値化するための評価関数に基づいて前記θａの解析解を求める工程と、
前記θａの前記解析解に基づいて、前記評価関数における前記θｂの取りうる範囲を減縮したグリッド探索範囲を設定する工程と、
設定された前記グリッド探索範囲で前記評価関数のグリッド探索を行い、前記θａのグリッド最適値αと前記θｂのグリッド最適値βとを導出する工程と、
前記評価関数に基づいて、前記αと前記βとを初期値として最適化法により前記θａと前記θｂとの最適解を計算する工程と、を有する、
冗長マニピュレータの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数のリンクを連結する冗長関節と非冗長関節とからなる複数の関節を有する冗長マニピュレータにおいて、逆運動学で各関節の関節角を設定するための制御方法に関する。
【背景技術】
【０００２】
複数のリンクが複数の関節で連結されたマニピュレータがある。多関節を有するマニピュレータを制御する場合、マニピュレータの手先の位置と姿勢を計算するために、それぞれの関節の角度を指定する必要がある。それぞれの関節の値は行列計算によって一意に決定される。この問題は、運動学問題と呼ばれている。一方、マニピュレータの手首の位置と先端の姿勢が与えられた場合、それらを満足させるマニピュレータの各関節角を求めることは逆運動学問題と呼ばれている。マニピュレータは、逆運動学問題で解を得られる６自由度の関節を有するものが多く用いられている。これに対して７自由度以上の関節を有するマニピュレータは、冗長関節を有しており、冗長マニピュレータと呼ばれている。冗長マニピュレータでは、冗長性のため、逆運動学の解析による解が一意に定まらないという問題がある。
【０００３】
特許文献１には、複数の関節を有する冗長マニピュレータにおいて、複数の関節から１個または複数の冗長関節を自動的に割り当て、逆運動学の数値解を計算し、これによって冗長マニピュレータを制御する制御方法が記載されている。この制御方法によると、複数の関節を冗長関節と非冗長関節とに切り分けて、冗長関節の関節角と非冗長関節の関節角についての評価関数、等式拘束条件および不等式拘束条件を設定する。その後、この制御方法は、冗長関節の各関節角の初期値または変更値を設定し、得られた冗長関節の各関節角に応じて手先の位置と方向に対応する非冗長関節の各関節角を逆運動計算により求めてマニピュレータを制御している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第４７３５７９５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
図６に示されるように、特許文献１に記載された方法において、数値最適化によって解を求める際にステップＳ３３で初期値を与える必要がある。しかし、この方法において、その初期値の与え方が考慮されていないため、初期値が解の近くでないと計算に時間を要する局所解に落ち込む虞がある。例えば、図７に示されるように、数値最適化は勾配方向に解が進んで行くため、最適解でなくても極値にたどり着くと計算が終了する。この方法によると、位置ａにある初期値から運良く計算が開始された場合のみに解が最適解にたどり着くことができる。例えば、初期値の計算が位置ｂまたは位置ｃから開始されると、解は評価関数の勾配に沿ってIIまたはIIIの極値にそれぞれ収束する虞がある。これは、数値最適化の計算の際に冗長軸の存在範囲を考えていないためである。また、この方法によると、解がない場合、判定が遅くなるという問題があり、また、遠い手先の計算に時間がかかる。従って、数値最適化で計算を行う場合、適切な初期値を選択することが重要な課題となる。
本発明は、複数の冗長関節と複数の非冗長関節とを有するマニピュレータにおいて冗長関節の関節角を数値最適化法によって解く際に、適切に初期値を与えることができる冗長マニピュレータの制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、複数の関節により複数のリンクが手先まで連結された冗長マニピュレータの制御方法であって、
複数の前記関節を複数の非冗長関節角と複数の冗長関節角とに切り分けて、冗長関節角であるθａとθｂとを選択する工程と、
予め定められた評価関数に基づいて前記θａの解析解を求める工程と、前記解析解に基づいて、前記評価関数におけるグリッド探索範囲を設定する工程と、
設定された前記グリッド探索範囲で前記評価関数のグリッド探索を行い、前記θａのグリッド最適値αと前記θｂのグリッド最適値βとを導出する工程と、
前記評価関数に基づいて、前記αと前記βとを初期値として最適化法により前記θａとθｂとの最適解を計算する工程と、を有する。
【０００７】
本発明はこのような構成により、関節角の数値最適化による最適解を計算するための初期値を導出するグリッド探索の範囲を大幅に絞り込むことができ、また、最適解を計算するための初期値が最適解から離れていても高速に最適解を計算することができる。
【発明の効果】
【０００８】
本発明によると、複数の冗長関節と複数の非冗長関節とを有するマニピュレータの制御方法において、冗長関節の関節角を数値最適化法によって解く際に、適切に初期値を与えることができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施形態にかかるマニピュレータを概略的に示した図である。
【図２】マニピュレータの内部構成を示したブロック図である。
【図３】グリッド探索範囲を示した図である。
【図４】評価関数において、最適解の計算を開始する初期値の位置を示した図である。
【図５】マニピュレータの制御方法を示したフローチャートである。
【図６】関連するマニピュレータの制御方法を示したフローチャートである。
【図７】関連するマニピュレータの制御方法における冗長関節の数値最適化の最適解を求める際の初期値の位置を示した図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明に係る冗長マニピュレータの実施形態について説明する。
【００１１】
図１に示されるように、マニピュレータ１０は、基端Ａ０に連結された複数のリンクＡ１〜Ａ７と、複数のリンクＡ１〜Ａ７との間を回転自在に連結する複数の関節Ｂ１〜Ｂ８と、連結された複数のリンクＡ１〜Ａ７の最先端に接続された手先２０とを有している。複数の関節Ｂ１〜Ｂ８は、８個設けられているため、マニピュレータ１０は、２個の冗長関節を有する８自由度の冗長マニピュレータである。本実施形態では、８個の関節を例示しているが、関節は１２個程度あってもよい。関節Ｂｎ（ｎは１〜８の整数）の回転軸の配置は、連結されたアームＡｍ（ｍは１〜７の整数）または手先２０の配置された位置によってそれぞれ異なる。複数の関節Ｂ１〜Ｂ８は、それぞれモータ等を有する駆動手段Ｄ：Ｄ１〜Ｄ８（不図示）によって駆動される。
【００１２】
図２に示されるように、マニピュレータ１０の内部構成は、各関節Ｂ１〜Ｂ８の関節角θ１〜θ８を数値化するための評価関数５を記憶する記憶手段４と、評価関数５に基づいて各種数値を算出する演算手段１１と、演算手段１１の算出した数値に基づいて各関節角θ１〜θ８を回転駆動する駆動手段Ｄと、各手段を総合的に制御するための制御手段２とを有する。
【００１３】
以下、演算手段１１が有する複数の構成要素について説明する。演算手段１１は、複数の関節Ｂ１〜Ｂ８を複数の非冗長関節角と複数の冗長関節角とに切り分ける冗長関節選択部１２を有する。冗長関節選択部１２は、複数の関節Ｂ１〜Ｂ８から冗長関節を選択する。冗長関節の選択方法については、冗長関節選択部１２は、冗長関節であるθａとθｂ（ａおよびｂは１〜８の整数）とを選択する。冗長関節選択部１２は、θａとθｂとを後述するように、非冗長関節の解析解の個数が少なくなるように選択する。θａとθｂとは、例えば、θ３とθ４である。θａとθｂとは連続した関節角でなくてもよい。冗長関節選択部１２は、θａとθｂとを冗長関節選択部１２に接続された解析解算出部１３に出力する。
【００１４】
解析解算出部１３は、予め記憶手段４に記憶された評価関数５：ｆｎ（θ）に基づいてθａの解析解を算出する。選択的にθａとθｂとを選んだ場合、解析解の候補Ｔは、以下のように複数存在する。
Ｔ＝ｆ１（θｏｔｈｅｒ，θａ，θｂ）
Ｔ＝ｆ２（θｏｔｈｅｒ，θａ，θｂ）
…
Ｔ＝ｆｎ（θｏｔｈｅｒ，θａ，θｂ）
（θｏｔｈｅｒはθａ，θｂ以外の関節）
解析解算出部１３は、解析解の複数の候補Ｔからθｏｔｈｅｒの解析解が少なくなる候補Ｔを選択する。本実施形態では、θｏｔｈｅｒは、θ３とθ４以外のθ１，θ２，θ５〜θ８である。
【００１５】
不等式算出部１４は、θａの解析解の結果に基づいて評価関数５におけるθｂの取りうる範囲、即ち、冗長軸であるθｂの範囲を絞る不等式の解を算出する。不等式算出部１４は、この不等式の解により、評価関数５におけるグリッド探索範囲Ｐ（図３参照）を設定する。グリッド探索範囲Ｐは、不等式の解により領域Ｓが除外された範囲である（図３参照）。不等式算出部１４は、グリッド探索範囲Ｐを不等式算出部１４に接続された初期値算出部１５に出力する。
【００１６】
初期値算出部１５は、グリッド探索範囲Ｐの端部からグリッド探索し、評価関数５が最小となるようなθａのグリッド最適値αと、θｂのグリッド最適値βをと導出する。グリッド探索において、外点ペナルティ法を用いることができる。外点ペナルティ法として、例えば精密ペナルティ法、又は不連続ペナルティ法を用いることができる。初期値算出部１５は、グリッド最適値α、βを初期値算出部１５に接続された最適解算出部１６に出力する。
【００１７】
最適解算出部１６は、グリッド最適値α、βを初期値として最適化法により評価関数５に基づいてθａとθｂとの最適解を算出する。このとき、最適化法として、Ｈｏｏｋｅ−ａｎｄ−Ｊｅｅｖｅｓ法、Ｒｏｓｅｎｂｒｏｃｋ法、準Ｎｅｗｔｏｎ法、最急降下法、共役勾配法、Ｌｅｖｅｎｂｅｒｇ-Ｍａｒｑｕａｒｄｔ法、等を用いることができる。最適解の算出過程において、直線探索法を用いることができる。直線探索法において、黄金分割法、二次フィット法、二分探索法、Ｎｅｗｔｏｎ探索法、Ａｒｍｉｊｏ規則法、等を用いることができる。直線探索法においても外点ペナルティ法を用いることができる。
【００１８】
図４に示されるように、最適化法によるθａとθｂとの最適解を計算する際、評価関数５において初期値αの位置は最適解の近傍となる。このため、本発明によると、出力される解が評価関数５の他の極値（例えば、II、IIIの極値）に落ち込むことがない。上記のように演算手段１１では、数値解と解析解とを扱うため高速に計算結果を得ることができる。制御手段２（図２参照）は、計算結果に基づいて駆動手段Ｄ１〜Ｄ８（図２参照）を駆動し、手先２０（図１参照）の位置および角度を調整する。
【００１９】
次に、本発明に係るマニピュレータ１０の制御方法のフローについて説明する。
【００２０】
図５に示されるように、冗長関節選択部１２は、関節Ｂ１〜Ｂ８を複数の非冗長関節角と複数の冗長関節角とに切り分けて非冗長関節角であるθａとθｂとを選択する（Ｓ１００）。解析解算出部１３は、予め定められた評価関数５に基づいてθａの解析解を求める（Ｓ１０１）。不等式算出部１４は、θａの解析解の結果に基づいて評価関数５におけるθｂの取りうる範囲、即ちθｂに対して予め定められた評価関数５におけるグリッド探索範囲Ｐを設定する（Ｓ１０２）。
【００２１】
初期値算出部１５は、設定されたグリッド探索範囲Ｐで評価関数のグリッド探索を行い、θａのグリッド最適値αとθｂのグリッド最適値βとを導出する（Ｓ１０３）。最適解算出部１６は、評価関数に基づいて、α、βを初期値として最適化法によりθａとθｂとの最適解を計算する（Ｓ１０４）。最適解算出部１６は、最適化法の計算において、移動距離が一定以下であった場合、又は繰り返し計算が最大反復数に達した場合（Ｓ１０５：Ｙｅｓ）計算を終了し、それ以外の場合（Ｓ１０５：Ｎｏ）、ステップＳ１０４に戻り、最適解の計算を繰り返す。
【００２２】
上述したように、本発明によると、冗長マニピュレータにおいて、各関節角の大域的な最適解が短時間で計算できる。更に、本発明によると、関節角の最適解を計算するための初期値を導出するグリッド探索の範囲を大幅に絞り込むことができる。更に、本発明によると、最適解を計算するための初期値が最適解から離れていても高速に最適解を計算することができる。そして、本発明によると、解の存在判定が冗長マニピュレータの逆運動学であっても高速で行うことができる。また、本発明によると、探索がローカルミニマムに落ち込まず、またはその可能性を非常に低くすることができる。
【符号の説明】
【００２３】
２…制御手段  ４…記憶手段  ５…評価関数  １０…マニピュレータ  １１…演算手段  １２…冗長関節選択部  １３…解析解算出部  １４…不等式算出部  １５…初期値算出部  １６…最適解算出部  ２０…手先  Ａ０…基端  Ａ１-Ａ７…アーム  Ｂ１-Ｂ８…関節  Ｄ１-Ｄ８…駆動手段
【図１】
0006493090.tif 000002
【図２】
0006493090.tif 000003
【図３】
0006493090.tif 000004
【図４】
0006493090.tif 000005
【図５】
0006493090.tif 000006
【図６】
0006493090.tif 000007
【図７】
0006493090.tif 000008