CN1145014C - 朝向角检测器 - Google Patents

Abstract

本发明的朝向角检测器用陀螺仪(301-303)检测X、Y和Z角速度，对它们进行时间积分(60)后产生朝向的俯仰、侧滚和偏航角(α，β，γ)。用了两个加速度计(403，404)来得到校正俯仰和侧滚角的推测俯仰和侧滚角，用了两个地磁仪(401，402)来得到校正偏航角的推测偏航角。在推测俯仰、侧滚和偏航角判为精确(50)时，积分俯仰、侧滚和偏航角用推测俯仰、侧滚和偏航角校正(60)。

Description

朝向角检测器

                       技术领域

本发明与采用陀螺仪的朝向角检测器有关，具体地说，涉及用在这样的朝向角检测器中的陀螺仪来补偿或校正所测量的朝向角的技术。

                       技术背景

在现有技术中，朝向角检测器在使用头戴式显示器(HMD)的虚拟环境、扩大的现实环境和遥控系统中用来进行头部跟踪。它也可以用作计算机的输入装置。

已知三维(3-D)朝向角检测器包括三个陀螺仪(称为“速率陀螺”)，在一个运动物体上沿三个正交轴配置，以得到三个正交的角速度。这些正交角速度经积分后，产生三个表示朝向角的角因子。朝向角通常用X-Y-Z的Euler角θ＝(α，β，γ)表示。在Yamashitaet等人的“利用三维姿态角度传感器对人头部运动的测量”(“Measurement of Human Head Motion using A Three-DimensionalPosture Angle Sensor”，National Convention Record of I.E.E.Japan，Vol.3(1997)，p.p.304-305)中揭示了这样一种已知的朝向角检测器。

为了补偿在对速率陀螺测量的角速度积分中的误差，已知可以利用三个也沿同样的三个正交轴配置的加速度计来测量在这三个正交方向上的加速度，在上述论文中也揭示了这种情况。另一组三个表示朝向角的角因子根据所测得的加速度计算，用来估计和校正用速率陀螺测量的朝向角。

通常用光纤陀螺(FOG)和半导体加速度计来作速率陀螺和加速度计。FOG在精度上是极好的，但是非常昂贵。一种称为压电振动陀螺仪的Coriolis振动陀螺仪由于成本低很经济，但是精度不高。

从美国专利4,254,465中可以看到一种如权利要求1的导言所述的朝向角检测器。用三个速率陀螺来提供绕三个相互正交的轴的角加速度的值。根据这些角加速度值，确定航向、侧滚和俯仰角的估计量。还用三个线性加速度计确定沿三个轴的线加速度。根据这些值可以得出沿着三个轴的速度分量。此外，还用一个磁通量表来确定磁航向角。为了使航向估计量收敛到一个等于磁航向的值，将磁航向值与根据角加速度确定的航向角的估计量相比较，得出一个校正反馈信号。用直线加速度来校正由辅助传感器确定的向心和Coriolis分量。

欧洲专利0,292,339揭示了一种采用陀螺仪、加速度计和地磁仪的系统。这些传感器各提供各自的相对一个飞机固定的三个正交轴的相对测量结果。这些陀螺仪和地磁仪的输出变换为地理坐标系的等效值。重力加速度通过从加速度计的输出中减去根据风速计输出确定的加速度的值确定。重力加速度的值用来确定加速度中由于飞机的姿态引起的误差。根据这些值，确定一个前向区域，用来提供精确的俯仰、侧滚和偏航角测量。

本发明的目的是提供一种精度高而结构简单的朝向角检测器，这种朝向角检测器当然可以用FOG作为速率陀螺，但是也可以用Coriolis振动陀螺仪，这样可以使检测器的成本低，但与用FOG的相比精度并没有降低多少。

                       发明概述

按照本发明，所得到的朝向角检测器包括：多个在检测器上分别与定义检测器座标系的多个正交轴(Xs-Zs)平行配置的陀螺仪，用来测量绕各自的轴(Xs-Zs)的角速度(Jx，Jy，Jz)；一个与这些陀螺仪连接的转动角计算器，用来根据角速度(Jx，Jy，Jz)计算转动角(ΔX，ΔY，ΔZ)；至少一个与至少正交轴(Xs-Zs)之一平行配置的加速度计，用来测量加速度(Ax，Ay，Az)；一个与这至少一个加速度计连接的静态角计算器，用来根据加速度(Ax，AY)计算静态角(R，P)；以及一个与转动角计算器和静态角计算器连接的朝向角计算器，用来对转动角(ΔX，ΔY，ΔZ)积分以得到一个积分角和根据积分角和静态角(R，P)计算一个朝向角(α，β，γ)，朝向角(α，β，γ)是检测器座标系(Xs-Ys-Zs)与检测器所在空间内的基准座标系(X-Y-Z)之间的角度差，其中有两个地磁仪分别与多个正交轴中的两个(Xs，Ys)平行配置，用来测量在多个轴中的两个(Xs，Ys)上的地磁分量(Mx，My)，而静态角计算器还与两个地磁仪连接，根据地磁分量(Mx，My)计算出方位角，再根据方位角产生一个偏离基准座标系(X-Y-Z)的方位偏离角Ф作为静态角(R，P)的一个辅助因子。

                       附图简要说明

图1为例示在按照本发明的一个实施例实现的朝向角检测器中速率陀螺、加速度计和地磁仪的配置情况的示意图；

图2为例示空间3-D基准座标系与检测器本身的3-D坐标在偏离基准座标系后的关系的示意图；

图3为按照本发明的一个实施例实现的朝向角检测器的原理方框图；

图4为图3的朝向角检测器中的静态角计算器的原理方框图；

图5为例示朝向角检测器在HMD中使用情况的示意图；

图6为可以校正图3的朝向角检测器中的各个高通滤波器的高通滤波器校正器的原理方框图；

图7示出了图6中各点的信号波形；

图8为用来代替图3的朝向角检测器中的各个高通滤波器的数字滤波装置的原理方框图；

图9为在图8的高通滤波器装置中的IIR数字高通滤波器的原理方框图；

图10示出了图8中各点的信号波形；

图11A-11C示出了图8的高通滤波器装置在不同情况下的输入和输出信号；

图12为例示在按照本发明的另一实施例实现的朝向角检测器中速率陀螺、加速度计和地磁仪的配置情况的示意图；

图13A和13B可以用于图1-12所示的朝向角检测器的速率陀螺组合的展开图和弯折图；

图14为例示在按照本发明的另一实施例实现的朝向角检测器中速率陀螺、加速度计和地磁仪的配置情况的示意图；以及

图15为图14的朝向角检测器的原理方框图。

                       本发明最佳实施例

下面，将参照图1-3说明按照其中所示的本发明的一个实施例实现的朝向角检测器。这个实施例的朝向角检测器用来检测一个物体在一个三维(3-D)空间内的朝向。

这个在这些图中所示出的朝向角检测器包括在检测器上分别与定义检测器座标系的三个正交轴Xs，Ys和Zs平行配置的第一至第三陀螺仪301-303，用来测量绕各自的轴Xs、Ys和Zs转动的第一至第三角速度Jx、Jy和Jz。

转动角计算器310与第一至第三陀螺仪301-303连接，用来根据第一至第三角速度Jx、Jy和Jz计算转动角(ΔX，ΔY，ΔZ)。第一和第二加速度计403和404与两个正交轴Xs和Ys平行配置，用来测量第一和第二加速度Ax和Ay。这两个加速度计用来测量重力加速度。

静态角计算器405与第一和第二加速度计403和404连接，用来根据加速度Ax和Ay计算静态角(R，P)。

朝向角计算器60与转动角计算器310和静态角计算器405连接，用来对转动角(ΔX，ΔY，ΔZ)积分以得到一个积分角(∑ΔX，∑ΔY，∑ΔZ)和根据积分角和静态角(R，P)计算朝向角(α，β，γ)。朝向角(α，β，γ)是检测器座标系(Xs-Ys-Zs)与检测器所在空间的基准座标系之间的角度差。

此外，第一和第二地磁仪401和402与这些正交轴中的两个Xs和Ys平行配置，用来测量地磁分量Mx和My。静态角计算器405与第一和第二地磁仪401和402连接，用来根据地磁分量Mx和My计算出方位角，再根据方位角产生偏离基准座标系(X-Y-Z)的方位偏离角Ф作为静态角(R，P)的一个辅助因子。

朝向角检测器还包括一个朝向角存储器70，与朝向角计算器60连接，用来存储朝向角(α，β，γ)。朝向角用Z-Y-X的Euler角表示，它包括绕Z轴转动角度的偏航角α、绕Y轴转动角度的俯仰角β和绕X轴转动角度的侧滚角γ这三个分量。

转动角有三个因子：绕X轴的X转动角Ax，绕Y轴的Y转动角AY，以及绕Z轴的Z转动角Az。

同时，静态角计算器405根据第一和第二磁检测信号Mx和My以及第一和第二加速度Ax和Ay计算出一个推测俯仰角P、一个推测侧滚角R和一个作为方位偏离角的推测偏航角Ф。推测俯仰角P、推测侧滚角R和推测偏航角Ф是静态角的因子。

朝向角检测器还包括一个静态角校正判定器50，用来判定静态角(P，R，Ф)的精确性，在判定静态角精确时产生一个校正信号。朝向角计算器60对积分角(∑ΔX，∑ΔY，∑ΔZ)进行校正，得出朝向角(α，β，γ)。

如图5所示，朝向角检测器20与HMD 100一起戴在用户的头上。检测器20和HMD 100通过电缆102连接到一个视频图像生成器101上。图像信号从视频图像生成器101传送给HMD 100。于是用户可以欣赏图像。在用户转头时，他的头的朝向角由朝向角检测器20检测后传送给视频图像生成器101。视频图像生成器101用朝向角来改变或调制图像信号。因此，用户可以通过转头来改变他看到的图像。

第一至第三陀螺仪301-303最好是具有压电振动器的Coriolis振动陀螺仪。

第一至第三高通滤波(H型滤波器)304-306分别与第一至第三Coriolis振动陀螺仪301-303连接，用来消除包含在第一至第三Coriolis振动陀螺仪的第一至第三陀螺仪输出信号Jx，Jy和Jz内的偏移电压，产生第一至第三滤波器输出。

第一至第三高通滤波304-306各设计成截止频率为0.1Hz甚至更低。具有低截止频率的高通滤波器从开始到稳定工作很慢。因此，第一至第三高通滤波器304-306最好各有一个可变的截止频率。在这种情况下，截止频率调整成在刚接通检测器的电源开关时比较高，从而可以使得滤波器迅速地进入稳定工作状态。在工作稳定后，高通滤波器可以调节到0.1Hz的低截止频率。

