水下滑翔机定位导航算法研究.docVIP

水下滑翔机定位导航算法研究 　　摘 要： 介绍了水下滑翔机定位导航的特点，提出了水面-水下导航定位算法。通过Matlab进行扩展卡尔曼滤波仿真处理，证明了该算法的有效性及鲁棒性。 　　关键字： 水下滑翔机； 导航定位； 卡尔曼滤波； Matlab仿真 　　中图分类号： TN96?34 文献标识码： A 文章编号： 1004?373X（2014）01?0054?03 　　0 引 言 　　水下滑翔机是一种无外挂推进系统、通过内置执行机构调整重心位置和净浮力来控制其运动的新型水下航行器，主要用于长时间、大范围的海洋环境监测。与传统水下航行器相比，水下滑翔机具有作业时间长、航行距离大、作业费用低和对母船的依靠性小等优点[1]。精确的导航能力是水下滑翔机有效应用和安全回收的一个关键技术[2]。然而，水下滑翔机成本较低，所携带的传感器数量和种类较少，同时需要长时间，大航程巡航，实现精确导航定位是不易的。 　　本文针对水下滑翔机导航定位的特点，提出了水面?水下导航定位算法，即水面通过GPS提供滑翔机的位置信息，水下采用电子罗盘和AHRS提供的滑翔机运动姿态信息[3]，结合数据融合理论[4]，运用扩展卡尔曼滤波对滑翔机进行航位推算。通过Matlab进行仿真处理，证明了该算法的有效性及鲁棒性。 　　1 导航基本算法 　　导航定位算法示意图如图1所示。 　　图1中设滑翔机位于[P（λ1，φ1）]点，搜索目标点[D（λ2，φ2），]其中[（λ，φ）]为点的经度、纬度。滑翔机要航行的东向和北向距离（单位：m）分别为： 　　[dy=kφ（φ2-φ1）] （1） 　　[dx=kλ（λ2-λ1）] （2） 　　式中：[kφ，kλ]为距离标度因子（单位：m/s）： 　　[kφ=30.636 7+0.005 19φ] （3） 　　[kλ=kφcosφ+0.250 67-0.003 9φ] （4） 　　 　　图1 导航算法示意图 　　2 坐标系的变换 　　由于水下滑翔机所携带的传感器中，GPS使用的是WGS?84坐标系，电子罗盘和AHRS使用的是载体坐标系，这就给数据处理带来了诸多不便。为解决这一问题，必须进行坐标系转换，将所用的导航数据全部转换到选定的坐标系中[5]。 以下坐标系变换都是将滑翔机从三位空间坐标[XYZ]投影到二维平面坐标系[XOY]中进行的。 　　2.1 地球坐标系 　　如图2所示，该坐标系与地球固联，坐标原点在地心，[Z]轴沿地球自转轴且指向北极，[X]轴与[Y]轴在地球赤道平面内，[X]轴指向零子午线，[Y]轴指向东经90°方向。运动体在该坐标系内的定位采用经度[λ]和纬度[φ]及距地心距离[R]来标定。 　　 　　图2 地球坐标系 　　2.2 GPS坐标系转换为地球坐标系 　　GPS使用的是WGS?84地心大地坐标系，将其中的点[（φ，λ，h）]转换为地球坐标系中的点[（x，y，z）]的转换关系如下： 　　[x=（N+h）cosφcosλy=（N+h）cosφsinλz=[N（1-e2）+h]sinφ] （5） 　　式中：[a]为地球长半轴，[b]为短半轴，[e2=（a2-b2）a2]为第一偏心率平方；[N=a（1-e2sin2φ）12]为东西圆曲率半径[6]。 　　2.3 载体坐标系转换为地球坐标系 　　水下滑翔机携带的电子罗盘和AHRS都是采用的载体坐标系（以当前航向所在方向作为[x]轴）。如图3所示，[xy]表示载体坐标系，[XY]表示地球坐标系，三角形代表载体。载体坐标系转换为地球坐标的推导如下： 　　[ΔY=Δxsinα+Δycosα] （6） 　　[ΔX=Δxcosα-Δysinα] （7） 　　[φK+1=φK+ΔXKφλK+1=λK+ΔYKλ] （8） 　　式中：从[k]时刻到[k+1]时刻，[Δx]表示载体在航向上的距离改变量，可以由AHRS在载体坐标系[x]轴方向上的加速度对时间积分求得，即：[Δx=12a2x；][Δy]表示在把航向逆时针旋转90°后的垂直航向方向上的距离改变量， 可以由AHRS在载体坐标系[y]轴方向上的加速度对时间积分求得，即：[Δy=12a2y；][Δα]表示[k]时刻地球坐标系[X]轴正方向在逆时针方向上与载体坐标系[x]轴的正方向的夹角，[α=α+Δθ。] 　　 　　图3 载体坐标系与地球坐标系转换 　　3 数据融合过程[7] 　　（1） 状态向量： 　　[Xv=（x，y，α，ax，ay）T] 　　式中：[x，y]是地球坐标系下滑翔机的位置坐标；[α]地球坐标系[X]轴正方向在逆时针方向上与载体坐标系[x]轴的正方向的夹角；[ax，ay]为滑翔机载体坐标系下东向和北向的分加速度。 　　（2） 状态方程：由[