JP6863818B2 - 位置制御装置 - Google Patents

Description

本発明の実施形態は、位置制御装置に関する。

従来、位置検出部により検出された制御対象の位置と、位置指令における位置とが一致するように位置制御を行う位置制御装置が知られている。この位置制御装置において、位置指令に対してフィルタ演算処理が行われる場合がある。しかしながら、上記のようにフィルタ演算が行われると、位置指令に対応する情報とフィルタ演算の結果に含まれる情報とが合致せず、位置制御が正確に行われない場合があった。

特許第３８９２８２４号公報

本発明が解決しようとする課題は、より正確に位置指令を反映した制御を行うことができる位置制御装置を提供することである。

実施形態の位置制御装置は、指令解析部と、フィルタ演算部と、偏差導出部と、補正部と、切替部と、位置制御部とを持つ。指令解析部は、位置に関する情報である上位位置指令を解析して、解析した解析位置指令を所定の周期で出力する。フィルタ演算部は、前記指令解析部により出力された解析位置指令に対して所定の周波数成分を減衰させるための演算結果である演算後位置指令を出力する。偏差導出部は、前記解析位置指令と、前記フィルタ演算部により演算された演算後位置指令との差を積分し、積分指令偏差を導出する。補正部は、解析位置指令がゼロで、前記偏差導出部により導出された積分指令偏差が第１の閾値以下であり、且つ前記フィルタ演算部により演算された演算後位置指令の示す値が第２の閾値以下である場合に、積分指令偏差を出力する。切替部は、前記補正部から出力された積分指令偏差と前記フィルタ演算部により演算された演算後位置指令とのいずれかを選択し、位置制御部へ位置指令として出力する。位置制御部は、前記切替部から出力された位置指令に基づいて、制御対象の位置を制御する。

位置制御装置２０を含む位置制御システム１の機能構成を示す図。 位置制御装置２０により実行される処理の流れを示すフローチャート。 解析位置指令とフィルタ演算部３０により演算された演算後位置指令および積分指令偏差の関係を模式的に示す図。 解析位置指令が無い状態で、フィルタ演算部３０により演算された演算後位置指令がアンダーシュートする場合を示す図。 指令解析部２２により実行される処理の流れを示すフローチャート。 上位位置指令が平滑化される際の概念図。

以下、実施形態の位置制御装置を、図面を参照して説明する。

（第１の実施形態）
図１は、位置制御装置２０を含む位置制御システム１の機能構成を示す図である。位置制御システム１は、例えば、上位指令装置１０と、位置制御装置２０とを備える。上位指令装置１０は、例えば、キーボードやタッチパネルなどの操作部と、画像を表示する表示部とを備える。上位指令装置１０は、オペレータによって操作部に対して入力された情報に応じた位置指令（制御対象の位置を指示する情報であって、移動距離を示す指標や値）を位置制御装置２０に出力する。以下、上位指令装置１０から出力された位置指令を「上位位置指令」と称する場合がある。

上位位置指令は、パルス方式や、通信方式、バス方式で位置制御装置２０に出力される。パルス方式では、上位指令装置１０は、移動距離に応じたパルスを出力することで上位位置指令を出力する。パルス方式では、１パルスの制御対象の移動距離は１μｍなどのように、１パルスに対する制御対象の移動距離が定められている。また、目標位置までの速度は、パルスの周波数で表される。通信方式、またはバス方式では、例えば、上位指令装置１０は、制御対象の移動距離に相当する値や速度等に相当する値を所定の周期、および所定のタイミングで上位位置指令として出力する。具体的には、上位指令装置１０は、例えば、５ｍ［ｓｅｃ］ごとに、５ｍ［ｓｅｃ］間で制御対象を移動させたい距離や速度等の情報を出力する。

