【技术实现步骤摘要】
：本专利技术涉及水下定位，具体的说是一种能够有效提高定位精度的基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法。
技术介绍
0、
技术介绍
：
1、水下定位是提供自主水下机器人(autonomous underwater vehicle,auv)的姿态、速度、位置等信息的技术，它是auv顺利完成各种水下任务的前提。基于单信标的主从auv协同工作能够提高工作效率，在水下定位时待定位的从auv缺少外界的辅助定位信息，而此时单信标与主auv可与从auv传输位置、距离等信息，以测距信息作为量测信息进行辅助定位，从而提高从auv的定位精度，单信标辅助的auv协同定位手段能够有效地降低定位误差，适用于水下协同定位体系。
2、目前，扩展卡尔曼滤波和ckf是常用于水下协同定位系统的滤波算法，但水下单信标主从auv协同定位系统为非线性系统，会产生重尾噪声，传统的扩展卡尔曼滤波、ckf等对于重尾噪声和异常值的处理效果不佳，且在基于单信标的主从auv协同定位系统中，设备间的信息传递通常会受到水中复杂环境的影响，从而导致声波传递的量测距离信息出现异常值。
3、gmcc是一种新的最优化准则，是通过引入一个广义高斯密度核函数来对非线性滤波算法的误差进行广义的相关熵的度量，广义高斯核函数具有参数多，变化灵活，能够灵活调试的优点，其在误差最小时，可以取得最大的相关熵，在广义高斯核中，由于包含两个变量误差的偶数高阶矩，而能够更好地处理重尾噪声及较大异常值。它具有更好的鲁棒性和可靠性，能够更好地处理异常值和噪声干扰等问题。p>
技术实现思路
0、
技术实现思路
：
1、本专利技术针对现有技术中存在的缺点和不足，提出了一种利用单信标和主auv的测距信息及位置信息，辅助从auv进行定位，从而有效提高了定位精度的基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法。
2、本专利技术通过以下措施得到：
3、一种基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法，包括以下步骤：
4、步骤1：构建单信标辅助主从auv定位系统的状态方程和观测方程：其中，定义gmcc的代价函数、混合熵、核函数，将单信标与从auv和主auv与从auv之间的信息传输时间作为观测信息，将从auv的横坐标、纵坐标、深度、速度、航向角和水下声速作为状态信息；
5、步骤2：根据单信标辅助主从auv定位系统的状态方程和观测方程，由k时刻的信息进行k+1时刻的一步预测，得到k+1时刻的状态估计值及估计协方差pk+1|k，进行量测步骤中的量测估计更新和状态量测交叉协方差pxz,k+1|k矩阵更新；
6、步骤3：设置迭代所需矩阵的值，并设定迭代初始值以及迭代结束值，获得误差向量和权重矩阵；
7、步骤4：开始迭代至稳定状态，并保留最后一次迭代的状态向量估计值，进行状态向量误差协方差矩阵pk+1|k+1的更新。
8、本专利技术所述的步骤1具体包括以下步骤：
9、步骤1-1：构建代价函数、混合熵、核函数分别如下：
10、jgmcc(x,y)＝e[gα，β(x,y))]＝e[γα,βexp(-λ|e|α)]
11、
12、
13、其中x和y分别为两个输入向量，α为核函数的形状参数，β为核函数的大小参数，λ＝1/βα为核参数，γα，β＝α/[2β·γ(1/α)]为归一化常数，γ(·)为伽马函数，m为混合核函数的个数，n表示输入向量的维度，σi表示核函数不同的权值，xj和yj分别表示x和y的第j个元素，gi(xj,yj)表示第i个核函数。
14、步骤1-2：在k时刻的观测信息zk为：
15、
16、其中，vk是观测噪声，t1是主auv与从auv之间的信号传输时间，t2是单信标与从auv之间的信号传输时间，d1是主auv与从auv之间的距离，d2是单信标与从auv之间的距离，ck是水下声速，xk和yk分别表示k时刻的从auv的横坐标与纵坐标，和分别表示k时刻的主auv的横坐标与纵坐标，x0和y0分别表示信标的横坐标与纵坐标；在k时刻的状态向量xk为：xk＝[x,y,z,v,θ,c]t，其中，x为从auv的横坐标，y为纵坐标，z为深度，v为速度，θ为航向角，c为水下声速；
17、构建状态方程和观测方程：
18、
19、其中，f(·)代表状态转移函数，wk表示过程噪声，h(·)为观测矩阵，vk表示观测噪声。
20、本专利技术所述的步骤2具体包括：
21、步骤2-1：由k时刻的信息进行k+1时刻的一步预测如下式：
22、根据ckf的三阶球面径向容积准则，可以使用若干个容积点来近似非线性系统中的状态向量的均值和协方差，对状态向量进行cholesky分解得到：
23、
24、其中，xi,k+1|k表示状态向量的采样点，i＝1,2,......2nx，nx为状态向量x的维度，sk|k＝chol(pk|k)，chol(·)表示cholesky分解函数，
25、对容积点进行状态预测，由时间更新的一步预测，得k+1时刻的一步预测状态向量为：其中容积点状态预测为：
26、
27、步骤2-2：k时刻的一步预测误差协方差矩阵为：
28、
29、其中，qk表示过程噪声矩阵。
30、同步骤2-1，可得到：
31、
32、其中，表示容积点量测预测，表示量测预测值，pxz,k+1|k表示状态量测交叉协方差矩阵。
33、本专利技术所述步骤3具体包括：
34、步骤3-1：将代价函数对xk+1求导可得到代价函数j在变化量xk+1的极大值点，这个极值点就是最优估计设：
35、gi(xj,yj)＝gi(xj-yj)＝gi(ej)
36、其中，ej＝xj-yj表示误差向量。则：
37、
38、步骤3-2：定义误差为状态误差和量测误差的综合，状态误差为状态值与状态预测值之差量测误差为量测值与经过量测变换矩阵后的状态值之差其中再在综合的误差向量的两边同时乘以其中得到：
39、
40、设上式等号的左侧为ak+1，以及且nz为观测向量的维度。那么标准误差向量可以表示为：ek+1＝ak+1-wk+1xk+1；
41、步骤3-3：设定迭代初始值其中t表示迭代次数，
42、
43、设定一个足够小的迭代结束值ε，如ε＝0.03。
44、本专利技术所述的步骤4具体包括：
45、步骤4-1：开始迭代，更新误差矩阵ek+1＝ak+1-wk+1xk+1|k+1,t-1，更新迭代值t＝t+1；
46、步骤4-2：迭代直到满足下式：
47、
48、使用最后一次的迭代值作为状态估计，可以得到：
49、本文档来自技高网...
【技术保护点】
1.一种基于广义最大相关熵及容积卡尔曼滤波的AUV协同定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的AUV协同定位方法，其特征在于，所述的步骤1具体包括以下步骤：
3.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的AUV协同定位方法，其特征在于，所述的步骤2具体包括：
4.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的AUV协同定位方法，其特征在于，所述步骤3具体包括：步骤3-1：将代价函数对xk+1求导可得到代价函数J在变化量xk+1的极大值点，这个极值点就是最优估计设：Gi(xj,yj)＝Gi(xj-yj)＝Gi(ej)
5.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的AUV协同定位方法，其特征在于，所述的步骤4具体包括：
【技术特征摘要】
1.一种基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法，其特征在于，所述的步骤1具体包括以下步骤：
3.根据权利要求1所述的一种基于广义最大相关熵及容积卡尔曼滤波的auv协同定位方法，其特征在于，所述的步骤2具体包括：
4.根据权利要求...
【专利技术属性】
技术研发人员：赵万龙，刘开源，刘功亮，齐帅杰，
申请(专利权)人：哈尔滨工业大学威海，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。