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

摘要

本发明涉及一种USV的航迹闭环控制方法，设定目标航行路径，且判断位置传感器和姿态传感器是否处于安全工作状态，USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离，如果该距离小于设定距离，则延时等待，根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息，计算目标航向角ψ_r(k)；根据目标航向角ψ_r(k)、当前航向角ψ(k)和航向角速度Δψ(k)，计算目标喷嘴角ω_r(k)；根据目标喷嘴角ω_r(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k)，计算输出动作量，进而控制USV动作。本发明采用PID经典控制理论与在线参数辨识并更新技术相结合的方法，适合USV在复杂海洋环境下的非线性时变模型系统，实现航迹闭环控制。

主权项

1.一种USV的航迹闭环控制方法，其特征在于，包括以下步骤：/n步骤1：设定目标航行路径，且判断位置传感器和姿态传感器是否处于安全工作状态，如果是，则执行步骤2，否则报警；/n步骤2：USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离，如果该距离小于设定距离，则延时等待后重新执行步骤2，否则执行步骤3；/n步骤3：根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息，计算目标航向角ψ_r(k)；所述目标航向角ψ_r(k)的计算过程为：/n期望航向ψ_r是垂足点(x’,y’)处的切线方向，则有：/n /nψ_r(k)＝arctan((y_i+1-y_i)/(x_i+1-x_i))/n其中，用若干个直线段代替规划的航线，(x_i,y_i),(x_i+1,y_i+1)为当前目标航行路径航迹段i的两个端点坐标，为目标航行路径；/n步骤4：根据目标航向角ψ_r(k)、当前航向角ψ(k)和航向角速度Δψ(k)，计算目标喷嘴角ω_r(k)；所述目标喷嘴角ω_r(k)的计算过程为：/n /n其中，ω_r(k)为目标喷嘴角，ψ_r(k)为目标航向角，ψ(k)为当前航向角，Δψ(k)为航向角速度，K_P1为1.05，K_I1为0.011，K_D1为-4.1；/n步骤5：根据目标喷嘴角ω_r(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k)，计算输出动作量，所述输出动作量的计算过程为：/n /n其中，ACT为输出动作量，ω_r(k)为目标喷嘴角，ω(k)为当前喷嘴角，Δω(k)为喷嘴角速度，K_P2为2.1，K_I2为0.03，K_D2为-2.5；/n进而控制USV动作，返回步骤2。/n