JP4987448B2 - 速度検出装置 - Google Patents

Description

本発明は、速度検出装置に関する。

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

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

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

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

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

さらに、乗算器も、デジタル回路で実現するためには加減算に比べてかなり大規模なデジタル回路を用いる必要がある。乗算器も、デジタル回路で実現すると、加算器および減算器に比べてかなり大規模なデジタル回路を用いる必要がある。

そこで、本発明は、小規模な演算回路で運動体の速度を高精度に検出することができる速度検出回路を提供することである。

本発明に係る実施形態に従った速度検出装置は、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の角速度を検出する速度検出装置であって、
予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、
第１のサンプリングで得られた運動体の第１の現実の座標を格納する第１のレジスタと、
前記第１のサンプリングの次の第２のサンプリングで得られた運動体の第２の現実の座標を格納する第２のレジスタと、
前記第１の現実の座標（ｘ１、ｙ１）のｘ１と前記三角関数の値に基いてｙ１の桁をシフトした結果値との加算または減算によって表される第１の計算上の座標を算出する第１の演算部と、
前記第２の現実の座標（ｘ２、ｙ２）のｙ２と前記三角関数の値に基いてｘ２の桁をシフトした結果値との加算または減算によって表される第２の計算上の座標を算出する第２の演算部と、
前記第１または前記第２の演算部における演算で用いられた前記仮の角度を積算する第３の演算部とを備え、
前記第１および前記第２の演算部は、前記第１および前記第２の計算上の座標を互いに接近させるように演算することを特徴とする。

前記第１および前記第２の演算部は、前記仮の角度を小さくしつつ、前記第１および前記第２の計算上の座標を収束させるように演算を繰り返し、前記第３の演算部は、前記第１または前記第２の演算部における演算で収束する前記仮の角度を積算し、前記第１および前記第２の計算上の座標が収束したときの該仮の角度に基づいて前記運動体の角速度を生成してもよい。

前記メモリは、前記仮の角度としてΦ_i（Φ_０＞Φ_１＞Φ_２＞・・・＞Φ_i＞・・・Φ_ｎ）（ｎは自然数）、前記三角関数の値としてｔａｎΦ_iを記憶し、
前記第１または前記第２の計算上の座標を（ｘｘｃ_ｉ，ｙｙｃ_ｉ）とすると、

（但し、Φ_i＝ｔａｎ^−１２⁻ⁱ（ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、前記第１の演算部においては、ｘｘｃ_ｉ−１＝ｘ１、ｙｙｃ_ｉ−１＝ｙ１であり、前記第２の演算部においては、ｘｘｃ_ｉ−１＝ｘ２、ｙｙｃ_ｉ−１＝ｙ２である）

前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１または式２を演算し、前記第３の演算部は、前記仮の角度をiごとに積算してもよい。

前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１および式２を交互または相補に演算し、かつ、前記第１の演算部が式１を演算するときには、前記第２の演算部は式２を演算し、前記第１の演算部が式２を演算するときには、前記第２の演算部は式１を演算してもよい。

当該速度検出装置は、前記第１の計算上の座標を保持する第３のレジスタと、前記第２の計算上の座標を保持する第４のレジスタと、前記第３の演算部で積算された前記仮の角度を保持する第５のレジスタとをさらに備えていてもよい。

前記サンプリング周期を単位時間にすることによって、前記第３の演算部で積算された前記仮の角度の２倍を前記運動体の角速度としてもよい。

前記第３の演算部で積算された前記仮の角度は、前記運動体の回転角度の２分の１でよい。

当該速度検出装置は、ロータリエンコーダまたはリニアスケールに配備されていてもよい。

本発明に係る実施形態に従った速度検出装置は、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の角速度を検出する速度検出装置であって、
予め設定された複数の仮の角度Φ_ｉ（Φ_０＞Φ_１＞Φ_２＞・・・＞Φ_ｉ＞・・・Φ_ｎ）（ｎは自然数）、および、該複数の仮の角度のそれぞれに対する三角関数の値ｔａｎΦ_ｉを記憶するメモリと、第１のサンプリングで得られた運動体の第１の現実の座標（ｘ１，ｙ１）を格納する第１のレジスタと、前記第１のサンプリングの次の第２のサンプリングで得られた運動体の第２の現実の座標（ｘ２，ｙ２）を格納する第２のレジスタと、前記第１の現実の座標（ｘ１、ｙ１）と前記三角関数の値ｔａｎΦ_iに基いて式１または式２を演算する第１の演算部と、前記第２の現実の座標（ｘ２、ｙ２）と前記三角関数の値ｔａｎΦ_iに基いて式１または式２を演算する第２の演算部と、前記第１または前記第２の演算部における演算で用いられた前記仮の角度Φ_iを積算する第３の演算部とを備える

（但し、Φ_i＝ｔａｎ^−１２⁻ⁱ（ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、前記第１の演算部においては、ｘｘｃ_ｉ−１＝ｘ１、ｙｙｃ_ｉ−１＝ｙ１であり、前記第２の演算部においては、ｘｘｃ_ｉ−１＝ｘ２、ｙｙｃ_ｉ−１＝ｙ２である）。

