JP5191738B2 - マニピュレータの制御方法および制御システム - Google Patents

Description

本発明は、複数の関節軸を有するマニピュレータを制御する技術に関する。

従来、多関節マニピュレータは、産業用や民生用のロボットアームなどに用いられている。このようなマニピュレータの位置制御に関する技術としては、様々な技術が存在する。

例えば、特定のマニピュレータの手先座標などを実現する各関節軸の変位や駆動力を、逆運動学を計算して求め、その変位や駆動力に基づいて制御を行う逆運動学を用いた技術がある。

この逆運動学を用いた従来技術の例を、図１９、図２０を用いて説明する。ここで、図１９は従来のマニピュレータの制御方法の概要を示す図であり、図２０は従来のマニピュレータの制御処理フロー図である。

図１９において、マニピュレータ５０１の各関節軸５１１には、エンコーダなどのセンサ５０２（センサＳ）と、関節軸５１１を個別に駆動するアクチュエータ５０３（アクチュエータＡ）が搭載されている。これらを用いて、マニピュレータ５０１の手先５０４を制御目標５０５（制御目標Ｇ）に対して位置制御する。マニピュレータ５０１は、全体の制御を行う全体制御部５０６を備えており、センサ５０２で計測した結果を用いてアクチュエータ５０３の制御指令値を算出している。なお、全体制御部５０６は全体評価処理部５０７、軌道計画処理部５０８、制御計算処理部５０９の３つの処理部を有している。手先５０４は制御目標５０５まで、制御軌道５１０のように位置制御される。

図２０において、制御処理フローは、まずステップＳ５１で、各軸に搭載されたセンサ５０２によって各関節軸５１１の変位（軸変位）、変位速度（軸速度）を計測する。次にステップＳ５２で、各関節軸５１１より収集した変位および変位速度の情報より手先５０４の座標を算出する。続いてステップＳ５３で、目標位置５０５に移動するための手先軌道５１０を算出する。続いてステップＳ５４で、目標軌道５１０を実現するための各関節軸５１１の変位および変位速度の制御指令値を算出する。続いてステップＳ５５で、制御指令値を実現するように各関節軸５１１のアクチュエータ５０３を駆動する。図１９の手先５０４が制御目標５０５に到達するまで、このようなステップＳ５１〜ステップＳ５５の処理を繰り返し実行している。なお、図２０の制御処理と図１９の制御要素との関係は、ステップＳ５１はセンサ５０２で実行し、ステップＳ５２は全体評価処理部５０７で実行し、ステップＳ５３は軌道計画処理部５０８で実行し、ステップＳ５４は制御計算処理部５０９で実行し、ステップＳ５５はアクチュエータ５０３で実行されている。

ここで、ステップＳ５２において手先５０４の座標を算出するために一般的に行われる計算式は、数式（１）のように表される。

Ｈ＝Ｔ（θ） ・・・（１）

数式（１）はリンク機構の運動学方程式と呼ばれ、Ｈは手先の座標ベクトル、θは関節軸の変位ベクトルを示す。

また、ステップＳ５４において、変位速度の制御指令を算出するために一般的に行われる計算式は、数式（２）のように表される。

θｃ＝Ｔ ^−１ （Ｈｏ） ・・・（２）

数式（２）はリンク機構の逆運動学方程式と呼ばれ、Ｈｏは目標軌道を実現する手先の目標座標ベクトル、θｃはその手先の目標座標ベクトルを実現する関節軸の変位ベクトルを示す。ただし、数式（２）は冗長な制御系では一意に解けないなどの問題があるため、数式（２）の代わりに別な計算方法を用いたものもある（例えば、特許文献１、特許文献２参照）。

特許文献１、特許文献２に開示された従来技術の例を、図２１Ａ、図２１Ｂ、及び図２２を用いて説明する。ここで、図２１Ａは従来のマニピュレータの制御処理フロー図であり、図２１Ｂは従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図であり、図２２は従来のマニピュレータの制御方法を示す図である。

特許文献１開示の制御方法は、図２１Ａ、図２１Ｂのように２つの方法のいずれかを用いてマニピュレータの手先座標を実現する関節軸の変位を決定している。図２１Ａにおいては、冗長マニピュレータ５２０において、各関節軸５２１の変位を均等に配分することで変位５２２を決定している。また、図２２Ｂにおいては、それぞれの各関節軸５２１において駆動させる関節軸５２３を必要次元だけ選択し、冗長マニピュレータ５２０を制御的に非冗長とすることで関節軸５２１の変位を算出可能としている。

特許文献２開示の制御方法は、制御部に逆運動学の関係を学習させることでアクチュエータの駆動力を算出できるようにしている。図２２において、目標軌道であるＰｄと実際の軌道Ｐの誤差から演算装置５３４でトルクＴを算出し、そのトルクＴによりアクチュエータ５３５を駆動している。この時、目標軌道Ｐｄを微分回路５３６に通したものとトルクＴとを多層神経回路５３７に入力することで多層神経回路５３７の学習を行っており、学習の結果であるその出力をトルクＴに加えてアクチュエータ５３５に入力することで制御を行っている。

また、逆運動学を用いずに分散的に制御を行う方法もある（例えば、特許文献３、特許文献４参照）。

特許文献３、特許文献４に開示された従来技術の例を、図２３、図２４を用いて説明する。ここで、図２３は特許文献３開示の制御方法を示す図であり、図２４は特許文献４開示の制御方法を示す図である。

特許文献３開示の制御方法は、マニピュレータによって多変数評価関数の各関節角の偏微分を用いて制御を行っている。図２３において、まず、マニピュレータ５４０は軌道５４９上を移動するように制御を行っている時の各関節５４１の関節角θ１、θ２を用いて多変数評価関数５４２を設定する。続いて、多変数評価関数５４２を任意の時点での各角度変数で偏微分した関節角の偏微分５４３を用いて、多変数評価関数５４２が所定の条件を満たすように分散的に各関節５４１の制御を行っている。

図２４において、特許文献４の方法は、制御装置によってマニピュレータ先端の位置に対応した関節部負荷閾値を用いて制御を行っている。位置検出装置５５３で検出されたマニピュレータの位置を計測し、動作制御装置５５４では計測された位置情報に従った負荷閾値を選択し、各関節角に設けられた負荷検出装置５５５が負荷閾値となるように関節角のサーボドライバ５５６を用いてアクチュエータ５５７を分散的に制御している。

従来、上記のような逆運動学を用いた制御や分散的な制御を行って、多関節マニピュレータの位置制御を行っている。

特開平０７−１６４３６０号公報 特開平０２−０５４３０４号公報 特開平０９−２０７０８７号公報 特開２０００−０９４３６８号公報

しかしながら、上記従来の制御方式を、高い自由度が要求される作業を行うマニピュレータに適応しようとする場合、以下のような課題がある。

従来の逆運動学を用いた方式では、非冗長で非線形要素が無い場合には使用できるが、環境によっては目的とする作業を行うことができない可能性がある。これは、マニピュレータの状態が一意に決まってしまうため、手先が目標位置まで到達不可能となるためである。一方、作業の自由度を上げるために冗長や非線形な駆動要素を取り入れると、逆運動学方程式である数式（２）が複雑になって一意に解けず、結果として制御ができない場合や、計算量が莫大になり、リアルタイムな制御が不可能になる場合がある。

特許文献１の場合は、冗長なマニピュレータに対して形状や自由度低減を利用して拘束条件を設け、その制御を可能としている。しかしながら、一方で、その拘束条件によって自由度を制限してしまうため、マニピュレータの作業性が制限されてしまう可能性がある。また、特許文献２の場合は、多層神経回路の学習によって冗長や非線形の駆動要素による計算上の問題やロバスト性を解決することは可能である。しかしながら、パラメータや教示データを適切に設定しないと、学習に時間を要する場合や計算結果が収束しない場合など、容易に適応できない可能性がある。

