CN106403957A - 一种全垫升气垫船航路点导引控制方法 - Google Patents

Abstract

本发明提供的是一种全垫升气垫船航路点导引控制方法。(1)测得气垫船当前的位置信息，船舶的艏向、回转率、横摇姿态信息；(2)结合气垫船的当前位置和目标位置，给定航路点；(3)LOS导引系统综合航路点坐标(x_k,y_k)和气垫船的当前位置(x(t),y(t))，计算出航迹跟踪的导引点P_LOS(x_los,y_los)；(4)根据气垫船的当前位置与导引点坐标计算出期望艏向角ψ_d，并与气垫船当前艏向角ψ比较，将二者之差Δψ＝ψ_d‑ψ作为PID控制器的输入；(5)PID控制器控制气垫船向着导引点航行；(6)切换。本发明实现了全垫升气垫船的航迹控制，可以减轻驾驶人员的工作强度和精神负担，提高气垫船航行的稳定性和安全性。

Description

一种全垫升气垫船航路点导引控制方法

技术领域

本发明涉及的是一种气垫船的控制方法，具体地说是一种全垫升气垫船的航路点导引控制方法。

背景技术

现阶段我国的全垫升气垫船主要是人工操作，而其特有操作面众多且控制复杂，要求驾驶员始终精神集中，导致极大的精神压力。同时，特殊的航行机理导致全垫升气垫船的稳定性和操纵性较差，航向和姿态较难控制，容易发生偏航与侧漂，因此采用驾驶员人工操控方式时，不易得到良好的控制品质。航路点导引将较为复杂的路径跟踪等效为对导引点的航向跟踪。其在飞行器领域以及机器人移动方面有着不俗的表现，取得了良好的控制效果。而在国内外的公开文献中，未见有将此方法应用到全垫升气垫船控制方法中的报道。

发明内容

本发明的目的在于提供一种能实现全垫升气垫船的航迹控制，减轻驾驶人员的工作强度和精神负担，提高气垫船航行的稳定性和安全性的全垫升气垫船航路点导引控制方法。

本发明的目的是这样实现的：

步骤一：通过DGPS、雷达组成的位置参考系统测得气垫船当前的位置信息，通过姿态传感器，电罗经组成的姿态传感系统测得船舶的艏向、回转率、横摇姿态信息；

步骤二：结合气垫船的当前位置和目标位置，给定航路点；

步骤三：LOS导引系统综合航路点坐标(x_k,y_k)和气垫船的当前位置(x(t),y(t))，计算出航迹跟踪的导引点P_LOS(x_los,y_los)；

步骤四：根据气垫船的当前位置与导引点坐标计算出期望艏向角ψ_d，并与气垫船当前艏向角ψ比较，将二者之差Δψ＝ψ_d-ψ作为PID控制器的输入；

步骤五：PID控制器结合海风、海浪环境干扰控制气垫船向着导引点航行；同时，气垫船实时将当前的位置信息反馈给LOS导引系统，将当前的艏向角反馈给步骤五的艏向角比较环节与期望艏向角进行比较；

步骤六：在航迹跟踪过程中，气垫船在同一时间只能对一段期望路径进行跟踪，根据两段期望路径的夹角大小，计算出切换半径与切换速度，以当前期望路径末端航路点为圆心，以切换半径为半径画圆，当气垫船进入圆的范围内就切换为对下一段期望路径的跟踪，同时在圆内以计算出的切换速度进行回转。

本发明提供了一种航路点导引控制方法，它实现了全垫升气垫船的航迹控制，可以减轻驾驶人员的工作强度和精神负担，提高气垫船航行的稳定性和安全性。本发明的主要技术特征体现在：

1、气垫船位置及姿态的获取

通过DGPS，雷达等组成的位置参考系统测得气垫船当前的位置信息，通过姿态传感器，电罗经等组成的姿态传感系统测得船舶的艏向、回转率、横摇等姿态信息。对获取的信息进行滤波处理，时空对准，得到精准的气垫船位置及姿态信息。

