CN114647250B - 适用于水下无人航行器的双层集群控制方法 - Google Patents
适用于水下无人航行器的双层集群控制方法 Download PDFInfo
- Publication number
- CN114647250B CN114647250B CN202210276041.8A CN202210276041A CN114647250B CN 114647250 B CN114647250 B CN 114647250B CN 202210276041 A CN202210276041 A CN 202210276041A CN 114647250 B CN114647250 B CN 114647250B
- Authority
- CN
- China
- Prior art keywords
- information
- follower
- pilot
- formation
- coordinate system
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000015572 biosynthetic process Effects 0.000 claims abstract description 131
- 238000004891 communication Methods 0.000 claims description 54
- 230000006855 networking Effects 0.000 claims description 12
- 239000013307 optical fiber Substances 0.000 claims description 12
- 239000010410 layer Substances 0.000 description 40
- 230000005540 biological transmission Effects 0.000 description 13
- 238000013144 data compression Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000006978 adaptation Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 238000007906 compression Methods 0.000 description 2
- 230000006835 compression Effects 0.000 description 2
- 230000001276 controlling effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000002355 dual-layer Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
-
- 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
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种适用于水下无人航行器的双层集群控制方法。其包括由若干水下无人航行器编队组网成的集群，配置得到领航者与跟随者；对任一跟随者，根据所确定的跟随航行位置信息、跟随航行运动信息以及所接收到的跟随控制状态信息，确定所述跟随者跟随领航者时的跟踪状态控制信息，以使得跟随者在所述跟踪状态控制信息下跟踪领航者时，与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配，其中，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态。本发明在水下弱通行环境下，提高多水下无人航行器编队控制的稳定性与可靠性。
Description
技术领域
本发明涉及一种控制方法，尤其是一种适用于水下无人航行器的双层集群控制方法。
背景技术
水下无人航行器是一种无人化的平台，通过自主控制技术实现在无人干预的情况下能够自主的执行水下作业任务。目前，水下无人航行器可以广泛的应用于湖泊、海洋科学调查、探测等多个领域。随着水下探测作业的要求越来越高、任务量越来越大，单个航行器作业已不能满足某些复杂作业的使用要求。
为满足大范围高精度的海洋观测的需求，可以采取多水下无人航行器编队组网作业来实现。和单个的水下无人航行器作业相比，通过多水下无人航行器编队组网作业的方式具有信息获取更精确、工作效率更高、观测范围更广、系统鲁棒性更高等优点。但是对于多个水下无人航行器，进行水下编队组网作业还面临着水下通信能力弱传输不稳定且数据量较少的问题，以及在弱通信条件下编队控制方法不成熟等问题。
发明内容
本发明的目的是克服现有技术中存在的不足，提供一种适用于水下无人航行器的双层集群控制方法，其在水下弱通行环境下，提高多水下无人航行器编队控制的稳定性与可靠性。
按照本发明提供的技术方案，一种适用于水下无人航行器的双层集群控制方法，包括由若干水下无人航行器编队组网成的集群，
将集群内的一水下无人航行器配置为领航者，集群内其余的水下无人航行器均配置为跟随者，其中，向领航者发送领航控制状态信息，向每个跟随者发送一相应的跟随控制状态信息，领航控制状态信息包括编队路径信息以及编队速度信息，跟随控制状态信息包括编队路径信息、编队速度信息以及与跟随者相适配的编队队形信息，编队队形信息至少包含表征相应接收所述编队队形信息的跟随者与领航者之间的相对位置信息；
根据所接收到领航控制状态信息航行后，领航者向所有的跟随者发送领航状态信息；
对任一跟随者，在接收到领航状态信息后，确定所述跟随者当前的跟随航行位置信息以及跟随航行运动信息，并根据所确定的跟随航行位置信息、跟随航行运动信息以及所接收到的跟随控制状态信息，确定所述跟随者跟随领航者时的跟踪状态控制信息，以使得跟随者在所述跟踪状态控制信息下跟踪领航者时，与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配，其中，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态。
还包括与集群适配的岸站，通过岸站与集群内水下无人航行器无线通信，且岸站与集群内所有的水下无人航行器通信后，配置得到所需的领航者以及与所配置领航者相对应的跟随者；
岸站与集群间的无线通信方式包括广播通信以及单点通信，其中，岸站通过广播通信方式将编队路径信息以及编队速度信息发送至领航者以及所有的跟随者，将向跟随者以单点通信方式发送对应的编队队形信息。
所述领航状态信息包括领航者当前位置、领航航向psi、编队速度LSP以及由领航者当前位置与编队路径信息确定的当前领航路径路段；
领航者以广播发送方式将领航状态信息发送至集群内所有的跟随者，其中，广播发送时，先将领航者当前位置、领航航向psi以及编队速度LSP进行所需的数据压缩，再将当前领航路径路段以及压缩后的领航者当前坐标、领航航向psi、编队速度LSP向跟随者发送。
对领航者当前坐标数据压缩时，将领航者当前位置相应的经度值、纬度值转换成3字节的int型数据；对领航航向psi数据压缩时，将领航航向psi转换成2字节的short型数据；对编队速度LSP数据压缩时，将编队速度LSP信息转换成2字节的short型数据。
确定跟踪状态控制信息的速度层控制状态时，包括如下步骤：
步骤1、根据领航者当前位置[Sx,Sy]以及当前跟随者接收的编队队形信息[Dissx,Dissy]，确定跟随者需跟随航行的虚拟点Ja，则有
其中，Sx为领航者当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Sy为领航者的当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Dissx为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在x方向的距离坐标，Dissy为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在y方向的距离坐标，Jax为虚拟点Ja在大地直角坐标系下的x轴坐标，Jay为虚拟点Ja在大地直角坐标系下的y轴坐标；
步骤2、在虚拟点Ja上建立局部大地直角坐标系，确定跟随者在建立的局部大地直角坐标系下的局部坐标
则有
其中，PGax为虚拟点Ja在局部大地直角坐标系下的x轴坐标，PGay为虚拟点Ja在局部大地直角坐标系下的y轴坐标，Gx为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的x轴坐标，Gy为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的y轴坐标，Gpsi为根据跟随航行运动信息确定跟随者的当前航向角；
步骤3、确定跟随者的跟踪航行速度GSP，则有
GSP＝LSP+KSP*PGax+KSD*(PGax-PG′ax)
其中，KSP为比例系数、KSD为微分控制系数；PG′ax为前一个虚拟点Ja在相应局部直角坐标系下的x轴坐标。
确定跟踪状态控制信息的位置层控制状态时，包括如下步骤：
步骤10、确定跟随者跟随路径时的路径航行角Tpsi，则有
Tpsi＝arctan2(yk+1-yk,xk+1-xk)
其中，(xk,yk)为当前领航路径路段k相应前端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标，(xk+1,yk+1)为当前领航路径路段k相应后端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标；
步骤11、根据跟随者所接收的编队队形信息，确定得到虚拟点P的坐标
则有
其中，Px为虚拟点P在大地直角坐标系下的x轴坐标，Py为虚拟点P在大地直角坐标系下的y轴坐标；
步骤12、跟随者以虚拟点P对领航者实施路径跟踪，确定虚拟点P与领航者路径间的横向偏差e，则有
e＝-(Px-xk)sin(Tpsi)+(Py-yk)cos(Tpsi)
步骤13、基于横向偏差e，确定跟随者跟踪目标航向角ψd，则有
其中，ψr为前视角，V为前视距离。
跟随者确定所述跟随者当前相对领航者的跟随航行位置信息以及跟随航行运动信息时，所用的确定方式包括利用光纤惯导单元确定。
领航者以定时间隔方式向所有的跟随者发送领航状态信息。
本发明的优点：将集群内的水下无人航行器分别配置为领航者与跟随者，跟随者利用确定的跟踪状态控制信息下跟踪领航者时，可使与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态。
通过速度层控制状态以及位置层控制状态配合，能有效适应了水下弱通信的环境条件，即能够有效的控制编队，提高编队控制精度的同时，又能够保持良好的可靠性，位置控制层状态的执行对水下通信的要求降低，位置控制层状态的运行可靠，使得编队保持不发散。
对领航者发送的领航状态信息进行数据压缩，能够降低编队控制的数据通信量，减轻水下通信负荷，提高通信的可靠性。
附图说明
图1为本发明控制方法的过程图。
图2为本发明利用岸站与集群通信的示意图。
图3为本发明双层集群控制的流程图。
图4为本发明数据压缩的示意图。
具体实施方式
下面结合具体附图和实施例对本发明作进一步说明。
如图1所示：在水下弱通行环境下，为了提高多水下无人航行器编队控制的稳定性与可靠性，本发明的双层集群控制方法，具体地：包括由若干水下无人航行器编队组网成的集群，
将集群内的一水下无人航行器配置为领航者，集群内其余的水下无人航行器均配置为跟随者，其中，向领航者发送领航控制状态信息，向每个跟随者发送一相应的跟随控制状态信息，领航控制状态信息包括编队路径信息以及编队速度信息，跟随控制状态信息包括编队路径信息、编队速度信息以及与跟随者相适配的编队队形信息，编队队形信息至少包含表征相应接收所述编队队形信息的跟随者与领航者之间的相对位置信息；
根据所接收到领航控制状态信息航行后，领航者向所有的跟随者发送领航状态信息；
对任一跟随者，在接收到领航状态信息后，确定所述跟随者当前的跟随航行位置信息以及跟随航行运动信息，并根据所确定的跟随航行位置信息、跟随航行运动信息以及所接收到的跟随控制状态信息，确定所述跟随者跟随领航者时的跟踪状态控制信息，以使得跟随者在所述跟踪状态控制信息下跟踪领航者时，与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配，其中，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态。
具体地，集群内包括若干水下无人航行器，集群内水下无人航行器的数量可根据应用场景等选择确定，水下无人航行器均可采用现有常用的航行器形式，多个水下无人航行器的具体编队组网形成集群的方式以及过程可与现有相一致，具体以能满足编队组网形成集群为准。
本发明实施例中，对于编队组网后的集群，需要配置一水下无人航行器作为领航者，当确定一水下无人航行器为领航者后，则其余的水下无人航行器主要配置为跟随者，在航行时，跟随者需要参照领航者航行，以保持编队组网的集群在航行时的稳定性与可靠性。具体实施时，为了能规划或控制领航者的运行，需要向领航者发送一领航控制状态信息；而为了能使得跟随者有效跟踪领航者，需要向每个跟随者发送一相应的跟随控制状态。通过编队路径信息能确定整个集群在水下航行的路径，一般地，编队路径信息以多点连线表示，利用编队路径信息的具体情况以能实现对整个集群的航行引导，具体与集群在水下航行的目的等相关。对于编队速度，具体是指约束或引导整个集群在水下航行的速度，具体与集群在水下编队组网航行等目的相关，以能满足实际的应用需求为准。
对于集群内的水下无人航行器，均会接收编队路径信息以及编队速度信息。由于每个跟随者相对领航者的位置不同，因此，每个跟随者所接收的跟随控制状态信息内的编队队形信息不同，编队队形信息具体包括以领航者为原点建立坐标系时，每个跟随者相对领航者的位置关系，从而领航者根据编队路径信息航行时，在确定领航者的航行位置后，即能根据编队队形信息确定每个跟随者的位置。一般地，集群内所有的水下无人航行器在水下编队组网航行时，所有的水下无人航行器位于同一水平面航行。
图2中示出了利用岸站与集群配合的示意图，具体地，还包括与集群适配的岸站，通过岸站与集群内水下无人航行器无线通信，且岸站与集群内所有的水下无人航行器通信后，配置得到所需的领航者以及与所配置领航者相对应的跟随者；
岸站与集群间的无线通信方式包括广播通信以及单点通信，其中，岸站通过广播通信方式将编队路径信息以及编队速度信息发送至领航者以及所有的跟随者，将向跟随者以单点通信方式发送对应的编队队形信息。
具体实施时，岸站可以采用现有常用的形式，岸站、水下无人航行器间实施无线通信时，需要确保岸站、水下无人航行器均具有相应的水声通信系统，水声通信系统可以采用现有常用的形式，具体以能满足水声通信为准。在采用水声通信时，岸站能以广播通信以及单点通信的方式与水下无人航行器通信，利用水声通信实现广播通信、单点通信的具体形式可以根据需要选择，以能实现广播通信、单点通信的目的均可，此处不再赘述。本发明实施例中，岸站通过广播通信方式将编队路径信息以及编队速度信息发送至领航者以及所有的跟随者，通过广播通信将编队路径信息以及编队速度信息发送至领航者以及所有的跟随者时，能提高通信的效率。编队队形信息，以单点通信方式发送至相应的跟随者，确保每个跟随者接收到的编队队形信息与跟随者在集群内的位置适配，提高集群控制的可靠性。
除了采用图2所示的通信方式，还可以采用其他方式与集群内水下无人航行器通信，具体通行方式可以根据实际需要选择，此处不再一一列举说明。
具体实施时，对领航者，根据所接收到领航控制状态信息航行后，领航者向所有的跟随者发送领航状态信息。具体地，领航者根据领航控制状信息航行，具体是指按编队速度信息的编队速度LSP下航行，航行的路径按照编队路径信息。领航者以定时间隔方式向所有的跟随者发送领航状态信息，所述定时间隔的时间可以根据需要选择，如间隔5s即向跟随者发送领航状态信息，时间间隔的具体大小以能满足对集群控制为准。
进一步地，所述领航状态信息包括领航者当前位置、领航航向psi、编队速度LSP以及由领航者当前位置与编队路径信息确定的当前领航路径路段；
领航者以广播发送方式将领航状态信息发送至集群内所有的跟随者，其中，广播发送时，先将领航者当前位置、领航航向psi以及编队速度LSP进行所需的数据压缩，再将当前领航路径路段以及压缩后的领航者当前坐标、领航航向psi、编队速度LSP向跟随者发送。
本发明实施例中，领航状态信息包括领航者当前位置、领航航向psi、编队速度LSP以及当前领航路径路段，其中，领航者当前位置一般为领航者所在位置的经纬度，编队速度LSP具体可以根据编队速度信息确定。当领航者当前位置确定后，由于编队路径信息以下发至领航者，因此，根据领航者的领航者当前位置，即可由领航者当前位置与编队路径信息确定的当前领航路径路段。对于任一当前领航路径路段，所述当前领航路径路段一般具有两个端点，对于一确定的当前领航路径路段，则所述当前领航路径路段两端点相应的经纬度或大地坐标能够具体确定得到。
具体实施时，领航者上一般安装有光纤惯导单元，利用光纤惯导单元能确定领航者当前位置以及领航航向psi，领航者的航行速度即为编队速度LSP。光纤惯导单元具体可以采用现有常用的形式，光纤惯导单元与水下无人航行器的具体配合形式，以及利用光纤惯导单元确定水下无人航行器的位置信息以及航向角等方式均可与现有相一致，具体为本技术领域人员所熟知，此处不再赘述。
为了能实现所有跟随者的同步跟随，领航者以广播发送方式将领航状态信息发送至集群内所有的跟随者，即在广播范围内，跟随者均能接收到领航状态信息。一般来说传输这些信息按照最简传输方法领航者实际坐标、航向、速度按照4字节浮点数表示，当前路径跟踪段按照1字节表示共需要17个字节。由于水声通信速度慢，通信量小，因此，具体实施时，先将领航者当前位置、领航航向psi以及编队速度LSP进行所需的数据压缩，然后将当前领航路径路段以及压缩后的领航者当前坐标、领航航向psi、编队速度LSP向跟随者发送，通过数据压缩能减少广播发送时的字节数，提高广播发送的可靠性。
进一步地，对领航者当前坐标数据压缩时，将领航者当前位置相应的经度值、纬度值转换成3字节的int型数据；对领航航向psi数据压缩时，将领航航向psi转换成2字节的short型数据；对编队速度LSP数据压缩时，将编队速度LSP信息转换成2字节的short型数据。
具体实施时，由上述说明可知，领航者当前位置一般为经纬度值，对于领航者当前位置的经纬度值，首先对经、纬度乘100000后舍弃小数点后转化成4字节int型，然后对int型后三个字节右以按照从高到低的方式存储到3个字节中，这样的表示精度可以到米级满足编队使用；如经度值为31.12345，再乘100000后舍弃小数点转化为4字节int型即3112345，在转换成3字节即3112345。对于领航航向psi，将其乘10后舍弃小数点后转化成2字节short型，这样精度达到0.1°。对于编队速度LSP，将其乘1000后舍弃小数点后转化成2字节short型，这样精度达到1mm/s；具体数据压缩的方式如图4所示。
通过压缩后，广播传输的领航状态信息共需要11字节。当然，在具体实施时，还可以采用其他的数据压缩形式，以能满足数据传输，提高弱通信条件下广播通信的可靠性为准，具体压缩手段此处不再一一列举说明。
具体实施时，领航者向所有的跟随者广播发送领航状态信息后，对任一跟随者，均能接收到领航状态信息。对任一跟随者，在接收到领航状态信息后，确定所述跟随者当前的跟随航行位置信息以及跟随航行运动信息。由上述说明可知，对形成跟随者的水下无人航行器，也设置有光纤惯导单元，因此，跟随者确定所述跟随者当前相对领航者的跟随航行位置信息以及跟随航行运动信息时，所用的确定方式包括利用光纤惯导单元确定，以能满足实际确定跟随者的跟随航行位置信息以及跟随航行运动信息为准。当然，还可以采用其他技术手段确定跟随航行位置信息以及跟随航行运动信息。
由上述说明可知，确定的跟随航行位置信息具体为跟随者所在位置的经纬度值，跟随航行运动信息一般为跟随者的当前航向。为了能提高跟随者对领航者的跟踪精度，跟随者需要根据跟随航行位置信息、跟随航行运动信息以及所接收到的跟随控制状态信息，确定跟踪状态控制信息，并利用所确定的跟踪状态控制信息控制跟随者的航行，跟随者在所述跟踪状态控制信息下跟踪领航者时，与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配。
跟随者与领航者间相应的编队航行状态与跟随者所接收编队队形信息匹配，具体是指跟随者与领航者之间的位置状态与通过编队队形信息配置的跟随者与领航者之间的位置状态相同，或者两者位置状态的从差异在允许的范围内，具体差异允许范围可以根据实际应用场景等确定，以能满足应用场景为准，此处不再赘述。
本发明实施例中，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态。具体地，通过速度层控制状态以及位置层控制状态能实现对跟随者航行的双层控制，速度层控制状态、位置层控制状态的具体过程可以参考图3的说明。
进一步地，确定跟踪状态控制信息的速度层控制状态时，包括如下步骤：
步骤1、根据领航者当前位置[Sx,Sy]以及当前跟随者接收的编队队形信息[Dissx,Dissy]，确定跟随者需跟随航行的虚拟点Ja，则有
其中，Sx为领航者当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Sy为领航者的当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Dissx为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在x方向的距离坐标，Dissy为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在y方向的距离坐标，Jax为虚拟点Ja在大地直角坐标系下的x轴坐标，Jay为虚拟点Ja在大地直角坐标系下的y轴坐标；
具体地，WGS-84坐标系(World Geodetic System-1984 Coordinate System)，可以采用现有常用的定位手段得到领航者在领航时在WGS-84坐标系下的经纬度，如上述提到采用光纤惯导单元可获取经纬度。在得到领航者当前位置的经纬度后，通过现有常用的高斯投影即可将经纬度转换为大地直角坐标系下的坐标，高斯投影的具体方式以及过程与现有相一致，为本技术领域人员所熟知，此处不再赘述。x轴坐标Sx、y轴坐标Sy、距离坐标Dissx以及距离坐标Dissy相应的单位均为米，领航航向psi的单位为弧度。
步骤2、在虚拟点Ja上建立局部大地直角坐标系，确定跟随者在建立的局部大地直角坐标系下的局部坐标
则有
其中，PGax为虚拟点Ja在局部大地直角坐标系下的x轴坐标，PGay为虚拟点Ja在局部大地直角坐标系下的y轴坐标，Gx为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的x轴坐标，Gy为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的y轴坐标，Gpsi为根据跟随航行运动信息确定跟随者的当前航向角；
本发明实施例中，对于局部大地直角坐标系，具体是指以虚拟点Ja为原点，以跟随者当前航向方向为x轴建立的坐标系，将x轴按照顺时针方向转90度即得到局部大地坐标系的y轴，其中，航向方向为当前航向角所指的方向。横坐标Gx以及纵坐标Gy的单位为米，跟随者的当前航向角Gpsi的单位为弧度。由上述说明可知，跟随航行位置信息一般为利用跟随者的光纤惯导单元确定的经纬度，将经纬度经高斯投影可以得到在大地直角坐标系下的x轴坐标与y轴坐标，具体可以参考上述说明。跟随航行运动信息一般即为当前航向角。
通过上述步骤1确定虚拟点Ja的坐标以及跟随者在局部大地直角坐标下的局部坐标，以能确定跟随者与领航者之间的位置差与实际需要的编队位置差两者对应的关系。
步骤3、确定跟随者的跟踪航行速度GSP，则有
GSP＝LSP+KSP*PGax+KSD*(PGax-PG′ax)
其中，KSP为比例系数、KSD为微分控制系数；PG′ax为前一个虚拟点Ja在相应局部直角坐标系下的x轴坐标。
本发明实施例中，比例系数KSP、微分控制系数KSD即为采用PD控制时的系数，具体取值可以根据经验等确定。由上述说明可知，领航者会定时间隔发送领航状态信息，而跟随者在收到领航状态信息后，一般均会执行速度层控制状态，在执行速度层控制状态后，即会得到一虚拟点Ja。PG′ax即为前次执行速度层控制状态时虚拟点Ja在相应局部坐标系下的横坐标。在第一次执行速度层控制状态时，PG′ax一般可设置为0。
通过上述步骤3确定跟随者的跟踪航行速度GSP后，控制水下无人航行器的螺旋桨等，以调节作为跟随者的水下无人航行器的速度，直至使得跟随者在水下航行的速度与跟踪航行速度GSP匹配，即航行速度为跟踪航行速度GSP，或者与跟踪航行速度GSP之间的差值位于一个允许的范围内，所述差值允许的范围具体可以根据需要确定，以能满足对集群控制为准。
综上，速度层控制状态，根据跟随者与领航者的位置偏差值(所述位置偏差值即为上述PGax-PG′ax)，以通过闭环控制方式确定跟踪航行速度GSP。
进一步地，位置层控制状态即要让跟随者按照编队队形信息，将领航者的轨迹曲线按照编队队形信息实施偏移后对该轨迹曲线实施跟踪，确定跟踪状态控制信息的位置层控制状态时，包括如下步骤：
步骤10、确定跟随者跟随路径时的路径航行角Tpsi，则有
Tpsi＝arctan2(yk+1-yk,xk+1-xk)
其中，(xk,yk)为当前领航路径路段k相应前端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标，(xk+1,yk+1)为当前领航路径路段k相应后端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标；
具体地，由上述说明可知，编队路径信息为多点连线构成，因此，对于任一路径路段，一般存在两个端点，前端点一般为领航者经过后的端点，后端点即为领航者待航行去的端点。从而，根据领航状态信息的领航者当前位置以及当前领航路径路段后，即可能确定当前领航路径路段前端点、后端点相应的经纬度，并在高斯投影转换后即可得到在大地直角坐标系下相应的坐标值。
步骤11、根据跟随者所接收的编队队形信息，确定得到虚拟点P的坐标
则有
其中，Px为虚拟点P在大地直角坐标系下的x轴坐标，Py为虚拟点P在大地直角坐标系下的y轴坐标；
步骤12、跟随者以虚拟点P对领航者实施路径跟踪，确定虚拟点P与领航者路径间的横向偏差e，则有
e＝-(Px-xk)sin(Tpsi)+(Py-yk)cos(Tpsi)
具体实施时，跟随者以虚拟点P对领航者实施路径跟踪，具体是指让虚拟点P指向编队路径，虚拟点P是个点，领航者当前路径是个直线，横向偏差e即点到直线距离。
步骤13、基于横向偏差e，确定跟随者跟踪目标航向角ψd，则有
其中，ψr为前视角，V为前视距离。
本发明实施例中，前视距离V一般为可选的常量，可以根据需要选择，一般为经验者。在确定跟随者跟踪目标航向角ψd后，通过控制尾舵等手段调节当前跟随者的航向角，直至跟随者在水下的航向角与跟随者跟踪目标航向角ψd适配，具体适配的关系可以参考上述说明，此处不再赘述。由上述说明可知，对于位置层控制，通过建立虚拟点P，通过虚拟点P对领航者路径实施跟踪，从而完成跟随者按照编队队形信息对领航者实施路径跟踪的目的。
综上，通过速度层控制状态可得到跟随者在跟随时所需要的跟踪航行速度GSP度，用于跟随者到达跟踪位置的时间。位置层控制状态对水声通信的要求较低，执行位置层控制状态可得到跟随者的跟随者跟踪目标航向角ψd，从而控制跟随者在跟随者跟踪目标航向角ψd下到达预定的位置。
由上述说明可知，在执行上述位置层控制状态时，对于跟随者而言，可只需要知道领航者当前位置所在的路径路段k，如可以配置领航者优先广播发送路径路段k，跟随者根据路径路段k具体执行上述步骤，即可实现位置层控制状态的目的，经位置层控制后即可保证水下无人航行器组网编队的队形能够保持不会乱。由于通过水声通信广播发送路径路段k的通信要求较低，即使得位置层控制状态对水声通信的要求较低。当然，在具体实施时，还可以采用其他通信处理方式，以能确保在水声通信能力较差情况下发送路径路段k为准，此处一一举例说明。
Claims (5)
1.一种适用于水下无人航行器的双层集群控制方法，包括由若干水下无人航行器编队组网成的集群，其特征是：
将集群内的一水下无人航行器配置为领航者，集群内其余的水下无人航行器均配置为跟随者，其中，向领航者发送领航控制状态信息，向每个跟随者发送一相应的跟随控制状态信息，领航控制状态信息包括编队路径信息以及编队速度信息，跟随控制状态信息包括编队路径信息、编队速度信息以及与跟随者相适配的编队队形信息，编队队形信息至少包含表征相应接收所述编队队形信息的跟随者与领航者之间的相对位置信息；
根据所接收到领航控制状态信息航行后，领航者向所有的跟随者发送领航状态信息；
对任一跟随者，在接收到领航状态信息后，确定所述跟随者当前的跟随航行位置信息以及跟随航行运动信息，并根据所确定的跟随航行位置信息、跟随航行运动信息以及所接收到的跟随控制状态信息，确定所述跟随者跟随领航者时的跟踪状态控制信息，以使得跟随者在所述跟踪状态控制信息下跟踪领航者时，与领航者间相应的编队航行状态与所接收跟随控制状态信息内的编队队形信息匹配，其中，所述跟踪状态控制信息包括用于确定跟随者跟踪航行速度的速度层控制状态以及用于确定跟随者跟踪目标航向角的位置层控制状态；
所述领航状态信息包括领航者当前位置、领航航向psi、编队速度LSP以及由领航者当前位置与编队路径信息确定的当前领航路径路段；
领航者以广播发送方式将领航状态信息发送至集群内所有的跟随者，其中，广播发送时，先将领航者当前位置、领航航向psi以及编队速度LSP进行所需的数据压缩，再将当前领航路径路段以及压缩后的领航者当前坐标、领航航向psi、编队速度LSP向跟随者发送；
确定跟踪状态控制信息的速度层控制状态时，包括如下步骤：
步骤1、根据领航者当前位置[Sx,Sy]以及当前跟随者接收的编队队形信息[Dissx,Dissy]，确定跟随者需跟随航行的虚拟点Ja，则有
其中，Sx为领航者当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Sy为领航者的当前位置在WGS-84坐标系下相应的经纬度经高斯投影在大地直角坐标系下的x轴坐标，Dissx为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在x方向的距离坐标，Dissy为根据跟随者所接收编队队形信息确定在大地直角坐标系下与领航者在y方向的距离坐标，Jax为虚拟点Ja在大地直角坐标系下的x轴坐标，Jay为虚拟点Ja在大地直角坐标系下的y轴坐标；
步骤2、在虚拟点Ja上建立局部大地直角坐标系，确定跟随者在建立的局部大地直角坐标系下的局部坐标
则有
其中，PGax为虚拟点Ja在局部大地直角坐标系下的x轴坐标，PGay为虚拟点Ja在局部大地直角坐标系下的y轴坐标，Gx为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的x轴坐标，Gy为根据跟随航行位置信息确定跟随者当前位置在大地直角坐标系下的y轴坐标，Gpsi为根据跟随航行运动信息确定跟随者的当前航向角；
步骤3、确定跟随者的跟踪航行速度GSP，则有
GSP＝LSP+KSP*PGax+KSD*(PGax-PG′ax)
其中，KSP为比例系数、KSD为微分控制系数；PG′ax为前一个虚拟点Ja在相应局部直角坐标系下的x轴坐标；
确定跟踪状态控制信息的位置层控制状态时，包括如下步骤：
步骤10、确定跟随者跟随路径时的路径航行角Tpsi，则有
Tpsi＝arctan2(yk+1-yk,xk+1-xk)
其中，(xk,yk)为当前领航路径路段k相应前端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标，(xk+1,yk+1)为当前领航路径路段k相应后端点在WGS-84坐标系下的经纬度经高斯投影在大地直角坐标系下的坐标；
步骤11、根据跟随者所接收的编队队形信息，确定得到虚拟点P的坐标
则有
其中，Px为虚拟点P在大地直角坐标系下的x轴坐标，Py为虚拟点P在大地直角坐标系下的y轴坐标；
步骤12、跟随者以虚拟点P对领航者实施路径跟踪，确定虚拟点P与领航者路径间的横向偏差e，则有
e＝-(Px-xk)sin(Tpsi)+(Py-yk)cos(Tpsi)
步骤13、基于横向偏差e，确定跟随者跟踪目标航向角ψd，则有
其中，ψr为前视角，V为前视距离。
2.根据权利要求1所述的适用于水下无人航行器的双层集群控制方法，其特征是：还包括与集群适配的岸站，通过岸站与集群内水下无人航行器无线通信，且岸站与集群内所有的水下无人航行器通信后，配置得到所需的领航者以及与所配置领航者相对应的跟随者；
岸站与集群间的无线通信方式包括广播通信以及单点通信，其中，岸站通过广播通信方式将编队路径信息以及编队速度信息发送至领航者以及所有的跟随者，将向跟随者以单点通信方式发送对应的编队队形信息。
3.根据权利要求1所述的适用于水下无人航行器的双层集群控制方法，其特征是：对领航者当前坐标数据压缩时，将领航者当前位置相应的经度值、纬度值转换成3字节的int型数据；对领航航向psi数据压缩时，将领航航向psi转换成2字节的short型数据；对编队速度LSP数据压缩时，将编队速度LSP信息转换成2字节的short型数据。
4.根据权利要求1或2所述的适用于水下无人航行器的双层集群控制方法，其特征是：跟随者确定所述跟随者当前相对领航者的跟随航行位置信息以及跟随航行运动信息时，所用的确定方式包括利用光纤惯导单元确定。
5.根据权利要求1或2所述的适用于水下无人航行器的双层集群控制方法，其特征是：领航者以定时间隔方式向所有的跟随者发送领航状态信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210276041.8A CN114647250B (zh) | 2022-03-21 | 2022-03-21 | 适用于水下无人航行器的双层集群控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210276041.8A CN114647250B (zh) | 2022-03-21 | 2022-03-21 | 适用于水下无人航行器的双层集群控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114647250A CN114647250A (zh) | 2022-06-21 |
| CN114647250B true CN114647250B (zh) | 2023-05-23 |
Family
ID=81995857
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210276041.8A Active CN114647250B (zh) | 2022-03-21 | 2022-03-21 | 适用于水下无人航行器的双层集群控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114647250B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115686004B (zh) * | 2022-10-27 | 2024-07-19 | 南京长峰航天电子科技有限公司 | 一种基于无人船分散式编队控制的路径动态规划方法 |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106896824A (zh) * | 2015-12-17 | 2017-06-27 | 中国科学院沈阳自动化研究所 | 一种基于目标函数的多auv编队控制方法 |
| US10114384B2 (en) * | 2016-09-13 | 2018-10-30 | Arrowonics Technologies Ltd. | Formation flight path coordination of unmanned aerial vehicles |
| CN107168329B (zh) * | 2017-06-20 | 2018-03-27 | 南京长峰航天电子科技有限公司 | 基于跟随领航者法的航速航向协同控制编队航行控制方法 |
| CN108549394A (zh) * | 2018-04-12 | 2018-09-18 | 哈尔滨工程大学 | 一种基于领航者和虚拟领航者的多auv直线编队控制方法 |
| CN113960994B (zh) * | 2020-07-01 | 2024-03-26 | 中国船舶集团有限公司第七一一研究所 | 一种多无人艇协同航行的s面自适应控制算法 |
| CN112148021B (zh) * | 2020-09-10 | 2021-10-19 | 北京理工大学 | 基于二叉树拓扑结构和特定规则的多智能体编队变换方法 |
| CN112148023A (zh) * | 2020-10-10 | 2020-12-29 | 上海海事大学 | 一种自治水下机器人等平面水下编队方法 |
| CN112731942A (zh) * | 2020-12-30 | 2021-04-30 | 哈尔滨工程大学 | 一种基于改进领航者虚拟结构法的多auv编队控制方法 |
| CN114089761A (zh) * | 2021-11-22 | 2022-02-25 | 江苏科技大学 | 一种基于ros的领航跟随者多无人船编队控制系统及其方法 |
| CN114115262B (zh) * | 2021-11-23 | 2024-02-02 | 上海交通大学 | 基于方位角信息的多auv执行器饱和协同编队控制系统和方法 |
-
2022
- 2022-03-21 CN CN202210276041.8A patent/CN114647250B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114647250A (zh) | 2022-06-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107389064B (zh) | 一种基于惯性导航的无人车变道控制方法 | |
| US11305854B2 (en) | Deep-sea low-cost long-endurance collaborative navigation and positioning system | |
| WO2016197986A1 (zh) | 一种无人机高精度自主避障飞行方法 | |
| CN110609570A (zh) | 一种基于无人机的自主避障巡检方法 | |
| CN106352872B (zh) | 一种无人机自主导航系统及其导航方法 | |
| CN103176451B (zh) | 一种舰船辅助航行及编队指挥的方法 | |
| CN107167126B (zh) | 一种自主式水下机器人组合导航方法和系统 | |
| CN106249755A (zh) | 一种无人机自主导航系统及导航方法 | |
| CN105843249A (zh) | 基于Pixhawk飞控的无人机自动巡航系统及其巡航方法 | |
| CN109813306A (zh) | 一种无人车规划轨迹卫星定位数据可信度计算方法 | |
| CN104180803A (zh) | 一种应用于无人机的非相似双余度组合导航装置 | |
| CN111595348A (zh) | 一种自主水下航行器组合导航系统的主从式协同定位方法 | |
| CN114647250B (zh) | 适用于水下无人航行器的双层集群控制方法 | |
| CN208569400U (zh) | 无人船用协同模式控制系统 | |
| CN103913163A (zh) | 船载卫星通信地球站a-e-c三轴天线坐标计算方法 | |
| CN109991993A (zh) | 基于rtk差分定位的天地双飞控系统及飞控 | |
| CN113037395A (zh) | 一种水上水下跨域无人装备协同作业方法 | |
| CN109508040A (zh) | 一种无人机精准定位降落系统及方法 | |
| CN115202383B (zh) | 一种无人机多维航迹表达及生成方法 | |
| CN116755472A (zh) | 一种基于通信链路切换的海上异构无人平台、编队控制方法 | |
| CN108287538A (zh) | 一种基于rtk技术无人驾驶船系统 | |
| CN103019249A (zh) | 一种应用在无人机上的提高导航解算精度的方法 | |
| CN113721188B (zh) | 拒止环境下的多无人机自身定位与目标定位方法 | |
| CN116150873A (zh) | 一种无人艇数字能力模型的搭建方法 | |
| CN112698273B (zh) | 一种多auv单标测距的协同作业方法 |
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 |