[发明专利]一种用于AUV自主对接潜标的水平航路解算方法

摘要

本发明提供了一种用于AUV自主对接潜标的水平航路解算方法，涉及水下航行器领域，本发明依据声传感器探测出的潜标相对AUV的航向角信息，采用追踪法向潜标航行得到航向指令，在一获悉潜标开口方位后，依据AUV相对于潜标所处态势，确定AUV的航向调整区域，依据航向调整区域，执行对应的航向指令，依据声传感器探测出的潜标相对AUV的航向角信息，采用追踪法向潜标航行，直至AUV与潜标完成对接；本发明本由于采用了航路分段细分的技术方案，综合考虑了AUV探测能力及航行性能，流程简单明确，易控制实现，较好的解决了AUV水下自主对接潜标的航路规划技术问题。

主权项

一种用于AUV自主对接潜标的水平航路解算方法，其特征在于包括下述步骤：步骤一：依据声传感器探测出的潜标相对AUV的航向角信息，AUV采用追踪法向潜标航行，航向指令如下：ψ′＝ψ+Δq                           (1)式(1)中，ψ′表示下一时间步的AUV航向指令，ψ为当前时间步的AUV航向角，Δq表示潜标相对AUV纵轴的夹角；AUV依据式(1)指令靠近潜标，直至距潜标距离小于L1时，转步骤二，其中，L1为AUV声传感器可分辨出潜标开口方位的最大距离值；步骤二：AUV在步骤一获悉潜标开口方位后，依据AUV相对于潜标所处态势，确定AUV的航向调整区域，具体判定原则如下：其中，Δψ=(ψ-(ψH+π)|-ππ)-ππψ1=Q4ψ2=π-q1-arcsin(L1sin(q1)L2)L2=max(2RtanQ2,8vT)---(3)]]>式(2)和(3)中，Δψ表示AUV相对于潜标开口方位的水平态势，ψ1表示AUV处于相向区域的界限值，ψ2表示AUV处于背向区域的界限值，ψH表示潜标开口方位，Q表示AUV声传感器探测范围扇面角，q1表示AUV避免碰撞潜标的最小提前角，L2表示保证AUV可实现与潜标精确对接的最短直线距离，R表示AUV的回转半径，v表示AUV的航速，T表示AUV的单次操舵时间周期；步骤三：依据步骤二中判定的AUV所处的航向调整区域，执行对应的航向指令，具体对应三种情况：(1)当AUV处于相向区域时，对应航向指令如下：ψ1′=ψ+sign(Δψ)Q4---(4)]]>Δt1=L1sin(Δψ)sin(π-Δψ-Q4)v---(5)]]>其中，ψ1′表示相向区域对应的航向指令；Δt1表示ψ1′航向指令的持续时间，AUV按式(4)定义的航向指令执行时间Δt1后，即转为步骤四；(2)当AUV处于侧向区域时，对应航向指令如下：ψ21′=ψ+sign(Δψ)arcsin(L2sin(|Δψ|)L12+L22-2L1L2cos(|Δψ|))---(6)]]>Δt21=L12+L22-2L1L2cos(|Δψ|)v---(7)]]>其中，ψ21′表示侧向区域对应的第一次航向指令；Δt21表示ψ21′航向指令的持续时间；AUV按式(6)定义的航向指令执行时间Δt21后，转为执行航向指令如下：ψ22′=ψ-sign(Δψ)(π-arcsin(L12+L22-2L1L2cos(|Δψ|)L1sin(|Δψ|)))---(8)]]>其中，ψ22′表示侧向区域对应的第二次航向指令，AUV按式(8)航向指令将航向角ψ调整到ψ22′后，转到步骤四；(3)当AUV处于背向区域时，对应航向指令如下：ψ31′＝ψ+sign(Δψ)q1                        (9)Δt31=L1v---(10)]]>AUV按式(9)定义的航向指令执行时间Δt31后，转为执行航向指令如下：ψ32′＝ψH                              (11)Δt32=L2v---(12)]]>AUV按式(11)定义的航向指令执行时间Δt32后，转为执行航向指令如下：ψ33′＝‑ψH                           (13)AUV按式(13)航向指令将航向角ψ调整到ψ33′后，转到步骤四；步骤四：依据声传感器探测出的潜标相对AUV的航向角信息，AUV采用追踪法向潜标航行，航向指令同式(1)，直至AUV与潜标完成对接。