CN116608864A - 一种通信时延影响下基于因子图的auv协同定位方法 - Google Patents

一种通信时延影响下基于因子图的auv协同定位方法 Download PDF

Info

Publication number
CN116608864A
CN116608864A CN202310882564.1A CN202310882564A CN116608864A CN 116608864 A CN116608864 A CN 116608864A CN 202310882564 A CN202310882564 A CN 202310882564A CN 116608864 A CN116608864 A CN 116608864A
Authority
CN
China
Prior art keywords
auv
follower
position information
time
function node
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310882564.1A
Other languages
English (en)
Other versions
CN116608864B (zh
Inventor
邢文
栾正琨
严浙平
孙岩
刘建波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao Harbin Engineering University Innovation Development Center
Original Assignee
Qingdao Harbin Engineering University Innovation Development Center
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qingdao Harbin Engineering University Innovation Development Center filed Critical Qingdao Harbin Engineering University Innovation Development Center
Priority to CN202310882564.1A priority Critical patent/CN116608864B/zh
Publication of CN116608864A publication Critical patent/CN116608864A/zh
Application granted granted Critical
Publication of CN116608864B publication Critical patent/CN116608864B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

一种通信时延影响下基于因子图的AUV协同定位方法，本发明涉及AUV协同定位方法。本发明为解决现有系统中往往没有考虑通信时间延迟的影响，致使定位误差大；以及水声通信中码间串扰易产生测距野值，造成厚尾非高斯的量测噪声，致使鲁棒性差的问题。过程为：1：建立领航者‑跟随者AUV编队的协同定位因子图模型；2：考虑通信时延的影响，基于跟随者AUV的航速和量测信息对量测信息进行时延补偿；3：利用最大相关熵作为因子图各函数节点的代价函数，推导出各节点最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新。本发明用于AUV协同定位技术领域。

Description

一种通信时延影响下基于因子图的AUV协同定位方法
技术领域
本发明涉及AUV协同定位方法，特别是涉及基于因子图的AUV协同定位技术领域。
背景技术
随着对海洋资源的深度开发，自主水下航行器（Autonomous UnderwaterVehicle, AUV）逐渐成为各国在海洋技术研发中的焦点。AUV可以通过多种传感器及时、准确的得到水中的信息，并根据科研任务及时的调整自我位置。如何在复杂的水下环境中获取自身准确位置是进行科学探测的关键。
由于在海洋中电磁波会快速衰减，因此基于全球定位系统（Global PositioningSystem, GPS）无法对水下目标进行定位。单AUV在水中主要依靠惯性导航系统（InertialNavigation System, INS）进行定位，但是由于INS使用推算方法进行导航，不接受外部信息，存在累计定位误差且无法消除，所以无法满足AUV长航程下高精度定位的要求，高精度INS虽然可以满足定位条件需求，但是由于其成本过高难以广泛应用于定位系统中。因此，在实际中往往采用多AUV协同定位的方法，多AUV之间通过声学设备实现相对定位。基于成本考虑，往往采用少数AUV携带高精度惯性导航设备或全球定位系统，基于水声通信设备对配备低精度导航设备的AUV进行定位误差的修正。在装备低精度惯性导航设备以及利用其他AUV的观测信息的情况下，对于跟随者AUV的高精度定位具有至关重要的意义。
查阅当前国内外参考文献，相关研究主要集中于基于距离量测的多AUV协同定位方法。在现有研究中，基于距离-角度量测的多AUV因子图协同定位方法研究较少，且基于因子图模型对于通信延迟对定位精度的影响几乎都没有考虑。在2021年公开的孙骞、张国昌、唐忠的一篇名为《一种基于旋转矩阵的因子图协同定位方法》(专利申请号：CN202011552599；公开号：CN112762938A)的专利介绍了一种基于距离量测的因子图协同定位方法，但是该方法没有考虑到通信时延对系统的影响，没有将量测模型扩展到距离-角度。同年公开的由张亚、夏秀玮、高伟、王庆鑫发表的《一种基于最大相关熵的因子图协同定位算法》(专利申请号：202011537149.5；公开号：CN112749481A)提出了一种基于最大相关熵的协同定位算法，但该算法只适用于量测信息为相对距离，且没有考虑时间延迟对系统定位精度的影响。2021年在《中国惯性技术学报》第29卷第5期中黄紫如、柴洪洲、向民志、李达、杜祯强、王栋民发表的《考虑信息滞后的AUV因子图多源信息融合定位算法》介绍了一种带滞后信息的因子图融合定位算法，但是其并未考虑水中由于多路径效应造成的非高斯噪声影响。在2022年公开的奔粤阳、孙炎、李倩、赵玉新的一篇名为《考虑洋流影响的基于因子图的主从式AUV协同定位方法》(专利申请号：CN202210191144；公开号：CN114577211A)中提出了一种在洋流影响下的因子图协同定位算法，但其并没有考虑通信时延的影响以及水中声波在传播过程中，会发生折射、反射、漫射等现象，从而导致同一信号在接收器处以不同的时间、不同的相位、不同的幅度到达的水声信号的多路径效应造成的量测信息中带有非高斯噪声的问题。同年公开的奔粤阳、孙炎、李倩、赵玉新发表的《一种采用因子图和和积算法的多AUV协同导航方法》(专利申请号：CN202110161315.4；公开号：CN112945224A)提出的方法基于的量测噪声为高斯噪声，并未考虑非高斯的重尾噪声，以及通信时延的影响。
综上，现有AUV协同定位方法存在以下问题：
1.目前的因子图算法模型往往采用双领航者-单跟随者的协同模型，这势必会增加实际试验中的成本，而且会增大协同系统中的通信负担。因此为了更好的简化AUV协同系统，采用单领航者-单跟随者的协同模型，能够在相同定位精度的条件下实现对跟随者AUV的定位。
2.现用的AUV协同系统中往往没有考虑通信时间延迟的影响，水下环境中的通信条件与陆上环境不同，水声通信在水中传播速度大约为1500m/s，加上AUV对水声信号的转译环节，跟随者接收到的信号往往是几秒前领航者发送的量测信息，这会大大增加协同系统中的定位误差。因此需要一种方法来对带有时间延迟的量测信息进行补偿。
3.由于海洋中环境复杂，水声通信中的多路径效应，各种反射物体，信号伴随着一系列回波到达。不同路径到达时间不一样，降低分辨能力，产生误码。码间串扰易产生测距野值，造成厚尾（重尾）非高斯的量测噪声，需要一种方法来降低带有非高斯噪声的量测信息对系统的影响，增强系统的鲁棒性。
发明内容
本发明的目的是为了解决现有AUV协同系统中往往没有考虑通信时间延迟的影响，致使定位误差大，以及水声通信中码间串扰易产生测距野值，造成厚尾非高斯的量测噪声，致使鲁棒性差的问题，而提出一种通信时延影响下基于因子图的AUV协同定位方法。
一种通信时延影响下基于因子图的AUV协同定位方法具体过程为：
步骤1：基于量测信息，建立领航者-跟随者AUV编队的协同定位因子图模型；
所述量测信息为领航者-跟随者间相对距离和领航者-跟随者间相对方位角度；
步骤2：考虑通信时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；
步骤3：利用最大相关熵作为因子图各函数节点的代价函数，推导出各函数节点的最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新。
本发明的有益效果为：
1.基于量测信息为距离-角度，采用单领航者-单跟随者AUV协同模型，大大降低了协同系统中的通信负担以及试验成本。
2.基于跟随者AUV的航速和量测信息，对观测信息进行时延补偿。降低了由于通信时延造成的定位误差对系统的干扰，提高了定位精度。
3.采用的基于代价函数为最大熵的因子图模型对协同系统进行状态更新，其定位估计能力如图4-图7所示，其能够有效地抑制如图5、图6的非高斯重尾量测噪声，有效地处理零均值的非高斯模型的观测噪声以及量测异常值的情况。
附图说明
图1为基于量测信息为距离-角度建立的协同定位算法的因子图模型，代表变量节点，/>代表函数节点；
图2为水下协同系统通信时间延迟示意图，1代表领航者AUV，2代表跟随者AUV，3代表时刻跟随者接收信息，4代表/>时刻领航者发送信息；
图3为单个时间周期对通信时间延迟补偿示意图，5代表跟随者AUV在时刻位置，6代表跟随者AUV在/>时刻位置；
图4为仿真实验中领航者和跟随者AUV运动轨迹图，X轴为轨迹在北东坐标系X轴坐标（单位为米），Y轴为轨迹在北东坐标系Y轴坐标（单位为米），领航者AUV代表领航者AUV轨迹（领航者AUV轨迹是已知的），跟随者AUV代表跟随者AUV轨迹，改进因子图算法代表改进因子图算法获得的跟随者AUV轨迹，未加时延补偿因子图算法代表未加时延补偿因子图算法获得的跟随者AUV轨迹，EKF算法代表EKF算法获得的跟随者AUV轨迹；
图5为仿真实验中量测信息中的距离误差图，X轴为时间序列(单位为秒)，Y轴为领航者AUV与跟随者AUV之间量测距离误差(单位为米)；
图6为仿真实验中量测信息中的角度误差图，X轴为时间序列(单位为秒)，Y轴为领航者AUV与跟随者AUV之间量测角度误差(单位为弧度)；
图7为仿真实验中定位误差图，X轴为时间序列(单位为秒)，Y轴为距离误差(单位为米)；
图8为仿真实验中，未加时延补偿方法的算法定位误差与加入时延补偿的算法定位误差之间比较图，X轴为时间序列(单位为秒)，Y轴为距离误差(单位为米)。
具体实施方式
具体实施方式一：本实施方式一种通信时延影响下基于因子图的AUV协同定位方法具体过程为：
本发明的目的在于设计一种通信时延影响下基于因子图的AUV协同定位方法，在存在通信时延以及量测信息带非高斯重尾噪声的前提下，优化多AUV的协同定位算法，提高系统定位精度。
本发明的目的通过如下技术方案来实现：
步骤1：基于量测信息，建立领航者-跟随者AUV编队的协同定位因子图模型；
所述量测信息为领航者-跟随者间相对距离和领航者-跟随者间相对方位角度；
步骤2：考虑通信时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；
步骤3：利用最大相关熵作为因子图各函数节点的代价函数，推导出各函数节点（函数节点A、函数节点B、函数节点C、函数节点D、函数节点E、函数节点F、函数节点G、函数节点H）的最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新。
具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤1中基于量测信息，建立领航者-跟随者AUV编队的协同定位因子图模型；具体过程为：
在实际中AUV会搭载深度计，能够准确的获知AUV所处深度，因而可以将水下协同系统的三维位置信息投影到二维水平面内；如附图1所示，即为基于量测信息为距离、角度信息建立的领航者-跟随者AUV协同定位因子图模型。图中圆形表示变量节点，方形表示函数节点，各节点只能与其不同的类型的节点相连。
根据附图1所示，跟随者AUV的历史位置信息经过函数节点A、D进入因子图中，经过函数节点B、C、E、F分别与领航者AUV的横纵坐标做差，得到领航者与跟随者之间的位置差，然后通过G函数节点与方位角信息进行信息融合、通过H函数节点与相对距离信息进行信息融合。建立如附图1所示因子图。
领航者-跟随者编队AUV协同定位的因子图模型包括函数节点A、函数节点B、函数节点C、函数节点D、函数节点E、函数节点F、函数节点G、函数节点H；
将时刻跟随者AUV的位置信息/>通过函数节点A获得k时刻跟随者AUV的位置信息/>；
将时刻跟随者AUV的位置信息/>通过函数节点D获得k时刻跟随者AUV的位置信息/>；
将k时刻领航者AUV的位置信息和k时刻跟随者AUV的位置信息/>经过函数节点B，函数节点B对k时刻领航者AUV的位置信息/>和k时刻跟随者AUV的位置信息/>（横坐标）做差，得到k时刻领航者AUV和跟随者AUV之间的位置差/>；
将k时刻领航者AUV的位置信息和k时刻跟随者AUV的位置信息/>经过函数节点E，函数节点E对k时刻领航者AUV的位置信息/>和k时刻跟随者AUV的位置信息/>（纵坐标）做差，得到k时刻领航者AUV和跟随者AUV之间的位置差/>；
将量测得到的领航者AUV与跟随者AUV之间的距离和/>输入函数节点H，函数节点H对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的距离和/>输入函数节点H，函数节点H对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的相对方位角和/>输入函数节点G，函数节点G对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的相对方位角和/>输入函数节点G，函数节点G对/>和/>处理得到/>；
、/>为/>的估计值，/>、/>为/>的估计值；
为时间信息，表示为/>时刻。
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤2中考虑通信时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；具体过程为：
所述量测信息为领航者-跟随者间相对距离和领航者-跟随者间相对方位角度；
由于水下环境中，水声信号的传输需要一定时间，所以领航者AUV探测到的相对量测信息到达跟随者AUV势必会存在一定的时间延迟。时间延迟对定位精度的影响是不能忽略的。系统通信延迟如附图2所示，时刻领航者AUV发送此时的量测信息及当前的位置信息，在/>时刻跟随者AUV接收到领航者AUV发送的信息，通信延迟时间/>；此时跟随者的位置信息已经改变，如果将此时的量测信息导入系统会引起较大误差影响定位性能。
本发明使用重建量测信息的方法以补偿带有时间延迟的量测信息。在附图2中，延迟时间为，系统更新的时间为/>，即时间延迟中共经历/>个更新周期；在每个更新周期内，由于惯性导航系统具有短时的高精度性的特点，所以可以获取单个更新周期内的航行器的航向角/>和航速/>，如附图3所示，在单个更新周期内，跟随者AUV的航向角为/>，跟随者AUV的航速为/>，则单个更新周期内跟随者AUV的航行距离为/>，领航者AUV观测到的跟随者AUV相对距离为/>，领航者AUV观测到的跟随者AUV相对方位角为/>，则附图3中/>；/>为领航者AUV与跟随者AUV相对方位角与跟随者AUV航向角的差值；
综上可得单个更新周期补偿后领航者-跟随者间相对距离量测为：
单个更新周期补偿后领航者-跟随者间相对方位角度量测为：
共经历个更新周期，每个更新周期都进行上述的量测信息补偿（/>更新为，/>更新为/>），最终将补偿后领航者-跟随者间相对距离/>，补偿后领航者-跟随者间相对方位角/>带入因子图模型中（补偿后量测信息领航者-跟随者间相对距离/>是各个/>相加，补偿后相对方位角/>各个/>相加)。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤3中利用最大相关熵作为因子图各函数节点的代价函数，推导出各函数节点（函数节点A、函数节点B、函数节点C、函数节点D、函数节点E、函数节点F、函数节点G、函数节点H；）的最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新；具体过程为：
步骤31、建立因子图中约束关系；
步骤32、列写出附图1因子图中各函数节点的相关熵；
步骤33、因子图中函数节点A、D为时间更新节点，将时刻跟随者AUV的位置信息/>输入函数节点A、D，得到/>时刻跟随者AUV的先验位置信息/>，/>；
步骤34、对,/>进行估计，得到/>,/>的估计值/>、/>、/>、/>；
为k时刻领航者AUV和跟随者AUV之间的位置差；
步骤35、在得到,/>的估计值/>、/>、/>、/>后，对/>,/>进行估计；为k时刻跟随者AUV的位置信息。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤31中建立因子图中约束关系；具体过程为：
在附图1因子图中约束表示为：
其中，为/>时刻跟随者AUV的航速，/>为/>时刻补偿后的领航者AUV与跟随者AUV之间的角度，/>为/>时刻跟随者AUV的航向角，/>为/>时刻跟随者AUV的位置信息，/>为/>时刻跟随者AUV的位置信息，(/>,/>)为/>时刻领航者AUV的位置信息，为/>时刻领航者AUV和跟随者AUV之间的位置差，/>为/>时刻补偿后的领航者AUV与跟随者AUV之间的距离，/>为相邻时刻之间的时间间隔；
当得到补偿后的量测信息时，首先计算变量节点和/>的相关熵，并通过计算得到其相关熵取最大值时/>和/>的取值；然后计算/>,/>的相关熵，通过计算/>,/>相关熵取最大值时/>,/>的取值即为/>,/>的估计值。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六：本实施方式与具体实施方式一至五之一不同的是，所述步骤32中列写出附图1因子图中各函数节点的相关熵，表达式为：
其中、/>、/>、/>、/>、/>、/>和/>分别为附图1因子图模型中函数节点A、B、C、D、E、F、G、H的相关熵，/>表示二范数；/>表示核宽度为/>的高斯核函数，/>表示/>经过时间更新得到的/>值，/>表示/>经过时间更新得到的/>值；/>为的估计值，表示将量测得到的领航者AUV与跟随者AUV之间的相对方位角/>和/>输入函数节点G，函数节点G对/>和/>处理得到的位置信息；/>为/>的估计值，表示将量测得到的领航者AUV与跟随者AUV之间的相对方位角/>和/>输入函数节点G，函数节点G对/>和/>处理得到的位置信息；/>为/>的估计值，表示将量测得到的领航者AUV与跟随者AUV之间的距离/>和/>输入函数节点H，函数节点H对/>和/>处理得到的位置信息；/>为/>的估计值，表示将量测得到的领航者AUV与跟随者AUV之间的距离/>和/>输入函数节点H，函数节点H对/>和/>处理得到的位置信息；
所述高斯核函数，其中/>表示自变量。
以相关熵作为代价函数，在信息融合时，每个函数节点以最大相关熵作为目标。由于信息熵理论中高斯核具有鲁棒性的逼近能力，除此之外其还能表现出期望的平滑性以及数据稳定性的特点，选取高斯核作为相关熵的核函数。
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七：本实施方式与具体实施方式一至六之一不同的是，所述步骤33中因子图中函数节点A、D为时间更新节点，将时刻跟随者AUV的位置信息/>输入函数节点A、D，得到/>时刻跟随者AUV的先验位置信息/>，/>；具体过程为：
时刻跟随者AUV的先验位置信息/>，/>的值就是使函数节点A的相关熵/>、函数节点D的相关熵/>取最大值的解；
当，/>时，即为/>、/>取最大值；
得下式：
其中表示核宽度；
解得，/>的估计为：
当跟随者AUV接收到领航者AUV发送的位置信息以及补偿更新过后的量测信息，/>（步骤二得到的/>和/>）后，在已知领航者AUV的位置信息(/>,/>)的情况下，为了求解跟随者AUV的位置信息(/>,/>)，需要计算/>，/>。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八：本实施方式与具体实施方式一至七之一不同的是，所述步骤34中对,/>进行估计；具体过程为：
步骤341、计算,/>的相关熵；
,/>的相关熵可以表示为：
其中表示位置信息/>的相关熵，/>表示位置信息/>的相关熵，/>表示位置信息/>的相关熵，/>表示位置信息/>的相关熵；
步骤342、分别求解、/>、/>、/>取最大值时/>、/>、/>、/>的取值；过程为：
求解取最大值时/>的取值：
解得取最大值时/>的取值：
同理解得取最大值时/>的取值：
求解取最大值时/>的取值：
解得取最大值时/>的取值：
同理可解得取最大值时/>的取值：
因为此时进行更新,/>，并未对/>,/>进行估计，因此/>,/>只能使用时间更新中的估计值。上述等式中右侧/>,/>为先验估计值，此时未使用量测信息，量测信息为距离、角度，设置以下方程：
其中表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，/>表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，/>表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，/>表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值；
将带入/>、/>、/>、/>取最大值时/>、/>、/>、的取值公式：/>
。
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九：本实施方式与具体实施方式一至七之一不同的是，所述步骤35中在得到,/>的估计值/>、/>、/>、/>后，对/>,/>进行估计；具体过程为：
,/>的相关熵可以表示为：
其中表示位置信息/>的相关熵，/>表示位置信息/>的相关熵；
分别求解、/>取最大值时/>、/>的取值；过程为：
解得：
，/>即为k时刻跟随者AUV的位置估计的最优值。
其它步骤及参数与具体实施方式一至八之一相同。
具体实施方式十：本实施方式与具体实施方式一至九之一不同的是，所述核宽度取值为：
在因子图最大熵优化算法中，还有一个自由变量核宽度；核宽度过大会使定位效果不平滑，核宽度过小会使系统跟踪效果减弱；因此根据误差的大小对/>进行自适应调节，选取误差函数的1/4作为核宽度/>大小；
在估计，/>时，选取/>为：
在估计，/>时，选取/>为：
在估计，/>时选取/>为/>和/>的均值：
。
其它步骤及参数与具体实施方式一至九之一相同。
采用以下实施例验证本发明的有益效果：
实施例一：
下面结合附图与具体实施方式对本发明作进一步详细描述。
本发明建立领航者-跟随者编队AUV协同定位的因子图模型，利用最大信息熵作为因子图各函数节点的代价函数，降低了带有非高斯重尾噪声对系统量测的干扰，在存在通信时延的情况下，通过对量测信息进行补偿，降低由于时延对定位精度的影响，最终实现对跟随者AUV的位置位置信息的融合估计。本发明目的通过以下步骤实现：
1、基于量测信息为距离、角度，建立领航者-跟随者AUV编队的协同定位因子图模型；
2、考虑通信时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；
3、利用最大相关熵作为因子图各节点的代价函数，推导出各变量节点的最优估计，利用观测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV位置信息进行估计更新。
为了验证本发明的有效性，利用仿真软件对通信时延影响下基于因子图的AUV协同定位方法进行了仿真验证。
如图1所示为建立的基于量测信息为距离、角度，建立领航者-跟随者编队AUV协同定位的因子图模型，图中A、B、C、D、E、F、G、H为函数节点；代表时间；/>，/>表示/>时刻跟随者AUV的位置坐标；/>，/>表示k时刻接收到的领航者AUV的位置信息；/>为/>时刻领航者AUV观测跟随者AUV距离经过时间延迟补偿后的值。/>为/>时刻领航者AUV观测跟随者AUV方位角经过时间延迟补偿后的值。/>为x方向上领航者与跟随者AUV之间的距离，/>为y方向上领航者与跟随者AUV之间的距离。
图4为仿真实验中的运动轨迹，仿真条件为：领航者AUV初始位置为(200,500),速度为1.8m/s，航向为0°；跟随者AUV初始位置为(-300,-200)，速度为1.7m/s，航向为0°。仿真中领航者AUV和跟随者AUV先作1300s的匀速直线运动，然后进行1300s的角加速度为的弧线匀速率运动，再进行1300s的角加速度为/>的弧线匀速率运动，最后进行1300s的匀速直线运动。以此来测定算法对于线性以及非线性运动模型的适用程度。在仿真中，跟随者AUV的速度过程噪声为当前速度的1%，跟随者AUV的航向角过程噪声为0.3/>，均为不相关的加性噪声。领航者AUV与跟随者AUV之间的量测噪声如图5、图6所示均为非高斯的重尾噪声。
每次时间更新周期为1s，使用算法进行位置估计更新周期为13s，通信时间延迟为3s。图7为仿真实验的定位误差图。结合图5、图6可以看出，在协同定位过程中，当量测信息中出现异常值，基于本发明提出的算法的定位误差要小于EKF滤波算法的定位误差。
图8为仿真实验中未加时延补偿方法的算法定位误差与加入时延补偿的算法定位误差之间比较图，可以清楚的表明量测信息的时延补偿对通信延迟造成的误差具有很好的抑制能力。
通过上述实验验证了本发明提出的通信时延影响下基于因子图的AUV协同定位方法的有效性，在存在通信时间延迟以及量测噪声为非高斯重尾噪声的条件下，能够对跟随者AUV进行有效的定位估计。
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明做出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (10)

