CN104406592B - 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 - Google Patents
一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 Download PDFInfo
- Publication number
- CN104406592B CN104406592B CN201410712437.8A CN201410712437A CN104406592B CN 104406592 B CN104406592 B CN 104406592B CN 201410712437 A CN201410712437 A CN 201410712437A CN 104406592 B CN104406592 B CN 104406592B
- Authority
- CN
- China
- Prior art keywords
- navigation
- coordinate system
- attitude angle
- attitude
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000012937 correction Methods 0.000 title claims abstract description 30
- 238000006880 cross-coupling reaction Methods 0.000 claims abstract description 54
- 230000033001 locomotion Effects 0.000 claims abstract description 28
- 238000012545 processing Methods 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims description 70
- 238000001914 filtration Methods 0.000 claims description 44
- 239000011159 matrix material Substances 0.000 claims description 34
- 238000005096 rolling process Methods 0.000 claims description 23
- 230000003044 adaptive effect Effects 0.000 claims description 17
- 230000008569 process Effects 0.000 claims description 15
- 238000006467 substitution reaction Methods 0.000 claims description 14
- 239000013598 vector Substances 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 10
- XEBWQGVWTUSTLN-UHFFFAOYSA-M phenylmercury acetate Chemical compound CC(=O)O[Hg]C1=CC=CC=C1 XEBWQGVWTUSTLN-UHFFFAOYSA-M 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 7
- 230000035772 mutation Effects 0.000 claims description 6
- 238000011045 prefiltration Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 14
- 238000013461 design Methods 0.000 abstract description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 8
- 230000008878 coupling Effects 0.000 description 5
- 238000010168 coupling process Methods 0.000 description 5
- 238000005859 coupling reaction Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- NCGICGYLBXGBGN-UHFFFAOYSA-N 3-morpholin-4-yl-1-oxa-3-azonia-2-azanidacyclopent-3-en-5-imine;hydrochloride Chemical compound Cl.[N-]1OC(=N)C=[N+]1N1CCOCC1 NCGICGYLBXGBGN-UHFFFAOYSA-N 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公布了一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法,属于水下滑翔器导航定位技术领域。该组合导航***包括数字信号处理(DSP)模块、微机电***(MEMS)惯性测量单元(IMU)等。由于俯仰或横滚运动使得姿态角(航向角、俯仰角、横滚角)之间的交叉耦合更加明显,姿态角之间的交叉耦合导致姿态角输出不准甚至错误,进而使随后的速度、位置等其它导航信息解算发生错误,基于回溯思想提出姿态角回溯解耦方法来消除姿态角之间的交叉耦合。本***设计可以满足水下滑翔器导航***低功耗、小体积、长航时的需求,姿态角回溯解耦方法有效地解决了姿态角之间的交叉耦合,大大提高了姿态角精度,达到了水下滑翔器长航时、低功耗、高精度导航定位的目的。
Description
技术领域
本发明涉及一种用于水下滑翔器的导航***及姿态角回溯解耦方法,属于水下滑翔器导航定位技术领域。
背景技术
水下滑翔器是一种使用内在制动器通过控制自身浮力和姿态角来随水流滑翔的自主水下载体。水下滑翔器具有结构简单、功耗较低、能在水下长时间作业等特点,所以被用作海洋勘测、海洋数据搜集等作业。近年,水下滑翔器已经成为临海和开放海洋观测的重要部分。在水下执行的任务中,精确的位姿信息是至关重要的,因此水下导航是一个难点也是重点问题。
水下滑翔器的体积小,成本低,所以不能安装太多高精度水下导航传感器。对于陆地车载来说,含有差分校正的全球定位***(DGPS)能提供高精度的位姿信息,而且成本较低。但全球定位***(GPS)信号在水下不能使用。惯性导航***(INS)可以在短时间内提供精确的位姿信息,且能在没有GPS的情况下实现自主导航,基于微机电***(MEMS)的INS的优势在使得MEMS IMU在低成本惯性技术领域起到了重要作用。然而,因为陀螺仪和加速度计自身的固有偏差使得INS的误差随时间不断积累,积累的误差将会导致姿态角和位置的巨大偏差,所以需要其他传感来补偿INS的误差。
传统的方式采用电子罗盘,这个在抑制INS的位姿漂移能起到一定作用,但仅仅是电子罗盘,所达到的效果还是局限的。用航位推算方式(DR)能在保证功耗、成本等不增加的情况下尽可能达到所需位姿精度要求。所以采用INS/DR组合方式完成水下滑翔器导航。
对于水下滑翔器,虽然在一定深度下,水流比较平稳均匀,滑翔器随水流滑翔,但俯仰和横滚运动是不可避免的,。对于惯性测量单元,由于安装轴和相应的参考轴之间的误差会造成姿态角(航向角、俯仰角和横滚角)之间的交叉耦合,非零的俯仰角和横滚角使得姿态角的交叉耦合更加明显,从而造成姿态角及其它导航信息解算不准甚至错误。俯仰和横滚运动在实际中是普遍存在的,滑翔器在一定深度的水中是以较平稳的速度随水滑翔,但是其特殊的构造,必须依靠水的浮力和调节自身的俯仰角来形成锯齿波状运动,通过这种运动使得滑翔器向前滑翔。惯性测量单元作为搭载在水下滑翔器的主要导航元件,其导航信息解算的精度对滑翔器的导航与定位起着至关重要的作用。因此,低成本、低功耗、长航时的水下滑翔器导航***及高精度的位姿估计方法是目前国内外研究的重点与难点。
发明内容
发明目的:为了克服现有技术中存在的不足,本发明提供一种新的用于水下滑翔器的导航***及姿态角回溯解耦方法。
为实现上述目的,本发明采用的技术方案为:
一种用于水下滑翔器的导航***姿态角校正方法,包括以下步骤:步骤A,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤B,根据导航解算出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点,用姿态角交叉耦合造成解算错误的前一步解算数据重新计算,得出新的姿态角、速度、位置信息;
步骤C,步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后,一方面利用误差方程得到状态量,一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量;将该状态量和观测量进入基于卡尔曼的自适应滤波算法,进行姿态角、速度、位置误差的最优估计,所得误差估计值校正惯导所得的导航信息,最终得到校正后准确的姿态角、速度和位置。
所述步骤B中的回溯解耦方法包括以下步骤:
步骤B1,根据导航解算得出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点;
步骤B2,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
步骤B3,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。
所述步骤B1中判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法,包括以下步骤:
步骤B11,因俯仰或横滚运动,引起的姿态角交叉耦合而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即失准角;
步骤B12,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,为姿态校正矩阵;
步骤B13,根据姿态角校正方程可得出四元数方程:
步骤B14,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误;从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。
所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;。
所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法,包括以下步骤:
步骤B31,令
将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤B32,四元数是由四个元素构成,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量。采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为x、y、z陀螺在[tk tk+1]采样时间间隔内的角增量;
归一化后的四元数:
步骤B33,用新的四元数更新姿态矩阵:
更新三个姿态角:
航向角
俯仰角
横滚角
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置。
一种基于用于水下滑翔器的导航***,包括DSP处理单元、MEMS IMU导航元件;所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块;所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据,所有原始数据进入DSP处理单元进行解算,得到姿态角、速度及位置信息,并将得到的姿态角、速度及位置信息传送给DSP处理单元;
所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息,并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块;
所述回溯解耦模块用于根据回溯解耦方法,将正常解算过程中解算的导航信息存储起来,当解算过程错误判断后,姿态角发生奇异突变,调用上一次正确解算的导航参数进行重新解算,得出新的姿态角、速度及位置信息并对其进行更新,同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块;若姿态角未发生奇异突变,则将姿态角、速度及位置信息发送给前置滤波去噪模块;
所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息;同时将姿态角、速度和位置信息进行滤波去噪,并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块;
所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量,并将该状态量传送给基于卡尔曼的自适应滤波算法模块;
所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量,并将该观测量传送给基于卡尔曼的自适应滤波算法模块;
所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计,再用所得误差的估计值校正惯导所得的姿态角、速度及位置,最终得到校正后准确的姿态角、速度和位置信息。
一种用于水下滑翔器的导航***及姿态角回溯解耦方法,包括以下步骤:步骤一,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤二,根据导航解算得出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点;
步骤三,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
步骤四,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。
步骤二中判断因为俯仰或横滚运动使得姿态角交叉耦合明显增大而导致姿态角解算错误的节点的方法,包括以下步骤:
步骤二a,因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即是失准角;
步骤二b,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,
为姿态校正矩阵;
步骤二c,根据姿态角校正方程可得出四元数方程:
步骤二d,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误,则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。
所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;
所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影导航得出新的姿态、速度、位置信息的方法,包括以下步骤:
步骤四a,将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤四b,四元数是有四个元素构成的,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为x、y、z陀螺在[tk tk+1]采样时间间隔内的角增量;
则Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,故归一化后的四元数:
步骤四c,用新的四元数更新姿态矩阵:
更新三个姿态角:
航向角
俯仰角
横滚角
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置。
本发明提供的一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法,相比现有技术,具有以下有益效果:
(1)设计了基于MEMS IMU的水下导航***,结构简单、体积较小、功耗较低,且能高精度、长航时测姿定位。
(2)提出了姿态角回溯解耦方法,智能判断姿态角因为交叉耦合造成的错误输出,实时回溯重新解算,再次更新导航信息得到准确的姿态角速度及位置信息。
综上所述,本发明可以满足水下滑翔器导航***低功耗、小体积、长航时的需求,姿态角回溯解耦方法有效地解决了姿态角之间的交叉耦合,大大提高了姿态角精度,达到了水下滑翔器长航时、低功耗、高精度导航定位的目的。
附图说明
图1为用于水下滑翔器的水下导航***及姿态角回溯解耦方法流程图;
图2为水下滑翔器模型;
图3为水下滑翔器导航***整体框图;
具体实施方式
下面结合附图对本发明作更进一步的说明。
一种用于水下滑翔器的导航***姿态角校正方法,如图1所示,包括以下步骤:步骤A,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤B,根据导航解算出的导航数据,判断因俯仰或横滚运动,引起的姿态角交叉耦合而造成的姿态角解算错误的节点,用姿态角交叉耦合造成解算错误的前一步解算数据重新计算,得出新的姿态角、速度、位置信息;
步骤C,步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后,一方面利用误差方程得到状态量,一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量;将该状态量和观测量进入基于卡尔曼的自适应滤波算法,进行姿态角、速度、位置误差的最优估计,所得误差估计值校正惯导所得的导航信息,最终得到校正后准确的姿态角、速度和位置。
所述步骤B中的回溯解耦方法包括以下步骤:
步骤B1,根据导航解算得出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点;
步骤B2,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
步骤B3,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。
所述步骤B1中判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法,包括以下步骤:
步骤B11,因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即失准角;
步骤B12,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,为姿态校正矩阵;
步骤B13,根据姿态角校正方程可得出四元数方程:
步骤B14,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误;从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。
所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;
所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法,包括以下步骤:
步骤B31,令
将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤B32,四元数是有四个元素构成的,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tk tk+1]采样时间间隔内的角增量;
归一化后的四元数:
步骤B33,用新的四元数更新姿态矩阵:
更新三个姿态角:
航向角
俯仰角
横滚角
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置。
一种基于用于水下滑翔器的导航***,如图3所示,包括DSP处理单元、MEMS IMU导航元件;所述DSP处理单元包括存储模块、回溯解耦模块、前置滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块;所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据,所有原始数据进入DSP处理单元进行解算,得到姿态角、速度及位置信息,并将得到的姿态角、速度及位置信息传送给DSP处理单元;
所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息,并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块;
所述回溯解耦模块用于根据回溯解耦方法,将正常解算过程中解算的导航信息存储起来,当解算过程错误判断后,姿态角发生奇异突变,调用上一次正确解算的导航参数进行重新解算,得出新的姿态角、速度及位置信息并对其进行更新,同时将更新后的姿态角、速度及位置信息发送给前置滤波去噪模块;若姿态角未发生奇异突变,则将姿态角、速度及位置信息发送给前置滤波去噪模块;
所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息;同时将姿态角、速度和位置信息进行滤波去噪,并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块;
所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量,并将该状态量传送给基于卡尔曼的自适应滤波算法模块;
所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量,并将该观测量传送给基于卡尔曼的自适应滤波算法模块;
所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计,再用所得误差的估计值校正惯导所得的姿态角、速度及位置,最终得到校正后准确的姿态角、速度和位置信息。
一种用于水下滑翔器的导航***及姿态角回溯解耦方法,如图1、2所示,包括以下步骤:步骤一,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤二,根据导航解算得出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点;
步骤三,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
步骤四,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息。
步骤二中判断因为俯仰或横滚运动使得姿态角交叉耦合明显增大导致姿态角解算错误的节点的方法,包括以下步骤:
步骤二a,因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即失准角;
步骤二b,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,为姿态校正矩阵;
步骤二c,根据姿态角校正方程可得出四元数方程:
步骤二d,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误,则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点。
所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影导航得出新的姿态、速度、位置信息的方法,包括以下步骤:
步骤四a,将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤四b,四元数是由四个元素构成的,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tk tk+1]采样时间间隔内的角增量;
归一化后的四元数:
步骤四c,用新的四元数更新姿态矩阵:
更新三个姿态角:
航向角
俯仰角
横滚角
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置。
为了便于理解本发明,现将本发明的原理进行如下说明:
一种用于水下滑翔器的导航***:
如图3所述,(1)选用低功耗元器件,包括数字信号处理(DSP)单元、微机电***(MEMS)惯性测量单元(IMU)模块等,使***设计简单、集成度高、体积小、功耗低。选用新型的基于MEMS的IMU模块,其中含有三轴陀螺仪、三轴加速度计及三轴磁力计。该IMU模块结合了iMEMS与混合信号处理技术,提供较准的数字惯性检测。SPI接口和简单的输出寄存器结构实现了方便的数据访问与配置控制。IMU内部的惯性传感器在各个轴上执行精密对准,并对失调和灵敏度进行较准。MCU等核心CPU能动态补偿对内部传感器的主要影响。***还具有以下特点:条件监控、数字滤波及采样速率、辅助数字输入/输出、自动检测、***内自动偏置较准及电源管理等。
所述的DSP单元选用TI公司生产的C5500系列芯片,MEMS IMU模块选用AD公司生产的ADIS16400系列产品。DSP芯片和MEMS IMU模块都是低功耗产品,在满足导航精度的前提下将功耗降到最低,体积减小到最小。
由IMU输出的原始数据(陀螺仪的角速度、加速度计的加速度、磁力计的磁场强度)进入导航解算模块进行位姿解算。本发明中优化了导航解算模块,使该模块准确高效。设计思路是:采用一个计时器定时触发测量,所有测量过程靠中断推进,用查询方式不断检查测量是否完成,完成即进入解算。这样,解算和测量同时进行,不会在等待测量上浪费大量时间,通过计时器触发测量,最大程度地保证积分间隔的准确性。
(2)姿态角的交叉耦合使得姿态的测量误差大大增加,甚至造成错误,建立水下滑翔器模型,分析姿态角交叉耦合原因。姿态角回溯解耦方法包括姿态角错误判断、重新解算、导航信息再更新等步骤。判断阶段准确判断出姿态角解算错误的节点,在正常导航解算过程中存储重要参数信息,然后利用存储的重要参数回溯解耦得到正确的解算数据,最后重新更新得到准确的姿态角速度位置信息。
水下滑翔器姿态角耦合建模如下:
(a)水下滑翔器在一定深度下随水滑翔,速度稳定且均匀。
(b)如图2所示,水下滑翔器的线速度和角速度分别定义为v=[vx vy vz]T和w=[ωx ωy ωz]T。姿态角定义为η=[ψ θ γ]T,姿态角包括航向角、俯仰角和横滚角。一般地,假定滑翔器动力学是不受滑翔器在惯性坐标系下三个方向的绝对位置的影响。
(c)动力学方程为:
其中,
03代表零矩阵,I3代表单位阵,定义
和
其中质量项mx,增加的质量项my=mz;惯性项Jy,增加的惯性项Jx=Jz。
(d)交叉耦合矩阵为:
重力中心在[xcg ycg zcg]T,船体质量为m。假定滑翔器的重心和浮力中心没有横向偏移,即xcg=0。若滑翔器对于xz轴不对称,则交叉增加的质量项非零。
(e)滑翔器的速度、冲角α和侧滑角β满足下式:
α=tan-1(vz/vy),β=sin-1(vx/v),(4)
其中
在交叉耦合模型中,滑翔器滑翔速度随水流并且比较平稳,,可假定v=v0,v0是滑翔器在某一深度水中的初始滑翔速度。
ωy(t)和γ(t)假定为随时间变化的参数,将方程(1)简化,将线速度vx、vz代入α、β,可得方程为:
其中χθ:=T1[α ωx θ]T、T1、T2分别为系数。
在线性模型中忽略式(5)中的β是一种有必要的对称方法,运动方程可变为:
其中
(f)对于非零的横滚角,即γ(t)≠0和ωy(t)≠0,在点χ0=0和δ0=0附近线性化方程(6)得:
其中
B是系数矩阵。
***矩阵Aγ(t)中包含非零的非对角耦合项:
(g)对于航向角俯仰角θ,式(7)的第三列和第六列显示了运动耦合项,对于俯仰角θ,则有
其中为T3系数。
对于非零的横滚角γ(t),航向角速率ωz和俯仰角速率ωx不再是航向角和俯仰角θ的导数。从方程(7)和(8)能清楚地看出,非零的横滚角使得姿态角之间的交叉耦合更加明显。
(h)同理,非零的俯仰角也使得姿态角之间的交叉耦合更加明显。在实际水下滑翔器中,俯仰角和横滚角的非零情况是经常发生的,所以交叉耦合是普遍存在的。
一种用于水下滑翔器的水下导航姿态角回溯解耦方法,包括下列步骤:
(1)判断因为交叉耦合造成的姿态角解算错误的节点。在水下滑翔器姿态角耦合建模过程中,可以看出,由于姿态角之间的交叉耦合会造成三个失准角φx,φy,φz不正确,姿态校正矩阵会发生错误,导致姿态角校正方程
错误,其中是错误解算前一步正确的导航坐标系到载体坐标系的姿态角矩阵。
可得四元数方程为:
因上述的发生错误,会导致根号下的变成负数,造成四元数q0错误。同理q1、q2、q3也会发生类似的错误。本次滤波过程中姿态角解算公式:
航向角
俯仰角
横滚角
因错误,导致式(12)的三个姿态角本次解算结果是错误的,同时比力方程的计算以及速度位置的计算都会发生错误。错误的四元数q0、q1、q2、q3又会引起下次整个更新过程的错误。所以一旦式(11)中根号下的计算发生突变或者变负,说明本次解算过程已经出现问题,要停止解算。
(2)回溯解耦将正常解算过程中的解算数据存储起来,当解算过程发生错误时,调用上一次正确的解算参数进行重新解算。具体过程如下:
用因姿态角交叉耦合造成解算错误的前一步解算数据计算为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;
将上述计算的代入
其中
为在三个轴x、y、z轴上的分量。
四元数是由四个元素构成的,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量。将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tk tk+1]采样时间间隔内的角增量。
归一化后的四元数:
(3)重新更新姿态、速度及位置等信息。
用新的四元数更新姿态矩阵:
更新三个姿态角:
航向角
俯仰角
横滚角
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出速度及位置。
由上述可知:导航解算之后输出姿态、速度、位置信息,判断是否因姿态角之间的交叉耦合造成姿态角发生奇异,若没有发生奇异,则正常输出进入前置滤波去噪模块;若发生奇异,则进入姿态角回溯解耦阶段。用因耦合造成错误的上一步正确的数据进行重新解算。求载体坐标系相对于导航坐标系的转动角速率在载体坐标系上的投影用毕卡逼近法求解算是微分方程,经泰勒公式及三角形式转换后,进行四元数归一化,得到准确的四元数值。用准确的四元数更新三个姿态角,用准确的姿态变换矩阵代入公式中,算得比力,得到速度及位置。得到的位姿信息进入前置滤波去噪,经滤波去噪后,得到惯性导航器件算得的姿态、速度和位置,这些导航信息作为三方面的输入:(a)进入误差模块利用误差方程求状态量,作为基于卡尔曼的自适应滤波算法的输入;(b)当作滤波算法前的校正量;(c)和航位推算(DR)的估计值做差作为滤波器的观测量。状态量X和观测量Z同时进入基于卡尔曼的自适应滤波算法,进行姿态角、速度、位置的最优估计,再用所得误差状态的估计值去校正惯导所得的位姿数据,最终得到校正后的准确的姿态角、速度和位置信息。
以上所述仅是本发明的优选实施方式,应当指出:对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (3)
1.一种用于水下滑翔器的导航***姿态角校正方法,其特征在于,包括以下步骤:
步骤A,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤B,根据导航解算出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点,用姿态角交叉耦合造成解算错误的前一步解算数据重新计算,得出新的姿态角、速度、位置信息;
所述步骤B中的回溯解耦方法包括以下步骤:
步骤B1,根据导航解算得出的导航数据,判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点;
步骤B1中判断因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成的姿态角解算错误的节点的方法,包括以下步骤:
步骤B11,因俯仰或横滚运动,使得姿态角交叉耦合而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即失准角;
步骤B12,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,
为姿态校正矩阵;
步骤B13,根据姿态角校正方程可得出四元数方程:
步骤B14,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误;从而得出上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点;
步骤B2,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
所述步骤B2中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;
步骤B3,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息;
所述步骤B3中用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态角、速度、位置信息的方法,包括以下步骤:
步骤B31,令
将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤B32,四元数是由四个元素构成,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量,将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tk tk+1]采样时间间隔内的角增量;
故归一化后的四元数:
步骤B33,用新的四元数更新姿态矩阵:
更新三个姿态角:
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置;
步骤C,步骤B中回溯解耦后的姿态角、速度及位置信息经前置滤波器去噪后,一方面利用误差方程得到状态量,一方面和步骤A中经导航解算后得到姿态角、速度、位置信息分别作差得到观测量;将该状态量和观测量进入基于卡尔曼的自适应滤波算法,进行姿态角、速度、位置误差的最优估计,所得误差估计值校正惯导所得的导航信息,最终得到校正后准确的姿态角、速度和位置。
2.一种基于权利要求1所述的用于水下滑翔器的导航***姿态角校正方法的导航***,其特征在于:包括DSP处理单元、MEMS IMU导航元件;所述DSP处理单元包括存储模块、回溯解耦模块、滤波去噪模块、误差模块、观测量模块以及基于卡尔曼的自适应滤波算法模块;
所述MEMS IMU导航元件用于采集原始的角速度、加速度及磁场数据,所有原始数据进入DSP处理单元进行解算,得到姿态角、速度及位置信息,并将得到的姿态角、速度及位置信息传送给DSP处理单元;
所述DSP处理单元用于接收MEMS IMU导航元件传送过来的姿态角、速度及位置信息,并将该姿态角、速度及位置信息传送给回溯解耦模块和观测量模块;
所述回溯解耦模块用于根据回溯解耦方法,将正常解算过程中解算的导航信息存储起来,当解算过程错误判断后,姿态角发生奇异突变,调用上一次正确解算的导航参数进行重新解算,得出新的姿态角、速度及位置信息并对其进行更新,同时将更新后的姿态角、速度及位置信息发送给滤波去噪模块;若姿态角未发生奇异突变,则将姿态角、速度及位置信息发送给滤波去噪模块;
所述滤波去噪模块用于接收回溯解耦模块传送过来姿态角、速度及位置信息;同时将姿态角、速度和位置信息进行滤波去噪,并将滤波去噪后的姿态角、速度和位置信息分别传送给误差模块和观测量模块;
所述误差模块用于根据滤波去噪模块传送的姿态角、速度和位置信息利用误差方程求导航信息误差得到状态量,并将该状态量传送给基于卡尔曼的自适应滤波算法模块;
所述观测量模块用于根据DSP处理单元推送的姿态角、速度及位置信息与滤波去噪模块推送的姿态角、速度及位置信息作差作为滤波器的观测量,并将该观测量传送给基于卡尔曼的自适应滤波算法模块;
所述基于卡尔曼的自适应滤波算法模块用于根据状态量和观测量进行姿态角、速度、位置误差的最优估计,再用所得误差的估计值校正惯导所得的姿态角、速度及位置,最终得到校正后准确的姿态角、速度和位置信息。
3.一种用于水下滑翔器的导航***姿态角回溯解耦方法,采用权利要求1所述的用于水下滑翔器的导航***姿态角校正方法,其特征在于,包括以下步骤:
步骤一,根据陀螺仪输出的角速度、加速度计输出的加速度、磁力计输出的磁场强度进行导航信息解算,经导航解算后得到姿态角、速度、位置信息的导航数据;
步骤二,根据导航解算得出的导航数据,判断因俯仰或横滚运动,引起的姿态角交叉耦合而造成的姿态角解算错误的节点;
步骤二中判断因为俯仰或横滚运动而使得姿态角交叉耦合明显增大导致姿态角解算错误的节点的方法,包括以下步骤:
步骤二a,因俯仰或横滚运动,引起的姿态角交叉耦合而造成三个失准角φx,φy,φz错误,其中φx,φy,φz分别为载体坐标系的三轴偏离导航坐标系对应的三轴的偏离角,也即失准角;
步骤二b,将三个失准角φx,φy,φz代入姿态角校正方程其中是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态角矩阵,
为姿态校正矩阵;
步骤二c,根据姿态角校正方程可得出四元数方程:
步骤二d,判断式(11)中的根号下的计算结果是否是负数,若是负数,则四元数q0、q1、q2、q3错误,后续的导航解算会依次错误,则上述式(11)中根号下计算结果出现负数是因姿态角交叉耦合造成导航解算错误的节点;
步骤三,用因俯仰或横滚运动,使得姿态角交叉耦合明显增大而造成解算错误的前一步解算数据,计算载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;
所述步骤三中载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影为:
其中,为载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影;为陀螺仪输出的角速率;为地球坐标系相对于惯性坐标系的自转角速率在导航坐标系中的投影;为导航坐标系相对于地球坐标系的角速率在导航坐标系上的投影;是错误解算节点前一步正确的导航坐标系到载体坐标系的姿态矩阵;
步骤四,用载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得到新的姿态角、速度、位置信息;
所述步骤四中通过载体坐标系相对导航坐标系的转动角速率在载体坐标系上的投影通过解算得出新的姿态、速度、位置信息的方法,包括以下步骤:
步骤四a,将代入
其中, 为在三个轴x、y、z轴上的分量;
步骤四b,四元数是由四个元素构成,将其定义为:Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k,q0、q1、q2、q3是实数,i、j、k是相互正交的单位向量,采用毕卡逼近方法求解四元数微分方程:
其中,Q(tk+1)、Q(tk)分别代表tk+1、tk时刻的四元数向量,将式(14)进行泰勒级数展开得:
将式(15)写成三角形式:
其中,Δθx、Δθy、Δθz分别为陀螺的x、y、z轴在[tk tk+1]采样时间间隔内的角增量;
故归一化后的四元数:
步骤四c,用新的四元数更新姿态矩阵:
更新三个姿态角:
用准确的姿态变换矩阵代入公式中,算得比力fn,从而计算出新的速度及位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410712437.8A CN104406592B (zh) | 2014-11-28 | 2014-11-28 | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410712437.8A CN104406592B (zh) | 2014-11-28 | 2014-11-28 | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104406592A CN104406592A (zh) | 2015-03-11 |
CN104406592B true CN104406592B (zh) | 2017-06-06 |
Family
ID=52644239
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410712437.8A Active CN104406592B (zh) | 2014-11-28 | 2014-11-28 | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104406592B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106516055B (zh) * | 2016-10-27 | 2019-03-12 | 中国船舶科学研究中心(中国船舶重工集团公司第七0二研究所) | 一种反馈自检型水下滑翔器姿态调节装置及控制方法 |
CN106602261A (zh) * | 2016-12-21 | 2017-04-26 | 中云卫星通信有限公司 | 一种船载卫星通信***及船载天线跟踪卫星的方法 |
CN106767792A (zh) * | 2017-01-16 | 2017-05-31 | 东南大学 | 一种水下滑翔器导航***及高精度姿态估计方法 |
CN109115212B (zh) * | 2018-10-30 | 2022-04-12 | 中国船舶重工集团公司第七0七研究所 | 一种惯导***全范围姿态角提取方法 |
CN109733567B (zh) * | 2018-12-31 | 2020-01-14 | 上海孚实船舶科技有限公司 | 一种利用auv对水下作业单元进行非接触式充电的方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101881619A (zh) * | 2010-06-25 | 2010-11-10 | 哈尔滨工程大学 | 基于姿态测量的船用捷联惯导与天文定位方法 |
CN102519450A (zh) * | 2011-12-12 | 2012-06-27 | 东南大学 | 一种用于水下滑翔器的组合导航装置及方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3211449B1 (en) * | 2011-05-06 | 2021-10-27 | Hadal, Inc. | Systems and methods for synthetic aperture sonar |
-
2014
- 2014-11-28 CN CN201410712437.8A patent/CN104406592B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101881619A (zh) * | 2010-06-25 | 2010-11-10 | 哈尔滨工程大学 | 基于姿态测量的船用捷联惯导与天文定位方法 |
CN102519450A (zh) * | 2011-12-12 | 2012-06-27 | 东南大学 | 一种用于水下滑翔器的组合导航装置及方法 |
Non-Patent Citations (3)
Title |
---|
Attitude Sensing and Dynamic Decoupling Based on Active Magnetic Bearing of MSDGCMG;Jiancheng Fang et al.;《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》;20120229;第61卷(第2期);第338-348页 * |
Precise control of a magnetically suspended double-gimbal control moment gyroscope using differential geometry decoupling method;Chen Xiaocen et al.;《Chinese Journal of Aeronautics》;20130702;第26卷(第4期);第1017-1028页 * |
基于变结构的水下航行体航行姿态控制***设计方法;张西勇等;《兵工学报》;20130331;第34卷(第3期);第301-309页 * |
Also Published As
Publication number | Publication date |
---|---|
CN104406592A (zh) | 2015-03-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7299261B2 (ja) | 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム | |
CN103822633B (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN104406592B (zh) | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 | |
CN102538781B (zh) | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 | |
CN103776446B (zh) | 一种基于双mems-imu的行人自主导航解算算法 | |
WO2018214227A1 (zh) | 一种无人车实时姿态测量方法 | |
CN109459019A (zh) | 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法 | |
CN102809377A (zh) | 飞行器惯性/气动模型组合导航方法 | |
CN103712598B (zh) | 一种小型无人机姿态确定方法 | |
CN102692225A (zh) | 一种用于低成本小型无人机的姿态航向参考*** | |
CN105737854B (zh) | 一种捷联惯导***在线标定方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN106643715A (zh) | 一种基于bp神经网络改善的室内惯性导航方法 | |
CN112146655B (zh) | 一种BeiDou/SINS紧组合导航***弹性模型设计方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN113252033A (zh) | 基于多传感器融合的定位方法、定位***及机器人 | |
CN107677267A (zh) | 基于mems‑imu的室内行人导航航向反馈修正方法 | |
CN104316058B (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN108592911A (zh) | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN110779514B (zh) | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 | |
CN101929862A (zh) | 基于卡尔曼滤波的惯性导航***初始姿态确定方法 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN108759814A (zh) | 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |