申请/专利权人：东南大学

申请日：2022-03-09

公开（公告）日：2024-03-19

公开（公告）号：CN114459477B

主分类号：G01C21/16

分类号：G01C21/16;G01C21/20;G01S19/45;G01S19/47

优先权：

专利状态码：有效-授权

法律状态：2024.03.19#授权;2022.05.27#实质审查的生效;2022.05.10#公开

摘要：本发明提供了一种基于改进的PSO‑ANFIS辅助的SINSDVL紧组合导航技术，包括：建立水下潜航过程中SINSDVL紧组合导航系统的状态方程和量测方程；在水面借助GNSS并通过变分贝叶斯卡尔曼滤波算法收集样本数据，获取包含各种异常测量类型的新息、马氏距离、量测噪声协方差矩阵作为ANFIS的输入信息，获取DVL绝对误差作为期望输出；通过粒子群优化算法优化ANFIS模型参数，训练得到更优的ANFIS模型；在水下航行时，采用训练得到的ANFIS模型对DVL的四波束绝对误差进行在线预测；进一步地，基于ANFIS预测结果，通过异常判别机制监测误差的特征变化，有选择的补偿DVL测量值，用于组合导航系统量测更新过程。本发明能够提高复杂水下环境中SINSDVL紧组合导航系统的定位精度和鲁棒性。

