CN102052923A - 一种小型水下机器人组合导航系统及导航方法 - Google Patents

Abstract

本发明提供的是一种小型水下机器人组合导航系统及导航方法。该组合导航系统包括嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计、水声通讯设备。当水下机器人载体在水下时，组合导航系统上电后，导航系统自主运行，通过AD板IO通道控制继电器打开传感器，进行数据采集，获得导航初始位置，根据磁偏角数据库计算得出磁偏角，接收规划导航位置校正指令信息，进行导航位置推算，从而获得水下机器人的经纬度位置信息。该系统体积小、重量轻、成本低，利于实现水下导航系统小型化，可应用于要求体积重量小的水下观察、探测及小型水下机器人。

Description

一种小型水下机器人组合导航系统及导航方法

技术领域

本发明涉及的是一种小型水下组合导航系统，本发明也涉及一种小型水下组合导航方法。

背景技术

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

申请号为200520020572.2，名称为“小型水下自主组合导航装置”的专利文件中，提供了一种小型水下自主组合导航装置。申请号为201010114558.4，名称为“一种小型水下航行器用组合导航系统及方法”的专利文件中，提供了一种由捷联惯性导航系统、多普勒导航系统、地形辅助导航系统和磁罗经组成的水下组合导航导航系统和方法。

发明内容

本发明的目的在于提供一种体积小、重量轻、成本低，利于实现水下导航系统小型化的小型水下机器人组合导航系统。本发明的目的还在于提供一种基于小型水下机器人组合导航系统的导航方法。

本发明的小型水下机器人组合导航系统包括嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计及继电器元件，微型姿态传感器安装在水下机器人本体耐压舱内，微型姿态传感器的信号传入嵌入式导航处理机，速度计、深度计安装在水下机器人本体下部，水声通讯声纳安装在水下机器人本体首部上方，速度计、深度计、水声通讯声纳信号通过水密线缆与布置在水下机器人本体耐压舱内的嵌入式导航处理机相连；嵌入式导航处理机包括CPU核心模块、AD板、4串口卡、电源板，电源板由24V电池供电并通过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)计算系统干扰均值

系统干扰方差矩阵量测噪声均值量测噪声方差矩阵

系统干扰均值：

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\Φ}(k,k-1)\hat{X}(k-1/k-1)]---\left(1\right)$

系统干扰方差矩阵：

$\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)K^T\left(k\right)+P(k/k)----\left(2\right)$

$\mathrm{\Φ}(k,k-1)P(k-1/k-1){\mathrm{\Φ}}^T(k/k-1)]$

量测噪声均值；

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k/k-1)]---\left(3\right)$

量测噪声方差矩阵：

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]---\left(4\right)$

其中

分别是状态X(k)、系统状态噪声序列的方差阵Q(k)、量测噪声序列方差阵R(k)的估计，

为新息矩阵，d_k＝1-b/(1-b^k+1)，b是遗忘因子，

(3)利用野值判据进行野值判别

$\left|\mathrm{\ϵ}\left(k\right)\right|\≥\mathrm{\α}\sqrt{H\left(k\right)\hat{P}(k,k-1)H{\left(k\right)}^T+R\left(k\right)}---\left(5\right)$

其中，α为常数，若(5)式判据成立则本次测量值为干扰数据即野值，转到下一步对新息进行修正；若(5)式判据不成立，则本次测量值为正常数据，不需要对新息进行修正；

(4)对新息进行修正

如果k时刻经野值判据(5)判断为野值，则采用下式对新息进行修正：

ε(k)＝ε(k-1)β₁+ε(k-2)β₂+…+ε(1)β_k-1

其中：{β_i}＝d_k-1，d_k-1b，d_k-1b²，…d_k-1b^k-1为前k-1次新息加权系数序列。

2、所述导航位置计算的方法为：

(1)通过速度计接收传机器人艇体速度、高度数据信息，姿态传感器获得机器人艏向、横摇、纵摇姿态信息；

(2)采用新息修正的抗野值自适应卡尔曼滤波方法对传感器数据信息进行滤波处理；

(3)将艇体速度进行坐标转换为大地坐标系下速度信息

将水下机器人艇体坐标系下的速度通过转换矩阵(6)式计算求得水下机器人大地坐标系下的速度，

