CN114115297B - 基于视线导引法的auv跟踪控制方法及装置 - Google Patents
基于视线导引法的auv跟踪控制方法及装置 Download PDFInfo
- Publication number
- CN114115297B CN114115297B CN202210078947.9A CN202210078947A CN114115297B CN 114115297 B CN114115297 B CN 114115297B CN 202210078947 A CN202210078947 A CN 202210078947A CN 114115297 B CN114115297 B CN 114115297B
- Authority
- CN
- China
- Prior art keywords
- auv
- coordinate system
- target
- position information
- rectangular coordinate
- 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 84
- 238000005259 measurement Methods 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 5
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000006854 communication Effects 0.000 description 17
- 238000004891 communication Methods 0.000 description 16
- 230000015654 memory Effects 0.000 description 14
- 238000012545 processing Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 5
- 230000002093 peripheral effect Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000004804 winding Methods 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/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
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)
- Navigation (AREA)
Abstract
本发明公开了一种于视线导引法的AUV跟踪控制方法及装置，该方法包括建立原点为初始位置的第一直角坐标系以及原点为观测点的第二直角坐标系，通过间隔预设时间时目标AUV在第二直角坐标系的位置信息，确定目标AUV在第一直角坐标系下的艏向角，进而根据艏向角驱动目标AUV的航行，通过控制目标AUV航行的偏向角和在第二直角坐标系下的位置信息始终在预设条件内，进而控制目标AUV在误差范围内按预设预定航迹前行。本发明通过对AUV的实时位置进行准确的定位，同时改善视线导轨法在目标相隔较远时误差比较大的问题，通过动态改变控制AUV航向时间间隔提高了系统控制效率，也提高了AUV航行控制的精确度。
Description
技术领域
本发明涉及自主式水下潜行器技术领域，尤其涉及到一种基于视线导引法的AUV跟踪控制方法及装置。
背景技术
自主式水下潜行器（AUV）作为一种可移动的智能搭载平台，在海洋的环境探测，以及在现代化军事战争的大量使用，已经使之成为现在许多国家研究的热点。国外已经拥有很多AUV的应用，在海洋的研究领域，它具有小巧、快速、便捷的优点。但是，国内的对于AUV的研究进行得很晚，自主设计研究的AUV很少，控制的策略应用也很少。AUV主要的控制方法有：
（1）视线法航路点导引（LOS）。LOS的方法用线性方法或者反演的方法来设计航向角的控制器，依据的是AUV的相对于定坐标系下观测点的相对位置，但是它往往不能获得AUV比较准确的位置状态且在AUV距离较远时漂角的误差很大，忽略了动态特性。它通过不断调整AUV运动的方向使之向预定的航线行驶，自身航线与期望航线的偏离误差会造成航程损失。
（2）模糊航路点跟踪控制。是一种基于模糊控制提出来的控制理论，包括PID反馈以及根据不同时态下目标的特点进行模糊规则的制定以及修改。得出模糊的控制器对AUV航行路线进行控制。然而这种方法受到经验影响很大，需要人为的制定规则和模糊隶属度函数确定，受到很多人为的操作因素影响。因此，如何提高AUV航行控制的精确度，是一个亟需解决的技术问题。
上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。
发明内容
本发明的主要目的在于提供一种基于视线导引法的AUV跟踪控制方法及装置，旨在解决目前基于视线导引法的AUV跟踪控制效率不高的技术问题。
为实现上述目的，本发明提供一种基于视线导引法的AUV跟踪控制方法，所述方法包括以下步骤：
根据目标AUV的初始位置，建立第一直角坐标系；
获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系；
每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点；
检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系；
基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行。
可选的，所述第一直角坐标系的原点为目标AUV的初始位置，所述第一直角坐标系的X轴为目标AUV的初始艏向，所述第一直角坐标系的Y轴为目标AUV在初始位置关于初始艏向的垂直方向。
可选的，所述检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤，具体包括：
获取GPS原始数据和IGS数据，构建距离测量方程；
将所述距离测量方程线性化，获得线性观测方程，并基于自适应抗差卡尔曼滤波进行噪声估计，获得目标AUV的位置坐标；
基于所述位置坐标，确定目标AUV在第一直角坐标系的第二位置信息。
可选的，所述基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点步骤，具体包括：
获取第二位置信息在第一直角坐标系中的X轴坐标；
基于所述第二位置信息的X轴坐标，确定关联观测点；其中，所述关联观测点包括相邻的第一观测点和第二观测点，所述第二位置信息对应的X轴坐标在第一观测点和第二观测点对应的X轴坐标的范围内。
可选的，所述观测点集包括多个顺序排列的观测点，所述第二直角坐标系的原点为第一位置信息对应的观测点位置，所述第二直角坐标系的X轴为相邻两个所述观测点的连接线，所述第二直角坐标系的Y轴为第一位置信息对应的观测点位置在相邻两个所述观测点的连接线的垂直方向。
可选的，所述基于所述第三位置信息，确定目标AUV的艏向角步骤，具体包括：
基于所述第三位置信息，确定所述目标AUV的前视距离；其中，所述前视距离为目标AUV与第一位置信息对应的观测点的距离；
根据所述第三位置信息对应的Y轴坐标和所述前视距离，获得目标AUV的航迹角，并根据所述航迹角确定目标AUV在第二直角坐标系下的艏向角；
利用第二直角坐标系与第一直角坐标系的坐标转换关系，获得目标AUV在第一直角坐标系下的艏向角。
可选的，所述基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行步骤之后，所述方法还包括：
获取目标AUV的偏向角；其中，所述偏向角为目标AUV的艏向与第一观测点和第二观测点连接线的夹角；
判断目标AUV航行时第三位置信息对应的Y轴坐标与偏向角是否分别满足第一预设条件和第二预设条件；其中，所述第一预设条件为所述第三位置信息对应的Y轴坐标在相邻的预设数量时间节点皆小于预设到达距离，所述第二预设条件为所述偏向角在相邻的预设数量时间节点皆小于预设到达角度；
若是，增大所述预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤；
若否，将所述预设时间调整为初始的预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤。
此外，为了实现上述目的，本发明还提供了一种基于视线导引法的AUV跟踪控制装置，所述基于视线导引法的AUV跟踪控制装置包括：
建立模块，用于根据目标AUV的初始位置，建立第一直角坐标系；
获取模块，用于获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系；
确定模块，用于每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点；
检测模块，用于检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系；
驱动模块，用于基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行。
此外，为了实现上述目的，本发明还提供了一种基于视线导引法的AUV跟踪控制设备，所述设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于视线导引法的AUV跟踪控制程序，所述基于视线导引法的AUV跟踪控制程序被所述处理器执行时实现上述的基于视线导引法的AUV跟踪控制方法的步骤。
此外，为了实现上述目的，本发明还提供了一种存储介质，所述存储介质上存储有基于视线导引法的AUV跟踪控制程序，所述基于视线导引法的AUV跟踪控制程序被处理器执行时实现上述的基于视线导引法的AUV跟踪控制方法的步骤。
本发明实施例提出的一种基于视线导引法的AUV跟踪控制方法及装置，该方法包括建立原点为初始位置的第一直角坐标系以及原点为观测点的第二直角坐标系，通过间隔预设时间时目标AUV在第二直角坐标系的位置信息，确定目标AUV在第一直角坐标系下的艏向角，进而根据艏向角驱动目标AUV的航行，通过控制目标AUV航行的偏向角和在第二直角坐标系下的位置信息始终在预设条件内，进而控制目标AUV在误差范围内按预设预定航迹前行。本发明通过对AUV的实时位置进行准确的定位，同时改善视线导轨法在目标相隔较远时误差比较大的问题，通过动态改变控制AUV航向时间间隔提高了系统控制效率，也提高了AUV航行控制的精确度。
附图说明
图1为本发明基于视线导引法的AUV跟踪控制设备的结构示意图；
图2为本发明基于视线导引法的AUV跟踪控制方法的流程示意图；
图3为本发明基于视线导引法的AUV跟踪控制方法的原理示意图；
图4为本发明基于视线导引法的AUV跟踪控制装置的结构框图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
参照图1，图1为本发明实施例方案涉及的基于视线导引法的AUV跟踪控制设备的结构示意图。
设备可以是移动电话、智能电话、笔记本电脑、数字广播接收器、个人数字助理(PDA)、平板电脑(PAD)等用户设备(User Equipment，UE)、手持设备、车载设备、可穿戴设备、计算设备或连接到无线调制解调器的其它处理设备、移动台(Mobile station，MS)等。设备可能被称为用户终端、便携式终端、台式终端等。
通常，设备包括：至少一个处理器301、存储器302以及存储在所述存储器上并可在所述处理器上运行的基于视线导引法的AUV跟踪控制程序，所述基于视线导引法的AUV跟踪控制程序配置为实现如前所述的基于视线导引法的AUV跟踪控制方法的步骤。
处理器301可以包括一个或多个处理核心，比如4核心处理器、8核心处理器等。处理器301可以采用DSP(Digital Signal Processing，数字信号处理)、FPGA(Field－Programmable Gate Array，现场可编程门阵列)、PLA(Programmable Logic Array，可编程逻辑阵列)中的至少一种硬件形式来实现。处理器301也可以包括主处理器和协处理器，主处理器是用于对在唤醒状态下的数据进行处理的处理器，也称CPU(CentralProcessingUnit，中央处理器)；协处理器是用于对在待机状态下的数据进行处理的低功耗处理器。在一些实施例中，处理器301可以在集成有GPU(Graphics Processing Unit，图像处理器)，GPU用于负责显示屏所需要显示的内容的渲染和绘制。处理器301还可以包括AI(Artificial Intelligence，人工智能)处理器，该AI处理器用于处理有关基于视线导引法的AUV跟踪控制操作，使得基于视线导引法的AUV跟踪控制模型可以自主训练学习，提高效率和准确度。
存储器302可以包括一个或多个计算机可读存储介质，该计算机可读存储介质可以是非暂态的。存储器302还可包括高速随机存取存储器，以及非易失性存储器，比如一个或多个磁盘存储设备、闪存存储设备。在一些实施例中，存储器302中的非暂态的计算机可读存储介质用于存储至少一个指令，该至少一个指令用于被处理器301所执行以实现本申请中方法实施例提供的基于视线导引法的AUV跟踪控制方法。
在一些实施例中，终端还可选包括有：通信接口303和至少一个外围设备。处理器301、存储器302和通信接口303之间可以通过总线或信号线相连。各个外围设备可以通过总线、信号线或电路板与通信接口303相连。具体地，外围设备包括：射频电路304、显示屏305和电源306中的至少一种。
通信接口303可被用于将I/O(Input/Output，输入/输出)相关的至少一个外围设备连接到处理器301和存储器302。通信接口303通过外围设备用于接收用户上传的多个移动终端的移动轨迹以及其他数据。在一些实施例中，处理器301、存储器302和通信接口303被集成在同一芯片或电路板上；在一些其他实施例中，处理器301、存储器302和通信接口303中的任意一个或两个可以在单独的芯片或电路板上实现，本实施例对此不加以限定。
射频电路304用于接收和发射RF(Radio Frequency，射频)信号，也称电磁信号。射频电路304通过电磁信号与通信网络以及其他通信设备进行通信，从而可获取多个移动终端的移动轨迹以及其他数据。射频电路304将电信号转换为电磁信号进行发送，或者，将接收到的电磁信号转换为电信号。可选地，射频电路304包括：天线系统、RF收发器、一个或多个放大器、调谐器、振荡器、数字信号处理器、编解码芯片组、用户身份模块卡等等。射频电路304可以通过至少一种无线通信协议来与其它终端进行通信。该无线通信协议包括但不限于：城域网、各代移动通信网络(2G、3G、4G及5G)、无线局域网和/或WiFi(WirelessFidelity，无线保真)网络。在一些实施例中，射频电路304还可以包括NFC(Near FieldCommunication，近距离无线通信)有关的电路，本申请对此不加以限定。
显示屏305用于显示UI(User Interface，用户界面)。该UI可以包括图形、文本、图标、视频及其它们的任意组合。当显示屏305是触摸显示屏时，显示屏305还具有采集在显示屏305的表面或表面上方的触摸信号的能力。该触摸信号可以作为控制信号输入至处理器301进行处理。此时，显示屏305还可以用于提供虚拟按钮和/或虚拟键盘，也称软按钮和/或软键盘。在一些实施例中，显示屏305可以为一个，电子设备的前面板；在另一些实施例中，显示屏305可以为至少两个，分别设置在电子设备的不同表面或呈折叠设计；在再一些实施例中，显示屏305可以是柔性显示屏，设置在电子设备的弯曲表面上或折叠面上。甚至，显示屏305还可以设置成非矩形的不规则图形，也即异形屏。显示屏305可以采用LCD(LiquidCrystal Display，液晶显示屏)、OLED(Organic Light-Emitting Diode,有机发光二极管)等材质制备。
电源306用于为电子设备中的各个组件进行供电。电源306可以是交流电、直流电、一次性电池或可充电电池。当电源306包括可充电电池时，该可充电电池可以支持有线充电或无线充电。该可充电电池还可以用于支持快充技术。
本领域技术人员可以理解，图1中示出的结构并不构成对基于视线导引法的AUV跟踪控制设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
本发明实施例提供了一种基于视线导引法的AUV跟踪控制方法，参照图2，图2为本发明基于视线导引法的AUV跟踪控制方法实施例的流程示意图。
本实施例中，所述基于视线导引法的AUV跟踪控制方法包括以下步骤：
步骤S100，根据目标AUV的初始位置，建立第一直角坐标系。
具体而言，如图3所示，第一直角坐标系的原点为目标AUV的初始位置，所述第一直角坐标系的X轴为目标AUV的初始艏向，所述第一直角坐标系的Y轴为目标AUV在初始位置关于初始艏向的垂直方向。
步骤S200，获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系。
具体而言，观测点集包括多个顺序排列的观测点，所述第二直角坐标系的原点为第一位置信息对应的观测点位置，所述第二直角坐标系的X轴为相邻两个所述观测点的连接线，所述第二直角坐标系的Y轴为第一位置信息对应的观测点位置在相邻两个所述观测点的连接线的垂直方向。
容易理解的，观测点集包括AUV水平面路径上的一系列观测点A1…An，由第i个观测点Ai的GPS位置计算得到在第一直角坐标系O中AUV要前进的水平面上的路径投影点(mi,ni)，依次建立从点Ai到Ai+1的坐标系Zi，它的x轴由路径上的点Ai到Ai+1。
步骤S300，每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点。
具体而言，在检测目标AUV当前时刻在第一直角坐标系的第二位置信息时，可通过获取GPS原始数据和IGS数据，构建距离测量方程；将所述距离测量方程线性化，获得线性观测方程，并基于自适应抗差卡尔曼滤波进行噪声估计，获得目标AUV的位置坐标；基于所述位置坐标，确定所述目标AUV在第一直角坐标系的第二位置信息。
容易理解的，由于直接使用卫星的定位系统存在误差，并且AUV水下也不能与卫星进行通信，本实施例设计一种通过自适应抗差卡尔曼滤波法进行AUV的精确GPS定位的位置信息获取方法。
位置信息获取利用AUV搭载的GPS与卫星通信获得GPS原始数据。定位要使用的钟差和卫星导轨数据采用的是经过高精度计算的IGS数据，之后对钟差和卫星导轨数据进行提取并且预处理之后获得需要的完整数据。
在此之后，处理获得的星历以及钟差的数据，由于转机观测时间比较大，中间时间星历以及钟差数据存在空白，在进行定位的当前时刻不一定在卫星的观测时间节点之上，这时可以通过内插法得到一定精度下任意时刻的钟差以及轨道信息。用模型改正法对于卫星偏差、相对缠绕效应、电离效应等引起的误差项建模计算误差影响大小，之后进行修正。
最后，为了提高数据精度，由于天气、信号、遮挡等造成接收机与GPS卫星短暂时间不能通信产生的周跳探测，采用卡尔曼滤波法进行周跳的探测与修复。同时删除数据之中的不合格数据，比如删除历元比较少的弧段以及删除当卫星观测的角度比较低的弧段。利用最小二乘法确定噪声矩阵，并由GPS数据得到位置的初始近似值。利用基于自适应抗差卡尔曼滤波方法进行噪声估计，以预测的向量来对噪声进行更新，建立方程进行定位求解，得到位置信息。
具体而言，在确定目标AUV当前运行的第一观测点和期望运行的第二观测点时，可通过获取第二位置信息在第一直角坐标系中的X轴坐标；基于所述第二位置信息的X轴坐标，确定关联观测点；其中，所述关联观测点包括相邻的第一观测点和第二观测点，所述第二位置信息对应的X轴坐标在第一观测点和第二观测点对应的X轴坐标的范围内。
容易理解的，每隔预设时间，获取目标AUV当前时刻在第一直角坐标系的第二位置信息，这时由经过与初始坐标原点的GPS位置对比，经过计算得到它在O坐标系下的坐标(x,y)，AUV在初始坐标系下横坐标x与AUV设定的路径点在初始坐标系下的x坐标mi比较，得到它在当前位置期望运行的下一个点Aj。
具体为：当mi≤x≤mi+1或者mi≥x≥mi+1，则认为此时AUV在Zi坐标系之中，Aj点即要运动的下一个点就为Ai+1。
步骤S400，检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系。
具体而言，根据目标AUV的第二位置信息，在目标第二直角坐标系进行坐标变换，计算AUV在Zi坐标系下的坐标
，同时在Zi坐标系下计算AUV艏向与规划路径的偏向角
。
具体为：第三步已知坐标系Zi的横坐标轴与坐标系O的横坐标夹角为
，Zi坐标原点Ai在初始坐标系下的坐标(mi, ni)计算得到Zi坐标系下的
：
偏向角
计算：
步骤S500，基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行。
具体而言，在确定目标AUV的艏向角时，可通过基于所述第三位置信息，确定所述目标AUV的前视距离；其中，所述前视距离为目标AUV与第一位置信息对应的观测点的距离；根据所述第三位置信息对应的Y轴坐标和所述前视距离，获得目标AUV的航迹角，并根据所述航迹角确定目标AUV在第二直角坐标系下的艏向角；利用第二直角坐标系与第一直角坐标系的坐标转换关系，获得目标AUV在第一直角坐标系下的艏向角。
需要说明的是，要使AUV沿着预定的轨迹运行，即要使
为0，且航迹角与Zi坐标系下相对于x轴的偏向角
趋近于0。于是需要实时调整艏向使之按轨迹运行。
进一步的，考虑漂角，此时可以使用视线导引LOS算法得出期望的艏向角，由于是不断动态建立坐标系，在相对的观测点之上AUV的距离比较进，这时可以减小AUV漂角的影响并且减小由于三角函数近似计算产生的误差。
进一步的，设漂角为β，使用视线导引LOS算法，因为在变换的坐标系之下求方位角，对于投影的Zi坐标系，方位角δ等0。前视距离Δ为点
到Zi坐标轴原点Ai的距离，此时得到航迹角
为：
此时计算的是相对于Zi坐标系下的艏向角，要得到相对于初始坐标系下的艏向角θ为：
进一步的航向角控制器转动使AUV在初始的标准坐标系下按得到的艏向角方向前进。
在另一实施例中，在确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行步骤之后，还可动态调整检测目标AUV当前时刻在第一直角坐标系的第二位置信息，进而控制目标AUV的控制精确度。
具体而言，调整检测目标AUV当前时刻在第一直角坐标系的第二位置信息可通过获取目标AUV的偏向角；其中，所述偏向角为目标AUV的艏向与第一观测点和第二观测点连接线的夹角；判断目标AUV航行时第三位置信息对应的Y轴坐标与偏向角是否分别满足第一预设条件和第二预设条件；若是，增大所述预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤；若否，将所述预设时间调整为初始的预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤。
其中，所述第一预设条件为所述第三位置信息对应的Y轴坐标在相邻的预设数量时间节点皆小于预设到达距离，所述第二预设条件为所述偏向角在相邻的预设数量时间节点皆小于预设到达角度。
容易理解的，一般来说AUV与卫星通信过程比较复杂，但是要求的精度也很高，需要改变预设时间的时间间隔从而提高效率。取时间间隔为kt测试，t这里取14分钟，k初始值为1，并且不停的调整AUV的运动方向使之按预定方向前进。
当变换后的
在相邻的五个测试时间节点都小于预设到达距离a且偏向角
小于预设到达角度b时，此时加大测试的时间，令k=k+1，并返回步骤S300。
当变换后的
大于预设到达距离a或偏向角
大于预设到达角度b时，令k=1，返回步骤S300，使AUV在水平方向按预定航迹前行。
在本实施例中，提供一种基于视线导引法的AUV跟踪控制方法，通过建立原点为初始位置的第一直角坐标系以及原点为观测点的第二直角坐标系，通过间隔预设时间时目标AUV在第二直角坐标系的位置信息，确定目标AUV在第一直角坐标系下的艏向角，进而根据艏向角驱动目标AUV的航行，通过控制目标AUV航行的偏向角和在第二直角坐标系下的位置信息始终在预设条件内，进而控制目标AUV在误差范围内按预设预定航迹前行。本发明通过对AUV的实时位置进行准确的定位，同时改善视线导轨法在目标相隔较远时误差比较大的问题，通过动态改变控制AUV航向时间间隔提高了系统控制效率，也提高了AUV航行控制的精确度。
参照图4，图4为本发明基于视线导引法的AUV跟踪控制装置第一实施例的结构框图。
如图4所示，本发明实施例提出的基于视线导引法的AUV跟踪控制装置包括：
建立模块10，用于根据目标AUV的初始位置，建立第一直角坐标系；
获取模块20，用于获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系；
确定模块30，用于每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点；
检测模块40，用于检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系；
驱动模块50，用于基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行。
本发明基于视线导引法的AUV跟踪控制装置的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。
此外，本发明实施例还提出一种存储介质，所述存储介质上存储有基于视线导引法的AUV跟踪控制程序，所述基于视线导引法的AUV跟踪控制程序被处理器执行时实现如上文所述的基于视线导引法的AUV跟踪控制方法的步骤。因此，这里将不再进行赘述。另外，对采用相同方法的有益效果描述，也不再进行赘述。对于本申请所涉及的计算机可读存储介质实施例中未披露的技术细节，请参照本申请方法实施例的描述。确定为示例，程序指令可被部署为在一个计算设备上执行，或者在位于一个地点的多个计算设备上执行，又或者，在分布在多个地点且通过通信网络互连的多个计算设备上执行。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，上述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，上述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(RandomAccessMemory，RAM)等。
另外需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用CPU、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本发明而言更多情况下软件程序实现是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在可读取的存储介质中，如计算机的软盘、U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、磁碟或者光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
Claims (7)
1.一种基于视线导引法的AUV跟踪控制方法，其特征在于，所述方法包括以下步骤：
根据目标AUV的初始位置，建立第一直角坐标系；
获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系；
每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点；
检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系；
基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行；
获取目标AUV的偏向角；其中，所述偏向角为目标AUV的艏向与第一观测点和第二观测点连接线的夹角；
判断目标AUV航行时第三位置信息对应的Y轴坐标与偏向角是否分别满足第一预设条件和第二预设条件；其中，所述第一预设条件为所述第三位置信息对应的Y轴坐标在相邻的预设数量时间节点皆小于预设到达距离，所述第二预设条件为所述偏向角在相邻的预设数量时间节点皆小于预设到达角度；
若是，增大所述预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤；
若否，将所述预设时间调整为初始的预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤。
2.如权利要求1所述的基于视线导引法的AUV跟踪控制方法，其特征在于，所述第一直角坐标系的原点为目标AUV的初始位置，所述第一直角坐标系的X轴为目标AUV的初始艏向，所述第一直角坐标系的Y轴为目标AUV在初始位置关于初始艏向的垂直方向。
3.如权利要求2所述的基于视线导引法的AUV跟踪控制方法，其特征在于，所述检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤，具体包括：
获取GPS原始数据和IGS数据，构建距离测量方程；
将所述距离测量方程线性化，获得线性观测方程，并基于自适应抗差卡尔曼滤波进行噪声估计，获得目标AUV的位置坐标；
基于所述位置坐标，确定目标AUV在第一直角坐标系的第二位置信息。
4.如权利要求2所述的基于视线导引法的AUV跟踪控制方法，其特征在于，所述基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点步骤，具体包括：
获取第二位置信息在第一直角坐标系中的X轴坐标；
基于所述第二位置信息的X轴坐标，确定关联观测点；其中，所述关联观测点包括相邻的第一观测点和第二观测点，所述第二位置信息对应的X轴坐标在第一观测点和第二观测点对应的X轴坐标的范围内。
5.如权利要求1所述的基于视线导引法的AUV跟踪控制方法，其特征在于，所述观测点集包括多个顺序排列的观测点，所述第二直角坐标系的原点为第一位置信息对应的观测点位置，所述第二直角坐标系的X轴为当前观测点与下一观测点之间的连接线，所述第二直角坐标系的Y轴为第一位置信息对应的观测点位置在当前观测点与下一观测点之间的连接线的垂直方向。
6.如权利要求5所述的基于视线导引法的AUV跟踪控制方法，其特征在于，所述基于所述第三位置信息，确定目标AUV的艏向角步骤，具体包括：
基于所述第三位置信息，确定所述目标AUV的前视距离；其中，所述前视距离为目标AUV与当前观测点的距离；
根据所述第三位置信息对应的Y轴坐标和所述前视距离，获得目标AUV的航迹角，并根据所述航迹角确定目标AUV在第二直角坐标系下的艏向角；
利用第二直角坐标系与第一直角坐标系的坐标转换关系，获得目标AUV在第一直角坐标系下的艏向角。
7.一种基于视线导引法的AUV跟踪控制装置，其特征在于，所述基于视线导引法的AUV跟踪控制装置包括：
建立模块，用于根据目标AUV的初始位置，建立第一直角坐标系；
获取模块，用于获取观测点集，检测所述观测点集中每个观测点在第一直角坐标系下的第一位置信息，并根据相邻两个所述观测点建立第二直角坐标系；
确定模块，用于每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息，并基于所述第一位置信息和所述第二位置信息，确定目标AUV当前运行的第一观测点和期望运行的第二观测点；
检测模块，用于检测目标AUV在目标第二直角坐标系的第三位置信息；其中，所述目标第二直角坐标系为第一观测点和第二观测点对应的第二直角坐标系；
驱动模块，用于基于所述第三位置信息，确定目标AUV的艏向角，并基于所述艏向角驱动所述目标AUV航行；
循环模块，用于获取目标AUV的偏向角；其中，所述偏向角为目标AUV的艏向与第一观测点和第二观测点连接线的夹角；判断目标AUV航行时第三位置信息对应的Y轴坐标与偏向角是否分别满足第一预设条件和第二预设条件；其中，所述第一预设条件为所述第三位置信息对应的Y轴坐标在相邻的预设数量时间节点皆小于预设到达距离，所述第二预设条件为所述偏向角在相邻的预设数量时间节点皆小于预设到达角度；若是，增大所述预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤；若否，将所述预设时间调整为初始的预设时间，并返回执行每隔预设时间，检测目标AUV当前时刻在第一直角坐标系的第二位置信息步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210078947.9A CN114115297B (zh) | 2022-01-24 | 2022-01-24 | 基于视线导引法的auv跟踪控制方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210078947.9A CN114115297B (zh) | 2022-01-24 | 2022-01-24 | 基于视线导引法的auv跟踪控制方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114115297A CN114115297A (zh) | 2022-03-01 |
| CN114115297B true CN114115297B (zh) | 2022-05-10 |
Family
ID=80361203
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210078947.9A Active CN114115297B (zh) | 2022-01-24 | 2022-01-24 | 基于视线导引法的auv跟踪控制方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114115297B (zh) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019044635A1 (ja) * | 2017-08-31 | 2019-03-07 | 三菱電機株式会社 | 通信装置 |
| CN113272673A (zh) * | 2018-12-26 | 2021-08-17 | 洛希克斯有限公司 | 基于节点和各种rf支持装置之间的射频通信使用测距和三角测量来确定无线传感器节点位置的系统和方法 |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014092840A (ja) * | 2012-11-01 | 2014-05-19 | Nec Corp | 飛翔体誘導システム、飛翔体誘導信号発生回路、飛翔体誘導方法及び飛翔体誘導プログラム |
| CN103618981B (zh) * | 2013-11-29 | 2016-08-17 | 成都航天通信设备有限责任公司 | 一种基于usb的双通道音频系统及其工作方法 |
| JP6821929B2 (ja) * | 2016-03-17 | 2021-01-27 | 株式会社Ihi | 航走管理装置及び航走管理方法 |
| CN106444838A (zh) * | 2016-10-25 | 2017-02-22 | 西安兰海动力科技有限公司 | 一种自主水下航行器的精确航路跟踪控制方法 |
| CN106707322B (zh) * | 2016-12-30 | 2017-11-28 | 立得空间信息技术股份有限公司 | 基于rtk/sins的高动态定位定姿系统及方法 |
| CN107561495A (zh) * | 2017-08-28 | 2018-01-09 | 成都航天通信设备有限责任公司 | 一种基于激光跟踪的室内三维定位方法 |
| CN109540154B (zh) * | 2019-01-23 | 2019-12-27 | 中国科学院声学研究所 | 一种基于粒子滤波算法的水声导航定位方法 |
| WO2021118442A1 (en) * | 2019-12-11 | 2021-06-17 | Delaval Holding Ab | Method and system for tracking position of a livestock animal |
| CN112197761B (zh) * | 2020-07-24 | 2022-07-19 | 北京理工大学 | 一种高精度多旋翼机协同定位方法及系统 |
| CN111854746A (zh) * | 2020-07-31 | 2020-10-30 | 南京理工大学 | Mimu/csac/高度计辅助卫星接收机的定位方法 |
-
2022
- 2022-01-24 CN CN202210078947.9A patent/CN114115297B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019044635A1 (ja) * | 2017-08-31 | 2019-03-07 | 三菱電機株式会社 | 通信装置 |
| CN113272673A (zh) * | 2018-12-26 | 2021-08-17 | 洛希克斯有限公司 | 基于节点和各种rf支持装置之间的射频通信使用测距和三角测量来确定无线传感器节点位置的系统和方法 |
Non-Patent Citations (1)
| Title |
|---|
| Design of Line-of-Sight Guidance Law and a Constrained Optimal Controller for an Autonomous Underwater Vehicle;Raja Rout等;《IEEE Transactions on Circuits and Systems II: Express Briefs》;20210131;第68卷(第1期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114115297A (zh) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106919171B (zh) | 一种机器人室内定位导航系统及方法 | |
| CN108141837B (zh) | 用于跟踪的装置和方法 | |
| WO2021128777A1 (en) | Method, apparatus, device, and storage medium for detecting travelable region | |
| CN109974727B (zh) | 一种机器人充电方法、装置及机器人 | |
| US20200206921A1 (en) | Robot movement control method, apparatus and robot using the same | |
| CN111366161B (zh) | 车辆定位方法及电子设备 | |
| JP2008299619A (ja) | モバイル機器、データ転送方法、およびデータ転送システム | |
| CN111781619B (zh) | 基于近距离通信网络的定位方法、装置、设备和存储介质 | |
| CN113055949A (zh) | 定位方法、装置、设备和介质 | |
| EP2980531A1 (en) | Positioning device, method for controlling same, and program | |
| CN112836889B (zh) | 路径优化方法、水下航行器以及计算机可读存储介质 | |
| CN114115297B (zh) | 基于视线导引法的auv跟踪控制方法及装置 | |
| CN110954114A (zh) | 电子地图的生成方法、装置、终端及存储介质 | |
| CN113965646A (zh) | 定位控制方法及装置、电子设备、存储介质 | |
| CN110491117B (zh) | 遥控方法、装置、设备和介质 | |
| US20230354258A1 (en) | Data processing method and apparatus | |
| CN114147711B (zh) | 机器人控制方法、装置、终端设备及存储介质 | |
| CN114115013B (zh) | 机器人电机的控制方法、终端设备以及存储介质 | |
| CN114234982B (zh) | 基于方位角定位的三维轨迹规划方法、系统、设备及介质 | |
| RU2478187C2 (ru) | Судовой навигационный комплекс | |
| CN115585819A (zh) | 自移动设备及其返航方法、返航装置、计算机可读介质 | |
| CN112799418A (zh) | 控制方法、装置、遥控设备及可读存储介质 | |
| CN114689054B (zh) | 塔康系统高精度导航方法、装置、飞行设备及存储介质 | |
| CN114594472B (zh) | 基于外信息的无线电高度表数据处理方法及装置 | |
| JP2021050969A (ja) | 情報端末装置、方法及びプログラム |
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 |