[发明专利]一种基于惩罚权重的路径规划方法、导航格以及栅格地图

权利要求书

1.一种基于惩罚权重的路径规划方法，其特征在于，包括以下步骤:

S1、在铺设有多个导航格的栅格地图上选取起始格和终点格，并将所述起始格放入计算集中；每一所述导航格内设置有坐标信息、主方向信息和权重信息；

S2、在所述计算集不为空时，选取所述计算集中的一个导航格作为起始计算格，根据所述起始计算格的主方向信息对应的可行方向，获得所述起始计算格的可通行邻近格集；所述可通行邻近格集包括所述起始计算格在所述可行方向上的相邻的导航格；

S3、在所述可通行邻近格集中选取一个导航格作为进阶计算格，将所述起始计算格移出所述计算集；并获取所述进阶计算格的路径代价，并判断：

S31、当所述进阶计算格的路径代价为零时，保存所述进阶计算格的路径代价等于当前代价，保存所述进阶计算格的前一连接格为所述起始计算格，若所述进阶计算格不在所述计算集中将所述进阶计算格放入所述计算集；所述当前代价为所述起始计算格的路径代价+所述进阶计算格的权重信息；当所述起始计算格为所述起始格时，所述起始计算格的路径代价等于所述起始计算格的权重信息；

S32、当所述进阶计算格的路径代价大于零且大于所述当前代价时，保存所述进阶计算格的路径代价等于所述当前代价，保存所述进阶计算格的前一连接格为所述起始计算格；

S33、当所述进阶计算格的路径代价大于零且小于所述当前代价时，所述进阶计算格的路径代价和前一连接格均保持不变；

S4、重复所述步骤S2-S33，直到所述计算集为空；

S5、从所述栅格地图上的所述终点格开始，依次获取当前格的所述前一连接格，直到得到整条规划路径。

2.如权利要求1所述的基于惩罚权重的路径规划方法，其特征在于，所述主方向信息可以为多个方向中的一个方向；所述主方向信息对应的可行方向为所述多个方向中除了与所述主方向信息相反的方向以外的其他所有方向。

3.如权利要求2所述的基于惩罚权重的路径规划方法，其特征在于，所述多个方向为上、下、左、右四个方向。

4.如权利要求1所述的基于惩罚权重的路径规划方法，其特征在于，所述步骤S31的所述当前代价为所述起始计算格的路径代价+所述进阶计算格的权重信息+行驶权重；

当所述起始计算格的前一连接格所在的方向与所述进阶计算格相对于所述起始计算格所在的方向不同时，判断为转弯行驶，所述行驶权重等于第一转弯权重；

当所述起始计算格的前一连接格所在的方向与所述进阶计算格相对于所述起始计算格所在的方向相同时，判断为直行行驶，所述行驶权重等于第一直行权重；

所述第一转弯权重大于所述第一直行权重。

5.如权利要求1所述的基于惩罚权重的路径规划方法，其特征在于，所述步骤S1之前还包括以下步骤：

预先设定所述导航格的所述权重信息。

6.如权利要求5所述的基于惩罚权重的路径规划方法，其特征在于，所述导航格在满足避开规则时，设定所述权重信息为避开权重，否则，设定所述权重信息为常规权重；

所述避开规则为所述导航格被占用或所述导航格处于非行驶区域；

所述避开权重大于所述常规权重。

7.如权利要求1所述的基于惩罚权重的路径规划方法，其特征在于，所述步骤S5之后还包括以下步骤：

S6、对处于所述规划路径上的导航格的权重信息进行更新：

当所述导航格的前一连接格所在的方向与所述导航格的下一连接格相对于所述导航格所在的方向不同时，判断所述导航格为转弯格，所述导航格的权重信息等于原先的权重信息加上第二转弯权重；

当所述导航格的前一连接格所在的方向与所述导航格的下一连接格相对于所述导航格所在的方向相同时，判断所述导航格为直行格，所述导航格的权重信息等于原先的权重信息加上第二直行权重；

所述第二转弯权重大于所述第二直行权重。