また、逆運動学を用いた方法では、いずれの場合も制御目標へ到達するため全軸の変位や駆動力が一意に決まるため、周囲に環境の影響や故障によって一つの軸が移動不可能な状態に陥ると全体の位置制御を実現できなくなる。

特許文献３は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、評価関数やその計算処理についてマニピュレータの形状や作業内容に応じて設定する必要であり、評価関数によっては作業が実行できない可能性がある。また、特許文献４は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、関節の負荷は位置姿勢だけでなく軌道や作業時にマニピュレータに取り付けるツールなどにより変化するため、作業毎に教示や学習により関節の負荷閾値を事前に登録する必要があり、想定外の状況には対応できない。

このようなマニピュレータの制御に関する課題は、マニピュレータの手先の位置制御が行われる場合だけでなく、手先の力制御が行われる場合にも生じる。

従って、本発明の目的は、上記課題を解決することにあって、周囲の環境や関節軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することにある。

上記目的を達成するために、本発明は以下のように構成する。

本発明の第１態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部に関する制御対象量と目標値との差分を計測する第１の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
夫々の上記軸制御部において、上記軸情報と上記差分情報とに基づいて、その余の軸では補正を行わないと仮定した上で、上記先端部に関する制御対象量が目標値に近づくように上記関節軸毎の現在の制御目標値を新たな制御目標値に補正し、当該新たな制御目標値に基づいて各関節軸を制御する第３の工程と、を備え、
上記先端部の制御対象量が上記目標値を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返す、マニピュレータの制御方法を提供する。

本発明の第２態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する第１の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記位置差分の位置差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
上記軸情報と上記位置差分情報とに基づいて、夫々の上記軸制御部で上記関節軸の軸変位と軸速度とを独立に補正する第３の工程と、を備え、
上記先端位置が上記目標位置を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返し、
上記第２の工程において、
上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記関節軸毎に送信し、
上記第３の工程において、
夫々の上記軸制御部は、自身以外の軸制御部とは独立に、
上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、
上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御方法を提供する。

本発明の第３態様によれば、上記複数の関節軸から取得された軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む軸情報を上記関節軸毎に作成し、
その後、上記第２の工程を実施する、第２態様に記載のマニピュレータの制御方法を提供する。

本発明の第４態様によれば、上記第２の工程において、上記先端位置と上記目標位置との距離に応じて設定された先端速度の上限値と実際の先端速度との速度比の情報を含む情報が上記関節軸に送信され、
上記第３の工程において、上記複数の軸制御部にて上記速度比に応じて上記軸速度を上記関節軸毎に補正される、第２態様に記載のマニピュレータの制御方法を提供する。

本発明の第５態様によれば、取得された上記関節軸の軸速度が全て０である場合に、上記目標位置の座標又は上記軸速度を一時的に変化させ、
その後、上記第２の工程を実施する、第３態様に記載のマニピュレータの制御方法を提供する。

本発明の第６態様によれば、取得された上記複数の関節軸が有する自軸の識別情報を統合して、上記複数の関節軸の接続状態の変化を検知する、第２態様に記載のマニピュレータの制御方法を提供する。

本発明の第７態様によれば、上記複数の関節軸の識別情報を上記全体制御部から上記複数の軸制御部に送信した後、上記複数の軸制御部において、予め保持された自軸の識別情報が上記送信された識別情報と一致した場合にのみ上記関節軸の制御を行う、第２態様に記載のマニピュレータの制御方法を提供する。

本発明の第８態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する第１の工程と、
上記複数の各関節軸の軸変位及び軸速度を含む軸情報と上記力差分の力差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
上記複数の関節軸の軸変位及び軸速度を含む情報と上記力差分の情報とに基づいて、上記軸制御部の上記関節軸の軸変位と軸速度とを上記関節軸毎に独立に修正する第３の工程と、を備え、
上記先端部に加わる接触力が上記目標接触力を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返し、
上記第１の工程において、
上記先端部の位置と目標位置との位置差分をさらに計測し、
上記第２の工程において、
上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として送信し、
上記第３の工程において、
夫々の上記軸制御部は、自身以外の軸制御部とは独立に、
上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、
上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御方法を提供する。

本発明の第９態様によれば、取得された上記複数の関節軸の軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む情報を上記関節軸毎に作成し、
その後、上記第２の工程を実施する、第８態様に記載のマニピュレータの制御方法を提供する。

本発明の第１０態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部に関する制御対象量と目標値との差分を計測する計測装置と、
上記差分の差分情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータを制御する全体制御部と、
上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記複数の軸制御部に夫々送信する送信装置と、を備え、
夫々の上記軸制御部において、上記軸情報と上記差分情報とに基づいて、その余の軸では補正を行わないものと仮定した上で、上記先端部に関する制御対象量が目標値に近づくように上記関節軸毎の現在の制御目標値を新たな制御目標値に補正し、当該新たな制御目標値に基づいて上記関節軸毎の駆動を制御する、マニピュレータの制御システムを提供する。

本発明の第１１態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する計測装置と、
上記位置差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータの位置を制御する全体制御部と、
上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記複数の軸制御部に夫々送信する送信装置と、を備え、
夫々の上記軸制御部において、自身以外の軸制御部とは独立に、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御システムを提供する。

本発明の第１２態様によれば、上記全体制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記記憶装置に記憶された上記軸情報に基づいて上記複数の関節軸の接続状態を監視する監視装置と、
上記監視装置により上記複数の関節軸の接続状態の変化を検出した際に、上記記憶装置に記憶された上記軸情報を更新する軸情報更新装置と、を備える、第１１態様に記載のマニピュレータの制御システムを提供する。

本発明の第１３態様によれば、上記各々の軸制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記関節軸が他の関節軸に接続されることにより、上記記憶装置に記憶されている上記軸情報を上記全体制御部に通知する通信装置と、を備える、第１１態様に記載のマニピュレータの制御システムを提供する。

本発明の第１４態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測すると共に、上記先端部の位置と目標位置との位置差分を計測する計測装置と、
上記力差分の情報と全ての上記関節軸の軸変位及び軸速度を含む情報に基づいて上記マニピュレータのトルク制御を行う全体制御部と、
上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として上記複数の軸制御部に夫々送信する送信装置と、を備え、
夫々の上記軸制御部において、自身以外の軸制御部とは独立に、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度のベクトルを上記関節軸毎に修正する、マニピュレータの制御システムを提供する。

以上のような本発明のマニピュレータの制御方法および制御システムによれば、制御目標（目標位置を含む所定の範囲）に向かう位置制御を軸単位に独立して行うため、冗長や非線形な駆動要素のあるマニピュレータにおいても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生せず、確実にマニピュレータの位置制御を行うことができる。

また、逆運動学の算出のための拘束条件の設定や、自由度の制限を行う必要も無いため、高い自由度を維持したまま制御を行うことができる。また、データの蓄積による学習も必要としないため、容易にマニピュレータを制御することもできる。

また、周囲の環境の影響や軸の故障によって一部の関節軸が稼動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮されるため、周囲の環境や関節軸の故障などの不確かさに依存しない制御を行うことができる。

従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。

本発明の記述を続ける前に、添付図面において同じ構成要素については同じ参照符号を付して、説明を省略している。以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

（第１実施形態）
以下に本発明の第１実施形態にかかるマニピュレータの制御システム及びその制御方法について、図面を参照しながら説明を行う。

