CN1651862A - 运动物体运动估计方法和系统 - Google Patents

Abstract

提供了一种运动物体运动估计方法和系统。该方法包括：从运动物体的罗盘信息中获得磁场信息；比较运动物体的磁场和预定值，并根据比较结果确定运动物体的位置是否属于特定区域；通过根据确定结果确定是否使用罗盘方位角估计运动物体的方位，来估计运动物体的方向。该系统中装有陀螺仪、里程表和罗盘，包括：磁场计算器，计算装有罗盘的运动物体的磁场强度；磁场比较器，获得磁场强度与地磁场强度之间的差值，并比较该差值与第一阈值；地磁场区域确定器，根据比较结果确定运动物体所在位置是否属于地磁磁力起作用的区域；以及运动方向估计器，通过根据确定结果确定是否用罗盘方位角进行运动物体的方向估计，来估计运动物体的运动方向。

Description

运动物体运动估计方法和系统

技术领域

本发明涉及对运动物体的运动估计，更具体地涉及使用罗盘数据的运动物体运动估计方法和系统。

背景技术

通过使用绝对传感器或相对传感器来估计运动物体的位置和方位可以获得运动物体的姿态估计。通过估计运动物体的位置(x，y)和方向θ可以获得如图1所示对二维平面上运动的运动物体的姿态估计。在这种情况下，所述绝对传感器可以测量运动物体的绝对位置或方位而不是其相对运动。可以使用摄像机、激光扫描仪、声纳传感器、全球定位系统(GPS)或磁罗盘作为绝对传感器。另一方面，所述相对传感器可以通过测量和积分运动物体的运动的相对增或减值来测量位置或方位。可以使用陀螺仪、加速计和里程表(附在发送机上的编码器)作为相对传感器。

绝对传感器的一些特征如下：摄像机对周围环境的光线状态敏感，并且具有较高的输出不可靠数据的可能性；激光扫描仪可以输出可靠的数据，但是非常昂贵，并且如果存在许多障碍物，那么很难使用激光扫描仪测量位置和方位；声纳具有较低的数据精确性；GPS只可在室外使用，并且具有较低的精确度；罗盘具有较高的受户外存在的磁场干扰影响的可能性。另一方面，由于相对传感器只测量可变值，因此通过积分该可变值不可避免地会产生积分误差，并且不能避免陀螺仪和加速计的漂移误差特性。在本发明的实施例中，通过使用罗盘作为绝对传感器并使用陀螺仪和里程表作为相对传感器的传感器合成(fusion)来降低上述的缺点。

传统的航位推测法姿态估计方法分为只使用里程表的方法、使用陀螺仪和里程表的方法以及使用罗盘和里程表的方法。只使用里程表的方法是最简单的一种。但是，只使用里程表的方法不能处理滑动误差、碰撞或胁持(kidnap)。另外，由于在该方法中误差连续累积，因此存在不受限制的误差。

为了解决上述的问题，已经研发了一种通过使用陀螺仪和里程表执行航位推测法的方法。但是，即使该方法与只使用里程表即编码器的方法相比，可以获得更精确的结果，但是由于编码器和陀螺仪都是相对传感器，因此不能在长时期内仍然保证有界性。还已经研发了一种通过引入作为绝对传感器的罗盘来代替陀螺仪从而在长时期内稳定地检测运动方向的方法。但是，由于这种方法容易受到在家庭或办公室环境中总是存在的磁场干扰的影响，因此很难实际使用该方法。

最近，已经提出了一种同时使用陀螺仪、罗盘和里程表的方法。由于这种方法同时使用陀螺仪、罗盘，因此陀螺仪和罗盘之间互助是可能的。但是，因为当前开发的大多数方法使用现有的卡尔曼(Kalman)滤波，这种滤波是基于统计的传感器合成方法，因此存在源于卡尔曼滤波固有缺点的各种限制。也就是，由于对于卡尔曼滤波的应用，系统噪声和测量噪声必须具有高斯白噪声特性，因此很难处理没有表现出高斯白噪声特性的各种情况，例如磁场、障碍物、光滑地面。此外，需要所有种类的传感器模型的精确信息，并且还需要假定误差源彼此不相关。但是，实际上，不可能实现满足上述要求的方法和系统。另外，由于传感器的性能对于传感器合成很重要，在这种方法中大多使用昂贵的传感器。

发明内容

本发明提供了一种运动物体运动估计方法和系统，该方法和系统使用传感器合成，通过精确并且稳健(robustly)地估计运动物体的方位来计算运动物体的姿态信息。

本发明还提供了一种计算机可读介质，该介质上有执行运动物体运动估计方法的计算机可读程序。

根据本发明的一个方面，提供了一种运动物体的运动估计方法，该方法包括：从附在该运动物体上的磁罗盘获得磁场信息；比较该运动物体的磁场和预定值，并且根据比较结果确定该运动物体的位置是否属于特定区域；以及通过根据该确定结果来确定是否使用罗盘方位角估计该运动物体的方位，来估计该运动物体的运动方向。

这里，预定值可以是地磁场强度，并且该特定区域可以是地球磁场起主要作用的区域。对该运动物体的位置是否属于特定区域的确定可以包括：如果该运动物体中装有两个罗盘，那么将第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个与地磁场强度相比较，把比较结果分为两个差值都小于第一阈值的情况、两个差值中只有一个小于第一阈值的情况以及两个差值都不小于第一阈值的情况，并且根据该比较结果确定该运动物体的位置是否属于该特定区域。

对该运动物体的运动方向的估计可以包括：如果按照该运动物体属于特定区域确定该罗盘信息是有效的，那么获得最终罗盘方位角，并且使用该最终罗盘方位角作为测量输入用卡尔曼滤波来估计航向改变角(heading angle)；而如果按照该运动物体不属于特定区域而确定该罗盘信息无效，那么比较陀螺仪的角速度和里程表的角速率，而如果该差值小于第二阈值，那么使用由该里程表获得的运动方向作为测量输入用卡尔曼滤波来估计航向改变角，并且如果该差值不小于第二阈值，那么通过积分陀螺仪的角速度来估计运动方向。