前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１または式２を演算し、前記第３の演算部は、前記仮の角度をiごとに積算する。

前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１および式２を交互または相補に演算し、かつ、前記第１の演算部が式１を演算するときには、前記第２の演算部は式２を演算し、前記第１の演算部が式２を演算するときには、前記第２の演算部は式１を演算してもよい。

本発明による速度検出装置は、小規模な演算回路で運動体の速度を高精度に検出することができる。

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

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

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

演算部５０は、ＦＰＧＡでよいが、ＦＰＧＡに代えて、他の任意のＬＳＩを用いることができる。演算部５０をカスタムＬＳＩ上でロジック回路として実現した場合、同じ処理を繰り返す反復処理を短時間で行うことができる。一方で、演算部５０は、汎用ＣＰＵおよびソフトウェアで実現してもよい。演算部５０を汎用ＣＰＵおよびソフトウェアで実現した場合、プログラムの変更で様々な処理を行うことができる。

図２は、演算部５０の内部構成を示すブロック図である。演算部５０は、第１の演算部としてのＣＯＲＤＩＣローテーション（以下、ローテータともいう）５１と、第２の演算部としてのＣＯＲＤＩＣローテーション５８と、メモリ５２、５９と、回転方向判別部５３と、エリア判別部５４、５５と、選択部５６、５７と、第１から第４のレジスタＲ１１、Ｒ２１、Ｒ３１およびＲ４１と、第５のレジスタとしての回転角度レジスタＲ５１と、角速度保持レジスタＲ６１と、第３の演算部としての加算器８０とを備えている。

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

図２および図３を参照し、ロータが第１の現実の座標から第２の現実の座標まで回転するときの角速度を、ｓｉｎ信号およびｃｏｓ信号から算出する過程を説明する。尚、往復運動を行う運動体の速度は、角速度から簡単に算出することができるので、ここでは、角速度の算出のみを説明する。

本実施形態では、角速度を算出するためにＣＯＲＤＩＣ（COordinate Rotation Digital Computer）アルゴリズムが利用される。ＣＯＲＤＩＣアルゴリズムには、ベクタリングモードおよびローテーションモードがある。本実施形態では、ローテーションモードを利用する。このＣＯＲＤＩＣアルゴリズムは、演算部５０内に内蔵されており、本実施形態ではロジック回路によって実現されている。

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

メモリ５２は、予め設定された複数の仮の角度Φ_０〜Φ_ｎ（Φ_０＞Φ_１＞Φ_２＞・・・Φ_ｎ）（ｎは自然数）、および、該複数の仮の角度のそれぞれに対する三角関数の値ｔａｎΦ_０〜ｔａｎΦ_ｎを記憶している。速度検出装置１００の初期状態において、第１から第４のレジスタは特定値を格納していない。第５のレジスタＲ５１は、ゼロである。尚、第５のレジスタＲ５１は、サンプリングごとにゼロにリセットされる。

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

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

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

第２の現実の座標を受けたときには、第３のレジスタＲ３１はまだ不定であるので、選択部５６は、第１のレジスタＲ１１を選択し、第１の現実の座標（ｓｉｎθ，ｃｏｓθ）をエリア判別部５４およびローテータ５１へ送る。また、このとき、第４のレジスタＲ４１もまだ不定であるので、選択部５７は、第２のレジスタＲ２１を選択し、第２の現実の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））をエリア判別部５５およびローテータ５８へ送る。

ローテータ５１は、第１の座標（ｘ１，ｙ１）＝（ｓｉｎθ，ｃｏｓθ）、および、メモリ５２からの三角関数の値ｔａｎΦ_０を用いて第１の計算上の座標（ｘｘｃ_１，ｙｙｃ_１）を計算する。一方、ローテータ５８は、第２の現実の座標（ｘ２，ｙ２）＝（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））、および、メモリ５９からの三角関数の値ｔａｎΦ_０を用いて第２の計算上の座標（ｘｘｃ_１，ｙｙｃ_１）を計算する。ローテータ５１および５８の具体的な演算内容に関しては後述する。尚、ローテータ５１および５８は、ともに後述の式１および式２を演算するので、第１および第２の計算上の座標は、（ｘｘｃ_ｉ，ｙｙｃ_ｉ）で表現している。しかし、第１の計算上の座標および第２の計算上の座標は、互いに異なる座標である。ｉは、ローテータ５１、５８が既に実行した演算回数に等しい。

