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

权利要求书

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)流程结束。

2.根据权利要求1所述的一种水下自主式航行器的局部路径规划方法，其特征在于，所述第一公式具体为：

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的最大值和最小值。

3.根据权利要求1所述的一种水下自主式航行器的局部路径规划方法，其特征在于，所述根据所述动态辅助圆和所述当前局部目标航迹有向线段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点为临时目标航点，修改期望航向使ψ_E＝ψ_PE；

6)当动态辅助圆和直线P_iP_i+1相交于点E、F且P在P_iP_i+1上的投影在P_iP_i+1上，且P_i和P_i+1在动态辅助圆外时，选择E点为临时目标航点，修改期望航向使ψ_E＝ψ_PE；

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点为临时目标航点，修改期望航向使

9)当动态辅助圆和直线P_iP_i+1相交且P在P_iP_i+1上的投影在P_iP_i+1的延长线上，选择P_i+1点为临时目标航点，修改期望航向使