第一至第三高通滤波器304 306通常是模拟型的高通滤波器。在这种情况下，朝向角检测器还包括分别与第一至第三高通滤波器304-306连接的第一至第三模数(A/D)变换器307-309。第一至第三模数变换器307-309按采样间隔St分别对第一至第三滤波器输出进行采样，产生第一至第三时间序列数字信号。转动角计算器310接收到输入的第一至第三时间序列数字信号，产生表示转动角的三个时间序列转动角信号(ΔX，ΔY，ΔZ)。

第一至第三时间序列转动角信号表示时间序列X转动角ΔX(t)，时间串行的Y转动角ΔY(t)，以及时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而n-(n-1)＝St，转动角计算器(310)按照下式计算在t＝n时的X转动角ΔX(n)，Y转动角ΔY(n)和Z转动角ΔZ(n)：

$\left[\begin{array}{c}\mathrm{\ΔX}(Xn)\\ \mathrm{\ΔY}\left(n\right)\\ \mathrm{\ΔZ}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}0& \tan \mathrm{\β}(n-1)\sin \mathrm{\γ}(n-1)& \tan \mathrm{\β}(n-1)\cos \mathrm{\γ}(n-1)\\ 1& \cos \mathrm{\γ}(n-1)& -\sin \mathrm{\γ}(n-1)\\ 0& -\sin \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)& \cos \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)\end{array}\right]\×\left[\begin{array}{c}\mathrm{Jx}\left(n\right)\\ \mathrm{Jy}\left(n\right)\\ \mathrm{Jz}\left(n\right)\end{array}\right]$

其中β(n-1)和γ(n-1)是从朝向角存储器70读出的在t＝n-1时的俯仰角β和侧滚角γ，而Jx(n)，Jy(n)和Jz(n)是第一至第三输入时间序列数字信号在t＝n时的第一至第三数字信号。

利用高通滤波器可以消除包含在陀螺仪输出信号内的偏移电压。然而，由于滤波器内的电容器的作用使滤波器输出有失真(见图7中的(c))。为了消除这种失真，朝向角检测器最好包括三个高通滤波器校正电路311，分别连接在第一至第三模数变换器307-309与转动角计算器310之间，如图3中的虚线所示。这些高通滤波器校正电路311对第一至第三高通滤波器的失真进行补偿，产生第一至第三经校信号。第一至第三经校信号传送给转动角计算器310作为第一至第三输入时间序列数字信号。

高通滤波器校正电路311各执行下式给定的计算：

J(n)＝F(n)+(Δt/RC)×∑F(n)

其中J(n)为在t＝n时输入时间序列数字信号中的相应的一个数字信号的经校信号，F(n)为在t＝n时第一至第三时间序列数字信号中的相应一个数字信号的数字值，而Δt/RC为一个常量。

参见图6，第一至第三高通滤波器校正电路311各包括一个有着一个第一加法器31101和一个寄存器31102的积分器，用来对相应的时间序列数字信号的数字值进行积分，将积分得的值保存在寄存器31102内。常量产生器31103用来产生常量。乘法器31104与寄存器31102和常量产生器31103连接，用来将积分值乘以常量，产生一个乘后值。第二加法器31105将乘后值与串行输入的数字值相加，产生经校信号。

参看图7，图中示出了如上所述的Coriolis振动陀螺仪的激励电压的输入波形(a)、输出波形(b)、高通滤波器的输出波形(c)和高通滤波器校正器311的输出波形(d)。从波形(c)和(d)可以看出，在(d)中已消除了在(c)中显示的失真。

再来看图6，高通滤波器校正电路311还包括一个偏移误差校正电路31106-3108，用来补偿在积分器31101-31102和第二加法31105的计算中所造成的偏移误差。偏移误差校正电路包括一个输出判定器31106，用来观察经校信号保持不变的时间，和判定偏移误差的程度。误差校正器31107与输出判定器31106连接，根据误差程度产生误差校正信号。误差校正信号提供给寄存器31102，由它校正积分值。常量校正电路31108还与误差校正器31107连接，根据误差校正信号对常量产生器31103产生的常量进行校正。

回到图7，其中示出了具有偏移误差校正电路31106-3108的高通滤波器校正器311的输出波形(e)。将波形(d)与波形(e)相比较可以看出，高通滤波校正器311的输出波形得到了进一步的改善。

第一至第三高通滤波器可以是分别产生第一至第三时间序列数字滤波输出的第一至第三数字高通滤波器。

参见图8，第一至第三数字高通滤波器(图8)各包括一个与第一至第三陀螺仪301-303中相应一个连接的模数变换器313，用来按采样间隔St对第一至第三陀螺仪输出信号中相应一个进行采样，产生一个时间序列数字信号。数字滤波器315与模数变换器313连接，用来对时间序列数字信号进行滤波，产生第一至第三时间序列数字滤波输出中相应一个输出。转动角计算器310接收第一至第三时间序列数字滤波输出，作为第一至第三输入时间序列数字信号。

第一至第三时间序列转动角信号表示时间序列X转动角ΔX(t)，时间序列Y转动角ΔY(t)和时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而St＝n-(n-1)。

数字滤波315的一个例子是无限冲激响应(IIR)类型的数字滤波，例如如图9所示。IIR数字滤波315具有延迟缓存器值P1和P2。

数字高通滤波器装置(图8)还包括一个与数字滤波器315的输出端连接的正/负判定器316，用来根据时间序列数字滤波输出判定时间序列数字信号在正、负之间改变的时间，产生一个改变信号。延迟缓存器值复位电路317与数字滤波315和正/负判定器316连接。延迟缓存器值复位电路317响应改变信号对数字滤波315的缓存器值P1和P2复位。

图10示出了各个波形a-d。波形a-c与图7中的(a)-(c)所示的类似，而波形d是以上结合图8和9说明的数字高通滤波器装置的输出波形。如波形d所示，由虚线示出的由于用模拟滤波器而出现的失真可以通过利用数字高通滤波器装置来消除。图11A示出了以上说明的数字高通滤波器装置的输入波形a1和输出波形b1。由图可见，漂移没有得到改善。

数字高通滤波器装置(图8)还包括一个用来产生偏移值的偏移值产生器319和一个与偏移值产生器319连接的乘法器320。乘法器320配置成使模数变换器313与数字滤波315连接。乘法器320用来将时间序列数字信号乘以偏移值。偏移值校正电路318与偏移值产生器319和正/负判定器316连接，按照改变信号校正偏移值。

在执行校正而将偏移值设置为时间序列数字滤波就在校正前的值的情况下，数字滤波器的输入和输出波形分别如图11B中的a2和a1所示。

最好按照下式执行偏移值校正：

OS(n)＝OS(n-1)-m×[OS(n-1)-D(n-1)]

其中OS(n)和OS(n-1)分别为在校正后和在校正前的偏移值，D(n-1)为来自模数变换器的数字信号，而0＜m≤0.5。对偏移值校正改善了输出的漂移，如图11C中的输入和输出波形a3和b3所示。

参见图4，静态角计算器405包括分别与第一和第二加速度计403和404连接的第一和第二模数变换器40501和40502。第一和第二模数变换器40501和40502按采样间隔St对第一和第二加速度检测信号Ax和Ay进行采样，产生第一和第二时间序列数字加速度信号。推测俯仰角计算器40504与第一模数变换器40501连接，根据第一时间序列数字加速度信号计算出推测俯仰角P。推测侧滚角计算器40503与第二模数变换器40502和推测俯仰角计算器40504连接。推测侧滚角计算器40503根据第二时间序列加速度信号和推测俯仰角P计算推测侧滚角R。

在第一和第二时间序列数字加速度信号表示为Ax(t)和Ay(t)时，其中t＝n，n-1，…，1，而St＝n-(n-1)，推测俯仰角计算器40504和推测侧滚角计算器40503分别按照下式计算推测俯仰角P和推测侧滚角R；

P(n)＝sin^-1Ax(n)而

R(n)＝sin^-1[Ay(n)/cosP(n)]

其中P(n)，R(n)，Ax(n)和Ay(n)分别表示在t＝n时的P，R，Ax(t)和Ay(t)。

静态角计算器405还包括分别与第一和第二地磁仪401和402连接的第三和第四模数变换器40505和40506。第三和第四模数变换器40505和40506按采样间隔St分别对第一和第二磁检测信号Mx和My进行采样，产生第一和第二时间序列数字磁检测信号。座标系变换计算器40507与第三和第四模数变换器40505和40506、推测侧滚角计算器40503和推测俯仰角计算器40504连接。座标系变换计算器40507将第一和第二时间序列数字磁检测信号变换成表示在基准X-Y-Z座标系的X-Y平面内的两个相应的地磁分量的第一和第二时间序列数字地磁分量信号Hx和Hy。方位角计算器40508与座标系变换计算器40507连接，根据地磁分量Hx和Hy计算方位角 产生一个作为方位偏离角的推测偏航角Ф。此外，方位角存储器40512与方位角计算器40508连接，用来存储方位角

静态角计算器405还包括一个与第三和第四模数变换器40505和40506连接的的Z方向磁分量产生器40509-40515。Z方向磁分量产生器40509-40515根据第一和第二时间序列磁检测信号Mx和My、先前测量的地磁Ht、由检测器座标系的Xs和Ys轴定义的Xs-Ys平面偏离X-Y平面的初始倾斜角θ0和Xs-Ys平面偏离X-Y平面的当前倾斜角θ2产生一个时间序列数字Z方向磁分量信号Mz。

座标系变换计算器40507按照下式根据第一和第二时间序列磁检测信号Mx和My、时间序列数字Z方向磁分量信号Mz和推测俯仰和侧滚角P和R计算地磁分量Hx和Hy

