CN102692219B - 操作设备上的磁罗盘的方法 - Google Patents

Abstract

使用安装在挖掘机上的磁场传感器，使用磁倾计，使用陀螺传感器以及使用处理器电路来确定工地的挖掘机的朝向的方法，所述磁倾计提供挖掘机的倾斜和摇摆的指示，所述陀螺传感器感测挖掘机围绕磁场传感器的Z轴的旋转率，以及所述处理器电路具有关联存储器。在挖掘机以匀速旋转时使用陀螺传感器来创建和存储数据的标定表。使用来自磁倾计的输出将感测到的磁场指示从传感器的坐标参考系转换至本地水平面坐标参考系。

Description

操作设备上的磁罗盘的方法

技术领域

本发明涉及一种用于设备中的磁罗盘，更特别地，涉及一种使用设置于可移动的设备或车辆(例如，挖掘机)中的磁罗盘装置的方法。

背景技术

感测地磁场的方向并提供用于显示和由控制系统使用的输出的传感器配置已经使用了一段时间。这样的配置已经经受并且必须补偿″硬铁″效应和″软铁″效应。这两种效应都使得传感器局部地区的地磁力线的形状失真，但以不同的方式。硬铁效应起因于永久磁体和车辆中被磁化的铁或钢。硬铁效应使得地磁场在特定方向上偏移，并且当设备(例如，挖掘机)围绕垂直轴旋转时不发生变化。由硬铁效应引起的误差在设备的完整循环(complete rotation)期间经过单个转。软铁效应起因于地磁场与挖掘机的磁性渗透材料的交互作用。软铁效应在设备旋转时发生变化，并且，由软铁效应引起的误差在设备的完整循环期间经过两个转。

典型地，地磁场由三个磁场传感器感测，所述传感器被定位以检测地磁场在三个正交方向上的强度，参考的传感器的坐标系的X、Y和Z方向。由所述三个传感器的相对输出，易于确定有关传感器的场的相对方位和强度。可以想到，如果传感器坐标参考系没有对准工地的本地水平面坐标参考系，则来自所述传感器的X、Y和Z坐标参考系的所感测的磁场指示必须被转换至本地水平面坐标参考系，以确定磁力线的方向，从而确定关于工地所述设备的朝向。

过去，已经使用了多种方法来针对硬铁效应和软铁效应补偿磁罗盘系统。作为繁重的标定工序的延伸，许多这些先有技术的标定技术需要在各个方向定位的设备和罗盘的磁场强度的大量测量。

发明内容

一种使用安装在挖掘机上的磁场传感器来确定工地的挖掘机的朝向的方法。所述磁场传感器提供传感器的X、Y和Z方向上的地磁场的大小的指示。所述方法使用磁倾计，其提供挖掘机的倾斜和摇摆的指示，陀螺传感器，其感测挖掘机围绕磁场传感器Z轴的旋转，以及处理器电路，其具有相关存储器。所述处理器电路响应于磁场传感器、磁倾计和陀螺传感器。所述方法包括创建并存储数据的标定表，用磁场传感器感测挖掘机处的磁场，提供传感器的X、Y和Z方向上的磁场大小的指示，使用来自磁倾计的输出将感测到的磁场指示从传感器的X、Y和Z坐标参考系转换至本地水平面坐标参考系，确定经转换的感测到的磁场指示的硬铁分量并除去这样的硬铁分量以在本地水平面坐标参考系中产生经过校正的磁场指示，并且，将经过校正的指示与存储的调节数据进行比较以确定挖掘机的方向。创建和存储数据的标定表的步骤包括基本上以恒速围绕大体垂直的Z轴旋转挖掘机，存储来自磁场传感器和陀螺传感器的输出，基于来自磁场传感器的数据值的重复来确定挖掘机的完整循环的开始和结束，将在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量，并存储这样的调节数据。

创建和存储数据的标定表还可以包括针对由硬铁引起的误差补偿所存储的输出。创建和存储数据的标定表还可以包括通过将挖掘机向预定参考朝向旋转而固定(bench)挖掘机，并确定来自表示预定朝向的表的对应数据，从而数据的平衡表示挖掘机的朝向在角度增量上偏离预定参考朝向。把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应指示数据调节成大致相等的角度增量可以包括把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应指示数据调节成一度的增量。

确定经转换的感测到的磁场指示的硬铁分量可以包括定位挖掘机以具有向着地磁北极的朝向，用磁场传感器感测挖掘机处的磁场，提供传感器的X、Y和Z方向上的磁场大小的指示，感测挖掘机的倾斜和摇摆，将挖掘机倾斜几度，用磁场传感器感测挖掘机处的磁场，提供传感器的X、Y和Z方向上的磁场大小的指示，并感测挖掘机的倾斜和摇摆。

附图说明

图1说明应用电子罗盘的类型的挖掘机；

图2是说明朝向参数的附图；

图3和4是说明设备、地的参照系和传感器的参照系之间关系的附图；

图5是说明软铁补偿计算的流程图；和

