CN116886569B - 一种基于自主机器人的通信延迟补偿方法及系统 - Google Patents
一种基于自主机器人的通信延迟补偿方法及系统 Download PDFInfo
- Publication number
- CN116886569B CN116886569B CN202310975105.8A CN202310975105A CN116886569B CN 116886569 B CN116886569 B CN 116886569B CN 202310975105 A CN202310975105 A CN 202310975105A CN 116886569 B CN116886569 B CN 116886569B
- Authority
- CN
- China
- Prior art keywords
- auv
- equation
- position information
- representing
- observation
- 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 41
- 238000004891 communication Methods 0.000 title claims abstract description 39
- 239000013598 vector Substances 0.000 claims abstract description 64
- 238000001914 filtration Methods 0.000 claims abstract description 41
- 238000005259 measurement Methods 0.000 claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims description 28
- 238000010276 construction Methods 0.000 claims description 21
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 4
- 230000009897 systematic effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 230000033001 locomotion Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
- H04L43/00—Arrangements for monitoring or testing data switching networks
- H04L43/08—Monitoring or testing based on specific metrics, e.g. QoS, energy consumption or environmental parameters
- H04L43/0852—Delays
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
- H04L41/00—Arrangements for maintenance, administration or management of data switching networks, e.g. of packet switching networks
- H04L41/14—Network analysis or design
- H04L41/145—Network analysis or design involving simulating, designing, planning or modelling of a network
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Environmental & Geological Engineering (AREA)
- Feedback Control In General (AREA)
Abstract
本申请公开了一种基于自主机器人的通信延迟补偿方法及系统,其中方法步骤包括:基于AUV定位系统的系统模型,建立状态方程;基于系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;根据状态方程和夹角,建立观测方程;基于系统模型、夹角和观测方程,建立扩展卡尔曼滤波方程;利用扩展卡尔曼滤波方程计算本次位置信息预测值,完成通信延迟补偿。本申请利用自主水下机器人的多普勒测量信息计算速度方向矢量与AUV和观测站位置矢量的夹角,重新构建EKF的观测方程,使用EKF进行最优滤波估计,本申请可以有效补偿水下通信延迟导致的定位误差,具有很好的实际效果。
Description
技术领域
本申请涉及水下通信补偿领域,具体涉及一种基于自主机器人的通信延迟补偿方法及系统。
背景技术
自主水下机器人是一种综合智能化技术和其他先进计算技术的高级水下潜器,其集成了自动控制、导航定位、能量转换、目标识别和故障处理等多个模块功能,具有自主性强、隐蔽性高、环境适应广、成本低、易拓展等诸多优势。
在水下导航任务中,准确的导航信息是上述应用的前提,位置信息不仅用于确定空间位置,而且是AUV整体有效应用和安全
回收的重要保障。由于声学信号的传播损耗远低于无线电信号,因此声学导航方法是最有效的方法,这使得水声定位系统成为自动水下航行器(AUV)必不可少的定位和导航组件,也是水下导航任务顺利执行的关键。
在实际的水下导航环境中,AUV和其他节点基站之间的相互通信,数据处理和水下数据传输均需要一定的时间,在滤波估计中引入量测信息时会存在数据的延迟,从而会影响最终的滤波效果。
卡尔曼滤波算法主要用于动态环境中传感器冗余数据的融合,该方法以测量模型的统计特性为基础,递推的确定融合数据在统计意义下的最优估计,这一特性使得卡尔曼滤波算法很适合用于不具备大量数据传输和存储能力的系统中。
发明内容
为解决上述背景中存在的技术问题,本申请通过构建扩展卡尔曼滤波的量测信息,提供一种基于自主水下机器人的通信延迟补偿方法,方法通过多普勒测量的AUV上一时刻和当前时刻方向矢量与上一时刻AUV与观测站方向矢量的方向角信息,将多普勒测量信息代入量测方程进行量测更新,采用所述的水下定位系统通信延迟补偿算法可以有效减小因水下通信延迟带来的定位误差,提高了定位精度,性能优于传统扩展卡尔曼滤波方法,具有很好的实际效果。
为实现上述目的,本申请提供了一种基于自主机器人的通信延迟补偿方法,步骤包括:
基于AUV定位系统的系统模型,建立状态方程;
基于所述系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;
根据所述状态方程和所述夹角,建立观测方程;
基于所述系统模型、所述夹角和所述观测方程,建立扩展卡尔曼滤波方程;
利用所述扩展卡尔曼滤波方程计算本次位置信息预测值,完成通信延迟补偿。
优选的,所述状态方程包括:
Xk=f(Xk-1,uk-1)+Γk-1Wk
式中,Xk表示k时刻的n维状态向量;Xk-1表示k-1时刻的n维状态向量;uk-1表示系统的输入量;Γk-1表示从k-1时刻到k时刻的系统噪声输入矩阵;Wk表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
优选的,获取所述夹角的方法包括:根据所述系统模型,建立多普勒测量方程,通过多普勒测量方程反推得到所述夹角:
式中,fs表示目标发射的声信号频率;fk是目标理论接收的声信号频率;表示目标实际接收的声信号频率;ek表示频率噪声;c表示传播信号在水中的传播速度;vk表示AUV的行驶速度;u是观测站位置;sk表示k时刻AUV位置;βk表示AUV到观测站的方向矢量与AUV速度矢量的夹角;T表示向量转置符号。
优选的,所述观测方程包括:
式中,d′表示上一时刻AUV距离观测站的距离;δt表示通信延迟时间;dδt表示δt时间内AUV位置行驶的距离。
优选的,计算所述位置信息预测值的方法包括:
根据AUV和观测站之间的距离信息,计算出AUV的位置信息,获取本次AUV位置信息测量值,同时获取上次AUV位置信息的预测值和上次AUV位置信息预测误差;
利用所述观测方程,根据所述本次AUV位置信息测量值,计算本次AUV位置信息观测值;
利用所述扩展卡尔曼滤波方程,根据所述本次AUV位置信息观测值、所述上次位置信息预测值和上次位置信息预测误差,计算出本次AUV位置信息的预测值,所述本次AUV位置信息的预测值即为所述位置信息预测值。
优选的,利用所述扩展卡尔曼滤波方程,根据所述上次AUV位置信息预测值,计算下一步位置信息预测值;步骤包括:
式中,为下一步位置预测值;/>为上次AUV位置信息的预测值;
利用所述扩展卡尔曼滤波方程,根据所述上次位置信息预测误差,计算下一步位置信息预测误差;步骤包括:
其中,Pk|k-1表示下一步位置预测误差;Pk-1表示上次AUV位置信息预测误差;Qk-1表示上次位置观测误差;Φk|k-1表示状态转移矩阵;Γk-1表示系统噪声输入矩阵;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测误差,计算滤波增益;步骤包括:
式中,Kk表示滤波增益;Pk|k-1表示下一步位置预测误差;Hk表示k时刻观测矩阵;Rk表示系统噪声误差;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测值、所述滤波增益和所述本次位置信息观测值,计算本次位置信息预测值;步骤包括:
式中,表示本次位置预测值;/>表示下一步位置预测值;Kk表示滤波增益,Zk表示本次位置观测预测值;/>上次AUV位置信息的预测值;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测误差,计算本次位置信息预测误差;步骤包括:
Pk=(I-KkHk)Pk|k-1
式中;Pk表示本次位置预测误差;I表示单位矩阵;Hk表示k时刻观测矩阵;Pk|k-1表示下一步位置预测误差。
本申请还提供了一种基于自主机器人的通信延迟补偿系统,包括:AUV定位系统,还包括:第一构建模块、获取模块、第二构建模块、第三构建模块、预测模块;
所述第一构建模块用于基于AUV定位系统的系统模型,建立状态方程;
所述获取模块用于基于所述系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;
所述第二构建模块用于根据所述状态方程和所述夹角,建立观测方程;
所述第三构建模块用于基于所述系统模型、所述夹角和所述观测方程,建立扩展卡尔曼滤波方程;
所述预测模块用于利用所述扩展卡尔曼滤波方程计算位置信息预测值,完成通信延迟补偿。
优选的,所述状态方程包括:
Xk=f(Xk-1,uk-1)+Γk-1Wk
式中,Xk表示k时刻的n维状态向量;Xk-1表示k-1时刻的n维状态向量;uk-1表示系统的输入量;Γk-1表示从k-1时刻到k时刻的系统噪声输入矩阵;Wk表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
与现有技术相比,本申请的有益效果如下:
本申请利用自主水下机器人的多普勒测量信息计算速度方向矢量与AUV和观测站位置矢量的夹角,重新构建EKF的观测方程,使用EKF进行最优滤波估计,本申请可以有效补偿水下通信延迟导致的定位误差,具有很好的实际效果。
附图说明
为了更清楚地说明本申请的技术方案,下面对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本申请实施例的方法流程示意图;
图2为本申请实施例的定位系统通信延迟示意图;
图3为本申请实施例的自主水下机器人(AUV)运动示意图;
图4为本申请实施例的基于多普勒测量的定位目标几何图;
图5为本申请实施例的系统结构示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
为使本申请的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本申请作进一步详细的说明。
实施例一
如图1所示,为本实施例的方法流程示意图,步骤包括:
S1.基于AUV定位系统的系统模型,建立状态方程。
基于投放的自主水下机器人(AUV)的系统模型,建立状态方程:
Xk=f(Xk-1,uk-1)+Γk-1Wk
式中,Xk表示k时刻的n维状态向量;Xk-1表示k-1时刻的n维状态向量;uk-1表示系统的输入量;Γk-1表示从k-1时刻到k时刻的系统噪声输入矩阵;Wk表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
本实施例中,考虑到水下定位系统中的通信延迟进而可能影响定位精度问题,若直接将AUV与观测站之间的距离作为测量信息代入观测方程中将会引入较大误差影响导航性能。在此场景下,本实施例引入多普勒测量信息,使用一种基于重建量测方程的EKF滤波方法,以补偿延迟引起的导航误差,定位系统通信延迟如图2所示。
S2.基于系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角(AUV的运动示意如图3所示),步骤包括:
如图4所示,基于多普勒测量的移动传感器对目标定位的几何图得到AUV的多普勒测量方程。
其中,fs是目标发射的声信号频率,fk是目标理论接收的声信号频率,是目标实际接收的声信号频率,ek表示频率噪声,c代表传播信号在水中的传播速度,vk是AUV的行驶速度,u是观测站位置,sk是k时刻AUV位置,βk为AUV到观测站的方向矢量与AUV速度矢量的夹角,T表示向量转置符号。
之后,基于收发频率值,由多普勒测量方程反推AUV的夹角:
最后,根据夹角,重新构建EKF的量测方程,更新后的观测方程为:
其中,Zk′是代表信号接受时刻t2时刻AUV距离观测站的欧氏距离,d′是上一时刻AUV距离观测站的距离,δt是通信延迟时间,dδt为δt时间内AUV行驶的距离,βk为AUV到观测站的方向矢量与AUV速度矢量的夹角。
由此获得了信号接收时刻t2更精确的AUV的位置信息,实现对通信延迟定位误差的补偿。
S3.根据状态方程和夹角,建立观测方程。
式中,d′表示上一时刻AUV距离观测站的距离;δt表示通信延迟时间;dδt表示δt时间内AUV位置行驶的距离。
利用观测方程,计算本次距离观测值;
其中,Zk是k时刻的观测向量,为系统观测向量的非线性函数,/>为k时刻的观测噪声向量。
S4.基于系统模型、夹角和观测方程,建立扩展卡尔曼滤波方程。其中,观测方程根据多普勒测量信息进行更新。
S5.利用扩展卡尔曼滤波方程计算位置信息预测值,完成通信延迟补偿。
根据AUV和观测站之间的距离信息,计算出AUV的位置信息,获取本次AUV位置信息测量值,同时获取上次AUV位置信息的预测值和上次AUV位置信息预测误差;
利用观测方程,根据本次AUV位置信息测量值,计算本次AUV位置信息观测值;
利用扩展卡尔曼滤波方程,根据本次AUV位置信息观测值、上次位置信息预测值和上次位置信息预测误差,计算出本次AUV位置信息的预测值,本次AUV位置信息的预测值即为位置信息预测值。
利用扩展卡尔曼方程,根据本次AUV位置信息观测值、上次AUV位置信息的预测值和上次AUV位置信息预测误差,计算本次位置预测值,完整的扩展卡尔曼滤波过程如下:
状态预测:
其中,为下一步位置预测值,/>为上次AUV位置信息的预测值。
利用扩展卡尔曼滤波方程,根据上次AUV位置信息预测误差,计算下一步位置预测。所建立的方程为:
其中,Pk|k-1为下一步位置预测误差,Pk-1为上次AUV位置信息预测误差,Qk-1为上次位置观测误差,Φk|k-1为状态转移矩阵,Γk-1为系统噪声输入矩阵,因为本实例为非线性问题,Φk|k-1和Γk-1分别为非线性函数f(·)关于和/>的雅克比矩阵,表示如下:
利用扩展卡尔曼滤波方程,根据下一步位置预测误差,计算滤波增益,建立的方程如下:
其中,Kk为滤波增益,Pk|k-1为下一步位置预测误差,Hk为k时刻观测矩阵,Rk为系统噪声误差。
利用扩展卡尔曼滤波方程,根据下一步位置预测值、滤波增益和本次AUV位置信息观测值,计算本次AUV的位置信息预测值(本次AUV位置信息的预测值即为位置信息预测值。);建立方程如下:
其中,为本次位置预测值,/>为下一步位置预测值,Kk为滤波增益,Zk为本次位置观测预测值,/>上次AUV位置信息的预测值。
利用卡尔曼滤波方程,根据下一步温度预测误差,计算本次温度预测误差;建立方程如下:
Pk=(I-KkHk)Pk|k-1
其中,Pk为本次位置预测误差,I为单位矩阵,Hk为k时刻观测矩阵,Pk|k-1为下一步位置预测误差。
实施例二
如图5所示,为本申请实施例的系统结构示意图,包括:AUV定位系统,还包括:第一构建模块、获取模块、第二构建模块、第三构建模块、预测模块。其中,第一构建模块用于基于AUV定位系统的系统模型,建立状态方程;获取模块用于基于系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;第二构建模块用于根据状态方程和夹角,建立观测方程;第三构建模块用于基于系统模型、夹角和观测方程,建立扩展卡尔曼滤波方程;预测模块用于利用卡尔曼滤波方程计算位置信息预测值,完成通信延迟补偿。
下面将结合本实施例,详细说明本申请如何解决实际生活中的技术问题。
第一构建模块基于AUV定位系统的系统模型,建立状态方程。
基于投放的自主水下机器人(AUV)的系统模型,建立状态方程:
Xk=f(Xk-1,uk-1)+Γk-1Wk
式中,Xk表示k时刻的n维状态向量;Xk-1表示k-1时刻的n维状态向量;uk-1表示系统的输入量;Γk-1表示从k-1时刻到k时刻的系统噪声输入矩阵;Wk表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
本实施例中,考虑到水下定位系统中的通信延迟进而可能影响定位精度问题,若直接将AUV与观测站之间的距离作为测量信息代入观测方程中将会引入较大误差影响导航性能。在此场景下,本实施例引入多普勒测量信息,使用一种基于重建量测方程的EKF滤波方法,以补偿延迟引起的导航误差,定位系统通信延迟如图2所示。
获取模块基于系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角(AUV的运动示意如图3所示),步骤包括:
如图4所示,基于多普勒测量的移动传感器对目标定位的几何图得到AUV的多普勒测量方程。
其中,fs是目标发射的声信号频率,fk是目标理论接收的声信号频率,是目标实际接收的声信号频率,ek表示频率噪声,c代表传播信号在水中的传播速度,vk是AUV的行驶速度,u是观测站位置,sk是k时刻AUV位置,βk为AUV到观测站的方向矢量与AUV速度矢量的夹角,T表示向量转置符号。
之后,基于收发频率值,由多普勒测量方程反推AUV的夹角:
最后,根据夹角,重新构建EKF的量测方程,更新后的观测方程为:
其中,Zk′是代表信号接受时刻t2时刻AUV距离观测站的欧氏距离,d′是上一时刻AUV距离观测站的距离,δt是通信延迟时间,dδt为δt时间内AUV行驶的距离,βk为AUV到观测站的方向矢量与AUV速度矢量的夹角。
由此获得了信号接收时刻t2更精确的AUV的位置信息,实现对通信延迟定位误差的补偿。
第二构建模块根据状态方程和夹角,建立观测方程。
式中,d′表示上一时刻AUV距离观测站的距离;δt表示通信延迟时间;dδt表示δt时间内AUV位置行驶的距离。
利用观测方程,计算本次距离观测值;
其中,Zk是k时刻的观测向量,为系统观测向量的非线性函数,/>为k时刻的观测噪声向量。
第三构建模块基于系统模型、夹角和观测方程,建立扩展卡尔曼滤波方程。其中,观测方程根据多普勒测量信息进行更新。
预测模块利用扩展卡尔曼滤波方程计算位置信息预测值,完成通信延迟补偿。
根据AUV和观测站之间的距离信息,计算出AUV的位置信息,获取本次AUV位置信息测量值,同时获取上次AUV位置信息的预测值和上次AUV位置信息预测误差;
利用观测方程,根据本次AUV位置信息测量值,计算本次AUV位置信息观测值;
利用扩展卡尔曼滤波方程,根据本次AUV位置信息观测值、上次位置信息预测值和上次位置信息预测误差,计算出本次AUV位置信息的预测值,本次AUV位置信息的预测值即为位置信息预测值。
利用扩展卡尔曼方程,根据本次AUV位置信息观测值、上次AUV位置信息的预测值和上次AUV位置信息预测误差,计算本次位置预测值,完整的扩展卡尔曼滤波过程如下:
状态预测:
其中,为下一步位置预测值,/>为上次AUV位置信息的预测值。
利用扩展卡尔曼滤波方程,根据上次AUV位置信息预测误差,计算下一步位置预测。所建立的方程为:
其中,Pk|k-1为下一步位置预测误差,Pk-1为上次AUV位置信息预测误差,Qk-1为上次位置观测误差,Φk|k-1为状态转移矩阵,Γk-1为系统噪声输入矩阵,因为本实例为非线性问题,Φk|k-1和Γk-1分别为非线性函数f(·)关于和/>的雅克比矩阵,表示如下:
利用扩展卡尔曼滤波方程,根据下一步位置预测误差,计算滤波增益,建立的方程如下:
其中,Kk为滤波增益,Pk|k-1为下一步位置预测误差,Hk为k时刻观测矩阵,Rk为系统噪声误差。
利用扩展卡尔曼滤波方程,根据下一步位置预测值、滤波增益和本次AUV位置信息观测值,计算本次AUV的位置信息预测值(本次AUV位置信息的预测值即为位置信息预测值。);建立方程如下:
其中,为本次位置预测值,/>为下一步位置预测值,Kk为滤波增益,Zk为本次位置观测预测值,/>上次AUV位置信息的预测值。
利用卡尔曼滤波方程,根据下一步温度预测误差,计算本次温度预测误差;建立方程如下:
Pk=(I-KkHk)Pk|k-1
其中,Pk为本次位置预测误差,I为单位矩阵,Hk为k时刻观测矩阵,Pk|k-1为下一步位置预测误差。
以上所述的实施例仅是对本申请优选方式进行的描述,并非对本申请的范围进行限定,在不脱离本申请设计精神的前提下,本领域普通技术人员对本申请的技术方案做出的各种变形和改进,均应落入本申请权利要求书确定的保护范围内。
Claims (6)
1.一种基于自主机器人的通信延迟补偿方法,其特征在于,步骤包括:
基于AUV定位系统的系统模型,建立状态方程;
基于所述系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;
根据所述状态方程和所述夹角,建立观测方程;获取所述夹角的方法包括:根据所述系统模型,建立多普勒测量方程,通过多普勒测量方程反推得到所述夹角:
式中,f s 表示目标发射的声信号频率;f k 是目标理论接收的声信号频率;/>表示目标实际接收的声信号频率;e k 表示频率噪声;c表示传播信号在水中的传播速度;v k 表示AUV的行驶速度;u是观测站位置;s k 表示k时刻AUV位置;/>表示AUV到观测站的方向矢量与AUV速度矢量的夹角;T表示向量转置符号;
所述观测方程包括:
式中,/>表示上一时刻AUV距离观测站的距离;/>表示通信延迟时间;/>表示/>时间内AUV位置行驶的距离;
基于所述系统模型、所述夹角和所述观测方程,建立扩展卡尔曼滤波方程;
利用所述扩展卡尔曼滤波方程计算本次位置信息预测值,完成通信延迟补偿。
2.根据权利要求1所述的基于自主机器人的通信延迟补偿方法,其特征在于,所述状态方程包括:
式中,/>表示k时刻的n维状态向量;/>表示k-1时刻的n维状态向量;/>表示系统的输入量;/>表示从k-1时刻到k时刻的系统噪声输入矩阵;表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
3.根据权利要求1所述的基于自主机器人的通信延迟补偿方法,其特征在于,计算所述位置信息预测值的方法包括:
根据AUV和观测站之间的距离信息,计算出AUV的位置信息,获取本次AUV位置信息测量值,同时获取上次AUV位置信息的预测值和上次AUV位置信息预测误差;
利用所述观测方程,根据所述本次AUV位置信息测量值,计算本次AUV位置信息观测值;
利用所述扩展卡尔曼滤波方程,根据所述本次AUV位置信息观测值、所述上次AUV位置信息的预测值和所述上次AUV位置信息预测误差,计算出本次AUV位置信息的预测值,所述本次AUV位置信息的预测值即为所述位置信息预测值。
4.根据权利要求3所述的基于自主机器人的通信延迟补偿方法,其特征在于,利用所述扩展卡尔曼滤波方程,根据所述上次AUV位置信息预测值,计算下一步位置信息预测值;步骤包括:
式中,/>为下一步位置预测值;/>为上次AUV位置信息的预测值;
利用所述扩展卡尔曼滤波方程,根据所述上次AUV位置信息预测误差,计算下一步位置信息预测误差;步骤包括:
其中,/>表示下一步位置预测误差;/>表示上次AUV位置信息预测误差;/>表示上次位置观测误差;/>表示状态转移矩阵;/>表示系统噪声输入矩阵;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测误差,计算滤波增益;步骤包括:
式中,/>表示滤波增益;/>表示下一步位置预测误差;/>表示k时刻观测矩阵;/>表示系统噪声误差;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测值、所述滤波增益和所述本次位置信息观测值,计算本次位置信息预测值;步骤包括:
式中,/>表示本次位置预测值;/>表示下一步位置预测值;/>表示滤波增益,/>表示本次位置观测预测值;/>上次AUV位置信息的预测值;
利用所述扩展卡尔曼滤波方程,根据所述下一步位置信息预测误差,计算本次位置信息预测误差;步骤包括:
式中;/>表示本次位置预测误差;/>表示单位矩阵;/>表示k时刻观测矩阵;/>表示下一步位置预测误差。
5.一种基于自主机器人的通信延迟补偿系统,包括:AUV定位系统,其特征在于,还包括:第一构建模块、获取模块、第二构建模块、第三构建模块、预测模块;
所述第一构建模块用于基于AUV定位系统的系统模型,建立状态方程;
所述获取模块用于基于所述系统模型,获取AUV至观测站的方向矢量与AUV的速度矢量的夹角;
所述第二构建模块用于根据所述状态方程和所述夹角,建立观测方程;获取所述夹角的方法包括:根据所述系统模型,建立多普勒测量方程,通过多普勒测量方程反推得到所述夹角:
式中,f s 表示目标发射的声信号频率;f k 是目标理论接收的声信号频率;/>表示目标实际接收的声信号频率;e k 表示频率噪声;c表示传播信号在水中的传播速度;v k 表示AUV的行驶速度;u是观测站位置;s k 表示k时刻AUV位置;/>表示AUV到观测站的方向矢量与AUV速度矢量的夹角;T表示向量转置符号;
所述观测方程包括:
式中,/>表示上一时刻AUV距离观测站的距离;/>表示通信延迟时间;/>表示/>时间内AUV位置行驶的距离;
所述第三构建模块用于基于所述系统模型、所述夹角和所述观测方程,建立扩展卡尔曼滤波方程;
所述预测模块用于利用所述扩展卡尔曼滤波方程计算位置信息预测值,完成通信延迟补偿。
6.根据权利要求5所述的基于自主机器人的通信延迟补偿系统,其特征在于,所述状态方程包括:
式中,/>表示k时刻的n维状态向量;/>表示k-1时刻的n维状态向量;/>表示系统的输入量;/>表示从k-1时刻到k时刻的系统噪声输入矩阵;/>表示k-1时刻的系统噪声向量;f表示系统状态的非线性状态函数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310975105.8A CN116886569B (zh) | 2023-08-04 | 2023-08-04 | 一种基于自主机器人的通信延迟补偿方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310975105.8A CN116886569B (zh) | 2023-08-04 | 2023-08-04 | 一种基于自主机器人的通信延迟补偿方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116886569A CN116886569A (zh) | 2023-10-13 |
CN116886569B true CN116886569B (zh) | 2024-05-03 |
Family
ID=88260497
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310975105.8A Active CN116886569B (zh) | 2023-08-04 | 2023-08-04 | 一种基于自主机器人的通信延迟补偿方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116886569B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109269497A (zh) * | 2018-07-31 | 2019-01-25 | 哈尔滨工程大学 | 基于auv切法向速度模型的多尺度无迹卡尔曼滤波估计方法 |
CN112507281A (zh) * | 2020-11-19 | 2021-03-16 | 东南大学 | 一种基于双状态多因子抗差估计的sins/dvl紧组合系统 |
CN114440925A (zh) * | 2021-12-29 | 2022-05-06 | 宜昌测试技术研究所 | 一种auv组合导航系统忽略水平姿态的dvl标定方法 |
JP2023024014A (ja) * | 2021-08-06 | 2023-02-16 | 株式会社デンソー | 処理システム、処理装置、処理方法、処理プログラム |
CN116125462A (zh) * | 2023-02-17 | 2023-05-16 | 南京理工大学 | 一种纯角度量测下机动目标跟踪方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11533593B2 (en) * | 2018-05-01 | 2022-12-20 | New York University | System method and computer-accessible medium for blockchain-based distributed ledger for analyzing and tracking environmental targets |
-
2023
- 2023-08-04 CN CN202310975105.8A patent/CN116886569B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109269497A (zh) * | 2018-07-31 | 2019-01-25 | 哈尔滨工程大学 | 基于auv切法向速度模型的多尺度无迹卡尔曼滤波估计方法 |
CN112507281A (zh) * | 2020-11-19 | 2021-03-16 | 东南大学 | 一种基于双状态多因子抗差估计的sins/dvl紧组合系统 |
JP2023024014A (ja) * | 2021-08-06 | 2023-02-16 | 株式会社デンソー | 処理システム、処理装置、処理方法、処理プログラム |
CN114440925A (zh) * | 2021-12-29 | 2022-05-06 | 宜昌测试技术研究所 | 一种auv组合导航系统忽略水平姿态的dvl标定方法 |
CN116125462A (zh) * | 2023-02-17 | 2023-05-16 | 南京理工大学 | 一种纯角度量测下机动目标跟踪方法 |
Non-Patent Citations (3)
Title |
---|
Tracking Performance Limitations of Netwoeked Control Systems With Repeated Zeros and Poles;C-Y Chen, Fliu, L Wu, H Yan, W HGui, H.Eugene Stanley;IEEE Transactions on Automatic Control;20210430;第66卷(第4期);全文 * |
基于EKF的无人潜航器航位推算算法;边信黔;周佳加;严浙平;赵大威;;华中科技大学学报(自然科学版);20110323(03);全文 * |
基于通信延迟误差补偿的协同导航算法;王文佳;孙睿智;高伟;徐博;;火力与指挥控制;20141215(12);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN116886569A (zh) | 2023-10-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111337020A (zh) | 引入抗差估计的因子图融合定位方法 | |
CN115143954B (zh) | 一种基于多源信息融合的无人车导航方法 | |
CN110191411B (zh) | 一种基于时空域联合处理的分布式协作定位系统和方法 | |
CN108134640B (zh) | 一种基于节点运动状态约束的协作定位系统和方法 | |
CN108717174B (zh) | 基于信息论的预测快速协方差交互融合无源协同定位方法 | |
CN108313330B (zh) | 一种基于增广Kalman滤波的卫星干扰力矩估计方法 | |
CN113178098A (zh) | 一种无人船事件触发分层协同控制系统 | |
CN111174774A (zh) | 一种某深度水层面模式下的导航信息融合方法及系统 | |
CN112577496A (zh) | 一种基于自适应选权的多源融合定位方法 | |
CN110213713B (zh) | 一种基于时空域联合处理的集中式协作定位系统和方法 | |
CN114370878B (zh) | 一种基于stackf的多auv协同定位方法 | |
CN111414696A (zh) | 一种基于模型预测扩展卡尔曼滤波的分级状态估计方法 | |
CN116886569B (zh) | 一种基于自主机器人的通信延迟补偿方法及系统 | |
CN114323034A (zh) | 一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法 | |
CN116358554A (zh) | 具有异常观测处理能力的水下航行器因子图融合导航方法 | |
CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
CN114035154B (zh) | 一种运动参数辅助的单站射频信号定位方法 | |
Ma et al. | Factor graph localization algorithm based on robust estimation | |
CN115113646A (zh) | 基于卡尔曼滤波的卫星编队平根状态连续估计方法及系统 | |
CN111025269B (zh) | 一种水下机器人传感器安装偏差估计方法 | |
CN115667845A (zh) | 用于移动载体的导航辅助方法 | |
Peng et al. | Maneuvering Target Tracking Using Current Statistical Model Based Adaptive UKF for Wireless Sensor Network. | |
CN117553787B (zh) | 水下无人航行器的协同导航方法、装置及系统 | |
CN116383966B (zh) | 一种基于交互多模型的多无人系统分布式协同定位方法 | |
JP3421242B2 (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 |