CN101196532B - 速度检测装置 - Google Patents

Abstract

提供可用小规模的运算电路高精度地检测移动速度或角速度的速度检测电路及伺服马达。所述速度检测装置设有：存储暂设角度Φ_i和tanΦ_i的存储器(52)；存储运动体的第1实际座标的第1寄存器(R11)；存储第2实际座标的第2寄存器(R21)；算出第1计算座标的运算部(51)，该第1计算座标通过将第1实际座标(x1、y1)的x1和按照tanΦ_i移动y1的数位后的结果值作加减运算而表达；算出第2计算座标的运算部(58)，该第2计算座标通过第2实际座标(x2、y2)的y2和按照tanΦ_i移动x2的数位后的结果值相加或相减来表达；以及累计暂设角度的运算部(80)。所述运算部(51、58)的演算使第1和第2实际座标相互接近。

Description

速度检测装置

技术领域

本发明涉及速度检测装置。

背景技术

在伺服马达的运动控制中，在位置检测器内部生成基于机械的移动距离或轴的移动角度的SIN信号和COS信号。位置检测器的运算电路将SIN信号和COS信号按每单位时间进行采样，从2个采样间的距离差或角度差和2次采样的时间间隔算出移动速度或角速度。

例如，如果将2次采样中得到的座标设为(x1、y1)和(x2、y2)，将采样的时间间隔设为t(sec)，则角速度ω(rad/sec)可以如式0表示。

ω＝(θ2-θ1)/t＝(tan^-1(y2/x2)-tan^-1(y1/x1))/t  (式0)

式中，θ1(rad)和θ2(rad)是2次采样时刻的绝对角度(偏转角)。如式1所示，为了得到角速度ω，须根据座标算出偏转角。

[非专利文献1]杉本英彦等“ACサ-ボシステムの理論と設計の實際(AC伺服系统的理论与设计实践)”綜合電子出版社(P142～149)

发明内容

为了对上述问题进行处理，作为运算电路，使用CPU或定制的LSI(Large-Scale Integrated Circuit：大规模集成电路)，考虑用数字逻辑电路实现式1的运算。

但是，反正切(tan^-1)需要进行sin/cos这样的除法。在反正切的计算中，存在以下两个问题。一个问题是，如果单位圆上的COS信号接近于0，则正切(sin÷cos)值的绝对值变得非常大。其结果，反正切的结果的误差会很大。

另一问题是，为了求出反正切和正切，必需进行除法(sin÷cos)运算。除法器是难以用数字电路实现的电路，需采用比起加减法来规模大得多的数字电路才能实现。

还有，为了用数字电路来实现，乘法器也需使用比起加减法来规模大得多的数字电路。

为此，本发明提供可用小规模运算电路高精度地检测运动体速度的速度检测电路。

本发明实施例的速度检测装置周期地采样表示进行旋转运动或往复运动的运动体位置，并根据上述座标检测上述运动体的角速度，其特征在于设有：

存储预设的多个暂设角度，以及分别对应于该多个暂设角度的三角函数值的存储器；存储由第1采样得到的运动体的第1实际座标的第1寄存器；存储由接着上述第1采样的第2采样得到的运动体的第2实际座标的第2寄存器；算出第1计算座标的第1运算部，该第1计算座标通过上述第1实际座标(x1、y1)的x1和按照上述三角函数值移动y1的数位后的结果值之间的相加或相减来表达；算出第2计算座标的第2运算部，该第2计算座标通过上述第2实际座标(x2、y2)的y2和按照上述三角函数值移动x2的数位后的结果值之间的相加或相减来表达；以及累计在上述第1或上述第2运算部的演算中所用的上述暂设角度的第3运算部；上述第1和上述第2运算部的演算使上述第1和上述第2实际座标相互接近。

上述第1和上述第2运算部一边减小上述暂设角度，一边使上述第1和上述第2计算座标收敛地重复运算，上述第3运算部累计上述第1或上述第2运算部的演算中收敛的上述暂设角度，根据上述第1和上述第2计算座标收敛时的该暂设角度，生成上述运动体的角速度。

上述存储器，存储Φ_i(Φ₀＞Φ₁＞Φ₂＞…＞Φ_i＞…Φ_n)(n为自然数)作为上述暂设角度，存储tanΦ_i作为上述三角函数值，

如果将上述第1或上述第2计算座标设为(xxc_i，yyc_i)，则有，

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式1)

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式2)

(式中，Φ_i＝tan^-12^-1(i＝0、1、2…n)，i＝0时，在上述第1运算部中，xxc_i-1＝x1、yyc_i-1＝y1、在上述第2运算部中，xxc_i-1＝x2、yyc_i-1＝y2)

上述第1和上述第2运算部按i＝0、1、2…n的顺序演算式1或式2，上述第3运算部也可以按每个i累计上述暂设角度。