第３のレジスタＲ３１は、第１の計算上の座標（ｘｘｃ_１，ｙｙｃ_１）を格納する。第４のレジスタＲ４１は、第２の計算上の座標（ｘｘｃ_１，ｙｙｃ_１）を格納する。その後、選択部５６および５７は、次の周期のサンプリングが行われるまで、それぞれ第３および第４のレジスタＲ３１およびＲ４１を選択する。ローテータ５１および５８は、それぞれ第３および第４のレジスタＲ３１およびＲ４１に格納された座標と、三角関数の値ｔａｎΦ_１とを用いて同様の計算を繰り返す。それらの計算結果は、第１の計算上の座標（ｘｘｃ_ｉ，ｙｙｃ_ｉ）および第２の計算上の座標（ｘｘｃ_ｉ，ｙｙｃ_ｉ）として、それぞれ第３および第４のレジスタＲ３１およびＲ４１に上書きされる。このように、ローテータ５１および５８は、三角関数の値ｔａｎΦ_０、ｔａｎΦ_１、ｔａｎΦ_２、・・・ｔａｎΦ_ｉを順に用いてそれぞれ第１の計算上の座標および第２の計算上の座標を計算する。ｉ＝０、１、２・・・に対応する各演算において、ローテータ５１および５８は、第１の計算上の座標および第２の計算上の座標を互いに接近させるように演算する。これにより、第１の計算上の座標および第２の計算上の座標は、ローテータ５１および５８による計算ごとに、第１の現実の座標と第２の現実の座標と間の中間座標（ｓｉｎ（θ＋Φ／２），ｃｏｓ（θ＋Φ／２））へ収束していく。中間座標は、サンプリングごとに決定され、回転角度レジスタＲ５１へ送られる。回転角度レジスタＲ５１内のデータは、サンプリングごとに更新される。

加算器８０は、回転角保持レジスタＲ５１に格納されている仮の角度＋Φ_i−１または−Φ_i−１に仮の角度−Φ_iまたは＋Φ_iを加算し、その加算結果を回転角保持レジスタＲ４１へ戻す。このように、加算器８０は、仮の角度±Φ_０、±Φ_１、±Φ_２、・・・±Φ_iを積算し、回転角保持レジスタＲ５１は、積算された仮の角度を保持する。仮の角度Φ_iが収束したときに、回転角保持レジスタＲ５１に格納された角度は、第１の現実の座標から中間座標への回転角Φ／２となる。回転角保持レジスタＲ５１内の回転角は、サンプリングごとに更新される。ここで、積算とは、前回の演算結果に対して加算または減算をさらに繰り返して演算することである。例えば、積算された仮の角度は、Φ_０−Φ_１＋Φ_２−Φ_３＋・・・のように表すことができる。

実際のロータの回転角度は、回転角度レジスタＲ５１内のデータを２倍にすることによって求め得る。データを２倍にすることは、デジタル値を１ビットだけ左シフトすれば足りる。左シフトとは、或る数値がより大きい値になる方向へ桁をシフトさせることである。従って、このときの演算に乗算器は不要である。この演算は、回転角度レジスタＲ５１、角速度保持レジスタＲ６１、あるいは、ＦＰＧＡ５０の外部回路のいずれが行ってもよい。

現実の座標のサンプリング周期は一定であるので、ロータの回転角度Φの値は、そのまま相対的な角速度として用いることができる。現実の座標のサンプリング周期を単位時間とすれば、ロータの回転角度Φの値は、そのまま絶対的な角速度として用いることができる。角速度は、角速度保持レジスタＲ６１に格納され、必要に応じて出力される。角速度保持レジスタＲ６１内の角速度は、サンプリングごとに更新される。

尚、加算器８０は、ローテータ５８から中間座標（ｓｉｎ（θ＋Φ／２），ｃｏｓ（θ＋Φ／２））を獲得してもよい。第１の計算上の座標および第２の計算上の座標は、同一中間座標へ収束するからである。

エリア判別部５４は、第１のレジスタＲ１１に格納された第１の現実の座標が属する領域または第３のレジスタＲ３１に格納された第１の計算上の座標が属する領域のいずれか一方を判別する。即ち、エリア判別部５４は、選択部５６により選択された座標が属する領域を判別する。判別部５５は、第２のレジスタＲ２１に格納された第２の現実の座標が属する領域または第４のレジスタＲ４１に格納された第２の計算上の座標が属する領域のいずれか一方を判別する。即ち、エリア判別部５５は、選択部５７により選択された座標が属する領域を判別する。

次に、ローテータ５１および５８の具体的演算内容を説明する。第１および第２の計算上の座標（ｘｘｃ_ｉ，ｙｙｃ_ｉ）は式１または式２のように表わすことができる。比較器５３は、判別部５４および５５の判別結果に基づいて式１または式２のいずれかを選択する。ローテータ５１および５８は、比較器５３の比較結果に基づいて式１または式２の演算を実行する。ローテータ５８は、インバータ６０によって比較器５３の比較結果の反転信号を受信する。従って、ローテータ５８は、第２の現実の座標または第２の計算上の座標を、第１の現実の座標または第１の計算上の座標の移動方向と反対方向へ移動させるように演算を行う。

但し、ｔａｎΦ_i＝２⁻ⁱ（ｉ＝０、１、２・・・ｎ）である。ｉ＝０のとき、ローテータ５１に与える初期値としての第１の現実の座標は、ｘｘｃ_ｉ−１＝ｘ１、ｙｙｃ_ｉ−１＝ｙ１である。また、ローテータ５８に与える初期値としての第２の現実の座標は、ｘｘｃ_ｉ−１＝ｘ２、ｙｙｃ_ｉ−１＝ｙ２である。

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

ｘ（ｉ）／ｃｏｓΦをｘｘｃ（ｉ）とし、ｙ（ｉ）／ｃｏｓΦをｙｙｃ（ｉ）とすると、式９および式１０はそれぞれ式１１および式１２のように表すことができる。
ｘｘｃ（ｉ）＝ｘ（ｉ−１）−ｙ（ｉ−１）・ｔａｎΦ （式９）
ｙｙｃ（ｉ）＝ｙ（ｉ−１）＋ｘ（ｉ−１）・ｔａｎΦ （式１０）

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

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

