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

Abstract

【課題】小さな演算回路で速度を高精度に検出することができる速度検出回路を提供する。
【解決手段】速度検出装置は、仮の角度に対する三角関数を記憶するメモリと、運動体の第１の座標を格納する第１のレジスタと、次のサンプリングで得られた現実の第２の座標を格納する第２のレジスタと、第１の座標から座標軸までの回転角度をθとした場合に、現実の第２の座標を角度θだけ座標軸の方向へ回転させて得られた点と原点とを結ぶ直線上にある仮座標を計算し、仮座標の符号に基づいて第１の座標から現実の第２の座標への回転方向を判別する回転方向判別部と、第１の座標と三角関数との演算で得られた計算上の第２の座標を現実の第２の座標に接近させるように演算し、このときの三角関数に対応する仮の角度に基づいて運動体の速度を演算する演算部とを備えている。
【選択図】図２

Description

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

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

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

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

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

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

さらに、実際の動作において、電源電圧のリップルまたはノイズにより、ＳＩＮ信号およびＣＯＳ信号が揺らぐ場合がある。このような場合に、速度検出装置は運動体の移動速度または角速度を誤検出するおそれがある。

そこで、本発明は、電源電圧のリップルまたはノイズがある場合であっても、小さな演算回路で運動体の移動速度または角速度を高精度に検出することができる速度検出回路およびサーボモータを提供することである。

本発明に係る実施形態に従った速度検出装置は、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出する速度検出装置であって、
予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、前記座標の原点を回転の中心として、前記現実の第１の座標から座標軸までの回転角度をθとした場合に、前記現実の第２の座標を前記回転角度θだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標の符号または前記仮座標のｙ座標の符号に基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判別する回転方向判別部と、前記第１の座標と前記三角関数の値との乗算、加算または減算によって表された計算上の第２の座標を、前記回転方向に基づいて前記現実の第２の座標に接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度に基づいて前記運動体の速度を演算する演算部と備え、
前記回転方向判別部は、前記座標の原点を回転の中心として、前記計算上の第２の座標から座標軸までの回転角度をβとした場合に、前記現実の第２の座標を前記回転角度βだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標または前記仮座標のｙ座標の符号に基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別することを特徴とする。

本発明に係る実施形態に従ったサーボモータは、回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出するサーボモータであって、
予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、前記座標の原点を回転の中心として、前記現実の第１の座標から座標軸までの回転角度をθとした場合に、前記現実の第２の座標を前記回転角度θだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標の符号または前記仮座標のｙ座標の符号に基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判別する回転方向判別部と、前記第１の座標と前記三角関数の値との乗算、加算または減算によって表現された計算上の第２の座標を、前記回転方向に基づいて前記現実の第２の座標へ接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度を積算することにより前記運動体の速度を検出する演算部とを備え、
前記回転方向判別部は、前記座標の原点を回転の中心として、前記計算上の第２の座標から座標軸までの回転角度をβとした場合に、前記現実の第２の座標を前記回転角度βだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標または前記仮座標のｙ座標の符号に基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別することを特徴とする。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

回転方向判別部５３は、最初、座標の原点Ｏを回転の中心として、現実の第１の座標（ｓｉｎθ，ｃｏｓθ）から座標軸Ｘまでの回転角度をθとした場合に、現実の第２の座標（ｓｉｎ（θ＋Φ），ｃｏｓ（θ＋Φ））を回転角度θだけ座標軸Ｘの方向へ回転させて得られた点と原点とを結ぶ直線上にある仮座標Ｐ（ｘ’、ｙ’）を計算する。図３では、原点Ｏから現実の第１の座標までの距離ｒと原点Ｏから現実の第２の座標までの距離ｒとが等しいので、仮座標Ｐ（ｘ’、ｙ’）は、現実の第２の座標を回転角度θだけ座標軸Ｘの方向へ回転させた点に等しい。

回転方向判別部５３は、この仮座標Ｐの座標ｙ’の符号が正または負のいずれであるかに基づいて現実の第１の座標から現実の第２の座標への回転方向を判定する。例えば、図３に示す実施形態では、ｙ’は正値であるので、回転方向判別部５３は、ロータがＣＣＷ方向に回転していると判別する。逆に、ｙ’が負値の場合、回転方向判別部５３は、ロータがＣＷ方向に回転していると判別する。

仮座標Ｐ（ｘ’、ｙ’）は、最初（ｉ＝０のとき）、式１および式２によって計算される。式１および式２の導出については後述する。
ｘ’＝ｘ２・ｘ１＋ｙ２・ｙ１ （式１）
ｙ’＝−ｘ２・ｙ１＋ｘ１・ｙ２ （式２）

回転方向判別部５３は、ｘ’およびｙ’の両方を算出してもよいが、その両方を算出する必要は必ずしも無く、その一方のみを算出してもよい。例えば、上記の具体例では、回転方向判別部５３は、ｙ’の符号に応じてロータの回転方向を判別する。従って、回転方向判別部５３は、式２のみを計算すれば足りる。

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

第３のレジスタＲ３１は、この計算の途中において、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を格納する。計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）は式３または式４のように表わすことができる。

回転方向判別部５３は、例えば、ｙ’の符号に応じて式３または式４のいずれかを選択する。ＣＯＲＤＩＣローテーション５１は、回転方向判別部５３の選択に従って式３または式４の演算を実行する。

あるいは、

例えば、ｙ’の符号が正である場合、図３に示す現実の第２の座標は、原点Ｏを中心として現実の第１の座標よりもＣＣＷ方向へ回転させた位置にあることが分かる。よって、ＣＯＲＤＩＣローテーション５１が現実の第２の座標をＣＷ（Clock Wise）方向へ移動させた計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を算出するように、回転方向判別部５３は、最初に式４を選択する。

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

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

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

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

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

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

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

図４（Ａ）および図４（Ｂ）は、現実の第２の座標および計算上の第２の座標の位置関係、および、回転方向判別部５３における初回以降の計算の概念を示す図である。

最初の計算以降（ｉ≧１）、回転方向判別部５３は、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）および現実の第２の座標（ｘ２，ｙ２）を用いて、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）から現実の第２の座標（ｘ２，ｙ２）への回転方向を判別する。より詳細には、図４（Ａ）に示す原点Ｏを回転の中心として、計算上の第２の座標（ｘｃ_ｉ−１，ｙｃ_ｉ−１）から座標軸Ｘまでの回転角度をβとした場合に、回転方向判別部５３は、現実の第２の座標（ｘ２，ｙ２）を回転角度βだけ座標軸Ｘの方向へ回転させて得られた点と原点Ｏとを結ぶ直線上にある仮座標Ｐ（ｘ’、ｙ’）のｘ’またはｙ’の少なくとも一方を計算する。図４（Ａ）および図４（Ｂ）では、原点Ｏから現実の第２の座標までの距離ｒと原点Ｏから計算上の第２の座標までの距離ｒとが等しいので、仮座標Ｐ（ｘ’、ｙ’）は、現実の第２の座標を回転角度βだけ座標軸Ｘの方向へ回転させた点に等しい。

回転方向判別部５３は、この仮座標Ｐの座標ｙ’ の符号が正または負のいずれであるかに基づいて計算上の第２の座標から現実の第２の座標への回転方向を判別する。例えば、図４（Ｂ）に示す実施形態では、ｙ’が負値であるので、回転方向判別部５３は、計算上の第２の座標から現実の第２の座標への回転方向がＣＷ方向であると判別する。逆に、距離ｙ’が正値の場合、回転方向判別部５３は、計算上の第２の座標から現実の第２の座標への回転方向がＣＣＷ方向であると判別する。

仮座標Ｐ（ｘ’、ｙ’）は、初回以降（ｉ≧１のとき）、式１６および式１７によって計算される。
ｘ’＝ｘｃ_ｉ−１・ｘ１＋ｙｃ_ｉ−１・ｙ１ （式１６）
ｙ’＝−ｘｃ_ｉ−１・ｙ１＋ｘ１・ｙｃ_ｉ−１ （式１７）

回転方向判別部５３は、ｘ’およびｙ’の両方を算出してもよいが、その両方を算出する必要は必ずしも無く、その一方のみを算出してもよい。例えば、上記の具体例では、回転方向判別部５３は、ｙ’の符号に応じて計算上の第２の座標から現実の第２の座標への回転方向を判別する。従って、回転方向判別部５３は、式２のみを計算すれば足りる。

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

