CN111176286B - 一种基于改进D*lite算法的移动机器人路径规划方法及系统 - Google Patents

Abstract

本发明请求保护一种基于改进D*lite算法的移动机器人路径规划方法及系统。具体步骤为：首先，根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；然后，根据若干单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；接着，根据邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；之后，根据单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表；最后，根据搜索链表引导D*lite路径规划算法完成移动机器人路径规划。实验结果证实了本方法能够在保证路径长度接近最短的同时降低路径规划时间。

Description

一种基于改进D*lite算法的移动机器人路径规划方法及系统

技术领域

本发明属于移动机器人自主导航领域，特别是一种基于改进D*lite算法的移动机器人路径规划方法。

背景技术

移动机器人的路径规划的能力，决定了机器人可胜任工作难度的高低。在构建好的环境模型中，规划一条从初始点到目标点的无碰撞最优路径，是路径规划的主要内容。在构建环境模型中，栅格法应用较多，该方法具有直观简洁、分辨率可变、容易创建和存储等优点，适用于室内环境路径规划地图模型的建立，鲁棒性强。图搜索算法是路径规划中的一类算法，它被广泛应用于栅格地图中的路径规划问题。A*算法是图搜索算法中的经典算法，它加入了启发式搜索方法。启发式搜索是评估状态空间中的每个搜索位置，找出下一步要搜索的最好位置G，再从G进行类似搜索直到目标位置。该搜索方法能节省大量搜索空间，提高搜索效率。在A*算法的基础上，研究者们先后提出了改进算法例如D*，LPA*与D*lite。学者们对路径规划算法的路径安全性，路径平滑度和重规划策略进行了诸多研究，但是在路径搜索中仍然存在问题。

路径规划的速度仍然较慢，如何提高路径规划的效率是路径规划问题中的难点。在栅格地图中，基于图搜索的路径规划算法在进行路径搜索时，会根据搜索函数向八个邻域方向寻找最优解，一些优先级较高的网格延申出的路径最后被障碍物阻挡而结束，这造成在这一搜索方向上大部分的之前进行的扩展操作变为无效，例如在凹型障碍物内或者当前网格与目标网格分别在两个房间中，都会有大量的无效网格被搜索和计算，这些无效的扩展降低了规划效率。如何确保路径规划算法在正确的搜索方向上进行扩展，是提高路径规划速度需要解决的问题。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种能够在保证路径长度接近最短的同时降低路径规划时间的基于改进D*lite算法的移动机器人路径规划方法及系统。本发明的技术方案如下：

一种基于改进D*lite算法的移动机器人路径规划方法，其包括以下步骤：

步骤S1，首先，根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；

步骤S2，根据S1中所得的若干有界单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；

步骤S3，根据S2中得到的邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；

步骤S4，根据S3得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表；

步骤S5，根据S4得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划。

进一步的，步骤S1根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元，具体步骤为：

S11：用一条垂直线从左至右扫描栅格地图；

S12：当扫描线扫入障碍空间时，扫描线的连通性增加，当扫描线扫出障碍空间时，扫描线的连通性降低，当扫描线的连通性增加时，一个旧单元结束，生成两个新单元；当扫描线的连通性降低时，两个旧单元结束，生成一个新单元；当扫描线的连通性不变但某段连通线长度发生了阶跃变化，若变化的大小超过当前单元最大宽度的一半，则旧单元结束，新单元生成；

S13：扫描结束后，环境地图被分割成若干独立的有界单元，每个单元内部都没有障碍物。

进一步的，步骤S2根据S1中所得的若干单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵，具体步骤为：

S21：若任意两个邻接单元有相互连通的自由区域，则视为这两个单元可用连接线相连，遍历完所有单元后得到表示单元间连通关系的单元连接图；

S22：将单元补全为长方形，计算连接线两端单元的中心之间的曼哈顿距离，计算公式如下：

d(i,j)＝abs(x_i-x_j)+abs(y_i-y_j)

此距离作为连接线的代价值，若两单元之间没有直接的自由连通区域，即两单元之间没有连接线，则两单元之间的代价值设置为无穷大；

S23：用单元之间的代价值组成矩阵edge，其中edge[i][j]表示单元i和单元j之间连接线的代价值，若无连接线则代价值为无穷大，矩阵edge中的值即为单元之间的原始距离代价值，这个矩阵即为邻接矩阵。

