一种基于多传感器数据融合的无人船定位方法

1.本发明属于定位航行技术领域，涉及一种基于多传感器信息采集的融合处理技术，尤其涉及一种基于多传感器数据融合的无人船定位方法。


背景技术：

2.以现代传感器信息技术为支撑的智能无人船定位系统的发展和应用，不仅能够推动水面无人船在近海巡逻、水质监控和水产养殖等领域的发展，而且能大幅度地减轻操作人员的工作强度及提高作业效率。
3.要实现无人船的高效精准工作，首先要依赖于所获得的传感器定位信息能否对水面无人船位置进行准确感知，即无人船能否利用所获得的定位信息来保证其工作效率及精确性。随着水面无人船自动导航技术的发展，对无人船定位精度和稳定性的要求越来越高。由于水面及岸边复杂工作环境的影响，可能导致传感器信息丢失而造成定位不准。因此，使用单一的传感器存在较大弊端，而多种传感器数据融合利用其优势互补会有效克服了上述弊端，成为了组合定位系统发展趋势。因此，开展无人船定位系统的多传感器数据融合算法的研究和应用，有助于提高算法的容错性能，进而实现多传感器数据融合算法对无人船位置数据的高效滤波处理，最终达到对无人船位置进行精确定位、提高生产作业效率的目的。


技术实现要素：