Hx(n)＝cos[P(n)×Mx(n)+sin[P(n)]×Mz(n)

Hy(n)＝sin[R(n)]×sin[P(n)]×Mx(n)

       +cos[R(n)]×cos[P(n)]×MZ(n)

其中Hx(n)，Hy(n)，Mx(n)，My(n)，Mz(n)，P(n)和R(n)分别表示在t＝n时的Hx，Hy，Mx，My，Mz，P和R。

方位角计算器40508按照下式根据Hx(n)和Hy(n)计算方位角在t＝n时的

方位角计算器(40508)还按照下式根据 和起始方位偏离角

计算推测偏航角Ф在t＝n时的Ф(n)：

Z方向磁分量产生器40509-40515包括一个地磁产生器(40509-40510)，用来产生地磁Ht。当前Z方向磁分量绝对值计算器40511与地磁产生器40509和40510、第三和第四模数变换器40505和40506连接。当前Z方向磁分量绝对值计算器40511按照下式根据地磁Ht、第一和第二时间序列磁检测信号Mx和My在t＝n时的当前值Mx(n)和My(n)计算Z方向磁分量绝对值|Mz(n)|：

|Mz(n)|＝[Ht²-{Mx(n)²+My(n)²}]^1/2

当前倾斜角计算器40513与朝向角存储器70和方位角存储器40512连接，计算当前倾斜角θ2。

符号判定器40514与当前倾斜角计算器40513连接，根据当前倾斜角θ2和初始倾斜角θ0产生当前Z方向磁分量Mz(n)的符号(+或-)。

当前Z方向磁分量计算器40515与当前Z方向磁分量绝对值计算器40511和符号判定器40514连接，产生Z方向磁分量Mz(n)。

地磁产生器(40509-40510)包括一个分别通过两个开关元件S1和S2可以有选择地与第三和第四模数变换器40505和40506连接的初始地磁存储器40509。初始地磁存储器40509预先存储了由第三和第四模数变换器40505和40506通过在初始时间有选择地接通的开关元件S1和S2提供的初始的X方向、Y方向和Z方向地磁分量Mx(0)、My(0)和Mz(0)。

处理器40510与初始地磁存储器40509连接，按照下式处理初始的X方向、Y方向和Z方向地磁分量Mx(0)、My(0)和Mz(0)，得出地磁Ht.：

|Ht|＝[Mx(0)²+My(0)²+Mz(0)²]^1/2

因此得到的地磁的绝对值|Ht|存储在初始地磁存储器40509内。

下面说明将初始X方向、Y方向和Z方向地磁分量Mx(0)、My(0)和Mz(0)存入初始地磁存储器40509的情况。

在初始状态，朝向角检测器一方面定位成使第一和第二地磁仪401和402之一与基准座标系的Z轴平行。然后，接通两个开关元件S1和S2中的相应一个，将这个地磁仪(401或402)检测到的地磁分量存入初始地磁存储器40509作为初始Z方向地磁分量Mz(0)。此后，朝向角检测器另一方面定位成使第一和第二地磁仪401和402分别与基准座标系的X和Y轴平行。然后，接通两个开关元件S1和S2。结果，第一和第二地磁仪401和402检测到的地磁分量存入初始地磁存储器40509，作为初始X方向地磁分量Mx(0)和初始Y方向地磁分量My(0)。

当前倾斜角计算器40513按照下式根据从方位角存储器40512读出的方位角 在t＝n-1时的角值 和从朝向角存储器70读出的侧滚角β和俯仰角γ在t＝n-1的侧滚角值β(n-1)和俯仰角值γ(n-1)计算当前倾斜角θ2：

θ2＝sin^-1[aa/(aa²+cc²)^1/2]，

其中

1)，而cc＝cosγ(n-1)×cosβ(n-1)。

符号判定器40514一方面在当前倾斜角θ2大于初始倾斜角θ0而小于90度时将符号判为正(+)，另一方面在当前倾斜角θ2等于或小于初始倾斜角θ0而大于-90度时将符号判为负(-)。

再来看图3，朝向角检测器还包括一个与静态角计算器405和朝向角计算器60连接的静态角校正判定器50。静态角校正判定器50用来判定静态角(P，R，Ф)的精确性，产生一个校正信号。然后，朝向角计算器60响应校正信号计算出一些校正值。于是，朝向角计算器60用这些校正值对积分值组进行校正，产生一个修正值组。这个修正值组作为朝向角(α，β，γ)发送。

朝向角计算器60按照下式计算第一至第三积分值∑ΔX、∑ΔY和∑ΔZ：

∑ΔX(n)＝γ(n-1)+ΔX(n)；

∑ΔY(n)＝β(n-1)+ΔY(n)；和

∑ΔZ(n)＝α(n-1)+ΔZ(n)，

其中ΔX(n)，ΔY(n)和ΔZ(n)分别为在时间t＝n时的X转动角ΔX，Y转动角ΔY和Z转动角ΔZ，而γ(n-1)，β(n-1)和α(n-1)分别为在时间t＝n-1时的侧滚角γ，俯仰角β和侧滚角α。γ(n-1)，β(n-1)和α(n-1)从朝向角存储器70读出。

在朝向角计算器60接收到校正信号时，朝向角计算器60按照下式计算第一至第三角误差Ex、Ey和Ez：

Ex＝∑ΔX(n)-R(n)

Ey＝∑ΔY(n)-P(n)

Ez＝∑ΔZ(n)-Ф(n)

然后，朝向角计算器60根据第一至第三角误差Ex，Ey和Ez确定第一至第三校正值C1，C2和C3作为校正值，再按照下式计算第一至第三修正值作为侧滚角γ，俯仰角β和偏航角α：

γ＝∑ΔX(n)-C1

β＝∑ΔY(n)-C2

α＝∑ΔZ(n)-C3

就此而论，C1、C2和C3的值可以选择为分别小于第一至第三角误差Ex，Ey和Ez。或者，C1、C2和C3的值可以分别确定为k1×Ex、k2×Ey和k3×Ez，其中k1、k2和k3均为小于1的常数值。

朝向角检测器最好配置有一个校正系数产生器80，如图所示。校正系数产生器80与静态角计算器405和朝向角计算器60连接，用来产生一组第一至第三系数m1、m2和m3。第一至第三系数m1、m2和m3是根据从静态角计算器405接收的推测俯仰角P和推测侧滚角R的角值从一些不同的值中选出来的。

更可取的是，校正系数产生器80具有一个如下面的表1所示的表。这个表列有按推测俯仰角P和推测侧滚角R的一些角值预先确定的各组第一至第三系数m1，m2和m3。

通常，对于大的推测俯仰角P和推测侧滚角R的角值将第一至第三系数m1，m2和m3取得比较小。

表1

		R(度)
					0-60	60-70	70-90
		P(度)	0-60	m1：1m2：1m3：1				m1：0.7m2：1m3：0.7	m1：0.3m2：1m3：0.3
60    -70	m1：1m2：0.7m3：0.7				m1：0.7m2：0.7m3：0.7	m1：0.3m2：0.7m3：0.3

70    -89

m1：1m2：0.3m3：0.3

m1：0.7m2：0.3m3：0.3

m1：0.3m2：0.3m3：0.3

朝向角计算器60按照这组第一至第三系数m1、m2和m3通过将第一至第三校正值C1，C2和C3分别乘以第一至第三系数m1，m2和m3加以修正。

下面将对校正判定器50进行详细说明。校正判定器50配置有一个静态角存储器501，用来存储从静态角计算器405接收的推测俯仰角P和推测侧滚角R。校正判定器50将在时间t＝n接收的推测俯仰角P(n)和推测侧滚角R(n)与从静态角存储器501读出的在t＝n-1时的早先推测俯仰角P(n-1)和推测侧滚角R(n-1)相比较，在分别满足以下式(1)和(2)时判定推测俯仰角P(n)和推测侧滚角R(n)是精确的：

|R(n)-R(n-1)|-B≈0                         (1)

|P(n)-P(n-1)|-D≈0                         (2)

其中B和D选择为正值。

在一个例子中，B和D预定为校正判定器50预先具有的几乎等于零(0)的很小的值。

在另一个例子中，校正判定器50与转动角计算器310连接，从那里接收X转动角ΔX(n)和Y转动角ΔY(n)。B和D分别选择为X转动角ΔX(n)和Y转动角ΔY(n)。

此外，校正判定器50可以作成判定推测偏航角Ф的精确性。校正判定器50分别从静态角计算器405和转动角计算器310接收推测偏航角Ф(t)和Z转动角ΔZ(t)。推测偏航角Ф(t)存储在静态角存储器501内。校正角判定器50按照下式(3)处理在t＝n时的推测偏航角Ф(n)和Z转动角ΔZ(n)以及从静态角存储器501读出的在t＝n-1的偏航角Ф(n-1)：

|Ф(n)-Ф(n-1)|-ΔZ(n)≈0                   (3)

校正角判定器50在式(3)满足时判定推测偏航角Ф(n)是精确的。

在校正判定器50中，可以用对转动角的滑动平均来判定静态角。

校正角判定器50将从转动角计算器310接收的X转动角ΔX，Y转动角ΔY和Z转动角ΔZ以及从静态角计算器405接收的推测俯仰角P(n)，推测侧滚角R(n)和推测偏航角Ф(n)存储在静态角存储器501内。校正判定器50计算推测俯仰角P(t)的变化的滑动平均、推测侧滚角R(t)的变化的滑动平均、推测偏航角Ф(n)的变化的滑动平均、X转动角ΔX(t)的滑动平均、Y转动角ΔY(t)的滑动平均和Z转动角ΔZ(t)的滑动平均。然后，在分别满足以下式(4)，(5)和(6)时校正判定器50判定推测俯仰角P(n)，推测侧滚角R(n)和推测偏航角Ф(n)是精确的：

[R(n)-R(n-1)]/C-[ΔX(n)+ΔX(n-1)+…+ΔX(n-C+1)]/C≈0  (4)

[P(n)-P(n-1)]/C-[ΔY(n)+ΔY(n-1)+…+ΔY(n-C+1)]/C≈0  (5)

[Ф(n)-Ф(n-1)]/C-[ΔZ(n)+ΔZ(n-1)+…+ΔZ(n-C+1)]/C≈0(6)

由于判定了静态角的精确性，校正判定器50在至少推测俯仰角P(n)和推测侧滚角R(n)之一判定为精确时产生校正信号，而在推测俯仰角和推测侧滚角R(n)没有一个判定为精确时产生一个不校正信号。响应不校正信号，朝向角计算器60产生积分值组∑ΔX(n)，∑ΔY(n)和∑ΔZ(n)作为朝向角(α，β，γ)。

在执行对推测偏航角Ф(n)的精确性判定中，校正判定器50在推测偏航角Ф(n)判定为精确时无论推测俯仰角和推测侧滚角R(n)是否判定为精确也产生校正信号。在推测俯仰角、推测侧滚角R(n)和偏航角Ф(n)中没有一个判定为精确时，校正判定器50产生不校正信号。