[v_n v_e v_u]^T＝T[v_x v_y v_z]^T             (6)

其中[v_n v_e v_u]^T为大地坐标系下水下机器人北向、东向、垂向速度，[v_x v_y v_z]^T分艇体坐标下的水下机器人纵向、横向、垂向速度，ψ，θ，

分别为水下机器人的横摇、纵摇以及艏向姿态角；

(4)根据速度信息前一时刻位置信息进位置推算

若不进行校正以前一时刻位置信息作为为位置推算初始点，若水面校正将GPS信息作为位置推算初始点，若在水下通过水声通讯校正则将根据母船位置及与水下机器人相对位置计算求得的机器人位置作为位置推算的初始点，对速度进行积分考虑地球椭球模型获得水下机器人位置。

本发明的目的在于提供一种小型水下机器人用的组合导航系统。本发明的目的还在于提供一种基于强跟踪无迹卡尔曼滤波方法和航位推算的导航方法。

本发明的的小型水下机器人组合导航系统的组成包括嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计及继电器元件。嵌入式导航处理机包括CPU核心模块、AD板、4串口卡、电源板；电源板由24V电池供电并通过PC/104总线给其他板卡供电；所有板卡数据信息通过PC/104总线传输。微型姿态传感器安装在水下机器人本体耐压舱内，其信号传与嵌入式导航处理机，速度计、深度计安装在水下机器人本体下部，声通讯声纳安装在水下机器人本体首部上方，耐压GPS、速度计、深度计、水声通讯声纳信号通过水密线缆与布置在水下机器人本体耐压舱内的嵌入式导航处理机相连，其中微型姿态传感器、耐压GPS、速度计、水声通讯声纳信号线与嵌入式导航处理机的串口板相连，深度计信号线与嵌入式导航处理机的AD相连，速度计、水声通讯声纳、耐压GPS的传感器电源通过AD板IO通道控制相应继电器。

本发明的导航方法主要包括：

1.给嵌入式导航处理机电源板供电，接通24V稳压电源，通过电源板给所有板卡上电。

2.嵌入式导航处理机导航程序启动，通过AD板IO通道控制继电器打开相应传感器。

3.通过相应传感器获得水下机器人本体姿态、深度信息、高度信息和运动速度信息，水下机器人若在水面获取耐压GPS信息。

4.采用新息修正的抗野值自适应卡尔曼滤波方法对获得的传感器信息进行数据处理。

5.跟据磁偏角数据库计算得出磁偏角。

6.接收规划导航位置校正指令信息，依据机器人本体姿态、深度和速度信息进行导航位置计算。

本发明与申请号为200520020572.2的专利文件中的技术方案相比有以下几个方面存在显著不同。

1.申请号为200520020572.2的专利文件中的技术方案中采用Ocant1000光纤罗经作为姿态传感器、WHN600多普勒速度仪作为速度计，虽然光纤罗经精度较高，但光纤罗经、WHN600多普勒速度仪体积大功耗大、成本高。本发明中姿态传感器、速度计均为小型化、功耗小、低成本传感器，结合数据滤波处理剔出数据中野点信息。

2.申请号为200520020572.2的专利文件中的技术方案中没有配备深度计传感器，而本发明中的技术方案中配有深度计，结合导航方法可以获得水下机器人三维导航信息。

3.申请号为200520020572.2的专利文件中的技术方案中没有配备传感器IO开关，不适于自主水下机器人上应用，而本发明中通过AD板IO开关可以自主控制传感器开关，安全性上更可靠。

4.申请号为200520020572.2的专利文件中的技术方案中只能通过水下机器人上浮通过接收GPS信息进行位置重调校正，而本发明还可以不用水下机器人上浮通过水声通讯进行位置重调校正。

5.申请号为200520020572.2的专利文件中的技术方案中没有对传感器数据处理和导航方法加以描述，而本发明对导航信息流程和采用的传感器数据处理方法都加以详尽描述。

附图说明

图1：小型水下机器人组合导航系统硬件结构；

图2：小型水下机器人组合导航系统软件体系结构；

图3：组合导航系统信息流程；

图4：航位推算流程；

图5：新息修正的抗野值自适应卡尔曼滤波方法流程。

具体实施方式

下面结合附图举例对本发明作更详细地描述：