図１は、本発明の第１実施形態のマニピュレータの模式図である。図１に示すように、マニピュレータ１は、７個の関節軸３〜９と、それぞれの関節軸を接続する８個のリンク１０〜１７と、リンク１０を介して一連に接続された関節軸３〜９を支持するベース２と、一連に接続された関節軸３〜９の先端に接続されたハンド（先端部あるいは手先）１８とを備えている。マニピュレータ１は、それぞれの関節軸３〜９の動作によって、ハンド１８を制御目標に位置させることが可能となっている。

図２は、本第１実施形態の制御システムの模式図である。制御システムは、それぞれの関節軸３〜９の動作制御を行うことで、マニピュレータ１のハンド１８を制御目標１９（Ｇ）に位置させるための制御を行う。図２に示すように、マニピュレータ１の制御システムは、各々の関節軸３〜９の駆動動作を軸毎に個別に制御する軸制御部２３〜２９と、マニピュレータ１の全体動作の評価を行う全体制御部の一例である全体評価部２０とを備えている。

軸制御部２３は、エンコーダなどのセンサを有し、関節軸３の状態（軸変位及び軸速度）を計測する計測装置３ｓと、関節軸３を駆動させるアクチュエータ３ａとを備えている。また、計測装置３ｓおよび全体評価部２０から入力される情報に基づいてアクチュエータ３ａを駆動制御するための情報を演算により作成し、アクチュエータ３ａの駆動制御を他の関節軸４〜９から独立して行う制御計算処理装置３ｃも備えている。なお、計測装置３ｓ及びアクチュエータ３ａは、関節軸３に装備されている。軸制御部２３と同様に、軸制御部２４〜２９にも、計測装置４ｓ〜９ｓ、アクチュエータ４ａ〜９ａ、及び制御計算処理装置４ｃ〜９ｃが備えられている。また、全体評価部２０は、全体評価処理装置２１を備えている。

図３は、本第１実施形態の制御処理フローを示す図である。図３においては、全体評価部２０における制御処理フローと、それぞれの軸制御部２３〜２９における制御処理フローとを分けて示すとともに、両者の関連について示している。

図３の制御処理フローにおいて、ステップＳ１にて、各関節軸３〜９の軸制御部２３〜２９が備える計測装置３ｓ〜９ｓにより関節軸の軸変位および軸速度を計測する。次に、ステップＳ２にて、各関節軸３〜９の軸制御部２４〜２９から全体評価部２０に計測された関節軸の軸変位および軸速度の情報が送信されて収集される。その後、ステップＳ３で、全ての関節軸３〜９の軸変位および軸速度の情報を用いて、マニピュレータ１のハンド１８の状態、および全体の状態変数（全体状態）を算出する。続いて、ステップＳ４で、ハンド１８の状態から、マニピュレータ１の位置制御状態を評価する（第１の工程）。その後、ステップＳ５にて、評価結果を参照して、マニピュレータ１のハンド１８が制御目標（目標位置）１９に到達しているか否かを示しているかどうかの判断を行う。制御目標１９に到達していない場合には、ステップＳ６にて、評価結果および全体状態の情報を、それぞれの軸制御部２３〜２９に送信する（第２の工程）。

次に、ステップＳ７にて、それぞれの軸制御部２３〜２９にて、全体評価部２０より送信されたマニピュレータ１の評価結果および全体状態の情報と、自軸の軸変位・軸速度の情報（ステップＳ１にて計測された情報）に基づいて、自軸のアクチュエータ３ａ〜９ａの制御指令値（修正量）を他の関節軸とは独立して算出する（第３の工程）。続いて、ステップＳ８にて、算出された制御指令値に基づいて、自軸のアクチュエータ３ａ〜９ａを駆動する。その後、ステップＳ１〜Ｓ８の制御処理が繰り返し実行される。ステップＳ５にて、評価結果が、マニピュレータ１のハンド１８が制御目標１９に到達していると判断された場合には、制御目標１９に対するマニピュレータ１の位置制御が完了する。なお、ステップＳ１、Ｓ７、及びＳ８の制御処理はそれぞれの軸制御部２３〜２９にて独立して実行され、ステップＳ２〜Ｓ６の制御処理は全体評価部２０にて実行される。また、全体評価部２０及び軸制御部２３〜２９においてステップＳ１〜Ｓ８までの処理が繰り返されるサンプリング時間は、１０ｍｓ以下の時間、例えば１ｍｓ程度の時間に設定される。ただし、このようなサンプリング時間の設定は、マニピュレータ１のハンド１８をどれくらいの時間で制御目標に近づけるのかという原因により決定される。

図３の制御処理フローに示すように、全体評価部２０とそれぞれの軸制御部２３〜２９との間における情報の受け渡し等の関係は次のようになっている。まず、ステップＳ１にて、それぞれの軸制御部２３〜２９で計測を行った関節軸の軸変位・軸速度の情報が、ステップＳ２にて、それぞれの軸制御部２３〜２９より送信されて、全体評価部２０にて収集される。また、ステップＳ６にて、全体評価部２０からそれぞれの軸制御部２３〜２９に、マニピュレータ１の評価結果と全体状態の情報が送信され、ステップＳ７にて、それぞれの軸制御部２３〜２９において、送信された評価結果および全体状態を参照して、アクチュエータの制御指令値の算出が行われる。

また、図２に示すマニピュレータ１の制御システムにおける各制御要素と、図３の制御処理フローとのより具体的な関係は次の通りである。まず、ステップＳ１は計測装置３ｓ〜９ｓにて実行され、ステップＳ２〜Ｓ６は全体評価処理装置２１にて実行される。さらに、ステップＳ７は制御計算処理装置３ｃ〜９ｃにて実行され、ステップＳ８はアクチュエータ３ａ〜９ａにて実行される。

なお、ステップＳ５において、マニピュレータ１のハンド１８が制御目標１９に到達したかどうかが評価結果として判断されることになるが、位置制御における制御幅や誤差を考慮して、制御目標１９を含む所定の範囲内にハンド１８が到達したかどうかを判断基準としている。

ここで、本第１実施形態の制御システムの動作の概要図を図４に示す。

図４において、全体評価部２０は、マニピュレータ１全体の制御状態の評価を行う全体評価処理装置２１と、複数の制御装置と相互通信可能な通信装置３０を備えている。また、それぞれの関節軸３〜９の軸制御部２３〜２９を代表して、関節軸９の軸制御部２９の構成について説明する。軸制御部２９は、関節軸９における軸変位および軸速度を計測してアクチュエータ９ａの制御指令値を算出する制御計算処理装置９ｃと、関節軸９を駆動するアクチュエータ９ａと、全体評価部２０内の通信装置３０と通信可能な通信装置３１を備えている。なお、その他の軸制御部２３〜２８も同様な構成を有している。さらに、全体評価部２０内の通信装置３０と、各関節軸３〜９の軸制御装置２３〜２９内の通信装置（通信装置３１等）とは、有線または無線の情報通信手段であるネットワーク３２で接続されている。

また、本第１実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図３の制御処理フロー図において、全体評価部２０のステップＳ３にて算出されるハンド１８の状態をハンド１８の座標と速度とする。そして、同じく全体評価部２０のステップＳ３にて算出される全体の状態変数（全体状態）を、ハンド１８の速度とヤコビ行列とする。なお、本第１実施形態においては、全体の状態変数、すなわちハンド１８の速度とヤコビ行列が、それぞれの関節軸３〜９の軸変位および軸速度を含む情報となっている。また、ハンド１８のパラメータとしては位置座標や速度があり、関節軸３〜９のパラメータとしては軸変位や軸速度がある。

