JP2015197714A - 位置制御装置および位置制御方法、光学機器、撮像装置 - Google Patents

Abstract

【課題】制御対象の姿勢が変化しても推定状態量の結果が誤推定になる可能性を低減することができる位置制御装置および位置制御方法を提供すること。
【解決手段】目標発生部６０１は制御対象の目標値信号を出力し、位置エンコード処理部６０２は制御対象の位置検出信号を出力する。ＰＩＤ補償器６０４は目標値信号と位置検出信号との差信号に基づき、制御対象を目標位置に追従させるための制御量を算出して加算部６０６に出力する。外乱推定オブザーバ６０５は外乱の推定結果を加算部６０６に出力する。加算部６０６はＰＩＤ補償器６０４の出力する制御量と外乱推定オブザーバ６０５の出力を加算して総制御量を算出する。記憶部６０７は固定位置への位置制御中の制御量を姿勢差補正量として記憶し、移動位置への位置制御中には記憶部６０７に保存された値を総制御量から減算した制御量を用いて外乱推定オブザーバ６０５が外乱推定を行う。
【選択図】図６

Description

本発明は、外乱推定オブザーバを用いて制御対象の位置を制御する装置および方法に関する。

制御対象をサーボ制御し、制御対象の絶対位置を検出し、位置、速度および制御対象に印加される外乱などの状態量を推定するオブザーバを用いるサーボ制御システムがある。特許文献１には、制御対象の任意の停止位置から目標位置に直接、安全に制御対象を制御することが可能なサーボ制御システムが開示されている。

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

特許文献１では、固定座標系に設置される制御装置を想定している。装置全体の設置姿勢が変化した場合には、駆動手段に印加される駆動指令に基づいて状態量が算出されるが、姿勢変化に抗するための駆動指令成分がバイアスになってしまい、推定状態量の結果が誤推定値になる可能性がある。
本発明の目的は、制御対象の姿勢が変化しても推定状態量の結果が誤推定になる可能性を低減することができる位置制御装置および位置制御方法の提供にある。

本発明に係る装置は、外乱推定オブザーバを備える位置制御装置であって、制御対象の位置を検出する位置検出手段と、前記制御対象の目標位置を出力する目標発生手段と、前記制御対象を目標位置に追従させる制御量を算出する算出手段と、前記算出手段により算出された制御量または当該制御量と前記外乱推定オブザーバにより算出された外乱相殺制御量とを加算した総制御量を出力して前記制御対象を制御する制御信号出力手段と、前記制御対象の固定位置への位置制御中に、前記制御対象の姿勢変化へ抗する制御量である姿勢差補正量を記憶する記憶手段と、を備える。前記外乱推定オブザーバは、前記制御対象を移動目標へ追従させる位置制御中に前記総制御量から前記記憶手段の記憶している姿勢差補正量を減算した制御量および前記制御対象の移動量から外乱を推定し、外乱を相殺する前記外乱相殺制御量を算出する。

本発明によれば、制御対象の姿勢が変化しても推定状態量の結果が誤推定になる可能性を低減することができる。

本発明の実施形態に係る位置制御装置のブロック図である。 インクリメンタル信号の処理の概念図である。 インクリメンタル位置エンコード処理を説明するフローチャートである。 インクリメンタル信号と合成信号の概念図である。 アブソリュート位置エンコード処理を説明するフローチャートである。 本発明の第１実施形態に係るブロック線図である。 第１実施形態に係る外乱推定オブザーバのブロック線図である。 第１実施形態の処理を説明するフローチャートである。 第１実施形態の外乱推定オブザーバ処理を説明するフローチャートである。 第１実施形態における動作目標例を説明する図である。 第１実施形態における数値変化例を説明する図である。 本発明の第２実施形態の処理を説明するフローチャートである。 第２実施形態に係る外乱推定オブザーバのブロック線図である。 第２実施形態の外乱推定オブザーバ処理を説明するフローチャートである。

以下、図面を参照しながら本発明の各実施形態を説明する。本発明は、例えばデジタル一眼レフやデジタルコンパクトカメラに限らず、デジタルビデオカメラ、監視カメラ、Ｗｅｂカメラ、携帯電話等の撮像装置における光学部材の駆動制御に適用可能である。また、デジタル一眼レフ用の交換レンズのような光学機器にも適用可能である。したがって、光学機器や撮像装置も本発明の一側面を構成する。各実施形態では、推力の未発生時に保持力が無いアクチュエータとしてボイスコイル型リニアモータを例示する。

［第１実施形態］
本発明の第１実施形態では、目標速度が０で固定位置への位置制御中のサーボ補償器の出力を姿勢差補正量として保存し、目標速度が０以外で制御対象を移動目標へ追従させる位置制御中に総制御量から保存された姿勢差補正量を減算して外乱推定を行う。

図１は、本発明の実施形態に共通する、位置制御装置１００の構成例を示す。内部バス１０９に対してＣＰＵ（中央演算処理装置）１０１、ＲＯＭ（リード・オンリ・メモリ）１０２、ＲＡＭ（ランダム・アクセス・メモリ）１０３、制御信号出力部１０４、ＡＤ入力部１０８が接続される。各部は内部バス１０９を介して互いにデータの送受信が可能である。ＲＯＭ１０２は、ＣＰＵ１０１が動作するための各種プログラムなどを格納する読出し専用メモリである。制御手段および計測手段としてのＣＰＵ１０１は、例えばＲＯＭ１０２に格納されるプログラムに従い、ＲＡＭ１０３をワークメモリとして用いて、位置制御装置１００の各部を制御する。

制御信号出力部１０４は、ＣＰＵ１０１から命令された制御信号をモータドライバ１０５に出力してリニアモータ１０６を駆動させる。また制御信号出力部１０４は、ＣＰＵ１０１から命令された制御信号をリニアエンコーダ１０７に出力して、異なる位相のインクリメンタル信号を出力させる。リニアモータ１０６はレンズ等の制御対象（不図示）を移動させる。位置検出用のリニアエンコーダ１０７は制御対象の移動に応じてインクリメンタル信号を出力する。ＡＤ入力部１０８は、リニアエンコーダ１０７が出力するインクリメンタル信号をＡ（アナログ）−Ｄ（デジタル）変換して、Ａ−Ｄ変換後の信号をＣＰＵ１０１に出力する。ＣＰＵ１０１は、ＡＤ入力部１０８により変換されたインクリメンタル信号からインクリメンタル位置エンコード処理（以下、第１エンコード処理という）、およびアブソリュート位置エンコード処理（以下、第２エンコード処理という）により現在位置を算出する。