图6是用于实践所述补偿法的处理器电路和相关部件的示意图。

具体实施方式

挖掘和建筑设备控制系统通常在许多传感器当中包含磁罗盘。这样的罗盘感测设备或一部分所述设备的关于地磁场的方向。这个朝向信息与其它传感器产生的数据相结合，与期望的恒值(contour)数据进行比较，从而控制所述设备。

虽然磁罗盘系统相对简易和可靠，但是传统的用于电子罗盘系统的标定技术很难实现且不能为安装于大型挖掘设备的电子罗盘提供非常精确的方法。这种设备的巨大铁含量或是增加或是扭曲地的磁场矢量，造成难以获得精确校准。此外，这种设备典型地包括电动机及其它电磁场源，其增加或减少地磁场的效应，更进一步造成朝向测量的误差。

图1示出这样一种类型的挖掘机10，其可包含用于通过感测地磁场并修正硬铁效应和软铁效应来确定朝向的系统。所述挖掘机包括机架11、在第一枢轴节14处枢轴地固定于机架11的主杆12、在第二枢轴节18处枢轴地固定于主杆12的铲斗柄16以及在第三枢轴节22处枢轴地固定于铲斗柄16的铲斗20。液压缸24、26和28被设置为分别移动主杆12、铲斗柄16和铲斗20。铲斗20包括刀刃30，其可具有刻齿刀片以帮助挖掘。机架11承载操作室31并由起落架支撑台和传送器32支撑，其可以包括履带，这便于挖掘机10在工地上的移动。机架11和它所承载的部件可以相对于起落架支撑台和传送器32围绕大体垂直的轴34旋转，以将铲斗20放置在挖掘所需要的精确位置。

对于要手动地或者通过自动化控制系统来控制挖掘机操作的操作者来说，必须确定关于挖掘机的位置和方向这两个信息。挖掘机的位置可以通过多种方法中的任意一种来确定，包括使用激光定位系统，使用GPS定位系统，使用将激光和GPS定位组合起来的系统，或使用机器人总站。可以通过角度编码器、基于重力的磁倾计或与枢轴节14、18和22相关联的类似传感器，或通过与汽缸24、26和28相关联的字符串编码器，或通过这种传感器的某些组合，确定主杆12、铲斗柄16和铲斗20的相对位置。挖掘机10相对于真垂线的方向由安装在机架11上的磁倾计36确定。磁倾计36在双轴中运行并且提供机架11的横摇角和俯仰角的指示。正如下面更完全的解释，所述系统还包括磁场传感器38，其提供传感器参考系的X、Y和Z方向上的地磁场大小的指示。此外，系统包括陀螺传感器40，其感测挖掘机围绕磁场传感器38的Z轴34的旋转。下面将说明的是，具有相关存储器52(图6)的处理器电路50对磁场传感器38、磁倾计36和陀螺传感器40进行响应，确定工地的挖掘机10的朝向。虽然显示为单独的部件，但是本领域技术人员可以想到的是磁倾计36和磁场传感器38可以安装在相同的外壳里。此外，所述磁倾计和陀螺传感器可以采取下列形式：IMU、惯性测量单元、或类似装置，其中，一些或所有磁场、重力和陀螺传感器被封装在一起，作为单个组件。此外，虽然下面描述的是磁倾计，但是可以使用其它等效的重力参考传感器。

挖掘机的朝向是机架11的纵轴所对齐的方向。这是由测定机架11相对于地磁场的相对方位的磁罗盘系统所确定的。通过考虑软铁效应和硬铁效应，所述磁罗盘系统增强精度。

为了补偿软铁效应，所述设备旋转通过完整循环来旋转。机架11的倾角被注释，连同在磁场传感器38的X、Y和Z方向上感测的磁场水平。陀螺仪40用来将所述设备的旋转分解变成小的角度增量，比如360等于一度增量。

在使用集成的陀螺信号来建立软铁标定表时遇到的困难是陀螺传感器的输出是略微非线性的。每个陀螺传感器都经过工厂标定，但是为了获得优异的结果，所述陀螺仪也必须在所述设备上进行重新标定。另外，传感器寿命、温度和旋转率的大小都促使需要在其使用寿命期间反复地重新标定设备上的陀螺传感器。当所述陀螺仪被恰当地重新调节时，它提供非常精确的角坐标的测量，允许编辑精确的标定表。已经发现磁场传感器的未补偿输出可以用来确定何时进行了完整旋转，因为当完成360度旋转时那些输出就恢复到相同水平。

接下来，基于特定等式提取磁性传感器上的硬铁效应，所述等式需要地磁矢量的本地倾斜角。为了确定地磁矢量的本地倾斜角，我们测量所述磁性传感器，以及当设备面向地磁北极时传感器的倾斜和摇摆。然后操作者将所述设备倾斜几度。获得一套新的磁性传感器读数和倾斜与摇摆读数。由此信息，本地倾斜角得以确定，如下所述。

就此，除去横向软铁效应，除去横向硬铁效应，并且除去纵向硬铁效。为了更进一步提高设备朝向的精度，除去纵向软铁效应。这是通过表征在标定过程的设备旋转部分期间的Z轴磁性传感器而实现的。

