CN110398250A - 一种无人艇全局路径规划方法 - Google Patents

Abstract

本发明公开了一种无人艇全局路径规划方法。主要步骤包括：(1)获取无人艇运动状态信息和环境感知信息；(2)建立感知环境模型；(3)采用K近邻学习算法对环境栅格进行危险度预测；(4)采用改进A*算法进行路径搜索。本发明针对水面无人艇在实际航行过程中的安全性要求，在建立路径规划环境模型时，采用K近邻算法对水面无人艇所处环境中的危险区域进行预测，同时，在采用A*算法进行路径搜索时，在其估价函数中引入安全代价，确保规划路径的安全性。

Description

一种无人艇全局路径规划方法

技术领域

本发明涉及水面无人艇路径规划领域，具体涉及一种无人艇全局路径规划方法，特别是一种基于K近邻学习算法和A*算法的无人艇全局路径规划方法。

背景技术

无人水面艇是一个拥有自主运行能力的小型水面任务平台，主要用于执行危险以及不适于有人船只执行的任务。随着无人艇的快速发展，其在军事和民用领域都展现出了良好的发展前景。水面无人艇的自主路径规划能力是水面无人艇智能水平的重要体现，也是水面无人艇实现自主航行的重要环节。

A*算法是一种广泛使用的启发式路径规划算法。但是传统A*算法在进行路径规划的时候只考虑了路径的长短，并没有考虑水面无人艇在航行过程中的其他因素。由于海上风浪流等不确定因素的影响，水面无人艇在海洋航行时的安全性会受到很大影响，因此，在进行水面无人艇自主路径规划时，考虑其航行的安全性尤为重要。K近邻(k-NearestNeighbor,简称KNN)学习是一种常用的机器学习方法，其工作机制非常简单，算法便于实现，通过K近邻学习算法对水面无人艇所处环境的危险程度进行预测，在路径规划前对当前环境状态有一个较好的估计，有利于路径规划的安全性。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种充分考虑水面无人艇规划路径的安全性的无人艇全局路径规划方法，在建立路径规划环境模型时，采用K近邻算法对水面无人艇所处环境中的危险区域进行预测，同时，在采用A*算法进行路径搜索时，在其估价函数中引入安全代价，确保规划路径的安全性。

为解决上述技术问题，本发明的一种无人艇全局路径规划方法，包括以下步骤：

S1：获取无人艇运动状态信息和环境感知信息，其中运动状态信息包括：无人艇当前位置坐标(s_x,s_y)、速度u、航向角姿态角，环境感知信息包括障碍物位置坐标；

S2：采用栅格法建立环境模型，包括：

S2.1：选定无人艇作业区域范围；

S2.2：对无人艇作业区域进行栅格化，取栅格边长l＝u·Δt，其中，u为无人艇的平均航速，Δt为无人艇的运动控制节拍；

S2.3：对S2.2中的作业区域的栅格进行编码；

S2.4：将通过感知设备得到的作业区域内的障碍物元素标识到S2.3编码后的栅格中，包含障碍物的栅格为障碍物栅格，不包含障碍物的栅格为自由栅格，将障碍物栅格标记为0，将自由栅格标记为1；

S3：对每一个自由栅格采用K近邻学习算法进行危险度预测，预测结果为危险栅格的自由栅格标记为2；

S4：采用A*算法进行路径搜索，包括：

S4.1：建立OPEN列表和CLOSE列表，把起始栅格s添加到OPEN列表；

S4.2：寻找OPEN列表中估价函数f(m)最低的栅格，记为当前栅格i，把该栅格i移动到CLOSE列表；

S4.3：扩展当前栅格i，得到当前栅格i的全部相邻栅格，对每个相邻栅格执行以下操作：

S4.3.1:如果相邻栅格m为障碍物栅格或者已经在CLOSE列表中，略过它；否则执行接下来的步骤：

S4.3.2:如果相邻栅格m不在OPEN列表中，则把相邻栅格m添加到OPEN列表中，把当前栅格i作为相邻栅格m的父栅格，记录相邻栅格m的估价函数值f(m)、路径代价与安全代价之和G(m)和从当前栅格m到终点栅格最小代价的估计h(m)；

S4.3.3:如果相邻栅格m已经在OPEN列表中，如果满足：

G(m)＜G(i)+L(i,m)

则将相邻栅格m的父节点改成当前栅格i，并且重新计算相邻栅格m的G(m)和f(m)值；其中，L(i,m)表示从栅格i到相邻栅格m的实际路径代价；

如果栅格i与相邻栅格m是直连接，即满足：

|i_x-m_x|+|i_y-m_y|＝1

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为一个栅格长度；

如果栅格i与其相邻栅格m是斜连接，即满足：

|i_x-m_x|+|i_y-m_y|＝2

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为个栅格长度；

S4.4：重复执行S4.2-S4.3直至目标节点t添加进了CLOSE列表或者OPEN列表已经空了；当目标节点t添加进了CLOSE列表，则表明最优路径被找到，从目标栅格开始依次回溯父栅格，直到起始栅格，即得到从起始栅格到目标栅格的最优路径；没有找到目标节点，OPEN列表已经空了，表明路径不存在。

本发明还包括：

1.S1中运动状态信息通过GPS和电子罗盘获取，所述环境感知信息通过激光雷达获取。

2.S3中危险度预测包括：

S3.1:针对每一个预测栅格(x_p,y_p)，选取与预测栅格(x_p,y_p)的距离小于等于K个栅格长度的栅格组成决策集合，其中K为一个设定的正整数；

S3.2:在决策集中计算自由栅格数量与障碍物栅格数量的比例ε；如果ε＞p，则预测栅格为完全自由栅格；如果ε≤p，则预测栅格为危险栅格，其中p为预测系数，0≤p≤1；

3.S4.2中估价函数f(m)满足：

f(m)＝(1-ω)g(m)+ωs(m)+h(m)

式中，f(m)是栅格m的估价函数；g(m)是起始栅格到栅格m的路径代价；h(m)为启发函数，是从栅格m到终点栅格最小代价的估计；s(m)表示起点到栅格m的安全代价，依据对栅格安全评估后的结果确定每一个栅格的安全代价值r(m)，如果m为完全自由栅格则

r(m)＝0，如果m为危险栅格则r(m)＝k，k为设定的常数，s(m)为从起始栅格到栅格m的每个路径节点安全代价值r(m)的累计和；ω为安全性与路径长短的代价比重，0≤ω＜1，ω越接近1表示安全性越重要；

S4.2所述路径代价与安全代价之和G(m)表示从起点栅格到栅格m的实际代价，即路径代价与安全代价之和：

G(m)＝(1-ω)g(m)+ωs(m)

S4.2所述从当前栅格m到终点栅格最小代价的估计h(m)在栅格地图中采用当前栅格到目标栅格的曼哈顿距离：

h(m)＝|m_x-t_x|+|m_y-t_y|

式中，m_x表示当前栅格m在栅格环境模型中横向坐标，m_y表示当前栅格m在栅格环境模型中纵向坐标，t_x表示目标栅格t在栅格环境模型中横向坐标，t_y表示目标栅格t在栅格环境中纵向坐标。

本发明有益效果：本方法考虑水面无人艇在实际航行过程中的安全性，通过K近邻学习算法对当前环境模型中的危险区域进行预测，对环境模型中危险区域进行标记，同时，对A*算法进行改进，在估价函数中增加了对路径安全代价的考虑，提高了A*算法规划路径的安全性。对于水面无人艇在实际情况自主航行的安全性有重要作用。

附图说明

图1为本发明的水面无人艇全局路径规划整体算法流程图；

图2为本发明的一种基于K近邻学习算法和A*算法的无人艇全局路径规划方法的整体步骤流程示意图。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明

本发明提出的一种基于K近邻学习算法和A*算法的无人艇全局路径规划方法，如图1所示，具体有以下几个步骤：

