本発明は、複数の関節軸を有するマニピュレータを制御する技術に関する。
従来、多関節マニピュレータは、産業用や民生用のロボットアームなどに用いられている。このようなマニピュレータの位置制御に関する技術としては、様々な技術が存在する。
例えば、特定のマニピュレータの手先座標などを実現する各関節軸の変位や駆動力を、逆運動学を計算して求め、その変位や駆動力に基づいて制御を行う逆運動学を用いた技術がある。
この逆運動学を用いた従来技術の例を、図19、図20を用いて説明する。ここで、図19は従来のマニピュレータの制御方法の概要を示す図であり、図20は従来のマニピュレータの制御処理フロー図である。
図19において、マニピュレータ501の各関節軸511には、エンコーダなどのセンサ502(センサS)と、関節軸511を個別に駆動するアクチュエータ503(アクチュエータA)が搭載されている。これらを用いて、マニピュレータ501の手先504を制御目標505(制御目標G)に対して位置制御する。マニピュレータ501は、全体の制御を行う全体制御部506を備えており、センサ502で計測した結果を用いてアクチュエータ503の制御指令値を算出している。なお、全体制御部506は全体評価処理部507、軌道計画処理部508、制御計算処理部509の3つの処理部を有している。手先504は制御目標505まで、制御軌道510のように位置制御される。
図20において、制御処理フローは、まずステップS51で、各軸に搭載されたセンサ502によって各関節軸511の変位(軸変位)、変位速度(軸速度)を計測する。次にステップS52で、各関節軸511より収集した変位および変位速度の情報より手先504の座標を算出する。続いてステップS53で、目標位置505に移動するための手先軌道510を算出する。続いてステップS54で、目標軌道510を実現するための各関節軸511の変位および変位速度の制御指令値を算出する。続いてステップS55で、制御指令値を実現するように各関節軸511のアクチュエータ503を駆動する。図19の手先504が制御目標505に到達するまで、このようなステップS51〜ステップS55の処理を繰り返し実行している。なお、図20の制御処理と図19の制御要素との関係は、ステップS51はセンサ502で実行し、ステップS52は全体評価処理部507で実行し、ステップS53は軌道計画処理部508で実行し、ステップS54は制御計算処理部509で実行し、ステップS55はアクチュエータ503で実行されている。
ここで、ステップS52において手先504の座標を算出するために一般的に行われる計算式は、数式(1)のように表される。
H=T(θ) ・・・(1)
数式(1)はリンク機構の運動学方程式と呼ばれ、Hは手先の座標ベクトル、θは関節軸の変位ベクトルを示す。
また、ステップS54において、変位速度の制御指令を算出するために一般的に行われる計算式は、数式(2)のように表される。
θc=T−1(Ho) ・・・(2)
数式(2)はリンク機構の逆運動学方程式と呼ばれ、Hoは目標軌道を実現する手先の目標座標ベクトル、θcはその手先の目標座標ベクトルを実現する関節軸の変位ベクトルを示す。ただし、数式(2)は冗長な制御系では一意に解けないなどの問題があるため、数式(2)の代わりに別な計算方法を用いたものもある(例えば、特許文献1、特許文献2参照)。
特許文献1、特許文献2に開示された従来技術の例を、図21A、図21B、及び図22を用いて説明する。ここで、図21Aは従来のマニピュレータの制御処理フロー図であり、図21Bは従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図であり、図22は従来のマニピュレータの制御方法を示す図である。
特許文献1開示の制御方法は、図21A、図21Bのように2つの方法のいずれかを用いてマニピュレータの手先座標を実現する関節軸の変位を決定している。図21Aにおいては、冗長マニピュレータ520において、各関節軸521の変位を均等に配分することで変位522を決定している。また、図22Bにおいては、それぞれの各関節軸521において駆動させる関節軸523を必要次元だけ選択し、冗長マニピュレータ520を制御的に非冗長とすることで関節軸521の変位を算出可能としている。
特許文献2開示の制御方法は、制御部に逆運動学の関係を学習させることでアクチュエータの駆動力を算出できるようにしている。図22において、目標軌道であるPdと実際の軌道Pの誤差から演算装置534でトルクTを算出し、そのトルクTによりアクチュエータ535を駆動している。この時、目標軌道Pdを微分回路536に通したものとトルクTとを多層神経回路537に入力することで多層神経回路537の学習を行っており、学習の結果であるその出力をトルクTに加えてアクチュエータ535に入力することで制御を行っている。
また、逆運動学を用いずに分散的に制御を行う方法もある(例えば、特許文献3、特許文献4参照)。
特許文献3、特許文献4に開示された従来技術の例を、図23、図24を用いて説明する。ここで、図23は特許文献3開示の制御方法を示す図であり、図24は特許文献4開示の制御方法を示す図である。
特許文献3開示の制御方法は、マニピュレータによって多変数評価関数の各関節角の偏微分を用いて制御を行っている。図23において、まず、マニピュレータ540は軌道549上を移動するように制御を行っている時の各関節541の関節角θ1、θ2を用いて多変数評価関数542を設定する。続いて、多変数評価関数542を任意の時点での各角度変数で偏微分した関節角の偏微分543を用いて、多変数評価関数542が所定の条件を満たすように分散的に各関節541の制御を行っている。
図24において、特許文献4の方法は、制御装置によってマニピュレータ先端の位置に対応した関節部負荷閾値を用いて制御を行っている。位置検出装置553で検出されたマニピュレータの位置を計測し、動作制御装置554では計測された位置情報に従った負荷閾値を選択し、各関節角に設けられた負荷検出装置555が負荷閾値となるように関節角のサーボドライバ556を用いてアクチュエータ557を分散的に制御している。
従来、上記のような逆運動学を用いた制御や分散的な制御を行って、多関節マニピュレータの位置制御を行っている。
特開平07−164360号公報
特開平02−054304号公報
特開平09−207087号公報
特開2000−094368号公報
しかしながら、上記従来の制御方式を、高い自由度が要求される作業を行うマニピュレータに適応しようとする場合、以下のような課題がある。
従来の逆運動学を用いた方式では、非冗長で非線形要素が無い場合には使用できるが、環境によっては目的とする作業を行うことができない可能性がある。これは、マニピュレータの状態が一意に決まってしまうため、手先が目標位置まで到達不可能となるためである。一方、作業の自由度を上げるために冗長や非線形な駆動要素を取り入れると、逆運動学方程式である数式(2)が複雑になって一意に解けず、結果として制御ができない場合や、計算量が莫大になり、リアルタイムな制御が不可能になる場合がある。
特許文献1の場合は、冗長なマニピュレータに対して形状や自由度低減を利用して拘束条件を設け、その制御を可能としている。しかしながら、一方で、その拘束条件によって自由度を制限してしまうため、マニピュレータの作業性が制限されてしまう可能性がある。また、特許文献2の場合は、多層神経回路の学習によって冗長や非線形の駆動要素による計算上の問題やロバスト性を解決することは可能である。しかしながら、パラメータや教示データを適切に設定しないと、学習に時間を要する場合や計算結果が収束しない場合など、容易に適応できない可能性がある。
また、逆運動学を用いた方法では、いずれも場合も制御目標へ到達するため全軸の変位や駆動力が一意に決まるため、周囲に環境の影響や故障によって一つの軸が移動不可能な状態に陥ると全体の位置制御を実現できなくなる。
特許文献3は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、評価関数やその計算処理についてマニピュレータの形状や作業内容に応じて設定する必要であり、評価関数によっては作業が実行できない可能性がある。また、特許文献4は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、関節の負荷は位置姿勢だけでなく軌道や作業時にマニピュレータに取り付けるツールなどにより変化するため、作業毎に教示や学習により関節の負荷閾値を事前に登録する必要があり、想定外の状況には対応できない。
このようなマニピュレータの制御に関する課題は、マニピュレータの手先の位置制御が行われる場合だけでなく、手先の力制御が行われる場合にも生じる。
従って、本発明の目的は、上記課題を解決することにあって、周囲の環境や関節軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することにある。
上記目的を達成するために、本発明は以下のように構成する。
本発明の第1態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部のパラメータと目標値との差分を計測する第1の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記軸情報と上記差分情報とに基づいて、夫々の上記軸制御部で上記関節軸のパラメータを独立に補正する第3の工程と、を備え、
上記先端部のパラメータが上記目標値を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第2態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する第1の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記位置差分の位置差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記軸情報と上記位置差分情報とに基づいて、夫々の上記軸制御部で上記関節軸の軸変位と軸速度とを独立に補正する第3の工程と、を備え、
上記先端位置が上記目標位置を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第3態様によれば、上記複数の関節軸から取得された軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む軸情報を上記関節軸毎に作成し、
その後、上記第2の工程を実施する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第4態様によれば、上記第2の工程において、上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記関節軸毎に送信し、
上記第3の工程において、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように上記軸速度ベクトルを上記関節軸毎に補正する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第5態様によれば、上記第2の工程において、上記先端位置と上記目標位置との距離に応じて設定された先端速度の上限値と実際の先端速度との速度比の情報を含む情報が上記関節軸に送信され、
上記第3の工程において、上記複数の軸制御部にて上記速度比に応じて上記軸速度を上記関節軸毎に補正される、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第6態様によれば、取得された上記関節軸の軸速度が全て0である場合に、上記目標位置の座標又は上記軸速度を一時的に変化させ、
その後、上記第2の工程を実施する、第3態様に記載のマニピュレータの制御方法を提供する。
本発明の第7態様によれば、取得された上記複数の関節軸が有する自軸の識別情報を統合して、上記複数の関節軸の接続状態の変化を検知する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第8態様によれば、上記複数の関節軸の識別情報を上記全体制御部から上記複数の軸制御部に送信した後、上記複数の軸制御部において、予め保持された自軸の識別情報が上記送信された識別情報と一致した場合にのみ上記関節軸の制御を行う、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第9態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する第1の工程と、
上記複数の各関節軸の軸変位及び軸速度を含む軸情報と上記力差分の力差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記複数の関節軸の軸変位及び軸速度を含む情報と上記力差分の情報とに基づいて、上記軸制御部の上記関節軸の軸変位と軸速度とを上記関節軸毎に独立に修正する第3の工程と、を備え、
上記先端部に加わる接触力が上記目標接触力を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第10態様によれば、取得された上記複数の関節軸の軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む情報を上記関節軸毎に作成し、
その後、上記第2の工程を実施する、第9態様に記載のマニピュレータの制御方法を提供する。
本発明の第11態様によれば、上記第1の工程において、上記先端部の位置と目標位置との位置差分をさらに計測し、
上記第2の工程において、上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として送信し、
上記第3の工程において、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて、上記関節軸の軸速度のベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように上記軸速度のベクトルを上記関節軸毎に修正する、第9態様に記載のマニピュレータの制御方法を提供する。
本発明の第12態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部のパラメータと目標値との差分を計測する計測装置と、
上記差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータを制御する全体制御部と、
上記制御の評価結果情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記評価結果情報に基づいて上記関節軸のパラメータを夫々の上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
本発明の第13態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する計測装置と、
上記位置差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータの位置を制御する全体制御部と、
上記位置制御情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記位置制御情報に基づいて上記関節軸の軸変位及び軸速度を夫々の上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
本発明の第14態様によれば、上記全体制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記記憶装置に記憶された上記軸情報に基づいて上記複数の関節軸の接続状態を監視する監視装置と、
上記監視装置により上記複数の関節軸の接続状態の変化を検出した際に、上記記憶装置に記憶された上記軸情報を更新する軸情報更新装置と、を備える、第13態様に記載のマニピュレータの制御システムを提供する。
本発明の第15態様によれば、上記各々の軸制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記関節軸が他の関節軸に接続されることにより、上記記憶装置に記憶されている上記軸情報を上記全体制御部に通知する通信装置と、を備える、第13態様に記載のマニピュレータの制御システムを提供する。
本発明の第16態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する計測装置と、
上記力差分の情報と全ての上記関節軸の軸変位及び軸速度を含む情報に基づいて上記マニピュレータのトルク制御を行う全体制御部と、
上記トルク制御情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記トルク制御情報に基づいて、上記関節軸の軸変位及び軸速度を上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
以上のような本発明のマニピュレータの制御方法および制御システムによれば、制御目標(目標位置を含む所定の範囲)に向かう位置制御を軸単位に独立して行うため、冗長や非線形な駆動要素のあるマニピュレータにおいても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生せず、確実にマニピュレータの位置制御を行うことができる。
また、逆運動学の算出のための拘束条件の設定や、自由度の制限を行う必要も無いため、高い自由度を維持したまま制御を行うことができる。また、データの蓄積による学習も必要としないため、容易にマニピュレータを制御することもできる。
また、周囲の環境の影響や軸の故障によって一部の関節軸が稼動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮されるため、周囲の環境や関節軸の故障などの不確かさに依存しない制御を行うことができる。
従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。
本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、
図1は、本発明の第1実施形態のマニピュレータの模式図であり、
図2は、第1実施形態の制御システムの模式図であり、
図3は、第1実施形態の制御処理フローを示す図であり、
図4は、第1実施形態の制御システムの動作の概念図であり、
図5Aは、マニピュレータのハンドと制御目標の位置関係を示す模式図であり、
図5Bは、マニピュレータの各関節軸の指令値の決定方法を示す模式図であり、
図6は、第1実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図であり、
図7Aは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図であり、
図7Bは、マニピュレータが特異姿勢から脱出するための第1の対応方法を示す模式図であり、
図7Cは、マニピュレータが特異姿勢から脱出するための第2の対応方法を示す模式図であり、
図8は、本発明の第2実施形態のマニピュレータ制御システムの模式図であり、
図9は、マニピュレータの構成要素を変更する前の状態を示す模式図であり、
図10は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図であり、
図11は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図であり、
図12は、軸制御部からパラメータが応答されている状態を示す模式図であり、
図13は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図であり、
図14は、本発明の第3実施形態にかかるマニピュレータの制御システムの模式図であり、
図15は、第3実施形態の制御処理フローを示す図であり、
図16は、第3実施形態の制御システムの動作の概念図であり、
図17は、マニピュレータの制御における評価方法を説明する模式図であり、
図18は、上記第1実施形態の変形例にかかるマニピュレータの模式図であり、
図19は、従来のマニピュレータの制御方法の概要を示す図であり、
図20は、従来のマニピュレータの制御処理フロー図であり、
図21Aは、従来のマニピュレータの制御において曲げ角度の均等配分する制御方法を示す図であり、
図21Bは、従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図であり、
図22は、従来のマニピュレータの制御方法を示す図であり、
図23は、従来のマニピュレータの制御方法を示す図であり、
図24は、従来のマニピュレータの制御方法を示す図である。
本発明の記述を続ける前に、添付図面において同じ構成要素については同じ参照符号を付して、説明を省略している。以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
(第1実施形態)
以下に本発明の第1実施形態にかかるマニピュレータの制御システム及びその制御方法について、図面を参照しながら説明を行う。
図1は、本発明の第1実施形態のマニピュレータの模式図である。図1に示すように、マニピュレータ1は、7個の関節軸3〜9と、それぞれの関節軸を接続する8個のリンク10〜17と、リンク10を介して一連に接続された関節軸3〜9を支持するベース2と、一連に接続された関節軸3〜9の先端に接続されたハンド(先端部あるいは手先)18とを備えている。マニピュレータ1は、それぞれの関節軸3〜9の動作によって、ハンド18を制御目標に位置させることが可能となっている。
図2は、本第1実施形態の制御システムの模式図である。制御システムは、それぞれの関節軸3〜9の動作制御を行うことで、マニピュレータ1のハンド18を制御目標19(G)に位置させるための制御を行う。図2に示すように、マニピュレータ1の制御システムは、各々の関節軸3〜9の駆動動作を軸毎に個別に制御する軸制御部23〜29と、マニピュレータ1の全体動作の評価を行う全体制御部の一例である全体評価部20とを備えている。
軸制御部23は、エンコーダなどのセンサを有し、関節軸3の状態(軸変位及び軸速度)を計測する計測装置3sと、関節軸3を駆動させるアクチュエータ3aとを備えている。また、計測装置3sおよび全体評価部20から入力される情報に基づいてアクチュエータ3aを駆動制御するための情報を演算により作成し、アクチュエータ3aの駆動制御を他の関節軸4〜9から独立して行う制御計算処理装置3cも備えている。なお、計測装置3s及びアクチュエータ3aは、関節軸3に装備されている。軸制御部23と同様に、軸制御部24〜29にも、計測装置4s〜9s、アクチュエータ4a〜9a、及び制御計算処理装置4c〜9cが備えられている。また、全体評価部20は、全体評価処理装置21を備えている。
図3は、本第1実施形態の制御処理フローを示す図である。図3においては、全体評価部20における制御処理フローと、それぞれの軸制御部23〜29における制御処理フローとを分けて示すとともに、両者の関連について示している。
図3の制御処理フローにおいて、ステップS1にて、各関節軸3〜9の軸制御部23〜29が備える計測装置3s〜9sにより関節軸の軸変位および軸速度を計測する。次に、ステップS2にて、各関節軸3〜9の軸制御部24〜29から全体評価部20に計測された関節軸の軸変位および軸速度の情報が送信されて収集される。その後、ステップS3で、全ての関節軸3〜9の軸変位および軸速度の情報を用いて、マニピュレータ1のハンド18の状態、および全体の状態変数(全体状態)を算出する。続いて、ステップS4で、ハンド18の状態から、マニピュレータ1の位置制御状態を評価する(第1の工程)。その後、ステップS5にて、評価結果を参照して、マニピュレータ1のハンド18が制御目標(目標位置)19に到達しているか否かを示しているかどうかの判断を行う。制御目標19に到達していない場合には、ステップS6にて、評価結果および全体状態の情報を、それぞれの軸制御部23〜29に送信する(第2の工程)。
次に、ステップS7にて、それぞれの軸制御部23〜29にて、全体評価部20より送信されたマニピュレータ1の評価結果および全体状態の情報と、自軸の軸変位・軸速度の情報(ステップS1にて計測された情報)に基づいて、自軸のアクチュエータ3a〜9aの制御指令値(修正量)を他の関節軸とは独立して算出する(第3の工程)。続いて、ステップS8にて、算出された制御指令値に基づいて、自軸のアクチュエータ3a〜9aを駆動する。その後、ステップS1〜S8の制御処理が繰り返し実行される。ステップS5にて、評価結果が、マニピュレータ1のハンド18が制御目標19に到達していると判断された場合には、制御目標19に対するマニピュレータ1の位置制御が完了する。なお、ステップS1、S7、及びS8の制御処理はそれぞれの軸制御部23〜29にて独立して実行され、ステップS2〜S6の制御処理は全体評価部20にて実行される。また、全体評価部20及び軸制御部23〜29においてステップS1〜S8までの処理が繰り返されるサンプリング時間は、10ms以下の時間、例えば1ms程度の時間に設定される。ただし、このようなサンプリング時間の設定は、マニピュレータ1のハンド18をどれくらいの時間で制御目標に近づけるのかという原因により決定される。
図3の制御処理フローに示すように、全体評価部20とそれぞれの軸制御部23〜29との間における情報の受け渡し等の関係は次のようになっている。まず、ステップS1にて、それぞれの軸制御部23〜29で計測を行った関節軸の軸変位・軸速度の情報が、ステップS2にて、それぞれの軸制御部23〜29より送信されて、全体評価部20にて収集される。また、ステップS6にて、全体評価部20からそれぞれの軸制御部23〜29に、マニピュレータ1の評価結果と全体状態の情報が送信され、ステップS7にて、それぞれの軸制御部23〜29において、送信された評価結果および全体状態を参照して、アクチュエータの制御指令値の算出が行われる。
また、図2に示すマニピュレータ1の制御システムにおける各制御要素と、図3の制御処理フローとのより具体的な関係は次の通りである。まず、ステップS1は計測装置3s〜9sにて実行され、ステップS2〜S6は全体評価処理装置21にて実行される。さらに、ステップS7は制御計算処理装置3c〜9cにて実行され、ステップS8はアクチュエータ3a〜9aにて実行される。
なお、ステップS5において、マニピュレータ1のハンド18が制御目標19に到達したかどうかが評価結果として判断されることになるが、位置制御における制御幅や誤差を考慮して、制御目標19を含む所定の範囲内にハンド18が到達したかどうかを判断基準としている。
ここで、本第1実施形態の制御システムの動作の概要図を図4に示す。
図4において、全体評価部20は、マニピュレータ1全体の制御状態の評価を行う全体評価処理装置21と、複数の制御装置と相互通信可能な通信装置30を備えている。また、それぞれの関節軸3〜9の軸制御部23〜29を代表して、関節軸9の軸制御部29の構成について説明する。軸制御部29は、関節軸9における軸変位および軸速度を計測してアクチュエータ9aの制御指令値を算出する制御計算処理装置9cと、関節軸9を駆動するアクチュエータ9aと、全体評価部20内の通信装置30と通信可能な通信装置31を備えている。なお、その他の軸制御部23〜28も同様な構成を有している。さらに、全体評価部20内の通信装置30と、各関節軸3〜9の軸制御装置23〜29内の通信装置(通信装置31等)とは、有線または無線の情報通信手段であるネットワーク32で接続されている。
また、本第1実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図3の制御処理フロー図において、全体評価部20のステップS3にて算出されるハンド18の状態をハンド18の座標と速度とする。そして、同じく全体評価部20のステップS3にて算出される全体の状態変数(全体状態)を、ハンド18の速度とヤコビ行列とする。なお、本第1実施形態においては、全体の状態変数、すなわちハンド18の速度とヤコビ行列が、それぞれの関節軸3〜9の軸変位および軸速度を含む情報となっている。また、ハンド18のパラメータとしては位置座標や速度があり、関節軸3〜9のパラメータとしては軸変位や軸速度がある。
ハンド18の座標は、数式(1)の順運動学方程式に全軸の変位を代入することで求めることができる。さらに数式(1)の両辺を微分すると下記の数式(3)が得られる。
V=Jω ・・・(3)
数式(3)において、Vはハンド18の速度ベクトル、Jはヤコビ行列、ωは関節軸の軸速度のベクトルを表す。従って、計測された関節軸の軸速度を数式(3)に代入することでハンド18の速度を算出することができ、同時にヤコビ行列も算出することができる。
また、全体評価部20のステップS4にて算出される評価結果を、ハンド18の座標(先端位置)から制御目標(目標位置)19の座標を結ぶ偏差ベクトルとする。なお、この偏差ベクトルが位置差分の一例となっている。
なお、偏差ベクトルは下記の数式(4)より求めることができる。
D=G−H ・・・(4)
数式(4)において、Dは偏差ベクトル、Gは制御目標の座標ベクトルである。
また、軸制御部23〜29における制御方法としては、まず対象の関節軸の軸速度の変化によるハンド18の移動ベクトルの変化を検討する。ここで、対象とする第n軸(第n関節軸)の軸速度によって生成されるハンド18の速度ベクトルについては、数式(3)を用いて次のように算出することができる。
Vn=Jωn ・・・(5)
数式(5)において、Vnは第n軸の軸速度によって生成されるハンド18の速度ベクトル、ωnは第n軸の要素以外0(ゼロ)となる関節軸の軸速度ベクトルである。従って、第n軸の軸速度が微小な速度変化αで加速又は減速した場合のハンド18の移動ベクトルは、加速の場合は数式(6)にて、減速の場合は数式(7)にてそれぞれ表すことができる。
V+αn=J(ω+αn) ・・・(6)
V−αn=J(ω−αn) ・・・(7)
数式(6)、数式(7)において、V+αnは第n軸のみ速度変化αで加速した場合のハンド18の速度ベクトル、V−αnは第n軸のみ速度変化αで減速した場合のハンド18の速度ベクトル、αnは第n軸だけ速度変化αとしそれ以外の要素を速度変化0(ゼロ)とする関節軸の軸速度ベクトルである。
数式(3)、数式(6)、数式(7)より、現状の速度を維持する場合、加速する場合、減速する場合の3通りの場合のハンド18の移動ベクトルが算出できるので、以下の数式(8)を用いてそれぞれの偏差ベクトル方向成分の大きさを算出する。
C=V・D/|V|×|D| ・・・(8)
数式(8)において、Cは偏差ベクトル方向成分の単位成分あたりの大きさであり、VとDの方向ベクトルが作る角度をφとするとC=cosφとなり、このCが大きいほどφの角度が小さくなりVとDの方向が一致に近づくことが判る。従って、V、V+αn、V−αnについてそれぞれCを求め、このCが最も大きくなる、すなわちハンド18の速度ベクトルが制御目標19に最も近くなる関節軸の軸速度を制御指令値として算出している。
次に、それぞれの軸制御部23〜29による各関節軸3〜9の制御方法について、図5A及び図5Bに示す模式図を用いて説明する。図5Aはマニピュレータのハンドと制御目標の位置関係を示す模式図であり、図5Bは、マニピュレータの各関節軸の指令値の決定方法を示す模式図である。
図5Aにおいて、マニピュレータ1のハンド18の座標(H)から制御目標19の座標(G)を結んだベクトルが偏差ベクトル40(D)である。
また、図5Bにおいて、制御対象の関節軸6(第n軸)において、現状の速度ω1(ω1=ω)と、加速した場合の速度ω2(ω2=ω+αn)と、減速した場合の速度ω3(ω3=ω−αn)におけるハンド18の速度ベクトル42の候補はそれぞれV1、V2、V3となる。これらの速度ベクトル42と偏差ベクトル40の作る角度φ1、φ2、φ3の大きさをもとに関節軸6の軸速度を設定する。図5Bの場合、角度φ3が最も小さい角度となることから、速度ベクトルV3が偏差ベクトル40に最も近いと判断して、現状の速度Vよりα減速した速度ベクトルV3を軸速度の制御指令値として設定する。
上記の制御方法では、マニピュレータ1全体として速度、すなわちハンド18の速度の調整を直接的に行わず、各々の関節軸3〜9の軸速度を設定している。そのため、ハンド18の速度が異常に早くなってしまい危険な状況になる可能性や、制御目標に停止できず通りすぎてしまう可能性がある。そこで、例えば図6の模式図に示すようにしてハンド18の速度を制限することでこれらの問題を解決する。
図6は、本第1実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図である。
図6において、全体評価部20にて、制御目標19から所定範囲内の減速領域43を設定する。ここで、減速領域43は、球状でも円状でも良い。また、制御目標19やハンド18によっては、一方向に長い領域も考えられる。ハンド18が減速領域43より離れている場合の位置44での速度45をハンド18の上限速度とし、ハンド18と制御目標19との距離が短いほど上限速度を下げるように制御して、ハンド18が制御目標19に到達した際に速度が0となるように設定する。すなわち、ハンド18の位置44での速度45よりも、減速領域43内で制御目標19に近づいている位置46での速度47を小さくするように制御する。さらに、図3のステップS4にて算出される新たな評価結果として上限速度と現在速度の速度比を加え、さらに図3のステップS7において上限速度と現在速度の速度比が1を超える場合に、この速度比と選択された関節軸の軸速度を掛けたものを新たな関節軸の軸速度の制御指令値とする。
このような処理を加えることで、結果的に全ての関節軸の軸速度が上限速度に合わせて減速されて、ハンド18の速度の上限が上限速度となるように制御される。上限速度が制御目標19に近づくにつれて小さくなることで、制御目標19にハンド18が近づくにつれてハンド18の速度は減速されるため、制御目標19において確実にハンド18を停止させることができる。
また、マニピュレータの制御において解決すべき問題として、マニピュレータの特異姿勢がある。「特異姿勢」とは、マニピュレータの制御においてエラーを引き起こす特異な姿勢であり、この状態を如何に回避するかがマニピュレータの制御において重要な問題である。マニピュレータの特異姿勢を説明するために、図7A、図7B、及び図7Cに、本第1実施形態のマニピュレータ1の特異姿勢時の対応を示す模式図を示す。ここで、図7Aは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図であり、図7Bは、マニピュレータが特異姿勢から脱出するための第1の対応方法を示す模式図であり、図7Cは、マニピュレータが特異姿勢から脱出するための第2の対応方法を示す模式図である。
まず、図7Aにおいて、マニピュレータ1は、その関節軸7〜9をどのように駆動しても、そのハンド18の速度ベクトルが制御目標19に対して垂直な方向となってしまい、制御目標19に近づく方向に速度ベクトルを生成できない姿勢となっている。このようなマニピュレータ1の姿勢が「特異姿勢」である。本第1実施形態において、これまでに説明した制御方法だけでは、マニピュレータ1が特異姿勢となった場合、ハンド18が制御目標19に近づくことができずに停止してしまう。
そこで、本第1実施形態では、次のような2つの方法を用いることで特異姿勢への対応、すなわち特異姿勢からの脱出を可能とする。本第1実施形態では、ハンド18が制御目標19(目標位置から所定の範囲内)に到達していない状態にも関らず、全ての関節軸3〜9(図7A中では関節軸7〜9)の軸速度が全て0となった場合に特異姿勢が生じたものと判断する。例えば、図3の全体評価部20のステップS4において任意の設定時間以上、全ての関節軸3〜9において軸速度0が維持されたか否かで判断することができる。
まず、第1の対応方法は、図7Bで示すように、マニピュレータ1が特異姿勢になった場合に全体評価部20のステップS4において、任意の設定時間の間だけ本来の制御目標19(G)とずれた仮制御目標48(G2)を設定する方法である。仮制御目標48を設定し、評価結果である偏差ベクトルを算出した後、偏差ベクトルを評価結果としてそれぞれの軸制御部23〜29に送信する。各々の軸制御部23〜29において、送信された偏差ベクトルに基づいて、仮制御目標48に向かうハンド18の移動制御を行うことで、マニピュレータ1を特異姿勢から脱出させることができる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標19を再設定し、通常の制御を継続する。
次に、第2の対応方法は、図3の全体評価部20のステップS4で算出される新たな評価結果として特異姿勢の有無を加え、全体評価部20からそれぞれの軸制御部23〜29に特異姿勢の有無情報を送信する方法である。特異姿勢が生じている場合は、制御目標19を一時的に目標から切り離し、図7Cで示すようにそれぞれの軸制御部23〜29で任意の設定時間の間、それぞれの関節軸3〜9に対して任意の軸速度の遥動動作49を実施させ、この揺動動作49の実施により強制的に特異姿勢から脱出させる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標19を再設定し、通常の制御を継続する。
上述の本第1実施形態の説明においては、全体評価部20から全ての軸制御部23〜29に、評価結果および全体状態の情報を送信する(図3のステップS6)場合について説明したが、本第1実施形態はこのような場合についてのみ限定されるものではない。このような場合に代えて、例えば、関節軸3〜9のうちの制御対象となる関節軸を制御する軸制御部のみに、上記情報が送信されるような場合であってもよい。
本第1実施形態のマニピュレータ1の制御システムによる制御方法によれば、以下のような種々の効果を得ることができる。
本第1実施形態の制御方法では、全体評価部20にて作成されたマニピュレータ1の全体状態の評価結果の情報をそれぞれの軸制御部23〜29に送信して、この評価結果の情報に基づいて、個々の軸制御部23〜29において、他の関節軸とは独立して自軸の制御指令値を作成する。これにより、各々の関節軸単位にて独立して制御目標19に向かって制御を行うことで位置制御を行うことができる。そのため、冗長や非線形な駆動要素のあるマニピュレータ1においても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生しない。また、逆運動学の算出のために拘束条件を設定することや、自由度を制限する必要も無いため、高い自由度を維持したまま制御を行うことができる。さらに、動作の学習も必要としないため容易にマニピュレータの制御を行うことができる。
また、周囲の環境の影響や関節軸の故障によって一部の関節軸が稼動不能に陥っても、その他の関節軸が個別に制御目標19に向かうことによって自然に冗長性が発揮されるため、周囲の環境や軸の故障にロバスト性を有した状態で位置制御を行うことができる。なお、ここで、ロバスト性とは、外乱や設計誤差などの不確定な変動に対して、システム特性が現状の機能を維持できる性質を示す。なお、本発明における「故障」とは、関節軸の動作制御を行う軸制御部や全体評価部が、対象となる関節軸に何らかの問題が生じていることを認識しておらず、関節軸に対して制御指令を出しているのに関節軸が動作しない状態を示す。
さらに、それぞれの関節軸単位で制御指令値を算出することに伴いハンド18が速度オーバーとなることや、制御目標19を通過してしまうという課題に対しても、全体評価部20にてハンド18の速度制御などを行うことで解決することができる。また、マニピュレータ1の特異姿勢に対しても、全体評価部20にて、仮制御目標を一時的に設定することや、揺動動作を実施させることにより対応することができる。
従って、本第1実施形態のマニピュレータの制御方法によれば、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御を行うことができ、冗長や非線形な駆動要素があっても容易に位置制御を実現することができる。
さらに、本第1実施形態の制御方法は、マニピュレータの形状や軸数が変わっても同じ制御則を用いており、マニピュレータの形状パラメータを変更するだけで形状や軸数の変化に対応できる。
(第2実施形態)
本発明は上記第1実施形態に限定されるものではなく、その他種々の態様で実施できる。以下、本発明の第2実施形態にかかるマニピュレータの制御方法について説明する。
実際にマニピュレータを使用する場面では、様々な作業に柔軟に対応する必要が有る。そのために、作業に応じてマニピュレータの形状や軸数などを変化させることも考えられるが、本第2実施形態の制御方法を活用することで、マニピュレータの形状や軸数に合わせたプログラムの作成およびプログラムの書換えを不要とすることができる。これにより、マニピュレータの形状や軸数などが変更されるような場合であっても、パラメータの変更だけで対応することができ、プラグアンドプレイのようにマニピュレータをロボット等に追加・変更してすぐに使用することができる。
以下に、プラグアンドプレイによるマニピュレータの追加変更を実現する方法を、図面を参照しながら説明を行う。本発明の第2実施形態のマニピュレータ制御システムの模式図を図8に示す。
図8において、冗長なマニピュレータ101の制御システムは、マニピュレータ101の全体評価を行う全体評価部120と、それぞれの関節軸毎に独立して制御を行う軸制御部123〜129とを備えている。
全体評価部120は、評価計算装置131、通信装置132、現在制御中のマニピュレータ101が備えるそれぞれの関節軸3〜9の識別情報や形態情報などの軸情報をデータとして保持する記憶装置133と、記憶装置133のデータに基づいてマニピュレータ101を構成する関節軸3〜9の接続状態の変化を監視する監視装置134と、監視装置134により接続状態の変化を検出した際に記憶装置133に保持されたデータを変化した軸情報に更新する軸情報更新装置135と、を備えている。
また、それぞれの軸制御部123〜129は同様の構成を有しているため、代表して軸制御部123の構成について説明する。軸制御部123は、計測装置3s、制御計算処理装置3c、アクチュエータ3a、通信装置31、自軸の識別情報や形態情報などの軸情報を記憶する記憶装置50を備えている。
ここで、軸制御部123における記憶装置50に記憶された関節軸の識別情報は、例えば、関節軸毎に固有の識別情報であることが望ましく、通信装置31で送信する通信データにこの識別情報を付加することで、通信データがその関節軸から通知されたものなのか識別できる。それとともに、全体評価部120からの関節軸3〜9への指令の宛先として指定することができ、それぞれの軸制御部123〜129では自軸の識別情報と一致した場合のみ自軸の制御を行うことができる。
全体評価部120の通信装置132と軸制御部123の通信装置31とは、ネットワーク51を介して接続されている。ネットワーク51としては、例えば、全体評価部120を始点とし、マニピュレータ101の先端の関節軸9が終端となるようにマニピュレータ101の接続と同じ順でそれぞれの軸制御部123〜129に接続されたネットワークがある。このように接続することで各関節軸3〜9の接続状態がネットワーク51のルーティングを用いることで容易に把握することができる。なお、ネットワーク51の接続方法はこのような方法に限るものでは無く、その他の方法を用いることも考えられる。その際は、例えば隣り合った各関節軸の接続状態を検知し、その接続状態をネットワーク51にデータをして載せる事で各関節軸の接続状態を把握することができる。
このように検知される識別データおよび接続状態は、全体評価部120内の監視装置134で、常時、記憶装置133に記憶されたマニピュレータ101の軸情報と比較することで監視され、違いが発生すれば接続状態の変化として検出する。また、各関節軸3〜9から収集した軸情報を用いて軸情報更新装置135によって記憶装置133の軸情報を更新する。
続いて、プラグアンドプレイによりマニピュレータ101の構成要素を変更する方法を、図9から図13に示すマニピュレータ101の模式図を用いて説明する。
まず、図9は、マニピュレータの構成要素を変更する前の状態を示す模式図である。
図9において、全体評価部120では、それぞれの軸制御部120から送られてくる各関節軸3〜9の通信情報64に含まれる関節軸の識別情報およびルーティング等を利用した接続情報と、記憶装置133に記憶されている軸情報データ65とを比較する。それとともに、全体評価部120において、全体の評価が行われ、記憶装置133の軸情報データに基づいて制御対象となる関節軸を設定し、その識別情報を含む通信情報66をそれぞれの軸制御部123〜129に送信する。それぞれの軸制御部123〜129では、その通信情報66に自軸の識別情報を含む場合にのみ自軸の制御を行う。なお、図9から図13において、説明の理解を容易にするために、それぞれの関節軸3〜9の制御を行う軸制御部123〜129をA1〜A7にて示すとともに、それぞれの関節軸3〜9の識別情報をD1〜D7にて示す。また、後述するように変更された構成要素である関節軸の軸制御部をA8にて示すとともに、その識別情報をD8にて示す。
図10は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図である。
図10において、マニピュレータ101の先端部(ハンド及び関節軸)67が、例えば、識別情報D7から識別情報D8の関節軸に変更された場合を例として説明する。関節軸が変更された瞬間にそれぞれの軸制御部(123等)では通常どおり各関節軸の制御計算処理装置(3c等)で収集されたデータが通信情報64として全体評価部120に送信される。ただし、この場合、構成要素の変更が行われているため、通信情報64には異なる識別情報D8が含まれている。全体評価部120では、監視装置134にて通信情報及びそのルーティング等と記憶装置133の軸情報との比較を行う。この比較実施の結果、先端部67の識別情報が異なることが検出され、監視装置134にてマニピュレータ101の構成要素に変更が生じていることが検知される。
図11は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図である。
図11において、監視装置134にて、マニピュレータ101の先端部67の変更を検知し、軸情報データ65内に不明な部分を発見した全体評価部120は、新たな先端部67の形態等が未知である識別情報D8を有する関節軸に対して、軸情報を要求する通信情報68を送信する。
図12は、軸制御部からパラメータが応答されている状態を示す模式図であり、図13は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図である。
図12において識別情報D8を含む通信情報68を受けたそれぞれの軸制御部A1〜A8(A7を除く)では、識別情報D8を有する軸制御部A8のみが反応し、自軸の形態情報を含む通信情報69を全体評価部120に返信する。全体評価部120では、この通信情報69を受信することで軸情報更新装置135を用いて記憶装置133内の軸情報データ65の更新を行う。なお、この時点までは識別情報D8の軸制御部A8は、アクチュエータの制御は行っていない。
図13において、変更の結果が記憶装置133内の軸情報データ65に登録されると、全体評価部120では、新しい軸情報データ65に従って全体評価の計算を行い、その結果を通信情報70として送付する。この通信情報70には変更後の構成を反映した識別情報を含んでおり、この識別情報によりこれまでアクチュエータの制御を行っていなかった識別情報D8の軸制御部A8も、他の軸制御部A1〜A6と同様に制御を開始する。その後、通常通り各関節軸の計測装置での収集したデータ71が送付され、このデータ71に基づいて全体評価部120では監視と制御が繰り返して行われる。
以上のようなマニピュレータの制御方法によれば、マニピュレータの形状や軸数に関係無く、統一した制御則で制御が行われるとともに、自律的にマニピュレータの構成を検出してパラメータを更新することができる。そのため、マニピュレータの構成要素が変化した場合でも、プラグアンドプレイ、すなわち、制御が途切れることなく安全に継続することができる。従って、作業に応じてマニピュレータの形状や軸数を容易に変化させることができ、さらに幅広い作業に柔軟かつ容易に適応することができる。
(第3実施形態)
次に、本発明の第3実施形態にかかるマニピュレータの制御システムによる制御方法について、図14に示すマニピュレータの制御システムの模式図を用いて説明する。
図14に示すように、本第3実施形態のマニピュレータ201の制御システムは、それぞれの関節軸3〜9の制御を他の関節軸から独立して行う軸制御部223〜229と、マニピュレータ201の全体状態等の評価を行う全体評価部220とを備えている。
それぞれの軸制御部223〜229は、関節軸3〜9に個別に装備されたエンコーダなどのセンサを有する計測装置(9s等)と、関節軸を駆動するアクチュエータ(9a等)と、入力された情報に基づいてアクチュエータの制御指令値を算出する制御計算処理装置(9c等)とを備えている。全体評価部220は、全体評価処理装置221を備えている。
また、本第3実施形態のマニピュレータ201の制御方法は、マニピュレータ201のハンド18を制御目標19に移動させるように位置制御を行いながら、ハンド18から作業対象物209に一定の接触力を付加させた状態とする力制御を行うものである。このような力制御を実現するために、ハンド18には接触力を計測する力計測装置222が備えられている。
このような構成の制御システムによるマニピュレータ201の位置制御及び力制御の制御処理フローを図15に示す。
図15に示すように、ステップS11にて、それぞれの計測装置(9s等)により各関節軸3〜9の軸変位および軸速度が計測される。次に、ステップS12にて、計測された軸変位・軸速度の情報が、それぞれの軸制御部223〜229から全体評価部220に送信されて、軸変位・軸速度の情報が収集される。それと共に、作業対象物209に接触した状態にあるハンド18の接触力が、力計測装置222により計測されて、全体評価部220に収集される。その後、ステップS13にて、各関節軸3〜9の軸変位および軸速度の情報、並びにハンド18の接触力の情報から、ハンド18の状態および全体の状態変数(全体状態)を算出する。続いて、ステップS14にて、ハンド18の状態に基づいて、マニピュレータ201の制御状態を評価する(第1の工程)。なお、この制御状態、すなわち位置制御と力制御の状態の詳しい評価方法については後述する。その後、ステップS15にて、評価結果に基づいて制御目標に到達していないと判断される場合には、ステップS16にて、評価結果および全体状態の情報が、全体評価部220からそれぞれの軸制御部223〜229に送信される(第2の工程)。
各々の軸制御部223〜229では、ステップS17にて、自軸の軸変位・軸速度の情報と、送信された評価結果およびマニピュレータの全体状態の情報とに基づいて、自軸のアクチュエータの制御指令値を他の関節軸とは独立して算出する(第3の工程)。続いて、ステップS18にて、算出された制御指令値に基づいて、自軸のアクチュエータを駆動する。これらステップS11〜S18の制御処理が繰り返し実行される。ステップS15にて、評価結果が、マニピュレータ201のハンド18が制御目標(目標位置及び目標接触力)に到達していると判断された場合には、マニピュレータ201の位置制御および力制御が完了する。なお、ステップS11、S17、及びS18の制御処理はそれぞれの軸制御部223〜229にて独立して実行され、ステップS12〜S16の制御処理は全体評価部120にて実行される。
ここで、本第3実施形態の制御システムの動作の概要図を図16に示す。
図16において、全体評価部220は、マニピュレータ201全体の制御状態の評価を行う全体評価処理装置221と、複数の制御装置と相互通信可能な通信装置230を備えている。また、それぞれの関節軸3〜9の軸制御部223〜229を代表して、関節軸9の軸制御部229の構成について説明する。軸制御部229は、関節軸9における軸変位および軸速度を計測してアクチュエータ9aの制御指令値を算出する制御計算処理装置9cと、関節軸9を駆動するアクチュエータ9aと、全体評価部220内の通信装置230に対して通信可能な通信装置231と、を備えている。なお、その他の軸制御部223〜228も軸制御部229と同様な構成を有している。さらに、全体評価部220内の通信装置230と、各関節軸3〜9の軸制御装置223〜229内の通信装置(通信装置231等)とは、有線または無線の情報通信手段であるネットワーク232で接続されている。
また、本第3実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図15の制御処理フローにおいて、全体評価部220のステップS13にて算出されるハンド18の状態をハンド18の座標及び速度並びにハンド18の接触力とする。そして、同じく全体評価部220のステップS13にて算出される全体の状態変数(全体状態)を、ハンド18の速度及び接触力とヤコビ行列とする。なお、本第3実施形態においては、全体の状態変数、すなわちハンド18の速度及び接触力とヤコビ行列が、それぞれの関節軸3〜9の軸変位および軸速度を含む情報となっている。また、本第3実施形態において、ハンド18のパラメータは、速度と接触力となっている。
マニピュレータ201のハンド18の座標は数式(1)に全て関節軸3〜9の軸変位を代入することで求めることができる。
また、計測された関節軸3〜9の軸速度を数式(3)に代入することで、ハンド18の速度を算出することができ、同時にヤコビ行列も算出することができる。
また、全体評価部220のステップS14にて算出される評価結果は、例えば図17で示すように算出する。まず、手先座標251から制御目標18の座標を結ぶ位置の偏差ベクトルを算出する。なお、位置の偏差ベクトルDpは数式(9)により求めることができる。なお、Gは制御目標19の座標ベクトルである。
Dp=G−H ・・・(9)
次に、制御目標の接触力Ftと実際の接触力Frとの接触力差、すなわち力差分を位置偏差に換算したもの算出する。この力の換算偏差ベクトルDfは数式(10)のように表すことができる。なお、Kfは制御ゲインであり、系によって適当な値が選択される。
Df=Kf(Ft−Fr) ・・・(10)
これらの数式(9)、(10)にて表される位置の偏差ベクトルDpと力の換算偏差ベクトルDfを用いて、制御評価に用いる総合偏差ベクトルDを数式(11)のように算出する。
D=Dp+Df ・・・(11)
このように数式(11)にて総合偏差ベクトルDを算出した後は、上記第1実施形態における位置制御方法と同様な手順にて、数式(3)、(5)、(6)、(7)、(8)を用いて、各々の関節軸3〜9における現状の速度を維持する場合(V)、加速する場合(V+αn)、減速する場合(V−αn)の3通りのハンド18の移動ベクトルと、それぞれの偏差ベクトル方向成分の大きさCとを算出する。算出した結果を参照して、このCが最も大きくなる、すなわちハンド18の速度が制御目標の方向にもっとも近くなる関節軸の軸速度を制御目標として採用する。
この制御目標に対して第n軸の制御は、数式(12)に示すように、現状の出力トルクTnにΔTnを加えた(Tn+ΔTn)のトルクが出力されるような制御として行われる。
ΔTn=Mn・Δωn/Δt+Kdn・Δωn ・・・(12)
ここで、Δωnは選出された第n軸の軸速度の制御目標と、現在の軸速度との速度差であり、Δtは制御単位時間を表す。また、Mnは第n軸の慣性モーメント、Kdnは第n軸の粘性抵抗成分とする。このように算出される制御トルク出力(Tn+ΔTn)を用いることより、ハンド18の目標位置への到達及び目標接触力の発生をより効果的に実現することができるような第n軸の軸速度の制御指令値を算出することができる。
以上のような解決手段のようなマニピュレータの制御方法によれば、周囲の環境の影響や軸の故障によってある関節軸が移動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮される。そのため、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御及び力制御を行うことができる。従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易にかつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。
なお、本第3実施形態においては、マニピュレータ201のハンドの位置制御と力制御とが組み合わされて行われるような場合を例として説明したが、このような場合に代えて、力制御だけが行われるような場合にも、本第3実施形態の制御方法を適用することができると考えられる。
なお、上述のそれぞれの実施形態の説明では、マニピュレータ1等のアーム部分を支持するベース2がある位置に固定された状態にて、マニピュレータ1等の動作が制御されるような場合について説明した。しかしながら、本発明はこのような場合についてのみ限定されるものではなく、例えば、図18の模式図に示すように、マニピュレータ301のアーム部分を支持するベース302に車輪等を用いた移動装置305を備えさせ、マニピュレータ301の設置位置を移動可能とするような構成を採用することもできる。
なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏する構成とすることができる。
本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
2006年3月24日に出願された日本国特許出願No.2006−082269号の明細書、図面、及び特許請求の範囲の開示内容は、全体として参照されて本明細書の中に取り入れられるものである。
本発明のマニピュレータの制御方法および制御システムによれば、周囲の環境や軸の故障などの不確かさ、変動を有した状態でも制御可能なロバスト性を有する位置制御を実現することができる。冗長や非線形な駆動要素があっても少ないセンサを用いて容易にかつ柔軟に位置制御を実現することができるため、より複雑で高い自由度を必要とする作業を行う産業用、および民生用ロボットのアームなどに使用することができる。特に、障害物が多く、さらにその位置が特定されていない環境が多く存在する家庭用ロボットのアームとしての用途に有用である。
本発明は、複数の関節軸を有するマニピュレータを制御する技術に関する。
従来、多関節マニピュレータは、産業用や民生用のロボットアームなどに用いられている。このようなマニピュレータの位置制御に関する技術としては、様々な技術が存在する。
例えば、特定のマニピュレータの手先座標などを実現する各関節軸の変位や駆動力を、逆運動学を計算して求め、その変位や駆動力に基づいて制御を行う逆運動学を用いた技術がある。
この逆運動学を用いた従来技術の例を、図19、図20を用いて説明する。ここで、図19は従来のマニピュレータの制御方法の概要を示す図であり、図20は従来のマニピュレータの制御処理フロー図である。
図19において、マニピュレータ501の各関節軸511には、エンコーダなどのセンサ502(センサS)と、関節軸511を個別に駆動するアクチュエータ503(アクチュエータA)が搭載されている。これらを用いて、マニピュレータ501の手先504を制御目標505(制御目標G)に対して位置制御する。マニピュレータ501は、全体の制御を行う全体制御部506を備えており、センサ502で計測した結果を用いてアクチュエータ503の制御指令値を算出している。なお、全体制御部506は全体評価処理部507、軌道計画処理部508、制御計算処理部509の3つの処理部を有している。手先504は制御目標505まで、制御軌道510のように位置制御される。
図20において、制御処理フローは、まずステップS51で、各軸に搭載されたセンサ502によって各関節軸511の変位(軸変位)、変位速度(軸速度)を計測する。次にステップS52で、各関節軸511より収集した変位および変位速度の情報より手先504の座標を算出する。続いてステップS53で、目標位置505に移動するための手先軌道510を算出する。続いてステップS54で、目標軌道510を実現するための各関節軸511の変位および変位速度の制御指令値を算出する。続いてステップS55で、制御指令値を実現するように各関節軸511のアクチュエータ503を駆動する。図19の手先504が制御目標505に到達するまで、このようなステップS51〜ステップS55の処理を繰り返し実行している。なお、図20の制御処理と図19の制御要素との関係は、ステップS51はセンサ502で実行し、ステップS52は全体評価処理部507で実行し、ステップS53は軌道計画処理部508で実行し、ステップS54は制御計算処理部509で実行し、ステップS55はアクチュエータ503で実行されている。
ここで、ステップS52において手先504の座標を算出するために一般的に行われる計算式は、数式(1)のように表される。
H=T(θ) ・・・(1)
数式(1)はリンク機構の運動学方程式と呼ばれ、Hは手先の座標ベクトル、θは関節軸の変位ベクトルを示す。
また、ステップS54において、変位速度の制御指令を算出するために一般的に行われる計算式は、数式(2)のように表される。
θc=T−1(Ho) ・・・(2)
数式(2)はリンク機構の逆運動学方程式と呼ばれ、Hoは目標軌道を実現する手先の目標座標ベクトル、θcはその手先の目標座標ベクトルを実現する関節軸の変位ベクトルを示す。ただし、数式(2)は冗長な制御系では一意に解けないなどの問題があるため、数式(2)の代わりに別な計算方法を用いたものもある(例えば、特許文献1、特許文献2参照)。
特許文献1、特許文献2に開示された従来技術の例を、図21A、図21B、及び図22を用いて説明する。ここで、図21Aは従来のマニピュレータの制御処理フロー図であり、図21Bは従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図であり、図22は従来のマニピュレータの制御方法を示す図である。
特許文献1開示の制御方法は、図21A、図21Bのように2つの方法のいずれかを用いてマニピュレータの手先座標を実現する関節軸の変位を決定している。図21Aにおいては、冗長マニピュレータ520において、各関節軸521の変位を均等に配分することで変位522を決定している。また、図22Bにおいては、それぞれの各関節軸521において駆動させる関節軸523を必要次元だけ選択し、冗長マニピュレータ520を制御的に非冗長とすることで関節軸521の変位を算出可能としている。
特許文献2開示の制御方法は、制御部に逆運動学の関係を学習させることでアクチュエータの駆動力を算出できるようにしている。図22において、目標軌道であるPdと実際の軌道Pの誤差から演算装置534でトルクTを算出し、そのトルクTによりアクチュエータ535を駆動している。この時、目標軌道Pdを微分回路536に通したものとトルクTとを多層神経回路537に入力することで多層神経回路537の学習を行っており、学習の結果であるその出力をトルクTに加えてアクチュエータ535に入力することで制御を行っている。
また、逆運動学を用いずに分散的に制御を行う方法もある(例えば、特許文献3、特許文献4参照)。
特許文献3、特許文献4に開示された従来技術の例を、図23、図24を用いて説明する。ここで、図23は特許文献3開示の制御方法を示す図であり、図24は特許文献4開示の制御方法を示す図である。
特許文献3開示の制御方法は、マニピュレータによって多変数評価関数の各関節角の偏微分を用いて制御を行っている。図23において、まず、マニピュレータ540は軌道549上を移動するように制御を行っている時の各関節541の関節角θ1、θ2を用いて多変数評価関数542を設定する。続いて、多変数評価関数542を任意の時点での各角度変数で偏微分した関節角の偏微分543を用いて、多変数評価関数542が所定の条件を満たすように分散的に各関節541の制御を行っている。
図24において、特許文献4の方法は、制御装置によってマニピュレータ先端の位置に対応した関節部負荷閾値を用いて制御を行っている。位置検出装置553で検出されたマニピュレータの位置を計測し、動作制御装置554では計測された位置情報に従った負荷閾値を選択し、各関節角に設けられた負荷検出装置555が負荷閾値となるように関節角のサーボドライバ556を用いてアクチュエータ557を分散的に制御している。
従来、上記のような逆運動学を用いた制御や分散的な制御を行って、多関節マニピュレータの位置制御を行っている。
特開平07−164360号公報
特開平02−054304号公報
特開平09−207087号公報
特開2000−094368号公報
しかしながら、上記従来の制御方式を、高い自由度が要求される作業を行うマニピュレータに適応しようとする場合、以下のような課題がある。
従来の逆運動学を用いた方式では、非冗長で非線形要素が無い場合には使用できるが、環境によっては目的とする作業を行うことができない可能性がある。これは、マニピュレータの状態が一意に決まってしまうため、手先が目標位置まで到達不可能となるためである。一方、作業の自由度を上げるために冗長や非線形な駆動要素を取り入れると、逆運動学方程式である数式(2)が複雑になって一意に解けず、結果として制御ができない場合や、計算量が莫大になり、リアルタイムな制御が不可能になる場合がある。
特許文献1の場合は、冗長なマニピュレータに対して形状や自由度低減を利用して拘束条件を設け、その制御を可能としている。しかしながら、一方で、その拘束条件によって自由度を制限してしまうため、マニピュレータの作業性が制限されてしまう可能性がある。また、特許文献2の場合は、多層神経回路の学習によって冗長や非線形の駆動要素による計算上の問題やロバスト性を解決することは可能である。しかしながら、パラメータや教示データを適切に設定しないと、学習に時間を要する場合や計算結果が収束しない場合など、容易に適応できない可能性がある。
また、逆運動学を用いた方法では、いずれも場合も制御目標へ到達するため全軸の変位や駆動力が一意に決まるため、周囲に環境の影響や故障によって一つの軸が移動不可能な状態に陥ると全体の位置制御を実現できなくなる。
特許文献3は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、評価関数やその計算処理についてマニピュレータの形状や作業内容に応じて設定する必要であり、評価関数によっては作業が実行できない可能性がある。また、特許文献4は、マニピュレータの形状に関係なく各関節で統一的な分散制御を行うことが可能である。しかしながら、関節の負荷は位置姿勢だけでなく軌道や作業時にマニピュレータに取り付けるツールなどにより変化するため、作業毎に教示や学習により関節の負荷閾値を事前に登録する必要があり、想定外の状況には対応できない。
このようなマニピュレータの制御に関する課題は、マニピュレータの手先の位置制御が行われる場合だけでなく、手先の力制御が行われる場合にも生じる。
従って、本発明の目的は、上記課題を解決することにあって、周囲の環境や関節軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することにある。
上記目的を達成するために、本発明は以下のように構成する。
本発明の第1態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部のパラメータと目標値との差分を計測する第1の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記差分の差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記軸情報と上記差分情報とに基づいて、夫々の上記軸制御部で上記関節軸のパラメータを独立に補正する第3の工程と、を備え、
上記先端部のパラメータが上記目標値を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第2態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する第1の工程と、
上記複数の関節軸の軸変位及び軸速度を含む軸情報と上記位置差分の位置差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記軸情報と上記位置差分情報とに基づいて、夫々の上記軸制御部で上記関節軸の軸変位と軸速度とを独立に補正する第3の工程と、を備え、
上記先端位置が上記目標位置を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第3態様によれば、上記複数の関節軸から取得された軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む軸情報を上記関節軸毎に作成し、
その後、上記第2の工程を実施する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第4態様によれば、上記第2の工程において、上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記関節軸毎に送信すると共に、上記目標位置に対する上記先端位置の偏差ベクトルの情報を上記位置差分の情報として上記関節軸毎に送信し、
上記第3の工程において、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて上記関節軸の軸速度ベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように上記軸速度ベクトルを上記関節軸毎に補正する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第5態様によれば、上記第2の工程において、上記先端位置と上記目標位置との距離に応じて設定された先端速度の上限値と実際の先端速度との速度比の情報を含む情報が上記関節軸に送信され、
上記第3の工程において、上記複数の軸制御部にて上記速度比に応じて上記軸速度を上記関節軸毎に補正される、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第6態様によれば、取得された上記関節軸の軸速度が全て0である場合に、上記目標位置の座標又は上記軸速度を一時的に変化させ、
その後、上記第2の工程を実施する、第3態様に記載のマニピュレータの制御方法を提供する。
本発明の第7態様によれば、取得された上記複数の関節軸が有する自軸の識別情報を統合して、上記複数の関節軸の接続状態の変化を検知する、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第8態様によれば、上記複数の関節軸の識別情報を上記全体制御部から上記複数の軸制御部に送信した後、上記複数の軸制御部において、予め保持された自軸の識別情報が上記送信された識別情報と一致した場合にのみ上記関節軸の制御を行う、第2態様に記載のマニピュレータの制御方法を提供する。
本発明の第9態様によれば、複数の関節軸を有するマニピュレータの制御方法であって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する第1の工程と、
上記複数の各関節軸の軸変位及び軸速度を含む軸情報と上記力差分の力差分情報とを、上記関節軸の軸制御部に夫々送信する第2の工程と、
上記複数の関節軸の軸変位及び軸速度を含む情報と上記力差分の情報とに基づいて、上記軸制御部の上記関節軸の軸変位と軸速度とを上記関節軸毎に独立に修正する第3の工程と、を備え、
上記先端部に加わる接触力が上記目標接触力を含む設定範囲内に達するまで上記第1の工程から上記第3の工程を繰り返す、マニピュレータの制御方法を提供する。
本発明の第10態様によれば、取得された上記複数の関節軸の軸変位及び軸速度の情報に基づいて、上記複数の関節軸の軸変位及び軸速度を含む情報を上記関節軸毎に作成し、
その後、上記第2の工程を実施する、第9態様に記載のマニピュレータの制御方法を提供する。
本発明の第11態様によれば、上記第1の工程において、上記先端部の位置と目標位置との位置差分をさらに計測し、
上記第2の工程において、上記軸変位及び上記軸速度を用いて算出されるヤコビ行列と上記マニピュレータの先端速度とを含む情報を上記複数の関節軸に送信すると共に、上記目標位置及び目標接触力に対する上記先端部の偏差ベクトルの情報を作成して、上記力差分が位置差分に換算された換算位置差分を上記位置差分に加えた合計差分の情報として送信し、
上記第3の工程において、上記先端速度、上記関節軸の軸速度、及び上記ヤコビ行列を用いて、上記関節軸の軸速度のベクトルに基づく上記先端位置の移動ベクトルを算出し、上記移動ベクトルが上記偏差ベクトルに近づくように上記軸速度のベクトルを上記関節軸毎に修正する、第9態様に記載のマニピュレータの制御方法を提供する。
本発明の第12態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部のパラメータと目標値との差分を計測する計測装置と、
上記差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータを制御する全体制御部と、
上記制御の評価結果情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記評価結果情報に基づいて上記関節軸のパラメータを夫々の上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
本発明の第13態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端位置と目標位置との位置差分を計測する計測装置と、
上記位置差分の情報と上記複数の関節軸の軸変位及び軸速度とを含む情報に基づいて上記マニピュレータの位置を制御する全体制御部と、
上記位置制御情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記位置制御情報に基づいて上記関節軸の軸変位及び軸速度を夫々の上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
本発明の第14態様によれば、上記全体制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記記憶装置に記憶された上記軸情報に基づいて上記複数の関節軸の接続状態を監視する監視装置と、
上記監視装置により上記複数の関節軸の接続状態の変化を検出した際に、上記記憶装置に記憶された上記軸情報を更新する軸情報更新装置と、を備える、第13態様に記載のマニピュレータの制御システムを提供する。
本発明の第15態様によれば、上記各々の軸制御部は、
上記関節軸の識別情報又は形態情報を含む軸情報を記憶する記憶装置と、
上記関節軸が他の関節軸に接続されることにより、上記記憶装置に記憶されている上記軸情報を上記全体制御部に通知する通信装置と、を備える、第13態様に記載のマニピュレータの制御システムを提供する。
本発明の第16態様によれば、複数の関節軸を有するマニピュレータの制御システムであって、
上記マニピュレータの先端部に加わる接触力と目標接触力との力差分を計測する計測装置と、
上記力差分の情報と全ての上記関節軸の軸変位及び軸速度を含む情報に基づいて上記マニピュレータのトルク制御を行う全体制御部と、
上記トルク制御情報を上記全体制御部から上記複数の関節軸に送信する送信装置と、
上記複数の関節軸に夫々備えられ、送信された上記トルク制御情報に基づいて、上記関節軸の軸変位及び軸速度を上記関節軸で独立に修正して上記関節軸の駆動を制御する複数の軸制御部と、を備える、マニピュレータの制御システムを提供する。
以上のような本発明のマニピュレータの制御方法および制御システムによれば、制御目標(目標位置を含む所定の範囲)に向かう位置制御を軸単位に独立して行うため、冗長や非線形な駆動要素のあるマニピュレータにおいても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生せず、確実にマニピュレータの位置制御を行うことができる。
また、逆運動学の算出のための拘束条件の設定や、自由度の制限を行う必要も無いため、高い自由度を維持したまま制御を行うことができる。また、データの蓄積による学習も必要としないため、容易にマニピュレータを制御することもできる。
また、周囲の環境の影響や軸の故障によって一部の関節軸が稼動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮されるため、周囲の環境や関節軸の故障などの不確かさに依存しない制御を行うことができる。
従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易かつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。
本発明の記述を続ける前に、添付図面において同じ構成要素については同じ参照符号を付して、説明を省略している。以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
(第1実施形態)
以下に本発明の第1実施形態にかかるマニピュレータの制御システム及びその制御方法について、図面を参照しながら説明を行う。
図1は、本発明の第1実施形態のマニピュレータの模式図である。図1に示すように、マニピュレータ1は、7個の関節軸3〜9と、それぞれの関節軸を接続する8個のリンク10〜17と、リンク10を介して一連に接続された関節軸3〜9を支持するベース2と、一連に接続された関節軸3〜9の先端に接続されたハンド(先端部あるいは手先)18とを備えている。マニピュレータ1は、それぞれの関節軸3〜9の動作によって、ハンド18を制御目標に位置させることが可能となっている。
図2は、本第1実施形態の制御システムの模式図である。制御システムは、それぞれの関節軸3〜9の動作制御を行うことで、マニピュレータ1のハンド18を制御目標19(G)に位置させるための制御を行う。図2に示すように、マニピュレータ1の制御システムは、各々の関節軸3〜9の駆動動作を軸毎に個別に制御する軸制御部23〜29と、マニピュレータ1の全体動作の評価を行う全体制御部の一例である全体評価部20とを備えている。
軸制御部23は、エンコーダなどのセンサを有し、関節軸3の状態(軸変位及び軸速度)を計測する計測装置3sと、関節軸3を駆動させるアクチュエータ3aとを備えている。また、計測装置3sおよび全体評価部20から入力される情報に基づいてアクチュエータ3aを駆動制御するための情報を演算により作成し、アクチュエータ3aの駆動制御を他の関節軸4〜9から独立して行う制御計算処理装置3cも備えている。なお、計測装置3s及びアクチュエータ3aは、関節軸3に装備されている。軸制御部23と同様に、軸制御部24〜29にも、計測装置4s〜9s、アクチュエータ4a〜9a、及び制御計算処理装置4c〜9cが備えられている。また、全体評価部20は、全体評価処理装置21を備えている。
図3は、本第1実施形態の制御処理フローを示す図である。図3においては、全体評価部20における制御処理フローと、それぞれの軸制御部23〜29における制御処理フローとを分けて示すとともに、両者の関連について示している。
図3の制御処理フローにおいて、ステップS1にて、各関節軸3〜9の軸制御部23〜29が備える計測装置3s〜9sにより関節軸の軸変位および軸速度を計測する。次に、ステップS2にて、各関節軸3〜9の軸制御部24〜29から全体評価部20に計測された関節軸の軸変位および軸速度の情報が送信されて収集される。その後、ステップS3で、全ての関節軸3〜9の軸変位および軸速度の情報を用いて、マニピュレータ1のハンド18の状態、および全体の状態変数(全体状態)を算出する。続いて、ステップS4で、ハンド18の状態から、マニピュレータ1の位置制御状態を評価する(第1の工程)。その後、ステップS5にて、評価結果を参照して、マニピュレータ1のハンド18が制御目標(目標位置)19に到達しているか否かを示しているかどうかの判断を行う。制御目標19に到達していない場合には、ステップS6にて、評価結果および全体状態の情報を、それぞれの軸制御部23〜29に送信する(第2の工程)。
次に、ステップS7にて、それぞれの軸制御部23〜29にて、全体評価部20より送信されたマニピュレータ1の評価結果および全体状態の情報と、自軸の軸変位・軸速度の情報(ステップS1にて計測された情報)に基づいて、自軸のアクチュエータ3a〜9aの制御指令値(修正量)を他の関節軸とは独立して算出する(第3の工程)。続いて、ステップS8にて、算出された制御指令値に基づいて、自軸のアクチュエータ3a〜9aを駆動する。その後、ステップS1〜S8の制御処理が繰り返し実行される。ステップS5にて、評価結果が、マニピュレータ1のハンド18が制御目標19に到達していると判断された場合には、制御目標19に対するマニピュレータ1の位置制御が完了する。なお、ステップS1、S7、及びS8の制御処理はそれぞれの軸制御部23〜29にて独立して実行され、ステップS2〜S6の制御処理は全体評価部20にて実行される。また、全体評価部20及び軸制御部23〜29においてステップS1〜S8までの処理が繰り返されるサンプリング時間は、10ms以下の時間、例えば1ms程度の時間に設定される。ただし、このようなサンプリング時間の設定は、マニピュレータ1のハンド18をどれくらいの時間で制御目標に近づけるのかという原因により決定される。
図3の制御処理フローに示すように、全体評価部20とそれぞれの軸制御部23〜29との間における情報の受け渡し等の関係は次のようになっている。まず、ステップS1にて、それぞれの軸制御部23〜29で計測を行った関節軸の軸変位・軸速度の情報が、ステップS2にて、それぞれの軸制御部23〜29より送信されて、全体評価部20にて収集される。また、ステップS6にて、全体評価部20からそれぞれの軸制御部23〜29に、マニピュレータ1の評価結果と全体状態の情報が送信され、ステップS7にて、それぞれの軸制御部23〜29において、送信された評価結果および全体状態を参照して、アクチュエータの制御指令値の算出が行われる。
また、図2に示すマニピュレータ1の制御システムにおける各制御要素と、図3の制御処理フローとのより具体的な関係は次の通りである。まず、ステップS1は計測装置3s〜9sにて実行され、ステップS2〜S6は全体評価処理装置21にて実行される。さらに、ステップS7は制御計算処理装置3c〜9cにて実行され、ステップS8はアクチュエータ3a〜9aにて実行される。
なお、ステップS5において、マニピュレータ1のハンド18が制御目標19に到達したかどうかが評価結果として判断されることになるが、位置制御における制御幅や誤差を考慮して、制御目標19を含む所定の範囲内にハンド18が到達したかどうかを判断基準としている。
ここで、本第1実施形態の制御システムの動作の概要図を図4に示す。
図4において、全体評価部20は、マニピュレータ1全体の制御状態の評価を行う全体評価処理装置21と、複数の制御装置と相互通信可能な通信装置30を備えている。また、それぞれの関節軸3〜9の軸制御部23〜29を代表して、関節軸9の軸制御部29の構成について説明する。軸制御部29は、関節軸9における軸変位および軸速度を計測してアクチュエータ9aの制御指令値を算出する制御計算処理装置9cと、関節軸9を駆動するアクチュエータ9aと、全体評価部20内の通信装置30と通信可能な通信装置31を備えている。なお、その他の軸制御部23〜28も同様な構成を有している。さらに、全体評価部20内の通信装置30と、各関節軸3〜9の軸制御装置23〜29内の通信装置(通信装置31等)とは、有線または無線の情報通信手段であるネットワーク32で接続されている。
また、本第1実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図3の制御処理フロー図において、全体評価部20のステップS3にて算出されるハンド18の状態をハンド18の座標と速度とする。そして、同じく全体評価部20のステップS3にて算出される全体の状態変数(全体状態)を、ハンド18の速度とヤコビ行列とする。なお、本第1実施形態においては、全体の状態変数、すなわちハンド18の速度とヤコビ行列が、それぞれの関節軸3〜9の軸変位および軸速度を含む情報となっている。また、ハンド18のパラメータとしては位置座標や速度があり、関節軸3〜9のパラメータとしては軸変位や軸速度がある。
ハンド18の座標は、数式(1)の順運動学方程式に全軸の変位を代入することで求めることができる。さらに数式(1)の両辺を微分すると下記の数式(3)が得られる。
V=Jω ・・・(3)
数式(3)において、Vはハンド18の速度ベクトル、Jはヤコビ行列、ωは関節軸の軸速度のベクトルを表す。従って、計測された関節軸の軸速度を数式(3)に代入することでハンド18の速度を算出することができ、同時にヤコビ行列も算出することができる。
また、全体評価部20のステップS4にて算出される評価結果を、ハンド18の座標(先端位置)から制御目標(目標位置)19の座標を結ぶ偏差ベクトルとする。なお、この偏差ベクトルが位置差分の一例となっている。
なお、偏差ベクトルは下記の数式(4)より求めることができる。
D=G−H ・・・(4)
数式(4)において、Dは偏差ベクトル、Gは制御目標の座標ベクトルである。
また、軸制御部23〜29における制御方法としては、まず対象の関節軸の軸速度の変化によるハンド18の移動ベクトルの変化を検討する。ここで、対象とする第n軸(第n関節軸)の軸速度によって生成されるハンド18の速度ベクトルについては、数式(3)を用いて次のように算出することができる。
Vn=Jωn ・・・(5)
数式(5)において、Vnは第n軸の軸速度によって生成されるハンド18の速度ベクトル、ωnは第n軸の要素以外0(ゼロ)となる関節軸の軸速度ベクトルである。従って、第n軸の軸速度が微小な速度変化αで加速又は減速した場合のハンド18の移動ベクトルは、加速の場合は数式(6)にて、減速の場合は数式(7)にてそれぞれ表すことができる。
V+αn=J(ω+αn) ・・・(6)
V−αn=J(ω−αn) ・・・(7)
数式(6)、数式(7)において、V+αnは第n軸のみ速度変化αで加速した場合のハンド18の速度ベクトル、V−αnは第n軸のみ速度変化αで減速した場合のハンド18の速度ベクトル、αnは第n軸だけ速度変化αとしそれ以外の要素を速度変化0(ゼロ)とする関節軸の軸速度ベクトルである。
数式(3)、数式(6)、数式(7)より、現状の速度を維持する場合、加速する場合、減速する場合の3通りの場合のハンド18の移動ベクトルが算出できるので、以下の数式(8)を用いてそれぞれの偏差ベクトル方向成分の大きさを算出する。
C=V・D/|V|×|D| ・・・(8)
数式(8)において、Cは偏差ベクトル方向成分の単位成分あたりの大きさであり、VとDの方向ベクトルが作る角度をφとするとC=cosφとなり、このCが大きいほどφの角度が小さくなりVとDの方向が一致に近づくことが判る。従って、V、V+αn、V−αnについてそれぞれCを求め、このCが最も大きくなる、すなわちハンド18の速度ベクトルが制御目標19に最も近くなる関節軸の軸速度を制御指令値として算出している。
次に、それぞれの軸制御部23〜29による各関節軸3〜9の制御方法について、図5A及び図5Bに示す模式図を用いて説明する。図5Aはマニピュレータのハンドと制御目標の位置関係を示す模式図であり、図5Bは、マニピュレータの各関節軸の指令値の決定方法を示す模式図である。
図5Aにおいて、マニピュレータ1のハンド18の座標(H)から制御目標19の座標(G)を結んだベクトルが偏差ベクトル40(D)である。
また、図5Bにおいて、制御対象の関節軸6(第n軸)において、現状の速度ω1(ω1=ω)と、加速した場合の速度ω2(ω2=ω+αn)と、減速した場合の速度ω3(ω3=ω−αn)におけるハンド18の速度ベクトル42の候補はそれぞれV1、V2、V3となる。これらの速度ベクトル42と偏差ベクトル40の作る角度φ1、φ2、φ3の大きさをもとに関節軸6の軸速度を設定する。図5Bの場合、角度φ3が最も小さい角度となることから、速度ベクトルV3が偏差ベクトル40に最も近いと判断して、現状の速度Vよりα減速した速度ベクトルV3を軸速度の制御指令値として設定する。
上記の制御方法では、マニピュレータ1全体として速度、すなわちハンド18の速度の調整を直接的に行わず、各々の関節軸3〜9の軸速度を設定している。そのため、ハンド18の速度が異常に早くなってしまい危険な状況になる可能性や、制御目標に停止できず通りすぎてしまう可能性がある。そこで、例えば図6の模式図に示すようにしてハンド18の速度を制限することでこれらの問題を解決する。
図6は、本第1実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図である。
図6において、全体評価部20にて、制御目標19から所定範囲内の減速領域43を設定する。ここで、減速領域43は、球状でも円状でも良い。また、制御目標19やハンド18によっては、一方向に長い領域も考えられる。ハンド18が減速領域43より離れている場合の位置44での速度45をハンド18の上限速度とし、ハンド18と制御目標19との距離が短いほど上限速度を下げるように制御して、ハンド18が制御目標19に到達した際に速度が0となるように設定する。すなわち、ハンド18の位置44での速度45よりも、減速領域43内で制御目標19に近づいている位置46での速度47を小さくするように制御する。さらに、図3のステップS4にて算出される新たな評価結果として上限速度と現在速度の速度比を加え、さらに図3のステップS7において上限速度と現在速度の速度比が1を超える場合に、この速度比と選択された関節軸の軸速度を掛けたものを新たな関節軸の軸速度の制御指令値とする。
このような処理を加えることで、結果的に全ての関節軸の軸速度が上限速度に合わせて減速されて、ハンド18の速度の上限が上限速度となるように制御される。上限速度が制御目標19に近づくにつれて小さくなることで、制御目標19にハンド18が近づくにつれてハンド18の速度は減速されるため、制御目標19において確実にハンド18を停止させることができる。
また、マニピュレータの制御において解決すべき問題として、マニピュレータの特異姿勢がある。「特異姿勢」とは、マニピュレータの制御においてエラーを引き起こす特異な姿勢であり、この状態を如何に回避するかがマニピュレータの制御において重要な問題である。マニピュレータの特異姿勢を説明するために、図7A、図7B、及び図7Cに、本第1実施形態のマニピュレータ1の特異姿勢時の対応を示す模式図を示す。ここで、図7Aは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図であり、図7Bは、マニピュレータが特異姿勢から脱出するための第1の対応方法を示す模式図であり、図7Cは、マニピュレータが特異姿勢から脱出するための第2の対応方法を示す模式図である。
まず、図7Aにおいて、マニピュレータ1は、その関節軸7〜9をどのように駆動しても、そのハンド18の速度ベクトルが制御目標19に対して垂直な方向となってしまい、制御目標19に近づく方向に速度ベクトルを生成できない姿勢となっている。このようなマニピュレータ1の姿勢が「特異姿勢」である。本第1実施形態において、これまでに説明した制御方法だけでは、マニピュレータ1が特異姿勢となった場合、ハンド18が制御目標19に近づくことができずに停止してしまう。
そこで、本第1実施形態では、次のような2つの方法を用いることで特異姿勢への対応、すなわち特異姿勢からの脱出を可能とする。本第1実施形態では、ハンド18が制御目標19(目標位置から所定の範囲内)に到達していない状態にも関らず、全ての関節軸3〜9(図7A中では関節軸7〜9)の軸速度が全て0となった場合に特異姿勢が生じたものと判断する。例えば、図3の全体評価部20のステップS4において任意の設定時間以上、全ての関節軸3〜9において軸速度0が維持されたか否かで判断することができる。
まず、第1の対応方法は、図7Bで示すように、マニピュレータ1が特異姿勢になった場合に全体評価部20のステップS4において、任意の設定時間の間だけ本来の制御目標19(G)とずれた仮制御目標48(G2)を設定する方法である。仮制御目標48を設定し、評価結果である偏差ベクトルを算出した後、偏差ベクトルを評価結果としてそれぞれの軸制御部23〜29に送信する。各々の軸制御部23〜29において、送信された偏差ベクトルに基づいて、仮制御目標48に向かうハンド18の移動制御を行うことで、マニピュレータ1を特異姿勢から脱出させることができる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標19を再設定し、通常の制御を継続する。
次に、第2の対応方法は、図3の全体評価部20のステップS4で算出される新たな評価結果として特異姿勢の有無を加え、全体評価部20からそれぞれの軸制御部23〜29に特異姿勢の有無情報を送信する方法である。特異姿勢が生じている場合は、制御目標19を一時的に目標から切り離し、図7Cで示すようにそれぞれの軸制御部23〜29で任意の設定時間の間、それぞれの関節軸3〜9に対して任意の軸速度の遥動動作49を実施させ、この揺動動作49の実施により強制的に特異姿勢から脱出させる。特異姿勢から脱出した後は、任意の設定時間後に再び本来の制御目標19を再設定し、通常の制御を継続する。
上述の本第1実施形態の説明においては、全体評価部20から全ての軸制御部23〜29に、評価結果および全体状態の情報を送信する(図3のステップS6)場合について説明したが、本第1実施形態はこのような場合についてのみ限定されるものではない。このような場合に代えて、例えば、関節軸3〜9のうちの制御対象となる関節軸を制御する軸制御部のみに、上記情報が送信されるような場合であってもよい。
本第1実施形態のマニピュレータ1の制御システムによる制御方法によれば、以下のような種々の効果を得ることができる。
本第1実施形態の制御方法では、全体評価部20にて作成されたマニピュレータ1の全体状態の評価結果の情報をそれぞれの軸制御部23〜29に送信して、この評価結果の情報に基づいて、個々の軸制御部23〜29において、他の関節軸とは独立して自軸の制御指令値を作成する。これにより、各々の関節軸単位にて独立して制御目標19に向かって制御を行うことで位置制御を行うことができる。そのため、冗長や非線形な駆動要素のあるマニピュレータ1においても逆運動学に付随する制御指令値を一意に決定できない問題や、計算量の膨大化に対する問題が発生しない。また、逆運動学の算出のために拘束条件を設定することや、自由度を制限する必要も無いため、高い自由度を維持したまま制御を行うことができる。さらに、動作の学習も必要としないため容易にマニピュレータの制御を行うことができる。
また、周囲の環境の影響や関節軸の故障によって一部の関節軸が稼動不能に陥っても、その他の関節軸が個別に制御目標19に向かうことによって自然に冗長性が発揮されるため、周囲の環境や軸の故障にロバスト性を有した状態で位置制御を行うことができる。なお、ここで、ロバスト性とは、外乱や設計誤差などの不確定な変動に対して、システム特性が現状の機能を維持できる性質を示す。なお、本発明における「故障」とは、関節軸の動作制御を行う軸制御部や全体評価部が、対象となる関節軸に何らかの問題が生じていることを認識しておらず、関節軸に対して制御指令を出しているのに関節軸が動作しない状態を示す。
さらに、それぞれの関節軸単位で制御指令値を算出することに伴いハンド18が速度オーバーとなることや、制御目標19を通過してしまうという課題に対しても、全体評価部20にてハンド18の速度制御などを行うことで解決することができる。また、マニピュレータ1の特異姿勢に対しても、全体評価部20にて、仮制御目標を一時的に設定することや、揺動動作を実施させることにより対応することができる。
従って、本第1実施形態のマニピュレータの制御方法によれば、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御を行うことができ、冗長や非線形な駆動要素があっても容易に位置制御を実現することができる。
さらに、本第1実施形態の制御方法は、マニピュレータの形状や軸数が変わっても同じ制御則を用いており、マニピュレータの形状パラメータを変更するだけで形状や軸数の変化に対応できる。
(第2実施形態)
本発明は上記第1実施形態に限定されるものではなく、その他種々の態様で実施できる。以下、本発明の第2実施形態にかかるマニピュレータの制御方法について説明する。
実際にマニピュレータを使用する場面では、様々な作業に柔軟に対応する必要が有る。そのために、作業に応じてマニピュレータの形状や軸数などを変化させることも考えられるが、本第2実施形態の制御方法を活用することで、マニピュレータの形状や軸数に合わせたプログラムの作成およびプログラムの書換えを不要とすることができる。これにより、マニピュレータの形状や軸数などが変更されるような場合であっても、パラメータの変更だけで対応することができ、プラグアンドプレイのようにマニピュレータをロボット等に追加・変更してすぐに使用することができる。
以下に、プラグアンドプレイによるマニピュレータの追加変更を実現する方法を、図面を参照しながら説明を行う。本発明の第2実施形態のマニピュレータ制御システムの模式図を図8に示す。
図8において、冗長なマニピュレータ101の制御システムは、マニピュレータ101の全体評価を行う全体評価部120と、それぞれの関節軸毎に独立して制御を行う軸制御部123〜129とを備えている。
全体評価部120は、評価計算装置131、通信装置132、現在制御中のマニピュレータ101が備えるそれぞれの関節軸3〜9の識別情報や形態情報などの軸情報をデータとして保持する記憶装置133と、記憶装置133のデータに基づいてマニピュレータ101を構成する関節軸3〜9の接続状態の変化を監視する監視装置134と、監視装置134により接続状態の変化を検出した際に記憶装置133に保持されたデータを変化した軸情報に更新する軸情報更新装置135と、を備えている。
また、それぞれの軸制御部123〜129は同様の構成を有しているため、代表して軸制御部123の構成について説明する。軸制御部123は、計測装置3s、制御計算処理装置3c、アクチュエータ3a、通信装置31、自軸の識別情報や形態情報などの軸情報を記憶する記憶装置50を備えている。
ここで、軸制御部123における記憶装置50に記憶された関節軸の識別情報は、例えば、関節軸毎に固有の識別情報であることが望ましく、通信装置31で送信する通信データにこの識別情報を付加することで、通信データがその関節軸から通知されたものなのか識別できる。それとともに、全体評価部120からの関節軸3〜9への指令の宛先として指定することができ、それぞれの軸制御部123〜129では自軸の識別情報と一致した場合のみ自軸の制御を行うことができる。
全体評価部120の通信装置132と軸制御部123の通信装置31とは、ネットワーク51を介して接続されている。ネットワーク51としては、例えば、全体評価部120を始点とし、マニピュレータ101の先端の関節軸9が終端となるようにマニピュレータ101の接続と同じ順でそれぞれの軸制御部123〜129に接続されたネットワークがある。このように接続することで各関節軸3〜9の接続状態がネットワーク51のルーティングを用いることで容易に把握することができる。なお、ネットワーク51の接続方法はこのような方法に限るものでは無く、その他の方法を用いることも考えられる。その際は、例えば隣り合った各関節軸の接続状態を検知し、その接続状態をネットワーク51にデータをして載せる事で各関節軸の接続状態を把握することができる。
このように検知される識別データおよび接続状態は、全体評価部120内の監視装置134で、常時、記憶装置133に記憶されたマニピュレータ101の軸情報と比較することで監視され、違いが発生すれば接続状態の変化として検出する。また、各関節軸3〜9から収集した軸情報を用いて軸情報更新装置135によって記憶装置133の軸情報を更新する。
続いて、プラグアンドプレイによりマニピュレータ101の構成要素を変更する方法を、図9から図13に示すマニピュレータ101の模式図を用いて説明する。
まず、図9は、マニピュレータの構成要素を変更する前の状態を示す模式図である。
図9において、全体評価部120では、それぞれの軸制御部120から送られてくる各関節軸3〜9の通信情報64に含まれる関節軸の識別情報およびルーティング等を利用した接続情報と、記憶装置133に記憶されている軸情報データ65とを比較する。それとともに、全体評価部120において、全体の評価が行われ、記憶装置133の軸情報データに基づいて制御対象となる関節軸を設定し、その識別情報を含む通信情報66をそれぞれの軸制御部123〜129に送信する。それぞれの軸制御部123〜129では、その通信情報66に自軸の識別情報を含む場合にのみ自軸の制御を行う。なお、図9から図13において、説明の理解を容易にするために、それぞれの関節軸3〜9の制御を行う軸制御部123〜129をA1〜A7にて示すとともに、それぞれの関節軸3〜9の識別情報をD1〜D7にて示す。また、後述するように変更された構成要素である関節軸の軸制御部をA8にて示すとともに、その識別情報をD8にて示す。
図10は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図である。
図10において、マニピュレータ101の先端部(ハンド及び関節軸)67が、例えば、識別情報D7から識別情報D8の関節軸に変更された場合を例として説明する。関節軸が変更された瞬間にそれぞれの軸制御部(123等)では通常どおり各関節軸の制御計算処理装置(3c等)で収集されたデータが通信情報64として全体評価部120に送信される。ただし、この場合、構成要素の変更が行われているため、通信情報64には異なる識別情報D8が含まれている。全体評価部120では、監視装置134にて通信情報及びそのルーティング等と記憶装置133の軸情報との比較を行う。この比較実施の結果、先端部67の識別情報が異なることが検出され、監視装置134にてマニピュレータ101の構成要素に変更が生じていることが検知される。
図11は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図である。
図11において、監視装置134にて、マニピュレータ101の先端部67の変更を検知し、軸情報データ65内に不明な部分を発見した全体評価部120は、新たな先端部67の形態等が未知である識別情報D8を有する関節軸に対して、軸情報を要求する通信情報68を送信する。
図12は、軸制御部からパラメータが応答されている状態を示す模式図であり、図13は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図である。
図12において識別情報D8を含む通信情報68を受けたそれぞれの軸制御部A1〜A8(A7を除く)では、識別情報D8を有する軸制御部A8のみが反応し、自軸の形態情報を含む通信情報69を全体評価部120に返信する。全体評価部120では、この通信情報69を受信することで軸情報更新装置135を用いて記憶装置133内の軸情報データ65の更新を行う。なお、この時点までは識別情報D8の軸制御部A8は、アクチュエータの制御は行っていない。
図13において、変更の結果が記憶装置133内の軸情報データ65に登録されると、全体評価部120では、新しい軸情報データ65に従って全体評価の計算を行い、その結果を通信情報70として送付する。この通信情報70には変更後の構成を反映した識別情報を含んでおり、この識別情報によりこれまでアクチュエータの制御を行っていなかった識別情報D8の軸制御部A8も、他の軸制御部A1〜A6と同様に制御を開始する。その後、通常通り各関節軸の計測装置での収集したデータ71が送付され、このデータ71に基づいて全体評価部120では監視と制御が繰り返して行われる。
以上のようなマニピュレータの制御方法によれば、マニピュレータの形状や軸数に関係無く、統一した制御則で制御が行われるとともに、自律的にマニピュレータの構成を検出してパラメータを更新することができる。そのため、マニピュレータの構成要素が変化した場合でも、プラグアンドプレイ、すなわち、制御が途切れることなく安全に継続することができる。従って、作業に応じてマニピュレータの形状や軸数を容易に変化させることができ、さらに幅広い作業に柔軟かつ容易に適応することができる。
(第3実施形態)
次に、本発明の第3実施形態にかかるマニピュレータの制御システムによる制御方法について、図14に示すマニピュレータの制御システムの模式図を用いて説明する。
図14に示すように、本第3実施形態のマニピュレータ201の制御システムは、それぞれの関節軸3〜9の制御を他の関節軸から独立して行う軸制御部223〜229と、マニピュレータ201の全体状態等の評価を行う全体評価部220とを備えている。
それぞれの軸制御部223〜229は、関節軸3〜9に個別に装備されたエンコーダなどのセンサを有する計測装置(9s等)と、関節軸を駆動するアクチュエータ(9a等)と、入力された情報に基づいてアクチュエータの制御指令値を算出する制御計算処理装置(9c等)とを備えている。全体評価部220は、全体評価処理装置221を備えている。
また、本第3実施形態のマニピュレータ201の制御方法は、マニピュレータ201のハンド18を制御目標19に移動させるように位置制御を行いながら、ハンド18から作業対象物209に一定の接触力を付加させた状態とする力制御を行うものである。このような力制御を実現するために、ハンド18には接触力を計測する力計測装置222が備えられている。
このような構成の制御システムによるマニピュレータ201の位置制御及び力制御の制御処理フローを図15に示す。
図15に示すように、ステップS11にて、それぞれの計測装置(9s等)により各関節軸3〜9の軸変位および軸速度が計測される。次に、ステップS12にて、計測された軸変位・軸速度の情報が、それぞれの軸制御部223〜229から全体評価部220に送信されて、軸変位・軸速度の情報が収集される。それと共に、作業対象物209に接触した状態にあるハンド18の接触力が、力計測装置222により計測されて、全体評価部220に収集される。その後、ステップS13にて、各関節軸3〜9の軸変位および軸速度の情報、並びにハンド18の接触力の情報から、ハンド18の状態および全体の状態変数(全体状態)を算出する。続いて、ステップS14にて、ハンド18の状態に基づいて、マニピュレータ201の制御状態を評価する(第1の工程)。なお、この制御状態、すなわち位置制御と力制御の状態の詳しい評価方法については後述する。その後、ステップS15にて、評価結果に基づいて制御目標に到達していないと判断される場合には、ステップS16にて、評価結果および全体状態の情報が、全体評価部220からそれぞれの軸制御部223〜229に送信される(第2の工程)。
各々の軸制御部223〜229では、ステップS17にて、自軸の軸変位・軸速度の情報と、送信された評価結果およびマニピュレータの全体状態の情報とに基づいて、自軸のアクチュエータの制御指令値を他の関節軸とは独立して算出する(第3の工程)。続いて、ステップS18にて、算出された制御指令値に基づいて、自軸のアクチュエータを駆動する。これらステップS11〜S18の制御処理が繰り返し実行される。ステップS15にて、評価結果が、マニピュレータ201のハンド18が制御目標(目標位置及び目標接触力)に到達していると判断された場合には、マニピュレータ201の位置制御および力制御が完了する。なお、ステップS11、S17、及びS18の制御処理はそれぞれの軸制御部223〜229にて独立して実行され、ステップS12〜S16の制御処理は全体評価部120にて実行される。
ここで、本第3実施形態の制御システムの動作の概要図を図16に示す。
図16において、全体評価部220は、マニピュレータ201全体の制御状態の評価を行う全体評価処理装置221と、複数の制御装置と相互通信可能な通信装置230を備えている。また、それぞれの関節軸3〜9の軸制御部223〜229を代表して、関節軸9の軸制御部229の構成について説明する。軸制御部229は、関節軸9における軸変位および軸速度を計測してアクチュエータ9aの制御指令値を算出する制御計算処理装置9cと、関節軸9を駆動するアクチュエータ9aと、全体評価部220内の通信装置230に対して通信可能な通信装置231と、を備えている。なお、その他の軸制御部223〜228も軸制御部229と同様な構成を有している。さらに、全体評価部220内の通信装置230と、各関節軸3〜9の軸制御装置223〜229内の通信装置(通信装置231等)とは、有線または無線の情報通信手段であるネットワーク232で接続されている。
また、本第3実施形態の制御方法における各処理ステップで算出される具体的な値、評価結果、制御計算方法を、例えば次のように設定する。まず、図15の制御処理フローにおいて、全体評価部220のステップS13にて算出されるハンド18の状態をハンド18の座標及び速度並びにハンド18の接触力とする。そして、同じく全体評価部220のステップS13にて算出される全体の状態変数(全体状態)を、ハンド18の速度及び接触力とヤコビ行列とする。なお、本第3実施形態においては、全体の状態変数、すなわちハンド18の速度及び接触力とヤコビ行列が、それぞれの関節軸3〜9の軸変位および軸速度を含む情報となっている。また、本第3実施形態において、ハンド18のパラメータは、速度と接触力となっている。
マニピュレータ201のハンド18の座標は数式(1)に全て関節軸3〜9の軸変位を代入することで求めることができる。
また、計測された関節軸3〜9の軸速度を数式(3)に代入することで、ハンド18の速度を算出することができ、同時にヤコビ行列も算出することができる。
また、全体評価部220のステップS14にて算出される評価結果は、例えば図17で示すように算出する。まず、手先座標251から制御目標18の座標を結ぶ位置の偏差ベクトルを算出する。なお、位置の偏差ベクトルDpは数式(9)により求めることができる。なお、Gは制御目標19の座標ベクトルである。
Dp=G−H ・・・(9)
次に、制御目標の接触力Ftと実際の接触力Frとの接触力差、すなわち力差分を位置偏差に換算したもの算出する。この力の換算偏差ベクトルDfは数式(10)のように表すことができる。なお、Kfは制御ゲインであり、系によって適当な値が選択される。
Df=Kf(Ft−Fr) ・・・(10)
これらの数式(9)、(10)にて表される位置の偏差ベクトルDpと力の換算偏差ベクトルDfを用いて、制御評価に用いる総合偏差ベクトルDを数式(11)のように算出する。
D=Dp+Df ・・・(11)
このように数式(11)にて総合偏差ベクトルDを算出した後は、上記第1実施形態における位置制御方法と同様な手順にて、数式(3)、(5)、(6)、(7)、(8)を用いて、各々の関節軸3〜9における現状の速度を維持する場合(V)、加速する場合(V+αn)、減速する場合(V−αn)の3通りのハンド18の移動ベクトルと、それぞれの偏差ベクトル方向成分の大きさCとを算出する。算出した結果を参照して、このCが最も大きくなる、すなわちハンド18の速度が制御目標の方向にもっとも近くなる関節軸の軸速度を制御目標として採用する。
この制御目標に対して第n軸の制御は、数式(12)に示すように、現状の出力トルクTnにΔTnを加えた(Tn+ΔTn)のトルクが出力されるような制御として行われる。
ΔTn=Mn・Δωn/Δt+Kdn・Δωn ・・・(12)
ここで、Δωnは選出された第n軸の軸速度の制御目標と、現在の軸速度との速度差であり、Δtは制御単位時間を表す。また、Mnは第n軸の慣性モーメント、Kdnは第n軸の粘性抵抗成分とする。このように算出される制御トルク出力(Tn+ΔTn)を用いることより、ハンド18の目標位置への到達及び目標接触力の発生をより効果的に実現することができるような第n軸の軸速度の制御指令値を算出することができる。
以上のような解決手段のようなマニピュレータの制御方法によれば、周囲の環境の影響や軸の故障によってある関節軸が移動不能に陥っても、その他の軸が個別に制御目標に向かうことによって自然に冗長性が発揮される。そのため、周囲の環境や軸の故障に対してロバスト性を有した状態で位置制御及び力制御を行うことができる。従って、周囲の環境や軸の故障などの不確かさに依存せず、冗長や非線形な駆動要素があっても容易にかつ柔軟に制御を行うことができるマニピュレータの制御方法および制御システムを提供することができる。
なお、本第3実施形態においては、マニピュレータ201のハンドの位置制御と力制御とが組み合わされて行われるような場合を例として説明したが、このような場合に代えて、力制御だけが行われるような場合にも、本第3実施形態の制御方法を適用することができると考えられる。
なお、上述のそれぞれの実施形態の説明では、マニピュレータ1等のアーム部分を支持するベース2がある位置に固定された状態にて、マニピュレータ1等の動作が制御されるような場合について説明した。しかしながら、本発明はこのような場合についてのみ限定されるものではなく、例えば、図18の模式図に示すように、マニピュレータ301のアーム部分を支持するベース302に車輪等を用いた移動装置305を備えさせ、マニピュレータ301の設置位置を移動可能とするような構成を採用することもできる。
なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏する構成とすることができる。
本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
2006年3月24日に出願された日本国特許出願No.2006−082269号の明細書、図面、及び特許請求の範囲の開示内容は、全体として参照されて本明細書の中に取り入れられるものである。
本発明のマニピュレータの制御方法および制御システムによれば、周囲の環境や軸の故障などの不確かさ、変動を有した状態でも制御可能なロバスト性を有する位置制御を実現することができる。冗長や非線形な駆動要素があっても少ないセンサを用いて容易にかつ柔軟に位置制御を実現することができるため、より複雑で高い自由度を必要とする作業を行う産業用、および民生用ロボットのアームなどに使用することができる。特に、障害物が多く、さらにその位置が特定されていない環境が多く存在する家庭用ロボットのアームとしての用途に有用である。
図1は、本発明の第1実施形態のマニピュレータの模式図である。
図2は、第1実施形態の制御システムの模式図である。
図3は、第1実施形態の制御処理フローを示す図である。
図4は、第1実施形態の制御システムの動作の概念図である。
図5Aは、マニピュレータのハンドと制御目標の位置関係を示す模式図である。
図5Bは、マニピュレータの各関節軸の指令値の決定方法を示す模式図である。
図6は、第1実施形態のマニピュレータのハンドの速度の上限設定方法を示す模式図である。
図7Aは、マニピュレータが特異姿勢となった場合の問題を説明するための模式図である。
図7Bは、マニピュレータが特異姿勢から脱出するための第1の対応方法を示す模式図である。
図7Cは、マニピュレータが特異姿勢から脱出するための第2の対応方法を示す模式図である。
図8は、本発明の第2実施形態のマニピュレータ制御システムの模式図である。
図9は、マニピュレータの構成要素を変更する前の状態を示す模式図である。
図10は、マニピュレータの構成要素を変更した後、変更箇所検出までの状態を示す模式図である。
図11は、軸制御部に対してパラメータの問合せを行っている状態を示す模式図である。
図12は、軸制御部からパラメータが応答されている状態を示す模式図である。
図13は、マニピュレータにおいて構成要素の変更が認識されて、通常の制御が行われている状態を示す模式図である。
図14は、本発明の第3実施形態にかかるマニピュレータの制御システムの模式図である。
図15は、第3実施形態の制御処理フローを示す図である。
図16は、第3実施形態の制御システムの動作の概念図である。
図17は、マニピュレータの制御における評価方法を説明する模式図である。
図18は、上記第1実施形態の変形例にかかるマニピュレータの模式図である。
図19は、従来のマニピュレータの制御方法の概要を示す図である。
図20は、従来のマニピュレータの制御処理フロー図である。
図21Aは、従来のマニピュレータの制御において曲げ角度の均等配分する制御方法を示す図である。
図21Bは、従来のマニピュレータの制御において駆動軸数を限定する制御方法を示す図である。
図22は、従来のマニピュレータの制御方法を示す図である。
図23は、従来のマニピュレータの制御方法を示す図である。
図24は、従来のマニピュレータの制御方法を示す図である。