CN102308183B - 用于改善物体取向估计的方法和实施所述方法的姿态控制系统 - Google Patents

Abstract

本发明涉及一种利用物体沿三个空间轴的总加速度(yA)、磁场(yM)和转速(yG)的测量结果来估计所述物体在时间k时在空间中的取向的方法，所述方法包括：A-预处理(200)时刻k时的所述测量结果(yA，yM＞yG)，以检测所述测量结果中扰动的存在并估计时间k时的无扰动测量结果，B-由观察器从在步骤A获得的时间k时的无扰动测量结果(公式(II))估计时间k时的取向(公式(I))。

Description

用于改善物体取向估计的方法和实施所述方法的姿态控制系统

技术领域

本发明涉及一种估计有或无固有加速度以及有或无磁性扰动的物体在空间中的取向的方法，还涉及一种适于实施所述方法以能够估计取向的装置。

背景技术

获得取向一般涉及使用若干传感器，形成被指定为运动俘获装置，也被指定为姿态控制单元的组件的一部分。

M EM S(微机电系统)传感器可用于构造该控制单元，所述M EM S传感器具有小巧廉价的优点。使用这样的M EM S传感器使得能够考虑在各种应用领域中使用姿态控制单元，尤其是在生物医学领域中用于监测居家老人、功能性再教育，在运动领域中用于分析体育运动，用于汽车、机器人、虚拟现实和三维动画领域，更一般地用于需要确定或观察运动的任何领域中。

然而，与非M EM S传感器(例如用于导航领域中)相比，这些M EM S传感器的缺点是较易受噪声和偏差影响。

此外，众所周知，有些姿态控制单元既使用加速度计又使用磁强计，从而使其能够重构三个自由度的运动，即与地球重力场和地球磁场相比固有加速度和磁性扰动分别可以忽略的运动。然而，在未观察到这种假设情况，即不能忽略固有加速度或磁性扰动时，所述运动表现出六个或九个自由度。此时不可能使用仅使用加速度计和磁强计的姿态控制单元来估计活动物体的取向。亦即，运动俘获应用的多样化使得必须要克服这些限制。

因此考虑使用额外的传感器，更具体而言是组合使用速率陀螺仪、加速度计和磁强计。从这些传感器获得的测量结果由两个部分构成：与活动物体的取向直接相关的信息部分和扰动部分，扰动部分的性质取决于相关传感器。首先，这些是针对加速度计提供的测量结果的固有加速度，对磁强计提供的测量结果的磁性扰动以及速率陀螺仪的偏差(bias)。这些扰动导致不正确的取向估计。

当前，有很多方法用于从加速度计、磁强计和速率陀螺仪提供的测量结果获得对物体取向的估计。

有一些实施一种或多种优化标准的所谓优化方法，但这些方法在计算时间方面成本较高。此外，在问题变得复杂时，定义优化标准是困难的。

还有些实施神经网络的方法，神经网络需要后续学习阶段，尤其是关于数据库大小和计算时间的学习阶段，以获得精确的估计。

此外，对于优化方法和那些实施神经网络的方法而言，难以考虑到状态随时间的变化趋势的概念，这使得它们鲁棒性不够好。

还有些实现观察器的方法，与上述方法不同的是，这些方法使得能够合并来自两个源的信息：源自传感器提供的测量结果的信息和源自趋势模型的信息，并且在这样做的同时保持与实时实施兼容的计算时间。

使用观察器的已知方法主要依赖于卡尔曼滤波器的使用。这项技术的优点是允许合并数据，同时考虑到传感器提供的测量结果所提供的信息的质量和模型的质量。

本领域技术人员都知道，有很多种卡尔曼滤波器：

-扩展卡尔曼滤波器(或EKF)；这种卡尔曼滤波器实施起来迅速且容易，其应用之一是运动俘获，在如下文献中特别地描述了这种应用：“Quaternion-based extended Kalm an filter for determ ination orientation byinertial and magnetic sensing”，SABATIN I A.M.，IEEE Transactions onBiom edicalEngineering，2006，53(7)。

-专用于强非线性问题的UKF卡尔曼滤波器(无气味卡尔曼滤波器)。碰巧，在运动俘获的背景下，遇到的问题是弱非线性的；因此，它并不是很适于估计取向。此外，与EKF滤波器相比计算成本大大增加。因此人们对它的兴趣不如对EKF滤波器多。例如，如下文献描述了一种便携式取向估计装置，其中利用加速度计和磁强计合并绝对取向并利用UKF滤波器合并转速：“Portable orientation estimation device based on accelerom eters，magnetom eters and gyroscope sensors for sensor network”，HARADA T.，UCH INO H.，MORIT.，SATO T.，IEEE Conferenceon MultisensorFusion andIntegration for IntelligentSystems，2003。

-互补卡尔曼滤波器：在这种相关情况下，目的是估计状态的误差而不是状态自身，其实施起来非常复杂。

除了滤波器的选择之外，注入滤波器中的测量结果的质量，尤其是它们值的可信性也是非常重要的。

在实践中，如前所述，测量结果包括与活动的物体的取向直接相关的信息部分和扰动部分，扰动部分的性质取决于相关传感器。首先，这些是针对加速度计提供的测量结果的固有加速度，对磁强计提供的测量结果的磁性扰动以及速率陀螺仪的偏差。还有必要考虑到测量噪声，但所述噪声常规是在滤波器中处理的。

当前有很多用于处理扰动的方法。

所述方法之一包括：认为扰动是可以忽略的，并为滤波器提供由传感器提供的测量结果，在如下文献中就是这种情况：“Design，implem entationand experimental results of a quaternion-based Kalman filter for human bodymotion tracking”，YUN X.，BACHM ANN E.R.IEEE Transactions On Robotics，2006，22(6)，以及“Application of M IM U/M agnetometer integrated system Onthe attitude determination of micro satellite”，SU K.，REN D.H.，YOU Z.，ZHOU Q.，International Conference on Intelligent M echatronics andAutomation，2004年8月，中国成都。结果，在传感器之一上实际发生了扰动的情况下，滤波器提供的测量结果是有误差的，但滤波器将认为它是正确的。此时取向的估计变得不正确。因此不论自由度的数目为几个，都不可能忽略扰动并以期望精度获得估计。因此大大劣化了性能指标，因为受扰动的测量结果是原样地提供给观察器的。