步骤1：获取无人艇运动状态信息和环境感知信息。

(1.1)通过串口接收GPS、电子罗盘(TCM)的数据信息，通过激光雷达获取环境感知信息。

(1.2)分别根据相应传感器的通讯协议，对接收到的数据串进行校验、解码，获得无人水面艇当前的位置坐标(s_x,s_y)、速度u、航向角姿态角等运动状态信息，以及障碍物位置等环境信息。

步骤2：建立感知环境模型，对于环境的表示，采用便捷高效的栅格法。

(2.1)依据水面无人艇作业区域建立环境模型范围。环境模型范围为以起点s与目标点t为对角线并将该对角线的长度沿两端方向分别扩展10％以后形成的矩形区域。

(2.2)对水面无人艇当前的作业区域进行栅格化，栅格的边长依据无人艇的运动能力而定，本方法取栅格边长l＝u·Δt，u为无人艇的平均航速，Δt为无人艇的运动控制节拍。

(2.3)对作业区域的栅格进行编码。采用直角坐标法对栅格环境进行编码。

(2.4)将每个障碍物元素标识到感知栅格环境中，为了便于算法实现，将栅格类型通过数字进行标记。包含障碍物的栅格为障碍物栅格，将该栅格标记为0；完全不包含障碍物的栅格为自由栅格，将该栅格标记为1。

步骤3：对自由栅格采用K近邻学习算法进行危险度预测，将危险栅格进行标记。

(3.1)依据障碍物复杂度设定一个K值，K值表示依据距离当前要预测栅格(x_p,y_p)K范围内的栅格信息进行预测。

(3.2)针对每一个预测栅格(x_p,y_p)，计算它与周围栅格的距离。距离的计算通过欧式距离来实现，欧式距离可以描述两点在n维空间的真实距离(物理距离)。已知两个n维向量x＝(x₁,x₂,x₃,···,x_n)和y＝(y₁,y₂,y₃,···,y_n)，这两个向量的欧式距离公式可以表示为：

(3.3)选取与预测栅格(x_p,y_p)的距离小于等于K的栅格组成决策集合。在决策集中计算自由栅格与障碍物栅格的比例ε。如果ε＞p，则预测栅格为完全自由栅格；如果ε≤p，则预测栅格为危险栅格。

(3.4)将预测后的危险栅格与完全自由栅格进行重新标记。预测后的危险栅格标记为2。

步骤4：采用改进A*算法进行路径搜索。

A*算法是一种启发式搜索算法，主要是通过启发函数来减少搜索的节点数，提高搜索效率。启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。改进A*算法的具体步骤如图2所示。

(4.1)考虑水面无人艇航行的安全性，在估价函数中引入安全代价s(v)，改进后的估价函数可以表示为：

f(v)＝(1-ω)g(v)+ωs(v)+h(v)

式中，f(v)是当前节点v的估价函数；g(v)是起点到当前节点v的路径代价；h(v)是从当前顶点v到终点最小代价的估计。若h(v)＝0，即没有利用全局信息时，A*算法就简化成了Dijkstra算法。h(v)函数的选择取决于路径优化的标准，在选择h(v)时，不能过高估计v的最小代价。s(v)表示起点到节点v的安全代价，依据对节点安全评估后的结果确定每一个节点的安全代价值r(v)，如果v为完全自由栅格则r(v)＝0，如果v为危险栅格则r(v)＝k，k为常数，s(v)为从起始节点到节点v的每个路径节点安全代价值r(v)的累计和。ω为安全性与路径长短的代价比重，0≤ω＜1，ω越接近1表示安全性越重要。在实际中，需要对最短路径和最安全路径进行一定的权衡。令G(v)表示从起点节点到当前节点v的实际代价，即路径代价与安全代价之和：

G(v)＝(1-ω)g(v)+ωs(v)

在栅格地图中，启发函数h(v)多采用当前节点到目标节点的曼哈顿距离。

h(v)＝|v_x-t_x|+|v_y-t_y|

