申请/专利权人：浙江大学

申请日：2022-01-27

公开（公告）日：2024-04-26

公开（公告）号：CN114485646B

主分类号：G01C21/16

分类号：G01C21/16;G01C21/20;G01S15/58;G01S15/86;G01S19/47;G01S5/18

优先权：

专利状态码：有效-授权

法律状态：2024.04.26#授权;2022.05.31#实质审查的生效;2022.05.13#公开

摘要：本发明公开了一种集成超短基线系统的UUV集群协同定位方法。本发明方法包括：主从UUV内部时钟驯服；主UUV融合INS和DVL数据，周期性将自身位置和位置估计协方差矩阵打包成协同定位数据报文广播给集群中的从UUV；从UUV接收协同定位报文，提取所需的协同定位数据和USBL定位数据；从UUV建立结合状态向量，根据距离测量构建测量方程，利用扩展卡尔曼滤波进行位置更新，得到更新后的从UUV位置；从UUV的USBL阵列与处理模块利用USBL定位数据计算输出USBL定位结果，并利用从UUV定位结果进行第二级滤波处理，完成协同定位。本发明方法采用两级滤波处理设计，提升集群整体的定位性能。

主权项：1.一种集成超短基线系统的UUV集群协同定位方法，其系统由一台主UUV和多台从UUV构成；主UUV搭载多普勒测速仪DVL、高精度惯性导航系统INS、卫星单元模块、水声通信机、主控单元模块、动力系统、数据处理模块和时钟模块；每台从UUV搭载低精度惯性导航系统INS、卫星单元模块、水声通信机、USBL阵列与处理模块、主控单元模块、动力系统、协同定位处理模块和时钟模块；其特征在于，具体方法是：步骤1主UUV和从UUV进行内部时钟驯服，完成驯服后开始水下工作；主、从UUV的卫星单元模块工作，主、从UUV位置信息初始化；步骤2主UUV的数据处理模块融合INS和DVL数据，输出高精度的定位结果，并以固定周期T′生成协同定位数据报文，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vMk、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间下标M表示主UUV，下标k表示在第k定位时隙；协同定位数据报文广播给集群中的从UUV；步骤3从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LS,k、纬度BS，k、深度dS，k、航速vS，k和航向对其中从UUV经度和纬度进行处理，下标S表示从UUV；转换成XY坐标系位置xS-INS,k,yS-INS,k＝LB2XYLS,k,BS,k，LB2XY·,·表示经纬度和XY坐标系的转换运算；将该位置和剩余信息输入给协同定位处理模块；从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻并解析其中包含的数据；对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置xM,k,yM,k＝LB2XYLM,k,BM,k；构建主UUV状态向量和主UUV状态协方差矩阵[·]T表示矩阵或矢量的转置运算；将ψM,k、PM,k、剩余协同定位报文数据和报文到达时刻输入给协同定位处理模块；步骤4从UUV的协同定位处理模块建立状态转移方程和测量方程，并进行第一级滤波处理；4-1建立状态向量和状态转移方程：从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态运动方程表示为：将当前状态和前序缓存的状态相结合，构建组合状态向量,其中N表示缓存的阶数，相应的协方差矩阵为组合后的运动预测模型其中，uS,k表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布；状态方程f·,·是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为其中，B表示输入控制矩阵； 相应的协方差矩阵表示为Qk为引入的状态转移误差，其中，和分别表示测速标准差和测向标准差；当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为Pk-1[1,1]、Pk-1[1,2]、Pk-1[2,1]、Pk-1[2,2]分别表示协方差矩阵Pk-1的左上、右上、左下和右下子矩阵；4-2结合ψM,k信息对状态向量进行增广：从UUV定位与协同定位处理模块在原有状态的基础上增添上接收到的主UUV信息ψM,k，构建增广状态向量相应的协方差矩阵为其中PM,k表示主UUV状态协方差矩阵；4-3建立测量方程：根据解析协同定位数据报文得到的发送时间和本地记录的到达时刻从UUV计算出传播距离其中c为等效声速；在第k个定位时隙，结合步骤4-1输出的中提取出从UUV的XY位置信息和步骤3的深度信息，得到从UUV的空间位置为主UUV的空间信息表示为xM,k,yM,k,dM,k；关于状态向量构建测距方程假设测量受一个零均值的高斯白噪声影响ηS,k，则测量模型表示为测量偏差矩阵为Rk；4-4滤波更新：以扩展卡尔曼滤波为基础，引入新息增益K吸收新息，以更新预测的状态：更新后预测的状态其中，Kk为时隙k的新息增益，表示测距方程，Hk为测量方程的Jacobi矩阵；新息增益通过测量矩阵和测量偏差矩阵得到：由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵；4-5输出利用测距信息得到的定位结果：从中提取出更新的从UUV位置与定位结果xS-INS,k,yS-INS,k进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从提取出从UUV位置将该结果与相应的协方差矩阵进行输出；第一级滤波处理的结果即为从UUV位置ψfirst,k和相应的协方差矩阵Pfirst,k；步骤5从UUV的USBL阵列与处理模块接收到协同定位数据报文后，将USBL定位数据，包括USBL阵元的间距、信号时延差，输入给协同定位处理模块，得到USBL计算结果，并利用从UUV第一级滤波结果进行第二级滤波处理，输出从UUV的协同定位结果；具体是：5-1从UUV协同定位处理模块根据输入的USBL定位数据，得到从UUV的USBL定位结果xUSBL,k,yUSBL,k＝getUSBLxM,k,yM,k,zM,k,rk,d,τx,τy；其中，getUSBL·表示USBL定位运算，d为USBL阵元的间距，τx和τy为USBL的X方向和Y方向阵元接收到的信号到达时延差；从第一级滤波处理的结果ψfirst,k中提取从UUV的位置xfirst,k,yfirst,k，与USBL定位结果xUSBL,k,yUSBL,k进行比较；当两者偏差大于设定阈值时，将当前USBL的定位结果作为野值进行剔除，并将xfirst,k,yfirst,k转换成经纬度坐标Lfirst,k,Bfirst,k＝XY2LBxfirst,k,yfirst,k,作为协同定位结果进行输出，跳过后续步骤，完成协同定位；否则执行5-2；5-2将xUSBL,k,yUSBL,k作为测量值zUSBL,k，相应的测量协方差矩阵RUSBL,k表示为σx,k和σy,k为USBL定位的X、Y方向估计标准差；此时的测量方程关于状态向量是线性的，测量矩阵HUSBL，k表示为5-3由第一级滤波处理的结果ψfirst,k和Pfirst,k，和第k-1定位时隙第二级滤波输出的状态更新和协方差矩阵得到第二级滤波器的预测，表示为：构建更新方程其中，新息增益更新后协方差5-4从中提取出更新的从UUV位置将其转换成经纬度坐标当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将4-5输出的结果作为协同定位结果，将其转换成经纬度坐标后进行输出，完成从UUV的协同定位。

全文数据：

权利要求：

百度查询： 浙江大学 一种集成超短基线系统的UUV集群协同定位方法

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

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