式９および式１０に三角関数の値ｔａｎΦｉを代入すると計算上の座標が得られる。ここで、ローテータ５１および５８の各初期値（ｉ＝０）は、それぞれ図２の第１のレジスタＲ１１に格納された第１の現実の座標（ｘ１、ｙ１）および第２のレジスタＲ２１に格納された第２の現実の座標（ｘ２、ｙ２）である。即ち、ローテータ５１において、（ｘｘｃ（０）、ｘｘｃ（０））＝（ｘ１、ｙ１）、ローテータ５８において、（ｘｘｃ（０）、ｘｘｃ（０））＝（ｘ２、ｙ２）である。第１および第２の現実の座標は、第１および第２のサンプリングで判明している。

（最初の演算（ｉ＝０））
ローテータ５１は、初期値としての第１の現実の座標（ｘ１、ｙ１）を式９および式１０に代入した演算を最初に実行する。即ち、ローテータ５１は、式１３および式１４を実行する。このとき、選択部５６は、第１のレジスタＲ１１を選択する。
ｘｘｃ１＝ｘ１−ｙ１・ｔａｎΦ０ （式１３）
ｙｙｃ１＝ｙ１＋ｘ１・ｔａｎΦ０ （式１４）

式１１に示したとおり、ｔａｎΦｉ＝±２⁻ⁱである。従って、式１３および式１４に含まれる乗算項（ｙ１・ｔａｎΦ０）および（ｘ１・ｔａｎΦ０）は、三角関数の値ｔａｎΦ０＝±２⁻ⁱに基いて、デジタル値（バイナリ値）として表されたｙ１およびｘ１の桁を単にシフトすれば足りる。つまり、ローテータ５１は、実際には乗算を演算する必要は無い。これにより、ローテータ５１には、乗算器が不要となる。ただし、式１３、式１４で示す最初の演算では、ｔａｎΦ０＝１であるので、ローテータ５１は、桁のシフトを行わない。その後、ローテータ５１は、ｘ１またはｙ１と乗算項の結果値とを加算または減算する。これにより、ローテータ５１は、乗算なしに、式１３および式１４を演算することができる。

一方、ローテータ５８は、初期値としての第２の現実の座標（ｘ２、ｙ２）を式９および式１０に代入した演算を最初に実行する。即ち、ローテータ５８は、式１５および式１６を実行する。このとき、選択部５７は、第２のレジスタＲ２１を選択する。
ｘｘｃ２＝ｘ２＋ｙ２・ｔａｎΦ０ （式１５）
ｙｙｃ２＝ｙ２−ｘ２・ｔａｎΦ０ （式１６）

ローテータ５８も、式１５および式１６に含まれる乗算項（ｙ２・ｔａｎΦ０）および（ｘ２・ｔａｎΦ０）の演算をするために、実際には乗算を演算する必要は無く、デジタル値（バイナリ値）として表されたｙ２またはｘ２の桁を単にシフトすればよい。これにより、ローテータ５８には、乗算器が不要となる。ただし、式１５および式１６で示された最初の演算では、ｔａｎΦ０＝１であるので、ローテータ５８は、桁のシフトを行わない。その後、ローテータ５８は、ｘ２またはｙ２と乗算項の結果値とを加算または減算する。これにより、ローテータ５８は、乗算なしに、式１５および式１６を演算することができる。

（２回目以降の演算（ｉ＝１・・・））
２回目以降の演算では、ローテータ５１および５８は、式１または式２を用いる。

ローテータ５１において演算された第１の計算上の座標（ｘｘｃ１、ｙｙｃ１）は、第３のレジスタＲ３１に格納される。その後、選択部５６は、第３のレジスタＲ３１を選択し、第１の計算上の座標（ｘｘｃ１、ｙｙｃ１）をローテータ５１に送る。ローテータ５１は、第１の計算上の座標（ｘｘｃ１、ｙｙｃ１）を、それぞれ式１または式２のｘｘｃ_ｉ−１およびｙｙｃ_ｉ−１に代入し、演算を繰り返す。これにより、第１の計算上の座標（ｘｘｃ２、ｙｙｃ２）が得られる。第１の計算上の座標（ｘｘｃ２、ｙｙｃ２）は、（ｘｘｃ１、ｙｙｃ１）に代わり、第３のレジスタＲ３１に上書きされる。つまり、ローテータ５１は、最初の１回目の演算では式１３および式１４を演算し、その後は、式１または式２の演算を繰り返す。