在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个都与地磁场强度相比，并且两个差值都不小于第一阈值的情况下，对运动物体的运动方向的估计可以包括：获得第一罗盘的方位角和该第二罗盘的方位角之间的差值；如果方位角之间的差值小于第三阈值时，那么通过根据这两个罗盘的磁场强度和地磁场强度之间的差值而改变权重，来获得该运动物体的罗盘方位角；而如果方位角之间的差值不小于该第三阈值，那么获得关于该运动物体的陀螺仪角速度以及关于该罗盘方位角的每个角速度之间的差值，并且根据该差值的大小获得该运动物体的罗盘方位角。

通过改变权重获得该运动物体的罗盘方位角可以包括：当Δθ_c1表示在采样周期中第一罗盘的方位角改变多少的量，Δθ_c2表示在采样周期中第二罗盘的方位角改变多少的量，ω_g表示陀螺仪的方位角，Δt表示采样时间，ΔH_1E表示第一罗盘的磁场强度和地磁场强度之间的差值，而ΔH_2E表示第二罗盘的磁场强度和地磁场强度之间的差值时，如果ΔH_1E乘ΔH_2E获得的值是负数，那么确定通过计算公式3获得的值为最终罗盘方位角θ_c；如果ΔH_1E乘ΔH_2E获得的值是正数，那么确定通过计算公式4获得的值为最终罗盘方位角θ_c；而如果通过ΔH_1E乘ΔH_2E获得的值是0，那么确定通过计算公式5获得的值为最终罗盘方位角θ_c。通过获得角速度之间的差值获得该运动物体的罗盘方位角可以包括：当Δθ_c1表示在采样周期中第一罗盘的方位角改变多少的量，Δθ_c2表示在采样周期中第二罗盘的方位角改变多少的量，ω_g表示陀螺仪的方位角，而Δt表示采样时间时，如果该第一罗盘的方位角θ_c1和该第二罗盘的方位角θ_c2之间的差值不小于该第三阈值，那么检查是否满足公式6；如果满足公式6，那么确定第一罗盘的方位角θ_c1为最终罗盘方位角θ_c；如果不满足公式6但满足公式7，那么确定第二罗盘的方位角θ_c2为最终罗盘方位角θ_c；而如果公式6和公式7每一个的两边都是相同的，那么确定通过计算公式5获得的值为最终罗盘方位角，即该运动物体的运动方向，θ_c。

[公式3]

${\mathrm{\θ}}_c\←\frac{{\mathrm{\θ}}_{c1}\left|\mathrm{\Δ}{H}_{2E}\right|+{\mathrm{\θ}}_{c2}\left|\mathrm{\Δ}{H}_{1E}\right|}{\left|\mathrm{\Δ}{H}_{1E}\right|+\left|\mathrm{\Δ}{H}_{2E}\right|{}_{}}$

[公式4]

${\mathrm{\θ}}_c\←\frac{{\mathrm{\θ}}_{c1}\mathrm{\Δ}{H}_{2E}-{\mathrm{\θ}}_{c2}\mathrm{\Δ}{H}_{1E}}{\mathrm{\Δ}{H}_{1E}-\mathrm{\Δ}{H}_{2E}}$

[公式5]

${\mathrm{\θ}}_c\←\frac{{\mathrm{\θ}}_{c1}+{\mathrm{\θ}}_{c2}}{2}$

[公式6]

