机器人路径规划：基于D*算法的机器人路径规划（提供Python代码）

一、D*算法介绍

 D*算法（Dynamic A Star）的主要思想是先利用静态规划算法计算出各个节点距离目标节点的最短路径，再从起始节点向后追溯，如果下一节点的状态发生变化，则调整弧的代价函数并重新搜索。相对于静态规划算法， D*算法的特点是可用于仅部分环境已知或环境不断变化的状况下的路径寻优，与其他重规划算法 的思想不同，动态的 D*算法是一种不完全重规划算法，利用原有的规划信息，在一定的程度上实现最优性及实时性的结合。

参考文献：

[1] Stentz A .Optimal and efficient path planning for partially-known environments[J].IEEE, 1994.DOI:10.1109/ROBOT.1994.351061.

[2] Stentz A .Optimal and Efficient Path Planning for Unknown and Dynamic Environments[J].International Journal of Robotics and Automation, 2003, 10(3).DOI:doi:10.1108/01439919510098461.

[3]谷润平,崔朋,唐建勋,等.基于D*算法的场面滑行动态规划研究[J].科学技术与工程, 2015(1):6.DOI:10.3969/j.issn.1671-1815.2015.01.059.

二、部分代码

import math


from sys import maxsize

import matplotlib.pyplot as plt

show_animation = True


class State:

    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.state = "."
        self.t = "new"  # tag for state
        self.h = 0
        self.k = 0

    def cost(self, state):
        if self.state == "#" or state.state == "#":
            return maxsize

        return math.sqrt(math.pow((self.x - state.x), 2) +
                         math.pow((self.y - state.y), 2))

    def set_state(self, state):
        """
        .: new
        #: obstacle
        e: oparent of current state
        *: closed state
        s: current state
        """
        if state not in ["s", ".", "#", "e", "*"]:
            return
        self.state = state

三、部分结果

四、完整Python代码

见下方联系方式