CN109582026B - 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，以提高AUV在洋流干扰下的跟踪性能，该方法在视线法(LOS)的基础上将虚拟视线的选取考虑到期望路径的曲率以及跟踪过程中的横向误差，同时根据跟踪过程中的漂角计算出合适的补偿度作用到期望艏向，使AUV在运动过程中根据实时的跟踪状况自整定虚拟视线长短，同时消除洋流对其运动方向产生的影响。且通过仿真试验证明该方法能够使AUV更快速地收敛到期望路径，实现了AUV在复杂海洋环境下精确的路径跟踪控制，具有较高的实用及推广价值。

Description

基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制 方法

技术领域

本发明涉及水下机器人智能控制领域，具体涉及一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法。

背景技术

自主水下航行器(Autonomous underwatervehicle，简称AUV)主要应用于海洋调查、水文观测、深海资源勘探等民用和军事领域。然而，要使水下航行器能够在海洋中自由航行并获取相关数据，关键是具有良好的运动控制能力，这反映了整个水下航行器的系统性能。

为了实现AUV在水平面上的精确路径跟踪，控制策略应使其快速、精确地收敛到期望路径，传统的视线法(LOS)通过设置固定的虚拟视线使AUV收敛到期望路径。事实上，这种方法对于不同弯曲程度的期望路径以及跟踪过程中的不确定性不能做出实时调整，具有很大的局限性；此外，AUV在复杂的海况下会发生摇摆方向的变化，在跟踪过程中极易产生漂角进而造成偏航，影响设定路径上信息的获取。

发明内容

本发明针对现有技术存在的上述缺陷，提供一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，以有效提高AUV的跟踪性能以及抗洋流干扰能力。

本发明是采用以下的技术方案实现的：基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，包括以下步骤：

步骤A、设定期望路径和期望速度，并通过AUV搭载的传感器实时获取AUV在执行任务过程中的姿态信息和位置坐标信息；

步骤B、判断是否为直线跟踪，若为直线跟踪，则以默认前视距离范围进行跟踪控制；若不是直线跟踪，则执行步骤C；

步骤C、AUV在执行任务过程中，根据跟踪路径的曲率确定前视距离的调整范围，并根据当前的跟踪横向误差确定前视距离的大小，实现自整定视线跟踪控制；

步骤D、在跟踪过程中，基于实时的漂角β计算补偿度Δ，通过当前漂角预测下一时刻海流对AUV横向造成的影响，以弥补漂角对运动航向的影响，进而得到期望艏向，最终使AUV的合运动方向朝着期望航向运动。

进一步的，所述步骤C具体通过以下方式实现：

(1)为了跟踪期望路径，在AUV当前位置在期望路径上的投影处切线正方向设定一距离为d_l的虚拟目标点Q(t_x，t_y)，该距离d_l称为“虚拟视线”或“前视距离”；

(2)基于虚拟视线d_l，计算出AUV的期望航向ψ_d，即希望朝着虚拟目标点Q航行的方向：

式中，c为期望路径的曲率，l_min、l_max为基准前视距离的最小值、最大值，sat(*)为饱和函数，k＞0，为整定因子，通常取值为1，d_lmin、d_lmax为根据路径曲率调整好的虚拟视线范围，d_e为横向误差，K为校正力度，根据不同AUV的稳定性进行调整，稳定性越好，K可选取更大的值增强校正灵敏度，通常取值为1；

(3)虚拟视线d_l确定后，即可得到虚拟目标点Q(t_x，t_y)，从而可得出期望航向ψ_d如下：

Δ_y＝t_y-y

Δx＝t_x-x；

虚拟视线的具体调整策略为弯曲程度越小的期望路径下视线越长，且在跟踪过程中横向误差越小，视线越短，校正力度越小，使AUV能够自主调整自己的期望航向，“激进地”或“平缓地”靠近期望路径，即根据虚拟视线长短可计算出期望航向；虚拟视线调整策略综合考虑了期望路径弯曲程度和跟踪过程中的实时误差两方面，虚拟视线对不确定性的灵活应变能力，提高AUV的跟踪性能。

进一步的，所述步骤D中，补偿度Δ与漂角β的关系为：

β×cos(Δ)＝Δ

其中，β＝atan2(v_y，v_x)，v_x为AUV的前向速度，v_y为AUV的横向速度；