位置制御装置２０は、例えば、指令解析部２２と、フィルタ演算部３０と、偏差導出部３２と、補正部４０と、切替部４９と、位置制御部５０と、速度制御部６０とを備える。位置制御装置２０は、例えば、ＣＰＵ（Central Processing Unit）等のプロセッサが位置制御装置２０の記憶装置に記憶されたプログラムを実行することにより実現されてよい。また、位置制御装置２０の全部または一部は、ＬＳＩ（Large Scale Integration）、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）等のハードウェアによって実現され、これらの機能部の機能を実現するための回路構成を有してもよい。また、これらの機能部は、ソフトウェアとハードウェアの協働によって実現されてもよい。

指令解析部２２は、上位位置指令を位置制御装置２０内で処理できるように解析して所定の周期でフィルタ演算部３０に位置指令として出力する。位置制御装置２０内で処理できるようにとは、位置制御装置２０の位置制御周期（例えば１００μ［ｓｅｃ］）に合わせて制御対象の移動距離を示す情報をフィルタ演算部３０に出力することである。以下、指令解析部２２により解析され、出力された位置指令を「解析位置指令」と称する場合がある。

フィルタ演算部３０は、指令解析部２２により出力された解析位置指令を取得し、取得した解析位置指令に対して所定の周波数成分（主に４０Ｈｚ前後の低周波数帯）を減衰させ、減衰させた位置指令を位置制御部５０に出力する。フィルタ演算部３０において用いられるフィルタは、例えば、ノッチフィルタや、ＩＩＲ（Infinite impulse response）フィルタである。以下、フィルタ演算部３０により演算された位置指令を「演算後位置指令」と称する場合がある。

偏差導出部３２は、解析位置指令と、フィルタ演算部３０により演算された演算後位置指令との差である指令偏差を積分した積分指令偏差を導出する。
補正部４０は、例えば、第１判定部４４と、第２判定部４６と、出力制御部４８とを備える。

第１判定部４４は、偏差導出部３２により導出された積分指令偏差が第１の閾値以下であるか否かを判定する。第２判定部４６は、フィルタ演算部３０により演算された演算後位置指令が第２の閾値以下であるか否かを判定する。

出力制御部４８は、解析位置指令が無い状態で、偏差導出部３２により導出された積分指令偏差が第１の閾値以下であり、且つフィルタ演算部３０により演算された演算後位置指令が第２の閾値以下である場合に、積分指令偏差を出力する。出力制御部４８から積分指令偏差が出力される場合は、出力制御部４８から出力された積分指令偏差が位置指令として位置制御部５０に入力される。出力制御部４８から積分指令偏差が出力されない場合は、演算後位置指令が位置指令として位置制御部に入力される。
以上の様な一連の処理をまとめると図２の様になる。
図２を用いてもう少し詳しく説明する。

まず、フィルタ演算部３０は、指令解析部２２から取得した解析位置指令に対して、フィルタ演算処理を実行し演算後位置指令を求める（ステップＳ１００）。次に、偏差導出部３２は、積分指令偏差を導出する（ステップＳ１０２）。

ここで、フィルタ演算部３０に入力される解析位置指令と、フィルタ演算部３０から出力される演算後位置指令との関係について説明する。例えば、フィルタ演算部３０に解析位置指令が入力されると、フィルタ演算が行われ、演算結果が出力される。上位位置指令を解析した解析位置指令が入力されたときの始期ではフィルタ演算部３０への入力とフィルタ演算部３０からの出力との差である指令偏差は、フィルタ演算部３０でフィルタ処理されている為、大きくなる。例えば入力される解析位置指令は「１００」、出力される演算後位置指令は「１」とすると、指令偏差は「９９」となる。また、次の周期のフィルタ演算において、入力される解析位置指令は「１００」、出力される演算後位置指令は「１０」とすると、積分指令偏差は、今回の指令偏差「９０」と、前回の指令偏差「９９」を積算した「１８９」となる。時間が経過するに従い、演算後位置指令は解析位置指令に追いついていき指令偏差は小さくなる。

一方、上位位置指令を解析した解析位置指令の終期では、積分指令偏差は小さくなる。例えば終期において入力された解析位置指令は「１」、出力される演算後位置指令は「９０」とすると、指令偏差は「−８９」となり、前回の積分指令偏差が「１８９」である場合、今回の積分指令偏差は、「１００（１８９−８９）」となる。

更に、上位位置指令に対応する解析位置指令の入力が完了後においても、フィルタの演算結果が出力される。例えば、フィルタ演算部３０に入力される解析位置指令が「０」である場合であっても、フィルタ演算による入力に対する出力の遅延によって、出力される演算後位置指令が「１００」になる場合がある。

上述したように、積分指令偏差は、解析位置指令の終期または解析位置指令のフィルタ演算部３０への入力が完了後に減少する傾向であるため、解析位置指令のフィルタ演算部３０への入力が完了した場合に、積分指令偏差の絶対値が、第１の閾値以下である場合、フィルタ演算部３０によって演算される演算後位置指令がすべて出力されるタイミングに近いと推測することができる。

次に、補正部４０は、フィルタ演算部３０への解析位置指令の入力が完了したか否かを判定する（ステップＳ１０４）。解析位置指令の入力が完了していない場合、位置制御部５０への入力である位置指令は、フィルタ演算部３０によってフィルタ演算された演算後位置指令のみになる（ステップＳ１０６）。

フィルタ演算部３０への解析位置指令の入力が完了した場合、第１判定部４４は、積分指令偏差の絶対値が、第１の閾値（例えば「３」）以下であるか否かを判定する（ステップＳ１０８）。

積分指令偏差の絶対値が、第１の閾値以下でない場合、ステップＳ１０６の処理に進む。積分指令偏差の絶対値が、第１の閾値以下の場合、ステップＳ１１０の処理に進む。

フィルタ演算部３０への解析位置指令の入力が完了し、積分指令偏差の絶対値が第１の閾値以下の場合、第２判定部４６は、フィルタ演算部３０により演算された演算後位置指令の絶対値が、第２の閾値以下であるか否かを判定する（ステップＳ１１０）。第２の閾値は、第１の閾値と同一であってもよいし、異なっていてもよい。フィルタ演算部３０により演算された演算後位置指令の絶対値が、第２の閾値以下でない場合、ステップＳ１０６に処理に進む。フィルタ演算部３０により演算された演算後位置指令の絶対値が、第２の閾値以下である場合、出力制御部４８は、積分指令偏差を出力する（ステップＳ１１２）。なお、積分指令偏差を出力した場合、積分指令偏差が位置指令として位置制御部５０に入力される。（ステップＳ１１４）これにより、フィルタ演算部３０に入力された解析位置指令の合計（指令位置指令）と、位置制御部５０に入力される位置指令の合計とが合致する。

切替部４９は、このように、補正部４０から出力された積分指令偏差とフィルタ演算部３０により演算された演算後位置指令とのいずれかを選択し、位置制御部５０へ位置指令として出力する。

位置制御部５０は、制御対象（例えばモータ）の位置を検出する位置検出部（不図示）により検出された位置情報（フィードバック信号）を取得する。位置検出部は、光学的または磁気的に制御対象の位置を検出するエンコーダやレゾルバ等である。位置制御部５０は、取得した位置情報、フィルタ演算部３０により演算された演算後位置指令、および出力制御部４８により出力される積分指令偏差に基づいて、上位指令装置１０により指示された位置と、制御対象の位置とが合致するようにフィードバック制御を行う。位置制御部５０は、フィードバック制御の結果に基づいて、速度指令を生成し、速度制御部６０に出力する。

速度制御部６０は、位置制御部５０から取得した速度指令と、制御対象の動作速度（例えばモータの回転速度）を算出する速度算出部により算出された速度とが合致するような電流指令を導出し、制御対象に出力する。制御対象は、速度制御部６０により出力された電流指令に基づいて、自装置を制御する。このように制御対象が制御されることによって、制御対象の動作によって制御されるアクチュエータ等の位置が制御される。

