CN104197927A - 水下结构检测机器人实时导航***及方法 - Google Patents

Abstract

本发明公开了一种水下结构检测机器人实时导航***及方法。所述导航***包括磁罗盘、陀螺仪、加速度计、深度计、导航微处理器，所述磁罗盘、陀螺仪、加速度计和深度计分别采集磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器，所述导航微处理器根据采集数据解算水下机器人的姿态和位置。所述导航方法包括姿态算法、速度算法和深度算法，所述姿态算法结合互补滤波、四元数梯度下降法和卡尔曼算法获取姿态矩阵和姿态角，所述速度算法采用带有旋转补偿的三阶迎风格式求解机器人速度和位置，所述深度算法运用滑动平均滤波处理深度计数据，获得下潜深度。本发明降低了导航成本并获得较好的导航精度。

Description

水下结构检测机器人实时导航***及方法

技术领域

本发明涉及一种水下机器人实时导航技术，尤其涉及一种应用于海洋工程的水下结构检测的机器人导航***及方法。

背景技术

远程遥控水下机器人(Remotely Operated Vehicle，ROV)是一种通过脐带电缆与水面支持平台联系的水下作业机器人，由水面控制***和水下潜航体两部分组成，被广泛应用于水下观察、海底勘探、水下结构检修、海底管线检测以及水下安装等作业，已经成为海洋工程水下结构安全检测及维护的关键装备。

精确导航是水下结构检测机器人的重要内容，为水下结构物检测提供必要的技术支撑。现有的水下导航技术，例如水下声学导航、惯性导航、利用声波影像、光学等的视觉导航方法，都可直接应用在潜水器上，但都有各自的缺点。组合导航是将多导航***组合构成的性能更为完善的导航***，可以取长补短，提高导航精度，是未来导航技术的发展方向。目前，水下导航大多以惯性导航为主，辅以其他导航设备如多普勒速度记录仪(DVL)、磁航向仪(MCP)和GPS等，但是这些设备或者体积庞大，或者水下导航性能欠佳，或者成本高昂，具有明显的缺陷。

采用基于微机电***(MEMS)的导航设备，组合惯性组件如磁罗盘、深度计等器件，改善现有的导航算法，可以最大程度上减小积累误差，抑制姿态角误差，取得良好的满足水下检测机器人需求的导航效果。申请号为“2013100128123”的专利文献公开了一种“水下机器人的线地形匹配导航方法”，但可靠性较低，导航延迟大；申请号为“2012103320229”的专利文献公开了一种“自主式水下机器人组合导航***及方法”，其导航***结构复杂，成本高昂，且GPS不适用于水下导航。

发明内容

本发明的目的是提供一种水下结构检测机器人实时导航***及方法，为水下结构物检测ROV设计出实用的导航***，提高精度的同时降低成本。

本发明的目的通过以下技术方案予以实现：

一种水下结构检测机器人导航***，包括磁罗盘1、陀螺仪2、加速度计3、深度计4、导航微处理器5，所述磁罗盘1、陀螺仪2、加速度计3和深度计4分别采集水下机器人磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器5；所述导航微处理器5对磁罗盘1、陀螺仪2、加速度计3、深度计4采集的数据进行A/D转换；所述导航微处理器5结合四元数梯度下降法、互补滤波、卡尔曼算法对磁罗盘1、陀螺仪2、加速度计3采集的数据进行数据融合，获取姿态矩阵和姿态角；所述导航微处理器5对陀螺仪2、加速度计3采集的数据采用带有旋转补偿的三阶迎风算法求解速度和位置；所述导航微处理器5对深度计采用滑动平均滤波求取水下机器人下潜深度，得到组合导航信息。

一种水下结构检测机器人导航方法，以四元数和角误差为滤波估计对象，首先进行互补滤波，对陀螺仪测量值作初步补偿；其次运用梯度下降法更新四元数，有效降低姿态角漂移；再进行卡尔曼滤波，采用矩阵中的UD分解方法，避免滤波器发散，得到水下机器人姿态矩阵和姿态角；然后对加速度计和陀螺仪数据运用三阶迎风算法求解速度和位置。同时，对深度计采集数据进行滑动平均滤波，得到水下机器人深度值，水下机器人的垂向位置以此为准。该方法包括以下步骤：

