一种基于PID的水下潜器航迹间接自动控制方法与流程

一种基于pid的水下潜器航迹间接自动控制方法
技术领域
1.本发明属于水下潜器操纵控制技术领域，更具体地，涉及一种基于pid的水下潜器航迹间接自动控制方法。


背景技术：

2.水下潜器在执行任务中往往要求沿已规划的航线行驶，而受海流扰动及误差积累的影响，在实际过程中，潜器的航向一般与既定航线有所偏差。在有人潜器上，指挥员在一定周期根据导航信息发出航向控制指令或操舵指令，调整潜器航向，使其回归到计划航线上来，航迹控制的好坏也依赖于指挥员的操作经验，这种人工的航迹控制方式一是对指挥员的操作水平提出了较高要求，也依赖于航行经验，二是水下潜器作为水下作业平台，其对自身的控制分散了指挥员的注意力，导致其不能专注于作业或者作战任务；而在无人潜器上，由于通讯受限，岸基人员难以及时地发出航向调整指令，故无人潜器往往要求潜器具有自主的航迹控制功能。


技术实现要素：

3.针对现有技术的以上缺陷或改进需求，本发明提出了一种基于pid的水下潜器航迹间接自动控制方法，旨在根据潜器的运动状态实时计算出指令航向角，发送给航向控制环，通过控制航向来实现航迹的自动控制。
4.为实现上述目的，本发明提供了一种基于pid的水下潜器航迹间接自动控制方法，包括：
5.确定切换半径r，其中，根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径r；或者，预先设定航迹段切换的切换半径r；
6.在航迹跟踪过程中，根据潜器当前位置和计划航线实时计算潜器距离下一航迹点的距离r，若r《r，则切换当前航迹段，以下一个航迹段为目标进行跟踪；
7.基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离dis，在pid控制律作用下得出指令航向，通过当前航迹段的最大偏转航向对指令航向进行限幅后输出给潜器的航向控制环，其中，限幅后指令航向不超过当前航迹段的最大偏转航向。
8.在一些可选的实施方案中，所述根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径求取航迹段切换的切换半径r，包括：
9.计算当前航迹段与下一航迹段的夹角，结合潜器的最小转弯半径，通过做辅助圆内切当前航迹段与下一航迹段，根据几何关系计算出航迹切换半径r。
10.在一些可选的实施方案中，所述基于当前航迹段中航迹点坐标，得到潜器当前位置与当前航迹段的航向偏差法向距离dis，包括：
11.由得到潜器当前位置与当前航迹段的航向偏差法向距离dis，其中，s为水下潜器，s1s2为潜器当前航迹段，s位置为(x，y)，s1位置为(x1，y1)，s2位置为(x2，y2)。
12.在一些可选的实施方案中，所述当前航迹段的最大偏转航向的获取方式为：
13.以正北向为0度，右偏为正向，左偏为负向，求取当前航迹段与正北向的夹角作为航迹向角度；在当前航迹段做法向辅助线，获得法向辅助线与正北向的夹角作为最大偏转航向。
14.在一些可选的实施方案中，所述在pid控制律作用下得出指令航向，包括：
15.将潜器当前位置与当前航迹段的航向偏差法向距离dis作为pid的输入量，对dis进行比例p、积分i和微分d的线性加权，输出量为指令航向，其中，航向偏差法向距离dis的微分信号由跟踪微分器获取。
16.总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
17.本发明提出了一种基于pid的水下潜器航迹间接自动控制方法。该方法能根据当前位置、计划航线计算出指令航向，而无需人工干预，在即使有水流干扰下，也能实现水下潜器的航迹自动控制。本发明可应用于具有航向控制能力的水下潜器，航迹稳定精度可达
±
10m以内。
附图说明
18.图1是本发明实施例提供的一种航迹控制方法示意图；
19.图2是本发明实施例提供的一种航向偏差法向距离及最大偏转航向示意图；
20.图3是本发明实施例提供的一种航迹切换半径示意图；
21.图4是本发明实施例提供的一种航迹自适应半径的几何关系图；
22.图5是本发明实施例提供的一种数字仿真平台图；
23.图6是本发明实施例提供的一种航迹自动控制数字仿真曲线图。
具体实施方式
24.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
25.本发明提供一种水下潜器航迹间接控制方法，可以在原潜器的航向控制环基础上，根据当前位置、计划航线计算出指令航向，而无需人工干预，在即使有水流干扰下，也能实现水下潜器的航迹自动控制。
26.图1所示为水下潜器航迹控制的原理图，从左往右的流程为：根据计划航线计算相邻航迹段的折线夹角，结合潜器最小转弯半径通过几何关系式求取切换半径r，各折线夹角对应于不同的切换半径r，实现切换半径的自适应；切换半径也可设置为常值r，具体模式的
选择可由外部设置。在航迹跟踪过程中，根据当前位置和计划航线实时计算潜器距离下一航迹点的距离r，当r《r时，切换航迹段；在航迹段确定的情况下，根据当前位置计算出航向偏差法向距离dis，在pid控制律作用下得出指令航向，通过限幅判断后输出给潜器的航向控制环。
27.本发明的水下潜器航迹间接控制方法，包括以下几个主要部分：
28.(1)采用pid作为反馈控制律，并对输出进行限幅。求取当前位置与当前指令航线的航向偏差法向距离dis作为pid的输入量。对dis进行比例p、积分i和微分d的线性加权，输出量为指令航向。其中比例p代表当前偏差的作用量；积分i代表历史偏差的作用量，可有效消除稳态海流所引起的稳态误差；微分d代表未来偏差的作用量，可增加动态过程的阻尼，提高动态性能。pid控制律计算方法如下：
[0029][0030]
本发明对pid控制律的输出量进行了限幅，其最大偏转航向不会超越法向，避免实际航向与计划航线的方向相反。最大偏转航向的计算方法是：以北向为航向、航迹向0
°
，求取当前航迹段与北向的夹角，即为航迹向角度；在当前航迹段做法向辅助线，获得法向与北向的夹角，即为最大偏转航向。
[0031]
(2)采用跟踪微分器获取法向距离dis的微分信号所述的航迹控制方法选用了pid控制律，涉及法向距离dis的微分信号。本发明采用跟踪微分器获取法向距离的微分信号，跟踪微分器的计算公式如下：
[0032][0033]
其中，dis(k)为t＝k*t时刻的航迹偏差法向距离，x1(k)为同时刻跟踪值，x2(k)为同时刻微分值。其中，fhan()为最速控制综合函数，k为时刻，t为采样时间，r为速度因子，h为滤波因子。
[0034]
(3)所述的航迹自动控制方法支持航迹切换半径自定义和自适应。在完成当前航迹段跟踪、需转换至下一航迹段时需进行航迹段的切换，航迹切换具有两种模式：预先设置常值切换半径、自适应切换。在预先设置常值切换半径模式下，设定切换半径为常值r，在潜器与下一个航迹点距离小于r时，开启航迹切换，此时以下一个航迹段为目标进行跟踪；在自适应切换模式下，通过计算当前航迹段与下一航迹段的夹角，结合潜器的最小转弯半径，通过做辅助圆内切两航迹段，根据几何关系计算出航迹切换半径，不同航迹段间的不同夹角对应不同的切换半径，实现航迹切换的自适应。
[0035]
图2是航向偏差法向距离及最大偏转航向示意图。图中s为水下潜器，航迹段s1s2为其当前航迹段。s、s1、s2的位置记为(x，y)，(x1，y1)、(x2，y2)，则s与s1s2的法向距离dis计算公式具体为：
[0036]
[0037]
其中sign为符号函数，用于给法向距离dis赋予符号。
[0038]
图2中航向定义为：正北向为0度，右偏为正向，左偏为负向。ψs为潜器当前航向，航向ψ
cmd
为pid控制律输出的指令航向，航向ψ
max1
为当前航迹段的最大偏转航向，用其对ψ
cmd
进行限幅，确保ψ
cmd
≤ψ
max1
成立，ψ
max1
的计算公式具体为：
[0039][0040]
图3是航迹切换半径示意图。水下潜器位于s处时，此时距离下一航迹点s2的距离r》r，潜器仍以s1s2为当前航迹段调整航向，向其靠拢；水下潜器位于s'处时，此时距离s2的距离r'《r，开启航迹段切换，将s2s3确定为当前航迹段并向其靠拢。
[0041]
所述的航迹自动控制方法中的航迹切换半径r可设为定值，也可为自适应值，计算自适应值的几何关系图如图4所示。其中，r
min
为潜器最小回转半径，具体的计算公式为：
[0042][0043]
其中，θ为当前航迹段与下一航迹段的夹角，即航迹段s1s2与s2s3的夹角，计算方法为：
[0044][0045]
下面以某型潜器为实施例进行仿真验证，潜器最小回转半径为40m。
[0046]
pid控制律具体为：
[0047][0048]
跟踪微分器具体为：
[0049][0050]
根据公式(1)计算法向距离dis，由公式(2)计算最大偏转航向ψ
max1
。
[0051]
设定既定航迹点分别为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。
[0052]
搭建数字仿真平台如图5所示，设定航速6kn，初始航向60
°
，初始位置(60,60)，加入恒定水流干扰(流速1kn，流向90
°
，以正北为0
°
)，采用自适应航迹切换半径，仿真步长0.1s，仿真时间2000s，本实施例中的水下潜器航迹自动控制曲线如图6所示。本实施例中应用了上述的一种基于航向控制的水下潜器航迹自动控制方法，结果表明实现了航迹的自动控制，稳定精度在
±
10m以内。
[0053]
需要指出，根据实施的需要，可将本技术中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，
以实现本发明的目的。
[0054]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。