図２および図３を参照して、第１位置エンコード処理を説明する。図２は、インクリメンタル信号の処理を説明する概念図である。図３はＣＰＵ１０１が実行する第１エンコード処理を説明するフローチャートである。
図2（Ａ）はインクリメンタル信号の波形例を示す。縦軸は10bitのＡＤ変換値を示し、横軸は位置を示す。サイン波２０１とコサイン波２０２が、制御対象の移動に応じて出力される。図2（Ｂ）は、サイン波２０１とコサイン波２０２による正接関数の逆関数（逆正接関数）を示す。縦軸は角度をラジアンで示し、横軸は位置を示す。逆正接関数値２０３は、０から２πまでの範囲に正規化されて演算される。図２（Ｃ）はインクリメンタル位置エンコード値を示す。縦軸は単調増加するインクリメンタル位置エンコード値２０４を示し、横軸は位置を示す。逆正接関数値２０３がゼロを跨いだ時にMSB（Most Significant Bit）での２πに相当する上位桁が増減する。例えば、逆正接関数値２０３がゼロから増加して再びゼロになってさらに増加した場合、インクリメンタル位置エンコード値２０４に２πが加算される。他方、逆正接関数値２０３が減少してゼロになった場合にインクリメンタル位置エンコード値２０４から２πが減算される。

図３を参照して第１エンコード処理を説明する。Ｓ３０１にてＣＰＵ１０１は、第１エンコード処理の許可フラグがセット状態であるかどうかを判断する。許可フラグがセット状態でない場合には処理を終了し、許可フラグがセット状態である場合、Ｓ３０２へ処理を進める。Ｓ３０２において、ＣＰＵ１０１は２相信号のＡＤ変換値を取得し、オフセット除去演算、ゲイン調整演算を行う。２相信号とは、図２（Ａ）のサイン波２０１およびコサイン波２０２のように、位相の異なる対をなす信号である。そして、ＣＰＵ１０１は、図２（Ｂ）に示す逆正接関数の演算結果（逆正接関数値２０３）のデータをＲＡＭ１０３へ保存する。

Ｓ３０３でＣＰＵ１０１は、前回の逆正接関数演算結果と今回の逆正接関数演算結果との差の絶対値を３π／８と比較する。差の絶対値が３π／８未満であるかどうかが判断され、そうであればＳ３０４へ処理を進め、差の絶対値が３π／８未満でなければＳ３１０へ移行する。Ｓ３０４にてＣＰＵ１０１は、前回の逆正接関数演算結果と今回の逆正接関数演算結果による移動で制御対象の位置がゼロを跨いだかどうかを判断する。制御対象の位置がゼロを跨いでいる場合、Ｓ３０５へ処理を移し、ゼロを跨いでいない場合にはＳ３０８へ移行する。Ｓ３０５でＣＰＵ１０１は、今回の逆正接関数演算結果が前回の逆正接関数演算結果未満であるかどうかを判断する。今回の逆正接関数演算結果が前回の逆正接関数演算結果未満であればＳ３０６へ移行し、今回の逆正接関数演算結果が前回の逆正接関数演算結果以上であればＳ３０７へ移行する。Ｓ３０６でＣＰＵ１０１は、２π単位である上位桁へ１を加算する。上位桁とはインクリメンタル位置エンコード値の２π以上の桁である。一方、Ｓ３０７でＣＰＵ１０１は上位桁から１を減算する。

Ｓ３０６またはＳ３０７の後でＳ３０８へ処理を進め、ＣＰＵ１０１は２π単位である上位桁と今回の逆正接関数演算結果を加算する。これにより、図2（Ｃ）のインクリメンタル位置エンコード値２０４が更新され、Ｓ３０９へ移行する。Ｓ３０９にてＣＰＵ１０１は、移動方向判別に係るエラー警告無しとして処理を終了する。Ｓ３０３からＳ３１０に移行した場合、ＣＰＵ１０１は移動方向判別に係るエラー警告有りとして処理を終了する。その理由は、今回と前回の逆正接関数演算結果の差の絶対値が３π／８以上であり、制御対象の移動方向に関して増加により差の絶対値が３π／８以上となったのか、減少により３π／８以上となったのかを判別しにくいためである。
以上のようにインクリメンタル信号からエンコード値が生成されるので、位置制御に用いることができる。

次に、図４と図５を参照して第２エンコード処理を説明する。図４は、ピッチの異なる３種類のインクリメンタル信号と合成信号を例示した概念図である。図４（Ａ）から（Ｆ）にて縦軸は角度をラジアンで示し、横軸は位置を示す。縦軸に示す逆正接関数値は、制御対象の移動に応じて逆正接関数として０から２πまでの範囲に正規化されて演算される。また図５はＣＰＵ１０１による第２エンコード処理を説明するフローチャートである。

図４（Ａ）は最小ピッチの逆正接関数演算結果４０１を例示し、全ストロークで２１周期出現する。図４（Ｂ）は中間ピッチの逆正接関数演算結果４０２を例示し、全ストロークで１０周期出現する。図４（Ｃ）は最大ピッチの逆正接関数演算結果４０３を例示し、全ストロークで４周期出現する。図４（Ｄ）から（Ｆ）については後述する。
図５のＳ５０１でＣＰＵ１０１は、ＲＡＭ１０３に保存されている最小ピッチの２相信号のＡＤ変換値を取得し、オフセット除去演算、ゲイン調整演算を行う。そしてＣＰＵ１０１は、図４（Ａ）の逆正接関数演算結果４０１を「p1」としてＲＡＭ１０３へ保存する。次のＳ５０２でＣＰＵ１０１は、ＲＡＭ１０３に保存されている中間ピッチの２相信号のＡＤ変換値を取得し、オフセット除去演算、ゲイン調整演算を行う。ＣＰＵ１０１は、図４（Ｂ）の逆正接関数演算結果４０２を「p2」としてＲＡＭ１０３へ保存する。次のＳ５０３でＣＰＵ１０１は、ＲＡＭ１０３に保存されている最大ピッチの２相信号のＡＤ変換値を取得し、オフセット除去演算、ゲイン調整演算を行う。ＣＰＵ１０１は、図４（Ｃ）の逆正接関数演算結果４０３を「p3」としてＲＡＭ１０３へ保存し、Ｓ５０４に移行する。