小型水下机器人导航系统的组成包括嵌入式导航处理机、耐压GPS、微型姿态传感器、速度计、深度计及继电器元件，其结构如图1所示。在图1中：1为CPU核心模块，2为AD板，3为4串口卡，5为继电器，6为GPS、7为小型速度计，8为水声通讯声纳，9为小型姿态传感器，10为深度计。

嵌入式导航处理机包括CPU核心模块、AD板、4串口卡、电源板；电源板由24V电池供电并通过PC/104总线给其他板卡供电；所有板卡数据信息通过PC/104总线传输。

微型姿态传感器安装在水下机器人本体耐压舱内，速度计、深度计安装在水下机器人本体下部，声通讯声纳安装在水下机器人本体首部上方，耐压GPS、速度计、深度计、水声通讯声纳信号通过水密线缆与布置在水下机器人本体耐压舱内的嵌入式导航处理机相连。

为节约成本及空间，利用AD板IO通道控制速度计、水声通讯声纳、耐压GPS的传感器电源相应继电器的开关，微型姿态传感器和深度计不设电源开关，与电源板同时上电。

微型姿态传感器、GPS、速度计、水声通讯声纳与嵌入式导航处理机的4串口板相连以RS232串行协议进行数据通讯，深度计采用小型低成本模拟信号深度计，其信号线与嵌入式导航处理机的AD相连，需经AD数据转换。

利用本发明的小型水下机器人导航系统实现导航的方法为：

(1)板卡供电

给嵌入式导航处理机电源板供电，接通24V稳压电源，同时，PC104CPU核心模块、串口板、AD板获得稳压电源，微型姿态传感器和深度计也获得电源。

(2)传感器工作

嵌入式导航处理机供电后，导航程序自主运行，通过AD板IO通道控制相应继电器元件打开GPS、速度计、水声通讯声纳开关。

(3)数据信息获取

通过微型姿态传感器、深度计分别获得水下机器人本体姿态(艏摇、横摇、纵摇)和深度信息，通过深度计获得机器人本体速度和高度信息，水下机器人若在水面获取耐压GPS信息，若在水下通过水声通讯声纳获取位置和指令信息；

7.传感器数据滤波处理

对传感器数据进行基于新息修正的抗野值自适应卡尔曼滤波方法步骤为：

a)用标准的卡尔曼滤波方程计算出状态估计值；

b)计算系统干扰均值系统干扰方差矩阵量测噪声均值

量测噪声方差矩阵

系统干扰均值：

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\Φ}(k,k-1)\hat{X}(k-1/k-1)]$

系统干扰方差矩阵：

$\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)K^T\left(k\right)+P(k/k)-$

$\mathrm{\Φ}(k,k-1)P(k-1/k-1){\mathrm{\Φ}}^T(k/k-1)]$

量测噪声均值；

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k/k-1)]$

量测噪声方差矩阵：

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]$

其中分别是状态X(k)、系统状态噪声序列的方差阵Q(k)、量测噪声序列方差阵R(k)的估计，

为新息矩阵，d_k＝1-b/(1-b^k+1)，b是遗忘因子，

c)利用野值判据进行野值判别

$\left|\mathrm{\ϵ}\left(k\right)\right|\≥\mathrm{\α}\sqrt{H\left(k\right)\hat{P}(k,k-1)H{\left(k\right)}^T+R\left(k\right)}$

其中，α为常数。若上式判据成立则本次测量值为干扰数据即野值，转到下一步对新息进行修正；若上式判据不成立，则本次测量值为正常数据，不需要对新息进行修正。

d)对新息进行修正

如果k时刻经野值判据判断为野值，则采用下式对新息进行修正：

ε(k)＝ε(k-1)β₁+ε(k-2)β₂+…+ε(1)β_k-1

其中：{β_i}＝d_k-1，d_k-1b，d_k-1b²，…d_k-1b^k-1为前k-1次新息加权系数序列。

8.机器人航位推算

(a)速度转换

将水下机器人艇体坐标系下的速度通过转换矩阵计算求得水下机器人大地坐标系下的速度，坐标转换矩阵应为：

[v_n v_e v_u]^T＝T[v_x v_y v_z]^T

其中[v_n v_e v_u]^T为大地坐标系下水下机器人北向、东向、垂向速度，[v_x v_y v_z]^T分艇体坐标下的水下机器人纵向、横向、垂向速度，ψ，θ，

