JP5146512B2 - 制御装置、制御システムおよび制御方法 - Google Patents

Description

本発明は、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置、当該制御装置を含む制御システム、および、制御方法に関する。

従来から、さまざまな産業分野において、いわゆる産業用ロボットやＮＣ（Numerical Control）工作機械といった自動化装置が導入されている。このような装置の導入により、生産効率を高めることができる。

このような装置は、ワークを掴んだりするためのアームや、ワークを加工するためのエンドミルといった可動部を有している。このような可動部は、複数の駆動軸により移動可能となっており、これらの各駆動軸は、サーボモータやステッピングモータによって駆動される。

このような装置を用いてワークに対して加工などを行なう場合には、その挙動を決定する必要がある。このような挙動の決定には、予め挙動を計算しておくオフライン計算方式と作動中に動的に計算するオンライン計算方式とが存在する。

このような装置の挙動を制御するための従来技術としては、種々のものが提案されている。たとえば、特開平０７−０６４６２０号公報（特許文献１）に開示される数値制御装置は、加減速制御を考慮した高速かつ高精度な速度制御が行なえ、スプライン曲線上を滑らかに駆動でき、工作機械の異常振動が低減でき、速度優先と精度優先の選択を可能とする。より具体的には、特許文献１に開示された数値制御装置は、始点および終点での急な加減速を抑制することで、機械振動を抑制するとともに、目標位置を超えることなく、かつ指定した加（減）速度で減速停止できる。

また、特開２０００−１４８２２３号公報（特許文献２）に開示される曲線補間加減速制御方法は、曲線の形状に適した速い速度で移動するなめらかな加減速度を得る。

また、特開２０００−１６３１１４号公報（特許文献３）に開示される補間曲線内加減速処理方法は、曲線の曲率にかかわらず、指令軌道の通りに機械を動かすことができる。

また、特開平０６−０７５６２１号公報（特許文献４）に開示されるＮＣ加工法は、曲率の大きな部分に対してのみ新たなサンプリングポイントを追加することにより、最小限のサンプリングポイント（データ）数で曲線を近似し、滑らかで形状差の少ない元の図形を再現する。

また、特開平０１−１４７６１０号公報（特許文献５）に開示される数値制御装置は、スプライン補間制御により補間制御を行なう。

ところで、同一の種類の製品を大量生産する生産ラインに設置される装置では、目標軌道などの設定値（加工プログラム）を予め設定しておき、この設定値に従って動作させればよい。しかしながら、さまざまな製品を生産する多種少量生産の生産ラインなどでは、このような設定値（加工プログラム）を製品毎に設定することはコスト面などから難しい。そこで、たとえば、共通の加工プログラムを利用しつつ、商品仕様などに応じて、目標軌道などを動的に設定することで、生産コストを低減したいというニーズがある。

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

上述のような目標軌道を動的に設定した場合には、その軌道長を正確に算出することが難しい。そのため、目標軌道上の速度設定が難しい。

また、ワークを切削加工するような処理では、エンドミルなどのツールの移動速度が切削量を左右するので、ツールの位置だけでなく移動速度の制御が重要である。すなわち、ツールの加速度の変動は、加工面の外観異常や品質不良につながる。そのため、移動中の加速度は可能な限り一定にしたいというニーズがある。特に、直線部分においては、加速度が変動すると不具合が発生し易い。

さらに、ワークを切削加工するような処理では、ある区間に対する加工処理の後、続いて別の区間に対して加工処理がなされる場合がある。そのため、設定した目標軌道の終点における移動速度が所定の目標値に維持されていることが好ましい。そうでなければ、目標軌道の終点の近くで移動速度が不連続になったり、後続の直線区間において加速度が不連続になったりする。

上述した特許文献１〜５に開示される技術は、このような課題を解決するものではなかった。

たとえば、特許文献１に開示されるような加速度を一定値に制限する方法では、算出される終点までの距離についての誤差や速度上昇中の速度変動によって、目標軌道の終点に到達するタイミングがばらつくため、終点の移動速度を所定の目標値に維持することができない。

また、特許文献２および３に開示される技術は、予め補間曲線や目標速度を設計（オフライン計算方式）していたため、目標軌道や終点を動的に変更することが困難であった。仮に、目標軌道や終点を動的に変更できたとしても、目標速度が固定的に設計されているので、移動中に出力飽和などによって速度変動があると、その速度変動からの回復のために過剰な速度変動が発生し、加工面の外観異常や品質変動といった不具合が生じる可能性があった。

さらに、移動速度が所定の目標値となるように目標軌道の終点へ到達させるためには、現在位置から終点までの軌道長（残距離）を制御周期毎に算出した上で、速度指令を計算する必要があり演算量が増大する。そのため、各制御周期内でこのような演算処理を完結するためには、処理能力の制約などによって容易ではなかった。

そこで、本発明は、このような課題を解決するためになされたものであり、その目的は、目標軌道が動的に生成された場合であっても、その目標軌道の終点における速度を目標値に維持することのできる制御装置、制御システムおよび制御方法を提供することである。

本発明のある局面によれば、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置を提供する。制御装置は、始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、目標軌道における現在位置から終点までの残距離を推定する残距離推定手段と、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む。

好ましくは、加速度算出手段は、現在の移動速度、目標速度、および、距離に基づいて加速度を算出する。

好ましくは、加速度算出手段は、各制御周期において、現在の移動速度を目標速度まで一定レートで変化させるように、加速度を算出する。

好ましくは、残距離推定手段は、目標軌道における現在位置から終点までの区間に対して予め定められた数のサンプル点を設定するとともに、設定したサンプル点を補間することで残距離を推定する。

さらに好ましくは、残距離推定手段は、目標軌道における現在位置から終点までの区間について、現在位置に遠い区間に比較して現在位置に近い区間ほど、サンプル点の間隔がより密になるように、サンプル点を設定する。

好ましくは、目標軌道は、媒介変数を含む関数の形で定義される。
好ましくは、制御装置は、移動速度更新手段によって算出された移動速度の新たな目標値となるように、可動部が１制御周期内に移動すべき量を示す指令を出力する移動指令手段をさらに含み、移動指令手段は、移動速度の新たな目標値に対応して、可動部が移動すべき距離を算出する手段と、可動部が移動すべき距離に対応して、目標軌道における可動部の移動すべき位置を算出する手段と、可動部の移動すべき位置に対応して、複数の駆動軸の各々に対する移動指令を算出する手段とを含む。

さらに好ましくは、目標軌道は、ベジェ曲線により定義される。
好ましくは、目標軌道生成手段は、外部入力される信号に応答して、目標軌道を生成する。

好ましくは、移動速度更新手段は、加速度および／または加加速度の変化量が予め定められた制限を超過しないように、移動速度の新たな目標値を算出する。

この発明の別の局面に従う制御システムは、複数の駆動軸により移動可能な可動部を有する可動機構と、可動機構を駆動するためのドライバと、ドライバに対して制御指令を与える制御装置とを含む。制御装置は、始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、目標軌道における現在位置から終点までの残距離を推定する残距離推定手段と、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む。

この発明のさらに別の局面に従えば、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御方法を提供する。制御方法は、始点および終点により定義される移動区間に対して目標軌道を生成するステップと、目標軌道における現在位置から終点までの残距離を推定するステップと、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出するステップと、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出するステップとを含む。

本発明によれば、目標軌道が動的に生成された場合であっても、その目標軌道の終点における速度を目標値に維持することができる。

本実施の形態に従うシステムの外観を示す模式図である。 本実施の形態に従うシステムに含まれる制御装置の外観を示す模式図である。 本実施の形態に従うシステムに含まれる制御装置のハードウェア構成を示す模式図である。 本実施の形態に従う制御ロジックの処理手順の概略を示すフローチャートである。 ワークへの切削加工処理の一例を示す図である。 ワークをＬ字状に切削する場合に円弧補間を用いて目標軌道を生成する処理例を示す図である。 ワークのコーナーを切削する場合に自由曲線を用いて目標軌道を生成する処理例を示す図である。 本実施の形態に従う目標軌道（ベジェ曲線）の生成方法について説明するための図である。 本実施の形態に従う残距離の推定方法について説明するための図である。 本実施の形態に従う目標加速度の算出方法について説明するための図である。 本実施の形態に従うベジェ曲線およびその軌道長の特性を示す図である。 ベジェ曲線における媒介変数の変位量と軌道長の変位量との関係を示す図である。 本実施の形態に従う制御ロジックにおける軌道長の変化量に対する媒介変数増分の算出方法について説明するための図である。 本実施の形態に従う制御ロジックを実現するための機能ブロックを示す模式図である。 本実施の形態に従う制御ロジックによって得られる可動機構の挙動例を示すタイムチャートである。 本実施の形態に従う制御ロジックの全体処理手順を示すフローチャートである。 本実施の形態の変形例に従う制御ロジックによって得られる可動機構のＴＣＰ速度を示すタイムチャートである。

本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰返さない。