上述第1和上述第2运算部按i＝0、1、2…n的顺序交互或互补地演算式1或式2，而且在上述第1运算部演算式1时上述第2运算部演算式2，上述第1运算部演算式2时上述第2运算部也可以演算式1。

该速度检测装置还可设有：保存上述第1计算座标的第3寄存器；保存上述第2计算座标的第4寄存器；保存在上述第3运算部中累计的上述暂设角度的第5寄存器。

通过将上述采样周期设为单位时间，可将上述第3运算部中累计的上述暂设角度的2倍作为上述运动体的角速度。

在上述第3运算部中累计的上述暂设角度也可以是上述运动体的旋转角度的二分之一。

该速度检测装置也可以设在旋转编码器或线性标尺上。

本发明实施例的速度检测装置周期地采样表示进行旋转运动或往复运动的运动体位置的座标，并根据上述座标检测上述运动体的角速度，设有：

存储预设的多个暂设角度Φ_i(Φ₀＞Φ₁＞Φ₂…Φ_i＞…Φ_n)(n为自然数)，以及分别对应于该多个暂设角度的三角函数值tanΦ₁的存储器；存放于第1采样中得到的运动体的第1实际座标(x1、y1)的第1寄存器；存放于接着上述第1采样的第2采样中得到的运动体的第2实际座标(x2、y2)的第2寄存器；基于上述第1实际座标(x1、y1)和上述三角函数值tanΦ_i来演算式1或式2的第1运算部；基于上述第2实际座标(x2、y2)和上述三角函数值tanΦ_i来演算式1或式2的第2运算部；累计在上述第1或上述第2运算部的演算中使用的上述暂设角度Φ_i的第3运算部；

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式1)

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式2)

(式中，Φ_i＝tan^-12^-i(i＝0、1、2…n)，i＝0时，在上述第1运算部中xxc_i-1＝x1、yyc_i-1＝y1，在上述第2运算部中xxc_i-1＝x2、yyc_i-1＝y2)

上述第1和上述第2运算部按i＝0、1、2…n的顺序演算式1或式2，上述第3运算部按每个i累计上述暂设角度。

上述第1和上述第2运算部按i＝0、1、2…n的顺序交互或互补地演算式1或式2，而且，可在上述第1运算部演算式1时由上述第2运算部演算式2，在上述第1运算部演算式2时由上述第2运算部演算式1。

本发明的速度检测装置可用小规模的运算电路高精度地检测运动体的速度。

附图说明

图1是本发明实施例的速度检测装置100的方框图。

图2是表示FPGA50的内部结构的方框图。

图3是表示第1实际座标和第2实际座标的位置关系的图。

图4是使第1计算座标和第2计算座标向中间座标收敛时的概念图。

图5是表示分割座标为8个区域的图。

图6是判别某个座标是否属于图5所示区域I～VIII中任何一个的流程图。

图7是根据第1座标、实际的第2座标和计算的第2座标各自所属的区域，规定计算的第2座标的旋转方向CCW或CW的表。

标记的说明

50…FPGA

51、58…CORDIC旋转单元

52、59…存储器

53…旋转方向判别部

54、55…区域判别部

56、57…选择部

R11…第1寄存器

R21…第2寄存器

R31…第3寄存器

R41…第4寄存器

R51…旋转角度寄存器

R61…角速度保持寄存器

具体实施方式

以下，参照附图说明本发明的实施例。本实施例不是对本发明的限定。

图1是本发明实施例的速度检测装置100的方框图。速度检测装置100设有：旋转编码器或线性标尺10；A/D转换器40；以及运算部50。运算部50是通用CPU或定制LSI。运算部50例如可为FPGA(Field Programmable Gate Array：现场可编程门阵列)。速度检测装置100也可设在例如具有进行旋转运动或往复运动的运动体的伺服马达上。运动体是例如设在工作机械上的伺服马达的转子或往复运动的臂等。速度检测装置100也可装在例如使用旋转编码器或线性标尺10的定位装置上。

旋转编码器或线性标尺10输出表示进行旋转运动或往复运动的运动体位置的座标(sin、cos)的信号。该座标(sin、cos)是相位正交的正弦波值。A/D转换器40将来自旋转编码器或线性标尺的sin信号和cos信号变换成数字信号。还有，运算部50接受分别基于来自A/D转换器40的sin信号和cos信号的数字信号，算出运动体的角速度或速度。

运算部50可以是FPGA，但也可以用其它任意的LSI取代FPGA。将运算部50在定制的LSI上作为逻辑电路实现时，可以用短时间进行重复相同处理的反复处理。另一方面，运算部50也可以用通用CPU及软件实现。将运算部50用通用CPU及软件实现时，可以用程序的变更进行各种处理。

图2是表示运算部50的内部结构的方框图。运算部50设有：作为第1运算部的CORDIC旋转单元(以下也称为旋转单元)51；作为第2运算部的CORDIC旋转单元58；存储器52、59；旋转方向判别部53；区域判别部54、55；选择部56、57；从第1至第4寄存器R11、R21、R31和R41；作为第5寄存器的旋转角度寄存器R51；角速度保持寄存器R61；以及作为第3运算部的加法器80。

