CN104950883A - 一种基于距离网格地图的移动机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于距离网格地图的移动机器人路径规划方法，在机器人运动过程中采用活动窗口的方法进行前沿网格的选择并进行距离网格地图更新，在更新过程中采用队列管理的方法对待更新的前沿网格进行管理，在基于距离网格地图的路径规划方法中采用A*搜索算法对移动机器人控制模型进行更新。该方法的实施对于未知环境下拓扑地图的生成，以及未知环境下的路径规划等高级任务的实现具有重要的基础意义。

Description

一种基于距离网格地图的移动机器人路径规划方法

技术领域

本发明涉及移动机器人路径规划领域，具体是一种基于距离网格地图的移动机器人路径规划方法。

背景技术

在移动机器人短短几十年的发展过程中，避障及路径规划算法一直是学者们研究的热门问题，该问题也是智能移动移动机器人自主能力的关键问题之一。如何在未知不确定环境中无碰撞的进行路径规划是高度自主化的行为，每年在该方向的论文与算法都非常多。目前一般采取全局路径规划和局部避障规避相结合的方法实现移动移动机器人的自主导航。其中比较流行的避障算法有基于边缘检测法(Edge-Detection Method)、势场法(PotentialField Methods)、虚拟力场法(Virtual Force Field Method)、矢量场直方图法(Vector FieldHistogram Method)以及后续改进的VFH+、VFH等方法，这些方法都是局部避障算法，除了VFH*算法在对后续路径的预测加入A*搜索来进行路径选择优化外，其余算法都不对全局的路径进行规划。

距离网格地图表示的是单元格与障碍距离的最小值，通常用于移动移动机器人的避障预警、非完备移动移动机器人的路径规划以及定位等问题。在静止的环境中，距离网格地图变动不大，所需要更新维护的计算成本几乎为零。然而，移动移动机器人所处的环境往往是一个动态的环境，障碍物的变动，存在其他移动物体，甚至是传感器噪声，都需要在改变概率网格地图的基础上来改变距离网格地图。

传统的基于距离网格地图的路径规划方法存在的缺陷：

(1)传统距离网格地图更新耗费计算量大，地图维护占用时间长，难以动态更新；

(2)路径规划时使用启发式搜索算法，在搜索时由于不考虑移动移动机器人的运动限制和移动机器人自身的尺寸，使得所规划路径在局部与障碍物距离过小、在部分转向之处(路径拐点)为锐角，无法直接使用规划出的路径来控制移动移动机器人的运动。

发明内容

本发明的目的在于提供一种基于距离网格地图的移动机器人路径规划方法，解决了基于距离网格地图的地图动态更新管理问题和未知地图下的地图构建及路径规划问题。

为实现上述目的，本发明提供如下技术方案：

一种基于距离网格地图的移动机器人路径规划方法，包含以下步骤：

步骤1：设定初始概率网格地图和距离网格地图为空地图；控制移动机器人在目标区域内移动，并通过传感器采集目标区域的地形信息，构建以移动机器人为中心的活动窗口，所述的活动窗口对应存储在移动机器人内的初始距离网格地图中的一个子区域，选取与活动窗口对应的目标区域中的部分地形信息对所述的子区域进行更新，直至活动窗口遍历整个初始距离网格地图，得到更新后的距离网格地图；

步骤2：在更新后的距离网格地图上，确定移动机器人的起点和终点，将每个网格的中心作为节点，采用A*搜索算法并根据移动机器人在每个节点的姿态角进行节点展开，确定移动机器人由起点运动至终点所有经过的节点，连线起点、所有经过的节点和终点，规划出移动机器人的路径。

作为本发明进一步的方案：步骤1中，距离网格地图更新过程中的活动窗口和前沿网格的选择方法；

为了分散距离网格地图的更新计算量，对距离网格地图的更新计算被限制在了活动窗口之内，活动窗口过大，会导致重复的、不必要的更新队列插入操作，而活动窗口选择过小，更新向量生成器所产生的某个更新向量(x，y，d)的更新坐标x，y超出活动窗口的范围，由此将会使得对应距离网格地图上没有体现数据的最近更新，导致距离网格地图与概率网格地图的不一致。

一般来讲在使用声纳等不精确数据时，数据值越大，即意味着离机器人越远，其可靠度越差，所以更新向量生成器在使用声纳数据时，将会接受一定范围内的数据，超过最远距离S_max的数据将会被丢弃，以保证数据的可靠性。所以在活动窗口的高和宽应该满足下式：

