CN112836889A - 路径优化方法、水下航行器以及计算机可读存储介质 - Google Patents
路径优化方法、水下航行器以及计算机可读存储介质 Download PDFInfo
- Publication number
- CN112836889A CN112836889A CN202110191809.7A CN202110191809A CN112836889A CN 112836889 A CN112836889 A CN 112836889A CN 202110191809 A CN202110191809 A CN 202110191809A CN 112836889 A CN112836889 A CN 112836889A
- Authority
- CN
- China
- Prior art keywords
- preset
- state vector
- motion information
- result
- underwater vehicle
- 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
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 55
- 238000000034 method Methods 0.000 title claims abstract description 51
- 230000000007 visual effect Effects 0.000 claims abstract description 161
- 230000033001 locomotion Effects 0.000 claims abstract description 134
- 239000013598 vector Substances 0.000 claims description 207
- 230000006870 function Effects 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 6
- 230000017105 transposition Effects 0.000 claims description 6
- 238000004891 communication Methods 0.000 description 14
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 9
- 230000002093 peripheral effect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004140 cleaning Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 210000001503 joint Anatomy 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/06—Resources, workflows, human or project management; Enterprise or organisation planning; Enterprise or organisation modelling
- G06Q10/063—Operations research, analysis or management
- G06Q10/0631—Resource planning, allocation, distributing or scheduling for enterprises or organisations
- G06Q10/06311—Scheduling, planning or task assignment for a person or group
Landscapes
- Business, Economics & Management (AREA)
- Human Resources & Organizations (AREA)
- Engineering & Computer Science (AREA)
- Strategic Management (AREA)
- Economics (AREA)
- Entrepreneurship & Innovation (AREA)
- Operations Research (AREA)
- Game Theory and Decision Science (AREA)
- Development Economics (AREA)
- Marketing (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- Physics & Mathematics (AREA)
- General Business, Economics & Management (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Educational Administration (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种路径优化方法,应用于预设定位***中的水下航行器,所述方法包括:基于接收到的控制指令,获得预设路径对应的预设运动信息;获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;基于所述结果视觉标识,获得最终路径。本发明还公开了一种水下航行器以及计算机可读存储介质。利用本发明的路径优化方法,获得的水下航行器最终路径准确率较高。
Description
技术领域
本发明涉及水下航行技术领域,特别涉及一种路径优化方法、水下航行器以及计算机可读存储介质。
背景技术
随着水下水下航行器(水下无人航行器)的发展,越来越多的水下任务可以通过人工遥控操作或自主操作***,控制水下航行器完成水下工作。水下航行器具有水下定位导航和先进控制的能力,降低了任务执行过程中人工参与度,使得水下航行器的作用愈发明显。
但是,利用现有路径优化方法,获得的水下航行器的最终路径准确率较低。
发明内容
本发明的主要目的是提供一种路径优化方法、水下航行器以及计算机可读存储介质,旨在解决现有技术中利用现有路径优化方法,获得的水下航行器的最终路径准确率较低的技术问题。
为实现上述目的,本发明提出一种路径优化方法,应用于预设定位***中的水下航行器,所述方法包括:
基于接收到的控制指令,获得预设路径对应的预设运动信息;
获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;
基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;
基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;
基于所述结果视觉标识,获得最终路径。
可选的,所述基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息的步骤包括:
利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量;
基于所述预测状态矢量和所述实时观测量,获得结果状态矢量;
基于所述结果状态矢量和所述预设运动信息中的预设控制信号,获得结果运动信息。
可选的,所述历史运动信息包括历史状态矢量和历史控制信号;所述利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量的步骤包括:
基于所述历史状态矢量、所述历史控制信号和运动不确定度,利用公式一,获得所述预测状态矢量;
所述公式一为:
xt=f(xt-1,ut-1,mt)1
mt:N(0,Mt)
其中,t为所述预测状态矢量对应的时刻,Mt为所述运动不确定度,xt为所述预测状态矢量,xt-1为所述历史状态矢量,ut-1为所述历史控制信号,f为第一函数。
可选的,所述基于所述预测状态矢量和所述实时观测量,获得结果状态矢量的步骤包括:
基于所述实时观测量、所述观测视觉信标和量测不确定度,利用公式二,对所述预测状态矢量进行修正,以获得结果状态矢量;
所述公式二:
zt=h(xt,nt,A)1
nt:N(0,Nt)
其中,zt为所述实时观测量,Nt为所述量测不确定度,A为所述观测视觉信标,h为第二函数。
可选的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第一预设优化目标、第一预设目标函数、所述结果状态矢量、所述预设状态矢量,获得第一输出状态矢量,所述第一预设优化目标为最小化二次代价函数;
基于所述第一输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;所述第一预设目标函数为:
可选的,所述结果运动信息包括结果状态矢量和结果控制信号,所述预设运动信息包括预设状态矢量和预设控制信号;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第二预设优化目标、第二预设目标函数、所述结果状态矢量、所述结果控制信号、所述预设状态矢量和所述预设控制信号,获得第二输出状态矢量和输出控制信号,所述第二预设优化目标为最小化二次代价函数;
基于所述第二输出状态矢量和所述输出控制信号,在所述视觉信标集群中确定出结果视觉标识;
所述第二预设目标函数为:
其中,E为数学期望,t为所述结果状态矢量对应的时刻,x′t为所述结果状态矢量,u′t为所述结果控制信号,为所述预设状态矢量中t时刻对应的预设状态矢量,为所述预设控制信号中t时刻对应的预设控制信号,T为矩阵的转置,C和D均为正定加权矩阵,M为所述预设状态矢量的个数。
可选的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第三预设优化目标、预设最大欧几里得距离、预设最小概率、所述结果状态矢量和所述预设状态矢量,利用公式三,获得第三输出状态矢量,所述第三预设优化目标为所述观测视觉信标的数量最小;
基于所述第三输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;
所述公式三为:
其中,B为所述被观测到的信标的信标数量最小值,dmax为所述预设最大欧几里得距离,Pmin为所述预设最小概率,M为所述预设状态矢量的个数,t为所述结果状态矢量对应的时刻,x′t为所述结果状态矢量,为所述预设状态矢量中t时刻对应的预设状态矢量。
可选的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第四预设优化目标、所述结果状态矢量和所述预设状态矢量,利用公式四,获得第四输出状态矢量,所述第四预设优化目标为所述观测视觉信标的数量为定值;
基于所述第四输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;所述公式四为:
此外,为实现上述目的,本发明还提出了一种水下航行器,所述水下航行器包括:存储器、处理器及存储在所述存储器上并在所述处理器上运行路径优化程序,所述路径优化程序被所述处理器执行时实现如上述任一项所述的路径优化方法的步骤。
此外,为实现上述目的,本发明还提出了一种计算机可读存储介质,所述计算机可读存储介质上存储有路径优化程序,所述路径优化程序被处理器执行时实现如上述任一项所述的路径优化方法的步骤。
本发明技术方案提出了一种路径优化方法,应用于预设定位***中的水下航行器,所述方法包括:基于接收到的控制指令,获得预设路径对应的预设运动信息;获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;基于所述结果视觉标识,获得最终路径。
现有的路径优化方法中,利用声学定位技术获得水下航行器最终的航行动作,但是,声学定位在水下的精度较差,使得获得的水下航行器的最终路径准确率较低;而本发明中,基于水下航行器的历史运动信息和观测视觉信标对应的实时观测量,获得结果运动信息,并基于结果运动信息对应的结果视觉标识,获得最终路径,视觉定位在水下的精度较高,使得获得水下航行器最终路径准确率较高。所以,利用本发明的路径优化方法,获得的水下航行器最终路径准确率较高。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例方案涉及的硬件运行环境的水下航行器结构示意图;
图2为本发明路径优化方法第一实施例的流程示意图;
图3为本发明水下航行器获得的最终路径的示意图;
图4为本发明水下航行器作业的示意图;
图5为本发明水下航行器移动到作业区域的示意图;
图6为多个水下航行器对一个作业区域作业的示意图;
图7为本发明多个水下航行器对多个作业区域作业示意图;
图8为本发明路径优化装置第一实施例的结构框图。
本发明目的的实现、功能特点及优点将结合实施例,参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
参照图1,图1为本发明实施例方案涉及的硬件运行环境的水下航行器结构示意图。
通常,水下航行器包括:至少一个处理器301、存储器302以及存储在所述存储器上并可在所述处理器上运行的路径优化程序,所述路径优化程序配置为实现如前所述的路径优化方法的步骤。
处理器301可以包括一个或多个处理核心,比如4核心处理器、8核心处理器等。
存储器302可以包括一个或多个计算机可读存储介质,该计算机可读存储介质可以是非暂态的。在一些实施例中,存储器302中的非暂态的计算机可读存储介质用于存储至少一个指令,该至少一个指令用于被处理器301所执行以实现本申请中方法实施例提供的路径优化方法。
在一些实施例中,水下航行器还可选包括有:通信接口303和至少一个***设备。处理器301、存储器302和通信接口303之间可以通过总线或信号线相连。各个***设备可以通过总线、信号线或电路板与通信接口303相连。具体地,***设备包括:射频电路304、显示屏305和电源306中的至少一种。
通信接口303可被用于将I/O(Input/Output,输入/输出)相关的至少一个***设备连接到处理器301和存储器302。在一些实施例中,处理器301、存储器302和通信接口303被集成在同一芯片或电路板上;在一些其他实施例中,处理器301、存储器302和通信接口303中的任意一个或两个可以在单独的芯片或电路板上实现,本实施例对此不加以限定。
射频电路304用于接收和发射RF(Radio Frequency,射频)信号,也称电磁信号。射频电路304通过电磁信号与通信网络以及其他通信设备进行通信。射频电路304可以通过至少一种无线通信协议来与其它终端进行通信。该无线通信协议包括但不限于:城域网、各代移动通信网络(2G、3G、4G及5G)、无线局域网和/或WiFi(Wireless Fidelity,无线保真)网络。
在另一实施例中,射频电路还可以利用有线传输电路替换,水下航行器通过有线传输电路与其他电子设备通信连接;同时,射频电路还可以是水声通信电路替换,水下航行器通过水声通信电路与其他电子设备通信连接。
显示屏305用于显示UI(User Interface,用户界面)。该UI可以包括图形、文本、图标、视频及其它们的任意组合。
电源306用于为水下航行器中的各个组件进行供电。电源306可以是交流电、直流电、一次性电池或可充电电池。
本领域技术人员可以理解,图1中示出的结构并不构成对水下航行器的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件布置。
此外,本发明实施例还提出一种计算机可读存储介质,所述计算机可读存储介质上存储有路径优化程序,所述路径优化程序被处理器执行时实现如上文所述的路径优化方法的步骤。因此,这里将不再进行赘述。另外,对采用相同方法的有益效果描述,也不再进行赘述。对于本申请所涉及的计算机可读存储介质实施例中未披露的技术细节,请参照本申请方法实施例的描述。确定为示例,程序指令可被部署为在一个水下航行器上执行,或者在位于一个地点的多个水下航行器上执行,又或者,在分布在多个地点且通过通信网络互连的多个水下航行器备上执行。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,上述的程序可存储于一计算机可读取存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中,上述的计算机可读存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。
基于上述硬件结构,提出本发明路径优化方法的实施例。
参照图2,图2为本发明路径优化方法第一实施例的流程示意图,所述方法应用于预设定位***中的水下航行器,所述方法包括:
步骤S11:基于接收到的控制指令,获得预设路径对应的预设运动信息。
步骤S12:获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标。
需要说明的是,本发明的执行主体是水下航行器,水下航行器安装有路径优化程序,水下航行器执行路径优化程序时,实现本发明的路径优化方法。在另一实施例中,路径优化程序还可以安装在主控设备中,主控设备可以是计算机或个人电脑等。主控设备执行本发明的路径优化方法,并将获得的最终路径,将最终路径发送至水下航行器,由水下航行器按照最终路径,运动到目标物体。
通常控制指令包括终点位置信息和起点位置信息等,基于控制指令,获得水下航行器的预设路径(可以是通过任何现有的技术手段,基于终点位置信息和起点位置信息,获得的预设路径),预设路径对应预设运动信息,预设运动信息包括预设状态矢量和预设控制信号,水下航行器按照预设运动信息运动时,对应的路径即为预设路径。水下航行器接收到控制指令时,会获得预设路径,预设路径并不是最优的路径,需要继续进行后面步骤S12-S15,以获得最优的最终路径。本发明的目的在于对预设路径进行优化,以获得较优的或最优的最终路径。
具体的,水下航行器通常是指水下无人航行器,水下航行器具有运动能力,可以是ROV(遥控无人潜水器)或者AUV(自主式水下航行器),即,本发明的路径优化方法对应的目标环境通常是水域,水下航行器通常在水下运动,获得的最终路径通常也是水下航行器在水域中运行时,获得的水下路径。水下航行器可以是一个,也可以是多个,水下航行器可以具有摄像头,摄像头可以是前视、后视、下视、上视等。目标物可以是移动的或者固定的,可以是一个或者多个,可以位于地面、空中、地下、水下、水面等。
需要说明的是,视觉信标集群中通常包括多个视觉信标,同时,被观测的观测视觉信标的数量通常也是多个,观测视觉信标包括多个被观测到的视觉信标。
具体的,预设定位***可以是指一个定位***,通常预设定位***包括水下航行器(可以是多个)和视觉信标集群(多个视觉信标),一个预设定位***通常用于一个目标对象的水下作业,例如,一个船舶对应一个预设定位***。
视觉信标可以是二维码,视觉信标也可以是类似圆形的或者黑白方格的码,视觉信标可以是一维、二维或者三维的视觉信标,视觉信标还可以是一种类型或者多种类型的组合。视觉信标的布置可以是嵌套排列、一字排列或多排排列等,视觉信标还可以按规则形状排列,比如圆形或方形等,视觉信标还可以按不规则形状排列,本发明不做限制。视觉信标的尺寸可以只有一种,也可以是多种。视觉信标可以是能够发射主动光源的LED或激光等,也可以是视觉光带等;视觉信标的最小尺寸应当使得水下航行器在最近距离操作时,水下航行器的摄像头仍能完整捕捉到至少一个视觉信标来确保定位功能的完备性,也可以要求能够完整捕捉多个视觉信标来确保丢失某个视觉信标的情况下还保持定位***的准确性。
通常,一个视觉信标对应一个视觉信号,不同视觉信标的视觉信号不同,一个视觉信标发射的视觉信号包括该信标的编号或设备信息等。
在另一实施例中,信标还可以是声学信标,但是,当信标为声学信标时,水下航行器采用的传感器为与声学信标对应的声学传感器,声学传感器在价格、功耗、精度、带宽、体积等方面均有一定的劣势,同时,声学传感器在水中获得的声学信标发射的声学信号时,声学信号中的多径和噪音干扰较大,使得水下航行器基于声学信号获得的路径的准确率较低。因此,在本发明中,信标以视觉信标为较优的选择。
在本申请中,以视觉信标代替声学信标,降低了水下航行器的成本,同时,水下航行器获得的视觉信标发射的实时信标信号时,实时信标信号包括较少的多径和噪音,使得获得的实时观测量的准确度较高。
步骤S13:基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息。
具体的,步骤S13包括:利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量;基于所述预测状态矢量和所述实时观测量,获得结果状态矢量;基于所述结果状态矢量和所述预设运动信息中的预设控制信号,获得结果运动信息。
其中,所述历史运动信息包括历史状态矢量和历史控制信号;所述利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量的步骤包括:
基于所述历史状态矢量、所述历史控制信号和运动不确定度,利用公式一,获得所述预测状态矢量;
所述公式一为:
xt=f(xt-1,ut-1,mt)1
mt:N(0,Mt)
其中,t为所述预测状态矢量对应的时刻,Mt为所述运动不确定度,xt为所述预测状态矢量,xt-1为所述历史状态矢量,ut-1为所述历史控制信号,f为第一函数。
需要说明的是,水下航行器在历史时刻(当前运行时刻以及当前运行时刻之前的时刻)的运动信息是连续的,通常将连续时刻和连续时刻对应的连续运动信息离散化,以获得多个离散时刻和多个离散时刻对应的离散的历史运动信息,当前时刻的运动信息即为历史运动信息(即,预测状态矢量是基于当前时刻的状态矢量获得),历史运动信息通常包括包括历史状态矢量和所述历史状态矢量对应的历史控制信号,一个历史状态矢量对应一个历史控制信号。
第一函数可以是牛顿-欧拉方程或拉格朗日方程等。一个控制信号可以是指水下航行器在该控制信号对应时刻的力学环境(包括力和力矩)的总和。
当获得一个时刻的结果状态矢量时,将该时刻的预设状态矢量对应的预设控制信号确定为该时刻结果状态信息中的结果控制信号,即该时刻的结果状态矢量对应的结果控制信号,同一时刻的预设控制信号与结果控制信号为相同的控制信号。
另外,所述基于所述预测状态矢量和所述实时观测量,获得结果状态矢量的步骤包括:
基于所述实时观测量、所述观测视觉信标和量测不确定度,利用公式二,对所述预测状态矢量进行修正,以获得结果状态矢量;
所述公式二:
zt=h(xt,nt,A)1
nt:N(0,Nt)
其中,zt为所述实时观测量,Nt为所述量测不确定度,A为所述观测视觉信标,h为第二函数。
需要说明的是,视觉信标集群中通常包括多个视觉信标,同时,被观测的观测视觉信标的数量通常也是多个,观测视觉信标包括多个被观测到的视觉信标,观测视觉信标A即为被观测到的视觉信标的集合。
另外,第二函数可以是线型或非线性方程,第二函数可以与第一函数相同。
另外,在一些实施例中,某个时刻(或某些时刻)水下航行器并未观测到任何视觉信标,则水下航行器在该时刻并未获取到任何的实时观测量,此时,则按照上述方法获得该时刻的预测状态矢量,并将该时刻的预测状态矢量作为该时刻的结果状态矢量,同时,该时刻的结果控制信号也可以是该时刻的预设运动信息中的预设控制信号,并基于该时刻的结果控制信号和该时刻的结果状态矢量,获得该时刻的结果运动信息。并继续执行本发明的步骤S14-S15,以获得最终路径。
步骤S14:基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识。
进一步的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第一预设优化目标、第一预设目标函数、所述结果状态矢量、所述预设状态矢量,获得第一输出状态矢量,所述第一预设优化目标为最小化二次代价函数;
基于所述第一输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;所述第一预设目标函数为:
需要说明的是,由于结果状态矢量是对预测状态矢量修正后获得,结果状态矢量对应的时刻t与预测状态矢量对应的时刻t是相同的。
可以理解的是,在本发明中,历史状态信息是离散化之后的状态信息,获得的预设状态信息也是离散化之后的状态信息,即一个预设离散状态信息对应一个预设状态矢量和一个预设控制信号。预设状态信息可以表示为: x为状态矢量,u为对应的控制信号。
进一步的,所述结果运动信息包括结果状态矢量和结果控制信号,所述预设运动信息包括预设状态矢量和预设控制信号;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第二预设优化目标、第二预设目标函数、所述结果状态矢量、所述结果控制信号、所述预设状态矢量和所述预设控制信号,获得第二输出状态矢量和输出控制信号,所述第二预设优化目标为最小化二次代价函数;
基于所述第二输出状态矢量和所述输出控制信号,在所述视觉信标集群中确定出结果视觉标识;
所述第二预设目标函数为:
其中,E为数学期望,t为所述结果状态矢量对应的时刻,x′t为所述结果状态矢量,u′t为所述结果控制信号,为所述预设状态矢量中t时刻对应的预设状态矢量,为所述预设控制信号中t时刻对应的预设控制信号,T为矩阵的转置,C和D均为正定加权矩阵,M为所述预设状态矢量的个数。
进一步的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第三预设优化目标、预设最大欧几里得距离、预设最小概率、所述结果状态矢量和所述预设状态矢量,利用公式三,获得第三输出状态矢量,所述第三预设优化目标为所述观测视觉信标的数量最小;
基于所述第三输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;
所述公式三为:
其中,B为所述被观测到的信标的信标数量最小值,dmax为所述预设最大欧几里得距离,Pmin为所述预设最小概率,M为所述预设状态矢量的个数,t为所述结果状态矢量对应的时刻,x′t为所述结果状态矢量,为所述预设状态矢量中t时刻对应的预设状态矢量。
另外,还可以基于所述结果状态矢量、所述预设状态矢量和预设最小概率,获得椭圆面,在所述椭圆面的主轴长小于或等于预设预设最大欧几里得距离时,确定出第三输出状态矢量。
进一步的,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第四预设优化目标、所述结果状态矢量和所述预设状态矢量,利用公式四,获得第四输出状态矢量,所述第四预设优化目标为所述观测视觉信标的数量为定值;
基于所述第四输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;所述公式四为:
需要说明的是,还可以设定其他的目标函数和针对目标函数的优化目标,以获得最终路径,本发明示例的提出四种情况,用户还可以根据自己的需求,获得其他的目标函数和对应的优化目标。
基于上述各种情况获得的不同的输出状态矢量(和输出控制信号),即可在视觉信标集群中确定出与所述输出状态矢量(和输出控制信号)对应的视觉标识,即结果视觉标识,结果视觉标识是对预设运动信息就行优化后获得,结果视觉标识通常是最优的布局。
步骤S15:基于所述结果视觉标识,获得最终路径。
在获得结果视觉标识后,基于结果视觉标识的布局,即可获得水下航行器从起点位置信息到终点位置信息对应的路径,即最终路径。
参照图3,图3为本发明水下航行器获得的最终路径的示意图;最终路径表现形式为多个视觉信标组成的路径,黑色块状即为视觉信标组成的最终路径,水下航行器沿着最终路径移动。
参照图4,图4为本发明水下航行器作业的示意图;船舶21对应的水下航行器包括水下航行器22、水下航行器23、水下航行器24、水下航行器25和水下航行器26,27和28为引导装置,水下航行器22执行水下任务,例如,船舶清理或船舶检测;水下航行器23与引导装置28对接;水下航行器24与水下航行器25对接;水下航行器26与引导装置27对接,以收回水下航行器26。船舶也可以空中、陆地、地下、水面、水下的载体,载体可以时动态的或者静态的。
参照图5,图5为本发明水下航行器移动到作业区域的示意图;船舶31对应的水下航行器有水下航行器32、水下航行器33和水下航行器34,水下航行器33执行水下任务,从非作业区域移动到作业区域,水下航行器34与作业区域的水下航行器32对接,水下航行器34需要从非作业区域移动到作业区域。
作业区域也是本发明提出的预设定位***所覆盖的区域,覆盖范围决定于标识的大小、布局、水质、光线、所容许的精度等,作业区域可以通过人工标定,也可以通过对定位数据进行实时评估来动态确定,比如利用统计学原理评估数据的一致性。当水下航行器进入到作业区域后,其自身搭载的摄像头获取到视觉标记的视频流,再利用定位实时估算出结果状态矢量。
参照图6,图6为多个水下航行器对一个作业区域作业的示意图;一个作业区域为一个船舶41对应的作业区域,水下航行器包括水下航行器42、水下航行器43和水下航行器44,水下航行器42用于对船舶进行检测和清理,定位43与船舶对接,水下航行器44运动到指定位置。
参照图7,图7为本发明多个水下航行器对多个作业区域作业示意图;多个作业区域对应多个对象,分别包括船舶51、船舶52和船舶53,水下航行器包括水下航行器54、水下航行器55、水下航行器56和水下航行器57;水下航行器54和水下航行器55用于对船舶51进行作业,作业区域为船舶51对应的作业区域,水下航行器56用于对船舶52进行作业,水下航行器用于对53进行作业。
通常,具体应用中,一个船舶对应一个作业区域,多个船舶通过安装的GPS或者其他位置感知单元获得各自的经纬度信息,利用公式五,计算得到各个船舶在ECEF坐标系(笛卡尔坐标系)下的位置,公式五为:
其中,xECEF、yECEF和ZECEF分别是船舶笛卡尔坐标系的三维坐标信息,φ为船舶转化前的维度信息,λ为船舶转化前的经度信息,h为船舶的海拔高度,re为地球半径,ε为地球第一扁偏率(船舶所在区域)。
利用公式五,可以将多个固连于船舶的局部坐标系通过坐标系之间的平移和转动,转换到同一个坐标系下,从而使得水下水下航行器在整个作业区域内能够任意自主导航作业。
本发明技术方案提出了一种路径优化方法,应用于预设定位***中的水下航行器,所述方法包括:基于接收到的控制指令,获得预设路径对应的预设运动信息;获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;基于所述结果视觉标识,获得最终路径。
现有的路径优化方法中,利用声学定位技术获得水下航行器最终的航行动作,但是,声学定位在水下的精度较差,使得获得的水下航行器的最终路径准确率较低;而本发明中,基于水下航行器的历史运动信息和观测视觉信标对应的实时观测量,获得结果运动信息,并基于结果运动信息对应的结果视觉标识,获得最终路径,视觉定位在水下的精度较高,使得获得水下航行器最终路径准确率较高。所以,利用本发明的路径优化方法,获得的水下航行器最终路径准确率较高。
参照图8,图8为本发明路径优化装置第一实施例的结构框图,所述装置应用于预设定位***中的水下航行器,所述装置包括:
接收模块10,用于基于接收到的控制指令,获得预设路径对应的预设运动信息;
获取模块20,用于获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;
获得模块30,用于基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;
确定模块40,用于基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;
路径获得模块50,用于基于所述结果视觉标识,获得最终路径。
以上所述仅为本发明的可选实施例,并非因此限制本发明的专利范围,凡是在本发明的发明构思下,利用本发明说明书及附图内容所作的等效结构变换,或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。
Claims (10)
1.一种路径优化方法,其特征在于,应用于预设定位***中的水下航行器,所述方法包括:
基于接收到的控制指令,获得预设路径对应的预设运动信息;
获取所述水下航行器的历史运动信息和观测视觉信标对应的实时观测量,所述观测视觉信标是所述预设定位***中视觉信标集群中被所述水下航行器观测到的视觉信标;
基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息;
基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识;
基于所述结果视觉标识,获得最终路径。
2.如权利要求1所述的方法,其特征在于,所述基于所述历史运动信息和所述实时观测量,获得所述水下航行器的结果运动信息的步骤包括:
利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量;
基于所述预测状态矢量和所述实时观测量,获得结果状态矢量;
基于所述结果状态矢量和所述预设运动信息中的预设控制信号,获得结果运动信息。
3.如权利要求2所述的方法,其特征在于,所述历史运动信息包括历史状态矢量和历史控制信号;所述利用所述历史运动信息,对所述水下航行器的状态进行预测,以获得所述水下航行器的预测状态矢量的步骤包括:
基于所述历史状态矢量、所述历史控制信号和运动不确定度,利用公式一,获得所述预测状态矢量;
所述公式一为:
xt=f(xt-1,ut-1,mt)1
mt:N(0,Mt)
其中,t为所述预测状态矢量对应的时刻,Mt为所述运动不确定度,xt为所述预测状态矢量,xt-1为所述历史状态矢量,ut-1为所述历史控制信号,f为第一函数。
4.如权利要求3所述的方法,其特征在于,所述基于所述预测状态矢量和所述实时观测量,获得结果状态矢量的步骤包括:
基于所述实时观测量、所述观测视觉信标和量测不确定度,利用公式二,对所述预测状态矢量进行修正,以获得结果状态矢量;
所述公式二:
zt=h(xt,nt,A)1
nt:N(0,Nt)
其中,zt为所述实时观测量,Nt为所述量测不确定度,A为所述观测视觉信标,h为第二函数。
5.如权利要求4所述的方法,其特征在于,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第一预设优化目标、第一预设目标函数、所述结果状态矢量、所述预设状态矢量,获得第一输出状态矢量,所述第一预设优化目标为最小化二次代价函数;
基于所述第一输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;
所述第一预设目标函数为:
6.如权利要求4所述的方法,其特征在于,所述结果运动信息包括结果状态矢量和结果控制信号,所述预设运动信息包括预设状态矢量和预设控制信号;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第二预设优化目标、第二预设目标函数、所述结果状态矢量、所述结果控制信号、所述预设状态矢量和所述预设控制信号,获得第二输出状态矢量和输出控制信号,所述第二预设优化目标为最小化二次代价函数;
基于所述第二输出状态矢量和所述输出控制信号,在所述视觉信标集群中确定出结果视觉标识;
所述第二预设目标函数为:
7.如权利要求4所述的方法,其特征在于,所述结果运动信息包括结果状态矢量,所述预设运动信息包括预设状态矢量;所述基于所述结果运动信息和所述预设运动信息,在所述视觉信标集群中确定出结果视觉标识的步骤包括:
基于第三预设优化目标、预设最大欧几里得距离、预设最小概率、所述结果状态矢量和所述预设状态矢量,利用公式三,获得第三输出状态矢量,所述第三预设优化目标为所述观测视觉信标的数量最小;
基于所述第三输出状态矢量,在所述视觉信标集群中确定出结果视觉标识;
所述公式三为:
9.一种水下航行器,其特征在于,所述水下航行器包括:存储器、处理器及存储在所述存储器上并在所述处理器上运行路径优化程序,所述路径优化程序被所述处理器执行时实现如权利要求1至7中任一项所述的路径优化方法的步骤。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有路径优化程序,所述路径优化程序被处理器执行时实现如权利要求1至7中任一项所述的路径优化方法的步骤。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110191809.7A CN112836889B (zh) | 2021-02-19 | 2021-02-19 | 路径优化方法、水下航行器以及计算机可读存储介质 |
PCT/CN2021/104062 WO2022174545A1 (zh) | 2021-02-19 | 2021-07-01 | 路径优化方法、水下航行器以及计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110191809.7A CN112836889B (zh) | 2021-02-19 | 2021-02-19 | 路径优化方法、水下航行器以及计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112836889A true CN112836889A (zh) | 2021-05-25 |
CN112836889B CN112836889B (zh) | 2024-07-19 |
Family
ID=75933857
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110191809.7A Active CN112836889B (zh) | 2021-02-19 | 2021-02-19 | 路径优化方法、水下航行器以及计算机可读存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN112836889B (zh) |
WO (1) | WO2022174545A1 (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114780644A (zh) * | 2022-06-17 | 2022-07-22 | 鹏城实验室 | 一种船舶航行数据处理方法、装置、设备及存储介质 |
WO2022174545A1 (zh) * | 2021-02-19 | 2022-08-25 | 鹏城实验室 | 路径优化方法、水下航行器以及计算机可读存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20170111582A (ko) * | 2016-03-29 | 2017-10-12 | 조선대학교산학협력단 | 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템 |
CN107300382A (zh) * | 2017-06-27 | 2017-10-27 | 西北工业大学 | 一种用于水下机器人的单目视觉定位方法 |
CN108444478A (zh) * | 2018-03-13 | 2018-08-24 | 西北工业大学 | 一种用于水下航行器的移动目标视觉位姿估计方法 |
CN109211240A (zh) * | 2018-09-01 | 2019-01-15 | 哈尔滨工程大学 | 一种单目视觉水下航行器导航定位校正方法 |
CN112184765A (zh) * | 2020-09-18 | 2021-01-05 | 西北工业大学 | 一种基于视觉的水下航行器自主跟踪方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR3006770B1 (fr) * | 2013-06-05 | 2016-12-09 | Ixblue | Procede et dispositif de metrologie pour la calibration de la geometrie d'un reseau de balises acoustiques sous-marines |
CN108303715B (zh) * | 2017-12-19 | 2020-05-01 | 浙江大学 | 基于北斗信标的水下移动节点无源定位方法及其*** |
CN109541606A (zh) * | 2018-09-30 | 2019-03-29 | 天津大学 | 单水声信标测距辅助的水下同步定位与构图方法 |
CN111380519B (zh) * | 2020-03-04 | 2021-08-31 | 哈尔滨工程大学 | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 |
CN112836889B (zh) * | 2021-02-19 | 2024-07-19 | 鹏城实验室 | 路径优化方法、水下航行器以及计算机可读存储介质 |
-
2021
- 2021-02-19 CN CN202110191809.7A patent/CN112836889B/zh active Active
- 2021-07-01 WO PCT/CN2021/104062 patent/WO2022174545A1/zh active Application Filing
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20170111582A (ko) * | 2016-03-29 | 2017-10-12 | 조선대학교산학협력단 | 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템 |
CN107300382A (zh) * | 2017-06-27 | 2017-10-27 | 西北工业大学 | 一种用于水下机器人的单目视觉定位方法 |
CN108444478A (zh) * | 2018-03-13 | 2018-08-24 | 西北工业大学 | 一种用于水下航行器的移动目标视觉位姿估计方法 |
CN109211240A (zh) * | 2018-09-01 | 2019-01-15 | 哈尔滨工程大学 | 一种单目视觉水下航行器导航定位校正方法 |
CN112184765A (zh) * | 2020-09-18 | 2021-01-05 | 西北工业大学 | 一种基于视觉的水下航行器自主跟踪方法 |
Non-Patent Citations (1)
Title |
---|
王宇杰;李宇;鞠东豪;黄海宁;: "一种基于水下无人航行器的多目标被动跟踪算法", 电子与信息学报, vol. 42, no. 08, 15 August 2020 (2020-08-15), pages 2013 - 2020 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022174545A1 (zh) * | 2021-02-19 | 2022-08-25 | 鹏城实验室 | 路径优化方法、水下航行器以及计算机可读存储介质 |
CN114780644A (zh) * | 2022-06-17 | 2022-07-22 | 鹏城实验室 | 一种船舶航行数据处理方法、装置、设备及存储介质 |
CN114780644B (zh) * | 2022-06-17 | 2022-09-13 | 鹏城实验室 | 一种船舶航行数据处理方法、装置、设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
WO2022174545A1 (zh) | 2022-08-25 |
CN112836889B (zh) | 2024-07-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Specht et al. | Application of an autonomous/unmanned survey vessel (ASV/USV) in bathymetric measurements | |
CN109975773B (zh) | 毫米波雷达标定方法、装置、设备和存储介质 | |
WO2019170093A1 (zh) | 一种生成频谱态势的方法、装置及计算机存储介质 | |
CN112836889B (zh) | 路径优化方法、水下航行器以及计算机可读存储介质 | |
US10578706B2 (en) | Wireless sonar receiver | |
EP3170019B1 (en) | Depth display using sonar data | |
Thompson et al. | Efficient lidar-based object segmentation and mapping for maritime environments | |
CN108827252B (zh) | 绘制水下实景地图的方法、装置、设备、***及存储介质 | |
EP3475872A1 (en) | System for taking inventory and estimating the position of objects | |
EP3989034B1 (en) | Automatic safe-landing-site selection for unmanned aerial systems | |
JP2021103396A (ja) | 船舶の航行支援システムにおける管理サーバ、船舶の航行支援方法、及び船舶の航行支援プログラム | |
US20220049961A1 (en) | Method and system for radar-based odometry | |
CN111123340A (zh) | 物流配送导航方法及***、近场定位导航装置、存储介质 | |
US9292971B2 (en) | Three-dimensional tactical display and method for visualizing data with a probability of uncertainty | |
CN114217303B (zh) | 目标定位跟踪方法及装置、水下机器人和存储介质 | |
Zacchini et al. | Autonomous underwater environment perceiving and modeling: An experimental campaign with feelhippo auv for forward looking sonar-based automatic target recognition and data association | |
US20220363357A1 (en) | Underwater vehicle control system | |
JP2019074918A (ja) | 制御装置、移動体、及び移動体の分散制御プログラム | |
CN116486377B (zh) | 可行驶区域的生成方法及装置 | |
US12007790B2 (en) | Systems and methods for service drone landing zone operations | |
JP2006065831A (ja) | 船舶の運航管理方法及び装置 | |
JP2023019846A (ja) | 無人航空機、捜索装置、捜索方法、プログラム、及び捜索システム | |
CN114115297B (zh) | 基于视线导引法的auv跟踪控制方法及装置 | |
Kapetanović et al. | Acoustical underwater localization of a remotely operated vehicle in mariculture | |
US20240230371A1 (en) | Map data providing device, storage medium, and map data providing method |
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 |