该系统可以用作挖掘机的朝向传感器。当设备在位置周围旋转时，所述系统连续不断地提供精确的朝向信息，其被用作用于精确控制的输入。当所述挖掘机被移到工地的不同位置时，为获得最大精度可重复所述再标定进程。

现在参考图2和3。定义正X轴为朝向的方向。当从负Z轴向地法平面上看时，是从地磁北极矢量到X轴来测量正朝向角(ρ)的。

Hn＝He·cos(φ)

Hx＝Hn·cos(ρ)

Hy＝Hn·sin(ρ)

1.Hx＝He·cos(φ)·cos(ρ)

2.Hy＝He·cos(φ)·sin(ρ)

3.Hz＝He·sin(φ)

$\left[\begin{array}{c}\mathrm{Hx}\\ \mathrm{Hy}\\ \mathrm{Hz}\end{array}\right]=\mathrm{He}\left[\begin{array}{c}\cos \mathrm{\φ}\cos \mathrm{\ρ}\\ \cos \mathrm{\φ}\sin \mathrm{\ρ}\\ \sin \mathrm{\φ}\end{array}\right]$

$M=\left[\begin{array}{c}\cos \mathrm{\φ\ρ}\cos \\ \cos \mathrm{\φ\ρ}\sin \\ \sin \mathrm{\φ}\end{array}\right]$

4.H_EN＝He·M

5.ρ＝-tan^-1(Hy/Hx)    Hx＞0，Hy＜＝0    0°＜＝ρ＜90°

6.ρ＝90°             Hx＝0，Hy＜0

7.ρ＝180-tan^-1(Hy/Hx) Hx＜0             90°＜ρ＜270°

8.ρ＝270              Hx＝0，Hy＞0

9.ρ＝360-tan^-1(Hy/Hx) Hx＞0，Hy＞0      270°＜ρ＜360°

接下来，我们导出朝向传感器平面与地面法线之间的旋转矩阵。现在参考图3和4。

摇摆(X)轴：

10.x′＝x

tanα＝z/y

cosα＝y/l

l＝y/cosα

sin(α-ψ)＝z′/l

cos(α-ψ)＝y′/l

z′＝l sin(α-ψ)＝(y/cosα)sin(α-ψ)

sin(α-ψ)＝sinαcos ψ-cosαsin ψ

z′＝y(tanαcos ψ-sin ψ)＝y[(z/y)cos ψ-sin ψ]

11.z′＝-y sin ψ+z cos ψ

y′＝l cos(α-ψ)＝(y/cosα)cos(α-ψ)

cos(α-ψ)＝cosαcos ψ+sinαsin ψ

y′＝y(cos ψ+tanαsin ψ)＝y[cos ψ+(z/y)sin ψ]

12.y′＝y cos ψ+z sin ψ

使用上面的等式10、11和12来定义摇摆中的旋转矩阵：

$R_{\mathrm{Roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ 0& -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]$

倾斜(Y)轴：

13.y′＝y

tanβ＝x/z

cosβ＝z/l

l＝z/cosβ

sin(β-μ)＝x′/l

cos(β-μ)＝z′/l

x′＝l sin(β-μ)＝(z/cos β)sin(β-μ)

sin(β-μ)＝sin βcos μ-cos βsin μ

x′＝z(tan βcos μ-sin μ)＝z[(x/z)cos μ-sin μ]

14.x′＝x cos μ-z sin μ

z′＝l cos(β-μ)＝(z/cos β)cos(β-μ)

cos(β-μ)＝cos βcos μ+sin βsin μ

z′＝z(cos  +tan βsin μ)＝z[cos μ+(x/z)sin μ]

15.z′＝x sin μ+z cos μ

使用上面的等式13、14和15来导出倾斜中的旋转矩阵：

$R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ 0& 1& 0\\ \sin \mathrm{\μ}& 0& \cos \mathrm{\μ}\end{array}\right]$

将倾斜旋转矩阵乘以摇摆矩阵以导出从地面法线到朝向传感器平面的完整循环矩阵。

R＝R_PitchR_Roll

$R=\left[\begin{array}{ccc}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ 0& 1& 0\\ \sin \mathrm{\μ}& 0& \cos \mathrm{\μ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ 0& -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]$

$R=\left[\begin{array}{ccc}\cos \mathrm{\μ}& \sin \mathrm{\μ}\sin \mathrm{\ψ}& -\sin \mathrm{\μ}\cos \mathrm{\ψ}\\ 0& \cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ \sin \mathrm{\μ}& -\cos \mathrm{\μ}\sin \mathrm{\ψ}& \cos \mathrm{\μ}\cos \mathrm{\ψ}\end{array}\right]$

要从朝向传感器平面旋转到地面法线使用R的转置。

$R^T=\left[\begin{array}{ccc}\cos \mathrm{\μ}& 0& \sin \mathrm{\μ}\\ \sin \mathrm{\μ}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& -\cos \mathrm{\μ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\μ}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& \cos \mathrm{\μ}\cos \mathrm{\ψ}\end{array}\right]$

H_EN＝R^TH_HS

$\left[\begin{array}{c}\mathrm{Hx}\\ \mathrm{Hy}\\ \mathrm{Hz}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\μ}& 0& \sin \mathrm{\μ}\\ \sin \mathrm{\μ}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& -\cos \mathrm{\μ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\μ}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& \cos \mathrm{\μ}\cos \mathrm{\ψ}\end{array}\right]\left[\begin{array}{c}{\mathrm{Hx}}^{\′}\\ H{y}^{\′}\\ {\mathrm{Hz}}^{\′}\end{array}\right]$