1.一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述方法具体过程为：
步骤1：基于量测信息，建立领航者-跟随者AUV编队的协同定位因子图模型；
所述量测信息为领航者-跟随者间相对距离和领航者-跟随者间相对方位角度；
步骤2：考虑通信时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；
步骤3：利用最大相关熵作为因子图各函数节点的代价函数，推导出各函数节点的最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新。
2.根据权利要求1所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤1中基于量测信息，建立领航者-跟随者AUV编队的协同定位因子图模型；具体过程为：
领航者-跟随者编队AUV协同定位的因子图模型包括函数节点A、函数节点B、函数节点C、函数节点D、函数节点E、函数节点F、函数节点G、函数节点H；
将时刻跟随者AUV的位置信息/>通过函数节点A获得k时刻跟随者AUV的位置信息；
将时刻跟随者AUV的位置信息/>通过函数节点D获得k时刻跟随者AUV的位置信息；
将k时刻领航者AUV的位置信息和k时刻跟随者AUV的位置信息/>经过函数节点B，函数节点B对k时刻领航者AUV的位置信息/>和k时刻跟随者AUV的位置信息/>做差，得到k时刻领航者AUV和跟随者AUV之间的位置差/>；
将k时刻领航者AUV的位置信息和k时刻跟随者AUV的位置信息/>经过函数节点E，函数节点E对k时刻领航者AUV的位置信息/>和k时刻跟随者AUV的位置信息/>做差，得到k时刻领航者AUV和跟随者AUV之间的位置差/>；
将量测得到的领航者AUV与跟随者AUV之间的距离和/>输入函数节点H，函数节点H对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的距离和/>输入函数节点H，函数节点H对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的相对方位角和/>输入函数节点G，函数节点G对/>和/>处理得到/>；
将量测得到的领航者AUV与跟随者AUV之间的相对方位角和/>输入函数节点G，函数节点G对/>和/>处理得到/>；
、/>为/>的估计值，/>、/>为/>的估计值；
k为时间信息，表示为k时刻。
3.根据权利要求2所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤2中考虑通讯时延的影响，基于跟随者AUV的航向航速信息，对领航者AUV的量测信息进行时延补偿；具体过程为：
时刻领航者AUV发送此时的量测信息及当前的位置信息，在/>时刻跟随者AUV接收到领航者AUV发送的信息，通讯延迟时间/>；
系统更新的时间为，即时间延迟中共经历/>个更新周期；
在单个更新周期内，跟随者AUV的航向角为，跟随者AUV的航速为/>，则单个更新周期内跟随者AUV的航行距离为/>，领航者AUV观测到的跟随者AUV相对距离为/>，领航者AUV观测到的跟随者AUV相对方位角为/>，/>；/>为领航者AUV与跟随者AUV相对方位角与跟随者AUV航向角的差值；
综上可得单个更新周期补偿后领航者-跟随者间相对距离量测为：
单个更新周期补偿后领航者-跟随者间相对方位角度量测为：
共经历个更新周期，每个更新周期都进行上述的量测信息补偿，最终将补偿后领航者-跟随者间相对距离/>，补偿后领航者-跟随者间相对方位角/>带入因子图模型中。
4.根据权利要求3所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤3中利用最大相关熵作为因子图各函数节点的代价函数，推导出各函数节点的最优估计，利用量测信息和AUV历史位置信息对AUV当前位置信息进行误差补偿，从而对跟随者AUV的位置信息进行估计更新；具体过程为：
步骤31、建立因子图中约束关系；
步骤32、列写出因子图中各函数节点的相关熵；
步骤33、因子图中函数节点A、D为时间更新节点，将时刻跟随者AUV的位置信息输入函数节点A、D，得到k时刻跟随者AUV的先验位置信息/>，/>；
步骤34、对,/>进行估计，得到/>,/>的估计值/>、/>、/>、/>；
为k时刻领航者AUV和跟随者AUV之间的位置差；
步骤35、在得到,/>的估计值/>、/>、/>、/>后，对/>,/>进行估计；
为k时刻跟随者AUV的位置信息。
5.根据权利要求4所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤31中建立因子图中约束关系；具体过程为：
因子图中约束表示为：
其中，为k时刻跟随者AUV的航速，/>为k时刻补偿后的领航者AUV与跟随者AUV之间的角度，/>为k时刻跟随者AUV的航向角，/>为/>时刻跟随者AUV的位置信息，为k时刻跟随者AUV的位置信息，(/>,/>)为k时刻领航者AUV的位置信息，为k时刻领航者AUV和跟随者AUV之间的位置差，/>为k时刻补偿后的领航者AUV与跟随者AUV之间的距离，/>为相邻时刻之间的时间间隔。
6.根据权利要求5所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤32中列写出因子图中各函数节点的相关熵，表达式为：
其中、/>、/>、/>、/>、/>、/>和/>分别为因子图模型中函数节点A、B、C、D、E、F、G、H的相关熵，/>表示二范数；/>表示核宽度为/>的高斯核函数，/>表示/>经过时间更新得到的/>值，/>表示/>经过时间更新得到的/>值；/>为/>的估计值；/>为/>的估计值；/>为/>的估计值；/>为/>的估计值；
所述高斯核函数，其中/>表示自变量。
7.根据权利要求6所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤33中因子图中函数节点A、D为时间更新节点，将时刻跟随者AUV的位置信息/>输入函数节点A、D，得到k时刻跟随者AUV的先验位置信息/>，/>；具体过程为：
k时刻跟随者AUV的先验位置信息，/>的值就是使函数节点A的相关熵/>、函数节点D的相关熵/>取最大值的解；
当，/>时，即为/>、/>取最大值；
得下式：
其中表示核宽度；
解得，/>的估计为：
。
8.根据权利要求7所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤34中对,/>进行估计；具体过程为：
步骤341、计算,/>的相关熵；
,/>的相关熵可以表示为：
其中表示位置信息/>的相关熵，/>表示位置信息/>的相关熵，/>表示位置信息/>的相关熵，/>表示位置信息/>的相关熵；
步骤342、分别求解、/>、/>、/>取最大值时/>、/>、/>、/>的取值；过程为：
求解取最大值时/>的取值：
解得取最大值时/>的取值：
同理解得取最大值时/>的取值：
求解取最大值时/>的取值：
解得取最大值时/>的取值：
同理可解得取最大值时/>的取值：
设置以下方程：
其中表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，/>表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值，/>表示跟随者AUV的先验位置信息/>和领航者AUV的位置信息/>的差值；
将带入/>、/>、/>、/>取最大值时/>、/>、/>、/>的取值公式：
。
9.根据权利要求8所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述步骤35中在得到,/>的估计值/>、/>、/>、/>后，对/>,/>进行估计；具体过程为：
,/>的相关熵可以表示为：
其中表示位置信息/>的相关熵，/>表示位置信息/>的相关熵；
分别求解、/>取最大值时/>、/>的取值；过程为：
解得：
，/>即为k时刻跟随者AUV的位置估计的最优值。
10.根据权利要求9所述的一种通信时延影响下基于因子图的AUV协同定位方法，其特征在于：所述核宽度取值为：
在估计，/>时，选取/>为：
在估计，/>时，选取/>为：
在估计，/>时选取/>为/>和/>的均值：
。
CN202310882564.1A 2023-07-19 2023-07-19 一种通信时延影响下基于因子图的auv协同定位方法 Active CN116608864B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310882564.1A CN116608864B (zh) 2023-07-19 2023-07-19 一种通信时延影响下基于因子图的auv协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310882564.1A CN116608864B (zh) 2023-07-19 2023-07-19 一种通信时延影响下基于因子图的auv协同定位方法