Ｓ５０４にてＣＰＵ１０１は下式を用い、Ｎを整数の変数として全ストロークをＮ周期とした逆正接関数に正規化する処理を行う。Ｎ＝１，６，１３，２１として具体的に説明する。

なお、式(1)はＮ＝１の場合、式(2)はＮ＝６の場合、式(3)はＮ＝１３の場合に使用する。

Ｎ＝１の場合、ＲＡＭ１０３に保存されているp1とp2の各データを用いて式(1)でph1が算出され、算出結果のデータがＲＡＭ１０３へ保存される。図４（Ｄ）に示す演算結果４０４は制御対象の移動に応じて式(1)の結果として、０から２πまでの範囲に正規化されて演算されるph1を例示し、全ストロークで１周期出現する。Ｎ＝６の場合、ＣＰＵ１０１はＲＡＭ１０３に保存されているp2とp3の各データを用いて式(2)でph6を算出し、算出結果のデータをＲＡＭ１０３へ保存する。図４（Ｅ）に示す演算結果４０５は制御対象の移動に応じて式(2)の結果として、０から２πまでの範囲に正規化されて演算されるph6を例示し、全ストロークで６周期出現する。Ｎ＝１３の場合、ＣＰＵ１０１はＲＡＭ１０３に保存されているp1とp3の各データを用いて式(3)でph13を算出し、算出結果のデータをＲＡＭ１０３へ保存する。図４（Ｆ）に示す演算結果４０６は制御対象の移動に応じて式(3)の結果として、０から２πまでの範囲に正規化されて演算されるph13を例示し、全ストロークで１３周期出現する。Ｎ＝２１の場合、ＣＰＵ１０１はＲＡＭ１０３へ保存されているp1をph21として（ph21=p1）、データをＲＡＭ１０３へ保存する。図４（Ａ）に示すようにph21では全ストロークで２１周期出現する。

次のＳ５０５でＣＰＵ１０１は、以下の式を用いて、演算結果がＮ周期中でどの周期に該当するかを算出する。

ＣＰＵ１０１はＲＡＭ１０３へ保存されているph1を用いて式(4)で演算し、全ストロークで６周期出現する中のどの周期になるか演算し、abs6のデータをＲＡＭ１０３へ保存する。そしてＣＰＵ１０１は式(5)により、abs6とph6の各データを用いて、全ストロークで１３周期出現する中のどの周期になるか演算し、abs13のデータをＲＡＭ１０３へ保存する。ＣＰＵ１０１は式(6)により、abs13とph13の各データを用いて、全ストロークで２１周期出現する中のどの周期になるか演算し、abs21のデータをＲＡＭ１０３へ保存する。ＣＰＵ１０１は式(7)により、abs21とph21の各データを用いて、最小ピッチの分解能で全ストローク中のどの位置になるか演算し、FullABSのデータをＲＡＭ１０３へ保存して処理を終える。

以上のように、ピッチの異なる３種類のインクリメンタル信号を合成して更にピッチの異なるインクリメンタル信号を生成する事ができる。制御対象が静止した状態で取得されるピッチの異なる複数のインクリメンタル信号を合成し、第２エンコード処理を行うことで、最小ピッチの分解能でアブソリュート位置を算出できる。以降は、最小ピッチの分解能が固定で第１エンコード処理を実行し続けても生成されたインクリメンタル位置エンコード値をアブソリュート位置エンコード値として扱うことができる。

次に図６を参照して制御系の構成を説明する。図６は本実施形態に係る制御系のブロック線図を例示する。なお、図１と同じ構成部については同一の符号を使用することでそれらの説明を省略する。また、加算部については加算処理および減算処理（負値の加算）を含むものとして説明する。
制御信号出力部１０４は、ＣＰＵ１０１から指定された制御量を入力としてデューティー比に変換したＰＷＭ（パルス幅変調）信号を、制御信号としてモータドライバ１０５へ出力する。モータドライバ１０５は、例えばＨブリッジドライバであり、制御信号として入力されるＰＷＭ信号のデューティー比に応じて正逆方向にリニアモータ１０６を駆動する。リニアモータ１０６は、例えばボイスコイルモータであり、水平姿勢でデューティー比が50%の場合、推力無しで停止する。また、デューティー比が100%の場合、リニアモータ１０６は正方向へ最大推力を発生し、デューティー比が0%の場合、逆方向へ最大推力を発生する。ボイスコイルモータは、推力の無い状態では保持力が無いので姿勢を傾けられると重力の方向に動いてしまう。そのため、水平姿勢以外への姿勢変化が発生した場合、停止中にもかかわらず、重力による位置ずれを防ぐ目的で推力を発生させ続けるための制御量を出力する位置制御が行われる。

リニアエンコーダ１０７は、リニアモータ１０６の移動量を入力とし、サイン波信号およびコサイン波信号（図２（Ａ）参照）に変換して出力する。ＡＤ入力部１０８は、リニアエンコーダ１０７が出力するサイン波信号およびコサイン波信号をＡＤ変換する。ＡＤ変換値は位置エンコード処理部６０２へ出力される。位置エンコード処理部６０２はＡＤ変換値がサイン波状に変化するのに応じた距離への変換を行う。変換後の信号は加算部６０３および外乱推定オブザーバ６０５に出力される。

目標発生部６０１は、リニアモータ１０６の目標値信号を発生して加算部６０３に出力する。目標値は、リニアモータ１０６が追従すべき目標速度および目標位置の情報を含む。加算部６０３は、目標発生部６０１が出力する目標値を正入力とし、位置エンコード処理部６０２が出力する現在位置を負入力として両者の差を算出して出力する。これはサーボ制御の位置エラーである。ＰＩＤ補償器６０４は、加算部６０３の出力である位置エラーを入力とし、ＰＩＤ制御量を生成して加算部６０６に出力する。Ｐ（比例）Ｉ（積分）Ｄ（微分）制御については公知であるため、詳細な説明は省略する。加算部６０６は外乱推定オブザーバ６０５の出力をＰＩＤ補償器６０４の出力に加算し、加算結果を加算部６０８および制御信号出力部１０４に出力する。