图3是表示第1实际座标(sinθ、cosθ)和第2实际座标(sin(θ+Φ)、cos(θ+Φ))的位置关系的图。第2实际座标(sin(θ+Φ)、cos(θ+Φ))如图3所示，是将第1实际座标(sinθ、cosθ)作为基准，使旋转单元旋转Φ时的实际座标。在图3的纸面上，旋转单元沿CCW(逆时针旋转)方向(图3的箭头方向)旋转。还有，θ及Φ的单位是rad(弧度)。

参照图2及图3，说明根据sin信号和cos信号算出旋转单元从第1实际座标至第2实际座标旋转时的角速度的计算过程。还有，由于往复运动的运动体速度可根据角速度简单地算出，这里，仅说明角速度的计算。

在本实施例中，用CORDIC(Coordinate Rotation Digital Computer：座标旋转数字计算机)算法来算出角速度。在CORDIC算法中，有定向方式和旋转方式。在本实施例中，利用旋转方式。该CORDIC算法内装在运算部50中，在本实施例中用逻辑电路实现。

为便于理解，暂设作为运动体的转子旋转1周，旋转编码器10输出1个周期的sin波和cos波。速度检测装置100在1个周期中，周期性(周期T)地进行转子座标的采样。速度检测装置100在第1采样中获得转子的二维座标(欧几里得座标)(x1、y1)，在接着第1采样的第2采样中获得一定时间后的二维座标(x2、y2)。x1和x2是sin值，y1和y2是cos值。速度检测装置100根据座标(x1、y1)和座标(x2、y2)，用CODIC算法计算从第1采样时转子的位置至第2采样时转子的位置的角度Φ。由于采样以预定周期进行，一旦该角度Φ判明，就可得到转子的角速度。

存储器52存储预设的多个暂设角度Φ₀～Φ_n(Φ₀＞Φ₁＞Φ₂＞…Φ_n)(n为自然数)，以及对应于该多个暂设角度的各三角函数值tanΦ₀～tanΦ_n。在速度检测装置100的初始状态中，第1至第4寄存器不存放特定值。第5寄存器R51为零。还有，第5寄存器R51每次采样被复位至零。

首先，运算部50从A/D转换器40以数字值接受在第1采样中得到的第1实际座标(x1、y1)＝(sinθ、cosθ)。第1座标存放于第2寄存器R21中。

最初，第1、第3和第4寄存器R11、R31和R41是不确定的。这时的计算值因不确定值而排除。

接着，运算部50在下一周期进行第2采样。运算部50从A/D转换器40以数字值接受在第2采样中得到的转子的第2实际座标(x2、y2)＝(sin(θ+Φ)，cos(θ+Φ))。这时，第1实际座标(sinθ，cosθ)被转移到第1寄存器R11，第2实际座标(sin(θ+Φ)，cos(θ+Φ))存放于第2寄存器R21中。

当接受了第2实际座标时，由于第3寄存器R31仍是不确定的，选择部56选择第1寄存器R11，将第1实际座标(sinθ，cosθ)向区域判别部54及旋转单元51发送。另外，这时，由于第4寄存器R41也还是不确定的，选择部57选择第2寄存器R21，将第2实际座标(sin(θ+Φ)，cos(θ+Φ))向区域判别部55及旋转单元58发送。

旋转单元51用第1座标(x1，y1)＝(sinθ，cosθ)及来自存储器52的三角函数值tanΦ₀计算第1计算座标(xxc₁，yyc₁)。另一方面，旋转单元58用第2实际座标(x2，y2)＝(sin(θ+Φ)，cos(θ+Φ))以及来自存储器59的三角函数值tanΦ₀计算第2计算座标(xxc₁，yyc₁)。有关旋转单元51和58的具体运算内容后述。再者，由于旋转单元51和58一起计算后述的式1和式2，第1和第2计算座标用(xxc_i，yyc_i)表示。但是，第1计算座标和第2计算座标是相互不同的座标。i等于旋转单元51、58已执行的运算次数。

第3寄存器R31存放第1计算座标(xxc₁，yyc₁)。第4寄存器R41存放第2计算座标(xxc₁，yyc₁)。其后，选择部56及57分别选择第3和第4寄存器R31和R41，直到进行下一周期的采样。旋转单元51和58用分别存放在第3和第4寄存器R31和R41上的座标和三角函数值tanΦ₁重复同样的计算。它们的计算结果作为第1计算座标(xxc_i，yyc_i)和第2计算座标(xxc_i，yyc_i)，分别重写在第3和第4的寄存器R31和R41上。这样，旋转单元51和58依次用三角函数值tanΦ₀、tanΦ₁、tanΦ₂…tanΦ_i计算第1计算座标和第2计算座标。在对应于i＝0、1、2…的各运算中，旋转单元51和58使第1计算座标和第2计算座标相互接近地进行运算。从而，第1计算座标和第2计算座标在每次由旋转单元51和58计算时，向第1实际座标与第2实际座标的座标之间的中间座标(sin(θ+Φ/2)，cos(θ+Φ/2))收敛。每次采样确定中间座标，向旋转角度寄存器R51发送。旋转角度寄存器R51内的数据在每次采样时被更新。

