CN106444838A - 一种自主水下航行器的精确航路跟踪控制方法 - Google Patents

Abstract

本发明涉及一种自主水下航行器的精确航路跟踪控制方法，用于直线航迹跟踪控制。该方法更为精确的一种航路点跟踪控制方法，它通过横向跟踪控制，使得AUV精确地沿连接两个航路点的直线航路运动，避免了视线导引中由于AUV的初始位置偏差造成的实际航路偏离期望直线，因此更加适用于对航路跟踪精度有更高要求的领域，如海底勘查、对接回收等。

Description

一种自主水下航行器的精确航路跟踪控制方法

技术领域

本发明涉及一种自主水下航行器的精确航路跟踪控制方法，属于自主水下航行器跟踪控制技术领域。

背景技术

自主水下航行器(autonomous underwater vehicle,AUV)作为海洋开发的一项重要工具,可以出色的完成多种民用与军事任务,近年来取得了长足的发展。AUV的航路点跟踪控制作为AUV研究中的一个关键问题，引起了人们的广泛关注。所谓航路点跟踪是指AUV出发后,通过控制输入作用使其向航路点运动，当与之距离小于某个阈值时，认为到达航路点并且开始向下一个航路点运动，直至完成所有航路点运动。

目前，AUV水下航路跟踪控制方法主要有：

(1)视线法航路点导引(LOS)。LOS根据AUV位置和航路点的相对位置，计算视线的方位角作为参考航向角，然后基于线性化方法或反演方法，设计航向角控制器，并往往忽略参考航向角的动态特性。LOS导引法通过航向角控制调整AUV的运动方向，使其始终向航路点运动，而不考虑AUV与航路点连线的位置偏差。AUV的运动轨迹会由于初始位置和航向角偏差而偏差期望的航线，造成航程的损失。

(2)模糊航路点跟踪控制。依据模糊控制理论的基础，并根据控制对象的特点，制定模糊控制规则，设计出模糊控制器，对AUV的航路点跟踪运动进行控制。该方法的主要缺点是模糊控制规则的获取以及输入模糊变量论域和隶属函数的确定都是实验分析和操作人员经验的汇总，受人为主观因素的影响较大。

发明内容

为了避免现有技术的不足之处，本发明提出了一种自主水下航行器的精确航路跟踪控制方法，这种航路跟踪控制方法是直线航迹跟踪控制。直线航迹跟踪控制是使从任意初始状态出发的AUV，收敛于给定直线，并沿该直线运动，直线航迹跟踪可以看作是一种特殊的路径跟踪。选择期望的直线航迹为地面坐标系的x轴，y轴与x轴垂直向右。

直线航迹跟踪误差用坐标y描述，称为横向机动误差。欠驱动AUV的水平面直线航迹跟踪如图1所示。

直线轨迹跟踪控制的目标是，设计偏航力矩N使AUV从任意初始状态出发，跟踪误差y渐近收敛到零。直线航迹跟踪控制以AUV与期望航线的偏差为控制目标，使AUV跟踪连接航路点的直线运动，直到完成所有航路点。

一种自主水下航行器的精确航路跟踪控制方法，步骤如下：

步骤1：建立AUV水平面数学模型：

步骤2：AUV以初始状态开始进行直线跟踪控制运动，其中，y(0)，ψ(0)，v(0)和r(0)可在AUV航行之前进行设定；

步骤3：假设点P₁-P_n为指定的一系列航路点；

步骤4：选择以前一航路点P_i为坐标原点，x轴指向当前目标航路点P_i+1的航路点坐标系{W_i}，如图2所示。

步骤5：AUV在{W_i}坐标系中的坐标(x_i,y_i)和航向角ψ_i可以通过下面的坐标变换得到

ψ_i＝ψ-ψ_pi

式中，(x_pi,y_pi)为航路点P_i的位置坐标，ψ_pi为向量的方位角，

为{W_i}到{U}的常值坐标转换矩阵。通过前面提出直线航迹跟踪控制，使AUV沿直线P_iP_i+1运动。

步骤6：在模型式中，航向角ψ可以看作是航迹跟踪误差y的虚拟控制，选择镇定函数