2、导引点及期望艏向角的计算

给定几个航路点，并定义连接这些点的连续线段为参考路径。结合气垫船的实时位置与参考路径，根据实际情况，计算出导引点的位置，从而进一步得到期望艏向角。导引点并不是固定不变的，而是随着气垫船的实时位置移动而移动。气垫船在对导引点进行航向跟踪的同时，艏向角逐渐变化到参考路径倾角的大小，船体也逐渐航行到参考路径上，从而实现了路径跟踪。达到了航路点导引的目的。

3、对参考路径跟踪的切换

由于全垫升气垫船没有水下回转装置，其在过弯时只能走一段弧线，不能原地回转，因此在导引过程中需要设计切换策略，即快到达一个航路点时需要切换到对下一段参考路径的导引。切换过程中根据两段期望路径的夹角选择切换半径和回转速度，以保证平滑过弯。

为了验证本发明的效果，对所设计的基于全垫升气垫船的航路点导引控制方法进行仿真验证。以某型全垫升气垫船为控制对象进行仿真。采用PID控制器，将期望艏向与当前航行艏向的差值作为控制器的输入。在仿真实验前，设定航路点及一定的海洋环境干扰条件。最后，绘制出气垫船航迹变化曲线，以及艏向角、横倾角、横倾角速度、横向速度的变化曲线，检验该控制方法的正确性及有效性。经验证本发明的效果主要体现在：

1、本发明引入了航路点导引的控制方法，针对稳定性、操纵性较差的全垫升气垫船能够满足其实现航迹跟踪的要求。从而减轻了驾驶人员的工作强度和精神负担，提高气垫船航行的稳定性和安全性。

2、本发明对获取的气垫船位置、姿态信息进行了滤波处理及时空对准，从而得到了更为精准的信息。

3、本发明所引入的航路点导引控制方法，将复杂的航迹控制等效为较为简单的航向控制。相较于传统的手动驾驶，其控制更有效、稳定。

4、本发明在注重控制效果的同时考虑到气垫船的实际航行情况与航行效率，在过弯时根据角度选择合适的回转半径与速度，在保证安全、稳定的基础上实现平滑过弯，避免不必要的减速。

附图说明

图1是本发明中计算导引点的基本原理；

图2是本发明中计算导引点的特殊情况；

图3是本发明的全垫升气垫船航路点导引控制方法流程图；

图4气垫船航迹变化曲线；

图5艏向角、横倾角、横倾角速度、横向速度变化曲线。

具体实施方式

下面结合附图举例对本发明作进一步的描述。

本发明按以下步骤实现：

1、气垫船位置及姿态的获取

通过DGPS，雷达等组成的位置参考系统测得气垫船当前的位置信息，通过姿态传感器，电罗经等组成的姿态传感系统测得船舶的艏向、回转率、横摇等姿态信息。对获取的信息进行滤波处理，时空对准，得到精准的气垫船位置及姿态信息。

2、导引点及期望艏向角的计算

附图1展示了计算导引点的基本原理。取气垫船的实时位置(x(t),y(t))为圆心，以一定长度R(通常取气垫船长的倍数即nL_pp)为半径画圆。若该圆跟与垂线夹角为α_k的期望路径相交，所产生的两个交点中，取离下一个航路点即点P_k+1(x_k+1,y_k+1)路程最短的交点为导引点P_LOS(x_los,y_los)，即航向跟踪的期望位置。导引点P_LOS满足方程组：

经计算可求得导引点P_LOS的坐标为：

其中，

Δy＝y_k+1-y_k

Δx＝x_k+1-x_k

从而可以得到期望艏向ψ_d：

ψ_d＝atan2(y_los-y,x_los-x)