加法器80在存放于转角保持寄存器R51的暂设角度+Φ_i-1或-Φ_i-1上相加暂设角度-Φ_i或+Φ_i，并将该相加结果送还转角保持寄存器R41。这样，加法器80累计暂设角度±Φ₀、±Φ₁、±Φ₂、…Φ_i，转角保持寄存器R51保存所累计的暂设角度。暂设角度Φ_i收敛时，存放于转角保持寄存器R51的角度成为从第1实际座标向中间座标的旋转角Φ/2。转角保持寄存器R51内的旋转角在每次采样时被更新。这里，所谓累计是对前次的运算结果再次重复进行加法或减法的运算。例如，被累计的暂设角度可以如Φ₀-Φ₁+Φ₂-Φ₃+…那样表示。

实际的转子旋转角度通过将旋转角度寄存器R51内的数据设为2倍来求得。要将数据设为2倍将数字值左移1位即可，所谓左移，是使某个数字向构成更大值的方向移动数位。因而，这时的运算不需要乘法器。该运算在旋转角度寄存器R51、角速度保持寄存器R61或FPGA50的外部电路这三者之一中进行即可。

由于实际座标的采样周期是一定的，转子的旋转角度Φ的值可直接作为相对角速度使用。如果将实际座标的采样周期设为单位时间，则转子的旋转角度Φ的值可以直接作为绝对角速度使用。角速度存放于角速度保持寄存器R61，根据需要而输出。角速度保持寄存器R61内的角速度在每次采样时被更新。

再有，加法器80可从旋转单元58获得中间座标(sin(θ+Φ/2)，cos(θ+Φ/2))，因为第1计算座标和第2计算座标向同一中间座标收敛。

区域判别部54判别存放于第1寄存器R11的第1实际座标所属的区域和存放于第3寄存器R31的第1计算座标所属的区域这两个区域中的任何一方。亦即，区域判别部54判别由选择部56选择的座标所属的区域。判别部55判别存放于第2寄存器R21的第2实际座标所属的区域和存放于第4寄存器R41的第2计算座标所属的区域这两个区域中的任何一方。亦即，区域判别部55判别由选择部57选择的座标所属的区域。

接着，说明旋转单元51和58的具体运算内容。第1和第2计算座标(xxc_i，yyc_i)可以如式1或式2那样表示。比较器53根据判别部54和55的判别结果，选择式1和式2中的一个。旋转单元51和58根据比较器53的比较结果，进行式1或式2的运算。旋转单元58接受经由倒相器60的比较器53的比较结果的反相信号。因而，旋转单元58进行运算，使第2实际座标或第2计算座标向着与第1实际座标或第1计算座标的移动方向相反的方向移动。

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式1)

或

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*{\tan \mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*{\tan \mathrm{\Φ}}_i\end{array}\right\}$ (式2)

式中，tanΦ_i＝2^-i(i＝0、1、2…n)。i＝0时，作为给予旋转单元51的初始值的第1实际座标是xxc_i-1＝x1、yyc_i-1＝y1。另外，作为给予旋转单元58的初始值的第2实际座标是xxc_i-1＝x2、yyc_i-1＝y2。

现就式1和式2进行说明。首先，第1实际座标和第2实际座标可以如式3～式6那样表示。

x(i-1)＝cosθ       (式3)

y(i-1)＝sinθ       (式4)

x(i)＝cos(θ+Φ)    (式5)

y(i)＝sin(θ+Φ)    (式6)

将式5和式6用三角函数的加法定理变形，得到式7和式8。

x(i)＝cosθ·cosΦ-sinθ·sinΦ     (式7)

y(i)＝sinθ·cosΦ+cosθ·sinΦ     (式8)

将式3和式4代入式7和式8进行变形，得到式9～式12。

x(i)/cosΦ＝x(i-1)-y(i-1)·tanΦ    (式9)

y(i)/cosΦ＝y(i-1)+x(i-1)·tanΦ    (式10)

设x(i)/cosΦ为xxc(i)、y(i)/cosΦ为yyc(i)，式9和式10可分别表示为式9′和式10′。

xxc(i)＝x(i-1)-y(i-1)·tanΦ    (式9′)

yyc(i)＝y(i-1)+x(i-1)·tanΦ    (式10′)

这里，利用CORDIC算法。更具体地说，将tanΦ的值如式11那样限制在±2^-i。设如此限制的tanΦ为tanΦi。

