CN103314274B - 活动元件或物体的轨迹的估算方法及系统 - Google Patents

Abstract

借助于传感器组件对活动元件或物体的轨迹的估算方法包括以下的步骤：接收由加速计提供的加速度值；接收由陀螺测量仪提供的角速度值；处理所提供的值，用以一方面借助于角速度值估算至少一角位置值，和另一方面，借助于加速度值和之前估算的角位置值估算限定活动元件或物体的轨迹的至少两个笛卡尔坐标位置值。所述方法还包括以下步骤：通过在轨迹的理论知识的约束下所估算的轨迹的旋转修正模型的反转，估算旋转参数；通过借助于所估算的旋转参数，在所述至少一估算的角位置值上应用旋转，追溯性校正所述至少一估算的角位置值，以提供至少一校正的角位置值。

Description

活动元件或物体的轨迹的估算方法及系统

技术领域

本发明提出活动元件或物体的一种轨迹估算系统，所述轨迹估算系统包括传感器组件，所述传感器组件包括相固连的加速计和陀螺测量仪。

元件的轨迹是元件的一组相继的笛卡尔坐标位置Position_C。

本发明尤其涉及在行人导航的范围中对该轨迹或定向的估算。该领域正值发展，具有全面专于在建筑物内部的导航和定位领域的至少一国际性会议（InternationalConferenceonIndoorPositionningandIndoorNavigation－IPIN，文章在IEEEXplore中发布）。

背景技术

本发明处在具有两种测量方式的一背景中，两种测量方式即加速计和陀螺测量仪（AG）。

在没有允许测定北方的外部系统或用以测量地球磁场的磁力计的情况下，不可能保证参考坐标系（repère）是北方坐标系——英语术语为“NorthFrame”，北方坐标系在地球参考系中是固定的坐标系。该坐标系被用在各种运动（行走或跑步）中。该坐标系可被考虑为参考坐标系：定向（角位置）和位置（笛卡尔坐标位置）是相对于该坐标系测量的。

北方坐标系通过其原点O_NF和其轴(x_NF,y_NF,z_NF)来规定，所述轴与地球坐标系的东方、北方和上行垂线（verticale）的轴相重合。其原点O_NF可对应运动开始的位置或所有其它位置。

因此，在该部分中，参照导航坐标系NF，导航坐标系通过围绕竖直轴的一恒定旋转与北方坐标系相联系。在接下来的说明书中，导航坐标系NF被使用作为参考坐标系，和在该导航坐标系NF中给出的测量值具有一指数n。

由传感器组件EC所经历的轨迹可通过在导航坐标系NF中，三维或3D的固有加速度的二重积分复得。

坐标系BF是与传感器相关联的一坐标系。在下文中，在该坐标系中表达的矢量具有一指数b。加速计的测量值在坐标系BF中给出并且重力（或引力）被叠加在固有加速度上。

因此，在实施二重积分之前，需要在坐标系NF中表示测量值和补偿重力。

为了实施从坐标系NF到坐标系BF的过渡，需要一过渡公式。如果知晓传感器的定向，可获得该公式。在背景AG中，定向通过对陀螺测量仪的测量值求积分而获得。然而，该积分产生本发明中提出要加以限制的偏差。

在文献中，定向偏差通常被表示为局部坐标系BF相对于坐标系NF的定向错误。从而可考虑，所估算的定向错误或偏差使局部坐标系BF的数据不是偏向导航坐标系NF，而是偏向另一导航坐标系NF′，导航坐标系NF′受偏差影响并在下文的说明中被定义。

受偏差影响的坐标系NF′通过其相对于与地球坐标系NF相关联的导航坐标系或固定总坐标系的定向来规定。NF′相对于NF的这种定向与通过给陀螺测量仪求积分所估算的定向偏差相关联。因此，坐标系NF′相对于坐标系NF的定向随时间缓慢偏移。在该坐标系NF′中给出的测量值具有一指数n′。

如果通过四元数表示坐标系NF在坐标系NF′中的定向，通过四元数q_bn表示坐标系NF在坐标系BF中的定向，通过四元数表示坐标系NF′在坐标系BF中的定向，从而可以写如下方程式：

$q_{\mathrm{bn}}\left(t\right)=a_{{\mathrm{bn}}^{\′}}\left(t\right)\⊗q_{n^{\′}n}\left(t\right),$

符号是与四元数相关联的积的符号。

在常见的一方法中，可利用通过加速计和陀螺测量仪测量的惯性数据估算使用者脚踝的轨迹。

在此情形中，（在脚踝处），行走循环可被分为两阶段：在期间脚部移动的迈步阶段（被称之为SW，用于英语术语“SWingphase”迈步期），和在期间脚部处于地面的阶段即抓地阶段（称之为ST，用于英语术语“STancephase”站立期）。一步伐由相继的两阶段ST和SW组成和行走以相继的步伐为特征。

将对于所探测的第k步的阶段SW的时间间隔记为为了限制通过加速度的二重积分所导致的笛卡尔坐标位置的偏差，仅仅在脚部的移动阶段SW中给加速度求积分，而速度在阶段ST中被置零。这种调整需要对两阶段的精确探测，以确定使用者的脚部是否处于运动中。

行人脚踝的速度可按步进行计算（在执行探测步骤后），而定向或角位置Position_A在每个时标l用从角速度测量值所计算的四元数（其规定总坐标系NF在局部坐标系BF中的定向）和如在方程式中的前一定向四元数来测定。

一旦确定在局部坐标系BF中的定向，加速计的测量值仅仅通过在总坐标系NF中的旋转，通过关系式转变，其中，是与四元数相关联的旋转矩阵，如通过以下所规定：

$R_{\mathrm{bn}}=\left[\begin{array}{ccc}{2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},1}^2& {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},2}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn}.3}& {2q}_{\mathrm{bn},1}{2q}_{\mathrm{bn},3}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},2}\\ {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},2}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},3}& {2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},2}^2& {2q}_{\mathrm{bn},2}{q}_{\mathrm{bn},3}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},1}\\ {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},3}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},2}& {2q}_{\mathrm{bn},2}{q}_{\mathrm{bn},3}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn}.,1}& {2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},3}^2\end{array}\right]$

四元数q_nb是q_bn的共轭四元数以及矩阵R_bn是R_nb的反转变换矩阵。

当在坐标系NF中的加速计的测量值aⁿ被计算时，重力应被补偿以计算固有加速度：

实际上，由于陀螺测量仪G的测量值的噪音和偏差，角位置的四元数相对于其“实际”值发生偏差。此外，通过关系式所获得角位置的四元数需要知悉初始条件。因此，由于为获得定向对陀螺测量仪的求积分导致的偏差，固有加速度被破坏。

加速计的测量值，和以及甚至固有加速度被噪音和有时被偏移或英语术语“offset”改变。

即便固有加速度用“完美的”角位置四元数进行计算，对该偏移和该噪音的二重积分导致在所估算的位置上，以及在速度上的错误。

此外，由于四元数的偏差，所补偿的重力场并不等于gⁿ=[00-1]^T，这是因为并不处于总坐标系NF中，而是处于坐标系NF′中。

在建筑物中，即在室内或英语术语“indoor”中的导航和定位的主题是文献中的一常见问题。在2010年，设立了完全专于室内导航和定位的会议（IPIN，用于“InternationalConferenceonIndoorPositionningandIndoorNavigation”，IEEE）。在所展示的文章中，九篇文章致力于带有固定在脚部的运动捕捉系统的行人导航或英语术语“footmountedpedestriannavigation”。接下来是对这些交流以及其它交流的概述。

第一篇文章"AHighPrecisionReferenceDataSetforPedestrianNavigationusingFoot-MountedInertialSensors"，作者MichaelAngermann，PatrickRobertson，ThomasKemptner和MohammedKhider，IPIN会议，苏黎世，2010，该文章展示一种数据采集方法，所述数据采集方法基于光学参考系统和固定在脚部上的惯性测量单元，未涉及对轨迹估算的问题。

第二篇文章，"EvaluationofZero-VelocityDetectorsforFoot-MountedInertialavigationSystems"，作者IsaacSkog，John-OlofNilsson和PeterIPIN会议，苏黎世，2010，该文章关注于对于配有惯性导航仪的脚部的零速运动阶段的探测问题("zerovelocitydetectorsforafootmountedinertialsensorbasedpedestriannavigationsystem")。在位置错误的标准上对四种零速探测技术进行比较。未涉及由偏移和测量值偏差所导致的位置偏差问题。

第三篇文章，"ComparisonandEvaluationofAccelerationBasedStepLengthEstimatorsforHandheldDevices"，作者JasperJahn，JochenSeitz，LucilaPatino-Studencka，UlrichBatzer和JavierGutiérrezBoronat，IPIN会议，苏黎世，2010，该文章比较多种从加速度测量值估算行人步伐长度的技术，未涉及偏差和修正问题。

第四篇文章，"Animprovedshoe-mountedinertialnavigationsystem"，作者NadirCastaneda和SylvieLamy-Perbal，IPIN会议，苏黎世，2010，该文章提出一种改良的行人跟踪系统，所述系统使用在使用者的鞋的位置安装的惯性传感器。所述系统基于模糊逻辑实施探测抓地阶段即"STancephase"的程序。设置间接卡尔曼滤波器IKF（并不直接估算状态，而是估算在状态上的错误，如在第五篇文章中，"PedestrianInertialNavigationwithGaitPhaseDetectionAssistedZeroVelocityUpdatingYoungSooSuh，SangkyungPark"，4thInternationalConferenceonAutonomousRobotsandAgents，Feb10-12，2009，威灵顿，新西兰，在其中，在行人导航范围内设置间接卡尔曼滤波器IKF，该文章使用惯性/磁力传感器、和分别在鞋头和鞋下方定位的力传感器）。惯性系统的构成并未明确进行描述，和设置卡尔曼滤波器。

