专利名称：：用于改善物体取向估计的方法和实施所述方法的姿态控制系统的制作方法用于改善物体取向估计的方法和实施所述方法的姿态控制系统本发明涉及一种估计有或无固有加速度以及有或无磁性扰动的物体在空间中的取向的方法，还涉及一种适于实施所述方法以能够估计取向的装置。获得取向一般涉及使用若干传感器，形成被指定为运动俘获装置，也被指定为姿态控制单元的组件的一部分。MEMS(微机电系统)传感器可用于构造该控制单元，所述MEMS传感器具有小巧廉价的优点。使用这样的MEMS传感器使得能够考虑在各种应用领域中使用姿态控制单元，尤其是在生物医学领域中用于监测居家老人、功能性再教育，在运动领域中用于分析体育运动，用于汽车、机器人、虚拟现实和三维动画领域，更一般地用于需要确定或观察运动的任何领域中。然而，与非MEMS传感器(例如用于导航领域中)相比，这些MEMS传感器的缺点是较易受噪声和偏差影响。此外，众所周知，有些姿态控制单元既使用加速度计又使用磁强计，从而使其能够重构三个自由度的运动，即与地球重力场和地球磁场相比固有加速度和磁性扰动分别可以忽略的运动。然而，在未观察到这种假设情况，即不能忽略固有加速度或磁性扰动时，所述运动表现出六个或九个自由度。此时不可能使用仅使用加速度计和磁强计的姿态控制单元来估计活动物体的取向。亦即，运动俘获应用的多样化使得必须要克服这些限制。因此考虑使用额外的传感器，更具体而言是组合使用速率陀螺仪、加速度计和磁强计。从这些传感器获得的测量结果由两个部分构成与活动物体的取向直接相关的信息部分和扰动部分，扰动部分的性质取决于相关传感器。首先，这些是针对加速度计提供的测量结果的固有加速度，对磁强计提供的测量结果的磁性扰动以及速率陀螺仪的偏差(bias)。这些扰动导致不正确的取向估计。当前，有很多方法用于从加速度计、磁强计和速率陀螺仪提供的测量结果获得对物体取向的估计。有一些实施一种或多种优化标准的所谓优化方法，但这些方法在计算时间方面成本较高。此外，在问题变得复杂时，定义优化标准是困难的。还有些实施神经网络的方法，神经网络需要后续学习阶段，尤其是关于数据库大小和计算时间的学习阶段，以获得精确的估计。此外，对于优化方法和那些实施神经网络的方法而言，难以考虑到状态随时间的变化趋势的概念，这使得它们鲁棒性不够好。还有些实现观察器的方法，与上述方法不同的是，这些方法使得能够合并来自两个源的信息源自传感器提供的测量结果的信息和源自趋势模型的信息，并且在这样做的同时保持与实时实施兼容的计算时间。使用观察器的已知方法主要依赖于卡尔曼滤波器的使用。这项技术的优点是允许合并数据，同时考虑到传感器提供的测量结果所提供的信息的质量和模型的质量。本领域技术人员都知道，有很多种卡尔曼滤波器-扩展卡尔曼滤波器(或EKF)；这种卡尔曼滤波器实施起来迅速且容易，其应用之一是运动俘获，在如下文献中特别地描述了这种应用“Quaternion-basedextendedKalmanfilterfordeterminationorientationbyinertialandmagneticsensing，，，SABATINIΑ.Μ.,IEEETransactionsonBiomedicalEngineering,2006,53(7)。-专用于强非线性问题的UKF卡尔曼滤波器(无气味卡尔曼滤波器)。碰巧，在运动俘获的背景下，遇到的问题是弱非线性的；因此，它并不是很适于估计取向。此外，与EKF滤波器相比计算成本大大增加。因此人们对它的兴趣不如对EKF滤波器多。例如，如下文献描述了一种便携式取向估计装置，其中利用加速度计和磁强计合并绝对取向并利用UKF滤波器合并转速”PortabIeorientationestimationdevicebasedonaccelerometers，magnetometersandgyroscopesensorsforsensornetwork”，HARADAT.，UCHINOH.，MORIΤ.，SATOΤ.，IEEEConferenceonMultisensorFusionandIntegrationforIntelligentSystems，2003。-互补卡尔曼滤波器在这种相关情况下，目的是估计状态的误差而不是状态自身，其实施起来非常复杂。除了滤波器的选择之外，注入滤波器中的测量结果的质量，尤其是它们值的可信性也是非常重要的。在实践中，如前所述，测量结果包括与活动的物体的取向直接相关的信息部分和扰动部分，扰动部分的性质取决于相关传感器。首先，这些是针对加速度计提供的测量结果的固有加速度，对磁强计提供的测量结果的磁性扰动以及速率陀螺仪的偏差。还有必要考虑到测量噪声，但所述噪声常规是在滤波器中处理的。当前有很多用于处理扰动的方法。所述方法之一包括认为扰动是可以忽略的，并为滤波器提供由传感器提供的测量结果，在如下文献中就是这种情况“Design，implementationandexperimentalresultsofaquaternion-basedKalmanfilterforhumanbodymotiontracking",YUNX.,BACHMANNE.R.IEEETransactionsOnRobotics，2006，22(6)，以及"ApplicationofMIMU/MagnetometerintegratedsystemOntheattitudedeterminationofmicrosatellite，，，SUK.,REND.H.,YOUΖ.,ZHOUQ.,InternationalConferenceonIntelligentMechatronicsandAutomation，2004年8月，中国成都。结果，在传感器之一上实际发生了扰动的情况下，滤波器提供的测量结果是有误差的，但滤波器将认为它是正确的。此时取向的估计变得不正确。因此不论自由度的数目为几个，都不可能忽略扰动并以期望精度获得估计。因此大大劣化了性能指标，因为受扰动的测量结果是原样地提供给观察器的。因此估计方法使得能够通过检测扰动的存在并通过更新这些测量结果的可信性来将一次性缺点考虑在内，例如，如以下文献中所述“Portableorientationestimationdevicebasedonaccelerometers,magnetometersandgyroscopesensorsforsensornetwork",HARADAT.，UCHINOH.，MORIΤ.，SATOΤ.，IEEEConferenceonMultisensorFusionandIntegrationforIntelligentSystems,2003。这种方法提供了用于检测测量结果中的扰动的额外步骤。在检测到扰动时，对应测量结果中的信任度最小化。因此在估计取向时不考虑包括扰动的测量结果提供的信息。因此取向的估计仅依赖于由其他传感器提供的测量结果。现在，在来自若干传感器的测量结果同时表现出扰动的情况下，观察器不再有充分多信息来提供正确的取向估计。最后，在如下文献中描述了一种方法^nertialandmagneticsensingofhumanmotion，，,ROETENBERGD.,doctoralthesis,Twenteuniversity,Netherlands,2006以及“Measuringorientationofhumanbodysegmentusingminiaturegyroscopesandaccelerometers，，，PhDThesis,Inertialsensingofhumanmovement,LUINGEH.J.n，2002b，其中，借助于观察器检测扰动的存在并对它们进行估计。为此，放大状态矢量，且测量模型中发生扰动，这变得更接近事实。原则上，这种技术似乎适于具有六或九个自由度的运动情形。然而，因为缺乏可观察性，对扰动和取向的组合估计是困难的。还需要大量的待设置参数，这增大了其实施的复杂性。因此本发明的一个目的是提供一种取向估计方法，不论是否存在固有加速度和磁性扰动，这种方法都提供精确的取向估计，并且与现有方法相比以简化的方式这样做。前述目的是通过一种基于加速度、磁场和转速沿三个空间轴的测量结果估计取向的方法实现的，所述方法包括-用于预处理这些测量结果以检测扰动的存在并估计无扰动的测量结果的步骤，以及-用于基于从预处理步骤获得的测量值估计取向的步骤。该方法未忽略扰动，这意味着估计是无误差的；该方法恒定地估计它们。如果它们存在，它并不像其他估计方法那样拒绝相关的一个或多个测量结果。此外，不在状态矢量或测量模型中并入它们，这简化了模型并且不会导致估计变得不可能的情况。因此在两个相继步骤中估计取向，并可能估计扰动。因此为观察器提供来自加速度计、磁强计和速率陀螺仪的测量结果，测量结果尽可能接近估计取向的理想条件即，分别没有固有加速度、没有磁性扰动且没有偏差。为此，使用在前一时刻估计的取向作为执行对测量结果的预处理的额外信息。因此根据本发明的估计方法使得能够以最佳方式从传感器的测量结果提取物体的取向而不论相关的运动如何。此外，这种方法实施起来简单，且仅包括少量设置参数。有利地，所述观察器为扩展卡尔曼滤波器。可以估计扰动，尤其是固有加速度，这使得能够通过积分和二次积分分别回溯到物体的速度和位置。因此本发明的主题是一种利用物体的沿三个空间轴的总加速度、磁场和转速的测量结果估计所述物体在时间k在空间中的取向的方法，包括以下步骤A-预处理时刻k的所述测量结果，以检测所述测量结果中扰动的存在，所述扰动属于包括所述物体的固有加速度、增加到地球磁场的磁场以及所述转速的测量结果的偏差的组，并估计时间k时的无扰动测量结果，B-由观察器从在步骤A获得的时间k时的估计的无扰动测量结果估计时间k时的取向。步骤A有利地包括Al-预处理所述转速测量结果，A2-检测所述总加速度和磁场的所述测量结果中存在还是不存在扰动，A3-在时间k没有扰动的情况下，估计的时间k时的无扰动测量结果等于时间k时的测量结果，在有扰动的情况下，在时间k估计的无扰动测量结果是基于在前一时间k-Ι时估计的取向计算的。步骤Al包括从转速测量结果减去在预备初始化步骤期间确定的平均偏差。可以通过在给定时间期间固定提供转速测量结果的�？椴⒓扑忝扛鲋嵘献俨饬拷峁档钠骄祷竦谜庖黄骄�。在姿态控制单元由人佩戴的情况下，这种固定需要从人身上去除控制单元以消除人不可避免的震颤。用于预处理加速度和磁场测量结果的步骤A2可以包括-步骤A2.1，包括将所述总加速度测量结果的范数与重力场的范数进行比较的测试，如果时间k时的加速度测量结果的范数与重力场的范数之间的差异的绝对值低于预定阈值，则假设加速度扰动为零，否则假设有扰动，在每个轴上所述扰动等于时间k时所述总加速度的测量结果和在时间k时估计的无扰动加速度测量结果之间的差异，-步骤A2.2，包括将所述磁场的测量结果的范数与地球磁场的范数进行比较的测试，如果所述磁场测量结果的范数与地球磁场的范数之间的差异的绝对值低于预定阈值，则假设磁性扰动为零，否则假设每个轴上所述磁性扰动等于时间k时所述磁场测量结果和在时间k时在每个轴上估计的无扰动磁场测量结果之间的差异。在步骤A2.1中，有利地对时间k-Ι时估计的扰动进行额外测试如果时间k时总加速度的测量结果的范数与重力场的范数之间的差异的绝对值低于预定阈值，则执行检查，以确定在时间k-Ι时估计的加速度扰动的范数是否低于预定阈值，如果该测试是肯定的，则假设加速度扰动在时间k实际为零，和/或在步骤A2.2中进行对时间k-Ι时估计的磁性扰动的额外测试如果磁场测量结果的范数与地球磁场的范数之间的差异的绝对值低于预定阈值，则执行检查，以确定在时间k-Ι时估计的磁性扰动的绝对值是否低于预定阈值，如果本测试是肯定的，则假设磁性扰动在时间k实际为零。该额外步骤使得能够改善估计方法的精确度。不过，在已知估计取向可能漂移的不利的使用情况下，仅在有限的时间窗口上确保了步骤A2.1和A2.2的这些比较测试的鲁棒性，所述时间窗口根据环境而不同。因此，有利地在说明书中稍后指出的时间带上执行比较测试。于是，使用K，、知,*表示完成预处理步骤时获得的测量结果，它们被称为估计的无扰动的测量结果-仅利用加速度测量结果的范数执行固有加速度的检测；如果在滑动时间窗口Ta的测量结果的至少一个上该范数与(^(地球重力)的范数不同，那么认为当前时刻的测量结果受到扰动；-通过类似方式执行磁性扰动的检测〇如果在滑动时间窗口Tm的测量结果的至少一个上磁性测量结果的范数与Htl(地球磁场)的范数不同〇或者，如果磁性测量结果和无扰动加速度测量结果的相反数-兄之间的角度与矢量和Htl之间的角度不同，此时，当前时刻的测量结果受到磁性扰动。Ta可以是恒定值的参数，而Tm的值可以与运动速度相关联。利用可能位于同一设备上并由用户激活的该变量，不需要查找时间k-Ι时的测量结果的值，因此消除了漂移。步骤B中使用的观察器优选为迅速而容易的扩展卡尔曼滤波器。用于从在时间k时估计的测量结果估计取向的步骤B可以包括-从在时间k-Ι后验估计的状态矢量估计时间k的先验状态矢量，-从时间k的先验状态矢量的估计(称为测量结果的先验估计)来估计时间k的先验测量结果，-通过计算在时间k估计的无扰动测量结果和先验估计测量结果之间的差异来计算扩展卡尔曼滤波器的增益并计算创新(innovation)，-通过用所述增益和所述创新校正在时间k先验估计的状态矢量来计算时间k时的估计取向。所述扩展卡尔曼滤波器中使用的状态矢量可以包含角速度和取向四元数的元。扩展卡尔曼滤波器中使用的状态矢量有利地仅包含取向四元数的元，这样能够简化状态结构和测量模型。本发明的另一主题是一种姿态控制单元，包括适于提供沿三个空间轴的加速度测量结果的�？椋饬垦厝隹占渲岬拇懦〉哪？楹筒饬垦厝隹占渲岬淖俚哪？�(所述�？橐庠诩尤胛锾宓脑硕�)，以及基于所述测量�？樘峁┑牟饬拷峁垂兰剖奔鋕时的取向的模块，所述估计�？榘�-预处理所述加速度、磁场和转速测量结果的�？椋鲈ご砟？槭视诩觳馑霾饬拷峁腥哦拇嬖诓⑻峁┕兰频奈奕哦铀俣炔饬拷峁�、估计的无扰动磁性测量结果和无偏差转速，-由观察器从预处理模块提供的测量结果估计时刻k时的取向的�？�。本观察器可以是扩展卡尔曼滤波器。根据本发明的姿态控制单元还可以包括计算控制中心的初始化期间转速测量�？榈钠骄畹哪？�。预处理模块包括用于检测所述加速度测量结果中固有加速度的存在的�？楹陀糜诩觳馑龃懦〔饬拷峁写判匀哦拇嬖诘哪？�。根据本发明的姿态控制单元还可以包括估计固有加速度以及计算物体的速度和位置的�？�。有利地，适于提供沿三个空间轴的总加速度测量结果、磁场测量结果和转速测量结果的�？槲狹EMS传感器。从以下描述和附图中将更好地理解本发明，其中-图1是时间k时的根据本发明的方法的流程图，-图2A到图2C示出了根据本发明用于预处理分别来自加速度计、速率陀螺仪和磁强计的测量结果的步骤的详细流程图。目的是获得在空间中活动的物体的取向，例如人的取向。为此，使用姿态控制单元，其包括适于提供沿三个空间轴的总加速度、磁场和转速的测量结果的传感器。有利地，传感器是MEMS传感器，其具有降低的成本价格和有限的覆盖面积。对于加速度测量而言，例如，可以是提供每个轴上的测量结果的三轴加速度计或三个单轴加速度计。类似地，对于磁场测量而言，例如，可以是三轴磁强计或三个单轴磁强计。对于转速测量而言，例如，可以是三个单轴速率陀螺仪，或有利地，是两个双轴速率陀螺仪。三个轴可以对准或不对准，但在后一种情况下，轴间的相对取向必须是已知的。在下文中，在描述中为了简单起见，将把一个或多个加速度计称为加速度计，把一个或多个磁强计称为磁强计，把一个或多个速率陀螺仪称为速率陀螺仪。这些传感器附着于取向已知的物体。我们仅有测量结果y，将通过下式进行建模'yA=~R.G0+a+vA权利要求1.一种利用物体沿三个空间轴的总加速度(yA)、磁场(yM)和转速(ye)的测量结果来估计所述物体在时间k时在空间中的取向的方法，所述方法包括A-对时刻k时的所述测量结果(yA，yM，yG)进行预处理，以检测所述测量结果中扰动的存在，并估计时间k时的无扰动测量结果，其中所述扰动来自包括以下各项的组所述物体的固有加速度、增加到地球磁场的磁场以及所述转速的测量结果的偏差，B-由观察器利用在步骤A获得的时间k时的估计的无扰动测量结果^m,k,来估计时间k时的取向。2.根据权利要求1所述的方法，其特征在于，估计物体在时间k时在空间中的取向仅利用所述物体沿三个空间轴的所述总加速度(yA)、磁场(y)和转速(ye)的测量结果。3.根据权利要求1所述的方法，其特征在于，步骤A包括Al-对所述转速(ye)的测量结果进行预处理，A2-检测所述总加速度和所述磁场的所述测量结果(yA，yM)中是否存在扰动，A3-在时间k时不存在扰动的情况下，时间k时的估计的无扰动测量结果(3^，*，yM,k)等于时间k时的测量结果，而在存在扰动的情况下，在时间k时估计的无扰动测量结果(y^k，)是基于在时间k-i时估计的取向计算的。4.根据权利要求3所述的方法，其特征在于，步骤Al包括从转速的测量结果减去在预备初始化步骤期间确定的平均偏差(Swerage)。5.根据权利要求4所述的方法，其特征在于，通过在给定时间期间固定提供转速的测量结果的�？椴⒓扑忝扛鲋嵘纤鲎俚牟饬拷峁闹档钠骄道椿竦盟銎骄畎�(baverage)。6.根据权利要求3所述的方法，其特征在于，所述步骤A2包括A2.1-将所述总加速度的测量结果的范数与重力场()的范数进行比较的测试，其特征在于，如果时间k时的所述加速度的测量结果的范数与重力场(Gtl)的范数之间的差异的绝对值低于预定阈值(αA)，则假设加速度扰动为零，否则假设存在扰动，在每个轴上所述扰动等于时间k时的所述总加速度的测量结果和在时间k时估计的无扰动加速度第测量结果之间的差异，A2.2-将所述磁场的测量结果的范数与所述地球磁场(Htl)的范数进行比较的测试，其特征在于，如果所述磁场的测量结果的范数与所述地球磁场的范数之间的差异的绝对值低于预定阈值(αM)，则假设磁性扰动为零，否则假设每个轴上的所述磁性扰动等于时间k时的所述磁场的测量结果和在时间k时估计的无扰动磁场的测量结果之间的差异。7.根据权利要求6所述的方法，其特征在于，在步骤A2.1中对时间k-Ι时的估计的扰动1)进行额外的测试如果时间k时的所述总加速度的测量结果的范数与所述重力场(G0)的范数之间的差异的绝对值低于预定阈值(αΑ)，则执行检查，以确定时间k-Ι时的估计的加速度扰动的范数是否低于预定阈值(βA)，如果该测试是肯定的，则假设加速度扰动在时间k时实际为零，和/或在步骤A2.2中对时间k-Ι时的估计的磁性扰动(i-i)进行额外的测试，其特征在于，如果所述磁场的测量结果的范数与所述地球磁场(Htl)的范数之间的差异的绝对值低于预定阈值(αM)，则检查时间k-Ι时的估计的磁性扰动的绝对值是否低于预定阈值(βM)，其特征在于，如果该测试是肯定的，则假设所述磁性扰动在时间k时实际为零。8.根据权利要求3所述的方法，其特征在于，在用户设置的时间窗口(TA，TM)上执行步骤A2中的至少一个检测。9.根据权利要求8所述的方法，其特征在于，通过以下形式的处理进行对所述固有加速度的检测10.根据权利要求8所述的方法，其特征在于，通过以下形式的处理执行对磁性扰动的检测11.根据权利要求9所述的方法，其特征在于，还在固有加速度的检测的输出处计算角度叩/e(-h，fc，;v，t)，然后通过以下形式的处理进行对磁性扰动的检测If存在固有加速度T=TIf对于至少一个值tk而言IIlyM,kIl-IlHJII>aM或|uk-uQ|>au，使得tke[tk-TM；12.根据权利要求1到11中的任一项所述的方法，其特征在于，步骤B中使用的观察器为扩展卡尔曼滤波器。13.根据权利要求12所述的方法，其特征在于，用于利用在时间k时估计的无扰动测量结果来估计所述取向的步骤B包括-利用在时间k-Ι时后验估计的状态矢量(^w)来估计时间k时的先验状态矢量(《)，-利用时间k时的先验状态矢量的估计来估计时间k时的先验测量结果(K)，-通过计算在时间k时估计的所述无扰动测量结果和估计的所述先验测量结果之间的差异来计算所述扩展卡尔曼滤波器的增益(Kk)和创新(Ik)，-通过用所述增益和所述创新对在时间k时先验估计的状态矢量进行校正来计算时间k时的估计取向(也)。14.根据权利要求13所述的方法，其特征在于，所述扩展卡尔曼滤波器中使用的状态矢量包含角速度的元和取向四元数的元。15.根据权利要求14所述的方法，其特征在于，所述扩展卡尔曼滤波器中使用的状态矢量仅包含取向四元数的元。16.一种姿态控制系统，至少包括用于提供沿三个空间轴的加速度的测量结果(yA)的感测单元、用于测量沿三个空间轴的磁场(yM)的感测单元、用于测量沿三个空间轴的转速(yG)的感测单元、以及用于基于所述感测单元提供的测量结果来估计时间k的取向的处理单元，所述控制系统包括-用于对所述加速度(yA)、磁场(y)和转速(ye)的测量结果进行预处理以及用于提供估计的无扰动加速度测量结果(％，”、估计的无扰动磁性测量结果(”和无偏差的转速(3^)的子单元，其中所述预处理子单元适于检测所述测量结果中扰动的存在，所述扰动来自包括以下各项的组所述物体的固有加速度、增加到所述地球磁场的磁场以及所述转速的测量结果的偏差，-用于由观察器利用所述预处理�？樘峁┑墓兰频奈奕哦铀俣炔饬拷峁�、估计的无扰动磁性测量结果和无偏差的转速测量结果来估计时刻k的取向的子单元。17.根据权利要求16所述的姿态控制系统，还包括用于计算在控制系统的初始化步骤期间转速测量�？榈钠骄�(^werage)的�？�。18.根据权利要求16所述的姿态控制系统，其特征在于，预处理子单元包括用于检测所述加速度的测量结果中固有加速度的存在的�？楹陀糜诩觳馑龃懦〉牟饬拷峁写判匀哦拇嬖诘哪？�。19.根据权利要求18所述的姿态控制系统，其特征在于，所述用于检测所述加速度的测量结果中固有加速度的存在的�？楹退鲇糜诩觳馑龃懦〉牟饬拷峁写判匀哦拇嬖诘哪？樵谝桓龌蚨喔鍪奔浯翱谀谥葱姓庑┘觳�。20.根据权利要求16所述的姿态控制系统，还包括用于估计所述固有加速度和所述磁性扰动并用于计算所述物体的速度和位置的�？�。21.根据权利要求16所述的姿态控制系统，其特征在于，所述观察器是扩展卡尔曼滤波器。22.根据权利要求16所述的姿态控制单元，其特征在于，用于提供沿三个空间轴的总加速度的测量结果(yA)、磁场的测量结果(yM)和转速的测量结果(ye)的感测单元是MEMS传感器。全文摘要本发明涉及一种利用物体沿三个空间轴的总加速度(yA)、磁场(yM)和转速(yG)的测量结果来估计所述物体在时间k时在空间中的取向的方法，所述方法包括A-预处理(200)时刻k时的所述测量结果(yA，yM＞yG)，以检测所述测量结果中扰动的存在并估计时间k时的无扰动测量结果，B-由观察器从在步骤A获得的时间k时的无扰动测量结果(公式(II))估计时间k时的取向(公式(I))。文档编号G01C17/38GK102308183SQ200980132305公开日2012年1月4日申请日期2009年7月17日优先权日2008年7月18日发明者A·瓦西列夫,C·巴松皮埃尔申请人:原子能和辅助替代能源委员会,莫韦公司
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