进一步的，所述步骤S3根据S2中得到的邻接矩阵edge，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序，具体步骤为：

S31：设定起始网格所在单元为起始单元，目标网格所在单元为目标单元，采用双向搜索的方法，同时进行两个搜索，分别以起始单元和目标单元作为搜索起始单元；

S32：给每个单元设置两个值，cost值表示与搜索起始单元的距离代价值，pre表示父单元；

S33：用集合C保存已经确认最短路径的单元，把搜索起始单元s放入C中，用集合W保存还没有找到最短路径的单元，把其余单元都放在W中，把W中每个单元j的cost值设为edge[s][j]，pre设为s；

S34：从集合W中选出cost最小的单元i，由于cost非负，所以此cost为s到i的最小代价值，将i移出W，移入C；

S35：对于W中的每个单元j，通过计算如果出现cost_j>cost_i+edge[i][j]，更新cost_j为cost_i+edge[i][j]，并把pre_j设为i；

S36：在两个搜索中分别重复S34、S35，直到两个搜索中的集合C出现交集，或者任意一个搜索中集合W中单元的cost值都为无穷大，此时循环结束；

S37：若两个搜索中的集合C没有交集就说明起始单元无法到达目标单元，若两个搜索中的集合C有交集，则从该共有单元的pre值开始查找，单元的pre值表示该单元的父单元，即可找到搜索起始单元到该共有单元的最短路径，将两个搜索的路径合在一起，即为目标单元到起始单元代价总值最短的路径，以目标单元为第一个单元，依次添加路径所经过的单元，最后以起始单元为最后一个单元，即可得到从目标单元到起始单元所经过的单元顺序。

进一步的，所述步骤S4根据S3得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表，具体步骤为：

S41：为每一个网格添加一个编号num，num等于网格所在单元的编号；

S42：根据S3中得到的单元顺序，从第二个单元开始，设当前单元为u，前一单元为u′，找出u中与u′相邻的网格作为核心区域core，记为：

core＝all(s_{s.num＝u.num}∈succ(s′_{s′.num＝u′.num}))

S_{s.num＝u.num}表示单元u中的网格。在当前单元u中，所有与前一单元u′相邻的网格共同组成了单元u的核心区域。按单元顺序依次设置核心区域，直到在起始单元中完成设置；

S43：以X_max，X_min，Y_max，Y_min表示核心区域内的网格中拥有的最大和最小横纵坐标，计算核心区域中心坐标core_mid，具体公式为：

S44：设置目标网格为第一个核心网格，起始网格为最后一个核心网格，设s为当前核心区域内的网格，cgrid为上一个核心网格，core_mid为下一个核心区域的中心坐标，给每一个核心区域内的网格设置一个kc值，由kc₁和kc₂组成，记为：

每个核心区域中选取kc值最小的网格作为核心网格，首先比较kc₁值，如果kc₁相同再比较kc₂值；

S45：用核心网格组成搜索链表，其中链表的头是目标网格，链表的尾是起始网格，其余核心网格按照单元顺序插入链表中，即可得到搜索链表。

进一步的，所述步骤S5根据S4得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划，具体步骤为：

S51：对所有网格s初始化，使g(s)＝rhs(s)＝∞，其中g(s)表示s网格到目标网格的实际代价，rhs(s)表示s网格的父网格的g值加上父网格到s网格的移动代价值，用c(s′,s)表示父网格s′到s网格的移动代价值，rhs(s)的计算公式如下：

设置一个地址P指向搜索链表的首个网格；

S52：将目标网格的rhs值设置为0，更新其k值后插入优先队列，其中网格s的k值的计算如下：

式中h值表示该网格s与起始网格start的启发距离，记为：

h(s)＝max(abs(x_start-x_s),abs(y_start-y_s))

S53：以地址P当前指向的网格为搜索目标网格，以地址P指向的下一个网格为搜索起始网格；

S54：从优先队列中移出k值最小的网格，此时先比较k₁，如果k₁相同再比较k₂，随后更新其g值，设置g值等于rhs值，然后找出该网格的邻接网格中num与搜索目标网格或者搜索起始网格的num相同的网格，计算这些网格的rhs值，若这些网格的rhs值与g值不相等，则更新这些网格的k值，并将它们插入优先队列；