因此估计方法使得能够通过检测扰动的存在并通过更新这些测量结果的可信性来将一次性缺点考虑在内，例如，如以下文献中所述：“Portableorientation estimation device based on accelerometers，magnetometers andgyroscope sensors for sensornetwork”，HARADA T.，UCH INO H.，M ORIT.，SATO T.，IEEE Conferenceon MultisensorFusion and Integration for IntelligentSystems，2003。这种方法提供了用于检测测量结果中的扰动的额外步骤。在检测到扰动时，对应测量结果中的信任度最小化。

因此在估计取向时不考虑包括扰动的测量结果提供的信息。因此取向的估计仅依赖于由其他传感器提供的测量结果。现在，在来自若干传感器的测量结果同时表现出扰动的情况下，观察器不再有充分多信息来提供正确的取向估计。

最后，在如下文献中描述了一种方法：“Inertialand magnetic sensing ofhuman motion”，ROETENBERG D.，doctoral thesis，Twente university，Netherlands，2006以及“Measuring orientation of human body segment usingminiature gyroscopes and accelerometers”，PhD Thesis,Inertial sensing ofhumanmovement，LUINGE H.J.n，2002b，其中，借助于观察器检测扰动的存在并对它们进行估计。为此，放大状态矢量，且测量模型中发生扰动，这变得更接近事实。原则上，这种技术似乎适于具有六或九个自由度的运动情形。然而，因为缺乏可观察性，对扰动和取向的组合估计是困难的。还需要大量的待设置参数，这增大了其实施的复杂性。

因此本发明的一个目的是提供一种取向估计方法，不论是否存在固有加速度和磁性扰动，这种方法都提供精确的取向估计，并且与现有方法相比以简化的方式这样做。

发明内容

前述目的是通过一种基于加速度、磁场和转速沿三个空间轴的测量结果估计取向的方法实现的，所述方法包括：

-用于预处理这些测量结果以检测扰动的存在并估计无扰动的测量结果的步骤，以及

-用于基于从预处理步骤获得的测量值估计取向的步骤。

该方法未忽略扰动，这意味着估计是无误差的；该方法恒定地估计它们。如果它们存在，它并不像其他估计方法那样拒绝相关的一个或多个测量结果。此外，不在状态矢量或测量模型中并入它们，这简化了模型并且不会导致估计变得不可能的情况。

因此在两个相继步骤中估计取向，并可能估计扰动。因此为观察器提供来自加速度计、磁强计和速率陀螺仪的测量结果，测量结果尽可能接近估计取向的理想条件：即，分别没有固有加速度、没有磁性扰动且没有偏差。

为此，使用在前一时刻估计的取向作为执行对测量结果的预处理的额外信息。

因此根据本发明的估计方法使得能够以最佳方式从传感器的测量结果提取物体的取向而不论相关的运动如何。此外，这种方法实施起来简单，且仅包括少量设置参数。

有利地，所述观察器为扩展卡尔曼滤波器。

可以估计扰动，尤其是固有加速度，这使得能够通过积分和二次积分分别回溯到物体的速度和位置。

因此本发明的主题是一种利用由人或机器人所携载的物体沿一坐标系的三个空间轴的通过加速度计获得的总加速度(y_A)、通过磁强计获得的磁场(y_M)和通过陀螺仪获得的转速(y_G)的测量结果来估计所述物体在时间k时在空间中的取向的方法，其中没有使用其它的运动或定位传感器，所述方法包括：

A-通过预处理器，在操作期间，对时刻k时的测量结果(y_A，y_M)进行系统地预处理，以确定所述测量结果中扰动的存在与否，并计算时间k时的无扰动测量结果，其中所述扰动包括：所述物体的固有加速度和增加到地球磁场的磁场，

B-通过处理器，由观察器利用在步骤A获得的时间k时的计算的无扰动测量结果和自陀螺仪获得的转速(y_G)，来估计所述物体在时间k时的取向。

步骤A有利地包括：

A1–预处理所述转速测量结果，

A2–检测所述总加速度和磁场的所述测量结果中存在还是不存在扰动，

A3-在时间k没有扰动的情况下，估计的时间k时的无扰动测量结果等于时间k时的测量结果，在有扰动的情况下，在时间k估计的无扰动测量结果是基于在前一时间k-1时估计的取向计算的。

步骤A1包括从转速测量结果减去在预备初始化步骤期间确定的平均偏差。可以通过在给定时间期间固定提供转速测量结果的模块并计算每个轴上转速测量结果值的平均值获得这一平均偏差。在姿态控制单元由人佩戴的情况下，这种固定需要从人身上去除控制单元以消除人不可避免的震颤。

用于预处理加速度和磁场测量结果的步骤A2可以包括：

-步骤A2.1，包括将所述总加速度测量结果的范数与重力场的范数进行比较的测试，如果时间k时的加速度测量结果的范数与重力场的范数之间的差异的绝对值低于预定阈值，则假设加速度扰动为零，否则假设有扰动，在每个轴上所述扰动等于时间k时所述总加速度的测量结果和在时间k时估计的无扰动加速度测量结果之间的差异，

-步骤A2.2，包括将所述磁场的测量结果的范数与地球磁场的范数进行比较的测试，如果所述磁场测量结果的范数与地球磁场的范数之间的差异的绝对值低于预定阈值，则假设磁性扰动为零，否则假设每个轴上所述磁性扰动等于时间k时所述磁场测量结果和在时间k时在每个轴上估计的无扰动磁场测量结果之间的差异。

