欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

motion planning notes

程序员文章站 2022-04-18 08:26:06
...

动画网址

动画
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
      motion planning notes
      motion planning notes

    • 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
      motion planning notes

      • 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)
        motion planning notes
    • 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
      motion planning notes
      x : 当前结点
      4:当前结点x的父亲结点
      5:当前结点x 在接下来搜索方向 (附近没有障碍物时,会递归的按照水平或者竖直方向一直向前探索,直到遇到forced neighbors, 这就是打破对称性)
    • Jumping Rules
      motion planning notes
      执行水平/竖直 规则,直到遇到forced neibor y, y作为x 的跳跃点
      执行对角线规则,直到遇到forced neibor 节点y, y 作为x 的跳跃点
      跳跃过程中,遇到障碍物或者边界,认为该方向上跳跃失败
      每个产生forced neibor 的节点 的 父节点 放入 优先队列
      当发现一个节点 存在forced node时,这个节点的父节点就是一个跳跃点,将其放入open list;
      x节点,就是下一个关键节点,将其放入open list,以后需要对其进行其他水平竖直对角方向的搜索,如果其他方向都搜索结束,没有其他非劣性节点发现时,将其放入closed list
  • JPS overview
    motion planning notes
相关标签: 定位与导航