SINS/GPS组合导航系统仿真.pdf-资料库

SINS/GPS 组合导航系统仿真 http://www.paper.edu.cn 马媛，杨树兴，张成北京理工大学宇航科学技术学院，北京 (100081) E-mail: maxinyan1@bit.edu.cn 摘要：本文利用 Matlab/Simulink 的仿真环境，建立了 SINS/GPS 组合导航系统仿真系统。首先由轨迹发生器产生所需的模拟导航参数和 IMU 参数，并根据惯性传感器的误差模型，得到包含噪声的 IMU 参数并进行 SINS 导航计算；然后由模拟载体轨迹和 GPS 卫星参数仿真得到 GPS 伪距信号；最后对 SINS 导航参数误差和 GPS 输出的伪距信号进行了 Kalman 滤波数据融合。试验结果表明，采用紧组合结构的 SINS/GPS 组合导航系统能够较好地估计出导航参数和 IMU 参数的误差。另外通过本仿真系统能够更好的进行滤波算法和组合导航系统的进一步研究。关键词：组合导航；SINS；GPS；系统仿真中图分类号：U666.1 文献标识码：A 1. 引言 INS 和 GPS 系统由于具有优势互补的特点，因此在导航领域内成为应用最广泛的系统，对二者数据进行数据融合，能够获得更高精度，更好的导航性能[1]。在进行惯导和 GPS 组合导航技术的研究中，由于费用和实际条件的限制，一般都是先用计算机仿真技术对系统的每一步骤进行试验，最终确定设计的组合系统的性能。因此组合导航系统仿真平台必不可少。本文利用 Matlab /Simulink 的强大的运算功能及易用性，搭建了 SINS/GPS 组合导航仿真系统，在此基础上，可以进行捷联惯导误差特性及导航融合算法的研究。另外考虑到进行转台试验时，Simulink 程序可下载到 dSPACE 上实时解算，通过 ControlDesk 显示数据图形，因此便于后期进行半实物仿真试验。整个仿真系统包括：轨迹发生器，传感器模块，SINS 解算模块，GPS 仿真模块和 Kalman 滤波器。 2. 组合导航系统结构首先考察 SINS/GPS 组合导航仿真系统需要实现的主要功能，主要有：产生原始的加速度、角速度和不含误差的导航信息；由于采用间接滤波，即以导航误差为状态变量，因此本系统还需得到系统真实的导航误差信息，使得在后面的组合导航研究中可以进行对比；陀螺加计误差模型，SINS 解算和滤波融合等各个模块可以轻松的进行组合、或改变，进行不同的力学编排和滤波算法的验证。因此整个系统结构图如下： SINS解算惯性器件误差模型 SINS解算真实导航误差＋ — 轨迹发生器比力角速度姿态速度位置 Kalman滤波器滤波导航误差 GPS信号仿真伪距图1 SINS/GPS组合导航系统结构图 - 1 -

http://www.paper.edu.cn 3. 组合导航仿真系统建模在 SINS/GPS 组合导航系统仿真中，关键在于惯导系统的仿真，而进行惯导系统仿真，首先要明确参考坐标系和捷联系统的选择。坐标系的选择包括两个坐标系，机体坐标系b 和导航坐标系 n 。捷联惯导系统根据导航系选择可分为：指北系统，自由方位系统和游移方位系。本文选用游移方位导航系统，此外由于采用飞行器六自由度模型产生真实导航信息，为了便于数据衔接，取北东地系为导航系，以前右下为载体坐标系。在进行 SINS 和 GPS 导航信息组合时，SINS 用游移方位系为导航系，而 GPS 给出的导航信息一般在 WGS-84 系中，因此仿真过程中，先要把 SINS 的导航信息转换为 WGS-84 系下的相应值，再进行融合。 3.1 轨迹发生器进行 SINS 解算，需要陀螺敏感的角速度和加计测量的比力值，因此本文利用一个飞行器六自由度模型仿真得到运动轨迹，角速度和加速度等信息，并通过捷联惯导逆运算，将这 ibω 。注意轨迹些值转换为和陀螺，加速度计测量的数据定义一致的数据，即载体系的 b 发生器产生的导航信息都是不含噪声的数据。 ibf 和 b 3.2 惯性器件模拟惯性器件模拟器的功能是通过前面轨迹发生器产生的不含噪声的角速度 b ibω ，和比力 ibf ，加上惯性器件的误差项来实现的。对捷联惯导系统，主要考虑陀螺和加计的常值漂移、 b 白噪声和一阶马尔可夫误差。其它误差如：安装误差和刻度因子误差容易检测并补偿，因此可以忽略。常值漂移以随机常值为模型，即一个没有输入但有随机初值的积分器的输出。随机常值的模型为：一阶马尔可夫过程的数学模型为： bε =& 0 ε & r = − 1 τ i ε r + w i 其中， iw 为白噪声， iτ 为相关时间。陀螺误差模型为： + ，其中 Wε 为白噪声。加速度计的误差模型和陀螺的误差模型相似，也分为常值偏差，白噪声和一阶马尔可夫项，这里只给出陀螺的误差模型。 ε ε ε ε W = + b r 3.3 SINS 解算 SINS 的解算主要分两个部分：姿态计算和位置计算。在下文中，T 表示理想导航系；b 表示载体系；g 表示地理系，即北东地系。由惯性器件模拟器产生角速度 b ibf ，由于游移方位惯导系统的导航系仍然是北东地系，但平台水平轴相对东向和北向轴存在一个偏转角，即游移方位角（如图 2）。根据 T iT ibω 和加速度 b Cω T ω ω gT + ，则 = g ig T g - 2 -