ハンド１８の座標は、数式（１）の順運動学方程式に全軸の変位を代入することで求めることができる。さらに数式（１）の両辺を微分すると下記の数式（３）が得られる。

Ｖ＝Ｊω ・・・（３）

数式（３）において、Ｖはハンド１８の速度ベクトル、Ｊはヤコビ行列、ωは関節軸の軸速度のベクトルを表す。従って、計測された関節軸の軸速度を数式（３）に代入することでハンド１８の速度を算出することができる。

また、全体評価部２０のステップＳ４にて算出される評価結果を、ハンド１８の座標（先端位置）から制御目標（目標位置）１９の座標を結ぶ偏差ベクトルとする。なお、この偏差ベクトルが位置差分の一例となっている。

なお、偏差ベクトルは下記の数式（４）より求めることができる。

Ｄ＝Ｇ−Ｈ ・・・（４）

数式（４）において、Ｄは偏差ベクトル、Ｇは制御目標の座標ベクトルである。

また、軸制御部２３〜２９における制御方法としては、まず対象の関節軸の軸速度の変化によるハンド１８の移動ベクトルの変化を検討する。ここで、対象とする第ｎ軸（第ｎ関節軸）の軸速度によって生成されるハンド１８の速度ベクトルについては、数式（３）を用いて次のように算出することができる。

Ｖｎ＝Ｊωｎ ・・・（５）

数式（５）において、Ｖｎは第ｎ軸の軸速度によって生成されるハンド１８の速度ベクトル、ωｎは第ｎ軸の要素以外０（ゼロ）となる関節軸の軸速度ベクトルである。従って、第ｎ軸の軸速度が微小な速度変化αで加速又は減速した場合のハンド１８の移動ベクトルは、加速の場合は数式（６）にて、減速の場合は数式（７）にてそれぞれ表すことができる。

Ｖ＋αｎ＝Ｊ（ω＋αｎ） ・・・（６）
Ｖ−αｎ＝Ｊ（ω−αｎ） ・・・（７）

数式（６）、数式（７）において、Ｖ＋αｎは第ｎ軸のみ速度変化αで加速した場合のハンド１８の速度ベクトル、Ｖ−αｎは第ｎ軸のみ速度変化αで減速した場合のハンド１８の速度ベクトル、αｎは第ｎ軸だけ速度変化αとしそれ以外の要素を速度変化０（ゼロ）とする関節軸の軸速度ベクトルである。

数式（３）、数式（６）、数式（７）より、現状の速度を維持する場合、加速する場合、減速する場合の３通りの場合のハンド１８の移動ベクトルが算出できるので、以下の数式（８）を用いてそれぞれの偏差ベクトル方向成分の大きさを算出する。

Ｃ＝Ｖ・Ｄ／｜Ｖ｜×｜Ｄ｜ ・・・（８）

数式（８）において、Ｃは偏差ベクトル方向成分の単位成分あたりの大きさであり、ＶとＤの方向ベクトルが作る角度をφとするとＣ＝ｃｏｓφとなり、このＣが大きいほどφの角度が小さくなりＶとＤの方向が一致に近づくことが判る。従って、Ｖ、Ｖ＋αｎ、Ｖ−αｎについてそれぞれＣを求め、このＣが最も大きくなる、すなわちハンド１８の速度ベクトルが制御目標１９に最も近くなる関節軸の軸速度を制御指令値として算出している。

次に、それぞれの軸制御部２３〜２９による各関節軸３〜９の制御方法について、図５Ａ及び図５Ｂに示す模式図を用いて説明する。図５Ａはマニピュレータのハンドと制御目標の位置関係を示す模式図であり、図５Ｂは、マニピュレータの各関節軸の指令値の決定方法を示す模式図である。

図５Ａにおいて、マニピュレータ１のハンド１８の座標（Ｈ）から制御目標１９の座標（Ｇ）を結んだベクトルが偏差ベクトル４０（Ｄ）である。

また、図５Ｂにおいて、制御対象の関節軸６（第ｎ軸）において、現状の速度ω１（ω１＝ω）と、加速した場合の速度ω２（ω２＝ω＋αｎ）と、減速した場合の速度ω３（ω３＝ω−αｎ）におけるハンド１８の速度ベクトル４２の候補はそれぞれＶ１、Ｖ２、Ｖ３となる。これらの速度ベクトル４２と偏差ベクトル４０の作る角度φ１、φ２、φ３の大きさをもとに関節軸６の軸速度を設定する。図５Ｂの場合、角度φ３が最も小さい角度となることから、速度ベクトルＶ３が偏差ベクトル４０に最も近いと判断して、現状の速度Ｖよりα減速した速度ベクトルＶ３を軸速度の制御指令値として設定する。

上記の制御方法では、マニピュレータ１全体として速度、すなわちハンド１８の速度の調整を直接的に行わず、各々の関節軸３〜９の軸速度を設定している。そのため、ハンド１８の速度が異常に早くなってしまい危険な状況になる可能性や、制御目標に停止できず通りすぎてしまう可能性がある。そこで、例えば図６の模式図に示すようにしてハンド１８の速度を制限することでこれらの問題を解決する。

図６は、本第１実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図である。

図６において、全体評価部２０にて、制御目標１９から所定範囲内の減速領域４３を設定する。ここで、減速領域４３は、球状でも円状でも良い。また、制御目標１９やハンド１８によっては、一方向に長い領域も考えられる。ハンド１８が減速領域４３より離れている場合の位置４４での速度４５をハンド１８の上限速度とし、ハンド１８と制御目標１９との距離が短いほど上限速度を下げるように制御して、ハンド１８が制御目標１９に到達した際に速度が０となるように設定する。すなわち、ハンド１８の位置４４での速度４５よりも、減速領域４３内で制御目標１９に近づいている位置４６での速度４７を小さくするように制御する。さらに、図３のステップＳ４にて算出される新たな評価結果として上限速度と現在速度の速度比を加え、さらに図３のステップＳ７において上限速度と現在速度の速度比が１を超える場合に、この速度比と選択された関節軸の軸速度を掛けたものを新たな関節軸の軸速度の制御指令値とする。

このような処理を加えることで、結果的に全ての関節軸の軸速度が上限速度に合わせて減速されて、ハンド１８の速度の上限が上限速度となるように制御される。上限速度が制御目標１９に近づくにつれて小さくなることで、制御目標１９にハンド１８が近づくにつれてハンド１８の速度は減速されるため、制御目標１９において確実にハンド１８を停止させることができる。

また、マニピュレータの制御において解決すべき問題として、マニピュレータの特異姿勢がある。「特異姿勢」とは、マニピュレータの制御においてエラーを引き起こす特異な姿勢であり、この状態を如何に回避するかがマニピュレータの制御において重要な問題である。マニピュレータの特異姿勢を説明するために、図７Ａ、図７Ｂ、及び図７Ｃに、本第１実施形態のマニピュレータ１の特異姿勢時の対応を示す模式図を示す。ここで、図７Ａは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図であり、図７Ｂは、マニピュレータが特異姿勢から脱出するための第１の対応方法を示す模式図であり、図７Ｃは、マニピュレータが特異姿勢から脱出するための第２の対応方法を示す模式図である。

まず、図７Ａにおいて、マニピュレータ１は、その関節軸７〜９をどのように駆動しても、そのハンド１８の速度ベクトルが制御目標１９に対して垂直な方向となってしまい、制御目標１９に近づく方向に速度ベクトルを生成できない姿勢となっている。このようなマニピュレータ１の姿勢が「特異姿勢」である。本第１実施形態において、これまでに説明した制御方法だけでは、マニピュレータ１が特異姿勢となった場合、ハンド１８が制御目標１９に近づくことができずに停止してしまう。