＜Ａ．システム構成＞
まず、本実施の形態に従うシステムＳＹＳの全体構成について説明する。以下の説明では、本実施の形態に従うシステムＳＹＳの適用例として、ワークを切削加工する生産ラインについて説明する。このような切削加工の典型例としては、木材加工、ガラス加工、金属加工などが挙げられる。当然のことながら、本発明は、これらの実施の形態に記載された分野に限定されるものではなく、各種の産業分野へ適用可能である。

図１は、本実施の形態に従うシステムＳＹＳの外観を示す模式図である。図２は、本実施の形態に従うシステムＳＹＳに含まれる制御装置１の外観を示す模式図である。図３は、本実施の形態に従うシステムＳＹＳに含まれる制御装置１のハードウェア構成を示す模式図である。

図１を参照して、本実施の形態に従うシステムＳＹＳは、制御装置１と、切削対象となる木材や金属などのワーク２の寸法などを示すメジャー３と、制御装置１からの指令によって駆動されるロボット（可動機構）４と、オペレータなどの操作に応答して軌道変更などのイベントを出力するスイッチ５とを含む。制御装置１については、図２および図３を参照して後に詳述する。

ワーク２は、ベース上に置載されるとともに、オペレータなどの操作によって、それぞれ所望の形状に切断される。典型的には、このワーク２に沿って、メジャー３が設けられており、可動機構４がある軌道に沿って切削を行い、目的の位置まで到達すると、オペレータがスイッチ５を操作する。これにより、後述するような制御ロジックに従って、軌道が変更される。

可動機構４は、複数の駆動軸により移動可能な可動部（アーム部分）を有する装置であり、このアーム部分の先端には、ワーク２を切削するためのツール（典型的には、エンドミル）が装着されている。このアーム部分は、後述するように、それぞれにモータが機械的に連結された複数の駆動軸（典型的には、３軸または６軸）に従って移動する。

制御装置１は、このエンドミルの位置を、後述するような制御ロジックに従って制御することで、ワーク２を目的の大きさや形状を有する部品に加工する。なお、このエンドミルの位置を、以下では「ＴＣＰ（Tool Center Point）」とも称する。また、可動機構４のアーム部分の移動速度（エンドミルの移動速度）を「ＴＣＰ速度」とも称する。

本実施の形態に従う制御装置１は、オペレータによるスイッチ５に対する操作などに応答して、目標軌道を動的に決定／変更する。たとえば、同一のワーク２に対して、品種によって異なる長さを有する略長方形の部品を製作することを考えると、加工を開始する部分（すなわち、直線部分の長さ）は異なるが、加工する内容（すなわち、加工面の曲率など）は同じとなることも多い。このような場合には、同一の加工プログラム（加工面における目標軌道などの情報が定義されている）を利用して、その絶対的な目標軌道を品種に応じて動的に決定することができる。このような目標軌道の動的な決定は、オペレータがメジャー３（および、その目盛り上に設けられた矢印）と可動機構４との位置関係を確認して、スイッチ５を操作することで実行される。

図２を参照して、制御装置１は、典型的には、いわゆるＰＬＣ（Programmable Logic Controller）をベースとして構成されている。具体的には、制御装置１は、ＰＬＣ２０と、モータドライバ３１，３２，３３とを含む。

ＰＬＣ２０は、後述する本実施の形態に従う制御ロジックを実行する処理主体である。なお、制御ロジックは、一般的にモーションプログラムと称されるものであり、可動機構４を駆動するためのモータを制御するプログラムである。このようなモーションプログラムのシーケンスは、ラダー図（ラダーダイアグラム）言語などを用いてシーケンスプログラムの形で記述される場合もある。さらに、このようなシーケンスプログラムに加えて、各種の算術演算を行なうためのプログラム（ファンクション）が実行される。

より具体的には、ＰＬＣ２０は、電源ユニット２１と、主演算ユニット２２と、ＩＯ（Input Output）ユニット２３と、ＭＣ（Motion Control）ユニット２４とを含む。電源ユニット２１、主演算ユニット２２、ＩＯユニット２３、および、ＭＣユニット２４は、バックプレーン２５を介して互いに電気的に接続される。

電源ユニット２１は、各ユニットを駆動するための直流電力や、ＩＯ用の直流電力を発生する。この電源ユニット２１で発生した電力は、バックプレーン２５を介して各ユニットに配分される。

主演算ユニット２２は、シーケンスプログラムを実行する。
ＩＯユニット２３は、制御対象の装置などに設置されたセンサなどからの検出信号を受け取るとともに、制御対象の装置に設置されたアクチュエータや表示装置などに対して制御指令を出力する。

ＭＣユニット２４は、主演算ユニット２２からの指令に従って、モーションプログラムを実行し、その結果算出された制御指令をモータドライバ３１，３２，３３へ出力する。ＭＣユニット２４は、時間の経過に従ってモータドライバ３１，３２，３３へ一連の制御指令を出力する。ＭＣユニット２４は、モーションプログラムの実行状況および実行結果を主演算ユニット２２へ報告する。

なお、図２には、主演算ユニット２２とは独立したＭＣユニット２４を設けた構成を例示するが、主演算ユニット２２がＭＣユニット２４の機能を兼ね備えることで、専用のＭＣユニット２４を省略する構成を採用することもできる。

図３を参照して、ＰＬＣ２０のハードウェア構成について説明する。主演算ユニット２２内には、マイクロプロセッサ（ＭＰＵ：Micro-Processing Unit）２２１と、システムプログラムメモリ（ＳＲＯＭ：System-program Read Only Memory）２２２と、バッファメモリ（ＢＲＯＭ：Buffer Read Only Memory）２２３と、ワークメモリ（ＷＲＡＭ：Working Random Access Memory）２２４と、通信インターフェイス（通信Ｉ／Ｆ）２２５と、ユーザメモリ（ＵＭ：User Memory）２２６と、ＩＯメモリ（ＩＯＭ：Input Output Memory）２２８とを含む。

マイクロプロセッサ（ＭＰＵ）２２１は、ユニット全体を統括制御する。より具体的には、マイクロプロセッサ２２１は、ユーザメモリ２２６から順次ユーザプログラムを構成する各命令語を読み出しては実行する。システムプログラムメモリ（ＳＲＯＭ）２２２は、マイクロプロセッサ２２１で実行されるプログラムを格納する。バッファメモリ（ＢＲＯＭ）２２３は、ユーザプログラムを格納する。ワークメモリ（ＷＲＡＭ）２２４は、マイクロプロセッサ２２１がシステムプログラムを実行する際のワークエリアとして使用される。通信インターフェイス２２５は、後述するように、ＰＣ（パーソナルコンピュータ）１０とのデータ通信を行なう。ユーザメモリ（ＵＭ：User Memory）２２６は、電源投入と共にバッファメモリ２２３から転送されるユーザプログラムを格納する。ＩＯメモリ（ＩＯＭ）２２８は、ＩＯユニット２３やＭＣユニット２４の入出力状態を反映するように保持する。

なお、マイクロプロセッサ（ＭＰＵ）に代えて、あるいは、それに加えて、特定用途向集積回路（ＡＳＩＣ：Application Specific Integrated Circuit）を採用してもよい。

ＩＯユニット２３内には、処理回路（ＭＰＵ）２３１と、入力回路２３２と、出力回路２３３とを含む。入力回路２３２は、可動機構４に設けられた各種センサなどからの検出信号を受け取って、その状態値を主演算ユニット２２へ出力する。典型的には、入力回路２３２は、フォトカプラ回路などで構成される。出力回路２３３は、制御対象の装置などに設置されたアクチュエータや表示装置などに対して制御指令を出力する。処理回路２３１は、入力回路２３２および出力回路２３３との間で内部的な指令を遣り取りする。

ＭＣユニット２４内には、処理回路（ＭＰＵ）２４１と、コントローラインターフェイス（コントローラＩ／Ｆ）２４２と、入力回路２４３と、出力回路２４４とを含む。コントローラインターフェイス２４２は、モータドライバ３１，３２，３３（図２参照）との間のデータ伝送を管理する。入力回路２４３および出力回路２４４は、それぞれ入力回路２３２および出力回路２３３と同様である。但し、入力回路２４３および出力回路２４４は、主として、モーション制御（モータドライバ３１，３２，３３およびモータ４１，４２，４３）に関する情報を遣り取りする。

再度、図２を参照して、モータドライバ３１，３２，３３は、ＭＣユニット２４からの制御指令を受けて、対応するモータ４１，４２，４３を駆動する。すなわち、モータドライバ３１，３２，３３は、モータ４１，４２，４３に駆動電力を供給する。

モータ４１，４２，４３は、可動機構４（図１参照）の構成要素である。可動機構４は、これらのモータが回転駆動することによって、そのアーム部分を移動させる。なお、モータ４１，４２，４３としては、サーボモータやステッピングモータなどが採用される。この場合、採用するモータの種類に応じたモータドライバが用いられる。

