[发明专利]一种基于航路点的UUV转弯速度自适应调整方法

摘要

本发明公开了一种基于航路点的UUV转弯速度自适应调整方法。从使命文本读取UUV的前一航路点、当前航路点、下一航路点以及规划速度，将规划速度作为速度指令输出；计算出当前航路角；计算当前航路的转弯半径；惯性导航仪实时采集UUV的实际位置；判断UUV是否满足启用转弯速度的条件，如果满足条件，计算并输出转弯速度指令；光纤罗经实时采集UUV的实际航向；判断UUV是否到达当前航路点，如果到达当前航路点，判断使命文本中的航路点是否都已到达，如果都到达，任务结束。本发明可以减少UUV转弯时的航路超调，改善航路跟踪效果。

主权项

一种基于航路点的UUV转弯速度自适应调整方法，其特征在于，包括以下几个步骤：步骤一：从使命文本读取UUV的前一航路点pk‑1的位置(xk‑1,yk‑1)、当前航路点pk的位置(xk,yk)、下一航路点pk+1的位置(xk+1,yk+1)以及规划速度信息，将规划速度信息作为UUV速度指令输出，根据读取的信息计算出当前航路角αk；步骤二：根据当前航路角αk，计算UUV当前航路的转弯半径Rk；步骤三：惯性导航仪实时采集UUV的实际位置(xuuv,yuuv)；步骤四：根据UUV的实际位置(xuuv,yuuv)和当前航路点pk的位置(xk,yk)，判断UUV是否满足启用转弯速度调整指令的条件，如果满足条件，进行步骤五，如果不满足条件，重复步骤三～步骤四；步骤五：根据当前航路角αk，计算并输出UUV的转弯速度调整指令；步骤六：光纤罗经实时采集UUV的实际航向ψuuv；步骤七：根据UUV的实际航向ψuuv、实际位置(xuuv,yuuv)、当前航路点pk的位置(xk,yk)、下一航路点pk+1的位置(xk+1,yk+1)以及当前航路的转弯半径Rk，判断UUV是否到达当前航路点，如果到达当前航路点，转步骤八，如果没有到达当前航路点，重复步骤六～步骤七；步骤八：判断使命文本中的航路点是否都已到达，如果都到达，任务结束，如果没有，重复步骤一～步骤八；所述的判断UUV是否满足启用转弯速度调整指令的条件为：|xk-xuuv|2+|yk-yuuv|2<Rk.]]>