そこで、本第１実施形態では、次のような２つの方法を用いることで特異姿勢への対応、すなわち特異姿勢からの脱出を可能とする。本第１実施形態では、ハンド１８が制御目標１９（目標位置から所定の範囲内）に到達していない状態にも関らず、全ての関節軸３〜９（図７Ａ中では関節軸７〜９）の軸速度が全て０となった場合に特異姿勢が生じたものと判断する。例えば、図３の全体評価部２０のステップＳ４において任意の設定時間以上、全ての関節軸３〜９において軸速度０が維持されたか否かで判断することができる。

まず、第１の対応方法は、図７Ｂで示すように、マニピュレータ１が特異姿勢になった場合に全体評価部２０のステップＳ４において、任意の設定時間の間だけ本来の制御目標１９（Ｇ）とずれた仮制御目標４８（Ｇ２）を設定する方法である。仮制御目標４８を設定し、評価結果である偏差ベクトルを算出した後、偏差ベクトルを評価結果としてそれぞれの軸制御部２３〜２９に送信する。各々の軸制御部２３〜２９において、送信された偏差ベクトルに基づいて、仮制御目標４８に向かうハンド１８の移動制御を行うことで、マニピュレータ１を特異姿勢から脱出させることができる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標１９を再設定し、通常の制御を継続する。

次に、第２の対応方法は、図３の全体評価部２０のステップＳ４で算出される新たな評価結果として特異姿勢の有無を加え、全体評価部２０からそれぞれの軸制御部２３〜２９に特異姿勢の有無情報を送信する方法である。特異姿勢が生じている場合は、制御目標１９を一時的に目標から切り離し、図７Ｃで示すようにそれぞれの軸制御部２３〜２９で任意の設定時間の間、それぞれの関節軸３〜９に対して任意の軸速度の遥動動作４９を実施させ、この揺動動作４９の実施により強制的に特異姿勢から脱出させる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標１９を再設定し、通常の制御を継続する。

上述の本第１実施形態の説明においては、全体評価部２０から全ての軸制御部２３〜２９に、評価結果および全体状態の情報を送信する（図３のステップＳ６）場合について説明したが、本第１実施形態はこのような場合についてのみ限定されるものではない。このような場合に代えて、例えば、関節軸３〜９のうちの制御対象となる関節軸を制御する軸制御部のみに、上記情報が送信されるような場合であってもよい。

本第１実施形態のマニピュレータ１の制御システムによる制御方法によれば、以下のような種々の効果を得ることができる。

本第１実施形態の制御方法では、全体評価部２０にて作成されたマニピュレータ１の全体状態の評価結果の情報をそれぞれの軸制御部２３〜２９に送信して、この評価結果の情報に基づいて、個々の軸制御部２３〜２９において、他の関節軸とは独立して自軸の制御指令値を作成する。これにより、各々の関節軸単位にて独立して制御目標１９に向かって制御を行うことで位置制御を行うことができる。そのため、冗長や非線形な駆動要素のあるマニピュレータ１においても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生しない。また、逆運動学の算出のために拘束条件を設定することや、自由度を制限する必要も無いため、高い自由度を維持したまま制御を行うことができる。さらに、動作の学習も必要としないため容易にマニピュレータの制御を行うことができる。

また、周囲の環境の影響や関節軸の故障によって一部の関節軸が稼動不能に陥っても、その他の関節軸が個別に制御目標１９に向かうことによって自然に冗長性が発揮されるため、周囲の環境や軸の故障にロバスト性を有した状態で位置制御を行うことができる。なお、ここで、ロバスト性とは、外乱や設計誤差などの不確定な変動に対して、システム特性が現状の機能を維持できる性質を示す。なお、本発明における「故障」とは、関節軸の動作制御を行う軸制御部や全体評価部が、対象となる関節軸に何らかの問題が生じていることを認識しておらず、関節軸に対して制御指令を出しているのに関節軸が動作しない状態を示す。

さらに、それぞれの関節軸単位で制御指令値を算出することに伴いハンド１８が速度オーバーとなることや、制御目標１９を通過してしまうという課題に対しても、全体評価部２０にてハンド１８の速度制御などを行うことで解決することができる。また、マニピュレータ１の特異姿勢に対しても、全体評価部２０にて、仮制御目標を一時的に設定することや、揺動動作を実施させることにより対応することができる。

従って、本第１実施形態のマニピュレータの制御方法によれば、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御を行うことができ、冗長や非線形な駆動要素があっても容易に位置制御を実現することができる。

さらに、本第１実施形態の制御方法は、マニピュレータの形状や軸数が変わっても同じ制御則を用いており、マニピュレータの形状パラメータを変更するだけで形状や軸数の変化に対応できる。

（第２実施形態）
本発明は上記第１実施形態に限定されるものではなく、その他種々の態様で実施できる。以下、本発明の第２実施形態にかかるマニピュレータの制御方法について説明する。

実際にマニピュレータを使用する場面では、様々な作業に柔軟に対応する必要が有る。そのために、作業に応じてマニピュレータの形状や軸数などを変化させることも考えられるが、本第２実施形態の制御方法を活用することで、マニピュレータの形状や軸数に合わせたプログラムの作成およびプログラムの書換えを不要とすることができる。これにより、マニピュレータの形状や軸数などが変更されるような場合であっても、パラメータの変更だけで対応することができ、プラグアンドプレイのようにマニピュレータをロボット等に追加・変更してすぐに使用することができる。

以下に、プラグアンドプレイによるマニピュレータの追加変更を実現する方法を、図面を参照しながら説明を行う。本発明の第２実施形態のマニピュレータ制御システムの模式図を図８に示す。

図８において、冗長なマニピュレータ１０１の制御システムは、マニピュレータ１０１の全体評価を行う全体評価部１２０と、それぞれの関節軸毎に独立して制御を行う軸制御部１２３〜１２９とを備えている。

全体評価部１２０は、評価計算装置１３１、通信装置１３２、現在制御中のマニピュレータ１０１が備えるそれぞれの関節軸３〜９の識別情報や形態情報などの軸情報をデータとして保持する記憶装置１３３と、記憶装置１３３のデータに基づいてマニピュレータ１０１を構成する関節軸３〜９の接続状態の変化を監視する監視装置１３４と、監視装置１３４により接続状態の変化を検出した際に記憶装置１３３に保持されたデータを変化した軸情報に更新する軸情報更新装置１３５と、を備えている。

また、それぞれの軸制御部１２３〜１２９は同様の構成を有しているため、代表して軸制御部１２３の構成について説明する。軸制御部１２３は、計測装置３ｓ、制御計算処理装置３ｃ、アクチュエータ３ａ、通信装置３１、自軸の識別情報や形態情報などの軸情報を記憶する記憶装置５０を備えている。

ここで、軸制御部１２３における記憶装置５０に記憶された関節軸の識別情報は、例えば、関節軸毎に固有の識別情報であることが望ましく、通信装置３１で送信する通信データにこの識別情報を付加することで、通信データがその関節軸から通知されたものなのか識別できる。それとともに、全体評価部１２０からの関節軸３〜９への指令の宛先として指定することができ、それぞれの軸制御部１２３〜１２９では自軸の識別情報と一致した場合のみ自軸の制御を行うことができる。

全体評価部１２０の通信装置１３２と軸制御部１２３の通信装置３１とは、ネットワーク５１を介して接続されている。ネットワーク５１としては、例えば、全体評価部１２０を始点とし、マニピュレータ１０１の先端の関節軸９が終端となるようにマニピュレータ１０１の接続と同じ順でそれぞれの軸制御部１２３〜１２９に接続されたネットワークがある。このように接続することで各関節軸３〜９の接続状態がネットワーク５１のルーティングを用いることで容易に把握することができる。なお、ネットワーク５１の接続方法はこのような方法に限るものでは無く、その他の方法を用いることも考えられる。その際は、例えば隣り合った各関節軸の接続状態を検知し、その接続状態をネットワーク５１にデータをして載せる事で各関節軸の接続状態を把握することができる。