姿勢差補正量の記憶部６０７は、外乱推定オブザーバ６０５での外乱推定の際に、総制御量から姿勢変化へ抗するための制御量を差し引く補正量（姿勢差補正量）を記憶している。この姿勢差補正量は加算部６０８へ負入力として出力される。加算部６０８は加算部６０６の出力である総制御量を正入力とし、総制御量から姿勢差補正量を減算する。減算結果は外乱推定オブザーバ６０５に出力される。

外乱推定オブザーバ６０５は、加算部６０８の出力および位置エンコード処理部６０２の出力を入力とし、リニアモータ１０６に印加した制御量と制御対象が実際に動いた速度との差に基づき外乱推定を行う。外乱推定オブザーバ６０５は推定した外乱に応じた外乱相殺制御量を生成して加算部６０６に出力する。外乱推定オブザーバ６０５の詳細については図７および図９を用いて後述する。加算部６０６は、ＰＩＤ補償器６０４の出力であるＰＩＤ制御量と外乱推定オブザーバ６０５の出力である外乱相殺制御量を加算する。両者の和は総制御量として制御信号出力部１０４へ出力される。
以上のように、目標値に対してリニアモータ１０６の位置を追従させるサーボ制御系が構成される。なお、姿勢差補正量の記憶部６０７に記憶される姿勢差補正量の決定方法については後述する。

次に図７のブロック線図を参照して、外乱推定オブザーバ６０５を詳細に説明する。図中の「ｓ」はラプラス変数を表す。
ブロック７０１は、位置エンコード処理部６０２の出力である位置情報を微分して速度情報へ変換する。ブロック７０２は、ブロック７０１の出力である速度情報を微分した加速度情報と質量情報Mの運動方程式から、リニアモータ１０６の実際の出力を示す実発生推力情報へ変換する。制御サンプル間の移動量、即ち速度から実発生推力情報を求めているため、原理的に目標速度がゼロであって固定位置への位置制御中には、実発生推力情報がゼロとなる。ブロック７０３は、加算部６０８の出力である制御量と、リニアモータ１０６のモデルから設計的に導出される発生推力情報へ変換する。リニアモータ１０６のモデルはＰＷＭのbit分解能数、モータ電源電圧Ｖ、抵抗値Ｒ、リアクタンス値Ｌ、推力変換定数Ｋｆを有し、抵抗値Ｒ、リアクタンス値Ｌについては「１／（ｓＬ＋Ｒ）」の因子として含む。リニアモータ１０６に印加している制御量から導出される発生推力情報を求めているため、原理的に目標速度がゼロであって固定位置への位置制御中でも重力に抗する制御量を印加している場合には相当する推力が発生しているものとして変換される。

加算部７０４は、実発生推力情報と、リニアモータ１０６のモデルから設計的に導出される発生推力情報との差分を、損失した外乱推力であるとして外乱推定結果へ変換する。この差分が外乱推力となるので、傾けられた状態の制御対象を移動目標へ追従させる位置制御中の外乱推定において、姿勢差を考慮せずに総制御量を用いると、速度発生に寄与せず重力に抗するための制御量が全て外乱推力扱いとされてしまう。そこで本実施形態では、姿勢変化で重力に抗するためにリニアモータ１０６に印加している制御量を外乱推定に使用しないように姿勢差補正量として総制御量から差し引く処理を加算部６０８が行う。

ブロック７０５は、加算部７０４の出力である外乱推定結果と、リニアモータ１０６の逆モデルから、外乱推定結果に相当する推力を発生させるための外乱相殺制御量へ変換する。リニアモータ１０６の逆モデルは、ブロック７０３に示すリニアモータ１０６のモデルに対して、ＰＷＭのbit分解能数、モータ電源電圧Ｖ、推力変換定数Ｋｆを逆数として有し、抵抗値Ｒ、リアクタンス値Ｌについては「ｓＬ＋Ｒ」の因子で含む。
以上のように、リニアモータ１０６に印加した制御量と制御対象の移動量から外乱を推定し、損失した推力として外乱を相殺する外乱相殺制御量を出力する外乱推定オブザーバ６０５が構成される。加算部６０６がＰＩＤ制御量と外乱相殺制御量を加算して制御信号出力部１０４に出力することで、ＰＩＤ補償器６０４による制御だけでは補償しきれないリニアモータ制御系の個別特性差、経時変化などの外乱を相殺することができる。本実施形態ではＰＩＤ制御を実施しており、目標速度がゼロであって固定位置への位置制御中でも重力に抗する制御量がＩ制御成分として蓄積して出力される。なお、外乱推定オブザーバと組み合わせが可能な制御器であれば、特にＰＩＤ制御のサーボ補償器に限定されない。

次に図６から図１１を参照して、制御対象の姿勢が変化しても推定状態量の結果が誤推定になる可能性を低減可能な姿勢差補正量の決定処理を説明する。
図８は、図６のブロック線図中でＣＰＵ１０１が実行する位置制御処理を説明するフローチャートである。図９は、図８のＳ８０４の外乱推定オブザーバ処理について詳細を示すフローチャートである。

図８のＳ８０１でＣＰＵ１０１は、目標発生部６０１が目標値をカウント増減中か、またはカウント停止中かを示すカウント状態の情報を取得する。Ｓ８０２でＣＰＵ１０１は目標発生部６０１がカウント停止中であるかどうかを判断する。カウント停止中の場合、Ｓ８０３へ処理を進め、カウント増減中の場合、Ｓ８０４へ移行する。カウント増減中とは、目標速度が０以外で制御対象を移動目標へ追従させる位置制御が指示されていることである。また、カウント停止中とは、目標速度が０であって制御対象を固定位置で停止させ続ける位置制御が指示されていることである。

Ｓ８０３でＣＰＵ１０１は外乱相殺制御量を０とし、外乱推定オブザーバ処理回数（以下、単に処理回数という）を０として設定する。原理的には目標速度が０で固定位置への位置制御中に実発生推力情報として０となって外乱推定ができないので、外乱相殺制御量を０に設定することで実質的に外乱推定オブザーバ６０５がOFF状態となる。一方、Ｓ８０４にてＣＰＵ１０１は、外乱推定オブザーバ６０５による外乱推定オブザーバ処理を実行する。外乱推定オブザーバ処理の詳細については図９を用いて後述する。

Ｓ８０３またはＳ８０４の後、Ｓ８０５に進んでＣＰＵ１０１は目標発生部６０１の目標値と、位置エンコード処理部６０２で処理された現在の位置を取得し、加算部６０３により目標値と現在位置との差から位置エラーを算出する。ＰＩＤ補償器６０４は位置エラーを入力とするＰＩＤ制御量を算出する。Ｓ８０６でＣＰＵ１０１は目標発生部６０１がカウント停止中であるかどうかを判断する。カウント停止中の場合、Ｓ８０７へ処理を進め、またカウント増減中の場合、Ｓ８０８へ移行する。Ｓ８０７でＣＰＵ１０１は次回の外乱推定オブザーバ処理用に姿勢差補正量として、Ｓ８０５で求めた現在のＰＩＤ制御量を設定する。カウント停止中には、例えば、制御対象の姿勢が水平状態であればＰＩＤ制御量が０となり、デューティー比が50%となる。姿勢変化がある状態であれば当該姿勢変化に抗して位置を維持するためにデューティー比が50%以外のＰＩＤ制御量となる。このように制御対象が停止しているだけで速度発生に寄与せず重力に抗するためだけに必要な制御量が姿勢差補正量として記憶部６０７に記憶される。

図１０は位置制御の動作目標例を説明するタイミングチャートである。図１０（Ａ）の横軸は時間軸であり、縦軸は制御対象の姿勢を示し、中央は水平を表す。図１０（Ｂ）の横軸は時間軸（時刻T0からT7参照）であり、縦軸は目標位置（P0からP4参照）を示す。図１１は動作中の数値変化例を説明する図である。（Ａ）から（Ｊ）まで姿勢と、進行方向の異なる９つのパターンを例示する。

図１０（Ａ）では、時刻T0からT1までの期間に制御対象の姿勢が水平姿勢である。この期間にて制御対象が図１０（Ｂ）の位置P1やP2の停止目標値に追従している状態で、図１１（Ａ）のように停止中のＰＩＤ制御量が0であれば、姿勢差補正量は0である。つまり、外乱推定オブザーバ６０５はOFF状態である。その後、時刻T1からT4までの期間に傾斜１の向きへの姿勢変化があった場合を想定する。この場合、制御対象が図１０（Ｂ）の位置P1の停止目標値に追従している時刻T2で、図１１（Ｄ）のように停止中のＰＩＤ制御量が「-20」であれば、姿勢差補正量として「-20」が記憶部６０７に記憶される。更に、制御対象が傾斜１の向きへの姿勢で、時刻T3にて図１０（Ｂ）の位置P2の停止目標値に追従している時刻T3にて停止中でのＰＩＤ制御量が「-25」であれば、姿勢差補正量として「-25」が記憶部６０７に記憶される。

その後、図１０（Ａ）に示す時刻T5から、傾斜１とは逆の傾斜２の向きへの姿勢変化があった場合を想定する。この場合、制御対象が図１０（Ｂ）の位置P4の停止目標値に追従している時刻T6で、図１１（Ｇ）のように停止中のＰＩＤ制御量が「20」であれば、姿勢差補正量として記憶部６０７に「20」が記憶される。更に制御対象が傾斜２の向きへの姿勢で、時刻T7にて図１０（Ｂ）の位置P3の停止目標値に追従している時刻T7にて停止中でのＰＩＤ制御量が「25」であれば、姿勢差補正量として「25」が記憶部６０７に記憶される。

図８のＳ８０６でカウント停止中でない場合、またはＳ８０７の後、Ｓ８０８に処理を進め、加算部６０６によりＰＩＤ制御量と外乱相殺制御量が加算されて総制御量が算出される。総制御量は加算部６０６から制御信号出力部１０４へ出力される。次のＳ８０９でＣＰＵ１０１は、次回の外乱推定オブザーバ処理用に総制御量をＲＡＭ１０３に保存して処理を終える。
以上のように制御対象の姿勢変化があった場合でも最新のカウント停止中におけるＰＩＤ制御量が姿勢差補正量として記憶部６０７に保存される。

次に、図９のフローチャートを参照して、図８のＳ８０４に示す外乱推定オブザーバ処理を説明する。
Ｓ９０１でＣＰＵ１０１は位置エンコード処理部６０２で処理された現在の位置を取得し、Ｓ９０２で処理回数に１を加算してＳ９０３へ処理を進める。Ｓ９０３は処理回数が２回以上であるかどうかの判定処理である。ＣＰＵ１０１は処理回数が２回以上であると判定した場合、Ｓ９０４へ処理を進め、２回未満であれば制御対象の移動量を計測できないのでＳ９０９に移行する。

Ｓ９０４にてＣＰＵ１０１は、ＲＡＭ１０３に保存されている前回出力の総制御量、現在の姿勢差補正量、前回保存の位置（前回位置）を取得する。前回出力の総制御量とは図８のＳ８０９でＲＡＭ１０３に保存されたデータの示す総制御量である。前回位置とは後述のＳ９０９でＲＡＭ１０３に保存されたデータの示す位置である。Ｓ９０５でＣＰＵ１０１は、現在の位置と前回位置との差分、即ち移動量から速度を推定する。これは図７のブロック７０１での変換に相当する。推定速度とリニアモータ１０６の逆モデルに基づいて、実際に発生していると推定される実発生推力が算出される。これは図７のブロック７０２での変換に相当する。

次のＳ９０６において加算部６０８は加算部６０６の出力する総制御量から姿勢差補正量（記憶部６０７の記憶値）を減算する。減算結果である制御量とリニアモータ１０６のモデルから、当該制御量を与えた場合に発生すると期待される発生推力（以下、期待発生推力という）が推定される。これは図７のブロック７０３での変換に相当する。

図１０（Ａ）のように、制御対象の姿勢が水平姿勢であった時刻T0からT1までの期間で、制御対象が図１０（Ｂ）の位置P1からP2への移動目標値に追従している場合を想定する。この期間にて図１１（Ｂ）のように移動中のＰＩＤ制御量が「10」の時点で、外乱推定オブザーバ処理に使用する制御量は、総制御量から姿勢差補正量を引いた値である。つまり、総制御量はＰＩＤ制御量「10」と外乱相殺制御量の和であり、姿勢差補正量がゼロであるので、実質的には、現在の総制御量が全て推定に使われる。同様に時刻T0からT1までの期間で、制御対象が図１０（Ｂ）の位置P2からP1への移動目標値に追従している場合を想定する。この期間にて図１１（Ｃ）のように移動中のＰＩＤ制御量が「-10」の時点で外乱推定オブザーバ処理に使用する制御量は、総制御量（ＰＩＤ制御量「-10」と外乱相殺制御量との和）から姿勢差補正量を引いた値である。この場合には姿勢差補正量がゼロであるので、実質的には、現在の総制御量が全て推定に使われる。

