CN108121338B - 一种usv的航迹闭环控制方法 - Google Patents
一种usv的航迹闭环控制方法 Download PDFInfo
- Publication number
- CN108121338B CN108121338B CN201611076983.2A CN201611076983A CN108121338B CN 108121338 B CN108121338 B CN 108121338B CN 201611076983 A CN201611076983 A CN 201611076983A CN 108121338 B CN108121338 B CN 108121338B
- Authority
- CN
- China
- Prior art keywords
- target
- usv
- course
- distance
- current
- 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 27
- 230000003111 delayed effect Effects 0.000 claims abstract description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 12
- 230000036544 posture Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 239000007921 spray Substances 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 206010044565 Tremor Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 101150097869 hasA gene Proteins 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
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
本发明涉及一种USV的航迹闭环控制方法,设定目标航行路径,且判断位置传感器和姿态传感器是否处于安全工作状态,USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离,如果该距离小于设定距离,则延时等待,根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息,计算目标航向角ψr(k);根据目标航向角ψr(k)、当前航向角ψ(k)和航向角速度Δψ(k),计算目标喷嘴角ωr(k);根据目标喷嘴角ωr(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k),计算输出动作量,进而控制USV动作。本发明采用PID经典控制理论与在线参数辨识并更新技术相结合的方法,适合USV在复杂海洋环境下的非线性时变模型***,实现航迹闭环控制。
Description
技术领域
本发明涉及USV控制技术领域,具体地说是一种USV的航迹闭环控制方法。
背景技术
当今世界的海洋和军事应用中,无人水面艇的发展越来越受到重视。所谓无人水面艇,系指那些有携载平台拖放,靠遥控或自主方式在水面航行的小型无人化、智能化平台。
无人水面艇较之传统水面舰艇具有一些突出的优势:
1.功能齐全,多种使命兼容。
2.小型轻量,反应快速,机动灵活。
3.替代人员完成一些危险的任务。
4.活动区域广,使用成本低,无人水面艇吃水浅,对航道,港口等处水深要求低,扩张其活动范围;
5.受气候影响较小,可全天候执勤。面对难以预知、复杂的海洋环境,无人水面艇能够精确、快速的执行预定使命路径成为无人水面艇发挥效能的最基本保证,是无人水面艇研制工作的基础。
但是目前无人水面艇自身存在吃水浅、重量轻的因素制约,导致控制过程存在航向不稳,航迹不精确等问题。
发明内容
针对现有技术的不足,本发明提供一种USV的航迹闭环控制方法,将PID经典控制理论与在线参数辨识并更新技术相结合,适合USV在复杂海洋环境下的非线性时变模型***,实现航迹闭环控制。
本发明为实现上述目的所采用的技术方案是:
一种USV的航迹闭环控制方法,包括以下步骤:
步骤1:设定目标航行路径,且判断位置传感器和姿态传感器是否处于安全工作状态,如果是,则执行步骤2,否则报警;
步骤2:USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离,如果该距离小于设定距离,则延时等待后重新执行步骤2,否则执行步骤3;
步骤3:根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息,计算目标航向角ψr(k);
步骤4:根据目标航向角ψr(k)、当前航向角ψ(k)和航向角速度Δψ(k),计算目标喷嘴角ωr(k);
步骤5:根据目标喷嘴角ωr(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k),计算输出动作量,进而控制USV动作,返回步骤2。
所述偏离预定航线的距离的计算过程为:
ε2(k)=(x(k)-x')2+(y(k)-y')2
其中,ε(k)为偏离预定航线的距离;x(k),y(k)为USV当前位置坐标;x’,y’为USV当前位置到目标航行路径的垂线段交点坐标;(xi,yi),(xi+1,yi+1)为当前目标航行路径的两个端点坐标。
所述目标航向角ψr(k)的计算过程为:
期望航向ψr是垂足点(x’,y’)处的切线方向,则有:
ψr(k)=arctan((yi+1-yi)/(xi+1-xi))
其中,(xi,yi),(xi+1,yi+1)为当前目标航行路径的两个端点坐标。
所述目标喷嘴角ωr(k)的计算过程为:
其中,ψr(k)为目标喷嘴角,ψr(k)为目标航向角,ψ(k)为当前航向角,Δψ(k)为航向角速度,KP1为1.05,KI1为0.011,KD1为-4.1。
所述输出动作量的计算过程为:
其中,ACT为输出动作量,ωr(k)为目标喷嘴角,ω(k)为当前喷嘴角,Δω(k)为喷嘴角速度,KP2为2.1,KI2为0.03,KD2为-2.5。
本发明具有以下有益效果及优点:
1.本发明对无人水面艇的航迹闭环控制效果明显,收敛速度快。
2.本发明计算过程简单,鲁棒性、适应性强。
3.本发明移植方便,可适用到其它无人水面艇。
附图说明
图1是本发明的方法流程图;
图2是本发明的闭环控制示意图;
图3是本发明的运动学示意图。
具体实施方式
下面结合附图及实施例对本发明做进一步的详细说明。
如图1所示为本发明的方法流程图。
步骤1:操作者操作人机交互计算机将航行路径下载到控制计算机中的硬盘中;
步骤2:USV等待位置传感器(校准源)、姿态传感器(惯组)进入准备状态且无故障上报后,USV开始进行航行任务。
步骤3:USV实时计算偏离预定航线的距离,若小于设定值,则延时等待后重复步骤3。若大于设定值,进入步骤4。
步骤4:控制计算机由预定路径、当前偏航距离、位置、姿态等信息,计算目标航向角ψr(k)。
步骤5:由目标航向角ψr(k)、当前航向角ψ(k)、航向角速度Δψ(k),计算目标喷嘴角ωr(k)。
步骤6:由目标喷嘴角ωr(k)、当前喷嘴角ω(k)、喷嘴角速度Δω(k),得出执行动作后返回步骤3。
如图2所示为本发明的闭环控制示意图。
水面无人艇采用惯性组合导航设备作为姿态信息输入。采用GPS校准源作为位置信息输入。
无人艇航迹偏差计算方法:
设定当前航向ψ(k),可以通过搭载在USV的设备采集得到,期望航向ψr(k)、期望航迹偏差εr(k)由航迹控制单元给出。实际的航迹偏差ε(k)通过计算获得,其实是USV当前位置(x,y)到规划航线的最近距离,即(x,y)到规划航线的垂线段长度:
ε2(k)=(x(k)-x')2+(y(k)-y')2
在知道规划的航线的情况下,垂足点(x’,y’)的坐标可有以下求出
当知道水面无人艇的(x(k),y(k)),则上式是关于x的高阶非线性方程,根就是求得的x’,而。期望航向ψr是垂足点(x’,y’)处的切线方向,有如图3所示。用若干个直线段代替规划的航线,根据生成的路径序列执行情况,可知USV当前位置属于直线航迹段i上,然后利用该航迹段的两个端点坐标(xi,yi),(xi+1,yi+1)就能够得到垂足点(x’,y’)的坐标,从而计算得到航迹偏差ε(k)和期望航向ψr(k)如下
ψr(k)=arctan((yi+1-yi)/(xi+1-xi))
输出动作量为:
式中ACT为最后输出动作量,ωr(k)为目标喷嘴角,ω(k)为当前喷嘴角,Δω(k)为当前喷嘴角速度,KP2为2.1,KI2为0.03,KD2为-2.5。
USV的控制参数可在线修改、调整。
本发明的硬件要求是一台USV,艉部配置柴油机,柴油机连接翻斗、喷泵,油门控制柴油机的转速。在USV设计完成后,首先进行水池水动力试验,获得USV的水动力系数。然后按照本发明方法设计USV的航迹闭环控制器。最后,将使用本发明方法的USV进行航行试验,验证控制方法的正确性。
Claims (2)
1.一种USV的航迹闭环控制方法,其特征在于,包括以下步骤:
步骤1:设定目标航行路径,且判断位置传感器和姿态传感器是否处于安全工作状态,如果是,则执行步骤2,否则报警;
步骤2:USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离,如果该距离小于设定距离,则延时等待后重新执行步骤2,否则执行步骤3;
步骤3:根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息,计算目标航向角ψr(k);所述目标航向角ψr(k)的计算过程为:
期望航向ψr是垂足点(x’,y’)处的切线方向,则有:
ψr(k)=arctan((yi+1-yi)/(xi+1-xi))
其中,用若干个直线段代替规划的航线,(xi,yi),(xi+1,yi+1)为当前目标航行路径航迹段i的两个端点坐标,为目标航行路径;
步骤4:根据目标航向角ψr(k)、当前航向角ψ(k)和航向角速度Δψ(k),计算目标喷嘴角ωr(k);所述目标喷嘴角ωr(k)的计算过程为:
其中,ωr(k)为目标喷嘴角,ψr(k)为目标航向角,ψ(k)为当前航向角,Δψ(k)为航向角速度,KP1为1.05,KI1为0.011,KD1为-4.1;
步骤5:根据目标喷嘴角ωr(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k),计算输出动作量,所述输出动作量的计算过程为:
其中,ACT为输出动作量,ωr(k)为目标喷嘴角,ω(k)为当前喷嘴角,Δω(k)为喷嘴角速度,KP2为2.1,KI2为0.03,KD2为-2.5;
进而控制USV动作,返回步骤2。
2.根据权利要求1所述的USV的航迹闭环控制方法,其特征在于:所述偏离预定航线的距离的计算过程为:
ε2(k)=(x(k)-x')2+(y(k)-y')2
其中,ε(k)为偏离预定航线的距离;x(k),y(k)为USV当前位置坐标;x’,y’为USV当前位置到目标航行路径的垂线段交点坐标。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611076983.2A CN108121338B (zh) | 2016-11-30 | 2016-11-30 | 一种usv的航迹闭环控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611076983.2A CN108121338B (zh) | 2016-11-30 | 2016-11-30 | 一种usv的航迹闭环控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108121338A CN108121338A (zh) | 2018-06-05 |
CN108121338B true CN108121338B (zh) | 2019-12-03 |
Family
ID=62226085
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201611076983.2A Active CN108121338B (zh) | 2016-11-30 | 2016-11-30 | 一种usv的航迹闭环控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108121338B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108958252B (zh) * | 2018-07-11 | 2021-05-14 | 哈尔滨工程大学 | 一种基于航迹偏差距离的动力浮标航迹控制方法 |
CN109612454A (zh) * | 2018-12-03 | 2019-04-12 | 华东师范大学 | 无人船测绘路径校准方法、***及无人船 |
CN109752957B (zh) * | 2019-01-11 | 2021-07-30 | 大连海事大学 | 一种无人艇的制导指令调节器结构及设计方法 |
CN111309036A (zh) * | 2020-05-14 | 2020-06-19 | 江苏博人文化科技有限公司 | 一种基于自动驾驶船的空间信息测量*** |
CN112068565A (zh) * | 2020-09-10 | 2020-12-11 | 四方智能(武汉)控制技术有限公司 | 一种结构化环境中的无人船自主导航方法和*** |
CN114020032A (zh) * | 2021-11-25 | 2022-02-08 | 江苏科技大学 | 一种基于人工势场法和退火算法的无人机路径规划方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102323820A (zh) * | 2011-04-19 | 2012-01-18 | 江苏科技大学 | 一种智能舵***及其控制方法 |
CN103407443A (zh) * | 2013-07-11 | 2013-11-27 | 哈尔滨工程大学 | 一种气垫船航行安全性的协调控制方法 |
CN104049639A (zh) * | 2014-06-24 | 2014-09-17 | 上海大学 | 一种基于支持向量回归机的无人艇抗浪涌控制装置和方法 |
JP2015055906A (ja) * | 2013-09-10 | 2015-03-23 | 株式会社日立産機システム | 移動体の走行制御手段に対して制御指令を出力する位置検出装置及び移動体システム |
CN105549611A (zh) * | 2014-10-30 | 2016-05-04 | 中国科学院沈阳自动化研究所 | 自主水下机器人的海洋环境自识别的航迹精确跟踪方法 |
CN105947187A (zh) * | 2016-05-16 | 2016-09-21 | 西北工业大学 | 一种垂直起降飞行器的姿态控制装置及控制方法 |
-
2016
- 2016-11-30 CN CN201611076983.2A patent/CN108121338B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102323820A (zh) * | 2011-04-19 | 2012-01-18 | 江苏科技大学 | 一种智能舵***及其控制方法 |
CN103407443A (zh) * | 2013-07-11 | 2013-11-27 | 哈尔滨工程大学 | 一种气垫船航行安全性的协调控制方法 |
JP2015055906A (ja) * | 2013-09-10 | 2015-03-23 | 株式会社日立産機システム | 移動体の走行制御手段に対して制御指令を出力する位置検出装置及び移動体システム |
CN104049639A (zh) * | 2014-06-24 | 2014-09-17 | 上海大学 | 一种基于支持向量回归机的无人艇抗浪涌控制装置和方法 |
CN105549611A (zh) * | 2014-10-30 | 2016-05-04 | 中国科学院沈阳自动化研究所 | 自主水下机器人的海洋环境自识别的航迹精确跟踪方法 |
CN105947187A (zh) * | 2016-05-16 | 2016-09-21 | 西北工业大学 | 一种垂直起降飞行器的姿态控制装置及控制方法 |
Non-Patent Citations (2)
Title |
---|
Fractional-Order PID Controller of USV Course-Keeping using Hybrid GA-PSO Algorithm;Guangyu Li,et al;《2015 8th International Symposium on Computational Intelligence and Design》;20151231;506-509页 * |
欠驱动无人艇直线航迹跟踪的反步自适应动态滑模控制;廖煜雷,等;《高技术通讯》;20131231;第23卷(第6期);598-604页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108121338A (zh) | 2018-06-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108121338B (zh) | 一种usv的航迹闭环控制方法 | |
CN108416152B (zh) | 基于电子海图的无人艇蚁群能耗最优全局路径规划方法 | |
CN107608347B (zh) | 一种分布式控制无人艇集群围追跟踪方法 | |
CN108445892A (zh) | 一种欠驱动无人船编队控制器结构及设计方法 | |
CN106773713A (zh) | 针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法 | |
CN110362089A (zh) | 一种基于深度强化学习和遗传算法的无人船自主导航的方法 | |
CN106444838A (zh) | 一种自主水下航行器的精确航路跟踪控制方法 | |
CN108803612A (zh) | 一种海流影响下的无人巡检船直线航迹跟踪实现方法 | |
CN103955218A (zh) | 一种基于非线性控制理论的无人艇轨迹跟踪控制装置及方法 | |
CN107024863A (zh) | 一种避免微分***的uuv轨迹跟踪控制方法 | |
CN108594807B (zh) | 一种无人水面艇折线路径跟踪控制***及方法 | |
CN106292293B (zh) | 一种固定翼无人舰载机的自适应自动着舰引导控制*** | |
CN106227223A (zh) | 一种基于动态滑模控制的uuv轨迹跟踪方法 | |
CN112363517B (zh) | 一种水下滑翔机的编队控制方法 | |
CN104199303B (zh) | 一种基于矢量场制导的平流层卫星平面路径跟踪控制方法 | |
Ihle et al. | Nonlinear formation control of marine craft with experimental results | |
CN104765368A (zh) | 一种基于模糊自适应算法的无人艇航向航速协同控制方法 | |
CN108267953A (zh) | 一种基于领航者-跟踪者水下机器人位置跟踪方法 | |
CN106708069A (zh) | 水下移动作业机器人的协调规划与控制方法 | |
CN103529842A (zh) | 一种基于渐近导引的船舶定位控制方法 | |
Dukan et al. | Sea floor geometry approximation and altitude control of ROVs | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制*** | |
CN109213203A (zh) | 一种基于预见控制的舰载机自动着舰控制方法 | |
CN106444359A (zh) | 一种喷水推进无人艇自主区域保持的仿人智能控制方法 | |
CN113359737A (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 | ||
GR01 | Patent grant |