[发明专利]一种水下自主式航行器的局部路径规划方法

说明书

技术领域

本发明涉及水下自主式航行器领域，特别涉及一种水下自主式航行器的局部路径规划方法。

背景技术

水下自主式航行器在对指定的未知海域进行海底勘查与测量时需要在水平投影内沿指定目标航迹行驶。如图1所示，水下自主式航行器一般采取在水平投影上沿“弓”字形航迹来回扫描的方式进行海底勘查与测量。该航迹可被分解为一系列有向直线段。水下自主式航行器在沿目标航迹行驶的过程中，由于受到洋流的干扰、传感器测量误差与自身的控制误差等，会偏离目标航迹行驶，导致水下自主式航行器不能按照目标航迹进行海底扫描测量，从而影响测量效果。水下自主式航行器必须能够减小和消除这种航迹跟踪偏差，要有自主进行局部路径规划的能力，实现在一定精度范围内的航迹跟踪。这是水下自主式航行器完成水下作业和使命的重要前提。目前水下自主式航行器常用的航迹跟踪方法有神经网络法(参考文献：汤莉，AUV神经网络水平面航迹跟踪控制研究，哈尔滨工程大学硕士学位论文，2009)等。但该方法抗瞬时扰动能力和抗恒定洋流干扰能力不是十分理想：调节距离长，跟踪里程长，计算复杂、算法的时间和空间开销大，难以满足水下自主式航行器进行海底勘查与测量的需求。

发明内容

本发明提供了一种水下自主式航行器的局部路径规划方法，本方法实现了调节距离短，跟踪里程短，计算简单、时间和空间开销小，满足了水下自主式航行器进行海底勘查与测量的需求，详见下文描述：

一种水下自主式航行器的局部路径规划方法，所述方法包括以下步骤：

(1)将全局目标航迹分解为一系列有向直线段P_iP_i+1|i＝1，2，3，...，N，并依次将N个预期航点存储在水下自主式航行器控制系统任务文件里；

(2)将P_i和P_i+1定义为一段航迹的起始点和结束点，P为水下自主式航行器当前位置，P到直线P_iP_i+1的垂线段的长度d＝|PH|定义为当前航迹跟踪偏差，H为垂足，为目标航迹的期望航向角，为未修正的期望航向角，ψ_PH为与期望航向角垂直的航向，ψ_E为修正以后的期望航向角；

(3)从所述水下自主式航行器控制系统任务文件里读取第一个目标航点P_i，i＝1；

(4)若i≥N，则执行步骤(12)，否则，从所述水下自主式航行器控制系统任务文件里读取下一目标航点P_i+1，确定当前局部目标航迹有向线段P_iP_i+1；

(5)从惯性导航系统获得水下自主式航行器当前位置P和当前航向，判断是否已经到达P_i+1，如果是，i＝i+1，执行步骤(4)；如果否，执行步骤(6)；

(6)计算水下自主式航行器当前航迹跟踪偏差d，判断所述当前航迹跟踪偏差d是否小于等于第一阈值d_min，如果是，执行步骤(7)；如果否，执行步骤(8)；

(7)修正以后的期望航向角ψ_E等于期望航向不做调整，执行步骤(12)；

(8)判断所述当前航迹跟踪偏差d是否小于等于第二阈值d_max，且大于第一阈值d_min，如果是，执行步骤(9)；如果否，执行步骤(10)；

(9)根据第一公式计算辅助动态圆半径r，以所述水下自主式航行器当前位置P为圆心、以r为半径建立动态辅助圆，根据所述动态辅助圆和所述当前局部目标航迹有向线段P_iP_i+1的位置关系选出临时目标航点，计算出修正以后的期望航向ψ_E；

(10)所述当前航迹跟踪偏差d大于所述第二阈值d_max，使期望航向ψ_E＝ψ_PH；

(11)控制所述水下自主式航行器沿ψ_E方向行驶，重新执行步骤(5)；

(12)流程结束。

所述第一公式具体为：

r＝f(d)，f(d)≤0，f(d_min)＝r_max，f(d_max)＝r_min，d_min＜d_max