第六篇文章，"Context-AdaptiveAlgorithmstoImproveIndoorPositioningwithInertialSensors"，作者UlrichWalder，ThomasBernoulli，GeraldGlanzer，和ThomasWieβflecker，IPIN会议，苏黎世，2010，该文章观察到，如果只是设置对加速度进行二重积分，对于在建筑物内部的行人导航，对（在身体上安装的）惯性系统的定位精度较弱。对零速阶段的探测上和地图的使用上分别提出两种改进方案，来修正轨迹或减小偏差。

第七篇文章，"DualIMUIndoorNavigationwithParticleFilterbasedMap-MatchingonaSmartphone"，作者C.Ascher，C.Kessler，M.Wankerl，G.F.Trommer，IPIN会议，苏黎世，2010，该文章用对在智能电话或英语术语"smartphone"中装载的地图上的校准来论述在建筑物内部的导航。使用两个低成本惯性测量单元、电子罗盘和高程计。安置微粒过滤器以对信息进行融合。设置零速阶段的探测。利用联合地图的构造和定位的算法（SLAMalgorithm—SimultaneousLocalisationAndMapping），即便局部地，允许构建地图的传感器可能是不可用的。

在第八篇文章"Designchoices，filterparametertuning，andcalibrationofzero-velocityupdateaidedinertialnavigationsystemsforpedestriannavigation"，作者John-OlofNilsson，IsaacSkog，和PeterIPIN会议，苏黎世，2010，该文章述及具有零速阶段探测（ZUPT："Zerovelocityupdate"）的惯性导航系统的设计的问题。所述系统使用扩展卡尔曼滤波器EKF和探测器ZUPT。作者关注于对设计的选择（传感器的布置，规格）和对于卡尔曼滤波器要调节的参数。在文献中，在位置的平均错误上的结果显示出相对于平常所遇到的结果而言良好的结果。

第九篇文章，"PedestrianIndoorNavigationbyaidingaFoot-mountedIMUwithRFIDSignalStrengthMeasurements"，作者AntonioR.Jiménez，FernandoSeco，J.CarlosPrieto和JorgeGuevara，IPIN会议，苏黎世，2010，该文章实施与行人脚部相连结的一惯性测量单元和RFID标签。利用零速阶段的探测和安置逆向卡尔曼滤波器以补偿位置、速度、姿态或角位置的错误。所设置的技术对于不同类型的运动是有效的（侧，后，步行，跑步）和在步伐线外不需要校准阶段，也不需要减弱环境中的无线电信号。借助于使用磁力计，减小方向偏差。

第十篇文章，"Ontheuseoffoot-mountedINS，UWB-rangingandopportunisticcooperationinhigh-accuracyindoorpositioningsystems"，作者PeterJouniRantakokko和ErikaEmilsson，IPIN会议，苏黎世，2010，该文章使惯性导航仪和超宽带无线电（IR-UWB）耦合以减小位置错误。用于定位的系统从而比包括加速计和陀螺测量仪的简单装置更为丰富和更为复杂。

第十一篇文章，"AGPS/INS-basedarchitectureforrescueteammonitoring"作者AlbertoCroci，MattiaDeAgostino，和AmbrogioManzino，IPIN会议，苏黎世，2010，该文章针对通过卫星/惯性导航仪设置一种混合型导航系统：使用惯性系统来计算步伐和测量一步伐的时间，使用磁力计和陀螺测量仪来估算移动的方向。当GPS数据足够可靠时，使用GPS数据来修正偏差（位置偏差）。

其它的文章，如第十二篇文章，"TowardsReal-TimeCameraEgomotionEstimationandThree-DimensionalSceneAcquisitionfromMonocularImageStreams"，作者Aufderheide和WernerKrybus，IPIN会议，苏黎世，2010，该文章使用可视系统来补偿由固有加速度的二重积分所引起的偏差，或使用具有外部交变磁场的系统，如在第十三篇文章中所描述的，"PositionEstimationUsingArtificialGeneratedMagneticFields"，作者Blankenbach和AbdelmoumenNorrdine，IPIN会议，苏黎世，2010。

对了概述前述十三篇文章，要么是使用与惯性导航仪（包括地图，GPS，UWB）相互补的一系统来补偿位置偏差，要么是设置卡尔曼滤波器（IKF，EKF），要么是明确地使用由磁力计所提供的测量值。

文献FR2918745提出一种个人导航辅助装置，所述装置包括至少一计算机，所述计算机包括：存入储存器的、个人路线被设置在其中的数字位置地图；人机界面；由个人负持并给送关于个人运动的信息的传感器组件。传感器组件例如包括惯性导航仪。由使用者负持的传感器例如是加速计、磁力计、陀螺测量仪和气压计。对要经过的路程的计算例如根据所到达的路程的被识别的中间对象来变更，这些对象在数字地图上做出标记。在此情形中，存在对中间对象的偏差的修正。本发明的装置不需要位置地图。本发明的装置不再需要相对于被识别的中间通行点的“手动校准”。

文献US20070018890提出一种室内和室外导航装置。便携版本（行人导航）融合由多个传感器、GPS、测步仪所提供的和/或从具有两测量轴的加速计和数字罗盘所实施的信息。传感器布置在向导犬的背部上。室内定位通过使用在许多环境中由于存在WiFi网络而可用的无线连接进行。信息融合在Dempster-Shafer理论的范围内实施。所述系统不使用陀螺测量仪。

文献EP1488197结合GPS装置和导航估算单元或英语术语"DeadReckoningUnit"，导航系统用于视力不佳的人。在所提出的实施方式中，系统还包括用于图像捕捉的数字摄像机。还利用环境的数字地图。导航估算单元或DRU——DeadReckoningUnit，包括加速计、陀螺测量仪和微控制器；导航估算单元在GPS信号丢失时接替或用于改进通过GPS所获得的定位精度。DRU允许估算所经过的距离和方向的变化，通过将这些信息与数字地图相结合，即便在丢失GPS信号的情况下。可对使用者进行定位。距离利用所实施的步数和一步伐的平均长度进行估算。与使用者相关的方向借助于陀螺仪进行估算，这是因为其较不敏感于磁干扰和是精确的。作者指出，可使用允许估算所经过的距离和方向的相对变化的其它传感器，以及其装置并不局限于加速计和磁力计的使用。陀螺计的输出值被求积分以提供被编码的角度值，例如呈四位，这提供16个可能的角度值，和22.5°的分辨率。未涉及由陀螺测量仪的求积分所产生的偏差问题，这可能源于使用集成有高精度陀螺仪的一装置。

文献US6323807示出具有“被动”传感器的室内或indoor导航系统。第一统计模型利用在已知地点进行的测量值构建。第二统计模型利用在未知点的测量值获取。计算机确定概率，以使得一未知的笛卡尔坐标位置实际上对应一已知位置。传感器包含三轴加速计、三轴磁力计、光度探测器、和温度传感器。也可添加低分辨率摄像机、湿度测定传感器、陀螺计——以测量倾度、气体探测器、和高程计。系统需要带有特征提取的一学习阶段。这继而从属于一分类系统。

第十四篇文章"GaitAssessmentinParkinson’sDisease:TowardanAmbulatorySystemforLong-TermMonitoringArashSalarian"，作者HeikeRussmann，J.G.Vingerhoets，CatherineDehollain，YvesBlanc，PierreR.Burkhard和KamiarAminian，IEEETransactions，BiomedicalEngineering，卷.51(8)，1434-1443页，2004年，该文章尤其公开一种测量系统，所述测量系统通过陀螺计测量人体某段的转动速度，以用于帕金森症患者。陀螺测速测量值的偏差问题通过预备于各种其它处理的高通滤波器进行处理。本发明不需要通过高通滤波进行的这类预处理阶段。此外，本发明使用测速计，这在该文章中未进行描述。

第十五篇文章，EstimationandVisualizationofSagittalKinematicsofLowerLimbsOrientationUsingBody-FixedSensorsHoomanDejnabadi，BrigitteM.Jolles，EmilioCasanova，PascalFua，KamiarAminian，IEEETransactionsOnBiomedicalEngineering，卷53(7)，1385-1393页，2006年7月，该文章公开用于估算小腿的定向（传感器在胫部固定）的一种陀螺计和一种双测量轴式加速计。来自这些传感器的数据融合使用一生物动力限制和步行模型。数据处理模式与本发明的模式完全不同。实际上，所提出的方法需要将传感器精确地在使用者的胫部定位，这不是本发明的情形。作者考虑在陀螺测速测量值中存在的偏差。当脚部是固定的时，胫部相对于水平线的角度直接地通过双轴加速计所提供的测量值获取。从胫部处的可用的测量值，计算对应的测量值——应在脚踝的位置测量的。对在通过由陀螺计所提供的测量值求积分而在角度计算中所引起的角度偏差的估算通过二阶巴特沃兹滤波器来实施低通滤波，所述滤波器应用在通过在并不精确知晓该角度的时刻的求积分所获得的角度上。继而，在已知的时刻应用三次样条插值或"piecewisecubichermiteinterpolation"，以找到对角度偏差的估算，而无超出也无振荡，同时保留其单调性。通过求积分所获得的角度继而被减去所估算的角度偏差，以获得无偏差的角度估算。

第十六篇文章，"Apracticalgaitanalysissystemusinggyroscopes"，作者KaiyuTong和MalcolmH.Granat，MedicalEngineering&Physics，卷21，87-94页，1999年，该文章述及通过使用陀螺计对步行进行分析。在使用者的胫部上，在脚踝附近布置一单轴陀螺计。胫部的倾度通过对由陀螺计所提供的测量值求积分获得。陀螺计的初始偏差通过当使用者处于息止状态时，在五秒的时窗上计算由陀螺计所给送的信号的平均值来获得。在步伐的一特定时刻即在抓地阶段或英语术语"mid-stance"中，对在下肢的段的一特别定向上的假设，允许重置角度为零，这允许在步伐末消除角度偏差。所测试的另一方法在于在胫部的倾度信号上应用高通滤波，以限制偏差和去除偏移。该文章集中在对角度的估算上和不是集中在对轨迹的估算上。因此，对角度偏差的校正不考虑在轨迹上的偏差。