tanΦi＝±2^-i＝±1，±2^-1，±2^-2，…(i＝0、1、2、…n)    (式11)

这时，Φi取得的值如式12那样被限制。该角度Φi取离散数值，该Φi为“暂设角度”。

Φi＝tan^-1(±2^-i)＝±0.78rad，±0.46rad，±0.25rad，±0.12rad…  (式12)

将三角函数值tanΦi代入式9和式10，得到计算座标。这里，旋转单元51和58的各初始值(i＝0)分别是存放于图2的第1寄存器R11的第1实际座标(x1、y1)和存放于第2寄存器R21的第2实际座标(x2、y2)。亦即，在旋转单元51中，(xxc(0)、xxc(0))＝(x1、y1)，在旋转单元58中，(xxc(0)、xxc(0))＝(x2、y2)。第1和第2实际座标由第1和第2采样来确定。

(最初的运算(i＝0))

旋转单元51最初执行将作为初始值的第1实际座标(x1、y1)代入到式9和式10的运算。即，旋转单元51演算式13和式14。这时，选择部56选择第1寄存器R11。

xxc1＝x1-y1·tanΦ0    (式13)

yyc1＝y1+x1·tanΦ0    (式14)

如式11所示，tanΦi＝±2^-i。因而，包含在式13和式14中的乘项(y1·tanΦ0)和(x1·tanΦ0)根据三角函数值tanΦ0＝±2^-i，将作为数字值(二进制值)表示的y1和x1移动数位即可。也就是，旋转单元51在实际上不必演算乘法。从而，旋转单元51不需要乘法器。但是，在由式13、式14表示的最初的运算中，由于tanΦ0＝1，旋转单元51不进行数位的移动。其后，旋转单元51将x1或y1与乘项的结果值相加或相减。从而，旋转单元51可不进行乘法地演算式13和式14。

另一方面，旋转单元58最初执行将作为初始值的第2实际座标(x2、y2)代入式9和式10后的运算。即，旋转单元58演算式15和式16。这时，选择部57选择第2寄存器R21。

xxc1＝x2+y2·tanΦ0    (式15)

yyc1＝y2-x2·tanΦ0    (式16)

实际上，旋转单元58也不必为进行式15和式16中所含乘项(y2·tanΦ0)和(x2·tanΦ0)的运算而进行乘法运算，仅移动作为数字值(二进制值)表示的y2或x2的数位即可。从而，旋转单元58不需要乘法器。但是，在用式15和式16表示的最初运算中，由于tanΦ0＝1，旋转单元58不进行数位移动。其后，旋转单元58将x2或y2与乘项的结果值相加或相减。从而，旋转单元58可以不进行乘法地演算式15和式16。

(第2次以后的运算(i＝1…))

在第2次以后的运算中，旋转单元51和58使用式1或式2。

在旋转单元51中使用的第1计算座标(xxc1、yyc1)存放于第3寄存器R3。其后，选择部56选择第3寄存器R31，将第1计算座标(xxc1、yyc1)发送至旋转单元51。旋转单元51将第1计算座标(xxc1、yyc1)分别代入式1或式2的xxc_i-1及yyc_i-1，重复运算。从而，得到第1计算座标(xxc2，yyc2)。第1计算座标(xxc2、yyc2)代替(xxc1、yyc1)，被重写到第3寄存器R31。也就是，旋转单元51在最初的第1次的运算中，演算式13和式14，其后，重复式1或式2的运算。

式1和式2中包含的乘项(yyc_i-1·tanΦ0)和(xxc_i-1·tanΦ0)根据三角函数值tanΦi＝±2^-i，仅移动以数字值(二进制值)表示的yyc_i-1及xxc_i-1的数位即可。在本实施例中，旋转单元51使yyc_i-1及xxc_i-1右移i位。所谓右移，是使某个数值向更小值的方向移动数位。也就是，对于式1或式2，旋转单元51实际上无需演算乘法。其后，旋转单元51将xxc_i-1或yyc_i-1与乘项的结果值相加或相减。因此，旋转单元51可不作乘法地演算式1或式2。由于i的值每次运算增加1，乘项的移位量每次运算增大。于是，计算座标(xxc_i，yyc_i)按每次运算而收敛。该运算中使用的暂设角度Φi按每次运算累计在旋转角度寄存器R51上。

另外，经旋转单元58运算后的第2计算座标(xxc1、yyc1)存放到第4寄存器R31。其后，选择部57选择第4寄存器R41，将xxc1和yyc1送入旋转单元58。旋转单元58将xxc1和yyc1代入至式1或式2的xxc_i-1和yyc_i-1，重复运算。从而，可得到xxc2和yyc2。也就是，旋转单元58在最初的第1次运算中演算式15和式16，其后，与旋转单元51同样地重复式1或式2的运算。