$|\frac{\mathrm{\&Delta;}{\mathrm{\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{\mathrm{\&Delta;}{\mathrm{\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|$

[公式7]

$|\frac{\mathrm{\&Delta;}{\mathrm{\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|$

在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个都与地磁场强度相比较，并且这两个差值中只有一个小于第一阈值的情况下，对运动物体的运动方向估计可以包括：确定差值小于第一阈值的罗盘的方位角为最终罗盘方位角。

在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个都与地磁场强度相比较，并且这两个差值都不小于该第一阈值的情况下，对运动物体的运动方向估计可以包括：在不使用罗盘信息的情况下，计算关于该运动物体的转轮速度的角速度(也就是，由里程表获得的角速度)；获得该运动物体的陀螺仪角速度和转轮角速度之间的差值；如果该差值小于第二阈值，那么使用由里程表获得的方向作为测量输入用卡尔曼滤波来估计最佳运动方向角；而如果该差值不小于第二阈值，那么通过积分该陀螺仪的角速度而不使用卡尔曼滤波来估计该运动方向角。通过积分该陀螺仪的角速度来估计该运动方向角可以还包括：如果在预定时间内该估计被执行的次数多于第二阈值，那么终止该操作。

该方法还可以包括：通过使用反馈误差状态的卡尔曼滤波来对该运动物体的运动方向角滤波，计算该运动物体的最佳运动方向估计值。

根据本发明的另一方面，提供了一种运动物体的运动估计系统，该运动物体中装有陀螺仪、里程表和罗盘，该系统包括：磁场计算器，计算装有罗盘的运动物体的磁场强度；磁场比较器，获得该磁场强度与地磁场强度之间的差值，并且比较该差值与第一阈值；地磁场区域确定器，根据比较结果确定该运动物体的位置是否属于地磁磁力起作用的区域；以及运动方向估计器，通过根据该确定结果确定是否使用罗盘方位角进行运动物体方向估计，来估计该运动物体的运动方向。

根据本发明的另一方面，提供了一种计算机可读介质，该介质上记录有用于执行上述方法的计算机可读程序。

附图说明

通过参照附图详细描述本发明的示范性实施例，本发明的上面和其他特点和优点将变得更加显而易见，附图中：

图1表示要估计位置(x，y)和方向θ的二维平面上运动的运动物体；

图2是根据本发明实施例的装在运动物体内用于估计该运动物体的运动方向的传感器合成系统的概念图；

图3是根据本发明实施例的运动物体的运动估计系统的框图；

图4是根据本发明实施例的运动物体的运动估计方法的流程图；

图5说明从罗盘信息获得磁场强度的实例；

图6是说明第一罗盘和第二罗盘获得的H₁和H₂是否属于S_H的流程图；

图7是使用加权划分方法计算运动物体的运动方向的过程的流程图；

图8是在图4中获得的罗盘磁场强度和地磁场强度之间的两个差值不都小于第一阈值的情况下确定运动物体的运动方向的过程的流程图；

图9是通过检查陀螺仪和里程表的角速度来估计运动物体的运动方向的过程的流程图；

图10表示运动物体在二维平面上的位置；以及

图11表示误差状态测量模型。

具体实施方式

下面将参照附图更全面地描述本发明，附图中示出了本发明的实施例。

可以使用加速计和陀螺仪来精确计算运动物体的横摇(roll)和纵摇(pitch)。但是，已知很难获得运动物体的偏航(yaw)。在本发明的实施例中，使用新传感器合成系统来估计运动物体的运动方向，该系统可以通过精确而稳健(robustly)地估计运动物体的运动方向来计算运动物体的姿态信息。也就是，通过结合绝对传感器和相对传感器，如罗盘、陀螺仪和里程表，有效地处理诸如磁场、障碍物、光滑地面的各种不确定环境，并且获得运动物体的偏航角，来最佳地估计运动物体的位置和方位。

图2是根据本发明的实施例的装在运动物体内用于估计运动物体的运动方向的传感器合成系统的概念图。

参照图2，运动物体200包括两个罗盘210和220、陀螺仪230，以及两个转轮编码器(里程表)240和250。

传感器具有以下特性。陀螺仪230在长期的计算偏移误差和角度值所需的积分处理中会产生积分误差。但是，陀螺仪230可以在短期内检测相对精确的角度值。因为罗盘210和220可以在长期内提供绝对方位角，因此使用罗盘210和220可以获得精确的信息。但是，由于在短期内在生活环境中存在磁场干扰，因此罗盘210和220会产生误差。另一方面，因为滑动而产生的误差和由于运动学条件(如转轮大小以及是否执行校准)而产生的误差被累积，因此装在运动物体的主动件(在运动物体的情况下是转轮)内的里程表240和250在长期内表现出不受限制的误差特性。里程表240和250还具有这样的局限性，就是短期内不能处理诸如碰撞和胁持这样的意外情况。但是，里程表240和250在大部分时间内可以提供相对精确的信息。

从罗盘210和220可以获得磁场的方向和强度。使用陀螺仪230可以检测角速度。通过使用里程表240和250检测转轮的旋转速度，可以获得运动物体的角速度。

在本发明的实施例中，通过结合绝对传感器和相对传感器，如罗盘、陀螺仪和里程表，有效地处理诸如磁场、障碍物、和光滑地面的各种不确定环境，并且获得运动物体的偏航角，从而使用基于规则的间接卡尔曼滤波来最佳地估计运动物体的位置和方位。

图3是根据本发明的实施例用于运动物体的运动估计系统的框图。

参照图3，运动物体的运动估计系统包括磁场计算器300、磁场比较器310、地磁区域确定器320、和运动方向估计器330。在该运动物体中装有陀螺仪、里程表和罗盘。

该运动物体从罗盘、陀螺仪和里程表获得传感器数据。从罗盘信息可以获得磁场强度。磁场计算器300计算装有罗盘的运动物体的磁场强度。磁场比较器310获得磁场强度和地磁场强度之间的差值，并且比较该差值与第一阈值ε_H。该地磁区域确定器320根据比较结果而确定该运动物体的位置是否属于地磁磁力起作用的区域。该运动方向估计器330通过根据确定结果确定是否使用用于该运动物体的方向估计的罗盘方位角，来估计该运动物体的运动方向。作为从这两个罗盘获得的磁场强度与地磁场强度之间的差值的、通过该磁场比较器310获得的比较结果，如果该差值的至少小于该第一阈值ε_H，那么用从该罗盘数据获得的方位角来估计该运动物体的运动方向。否则，用该陀螺仪和里程表的角速度来估计该运动物体的运动方向。现在参照附图更详细地描述对该运动物体的运动估计。

图4是根据本发明的实施例的运动物体的运动估计方法的流程图。

在操作400中读取罗盘、陀螺仪和里程表的传感器数据。首先，从传感器数据的罗盘数据中获得每个磁场的强度。图5图解了从罗盘信息中获得磁场强度的实例。参照图5，磁场强度|H|可以从x轴的分量磁场H_x和y轴的分量磁场H_y中获得，这两个分量磁场可以从每个罗盘中获得。现在，从每个罗盘获得的每个磁场强度和地磁场强度之间的差值如公式1所示来定义，这里ΔH_iE表示该差值，i表示罗盘的标识，H_E表示地磁场。

[公式1]

ΔH_iE≡|H_i|-|H_E|

定义公式2。

[公式2]

如果|ΔH_iE|＜ε_H，那么H_i∈S_H

其中，ε_H表示阈值，而S_H表示成功区域，也就是地磁场起作用的区域。

在定义公式1和2之后，在操作600中如图6所示，从第一罗盘和第二罗盘中获得H₁和H₂，并且在操作410中检查H₁和H₂是否属于S_H。

该第一罗盘的磁场强度|H₁|和该第二罗盘的磁场强度|H₂|中的每一个都与地磁场强度|H_E|相比较，并且如果在操作610中这两个差值都小于该第一阈值ε_H，那么在操作420中使用如图7所示的加权划分方法来计算最终罗盘方位角。在操作700中获得该第一罗盘的方位角θ_c1和该第二罗盘的方位角θ_c2之间的差值。如果该方位角差值小于第三阈值ε_c，那么通过根据罗盘的磁场强度和地磁场强度之间的差值而改变权重，以获得该运动物体的罗盘方位角。也就是，如果在操作710中通过ΔH_1E乘以ΔH_2E获得的值是负数，那么在操作720中确定通过公式3获得的值为最终罗盘方位角θ_c。

[公式3]

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}\left|{\mathrm{\&Delta;H}}_{2E}\right|+{\mathrm{\&theta;}}_{c2}\left|{\mathrm{\&Delta;H}}_{1E}\right|}{\left|{\mathrm{\&Delta;H}}_{1E}\right|+\left|{\mathrm{\&Delta;H}}_{2E}\right|}$

在操作730中如果在操作610中通过ΔH_1E和ΔH_2E相乘获得的值是正数，那么在操作740中确定通过公式4获得的值为最终罗盘方位角θ_c。

[公式4]

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}{\mathrm{\&Delta;H}}_{2E}-{\mathrm{\&theta;}}_{c2}{\mathrm{\&Delta;H}}_{1E}}{{\mathrm{\&Delta;H}}_{1E}-{\mathrm{\&Delta;H}}_{2E}}$

如果在操作610中通过ΔH_1E和ΔH_2E相乘获得的值是0，那么在操作750中确定通过公式5获得的值为最终罗盘方位角θ_c。

[公式5]

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}+{\mathrm{\&theta;}}_{c2}}{2}$

另一方面，在操作700中如果该第一罗盘的方位角θ_c1和该第二罗盘的方位角θ_c2之间的差值不小于该第三阈值ε_c，那么在操作760中检查是否满足公式6。

[公式6]

$|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|$

如果在操作760中满足公式6，那么在操作770中确定该第一罗盘的方位角θ_c1为最终罗盘方位角θ_c。如果在操作760中不满足公式6，而在操作780中满足公式7，那么在操作790中把该第二罗盘的方位角θ_c2确定为最终罗盘方位角θ_c。

[公式7]

$|\frac{{\mathrm{\&Delta;\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|$

如果在操作760和780中公式6和7的两边都相同，那么如在操作750中一样，确定通过公式5获得的值为最终罗盘方位角θ_c。

在公式3到7中，Δθ_c1表示对于采样周期该第一罗盘的方位角改变多少的量，Δθ_c2表示对于该采样周期该第二罗盘的方位角改变多少的量，ω_g表示该陀螺仪的角速度，Δt表示采样时间。

下面参照图6和8详细描述图4的操作450。在图6中，该第一罗盘的磁场强度|H₁|和该第二罗盘的磁场强度|H₂|中的每一个与地磁场强度|H_E|相比较，在操作620中，该第一罗盘的磁场强度|H₁|和地磁场强度|H_E|之间的差值小于第一阈值ε_H，而该第二罗盘的磁场强度|H₂|和地磁场强度|H_E|之间的差值不小于第一阈值ε_H的情况下，如图8中所示，如在操作800中一样确定第一罗盘的方位角Δθ_c1为最终罗盘方位角θ_c。

同样，在图6中，在该第一罗盘的磁场强度|H₁|和该第二罗盘的磁场强度|H₂|中的每一个与该地磁场强度|H_E|相比较，在操作630中，该第一罗盘的磁场强度|H₁|和地磁场强度|H_E|之间的差值不小于第一阈值ε_H，而该第二罗盘的磁场强度|H₂|和地磁场强度|H_E|之间的差值小于第一阈值ε_H的情况下，如在操作810中一样确定第二罗盘的方位角Δθ_c2为最终的罗盘方位角θ_c。

在操作430中使用从上述的方法中获得的最终罗盘方位角Δθ_c2驱动误差状态反馈卡尔曼滤波，并且在操作440中使用该误差状态反馈滤波器获得最佳估计的运动方向。

另一方面，在图6中，在该第一罗盘的磁场强度|H₁|和该第二罗盘的磁场强度|H₂|中的每一个与该地磁场的强度|H_E|相比较，并且在操作410和630中，这两个差值都不小于第一阈值ε_H的情况下，不使用罗盘数据，并且在操作460和820中检查陀螺仪角速度和里程表角速度之间的差值。

现在参照图9详细描述图4的操作460到495。图9是通过检查陀螺仪和里程表的角速度来估计该运动物体的运动方向的过程的流程图。首先，在操作900中如公式8中所示从里程表获得里程表角速度ω_o。

[公式8]

${\mathrm{\&omega;}}_0=\frac{(v_r-v_1)}{l}$

其中，v_r和v_l表示转轮速度，l表示轮距长度，即两个转轮之间的距离。

在操作910中，检查该陀螺仪角速度ω_g和里程表角速度ω_o之间的差值是否小于第二阈值ε_ω，并且如果该差值小于该第二阈值ε_ω，那么在操作920中使用该里程表数据驱动该误差状态反馈卡尔曼滤波，并且在操作930中使用该误差状态反馈卡尔曼滤波获得最佳估计的运动方向。

同样，如果作为操作910的检查结果，该差值不小于该第二阈值ε_ω，那么不使用卡尔曼滤波，并且在操作940中积分陀螺仪角速度，并且在操作950中计算次佳的运动方向。在操作960中增加失败指标值，并且在操作970中如果该失败指标值超过第四阈值，那么在操作980中发出警告信号并且停止系统操作。在操作970中如果该失败指标值等于或小于该第四阈值，那么读取罗盘、陀螺仪和里程表的传感器数据。

如下执行对应于运动物体实例的里程表和运动物体的建模。

图10表示运动物体在二维平面上的位置。当t_s表示采样时间时，如公式9所示对该运动物体建模。

[公式9]

$\left(\begin{array}{c}x\left(t_{k+1}\right)\\ y\left(t_{k+1}\right)\\ \mathrm{\&theta;}\left(t_{k+1}\right)\end{array}\right)=\left(\begin{array}{c}x\left(t_k\right)\\ y\left(t_k\right)\\ \mathrm{\&theta;}\left(t_k\right)\end{array}\right)+\left(\begin{array}{c}\frac{(v_r\left(t_k\right)+v_1\left(t_k\right))}{2}\&CenterDot;\mathrm{\&Delta;t}\&CenterDot;\cos \mathrm{\&theta;}\left(t_k\right)\\ \frac{(v_r\left(t_k\right)+v_1\left(t_k\right))}{2}\&CenterDot;\mathrm{\&Delta;t}\&CenterDot;\sin \mathrm{\&theta;}\left(t_k\right)\\ \frac{(v_r\left(t_k\right)-v_1\left(t_k\right))}{l}\&CenterDot;\mathrm{\&Delta;t}\end{array}\right)$

同样，当D_l，r表示转轮的直径，C_e表示编码器的分辨率(即转轮转一周的脉冲数)，n表示齿轮齿数比，N_l，r表示在采样周期内脉冲的数量时，如公式10所示对里程表建模。

[公式10]

$v_{1,r}=\frac{{\mathrm{\&pi;D}}_{1,r}}{n{C}_e}\&CenterDot;\frac{N_{1,r}}{t_s}$

相应地，如公式11中所示表示里程表角速度。

[公式11]

${\mathrm{\&omega;}}_0\left(t_k\right)=\frac{(v_r\left(t_k\right)-v_1\left(t_k\right))}{l}$

如下执行对陀螺仪的建模。首先，当θ表示运动物体的运动方向，b表示陀螺仪漂移率，ω_g表示陀螺仪输出，n_θ表示具有σ_θ分布的高斯白噪声，而n_b表示具有σ_b分布的高斯白噪声时，如公式12中所示表示陀螺仪实际模型。

[公式12]

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}={\mathrm{\&omega;}}_g+b+n_g\\ \stackrel{\&CenterDot;}{b}=n_b\end{array}\right.\&DoubleRightArrow;\left[\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \stackrel{\&CenterDot;}{b}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&theta;}\\ b\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&omega;}}_g\\ 0\end{array}\right]+\left[\begin{array}{c}{n}_{\mathrm{\&theta;}}\\ n_b\end{array}\right]$

同样，当

表示所估计的运动方向，而 表示估计的陀螺仪漂移率(假定

是常数)时，可以如公式13中所示表示陀螺仪估计模型即积分器的输出。

[公式13]

$\left[\begin{array}{c}\stackrel{\stackrel{\&CenterDot;}{~}}{\mathrm{\&theta;}}\\ \stackrel{\stackrel{\&CenterDot;}{~}}{b}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\widetilde{\mathrm{\&theta;}}\\ \tilde{b}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&omega;}}_g\\ 0\end{array}\right]$