http://www.paper.edu.cn N X α α 图 2 游移方位角 E Y T g = C T e C C g e 游移方位角变化率为 C ⎡ 11 ⎢ C = ⎢ 21 C ⎢ ⎣ 31 0 ⎤ ⎡ ⎥ ⎢ 0 ⎥ ⎢ 1 ⎥ ⎢ ⎦ ⎣ c s α α ⎡ ⎢ c s = − α α ⎢ 0 0 ⎢ ⎣ 由上式可得到： α λ φ & = & C 12 C 22 C 32 s c c − φλ φλ φ ⎤ ⎥ s 0 − λ ⎥ c c s ⎥ − − φλ φλ φ ⎦ sin C ⎤ 12 ⎥ C ⎥ 23 C ⎥ ⎦ 33 s s − c λ c s − （1） φ= − arcsin C 33 λ = arctan ⎛ −⎜ ⎝ C C 21 22 α = arctan ⎞ ⎟ ⎠ ⎛ ⎜ ⎝ 23 C − C 13 ⎞ ⎟ ⎠ （2） C T & e ) Cω= − T e T eT × ( ，另外根据 T eC 是单位正交阵，其逆和转置相等，因此只需计算出 T eC 的后面两列，其第一列等于后面两列的向量叉乘值。其中 T eTω 的计算： = T eT T ω ω ω gT + 写成分量式为： T eg = C T g T ω ω gT g eg + （3） T ⎡ ω eTx ⎢ T ω ⎢ ⎣ eTy cos ⎤ α α ⎡ ⎤ = ⎥ ⎢ ⎥ sin α α − ⎥ ⎣ ⎦ ⎦ sin cos ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ 1 R h + N 0 V T & eT 0 ⎤ ⎥ ⎥ 1 − ⎥ ⎥+ R h ⎦ M f T = 22 12 ⎤ ⎥ ⎥ ⎥ ⎦ V T ⎡ & eTx ⎢ V T & ⎢ eTy ⎢ V T & ⎣ eTz ⎧ C C C 0 ⎡ ⎤⎡ ⎤ 11 13 ⎪ ⎢ ⎥⎢ ⎥ C C C 2 0 =− ⎨ ⎢ ⎥⎢ ⎥ 23 21 ⎪ C C C ⎢ ⎥⎢ ⎥ ω ⎣ ⎦⎣ ⎦ ⎩ ie 31 33 32 f 0 T ⎡ ⎤ ⎡ ⎤ x ⎢ ⎥ ⎢ ⎥ f 0 T + ⎢ ⎥ ⎢ ⎥ y ⎢ ⎥ ⎢ ⎥−⎣ ⎦ f g T ⎣ ⎦ z 四元数微分方程为： + sin ⎡ ⎢ cos ⎣ v T α α ⎡ ⎤ ⎤ y ⎢ ⎥ ⎥ v T α α ⎦⎣ ⎦ x cos sin − （4） − ( T ⎡ ω eTx ⎢ T ω + ⎢ eTy ⎢ 0 ⎣ × ) V T eT + T g T ω ω eT + e ie C T 2 e V T ⎤ ⎡ eTx ⎥ ⎢ V T × ⎥ ⎢ eTy ⎥ ⎢ V T ⎦ ⎣ eTz ⎫ ⎤ ⎪ ⎥ ⎬ ⎥ ⎪ ⎥ ⎦ ⎭ （5） x y 0 − − − ω ω ω ⎡ z ⎢ 0 ω ω ω − ⎢ x z y 0 ⎢ − ω ω ω x z ⎢ 0 − ω ω ω ⎢ ⎣ x y z y • q 0 • q 1 • q 2 • q 0 3 ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ = 1 2 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ bC ：由四元数可求出 n （6） ⎤ ⎡ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ q 0 q 1 q 2 q 3 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ - 3 -

