一种水下机器人多传感器融合数据处理方法与流程

本发明属于水下机器人数据处理技术领域，具体涉及一种基于差分进化算法的扩展卡尔曼滤波算法的水下机器人多传感器融合数据处理方法。

背景技术：

海洋中蕴藏着巨大的经济潜力，受到世界各国的广泛关注。为了更好的探测和开发深海资源，各国学者都加紧了对海洋工程探测装置和开采设备的研发。其中，水下机器人在水下目标搜寻、海洋资源探测、海洋军事任务等军民两用方面得到了广泛应用。在实际应用中，如何在水下复杂环境下提高水下机器人的机动性、操纵性是急需解决的难点，研究面向水下机器人的多传感器融合数据处理方法对获取水下机器人的精确位置和姿态进而控制水下机器人运动具有重要意义。

目前扩展卡尔曼滤波已被广泛应用到无人机等工程实际各领域，而由于水下环境的复杂性和多样性，多传感器融合数据处理方法在水下机器人中的应用还处于空白阶段。扩展卡尔曼滤波假设过程噪声和观测噪声是一个恒定的矩阵，然而在实际的工程当中，由于外力或者磁力等干扰使得滤波器的工作的环境并不是一成不变的，而不同的环境对应的最佳噪声矩阵并不是一样的，而滤波器并不能对这些变换的环境实时的修正噪声矩阵，给姿态、位置和速度的估计带来误差，精度不高。

因此，研发一种基于基于差分进化算法的扩展卡尔曼滤波算法的水下机器人多传感器融合数据处理方法势在必行。

技术实现要素：

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种水下机器人多传感器融合数据处理方法，通过假设过程噪声由滤波结果和观测结果得到，再用差分进化算法对所得到的过程噪声方差进行最优化选择来提高滤波精度。

本发明采用以下技术方案：

一种水下机器人多传感器融合数据处理方法，包括以下步骤：

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观测到的位置和速度信息z^g为：

