motion planning notes

动画网址

动画
JPS
A*

运动规划

• front-end: path finding (global planner)
• back-end: trajectory generation（local planner or controlling planner）

path finding

• search-based
  • graph search basis
  • Dijskra/A*
  • jump point search
• sampling-based
  • probabilistic road map (PRM)
  • rapid-exploring random tree (RRT)
  • optimal sampling based method
  • advanced sampling base method
• kinodynamic
  • state-state boundary valude optimal control problem
  • state lattice search
  • kinodynamic rrt*
  • hybrid A*

trajectory generation

• minimum SNAP trajectory generation
  • differential flatness
  • minimum smap optimization
  • closed-form solution to minimum snap
  • time allocation
  • implementation in pratice

search-based method

graph includes vertices（节点） and edges(边)
grid-based graph: use grid as vertices, use grid connections as edges

• BFS
• DFS
• DIJSKRA (all-direction; DFS, 最优)
• A*（不一定是最优）
• JPS

map represention

• occupancy grid map
  二维，2.5维栅格, 导航地图

  • Most dense
  • structural
  • direct index query

• octomap
  八叉树

  • sparse
  • structural
  • indirect index query

• voxel hashing
  哈希，建立hash-table，sfm

  • Most sparse
  • structural
  • indirect index query

• point cloud map

  • un-ordered
  • no index query
  • pcl lib

• TSDF map
  Truncated Signed Distance Map
  -OpenChisel project

• ESDF

  • Euclidean Signed Distance Functions
  • incremental update

• Configuration

  • Robot Configuration : a specification of the positions of all point of robot
  • DOF: the minimum number of n of real -valued coordiantes
  • Robot Configuration Space: a n-dim space containing all possible robot configuration, C-Space
    • mobile robot: 3-dim , (x,y.theta)
    • 6 axis arm: 6-dim,(x,y,z,r,p,y)
  • Each robot pose is a point in the C-Space

• planing in C-space

  • robot is represented by a point in C-space
  • Obstacle need to be represeted in configuration space, C-obstacle (inflate obstacle)
  • C-Space = C-obstacle U C-Free

• Search-Based Method
  Graph: nodes(vertices) + edges
  Grid-Based Graph: grid as vertices, grid connecdtion as edges

  • Graph Search Overview
    
    

  • BFS (FIFO, queue)

  • DFS (LIFO), stack)

    • queue/stack 代表以上提到的search 使用的容器
    • container 代表常见算法描述中的open_llist
    • remove/expanded 的结点放入closed_list, 用来标记，以后不再访问
    • 图索搜 多用BFS
    • BFS, DIJSKRA 保证全局最优的，启发式搜索不一定
    • A* 不一定是最优，重点在启发代价函数：
      • 条件：启发代价函数h < 真实启发代价函数h*， 基本保证是最优的
      • 启发函数：Euclidean_distance / Manhattan_distance

  • Dijkstra
    

    • open_list container 使用 priory queue, 优先队列将对容器里的元素进行排序
    • 排序按照从小到大的顺序，按照g值排序
    • expanded node 放在另一个容器 closed_list，应该也是按照按照g值从小到大排序，因为dijkstra 目标是对于每个点都是最短路径！！
    • open_list 还没有被访问，等待被访问的
    • if g(m) > g(n) + Cnm 这一行代表m已经在openlist 容器中，所以需要修改cost，使其根据cost自动排序
    • 已经被expanded的结点就不需要再处理！
    • 当goal 点在目标点在open_list被弹出时即被找到

  • Search Heuristics
    greedy best first search
    启发函数一般是：距离goal的欧式、棋盘距离 inferring the lease cost to goal

  • A*： Dijkstra with a heuristic
    A* 本质上和dijkstra 是一样的, 评价函数多了一部分

    • Accumulated cost： 类似dijkstra 在g(n) , 指当前结点n，到start 的累计cost
    • Heuristic cost: 新增的cost，指当前结点n，到goal的估计最小cost，THE Estimated Least cost, h(n)
    • 所以 f(n) = g(n) + h(n)
    • 策略： 扩展open_list最小的f(n)
      

  • A* Optimality
    h(n) < h*(n) //实际使用启发代价函数小于 真实代价，否则可能产生非最优结果

  • Admissible Heuristics 如何设计启发函数

启发函数	范数	是否可行
欧式距离	L2 norm	1
曼哈顿距离	L1 norm	如果4链接栅格可以，八连接不可以
无穷		1
0	L0	1

- sub-optimal solution for A*  次优
	均衡搜索效率和结果次优路径，牺牲最优性换取速度
		- Weighted A*:    f = g + e * h,   一般e >1 意味着搜索时更接近于目标点
		- e-suboptimal:  cost(solution)  <= e*cost(optimal solution)
		- it can be orders of magnitude faster than A*
		- e = 0  -> dijkstra
		- e = 1 ->  A*
		- e > 1 -> wighted A*
		- Weighted A* ->  Anytime A* -> ARA*  -> D*

• Grid-based Path Search
  • occupied grid map
  • priority queue in c++
    std::priority_queue
    std::make_heap
    std::multimap
• The Best Heuristic (more tight heuristic)
  dx = abs(node.x - goal.x)
  dy = abs(node.y - goal.y)
  h = dx+dy + （ sqrt(2) - 2 ）* min(dx,dy) // 优化欧式距离启发函数，搜索更快
• Tie Breaker
  很多点具有相同的f指，打破这种相同f值的情况，使其具有差异
  • h = h * ( 1 + p )
    p = minimum_cost_of_one_step / expected_maximum_path_cost //一个很小的值
  • dx1 = abs(node.x - goal.x)
    dy1 = abs(node.y - goal.y)
    dx2 = abs(start.x - goal.x)
    dy2 = abs(start.y - goal.y)
    cross = abs(dx1 * dx2 - dx2 * dy1)
    h = h + cross * 0.001

---

• Jump Point Search
  系统性消灭（打破）路径对称性的方法，也是一种tie breaker
  core idea: 找到对称性，并打破他们
  区别于A*: explore all symmetric path
  JPS choose one path, JPS explores intelligently, because it always look ahead based on a rule (look ahead rule)
  特点：复杂环境（迷宫），大型地图, 大部分情况下，JPS 速度更快
  缺点：robot with limited Fov，会在会在未知区域或者空旷区域一直搜索；仅在uniform grid map
  • how it works with its rules
    
    x : 当前结点
    4：当前结点x的父亲结点
    5：当前结点x 在接下来搜索方向 （附近没有障碍物时，会递归的按照水平或者竖直方向一直向前探索，直到遇到forced neighbors, 这就是打破对称性）
  • Jumping Rules
    
    执行水平/竖直 规则，直到遇到forced neibor y, y作为x 的跳跃点
    执行对角线规则，直到遇到forced neibor 节点y, y 作为x 的跳跃点
    跳跃过程中，遇到障碍物或者边界，认为该方向上跳跃失败
    每个产生forced neibor 的节点 的 父节点 放入 优先队列
    当发现一个节点 存在forced node时，这个节点的父节点就是一个跳跃点，将其放入open list;
    x节点，就是下一个关键节点，将其放入open list，以后需要对其进行其他水平竖直对角方向的搜索，如果其他方向都搜索结束，没有其他非劣性节点发现时，将其放入closed list
• JPS overview