$\left\{\begin{array}{c}{H}_w>\mathrm{INT}(2\·S_{\max })\\ W_w>\mathrm{INT}(2\·S_{\max })\end{array}\right.$

式中：H_w为活动窗口的高，W_w为活动窗口的宽，S_max为保持声呐数据可靠的最远距离，INT()为取整操作。

由于移动机器人的数据获取与控制并非连续的过程，而是一系列离散过程所组成的。传感器数据更新间隔一个固定的时间Δt_s，在一个或者几个Δt_s内完成移动机器人的地图构建、导航计算等工作，所以虽然在现实中，宏观上移动机器人的位置是连续变化的过程，但是对于导航计算的算法程序而言，改变是离散的、跳变的，无论移动机器人沿着哪一条路径由一点到达另一点，所引起的变化对局部的距离网格地图而言，首先需要根据活动窗口的移动情况，计算出需要进行距离网格地图更新计算的“前沿”(待更新距离网格)，然后再采用队列方法进行地图更新。

作为本发明进一步的方案：步骤1中，将时间为t-1时移动机器人的中心坐标记为(x_p,y_p)，当前计算更新时间为t时的移动机器人中心坐标记为(x_r,y_r)，移动机器人坐标变化量为(Δx,Δy)，Δi和Δj分别为活动窗口中心坐标行列序号的变化量，为整数，距离网格单元格的边长为S_c，以单元格数目来表示的活动窗口的宽和高分别为W_n、H_n。

Δx和Δy按照下式计算：

$\left\{\begin{array}{c}\mathrm{\Δx}=x_r-x_p\\ \mathrm{\Δy}=y_r-y_p\end{array}\right.$

Δi和Δj按照下式计算：

$\left\{\begin{array}{c}\mathrm{\Δi}=\mathrm{INT}\left(\frac{\left|\mathrm{\Δx}\right|}{S_c}\right)\\ \mathrm{\Δj}=\mathrm{INT}\left(\frac{\left|\mathrm{\Δy}\right|}{S_c}\right)\end{array}\right.$

分为以下四种情况计算“前沿”网格的行列序号范围：

(1)如果Δi、Δj全为0，则不进行活动窗口刷新计算；

(2)如果Δi＝0，Δj≠0，则不存在纵向“前沿”网格，仅需计算出横向“前沿”网格的序号范围；

(3)如果Δi≠0，Δj＝0，则不存在横向“前沿”网格，仅需计算出纵向“前沿”网格的序号范围；

(4)如果Δi、Δj均非零值，则需要分别计算出横向和纵向两个方向上的“前沿”网格的序号范围。

作为本发明进一步的方案：步骤1中，距离网格地图障碍物增加及删除时的距离网格地图动态更新方法；

在障碍增加时对距离网格地图的更新需要设定一个障碍增加更新队列Qu，其中存放的是距离网格地图更新单元，更新单元包含单元格的行列序号(i,j)。当距离网格地图中的某个单元格被更新时，根据该单元格的信息生成对应的增加更新单元，并将其加入障碍增加更新队列Qu。障碍增加更新算法从障碍增加更新队列Qu取出一个更新单元，并将其从障碍增加更新队列Qu中删除，根据增加更新单元提供的行列序号(i,j)，尝试更新(i,j)周围的八个单元格，并将新生成的更新单元加入障碍增加更新队列Qu。只要障碍增加更新队列Qu不为空，障碍增加更新算法将一直继续下去；

在进行清除时同样需要使用一个障碍清除更新队列Qe，根据被清除的单元格信息生成清除更新单元，并将其加入障碍清除更新队列Qe，障碍清除更新算法从障碍清除更新队列Qe中取出一个清除更新单元，按照与障碍增加更新算法类似的迭代方式，对所有与清除更新单元的单元格处相关联的单元格进行清除操作，直到到达与之不相关的单元格，之后按照“边缘”的信息生成更新单元，并将其加入障碍增加更新队列Qu，并调用障碍增加更新算法对距离网格地图进行更新。

作为本发明进一步的方案：步骤1中，距离网格地图更新队列的优化方法。

在实际应用中，距离传感器数据的更新往往是一组多个，会导致概率网格地图Mc中数个单元产生超过或低于阈值Tc的变化，这种变化使得对应的距离网格地图进行更新操作，体现在障碍增加更新队列Qu上，就是队列中同时加入了数个元素，随着更新操作的进行，一些重复的元素将被添加进来。因此，在设计更新队列排序函数的时候需要考虑位置参数，同时需要纳入考虑的是更新的层级L_u，所谓的L_u反映的是网格单元展开的层次，L_u越大的单元距离障碍的距离必然比较大，应该放到队列的尾部，越晚被更新。因此需要设计一个考虑层级L_u和位置的排序估值函数，对该函数的表述如下式所示：

P(i，j)＝L_u·W_g·H_g+W_g·j+i

在上式中W_g和H_g为距离网格地图的高度和宽度，i和j是待更新单元的行序号和列序号，该函数将不同的待更新网格单元进行了区分。

作为本发明进一步的方案：步骤2中，对考虑机器人姿态方位的状态进行离散化处理后搜索节点展开方法；

本发明中，移动机器人的状态向量除了其坐标值之外，还加上了移动机器人姿态方位(即移动机器人出发时朝向的方位)，在对节点进行展开时，排除了那些不可迁移的状态，使得所规划的路径对于这些有运动限制的移动机器人可行，同时使得生成的路径更容易进行平滑处理。

本发明中，移动机器人的状态向量使用S_i＝[x，y，θ]来进行表示，其中x、y分别为移动机器人的横纵坐标，θ为移动机器人的姿态角。同时为了简化搜索的复杂度，在搜索时将横纵坐标以及姿态角离散化，即横纵坐标为距离网格地图的网格行列序号值，而将θ量化为整数，表示的是方位的序号，离散化后的状态向量为I_i＝[i，j，n]，其中i、j为网格行列序号，n为方位序号。在进行节点展开时，该量化的方位序号将限定可以展开的节点，即可以迁移到的状态。这样做的好处是可以减少搜索算法所用时间，同时使得规划出来的路径是可行的。

在对节点进行展开操作时，除了需要考虑不同方位序号下(即量化后的移动机器人姿态角)，可能展开的节点，在考虑姿态方位后，每个节点最多可以展开生成三个子节点，即最多有三个不同的迁移方位可供选择，这三个不同的迁移方位分别是出发时朝向的方位，出发时朝向的方位向左偏转不大于45°角度的方位，出发时朝向的方位向右偏转不大于45°角度的方位。另外一个需要考虑的条件是子节点(子节点是可能展开的节点)对应的行列序号(i,j)是否处于距离网格地图内，以及该处对应的网格单元的距离网格值(该距离网格值表示网格单元与障碍物之间的网格数量)是否大于安全距离d_safe(该安全距离d_safe是由机器人本身的性能、尺寸决定的)，如果大于，则根据其估价函数值的大小找到合适的位置将该节点插入OPEN表，否则丢弃该节点。

作为本发明进一步的方案：步骤2中，应用距离网格地图信息的估价函数设计方法；

使用下式进行A*搜索过程中各个节点估价函数的表示：

f(i，j，k)＝w₀·g(k)+w₁·h₁(i，j)+w₂·h₂(i，j)

其中w₀+w₁+w₂＝1

其中i、j为网格的行列序号值；k为节点的层次序号，对于根节点(根节点是机器人的出发点)有k＝0；w₀、w₁、w₂为各个函数的权重，通过改变权重的大小，可以突出需要进行优化的部分；g(k)为代价函数，反映的是当前节点的深度信息；h₁(i,j)反映的是当前节点对应网格单元与最近障碍的距离信息(通过距离网格地图获取对应计算值)，越远的节点拥有越小的距离值；h₂(i,j)反映的是与目标点距离的信息。

使用下式计算代价函数g(k)：

$g\left(k\right)=\underset{m=0}{\overset{k}{\mathrm{\Σ}}}\sqrt{{(x_m-x_{m-1})}^2+{(y_m-y_{m-1})}^2)}$

其中(x_m,y_m)为第m层次节点对应网格单元的坐标值，g(k)所求的值是从根节点到当前节点路径长度之和。

使用下式计算当前节点对应网格单元与最近障碍的距离信息h₁(i,j)：

h₁(i，j)＝d_max-Md_i，j

$d_{\max }=\sqrt{W^2+H^2}$

其中d_max为最远距离值，W、H为距离网格地图的高度和宽度，Md_i,j为(i,j)处距离网格单元与最近障碍的距离值，可见节点所对应网格单元距离障碍越远，估价函数值越小。在进行A*搜索过程中，通过选择比w₀、w₂大的w₁可以使得规划出来的路径远离障碍，避免出现“贴墙”的路径。

使用下式计算当前节点到目标点的距离h₂(i,j)：

$h_2(i,j)=L_{\mathrm{cell}}\·\sqrt{{(i-i_d)}^2+{(j-j_d)}^2}$

其中i_d、j_d为目标网格的行列序号值，L_cell为距离网格地图网格单元的边长值，h₂(i,j)所表达的是到达目标节点的距离值。

作为本发明进一步的方案：步骤2中，路径简单平滑与精简处理方法。

通过搜索算法所得到的路径在实际应用过程中需要进行平滑处理，才能用于移动机器人的行走控制。因为所规划出来的路径有可能不满足移动机器人的运动限制，同时在路径中还有很多可以精简掉的点。由于在搜索算法中对节点展开时，使用方位序号限制了能够展开的节点，故使用前面所述路径规划算法生成的路径中，相邻两条连接线的夹角的锐角为0(直行)或是为π/4(转向)。

路径平滑时，首先按照下式求出两两相邻的两个点P_m、P_m+1生成的向量的方位角β_m：

${\mathrm{\α}}_m=\arcsin \left(\frac{y_{m+1}-y_m}{\sqrt{{(x_{m+1}-x_m)}^2+{(y_{m+1}-y_m)}^2}}\right)$

其中，x_m，y_m为点P_m的横纵坐标值，x_m+1，y_m+1为点P_m+1的横纵坐标值。之后，对相邻的两个方位角β_m、β_m+1比较，β_m、β_m+1涉及到的点为P_m、P_m+1、P_m+2。若β_m＝β_m+1则将P_m+1从路径数组中删除，若不相等说明路径有转向，则按照转向的路径对生成的路径进行平滑处理。

在路径方向发生改变时，使用圆心位于O，半径为R且与P_mP_m+1和P_m+1P_m+2在P_m′及P_m+1′处相切的圆弧来代替该段路径，因此需要加入两个新点P_m′、P_m+1′并删除点P_m+1。点P_m′、P_m+1′的坐标由点P_m+1进行确定。以P_m+1为基础，根据上述两个公式求出向量的方位角β′_m+1，根据下式计算点P_m′的坐标：

$\left\{\begin{array}{c}{x}^{\′}=x_{m+1}+\frac{L_c}{2}\sin {\mathrm{\β}}_{m+1}^{\′}\\ y^{\′}=y_{m+1}+\frac{L_c}{2}\sin {\mathrm{\β}}_{m+1}\end{array}\right.$

${\mathrm{\ρ}}_{m+1}=\frac{\mathrm{\π}}{4L_c}$

其中，x′、y′为点P_m′的横纵坐标，L_c为网格单元边长。由于在转向时所有的转向圆弧曲率均相等(转向角度变化率均相等)，求取路径曲率ρ_m+1，并与新路径一同保存。这时，对路径的描述方式不仅仅只有一序列路径关键点位置的集合，而且包含了各个连接路径的曲率，这些为导航计算时提供参考。

与现有技术相比，本发明的有益效果是：

(1)本发明提出了一种动态更新距离网格地图的方法，只对发生变化的网格单元进行更新，从而减少了更新耗费计算量；使用排序函数计算每个将要被更新的网格单元的序值，维护一个有序更新队列并排除无用的更新，进一步减少不必要的更新计算；使用了活动窗口，将距离网格地图的扩散更新限制于活动窗口内，更进一步减少无用的计算，分散了计算任务，使得在活动窗口内与移动机器人活动密切相关的距离网格地图能够更灵敏的反应环境的变化，大大减少地图维护占用的处理时间。

(2)本发明提出的路径规划算法在搜索时使用了方位序号的限制来控制搜索算法在展开子节点时可以展开的范围，且由于考虑了移动机器人的姿态方位，排除了那些实际上不可行的前进方向，从而在减少搜索算法运算量的同时，规划出的路径较传统算法平滑；在启发式搜索算法的估价函数设计中，充分利用距离网格地图所提供信息，将节点对应网格单元处的距离值纳入考虑，通过改变估价函数中该项的系数，能够生成与障碍保持一定距离的路径，从而使得规划的路径切实可行。

(3)该方法的实施对于未知环境下拓扑地图的生成，以及未知环境下的路径规划等高级任务的实现具有重要的基础意义。

附图说明

图1是本发明基于距离网格地图的移动机器人路径规划方法过程示意图；

图2是本发明中基于距离网格地图的障碍物增加过程示意图；

图3是本发明中基于距离网格地图的障碍物删除过程距离网格地图清除方法示意图；

图4本发明中基于距离网格地图的障碍物删除过程距离网格地图修复更新示意图；

图5是本发明中距离网格地图更新过程中前沿网格的选择方法示意图；

图6是本发明中距离网格地图更新队列的优化示意图；

图7是本发明中的机器人状态空间离散化示意图；

图8是本发明中的考虑机器人姿态方位的可行邻格示意图；

图9路径简单平滑与精简处理方法示意图。

具体实施方式

下面将结合本发明实施例及附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例中，一种基于距离网格地图的移动机器人路径规划方法，采用Player/Stage平台进行方法开发。Player/Stage移动机器人模拟平台是一个优秀的开源机器人开发及模拟软件，本专利涉及的算法，均基于该软件进行实现。该软件平台分为Player和Stage。其中Player为机器人硬件包装服务器，而Stage为移动机器人模拟软件。该软件平台的优点在于开源、免费，而且在先锋机器人上进行过验证。用Player接口所编写的移动机器人软件，通过Stage模拟器进行模拟验证后，可以直接或是修改少量代码就可以用到真实的移动机器人上。

请参阅图1，一种基于距离网格地图的移动机器人路径规划方法，解决了距离网格地图的构建、动态更新问题和基于距离网格地图的路径规划问题，包含以下几个步骤：

步骤1：设定初始概率网格地图和距离网格地图为空地图；机器人执行机构桥接器控制移动机器人在目标区域内移动，并通过传感器桥接器采集目标区域的地形信息；HIMM(histogramic in motion mapping，移动直方地图更新法)更新向量生成器根据采集到的地形信息生成地图更新向量组；CM(Probabilistic Grid Map，概率网格地图)地图维护更新器根据前一更新周期更新的概率网格地图和更新向量组对概率网格地图进行维护和更新；DM(Distance Grid Map，距离网格地图)地图维护更新器使用活动窗口限制距离网格地图更新范围，通过活动窗口包装接口读取概率网格地图数据，查找是否有障碍物增加或消失，将与障碍增加或消失有关的增加更新单元和清除更新单元加入障碍增加更新队列Qu和障碍清除更新队列Qe，并根据前一周期更新的距离网格地图和障碍清除更新队列Qe、障碍增加更新队列Qu的内容调用障碍清除更新算法和障碍增加更新算法更新距离网格地图；活动窗口变更计算器监控机器人位置变化，根据变化内容生成活动窗口更新区域，将“更新前沿”对应的更新单元加入障碍增加更新队列Qu，并通知DM地图维护地图更新器调用障碍增加更新算法对距离网格地图进行更新；执行机构桥接器控制机器人在目标区域内移动并重复上述更新步骤，直至机器人遍历整个区域，则得到完整的更新后的距离网格地图。

步骤2：确定移动机器人的起点和终点，路径规划器根据完整的更新后的距离网格地图信息和机器人的状态向量，采用启发式搜索算法规划出一条可行路径，对路径进行精简与平滑处理后生成与其对应的控制向量组；安全监控器读取传感器桥接器数据，监控机器人是否处于安全位置，并得出安全状况评估值；控制优先决策模块根据安全状况评估值选择是否采取紧急避障措施，并将避障结果和控制向量组传递给执行机构桥接器；执行机构桥接器根据控制向量组和实时更新的避障结果来控制机器人运动，直到机器人到达终点。

本发明使用了桥接器对各个传感器、执行机构的接口进行包装；HIMM向量生成器采用了HIMM地图构建算法来形成地图更新向量组，该向量组是两个地图维护更新器(CM地图维护更新器、DM地图维护更新器)更新对应地图的依据；CM地图维护更新器根据地图更新向量来对概率网格地图进行维护和更新，而DM地图维护更新器通过活动窗口包装接口来对概率网格地图进行读取，分析其中数据，再通过活动窗口包装接口把计算结果写入距离网格地图；活动窗口变更器通过监控机器人位置变化并形成活动窗口刷新区域，将更新“前沿”加入更新队列，并通知DM地图维护地图更新器对距离网格地图进行刷新；路径规划器根据上级规划器的指令、当前活动窗口内的距离网格地图情况，规划一条可行路径，并以此生成控制向量；安全监控器通过读取红外传感器读数，来监控机器人是否处于安全位置，并得出安全状况评估值；控制优先决策模块通过对安全状况评估进行判断决定采取紧急避障动作还是根据控制向量来通过执行机构包装器对移动机器人进行控制。

请参阅图2，步骤1中，障碍物增加过程为：如果障碍增加更新队列Qu为空，则不进行任何障碍物的增加。否则从障碍增加更新队列Qu的头部弹出一个待展开的单元格，依次计算该单元格周边8个单元格的坐标(i,j)，计算完毕则重新进行障碍增加更新队列Qu的判断。在对各个单元格的坐标计算过程中，如果(i,j)不在距离网格地图内，则判断下一个单元格，否则计算该单元格(i,j)和新增障碍物的距离并更新该(i,j)处的距离值为该距离，并将(i,j)添加到障碍增加更新队列Qu中，重新从障碍增加更新队列Qu中取单元格再次进行计算。

图2所示的网格中的数字和字母表示该网格单元距最近障碍物的最短距离，数字0表示该网格单元距最近障碍物距离为0，即该网格单元就是障碍物所在网格单元。字母a、b、c、d、e、f、g、h和i表示字母所在网格单元距离最近障碍物的距离，这些字母只表示距离大小关系，不代表具体数值，其大小关系为a<b<c<d<e<f<g<h<i。从更新层级上来说，数字0所代表的层级为第0层，是最先被更新的层级，这些字母之间的层级关系为a＝b<c＝d＝e<f＝g＝h＝i，层级越小所对应的网格越早被更新。图中初始距离网格地图中字母所在网格单元没有填充和其距离对应的灰度颜色，这只是为了在后面的演示中更好地区分地图中原有部分和被更新的部分。

在图2(1)中，在初始距离网格地图中只有一个障碍(左上方标注数字0的网格)，然后检测到增加了一个障碍，设障碍增加位置的行列序号为(i,j)，则将行列序号(i,j)相对应的网格单元的距离值改为0(右下方标注数字0的网格)，生成与行列序号(i,j)对应的增加更新单元并将其加入障碍增加更新队列Qu。障碍物所在网格单元是最早受到新增障碍影响的网格单元，它的更新层级为0，最早被更新的网格单元。

在图2(2)中，从障碍增加更新队列Qu删除一个增加更新单元(i,j)，分别对行列序号(i,j)周围的行列序号为(i-1,j+1)、(i,j+1)、(i+1,j+1)、(i+1,j)、(i+1,j-1)、(i,j-1)、(i-1,j-1)和(i-1,j)的八个网格单元进行更新操作，如果更新值小于原值则进行更新并根据更新结果生成与其对应的增加更新单元，并将其加入障碍增加更新队列Qu；否则，不进行更新操作。这八个网格单元的更新层级为引起它们更新操作的网格单元的更新层级加1，即行列序号为(i,j)的网格单元更新层级加1，所以它们的更新层级为1。具体更新过程如图2(2)下所示：首先考虑行列序号为(i-1,j+1)的网格单元，它的更新值为b，原值也为b，更新值不小于原值，所以该网格单元不进行更新，改网格单元仍与原来的障碍相关该网格单元仍与原来的障碍相关，与新增的障碍无关；然后考虑行列序号为(i,j+1)的网格单元，更新值为a，原值为d，a<d，所以设置该网格单元与新增障碍有关，将该网格单元距离值改为a，并且根据这个更新结果生成对应的增加更新单元，并将其加入障碍增加更新队列Qu；接着考虑行列序号为(i+1,j+1)的网格单元，更新值为b，原值为g，b<g，所以设置该网格单元与新增障碍有关，将该网格单元距离值改为b，并且根据这个更新结果生成对应的增加更新单元，并将其加入障碍增加更新队列Qu；同理对行列序号为(i+1,j)、(i+1,j-1)、(i,j-1)、(i-1,j-1)和(i-1,j)的网格单元进行更新，并设置网格单元与新增障碍有关，根据更新结果生成对应的增加更新单元，并将其加入障碍增加更新队列Qu。这八个更新层级为1的网格单元更新后的结果如图2(2)上所示，右下角的填充灰度颜色的网格单元表示这些网格单元与新增障碍有关。

重复图2(2)所示更新过程，得到图2(3)所示结果。当遇到图2(3)下所示情况时，填充格子的网格表示这些网格超出活动窗口范围，在更新时不予考虑。最后的更新结果如图2(3)上所示，右下角的填充灰度颜色的网格单元表示这些网格单元与新增障碍有关。

请参阅图3-图4，步骤1中，障碍物删除过程为：如果障碍清除更新队列Qe为空，则不进行任何障碍物的清除。否则从障碍清除更新队列Qe头部弹出一个待展开的单元格，令其距离网格地图设置为最大距离D_max，依次计算该单元格周边8个单元格的坐标(i,j)，计算完毕则重新进行障碍清除更新队列Qe的判断。在对各个单元格的坐标计算过程中，如果(i,j)与清除单元格相关联，则根据(i,j)处单元格信息生成增加更新单元并加入障碍增加更新队列Qu，否则根据(i,j)处单元格信息生成清除更新单元并加入障碍清除更新队列Qe。接着重新从障碍清除更新队列Qe中取单元格再次进行计算。在进行清除更新之后，再调用障碍增加时的更新算法即可“修复”被清除的部分，恢复原有距离网格地图。

图3所示的网格中的数字和字母表示该网格单元距最近障碍物的最短距离，数字0表示该网格单元距最近障碍物距离为0，即该网格单元就是障碍物所在网格单元。字母a、b、c、d、e、f、g、h和i表示字母所在网格单元距离最近障碍物的距离，这些字母只表示距离大小关系，不代表具体数值，其大小关系为a<b<c<d<e<f<g<h<i。从更新层级上来说，数字0所代表的层级为第0层，是最先被更新的层级，这些字母之间的层级关系为a＝b<c＝d＝e<f＝g＝h＝i，层级越小所对应的网格越早被更新。图中初始距离网格地图中字母所在网格单元填充了和其距离对应的灰度颜色，其中网格灰度颜色越深表示该网格单元距离最近障碍的距离越小。

在图3(1)中，在初始距离网格地图中有两个障碍(两个标注数字0的网格)，与左上方障碍有关的距离网格单元和与右下方障碍有关的距离网格单元之间已经用白色线条分隔开。

在图3(2)中，检测到右下角的障碍消失，设障碍消失位置的行列号为(i,j)，则将行列序号(i,j)相对应的网格单元的距离值清空(右下方标注为空的网格)，生成行列序号(i,j)对应的更新层级为0的清除更新单元，并将其加入障碍清除更新队列QeQe。从障碍清除更新队列QeQe删除一个清除更新单元(i,j)，对分别行列序号(i,j)周围的行列序号为(i-1,j+1)、(i,j+1)、(i+1,j+1)、(i+1,j)、(i+1,j-1)、(i,j-1)、(i-1,j-1)和(i-1,j)的八个网格单元进行更新操作，这八个网格单元的更新层级为1。如果网格所对应更新单元与行列序号为(i,j)的网格所对应的清除更新单元无关，则对该网格单元和网格单元所对应的清除更新单元进行标记，如行列坐标为(i-1,j+1)的网格单元被标记与消失障碍无关(障碍消失网格单元左上角的b被标记)；如果网格所对应更新单元与行列序号为(i,j)的网格所对应的清除更新单元有关，则根据该网格单元生成对应的清除更新单元并将其加入障碍清除更新队列Qe，如行列坐标为(i,j+1)、(i+1,j+1)、(i+1,j)、(i+1,j-1)、(i,j-1)、(i-1,j-1)和(i-1,j)的七个网格单元(障碍消失网格单元除左上角之外的七个网格单元)。然后重复上述清除更新操作，得到如图3(3)所示的结果。

图3(3)为清除了更新层级为1的网格单元所得到的结果，并且标记了更新层级为2的网格单元中与消失障碍无关的网格(两个被标记为字母c的网格单元)。继续重复上述清除更新操作，得到如图3(4)所示的结果。

图3(4)为清除了更新层级为2的网格单元所得到的结果，并且标记了更新层级为3的网格单元中与消失障碍无关的网格(两个被标记为字母g的网格单元)。至此，障碍清除更新队列Qe为空，与消失障碍有关的网格单元已经全部被清除，接下来进行距离网格地图的修复工作。

图4(1)中标记的g、c、b、c和g这五个网格单元与障碍消失单元格无关，将它们定义为清除边沿单元格。生成与这五个清除边缘单元格相对应的增加更新单元并将其加入障碍增加更新队列Qu，并调用障碍增加更新算法对距离网格地图进行更新。由于现在这些清除边沿网格单元是与它们左上方的标记为0的网格单元(障碍所在网格单元)相关的，所以被标记的b的更新层级为1，两个c的更新层级为2，两个g的更新层级为3。

图4(2)为调用障碍增加更新算法更新更新层级为1的网格单元的结果。

图4(3)为调用障碍增加更新算法更新更新层级为2的网格单元的结果。

图4(4)为调用障碍增加更新算法更新更新层级为3的网格单元的结果。至此，距离网格地图修复更新过程结束，障碍物消失距离网格地图的整个更新过程结束。

由于移动机器人的数据获取与控制并非连续的过程，而是一系列离散过程所组成的。传感器数据更新间隔一个固定的时间Δt_s，在一个或者几个Δt_s内完成移动机器人的地图构建、导航计算等工作，所以虽然在现实中，宏观上移动机器人的位置是连续变化的过程，但是对于导航计算的算法程序而言，改变是离散的、跳变的，无论移动机器人沿着哪一条路径由一点到达另一点，所引起的变化对局部的距离网格地图而言，首先需要根据活动窗口的移动情况，计算出需要进行距离网格地图更新计算的“前沿”(待更新距离网格)，然后再采用队列方法进行地图更新。

将时间为t-1时移动机器人的中心坐标记为(x_p,y_p)，当前计算更新时间为t时的移动机器人中心坐标记为(x_r,y_r)，移动机器人坐标变化量为(Δx,Δy)，Δi和Δj分别为活动窗口中心坐标行列序号的变化量，为整数，距离网格单元格的边长为S_c，以单元格数目来表示的活动窗口的宽和高分别为W_n、H_n。

Δx和Δy按照下式计算：

$\left\{\begin{array}{c}\mathrm{\&Delta;x}=x_r-x_p\\ \mathrm{\&Delta;y}=y_r-y_p\end{array}\right.$

Δi和Δj按照下式计算：

$\left\{\begin{array}{c}\mathrm{\&Delta;i}=\mathrm{INT}\left(\frac{\left|\mathrm{\&Delta;x}\right|}{S_c}\right)\\ \mathrm{\&Delta;j}=\mathrm{INT}\left(\frac{\left|\mathrm{\&Delta;y}\right|}{S_c}\right)\end{array}\right.$

分为以下四种情况计算“前沿”网格的行列序号范围：

(1)如果Δi、Δj全为0，则不进行活动窗口刷新计算；

(2)如果Δi＝0，Δj≠0，则不存在纵向“前沿”网格，仅需计算出横向“前沿”网格的序号范围；

(3)如果Δi≠0，Δj＝0，则不存在横向“前沿”网格，仅需计算出纵向“前沿”网格的序号范围；

(4)如果Δi、Δj均非零值，则需要分别计算出横向和纵向两个方向上的“前沿”网格的序号范围。

请参阅图5，本发明中距离网格地图更新过程中前沿网格的选择方法为；

对于第(4)种情况，需要分别计算出横向和纵向两个方向上的“前沿”网格(i,j)的序号范围，根据机器人运动方向的不同分为四种情况，分别对应 $\begin{array}{c}\mathrm{\&Delta;x}>0\\ \mathrm{\&Delta;y}>0\end{array},\begin{array}{c}\mathrm{\&Delta;x}>0\\ \mathrm{\&Delta;y}<0\end{array},\begin{array}{c}\mathrm{\&Delta;x}<0\\ \mathrm{\&Delta;y}<0\end{array}$ 和 $\begin{array}{c}\mathrm{\&Delta;x}<0\\ \mathrm{\&Delta;y}>0\end{array}$ 这四种情况。在此只对 $\begin{array}{c}\mathrm{\&Delta;x}>0\\ \mathrm{\&Delta;y}>0\end{array}$ 的情况(图5中左上角的图)进行详细描述，其他三种情况类似。

当机器人从左下方移动到右上方，活动窗口也相应的从左下方(实线方框)移动到右上方(虚线方框)，图中阴影部分则为更新前沿网格，包括横向更新前沿网格和纵向更新前沿网格两部分。更新前沿网格的计算方法如下：

机器人坐标变化量为(x,y),网格行列号变化量(i,j)根据下式计算：

$\left\{\begin{array}{c}\mathrm{\&Delta;i}=\mathrm{INT}\left(\frac{\left|\mathrm{\&Delta;x}\right|}{S_c}\right)\\ \mathrm{\&Delta;j}=\mathrm{INT}\left(\frac{\left|\mathrm{\&Delta;y}\right|}{S_c}\right)\end{array}\right.$

Wn和Hn为以单元格数目表示的活动窗口的宽和高，图中的横向更新前沿网格的坐标(i,j)的范围为 $\begin{array}{c}0\&le;i\&le;W_n-\mathrm{\&Delta;i}-1\\ j=H_n-\mathrm{\&Delta;j}-1\end{array},$ 纵向更新前沿网格的坐标(i,j)的范围为 $\begin{array}{c}0\&le;j\&le;H_n-\mathrm{\&Delta;j}-1\\ i=W_n-\mathrm{\&Delta;i}-1\end{array}.$

活动窗口位置改变所对应的“前沿网格单元”行列序号的计算方式如下表所示：

图5中描述的其他三种情况采用与第一种类似方法计算。

第(2)种情况，即当Δi＝0，Δj≠0时，不存在纵向更新前沿网格，仅需采用与第(4)种情况相似的计算方法，计算出横向更新前沿网格(i,j)的序号范围即可。

第(3)种情况，即当Δi≠0，Δj＝0时，不存在横向更新前沿网格，仅需采用与第(4)种情况相似的计算方法，计算出纵向更新前沿网格(i,j)的序号范围即可。

请参阅图6，步骤1中，在实际应用中，在障碍增加更新队列更新队列Qu中同时加入了数个元素，随着更新操作的进行，一些重复的元素将被添加进来。因此，在设计更新队列排序函数的时候需要考虑位置参数，同时需要纳入考虑的是更新层级L_u，L_u越大的网格单元距离障碍的距离也大，放到更新队列的尾部可以保证该网格单元被晚些更新，而且在后期的更新中还有可能会被其他更新过程添加的更新单元替换。根据图6来描述，具体如下：

假设在某一时刻，由于地图的变化，如最左边的图所示，其中阴影部分的A和B表示新增障碍，其他网格标注的aa、ab、ac、ad、ae、、af、ag和ba、bb、bc、bd、be、bf、bg不代表距离值，仅仅是对这些网格进行标号，方便接下来的说明。障碍增加更新队列Qu更新队列中加入两个节点A和B，如图6(1)所示。按照障碍增加更新算法，对节点A进行展开(计算A周围8个邻近网格单元)时，又有新的增加更新单元被插入障碍增加更新队列更新队列Qu，aa、ab、ac、ad、ae、af和ag的更新层级L_u为1，B的更新层级L_u为0，所以按照公式P(i，j)＝L_u-W_g-H_g+W_g-j+i分别求出这些网格单元(更新单元)的P(i，j)，B的P(i，j)小于其他网格单元的P(i，j)，所以B在障碍增加更新队列更新队列Qu中排在aa、ab、ac、ad、ae、af和ag的前面，如图6(2)所示。按照队列的顺序对B进行展开，为了缩短队列，优化后续的计算，在插入一个距离网格单元时将与已经在队列中的距离网格单元进行比较，如果是对同一个位置的单元格进行更新，则比较更新距离，距离网格单元更新值较大的单元将被丢弃，如图，在展开B时，由于bb的更新值大于aa的更新值，因此bb被从障碍增加更新队列更新队列Qu中删除，而ba更新值小于ag，故ag被删除而加入ba，如图6(3)所示。

请参阅图7，本发明在搜索时将移动机器人的状态向量(横纵坐标以及姿态角)离散化，即横纵坐标(i,j)为距离网格地图的网格行列序号值，如图7左所示，而将θ量化为整数n，表示的是机器人姿态角所对应方位的序号，如图7右所示，离散化后的状态向量为I_i＝[i，j，n]。对于行列序号为(i,j)的网格单元，在不考虑移动机器人运动限制时，可以迁移到的状态为与之相邻的8个网格单元；每个方位的序号对应一定的角度范围，图中不同的阴影表示不同方位序号所表示的角度范围，即将一个圆周的角度2π划分为8个区间，每个区间用一个方位序号来进行表示。对移动机器人姿态角进行离散化处理时，方位序号与移动机器人姿态角范围的对应关系如下表所示。

图8为本发明中的考虑机器人姿态方位的可行邻格示意图。

在对节点进行展开操作时，需要考虑在当前的姿态方位下可能展开的节点。在考虑姿态方位后，每个节点最多可以展开生成三个子节点，即最多有三个不同的迁移方位可供选择，如图8左上角的方位n＝0的情况，考虑到机器人的姿态方位后，机器人最多有三种不同的迁移方向可以选择，分别是向右，向右上和向右下。当机器人向右运动时，机器人的行列坐标变化量为(0,1)，迁移后的机器人方位n’保持不变，为n’＝0；当机器人向右上运动时，机器人的行列坐标变化量为(1,1)，迁移后的机器人方位n’发生变化，为n’＝1；当机器人向右下运动时，机器人的行列坐标变化量为(1,1)，迁移后的机器人方位n’发生变化，为n’＝7。图8中其他七种情况类似，总结得到的机器人姿态迁移表如下：

请参阅图9，步骤2中，路径简单平滑与精简处理方法如下：

通过搜索算法所得到的路径在实际应用过程中需要进行平滑处理，才能用于移动机器人的行走控制。因为所规划出来的路径有可能不满足移动机器人的运动限制，同时在路径中还有很多可以精简掉的点。由于在搜索算法中对节点展开时，使用方位序号限制了能够展开的节点，故使用前面所述路径规划算法生成的路径中，相邻两条连接线的夹角的锐角为0(直行)或是为π/4(转向)。

和为规划出的机器人未平滑与精简的路径中的一部分，有如图9所示的四种情况，在此仅就图9(1)解释其中一种情况，其他三种情况处理方法类似。

路径平滑时，首先按照下式求出两两相邻的两个点P₀′、P₁′生成的向量的方位角β_m：

${\mathrm{\&alpha;}}_m=\arcsin \left(\frac{y_{m+1}-y_m}{\sqrt{{(x_{m+1}-x_m)}^2+{(y_{m+1}-y_m)}^2}}\right)$

其中，x₀，y₀为点P₀的横纵坐标值，x₁，y₁为点P₁的横纵坐标值。之后，对相邻的两个方位角β₀、β₁比较，β₁、β₁涉及到的点为P₀、P₁、P₂。若β₀＝β₁则将P₁从路径数组中删除，若不相等说明路径有转向，则按照转向的路径对生成的路径进行平滑处理。

在路径方向发生改变时，使用圆心位于O，半径为R且与P₀P₁和P₁P₂在P₀′及P₁′处相切的圆弧来代替该段路径，因此需要加入两个新点P₀′、P₁′并删除点P₁。点P₀′、P₁′的坐标由点P₁进行确定。以P₁为基础，根据上述两个公式求出向量的方位角β′₁，根据下式计算点P₀′的坐标；

$\left\{\begin{array}{c}{x}^{\&prime;}=x_1+\frac{L_c}{2}\sin {\mathrm{\&beta;}}_1^{\&prime;}\\ y^{\&prime;}=y_1+\frac{L_c}{2}\sin {\mathrm{\&beta;}}_1\end{array}\right.$

${\mathrm{\&rho;}}_1=\frac{\mathrm{\&pi;}}{4L_c}$

其中，x′、y′为点P₀′的横纵坐标，L_c为网格单元边长。由于在转向时所有的转向圆弧曲率均相等(转向角度变化率均相等)，求取路径曲率ρ₁，并与新路径一同保存。这时，对路径的描述方式不仅仅只有一序列路径关键点位置的集合，而且包含了各个连接路径的曲率，这些为导航计算时提供参考。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (8)

1.一种基于距离网格地图的移动机器人路径规划方法，其特征在于，包括以下步骤：

步骤1：控制移动机器人在目标区域内移动，并通过传感器采集目标区域的地形信息，构建以移动机器人为中心的活动窗口，所述的活动窗口对应存储在移动机器人内的初始距离网格地图中的一个子区域，选取与活动窗口对应的目标区域中的部分地形信息对所述的子区域进行更新，直至活动窗口遍历整个初始距离网格地图，得到更新后的距离网格地图；

步骤2：在更新后的距离网格地图上，确定移动机器人的起点和终点，将每个网格的中心作为节点，采用A*搜索算法并根据移动机器人在每个节点的姿态角进行节点展开，确定移动机器人由起点运动至终点所有经过的节点，即连线起点、所有经过的节点和终点，规划出移动机器人的路径。

2.根据权利要求1所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，所述步骤1中，所述活动窗口为矩形，其高和宽应满足：

$\left\{\begin{array}{c}{H}_w>\mathrm{INT}(2\&CenterDot;S_{\max })\\ W_w>\mathrm{INT}(2\&CenterDot;S_{\max })\end{array}\right.$

式中，H_w为活动窗口的高，W_w为活动窗口的宽，S_max为保持声呐数据可靠的最远距离，INT()为取整操作。

3.根据权利要求2所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，所述步骤1中，当时间为t-1时，移动机器人的中心坐标为(x_p,y_p)，当时间为t时，移动机器人的中心坐标为(x_r,y_r)，移动机器人坐标变化量为(Δx,Δy)：

$\left\{\begin{array}{c}\mathrm{\&Delta;x}=x_r-x_p\\ \mathrm{\&Delta;y}=y_r-y_p\end{array}\right.$

将距离网格地图中的每个单元格标注行列序号，用Δi和Δj分别表示移动机器人中心坐标行列序号的变化量，每个活动窗口对应更新的子区域存在以下四种情况：

a)Δi＝0、Δj＝0，表示活动窗口对应的子区域中无新加入单元格，无须进行更新；

b)Δi＝0，Δj≠0，表示活动窗口发生纵向移动，对应的子区域纵向上存在新加入单元格，计算出新加入单元格的序号，并对新加入单元格进行更行；

c)Δi≠0，Δj＝0，表示活动窗口发生横向移动，对应的子区域横向上存在新加入单元格，计算出新加入单元格的序号，并对新加入单元格进行更行；

d)Δi≠0，Δj≠0，表示活动窗口发生纵向和横向两个方向上的移动，对应的子区域在横向和纵向上都存在新加入单元格，计算出新加入单元格的序号，并对新加入单元格进行更行。

4.根据权利要求3所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，所述步骤1中，对初始距离网格地图进行更新时，将地形信息与初始距离网格地图比较，存在障碍物增加和障碍物清除两种情况；

障碍物增加时，设定障碍增加更新队列Qu，所述障碍增加更新队列Qu内存放有包含单元格的行列序号(i,j)的增加更新单元，对行列序号(i,j)周围的八个单元格进行更新，以这八个单元格作为新生成的增加更新单元加入障碍增加更新队列Qu继续进行更新，直至障碍增加更新队列Qu中不存在增加更新单元；

障碍物清除时，设定障碍清除更新队列Qe，所述障碍清除更新队列Qe内存放有包含单元格的行列序号(i,j)的清除更新单元，对行列序号(i,j)周围的八个单元格进行更新，以这八个单元格作为新生成的清除更新单元加入障碍清除更新队列Qe继续进行更新，直到得到所有与行列序号(i,j)不相关联的清除边缘单元格，生成与所述清除边缘单元格相对应的增加更新单元并将其加入障碍增加更新队列Qu；再对增加更新单元周围的八个单元格进行更新，以这八个单元格作为新生成的增加更新单元加入障碍增加更新队列Qu继续进行更新，直至障碍增加更新队列Qu中不存在增加更新单元，即完成距离网格地图的更新。

5.根据权利要求4所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，所述步骤2中，对节点进行展开时，移动机器人的状态向量为I_i＝[i，j，n]，其中：i、j为单元格行列序号；n为方位序号，也为移动机器人的姿态角。

6.根据权利要求5所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，所述步骤2中，A*搜索算法对各节点估价函数表示为：

f(i，j，k)＝w₀·g(k)+w₁·h₁(i，j)+w₂·h₂(i，j)

其中w₀+w₁+w₂＝1

$g\left(k\right)=\underset{m=0}{\overset{k}{\mathrm{\&Sigma;}}}\sqrt{{(x_m-x_{m-1})}^2+{(y_m-y_{m-1})}^2)}$

h₁(i，j)＝d_max-Md_i，j

$d_{\max }=\sqrt{W^2+H^2}$

$h_2(i,j)=L_{\mathrm{cell}}\&CenterDot;\sqrt{{(i-i_d)}^2+{(j-j_d)}^2}$

式中：i、j为单元格的行列序号值，k为节点的层次序号，w₀、w₁、w₂为对应函数的权重，(x_m,y_m)为第m层次节点对应单元格的坐标值，d_max为最远距离值，W、H为距离网格地图的高度和宽度，Md_i,j为(i,j)处距离单元格与最近障碍的距离值，i_d、j_d为终点所处单元格的行列序号值，L_cell为单元格的边长值，h₂(i,j)所表达的是到达终点的距离值。

7.根据权利要求6所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，计算相邻两个节点P_m、P_m+1生成的向量的方位角β_m

${\mathrm{\&alpha;}}_m=\arcsin \left(\frac{y_{m+1}-y_m}{\sqrt{{(x_{m+1}-x_m)}^2+{(y_{m+1}-y_m)}^2}}\right)$

其中：x_m，y_m为点P_m的横纵坐标值，x_m+1，y_m+1为点P_m+1的横纵坐标值；

将相邻两个方位角β_m、β_m+1比较，β_m、β_m+1涉及到的点为P_m、P_m+1、P_m+2，若β_m＝β_m+1则将P_m+1从路径数组中删除，若不相等说明路径有转向，则按照转向的路径对生成的路径进行平滑处理。

8.根据权利要求7所述的基于距离网格地图的移动机器人路径规划方法，其特征在于，在路径有转向时，使用圆心位于O，半径为R且与P_mP_m+1和P_m+1P_m+2在P_m′及P_m+1′处相切的圆弧来代替该段路径，点P_m′的坐标为：

$\left\{\begin{array}{c}{x}^{\&prime;}=x_{m+1}+\frac{L_c}{2}\sin {\mathrm{\&beta;}}_{m+1}^{\&prime;}\\ y^{\&prime;}=y_{m+1}+\frac{L_c}{2}\sin {\mathrm{\&beta;}}_{m+1}\end{array}\right.$

${\mathrm{\&rho;}}_{m+1}=\frac{\mathrm{\&pi;}}{4L_c}$

其中，x′、y′为点P_m′的横纵坐标，L_c为网格单元边长。