在步骤A2.1中，有利地对时间k-1时估计的扰动进行额外测试：如果时间k时总加速度的测量结果的范数与重力场的范数之间的差异的绝对值低于预定阈值，则执行检查，以确定在时间k-1时估计的加速度扰动的范数是否低于预定阈值，如果该测试是肯定的，则假设加速度扰动在时间k实际为零，和/或在步骤A2.2中进行对时间k-1时估计的磁性扰动的额外测试：如果磁场测量结果的范数与地球磁场的范数之间的差异的绝对值低于预定阈值，则执行检查，以确定在时间k-1时估计的磁性扰动的绝对值是否低于预定阈值，如果本测试是肯定的，则假设磁性扰动在时间k实际为零。该额外步骤使得能够改善估计方法的精确度。

不过，在已知估计取向可能漂移的不利的使用情况下，仅在有限的时间窗口上确保了步骤A2.1和A2.2的这些比较测试的鲁棒性，所述时间窗口根据环境而不同。因此，有利地在说明书中稍后指出的时间带上执行比较测试。于是，使用表示完成预处理步骤时获得的测量结果，它们被称为估计的无扰动的测量结果：

-仅利用加速度测量结果的范数执行固有加速度的检测；如果在滑动时间窗口T_A的测量结果的至少一个上该范数与G₀(地球重力)的范数不同，那么认为当前时刻的测量结果受到扰动；

-通过类似方式执行磁性扰动的检测：

o如果在滑动时间窗口T_M的测量结果的至少一个上磁性测量结果的范数与H₀(地球磁场)的范数不同

o或者，如果磁性测量结果和无扰动加速度测量结果的相反数之间的角度与矢量G₀和H₀之间的角度不同，

此时，当前时刻的测量结果受到磁性扰动。

T_A可以是恒定值的参数，而T_M的值可以与运动速度相关联。

利用可能位于同一设备上并由用户激活的该变量，不需要查找时间k-1时的测量结果的值，因此消除了漂移。

步骤B中使用的观察器优选为迅速而容易的扩展卡尔曼滤波器。

用于从在时间k时估计的测量结果估计取向的步骤B可以包括：

-从在时间k-1后验估计的状态矢量估计时间k的先验状态矢量，

-从时间k的先验状态矢量的估计(称为测量结果的先验估计)来估计时间k的先验测量结果，

-通过计算在时间k估计的无扰动测量结果和先验估计测量结果之间的差异来计算扩展卡尔曼滤波器的增益并计算创新(innovation)，

-通过用所述增益和所述创新校正在时间k先验估计的状态矢量来计算时间k时的估计取向。

所述扩展卡尔曼滤波器中使用的状态矢量可以包含角速度和取向四元数的元。

扩展卡尔曼滤波器中使用的状态矢量有利地仅包含取向四元数的元，这样能够简化状态结构和测量模型。

本发明的另一主题是空间内由人携载的物体的一种姿态控制系统，至少包括用于提供沿三个空间轴的加速度的测量结果(y_A)的感测单元、用于测量沿三个空间轴的磁场(y_M)的感测单元、用于测量沿三个空间轴的转速(y_G)的感测单元、以及用于基于所述感测单元提供的测量结果来估计时间k的取向的处理单元，所述控制系统包括：

-用于对所述加速度(y_A)、磁场(y_M)和转速(y_G)的测量结果进行预处理的子单元，所述预处理子单元适于检测所述测量结果中扰动的存在，所述扰动包括所述物体的固有加速度和增加到所述地球磁场的磁场，而且所述预处理子单元适于提供估计的无扰动加速度测量结果和估计的无扰动磁性测量结果

-用于由观察器利用所述预处理模块提供的估计的无扰动加速度测量结果和估计的无扰动磁性测量结果和所述转速测量结果来估计时刻k的取向的子单元。根据本发明的姿态控制单元还可以包括计算控制中心的初始化期间转速测量模块的平均偏差的模块。

预处理模块包括用于检测所述加速度测量结果中固有加速度的存在的模块和用于检测所述磁场测量结果中磁性扰动的存在的模块。

根据本发明的姿态控制单元还可以包括估计固有加速度以及计算物体的速度和位置的模块。

有利地，适于提供沿三个空间轴的总加速度测量结果、磁场测量结果和转速测量结果的模块为M EM S传感器。

本发明的再另一主题是空间内由人携载的物体的一种姿态控制系统，至少包括用于提供沿三个空间轴的加速度的测量结果(y_A)的感测单元、用于测量沿三个空间轴的磁场(y_M)的感测单元、用于测量沿三个空间轴的转速(y_G)的感测单元、以及处理单元，所述处理单元包括用于对所述加速度(y_A)和磁场(y_M)的测量结果进行预处理的子单元，所述预处理子单元适于检测所述测量结果中扰动的存在，所述扰动包括所述物体的固有加速度和增加到地球磁场的磁场。

附图说明

从以下描述和附图中将更好地理解本发明，其中：

-图1是时间k时的根据本发明的方法的流程图，

-图2A到图2C示出了根据本发明用于预处理分别来自加速度计、速率陀螺仪和磁强计的测量结果的步骤的详细流程图。

具体实施方式

目的是获得在空间中活动的物体的取向，例如人的取向。为此，使用姿态控制单元，其包括适于提供沿三个空间轴的总加速度、磁场和转速的测量结果的传感器。有利地，传感器是M EM S传感器，其具有降低的成本价格和有限的覆盖面积。

对于加速度测量而言，例如，可以是提供每个轴上的测量结果的三轴加速度计或三个单轴加速度计。

类似地，对于磁场测量而言，例如，可以是三轴磁强计或三个单轴磁强计。

对于转速测量而言，例如，可以是三个单轴速率陀螺仪，或有利地，是两个双轴速率陀螺仪。

三个轴可以对准或不对准，但在后一种情况下，轴间的相对取向必须是已知的。