n C b = 2 3 2 1 0 2 2 2 - + ⎡ q q q q - ⎢ ⎢ ⎢ ⎣ qq qq 2( qq qq 2( + ) - ) 1 2 0 3 1 3 0 2 qq qq 2( - ) 1 2 0 3 qq qq 2( + 1 3 0 2 2 2 q q q q - + - 2 2 0 1 2 3 qq qq 2( - 2 3 0 1 ) ) qq qq 2( + ) 2 3 0 1 2 2 q q q q + - - 2 2 0 1 2 3 http://www.paper.edu.cn ⎤ ⎥ ⎥ ⎥ ⎦ （7） 3.4 GPS 信号仿真模块由轨迹发生器提供的用户的位置（经度、纬度和高度），将用户的位置转换到 ECEF 系中，再在给定的星历参数下，计算出 GPS 卫星在 ECEF 系下的位置坐标，对二者相减就可以获得伪距信号[2]。由导航星历计算出卫星的地心矢径 r 和升交点角距ϕ，分别为： r = a ( 1 − e sin E ) 和 ϕ ω= + ( ) t f ( ) t （8）而卫星在轨道直角坐标系中的坐标： x 0 y 0 z 0 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ = r cos ϕ ⎤ ⎥ sin ϕ ⎥ 0 ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎣ （9）再利用轨道面倾角i 和升交点赤经 Ω 将卫星在轨道直角系中的位置，转换到地球坐标系（即 ECEF 系）中。GPS 距离仿真模块还提供根据卫星的高度角和距离来选择可视的卫星，并计算出当前卫星的几何精度因子，给出 GPS 信号的质量信息。每颗卫星对当地的高度角按照下面的方法计算。先由用户在 ECEF 系的位置，计算用户当地的垂直单位向量： e e up = P e a R ； R = x 2 u + y 2 u + z 2 u （10）再由卫星在 ECEF 系的位置减去用户在 ECEF 系的位置，得到视线向量： − （11） r Δ = p s e e e ) 因此可求出从接收机到卫星的视线单位向量伪距： ρ= r e x Δ ( 2 ( + Δ r e y 2 ) ( + Δ r e z ) 2 e e LOS = e r Δ ρ 高度角为：（12） sin θ = LOS e e LOS e e up （13） erΔ LOSθ 图3 卫星视线高度角 e 由 L LOS = C e L e e LOS ，求出各卫星在当地地平系的 LOS 单位向量，由所选卫星的 L LOSe ，根 - 4 -

据测码伪距观测方程的线性化形式，构成权系数阵： http://www.paper.edu.cn zQ ⎡ = ⎣ 1 A A − T ⎤ ⎦ ，其中 A ⎡ = ⎣ e L LOS 1 e L LOS 2 e L LOS 3 e L LOS 4 T ⎦ ⎤ q 2 11 = GDOP + q 2 22 + +L q 2 nn （14） 4. Kalman 滤波融合本文采用间接法组合，即采用导航参数误差为状态变量，而不是直接用导航参数进行融合。这种方法的优点在于导航参数误差的数量级相近，因此数值较稳定；另外考虑到 GPS 伪距输出为 ECEF 系的位置，因此在该系中进行组合滤波。另外本文选用 GPS 接收机仿真器提供的伪距信号与 SINS 输出的导航信息融合，因此属于紧组合架构。状态变量为 x = ⎡ ⎢ x δ δ δ δ δ δ ψ ψ ψ δ E z ⎢ 1442443 144424443 1442443 ⎣ y E e R δ v Ey v Ex v Ez z E V δ ψ x y e h b d T ⎤ ⎥ ⎥ ⎦ 其中b 表示与接收机时钟偏差等效的距离误差，等于接收机钟相位误差乘以光速。d 表示与接收机时钟漂移等效的距离误差，等于接收机钟频率误差乘以光速。 hδ 为气压高度偏差。观测量为 GPS 卫星相对用户位置的伪距误差。由于从所有卫星中根据高度角和距离选出四颗条件最好的卫星，记录其卫星号和伪距。 INS 在 ECEF 系的误差方程[4]： R V + δ ω δ δ & ( V V g 2 ω ω δ δ + & = × R n × en R e e δ × n e ψ ψ ω ε + & in = = n en + × n ie ) e e e e e − n f ψ × （15） GPS 的误差模型： b b = + & d = − & ω t d β ω u + tu 观测方程为： H ⎡ = ⎣ e Te LOS 0 1 3 × 0 1 3 × 0 1 0 ⎤ ⎦ 5. 仿真与结论（16）仿真中的部分参数为：位置噪声方差为 100 m ，速度噪声为 0.1( /m s )，姿态误差 1e-3 rad s ），GPS 距离偏差为 0.2 m ，GPS 观测量距离漂移为 0.002 m 。过程噪声方差为 210 ,1]；GPS 伪距观测噪声均方差 20.1 ,1e-6,1e-6, 1e-6, 100 , 2 20.1 , 20.1 , 210 , / 2 （ 100 ， 2 100 , [ 为 10 m。通过 Matlab/Simulink 数值仿真，得到 GPS 仿真的伪距和 GDOP 值如图 4，图 5 和 6 是滤波得到的速度误差曲线和姿态误差曲线。 - 5 -