通过上述等式4到8，我们只需要地磁场的X和Y分量来计算朝向，因此使用下列等式：

16.Hx＝Hx’cos μ+Hz’sin μ

17.Hy＝Hx’sin μsin ψ+Hy’cosψ-Hz’cos μsin ψ

接下来，我们导出硬铁影响下朝向传感器平面中的地磁分量。

H_HS＝RH_EN

加上硬铁：

H_HSmeas＝RH_EN+H_HI

代替等式4：

18.H_HSmeas＝He·RM+H_HI

使用在水平和倾斜表面上获得的数据来除去硬铁：

H_HSmeasLVL-H_HSmeasSLP＝He·R_LVL M+H_HI-He·R_SLP M-H_HI

H_HSmeasLVL-H_HSmeasSLP＝He·[R_LVL M-R_SLP M]

19.H_HSmeasLVL-H_HSmeasSLP＝He·[R_LVL-R_SLP]M

展开等式19：

20.(H_HSmeasLVL-H_HSmeasSLP)x＝He[cosφcosρ(cosμ_L-cosμ_S)+cosφsinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)-sinφ(sinμ_L cosψ_L-sinμ_S cosψ_S)]

21.(H_HSmeasLVL-H_HSmeasSLP)y＝He[cosφsinρ(cosψ_L-cosψ_S)+sinφ(sinψ_L-sinψ_S)]

22.(H_HSmeasLVL-H_HSmeasSLP)z＝He[cosφcosρ(sinμ_L-sinμ_S)-cosφsinρ(cosμ_L sinψ_L-cosμ_Ssinψ_S)+sinφ(cosμ_L cosψ_L-cosμ_S cosψ_S)]

因为19的左侧由设备测量组成，因此我们写出：

K＝(H_HSmeasLVL-H_HSmeasSLP)x/(H_HSmeasLVL-H_HSmeasSLP)z

K＝He[cosφcosρ(cosμ_L-cosμ_S)+cosφsinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)-sinφ(sin_L cosψ_L-sinμ_S cosψ_S)]/He[cosφcosρ(sinμ_L-sinμ_S)-cosφsinρ(cosμ_L sinψ_L-cosμ_S sinψ_S)+sinφ(cosμ_L cosψ_L-cosμ_Scosψ_S)]

cosφcosρ(cosμ_L-cosμ_S)+cosφsinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)-sinφ(sinμ_L cosψ_L-sinμ_S cosψ_S)＝K cosφcosρ(sinμ_L -sinμ_S)-K cosφsinρ(cosμ_L sinψ_L-cosμ_S sinψ_S)+K sinφ(cosμ_L cosψ_L-cosμ_Scosψ_S)

除以cosφ：

cosρ(cosμ_L-cosμ_S)+sinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)-tanφ(sinμ_L cosω_L-sinμ_S cosψ_S)＝K cosρ(sinμ_L-sinμ_S)-K sinρ(cosμ_L sinψ_L-cosμ_S sinψ_S)+K tanφ(cosμ_L cosψ_L-cosμ_S cosψ_S)tanφ(sinμ_L cosω_L-sinμ_S cosψ_S)+K tanφ(cosμ_L cosω_L-cosμ_S cosψ_S)＝-K cosρ(sinμ_L-sinμ_S)+Ksinρ(cosμ_L sinψ_L-cosμ_S sinψ_S)+cosρ(cosμ_L-cosμ_S)+sinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)

23.φ＝tan^-1{[Ksinρ(cosμ_L sinψ_L-cosμ_S sinψ_S)-K cosρ(sinμ_L-sinμ_S)+cosρ(cosμ_L-cosμ_S)+sinρ(sinμ_L sinψ_L-sinμ_S sinψ_S)]/[(sinμ_L cosψ_L-sinμ_S  cosψ_S)+K(cosμ_L cosψ_L-cosμ_Scosψ_S)]}

接下来，通过针对水平和倾斜为H_HSmeasx展开等式18我们找到X轴方向上的硬铁分量：

H_HSmeasX＝He(cosφcosρcosμ+sinμsinψcosφsinρ-sinμcos ψsinφ)+H_HIX

H_{HSmeasX_L}＝He(cosφcosρcosμ_L+sinμ_L sinψ_L cosφsinρ-sinμ_L cos ψ_L sinφ)+H_HIX

