Motion Planning for Mobile Robots 学习笔记1

目录

Outline

1.Introduction
2.Course Outline
3.Typical Planning Methods Overview
4.Map Representation
5.Pre-requirement
6.Homework

Introduction

About the Course

This course is about:

·Academism(old school)planning pipeline
遵循“学院派”的思路与流程，模块化机器人的自主导航系统，包括控制、定位、感知、规划、决策，分别介绍各部分的解决方案，再进行级联。
·Path finding algorithm
【前端】在复杂的环境里、相对低维的空间里，找到在几何上安全可行的（collision free）的路径。path finding 是机器人运动规划的前端（不希望花费过多的算力与代价，先找到一个粗略的可行的低维解），可视为降维处理。
·Trajectory generation/optimization
【后端】从前端拿到粗略解之后，运用一些优化的技巧，使之成为真正的满足机器人动力学要求的路径。光滑的、高阶连续的以及有安全保证的。称之为高维的路径。
·Real-time software implementation
实时软件开发技巧。

This course is NOT about:

·Dynamics Modellng
运动学建模
·Robot designe
机器人设计
·End-to-end navigation
端到端的导航（学术活跃，不够实用）
·Learning-based method
基于学习、神经网络的方法
·Mathematical/theoretical proof
数学/理论论证

This course may also cover:

·Autonomy for mobile robots
包含定位、感知、建图等内容
·Paper recommendations
论文推荐
·Cutting-edge research direction/ethics
Cutting-edge指先进、前沿的研究方向与准则

Basic Expectation

Discipline

· Every researcher has his own taste/style. Since you choose this course, please follow my style.
坚持实用性的风格。
·Q&A only at appointed office time.
· Finish your homework by yourself.

Project

·Basic algorithm validation (MATLAB)
·Sophisticated engineering implementation (ROS/C++)

Workload

Expected Student Background

·Linear algebra
·Probability
·MATLAB programming skills
·C++programming skills (VERY IMPORTANT)
·Linux
·Love robots

Workload

·Attend lectures
·Lots of project work
·Have fun with robots

What is autonomous robot

Definition: an autonomous robot is a robot that performs behaviors or tasks with a high degree of autonomy(without external influence).

Subjects: Computer Science, Automation, Mechanism, Electronics...

· Perception-Planning-Control action loop

Sensing：传感器
Localization：定位
Mapping：感知，即建图
Planning：运动规划
Control：控制

Aerial Robots

Dam Inspection

Cellular Tower Inspection

Precision Farming

Delivery

Ground Robots

Search and Rescue

Housekeeping

Coverage planning，覆盖规划。
SLAM

Hills Driving

Hills，丘陵
判断能否通行与通行代价。

Urban Driving

无人驾驶

Crowd Driving

涉及决策规划。

What is autonomous robot?

Estimation

状态估计（定位）

Low latency
低延时
High accuracy & consistency
高精确度、高一致性

Preception

建图（感知）

3D sensing & dense perception
3D稠密建图
Map fusion &integration for planning
地图前后信息融合与如何用于规划

Planning（Focal Point）

Complex & unknown environments
复杂未知环境
Safety & dynamical feasibility
安全、动力学可行
Limited sensing & computation
有限的感知与计算能力

Control

Aggressive maneuvers
Smooth trajectory tracking

What is motion planning

Basic requirements

Safety:collision avoidance
安全性
Smoothness:energy saving,comfort
节省能量，光滑性：拍摄平稳程度、自动驾驶舒适度
Kinodynamic feasibility:executable,controllable
动力学可行性：可执行、可控制

Old-school pipeline

Front-end path finding

前端：路径发现（路径搜索）

Search for an initial safe path
相对低维空间、通常是离散的，寻找初始的路径（path）。
path通常没有时间信息，并且没有高维信息。
Low dimensional
Discrete space

Back-end trajectory generation

Search for an executable trajectory
High dimensional
Continuous space
需要使用很多的优化技巧

How to do robotics research

Find a problem

A problem only in your imagination is not a problem at all.
A robotician must be an engineer first.
Hot topic is just hot.
不要盲目追求热点。
Be honest.

Solve a problem

Don't intentionally making things complicated.
不要故意的让问题变得复杂。
Simple but effective solution is always preferable.
Simulation tells nothing, show everyone real robots.
Solve real problems.
解决真实存在的问题。
Solve it 100%, or not.

Overall knowledge of planning

Choose suitable methods for different scenarios.