1)姿态、速度、位置解算，过程如下：

(1)水下机器人导航***采用的离散卡尔曼方程为：

X_k＝Φ_k，k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

式中，k表示***离散采样时间点，k≥0；X_k为k时刻的***估计状态量；Φ_k,k-1为X_k-1到X_k的一步转移矩阵；W_k-1为***激励高斯白噪声序列，均值为E[w(k)]＝0，方差为E[w(k)w(k)^T]＝Q_k，Q_k即***噪声方差；Z_k为k时刻的传感器量测值；H_k为量测矩阵，反应量测量和估计量之间的数学关系；V_k为量测值高斯白噪声序列，均值为E[v(k)]＝0，方差为E[v(k)v(k)^T]＝R_k，R_k即测量噪声方差；W_k、V_k互不相关；

(2)确定参数初始状态，包括：

当地重力加速度g，***采样时间t，角速度误差Δω₁、Δω₂、Δω₃，互补滤波参数k_p、k_i，被估计状态量初值估计均方误差矩阵初值P_0|0；其中k_p、k_i取值规则为：k_p+k_i＝1，0.95＜k_p＜1，通过多次试验确定最优值；P_0|0按如下取值：设定导航坐标系为东北天坐标系，记为n系；设定机体坐标系为b系，初始时刻与导航坐标系重合，n系和b系均为三轴正交坐标系；四元数表示坐标旋转关系Q＝[q₀，q₁，q₂，q₃]，其中q₀为旋转标量，q₁、q₂、q₃为坐标旋转矢量，用四元数表述n系到b系转换关系表示为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

而b系到n系为r_ij(1≤i，j≤3)表示矩阵中各元素值；

(3)利用陀螺仪和磁罗盘、加速度计的频域互补特性校正角速度矢量，过程如下：

设磁罗盘测量值为m_b，加速度计测量值为a，陀螺仪测量值为ω，m_b、a、ω均为3×1列矩阵；

首先把磁场量由机体坐标系转换到导航坐标系 $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ 并计算水平和垂直方向磁场：by＝0、bz＝m_nz；

然后把磁场转换到机体坐标系： $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T,$ 并利用第三列估计重力加速度矢量：

再计算加速度和磁场补偿误差：

式中，“×”表示向量乘法；

得到累积误差为：errInt＝Ki∑err*t；

最后得到校正之后的角速度矢量：ω_c＝ω+k_p*err+errInt

其中，K_i、K_p是互补滤波参数；

(4)利用梯度下降法更新四元数：

由得到新四元数q₀、q₁、q₂、q₃，其中，μ是梯度下降的步长值，由确定； $\▿f=\frac{\∂f}{\∂Q},f(Q,d,s)=Q*\⊗d\⊗Q-s,$ d为传感器基于机体坐标系的测量值，s为传感器基于导航坐标系的实际值，“*”表示伴随矩阵，“”为梯度乘法，“ο”为四元数乘法；

(5)基于UD分解的卡尔曼滤波过程依次为：

状态一步预测：

UD分解：H_kP_k/k-1H_k ^T+R＝U∧D；

一步预测均方误差：P_k/k-1＝Φ_k，k-1P_k-1/k-1Φ_k/k-1 ^T+Q_k-1；

计算滤波增益：K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R)^-1＝P_k/k-1H_k ^T(UDU^T)^-1；

计算新息(新息由最新量测值计算获得)：

状态估值计算： ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\γ}}_k;$

估计均方误差：P_k/k＝(I-K_kH_k)P_k/k-1；

(6)运用三阶迎风格式求解速度和位置，并进行旋转补偿，按如下过程计算：

设加速度计在k时刻、k-1时刻、k-2时刻的测量值分别为a_k、a_k-1、a_k-2，陀螺仪在k时刻、k-1时刻、k-2时刻的测量值分别为ω_k、ω_k-1、ω_k-2，令T＝2t，则T_m、T_m-1、T_m-2时刻的速度分别设为v_m、v_m-1、v_m-2，T_m、T_m-1时刻的位置分别为P_m、P_m-1；

首先计算两个采样时刻的速度增量和角增量：

速度增量为 $\mathrm{\Δv}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2});$

角增量为 $\mathrm{\Δ\θ}=T(\frac{1}{6}{\mathrm{\ω}}_k+\frac{4}{6}{\mathrm{\ω}}_{k-1}+\frac{1}{6}{\mathrm{\ω}}_{k-2});$

然后采用三阶迎风格式求解速度和位置：

速度为 $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\Δv}\×\mathrm{\Δ\θ})$

位置为 $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

式中，即为旋转补偿部分，“×”表示向量乘法；

2)深度计解算

对深度计采用等权端点平滑作滑动平均滤波，计算水下机器人下潜深度：

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{d}_i$

不断逐个滑动地去取m个相邻数据作算数平均计算，式中，d_i是深度计采集的第i个数据，depth为滤波后的深度值。

本发明的目的还可以通过以下技术措施进一步予以实现：

前述水下结构检测机器人导航***，其中磁罗盘1采用Honeywell公司的电子罗盘IC HMC5883L。

前述水下结构检测机器人导航***，其中陀螺仪2、加速度计3是InvenSense公司生产的微机电(MEMS)产品。

前述水下结构检测机器人导航***，其中深度计4是扩散硅型压力变送器，在100m量程范围内具有良好的精度。

前述水下结构检测机器人导航***，其中导航微处理器5采用基于ARMCortex^TM-M3内核的STM32F103T8。

与现有技术相比，本发明的有益效果是：与现有的导航***及算法相比，采用基于梯度下降法的四元数更新算法，有效降低姿态漂移；对扩展卡尔曼滤波算法，采用矩阵中的UD分解方法，有效抑制滤波器发散。

附图说明

图1为本发明导航***结构图；

图2为本发明导航方法解算流程图；

图3为本发明四元数梯度下降法原理图；

图4为本发明卡尔曼滤波原理图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

本发明的基本思想是，基于微机电设备，利用四元数梯度下降法、互补滤波、卡尔曼滤波融合导航信息获得水下机器人的姿态信息，运用三阶迎风格式求解水下机器人的速度和位置，采用滑动平均滤波计算水下机器人下潜深度，可以在一定程度上降低成本、提高精度。

如图1所示，一种水下结构检测机器人导航***，由磁罗盘1、陀螺仪2、加速度计3、深度计4、导航微处理器5组成，所述磁罗盘1、陀螺仪2、加速度计3和深度计4分别采集水下机器人磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器5，其中磁罗盘1、陀螺仪2、加速度计3是三轴传感器，深度计4是基于水压测量深度的器件；所述导航微处理器5对磁罗盘1、陀螺仪2、加速度计3、深度计4采集数据进行16位A/D转换，进行数据融合、滤波，解算出水下机器人的姿态角、速度和位置，并传输至图1中的水下微控制器6。其中磁罗盘1采用的是Honeywell电子罗盘IC HMC5883L，陀螺仪2、加速度计3是InvenSense公司生产的微机电(MEMS)产品，深度计4是扩散硅型压力变送器，在100m量程范围内具有良好的精度，导航微处理器5采用基于ARM Cortex^TM-M3内核的STM32F103T8。

如图2所示，一种水下结构检测机器人导航方法，对磁罗盘、陀螺仪、加速度计、深度计采集数据进行数据融合。首先对卡尔曼初始化，对四元数初始化，对采样时间、重力加速度初始化。然后进行互补滤波，对陀螺仪测量值角加速度作初步补偿；运用梯度下降法更新四元数，有效降低姿态角漂移；进行卡尔曼滤波，采用矩阵中的UD分解方法，抑制滤波器的发散，得到水下机器人姿态和位置；对加速度计和陀螺仪数据运用三阶迎风格式求解速度和位置。同时，对深度计采集数据进行滑动平均滤波，得到水下机器人深度值，水下机器人的垂向位置以此为准。如图2所示，结合具体实施例，实现步骤如下：