図３は、解析位置指令とフィルタ演算部３０により演算された演算後位置指令および積分指令偏差の関係を模式的に示す図である。図３の（ａ）は解析位置指令が示す位置の時間的経緯を示している。図３の（ｂ）の縦軸は処理周期ごとの解析位置指令を示し、横軸は時間を示す。図３の（ｃ）は演算後位置指令が示す位置の時間的経緯を示す。図３の（ｄ）の縦軸は処理周期ごとの演算後位置指令を示し、横軸は時間を示す。図３の（ｅ）の縦軸は解析位置指令と演算後位置指令との差である指令偏差を示し、横軸は時間を示す。図３の（ｆ）の縦軸は指令偏差を積分した積分指令偏差を示し、横軸は時間を示す。

図４は、解析位置指令が無い状態で、フィルタ演算部３０により演算された演算後位置指令がアンダーシュートする場合を示す図である。図３と重複する説明は省略する。図４の（ａ）の縦軸はフィルタ演算部３０の処理周期ごとに出力される演算後位置指令Ｓ１〜Ｓｎを示している。図示するように、フィルタ演算部３０の演算結果において、演算後位置指令Ｓｎ−２からＳｎのようにアンダーシュートした演算後位置指令が出力される場合がある。この場合、積分指令偏差の絶対値は、第１の閾値（Ｔｈ１）以下になる場合がある。図４（ｃ）の時刻Ｔｎ−１およびＴｎの期間で積分指令偏差の絶対値がＴｈ１以下になっている。しかしながら、積分指令偏差の絶対値が第１の閾値以下であっても、フィルタ演算部３０の演算結果の演算後位置指令が第２の閾値（Ｔｈ２）を超える場合が有る。図４（ａ）の時刻Ｔｎ−１およびＴｎの期間で積分指令偏差の絶対値がＴｈ１以下であるが、演算後位置指令の絶対値の｜Ｓｎ−１｜がＴｈ２以上になっている。この場合、フィルタ演算の結果が全て出しきられていない可能性が有る。図４で、時刻Ｔｎ−１およびＴｎの期間で積分指令偏差の絶対値がＴｈ１以下になっている為、その時点で補正部４０が積分指令偏差を出力し、その積分指令偏差が位置制御部５０に位置指令として入力して位置指令の位置制御部５０への入力が完了したと判断してしまう。しかし、それだけでは、実際には演算後位置指令Ｓｎ−１の後にＳｎ〜Ｓｎ＋３が有り、本来の動きと相違を生じる為、制御対象に振動が発生してしまう可能性がある。

このため、補正部４０は、フィルタ演算部３０に入力された解析位置指令と、フィルタ演算部３０により出力された演算後位置指令との差である指令偏差を積分した積分指令偏差の絶対値が第１の閾値以下と判定した場合であっても、演算後位置指令の絶対値が第２の閾値以下でない場合は、積分指令偏差は出力しないようにする。この結果、演算後位置指令がアンダーシュートする場合における誤判定を抑制させることができる。

上述したように、解析位置指令が無い状態で、補正部４０が、積分指令偏差の絶対値が第１の閾値以下であり、且つフィルタ演算部３０によりフィルタ演算された演算後位置指令の絶対値が第２の閾値以下である場合に、積分指令偏差を出力するため、積分指令偏差が位置指令として位置制御部５０に入力される。これにより、位置制御部５０に入力される位置指令の信頼性を向上させることができ、より正確に位置指令を反映した制御を実現することができる。また、フィルタ演算部３０の演算の終了タイミングを迅速に判定することができる。

（第２の実施形態）
以下、第２の実施形態について説明する。第２の実施形態では、指令解析部２２が、上位位置指令を平滑処理する。上位位置指令が第２の実施形態のように指令されない時は、適用できない場合がある。以下、図５、６を使って説明する。

図５は、指令解析部２２により実行される処理の流れを示すフローチャートで、図６は、指令解析部２２により上位位置指令が平滑化される際の概念図である。
指令解析部２２は、
（１）第１のタイミングで第１の上位位置指令（図６のＰ１）を取得する。第１のタイミングが初回の場合、次のタイミングである第２のタイミングとの時間間隔が分からないので予め設定した分割数の上限値で平滑して第１の解析位置指令として所定の周期ごとにフィルタ演算部３０に出力する（図６の例ではＰ１１〜Ｐ１４）。所定の周期は、位置制御部５０で処理される位置制御周期と同じ周期である。予め設定した分割数の上限値をＮｍとすると、Ｐ１１＝Ｐ１２＝…＝Ｐｎｍ＝Ｐ１／Ｎｍを所定の周期ごとにフィルタ演算部３０に出力する。例えばＰ１を１０００、Ｎｍを１０とし、所定の周期を１００μｓｅｃとすると、１０００／１０＝１００（Ｐ１１、Ｐ１２、…）を１００μｓｅｃごとにフィルタ演算部３０に出力する。
（１）の一連の処理の流れが、図５のＳＴＡＲＴ→Ｓ２００→Ｓ２０２→Ｓ２０４→ＲＥＴＵＲＮとなる。
次に、
（２）第１のタイミングの後の第２のタイミングで第２の上位位置指令（図６のＰ２）を取得し、第１の上位位置指令と第２の上位位置指令との時間間隔（図６の例ではＰ１からＰ２の時間間隔、所定の周期の４回分の時間）に基づいて、第２の上位位置指令と第１の解析位置指令の出力していない残り分との合算を平滑化し、平滑化した第２の解析位置指令として所定の周期ごとにフィルタ演算部３０に出力する。平滑とは、初回は予め設定した分割数の上限値、それ以降は、各タイミング間の時間間隔を所定の周期で除算した値（周期の回数Ｎ）の２倍の値を分割数とし、その分割数で除算する事をいう。Ｐ１＝Ｐ２＝１０００、所定の周期：１００μｓｅｃとすると、図６の例では所定の周期の４回分の解析位置指令をフィルタ演算部３０に出力しているので、出力していない残り分は、１０００−１００×４＝６００、そしてＰ２は１０００なので６００＋１０００＝１６００を周期の４回（Ｎ＝４）の２倍である８が分割数となるので８で除算した２００（Ｐ１５、Ｐ１６、…）を１００μｓｅｃごとにフィルタ演算部３０に出力する。
（２）の一連の処理の流れが、図５のＳＴＡＲＴ→Ｓ２００→Ｓ２０２→Ｓ２０６→ＲＥＴＵＲＮとなる。

（３）第２のタイミングの後の第３のタイミングで第３の上位位置指令（図６のＰ３）を取得し、第３の上位位置指令と、第２の解析位置指令の出力していない残り分とを合算し、第２の上位位置指令と第３の上位位置指令との時間間隔（図６の例ではＰ２からＰ３の時間間隔、所定の周期の４回分の時間）とに基づいて、合算した値を平滑化し、第３の解析位置指令として所定の周期ごとにフィルタ演算部３０に出力する。
（３）の一連の処理の流れは（２）と同様、図５のＳＴＡＲＴ→Ｓ２００→Ｓ２０２→Ｓ２０６→ＲＥＴＵＲＮとなる。

なお、指令解析部２２は、位置制御周期ごとに上位位置指令を取得した場合、「Ｎ＝２」として上位位置指令を平滑化する。また、上記の一例では、上位位置指令が４つの位置制御周期が経過した際に指令解析部２２に入力されるものとして説明したが、これに限らず、上位位置指令が入力される周期が可変である場合においても、指令解析部２２は上記と同等の処理を実行する。

このように、指令解析部２２は、入力された上位位置指令を平滑化するのでフィルタ演算部３０のフィルタ演算は不要とすることもできる。

更に、本実施形態２の指令解析部２２は、入力される上位位置指令の周期が変化した場合でも、上位位置指令が入力される周期に基づいて自動的に上位位置指令を適切に平滑化することができる。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１…位置制御システム、２０、２０Ａ…位置制御装置、２２…指令解析部、３０…フィルタ演算部、４０…補正部、３２…偏差導出部、４４…第１判定部、４６…第２判定部、４８…出力制御部、４９…切替部、５０…位置制御部

Claims (10)

1. 位置に関する情報である上位位置指令を解析して、解析した解析位置指令を所定の周期で出力する指令解析部と、
   前記指令解析部により出力された解析位置指令に対して所定の周波数成分を減衰させるための演算結果である演算後位置指令を出力するフィルタ演算部と、
   前記解析位置指令と、前記フィルタ演算部により演算された演算後位置指令との差を積分し、積分指令偏差を導出する偏差導出部と、
   解析位置指令がゼロで、前記偏差導出部により導出された積分指令偏差が第１の閾値以下であり、且つ前記フィルタ演算部により演算された演算後位置指令の示す値が第２の閾値以下である場合に、積分指令偏差を出力する補正部と、
   前記補正部から出力された積分指令偏差と前記フィルタ演算部により演算された演算後位置指令とのいずれかを選択する切替部と、
   前記切替部から出力された位置指令に基づいて、制御対象の位置を制御する位置制御部と、
   を備える位置制御装置。
2. 前記フィルタ演算部は、所定の周波数帯域の成分を減衰させるノッチフィルタを含む、
   請求項１記載の位置制御装置。
3. 前記切替部は、前記指令解析部により出力される解析位置指令の前記フィルタ演算部への入力が完了していないと前記補正部が判定した場合、前記位置制御部に前記フィルタ演算部により演算された演算後位置指令を出力させる、
   請求項１または請求項２記載の位置制御装置。
4. 前記補正部は、前記指令解析部により出力される解析位置指令の前記フィルタ演算部への入力が完了したと判定した場合、前記偏差導出部により導出された積分指令偏差が第１の閾値以下であるか否かを判定する、
   請求項１または請求項２記載の位置制御装置。
5. 前記切替部は、前記偏差導出部により導出された積分指令偏差が第１の閾値以下でないと前記補正部が判定した場合、前記位置制御部に前記フィルタ演算部により演算された演算後位置指令を出力させる、
   請求項４項に記載の位置制御装置。
6. 前記切替部は、前記フィルタ演算部から出力された演算後位置指令が第２の閾値以下でないと前記補正部が判定した場合、前記位置制御部に前記フィルタ演算部により演算された演算後位置指令を出力させる、
   請求項４項に記載の位置制御装置。
7. 前記切替部は、前記偏差導出部により導出された積分指令偏差が第１の閾値以下で、且つ前記フィルタ演算部から出力された演算後位置指令が第２の閾値以下と前記補正部が判定した場合、前記位置制御部に前記補正部が出力した積分指令偏差を出力させる、
   請求項４項に記載の位置制御装置。
8. 前記指令解析部は、第１のタイミングで第１の上位位置指令を取得し、取得した前記第１の上位位置指令を予め設定した分割数の上限値で平滑し、平滑した第１の解析位置指令を所定の周期ごとに前記フィルタ演算部に出力する、
   請求項１から７のうちいずれか１項に記載の位置制御装置。
9. 前記指令解析部は、前記第１のタイミングの後の第２のタイミングで第２の上位位置指令を取得し、前記フィルタ演算部に出力が完了していない前記第１の解析位置指令と、取得した前記第２の上位位置指令との合算を、前記第１の上位位置指令と前記第２の上位位置指令との間隔に基づいて平滑し、平滑した第２の解析位置指令を、所定の周期ごとに前記フィルタ演算部に出力する、
   請求項８記載の位置制御装置。
10. 前記指令解析部は、前記第２のタイミングの後の第３のタイミングで第３の上位位置指令を取得し、前記フィルタ演算部に出力が完了していない前記第２の解析位置指令と、取得した前記第３の上位位置指令との合算を、前記第２の上位位置指令と前記第３の上位位置指令との間隔とに基づいて平滑し、平滑した第３の解析位置指令を、所定の周期ごとに前記フィルタ演算部に出力する、
    請求項９記載の位置制御装置。

Images