Publications (2)

Publication Number Publication Date
CN116608864A true CN116608864A (zh) 2023-08-18
CN116608864B CN116608864B (zh) 2023-10-27

Family

ID=87678610

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310882564.1A Active CN116608864B (zh) 2023-07-19 2023-07-19 一种通信时延影响下基于因子图的auv协同定位方法

Country Status (1)

Country Link
CN (1) CN116608864B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117739978A (zh) * 2023-12-06 2024-03-22 无锡学院 一种基于因子图的多auv并行式协同导航定位方法及系统

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080165617A1 (en) * 2007-01-04 2008-07-10 Ocean Acoustical Services And Insrumentation Systems (Oasis) Inc. Methods of and systems for continually measuring the range between mobile underwater vehicles carrying acoustical signal transmitters and remotely deployed synchronized underwater acoustical receivers provided with signal processing for continually determining such range during the underwater moving of the vehicle, and for measuring acoustic underwater transmission loss, geoacoustical properties and for other purposes
KR20080085509A (ko) * 2007-03-20 2008-09-24 한국해양연구원 2개 기준점에 대한 거리정보와 관성센서를 이용한무인잠수정 선단의 수중 항법시스템
CN102636771A (zh) * 2012-04-25 2012-08-15 西北工业大学 一种基于双移动信标的auv水下声学定位方法
US20190204430A1 (en) * 2017-12-31 2019-07-04 Woods Hole Oceanographic Institution Submerged Vehicle Localization System and Method
CN111595348A (zh) * 2020-06-23 2020-08-28 南京信息工程大学 一种自主水下航行器组合导航系统的主从式协同定位方法
CN112749481A (zh) * 2020-12-23 2021-05-04 哈尔滨工业大学 一种基于最大相关熵的因子图协同定位算法
CN113048983A (zh) * 2021-03-29 2021-06-29 河海大学 一种异时序贯量测的改进分层式auv协同导航定位方法
CN113156368A (zh) * 2021-03-23 2021-07-23 哈尔滨工业大学 一种基于因子图的误差参数辨识协同定位方法
US20210284305A1 (en) * 2020-03-12 2021-09-16 National Deep Sea Center Deep-sea low-cost long-endurance collaborative navigation and positioning system
CN114577211A (zh) * 2022-02-25 2022-06-03 哈尔滨工程大学 考虑洋流影响的基于因子图的主从式auv协同定位方法
CN116086445A (zh) * 2023-02-28 2023-05-09 东南大学 一种基于因子图优化的多源信息时延融合导航方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080165617A1 (en) * 2007-01-04 2008-07-10 Ocean Acoustical Services And Insrumentation Systems (Oasis) Inc. Methods of and systems for continually measuring the range between mobile underwater vehicles carrying acoustical signal transmitters and remotely deployed synchronized underwater acoustical receivers provided with signal processing for continually determining such range during the underwater moving of the vehicle, and for measuring acoustic underwater transmission loss, geoacoustical properties and for other purposes
KR20080085509A (ko) * 2007-03-20 2008-09-24 한국해양연구원 2개 기준점에 대한 거리정보와 관성센서를 이용한무인잠수정 선단의 수중 항법시스템
CN102636771A (zh) * 2012-04-25 2012-08-15 西北工业大学 一种基于双移动信标的auv水下声学定位方法
US20190204430A1 (en) * 2017-12-31 2019-07-04 Woods Hole Oceanographic Institution Submerged Vehicle Localization System and Method
US20210284305A1 (en) * 2020-03-12 2021-09-16 National Deep Sea Center Deep-sea low-cost long-endurance collaborative navigation and positioning system
CN111595348A (zh) * 2020-06-23 2020-08-28 南京信息工程大学 一种自主水下航行器组合导航系统的主从式协同定位方法
CN112749481A (zh) * 2020-12-23 2021-05-04 哈尔滨工业大学 一种基于最大相关熵的因子图协同定位算法
CN113156368A (zh) * 2021-03-23 2021-07-23 哈尔滨工业大学 一种基于因子图的误差参数辨识协同定位方法
CN113048983A (zh) * 2021-03-29 2021-06-29 河海大学 一种异时序贯量测的改进分层式auv协同导航定位方法
CN114577211A (zh) * 2022-02-25 2022-06-03 哈尔滨工程大学 考虑洋流影响的基于因子图的主从式auv协同定位方法
CN116086445A (zh) * 2023-02-28 2023-05-09 东南大学 一种基于因子图优化的多源信息时延融合导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
XIN SUN; HONGLI XU; JING JIANG: "AUV cooperative navigation algorithm based on factor graph", 2019 CHINESE CONTROL AND DECISION CONFERENCE *
张福斌;张小龙;马朋;: "一种考虑时钟同步问题的多AUV协同定位算法", 鱼雷技术, vol. 21, no. 05 *
李成凤，张阳伟，张弘强: "通信时延下AUV集群的分群控制算法", 控制工程, vol. 29, no. 8 *
王文佳;孙睿智;高伟;徐博;: "基于通信延迟误差补偿的协同导航算法", 火力与指挥控制, vol. 39, no. 12 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117739978A (zh) * 2023-12-06 2024-03-22 无锡学院 一种基于因子图的多auv并行式协同导航定位方法及系统