S55：重复S54直到搜索起始网格的g值等于rhs值，此时单次搜索结束，地址P往后移一位；

S56：重复从S53到S55，直到起始网格的g值等于rhs值，从起始网格开始循着邻接网格中g值最小的网格直到目标网格，就可完成从起始网格到目标网格的路径规划。

一种基于改进D*lite算法的移动机器人路径规划系统，其包括：

地图分割模块：用于根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；

计算模块：用于根据所得的若干有界单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；

双向图搜索模块：用于根据得到的邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；

搜索链表构建模块：用于根据得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表；

路径规划模块：用于根据得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划。

本发明的优点及有益效果如下：

本发明针对目前主流的移动机器人路径规划方法存在搜索方向不准确、无效计算次数多，规划效率低的问题，提出一种优化的路径规划方法。该方法采用地图分割算法将栅格地图分割为若干内部没有障碍物的有界单元，找出目标单元到起始单元需要经过的单元顺序，从而确定了正确的搜索方向。该方法使用双向图搜索算法，用于计算从目标单元到起始单元所经过的单元顺序，从两端同时对单元图进行最短代价搜索，从而提高了计算单元顺序的速度。该方法在路径必经的单元中设置核心区域，并对核心区域内的网格设置关键值，选取优先级最高的网格作为核心网格，从而确保了规划出的路径长度接近最短。该方法将核心网格组成搜索链表用以引导算法完成路径规划，从而确保路径规划算法在正确的方向搜索，这样可以快速找到绕过障碍物的正确路径，大大减少了无效计算次数，提高了规划效率。

附图说明

图1是本发明提供优选实施例的基于改进D*lite算法的移动机器人路径规划方法实现框架；

图2是本发明提供优选实施例的地图分割示意图；

图3是本发明提供优选实施例的核心网格设置示意图；

图4是本发明提供优选实施例的路径规划示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

如图1所示，本方法的主要步骤为：

S1，根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；

S2，根据S1中所得的若干单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；

S3，根据S2中得到的邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；

S4，根据S3得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表；

S5，根据S4得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划。

步骤S1的具体实施过程为：

S11：用一条垂直线从左至右扫描栅格地图；

S12：当扫描线扫入障碍空间时，扫描线的连通性增加，当扫描线扫出障碍空间时，扫描线的连通性降低。当扫描线的连通性增加时，一个旧单元结束，生成两个新单元。当扫描线的连通性降低时，两个旧单元结束，生成一个新单元。当扫描线的连通性不变但某段连通线长度发生了阶跃变化，若变化的大小超过当前单元最大宽度的一半，则旧单元结束，新单元生成；

S13：扫描结束后，环境地图被分割成若干独立的有界单元，每个单元内部都没有障碍物。

步骤S2的具体实施过程为：

S21：若任意两个邻接单元有相互连通的自由区域，则视为这两个单元可用连接线相连，遍历完所有单元后得到表示单元间连通关系的单元连接图；

S22：将单元补全为长方形，计算连接线两端单元的中心之间的曼哈顿距离，计算公式如下：

d(i,j)＝abs(x_i-x_j)+abs(y_i-y_j)

此距离作为连接线的代价值，若两单元之间没有直接的自由连通区域，即两单元之间没有连接线，则两单元之间的代价值设置为无穷大；

S23：用单元之间的代价值组成矩阵edge，其中edge[i][j]表示单元i和单元j之间连接线的代价值，若无连接线则代价值为无穷大，矩阵edge中的值即为单元之间的原始距离代价值，这个矩阵即为邻接矩阵。

步骤S3的具体实施过程为：

S31：设定起始网格所在单元为起始单元，目标网格所在单元为目标单元，为了提高算法的速度，可以采用双向搜索的方法，同时进行两个搜索，分别以起始单元和目标单元作为搜索起始单元；

S32：给每个单元设置两个值，cost值表示与搜索起始单元的距离代价值，pre表示父单元；

S33：用集合C保存已经确认最短路径的单元，把搜索起始单元s放入C中。用集合W保存还没有找到最短路径的单元，把其余单元都放在W中。把W中每个单元j的cost值设为edge[s][j]，pre设为s；

