[发明专利]一种基于航迹偏差距离的动力浮标航迹控制方法

权利要求书

1.一种基于航迹偏差距离的动力浮标航迹控制方法，其特征在于，具体的实现步骤如下：

步骤1.电磁罗经获取动力浮标的实时航行信息，电磁罗经安装在动力浮标上，且动力浮标在已设定航迹点的路径上沿某一直线航迹段航行；

步骤2.根据实时航行信息判断动力浮标的位置，未到达规划航迹点，运行基于航迹偏差距离的动力浮标航迹控制；

步骤3.根据实时航行信息判断动力浮标的位置，到达规划航迹点并且未到达最后一个规划航迹点，则切换规划航迹点，计算下一规划航迹段的提前转向距离并开始下一规划航迹段的航迹控制；

步骤4.重复步骤2～步骤3直至到达最后一个规划航迹点完成航迹控制任务；

步骤2具体包括以下步骤：

步骤2.1.取固定在地球上的坐标系为XY轴的参考坐标系；设D_k(x_k,y_k)、D_k+1(x_k+1,y_k+1)分别代表两个相邻的规划航迹点，动力浮标沿规划航迹点D_k、D_k+1之间的连线航行；设B(t_i)代表t_i时刻动力浮标的位置，坐标为(x_i,y_i)；此时动力浮标的航向为ψ_B(t_i)；

步骤2.2.得到规划航迹线D_kD_k+1的角度动力浮标到规划航迹点D_k+1的距离动力浮标到规划航迹点D_k+1的期望航向角计算出动力浮标到规划航迹线D_kD_k+1的航迹偏差距离d(t_i)＝d_D(t_i)*sin(β)，其中，β＝ψ_dd+ψ_bd；

步骤2.3.根据航迹偏差距离计算出目标航向增量，根据动力浮标与规划航迹段的相对位置计算出目标航向；

步骤2.4.根据计算的目标航向和采集的航向信息，按照PID控制方法对航向进行控制；

步骤2.5.若未到达当前目标航迹点，循环执行步骤2；若到达当前规划航迹点，执行步骤3；

步骤3所述的提前转向距离为

其中R_d(k+1)为提前转向距离，ψ_d为相邻航迹段的夹角，α为常系数，R_b为当舵角为δ_b时动力浮标的回转半径，ψ_dmin、ψ_dmax分别为相邻航迹段的夹角的最小限定值和最大限定值；

步骤2.3所述的目标航向增量为

步骤2.3所述的目标航向为

其中，m、n为常系数，d_min，d_max为选定的航迹偏差距离临界值，ψ₁＝md_min²，ψ_max＝n(|d_max|-d_min)+ψ₁。