また、図１０（Ａ）に示す時刻T1からT4までの期間では、制御対象について傾斜１の向きへの姿勢変化が生じており、制御対象が図１０（Ｂ）の時刻T2以降の位置P1からP2への移動目標値に追従している場合を想定する。この期間にて図１１（Ｅ）のように移動中のＰＩＤ制御量が「-10」の時点で外乱推定オブザーバ処理に使用する制御量は、総制御量（ＰＩＤ制御量「-10」と外乱相殺制御量の和）から姿勢差補正量「-20」を引いた値になる。このように傾斜１の向きへの位置ずれを引き起こさないための制御量「-20」は姿勢差補正量として総制御量から減算され、実質的に速度を発生させるための制御量「+10」が外乱推定に使われる。

更に制御対象の姿勢が傾斜１の向きになった状態で、図１０（Ｂ）の位置P2の停止目標値に追従している時刻T3の後に図１０（Ｂ）の位置P2からP1への移動目標値に制御対象が追従している場合を想定する。この場合、図１１（Ｆ）のように移動中のＰＩＤ制御量が「-35」の時点で外乱推定オブザーバ処理に使用する制御量は、総制御量（ＰＩＤ制御量「-35」と外乱相殺制御量の和）から姿勢差補正量「-25」を引いた値になる。このように傾斜１の向きへの位置ずれを引き起こさないための制御量「-25」は姿勢差補正量として総制御量から減算され、実質的に速度を発生させるための制御量「-10」が外乱推定に使われる。

図１０（Ａ）に示す時刻T5以降には傾斜２の向きへの姿勢変化が起きており、制御対象が図１０（Ｂ）の時刻T6以降の位置P4からP3への移動目標値に追従している場合を想定する。この場合、図１１（Ｈ）のように移動中のＰＩＤ制御量が「30」の時点で外乱推定オブザーバ処理に使用する制御量は、総制御量（ＰＩＤ制御量「30」と外乱相殺制御量の和）から姿勢差補正量「20」を引いた値になる。傾斜２の向きへの位置ずれを引き起こさないための制御量「20」は総制御量から減算され、実質的に速度を発生させるための制御量「10」が外乱推定に使われる。更に、傾斜２の向きになった状態での制御対象が図１０（Ｂ）の位置P3の停止目標値に追従している時刻T7の後に図１０（Ｂ）の位置P3からP4への移動目標値に追従している場合を想定する。この場合、図１１（Ｊ）のように移動中のＰＩＤ制御量が「15」の時点で外乱推定オブザーバ処理に使用する制御量は、総制御量（ＰＩＤ制御量「15」と外乱相殺制御量の和）から姿勢差補正量「25」を引いた値になる。傾斜２の向きへの位置ずれを引き起こさないための制御量「25」が総制御量から減算され、実質的に速度を発生させるための制御量「-10」が外乱推定に使われる。

図９のＳ９０７にてＣＰＵ１０１は、Ｓ９０６で算出した期待発生推力の推定値と、Ｓ９０５で算出した実発生推力の推定値との差分から、リニアモータ１０６の内部で失われた外乱を推定する。これは図７の加算部７０４による減算処理に相当する。次のＳ９０８でＣＰＵ１０１は、Ｓ９０７で算出した外乱推定値から、それを相殺する推力を発生させるための外乱相殺制御量を算出する。これは図７のブロック７０５での変換に相当する。Ｓ９０９でＣＰＵ１０１は位置エンコード処理部６０２から取得した現在の位置を、次回の外乱推定オブザーバ処理用としてＲＡＭ１０３に保存して処理を終了する。保存されたデータの示す位置が前回位置として次回の処理で使用される。

例えば、位置制御装置の適用例としてカメラのレンズ位置制御装置について説明する。チルティングという撮影手法中には、ユーザがカメラをゆっくりと上下方向に振る操作を行うので姿勢変化が発生する。また、地面の花などの被写体になるべく接近して撮影する接写撮影中においても、カメラが長時間の下向きや上向きの姿勢をとる場合があるので姿勢変化が発生する。撮影動作中には、数秒単位のゆっくりとした周期で姿勢が変化するのに対し、位置制御では数ミリ秒単位の高速な周期で駆動と停止が繰り返される。可動光学部材であるレンズの移動目標値への追従中、即ち外乱推定中の姿勢変化は推定ずれの原因となるが、本実施形態では推定誤差を低減することができる。これは前述したように、直近の停止期間での姿勢差補正量を総制御量から減算する効果に依るものであり、姿勢変化を考慮しない場合に比べて推定誤差が少ない。

本実施形態では、固定位置への位置制御中に制御対象の姿勢変化へ抗するために出力している制御量を記憶部に保存し、移動位置への位置制御中には記憶部に保存された制御量を総制御量から減算して外乱推定を行う。よって、制御対象の姿勢変化へ抗するための速度発生に寄与しない制御量が除去されるので、姿勢変化による誤推定の可能性を低減し、位置サーボ制御を安定化させることができる。

［第２実施形態］
次に本発明の第２実施形態を説明する。本実施形態では、目標速度が０で固定位置への位置制御中の外乱推定オブザーバの出力を姿勢差補正量として記憶部に保存し、記憶データを、目標速度が０以外で制御対象を移動目標へ追従させる位置制御中の外乱推定に使用する例を説明する。第１実施形態の場合と同様の構成部については既に使用した符号を用いることで、それらの詳細な説明を省略し、主として相違点を説明する。

図１、図６、図１２から図１４を参照し、制御対象の姿勢が変化しても推定状態量の結果が誤推定になる可能性を低減するための、姿勢差補正量の決定処理を説明する。
図１２は、図６に示す制御系のブロック線図にてＣＰＵ１０１が実行する位置制御処理を説明するフローチャートである。