H_{HSmeasX_S}＝He(cosφcosρcosμ_S+sinμ_S sinψ_S cosφsinρ-sinμ_S cos ψ_S sinφ)+H_HIX

求出He并设置等于：

(H_{HSmeasX_L}-H_HIX)/(cosφcosρcosμ_L+sinμ_L sinψ_L cosφsinρ-sinμ_L cos ψ_L sinφ)＝

(H_{HSmeasX_S}-H_HIX)/(cosφcosρcosμ_S+sinμ_S sinψ_S cosφsinρ-sinμ_S cos ψ_S sinφ)

Ratio＝(cosφcosρcosμ_L+sinμ_L sinψ_L cosφsinρ-sinμ_L cos ψ_L sinφ)/(cosφcosρcosμ_S+sinμ_S sinψ_S cosφsinρ-sinμ_S cos ψ_S sinφ)

H_{HSmeasX_L}-H_HIX＝(H_{HSmeasX_S}-H_HIX)Ratio

H_HIX-H_HIXRatio＝H_{HSmeasX_L}-H_{HSmeasX_S} Ratio

24.H_HIX＝(H_{HSmeasX_L}-H_{HSmeasX_S} Ratio)/(1-Ratio)

通过针对水平和倾斜为H_HSmeasZ展开等式18我们找到Z轴方向上的硬铁分量：

H_HSmeasZ＝He(cosφcosρsinμ-cosφsinρcosμsinψ+sinφcosμcosψ)+H_HIZ

H_{HSmeasZ_L}＝He(cosφcosρsinμ_L-cosφsinρcosμ_L sinψ_L+sinφcosμ_L cosψ_L)+H_HIZ

H_{HSmeasZ_S}＝He(cosφcosρsinμ_S-cosφsinρcosμ_S sinψ_S+sinφcosμ_S cosψ_S)+H_HIZ

求出He并设置等于：

(H_{HSmeasZ_L}-H_HIZ)/(cosφcosρsinμ_L-cosφsinρcosμ_L sinψ_L+sinφcosμ_L cosψ_L)＝

(H_{HSmeasZ_S}-H_HIZ)/(cosφcosρsinμ_S-cosφsinρcosμ_S sinψ_S+sinφcosμ_S cosψ_S)

Ratio＝(cosφcosρsinμ_L-cosφsinρcosμ_L sinψ_L+sinφcosμ_L cosψ_L)/(cosφcosρsinμ_S-cosφsinρcosμ_Ssinψ_S+sinφcosμ_S cosψ_S)

H_{HSmeasZ_L}-H_HIZ＝(H_{HSmeasZ_S}-H_HIZ)Ratio

H_HIZ-H_HIZRatio＝H_{HSmeasZ_L}-H_{HSmeasZ_S} Ratio

25.H_HIZ＝(H_{HSmeasZ_L}-H_{HSmeasZ_S} Ratio)/(1-Ratio)

接下来，通过收集设备围绕向北方向旋转ccw和cw的磁性数据，条件是地磁场强度是常数(对于标定所要求的时间并在设备改变方向时)，我们找到Y轴方向上的硬铁分量：

He²＝H_{HSmeasX_CCW}-H_HIX)²+(H_{HSmeasY_CCW}-H_HIY)²+(H_{HSmeasZ_CCW}-H_HIZ)²

He²＝H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasY_CW}-H_HIY)²+(H_{HSmeasZ_CW}-H_HIZ)²

(H_{HSmeasX_CCW}-H_HIX)²+(H_{HSmeasY_CCW}-H_HIY)²+(H_{HSmeasZ_CCW}-H_HIZ)²＝(H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasY_CW}-H_HIY)²+(H_{HSmeasZ_CW}-H_HIZ)²

(H_{HSmeasY_CCW}-H_HIY)²-(H_{HSmeasY_CW}-H_HIY)²＝(H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasZ_CW}-H_HIZ)²-(H_{HSmeasX_CCW}-H_HIX)²-(H_{HSmeasZ_CCW}-H_HIZ)²

H_{HSmeasY_CCW} ²-2H_{HSmeasY_CCW}H_HIY+H_HIY ²-(H_{HSmeasY_CW} ²-2H_{HSmeasY_CW} H_HIY+H_HIY ²)＝(H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasZ_CW}-H_HIZ)²-(H_{HSmeasX_CCW}-H_HIX)²-(H_{HSmeasZ_CCW}-H_HIZ)²

2H_{HSmeasY_CW} H_HIY-2H_{HSmeasY_CCW}-H_HIY＝(H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasZ_CW}-H_HIZ)²-(H_{HSmeasX_CCW}-H_HIX)²-(H_{HSmeasZ_CCW}-H_HIZ)²-H_{HSmeasY_CCW} ²+H_{HSmeasY_CW} ²

26.H_HIY＝[(H_{HSmeasX_CW}-H_HIX)²+(H_{HSmeasZ_CW}-H_HIZ)²-(H_{HSmeasX_CCW}-H_HIX)²-(H_{HSmeasZ_CCW}-H_HIZ)²-H_{HSmeasY_CCW} ²+H_{HSmeasY_CW} ²]/[2(H_{HSmeasY_CW}-H_{HSmeasY_CCW})]