旋转单元58与旋转单元51一样，也无需对式1和式2演算乘法。其后，旋转单元58将xxc_i-1或yyc_i-1与乘项的结果值相加或相减。因此，旋转单元58也可不作乘法地演算式1或式2。

在式9和式10中，将x(i)/cosΦ置换成xxc(i)，将y(i)/cosΦ置换成yyc(i)，其意图是将cosΦ的乘法从运算中除去。由于这样的置换，旋转单元实质上无需演算乘法。还有，作为计算座标得到的结果(xxc1，yyc2)是在座标上除以cosΦ后的值。这终久只是运算过程的计算结果，只要最终得到角度Φ/2，则无妨碍。

为了缩短运算时间，就各i同时执行上述旋转单元51和58的运算动作。再者，式13～式16内的+、-符号会有根据旋转单元的旋转方向而取相反符号的情况。

对应于式11和式12中的各i的离散数值被预先设定，存放于存储器52和59中。即，存储器52和59将暂设角度Φi、对应于暂设角度Φi的各三角函数值tanΦi按各i加以存储。

旋转单元51和58分别获得存于存储器52和59的式11中的数值。旋转单元51和58将式1或式2中的第1和第2计算座标(xxc_i，yyc_i)关于各i以i＝0、1、2…的顺序进行计算。

式1在暂设角度为正时成立，式2在暂设角度为负时成立。式1和式2的Φi用绝对值表示。即，式2是在式1的Φi上代入-Φi得到的式。暂设角度是正(+Φi)时，旋转单元51和58沿图4所示的逆时针方向CCW使计算座标移动。暂设角度为负(-Φi)时，旋转单元51和58沿图4所示的顺时针方向CW使计算座标移动。旋转单元51和58根据来自旋转方向判断部53的信号来选择式1和式2中的一个。

为了使第1和第2计算座标相互接近，旋转单元51和58按i＝0、1、2…n的顺序交互或互补地演算式1和式2。也就是，在旋转单元51演算式1时旋转单元58演算式2，在旋转单元51演算式2时旋转单元58演算式1。第1计算座标的移动方向和第2计算座标的移动方向中，若一方确定，则另一方也确定，因此用一个旋转方向判别部53即可。

暂设角度Φi如式12所示，随着i增大而变小。因而，如图4所示，可以使第1计算座标和第2计算座标向第1实际座标(x1、y1)与第2实际座标(x2、y2)之间的中间座标收敛。由于旋转单元51和58以使第1和第2计算座标相互接近的方式进行运算，作为收敛的结果，暂设角度Φi变成实际的旋转角度的二分之一(Φ/2)。暂设角度±Φi的累计，通过将存放于寄存器R51的旋转单元51的运算结果累计在加法器80的每次旋转单元51的运算上来执行。再者，暂设角度±Φi的累计结果同样也可通过将旋转单元58的运算结果累计来得到。

参照图4，概念性地说明运算部50的动作。旋转单元51首先使第1实际座标(x1、y1)朝CCW方向转过Φ0。实际上，旋转单元51演算式13和式14，算出第1计算座标。CCW是以第1实际座标(x1、y1)作为基准，朝向第2个第2实际座标(x2、y2)的方向。

与此同时，旋转单元58使第2实际座标(x2、y2)朝CW方向转过Φ0。实际上，旋转单元58演算式15和式16，算出第2计算座标(xxc1、yyc1)。CW是将第2个第2实际座标(x2、y2)作为基准，朝向第1实际座标(x1、y1)的方向。作为结果，旋转单元51和58的运算使得旋转向着使第1实际座标和第2实际座标相互接近的方向(相互面对的方向)进行。

接着，旋转单元51使第1计算座标(xxc1、yyc1)朝CW方向转过Φ1。实际上，旋转单元51演算式2(i＝1)来再计算第1计算座标。

与此同时，旋转单元58使第2计算座标(xxc2、ycc2)朝CCW方向转过Φ1。实际上，旋转单元58演算式1来再计算第2计算座标。

接着，旋转单元51使第1计算座标(xxc1，yyc1)朝CW方向转过Φ1。实际上，旋转单元51演算式2(i＝1)来再计算第1计算座标。与此同时，旋转单元58使第2计算座标(xxc2，yyc2)朝CCW方向转过Φ1。实际上，旋转单元58演算式1来再计算第2计算座标。

继之，旋转单元51使第1计算座标朝CCW方向转过Φ2，实际上，旋转单元51演算式1(i＝2)来再计算第1计算座标。与此同时，旋转单元58使第2计算座标朝CW方向转过Φ2。实际上，旋转单元58演算式2来再计算第2计算座标。

如式12所示，暂设角度Φi的值随着i增大而变小。另外，旋转单元51和58在各自的运算中使得旋转向着使第1实际座标和第2实际座标相互接近的方向(相互面对的方向)进行。因此，旋转单元51和58每次重复运算，第1计算座标和第2计算座标都向中间座标(sin(θ+Φ/2)，cos(θ+Φ/2))收敛。第1计算座标和第2计算座标向中间座标收敛后，暂设角度Φi大致等于Φ/2。

