技术摘要：
本发明公开了一种自主水下航行器组合导航系统的主从式协同定位方法，该方法包括对于单领航定位系统和双领航定位系统下的主、从AUV建立三维协同导航系统模型，通过主AUV广播自身的位置信息，从AUV根据声速和时延获取两者时间的相对距离，进而有主AUV利用测速信息、测距  全部
背景技术：
自主水下航行器(Autonomous  underwater  vehicles，AUVs)是无人水下航行器 (Unmanned  underwater  vehicle，UUV)的一种，用于海洋资源的开发和研究。由于AUV摆脱 了缆线的束缚，因此，AUV具有作业范围广，作业时间长，不怕线缆缠绕，成本低等优点，这些 优点使得AUV在军事侦察和监视、反潜和巡逻、海洋测绘、海洋资源勘探和潜水支援等方面 发挥着重要作用。目前，AUV仍面临着高精度水下定位问题带来的技术性问题。AUV有长时间 和远距离作业需求，并且考虑到其体积、质量、特殊的水下环境、能量的约束等因素，准确定 位AUV成为一项艰巨的任务。 根据定位误差是否有界，将水下潜器的定位方式归为两类，一类是定位误差有界， 如水下声学定位系统，其要求母船跟踪或事先布放并标定长基线阵，作业成本高；另一类是 定位误差会随着时间累积，将导致误差无界，如惯性导航系统、航位推算系统等，其工作方 式独立自主、抗干扰性好，然而其固有的误差随着时间累积的特性在很大程度上限制其工 作能力。 针对以上定位方式的弊端，通过各AUV之间进行信息共享而使团队中所有AUV的定 位精度都得到提升并使得误差有界的方法逐渐成为水下AUV定位的主流研究方向，被称之 为协同定位。协同定位概念最早是由日本学者Ryo  Kurazume和Shigemi  Nagata两人在20世 纪90年代研究多机器人定位时提出，逐渐变成在该领域最有研究前景的方向之一。 目前，在陆地多机器人定位、无线传感器网络、卫星定位等领域己经广泛进行研究 和发展协同定位。随着水下通信研究的进步和AUV产品的成熟，基于水声研究的协同定位方 法得到了长足的发展和进步。在此趋势下，未来AUV进行协同集群化作业将成为主要作业方 式。协同作业允许每个AUV负责不同的任务，使得AUV可以完成搭载不同传感器设备并实现 某一特定的操作能力，大大提升了AUV团队的水下持续工作时间和单次作业能力。当AUV在 某些特定情况下，无法获得外界的GPS信息、没有母船跟踪，也无法通过地形匹配信息或重 力来获得自身的位置信息，若AUV定位能力丧失，多个AUV可以在每个定位周期通过AUV之间 进行信息共享，从而使各个AUV的定位精度达到相同量级，定位能力得以恢复。根据上述分 析可以得到该研究的必要性。本发明所研究的协同定位方法正是针对无法预知地形信息或 地标并且没有母船跟踪的前提下，满足多AUV作业过程中对高精度位置的要求。
技术实现要素：
发明目的：针对现有技术在无法预知地形信息或地标并且没有母船跟踪的前提 下，多AUV作业过程中对位置的定位不精准问题，本发明提供一种自主水下航行器组合导航 系统的主从式协同定位方法。 5 CN 111595348 A 说　明　书 2/7 页 技术方案：一种自主水下航行器组合导航系统的主从式协同定位方法，所述主从 式协同定位方法通过增加深度测量信息的三维协同导航模型，适用于单领航定位系统和双 领航定位系统下的协同定位，定位过程如下： (1)主AUV广播自身位置信息； (2)在主AUV通信范围内的从AUV收到来自主AUV的位置信息后，根据声速和时延获 得两者之间的距离，从AUV获得测速信息； (3)主AUV利用测速信息、测距信息对任意从AUV进行协同定位，给出从AUV修正后 的位置； (4)从AUV广播自身修正后的位置信息。 进一步的，所述主从式协同定位方法中，系统的状态向量的表达式如下： xk＝[(pn)T ΘT  (vb)T]T ＝[x  y  z ψ θ φ  u  v  w]T 由航位推算公式可以得到状态转移方程： 式中，依次选取导航坐标系下位置信息、载体坐标系下的速度信息、三维姿态信息 作为系统的状态向量。 述主从式协同定位方法中，系统的观测向量的表达式如下： zk＝[(z )T  (z )T  (z )T(z T]Tdepth attitude velocity range) ＝[z φ θ ψ  u  v  w  rM1  …  rMi]T 在AUV取到距离测量信息时，对应的测量方程为 式中，依次选取深度信息、姿态信息、载体速度信息以及从AUV之间的相对距离信 息构成观测向量。 更进一步的，所述主从式协同定位方法中针对AUV没有收到距离测量信息的情况 下的测量方程如下： 6 CN 111595348 A 说　明　书 3/7 页 更进一步的，所述方法中单领航协同定位具体过程如下： 在每一个定位周期内，由主AUV进行发起定位，主AUV将自身的位置信息进行广播， 当在主AUV通信范围内的从AUV收到信息后，所有AUV在入水前进行对时，保持时间同步，通 过获取的时延和声速剖面得到主从AUV之间的距离，从AUV利用主AUV提供的位置信息、测距 信息和自身的测速信息，通过滤波算法计算出自身位置并对位置更新。 更进一步的，所述方法中双领航协同定位具体过程如下： 在每一个定位周期内，双领航AUV同时将自身位置信息进行广播，当在主AUV通信 范围内的从AUV收到信息后，获得两个不同方位的测距信息，从AUV通过分别两个领航AUV的 测距信息、位置信息，以及测距信息，通过滤波算法计算出自身位置并对位置更新。 有益效果：与现有技术相比，本发明显著效果包括如下几点： (1)本发明针对AUV路径点跟随问题，给出了纯跟踪制导时系统下的全局渐进收敛 至期望路径点的充分条件，能够方便结合S面控制算法设计基于纯跟踪制导的路径点跟随 控制器； (2)通过将前视距离表示为路径跟随法向误差的函数，改进现有法制导的路径跟 随控制方法，提高了算法及函数计算的收敛速度； (3)针对多AUV编队问题，本发明适用于路径跟随的多AUV编队控制，并且也提高了 系统控制的同步和稳定性； (4)本发明在方法中涉及到极大似然估计，对由水声通信获得的领队导航信息和 前视声呐探测得到的相应信息进行融合，改进了对领队跟踪过程中导航信息传输不稳定的 情况。 附图说明 图1是单领航定位模式示意图； 图2是双领航定位模式示意图。