図２には、モータドライバ３１，３２，３３は、イーサキャット（EtherCAT）（登録商標）などの通信線６０を介してＭＣユニット２４と接続される構成例を示す。但し、ＭＣユニット２４とモータドライバ３１，３２，３３とを直接的に接続してもよい。

ＭＣユニット２４からモータドライバ３１，３２，３３へ与えられる制御指令は、モータの位置、速度、トルクなどの値である。本実施の形態においては、モータドライバ３１，３２，３３の各々に対する位置指令（目標位置）がＭＣユニット２４から一定周期で与えられるものとする。この目標位置としては、モータ４１，４２，４３の回転位置（回転角度）である場合であってもよいし、モータ４１，４２，４３によって駆動される可動機構４の特定部位の位置であってもよい。すなわち、いずれの座標を基準として目標位置を定めるかの問題であって、状況に応じて、適切な基準が採用される。

なお、本実施の形態においては、目標位置として、いわゆる「溜りパルス」が用いられる。すなわち、目標位置が「制御周期内に送られるパルスの数」として表現される。この場合、パルス数は、直前の制御周期における位置からの相対位置（位置の偏差量／変化量）を示す。

モータドライバ３１，３２，３３は、位置指令（目標位置）を解釈して、モータ４１，４２，４３を駆動するために必要な設定情報（たとえば、位置指令を電流値に変換するための係数など）を有している。たとえば、モータドライバ３１，３２，３３には、モータ４１，４２，４３の１回転に対応するパルス数の情報が予め設定される。

モータドライバ３１，３２，３３は、モータ４１，４２，４３に設けられたサーボモータの回転位置を検出するエンコーダから、現在位置（実測値）のフィードバックを受ける。モータドライバ３１，３２，３３は、実測値および実測値の変化分として算出される実測速度を用いて、実測値が指令値に追従するように駆動電力の大きさを制御する（フィードバック制御）。

また、ＭＣユニット２４には、モータドライバ３１，３２，３３と同様にモータ４１，４２，４３の１回転に対応するパルス数の情報が予め設定されるほか、モータ４１，４２，４３の１回転に対応する可動機構４の特定部位の位置についての移動量の情報が予め設定される。これらの設定値を用いて、ＭＣユニット２４は、制御周期ごとにプログラムされた制御対象の運動を実現するのに必要な数のパルスを算出してモータドライバ３１，３２，３３に送る。

一方、ＰＣ１０は、汎用的なコンピュータをベースとして構成され、ＵＳＢ（Universal Serial Bus）などの通信線を介して主演算ユニット２２に接続される。ＰＣ１０からは、本実施の形態に従う制御ロジックを実現するためのプログラムが主演算ユニット２２へ転送されるとともに、ＰＣ１０のディスプレイ上では、ＰＬＣ２０の動作状況やログなどを確認することができる。なお、ＰＣ１０の構成について、公知であるので、ここでは詳細な説明は行なわない。

なお、図２には、ＰＬＣベースの制御装置１を図示したが、この構成に限られることはない。たとえば、汎用的なコンピュータベースの処理装置と、モータドライバ３１，３２，３３やセンサなどとの間で信号を遣り取りする入出力装置とを組み合わせて、制御装置１を構成してもよい。

このような構成を採用した場合には、本実施の形態に従う制御ロジックを実現するために、ＯＳ（Operating System）が提供する汎用モジュールを利用する場合もある。すなわち、本実施の形態に従う制御ロジックを提供するためのプログラムとして、一部の共通的な機能を有さないような形態も存在するが、そのような形態であっても、本発明の範囲に含まれることは明らかである。

さらに、制御装置１が提供する機能を、ソフトウェアと専用ハードウェア回路との組み合わせとして具現化してもよい。

＜Ｂ．概要＞
本発明の実施の形態に従う制御ロジックでは、予め設定された目標軌道の終点におけるＴＣＰ速度が最終目標速度と一致するように、現在速度をなるべく変動の少ない加速度で加減速させる。すなわち、ＴＣＰ速度が最終目標速度に向けて滑らかに変化するとともに、後続の軌道における目標速度との連続性も維持できる。これにより、仕上げ面の均一性を向上させる。

このような現在速度を最終目標速度へ変化させるための目標加速度は、各制御周期において、対象の目標軌道上における現在位置から終点までの残距離を推定するとともに、当該推定した残距離に基づいて最適な値が算出される。このように、各制御周期において、目標軌道における終点を考慮しながら、目標加速度が逐次算出されるので、目標軌道の終点において速度や加速度が急激に増加／減少するといった事態を解消することができる。

本実施の形態に従う制御ロジックにおいては、現在速度および目標速度を線形補間または多項式補間した速度軌道に基づいて算出する。特に、区間を分割するためのサンプル数を残距離にかかわらず一定数にするので、計算時間が確定的となり、オンライン計算方式により適した構成となる。これにより、目標軌道としてベジェ曲線といった自由関数を用いた場合であっても、上述のような残距離の推定における処理時間を１制御周期内で完了することを保証できる。

本実施の形態に従う制御ロジックにおいては、ＴＣＰ速度の変更に係る、加速度および加加速度の変化量がそれぞれ予め定められた制限値を超過しないように制約される。そのため、可動機構４における「びびり」やトルクオーバーの発生を抑制できる。

＜Ｃ．全体処理手順＞
次に、本実施の形態に従う制御ロジックの概略について説明する。

図４は、本実施の形態に従う制御ロジックの処理手順の概略を示すフローチャートである。なお、図４に示す各ステップは制御装置１によって実行されるが、その実行主体は実装形態によって異なる。すなわち。主演算ユニット２２（図２参照）がすべての処理を行なう場合もあるし、主演算ユニット２２とＭＣユニット２４（図２参照）とが共同して処理を行なう場合もある。

図４を参照して、まず、システムＳＹＳでの制御が開始される。このとき、可動機構４によるワーク２の切削が開始される（図１参照）（ステップＳ２）。続いて、目標軌道の生成タイミングであるか否かが判断される（ステップＳ４）。すなわち、可動機構４がワーク２の所定の位置まで到達した場合に当該位置に応じた目標軌道が生成されることになるが、この目標軌道を生成すべきか否かが判断される。より具体的には、図１に示すようなスイッチ５から信号（検出信号）が外部入力されたか否かが判断される。目標軌道の生成タイミングでなければ（ステップＳ４においてＮＯ）、ステップＳ４の処理が繰返される。

これに対して、目標軌道の生成タイミングであれば（ステップＳ４においてＹＥＳ）、始点および終点により定義される移動区間に対して目標軌道が生成される（ステップＳ６）。すなわち、外部入力される信号に応答して目標軌道が生成される。なお、始点および終点は、予め設定されている情報に基づいて決定される。言い換えれば、ワーク２の現実の位置に関連付けて始点および終点が設定されてもよいし、ワーク２の現実の位置とは独立して始点および終点が設定されてもよい。そして、以下に示すような、可動機構４を目標軌道に沿って移動させるための処理が開始される。

まず、目標軌道における現在位置から終点までの残距離が推定される（ステップＳ８）。すなわち、可動機構４のＴＣＰの現在値（現在位置を示す情報）が取得されるとともに、決定された目標軌道（を示す関数）を用いて、目標軌道の終点までの距離が推定される。

続いて、可動機構４の現在のＴＣＰ速度（可動部の移動速度）を目標軌道の終点において可動機構４の可動部が満たすべき移動速度である最終目標速度まで変更した場合に、可動機構４の可動部が移動する距離がステップＳ８において推定された残距離と実質的に一致するように、当該目標加速度が算出される（ステップＳ１０）。さらに、現在のＴＣＰ速度をステップＳ１０において算出した目標加速度で補正することでＴＣＰ速度の新たな目標値が算出される（ステップＳ１２）。

ここで、ＴＣＰが設定した目標軌道の終点に到達しているか否かが判断される（ステップＳ１４）。すなわち、目標軌道に沿ったモーション制御が終了したか否かが判断される。ＴＣＰが設定した目標軌道の終点に到達していれば（ステップＳ１４においてＹＥＳ）、今回の目標軌道に基づくモーション制御は終了する。なお、同一のワーク２に対して複数の目標軌道を設定するようにプログラムされている場合には、ステップＳ２以下の処理が再度実行される。

これに対して、ＴＣＰが設定した目標軌道の終点に到達していなければ（ステップＳ１４においてＮＯ）、ステップＳ１２において算出されたＴＣＰ速度の新たな目標値となるように、可動機構４の可動部が１制御周期内に移動すべき量を示す指令が出力される（ステップＳ１６）。すなわち、次の制御周期の開始タイミングにおいて、ＴＣＰが新たな目標値に対応した位置に移動しているように、所定数のパルス（溜りパルス）がそれぞれのモータドライバ３１，３２，３３へ与えられる。そして、次の制御周期まで待機し（ステップＳ１８）、ステップＳ８以下の処理が繰返される。

以下、図４に示す主要ステップの内容について詳述する。
＜Ｄ．目標軌道の生成（ステップＳ６）＞
（ｄ１：従来技術における課題など）
まず、本実施の形態に従う目標軌道の生成処理について説明する前に、従来技術における課題などについて説明する。