初回以降、計算上の第２の座標が更新されるごとに、回転方向判別部５３は、式１６または式１７を繰り返し実行し、並びに、ＣＯＲＤＩＣローテーション５１は、式３または式４を繰り返し実行する。これにより、図５に示すように、計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標（ｘ２，ｙ２）へ収束させることができる。

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

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

次に、ＣＯＲＤＩＣローテーション５１は、計算上の第２の座標（ｘｃ_０，ｙｃ_０）、および、ｉ＝１における式１３から式１５の値を式４に代入し、計算上の第２の座標（ｘｃ_１，ｙｃ_１）を得る。このとき、図２の選択部５６は、第３のレジスタＲ３１を選択して計算上の第２の座標（ｘｃ_０，ｙｃ_０）をＣＯＲＤＩＣローテーション５１および回転方向判別部５３へ送る。ＣＯＲＤＩＣローテーション５１は式４を用いる。回転方向判別部５３が式４を選択する方法については上述の通りである。

計算上の第２の座標（ｘｃ_１，ｙｃ_１）は、計算上の第２の座標（ｘｃ_０，ｙｃ_０）から現実の第２の座標（ｘ２，ｙ２）へ向かってＣＷ方向へΦ１だけ回転させた座標になる。ここで、加算器８０は、回転角保持レジスタＲ４１に格納されていた仮の角度＋Φ_０または−Φ_０に仮の角度＋Φ_１または−Φ_１を加算し、この加算結果を回転角保持レジスタＲ４１に戻す。

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

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

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

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

本実施形態では、現実の第１の座標または計算上の第２の座標とＸ軸との間の回転角度（θまたはβ）だけ、現実の第２の座標を回転移動させる。これによって、回転方向判別部５３は、現実の第１の座標または計算上の第２の座標が、現実の第２の座標に対してＣＷ方向側にあるのか、ＣＣＷ方向側にあるのかを正確に知ることができる。

原点Ｏから現実の第１の座標までの距離および原点Ｏから現実の第２の座標までの距離が異なる場合であっても、同様に、回転方向判別部５３は、現実の第１の座標または計算上の第２の座標と現実の第２の座標との位置関係を正確に知ることができる。

例えば、実際の動作においては、電源電圧のリップルまたはノイズにより、ＳＩＮ信号およびＣＯＳ信号が揺らぐことがある。このような場合、図６に示すように、原点Ｏから現実の第１の座標までの距離と原点Ｏから現実の第２の座標までの距離とが異なる。このような場合、ロータはＣＣＷ方向へ回転しているにもかかわらず、ｙ２はｙ１よりも小さい。従って、単にｙ１およびｙ２の大小関係から判断すると、ロータの回転方向を誤って判断するおそれがある。

しかし、本実施形態によれば、図６に示すように仮座標Ｐのｙ’は正値であるので、回転方向判別部５３は、現実の第２の座標が現実の第１の座標のＣＣＷ方向側にあることを判別することができる。つまり、本実施形態によれば、電源電圧にリップルまたはノイズが生じたとしても、回転方向判別部５３は、現実の第１の座標と現実の第２の座標との位置関係を正確に判別することができる。このことは、計算上の第２の座標と現実の第２の座標との位置関係についても同様のことが言える。

原点Ｏから現実の第１の座標までの距離と原点Ｏから現実の第２の座標までの距離とが異なったとしても、式１および式２、式１３および式１４は、そのまま回転方向の判別に適用することができる。例えば、図６に示す現実の第２の座標（ｘ２、ｙ２）と仮座標Ｐとの関係は、式１８のように表わされる。

式１８〜式２１により、式２２および式２３が導出される。
ｘ’＝（１／ｒ１）・（ｘ２・ｘ１＋ｙ２・ｙ１） 式２２
ｙ’＝（１／ｒ１）・（−ｘ２・ｙ１＋ｘ１・ｙ２） 式２３

原点Ｏから現実の第１の座標（ｘ１、ｙ１）までの距離ｒ１は常に正であるので、距離ｒ１に関わらず、ｘ’およびｙ’の符号は式２で決定される。

以上の実施形態による回転方向判別部５３は、Ｘ軸を基準として、現実の第１の座標または計算上の第２の座標と現実の第２の座標との位置関係を判別していた。しかし、勿論、回転方向判別部５３は、Ｙ軸を基準としてこれらの位置関係を判別してよい。この場合には、式２のｘ’の符号が正であるか負であるかによって、現実の第１の座標または計算上の第２の座標と現実の第２の座標との位置関係を判別する。つまり、回転方向判別部５３は、式２のみ演算すれば足り、それにより得られたｘ’の符号に応じて式３または式４のいずれかを選択すればよい。Ｙ軸を基準とした場合の回転方向判別部５３の動作は、上記実施形態から容易に推測可能であるので、ここでは省略する。

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

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

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

