专利名称：：一种基于微惯性和地磁技术的自适应三维姿态定位方法
技术领域：
：本发明属于三维姿态控制
技术领域：
，涉及一种基于微惯性和地磁技术的高效自适应三维姿态定位方法。
背景技术：
：随着微电子机械系统(MEMS)的发展，特别是微惯性技术的发展，载体的位姿获取已经从原来的平台惯导发展到捷联惯导和组合导航技术，其系统的体积、重量和成本都大大降低，因此采用惯性/地磁技术进行姿态的跟踪检测有着广泛的前景。目前，国内外已经设计出了相当多的微惯性姿态测量装置用于姿态测量跟踪，本申请人在申请号为"200810114391.4”的中国专利“基于ZigBee无线单片机的微惯性测量装置”中提供了一种可用于运动载体姿态测量的装置，该专利在本申请中引入作为参考。在该申请中，采用六轴微惯性传感器(集成了三轴微陀螺传感器和三轴微加速度传感器)和三轴磁场传感器来测量运动载体的姿态，通过基于ZigBee无线单片机对所测得的信号进行姿态解算，并将解算得到的姿态信息以无线方式传送给其他系统或者上位机。惯性跟踪技术的基本原理在于在已知初始位置和初始姿态等初始信息的基础上，依据惯性原理，对三轴微陀螺传感器获取的角速度信息进行积分获得姿态；对三轴维加速度传感器获取的加速度信息进行两次积分获取位置。但是由于存在积分累计误差效应，需要结合其他传感器对其误差值进行实时更正，从而保证其数据的准确性。基于三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器进行姿态定位的方法，其姿态值往往用四元数进行描述，四元数的引入不仅简化了计算，而且避免了欧拉角系统的万向节死锁问题。三轴微陀螺传感器的一次积分获得预测的姿态值，三轴微加速度传感器和三轴磁场传感器对预测的姿态值进行实时更正。目前的主流姿态跟踪算法为卡尔曼滤波通过三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器的更正来获取姿态信息。但是该算法复杂，卡尔曼滤波涉及到的观测噪声以及系统噪声对结果精度影响较大，而周围的噪声往往可能不是假定的白噪声，且对噪声参数的获取往往较为困难。Bachmann提出了一种基于四元数的“互补“滤波的方法(见Bachmarm，E.R.，Duman,I.,Usta,U.Y.,McGhee,R.B.,Yun,X.P.,Zyda,M.J.,“OrientationTrackingforHumansandRobotsUsingInertialSensors,"Proc.of1999SymposiumonComputationalIntelligenceinRobotics&Automation,Monterey,CA,November1999)。该方法通过使用高斯-牛顿迭代方法，利用三轴微加速度传感器和三轴磁场传感器的数据获得当前姿态的最佳误差值，然后与三轴微陀螺传感器的数据融合，从而获得姿态值。与卡尔曼滤波相比，该算法大大降低了计算复杂性，并能取得较好的姿态准确性。然而该算法没有考虑到噪声干扰等问题。在此基础上，JungKeunLee提出了利用虚拟旋转的概念来避免高斯-牛顿算法中的矩阵逆运算(见JungKeunLee；Park,Ε.J,"AFastQuaternion-BasedOrientationOptimizerviaVirtualRotationforHumanMotionTracking，，，BiomedicalEngineering,IEEETransactionsonVolume56,Issue5,May2009Page(S)1574-1582)，并提出了一种通过检测三轴微加速度传感器和三轴磁场传感器噪声是否超出阈值来确定某时刻获取的加速传感器和磁场传感器数据是否有效。但是该方法对于当加速度传感器和磁场传感器数据有效时的存在的噪声没有进行考虑。国内方面也有一些关于利用三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器来获取姿态的相关专利，本申请人在申请号为“200810224898.5”的中国专利“考虑加速度补偿和基于无�？ǘ瞬ǖ墓咝晕蛔烁俜椒ā敝刑峁┮恢侄约铀俣冉胁钩サ奈藜？ǘ瞬ü咝晕蛔烁俜椒ǎ米ɡ诒旧昵胫幸胱魑慰�。
发明内容为了解决现有技术的问题，本发明的目的是充分考虑计算复杂度以及运动载体瞬时加速度或周围环境电磁场的干扰对三轴微加速度传感器和三轴磁场传感器的影响，提出一种考虑微惯性组合测量装置瞬时加速度和周围环境电磁场干扰的基于降阶高斯-牛顿方法的快速自适应三维姿态的定位方法。为了达到上述的发明目的，本发明提出了一种基于微惯性和地磁技术的自适应三维姿态定位方法，该方法包括以下步骤步骤Sl利用集成了三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；步骤S2设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i=0时的四元数形式姿态值qO=[1000]τ、三轴微加速度传感器矢量nYA=[a0xa0yaQz]T和三轴磁场传感器矢量1^=[m0xm0ymQz]T，对三轴微加速度传感器矢量nYA和三轴磁场传感器矢量nYM进行单位化，得到单位三轴微加速度传感器矢量nsA和单位三轴磁场传感器矢量nSM，所述单位化公式为nsA=1VιιnYA11和nsM=1VιιnYM11，式中aQx、aQy、a0z表示初始时刻即i=0时的全局坐标系下的三轴微加速度传感器的加速度数据，m0x,m0y,Hitlz表示初始时刻即i=0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标η表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；步骤S3对微惯性组合测量装置的姿态值进行求解利用当前时刻即i=t时的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值qt以及步骤2获得的初始姿态值％、三轴微加速度传感器的加速度数据nYA、nSA和三轴磁场传感器的地球磁场数据nYM、nSM，来获取微惯性组合测量装置下一时刻即i=t+At时的姿态值qt+M；步骤S4利用三轴微陀螺传感器数据对i=t+At时刻微惯性组合测量装置的姿态值进行预测将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Qe=W"txcotyωtx]T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的磁场强度数据分别为bYA=[atxatyatz]mbYM=[mtxmtymtz]T，其中G、b为标记号，ωχ、coty、ωζ表示的是当前时刻t的三轴陀螺传感器的角速度数据，atx、aty、atz表示的是当前时刻t的三轴微加速度传感器的加速度数据，mtx、mty、mtz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；利用微惯性组合测量装置当前时刻t的姿态值qt和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Qe，对i=t+Δt时刻的微惯性组合测量装置的姿态值么进行预测，由于两个时刻之间的间隔Δt较�。偕枞衔猼和t+Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测纟…,方程中式对预测后的四元数进行单位化式中，qt为当前时刻t的微惯性组合测量装置的姿态值义为下一时刻预测姿态值，At为采样时间间隔，么为四元数形式的当前时刻t全局姿态下的旋转角速度；步骤S5对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子，该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小，从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置如下Μ=\式中bSM为调整后的三轴磁场传感器矢量、bSA为调整后的三轴微加速度传感器矢量；kA为三轴微加速度传感器自适应因子，kM为三轴磁场传感器自适应因子；εΑ为三轴微加速度传感器噪声上界，εΜ为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界εΑ或三轴磁场传感器噪声上界εΜ，εΑ、εΜ的取值范围是(0-1)，周围环境噪声越大，该噪声上界εΑ、εΜ值应取值越�。咛迦≈涤σ谰菰谙殖』肪持械氖笛榫龆ǎ徊街鑃6利用步骤S5调整后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值瓦，该姿态误差值通过高斯_牛顿方法获得式中bnm,+J=2[λ]Χ==个分量，式中λ表示为Wl+&,ysA，μ表示为:U"SM，λχ、λy、λz为向量λ的Jμχ、yy、μζ为向量μ的三个分量，qQ、如q2、q3为四元数礼J勺四个分量，1(《+Δ,)为四元数礼Δ,转换成的旋转矩阵；矩阵Ξ只与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据有关，所以在姿态方法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；步骤S7基于步骤S4得到的姿态预测信息、步骤S6得到微惯性组合测量装置的姿态误差值和步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值qt+At=qt+At+qt+At[1qe]kAkMAt然后再对其单位化+Δ=qt+.t/||qt+M(21)；其中表示的是四元数的乘法运算；步骤S8输出姿态信息根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义的、自适应三维姿态的俯仰角α、横滚角β和航向角γ如下a=arctg(P=arcsm(-2(qiq3+q0q2))r=arctg(ae(-180,180]β&(-90,90]re(-180,180](22)。其中利用三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器数据的更正获取姿态信息。其中微惯性组合测量装置的初始姿态标定需要进行一次矩阵逆求解，在预测过程和更正过程所有的姿态信息均以四元数的形式进行表示和运算。其中考虑周围环境中的电磁场对地球磁场的干扰和瞬时加速度的影响，如果干扰变化超过阈值，则三轴微加速度传感器和三轴磁场传感器的数据进行舍弃；如果干扰变化没超过阈值，则按照干扰的大小设置三轴微加速度传感器和三轴磁场传感器数据的自适应因子。本发明的有益效果采用本发明方法可以得到如下优点一方面，本发明与传统的卡尔曼滤波算法相比，大大降低了算法的复杂性，而算法的准确性与其他算法相比却没有明显的降低，甚至有些情况下与传统算法相比计算精度有了提高。从图4、图5、图6和表7中能够看到本发明方法与主流的姿态解算算法的输出没有差别，表1中列出了与主流的姿态解算算法的比较数据。另一方面，本发明方法中的自适应性使得在周围环境存在电磁场干扰以及瞬时加速度的情况下，降低了干扰对装置姿态值准确性的影响。如图4示出为静止、周围环境的无磁干扰的情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图4中含有的(a)图为MTi传感器与四阶龙格库塔方法的航向角比较；图4中含有的(b-d)图为MTi传感器和本算法的横滚角、航向角和俯仰角的比较；如图5示出为静止，周围环境存在磁干扰的情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图5中含有的(a-c)图三个坐标轴的测得的地磁场变化；图5中含有的(d-e)图为MTi传感器和本算法的横滚角、俯仰角和航向角的比较；在图5的情况下，当装置静止时，本发明方法和传统方法都可以准确得出当前姿态值，但是对于周围环境突然的磁场干扰，传统方法已经无法准确计算姿态值，而本发明方法却不受到相应的干扰。如图6示出为周围环境比较理想，瞬时加速情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图6中含有的(a-c)图为三个坐标轴的测得的加速度的变化；图6中含有的(d-e)图为MTi传感器和本发明方法的横滚角、俯仰角和航向角的比较；在图6情况下，对于瞬时的加速度变化，可以看到本发明方法提出的自适应因子能够缓和姿态值的突然的较大波动，从而提高了姿态值的准确性。如下示出表1为在图4和图6测试环境下，MTi和本发明方法的姿态准确性的比较，RMSE表示为均方误差大小。MTi本发明差异RMSEroll0.40/0.350.38/0.20-0.02/-0.15RMSEpitch0.31/4.810.14/4.64-0.17/-0.17RMSEyaw0.18/0.640.44/0.880.26/0.2图1为本发明所适用的惯性/地磁姿态跟踪装置的结构图；图2为本发明的一种基于微惯性/地磁技术的快速自适应算法的结构框图3进行高斯_牛顿迭代次数为1次，2次，3次的误差更正值的模值变化；图4为静止，无周围环境的磁干扰的情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。图5为静止，存在周围磁干扰的情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。图6为周围环境比较理想时，突然加速情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。图7为三维姿态定位算法的软件流程图。具体实施例方式为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。如图1示出利用本发明三维微惯性和地磁姿态跟踪装置，在三轴坐标系的X轴、Y轴和Z轴上分别设有由微加速度传感器、微陀螺传感器和磁场传感器组成的一组传感器，图1中“□”表示微加速度传感器、“〇”表示微陀螺传感器、“□”表示磁场传感器。该惯性和地磁姿态跟踪装置以正交方式集成了三个微加速度传感器、三个微陀螺传感器和三个磁场传感器，三组传感器所在的正交轴构成了装置X轴、Y轴和Z轴的三轴坐标系。如图2所示，本发明一种基于微惯性和地磁技术的自适应三维姿态的定位方法，在此仅对本发明方法相关步骤中的具体实现细节进行如下说明本发明所描述的算法的采样周期At依据不同的运行环境而不同。步骤Sl利用集成的三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；步骤S2设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i=0时的四元数形式姿态值Qtl=[1000]τ、三轴微加速度传感器矢量nYA=[a0xa0yaQz]T和三轴磁场传感器矢量1^=[m0xm0ymQz]T，对三轴微加速度传感器矢量nYA和三轴磁场传感器矢量nYM进行单位化，得到单位三轴微加速度传感器矢量nSA和单位三轴磁场传感器矢量nSM，所述单位化公式为nsA=1VιιnYA11和nsM=1VιιnYM11，其中aQx、aQy、a0z表示初始时刻即i=0时的全局坐标系下的三轴微加速度传感器的加速度数据，m0x,m0y,Hitlz表示初始时刻即i=0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标η表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；步骤S3对微惯性组合测量装置的姿态值进行求解利用当前时刻i=t的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值qt以及步骤2设定的初始姿态值％、三轴微加速度传感器的加速度数据nYA、nsA和三轴磁场传感器的地球磁场数据nYM、nSM，来获取微惯性组合测量装置下一时刻即i=t+Δt时的姿态值qt+,t；步骤S4利用三轴微陀螺传感器数据对i=t+Δt时刻微惯性组合测量装置的姿态值进行预测将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Qe=W"txcotyωtz]T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据分别为bYA=[atxatyatz]mbYM=[mtxmtymtz]T，其中G、b为标记号，ωχ、coty、ωζ表示的是当前时刻t的三轴陀螺传感器的角速度数据，atx、aty、atz表示的是当前时刻t的三轴微加速度传感器的加速度数据，mtx、mty、mtz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；利用微惯性组合测量装置当前时刻t的姿态值qt和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Qe，对i=t+Δt时刻的微惯性组合测量装置的姿态值么行预测，由于两个时刻之间的间隔Δt较�。偕枞衔猼和t+Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测方程其中么二(2)对预测后的四元数进行单位化(3)其中，qt为当前时刻t的微惯性组合测量装置的姿态值，么为下一时刻预测姿态值，At为采样时间间隔，么为四元数形式的当前时刻t全局姿态下的旋转角速度；步骤S5对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子(该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小)从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置；置信判断标准如下假设目前时刻的姿态数据准确，将初始标定时刻获得的三轴微加速度传感器和三轴磁场传感器数据结合当前的姿态数据预测出当前测得的三轴微加速度传感器和三轴磁场传感器数据，通过将预测的三轴微加速度传感器和三轴磁场传感器数据与当前时刻的三轴微加速度传感器和三轴磁场传感器数据进行模值比较如果不存在任何干扰和噪声，二者应该大小一致；如果二者之间的差值超过了设定的阈值，则认为当前测得的三轴微加速度传感器或是三轴磁场传感器数据不准确，应该予以舍弃，并以预测的三轴微加速度传感器和三轴磁场传感器的数据为准；如果二者之间的差值在设定的阈值内，则表明当前时刻测得的三轴微加速度传感器和三轴磁场传感器不存在干扰，只存在噪声，并且差值的大小表明了噪声的强弱，对差值信息进行处理，从而获得三轴微加速度传感器和三轴磁场传感器的自适应因子，具体的实现公式如下其中bSM为调整后的三轴磁场传感器矢量、bSA为调整后的三轴微加速度传感器矢量；kA为三轴微加速度传感器自适应因子，kM为三轴磁场传感器自适应因子；εΑ为三轴微加速度传感器噪声上界，εΜ为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界εΑ或三轴磁场传感器噪声上界εΜ，εΑ、εΜ的取值范围是(0-1)，周围环境噪声越大，该噪声上界εΑ、εΜ值应取值越�。凳┦比嵛⒓铀俣却衅鞯脑肷辖绂纽⊙∪】晌�0.8;三轴磁场传感器噪声上界εΜ选取可为0.9。噪声上界εΑ、εΜ的数值选择，不仅仅限于实施例的数据，噪声上界εΑ、εΜ的具体取值应依据在现场环境中的实验决定；步骤S6利用步骤S5处理后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值瓦，该误差值基于高斯_牛顿迭代法获得。利用高斯牛顿方法来获得误差更正值的原理如下给定预测的单位化的姿态值，初始标定的三轴微加速度传感器和三轴磁场传感器的矢量nsA和nSM，以及对干扰判定后的三轴微加速度传感器和三轴磁场传感器的矢量bsA，bsM，可以使用高斯-牛顿迭代来寻求当前最优的四元数，从而满足如下公式其中α，β为三轴微加速度传感器和三轴磁场传感器的在高斯牛顿方法中的姿态更正的影响权重，一般设置为1。高斯-牛顿方法可以找到使得误差值豹最小的最优姿态四元数$。一般情况下，预测值与最优姿态四元数之间只存在一个较小的旋转误差其中θ^表示旋转的角度误差，w表示的是旋转轴。从而高斯-牛顿迭代转化为寻求更正误差值瓦1qe]))假定初始更正误差值为0，则一次迭代后的更正误差值为其中，高斯_牛顿迭代的雅各比矩阵为经过相应的矩阵化简运算其中，J为/(.J的缩写，纟为纟的缩写。从而得到了下面表达式。又由于从而得到一次迭代的更正误差值为^其中α，β设置为1，从而获得更正误差值为其中Ξ=21-CSa"S/+"SM"Smt)(17)bnR(q'l+J=20.5-W-WH-她H+<1^2mi+<h<h0.51212财3-H则3—紐2H+<h<h0·512~2(18)W其中λ表示为M;+J"SA,U表示为XUSm,，λχ、λ”λζ为向量λ的三个分量，μχ、yy、μζ为向量μ的三个分量，qQ、Ql、q2、q3为四元数礼J勺四个分量，为四元数么、,转换成的旋转矩阵；因为矩阵Ξ只是与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据有关，所以在算法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；对于高斯-牛顿迭代次数问题，通过实验——迭代一次，两次，三次后姿态误差值的模值变化，见附图3中含有的(a)图、(b)图、(c)图，得知迭代次数的增加对姿态的准确性没有明显的变化(姿态误差值模值没有明显的减小)，在本方法中只需进行一次高斯-牛顿迭代即可获得较为准确的姿态误差值；步骤S7对步骤S4得到的姿态预测信息以及步骤S6得到微惯性组合测量装置的姿态误差值以及步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值^t^i^t+L·.,^IKkuAt(21)然后再对其单位化qt+At=qt+At/|qt+.t|；其中，为步骤S3获取，kAkM为步骤S5获�。硎镜氖撬脑某朔ㄔ怂悖徊街鑃8输出姿态信息根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义、自适应三维姿态的的俯仰角α、横滚角β和航向角γ如下a=arctg(^2|3ZMll)厂=arcsin(-2(仍妁+犰办))r=arctg(ae(-180,180])8e(-90,90]re(-180,180](22)。如图7所示，本实施例的工作流程如下1)系统初始化�？樽颂偕璞敢话阍诰蔡榭鱿缕舳�。首先是进行系统初始化，如对各个传感器的信号的检测，处理器芯片的初始化设置，通信系统的初始化以及自适应三维姿态方法的步骤S2的初始信息的设定。2)坐标轴校准14通过相应的转动平台等设备，检测三轴微陀螺传感器，三轴微加速度传感器和三轴磁场传感器的坐标轴校准情况，对于三个坐标轴之间的校准偏差，可以通过相应的旋转矩阵来进行补偿。如当某传感器坐标轴与设定的标准坐标轴之间存在横滚角Y，俯仰角θ，和航向角Ψ的偏差时，可以通过如下公式进行传感器数据的标定其中s是某传感器的原始数据，S'为坐标轴对准后的传感器的数据。3)传感器数据的采集采集三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的数据，为下一步的做准备。4)自适应三维姿态定位方法对于采集到的传感器数据，基于自适应三维姿态定位方法的步骤S4，S5，S6，S7，S8进行装置姿态值的计算。5)姿态参数输出对自适应三维姿态定位算法得到的姿态信息，通过相应的通信接口与外接的显示设备或是其他姿态处理设备进行通信输出。以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的权利要求书的保护范围之内。权利要求一种基于微惯性和地磁技术的自适应三维姿态定位方法，其特征在于，包括以下步骤步骤S1利用集成了三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；步骤S2设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i＝0时的四元数形式姿态值q0＝[1000]T、三轴微加速度传感器矢量nYA＝[a0xa0ya0z]T和三轴磁场传感器矢量nYM＝[m0xm0ym0z]T，对三轴微加速度传感器矢量nYA和三轴磁场传感器矢量nYM进行单位化，得到单位三轴微加速度传感器矢量nSA和单位三轴磁场传感器矢量nSM，所述单位化公式为nSA＝nYA/||nYA||和nSM＝nYM/||nYM||，式中，a0x、a0y、a0z表示初始时刻即i＝0时的全局坐标系下的三轴微加速度传感器的加速度数据，m0x、m0y、m0z表示初始时刻即i＝0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标n表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；步骤S3对微惯性组合测量装置的姿态值进行求解利用当前时刻即i＝t时的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值qt以及步骤2获得的初始姿态值q0、三轴微加速度传感器的加速度数据nYA、nSA和三轴磁场传感器的地球磁场数据nYM、nSM，来获取微惯性组合测量装置下一时刻即i＝t+Δt时的姿态值qt+Δt；步骤S4利用三轴微陀螺传感器数据对i＝t+Δt时刻微惯性组合测量装置的姿态值进行预测将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数QG＝T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的磁场强度数据分别为bYA＝[atxatyatz]T和bYM＝[mtxmtymtz]T，其中G、b为标记号，ωtx、ωty、ωtz表示的是当前时刻t的三轴陀螺传感器的角速度数据，atx、aty、atz表示的是当前时刻t的三轴微加速度传感器的加速度数据，mtx、mty、mtz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；利用微惯性组合测量装置当前时刻t的姿态值qt和当前时刻t的三轴微陀螺传感器的旋转角速度四元数QG，对i＝t+Δt时刻的微惯性组合测量装置的姿态值进行预测，由于两个时刻之间的间隔Δt较�。偕枞衔猼和t+Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测方程<mrow><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>=</mo><msub><mi>q</mi><mi>t</mi></msub><mo>+</mo><msub><mover><mi>q</mi><mo>·</mo></mover><mi>t</mi></msub><mi>Δt</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>式中<mrow><msub><mover><mi>q</mi><mo>·</mo></mover><mi>t</mi></msub><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><msub><mi>q</mi><mi>t</mi></msub><mo>⊗</mo><msub><mi>Q</mi><mi>G</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></mrow>对预测后的四元数进行单位化<mrow><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>=</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>/</mo><mo>|</mo><mo>|</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>|</mo><mo>|</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>式中，qt为当前时刻t的微惯性组合测量装置的姿态值，为下一时刻预测姿态值，Δt为采样时间间隔，为四元数形式的当前时刻t全局姿态下的旋转角速度；步骤S5对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子，该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小，从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置如下<mrow><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>=</mo><mfencedopen='{'close=''><mtable><mtr><mtd><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>/</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo></mtd><mtd><mi>if</mi></mtd><mtd><mrow><mo>(</mo><msub><mi>k</mi><mi>M</mi></msub><mo>=</mo><mn>1</mn><mo>-</mo><mo>|</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo><mo>-</mo><mo>|</mo><mo>|</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo>|</mo><mo>/</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo><</mo><msub><mi>ϵ</mi><mi>M</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub></mtd><mtd><mi>else</mi></mtd><mtd><msub><mi>k</mi><mi>M</mi></msub><mo>=</mo><mn>1</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow><mrow><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>=</mo><mfencedopen='{'close=''><mtable><mtr><mtd><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>/</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo></mtd><mtd><mi>if</mi></mtd><mtd><mrow><mo>(</mo><msub><mi>k</mi><mi>A</mi></msub><mo>=</mo><mn>1</mn><mo>-</mo><mo>|</mo><mo>|</mo><mo>|</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>|</mo><mo>|</mo><mo>-</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo>|</mo><mo>/</mo><mo>|</mo><mo>|</mo><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>Y</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub><mo>|</mo><mo>|</mo><mo><</mo><msub><mi>ϵ</mi><mi>A</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msubsup><mi>q</mi><mi>t</mi><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mo>⊗</mo><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>⊗</mo><msub><mi>q</mi><mi>t</mi></msub></mtd><mtd><mi>else</mi></mtd><mtd><msub><mi>k</mi><mrow><mi>A</mi><mo>=</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow>式中，bSM为调整后的三轴磁场传感器矢量、bSA为调整后的三轴微加速度传感器矢量；kA为三轴微加速度传感器自适应因子，kM为三轴磁场传感器自适应因子；εA为三轴微加速度传感器噪声上界，εM为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界εA或三轴磁场传感器噪声上界εM，εA、εM的取值范围是(01)，周围环境噪声越大，该噪声上界εA、εM值应取值越小，具体取值应依据在现场环境中的实验决定；步骤S6利用步骤S5调整后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值该姿态误差值通过高斯牛顿方法获得<mrow><msub><mover><mi>q</mi><mo>→</mo></mover><mi>e</mi></msub><mo>=</mo><mo>-</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><msup><mi>Ξ</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><msup><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mi>T</mi></msup><mrow><mo>(</mo><msup><mrow><mo>[</mo><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>]</mo></mrow><mo>×</mo></msup><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>+</mo><msup><mrow><mo>[</mo><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><msup><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mi>n</mi></msup><msub><mi>S</mi><mi>M</mi></msub><mo>]</mo></mrow><mo>×</mo></msup><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>b</mi></mmultiscripts><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow>式中<mrow><mi>Ξ</mi><mo>=</mo><mn>2</mn><mi>I</mi><mo>-</mo><mrow><mo>(</mo><mmultiscripts><mi>S</mi><mi>A</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mmultiscripts><mi>S</mi><mi>A</mi><mi>T</mi><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>+</mo><mmultiscripts><mi>S</mi><mi>M</mi><none/><mprescripts/><none/><mi>n</mi></mmultiscripts><mmultiscripts><mi>S</mi><mi>M</mi><mi>T</mi><mprescripts/><none/><mi>n</mi></mmultiscripts><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>17</mn><mo>)</mo></mrow></mrow><mrow><mmultiscripts><mi>R</mi><mprescripts/><mi>n</mi><mi>b</mi></mmultiscripts><mrow><mo>(</mo><msubsup><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow><mo>′</mo></msubsup><mo>)</mo></mrow><mo>=</mo><mn>2</mn><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mtd><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub></mtd><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub></mtd></mtr><mtr><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub></mtd><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mtd><mtd><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub></mtd></mtr><mtr><mtd><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub></mtd><mtd><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub></mtd><mtd><mn>0.5</mn><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>18</mn><mo>)</mo></mrow></mrow><mrow><msup><mrow><mo>[</mo><mi>λ</mi><mo>]</mo></mrow><mo>×</mo></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>λ</mi><mi>z</mi></msub></mtd><mtd><msub><mi>λ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>λ</mi><mi>z</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>λ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msub><mi>λ</mi><mi>y</mi></msub></mtd><mtd><msub><mi>λ</mi><mi>x</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>19</mn><mo>)</mo></mrow></mrow><mrow><msup><mrow><mo>[</mo><mi>μ</mi><mo>]</mo></mrow><mi>x</mi></msup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>μ</mi><mi>z</mi></msub></mtd><mtd><msub><mi>μ</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>μ</mi><mi>z</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mo>-</mo><msub><mi>μ</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msub><mi>μ</mi><mi>y</mi></msub></mtd><mtd><msub><mi>μ</mi><mi>x</mi></msub></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>20</mn><mo>)</mo></mrow></mrow>式中λ表示为μ表示为λx、λy、λz为向量λ的三个分量，μx、μy、μz为向量μ的三个分量，q0、q1、q2、q3为四元数的四个分量，为四元数转换成的旋转矩阵；矩阵Ξ只与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据有关，所以在姿态方法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；步骤S7基于步骤S4得到的姿态预测信息、步骤S6得到微惯性组合测量装置的姿态误差值和步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值<mrow><msub><mi>q</mi><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>=</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>+</mo><msub><mover><mi>q</mi><mo>^</mo></mover><mrow><mi>t</mi><mo>+</mo><mi>Δt</mi></mrow></msub><mo>⊗</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>1</mn></mtd><mtd><msub><mover><mi>q</mi><mo>→</mo></mover><mi>e</mi></msub></mtd></mtr></mtable></mfenced><msub><mi>k</mi><mi>A</mi></msub><msub><mi>k</mi><mi>M</mi></msub><mi>Δt</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>21</mn><mo>)</mo></mrow></mrow>然后再对其单位化qt+Δt＝qt+Δt/||qt+Δt||；其中表示的是四元数的乘法运算；步骤S8输出姿态信息根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义的、自适应三维姿态的俯仰角α、横滚角β和航向角γ如下<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><mi>α</mi><mo>=</mo><mi>arctg</mi><mrow><mo>(</mo><mfrac><mrow><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>1</mn></msub><mo>)</mo></mrow></mrow><mrow><msubsup><mi>q</mi><mn>0</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mrow></mfrac><mo>)</mo></mrow></mtd><mtd><mi>α</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>180,180</mn><mo>]</mo></mtd></mtr><mtr><mtd><mi>β</mi><mo>=</mo><mi>arcsin</mi><mrow><mo>(</mo><mo>-</mo><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>)</mo></mrow><mo>)</mo></mrow></mtd><mtd><mi>β</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>90,90</mn><mo>]</mo></mtd></mtr><mtr><mtd><mi>γ</mi><mo>=</mo><mi>arctg</mi><mrow><mo>(</mo><mfrac><mrow><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>)</mo></mrow></mrow><mrow><msubsup><mi>q</mi><mn>0</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup></mrow></mfrac><mo>)</mo></mrow></mtd><mtd><mi>γ</mi><mo>∈</mo><mo>(</mo><mo>-</mo><mn>180,180</mn><mo>]</mo></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>22</mn><mo>)</mo></mrow><mo>.</mo></mrow>FSA00000198254300021.tif,FSA00000198254300022.tif,FSA00000198254300026.tif,FSA00000198254300027.tif,FSA00000198254300033.tif,FSA00000198254300042.tif,FSA00000198254300043.tif,FSA00000198254300044.tif,FSA00000198254300045.tif,FSA00000198254300046.tif,FSA00000198254300048.tif2.根据权利要求1中所述的自适应三维姿态定位方法，其特征在于利用三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器数据的更正获取姿态信息。3.根据权利要求1中所述自适应三维姿态定位方法，其特征在于微惯性组合测量装置的初始姿态标定需要进行一次矩阵逆求解，在预测过程和更正过程所有的姿态信息均以四元数的形式进行表示和运算。4.据权利要求1中所述的自适应三维姿态定位方法，其特征在于考虑周围环境中的电磁场对地球磁场的干扰和瞬时加速度的影响，如果干扰变化超过阈值，则三轴微加速度传感器和三轴磁场传感器的数据进行舍弃；如果干扰变化没超过阈值，则按照干扰的大小设置三轴微加速度传感器和三轴磁场传感器数据的自适应因子。全文摘要本发明公开一种基于微惯性和地磁技术的自适应三维姿态定位方法，包括1)利用微惯性测量装置中的传感器感应载体的运动姿态；2)设置装置的初始姿态及全局坐标系下的加速度和地磁场信息；3)对装置的姿态值进行求解；4)同时利用三轴微陀螺传感器数据对装置姿态值进行预测；5)对三轴微加速度传感器和三轴磁场传感器数据进行置信判定，检测周围环境干扰，设置自适应参数；6)利用步骤5处理后的三轴微加速度传感器数据和三轴磁场传感器数据获得装置姿态误差值；7)对步骤4得到的姿态预测值以及步骤6得到的更正信息以及步骤5得到的自适应参数进行融合，获得装置姿态值；8)输出姿态信息。文档编号G01C21/20GK101915580SQ20101023121公开日2010年12月15日申请日期2010年7月14日优先权日2010年7月14日发明者弭鹏,杜清秀申请人:中国科学院自动化研究所
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
