CN114019989A - 一种基于pid的水下潜器航迹间接自动控制方法 - Google Patents

一种基于pid的水下潜器航迹间接自动控制方法 Download PDF

Info

Publication number
CN114019989A
CN114019989A CN202111257437.XA CN202111257437A CN114019989A CN 114019989 A CN114019989 A CN 114019989A CN 202111257437 A CN202111257437 A CN 202111257437A CN 114019989 A CN114019989 A CN 114019989A
Authority
CN
China
Prior art keywords
track
course
current
switching
track section
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
Application number
CN202111257437.XA
Other languages
English (en)
Other versions
CN114019989B (zh
Inventor
曾浩然
吕策
付敏飞
彭威
周凌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Ship Development and Design Centre
Original Assignee
China Ship Development and Design Centre
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China Ship Development and Design Centre filed Critical China Ship Development and Design Centre
Priority to CN202111257437.XA priority Critical patent/CN114019989B/zh
Publication of CN114019989A publication Critical patent/CN114019989A/zh
Application granted granted Critical
Publication of CN114019989B publication Critical patent/CN114019989B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

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

本发明公开了一种基于PID的水下潜器航迹间接自动控制方法，属于水下潜器操纵控制技术领域，根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径R；或者，预先设定航迹段切换的切换半径R；在航迹跟踪过程中，根据潜器当前位置和计划航线实时计算潜器距离下一航迹点的距离r，若r<R，则切换航迹；基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，在PID控制律作用下得出指令航向，通过当前航迹段的最大偏转航向对指令航向进行限幅后输出给潜器的航向控制环，其仅根据潜器的实时状态计算所需的指令航向，并发送给航向控制环，而无需对潜器的执行机构进行直接控制。

Description

一种基于PID的水下潜器航迹间接自动控制方法
技术领域
本发明属于水下潜器操纵控制技术领域，更具体地，涉及一种基于PID的水下潜器航迹间接自动控制方法。
背景技术
水下潜器在执行任务中往往要求沿已规划的航线行驶，而受海流扰动及误差积累的影响，在实际过程中，潜器的航向一般与既定航线有所偏差。在有人潜器上，指挥员在一定周期根据导航信息发出航向控制指令或操舵指令，调整潜器航向，使其回归到计划航线上来，航迹控制的好坏也依赖于指挥员的操作经验，这种人工的航迹控制方式一是对指挥员的操作水平提出了较高要求，也依赖于航行经验，二是水下潜器作为水下作业平台，其对自身的控制分散了指挥员的注意力，导致其不能专注于作业或者作战任务；而在无人潜器上，由于通讯受限，岸基人员难以及时地发出航向调整指令，故无人潜器往往要求潜器具有自主的航迹控制功能。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提出了一种基于PID的水下潜器航迹间接自动控制方法，旨在根据潜器的运动状态实时计算出指令航向角，发送给航向控制环，通过控制航向来实现航迹的自动控制。
为实现上述目的，本发明提供了一种基于PID的水下潜器航迹间接自动控制方法，包括：
确定切换半径R，其中，根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径R；或者，预先设定航迹段切换的切换半径R；
在航迹跟踪过程中，根据潜器当前位置和计划航线实时计算潜器距离下一航迹点的距离r，若r<R，则切换当前航迹段，以下一个航迹段为目标进行跟踪；
基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，在PID控制律作用下得出指令航向，通过当前航迹段的最大偏转航向对指令航向进行限幅后输出给潜器的航向控制环，其中，限幅后指令航向不超过当前航迹段的最大偏转航向。
在一些可选的实施方案中，所述根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径R，包括：
计算当前航迹段与下一航迹段的夹角，结合潜器的最小转弯半径，通过做辅助圆内切当前航迹段与下一航迹段，根据几何关系计算出航迹切换半径R。
在一些可选的实施方案中，所述基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，包括：
由
Figure BDA0003324551600000021
得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，其中，S为水下潜器，S1S2为潜器当前航迹段，S位置为(x，y)，S1位置为(x1，y1)，S2位置为(x2，y2)。
在一些可选的实施方案中，所述当前航迹段的最大偏转航向的获取方式为：
以正北向为0度，右偏为正向，左偏为负向，求取当前航迹段与正北向的夹角作为航迹向角度；在当前航迹段做法向辅助线，获得法向辅助线与正北向的夹角作为最大偏转航向。
在一些可选的实施方案中，所述在PID控制律作用下得出指令航向，包括：
将潜器当前位置与当前航迹段的航向偏差法向距离Dis作为PID的输入量，对Dis进行比例P、积分I和微分D的线性加权，输出量为指令航向，其中，航向偏差法向距离Dis的微分信号由跟踪微分器获取。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
本发明提出了一种基于PID的水下潜器航迹间接自动控制方法。该方法能根据当前位置、计划航线计算出指令航向，而无需人工干预，在即使有水流干扰下，也能实现水下潜器的航迹自动控制。本发明可应用于具有航向控制能力的水下潜器，航迹稳定精度可达±10m以内。
附图说明
图1是本发明实施例提供的一种航迹控制方法示意图；
图2是本发明实施例提供的一种航向偏差法向距离及最大偏转航向示意图；
图3是本发明实施例提供的一种航迹切换半径示意图；
图4是本发明实施例提供的一种航迹自适应半径的几何关系图；
图5是本发明实施例提供的一种数字仿真平台图；
图6是本发明实施例提供的一种航迹自动控制数字仿真曲线图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明提供一种水下潜器航迹间接控制方法，可以在原潜器的航向控制环基础上，根据当前位置、计划航线计算出指令航向，而无需人工干预，在即使有水流干扰下，也能实现水下潜器的航迹自动控制。
图1所示为水下潜器航迹控制的原理图，从左往右的流程为：根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径通过几何关系式求取切换半径R，各折线夹角对应于不同的切换半径R，实现切换半径的自适应；切换半径也可设置为常值R，具体模式的选择可由外部设置。在航迹跟踪过程中，根据当前位置和计划航线实时计算潜器距离下一航迹点的距离r，当r<R时，切换航迹段；在航迹段确定的情况下，根据当前位置计算出航向偏差法向距离Dis，在PID控制律作用下得出指令航向，通过限幅判断后输出给潜器的航向控制环。
本发明的水下潜器航迹间接控制方法，包括以下几个主要部分：
(1)采用PID作为反馈控制律，并对输出进行限幅。求取当前位置与当前指令航线的航向偏差法向距离Dis作为PID的输入量。对Dis进行比例P、积分I和微分D的线性加权，输出量为指令航向。其中比例P代表当前偏差的作用量；积分I代表历史偏差的作用量，可有效消除稳态海流所引起的稳态误差；微分D代表未来偏差的作用量，可增加动态过程的阻尼，提高动态性能。PID控制律计算方法如下：
Figure BDA0003324551600000041
本发明对PID控制律的输出量进行了限幅，其最大偏转航向不会超越法向，避免实际航向与计划航线的方向相反。最大偏转航向的计算方法是：以北向为航向、航迹向0°，求取当前航迹段与北向的夹角，即为航迹向角度；在当前航迹段做法向辅助线，获得法向与北向的夹角，即为最大偏转航向。
(2)采用跟踪微分器获取法向距离Dis的微分信号
Figure BDA0003324551600000042
所述的航迹控制方法选用了PID控制律，涉及法向距离Dis的微分信号。本发明采用跟踪微分器获取法向距离的微分信号，跟踪微分器的计算公式如下：
Figure BDA0003324551600000051
其中，Dis(k)为t＝k*T时刻的航迹偏差法向距离，x1(k)为同时刻跟踪值，x2(k)为同时刻微分值。其中，fhan()为最速控制综合函数，k为时刻，T为采样时间，r为速度因子，h为滤波因子。
(3)所述的航迹自动控制方法支持航迹切换半径自定义和自适应。在完成当前航迹段跟踪、需转换至下一航迹段时需进行航迹段的切换，航迹切换具有两种模式：预先设置常值切换半径、自适应切换。在预先设置常值切换半径模式下，设定切换半径为常值R，在潜器与下一个航迹点距离小于R时，开启航迹切换，此时以下一个航迹段为目标进行跟踪；在自适应切换模式下，通过计算当前航迹段与下一航迹段的夹角，结合潜器的最小转弯半径，通过做辅助圆内切两航迹段，根据几何关系计算出航迹切换半径，不同航迹段间的不同夹角对应不同的切换半径，实现航迹切换的自适应。
图2是航向偏差法向距离及最大偏转航向示意图。图中S为水下潜器，航迹段S1S2为其当前航迹段。S、S1、S2的位置记为(x，y)，(x1，y1)、(x2，y2)，则S与S1S2的法向距离Dis计算公式具体为：
Figure BDA0003324551600000052
其中sign为符号函数，用于给法向距离Dis赋予符号。
图2中航向定义为：正北向为0度，右偏为正向，左偏为负向。ψs为潜器当前航向，航向ψcmd为PID控制律输出的指令航向，航向ψmax1为当前航迹段的最大偏转航向，用其对ψcmd进行限幅，确保ψcmd≤ψmax1成立，ψmax1的计算公式具体为：
Figure BDA0003324551600000061
图3是航迹切换半径示意图。水下潜器位于S处时，此时距离下一航迹点S2的距离r>R，潜器仍以S1S2为当前航迹段调整航向，向其靠拢；水下潜器位于S'处时，此时距离S2的距离r'<R，开启航迹段切换，将S2S3确定为当前航迹段并向其靠拢。
所述的航迹自动控制方法中的航迹切换半径R可设为定值，也可为自适应值，计算自适应值的几何关系图如图4所示。其中，Rmin为潜器最小回转半径，具体的计算公式为：
Figure BDA0003324551600000062
其中，θ为当前航迹段与下一航迹段的夹角，即航迹段S1S2与S2S3的夹角，计算方法为：
Figure BDA0003324551600000063
下面以某型潜器为实施例进行仿真验证，潜器最小回转半径为40m。
PID控制律具体为：
Figure BDA0003324551600000064
跟踪微分器具体为：
Figure BDA0003324551600000065
根据公式(1)计算法向距离Dis，由公式(2)计算最大偏转航向ψmax1。
设定既定航迹点分别为S1(0,0)、S2(800,600)、S3(900,1200)、S4(1800,1800)、S5(1500,2400)、S6(700,3000)、S7(900,3600)，共6个航迹段，5个折线夹角，根据公式(3)、(4)可算出夹角θ依次为：136.4°、133.2°、97.2°、153.3°、108.5°；自适应切换半径R依次为：16m、17.3m、35.3m、9.5m、28.8m。
搭建数字仿真平台如图5所示，设定航速6kn，初始航向60°，初始位置(60,60)，加入恒定水流干扰(流速1kn，流向90°，以正北为0°)，采用自适应航迹切换半径，仿真步长0.1s，仿真时间2000s，本实施例中的水下潜器航迹自动控制曲线如图6所示。本实施例中应用了上述的一种基于航向控制的水下潜器航迹自动控制方法，结果表明实现了航迹的自动控制，稳定精度在±10m以内。
需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于PID的水下潜器航迹间接自动控制方法，其特征在于，包括：
确定切换半径R，其中，根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径R；或者，预先设定航迹段切换的切换半径R；
在航迹跟踪过程中，根据潜器当前位置和计划航线实时计算潜器距离下一航迹点的距离r，若r<R，则切换当前航迹段，以下一个航迹段为目标进行跟踪；
基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，在PID控制律作用下得出指令航向，通过当前航迹段的最大偏转航向对指令航向进行限幅后输出给潜器的航向控制环，其中，限幅后指令航向不超过当前航迹段的最大偏转航向。
2.根据权利要求1所述的方法，其特征在于，所述根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径R，包括：
计算当前航迹段与下一航迹段的夹角，结合潜器的最小转弯半径，通过做辅助圆内切当前航迹段与下一航迹段，根据几何关系计算出航迹切换半径R。
3.根据权利要求1或2所述的方法，其特征在于，所述基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，包括：
由
Figure FDA0003324551590000011
得到潜器当前位置与当前航迹段的航向偏差法向距离Dis，其中，S为水下潜器，S1S2为潜器当前航迹段，S位置为(x，y)，S1位置为(x1，y1)，S2位置为(x2，y2)。
4.根据权利要求3所述的方法，其特征在于，所述当前航迹段的最大偏转航向的获取方式为：
以正北向为0度，右偏为正向，左偏为负向，求取当前航迹段与正北向的夹角作为航迹向角度；在当前航迹段做法向辅助线，获得法向辅助线与正北向的夹角作为最大偏转航向。
5.根据权利要求4所述的方法，其特征在于，所述在PID控制律作用下得出指令航向，包括：
将潜器当前位置与当前航迹段的航向偏差法向距离Dis作为PID的输入量，对Dis进行比例P、积分I和微分D的线性加权，输出量为指令航向，其中，航向偏差法向距离Dis的微分信号由跟踪微分器获取。
CN202111257437.XA 2021-10-27 2021-10-27 一种基于pid的水下潜器航迹间接自动控制方法 Active CN114019989B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111257437.XA CN114019989B (zh) 2021-10-27 2021-10-27 一种基于pid的水下潜器航迹间接自动控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111257437.XA CN114019989B (zh) 2021-10-27 2021-10-27 一种基于pid的水下潜器航迹间接自动控制方法

Publications (2)

Publication Number Publication Date
CN114019989A true CN114019989A (zh) 2022-02-08
CN114019989B CN114019989B (zh) 2024-07-12

Family

ID=80058325

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111257437.XA Active CN114019989B (zh) 2021-10-27 2021-10-27 一种基于pid的水下潜器航迹间接自动控制方法

Country Status (1)

Country Link
CN (1) CN114019989B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103576555A (zh) * 2013-11-14 2014-02-12 哈尔滨工程大学 一种动力定位船舶循迹导引控制方法
CN105511464A (zh) * 2015-12-01 2016-04-20 南京长峰航天电子科技有限公司 一种船舶自主航行控制***及控制方法
CN106950959A (zh) * 2017-04-01 2017-07-14 武汉理工大学 新型的船舶视距制导方法
CN108801262A (zh) * 2018-05-31 2018-11-13 青岛中乌特种船舶研究设计院有限公司 一种船舶自动航行控制器航路规划与纠偏修正方法
CN108958252A (zh) * 2018-07-11 2018-12-07 哈尔滨工程大学 一种基于航迹偏差距离的动力浮标航迹控制方法
CN109164797A (zh) * 2018-07-17 2019-01-08 袁乐铮 一种用于船舶智能控制的航迹制导方法及***
CN113126644A (zh) * 2021-06-03 2021-07-16 北京理工大学 基于自适应视线法的无人机三维航迹跟踪方法
CN113515138A (zh) * 2021-06-15 2021-10-19 西安爱生技术集团有限公司 一种固定翼无人机航线重规划方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103576555A (zh) * 2013-11-14 2014-02-12 哈尔滨工程大学 一种动力定位船舶循迹导引控制方法
CN105511464A (zh) * 2015-12-01 2016-04-20 南京长峰航天电子科技有限公司 一种船舶自主航行控制***及控制方法
CN106950959A (zh) * 2017-04-01 2017-07-14 武汉理工大学 新型的船舶视距制导方法
CN108801262A (zh) * 2018-05-31 2018-11-13 青岛中乌特种船舶研究设计院有限公司 一种船舶自动航行控制器航路规划与纠偏修正方法
CN108958252A (zh) * 2018-07-11 2018-12-07 哈尔滨工程大学 一种基于航迹偏差距离的动力浮标航迹控制方法
CN109164797A (zh) * 2018-07-17 2019-01-08 袁乐铮 一种用于船舶智能控制的航迹制导方法及***
CN113126644A (zh) * 2021-06-03 2021-07-16 北京理工大学 基于自适应视线法的无人机三维航迹跟踪方法
CN113515138A (zh) * 2021-06-15 2021-10-19 西安爱生技术集团有限公司 一种固定翼无人机航线重规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
CHEN, CHEN-WEI等: "Modeling and Simulation of an AUV Simulator With Guidance System", IEEE JOURNAL OF OCEANIC ENGINEERING, vol. 38, no. 2, pages 211 - 225, XP011499985, DOI: 10.1109/JOE.2012.2220236 *
李岳明等: "基于改进二次规划算法的X舵智能水下机器人控制分配", 上海交通大学学报, no. 05, pages 88 - 95 *

Also Published As

Publication number Publication date
CN114019989B (zh) 2024-07-12

Similar Documents

Publication Publication Date Title
CN108267955B (zh) 面向无人艇自主靠泊的运动控制方法
CN111487966B (zh) 一种基于航路点的水面无人艇自适应路径跟踪控制方法
Fossen et al. Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces
CN110618686B (zh) 一种基于显式模型预测控制的无人船航迹控制方法
CN111324132B (zh) 一种基于坐标补偿的智能船舶自动靠泊控制方法
CN110609556A (zh) 一种基于los导航法的多无人艇协同控制方法
CN108415423A (zh) 一种高抗扰自适应路径跟随方法及***
CN109917795B (zh) 一种欠驱动无人船集群协同制导结构及设计方法
CN109656142B (zh) 一种无人艇的串级结构无模型自适应制导方法
CN116360260B (zh) 基于触发制导和自更新阈值的asv预设性能控制方法
CN103645736A (zh) 基于非线性h∞逆优化输出反馈控制器的船舶航向运动控制方法
CN108646766A (zh) 一种基于协同控制的无人船安全跟踪方法及跟踪***
CN111930132A (zh) 一种用于水下自主航行器的航迹控制方法
CN114527747A (zh) 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法
CN108958252B (zh) 一种基于航迹偏差距离的动力浮标航迹控制方法
CN108944899A (zh) 一种基于模糊控制的无人驾驶车辆转向盘控制***及方法
CN114967702A (zh) 一种无人艇控制***及路径跟踪方法
CN111650932B (zh) 无人艇折线航迹跟踪控制方法、控制器以及无人艇
CN106339000B (zh) 基于舰载机进舰速度调节的母舰甲板运动补偿策略
CN114019989A (zh) 一种基于pid的水下潜器航迹间接自动控制方法
CN113960994A (zh) 一种多无人艇协同航行的s面自适应控制算法
CN114564015B (zh) 一种拒止环境下的欠驱动无人艇分布式编队控制方法
CN115857483A (zh) 一种基于控制障碍函数模型预测控制算法的无人船避障控制方法
CN115562266A (zh) 基于变参数视线法的无人船航迹控制方法及存储介质
CN114047743A (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