このように検知される識別データおよび接続状態は、全体評価部１２０内の監視装置１３４で、常時、記憶装置１３３に記憶されたマニピュレータ１０１の軸情報と比較することで監視され、違いが発生すれば接続状態の変化として検出する。また、各関節軸３〜９から収集した軸情報を用いて軸情報更新装置１３５によって記憶装置１３３の軸情報を更新する。

続いて、プラグアンドプレイによりマニピュレータ１０１の構成要素を変更する方法を、図９から図１３に示すマニピュレータ１０１の模式図を用いて説明する。

まず、図９は、マニピュレータの構成要素を変更する前の状態を示す模式図である。

図９において、全体評価部１２０では、それぞれの軸制御部１２０から送られてくる各関節軸３〜９の通信情報６４に含まれる関節軸の識別情報およびルーティング等を利用した接続情報と、記憶装置１３３に記憶されている軸情報データ６５とを比較する。それとともに、全体評価部１２０において、全体の評価が行われ、記憶装置１３３の軸情報データに基づいて制御対象となる関節軸を設定し、その識別情報を含む通信情報６６をそれぞれの軸制御部１２３〜１２９に送信する。それぞれの軸制御部１２３〜１２９では、その通信情報６６に自軸の識別情報を含む場合にのみ自軸の制御を行う。なお、図９から図１３において、説明の理解を容易にするために、それぞれの関節軸３〜９の制御を行う軸制御部１２３〜１２９をＡ１〜Ａ７にて示すとともに、それぞれの関節軸３〜９の識別情報をＤ１〜Ｄ７にて示す。また、後述するように変更された構成要素である関節軸の軸制御部をＡ８にて示すとともに、その識別情報をＤ８にて示す。

図１０は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図である。

図１０において、マニピュレータ１０１の先端部（ハンド及び関節軸）６７が、例えば、識別情報Ｄ７から識別情報Ｄ８の関節軸に変更された場合を例として説明する。関節軸が変更された瞬間にそれぞれの軸制御部（１２３等）では通常どおり各関節軸の制御計算処理装置（３ｃ等）で収集されたデータが通信情報６４として全体評価部１２０に送信される。ただし、この場合、構成要素の変更が行われているため、通信情報６４には異なる識別情報Ｄ８が含まれている。全体評価部１２０では、監視装置１３４にて通信情報及びそのルーティング等と記憶装置１３３の軸情報との比較を行う。この比較実施の結果、先端部６７の識別情報が異なることが検出され、監視装置１３４にてマニピュレータ１０１の構成要素に変更が生じていることが検知される。

図１１は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図である。

図１１において、監視装置１３４にて、マニピュレータ１０１の先端部６７の変更を検知し、軸情報データ６５内に不明な部分を発見した全体評価部１２０は、新たな先端部６７の形態等が未知である識別情報Ｄ８を有する関節軸に対して、軸情報を要求する通信情報６８を送信する。

図１２は、軸制御部からパラメータが応答されている状態を示す模式図であり、図１３は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図である。

図１２において識別情報Ｄ８を含む通信情報６８を受けたそれぞれの軸制御部Ａ１〜Ａ８（Ａ７を除く）では、識別情報Ｄ８を有する軸制御部Ａ８のみが反応し、自軸の形態情報を含む通信情報６９を全体評価部１２０に返信する。全体評価部１２０では、この通信情報６９を受信することで軸情報更新装置１３５を用いて記憶装置１３３内の軸情報データ６５の更新を行う。なお、この時点までは識別情報Ｄ８の軸制御部Ａ８は、アクチュエータの制御は行っていない。

図１３において、変更の結果が記憶装置１３３内の軸情報データ６５に登録されると、全体評価部１２０では、新しい軸情報データ６５に従って全体評価の計算を行い、その結果を通信情報７０として送付する。この通信情報７０には変更後の構成を反映した識別情報を含んでおり、この識別情報によりこれまでアクチュエータの制御を行っていなかった識別情報Ｄ８の軸制御部Ａ８も、他の軸制御部Ａ１〜Ａ６と同様に制御を開始する。その後、通常通り各関節軸の計測装置での収集したデータ７１が送付され、このデータ７１に基づいて全体評価部１２０では監視と制御が繰り返して行われる。

以上のようなマニピュレータの制御方法によれば、マニピュレータの形状や軸数に関係無く、統一した制御則で制御が行われるとともに、自律的にマニピュレータの構成を検出してパラメータを更新することができる。そのため、マニピュレータの構成要素が変化した場合でも、プラグアンドプレイ、すなわち、制御が途切れることなく安全に継続することができる。従って、作業に応じてマニピュレータの形状や軸数を容易に変化させることができ、さらに幅広い作業に柔軟かつ容易に適応することができる。

（第３実施形態）
次に、本発明の第３実施形態にかかるマニピュレータの制御システムによる制御方法について、図１４に示すマニピュレータの制御システムの模式図を用いて説明する。

図１４に示すように、本第３実施形態のマニピュレータ２０１の制御システムは、それぞれの関節軸３〜９の制御を他の関節軸から独立して行う軸制御部２２３〜２２９と、マニピュレータ２０１の全体状態等の評価を行う全体評価部２２０とを備えている。

それぞれの軸制御部２２３〜２２９は、関節軸３〜９に個別に装備されたエンコーダなどのセンサを有する計測装置（９ｓ等）と、関節軸を駆動するアクチュエータ（９ａ等）と、入力された情報に基づいてアクチュエータの制御指令値を算出する制御計算処理装置（９ｃ等）とを備えている。全体評価部２２０は、全体評価処理装置２２１を備えている。

また、本第３実施形態のマニピュレータ２０１の制御方法は、マニピュレータ２０１のハンド１８を制御目標１９に移動させるように位置制御を行いながら、ハンド１８から作業対象物２０９に一定の接触力を付加させた状態とする力制御を行うものである。このような力制御を実現するために、ハンド１８には接触力を計測する力計測装置２２２が備えられている。

このような構成の制御システムによるマニピュレータ２０１の位置制御及び力制御の制御処理フローを図１５に示す。

図１５に示すように、ステップＳ１１にて、それぞれの計測装置（９ｓ等）により各関節軸３〜９の軸変位および軸速度が計測される。次に、ステップＳ１２にて、計測された軸変位・軸速度の情報が、それぞれの軸制御部２２３〜２２９から全体評価部２２０に送信されて、軸変位・軸速度の情報が収集される。それと共に、作業対象物２０９に接触した状態にあるハンド１８の接触力が、力計測装置２２２により計測されて、全体評価部２２０に収集される。その後、ステップＳ１３にて、各関節軸３〜９の軸変位および軸速度の情報、並びにハンド１８の接触力の情報から、ハンド１８の状態および全体の状態変数（全体状態）を算出する。続いて、ステップＳ１４にて、ハンド１８の状態に基づいて、マニピュレータ２０１の制御状態を評価する（第１の工程）。なお、この制御状態、すなわち位置制御と力制御の状態の詳しい評価方法については後述する。その後、ステップＳ１５にて、評価結果に基づいて制御目標に到達していないと判断される場合には、ステップＳ１６にて、評価結果および全体状態の情報が、全体評価部２２０からそれぞれの軸制御部２２３〜２２９に送信される（第２の工程）。