Ｓ１２０１では目標発生部６０１が目標値をカウント増減中か、またはカウント停止中かを示すカウント状態の情報が取得される。次のＳ１２０２でＣＰＵ１０１は目標発生部６０１がカウント停止中であるかどうかを判断する。カウント停止中の場合、Ｓ１２０３へ処理を進め、カウント増減中の場合、Ｓ１２０４へ移行する。Ｓ１２０３で処理回数を１に設定し、姿勢差補正量をゼロに設定する処理を行い、Ｓ１２０４へ処理を移す。Ｓ１２０４では外乱推定オブザーバ６０５が外乱推定オブザーバ処理を行う。この処理の詳細については図１４を用いて後述する。Ｓ１２０５へ処理を移し、ＣＰＵ１０１は目標発生部６０１がカウント停止中であるかどうかを判断する。カウント停止中の場合、Ｓ１２０６へ処理を進め、カウント増減中の場合、Ｓ１２０７へ移行する。Ｓ１２０６にてＣＰＵ１０１は、姿勢差補正量の記憶部６０７に外乱相殺制御量を記憶させた後、外乱相殺制御量をゼロに設定し、Ｓ１２０７へ処理を移す。

Ｓ１２０７では目標発生部６０１の目標値と、位置エンコード処理部６０２で処理された現在の位置が取得され、加算部６０３が目標値と現在位置との差から位置エラーを算出する。ＰＩＤ補償器６０４は位置エラーを入力としてＰＩＤ制御量を算出する。Ｓ１２０８で加算部６０６はＰＩＤ制御量と外乱相殺制御量を加算して総制御量とし、制御信号出力部１０４へ出力する。Ｓ１２０９でＣＰＵ１０１は次回の外乱推定オブザーバ処理用に総制御量をＲＡＭ１０３に保存して処理を終える。

図１３は本実施形態の外乱推定オブザーバ６０５を詳細に示すブロック線図である。
ブロック１３０１は、位置エンコード処理部６０２の出力である位置情報をハイパスフィルタ（ＨＰＦ）処理して速度情報へ変換する。ブロック１３０２は、ブロック１３０１の出力である速度情報とモータ情報の運動方程式からリニアモータ１０６が実際に出力している実発生推力情報へ変換する。ブロック１３０３は、加算部６０８の出力である制御量をローパスフィルタ（ＬＰＦ）処理する（以下、第１ＬＰＦ処理という）。

ブロック１３０４は、ブロック１３０３の出力とリニアモータ１０６のモデルから設計的に導出される発生推力情報へ変換する。リニアモータ１０６のモデルは、ＰＷＭのbit分解能数、モータ電源電圧Ｖ、抵抗値Ｒ、推力変換定数Ｋｆを有する。加算部１３０５は、ブロック１３０２の出力する実発生推力情報と、ブロック１３０４の出力する発生推力情報とを加算し、加算結果をブロック１３０６へ出力する。ブロック１３０６は、加算部１３０５の出力をローパスフィルタ処理する（以下、第２ＬＰＦ処理という）。Ｔは第２ＬＰＦ処理における１次ＬＰＦ「１／（ｓＴ＋１）」のフィルタ時定数である。

加算部１３０７は、第２ＬＰＦ処理された出力情報（低域通過情報）を正入力とし、ブロック１３０２の出力する実発生推力情報を負入力して、両者の差分を算出する。差分は、損失した外乱推力の低周波数成分として外乱推定結果へ変換される。差分が外乱推力となるので、傾けられた状態で制御対象が移動目標へ追従する位置制御中の外乱推定にて姿勢差を考慮せずに総制御量を用いると、速度発生に寄与せず重力に抗するための制御量が全て外乱推力扱いとされてしまう。そこで本実施形態では、姿勢変化で重力に抗するためにリニアモータ１０６に印加している制御量を外乱推定に使用しないように、姿勢差補正量として総制御量から差し引く処理を行う。ブロック１３０８は、加算部１３０７の出力である外乱推定結果とリニアモータ１０６の逆モデルから、前記低周波数成分の外乱推定結果に相当する推力を発生させるための外乱相殺制御量へ変換する。リニアモータ１０６の逆モデルは、ブロック１３０４に示すリニアモータ１０６のモデルに対して、ＰＷＭのbit分解能数、モータ電源電圧Ｖ、抵抗値Ｒ、推力変換定数Ｋｆを逆数として有する。
以上のように外乱推定オブザーバは、リニアモータ１０６に印加した制御量および検出された移動量から外乱を推定し、損失した推力として外乱を相殺する外乱相殺制御量を出力する。図１４を参照して外乱推定オブザーバ処理の詳細を説明する。

図１４は、図１２のＳ１２０４に示す外乱推定オブザーバ処理の詳細を説明するフローチャートである。
Ｓ１４０１においてＣＰＵ１０１は、前回出力の総制御量を取得して第１ＬＰＦ処理を実行する。これは図１３のブロック１３０３での変換に相当する。Ｓ１４０２でＣＰＵ１０１は現時点にて外乱推定オブザーバ処理周期であるかどうかを判断する。例えば、位置制御周期と同じ周期で外乱推定オブザーバ処理を行うと外乱推定結果に高域ノイズが多い場合には、回避策として位置制御周期のＮ分の１の周期で外乱推定オブザーバ処理が実行される。Ｓ１４０２で判定されるタイミングが外乱推定オブザーバ処理周期であればＳ１４０３へ処理を進め、外乱推定オブザーバ処理周期でなければ処理を終了する。

Ｓ１４０３では、位置エンコード処理部６０２で処理された現在の位置を取得してハイパスフィルタ処理が実行される。これは図１３のブロック１３０１での変換に相当する。次のＳ１４０４でＣＰＵ１０１は処理回数に１を加算した後、Ｓ１４０５では処理回数が２回以上であるかどうかを判断する。処理回数が２回以上の場合、Ｓ１４０６へ処理を進め、２回未満であれば処理を終了する。Ｓ１４０６でＣＰＵ１０１は第１ＬＰＦ処理後の総制御量と、現在の姿勢差補正量を取得し、Ｓ１４０７では目標発生部６０１がカウント停止中であるかどうかを判断する。カウント停止中の場合、Ｓ１４０８へ処理を移し、カウント増減中の場合、Ｓ１４０９へ移行する。

