CN108490968B - 一种基于反馈融合结构的自主水下航行器控制行为融合方法 - Google Patents
一种基于反馈融合结构的自主水下航行器控制行为融合方法 Download PDFInfo
- Publication number
- CN108490968B CN108490968B CN201810221654.5A CN201810221654A CN108490968B CN 108490968 B CN108490968 B CN 108490968B CN 201810221654 A CN201810221654 A CN 201810221654A CN 108490968 B CN108490968 B CN 108490968B
- Authority
- CN
- China
- Prior art keywords
- auv
- behavior
- angle
- obstacle
- psi
- 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
- 230000004927 fusion Effects 0.000 title claims abstract description 14
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 6
- 238000000034 method Methods 0.000 claims abstract description 13
- 239000013598 vector Substances 0.000 claims description 13
- 238000001514 detection method Methods 0.000 claims description 5
- 230000004888 barrier function Effects 0.000 claims description 2
- 230000010354 integration Effects 0.000 abstract 2
- 230000006399 behavior Effects 0.000 description 41
- 230000009471 action Effects 0.000 description 5
- 238000004088 simulation Methods 0.000 description 2
- 230000001960 triggered effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 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/10—Simultaneous control of position or course in three dimensions
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
技术领域
本发明涉及水下自动控制技术领域,具体为一种基于反馈融合结构的自主水下航行器控制行为融合方法。
背景技术
对于自主水下航行器(AUV)的单个行为来说,每一个基本行为是由控制系统和触发器两部分组成,其中控制系统能使被控对象按照特定方式运行,而触发器则决定了机器人应该合适进行相关的操作。为了能够同时实现多个目标,有时候被控对象需要执行很多的基本行为。当某个时刻只要仅有一个行为触发,系统都能够比较平稳地运行。然而当两个甚至更多的行为同时触发,并且每个行为都需要执行不同的操作时,控制系统将使用一定的仲裁方法,来对输出结果进行融合,最后产生一个综合的输出结果。
建立AUV控制系统行为层的过程时,从AUV的性质和使命开始分析,可以确定控制系统行为层包含的具体行为,有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。
通过分析可以发现,轨迹跟踪行为和避障行为的作用是相互矛盾的,如果自主航行器航路任务上存在障碍,那么避障行为起作用的时候必然会造成AUV偏离任务航行轨迹,这个时候轨迹跟踪行为必然会起相反的作用将AUV拉回任务轨迹。而航行器需要的逻辑是,既能够保证航行器在距离障碍的安全距离之外,又能够偏离任务轨迹最少。
在基于行为体系结构的基础上,根据多个行为产生动作的不同仲裁方法,研究人员又提出了更详细的分类方法。Carreras M总结出了在AUV中应用的四种典型的基于行为的结构体系:包含结构,融合结构,动态选择结构和带反馈的融合结构。
本发明就是一种带反馈的融合结构,是综合上述结构的优点选择的一种比较好的结构,带反馈的融合结构是根据环境反馈,修改融合各个行为输出结果加权和的每一个权值,得到一个可预测的期望结果,降低了不可预测的错误出现的几率,同时曲线的曲折也比较少,可以得到连续的曲线。
发明内容
行为层的行为有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。其中深度控制行为和速度控制行为分别输出参考俯仰角和参考速度,它们的结果不需要进行融合,而另外三个行为的输出是相互关联的,它们之间需要融合得到总的输出结果ψ(参考航向角):
其中ψref是视线导引航向角,ψΔ是轨迹跟踪旋转角,是避障旋转角,f1,f2为调整系数。其中视线导引行为是主要的导引方法,轨迹跟踪行为和避障行为只是对视线导引行为做一个修正,因此直接设ψref的系数等于1,动态融合主要是调整f1和f2的关系。
为实现上述目的,本发明的技术方案为:
所述一种基于反馈融合结构的自主水下航行器控制行为融合方法,其特征在于:包括以下步骤:
步骤1:通过AUV上的前视声呐测得障碍物,并获得障碍物在AUV载体系的坐标;
其中kb为避障比例系数,
计算得到,其中kf为比例系数,I为声呐总波束数量,di为第i个声呐波束探测到的距离,d0为AUV的安全距离,dm为声呐的最大探测距离,ψi为探测波束在AUV载体系的视线角,j为虚数符号;ψF为与AUV载体系ox轴的夹角;
步骤3:计算轨迹跟踪误差
ε(t)=S(t)j+1tan(δp(t))
轨迹跟踪误差角δp(t)=ψtrk(j+1)-ψ(j+1),而
ψtrk(j+1)=-arctan2(Yj+1-Yj,Xj+1-Xj),ψ(j+1)=-arctan2(Yj+1-Y,Xj+1-X);
步骤4:根据步骤3得到的轨迹跟踪误差,通过公式
ψΔ=kpe·ε(t)+kie∫ε(t)
计算轨迹跟踪旋转角ψΔ,其中kpe和kie分别为比例系数和积分系数;
步骤5:根据AUV当前障碍的中的最短障碍距离dmin,按照公式
调整系数f1和f2,其中k1,k2,d1,d2,Rs为设定参数,且设定参数满足当dmin≥100时,有f1>f2,当dmin<100之后,有f1<f2;
有益效果
本发明既能够保证航行器在距离障碍的安全距离之外,又能够偏离任务轨迹最少,融合结构刚好满足这个需求。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解,其中:
图1:轨迹跟踪模型示意图;
图2:行为融合仿真结果图。
具体实施方式
下面详细描述本发明的实施例,所述实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。
本实施例中的基于反馈融合结构的自主水下航行器控制行为融合方法包括以下步骤:
步骤1:通过AUV上的前视声呐测得障碍物,并获得障碍物在AUV载体系的坐标。
用势场法计算AUV受到的斥力的合力:
其中kf为比例系数,I为声呐总波束数量,di为第i个声呐波束探测到的距离,d0为AUV的安全距离,dm为声呐的最大探测距离,ψi为探测波束在AUV载体系的视线角,j为虚数符号;
在ox轴上的投影为:
其中kb为避障比例系数。
步骤3:计算轨迹跟踪误差。
定义轨道跟踪误差(CTE:Cross Track Error)ε(t)为需要控制到最小的轨迹跟踪误差。
ε(t)=S(t)j+1tan(δp(t))
轨迹跟踪误差角δp(t)=ψtrk(j+1)-ψ(j+1),而
ψtrk(j+1)=-arctan2(Yj+1-Yj,Xj+1-Xj),ψ(j+1)=-arctan2(Yj+1-Y,Xj+1-X)。
步骤4:结合AUV导引弹道中的变提前角法,使用ε(t)计算轨迹跟踪旋转角ψΔ来修正ψref以达到消除ε(t)的目的。根据步骤3得到的轨迹跟踪误差,使用PI控制方法通过公式
ψΔ=kpe·ε(t)+kie∫ε(t)
计算轨迹跟踪旋转角ψΔ,其中kpe和kie分别为比例系数和积分系数;
步骤5:根据AUV当前障碍的中的最短障碍距离dmin,按照公式
计算调整系数f1和f2,其中k1,k2,d1,d2,Rs为设定参数。
在实际中将AUV靠近障碍的距离分为四个阶段:可见(Sight),接近(Reach),亲近(Close),危险(Danger),则有
所以设定参数k1,k2,d1,d2,Rs满足但dmin≥100时,有f1>f2,此时轨迹跟踪行为起主要作用;当dmin<100之后,有f1<f2,避障行为开始起作用;当dmin≤80之后,f1远小于f2,避障行为起主要作用,AUV将绕开障碍,直到前视声呐探测到没有障碍为止。
下面以TH50I-AUV预编程型AUV为具体实施例描述本发明:
设定AUV初始点为wp0(0,0,0),初始位置为在水面的零点。
设定三个任务航路点:
wp1(2,-5,600),表示在到达目标点的路径上,要求航行器的深度为水下5米,目标点的坐标为(2,600)。
wp2(800,-5,602),要求航行器航行的深度为水下5米,目标点的坐标为(800,602)。
wp3(790,-10,260),要求航行器航行的深度为水下10米,目标点的坐标为(790,260)。
三个障碍的坐标分别为:
O1(1,-5,280),直径为25米,
O2(300,-5,605),直径为30米,
O3(785,-10,450),直径为20米。
得到一组航行结果。
对比仿真结果,在最基本的航路点导引上,航行器能够按照设定航路点完成任务,对轨迹的跟随方面也能够达到要求。对于分别处在航线正中间、偏左和偏右的三个障碍都能够做到绕开并继续航行。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (1)
1.一种基于反馈融合结构的自主水下航行器控制行为融合方法,其特征在于:包括以下步骤:
步骤1:通过AUV上的前视声呐测得障碍物,并获得障碍物在AUV载体系的坐标;
其中kb为避障比例系数,
计算得到,其中kf为比例系数,I为声呐总波束数量,di为第i个声呐波束探测到的距离,d0为AUV的安全距离,dm为声呐的最大探测距离,ψi为探测波束在AUV载体系的视线角,j为虚数符号;ψF为与AUV载体系ox轴的夹角;
步骤3:计算轨迹跟踪误差
ε(t)=S(t)j+1tan(δp(t))
轨迹跟踪误差角δp(t)=ψtrk(j+1)-ψ(j+1),而
ψtrk(j+1)=-arctan2(Yj+1-Yj,Xj+1-Xj),ψ(j+1)=-arctan2(Yj+1-Y,Xj+1-X);
步骤4:根据步骤3得到的轨迹跟踪误差,通过公式
ψΔ=kpe·ε(t)+kie∫ε(t)
计算轨迹跟踪旋转角ψΔ,其中kpe和kie分别为比例系数和积分系数;
步骤5:根据AUV当前障碍的中的最短障碍距离dmin,按照公式
调整系数f1和f2,其中k1,k2,d1,d2,Rs为设定参数,且设定参数满足当dmin≥100时,有f1>f2,当dmin<100之后,有f1<f2;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810221654.5A CN108490968B (zh) | 2018-03-17 | 2018-03-17 | 一种基于反馈融合结构的自主水下航行器控制行为融合方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810221654.5A CN108490968B (zh) | 2018-03-17 | 2018-03-17 | 一种基于反馈融合结构的自主水下航行器控制行为融合方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108490968A CN108490968A (zh) | 2018-09-04 |
CN108490968B true CN108490968B (zh) | 2020-10-27 |
Family
ID=63339686
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810221654.5A Active CN108490968B (zh) | 2018-03-17 | 2018-03-17 | 一种基于反馈融合结构的自主水下航行器控制行为融合方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108490968B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109543135B (zh) * | 2018-11-09 | 2021-01-05 | 西北工业大学 | 一种基于信息最大化的多auv协同同时导引方法 |
CN110456357B (zh) * | 2019-08-27 | 2023-04-07 | 吉林大学 | 一种导航定位方法、装置、设备及介质 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2009288930A (ja) * | 2008-05-28 | 2009-12-10 | Murata Mach Ltd | 自律移動体及びその移動制御方法 |
CN100568144C (zh) * | 2008-09-04 | 2009-12-09 | 湖南大学 | 一种未知环境下移动机器人多行为融合自动导航方法 |
CN101713999B (zh) * | 2009-11-18 | 2011-09-07 | 北京矿冶研究总院 | 地下自主铲运机的导航控制方法 |
CN102231082B (zh) * | 2011-04-08 | 2013-06-12 | 中国船舶重工集团公司第七○二研究所 | 基于迷你声纳的水下目标探测与auv自动避碰方法及其系统 |
CN102621988A (zh) * | 2012-04-10 | 2012-08-01 | 西北工业大学 | 一种水下航行器的避障方法 |
CN104020776B (zh) * | 2014-06-13 | 2016-08-24 | 哈尔滨工程大学 | Uuv对拥有多个禁航区的运动母船的跟踪方法 |
-
2018
- 2018-03-17 CN CN201810221654.5A patent/CN108490968B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN108490968A (zh) | 2018-09-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US7228227B2 (en) | Bezier curve flightpath guidance using moving waypoints | |
Zhang et al. | Novel DVS guidance and path-following control for underactuated ships in presence of multiple static and moving obstacles | |
KR102072356B1 (ko) | 차선 유지 제어 장치 및 방법 | |
US7957858B1 (en) | Method for determining projected obstacle areas for moving obstacles | |
US10437261B2 (en) | Formation flying method and system | |
Curry et al. | L+ 2, an improved line of sight guidance law for UAVs | |
EP2960743B1 (en) | Flight vehicle autopilot | |
CN113126644B (zh) | 基于自适应视线法的无人机三维航迹跟踪方法 | |
US10417920B2 (en) | Method and system for aiding landing of an aircraft | |
CN110546459A (zh) | 具有数据融合的机器人跟踪导航 | |
CN108490968B (zh) | 一种基于反馈融合结构的自主水下航行器控制行为融合方法 | |
JP4644522B2 (ja) | 小型無人ヘリコプタの自律飛行制御装置及び自律飛行制御方法 | |
KR102565482B1 (ko) | 차량의 측위 장치 및 그 방법 | |
CN110687931B (zh) | 一种方位角姿态与前置导引切换的一体化机动导引方法 | |
Slegers et al. | Terminal guidance of autonomous parafoils in high wind-to-airspeed ratios | |
CN109213203B (zh) | 一种基于预见控制的舰载机自动着舰控制方法 | |
US20190250628A1 (en) | Positioning System for a Mobile Unit, Vehicle and Method for Operating a Positioning System | |
Serigstad et al. | Hybrid collision avoidance for autonomous surface vehicles | |
KR102156852B1 (ko) | 비행체의 지향각 제한을 고려한 충돌각 제어 장치 및 그 방법 | |
KR20170004164A (ko) | 무인항공기 기반의 선박 항해 방법 및 시스템 | |
CN109613918A (zh) | 一种高精度轨迹跟踪控制方法 | |
Gautam et al. | Autonomous quadrotor landing using vision and pursuit guidance | |
CN113110504A (zh) | 一种基于强化学习和视线法的无人艇路径跟踪方法 | |
Wang et al. | Vector field-based integral LOS path following and target tracking for underactuated unmanned surface vehicle | |
EP2442201A2 (en) | Formation flying method and system |
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 |