如下执行对误差状态的建模。首先，如公式14所示定义误差状态。

[公式14]

$\mathrm{\&delta;\&theta;}\left(t\right)=\mathrm{\&theta;}\left(t\right)-\widetilde{\mathrm{\&theta;}}\left(t\right)$

$\mathrm{\&delta;b}\left(t\right)=b\left(t\right)-\tilde{b}\left(t\right)$

当θ_m表示测量的运动方向，而v表示具有σ_v分布的高斯白噪声时，可以如公式15中所示表示外部传感器测量模型。这里，根据上面描述的方法选择罗盘最终方位角和里程表角速度中的一个作为外部传感器测量输入。

[公式15]

θ_m(t)＝θ(t)+v(t)

$z\left(t\right)={\mathrm{\&theta;}}_m\left(t\right)-\widetilde{\mathrm{\&theta;}}\left(t\right)=(\mathrm{\&theta;}\left(t\right)+v\left(t\right))-(\mathrm{\&theta;}\left(t\right)-\mathrm{\&delta;\&theta;}\left(t\right))=\mathrm{\&delta;\&theta;}\left(t\right)+v\left(t\right)=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;\&theta;}\left(t\right)\\ \mathrm{\&delta;b}\left(t\right)\end{array}\right]+v\left(t\right)$

$\&DoubleRightArrow;z\left(t\right)=\mathrm{Hx}\left(t\right)+v\left(t\right)$

通过从公式13中减去公式12，如公式16所示获得误差状态模型。

[公式16]

图11表示误差状态测量模型。

上面的模型可以表示为如下的离散模型。可以如公式17中所示定义离散误差状态。

[公式17]

$\mathrm{\&delta;\&theta;}\left(t_k\right)\&equiv;\mathrm{\&theta;}\left(t_k\right)-\widetilde{\mathrm{\&theta;}}\left(t_k\right),\mathrm{\&delta;b}\left(t_k\right)\&equiv;b\left(t_k\right)-\tilde{b}\left(t_k\right)$

可以如公式18所示表示公式12的离散形式。

[公式18]

$\left[\begin{array}{c}\mathrm{\&theta;}\left(t_{k+1}\right)\\ b\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&theta;}\left(t_k\right)\\ b\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;t}\&CenterDot;{\mathrm{\&omega;}}_g\left(t_k\right)\\ 0\end{array}\right]+\left[\begin{array}{c}{n}_{\mathrm{\&theta;}}\left(t_k\right)\\ n_b\left(t_k\right)\end{array}\right],\mathrm{\&Delta;t}\&equiv;t_{k+1}-t_k$

可以如公式19所示表示公式13的离散形式。

[公式19]

$\left[\begin{array}{c}\widetilde{\mathrm{\&theta;}}\left(t_{k+1}\right)\\ \tilde{b}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\widetilde{\mathrm{\&theta;}}\left(t_k\right)\\ \tilde{b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;t}\&CenterDot;{\mathrm{\&omega;}}_g\left(t_k\right)\\ 0\end{array}\right]$

可以从公式18和19导出公式20。

[公式20]

$\left[\begin{array}{c}\mathrm{\&delta;\&theta;}\left(t_{k+1}\right)\\ \mathrm{\&delta;b}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;\&theta;}\left(t_k\right)\\ \mathrm{\&delta;b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}{n}_{\mathrm{\&theta;}}\left(t_k\right)\\ n_b\left(t_k\right)\end{array}\right]\&DoubleRightArrow;x\left(t_{k+1}\right)=\mathrm{Ax}\left(t_k\right)+n\left(t_k\right)$