在下文中，在描述中为了简单起见，将把一个或多个加速度计称为加速度计，把一个或多个磁强计称为磁强计，把一个或多个速率陀螺仪称为速率陀螺仪。这些传感器附着于取向已知的物体。

我们仅有测量结果y，将通过下式进行建模：

$\left\{\begin{array}{c}{y}_A=-{\mathrm{RG}}_0+a+v_A\\ y_M={\mathrm{RH}}_0+d+v_M\\ y_G=\mathrm{\ω}+b+v_G\end{array}\right.---\left(I\right)$

其中y_A:加速度计提供的总加速度的三轴测量结果，

y_M:磁强计提供的磁场的三轴测量结果，

y_G:速率陀螺仪提供的转速的三轴测量结果，

R:旋转矩阵，

G₀:地球重力场(矢量3×1)，

H₀:地球磁场(矢量3×1)，

ω:角速度，

a:适当的加速度，

d:磁性扰动，

b:速率陀螺仪偏差，

v_A:加速度计测量噪声，

v_M:磁强计测量噪声，

v_G:速率陀螺仪测量噪声。

相对于完全由矢量G₀和H₀的数据定义的参考坐标估计取向。例如，由矢量G₀(0；0；1)和定义地心坐标。

为了简单起见，将不会区分三个空间方向上的测量结果。

从测量结果的数学定义(I)显然将看出，这些测量结果的每个分别包括第一部分“-RG_O”,“R.H_O”和ω，其包含获得取向估计所用的信息；第二部分a，d和b，其代表测量结果中可能随机出现的可能扰动；最后为第三部分v_A、v_M、V_G，表示每个传感器上的测量噪声。

在根据本发明的方法中，在收集测量结果之后，在将所述测量结果用于意在提供对取向的估计的处理步骤之前，进行预处理步骤。

图1示出了根据本发明的方法的大致流程图。

在对该方法的以下描述中，以对时间k时的取向的估计作为范例，k是大于或等于2的适当整数。

根据本发明的方法包括对姿态控制单元进行初始化的步骤100，对传感器所提供的测量结果进行预处理的步骤200以及观察器进行的第三处理步骤300。在以下描述中将详细描述每个所述步骤。

在详细描述根据本发明的方法的各步骤之前，我们将描述观察器。有利地，用于测量结果处理步骤中的观察器是扩展卡尔曼滤波器，其实施起来简单、鲁棒且迅速。

这种滤波器为本领域技术人员所公知，将不会加以详细描述。我们将仅给出状态模型和测量模型的数学表达式。

卡尔曼滤波器包括定义状态的时间趋势和动态趋势的状态模型以及用于使传感器测量结果和状态相关联的测量模型。

根据第一种建模方法，卡尔曼滤波器的状态矢量包括角速度的三个元和定义取向的四元数的四个元。

关联的状态和测量模型分别可以是：

$\stackrel{\·}{x}=f\left(x\right)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\ω}}\\ \stackrel{\·}{q}\end{array}\right)=\left(\begin{array}{c}-\frac{1}{\mathrm{\τ}}\mathrm{\ω}\\ \frac{1}{2}\·q\⊗\mathrm{\ω}\end{array}\right)+w_x$

$y=h\left(x\right)=\left(\begin{array}{c}{y}_G\\ y_A\\ y_M\end{array}\right)=\left(\begin{array}{c}\mathrm{\ω}\\ -\stackrel{\‾}{q}\⊗G_0\⊗q\\ \stackrel{\‾}{q}\⊗H_0\⊗q\end{array}\right)+\left(\begin{array}{c}{v}_G\\ v_A\\ v_M\end{array}\right)$

其中x:状态矢量

ω:角速度，

q:四元数，

τ:角速度演变模型的时间常数，

y:测量结果，

w_x:建模噪声

为简单起见，维度为4×1的矢量四元数[0，G₀]用维度为3×1的矢量G₀标识，对于维度为4×1的矢量四元数[0，H₀]同样适用，矢量H₀的维度也是3×1。

有利地，根据第二种建模方法，可以使用状态矢量，状态矢量仅包含四元数的元，后者不超过维度4，而在第一种建模方法中其维度为7。然后将陀螺仪测量结果直接注入状态模型中，测量矢量仅包含来自加速度计和磁强计的测量结果。

然后，状态模型和测量模型能够表达为：

$\stackrel{\·}{x}=f\left(x\right)=\stackrel{\·}{q}=\frac{1}{2}\·q\⊗y_G+w_x---\left(\mathrm{II}\right),$ 且

$y=h\left(x\right)=\left(\begin{array}{c}{y}_A\\ y_M\end{array}\right)=\left(\begin{array}{c}-\stackrel{\‾}{q}\⊗G_0\⊗q\\ \stackrel{\‾}{q}\⊗H_0\⊗q\end{array}\right)+\left(\begin{array}{c}{v}_A\\ v_M\end{array}\right)---\left(\mathrm{III}\right)$

可以使用该第二种建模方法简化状态结构和测量模型，因为它们的维度被直接减少。此外，设置参数的数目，尤其是建模噪声、测量噪声和状态矢量估计误差的协方差矩阵元的数目也受到限制，这使得更容易实施这种方法。通过这种方式获得的估计结果与使用第一种建模方法获得的那些估计结果具有类似的精确度。

现在将详细描述根据本发明的步骤100、200和300。

初始化步骤100规定要估计的速率陀螺仪的平均扰动。该扰动b实际上是速率陀螺仪的偏差，其在两个极限值之间变化。

对于初始化步骤(k＝1)：

在最初时刻固有加速度a₁和磁性扰动d₁都是已知的，在这种情况下，初始化步骤包括状态矢量x₁的确定：假设角速度在最初时刻为零，利用扰动a₁和d₁的校正的加速度和磁场测量结果通过优化确定四元数；或者初始状态的取向是已知的，在这种情况下可以推断出a₁和d₁。

用于在k＝1时对控制单元进行初始化的步骤100还包括：

