CN114840003A - 一种单领航多auv协同定位及轨迹跟踪控制方法 - Google Patents
一种单领航多auv协同定位及轨迹跟踪控制方法 Download PDFInfo
- Publication number
- CN114840003A CN114840003A CN202210279687.1A CN202210279687A CN114840003A CN 114840003 A CN114840003 A CN 114840003A CN 202210279687 A CN202210279687 A CN 202210279687A CN 114840003 A CN114840003 A CN 114840003A
- Authority
- CN
- China
- Prior art keywords
- tracker
- target
- vector
- state
- trajectory
- 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
- 238000000034 method Methods 0.000 title claims abstract description 24
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 7
- 230000033001 locomotion Effects 0.000 claims description 39
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000009795 derivation Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims 1
- 238000007689 inspection Methods 0.000 claims 1
- 230000000694 effects Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 5
- 230000015572 biosynthetic process Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 2
- 240000006829 Ficus sundaica Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 239000011541 reaction mixture Substances 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 239000000758 substrate Substances 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开了一种单领航多AUV协同定位及轨迹跟踪控制方法,步骤一:建立***运动学模型、领航器运动学模型以及两者之间距离量测的模型;步骤二:根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计;步骤三:根据可观测性进行协同轨迹规划,确定***理想跟踪轨迹;步骤四:根据步骤二以及步骤三中的目标状态以及跟踪轨迹,设计控制器,完成对目标的跟踪。本发明所提出的方法具有很强的鲁棒性和有效性。对不同的多个***均有很好的效果。
Description
技术领域
本发明属于水下多航行器协同定位及轨迹跟踪控制领域,涉及一种单领航多AUV协同定位及轨迹跟踪控制方法,特别是一种基于扩展卡尔曼滤波的单领航多AUV协同定位及轨迹跟踪控制方法。
背景技术
基于距离的同步目标定位与追踪由于其在海洋科学领域的大量应用的重要性,近年来受到了广泛的关注。在执行任务中,***配备了测量目标距离的声学设备,并利用这些信息估计目标的状态(定位任务),同时在预定目标附近保持所需的相对几何编队,以获取目标的最大距离信息用于估计(追踪任务)。由于低功率声学设备所能测量的最大量程有限制,所以追踪任务在海洋环境中具有极大的相关性。主要问题与定位任务有关。除了设计一个合适的目标状态估计器外,关键问题是如何规划***的运动,以便通过适当的机动使***到目标的范围提供足够的信息来估计目标的状态。从技术上讲,这个问题的答案必须提供***运动的条件,使获得的距离产生目标运动的可观测性。第二个问题与跟踪任务有关,即如何控制水下航行器,使它们收敛到目标附近,同时保持一个所需的相对几何编队。
发明内容
针对上述现有技术,本发明要解决的技术问题是提供一种基于扩展卡尔曼滤波的单领航多AUV协同定位及轨迹跟踪控制方法,具有很强的鲁棒性和有效性。
为解决上述技术问题,本发明一种单领航多AUV协同定位及轨迹跟踪控制方法,包括:
步骤一:建立***运动学模型、领航器运动学模型以及两者之间距离量测的模型;
步骤二:根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计;
步骤三:根据可观测性进行协同轨迹规划,确定***理想跟踪轨迹;
步骤四:根据步骤二以及步骤三中的目标状态以及跟踪轨迹,设计控制器,完成对目标的跟踪。
进一步的,步骤一具体为:
建立***运动学模型:
假设有N个***(N≥1),{I}={xI,yI,zI}为惯性坐标系,为第i个***的自身坐标系,i∈N;代表惯性坐标系中第i个***的坐标;表示第i个***在自身坐标系中的速度向量,则***的运动学模型是:
其中,R(η[i])是从自身坐标系到惯性系的转换矩阵,其中向量是由滚转角、俯仰角、偏航角组成的向量,转换矩阵的导数满足:其中ω[i]=[p[i],q[i],r[i]]T为自身固定的角速度向量,p[i],q[i],r[i]分别为x、y、z方向对应的角速度,S是一个矩阵,当***运动为三维运动时,定义为:
当***运动为二维运动时:
建立领航器运动学模型:
xk+1=Fxk+wk
建立距离量测模型:
进一步的,步骤二具体为:
假设对目标状态x的估计为:
根据扩展卡尔曼滤波进行估计,其基本方程为:
距离量测估计为:
量测向量估计为:
量测向量更新为:
状态一步预测为:
量测向量,量测矩阵一步预测为:
进而获得k时刻对目标状态估计的先验和后验密度,即和其中,xk为第k时刻目标状态,为k时刻估计状态,为根据k-1时刻的状态估计对k时刻状态进行预测得到的结果,Pk|k-1为根据k-1时刻的均方误差阵进行预测得到k时刻的均方误差矩阵。
进一步的,步骤三具体为:
当***运动为三维运动时,定义***在三维惯性系下的真实环绕轨迹为:
当***运动为二维运动时,定义***在二维惯性系下的真实环绕轨迹为:
根据路径跟随理念MPF,***理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成,即:
Pd(t)=r(γ[i](t))+q(t),i∈N
其中,q(t)为目标位置信息,r[i](γ)为***环绕目标时的运动信息;
***间的夹角应满足:
进一步的,步骤四具体为:
***实际与理想轨迹之间的位置误差为:
对速度误差eγ求导可得:
为使跟踪误差e到零,设计了以下控制器:
本发明的有益效果:本发明提出了一个完整的路径规划,控制,和估计框架来解决基于距离的多个潜航器进行目标定位和追踪的方法。本发明所提出的方法具有很强的鲁棒性和有效性,对不同的多个***均有很好的效果。本发明具体涉及到多个***进行轨迹规划、利用协同扩展卡尔曼滤波算法(DEKF)对领航器协同定位、以及协同跟踪控制器使***完成轨迹跟踪任务。主要计算了每个***经过的轨迹跟踪曲线形式的轨迹。通过适当地参数化曲线,可以保证如果***精确地跟踪期望的轨迹跟踪曲线,那么目标运动就可以被观察到。同时,***收敛到一个理想的相对编队,在一个明确定义的数学意义上,将获得的距离信息渲染为“最大”,用于目标状态估计。针对多***的情况,提出了一种高效的分布式估计与控制(DEC)策略,用于***以协同的方式定位和跟踪目标。所提出的DEC可确保所有***渐进地对目标状态的估计达成一致,并在目标附近保持所需的几何结构。该策略要求***之间交换的信息非常有限,因此在实际实施中非常有效。
附图说明
图1为二维多***理想轨迹规划示意图;
图2为三个***相互之间的通信拓扑图;
图3为单领航三个***在二维下的目标定位及轨迹跟踪控制仿真图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
本发明首先为多个***进行轨迹规划、其次利用协同扩展卡尔曼滤波算法(DEKF)对领航器协同定位、而后利用协同跟踪控制器使***完成轨迹跟踪任务。包括:
步骤一:建立领航器、***以及两者之间距离量测的模型。
***模型:
所以***的运动学模型是:
其中R(η[i])是从自身坐标系到惯性系的转换矩阵。其中向量这是一个由三个方向欧拉角组成的向量(roll,pitch,yaw)。旋转矩阵的导数满足:其中ω[i]=[p[i],q[i],r[i]]T为自身固定的角速度向量,S是一个矩阵,被定义为
领航器模型:
将领航器看作被跟踪的目标,则代表着可以被追踪的目标轨迹即领航器的轨迹,代表惯性系下目标的速度向量即领航器的速度,即便目标的轨迹是未知的,我们也可以假设目标在缓慢的改变速度,其运动模型可以用形式的准稳态模型来描述即:
xk+1=Fxk+wk (4)
距离量测模型:
步骤二:根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计。
假设对目标状态x的估计为:
根据扩展卡尔曼滤波进行估计,其基本方程为:
距离量测估计:
量测向量估计:
量测向量更新:
状态一步预测:
量测向量,量测矩阵一步预测:
经过上述估计过程就可获得k时刻对目标状态估计的先验和后验密度,即和其中,xk为第k时刻目标状态,为k时刻估计状态,为根据k-1时刻的状态估计对k时刻状态进行预测得到的结果,Pk|k-1为根据k-1时刻的均方误差阵进行预测得到k时刻的均方误差矩阵。
步骤三:根据可观测性进行协同轨迹规划,确定***理想跟踪轨迹。
为了估计目标的距离信息使用Fisher信息矩阵(FIM)作为一种量化方法,它提出了一类最大距离信息的最优轨迹,其中包含***针对目标作环绕运动。因此,定义***在三维惯性系下的真实环绕轨迹为:
对于二维空间,
根据路径跟随理念(MPF),***理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成,即
Pd(t)=r(γ[i](t))+q(t),i∈N (17)
其中,q(t)为目标位置信息,r[i](γ)即为上式所定义的***环绕目标时的运动信息。
对于多个***,为了获得更好的可观测性,***间的夹角应满足一定要求,如图1所示,当有两个***时,每个***到目标的向量之间夹角应为π/2,当***超过3个时,两个相邻***的相对向量与目标所成的角度为2π/N。即:
步骤四:根据步骤二以及步骤三中的目标状态以及跟踪轨迹,设计控制器,完成对目标的跟踪。
***实际与理想轨迹之间的位置误差为:
对速度误差eγ求导可得
所以为了让跟踪误差e到零,设计了以下控制器:
为进一步说明本发明的效果,采用三个***对单目标在二维情况下进行定位及轨迹跟踪控制仿真。其中三个***之间的通信拓扑结构如图2所示;二维情况下目标的轨迹为q=[20sin(0.1t),0.3t]T(m)。其余参数均为量测距离σ=0.5m,轨迹曲线(轨迹跟踪曲线)轨迹跟踪控制器参数ε=[-0.5,0]T,Kp=diag([0.4,0.2]),kγ=500,kc=0.2。EKF算法相关参数为V[i]=10,Q=10-3diag(1,1,0.1,0.1),πi=πj=0.5,Ts=2s。图3是目标在二维情况下沿曲线运动情况下多***仿真过程图。从结果示意图可以看出,***可以很好的完成目标定位及轨迹跟踪任务,***最终沿预定轨迹进行跟踪,而且***具有稳定性。
Claims (5)
1.一种单领航多AUV协同定位及轨迹跟踪控制方法,其特征在于,包括:
步骤一:建立***运动学模型、领航器运动学模型以及两者之间距离量测的模型;
步骤二:根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计;
步骤三:根据可观测性进行协同轨迹规划,确定***理想跟踪轨迹;
步骤四:根据步骤二以及步骤三中的目标状态以及跟踪轨迹,设计控制器,完成对目标的跟踪。
2.根据权利要求1所述的一种单领航多AUV协同定位及轨迹跟踪控制方法,其特征在于:步骤一具体为:
建立***运动学模型:
假设有N个***(N≥1),{I}={xI,yI,zI}为惯性坐标系,为第i个***的自身坐标系,i∈N;代表惯性坐标系中第i个***的坐标;表示第i个***在自身坐标系中的速度向量,则***的运动学模型是:
其中,R(η[i])是从自身坐标系到惯性系的转换矩阵,其中向量是由滚转角、俯仰角、偏航角组成的向量,转换矩阵的导数满足:其中ω[i]=[p[i],q[i],r[i]]T为自身固定的角速度向量,p[i],q[i],r[i]分别为x、y、z方向对应的角速度,S是一个矩阵,当***运动为三维运动时,定义为:
当***运动为二维运动时:
建立领航器运动学模型:
xk+1=Fxk+wk
建立距离量测模型:
3.根据权利要求2所述的一种单领航多AUV协同定位及轨迹跟踪控制方法,其特征在于,步骤二具体为:
假设对目标状态x的估计为:
根据扩展卡尔曼滤波进行估计,其基本方程为:
距离量测估计为:
量测向量估计为:
量测向量更新为:
状态一步预测为:
量测向量,量测矩阵一步预测为:
4.根据权利要求2所述一种单领航多AUV协同定位及轨迹跟踪控制方法,其特征在于:步骤三具体为:
当***运动为三维运动时,定义***在三维惯性系下的真实环绕轨迹为:
当***运动为二维运动时,定义***在二维惯性系下的真实环绕轨迹为:
根据路径跟随理念MPF,***理想的跟踪轨迹为目标运动信息以及环绕目标的运动信息构成,即:
Pd(t)=r(γ[i](t))+q(t),i∈N
其中,q(t)为目标位置信息,r[i](γ)为***环绕目标时的运动信息;
***间的夹角应满足:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210279687.1A CN114840003B (zh) | 2022-03-21 | 一种单领航多auv协同定位及轨迹跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210279687.1A CN114840003B (zh) | 2022-03-21 | 一种单领航多auv协同定位及轨迹跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114840003A true CN114840003A (zh) | 2022-08-02 |
CN114840003B CN114840003B (zh) | 2024-08-02 |
Family
ID=
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109596128A (zh) * | 2019-01-14 | 2019-04-09 | 哈尔滨工程大学 | 一种基于多水听器提高多auv协同定位性能的方法 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
CN112632876A (zh) * | 2020-08-26 | 2021-04-09 | 西北工业大学 | 一种基于dmhe和dmpc的无人船协同目标跟踪控制方法 |
WO2021114888A1 (zh) * | 2019-12-10 | 2021-06-17 | 南京航空航天大学 | 一种双agv协同运载控制***及方法 |
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109596128A (zh) * | 2019-01-14 | 2019-04-09 | 哈尔滨工程大学 | 一种基于多水听器提高多auv协同定位性能的方法 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
WO2021114888A1 (zh) * | 2019-12-10 | 2021-06-17 | 南京航空航天大学 | 一种双agv协同运载控制***及方法 |
CN112632876A (zh) * | 2020-08-26 | 2021-04-09 | 西北工业大学 | 一种基于dmhe和dmpc的无人船协同目标跟踪控制方法 |
Non-Patent Citations (1)
Title |
---|
徐博;李盛新;张奂: "一种基于双模型的低成本多AUV协同定位方法", 中国舰船研究, no. 003, 31 December 2020 (2020-12-31) * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
Liang et al. | Leader-following formation tracking control of mobile robots without direct position measurements | |
CN103759742B (zh) | 基于模糊自适应控制技术的捷联惯导非线性对准方法 | |
CN106950974B (zh) | 欠驱动自主水下航行器的对三维路径进行理解及跟踪控制方法 | |
CN105807789B (zh) | 基于t-s模糊观测器补偿的uuv控制方法 | |
Wang et al. | Design, modeling, control, and experiments for multiple AUVs formation | |
CN109974706A (zh) | 一种基于双运动模型的主从式多auv协同导航方法 | |
CN106444806A (zh) | 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法 | |
Hung et al. | Cooperative distributed estimation and control of multiple autonomous vehicles for range-based underwater target localization and pursuit | |
CN112068440B (zh) | 基于模型预测控制的auv回收对接动力定位控制方法 | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制*** | |
CN113110468A (zh) | 一种应用于欠驱动双桨双舵船舶自主靠泊的控制方法 | |
CN114397899A (zh) | 一种仿生机器鱼三维路径跟踪控制方法及装置 | |
Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
Herlambang et al. | Design of Navigation and Guidance Control System of Mobile Robot with Position Estimation Using Ensemble Kalman Filter (EnKF) and Square Root Ensemble Kalman Filter (SR-EnKF) | |
CN108829132A (zh) | 一种考虑大时滞的多uuv空间机动控制方法 | |
CN112578814B (zh) | 一种用于多自主水下航行器编队直线航迹跟踪控制方法 | |
CN114840003B (zh) | 一种单领航多auv协同定位及轨迹跟踪控制方法 | |
CN114943168B (zh) | 一种水上浮桥组合方法及*** | |
CN114840003A (zh) | 一种单领航多auv协同定位及轨迹跟踪控制方法 | |
CN112945245B (zh) | 基于条件数理论在多auv协同导航***中可观测性分析方法 | |
CN112947068B (zh) | 一种自主水下航行器一体化声学定位与跟踪控制方法 | |
Wang et al. | Dynamic position predicting of underactuated surface vessel with unscented Kalman filter | |
CN115480580A (zh) | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 | |
CN112904719B (zh) | 一种适用于水下机器人位置环形区域跟踪控制方法 |
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 |