接着，说明旋转方向判别部53选择方向CW或CCW(式1或式2)的方法。

图5是表示将座标分割为8个区域的图。图6是用以判别某个座标(x、y)是否属于图5所示的区域I～VIII中的任何一个的流程图。判别部54和55判别第1实际座标、第2实际座标、第1计算座标和第2计算座标是否属于图5的区域I～VIII中的任何一个。判别第1实际座标时，将(x1、y1)代入图6的(x、y)即可。判别第2实际座标时，将(x2、y2)代入图6的(x、y)即可。判别第1或第2计算座标时，将(xxci、yyci)代入图6的(x、y)即可。

可知，在步骤S11和步骤S12中，若x＞0且y＞0，则座标(x、y)属于图5的区域I或II。还有，在步骤S13中，若x≥y，则座标(x、y)属于区域I，若x＜y，则座标(x、y)属于区域II。

可知，在步骤S11和步骤S12中，若x＞0且y≤0，则座标(x、y)属于图5的区域VII或VIII。还有，在步骤S14中，若|x|≥|y|，则座标(x、y)属于区域VIII，若|x|＜|y|，则座标(x、y)属于区域VII。

可知，在步骤S11和步骤S15中，若x≤0且y＞0，则座标(x、y)属于图5的区域III或IV。还有，在步骤S16中，若|x|≥|y|，则座标(x、y)属于区域IV，若|x|＜|y|，则座标(x、y)属于区域III。

可知，在步骤S11和步骤S15中，若x≤0且y≤0，则座标(x、y)属于图5的区域V或VI。还有，在步骤S17中，若|x|≥|y|，则座标(x、y)属于区域V，若|x|＜|y|，则座标(x、y)属于VI。

区域判别部54和55中，可用ASIC(Application Specific IntegratedCircuit：专用集成电路)那样的定制LSI来实现图6的流程。

图7(A)和图7(B)所示的表中，根据第1实际座标和第2实际座标或第1计算座标和第2计算座标所属的区域，规定第1和第2计算座标的旋转方向(CCW或CW)。旋转方向判别部53按照图7(A)和图7(B)的表，选择旋转单元51和58使用的算式(式1或式2)。

图7(A)和图7(B)的“目标”表示构成目标的座标所属的区域。第1实际座标和第2实际座标中的一方将另一方作为目标。第1计算座标和第2计算座标中的一方将另一方作为目标。图7(A)和图7(B)中的“前次”表示用前次的运算得到的计算座标(xxc_i-1，yyc_i-1)所属的区域。或者，在最初的运算中，示出第1实际座标和第2实际座标。例如，若第1实际座标属于区域I、第2实际座标属于区域II，则对于第1实际座标，区域II成为目标。因而，旋转方向判别部53选择“目标”区域为II且“前次”区域为I的旋转方向CCW。也就是，旋转方向判别部53发送信号使旋转单元51选择式1。此时，旋转单元58选择式2。

同样，若i＝1时的第1计算座标属于区域II、第2计算座标属于区域I，则对于第1计算座标，目标为区域I。因而，旋转方向判别部53选择“目标”区域为I且“前次”区域为II的旋转方向CW。也就是，旋转方向判别部53发送信号使旋转单元51选择式2。此时，旋转单元58选择式1。

若第1实际座标和第2实际座标属于同一区域，则比较x1和x2或者y1和y2。例如，若第1实际座标和第2实际座标同属于区域I，则比较y1和y2。在y1＜y2时，使第1实际座标朝CCW方向旋转，使第2实际座标朝CW方向旋转。

若第1计算座标和第2计算座标属于同一区域，则比较第1计算座标的xxci和第2计算座标xxci。或者，比较第1计算座标的yyci和第2计算座标的yyci。例如，在i＝1时，若第1计算座标和第2计算座标同属于区域I，则比较第1计算座标xxc1和第2计算座标xxc1。若第1计算座标的xxc1比第2计算座标的xxc1大，则使第1计算座标朝CCW方向旋转，使第2计算座标朝CW方向旋转。

如此，即使第1实际座标和第2实际座标属于同一区域或者第1计算座标和第2计算座标属于同一区域，旋转方向判别部53也可正确地选择式1或式2。

旋转方向判别部53可用ASIC那样的定制LSI来实现图7(A)和图7(B)的选择条件。

在本实施例中，速度检测装置100算出转子的角速度。但是，速度检测装置100也适用于进行往复运动的运动体。这时，速度检测装置100可将FPGA50得到的角速度变换成运动体的速度。

