一种水下机器人多传感器融合数据处理方法技术

本发明专利技术公开了一种水下机器人多传感器融合数据处理方法，基于多个传感器估计水下机器人的姿态、位置和速度信息；通过融合所有传感器测量数据，能够更好地消除具有明显误差的传感器测量数据，从而使水下机器人不易受到单个传感器故障的影响，更适用于水下机器人这类非线性系统；在构建状态向量时把角度偏差和速度偏差加进了状态向量，考虑了角度偏差和速度偏差对于状态更新的影响，来精确地估计水下机器人的姿态、位置和速度信息。通过假设过程噪声由滤波结果和观测结果得到，再用差分进化算法对所得到的过程噪声方差进行最优化选择来提高滤波精度。本发明专利技术适用于水下机器人类非线性系统来精确地估计水下机器人的姿态、位置和速度信息。

A Data Processing Method for Multisensor Fusion of Underwater Vehicle

【技术实现步骤摘要】
一种水下机器人多传感器融合数据处理方法
本专利技术属于水下机器人数据处理
，具体涉及一种基于差分进化算法的扩展卡尔曼滤波算法的水下机器人多传感器融合数据处理方法。
技术介绍
海洋中蕴藏着巨大的经济潜力，受到世界各国的广泛关注。为了更好的探测和开发深海资源，各国学者都加紧了对海洋工程探测装置和开采设备的研发。其中，水下机器人在水下目标搜寻、海洋资源探测、海洋军事任务等军民两用方面得到了广泛应用。在实际应用中，如何在水下复杂环境下提高水下机器人的机动性、操纵性是急需解决的难点，研究面向水下机器人的多传感器融合数据处理方法对获取水下机器人的精确位置和姿态进而控制水下机器人运动具有重要意义。目前扩展卡尔曼滤波已被广泛应用到无人机等工程实际各领域，而由于水下环境的复杂性和多样性，多传感器融合数据处理方法在水下机器人中的应用还处于空白阶段。扩展卡尔曼滤波假设过程噪声和观测噪声是一个恒定的矩阵，然而在实际的工程当中，由于外力或者磁力等干扰使得滤波器的工作的环境并不是一成不变的，而不同的环境对应的最佳噪声矩阵并不是一样的，而滤波器并不能对这些变换的环境实时的修正噪声矩阵，给姿态、位置和速度的估计带来误差，精度不高。因此，研发一种基于基于差分进化算法的扩展卡尔曼滤波算法的水下机器人多传感器融合数据处理方法势在必行。
技术实现思路
本专利技术所要解决的技术问题在于针对上述现有技术中的不足，提供一种水下机器人多传感器融合数据处理方法，通过假设过程噪声由滤波结果和观测结果得到，再用差分进化算法对所得到的过程噪声方差进行最优化选择来提高滤波精度。本专利技术采用以下技术方案：一种水下机器人多传感器融合数据处理方法，包括以下步骤：S1、搭建水下机器人姿态、位置和速度估计系统，获取载体坐标系下的传感器数据；S2、建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系；S3、综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵；S4、根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程；S5、采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，综合磁力计与GPS的观测信息得到观测矩阵；S6、进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程；S7、根据更新后得到的四元数q解算水下机器人的偏航角、翻滚角、俯仰角，再结合其他状态变量VN,VE,VD,PN,PE,PD估计出水下机器人的姿态、位置和速度信息。具体的，步骤S1中，定义导航坐标系与载体坐标系，姿态为载体坐标系相对于导航坐标系的转换关系，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据，GPS采集水下机器人的经度、纬度、高度信息，深度传感器采集水下机器人的深度数据。具体的，步骤S2具体为：S201、设定水下机器人的状态变量为：其中，q＝[q0q1q2q3]T为表示姿态信息的四元数，V＝[VNVEVD]T为导航坐标系下的北-东-天三个方向的速度，P＝[PNPEPD]T为导航坐标系下北-东-天三个方向的位置；为载体坐标系下的角度偏差；为载体坐标系下的速度偏差；S202、对应状态变量，采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，定义观测变量z为：GPS观测到的位置和速度信息zG为：其中，mb＝[mxmymz]T为磁力计在载体坐标系下测得的三轴磁力分量。具体的，步骤S3中，综合四元数、速度、位置的预测过程得到状态向量x的状态转移矩阵为：其中，为为四元数更新对自身状态转移的影响矩阵，为四元数对速度状态转移的影响矩阵，为为速度对位置状态转移的影响矩阵，为为四元数对速度状态转移的影响矩阵，I为3阶单位矩阵。具体的，步骤S4中，由滤波结果减去观测结果计算过程噪声，根据不同时刻的过程噪声求出其过程噪声的方差，然后使用差分进化算法求出这一组方差的最优值，t时刻的过程噪声Et为：Et＝xt-zt其中，xt为t采样时刻滤波后的状态矩阵，zt为t次采样时刻的观测矩阵，t时刻的过程噪声方差Qt为：Qt＝var(Et)其中，var(Et)是使用差分进化算法求过程噪声方差的运算。进一步的，使用差分进化算法求过程噪声方差的步骤如下：S401、种群初始化xi,k(0)＝lk+rand()*(uk-lk),k＝1,2,…,d,i＝1,2,…,N其中，rand()为0—1之间均匀分布的随机数，uk和lk为搜索的上界和下界，以Qt-1和Q0为上界和下界；S402、假设变异机制为：Xi(g)＝xr1(g)+F*[xr2(g)-xr3(g)]其中，Xi(g)为变异的个体，F为压缩比例因子，xr1、xr2和xr3为3个父代；S403、交叉操作：采用二项交叉方式，二项交叉的执行方式为：其中，r为每个变量生成的一个0—1之间均匀分布的随机数，cr为变量的交叉概率；rnd为1—d之间均匀分布的整数，r＜cr如果则接受目标个体对应的分量，否则保留当前个体对应的分量。S404、选择操作：采用贪婪选择的方式，操作为：把最优解赋值给Qs运用到下一次滤波中，贪婪选择方式使种群性能提高；过程噪声对状态预测影响的控制雅克比矩阵为：初始化协方差P和过程噪声方差Qs的初始值后建立扩展卡尔曼滤波的预测方程：其中，为t-1次采样时刻状态矩阵，为t次采样时刻预测状态矩阵，Pt-1为t-1次采样时刻估计误差方差矩阵，Qt-1为t-1次采样时刻系统过程噪声方差矩阵，Qs是预测过程所用传感器的噪声，Pt_为t次采样时刻预测误差方差矩阵。具体的，步骤S5中，综合磁力计的观测雅克比矩阵与GPS的观测矩阵，得到总的观测矩阵H为：其中，Hmag为磁力计的观测雅克比矩阵，Hgps为GPS的观测矩阵。进一步的，磁力计的观测雅克比矩阵Hmag为：其中，α为磁偏角，q＝[q0q1q2q3]T为姿态信息的四元数；GPS的观测矩阵为：具体的，步骤S6中，扩展卡尔曼滤波的状态更新方程，t次采样时刻的增益矩阵Kt、t次采样时刻状态矩阵和t次采样时刻估计误差方差矩阵Pt为：Kt＝Pt_HtT(HPt_HT+Rt)-1Pt＝(I-KtHt)Pt_其中，Rt为t次采样时刻系统量测噪声误差方差矩阵，HtT为t次采样时刻系统总的观测矩阵H转置，zt为t次采样时刻的观测矩阵，I为16阶单位矩阵。具体的，步骤S7中，根据欧拉角和四元数的坐标转换矩阵得到偏航角γ、翻滚角β、俯仰角α和四元数的关系为：β＝arctan2(q1q3-q0q2)其中，q＝[q0q1q2q3]T为姿态信息的四元数。与现有技术相比，本专利技术至少具有以下有益效果：本专利技术一种水下机器人多传感器融合数据处理方法，通过融合所有可用的传感器测量数据来估计水下机器人的姿态、位置和速度，能够更好地消除具有明显误差的传感器测量数据，从而使水下机器人不易受到单个传感器故障的影响；在构建状态向量时把角度偏差和速度偏差加进了状态向量，考虑了角度偏差和速度偏差对于状态更新的影响，来精确地估计水下机器人的姿态、位置和速度信息；通过假设过程噪声由滤波结果和观测结果得到，再用差分进化算法对所得到的过程噪声方差进行最优化选本文档来自技高网...

【技术保护点】
1.一种水下机器人多传感器融合数据处理方法，其特征在于，包括以下步骤：S1、搭建水下机器人姿态、位置和速度估计系统，获取载体坐标系下的传感器数据；S2、建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系；S3、综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵；S4、根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程；S5、采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，综合磁力计与GPS的观测信息得到观测矩阵；S6、进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程；S7、根据更新后得到的四元数q解算水下机器人的偏航角、翻滚角、俯仰角，再结合其他状态变量VN,VE,VD,PN,PE,PD估计出水下机器人的姿态、位置和速度信息。

【技术特征摘要】
1.一种水下机器人多传感器融合数据处理方法，其特征在于，包括以下步骤：S1、搭建水下机器人姿态、位置和速度估计系统，获取载体坐标系下的传感器数据；S2、建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系；S3、综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵；S4、根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程；S5、采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，综合磁力计与GPS的观测信息得到观测矩阵；S6、进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程；S7、根据更新后得到的四元数q解算水下机器人的偏航角、翻滚角、俯仰角，再结合其他状态变量VN,VE,VD,PN,PE,PD估计出水下机器人的姿态、位置和速度信息。2.根据权利要求1所述的方法，其特征在于，步骤S1中，定义导航坐标系与载体坐标系，姿态为载体坐标系相对于导航坐标系的转换关系，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据，GPS采集水下机器人的经度、纬度、高度信息，深度传感器采集水下机器人的深度数据。3.根据权利要求1所述的方法，其特征在于，步骤S2具体为：S201、设定水下机器人的状态变量为：其中，q＝[q0q1q2q3]T为表示姿态信息的四元数，V＝[VNVEVD]T为导航坐标系下的北-东-天三个方向的速度，P＝[PNPEPD]T为导航坐标系下北-东-天三个方向的位置；为载体坐标系下的角度偏差；为载体坐标系下的速度偏差；S202、对应状态变量，采用磁力计的数据作为四元数的观测信息，采用GPS的数据作为位置与速度的观测信息，定义观测变量z为：GPS观测到的位置和速度信息zG为：其中，mb＝[mxmymz]T为磁力计在载体坐标系下测得的三轴磁力分量。4.根据权利要求1所述的方法，其特征在于，步骤S3中，综合四元数、速度、位置的预测过程得到状态向量x的状态转移矩阵为：其中，为为四元数更新对自身状态转移的影响矩阵，为四元数对速度状态转移的影响矩阵，为为速度对位置状态转移的影响矩阵，为为四元数对速度状态转移的影响矩阵，I为3阶单位矩阵。5.根据权利要求1所述的方法，其特征在于，步骤S4中，由滤波结果减去观测结果计算过程噪声，根据不同时刻的过程噪声求出其过程噪声的方差，然后使用差分进化算法求出这一组方差的最优值，t时刻的过程噪声Et为：Et＝xt-zt其中，xt为t采样时刻滤波后...

【专利技术属性】
技术研发人员：胡桥，丁明杰，苏文斌，李俊，赵振轶，李庚，宋雪漪，
申请(专利权)人：西安交通大学，
类型：发明
国别省市：陕西,61