Ｓ１４０８にてＣＰＵ１０１は実発生推力の推定結果をゼロとした後、Ｓ１４１０へ処理を進める。Ｓ１４０９にてＣＰＵ１０１はハイパスフィルタ処理後の位置情報から速度を推定する。推定速度とリニアモータ１０６の逆モデルから、実際に発生しているとみられる実発生推力が推定される。これは図１３のブロック１３０２での変換に相当する。Ｓ１４１０において、ＣＰＵ１０１は期待発生推力を推定する。期待発生推力とは、Ｓ１４０６で取得した総制御量から姿勢差補正量（記憶部６０７の記憶値）を引いた制御量と、リニアモータ１０６のモデルから、当該制御量を与えたならば発生することが期待される発生推力である。期待発生推力の推定は、図１３のブロック１３０４での変換に相当する。

次にＳ１４１１にてＣＰＵ１０１は低周波数域の外乱を推定する。つまり、期待発生推力の推定値と実発生推力の推定値との加算値を時定数Ｔの第２ＬＰＦ処理した値と、実発生推力の推定値との差分から、リニアモータ１０６の内部で失われた時定数Ｔ以下での低周波数域の外乱が推定される。これは図１３の加算部１３０５とブロック１３０６と加算部１３０７での処理に相当する。Ｓ１４１２でＣＰＵ１０１は低周波数域の外乱推定値を相殺する推力を発生させるための外乱相殺制御量を算出し、処理を終了する。これは図１３のブロック１３０８での変換に相当する。
以上のように制御対象の姿勢変化が生じた場合でも最新の停止中の外乱相殺制御量が姿勢差補正量として記憶部６０７に保存される。

本実施形態では、固定位置への位置制御中に姿勢変化へ抗するために出力しようした外乱相殺制御量を記憶部に保存し、移動位置への位置制御中には保存された外乱相殺制御量を総制御量から減算して外乱推定を行う。このように姿勢変化へ抗するための速度発生に寄与しない制御量を除去することにより、姿勢変化による誤推定の可能性を低減し、位置サーボ制御を安定化させる効果が得られる。

１００ 位置制御装置
１０１ ＣＰＵ
１０２ ＲＯＭ
１０３ ＲＡＭ
１０６ リニアモータ
１０７ リニアエンコーダ
６０１ 目標発生部
６０４ ＰＩＤ補償器
６０５ 外乱推定オブザーバ
６０７ 姿勢差補正量の記憶部

本発明に係る装置は、位置制御装置であって、制御対象の検出位置に応じて、前記制御対象を目標位置に追従させる制御量を算出する算出手段と、前記算出手段により算出された制御量、または当該制御量と前記制御対象の移動量に応じた外乱相殺制御量とを加算した総制御量、に応じて前記制御対象を制御する制御手段と、前記制御対象が停止状態の場合の当該停止状態を維持するための制御量である姿勢差補正量を記憶する記憶手段と、を備える。前記外乱相殺制御量は、さらに、前記制御対象を移動目標へ追従させる位置制御中に前記総制御量から前記記憶手段の記憶している姿勢差補正量を演算した制御量に応じた制御量である。

Claims (9)

1. 外乱推定オブザーバを備える位置制御装置であって、
   制御対象の位置を検出する位置検出手段と、
   前記制御対象の目標位置を出力する目標発生手段と、
   前記制御対象を目標位置に追従させる制御量を算出する算出手段と、
   前記算出手段により算出された制御量または当該制御量と前記外乱推定オブザーバにより算出された外乱相殺制御量とを加算した総制御量を出力して前記制御対象を制御する制御信号出力手段と、
   前記制御対象の固定位置への位置制御中に、前記制御対象の姿勢変化へ抗する制御量である姿勢差補正量を記憶する記憶手段と、を備え、
   前記外乱推定オブザーバは、前記制御対象を移動目標へ追従させる位置制御中に前記総制御量から前記記憶手段の記憶している姿勢差補正量を減算した制御量および前記制御対象の移動量から外乱を推定し、外乱を相殺する前記外乱相殺制御量を算出することを特徴とする位置制御装置。
2. 前記記憶手段は、前記制御対象の固定位置への位置制御中に前記算出手段が算出した制御量を前記姿勢差補正量として記憶することを特徴とする請求項１に記載の位置制御装置。
3. 前記記憶手段は、前記制御対象の固定位置への位置制御中に前記外乱推定オブザーバが算出した外乱相殺制御量を前記姿勢差補正量として記憶することを特徴とする請求項１に記載の位置制御装置。
4. 前記制御対象は、推力の未発生時に保持力が無いアクチュエータにより駆動されることを特徴とする請求項１ないし３のいずれか１項に記載の位置制御装置。
5. 前記制御対象の固定位置への位置制御中に、前記制御信号出力手段は前記外乱相殺制御量を加算せずに前記算出手段の出力する制御量を前記制御対象に対して出力することを特徴とする請求項１ないし４のいずれか１項に記載の位置制御装置。
6. 前記制御対象の固定位置への位置制御中に、前記制御信号出力手段は前記外乱相殺制御量を加算せずに前記算出手段の出力する制御量を前記制御対象に対して出力し、前記外乱推定オブザーバは、前記制御対象の移動量をゼロとして前記総制御量から外乱を推定することを特徴とする請求項３に記載の位置制御装置。
7. 請求項１ないし６のいずれか１項に記載の位置制御装置を備えることを特徴とする光学機器。
8. 請求項１ないし６のいずれか１項に記載の位置制御装置を備えることを特徴とする撮像装置。
9. 外乱推定オブザーバを備える位置制御装置にて実行される位置制御方法であって、
   位置検出手段が制御対象の位置を検出するステップと、
   目標発生手段が前記制御対象の目標位置を出力するステップと、
   算出手段が前記制御対象を目標位置に追従させる制御量を算出するステップと、
   前記外乱推定オブザーバが外乱を推定して外乱相殺制御量を算出するステップと、
   前記算出手段により算出された制御量または当該制御量と前記外乱推定オブザーバにより算出された外乱相殺制御量とを加算した総制御量を制御信号出力手段が出力して前記制御対象を制御するステップと、
   前記制御対象の固定位置への位置制御中に、前記制御対象の姿勢変化へ抗する制御量である姿勢差補正量を記憶手段が記憶するステップと、を有し、
   前記外乱相殺制御量を算出するステップにて前記外乱推定オブザーバは、前記制御対象を移動目標へ追従させる位置制御中に前記総制御量から前記記憶手段の記憶している姿勢差補正量を減算した制御量および前記制御対象の移動量から外乱を推定し、外乱を相殺する前記外乱相殺制御量を算出することを特徴とする位置制御方法。
   
   

Images