其中，m^b＝[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^_ht^t(hpt^_h^t+rt)^-1

pt＝(i-ktht)pt^_

其中，rt为t次采样时刻系统量测噪声误差方差矩阵，ht^t为t次采样时刻系统总的观测矩阵h转置，zt为t次采样时刻的观测矩阵，i为16阶单位矩阵。

具体的，步骤s7中，根据欧拉角和四元数的坐标转换矩阵得到偏航角γ、翻滚角β、俯仰角α和四元数的关系为：

β＝arctan2(q1q3-q0q2)

其中，q＝[q0q1q2q3]^t为姿态信息的四元数。

与现有技术相比，本发明至少具有以下有益效果：

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

进一步的，步骤s1中，定义导航坐标系与载体坐标系，姿态为载体坐标系相对于导航坐标系的转换关系，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据，gps采集水下机器人的经度、纬度、高度信息，深度传感器采集水下机器人的深度数据。上述数据为后续的多传感器数据处理提供数据来源。并且坐标系是描述物体在空间的相对位置和运动规律所使用的基准，由于地面遥控系统与载体(水下机器人)控制系统所基于的坐标系不同，因此需要建立导航坐标系与载体坐标系，进行两个坐标系之间进行转换。

进一步的，步骤s2中，建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系。使用四元数算法数学微分方程维度降到4维，对结果进行归一化处理后就能完成正交化解算，很大程度上降低了导航系统的运算量，此外很好地避开奇异值的问题，不存在方程退化的缺点，可令系统全姿态工作。

进一步的，步骤s3中，综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵。求出的状态转移矩阵用来表示水下机器人姿态、位置和速度估计系统前后两个时刻状态向量的传递过程。

进一步的，步骤s4中，根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程。上述过程实现了水下机器人姿态、位置和速度估计系统的初始化和先验预测。

进一步的，步骤s4中，由滤波结果减去观测结果计算出过程噪声，根据不同时刻的过程噪声求出其过程噪声的方差，然后使用差分进化算法求出这一组方差的最优值。此算法可以有效地提高滤波精度，减小估计偏差。

进一步的，步骤s5中，采用磁力计的数据作为四元数的观测信息，采用gps的数据作为位置与速度的观测信息，综合磁力计与gps的观测信息得到观测矩阵。通过融合所有可用的传感器测量数据，能够更好地消除具有明显误差的传感器测量数据，从而使水下机器人不易受到单个传感器故障的影响，更适用于水下机器人这类非线性系统。

进一步的，步骤s6中，进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程。上述过程实现了水下机器人姿态、位置和速度估计系统的后验校正。

进一步的，步骤s7中，根据更新后得到的四元数q解算水下机器人的偏航角、翻滚角、俯仰角，再结合其他状态变量vn,ve,vd,pn,pe,pd估计出水下机器人的姿态、位置和速度信息。上述过程实现了利用多传感器融合数据处理方法来估计水下机器人的姿态、位置和速度信息，估计误差小、精度高。

综上所述，本发明适用于水下机器人类非线性系统来精确地估计水下机器人的姿态、位置和速度信息，具有广阔的应用前景。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明多传感器融合数据处理方法流程图；

图2为本发明水下机器人导航坐标系与载体坐标系示意图；

图3为本发明静态条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图，其中，(a)为俯仰角对比图，(b)为翻滚角对比图，(c)为偏航角对比图；

图4为本发明水平滑动条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图，其中，(a)为水平滑动俯仰角对比，(b)为水平滑动翻滚角对比图；

图5为本发明动态条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图，其中，(a)为动态翻滚角对比图，(b)为动态俯仰角对比图，(c)为动态偏航角对比图；

图6为本发明水下机器人姿态估计值和观测值对比图；

图7为本发明水下机器人位置估计值和观测值对比图；

图8为本发明水下机器人速度估计值和观测值对比图。

具体实施方式

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

请参阅图1，本发明一种水下机器人多传感器融合数据处理方法，包括以下步骤：

s1、搭建水下机器人姿态、位置和速度估计系统，获取载体坐标系(坐标原点位于惯性测量单元imu中心的“右-前-上”直角坐标系)下的多个传感器数据；

请参阅图2，定义导航坐标系与载体坐标系，姿态就是载体坐标系相对于导航坐标系的转换关系。陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据，gps采集水下机器人的经度、纬度、高度信息，深度传感器采集水下机器人的深度数据；

s2、建立状态变量与观测变量，使用四元数表征姿态信息，并利用姿态信息将位置与速度解算转换到导航坐标系；

对于水下机器人的控制系统来说，最重要的状态为水下机器人的姿态、位置和速度信息。在计算姿态的过程中还需要考虑角度偏差与速度偏差的影响，设定水下机器人的16个状态变量。扩展卡尔曼滤波的特点在于能够融合多个传感器的信息，在水下机器人控制系统中采用gps的数据作为位置与速度的观测信息，采用磁力计的数据作为四元数的观测信息，从而定义9个观测变量；

s201、设定水下机器人的状态变量为：

其中，q＝[q0q1q2q3]^t为姿态信息的四元数，v＝[vnvevd]^t为导航坐标系下的“北-东-天”三个方向的速度，p＝[pnpepd]^t为导航坐标系下“北-东-天”三个方向的位置。为载体坐标系下的角度偏差，为载体坐标系下的速度偏差。

s202、对应状态变量，采用磁力计的数据作为四元数的观测信息，采用gps的数据作为位置与速度的观测信息；

定义观测变量为：其中m^b＝[mxmymz]^t为磁力计在载体坐标系下测得的三轴磁力分量。为gps观测到的位置和速度信息。

s3、综合四元数、速度和位置的预测过程得到状态向量的状态转移矩阵；

根据四元数微分方程和姿态矩阵构建载体系统的状态方程，状态预测过程依赖于运动学方程，水下机器人控制系统采用imu(惯性测量单元，包含陀螺仪与加速度计)来获取角速度信息，积分得到角度变化量，从而更新四元数得到新的坐标转换矩阵。通过加速度积分获取载体坐标系的速度信息，速度积分得到位置信息，再经过坐标转换就可以得到导航坐标系的速度与位置信息。

对于四元数的预测过程，定义一个特殊的四元数：

其中φ＝[φxφyφz]^t为旋转矢量，而且与旋转矢量的模相等。在角度增量很小的情况下，陀螺仪测量的三轴角度增量与旋转矢量是相等的。可以把四元数的变化量写成如下形式：

其中δang为陀螺仪测得的角度变化值。为了得到四元数的状态转移矩阵，需要找到四元数本身以及角度测量偏差对四元数状态更新的影响。为此，建立了如下的状态向量：

q＝[q0q1q2q3bgxbgybgz]^t

其中bg＝[bgxbgybgz]^t为陀螺仪的角速度测量偏差。四元数q的微分方程为：

其中为载体坐标系下的三轴角速度。建立向量q的微分关系式如下：

可得四元数的状态转移矩阵为：

从中提取出四元数的更新方程为：

对于位置和状态信息，使用运动学基本方程得到速度的状态预测过程为：

其中为四元数对速度状态转移的影响，g为重力加速度，为：

再由速度的信息进行位置的预测过程

综合四元数、速度、位置的预测过程可得状态向量x的状态转移矩阵为：

其中，为为四元数更新对自身状态转移的影响矩阵，为四元数对速度状态转移的影响矩阵，为为速度对位置状态转移的影响矩阵，为为四元数对速度状态转移的影响矩阵，i为3阶单位矩阵。

s4、根据状态转移矩阵和过程噪声求出对状态预测影响的控制雅克比矩阵，初始化协方差和过程噪声方差的初始值后就可以建立扩展卡尔曼滤波的状态预测方程；

由滤波结果减去观测结果计算出过程噪声，根据不同时刻的过程噪声求出其过程噪声的方差，然后使用差分进化算法求出这一组方差的最优值，在滤波过程中就使用这个不断变化的最优过程噪声方差；

由滤波结果减去观测结果计算出过程噪声，根据不同时刻的过程噪声求出其过程噪声的方差，然后使用差分进化算法求出这一组方差的最优值。

过程噪声：

et＝xt-zt

其中，xt为t采样时刻滤波后的状态矩阵，zt为t次采样时刻的观测矩阵，t时刻的过程噪声：

qt＝var(et)

其中，et为t时刻的过程噪声，qt为t时刻的过程噪声方差，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运用到下一次滤波中。贪婪选择方式使种群性能提高，逐步达到最优解。使用差分进化算法对扩展卡尔曼滤波算法进行改进可以有效地提高了滤波精度。

过程噪声对状态预测影响的控制雅克比矩阵为：

获得了f和g，初始化协方差p和过程噪声方差qs的初始值后就可以建立扩展卡尔曼滤波的预测方程：

其中，为t-1次采样时刻状态矩阵，为t次采样时刻预测状态矩阵。pt-1为t-1次采样时刻估计误差方差矩阵，qt-1为t-1次采样时刻系统过程噪声方差矩阵，qs是预测过程所用传感器的噪声，pt^_为t次采样时刻预测误差方差矩阵。

s5、采用磁力计的数据作为四元数的观测信息，采用gps的数据作为位置与速度的观测信息，综合磁力计与gps的观测信息得到观测矩阵；

观测矩阵包含了磁力计与gps两部分观测矩阵。采用磁力计的数据作为四元数的观测信息，采用gps的数据作为位置与速度的观测信息。

对于磁力计来说，载体坐标系下的观测变量m^b＝[mxmymz]^t与导航坐标系下的三轴磁场分量mⁿ由如下关系：

其中ν为测量噪声，mⁿ＝m[cosα0-sinα]^t，m为地磁场向量的模，α为磁偏角，这两个参数可由gps测出的经纬度和地磁场模型得到，得到磁力计的观测雅克比矩阵为：

gps的输出观测量为但这并不是gps的原始数据，在这里定义等式：

其中lon，lat，hgt分别为gps返回的经度、纬度、高度信息。lonreference，latreference和hgtreference分别为水下机器人初始位置的经纬度和海拔，re为地球半径。由此得到gps的观测矩阵为：

综合磁力计与gps的观测矩阵，我们就可以得到总的观测矩阵为：

其中，hmag为磁力计的观测雅克比矩阵，hgps为gps的观测矩阵。

s6、进行观测过程噪声的初始化，建立扩展卡尔曼滤波的状态更新方程；

扩展卡尔曼滤波的状态更新方程为：

kt＝pt^_ht^t(hpt^_h^t+rt)^-1

pt＝(i-ktht)pt^_

其中，rt为t次采样时刻系统量测噪声误差方差矩阵，kt为t次采样时刻的增益矩阵，zt为t次采样时刻的观测矩阵，为t次采样时刻状态矩阵，pt为t次采样时刻估计误差方差矩阵，i为16阶单位矩阵。

s7、根据更新后得到的四元数q解算水下机器人的三个姿态角：偏航角、翻滚角、俯仰角，再结合其他状态变量vn,ve,vd,pn,pe,pd从中而估计出水下机器人的姿态、位置和速度信息。

根据欧拉角和四元数的坐标转换矩阵，从而得到偏航角、翻滚角、俯仰角和四元数的关系为：

β＝arctan2(q1q3-q0q2)

根据更新后得到的四元数q按照上式可以解算水下机器人的三个姿态角偏航角γ、翻滚角β、俯仰角α。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图3，为本发明静态条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图。静态条件下理想的俯仰角、翻滚角和偏航角三个姿态角输出应该是0，由图可知，扩展卡尔曼滤波的俯仰角、翻滚角和偏航角的平均值分别为-0.0273，0.0046，-0.0293。互补滤波的的俯仰角、翻滚角和偏航角的平均值分别为-0.0707，-0.0284，-0.0736。因此扩展卡尔曼滤波的俯仰角、翻滚角和偏航角三个姿态角输出相比互补滤波更接近零线，输出精度更高。

请参阅图4，为本发明水平滑动条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图。水下机器人在经受水平滑动的非重力加速度干扰下，两种算法的俯仰角和翻滚角输出对比图。由图可知，扩展卡尔曼滤波的俯仰角和翻滚角的平均值分别为-0.0456，-0.0526。互补滤波的的俯仰角和翻滚角的平均值分别为0.0955，-0.1009。因此扩展卡尔曼滤波的俯仰角和翻滚角相比于传统的互补滤波收到的干扰更小，整体曲线更接近零线，而且波动更小。扩展卡尔曼滤波抑制非重力加速度带来的干扰的能力更强。

请参阅图5，为本发明动态条件下扩展卡尔曼滤波与传统互补的姿态角输出对比图。在动态实验下让水下机器人的俯仰角、翻滚角和偏航角三个姿态角在-20°到20°之间变化，以观察两种算法的动态性能。由图可知，扩展卡尔曼滤波相比传统的互补滤波的动态输出精度更高，且在经历多次来回旋转回到零线后仍能保持较高的精度。

请参阅图6、图7和图8，为水下机器人进行综合实验时，对得到的姿态、位置和速度的估计值与观测值对比。图6为本发明水下机器人综合实验时姿态估计值和观测值对比图，图7为本发明水下机器人综合实验时位置估计值和观测值对比图，图8为本发明水下机器人综合实验时速度估计值和观测值对比图。由图可知，采用本发明的多传感器融合数据处理方法所得到的姿态、位置和速度的估计值与观测值误差很小，精度较高，同时相比较观测值去掉了数据的噪声，有滤波作用。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。