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

一、D*Lite简介

D* Lite算法是栅格模型下针对动态环境的路径规划算法, 其将LPA*算法的思想运用于动态环境下, 使得移动机器人在未知环境下可以快速重规划。D* Lite算法的优势在于, 当环境中出现新的未知障碍物时, 可以快速更新该障碍物周边节点的信息, 并将由此而导致不连续的节点重新压入优先列表中进行快速地重规划.但是, 由于是在栅格环境下, D* Lite算法所规划出的路径是八邻域的, 得到的路径并不平滑.而另一方面, 在现实未知环境下, 极有可能在距当前位置较近处出现障碍物, 此时D* Lite规划出的路径过于靠近障碍物.

D* Lite算法采用从目标节点到起始节点的搜索方式, 可以运用于起始点改变的情况, 并且引入了LPA*算法中rhs(s)的定义, 即

(1)

其中: Succ(s)表示所有节点s的后继节点, g(s')表示节点s'到目标节点的距离. D* Lite对每个栅格维护g(s)与rhs(s)两个变量, 当两者相等时, 表示该节点处于一致状态, 否则处于不一致状态.当处于不一致状态时表示该节点的路径信息是不准确的, 只有处于一致状态下的节点是已经扩展完毕、可以通行的.

参考文献：

[1]Koenig, Sven and Maxim Likhachev. “D*lite.” AAAI/IAAI (2002).

[2]杨奇峰,曲道奎,徐方.基于障碍物运动预测的移动机器人路径规划[J].计算机工程与设计, 2021.DOI:10.16208/j.issn1000-7024.2021.01.027.

二、部分代码

import math
import matplotlib.pyplot as plt
import random
import numpy as np

show_animation = False
pause_time = 0.001
p_create_random_obstacle = 0


class Node:
    def __init__(self, x: int = 0, y: int = 0, cost: float = 0.0):
        self.x = x
        self.y = y
        self.cost = cost


def add_coordinates(node1: Node, node2: Node):
    new_node = Node()
    new_node.x = node1.x + node2.x
    new_node.y = node1.y + node2.y
    new_node.cost = node1.cost + node2.cost
    return new_node


def compare_coordinates(node1: Node, node2: Node):
    return node1.x == node2.x and node1.y == node2.y

三、部分结果

四、完整Python代码

见下方名片的联系方式