CN104864869B - 一种载体初始动态姿态确定方法 - Google Patents
一种载体初始动态姿态确定方法 Download PDFInfo
- Publication number
- CN104864869B CN104864869B CN201510306597.7A CN201510306597A CN104864869B CN 104864869 B CN104864869 B CN 104864869B CN 201510306597 A CN201510306597 A CN 201510306597A CN 104864869 B CN104864869 B CN 104864869B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- mtr
- msup
- 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
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Secondary Cells (AREA)
Abstract
本发明公开了一种载体初始动态姿态确定方法,本方法根据获得的初始姿态矩阵求解得到θ、γ,从而得到载体初始动态姿态;所述初始姿态矩阵按如下方法确定,初始姿态矩阵是一个时变矩阵,以表示,将分解成和三部分并分别求解可得到本发明利用了GPS和加速度计信息的融合,获得精确真实的运动姿态矩阵信息。本发明可解决惯性导航产品初始姿态为晃动或者线运动状态下常规姿态测量方法不能正确获取姿态信息的缺陷,提高惯性导航产品姿态测量的准确性和适应性。
Description
技术领域
本发明涉及一种载体初始动态姿态确定方法,其作用是为机器的姿态稳定控制提供动态的高带宽的姿态信息,属于惯性导航技术领域。
背景技术
机器的姿态稳定控制中所需的姿态信息是通过惯性导航产品提供的,惯性导航产品有体积小、重量轻、可靠性高等特点,几乎为机器姿态稳定控制领域的标准配件。在机器姿态稳定控制的应用中,机器初始姿态分为两大类:一,静态,即初始姿态为一稳定的固定值;二,动态,即晃动或者运动情况下的姿态为有一定带宽的不断变化的值。机器初始姿态确定后,还要连续不间断的确定机器的实时姿态值,也就是通过惯性导航产品提供导航姿态。因此,在工程应用中需要对初始姿态为动态的机器姿态进行测量,这样才能提高机器姿态稳定控制的环境实用性以及用户体验感。
现有许多同类惯性导航产品中大都能够提供机器初始姿态为静态条件下的姿态导航。初始姿态的确定的常规方法是通过加速度计敏感重力加速度分量的方式确定的。常规方法的具体原理如下:
设n系为地理坐标系(东-北-天),粗对准后的坐标系为n’系,载体坐标系为b系,首先进行系统的粗对准。
把重力加速度和地球自转向b系投影,得到下面两式:
其中gb及ωie b通过加速度计及陀螺数据换算得到,设加速度计和陀螺输出分别为fb和ωb,为从n系到b系的坐标变换矩阵。
近似地有:
ωie b≈ωb,gb=-fb
即
其中:
从而有:
解出C11-C33后,得
求得了θ、γ后,即完成了惯性导航产品的静态初始姿态的确定。
采用这种技术措施,gb及ωie b通过加速度计及陀螺(忽略了载体晃动和线运动)的数据换算得到,导航产品只能提供静态初始姿态信息。但机器(载体)往往受到以下两种初始状态的影响:
①机器(载体)有晃动的初始状态;
②机器(载体)有线运动的初始状态;
常规方法只能在机器(载体)初始状态为静止时,才能准确获取其初始姿态信息。在机器(载体)有晃动或者线运动的初始状态时,这种姿态信息不能准确的反映机器(载体)的姿态。在实际应用中,机器的初始姿态很少有保持静止不变的,其初始姿态的运动状态都不能预先确定,可能静止、只有晃动、只有线运动或者既有晃动又有线运动等复杂的初始状态。当初始状态不为静止时,常规方案给出的结果将会产生严重偏离,导致机器稳定控制部分出现错误的判断。因此,依据这种姿态信息进行机器的稳定控制将会带来错误的控制结果。
发明内容
针对现有技术存在的上述不足,本发明的目的是提供一种载体初始动态姿态确定方法,本发明可解决惯性导航产品初始姿态为晃动或者线运动状态下常规姿态测量方法不能正确获取姿态信息的缺陷,提高惯性导航产品姿态测量的准确性和适应性。
本发明的技术方案是这样实现的:
一种载体初始动态姿态确定方法,本方法根据获得的初始姿态矩阵求解得到θ、γ,从而得到载体初始动态姿态;其特征在于:所述初始姿态矩阵按如下方法确定,初始姿态矩阵是一个时变矩阵,以表示,将分解成和三部分并分别求解可得到
矩阵其中和由运载体所在的经度和纬度求得,由初始对准经历的时间t实时确定,即
其中,L0和λ0分别为对准起始t0时刻惯导的纬度和经度,Lt和λt分别为对准起始t时刻惯导的纬度和经度;ωie指地球自转角速率;上述几式经过合并整理,得
矩阵利用陀螺输出的角运动信息,通过惯导姿态更新算法求得,且为i0系到b系的角速度变换向量,的反对称矩阵。
矩阵表示两个惯性坐标系之间的变换矩阵,为一个常值阵,可根据惯导比力方程求得;其中,
首先,将惯导比力方程改写成由等式两边同时求导,并考虑到则得故比力方程变为并将该式两边同时左乘即在ib0系上投影得其中,vb(t)是惯导对地速度在b系上的投影,通过加速度计和GPS测量信息融合后经过变换得到;
将比力方程两边同时积分,得其中分别取t=tl和t=tm两个对准过程中的不同时刻,则有和再利用矩阵构造算法可求得坐标系变换常值矩阵即
其中,为GPS速度信息和加速度计信息融合信息;
其中,vn(t)为n系下的速度,为n系下的速度的导数即加速度,为n系下的比力,gn为n系下的重力加速度,为n系下的地球自转角速度,为n系下的位置速率矩阵,为n系下的速率矩阵,n系为导航坐标系,b系为载体坐标系,i0系为惯性初始坐标系,系为载体相对于初始时刻惯性空间的惯性坐标系,为系到i0系的变换矩阵,系到n系的变换矩阵,为系下的速度,为系下的GPS速度矢量,为加速度计积分得到的速度矢量。
相比现有技术,本发明具有如下优点:
信息融合的动态姿态确定方法的关键点是实时、精确地获取晃动或者线运动状态下机器(载体)的姿态信息。本方案的优点是利用了GPS和加速度计信息的融合,获得精确真实的运动姿态矩阵信息。与常规方法相比,本方法姿态信息获取的适应性、实时性、准确性大幅度提高。从而使得机器(载体)不需要首先保持一定时间的静止状态后再开始运动,而是从始至终机器(载体)都可以处在运动状态而其稳定控制的精度和体验不会受到任何不良影响。
附图说明
图1-机器(载体)初始状态为晃动或者线运动状态的惯性导航原理框图。
图2-本发明和常规方案实施效果的比较曲线。
具体实施方式
本发明跟现有技术一样,也是根据获得的初始姿态矩阵求解得到θ、γ,从而得到载体初始动态姿态。其不同在于,本方法中初始姿态矩阵是一个时变矩阵,变量以·(t)方式表示,将t时刻的拆写成两个矩阵相乘的形式式中其中和可由运动载体所在的经度和纬度求得,可由初始对准经历的时间t实时确定,即
其中,L0和λ0分别为对准起始t0时刻惯导的纬度和经度,Lt和λt分别为对准起始t时刻惯导的纬度和经度。上述几式经过合并整理,得
求得矩阵之后,姿态矩阵的求解就转化为的求解了,表示惯导相对于初始时刻惯性空间的变换矩阵。至此,将相对于导航坐标系的初始对准姿态矩阵求解问题转化为了惯性空间为参考基准的变化矩阵求解问题。进一步可将拆写成两部分,即该式中,利用陀螺输出的角运动信息,通过惯导姿态更新算法可以实时求得矩阵且
表示两个惯性坐标系之间的变换矩阵,为一个常值阵,可根据惯导比例方程适当变形后求得。其中,
以下详细叙述变换矩阵的求解过程。
首先,将惯导比力方程改写成若由等式两边同时求导,并考虑到则可得因此,改写的比力方程变为并将该式两边同时左乘即在ib0系上投影得其中,vb(t)是惯导对地速度在b系上的投影,通过加速度计和GPS测量信息融合后经过变换得到。
将比力方程两边同时积分,得其中这两个积分式的关键在于构造它表示运动载体的对地加速度。分别取t=tl和t=tm两个对准过程中的不同时刻,如中间时刻和末了时刻,则有和再利用矩阵构造算法可求得常值矩阵即
其中,为GPS速度信息和加速度计信息融合信息。
综上所述,本发明将初始姿态矩阵分解成和三部分分别求解,其惯性导航原理如图1所示,它们均以惯性坐标系作为参考基准,因此能有效解决机器(载体)有晃动或者线运动的初始状态的姿态信息获取问题。
本方案的实施不需要在原有的常规方法硬件的基础上增加任何硬件设施,只需将新方法的运算代码写入CPU即可实现。图2所示的是本方案和常规方案实施效果的比较。机器(载体)实际姿态如图中红线所示,并非如图中蓝线所示,实际姿态是抑制有变化的。
本发明的上述实施例仅仅是为说明本发明所作的举例,而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其他不同形式的变化和变动。这里无法对所有的实施方式予以穷举。凡是属于本发明的技术方案所引申出的显而易见的变化或变动仍处于本发明的保护范围之列。
Claims (1)
1.一种载体初始动态姿态确定方法,本方法根据获得的初始姿态矩阵求解得到θ、γ,从而得到载体初始动态姿态;其特征在于:所述初始姿态矩阵按如下方法确定,初始姿态矩阵是一个时变矩阵,以表示,将分解成和三部分并分别求解可得到
矩阵其中和由运载体所在的经度和纬度求得,由初始对准经历的时间t实时确定,即
<mrow>
<msubsup>
<mi>C</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<msub>
<mi>cos&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<msub>
<mi>sin&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<msub>
<mi>cos&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<msub>
<mi>sin&lambda;</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msubsup>
<mi>C</mi>
<mi>e</mi>
<msub>
<mi>n</mi>
<mn>0</mn>
</msub>
</msubsup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msubsup>
<mi>C</mi>
<msub>
<mi>e</mi>
<mn>0</mn>
</msub>
<msub>
<mi>n</mi>
<mn>0</mn>
</msub>
</msubsup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<msubsup>
<mi>C</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
<msub>
<mi>e</mi>
<mn>0</mn>
</msub>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
其中,L0和λ0分别为对准起始t0时刻惯导的纬度和经度,Lt和λt分别为对准起始t时刻惯导的纬度和经度;ωie指地球自转角速率;经过合并整理,得
<mrow>
<msubsup>
<mi>C</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>&lambda;</mi>
<mi>t</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mn>0</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
矩阵利用陀螺输出的角运动信息,通过惯导姿态更新算法求得,且 为i0系到b系的角速度变换向量,为的反对称矩阵;
矩阵表示两个惯性坐标系之间的变换矩阵,为一个常值阵,可根据惯导比力方程求得;其中,
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
</msub>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>,</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mover>
<mi>L</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>t</mi>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mover>
<mi>&lambda;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>t</mi>
</msub>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>L</mi>
<mi>t</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
首先,将惯导比力方程改写成由等式两边同时求导,并考虑到则得故惯导比力方程变为并将该式两边同时左乘即在ib0系上投影得其中,vb(t)是惯导对地速度在b系上的投影,通过加速度计和GPS测量信息融合后经过变换得到;
将惯导比力方程两边同时积分,得其中分别取t=tl和t=tm两个对准过程中的不同时刻,则有和再利用矩阵构造算法可求得坐标系变换常值矩阵即
<mrow>
<msubsup>
<mi>C</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msubsup>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>m</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>m</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>u</mi>
<msub>
<mi>i</mi>
<mn>0</mn>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>m</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>m</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>v</mi>
<msub>
<mi>i</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msub>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>l</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>.</mo>
</mrow>
其中,为GPS速度信息和加速度计信息融合信息;
其中,vn(t)为n系下的速度,为n系下的速度的导数即加速度,为n系下的比力,gn为n系下的重力加速度,为n系下的地球自转角速度,为n系下的位置速率矩阵,为n系下的速率矩阵,n系为导航坐标系,b系为载体坐标系,i0系为惯性初始坐标系,系为载体相对于初始时刻惯性空间的惯性坐标系,为系到i0系的变换矩阵,为i0系到n系的变换矩阵,为系下的速度,为系下的GPS速度矢量,为加速度计积分得到的速度矢量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510306597.7A CN104864869B (zh) | 2015-06-05 | 2015-06-05 | 一种载体初始动态姿态确定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510306597.7A CN104864869B (zh) | 2015-06-05 | 2015-06-05 | 一种载体初始动态姿态确定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104864869A CN104864869A (zh) | 2015-08-26 |
CN104864869B true CN104864869B (zh) | 2017-11-21 |
Family
ID=53910871
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510306597.7A Active CN104864869B (zh) | 2015-06-05 | 2015-06-05 | 一种载体初始动态姿态确定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104864869B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106908095B (zh) * | 2017-01-09 | 2019-04-26 | 浙江大学 | 一种传感器数据校准特征的提取和评估方法 |
CN110036259B (zh) * | 2017-09-30 | 2021-03-30 | 华为技术有限公司 | 一种姿态矩阵的计算方法及设备 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航系统 |
CN103557871A (zh) * | 2013-10-22 | 2014-02-05 | 北京航空航天大学 | 一种浮空飞行器捷联惯导空中初始对准方法 |
CN103557876A (zh) * | 2013-11-15 | 2014-02-05 | 山东理工大学 | 一种用于天线跟踪稳定平台的捷联惯导初始对准方法 |
CN103575299A (zh) * | 2013-11-13 | 2014-02-12 | 北京理工大学 | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 |
CN103727938A (zh) * | 2013-10-28 | 2014-04-16 | 北京自动化控制设备研究所 | 一种管道测绘用惯导里程计组合导航方法 |
CN103900565A (zh) * | 2014-03-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于差分gps的惯导系统姿态获取方法 |
-
2015
- 2015-06-05 CN CN201510306597.7A patent/CN104864869B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103528587A (zh) * | 2013-10-15 | 2014-01-22 | 西北工业大学 | 自主组合导航系统 |
CN103557871A (zh) * | 2013-10-22 | 2014-02-05 | 北京航空航天大学 | 一种浮空飞行器捷联惯导空中初始对准方法 |
CN103727938A (zh) * | 2013-10-28 | 2014-04-16 | 北京自动化控制设备研究所 | 一种管道测绘用惯导里程计组合导航方法 |
CN103575299A (zh) * | 2013-11-13 | 2014-02-12 | 北京理工大学 | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 |
CN103557876A (zh) * | 2013-11-15 | 2014-02-05 | 山东理工大学 | 一种用于天线跟踪稳定平台的捷联惯导初始对准方法 |
CN103900565A (zh) * | 2014-03-04 | 2014-07-02 | 哈尔滨工程大学 | 一种基于差分gps的惯导系统姿态获取方法 |
Also Published As
Publication number | Publication date |
---|---|
CN104864869A (zh) | 2015-08-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104736963B (zh) | 测绘系统和方法 | |
CN102289306B (zh) | 姿态感知设备及其定位、鼠标指针的控制方法和装置 | |
CN101514900B (zh) | 一种单轴旋转的捷联惯导系统初始对准方法 | |
CN103090867B (zh) | 相对地心惯性系旋转的光纤陀螺捷联惯性导航系统误差抑制方法 | |
CN103900565B (zh) | 一种基于差分gps的惯导系统姿态获取方法 | |
CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
CN103727941B (zh) | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 | |
CN104501838B (zh) | 捷联惯导系统初始对准方法 | |
CN103245360A (zh) | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 | |
Li et al. | Observability analysis of non-holonomic constraints for land-vehicle navigation systems | |
CN104567931A (zh) | 一种室内惯性导航定位的航向漂移误差消除方法 | |
CN101290229A (zh) | 硅微航姿系统惯性/地磁组合方法 | |
CN104697526A (zh) | 用于农业机械的捷联惯导系统以及控制方法 | |
CN103727940B (zh) | 基于重力加速度矢量匹配的非线性初始对准方法 | |
CN105806340A (zh) | 一种基于窗口平滑的自适应零速更新算法 | |
CN104977002A (zh) | 基于sins/双od的惯性组合导航系统及其导航方法 | |
CN103674064B (zh) | 捷联惯性导航系统的初始标定方法 | |
CN102680000A (zh) | 应用零速/航向修正的光纤捷联惯组在线标定方法 | |
CN103900566B (zh) | 一种消除地球自转角速度对旋转调制型捷联惯导系统精度影响的方法 | |
Xiong et al. | A two-position SINS initial alignment method based on gyro information | |
CN109631938A (zh) | 掘进机自主定位定向系统及方法 | |
Zhang et al. | Research on accuracy enhancement of low-cost MEMS INS/GNSS integration for land vehicle navigation | |
CN104482942B (zh) | 一种基于惯性系的最优两位置对准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
EXSB | Decision made by sipo to initiate substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |