运动规划之搜索算法：前端规划、后端轨迹生成到状态求解( 四 ) _算法

问题3：如何移除正确的节点以便尽快到达目标状态，从而减少图节点的扩展。

深度优先算法：数据结构维护一个后进先出（LIFO）的容器（即栈），算法移除/扩展容器中最深的节点

文章插图

#生成示例数据graph = {}graph["A"] = ["B", "D", "F"]graph["B"] = ["C", "E"]graph["D"] = ["C"]graph["F"] = ["G", "H"]graph["C"] = []graph["E"] = []graph["G"] = []graph["H"] = []from collections import dequesearch_queue = deque()# 创建一个节点列表search_queue += graph["A"]# 表示将"A"的相邻节点都添加到节点列表中from collections import dequedef search(start_node):search_queue = deque()search_queue += graph[start_node]searched = []# 这个数组用于记录检查过的节点while search_queue:# 只要节点列表不为空node = search_queue.pop()#深度优先#node = search_queue.popleft()# 广度优先取出节点列表中最左边的节点print(node, end=' ')# 打印出当前节点if not node in searched:# 如果这个节点没检查过if node == 'G':# 检查这个节点是否为终点"G"print("nfind the destination!")return Trueelse:search_queue += graph[node]# 将此节点的相邻节点都添加到节点列表中searched.Append(node)# 将这个节点标记为检查过# 如果节点列表为空仍没找到终点，则返回Falsereturn Falseprint(search("A"))

广度优先搜索算法：
数据结构：维护一个先进先出（FIFO）的容器（即队列），算法操作：移除/扩展容器中最浅的节点。具体代码参考上面深度搜索算法，把“node = search_queue.pop() #深度优先”换成“node = search_queue.popleft() # 广度优先取出节点列表中最左边的节点”即可。可以看出BFS和DFS差别就在于根据“先入”或“后入”的原则，从边界中选择下一个节点。

文章插图
贪婪搜索（Greedy Search）：
贪心算法的特点是考虑了从目标节点找到任意点的代价，而一般算法考虑的是从起始节点到任意点的代价。即贪心算法考虑的是如何快速的找到目标节点，使得到达目标节点的时间成本最?。欢?话闼惴?悸堑氖悄勘杲诘愕酱锬勘杲诘愕幕ǚ汛?凼亲钚〉模??皇强焖僬业侥勘杲诘?。基于贪心策略试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。
贪婪算法中“行动的成本”可以用启发式函数h(n)来算从任意结点n到目标结点的最小代价评估值；启发函数决定了贪婪算法运算书读，所以选择一个好的启发函数很重要。

实际的搜索问题中，从一个节点到其邻居有一个“C”的成本
可以作为启发函数计算代价的有：长度，时间，能量等
当所有权重都为1时，贪婪算法找到最优解
对于一般情况，如何尽快找到最小成本路径？
Dijkstra算法：
Dijkstra算法算是贪心思想实现的，其可以适用与拓扑图或者栅格图，具体实现方法是，首先把起点到所有点的距离存下来找个最短的，然后松弛一次再找出最短的，所谓的松弛操作就是，遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近，如果更近了就更新距离，这样把所有的点找遍之后就存下了起点到其他所有点的最短距离：