式１および式２に含まれる乗算項（ｙｙｃ_ｉ−１・ｔａｎΦ０）および（ｘｘｃ_ｉ−１・ｔａｎΦ０）は、三角関数の値ｔａｎΦｉ＝±２⁻ⁱに基いて、デジタル値（バイナリ値）として表されたｙｙｃ_ｉ−１およびｘｘｃ_ｉ−１の桁を単にシフトすれば足りる。本実施形態では、ローテータ５１は、ｙｙｃ_ｉ−１およびｘｘｃ_ｉ−１をｉだけ右シフトさせる。右シフトとは、或る数値がより小さい値になる方向へ桁をシフトさせることである。つまり、式１および式２に関して、ローテータ５１は、実際には乗算を演算する必要は無い。その後、ローテータ５１は、ｘｘｃ_ｉ−１またはｙｙｃ_ｉ−１と乗算項の結果値とを加算または減算する。これにより、ローテータ５１は、乗算なしに、式１または式２を演算することができる。ｉの値は演算ごとに１つずつ大きくなるので、乗算項のシフト量は演算ごとに大きくなる。これにより、計算上の座標（ｘｘｃ_ｉ、ｙｙｃ_ｉ）は、演算ごとに収束する。この演算で用いられた仮の角度Φｉは、演算ごとに回転角度レジスタＲ５１に積算される。

また、ローテータ５８で演算された第２の計算上の座標（ｘｘｃ１、ｙｙｃ１）は、第４のレジスタＲ３１に格納される。その後、選択部５７は第４のレジスタＲ４１を選択し、ｘｘ１およびｙｙ１をローテータ５８に送る。ローテータ５８は、ｘｘ１およびｙｙ１を、式１または式２のｘｘｉ−１およびｙｙｉ−１に代入し、演算を繰り返す。これにより、ｘｘ２およびｙｙ２が得られる。つまり、ローテータ５８は、最初の１回目の演算では式１５および式１６を演算し、その後、ローテータ５１と同様に式１または式２の演算を繰り返す。

ローテータ５８も、ローテータ５１と同様に、式１および式２に関して乗算を演算する必要は無い。その後、ローテータ５８は、ｘｘｃ_ｉ−１またはｙｙｃ_ｉ−１と乗算項の結果値とを加算または減算する。これにより、ローテータ５８も、乗算なしに、式１または式２を演算することができる。

式９および式１０において、ｘ（ｉ）／ｃｏｓΦをｘｘｃ（ｉ）と置換し、ｙ（ｉ）／ｃｏｓΦをｙｙｃ（ｉ）と置換した意味は、ｃｏｓΦの乗算を演算から除くためである。このように置換することで、ローテータが実質的に乗算を演算する必要が無くなる。尚、計算上の座標として得られる結果（ｘｘｃ１、ｙｙｃ２）は、座標にｃｏｓΦを割り算した値である。これは、あくまでも演算途中の計算上の結果であり、角度Φ／２が最終的に得られれば差し支えない。

上記ローテータ５１および５８の演算動作は、演算時間短縮のために各ｉについて同時に実行される。尚、ロータの回転方向によって、式１３〜式１６内の＋、−の符号は、逆符号になる場合もある。

式１１および式１２に示す各ｉに対する離散的な数値は、予め設定されており、メモリ５２および５９に記憶されている。即ち、メモリ５２および５９は、仮の角度Φｉ、仮の角度Φｉのそれぞれに対する三角関数の値ｔａｎΦｉを各ｉごとに記憶している。

ローテータ５１および５８は、それぞれメモリ５２および５９に記憶された式１１に示す数値を獲得する。ローテータ５１および５８は、式１または式２に示す第１および第２の計算上の座標（ｘｘｃ_i，ｙｙｃ_i）を各ｉに関してｉ＝０、１、２・・・の順番で計算する。

式１は、仮の角度が正である場合に成り立つ式であり、式２は、仮の角度が負である場合に成り立つ式である。式１および式２のΦｉは、絶対値で表示されている。即ち、式２は、式１のΦｉに−Φｉを代入して得られる式である。仮の角度が正（＋Φｉ）である場合、ローテータ５１および５８は、図４に示す反時計回り方向ＣＣＷへ計算上の座標を移動させる。仮の角度が負（−Φｉ）である場合、ローテータ５１および５８は、図４に示す時計回り方向ＣＷへ計算上の座標を移動させる。ローテータ５１および５８は、回転方向判断部５３からの信号に応じて式１または式２のいずれかを選択する。

ローテータ５１および５８は、第１および第２の計算上の座標を互いに接近させるために、ｉ＝０、１、２・・・ｎの順に式１および式２を交互または相補に演算する。つまり、ローテータ５１が式１を演算するときには、ローテータ５８は式２を演算し、ローテータ５１が式２を演算するときには、ローテータ５８は式１を演算する。第１の計算上の座標の移動方向および第２の計算上の座標の移動方向は、一方が決まれば、他方も決まるので、回転方向判別部５３は１つで足りる。

仮の角度Φｉは、式１２に示すようにｉが大きくなるにつれて小さくなる。従って、図４に示すように、第１の計算上の座標および第２の計算上の座標を、第１の現実の座標（ｘ１，ｙ１）と第２の現実の座標（ｘ２，ｙ２）との中間の座標へ収束させることができる。ローテータ５１および５８は、第１および第２の計算上の座標を互いに接近させるように演算するので、仮の角度Φｉは、収束の結果、実際の回転角度の２分の１（Φ／２）となる。仮の角度±Φｉの積算は、レジスタＲ５１に格納されたローテータ５１の演算結果を、加算器８０がローテータ５１の演算ごとに積算することによって実行される。尚、仮の角度±Φｉの積算結果は、ローテータ５８の演算結果を積算することによっても同様に得られる。

図４を参照して、演算部５０の動作を概念的に説明する。ローテータ５１は、まず、第１の現実の座標（ｘ１，ｙ１）をＣＣＷ方向へΦ０だけ回転させる。実際には、ローテータ５１は、式１３および式１４を演算し、第１の計算上の座標を算出する。ＣＣＷは、第１の現実の座標（ｘ１，ｙ１）を基準として第２の第２の現実の座標（ｘ２，ｙ２）へ向かう方向である。

これと同時に、ローテータ５８は、第２の現実の座標（ｘ２，ｙ２）をＣＷ方向へΦ０だけ回転させる。実際には、ローテータ５８は、式１５および式１６を演算し、第２の計算上の座標（ｘｘｃ１，ｙｙｃ１）を算出する。ＣＷは、第２の第２の現実の座標（ｘ２，ｙ２）を基準として第１の現実の座標（ｘ１，ｙ１）へ向かう方向である。結果として、ローテータ５１および５８は、第１の現実の座標と第２の現実の座標とを互いに接近させる方向（互いに向かい合う方向）へ回転させるように演算する。

次に、ローテータ５１は、第１の計算上の座標（ｘｘｃ１，ｙｙｃ１）をＣＷ方向へΦ１だけ回転させる。実際には、ローテータ５１は、式２（ｉ＝１）を演算し、第１の計算上の座標を再計算する。

これと同時に、ローテータ５８は、第２の計算上の座標（ｘｘｃ２，ｙｙｃ２）をＣＣＷ方向へΦ１だけ回転させる。実際には、ローテータ５８は、式１を演算し、第２の計算上の座標を再計算する。

次に、ローテータ５１は、第１の計算上の座標（ｘｘｃ１，ｙｙｃ１）をＣＷ方向へΦ１だけ回転させる。実際には、ローテータ５１は、式２（ｉ＝１）を演算し、第１の計算上の座標を再計算する。これと同時に、ローテータ５８は、第２の計算上の座標（ｘｘｃ２，ｙｙｃ２）をＣＣＷ方向へΦ１だけ回転させる。実際には、ローテータ５８は、式１を演算し、第２の計算上の座標を再計算する。

次に、ローテータ５１は、第１の計算上の座標をＣＣＷ方向へΦ２だけ回転させる実際には、ローテータ５１は、式１（ｉ＝２）を演算し、第１の計算上の座標を再計算する。これと同時に、ローテータ５８は、第２の計算上の座標をＣＷ方向へΦ２だけ回転させる。実際には、ローテータ５８は、式２を演算し、第２の計算上の座標を再計算する。

式１２に示すようにｉが大きくなるにつれて、仮の角度Φｉの値は小さくなっていく。また、ローテータ５１および５８は、各演算において、第１の計算上の座標と第２の計算上の座標とを互いに接近させる方向（互いに向かい合う方向）へ回転させるように演算する。よって、ローテータ５１および５８が演算を繰り返すごとに、第１の計算上の座標および第２の計算上の座標は、中間座標（ｓｉｎ（θ＋Φ／２），ｃｏｓ（θ＋Φ／２））へ収束する。第１の計算上の座標および第２の計算上の座標が中間座標に収束したとき、仮の角度Φｉは、ほぼΦ／２に等しい。

次に、回転方向判別部５３が方向ＣＷまたはＣＣＷ（式１または式２）を選択する方法を説明する。

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

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

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

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

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

エリア判別部５４および５５は、図６のフローをＡＳＩＣ（Application Specific Integrated Circuit）のようなカスタムＬＳＩで構成すればよい。

図７（Ａ）および図７（Ｂ）は、第１の現実の座標および第２の現実の座標、または、第１の計算上の座標および第２の計算上の座標が属する領域に基づいて、第１および第２の計算上の座標の回転方向（ＣＣＷまたはＣＷ）を規定した表である。回転方向判別部５３は、図７（Ａ）および図７（Ｂ）の表に従って、ローテータ５１および５８が用いる演算式（式１または式２）を選択する。

図７（Ａ）および図７（Ｂ）の“目標”は、目標となる座標が属する領域を示す。第１の現実の座標および第２の現実の座標は、その一方が他方を目標とする。第１の計算上の座標および第２の計算上の座標は、その一方が他方を目標とする。図７（Ａ）および図７（Ｂ）の“前回”は、前回の演算によって得られた計算上の座標（ｘｘｃ_ｉ−１，ｙｙｃ_ｉ−１）が属する領域を示す。あるいは、最初の演算では、第１の現実の座標および第２の現実の座標を示す。例えば、第１の現実の座標が領域Ｉに属し、第２の現実の座標が領域IIに属する場合、第１の現実の座標にとって、目標は領域IIとなる。従って、回転方向判別部５３は、“目標”の領域がII であり、かつ、“前回”の領域がＩである回転方向ＣＣＷを選択する。つまり、回転方向判別部５３は、ローテータ５１が式１を選択するように信号を送る。このとき、ローテータ５８は式２を選択する。