各々の軸制御部２２３〜２２９では、ステップＳ１７にて、自軸の軸変位・軸速度の情報と、送信された評価結果およびマニピュレータの全体状態の情報とに基づいて、自軸のアクチュエータの制御指令値を他の関節軸とは独立して算出する（第３の工程）。続いて、ステップＳ１８にて、算出された制御指令値に基づいて、自軸のアクチュエータを駆動する。これらステップＳ１１〜Ｓ１８の制御処理が繰り返し実行される。ステップＳ１５にて、評価結果が、マニピュレータ２０１のハンド１８が制御目標（目標位置及び目標接触力）に到達していると判断された場合には、マニピュレータ２０１の位置制御および力制御が完了する。なお、ステップＳ１１、Ｓ１７、及びＳ１８の制御処理はそれぞれの軸制御部２２３〜２２９にて独立して実行され、ステップＳ１２〜Ｓ１６の制御処理は全体評価部１２０にて実行される。

ここで、本第３実施形態の制御システムの動作の概要図を図１６に示す。

図１６において、全体評価部２２０は、マニピュレータ２０１全体の制御状態の評価を行う全体評価処理装置２２１と、複数の制御装置と相互通信可能な通信装置２３０を備えている。また、それぞれの関節軸３〜９の軸制御部２２３〜２２９を代表して、関節軸９の軸制御部２２９の構成について説明する。軸制御部２２９は、関節軸９における軸変位および軸速度を計測してアクチュエータ９ａの制御指令値を算出する制御計算処理装置９ｃと、関節軸９を駆動するアクチュエータ９ａと、全体評価部２２０内の通信装置２３０に対して通信可能な通信装置２３１と、を備えている。なお、その他の軸制御部２２３〜２２８も軸制御部２２９と同様な構成を有している。さらに、全体評価部２２０内の通信装置２３０と、各関節軸３〜９の軸制御装置２２３〜２２９内の通信装置（通信装置２３１等）とは、有線または無線の情報通信手段であるネットワーク２３２で接続されている。

また、本第３実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図１５の制御処理フローにおいて、全体評価部２２０のステップＳ１３にて算出されるハンド１８の状態をハンド１８の座標及び速度並びにハンド１８の接触力とする。そして、同じく全体評価部２２０のステップＳ１３にて算出される全体の状態変数（全体状態）を、ハンド１８の速度及び接触力とヤコビ行列とする。なお、本第３実施形態においては、全体の状態変数、すなわちハンド１８の速度及び接触力とヤコビ行列が、それぞれの関節軸３〜９の軸変位および軸速度を含む情報となっている。また、本第３実施形態において、ハンド１８のパラメータは、速度と接触力となっている。

マニピュレータ２０１のハンド１８の座標は数式（１）に全て関節軸３〜９の軸変位を代入することで求めることができる。

また、計測された関節軸３〜９の軸速度を数式（３）に代入することで、ハンド１８の速度を算出することができる。

また、全体評価部２２０のステップＳ１４にて算出される評価結果は、例えば図１７で示すように算出する。まず、手先座標２５１から制御目標１８の座標を結ぶ位置の偏差ベクトルを算出する。なお、位置の偏差ベクトルＤｐは数式（９）により求めることができる。なお、Ｇは制御目標１９の座標ベクトルである。

Ｄｐ＝Ｇ−Ｈ ・・・（９）

次に、制御目標の接触力Ｆｔと実際の接触力Ｆｒとの接触力差、すなわち力差分を位置偏差に換算したもの算出する。この力の換算偏差ベクトルＤｆは数式（１０）のように表すことができる。なお、Ｋｆは制御ゲインであり、系によって適当な値が選択される。

Ｄｆ＝Ｋｆ（Ｆｔ−Ｆｒ） ・・・（１０）

これらの数式（９）、（１０）にて表される位置の偏差ベクトルＤｐと力の換算偏差ベクトルＤｆを用いて、制御評価に用いる総合偏差ベクトルＤを数式（１１）のように算出する。

Ｄ＝Ｄｐ＋Ｄｆ ・・・（１１）

このように数式（１１）にて総合偏差ベクトルＤを算出した後は、上記第１実施形態における位置制御方法と同様な手順にて、数式（３）、（５）、（６）、（７）、（８）を用いて、各々の関節軸３〜９における現状の速度を維持する場合（Ｖ）、加速する場合（Ｖ＋αｎ）、減速する場合（Ｖ−αｎ）の３通りのハンド１８の移動ベクトルと、それぞれの偏差ベクトル方向成分の大きさＣとを算出する。算出した結果を参照して、このＣが最も大きくなる、すなわちハンド１８の速度が制御目標の方向にもっとも近くなる関節軸の軸速度を制御目標として採用する。

この制御目標に対して第ｎ軸の制御は、数式（１２）に示すように、現状の出力トルクＴｎにΔＴｎを加えた（Ｔｎ＋ΔＴｎ）のトルクが出力されるような制御として行われる。

ΔＴｎ＝Ｍｎ・Δωｎ／Δｔ＋Ｋｄｎ・Δωｎ ・・・（１２）

ここで、Δωｎは選出された第ｎ軸の軸速度の制御目標と、現在の軸速度との速度差であり、Δｔは制御単位時間を表す。また、Ｍｎは第ｎ軸の慣性モーメント、Ｋｄｎは第ｎ軸の粘性抵抗成分とする。このように算出される制御トルク出力（Ｔｎ＋ΔＴｎ）を用いることより、ハンド１８の目標位置への到達及び目標接触力の発生をより効果的に実現することができるような第ｎ軸の軸速度の制御指令値を算出することができる。

以上のような解決手段のようなマニピュレータの制御方法によれば、周囲の環境の影響や軸の故障によってある関節軸が移動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮される。そのため、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御及び力制御を行うことができる。従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易にかつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。

なお、本第３実施形態においては、マニピュレータ２０１のハンドの位置制御と力制御とが組み合わされて行われるような場合を例として説明したが、このような場合に代えて、力制御だけが行われるような場合にも、本第３実施形態の制御方法を適用することができると考えられる。

なお、上述のそれぞれの実施形態の説明では、マニピュレータ１等のアーム部分を支持するベース２がある位置に固定された状態にて、マニピュレータ１等の動作が制御されるような場合について説明した。しかしながら、本発明はこのような場合についてのみ限定されるものではなく、例えば、図１８の模式図に示すように、マニピュレータ３０１のアーム部分を支持するベース３０２に車輪等を用いた移動装置３０５を備えさせ、マニピュレータ３０１の設置位置を移動可能とするような構成を採用することもできる。

なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏する構成とすることができる。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

２００６年３月２４日に出願された日本国特許出願Ｎｏ．２００６−０８２２６９号の明細書、図面、及び特許請求の範囲の開示内容は、全体として参照されて本明細書の中に取り入れられるものである。

本発明のマニピュレータの制御方法および制御システムによれば、周囲の環境や軸の故障などの不確かさ、変動を有した状態でも制御可能なロバスト性を有する位置制御を実現することができる。冗長や非線形な駆動要素があっても少ないセンサを用いて容易にかつ柔軟に位置制御を実現することができるため、より複雑で高い自由度を必要とする作業を行う産業用、および民生用ロボットのアームなどに使用することができる。特に、障害物が多く、さらにその位置が特定されていない環境が多く存在する家庭用ロボットのアームとしての用途に有用である。