-设置分别关联到建模噪声、测量噪声和初始状态矢量估计误差的协方差矩阵Q、R和P₁；

-计算偏差b的估计值，表示为在预定时间内，例如大约一秒内保持姿态控制单元固定，并计算每个轴上来自速率陀螺仪的输出值的平均值。接下来，在预处理步骤200期间，从速率陀螺仪的每个测量结果减去平均偏差的这一估计值这使得能够使偏差的影响最小化，从而改善所得结果的精确度。

平均偏差的这一估计值优选发生于每次采集的开始。还可以在固定期间刷新这一估计值。

图2A到图2C示出了根据本发明的方法的步骤细节。

在步骤200期间内，在三个步骤210、220和230期间对三个传感器提供的测量结果进行预处理。根据使用该方法的背景和希望的精确度，有很多可能的变化实施例。

第一步骤210在所有实施例中都是相同的。在图2B所示的这一步骤210期间，对来自速率陀螺仪的测量结果y_G,k进行预处理。如上所述，通过从实际测量结果y_G,k减去平均偏差实现对测量结果y_G,k的这种预处理，获得被表示为的时间k的来自速率陀螺仪的预处理测量结果，作为输出。

在描述中，为了简单起见，以G₀(地球磁场)的倍数给出加速度，磁场被设定为H₀的倍数。

在第一实施例中，有利地并行执行两项测试，以检测加速度扰动(步骤220)和磁性扰动(步骤230)的存在。在步骤220期间，对加速度计在时间k提供的测量结果y_A,k进行预处理。这个步骤220包括：第一子步骤220.1，用于检测是否存在扰动，即固有加速度a的扰动；以及第二子步骤220.2，用于基于在前一时刻k-1估计的取向构造加速度的预处理测量结果 ${\tilde{y}}_{A,k}.$

在步骤220.1期间，为了检测是否存在固有加速度a，将测量结果y_A,k的范数与重力场的范数进行比较(作为提示，该方法在G₀的倍数下工作)，因此相对于1进行比较：

If|||y_A，k||-1|＜α_A，

有利地，如果测试结果是肯定的，则增加以下测试：

$\left|\widehat{a_{k-1}}\right|<{\mathrm{\&beta;}}_A$

有利地，使用在时间k-1估计的固有加速度的范数在β_A的比较排除第一测试不充分的特定情况。在实践中，假设如果在时间k-1，固有加速度具有大值，即大于β_A，则在时间k，固有加速度将不大可能小于β_A。α_A和β_A例如分别等于0.04和0.2。

因此，这种第二测试改进了无扰动测量的估计精度，并因此改进了取向的估计精度。

如果以上两项测试是肯定的，然后判定在时间k固有加速度为零。在步骤220.2期间，估计的测量结果然后等于y_A,k，可以由观察器直接使用。

$\widehat{a_k}=0$

${\tilde{y}}_{A,k}=y_{A,k}$

否则，在步骤220.2期间，利用在前一时刻估计的估计取向构造新的加速度测量结果。

然后使用测量模型表达在时间k估计的无扰动加速度测量结果：

${\tilde{y}}_{A,k}=-\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;G_0\&CircleTimes;\widehat{q_{k-1}},$

还可以从其中推导出时间k处固有加速度的值(步骤220.3)，这使得能够分别通过积分和二重积分从其中推导出物体的速度和位置。

固有加速度等于：

$\widehat{a_k}=y_{A,k}+\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;G_0\&CircleTimes;\widehat{q_{k-1}},$ 因为 $y_{A,k}={\tilde{y}}_{A,k}+\widehat{a_k}$

在步骤230期间，类似于步骤220，对磁强计在时间k提供的测量结果y_M,k进行预处理。这个步骤230包括：第一子步骤230.1，用于检测是否存在磁性扰动d；以及第二子步骤230.2，用于基于在前一时刻k-1估计的取向构造磁场的预处理测量结果

在步骤230.1期间，为了检测是否存在磁性扰动d，将测量结果y_M,k的范数与磁场的范数进行比较(作为提示，该方法在H₀的倍数下工作)，因此相对于1进行比较：

If|||y_M，k||-1|＜α_M，

有利地，在测试结果为肯定不存在磁场扰动的情况下，增加以下测试：

$\left|\widehat{d_{k-1}}\right|<{\mathrm{\&beta;}}_M$

有利地，使用在时间k-1估计的磁性扰动的范数在β_M的比较排除第一测试不充分的特定情况。在实践中，假设如果在时间k-1，磁性扰动具有大值，即大于β_M，在时间k，磁性扰动将不可能小于β_M。α_M和β_M例如分别等于0.04和0.2。

因此，这种第二测试改进了无扰动测量结果的估计的精度，并因此改进了取向的估计精度。

如果以上两项测试是肯定的，然后判定在时间k磁性扰动为零。在步骤230.2期间，然后估计的测量结果等于y_M,k，可以由观察器直接使用。

$\widehat{d_k}=0$

${\tilde{y}}_{M,k}=y_{M,k}$

否则，在步骤230.2期间，利用在前一时刻估计的估计取向构造新的磁场测量结果。

然后使用测量模型表达在时间k估计的无扰动磁性测量结果：

${\tilde{y}}_{M,k}=-\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;H_0\&CircleTimes;\widehat{q_{k-1}},$

也可以从其推导出时间k时的磁性扰动的值(步骤230.3)，其等于：

$\widehat{d_k}=y_{M,k}+\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;H_0\&CircleTimes;\widehat{q_{k-1}},$ 因为 $y_{M,k}={\tilde{y}}_{M,k}+\widehat{d_k}$

在已知取向估计可能有显著漂移的情况下，第二实施例尤其适用。在这些情况下，有利的是定义在其中执行是否存在加速度扰动和磁性扰动的测试的时间窗口。