式中，v_x表示当前节点v在栅格地图中横向坐标，v_y表示当前节点v在栅格地图中纵向坐标，t_x表示目标点t在栅格地图中横向坐标，t_y表示目标点t在栅格地图中纵向坐标。

(4.2)建立OPEN列表和CLOSE列表，把起始栅格s添加到OPEN列表。

(4.3)寻找OPEN列表中f值最低的栅格，记为当前栅格i。把该栅格i切换到CLOSE列表。

(4.4)对当前栅格i的每一个相邻的栅格m执行以下操作：

①如果栅格m不可通过或者已经在CLOSE列表中，略过它。反之执行接下来的步骤。

②如果栅格m不在OPEN列表中，把它添加进去。把当前栅格i作为栅格m的父节点。记录节点m的f，G，和h值。

③如果栅格m已经在OPEN列表中，用G值为参考检查新的路径是否更好，即：

G(m)＜G(i)+L(i,m)

其中，L(i,m)表示从栅格i到栅格m的实际代价。更低的G值意味着更好的路径。如果上式成立，就把栅格m的父节点改成当前栅格i，并且重新计算栅格m的G和f值。

(4.5)重复执行(4.3)-(4.4)直至目标栅格t添加进了CLOSE列表或者OPEN列表已经空了。把目标栅格t添加进了CLOSE列表，最优路径被找到；没有找到目标节点，OPEN列表已经空了，路径不存在。

本发明具体实施方式还包括：

如图1和图2所示包括以下步骤：

(1)获取无人艇运动状态信息和环境感知信息

(1.1)通过串口接收GPS、电子罗盘(TCM)的数据信息，通过激光雷达获取环境感知信息。

(1.2)分别根据相应传感器的通讯协议，对接收到的数据串进行校验、解码，获得无人水面艇当前的位置坐标(s_x,s_y)、速度u、航向角姿态角等运动状态信息，以及障碍物位置等环境信息。

(2)采用栅格法建立环境模型。

(2.1)依据水面无人艇作业区域确定环境模型范围。

(2.2)对无人艇的作业区域进行栅格化，栅格的边长依据无人艇的运动能力而定，本方法取栅格边长l＝u·Δt，u为无人艇的平均航速，Δt为无人艇的运动控制节拍。

(2.3)对作业区域的栅格进行编码。

(2.4)将通过感知设备得到的障碍物元素标识到栅格环境模型中，包含障碍物的栅格为障碍物栅格，完全不包含障碍物的栅格为自由栅格。为了便于算法实现，将栅格类型通过数字进行标记。将障碍物栅格标记为0，将自由栅格标记为1。

(3)对自由栅格采用K近邻学习算法进行危险度预测，将危险栅格进行标记。

(3.1)依据障碍物复杂度设定一个K值，K为一个距离值，表示距离当前要预测栅格(x_p,y_p)的距离小于等于K个栅格长度。依据K个栅格长度范围内的栅格信息对(x_p,y_p)的危险度进行预测。

(3.2)针对每一个预测栅格(x_p,y_p)，计算它与周围相近栅格的距离，距离的计算通过欧式距离来实现。，

(3.3)选取与预测栅格(x_p,y_p)的距离小于等于K的栅格组成决策集合。在决策集中计算自由栅格与障碍物栅格的比例ε。如果ε＞p，则预测栅格为完全自由栅格；如果ε≤p，则预测栅格为危险栅格，其中p为预测系数，0≤p≤1。

(3.4)将预测后的危险栅格与完全自由栅格进行重新标记。预测后的危险栅格标记为2。

(4)采用改进A*算法进行路径搜索。A*算法是一种启发式搜索算法，主要是通过启发函数来减少搜索的节点数，提高搜索效率。启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。

(4.1)考虑水面无人艇航行的安全性，在估价函数中引入安全代价s(v)，改进后的估价函数可以表示为：

f(v)＝(1-ω)g(v)+ωs(v)+h(v)

式中，f(v)是当前栅格v的估价函数；g(v)是起始栅格到当前栅格v的路径代价；h(v)为启发函数，是从当前栅格v到终点栅格最小代价的估计。s(v)表示起点到节点v的安全代价，依据对节点安全评估后的结果确定每一个节点的安全代价值r(v)，如果v为完全自由栅格则r(v)＝0，如果v为危险栅格则r(v)＝k，k为常数，s(v)为从起始节点到节点v的每个路径节点安全代价值r(v)的累计和。。ω为安全性与路径长短的代价比重，0≤ω＜1，ω越接近1表示安全性越重要。令G(v)表示从起点节点到当前节点v的实际代价，即路径代价与安全代价之和：

G(v)＝(1-ω)g(v)+ωs(v)

在栅格地图中，启发函数h(v)多采用当前栅格到目标栅格的曼哈顿距离。

h(v)＝|v_x-t_x|+|v_y-t_y|

式中，v_x表示当前栅格v在栅格环境模型中横向坐标，v_y表示当前栅格v在栅格环境模型中纵向坐标，t_x表示目标栅格t在栅格环境模型中横向坐标，t_y表示目标栅格t在栅格环境中纵向坐标。

(4.2)建立OPEN列表和CLOSE列表，把起始栅格s添加到OPEN列表。

(4.3)寻找OPEN列表中f值最低的栅格，记为当前栅格i。把该栅格i移动到CLOSE列表。

(4.4)扩展当前栅格i，得到其相邻栅格m，对栅格m执行以下操作：

①如果栅格m为障碍物栅格或者已经在CLOSE列表中，略过它。反之执行接下来的步骤。

②如果栅格m不在OPEN列表中，则把它添加到OPEN列表中。把当前栅格i作为栅格m的父栅格。记录栅格m的f，G，和h值。

③如果栅格m已经在OPEN列表中，以G值为参考，检查新的路径是否更好，如果下式成立，则将节点m的父节点改成当前节点i，并且重新计算节点m的G和f值。

G(m)＜G(i)+L(i,m)

其中，L(i,m)表示从栅格i到其相邻栅格m的实际路径代价，如果栅格i与其相邻栅格m是直连接，即

|i_x-m_x|+|i_y-m_y|＝1

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为一个栅格单位长度；如果栅格i与其相邻栅格m是斜连接，即

|i_x-m_x|+|i_y-m_y|＝2

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为个栅格单位长度。

(4.5)重复执行(4.3)-(4.4)直至目标节点t添加进了CLOSE列表或者OPEN列表已经空了。把目标节点t添加进了CLOSE列表，表明最优路径被找到，从目标栅格开始依次回溯父栅格，直到起始栅格，即得到从起始栅格到目标栅格的最优路径；没有找到目标节点，OPEN列表已经空了，表明路径不存在。

Claims (4)

1.一种无人艇全局路径规划方法，其特征在于，包括以下步骤：

S1：获取无人艇运动状态信息和环境感知信息，其中运动状态信息包括：无人艇当前位置坐标(s_x,s_y)、速度u、航向角姿态角，环境感知信息包括障碍物位置坐标；

S2：采用栅格法建立环境模型，包括：

S2.1：选定无人艇作业区域范围；

S2.2：对无人艇作业区域进行栅格化，取栅格边长l＝u·Δt，其中，u为无人艇的平均航速，Δt为无人艇的运动控制节拍；

S2.3：对S2.2中的作业区域的栅格进行编码；

S2.4：将通过感知设备得到的作业区域内的障碍物元素标识到S2.3编码后的栅格中，包含障碍物的栅格为障碍物栅格，不包含障碍物的栅格为自由栅格，将障碍物栅格标记为0，将自由栅格标记为1；

S3：对每一个自由栅格采用K近邻学习算法进行危险度预测，预测结果为危险栅格的自由栅格标记为2；

S4：采用A*算法进行路径搜索，包括：

S4.1：建立OPEN列表和CLOSE列表，把起始栅格s添加到OPEN列表；

S4.2：寻找OPEN列表中估价函数f(m)最低的栅格，记为当前栅格i，把该栅格i移动到CLOSE列表；

S4.3：扩展当前栅格i，得到当前栅格i的全部相邻栅格，对每个相邻栅格执行以下操作：

S4.3.1:如果相邻栅格m为障碍物栅格或者已经在CLOSE列表中，略过它；否则执行接下来的步骤：

S4.3.2:如果相邻栅格m不在OPEN列表中，则把相邻栅格m添加到OPEN列表中，把当前栅格i作为相邻栅格m的父栅格，记录相邻栅格m的估价函数值f(m)、路径代价与安全代价之和G(m)和从当前栅格m到终点栅格最小代价的估计h(m)；

S4.3.3:如果相邻栅格m已经在OPEN列表中，如果满足：

G(m)＜G(i)+L(i,m)

则将相邻栅格m的父节点改成当前栅格i，并且重新计算相邻栅格m的G(m)和f(m)值；其中，L(i,m)表示从栅格i到相邻栅格m的实际路径代价；

如果栅格i与相邻栅格m是直连接，即满足：

|i_x-m_x|+|i_y-m_y|＝1

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为一个栅格长度；

如果栅格i与其相邻栅格m是斜连接，即满足：

|i_x-m_x|+|i_y-m_y|＝2

式中，i_x表示栅格i在栅格环境模型中的横向坐标，i_y表示栅格i在栅格环境模型中的纵向坐标，m_x表示栅格m在栅格环境模型中的横向坐标，m_y表示栅格m在栅格环境模型中的纵向坐标，则L(i,m)为个栅格长度；

S4.4：重复执行S4.2-S4.3直至目标节点t添加进了CLOSE列表或者OPEN列表已经空了；当目标节点t添加进了CLOSE列表，则表明最优路径被找到，从目标栅格开始依次回溯父栅格，直到起始栅格，即得到从起始栅格到目标栅格的最优路径；没有找到目标节点，OPEN列表已经空了，表明路径不存在。

2.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：S1中所述运动状态信息通过GPS和电子罗盘获取，所述环境感知信息通过激光雷达获取。

3.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：S3中危险度预测包括：

S3.1:针对每一个预测栅格(x_p,y_p)，选取与预测栅格(x_p,y_p)的距离小于等于K个栅格长度的栅格组成决策集合，其中K为一个设定的正整数；

S3.2:在决策集中计算自由栅格数量与障碍物栅格数量的比例ε；如果ε＞p，则预测栅格为完全自由栅格；如果ε≤p，则预测栅格为危险栅格，其中p为预测系数，0≤p≤1。

4.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：S4.2所述估价函数f(m)满足：

f(m)＝(1-ω)g(m)+ωs(m)+h(m)

式中，f(m)是栅格m的估价函数；g(m)是起始栅格到栅格m的路径代价；h(m)为启发函数，是从栅格m到终点栅格最小代价的估计；s(m)表示起点到栅格m的安全代价，依据对栅格安全评估后的结果确定每一个栅格的安全代价值r(m)，如果m为完全自由栅格则r(m)＝0，如果m为危险栅格则r(m)＝k，k为设定的常数，s(m)为从起始栅格到栅格m的每个路径节点安全代价值r(m)的累计和；ω为安全性与路径长短的代价比重，0≤ω＜1，ω越接近1表示安全性越重要；

S4.2所述路径代价与安全代价之和G(m)表示从起点栅格到栅格m的实际代价，即路径代价与安全代价之和：

G(m)＝(1-ω)g(m)+ωs(m)

S4.2所述从当前栅格m到终点栅格最小代价的估计h(m)在栅格地图中采用当前栅格到目标栅格的曼哈顿距离：

h(m)＝|m_x-t_x|+|m_y-t_y|

式中，m_x表示当前栅格m在栅格环境模型中横向坐标，m_y表示当前栅格m在栅格环境模型中纵向坐标，t_x表示目标栅格t在栅格环境模型中横向坐标，t_y表示目标栅格t在栅格环境中纵向坐标。