JP4987269B2 - 速度検出装置およびサーボモータ - Google Patents

Description

本発明は、速度検出装置およびサーボモータに関する。

サーボモータの運動制御では、位置検出器が機械の移動距離または軸の移動角度に基づいたＳＩＮ信号およびＣＯＳ信号を出力する。位置検出器の演算回路は、ＳＩＮ信号およびＣＯＳ信号を単位時間ごとにサンプリングし、２つのサンプル間の距離差または角度差と２回のサンプリングの時間間隔とから移動速度または角速度を算出していた。

例えば、図８（Ａ）および図８（Ｂ）に示すように２回のサンプリングで得られた座標を（ｘ１，ｙ１）および（ｘ２，ｙ２）とし、サンプリングの時間間隔をｔ（ｓｅｃ）とすると、角速度ω（ｒａｄ／ｓｅｃ）は式１のように表わすことができる。
ω＝（θ２−θ１）／ｔ＝（ｔａｎ^−１（ｙ２／ｘ２）−ｔａｎ^−１（ｙ１／ｘ１））／ｔ （式１）
ここで、θ１（ｒａｄ）およびθ２（ｒａｄ）は、２回のサンプリングの時刻における絶対角度（偏角）である。式１に示すように、角速度ωを得るためには、座標から偏角を算出しなければならない。
杉本 英彦等"ＡＣサーボシステムの理論と設計の実際"総合電子出版社 （Ｐ１４２〜Ｐ１４９）

上記問題に対処するために、演算回路としてＣＰＵあるいはカスタムＬＳＩ（Large-Scale Integrated circuit）を用い、デジタルロジック回路で式１の演算を実現することが考えられる。

しかし、アークタンジェント（ｔａｎ^−１）は、ｓｉｎ／ｃｏｓという割り算を必要とする。アークタンジェントの計算には次の２つの問題点がある。その１つの問題点は、単位円上のｃｏｓ信号が０に近づくと、タンジェント（ｓｉｎ÷ｃｏｓ）の値が絶対値が非常に大きくなることである。その結果、アークタンジェントの結果の誤差が非常に大きくなってしまう。

もう１つの問題点は、アークタンジェントおよびタンジェントを求めるために割り算（ｓｉｎ÷ｃｏｓ）が必要になることである。割り算は、デジタル回路で実現することが困難な回路であり、実現するためには乗算や加算に比べてかなり大規模なデジタル回路を用いる必要がある。

そこで、本発明は、小さな演算回路で移動速度または角速度を高精度に検出することができる速度検出回路およびサーボモータを提供することである。

本発明に係る実施形態に従った速度検出装置は、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出する速度検出装置であって、
予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、前記第１の座標と前記三角関数の値との乗算、加算または減算によって表された計算上の第２の座標を前記現実の第２の座標に接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度に基づいて前記運動体の速度を演算する演算部と、前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標が座標面のいずれの領域に属するかを判別する判別部と、前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標のそれぞれが属する領域に基づいて、前記演算部が実行する演算式を選択する比較部とを備えている。

本発明に係る実施形態に従ったサーボモータは、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出するサーボモータであって、
予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、前記第１の座標と前記三角関数の値との乗算、加算または減算によって表現された計算上の第２の座標を前記現実の第２の座標へ接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度を積算することにより前記運動体の速度を検出する演算部と、前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標が座標面のいずれの領域に属するかを判別する判別部と、前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標のそれぞれが属する領域に基づいて、前記演算部が実行する演算式を選択する比較部とを備えている。

本発明による速度検出回路およびサーボモータは、小さな演算回路で移動速度または角速度を高精度に検出することができる。

以下、図面を参照して本発明に係る実施形態を説明する。本実施形態は、本発明を限定するものではない。

図１は、本発明に係る実施形態に従った速度検出装置１００のブロック図である。速度検出装置１００は、ロータリエンコーダまたはリニアスケール１０と、Ａ／Ｄコンバータ４０と、演算部５０とを備えている。演算部５０は、汎用ＣＰＵまたはカスタムＬＳＩである。演算部５０は、例えば、ＦＰＧＡ（Field Programmable Gate Array）でよい。速度検出装置１００は、例えば、回転運動または往復運動を行う運動体を備えたサーボモータに配設されてもよい。運動体は、例えば、工作機械に配備されたサーボモータのロータまたは往復運動するアーム等である。

