基于Matlab 的捷联惯导算法设计及仿真.pdf-资料库

基于 Matlab 的捷联惯导算法设计及仿真1 http://www.paper.edu.cn 严恭敏西北工业大学航海学院，西安（710072） E-mail：yangongmin@163.com 摘要：根据圆锥误差补偿算法和划船误差补偿算法的研究成果，考虑到实际捷联惯导算法仿真程序编写的方便性，总结了一些与捷联惯导更新算法有关的函数的计算公式。对圆锥误差补偿算法和捷联惯导算法进行了仿真，仿真结果和理论分析结论吻合。在附录中给出了 Matlab 的 m 文件源程序代码，具有一定的参考价值。关键词：捷联惯导；四元数；等效旋转矢量；Matlab；算法；仿真中图分类号：V249.3 1. 引言在捷联惯导系统中采用数学平台，姿态更新解算是捷联惯导系统算法的核心部分，由于四元数法算的优良特性，它在工程实际中经常被采用。为了减小姿态计算的不可交换性误差，前人研究并建立了等效旋转矢量方程，高精度姿态更新解算的研究主要集中在等效旋转矢量方程的求解上，在圆锥运动环境下，许多研究者提出并完善了圆锥误差补偿算法。基于圆锥误差补偿算法和划船误差补偿算法的等效原理，可将圆锥误差补偿算法移植到划船误差补偿算法中去，从而减少了划船误差推导的繁琐过程。上述研究都已经比较成熟[1-6]，本文根据这些研究结果，并考虑到实际仿真程序编写的方便性，总结了一些与捷联惯导算法有关的函数的计算公式或步骤，其中更详细的推导过程可见参考文献[7,8]。最后，对圆锥误差补偿算法和捷联惯导算法进行了仿真。附录中给出了Matlab的m文件源程序代码具有一定的参考价值。 2. 捷联惯导算法文中选取东-北-天（E-N-U）地理坐标系为导航坐标系，记为 n 系；捷联惯组坐标系记为b 系。 2.1 相关函数（1）四元数的共轭与乘积。四元数q 可表示为 q = 。用 ∗q 表示四元数 q 是四元数 1q 与 2q 的乘积，四元数相乘是不可 jqiq 1 2 kq 3 + + + q v q 0 q 0 = + 表示 q 的共轭四元数；交换的。 q = 21qq （2）四元数与向量相乘。一般情况下利用矩阵进行坐标变换，如关系式同样利用四元数也可以表示坐标变换。设变换四元数 n 四元数 n bq 和向量 bv 的乘法，记为 v = n vq bn b ， bC 相对应，则定义变换，它由以下规则实现：首先，将向量扩展成四 bq 与变换矩阵 n v = n vC bn b 元数，令向量，有 b b q v v = 。 n += 0 n v q ；其次，做四元数乘法，令 n q = qqq n b b n b ∗ ；最后，从四元数 nq 中提取（3）等效旋转矢量与四元数之间的转换。等效旋转矢量 v 转化为四元数q 的公式为 1 本课题得到水下信息处理与控制国家级重点实验室基金（9140C230206070C2306）资助。 - 1 -

