移动方向置信区间估计

申请号 CN201480012882.1 申请日 2014-03-02 公开(公告)号 CN105190237B 公开(公告)日 2017-12-05
申请人 应美盛股份有限公司; 发明人 郑元; 林尚鸿; 凯瑞·基尔;
摘要 本 发明 公开了惯性测量系统。所述惯性测量系统具有产生已校正 加速 度计 数据的加速度计处理单元。所述惯性测量系统进一步包含产生已校正磁强仪数据的磁强仪处理单元,和产生已校正 陀螺仪 数据的陀螺仪处理单元。使用所述已校正加速度计数据、所述已校正磁强仪数据以及所述已校正陀螺仪数据,所述惯性测量系统产生指示移动方向 角 误差的准确度的所述移动方向角误差。
权利要求

1.一种惯性测量系统,其包括:
加速度计处理单元,其可操作以产生已校正加速度计数据;
磁强仪处理单元,其可操作以产生已校正磁强仪数据;以及
陀螺仪处理单元,其可操作以产生已校正陀螺仪数据,
其中所述惯性测量系统可操作以使用所述已校正加速度计数据、所述已校正磁强仪数据和所述已校正陀螺仪数据来产生指示定向的准确度的所述定向角,
其中所述惯性测量系统进一步包含转换器,其产生所述惯性测量系统的置信区间。
2.根据权利要求1所述的惯性测量系统,其中所述定向角的所述准确度是在时间间隔内所述定向角的正确性的量度。
3.根据权利要求2所述的惯性测量系统,其中所述置信区间指示所述定向角的所述准确度。
4.根据权利要求2所述的惯性测量系统,其中所述置信区间与加速度计向量和磁强仪向量的内积的统计特性相关,所述加速度计向量是与所述已校正加速度计数据相关的向量并且所述磁强仪向量是与所述已校正磁强仪数据相关的向量。
5.根据权利要求1所述的惯性测量系统,其中所述加速度计处理单元包含加速度计校正和重提取,其响应于加速度计数据和反四元数信号并且可操作以在世界坐标中产生所述已校正加速度计数据。
6.根据权利要求5所述的惯性测量系统,其中所述加速度计处理单元进一步包含第一比较器,所述第一比较器可操作以比较所述已校正加速度计数据和世界坐标中的预定义参考以产生第一比较输出。
7.根据权利要求6所述的惯性测量系统,其中所述磁强仪处理单元包含磁强仪校正和异常抑制块,其响应于磁强仪数据和所述反四元数信号并且可操作以在世界坐标中产生所述已校正磁强仪数据。
8.根据权利要求7所述的惯性测量系统,其中所述磁强仪处理单元进一步包含第二比较器,所述第二比较器可操作以比较所述已校正磁强仪数据和世界坐标中的所述预定义参考以产生第二比较输出。
9.根据权利要求8所述的惯性测量系统,其中所述陀螺仪处理单元包含陀螺仪校正块,其响应于陀螺仪数据并且可操作以产生所述已校正陀螺仪数据。
10.根据权利要求9所述的惯性测量系统,其中所述陀螺仪处理单元进一步包含陀螺仪积分块,所述陀螺仪积分块可操作以对所述已校正陀螺仪数据积分并且产生陀螺仪四元数数据。
11.根据权利要求10所述的惯性测量系统,其进一步包含传感器融合块,其响应于所述第一比较输出和所述第二比较输出以及所述陀螺仪四元数数据的求和,所述传感器融合块可操作以产生表示所述定向的四元数输出。
12.根据权利要求11所述的惯性测量系统,其中所述转换器响应于所述第一比较输出和所述第二比较输出的求和并且产生所述置信区间。
13.根据权利要求12所述的惯性测量系统,其进一步包含求反块,其响应于所述四元数输出并且可操作以产生所述反四元数信号。
14.根据权利要求13所述的惯性测量系统,其进一步包含变换块,其响应于所述四元数输出以产生定向。
15.根据权利要求1所述的惯性测量系统,其进一步包含加速度计、磁强仪和陀螺仪,其中所述加速度计、所述磁强仪和所述陀螺仪驻留于一个半导体芯片上,并且校正和重力提取块、磁强仪校正和异常抑制块、陀螺仪校正块、陀螺仪积分块、传感器融合、比较器、求反块以及转换器驻留于另一个半导体芯片上。
16.根据权利要求1所述的惯性测量系统,其进一步包含加速度计、磁强仪和陀螺仪,其中所述加速度计、所述磁强仪、所述陀螺仪、所述加速度计处理单元、所述磁强仪处理单元、所述陀螺仪处理单元、传感器融合、比较器、求反块以及转换器驻留于一个相同的半导体芯片上。
17.根据权利要求7所述的惯性测量系统,其中所述惯性测量系统将所述加速度计数据用作重力方向的参考并且将所述磁强仪数据用作磁北方向的参考。
18.根据权利要求6所述的惯性测量系统,其中所述校正和重力提取单元与所述第一比较器用硬件软件实施。
19.根据权利要求6所述的惯性测量系统,其中所述磁强仪校正和异常抑制块与所述第一比较器用硬件或软件实施。
20.根据权利要求1所述的惯性测量系统,其中陀螺仪校正块和陀螺仪积分块用硬件或软件实施。
21.一种惯性测量系统,其包括:
加速度计处理单元,其可操作以产生已校正加速度计数据;
磁强仪处理单元,其可操作以产生已校正磁强仪数据;
陀螺仪处理单元,其可操作以产生已校正陀螺仪数据,
其中所述惯性测量系统可操作以产生角误差,使用与所述已校正加速度计数据和所述已校正磁强仪数据相关的置信区间来测量所述角误差的准确度,
其中所述惯性测量系统进一步包含转换器,其产生所述置信区间。
22.一种使用惯性测量系统估计移动方向的方法,其包括:
用加速度计校正和重力提取块校正加速度计数据以产生已校正加速度计数据;
用磁强仪校正和异常抑制块校正磁强仪数据以产生已校正磁强仪数据;
用陀螺仪校正块校正陀螺仪数据以产生已校正陀螺仪数据;
使用所述已校正加速度计数据、所述已校正磁强仪数据和所述已校正陀螺仪数据来产生角误差,所述角误差是指示定向角的准确度的量度;以及
用转换器产生所述量度的置信区间。
23.根据权利要求22所述的估计移动方向的方法,其进一步包含将所述已校正加速度计数据与世界坐标中的预定义参考比较以产生第一比较输出。
24.根据权利要求23所述的估计移动方向的方法,其进一步包含将所述已校正磁强仪数据和世界坐标中的所述预定义参考比较以产生第二比较输出。
25.根据权利要求24所述的估计移动方向的方法,其中对已校正陀螺仪数据积分。
26.根据权利要求25所述的估计移动方向的方法,其中所述转换器响应于所述第一比较输出和所述第二比较输出的求和并且产生所述量度的置信区间。
27.一种产生角误差的方法,其包括:
判定是否检测到磁异常;
如果未检测到磁异常,则从已校正加速度计数据、已校正磁强仪数据和已校正陀螺仪数据用倾斜和移动方向补偿来计算定向置信区间;以及
如果检测到磁异常,则使用陀螺仪数据计算定向置信区间。

说明书全文

移动方向置信区间估计

[0001] 相关申请的交叉参考
[0002] 本申请要求在2013年3月14日由Zheng提交的题为“移动方向置信区间估计”的美国临时申请No.61/783,220的优先权,并且要求在2013年3月13日由Zheng提交的题为“移动方向置信区间估计”的美国临时申请No.61/780,475的优先权。

技术领域

[0003] 本发明的各种实施例一般涉及惯性测量系统,并且尤其是涉及惯性测量系统中所采用的定向的准确度的测量。

背景技术

[0004] 惯性测量系统中所采用的或包括惯性测量系统的运动跟踪系统使用运动传感器。运动传感器一般是陀螺仪传感器、加速度计传感器以及磁强仪传感器。移动方向数据或世界坐标系中的偏航定向是来自应用诸如车辆导航或手机罗盘中的运动跟踪系统的关键输出中的一个。从运动跟踪系统中的若干运动传感器计算移动方向输出,运动传感器如下所示:3轴陀螺仪传感器、3轴加速度计以及3轴磁强仪。每个传感器具有互相正交的3轴(x、y、z)测量输出。3轴测量元件(偏航、横摇和俯仰)在三个传感器之间对齐。
[0005] 在三个传感器之间,加速度计数据被看作是重方向的参考(推测上当装置移动慢时为加速度计数据的主导分量)并且磁强仪被看作是对地球磁场方向的参考(假定没有磁扰)。理论上,通过将磁强仪数据投影到垂直于重力向量的平面上可获得移动方向向量,即指向磁北的向量。然而,实际上,因为当装置处于运动中时,磁强仪遭受环境磁扰并且加速度计感测到连同重力的其它形式的加速,所以磁强仪-加速度计组合经常导致大的移动方向误差。通常,用于传感器融合(运动跟踪系统的一部分)的算法通过调控陀螺仪来补救扰动问题。当动态地跟踪定向改变时,陀螺仪免受磁扰和线性加速干扰,并同时将磁强仪和加速度计数据用作长期参考以取消来自陀螺仪缺点的潜在角漂移。
[0006] 换句话说,运动跟踪系统通常使用具有失真的运动传感器。运动跟踪系统包含运动跟踪信号,该信号因运动跟踪系统被模拟为随机系统,所以特别地具有一些不确定性。例如,在计算定向中,计算到误差,因为一旦误差已知,它可用作所计算定向中的置信度的量度。当前,基本算法用于计算角误差的估计。算法仅是置信度的标度诸如离散指示符1、2、3、4,其中例如指示低置信度的1至指示在加或减5度内的最大置信度的4。这明显不是误差的精确估计。进一步地,在当前系统中,误差的估计不可放入卡尔曼滤波器中,因为需要计算大矩阵的反函数,从而使计算太过复杂而难以实现。因此,在当前系统中未有效地提供误差测量,而是提供了对误差的描述。因此,希望更准确地估计运动跟踪系统中的移动方向。