可以如公式21所示表示公式15的离散形式。

[公式21]

$z\left(t_k\right)={\mathrm{\&theta;}}_m\left(t_k\right)-\widetilde{\mathrm{\&theta;}}\left(t_k\right)=\mathrm{\&delta;\&theta;}\left(t_k\right)+v\left(t_k\right)$

$=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;\&theta;}\left(t_k\right)\\ \mathrm{\&delta;b}\left(t_k\right)\end{array}\right]+v\left(t_k\right)---\&DoubleRightArrow;z\left(t_k\right)=\mathrm{Hx}\left(t_k\right)+v\left(t_k\right)$

如下详细描述离散误差状态卡尔曼滤波。可以如公式22和23表示初始条件。公式22表示高斯随机变量(RV)，其均值基本为0，公式23表示初始值为θ和b的分布。

[公式22]

x(t₀)＝[0  0]^T

[公式23]

$P\left(t_0\right)=\left[\begin{array}{cc}{\mathrm{\&sigma;}}_{\mathrm{\&theta;}}^2& 0\\ 0& {\mathrm{\&sigma;}}_b^2\end{array}\right]$

可以如公式24中所示表示高斯噪声的离散形式。

[公式24]

$Q=\left[\begin{array}{cc}{\mathrm{\&sigma;}}_{n_{\mathrm{\&theta;}}}^2& 0\\ 0& {\mathrm{\&sigma;}}_{n_b}^2\end{array}\right]\&DoubleLeftArrow;E\left\{\left[\begin{array}{c}{n}_{\mathrm{\&theta;}}\left(t_i\right)\\ n_b\left(t_i\right)\end{array}\right]\left[\begin{array}{cc}{n}_{\mathrm{\&theta;}}\left(t_j\right)& n_b\left(t_j\right)\end{array}\right]\right\}=Q\left(t_i\right)\mathrm{\&delta;}(t_i-t_j)$

$R={\mathrm{\&sigma;}}_v^2$

可以如公式25和26中所示表示根据瞬时更新的离散形式。

[公式25]

$\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right)\\ \mathrm{\&delta;}\hat{b}\left(t_{k+1}^{-}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_k\right)\\ \mathrm{\&delta;}\hat{b}\left(t_k\right)\end{array}\right]$

[公式26]

$P\left(t_{k+1}^{-}\right)=\mathrm{AP}\left(t_k\right)A^T+Q=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\&CenterDot;P\left(t_k\right)\&CenterDot;\left[\begin{array}{cc}1& 0\\ \mathrm{\&Delta;t}& 1\end{array}\right]+Q$

$=\left[\begin{array}{cc}{P}_{11}\left(t_k\right)+2\mathrm{\&Delta;t}{P}_{12}\left(t_k\right)+{\left(\mathrm{\&Delta;t}\right)}^2{P}_{22}\left(t_k\right)+\mathrm{\&Delta;}{\mathrm{\&sigma;}}_{n_{\mathrm{\&theta;}}}^2& P_{12}\left(t_k\right)+\mathrm{\&Delta;t}\&CenterDot;P_{22}\left(t_k\right)\\ P_{12}\left(t_k\right)+\mathrm{\&Delta;t}\&CenterDot;P_{22}\left(t_k\right)& P_{22}\left(t_k\right)+{\mathrm{\&sigma;}}_{n_b}^2\end{array}\right]$

可以如公式27、28和29中所示表示根据测量更新的离散形式。

[公式27]

$K\left(t_{k+1}\right)=P\left(t_{k+1}^{-}\right)H^T{(\mathrm{HP}\left(t_{k+1}^{-}\right)H^T+R)}^{-1}=\frac{1}{R_{11}\left(t_{k+1}^{-}\right){\mathrm{\&sigma;}}_v^2}\left[\begin{array}{c}{P}_{11}\left(t_{k+1}^{-}\right)\\ P_{21}\left(t_{k+1}^{-}\right)\end{array}\right]$

[公式28]

$\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}\right)\\ \mathrm{\&delta;}\hat{b}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right)\\ \mathrm{\&delta;}\hat{b}\left(t_{k+1}^{-}\right)\end{array}\right]+K\left(t_{k+1}\right)\&CenterDot;(z\left(t_{k+1}\right)-H\&CenterDot;\begin{array}{}\end{array}\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right)\\ \mathrm{\&delta;}\hat{b}\left(t_{k+1}^{-}\right)\end{array}\right])$

$=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_k\right)\\ \mathrm{\&delta;}\hat{b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}{K}_1\left(t_{k+1}\right)\\ K_2\left(t_{k+1}\right)\end{array}\right]\&CenterDot;(z\left(t_{k+1}\right)-\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right))$

[公式29]

$P\left(t_{k+1}\right)=P\left(t_{k+1}^{-}\right)-K\left(t_{k+1}\right)\&CenterDot;H\&CenterDot;P\left(t_{k+1}^{-}\right)$

$=\left[\begin{array}{cc}(1-K_1\left(t_{k+1}\right))P_{11}\left(t_{k+1}^{-}\right)& (1-K_1\left(t_{k+1}\right))P_{12}\left(t_{k+1}^{-}\right)\\ P_{12}\left(t_{k+1}^{-}\right)-K_2\left(t_{k+1}\right)P_{11}\left(t_{k+1}^{-}\right)& P_{22}\left(t_{k+1}^{-}\right)-K_2\left(t_{k+1}\right)P_{12}\left(t_{k+1}^{-}\right)\end{array}\right]$

可以从公式19中导出公式30，并且可以从公式28导出公式31。

[公式30]

$\left[\begin{array}{c}\widetilde{\mathrm{\&theta;}}\left(t_{k+1}\right)\\ \tilde{b}\left(t_{k+1}\right)\end{array}\right]=\begin{array}{c}\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\widetilde{\mathrm{\&theta;}}\left(t_k\right)\\ \tilde{b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;t}\&CenterDot;{\mathrm{\&omega;}}_g\left(t_k\right)\\ 0\end{array}\right]\end{array}\begin{array}{c}\\ \end{array}$

[公式31]

$\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}\right)\\ \mathrm{\&delta;}\hat{b}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_k\right)\\ \mathrm{\&delta;}\hat{b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}{K}_1\left(t_{k+1}\right)\\ K_2\left(t_{k+1}\right)\end{array}\right]\&CenterDot;(z\left(t_{k+1}\right)-\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right))$

当定义 $\widehat{\mathrm{\&theta;}}\&equiv;\widetilde{\mathrm{\&theta;}}+\mathrm{\&delta;}\widehat{\mathrm{\&theta;}},$ $\hat{b}\&equiv;\tilde{b}+\mathrm{\&delta;}\hat{b}$ 时，可以从公式30和公式31导出公式32。

[公式32]

$\left[\begin{array}{c}\widehat{\mathrm{\&theta;}}\left(t_{k+1}\right)\\ \hat{b}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\widehat{\mathrm{\&theta;}}\left(t_k\right)\\ \hat{b}\left(t_{k1}\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;t}\&CenterDot;{\mathrm{\&omega;}}_g\left(t_k\right)\\ 0\end{array}\right]+\left[\begin{array}{c}{K}_1\left(t_{k+1}\right)\\ K_2\left(t_{k+1}\right)\end{array}\right]\&CenterDot;(z\left(t_{k+1}\right)-\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right))$

$=\left[\begin{array}{cc}1& \mathrm{\&Delta;t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\widehat{\mathrm{\&theta;}}\left(t_k\right)\\ \hat{b}\left(t_k\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;t}\&CenterDot;{\mathrm{\&omega;}}_g\left(t_k\right)\\ 0\end{array}\right]+\left[\begin{array}{c}{K}_1\left(t_{k+1}\right)\\ K_2\left(t_{k+1}\right)\end{array}\right]\&CenterDot;({\mathrm{\&theta;}}_m\left(t_{k+1}\right)-\widetilde{\mathrm{\&theta;}}\left(t_{k+1}\right)-\mathrm{\&delta;}\widehat{\mathrm{\&theta;}}\left(t_{k+1}^{-}\right))$

初始条件如公式33中所示。

[公式33]

$\widehat{\mathrm{\&theta;}}\left(t_0\right)={\mathrm{\&theta;}}_m\left(t_0\right)$ $\widetilde{\mathrm{\&theta;}}\left(t_0\right)={\mathrm{\&theta;}}_m\left(t_0\right)$ $\mathrm{\&delta;\&theta;}\left(t_0\right)=0$

$\hat{b}\left(t_0\right)=\tilde{b}\left(t_0\right)=\mathrm{\&delta;b}\left(t_0\right)=0$

公式32是在本发明的实施例中使用的误差状态卡尔曼滤波的代表性公式。根据上述的方法，通过选择罗盘方位角和里程表角速度之一作为测量输入并且用测量输入代替公式32中的θ_m，可获得最佳运动方向估计值

本发明可以实现为计算机(包括具有信息处理功能的所有设备)可读记录介质上的计算机可读代码。计算机可读记录介质是可以存储数据的任何数据存储装置，这些数据然后可以被计算机系统读取。计算机可读记录介质的例子包括只读存储器(ROM)、随机存取存储器(RAM)、CD-ROM、磁带、软盘、光数据存储装置。

如上所述，在根据本发明的实施例的传感器合成方法中，不需要具有高规格的传感器。相应地，可以用低价格传感器来实现高性能。另外，可以估计运动物体运动时可能要处理的各种意外情况以及各种误差源。此外，可以克服卡尔曼滤波中包含的各种典型限制，例如不需要传感器的误差特性的精确信息并且可以处理偏离、有色噪声和非系统误差。作为辅助效果，可以实现系统故障传感功能。

由于根据本发明的实施例，通过有效获得偏航角，可以稳健地提供三维角度信息，因此本发明可以应用于需要绝对位置和方位估计的所有领域。可应用领域如下：自主行进车辆、智能车辆、汽车导航、医学机器人、虚拟现实、娱乐、无人驾驶飞机和个人导航。

尽管已经参照示范性实施例具体示出并描述了本发明，但是本领域普通技术人员应该明白，在不脱离所附权利要求限定的本发明的精神和范围的情况下，可以在其中进行各种形式和细节的改变。

Claims (14)

1.一种运动物体的运动估计方法，该方法包括：

从该运动物体的罗盘信息中获得磁场信息；

比较该运动物体的磁场和预定值，并根据比较结果确定该运动物体的位置是否属于特定区域；以及

通过根据该确定结果确定是否使用罗盘方位角估计该运动物体的运动方向，来估计该运动物体的运动方向。

2.如权利要求1的方法，其中该预定值是地磁场强度，并且该特定区域是地磁磁力起作用的区域。

3.如权利要求2的方法，其中确定该运动物体的位置是否属于特定区域可以包括：

如果该运动物体中装有两个罗盘，那么将第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个与地磁场强度比较，把比较结果分为两个差值都小于第一阈值的情况、两个差值中只有一个小于该第一阈值的情况、以及两个差值都不小于该第一阈值的情况，并且根据该比较结果确定该运动物体的位置是否属于该特定区域。

4.如权利要求3的方法，其中对运动物体的运动方向的估计包括：

如果在该运动物体属于特定区域时确定该罗盘信息有效，那么获得最终罗盘方位角，并使用该最终罗盘方位角作为测量输入用卡尔曼滤波来估计航向改变角；并且

如果在该运动物体不属于特定区域时确定该罗盘信息无效，那么比较陀螺仪的角速度和里程表的角速率，如果该差值小于第二阈值，那么使用由该里程表获得的运动方向为测量输入用卡尔曼滤波来估计航向改变角，而如果该差值不小于第二阈值，那么通过积分陀螺仪的角速度来估计运动方向。

5.如权利要求3的方法，其中，在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个都与地磁场强度相比较，并且两个差值都不小于第一阈值的情况下，对运动物体的运动方向的估计包括：

获得该第一罗盘的方位角和该第二罗盘的方位角之间的差值；

如果方位角之间的差值小于第三阈值，那么通过根据这两个罗盘的磁场强度和地磁场强度之间的差值而改变权重，来获得该运动物体的罗盘方位角；而

如果方位角之间的差值不小于第三阈值，那么获得关于该运动物体的陀螺仪角速度和关于罗盘方位角的每个角速度之间的差值，并且根据这些差值的大小获得该运动物体的罗盘方位角。

6.如权利要求5的方法，其中通过改变权重获得该运动物体的罗盘方位角包括：

当Δθ_c1表示在采样周期中第一罗盘的方位角改变多少的量，Δθ_c2表示在采样周期中第二罗盘的方位角改变多少的量，ω_g表示陀螺仪的角速度，Δt表示采样时间，ΔH_1E表示第一罗盘的磁场强度和地磁场强度之间的差值，而ΔH_2E表示第二罗盘的磁场强度和地磁场强度之间的差值时，

如果通过ΔH_1E乘ΔH_2E获得的值是负数时，那么如下确定最终罗盘方位角θ_c；

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}\left|{\mathrm{\&Delta;H}}_{2E}\right|+{\mathrm{\&theta;}}_{c2}\left|{\mathrm{\&Delta;H}}_{1E}\right|}{\left|{\mathrm{\&Delta;H}}_{1E}\right|+\left|{\mathrm{\&Delta;H}}_{2E}\right|}$

如果通过ΔH_1E乘ΔH_2E获得的值是正数时，那么如下确定最终罗盘方位角θ_c；

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}\mathrm{\&Delta;}{H}_{2E}-{\mathrm{\&theta;}}_{c2}{\mathrm{\&Delta;H}}_{1E}}{{\mathrm{\&Delta;H}}_{1E}-{\mathrm{\&Delta;H}}_{2E}}$

而如果通过ΔH_1E乘ΔH_2E获得的值是0，那么如下确定最终罗盘方位角θ_c

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}+{\mathrm{\&theta;}}_{c2}}{2}.$

7.如权利要求5的方法，其中通过获得角速度之间的差值而获得该运动物体的罗盘方位角可以包括：

当Δθ_c1表示在采样周期中第一罗盘的方位角改变多少的量，Δθ_c2表示在采样周期中第二罗盘的方位角改变多少的量，ω_g表示陀螺仪的角速度，而Δt表示采样时间时，

如果第一罗盘的方位角θ_c1和第二罗盘的方位角θ_c2之间的差值不小于第三阈值，如果满足 $|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|,$ 那么确定第一罗盘的方位角θ_c1为最终罗盘方位角θ_c；

如果不满足 $|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|,$ 但满足 $|\frac{{\mathrm{\&Delta;\&theta;}}_{c2}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|<|\frac{{\mathrm{\&Delta;\&theta;}}_{c1}}{\mathrm{\&Delta;t}}-{\mathrm{\&omega;}}_g|,$ 那么确定第二罗盘的方位角θ_c2为最终罗盘方位角θ_c；而

如果Δθ_c1和Δθ_c2彼此相等，那么如下确定该运动物体的运动方向角θ_c为最终罗盘方位角，

${\mathrm{\&theta;}}_c\&LeftArrow;\frac{{\mathrm{\&theta;}}_{c1}+{\mathrm{\&theta;}}_{c2}}{2}.$

8.如权利要求3的方法，其中在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个与地磁场强度相比较，并且这两个差值中只有一个小于第一阈值的情况下，对运动物体的运动方向的估计包括：

确定差值小于第一阈值的罗盘的方位角为最终罗盘方位角。

9.如权利要求3的方法，其中在第一罗盘的磁场强度和第二罗盘的磁场强度中的每一个与地磁场强度相比较，并且这两个差值都不小于第一阈值的情况下，对运动物体的运动方向的估计包括：

在不使用罗盘信息的情况下，计算与该运动物体转轮速度相关的角速度(也就是由里程表获得的角速度)；

获得该运动物体的陀螺仪角速度和转轮角速度之间的差值；

如果该差值小于第二阈值，那么使用由里程表获得的方向作为测量输入用卡尔曼滤波来估计最佳运动方向角；而

如果该差值不小于第二阈值，那么通过积分陀螺仪角速度而不使用卡尔曼滤波来估计运动方向角。

10.如权利要求9的方法，其中通过积分陀螺仪角速度来估计运动方向角还包括：如果在预定时间内该估计被执行的次数多于第二阈值，那么终止系统。

11.如权利要求5的方法，还包括：通过使用反馈误差状态的卡尔曼滤波对该运动物体的运动方向角滤波，来计算该运动物体的最佳运动方向估计值。

12.如权利要求8的方法，还包括：通过使用反馈误差状态的卡尔曼滤波对该运动物体的运动方向角滤波，来计算该运动物体的最佳运动方向估计值。

13.一种计算机可读介质，其上记录有用于执行权利要求1的方法的计算机可读程序。

14.一种运动物体的运动估计系统，该运动物体中装有陀螺仪、里程表和罗盘，该系统包括：

磁场计算器，计算该运动物体的磁场强度；

磁场比较器，获得该磁场强度与地磁场强度之间的差值，并且比较该差值与第一阈值；

地磁场区域确定器，根据该比较结果来确定该运动物体的位置是否属于地磁磁力起作用的区域；以及

运动方向估计器，通过根据该确定结果确定是否使用罗盘方位角进行该运动物体的方向估计，来估计该运动物体的运动方向。

Images