第十七篇文章，"Threedimensionalinertialsensingoffootmovementsforautomatictuningofatwo-channelimplantabledrop-footstimulator"，作者P.H.Veltink，P.Slycke，J.Hemssems，R.Buschman，G.Bultstra和H.Hermens，MedicalEngineering&Physics，卷25，21-28页，2003年，该文章关注脚部的三维运动，以安置激励器。脚部的笛卡尔坐标位置和定向或角位置利用三轴的加速度和磁力的测量值进行估算。通过陀螺测量仪所提供的测量值被求积分以确定定向，和位置通过置于惯性坐标系中的加速度的二重积分获得。所公开的是，由于认知不佳的偏差和估算不佳的增益，这些求积分阶段会导致对于定向和位置的估算而言错误的一结果。还描述的是，对于人类的步行，特定的初始和最终条件可被考虑，例如脚部在地面上平直置位，在步伐开始和结束相同的垂直位置。此外，在一步伐的时间上，求积分阶段被限制。对于估算脚部在三维方面的位置和定向所提出的方法如下：在人不移动的静态阶段时，传感器在脚部上的定向在开始时被确定。这允许在传感器的坐标系中测量重力场。一种步伐探测算法允许规定一步伐的开始和结束。在每个步伐结束时，角速度的测量值被求积分和通过使用关于步伐开始和结束时的角度的条件，偏差被补偿。根据时间所获得的定向继而被使用于在惯性坐标系中表示加速度。重力场继而被减去，这提供固有加速度。固有加速度被求两次积分以确定位置，并利用在步伐开始和结束时的条件以消除位置偏差。通过第十七篇文章的图1所示的该偏差校正方法与本发明的方法完全不同。

第十八篇文章，“Self-containedPositionTrackingofHumanMovementUsingSmallInertial/MagneticSensorModules”，作者YunXiaoping，E.R.Bachmann，H.Moore，和J.Calusdian，IEEEInternationalConferenceonRoboticsandAutomation，2007年，该文章展示一种个人导航系统，所述导航系统不需要特别的基础结构。通过在处于移动的个人的四肢上固定的一组惯性/磁力传感器获得继续的姿态。所考虑的传感器模块包括三个陀螺测量仪、三个加速计、和三个磁力计（这些传感器是三轴的以及三轴的每个呈直角安装）。作者标记由加速计引起的偏差：作者从而利用步伐的特性（即，零速阶段），以校正由加速计所发送的测量值。从活动坐标系到固定坐标系的过渡通过使用由传感器模块直接提供的姿态（以四元数的形式给出）来实施，传感器模块集成有一卡尔曼滤波器。描述未提供关于被使用于计算该定向的传感器的精确信息。

第十九篇文章，“Spatio-temporalparametersofgaitmeasuredbyanambulatorysystemusingminiaturegyroscopes”，作者K.Aminian，B.Najafia，C.Bülab，P.-F.Leyvrazc和Ph.Robert，在JournalofBiomechanics中，卷35(5)，689-699页，2002年，该文章描述一种数据采集系统，用于估算步行的时空参数。为此，作者利用在下肢位置的角速度的测量值。为了有效，测量装置利用陀螺测量仪和力传感器。对参数的估算通过在子波方面的变换式进行。这里从而非常远离如在本发明中所提出的导航系统。此外，作者未涉及陀螺测量仪的偏差，这是因为其通过在子波方面的变换式预处理数据。

这些不同的系统具有有限的精度和高成本。这些不同的系统并未提出通过考虑关于轨迹的信息从整体上解决定向偏差的问题。这些不同的系统通常需要的传感器数大于本发明的传感器数。

发明内容

根据本发明的一方面，提出估算活动元件或物体的轨迹的一种估算系统，其包括传感器组件，传感器组件包括相固连的加速计和陀螺测量仪。所述系统包括追溯性校正部件，对于在一时间间隔的多个相继时刻中的至少一个时刻利用陀螺测量仪的测量值得到的角位置，所述校正部件利用加速计在至少两个所述相继时刻的笛卡尔坐标位置和表示加速计A在所述时间间隔期间的轨迹的预定信息，并通过合适的旋转，对所述角位置进行追溯性校正，所述旋转用于最小化在所述预定信息与旋转后所获得的轨迹上的对应信息之间的差异。

预定信息可与轨迹的定向相关。该定向可从所估算的轨迹进行计算。例如，如果预定信息是相对于北方（或步行方向）的步伐方向的定向，那么在轨迹上的对应信息例如通过取用在北-南方向与连接步伐轨迹的第一点和最后一点的矢量在水平面上的投影之间的角度进行计算。

这类系统允许通过从获得的轨迹对陀螺测量仪的测量值求积分而作用在所估算的角度上来对偏差进行补偿。这允许从整体上管理偏差问题。

此外，本发明的目的在于获得对轨迹的校正，而不使用来自磁力计的测量数据。校正仅仅基于来自加速计和陀螺测量仪的测量值。

在本发明中，活动坐标系的姿态通过对陀螺测量值的数字积分获得，并且通过考虑在轨迹上的假设，例如行人在水平面中的移动，偏差不是在加速测量值上进行补偿，而是在所估算的姿态上进行补偿。

时间间隔可对应脚部的一行进阶段。

在一实施方式中，所述系统还包括：

-存储部件，存储部件存储在所述时间间隔的所述多个相继时刻、在与传感器组件相关联的活动坐标系中，由加速计给送的第一测量值和由陀螺测量仪给送的第二测量值；

-第一确定部件，第一确定部件从所述第二测量值确定，在所述相继时刻，由相对于基准位置沿与地球坐标系相关联的固定总坐标系的轴的旋转角所规定的陀螺测量仪的所述角位置；以及

-第二确定部件，第二确定部件从所述第一测量值和由所述第一确定部件提供的所述角位置，确定在所述相继时刻在所述固定总坐标系中加速计的所述笛卡尔坐标位置。

因此，可获得轨迹并且可校正在位置上的偏差。

更为特别地，本发明的系统能够估算活动元件或物体的轨迹，通过使用：

-加速计，能够给送数据测量值；陀螺测量仪，能够给送角速度测量值，

-第一估算部件，第一估算部件在一时间间隔的多个相继时刻中的至少两时刻，从由陀螺测量仪测量的角速度估算角位置，

-第二估算部件，第二估算部件从该加速计的测量值和从之前估算的角位置，估算在至少两所述相继时刻包括加速计的一笛卡尔坐标位置的活动元件或物体的轨迹，以及

-第三估算部件，第三估算部件估算合适的旋转，以最小化在表示要估算的轨迹的预定信息与在之前估算的轨迹的旋转之后得到的转变的轨迹上的对应信息之间的差异，

-追溯性校正部件，追溯性校正部件从通过第三估算部件所估算的旋转结果，对通过第一估算部件所估算的在所述至少两个时刻的角位置进行追溯性校正，

-第四估算部件，第四估算部件从该加速计的测量值，和从被校正的角位置，对在至少两个所述相继时刻包括加速计的被校正的笛卡尔坐标位置的活动元件或物体的被校正的轨迹进行估算。

根据一实施方式，所述校正部件适于迭代地校正所述角位置。

因此，精度得到改进。

在一实施方式中，表示加速计的轨迹的所述预定信息包括，在所述时间间隔的所述相继时刻之中，在不同的第一和第二时刻之间，加速计的基本上平面的一轨迹。

因此，取用在运动特别是人或动物或机器人的运动中共同遇到的特征，并且精度从而得到改进。

例如，在步行中脚踝的运动基本上是平面的。

在一实施方式中，轨迹平面的定向相对于垂线或北方是已知的。

因此，精度得到改进。

根据一实施方式，在所述第一和第二时刻之间的轨迹包括已知的定向和/或已知的斜率。

因此，可取用所存储的、涉及使用者所处位置的斜率或移动方向（在例如在走廊中移动时）的信息，对此，角位置规律地回到同一值。

在一实施方式中，所述第一和第二时刻是所述时间间隔的界限。

因此，在循环运动的情形中，时间间隔对应一循环或一部分循环（例如，在步行的情形中，时间间隔对应于将脚部的两个相继零速时刻分开的阶段）。

此外，如果时间间隔对应于如在步行的情形中被两个静止阶段围绕的一运动阶段（或在步行的情形中的英语术语“MidStance”），对平面运动的假设涉及在一步伐中的各种运动。

根据一实施方式，表示加速计的轨迹的所述预定信息包括所存储的可能的笛卡尔坐标位置。

因此，对轨迹的旋转的计算取用这些可能的通过点。

在一实施方式中，所述追溯性校正部件适于预定地将校正在整个或部分所述多个相继时刻上分布。例如，校正是线性的，甚至等分的。

因此，校正在每个时刻不是相同的，并且可考虑偏差的可能的时间变化。

例如，所述校正部件适于使校正在全部或部分所述多个相继时刻上线性地增大。

校正的线性分布允许不是理论地在校正应被进行的时刻上，而是允许考虑该校正随时间增大的事实，这是当陀螺测量仪具有偏差时的情形。

在一实施方式中，系统适于固定至：使用者；移动终端；或陆地、空中或海上运输工具。

使用者可是有生命的（人或动物），或是人造的。

因此，可估算系统被固定处的轨迹。

根据一实施方式，系统包括在使用者脚踝附近的固定部件。

根据本发明的另一方面，还提出活动元件或物体的轨迹的一种估算方法，在其中，追溯性地从以下相继步骤的结果对角位置进行校正，所述步骤在于：

-在一时间间隔的多个相继时刻中的至少一时刻，估算从陀螺测量仪的测量值所获得的角位置，