在这种变体中，对于加速度测量测试而言，在终止于时刻t_k的窗口上执行步骤220中进行的比较：如果检测到固有加速度(在窗口[t_k–t_A；t_k]的至少一个测量结果上与范数G₀相差α_A的加速度测量结果的范数)，然后利用在前一时刻估计的取向构造无扰动加速度测量结果；否则，无扰动加速度测量结果等于预处理阶段中涉及的测量结果(传感器测量结果)。从传感器测量结果计算固有加速度的值。仅作为范例给出的α_A和T_A的典型值为0.2g和0.4s。

那么对没有固有加速度的测试表达为：

If|||y_A，k||-||G₀|||＜α_A， ${\&ForAll;t}_k\&Element;[t_k-T_A;t_k]$

$\widehat{a_k}=0$

${\tilde{y}}_{A,k}=y_{A,k}$

Else

${\tilde{y}}_{A,k}=-\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;G_0\&CircleTimes;\widehat{q_{k-1}}$

有利地，即使在未超过阈值的情况下，也可以利用与第一变体实施例中相同的公式系统地计算固有加速度：

$\widehat{a_k}=y_{A,k}+\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;G_0\&CircleTimes;\widehat{q_{k-1}}$

然后，或者并行地(这一第二选项是有利的，因为其实现了时间的节省)，执行步骤230中提供的对检测磁性信号扰动的测试：如果检测到磁性扰动(窗口[t_k-T_M；t_k]的至少一个测量结果上与H₀范数相差α_M的磁性测量结果的范数)，那么利用在前一时刻估计的取向构造无扰动磁性测量结果。否则，无扰动磁性测量结果等于预处理阶段中涉及的测量结果(传感器测量结果)。然后从传感器测量结果计算磁性扰动的值。仅作为范例给出的α_M和T_M的典型值分别为0.1h和0.5s。

然后将用于不存在扰动的测试表达为：

If|||y_M，k||-||H₀|||＜α_M， ${\&ForAll;t}_k\&Element;[t_k-T_M;t_k]:$

$\widehat{d_k}=0$

${\tilde{y}}_{M,k}=y_{M,k}$

else

${\tilde{y}}_{M,k}={\stackrel{\&OverBar;}{q}}_{k-1}\&CircleTimes;H_0\&CircleTimes;q_{k-1}$

End if

有利地，即使在未超过阈值的情况下，也可以利用与第一变体实施例中相同的公式系统地计算磁性扰动：

$\widehat{d_k}=y_{M,k}-\stackrel{\stackrel{\&OverBar;}{^}}{q_{k-1}}\&CircleTimes;H_0\&CircleTimes;\widehat{q_{k-1}}$

在这前两个实施例中，有利地并行执行步骤202和203。

另一方面，在需要时以及在装置包括充分多计算存储设备以使用三角函数时，可以使用第三实施例来增强检测的精确度。在这种情况下，省去了通过并行执行检测计算获得的优点，更有利的是在对固有加速度的存在的测试之后执行对磁性扰动的存在的测试。在步骤202的这第一计算的输出，还计算并使用u₀表示矢量G₀和H₀之间测量的角度。然后可以在初始化步骤100期间计算这个参数。然后，如下执行对磁性扰动的检测的测试：如果检测到磁性扰动(窗口[t_k-T_M；t_k]的至少一个测量结果上与H₀的范数相差α_M的磁性测量结果的范数或与u₀相差α_u的角度u_k)，那么利用在前一时刻估计的取向构造无扰动磁性测量结果。否则，无扰动磁性测量结果等于预处理阶段中涉及的测量结果(传感器测量结果)。然后从传感器测量结果计算磁性扰动的值。有利地，根据是否有固有加速度，将使用T_M的两个不同值(分别为T_{M_fast}和T_{M_slow})。仅作为范例给出的α_M、α_u、T_{M_fast}和T_{M_slow}的典型值分别为0.1h、10°、0.5s和3s。

然后将用于扰动的存在的测试表达为：

If a properacceleration is presen t/*如果存在固有加速度*/

T_M＝T_{M_fast}

else

T_M＝T_{M_slow}

End if

If|||y_M，k||-||H₀|||＞α_Mor|u_k-u₀|＞α_ufor at least one value t_k such thatt_k∈[t_k-T_M；t_k]:/*如果对于至少一个值t_k而言|||y_M,k||-||H₀|||>α_M或|u_k-u₀|>α_u,使得t_k∈[t_k-T_M；t_k]*/

${\tilde{y}}_{M,k}={\stackrel{\&OverBar;}{q}}_{k-1}\&CircleTimes;H_0\&CircleTimes;q_{k-1}$

else

${\tilde{y}}_{M,k}=y_{M,k}$

End if

$d_k=y_{M,k}-{\stackrel{\&OverBar;}{q}}_{k-1}\&CircleTimes;H_0\&CircleTimes;q_{k-1}$

在步骤300期间，由观察器使用预处理的测量结果例如，以其因式分解的形式使用扩展的卡尔曼滤波器。

现在将解释滤波器执行的计算步骤。

将假设正在使用第二建模方法。不过，显然可以类似地使用第一建模方法。

步骤300包括以下步骤：

a)对状态矢量进行先验估计，

b)对测量结果进行先验估计，

c)计算卡尔曼滤波器的增益K_k和创新I_k，

d)校正先验估计的状态。

下面将详细描述步骤a)到d)。

在步骤a)期间，先验估计由来自时间k-1的状态矢量的后验估计的时间k的状态矢量构成。

由下式给出先验状态矢量的估计：

$\widehat{x_k^{-}}={\hat{x}}_{k-1}+\mathrm{Te}.f\left({\hat{x}}_{k-1}\right)$

其中时间k-1时状态矢量的先验估计，

时间k时状态矢量的先验估计，

T_e:采样间隔。

在步骤b)期间，使用测量模型(III)，利用在步骤a)中估计的状态矢量进行对测量结果的估计：

$\widehat{y_k^{-}}=h\left(\widehat{x_k^{-}}\right)$

在步骤c)期间，计算增益K_k和创新I_k，创新是通过从预处理测量结果减去先验估计的测量结果获得的。

