一种水下自主式航行器的局部路径规划方法

摘要

本发明公开了一种水下自主式航行器的局部路径规划方法，涉及水下自主式航行器领域，当航迹跟踪偏差d小于等于第二阈值dmax，且大于第一阈值dmin时，通过动态辅助圆和当前局部目标航迹有向线段PiPi+1的位置关系选出临时目标航点，计算出修正以后的期望航向ψE，控制水下自主式航行器沿ψE方向行驶，本方法能够有效实现水下自主式航行器的局部路径规划，其抵抗瞬间干扰的能力和抵抗恒定洋流干扰的能力都强于传统的神经网络法，并且具有时空开销小、计算简便的优点，满足了水下自主式航行器进行海底勘查与测量的需求。

主权项

1.一种水下自主式航行器的局部路径规划方法，其特征在于，所述方法包括以下步骤：（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，执行步骤（11）；（10）所述当前航迹跟踪偏差d大于所述第二阈值d_max，使期望航向ψ_E=ψ_PH，执行步骤（11）；（11）控制所述水下自主式航行器沿ψ_E方向行驶，重新执行步骤（5）；（12）流程结束；所述第一公式具体为：r=f(d),f(d)'≤0,f(d_min)=r_max,f(d_max)=r_min,d_min<d_max其中，r_min，r_max分别表示动态圆半径r的最大值和最小值；其中，所述根据所述动态辅助圆和所述当前局部目标航迹有向线段P_iP_i+1的位置关系选出临时目标航点，计算出修正以后的期望航向ψ_E具体为：1）当动态辅助圆与直线P_iP_i+1相离且P在P_iP_i+1上的投影H在P_i+1P_i的延长线上时，选择P_i作为临时目标航点，修改期望航向使2）当动态辅助圆与直线P_iP_i+1相离且P在P_iP_i+1上的投影H在P_iP_i+1上时，选择H作为临时目标航点，修改期望航向使ψ_E=ψ_PH；3）当动态辅助圆与直线P_iP_i+1相离且P在P_iP_i+1上的投影H在P_iP_i+1的延长线上时，选择P_i+1作为临时目标航点，修改期望航向使4）当动态辅助圆与直线P_iP_i+1相交且P在P_iP_i+1上的投影H在P_i+1P_i的延长线上时，选择P_i为临时目标航点，修改期望航向使5）当动态辅助圆和直线P_iP_i+1相交于点E、F且P在P_iP_i+1上的投影在P_iP_i+1上，且P_i在动态辅助圆内P_i+1在动态辅助圆外时，选择靠近P_i+1的E点为临时目标航点，修改期望航向使6）当动态辅助圆和直线P_iP_i+1相交于点E、F且P在P_iP_i+1上的投影在P_iP_i+1上，且P_i和P_i+1在动态辅助圆外时，选择靠近P_i+1的E点为临时目标航点，修改期望航向使7）当P在P_iP_i+1上的投影在P_iP_i+1上且P_i和P_i+1在动态辅助圆内时，选择P_i+1点为临时目标航点，修改期望航向使8）当动态辅助圆和直线P_iP_i+1相交，且P在P_iP_i+1上的投影在P_iP_i+1上，且P_i+1在动态辅助圆内P_i在动态辅助圆外时，选择P_i+1点为临时目标航点，修改期望航向使${\psi }_E={\psi }_{{\mathrm{PP}}_{i+1}};$9）当动态辅助圆和直线P_iP_i+1相交且P在P_iP_i+1上的投影在P_iP_i+1的延长线上，选择P_i+1点为临时目标航点，修改期望航向使