同様に、ｉ＝１における第１の計算上の座標が領域IIに属し、第２の計算上の座標が領域Iに属する場合、第１の計算上の座標にとって、目標は領域Iとなる。従って、回転方向判別部５３は、“目標”の領域がI であり、かつ、“前回”の領域がIIである回転方向ＣＷを選択する。つまり、回転方向判別部５３は、ローテータ５１が式２を選択するように信号を送る。このとき、ローテータ５８は式１を選択する。

第１の現実の座標と第２の現実の座標とが同一の領域に属する場合、ｘ１とｘ２とを、あるいは、ｙ１とｙ２とを比較する。例えば、第１の現実の座標と第２の現実の座標とがともに領域Ｉに属する場合、ｙ１とｙ２とを比較する。ｙ１＜ｙ２の場合には、第１の現実の座標をＣＣＷ方向へ回転させ、第２の現実の座標をＣＷ方向へ回転させる。

第１の計算上の座標と第２の計算上の座標とが同一の領域に属する場合、第１の計算上の座標のｘｘｃiと第２の計算上の座標のｘｘｃiとを比較する。あるいは、第１の計算上の座標のｙｙｃiと第２の計算上の座標のｙｙｃiとを比較する。例えば、i＝１において第１の計算上の座標と第２の計算上の座標とがともに領域Ｉに属する場合、第１の計算上の座標のｘｘｃ１と第２の計算上の座標のｘｘｃ１とを比較する。第１の計算上の座標のｘｘｃ１が第２の計算上の座標のｘｘｃ１より大きい場合には、第１の計算上の座標をＣＣＷ方向へ回転させ、第２の計算上の座標をＣＷ方向へ回転させる。

このように、第１の現実の座標と第２の現実の座標とが同一の領域に属する場合、あるいは、第１の計算上の座標と第２の計算上の座標とが同一の領域に属する場合であっても、回転方向判別部５３は、式１または式２を正確に選択することができる。

回転方向判別部５３は、図７（Ａ）および図７（Ｂ）の選択条件をＡＳＩＣのようなカスタムＬＳＩで構成すればよい。

本実施形態において、速度検出装置１００は、ロータの角速度を算出する。しかし、速度検出装置１００は、往復運動をする運動体にも適用することができる。この場合、速度検出装置１００は、ＦＰＧＡ５０で得られた角速度を運動体の速度に変換すればよい。

本実施形態によれば、速度検出装置１００は、速度検出の精度を悪化させていた除算や乗算を用いることなく、ｓｉｎ信号およびｃｏｓ信号の加算および減算のみを用いて回転角度を検出することができるので高精度である。ここで、式１および式２には、ｔａｎΦ_iを乗算する項が存在する。しかし、ｔａｎΦ_iは、＝２⁻ⁱであるので、ｔａｎΦ_iの乗算は、ｘｘｃ_ｉ−１またはｙｙｃ_ｉ−１のデジタル値をiビットだけ右シフトすることと等価である。従って、ローテータ５１および５８は、実際には、乗算器を含まず、単に、デジタル値を桁下げする回路を含むだけである。

一般に、演算器をロジック回路で実現しようとすると、除算器および乗算器は、加算器および減算器に比べ、非常に回路規模が大きくなる。本実施形態では、ＦＰＧＡ５０は、２つのローテータを含むが、これらのローテータから除算器および乗算器を省くことによって、全体の回路規模は、非常に小さいものとなる。また、除算器および乗算器がないので、本実施形態によるＦＰＧＡ５０は、カスタムロジックＬＳＩで実現しやすい。本実施形態のローテータ５１、５８は、上述のように簡単な式を繰り返し演算する。カスタムロジックＬＳＩは、このように簡単な演算を高速に演算することに適している。従って、本実施形態によるＦＰＧＡ５０は、カスタムロジックＬＳＩを用いて演算速度を高速化することができる。即ち、本実施形態によるＦＰＧＡ５０は、ロジックＬＳＩで実現した場合に特に有利であるということができる。

本実施形態において、回転角度レジスタＲ５１は、ローテータ５１の演算ごとに仮の角度を２倍した値を保持してもよい。この場合、加算器８０で積算され回転角度レジスタＲ５１に保持された仮の角度の値をそのままロータの回転角度とすることができる。

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

符号の説明

５０…ＦＰＧＡ
５１、５８…ＣＯＲＤＩＣローテーション
５２、５９…メモリ
５３…回転方向判別部
５４、５５…エリア判別部
５６、５７…選択部
Ｒ１１…第１のレジスタ
Ｒ２１…第２のレジスタ
Ｒ３１…第３のレジスタ
Ｒ４１…第４のレジスタ
Ｒ５１…回転角度レジスタ
Ｒ６１…角速度保持レジスタ

Claims (10)

