[发明专利]基于目标方向约束的路径规划算法

权利要求书

1.基于目标方向约束的路径规划算法，其特征在于，在路径搜索过程中，只保留当前节点与目标节点在同一方向上的可扩展节点，并将这些可扩展节点加入可扩展节点的状态空间，对状态空间中的每一个可扩展节点进行评估，得到估价函数值最小的可扩展节点作为下一个当前节点，重复路径搜索，直到状态空间中估价函数值最小的可扩展节点为目标节点，得到最优路径；

所述状态空间的确定方法为：设当前节点为S，当前节点S的可扩展节点为N个，连线当前节点S与目标节点D，分别作该N个可扩展节点在连线方向上的投影，选取投影恰好落在连线上方向上的节点加入状态空间；

针对每个可扩展节点的评估，采用该可扩展节点的启发式估价函数进行计算，f'(x)函数定义式为：

f'(x)＝g(x)+h'(x)

式中，f'(x)是f(x)的估价函数，其中f(x)是从当前节点S到达目标节点D的实际代价值，g(x)是从当前节点S到可扩展节点X的实际代价值；h'(x)是启发函数，是h(x)的估计函数，其中h(x)是从可扩展节点X到目标节点D的实际的最小代价值，h'(x)小于可扩展节点X到目标节点D的最小代价；

利用上述估价函数f'(x)来衡量状态空间中所有可扩展节点的重要程度，可扩展节点的估价函数的值越小，该可扩展节点对寻路来讲就越重要，因此最终即选取估价函数值最小的可扩展节点，将其作为下一步待扩展的当前节点。

2.根据权利要求1所述的基于目标方向约束的路径规划算法，其特征在于，所述的状态空间的确定的具体操作方法为：设当前节点S的坐标为(x₁，y₁，z₁)，其可扩展节点X的坐标为(x₂，y₂，z₂)，目标节点D的坐标为(x₃，y₃，z₃)，分别连线SX、SD，则

|SX|²＝(x₁-x₂)²+(y₁-y₂)²+(z₁-z₂)²

|SD|²＝(x₁-x₃)²+(y₁-y₃)²+(z₁-z₃)²

|XD|²＝(x₂-x₃)²+(y₂-y₃)²+(z₂-z₃)²

$\cos XSD=\frac{|SX{|}^2+|SD{|}^2-|XD{|}^2}{2\·\left|SX\right|\·\left|SD\right|}$

判断cosXSD值的大小：

(1)如果cosXSD＜0，则可扩展节点X在连线SD上的投影在有向线段SD的反向延长线上，则舍弃该可扩展节点X；

(2)如果cosXSD≥0，则可扩展节点X在连线SD上的投影恰好在有向线段SD上，保留符合该条件的可扩展节点X，并将所有的可扩展节点X形成当前节点S的可扩展节点的状态空间。