図１は、本発明の第１実施形態のマニピュレータの模式図である。 図２は、第１実施形態の制御システムの模式図である。 図３は、第１実施形態の制御処理フローを示す図である。 図４は、第１実施形態の制御システムの動作の概念図である。 図５Ａは、マニピュレータのハンドと制御目標の位置関係を示す模式図である。 図５Ｂは、マニピュレータの各関節軸の指令値の決定方法を示す模式図である。 図６は、第１実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図である。 図７Ａは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図である。 図７Ｂは、マニピュレータが特異姿勢から脱出するための第１の対応方法を示す模式図である。 図７Ｃは、マニピュレータが特異姿勢から脱出するための第２の対応方法を示す模式図である。 図８は、本発明の第２実施形態のマニピュレータ制御システムの模式図である。 図９は、マニピュレータの構成要素を変更する前の状態を示す模式図である。 図１０は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図である。 図１１は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図である。 図１２は、軸制御部からパラメータが応答されている状態を示す模式図である。 図１３は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図である。 図１４は、本発明の第３実施形態にかかるマニピュレータの制御システムの模式図である。 図１５は、第３実施形態の制御処理フローを示す図である。 図１６は、第３実施形態の制御システムの動作の概念図である。 図１７は、マニピュレータの制御における評価方法を説明する模式図である。 図１８は、上記第１実施形態の変形例にかかるマニピュレータの模式図である。 図１９は、従来のマニピュレータの制御方法の概要を示す図である。 図２０は、従来のマニピュレータの制御処理フロー図である。 図２１Ａは、従来のマニピュレータの制御において曲げ角度の均等配分する制御方法を示す図である。 図２１Ｂは、従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図である。 図２２は、従来のマニピュレータの制御方法を示す図である。 図２３は、従来のマニピュレータの制御方法を示す図である。 図２４は、従来のマニピュレータの制御方法を示す図である。

Claims (14)

1. 複数の関節軸を有するマニピュレータの制御方法であって、
   上記マニピュレータの先端部に関する制御対象量と目標値との差分を計測する第１の工程と、
   上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
   夫々の上記軸制御部において、上記軸情報と上記差分情報とに基づいて、その余の軸では補正を行わないと仮定した上で、上記先端部に関する制御対象量が目標値に近づくように上記関節軸毎の現在の制御目標値を新たな制御目標値に補正し、当該新たな制御目標値に基づいて各関節軸を制御する第３の工程と、を備え、
   上記先端部の制御対象量が上記目標値を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返す、マニピュレータの制御方法。
2. 複数の関節軸を有するマニピュレータの制御方法であって、
   上記マニピュレータの先端位置と目標位置との位置差分を計測する第１の工程と、
   上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記位置差分の位置差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
   上記軸情報と上記位置差分情報とに基づいて、夫々の上記軸制御部で上記関節軸の軸変位と軸速度とを独立に補正する第３の工程と、を備え、
   上記先端位置が上記目標位置を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返し、
   上記第２の工程において、
   上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記関節軸毎に送信し、
   上記第３の工程において、
   夫々の上記軸制御部は、自身以外の軸制御部とは独立に、
   上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、
   上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御方法。
3. 上記複数の関節軸から取得された軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む軸情報を上記関節軸毎に作成し、
   その後、上記第２の工程を実施する、請求項２に記載のマニピュレータの制御方法。
4. 上記第２の工程において、上記先端位置と上記目標位置との距離に応じて設定された先端速度の上限値と実際の先端速度との速度比の情報を含む情報が上記関節軸に送信され、
   上記第３の工程において、上記複数の軸制御部にて上記速度比に応じて上記軸速度を上記関節軸毎に補正される、請求項２に記載のマニピュレータの制御方法。
5. 取得された上記関節軸の軸速度が全て０である場合に、上記目標位置の座標又は上記軸速度を一時的に変化させ、
   その後、上記第２の工程を実施する、請求項３に記載のマニピュレータの制御方法。
6. 取得された上記複数の関節軸が有する自軸の識別情報を統合して、上記複数の関節軸の接続状態の変化を検知する、請求項２に記載のマニピュレータの制御方法。
7. 上記複数の関節軸の識別情報を上記全体制御部から上記複数の軸制御部に送信した後、上記複数の軸制御部において、予め保持された自軸の識別情報が上記送信された識別情報と一致した場合にのみ上記関節軸の制御を行う、請求項２に記載のマニピュレータの制御方法。
8. 複数の関節軸を有するマニピュレータの制御方法であって、
   上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する第１の工程と、
   上記複数の各関節軸の軸変位及び軸速度を含む軸情報と上記力差分の力差分情報とを、上記関節軸の軸制御部に夫々送信する第２の工程と、
   上記複数の関節軸の軸変位及び軸速度を含む情報と上記力差分の情報とに基づいて、上記軸制御部の上記関節軸の軸変位と軸速度とを上記関節軸毎に独立に修正する第３の工程と、を備え、
   上記先端部に加わる接触力が上記目標接触力を含む設定範囲内に達するまで上記第１の工程から上記第３の工程を繰り返し、
   上記第１の工程において、
   上記先端部の位置と目標位置との位置差分をさらに計測し、
   上記第２の工程において、
   上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として送信し、
   上記第３の工程において、
   夫々の上記軸制御部は、自身以外の軸制御部とは独立に、
   上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、
   上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御方法。
9. 取得された上記複数の関節軸の軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む情報を上記関節軸毎に作成し、
   その後、上記第２の工程を実施する、請求項８に記載のマニピュレータの制御方法。
10. 複数の関節軸を有するマニピュレータの制御システムであって、
    上記マニピュレータの先端部に関する制御対象量と目標値との差分を計測する計測装置と、
    上記差分の差分情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータを制御する全体制御部と、
    上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
    上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記複数の軸制御部に夫々送信する送信装置と、を備え、
    夫々の上記軸制御部において、上記軸情報と上記差分情報とに基づいて、その余の軸では補正を行わないものと仮定した上で、上記先端部に関する制御対象量が目標値に近づくように上記関節軸毎の現在の制御目標値を新たな制御目標値に補正し、当該新たな制御目標値に基づいて上記関節軸毎の駆動を制御する、マニピュレータの制御システム。
11. 複数の関節軸を有するマニピュレータの制御システムであって、
    上記マニピュレータの先端位置と目標位置との位置差分を計測する計測装置と、
    上記位置差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータの位置を制御する全体制御部と、
    上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
    上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記複数の軸制御部に夫々送信する送信装置と、を備え、
    夫々の上記軸制御部において、自身以外の軸制御部とは独立に、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度ベクトルの自身と対応する関節軸の成分だけを補正する、マニピュレータの制御システム。
12. 上記全体制御部は、
    上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
    上記記憶装置に記憶された上記軸情報に基づいて上記複数の関節軸の接続状態を監視する監視装置と、
    上記監視装置により上記複数の関節軸の接続状態の変化を検出した際に、上記記憶装置に記憶された上記軸情報を更新する軸情報更新装置と、を備える、請求項１１に記載のマニピュレータの制御システム。
13. 上記各々の軸制御部は、
    上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
    上記関節軸が他の関節軸に接続されることにより、上記記憶装置に記憶されている上記軸情報を上記全体制御部に通知する通信装置と、を備える、請求項１１に記載のマニピュレータの制御システム。
14. 複数の関節軸を有するマニピュレータの制御システムであって、
    上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測すると共に、上記先端部の位置と目標位置との位置差分を計測する計測装置と、
    上記力差分の情報と全ての上記関節軸の軸変位及び軸速度を含む情報に基づいて上記マニピュレータのトルク制御を行う全体制御部と、
    上記複数の関節軸に夫々備えられ、対応する関節軸のみを制御する複数の軸制御部と、
    上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として上記複数の軸制御部に夫々送信する送信装置と、を備え、
    夫々の上記軸制御部において、自身以外の軸制御部とは独立に、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように、上記軸速度のベクトルを上記関節軸毎に修正する、マニピュレータの制御システム。

Images