4.本发明的目的是为了克服现有技术使用单一的传感器存在较大弊端，针对无人船航行轨迹的多传感器数据融合定位，提供一种基于多传感器数据融合的无人船定位方法。
5.为了达到上述目的，本发明采取如下技术方案予以实现：
6.一种基于多传感器数据融合的无人船定位方法，首先对无人船多传感器定位系统所采集的定位数据进行预处理；然后对无人船定位数据进行置信距离判定并赋予相应置信因子；同时对定位数据进行故障检验及加权补偿；接着对定位数据滤波增强；最后利用数据融合算法进行多传感器数据融合滤波输出，从而实现对无人船航行轨迹的定位。其具体步骤如下：
7.步骤1.数据预处理；
8.对无人船多传感器定位系统所采集的无人船定位数据进行预处理。
9.步骤2.数据置信度判定赋值；
10.所述数据置信度判定赋值，包括根据多传感器所采集数据进行置信度判定并进行置信因子赋值。
11.步骤3.数据故障检验补偿；
12.所述数据故障检验补偿，包括依据对多传感器数据进行一致性检验，来对定位系统所采集的不一致数据进行加权补偿处理。
13.步骤4.数据增强；
14.所述数据增强，包括基于基本粒子滤波算法，分别对经预处理及检验、补偿后的多
传感器数据进行滤波处理。
15.步骤5.数据融合。
16.所述数据融合，包括通过构造高斯混合模型，并设置自适应门限及与置信因子相关联的分层采样比例容量，来设计出新的门限分层粒子滤波算法，进而对无人船航行轨迹定位数据进行融合滤波，输出无人船航行轨迹定位信息。
17.进一步，步骤1所述数据预处理，对无人船多传感器定位系统所采集的位置数据进行预处理，具体内容和方法步骤是：
18.a)根据定位系统传感器所采集的无人船航行轨迹经纬度信息，对其进行时空基准统一；
19.b)依据无人船航行河道，构建满足定位系统多节点有效连接的通讯环境，使得位于无人船上盲节点处于4个发射信号强度和位置坐标已知的信号接收节点网络内，以此计算采集无人船在河道航行时位置坐标；
20.c)根据高斯-克吕格投影原理，对多传感器定位系统坐标进行转换统一。
21.进一步，步骤2所述数据置信度判定赋值，包括根据多传感器所采集数据进行置信度判定并进行置信因子赋值，其中，所述数据置信度判定的具体内容和方法步骤是：
22.1)依据无人船定位系统测量数据的测量模型pi(x)，计算传感器数据间的置信距离di，所述pi(x)，di的计算公式如下：
[0023][0024]
式中，xi为第i个传感器的测量值，μ为测量特征的真值，θi为第i个传感器信息测量精度，σi为第i个传感器信息测量误差。
[0025][0026]
式中，x
′i、x
″i分别为无人船定位系统多传感器i时刻所采集测量值，τ
′i、τ
″i为相应传感器测量方差，为测量方差均值，z为服从标准正态分布的随机变量，i＝1,2,
…
,n。
[0027]
2)依据高斯概率模型将步骤1)所得di，将其改写为传感器测量数据间概率意义上的度量pr(zi)，设定传感器支持度置信水平，来判别不同传感器信息的可信度，所述pr(zi)的计算公式如下：
[0028][0029]
式中，zi为无人船多传感器定位系统的测量数据，ε为置信水平，k为采样样本概率区间变量系数。
[0030]
3)依据测量数据间概率意义上的信息度量值大小，将所判定的定位数据划分入不同置信区间，使得可信数据向置信度较高区域靠拢；同时，所设定的置信水平ε与传感器测量方差均值相对应，其用来表示所采集的无人船多传感器定位系统的测量数据落入区间相对应，其用来表示所采集的无人船多传感器定位系统的测量数据落入区间的概率，因此测量信息的方差不同，相应的对无人船定位信息的置信区间分
类也是变化的，可根据传感器测量方差大小进行调节，提高了对传感器测量数据的采样可靠性。
[0031]
进一步，步骤2所述数据置信度判定赋值中所述数据置信因子赋值的具体内容和方法步骤是：
[0032]
(a)构建k时刻多传感器所采集测量信息的动态支持度因子βi(k)与高斯概率测量模型pi(x)的范数方程，所述βi(k)的计算公式如下：
[0033][0034]
式中，||.||f为frobenius范数，k＝1,2,
…
,t,i＝1,2,
…
,n。
[0035]
(b)根据步骤(a)计算所得多传感器动态支持度因子βi(k)，计算系统测量误差wi，所述wi的计算公式如下：
[0036]
wi＝z
i-aβi(k)
[0037]
式中，a为状态转移矩阵，zi为无人船多传感器定位系统测量值，i＝1,2,
…
,n。
[0038]
(c)设系统测量误差wi的方差为利用来对测量信息赋予相应的置信因子所述置信因子的计算公式如下：
[0039][0040]
式中，为无人船多传感器定位系统测量数据的置信因子，i＝1,2,
…
,n。
[0041]
依据计算所得无人船多传感器测量数据的可信度大小，对其赋予相应置信因子，并将置信因子与新的门限分层粒子滤波算法相关联，置信度越高的测量数据，其相应融合偏向度也高，提高了无人船多传感器数据融合算法的处理精度。
[0042]
进一步，步骤3所述数据故障检验补偿，包括依据对多传感器数据进行一致性检验，来对定位系统所采集的不一致故障数据进行加权补偿处理，其中，所述一致性检验的具体内容和方法，采用如下步骤：
[0043]
(1)将无人船定位系统所获得的传感器测量数据做算术平均，求出算术平均值所述的计算公式如下：
[0044][0045]
式中，xi为感器测量信息，i＝1,2,
…
,n。
[0046]
(2)利用步骤(1)所求的传感器测量数据算术平均值与定位系统后续采样值xh作差。
[0047]
(3)设定系统要求误差，若传感器测量数据算术平均值与定位系统后续采样值差值小于系统要求误差，判定多传感器定位系统所采集的无人船位置数据具有一致性，为可信数据；若差值大于系统要求误差，则需对采样数据进行方差加权补偿，以此满足基本粒子
滤波对数据样本的采样需求。
[0048]
进一步，步骤3所述数据故障检验补偿中所述不一致故障数据进行加权补偿处理的方法，具体采用如下步骤：
[0049]
(i)取无人船多传感器测量数据算术平均值作为真值的无偏估计值，来表示无人船多传感器定位系统在同一空间的不同位置对无人船航行轨迹进行测量时，第i个传感器系统的信息测量方差所述的计算公式如下：
[0050][0051]
(ii)依据无人船多传感器定位系统对无人船进行m次测量所记录的数据集，记第i个传感器的第j次测量数据为x
ij
，将x
ij
替换步骤(i)中感器测量信息xi，以此获得经多次测量所获得的数据集信息方差所述的计算公式如下：
[0052][0053]
式中，i＝1，2，
…
，n，j＝1,2,
…
,m。
[0054]
(iii)根据步骤(ii))所计算的无人船多传感器定位系统所采集的位置信息数据集方差，来计算不一致故障数据的融合权值κi。所述κi的计算公式如下：
[0055][0056]
(iv)根据所求的融合权值κi，对不一致故障数据进行方差加权补偿，以此获得满足基本粒子滤波处理要求的传感器测量数据所述的计算公式如下：
[0057][0058]
进一步，步骤4所述数据增强，包括基于基本粒子滤波算法，分别对经预处理及检验、补偿后的多传感器数据进行滤波处理，所述数据增强的具体内容和方法步骤是：
[0059]
(i)建立无人船多传感器定位系统的基本粒子滤波模型，将经一致性检验及方差加权后的无人船位置信息代入系统模型，传感器系统的状态和测量模型由下式概述：
[0060][0061]
式中，xk为传感器系统k时刻的位置预测值，xh为传感器后续采样值，方差加权后的传感器测量值，zk为k时刻的无人船位置测量值，λk为估计噪声，νk为测量噪声。
[0062]
(ii)粒子集合样本初始化，从先验密度p(x0)中随机采样生成初始化粒子集，所有粒子权值为1/n，
[0063]
(iii)从重要性密度函数中随机抽取n个粒子样本。
[0064]
(iv)计算采样粒子的权值并更新，所述的计算公式如下：
[0065][0066]
式中，为采样粒子权值,i＝1,2,
…
n。
[0067]
(v)归一化重要性权重。
[0068]
(vi)计算基本粒子滤波算法中有效粒子数n
eff
，并与阈值n
th
比较，若n
eff
《n
th
则进行重采样，所述n
eff
的计算公式如下：
[0069][0070]
(vii)状态输出，得到无人船多传感器定位系统所采集的测量信息经滤波后的局部估计及协方差阵所述的计算公式如下：
[0071][0072][0073]
粒子传播是通过对无人船多传感器定位系统状态转移模型pz(xk|x
k-1
,zk)进行采样后，来生成新粒子状态xk，其中x
k-1
为上一步重采样后粒子状态，zk为传感器系统观测数据。利用基本粒子滤波算法对无人船多传感器定位系统测量数据进行滤波处理实现数据增强，降低了环境噪声对测量数据的干扰，提高了无人船多传感器测量数据的融合精度。
[0074]
进一步，步骤5所述数据融合，包括通过构造高斯混合模型，并设置自适应门限及与置信因子相关联的分层采样比例容量，来设计出新的门限分层粒子滤波算法，进而对无人船航行轨迹定位数据进行融合滤波，输出无人船航行轨迹定位信息，所述构造高斯混合模型的具体内容和方法步骤是：
[0075]
(a)提取经基本粒子滤波算法处理后的多传感器数据集，并计算sigma点集所述的计算公式如下：
[0076][0077]
式中，为无迹变换后的sigma点集，na为sigma点的维数，λ为尺度参数。
[0078]
(b)在所获得的sigma采样点集中融入最新的量测信息，并对系统状态和协方差进
[0079][0080]
式中，kk为卡尔曼增益，zk为量测信息，为加权sigma点集量测的协方差。
[0081]
(c)利用步骤(b)所获得的无人船多传感器定位系统状态和协方差来获得更接近目标概率函数的建议分布并从所构造的建议分布中采样。
[0082]
(d)依据高斯混合模型，生成时间步长为k的后验概率密度函数p(xk|z
1:k
)，所述p(xk|z
1:k
)的计算公式如下：
[0083][0084]
式中，n(xk|mi,vi)为混合高斯模型中的第i个分量，c(k)为离散样本的组件单元数量，ξ为离散点组件权重。
[0085]
(e)将由步骤(c)所采样的离散采样点及其所对应权重融入步骤(d)高斯混合分量组件单元中，并利用构造好的连续后验概率密度函数p(xk|z
1:k
)对离散粒子进行重采样，所述p(xk|z
1:k
)的计算公式如下：
[0086][0087]
其中：
[0088][0089][0090][0091]
式中，p(k)为离散粒子滤波分布的协方差，为离散粒子滤波分布的均值，h为标准化常量，n
x
为粒子分布维数。
[0092]
(f)采用聚类分析来对步骤(e)连续后验概率密度函数p(xk|z
1:k
)中高斯混合相似单元进行合并处理。
[0093]
首先将代表状态的后验概率密度函数，用离散粒子构造出连续概率密度函数来表示，然后将所构造的连续后验概率密度函数近似为高斯混合分布，并在其上抽取粒子样本来代替重采样，以此来保持粒子多样性，避免样本匮乏，提高滤波融合精度。
[0094]
进一步，步骤5所述数据融合中所述设置自适应门限的具体内容和方法步骤是：
[0095]
a)选取重要性采样过程后，离散粒子样本集中权值最大的粒子xc作为聚类中心，并计算其它粒子i与其之间的马氏距离di，所述di的计算公式如下：
[0096][0097]
式中，i＝1,2,
…
,n，为粒子i概率密度，s为协方差矩阵。
[0098]
b)计算聚类单元中有效粒子样本数ne，所述ne的计算公式如下：
[0099][0100]
式中，n为粒子样本数量，为粒子概率密度协方差。
[0101]
c)构造门限t，所述t的计算公式如下：
[0102][0103]
式中，t0为门限初值，ke为比例系数，r为分类次数。
[0104]
d)将步骤b)所得有效粒子样本数ne代入门限t中，构建自适应门限tc，所述tc的计算公式如下：
[0105][0106]
e)将di与自适应门限tc进行比较。若di小于tc，则将粒子归入与其概率质量相关的组件单元中；若di大于tc，则跳过该粒子，对其他粒子进行聚类。
[0107]
f)从剩余粒子样本中选取权值最大粒子作为聚类中心，重复执行步骤e)，直至聚类结束。
[0108]
g)依据聚类后的组件单元，代入所构造的粒子集合连续概率密度函数所述构造的计算公式如下：：
[0109][0110]
式中，βi为相似组件单元分量i的概率质量，γi为组件单元分量i的均值，pi为组件单元分量i的协方差，i＝1,2,
…
,n。
[0111]
概率密度越大的粒子，其在步骤a)中计算所得di越小。因自适应门限tc与粒子概率密度协方差成正比，则在步骤e)中与di相比较的tc可根据di大小相应地调节高低，因此在对高斯混合进行聚类时，无需不断调整门限来对相似单元进行合并，减少了对剩余粒子样本重复聚类次数，提高了聚类效率。此外，因所构造的自适应门限与粒子概率质量相关联，则在对粒子集进行相似单元合并时，可依据门限高低，将权值大小相似的粒子聚类到一个组件单元中，确保同一个组件单元内粒子集之间权值差值最小，使得由步骤(e)所构造的所构成的加权点集混合高斯分布在对相似单元进行合并后更逼近后验概率密度函数，提高重采样过程中采样精度。
[0112]
进一步，步骤5所述数据融合中所述置信因子相关联与分层采样比例容量，进而对无人船航行轨迹定位数据进行融合滤波，输出无人船航行轨迹定位信息的具体内容和方法步骤是：
[0113]
(一)根据分层理论，将连续概率密度函数分为l层，每一层的概率密度函数为p(x)，并依据其概率质量大小将组层分为一组权值优势层和两组劣势层，且分别定义为la，lb,，lc。
[0114]
(二)分别设置la，lb，lc层粒子数的比例容量为：n/4、n/3、n/3。
[0115]
(三)将置信因子代入权值优化组合计算。
[0116]
(四)对lb，lc层中权值小于均值的粒子进行权值优化组合，获得优化后粒子权值并对样本数据进行分层采样，所述的计算公式如下：
[0117][0118][0119]
(五)获取步骤(四)所获得的多传感器数据融合采样结果。
[0120]
(六)将步骤(五)所获得的数据融合采样结果，以日志文件的形式从无人船所搭载的盲节点上输出。
[0121]
(七)安置在河道边的pc端协调器节点通过与无人船盲节点进行组网，来实时获取步骤(七)盲节点所输出的无人船位置信息，从而实现对无人船航行轨迹的定位。
[0122]
对所构造的加权点集连续概率密度函数的采样样本进行分层，并设置每个采样层的比例容量，确保了分层中采样粒子数分配合理，接着对la层组进行采样，并对lb，lc层组中的粒子权值进行优化组合，增加其概率质量，同时将无人船多传感器定位系统所采集的测量数据置信因子与多传感器数据算法中分层采样相关联，使得在利用新的门限分层粒子滤波算法对无人船多传感器测量数据进行数据融合时，置信因子越大的传感器测量值优先被采样融合。
[0123]
本发明具有以下优点和有益效果：
[0124]
(1)本发明利用无人船多传感器所采集的定位数据进行信息互补，有效克服了因环境信号遮蔽所造成的传感器信号丢失问题，保证了传感器采集数据的有效性。
[0125]
(2)本发明通过对多传感器定位系统所采集的无人船位置信息进行一致性检验，以及对故障数据进行加权补偿，在提高数据融合算法容错性能的同时，保证了基本粒子滤波算法所进行采样的粒子集合样本的可信度。
[0126]
(3)依据基本粒子滤波所采用的蒙特卡罗方法来求解贝叶斯估计中的积分运算，来消除环境噪声干扰，保证了对采样样本数据集的处理精度，进而提高无人船多传感器测量数据的可靠性。
[0127]
(4)本发明在数据融合算法的重要性采样过程中将当前量测信息融入粒子集合建议分布中，使得建议分布更加贴近真实后验概率密度，提高了算法估计性能；同时在对高斯混合单元的聚类分析中构造自适应门限，将离散粒子样本合并为相似组件单元，降低了聚类运算复杂度，提高了系统对信号处理实时性及运算效率。
[0128]
(5)本发明对所构造的加权点集连续概率密度函数的采样样本进行分层，并设置每个采样层的比例容量，确保了分层中采样粒子数分配合理，接着对la层组进行采样，并对
lb，lc层组中的粒子权值进行优化组合，增加其概率质量，同时将无人船多传感器定位系统所采集的测量数据置信因子与多传感器数据算法中分层采样相关联，使得在利用新的门限分层粒子滤波算法对无人船多传感器测量数据进行数据融合时，优先采样融合置信因子大的传感器测量值，进而增大整个信息样本的参考价值，最终提高数据融合算法对无人船位置数据的融合定位精度。
附图说明
[0129]
图1本发明的一种基于多传感器数据融合的无人船定位方法总流程图；
[0130]
图2无人船多传感平台搭建及预处理示意图；
[0131]
图3置信度判定流程图；
[0132]
图4一致性检验及加权补偿流程图；
[0133]
图5基本粒子滤波数据增强处理流程图；
[0134]
图6各时刻后验概率分布图；
[0135]
图7构造高斯混合模型流程图；
[0136]
图8采样时刻k＝30时，采样粒子密度分布图；
[0137]
图9设置自适应门限及与置信因子相关联的分层采样比例容量流程图；
[0138]
图10(a)表示算法对比均方根测试图，
[0139]
图10(b)表示算法对比标准差测试图；
[0140]
图11河道测试图；
[0141]
图12无人船航行轨迹测试实验图。
具体实施方式
[0142]
为使本发明实施例的目的和技术方案更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0143]
如图1所示，本发明的一种基于多传感器数据融合的无人船定位方法，首先对无人船多传感器定位系统所采集的定位数据进行预处理；然后对无人船定位数据进行置信距离判定并赋予相应置信因子；同时对定位数据进行故障检验及加权补偿；接着对定位数据进行滤波处理实现数据增强；最后利用新的门限分层粒子滤波算法进行多传感器数据融合滤波输出，实现对无人船航行轨迹的精确定位。
[0144]
具体采用如下步骤：
[0145]
(1)数据预处理；
[0146]
(2)数据置信度判定赋值；
[0147]
(3)数据故障检验补偿；
[0148]
(4)数据增强；
[0149]
(5)数据融合。
[0150]
如图2所示，所述对无人船多传感器定位系统所采集的无人船定位数据进行预处理的方法，包括如下步骤：
[0151]
(a)根据定位系统传感器所采集的无人船航行轨迹经纬度信息，对其进行时空基准统一；
[0152]
(b)依据无人船航行河道，构建满足定位系统多节点有效连接的通讯环境，使得位于无人船上盲节点处于4个发射信号强度和位置坐标已知的信号接收节点网络内，以此计算采集无人船在河道航行时位置坐标；
[0153]
(c)根据高斯-克吕格投影原理，对多传感器定位系统坐标进行转换统一。
[0154]
利用无人船多传感器所采集的定位数据进行信息互补，有效克服了因环境信号遮蔽所造成的传感器信号丢失问题，保证了传感器采集数据的有效性。
[0155]
如图3所示，所述根据多传感器所采集数据进行置信度判定并进行置信因子赋值的方法，包括如下步骤：
[0156]
1)依据定位系统所采集的测量数据，构建多传感器测量模型pi(x)，并计算传感器数据间的置信距离di；所述pi(x)、di计算如下：
[0157][0158]
式中，xi为第i个传感器的测量值，μ为测量特征的真值，θi为第i个传感器信息测量精度，σi为第i个传感器信息测量误差。
[0159][0160]
式中，x
′i、x
″i分别为多传感器i时刻所采集测量值，τ
′i、τ
″i为相应传感器测量方差，为测量方差均值，z为服从标准正态分布的随机变量，i＝1,2,
…
,n。
[0161]
2)依据高斯概率模型将步骤1)所得di，改写为传感器测量数据间概率意义上的度量pr(zi)，并设定传感器支持度置信水平，以此来判别不同传感器信息的可信度；所述pr(zi)计算如下：
[0162][0163]
式中，zi为多传感器的测量数据，ε为置信水平，k为采样样本概率区间变量系数。
[0164]
3)依据传感器置信距离判别多传感器所采集的位置信息可信度大小，并根据信息可信度大小将定位数据划分入不同置信区间；
[0165]
4)构建k时刻多传感器所采集测量信息的动态支持度因子βi(k)与高斯概率测量模型pi(x)的范数方程；所述βi(k)计算如下：
[0166][0167]
式中，||.||f为frobenius范数，k＝1,2,
…
,t，i＝1,2,
…
,n。
[0168]
5)根据步骤4)所得多传感器的动态支持度因子βi(k)，计算系统测量误差wi；所述wi计算如下：
[0169]
wi＝z
i-aβi(k)
[0170]
式中，a为状态转移矩阵，zi为无人船多传感器定位系统测量值，i＝1,2,
…
,n。
[0171]
6)设系统测量误差wi的方差为利用来对测量信息赋予相应的置信因子所述置信因子计算如下：
[0172][0173]
式中，为传感器测量数据的置信因子，i＝1,2,
…
,n。
[0174]
依据测量数据间概率意义上的信息度量值大小，将所判定的定位数据划分入不同置信区间，使得可信数据向置信度较高区域靠拢；同时，所设定的置信水平ε与传感器测量方差均值相对应，其用来表示所采集的无人船多传感器定位系统的测量数据落入区间相对应，其用来表示所采集的无人船多传感器定位系统的测量数据落入区间的概率，因此测量信息的方差不同，相应的对无人船定位信息的置信区间分类也是变化的，可根据传感器测量方差大小进行调节，提高了对传感器测量数据的采样可靠性。
[0175]
如图4所示，所述通过对多传感器数据进行一致性检验，来对多传感器所采集的不一致故障数据进行加权补偿处理的方法，包括如下步骤：
[0176]
(i)将无人船定位系统所获得的传感器测量数据做算术平均，求出算术平均值所述计算如下:
[0177][0178]
式中，xi为传感器测量信息，i＝1,2,
…
,n。
[0179]
(ii)利用步骤(i)所求的传感器测量数据算术平均值与定位系统后续采样值xh作差；
[0180]
(iii)设定系统要求误差，若传感器测量数据算术平均值与定位系统后续采样值差值小于系统要求误差，说明多传感器定位系统所采集的无人船位置数据具有一致性，为可信数据；若差值大于系统要求误差，则需对采样数据进行方差加权补偿，以此满足基本粒子滤波算法对数据样本的采样需求；
[0181]
(iv)取多传感器测量数据算术平均值作为真值的无偏估计值，来表示无人船多传感器定位系统在同一空间的不同位置对无人船航行轨迹进行测量时，第i个传感器系统的信息测量方差所述计算如下：
[0182][0183]
(v)依据无人船多传感器定位系统对无人船进行m次测量所记录的数据集，记第i个传感器的第j次测量数据为x
ij
，将x
ij
替换步骤(iv)中感器测量信息xi，以此获得经多次测量所获得的数据集信息方差所述计算如下：
[0184][0185]
式中，i＝1，2，
…
，n，j＝1,2,
…
,m。
[0186]
(vi)根据步骤(v)所计算的多传感器所采集的位置信息数据集方差，来定义步骤(iii)中不一致故障数据的融合权值κi；所述κi计算如下：
[0187][0188]
(vii)根据所求的融合权值κi，对不一致故障数据进行方差加权融合，以此获得满足基本粒子滤波处理要求的传感器测量数据所述计算如下：
[0189][0190]
通过对多传感器定位系统所采集的无人船位置信息进行一致性检验，以及对故障数据进行加权补偿，在提高数据融合算法容错性能的同时，保证了基本粒子滤波算法所进行采样的粒子集合样本的可信度。
[0191]
如图5所示，所述基于基本粒子滤波算法，分别对经预处理及检验补偿后的多传感器数据进行滤波处理的方法，采用如下步骤：
[0192]
(i)建立无人船多传感器定位系统的基本粒子滤波模型，将经一致性检验及方差加权后的无人船位置信息代入系统模型；传感器系统的状态和测量模型由下式概述：
[0193][0194]
式中，xk为传感器系统k时刻的位置预测值，xh为传感器后续采样值，方差加权后的传感器测量值，zk为k时刻的无人船位置测量值，λk为估计噪声，νk为测量噪声；
[0195]
(ii)粒子集合样本初始化，从先验密度p(x0)中随机采样生成初始化粒子集，所有粒子权值为1/n，
[0196]
(iii)从重要性密度函数中随机抽取n个粒子样本；
[0197]
(iv)计算采样粒子的权值并更新；所述计算如下：
[0198][0199]
式中，为采样粒子权值，i＝1,2,
…
n；
[0200]
(v)归一化重要性权重；
[0201]
(vi)计算基本粒子滤波算法中有效粒子数n
eff
，并与阈值n
th
比较，若n
eff
《n
th
则进行重采样；所述n
eff
计算如下：
[0202][0203]
(vii)状态输出，得到无人船多传感器定位系统所采集的测量信息经滤波后的局部估计及协方差阵所述计算如下：
[0204][0205][0206]
粒子传播是通过对无人船多传感器定位系统状态转移模型pz(xk|x
k-1
,zk)进行采样后，来生成新粒子状态xk，其中x
k-1
为上一步重采样后粒子状态，zk为传感器系统观测数据。利用基本粒子滤波算法对无人船多传感器定位系统测量数据进行滤波处理实现数据增强，降低了环境噪声对测量数据的干扰，保证了对采样样本数据集的处理精度，进而提高无人船多传感器测量数据的可靠性。
[0207]
如图6、图7所示，所述构造高斯混合模型的方法，采用如下步骤：
[0208]
(a)提取经基本粒子滤波算法处理后的多传感器数据集，并计算sigma点集所述计算如下：
[0209][0210]
式中，为无迹变换后的sigma点集，na为sigma点的维数，λ为尺度参数；
[0211]
(b)在所获得的sigma采样点集中融入最新的量测信息，并对系统状态和协方差进行更新；所述计算如下：
[0212][0213][0214]
式中，kk为卡尔曼增益，zk为量测信息，为加权sigma点集量测的协方差；
[0215]
(c)利用步骤(b)所获得的多传感器定位系统状态和协方差来获得更接近目标概率函数的建议分布并从所构造的建议分布中采样；
[0216]
(d)依据高斯混合模型，生成时间步长为k的后验概率密度函数p(xk|z
1:k
)；所述p(xk|z
1:k
)表示如下：
[0217][0218]
式中，n(xk|mi,vi)为混合高斯模型中的第i个分量，c(k)为离散样本的组件单元数量，ξ为离散点组件权重；
[0219]
(e)将由步骤(c)所采样的离散采样点及其所对应权重融入步骤(d)高斯混合分量组件单元中，并利用构造好的连续后验概率密度函数p(xk|z
1:k
)对离散粒子进行重采样；所述p(xk|z
1:k
)表示如下：
[0220][0221]
其中：
[0222][0223][0224][0225]
式中，p(k)为离散粒子滤波分布的协方差，为离散粒子滤波分布的均值，h为标准化常量，n
x
为粒子分布维数；
[0226]
(f)采用聚类分析来对步骤(e)连续后验概率密度函数p(xk|z
1:k
)中高斯混合相似单元进行合并处理。
[0227]
在新的门限分层粒子滤波算法的重要性采样过程中将当前量测信息融入粒子集合建议分布中，使得建议分布更加贴近真实后验概率密度，提高了算法估计性能。由图6可看出所构造的加权高斯混合分布函数在各时刻都满足连续多峰正态分布，说明其由相似组件单元所构成的加权点集混合高斯分布逼近后验概率密度函数，并且函数峰值分布集中，说明粒子样本有效表示了重采样后概率分布特性。
[0228]
如图8、图9所示，所述设置自适应门限及与置信因子相关联的分层采样比例容量，进而对无人船航行轨迹定位数据进行融合滤波，输出无人船航行轨迹定位信息的方法，具体步骤如下：
[0229]
a)选取重要性采样过程后，离散粒子样本集中权值最大的粒子xc作为聚类中心，并计算其它粒子i与其之间的马氏距离di，所述di表示如下：
[0230][0231]
式中，i＝1,2,
…
,n，为粒子i概率密度，s为协方差矩阵。
[0232]
b)计算聚类单元中有效粒子样本数ne，所述ne计算如下：
[0233][0234]
式中，n为粒子样本数量，为粒子概率密度协方差。
[0235]
c)构造门限t，所述t表示如下：
[0236]
[0237]
式中，t0为门限初值，ke为比例系数，r为分类次数。
[0238]
d)将步骤b)所得有效粒子样本数ne代入门限t中，构建自适应门限tc，所述tc表示如下：
[0239][0240]
e)将di与自适应门限tc进行比较。若di小于tc，则将粒子归入与其概率质量相关的组件单元中；若di大于tc，则跳过该粒子，对其他粒子进行聚类。
[0241]
f)从剩余粒子样本中选取权值最大粒子作为聚类中心，重复执行步骤e)，直至聚类结束。
[0242]
g)依据聚类后的组件单元，代入所构造的粒子集合连续概率密度函数所述构造如下：
[0243][0244]
式中，βi为相似组件单元分量i的概率质量，γi为组件单元分量i的均值，pi为组件单元分量i的协方差，i＝1,2,
…
,n。
[0245]
h)根据分层理论，将连续概率密度函数分为l层，每一层的概率密度函数为p(x)，并依据其概率质量大小将组层分为一组权值优势层和两组劣势层，且分别定义为la，lb,，lc。
[0246]
i)分别设置la，lb，lc层粒子数的比例容量为：n/4、n/3、n/3。
[0247]
j)将置信因子代入权值优化组合计算。
[0248]
k)对lb，lc层中权值小于均值的粒子进行权值优化组合，获得优化后粒子权值并对样本数据进行分层采样，所述计算如下：
[0249][0250][0251]
l)获取步骤(k)所获得的多传感器数据融合采样结果。
[0252]
m)将步骤(l)所获得的数据融合采样结果，以日志文件的形式从无人船所搭载的盲节点上输出。
[0253]
n)安置在河道边的pc端协调器节点通过与无人船盲节点进行组网，来实时获取步骤(u)盲节点所输出的无人船位置信息，从而实现对无人船航行轨迹的定位。
[0254]
对所构造的加权点集连续概率密度函数的采样样本进行分层，并设置每个采样层的比例容量，确保了分层中采样粒子数分配合理，接着对la层组进行采样，并对lb，lc层组中
的粒子权值进行优化组合，增加其概率质量，同时将无人船多传感器定位系统所采集的测量数据置信因子与多传感器数据算法中分层采样相关联，使得在利用新的门限分层粒子滤波算法对无人船多传感器测量数据进行数据融合时，置信因子越大的传感器测量值优先被采样融合，使得整个信息样本的参考价值增大。由图8可看出，当时刻k＝30时，采样点集中在连续概率密度函数最值处，说明通过将传感器数据置信因子代入分层采样计算，使得在对连续概率密度函数进行重采样时，保证了采样点集中于概率密度函数值较大处，提高了整个信息样本的参考价值，进而保证新的门限分层粒子滤波算法对测量数据的融合定位精度。
[0255]
为了验证本发明新的门限分层粒子滤波算法的优化性能，文中针对计算机仿真测试系统模型，围绕均方根误差rmse、标准差std进行了30次独立测试，并将测试结果与扩展卡尔曼滤波(extended kalman filter,ekf)
[1]
、无迹卡尔曼滤波(unscented kalman filter,ukf)
[2]
、无迹粒子滤波(unscented particle filter,upf)
[3]
和基本粒子滤波bpf
[4]
进行了对比，以此来验证滤波算法改进步骤的有效性。五种算法的仿真时间步长k＝50，时间间隔δt＝1，待比较算法的参数取自对应参考文献。
[0256]
附计算机仿真测试模型：
[0257][0258]
附参考文献：
[0259]
[1]贺军义,李男男,安葳鹏.改进的扩展卡尔曼滤波算法研究[j].测控技术,2018,37(12):102-106.
[0260]
[2]李兴佳,李建芬,朱敏,等.基于无迹卡尔曼滤波的定位融合与校验算法研究[j].汽车工程,2021,43(6):825-832.
[0261]
[3]武斌,田清.改进无迹粒子滤波的室内移动目标定位算法[j].传感器与微系统,2021,40(3):153-156,160.
[0262]
[4]高怡,毛艳慧,杨一.人工鱼群粒子滤波算法[j].现代电子技术,2021,44(16):170-174.
[0263]
由表一、图10(a)、图10(b)可看出，首先无论是rmse均值还是最大值，文中tlpf都小于其他四种算法，说明了本专利新的门限分层粒子滤波算法的滤波精度是最高的，这主要是因为文中在高斯混合中通过构造自适应门限来聚类离散粒子，并对劣势层中粒子进行权值优化组合，提高了粒子样本的多样性。从std均值以及最大值来看，相比于其它四种算法，tlpf的值也是最小，说明了tlpf的滤波稳定性也是最好的，这主要得益于在重要性采样过程中对重要性函数的选取，通过无迹变换将最新量测数据融入重要性函数，提高了粒子样本可信度，以及在重采样过程中对劣势层中粒子进行重新分层采样计算，将更多粒子样本代入采样过程，从而保证了粒子样本集的多样性。
[0264]
为了验证本发明的多传感器数据融合算法的优化性能，开展了基于无人船平台的多传感系统算法对比定位测试，分别选取实验河道多传感器系统所采集的100组定位数据，利用均方根误差rmse、标准差std作为实验测试结果性能指标，并将测试结果与扩展卡尔曼滤波、无迹卡尔曼滤波、无迹粒子滤波和基本粒子滤波进行了对比。图4所示为各算法对无
人船航行定位轨迹数据所进行的滤波融合结果。
[0265]
由表二、图11、图12可以看出，当无人船航行于测试河道时，由于树木茂密、河道狭长，对传感器信号传输具有一定的环境扰动影响，使得各数据融合算法的滤波定位结果在一些地方严重偏离了无人船所航行的真实路径，且在路径转弯处定位结果易发生较大跳变，逐渐偏离无人船航行真实轨迹，降低了定位结果的可靠性，随着时间推移，定位结果误差越来越大。但本发明的多传感器数据融合算法的定位精度高于任何一个算法的融合定位精度，这主要由于本发明首先通过一致性检验对无人船多传感器测量数据进行故障检验，并对不一致数据进行加权补偿处理，提高其可信度，使得基本粒子滤波在对数据样本进行数据增强时，具有更多更可靠的采样样本，提高了数据融合算法处理精度及容错性能；其次，通过置信度检验对无人船多传感器定位系统所采集的数据进行置信检验，并依据置信距离大小赋予相应定位数据置信因子，并使其与后续传感器数据融合算法分层采样操作步骤相关联；最后利用新的门限分层粒子滤波算法，通过构造高斯混合连续概率密度函数，以及设置自适应门限提高聚类合并效率，提高了数据融合算法对定位数据处理的实时性，并将置信因子关于关联与分层采样权值计算，使得置信因子较高的粒子样本优先被采样，提高了数据融合算法的定位精度。相较于其它四种数据融合算法，本发明的多传感器数据融合算法平均定位误差下降了47％，充分保证了对无人船航行轨迹的定位精度。
[0266]
表一:
[0267][0268]
表二：
[0269][0270]
以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。