专利名称：一种小型水下机器人组合导航系统及导航方法
技术领域：
本发明涉及的是一种小型水下组合导航系统，本发明也涉及一种小型水下组合导 航方法。
背景技术：
目前智能水下机器人向小型化发展是其发展趋势之一。小型水下机器人由于其体 积小、机动灵活、成本低、搭载方便，在军民用都有广泛的应用前景。导航问题仍然是水下机 器人设计所面临的主要关键技术之一。现在水下机器人导航常用方法有航位推算法、惯性 导航及声学方法等。惯导系统一般体积较大，价格昂贵，最重要的是纯惯性导航系统导航 在没有其它传感器数据信息进行补偿的情况下其定位误差将随时间和航程累积和发散，水 声定位系统(长基线、短基线、超短基线等)定位精度有很大提高，其缺点就是作用距离有 限。航位推算导航是一种低成本的导航方法，是水下机器人重要的导航手段，其方法简单、 经济，水下机器人除了配备测速仪、姿态、深度传感器等外，只需要给定初始位置信息，通过 一定的算法就可构成具有一定精度的实时、可靠的自主式导航系统。微小型水下机器人受 限于体积、成本、能源等限制，这对导航系统提出了更高的要求，这对构造一种小型化组合 导航系统增加了难度。导航系统一般由小型化、低成本传感器构成，传感器精度降低，同时， 水下机器人一般工作在特有的海洋水下环境，干扰噪声信号大，各种水声传感器普遍存在 精度低、野点率高的缺点，对传感器数据进行较好的滤波剔出野点信息尤为重要。航位推算 导航一般需要定期进行位置校正，如果潜深较大通过浮出水面通过GPS进行位置重调，这 将消耗较多能源，这对能源有限的微小型水下机器人尤其不利。因此，研制一套可实现多种 途径进行位置校正和能够有效地剔除传感器野点信息的小型化水下机器人导航系统对微 小型水下机器人的实际工程应用具有重要的意义。申请号为200520020572. 2，名称为“小型水下自主组合导航装置”的专利文件中， 提供了一种小型水下自主组合导航装置。申请号为201010114558. 4，名称为“一种小型水 下航行器用组合导航系统及方法”的专利文件中，提供了一种由捷联惯性导航系统、多普勒 导航系统、地形辅助导航系统和磁罗经组成的水下组合导航导航系统和方法。