-从加速计的测量值和表示在所述时间间隔中加速计的轨迹的预定信息，估算在至少一所述相继时刻与陀螺测量仪相固连的加速计的笛卡尔坐标位置，以及

-估算合适的旋转，以最小化在所述预定信息和在旋转后获得的轨迹上的对应信息之间的差异。

为此，本发明的目标在于借助于传感器组件估算活动元件或物体的轨迹的一种估算方法，该传感器组件固定在活动元件或物体上并包括相固连的加速计和陀螺测量仪，所述方法包括以下的步骤：

-接收由加速计提供的和在一时间间隔种的多个相继时刻中的至少两个时刻测量的加速度值，

-接收由陀螺测量仪提供的和在该时间间隔的所述多个相继时刻的至少两时刻测量的角速度值，

-处理所提供的值，用以一方面借助于角速度值估算至少一角位置值，和另一方面借助于加速度值和所述至少一之前估算的角位置值估算限定活动元件或物体的轨迹的至少两个笛卡尔坐标位置值，

所述方法还包括以下步骤：

-通过在轨迹的理论知识的约束下所估算的轨迹的旋转修正模型的反转（inversion），估算旋转参数，该理论知识由该轨迹的预定参数组成，以及

-通过借助于所估算的旋转参数，在所述至少一估算的角位置值上应用旋转，追溯性校正所述至少一估算的角位置值，以提供所校正的至少一角位置值。

作为选择，追溯性校正的步骤还包括借助于加速度值和所述至少一校正的角位置值对笛卡尔坐标位置值进行追溯性校正。

同样地，作为选择：

-由加速计提供的加速度值和由陀螺测量仪提供的角速度值被表示在与传感器组件相关联的一活动坐标系中，以及

-在对所提供的值进行处理的处理步骤时，对所述至少一角位置值的估算以相对于与地球坐标系相联系的一固定总坐标系的轴的旋转角度参数的形式表示，并且对所述笛卡尔坐标位置值的估算以在该相同的固定总坐标系中以笛卡尔坐标的形式表示。

同样地作为选择，追溯性校正的步骤多次迭代实施，用于对所述至少一估算的角位置值的迭代校正。

同样地作为选择，旋转参数是四元数参数。

同样地作为选择，轨迹的理论知识包括被认为应包含轨迹的一平面的角度定向参数，尤其是相对于垂线或北方的角度定向参数。

同样地作为选择，轨迹的理论知识包括被认为应构成轨迹的组成部分的至少两个点的笛卡尔坐标。

同样地作为选择，轨迹的理论知识包括轨迹的方向或斜率的参数。

同样地作为选择，追溯性校正的步骤被设计以使得借助于所估算的旋转参数所实施的校正预定地分布在整个或部分所述多个相继时刻上。

同样地作为选择，追溯性校正的预定分布被限定以使得借助于所估算的旋转参数计算的校正参数在整个或部分所述多个相继时刻上线性地增大。

本发明的对象还在于一种计算机程序，所述计算机程序可从通信网络加载和/或记录在由计算机可读的介质上和/或可通过处理器运行，所述计算机程序包括指令，所述指令用于当所述程序在计算机上被运行时运行根据本发明的轨迹估算方法的步骤。

最后，本发明的对象还在于活动元件或物体的轨迹的一种估算系统，所述轨迹估算系统包括：

-加速计，加速计适于提供在给定时间间隔的相继时刻测量的加速度值，

-陀螺测量仪，其与加速计相固连，适于提供在该时间间隔的相继时刻测量的角速度值，以及

-处理部件，处理部件处理由加速计和陀螺测量仪所提供的值，用于实施根据本发明的轨迹估算方法。

作为选择，这类的轨迹估算系统可包括固连部件，所述固连部件将加速计和陀螺测量仪固连至：使用者，例如固连至该使用者的脚踝；移动终端；或陆地、空中或海上的运输工具。

附图说明

通过研究作为非限定性示例进行描述的和通过附图示出的数个实施方式，本发明将被更好地理解，附图中：

-图1示出根据本发明的一方面的系统的一实施方式；

-图2a和图2b示出与行人步行相关的一示例；

-图3示出固定总坐标系NF和局部坐标系BF；

-图4示出在固定总坐标系NF和NF′中的轨迹；

-图5示出对定向或角位置的追溯性校正；

-图6a、图6b和图6c示出不校正偏差所估算的轨迹（6a），通过对偏差进行追溯性校正但是是在每个步伐的时间期间应用恒定的角度校正所估算的轨迹（6b），和通过进行递归性校正和线性的追溯性角度校正所估算的轨迹（6c），以及

-图7a和图7b示出图6b和图6c的轨迹在水平面上的投影：粗曲线示出所估算的一个来回，细曲线示出实际实施的路线。

具体实施方式

在所有附图中，具有相同附图标记的元件是相似的。

本发明提出，一但（借助于允许确定步伐的开始与结束的步伐探测算法）探测到一步伐，就对陀螺测量仪的测量值求积分以确定对定向的估算，将加速计的测量值引入惯性坐标系中，补偿重力场，以及对在每个取样时刻所获得的固有加速度求两次积分以在步伐的时间上获得所估算的一轨迹。

对一步伐的理论知识被使用并与所述轨迹相结合，即以表示预定信息的该轨迹的预定参数的形式表示，以确定应用在初始估算的角度上的定向校正（其最小化预定信息与所估算轨迹上的对应信息之间的差异）。

继而，差时地，重新使用由加速计所提供的测量值，这允许估算所校正的轨迹。如果预定信息与轨迹上的对应信息之间的差异超出一阈值，重新开始角度校正的估算循环，继而重新估算轨迹。

在图1上，轨迹的估算系统包括传感器组件EC，传感器组件配有固连连接的加速计A和陀螺测量仪G，加速计具有至少一测量轴，以及陀螺测量仪具有至少一测量轴。

所述系统包括追溯性校正模块CORR_POS_A，追溯性校正模块在与在一时间间隔上取样的时刻对应的多个相继时刻之中，利用加速计A在至少两个所述相继时刻的笛卡尔坐标位置Position_C以及表示在时间间隔[t₁;t₂]期间的加速计A的轨迹的预定信息INF_PRED_TRAJ，通过旋转对陀螺测量仪G的角位置Position_A进行追溯性校正，所述旋转应用于轨迹，允许其更好地校验所述预定信息，即允许最小化所述预定信息INF_PRED_TRAJ与在旋转后所获得的轨迹上的对应信息之间的差异。换句等同的话说，借助于加速计A的测量值所获得的轨迹的旋转修正模型——为使得轨迹尽可能好地对应于预定信息（即以该轨迹的预定参数的形式表示的理论知识）——被反转，以找到该旋转修正的旋转参数，这些旋转参数继而被使用于校正前述的角位置Position_A。

时间间隔由第一界限t₁和之后的第二界限t₂界定。在时间间隔[t₁;t₂]上的取样被加上上标t^l以使得l在1和N之间变化，t¹等于t₁，t^N等于t₂。规定轨迹如相继的笛卡尔坐标位置Position_C组。

界限t₁和t₂可对应脚部的相继的两零速时刻，界定脚部的两零速时刻之间的一阶段即英语术语"MidStance支掌中期"。

所述系统还包括存储模块MEM_A和存储模块MEM_G，存储模块MEM_A存储在所述时间间隔的所述多个相继时刻、在与传感器组件EC相联系的活动坐标系BF中由加速计A给送的第一测量值a^b，和存储模块MEM_G存储在所述时间间隔的所述多个相继时刻、在与传感器组件EC相关联的活动坐标系BF中由陀螺测量仪G给送的第二测量值ω^b。

第一确定模块DET_POS_A利用第二测量值ω^b确定由角度或一四元数所规定的陀螺测量仪G的角位置Position_A，四元数表示允许在所述相继时刻t¹，t²，…t^N-1，t^N从坐标系NF过渡到坐标系BF的旋转。

第二确定模块DET_POS_C利用第一测量值a^b和由第一确定模块DET_POS_A所提供的角位置Position_A，在由坐标系变换模块CR执行的坐标系更换后，确定在所述相继时刻t¹，t²，…t^N-1，t^N，在固定总坐标系NF中，加速计A的笛卡尔坐标位置Position_C，所述坐标系变换模块CR允许从局部坐标系BF过渡到坐标系NF（因为在迭代后，NF′趋于与NF重合，如说明书中后文所述）。

追溯性校正模块CORR_POS_A适于迭代地校正角位置Position_A。校正可以要么被应用在角速度上（即在对陀螺测量仪G的测量值求积分之前），要么应用在角度上（即在对陀螺测量仪G的测量值的求积分之后）。

追溯性校正模块CORR_POS_A还适于在至少一所述相继时刻t¹，t²，…t^N-1，t^N校正笛卡尔坐标位置Position_C。

预定信息INF_PRED_TRAJ可包括，在所述时间间隔[t₁;t₂]的所述相继时刻t¹，t²，…t^N-1，t^N中的不同的一第一时刻和一第二时刻之间的加速计的基本平面的轨迹信息。

轨迹的点所围绕的平面的定向相对于垂线或北方可以是已知的。在所述第一和第二时刻之间的轨迹可以包括已知的朝向和/或已知的斜率。

第一和第二时刻可以是时间间隔的界限。

预定信息INF_PRED_TRAJ可包括存储的可能的笛卡尔坐标位置，即被理论地考虑应构成轨迹的组成部分的点的笛卡尔坐标。

系统可适于被固定在使用者、移动终端、或陆地、空中或海上运输工具上。例如，系统可配有固定在使用者脚踝附近的部件。

本发明与利用由陀螺测量仪和加速计提供的测量值估算实物轨迹的常见方法进行比较，常见方法包括四个步骤：

-对陀螺测量仪测量值求积分，以估算相对于参考坐标系NF与传感器组件EC相联系的坐标系BF的定向；

-将传感器组件EC的坐标系BF的加速计测量值引入参考坐标系NF中；

-对重力进行补偿，以获得实物在参考坐标系中的固有加速度；和

-执行该加速度在时间上的二重积分。

考虑到在测量值上存在的偏差，众所周知的是，这种方法不能长期地提供正确的轨迹。同样地，对于尝试校正轨迹偏差或英语术语“drift”的技术是已知的。

本发明的一独创性来自这样的事实，利用对轨迹的认知（轨迹由在两给定时刻之间的笛卡尔坐标位置组进行规定），确定旋转，所述旋转通过转变轨迹使得能够满足对轨迹的认知，允许通过使用该旋转校正角位置Position_A或定向，继而允许重新计算轨迹。

需要注意的是，校正在给定的时窗上应用。

此外，校正的方法可以是迭代的。

取在图2a和图2b上所示的步行为例。利用在运动上的假定，例如在步伐开始和结束时的高度是相同的，来推断要应用的角度校正θ，以使得在轨迹上的约束条件是令人满意的，这里在时刻t₁的高度等于在时刻t₂的高度。

该校正Pos_A_C以待规定的预定方式，例如线性地，通过由样本数N划分的校正的等分分隔，在利用陀螺测量仪测量值确定的角位置Position_A或姿态上进行分布。继而重新计算轨迹。

在下文规定在该专利申请中所使用的坐标系以及所使用的传感器的模型。

假定，惯性测量单元或IMU用于英语术语"InertialMeasurementUnits"，即传感器组件EC，测量三维或3D矢量。

所有这些矢量在传感器组件EC的坐标系BF中测定，这意味着测量值取决于传感器的姿态或定向。

传感器组件EC固定在活动物体上。因此，由于传感器组件EC的姿态或角位置随时间的改变，瞬时旋转的矢量测量值是必需的。

首先，需要规定分别标记为NF、BF、SF和NF′的四个坐标系。

北方坐标系或固定总坐标系或NF用于英语术语"NorthFrame"，是相对于地球参考系的固定坐标系。其在各种运动（步行或跑步）中被使用。北方坐标系NF可被认为是参考坐标系：定向或角位置以及位置或笛卡尔坐标位置在该坐标系中进行测量。固定总坐标系NF通过其原点O_NF和其轴(x_NF,y_NF,z_NF)进行规定。轴按东、北和上的方向参照地球坐标系。其原点O_NF可对应运动开始的位置或任何其它位置。在该坐标系中所表示的数据用一指数n进行标记。

局部坐标系或与传感器相联系的坐标系或BF用于英语术语"BodyFrame"，是传感器组件EC的参考系，在其中，测量粗略数据。其中心为O_BF。在该坐标系中的数据用一指数b进行标记。其轴(x_BF,y_BF,z_BF)对应在每个时刻传感器的轴（在三轴传感器的情形下）。其原点O_BF在固定总坐标系NF中的坐标对应传感器在固定总坐标系NF中的位置。

中间坐标系或SF用于英语术语"StepFrame"被规定。在本申请中，中间坐标系SF是与地球相联系的一坐标系，不过仅仅是在一时间间隔[t₁,t₂]中（例如在一步伐期间）。该坐标系被使用于具有小于六度的自由度或sixDOF用于英语术语"DegreesOfFreedom"的运动。例如，当传感器组件EC在时间间隔[t₁,t₂]期间具有不变的旋转轴和/或平面运动时使用该坐标系。

x_SF是旋转轴并且传感器组件EC经历在由(y_SF,z_SF)规定的平面中的平面运动。每次平面是竖直的，y_SF被选择以作为水平轴，而轴z_SF是竖直轴。该坐标系SF的原点对应于在时间间隔开始时（例如t=t₁）传感器的位置。在该坐标系中所表示的数据用一指数s进行标记。

坐标系NF′是接近坐标系NF的一坐标系，其考虑源于陀螺测量仪求积分的偏差。假定该偏差为零，NF和NF′是重合的。否则，坐标系NF′通过轴(x_NF'',y_BF',z_BF')和中心O_BF来规定。

坐标系NF′是这样的坐标系，相对于该坐标系表示在进行校正之前的通过给陀螺测量仪求积分所获得的角位置Position_A和通过给固有加速度进行二重积分所估算的笛卡尔坐标位置Position_C。

图3示出固定总坐标系NF和局部坐标系BF。如可以看见，局部坐标系BF通过平移和旋转与固定总坐标系NF相联系。

应计算用于获取在坐标系NF中通过传感器组件EC测量的数据所需的坐标系间的旋转。为此，规定旋转矩阵R_nb，该旋转矩阵通过以下关系式，使在与传感器组件相联系的坐标系BF中表示的一矢量x过渡至在固定总坐标系NF中表示的相同矢量：

xⁿ=R_nbx^b(1)

反转变换矩阵是

固定坐标系NF在局部坐标系BF中的定向可通过单位四元数q_bn表示，||q_bn||₂=1。

需要注意的是，在接下来的说明书中，角位置或姿态和角度校正通过四元数进行表示，但以非限定性的方式，这些值可被不同地表示，例如欧拉角。

在本申请中，所有四元数例如是单位四元数。

单位四元数q_bn被描述为，具有围绕由三维的单位方向矢量q_bn规定的旋转轴的旋转角度ψ，或具有四维的一单位矢量，如下：

$q_{\mathrm{bn}}=\left[\begin{array}{c}\cos (\mathrm{\ψ}/2)\\ \sin (\mathrm{\ψ}/2)q_{\mathrm{bn}}\end{array}\right]={\left[\begin{array}{cccc}{q}_{\mathrm{bn},0}& q_{\mathrm{bn},1}& q_{\mathrm{bn},2}& q_{\mathrm{bn},3}\end{array}\right]}^T---\left(2\right)$

可标记：q_nb是q_bn的共轭；以及：

$q_{\mathrm{nb}}=\left[\begin{array}{c}\cos (\mathrm{\ψ}/2)\\ -\sin (\mathrm{\ψ}/2)q_{\mathrm{bn}}\end{array}\right]={[q_{\mathrm{bn},0}-q_{\mathrm{bn},1}-q_{\mathrm{bn},2}-q_{\mathrm{bn},3}]}^T---\left(3\right)$

旋转矩阵R_nb是通过以下关系式与四元数相关联:

$R_{\mathrm{bn}}=\left[\begin{array}{ccc}{2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},1}^2& {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},2}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},3}& {2q}_{\mathrm{bn},1}{2q}_{\mathrm{bn},3}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},2}\\ {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},2}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},3}& {2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},2}^2& {2q}_{\mathrm{bn},2}{q}_{\mathrm{bn},3}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},1}\\ {2q}_{\mathrm{bn},1}{q}_{\mathrm{bn},3}-{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},2}& {2q}_{\mathrm{bn},2}{q}_{\mathrm{bn},3}+{2q}_{\mathrm{bn},0}{q}_{\mathrm{bn},1}& {2q}_{\mathrm{bn},0}^2-1+{2q}_{\mathrm{bn},3}^2\end{array}\right]---\left(4\right)$

现在提供由在一实施方式中所使用的传感器所提供的测量值的模型，对该实施方式加速计和陀螺测量仪是三轴的。

加速计测量在传感器上应用的加速度之和。

在地球上，除地球重力之外，加速计测量由传感器经历的固有加速度。固有加速度a_p被规定如由传感器所经历的外部加速度，其等于传感器速度的时间导数。因此，由传感器在局部坐标系BF中所提供的加速度a^b通过以下关系式给出：

$a^b\left(t\right)=R_{\mathrm{bn}}\left(t\right)a^n\left(t\right)=R_{\mathrm{bn}}\left(t\right)(a_p^n\left(t\right)-g^n)---\left(5\right)$

在其中，t表示时间，gⁿ=[00-g]^T表示地球重力矢量，具有g=9.81m/s²以及表示在固定总坐标系NF中的固有加速度。

需要注意的是，该方程是纯静态的，这与是动态的、与陀螺测量仪的测量值相关的方程式相反。此外，噪音会破坏测量值。

陀螺测量仪提供这样的测量值，允许确定定向四元数。即q_bn(t)=[q_bn,0(t)q_bn,1(t)q_bn,2(t)q_bn,3(t)]^T，随时间变化的四元数，描述局部坐标系BF相对于固定总坐标系NF的定向。已知的是，定向四元数q_bn(t)的动态模型随循以下的一阶微分方程：

${\stackrel{\·}{q}}_{\mathrm{bn}}\left(t\right)={\mathrm{\Ω}}^b\left(t\right)q_{\mathrm{bn}}\left(t\right)=\frac{1}{2}\left[\begin{array}{cccc}0& {-\mathrm{\ω}}_x^b\left(t\right)& {-\mathrm{\ω}}_y^b\left(t\right)& {-\mathrm{\ω}}_z^b\left(t\right)\\ {\mathrm{\ω}}_x^b\left(t\right)& 0& {\mathrm{\ω}}_z^b\left(t\right)& {-\mathrm{\ω}}_y^b\left(t\right)\\ {\mathrm{\ω}}_y^b\left(t\right)& {-\mathrm{\ω}}_z^b\left(t\right)& 0& {\mathrm{\ω}}_x^b\left(t\right)\\ {\mathrm{\ω}}_z^b\left(t\right)& {\mathrm{\ω}}_y^b\left(t\right)& {-\mathrm{\ω}}_x^b\left(t\right)& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{bn},0}\left(t\right)\\ q_{\mathrm{bn},1}\left(t\right)\\ q_{\mathrm{bn},2}\left(t\right)\\ q_{\mathrm{bn},3}\left(t\right)\end{array}\right]---\left(6\right)$

其中， ${\mathrm{\ω}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b\left(t\right)& {\mathrm{\ω}}_y^b\left(t\right)& {\mathrm{\ω}}_z^b\end{array}\right]}^T$ 表示在局部坐标系BF中所表示的由陀螺测量仪提供的角速度。

