一种基于信息熵的多领航auv协同导航方法
阅读说明：本技术 一种基于信息熵的多领航auv协同导航方法 (Multi-pilot AUV (autonomous Underwater vehicle) collaborative navigation method based on information entropy ) 是由 张国成 张红星 孙玉山 薛源 张强 张力文 于 2021-07-13 设计创作，主要内容包括：本发明公开了一种基于信息熵的多领航AUV协同导航方法,具体涉及一种基于信息熵的多领航AUV协同导航方法。本发明将从AUV与各个主AUV的相对距离信息分别作为子观测模型,建立子滤波器,最终采用信息熵的方法将各子滤波器的结果进行融合,并更新各个子滤波器的误差协方差矩阵,以此来提高从AUV的导航精度。本发明将各主AUV与从AUV分别进行滤波,形成子滤波器,降低观测量的维数,雅可比矩阵计算简单,具有更好的实时性。相比于传统的方法,导航精度以及稳定性都得到了提高,能有效抑制滤波发散,具有更强的鲁棒性。(The invention discloses a multi-pilot AUV collaborative navigation method based on information entropy, and particularly relates to a multi-pilot AUV collaborative navigation method based on information entropy. The method takes the relative distance information of the slave AUV and each main AUV as sub-observation models respectively, establishes sub-filters, finally fuses the results of each sub-filter by adopting an information entropy method, and updates the error covariance matrix of each sub-filter, thereby improving the navigation accuracy of the slave AUV. The method filters each main AUV and each slave AUV respectively to form the sub-filters, reduces the dimension of observed quantity, and has simple calculation of the Jacobian matrix and better real-time property. Compared with the traditional method, the navigation precision and stability are improved, filtering divergence can be effectively inhibited, and the robustness is stronger.)
技术领域
本发明涉及一种基于信息熵的多领航AUV协同导航方法，属于水下航行器导航技术领域。
背景技术
高精度的水下导航是AUV完成任务的关键所在，在中层水域多AUV最有效的导航方式之一为协同导航。协同导航可具体分为两种形式：1)并行式，系统中每个AUV的功能和结构都相同，各自有自己完备且精度较高的导航系统，可独立完成高精度导航，并且通过水声通信，获取系统周围其他AUV的位置信息；2)主从式，系统中只有少数的领航AUV配备高精度的导航设备，而从AUV的导航设备精度低，从AUV通过获取与主AUV之间的位置关系来提高自身的导航精度，即通过水声通信来确定自身在系统中的位置。并行式系统结构简单，由于每个AUV都具有同等的地位，配备高精度的导航设备，导致系统成本大大增加，主从式由于从AUV不需要配备高精度导航设备，兼顾了导航精度和成本，成为多AUV协同导航研究的主要方向。主从式协同导航系统中，由于从AUV需要与各个主AUV建立通信，获得与各个主AUV之间的相对位置信息，若同时把从AUV与各个主AUV之间的相对距离统一作为观测模型采用EKF进行滤波，由于系统模型和观测模型的非线性，以及环境噪声的复杂性和观测状态维数的增加，势必导致滤波效果不理想或者直接发散，为了解决此问题，本专利提出，将从AUV与各个主AUV的相对距离信息分别作为子观测模型，建立子滤波器，最终采用信息熵的方法将各子滤波器的结果进行融合，并更新各个子滤波器的误差协方差矩阵，以此来提高从AUV的导航精度。
发明内容
本发明的目的在于提出一种基于信息熵的多领航AUV协同导航方法，通过将从AUV与各个主AUV的相对距离信息分别作为子观测模型，建立子滤波器，最终采用信息熵的方法将各子滤波器的结果进行融合，并更新各个子滤波器的误差协方差矩阵，从而解决现有技术存在的问题。
一种基于信息熵的多领航AUV协同导航方法，所述多领航AUV协同导航方法包括以下步骤：
步骤一、各个主AUV通过水声通信的方式与从AUV建立连接，分别将自身的位置信息和速度信息发送给从AUV；
步骤二、建立从AUV的运动状态空间模型；
步骤三、根据步骤二建立的从AUV运动状态空间模型，把各个主AUV与从AUV之间的相对距离信息作为量测信息，建立量测方程；
步骤四、根据步骤二和步骤三建立的从AUV状态空间模型和量测方程，各个主AUV与从AUV通过EKF估算出从AUV的位置信息，形成子滤波器；
步骤五、根据步骤四建立的子滤波器，用信息熵计算各个子滤波器导航信息融合权重，将各子滤波器的导航信息进行融合，得到从AUV更为准确的位置信息。
进一步的，在步骤二中，所述从AUV的运动状态空间模型包括状态方程，所述状态方程包括：
从AUV在导航坐标系下的三个位置状态为(x,y,z)，分别表示以导航原点为参考点的AUV水平面位置和深度，此外，导航坐标系下与从AUV运动相关的两个姿态角为分别表示从AUV的偏航角、俯仰角，
定义tk时刻从AUV的运动状态为则从AUV建立如下采样周期为T的空间离散状态方程：
其中V(k)、z(k)、ω(k)、θ(k)，分别为从AUV的前向速度、航行深度、偏航角速度和俯仰角，
近似认为cos(θ)≈1，所以定义从AUV平面运动状态量为X(k)＝[x(k)y(k)z(k)]T，分别表示从AUV在导航坐标系下的位置座标和偏航角，则采样周期为T的二维离散状态方程为：
其中，输入V(k)和ω(k)分别为从AUV的前向速度和偏航角速度，上述为理想情况下的状态方程，而实际模型中的传感器量测输入均受到高斯白噪声的干扰，则量测输入、真实输入、传感器噪声分别为：
且满足噪声协方差为：
则从AUV的平面状态方程简写为：
进一步的，在步骤三中，具体的，
从AUV在tk时刻的位置向量为第i个主AUV在tk时刻的位置向量为将主AUV与从AUV之间的距离的平方作为量测量构建量测方程：
其中，为量测噪声。
进一步的，在步骤四中，具体的，
状态向量X(k)＝[x(k) y(k) z(k)]T，分别表示从AUV在导航坐标系下的位置座标和偏航角，
第i个子滤波器的量测方程表示如下：
其中，为量测噪声，假设为相互独立不相关的零均值高斯白噪声，其方差为：
则第i个子滤波器量测方程的Jacobian矩阵为：
其中，分别表示从AUV在导航坐标系下tk+1时刻的位置座标，分别表示第i个主AUV在导航坐标系下tk+1时刻的位置座标，
第i个子滤波器的状态方程为：
其中，
由EKF算法得到第i个子滤波器一步状态预测为：
一步状态预测的协方差为：
其中，是f关于的Jacobian矩阵，是f关于的Jacobian矩阵，是第i个子滤波器的过程噪声方差矩阵，
第i个子滤波器的协方差矩阵更新：
第i个子滤波器的滤波增益更新为：
状态更新为：
进一步的，在步骤五中，具体的，
熵时信息论中量化信息量大小的指标，描述信息量大小的离散变量为：
若p(x)的概率密度函数是服从高斯分布的，则上式变为：
为确定各个子滤波器分量融合的权重，计算
SH(Pi)＝σi·H(Pi)
其中σi为第i个子滤波器的误差协方差矩阵中的位置参数的标准差，H(Pi)为第i个子滤波器的的熵值，
SH(Pi)值越小，代表子系统的可靠性越高，将tk时刻的第i个子滤波器的融合权重计算公式定义为：
其中，为归一化因子，l为子滤波器的数量，
各个子滤波器的状态空间模型与量测模型的EKF滤波结果服从高斯分布，融合各个高斯分量，通过计算各个分量信息熵的值，量化各个子滤波器的可靠性，从而求得融合权重，最终得到从AUV最佳的状态估计值。具体过程如下：
步骤五一、根据各个子滤波器的状态方程和观测方程计算tk时刻状态估计值与相应的误差协方差矩阵
步骤五二、计算各子滤波器tk时刻的信息熵H(X)，利用H(X)及协方差矩阵中位置参数的标准差值计算
步骤五三、将代入中计算出tk时刻各个子滤波器的融合权重；
步骤五四、tk时刻最佳状态估计值其中l为子滤波器的数量；
步骤五五、更新各子滤波器的误差协方差矩阵：
步骤五六、将更新的误差协方差矩阵用于步骤五一，得到tk+1时刻各子滤波器状态估计值与相应的误差协方差矩阵重复步骤五一至步骤五六，直到计算终止。
本发明的有以下有益效果：针对多主AUV协同导航过程中，从AUV同时接收各个主AUV的导航信息，并将自身航位推算状态值于其做差，将差值作为扩展卡尔曼滤波的观测量进行滤波估计，由于观测量维数的增加，雅可比矩阵计算量增加，可能会滤波发散的问题。本发明将各主AUV与从AUV分别进行滤波，形成子滤波器，降低观测量的维数，雅可比矩阵计算简单，具有更好的实时性。由于水下复杂的环境，针对状态方程和观测方程噪声增大易引起滤波发散的问题，基于信息熵建立信息量与不确定度之间的关系，通过计算各个子滤波器的融合权重，融合各子滤波器的状态估计值得到从AUV状态一个最优估计值。相比于传统的方法，导航精度以及稳定性都得到了提高，能有效抑制滤波发散，具有更强的鲁棒性。
附图说明
图1为本发明的一种基于信息熵的多领航AUV协同导航方法的方法流程图；
图2为从AUV三维运动示意图；
图3为从AUV二维运动示意图；
图4为主从AUV水声通信示意图。
具体实施方式
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参照图1-图4所示，本发明提出了一种基于信息熵的多领航AUV协同导航方法，所述多领航AUV协同导航方法包括以下步骤：
步骤一、各个主AUV通过水声通信的方式与从AUV建立连接，分别将自身的位置信息和速度信息发送给从AUV；
步骤二、建立从AUV的运动状态空间模型；
步骤三、根据步骤二建立的从AUV运动状态空间模型，把各个主AUV与从AUV之间的相对距离信息作为量测信息，建立量测方程；
步骤四、根据步骤二和步骤三建立的从AUV状态空间模型和量测方程，各个主AUV与从AUV通过EKF估算出从AUV的位置信息，形成子滤波器；
步骤五、根据步骤四建立的子滤波器，用信息熵计算各个子滤波器导航信息融合权重，将各子滤波器的导航信息进行融合，得到从AUV更为准确的位置信息。
进一步的，在步骤二中，所述从AUV的运动状态空间模型包括状态方程，所述状态方程包括：
从AUV在导航坐标系下的三个位置状态为(x,y,z)，分别表示以导航原点为参考点的AUV水平面位置和深度，此外，导航坐标系下与从AUV运动相关的两个姿态角为分别表示从AUV的偏航角、俯仰角，
定义tk时刻从AUV的运动状态为则从AUV建立如下采样周期为T的空间离散状态方程：
其中V(k)、z(k)、ω(k)、θ(k)，分别为从AUV的前向速度、航行深度、偏航角速度和俯仰角，
由于深度z可由压力深度计直接测得，为简化状态方程，减小递推状态的维数，可以不考虑深度信息，此外，由于从AUV在稳定状态运动时的俯仰角θ较小，可近似认为cos(θ)≈1，所以定义从AUV平面运动状态量为X(k)＝[x(k)y(k)z(k)]T，分别表示从AUV在导航坐标系下的位置座标和偏航角，则采样周期为T的二维离散状态方程为：
其中，输入V(k)和ω(k)分别为从AUV的前向速度和偏航角速度，上述为理想情况下的状态方程，而实际模型中的传感器量测输入均受到高斯白噪声的干扰，则量测输入、真实输入、传感器噪声分别为：
且满足噪声协方差为：
则从AUV的平面状态方程简写为：
其中
进一步的，在步骤三中，具体的，
从AUV在tk时刻的位置向量为第i个主AUV在tk时刻的位置向量为将主AUV与从AUV之间的距离的平方作为量测量构建量测方程：
其中，为量测噪声。
进一步的，在步骤四中，具体的，
所述根据步骤(2)和(3)建立的从AUV状态空间模型和子系统量测方程，各个主AUV与从AUV通过EKF估算出从AUV的位置信息，形成子滤波器；
状态向量X(k)＝[x(k) y(k) z(k)]T，分别表示从AUV在导航坐标系下的位置座标和偏航角，
第i个子滤波器的量测方程表示如下：
其中，为量测噪声，假设为相互独立不相关的零均值高斯白噪声，其方差为：
则第i个子滤波器量测方程的Jacobian矩阵为：
其中，分别表示从AUV在导航坐标系下tk+1时刻的位置座标，分别表示第i个主AUV在导航坐标系下tk+1时刻的位置座标，
第i个子滤波器的状态方程为：
其中，
由EKF算法得到第i个子滤波器一步状态预测为：
一步状态预测的协方差为：
其中，是f关于的Jacobian矩阵，是f关于的Jacobian矩阵，是第i个子滤波器的过程噪声方差矩阵，
第i个子滤波器的协方差矩阵更新：
第i个子滤波器的滤波增益更新为：
状态更新为：
进一步的，在步骤五中，具体的，根据步骤四建立的子滤波器，用信息熵计算各个子滤波器导航信息融合权重，将各子滤波器的导航信息进行融合，得到从AUV更为准确的位置信息；
根据信息论的思想，信息量的大小与不确定性存在相关性，系统包含的信息量越大，不确定性反而越高；反之，若信息量越小则代表系统的不确定性越低。定位过程是各个子滤波器不断获取从AUV位置信息的过程。子滤波器单次所获取的信息量越大，其概率后验密度函数所包含的信息量就越小，不确定性也越小，定位精度越高；反之，如果子滤波器单次获取的信息量小，后验概率密度函数所包含的信息量就越大，不确定性也越大，子系统的可靠性反而更低，融合时权重也应该变低。
熵时信息论中量化信息量大小的指标，描述信息量大小的离散变量为：
若p(x)的概率密度函数是服从高斯分布的，则上式变为：
为确定各个子滤波器分量融合的权重，计算
SH(Pi)＝σi·H(Pi)
其中σi为第i个子滤波器的误差协方差矩阵中的位置参数的标准差，H(Pi)为第i个子滤波器的的熵值，
值越小，代表子系统的可靠性越高，为保持权重的选取与实际的情况相统一，将tk时刻的第i个子滤波器的融合权重计算公式定义为：
其中，为归一化因子，l为子滤波器的数量，
各个子滤波器的状态空间模型与量测模型的EKF滤波结果服从高斯分布，融合各个高斯分量，通过计算各个分量信息熵的值，量化各个子滤波器的可靠性，从而求得融合权重，最终得到从AUV最佳的状态估计值。具体过程如下：
步骤五一、根据各个子滤波器的状态方程和观测方程计算tk时刻状态估计值与相应的误差协方差矩阵
步骤五二、计算各子滤波器tk时刻的信息熵H(X)，利用H(X)及协方差矩阵中位置参数的标准差值计算
步骤五三、将代入中计算出tk时刻各个子滤波器的融合权重；
步骤五四、tk时刻最佳状态估计值其中l为子滤波器的数量；
步骤五五、更新各子滤波器的误差协方差矩阵：
步骤五六、将更新的误差协方差矩阵用于步骤五一，得到tk+1时刻各子滤波器状态估计值与相应的误差协方差矩阵重复步骤五一至步骤五六，直到计算终止。
以上实施示例只是用于帮助理解本发明的方法及其核心思想，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。