发明内容
本发明的目的在于提供一种体积小、重量轻、成本低，利于实现水下导航系统小型 化的小型水下机器人组合导航系统。本发明的目的还在于提供一种基于小型水下机器人组 合导航系统的导航方法。本发明的小型水下机器人组合导航系统包括嵌入式导航处理机、耐压GPS、微型 姿态传感器、速度计、深度计及继电器元件，微型姿态传感器安装在水下机器人本体耐压舱 内，微型姿态传感器的信号传入嵌入式导航处理机，速度计、深度计安装在水下机器人本体 下部，水声通讯声纳安装在水下机器人本体首部上方，速度计、深度计、水声通讯声纳信号 通过水密线缆与布置在水下机器人本体耐压舱内的嵌入式导航处理机相连；嵌入式导航处理机包括CPU核心模块、AD板、4串口卡、电源板，电源板由MV电池供电并通过PC/104总 线给其他板卡供电；所有板卡数据信息通过PC/104总线传输。本发明的小型水下机器人组合导航系统还可以包括1、所述速度计、深度计、水声通讯声纳电源开关由AD板IO通道执行。2、耐压GPS、微型姿态传感器、速度计、水声通讯声纳接入嵌入式导航处理机8串 口卡进行数据通讯，深度计接入嵌入式导航处理机AD板。3、耐压GPS、微型姿态传感器、速度计、水声通讯声纳通过嵌入式导航处理串口卡 以RS232协议方式接收数据或指令信息。4、所述深度计为小型模拟信号输出传感器，通过嵌入式导航处理机AD板采集。本发明的基于小型水下机器人组合导航系统的导航方法为采用由嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计、水声通讯 声纳及继电器元件组成的小型水下机器人导航系统；(1)通过微型姿态传感器、深度计分别获得水下机器人本体姿态和深度信息，通过 深度计获得机器人本体速度和高度信息，水下机器人若在水面获取耐压GPS信息，若在水 下通过水声通讯声纳获取位置和指令信息；所述本体姿态包括艏摇、横摇、纵摇；(2)采用新息修正的抗野值自适应卡尔曼滤波方法对获得的传感器信息进行数据 处理；(3)跟据磁偏角数据库计算得出磁偏角；(4)接收规划导航位置校正指令信息，依据机器人本体姿态、深度和速度信息进行 导航位置计算。本发明的基于小型水下机器人组合导航系统的导航方法还可以包括1、所述的基于新息修正的抗野值自适应卡尔曼滤波方法为(1)用标准的卡尔曼滤波方程计算出状态估计值；(2)计算系统干扰均值q⑷,系统干扰方差矩阵,量测噪声均值,量测噪声方差矩阵；系统干扰均值
权利要求
1.一种小型水下机器人组合导航系统，包括嵌入式导航处理机、耐压GPS、微型姿态传 感器、速度计、深度计及继电器元件，其特征是微型姿态传感器安装在水下机器人本体耐 压舱内，微型姿态传感器的信号传入嵌入式导航处理机，速度计、深度计安装在水下机器人 本体下部，水声通讯声纳安装在水下机器人本体首部上方，速度计、深度计、水声通讯声纳 信号通过水密线缆与布置在水下机器人本体耐压舱内的嵌入式导航处理机相连；嵌入式导 航处理机包括CPU核心模块、AD板、4串口卡、电源板，电源板由24V电池供电并通过PC/104 总线给其他板卡供电；所有板卡数据信息通过PC/104总线传输。
2.根据权利要求1所述的小型水下机器人组合导航系统，其特征是所述速度计、深度 计、水声通讯声纳电源开关由AD板IO通道执行。
3.根据权利要求2所述的小型水下机器人组合导航系统，其特征是耐压GPS、微型姿 态传感器、速度计、水声通讯声纳接入嵌入式导航处理机8串口卡进行数据通讯，深度计接 入嵌入式导航处理机AD板。
4.根据权利要求3所述的小型水下机器人组合导航系统，其特征是耐压GPS、微型姿 态传感器、速度计、水声通讯声纳通过嵌入式导航处理串口卡以RS232协议方式接收数据 或指令信息。
5.根据权利要求4所述的小型水下机器人组合导航系统，其特征是所述深度计为小 型模拟信号输出传感器，通过嵌入式导航处理机AD板采集。
6.基于小型水下机器人组合导航系统的导航方法，其特征是采用由嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计、水声通讯声纳 及继电器元件组成的小型水下机器人导航系统；(1)通过微型姿态传感器、深度计分别获得水下机器人本体姿态和深度信息，通过深度 计获得机器人本体速度和高度信息，水下机器人若在水面获取耐压GPS信息，若在水下通 过水声通讯声纳获取位置和指令信息；所述本体姿态包括艏摇、横摇、纵摇；(2)采用新息修正的抗野值自适应卡尔曼滤波方法对获得的传感器信息进行数据处理；(3)跟据磁偏角数据库计算得出磁偏角；(4)接收规划导航位置校正指令信息，依据机器人本体姿态、深度和速度信息进行导航位置计算。
7.根据权利要求6所述的基于小型水下机器人组合导航系统的导航方法，其特征是所 述的基于新息修正的抗野值自适应卡尔曼滤波方法为(1)用标准的卡尔曼滤波方程计算出状态估计值；(2)计算系统干扰均值,系统干扰方差矩阵,量测噪声均值,量测噪声方差 矩阵R⑷；系统干扰均值c{(k) = (\-dk_l)({(k-\) + dk_l X(k/k)-0(k,k-l)X(k-l/k-l)系统干扰方差矩阵 Q(^) = (1-d(k-I) + 式—JKi (众)/ (k)Kr (k) + F(k / k)- Φ(々, k-\)V(k-Hk- 1)ΦΓ (k/k-1)]量测噪声均值；r(k) = (1-dk_,)r(k-1) + dk_, Z(k)-Η(Λ)X(k/k-l)( 3 )量测噪声方差矩阵R(yt) = (1-dk_,) R(yt-1) + dk_, [ε(Λ)εΓ (k) - H⑷/ k - 1)ΗΓ ⑷](4 )其中Χ@)、(m、R⑷分别是状态X(k)、系统状态噪声序列的方差阵Q(k)、量测噪 声序列方差阵R(k)的估计，ε (k) = Z(k)-r(k-l)-H(k)X(k|k-l)为新息矩阵，dk = l~h/(l"bk+1)，b 是遗忘因子, = ‘ ‘ ； ^ ^1t 1 tJ；(3)利用野值判据进行野值判别|ε(Λ)| > ayjΗ(Λ) P(k, k - 1)Η(Λ)Τ + R(yt)( 5 )其中，α为常数，若( 式判据成立则本次测量值为干扰数据即野值，转到下一步对新 息进行修正；若(5)式判据不成立，则本次测量值为正常数据，不需要对新息进行修正；(4)对新息进行修正如果k时刻经野值判据(5)判断为野值，则采用下式对新息进行修正ε (k) = ε (k-1) β !+ ε (k_2) β 2+…+ ε (1) β其中{ β J = dH，d^b, d^b2, ... ‘Ι/-1为前k-1次新息加权系数序列。
8.根据权利要求6或7所述的基于小型水下机器人组合导航系统的导航方法，其特征 是所述导航位置计算的方法为(1)通过速度计接收传机器人艇体速度、高度数据信息，姿态传感器获得机器人艏向、 横摇、纵摇姿态信息；(2)采用新息修正的抗野值自适应卡尔曼滤波方法对传感器数据信息进行滤波处理；(3)将艇体速度进行坐标转换为大地坐标系下速度信息将水下机器人艇体坐标系下的速度通过转换矩阵(6)式计算求得水下机器人大地坐 标系下的速度，[Vn Ve vu]T = T [Vx Vy vjT(6)αψχεθ ηψ3ηφΗ3￡ψ3 ιθιΧ￡φT= 3ηψ ￡θ( )(εεθ ηφ<xs6(mp其中[Vn Ve VU]T为大地坐标系下水下机器人北向、东向、垂向速度，[Vx Vy νζ]τ分艇体 坐标下的水下机器人纵向、横向、垂向速度，ψ，θ，识分别为水下机器人的横摇、纵摇以及艏 向姿态角；(4)根据速度信息前一时刻位置信息进位置推算若不进行校正以前一时刻位置信息作为为位置推算初始点，若水面校正将GPS信息作 为位置推算初始点，若在水下通过水声通讯校正则将根据母船位置及与水下机器人相对位 置计算求得的机器人位置作为位置推算的初始点，对速度进行积分考虑地球椭球模型获得 水下机器人位置。
全文摘要
本发明提供的是一种小型水下机器人组合导航系统及导航方法。该组合导航系统包括嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计、水声通讯设备。当水下机器人载体在水下时，组合导航系统上电后，导航系统自主运行，通过AD板IO通道控制继电器打开传感器，进行数据采集，获得导航初始位置，根据磁偏角数据库计算得出磁偏角，接收规划导航位置校正指令信息，进行导航位置推算，从而获得水下机器人的经纬度位置信息。该系统体积小、重量轻、成本低，利于实现水下导航系统小型化，可应用于要求体积重量小的水下观察、探测及小型水下机器人。
文档编号G01C21/16GK102052923SQ20101055904
公开日2011年5月11日 申请日期2010年11月25日 优先权日2010年11月25日
发明者万磊, 孙玉山, 常文田, 曹建, 王波, 秦再白, 苏玉民 申请人:哈尔滨工程大学