基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制
方法
技术领域
背景技术
[0002] 自主水下航行器(Autonomous underwatervehicle，简称AUV)主要应用于海洋调查、水文观测、深海资源勘探等民用和军事领域。然而，要使水下航行器能够在海洋中自由航行并获取相关数据，关键是具有良好的运动控制能
力，这反映了整个水下航行器的系统性能。
[0003] 为了实现AUV在水平面上的精确路径跟踪，控制策略应使其快速、精确地收敛到期望路径，传统的视线法(LOS)通过设置固定的虚拟视线使AUV收敛到期望路径。事实上，这种方法对于不同弯曲程度的期望路径以及跟踪过程中的不确定性不能做出实时调整，具有很大的局限性；此外，AUV在复杂的海况下会发生摇摆方向的变化，在跟踪过程中极易产生漂角进而造成
偏航，影响设定路径上信息的获取。
发明内容
[0004] 本发明针对
现有技术存在的上述
缺陷，提供一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，以有效提高AUV的跟踪性能以及抗洋流干扰能力。
[0005] 本发明是采用以下的技术方案实现的：基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，包括以下步骤：
[0006] 步骤A、设定期望路径和期望速度，并通过AUV搭载的
传感器实时获取AUV在执行任务过程中的
姿态信息和
位置坐标信息；
[0007] 步骤B、判断是否为直线跟踪，若为直线跟踪，则以默认前视距离范围进行跟踪控制；若不是直线跟踪，则执行步骤C；
[0008] 步骤C、AUV在执行任务过程中，根据跟踪路径的
曲率确定前视距离的调整范围，并根据当前的跟踪横向误差确定前视距离的大小，实现自整定视线跟踪控制；
[0009] 步骤D、在跟踪过程中，基于实时的漂角β计算补偿度Δ，通过当前漂角预测下一时刻海流对AUV横向造成的影响，以弥补漂角对运动航向的影响，进而得到期望艏向，最终使AUV的合运动方向朝着期望航向运动。
[0010] 进一步的，所述步骤C具体通过以下方式实现：
[0011] (1)为了跟踪期望路径，在AUV当前位置在期望路径上的投影处切线正方向设定一距离为dl的虚拟目标点Q(tx，ty)，该距离dl称为“虚拟视线”或“前视距离”；
[0012] (2)基于虚拟视线dl，计算出AUV的期望航向ψd，即希望朝着虚拟目标点Q航行的方向：
[0013]
[0014]
[0015]
[0016] 式中，c为期望路径的曲率，lmin、lmax为基准前视距离的最小值、最大值，sat(*)为饱和函数，k＞0，为整定因子，通常取值为1，dlmin、dlmax为根据路径曲率调整好的虚拟视线范围，de为横向误差，K为校正力度，根据不同AUV的
稳定性进行调整，稳定性越好，K可选取更大的值增强校正灵敏度，通常取值为1；
[0017] (3)虚拟视线dl确定后，即可得到虚拟目标点Q(tx，ty)，从而可得出期望航向ψd如下：
[0018]
[0019] Δy＝ty-y
[0020] Δx＝tx-x；
[0021] 虚拟视线的具体调整策略为弯曲程度越小的期望路径下视线越长，且在跟踪过程中横向误差越小，视线越短，校正力度越小，使AUV能够自主调整自己的期望航向，“激进地”或“平缓地”靠近期望路径，即根据虚拟视线长短可计算出期望航向；虚拟视线调整策略综合考虑了期望路径弯曲程度和跟踪过程中的实时误差两方面，虚拟视线对不确定性的灵活应变能力，提高AUV的跟踪性能。
[0022] 进一步的，所述步骤D中，补偿度Δ与漂角β的关系为：
[0023] β×cos(Δ)＝Δ
[0024] 其中，β＝atan2(vy，vx)，vx为AUV的前向速度，vy为AUV的横向速度；
[0025] 使用泰勒公式将cos(Δ)展开为：
[0026]
[0027] 最终得到补偿度为：
[0028]
[0029] AUV在艏向得到补偿(ψdheading+Δ)的情况下，合运动方向会朝着期望航向运动，得到AUV的期望艏向为：
[0030] ψdheading＝ψd-Δ
[0031] 最终使AUV的合运动方向朝期望航向运动，实现AUV的抗洋流干扰能力。
[0032] 与现有技术相比，本发明的优点和积极效果在于：
[0033] 本方案在传统视线法的
基础上，基于期望路径曲率的影响以及跟踪过程中的横向误差大小，结合实时的漂角预测洋流对AUV造成的影响，提出根据期望路径的曲率以及实时横向误差大小整定合适的虚拟视线得到自整定视线导引律(ALOS)，并结合实时的漂角预测出适合的补偿度(Adaptive Compensation,AC)作用到期望艏向，使AUV最终的运动方向达到期望航向；
[0034] 该方法结合自整定视线以及自适应补偿的优点，得到了基于自整定视线与漂角补偿(ALOS+AC)的路径跟踪控制方法提升AUV的跟踪性能，通过考虑期望跟踪路径的类型和实时的跟踪误差调整视线提升AUV的跟踪效果，同时对艏向提供相应的补偿使AUV最终运动方向在期望路径上，提高在期望路径上的拟合程度，实用价值高。
附图说明
[0035] 图1为本发明
实施例所述路径跟踪控制方法的原理
框图；
[0036] 图2为本发明实施例所述路径跟进控制方法的流程示意图；
[0037] 图3为本发明实施例中AUV的跟踪示意图；
[0038] 图4为本发明实施例所述方法与其它
算法在固定洋流干扰下的直线跟踪轨迹对比图；
[0039] 图5为本发明实施例所述方法与其它算法在固定洋流干扰下的直线跟踪误差对比图；
[0040] 图6为本发明实施例所述方法与其它算法在变洋流干扰下的直线跟踪轨迹对比图；
[0041] 图7为本发明实施例所述方法与其它算法在变洋流干扰下的直线跟踪误差对比图；
[0042] 图8为本发明实施例所述方法与其它算法在固定洋流干扰下的曲线跟踪轨迹对比图；
[0043] 图9为本发明实施例所述方法与其它算法在固定洋流干扰下的曲线跟踪误差对比图；
[0044] 图10为本发明实施例所述方法与其它算法在变洋流干扰下的曲线跟踪轨迹对比图；
[0045] 图11为本发明实施例所述方法与其它算法在变洋流干扰下的曲线跟踪误差对比图。
具体实施方式
[0046] 为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本
申请的实施例及实施例中的特征可以相互组合。
[0047] 实施例，一种基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法，其原理框图及
流程图分别如图1和图2所示，具体的，包括以下步骤：
[0048] 步骤A、设定期望路径和期望速度，并通过AUV搭载的传感器实时获取AUV在执行任务过程中的姿态信息和位置坐标信息；
[0049] 步骤B、判断是否为直线跟踪，若为直线跟踪，则在用户设定的默认前视距离范围内调整虚拟视线进行跟踪控制，一般情况下，AUV的跟踪路径均为曲线；若不是直线跟踪，则执行步骤C；
[0050] 步骤C、AUV在执行任务过程中，根据跟踪路径的曲率确定前视距离的调整范围，并根据当前的跟踪横向误差确定前视距离的大小，实现自整定视线跟踪控制，具体的，如图3所示，为AUV的路径跟踪模型；
[0051] (1)为了跟踪期望路径，在AUV当前位置C(x,y)在期望路径上的投影P(nx,ny)处切线正方向设定一距离为dl的虚拟目标点Q(tx，ty)，该距离dl称为“虚拟视线”或“前视距离”，CP之间的距离de称为“横向误差”；
[0052] (2)基于虚拟视线dl，计算出AUV的期望航向ψd，即希望朝着虚拟目标点Q航行的方向：
[0053]
[0054]
[0055]
[0056]
[0057] 式中，c为期望路径的曲率，lmin、lmax为基准前视距离，即由用户设定的默认前视距离范围，通常将lmin设置为10m，lmax设置为20m，的最小值、最大值，sat(*)为饱和函数，k＞0，为整定因子，通常取值为1，dlmin、dlmax为根据路径曲率调整好的虚拟视线范围，de为横向误差，K为校正力度，根据不同AUV的稳定性进行调整，稳定性越好，K可选取更大的值增强校正灵敏度，通常取值为1；
[0058] (3)虚拟视线dl确定后，即可得到虚拟目标点Q(tx，ty)，从而可得出期望航向ψd如下：
[0059]
[0060] Δy＝ty-y
[0061] Δx＝tx-x；
[0062] 虚拟视线的具体调整策略为弯曲程度越小的期望路径下视线越长，且在跟踪过程中横向误差越小，视线越短，校正力度越小，使AUV能够自主调整自己的期望航向，“激进地”或“平缓地”靠近期望路径，即根据虚拟视线长短可计算出期望航向(希望AUV的运动方向)；虚拟视线调整策略综合考虑了期望路径弯曲程度和跟踪过程中的实时误差两方面，虚拟视线对不确定性的灵活应变能力，提高AUV的跟踪性能；
[0063] 步骤D、在跟踪过程中，由于洋流的干扰，会对AUV的前向、横向速度造成一定影响，如图3所示，通过AUV搭载的DVL、AHRS等传感器检测其前向速度vx、横向速度vy，vx可通过速度PID
控制器调节AUV螺旋桨的转速，从而消除洋流造成的影响达到期望速度，由于AUV没有横向
推进器，因此无法消除vy的影响。本方案中通过计算此时的漂角β，预测AUV达到期望艏向时洋流对其横向造成的影响，从而计算出合适的补偿度到期望艏向上，抵消洋流的影响，具体的，获得漂角β与补偿度Δ的关系：
[0064] β×cos(Δ)＝Δ
[0065] 其中，β＝atan2(vy，vx)；
[0066] 使用泰勒公式将cos(Δ)展开为：
[0067]
[0068] 最终得到补偿度为：
[0069]
[0070] AUV在艏向得到补偿(ψdheading+Δ)的情况下，合运动方向会朝着期望航向运动，期望航向步骤二已求得，因此得到AUV的期望艏向为：
[0071] ψdheading＝ψd-Δ
[0072] 最终使AUV的合运动方向朝期望航向运动，实现AUV的抗洋流干扰能力。
[0073] MATLAB仿真结果见图4～图11，分别仿真了AUV在固定洋流与变洋流环境下的直线、曲线跟踪，通过比较跟踪轨迹和跟踪误差图可表明本发明(AC+ALOS)能够使AUV更快地到达期望路径，并且在期望跟踪上具有更好的稳定性，以及更强的抗干扰能力，最终达到对期望路径的高拟合度。
[0074] 以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单
修改、等同变化与改型，仍属于本发明技术方案的保护范围。