ロータリエンコーダまたはリニアスケール１０は、回転運動または往復運動を行う運動体の位置を示す座標（ｓｉｎ，ｃｏｓ）の信号を出力する。Ａ／Ｄコンバータ４０は、ロータリエンコーダまたはリニアスケール１０からのｓｉｎ信号およびｃｏｓ信号をデジタル信号へ変換する。さらに、演算部５０はＡ／Ｄコンバータ４０からのｓｉｎ信号およびｃｏｓ信号のそれぞれに基づくデジタル信号を受け、運動体の角速度または速度を算出する。

演算部５０は、ＦＰＧＡでよいが、ＦＰＧＡに代えて、他の任意のＬＳＩを用いることができる。また、演算部５０は、汎用ＣＰＵおよびソフトウェアで実現してもよい。

図２は、演算部５０の内部構成を示すブロック図である。演算部５０は、ＣＯＲＤＩＣローテーション５１と、メモリ５２と、比較器５３と、判別部５４、５５と、選択部５６と、第１のレジスタＲ１１と、第２のレジスタＲ２１と、第３のレジスタＲ３１と、第４のレジスタとしての角度積算レジスタＲ４１と、角速度保持レジスタＲ５１と、加算器８０を備えている。

図２を参照し、ｓｉｎ信号およびｃｏｓ信号から角速度を算出する過程を説明する。角速度を算出するためにＣＯＲＤＩＣ（COordinate Rotation Digital Computer）アルゴリズムが利用される。ＣＯＲＤＩＣアルゴリズムには、ベクタリングモードおよびローテーションモードがある。本実施形態は、ローテーションモードを利用する。このＣＯＲＤＩＣアルゴリズムは、演算部５０内に内蔵されており、ロジック回路によって実現されている。

本実施形態では、理解を容易にするために、運動体としてのロータが１回転すると、ロータリエンコーダ１０が１サイクルのｓｉｎ波およびｃｏｓ波を出力するものとする。速度検出装置１００は、１サイクルの間に周期的にロータの座標をサンプリングする。速度検出装置１００は、第1のサンプリングでロータの２次元座標（ユークリッド座標）（ｘ１，ｙ１）を獲得し、第1のサンプリングの次の第２のサンプリングで一定時間後の２次元座標（ｘ２，ｙ２）を獲得する。（ｘ１およびｘ２はｓｉｎ値であり、ｙ１およびｙ２はｃｏｓ値である。速度検出装置１００は、座標（ｘ１，ｙ１）および座標（ｘ２，ｙ２）に基づいて第1のサンプリング時におけるロータの位置から第２のサンプリング時のロータの位置までの角度ΦをＣＯＲＤＩＣアルゴリズムによって計算する。サンプリングは周期的に行われるので、この角度Φが判明すると、ロータの角速度が得られる。

メモリ５２は、予め設定された複数の仮の角度Φ_０〜Φ_ｎ（Φ_０＞Φ_１＞Φ_２＞・・・Φ_ｎ）（ｎは自然数）、および、該複数の仮の角度のそれぞれに対する三角関数の値ｃｏｓΦ_０〜ｃｏｓΦ_ｎ、および、ｔａｎΦ_０〜ｔａｎΦ_ｎを記憶している。速度検出装置１００の初期状態において、第１から第３のレジスタは特定値を格納していない。

まず、演算部５０は、第１のサンプリングで得られたロータの第１の座標（ｘ１，ｙ１）＝（ｓｉｎθ，ｃｏｓθ）を、Ａ／Ｄコンバータ４０からデジタル値で受け取る。第１の座標は、第２のレジスタＲ２１に格納される。

当初、第１のレジスタＲ１および第３のレジスタＲ３１は不定である。このときの計算値は、不定値に基づくので排除する。