使用泰勒公式将cos(Δ)展开为：

最终得到补偿度为：

AUV在艏向得到补偿(ψ_dheading+Δ)的情况下，合运动方向会朝着期望航向运动，得到AUV的期望艏向为：

ψ_dheading＝ψ_d-Δ

最终使AUV的合运动方向朝期望航向运动，实现AUV的抗洋流干扰能力。

与现有技术相比，本发明的优点和积极效果在于：

本方案在传统视线法的基础上，基于期望路径曲率的影响以及跟踪过程中的横向误差大小，结合实时的漂角预测洋流对AUV造成的影响，提出根据期望路径的曲率以及实时横向误差大小整定合适的虚拟视线得到自整定视线导引律(ALOS)，并结合实时的漂角预测出适合的补偿度(Adaptive Compensation,AC)作用到期望艏向，使AUV最终的运动方向达到期望航向；

该方法结合自整定视线以及自适应补偿的优点，得到了基于自整定视线与漂角补偿(ALOS+AC)的路径跟踪控制方法提升AUV的跟踪性能，通过考虑期望跟踪路径的类型和实时的跟踪误差调整视线提升AUV的跟踪效果，同时对艏向提供相应的补偿使AUV最终运动方向在期望路径上，提高在期望路径上的拟合程度，实用价值高。

附图说明

图1为本发明实施例所述路径跟踪控制方法的原理框图；

图2为本发明实施例所述路径跟进控制方法的流程示意图；

图3为本发明实施例中AUV的跟踪示意图；

图4为本发明实施例所述方法与其它算法在固定洋流干扰下的直线跟踪轨迹对比图；

图5为本发明实施例所述方法与其它算法在固定洋流干扰下的直线跟踪误差对比图；

图6为本发明实施例所述方法与其它算法在变洋流干扰下的直线跟踪轨迹对比图；

图7为本发明实施例所述方法与其它算法在变洋流干扰下的直线跟踪误差对比图；

图8为本发明实施例所述方法与其它算法在固定洋流干扰下的曲线跟踪轨迹对比图；

图9为本发明实施例所述方法与其它算法在固定洋流干扰下的曲线跟踪误差对比图；

图10为本发明实施例所述方法与其它算法在变洋流干扰下的曲线跟踪轨迹对比图；

图11为本发明实施例所述方法与其它算法在变洋流干扰下的曲线跟踪误差对比图。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

实施例，一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，其原理框图及流程图分别如图1和图2所示，具体的，包括以下步骤：

步骤A、设定期望路径和期望速度，并通过AUV搭载的传感器实时获取AUV在执行任务过程中的姿态信息和位置坐标信息；

步骤B、判断是否为直线跟踪，若为直线跟踪，则在用户设定的默认前视距离范围内调整虚拟视线进行跟踪控制，一般情况下，AUV的跟踪路径均为曲线；若不是直线跟踪，则执行步骤C；

步骤C、AUV在执行任务过程中，根据跟踪路径的曲率确定前视距离的调整范围，并根据当前的跟踪横向误差确定前视距离的大小，实现自整定视线跟踪控制，具体的，如图3所示，为AUV的路径跟踪模型；

(1)为了跟踪期望路径，在AUV当前位置C(x,y)在期望路径上的投影P(n_x,n_y)处切线正方向设定一距离为d_l的虚拟目标点Q(t_x，t_y)，该距离d_l称为“虚拟视线”或“前视距离”，CP之间的距离d_e称为“横向误差”；

(2)基于虚拟视线d_l，计算出AUV的期望航向ψ_d，即希望朝着虚拟目标点Q航行的方向：

式中，c为期望路径的曲率，l_min、l_max为基准前视距离，即由用户设定的默认前视距离范围，通常将l_min设置为10m，l_max设置为20m，的最小值、最大值，sat(*)为饱和函数，k＞0，为整定因子，通常取值为1，d_lmin、d_lmax为根据路径曲率调整好的虚拟视线范围，d_e为横向误差，K为校正力度，根据不同AUV的稳定性进行调整，稳定性越好，K可选取更大的值增强校正灵敏度，通常取值为1；

(3)虚拟视线d_l确定后，即可得到虚拟目标点Q(t_x，t_y)，从而可得出期望航向ψ_d如下：

Δ_y＝t_y-y

Δx＝t_x-x；

虚拟视线的具体调整策略为弯曲程度越小的期望路径下视线越长，且在跟踪过程中横向误差越小，视线越短，校正力度越小，使AUV能够自主调整自己的期望航向，“激进地”或“平缓地”靠近期望路径，即根据虚拟视线长短可计算出期望航向(希望AUV的运动方向)；虚拟视线调整策略综合考虑了期望路径弯曲程度和跟踪过程中的实时误差两方面，虚拟视线对不确定性的灵活应变能力，提高AUV的跟踪性能；

步骤D、在跟踪过程中，由于洋流的干扰，会对AUV的前向、横向速度造成一定影响，如图3所示，通过AUV搭载的DVL、AHRS等传感器检测其前向速度v_x、横向速度v_y，v_x可通过速度PID控制器调节AUV螺旋桨的转速，从而消除洋流造成的影响达到期望速度，由于AUV没有横向推进器，因此无法消除v_y的影响。本方案中通过计算此时的漂角β，预测AUV达到期望艏向时洋流对其横向造成的影响，从而计算出合适的补偿度到期望艏向上，抵消洋流的影响，具体的，获得漂角β与补偿度Δ的关系：

β×cos(Δ)＝Δ

其中，β＝atan2(v_y，v_x)；

使用泰勒公式将cos(Δ)展开为：

最终得到补偿度为：

AUV在艏向得到补偿(ψ_dheading+Δ)的情况下，合运动方向会朝着期望航向运动，期望航向步骤二已求得，因此得到AUV的期望艏向为：

ψ_dheading＝ψ_d-Δ

最终使AUV的合运动方向朝期望航向运动，实现AUV的抗洋流干扰能力。

MATLAB仿真结果见图4～图11，分别仿真了AUV在固定洋流与变洋流环境下的直线、曲线跟踪，通过比较跟踪轨迹和跟踪误差图可表明本发明(AC+ALOS)能够使AUV更快地到达期望路径，并且在期望跟踪上具有更好的稳定性，以及更强的抗干扰能力，最终达到对期望路径的高拟合度。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (2)

1.基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，其特征在于，包括以下步骤：

步骤A、设定期望路径，并通过AUV搭载的传感器实时获取AUV在执行任务过程中的姿态信息和位置坐标信息；

步骤B、判断是否为直线跟踪，若为直线跟踪，则以默认前视距离范围进行跟踪控制；若不是直线跟踪，则执行步骤C；

步骤C、AUV在执行任务过程中，根据跟踪路径的曲率确定前视距离的调整范围，并根据当前的跟踪横向误差确定前视距离的大小，实现自整定视线跟踪控制，具体包括：

(1)为了跟踪期望路径，在AUV当前位置C(x,y)在期望路径上的投影处切线正方向设定一距离为d_l的虚拟目标点Q(t_x，t_y)，该距离d_l称为“虚拟视线”或“前视距离”；

(2)基于虚拟视线d_l，计算出AUV的期望航向ψ_d：

式中，c为期望路径的曲率，l_min、l_max为基准前视距离的最小值、最大值，sat(*)为饱和函数，k＞0，为整定因子，d_lmin、d_lmax为根据路径曲率调整好的虚拟视线范围，d_e为横向误差，记AUV的当前位置C在期望路径上的投影为P，则CP之间的距离即为横向误差d_e，K为校正力度，根据不同AUV的稳定性进行调整；

(3)虚拟视线d_l确定后，即可得到虚拟目标点Q(t_x，t_y)，从而可得出期望航向ψ_d：

Δy＝t_y-y

Δx＝t_x-x；

步骤D、在跟踪过程中，基于实时的漂角β计算补偿度Δ，由期望航向ψ_d减去补偿度Δ进而得到期望艏向，结合漂角补偿最终使AUV的合运动方向朝着期望航向运动。

2.根据权利要求1所述的基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，其特征在于：所述步骤D中，补偿度Δ与漂角β的关系为：

β×cos(Δ)＝Δ

其中，β＝atan2(v_y，v_x)，v_x为AUV的前向速度，v_y为AUV的横向速度；

使用泰勒公式将cos(Δ)展开为：

最终得到补偿度为：

AUV在艏向得到补偿(ψ_dheading+Δ)的情况下，合运动方向会朝着期望航向运动，得到AUV的期望艏向为：

ψ_dheading＝ψ_d-Δ。

Images