1.参数初始化：当地重力加速度g＝9.79973，采样时间t＝0.02s，四元数初值q₀＝1，q₁＝0，q₂＝0，q₃＝0，角速度误差Δω₁＝0，Δω₂＝0，Δω₃＝0；根据互补滤波原理，确定滤波参数k_p＝0.0132，k_i＝0.9868；***噪声方差Q_k和测量噪声方差R_k为：

Q＝diag([0.0001 0.0001 0.0001 0.0001 0.005 0.005 0.005])

R＝diag([0.005 0.005 0.005 0.003 0.003 0.003])

估计均方误差矩阵初值：

P_0/0＝diag([0.001 0.001 0.001 0.001 0.005 0.005 0.005])

被估计矢量初值：

设定导航坐标系为东北天坐标系，记为n系；设定机体坐标系为b系，初始与导航坐标系重合，四元数表述n系到b系转换关系表示为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

$C_b^n={\left(C_n^b\right)}^T$

式中，表示从导航坐标系到机体坐标系的转换，表示从机体坐标系到导航坐标系的转换。

2.利用陀螺仪和加速度计、磁罗盘频域互补的思想，对角加速度补偿：

设磁罗盘测量值为m_b，加速度计测量值为a，陀螺仪测量值为ω，m_b、a、ω均为3×1列矩阵。

1)把磁场量由机体坐标系转换到导航坐标系 $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ 然后

计算水平和垂直方向磁场：by＝0；bz＝m_nz；

2)把磁场转换到机体坐标系： $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T;$

3)利用第三列估计重力加速度矢量：

4)计算加速度和磁场补偿误差：由此得到累积误差为：errInt＝Ki*∑err*t；

5)最后得到校正之后的角速度矢量：ω_c＝ω+k_p*err+errInt。

3.如图3所示，采用梯度下降法更新四元数：

1)定义相关变量

$\stackrel{\→}{G}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$ ：重力加速度，导航坐标系内表示；

$\stackrel{\→}{H}={\left[\begin{array}{ccc}0& h_2& h_3\end{array}\right]}^T$ ：地磁场强度常量，导航坐标系内表示；

加速度测量值，机体坐标系内表示；

磁场强度测量值，机体坐标系内表示；

2)误差函数

根据式 $f(Q,d,s)=Q*\⊗d\⊗Q-s,$ 可得

$\mathrm{\Δ}\stackrel{\→}{a}=C_n^b*\stackrel{\→}{G}-\stackrel{\→}{a}=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g-a_x\\ 2(q_2{q}_3+q_0{q}_1)g-a_y\\ (q_0^2-q_1^2-q_2^2+q_3^2)g-a_z\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}{a}_x\\ \mathrm{\Δ}{a}_y\\ \mathrm{\Δ}{a}_z\end{array}\right]$

$\mathrm{\Δ}\stackrel{\→}{h}=C_n^b*\stackrel{\→}{h}=\left[\begin{array}{c}2(q_1{q}_2+q_0{q}_3)h_2+2(q_1{q}_3-q_0{q}_2)h_3-h_x\\ (q_0^2-q_1^2+q_2^2-q_3^2)h_2+2(q_2{q}_3+q_0{q}_1)h_3-h_y\\ 2(q_2{q}_3-q_0{q}_1)h_2+(q_0^2-q_1^2-q_1^2+q_3^2)h_3-h_z\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}{h}_x\\ \mathrm{\Δ}{h}_y\\ \mathrm{\Δ}{h}_z\end{array}\right]$

3)目标误差函数

将上述两个误差函数合成一个，这里用平方和取标量值，得到

f(a，h)＝|a|²+|h|²，此时函数值大于等于0。

4)梯度

$\▿f(a,h)=\frac{\∂f(a,h)}{\∂Q}={\left[\begin{array}{cccc}\frac{\∂f}{\∂q_0}& \frac{\∂f}{\∂q_1}& \frac{\∂f}{\∂q_2}& \frac{\∂f}{\∂q_3}\end{array}\right]}^T,\frac{\∂f}{\∂q_0},\frac{\∂f}{\∂q_1},\frac{\∂f}{\∂q_2},\frac{\∂f}{\∂q_3}$ 依次为

$\left\{\begin{array}{c}-4\mathrm{\Δ}{a}_x{q}_2+4\mathrm{\Δ}{a}_y{q}_1+4\mathrm{\Δ}{h}_x(h_2{q}_3-h_3{q}_2)+4\mathrm{\Δ}{h}_y{h}_3{q}_1-\mathrm{\Δ}{h}_z{h}_2{q}_1\\ 4\mathrm{\Δ}{a}_x{q}_3+4\mathrm{\Δ}{a}_y{q}_0-4\mathrm{\Δ}{a}_z{q}_1+4\mathrm{\Δ}{h}_x(h_2{q}_2+h_3{q}_3)+4\mathrm{\Δ}{h}_y(h_3{q}_0-h_2{q}_1)-4\mathrm{\Δ}{h}_z(h_2{q}_0+h_{{3q}_1})\\ -4\mathrm{\Δ}{a}_x{q}_0+4\mathrm{\Δ}{a}_y{q}_3-4\mathrm{\Δ}{a}_z{q}_2+4\mathrm{\Δ}{h}_x(h_2{q}_1-h_3{q}_0)+4\mathrm{\Δ}{h}_y{h}_3{q}_3+4\mathrm{\Δ}{h}_z(h_2{q}_3-h_3{q}_2)\\ 4\mathrm{\Δ}{a}_x{q}_1+4\mathrm{\Δ}{a}_y{q}_2+4\mathrm{\Δ}{h}_x(h_2{q}_0+h_3{q}_1)+4\mathrm{\Δ}{h}_y(h_3{q}_2-h_2{q}_3)+4\mathrm{\Δ}{h}_z{h}_2{q}_2\end{array}\right.$

5)四元数更新

步长μ设置为梯度长度的0.041倍，更新方程为

6)最后对四元数归一化，得到新的四元数：

4.基于UD分解的扩展卡尔曼滤波，如图4所示，滤波过程依次为：

状态一步预测：

UD分解：H_kP_k/k-1H_k ^T+R＝U∧D；

一步预测均方误差：P_k/k-1＝Φ_k，k-1P_k-1/k-1Φ_k/k-1 ^T+Q_k-1；

计算滤波增益：K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R)^-1＝P_k/k-1H_k ^T(UDU^T)^-1；

计算新息： ${\mathrm{\γ}}_k=Z_k-H_k{\hat{X}}_{k/k-1};$

状态估值计算： ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\γ}}_k;$

估计均方误差：P_k/k＝(I-K_kH_k)P_k/k-1。

反复循环更新。

式中，

$H_k=\left[\begin{array}{ccccccc}-q_{2}g& q_{3}g& -q_{0}g& q_{1}g& 0& 0& 0\\ q_{1}g& q_{0}g& q_{3}g& q_{2}g& 0& 0& 0\\ q_{0}g& -q_{1}g& {-q}_{2}g& q_{3}g& 0& 0& 0\\ -\mathrm{bx}{q}_0-\mathrm{bz}{q}_2& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& -\mathrm{bx}{q}_2-{\mathrm{bzq}}_0& -\mathrm{bx}{q}_3+\mathrm{bz}{q}_1& 0& 0& 0\\ -\mathrm{bx}{q}_3+\mathrm{bz}{q}_1& \mathrm{bx}{q}_2+\mathrm{bz}{q}_0& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& -\mathrm{bx}{q}_0-\mathrm{bz}{q}_2& 0& 0& 0\\ \mathrm{bx}{q}_2+\mathrm{bz}{q}_0& \mathrm{bx}{q}_3-\mathrm{bz}{q}_1& \mathrm{bx}{q}_0-\mathrm{bz}{q}_2& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& 0& 0& 0\end{array}\right]$

5.每完成一次数据融合，计算一次姿态矩阵：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

由上式可计算出水下机器人姿态角；

ψ＝arctan(r₁₂/r₂₂)

θ＝arcsinr₃₂

γ＝arctan(-r₃₁/r₃₃)

6.运用三阶迎风格式求解速度和位置，并进行旋转补偿，按如下过程计算：

设加速度计在k时刻、k-1时刻、k-2时刻的测量值分别为a_k、a_k-1、a_k-2，陀螺仪在k时刻、k-1时刻、k-2时刻的测量值分别为ω_k、ω_k-1、ω_k-2，令T＝2t，则T_m、T_m-1、T_m-2时刻的速度分别设为v_m、v_m-1、v_m-2，T_m、T_m-1时刻的位置分别为P_m、P_m-1。

首先计算两个采样时刻的速度增量和角增量：

速度增量为 $\mathrm{\Δv}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2})$

角增量为 $\mathrm{\Δ\θ}=T(\frac{1}{6}{\mathrm{\ω}}_k+\frac{4}{6}{\mathrm{\ω}}_{k-1}+\frac{1}{6}{\mathrm{\ω}}_{k-2})$

然后采用三阶迎风格式求解速度和位置：

速度为 $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\Δv}\×\mathrm{\Δ\θ})$

位置为 $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

式中，即为旋转补偿部分，“×”表示向量乘法。

7.对深度计采用等权端点平滑作滑动平均滤波，计算下潜深度：

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{d}_i$

不断逐个滑动地去取m个相邻数据作算数平均计算。式中，d_i是深度计采集的第i个数据，depth为滤波后的深度值。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (6)

1.一种水下结构检测机器人导航***，包括磁罗盘(1)、陀螺仪(2)、加速度计(3)、深度计(4)、导航微处理器(5)，其特征在于，所述磁罗盘(1)、陀螺仪(2)、加速度计(3)和深度计(4)分别采集水下机器人磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器(5)；所述导航微处理器(5)对磁罗盘(1)、陀螺仪(2)、加速度计(3)、深度计(4)采集的数据进行A/D转换；所述导航微处理器(5)结合四元数梯度下降法、互补滤波、卡尔曼算法对磁罗盘(1)、陀螺仪(2)、加速度计(3)采集的数据进行数据融合，获取姿态矩阵和姿态角；所述导航微处理器(5)对陀螺仪(2)、加速度计(3)采集的数据采用带有旋转补偿的三阶迎风算法求解速度和位置；所述导航微处理器(5)对深度计采用滑动平均滤波求取水下机器人下潜深度，得到组合导航信息。

2.如权利要求1所述水下结构检测机器人导航***，其特征在于，所述磁罗盘(1)采用Honeywell公司的电子罗盘IC HMC5883L。

3.如权利要求1所述水下结构检测机器人导航***，其特征在于，所述陀螺仪(2)、加速度计(3)是InvenSense公司生产的微机电产品。

4.如权利要求1所述水下结构检测机器人导航***，其特征在于，所述深度计(4)是扩散硅型压力变送器，在100m量程范围内具有良好的精度。

5.如权利要求1所述水下结构检测机器人导航***，其特征在于，所述导航微处理器(5)采用基于ARM Cortex^TM-M3内核的STM32F103T8。

6.如权利要求1所述水下结构检测机器人导航***的导航方法，其特征在于，以四元数和角误差为滤波估计对象，首先进行互补滤波，对陀螺仪测量值作初步补偿；其次运用梯度下降法更新四元数，有效降低姿态角漂移；再进行卡尔曼滤波，采用矩阵中的UD分解方法，避免滤波器发散，得到水下机器人姿态矩阵和姿态角；然后对加速度计和陀螺仪数据运用三阶迎风算法求解速度和位置。同时，对深度计采集数据进行滑动平均滤波，得到水下机器人深度值，水下机器人的垂向位置以此为准。该方法包括以下步骤：

1)姿态、速度、位置解算，过程如下：

(1)水下机器人导航***采用的离散卡尔曼方程为：

X_k＝Φ_k，k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

式中，k表示***离散采样时间点，k≥0；X_k为k时刻的***估计状态量；Φ_k，k-1为X_k-1到X_k的一步转移矩阵；W_k-1为***激励高斯白噪声序列，均值为E[w(k)]＝0，方差为E[w(k)w(k)^T]＝Q_k，Q_k即***噪声方差；Z_k为k时刻的传感器量测值；H_k为量测矩阵，反应量测量和估计量之间的数学关系；V_k为量测值高斯白噪声序列，均值为E[v(k)]＝0，方差为E[v(k)v(k)^T]＝R_k，R_k即测量噪声方差；W_k、V_k互不相关；