次に、演算部５０は、次の周期において第２のサンプリングを行う。演算部５０は、第２のサンプリングで得られたロータの現実の第２の座標（ｘ２，ｙ２）＝（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））を、Ａ／Ｄコンバータ４０からデジタル値で受け取る。第２の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））は、第２のレジスタＲ２１に格納される。このとき、第１のレジスタＲ１１は、第１の座標（ｓｉｎθ，ｃｏｓθ）を格納している。

図３は、第１の座標（ｓｉｎθ，ｃｏｓθ）および現実の第２の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））の位置関係を示す図である。現実の第２の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））は、図３に示すように第１の座標（ｓｉｎθ，ｃｏｓθ）を基準としてΦだけロータを回転させたときの現実の座標である。尚、θおよびΦの単位は、ｒａｄ（ラジアン）である。図３の紙面において、ロータはＣＣＷ（Counter Clock Wise）方向（図３の矢印の方向）に回転している。

現実の第２の座標を受けたときには、第３のレジスタＲ３１はまだ不定であるので、図２の選択部５６は、第１のレジスタＲ１１を選択し、第１の座標（ｓｉｎθ，ｃｏｓθ）を判別部５４およびＣＯＲＤＩＣローテーション５１へ送る。一方、第２のレジスタＲ２１は、現実の第２の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））を判別部５５およびＣＯＲＤＩＣローテーション５１へ送る。

ＣＯＲＤＩＣローテーション５１は、第１の座標（ｘ１，ｙ１）＝（ｓｉｎθ，ｃｏｓθ）、および、メモリ５２からの三角関数の値ｃｏｓΦ_０〜ｃｏｓΦ_ｎ、ｔａｎΦ_０〜ｔａｎΦ_ｎを用いて計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を計算する。計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）は、ＣＯＲＤＩＣローテーション５１が角度Φを求めるときに、現実の第２の座標（ｘ２，ｙ２）に収束するように繰り返し計算される座標である。

第３のレジスタＲ３１は、この計算の途中において、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を格納する。判別部５５は第２のレジスタＲ２１に格納された現実の第２の座標が属する領域を判別する。判別部５４は、選択部５６によって選択された第１の座標または計算上の第２の座標が属する領域を判別する。計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）は式１または式２のように表わすことができる。比較器５３は、判別部５４および５５の判別結果に基づいて式１または式２のいずれかを選択する。ＣＯＲＤＩＣローテーション５１は、比較器５３の比較結果に基づいて式１または式２の演算を実行する。

但し、Φ_ｉ＝ｔａｎ^−１２^−ｉ（ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、ｘｃ_ｉ−１＝ｘ１、ｙｃ_ｉ−１＝ｙ１である。

式１および式２ついて説明する。まず、第1の座標および現実の第２の座標は式３〜式６のように表わすことができる。
ｘ１＝ｃｏｓθ （式３）
ｙ１＝ｓｉｎθ （式４）
ｘ２＝ｃｏｓ（θ＋Φ） （式５）
ｙ２＝ｓｉｎ（θ＋Φ） （式６）
式５および式６を三角関数の加法定理を用いて変形すると、式７および式８となる。
ｘ２＝ｃｏｓθ・ｃｏｓΦ−ｓｉｎθ・ｓｉｎΦ （式７）
ｙ２＝ｓｉｎθ・ｃｏｓΦ＋ｃｏｓθ・ｓｉｎΦ （式８）
式７および式８に式３および式４を代入し、変形すると、式９および式１０が得られる。
ｘ２＝ｃｏｓΦ（ｘ１−ｙ１・ｔａｎΦ） （式９）
ｙ２＝ｃｏｓΦ（ｙ１＋ｘ１・ｔａｎΦ） （式１０）

ここで、ＣＯＲＤＩＣアルゴリズムを利用する。より詳細には、ｔａｎΦの値を式１１のように±２^−ｉに制限する。このように制限されたｔａｎΦをｔａｎΦｉとする。
ｔａｎΦｉ＝±２^−ｉ＝±１，±２^−１，±２^−２，・・・ （ｉ＝０、１、２、・・・ｎ） （式１１）

このとき、Φｉが取り得る値は、式１２のように制限される。この角度Φｉは離散的な数値であり、以下、“仮の角度Φｉ”という。
Φｉ＝ｔａｎ^−１（±２^−ｉ）＝±０．７８ｒａｄ，±０．４６ｒａｄ，±０．２５ｒａｄ，±０．１２ｒａｄ・・・ （式１２）

また、ｔａｎΦｉが式１１のように決定されると、自ずとｃｏｓΦｉが式１３のように決定される。
ｃｏｓΦｉ＝ｃｏｓ（ｔａｎ^−１（±２^−ｉ））＝０．７１，０．８９，０．９７，０．９９・・・ （式１３）

式９および式１０に仮の角度Φｉを代入すると計算上の第２の座標が得られる。尚、式１は、仮の角度Φｉが正である場合に成り立ち、式２は、仮の角度Φｉが負である場合に成り立つ。

式１１から式１３に示す各ｉに対する離散的な数値は、予め設定されており、メモリ５２に記憶されている。即ち、メモリ５２は、仮の角度Φｉ、仮の角度Φｉのそれぞれに対する三角関数の値ｔａｎΦｉ，ｃｏｓΦｉを各ｉごとに記憶している。ＣＯＲＤＩＣローテーション５１は、メモリ５２に記憶された式１１から式１３に示す数値を参照して、式１または式２に基づいて仮の角度Φｉを獲得する。

ＣＯＲＤＩＣローテーション５１は、式１または式２に示す計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を各ｉに関してｉ＝０、１、２・・・の順番で計算する。

ＣＯＲＤＩＣローテーション５１は、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）が現実の第２の座標（ｘ２，ｙ２）に接近するように式１または式２を計算する。計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＣＷ方向へ移動させるためには、ＣＯＲＤＩＣローテーション５１は、式１を用いる。計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＷ方向へ移動させるためには、ＣＯＲＤＩＣローテーション５１は式２を用いる。これにより、図４に示すように、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標（ｘ２，ｙ２）へ収束させることができる。

以上の収束計算の過程において、仮の角度±Φｉを積算した角度が求めていた回転角度となる。

図４は、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標（ｘ２，ｙ２）へ収束させるときの概念図である。ＣＯＲＤＩＣローテーション５１は、まず、第１の座標（ｘ１，ｙ１）を（ｘｃ_ｉ−１，ｙｃ_ｉ−１）、および、ｉ＝０における式１１から式１３の値を式１に代入し、計算上の第２の座標（ｘｃ_０，ｙｃ_０）を得る。このとき、図２の選択部５６は、第１のレジスタＲ１１を選択し、第１の座標（ｘ１，ｙ１）をＣＯＲＤＩＣローテーション５１および判別部５４へ送る。ＣＯＲＤＩＣローテーション５１は式１を用いる。比較器５３が式１を選択する方法については後述する。計算上の第２の座標（ｘｃ_０，ｙｃ_０）は、第１の座標（ｘ１，ｙ１）から第２の座標（ｘ２，ｙ２）へ向かってＣＣＷ方向へΦ_０だけ回転させた座標になる。計算上の第２の座標（ｘｃ_０，ｙｃ_０）は、その計算後、図２の第３のレジスタＲ３１が保持する。ここで、当初の仮の角度＋Φ_０または−Φ_０は、回転角保持レジスタＲ４１に格納される。

次に、ＣＯＲＤＩＣローテーション５１は、計算上の第２の座標（ｘｃ_０，ｙｃ_０）、および、ｉ＝１における式１１から式１３の値を式２に代入し、計算上の第２の座標（ｘｃ_１，ｙｃ_１）を得る。このとき、図２の選択部５６は、第３のレジスタＲ３１を選択して計算上の第２の座標（ｘｃ_０，ｙｃ_０）をＣＯＲＤＩＣローテーション５１および判別部５４へ送る。ＣＯＲＤＩＣローテーション５１は式２を用いる。比較器５３が式２を選択する方法については後述する。計算上の第２の座標（ｘｃ_１，ｙｃ_１）は、計算上の第２の座標（ｘｃ_０，ｙｃ_０）から現実の第２の座標（ｘ２，ｙ２）へ向かってＣＷ方向へΦ１だけ回転させた座標になる。ここで、加算器８０は、回転角保持レジスタＲ４１に格納されていた仮の角度＋Φ_０または−Φ_０に仮の角度＋Φ_１または−Φ_１を加算し、この加算結果を回転角保持レジスタＲ４１に戻す。

ＣＯＲＤＩＣローテーション５１は、ｉ＝３，４・・・について同様の演算を繰り返す。式１２に示すようにｉが大きくなるにつれて、仮の角度Φｉの値は小さくなっていくので、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）は、現実の第２の座標（ｘ２，ｙ２）へ収束する。このとき、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）は、第３のレジスタＲ３１に格納され、次の計算上の第２の座標（ｘｃ_ｉ＋１，ｙｃ_ｉ＋１）を算出する際にＣＯＲＤＩＣローテーション５１および判別部５４へ送られる。

加算器８０は、回転角保持レジスタＲ４１に格納されていた仮の角度＋Φ_ｉ−１または−Φ_ｉ−１に仮の角度＋Φ_ｉまたは−Φ_ｉを加算し、その加算結果を回転角保持レジスタＲ４１へ戻す。このように、加算器８０は、仮の角度±Φ_０、±Φ_１、±Φ_２、・・・±Φ_ｉを積算し、回転角保持レジスタＲ４１は、積算された仮の角度を保持する。仮の角度Φ_ｉが収束したときに、回転角保持レジスタＲ４１に格納された角度が現実の第１の座標から第２の座標への回転角Φとなる。

これらの一連の演算は、第２のサンプリングから第３のサンプリングまでの間に実行される。即ち、ＣＯＲＤＩＣローテーション５１は、１サイクル期間の間に計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標（ｘ２，ｙ２）へ収束させる。これにより、現実の第１の座標から第２の座標への回転角Φを得ることができる。

これにより、ＣＯＲＤＩＣローテーション５１は、現実の回転角度Φをサンプリングごとに得ることができる。サンプリングの１サイクル期間は予め設定された期間である。従って、速度検出装置１００は、サンプリングごとに得られた現実の角度θ＋Φに等しい、あるいは、現実の角度θ＋Φに接近した仮の角度Φｉによって角速度を得ることができる。速度検出装置１００は、この角速度を出力する。

比較器５３が式１または式２を選択する方法を説明する。

図５は、座標を分割した８つの領域を示す図である。図６は、或る座標（ｘ，ｙ）が図５に示す領域Ｉ〜VIIIのいずれに属するかを判別するためのフロー図である。判別部５４および５５は、第1の座標、現実の第２の座標および計算上の第２の座標が図５の領域Ｉ〜VIIIのいずれに属するかを判別する。第1の座標を判別する場合には、図６の（ｘ，ｙ）に（ｘ１，ｙ１）を代入すればよい。現実の第２の座標を判別する場合には、図６の（ｘ，ｙ）に（ｘ２，ｙ２）を代入すればよい。計算上の第２の座標を判別する場合には、図６の（ｘ，ｙ）に（ｘｃ_ｉ，ｙｃ_ｉ）を代入すればよい。

ステップＳ１１およびＳ１２において、ｘ＞０かつｙ＞０である場合、座標（ｘ，ｙ）は、図５の領域ＩまたはIIに属する。さらに、ステップＳ１３において、ｘ≧ｙである場合、座標（ｘ，ｙ）は領域Iに属し、ｘ＜ｙである場合、座標（ｘ，ｙ）は領域IIに属することがわかる。

ステップＳ１１およびＳ１２において、ｘ＞０かつｙ≦０である場合、座標（ｘ，ｙ）は、図５の領域VIIまたはVIIIに属する。さらに、ステップＳ１４において、｜ｘ｜≧｜ｙ｜である場合、座標（ｘ，ｙ）は領域VIIIに属し、｜ｘ｜＜｜ｙ｜である場合、座標（ｘ，ｙ）は領域VIIに属することがわかる。

ステップＳ１１およびＳ１５において、ｘ≦０かつｙ＞０である場合、座標（ｘ，ｙ）は、図５の領域IIIまたはIVに属する。さらに、ステップＳ１６において、｜ｘ｜≧｜ｙ｜である場合、座標（ｘ，ｙ）は領域IVに属し、｜ｘ｜＜｜ｙ｜である場合、座標（ｘ，ｙ）は領域IIIに属することがわかる。

ステップＳ１１およびＳ１５において、ｘ≦０かつｙ≦０である場合、座標（ｘ，ｙ）は、図５の領域VまたはVIに属する。さらに、ステップＳ１７において、｜ｘ｜≧｜ｙ｜である場合、座標（ｘ，ｙ）は領域Vに属し、｜ｘ｜＜｜ｙ｜である場合、座標（ｘ，ｙ）は領域VIに属することがわかる。

図７（Ａ）および図７（Ｂ）は、第１の座標、現実の第２の座標および計算上の第２の座標のそれぞれが属する領域に基づいて、計算上の第２の座標の回転方向（ＣＣＷまたはＣＷ）を規定した表である。比較部５３は、図７（Ａ）および図７（Ｂ）の表に従って、ＣＯＲＤＩＣローテーション５１が用いる演算式（式１または式２）を選択する。

図７（Ａ）および図７（Ｂ）の“目標”は、目標となる現実の第２の座標が属する領域を示す。図７（Ａ）および図７（Ｂ）の“前回”は、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）が属する領域を示す。例えば、現実の第２の座標が領域Ｉに属し、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）が領域IIに属する場合、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＷ方向へ回転させるように、比較部５３は、式２を選択する。図５を参照すると明白なように、現実の第２の座標が領域Ｉに属し、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）が領域IIに属する場合、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標に近づけるためには、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）を基準に計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＷ方向へ回転させればよい。従って、比較部５３が式２を選択することによって、ＣＯＲＤＩＣローテーション５２は、計算上の第２の座標を現実の第２の座標へ近付けることができる。

現実の第２の座標と計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）とが同一の領域に属する場合、ｘ２とｘｃ_ｉ−１とを、あるいは、ｙ２とｙｃ_ｉ−１とを比較する。例えば、現実の第２の座標と計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）とがともに領域Ｉに属する場合、ｙ２とｙｃ_ｉ−１とを比較する。ｙ２＞ｙｃ_ｉ−１の場合には、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＣＷ方向へ回転させる。即ち、比較部５３は式１を選択する。ｙ２＜ｙｃ_ｉ−１の場合には、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）をＣＷ方向へ回転させる。即ち、比較部５３は式２を選択する。

ｃｏｓ値（ｘ値）の角度あたりの変化率は、絶対角度がπ／２（ｒａｄ）近傍および３π／２（ｒａｄ）近傍においてｓｉｎ値（ｙ値）のそれよりも高い。ｓｉｎ値（ｙ値）の角度あたりの変化率は、絶対角度がπ（ｒａｄ）近傍および０（２π）（ｒａｄ）近傍においてｃｏｓ値（ｘ値）のそれよりも角度あたりの変化率が高い。従って、現実の第２の座標と計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）との属する領域が、Ｉ、IV、VまたはVIIIのいずれかである場合、ｙ２とｙｃ_ｉ−１とを比較する。現実の第２の座標と計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）とが属する領域が、II、III、VIまたはVIIのいずれかである場合、ｘ２とｘｃ_ｉ−１とを比較する。これにより、現実の第２の座標と計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）とが同一の領域に属する場合であっても、比較部５３は、式１または式２を正確に選択することができる。

このように、判別部５４および５５は、第１の座標、現実の第２の座標、および、計算上の第２の座標のそれぞれが属する領域を判別することができる。さらに、図５および図７（Ａ）および図７（Ｂ）を参照して説明したように、比較部５３は、判別部５４および５５の判別結果に基づいて式１または式２を選択することができる。

本実施形態において、速度検出装置１００は、ロータの角速度を算出する。しかし、速度検出装置１００は、往復運動をする運動体にも適用することができる。この場合、速度検出装置１００は、ＣＯＲＤＩＣローテーション５１で算出された角速度を運動体の速度とすればよい。

本実施形態によれば、速度検出装置１００は、速度検出の精度を悪化させていた除算（アークタンジェント）を用いることなく、ｓｉｎ信号およびｃｏｓ信号のみを用いて回転角度を検出することができるので高精度である。

本実施形態によれば、速度検出装置１００は、式１および式２に示すように、除算以外の四則演算、即ち、乗算、加算または減算を用いて計算上の第２の座標を現実の第２の座標に接近させるように演算する。よって、速度検出装置１００内部の演算回路は比較的簡単に構成することができるので、装置全体の大きさを小さくすることができ、尚且つ、量産に適している。

本発明に係る実施形態に従った速度検出装置１００のブロック図。 演算部５０の内部構成を示すブロック図。 第１の座標および現実の第２の座標の位置関係を示す図。 計算上の第２の座標を現実の第２の座標へ収束させるときの概念図。 座標を分割した８つの領域を示す図。 或る座標が図５に示す領域Ｉ〜VIIIのいずれに属するかを判別するためのフロー図。 第1の座標、現実の第２の座標および計算上の第２の座標のそれぞれが属する領域に基づいて、計算上の第２の座標の回転方向ＣＣＷまたはＣＷを規定した表。 ２回のサンプリングで得られた座標を示す図。

符号の説明

５０ 演算部
５１ ＣＯＲＤＩＣローテーション
５２ メモリ
５３ 比較器
５４、５５ 判別部
５６ 選択部
Ｒ１１ 第１のレジスタ
Ｒ２１ 第２のレジスタ
Ｒ３１ 第３のレジスタ

Claims (1)

1. 回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出する速度検出装置であって、
   予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、
   第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、
   前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、
   前記第１の座標と前記三角関数の値との乗算、加算または減算によって表された計算上の第２の座標を前記現実の第２の座標に接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度に基づいて前記運動体の速度を演算する演算部と、
   前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標が座標面のいずれの領域に属するかを判別する判別部と、
   前記第１の座標、前記現実の第２の座標および前記計算上の第２の座標のそれぞれが属する領域に基づいて、前記演算部が実行する演算式を選択する比較部と、
   前記計算上の第２の座標を逐次保持する第３のレジスタと、
   前記仮の角度を積算するための第４のレジスタとを備え、
   前記メモリは、前記仮の角度としてΦ _０ 〜Φ _ｎ （Φ _０ ＞Φ _１ ＞Φ _２ ＞・・・Φ _ｎ ）（ｎは自然数）、前記三角算数の値としてｃｏｓΦ _０ 〜ｃｏｓΦ _ｎ 、および、ｔａｎΦ _０ 〜ｔａｎΦ _ｎ を記憶し、
   前記演算部は、前記第１の座標を（ｘ１，ｙ１）、前記現実の第２の座標を（ｘ２，ｙ２）、および、前記計算上の第２の座標を（ｘｃ _ｉ ，ｙｃ _ｉ ）とすると、
   

   

   （但し、Φ _ｉ ＝ｔａｎ ^−１ ２ ^−ｉ （ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、ｘｃ _ｉ−１ ＝ｘ１、ｙｃ _ｉ−１ ＝ｙ１である）
   を各ｉに関してｉ＝０、１、２・・・の順番で演算し、
   前記演算部が演算するときに、前記比較部は、各ｉに関して前記現実の第２の座標（ｘ２，ｙ２）と前記計算上の第２の座標（ｘｃ，ｙｃ）とを比較して、前記演算部が演算すべき式として式１または式２のいずれかを選択し、
   前記第３のレジスタは、各ｉに関して前記演算部で演算された前記計算上の第２の座標を積算し、
   前記第４のレジスタは、各ｉに関して前記計算上の第２の座標に用いられた前記仮の角度を積算し、
   前記演算部は、前記計算上の第２の座標が前記現実の第２の座標に最も近いときに、前記第４のレジスタに格納された前記仮の角度の積算値に基づいて前記運動体の速度を演算することを特徴とする速度検出装置。

Images