x 107 8.5 ] m [ e g n a r o d u e s P P O D G 8 7.5 7 6.5 0 3.9 3.8 3.7 3.6 3.5 0 [ ] s / m 计估差误度速差方计估差误度速 0 -0.1 -0.2 -0.3 -0.4 0 30 25 20 15 10 5 0 0 50 100 150 200 250 time[s] 50 100 150 200 250 time[s] http://www.paper.edu.cn δvn δve 50 100 150 200 250 time[s] vn ve σδ σδ 50 100 150 200 time[s] 250 图4 GPS仿真的伪距和GDOP值图5 滤波的速度误差和模拟速度误差 ] s / d a r [ 计估差误态姿 0.1 0.05 0 -0.05 -0.1 0 0.02 0.015 0.01 0.005 差方计估差误态姿 0 0 φn φu φe 50 100 150 200 250 time[s] σφn σφu σφe 50 100 time[s] 150 200 250 图6 滤波的姿态误差和模拟姿态误差本文采用弹道的六自由度运动模型产生仿真轨迹，经过 226s 的仿真，验证了所设计的 INS/GPS 组合导航滤波算法的有效性。结果表明，在本例中方位通道的估计效果较水平通道的估计效果差，这是由于惯导的高度通道发散引起的；另外，采用 INS 的导航误差和 GPS 伪距融合，比传统的采用 INS 的位置，速度和姿态误差及 GPS 的位置/速度融合的精度更高。参考文献 [1] 秦永元. 惯性导航[M]. 北京：科学出版社. 2006.05 [2] 王惠南. GPS 导航原理与应用[M].北京：科学出版社.2003.08 [3] 李鹏,郑志强. SINS/GPS 组合导航系统仿真实验[J].声学与电子工程. Vol 2. 2007. [4] 董绪荣, 张守信, 华仲春. GPS/INS 组合导航定位及其应用[M].长沙：国防科技大学出版社.1987.08 [5] Adrian Schumacher. Integration of a GPS aided Strapdown Inertial Navigation System for Land Vehicle [D]. Stochkholm, Sweden. Mar. 2006. [6] Robert M. Rogers. Applied Mathematics in Integrated Navigation Systems[M].AIAA Education Series. 2003. [7] S.Winkler, Buschmann, T.Kordes. Application of Micro-Technology to Integrated Navigation of Autonomous Micro Air.[J]. AIAA 2004-6725. Simulation of the SINS/GPS Integration Navigation System Ma Yuan, Yang Shuxing, Zhang Cheng School of Aerospace Science and Engineering, Beijing Institute of Technology, Beijing (100081) Abstract The SINS/GPS integrated navigation system simulation platform was established via Matlab/Simulink. Firstly, the required navigation state variables and inertial sensor parameters were generated by the trajectory generator. Then, perturbed sensed sensor signal is attained in terms of inertial sensors’ error models. From all these information, the strap-down inertial navigation states can be solved; Secondly, the pseudo-range signals can computed from the aircraft trajectories and the GPS satellites parameters; - 6 -

http://www.paper.edu.cn At last, the SINS navigation states errors and the pseudo-range output of the GPS system was integrated in the Kalman filter. Experiment results shown that the SINS/GPS integrated navigation system, which use the tightly coupling structure, can efficiently estimate navigation and sensors errors. In addition, this simulation platform can also be used as the basis of the further study of filtering algorithms and integrated navigation system. Keywords: integrated navigation; SINS; GPS; system simulation - 7 -

资料库

SINS/GPS组合导航系统仿真.pdf

相关推荐

开发技术

热门标签

最新资料