本发明涉及水下潜器运动控制，尤其涉及一种基于自适应前视距离视线法的水下潜器跟踪控制方法。
背景技术：
1、海洋总面积约占地球表面积的71％，丰富的海洋资源等待着人们探索和开发，水下潜器应运而生。水下潜器在完成各项作业任务时，大多会沿着既定的规划路径运动，为了保证作业任务的高效完成和水下潜器的自身安全，精准的跟踪控制尤为重要。
2、视线法由于其参数较少，成为水下潜器跟踪控制中常用的方法之一，该方法的原理是基于路径上的前视点对水下潜器的期望航向进行引导，因此前视点的选取直接影响水下潜器的跟踪效果。前视距离是前视点选取的依据，传统的视线法中前视距离通常为固定值，大约是潜器长度的2～5倍，固定的前视距离意味着水下潜器向期望路径的收敛能力是不变的。但在跟踪过程中，水下潜器距离期望路径的横向距离、路径曲率以及前进速度都在不断变化，恒定的前视距离会导致水下潜器不断穿梭于期望路径的两侧，很难稳定在期望路径上。此外由于水下环境摩擦力小，水下潜器在转弯时会发生侧滑，跟踪效果也会大打折扣。
3、此外公告号为cn116430856a和cn116027778a的中国专利申请，也分别公开了一种基于变前视距离的视线法，cn116430856a中前视距离以与曲率有关的指数表达式呈现，且该表达式基于前视距离的最大、最小值。而cn116027778a中前视距离也以表达式形式存在，并与航行速度和跟踪误差有关。而公告号为cn116295447a和cn116295447a的中国专利申请，分别公开了一种通过模糊控制器获取前视距离的方法，但cn116295447a仅考虑了横向误差和路径曲率对前视距离的影响，而cn116295447a仅考虑了参考速度和参考曲率，且模糊控制器得到的是前视距离增益，前视距离是前视距离增益与当前速度的乘积。以上两个专利用于车辆的路径跟踪领域，不存在漂移角补偿。
技术实现思路
1、本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于自适应前视距离视线法的水下潜器跟踪控制方法，采用自适应前视距离改进传统视线法，并补偿漂移角，有效提高水下潜器的跟踪精度。
2、为解决上述技术问题，本发明所采取的技术方案是：
3、一种基于自适应前视距离视线法的水下潜器跟踪控制方法，包括以下步骤：
4、步骤1：建立水下潜器在水平面的运动学和动力学模型以及推进器分布，给定水下潜器跟踪控制的期望路径；
5、步骤2：将期望路径分成n段，选取期望路径上某点(xk(θ)，yk(θ))，通过该点做期望路径的切线，切线与地坐标系x轴的夹角为路径切向角γp；
6、步骤3：根据水下潜器所在位置(x，y)和路径切向角γp构建水下潜器跟踪的横向误差ye；
7、步骤4：利用速度传感器测量潜器体坐标系x、y轴的线速度u、v，其中x轴的线速度u也称为前进速度；根据线速度u、v计算漂移角β；
8、步骤5：计算期望路径曲率c，根据横向误差ye、路径曲率c、前进速度u通过模糊控制器得到前视距离δ；
9、步骤6：根据路径切向角γp、横向误差ye、漂移角β、前视距离δ计算期望艏向角ψd；控制水下潜器的艏向角依照期望艏向角运动，即可实现精准的跟踪控制。
10、进一步地，所述步骤1中水平面运动学模型为：
11、
12、水平面动力学模型为：
13、
14、v＝[uvr]
15、推力分布为：
16、
17、u＝[t1t2]
18、式中，x、y为水下潜器在地坐标系x、y轴的移动距离；所述地坐标系，以水平面任意点为坐标系原点，x轴指向正北，y轴指向正东；ψ为水下潜器的艏向角；m为惯性矩阵，包括水动力引起的附加质量矩阵；v为水下潜器的运动速度，其中u、v分别为水下潜器在体坐标系x、y轴的速度分量；所述体坐标系，以水下潜器的质心点为坐标系原点，x轴平行于艇体基线指向艇首，y轴平行于基面指向右舷；r为水下潜器在水平面的角速度；c(v)为哥式力和向心力矩阵；d(v)为水阻力与动力矩阵；g为重力和浮力矩阵；t为外部施加的作用力和力矩；f为外部施加的作用力；q为外部施加的力矩；l为转换矩阵；u为推进器推力向量，t1代表水下潜器左侧推进器推力之和，t2代表水下潜器右侧推进器推力之和。
19、进一步地，所述步骤2中路径切向角γp表示为：
20、γp＝atan2(y′k(θ),x′k(θ))
21、
22、
23、式中，θ为水下潜器期望路径的参数。
24、进一步地，所述步骤3中横向误差ye表示为：ye＝-(x-xk(θ))sinγp+(y-yk(θ))cosγp。
25、进一步地，所述步骤4中的速度传感器采用多普勒计程仪；漂移角β表示为：β＝atan2(v,u)。
26、进一步地，所述步骤5中期望路径曲率c表示为：
27、c＝(x′k(θ)y″k(θ)-x″k(θ)y′k(θ))/((x′k(θ))2+(y′k(θ))2)3/2
28、模糊控制器输入变量为横向误差ye、路径曲率c、前进速度u，输出变量为前视距离δ；设置ye、c和u的论域为[0,p]，δ的论域均为[s,t]；模糊控制器的输出为g个语言值，构成一个输出模糊集合a，实际控制时需要从输出模糊集合中采取加权平均法判决出一个精确控制量，针对论域中的每个元素ai，其中i＝1,2,…,g，以ai作为待判决输出模糊集合a的隶属度μa(ai)的加权系数，加权判断结果即前视距离δ，表示为：
29、
30、进一步地，所述步骤6中期望艏向角ψd表示为：ψd＝γp-arctan(ye/δ)-β。
31、采用上述技术方案所产生的有益效果在于：本发明提供的基于自适应前视距离视线法的水下潜器跟踪控制方法，充分考虑了影响前视距离选取的横向误差、路径曲率、前进速度三个因素，将三个影响因素与前视距离的关系根据以往经验整理成模糊规则，建立模糊控制器，根据水下潜器跟踪控制过程中的状态实时调整前视距离，从而改变期望艏向角，并加入漂移角补偿，使水下潜器更精准地沿期望路径运动。
1.一种基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述方法包括以下步骤：
2.根据权利要求1所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤1中水平面运动学模型为：
3.根据权利要求2所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤2中路径切向角γp表示为：
4.根据权利要求3所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤3中横向误差ye表示为：ye＝-(x-xk(θ))sinγp+(y-yk(θ))cosγp。
5.根据权利要求4所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤4中的速度传感器采用多普勒计程仪；漂移角β表示为：β＝atan2(v,u)。
6.根据权利要求5所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤5中期望路径曲率c表示为：
7.根据权利要求6所述的基于自适应前视距离视线法的水下潜器跟踪控制方法，其特征在于：所述步骤6中期望艏向角ψd表示为：ψd＝γp-arctan(ye/δ)-β。