水下结构检测机器人实时导航系统及方法
【专利摘要】本发明公开了一种水下结构检测机器人实时导航系统及方法。所述导航系统包括磁罗盘、陀螺仪、加速度计、深度计、导航微处理器，所述磁罗盘、陀螺仪、加速度计和深度计分别采集磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器，所述导航微处理器根据采集数据解算水下机器人的姿态和位置。所述导航方法包括姿态算法、速度算法和深度算法，所述姿态算法结合互补滤波、四元数梯度下降法和卡尔曼算法获取姿态矩阵和姿态角，所述速度算法采用带有旋转补偿的三阶迎风格式求解机器人速度和位置，所述深度算法运用滑动平均滤波处理深度计数据，获得下潜深度。本发明降低了导航成本并获得较好的导航精度。
【专利说明】水下结构检测机器人实时导航系统及方法
【技术领域】
[0001]本发明涉及一种水下机器人实时导航技术，尤其涉及一种应用于海洋工程的水下结构检测的机器人导航系统及方法。
【背景技术】
[0002]远程遥控水下机器人(Remotely Operated Vehicle,R0V)是一种通过脐带电缆与水面支持平台联系的水下作业机器人，由水面控制系统和水下潜航体两部分组成，被广泛应用于水下观察、海底勘探、水下结构检修、海底管线检测以及水下安装等作业，已经成为海洋工程水下结构安全检测及维护的关键装备。
[0003]精确导航是水下结构检测机器人的重要内容，为水下结构物检测提供必要的技术支撑。现有的水下导航技术，例如水下声学导航、惯性导航、利用声波影像、光学等的视觉导航方法，都可直接应用在潜水器上，但都有各自的缺点。组合导航是将多导航系统组合构成的性能更为完善的导航系统，可以取长补短，提高导航精度，是未来导航技术的发展方向。目前，水下导航大多以惯性导航为主，辅以其他导航设备如多普勒速度记录仪(DVL)、磁航向仪(MCP)和GPS等，但是这些设备或者体积庞大，或者水下导航性能欠佳，或者成本高昂，具有明显的缺陷。
[0004]采用基于微机电系统(MEMS)的导航设备，组合惯性组件如磁罗盘、深度计等器件，改善现有的导航算法，可以最大程度上减小积累误差，抑制姿态角误差，取得良好的满足水下检测机器人需求的导航效果。申请号为“2013100128123”的专利文献公开了一种“水下机器人的线地形匹配导航方法”，但可靠性较低，导航延迟大；申请号为“2012103320229”的专利文献公开了一种“自主式水下机器人组合导航系统及方法”，其导航系统结构复杂，成本高昂，且GPS不适用于水下导航。
【发明内容】
[0005]本发明的目的是提供一种水下结构检测机器人实时导航系统及方法，为水下结构物检测ROV设计出实用的导航系统，提高精度的同时降低成本。
[0006]本发明的目的通过以下技术方案予以实现:
[0007]一种水下结构检测机器人导航系统，包括磁罗盘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对深度计采用滑动平均滤波求取水下机器人下潜深度，得到组合导航信息。
[0008]一种水下结构检测机器人导航方法，以四元数和角误差为滤波估计对象，首先进行互补滤波，对陀螺仪测量值作初步补偿；其次运用梯度下降法更新四元数，有效降低姿态角漂移；再进行卡尔曼滤波，采用矩阵中的UD分解方法，避免滤波器发散，得到水下机器人姿态矩阵和姿态角；然后对加速度计和陀螺仪数据运用三阶迎风算法求解速度和位置。同时，对深度计采集数据进行滑动平均滤波，得到水下机器人深度值，水下机器人的垂向位置以此为准。该方法包括以下步骤:
[0009]I)姿态、速度、位置解算，过程如下:
[0010](I)水下机器人导航系统采用的离散卡尔曼方程为:
[0011]Xk = c^kjk-1Xk-1+Wk-!
[0012]Zk = HkXk+Vk
[0013]式中，k表示系统离散采样时间点，k > O ;Xk为k时刻的系统估计状态量为Xlri到Xk的一步转移矩阵为系统激励高斯白噪声序列，均值为E [w (k) ] = 0，方差为E[w(k)w(k)T] =Qk, Qk即系统噪声方差；Zk为k时刻的传感器量测值；Hk为量测矩阵，反应量测量和估计量之间的数学关系；Vk为量测值高斯白噪声序列，均值为E[V (k)] = 0，方差SE[v(k)v(k)T] = Rk，Rk即测量噪声方差；Wk、Vk互不相关；
[0014](2)确定参数初始状态，包括:
[0015]当地重力加速度g，系统采样时间t,角速度误差Δ ω” Δ ω2、Δ ω3，互补滤波参数kp、ki；被估计状态量初值，估计均方误差矩阵初值Ptlltl;其中kp、ki取值规则为:kp+ki = 1,0.95 < kp < 1，通过多次试验确定最优值χω、Ptlltl按如下取值:
X1 =￡{Z0}，?，设定导航坐标系为东北天坐标系，记为η
系；设定机体坐标系为b系,初始时刻与导航坐标系重合,η系和b系均为三轴正交坐标系；四元数表示坐标旋转关系Q = [qci，qp q2，q3]，其中为旋转标量，q2、q3为坐标旋转矢量，用四元数表述n系到b系转换关系表示为:
【权利要求】
1.一种水下结构检测机器人导航系统，包括磁罗盘(I)、陀螺仪(2)、加速度计(3)、深度计(4)、导航微处理器(5)，其特征在于，所述磁罗盘(I)、陀螺仪(2)、加速度计(3)和深度计(4)分别采集水下机器人磁场强度、角速度、线速度和下潜深度数据，传输至导航微处理器(5);所述导航微处理器(5)对磁罗盘(I)、陀螺仪(2)、加速度计(3)、深度计(4)采集的数据进行A/D转换；所述导航微处理器(5)结合四元数梯度下降法、互补滤波、卡尔曼算法对磁罗盘(I)、陀螺仪(2)、加速度计(3)采集的数据进行数据融合，获取姿态矩阵和姿态角；所述导航微处理器(5)对陀螺仪(2)、加速度计(3)采集的数据采用带有旋转补偿的三阶迎风算法求解速度和位置；所述导航微处理器(5)对深度计采用滑动平均滤波求取水下机器人下潜深度，得到组合导航信息。
2.如权利要求1所述水下结构检测机器人导航系统，其特征在于，所述磁罗盘(I)采用Honeywell公司的电子罗盘IC HMC5883L。
3.如权利要求1所述水下结构检测机器人导航系统，其特征在于，所述陀螺仪(2)、加速度计(3)是InvenSense公司生产的微机电产品。
4.如权利要求1所述水下结构检测机器人导航系统，其特征在于，所述深度计(4)是扩散硅型压力变送器，在10m量程范围内具有良好的精度。
5.如权利要求1所述水下结构检测机器人导航系统，其特征在于，所述导航微处理器(5)采用基于 ARM Cortex?-M3 内核的 STM32F103T8。
6.如权利要求1所述水下结构检测机器人导航系统的导航方法，其特征在于，以四元数和角误差为滤波估计对象，首先进行互补滤波，对陀螺仪测量值作初步补偿；其次运用梯度下降法更新四元数，有效降低姿态角漂移；再进行卡尔曼滤波，采用矩阵中的UD分解方法，避免滤波器发散，得到水下机器人姿态矩阵和姿态角；然后对加速度计和陀螺仪数据运用三阶迎风算法求解速度和位置。同时，对深度计采集数据进行滑动平均滤波，得到水下机器人深度值，水下机器人的垂向位置以此为准。该方法包括以下步骤: I)姿态、速度、位置解算，过程如下: (1)水下机器人导航系统采用的离散卡尔曼方程为:
Xk = Φ ^k-1Xk-1+Wk-!
Zk = HkXk+Vk 式中，k表不系统尚散米样时间点，k > O ;Xk为k时刻的系统估计状态量；fI3k,H为Xk-1到Xk的一步转移矩阵为系统激励高斯白噪声序列，均值为E [w(k)] =0，方差为E[w(k)w(k)T] =Qk，Qk即系统噪声方差；Zk为k时刻的传感器量测值；Hk为量测矩阵，反应量测量和估计量之间的数学关系；Vk为量测值高斯白噪声序列，均值为E[V(k)] = 0，方差为E[v(k)v(k)T] = Rk, Rk即测量噪声方差；Wk、Vk互不相关； (2)确定参数初始状态,包括: 当地重力加速度g，系统采样时间t，角速度误差Λ ωρ Δ ω2> Λ ω3，互补滤波参数kp、ki；被估计状态量初值，估计均方误差矩阵初值Ptlltl ;其中kp、Iii取值规则为:kp+ki = 1,0.95 < kp < 1，通过多次试验确定最优值Χα()、Ptlltl按如下取值:X1 =￡{X()}，设定导航坐标系为东北天坐标系，记为n系；设定机体坐标系为b系,初始时刻与导航坐标系重合,η系和b系均为三轴正交坐标系；四元数表示坐标旋转关系Q = [qci，qp q2，q3]，其中为旋转标量，q2、q3为坐标旋转矢量，用四元数表述n系到b系转换关系表示为:
而b系到n系为G = (C；；)T， ?.(I≤i，j≤3)表示矩阵中各元素值； (3)利用陀螺仪和磁罗盘、加速度计的频域互补特性校正角速度矢量，过程如下:设磁罗盘测量值为mb，加速度计测量值为a，陀螺仪测量值为ω，mb、a、ω均为3Χ I列矩阵；首先把磁场量由机体坐标系转换到导航坐标系=
，并计算水平和垂直方向磁场
、by = O、bz = Iiinz ; 然后把磁场转换到机体坐标系:'、，= C:;lbx by bz I1，并利用Ct第三列估计重力加速度矢量:々 =kd 再计算加速度和磁场补偿误差:(
式中，“ X ”表示向量乘法； 得到累积误差为:errInt = Ki Σ err*t ； 最后得到校正之后的角速度矢量:ω。= Q+kp*err+errInt 其中，K1、Kp是互补滤波参数； (4)利用梯度下降法更新四元数:
由Qk ，得到新四元数q。、1、％、，其中，μ是梯度下降的步长值，由 自)确定?女,
d为传感器基于机体坐标系的测量值，S为传感器基于导航坐标系的实际值，表示伴随矩阵，“?”为梯度乘法，“ O ”为四元数乘法； (5)基于UD分解的卡尔曼滤波过程依次为: 状态一步预测:
UD 分解
一步预测均方误差:
计算滤波增?:Kk = Pk/k—1? (HkPk/k—1? +R) = Pk/k—1? (UDU )；计算新息(新息由最新量测值计算获得):Yk =Z, -HkXuk^ ； 状态估值计算:= xk/k_,+Kk Tk ； 估计均方误差:Pk/k = (1-KkHk) Pk7^1 ； (6)运用三阶迎风格式求解速度和位置，并进行旋转补偿，按如下过程计算: 设加速度计在k时亥IJ、k-Ι时亥lj、k-2时刻的测量值分别为ak、a,+ ak_2，陀螺仪在k时刻、k-Ι时刻、k-2时刻的测量值分别为ω,、ω,+ ω,_2，令T = 2t，则时刻的速度分别设为vm、Vm+ vm-2，Tm、Tnri时刻的位置分别为Pm、Pnrl ； 首先计算两个采样时刻的速度增量和角增量: 速度增量为
角i曾量为
然后采用三阶迎风格式求解速度和位置:
速度力
位直为
式中，|ΔνχΔ0即为旋转补偿部分，“X”表示向量乘法； 2)深度计解算 对深度计采用等权端点平滑作滑动平均滤波，计算水下机器人下潜深度:
不断逐个滑动地去取m个相邻数据作算数平均计算，式中，Cli是深度计采集的第i个数据，depth为滤波后的深度值。
【文档编号】G01C21/20GK104197927SQ201410413791
【公开日】2014年12月10日 申请日期:2014年8月20日 优先权日:2014年8月20日
【发明者】曾庆军, 张明, 眭翔, 黄巧亮 申请人:江苏科技大学