接下来，我们转向软铁标定。为了除去任何由地磁场的失真引起的非线性，所述软铁标定是必要的。有许多已公开的方法来解决这个问题。最常用的、也是最节约的方法，是在水平面中最佳拟合单位圆。然而，由这种方法可获得的准确度有所局限。解决这个问题的最佳方法是将传感器旋转一个完整循环，并将计算的朝向与″实际源″即实际朝向的指示进行比较。根据实际源，可以创建查找表，以补偿所述失真。这个任务的主要难度在于标定工地的实际源。

本方法使用设备上的陀螺仪来确定在传感器的旋转期间得到的测量值的角定向。如果我们集成所述陀螺信号，我们导出在一个完整循环期间的等间隔角度增量的值的表格。由于非线性、温度和零点偏移漂移，这个方法的困难在于陀螺传感器输出具有误差。为了解决这些问题，所述方法在标定处理期间重新调节所述陀螺输出。单个磁力仪轴可以为所述传感器提供指示所述设备和传感器已经刚好旋转360度的参考点。然后，可调节比例系数，直到累计的陀螺仪信号在每次经过参考点时刚好是360度。为了解决所述零点偏移漂移问题，所述设备在标定开始时是静止的，以测量补偿。标定的旋转数据收集部分由以均匀转速旋转的机器设备实现。场值的漂移在这个短时间段中应当是最小的。

现在朝向实际源已经建立，系统内部装有软铁标定查找表。所述软铁算法示于图5的流程图。

由于旋转是围绕Z轴34的，因此实际信号位于罗盘面中。然而，所述软铁列表是在地法平面中计算的。为了在基本水平面上执行的标定，可忽略罗盘面和地法平面之间的差别。下面是概述如果实际信号没有被转换至地法平面，针对各个斜度，罗盘面和地法平面之间的最大误差的列表：

斜度	平面之间的最大误差
		1度	0.01度朝向误差
2度	0.02度朝向误差
		3度	0.04度朝向误差
10度	0.43度朝向误差r
		20度	1.78度朝向误差

将罗盘面实际信号转换到地法线，如下。首先，找出对于给定的初始倾斜和初始摇摆的最大/最小倾斜和旋转偏移。

Offset＝atan(-sin(InitialRoll)/sin(InitialPitch))；

PitchAmplitude＝-asin(sin(InitialRoll)*sin(Offset)-sin(InitialPitch)*cos(Offseet))；

在集成陀螺信号之后为给定的旋转偏移计算{X，Y，Z}坐标：

X＝-cos(PitchAmplitude)*sin(IntegratedAngle-Offset+270°)

Y＝sin(IntegratedAngle-Offset)

Z＝sin(PitchAmplitude)*sin(IntegratedAngle-Offset+270°)

(其中，Offset为偏移，InitialRoll为初始摇摆，InitialPitch为初始倾斜，PitchAmplitude为倾斜幅度，IntegratedAngle为积分角度)

在定义三个矢量之后，通过导出联立的方程组来确定这些等式。所述三个矢量是：i)沿着罗盘面的X轴(由初始倾斜确定)。称这个矢量为V1。ii)沿着罗盘面的Y轴(由初始摇摆确定)。称这个矢量为V2。iii)关于V1和V2的法线矢量。称这个矢量为N。

接下来，用矢量积法则来建立6组等式，以找出未知矢量，我们将称之为B。

V1xB＝len(V1)*len(B)*sin(IntegratedAngle)*N

V2xB＝len(V2)*len(B)*sin(IntegratedAngle+270°)*N

len(V1)＝len(V2)＝len(B)＝1

一旦找出X/Y坐标，我们可以轻易地发现地法线朝向：atan2(Y/X)。

再次参考图5，可以看出在70确定初始静态朝向。这基本上将设备在工地上固定于已知方向。接下来，所述设备以恒定转速旋转一个循环，如72所示。在74积分来自陀螺传感器的数据，并且磁性传感器的值在76被转换至地法线座标系统。这将继续下去，直到地法线旋转信号经过1度的阈值。然后，在78和80在1度阈值之前和1度阈值之后对值进行取样，并在82进行插值以为数据查找表中的下一点产生数据。这个处理将继续下去，直到来自完整循环的数据已被存储。

所述软铁标定适用于补偿地磁场的横向失真。然而，它不能补偿任何纵轴上的失真。通过实验人们发现，纵向失真可以通过表征经过一个完整循环的失真而得以补偿。如果我们假定磁性传感器的Z轴被设置为与设备旋转的Z轴34平行，则在Z轴磁力仪上收集的测量应该是常数。与此不符的是纵向软铁失真的测量。要表征并随后补偿之，计算通过一个完整旋转的Z轴测量的幅度和相移。