为了计算定向四元数（单位的）q_bn(t)，陀螺测量仪G的角位置Position_A的第一确定模块DET_POS_A从q_bn(0)对微分方程（6）求积分，其中q_bn(0)对应初始定向。

由于连续函数ω^b(t)不是已知的，而是仅仅样本ω^b(l)的取样，定向四元数的样本可通过方程（6）的递归求积分来确定。

假定取样周期T_s足够小以假定ω^b(t)在时标(l-1)和(l)之间是恒定的，和ω^b(t)在该时间间隔上等于ω^b(t-1)。从而，通过在时间T_s期间，从对微分方程(6)求积分进行计算：

$q_{\mathrm{bn}}^{\left(l\right)}=\exp \left({\mathrm{\Ω}}^{b(l-1)}{T}_s\right)q_{\mathrm{bn}}^{(l-1)}---\left(7\right)$

可体现的是，在关系式(7)中对的计算可通过以下关系式从与四元数相关联的积(张量积)获得：

$q_{\mathrm{bn}}^{\left(l\right)}{f}_q(q_{\mathrm{bn}}^{(l-1)},{\mathrm{\ω}}^{b(l-1)})q_{-\mathrm{\ω}}^{b(l-1)}\⊗q_{\mathrm{bn}}^{(l-1)}---\left(8\right)$

在其中：

$q_{-\mathrm{\ω}}^{b\left(l\right)}={\left[\begin{array}{cccc}{q}_{-\mathrm{\ω},0}^{b\left(l\right)}& q_{-\mathrm{\ω},1}^{b\left(l\right)}& q_{-\mathrm{\ω},2}^{b\left(l\right)}& q_{-\mathrm{\ω},3}^{b\left(l\right)}\end{array}\right]}^T$