図５は、ワーク２への切削加工処理の一例を示す図である。図５に示すように、たとえば、一辺が同様の形状に切削される複数種類の製品（品種Ａおよび品種Ｂ）を製作する場合を考える。図５に示す品種Ａと品種Ｂとの間では、切削の形状は同一であるが、その切削を開始する位置が異なっている。この場合、ワーク２は長手方向に搬送されるのであるから、この切削の開始タイミングを異ならせるだけで、品種Ａおよび品種Ｂを製作することができる。この場合、切削される部分について見れば、同じ軌道で切削すればよいことになる。

このようなアプリケーションにおいては、各品種について加工図面や加工プログラムを作成することなく、共通の加工プログラムを用意するとともに、ワーク２とロボットとの間の相対位置関係を検出して、目標軌道を動的に（すなわち、オンラインで）設定したいというニーズがある。このような加工プログラムを共用することにより、生産コストを低減できる。

まず、このようなオンライン計算方式を利用する場合には、制御周期（サンプリング間隔）内に新たな目標軌道の生成処理を完了して、それまで使用していた目標軌道を変更する必要がある。すなわち、新たな目標軌道の設定が一制御周期内に完了することを保証する。そのため、目標軌道としては、複雑なもの、あるいは、対象の形状に依存して処理量が変化するものを採用することは現実的ではない。

そこで、従来からある一つのアプローチとしては、円弧補間を用いる方法が存在していた。

図６は、ワーク２をＬ字状に切削する場合に円弧補間を用いて目標軌道を生成する処理例を示す図である。

図６（ａ）には、円弧補間が適切に設定された例を示す。一方、図６（ｂ）および図６（ｃ）には、円弧補間が不適切に設定された例を示す。

図６（ｂ）は、ワーク２の切削形状を維持するために、曲率が不適切に設定された結果、軌道上での方向ベクトルが不連続になっている例を示す。図６（ｃ）は、軌道上での方向ベクトルの連続性を維持するために、一定の曲率を有する円弧を用いて無理に方向ベクトルを連続化し、その結果、目的の切削形状を実現できずにワーク２の加工面への食い込みが発生した例を示す。

また、始点の速度ベクトルおよび終点の速度ベクトルをそれぞれ連続にしたいというニーズも存在する。速度ベクトルが不連続になると、面のつなぎ目が顕在化したり、削り量の変化が発生したりしてしまい、加工後の仕上がり（外観）の品質が低下してしまうからである。しかしながら、円弧補間を用いた場合には、始点ベクトルと終点ベクトルとが同一面内にはない場合に、軌道上での速度ベクトルが不連続になるという本質的な課題もある。

また、円弧補間においては、面取りのカーブの曲率設計が必要となるが、目標形状に含まれるすべてのコーナーについて、この作業をすることはコスト的および時間的に難しい。そのため、円弧の大まかな曲率半径だけを設定しておき、この予め設定した曲率半径では対処できないコーナーについては、目標軌道の再設計を自動化したいというニーズも存在する。しかしながら、従来の円弧補間では、このようなニーズに対応することができなかった。

従来からある別のアプローチとしては、自由曲線を用いる方法が存在していた。
図７は、ワーク２のコーナーを切削する場合に自由曲線を用いて目標軌道を生成する処理例を示す図である。図７（ａ１）は、従来技術に従って生成された目標軌道に沿って移動する可動機構４のＴＣＰ速度の例を示し、図７（ａ２）は、従来技術に従って生成された目標軌道の例を示す。

図７（ａ１）に示すように、終点の速度ベクトルが不連続になると、端面の切削量が急激に変化して仕上がり（外観）にムラが生じる。すなわち、終点において可動機構４が急加速して切削面の荒れや膨らみが生じ得る。これにより、外観を損なうほか、隙間があくなどの機能上の問題が発生する。また、可動機構４の急加速によって、方向ベクトルも不連続化して、その切削面に不連続な線が生じてしまう。

図７（ｂ１）は、本実施の形態に従う制御ロジックによって制御される可動機構４のＴＣＰ速度の例を示し、図７（ｂ２）は、本実施の形態に従う制御ロジックによって生成された目標軌道の例を示す。

本実施の形態においては、図７（ｂ１）に示すように、後続の軌道速度との連続性を保つために、目標軌道の終点におけるＴＣＰ速度が所定の最終目標速度と一致するように制御する。同時に、図７（ｂ１）に示すように、目標軌道上における速度ベクトルの連続性を保つように制御する。これにより、図７（ｂ２）に示すように、所望の目標軌道を生成することができる。

本実施の形態に従う制御ロジックでは、主として、自由曲線を用いて目標軌道を生成する。

しかしながら、上述したような円弧補間では、目標軌道上のＴＣＰ速度を目標速度に一致させることができるが、それ以外の自由曲線による補間では、目標軌道上のＴＣＰ速度（特に終点速度）を目標速度に一致させることができていない。また、自由曲線を用いて目標軌道を生成する場合の課題としては、制御周期内での演算処理の完了が保証されない可能性がある。すなわち、目標軌道の終点における速度ベクトルを目標値に維持するためには、オンラインで残距離（残軌道長）を算出することが必要となる。しかしながら、自由曲線（円弧補間を除く）では、目標軌道の軌道長を初等関数で表現することができないため、目標軌道の軌道長さを短時間で算出することは難しい。また、数値積分を用いれば残距離を計算することもできるが、正確な残距離を算出するために、曲率に応じて分割数を変更しつつ積分処理を行なうと、一制御周期内での残距離の算出処理の完了を保証できない。

（ｄ２：本実施の形態における方法）
上述のような課題を考慮して、本実施の形態に従う制御ロジックでは、目標軌道の始点および終点におけるそれぞれの方向ベクトルの連続性を維持するために自由曲線を用いて、目標軌道を設定する。このような目標軌道は、媒介変数を含む関数の形で定義する。

具体的には、ベジェ曲線、βスプライン曲線、ＮＵＲＢＵＳ、エルミート曲線、カットマル−ロムスプライン曲線、Ferguson/Coons曲線といった補間曲線を用いることができる。以下に説明する制御ロジックでは、これらの補間曲線のうち「ベジェ曲線」を採用した場合を例に説明する。

このような補間曲線（ベジェ曲線）を採用することで、図６（ｃ）に示すような、目標軌道のワークへの食い込みを防止することができる。

図８は、本実施の形態に従う目標軌道（ベジェ曲線）の生成方法について説明するための図である。図８を参照して、ベジェ曲線を用いて目標軌道を生成する場合には、少なくとも、始点および終点を含む移動区間が定義される必要がある。

このとき、目標軌道を設定するための基準となる第１軌道および当該第１軌道に連なる軌道である第２軌道が定義される。目標軌道の始点は、第１軌道上に位置し、目標軌道の終点は、第２軌道上に位置することになる。

第１および第２軌道は任意に設定できるが、図１に示すようなシステムＳＹＳでは、ワークの加工面が第１および第２軌道として定義される。始点Ｐ１（ｘ１，ｘ２，ｘ３）は、スイッチ５（図１参照）などでワーク２が所定位置に到達したことに応答して決定されてもよい。ベジェ曲線を設定するためには、２つのコントロールベクトルが必要であるため、始点Ｐ１の決定に伴って、コントロール点Ｐ２およびＰ３が決定されるとともに、これらの点によってコントロールベクトル１および２も決定される。

なお、図１に示すような切削への適用例においては、コントロール点Ｐ２（ｘ２，ｙ２，ｚ２）およびＰ３（ｘ３，ｙ３，ｚ３）は予め定められていることが一般的である。そのため、始点Ｐ１が決定されると、この始点Ｐ１とコントロール点Ｐ２とを結ぶ線分として、コントロールベクトル１が決定される。また、始点Ｐ１とコントロール点Ｐ２との位置関係などに対応して、終点Ｐ４（ｘ４，ｙ４，ｚ４）が決定される。すると、コントロール点Ｐ３と終点Ｐ４とを結ぶ線分として、コントロールベクトル２が決定される。

このようにコントロールベクトル１および２が決定されることで、ベジェ曲線、すなわち、目標軌道が決定される。

より具体的には、媒介変数ｒ（０≦ｒ≦１）を用いて、目標軌道上の目標位置Ｐ（ｒ）は、以下のように表すことができる。なお、一般的には、目標位置Ｐ（ｒ）は、３次元座標上の値として定義される。

Ｐ（ｒ）＝（１−ｒ）^３×Ｐ１＋３（１−ｒ）^２×ｒ×Ｐ２＋３（１−ｒ）×ｒ^２×Ｐ３＋ｒ^３×Ｐ４
なお、ベジェ曲線以外の補間曲線を用いる場合には、第１および第２軌道とは独立した位置にコントロールベクトルを設定してもよい。また、第１軌道と第２軌道とは必ずしも直交する必要はない。