Also Published As

Publication number Publication date
CN116608864B (zh) 2023-10-27

Similar Documents

Publication Publication Date Title
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
Jinwu et al. Study on installation error analysis and calibration of acoustic transceiver array based on SINS/USBL integrated system
CN104076348B (zh) 一种雷达超视距基线无源协同定位方法
Morgado et al. USBL/INS tightly-coupled integration technique for underwater vehicles
CN111896962B (zh) 一种海底应答器定位方法、系统、存储介质及应用
CN107561562B (zh) 一种gnss-r遥感中镜面反射点快速确定方法
CN112284384B (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN116608864B (zh) 一种通信时延影响下基于因子图的auv协同定位方法
CN107193023A (zh) 一种具有闭式解的高精度北斗卫星系统单点定位方法
CN111208520B (zh) 一种海底声学应答器的定位方法及装置
CN110132281B (zh) 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
CN109085655B (zh) 一种水下平台重力测量方案与验证方法
CN116106875B (zh) 岸基阵坐标联合校准方法、系统、电子设备及存储介质
Wang et al. Underwater acoustic positioning based on the robust zero-difference Kalman filter
CN106802143A (zh) 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
Wang et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN113821766B (zh) 一种海洋声学导航系统误差改正方法
CN116358544A (zh) 基于声学特征匹配定位修正惯性导航误差的方法和系统
Huang An Underwater Target Tracking Algorithm Based on Extended Kalman Filter
CN112230249B (zh) 一种基于城市多路径误差抑制的相对定位方法
Koshaev Multiple model algorithm for single-beacon navigation of autonomous underwater vehicle without its a priori position. part 2. simulation
CN113791432A (zh) 一种提高ais定位精度的组合导航定位方法
CN113156368A (zh) 一种基于因子图的误差参数辨识协同定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant