一种无人机导航系统的制作方法
【专利摘要】本发明公开了一种能够准确反映无人机的姿态和位置信息的无人机导航系统。该无人机导航系统包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本系统利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，另外，此系统成本较低，系统稳定性较强，能够输出比较满意的导航定位信息，从而能够准确反映无人机的姿态和位置信息。适合在导航技术领域推广应用。
【专利说明】
一种无人机导航系统
技术领域
[0001] 本发明涉及导航技术领域，尤其是一种无人机导航系统。
【背景技术】
[0002] 在导航技术方面，目前应用得最多，最成熟的导航方式有INS惯性导航和卫星导 航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点，但缺点是信号易 受干扰和遮挡，在强电磁环境下和有高楼遮挡时，信号质量变差，并且其输出频率有限，一 般为1 一 10Hz，输出不连续，在需要快速更新信息的场合，如机动性和实时性要求较高的无 人机系统上，GPS卫星导航的缺点便凸显出来。而INS惯性导航系统是一种全自主式的导航 方式，因此具有很强的隐蔽性和抗干扰的能力，并且输出信息连续，短时间内定位精度高。 但由于MEMS-INS器件自身的特点，陀螺仪和加速度计有初始零偏、随机漂移等误差，随着时 间的累计作用，其误差越来越大，长时间定位精度较差，最终无法准确反映无人机的姿态和 位置信息。
【发明内容】
[0003] 本发明所要解决的技术问题是提供一种能够准确反映无人机的姿态和位置信息 的无人机导航系统。
[0004] 本发明解决其技术问题所采用的技术方案为:该无人机导航系统，包括INS惯性导 航系统模块、磁力计、GPS导航系统模块、控制中心，所述INS惯性导航系统模块包括陀螺仪 和加速度计，所述陀螺仪、加速度计、磁力计、GPS导航系统模块分别与控制中心信号连接， 所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心，所述 磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心，所述加速度计 用于测量无人机的加速度并将其测得的加速度参数传递给控制中心，所述GPS导航系统模 块用于测量无人机在GPS导航系统中的位置速度信息，所述控制中心根据接收到的角速度 参数、偏航角参数、加速度参数计算出无人机在INS惯性导航系统中的位置速度信息，控制 中心将无人机在GPS导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信 息进行Kalman滤波并进行导航误差估计，然后利用导航误差估计修正无人机在INS惯性导 航系统中的位置速度信息从而得到最终的导航信息。
[0005] 进一步的是，所述陀螺仪、加速度计、磁力计、分别与控制中心通过12C总线连接。
[0006] 进一步的是，所述GPS导航系统模块与控制中心通过USART串口连接。
[0007] 进一步的是，所述控制中心采用如下方法计算得到最终的导航信息，其具体计算 方法如下所述：
[0008] A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得 到四元数9〇^142^3;其中》1，《1.，》^为陀螺仪在无人机自身坐标系下的测得的三个 轴的角速度信息；
[0009] 所述四元数微分方程为： % 0 -°thy fo 0 ①L -^ihy <h
[0010] ? =0.5 b b n b % ^nhy 0 組-€2 _4」 卜L < -《 0」U」<
[0011] B、将步骤A中求解的卯，(11，(12，(13代入下式求解得到姿态矩阵0， qQ+q]+q2+q% 2(明2+她)2(q'q:「q(、c，2)
[0012] Cbn= 2(q]q1-q{)q-) q.-q.+q.-q, 2(^+M) _2(q]q:,+q{)q1) 2\q2q,-q()q,) q{)-q,-q2^q,_
[0013] 根据下述C"6与方向余弦的关系式
[0014] cos 妒cos / -sin f/>cos v+a^s (pm\ Osmy -cos 沒sin / Qt - sin<^cos6? cospcos 夕 sin 汐 cos (psm /-sin ^sin ^cos y -sinpsiiif-cos 供sin0eosf cos沒cos,
[0015] 计算得出无人机的INS惯性导航模块姿态角0、丫，；
[0016] C、利用磁力计测得的偏航角P替换步骤B计算得到的偏航角
[0017] D、将加速度计测得的加速度参数炉和步骤B中求解得到姿态矩阵g代入下述微分 方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息VN VE VU，所述微分方程为：
[0018] vw=C；/-(2<+<)xvw+gw
[0019] 其中，vn=[VN VE vu]'分别为INS惯性导航坐标系中东、北、天方向上的速度，<为 地球自转角速度，?i为无人机绕INS惯性导航坐标系各轴向的转动角速率，g n为重力加速 度；
[0020] E、将步骤D计算得出的VN VE VU分别代入下式求解得出无人机在INS惯性导航系统 中的位置信息，其中L为炜度，A为经度，h为高度，
h = h(0)+/vudt，其中L(0)表示无人机初始 位置的炜度值A(0)表示无人机初始位置的经度值，h(0)表示无人机距离地球表面的初始 高度。Rm表示地球子午圈上的曲率半径，Rn表示炜度圈上的曲率半径；
[0022] F、建立状态方程尤（/) = 6(/) A', (/) + 0)(/)%(/)和观测方程ZU^mOXKO+V (thXKt)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示： &(/)=[加 r %為疼铨见况浼尽今巧%]，51， 5々，^为1奶惯性 导航系统沿东、北、天方向上的速度误差；4>x，4>y，4>z为无人机的姿态角误差;SL，SA，Sh分 别代表无人机所在炜度、经度和高度误差；ex，ey，ez分别代表陀螺仪的随机漂移； 分别为加速度计的随机漂移，其中，是一个15X15的矩阵;其中FN M V /Jl5xl'5 (t)对应于"，^，^，(}^，(^，伞2，乩，6入，恥这9个参数的1吧惯性导航系统矩阵，其非零 元素如下：
[0036] 卩5(1:)为5~，5%，"，（^，<^，伞2,81，5人，5]1这9个参数与陀螺仪及加速度计漂移 _u_ 之间的变换矩阵，其维数是9\6,6(『)=C 〇3x3 ; _〇3x3 〇3x3_
[0037] ?(^)为￡\士，￡2，1，￥,.，1与陀螺仪及加速度计漂移对应的1吧惯性导航系统矩 阵，是一个维数为6 X 6的对角线矩阵，表示如下：
[0038] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺仪x 轴的误差模型的时间常数，T gy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的 误差模型的时间常数，T ax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差 模型的时间常数，T az表示加速度计z轴误差模型的时间常数；
[0039] Gi(t) = diag[l 1 1......11] 15X15 ；
[0040] Wi(t)是一个15维的向量，如下所示：
[0041] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0042] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序
[0043] 列；
[0044] Z(t)为无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中 的位置速度信息的差值，是一个6维向量，
[0045] 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方向上的速度误差，N x表示GPS导航系统在x方向上的位置误差，Ny表示 GPS导航系统在y方向上的位置误差，N h表示GPS导航系统在z方向上的位置误差；
[0046]冲)=un 其中
[0047] //,.(/) = {?'/V/g[l 1 l]：〇.xl2}
[0048] //,,(/) = {〇￥;必喂[(/?,,,+/?)(义+/?)cosL
[0049] Vv(t) = [Nvx Nvy Nvz]t
[0050] VP(t) = [Nx Ny Nz]t
[0051] G、将上述得到的连续状态方程鳥(今⑷:+句的心⑴离散化后得到xk = ① k, k-lXk-l+Wk-1，其中
[0052] 将上述得到的连续观测方程ZUhmOXKtHVU)离散化后得到Zk = HkXk+Vk;
[0053] 其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，A t是离散化后INS惯性 导航系统的采样时间；
[0054] H、将无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中的 位置速度信息作差得到Z(t)在k时刻的观测信息z;
[0055] I、计算k时刻INS惯性导航系统状态的最优估计值％，夂|lt = + Aw)， 其中，之|iM = 为在k-1时刻，INS惯性导航系统状态的最优估计值， 心=&_|//((//而_,扣'+从1，匕_1=~_心(^， 1+0._14-1是1吣惯性导航系统的噪声矩阵，其 'Bl T 大小是由INS惯性导航元件的性能决定，私= +KkRkKl， Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；
[0056] J、将计算得到的值与无人机在INS惯性导航系统中的位置速度信息作差得到 最优的导航参数；
[0057] K、重复步骤H-J，得到连续的无人机导航信息。
[0058]本发明的有益效果:该无人机导航系统包括INS惯性导航系统模块、磁力计、GPS导 航系统模块、控制中心，通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS 导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时 也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用 GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发 散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰 和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂 移较大的情况，本系统利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏 航角，另外，此系统成本较低，系统稳定性较强，能够输出比较满意的导航定位信息，从而能 够准确反映无人机的姿态和位置信息。
【附图说明】
[0059] 图1为本发明所述无人机导航系统计算得出的炜度误差值；
[0060] 图2为本发明所述无人机导航系统计算得出的炜度误差方差值；
[0061] 图3为本发明所述无人机导航系统计算得出的经度误差值；
[0062] 图4为本发明所述无人机导航系统计算得出的经度误差方差值；
[0063] 图5为本发明所述无人机导航系统计算得出的高度误差值；
[0064] 图6为本发明所述无人机导航系统计算得出的高度误差方差值；
[0065] 图7为本发明所述无人机导航系统计算得出的东向速度误差值；
[0066] 图8为本发明所述无人机导航系统计算得出的东向速度误差方差值；
[0067] 图9为本发明所述无人机导航系统计算得出的北向速度误差值；
[0068] 图10为本发明所述无人机导航系统计算得出的北向速度误差方差值；
[0069] 图11为本发明所述无人机导航系统计算得出的天向速度误差值；
[0070] 图12为本发明所述无人机导航系统计算得出的天向速度误差方差值。
【具体实施方式】
[0071] 本发明所述的无人机导航系统，包括INS惯性导航系统模块、磁力计、GPS导航系统 模块、控制中心，所述INS惯性导航系统模块包括陀螺仪和加速度计，所述陀螺仪、加速度 计、磁力计、GPS导航系统模块分别与控制中心信号连接，所述陀螺仪用于测量无人机的角 速度参数并将其测得的角速度参数传递给控制中心，所述磁力计用于测量无人机的偏航角 并将其测得的偏航角参数传递给控制中心，所述加速度计用于测量无人机的加速度并将其 测得的加速度参数传递给控制中心，所述GPS导航系统模块用于测量无人机在GPS导航系统 中的位置速度信息，所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算 出无人机在INS惯性导航系统中的位置速度信息，控制中心将无人机在GPS导航系统中的位 置速度信息与无人机在GPS导航系统中的位置速度信息进行Kalman滤波并进行导航误差估 计，然后利用导航误差估计修正无人机在INS惯性导航系统中的位置速度信息从而得到最 终的导航信息。该无人机导航系统包括INS惯性导航系统模块、磁力计、GPS导航系统模块、 控制中心，通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易 受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时也解决了单 一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用GPS导航长时 间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利 用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有 限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况， 本系统利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，另外，此系 统成本较低，能够输出比较满意的导航定位信息，从而能够准确反映无人机的姿态和位置 fg息。
[0072]在上述实施方式中，为了保证信号传递的时效性与准确性，所述陀螺仪、加速度 计、磁力计、分别与控制中心通过I2C总线连接。所述GPS导航系统模块与控制中心通过 USART串口连接。
[0073] 所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所 述：
[0074] A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得 到四元数9〇^142^3;其中《4，《4，￡4为陀螺仪在无人机自身坐标系下的测得的三个 轴的角速度信息；
[0075]所述四元数微分方程为： 4 0 -^nby ^nbz 4 A.似L 〇 -°tby %
[0076] . =〇-5 , b b % ^nby ^nbz 0 % _d Q」U_. *
[0077] B、将步骤A中求解的(^，(^，(^，阳代入下式求解得到姿态矩阵^；4， %+%+(h+^ 2(循
[0078] C=: 2(桃-姚)q^-q.+q.-q, l(q2qi+q0ql) -
[0079] 根据下述与方向余弦的关系式
[0080] cos (pees/ -sin rpcos/+ccs (psm 0siny -cos 沒 sin/ Cj = sir^9cos<9 cosf>cos^ sin/9 cospsinv-siru^sin^cos/ -sin^sin;/-cos^sin 6?cos/ cos?9cos;/
[0081] 计算得出无人机的INS惯性导航模块姿态角0、y、P;
[0082] C、利用磁力计测得的偏航角识替换步骤B计算得到的偏航角
[0083] D、将加速度计测得的加速度参数护和步骤B中求解得到姿态矩阵Cf代入下述微分 方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息VN VE VU，所述微分方程为：
[0084] vw =Clfb-( 2< + < ) x vw +
[0085] 其中，vn=[VN ve vu] '分别为INS惯性导航坐标系中东、北、天方向上的速度，<丨为 地球自转角速度，为无人机绕INS惯性导航坐标系各轴向的转动角速率，g n为重力加速 度；
[0086] E、将步骤D计算得出的VN VE vu分别代入下式求解得出无人机在INS惯性导航系统 中的位置信息，其中L为炜度，A为经度，h为高度，
h = h (0) +/vudt，其中L (0)表示无人机初始 位置的炜度值A(0)表示无人机初始位置的经度值，h(0)表示无人机距离地球表面的初始 高度。Rm表示地球子午圈上的曲率半径，Rn表示炜度圈上的曲率半径；
[0088] F、建立状态方程尤（〇 = 6 (/)A(/) + G,,(/)%(/)和观测方程ZUhmOXKO+V (thXKt)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示： 不(和% 喪略4:说说激今今4 ▽2]，~，^，^为1奶惯性 导航系统沿东、北、天方向上的速度误差；4>x，4>y，为无人机的姿态角误差;SL，SA，Sh分 别代表无人机所在炜度、经度和高度误差；ex，ey，ez分别代表陀螺仪的随机漂移；V.pV,.，V z 分别为加速度计的随机漂移，其中巧(0= ，是一个15X15的矩阵;其中FN L 0 ~(，)L (t)对应于"，^，^，(}^，(^，伞2，乩，6入，恥这9个参数的1吧惯性导航系统矩阵，其非零 元素如下：
[0102] 卩5(1:)为5~，5%，"，（^，<^，伞2,81，5入，5]1这9个参数与陀螺仪及加速度计漂移 _03x3 c_ 之间的变换矩阵，其维数是96,巧(/)= q 03x3 ; _〇3x3 〇3x3_
[0103] FM(t)为￡^￡^￡2，'^，％，'?2与陀螺仪及加速度计漂移对应的1奶惯性导航系统矩 阵，是一个维数为66的对角线矩阵，表示如下：
[0104] FM(t) = diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中，Tgx表示陀螺仪x 轴的误差模型的时间常数，T gy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的 误差模型的时间常数，T ax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差 模型的时间常数，T az表示加速度计z轴误差模型的时间常数；
[0105] Gi(t) = diag[l 1 1......1 I]i5i5;
[0106] Wi(t)是一个15维的向量，如下所示：
[0107] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0108] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序
[0109] 列；
[0110] z(t)为无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中 的位置速度信息的差值，是一个6维向量，
[0111] 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方向上的位置误差；
[0112]冲)=U,(朴作)=k(,)J，其中
[0113] //, (/) = |^/Mg[l 1 1 ]!〇,,,：}
[0114] (/^； +/?) +/?)cosL l]：〇w,}
[0115] Vv(t) = [Nvx Nvy Nvz]T
[0116] Vp(t) = [Nx Ny Nz]t
[0117] G、将上述得到的连续状态方程< = F, (/)H) + G,, (/)%(/)离散化后得到Xk = ① k, k-lXk-l+Wk-1，其中
[0118] 将上述得到的连续观测方程Z(t)=H(t)XI(t)+V(t)离散化后得到Zk = HkXk+Vk; [0119]其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，A t是离散化后INS惯性 导航系统的采样时间；
[0120] H、将无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中的 位置速度信息作差得到Z(t)在k时刻的观测信息z;
[0121] I、计算k时刻INS惯性导航系统状态的最优估计值鳥t&卜-耳之, 其中，H，尤-^为在k-1时刻，INS惯性导航系统状态的最优估计值， A ,祀(巧I此+4)'巧卜,-尤死,-,:是燃惯性导航系统的噪声矩阵， 其大小是由INS惯性导航元件的性能决定，巧，t-[/--人A人Y， Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；
[0122] J、将计算得到的值与无人机在INS惯性导航系统中的位置速度信息作差得到 最优的导航参数；
[0123] K、重复步骤H-J，得到连续的无人机导航信息。
[0124] 利用上述方法计算得出的导航信息，能够准确反映无人机的姿态和位置信息，可 以实现较好的定位导航精度，图1为本发明所述无人机导航系统计算得出的炜度误差值;图 2为本发明所述无人机导航系统计算得出的炜度误差方差值；图3为本发明所述无人机导航 系统计算得出的经度误差值；图4为本发明所述无人机导航系统计算得出的经度误差方差 值；图5为本发明所述无人机导航系统计算得出的高度误差值；图6为本发明所述无人机导 航系统计算得出的高度误差方差值；图7为本发明所述无人机导航系统计算得出的东向速 度误差值；图8为本发明所述无人机导航系统计算得出的东向速度误差方差值；图9为本发 明所述无人机导航系统计算得出的北向速度误差值;图10为本发明所述无人机导航系统计 算得出的北向速度误差方差值；图11为本发明所述无人机导航系统计算得出的天向速度误 差值;图12为本发明所述无人机导航系统计算得出的天向速度误差方差值；
[0125] 从上述测试结果图可以看出，本发明所述无人机导航系统得出的经度、炜度、高度 的误差方差均能快速收敛至比较小的数值;对位置、速度等导航信息也能实现滤平滑作用， 不会产生大的跳变，系统的稳定性较强。
【主权项】
1. 一种无人机导航系统，其特征在于:包括INS惯性导航系统模块、磁力计、GPS导航系 统模块、控制中心，所述INS惯性导航系统模块包括陀螺仪和加速度计，所述陀螺仪、加速度 计、磁力计、GPS导航系统模块分别与控制中心信号连接，所述陀螺仪用于测量无人机的角 速度参数并将其测得的角速度参数传递给控制中心，所述磁力计用于测量无人机的偏航角 并将其测得的偏航角参数传递给控制中心，所述加速度计用于测量无人机的加速度并将其 测得的加速度参数传递给控制中心，所述GPS导航系统模块用于测量无人机在GPS导航系统 中的位置速度信息，所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算 出无人机在INS惯性导航系统中的位置速度信息，控制中心将无人机在GPS导航系统中的位 置速度信息与无人机在GPS导航系统中的位置速度信息进行Kalman滤波并进行导航误差估 计，然后利用导航误差估计修正无人机在INS惯性导航系统中的位置速度信息从而得到最 终的导航信息。2. 如权利要求1所述的无人机导航系统，其特征在于:所述陀螺仪、加速度计、磁力计、 分别与控制中心通过I2C总线连接。3. 如权利要求2所述的无人机导航系统，其特征在于:所述GPS导航系统模块与控制中 心通过USART串口连接。4. 如权利要求1所述的无人机导航系统，其特征在于:所述控制中心采用如下方法计算 得到最终的导航信息，其具体计算方法如下所述： A、 将陀螺仪测得的无人机角速度参数4,，代入四元数微分方程求解得到四 元数9〇4142^3;其中<、.，《1.，》^为陀螺仪在无人机自身坐标系下的测得的三个轴的 角速度信息； 所述四元数微分方程为：B、 将步骤A中求解的qo，qi，q2，q3代入下式求解得到姿态矩阵0，根据下述Cf与方向余弦的关系式计算得出无人机的INS惯性导航模块姿态角θ、γ C、 利用磁力计测得的偏航角免替换步骤B计算得到的偏航角 D、 将加速度计测得的加速度参数护和步骤B中求解得到姿态矩阵 < 代入下述微分方程 中求解得到无人机在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、 建立状态方程矣(i) = 6 (f)JT, (V)+:(? (4% (?)和观测方程Z(t) =H(t )Xi(t) +V(t)， XMt)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示： 不⑴二A dV:我 $ 4 况.况漁 < A 6 ▽i].，.5vx，3v y，5vz*INS 惯性导航系统沿东、北、天方向上的速度误差；Φχ，Φγ，Φζ为无人机的姿态角误差;δ?，δλ，δ h分别代表无人机所在炜度、经度和高度误差；ex，ey，％分别代表陀螺仪的随机漂移； 别为加速度计的随机漂移，，是一个15X15的矩 阵；其中FN(t)对应于δνχ，δνγ，δνζ，φχ, φγ, φζ，δΙ^，δλ，δ]ι这9个参数的INS惯性导航系统矩 阵，其非零元素如下：Fs(t)为3"，5%，5￥2，(^，(^，(|)2况，3入，5]1这9个参数与陀螺仪及加速度计漂移之间的 变换矩阵，其维数是9X6FM( t)为εχ，ey，εz，▽,:，▽,.，▽。与陀螺仪及加速度计漂移对应的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、 将上述得到的连续状态方秤尤(/) = 6 (/) A (/) + (7,⑴it; (/)离散化后得到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时刻的观测fg息z; I、 计算k时刻INS惯性导航系统状态的最优估计值爲_ +[4Z-氧尤-iA--小 其中，之,^，之H为在k-1时刻，IN S惯性导航系统状态的最优估计值， 心=山，"；（";VX + 鳥)' = φ?--Α,φΙη +a-丨办―!是1 咐 矩阵，其大小是由INS惯性导航元件的性能决定，，pk，」KkHAHK kH；f +KkRkKl， Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定； J、 将计算得到的名^值与无人机在INS惯性导航系统中的位置速度信息作差得到最优 的导航参数； K、 重复步骤H-J，得到连续的无人机导航信息。
【文档编号】G01S19/49GK105928515SQ201610242663
【公开日】2016年9月7日
【申请日】2016年4月19日
【发明人】张瑜, 李诗扬
【申请人】成都翼比特自动化设备有限公司