さらに、始点Ｐ１、コントロール点Ｐ２おおびＰ３、終点Ｐ４についても任意に設定することができる。すなわち、これらの点をセンサによる検出に応じて定めてもよいし、予め固定された値を設定しておいてもよい。

このような、ベジエ曲線を用いて目標軌道を設定することで、図８に示すように、２つのコントロールベクトルの間に目標軌道が生成されるので、木工加工などの加工面への食い込みが許されないアプリケーションでの目標軌道の生成に適している。また、目標軌道の始点および終点がコントロールベクトルの始点および終点にそれぞれ一致するので、コントロールベクトルを容易に生成できる。

＜Ｅ．残距離の推定（ステップＳ８）＞
次に、目標加速度を算出するための基準となる、目標軌道における現在位置から終点までの残距離の推定処理について説明する。

後述するように、ベジェ曲線は、媒介変数により定義される高次の関数となり、媒介変数と軌道長との関係を初等関数で表現することができない。そのため、目標軌道上のある位置（現在位置）から終点までの残距離を正確に算出しようとすると、数値積分を用いる必要がある。一方で、数値積分をするための分割点を多く設定してしまうと、演算処理が増大して、算出処理が一制御周期内で完了しない可能性がある。

そこで、本実施の形態に従う制御ロジックにおいては、目標軌道における現在位置から終点までの区間に対して予め定められた一定数のサンプル点を設定するとともに、設定したサンプル点を線形補間または多項式補間することで残距離を推定する。

図９は、本実施の形態に従う残距離の推定方法について説明するための図である。図９（ａ）は、現在位置が目標軌道の始点に近い場合（時刻ｔ１）を示し、図９（ｂ）は、現在位置が目標軌道の終点に近い場合（時刻ｔ２）を示す。

図９（ａ）および図９（ｂ）に示すように、残距離ｌＬｅｆｔ（ｔ）は、隣接するサンプル点Ｐ１，Ｐ２，…，Ｐｎ−１の間の距離Ｌ１，Ｌ２，…，Ｌｎの合計ΣＬｉとして算出される。なお、設定するサンプル点の数は、制御装置１の処理能力などに依存して決定すればよいが、たとえば、４〜６点が好ましい。

図９（ａ）と図９（ｂ）とを比較してわかるように、目標軌道上の現在位置が始点に近い場合であっても、終点に近い場合であっても、目標軌道上に設定するサンプル点は一定数に維持される。そのため、目標軌道上の現在位置が始点に近い場合には、サンプル点の間の距離が相対的に大きくなり、推定精度が低下するが、逆に、目標軌道上の現在位置が終点に近い場合には、サンプル点の間の距離が相対的に小さくなり、推定精度が向上する。すなわち、目標軌道の曲率が大きくなる程、残距離についての推定誤差は大きくなるが、終点に近付く程にその誤差は解消する。

後述するように、この推定の残距離ｌＬｅｆｔ（ｔ）は、目標加速度（すなわち、目標軌道上のＴＣＰ速度の速度変化分）を決定することになるが、目標軌道上の現在位置が終点に近くなる程、その算出される目標加速度の精度が高くなる。その結果、終点に近付く程、ＴＣＰ速度が終点での最終目標速度に一致することになる。

好ましくは、図９（ａ）および図９（ｂ）に示すように、サンプル点は、目標軌道における現在位置から終点までの区間について、現在位置に遠い区間に比較して現在位置に近い区間ほど、サンプル点の間隔がより密になるように設定される。

上述したように、目標軌道の始点は媒介変数ｒ＝０に相当し、目標軌道の終点は媒介変数ｒ＝１に相当する。すなわち、媒介変数ｒを０から１まで増加させることで、目標軌道上の現在位置が始点から終点まで移動する。そこで、各現在位置を示す媒介変数ｒの値と目標軌道の終点に相当する媒介変数ｒ＝１との差を算出し、その差についての相対比率が所定値となる位置にポイントをそれぞれ設定する。

たとえば、現在位置と終点との間に４つのサンプル点を設定する場合には、媒介変数ｒの差に対して、２／１２，４／１２，６／１２，９／１２に相当する媒介変数ｒの位置にそれぞれサンプル点を設定することが考えられる。具体的には、現在位置が媒介変数ｒ＝「０．４」に相当する位置であれば、終点における媒介変数ｒ＝「１．０」との差である「０．６」に対して、上述のような比率でサンプル点が設定される。この場合には、媒介変数ｒ＝「０．５」，「０．６」，「０．７」，「０．８５」にそれぞれ対応する位置に、サンプル点が設定される。

代替の方法として、目標軌道における曲率が最大となる点を考慮して、サンプル点を設定してもよい。すなわち、目標軌道上において曲率が最大となる媒介変数ｒは、以下の式に従って算出される。

ｒ＝［√｛（Ｐ１−Ｐ２）×Ｐ４＋Ｐ３^２＋（−Ｐ２−Ｐ１）×Ｐ３＋Ｐ２^２｝−Ｐ３＋２×Ｐ２−Ｐ１］／（Ｐ４−３×Ｐ３＋３×Ｐ２−Ｐ１）
したがって、この曲率が最大となる媒介変数ｒの位置を基準として、所定数のサンプル点が設定されてもよい。

＜Ｆ．目標加速度の算出（ステップＳ１０）＞
次に、目標軌道上を移動中における目標加速度を算出する処理について説明する。

本実施の形態に従う制御ロジックにおいては、目標軌道の終点におけるＴＣＰ速度の目標値が与えられているので、各現在位置から当該ＴＣＰ速度の目標値への速度変化（加速度）が線形的となるように、目標加速度が算出される。

図１０は、本実施の形態に従う目標加速度の算出方法について説明するための図である。図１０を参照して、ある時点における目標軌道上のＴＣＰ速度を現在速度Ｖｅｌとし、目標軌道の終点におけるＴＣＰ速度の目標値を最終目標速度ｖＥｎｄとすると、時間−ＴＣＰ速度の座標において、現在速度Ｖｅｌと最終目標速度ｖＥｎｄとを直線的につなぐ加速度を目標加速度ＡｃｃＲｅｆとする。言い換えれば、各制御周期において、現在速度Ｖｅｌを最終目標速度ｖＥｎｄまで一定レートで変化させるように、目標加速度ＡｃｃＲｅｆが算出される。すなわち、目標加速度ＡｃｃＲｅｆは、以下の式に従って算出される。

ＡｃｃＲｅｆ＝（ｖＥｎｄ−Ｖｅｌ）／Ｔｎｅｃ
ここで、時間Ｔｎｅｃは、推定された残距離ｌＬｅｆｔだけ移動するために必要な時間であり、以下の式に従って算出される。

Ｔｎｅｃ＝２×ｌＬｅｆｔ／（Ｖｅｌ＋ｖＥｎｄ）
したがって、これらの２式を組み合わせると、目標加速度ＡｃｃＲｅｆは、以下の式に従って算出される。

ＡｃｃＲｅｆ＝（ｖＥｎｄ^２−Ｖｅｌ^２）／（２×ｌＬｅｆｔ）
すなわち、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、および推定された残距離ｌＬｅｆｔに基づいて目標加速度ＡｃｃＲｅｆが算出される。

＜Ｇ．ＴＣＰ速度の新たな目標値の算出（ステップＳ１２）＞
次に、各制御周期において、更新されるＴＣＰ速度の目標値（目標速度）を算出する処理について説明する。

すなわち、各制御周期において算出された目標加速度ＡｃｃＲｅｆで、現在のＴＣＰ速度（現在速度Ｖｅｌ）を補正することで、次の制御周期におけるＴＣＰ速度の目標値（目標速度ＶｅｌＲｅｆ）が算出される。すなわち、目標速度ＶｅｌＲｅｆは、以下の式に従って算出される。

ＶｅｌＲｅｆ＝Ｖｅｌ＋ＡｃｃＲｅｆ
＜Ｈ．移動指令の出力（ステップＳ１６）＞
次に、目標速度ＶｅｌＲｅｆを実現するために必要な移動すべき量を示す指令を出力する処理について説明する。ここで、目標速度ＶｅｌＲｅｆは、可動機構４のアーム部分についての１制御周期内での軌道長の変化量ΔＬ（＝移動距離／制御周期）に相当する。

図１１は、本実施の形態に従うベジェ曲線およびその軌道長の特性を示す図である。図１１（ａ）に示すようなベジェ曲線を目標軌道に設定した場合には、その目標軌道上の位置が媒介変数ｒによって定義されることになる。一方で、図１１（ｂ）に示すように、目標軌道の軌道長Ｌは、媒介変数ｒに関して非線形の関係を有している。

図１２は、ベジェ曲線における媒介変数ｒの変位量と軌道長Ｌの変位量との関係を示す図である。図１２に示すように、ある制御周期内において、可動機構４のアーム部分を目標軌道に沿って変化量ΔＬだけ移動させた場合の媒介変数増分Δｒは、ｒの値に依存して変化することになる。すなわち、媒介変数ｒと軌道長Ｌとの関係は非線形であるので、媒介変数増分Δｒを一定値にしても、ＴＣＰ速度（現在速度Ｖｅｌ）は一定値にはならない。

したがって、各制御周期において、次の制御周期までの変化量ΔＬが算出されたとしても、目標軌道上のいずれの位置まで移動する必要があるかについては単純な計算では算出できないことになる。

そこで、本実施の形態に従う制御ロジックにおいては、媒介変数ｒの値に依存して、媒介変数増分Δｒを逐次算出することで、目標軌道上における移動速度を制御する。より具体的には、以下に詳述するように、軌道長Ｌを媒介変数ｒについて偏微分して得られた値を用いて、非線形の関係を簡易的に線形化する。

図１３は、本実施の形態に従う制御ロジックにおける軌道長の変化量ΔＬに対する媒介変数増分Δｒの算出方法について説明するための図である。

基本的には、図１３（ａ）に示すように、ある媒介変数ｒにおける、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を算出しておく。このとき、媒介変数ｒと軌道長Ｌとの関係が線形である（破線で示す特性）とすると、目標速度ＶｅｌＲｅｆに対応する軌道長の変化量ΔＬは、媒介変数増分Δｒ＿ｌｉｎｅａｒに相当する。

一方、媒介変数ｒと軌道長Ｌとの本来の関係（非線形の関係）によれば、媒介変数増分Δｒ＿ｌｉｎｅａｒに対応する軌道長の変化量は、目標速度ＶｅｌＲｅｆと媒介変数増分Δｒ＿ｌｉｎｅａｒで算出した時の移動長の行き過ぎ量との合計に相当する。

したがって、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を用いて、目標速度ＶｅｌＲｅｆを実現するために必要な媒介変数増分Δｒは、媒介変数増分Δｒ＿ｌｉｎｅａｒに軌道長Ｌの傾きａの逆数を乗じた値として算出することができる。

すなわち、媒介変数増分Δｒ＝媒介変数増分Δｒ＿ｌｉｎｅａｒ×１／ａとして算出される。

なお、軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）は、軌道長Ｌについての媒介変数増分Δｒについての導関数（１次微分式）を予め保持しておき、この導関数を逐次算出することで得られる。

さらに、上述のような算出処理によって得られた媒介変数増分Δｒに基づいて、次の制御周期における目標軌道上の目標位置を示す媒介変数ｒが更新される（ｒ（ｔ＋１）＝ｒ（ｔ）＋Δｒ（ｔ））。可動機構４のアーム部分がこの更新後の媒介変数ｒによって示される目標軌道上の位置（目標位置Ｐ（ｒ（ｔ＋１）））に移動するように位置指令（目標位置）が与えられる。すなわち、新たに算出された位置に可動機構４のアーム部分を移動させるための溜りパルスがＭＣユニット２４からモータドライバ３１，３２，３３へ与えられる。

以上のように、可動機構４に与えられる移動指令は、移動速度の新たな目標値（目標速度ＶｅｌＲｅｆ）に対応して、可動機構４のアーム部分が移動すべき距離（軌道長の変化量ΔＬ）を算出し、可動機構４のアーム部分が移動すべき距離（軌道長の変化量ΔＬ）に対応して、目標軌道における可動機構４のアーム部分が移動すべき目標軌道上の位置（目標位置Ｐ（ｒ））を算出し、可動機構４のアーム部分の移動すべき位置に対応して、複数の駆動軸の各々に対する移動指令（溜りパルス）として算出される。

＜Ｉ．機能ブロック図＞
次に、上述の一連の処理を実現するための機能ブロックについて説明する。

図１４は、本実施の形態に従う制御ロジックを実現するための機能ブロックを示す模式図である。

図１４を参照して、本実施の形態に従う制御ロジックは、Ｊｅｒｋ処理モジュール３０１と、加速度処理モジュール３０２と、目標速度算出モジュール３０３と、遅延モジュール３０４と、媒介変数増分算出モジュール３０５と、傾き算出モジュール３０６と、媒介変数算出モジュール３０７と、ベジェ曲線算出モジュール３０８と、溜りパルス算出モジュール３０９とを含む。

Ｊｅｒｋ処理モジュール３０１は、モータ４１，４２，４３（図２）に与える制御指令により生じる加加速度（「Ｊｅｒｋ」あるいは「躍度」とも称される。）が予め定められた制限値（Ｊｅｒｋ制限値ｊＭａｘＬｉｍ）を超過しないように制限する。すなわち、Ｊｅｒｋ処理モジュール３０１は、目標加速度ＡｃｃＲｅｆを算出するとともに、この算出した目標加速度ＡｃｃＲｅｆと現在加速度Ａｃｃとを比較することで、加加速度の大きさを評価する。

より具体的には、Ｊｅｒｋ処理モジュール３０１には、現在加速度Ａｃｃ、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、（推定された）残距離ｌＬｅｆｔが入力される。そして、Ｊｅｒｋ処理モジュール３０１は、目標加速度ＡｃｃＲｅｆをＡｃｃＲｅｆ＝（ｖＥｎｄ^２−Ｖｅｌ^２）／（２×ｌＬｅｆｔ）に従って算出する。続いて、この算出した目標加速度ＡｃｃＲｅｆに対応する目標加加速度ＪｅｒｋＲｅｆをＪｅｒｋＲｅｆ＝ＡｃｃＲｅｆ−Ａｃｃに従って算出する。

続いて、Ｊｅｒｋ処理モジュール３０１は、算出された目標加加速度ＪｅｒｋＲｅｆが予め定められたＪｅｒｋ制限値ｊＭａｘＬｉｍを超えているか否かを判断する。そして、Ｊｅｒｋ処理モジュール３０１は、算出された目標加加速度ＪｅｒｋＲｅｆがＪｅｒｋ制限値ｊＭａｘＬｉｍを超えている場合には、目標加加速度ＪｅｒｋＲｅｆとしてＪｅｒｋ制限値ｊＭａｘＬｉｍの値を出力し、そうでなければ、算出された目標加加速度ＪｅｒｋＲｅｆの値をそのまま出力する。

加速度処理モジュール３０２は、モータ４１，４２，４３（図２）に与える制御指令により生じる加速度が予め定められた制限値（加速度制限値ａＭａｘＬｉｍ）を超過しないように制限する。すなわち、加速度処理モジュール３０２は、目標加速度ＡｃｃＲｅｆを算出するとともに、この算出した目標加速度ＡｃｃＲｅｆの大きさを評価する。

より具体的には、加速度処理モジュール３０２には、現在加速度Ａｃｃおよび目標加加速度ＪｅｒｋＲｅｆが入力される。そして、加速度処理モジュール３０２は、目標加速度ＡｃｃＲｅｆをＡｃｃＲｅｆ＝Ａｃｃ＋ＪｅｒｋＲｅｆに従って算出する。続いて、加速度処理モジュール３０２は、算出された目標加速度ＡｃｃＲｅｆが予め定められた加速度制限値ａＭａｘＬｉｍを超えているか否かを判断する。この算出した目標加速度ＡｃｃＲｅｆが加速度制限値ａＭａｘＬｉｍを超えている場合には、目標加速度ＡｃｃＲｅｆとして加速度制限値ａＭａｘＬｉｍの値を出力し、そうでなければ、算出された加速度制限値ａＭａｘＬｉｍの値をそのまま出力する。

目標速度算出モジュール３０３は、目標速度ＶｅｌＲｅｆを算出する。より具体的には、目標速度算出モジュール３０３には、遅延モジュール３０４を通じて、直前の制御周期における目標速度ＶｅｌＲｅｆが入力されるとともに、加速度処理モジュール３０２で算出された目標加速度ＡｃｃＲｅｆが入力される。そして、目標速度算出モジュール３０３は、今回の制御周期における目標速度ＶｅｌＲｅｆ（ｔ）をＶｅｌＲｅｆ（ｔ）＝ＶｅｌＲｅｆ（ｔ−１）＋ＡｃｃＲｅｆに従って算出する。

なお、目標速度算出モジュール３０３では、算出された目標速度ＶｅｌＲｅｆに対する制限処理を行なう必要がない。なぜならば、目標速度ＶｅｌＲｅｆの制限値は、最終目標速度ｖＥｎｄであり、この最終目標速度ｖＥｎｄを用いて目標加加速度ＪｅｒｋＲｅｆおよび目標加速度ＡｃｃＲｅｆが算出されているからである。

媒介変数増分算出モジュール３０５は、目標速度算出モジュール３０３により算出された目標速度ＶｅｌＲｅｆ（軌道長の変化量ΔＬ）に対応する媒介変数増分Δｒを算出する。この際、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）が必要となるので、媒介変数増分算出モジュール３０５により処理に同期して、傾き算出モジュール３０６が当該時点の媒介変数ｒに応じた、軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を算出する。媒介変数増分算出モジュール３０５における詳細な処理については、図１３を参照して説明したので、ここではその内容については繰返さない。

傾き算出モジュール３０６は、典型的には、予め設定されているベジェ曲線である目標位置Ｐ（ｒ）に対応する軌道長Ｌの関数から算出される、媒介変数増分Δｒについての導関数（１次微分式）を予め保持している。