依据本实施例，速度检测装置100不使用让速度检测精度恶化的除法或乘法而使用sin信号和cos信号的加法和减法来检测旋转角度，因此具有高精度。式1和式2中，存在将tanΦi相乘的项。但是，由于tanΦi＝2^-i，用tanΦi相乘与将xxc_i-1或yyc_i-1的数字值右移i位等效。因而，旋转单元51和58实际上不含乘法器，仅包含将数字值向下移位的电路。

通常用逻辑电路来实现运算器，与使用加法器和减法器相比，使用除法器和乘法器时电路规模会很大。在本实施例中，FPGA50包含2个旋转单元，但由于这些旋转单元中省去了除法器和乘法器，整体的电路规模变得非常小。另外，由于无除法器和乘法器，本实施例的FPGA50容易用定制逻辑LSI来实现。本实施例的旋转单元51、58重复运算上述那样的简式。定制的逻辑LSI适于高速进行这种简单的运算。因而，本实施例的FPGA50可用定制的逻辑LSI来提高运算速度。即，本实施例的FPGA50特别有利于用逻辑LSI实现。

本实施例中，旋转角度寄存器R51也可保存旋转单元51每次运算的暂设角度2倍的值。这时，可以用加法器80累计，将保存在旋转角度寄存器R51上的暂设角度的值直接作为转子的旋转角度。

Claims (7)

1.一种将表示进行旋转运动或往复运动的运动体位置的座标周期地采样，并基于所述座标检测出所述运动体的角速度的速度检测装置，其特征在于：

设有：存储预设的多个暂设角度和与该多个暂设角度中各角度对应的三角函数值的存储器；

存放通过第1采样得到的运动体的第1实际座标(x1、y1)的第1寄存器；

存放通过接着所述第1采样的第2采样得到的运动体的第2实际座标(x2、y2)的第2寄存器；

算出第1计算座标的第1运算部，该第1计算座标通过将所述x1和按照所述三角函数值移动y1的数位后的结果值相加或相减来表达；

算出第2计算座标的第2运算部，该第2计算座标通过将所述y2和按照所述三角函数值移动x2的数位后的结果值相加或相减来表达；以及

累计由所述第1或所述第2运算部的运算中使用的所述暂设角度的第3运算部，

所述第1和所述第2运算部重复运算，一边减小所述暂设角度，一边使所述第1和所述第2计算座标收敛，以使所述第1和所述第2实际座标相互接近，

所述第3运算部累计经所述第1或所述第2运算部的运算而收敛的所述暂设角度，并基于所述第1和所述第2计算座标收敛后的该暂设角度，生成所述运动体的角速度。

2.如权利要求1所述的速度检测装置，其特征在于：

所述存储器存储Φ_i(Φ₀＞Φ₁＞Φ₂＞…＞Φ_i＞…Φ_n)作为所述暂设角度，其中，n为自然数，并存储tanΦ_i作为所述三角函数值，

设所述第1或所述第2计算座标为(xxc_i，yyc_i)，则有：

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}-{\mathrm{yyc}}_{i-1}*\tan {\mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}+{\mathrm{xxc}}_{i-1}*\tan {\mathrm{\Φ}}_i\end{array}\right\}$ 式1

$\left.\begin{array}{c}{\mathrm{xxc}}_i={\mathrm{xxc}}_{i-1}+{\mathrm{yyc}}_{i-1}*\tan {\mathrm{\Φ}}_i\\ {\mathrm{yyc}}_i={\mathrm{yyc}}_{i-1}-{\mathrm{xxc}}_{i-1}*\tan {\mathrm{\Φ}}_i\end{array}\right\}$ 式2

式中，Φ_i＝tan^-12^-i，其中，i＝0、1、2…n，i＝0时所述第1运算部中xxc_i-1＝x1、yyc_i-1＝y1，所述第2运算部中xxc_i-1＝x2、yyc_i-1＝y2，

所述第1和所述第2运算部按i＝0、1、2…n的顺序演算式1或式2，

所述第3运算部按各个i累计所述暂设角度。

3.如权利要求2所述的速度检测装置，其特征在于：

所述第1和所述第2运算部按i＝0、1、2…n的顺序交互或互补地演算式1和式2，并且，在所述第1运算部演算式1时所述第2运算部演算式2，在所述第1运算部演算式2时所述第2运算部演算式1。

4.如权利要求1所述的速度检测装置，其特征在于还设有：

保存所述第1计算座标的第3寄存器；

保存所述第2计算座标的第4寄存器；以及

保存由所述第3运算部累计的所述暂设角度的第5寄存器。

5.如从权利要求1至权利要求4中任一项所述的速度检测装置，其特征在于：

通过将所述采样周期设为单位时间，将由所述第3运算部累计的所述暂设角度的2倍作为所述运动体的角速度。

6.如从权利要求1至权利要求4中任一项所述的速度检测装置，其特征在于：

经所述第3运算部累计后的所述暂设角度是所述运动体的旋转角度的二分之一。

7.如权利要求1所述的速度检测装置，其特征在于：

所述速度检测装置设在旋转编码器或线性标尺上。

Images