[发明专利]一种USV的航迹闭环控制方法

权利要求书

1.一种USV的航迹闭环控制方法，其特征在于，包括以下步骤：

步骤1：设定目标航行路径，且判断位置传感器和姿态传感器是否处于安全工作状态，如果是，则执行步骤2，否则报警；

步骤2：USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离，如果该距离小于设定距离，则延时等待后重新执行步骤2，否则执行步骤3；

步骤3：根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息，计算目标航向角ψ_r(k)；所述目标航向角ψ_r(k)的计算过程为：

期望航向ψ_r是垂足点(x’,y’)处的切线方向，则有：

ψ_r(k)＝arctan((y_i+1-y_i)/(x_i+1-x_i))

其中，用若干个直线段代替规划的航线，(x_i,y_i),(x_i+1,y_i+1)为当前目标航行路径航迹段i的两个端点坐标，为目标航行路径；

步骤4：根据目标航向角ψ_r(k)、当前航向角ψ(k)和航向角速度Δψ(k)，计算目标喷嘴角ω_r(k)；所述目标喷嘴角ω_r(k)的计算过程为：

其中，ω_r(k)为目标喷嘴角，ψ_r(k)为目标航向角，ψ(k)为当前航向角，Δψ(k)为航向角速度，K_P1为1.05，K_I1为0.011，K_D1为-4.1；

步骤5：根据目标喷嘴角ω_r(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k)，计算输出动作量，所述输出动作量的计算过程为：

其中，ACT为输出动作量，ω_r(k)为目标喷嘴角，ω(k)为当前喷嘴角，Δω(k)为喷嘴角速度，K_P2为2.1，K_I2为0.03，K_D2为-2.5；

进而控制USV动作，返回步骤2。

2.根据权利要求1所述的USV的航迹闭环控制方法，其特征在于：所述偏离预定航线的距离的计算过程为：

ε²(k)＝(x(k)-x')²+(y(k)-y')²

其中，ε(k)为偏离预定航线的距离；x(k),y(k)为USV当前位置坐标；x’,y’为USV当前位置到目标航行路径的垂线段交点坐标。