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(ss.num=u.num∈succ(s′s′.num=u′.num))Ss.num=u.num表示单元u中的网格,在当前单元u中,所有与前一单元u′相邻的网格共同组成了单元u的核心区域;按单元顺序依次设置核心区域,直到在起始单元中完成设置;
S43:以Xmax,Xmin,Ymax,Ymin表示核心区域内的网格中拥有的最大和最小横纵坐标,计算核心区域中心坐标coremid,具体公式为:S44:设置目标网格为第一个核心网格,起始网格为最后一个核心网格,设s为当前核心区域内的网格,cgrid为上一个核心网格,coremid为下一个核心区域的中心坐标,给每一个核心区域内的网格设置一个kc值,由kc1和kc2组成,记为:每个核心区域中选取kc值最小的网格作为核心网格,首先比较kc1值,如果kc1相同再比较kc2值;
S45:用核心网格组成搜索链表,其中链表的头是目标网格,链表的尾是起始网格,其余核心网格按照单元顺序插入链表中,即可得到搜索链表。
2.根据权利要求1所述的一种基于改进D*lite算法的移动机器人路径规划方法,其特征在于,步骤S1根据机器人所在环境的栅格地图,使用地图分割算法将地图分割为若干内部没有障碍物的有界单元,具体步骤为:S11:用一条垂直线从左至右扫描栅格地图;
S12:当扫描线扫入障碍空间时,扫描线的连通性增加,当扫描线扫出障碍空间时,扫描线的连通性降低,当扫描线的连通性增加时,一个旧单元结束,生成两个新单元;当扫描线的连通性降低时,两个旧单元结束,生成一个新单元;当扫描线的连通性不变但某段连通线长度发生了阶跃变化,若变化的大小超过当前单元最大宽度的一半,则旧单元结束,新单元生成;
S13:扫描结束后,环境地图被分割成若干独立的有界单元,每个单元内部都没有障碍物。
3.根据权利要求2所述的一种基于改进D*lite算法的移动机器人路径规划方法,其特征在于,步骤S2根据S1中所得的若干单元之间的连通关系得到单元连接图,计算得到各单元之间的原始距离代价值和邻接矩阵,具体步骤为:S21:若任意两个邻接单元有相互连通的自由区域,则视为这两个单元可用连接线相连,遍历完所有单元后得到表示单元间连通关系的单元连接图;
S22:将单元补全为长方形,计算连接线两端单元的中心之间的曼哈顿距离,计算公式如下:d(i,j)=abs(xi‑xj)+abs(yi‑yj)
此距离作为连接线的代价值,若两单元之间没有直接的自由连通区域,即两单元之间没有连接线,则两单元之间的代价值设置为无穷大;
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,通过计算如果出现costj>costi+edge[i][j],更新costj为costi+edge[i][j],并把prej设为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(xstart‑xs),abs(ystart‑ys))S53:以地址P当前指向的网格为搜索目标网格,以地址P指向的下一个网格为搜索起始网格;
S54:从优先队列中移出k值最小的网格,此时先比较k1,如果k1相同再比较k2,随后更新其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(ss.num=u.num∈succ(s′s′.num=u′.num))Ss.num=u.num表示单元u中的网格,在当前单元u中,所有与前一单元u′相邻的网格共同组成了单元u的核心区域;按单元顺序依次设置核心区域,直到在起始单元中完成设置;
S43:以Xmax,Xmin,Ymax,Ymin表示核心区域内的网格中拥有的最大和最小横纵坐标,计算核心区域中心坐标coremid,具体公式为:S44:设置目标网格为第一个核心网格,起始网格为最后一个核心网格,设s为当前核心区域内的网格,cgrid为上一个核心网格,coremid为下一个核心区域的中心坐标,给每一个核心区域内的网格设置一个kc值,由kc1和kc2组成,记为:每个核心区域中选取kc值最小的网格作为核心网格,首先比较kc1值,如果kc1相同再比较kc2值;
S45:用核心网格组成搜索链表,其中链表的头是目标网格,链表的尾是起始网格,其余核心网格按照单元顺序插入链表中,即可得到搜索链表;
路径规划模块:用于根据得到的搜索链表,将D*lite算法多方向的搜索依照搜索链表的顺序分为多段的朝着正确方向的搜索,用搜索链表引导D*lite算法完成路径规划。