分别为水下机器人的横摇、纵摇以及艏向姿态角。

(b)根据速度信息前一时刻位置信息进位置推算

若不进行校正以前一时刻位置信息作为为位置推算初始点，若水面校正将GPS信息作为位置推算初始点，若在水下通过水声通讯校正则将根据母船位置及与水下机器人相对位置计算求得的机器人位置作为位置推算的初始点。对速度进行积分考虑地球椭球模型获得水下机器人位置。

Claims (8)

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)计算系统干扰均值

系统干扰方差矩阵

量测噪声均值

量测噪声方差矩阵

系统干扰均值：

$\hat{q}\left(k\right)=(1-d_{k-1})\hat{q}(k-1)+d_{k-1}[\hat{X}(k/k)-\mathrm{\Φ}(k,k-1)\hat{X}(k-1/k-1)]---\left(1\right)$

系统干扰方差矩阵：

$\hat{Q}\left(k\right)=(1-d_{k-1})\hat{Q}(k-1)+d_{k-1}[K\left(k\right)\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)K^T\left(k\right)+P(k/k)----\left(2\right)$

$\mathrm{\Φ}(k,k-1)P(k-1/k-1){\mathrm{\Φ}}^T(k/k-1)]$

量测噪声均值；

$\hat{r}\left(k\right)=(1-d_{k-1})\hat{r}(k-1)+d_{k-1}[Z\left(k\right)-H\left(k\right)\hat{X}(k/k-1)]---\left(3\right)$

量测噪声方差矩阵：

$\hat{R}\left(k\right)=(1-d_{k-1})\hat{R}(k-1)+d_{k-1}[\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)-H\left(k\right)P(k/k-1)H^T\left(k\right)]---\left(4\right)$

其中

分别是状态X(k)、系统状态噪声序列的方差阵Q(k)、量测噪声序列方差阵R(k)的估计，ε(k)＝Z(k)-r(k-1)-H(k)X(k|k-1)为新息矩阵，d_k＝1-b/(1-b^k+1)，b是遗忘因子，

(3)利用野值判据进行野值判别

$\left|\mathrm{\ϵ}\left(k\right)\right|\≥\mathrm{\α}\sqrt{H\left(k\right)\hat{P}(k,k-1)H{\left(k\right)}^T+R\left(k\right)}---\left(5\right)$

其中，α为常数，若(5)式判据成立则本次测量值为干扰数据即野值，转到下一步对新息进行修正；若(5)式判据不成立，则本次测量值为正常数据，不需要对新息进行修正；

(4)对新息进行修正

如果k时刻经野值判据(5)判断为野值，则采用下式对新息进行修正：

ε(k)＝ε(k-1)β₁+ε(k-2)β₂+…+ε(1)β_k-1

其中：{β_i}＝d_k-1，d_k-1b，d_k-1b²，…d_k-1b^k-1为前k-1次新息加权系数序列。

8.根据权利要求6或7所述的基于小型水下机器人组合导航系统的导航方法，其特征是所述导航位置计算的方法为：

(1)通过速度计接收传机器人艇体速度、高度数据信息，姿态传感器获得机器人艏向、

横摇、纵摇姿态信息；

(2)采用新息修正的抗野值自适应卡尔曼滤波方法对传感器数据信息进行滤波处理；

(3)将艇体速度进行坐标转换为大地坐标系下速度信息

将水下机器人艇体坐标系下的速度通过转换矩阵(6)式计算求得水下机器人大地坐标系下的速度，

[v_n v_e v_u]^T＝T[v_x v_y v_z]^T            (6)

其中[v_n v_e v_u]^T为大地坐标系下水下机器人北向、东向、垂向速度，[v_x v_y v_z]^T分艇体坐标下的水下机器人纵向、横向、垂向速度，ψ，θ，

分别为水下机器人的横摇、纵摇以及艏向姿态角；

(4)根据速度信息前一时刻位置信息进位置推算

若不进行校正以前一时刻位置信息作为为位置推算初始点，若水面校正将GPS信息作为位置推算初始点，若在水下通过水声通讯校正则将根据母船位置及与水下机器人相对位置计算求得的机器人位置作为位置推算的初始点，对速度进行积分考虑地球椭球模型获得水下机器人位置。

Images