S34：从集合W中选出cost最小的单元i，由于cost非负，所以此cost为s到i的最小代价值，将i移出W，移入C；

S35：对于W中的每个单元j，通过计算如果出现cost_j>cost_i+edge[i][j]，更新cost_j为cost_i+edge[i][j]，并把pre_j设为i；

S36：在两个搜索中分别重复S34、S35，直到两个搜索中的集合C出现交集，或者任意一个搜索中集合W中单元的cost值都为无穷大，此时循环结束；

S37：若两个搜索中的集合C没有交集就说明起始单元无法到达目标单元，若两个搜索中的集合C有交集，则从该共有单元的pre值开始查找，单元的pre值表示该单元的父单元，即可找到搜索起始单元到该共有单元的最短路径，将两个搜索的路径合在一起，即为目标单元到起始单元代价总值最短的路径，以目标单元为第一个单元，依次添加路径所经过的单元，最后以起始单元为最后一个单元，即可得到从目标单元到起始单元所经过的单元顺序。

步骤S4的具体实施过程为：

S41：为每一个网格添加一个编号num，num等于网格所在单元的编号；

S42：根据S3中得到的单元顺序，从第二个单元开始，设当前单元为u，前一单元为u′，找出u中与u′相邻的网格作为核心区域core，记为：

core＝all(s_{s.num＝u.num}∈succ(s′_{s′.num＝u′.num}))

S_{s.num＝u.num}表示单元u中的网格。在当前单元u中，所有与前一单元u′相邻的网格共同组成了单元u的核心区域。按单元顺序依次设置核心区域，直到在起始单元中完成设置；

S43：以X_max，X_min，Y_max，Y_min表示核心区域内的网格中拥有的最大和最小横纵坐标，计算核心区域中心坐标core_mid，具体公式为：

S44：设置目标网格为第一个核心网格，起始网格为最后一个核心网格，设s为当前核心区域内的网格，cgrid为上一个核心网格，core_mid为下一个核心区域的中心坐标，给每一个核心区域内的网格设置一个kc值，由kc₁和kc₂组成，记为：

每个核心区域中选取kc值最小的网格作为核心网格，首先比较kc₁值，如果kc₁相同再比较kc₂值；

S45：用核心网格组成搜索链表，其中链表的头是目标网格，链表的尾是起始网格，其余核心网格按照单元顺序插入链表中，即可得到搜索链表。

步骤S5的具体实施过程为：

S51：对所有网格s初始化，使g(s)＝rhs(s)＝∞，其中g(s)表示s网格到目标网格的实际代价，rhs(s)表示s网格的父网格的g值加上父网格到s网格的移动代价值，用c(s′,s)表示父网格s′到s网格的移动代价值，rhs(s)的计算公式如下：

设置一个地址P指向搜索链表的首个网格；

S52：将目标网格的rhs值设置为0，更新其k值后插入优先队列，其中网格s的k值的计算如下：

式中h值表示该网格s与起始网格start的启发距离，记为：

h(s)＝max(abs(x_start-x_s),abs(y_start-y_s))

S53：以地址P当前指向的网格为搜索目标网格，以地址P指向的下一个网格为搜索起始网格；

S54：从优先队列中移出k值最小的网格，此时先比较k₁，如果k₁相同再比较k₂，随后更新其g值，设置g值等于rhs值，然后找出该网格的邻接网格中num与搜索目标网格或者搜索起始网格的num相同的网格，计算这些网格的rhs值，若这些网格的rhs值与g值不相等，则更新这些网格的k值，并将它们插入优先队列；

S55：重复S54直到搜索起始网格的g值等于rhs值，此时单次搜索结束，地址P往后移一位；

S56：重复从S53到S55，直到起始网格的g值等于rhs值，从起始网格开始循着邻接网格中g值最小的网格直到目标网格，就可完成从起始网格到目标网格的路径规划。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (6)

1.一种基于改进D*lite算法的移动机器人路径规划方法，其特征在于，包括以下步骤：

步骤S1，首先，根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；

步骤S2，根据S1中所得的若干有界单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；

步骤S3，根据S2中得到的邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；

步骤S4，根据S3得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表；

步骤S5，根据S4得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划；

所述步骤S4根据S3得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表，具体步骤为：

S41：为每一个网格添加一个编号num，num等于网格所在单元的编号；