Design customized strategy.
设计自定义策略：要求对规划领域具有一定的知识（上帝视角）。

Dirty hands

Don't wait, don't just read papers. Do it yourself.
A lot of coding work.
A lot of field work.
field work：野外实验。

Know the whole system well

Take care every component in your robot.
首先要是一名优秀的系统工程师，熟知各个部件。

Groups and Researchers

推荐的课题组和研究者

University of Pennsylvania

GRASP Lab，Vijay Kumar

Research Interests:planning，control，swarm
Homepage:www.kumarrobotics.org

Massachusetts Institute of Technology

Jonathan How

Research Interests:modeling，control，planning
Homepage:www.mit.edu/~jhow

Nicholas Roy

Research Interests:perception，learning
Homepage:groups.csail.mit.edu/rrg

Carnegie Mellon University

Nathan Michael

Homepage:www.rislab.org

Sebastian Scherer

Research Interests:perception，planning
Homepage:theairlab.org

University of California，Berkeley

Markus Mueller

Research Interests:control，planning

ETH Zurich

ASL Team，Roland Siegwart

Research Interests:perception，control
Homepage:aslethz.ch

Raffaello D'Andrea

Research Interests:control，swarm
Homepage:raffaello.name

University of Zurich

Davide Scaramuzza

Research Interests:perception，control
Homepage:rpq.ifi.uzh.ch

Hong Kong University of Science Technology

Shaojie Shen

Research Interests:UAV
Homepage:uav.usthk

Ming Liu

Research Interests:UGV
Homepage:ram-lab.com

Course Outline

Front-end: Path finding

前端

SEARCH-BASED PATH FINDING

基于搜索的路径规划

Graph Search Basis
图的概念与图建模：图搜索
Dijkstra and A*
Jump Point Search
前几年出现的A*的改良版本
Homework

SAMPLING-BASED PATH FINDING

基于随机采样的路径规划

Probabilistic Road Map
概率路线图：比较经典的方法
Rapidly-exploring Random Tree(RRT)
Optimal Sampling-based Methods
RRT*
Advanceld Sampling-based Methods
进阶的采样路径规划方法
Homework

KINODYNAMIC PATH FINDING

满足动力学要求的路径规划
不再简单的只考虑非常低维的（欧氏空间，\(x,y,z\)坐标的规划），而回考虑移动机器人的简单的动力学模型。
其方法与上面两个相似。

Introduction
State-state Boundary Value Optimal Control Problem
满足两点边界值约束的最优控制问题。
很多基于动力学的运动规划的最基本的数学模型。
State Lattice Search
状态图的搜索（高维的Dijkstra或A*）。
Kinodynamic RRT*
带有动力学的RRT*
Hybrid A*
State Lattice Search的改进版本，自动驾驶应用最广泛的前端路径搜索算法。
Homework

Back-end: Trajectory Generation

后端

MINIMUM SNAP TRAJECTORY GENERATION

最基本的

Differential Flatness
微分平坦性质。（理论支撑）
Minimum Snap Optimization
Minimum Snap数学表达形式与优化方法。
Closed-form Solution to Minimum Snap
Minimum Snap闭式解的求解方法。
Time Allocation
时间分配问题
Implementation in Practice
Homework

SOFT AND HARD CONSTRAINED TRAJECTORY OPTIMIZATION

硬约束、软约束下的Minimum Snap轨迹优化方法（上一章的进阶）

Soft Constrained Trajectory Optimization
软约束：轨迹优化的约束加在目标函数上，会有一个倾向性得到生成轨迹。但该约束不一定全部时间满足。
Hard Constrained Trajectory Optimization
Homework

MDP&MPC

马尔可夫决策过程与模型预测控制
经常用于移动机器人的规划中

MARKOV DECISION PROCESS-BASED PLANNING

Uncertainties in Planning and MDP
Minimax Cost Planning and Expected Cost Minimal Planning
Value lteration and Real-Time Dynamic Programming
Homework

MODEL PREDICTIVE CONTROL FOR ROBOTICS PLANNING

Introduction
Linear MPC
Non-linear MPC
Homework

Overview

主要算法概览

Front-end: Path Finding

Search-based methods

概览（Overview）

·General graph search:DFS,BFS
·Dijkstra and A* search与高效的工程实践
·Jump point search

A*：彩色的是搜索中所扩散到的节点。地图是二维栅格网络地图，每个节点与邻居是8连通的。

图建模

·For every search problem,there's a corresponding state space graph
对于任一个搜索问题，均有一个搜索图。
首先，需要构建其搜索图。

·Connectivity between nodes in the graph is represented by(directed or undirected)edges

Grid-based graph:use grid as vertikes and grid connections as edges:

For the graph generated by probabilistic roadmap(PRM):

与抽象拓扑图对照，每一个点就是节点，每一条边就是其edge。

搜索算法中应不应该有目的性？

Dijkstra VS A*

Dijkstra's algorithm expanded in all directions

A* expands mainly towards the goal，but does not hedge its bets to ensure optimality

A* VS JPS(Jump point search)

A*

JPS：跳点搜索，尽可能减少A*算法中需要扩展的节点数量。
跳跃：很长的边。
JPS并不是一定优于A*，其在大多数场景下优于A*，但在实际使用中有很大局限性。

Sampling-based methods

·Probabilistic roadmap(PRM)
·Rapidly exploring random tree(RRT)
·RRT*,informed RRT*

Probabilistic roadmap(PRM)

概率路线图

核心思想：

  1. 基于随机采样，那么在图中随机撒较多的采样点。
  2. 各采样点尽可能的与其相邻的（相邻指拓扑结构上或欧氏距离上）节点连接。同时检查边是否经过障碍物或者采样点是否在障碍物内。
  3. 得到由采样点和采样点之间的边组成的图。运用上一部分的图搜索算法，得出其path。

Rapidly exploring random tree(RRT)

快速扩展随机生成树

可理解为PRM的Incremental版本，即增量式版本。
PRM是一撒完所有采样点，然后尽可能的连接各点，并进行搜索。

而RRT是随着采样的进行，树不断地向外扩展，同时尽可能维护其上一条到起点尽可能短的路径。

RRT VS RRT*

起初人们认为，通过缩短步长，我会得到连接起点到终点的最优路径。
但是，现在人们已证明RRT不具备渐进最优性。

RRT*会对采样点周围的树进行重新组织，以维护更短的路径。
RRT*具备渐进最优性。

另外，RRT算法维护的树是杂乱无章的，但是RRT*顺滑的。

Informed RRT*

核心思想：

  1. 使用RRT*找到一条可行路径。
  2. 以起点、终点为焦点，以路径长度为长轴，做一个椭圆。（在三维空间下应为椭球）
  3. 之后的采样仅在椭圆（椭球）内进行。
  4. 迭代进行此过程。

In the following examples, RRT* and Informed RRT* were run until they found solutions of equivalent cost（等效成本）.
Informed RRT* talkes an order of magnitude fewer iterations than RRT* with no practical increase in computational cost.
Informed RRT* makes no simplifying assumptions or approximations about the state space.

Kinodynamic Path Finding

基本思想：

  1. 建立机器人的动力学模型；

  2. 把机器人的控制量进行离散化；
    例如，对于汽车，在固定油门下，在不同的方向盘角度，即可产生不同的汽车行进路径，

    将很多条这样的路径连接起来（意为：在第一次的终点，再按相同的逻辑，得到新的一簇曲线），会得到：

    图内节点与节点间仍以边连接，但与栅格网络节点所不同的是，这里的每一个节点不光包括\(x,y,z\)坐标，还包含状态信息（例如，速度或更高维的信息）。

  3. 节点之间的状态转移的曲线是上图中的边，这也是可以用图搜索算法寻找路径的。

注意：这种算法的代价高昂，如果希望在线构建Lattice Graph代价很高。即使是仅进行在线搜索（离线建好Lattice Graph）也是代价高昂的。

如何使这个算法可以有效地运用于移动机器人呢？

Hybrid A*

基本框架与State Lattice Search很像，都是建立一个状态空间模型或非线性模型，离散化后控制量驱动其完成状态转移。

不同之处：Hybrid A*会维护一个栅格网络地图，会在State Lattice扩展的过程中（如下图），

依据栅格网络地图本身的结构，对树进行一个很暴力的剪枝。

特点：

  1. Follow A* algorithm
  2. Forward simulate states with different discrete control inputs
  3. Keep only 1state in each grid

 

  1. 从起点开始，将不同的离散控制输入应用在机器人上，其会得到不同的状态转移。Forward simulate states with different discrete control inputs

  2. 当状态转移到第2个格子时，记录格子里保留的状态。Keep only 1 state in each grid。状态点可能不同于传统的A*算法（落在栅格中心），其可落在栅格内任意位置。

  3. 当网格内出现另一个状态，且其可维护一个从起点到此栅格代价更小的路径时，用新出现的状态取代原有状态。

例子

无人车的路径规划，起点与终点均要求了速度的指向。

无人机的运动规划的前端：

Kinodynamic RRT*

前面所提到的还是基于搜索的路径规划算法，Kinodynamic RRT*是基于随机采样的Kinodynamic算法。

特点：

  1. Follow RRT* algorithm
  2. Sample a random state
  3. Solve two state boundary optimal control problem

例子

例如，对起点和终点，除位置信息之外，还包含状态信息（如速度等）。终点的速度指向是采样出来的。
显然，连接两个向量的线段有无穷多个，但如何寻求最优解呢？

这就是Solve two state boundary optimal control problem（两点边界值的最优控制问题）。

与前面不同的是，前面是离散控制量，这里是离散状态。

应用

Back-end: Trajectory Optimization

Basic Minimum-snap

  1. 使用RRT*，寻找到几何上安全的一条路径

  2. 将path的分段点，作为Minimum-snap的路标点，生成Minimum-snap轨迹。
    注意：path本身是不会碰到障碍物的，但生成的trajectory有可能会碰到障碍物。如果某处可能会碰撞，则在两个路标点之间的中点作为新的路标点。相当于，在曲线trajectory可能发生碰撞的地方，使其逼近前端的path（直线）。

比较先进的轨迹优化方法

Hard constrained Minimum-snap

在复杂环境中发现安全的区域将其表示为一个个互相连接起来的凸的解空间，在其中施加一些约束，使得最后生成的轨迹仍然在解空间内。

涉及的问题：

  1. 如何在机器人运行过程时，从未知复杂环境中的安全区域将用凸的解空间表示出来；

  2. 有了凸的解空间之后，如何数学表示这一问题；（数学建模）

  3. 如何求解。

Soft constrained Minimum-snap

一般用于局部的在线轨迹规划/重规划（Local Planning/Replanning）。

基本思想：

  1. 环境里的障碍物会提供一个距离场（梯度信息），会将机器人推向障碍物中间位置。
  2. 优化时保证整个轨迹是光滑的，并在目标函数中加入机器人动力学可行性的期望。最终表述为多项相加并加权的Minimum-snap轨迹优化问题。

与Hard constrained Minimum-snap的不同：

障碍物不是硬性加在上面的（解空间），而是靠梯度一点点推进去的。

Map Representation

移动机器人的地图部分，分为地图存储信息的Data Structure（数据结构）和信息融合的方法Fusion Method（融合算法）两部分。这两部分是相互独立的。

Occupancy grid map

占用的栅格地图

3D：

2D：

2.5D：一般称之为“海拔地图”，在2D平面对地图空间进行划分，但每个网格有一个高度。

特点

·Most Dense
最稠密的
·Structural
非常结构化的
·Direct Index Query
优势：可直接进行坐标索引查询

https://github.com/ANYbotics/grid_map

当对空间进行细密的切分时（网格小，分辨率高），占用的空间非常大。
是否有必要对所有空间进行细密切分？

没有必要。
真正包含障碍物的区域很小。👇

Octo-map

八叉数据图

来源于八叉树。

操作方法

对于空旷的区域，用一个大方块表示。如果发现其中有障碍物，则递归的切分下去，直到得到最小的、包含障碍物的方块为止。

特点

·Sparse
稀疏的
·Structural
很结构化的
·Indirect Index Query
非直接的坐标索引查询（按树的结构递归查询）

Demo

八叉树地图表示激光雷达数据

调节八叉树地图的分辨率（即允许的最小的装载障碍物的方块大小）

https://octomap.github.io/

Voxel hashing

与八叉树地图思路相近的方法。

Voxel：三维grid
hashing：用哈希表的形式将其存储到内存中

操作方法

相机（绿色）对障碍物曲面（蓝色）进行观测，只在受到影响的位置（蓝色与红色方格），加入内存。

加入内存的方式不像前面两种，而是在内存中离散的存在的。为了访问数据，需要使用哈希表。
哈希表的作用在于将当前格子的\(x,y,z\)坐标信息通过复杂的哈希函数，映射为哈希键值。（类比于字典）

注意

我们一般不会将所有的格子都进行哈希存储，而是分成两级：blocks（一大块）与voxel（最精细的小栅格）。一般地，\(5\times 5\)的voxel组成一个block，而后我们将每个block哈希存储。

因此，当查询时：

  1. 根据坐标算出所查内容在哪个block中；
  2. 根据block坐标计算哈希键值；
  3. 提取block信息并在其中精细访问所查voxel信息。

特点

·Most Sparse
最稀疏的
典型应用：基于RGBD视觉的稠密的三维重建/SLAM。

InfiniTAM
前端的三维重建与后端用Voxel Hashing均有很好的开源代码。

·Structural
·Indirect Index Query

Point cloud map

点云
点云一般是传感器最原始测量的数据（Raw Measurement）

特点

·Un-ordered
无序的
·No Index Query
没有办法进行坐标索引查询

例子


上图中的点云可以视为点的无序的集合，如果给出一个坐标，无法得知此点对应的内存位置。

如何用点云进行运动规划的研究？
后续会讲到相关技巧。

点云一般用来表示非常稠密的障碍物信息。

PCL

TSDF(Truncated Signed Distance Functions) map

Truncated Signed Distance Functions Map
截断符号距离函数图


如上图，有一传感器对障碍物（曲面）进行观测，假设在相机与曲面之间存在一距离场，距离场的值均为正值，表示当前点到最邻近的障碍物（曲面上的点）的距离。障碍物后面仍存在距离场（红色），同样地表示到最邻近障碍物的距离，但为负值。

Truncated：截断
意为不需要维护整个地图所有格点的值：

  1. 首先，视锥以外的灰色点不关心；
  2. 其次，按照距离进行截断，例如，仅考虑距离曲面2m内的点；

从不同视角观测到的距离场可进行一定的融合。

OpenChisel

ESDF(Euclidean Signed Distance Functions) map

Euclidean Signed Distance Functions Map
欧式符号距离函数图

与TSDF的区别

不进行截断，所有位置的距离场值都关心为什么？
原因在于，需要用这些值做planning。
例如，在软约束轨迹优化方法中，为保证所生成轨迹尽可能地远离障碍物，需要使用距离场里面轨迹到障碍物的梯度信息，将轨迹推向中间。在这种情况下，ESDF非常重要。为什么？
因为若使用TSDF，在灰色格点（无距离场信息），则无法评估当前的梯度，则无法将轨迹推向中心。因此需要在整个所关心的范围内的距离场信息。

Batch Update,Local Map
颜色越接近红色离障碍物越近，绿色反之

Distance Transforms of Sampled Functions,PF Felzenszwalb
 
还有一种是Incremental Update,Global Map

VoxBlox，ETH Helen的作品，Global Map，实现尽快、尽量节省计算资源的条件下，建立全局ESDF
FIESTA，高飞作品，Incremental Update,Global Map，算法精度和性能远超VoxBlox
TRR's Local Map，高飞作品，ESDF在完整的无人机自主导航系统的集成，Batch Update,Local Map，参考论文：Distance Transforms of Sampled Functions,PF Felzenszwalb，采用批次处理方式，复杂度等于节点个数。当机器人移动后，不在范围内的ESDF值会被遗忘（Local）。

More

Free-space Roadmap

在复杂环境下用凸多边形提取安全空间的工具（Teach-Repeat-Replan功能下的），构建Free-space Roadmap（路线图）。

在概率路线图中，节点是通过随机采样的点，而在Free-space Roadmap中，节点是可通行的安全区域（Free-space）用凸多面体表示，同样地，依据概率路线图中的规划算法，将各个凸多面体连接起来，在可通行区域里生成轨迹。

优点

得到了比较大的解空间，且解空间的每个模块都是凸多面体，会具有很多良好的性质。

https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan

Voronoi Diagram Map

ETH的拓扑地图

基于ESDF地图，高效地提取地图的骨架（拓扑结构）。

如图，提取出稀疏的骨架。
当你做大规模、全局移动机器人的路径规划时，尤其是重视路径规划的拓扑结构时可以先在拓扑地图上进行。

https://github.com/ethz-asl/mav_voxblox_planning

Pre-requirement

Linux

·Linux file system
·How to install software in linux
·Useful commands

C++ and GCC Toolchain

·C with class?
·Gcc,Makefile,CMakeList
·Write CMakeList
·How to solve problems:google and document

ROS

·Please install ROS
·Follow ROS tutorial
跑一下ROS入门的Demo
·Ubuntu 16.04 + ros kinetic is recommended

Matlab

·Please install Matlab

Homework

·Learn Linux
·Configure ROS
·Configure the environment for C++ homework
了解框架：包括：复杂地图的随机生成、交互为移动机器人选择目标点等

posted @ 2020-05-29 11:31  HAN_Letisl  阅读(828)  评论(0)  编辑  收藏  举报