位置決め制御装置及び位置決め制御方法
【課題】位置決め機構内の弾性変形を考慮してより正確に位置決めが可能であるとともに、より短時間に位置決め制御が可能な位置決め制御装置及び位置決め制御方法を提供する。
【解決手段】入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、モータの回転数に関する物理量を検出してモータの現在位置信号を出力する検出部と、制御電流とモータの回転数に関する物理量とが入力されて外乱トルク信号を出力する外乱トルク推定部とを備え、外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する。
【解決手段】入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、モータの回転数に関する物理量を検出してモータの現在位置信号を出力する検出部と、制御電流とモータの回転数に関する物理量とが入力されて外乱トルク信号を出力する外乱トルク推定部とを備え、外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、産業用ロボット等に用いる位置決め制御装置及び位置決め制御方法に関する。
【背景技術】
【０００２】
従来より、工場の生産ライン等では種々の産業用ロボットが導入されている。
産業用ロボットはサーボモータ等にてアーム等を駆動させて作業を行うため、サーボモータ等の位置決め制御が作業精度と生産性に大きく影響している。
しかし、産業用ロボットは弾性変形（しなり、たわみ等）が発生する場合があり、この場合は弾性変形量に応じて位置決め制御の誤差が大きくなる。
そこで、メカの持つ弾性変形を考慮し、高精度な位置決めを行う位置決め制御装置が提案されている（例えば特許文献１参照）。
特許文献１に記載されている従来の位置決め制御装置は、図８に示す構成を有しており、弾性変形を考慮した位置補正量ΔＰｒｅｆを出力する位置補正部９ｗには、外乱推定部７ｗからの推定外乱Ｃｓが入力されている。そして外乱推定部７ｗには、速度制御部２ｗからのトルク指令Ｔｒｅｆと差分器８ｗからのモータ速度Ｖｆｂが入力されている。
この構成にて従来の位置決め制御装置は、一旦仮の目標位置に到達させ、仮の目標位置に到達した時点で位置決め機構内に残存する弾性変形量を推定して、既に到達している仮の目標位置に弾性変形量を加えた位置を新たな目標位置として位置決め制御を行っている。
【特許文献１】特開２００３−３０３０２２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
特許文献１に記載されている従来の位置決め制御装置は、弾性変形が発生していても、一旦仮の目標位置に位置決め制御を行い（最初は弾性変形を無視して位置決め制御を行い）、仮の目標位置に到達させてから、弾性変形量に基づいた補正位置に位置決め制御を行うため、位置決め制御に費やす時間が増加し、生産性が低下する可能性がある。
本発明は、このような点に鑑みて創案されたものであり、位置決め機構内の弾性変形を考慮してより正確に位置決めが可能であるとともに、より短時間に位置決め制御が可能な位置決め制御装置及び位置決め制御方法を提供することを課題とする。
【課題を解決するための手段】
【０００４】
上記課題を解決するための手段として、本発明の第１発明は、請求項１に記載されたとおりの位置決め制御装置である。
請求項１に記載の位置決め制御装置は、入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、モータの回転数に関する物理量を検出して、制御対象（例、モータ）の制御結果上の現在位置を示す現在位置信号を出力する検出部と、電流制御部からモータに出力される制御電流と、モータの回転数に関する物理量とが入力され、外乱トルクを推定して外乱トルク信号を出力する外乱トルク推定部とを備えた位置決め制御装置である。
そして、更に前記外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する。
【０００５】
また、本発明の第２発明は、請求項２に記載されたとおりの位置決め制御装置である。
請求項２に記載の位置決め制御装置は、請求項１に記載の位置決め制御装置であって、位置補正部は、位置決め制御装置で位置決めされる位置決め機構が有する弾性変形に基づいたバネ定数Ｋｆに対して、外乱トルク信号に１／Ｋｆを乗じた位置補正信号を出力する。
【０００６】
また、本発明の第３発明は、請求項３に記載されたとおりの位置決め制御方法である。
請求項３に記載の位置決め制御方法は、移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法である。
位置決め制御の開始位置から目標位置に到達するまで、目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動し、前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する。
【発明の効果】
【０００７】
請求項１に記載の位置決め制御装置を用いれば、弾性変形を考慮した位置補正信号を、位置決め制御の開始位置から目標位置に到達するまで常に位置制御部に入力するので、位置決め制御の開始位置から目標位置に到達するまで常に目標位置そのものを補正して位置決め制御を行う。
弾性変形を考慮して目標位置を補正することで、位置決め精度をより向上させることができるとともに、一旦仮の目標位置に到達してから補正位置に移動させる従来技術の動作よりも、より短時間に位置決め制御を行うことができる。
【０００８】
また、請求項２に記載の位置決め制御装置によれば、位置補正部の定数を比較的容易に設定することができる。
【０００９】
また、請求項３に記載の位置決め制御方法によれば、駆動方法に関わらず（モータに限定されず）、物体を駆動させるために必須であるトルクという抽象化された物理量を用いて位置決め精度をより向上させ、より短時間に位置決め制御が可能であるとともに、種々の駆動方法を用いた位置決め制御装置に適用できる。
【発明を実施するための最良の形態】
【００１０】
以下に本発明を実施するための最良の形態を図面を用いて説明する。図１は、本発明の位置決め制御装置における制御ブロック図を示している。
●［制御ブロック図（図１）］
図１（Ａ）及び（Ｂ）を用いて、本発明の位置決め制御装置における制御ブロック図を説明する。図１（Ａ）は制御ブロック全体の構成を示しており、図１（Ｂ）は制御ブロック内における外乱トルク推定部７０と位置補正部８０の詳細を示している。
【００１１】
ノードＮａには、位置指令信号Ｐｔｇｔ（目標位置）が加算項で入力されるとともに位置補正信号ΔＰｔｇｔ（目標位置に対してリアルタイムに求めた、目標位置を補正する位置補正量（後述））が加算項で入力される。
この位置補正信号ΔＰｔｇｔを位置指令信号Ｐｔｇｔに加算項として追加している点、及び位置補正信号ΔＰｔｇｔを求める位置補正部８０を追加している点、そして位置補正信号ΔＰｔｇｔは目標トルクと実際のトルクに基づいて求める点、そして位置補正信号ΔＰｔｇｔは位置決め制御の開始位置から目標位置に到達するまで常に演算されて位置指令信号Ｐｔｇｔを補正する点が特徴である。
なお、位置指令信号Ｐｔｇｔを求める目標設定部は既存の技術で実現可能であり、図示を省略している。
【００１２】
ノードＮｂにはノードＮａからの出力が加算項で入力され、位置検出部６６ｂから出力される現在位置信号Ｐｆｂが減算項で入力される。
位置制御部６１には、位置指令信号Ｐｔｇｔ（加算項）、現在位置信号Ｐｆｂ（減算項）、位置補正信号ΔＰｔｇｔ（加算項）が入力され、速度制御部６２に速度指令信号Ｖｔｇｔを出力する。
速度制御部６２には位置制御部６１から出力される速度指令信号Ｖｔｇｔが入力され、ノードＮｃにトルク指令信号Ｔｔｇｔを出力する。
ノードＮｃには速度制御部６２から出力されるトルク指令信号Ｔｔｇｔが加算項で入力され、外乱トルク推定部７０から出力される外乱トルク信号Ｃｔが減算項で入力される。
電流制御部６３には、ノードＮｃからの出力（トルク指令信号Ｔｔｇｔ（加算項）と外乱トルク信号Ｃｔ（減算項）が入力され、モータ６４に制御電流Ｉｔｇｔを出力する。
【００１３】
モータ６４には電流制御部６３が出力した制御電流Ｉｔｇｔが入力され、機械部６５（機構部）を駆動する。
回転検出部６６ａは、例えばモータ６４の軸の回転数を検出可能なエンコーダ等であり、モータ６４の軸の回転が入力され、回転数に関する物理量（例えば角速度ω）を出力する。そして位置検出部６６ｂは、回転検出部６６ａの検出値に基づいて制御対象（例、モータ）の制御結果上の現在位置を示す現在位置信号Ｐｆｂを出力する。本実施の形態では、回転検出部６６ａと位置検出部６６ｂとで検出部６６を構成している。
外乱トルク推定部７０には、電流制御部６３から出力される制御電流Ｉｔｇｔと、回転検出部６６ａから出力されるモータ６４の軸の回転数に関する物理量とが入力され、外乱トルク信号Ｃｔを出力する。
位置補正部８０には、外乱トルク推定部７０から出力される外乱トルク信号Ｃｔが入力され、位置補正信号ΔＰｔｇｔを出力する。
【００１４】
外乱トルク推定部７０の内部では、電流制御部６３から入力される制御電流Ｉｔｇｔにトルク定数Ｋｔ１（図１（Ｂ）中の７１）を乗じた「目標トルク」をノードＮｄに出力し、回転検出部６６ａから入力される角速度（ω）に慣性モーメントＪｍ１（図１（Ｂ）中の７２）を乗じて更に微分器７３にて微分した「実際のトルク（実トルク）」をノードＮｄに出力する。
ノードＮｄでは目標トルクが加算項として入力され、実際のトルクが減算項として入力される。
そしてノードＮｄから入力された、「目標トルク」と「実際のトルク」との差分に基づいて外乱トルクを求め、外乱トルク信号Ｃｔを出力する。外乱トルク推定部７０は、その内部にローパスフィルタ７４を有している。ローパスフィルタ７４内の記号ｔｑ１は、外乱トルク推定部７０の時定数である。この時定数ｔｑ１は、設計者ないしユーザが適宜に定める。すなわち、サーボ系で発生する外乱ノイズを適切に低減し得るように、または、サーボ系の振動を適切に抑制し得るように、設計者ないしユーザが（実験的に、あるいはシミュレーションソフト等を使って）適当なる値に定める。
位置補正部８０の内部では、外乱トルク推定部７０から入力される外乱トルク信号Ｃｔに、１／Ｋｆ（Ｋｆは位置決め制御機構のバネ定数であり、位置決め制御機構に固有の定数であり、実験的に求める）を乗じて位置補正量を求め、位置補正信号ΔＰｔｇｔを出力する。
【００１５】
図１に示す本実施の形態の制御ブロック図は、図８に示す従来技術の制御ブロック図に対して、以下の点が異なる。
本実施の形態では、外乱トルク推定部７０への入力に制御電流Ｉｔｇｔを入力しているが、図８に示す従来技術では外乱推定部７ｗへの入力に制御電流の前段のトルク指令Ｔｒｅｆを入力している。本実施の形態では、外乱を考慮したトルク補正を行った後の制御電流Ｉｔｇｔを用いて目標トルクを求めている。
また、図８に示す従来技術では、外乱推定部７ｗは次段の位置補正部９ｗのみに使用されている。従って、従来技術では外乱推定部７ｗは位置補正専用である。しかし本実施の形態では、外乱トルク推定部７０は、外乱を考慮したトルク補正用として設けたものをそのまま流用しており、新たに設ける必要がない。
また、制御方法において、従来技術では、（まず、弾性変形を考慮しない状態で移動させて）一旦仮の目標位置に到達してから位置補正量分だけ移動させて真の目標位置に到達させている。しかし本実施の形態における制御方法では、位置決め制御の開始位置から目標位置に到達するまで常に（リアルタイムに）位置補正を行っている。
【００１６】
●［位置決め制御装置を用いた産業用ロボットの例（図２）］
次に図２を用いて、図１にて説明した位置決め制御装置を用いた産業用ロボットの例（この例は樹脂整形装置）を説明する。
樹脂成形装置は、基台４０に載置された射出装置１０、固定プラテン２０及び可動プラテン３０から構成されている。固定プラテン２０には固定金型２２が取付けられており、可動プラテン３０には可動金型３２が取付けられている。
ここで、互いに直交する３軸（ｘ軸／ｙ軸／ｚ軸）を設定する（垂直方向がｚ軸）。
位置決め制御装置は、固定プラテン２０に固定されている固定部１、固定部１に取付けられた制御手段４及び支持体２ｘ、支持体２ｘに沿って移動する移動体３ｘ、移動体３ｘに取付けられた支持体２ｙ、支持体２ｙに沿って移動する移動体３ｙ、移動体３ｙに取付けられた支持体２ｚ、支持体２ｚに沿って移動する移動体３ｚ、移動体３ｚに取付けられた捕獲手段５で構成されている。支持体２ｘ〜支持体２ｚ及び移動体３ｘ〜移動体３ｚが移動手段に相当する。また、支持体２ｘはｘ軸にほぼ平行であり、支持体２ｙはｙ軸にほぼ平行であり、支持体２ｚはｚ軸にほぼ平行である。
制御手段４は、記憶手段に接続されており、移動体３ｘ〜移動体３ｚを駆動して捕獲手段５を任意の位置（支持体２ｘ〜支持体２ｚにより移動可能な範囲の位置）に移動させることが可能であり、捕獲手段５を用いて成形品７（被移動部材等）の捕獲、あるいは捕獲の解除が可能である。
【００１７】
●［弾性変形の例（図３）及び制御ブロック図の例（図４）］
次に図３を用いて位置決め制御装置における弾性変形について説明する。なお、図３（Ａ）は図２から位置決め制御装置に相当する構成要素を抽出した図であり、図３（Ｂ）は図３（Ａ）の中から更にｘ軸方向に関する構成要素を抽出した図である。なお、図３、図５、図６では、弾性変形の様子をわかり易くするために、実際の弾性変形量よりも大きく変形させて記載している。
まず、図３（Ａ）を用いて弾性変形の状態について説明する。
図３（Ａ）に示すように、支持体２ｘは固定部１に固定されており、移動体３ｘが支持体２ｘに沿って移動して固定部１から遠ざかるにつれて、支持体２ｘが一点鎖線で示す支持体２ｘｒのように、重力により弾性変形する可能性がある。また、支持体２ｙは移動体３ｘに固定されており、移動体３ｙが支持体２ｙに沿って移動して移動体３ｘから遠ざかるにつれて、支持体２ｙが一点鎖線で示す支持体２ｙｒのように、重力により弾性変形する可能性がある。支持体２ｘｒ及び支持体２ｙｒに示すような弾性変形が発生した場合、位置決め精度が要求される捕獲手段５の位置が、目標位置（図３（Ａ）中の捕獲手段５の位置）から、一点鎖線で示す捕獲手段５ｒの位置にずれることとなる。
【００１８】
次に、図３（Ｂ）を用いて弾性変形による位置の誤差（この例では、支持体２ｘの弾性変形によるｘ軸方向とｚ軸方向の誤差）について説明する。
図３（Ｂ）に示すように、支持体２ｘが支持体２ｘｒに示すように弾性変形した場合、移動体３ｘは支持体２ｘｒに沿って移動し、真の目標位置Ｐｔｇｔに移動したつもりでも、実際の位置Ｐｒに到達する。この場合、ｘ軸方向にはΔＸ、ｚ軸方向にはΔＺの誤差が発生している。つまり、支持体２ｘの弾性変形を考慮して位置補正を行う場合は、ｘ軸方向とｚ軸方向の位置補正が必要である。図示しないが同様に、支持体２ｙの弾性変形を考慮して位置補正を行う場合は、ｙ軸方向とｚ軸方向の位置補正が必要である。
なお、支持体２ｚは重力方向に平行であり、支持体２ｚには弾性変形が発生しないと考えられるため、本実施の形態では、支持体２ｚの弾性変形を考慮した位置補正は行っていない。
【００１９】
上記の内容（ｘ軸方向に移動する際はｘ軸方向とｚ軸方向とに位置補正が必要、ｙ軸方向に移動する際はｙ軸方向とｚ軸方向とに位置補正が必要、ｚ軸方向に移動の際は特に位置補正は必要ない）に基づいて、図１に示す制御ブロック図を、図２に示す樹脂整形装置に適用した例を図４に示す。
図４に示すｘ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０ｘのバネ定数として支持体２ｘ（すなわちｘ軸）に固有のバネ定数Ｋｆｘを用いている点と、ｚ軸方向の位置補正を出力するための位置補正部８０ｘｚ（バネ定数は固有のＫｆｘｚ）が追加されている。
また、図４に示すｙ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０ｙのバネ定数として支持体２ｙ（すなわちｙ軸）に固有のバネ定数Ｋｆｙを用いている点と、ｚ軸方向の位置補正を出力するための位置補正部８０ｙｚ（バネ定数は固有のＫｆｙｚ）が追加されている。
なお、各々のバネ定数は実験的に求める。
【００２０】
また、図４に示すｚ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０が省略されている。また制御ブロック図の先頭のノードＮａｚに、位置指令信号Ｐｔｇｔの他に、ｘ軸方向の制御ブロック図の位置補正部８０ｘｚからの位置補正信号ΔＰｔｇｔとｙ軸方向の制御ブロック図の位置補正部８０ｙｚからの位置補正信号ΔＰｔｇｔとが加算項として入力されている。
この制御ブロック図にて、ｘ軸方向に移動する際はｘ軸方向とｚ軸方向とに位置補正を行い、ｙ軸方向に移動する際はｙ軸方向とｚ軸方向とに位置補正を行い、ｚ軸方向に移動する際は特に位置補正を行わない、という構成を実現している。その他は図１で既に説明しているので説明を省略する。
【００２１】
●［位置決め制御動作の比較（目標に対して実際のトルクが小さい場合）（図５）］
次に図５（Ａ）〜（Ｄ）を用いて、支持体２ｘの弾性変形を例にして、本発明と従来の動作の違いを説明する。なお、図５（Ａ）〜（Ｄ）は、移動体３ｘにおいて目標トルクよりも実際のトルクが小さい場合の例（移動体３ｘが、支持体２ｘの弾性変形量が大きくなる方向に移動する場合の例）を示している。
図５（Ａ）〜（Ｄ）は、移動体３ｘが位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動する場合の様子を示しており、最終的に位置決め精度が要求される捕獲手段５（図２を参照）の中心Ｓが、位置Ｓｏｒｇから目標位置Ｓｔｇｔに移動する様子を示しており、中心Ｓの軌跡も示している。
【００２２】
［理想状態（図５（Ａ）：弾性変形が発生しない場合）］
まず、図５（Ａ）を用いて、支持体２ｘに弾性変形が発生しない理想状態の例を説明する。支持体２ｘに弾性変形が発生しないため、移動体３ｘの位置を位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させても、移動体３ｘから支持体２ｙ及び支持体２ｚ等を介して接続された捕獲手段５の中心Ｓは目標位置のＳｔｇｔに移動し、中心Ｓの軌跡も最短距離である直線となる。
【００２３】
［従来１の動作（図５（Ｂ）：発生した弾性変形に対する考慮なし）］
図５（Ｂ）は、発生した弾性変形に対して位置補正を行わない従来の例（図１に示す制御ブロック図から位置補正部８０を省略した状態）を示しており、支持体２ｘに弾性変形が発生し、支持体２ｘが支持体２ｘｒの形状に湾曲した場合の例を示している。
移動体３ｘを位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させた場合、移動体３ｘの実際の位置は支持体２ｘｒ上の位置Ｐｒである。従って、ｘ軸方向に対しては距離ΔＸ分だけ誤差が発生しており、ｚ軸方向に対しては距離ΔＺ分だけ誤差が発生している。従って移動先の中心Ｓｒは目標位置の中心ＳｔｇｔからΔＸ及びΔＺずれた位置にある。また、中心Ｓの軌跡も支持体２ｘｒの湾曲形状に沿った曲線となる。
誤差ΔＸ及びΔＺが許容範囲内の誤差であれば特に問題ないが、そうでない場合は支持体２ｘの材質または形状等を変更して剛性を高くして、弾性変形量を低減させなければならない。
このため、再設計及び再評価による工数の増加、そして材質または形状変更によるコストの増加等の種々の問題が発生する可能性がある。
【００２４】
［従来２の動作（図５（Ｃ）：発生した弾性変形に対して仮の目標位置に到達した後に位置を補正）］
図５（Ｃ）は、発生した弾性変形に対して、仮の目標位置に一旦移動した後、弾性変形量に基づいた補正量分だけ位置を補正する、従来の技術の例を示している。
まず最初は図５（Ｂ）と同様の動作（一旦仮の目標位置まで移動）をさせるため、中心Ｓｒの位置及び支持体２ｘｒに沿った曲線状の軌跡も図５（Ｂ）と同様である。そして中心Ｓｒの位置（仮の目標位置）に到達してから、ΔＸ及びΔＺの位置補正を行い、中心Ｓｒの位置を中心Ｓｔｇｔの位置（真の目標位置）に移動させる。つまり、中心Ｓｏｒｇを中心Ｓｒまで移動させる第１動作と、中心Ｓｒを中心Ｓｔｇｔまで移動させる第２動作を行う２段階の動作で移動を完了させる。
従って、２段階目の動作時間分、時間が余計にかかる。また、図５（Ａ）と比較して異なる軌跡となるため、障害物等が存在した場合、理想状態（図５（Ａ））では障害物に触れることがなくても、図５（Ｃ）に示す動作では障害物に触れる可能性がある。
【００２５】
［本発明の動作（図５（Ｄ））］
図５（Ｄ）は、発生した弾性変形に対して、位置決め制御の開始位置から目標位置に到達するまで常に目標位置を補正する（リアルタイムに位置補正を行う）本発明の例を示している。
移動体３ｘは、鉛直下方に向かって湾曲した（弾性変形した）支持体２ｘｒに沿って位置Ｐｏｒｇから目標位置Ｐｔｇｔに向かって移動していくと、湾曲した支持体２ｘｒの傾斜を下ることになるので、目標トルクよりも小さなトルクで移動することになる。
ここで図１（Ｂ）に示すように外乱トルク推定部７０にて、目標トルク＞実際のトルクの場合、外乱トルク信号Ｃｔが正の値として出力される。
続いて図４に示すように、ｘ軸方向の制御ブロック図における外乱トルク信号Ｃｔが正の値の場合、同ブロック図における位置補正部８０ｘにてｘ軸位置補正信号ΔＰｔｇｔが正の値として出力される。このｘ軸位置補正信号ΔＰｔｇｔがΔＸ（＋ΔＸ）となるわけである。
また、同ブロック図における位置補正部８０ｘｚにてｚ軸位置補正信号ΔＰｔｇｔが正の値として出力される。このｚ軸位置補正信号ΔＰｔｇｔがΔＺ（＋ΔＺ）となるわけである。
これらがリアルタイムに動作するため、中心Ｓｏｒｇが中心Ｓｔｇｔに移動した軌跡は、図５（Ａ）に示す理想状態と同じ直線状となる。もちろん移動先の中心Ｓｔｇｔは理想状態と同じ位置である。すなわち、本発明では、図５（Ａ）に示す理想状態と同じ軌跡を通って、同じ位置に到達する。このため、余計な時間がかからないとともに、障害物等に触れることもない。
【００２６】
●［位置決め制御動作の比較（目標に対して実際のトルクが大きい場合）（図６）］
次に図６（Ａ）〜（Ｄ）を用いて、支持体２ｘの弾性変形を例にして、本発明と従来の動作の違いを説明する。なお、図６（Ａ）〜（Ｄ）は、図５（Ａ）〜（Ｄ）に示した例に対して逆方向へ移動する場合の例を示しており、移動体３ｘにおいて目標トルクよりも実際のトルクが大きい場合の例（移動体３ｘが、支持体２ｘの弾性変形量が小さくなる方向に移動する場合の例）を示している。
図６（Ａ）〜（Ｄ）は、移動体３ｘが位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動する場合の様子を示しており、図５（Ａ）〜（Ｄ）と同様に、最終的に位置決め精度が要求される捕獲手段５（図２を参照）の中心Ｓが、位置Ｓｏｒｇから目標位置Ｓｔｇｔに移動する様子を示しており、中心Ｓの軌跡も示している。
【００２７】
［理想状態（図６（Ａ）：弾性変形が発生しない場合）］
まず、図６（Ａ）を用いて、支持体２ｘに弾性変形が発生しない理想状態の例を説明する。支持体２ｘに弾性変形が発生しないため、移動体３ｘの位置を位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させても、移動体３ｘから支持体２ｙ及び支持体２ｚ等を介して接続された捕獲手段５の中心Ｓは目標位置のＳｔｇｔに移動し、中心Ｓの軌跡も最短距離である直線となる。
【００２８】
［従来１の動作（図６（Ｂ）：発生した弾性変形に対する考慮なし）］
図６（Ｂ）は、発生した弾性変形に対して位置補正を行わない従来の例（図１に示す制御ブロック図から位置補正部８０を省略した状態）を示しており、支持体２ｘに弾性変形が発生し、支持体２ｘが支持体２ｘｒの形状に湾曲した場合の例を示している。
移動体３ｘを位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させた場合、移動体３ｘの実際の位置は支持体２ｘｒ上の位置Ｐｒである。従って、ｘ軸方向に対しては距離ΔＸ１分だけ誤差が発生しており、ｚ軸方向に対しては距離ΔＺ１分だけ誤差が発生している。従って移動先の中心Ｓ１ｒは目標位置の中心ＳｔｇｔからΔＸ１及びΔＺ１ずれた位置にある。また、中心Ｓの軌跡も支持体２ｘｒの湾曲形状に沿った曲線となる。以下、図５（Ｂ）の説明と同様であるので説明を省略する。
【００２９】
［従来２の動作（図６（Ｃ）：発生した弾性変形に対して仮の目標位置に到達した後に位置を補正）］
図６（Ｃ）は、発生した弾性変形に対して、仮の目標位置に一旦移動した後、弾性変形量に基づいた補正量分だけ位置を補正する、従来の技術の例を示している。
まず最初は図６（Ｂ）と同様の動作をさせるため、支持体２ｘｒに沿った曲線状の軌跡は図６（Ｂ）と同様である（ただし、最初の位置Ｐｏｒｇが補正された正しい位置であるため、中心Ｓｏｒｇの位置及び仮の目標位置の中心Ｓ２ｒの位置は異なる）。そして中心Ｓ２ｒの位置（仮の目標位置）に到達してから、ΔＸ１及びΔＺ１の位置補正を行い、中心Ｓ２ｒの位置を中心Ｓｔｇｔの位置（真の目標位置）に移動させる。つまり、中心Ｓｏｒｇを中心Ｓ２ｒまで移動させる第１動作と、中心Ｓ２ｒを中心Ｓｔｇｔまで移動させる第２動作を行う２段階の動作で移動を完了させる。
従って、２段階目の動作時間分、時間が余計にかかる。また、図６（Ａ）と比較して異なる軌跡となるため、障害物等が存在した場合、理想状態（図６（Ａ））では障害物に触れることがなくても、図６（Ｃ）に示す動作では障害物に触れる可能性がある。
【００３０】
［本発明の動作（図６（Ｄ））］
図６（Ｄ）は、発生した弾性変形に対して、位置決め制御の開始位置から目標位置に到達するまで常に目標位置を補正する（リアルタイムに位置補正を行う）本発明の例を示している。
移動体３ｘは、鉛直下方に向かって湾曲した（弾性変形した）支持体２ｘｒに沿って位置Ｐｏｒｇから目標位置Ｐｔｇｔに向かって移動していくと、湾曲した支持体２ｘｒの傾斜を上ることになるので、目標トルクよりも大きなトルクで移動することになる。
ここで図１（Ｂ）に示すように外乱トルク推定部７０にて、目標トルク＜実際のトルクの場合、外乱トルク信号Ｃｔが負の値として出力される。
続いて図４に示すように、ｘ軸方向の制御ブロック図における外乱トルク信号Ｃｔが負の値の場合、同ブロック図における位置補正部８０ｘにてｘ軸位置補正信号ΔＰｔｇｔが負の値として出力される。このｘ軸位置補正信号ΔＰｔｇｔがΔＸ１（−ΔＸ１）となるわけである。
また、同ブロック図における位置補正部８０ｘｚにてｚ軸位置補正信号ΔＰｔｇｔが負の値として出力される。このｚ軸位置補正信号ΔＰｔｇｔがΔＺ１（−ΔＺ１）となるわけである。
これらがリアルタイムに動作するため、中心Ｓｏｒｇが中心Ｓｔｇｔに移動した軌跡は、図６（Ａ）に示す理想状態と同じ直線状となる。もちろん移動先の中心Ｓｔｇｔは理想状態と同じ位置である。すなわち、本発明では、図６（Ａ）に示す理想状態と同じ軌跡を通って、同じ位置に到達する。このため、余計な時間がかからないとともに、障害物等に触れることもない。
【００３１】
●［シミュレーション結果（図７）］
図７に、本実施の形態におけるシミュレーションの結果を示す。
図７（Ａ）は従来１（図１に示す制御ブロック図から位置補正部８０を省略した状態）の動作の結果を示しており、図７（Ｂ）は本発明の動作の結果を示している。グラフは横軸が時間［sec］を示しており、縦軸が位置［cm］を示している。
どちらも同じ位置（０［cm］の位置）からスタートし、位置指令信号Ｐｔｇｔ（目標位置）＝４０．０［cm］を入力した結果である。
応答特性（通常、目標位置の９０％位置（この場合３６［cm］の位置）までに要する時間）は、図７（Ａ）に示す従来１の動作も、図７（Ｂ）に示す本発明の動作も同じである。
しかし、到達位置が大きく異なる。図７（Ａ）に示す従来１の動作では目標位置４０．０［cm］に対してΔＸの誤差が発生している。これが図５（Ｂ）に示すΔＸの誤差である。これに対して図７（Ｂ）に示す本発明の動作では、目標位置４０．０［cm］に完全に一致しており、非常に高い精度で位置決め制御できていることを確認できた。また、オーバーシュートも発生せず、非常に良好な制御特性を実現できることが確認された。
【００３２】
以上に説明した実施の形態では、移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法を説明した。
すなわち、位置決め制御の開始位置から目標位置に到達するまで、目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動させる。そして、前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する、という位置決め制御方法を説明した。
また、以上に説明した実施の形態では、サーボモータを用いた直交３軸（ｘ軸／ｙ軸／ｚ軸）の位置決め制御装置に適用した例で説明したが、トルクという抽象化された物理量を用いているため、サーボモータに限定されず、種々の駆動手段（例えばリニアモータ、内燃機関等）を用いた位置決め制御に適用することができる。
【００３３】
本発明の位置決め制御装置及び位置決め制御方法は、本実施の形態で説明した構成、動作等に限定されず、本発明の要旨を変更しない範囲で種々の変更、追加、削除が可能である。例えば、外乱トルク推定部７０の内部にて求める（検出する）実際のトルクは、角速度ωから求めずに、トルクセンサ等を用いて求める（検出する）ようにしてもよい。
また、本実施の形態の説明に用いた数値は一例であり、この数値に限定されるものではない。
【図面の簡単な説明】
【００３４】
【図１】本発明の位置決め制御装置における制御ブロック図の例を説明する図である。
【図２】本発明の位置決め制御装置を用いた産業用ロボットの例を説明する図である。
【図３】図２に示す産業用ロボットの弾性変形の例を説明する図である。
【図４】図２に示す産業用ロボットの位置決め制御に、図１に示す制御ブロック図を適用した例を説明する図である。
【図５】位置決め制御動作の比較（目標に対して実際のトルクが小さい場合）を説明する図である。
【図６】位置決め制御動作の比較（目標に対して実際のトルクが大きい場合）を説明する図である。
【図７】従来技術（図７（Ａ））と本発明（図７（Ｂ））とのシミュレーション結果を示すグラフである。
【図８】従来の技術（当初の目標位置に到達した後に位置を補正）の制御ブロック図を説明する図である。
【符号の説明】
【００３５】
６１ 位置制御部
６２ 速度制御部
６３ 電流制御部
６４ モータ
６５ 機械部（機構部）
６６ 検出部
６６ａ 回転検出部
６６ｂ 位置検出部
７０ 外乱トルク推定部
８０ 位置補正部
Ｎａ〜Ｎｄ ノード
Ｐｔｇｔ 位置指令信号（目標位置）
Ｖｔｇｔ 速度指令信号
Ｔｔｇｔ トルク指令信号
Ｉｔｇｔ 制御電流
Ｃｔ 外乱トルク信号
Ｐｆｂ 現在位置信号
ΔＰｔｇｔ 位置補正信号
Ｋｔ１ トルク定数
Ｋｆ、Ｋｆｘ、Ｋｆｘｚ、Ｋｆｙ、Ｋｆｙｚ バネ定数
【技術分野】
【０００１】
本発明は、産業用ロボット等に用いる位置決め制御装置及び位置決め制御方法に関する。
【背景技術】
【０００２】
従来より、工場の生産ライン等では種々の産業用ロボットが導入されている。
産業用ロボットはサーボモータ等にてアーム等を駆動させて作業を行うため、サーボモータ等の位置決め制御が作業精度と生産性に大きく影響している。
しかし、産業用ロボットは弾性変形（しなり、たわみ等）が発生する場合があり、この場合は弾性変形量に応じて位置決め制御の誤差が大きくなる。
そこで、メカの持つ弾性変形を考慮し、高精度な位置決めを行う位置決め制御装置が提案されている（例えば特許文献１参照）。
特許文献１に記載されている従来の位置決め制御装置は、図８に示す構成を有しており、弾性変形を考慮した位置補正量ΔＰｒｅｆを出力する位置補正部９ｗには、外乱推定部７ｗからの推定外乱Ｃｓが入力されている。そして外乱推定部７ｗには、速度制御部２ｗからのトルク指令Ｔｒｅｆと差分器８ｗからのモータ速度Ｖｆｂが入力されている。
この構成にて従来の位置決め制御装置は、一旦仮の目標位置に到達させ、仮の目標位置に到達した時点で位置決め機構内に残存する弾性変形量を推定して、既に到達している仮の目標位置に弾性変形量を加えた位置を新たな目標位置として位置決め制御を行っている。
【特許文献１】特開２００３−３０３０２２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
特許文献１に記載されている従来の位置決め制御装置は、弾性変形が発生していても、一旦仮の目標位置に位置決め制御を行い（最初は弾性変形を無視して位置決め制御を行い）、仮の目標位置に到達させてから、弾性変形量に基づいた補正位置に位置決め制御を行うため、位置決め制御に費やす時間が増加し、生産性が低下する可能性がある。
本発明は、このような点に鑑みて創案されたものであり、位置決め機構内の弾性変形を考慮してより正確に位置決めが可能であるとともに、より短時間に位置決め制御が可能な位置決め制御装置及び位置決め制御方法を提供することを課題とする。
【課題を解決するための手段】
【０００４】
上記課題を解決するための手段として、本発明の第１発明は、請求項１に記載されたとおりの位置決め制御装置である。
請求項１に記載の位置決め制御装置は、入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、モータの回転数に関する物理量を検出して、制御対象（例、モータ）の制御結果上の現在位置を示す現在位置信号を出力する検出部と、電流制御部からモータに出力される制御電流と、モータの回転数に関する物理量とが入力され、外乱トルクを推定して外乱トルク信号を出力する外乱トルク推定部とを備えた位置決め制御装置である。
そして、更に前記外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する。
【０００５】
また、本発明の第２発明は、請求項２に記載されたとおりの位置決め制御装置である。
請求項２に記載の位置決め制御装置は、請求項１に記載の位置決め制御装置であって、位置補正部は、位置決め制御装置で位置決めされる位置決め機構が有する弾性変形に基づいたバネ定数Ｋｆに対して、外乱トルク信号に１／Ｋｆを乗じた位置補正信号を出力する。
【０００６】
また、本発明の第３発明は、請求項３に記載されたとおりの位置決め制御方法である。
請求項３に記載の位置決め制御方法は、移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法である。
位置決め制御の開始位置から目標位置に到達するまで、目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動し、前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する。
【発明の効果】
【０００７】
請求項１に記載の位置決め制御装置を用いれば、弾性変形を考慮した位置補正信号を、位置決め制御の開始位置から目標位置に到達するまで常に位置制御部に入力するので、位置決め制御の開始位置から目標位置に到達するまで常に目標位置そのものを補正して位置決め制御を行う。
弾性変形を考慮して目標位置を補正することで、位置決め精度をより向上させることができるとともに、一旦仮の目標位置に到達してから補正位置に移動させる従来技術の動作よりも、より短時間に位置決め制御を行うことができる。
【０００８】
また、請求項２に記載の位置決め制御装置によれば、位置補正部の定数を比較的容易に設定することができる。
【０００９】
また、請求項３に記載の位置決め制御方法によれば、駆動方法に関わらず（モータに限定されず）、物体を駆動させるために必須であるトルクという抽象化された物理量を用いて位置決め精度をより向上させ、より短時間に位置決め制御が可能であるとともに、種々の駆動方法を用いた位置決め制御装置に適用できる。
【発明を実施するための最良の形態】
【００１０】
以下に本発明を実施するための最良の形態を図面を用いて説明する。図１は、本発明の位置決め制御装置における制御ブロック図を示している。
●［制御ブロック図（図１）］
図１（Ａ）及び（Ｂ）を用いて、本発明の位置決め制御装置における制御ブロック図を説明する。図１（Ａ）は制御ブロック全体の構成を示しており、図１（Ｂ）は制御ブロック内における外乱トルク推定部７０と位置補正部８０の詳細を示している。
【００１１】
ノードＮａには、位置指令信号Ｐｔｇｔ（目標位置）が加算項で入力されるとともに位置補正信号ΔＰｔｇｔ（目標位置に対してリアルタイムに求めた、目標位置を補正する位置補正量（後述））が加算項で入力される。
この位置補正信号ΔＰｔｇｔを位置指令信号Ｐｔｇｔに加算項として追加している点、及び位置補正信号ΔＰｔｇｔを求める位置補正部８０を追加している点、そして位置補正信号ΔＰｔｇｔは目標トルクと実際のトルクに基づいて求める点、そして位置補正信号ΔＰｔｇｔは位置決め制御の開始位置から目標位置に到達するまで常に演算されて位置指令信号Ｐｔｇｔを補正する点が特徴である。
なお、位置指令信号Ｐｔｇｔを求める目標設定部は既存の技術で実現可能であり、図示を省略している。
【００１２】
ノードＮｂにはノードＮａからの出力が加算項で入力され、位置検出部６６ｂから出力される現在位置信号Ｐｆｂが減算項で入力される。
位置制御部６１には、位置指令信号Ｐｔｇｔ（加算項）、現在位置信号Ｐｆｂ（減算項）、位置補正信号ΔＰｔｇｔ（加算項）が入力され、速度制御部６２に速度指令信号Ｖｔｇｔを出力する。
速度制御部６２には位置制御部６１から出力される速度指令信号Ｖｔｇｔが入力され、ノードＮｃにトルク指令信号Ｔｔｇｔを出力する。
ノードＮｃには速度制御部６２から出力されるトルク指令信号Ｔｔｇｔが加算項で入力され、外乱トルク推定部７０から出力される外乱トルク信号Ｃｔが減算項で入力される。
電流制御部６３には、ノードＮｃからの出力（トルク指令信号Ｔｔｇｔ（加算項）と外乱トルク信号Ｃｔ（減算項）が入力され、モータ６４に制御電流Ｉｔｇｔを出力する。
【００１３】
モータ６４には電流制御部６３が出力した制御電流Ｉｔｇｔが入力され、機械部６５（機構部）を駆動する。
回転検出部６６ａは、例えばモータ６４の軸の回転数を検出可能なエンコーダ等であり、モータ６４の軸の回転が入力され、回転数に関する物理量（例えば角速度ω）を出力する。そして位置検出部６６ｂは、回転検出部６６ａの検出値に基づいて制御対象（例、モータ）の制御結果上の現在位置を示す現在位置信号Ｐｆｂを出力する。本実施の形態では、回転検出部６６ａと位置検出部６６ｂとで検出部６６を構成している。
外乱トルク推定部７０には、電流制御部６３から出力される制御電流Ｉｔｇｔと、回転検出部６６ａから出力されるモータ６４の軸の回転数に関する物理量とが入力され、外乱トルク信号Ｃｔを出力する。
位置補正部８０には、外乱トルク推定部７０から出力される外乱トルク信号Ｃｔが入力され、位置補正信号ΔＰｔｇｔを出力する。
【００１４】
外乱トルク推定部７０の内部では、電流制御部６３から入力される制御電流Ｉｔｇｔにトルク定数Ｋｔ１（図１（Ｂ）中の７１）を乗じた「目標トルク」をノードＮｄに出力し、回転検出部６６ａから入力される角速度（ω）に慣性モーメントＪｍ１（図１（Ｂ）中の７２）を乗じて更に微分器７３にて微分した「実際のトルク（実トルク）」をノードＮｄに出力する。
ノードＮｄでは目標トルクが加算項として入力され、実際のトルクが減算項として入力される。
そしてノードＮｄから入力された、「目標トルク」と「実際のトルク」との差分に基づいて外乱トルクを求め、外乱トルク信号Ｃｔを出力する。外乱トルク推定部７０は、その内部にローパスフィルタ７４を有している。ローパスフィルタ７４内の記号ｔｑ１は、外乱トルク推定部７０の時定数である。この時定数ｔｑ１は、設計者ないしユーザが適宜に定める。すなわち、サーボ系で発生する外乱ノイズを適切に低減し得るように、または、サーボ系の振動を適切に抑制し得るように、設計者ないしユーザが（実験的に、あるいはシミュレーションソフト等を使って）適当なる値に定める。
位置補正部８０の内部では、外乱トルク推定部７０から入力される外乱トルク信号Ｃｔに、１／Ｋｆ（Ｋｆは位置決め制御機構のバネ定数であり、位置決め制御機構に固有の定数であり、実験的に求める）を乗じて位置補正量を求め、位置補正信号ΔＰｔｇｔを出力する。
【００１５】
図１に示す本実施の形態の制御ブロック図は、図８に示す従来技術の制御ブロック図に対して、以下の点が異なる。
本実施の形態では、外乱トルク推定部７０への入力に制御電流Ｉｔｇｔを入力しているが、図８に示す従来技術では外乱推定部７ｗへの入力に制御電流の前段のトルク指令Ｔｒｅｆを入力している。本実施の形態では、外乱を考慮したトルク補正を行った後の制御電流Ｉｔｇｔを用いて目標トルクを求めている。
また、図８に示す従来技術では、外乱推定部７ｗは次段の位置補正部９ｗのみに使用されている。従って、従来技術では外乱推定部７ｗは位置補正専用である。しかし本実施の形態では、外乱トルク推定部７０は、外乱を考慮したトルク補正用として設けたものをそのまま流用しており、新たに設ける必要がない。
また、制御方法において、従来技術では、（まず、弾性変形を考慮しない状態で移動させて）一旦仮の目標位置に到達してから位置補正量分だけ移動させて真の目標位置に到達させている。しかし本実施の形態における制御方法では、位置決め制御の開始位置から目標位置に到達するまで常に（リアルタイムに）位置補正を行っている。
【００１６】
●［位置決め制御装置を用いた産業用ロボットの例（図２）］
次に図２を用いて、図１にて説明した位置決め制御装置を用いた産業用ロボットの例（この例は樹脂整形装置）を説明する。
樹脂成形装置は、基台４０に載置された射出装置１０、固定プラテン２０及び可動プラテン３０から構成されている。固定プラテン２０には固定金型２２が取付けられており、可動プラテン３０には可動金型３２が取付けられている。
ここで、互いに直交する３軸（ｘ軸／ｙ軸／ｚ軸）を設定する（垂直方向がｚ軸）。
位置決め制御装置は、固定プラテン２０に固定されている固定部１、固定部１に取付けられた制御手段４及び支持体２ｘ、支持体２ｘに沿って移動する移動体３ｘ、移動体３ｘに取付けられた支持体２ｙ、支持体２ｙに沿って移動する移動体３ｙ、移動体３ｙに取付けられた支持体２ｚ、支持体２ｚに沿って移動する移動体３ｚ、移動体３ｚに取付けられた捕獲手段５で構成されている。支持体２ｘ〜支持体２ｚ及び移動体３ｘ〜移動体３ｚが移動手段に相当する。また、支持体２ｘはｘ軸にほぼ平行であり、支持体２ｙはｙ軸にほぼ平行であり、支持体２ｚはｚ軸にほぼ平行である。
制御手段４は、記憶手段に接続されており、移動体３ｘ〜移動体３ｚを駆動して捕獲手段５を任意の位置（支持体２ｘ〜支持体２ｚにより移動可能な範囲の位置）に移動させることが可能であり、捕獲手段５を用いて成形品７（被移動部材等）の捕獲、あるいは捕獲の解除が可能である。
【００１７】
●［弾性変形の例（図３）及び制御ブロック図の例（図４）］
次に図３を用いて位置決め制御装置における弾性変形について説明する。なお、図３（Ａ）は図２から位置決め制御装置に相当する構成要素を抽出した図であり、図３（Ｂ）は図３（Ａ）の中から更にｘ軸方向に関する構成要素を抽出した図である。なお、図３、図５、図６では、弾性変形の様子をわかり易くするために、実際の弾性変形量よりも大きく変形させて記載している。
まず、図３（Ａ）を用いて弾性変形の状態について説明する。
図３（Ａ）に示すように、支持体２ｘは固定部１に固定されており、移動体３ｘが支持体２ｘに沿って移動して固定部１から遠ざかるにつれて、支持体２ｘが一点鎖線で示す支持体２ｘｒのように、重力により弾性変形する可能性がある。また、支持体２ｙは移動体３ｘに固定されており、移動体３ｙが支持体２ｙに沿って移動して移動体３ｘから遠ざかるにつれて、支持体２ｙが一点鎖線で示す支持体２ｙｒのように、重力により弾性変形する可能性がある。支持体２ｘｒ及び支持体２ｙｒに示すような弾性変形が発生した場合、位置決め精度が要求される捕獲手段５の位置が、目標位置（図３（Ａ）中の捕獲手段５の位置）から、一点鎖線で示す捕獲手段５ｒの位置にずれることとなる。
【００１８】
次に、図３（Ｂ）を用いて弾性変形による位置の誤差（この例では、支持体２ｘの弾性変形によるｘ軸方向とｚ軸方向の誤差）について説明する。
図３（Ｂ）に示すように、支持体２ｘが支持体２ｘｒに示すように弾性変形した場合、移動体３ｘは支持体２ｘｒに沿って移動し、真の目標位置Ｐｔｇｔに移動したつもりでも、実際の位置Ｐｒに到達する。この場合、ｘ軸方向にはΔＸ、ｚ軸方向にはΔＺの誤差が発生している。つまり、支持体２ｘの弾性変形を考慮して位置補正を行う場合は、ｘ軸方向とｚ軸方向の位置補正が必要である。図示しないが同様に、支持体２ｙの弾性変形を考慮して位置補正を行う場合は、ｙ軸方向とｚ軸方向の位置補正が必要である。
なお、支持体２ｚは重力方向に平行であり、支持体２ｚには弾性変形が発生しないと考えられるため、本実施の形態では、支持体２ｚの弾性変形を考慮した位置補正は行っていない。
【００１９】
上記の内容（ｘ軸方向に移動する際はｘ軸方向とｚ軸方向とに位置補正が必要、ｙ軸方向に移動する際はｙ軸方向とｚ軸方向とに位置補正が必要、ｚ軸方向に移動の際は特に位置補正は必要ない）に基づいて、図１に示す制御ブロック図を、図２に示す樹脂整形装置に適用した例を図４に示す。
図４に示すｘ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０ｘのバネ定数として支持体２ｘ（すなわちｘ軸）に固有のバネ定数Ｋｆｘを用いている点と、ｚ軸方向の位置補正を出力するための位置補正部８０ｘｚ（バネ定数は固有のＫｆｘｚ）が追加されている。
また、図４に示すｙ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０ｙのバネ定数として支持体２ｙ（すなわちｙ軸）に固有のバネ定数Ｋｆｙを用いている点と、ｚ軸方向の位置補正を出力するための位置補正部８０ｙｚ（バネ定数は固有のＫｆｙｚ）が追加されている。
なお、各々のバネ定数は実験的に求める。
【００２０】
また、図４に示すｚ軸方向の制御ブロック図は、図１に示す制御ブロック図に対して、位置補正部８０が省略されている。また制御ブロック図の先頭のノードＮａｚに、位置指令信号Ｐｔｇｔの他に、ｘ軸方向の制御ブロック図の位置補正部８０ｘｚからの位置補正信号ΔＰｔｇｔとｙ軸方向の制御ブロック図の位置補正部８０ｙｚからの位置補正信号ΔＰｔｇｔとが加算項として入力されている。
この制御ブロック図にて、ｘ軸方向に移動する際はｘ軸方向とｚ軸方向とに位置補正を行い、ｙ軸方向に移動する際はｙ軸方向とｚ軸方向とに位置補正を行い、ｚ軸方向に移動する際は特に位置補正を行わない、という構成を実現している。その他は図１で既に説明しているので説明を省略する。
【００２１】
●［位置決め制御動作の比較（目標に対して実際のトルクが小さい場合）（図５）］
次に図５（Ａ）〜（Ｄ）を用いて、支持体２ｘの弾性変形を例にして、本発明と従来の動作の違いを説明する。なお、図５（Ａ）〜（Ｄ）は、移動体３ｘにおいて目標トルクよりも実際のトルクが小さい場合の例（移動体３ｘが、支持体２ｘの弾性変形量が大きくなる方向に移動する場合の例）を示している。
図５（Ａ）〜（Ｄ）は、移動体３ｘが位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動する場合の様子を示しており、最終的に位置決め精度が要求される捕獲手段５（図２を参照）の中心Ｓが、位置Ｓｏｒｇから目標位置Ｓｔｇｔに移動する様子を示しており、中心Ｓの軌跡も示している。
【００２２】
［理想状態（図５（Ａ）：弾性変形が発生しない場合）］
まず、図５（Ａ）を用いて、支持体２ｘに弾性変形が発生しない理想状態の例を説明する。支持体２ｘに弾性変形が発生しないため、移動体３ｘの位置を位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させても、移動体３ｘから支持体２ｙ及び支持体２ｚ等を介して接続された捕獲手段５の中心Ｓは目標位置のＳｔｇｔに移動し、中心Ｓの軌跡も最短距離である直線となる。
【００２３】
［従来１の動作（図５（Ｂ）：発生した弾性変形に対する考慮なし）］
図５（Ｂ）は、発生した弾性変形に対して位置補正を行わない従来の例（図１に示す制御ブロック図から位置補正部８０を省略した状態）を示しており、支持体２ｘに弾性変形が発生し、支持体２ｘが支持体２ｘｒの形状に湾曲した場合の例を示している。
移動体３ｘを位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させた場合、移動体３ｘの実際の位置は支持体２ｘｒ上の位置Ｐｒである。従って、ｘ軸方向に対しては距離ΔＸ分だけ誤差が発生しており、ｚ軸方向に対しては距離ΔＺ分だけ誤差が発生している。従って移動先の中心Ｓｒは目標位置の中心ＳｔｇｔからΔＸ及びΔＺずれた位置にある。また、中心Ｓの軌跡も支持体２ｘｒの湾曲形状に沿った曲線となる。
誤差ΔＸ及びΔＺが許容範囲内の誤差であれば特に問題ないが、そうでない場合は支持体２ｘの材質または形状等を変更して剛性を高くして、弾性変形量を低減させなければならない。
このため、再設計及び再評価による工数の増加、そして材質または形状変更によるコストの増加等の種々の問題が発生する可能性がある。
【００２４】
［従来２の動作（図５（Ｃ）：発生した弾性変形に対して仮の目標位置に到達した後に位置を補正）］
図５（Ｃ）は、発生した弾性変形に対して、仮の目標位置に一旦移動した後、弾性変形量に基づいた補正量分だけ位置を補正する、従来の技術の例を示している。
まず最初は図５（Ｂ）と同様の動作（一旦仮の目標位置まで移動）をさせるため、中心Ｓｒの位置及び支持体２ｘｒに沿った曲線状の軌跡も図５（Ｂ）と同様である。そして中心Ｓｒの位置（仮の目標位置）に到達してから、ΔＸ及びΔＺの位置補正を行い、中心Ｓｒの位置を中心Ｓｔｇｔの位置（真の目標位置）に移動させる。つまり、中心Ｓｏｒｇを中心Ｓｒまで移動させる第１動作と、中心Ｓｒを中心Ｓｔｇｔまで移動させる第２動作を行う２段階の動作で移動を完了させる。
従って、２段階目の動作時間分、時間が余計にかかる。また、図５（Ａ）と比較して異なる軌跡となるため、障害物等が存在した場合、理想状態（図５（Ａ））では障害物に触れることがなくても、図５（Ｃ）に示す動作では障害物に触れる可能性がある。
【００２５】
［本発明の動作（図５（Ｄ））］
図５（Ｄ）は、発生した弾性変形に対して、位置決め制御の開始位置から目標位置に到達するまで常に目標位置を補正する（リアルタイムに位置補正を行う）本発明の例を示している。
移動体３ｘは、鉛直下方に向かって湾曲した（弾性変形した）支持体２ｘｒに沿って位置Ｐｏｒｇから目標位置Ｐｔｇｔに向かって移動していくと、湾曲した支持体２ｘｒの傾斜を下ることになるので、目標トルクよりも小さなトルクで移動することになる。
ここで図１（Ｂ）に示すように外乱トルク推定部７０にて、目標トルク＞実際のトルクの場合、外乱トルク信号Ｃｔが正の値として出力される。
続いて図４に示すように、ｘ軸方向の制御ブロック図における外乱トルク信号Ｃｔが正の値の場合、同ブロック図における位置補正部８０ｘにてｘ軸位置補正信号ΔＰｔｇｔが正の値として出力される。このｘ軸位置補正信号ΔＰｔｇｔがΔＸ（＋ΔＸ）となるわけである。
また、同ブロック図における位置補正部８０ｘｚにてｚ軸位置補正信号ΔＰｔｇｔが正の値として出力される。このｚ軸位置補正信号ΔＰｔｇｔがΔＺ（＋ΔＺ）となるわけである。
これらがリアルタイムに動作するため、中心Ｓｏｒｇが中心Ｓｔｇｔに移動した軌跡は、図５（Ａ）に示す理想状態と同じ直線状となる。もちろん移動先の中心Ｓｔｇｔは理想状態と同じ位置である。すなわち、本発明では、図５（Ａ）に示す理想状態と同じ軌跡を通って、同じ位置に到達する。このため、余計な時間がかからないとともに、障害物等に触れることもない。
【００２６】
●［位置決め制御動作の比較（目標に対して実際のトルクが大きい場合）（図６）］
次に図６（Ａ）〜（Ｄ）を用いて、支持体２ｘの弾性変形を例にして、本発明と従来の動作の違いを説明する。なお、図６（Ａ）〜（Ｄ）は、図５（Ａ）〜（Ｄ）に示した例に対して逆方向へ移動する場合の例を示しており、移動体３ｘにおいて目標トルクよりも実際のトルクが大きい場合の例（移動体３ｘが、支持体２ｘの弾性変形量が小さくなる方向に移動する場合の例）を示している。
図６（Ａ）〜（Ｄ）は、移動体３ｘが位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動する場合の様子を示しており、図５（Ａ）〜（Ｄ）と同様に、最終的に位置決め精度が要求される捕獲手段５（図２を参照）の中心Ｓが、位置Ｓｏｒｇから目標位置Ｓｔｇｔに移動する様子を示しており、中心Ｓの軌跡も示している。
【００２７】
［理想状態（図６（Ａ）：弾性変形が発生しない場合）］
まず、図６（Ａ）を用いて、支持体２ｘに弾性変形が発生しない理想状態の例を説明する。支持体２ｘに弾性変形が発生しないため、移動体３ｘの位置を位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させても、移動体３ｘから支持体２ｙ及び支持体２ｚ等を介して接続された捕獲手段５の中心Ｓは目標位置のＳｔｇｔに移動し、中心Ｓの軌跡も最短距離である直線となる。
【００２８】
［従来１の動作（図６（Ｂ）：発生した弾性変形に対する考慮なし）］
図６（Ｂ）は、発生した弾性変形に対して位置補正を行わない従来の例（図１に示す制御ブロック図から位置補正部８０を省略した状態）を示しており、支持体２ｘに弾性変形が発生し、支持体２ｘが支持体２ｘｒの形状に湾曲した場合の例を示している。
移動体３ｘを位置Ｐｏｒｇから目標位置Ｐｔｇｔに移動させた場合、移動体３ｘの実際の位置は支持体２ｘｒ上の位置Ｐｒである。従って、ｘ軸方向に対しては距離ΔＸ１分だけ誤差が発生しており、ｚ軸方向に対しては距離ΔＺ１分だけ誤差が発生している。従って移動先の中心Ｓ１ｒは目標位置の中心ＳｔｇｔからΔＸ１及びΔＺ１ずれた位置にある。また、中心Ｓの軌跡も支持体２ｘｒの湾曲形状に沿った曲線となる。以下、図５（Ｂ）の説明と同様であるので説明を省略する。
【００２９】
［従来２の動作（図６（Ｃ）：発生した弾性変形に対して仮の目標位置に到達した後に位置を補正）］
図６（Ｃ）は、発生した弾性変形に対して、仮の目標位置に一旦移動した後、弾性変形量に基づいた補正量分だけ位置を補正する、従来の技術の例を示している。
まず最初は図６（Ｂ）と同様の動作をさせるため、支持体２ｘｒに沿った曲線状の軌跡は図６（Ｂ）と同様である（ただし、最初の位置Ｐｏｒｇが補正された正しい位置であるため、中心Ｓｏｒｇの位置及び仮の目標位置の中心Ｓ２ｒの位置は異なる）。そして中心Ｓ２ｒの位置（仮の目標位置）に到達してから、ΔＸ１及びΔＺ１の位置補正を行い、中心Ｓ２ｒの位置を中心Ｓｔｇｔの位置（真の目標位置）に移動させる。つまり、中心Ｓｏｒｇを中心Ｓ２ｒまで移動させる第１動作と、中心Ｓ２ｒを中心Ｓｔｇｔまで移動させる第２動作を行う２段階の動作で移動を完了させる。
従って、２段階目の動作時間分、時間が余計にかかる。また、図６（Ａ）と比較して異なる軌跡となるため、障害物等が存在した場合、理想状態（図６（Ａ））では障害物に触れることがなくても、図６（Ｃ）に示す動作では障害物に触れる可能性がある。
【００３０】
［本発明の動作（図６（Ｄ））］
図６（Ｄ）は、発生した弾性変形に対して、位置決め制御の開始位置から目標位置に到達するまで常に目標位置を補正する（リアルタイムに位置補正を行う）本発明の例を示している。
移動体３ｘは、鉛直下方に向かって湾曲した（弾性変形した）支持体２ｘｒに沿って位置Ｐｏｒｇから目標位置Ｐｔｇｔに向かって移動していくと、湾曲した支持体２ｘｒの傾斜を上ることになるので、目標トルクよりも大きなトルクで移動することになる。
ここで図１（Ｂ）に示すように外乱トルク推定部７０にて、目標トルク＜実際のトルクの場合、外乱トルク信号Ｃｔが負の値として出力される。
続いて図４に示すように、ｘ軸方向の制御ブロック図における外乱トルク信号Ｃｔが負の値の場合、同ブロック図における位置補正部８０ｘにてｘ軸位置補正信号ΔＰｔｇｔが負の値として出力される。このｘ軸位置補正信号ΔＰｔｇｔがΔＸ１（−ΔＸ１）となるわけである。
また、同ブロック図における位置補正部８０ｘｚにてｚ軸位置補正信号ΔＰｔｇｔが負の値として出力される。このｚ軸位置補正信号ΔＰｔｇｔがΔＺ１（−ΔＺ１）となるわけである。
これらがリアルタイムに動作するため、中心Ｓｏｒｇが中心Ｓｔｇｔに移動した軌跡は、図６（Ａ）に示す理想状態と同じ直線状となる。もちろん移動先の中心Ｓｔｇｔは理想状態と同じ位置である。すなわち、本発明では、図６（Ａ）に示す理想状態と同じ軌跡を通って、同じ位置に到達する。このため、余計な時間がかからないとともに、障害物等に触れることもない。
【００３１】
●［シミュレーション結果（図７）］
図７に、本実施の形態におけるシミュレーションの結果を示す。
図７（Ａ）は従来１（図１に示す制御ブロック図から位置補正部８０を省略した状態）の動作の結果を示しており、図７（Ｂ）は本発明の動作の結果を示している。グラフは横軸が時間［sec］を示しており、縦軸が位置［cm］を示している。
どちらも同じ位置（０［cm］の位置）からスタートし、位置指令信号Ｐｔｇｔ（目標位置）＝４０．０［cm］を入力した結果である。
応答特性（通常、目標位置の９０％位置（この場合３６［cm］の位置）までに要する時間）は、図７（Ａ）に示す従来１の動作も、図７（Ｂ）に示す本発明の動作も同じである。
しかし、到達位置が大きく異なる。図７（Ａ）に示す従来１の動作では目標位置４０．０［cm］に対してΔＸの誤差が発生している。これが図５（Ｂ）に示すΔＸの誤差である。これに対して図７（Ｂ）に示す本発明の動作では、目標位置４０．０［cm］に完全に一致しており、非常に高い精度で位置決め制御できていることを確認できた。また、オーバーシュートも発生せず、非常に良好な制御特性を実現できることが確認された。
【００３２】
以上に説明した実施の形態では、移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法を説明した。
すなわち、位置決め制御の開始位置から目標位置に到達するまで、目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動させる。そして、前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する、という位置決め制御方法を説明した。
また、以上に説明した実施の形態では、サーボモータを用いた直交３軸（ｘ軸／ｙ軸／ｚ軸）の位置決め制御装置に適用した例で説明したが、トルクという抽象化された物理量を用いているため、サーボモータに限定されず、種々の駆動手段（例えばリニアモータ、内燃機関等）を用いた位置決め制御に適用することができる。
【００３３】
本発明の位置決め制御装置及び位置決め制御方法は、本実施の形態で説明した構成、動作等に限定されず、本発明の要旨を変更しない範囲で種々の変更、追加、削除が可能である。例えば、外乱トルク推定部７０の内部にて求める（検出する）実際のトルクは、角速度ωから求めずに、トルクセンサ等を用いて求める（検出する）ようにしてもよい。
また、本実施の形態の説明に用いた数値は一例であり、この数値に限定されるものではない。
【図面の簡単な説明】
【００３４】
【図１】本発明の位置決め制御装置における制御ブロック図の例を説明する図である。
【図２】本発明の位置決め制御装置を用いた産業用ロボットの例を説明する図である。
【図３】図２に示す産業用ロボットの弾性変形の例を説明する図である。
【図４】図２に示す産業用ロボットの位置決め制御に、図１に示す制御ブロック図を適用した例を説明する図である。
【図５】位置決め制御動作の比較（目標に対して実際のトルクが小さい場合）を説明する図である。
【図６】位置決め制御動作の比較（目標に対して実際のトルクが大きい場合）を説明する図である。
【図７】従来技術（図７（Ａ））と本発明（図７（Ｂ））とのシミュレーション結果を示すグラフである。
【図８】従来の技術（当初の目標位置に到達した後に位置を補正）の制御ブロック図を説明する図である。
【符号の説明】
【００３５】
６１ 位置制御部
６２ 速度制御部
６３ 電流制御部
６４ モータ
６５ 機械部（機構部）
６６ 検出部
６６ａ 回転検出部
６６ｂ 位置検出部
７０ 外乱トルク推定部
８０ 位置補正部
Ｎａ〜Ｎｄ ノード
Ｐｔｇｔ 位置指令信号（目標位置）
Ｖｔｇｔ 速度指令信号
Ｔｔｇｔ トルク指令信号
Ｉｔｇｔ 制御電流
Ｃｔ 外乱トルク信号
Ｐｆｂ 現在位置信号
ΔＰｔｇｔ 位置補正信号
Ｋｔ１ トルク定数
Ｋｆ、Ｋｆｘ、Ｋｆｘｚ、Ｋｆｙ、Ｋｆｙｚ バネ定数
【特許請求の範囲】
【請求項１】
入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、
入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、
入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、
モータの回転数に関する物理量を検出して、制御対象の現在位置を示す現在位置信号を出力する検出部と、
電流制御部からモータに出力される制御電流と、モータの回転数に関する物理量とが入力され、外乱トルクを推定して外乱トルク信号を出力する外乱トルク推定部と、
を備えた位置決め制御装置であって、
更に、前記外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する、
ことを特徴とする位置決め制御装置。
【請求項２】
請求項１に記載の位置決め制御装置であって、
位置補正部は、位置決め制御装置で位置決めされる位置決め機構が有する弾性変形に基づいたバネ定数Ｋｆに対して、外乱トルク信号に１／Ｋｆを乗じた位置補正信号を出力する、
ことを特徴とする位置決め制御装置。
【請求項３】
移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法であって、
位置決め制御の開始位置から目標位置に到達するまで、
目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動し、
前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する、
ことを特徴とする位置決め制御方法。
【請求項１】
入力された位置指令信号と現在位置信号とに基づいた速度指令信号を速度制御部に出力する位置制御部と、
入力された速度指令信号に基づいたトルク指令信号を電流制御部に出力する速度制御部と、
入力されたトルク指令信号と外乱トルク信号とに基づいた制御電流を出力してモータを駆動する電流制御部と、
モータの回転数に関する物理量を検出して、制御対象の現在位置を示す現在位置信号を出力する検出部と、
電流制御部からモータに出力される制御電流と、モータの回転数に関する物理量とが入力され、外乱トルクを推定して外乱トルク信号を出力する外乱トルク推定部と、
を備えた位置決め制御装置であって、
更に、前記外乱トルク信号が入力されて位置補正信号を出力する位置補正部を備え、位置決め制御の開始位置から目標位置に到達するまで位置補正信号を位置制御部の入力に追加する、
ことを特徴とする位置決め制御装置。
【請求項２】
請求項１に記載の位置決め制御装置であって、
位置補正部は、位置決め制御装置で位置決めされる位置決め機構が有する弾性変形に基づいたバネ定数Ｋｆに対して、外乱トルク信号に１／Ｋｆを乗じた位置補正信号を出力する、
ことを特徴とする位置決め制御装置。
【請求項３】
移動体を支持体上の目標位置に移動させるにあたって、移動体の重力で支持体が弾性変形する場合における位置決め制御方法であって、
位置決め制御の開始位置から目標位置に到達するまで、
目標位置と移動体の現在位置との位置の差分に基づいて、移動体を移動させる目標トルクを求めて移動体を駆動し、
前記目標トルクと移動体が実際に移動した際に検出した実トルクとのトルクの差分を求めるとともに、前記トルクの差分に基づいて目標位置を補正する位置補正量を求め、当該位置補正量を目標位置に加算する、
ことを特徴とする位置決め制御方法。
【図７】
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図８】
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図８】
【公開番号】特開２００６−３１６２７（Ｐ２００６−３１６２７Ａ）
【公開日】平成１８年２月２日（２００６．２．２）
【国際特許分類】
【出願番号】特願２００４−２１３３２２（Ｐ２００４−２１３３２２）
【出願日】平成１６年７月２１日（２００４．７．２１）
【出願人】（３９１００６３４８）株式会社タイテック (79)
【Ｆターム（参考）】
【公開日】平成１８年２月２日（２００６．２．２）
【国際特許分類】
【出願日】平成１６年７月２１日（２００４．７．２１）
【出願人】（３９１００６３４８）株式会社タイテック (79)
【Ｆターム（参考）】
[ Back to top ]