在另一个设计中，校正判定器50在判定推测侧滚角R、推测俯仰角P和推测偏航角Ф中有几个精确后可以产生一个指出推测侧滚角R、推测俯仰角P和推测偏航角Ф中哪几个判定为精确的指示信号作为校正信号。根据指示信号，朝向角计算器60用推测俯仰角P、推测侧滚角R和推测偏航角Ф修正第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中与推测侧滚角R、推测俯仰角P和推测偏航角Ф中判定为精确的相应的那几个积分值，产生几个指定修正值。然后，朝向角计算器60发送这几个指定修正值，作为侧滚角γ、俯仰角β和偏航角α中分别与第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中指定修正的相应的那几个的规定值。朝向角计算器60还发送第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中指定修正的之外的其余几个，作为侧滚角γ、俯仰角β和偏航角α中所规定的之外的其余几个。

参见图12，按照本发明的另一个实施例实现的朝向角检测器还配置了一个与检测器座标系的Zs轴平行配置的第三加速度计406，用来产生一个第三加速度检测信号Az。在这个实施例中，静态角计算器405还包括一个附加的模数变换器4061，如图4中虚线所示。

这个附加模数变换器4061与第三加速度计406连接，按采样间隔St对第三加速度检测信号Az进行采样，产生一个第三时间序列数字加速度信号。第三时间序列加速度信号还在侧滚角计算器40504内用来计算推测侧滚角R。

第一，第二和第三时间序列数字加速度信号表示为Ax(t)、Ay(t)和Az(t)，其中t＝n，n-1，…，1，而St＝n-(n-1)，推测侧滚角计算器40503按照下式分别计算推测侧滚角R；

在Az＞0时，

R(n)＝sin^-1[Ay(n)/cosP(n)]

在Az＜0而Ax＞0时，

R(n)＝π-sin^-1[Ay(n)/cosP(n)]

而在Az＜0而Ax＜0时，

R(n)＝-π-sin^-1[Ay(n)/cosP(n)]，

其中P(n)，R(n)，Ax(n)，Ay(n)和Az(n)分别表示在t＝n时的P，R，Ax(t)，Ay(t)和Az(t)。

在本实施例中，静态角校正判定器50接收第一、第二和第三加速度检测信号(Ax，Ay，Az)，如图3和4中虚线所示。根据第一、第二和第三加速度检测信号(Ax，Ay，Az)，校正判定器50按照下式计算Xs轴方向、Ys轴方向和Zs轴方向的加速度的合成加速度矢量的绝对值：

|A|＝(Ax+Ay+Az)^1/2

校正判定器(50)在|A|≈1G时产生校正信号，其中G为重力加速度。

在另一方面，本发明提供了一种包括Coriolis振动陀螺仪的电路组件，能简化用Coriolis振动陀螺仪的朝向角检测器的安装。

参见图13A和13B，第一至第三陀螺仪陀螺的三个压电陶瓷振动器301-303分别固定地安装在一个挠性印制电路板201的三个段上。这个挠性印制电路板201弯折成如图11B所示的直角三角形筒，具有相互垂直的第一和第二侧板段以及一个第三侧板段，形成一个直角三角形筒。挠性印制电路板201在弯折部分有两个狭缝202和203，有利于悬挂操作。

在第一至第三压电陶瓷振动器301、302和303安装到软性印制电路201上后，将挠性印制电路板201弯折成三角形筒。于是，第一至第三陀螺仪301、302和303的第一至第三压电陶瓷振动器就自动地配置成与检测座标系的Xs轴、Ys轴和Zs轴平行。

挠性印制电路板201的直角三角形筒的一端边缘上有一些接线端204伸出。这些接线端204用来与外部电部件电连接。第三侧板段在与这端缘相反的方向上延伸，超过第一和第二侧板段，使得在Z轴方向的第三陀螺仪303可以可靠地安装在挠性印制电路板上。

可以不用单个挠性印制电路板201，而用三个印刷电路板分别作为第一至第三侧板部分。

在本发明的朝向角检测器所用的Coriolis振动陀螺仪中，可以使用任何类型的压电振动器，但是最好采用无磁性的压电材料制成，例如压电陶瓷，压电单晶材料和硅。

在朝向角检测器的某些应用中，只需要朝向角的两个因子(例如，俯仰角和偏航角)，而不用其他因子(侧滚角)。本发明的另一个实施例就是用于这样应用的二维(2-D)朝向角检测器。

参见图14和15，其中所示的2-D朝向角检测器与图1-11所示的3-D朝向角检测器类似，只是省略了三个陀螺仪中的一个和与之配合的部件和功能。类似的部件用相同的标示符号(带或不带上撇(′))标示。

2-D朝向角检测器只传送偏航角α和俯仰角β作为一个2-D朝向角。第一和第二陀螺仪302和303配置成与Ys和Zs轴平行，如图14所示。第一和第二陀螺仪302和303分别与图1中的测量第一和第二角速度Jy和Jz的第二和第三陀螺仪302和303相应。

转动角计算器310′根据第一和第二角速度Jy和Jz计算绕Y轴的Y转动角ΔY和绕Z轴的Z转动角ΔZ，作为转动角的两个因子。

如图14所示，第一和第二地磁仪401和402配置成分别与Xs和Ys轴平行，用来产生第一和第二磁检测信号(Mx，My)，而加速度计403配置成与Xs轴平行，用来产生加速度检测信号(Ax)，与图1的类似。静态角计算器405′根据第一和第二磁检测信号(Mx，My)和加速度检测信号(Ax)计算推测俯仰角P和作为方位偏离角的推测偏航角Ф。

朝向角计算器60′通过对Y转动角ΔY和Z转动角ΔZ进行时间积分计算具有第一和第二积分值(∑ΔY，∑ΔZ)的积分值组，再根据积分值组(∑ΔY，∑ΔZ)和静态角产生2-D朝向角(α，β)，然后将2-D朝向角存储在朝向角存储器70内。

在第一和第二陀螺仪302和303与转动角计算器310之间有第一和第二高通滤波器305和306，用途与图3中的第二和第三高通滤波器类似。当然，在高通滤波器的输出端也用了第一和第二A/D变换器，用来对信号进行数字处理，但是为了简明起见图中没有示出。

在这个实施例中可以用图6的高通滤波器校正器311，也可以用图8的数字高通滤波器代替高通滤波器305和306。为了简明起见，就不再对高通滤波器，A/D变换器，数字A/D变换器进行说明。

接收到时间序列转动角信号表示的时间序列Y转动角ΔY(t)和时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而n-(n-1)＝St，转动角计算器310′按照下式计算在t＝n时的Y转动角ΔY(n)和Z转动角ΔZ(n)：

$\left[\begin{array}{c}\mathrm{\ΔY}\left(n\right)\\ \mathrm{\ΔZ}\left(n\right)\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1/\cos \mathrm{\β}(n-1)\end{array}\right]\×\left[\begin{array}{c}\mathrm{Jy}\left(n\right)\\ \mathrm{Jz}\left(n\right)\end{array}\right]$

按照这个实施例实现的2-D朝向角检测器还包括第一和第二振荡消除器321和322，用来消除由于检测器本身例如因用户的头偶然摆动而振荡在第一和第二陀螺仪输出信号(Jy，Jz)中所引起的的噪声。

第一和第二振荡消除器321和322分别将第一和第二高通滤波器305和306与转动角计算器310连接。第一和第二振荡消除器321和322进行处理，分别产生第一和第二滤波器输出在一段预定时间上平均的第一和第二时间平均值。然后，第一和第二振荡消除器321和322分别将第一和第二时间平均值与第一和第二预定门限值相比较。第一和第二振荡消除器321和322分别在第一和第二时间平均值小于第一和第二门限值时临时除去部分第一和第二滤波器输出。产生第一和第二经处理信号提供给转动角计算器310′，作为第一和第二输入信号。

在每个振荡消除器321和322中，输入信号在t＝n时表示为J(n)，门限值为ε，振荡消除器(321和322)在满足以下表达式时判定输入信号有噪声：

|J(n)+J(n-1)+…+J(n-a)|/a＜ε

其中a为采样间隔数，它确定了计算平均值的时间长度(a×St)。a的值通常选择为3。于是，振荡消除器将输入信号J(n)处理为零(0)，也就是说，根据这个输入信号J(n)传送一个表示值为零(0)的输出信号。

静态角计算器405与图4中的静态角计算器405类似，只是它既没有A/D变换器40502也没有为了得到推测侧滚角R的侧滚角计算器40503。因此，静态角计算器405′的情况可以参看结合图4的说明。

2-D朝向角检测器也有一个静态角校正判定器，在推测俯仰角P和/或推测偏航角Ф判为精确时向朝向角计算器50′传送一个校正信号。

判定器的工作情况与图3中的校正判定器50类似，但是不判定推测侧滚角R。判定操作可以按照以上所示的式(5)和(6)执行。

此外，朝向角计算器60′只计算两个因子(α，β)。因此，响应来自校正判定器的校正信号对朝向角执行的校正操作与在图3中的朝向角计算器60中的类似，只是不处理侧滚角γ。因此，为了简明起见，不再对朝向角计算器60’进行说明。

                      工业应用

按照本发明，可以得到一种体积小、结构简单、精度高而成本低的朝向角检测器。这种朝向角检测器可以用于HMD中的检测器，各种试验各个活动物体的设备中的检测器，或者计算机、数据处理器和计算机游戏机的输入终端。

Claims (63)

1.一种朝向角检测器，所述朝向角检测器包括：多个在所述检测器上分别与定义一个检测器座标系的多个正交轴(Xs-Zs)平行配置的陀螺仪(301-303)，用来测量绕各自的轴(Xs-Zs)的角速度(Jx，Jy，Jz)；一个与所述多个陀螺仪(301-303)连接的转动角计算器(310)，用来根据所述角速度(Jx，Jy，Jz)计算转动角(ΔX，ΔY，ΔZ)；至少一个与至少所述正交轴(Xs-Zs)之一平行配置的加速度计(403，404，406)，用来测量加速度(Ax，Ay，Az)；一个与所述加速度计(403，404)连接的静态角计算器(405)，用来根据所述加速度(Ax，Ay)计算静态角(R，P)；以及一个与所述转动角计算器(310)和所述静态角计算器(405)连接的朝向角计算器(60)，用来将所述转动角(ΔX，ΔY，ΔZ)积分成一个积分角，再根据所述积分角、所述静态角(R，P)和一个方位偏离角(Ф)来计算朝向角(α，β，γ)，所述朝向角(α，β，γ)为所述检测器座标系(Xs-Ys-Zs)与所述检测器所在空间的基准座标系(X-Y-Z)之间的角度差；其中：有两个地磁仪(401，402)与所述多个正交轴中的两个(Xs，Ys)平行配置，用来测量在所述多个轴中的这两个轴(Xs，Ys)上的地磁分量(Mx，My)；而所述静态角计算器(405)与所述两个地磁仪(401，402)连接，用来根据所述地磁分量(Mx，My)计算方位角，再根据所述方位角产生偏离所述基准座标系(X-Y-Z)的所述方位偏离角(Ф)，作为所述静态角(R，P)的一个附加因子。

2.一种如在权利要求1中所提出的朝向角检测器，所述朝向角检测器包括一个与所述朝向角计算器(60)连接的朝向角存储器(76)，用来存储所述朝向角(α，β，γ)，其中：所述基准坐标系是一个具有一个垂直的Z轴和两个水平的Y和X轴，而所述检测器座标系是有三个分别与所述Z，Y，和X轴相应的正交轴Zs、Ys和Xs，所述朝向角用Z-Y-X的Euler角表示，它包括绕Z轴转动角度的偏航角α、绕Y轴转动角度的俯仰角β和绕X轴转动角度的侧滚角γ这三个分量；所述多个陀螺仪是分别与所述Xs、Ys和Zs轴平行配置的第一至第三陀螺仪(301-303)，用来测量分别绕所述Xs、Ys和Zs轴的第一至第三角速度(Jx，Jy，Jz)，所述转动角计算器(310)根据所述第一至第三角速度(Jx，Jy；Jz)计算绕X轴的X转动角ΔX、绕Y轴的Y转动角ΔY和绕Z轴的Z转动角ΔZ，作为所述转动角的三个因子；所述两个地磁仪是分别与Xs和Ys轴平行配置的第一和第二地磁仪(401，402)，分别用来产生第一和第二磁检测信号(Mx，My)，而所述至少一个加速度计是分别与Xs和Ys轴配置的第一和第二加速度计(403，404)，用来产生第一和第二加速度检测信号(Ax，Ay)，所述静态角计算器(405)根据所述第一和第二磁检测信号(Mx，My)和所述第一和第二加速度检测信号(Ax，Ay)计算推测俯仰角P、推测侧滚角R和作为所述方位偏离角的推测偏航角Ф，所述推测俯仰角P、所述推测侧滚角R和所述推测偏航角Ф是所述静态角的几个因子，以及所述朝向角计算器(60)通过对所述X转动角ΔX、Y转动角ΔY和Z转动角ΔZ进行时间积分计算由第一至第三积分值组成的积分值组(∑ΔX，∑ΔY，∑ΔZ)，再根据所述积分值组(∑ΔX，∑ΔY，∑ΔZ)和所述静态角产生所述朝向角，所述朝向角存储在所述朝向角存储器(70)内。

3.一种如在权利要求2中所提出的朝向角检测器，其中所述第一至第三陀螺仪(301-303)是各有一个压电振动器的Coriolis振动陀螺仪，所述第一至第三陀螺仪(301-303)产生表示绕所述Xs、Ys和Zs轴的角速度的第一至第三陀螺仪输出信号(Jx，Jy，Jz)。

4.一种如在权利要求3中所提出的朝向角检测器，所述朝向角检测器还包括分别与所述第一至第三Coriolis振动陀螺仪(301-303)连接的第一至第三高通滤波器(304-306)，分别用来消除包括在所述第一至第三Coriolis振动陀螺仪的第一至第三陀螺仪输出信号(Jx，Jy，Jz)内的偏移电压，产生第一至第三滤波器输出。

5.一种如在权利要求4中所提出的朝向角检测器，其中所述第一至第三高通滤波器(304-306)各有一个为0.1Hz甚至更低的截止频率。

6.一种如在权利要求4中所提出的朝向角检测器，其中所述第一至第三高通滤波器(304-306)各有一个可变的截止频率。

7.一种如在权利要求4中所提出的朝向角检测器，所述第一至第三高通滤波器(304-306)是模拟型高通滤波器，所述朝向角检测器还包括分别与所述第一至第三高通滤波器(304-306)连接的第一至第三模数变换器(307-309)，用来按采样间隔St分别对所述第一至第三滤波器输出进行采样，产生第一至第三时间序列数字信号，所述转动角计算器(310)接收所述第一至第三时间序列数字信号作为输入第一至第三时间序列数字信号，产生表示所述转动角的三个时间序列转动角信号(ΔX，ΔY，ΔZ)，所述朝向角计算器(60)接收所述三个时间序列转动角信号(ΔX，ΔY，ΔZ)，串行产生所述朝向角，以及所述朝向角存储器(70)与所述朝向角计算器(60)连接，用来存储所述朝向角计算器(60)串行发送的所述朝向角。

8.一种如在权利要求7中所提出的朝向角检测器，其中所述第一至第三时间序列转动角信号表示时间序列X转动角ΔX(t)、时间序列Y转动角ΔY(t)和时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而n-(n-1)＝St；所述转动角计算器(310)按照下式计算在t＝n时的X转动角ΔX(n)、Y转动角ΔY(n)和Z转动角ΔZ(n)：

$\left[\begin{array}{c}\mathrm{\ΔX}\left(n\right)\\ \mathrm{\ΔY}\left(n\right)\\ \mathrm{\ΔZ}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}0& \tan \mathrm{\β}(n-1)\sin \mathrm{\γ}(n-1)& \tan \mathrm{\β}(n-1)\cos \mathrm{\γ}(n-1)\\ 1& \cos \mathrm{\γ}(n-1)& -\sin \mathrm{\γ}(n-1)\\ 0& -\sin \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)& \cos \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)\end{array}\right]\×\left[\begin{array}{c}\mathrm{Jx}\left(n\right)\\ \mathrm{Jy}\left(n\right)\\ \mathrm{Jz}\left(n\right)\end{array}\right]$

其中β(n-1)和γ(n-1)为从所述朝向角存储器(70)读出的在t＝n-1时的俯仰角β和侧滚角γ，而Jx(n)，Jy(n)和Jz(n)是所述第一至第三输入时间序列数字信号在t＝n时的第一至第三数字信号。

9.一种如在权利要求8中所提出的朝向角检测器，所述朝向角检测器还包括三个分别连接在所述第一至第三模数变换器(307-309)与所述转动角计算器(310)之间的高通滤波器校正电路(311)，用来补偿所述第一至第三高通滤波器伴生的误差，产生第一至第三经校信号，所述第一至第三经校信号传送给所述转动角计算器(310)作为所述第一至第三输入时间序列数字信号，所述高通滤波器校正电路(311)各执行由下式给出的计算：

J(n)＝F(n)+(Δt/RC)×∑F(n)

其中J(n)为在t＝n的是所述输入时间序列数字信号中相应一个的所述经校信号，F(n)为所述第一至第三时间序列数字信号中相应一个在t＝n时的数字值，而Δt/RC是一个常数值。

10.一种如在权利要求9中所提出的朝向角检测器，其中所述高通滤波器校正电路(311)包括：一个包括一个第一加法器(31101)和一个寄存器(31102)的积分器，用来对所述相应时间序列数字信号进行积分，将积分值保存在所述寄存器(31102)内；一个常数值产生器(31103)，用来产生所述常数值；一个与所述寄存器(31102)和所述常数值产生器(31103)连接的乘法器(31104)，用来将所述积分值乘以所述常数值，产生一个乘后值；以及一个第二加法器(31105)，用来将所述乘后值与所述串行输入的数字值相加，产生所述经校信号。

11.一种如在权利要求10中所提出的朝向角检测器，其中所述高通滤波器校正电路(311)还包括一个偏移误差校正电路(31106-3108)，用来补偿在所述积分器(31101-31102)和所述第二加法器(31105)内的计算所引起的偏移误差，而所述偏移误差校正电路包括：一个输出判定器(31106)，用来观察什么时侯所述经校信号保持恒定，判定偏移误差程度；一个与所述输出判定器连接的误差校正器(31107)，用来响应所述误差程度产生一个误差校正信号，所述误差校正信号提供给所述寄存器(31102)，用来校正所述积分值；以及一个与所述误差校正器(31107)连接的常数值校正电路(31108)，用来响应所述误差校正信号校正所述常数值产生器(31103)的所述常数值。

12.一种如在权利要求4中所提出的朝向角检测器，其中所述第一至第三高通滤波器是分别产生第一至第三时间序列数字滤波器输出的第一至第三数字高通滤波器装置(图8)，所述第一至第三数字高通滤波器装置(图8)各包括：一个与所述第一至第三陀螺仪(301-303)中相应一个连接的模数变换器(313)，用来按采样间隔St对所述第一至第三陀螺仪输出信号中相应一个进行采样，产生一个时间序列数字信号；一个与所述模数变换器(313)连接的数字滤波器(315)，用来对所述时间序列数字信号进行滤波，产生所述第一至第三时间序列数字滤波器输出中相应一个，所述转动角计算器(310)接收所述第一至第三时间序列数字滤波器输出作为第一至第三输入时间序列数字信号，产生表示所述转动角的三个时间序列转动角信号(ΔX，ΔY，ΔZ)，所述朝向角计算器(60)接收所述三个时间序列转动角信号(ΔX，ΔY，ΔZ)，串行产生所述朝向角，而所述朝向角存储器(70)存储所述朝向角计算器串行传送的所述朝向角。

13.一种如在权利要求12中所提出的朝向角检测器，其中所述第一至第三时间序列转动角信号表示时间序列X转动角ΔX(t)，时间序列Y转动角ΔY(t)和时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而St＝n-(n-1)；所述转动角计算器(310)按照下式计算在t＝n时的X转动角ΔX(n)、Y转动角ΔY(n)和Z转动角ΔZ(n)：

$\left[\begin{array}{c}\mathrm{\ΔX}\left(n\right)\\ \mathrm{\ΔY}(n)\\ \mathrm{\ΔZ}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}0& \tan \mathrm{\β}(n-1)\sin \mathrm{\γ}(n-1)& \tan \mathrm{\β}(n-1)\cos \mathrm{\γ}(n-1)\\ 1& \cos \mathrm{\γ}(n-1)& -\sin \mathrm{\β}(n-1)\\ 0& -\sin \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)& \cos \mathrm{\γ}(n-1)/\cos \mathrm{\β}(n-1)\end{array}\right]\×\left[\begin{array}{c}\mathrm{Jx}\left(n\right)\\ \mathrm{Jy}\left(n\right)\\ \mathrm{Jz}\left(n\right)\end{array}\right]$

其中β(n-1)和γ(n-1)为从所述朝向角存储器(7 0)读出的在t＝n-1时的俯仰角β和侧滚角γ，而Jx(n)，Jy(n)和Jz(n)是所述第一至第三输入时间序列数字信号在t＝n时的第一至第三数字信号。