(2)确定参数初始状态，包括：

当地重力加速度g，***采样时间t，角速度误差Δω₁、Δω₂、Δω₃，互补滤波参数k_p、k_i，被估计状态量初值估计均方误差矩阵初值P_0|0；其中k_p、k_i取值规则为：k_p+k_i＝1，0.95＜k_p＜1，通过多次试验确定最优值；P_0|0按如下取值：设定导航坐标系为东北天坐标系，记为n系；设定机体坐标系为b系，初始时刻与导航坐标系重合，n系和b系均为三轴正交坐标系；四元数表示坐标旋转关系Q＝[q₀，q₁，q₂，q₃]，其中q₀为旋转标量，q₁、q₂、q₃为坐标旋转矢量，用四元数表述n系到b系转换关系表示为：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

而b系到n系为r_ij(1≤i，j≤3)表示矩阵中各元素值；

(3)利用陀螺仪和磁罗盘、加速度计的频域互补特性校正角速度矢量，过程如下：

设磁罗盘测量值为m_b，加速度计测量值为a，陀螺仪测量值为ω，m_b、a、ω均为3×1列矩阵；

首先把磁场量由机体坐标系转换到导航坐标系 $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ 并计算水平和垂直方向磁场：by＝0、bz＝m_nz；

然后把磁场转换到机体坐标系： $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T,$ 并利用第三列估计重力加速度矢量：

再计算加速度和磁场补偿误差：

式中，“×”表示向量乘法；

得到累积误差为：errInt＝Ki∑err*t；

最后得到校正之后的角速度矢量：ω_c＝ω+k_p*err+errInt

其中，K_i、K_p是互补滤波参数；

(4)利用梯度下降法更新四元数：

由得到新四元数q₀、q₁、q₂、q₃，其中，μ是梯度下降的步长值，由确定； $\▿f=\frac{\∂f}{\∂Q},f(Q,d,s)=Q*\⊗d\⊗Q-s,$ d为传感器基于机体坐标系的测量值，s为传感器基于导航坐标系的实际值，“*”表示伴随矩阵，“”为梯度乘法，“ο”为四元数乘法；

(5)基于UD分解的卡尔曼滤波过程依次为：

状态一步预测：

UD分解：H_kP_k/k-1H_k ^T+R＝U∧D；

一步预测均方误差：P_k/k-1＝Φ_k，k-1P_k-1/k-1Φ_k/k-1 ^T+Q_k-1；

计算滤波增益：K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R)^-1＝P_k/k-1H_k ^T(UDU^T)^-1；

计算新息(新息由最新量测值计算获得)：

状态估值计算： ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\γ}}_k;$

估计均方误差：P_k/k＝(I-K_kH_k)P_k/k-1；

(6)运用三阶迎风格式求解速度和位置，并进行旋转补偿，按如下过程计算：

设加速度计在k时刻、k-1时刻、k-2时刻的测量值分别为a_k、a_k-1、a_k-2，陀螺仪在k时刻、k-1时刻、k-2时刻的测量值分别为ω_k、ω_k-1、ω_k-2，令T＝2t，则T_m、T_m-1、T_m-2时刻的速度分别设为v_m、v_m-1、v_m-2，T_m、T_m-1时刻的位置分别为P_m、P_m-1；

首先计算两个采样时刻的速度增量和角增量：

速度增量为 $\mathrm{\Δv}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2});$

角增量为 $\mathrm{\Δ\θ}=T(\frac{1}{6}{\mathrm{\ω}}_k+\frac{4}{6}{\mathrm{\ω}}_{k-1}+\frac{1}{6}{\mathrm{\ω}}_{k-2});$

然后采用三阶迎风格式求解速度和位置：

速度为 $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\Δv}\×\mathrm{\Δ\θ})$

位置为 $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

式中，即为旋转补偿部分，“×”表示向量乘法；

2)深度计解算

对深度计采用等权端点平滑作滑动平均滤波，计算水下机器人下潜深度：

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{d}_i$

不断逐个滑动地去取m个相邻数据作算数平均计算，式中，d_i是深度计采集的第i个数据，depth为滤波后的深度值。