发明内容

[0007] 简要地,本发明的实施例包含具有产生已校正加速度计数据的加速度计处理单元的惯性测量系统。惯性测量系统进一步包含产生已校正磁强仪数据的磁强仪处理单元。惯性测量系统进一步包含产生已校正陀螺仪数据的陀螺仪处理单元。使用已校正加速度计数据,已校正磁强仪数据以及已校正陀螺仪数据,惯性测量系统产生移动方向角和指示角误差的准确度的误差。
[0008] 通过参考说明书的其余部分和附图可以实现进一步理解本文公开的具体实施例的本质和优点。

附图说明

[0009] 图1示出根据本发明的实施例的运动跟踪系统105。
[0010] 图2a至2f示出根据本发明的各种实施例的系统105的示例性应用。
[0011] 图3a示出根据本发明的实施例的运动处理单元的相关特征的框图
[0012] 图3b示出运动处理单元的输出与变化的磁场的关系。
[0013] 图4示出关于方框316的磁异检测的由系统300执行的步骤的流程图400。
[0014] 图5示出移动方向统计计算。
[0015] 图6示出由加速度计偏移估计误差引起的潜在移动方向误差。

具体实施方式

[0016] 在所述实施例中,也被称为运动处理单元(MPU)的运动跟踪装置包含除电子电路之外的至少一个传感器。考虑这样的传感器,诸如陀螺仪、磁强仪、加速度计、麦克压力传感器、近程光传感器,环境光传感器,尤其是本领域技术人员已知的。一些实施例包含加速度计、陀螺仪以及磁强仪,其每个都提供沿相对于彼此正交的三轴的测量,被称为9轴装置。其它实施例可以不包含所有传感器或可提供沿一或多个轴的测量。传感器形成于第一衬底上。其它实施例可包含固态传感器或任何其它传感器类型。运动跟踪装置中的电子电路接收来自一或多个传感器的测量输出。在一些实施例中,电子电路处理传感器数据。而在其它实施例中,在不同芯片上的处理器上处理传感器数据。电子电路实施于第二衬底上。第一衬底垂直地堆叠、附着并且电连接至单个半导体芯片中的第二衬底。
[0017] 在所述实施例中,“原始数据”指来自传感器的尚未被处理的测量输出。“运动数据”指已处理传感器数据。处理可包含应用传感器融合算法或应用任何其它算法,诸如计算置信区间。在传感器融合算法的情况下,来自一或多个传感器的数据经组合以提供装置的定向。在实施例中,定向包含移动方向角和/或置信值。在所述实施例中,MPU可包含结构中的处理器、存储器、控制逻辑和传感器。在实施例中,置信区间指示了移动方向不确定性的范围,该范围指示误差,并因此指示了移动方向估计的值的置信度,在所述实施例中,世界坐标中的预定义参考指这样的坐标系:其中坐标系的一个轴与地球重力对齐,坐标系坐标的第二轴指向磁北并且第三坐标正交于第一坐标和第二坐标。
[0018] 图1示出根据本发明的实施例的运动跟踪系统105。示出的系统105包含MPU 110、应用处理器114、应用存储器112以及外部传感器108。在实施例中,MPU 110包含处理器102、存储器104以及传感器106。示出的存储器104存储算法、原始数据和/或来自传感器106和/或外部传感器108的已处理传感器数据。在实施例中,传感器106包含加速度计、陀螺仪、磁强仪、压力传感器、麦克风和其它传感器。外部传感器108可包含加速度计、陀螺仪、磁强仪、压力传感器、麦克风、近程传感器、触觉传感器和环境光传感器,以及其它传感器。
[0019] 在一些实施例中,处理器102、存储器104和传感器106形成于不同芯片上,并且在其它实施例中,处理器102、存储器104和传感器106驻留于相同芯片上。在另外的其它实施例中,在计算定向中采用的传感器融合算法在处理器102和MPU 110外部执行。在还有的其它实施例中,传感器融合和置信区间由MPU 110确定。
[0020] 在实施例中,处理器102根据存储器104中的算法执行代码,以处理存储器104中的数据。在另一个实施例中,应用处理器发送至应用存储器112或从中检索,并且耦合至处理器102。处理器102根据处理器114中的应用执行存储器104中的算法。应用的实例如下:导航系统、罗盘准确度、遥控、三维照相机、工业自动化或任何其它运动跟踪应用。在3维应用的情况下,通过处理器102估计偏移误差或灵敏度误差。应当理解,这不是应用的详尽列表,并且考虑其它的应用。
[0021] 图2a至2f示出根据本发明的各种实施例的系统105的示例性应用。图2a示出包含系统105的汽车。系统105计算汽车的定向以输出移动方向和置信区间。类似地,在图2b中,计步器示出为包含用于计算计步器的定向的系统105。图2c示出用户腕部上的可佩带传感器,该可佩带传感器采用系统105。在一些实施例中,可佩带传感器可佩戴在身体的任何部分。系统105计算可佩带传感器的定向。在图2d中,智能手机/平板计算机示出为采用系统105。系统105计算智能手机/平板计算机的定向,诸如用于全球定位应用。图2e示出3维照相机,其采用系统105以用于计算照相机的定向。图2f示出导航系统,其采用系统105以用于计算导航系统的定向。应当理解,图2a到图2f的应用仅是太过详尽以至于难以枚举的一系列其它应用的实例。
[0022] 图3a示出根据本发明的实施例的具有移动方向和置信区间的MPU 300的相关特征的框图,其中置信区间也被定义为移动方向误差的范围。MPU 300类似于图1的MPU 110。
[0023] 在图3a中,MPU 300示出为包含加速度计处理单元302、磁强仪处理单元304、陀螺仪处理单元306、Q求反330、加法器328、传感器融合326、变换333以及转换器332。
[0024] 加速度计处理单元302示出为包含响应于加速度并产生加速度计输出348的加速度计传感器308。加速度计传感器308示出为包含MA加速度计、由“bA”表示的偏移以及由“nA”表示的噪声,这两者都被引入加速度计感测并且都显然是不希望的。加速度计处理单元302进一步示出具有校正和重力提取块314与比较器320。加速度计输出348用作至校正和重力提取块314的输入,所述校正和重力提取块进一步接收来自Q求反块330的输入。校正和重力提取块314在世界坐标中产生被比较器320使用的已校正加速度计数据350(在本文中也称为已校正加速度计输出350)。比较器320比较已校正加速度计输出350和世界坐标中的预定义参考(在图3a中示出为输出340),并且产生比较器输出Δ(Delta)_A,342。
[0025] 磁强仪处理单元304示出为包含磁强仪310、校正和异常抑制块316以及比较器322。磁强仪310示出为接收磁场并且产生磁强仪输出352。磁强仪传感器310示出为包含MM磁强仪、由“bM”表示的偏移以及由“nM”表示的噪声,这两者都被引入磁强仪感测并且都显然是不希望的。
[0026] 磁强仪输出352用作输入至校正和异常抑制块316的输入。校正和异常抑制块316也接收来自Q求反块330的输入,其进而在世界坐标中产生已校正磁强仪数据354。比较器322比较世界坐标中的已校正加速度计输出354和预定义参考并且产生差Δ_M,344,其作为输入至加法器328的输入。输出344和342通过加法器328加在一起并且求和AM 346的结果被提供作为传感器融合326的输入中的一个。
[0027] 示出陀螺仪处理单元306包含陀螺仪312、陀螺仪校正块318以及陀螺仪积分块324。示出陀螺仪传感器312包含MG陀螺仪,由“b“G 表示的偏移以及由“nG”表示的噪声,这两者都被引入陀螺仪传感器中并且都显然是不希望的。示出陀螺仪312接收角速度并且产生陀螺仪输出356。陀螺仪输出356用作输入至陀螺仪校正块318的输入,其进而产生已校正陀螺仪数据358(在本文中作为已校正陀螺仪输出358是已知的)。输出358用作输入至陀螺仪积分块324的输入,其产生用作传感器融合326的输入中的一个的陀螺仪四元数数据360。传感器融合326组合求和AM 346和陀螺仪四元数数据360以产生四元数形式的运动数据327。
变换333将四元数输出转换为是运动角的移动方向363。
[0028] 加法器328将Δ_A342和Δ_M344相加,并且在转换块332中将求和AM346转换以产生移动方向置信区间估计362。四元数形式的传感器融合的输出也作为输入被提供至块330,其进而产生被提供给比较器320和块314的输出。
[0029] 传感器348、352和356的输出表示原始传感器数据。角速度和磁场指示运动,而加速和磁场指示倾斜。
[0030] 在本发明的一些实施例中,用硬件实施单元302、304和306中的每个。在本发明的一些实施例中,用软件实施这些单元,除了MA、MM和MG。在本发明的其它实施例中,用硬件和软件实施这些单元。
[0031] 在一些实施例中,单元302、304、306以及图3a的其余组件驻留于相同集成电路(或半导体)上。在一些实施例中,单元302、304、306以及图3a的其余组件的任何组合驻留于不同集成电路而不是其余组件上。
[0032] 内反馈环路由输出342、求和AM 346、传感器融合326、四元数输出、q求反330以及比较320形成。外反馈环路由四元数输出、四元数求反输出340、输出354、输出322、输出344、求和AM 346以及传感器融合326形成。因此,由内反馈环路和外反馈环路构成的双环路反馈机构用于检测陀螺仪积分和加速度计/磁强仪定向之间的缺点或错配,该陀螺仪积分通过块324执行。反馈机构的目标是发现并且减少错配。
[0033] 块314、316和318中的每个校正相应输入。块316检测偏移和扰动。在实施例中,磁强仪校正可以如在2012年8月10日由Kerry Keal提交的题为“磁强仪偏移和异常检测器”的美国专利申请No.13/572,441所述,该申请通过引用并入本文,如同本文做出完全阐述。
[0034] 在三个传感器308、310和312之中,加速度计数据被看作是重力方向的参考(推测上当装置移动慢时为加速度计数据的主导分量)并且磁强仪被看作是对地球磁场方向的参考(假定没有磁扰)。理论上,通过将磁强仪数据投影到垂直于重力向量的平面上可获得移动方向向量,即指向磁北的向量。然而,实际上,因为当装置处于运动中时,磁强仪遭受环境磁扰并且加速度计感测到连同重力的其它形式的加速,所以磁强仪-加速度计组合经常导致大的移动方向误差。通常,传感器融合算法通过调控免受磁扰和线性加速扰动的陀螺仪数据来补救扰动问题,以动态地跟踪定向改变,同时将磁强仪和加速度计数据用作长期参考以取消来自陀螺仪缺点的潜在角漂移。
[0035] 转换器332的输出是误差边界。角误差是指示角的准确度的量度。角的准确度是在时间间隔内角的正确性的量度。角误差的示例性准确度是置信区间。
[0036] 图3b示出MPU 300的输出,即具有变化的磁场强度的移动方向置信区间的绘图。表示角的估计的变换333的输出在图3a和3b中示为移动方向。转换块332接收来自加法器328的角误差并产生图3b所示的置信区间值作为指示移动方向的误差界限的置信度+和置信度-。图3b还示出随变化的磁场变化的置信区间。
[0037] 图3a的基于反馈环路的传感器融合算法被用于估计3个运动传感器,即传感器308、310和312的移动方向。图3a示出双环路反馈机构,该结构经采用检测陀螺仪积分和加速度计/磁强仪定向之间的错配。陀螺仪积分块324通过非线性积分产生角位置(包含移动方向)。通过比较器322和320,比较来自陀螺仪积分块324的定向角与加速度计和磁强仪的输出。在来自不同传感器的冗余信息之间通常存在错配,该错配具有传感器系统的不确定性界限。反馈机构的目标中的一个是发现并减少错配。
[0038] 陀螺仪传感器312测量3轴角速度。传感器就灵敏度和零偏漂移方面存在缺点。在陀螺仪积分后,这些传感器不确定性转移到无界限定向误差。因此,开环陀螺仪积分不可在长时间内用于定向估计并且必须通过反馈环路中的加速度计传感器308和磁强仪传感器310校正。
[0039] 3轴加速度计传感器308用于估计作为惯性参考框架的重力向量。传感器具有灵敏度、偏移和线性加速不确定性。3轴磁强仪传感器310用于估计北点向量以用于移动方向估计。传感器具有灵敏度、偏移和来自磁扰的不确定性。
[0040] 在环路中(例如使用陀螺仪信息来校正偏移或抑制干扰)、环路外(例如常规的“图-8”磁强仪校正,其仅使用磁强仪数据取消磁强仪偏移)或两者的组合来处理加速度计和磁强仪的不确定性。本发明的各种实施例建议统一框架以产生用于所有的三种架构的置信区间。
[0041] 来自陀螺仪传感器的角速度测量经积分产生角位置信号(定向)。该过程是第一阶非线性动力学,表示如下:
[0042]   方程1
[0043] 其中ω(t)=(0 gx gy gz)是由陀螺仪向量构成的四元数向量,并且乘法是四元数乘法。
[0044] 对于采样样本数据系统,可通过如下方程接近ΔQ
[0045]   方程2
[0046] 然后,仅陀螺仪四元数积分如下:
[0047]   方程3
[0048] 可通过代数公式从四元数中推导移动方向角。
[0049] 陀螺仪误差模型
[0050] 使所测量的角速率ωm与真实角速率ω相关的简单模型如下:
[0051] ωm=ω+b+nr  方程4
[0052] 其中b表示陀螺仪偏移并且nr表示比率噪声。该比率噪声假定为具有标准差σr(为简单起见,假定所有三轴中的噪声级相等)的高斯白噪声。陀螺仪偏移b随时间慢慢漂移,所以其建模为随机走动过程,如下:
[0053] b'=nw   方程5
[0054] 其中b'是b的导数。nw是具有级σw的零均值高斯白噪声。可使用阿伦方差技术测量陀螺仪的噪声特性σr和σw。
[0055] 基于陀螺仪的四元数的误差协方差
[0056] 基于陀螺仪噪声模型,通过定义误差状态向量确定积分四元数的误差协方差的估计,如下:
[0057]   方程6
[0058] 其中δθ是表示已估计四元数和真实四元数之间的小姿态角误差的3x1向量。Δb也是表示真实偏移和其估计之间的偏移误差的3x1向量。可基于误差状态向量从方程1推导连续时间误差状态方程
[0059]   方程7
[0060] 其中 是由已估计角速率所形成的3x3反对称矩阵。该连续时间系统中的噪声协方差矩阵是
[0061]   方程8
[0062] 对于离散时间系统,可通过以下方程接近噪声协方差矩阵(假定采样时间Δt为小):
[0063]   方程9
[0064]   方程10
[0065]   方程11
[0066]   方程12
[0067] 可然后在每个陀螺仪采样步骤k通过以下方程(其中同样假定采样时间Δt为小)迭代地估计状态误差协方差P。
[0068]   方程13
[0069] Pk+1|k=FPk|kFT+Qd  方程14
[0070]   方程15
[0071]   方程16
[0072]   方程17
[0073] 3x3矩阵P11是用于四元数估计的误差协方差,并且3x3矩阵P22是用于陀螺仪偏移估计的误差协方差。在每个陀螺仪采样步骤k处,加入传感器噪声协方差Qd,所以状态误差协方差P将随时间而增加。P11的对角元素的平方根可用作陀螺仪积分结果的置信区间(1σ)。
[0074] 通过环路外陀螺仪偏移校正算法的置信区间更新
[0075] 在上述分析(即P22)中也估计陀螺仪偏移的误差协方差(置信区间)。在一个实施例中,系统使用环路外运行时间陀螺仪偏移校正算法。陀螺仪偏移校正算法可在装置静止时将陀螺仪偏移归零。当发生该类型的陀螺仪偏移校正算法时,偏移误差协方差P22将被重置为 其中σs是根据陀螺仪偏移算法的校正陀螺仪数据的置信区间(以一个σ形式)。
[0076] 用于9轴融合的置信区间
[0077] 传感器融合系统使用加速度计和/或磁强仪改善姿态角估计。加速度计数据被看作是重力方向的参考(推测上当装置移动慢时为加速度计数据的主导分量)并且磁强仪被看作是磁北方向的参考(假定没有磁扰)。以下示出用于以参考数据更新状态向量x的通式:
[0078] xk|k=xk|k-1+Kk·yk   方程18
[0079] 向量y是由参考数据z和当前状态估计x推导的校正项。存在许多可能的误差度量以推导校正项y。在一个实施例中,度量是z和Hx之间的差,其中Hx是状态向量到参考空间的投影。在一个实施例中,投影是非线性的并且可表示为x的函数,即H(x)。矩阵K是增益系数,其确定了校正项对状态向量更新的促成作用。在一个实施例中,增益系数是固定值。在另一个实施例中,K在每个步骤k处自适应。给定状态更新方程,误差协方差也可通过参考数据更新:
[0080] Pk|k=cov(xk-xk|k)   方程19
[0081] =cov(xk-(xk|k-1+Kk·yk))
[0082] =cov((xk-xk|k-1)-Kk·yk)
[0083] 在一个实施例中,如果校正项y等于z–Hx,则上述方程变为:
[0084] Pk|k=cov((xk-xk|k-1)-Kk·yk)   方程20
[0085] =cov((xk-xk|k-1)-Kk(zk-Hkxk|k-1))
[0086] =cov((xk-xk|k-1)-Kk(zk-Hkxk|k-1))
[0087] =cov((xk-xk|k-1)-Kk(zk-Hkxk|k-1))
[0088] =cov((xk-xk|k-1)-Kk(Hkxk+Vk-Hkxk|k-1))
[0089] =cov((I-KkHk)(xk-xk|k-1))-cov(Kkvk)
[0090] Pk|k=(I-KkHk)Pk|k-1(I-KkHk)T-KkRkKkT
[0091] 其中vk是在参考zk和来自状态向量的当前估计之间的差。Rk是vk的协方差。Pk|k-1是从陀螺仪积分获得的误差协方差矩阵。上述方程用来自参考数据的信息更新P。
[0092] 如先前章节中提及的,来自加速度计和磁强仪的“参考数据”易于被传感器缺点和外界干扰歪曲。为了处理传感器缺点,一个实施例通过将加速度计/磁强仪偏移和/或比例因数加入状态向量x,即使x向量的维数从6增加至12或更高,将应用环路内校正算法。通过检查加速度计/磁强仪数据和通过状态向量的已估计值(即vk)之间的差是否超过置信区间,可检测外部干扰。如果检测到干扰,则系统舍弃当前加速度计/磁强仪数据。
[0093] 通过环路外加速度计和磁强仪偏移校正算法的置信区间更新
[0094] 对于使用环路外加速度计和磁强仪偏移校正算法的系统,通过运行蒙特卡洛测试或基于输入数据概率分布(由其置信区间指示)的粒子滤波,可估计系统输出的置信区间。
[0095] 扰动情况
[0096] 在磁扰期间,磁强仪传感器不可向系统提供移动方向参考。单纯地根据陀螺仪传感器计算移动方向统计。
[0097] 当存在非运动情况时,更新陀螺仪偏移。通过新的陀螺仪偏移更新,可将陀螺仪偏移随机走动输出重置为零,即移动方向误差界限不增加。仅当陀螺仪偏移更新停止时,移动方向误差界限将继续增大。
[0098] 当移动方向误差界限超过特定阈值(例如10度)时,移动方向准确度值应当从3掉落至0,以给予用户关于潜在巨大移动方向误差的警告。
[0099] 通过适当的“图8”运动,系统将通过产生新的磁强仪偏移估计而适应磁扰。然后,系统将恢复至正常情况。
[0100] 降阶次优化不确定性估计
[0101] 数学上,可通过具有某些统计特性(诸如平均值、标准差)的随机过程建模不确定性变量。通过这些统计特性,能够分析(并估计)随机动态系统中的不确定信号的统计。
[0102] 现在,我们可将所有传感器数据看作是具有某些统计(平均值和标准差)的随机变量。陀螺仪积分环路的四元数输出是平均值,而所有不确定性被推至误差环路。然后通过对其误差信号的统计特性的研究充分地估计统计特性。
[0103] 通过该想法,能够通过串联的误差环路使多种状态去耦并且使之间的相互作用简化。多种状态之间的相互作用被包含至环路误差。
[0104] 3D海拔高度估计系统产生移动方向(偏航角)、俯仰角和横摇角的输出。在图3a中示出这3种状态的估计环路误差。移动方向统计可通过外环路误差估计,而俯仰统计和横摇统计可通过内环路误差估计。环路误差将提供不确定性界限估计。
[0105] 在内反馈环路中,倾斜误差通过来自加速度计的重力参考补偿。如第3节所述,如果无环路外校正模块补偿陀螺仪缺点,则内环路中的移动方向误差界限将随时间而增大。
[0106] 外反馈环路补偿由磁强仪引起的移动方向误差。然而,因为磁强仪用作外反馈环路中的参考,所以磁强仪的缺点(例如,偏移误差、磁扰等)无法通过其自身的反馈机构检测和补偿。因此,磁强仪移动方向误差估计被分成2种情况,如图4所示:
[0107] 1)磁场异常情况:当检测到磁场扰动时,磁强仪数据是不可信的。因此,外反馈环路打开。移动方向估计基本上来自内反馈环路(来自加速度计的倾斜补偿)。陀螺仪积分是该情况下的移动方向误差的主要原因。
[0108] 2)正常情况:通过双反馈环路估计移动方向。通过加速度计和磁强仪取消陀螺仪积分误差。移动方向误差主要来自于加速度计和磁强仪的缺点。
[0109] 磁扰可通过磁场强度和方向的改变来检测。用于磁扰检测的可能的实施例使用高通滤波器或比较陀螺仪和磁强仪之间的变化率。
[0110] 图4示出关于通过方框316的磁异检测的由系统300所执行的步骤的流程图400。在图4中,在步骤402处,传感器数据输入至方框316。接下来,在404处,如果检测到磁异常,该过程前进至步骤408,在该处,根据内反馈环路(通过倾斜补偿)划定定向误差,否则,该过程前进至步骤406。在步骤406处,根据双反馈环路(通过倾斜和移动方向补偿)划定定向误差。
[0111] 图5示出在正常情况期间的移动方向统计计算过程。在图5中,在506处,示出加速度计传感器502和磁强仪传感器504的输出用于计算移动方向不确定性估计,其输出移动方向置信区间508,类似于图3a的输出362。加速度计传感器502类似于传感器308并且磁强仪传感器504类似于磁强仪传感器310。
[0112] 正常情况下的移动方向误差来自两种来源:在静止条件下,移动方向误差来自加速度计和磁强仪的缺点。例如,图6示出由加速度计偏移估计误差所引起的潜在移动方向误差。可通过来自环路外校正算法的置信区间计算静止移动方向误差界限,如下:
[0113]   方程21
[0114] 在动态条件下,移动方向可经历因线性加速的干扰所致的额外变化。用于估计动态条件下的移动方向误差界限的一个实施例是通过计算由加速度计数据和磁强仪数据形成的两个3D向量的内积的标准差:
[0115]   方程22
[0116] 在静止条件下,θ应当保持不变并且标准差最小。
[0117] 虽然具体实施方式已关于其特定实施例被描述,但这些特定实施例仅是示例性的,而非限制性的。
[0118] 如本文具体实施方式和整个随附权利要求中所使用的,“一”、“一个”以及“该”包含复数个参考,除非上下文另外清楚地说明。同样地,如本文具体实施方式和整个随附权利要求中所使用的,“在……中”的意思包含“在……中”和“在……上”,除非上下文另外清楚地说明。
[0119] 因此,虽然本文已描述了特定实施例,但是一定范围的修改、各种变化和替换都旨在处于上述公开内,并且应当理解在一些实例中,在不脱离如前所述的范围和精神的情况下,将采用特定实施例的一些特征而未对应使用其它特征。因此,可做出许多修改以使特定状况或材料适应于实质范围和精神。
QQ群二维码
意见反馈