14.一种如在权利要求13中所提出的朝向角检测器，其中所述数字滤波器是具有延迟缓存器值(P1，P2)的无限冲激响应型数字滤波器(315)，而其中所述数字高通滤波器装置(图8)还包括：一个与所述数字滤波器(315)的输出端连接的正/负判定器(316)，用来根据所述时间序列数字滤波器输出判定什么时侯所述时间序列数字信号在正、负之间改变，产生一个改变信号；以及一个与所述数字滤波器(315)和所述正/负判定器(316)连接的延迟缓存器值复位电路(317)，用来响应所述改变信号对所述数字滤波器(315)的所述延迟缓存器值(P1，P2)复位。

15.一种如在权利要求14中所提出的朝向角检测器，其中所述数字高通滤波器装置(图8)还包括：一个偏移值产生器(319)，用来产生偏移值；一个与所述偏移值产生器(319)连接的、将所述模数变换器(313)与所述数字滤波器(315)连接的乘法器(320)，用来将所述时间序列数字信号乘以所述偏移值；以及一个与所述偏移值产生器(319)和所述正/负判定器(316)连接的偏移值校正电路(318)，用来响应所述改变信号校正所述偏移值。

16.一种如在权利要求2中所提出的朝向角检测器，其中所述静态角计算器(405)包括：分别与所述第一和第二加速度计(403，404)连接的第一和第二模数变换器(40501，40502)，用来按采样间隔St对所述第一和第二加速度检测信号(Ax，Ay)进行采样，产生第一和第二时间序列数字加速度信号；一个与所述第一模数变换器(40501)连接的推测俯仰角计算器(40504)，用来根据所述第一时间序列数字加速度信号计算所述推测俯仰角P；以及一个与所述第二模数变换器(40502)和所述推测俯仰角计算器(40504)连接的推测侧滚角计算器(40503)，用来根据所述第二时间序列加速度信号和所述推测俯仰角P计算所述推测侧滚角R，所述推测侧滚角R和所述推测俯仰角P是所述静态角的因子。

17.一种如在权利要求16中所提出的朝向角检测器，其中所述第一和第二时间序列数字加速度信号表示为Ax(t)和Ay(t)，其中t＝n，n-1，…，1，而St＝n-(n-1)；所述推测俯仰角计算器(40504)和所述推测侧滚角计算器(40503)分别按照下式计算推测俯仰角P和推测侧滚角R：

P(n)＝sin^-1Ax(n)

R(n)＝sin^-1[Ay(n)/cosP(n)]

其中P(n)，R(n)，Ax(n)和Ay(n)分别表示在t＝n时的P，R，Ax(t)和Ay(t)。

18.一种如在权利要求17中所提出的朝向角检测器，其中所述静态角计算器(405)包括：分别与所述第一和第二地磁仪(401，402)连接的第三和第四模数变换器(40505，40506)，用来按采样间隔St对所述第一和第二磁检测信号Mx和My进行采样，产生第一和第二时间序列数字磁检测信号；一个与所述第三和第四模数变换器(40505，40506)、所述推测侧滚角计算器(40503)和所述推测俯仰角计算器(40564)连接的座标系变换计算器(40507)，用来将所述第一和第二时间序列数字磁检测信号变换成表示在所述基准X-Y-Z座标系的X-Y平面内两个相应地磁分量Hx和Hy的第一和第二时间序列数字地磁分量信号；一个与所述座标系变换计算器(40507)连接的方位角计算器(40508)，用来根据所述地磁分量Hx和Hy计算方位角 产生作为所述方位偏离角的推测偏航角Ф；以及一个与所述方位角计算器(40508)连接的方位角存储器(40512)，用来存储所述方位角

19.一种如在权利要求18中所提出的朝向角检测器，其中所述静态角计算器(405)还包括一个与所述第三和第四模数变换器(40505，40506)连接的Z方向磁分量产生器(40509-40515)，用来根据所述第一和第二时间序列磁检测信号Mx和My、预先测量的地磁Ht、由所述检测器座标系的Xs和Ys轴定义的Xs-Ys平面偏离所述X-Y平面的初始倾斜角θ0和所述Xs-Ys平面偏离所述X-Y平面的当前倾斜角θ2产生一个时间序列数字Z方向磁分量信号Mz；而所述座标系变换计算器(40507)按照下式根据所述第一和第二时间序列磁检测信号Mx和My、所述时间序列数字Z方向磁分量信号Mz和所述推测俯仰和侧滚角P和R计算所述地磁分量Hx和Hy：

Hx(n)＝cos[P(n)]×Mx(n)+sin[P(n)]×Mz(n)

Hy(n)＝sin[R(n)]×sin[P(n)]×Mx(n)

       +cos[R(n)]×cos[P(n)]×Mz(n)

其中Hx(n)，Hy(n)，Mx(n)，My(n)，Mz(n)，P(n)和R(n)分别表示在t＝n时的Hx，Hy，Mx，My，Mz，P和R，而所述方位角计算器(40508)按照下式根据Hx(n)和Hy(n)计算所述方位角 在t＝n时的

所述方位角计算器(40508)还按照下式根据所述 和起始方位偏离角 计算所述推测偏航角Ф在t＝n时的Ф(n)：

20.一种如在权利要求19中所提出的朝向角检测器，其中所述Z方向磁分量产生器(40509-40515)包括：

一个地磁产生器(40509，40510)，用来产生所述地磁Ht；

一个与所述地磁产生器(40509，40510)和所述第三和第四模数变换器(40505，40506)连接的当前Z方向磁分量绝对值计算器(40511)，用来按照下式根据所述地磁Ht和所述第一和第二时间序列磁检测信号Mx和My在t＝n时的所述当前值Mx(n)和My(n)计算Z方向磁分量绝对值|Mz(n)|：

|Mz(n)|＝[Ht²-{Mx(n)²+My(n)²}]^1/2

一个与所述朝向角存储器(70)和所述方位角存储器(40512)连接的当前倾斜角计算器(40513)，用来计算所述当前倾斜角θ2；

一个与所述当前倾斜角计算器(40513)连接的符号判定器(40514)，用来根据所述当前倾斜角θ2和所述初始倾斜角θ0产生所述当前Z方向磁分量Mz(n)的符号(+或-)；以及

一个与所述当前Z方向磁分量绝对值计算器(40511)和所述符号判定器(40514)连接的当前Z方向磁分量计算器(40515)，用来产生所述Z方向磁分量Mz(n)。

21.一种如在权利要求20中所提出的朝向角检测器，其中所述地磁产生器(40509，40510)包括：

一个分别通过两个开关元件(S1，S2)有选择地与所述第三和第四模数变换器(40505，40506)连接的初始地磁存储器(40509)，用来通过在初始时间有选择地接通所述两个开关元件(S1，S2)存储来自所述第三和第四模数变换器(40505，40506)的初始X方向、Y方向和Z方向地磁分量Mx(0)、My(0)和Mz(0)；以及

一个与所述初始地磁存储器(40509)连接的处理器(40510)，用来按照下式将所述初始X方向、Y方向和Z方向地磁分量Mx(0)、My(0)和Mz(0)处理成所述地磁Ht：

|Ht|＝[Mx(0)²+My(0)²+Mz(0)²]^1/2

所述绝对值|Ht|作为所述地磁Ht存储在所述初始地磁存储器(40509)内。

22.一种如在权利要求21中所提出的朝向角检测器，其中，在初始状态：

所述朝向角检测器一方面定位成使所述第一和第二地磁仪(401，402)中特定的一个与所述基准座标系的所述Z轴平行，然后接通所述两个开关元件(S1，S2)中相应的一个，将所述特定的地磁仪(401或402)检测的所述地磁分量存储在所述初始地磁存储器(40569)内作为所述初始Z方向地磁分量Mz(0)；以及

所述朝向角检测器另一方面定位成使所述第一和第二地磁仪(401，402)与所述基准座标系的所述X和Y轴平行，然后接通所述两个开关元件(S1，S2)，将所述第一和第二地磁仪(401，402)检测的所述地磁分量存储在所述初始地磁存储器(40569)内作为所述初始X方向地磁分量Mx(0)和所述初始Y方向地磁分量My(0)。

23.一种如在权利要求20中所提出的朝向角检测器，其中所述当前倾斜角计算器(40513)按照下式根据从所述方位角存储器(40512)读出的所述方位角

在t＝n-1时的角值 和从所述朝向角存储器(70)读出的所述侧滚和俯仰角β和γ的侧滚和俯仰角值β(n-1)和γ(n-1)计算所述当前倾斜角θ2：

θ2＝sin^-1[aa/(aa²+cc²)^1/2]

其中，

而cc＝cosγ(n-1)×cosβ(n-1)。

24.一种如在权利要求23中所提出的朝向角检测器，其中所述符号判定器(40514)一方面在所述当前倾斜角θ2大于所述初始倾斜角θ0而小于90度时将所述符号判为正(+)，另一方面在所述当前倾斜角θ2等于或小于所述初始倾斜角θ0而大于-90度时将所述符号判为负(-)。

25.一种如在权利要求2中所提出的朝向角检测器，还包括一个与所述静态角计算器(405)和所述朝向角计算器(60)连接的静态角校正判定器(50)，用来判定所述静态角(P，R，Ф)的精确性，产生一个校正信号，而其中所述朝向角计算器(60)响应所述校正信号计算一个校正值，再用所述校正值校正所述积分值组，产生一个修正值组，所述修正值组作为所述朝向角(α，β，γ)发送。

26.一种如在权利要求25中所提出的朝向角检测器，其中所述朝向角计算器(60)按照下式计算第一至第三积分值∑ΔX、∑ΔY和∑ΔZ：

∑ΔX(n)＝γ(n-1)+ΔX(n)，

∑ΔY(n)＝β(n-1)+ΔY(n)，及

∑ΔZ(n)＝α(n-1)+ΔZ(n)

其中ΔX(n)、ΔY(n)和ΔZ(n)分别为在时间t＝n时的X转动角ΔX、Y转动角ΔY和Z转动角ΔZ，而γ(n-1)、β(n-1)和α(n-1)分别为在时间t＝n-1时的所述侧滚角γ、所述俯仰角β和所述侧滚角α，所述γ(n-1)、β(n-1)和α(n-1)从所述朝向角存储器(70)读出。

27.一种如在权利要求26中所提出的朝向角检测器，其中所述朝向角计算器(60)在接收到所述校正信号后按照下式计算第一至第三角误差Ex、Ey和Ez：

Ex＝∑ΔX(n)-R(n)，

Ey＝∑ΔY(n)-P(n)，及

Ez＝∑ΔZ(n)-Ф(n)

所述朝向角计算器(60)还根据所述第一至第三角误差确定作为所述校正值的第一至第三校正值C1、C2和C3，再按照下式计算所述第一至第三修正值作为所述侧滚角γ、所述俯仰角β和所述偏航角α：