这些特性是朝向的函数。我们首先使用粗测量来计算粗略的朝向估计，通过特性方程来补偿Z轴测量，并接着再计算最终朝向测量。我们能这样做出于两个原因：i)所述软铁效应是正弦曲线，因此朝向中任何5-10度之间的所述效应将是相似的，和ii)Z轴精度并不像X/Y轴精度那样重要(尤其是有较小斜度)。

可以想到的是，在一些情况下，对于设备来说，可以将整个360度旋转作为建立软铁标定表的处理的一部分。例如，靠近设备的地面或建筑物可以防止这种移动。在此情况下，可以进行设备的部分旋转，而数据在部分旋转期间被收集。因为磁场传感器的未补偿输出在小于正在进行的完整旋转时不能用于重新调节或重新标定陀螺传感器，因此，可以想到以这种方式运行而获得的精度将比较低。

Claims (20)

1.一种使用安装在挖掘机上的磁场传感器，使用磁倾计，使用陀螺传感器以及使用处理器电路来确定工地的挖掘机的朝向的方法，所述磁场传感器提供所述磁场传感器的X、Y和Z方向上的地磁场大小的指示，所述磁倾计提供挖掘机的倾斜和摇摆的指示，所述陀螺传感器用于感测挖掘机围绕磁场传感器的Z轴的旋转，以及所述处理器电路具有相关联的存储器，响应于所述磁场传感器、所述磁倾计和所述陀螺传感器，所述方法包括：

创建并存储数据的标定表，包括：

基本上以恒速围绕大体纵向的Z轴旋转所述挖掘机，

存储来自所述磁场传感器和所述陀螺传感器的输出，

基于来自所述磁场传感器的数据值的重复来确定挖掘机的完整循环的开始和结束，并且

把在完整循环期间从所述陀螺传感器接收的数据和从所述磁场传感器接收的对应数据调节成大致相等的角度增量，并且存储该调节的数据，

用磁场传感器感测挖掘机处的地磁场，提供所述磁场传感器的X、Y和Z方向上地磁场大小的指示，

使用来自磁倾计的输出，将感测到的地磁场大小的指示从磁场传感器的X、Y和Z坐标参考系转换至本地水平面坐标参考系，

确定经转换的感测到的地磁场大小的指示的硬铁分量并除去该硬铁分量，以产生经过校正的在本地水平面坐标参考系中的地磁场大小的指示，并且

将经过校正的指示与存储的调节的数据进行比较，以确定挖掘机的朝向。

2.如权利要求1所述的方法，其中，创建并存储数据的标定表包括针对硬铁引起的误差补偿所存储的输出。

3.如权利要求1所述的方法，其中，把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量包括：把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成一度的增量。

4.如权利要求1所述的方法，其中，确定经转换的感测到的地磁场大小的指示的硬铁分量包括：

定位挖掘机以基本上朝向地磁北极，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

感测挖掘机的倾斜和摇摆，

将挖掘机倾斜几度，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，并且

感测挖掘机的倾斜和摇摆。

5.如权利要求1所述的方法，其中，创建并存储数据的标定表还包括通过将挖掘机旋转至预定参考朝向来固定所述挖掘机，并确定来自表示预定参考朝向的表的对应数据，从而数据的平衡表示挖掘机的朝向在角度增量上偏离预定参考朝向。

6.一种使用安装在挖掘机上的磁场传感器，使用重力参考传感器，使用陀螺传感器，并使用处理器电路来确定工地的挖掘机的朝向并补偿软铁误差的方法，所述磁场传感器提供在磁场传感器的X、Y和Z方向上的地磁场大小的指示，所述重力参考传感器提供挖掘机的倾斜和摇摆的指示，所述陀螺传感器用于感测挖掘机围绕磁场传感器的Z轴的旋转，以及所述处理器电路具有相关联的存储器，响应于所述磁场传感器、所述重力参考传感器和所述陀螺传感器，所述方法包括：

创建并存储数据的标定表，包括：

记录挖掘机的倾斜和摇摆，

基本上以恒速围绕基本上纵向的Z轴旋转所述挖掘机，通过完整循环，

存储来自所述磁场传感器、所述重力参考传感器和所述陀螺传感器的输出，

基于来自所述磁场传感器的数据值的重复来确定挖掘机的完整循环的开始和结束，并且

把在完整循环期间从所述陀螺传感器接收的数据和从所述磁场传感器接收的对应数据调节成大致相等的角度增量，并且存储该调节的数据，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

使用来自重力参考传感器的输出将感测到的地磁场大小的指示从磁场传感器的X、Y和Z坐标参考系转换至本地水平面坐标参考系，

确定经转换的感测到的地磁场大小的指示的硬铁分量并除去该硬铁分量，以产生经过校正的在本地水平面坐标参考系中的地磁场大小的指示，并且

将经过校正的指示与存储的调节的数据进行比较，以确定挖掘机的朝向。

7.如权利要求6所述的方法，其中，创建并存储数据的标定表包括针对硬铁引起的误差补偿所存储的输出。

8.如权利要求6所述的方法，其中，把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量包括：把在完整循环期间从陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成一度的增量。

9.如权利要求6所述的方法，其中，确定经转换的感测到的地磁场大小的指示的硬铁分量包括：