附图2展示的是计算导引的特殊情况。在特殊情况下，导引开始时气垫船离参考路径较远，以实时位置(x(t),y(t))为圆心，以R为半径的圆与期望路径没有交点。这时候采用另一种方法计算出导引点P_LOS的坐标。从气垫船的实时位置向期望路径作垂线，取位于参考路径上垂点Q(x_p,y_p)与下一个航路点P_k+1(x_p,i+1,y_p,i+1)之间的并与垂点相距Δ长度的点P_LOS为导引点。这同样可以达到导引的效果。在气垫船距离期望路径足够近，即存在交点时，便可切换为附图1所示的基本计算方法。

具体计算过程如下：

垂点位置符合以下的方程组：

其中：

Δx＝x_p,i+1-x_p,i

Δy＝y_p,i+1-y_p,i

设以垂点为起点，以气垫船实时位置为终点的距离向量为：

其方向上的单位向量为：

通过计算可以得到期望艏向角ψ_d：

随着气垫船的实时位置不断改变，导引点的位置也在期望路径上不断移动且距离下一个航路点P_k+1越来越近，而气垫船的航迹也越来越逼近期望路径，从而达到了航迹跟踪的目的。

3、对参考路径跟踪的切换

取以末端点为圆心的圆作为切换范围，当气垫船进入切换范围时便切换跟踪下一段参考路径。切换条件如下：

式中，(x(t),y(t))——气垫船实时位置

(x_k+1,y_k+1)——第k段参考路径末端航路点

R_k+1——第k个航路点切换半径

在实际航行中，根据两段期望路径的夹角大小，选择合适的切换半径和回转速度。当气垫船航行过小弯时应取稍小的切换半径，同时较大幅度减速以保证回转时气垫船的安全以及控制效果。而当气垫船过大弯时可以设定为更大的切换半径，同时小幅减速，实现快速过弯并完成切换。

具体表达式如下：

式中，R(α)、v(α)——切换半径、切换航速

R_max、R_min——切换半径上下限

v_max、v_min——切换航速上下限

α——两段相连参考路径夹角

σ、σ_R——比例系数

4、对所设计的基于全垫升气垫船的航路点导引控制方法进行仿真验证

附图3展示了仿真控制系统的框图。在仿真实验前，设定气垫船的起始位置为(0,0)，艏向角为0°，三个航路点为(400，1500)、(2200，2050)、(5000，500)。以某型全垫升气垫船为控制对象进行仿真。附图4展示了气垫船的航迹变化曲线。附图5展示了艏向角、横倾角、横倾角速度、横向速度的变化曲线。通过仿真结果可以看出，气垫船能够快速且平滑的实现航迹跟踪，跟踪效果良好，满足实际工作需求。

Claims (7)

1.一种全垫升气垫船航路点导引控制方法，其特征是：

步骤一：通过DGPS、雷达组成的位置参考系统测得气垫船当前的位置信息，通过姿态传感器，电罗经组成的姿态传感系统测得船舶的艏向、回转率、横摇姿态信息；

步骤二：结合气垫船的当前位置和目标位置，给定航路点；

步骤三：LOS导引系统综合航路点坐标(x_k,y_k)和气垫船的当前位置(x(t),y(t))，计算出航迹跟踪的导引点P_LOS(x_los,y_los)；

步骤四：根据气垫船的当前位置与导引点坐标计算出期望艏向角ψ_d，并与气垫船当前艏向角ψ比较，将二者之差Δψ＝ψ_d-ψ作为PID控制器的输入；

步骤五：PID控制器结合海风、海浪环境干扰控制气垫船向着导引点航行；同时，气垫船实时将当前的位置信息反馈给LOS导引系统，将当前的艏向角反馈给步骤五的艏向角比较环节与期望艏向角进行比较；

步骤六：根据两段期望路径的夹角大小，计算出切换半径与切换速度，以当前期望路径末端航路点为圆心，以切换半径为半径画圆，当气垫船进入圆的范围内就切换为对下一段期望路径的跟踪，同时在圆内以计算出的切换速度进行回转。

2.根据权利要求1所述的全垫升气垫船航路点导引控制方法，其特征是：对获取的当前的位置信息、姿态信息进行滤波处理和时空对准。

3.根据权利要求1或2所述的全垫升气垫船航路点导引控制方法，其特征是所述根据气垫船的当前位置与导引点坐标计算出期望艏向角ψ_d的方法为：

以气垫船的实时位置(x(t),y(t))为圆心，以一定长度R为半径画圆，若该圆跟与垂线夹角为α_k的期望路径相交，所产生的两个交点中，取离下一个航路点即点P_k+1(x_k+1,y_k+1)路程最短的交点为导引点P_LOS(x_los,y_los)，即航向跟踪的期望位置，导引点P_LOS满足方程组：

$\left\{\begin{array}{c}(x_{los}-x(t\left)\right)+{(y_{los}-y(t\left)\right)}^2=R^2\\ {\mathrm{tan\α}}_k=\frac{y_{k+1}-y_k}{x_{k+1}-x_k}=\frac{\mathrm{\Δ}y}{\mathrm{\Δ}x}=\frac{y_{los}-y_k}{x_{los}-x_k}\end{array}\right.$

经计算得导引点P_LOS的坐标为：

$x_{los}=\left\{\begin{array}{cc}\frac{-b+\sqrt{b^2-4ac}}{2a}& \mathrm{\&Delta;}x>0\\ \frac{-b-\sqrt{b^2-4ac}}{2a}& \mathrm{\&Delta;}x<0\end{array}\right.$

$y_{los}=\left\{\begin{array}{cc}y\left(t\right)+\sqrt{R^2-{(x_{los}-x(t\left)\right)}^2}& \mathrm{\&Delta;}y>0\\ y\left(t\right)-\sqrt{R^2-{(x_{los}-x(t\left)\right)}^2}& \mathrm{\&Delta;}y<0\end{array}\right.$

其中，

Δy＝y_k+1-y_k

Δx＝x_k+1-x_k

$a=1+{\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)}^2$

$b=-2x\left(t\right)-2x_k{\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)}^2+2y_k\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)-2y\left(t\right)\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)$

$c=x{\left(t\right)}^2+y{\left(t\right)}^2+x_i^2{\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)}^2+y_i^2-2y_k{x}_k\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)+2y\left(t\right)x_k\left(\frac{\mathrm{\&Delta;}y}{\mathrm{\&Delta;}x}\right)-2y\left(t\right)y_k-R^2$

从而得到期望艏向ψ_d：

ψ_d＝atan2(y_los-y,x_los-x)。

4.根据权利要求1或2所述的全垫升气垫船航路点导引控制方法，其特征是所述根据气垫船的当前位置与导引点坐标计算出期望艏向角ψ_d的方法为：

以气垫船的实时位置(x(t),y(t))为圆心，以一定长度R为半径画圆，若圆与期望路径没有交点，从气垫船的实时位置向期望路径作垂线，取位于参考路径上垂点与下一个航路点P_k+1(x_p,i+1,y_p,i+1)之间的并与垂点相距Δ长度的点为导引点；若圆与期望路径存在交点时，则按如下过程：

垂点位置符合以下的方程组：