主权项：1.一种基于改进的PSO-ANFIS辅助的SINSDVL紧组合导航方法，其特征在于，该方法包括以下步骤：步骤1：根据系统误差方程建立SINSDVL组合导航系统的状态方程，根据SINS解算的导航信息计算的伪量测信息和DVL测量的四波束速度信息、深度计测量的深度信息之差作为量测量建立SINSDVL组合导航系统的量测方程；步骤1中所述建立SINSDVL紧组合导航系统的状态方程和量测方程，具体过程如下：步骤1.1对需要用到的坐标系进行定义：e——地球坐标系：与地球固联，原点位于地心，xe轴穿越本初子午线与赤道交点，ze轴指向北极，ye轴、xe轴、ze轴构成右手坐标系；n——与东-北-天地理坐标系重合的导航坐标系；b——载体坐标系：原点位于运载体中心，zb轴垂直运载体向上，xb轴指向运载体前方，yb轴与xb轴、zb轴构成右手坐标系；d——与DVL的波束中心对齐的正交坐标系，在这里表示beam系；步骤1.2建立SINSDVL紧组合导航系统的状态方程，具体步骤如下：取姿态误差角φ＝[φxφyφz]、速度误差δV＝[δVEδVNδVU]、位置误差δP＝[δLδλδh]、陀螺常值漂移ε和加速度计随机常值误差作为SINS系统的状态量，记为： 其中，φx是东向失准角，φy是北向失准角，φz是天向失准角；δVE是东向速度误差，δVN是北向速度误差，δVU是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；εx是x向陀螺漂移，εy是y向陀螺漂移，εz是z向陀螺漂移；是x向加速度计随机常值误差，是y向加速度计随机常值误差，是z向加速度计随机常值误差；SINS系统的噪声：WSINS＝[ωgxωgyωgzωaxωayωaz]T其中，ωg为陀螺的过程噪声矢量，ωa为加速度计的过程噪声矢量；取DVL四波束速度零偏δb＝[δb1δb2δb3δb4]、刻度系数误差δk作为DVL系统状态变量，记为：XDVL＝[δb1δb2δb3δb4δk]T其中，δb1是beam1速度零偏，δb2是beam2速度零偏，δb3是beam3速度零偏，δb4是beam4速度零偏；DVL系统的噪声为ωd；取深度计的偏置δbps作为深度计的状态变量：XPS＝δbps深度计系统的噪声为ωps；组合导航系统的状态量表示为：X＝[XSINSXDVLXPS]T由导航系统的误差模型得到状态方程： 其中，F是状态转移矩阵，W是系统噪声；步骤1.3建立SINSDVL紧组合导航系统的量测方程，具体步骤如下：在忽略传感器误差的情况下，速度定义如下： 其中，是n系下SINS的速度，是b系下SINS的速度，是beam系下的SINS速度，是DVL测量的速度；它们之间有这样的关系： 其中，表示由n系到b系的转换矩阵，表示由b系到beam系的转换矩阵，其具体表示如下式： 其中bi是根据DVL波束与AUV的几何关系，从Vb到Vd的方向矢量，表示为： 其中α是DVL的波束倾角，是DVL固定的特性；表示为其中用于“+”配置的DVL，而用于“×”配置的DVL；由SINS、DVL和深度计形成的量测量： 其中，是SINS解算出的深度信息，是深度计测量的深度信息；定义深度计的量测误差模型为： 其中，HPS为深度真值；根据上述分析，SINS解算的计算公式为： 定义DVL的量测误差模型为： 根据上述分析，将SINS解算速度转换到beam系得到的计算公式为： 其中，[·×]表示叉乘运算；由此得到组合导航系统的量测方程：Z＝HX+V其中， V＝[ωdωps]T；步骤2：控制AUV的潜行深度到达水面附近，完成导航定位系统的初始对准后，借助GNSS信息和变分贝叶斯卡尔曼滤波算法VBKF辅助收集包含各种异常测量类型的样本数据，用于训练ANFIS；步骤2中所述借助GNSS信息和VBKF算法辅助收集包含各种异常测量类型的样本数据，用于训练ANFIS，具体包括如下步骤：步骤2.1收集与DVL测量信息发生异常最相关的变量，并将其作为特征信息，具体步骤如下：在SINSDVL组合导航系统中，卡尔曼滤波中的新息vk映射系统模型的估计值和DVL实际测量值之间的差异；计算表达式如下： 其中，为时刻k的DVL实际测量信息；是时刻k的一步预测值；我们认为系统模型是精确建模的，因此，当新息突然变大时，DVL测量值是不够精确的，测量误差增大；因此，将新息作为反映DVL测量误差的变量之一，作为ANFIS系统的输入；基于马氏距离构建第二个输入： 其中，λk为根据马氏距离定义的测量异常特征信息；Pkk-1是一步预测方差矩阵；为了方便起见，将λk称为马氏距离；当DVL测量信息异常时，测量噪声会发生变化；使测量噪声协方差矩阵显示这种变化的具体方法；测量噪声协方差矩阵是代表测量异常的特征信息，因此作为第三个输入；步骤2.2收集DVL测量信息的绝对误差作为训练数据的输出，具体步骤如下：模型的学习过程需要精确的理想输出值；在测试和验证过程中，考虑到AUV上装载的传感器，选择准确的测量误差作为训练数据的输出；这种精确的测量误差从AUV装载的PHINS上获得： 式中，为第k时刻DVL测量的绝对误差；PHINS是一种光纤惯性导航传感器，它整合GPS信息，提供最精确的姿态、速度和位置信息；ZGPS，k为根据PHINS提供的姿态、速度信息在第k时刻获得的准确伪测量信息，与求解方式相同；步骤2.3展开基于VBKF的噪声不确定性处理方法，具体步骤如下：为了准确地响应可能出现的异常，系统必须对未知的异常做出特别的响应；考虑到测量信息异常时需要得到相应的马氏距离和Rk，引入VBKF对其变化进行表征；基于VBKF估计未知的以提高ANFIS输入信息的有效性；当系统噪声已知，测量噪声未知时，包含测量噪声的最优贝叶斯滤波总结为预测和更新：pxk，Rk|z1：k-1＝∫pxk|xk-1pRk|Rk-1pxk-1，Rk-1|z1：k-1dxk-1dRk-1pxk，Rk|z1xk∝pzk|xk，zkpxk，Rk|z1：k-1由于上述贝叶斯滤波难以求解，故采用多个已知分布的统一密度qxk，Rk来近似真实后验概率分布函数pxk，Rk|z1：k，即变分贝叶斯算法：pxk，Rk|z1：k≈qxkqRk其中，q·是p·的近似后验概率密度函数；该表达式的最优解可通过最小化真实后验概率密度函数PDF，pxk，Rk|z1：k与近似后验PDFqxkqRk之间的Kullback-Leibler散度来实现；qxkqRk更新为高斯分布和逆Wishart分布： 上式需要定点迭代求解，qi+1Rk可以更新为： 式中，自由度因子和逆尺度矩阵可以表示为： 其中，可以表示为： 期望表示为： qi+1xk更新为： 其中，通过标准卡尔曼滤波可获得均值和协方差矩阵 以上是VBKF的简单推导，通过VBKF获得更加显著的特征信息；步骤2.4展示部分DVL波束缺失时的应用情况，具体步骤如下：基于DVL波束有以下特点的假设： 此时需要满足AUV没有垂直速度，当AUV随洋流上下起伏幅度过大时，该公式不成立；对部分DVL波束缺失时的应用有如下限制条件：1三个波束有效：此时通过上述公式依旧可以得到完整的伪波束信息，能够进行误差预测补偿；2两个正交波束有效：此时通过上述公式依旧可以得到完整的伪波束信息，能够进行误差预测补偿；除此以外的两个平行波束有效或只有一个波束有效以及全部失效这三类情况，DVL信息都有很多缺失，此时可以通过选取不同的特征信息；预测失效的DVL波束，步骤3：将步骤2中收集到的样本数据标准化，并通过粒子群优化算法对样本数据进行处理实现ANFIS模型参数寻优，以此完成ANFIS模型的训练过程；步骤3中所述将步骤2中收集到的样本数据标准化，并通过粒子群优化算法对样本数据进行处理实现ANFIS模型参数寻优，完成ANFIS模型的训练过程，具体步骤如下：步骤3.1对样本数据标准化：对新息进行z-score标准化；对马氏距离和量测噪声协方差矩阵进行min-max标准化，使结果落到[0，1]区间；步骤3.2具体的ANFIS算法过程如下：对于简单的TSK模糊系统模型：函数组合或相互作用的方式称为规则，规则包括前置参数和后置参数；为了实现TSK模糊模型的学习过程，将其转化为自适应神经网络，通过对样本数据进行训练，得到神经网络的隶属函数，即ANFIS；给定前置参数，ANFIS的输出可以表示为后置参数的线性组合；ANFIS共有五层，具体如下；步骤3.2.1第一层，输入变量的隶属函数层：每个节点i都有一个输出函数： 其中in是输入，包括x，y，z；Mi是模糊集，包括Ai，Bi，Ci；是模糊集M的隶属度函数，表示给定的输入in满足M的程度；常用的隶属度函数有很多种，包括钟形、高斯形、三角形；我们选择μM为广义钟形隶属函数： 其中，ai，bi，ci是参数集，这些参数值的变化会改变钟形函数的形状，从而得到不同的隶属度函数；这些参数称为前件参数，在算法的学习过程中会进行自适应调整；步骤3.2.2第二层，规则的强度释放层：每个节点i负责将输入信号相乘： 其中，ωi是每个节点的输出，表示该规则的可信度；步骤3.2.3第三层，所有规则强度的归一化过程：第i个节点计算规则i的释放强度与所有规则释放强度总和的比值： 步骤3.2.4第四层，计算模糊规则的输出：这一层的每个节点i都是一个自适应节点，其输出为： 其中，是第三层的输出，根据后向参数{mi，pi，qi，ri}和隶属函数计算模糊规则的输出；步骤3.2.5第五层，计算输入信号的总输出： ，ANFIS的输出可表示为后向参数的线性组合，前件参数已给定： ANFIS的训练过程首先通过采集的样本数据提取出初始模糊模型，然后从Layer1到Layer5对模型参数进行优化；其中，第一层和第四层的节点参数是自适应的，第二层和第三层的节点参数是固定的；采用PSO算法来优化ANFIS模型的前件和后件参数；从第一层到第四层，后件参数由最小二乘估计计算，计算迭代值和训练数据的期望值之间的误差，在反向传输过程中，将误差信号从输出层传播回输入层，通过PSO调整前件参数；在改变这些参数的过程中，不断修改隶属度函数的形状，以达到在设定周期内输出误差最小的目的；步骤3.3具体的PSO参数寻优算法过程如下：PSO是一种随机优化算法，问题的解称为粒子，可以通过模拟粒子群中个体的合作和竞争模式来检验优化结果；对于n个粒子的群，存在N维搜索空间；在PSO中，为每一个粒子分配一个位置向量xi和一个速度向量vi，相应的目标函数允许粒子获得适应度并从之前的位置和当前的位置xi中选择最佳位置pbest；另外，在一个群中，所有的粒子都有其全局最佳位置gbest；第i个粒子的速度向量和位置向量更新方式表达为： 上述公式中，表示粒子i在d维第k时刻的速度向量；表示粒子i在d维第k时刻的位置向量；ω表示惯性权值；r1和r2表示从0到1的随机数；c1和c2为加速系数，即认知系数c1和社会系数c2；上述速度向量方程主要由认知、社会和惯性成分三个部分组成；其中惯性分量是使粒子在k时刻飞越其路径的前一个运动方向的记忆；认知分量是通过将粒子移动到先前的最佳位置而产生速度分量；社会分量是评估粒子相对于它的邻居和整个粒子群的性能；这三个分量定义了粒子在整个搜索空间中的轨迹；根据步骤2中采集的包括输入数据和目标输出的训练数据，采用PSO算法对ANFIS模型的参数进行优化，获得训练好的ANFIS模型；步骤4：控制AUV潜入水下，采用改进的PSO-ANFIS算法对DVL的四波束绝对误差进行预测，其中需要在线获取特征信息；步骤4中所述控制AUV潜入水下，采用改进的PSO-ANFIS算法对DVL的四波束绝对误差进行预测的具体步骤如下：步骤4.1通过初始信息和IMU信息进行惯导解算；步骤4.2根据DVL更新频率，通过惯导解算信息计算与DVL四波束速度相应的伪量测信息；步骤4.3将上述求解的伪量测信息与DVL四波束测量值的差值作为观测量通过VBKF进行滤波，以此获得与步骤2中一致的三种特征信息vk，λk，作为ANFIS模型的输入；步骤4.4将收集的上述特征信息进行与步骤3中一致的标准化过程，并且通过步骤3中PSO辅助训练的ANFIS模型进行DVL测量信息误差预测；步骤5：基于PSO-ANFIS预测结果，监测误差的特征变化，通过异常判别机制对DVL的实际测量进行补偿，并将补偿后的DVL量测量与SINS计算的伪观测量进行卡尔曼滤波。

全文数据：

权利要求：

百度查询： 东南大学 一种基于改进的PSO-ANFIS辅助的SINS/DVL紧组合导航方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD