CN112083377B - 一种水下机器人的位置估计方法及装置 - Google Patents
一种水下机器人的位置估计方法及装置 Download PDFInfo
- Publication number
- CN112083377B CN112083377B CN202010978540.2A CN202010978540A CN112083377B CN 112083377 B CN112083377 B CN 112083377B CN 202010978540 A CN202010978540 A CN 202010978540A CN 112083377 B CN112083377 B CN 112083377B
- Authority
- CN
- China
- Prior art keywords
- state
- continuous
- underwater robot
- determining
- particle
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
-
- 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
Abstract
本发明提供了一种水下机器人的位置估计方法及装置,涉及跟踪定位技术领域,包括:获取水下机器人当前时刻的传感位置信息;若当前时刻的传感位置信息为有效传感信息,并确定跟随设备在当前时刻下连续接收到有效传感信息的连续通信次数;根据连续通信次数,确定水下机器人当前时刻的预估位置,以使跟随设备对水下机器人进行跟踪定位。本发明根据接收的传感位置信息判断当前时刻的通信是否有效,根据连续通信次数判断是否在连续时刻进行了有效通信,综合判断当前时刻定位的强弱,根据不同的定位情况,针对性地进行跟踪定位,以此实时保证跟随设备对水下机器人的有效跟踪。
Description
技术领域
本发明涉及跟踪定位技术领域,具体而言,涉及一种水下机器人的位置估计方法及装置。
背景技术
水下机器人(AutonomousUnderwaterVehicle,简称为AUV)因其灵活性、智能性等特点,被广泛用于深海探索的多种任务之中。为了实时确定AUV位置,通常采用对应的跟随设备对AUV进行跟踪,完成与AUV的协同工作,充当AUV的通信中介,实时传输AUV获取的相关水下信息。
水面无人艇(UnmannedSurfaceVessel,简称为USV)作为海上的智能体,将其与AUV配合工作,可以改善任务的整体运作、对任务的执行起到重大作用,对于推进油气、科研和国防等自主海洋系统的发展也是至关重要的。USV可以通过水声实时接收AUV的位置信息并跟踪AUV,以保证水声通信的距离,并在无人艇上通过卫星等无线手段进行通信和信息传递。然而,USV和AUV之间的水声通信相比于通常的无线电等通信方式,具有带宽低、传输效率慢等特点,并且受海洋传播特性的影响。因此,在其通信过程中,USV和AUV之间的通信程度有强有弱,难免会出现接收不到数据、接收到无效数据的情况,即弱定位现象。现有技术往往是利用历史航迹或者利用某一种目标定位算法,来对水下机器人进行定位,忽略了时强时弱的通信传递对定位准确性的影响。因而,如何在时强时弱的通信过程中实时保持对AUV的有效跟踪定位是亟待解决的问题。
发明内容
本发明解决的技术问题是如何在时强时弱的通信过程中实时保持对AUV的有效跟踪定位,为达上述目的,第一方面,本发明提供了一种水下机器人的位置估计方法,其包括:
获取水下机器人当前时刻的传感位置信息;
若所述当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到所述有效传感信息的连续通信次数;
根据所述连续通信次数,确定所述水下机器人当前时刻的预估位置,以使所述跟随设备对所述水下机器人进行跟踪定位。
由此,本发明提供的水下机器人的位置估计方法利用了接收的传感位置信息判断当前时刻的通信是否有效,并根据连续通信次数判断是否在连续时刻进行了有效通信,即有效判断了通信间隔。结合传感位置信息和连续通信次数反映通信有效性和通信时间间隔的情况,判断当前时刻定位的强弱,根据不同的定位情况,针对性地进行跟踪定位,以此实时保证跟随设备对水下机器人的有效跟踪。
进一步地,上述水下机器人的位置估计方法还包括:
若所述当前时刻的传感位置信息为无效传感信息,则根据所述水下机器人的历史航迹,确定所述水下机器人当前时刻的预估位置。
由此,在当前时刻的传感位置信息非有效传感信息的情况下,说明当前的通信数据并不准确,即已经出现了通信间隔,观测信息已经丢失,因而依据当前时刻的传感位置信息进行跟踪定位会导致定位不准,因而依据水下机器人的历史航迹进行定位,保证通信间隔下,对水下机器人定位的准确性。
进一步地,所述根据所述连续通信次数,确定所述水下机器人当前时刻的预估位置包括:
根据所述连续通信次数,确定初始状态粒子集;
根据所述初始状态粒子集建立对应的交互式多模型,将所述初始状态粒子集作为所述交互式多模型的初始输入,确定所述水下机器人当前时刻的预估位置。
由此,在当前时刻的传感位置信息为有效传感信息的情况下,说明当前的通信数据准确,没有出现通信间隔,因而根据连续通信次数判断当前时刻跟随设备和水下机器人之间的定位强弱,依据定位的强弱确定不同的初始状态粒子集,建立不同的交互式多模型,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
进一步地,所述初始状态粒子集包括第一初始粒子集,所述根据所述连续通信次数,确定初始状态粒子集包括:
当所述连续通信次数大于预设次数时,根据所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息,确定所述连续N个时刻中每个时刻对应的一组运动状态值,其中,N等于所述预设次数;
根据所述连续N个时刻中每个时刻对应的所述一组运动状态值,确定所述连续N个时刻中每个时刻对应的第一状态粒子,所述连续N个时刻的所述第一状态粒子构成所述第一初始粒子集。
由此,当连续通信次数大于预设次数,说明当前时刻的通信有效且在较长时间段内未发生通信间隔,此时定位状态良好,连续N个时刻中的传感位置信息都是有效、可靠的数据,因而根据通信连续N个时刻中的传感位置信息建立初始状态粒子集,建立不同的交互式多模型,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
进一步地,所述初始状态粒子集包括第二初始粒子集,所述根据所述连续通信次数,确定初始状态粒子集包括:
当所述连续通信次数等于预设次数时,根据所述水下机器人的历史航迹对所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集,其中,N等于所述预设次数。
由此,当连续通信次数等于预设次数,说明当前时刻的通信有效且在短时间段内未发生通信间隔,此时定位状态较好,刚好接收到N个时刻的有效、可靠的观测信息,但由于仅仅只是短时间内未发生通信间隔,为了更好地保证传感位置信息的可靠性,因而结合水下机器人的历史航迹对传感位置信息进行修正,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
进一步地,所述根据所述水下机器人的历史航迹对所述跟随设备连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集包括:
根据所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息,确定所述连续N个时刻中每个时刻对应的一组运动状态值;
根据所述连续N个时刻中每个时刻对应的所述一组运动状态值,确定所述连续N个时刻中每个时刻对应的第一状态粒子,所述连续N个时刻的所述第一状态粒子构成候选粒子集;
根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集。
由此,针对第一状态粒子进行对应的矫正,以此得到更为准确可靠的第二初始粒子集,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
进一步地,所述根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集包括:
根据所述历史航迹,确定所述连续N个时刻中每个时刻对应的一组预测状态值;
根据所述连续N个时刻中每个时刻对应的所述一组预测状态值,确定所述连续N个时刻中每个时刻对应的预测粒子,所述连续N个时刻的所述预测粒子构成预测状态集;
根据所述候选粒子集和所述预测状态集之间的粒子转移概率,确定所述候选粒子集中每个所述第一状态粒子对应的修正权值;
根据每个所述第一状态粒子和对应的所述修正权值,确定修正后的第二状态粒子,所述连续N个时刻的所述第二状态粒子构成所述第二初始粒子集。
由此,根据历史航迹建立对应的预测状态集,利用预测状态集中的预测粒子预测每个时刻对应的运动状态,以此确定修正权值,保证对第一状态粒子的有效修正,建立准确可靠的第二初始粒子集。
进一步地,所述根据所述初始状态粒子集建立对应的交互式多模型,将所述初始状态粒子集作为所述交互式多模型的初始输入,确定所述水下机器人当前时刻的预估位置包括:
将所述初始状态粒子集中的每个状态粒子输入至设置的多个滤波模型中,以进行模型交互;
根据所述有效传感信息确定状态观测值,并根据所述状态观测值更新每个所述滤波模型的模型权重;
根据每个所述滤波模型的所述模型权重进行数据融合,输出所述水下机器人当前时刻的预估位置。
由此,利用多个滤波模型进行信息融合,以求得误差最小的状态估计值,以此作为水下机器人当前时刻的预估位置。
进一步地,所述根据所述连续通信次数确定所述水下机器人当前时刻的预估位置包括:
当所述连续通信次数小于预设次数时,控制所述跟随设备原地等待。
由此,此时虽然接收到有效的传感位置信息,但连续通信次数较小,没有足够数据进行粒子初始化,因而控制跟随设备原地等待,以此保证通信连续。
为达上述目的,第二方面,本发明提供了一种水下机器人的位置估计装置,包括:
获取单元:用于获取水下机器人当前时刻的传感位置信息;
第一处理单元:用于若所述当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到所述有效传感信息的连续通信次数;
第二处理单元:用于根据所述连续通信次数,确定所述水下机器人当前时刻的预估位置,以使所述跟随设备对所述水下机器人进行跟踪定位。
本发明提供的水下机器人的位置估计装置,具有与本发明第一方面提供的水下机器人的位置估计方法具有类似的有益效果,在此不再赘述。
附图说明
图1所示为本发明实施例的水下机器人的位置估计方法的流程示意图一;
图2所示为本发明实施例的根据连续通信次数确定预测位置的流程示意图;
图3所示为本发明实施例的建立第一初始粒子集的流程示意图;
图4所示为本发明实施例的建立第二初始粒子集的流程示意图;
图5所示为本发明实施例的第一状态粒子权重修正的流程示意图;
图6所示为本发明实施例的模型交互的流程示意图;
图7所示为本发明实施例的交互式多模型的原理示意图;
图8所示为本发明实施例的水下机器人的位置估计方法的流程示意图二;
图9所示为本发明实施例的基于历史航迹算法的的原理示意图;
图10所示为本发明实施例的水下机器人的位置估计装置的结构示意图。
具体实施方式
下面将参照附图详细描述根据本发明的实施例,描述涉及附图时,除非另有表示,不同附图中的相同附图标记表示相同或相似的要素。要说明的是,以下示例性实施例中所描述的实施方式并不代表本发明的所有实施方式。它们仅是与如权利要求书中所详述的、本发明公开的一些方面相一致的装置和方法的例子,本发明的范围并不局限于此。在不矛盾的前提下,本发明各个实施例中的特征可以相互组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
AUV因其灵活性、智能性等特点,被广泛用于深海探索的多种任务之中,诸如水下安全搜救、水下目标观察、水下管道系统检修等。在AUV多种应用中,都需要将探测信息回传,维持其与工作人员之间的实时通讯是必不可少的。为了实时确定AUV位置,通常采用对应的跟随设备对AUV进行跟踪,完成与AUV的协同工作,充当AUV的通信中介,实时传输AUV获取的相关水下信息。
USV作为海上的智能体,在许多危险或者不必需要人力的情况下执行任务能起到很出色的效果。将USV作为跟随设备,与AUV配合工作,可以改善任务的整体运作,对任务的执行起到重大作用,对于推进油气勘探等自主海洋系统的发展也是至关重要的。USV和AUV之间采用水声通信,在此过程中,USV通过水声实时接收AUV的位置信息并跟踪AUV,以保证水声通信的距离,再由USV通过卫星等无线手段将探测信息传输至岸上。然而,水声通信相比于通常的无线电等通信方式,具有带宽低、传输效率慢等特点,并且受海洋传播特性的影响,易出现通信被噪声干扰的现象。因此,USV会出现接收不到数据或接收到无效数据的情况,即弱定位现象,导致在整个通信过程中,USV和AUV之间的通信程度有强有弱。
现有跟踪定位技术中,往往是利用历史航迹估算水下机器人的定位,或者固定利用某一种目标定位算法对水下机器人进行定位,忽略了时强时弱的通信传递对定位准确性的影响。因而,如何在时强时弱的通信过程中实时保持对AUV的有效跟踪定位是亟待解决的问题。
本发明第一方面的实施例提供了一种水下机器人的位置估计方法,结合图1来看,图1所示为本发明实施例的水下机器人的位置估计方法的流程示意图一,包括步骤S1至S3,其中:
在步骤S1中,获取水下机器人当前时刻的传感位置信息。由此,利用接收的传感位置信息判断当前时刻的通信是否有效。
可选地,本发明实施例中的跟随设备包括但不限于水面无人艇(USV),以此高效完成和水下机器人的协同工作。可以理解的是,本发明实施例提供的跟随设备包括多种海面智能工作设备,只要能实现对水下机器人的跟踪制导即可。
可选地,通过跟随设备上的多种传感装置获取水下机器人当前时刻的传感位置信息;或通过水下机器人向跟随设备发送自身的位置信息,跟随设备接收到位置信息后获取水下机器人当前时刻的传感位置信息。以此有效利用传感装置或通信传递进行信息获取。
在步骤S2中,若当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到有效传感信息的连续通信次数。由此,利用接收的传感位置信息判断当前时刻的通信是否有效,如果有效,说明当前时刻通信连续且传感位置信息包含的通信数据是有效的,并根据连续通信次数判断是否在连续时刻进行了有效通信,即有效判断了通信间隔情况。
其中,当传感位置信息中的数据有效时,传感位置信息为有效传感信息。由此,只有在跟随设备接收的传感位置信息中,包含有效的通信数据,才能记为一次有效通信,如果传感位置信息中,包含的通信数据为空或包含大量噪声,则为无效传感信息,为一次无效通信,即出现了通信间隔。
以一个具体的实施例进行说明,在跟随设备和水下机器人进行通信的过程中,k-3时刻对应的传感位置信息中的数据无效(即为无效传感信息,发生了通信间隔),k-2时刻对应的传感位置信息中的数据有效(是有效传感信息,通信恢复),k-1时刻对应的传感位置信息中的数据有效(是有效传感信息,通信连续)。若当前时刻为k时刻,当k时刻对应的传感位置信息中的数据有效时,通信依然连续,则对于k时刻,从通信恢复开始,k-2时刻、k-1时刻、k时刻连续3个时刻通信有效,连续通信次数为3;若当前时刻为k时刻,当k时刻对应的传感位置信息中的数据无效时,通信中断,则对于k时刻,通信已经中断,通信不连续,因而不需要确定连续通信次数(也相当于连续通信次数记为0)。
在步骤S3中,根据连续通信次数,确定水下机器人当前时刻的预估位置,以使跟随设备对水下机器人进行跟踪定位。由此,结合传感位置信息和连续通信次数反映通信有效性和通信时间间隔的情况,判断当前时刻定位的强弱,根据不同的定位情况,针对性地进行跟踪定位,以此实时保证跟随设备对水下机器人的有效跟踪。
可选地,结合图2来看,图2所示为本发明实施例的根据连续通信次数确定预测位置的流程示意图,包括步骤S31至S32,其中:
在步骤S31中,根据连续通信次数,确定初始状态粒子集。由此,根据不同的连续通信次数,有效确定不同的初始状态粒子集。
可选地,初始状态粒子集包括第一初始粒子集,结合图3来看,图3所示为本发明实施例的建立第一初始粒子集的流程示意图,包括步骤S311至S312,其中:
在步骤S311中,当连续通信次数大于预设次数时,根据跟随设备在当前时刻下连续N个时刻接收到的有效传感信息,确定连续N个时刻中每个时刻对应的一组运动状态值,其中,N等于预设次数。由此,当连续通信次数大于预设次数,说明连续N个时刻接收到的传感位置信息接收的信息都是有效的,因而根据对应的传感位置信息确定对应时刻的运动状态值,即可有效反映当前时刻水下机器人的运动状态。
可选地,一组运动状态值包括位置观测值、速度观测值、加速度观测值,以此有效反映当前时刻观测的水下机器人的运动状态,保证初始状态粒子集的完整性和有效性。
在本发明实施例中,预设次数优选为3。当预设次数为3时,若当前时刻为k+2时刻,连续N个时刻包括k时刻、k+1时刻以及k+2时刻,则Zk+2为当前时刻传感器接收到的位置观测值,为当前时刻传感器接收到的速度观测值,为当前时刻传感器接收到的加速度观测值。
在步骤S312中,根据连续N个时刻中每个时刻对应的一组运动状态值,确定连续N个时刻中每个时刻对应的第一状态粒子,连续N个时刻的第一状态粒子构成第一初始粒子集。由此,当连续通信次数大于预设次数,说明当前时刻的通信有效且在较长时间段内未发生通信间隔,此时定位状态良好,连续N个时刻中的传感位置信息都是有效、可靠的数据,因而根据通信连续N个时刻中的传感位置信息建立初始状态粒子集,建立不同的交互式多模型,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
可选地,初始状态粒子集包括第二初始粒子集,当连续通信次数等于预设次数时,根据水下机器人的历史航迹对跟随设备在当前时刻下连续N个时刻接收到的传感位置信息进行初始化修正,确定第二初始粒子集。由此,当连续通信次数等于预设次数,说明当前时刻的通信有效且在短时间段内未发生通信间隔,此时定位状态较好,刚好接收到N个时刻的有效、可靠的观测信息,但由于仅仅只是短时间内未发生通信间隔,为了更好地保证传感位置信息的可靠性,因而结合水下机器人的历史航迹对传感位置信息进行修正,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
具体地,结合图4来看,图4所示为本发明实施例的建立第二初始粒子集的流程示意图,包括步骤S313至S314,其中:
在步骤S313中,根据跟随设备在当前时刻下连续N个时刻接收到的有效传感信息,确定连续N个时刻中每个时刻对应的一组运动状态值。由此,根据对应的传感位置信息确定对应时刻的运动状态值,即可有效反映当前时刻水下机器人的运动状态。
在步骤S314中,根据连续N个时刻中每个时刻对应的一组运动状态值,确定连续N个时刻中每个时刻对应的第一状态粒子,连续N个时刻的第一状态粒子构成候选粒子集。由此,根据第一状态粒子构建对应的候选粒子集,以便后续的修正。在本发明实施例中,第一状态粒子的表达式参见式(1),候选粒子集的构成方式参见第一初始粒子集的构成方式,在此不再赘述。
在步骤S315中,根据历史航迹对候选粒子集中的第一状态粒子进行权重修正,确定第二初始粒子集。由此,针对第一状态粒子进行对应的矫正,以此得到更为准确可靠的第二初始粒子集,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
可选地,结合图5来看,图5所示为本发明实施例的第一状态粒子权重修正的流程示意图,包括步骤S3151至S3154,其中:
在步骤S3151中,根据历史航迹,确定连续N个时刻中每个时刻对应的一组预测状态值。由此,根据历史航迹,有效估计个时刻对应的一组预测状态值。
可选地,一组预测状态值包括位置预测值、速度预测值、加速度预测值,以此有效反映当前时刻预测的水下机器人的运动状态,保证预测状态集的完整性和有效性。
在本发明实施例中,预设次数优选为3。当预设次数为3(即N也为3)时,若当前时刻为k+2时刻,连续N个时刻包括k时刻、k+1时刻以及k+2时刻,则为当前时刻传感器接收到的位置预测值,为当前时刻传感器接收到的速度预测值,为当前时刻传感器接收到的加速度预测值。
在步骤S3152中,根据连续N个时刻中每个时刻对应的一组预测状态值,确定连续N个时刻中每个时刻对应的预测粒子,连续N个时刻的预测粒子构成预测状态集。由此,根据历史航迹预测的每个时刻的预测粒子,有效构成对应的预测状态集。
在步骤S3153中,根据候选粒子集和所述预测状态集之间的粒子转移概率,确定候选粒子集中每个第一状态粒子对应的修正权值。由此,根据粒子转移概率,有效确定修正权值。
在步骤S3154中,根据每个第一状态粒子和对应的修正权值,确定修正后的第二状态粒子,连续N个时刻的第二状态粒子构成第二初始粒子集。由此,根据历史航迹建立对应的预测状态集,利用预测状态集中的预测粒子预测每个时刻对应的运动状态,以此确定修正权值,保证对第一状态粒子的有效修正,建立准确可靠的第二初始粒子集。
在本发明实施例中,第二初始粒子集X表示为下式(4):
在步骤S32中,根据初始状态粒子集建立对应的交互式多模型,将初始状态粒子集作为交互式多模型的初始输入,确定水下机器人当前时刻的预估位置。由此,在当前时刻的传感位置信息为有效传感信息的情况下,说明当前的通信数据准确,没有出现通信间隔,因而根据连续通信次数判断当前时刻跟随设备和水下机器人之间的定位强弱,依据定位的强弱确定不同的初始状态粒子集,建立不同的交互式多模型,以便对水下机器人当前时刻的预估位置进行准确估计,保证定位的准确性。
可选地,结合图6来看,图6所示为本发明实施例的模型交互的流程示意图,步骤S32具体包括步骤S321至步骤S323,其中:
在步骤S321中,将初始状态粒子集中的每个状态粒子输入至设置的多个滤波模型中,以进行模型交互。由此,将初始状态粒子集作为多个滤波模型的有效输入。
在步骤S322中,根据传感位置信息确定状态观测值,并根据状态观测值更新每个滤波模型的模型权重。一般而言,根据状态观测值,确定每个滤波模型的权重,以使滤波模型的输出和状态观测值之间的误差最小。
在步骤S323中,根据每个滤波模型的模型权重进行数据融合,输出水下机器人当前时刻的预估位置。由此,利用多个滤波模型进行信息融合,以求得误差最小的状态估计值,以此作为水下机器人当前时刻的预估位置。
在本发明实施例中,结合图7来看,图7所示为本发明实施例的交互式多模型的原理示意图,在k-1时刻下,初始状态粒子集为X(k-1|k-1),包含N个粒子(第一状态粒子或第二状态粒子),Z(k)为根据传感位置信息确定的对应的状态观测值,为粒子j在滤波模型k下的输入,对于粒子j,将其与r个滤波模型,即图7中的r个PF(PassiveFilter)滤波器进行交互,交互状态输出表示为下式(5):
协方差Pi表示为下式(7):
上式中,P0为预设的矩阵。
在本发明实施例中,权重更新的步骤包括一步预测、计算残差和残差的协方差、赋予粒子权重以及重采样。
其中,一步预测的公式表示为下式(8):
上式中,为粒子j在滤波模型i下的交互状态输出,Z(k)为根据传感位置信息确定的对应的状态观测值。ej是一个2×1的矩阵,且其残差的量纲不一致,因此这里将不同的量纲的残差进行归一化处理,均是一个(0,1)的数,越靠近1说明偏差越大。
在本发明实施例中,在重采样的过程中,根据粒子的权重不同复制粒子,权重大的复制个数多,权重小的复制次数少或不复制,重采样的方法已有许多学者提出并改善,也是影响粒子有效性的因素之一。重采样后的粒子权重均相同。
在本发明实施例中,根据上述求得的每个滤波模型的残差和残差的协方差计算粒子j在不同滤波模型的相似度,表示为下式(11):
更新滤波模型概率的公式表示为下式(12):
更新权重后的交互状态输出xj(k|k)表示为下式(13):
在本发明实施例中,在步骤S323中,由于之前取权重时对每个粒子的权重做了整体粒子的归一化,因此在融合过程中只需将每个粒子的状态进行叠合即可。因而,水下机器人当前时刻的预估位置X(k|k)表示为下式(14):
其中,xj(k|k)为更新权重后的滤波模型的交互状态输出。
可选地,结合图8来看,图8所示为本发明实施例的水下机器人的位置估计方法的流程示意图二,包括步骤S4,其中:
在步骤S4中,若当前时刻的传感位置信息为无效传感信息,则根据水下机器人的历史航迹,确定水下机器人当前时刻的预估位置。由此,在当前时刻的传感位置信息为无效传感信息的情况下,说明当前的通信数据并不准确,即已经出现了通信间隔,观测信息已经丢失,因而依据当前时刻的传感位置信息进行跟踪定位会导致定位不准,所以依据水下机器人的历史航迹进行定位,保证通信间隔下,对水下机器人定位的准确性。
具体地,结合图9来看,图9所示为本发明实施例的基于历史航迹算法的原理示意图。在基于历史航迹的算法中,根据之前多个时刻的横坐标外推出当前时刻的横坐标,根据之前多个时刻的纵坐标外推出当前时刻的纵坐标。图中,在当前时刻为k时刻的情况下,根据第k-j时刻之前的m+1个时刻的横坐标外推出当前时刻的横坐标,根据第k-j时刻之前的m+1个时刻的纵坐标外推出当前时刻的纵坐标,即根据第k-j-m时刻的横坐标xk-j-m至第k-j时刻的横坐标xk-j外推出xk,根据第k-j-m时刻的纵坐标yk-j-m至第k-j时刻的横坐标yk-j外推出yk。
本发明第一方面实施例提供的水下机器人的位置估计方法,利用了接收的传感位置信息判断当前时刻的通信是否有效,并根据连续通信次数判断是否在连续时刻进行了有效通信,即有效判断了通信间隔。结合传感位置信息和连续通信次数反映通信有效性和通信时间间隔的情况,判断当前时刻定位的强弱,根据不同的定位情况,针对性地进行跟踪定位,以此实时保证跟随设备对水下机器人的有效跟踪。
本发明第二方面的实施例还提供了一种水下机器人的位置估计装置。图10所示为本发明实施例的水下机器人的位置估计装置1000的结构示意图,包括获取单元1001、第一处理单元1002以及第二处理单元1003。
获取单元1001:用于获取水下机器人当前时刻的传感位置信息;
第一处理单元1002:用于若当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到有效传感信息的连续通信次数;
第二处理单元1003:用于根据连续通信次数,确定水下机器人当前时刻的预估位置,以使跟随设备对水下机器人进行跟踪定位。
水下机器人的位置估计装置1000的各个单元的更具体实现方式可以参见对于本发明的水下机器人的位置估计方法的描述,且具有与之相似的有益效果,在此不再赘述。
本发明第三方面的实施例提出了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时,实现根本发明第一方面实施例的水下机器人的位置估计方法。
一般来说,用于实现本发明方法的计算机指令的可以采用一个或多个计算机可读的存储介质的任意组合来承载。非临时性计算机可读存储介质可以包括任何计算机可读介质,除了临时性地传播中的信号本身。
计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码,程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言,特别是可以使用适于神经网络计算的Python语言和基于TensorFlow、PyTorch等平台框架。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
本发明第四方面的实施例提供了一种计算设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,处理器执行程序时,实现本发明第一方面实施例的水下机器人的位置估计方法。
根据本发明第三、四方面的计算机可读存储介质和计算设备,可以参照根据本发明第一方面实施例具体描述的内容实现,并具有与本发明第一方面实施例的水下机器人的位置估计方法具有类似的有益效果,在此不再赘述。
尽管上面已经示出和描述了本发明的实施例,应当理解的是,上述实施例是示例性的,不能解释为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (7)
1.一种水下机器人的位置估计方法,其特征在于,包括:
获取水下机器人当前时刻的传感位置信息;
若所述当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到所述有效传感信息的连续通信次数;
根据所述连续通信次数,确定初始状态粒子集,进而确定所述水下机器人当前时刻的预估位置,以使所述跟随设备对所述水下机器人进行跟踪定位,其中,所述初始状态粒子集包括第二初始粒子集;
所述根据所述连续通信次数,确定初始状态粒子集包括:当所述连续通信次数等于预设次数时,根据所述水下机器人的历史航迹对所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集;
所述根据所述水下机器人的历史航迹对所述跟随设备连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集包括,根据所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息,确定所述连续N个时刻中每个时刻对应的一组运动状态值;根据所述连续N个时刻中每个时刻对应的所述一组运动状态值,确定所述连续N个时刻中每个时刻对应的第一状态粒子,所述连续N个时刻的所述第一状态粒子构成候选粒子集,其中,所述初始状态粒子集包括所述候选粒子集;根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集,其中,N等于所述预设次数;
所述根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集包括:
根据所述历史航迹,确定所述连续N个时刻中每个时刻对应的一组预测状态值;
根据所述连续N个时刻中每个时刻对应的所述一组预测状态值,确定所述连续N个时刻中每个时刻对应的预测粒子,所述连续N个时刻的所述预测粒子构成预测状态集;
根据所述候选粒子集和所述预测状态集之间的粒子转移概率,确定所述候选粒子集中每个所述第一状态粒子对应的修正权值;
2.如权利要求1所述的水下机器人的位置估计方法,其特征在于,还包括:
若所述当前时刻的传感位置信息为无效传感信息,则根据所述水下机器人的历史航迹,确定所述水下机器人当前时刻的预估位置。
3.如权利要求1所述的水下机器人的位置估计方法,其特征在于,所述根据所述连续通信次数,确定初始状态粒子集,进而确定所述水下机器人当前时刻的预估位置包括:
根据所述初始状态粒子集建立对应的交互式多模型,将所述初始状态粒子集作为所述交互式多模型的初始输入,确定所述水下机器人当前时刻的预估位置。
4.如权利要求3所述的水下机器人的位置估计方法,其特征在于,所述根据所述连续通信次数,确定初始状态粒子集包括:
当所述连续通信次数大于预设次数时,根据所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息,确定所述连续N个时刻中每个时刻对应的一组运动状态值,其中,N等于所述预设次数;
根据所述连续N个时刻中每个时刻对应的所述一组运动状态值,确定所述连续N个时刻中每个时刻对应的第一状态粒子,所述连续N个时刻的所述第一状态粒子构成所述候选粒子集。
5.如权利要求3所述的水下机器人的位置估计方法,其特征在于,所述根据所述初始状态粒子集建立对应的交互式多模型,将所述初始状态粒子集作为所述交互式多模型的初始输入,确定所述水下机器人当前时刻的预估位置包括:
将所述初始状态粒子集中的每个状态粒子输入至设置的多个滤波模型中,以进行模型交互;
根据所述有效传感信息确定状态观测值,并根据所述状态观测值更新每个所述滤波模型的模型权重;
根据每个所述滤波模型的所述模型权重进行数据融合,输出所述水下机器人当前时刻的预估位置。
6.如权利要求1所述的水下机器人的位置估计方法,其特征在于,所述根据所述连续通信次数确定所述水下机器人当前时刻的预估位置包括:
当所述连续通信次数小于预设次数时,控制所述跟随设备原地等待。
7.一种水下机器人的位置估计装置,其特征在于,包括:
获取单元:用于获取水下机器人当前时刻的传感位置信息;
第一处理单元:用于若所述当前时刻的传感位置信息为有效传感信息,则确定跟随设备在当前时刻下连续接收到所述有效传感信息的连续通信次数;根据所述连续通信次数,确定初始状态粒子集,进而确定所述水下机器人当前时刻的预估位置,以使所述跟随设备对所述水下机器人进行跟踪定位,其中,所述初始状态粒子集包括第二初始粒子集;
第二处理单元:用于根据所述连续通信次数,确定初始状态粒子集包括:当所述连续通信次数等于预设次数时,根据所述水下机器人的历史航迹对所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集;所述根据所述水下机器人的历史航迹对所述跟随设备连续N个时刻接收到的所述有效传感信息进行初始化修正,确定所述第二初始粒子集包括,根据所述跟随设备在当前时刻下连续N个时刻接收到的所述有效传感信息,确定所述连续N个时刻中每个时刻对应的一组运动状态值;根据所述连续N个时刻中每个时刻对应的所述一组运动状态值,确定所述连续N个时刻中每个时刻对应的第一状态粒子,所述连续N个时刻的所述第一状态粒子构成候选粒子集,其中,所述初始状态粒子集包括所述候选粒子集;根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集,其中,N等于所述预设次数;所述根据所述历史航迹对所述候选粒子集中的所述第一状态粒子进行权重修正,确定所述第二初始粒子集包括:根据所述历史航迹,确定所述连续N个时刻中每个时刻对应的一组预测状态值;根据所述连续N个时刻中每个时刻对应的所述一组预测状态值,确定所述连续N个时刻中每个时刻对应的预测粒子,所述连续N个时刻的所述预测粒子构成预测状态集;根据所述候选粒子集和所述预测状态集之间的粒子转移概率,确定所述候选粒子集中每个所述第一状态粒子对应的修正权值;根据每个所述第一状态粒子和对应的所述修正权值,确定修正后的第二状态粒子,所述连续N个时刻的所述第二状态粒子构成所述第二初始粒子集,所述修正权值表示为其中,表示第i个第一状态粒子和预测状态集之间粒子转移概率,表示N个第一状态粒子的转移概率转移之和。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010978540.2A CN112083377B (zh) | 2020-09-17 | 2020-09-17 | 一种水下机器人的位置估计方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010978540.2A CN112083377B (zh) | 2020-09-17 | 2020-09-17 | 一种水下机器人的位置估计方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112083377A CN112083377A (zh) | 2020-12-15 |
CN112083377B true CN112083377B (zh) | 2023-03-17 |
Family
ID=73737246
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010978540.2A Active CN112083377B (zh) | 2020-09-17 | 2020-09-17 | 一种水下机器人的位置估计方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112083377B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113433553B (zh) * | 2021-06-23 | 2022-08-02 | 哈尔滨工程大学 | 一种水下机器人多源声学信息融合精确导航方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105447574A (zh) * | 2015-11-10 | 2016-03-30 | 深圳大学 | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 |
CN107192995A (zh) * | 2017-05-23 | 2017-09-22 | 西北工业大学 | 一种多层次信息融合的纯方位水下目标跟踪算法 |
CN107704432A (zh) * | 2017-07-28 | 2018-02-16 | 西安理工大学 | 一种转移概率自适应的交互多模型目标跟踪方法 |
CN110347168A (zh) * | 2019-07-10 | 2019-10-18 | 哈尔滨工程大学 | 一种实时追踪水下机器人的方法、系统和无人中继装备 |
CN111082878A (zh) * | 2019-11-15 | 2020-04-28 | 电子科技大学 | 一种基于水下移动无线传感网的目标跟踪方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
IL179344A (en) * | 2006-11-16 | 2014-02-27 | Rafael Advanced Defense Sys | Mobile Platform Tracking Method |
CN109959381B (zh) * | 2017-12-22 | 2021-06-04 | 深圳市优必选科技有限公司 | 一种定位方法、装置、机器人及计算机可读存储介质 |
-
2020
- 2020-09-17 CN CN202010978540.2A patent/CN112083377B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105447574A (zh) * | 2015-11-10 | 2016-03-30 | 深圳大学 | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 |
CN107192995A (zh) * | 2017-05-23 | 2017-09-22 | 西北工业大学 | 一种多层次信息融合的纯方位水下目标跟踪算法 |
CN107704432A (zh) * | 2017-07-28 | 2018-02-16 | 西安理工大学 | 一种转移概率自适应的交互多模型目标跟踪方法 |
CN110347168A (zh) * | 2019-07-10 | 2019-10-18 | 哈尔滨工程大学 | 一种实时追踪水下机器人的方法、系统和无人中继装备 |
CN111082878A (zh) * | 2019-11-15 | 2020-04-28 | 电子科技大学 | 一种基于水下移动无线传感网的目标跟踪方法 |
Non-Patent Citations (9)
Title |
---|
《中国优秀硕士学位论文全文数据库信息科技辑》;赵丹丹;《中国优秀硕士学位论文全文数据库信息科技辑》;20180515;全文 * |
Actuator Fault Diagnosis of Automatic Underwater Vehicle Using Gaussian Particle Filter;Wan Lei 等;《Journal of Shanghai Jiaotong University》;20130728;第47卷(第07期);全文 * |
Formation Control of Multiple Unmanned Surface Vehicles Using the Adaptive Null-Space-Based Behavioral Method;Jiajia Fan 等;《IEEE Access》;20190627;第7卷;全文 * |
Interacting multiple model particle filter optimization resampling algorithm;Zhou Weidong 等;《Journal of Beijing University of Aeronautics and Astronautics》;20170531;第43卷(第05期);全文 * |
Interacting multiple model particle filtering using new particle resampling algorithm;DahChung Chang 等;《2014 IEEE Global Communications Conference》;20150212;全文 * |
交互式多模型粒子滤波算法综述;周云锋 等;《军械工程学院学报》;20100629;第22卷(第01期);第57-62页 * |
冀大雄 等.弱通信条件下USV对AUV的自主跟踪控制研究.《科学通报》.2013,第58卷(第S2期), * |
弱通信条件下USV对AUV的自主跟踪控制研究;冀大雄 等;《科学通报》;20131231;第58卷(第S2期);第49-54页 * |
粒子滤波理论在单目标跟踪中的应用综述;刘畅 等;《飞航导弹》;20171015(第10期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN112083377A (zh) | 2020-12-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109186610B (zh) | 一种auv地形匹配导航的鲁棒bslam方法 | |
WO2018224695A1 (en) | Training action selection neural networks | |
CN110378411B (zh) | 一种基于交互式多模型的支持向量机辅助水下机动目标跟踪方法 | |
CN112083377B (zh) | 一种水下机器人的位置估计方法及装置 | |
CN116071667B (zh) | 基于历史数据的指定区域内异常飞机目标检测方法及系统 | |
Zhang et al. | Application of modified ekf based on intelligent data fusion in auv navigation | |
Gongguo et al. | Sensor scheduling for ground maneuvering target tracking in presence of detection blind zone | |
Hu et al. | Research on ship motion prediction algorithm based on dual-pass long short-term memory neural network | |
CA3195472A1 (en) | Computing an explainable event horizon estimate | |
Liu et al. | Navigation algorithm based on PSO-BP UKF of autonomous underwater vehicle | |
Noureldin et al. | Adaptive neuro-fuzzy module for inertial navigation system/global positioning system integration utilising position and velocity updates with real-time cross-validation | |
CN110749325B (zh) | 航迹规划方法和装置 | |
CN110208808B (zh) | 一种被动声纳非合作目标线谱信息融合方法 | |
CN116047886B (zh) | 一种基于神经网络的管道潜航机器人控制方法及系统 | |
Moretti et al. | Autonomous space object catalogue construction and upkeep using sensor control theory | |
Zhang et al. | Bathymetric particle filter SLAM with graph-based trajectory update method | |
CN114742644A (zh) | 训练多场景风控系统、预测业务对象风险的方法和装置 | |
KR102261155B1 (ko) | 가상 센서 이중화를 통한 차량 제어 방법 및 그 장치 | |
Yang et al. | Online risk modeling of autonomous marine systems: Case study of autonomous operations under sea ice | |
Huang et al. | Bayesian Real-Time System Identification: From Centralized to Distributed Approach | |
Zhang et al. | Sequential learning navigation method and general correction model for Autonomous Underwater Vehicle | |
Palmier et al. | Interacting weighted ensemble kalman filter applied to underwater terrain aided navigation | |
Cai et al. | Multi-AUV collaborative hunting method for the non-cooperative target in underwater environment | |
Chen et al. | Real-time UAV Flight Path Prediction Using A Bi-directional Long Short-term Memory Network with Error Compensation [J] | |
CN116993832B (zh) | 一种基于cnn的飞行目标的位置预测方法及装置 |
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 |