$\left\{\begin{array}{c}\mathrm{\&Delta;}x\&CenterDot;(x-x_p)+\mathrm{\&Delta;}y\&CenterDot;(y-y_p)=0\\ \frac{x_p-x_{p,i}}{\mathrm{\&Delta;}x}=\frac{y_p-y_{p,i}}{\mathrm{\&Delta;}y}\end{array}\right.$

其中：

Δx＝x_p,i+1-x_p,i

Δy＝y_p,i+1-y_p,i

设以垂点为起点，以气垫船实时位置为终点的距离向量为：

$\stackrel{\&RightArrow;}{e}=(x-x_p,y-y_p)$

其方向上的单位向量为：

$\stackrel{\&RightArrow;}{n}=\left[\begin{array}{cc}\cos (-90)& -\sin (-90)\\ \sin (-90)& \cos (-90)\end{array}\right]\frac{1}{\sqrt{{\left(\mathrm{\&Delta;}x\right)}^2+{\left(\mathrm{\&Delta;}y\right)}^2}}\left(\begin{array}{c}\mathrm{\&Delta;}x\\ \mathrm{\&Delta;}y\end{array}\right)=\frac{1}{\sqrt{{\left(\mathrm{\&Delta;}x\right)}^2+{\left(\mathrm{\&Delta;}y\right)}^2}}\left(\begin{array}{c}\mathrm{\&Delta;}y\\ -\mathrm{\&Delta;}x\end{array}\right)$

通过计算得到期望艏向角ψ_d：

${\mathrm{\&psi;}}_d=arctan\left(\frac{\stackrel{\&RightArrow;}{e}\&CenterDot;\stackrel{\&RightArrow;}{n}}{\mathrm{\&Delta;}}\right)+arctan2(\mathrm{\&Delta;}y,\mathrm{\&Delta;}x).$

5.根据权利要求1或2所述的全垫升气垫船航路点导引控制方法，其特征是所述当气垫船进入圆的范围内就切换为对下一段期望路径的跟踪的切换条件为：

${\left(x\right(t)-x_{k+1})}^2+{\left(y\right(t)-y_{k+1})}^2<R_{k+1}^2$

其中，(x(t),y(t))——气垫船实时位置，

(x_k+1,y_k+1)——第k段参考路径末端航路点，

R_k+1——第k个航路点切换半径，

切换半径与切换航速具体表达式为：

$\left\{\begin{array}{c}R\left(\mathrm{\&alpha;}\right)=R_{max}-(R_{max}-R_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}_R^2}}\\ v\left(\mathrm{\&alpha;}\right)=v_{\min }+(v_{\max }-v_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}^2}}\end{array}\right.$

其中，R(α)、v(α)——切换半径、切换航速

R_max、R_min——切换半径上下限，

v_max、v_min——切换航速上下限，

α——两段相连参考路径夹角，

σ、σ_R——比例系数。

6.根据权利要求3所述的全垫升气垫船航路点导引控制方法，其特征是所述当气垫船进入圆的范围内就切换为对下一段期望路径的跟踪的切换条件为：

${\left(x\right(t)-x_{k+1})}^2+{\left(y\right(t)-y_{k+1})}^2<R_{k+1}^2$

其中，(x(t),y(t))——气垫船实时位置，

(x_k+1,y_k+1)——第k段参考路径末端航路点，

R_k+1——第k个航路点切换半径，

切换半径与切换航速具体表达式为：

$\left\{\begin{array}{c}R\left(\mathrm{\&alpha;}\right)=R_{max}-(R_{max}-R_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}_R^2}}\\ v\left(\mathrm{\&alpha;}\right)=v_{\min }+(v_{\max }-v_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}^2}}\end{array}\right.$

其中，R(α)、v(α)——切换半径、切换航速

R_max、R_min——切换半径上下限，

v_max、v_min——切换航速上下限，

α——两段相连参考路径夹角，

σ、σ_R——比例系数。

7.根据权利要求4所述的全垫升气垫船航路点导引控制方法，其特征是所述当气垫船进入圆的范围内就切换为对下一段期望路径的跟踪的切换条件为：

${\left(x\right(t)-x_{k+1})}^2+{\left(y\right(t)-y_{k+1})}^2<R_{k+1}^2$

其中，(x(t),y(t))——气垫船实时位置，

(x_k+1,y_k+1)——第k段参考路径末端航路点，

R_k+1——第k个航路点切换半径，

切换半径与切换航速具体表达式为：

$\left\{\begin{array}{c}R\left(\mathrm{\&alpha;}\right)=R_{max}-(R_{max}-R_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}_R^2}}\\ v\left(\mathrm{\&alpha;}\right)=v_{\min }+(v_{\max }-v_{min})e^{-\frac{{\mathrm{\&alpha;}}^2}{{\mathrm{\&sigma;}}^2}}\end{array}\right.$

其中，R(α)、v(α)——切换半径、切换航速

R_max、R_min——切换半径上下限，

v_max、v_min——切换航速上下限，

α——两段相连参考路径夹角，

σ、σ_R——比例系数。