1. 回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の角速度を検出する速度検出装置であって、
   予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、
   第１のサンプリングで得られた運動体の第１の現実の座標を格納する第１のレジスタと、
   前記第１のサンプリングの次の第２のサンプリングで得られた運動体の第２の現実の座標を格納する第２のレジスタと、
   前記第１の現実の座標（ｘ１、ｙ１）のｘ１と前記三角関数の値に基いてｙ１の桁をシフトした結果値との加算または減算によって表される第１の計算上の座標を算出する第１の演算部と、
   前記第２の現実の座標（ｘ２、ｙ２）のｙ２と前記三角関数の値に基いてｘ２の桁をシフトした結果値との加算または減算によって表される第２の計算上の座標を算出する第２の演算部と、
   前記第１または前記第２の演算部における演算で用いられた前記仮の角度を積算する第３の演算部とを備え、
   前記第１および前記第２の演算部は、前記第１および前記第２の計算上の座標を互いに接近させるように演算することを特徴とする速度検出装置。
2. 前記第１および前記第２の演算部は、前記仮の角度を小さくしつつ、前記第１および前記第２の計算上の座標を収束させるように演算を繰り返し、
   前記第３の演算部は、前記第１または前記第２の演算部における演算で収束する前記仮の角度を積算し、前記第１および前記第２の計算上の座標が収束したときの該仮の角度に基づいて前記運動体の角速度を生成することを特徴とする請求項１に記載の速度検出装置。
3. 前記メモリは、前記仮の角度としてΦ_i（Φ_０＞Φ_１＞Φ_２＞・・・＞Φ_i＞・・・Φ_ｎ）（ｎは自然数）、前記三角関数の値としてｔａｎΦ_iを記憶し、
   前記第１または前記第２の計算上の座標を（ｘｘｃ_ｉ，ｙｙｃ_ｉ）とすると、
   

   

   （但し、Φ_i＝ｔａｎ^−１２⁻ⁱ（ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、前記第１の演算部においては、ｘｘｃ_ｉ−１＝ｘ１、ｙｙｃ_ｉ−１＝ｙ１であり、前記第２の演算部においては、ｘｘｃ_ｉ−１＝ｘ２、ｙｙｃ_ｉ−１＝ｙ２である）
   前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１または式２を演算し、
   前記第３の演算部は、前記仮の角度をiごとに積算することを特徴とする請求項１または請求項２に記載の速度検出装置。
4. 前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１および式２を交互または相補に演算し、かつ、前記第１の演算部が式１を演算するときには、前記第２の演算部は式２を演算し、前記第１の演算部が式２を演算するときには、前記第２の演算部は式１を演算することを特徴とする請求項３に記載の速度検出装置。
5. 前記第１の計算上の座標を保持する第３のレジスタと、
   前記第２の計算上の座標を保持する第４のレジスタと、
   前記第３の演算部で積算された前記仮の角度を保持する第５のレジスタとをさらに備えたことを特徴とする請求項２に記載の速度検出装置。
6. 前記サンプリング周期を単位時間にすることによって、前記第３の演算部で積算された前記仮の角度の２倍を前記運動体の角速度とすることを特徴とする請求項２から請求項５のいずれかに記載の速度検出装置。
7. 前記第３の演算部で積算された前記仮の角度は、前記運動体の回転角度の２分の１であることを特徴とする請求項２から請求項５のいずれかに記載の速度検出装置。
8. 当該速度検出装置は、ロータリエンコーダまたはリニアスケールに配備されていることを特徴とする請求項１に記載の速度検出装置。
9. 回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の角速度を検出する速度検出装置であって、
   予め設定された複数の仮の角度Φ_ｉ（Φ_０＞Φ_１＞Φ_２＞・・・＞Φ_ｉ＞・・・Φ_ｎ）（ｎは自然数）、および、該複数の仮の角度のそれぞれに対する三角関数の値ｔａｎΦ_ｉを記憶するメモリと、
   第１のサンプリングで得られた運動体の第１の現実の座標（ｘ１，ｙ１）を格納する第１のレジスタと、
   前記第１のサンプリングの次の第２のサンプリングで得られた運動体の第２の現実の座標（ｘ２，ｙ２）を格納する第２のレジスタと、
   前記第１の現実の座標（ｘ１、ｙ１）と前記三角関数の値ｔａｎΦ_iに基いて式１または式２を演算する第１の演算部と、
   前記第２の現実の座標（ｘ２、ｙ２）と前記三角関数の値ｔａｎΦ_iに基いて式１または式２を演算する第２の演算部と、
   前記第１または前記第２の演算部における演算で用いられた前記仮の角度Φ_iを積算する第３の演算部とを備え、
   

   

   （但し、Φ_i＝ｔａｎ^−１２⁻ⁱ（ｉ＝０、１、２・・・ｎ）であり、ｉ＝０のとき、前記第１の演算部においては、ｘｘｃ_ｉ−１＝ｘ１、ｙｙｃ_ｉ−１＝ｙ１であり、前記第２の演算部においては、ｘｘｃ_ｉ−１＝ｘ２、ｙｙｃ_ｉ−１＝ｙ２である）
   前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１または式２を演算し、
   前記第３の演算部は、前記仮の角度をiごとに積算することを特徴とする速度検出装置。
10. 前記第１および前記第２の演算部は、ｉ＝０、１、２・・・ｎの順に式１および式２を交互または相補に演算し、かつ、前記第１の演算部が式１を演算するときには、前記第２の演算部は式２を演算し、前記第１の演算部が式２を演算するときには、前記第２の演算部は式１を演算することを特徴とする請求項９に記載の速度検出装置。

Images