媒介変数算出モジュール３０７には、媒介変数増分算出モジュール３０５において算出された媒介変数増分Δｒが入力される。そして、媒介変数算出モジュール３０７は、今回の制御周期における媒介変数ｒ（ｔ）をｒ（ｔ）＝ｒ（ｔ−１）＋ＡｃｃＲｅｆに従って算出する。

ベジェ曲線算出モジュール３０８は、媒介変数算出モジュール３０７において算出された媒介変数ｒに基づいて、目標位置Ｐ（ｒ）を算出する。すなわち、算出された目標位置Ｐ（ｒ）は、可動機構４のアーム部分が移動すべき座標を示す。

溜りパルス算出モジュール３０９は、ベジェ曲線算出モジュール３０８により算出された目標位置Ｐ（ｒ）に基づいて、モータ４１，４２，４３がそれぞれ現制御周期内で移動すべき量を算出する。そして、溜りパルス算出モジュール３０９は、モータドライバ３１，３２，３３のそれぞれへ与える溜りパルスＰ_１，Ｐ_２，Ｐ_３の数を算出する。

＜Ｊ．動作例＞
次に、本実施の形態に従う制御ロジックによって得られる可動機構４の挙動の一例について説明する。

図１５は、本実施の形態に従う制御ロジックによって得られる可動機構４の挙動例を示すタイムチャートである。図１５（ａ）は、可動機構４のアーム部分の移動速度（ＴＣＰ速度）の時間的変化の例を示し、図１５（ｂ）は、可動機構４のアーム部分の加速度の時間的変化の例を示す。

なお、ワークを切削加工するような処理では、ワーク２に接近する速度に比較して切削時の速度が相対的に低く設定されている場合が多い。そのため、図１５（ａ）および図１５（ｂ）には、切削開始の前処理として、可動機構４のアーム部分のＴＣＰ速度が所定レート（加速度−Ｄｅｃ１）での減速を時刻ｔ１から開始する例を示す。その後、時刻ｔ２において、ＴＣＰ速度が補間開始速度ｖＳｔａｒｔまで低下したとする。この補間開始速度ｖＳｔａｒｔは、加工対象のワークの材質などに応じて定められてもよい。

そして、時刻ｔ２において目標軌道が生成されたとする。すなわち、時刻ｔ２における可動機構４のアーム部分の位置が始点Ｐ１として決定されるとともに、この始点Ｐ１に対応して終点Ｐ４が決定される。すると、始点Ｐ１とコントロール点Ｐ２とからコントロールベクトル１が決定されるとともに、コントロール点Ｐ３と終点Ｐ４とからコントロールベクトル２が決定される。最終的に、これらのコントロールベクトル１および２からベジェ曲線、すなわち目標軌道が決定される（図８参照）。

時刻ｔ２以降では、ＴＣＰ速度が終点Ｐ４において最終目標速度ｖＥｎｄとなるように、ＴＣＰ速度が徐々に変更される。なお、速度の連続性を維持するため、時刻ｔ２における補間開始加速度ａＳｔａｒｔは、直前の加速度（−Ｄｅｃ１）とほぼ同様の大きさに維持されている。

上述したように、目標加速度ＡｃｃＲｅｆは、各制御周期において推定される残距離ｌＬｅｆｔに基づいて算出される。但し、移動開始直後では、補間開始速度ｖＳｔａｒｔと最終目標速度ｖＥｎｄとの差が大きいため、加加速度についての制限（Ｊｅｒｋ制限値ｊＭａｘＬｉｍ）によってＴＣＰ速度の変化量が制約される。

しかしながら、時刻ｔ３以降では、ＴＣＰ速度（現在速度Ｖｅｌ）と最終目標速度ｖＥｎｄとの差が小さくなるため、各制御周期において算出される目標加速度ＡｃｃＲｅｆに従って、ＴＣＰ速度（現在速度Ｖｅｌ）が徐々に最終目標速度ｖＥｎｄに近付くことがわかる。そして、時刻ｔ５において、可動機構４のアーム部分が終点に到達すると、そのときのＴＣＰ速度（現在速度Ｖｅｌ）が最終目標速度ｖＥｎｄに一致していることがわかる。

なお、図１５（ｂ）に示すように、時刻ｔ３〜ｔ５の期間においては、目標加速度ＡｃｃＲｅｆが大きく変動することなく、徐々に予め定められた制限値（加速度制限値ａＭａｘＬｉｍ）に接近していることがわかる。

＜Ｋ．全体処理フロー＞
上述した本実施の形態に従う制御ロジックの全体処理手順としては、以下のようにまとめることができる。

図１６は、本実施の形態に従う制御ロジックの全体処理手順を示すフローチャートである。なお、図１６のフローチャートに含まれるステップのうち図４のフローチャートに含まれるステップと実質的に同じものについては、同一の参照番号を付与している。

また、図１６に示す各ステップは制御装置１によって実行されるが、その実行主体は実装形態によって異なる。すなわち。主演算ユニット２２（図２参照）がすべての処理を行なう場合もあるし、主演算ユニット２２とＭＣユニット２４（図２参照）が共同して処理を行なう場合もある。

図１６を参照して、制御装置１は、パラメータ初期値および処理設定値を受付ける（ステップＳ１）。このパラメータ初期値には、補間開始速度ｖＳｔａｒｔ、最終目標速度ｖＥｎｄ、補間開始加速度ａＳｔａｒｔ、加速度制限値ａＭａｘＬｉｍ、Ｊｅｒｋ制限値ｊＭａｘＬｉｍなどが含まれる。また、処理設定値には、目標軌道を生成するための加工プログラムや、コントロール点の設定条件などが含まれる。続いて、可動機構４によるワーク２の切削が開始される（ステップＳ２）。

続いて、目標軌道の生成タイミングであるか否かが判断される（ステップＳ４）。目標軌道の生成タイミングでなければ（ステップＳ４においてＮＯ）、ステップＳ４の処理が繰返される。これに対して、目標軌道の生成タイミングであれば（ステップＳ４においてＹＥＳ）、始点および終点により定義される移動区間に対して目標軌道が生成される（ステップＳ６）。

この目標軌道の生成においては、始点Ｐ１の位置情報の取得（ステップＳ６１）、始点Ｐ１に対応する終点Ｐ４の位置決定（ステップＳ６２）、始点Ｐ１とコントロール点Ｐ２とによるコントロールベクトル１、および、コントロール点Ｐ３と終点Ｐ４とによるコントロールベクトル２の決定（ステップＳ６３）、ならびに、コントロールベクトル１およびコントロールベクトル２に基づくベジェ曲線の決定（ステップＳ６４）という処理が含まれる。

続いて、目標軌道における現在位置から終点までの残距離が推定される（ステップＳ８）。この残距離の推定においては、可動機構４のＴＣＰの現在値（現在位置を示す情報）の取得（ステップＳ８１）、目標軌道における現在位置から終点までの区間に対して予め定められた一定数のサンプル点の設定（ステップＳ８２）、隣接するサンプル点の間の距離を算出（ステップＳ８３）、各サンプル点の間の距離を合計することで残距離の算出（ステップＳ８４）という処理が含まれる。

続いて、目標加速度が算出される（ステップＳ１０）。この目標加速度の算出においては、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、推定された残距離ｌＬｅｆｔを用いて目標加速度ＡｃｃＲｅｆの算出（ステップＳ１０１）、算出された目標加速度ＡｃｃＲｅｆを用いて目標加加速度ＪｅｒｋＲｅｆの算出（ステップＳ１０２）、算出された目標加加速度ＪｅｒｋＲｅｆの制限処理（ステップＳ１０３）、制限処理後の目標加加速度ＪｅｒｋＲｅｆを用いて目標加速度ＡｃｃＲｅｆの算出（ステップＳ１０４）、算出された目標加速度ＡｃｃＲｅｆの制限処理（ステップＳ１０５）という処理が含まれる。

続いて、現在のＴＣＰ速度（現在速度Ｖｅｌ）をステップＳ１０において算出した目標加速度（目標加速度ＡｃｃＲｅｆ）で補正することでＴＣＰ速度の新たな目標値（目標速度ＶｅｌＲｅｆ）が算出される（ステップＳ１２）。

ＴＣＰが設定した目標軌道の終点に到達しているか否かが判断される（ステップＳ１４）。ＴＣＰが設定した目標軌道の終点に到達していれば（ステップＳ１４においてＹＥＳ）、今回の目標軌道に基づくモーション制御は終了する。これに対して、ＴＣＰが設定した目標軌道の終点に到達していなければ（ステップＳ１４においてＮＯ）、可動機構４の可動部が１制御周期内に移動すべき量を示す指令が出力される（ステップＳ１６）。