ψ_d＝α(y)＝-atan(k₁y)

显然，ψ_d∈(-π/2,π/2)，参考航向ψ_d可以看作是AUV以x轴上前方Δ＝1/k₁的点P为瞄准点的一种视线导引法，如图1所示。

步骤7：定义相应的参考航向角速度

步骤8：定义航向角跟踪误差

步骤9：AUV沿直线P_iP_i+1运动，AUV与P_i+1距离小于l(到达半径)时，建立下一个航路点坐标系{W_i+1}，使AUV沿P_i+1P_i+2运动，直到完成所有航路点。

本发明的有益效果是：

直线航迹跟踪控制以与期望航线的偏差为待控制量，从而保证了AUV沿期望的航线运动，更加符合精确测量、侦察任务的要求。

附图说明

图1欠驱动AUV直线航迹跟踪。

图2基于直线航迹跟踪控制的AUV航路点跟踪图。

图3水平面航路点跟踪运动轨迹；其中“CT”为直线航迹跟踪控制轨迹，“LOS”为LOS导引法轨迹。

图4直线航迹跟踪误差曲线。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

参照图1和图2，一种自主水下航行器的精确航路跟踪控制方法，步骤如下：

步骤1：建立AUV水平面数学模型：

步骤2：AUV以初始状态开始进行直线跟踪控制运动，其中，y(0)，ψ(0)，v(0)和r(0)可在AUV航行之前进行设定；

步骤3：假设点P₁-P_n为指定的一系列航路点；

步骤4：选择以前一航路点P_i为坐标原点，x轴指向当前目标航路点P_i+1的航路点坐标系{W_i}，如图2所示。

步骤5：AUV在{W_i}坐标系中的坐标(x_i,y_i)和航向角ψ_i可以通过下面的坐标变换得到

ψ_i＝ψ-ψ_pi

式中，(x_pi,y_pi)为航路点P_i的位置坐标，ψ_pi为向量的方位角，

为{W_i}到{U}的常值坐标转换矩阵。通过前面提出直线航迹跟踪控制，使AUV沿直线P_iP_i+1运动。

步骤6：在模型式中，航向角ψ可以看作是航迹跟踪误差y的虚拟控制，选择镇定函数

ψ_d＝α(y)＝-atan(k₁y)

显然，ψ_d∈(-π/2,π/2)，参考航向ψ_d可以看作是AUV以x轴上前方Δ＝1/k₁的点P为瞄准点的一种视线导引法，如图1所示。

步骤7：定义相应的参考航向角速度

步骤8：定义航向角跟踪误差

步骤9：AUV沿直线P_iP_i+1运动，AUV与P_i+1距离小于l(到达半径)时，建立下一个航路点坐标系{W_i+1}，使AUV沿P_i+1P_i+2运动，直到完成所有航路点。

下述给出一个实施例具体予以说明：航路点跟踪

步骤1：以REMUS AUV为研究对象，建立数学模型。

步骤2：AUV运动的初始状态为y(0)＝10，ψ(0)＝π/2，v(0)＝1，r(0)＝0，常值前向速度u_c＝1.0。

步骤3：为AUV设计所谓的“割草(Lawn mowing)”式搜索任务，选择如下航路点

P₀(0,0),P₁(200,0),P₂(200,50),P₃(0,50),P₄(0,100),

P₅(200,100),P₆(200,150),P₇(200,150),P₈(0,150),P₉(0,200),

P₁₀(200,200),P₁₁(200,250),P₁₂(0,250),P₁₃(0,300)

其中，P₀为航行任务的起点，P₁～P₁₃为待完成的航路点。

步骤4：选择到达半径l＝10m。

步骤5：比较直线航迹跟踪法和LOS导引法的性能，采用如下的LOS导引律

ψ_LOS＝arctan2(y_i+1-y,x_i+1-x)

步骤6：采用航行角跟踪航向角跟踪控制

ψ_e＝ψ-ψ_LOS

步骤7：当AUV与P_i+1距离小于l(到达半径)时，建立下一个航路点坐标系{W_i+1}，使AUV沿P_i+1P_i+2运动，直到完成所有航路点。

仿真结果如图3、4所示，从航行轨迹看，直线航迹跟踪控制以与期望航线的偏差为待控制量，从而保证了AUV沿期望的航线运动，更加符合精确测量、侦察任务的要求，如海底勘查、对接回收等。而LOS导引法则以沿视线方向运动为控制目标，不考虑与期望航线的偏差，会由于航路点跟踪的初始误差而偏离期望航线，降低了航路点跟踪的精度。

Claims (3)

1.一种自主水下航行器的精确航路跟踪控制方法，用于直线航迹跟踪控制，其特征在于，其中直线航迹为地面坐标系的x轴，y轴与x轴垂直向右；直线航迹跟踪误差用坐标y描述，称为横向机动误差。

2.根据权利要求1所述的一种自主水下航行器的精确航路跟踪控制方法，其特征在于，直线轨迹跟踪控制设计偏航力矩N使AUV从任意初始状态出发，跟踪误差y渐近收敛到零。

3.根据权利要求1或2所述的一种自主水下航行器的精确航路跟踪控制方法，其特征在于，包括以下步骤：

步骤1：建立AUV水平面数学模型

$\stackrel{\·}{y}=u_{c}sin\mathrm{\ψ}+vcos\mathrm{\ψ}$

$\stackrel{\·}{v}=-\frac{m_{11}}{m_{22}}{u}_{c}r-\frac{d_{22}}{m_{22}}v$

$\stackrel{\·}{\mathrm{\ψ}}=r$

$\stackrel{\·}{r}=\frac{m_{11}-m_{22}}{m_{33}}{u}_{c}v-\frac{d_{33}}{m_{33}}r+\frac{1}{m_{33}}N$

步骤2：AUV以初始状态开始进行直线跟踪控制运动，其中，y(0)，ψ(0)，v(0)和r(0)可在AUV航行之前进行设定；

步骤3：假设点P₁-P_n为指定的一系列航路点；

步骤4：选择以前一航路点P_i为坐标原点，x轴指向当前目标航路点P_i+1的航路点坐标系{W_i}；

步骤5：AUV在{W_i}坐标系中的坐标(x_i,y_i)和航向角ψ_i可以通过下面的坐标变换得到

$\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]=R^T\left({\mathrm{\ψ}}_{pi}\right)\left[\begin{array}{c}x-x_{pi}\\ y-y_{pi}\end{array}\right]$

ψ_i＝ψ-ψ_pi

式中，(x_pi，y_pi)为航路点P_i的位置坐标，ψ_pi为向量的方位角，

$R\left({\mathrm{\ψ}}_{pi}\right)=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_{pi}& -{\mathrm{sin\ψ}}_{pi}\\ {\mathrm{sin\ψ}}_{pi}& {\mathrm{cos\ψ}}_{pi}\end{array}\right]$

为{W_i}到{U}的常值坐标转换矩阵。通过前面提出直线航迹跟踪控制，使AUV沿直线P_iP_i+1运动；

步骤6：在模型式中，航向角ψ可以看作是航迹跟踪误差y的虚拟控制，选择镇定函数

ψ_d＝α(y)＝-atan(k₁y)

显然，ψ_d∈(-π/2,π/2)，参考航向ψ_d可以看作是AUV以x轴上前方Δ＝1/k₁的点P为瞄准点的一种视线导引法；

步骤7：定义相应的参考航向角速度

$r_d\stackrel{\mathrm{\Δ}}{=}{\stackrel{\·}{\mathrm{\ψ}}}_d=-k_1{\cos }^2{\mathrm{\ψ}}_d(u_{c}sin\mathrm{\ψ}+vcos\mathrm{\ψ})$

步骤8：定义航向角跟踪误差

${\mathrm{\ψ}}_e\stackrel{\mathrm{\Δ}}{=}{k}_p(\mathrm{\ψ}-{\mathrm{\ψ}}_d)+k_i\∫(\mathrm{\ψ}-{\mathrm{\ψ}}_d)dt+k_{d}r$

步骤9：AUV沿直线P_iP_i+1运动，AUV与P_i+1距离小于l(到达半径)时，建立下一个航路点坐标系{W_i+1}，使AUV沿P_i+1P_i+2运动，直到完成所有航路点。