专利名称：一种用于水下滑翔器的组合导航装置及方法
技术领域：
本发明涉及一种用于水下滑翔器的组合导航装置设计及方法，该装置具有体积小、低功耗、长航时等特点，可实现水下滑翔器的准确定位和自主导航。
背景技术：
电子罗盘可分为平面电子罗盘和三维电子罗盘，比起平面罗盘，三维电子罗盘因其内部加入了倾角传感器，克服了平面电子罗盘在使用中的严格限制，即使罗盘发生了倾斜，航向数据依然准确无误。有的罗盘还内置了温度补偿，最大限度得减少倾斜角和指向角的温度漂移。电子罗盘技术在国外应用比较多，但是在水下应用还存在一些问题。GPS技术在导航、定位、测速、定向方面有着广泛的应用，但由于其信号常被地形、 地物遮挡，导致精度大大降低，甚至不能使用。尤其是在高楼林立城区和植被茂密的林区， GPS信号的有效性仅为60 %。该技术在国内外应用都比较广泛，也比较成熟，但应用于海上潜水器的导航还没有成熟的应用方案。以惯性导航为主与GPS、电子罗盘集成的组合导航系统在国内外陆用、航空、航天以及水面载体有着广泛的应用，但水下滑翔器在水下工作时间很长，并且GPS信息在水下不能使用，单一惯性导航不能满足定位要求，且由于水下滑翔器对功耗、体积和精度要求较高，从而在水下滑翔器中组合导航系统均未能得到较好应用。如何使GPS、电子罗盘、捷联式惯性组合导航系统能利用三者的优势，使系统整体性能达到最优，并能满足水下滑翔器长航时、低功耗、小体积的要求，这种系统在国内、外均无报道。
发明内容
本发明的技术解决问题是克服现有技术的不足，设计了一种新的水下滑翔器的组合导航装置及方法，其算法克服了传统组合导航系统由于误差随时间累积而不能准确长时间定位的欠缺，并采取智能导航工作模式切换、多源系统自适应匹配、滤波和智能导航定位优化算法，提供了可靠性更高也更精确的定位服务。再加上硬件设计和软件优化，使得设备体积更小、功耗更低、航时更长，完成水下潜水器的自主导航和定位。本发明的技术解决方案为一种用于水下滑翔器的组合导航装置，包括电子罗盘、 微机电系统惯性测量单元(MEMS-IMU)、全球定位系统(GPS)接收模块、数字信号处理(DSP) 处理模块；电子罗盘测量方位角得到航向信息；微机电系统惯性测量单元(MEMS-IMU)测量姿态和航向变化，并借助电子罗盘航向信息进行校正；全球定位系统(GPS)接收模块则完成对水下滑翔器的水面定位、微机电系统惯性测量单元(MEMS-IMU)的标定及校正；数字信号处理(DSP)处理模块完成对导航系统中各个模块数据的转化、处理，并将航向位置等数据输出到水下滑翔器中心计算机，实现自主导航与定位。所述电子罗盘采用型号为ADIS16405，其内包括三轴陀螺，三轴加速度计，三轴磁力计。
4
所述数字信号处理(DSP)处理模块采用型号为TMS320C5505。所述方法如下采用电子罗盘测量方位角得到航向信息；采用微机电系统惯性测量单元(MEMS-IMU)测量姿态和航向变化，并借助电子罗盘航向信息进行校正；采用全球定位系统(GPQ接收模块则完成对水下滑翔器的水面定位、微机电系统惯性测量单元 (MEMS-IMU)的标定及校正；采用数字信号处理(DSP)处理模块完成对导航系统中各个模块数据的转化、处理，并将航向位置等数据输出到水下滑翔器中心计算机，实现自主导航与定位。所述电子罗盘是由三维磁阻传感器、双轴倾角传感器和微控制单元(MCU)组成； 三维磁阻传感器用来测量地球磁场；倾角传感器是在罗盘随着潜水体的倾斜而处于非水平状态时，对罗盘进行倾角补偿，保证了数据依然准确无误；微控制单元(MCU)处理磁力仪和倾角传感器的信号以及数据输出和软铁、硬铁补偿。一种用于水下滑翔器的组合导航装置的方法，算法包括以下(1)初始时刻，GPS对导航测量单元包括三维电子罗盘、三轴加速度计和三轴陀螺进行初始校正，得到初始信息；电子罗盘提供载体的初始航向信息，通过传递对准给捷联惯性导航系统以获得初始的失准角信息，而微机电系统惯性测量单元测得的角速率和加速度信息由捷联惯性导航系统导航姿态解算模块来处理，导航姿态解算系统由角速率和加速度信息解算出载体的速度、位置及姿态信息；陀螺和加速度计分别用来测量载体的角运动和线运动信息，导航解算模块根据这些测量信息建立姿态阵解算的数学平台，从而解算出运动载体的航向、姿态、速度及位置；(2)在长时间运行时，采用电子罗盘的航向角信息来对MEME-SINS输出的信息进行校正，相应的差值作为无迹卡尔曼滤波器的观测量，经无迹卡尔曼滤波估计出的惯导参数误差反馈到捷联惯性导航系统导航解算模块内，在力学编排方程中校正惯性传感器的输出，推算的速度和经纬度、建立的姿态矩阵数字平台，将校正后的参数代入下一次运算；经过反馈校正后，捷联惯性导航系统输出的导航参数就是组合导航系统的输出；(3)针对水下滑翔器的具体情况，利用航位推算原理，分析航位推算的误差源，在其基础上推导航位推算误差方程，并应用航位推算误差方程对初始误差角、刻度因子和陀螺随机常值漂移进行补偿；对于电子罗盘，通过对软铁及硬铁干扰的分析和建模，提出应用分段补偿的方法对其进行误差补偿；(4)水下滑翔器工作时间是以水下为主，隔段时间会上浮接收GPS信息，为了保证系统低功耗，在水上水下采取不同的算法水下采用AUKF算法；水上采用联邦滤波算法；(5)采用水下滑翔器每次工作前首先进行静止零位误差校正；滑翔体在水中滑翔后浮出水面，首先进行静止零位误差校正，再利用航位推算位置误差来计算初始航向误差角的方法，对动态粗对准的初始航向角进行补偿，完成动态精对准过程；(6)在陀螺、加速度计和电子罗盘等多传感器数据中，采用新息法来剔除野值。本发明与现有技术相比的优点在于(1)应用于水下滑翔器基于电子罗盘、MEMS捷联惯性系统与GPS的系统集成技术， 实现较简单，体积较小、成本较低、易于优化；(2)本发明可智能切换导航工作模式和信息融合算法，采用组合系统的低功耗设计技术，使水下滑翔器更具有自主性、更合理地利用导航信息资源，适应环境能力强；(3)基于联邦无迹卡尔曼滤波器与自适应无迹卡尔曼滤波器的导航和定位算法， 使得系统具有长航时性、高可靠性和高精度。
图1为一种用于水下滑翔器的组合导航系统原理示意图；图2为捷联式惯性导航系统原理图；图3为航位推算原理图；图4为理想原始测量数据获取具体实施方案下面结合附图对发明的技术方案进行详细说明图1是水下滑翔器的组合导航系统原理示意图，选用的新型IMU单元将三轴陀螺， 三轴加速度计，三轴高性能磁力计集成与一体，该单元体积小，集成度高，功耗低，带有自校正和标定功能。在水下滑翔器放入水中之前，要首先用静止零位误差校正的方法对陀螺产生零位误差进行校正。电子罗盘除了自带的误差补偿外，还应利用GPS信息和分段补偿的方法对其进行误差补偿。这样就使导航测量单元(包括三维电子罗盘、三轴加速度计和三轴陀螺)得到精确的初始信息。图4是理想原始测量数据信息获取路径，在经过对惯性器件标定并获取理想原始信息的基础上，设计基于捷联状态下的适合水下滑翔器运动环境的姿态测量算法。当滑翔器进入水下前行时，因水下的环境相对稳定，前行速度均勻，利用海流数据库得出海流速度，推算出恒定的外在干扰以用于后续的航位推算。水下只有电子罗盘和 MEMS-IMU单元进行工作，电子罗盘提供载体的初始航向信息，通过传递对准给捷联惯性导航系统以获得初始的失准角信息，而MEMS测得的角速率和加速度信息由SINS导航姿态解算模块来处理，导航姿态解算系统由角速率和加速度信息解算出载体的速度、位置和姿态信息。陀螺和加速度计分别用来测量载体的角运动和线运动信息，导航解算模块根据这些测量信息建立姿态阵解算的数学平台，从而解算出运动载体的航向、姿态、速度及位置。由于惯性导航仪因陀螺的漂移存在随时间积累的误差，故在长时间运行时，采用电子罗盘的航向角信息来对MEMS-SINS输出的信息进行校正，相应的差值作为卡尔曼滤波器的观测量，经卡尔曼滤波估计出的惯导系统导航参数误差反馈到SINS导航解算模块内，在力学编排方程中校正惯性传感器的输出、推算的速度和经纬度、建立的姿态矩阵数字平台，将校正后的参数代入下一次运算。因此，经过反馈校正后，惯导系统输出的导航参数就是组合导航系统的输出。 在水下只有MEMS-IMU和电子罗盘的作用，采用自适应UKF方式来进行滤波，但当滑翔器经过M小时浮到水面，接收到的GPS信号对MEMS-IMU进行校正，由于导航传感器的增加，就需采用基于UKF的联邦滤波算法处理多传感器的信息融合来提高导航精度。这时导航系统处在动态环境下，要在短时间和复杂环境下对捷联惯组进行动态高精度初始对准，利用GPS信息对MEMS捷联惯性导航系统予以重新对准和校正的同时，还要利用航位推算位置误差来计算初始航向误差角的方法，对动态粗对准的初始航向角进行补偿，通过信页
息融合方法，完成动态精对准过程。在水下到水上，再到水下的过程中，由于传感器的数量变化导致不同信息量的融合，为了降低功耗，提高系统的处理效率，采用切换式组合方式进行航位推算，即GPS模式和DR模式。对于陀螺、加速度计和电子罗盘等多传感器信号数据中出现的野值，采用新息法来剔除这些野值，这种算法不再依赖于先验的传感器特性，而是通过每次滤波迭代的时候进行判断，为了滤波的连续性，必须在野值被剔除后进行补值，利用对当前新息值加权的方法进行补值，减小新息对滤波精度的影响。本发明具体如下一种用于水下滑翔器的组合导航装置及方法，其特点在于由 GPS接收模块、MEMS-IMU单元及电子罗盘组合成的导航系统，根据不同环境自主重构组合方法，并融入最优算法，充分利用有限的导航数据实现高精度、高可靠性的水下滑翔体定位与导航，具体步骤如下(1)算法方面具体为(a)航位推算在MEMS捷联姿态算法对准的基础上，结合电子罗盘信息、GPS接收机输出信息、海流数据等进行推算，它们当中任何一个精度出现误差都会影响到航位推算的精度，这其中以MEMS陀螺和电子罗盘的影响最大，除航位推算系统外，海流、潮汐和风是影响航位推算的三个重要的因素。(b)海流数据库的应用海流主要包括洋流、潮汐或潮流、风、涌、浪等。在近海航行或远海航行时，掌握大洋海流的运动规律，准确估计其流向和流速是提高定位精度、减少事故、节省能源的重要手段。为了准确地计算或者估算海流的大小，还需要通过对一些固定目标进行观测，对数据加以修正。(c)切换式组合的应用采用切换式组合方式的航位推算方案，切换式组合方案有两种工作状态GPS模式和航位推算(DR)模式，系统工作在何种模式取决于GPS信号的有效性和DOP精度因子。当滑翔器浮到水面时，系统工作在GPS模式，同时利用GPS的输出数据，刷新DR系统的推算位置，对航位进行校正；一旦滑翔器潜入水中，GPS定位数据失效或DOP增大到预定的门限，则切换到DR模式。(d)如图3，假设载体在tg时刻的位置为(Xh，y^，^)，行驶了 At(W1)时间，在、时刻到达位置(Xi，Ji, Zi)，在、时刻方位角为Ai,倾斜角为Ii,坐标位置可以表示为
Xi = X^1 +ALgpos 4 经 in Zij yt = yt_x + AL 经 in A1 经 in I1 .W1+ALgposTiVi = Vh+VV在图3中的非水平段，位置除了有χ和y方向上的变化，还有ζ方向的变化，在水平段位置只有X和y方向上的变化。由于滑翔器是缓慢勻速前行，再结合海底环境，可以认为是相对稳定的环境下前行，所以可以将速度ν看成恒值，根据水下海流速度确定水下滑翔器的滑翔速度。前面(1)的(b)中提到了建立海流的数据库，这样就可假定在水下前行受到的阻力也是恒定值，采用一些白噪声作为随机干扰Vv，推算出Vi。由以上可以得出位置和速度信息。(e)由导航传感器本身的元件误差和外界环境及算法引起的系统误差是船位推算
7的误差源。在元件误差中，对陀螺影响较大的误差主要是陀螺的常值误差和陀螺漂移误差， 在水下采用将电子罗盘和MEMS捷联惯性导航系统输出的两个航向信息进行融合，用电子罗盘的信息估计陀螺误差，不断修正MEMS捷联惯性导航系统输出的航向。(f)电子罗盘的航向输出极易受外界干扰磁场的影响而使精度降低，本发明选用的新型组合导航单元内含了三维电子罗盘，其微处理器中内置了误差补偿算法，可以对电子罗盘受到的软、硬磁干扰进行补偿，但效果有限。为了提高电子罗盘的定向精度，在使用前进行误差补偿。因电子罗盘误差方程在整量程拟合曲线时，尾端出现较大误差，所以将整个区域划分为两部分，前部分应用八位置最小二乘法进行误差补偿；后部分不考虑高次谐波项再构造另一误差模型方程，在该部分取若干采样点，将采样点代入误差方程，利用最小二乘法解算出误差补偿系数，再将误差补偿系数代入模型方程可得到磁航向角。(g)在实际水下导航过程中，由于数学模型本身的不确定性，再加上环境噪声的影响，预测出来的结果带有误差。为了减小误差，使得结果尽可能地接近真实值，需要进行补偿。在传统的卡尔曼滤波器的基础上提出将自适应滤波和无迹卡尔曼滤波器(UKF)相结合的自适应UKF(AUKF)算法。采用方差调整的原则，将存在偏差的协方差矩阵进行自适应调整，以合理调整在滤波解中的作用。合理的自适应因子能平衡动力学模型信息与观测信息的权比，而且能够控制动力学模型误差对导航参数解的影响。由于预测残差能反映动力学模型的误差量级。若观测信息可靠，动力学模型也可靠，则由动力学模型预测的状态参数， 并由状态参数与观测信息求得的预测残差应该很小；反之，若观测信息可靠，且预测残差很大，则表明动力学模型误差较大。如此可以依据预测残差构造判别统计量。自适应因子能够很好地抑制初始值偏差和动力学模型异常扰动误差对导航解的影响，所以将自适应因子引入到UKF算法能够很好地提高滤波器的性能。(h)在水下只有MEMS-IMU和电子罗盘的作用，采用上述方式来滤波，但当滑翔器浮到水面，接收GPS信号进行信息校正时，由于导航传感器的增加，这时需采用新的滤波算法处理多传感器的信息融合来提高导航精度。组合导航系统测量信息有冗余，当某个敏感器性能下降，测量噪声增大时，联邦滤波算法通过调整动态分配因子，降低该子滤波器的置信度，从而保证主滤波器的高精度估计结果，确保了导航系统的精度。同时将UKF方法应用到联邦滤波中，由于UKF避免了线性化引入的截断误差，所以基于UKF的联邦滤波算法在观测信息较少以及系统初始估计偏差较大时，仍能保持比传统的EKF方法更高的精度，具有更好的鲁棒性。(i)因每次的系统启动而使陀螺产生零位误差，采用水下滑翔器每次工作前首先进行静止零位误差校正的方法。算法如下
权利要求
1.一种用于水下滑翔器的组合导航装置，其特征在于包括电子罗盘、微机电系统惯性测量单元(MEMS-IMU)、全球定位系统(GPS)接收模块、数字信号处理(DSP)处理模块；电子罗盘测量方位角得到航向信息；微机电系统惯性测量单元(MEMS-IMU)测量姿态和航向变化，并借助电子罗盘航向信息进行校正；全球定位系统(GPS)接收模块则完成对水下滑翔器的水面定位、微机电系统惯性测量单元(MEMS-IMU)的标定及校正；数字信号处理(DSP) 处理模块完成对导航系统中各个模块数据的转化、处理，并将航向位置等数据输出到水下滑翔器中心计算机，实现自主导航与定位。
2.根据权利要求1所述的一种用于水下滑翔器的组合导航装置，其特征在于所述电子罗盘采用型号为ADIS16405，其内包括三轴陀螺，三轴加速度计，三轴磁力计。
3.根据权利要求1所述的一种用于水下滑翔器的组合导航装置，其特征在于所述数字信号处理(DSP)处理模块采用型号为TMS320C5505。
4.一种如权利要求1所述的一种用于水下滑翔器的组合导航装置的方法，其特征在于所述方法如下采用电子罗盘测量方位角得到航向信息；采用微机电系统惯性测量单元 (MEMS-IMU)测量姿态和航向变化，并借助电子罗盘航向信息进行校正；采用全球定位系统 (GPS)接收模块则完成对水下滑翔器的水面定位、微机电系统惯性测量单元(MEMS-IMU)的标定及校正；采用数字信号处理(DSP)处理模块完成对导航系统中各个模块数据的转化、 处理，并将航向位置等数据输出到水下滑翔器中心计算机，实现自主导航与定位。
5.根据权利要求4所述的一种用于水下滑翔器的组合导航装置的方法，其特征在于所述电子罗盘是由三维磁阻传感器、双轴倾角传感器和微控制单元(MCU)组成；三维磁阻传感器用来测量地球磁场；倾角传感器是在罗盘随着潜水体的倾斜而处于非水平状态时，对罗盘进行倾角补偿，保证了数据依然准确无误；微控制单元(MCU)处理磁力仪和倾角传感器的信号以及数据输出和软铁、硬铁补偿。
6.根据权利要求4所述的一种用于水下滑翔器的组合导航装置的方法，其特征在于， 算法包括以下(1)初始时刻，GPS对导航测量单元包括三维电子罗盘、三轴加速度计和三轴陀螺进行初始校正，得到初始信息；电子罗盘提供载体的初始航向信息，通过传递对准给捷联惯性导航系统以获得初始的失准角信息，而微机电系统惯性测量单元测得的角速率和加速度信息由捷联惯性导航系统导航姿态解算模块来处理，导航姿态解算系统由角速率和加速度信息解算出载体的速度、位置及姿态信息；陀螺和加速度计分别用来测量载体的角运动和线运动信息，导航解算模块根据这些测量信息建立姿态阵解算的数学平台，从而解算出运动载体的航向、姿态、速度及位置；(2)在长时间运行时，采用电子罗盘的航向角信息来对MEME-SINS输出的信息进行校正，相应的差值作为无迹卡尔曼滤波器的观测量，经无迹卡尔曼滤波估计出的惯导参数误差反馈到捷联惯性导航系统导航解算模块内，在力学编排方程中校正惯性传感器的输出， 推算的速度和经纬度、建立的姿态矩阵数字平台，将校正后的参数代入下一次运算；经过反馈校正后，捷联惯性导航系统输出的导航参数就是组合导航系统的输出；(3)针对水下滑翔器的具体情况，利用航位推算原理，分析航位推算的误差源，在其基础上推导航位推算误差方程，并应用航位推算误差方程对初始误差角、刻度因子和陀螺随机常值漂移进行补偿；对于电子罗盘，通过对软铁及硬铁干扰的分析和建模，提出应用分段补偿的方法对其进行误差补偿；(4)水下滑翔器工作时间是以水下为主，隔段时间会上浮接收GPS信息，为了保证系统低功耗，在水上水下采取不同的算法水下采用AUKF算法；水上采用联邦滤波算法；(5)采用水下滑翔器每次工作前首先进行静止零位误差校正；滑翔体在水中滑翔后浮出水面，首先进行静止零位误差校正，再利用航位推算位置误差来计算初始航向误差角的方法，对动态粗对准的初始航向角进行补偿，完成动态精对准过程；(6)在陀螺、加速度计和电子罗盘等多传感器数据中，采用新息法来剔除野值。
全文摘要
本发明公布了一种用于水下滑翔器的组合导航装置及方法，装置包括电子罗盘、微机电系统惯性测量单元、全球卫星系统接收模块、数字信号处理处理模块。本发明采用MEMS陀螺、加速度计组成的姿态测量单元和电子罗盘集成为惯性组合导航与定位系统来与控制系统配合，使用DSP技术作为导航解算部件，经过在全温范围内对各传感器进行降噪、温度补偿、非线性校正、交叉耦合补偿及航位推算等多种算法，完成水下滑翔体的自主准确定位。本发明的优点在于体积小、集成度高、功耗低、长航时、成本低等，能迅速准确地得到水下滑翔器当前位姿信息，使它保持本体平衡，同时为其提供航迹和位置参数。
文档编号G01C17/32GK102519450SQ20111041211
公开日2012年6月27日 申请日期2011年12月12日 优先权日2011年12月12日
发明者刘虎, 周智恺, 徐元, 陈熙源, 黄浩乾 申请人:东南大学