motion planning notes

最新推荐文章于 2024-05-22 09:43:09 发布

hoppss

最新推荐文章于 2024-05-22 09:43:09 发布

阅读量349

点赞数 1

分类专栏：定位与导航

本文链接：https://blog.csdn.net/qq_35508344/article/details/105877869

版权

定位与导航专栏收录该内容

46 篇文章 7 订阅

订阅专栏

动画网址

动画
 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

BFS: 本质上先入先出的queue
策略: remove / expand the shallowest node in the container
DFS: 本质上后入先出的stack
策略: remove / expand the deepest node in the container

多数基于搜索的算法都是基于BFS

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