γ＝∑ΔX(n)-C1，

β＝∑ΔY(n)-C2，及

α＝∑ΔZ(n)-C3，

其中C1、C2和C3分别选择为小于所述第一至第三角误差Ex、Ey和Ez。

28.一种如在权利要求27中所提出的朝向角检测器，其中所述C1、C2和C3分别由k1×Ex、k2×Ey和k3×Ez确定，其中k1、k2和k3是小于1的常数值。

29.一种如在权利要求27中所提出的朝向角检测器，所述朝向角检测器还包括一个与所述静态角计算器(405)和所述朝向角计算器(60)连接的校正系数产生器(80)，用来产生一组第一至第三系数m1、m2和m3，所述第一至第三系数m1、m2和m3根据从所述静态角计算器(405)接收的所述推测俯仰角P和所述推测侧滚角R的角值从一些预定的不同值中选取，而其中所述朝向角计算器按照所述这组第一至第三系数m1，m2和m3通过分别乘以所述第一至第三系数m1，m2和m3修正所述第一至第三校正值C1，C2和C3。

30.一种如在权利要求26中所提出的朝向角检测器，其中所述校正判定器(50)配置有一个静态角存储器(501)，用来存储从所述静态角计算器(405)接收的所述推测俯仰角P和所述推测侧滚角R，而其中所述校正判定器(50)将在时间t＝n时接收的推测俯仰角P(n)和推测侧滚角R(n)与从所述静态角存储器(501)读出的在t＝n-1时的早先推测俯仰角P(n-1)和推测侧滚角R(n-1)相比较，在分别满足以下二式(1)和(2)时判定所述推测俯仰角P(n)和推测侧滚角R(n)为精确：

|R(n)-R(n-1)|-B≈0                        (1)，及

|P(n)-P(n-1)|-D≈0                        (2)，

其中B和D选择为正值。

31.一种如在权利要求30中所提出的朝向角检测器，其中所述B和D为几乎等于零(0)的很小的值。

32.一种如在权利要求30中所提出的朝向角检测器，其中所述校正判定器(50)与所述转动角计算器(310)连接，接收所述X转动角ΔX(n)和所述Y转动角ΔY(n)，而所述B和D分别选择为所述X转动角ΔX(n)和所述Y转动角ΔY(n)。

33.一种如在权利要求30中所提出的朝向角检测器，其中所述校正判定器(56)还分别从所述静态角计算器(405)和转动角计算器(310)接收所述推测偏航角Ф(t)和所述Z转动角ΔZ(t)，所述推测偏航角Ф(t)存储在所述静态角存储器(501)内，所述校正角判定器(50)还按照下式(3)处理在t＝n时的推测偏航角Ф(n)和Z转动角ΔZ(n)和从所述朝向角存储器(501)读出的在t＝n-1的偏航角Ф(n-1)：

|Ф(n)-Ф(n-1)|-ΔZ(n)≈0         (3)

在式(3)成立时判定所述推测偏航角Ф(n)是精确的。

34.一种如在权利要求26中所提出的朝向角检测器，其中所述校正判定(50)配置有一个静态和转动角存储器(501)，用来存储从所述静态角计算器(405)接收的所述推测俯仰角P(n)和所述推测侧滚角R(n)以及从所述转动角计算器(310)接收的所述X转动角ΔX和所述Y转动角ΔY，而其中所述校正判定器(50)计算所述推测俯仰角P(t)的变化的滑动平均、所述推测侧滚角R(t)的变化的滑动平均、所述X转动角ΔX(t)的滑动平均和所述Y转动角Δy(t)的滑动平均，并且在分别满足以下二式时判定所述推测俯仰角P(n)和所述推测侧滚角R(n)是精确的：

[R(n)-R(n-1)]/C-(ΔX(n)+ΔX(n-1)+…+ΔX(n-C+1)]/C≈0  (1)

[P(n)-P(n-1)]/C-[ΔY(n)+ΔY(n-1)+…+ΔY(n-C+1)]/C≈0  (2)

35.一种如在权利要求34中所提出的朝向角检测器，其中所述校正判定器(50)将来自所述静态角计算器(405)的所述推测偏航角Ф(n)和来自所述转动角计算器(310)的所述Z转动角ΔZ存储在所述静态和转动角存储器(501)内，而其中所述校正判定器(59)还计算所述推测偏航角Ф(t)的变化的滑动平均和所述Z转动角ΔZ(t)的滑动平均，而且在满足下式(3)时判定所述推测偏航角Ф(n)是精确的：

[Ф(n)-Ф(n-1)]/C-[ΔZ(n)+ΔZ(n-1)+…+ΔZ(n-C+1)]/C≈0(3)

36.一种如在权利要求30或34中所提出的朝向角检测器，其中所述校正判定器(50)在所述推测俯仰角P(n)和所述推测侧滚角R(n)中至少一个判为精确时产生所述校正信号，而所述校正判定器(50)在所述推测俯仰角和所述推测侧滚角R(n)中没有一个判为精确时产生一个不校正信号，所述朝向角计算器(60)响应所述不校正信号产生所述积分值组(∑ΔX，∑ΔY，∑ΔZ)作为所述朝向角(α，β，γ)。

37.一种如在权利要求33或35中所提出的朝向角检测器，其中所述校正判定器(50)在所述推测偏航角Ф(n)判为精确时无论所述推测俯仰角和所述推测侧滚角R(n)是否判为精确都产生所述校正信号，而所述校正判定器(50)在所述推测俯仰角、所述推测侧滚角R(n)和所述偏航角Ф(n)中没有一个判为精确时产生一个不校正信号，所述朝向角角计算器(60)响应所述不校正信号产生所述积分值组(∑ΔX，∑ΔY，∑ΔZ)作为所述朝向角(α，β，γ)。

38.一种如在权利要求30、33、34和35中任何一个权利要求中所提出的朝向角检测器，其中所述校正判定器(50)在判定所述推测侧滚角R、所述推测俯仰角P和所述推测偏航角Ф中有几个精确后产生一个指出所述推测侧滚角R、所述推测俯仰角P和所述推测偏航角中哪几个判为精确的指示信号作为所述校正信号，而其中所述朝向角计算器(60)按照所述指示信号用所述推测俯仰角P、所述推测侧滚角R和所述推测偏航角Ф修正所述第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中与所述推测侧滚角R、所述推测俯仰角P和所述推测偏航角Ф中判为精确的相应的那几个积分值，产生几个指定修正值，所述朝向角计算器(60)发送所述指定修正值，作为所述侧滚角γ、所述俯仰角β和所述偏航角α中分别与所述第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中指定修正的相应的那几个的规定值，所述朝向角计算器(60)还发送所述第一至第三积分值∑ΔX(n)、∑ΔY(n)和∑ΔZ(n)中所述指定修正的之外的其余几个，作为所述侧滚角γ、所述俯仰角β和所述偏航角α中所述规定的几个之外的其余几个。

39.一种如在权利要求16中所提出的朝向角检测器，所述朝向角检测器还包括一个与所述检测器座标系的Zs轴平行配置的第三加速度计(406)，用来产生一个第三加速度检测信号(Az)，其中所述静态角计算器(405)还包括一个与所述第三加速度计(406)连接的附加模数变换器(4061)，用来按采样间隔St对所述第三加速度检测信号(Az)进行采样，产生一个第三时间序列数字加速度信号，所述第三时间序列加速度信号供所述侧滚角计算器(40504)计算所述推测侧滚角R用。

40.一种如在权利要求39中所提出的朝向角检测器，其中所述第一、第二和第三时间序列数字加速度信号表示为Ax(t)、Ay(t)和Az(t)，其中t＝n，n-1，…，1，而St＝n-(n-1)；所述推测俯仰角计算器(40504)和所述推测侧滚角计算器(40503)分别按照下式计算推测俯仰角P和推测侧滚角R：

P(n)＝sin^-1Ax(n)

在Az＞0时，

R(n)＝sin^-1[Ay(n)/cosP(n)]

在Az＜0而Ax＞0时

R(n)＝π-sin^-1[Ay(n)/cosP(n))，and

在Az＜0而Ax＜0时

R(n)＝-π-sin^-1[Ay(n)/cosP(n)]，

其中P(n)，R(n)，Ax(n)，Ay(n)和Az(n)分别表示在t＝n时的P，R，Ax(t)，Ay(t)和Az(t)。

41.一种如在权利要求25中所提出的朝向角检测器，所述朝向角检测器还包括一个与所述检测器座标系的Zs轴平行配置的第三加速度计(406)，用来产生一个第三加速度检测信号(Az)，其中所述静态角校正判定器(50)接收所述第一、第二和第三加速度检测信号(Ax，Ay，Az)，按照以下表达式计算Xs轴方向、Ys轴方向和Zs轴方向的合成加速度矢量的绝对值：

|A|＝(Ax+Ay+Az)^1/2

所述校正判定器(50)在所述|A|≈1G时产生所述校正信号，其中G为重力加速度。

42.一种如在权利要求3中所提出的朝向角检测器，所述朝向角检测器还包括一个印刷电路板装置(201)，它具有相互垂直于的一个第一和一个第二侧板段以及一个第三侧板段，形成一个直角三角形筒，所述第一至第三陀螺仪(301，302，303)的所述第一至第三压电陶瓷振动器分别固定地安装在所述第一、第二和第三侧板段上，使得所述第一至第三陀螺仪与所述检测座标系的Xs轴、Ys轴和Zs轴平行配置。

43.一种如在权利要求42中所提出的朝向角检测器，其中所述印刷电路板装置(201)具有一个所述直角三角形筒在一个单平面上展开的端缘，有一些接线端(204)从所述端缘伸出，用来与外部电部件电连接。

44.一种如在权利要求43中所提出的朝向角检测器，其中所述第三侧板段在与所述端缘相反的方向上延伸，超过所述第一和第二侧板段。

45.一种如在权利要求44中所提出的朝向角检测器，其中所述印刷电路板装置(201)是单个挠性印制电路板，弯折成具有所述第一至第三侧板部分的所述直角三角形筒。

46.一种如在权利要求45中所提出的朝向角检测器，其中所述挠性印制电路板(201)具有两个狭缝(202，203)，开在所述印刷电路板的弯折部分。

47.一种如在权利要求42中所提出的朝向角检测器，其中所述第一至第三压电振动器备用用压电陶瓷、压电单晶和硅材料中任何一种材料制成。