S42：根据S3中得到的单元顺序，从第二个单元开始，设当前单元为u，前一单元为u′，找出u中与u′相邻的网格作为核心区域core，记为：

core＝all(s_{s.num＝u.num}∈succ(s′_{s′.num＝u′.num}))

S_{s.num＝u.num}表示单元u中的网格，在当前单元u中，所有与前一单元u′相邻的网格共同组成了单元u的核心区域；按单元顺序依次设置核心区域，直到在起始单元中完成设置；

S43：以X_max，X_min，Y_max，Y_min表示核心区域内的网格中拥有的最大和最小横纵坐标，计算核心区域中心坐标core_mid，具体公式为：

S44：设置目标网格为第一个核心网格，起始网格为最后一个核心网格，设s为当前核心区域内的网格，cgrid为上一个核心网格，core_mid为下一个核心区域的中心坐标，给每一个核心区域内的网格设置一个kc值，由kc₁和kc₂组成，记为：

每个核心区域中选取kc值最小的网格作为核心网格，首先比较kc₁值，如果kc₁相同再比较kc₂值；

S45：用核心网格组成搜索链表，其中链表的头是目标网格，链表的尾是起始网格，其余核心网格按照单元顺序插入链表中，即可得到搜索链表。

2.根据权利要求1所述的一种基于改进D*lite算法的移动机器人路径规划方法，其特征在于，步骤S1根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元，具体步骤为：

S11：用一条垂直线从左至右扫描栅格地图；

S12：当扫描线扫入障碍空间时，扫描线的连通性增加，当扫描线扫出障碍空间时，扫描线的连通性降低，当扫描线的连通性增加时，一个旧单元结束，生成两个新单元；当扫描线的连通性降低时，两个旧单元结束，生成一个新单元；当扫描线的连通性不变但某段连通线长度发生了阶跃变化，若变化的大小超过当前单元最大宽度的一半，则旧单元结束，新单元生成；

S13：扫描结束后，环境地图被分割成若干独立的有界单元，每个单元内部都没有障碍物。

3.根据权利要求2所述的一种基于改进D*lite算法的移动机器人路径规划方法，其特征在于，步骤S2根据S1中所得的若干单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵，具体步骤为：

S21：若任意两个邻接单元有相互连通的自由区域，则视为这两个单元可用连接线相连，遍历完所有单元后得到表示单元间连通关系的单元连接图；

S22：将单元补全为长方形，计算连接线两端单元的中心之间的曼哈顿距离，计算公式如下：

d(i,j)＝abs(x_i-x_j)+abs(y_i-y_j)

此距离作为连接线的代价值，若两单元之间没有直接的自由连通区域，即两单元之间没有连接线，则两单元之间的代价值设置为无穷大；

S23：用单元之间的代价值组成矩阵edge，其中edge[i][j]表示单元i和单元j之间连接线的代价值，若无连接线则代价值为无穷大，矩阵edge中的值即为单元之间的原始距离代价值，这个矩阵即为邻接矩阵。

4.根据权利要求3所述的一种基于改进D*lite算法的移动机器人路径规划方法，其特征在于，所述步骤S3根据S2中得到的邻接矩阵edge，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序，具体步骤为：

S31：设定起始网格所在单元为起始单元，目标网格所在单元为目标单元，采用双向搜索的方法，同时进行两个搜索，分别以起始单元和目标单元作为搜索起始单元；

S32：给每个单元设置两个值，cost值表示与搜索起始单元的距离代价值，pre表示父单元；

S33：用集合C保存已经确认最短路径的单元，把搜索起始单元s放入C中，用集合W保存还没有找到最短路径的单元，把其余单元都放在W中，把W中每个单元j的cost值设为edge[s][j]，pre设为s；

S34：从集合W中选出cost最小的单元i，由于cost非负，所以此cost为s到i的最小代价值，将i移出W，移入C；

S35：对于W中的每个单元j，通过计算如果出现cost_j>cost_i+edge[i][j]，更新cost_j为cost_i+edge[i][j]，并把pre_j设为i；

S36：在两个搜索中分别重复S34、S35，直到两个搜索中的集合C出现交集，或者任意一个搜索中集合W中单元的cost值都为无穷大，此时循环结束；

S37：若两个搜索中的集合C没有交集就说明起始单元无法到达目标单元，若两个搜索中的集合C有交集，则从该共有单元的pre值开始查找，单元的pre值表示该单元的父单元，即可找到搜索起始单元到该共有单元的最短路径，将两个搜索的路径合在一起，即为目标单元到起始单元代价总值最短的路径，以目标单元为第一个单元，依次添加路径所经过的单元，最后以起始单元为最后一个单元，即可得到从目标单元到起始单元所经过的单元顺序。

5.根据权利要求4所述的一种基于改进D*lite算法的移动机器人路径规划方法，其特征在于，所述步骤S5根据S4得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划，具体步骤为：

S51：对所有网格s初始化，使g(s)＝rhs(s)＝∞，其中g(s)表示s网格到目标网格的实际代价，rhs(s)表示s网格的父网格的g值加上父网格到s网格的移动代价值，用c(s′,s)表示父网格s′到s网格的移动代价值，rhs(s)的计算公式如下：

设置一个地址P指向搜索链表的首个网格；

S52：将目标网格的rhs值设置为0，更新其k值后插入优先队列，其中网格s的k值的计算如下：

式中h值表示该网格s与起始网格start的启发距离，记为：

h(s)＝max(abs(x_start-x_s),abs(y_start-y_s))

S53：以地址P当前指向的网格为搜索目标网格，以地址P指向的下一个网格为搜索起始网格；

S54：从优先队列中移出k值最小的网格，此时先比较k₁，如果k₁相同再比较k₂，随后更新其g值，设置g值等于rhs值，然后找出该网格的邻接网格中num与搜索目标网格或者搜索起始网格的num相同的网格，计算这些网格的rhs值，若这些网格的rhs值与g值不相等，则更新这些网格的k值，并将它们插入优先队列；

S55：重复S54直到搜索起始网格的g值等于rhs值，此时单次搜索结束，地址P往后移一位；

S56：重复从S53到S55，直到起始网格的g值等于rhs值，从起始网格开始循着邻接网格中g值最小的网格直到目标网格，就可完成从起始网格到目标网格的路径规划。

6.一种基于改进D*lite算法的移动机器人路径规划系统，其特征在于，包括：

地图分割模块：用于根据机器人所在环境的栅格地图，使用地图分割算法将地图分割为若干内部没有障碍物的有界单元；

计算模块：用于根据所得的若干有界单元之间的连通关系得到单元连接图，计算得到各单元之间的原始距离代价值和邻接矩阵；

双向图搜索模块：用于根据得到的邻接矩阵，使用双向图搜索算法，计算得到从目标单元到起始单元所经过的单元顺序；

搜索链表构建模块：用于根据得到的单元顺序，依次按照核心网格设置方法在对应单元中设置核心网格，按顺序组成搜索链表，具体为：

S41：为每一个网格添加一个编号num，num等于网格所在单元的编号；

S42：根据S3中得到的单元顺序，从第二个单元开始，设当前单元为u，前一单元为u′，找出u中与u′相邻的网格作为核心区域core，记为：

core＝all(s_{s.num＝u.num}∈succ(s′_{s′.num＝u′.num}))

S_{s.num＝u.num}表示单元u中的网格，在当前单元u中，所有与前一单元u′相邻的网格共同组成了单元u的核心区域；按单元顺序依次设置核心区域，直到在起始单元中完成设置；

S43：以X_max，X_min，Y_max，Y_min表示核心区域内的网格中拥有的最大和最小横纵坐标，计算核心区域中心坐标core_mid，具体公式为：

S44：设置目标网格为第一个核心网格，起始网格为最后一个核心网格，设s为当前核心区域内的网格，cgrid为上一个核心网格，core_mid为下一个核心区域的中心坐标，给每一个核心区域内的网格设置一个kc值，由kc₁和kc₂组成，记为：

每个核心区域中选取kc值最小的网格作为核心网格，首先比较kc₁值，如果kc₁相同再比较kc₂值；

S45：用核心网格组成搜索链表，其中链表的头是目标网格，链表的尾是起始网格，其余核心网格按照单元顺序插入链表中，即可得到搜索链表；

路径规划模块：用于根据得到的搜索链表，将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索，用搜索链表引导D*lite算法完成路径规划。

Images