获得以下结果：

$I_k=\left[\begin{array}{c}{\tilde{y}}_{A,k}\\ {\tilde{y}}_{M,k}\end{array}\right]-\widehat{y_k^{-}}$

在步骤d)期间，利用增益和创新校正先验估计状态。

$\widehat{x_k}=\widehat{x_k^{-}}+K_k{I}_k$

这一校正给出时间k时取向的先验估计

有利地，在后续步骤e)期间，对估计的四元数进行归一化

这样能够避免计算的每个步骤中有漂移。

根据本发明的方法提供了如下优点，即向观察器提供了与测量模型一致的接近无扰动测量结果的测量结果。因此大大减小了扰动对取向估计的影响。还能够即使在存在扰动时，通过在每个测量结果中保持恒定的信任度，从而持久地为观察器提供源自每个传感器的信息。在实践中，组合使用来自加速度计、来自磁强计和来自速率陀螺仪的测量结果使得能够减小测量误差(测量噪声、剩余扰动、剩余速率陀螺仪偏差)对估计取向的影响。

根据本发明的方法不仅能够估计取向，而且能够在每个采样间隔处估计固有加速度和磁性扰动，即使执行高达九个自由度的运动也是如此。实施这种方法非常简单，因为它依赖于基本结构单元的使用：值测试、分析计算、扩展卡尔曼滤波器。

Claims (22)

1.一种利用由人所携载的物体沿一坐标系的三个空间轴的通过加速度计获得的总加速度(y_A)、通过磁强计获得的磁场(y_M)和通过陀螺仪获得的转速(y_G)的测量结果来估计所述物体在时间k时在空间中的取向的方法，其中没有使用其它的运动或定位传感器，所述方法包括：

A-通过预处理器，在操作期间，对时刻k时的测量结果(y_A，y_M)进行系统地预处理，以检测所述测量结果中扰动的存在与否，并估计时间k时的无扰动测量结果，其中所述扰动包括：所述物体的固有加速度和增加到地球磁场的磁场，

B-通过处理器，由观察器利用在步骤A获得的时间k时的估计的无扰动测量结果和在时间k时的转速(y_Gk)，来估计在时间k时的所述取向。

2.根据权利要求1所述的方法，其特征在于，估计物体在时间k时在空间中的取向仅利用所述物体沿三个空间轴的所述总加速度(y_A)、磁场(y_M)和转速(y_G)的测量结果。

3.根据权利要求1所述的方法，其特征在于，步骤A包括：

A1–对所述转速(y_G)的测量结果进行预处理，

A2–检测在时间k，所述总加速度和所述磁场的所述测量结果(y_A，y_M)中是否存在扰动，

A3-在时间k时不存在扰动的情况下，时间k时的估计的无扰动测量结果等于时间k时的测量结果并存储在存储器中作为最后可用的无扰动测量结果，而在存在扰动的情况下，在时间k时估计的无扰动测量结果是基于在时间k-1时的所述估计的取向而计算的。

4.根据权利要求3所述的方法，其特征在于，步骤A1包括从转速的测量结果减去在初步初始化步骤期间确定的平均偏差

5.根据权利要求4所述的方法，其特征在于，通过在给定时间期间固定提供转速的测量结果的模块并计算每个轴上所述转速的测量结果的值的平均值来获得所述平均偏差

6.根据权利要求3所述的方法，其特征在于，所述步骤A2包括：

A2.1-将所述总加速度的测量结果的范数与重力场(G₀)的范数进行比较的测试，其中，如果时间k时的所述加速度的测量结果的范数与重力场(G₀)的范数之间的差异的绝对值低于预定阈值(α_A)，则假设加速度扰动为零，否则假设存在扰动，在每个轴上所述扰动等于时间k时的所述总加速度的测量结果和在时间k时估计的无扰动加速度测量结果之间的差异，

A2.2-将所述磁场的测量结果的范数与所述地球磁场(H₀)的范数进行比较的测试，其中，如果所述磁场的测量结果的范数与所述地球磁场的范数之间的差异的绝对值低于预定阈值(α_M)，则假设磁性扰动为零，否则假设每个轴上的所述磁性扰动等于时间k时的所述磁场的测量结果和在时间k时估计的无扰动磁场的测量结果之间的差异。

7.根据权利要求6所述的方法，其特征在于，在步骤A2.1中对时间k-1时的估计的扰动进行额外的测试：如果时间k时的所述总加速度的测量结果的范数与所述重力场(G₀)的范数之间的差异的绝对值低于预定阈值(α_A)，则执行检查，以确定时间k-1时的估计的加速度扰动的范数是否低于预定阈值(β_A)，如果该测试是肯定的，则假设加速度扰动在时间k时实际为零，和/或在步骤A2.2中对时间k-1时的估计的磁性扰动进行额外的测试，其中，如果所述磁场的测量结果的范数与所述地球磁场(H₀)的范数之间的差异的绝对值低于预定阈值(α_M)，则检查时间k-1时的估计的磁性扰动的绝对值是否低于预定阈值(β_M)，其中，如果该测试是肯定的，则假设所述磁性扰动在时间k时实际为零。

8.根据权利要求3所述的方法，其特征在于，在用户设置的时间窗口(T_A，T_M)上执行步骤A2中的至少一个检测。

9.根据权利要求8所述的方法，其特征在于，所述物体的固有加速度的检测是通过以下处理进行的：

测试在所述时间窗(T_A)中是否存在有时间(t_k)，在时间(t_k)的所述测量的加速度(y_A,k)的范数和表示所述重力场的矢量(G₀)的范数之间的差的绝对值超过一预定参数值(α_A)，

