基于搜索的路径规划——Lecture 2

1 Graph Search Basis

1.1 Configuration Space

1.1.1 Configuration Space Obstacle

1.1.2 Workspace and Configuration Space Obstacle

1.2 Graph and Search Methods

1.2.1 Search-based Method

1.3 Heuristic Search

2 Dijkstra and A*

2.1 Algorithm Workflow

A*: Dijkstra with a Heuristic

A* algorithm

+ Maintain a priority queue to store all the nodes to be expanded.
+ The heuristic function h(n) for all nodes are pre-defined.
+ The priority queue is initialized with the start state $X_s$.
+ Assign g(X_s)=0, and g(n)=infinite for all other nodes in the graph.
+ Loop 
		+ if the queue is empty, return FALSE; break;
		+ Remove the node " n " with the lowest f(n)=g(n)+h(n) from the priority queue
		+ Mark node " n " as expanded
		+ if the node " n " is the goal state, return TRUE; break;
		+ for all unexpanded neighbors " m " of node " n ":
					+ if g(m)= infinite
								+ g(m)=g(n)+C_nm
								+ Push node " m " into the queue
					+ if g(m)> g(n)+C_nm
							+ g(m)=g(n)+ C_nm
		+ end
+ End Loop
Matlab A* 流程
+ 1 把Start_node加入OPENLIST
+  2 重复以下步骤：
	+  (1) 找到openList中f最小的节点n
	+  (2) 把节点n从openlist 移除，加入closedlist
	+  (3) 扩散节点n的邻近节点m:
			+ a. 若m为障碍物或在closedlist或越界，则continue下一节点
			+ b. 若m不在openlist中，则加入opelist中，记录父节点， f，h，g等值
			+  c. 若m已在openlist中，则比较旧有的g值和新g值。
					若新g值更小，则更新节点的数据，包括父节点、h，g，f等
	+ (4) 直至以下条件时跳出循环
			a. 找到target_node，回溯路径
			b. OPENLIST为空，招不到路径
+ 3 回溯路径

2.2 Engineering Considerations

3 Jump Point Search

3.1 Algorithm Workflow

4 Homework