（精密仪器及机械专业论文）基于计算机视觉辅助的组合导航系统.pdf

摘要 无人机( u a v ) 与有人驾驶飞机相比，具有生产成本低，便于运输、维修和保 养，且无需担一i i , 人员伤亡的优点因此在民用和军事领域部有非常广泛的应用前 景。但是现阶段无人机的回收还存在许多问题，这就限制了无人机的使用。 本文借鉴目前国内外研究较多的g p s s i n s 组合方法，提出了利用卡尔曼滤 波的方法，实现训算机视觉和捷联惯导系统组合，为固定翼无人机自主着陆进行 导航。 在设计组合系统的卡尔曼滤波器时，采用了反馈校正模式。卡尔曼滤波器的 输入信息是以下的两个位置量的差值：一是计算机视觉系统获得的地面特征点在 图像中的位置信息，二是把g i n s 输出的无人机的位置和姿态信息输入到摄像机 模型中所计算出的地面特征点在投影面中的位置信息。经过滤波以后，卡尔曼滤 波器的输出就是对惯导误差值的估计，这样就可以对惯导系统进行校i e 。 通过对无人机入场过程进行仿真，可以看出，计算机视觉获得的图像精度较 高，即使在陀螺仪和加计精度较差的情况下也可以获得良好的定位精度。在仿真 过程中，调整陀螺仪、加速度计和摄像机这三者的精度，可以得到系列结果。 对结果的比较分析说明：摄像机的分辨率对最后定位精度的影响最大，陀螺仪次 之，加计影响最小；并且摄像机分辨率对定位精度的影响呈指数关系，当精度达 到一定程度后，继续提高摄像机的精度对结果影响不大。 仿真结果证明，本论文所提出的方案在一定程度卜提高了无人机的入场精 度，降低了无人机回收的难度，具有一定的实用价值。 关键字：计算机视觉，捷联惯导系统，卡尔曼滤波，全球定位系统( g p s ) 组合导航系统 a b s t r ac t c o m p a r i n g t ot h eg e n e r a la i r p l a n e s ，t h eu a vh a st h ea d v a n t a g e so fl o wc o s t ，c o n v e n i e n c eo f t r a n s p o r t a t i o na n dm a i n t e n a n c e s ，t h r t h e r m o r e ，t h e r ei s n on e e dt ow o r r ya b o u tt h ec a s u a l t y p r o b l e m s ，t h e r e f o r e t h eu a vc a nb ew i d e l yu s e di nm i l i t a r ya n dc i v i lf i e l d b u tt h er e c o v e r yo f t h eu a vc a n tb es a t i s f i e d ，t h u si tl i m i t st h ea p p l i c a t i o nt oa l lf i e l d s t h i st h e s i st a k e st h eg p s s i n si n t e g r a t e ds c h e m ew h i c hi sh o td o m e s t i c a l l ya n da b r o a df o r r e f e r e n c e ，a n dt h e nb r i n g sf o r w a r dt h es c h e m eo f u t i l i z i n gt h ew a yo f k a l m a nf i l t e rt oc o m b i n et h e i n f o r m a t i o nf r o mc o m p u t e rv i s i o na n ds i n st on a v i g a t et h el a n d i n gp r o c e d u r eo ft h eg a u z yf i x e d u n m a n n e dp l a n e d u r i n gt h ep e r i o do fd e s i g n i n gt h ek a l m a nf i l t e r , w ea d o p tt h ef e e d b a c kc o r r e c t i o nm o d e t h ei n p u to fk a h n a nf i l t e ri st h ed i f f e r e n t i a lo ft h et w op o s i t i o nv a l u e s i sf o l l o w s ：o n ei st h e p o s i t i o ni n f o r m a t i o no fg r o u n ds u r f a c ef e a t u r ep o i n tt h a ti sc a p t u r e di nt h ei m a g eb yt h ec o m p u t e r v i s i o ns y s t e m ；t i l eo t h e ri st h ep o s i t i o ni n f o r m a t i o no ft h eg r o u n ds u r f a c ef e a t u r ep o i n ti nt h e p r o j e c t i o ns u r f a c et h a ti sc a l c u l a t e dw i t ht h ep o s i t i o na n dp o s t u r ei n f o r m a t i o no ft h eu a vf r o m s i n sb yt h ev i d e oc a m e r am o d e l a f t e rf i l t e r i n g ，t h eo u t p u to fk a l m a nf i l t e ri st i l ee s t i m a t i o no f i n e r t i a ln a v i g a t i o nd e v i a t i o n ，a n dt h e nw ec a nc a l i b r a t et h es i n s a t t e rs i m u l a t i n gt h ee n t e r i n gp r o c e d u r e ，w ef i n dt h a tc o m p u t e rv i s i o nc a nc a p t u r ea h i g h e r r e s o l u t i o ni m a g e c v c nt h o u g ht h ep r e c i s i o no ft h eg y r o s c o p ea n da c c e l e r o m e t e ri sl o w , i tc a ns t i l l g e tg o o dl o c a t i n gp r e c i s i o n i nt h ep r o c e s so fs i m u l a t i o n ，b ya d j u s t i n gt h ep r e c i s i o no ft h e g y r o s c o p e ，a c c e l e r o m e t e ra n dv i d e oc a m e r a , w ec a r lg e tas e r i e so f r e s u l t s t h ec o m p a r i s o no f t h e r e s u l t ss h o w st h a tt h er e s o l u t i o no f v i d e oc a m e r aa f f e c t st h el o c a t i n gp r e c i s i o nm o s t ，t h eg y r o s c o p e d o e sl e s s ，a n dt h ea c o e l e r o m e t e rd o e st h ei e a s t f u r t h e r m o r e ，t h ei n f l u e n c eo fv i d e oc a m e r a r e s o l u t i o nt ot h el o c a t i n gp r e c i s i o ne x p r e s s e se x p o n e n tr e l a t i o n ；w h e nt h ep r e c i s i o nr e a c h e sa c e r t a i ns c o p e ，c o n t i n u i n gi n c r e a s i n gt h er e s o l u t i o no f v i d e oc a m e r ac a nd ol i t t l et ot h ep r e c i s i o n t h er e s u l to f t h es i m u l a t i o ns h o w st h a tt h es c h e m ef r o mt h i st h e s i sc a ni n c r e a s et h ee n t e r i n g p r e c i s i o no f u n m a n n e dp l a n ei nc e r t a i nd e g r e e ，a n dt h e r ei ss o m ep r a c t i c a lv a l u e k e y w o r d s ：c o m p u t e rv i s i o n ，s i n s k a h n a nf i l t e r , g l o b a lp o s i t i o ns y s t e m ， i n t e g r a t e dn a v i g a t i o n 1 1 引言 第一章绪论 捷联式惯性导航系统( s i n s ) 是以牛顿力学定律为基础的一种自主式导航 系统，同时也是一种高精度的测量与计算系统。通过精密测量航行器的加速度矢 量和角速率矢量，再通过数学解算，即可获得航行器的水平速度矢量以及相对地 球的位置和姿态，由于s i n s 在工作时不需要任何外来的信息，仅靠系统本身就 能在全天候条件下，在全球范围内和任何介质环境里自主地、隐蔽地进行连续的 三维空问定位和定向，能够提供反映航行器完备的运动状态的信息，因此被广泛 地应用f 军用和民用的航行器中，如飞机、舰船、车辆等。尤其是在军事领域， 由于s i n s 可以自主地获得完备的导航信息( 位置、速度、姿态等) ，使它成为 许多武器系统的必要设备。但是由于s i n s 中具有积分环节，因此其导航参数误 差会随时间积累而增大，长时间工作时往往不能满足导航精度要求。克服这一缺 点的最好办法是采用组合导航技术，将外部信息引入惯性导航系统剥系统的漂移 进行限锫4 。 目前在组合导航中较为普遍的做法是采用最优估计法，即采用卡尔曼滤波， 这种方法是从概率统计最优的角度出发估计出系统误差并将其消除。卡尔曼滤波 是一种实用的线性最小方差估计算法，适用于非平稳过程，并与最小二乘估计一 样，采用递推算法，从量测信息中实时提取出被估计量信息并积存在估计值巾。 书尔曼滤波具有如下特点： 1 ) 算法是递推的，且使用状态空问法在时域内设计滤波器，所以卡尔曼滤 波适用于多维随机过程的估汁。 2 ) 采用动力学方程即状态方程描述被估汁量的动态变化规律，被估计量的 动态统计信息由激励白噪声的统计信息和动力学方程确定。 3 ) 卡尔曼滤波具有连续型和离散型两类算法，离散型算法可直接在数字计 算机上实现。“。 由于实际系统一般都是连续的，动力学特性用连续的微分方程描述，为了方 便在计算机上对其操作分析，在使用之前必须对系统方程和量测方程作离散化 第一章绪论 处理。 目前较为理想的外部信息是g p s ，g p s s i n s 的组合导航系统可以获得相当 高的导航精度，但是g p s 是由美国国防部控制，在战场环境下，g p s 并非总是 可靠的，这使得g p s 的应用领域受到很大限制。 计算机视觉导航是通过对视觉传感器获得的图像进行处理和分析，获得导航 定位参数的一种技术，它具有获得信息量大，完全自主和无源性等优点。目前计 算机视觉导航技术在许多场合得到了应用，例如智能机器人、公路交通管理、汽 车安全行驶和无人机自主着陆。 1 。2 国内外研究现状 从二战后期开始，到战后美苏两国长期冷战的军备竞赛。在长达3 0 多年的 时间内，惯性技术得到了空前的发展。其中平台系统更是得到重点发展，陀螺漂 移从1 0 。h 左右提高到0 0 0 0 0 1 5 1 ，几乎提高了6 个数量级，采用高精度的平台 式惯导系统的美国的m x 战略导弹精度可达到1 0 0 m 左右【l o1 0 目前，捷联惯导 系统的精度还没有达到平台系统所取得的精度水平，捷联惯导系统中陀螺的漂移 达到o 0 1 “ ，加速度计误差达到1 1 0 一，还不能完全满足各种军用和民用的要 求【l 。通过采用组合导航技术解脱了惯性系统的精度负担，同时保留了惯性系 统的自主性、短时间的相对高精度和连续提供全部导航参数的优点。目前广泛采 用的有s i n s g p s 组合方案，但是由于g p s 自身的缺点，使得该方案在军事上的 应用得到了限制。 对无人机自主着陆过程中视觉导航的研究国内外已有许多学校和机构在做， 并取得了很好的成果。加州大学伯克力分校将视觉导航应用到无人直升机自主着 陆中，他们研究了无人直升机着陆在人造着陆平台过程中的几个部分：图像处理、 状态估计和着陆控制，并给出了实际的飞行实验结果。最后飞行实验表明该视觉 导航系统可以使无人直升机的着陆精度达到：轴向定位精度5 c m ，姿念角精度为 5 度，南d m ) 、i _ | 大学也作了类似的研究工作，最后实际的飞行实验结果表明方向角 误差为7 度，平均定位精度为4 2 c m t2 “。目前，计算机视觉导航已经在无人直 升机着陆控制中得到了广泛的应用。佛罗里达大学的研究工作与上述两所大学有 所不同，他们研究了一套通过图像获得飞行器滚转角和俯仰角的方法，另外国内 外一些研究机构也对计算机视觉在固定翼无人机自主回收中的应用作了一定的 研究，本文的主要工作也是围绕着计算机视觉在固定翼无人机自主着陆过程中的 应用所展丌的。 两北r 业人学硕十学托沦文第一章绪论 1 3 本文的主要工作 本文的主要内容包括s i n s 与计算机视觉对固定翼无人机自主着陆过程进行 组合导航和s i n s 与计算机视觉组合时各种传感器的精度与最后定位精度的关 系，另外本文还对目前。泛采用的s i n s g p s 组合方案做了一定程度的研究。在 研究过程中采用了理论分析、数学建模、仿真相结合的方式。 1 ) 通过对惯导系统原理的学习，推导指北方位惯导系统的力学编排方程、 误差方程并建立惯性元件的误差模型。 2 ) 建立s i n s g p s 组合导航系统数学模型，并进行仿真。， 3 ) 借鉴s i n s g p s 组合导航系统提出了s i n s 与计算机视觉对无人机自主着 陆过程进行导航，并进行仿真分析，最后得出惯导器件精度和摄像机精度与最终 导航精度的关系，以及图像测量的角度误差与最后定位误差的关系。 论文各章安排如下： 第二章介绍了惯导系统的基本原理，对惯导系统的力学编排方程和误差方程 进行了推导，最后对计算机视觉和卡尔曼滤波方法进行了简单介绍。 第三章提出了s i n s 与计算机视觉对无人机自主着陆过程进行组合导航，并 进行建模和仿真。 第四章通过仿真得出惯导器件精度和摄像机精度与最终导航精度的关系，然 后推导得出图像测量的角度误差与最后定位误差的关系。 第五章介绍了s i n s g p s 进行组合导航的方法，并对其进行建模和仿真。 第六章对全文工作进行总结，并对一些本文不能完成的后续工作进行展望。 第二章基本理论 2 1s s 的基本原理、力学编排方程及误差分析 2 1 1 导航常用坐标系与参数说明 惯性导航是以牛顿力学定律为基础的，而牛顿力学定律又是在惯性空间内成 立，这就要求在对惯性导航迸行分析时需要引入惯性坐标系，作为讨论惯性导航 基本原理的基准。另外飞行器的位置、姿态、速度等导航参数又是通过各种坐标 系之间的关系来确定的，这些坐标系与惯性坐标系不同，它们可以根据导航需要 而选取，我们将它们统称为非惯性坐标系，如地球坐标系、地理坐标系、平台坐 标系、机体坐标系等。 ( 1 ) 惯性坐标系 1 ) 惯性坐标系( 下标为f ) 。x y , z , 。 理想的惯性坐标系是指绝对静止或只是做匀速直线运动的坐标系。由于宇宙 中的万物都是处于运动之中，因此想寻找绝对的惯性坐标系不可能的，我们只能 根据需要来选取惯性坐标系。对于地球附近运动的物体，我们通常采用地球地心 惯性系，这是因为虽然地球绕太阳公转速度的向心加速度为6 0 5 1 0 一g ，角速度 为o 0 4 1 。h ，但是因为在地球附近运动的载体，都有和地球一样的相对太阳的公 转运动，所以也都具有相同的向心加速度和转动角速度，这些都可以在导航计算 时忽略。地一t l , 坐标系以地球中心o o 为坐标原点，q 轴和q 咒轴在地球赤道平 面内，q x ，轴指向春分点，q z ，轴指向地球极轴，也就是地球中心惯性坐标系。 我们把它近似看作固定于惯性空间的坐标系，认为它是惯性元件测量的参考基 准。 另外在导航过程中常常会由于使用要求的不同而需要选用不同指向的惯性 坐标系。但我们通常所使用的惯性坐标系，如果没有特殊说明，指的就是地心惯 性坐标系。 ( 2 ) 非惯性坐标系 1 ) 地球坐标系( 下标为e ) q x , y 。 地球坐标系是固连在地球上的坐标系，它相对惯性坐标系以地球自转速率吼 西北一r 业人学坝十。7 ：能沦文 第二章基本理论 旋转。地球坐标系以地球中心q 为坐标原点，q 乙轴与q t 轴重合，q t 轴和 o o 儿轴在赤道平面内。q t 轴指向本初子午线，q 虬轴指向东经9 0 。方向。该坐 标系相对地球是静止的。通常，该坐标系在物体相对于地球定位时使用。 2 ) 地理坐标系( 下标为g ) o 咚。 地理坐标系是飞行器上用来表示飞行器所在位置的东向、北向和垂线方向的 坐标系。地理坐标系以载体重心o 为原点，o x , 轴指向东，即e ；c ，轴指向北， 即n ；o z 轴指向天顶，即u 。在本文中记为e n u 坐标系( 该坐标系在欧美国 家多选北西天方向，或者北东地) 通常，轴向的确定主要与使用习惯以及所处的 东西半球等情况有关，但是如果只是从导航计算的角度来讲的话，他i f - - 者之间 韵差别并不大。 3 ) 导航坐标系( 下标为 ) 魄虬0 。 导航坐标系是在导航时根据系统工作的需要而选取的作为导航基准的坐标 系。当导航坐标系选得与地理坐标系相重合时，可将这种导航坐标系称为指北方 位系统；为了适应在高纬度地区导航的需要往往将导航坐标系的o z 轴仍选得与 。轴重合，而使o 与呶及魄与回。之间相差一个自由方位角或游动方位角 口，这种导航坐标系可称为自由方位系统或游动自由方位系统。 4 ) 平台坐标系( 下标为p ) m ，y ，z ，。 平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系。平台坐标系 的坐标原点d 位于飞行器的重心处。当惯导系统不存在误差时，平台坐标系与导 航坐标系重合：当惯导系统出现误差时，平台坐标系就要相对导航坐标系出现误 差角。在平台式惯导系统中，该坐标系所描述的是真实平台( 物理平台) 所指向 的坐标系：在捷联式惯导系统中，由于没有真实的物理平台，该坐标系是通过存 储在计算机中的方向余弦矩阵来实现的，因此又叫做“数学平台”。 5 ) 机体坐标系( 下标为b ) 毛。 机体坐标系是固连在机体上得坐标系，时刻随着机体的运动而运动。该坐标 系以机体重心o 为坐标原点，o 轴沿机体横轴向右， 0 轴沿机体纵轴向前， 晓。轴沿机体竖轴向上，即右前上坐标系。 ( 2 ) 常用参数说明 目前，不同的资料中一些参数及变量的表示方法各不相同，这使得理解起来 有诸多的不便。因此，特将本文中常用的参数及变量统一作说明如下。 1 ) 兄和e 地球长半轴r 。= 6 3 7 8 1 3 7 k m ： 5 第二章基本理论 8 为地球的椭圆度，e = 1 2 9 8 2 5 7 。 2 ) l ，五和h l ，兄和h 分别表示飞行器所处的纬度、经度和高度。 3 ) 吼和g q ，为地球自转的角速度，0 3 i 。= 1 5 0 4 1 r h = 7 2 9 2 1 2 1 0 - 5 r a d s 。 g 为重力加速度，它是引力加速度和离心加速度的合成，其大小随纬度妒变 化而变化。对于国际椭球体，一般采用的重力公式如下： g = g 。( 1 + o 0 0 5 2 8 8 4s i n 2 妒一0 0 0 0 0 0 0 5 9s i n 22 f a ) 一o 0 0 0 0 0 0 3 0 8 6 h 其中：g o = 9 7 8 0 4 9 c m s 2z 9 8 州j 2 。 在地球表面附近，如忽略离心加速度的影响则可以简化为： r 2 r ，2 h 1 舻而f 吣。l 卜百j 。 4 ) r ，和r 在导航计算中把地球近似看成一个参考椭球体，那么在地球表面任一点的曲 率半径就不再是一个定值了。 r 。，表示当地予午面内主曲率半径： 咄( 1 - 2 e + 3 e s i n 2 l ) 或表示为专= 去( - 砌也s i n 2 上) 峨为和子午面垂直的法线平面内的主曲率半径： 目z r 。( 1 + e s i n 2l ) 或表示为毒2 1 ( 1 - e s i n 2 l ) 。 在本文中近似认为r m = r 。= r 。 5 ) ，y 和0 ，y 和0 是飞行器的三个姿态角，分别表示航向角、滚转角和俯仰角。飞 行器的姿态其实就是机体坐标系o x b y h z b 和地理坐标系e n u 之间的方位关系。当 飞行器绕垂线【，转动时，飞行器纵轴0 n 在水平面上的投影与地理子午线之间 的夹角叫做飞行器的航向角。航向角的数值是以地理北向为起点顺时针方向计算 的，其定义域为0 3 6 0 0 。飞行器纵向对称面( 0 h 轴与o z b 轴组成的平面) 与纵 向铅垂面( g 弘轴和u 组成的平面) 之间的夹角叫做飞行器的滚转角。滚转角实 际上就是飞行器绕纵轴的转角，从铅垂面起，右倾为正，左倾为负，定义域为 0 l8 0 ”。当飞行器绕横向水平轴转动时，飞行器纵轴和纵向水平轴之问的夹 角叫做飞行器的俯仰角。俯仰角从纵向水平轴算起，向上为正，向下为负，定义 域为0 一+ 9 0 0 。 6 西北工业大学硕士学位论文第二章基本理论 2 1 2 捷联惯导系统的基本原理 捷联惯导系统就是指将惯性敏感元件( 主要是加速度计和陀螺仪) 直接“捆 绑”在载体上的惯导系统。同平台式惯导系统相比，捷联惯导系统的最大特点是 取消了机械平台，改由导航计算机来完成机械平台的功能，即采用所谓的“数学 平台”。由于捷联惯导取消了机械平台，所以使惯导系统的结构简化、体积变小、 重量减轻、成本降低、可靠性提高。但是与此同时由于取消了机械平台，惯性器 件的量测值就不能直接用于导航计算，必须先经过复杂的坐标变换，把其变为符 合导航计算要求的值。这使得捷联惯导系统在计算上十分的复杂。 速度、位置初值 图1 捷联惯导系统原理简图 、姿态 捷联惯导系统基本原理如图1 捷联惯导系统原理简图所示。用陀螺仪测量 的载体坐标系( b 系) 相对于惯性坐标系( i 系) 的角速度历：，减去导航计算机 计算出的导航坐标系( 胛系) 相对于惯性坐标系的角速度脚：，则得到载体坐标 系相对于导航坐标系的角速度o ) n 。b ，于是利用这个角速度对姿态矩阵进行更新。 利用姿态矩阵把加速度计测量的沿载体坐标系轴向的载体的比力信息7 。通过坐 标变换，变换到导航坐标系内得到比力分量广，然后进行导航解算。同时利用 姿态矩阵的元素可以提取航向角和姿态信息。 由以上捷联惯导系统的基本原理可以看出，其具有如下特点： 1 ) 惯性元件直接安装在机体上，便于安装维护和更换。 2 ) 由于惯性元件可直接给出机体线加速度和角速度信息，可以省略专门为 飞行控制系统提供上述信息的传感器。 3 ) 因为取消了机械平台，减少了惯导系统中的机械零件，加之惯性元件体 积小、重量轻( 只有机械平台重量的1 7 左右) ，故便于采用更多的惯性元件来 实现余度技术，从而大大提高了系统的可靠性。 惯性元件的工作环境比平台式惯导中的惯性元件要差，惯性元件误差对系统 西北l 业大学硕士学位论文第二章基本理论 误差的影响要比平台惯导大。因此，捷联惯导系统对惯性元件的要求比平台式 惯导要高，要求惯性元件在机体的振动、冲击、温度等环境条件下精确工作，相 应的参数和性能要有很高的稳定性。同时，由于机体角运动的干扰直接作用在惯 性元件上，会产生严重的动态误差，因此，系统中必须采取误差补偿措施，如对 划桨效等误差。 2 1 3 捷联惯导系统的力学编排方程 捷联惯导系统的力学编排方程主要包括导航解算和姿态解算两部分。其中姿 态矩阵的计算是捷联式惯导算法的基础，也是捷联式系统所特有的。 2 1 3 1 姿态解算 姿态矩阵与姿态角的关系 由导航坐标系到机体坐标系的转换可以通过三次旋转变换实现： _ 儿乙坌l 量堕；! 鼙雨h 毛兰薹王雩：! ：! 上堑，五北z f 坌塑型! 笋兰皇上壁m z 。 川引现毫挪喾 口目 因为方向余弦阵是正交矩阵，因此有 g = r 令 涮 ( 2 ) ( 3) 哕y 锄o _ c !正巧五五疋五 。，。l = 四 西北工业大学硕士学位论文第二章基本理论 墨1 = c o s 7 c o s + s i n 7 s i n t z s i n 目 石2 = s i n c o s 8 死= s i n 7 c o s c o s y s i n s i n 8 毛= 一c o s y s i n p , + s i n z c o s s i n t 9 = c o s 妒c o s 8 ( 4 ) 毛= 一s i n y s i n y c o s 7 c o s p , s i n t 9 五 = 一s i n y c o s 8 正，= s i n 占 毛= g o s y c o s 8 可以看出这个变换矩阵的元素是飞机的姿态角的函数，如果求出了这个矩阵 的元素值，就可以计算出飞机的姿态。可得其主值分别为： 只，= s i n “( 瓦：) 纠a n - l ( 一夸 = t a n 一1 f - 兰t q - 一2 2 ( 5 ) ( 6 ) ( 7 ) 俯仰角曰定义在- + 9 0 0 区间，和反正弦函数的主值一致，不存在多值问题。 横滚角y 定义在+ 1 8 0 0 区间，航向角5 f ，定义在0 3 6 0 0 ，故都存在多值问题。、因此， 在算出主值之后，还要判断是在哪一个区间，从而确定姿态角和航向角的真值。 故需要用下面方法加以判断。 表1 横滚角主值确定表 乃。， 象限。 ，。 i + + y 。 + + 1 8 0 。 一1 8 0 i i i 及 9 西北工业大学硕士学立论文第二章基本理论 表2 航向角主值确定表 疋：五： 象限 + y 。 i 士 + 3 6 0 。 + 1 8 0 。 i i + 1 8 0 。 i i i 姿态矩阵求解 1 ) 欧拉角法 因为从导航坐标系依次转动i f ，口，角可得到机体坐标系，所以机体坐标系相 对导航坐标系的角速度向量缈可以表示为。 珊= 妒+ 目+ 户 即 国 出 国 +抖戮跏二施 。josy。1一sionyf扫0slny 0 c o s ) 0 1 + l ；q | |l + l 矿l | j| 一sinsiyn芗5目c。0sy。1cosycos9s i n y0 摹 占 | | 扫1 0 尹l 整理可得到 眵 c o s 口 求解上式就可得到姿态角，从而可以求出姿态矩阵。 2 ) 方向余弦法 设n 为导航坐标系，b 为机体坐标系，机体系相对于导航系以角速度d 转动， 1 0 8( 1llj o o y ，。，l1lj 口口o 口 s c 蠹 v 锄一置1| 硅北工业大学硕士学位论文 第二章基本理论 为空间内的一固定点，可以用向量尹来表示。 根据哥氏定理 疗l莎1一 i f 。2 现+ 0 3 r i b ” 将上式两边所有向量向咒坐标系投影可得 剖：= 鲁( + c x 秽 即 2 氛+ 噶灯” 根据向量的坐标系变换关系： 芦= c ；b 对上式两边求时间导数，得 声“= 四尹+ g ；6 = 。尹+ 篆f ： 联列( 1 0 ) 与( 1 1 ) 得 e ； b = 峨h x f ”= 毫c ：r b e l i c ：( d 尝c ： 础是国盘的反对称矩阵。将上式展开有： 陬t ：t ，i 正。正：训0 一畋咄i lt 宅：宅，1 = f 正，瓦：墨，| | q o q l 1 2 ) i 毫，t ：毫，jl 墨，：e 3 ：正，j 1 一哆山0l 式中四元数的基乇，五，吒取得与机体系的基一致。从而四元数的微分方程为 q = 三 式中国= o + c o ，+ 国， + c o ：k b 将上式写成矩阵形式，得 0 一l 一v 一2 一u q o g o g l q 2 吼 ( 1 4 ) 求解上述微分方程可实时地求出q lg ：，吼，吼根据下述公式即可把四元数转化 ) ) m u ( ( 皱。嘎 0 劬哆 班巩织 1 2 = ，j 吼吼吼 西北r ：业大学硕士学位论文第二章基本理论 为捷联姿态矩阵 l 舔+ g 一g ；一q ；2 ( q l q 。- q 。q 3 )2 ( q 。q 3 + 9 0 q 2 ) 丁= l2 ( 吼q 2 + 吼吼) 簖一薪+ g ；一右2 ( q 2 q 3 一9 09 1 ) f ( 1 5 ) 2 ( q ：q 3 一q o q 二)2 ( q 2 9 ，+ g 。q 1 ) q ：一q j q ；+ g ；f 4 ) 等效旋转矢量算法 四元数乘表示法和矩阵表示法的关系 ( ，y 。，z b ) 为向量r h 端点在机体坐标系内的坐标，机体系发生转动后该向量 变为r h , 端点坐标变为( ，y 。气，) ，q 为机体系的旋转四元数，o 为g 的共轭， 如果把向量吒和吒，看作零标量四元数，根据四元数的运算法则有 ，= o o r b 0 9 即 q r 耽， z q o- q l- q 2 - q 3 g lq o- q 39 2 9 2吼g o一吼 q 3一9 2q lg o 0 q j + q ? 一酲一菇 2 ( q l q 2 十g o q 3 ) 2 ( q l q 3 - q o q 2 ) 00 q + 0 虬 磊 留og l9 2碍3 一吼吼一9 39 2 - q 2q 3q o- q - q 3 - q 2吼q o 0 2 ( q 。q ：- q o q 3 ) q ；一q ? + q ；一菇 2 ( q 2 q 3 + g o q l ) 0 ( q l q 3 + 吼吼) j ( q z q 3 一q t ) i g ? 一q ；+ 爵 0 此 毛 ( 1 6 ) 由式( 1 5 ) 和( i 6 ) 可以看出坐标变换的四元数乘表示法和矩阵表示法 是等价的。 虽然旋转矢量法和四元数法都用到了四元数但是两者并不相同，二者关系如 下： 设如时刻的机体坐标系为b ( k ) ，导航坐标系为n ( k ) ，o 。时刻的机体坐标系 为b ( k 十1 ) ，导航坐标系为n ( k + 1 ) 。 则( ) = c 嚣2 ( 1 办即 ( m ) = c 掰”t “n ( k ) c b ( k + i ) ) ( ) ( 17 ) 设6 ( 尼+ 1 ) 至6 ( ) 的旋转四元数为y q b ( ( k ) ) ，z ( 七) 至6 ( 七) 的旋转四元数为q n ( k ) h ( k ) 至n ( k + o 的旋转四元数为叫q n ( ”k + ”，n ( 七+ 1 ) 至6 ( 七+ 1 ) 的旋转四元数为g 嬲。 与( 1 7 ) 式等价的四元数乘法表示式为： = 。r a n 。o 。：，+ ”。： 。碟：i ) 。 礤：j ”。g 裂。旅嚣， + ( 1 8 ) 幽北- _ l 业人学碰j 位沦文第：章基本理论 因此可得与姿态矩阵q 黼等价的四元数为q 卅n ( k + + ，l 卜吼i i ( ( 。k ) + 1 o 群j 旅) 百为6 ( 女) 至b ( k + 1 ) 的等效旋转矢量( 即各个坐标轴方向的角增量所构成的 女+ i 向量) 。百可以根据下式求得百= f 硝( f ) 出。 记目= i 百| ，则有g 嚣2 ，) = c o s 詈+ 告s i n 导。 需要注意的是由于陀螺角速度矢量的方向是变化的。如果直接采用角速度积 分，就会产生误差，可以通过采用多子样算法来减小这个误差。本文采用的等效 二子样算法如下：百= ( 巨+ 睫) + 要( 萌睫) 其中鼋，巨为姿态更新周期内陀螺两次 j 等删隔采样的角增量。 设t k 至0 + ，时刻运动载体的经纬度增量分别为丑和三，则n ( k ) 至盯( + 1 ) 的 旋转矢量为： 歹f m h 剖 g 测= c o s 2 4 + 7 。s i n 其中= 阱 鲥嚣为前一时刻得姿态四元数。 利用g 黪f ) ) _ q ：譬o 群；圆玩凇) 即可求出实时得姿态四元数 ( 1 9 ) 从而可以得到 四元数法和旋转矢量法都通过计算姿态四元数实现姿态更新，但前者直接求 解姿态四元数微分方程，而后者通过求解姿态变化四元数再求解姿态四元数，两 者的算法思路并不相同 各种姿态更新算法的比较 欧拉角算法通过求解三个欧拉微分方程可以直接训算出航向角、俯仰角和横 滚角。优点是关系简单明了，概念直观，容易理解，结算过程无需作正交化处理， 但是在计算机上进行数值积分时要进行超越函数的运算，这反而加大了计算的工 作量。当0 = + 9 0 。时，该算法出现奇点，不适合全姿态导航。 方向余弦法是一种九参数法，通过求解姿态矩阵微分方程来计算姿态矩阵。 优点是可以j 卣接得到姿态矩阵，避免了欧拉角法中的奇点问题，可以全姿态工作， 但是此方法实际卜是包含了九个未知量的线性微分方程组，计算量很大并且存在 西，i l - ；业火学硕十学位论文第二章基本理沧 非正交化i 5 4 2 ；，需进行n j 交化处理，而其正交化过程很复杂，这种方法在捷联姿 态计算中很少用。 四元数法只需要求解四个未知量的线性微分方程组，计算量比方向余弦小， 且算法简单，易于操作，是较实用的工程方法。但是四元数法实质上是旋转矢量 法中的单子样算法，对有限转动引起的不可交换误差的补偿程度不够，所以只适 用于低动态运载体的姿态解算，而对高动态运载体，姿态解算中的算法漂移会十 分严重。 旋转矢量法可采用多予样算法实现对不可交换性误差作有效补偿，算法关系 简单，易于操作，特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态 更新。 2 1 32 导航解算 速度更新 把捷联式加速度计测量的比力信息，6 用a 变换到导航坐标系厂”，可得到下 面的比力方程。 圪= ，”一( 2 卯：十曲二) 圪，+ g( 2 0 ) 把上式表示为矩阵形式为： 圪： v 麓 r 0 l2 v 十：，二 l 一( 2 + 。品) 一( 2 墨+ 印：。) o 2 0 , g + 国品 ，( 2 1 ) 联立求解这三个微分方程可以得到喘、f ：和曙：三个速度信息。 位置更新 飞行器所处位置的经、纬度和高度，可以直接由上面所求得的飞行器相对地 球的运动速度计算求得。 o o 僖 矿i训训业 哮机 斗 o 陇 西北i 业大学硕士学位论文第：章基本理论 上：上l + h 二 见2 ( r + h 。) c o s ； h = ( 2 2 ) 特别需要注意的是由于重力加速度f 不是一个常值，由下式 r 2 r 2 向、 舻岛丽镏。r i j 可以看出g 会随着高度的增加而减小。因此如果由于误差的影响求出的高度 大于真实高度，则求出的g 就会比真实值小，这会造成高度误差进一步增大，所 以纯惯导高度通道是不稳定的。因此，就不能仅仅用速度来计算飞行器在较长时 间内的高度信息，必须借用其它信号来进行修正。 2 1 4 捷联惯导系统的误差方程 2 1 4 1 捷联惯导系统误差源 实际的惯导系统不是一个理想的系统，它在工程实现中各个环节都不可避免 地存在误差。根据误差产生的原因和性质，惯导系统的误差源可以分为： ( 1 ) 元件误差：它包括加速度计和陀螺所引起的误差。 1 ) 陀螺误差 陀螺是运载体角运动的测量器件，对惯导系统的姿态误差产生直接的影响。 陀螺的随机漂移大致可概括为三种分量： a 启动漂移。它取决于启动时刻的环境条件和电器参数的随机性等因素，一 旦启动完成，这种漂移便保持在某一固定值上，但这一固定值是一个随机变量， 所以这种分量可用随机常数描述之。 哓。= 0( f = x ，y ，z )( 2 3 ) b 慢变漂移。陀螺在工作过程中，环境条件、电器参数都在作随机改变，所 以陀螺漂移在随机常数分量的基础上以较慢的速率变化。由于变化较缓慢，变化 过程中前后时刻上的漂移只有一定的关联性，即后一时刻的漂移值程度不同的取 决于前一时刻的漂移值，两者的时间点靠的越近，这种依赖关系就越明显。这种 漂移分量可用一阶马尔科夫过程来描述。 1 5 西北工业大学硕士学位论文第二章基本理论 一s h + 拼ho g ( 2 4 ) 其中0 3 ，。为零均值白噪声，若s 。的均方根为r ，则白噪声的方差强度满足 一下关系： 2 r ， q 。2 。 ( 2 5 ) c 快变漂移。表现为上述两种分量基础上的杂乱无章的高频跳变，不管两时 间点靠得多近，该两时间点上的漂移值依赖关系十分微弱或几乎不存在。这种漂 移分量可抽象化为白噪声过程。，即： e 【甜( t ) c o ( f ) = q 8 ( t r ) ( f2x ，y ，z ) ( 2 6 ) t ( f ) ：毛( ，) + 靠( f ) + 珊。( f ) ( 2 7 ) 阱四髀：兰纠 ， l 勘jl 毛：+ + 珊，j v ，：v m + ( j = z ，y ，z ) ( 2 9 ) 式中审。= o ，i = x ，y ，z ，。白噪声过程。 卧c 怪纠 cs 。， 理想情况下，三个加速度计的敏感轴l ，乙严格保持相互垂直，三个陀螺 西北t 业夫学硕士学位论文 第二章基本理论 仪的敏感轴，圪，z g 不仅要相互垂直，而且应当分别和加速度计的敏感轴保待 平行。但是，实际上不可能完全达到上述要求，总会存在一定的偏差，这种偏差 就是安装误差。 ( 3 ) 初始条件误差。指初始对准及输入计算机的初始位置，初始速度不准所 造成的计算误差。 这类误差主要指惯导系统开始导航状态工作之前，给计算机引入初始经、纬 度，高度所具有的操作误差矗，“及a h ，以及系统初始对准后所具有的姿态 误差。 ( 4 ) 计算误差。由于导航计算机的字长限制和量化器的位数限制等所造成的 计算误差。 ( 5 ) 模型误差。是由于力学编排中数学模型的近似造成的误差。 其中对惯导系统的工作性能影响较大的是元件误差，安装误差和初始条件误 著。 2 1 4 2 捷联惯导误差分析 速度误差方程 根据比力方程 审”= f ”一( 2 ：+ m ：) v ”+ 旷 而实际系统中总存在各种误差， 审。= e 乒6 一( 2 ：+ 二) v 。+ 酽 ( 3 1) 所以实际的速度计算值应由下述方程确定 ( 3 2 ) 式中 v 。= v ”+ s v ”：= ：+ 占：；二= 二+ a c o ：o g 。= g ”+ 万g ；c ：= c ：c ：= ( i 一叩”) c ： p = ( i + 占k 。】) ( i “占a ) f 6 + v 6 ： 0 一九九l p ”= l 九0一九l ； l 一九九0j 占k 。 = d i a g 6 k 。占巧，占e z 1 翌i 1 2 ：些奎堂堡主兰堡兰奎 堑三兰兰! ：! 坠 | o 5 a z - 6 a y i 【c s a _ 卜j 4 0 g s a 1 ： l 占4 5 a 。0j 其中，九，如，九为姿态误差角，# k a ，和6 a , ( i = x ，y ，z ) 分别为加速度计的刻 度系数和安装误差角 ( 3 1 ) 式减去( 3 2 ) ，忽略8 9 的影响，并略去二阶小量，得： j v = 一叩“f ”+ c ：( 【占k 卜 占a 1 ) f 6 + s v ”( 2 ：+ ：) + r 气， v “( 2n ，。+ 占二) + v ” 当取地理坐标系为导航坐标系时， r 0 ：= l q 。c o s l l 脚。s i n l r 0 l 6 ：= 1 5 l 6 0 。c o s li ； l6 l e o 庐i n 三j ：= 占二= 记 + h k r m 七h l t