48.一种如在权利要求1中所提出的朝向角检测器，其中所述基准座标系是一个具有一个垂直的Z轴和两个水平的Y和X轴的三维座标系，而所述检测器座标系是另一个具有三个分别与所述Z，Y，和X轴相应的正交轴Zs、Ys和Xs的三维座标系，所述朝向角用Z-Y-X的Euler角表示，它包括绕Z轴转动角度的偏航角α、绕Y轴转动角度的俯仰角β和绕X轴转动角度的侧滚角γ所述三个分量，所述朝向角只发送作为一个2-D朝向角的所述偏航角α和所述俯仰角β，其中所述多个陀螺仪是分别与所述Ys和Zs轴平行配置的第一和第二陀螺仪(302，303)，用来测量绕所述Ys和Zs轴的第一和第二角速度(Jy，Jz)，所述转动角计算器(310′)根据所述第一和第二角速度(Jy，Jz)计算绕Y轴的Y转动角ΔY和绕Z轴的Z转动角ΔZ作为所述转动角的两个因子，而其中所述两个地磁仪是分别与Xs和Ys轴平行配置的第一和第二地磁仪(401，402)，分别用来产生第一和第二磁检测信号(Mx，My)，而所述至少一个加速度计是一个与Xs轴平行配置的加速度计(403)，用来产生一个加速度检测信号(Ax)，所述静态角计算器(405′)根据所述第一和第二磁检测信号(Mx，My)和所述加速度检测信号(Ax)计算推测俯仰角P和作为所述方位偏离角的推测偏航角Ф，所述推测俯仰角P和所述推测偏航角Ф是所述静态角的因子，而其中所述朝向角计算器(60′)通过对所述Y转动角ΔY和Z转动角ΔZ进行时间积分计算一个由第一和第二积分值组成的积分值组(∑ΔY，∑ΔZ)，再根据所述积分值组(∑ΔY，∑ΔZ)和所述静态角产生所述2-D朝向角(α，β)，所述2-D朝向角存储在一个朝向角存储器(70)内。

49.一种如在权利要求48中所提出的朝向角检测器，其中所述第一和第二陀螺仪(302，303)是各有一个压电振动器的Coriolis振动陀螺仪，所述第一和第二陀螺仪(302，303)产生表示绕所述Ys和Zs轴的角速度的第一和第二陀螺仪输出信号(Jy，Jz)。

50.一种如在权利要求49中所提出的朝向角检测器，朝向角检测器所述还包括分别与所述第一和第二Coriolis振动陀螺仪(302，303)连接的第一和第二高通滤波器(305，306)，用来消除包括在所述第一和第二Coriolis振动陀螺仪的第一和第二陀螺仪输出信号(Jy，Jz)内的偏移电压，产生第一和第二滤波器输出。

51.一种如在权利要求50中所提出的朝向角检测器，其中所述转动角计算器(310′)接收所述第一和第二滤波器输出作为第一和第二输入信号，产生表示所述转动角的第一和第二时间序列转动角信号(ΔY，ΔZ)，所述朝向角计算器(60′)接收所述第一和第二时间序列转动角信号(ΔY，ΔZ)，串行产生所述朝向角，而其中所述朝向角存储器(70)与所述朝向角计算器(60)连接，用来存储所述朝向角计算器(60′)串行传送的所述朝向角。

52.一种如在权利要求51中所提出的朝向角检测器，其中所述第一和第二时间序列角信号表示时间序列Y转动角ΔY(t)和时间序列Z转动角ΔZ(t)，其中t＝n，(n-1)，(n-2)，…，1，而n-(n-1)＝St，所述转动角计算器(310)按照下式计算在t＝n时的Y转动角ΔY(n)和Z转动角ΔZ(n)：

$\left[\begin{array}{c}\mathrm{\ΔY}\left(n\right)\\ \mathrm{\ΔZ}\left(n\right)\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1/\cos \mathrm{\β}(n-1)\end{array}\right]\×\left[\begin{array}{c}\mathrm{Jy}\left(n\right)\\ \mathrm{Jz}\left(n\right)\end{array}\right]$

其中β(n-1)为从所述朝向角存储器(70)读出的在t＝n-1时的俯仰角β，Jy(n)和Jz(n)是在t＝n时的所述第一和第二输入信号。

53.一种如在权利要求51中所提出的朝向角检测器，还包括分别将所述第一和第二高通滤波器(305，306)与所述转动角计算器(310′)连接的第一和第二振荡消除器(321，322)，用来消除由于所述检测器本身振荡而包括在所述第一和第二陀螺仪输出信号(Jy，Jz)内的噪声。

54.一种如在权利要求53中所提出的朝向角检测器，其中所述第一和第二振荡消除器(321，322)进行处理，分别产生所述第一和第二滤波器输出在一段预定时间上的第一和第二时间平均值，再将所述第一和第二时间平均值分别与第一和第二预定门限值相比较，所述第一和第二振荡消除器(321，322)在所述第一和第二时间平均值分别小于所述第一和第二门限值时临时除去部分所述第一和第二滤波器输出，所述第一和第二经处理信号提供给所述转动角计算器(310′)作为所述第一和第二输入信号。

55.一种如在权利要求48中所提出的朝向角检测器，其中所述静态角计算器(405′)包括一个与所述加速度计(403)连接的第一模数变换器(40501)，用来按采样间隔St对所述加速度检测信号(Ax)进行采样，产生一个时间序列数字加速度信号，一个与所述第一模数变换器(40501)连接的推测俯仰角计算器(40504)，用来根据所述时间序列数字加速度信号计算所述推测俯仰角P。

56.一种如在权利要求55中所提出的朝向角检测器，其中所述时间序列数字加速度信号表示为Ax(t)，其中t＝n，n-1，…，1，而St＝n-(n-1)，所述推测俯仰角计算器(40504)按照下式计算所述推测俯仰角P；

P(n)＝sin^-1Ax(n)

其中P(n)和Ax(n)分别表示在t＝n时的P和Ax(t)。

57.一种如在权利要求55中所提出的朝向角检测器，其中所述静态角计算器(405′)包括分别与所述第一和第二地磁仪(401，402)连接的第二和第三模数变换器(40505，40506)，用来按采样间隔St对所述第一和第二磁检测信号Mx和My进行采样，产生第一和第二时间序列数字磁检测信号，有一个方位角计算器(40508)与所述第二和第三模数变换器(40505，40506)连接，用来根据所述第一和第二时间序列数字磁检测信号计算方位角

产生推测偏航角Ф作为所述方位偏离角。

58.一种如在权利要求57中所提出的朝向角检测器，所述朝向角检测器还包括一个与所述静态角计算器(405′)和所述朝向角计算器(60′)连接的静态角校正判定器(50′)，用来判定器所述静态角(P，Ф)的精确性，产生一个校正信号，其中所述朝向角计算器(60′)响应所述校正信号计算一个校正值，再用所述校正值校正所述积分值组，产生一个修正值组，所述修正值组作为所述2-D朝向角(α，β)发送。

59.一种如在权利要求58中所提出的朝向角检测器，其中所述朝向角计算器(60′)按照下式计算第一和第二积分值∑ΔY和∑ΔZ：

∑Y(n)＝β(n-1)+ΔY(n)，及

∑Z(n)＝α(n-1)+ΔZ(n)，

其中ΔY(n)和ΔZ(n)分别为在时间t＝n时的Y转动角ΔY和Z转动角ΔZ，而β(n-1)和α(n-1)分别为在时间t＝n-1时的所述俯仰角β和所述侧滚角α，所述β(n-1)和α(n-1)从朝向角存储器(70)读出。

60.一种如在权利要求59中所提出的朝向角检测器，其中所述朝向角计算器(60′)在接收到所述校正信号后按照下式计算第一和第二角误差Ey和Ez：

Ey＝∑ΔY(n)-P(n)，及

Ez＝∑ΔZ(n)-Ф(n)，

所述朝向角计算器(60′)还根据所述第一和第二角误差确定第一和第二校正值C1和C2作为所述校正值，再按照下式计算所述第一和第二修正值作为所述俯仰角β和所述偏航角α：

β＝∑ΔY(n)-C1，及

α＝∑ΔZ(n)-C2，

其中C1和C2选择为分别小于所述第一和第二角误差Ey和Ez。

61.一种如在权利要求60中所提出的朝向角检测器，其中所述C1和C2分别由k1×Ey和k2×Ez确定，其中k1和k2为小于1的常数值。

62.一种如在权利要求59中所提出的朝向角检测器，其中所述校正判定器(50′)与所述转动角计算器(310)连接，所述校正判定器(50′)计算所述推测俯仰角P(t)的变化的滑动平均、所述Y转动角ΔY(t)的滑动平均、所述推测偏航角Ф(t)的变化的滑动平均和所述z转动角ΔZ(t)的滑动平均，在以下二式(1)和(2)满足时判定所述推测俯仰角P(n)和所述推测偏航角Ф(n)是精确的：

[P(n)-P(n-1)]/C-[ΔY(n)+ΔY(n-1)+…+ΔY(n-C+1)]/C≈0   (1)

[Ф(n)-Ф(n-1)]/C-[ΔZ(n)+ΔZ(n-1)+…+ΔZ(n-C+1)]/C≈0 (2)

63.一种如在权利要求62中所提出的朝向角检测器，其中所述校正判定器(50′)在所述推测俯仰角和所述推测偏航角Ф(n)中至少一个判为精确时产生所述校正信号，而所述校正判定器(50′)在所述推测俯仰角P(n)和所述推测偏航角Ф(n)中没有一个判为精确时产生一个不校正信号，所述朝向角计算器(60′)响应所述不校正信号产生所述积分值组(∑ΔY(n)和∑ΔZ(n))作为所述2-D朝向角(β，α)。

64.一种如在权利要求62中所提出的朝向角检测器，其中所述校正判定器(50′)在判定所述推测俯仰角P和所述推测偏航角Ф中有一个精确后产生一个指出所述推测俯仰角P和所述推测偏航角中哪一个判为精确的指示信号作为所述校正信号，而其中所述朝向角计算器(60′)响应所述指示信号修正所述第一和第二积分值∑ΔY(n)和∑ΔZ(n)中与所述推测俯仰角P和所述推测偏航角Ф中判为精确的相应的积分值，产生一个指定修正值，所述朝向角计算器(60′)发送所述指定修正值，作为所述俯仰角β和所述偏航角α中与所述第一和第二积分值∑ΔY(n)和∑ΔZ(n)中指定修正的相应的这个的规定值，所述朝向角计算器(60′)还发送所述第一和第二积分值∑ΔY(n)和∑ΔZ(n)中所述指定修正的之外的另一个积分值，作为所述俯仰角β和所述偏航角α中所述规定的之外的另一个。

Images