【CN110006430A】一种航迹规划算法的优化方法【专利】

(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 201910230859.4
(22)申请日 2019.03.26
(71)申请人 智慧航海（青岛）科技有限公司
地址 266200 山东省青岛市即墨区青岛蓝
色硅谷创业中心
(72)发明人 王晓原　夏媛媛　姜雨函　刘亚奇　
柴垒　唐学大　高杰　朱慎超　
(74)专利代理机构 北京易捷胜知识产权代理事
务所(普通合伙) 11613
代理人 齐胜杰
(51)Int.Cl.
G01C 21/20(2006.01)
(54)发明名称一种航迹规划算法的优化方法(57)摘要一种航迹规划算法的优化方法，包括：S1、根据船舶当前位置、目标点位置，采用航迹规划算法获取航迹规划中用于判断航路点的节点集合O 和已经判断航路点的节点集合C；S2、对集合C中每一个节点，采用Dijkstra搜索方法获取每一个节点目标边界框；S3、在遍历集合C中每个节点的邻域搜索下一可行节点时，选取当前节点邻域内处于非障碍物区域且处于当前节点目标边界框内的节点作为下一可行节点；根据下一可行节点，对集合O更新来进行航路点的判断；S4、遍历集合C，获取最优航迹路线。

不仅避免航迹规划算法探索冗余无效的节点，而且避免航迹规划算法探索节点向错误的方向扩展，从而加快寻路的速度，
提高算法运行的效率。

权利要求书2页 说明书5页 附图3页CN 110006430 A 2019.07.12
C N 110006430
A
权　利　要　求　书1/2页CN 110006430 A
1.一种航迹规划算法的优化方法，其特征在于，包括：
S1、根据船舶的当前位置坐标、目标点的位置坐标，采用航迹规划算法获取进行航迹规划中的用于判断航路点的节点集合O和已经判断航路点的节点集合C；
S2、针对集合C中的每一个节点，采用Dijkstra迪杰斯特拉搜索方法获取每一个节点的目标边界框；
S3、在遍历集合C中的每一个节点的邻域搜索下一可行节点时，选取当前节点邻域内处于非障碍物区域且处于当前节点目标边界框内的节点作为下一可行节点；根据下一可行节点，对集合O更新来进行航路点的判断；
S4、遍历集合C，获取最终输出的航迹路线。

2.根据权利要求1所述的航迹规划算法的优化方法，其特征在于，步骤S1中，采用航迹规划算法获取进行航迹规划中的用于判断航路点的节点集合O和已经判断航路点的节点集合C，包括：
根据用于判断航路点的节点集合O和节点代价计算规则，选取集合O中代价最小的节点加入已经判断航路点的节点集合C。

3.根据权利要求2所述的航迹规划算法的优化方法，其特征在于，
所述节点代价计算规则为：F＝G+H，其中，F为节点的路径总代价，G为从船舶的起点到节点的路径耗费，H为从节点到船舶的终点的路径耗费。

4.根据权利要求3所述的航迹规划算法的优化方法，其特征在于，步骤S3中，根据下一可行节点，对集合O更新来进行航路点的判断，包括：
判断下一可行节点是否位于集合O中，如果是，以可行节点G值较小的路径为依据，更新可行节点的父节点、G值与F值，否则，将可行节点加入集合O中，并对该可行节点的父节点进行标记。

5.根据权利要求4所述的航迹规划算法的优化方法，其特征在于，步骤S4包括：
遍历集合C；
以船舶航行的终点为起始，沿着父节点逆向循迹，直至船舶航行的起点；
将逆向循迹的路径作为船舶航行起点至终点的最优路径，进行输出。

6.根据权利要求1所述的航迹规划算法的优化方法，其特征在于，步骤S1中还包括：
采用宽度优先搜索算法判断船舶的当前位置坐标和目标点的位置坐标是否在同一连通区；
如果在，采用航迹规划算法，获取集合O和集合C；
如果不在，重新设置船舶的当前位置坐标和目标点坐标。

7.根据权利要求1所述的航迹规划算法的优化方法，其特征在于，步骤S2中，采用Dijkstra搜索方法获取每一个节点的目标边界框，包括：
对节点进行无目的地Dijkstra搜索，获得节点每个搜索方向上Dijkstra搜索映射中的节点；
根据每个搜索方向上Dijkstra搜索映射中的节点的最小外接矩形，构建每个搜索方向边界框；
选取包含有船舶目标点的搜索方向边界框，作为当前节点的目标边界框。

8.根据权利要求1所述的航迹规划算法的优化方法，其特征在于，所述航迹规划算法包
2。