Fq v = →q v )( = cos( v )2/ + v v sin( v )2/ (1) http://www.paper.edu.cn vn = v 2 v 2 ⋅ n v sin( 其中 vF )(•→q 表示从等效旋转矢量转换到四元数的函数，容易看出有假设 v 是小量，则从 q 中可以求出v ，首先令 2/ = arccos( ∗ q q ) 0 = F v → q ( v − ) 成立。，再求得 Fv q = q )( = v 2 q (2) v ) 表示从四元数转换到等效旋转矢量的函数。 n 2 → 其中 )(•→v qF （4）姿态向量 A 与姿态四元数 n 成的向量 ]Tψγθ=A [ bq 之间的转换。记由俯仰角θ、横滚角γ和航向角ψ组 bq 之间 bC 作为过渡，容易实现 A 与 n 为姿态向量，通过姿态矩阵 n 的转换，此处不作详细分析。 2.2 地球模型通常给出的地球椭球模型参数为长半轴 eR 和扁率 f ，由椭球几何学容易得到其它参数，在惯性导航算法中经常用到，它们是偏心率 e = 2 f − 2 f 、短半轴 R p 1( −= Rf ) e 、第二偏心率 ep = R 2 + e RR /2 p L 、子午圈半径 R M = R e 1( − 2 e 1/() − 2 e 2 sin L ) 2/3 p 。另外，重力加速度 g 和地球自转角速率 ieω 也是惯性导航解算、卯酉圈半径 2 2 N e = − R e 1/( sin ) 2/1 R 的必备参数，在 GRS80 椭球模型中，正常重力公式为 10 32718 × =ieω g g = 0 =g .9 7803267714 m/s2，h 为海拔高度，而 27094 .51( sin 其中 10 .2 L + × + − 2 3 0 球自转角速度矢量在导航坐标系投影为 n 则 5 4 − L sin h 086.3) 10 − × 7.29211514 10 rad/s -5 × 67 − 6 (3) 。记地 enω ， ieω ，惯导速度 nv 引起导航坐标系转动速度为 n [ −=ω n en v n N /( 并且记 n ie ω=ω ie /( [ 0 ) h + v n E R M cos L R N + h ) ω ie v sin L n E tan ]T RL /( (4a) ]T) + h N (4b) 2.3 圆锥误差与划船误差补偿算法 ω n in = ωω n en + n ie (4c) 假设陀螺和加速度计均为等周期采样（采样周期 h ），圆锥误差与划船误差补偿周期均， n 为子样数。再假设在第 m 个补偿周期中，陀螺角增量和加速度计比），并令采样总角增量为T ，且力速度增量采样输出分别为 hn ⋅= 和（ T )(im∆θ 和采样总比力速度增量，则考虑圆锥误差补偿后的 m n i = ∑ = )(∆θ i ∆θ 等效旋转矢量计算公式为 m 1 )(im∆v ∆v = m i ∑ = n ...3,2,1= n i )(∆v i m 1 × 上式中第二项为圆锥误差补偿量，系数 ik 的选择见表 1。 + = m m m i i ])( [ ∑ − n 1 k i 1 = ∆θ ∆θ Φ ∆θ n )( m (5) - 2 -

根据划船误差补偿算法与圆锥误差补偿算法的对偶原理，则考虑划船误差补偿后的比力 http://www.paper.edu.cn 速度增量计算公式为 ∆v = sfm ∆v m (2/1 + ∑ n 1 − k i 1 = i ∆θ × ∆v ) m i ])( m ∆v ∑ 上式中第二项为旋转速度增量，第三项为划船误差补偿量。 n )( ∆θ {[ + × + [ m m n 1 − k i 1 = k2 - 表 1 圆锥误差补偿算法系数 k1 2/3 9/20 54/105 250/504 27/20 92/105 525/504 k3 - - 214/105 650/504 子样数 n 2 3 4 5 2.4 捷联惯导更新算法 ∆v i m i ])( × ∆θ m n )}( (6) k4 - - - 1375/504 捷联惯导更新的基本思想是，将前一时刻的导航参数（姿态、速度和位置）作为初值，利用前一时刻至当前时刻的惯性器件采样输出，解算当前时刻的导航参数，作为下一时刻捷联惯导解算的初值，如此反复不断。惯性器件采样经过 2.3 小节的误差补偿后获得等效旋转矢量 mΦ 和比力速度增量 sfm∆v ，再经过以下三步骤便可实现捷联惯导更新，这里直接给出计算公式。（1）速度更新算法 v n m = v n m 1 − q → （2）位置更新算法 + F v ( − ω n inm T m 1 − )2/ q n bm 1 − ∆v sfm + [ g m 1 − − 2( ω n iem 1 − + ω n enm 1 − ) × v n m 1 − T ] m (7) L m R M , vT m sec L = − + m 1 T − + m 1 h = − + m 1 /1 n mNm − vL n mEm m 1 − vT m n mUm 1 − , , 1 − m = λλ m h m (8a) R / (8b) N (8c) （3）姿态更新算法 q n bm = 3. 仿真与分析 qΦFq n bm − ( 1 − → m q v n bm ∗ 1 − ω n inm T− m 1 ) (9) 根据第 2 节的分析，将各函数和算法编制成 Matlab 的 m 文件，文件名及简要功能如表 2 所示，详细的源程序代码参见附录，文中还着重对圆锥误差补偿算法和捷联惯导更新算法进行了仿真。表 2 Matlab 文件功能说明功能全局变量有关地球参数计算四元数共轭四元数相乘文件 glvs.m earth.m qconj.m qmul.m qmulv.m 四元数乘向量 rv2q.m q2rv.m 等效旋转转换为四元数四元数转换为等效旋转文件功能 a2quat.m 由姿态向量求姿态四元数 q2att.m 由姿态四元数求姿态向量 cnscl.m 圆锥误差和划船误差补偿 sins.m 捷联惯导更新算法 test1.m 圆锥误差补偿算法仿真 test2.m 捷联惯导算法算法仿真 - 3 -

3.1 圆锥误差补偿算法仿真 http://www.paper.edu.cn 如图 1，动坐标系 b 系相对参考坐标系 r 系绕 r r zo 轴作圆锥运动，半锥角为α，锥运动平面内平面内幅值为α的振动； bb zo 轴 r zo 轴为中心的圆锥面上运动。动坐标系 b 相对参考坐标系 r 的方位关系还可用姿态四频率为ω，即：b 系与 r 系坐标原点重合， b 作幅值为α的振动； b 在以 r 元数描述为 b xo 轴以 r zoy r r r xo 轴为对称中心，在 b yo 轴以 r r yo 轴为中心，在 zox r r r r q r b t )( = [ cos( α )2/ sin( α )2/ cos t ω sin( α sin)2/ t ω ]T0 (10) zr zb α or (ob) yr yb xb xr 图 1 b 系相对 r 系作圆锥运动两个相邻采样时刻之间的角增量输出为 sin2 − sin2 k∆θ =+ 1 ⎡ ⎢ ⎢ ⎢ ⎣ t h )2/ sin( ωα h )2/ sin( ωα h (2 sin) ω − sin[ ( ω t cos[ ( ω k )2/ ( 2 α k h ])2/ + ⎤ ⎥ h )]2/ + ⎥ ⎥ ⎦ (11) t h = +1 k 其中，输出。理论上可以证明，圆锥误差补偿算法在一个补偿周期T 内的算法漂移误差为 1+k∆θ 为从 kt 时刻到 1+kt 时刻的角增量，即 1+kt 时刻采样获得的角增量， − t k ε = n 2 nn ! × n 1 + 2( k 1 = 1 + ∏ 2 h ωα ( k − )1 2 n 1 + ) (12) 图 2 圆锥误差补偿仿真 - 4 -

http://www.paper.edu.cn 曲线为圆锥误差补偿姿态与真实姿态之间的误差，可见在 r 当α=1º、ω=5Hz、 h =10ms、 n =3时，仿真一分钟的姿态误差如图2所示。其中蓝色 r yo 轴有微小振荡误 r zo 轴为圆锥漂移误差。最下面小图中红色曲线为利用公式（12）计算的理论圆锥 r xo 轴和 r 差，而 r 漂移误差，它与蓝色曲线非常接近。此外，当ω=1Hz、 h =10ms，而α和 n 取不同值时，进行了一系列的圆锥漂移误差仿真和理论漂移误差计算，结果如表 3 所示。从表中可以看出，只有当半锥角α很小时，高子样数的仿真误差和理论误差才比较接近，而当α较大时，高子样数的仿真误差和理论误差之间相差倍数很大，甚至在仿真误差中出现高子样补偿效果不如低子样的现象。产生这种现象的原因是，由于在圆锥漂移补偿算法推导过程中作出了半锥角α是小角度的假设，还须指出的是，在推导过程中还假设圆锥频率和采样周期乘积 h⋅ω 必须为小量。因此，在实际中必须结合应用环境选择合适的圆锥误差补偿算法，而并非子样数越高计算精度就越高，但是，如果考虑到实际陀螺的精度（如陀螺漂移 1×10-4 º/h），即使α为 10º 时，选用 3 子样的计算精度也足够了，或者提高采样频率有利于降低计算误差。表 3 一分钟圆锥漂移误差仿真和理论计算（单位："） α 1" 1′ 1º 10º 仿真误差理论误差仿真误差理论误差仿真误差理论误差仿真误差理论误差 1 子样 6.013e-7 6.012e-7 2.164e-3 2.164e-3 7.790 7.792 771.242 779.272 2 子样 4.745e-10 4.747e-10 1.708e-6 1.709e-6 6.148e-3 6.152e-3 0.596 0.615 3 子样 4.016e-13 4.013e-13 1.444e-9 1.445e-9 4.596e-6 5.205e-6 -5.455e-3 5.205e-4 4 子样 3.522e-16 3.523e-16 1.612e-12 1.268e-12 4.480e-6 4.566e-9 4.416e-2 4.566e-7 5 子样 9.558e-19 3.161e-19 1.623e-12 1.138e-15 2.103e-5 4.097e-12 2.075e-2 4.097e-10 3.2 捷联惯导算法仿真 bC 与真实数学平台 n 传统上用姿态矩阵表示的计算数学平台 'n ( ≈ (13) 其中 φ 为平台误差角，它表示从 'n 系到 n 系的小转动角向量（可以看成是等效旋转矢量）。假设变换矩阵 'n bC 之间的关系为 CφI ) ×− CC C ， = n b n b n b n n ' ' bC 、 'n nC 、 n qq n ' n n b q n b ' = 并且利用矩阵与四元数之间的关系可以证明 bC 分别与变换四元数 'n bq 、 'n nq 、 n F φ ( ) − →q v qφ ) n b q n n F v = ( − = q n b → q ' ' nq 相对应，则有成立，所以有 (14a) (14b) 在仿真时，通过（14）式能够方便地进行平台误差角处理，与传统矩阵描述平台误差角相比，四元数描述方法的优点是不必再作正交化处理。 −=⇔ = − → → q v ( φ ) F v ∗ qq n ' b n b φ F q ( qq n ' b n b ∗ ) 对静态导航进行了仿真，初始平台误差角分别设置为0.5′、0.5′和3′，仿真一小时导航参数结果如图3所示，其中速度误差和位置误差小图中，红色曲线为高度通道的误差，它们是发散的，而水平通道（包括水平平台误差、水平速度和经纬度误差）呈现振荡趋势，方位平台误差变化比较小。仿真结果符合惯导系统误差理论分析的结论。 - 5 -

http://www.paper.edu.cn 图 3 捷联惯导算法仿真 4. 小结总结了一些与捷联惯导更新算法有关的函数的计算公式，对圆锥误差补偿算法和捷联惯导算法进行了仿真。在圆锥误差补偿算法中发现，只有当半锥角很小时，高子样数的仿真误差和理论误差才比较接近，否则相差倍数很大，因此在高精度应用场合，必须通过提高采样频率来降低计算误差。捷联惯导静态导航仿真结果与惯导系统误差理论分析结论相吻合，证明了所设计算法的正确性。参考文献 [1]Bortz J E, A new mathematical formulation for strapdown inertial navigation[J], IEEE Transactions on Aerospace and Electronic Systems,1971,7(1):61-66. [2]Miller R B, A new strapdown attitude algorithm[J], Journal of Guidance, Control and Dynamics, 1983,6(4):287-291. [7]严恭敏. 捷联惯导算法及车载组和导航系统研究[D]. 西安：西北工业大学硕士论文，2004,3. [8]严恭敏. 车载自主定位定向系统研究[D]. 西安：西北工业大学博士论文，2006,5. [3]Roscoe K M, Equivalency between strapdown inertial navigation coning and sculling integrals / algorithms[J]． Journal of Guidance, Control and Dynamics，2001，24(2)：201-205． [4]秦永元. 惯性导航[M]. 北京：科学出版社，2006. [5]张玲霞，陈明，曹晓青. 基于对偶性原理捷联惯导划船误差补偿优化算法[J]. 中国惯性技术学报， [6]潘献飞,吴文启,吴美平. 考虑机抖激光陀螺信号滤波特性的圆锥算法修正[J]. 中国惯性技术学报， 2003,11(5):33-38. 2007,15(3):259-264. - 6 -

Algorithm design and simulation for SINS based on Matlab College of Marine，Northwestern Polytechnical University，Xi’an （710072） Yan Gongmin http://www.paper.edu.cn Abstract Based on the research achievements of coning error compensation and sculling error compensation algorithm in strapdown inertial navigation system(SINS), and considering the convenience of simulation software program, some calculation formulas about the SINS updating algorithm are summarized in this paper. Coning error compensation algorithm simulation and SINS algorithm simulation are carried out and the simulation results are consistent with the theory analysis. Simulation source codes written in Matlab’s m file are present in the appendix, and provide valuable references for SINS software designer. Keywords：strapdown inertial navigation system；quaternion；equivalent rotate vector；Matlab； algorithm；simulation 作者简介：严恭敏（1977-），男，福建建瓯人，2006 年 9 月毕业于西北工业大学自动化学院，获导航、制导与控制专业博士学位，现在西北工业大学航海学院兵器科学与技术博士后流动站工作，主要从事陆用定位定向系统、水下航行器自主导航技术与组合导航系统理论研究。附录 Matlab仿真程序 % glvs.m 全局变量 %全局变量 global glv glv.Re = 6378160; %地球半径 glv.f = 1/298.3; %地球扁率 glv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; %地球椭圆度等其它几何参数 glv.Rp = (1-glv.f)*glv.Re; glv.ep = sqrt(glv.Re^2+glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; glv.wie = 7.2921151467e-5; %地球自转角速率 glv.g0 = 9.7803267714; %重力加速度 glv.mg = 1.0e-3*glv.g0; %毫重力加速度 glv.ug = 1.0e-6*glv.g0; %微重力加速度 glv.ppm = 1.0e-6; %百万分之一 glv.deg = pi/180; %角度 glv.min = glv.deg/60; %角分 glv.sec = glv.min/60; %角秒 glv.hur = 3600; %小时 glv.dph = glv.deg/glv.hur; %度/小时 glv.mil = 2*pi/6000; %密位 glv.nm = 1853; %海里 glv.kn = glv.nm/glv.hur; %节 glv.cs = [ %圆锥和划船误差补偿系数 2./3, 0, 0, 0 9./20, 27./20, 0, 0 54./105, 92./105, 214./105, 0 250./504, 525./504, 650./504, 1375./504 ]; - 7 -

http://www.paper.edu.cn % earth.m 有关地球参数计算 function [wnie, wnen, RMh, RNh, gn] = earth(pos, vn) global glv sl=sin(pos(1)); cl=cos(pos(1)); tl=sl/cl; sl2=sl*sl; sl4=sl2*sl2; wnie = glv.wie*[0; cl; sl]; sq = 1-glv.e2*sl2; sq2 = sqrt(sq); RMh = glv.Re*(1-glv.e2)/sq/sq2+pos(3); RNh = glv.Re/sq2+pos(3); wnen = [-vn(2)/RMh; vn(1)/RNh; vn(1)/RNh*tl]; g = glv.g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80 gn = [0;0;-g]; % qconj.m 四元数共轭 function qo = qconj(qi) qo = [qi(1); -qi(2:4)]; % qmul.m 四元数相乘 function q = qmul(q1,q2) q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4); q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3); q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4); q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ]; % qmulv.m 四元数乘向量 function vo = qmulv(q, vi) qi = [0;vi]; qo = qmul(qmul(q,qi),qconj(q)); vo = qo(2:4,1); % rv2q.m 等效旋转转换为四元数 function q = rv2q(rv) norm = sqrt(rv'*rv); if norm>1.e-20 f = sin(norm/2)/(norm/2); else f = 1; end q = [cos(norm/2); f/2*rv]; % q2rv.m 四元数转换为等效旋转 function v = q2rv(q) n2 = acos(q(1)); if n2>1e-20 k = 2*n2/sin(n2); else k = 2; end v = k*q(2:4); % a2quat.m 由姿态向量求姿态四元数 function q = a2quat(att) % 先求姿态矩阵 si = sin(att(1)); ci = cos(att(1)); sj = sin(att(2)); cj = cos(att(2)); sk = sin(att(3)); ck = cos(att(3)); M = [ cj*ck+si*sj*sk, ci*sk, sj*ck-si*cj*sk; -cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck; -ci*sj, si, ci*cj ]; % 再求四元数 - 8 -

资料库

基于Matlab 的捷联惯导算法设计及仿真.pdf

相关推荐

课程资源

热门标签

最新资料