如果测试结果为肯定，估计在时间(t_k)的所述加速度的无扰动测量结果为(-1)标量、在时间(t_k-1)所述物体的取向四元估计量(q_k-1)，所述矢量(G₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的乘积；

如果测试结果为否定，估计在时间(t_k)的所述加速度的无扰动测量结果为在时间(t_k)的所述测量的加速度(y_A,k)；

当在时间(t_k)的所述固有加速度的估计量(α_k)等于所述测量的加速度(y_A,k)和所述(-1)标量、在时间(t_k-1)所述物体的取向四元估计量(q_k-1)，所述矢量(G₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的所述乘积的差时，停止。

10.根据权利要求8所述的方法，其特征在于，所述磁性扰动的检测是通过以下处理进行的：

测试在所述时间窗(T_M)中是否存在有时间(t_k)，在时间(t_k)的所述测量的磁场(y_M,k)的范数和表示所述重力场的矢量(H₀)的范数之间的差的绝对值超过一预定参数值(α_M)，

如果测试结果为肯定，估计在时间(t_k)的所述磁场的无扰动测量结果为在时间(t_k-1)所述物体的取向的四元估计量(q_k-1)，所述矢量(H₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的乘积；

如果测试结果为否定，估计在时间(t_k)的所述加速度的无扰动测量结果为在时间(t_k)的所述测量的磁场(y_M,k)；

当在时间(t_k)的加至所述重力场的所述磁场的估计量(d_k)等于在时间(t_k)的所述测量的磁场和在时间(t_k-1)所述物体的取向的四元估计量(q_k-1)，所述矢量(H₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的所述乘积的差时，停止。

11.根据权利要求8所述的方法，其特征在于，还在固有加速度的检测的输出处由所述预处理器计算在时间(t_k)的角度然后通过以下形式的处理进行对磁性扰动的检测：

当检测到固有加速度时，将所述时间窗(T_M)设置在第一预设参数值(T_M)，否则，将所述时间窗(T_M)设置在第二预设参数值(T_M＝T_M slow),

当处于以下情况时，测试在所述时间窗(T_M)中是否存在有时间(t_k)，

在时间(t_k)的所述测量的磁场(y_M,k)的范数和表示所述重力场的矢量(H₀)的范数之间的差的绝对值超过一预定参数值(α_M)，或者

角度u_k和参数角度u₀之间的差的绝对值超过一预定参数值(α_u)，

如果测试结果为肯定，估计在时间(t_k)的所述磁场的无扰动测量结果为在时间(t_k-1)所述物体的取向的四元估计量(q_k-1)，所述矢量(H₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的乘积；

如果测试结果为否定，估计在时间(t_k)的所述加速度的无扰动测量结果为在时间(t_k)的所述测量的磁场(y_M,k)；

当在时间(t_k)的加至所述重力场的所述磁场的估计量(d_k)等于在时间(t_k)的所述测量的磁场和在时间(t_k-1)所述物体的取向的四元估计量(q_k-1)，所述矢量(H₀)和所述四元估计量(q_k-1)的共轭(q_k-1)的所述乘积的差时，停止。

12.根据权利要求1到11中的任一项所述的方法，其特征在于，步骤B中使用的观察器为扩展卡尔曼滤波器。

13.根据权利要求12所述的方法，其特征在于，用于利用在时间k时估计的无扰动测量结果来估计所述取向的步骤B包括：

-利用在时间k-1时后验估计的状态矢量来估计时间k时的先验状态矢量

-利用时间k时的先验状态矢量的估计来估计时间k时的先验测量结果

-通过计算在时间k时估计的所述无扰动测量结果和估计的所述先验测量结果之间的差异来计算所述扩展卡尔曼滤波器的增益(K_k)和创新(I_k)，

-通过用所述增益和所述创新对在时间k时先验估计的状态矢量进行校正来计算时间k时的估计取向

14.根据权利要求13所述的方法，其特征在于，所述扩展卡尔曼滤波器中使用的状态矢量包含角速度的元和取向四元数的元。

15.根据权利要求14所述的方法，其特征在于，所述扩展卡尔曼滤波器中使用的状态矢量仅包含取向四元数的元。

16.空间内由人携载的物体的一种姿态控制系统，至少包括用于提供沿三个空间轴的加速度的测量结果(y_A)的感测单元、用于测量沿三个空间轴的磁场(y_M)的感测单元、用于测量沿三个空间轴的转速(y_G)的感测单元、以及用于基于所述感测单元提供的测量结果来估计时间k的取向的处理单元，所述控制系统包括：

-用于对所述加速度(y_A)和磁场(y_M)的测量结果进行预处理的子单元，所述预处理子单元适于检测所述测量结果中扰动的存在，所述扰动包括所述物体的固有加速度和增加到地球磁场的磁场，而且所述预处理子单元适于提供估计的无扰动加速度测量结果和估计的无扰动磁性测量结果

-用于由观察器利用所述预处理模块提供的估计的无扰动加速度测量结果和估计的无扰动磁性测量结果和自所述感测单元获得的所述转速测量结果(y_G)来估计时刻k的所述取向的子单元。

17.根据权利要求16所述的姿态控制系统，还包括用于计算在控制系统的初始化步骤期间转速测量模块的平均偏差的模块。

18.根据权利要求16所述的姿态控制系统，其特征在于，预处理子单元包括用于检测所述加速度的测量结果中固有加速度的存在的模块和用于检测所述磁场的测量结果中磁性扰动的存在的模块。

19.根据权利要求18所述的姿态控制系统，其特征在于，所述用于检测所述加速度的测量结果中固有加速度的存在的模块和所述用于检测所述磁场的测量结果中磁性扰动的存在的模块在一个或多个时间窗口内执行这些检测。

20.根据权利要求16所述的姿态控制系统，还包括用于估计所述固有加速度和所述磁性扰动并用于计算所述物体的速度和位置的模块。

21.根据权利要求16所述的姿态控制系统，其特征在于，所述观察器是扩展卡尔曼滤波器。

22.根据权利要求16所述的姿态控制系统，其特征在于，用于提供沿三个空间轴的总加速度的测量结果(y_A)、磁场的测量结果(y_M)和转速的测量结果(y_G)的感测单元是M EM S传感器。