この指令の出力処理においては、媒介変数ｒと軌道長Ｌとが線形であると仮定して、算出された目標速度ＶｅｌＲｅｆに相当する軌道長の変化量ΔＬに対応する媒介変数増分Δｒ＿ｌｉｎｅａｒの算出（図１３参照）（ステップＳ１６１）、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）の算出（ステップＳ１６２）、媒介変数増分Δｒ＿ｌｉｎｅａｒに軌道長Ｌの傾きａの逆数を乗じることで媒介変数増分Δｒの算出（ステップＳ１６３）、算出された媒介変数増分Δｒを用いて今回の制御周期における媒介変数ｒの算出（ステップＳ１６４）、算出された媒介変数ｒを用いた目標位置Ｐ（ｒ）の算出（ステップＳ１６５）、算出された目標位置Ｐ（ｒ）を用いた現制御周期内で移動すべき量の算出（ステップＳ１６６）、モータドライバ３１，３２，３３のそれぞれへ与えるべき溜りパルスＰ_１，Ｐ_２，Ｐ_３の数の算出（ステップＳ１６７）、算出された数の溜りパルスＰ_１，Ｐ_２，Ｐ_３のＭＣユニット２４からモータドライバ３１，３２，３３への出力（ステップＳ１６８）という処理が含まれる。

そして、次の制御周期まで待機し（ステップＳ１８）、ステップＳ８以下の処理が繰返される。

なお、ステップＳ１６の処理において、モータ４１，４２，４３に設けられたサーボモータの回転位置を検出するエンコーダから現実に生じた加速度を算出するとともに、目標加速度（目標加速度ＡｃｃＲｅｆ）との差を加速度誤差として評価し、この加速度誤差を次の制御周期における処理に反映してもよい。これにより、過負荷などでモータが指令に追従しない場合であっても、フィードバック的にその挙動を補正することができる。

＜Ｌ．変形例＞
（ｌ１：目標軌道上での移動速度を一定に維持できる範囲を最大化する処理例）
対象のアプリケーションによっては、目標軌道を移動中（典型的には、切削中）のＴＣＰ速度を可能な限り一定化した場合も存在する。このようにＴＣＰ速度を一定値に維持することで、加工面の仕上がり品質を向上させることができる。

この変形例に従う制御ロジックでは、加速度制限値によって制限された状況下において、目標軌道の終点において最終目標速度に到達することができる範囲まで、ＴＣＰ速度を補間速度Ｖｉｎｔに維持する。すなわち、ＴＣＰ速度を補間速度Ｖｉｎｔまで低下させた後、目標軌道の終点において最終目標速度ｖＥｎｄに到達するためにＴＣＰ速度の増加を開始するタイミングを可能な限り遅くする。

図１７は、本実施の形態の変形例に従う制御ロジックによって得られる可動機構４のＴＣＰ速度を示すタイムチャートである。図１７を参照して、まず、処理が開始されると、現在速度Ｖｅｌ（開始時の現在速度Ｖｅｌ１）を減速度制限Ｄｅｃ１の制限下で補間速度Ｖｉｎｔまで低減させる。このとき、目標加速度ＡｃｃＲｅｆは、以下のような式に従って算出される。

ＡｃｃＲｅｆ＝（Ｖｉｎｔ−Ｖｅｌ）
但し、ＡｃｃＲｅｆ＜Ｄｅｃ１であれば、ＡｃｃＲｅｆ＝Ｄｅｃ１
したがって、現在速度Ｖｅｌが補間速度Ｖｉｎｔまで低下した後は、目標加速度ＡｃｃＲｅｆがゼロとなるので、現在速度Ｖｅｌは補間速度Ｖｉｎｔを維持する。

また、速度変更開始距離Ｌａｐｐが以下のような式に従って予め算出される。但し、Ａｃｃ２は、加速度制限値である。

Ｌａｐｐ＝ａｂｓ（（ｖＥｎｄ^２−Ｖｅｌ^２）／２×ｌＬｅｆｔ）／（２×Ａｃｃ２）
そして、各制御周期において、残距離ｌＬｅｆｔが逐次推定されるとともに、この推定される残距離ｌＬｅｆｔが速度変更開始距離Ｌａｐｐと略一致すると、ＴＣＰ速度は加速度制限値Ａｃｃ２の制限下で最終目標速度ｖＥｎｄまで増加する。

このような処理を採用することで、ＴＣＰ速度を補間速度Ｖｉｎｔに維持できる区間を最大化できるため、ワークの加工面の品質などを向上させることができる。

＜Ｍ．作用効果＞
本発明の実施の形態によれば、予め設定された目標軌道の終点におけるＴＣＰ速度が最終目標速度と一致するように、現在速度をなるべく変動の少ない加速度で加減速させる。すなわち、ＴＣＰ速度が最終目標速度に向けて滑らかに変化するとともに、後続の軌道における目標速度との連続性も維持できる。これにより、仕上げ面の均一性を向上させることができる。

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

１ 制御装置、２ ワーク、３ メジャー、４ 可動機構、５ スイッチ、２１ 電源ユニット、２２ 主演算ユニット、２３ ＩＯユニット、２４ ＭＣユニット、２５ バックプレーン、３１，３２，３３ モータドライバ、４１，４２，４３ モータ、６０ 通信線、２２１ マイクロプロセッサ、２２３ バッファメモリ、２２５ 通信インターフェイス、２２６ ユーザメモリ、２３１ 処理回路、２３２，２４３ 入力回路、２３３，２４４ 出力回路、２４２ コントローラインターフェイス、３０１ Ｊｅｒｋ処理モジュール、３０２ 加速度処理モジュール、３０３ 目標速度算出モジュール、３０４ 遅延モジュール、３０５ 媒介変数増分算出モジュール、３０６ 傾き算出モジュール、３０７ 媒介変数算出モジュール、３０８ ベジェ曲線算出モジュール、３０９ 溜りパルス算出モジュール、ＳＹＳ システム。

Claims (12)

1. 複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置であって、
   始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、
   前記目標軌道における現在位置から前記終点までの残距離を推定する残距離推定手段と、
   前記可動部を前記残距離移動させた場合に前記可動部の速度が現在速度から目標速度にまで変化するような加速度を算出する加速度算出手段と、
   現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを備える、制御装置。
2. 前記加速度算出手段は、前記現在の移動速度を前記目標速度まで一定の加速度で変化させるように、前記加速度を算出する、請求項１に記載の制御装置。
3. 前記加速度算出手段は、各制御周期において前記加速度を算出する、請求項１または２に記載の制御装置。
4. 前記残距離推定手段は、前記目標軌道における前記現在位置から前記終点までの区間に対して予め定められた数のサンプル点を設定するとともに、設定したサンプル点を補間することで前記残距離を推定する、請求項１〜３のいずれか１項に記載の制御装置。
5. 前記残距離推定手段は、前記目標軌道における前記現在位置から前記終点までの区間について、前記現在位置に遠い区間に比較して前記現在位置に近い区間ほど、前記サンプル点の間隔がより密になるように、前記サンプル点を設定する、請求項４に記載の制御装置。
6. 前記目標軌道は、媒介変数を含む関数の形で定義される、請求項１〜５のいずれか１項に記載の制御装置。
7. 前記移動速度更新手段によって算出された前記移動速度の新たな目標値となるように、前記可動部が１制御周期内に移動すべき量を示す指令を出力する移動指令手段をさらに備え、
   前記移動指令手段は、
   前記移動速度の新たな目標値に対応して、前記可動部が移動すべき距離を算出する手段と、
   前記可動部が移動すべき距離に対応して、前記目標軌道における前記可動部の移動すべき位置を算出する手段と、
   前記可動部の移動すべき位置に対応して、前記複数の駆動軸の各々に対する移動指令を算出する手段とを含む、請求項１〜４のいずれか１項に記載の制御装置。
8. 前記目標軌道は、ベジェ曲線により定義される、請求項６または７に記載の制御装置。
9. 前記目標軌道生成手段は、外部入力される信号に応答して、前記目標軌道を生成する、請求項１に記載の制御装置。
10. 前記移動速度更新手段は、加速度および／または加加速度の変化量が予め定められた制限を超過しないように、前記移動速度の新たな目標値を算出する、請求項１に記載の制御装置。
11. 制御システムであって、
    複数の駆動軸により移動可能な可動部を有する可動機構と、
    前記可動機構を駆動するためのドライバと、
    前記ドライバに対して制御指令を与える制御装置とを備え、
    前記制御装置は、
    始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、
    前記目標軌道における現在位置から前記終点までの残距離を推定する残距離推定手段と、
    前記可動部を前記残距離移動させた場合に前記可動部の速度が現在速度から目標速度にまで変化するような加速度を算出する加速度算出手段と、
    現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む、制御システム。
12. 複数の駆動軸により移動可能な可動部を有する装置を制御するための制御方法であって、
    始点および終点により定義される移動区間に対して目標軌道を生成するステップと、
    前記目標軌道における現在位置から前記終点までの残距離を推定するステップと、
    前記可動部を前記残距離移動させた場合に前記可動部の速度が現在速度から目標速度にまで変化するような加速度を算出するステップと、
    現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出するステップとを備える、制御方法。

Images