定位挖掘机以基本上朝向地磁北极，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

感测挖掘机的倾斜和摇摆，

将挖掘机倾斜几度，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，并且

感测挖掘机的倾斜和摇摆。

10.如权利要求6所述的方法，其中，创建并存储数据的标定表还包括：通过将挖掘机旋转至预定参考朝向来固定所述挖掘机，并确定来自表示预定参考朝向的表的对应数据，从而数据的平衡表示挖掘机的朝向在角度增量上偏离预定参考朝向。

11.一种使用安装在挖掘机上的磁场传感器，使用磁倾计，使用陀螺传感器，并使用处理器电路来确定工地的挖掘机的朝向的方法，所述磁场传感器提供在磁场传感器的X、Y和Z方向上的地磁场大小的指示，所述磁倾计提供挖掘机的倾斜和摇摆的指示，所述陀螺传感器用于感测挖掘机围绕磁场传感器的Z轴的旋转，以及处理器电路具有相关联的存储器，响应于所述磁场传感器、所述磁倾计和所述陀螺传感器，包括：

创建并存储数据的标定表，包括：

围绕大体上纵向的Z轴旋转所述挖掘机，小于完整循环，

存储来自所述磁场传感器和所述陀螺传感器的输出，并且

将在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量，并存储该调节的数据，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

使用来自磁倾计的输出，将感测到的地磁场大小的指示从磁场传感器的X、Y和Z坐标参考系转换至本地水平面坐标参考系，

确定经转换的感测到的地磁场大小的指示的硬铁分量并除去该硬铁分量以在本地水平面坐标参考系中产生经过校正的地磁场大小的指示，并且

将经过校正的指示与存储的调节的数据进行比较，以确定挖掘机的朝向。

12.如权利要求11所述的方法，其中，创建并存储数据的标定表包括针对硬铁引起的误差补偿所存储的输出。

13.如权利要求11所述的方法，其中，把在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量包括：把在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成相等的增量。

14.如权利要求11所述的方法，其中，确定经转换的感测到的地磁场大小的指示的硬铁分量包括：

定位挖掘机以基本上朝向地磁北极，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

感测挖掘机的倾斜和摇摆，

将挖掘机倾斜几度，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，并且

感测挖掘机的倾斜和摇摆。

15.如权利要求11所述的方法，其中，创建并存储数据的标定表还包括：通过将挖掘机旋转至预定参考朝向来固定所述挖掘机，并确定来自表示预定参考朝向的表的对应数据，从而数据的平衡表示挖掘机的朝向在角度增量上偏离预定参考朝向。

16.一种使用安装在挖掘机上的磁场传感器，使用重力参考传感器，使用陀螺传感器，并使用处理器电路来确定工地的挖掘机的朝向并补偿软铁误差的方法，所述磁场传感器提供磁场传感器的X、Y和Z方向上地磁场大小的指示，所述重力参考传感器提供挖掘机的倾斜和摇摆的指示，所述陀螺传感器用于感测挖掘机围绕磁场传感器的Z轴的旋转，以及所述处理器电路具有相关联的存储器，响应于所述磁场传感器，所述重力参考传感器和所述陀螺传感器，包括：

创建并存储数据的标定表，包括：

记录挖掘机的倾斜和摇摆，

围绕基本上纵向的Z轴旋转所述挖掘机，小于完整循环，

存储来自所述磁场传感器、所述重力参考传感器和所述陀螺传感器的输出，并且

把在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量，并存储该调节的数据，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

使用来自重力参考传感器的输出，将感测到的地磁场大小的指示从磁场传感器的X、Y和Z坐标参考系转换至本地水平面坐标参考系，

确定经转换的感测到的地磁场大小的指示的硬铁分量并除去该硬铁分量，以产生经过校正的在本地水平面坐标参考系中的地磁场大小的指示，并且

将经过校正的指示与存储的调节的数据进行比较，以确定挖掘机的朝向。

17.如权利要求16所述的方法，其中，创建并存储数据的标定表包括针对硬铁引起的误差补偿所存储的输出。

18.如权利要求16所述的方法，其中，把在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成大致相等的角度增量包括：把在小于完整循环期间从所述陀螺传感器接收的数据和从磁场传感器接收的对应数据调节成相等的增量。

19.如权利要求16所述的方法，其中，确定经转换的感测到的地磁场大小的指示的硬铁分量包括：

定位挖掘机以基本上朝向地磁北极，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，

感测挖掘机的倾斜和摇摆，

将挖掘机倾斜几度，

用磁场传感器感测挖掘机处的地磁场，提供磁场传感器的X、Y和Z方向上地磁场大小的指示，并且

感测挖掘机的倾斜和摇摆。

20.如权利要求16所述的方法，其中，创建并存储数据的标定表还包括：通过将挖掘机旋转至预定参考朝向来固定所述挖掘机，并确定来自表示预定参考朝向的表的对应数据，从而数据的平衡表示挖掘机的朝向在角度增量上偏离预定参考朝向。