$=\left[\begin{array}{c}\cos \left(\right|\left|{\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2)\\ -\sin \left({\left|\right|\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2)q_{\mathrm{\ω}}^{b\left(l\right)}\end{array}\right]=\left[\begin{array}{c}\cos \left(\right|\left|{\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2)\\ -\frac{\sin \left(\right|\left|{\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2)}{\left|\right|{\mathrm{\ω}}^{b\left(l\right)}\left|\right|}{\mathrm{\ω}}^{b\left(l\right)}\end{array}\right]---\left(9\right)$

表示角度||ω^b(l)||T_s围绕矢量的旋转四元数。

需要注意的是，陀螺测量仪的测量值被噪音和偏差(或offset)破坏。因此，方程(6)(或其离散形式)的直接求积分引起对姿态四元数和从而对角位置或姿态的曲解估算。

可以注意到，在现有技术中，定向偏差通常被表示为局部坐标系BF的错误定向。由于定向的主要问题在于对坐标系进行变换，以从局部坐标系BF到固定总坐标系NF表示加速数据，考虑的是，错误四元数q_nb不允许从局部坐标系BF变换到固定总坐标系NF，而是变换到偏离即逐渐转向的坐标系NF'。因此，NF'被规定如相对于固定总坐标系NF被偏离的一坐标系；这通过其由四元数q_n′n给出的相对于固定总坐标系NF的定向进行描述。坐标系NF'随时间慢慢偏离，其定向四元数q_n'n从而随时间慢慢变化，以使得这些四元数满足以下方程：

$q_{\mathrm{bn}}\left(t\right)=q_{{\mathrm{bn}}^{\′}}\left(t\right)\⊗q_{{nn}^{\′}}\left(t\right).$

接下来是一实施例的说明，用四元数示出，应用于个人步行的情形。

现在描述在计算固有加速度时考虑与重力场相关的错误。

在κ号探测步骤(对应κ号步伐)的跨步在脚部移动或(脚部在空中）行进阶段SW的时间间隔为假定对于在每个行进阶段SW中，q_n′n(t)随时间是不变化的。

另一问题在于源自加速测量值在时间上的二重积分的位置偏差。

为了限制位置偏差，加速度仅仅在行进阶段SW期间被求积分，而在余下的时间中，速度被固定在零。

利用步伐的探测方法，可确定脚部是否处于运动。

行人脚踝的速度按步进行计算，而定向连续地在时标l用四元数确定，所述四元数确定坐标系NF′在局部坐标系BF中的定向（和不是固定总坐标系NF，这是由于给在陀螺测量仪的测量值上存在的噪音和偏差的求积分）。

在不存在对定向的观察时，不可能确定旋转四元数q_nn′。这是为什么不能够测定q_bn。

因此，满足于通过对三维角速度的测量值求积分，计算传感器在坐标系NF′中的定向:

${\mathrm{\ω}}^b={\left[{\mathrm{\ω}}_x^b{\mathrm{\ω}}_y^b{\mathrm{\ω}}_z^b\right]}^T---\left(10\right)$

如在关系式中 $q_{{\mathrm{bn}}^{\′}}^{\left(l\right)}=q_{-\mathrm{\ω}}^{b(l-1)}\⊗q_{{\mathrm{bn}}^{\′}}^{(l-1)}---\left(11\right)$

在其中，表示两个四元数的积以及

$q_{-\mathrm{\ω}}^{b\left(l\right)}={\left[\cos \left(\right|\left|{\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2)\sin \left(\right|\left|{\mathrm{\ω}}^{b\left(l\right)}\right||T_s/2){q_{-\mathrm{\ω}}^b}^T\right]}^T---\left(12\right)$

T_s是传感器A和G的取样周期，是通过四元数q_bn'规定的旋转轴。

一旦局部坐标系BF的定向被确定，加速度项仅在已偏离的坐标系NF′中表示，如以下关系式：

$a^{n^{\′}\left(l\right)}={R_{n^{\′}b}^{\left(l\right)}a}^{b\left(l\right)}---\left(13\right)$

其中，如在关系式（4）中，与相关联。该步骤由坐标系变换模块CR执行。

一旦知晓a^n′，重力应被补偿以计算固有加速度。

主要问题在于坐标系NF′的竖直方向与固定总坐标系NF的竖直方向不同。

即便在起初，用已知的传感器定向开始，所计算的定向随时间偏移。这意味着地球引力g^n′不能被精确确定。不再能接受的是，假定g^n′=g[00-1]^T，以利用加速度a^n′推断重力。

因此，以下关系式：

$a_p^{n^{\′}}=a^{n^{\′}}+g^{n^{\′}}\≠a^{n^{\′}}+g^n---\left(14\right)$

示出重力在两坐标系中是不相同的。

实际上，这类近似使固有加速度被破坏。因此，在影响固有加速度的该偏差的二重积分后，在位置或笛卡尔坐标位置中将导致一重大错误。

另一确定g^n′的方式在于，在时间间隔期间使用加速度a^n′(t)。更为精确地，有 表示从在时间上的求积分所获得的三维速度矢量。

假定四元数q_n′n，对于随时间是不变的，获得以下关系式：

${\∫}_{t_1^{\mathrm{\κ}}}^{t_2^{\mathrm{\κ}}}{a}^{n^{\′}}\left(t\right)\mathrm{dt}={\∫}_{t_1^{\mathrm{\κ}}}^{t_2^{\mathrm{\κ}}}{R}_{n^{\′}n}(a_p^n\left(t\right)-g^n)\mathrm{dt}$

$=R_{n^{\′}n}{\∫}_{t_1^{\mathrm{\κ}}}^{t_2^{\mathrm{\κ}}}(a_p^n\left(t\right)-g^n)\mathrm{dt}$

$=v_I^{n^{\′}}\left(t_2^{\mathrm{\κ}}\right)-v_I^{n^{\′}}\left(t_1^{\mathrm{\κ}}\right)-(t_2^{\mathrm{\κ}}-t_1^{\mathrm{\κ}})g^{n^{\′}}$

$=-(t_2^{\mathrm{\κ}}-t_1^{\mathrm{\κ}})g^{n^{\′}}$

在其中R_n′n根据关系式(4)与q_n′n相联系。

根据关系式(15)，a^n′在时间间隔上的平均值等于重力g^n′。因此，需要在二重积分之前使a^n′中心化，以补偿地球重力，如以下关系式：

$a_p^{n^{\′}}\left(t\right)=a^{n^{\′}}\left(t\right)-\frac{1}{t_2^{\mathrm{\κ}}-t_1^{\mathrm{\κ}}}{\∫}_{t_1^{\mathrm{\κ}}}^{t_2^{\mathrm{\κ}}}{a}^{n^{\′}}\left(t\right)\mathrm{dt}---\left(16\right)$

在计算末，对于每个步伐单独地，加速度被两次求积分，以在之后计算脚踝的完整轨迹。具有以下的方程组：

$v_I^{n^{\′}}\left(t\right)=v_I^{n^{\′}}\left(t_1^{\mathrm{\κ}}\right){+}_{t_1^{\mathrm{\κ}}}^t{a}_p^{n^{\′}}\left(t\right)\mathrm{dt}$

$p_I^{n^{\′}}\left(t\right)=p_I^{n^{\′}}\left(t_2^{\mathrm{\κ}-1}\right)+{\∫}_{t_1^{\mathrm{\κ}}}^t{v}_I^{n^{\′}}\left(t\right)\mathrm{dt}$

其中， $p_I^{n^{\′}}={\left[\begin{array}{ccc}{x}_I^{n^{\′}}& y_I^{n^{\′}}& z_I^{n^{\′}}\end{array}\right]}^T$ 表示传感器组件EC在NF’中的三维笛卡尔坐标，和(o)在O_NF被初始化。

由于在坐标系NF’中的加速度在每个步伐时被定中心(16)，那么 $v_I^{n^{\′}}\left(t_2^{\mathrm{\κ}}\right)=0m/s\∀\mathrm{\κ}.$

对应关系式(14)到(17)的步骤通过加速计A的笛卡尔坐标位置Position_C的第二确定模块DET_POS_C来实施。

这里，所计算的位置可还受源自q_n′n(t)的偏差的约束。为了限制偏差和校正轨迹，可在步行的特征上包含附加的信息。

此外，利用脚踝的轨迹的某些特征。例如，可容许脚踝的轨迹包含在一平面(平面运动)中。通过假定步伐的实际轨迹平面(步行平面)是竖直的，则各种定向偏差使竖直平面倾斜。

假定以下假设是满足的，与预定信息INF_PRED_TRAJ对应(即以该轨迹的预定参数形式表示的轨迹理论知识)：

一行人在水平地板上行走：和

一行走平面是竖直的： $z_{\mathrm{SF}}^n=z_{\mathrm{NF}}^n={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T.$

转变所计算的轨迹以更好地满足前述假设的四元数是图4示出轨迹的偏差的情形(对于一步伐)和假定是“实际轨迹”或“真实轨迹”的对应轨迹。四元数允许校正该步骤的轨迹。因此，确切地说其构成轨迹——如在由前述假设组成的对该轨迹的理论知识的约束下，在与关系式(14)到(17)对应的步骤时所计算的——的旋转修正模型的应用。

考虑足对于在坐标系NF′中的一个步伐、与行走平面相关联的三个矢量，在步骤结束时计算的，具有以下方程组：

$e_1^{n^{\′}}={\left[\begin{array}{ccc}{e}_{1,x}^{n^{\′}}& e_{1,y}^{n^{\′}}& e_{1,z}^{n^{\′}}\end{array}\right]}^T=x_{\mathrm{SF}}^{n^{\′}}$

$e_2^{n^{\′}}={\left[\begin{array}{ccc}{e}_{2,x}^{n^{\′}}& e_{2,y}^{n^{\′}}& e_{2,z}^{n^{\′}}\end{array}\right]}^T=\frac{p_I^{n^{\′}}{t}_2^{\mathrm{\κ}}-p_I^{n^{\′}}\left(t_1^{\mathrm{\κ}}\right)}{\left|\right|p_I^{n^{\′}}\left(t_2^{\mathrm{\κ}}\right)-p_I^{n^{\′}}\left(t_1^{\mathrm{\κ}}\right)\left|\right|}---\left(18\right)$

p表示通过固有加速度的二重积分所计算的笛卡尔坐标位置。参照在所述步伐时间期间不变化的旋转轴，通过方程组(17)提供以及^表示矢量积。

为了满足前述假设，在固定坐标系NF中所表示的这些矢量可根据以下方程组书写：

$e_1^n={\left[\begin{array}{ccc}{e}_{1,x}^n& e_{1,y}^n& 0\end{array}\right]}^T$

$e_2^n={\left[\begin{array}{ccc}{e}_{2,x}^n& e_{2,y}^n& 0\end{array}\right]}^T---\left(19\right)$

由于忽视“实际’’的朝向，不可能确定朝向在固定总坐标系NF中的精确值。换句话说，独立于所述朝向地对轨迹进行校正，这是因为不能调整步伐的朝向。在此情形下，在坐标系NF中考虑与在坐标系NF’中的步伐朝向相同的步伐朝向。

从而可书写：

$\left\{\begin{array}{c}{e}_{2,x}^n=\frac{e_{2,x}^{n^{\′}}}{\sqrt{{\left(e_{2,x}^n\right)}^2+{\left(e_{2,y}^n\right)}^2}}\\ e_{2,x}^n=\frac{e_{2,y}^{n^{\′}}}{\sqrt{{\left(e_{2,x}^n\right)}^2+{\left(e_{2,y}^n\right)}^2}}\end{array}\right.$ $\mathrm{et}\left\{\begin{array}{c}{e}_{1,x}^n=e_{2,y}^n\\ e_{1,y}^n=-e_{2,x}^n\end{array}\right.---\left(20\right)$

定向偏差可通过旋转矩阵R_c表示，或等同地，用四元数表示。为了确定旋转四元数或旋转矩阵R_c，即为了反转已偏离的轨迹的旋转修正模型，仅仅需要知晓在坐标系NF’中非共线的两矢量的值和它们在固定总坐标系NF中的值。

需要注意的是，矢量{e₁,e₂，e₃}校验旋转特征(范数和标量积)。还假定这些矢量是线形无关的，否则旋转四元数不是唯一的。同样地，校正旋转矩阵R_c通过以下关系式给出：

$R_c=\begin{array}{c}\left[\begin{array}{ccc}{e}_1^n& e_2^n& e_3^n\end{array}\right]{\left[\begin{array}{ccc}{e}_1^{n^{\′}}& e_2^{n^{\′}}& e_3^{n^{\′}}\end{array}\right]}^{-1}\end{array}---\left(21\right)$

同样地，可通过公式(4)的常见反转计算从R_c推导。

计算的另一方法在于确定旋转轴q_c和旋转角ψ_c，不计算旋转矩阵。在此情形下，获得以下关系式：

和

(x·y)表示两矢量的标量积，||对应正方形矩阵的行列式，和||||对应欧几里德范数。

通过关系式（23）中atan()给出的角度被限定在时间间隔[0π]中。

在所有情形中，可以注意到，的直接计算或旋转轴q_c和旋转角ψ_c的确定很好地组成旋转修正模型的反转。

对应于关系式（18）到（23）的步骤通过追溯性校正模型CORR_POS_A来实施。

在行人在遮闭的或英语术语“indoor”的空间中移动的过程中，经常可假定水平的步行（或对应于楼梯的固定倾斜）。因此，可转变所计算的轨迹，以具有预定的倾斜移动，例如水平的。在本情境中，在水平移动的假设之外，基本上在竖直运动的平面中规定脚踝的轨迹。

在这些假设之外，关于坐标系NF′相对于坐标系NF的偏差的变化的两种假设是可能的。

第一假设假定对于短期是随时间不变的，更为确切地说，在每一步伐的行进阶段中是不变的。因此，对于每个步骤，坐标系NF′被假定通过恒定旋转与固定总坐标系NF相关联。如果随时间是不变的，那么对于每个步伐四元数是确定的。

继而，轨迹用按照步伐应用的校正重新进行计算，如在以下方程组中所阐述的：

${\tilde{a}}_I^n\left(t\right)=R_c^{\mathrm{\κ}}{a}_I^{n^{\′}}\left(t_1^{\mathrm{\κ}}\right)\mathrm{fort}\∈[t_1^{\mathrm{\κ}},t_2^{\mathrm{\κ}}]$

${\tilde{v}}_I^n\left(t\right)={\tilde{v}}_I^n\left(t_1^{\mathrm{\κ}}\right)+{\∫}_{t_1^{\mathrm{\κ}}}^t{\tilde{a}}_p^n\left(t\right)\mathrm{dt}=R_c^{\mathrm{\κ}}{\tilde{v}}_I^{n^{\′}}\left(t\right)$ (24)

${\tilde{p}}_I^n\left(t\right)={\tilde{p}}_I^n\left(t_2^{\mathrm{\κ}-1}\right)+{\∫}_{t_1^{\mathrm{\κ}}}^t{\tilde{v}}_I^n\left(t\right)\mathrm{dt}$

在其中是与第k步相关的校正旋转矩阵。

不过，坐标系NF′随时间连续地偏移并且即便是在脚部的行进阶段中。

假定，定向或角位置的错误源于陀螺测量仪G的测量值随时间的偏差的求积分。如此，恒定偏差的求积分使定向线性地偏离。因此，实用的是，不仅校正最终的定向，还如在图5上所示沿着轨迹再注入和分布这种校正。

如果坐标系NF′由于陀螺测量仪G的恒定偏差而偏移，那么由给出的旋转角随时间线性地增大。如果陀螺测量仪G的偏差随时间慢慢地变化，那么旋转角q_n′n的偏差仍可被认为对于短期(例如在脚部的行进阶段中)是线性的。

如在图1上所示，相对于现有技术可添加两附加步骤：

一第一步骤计算校正四元数的定向；

一第二步骤借助于一线性增大的校正确定所有定向四元数q_bn′(t)的所校正的四元数所述校正通过由之前校正的步伐的最后一时标开始，确定坐标系NF’在局部坐标系BF中的定向。继而，被计算和脚踝的轨迹被重新计算。最后，提出一循环，该循环允许改进的计算和使得脚踝的轨迹验证上述的约束。

为了更为精确，给出被用于估算所校正的轨迹的关系式。在前一步伐末，已知“实际”的最后定向的四元数为继而角速度的样本L在观察当前步伐末的位置之前被求积分，其中相关联的四元数被标记为

没有校正控制，利用陀螺测量仪的测量值的求积分所计算的定向随时间发生偏差。

本发明的一目的在于，通过将随时间线性地变化的一校正应用至所计算的定向(在此情形下，所述校正随时间线性地增大)，来计算所校正的定向以通过应用以下关系式补偿的偏差：

$q_{\mathrm{bn}}^{+\left(l\right)}=q_{{\mathrm{bn}}^{\′}}^{\left(l\right)}\text{\⊗}{q}_{-\frac{1}{L}{\mathrm{\ψ}}_c}$ $\mathrm{forl}\∈\{l_2^{\mathrm{\κ}-1}:l_2^{\mathrm{\κ}}\}---\left(25\right)$

在其中：

$q_{{\mathrm{bn}}^{\′}}^{\left(l\right)}=q_{-\mathrm{\ω}}^{b(l-1)}\⊗...\⊗q_{-\mathrm{\ω}}^{b(l_2^{\mathrm{\κ}-1}+1)}\⊗q^{+\left(l_2^{\mathrm{\κ}-1}\right)}$

$q_{-\frac{1}{L}{\mathrm{\ψ}}_c={[\cos (\frac{1}{L}{\mathrm{\ψ}}_c/2)-\sin (\frac{1}{L}{\mathrm{\ψ}}_c/2)q_c]}^T---\left(26\right)}$

在关系式(25)中，被线性地校正，以从步伐开始的定向到步伐结束的定向——这等同于——具有连续的校正。

轨迹从所校正的定向被重新计算。为此，首先计算以下方程式：

在其中，量值表示从偏差的线性增大假设和因此从线性校正来计算的数量a(或g)。

如同在关系式（4）中，矩阵从获得，并具有以下关系式：

${\stackrel{\∪}{g}}^n=-\frac{1}{t_2^{\mathrm{\κ}}-t_1^{\mathrm{\κ}}}{\∫}_{{t=t}_1^{\mathrm{\κ}}}^t{\stackrel{\∪}{a}}^n\left(t\right)---\left(28\right)$

同样地，传感器组件EC的速度和位置通过以下关系式进行计算：

${\stackrel{\∪}{v}}_I^n\left(t\right)={\stackrel{\∪}{v}}_I^n\left(t_1^{\mathrm{\κ}}\right)+{\∫}_{t_1^{\mathrm{\κ}}}^t{\stackrel{\∪}{a}}_p^n\left(t\right)\mathrm{dt}$

${\stackrel{\∪}{p}}_I^n\left(t\right)={\tilde{p}}_I^n\left(t_2^{\mathrm{\κ}-1}\right)+{\∫}_{t_1^{\mathrm{\κ}}}^t{\stackrel{\∪}{v}}_I^n\left(t\right)\mathrm{dt}$ (29)

一旦脚踝的轨迹被计算，可在另一循环中从脚踝的最后一轨迹重新计算新的校正四元数qψc。继而，重新执行一循环，以及计算新的四元数和新的位置

可以注意到，在关系式（25）中，可根据以下关系式根据递归地进行书写：

$q_{\mathrm{bn}}^{+\left(l\right)}=q_{-{\mathrm{\ω}}^{+}}^{b(l-1)}\⊗q_{\mathrm{bn}}^{+(l-1)}---\left(30\right)$

在其中，表示所校正的角速度四元数并通过以下关系式进行规定：

$q_{{-\mathrm{\ω}}^{+}}^{b\left(l\right)}=q_{-\mathrm{\ω}}^{b\left(l\right)}\⊗q_{{-\frac{1}{L}\mathrm{\ψ}}_c}^{+\left(l\right)}=q_{-\mathrm{\ω}}^{b\left(l\right)}\⊗(q_{{\mathrm{bn}}^{\′}}^{\left(l\right)}\⊗q_{{-\frac{1}{L}\mathrm{\ψ}}_c}\⊗q_{n^{\′}b}^{\left(l\right)})---\left(31\right)$

在此情形下，校正是等分的，关系式（31）通过以下关系式获得：

$\begin{array}{c}{q}_{\mathrm{bn}}^{+\left(l\right)}=q_{{\mathrm{bn}}^{\′}}^{\left(l\right)}\⊗q_{{-\frac{1}{L}\mathrm{\ψ}}_c}\\ =[q_{-\mathrm{\ω}}^{b(l-1)}\⊗q_{{\mathrm{bn}}^{\′}}^{(l-1)}]\⊗[q_{{-\frac{1}{L}\mathrm{\ψ}}_c}\⊗q_{{-\frac{l-1}{L}\mathrm{\ψ}}_c}]\\ =[q_{-\mathrm{\ω}}^{b(l-1)}\⊗q_{{\mathrm{bn}}^{\′}}^{(l-1)}]\⊗[q_{{-\frac{1}{L}\mathrm{\ψ}}_c}\⊗(q_{n^{\′}b}^{(l-1)}\⊗q_{{\mathrm{bn}}^{\′}}^{(l-1)})\⊗q_{{-\frac{l-1}{L}\mathrm{\ψ}}_c}]\\ =q_{-\mathrm{\ω}}^{b(l-1)}\⊗(q_{{\mathrm{bn}}^{\′}}^{(l-1)}\⊗q_{{-\frac{1}{L}\mathrm{\ψ}}_c}\⊗q_{n^{\′}b}^{(l-1)})\⊗q_{\mathrm{bn}}^{+(l-1)}\\ =q_{-\mathrm{\ω}}^{b(l-1)}\⊗q_{\mathrm{bn}}^{+(l-1)}\end{array},---\left(32\right)$

所提出的本发明被测试并利用相同的实验数据与常见的不同方法进行比较。行人配有传感器组件3A3G，三轴加速计和三轴陀螺测量仪固连地固定在脚踝上，行人在地面被假设是平坦的和水平的建筑物内部行走，行走平面被假设是竖直的。

图7a、7b表示行人脚踝的三维或3D轨迹。在图7a上，不执行任何校正，在图7b上，用角度在NF和NF′之间恒定的第一假设进行校正（通过方程（24）和之前的方程式）。与如图7a所示发生偏差的无校正的所计算的轨迹相反，清晰的是，所提出的校正遵守约束，如在图7b上所示。如前文所阐述的，校正四元数不能校正位置的偏差。将通过使用第一假设（即，定向偏差随时间是不变的）给出的水平速度标为和相对于北方的偏移的移动方向标为将通过使用第二假设（即，随时间的线性定向偏差）给出的水平速度标为和相对于北方的移动方向标为具有以下关系式:

和

和分别在关系式（24）和（29）中给出。

Claims (13)

1.借助于传感器组件估算活动元件或物体的轨迹的轨迹估算方法，该传感器组件固定在活动元件或物体上并包括相固连的加速计和陀螺测量仪，所述轨迹估算方法包括以下的步骤：

-接收由加速计提供的和在一时间间隔中的多个相继时刻中的至少两个时刻测量的加速度值，

-接收由陀螺测量仪提供的和在该时间间隔的所述多个相继时刻的至少两时刻测量的角速度值，

-处理所提供的值，用以一方面借助于角速度值估算至少一角位置值，和另一方面借助于加速度值和之前所述的至少一估算的角位置值估算限定活动元件或物体的轨迹的至少两个笛卡尔坐标位置值，

其特征在于，所述轨迹估算方法还包括以下步骤：

-通过在轨迹的理论知识的约束下所估算的轨迹的旋转修正模型的反转，估算旋转参数，该理论知识由该轨迹的预定参数组成，以及

-通过借助于所估算的旋转参数，在所述至少一估算的角位置值上应用旋转，追溯性校正所述至少一估算的角位置值，以提供至少一校正的角位置值。

2.根据权利要求1所述的轨迹估算方法，其特征在于，追溯性校正的步骤还包括借助于加速度值和所述至少一校正的角位置值对笛卡尔坐标位置值进行追溯性校正。

3.根据权利要求1或2所述的轨迹估算方法，其特征在于，

-由加速计提供的加速度值和由陀螺测量仪提供的角速度值被表示在与传感器组件相关联的一活动坐标系中，以及

-在对所提供的值进行处理的处理步骤时，对所述至少一角位置值的估算以相对于与地球坐标系相联系的一固定总坐标系的轴的旋转角度参数的形式表示，并且对所述笛卡尔坐标位置值的估算以在该相同的固定总坐标系中以笛卡尔坐标的形式表示。

4.根据权利要求1所述的轨迹估算方法，其特征在于，追溯性校正的步骤被多次迭代地实施，用于对所述至少一估算的角位置值的迭代校正。

5.根据权利要求1所述的轨迹估算方法，其特征在于，旋转参数是四元数参数。

6.根据权利要求1所述的轨迹估算方法，其特征在于，轨迹的理论知识包括被认为应包含轨迹的一平面的角度定向参数。

7.根据权利要求1所述的轨迹估算方法，其特征在于，轨迹的理论知识包括被认为应构成轨迹的组成部分的至少两个点的笛卡尔坐标。

8.根据权利要求1所述的轨迹估算方法，其特征在于，轨迹的理论知识包括轨迹的方向或斜率的参数。

9.根据权利要求1所述的轨迹估算方法，其特征在于，追溯性校正的步骤被设计以使得借助于所估算的旋转参数所实施的校正预定地分布在整个或部分所述多个相继时刻上。

10.根据权利要求9所述的轨迹估算方法，其特征在于，追溯性校正的预定分布被限定以使得借助于所估算的旋转参数计算的校正参数在整个或部分所述多个相继时刻上线性地增大。

11.借助于传感器组件估算活动元件或物体的轨迹的轨迹估算装置，该传感器组件固定在活动元件或物体上并包括相固连的加速计和陀螺测量仪，所述轨迹估算装置包括以下的设备：

-被配置为接收由加速计提供的和在一时间间隔中的多个相继时刻中的至少两个时刻测量的加速度值的设备，

-被配置为接收由陀螺测量仪提供的和在该时间间隔的所述多个相继时刻的至少两时刻测量的角速度值的设备，

-被配置为处理所提供的值的设备，用以一方面借助于角速度值估算至少一角位置值，和另一方面借助于加速度值和之前所述的至少一估算的角位置值估算限定活动元件或物体的轨迹的至少两个笛卡尔坐标位置值，

其特征在于，所述轨迹估算装置还包括以下设备：

-被配置为通过在轨迹的理论知识的约束下所估算的轨迹的旋转修正模型的反转，估算旋转参数的设备，该理论知识由该轨迹的预定参数组成，以及

-被配置为通过借助于所估算的旋转参数，在所述至少一估算的角位置值上应用旋转，追溯性校正所述至少一估算的角位置值的设备，以提供至少一校正的角位置值。

12.活动元件或物体的轨迹估算系统，所述轨迹估算系统包括：

-加速计，其适于提供在给定时间间隔的相继时刻测量的加速度值，

-陀螺测量仪，其与加速计相固连，适于提供在该时间间隔的相继时刻测量的角速度值，以及

-处理部件，处理部件处理由加速计和陀螺测量仪所提供的值，用于实施根据权利要求1到10中任一项所述的轨迹估算方法。

13.根据权利要求12所述的轨迹估算系统，其特征在于，所述轨迹估算系统包括固连部件，所述固连部件将加速计和陀螺测量仪固连至：使用者；移动终端；或陆地、空中或海上的运输工具。