捷联惯性导航原理

捷联惯性导航原理2010.11.30北航通信导航与自动测试实验室如果载体真实地理位置以纬度、经度、高度表示，则与此对应的载体在地球坐标系中的真实位置(x，y，z)可通过下式求得:地球各点重力加速度近似计算公式:g=g0(1-0.00265cos&)/1+(2h/R)g0:地球标准重力加速度9.80665(m/平方秒)&:测量点的地球纬度h:测量点的海拔高度R:地球的平均半径(R=6370km)s:时间？？？？？？？？？？？？？？？？？？？？内容惯性导航中的常用坐标系1捷联惯导力学编排方程2捷联惯导系统的算法3捷联惯导系统的误差分析41.惯性导航中的常用坐标系地心惯性坐标系(下标为i)---惯性坐标系是符合牛顿力学定律的坐标系，即是绝对静止或只做匀速直线运动的坐标系。以地心为原点作右手坐标系，轴沿地轴指向地球的北极，，轴在地球赤道平面内与地轴垂直并不随地球自转，其中，轴指向春分点。（惯性-不随地球自转，所以指向春分点）春分点是天文测量中确定恒星时的起始点，因此、、均指向惯性空间某一方向不变。eiiiOxyzeOeiOzeiOxeiOyeiOxeiOxeiOyeiOz1.惯性导航中的常用坐标系地球坐标系(下标为e)—地球坐标系的原点在地球中心，轴与轴重合，在赤道平面内，轴指向格林威治经线，轴指向东经90°方向。又称为空间直角坐标系或地心地固坐标系。（地球-x轴指向0子午线）地理坐标系(东北天)(下标为t)—原点选在载体重心处，指向东，指向北，沿垂线方向指向天。是在载体上用来表示载体所在位置的东向、北向和垂线方向的坐标系。eeeeOxyzeOeeOzeiOzeeeOxyexeytttOxyztxtytz1.惯性导航中的常用坐标系导航坐标系(下标为n)—导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。指北方位系统，游离方位系统；载体坐标系（下标为b）—坐标原点位于载体的重心，轴沿载体纵轴指向前，轴沿载体横轴指向右，轴垂直于平面指向上。nnnOxyzbbbOxyzbybxbz1.惯性导航中的常用坐标系ixiyexeyeziz北东eObxbybztztxtyO地球坐标系到地理坐标系转换矩阵Ce-g=若为地理坐标系转为地球坐标系则为转置阵2.捷联惯导力学编排方程上图理解上图理解：由陀螺仪的角速度（以及地球自转等角速度得到四元数微分方程，求解出姿态矩阵：一方面提取姿态角，一方面把加速度计比力转化为导航坐标系；再由比力方程得到速度，由速度得到位置。）2.捷联惯导力学编排方程姿态角定义：ψ航向角----载体纵轴在水平面的投影与地理子午线之间的夹角，用ψ表示，规定以地理北向为起点，偏东方向为正，定义域0~360°。θ俯仰角----载体纵轴与纵向水平轴之间的夹角，用θ表示，规定以纵向水轴为起点，向上为正，向下为负，定义域-90°~+90°。γ横滚角----载体纵向对称面与纵向铅垂面之间的夹角，用γ表示，规定从铅垂面算起，右倾为正，左倾为负，定义域-180°~+180°。（载体纵向对称面和纵轴空间铅垂面）2.捷联惯导力学编排方程载体坐标系与地理坐标系之间的关系-----姿态矩阵地理坐标系绕轴负向（拇指指z负旋转）转角ψ得ox’y’z’，ox’y’z’绕x’轴转角θ得ox’’y’’’z’’，ox’’y’’’z’’再绕y’’轴转角γ则得到载体坐标系。（地理到载体即是z负xy正正，ψθγ形式）tttOxyztzbbbOxyz2.捷联惯导力学编排方程姿态矩阵：从导航坐标系(n系)到载体坐标系(b系)的变换矩阵；sinsinsincoscoscossinsinsincoscossinsincoscoscossincossinsinsincossinsincossincoscoscosbnC•ψ为航向角，θ为俯仰角，γ为横滚角•程序中转换采用这一矩阵形式导航坐标系绕三轴（zxy）依次旋转ψ角θ角γ角，则得到机体坐标系。由此，导航坐标系和机体坐标系之间的转换矩阵为（和前面的不一样？）100cos0sincossin00cossin010sincos00sincossin0cos001coscossincossinsincossinsincossinsinsincoscossincossincoscossinsinsinsinbnCcoscossincoscos2.捷联惯导力学编排方程当载体姿态发生变化时，陀螺仪就能敏感出相应的角速率，姿态矩阵亦之发生了变化，其微分方程为即更新式中，为姿态角速度构成的反对称阵。-----看陀螺加速度输出是哪个坐标系，就看小上标。，而且解算欧拉角的积分运算随着时间的增加会带来误差的累积bbbnnbnCCbnbTbbxbybznbnbnbnb0000bxbybznbnbnbbxbzbybnbnbnbnbbybzbxnbnbnbbzbybxnbnbnb2.捷联惯导力学编排方程()bbbbbbnnnbibieenibnieenniew也就是wnbb=wibb-winb式中：为地球（坐标系相对于惯性坐标系的）自转角速率在导航坐标系中的投影；为导航坐标系中相对地球坐标系的角速率在导航坐标系上的投影。（由瞬时速度求得）导航相对于地球，地球相对于惯性，相加=导航相对于惯性在n上投影。再经姿态矩阵，得到相对于b上。（如后面一页中图所示）nenwsincosNMnEenNENVRhVwRhVLRhL0cossinnieieiewwLwLnenVhRRhRRMN212.捷联惯导力学编排方程111213212223313233bnCCCCCCCCCC22C12C主主主→0+90°→0--90°+++--++180+--180主主33C主主主++-+--180--+180航向角真值表（多值问题）横滚角真值表2.捷联惯导力学编排方程速度更新计算比力方程：里面的值在前面都已列出有害加速度（即上式中所提到的加速度）哥氏加速度—向心加速度—重力加速度—g速度方程：比例方程展开2nnbnnnnbenieVCfwwVg2nniewVnnenwV11122212(2sin)(2cos)(2sin)(2cos)eeeeienieuennnieeuenuuieenbVVVfLtgLVLVRRVVVfLtgLVVRRVVVfLVVgkhkhRR2.捷联惯导力学编排方程位置更新计算（航向推算）考虑地球的椭球型：载体地理位置如下求导如右得位置更新方程式中：为载体所在的子午圈的曲率半径，为载体所在的卯酉圈的曲率半径，为地球椭球模型的半长轴，f为地球椭球模型的椭圆度，f=1/298.257223563MRNReR6378137eRm21cosRvLLRvneuhvhRRhRRMN21tntedtRvLLdtLRv020010cos00tuhhvdt2Re(1sin)NRfL)sin321Re(2LffRM注意从瞬时速度过来那条线，用来计算w（enn）3、捷联惯导系统的算法01230123(,,,)Qqqqqqqiqjqkcossin22Qu2222012312031302222212030123230122221302230101232()2()2()2()2()2()nbqqqqqqqqqqqqCqqqqqqqqqqqqqqqqqqqqqqqq3.1姿态更新算法四元数法：四元数的三角式为：四元数与姿态矩阵之间的关系：3、捷联惯导系统的算法3.1姿态更新算法四元数微分方程及计算表征n系至b系的旋转四元数为通过求导变换可得四元数微分方程（姿态角速率在变（更新），但是每一值都知道）表示成矩阵形式，则有：cossin22nQu12()()*()bnbqtqtt00112233001020bbbnbxnbynbzbbbnbxnbznbybbbnbynbznbxbbbnbznbynbxqq3、捷联惯导系统的算法四元数微分方程的解法---四阶龙格-库塔法—(适用于陀螺输出为角速度）-----应将(h/6)（1/2）等括起来实质：就是在（）时间间隔内求取多个斜率值，加权求平均得到更精确的平均斜率。)22(6/)()(4321kkkkhtqhtq)](*)([2/11ttqkbtb)}2/(*]2)({[2/112hthktqkbtb)}2/(*]2)({[2/123hthktqkbtb)}(*])({[2/134hthktqkbtbhtt,3、捷联惯导系统的算法速度更新算法-----二阶龙格库塔法速度更新微分方程为：二阶龙格库塔法求取速度增量的表达式为（加速度的均值*时间差）ak是k时刻速度微分一小段时间后的速度表示{其实不是，因为不匀加速）ak+1就是k+1时刻速度微分，注意此时括号内变成了v其中：为采样周期，、分别为k时刻及k+1时刻的加速度。则k+1时刻的速度为：1(,())kkbkVfVft21kkaaV11(,())(,())kkbkkkkbkafVftVVaafVft1kkVVVka1ka3、捷联惯导系统的算法位置更新算法—一阶龙格库塔法其中，为更新周期。0102cosenvRLvLLR0uhhv初始对准对于平台式惯导系统而言，初始对准的任务是控制平台使其跟踪导航坐标系而捷联惯性导航系统没有实体平台，其初始对准的目的是建立姿态矩阵的初始值4、捷联惯导系统的误差分析误差来源数学模型的近似性所引起的误差惯性敏感元件的误差------占系统误差的90%左右算法误差初始对准误差---一旦出现初始对准误差，它将在系统中以舒拉周期的形式传播4、捷联惯导系统的误差分析位置、速度、姿态误差方程：误差传播方程（惯导平台应模拟导航坐标系（这里模拟地理坐标系），但由于平台有误差，故平台坐标系（p）和导航坐标系（n）之间存在误差角）（即对位置速度姿态角更新方程求导）LRVhLRVRLfVLLRVLeeeecoscos1)sinRe2tansec(2112112222212sinRe3RVhRVRLfVLLnnnuhV4、捷联惯导系统的误差分析enuunnueeieueienunenuneunieeffRtgLVVVhRVLVtgLRVLVRVtgLVVLVLVLRfLVRVLVLVLV21111121)()cos2()sin2()())]sincos(sinRe2cos()sincos(2[nueeunuenuuneieeeieeenunffRVVtgLRVhRVVRVVtgLRVLVLVRLVRLfVRLVfVLV22212221122212222