本発明に係る実施形態に従った速度検出装置１００のブロック図。 演算部５０の内部構成を示すブロック図。 回転方向判別部５３における最初の計算の概念を示す図。 回転方向判別部５３における初回以降の計算の概念を示す図。 計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を現実の第２の座標（ｘ２，ｙ２）へ収束させるときの概念図。 電源電圧にリップルまたはノイズが生じた場合における回転方向の計算の概念を示す図。 ２回のサンプリングで得られた座標を示す図。

符号の説明

５０…演算部
５１…ＣＯＲＤＩＣローテーション
５２…メモリ
５３…回転方向判別部
５６…選択部
Ｒ１１…第１のレジスタ
Ｒ２１…第２のレジスタ
Ｒ３１…第３のレジスタ

Claims (10)

1. 回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出する速度検出装置であって、
   予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、
   第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、
   前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、
   前記座標の原点を回転の中心として、前記現実の第１の座標から座標軸までの回転角度をθとした場合に、前記現実の第２の座標を前記回転角度θだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標の符号または前記仮座標のｙ座標の符号に基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判別する回転方向判別部と、
   前記第１の座標と前記三角関数の値との乗算、加算または減算によって表された計算上の第２の座標を、前記回転方向に基づいて前記現実の第２の座標に接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度に基づいて前記運動体の速度を演算する演算部と備え、
   前記回転方向判別部は、前記座標の原点を回転の中心として、前記計算上の第２の座標から座標軸までの回転角度をβとした場合に、前記現実の第２の座標を前記回転角度βだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標または前記仮座標のｙ座標の符号に基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別することを特徴とする速度検出装置。
2. 前記メモリは、前記仮の角度としてΦ_０〜Φ_ｎ（Φ_０＞Φ_１＞Φ_２＞・・・Φ_ｎ）（ｎは自然数）、前記三角算数の値としてｃｏｓΦ_０〜ｃｏｓΦ_ｎ、および、ｔａｎΦ_０〜ｔａｎΦ_ｎを記憶し、
   前記演算部は、前記第１の座標を（ｘ１，ｙ１）、前記現実の第２の座標を（ｘ２，ｙ２）、および、前記計算上の第２の座標を（ｘｃ_ｉ，ｙｃ_ｉ）とすると、
   

   

   あるいは、
   

   

   を積算することを特徴とする請求項１に記載の速度検出装置。
3. 前記回転方向判別部は、ｉ＝０のとき、式１または式２を演算することによって前記仮座標（ｘ’、ｙ’）を計算し、
   ｘ’＝ｘ２・ｘ１＋ｙ２・ｙ１ （式１）
   ｙ’＝−ｘ２・ｙ１＋ｘ１・ｙ２ （式２）
   前記ｘ’の符号または前記ｙ’の符号が正または負のいずれであるかに基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判定し、前記回転方向に応じて式３または式４のいずれかを選択し、
   ｉ≧１のとき、式１６または式１７を演算することによって前記仮座標（ｘ’、ｙ’）を計算し、
   ｘ’＝ｘ２・ｘｃ_ｉ−１＋ｙ２・ｙｃ_ｉ−１ （式１６）
   ｙ’＝−ｘ２・ｙｃ_ｉ−１＋ｘｃ_ｉ−１・ｙ２ （式１７）
   前記ｘ’の符号または前記ｙ’の符号が正または負のいずれであるかに基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別し、前記回転方向に応じて式３または式４のいずれかを選択することを特徴とする請求項２に記載の速度検出装置。
4. 前記演算部は、前記回転方向判別部によって選択された式３または式４を用いて、前記計算上の第２の座標（ｘｃ_ｉ，ｙｃ_ｉ）を各ｉに関してｉ＝０、１、２・・・の順番で計算し、前記現実の第２の座標に最も近い前記計算上の第２の座標に用いられた前記仮の角度に基づいて前記運動体の速度を演算することを特徴とする請求項３に記載の速度検出装置。
5. 前記計算上の第２の座標を逐次保持する第３のレジスタと、
   前記仮の角度を積算するための第４のレジスタとをさらに備えたことを特徴とする請求項１に記載の速度検出装置。
6. 回転運動または往復運動を行う運動体の位置を示す座標を周期的にサンプリングし、前記座標に基づいて前記運動体の速度を検出するサーボモータであって、
   予め設定された複数の仮の角度、および、該複数の仮の角度のそれぞれに対する三角関数の値を記憶するメモリと、
   第１のサンプリングで得られた運動体の第１の座標を格納する第１のレジスタと、
   前記第１のサンプリングの次の第２のサンプリングで得られた運動体の現実の第２の座標を格納する第２のレジスタと、
   前記座標の原点を回転の中心として、前記現実の第１の座標から座標軸までの回転角度をθとした場合に、前記現実の第２の座標を前記回転角度θだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標の符号または前記仮座標のｙ座標の符号に基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判別する回転方向判別部と、
   前記第１の座標と前記三角関数の値との乗算、加算または減算によって表現された計算上の第２の座標を、前記回転方向に基づいて前記現実の第２の座標へ接近させるように演算し、この計算上の第２の座標に用いられた前記三角関数の値に対応する前記仮の角度を積算することにより前記運動体の速度を検出する演算部とを備え、
   前記回転方向判別部は、前記座標の原点を回転の中心として、前記計算上の第２の座標から座標軸までの回転角度をβとした場合に、前記現実の第２の座標を前記回転角度βだけ前記座標軸の方向へ回転させて得られた点と前記原点とを結ぶ直線上にある仮座標のｘ座標またはｙ座標の少なくとも一方を計算し、前記仮座標のｘ座標または前記仮座標のｙ座標の符号に基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別することを特徴とするサーボモータ。
7. 前記メモリは、前記仮の角度としてΦ_０〜Φ_ｎ（Φ_０＞Φ_１＞Φ_２＞・・・Φ_ｎ）（ｎは自然数）、前記三角算数の値としてｃｏｓΦ_０〜ｃｏｓΦ_ｎ、および、ｔａｎΦ_０〜ｔａｎΦ_ｎを記憶し、
   前記演算部は、前記第１の座標を（ｘ１，ｙ１）、前記現実の第２の座標を（ｘ２，ｙ２）、および、前記計算上の第２の座標を（ｘｃ，ｙｃ）とすると、
   

   

   あるいは、
   

   

   を計算することを特徴とする請求項５に記載のサーボモータ。
8. 前記回転方向判別部は、ｉ＝０のとき、式１または式２を演算することによって前記仮座標（ｘ’、ｙ’）を計算し、
   ｘ’＝ｘ２・ｘ１＋ｙ２・ｙ１ （式１）
   ｙ’＝−ｘ２・ｙ１＋ｘ１・ｙ２ （式２）
   前記ｘ’の符号または前記ｙ’の符号が正または負のいずれであるかに基づいて前記現実の第１の座標から前記現実の第２の座標への回転方向を判定し、前記回転方向に応じて式３または式４のいずれかを選択し、
   ｉ≧１のとき、式１６または式１７を演算することによって前記仮座標（ｘ’、ｙ’）を計算し、
   ｘ’＝ｘ２・ｘｃ_ｉ−１＋ｙ２・ｙｃ_ｉ−１ （式１６）
   ｙ’＝−ｘ２・ｙｃ_ｉ−１＋ｘｃ_ｉ−１・ｙ２ （式１７）
   前記ｘ’の符号または前記ｙ’の符号が正または負のいずれであるかに基づいて前記計算上の第２の座標から前記現実の第２の座標への回転方向を判別し、前記回転方向に応じて式３または式４のいずれかを選択することを特徴とする請求項７に記載の速度検出装置。
9. 前記演算部は、前記回転方向判別部によって選択された式３または式４を用いて、前記計算上の第２の座標（ｘｃ，ｙｃ）を各ｉに関してｉ＝０、１、２・・・の順番で計算し、前記現実の第２の座標に最も近い前記計算上の第２の座標に用いられた前記仮の角度を積算することにより前記運動体の速度を検出することを特徴とする請求項８に記載のサーボモータ。
10. 前記計算上の第２の座標を逐次保持する第３のレジスタをさらに備えたことを特徴とする請求項６に記載のサーボモータ。

Images