基于ins惯性导航与gps导航以及磁力计的导航算法

文档序号:10568139阅读:261来源:国知局
基于ins惯性导航与gps导航以及磁力计的导航算法
【专利摘要】本发明公开了一种能够准确反映无人机的姿态和位置信息的基于INS惯性导航与GPS导航以及磁力计的导航算法。该导航算法利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本算法利用磁力计计算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此算法稳定性较强,能够输出比较满意的导航定位信息,从而能够准确反映载体的姿态和位置信息。适合在导航技术领域推广应用。
【专利说明】
基于I NS惯性导航与GPS导航以及磁力计的导航算法
技术领域
[0001]本发明涉及导航技术领域,尤其是一种基于INS惯性导航与GPS导航以及磁力计的 导航算法。
【背景技术】
[0002] 在导航技术方面,目前应用得最多,最成熟的导航方式有INS惯性导航和卫星导 航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点,但缺点是信号易 受干扰和遮挡,在强电磁环境下和有高楼遮挡时,信号质量变差,并且其输出频率有限,一 般为1 一 10Hz,输出不连续,在需要快速更新信息的场合,如机动性和实时性要求较高的无 人机系统上,GPS卫星导航的缺点便凸显出来。而INS惯性导航系统是一种全自主式的导航 方式,因此具有很强的隐蔽性和抗干扰的能力,并且输出信息连续,短时间内定位精度高。 但由于MEMS-INS器件自身的特点,陀螺仪和加速度计有初始零偏、随机漂移等误差,随着时 间的累计作用,其误差越来越大,长时间定位精度较差,最终无法准确反映无人机的姿态和 位置信息。

【发明内容】

[0003] 本发明所要解决的技术问题是提供一种能够准确得出载体的姿态和位置信息的 基于INS惯性导航与GPS导航以及磁力计的导航算法。
[0004] 本发明解决其技术问题所采用的技术方案为:该基于INS惯性导航与GPS导航以及 磁力计的导航算法,包括以下步骤:
[0005] A、将INS惯性导航系统的陀螺仪测得的载体角速度参数乂代入四元 数微分方程求解得到四元数耶^^^^其中^^^^^^^为陀螺仪在载体自身坐标系 下的测得的三个轴的角速度信息;
[0006] 所述四元数微分方程为: % 0 % 4l n c fl
[0007] . =0.5 b h n b mnby ^nbz 0 .?-% _么」 卜二:< 0 _Lf3_.
[0008] B、将步骤A中求解的(^,(^,(^,(^代入下式求解得到姿态矩阵^, q〇+%+q2+<h 2(m-關2)
[0009] 2(顿-祕)qo-q.+q.-q, 2(q2q, +q0ql) _2{q,tq:+q0q2) 2{q1qy-q0q]) q()-q,-q^qy _
[0010] 根据下述与方向余弦的关系式
[0011] ccs (pees/ -sin pcos f+cos psin Psin, -cos 汐 sin / €^= sin^cos# cos^cos^ sin^ cousin;/-sinpsinPeos/ -sin ^?sin;/ - cos (psm Goes y cos^cos/
[0012] 计算得出载体的INS惯性导航模块姿态角0、y,;
[0013] C、利用磁力计测得的偏航角逆替换步骤B计算得到的偏航角
[0014] D、将INS惯性导航系统的加速度计测得的加速度参数护和步骤B中求解得到姿态 矩阵 < 代入下述微分方程中求解得到载体在INS惯性导航坐标系下的东、北、天三个方向上 的速度信息VN VE VU,所述微分方程为:
[0015] r=C:fb-{2〇i+m:n)xv^gn
[0016] 其中,vn=[VN VE VU] '分别为INS惯性导航坐标系中东、北、天方向上的速度,<为 地球自转角速度,为载体绕INS惯性导航坐标系各轴向的转动角速率,g nS重力加速度;
[0017] E、将步骤D计算得出的VN VE VU分别代入下式求解得出载体在INS惯性导航系统中 的位置信息,其中L为炜度,A为经度,h为高度,
h = h (0) +/vudt,其中L (0)表示载体初始位 - ,丨 置的炜度值A(〇)表示载体初始位置的经度值,h (0)表示载体距离地球表面的初始高度。RM 表示地球子午圈上的曲率半径,Rn表示炜度圈上的曲率半径;
[0019] F、建立状态方程元(,)=f;⑴义,,(〇 + (/) % (〇和观测方程Z (t) = H(t) X〗(t) +V ⑴,
[0020] XHt)表示INS惯性导航系统在t时刻的误差状态,它是一个15维的向量,如下所 示:
[0021] INS惯性导航系统沿东、北、天方向上的速度误差;(i>x,4>y,4> z为载体的姿态角误差;SL,S入, Sh分别代表载体所在炜度、经度和高度误差;ex,ey,£2分别代表陀螺仪的随机漂移; FN(t] F\(t) ▽,.,Uz分别为加速度计的随机漂移,其中巧(〇= ,是一个15X15的矩 _ U ~Ir)」i5xl5 阵;其中?〃(〇对应于61,6%,6^,(}^,<^,4)2,81,从,611这9个参数的1吧惯性导航系统矩 阵,其非零元素如下:

[0035] 卩5(1:)为5~,5%,",(^,<^,伞2,81,5人,5]1这9个参数与陀螺仪及加速度计漂移 u;- 之间的变换矩阵,其维数是9X6,&(0= C( 03x5 ?: -〇3x3 〇3x3 _
[0036] ?(^)为£\士,£2,\,'^,孓与陀螺仪及加速度计漂移对应的1吧惯性导航系统矩 阵,是一个维数为6 X 6的对角线矩阵,表示如下:
[0037] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中,Tgx表示陀螺仪x 轴的误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的 误差模型的时间常数,T ax表示加速度计x轴误差模型的时间常数,Tay表示加速度计y轴误差 模型的时间常数,T az表示加速度计z轴误差模型的时间常数;
[0038] Gi(t) = diag[l 1 1......11] 15X15 ;
[0039] Wi(t)是一个15维的向量,如下所示:
[0040] ffi(t) = [ai a2 a3 a4 as a6 a7 as a9 aio an ai2 ai3 ai4 ais],
[0041] ai a2 a3 a4 as a6 a7 as a9 aio aii ai2 ai3 ai4 ais表;^系统过程噪声序列;
[0042] Z(t)为载体在INS惯性导航中的位置速度信息与载体在GPS导航系统中的位置速 度信息的差值,是一个6维向量,
[0043] Z(t) = [5vx+NVx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5A+Nx 5h+Nh]T,其中,Nvx 表示GPS导航系统在x方向上的速度误差,Nvy表示GPS导航系统在y方向上的速度误差,Nvz表 示GPS导航系统在z方向上的速度误差,Nx表示GPS导航系统在x方向上的位置误差,Ny表示 GPS导航系统在y方向上的位置误差,Nh表示GPS导航系统在z方向上的位置误差; /、'氧(01 ^
[_ H(和:k:(/)},r(今卞
[0045] ii.{t) = \diag[\ 1 l]:〇3xl2}
[0046] //4〇 = {〇,x(,^^[(^1/{Rx+h)cmL l];03x6}
[0047] Vv(t) = [Nvx Nvy Nvz]t
[0048] VP(t) = [Nx Ny Nz]t
[0049] G、将上述得到的连续状态方程尤G') = M0H) + G, (0% (0离散化后得到知= ① k, k-lXk-l+Wk-1,其中
[0050] 将上述得到的连续观测方程ZUhmOXKtHVU)离散化后得到Zk = HkXk+Vk;
[0051] 其中I是单位矩阵,F是INS惯性导航系统的状态转移矩阵,A t是离散化后INS惯性 导航系统的采样时间;
[0052] H、将载体在INS惯性导航系统中的位置速度信息与载体在GPS导航系统中的位置 速度信息作差得到Z( t)在k时刻的观测fg息z;
[0053] I、计算k时亥ijlNS惯性导航系统状态的最优估计值 其中,矣^ =气wm,义/MM为在k -1时刻1 N S惯性导航系统状态的最优估计值, & =化I杧是INS惯性导航系统的噪声矩阵,其大 小是由INS惯性导航元件的性能决定,ffp # $ = [/ -Rk是
[HP__ 系统测量噪声的方差阵,其大小是邮PS接收机的性能决定;
[0054] J、将计算得到的值与载体在INS惯性导航系统中的位置速度信息作差得到最 优的导航参数;
[0055] K、重复步骤H-J,得到连续的载体的导航信息参数。
[0056]本发明的有益效果:该基于INS惯性导航与GPS导航以及磁力计的导航算法利用 INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡,短时 定位精度不高,输出频率有限并且输出不连续的缺点;同时也解决了单一的INS惯性导航参 数累计误差越来越大,长时间定位精度发散的缺点,利用GPS导航长时间具有高的定位精度 的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外 界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解 决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本算法利用磁力计计 算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此算法稳定性较强,能够 输出比较满意的导航定位信息,从而能够准确反映载体的姿态和位置信息。
【附图说明】
[0057]图1为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 炜度误差值;
[0058]图2为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 炜度误差方差值;
[0059]图3为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 经度误差值;
[0060] 图4为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 经度误差方差值;
[0061] 图5为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 高度误差值;
[0062]图6为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 高度误差方差值;
[0063]图7为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 东向速度误差值;
[0064]图8为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 东向速度误差方差值;
[0065]图9为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 北向速度误差值;
[0066]图10为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出 的北向速度误差方差值;
[0067]图11为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出 的天向速度误差值;
[0068]图12为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出 的天向速度误差方差值。
【具体实施方式】
[0069]本发明所述的基于INS惯性导航与GPS导航以及磁力计的导航算法,包括以下步 骤:
[0070] A、将INS惯性导航系统的陀螺仪测得的载体角速度参数^4, <2代入四元 数微分方程求解得到四元数9(),91,92^;其中《4,64,< &为陀螺仪在载体自身坐标系 下的测得的三个轴的角速度信息;
[0071 ]所述四元数微分方程为: 4) 0 --ml-y ~minbz q〇 4i _ A ^
[0072] ? -0.5 b b (\ b % 份nby -①- 〇 ⑴nbx q! _4」 卜L <, -0 jLft」,
[0073] B、将步骤A中求解的阶叫沿冲代入下式求解得到姿态矩阵勺 ? %n.i+% 2[關2+鶴)2(muh)
[0074] Cbn= 2(q'q2-制')q^-q.+q.-q, 2{q1q^q {)q]) _2(肌+%七)2(%n 而)〇n)~q,~q2-^q,_
[0075] 根据下述0与方向余弦的关系式
[0076] cos (pees/ -sin rpcos/+ccs (psm 0siny -cos 沒 sin/ = sinpeos^ cos^?cos^ sin/9 cospsinv-siru^sin^cos/ -sin^sin;/-cos^sin 6?cos/ cos?9cos;/
[0077] 计算得出载体的INS惯性导航模块姿态角0、Y、A
[0078] C、利用磁力计测得的偏航角口替换步骤B计算得到的偏航角口:;
[0079] D、将INS惯性导航系统的加速度计测得的加速度参数护和步骤B中求解得到姿态 矩阵0代入下述微分方程中求解得到载体在INS惯性导航坐标系下的东、北、天三个方向上 的速度信息VN VE VU,所述微分方程为:
[0080] V" = c;jb - (2< + <) x vw +
[0081] 其中,vn=[VN vE vu] '分别为INS惯性导航坐标系中东、北、天方向上的速度,6^为 地球自转角速度,为载体绕INS惯性导航坐标系各轴向的转动角速率,g nS重力加速度; [0082] E、将步骤D计算得出的VN VE vu分别代入下式求解得出载体在INS惯性导航系统中 的位置信息,其中L为炜度,A为经度,h为高度,
h = h(0)+/vudt,其中L(0)表示载体初始位 置的炜度值A(〇)表示载体初始位置的经度值,h (0)表示载体距离地球表面的初始高度。Rm 表示地球子午圈上的曲率半径,Rn表示炜度圈上的曲率半径;
[0084] F、建立状态方程乂,(/) = (/)尤,(/) + G, 〇声;(〇 和观测方程Z(t)=H(t)Xi(t)+V ⑴,
[0085] X〗(t)表示INS惯性导航系统在t时刻的误差状态,它是一个15维的向量,如下所 示:
[0086] X,⑴5vr (% 戎终於况说漁.4 5 ,5Vx,Svy,Sv^ INS惯性导航系统沿东、北、天方向上的速度误差;(K,(i>y,(^为载体的姿态角误差;SL,S入, Sh分别代表载体所在炜度、经度和高度误差;ex,ey,ez分别代表陀螺仪的随机漂移; Vt,V,.,V2分别为加速度计的随机漂移,其中,是一个15X15的矩 L 0巧々)丄&15 阵;其中?〃(1:)对应于5^,5%,^,(^,<^,巾2,81,5\,5]1这9个参数的1吧惯性导航系统矩 阵,其非零元素如下:
[0100]卩5(1:)为5~,5%,^,(^,<^,巾2,81,5人,5]1这9个参数与陀螺仪及加速度计漂移 '〇3x3 Cf 之间的变换矩阵,其维数是9X6,&⑴=C: 03x3 ; _〇3x3 〇r?3_
[0101] FM(t)为£\士,£2,1,¥^'^与陀螺仪及加速度计漂移对应的1吧惯性导航系统矩 阵,是一个维数为6 X 6的对角线矩阵,表示如下:
[0102] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中,Tgx表示陀螺仪x 轴的误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的 误差模型的时间常数,T ax表示加速度计x轴误差模型的时间常数,Tay表示加速度计y轴误差 模型的时间常数,T az表示加速度计z轴误差模型的时间常数;
[0103] Gi(t) = diag[l 1 1......11] 15X15 ;
[0104] Wi(t)是一个15维的向量,如下所示:
[0105] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag ai。an ai2 ai3 ai4 ai5],
[0106] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序列;
[0107] Z(t)为载体在INS惯性导航中的位置速度信息与载体在GPS导航系统中的位置速 度信息的差值,是一个6维向量,
[0108] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5人+NX 5h+Nh]T,其中,Nvx 表示GPS导航系统在x方向上的速度误差,Nvy表示GPS导航系统在y方向上的速度误差,Nvz表 示GPS导航系统在z方向上的速度误差,N x表示GPS导航系统在x方向上的位置误差,Ny表示 GPS导航系统在y方向上的位置误差,Nh表示GPS导航系统在z方向上的位置误差; 邱):[説,?[,其中
[0110] H,(i) = {diag[\ 1 l];〇K12}
[0111] Hp {t) = {〇3x6 Idiag[(i?A/ + h) {R, + h)cosL l]:03x(l}
[0112] Vv(t) = [Nvx Nvy Nvz]T
[0113] VP(t) = [Nx Ny Nz]t
[0114] G、将上述得到的连续状态方程尤(?)= M/) .U〇 + ⑴离散化后得到Xk = ① k,k-iXk-i+Wk-丄,其中 ! =/ + 姻 +士 (厂泣)2 + 士 '21 3! 4!
[0115] 将上述得到的连续观测方程2(〇=讯〇乂1(〖)+¥(〇离散化后得到2 1< = 111^1<+¥1<; [0116]其中I是单位矩阵,F是INS惯性导航系统的状态转移矩阵,A t是离散化后INS惯性 导航系统的采样时间;
[0117] H、将载体在INS惯性导航系统中的位置速度信息与载体在GPS导航系统中的位置 速度彳g息作差得到Z(t)在k时刻的观测彳g息z;
[0118] I、计算k时刻INS惯性导航系统状态的最优估计值矣丨,為l:i_ = + A, 其中, 尤,-1 =① A--1 之-l|/r~l, 之为在k-1时刻INS惯性导航系统状态的最优估计值, 《=X (私 V此 + 及)' 巧-m = I-AAL,# 其大小是由ins惯性导航元件的性能决定,-心凡kw 系统测量噪声的方差阵,其大小是邮PS接收机的性能决定;
[0119] J、将计算得到的武^值与载体在INS惯性导航系统中的位置速度信息作差得到最 优的导航参数;
[0120] K、重复步骤H-J,得到连续的载体的导航信息参数。
[0121]本发明的有益效果:该基于INS惯性导航与GPS导航以及磁力计的导航算法利用 INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡,短时 定位精度不高,输出频率有限并且输出不连续的缺点;同时也解决了单一的INS惯性导航参 数累计误差越来越大,长时间定位精度发散的缺点,利用GPS导航长时间具有高的定位精度 的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外 界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解 决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本算法利用磁力计计 算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此算法稳定性较强,能够 输出比较满意的导航定位信息,从而能够准确反映载体的姿态和位置信息。
[0122] 图1为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 炜度误差值;图2为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得 出的炜度误差方差值;图3为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算 法计算得出的经度误差值;图4为本发明所述基于INS惯性导航与GPS导航以及磁力计的导 航算法计算得出的经度误差方差值;图5为本发明所述基于INS惯性导航与GPS导航以及磁 力计的导航算法计算得出的高度误差值;图6为本发明所述基于INS惯性导航与GPS导航以 及磁力计的导航算法计算得出的高度误差方差值;图7为本发明所述基于INS惯性导航与 GPS导航以及磁力计的导航算法计算得出的东向速度误差值;图8为本发明所述基于INS惯 性导航与GPS导航以及磁力计的导航算法计算得出的东向速度误差方差值;图9为本发明所 述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的北向速度误差值;图10为 本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的北向速度误差 方差值;图11为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计算得出的 天向速度误差值;图12为本发明所述基于INS惯性导航与GPS导航以及磁力计的导航算法计 算得出的天向速度误差方差值;
[0123] 从上述测试结果图可以看出,本发明所述基于INS惯性导航与GPS导航以及磁力计 的导航算法得出的经度、炜度、高度的误差方差均能快速收敛至比较小的数值;对位置、速 度等导航信息也能实现滤平滑作用,不会产生大的跳变,算法的稳定性较强。
【主权项】
1.基于INS惯性导航与GPS导航以及磁力计的导航算法,其特征在于包括以下步骤: A、 将INS惯性导航系统的陀螺仪测得的载体角速度参数《1,代入四元数微 分方程求解得到四元数耶^1^243;其中6匕,,^4/64 2为陀螺仪在载体自身坐标系下的 测得的三个轴的角速度信息; 所述四元数微分方程为:B、 将步骤A中求解的qo,qi,q2,q3代入下式求解得到姿态矩阵Cf,根据下述与方向余弦的关系式计算得出载体的INS惯性导航模块姿态角θ、γ C、 利用磁力计测得的偏航角Ρ替换步骤Β计算得到的偏航角 D、 将INS惯性导航系统的加速度计测得的加速度参数户和步骤Β中求解得到姿态矩阵0 代入下述微分方程中求解得到载体在INS惯性导航坐标系下的东、北、天三个方向上的速度 信息VN VE VU,所述微分方程为:其中,vn=[VN VE VU]'分别为INS惯性导航坐标系中东、北、天方向上的速度,贫〗为地球 自转角速度,<为载体绕INS惯性导航坐标系各轴向的转动角速率,gns重力加速度; E、 将步骤D计算得出的VN VE Vu分别代入下式求解得出载体在INS惯性导航系统中的位 置信息,其中L为炜度,λ为经度,h为高度,h = h(0)+Jvudt,其中L(0)表示载体初始位置的炜 度值,λ(0)表示载体初始位置的经度值,h(0)表示载体距离地球表面的初始高度。RM表示地 球子午圈上的曲率半径,RN表示炜度圈上的曲率半径; F、建立状态方程夂⑴⑴.\^) + 6(/)沙:,(/)和观测方程2(〇=11(〇沿(〇+¥(七), XMt)表示INS惯性导航系统在t时刻的误差状态,它是一个15维的向量,如下所示: ΑΜψ、'·'.. A 况:戎 4 於说汾浼 $ € Vv. VjjvxJvyJvAlNS 惯性导航系统沿东、北、天方向上的速度误差;Φχ,Φγ,Φζ为载体的姿态角误差;δ?,δλ,δ?! 分别代表载体所在炜度、经度和高度误差;ε χ,4,εζ分别代表陀螺仪的随机漂移; 分别为加速度计的随机漂移,,是一个15 X 15的矩阵;其中F N (t)对应于δνχ,δνγ,δνζ,φ χ,φ y,φ z,SL,δλ,Sh这9个参数的INS惯性导航系统矩阵,其非零 元素如下:Fs(t)为3",5%,5¥2,(^,(^,(|)2况,3入,5]1这9个参数与陀螺仪及加速度计漂移之间的 变换矩阵,Fm⑴为εχ,ey,εζ,V,,V,.,丨与陀螺仪及加速度计漂移对应的 INS惯性导航系统矩阵,是 一个维数为6 X 6的对角线矩阵,表示如下: FM(t)=diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中,Tgx表示陀螺仪X轴的 误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的误差 模型的时间常数,T ax表示加速度计X轴误差模型的时间常数,Tay表示加速度计y轴误差模型 的时间常数,T az表示加速度计z轴误差模型的时间常数; Gi(t)=diag[l 11......1 1]?5χ?5; Wi(t)是一个15维的向量,如下所示: Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5], ai a2 a3 a4 as a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序列; Z(t)为载体在INS惯性导航中的位置速度信息与载体在GPS导航系统中的位置速度信 息的差值,是一个6维向量, Ζ(?) = [δνχ+Ννχ δνγ+Ννγ δνζ+Ννζ (RM+h)3L+Ny (RM+h)cosL3A+Nx 3h+Nh]T,其中,Ννχ表不 GPS导航系统在x方向上的速度误差,Nvy表示GPS导航系统在y方向上的速度误差,Nvz表示 GPS导航系统在ζ方向上的速度误差,Νχ表示GPS导航系统在X方向上的位置误差,Ny表示GPS 导航系统在y方向上的位置误差,Nh表示GPS导航系统在z方向上的位置误差;Vv(t) = [Nvx Nvy Nvz]T Vp(t) = [Nx Ny Nz]T G、 将上述得到的连续状态方柷夂(/)二dR, (/) + 6,(/)%⑴离散化后得到Xk = ?k,k-lXk-l+Wk-l:将上述得到的连续观测方程z (t) =H( t )X: (t) +V( t)离散化后得到Zk=HkXk+Vk; 其中I是单位矩阵,F是INS惯性导航系统的状态转移矩阵,△ t是离散化后INS惯性导航 系统的采样时间; H、 将载体在INS惯性导航系统中的位置速度信息与载体在GPS导航系统中的位置速度 信息作差得到Z(t)在k时刻的观测信息ζ; I、 计算k时亥[|INS惯性导航系统状态的最优估计值之|it,之> =矣+ 卜-i/Jn), 其中,之Η = 龙为在k -1时刻IN S惯性导航系统状态的最优估计值, [,二怂)-1,尸…=Φ.-+α-^INS惯性导航系统的噪 声矩阵,其大小是由I N S惯性导航元件的性能决定:6 =[/-人J/-人ν? ,Rk是系统测量噪声的方差阵,其大小是由GPS接 收机的性能决定; J、 将计算得到的龙^值与载体在INS惯性导航系统中的位置速度信息作差得到最优的 导航参数; K、 重复步骤H-J,得到连续的载体的导航信息参数。
【文档编号】G01C21/20GK105928519SQ201610244759
【公开日】2016年9月7日
【申请日】2016年4月19日
【发明人】张瑜, 李诗扬
【申请人】成都翼比特自动化设备有限公司
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1