Motion Planning for Mobile Robots 学习笔记4

What this note covers

第1节: 动力学概念介绍

动力学概念简介(Introduction)

第2节: 状态栅格搜索算法

状态栅格搜索算法(State Lattice Search)

第3节: 两点边界值最优控制问题

两点边界值最优控制问题

第4节: 混合A*算法

混合A*算法(Hybrid A*)

第5节: 动力学约束RRT*算法

动力学约束RRT*算法(Kinodynamic RRT*)

Outline

Introduction
State Lattice Planning
Kinodynamic RRT*
Hybrid A*
Homework

Introduce

What is Kinodynamic

Kinodynamic: Kinematic + Dynamic

The kinodynamic planning problem is to synthesize a robot motion subject to simultaneous kinematic onstraints, such as avoiding obstacles, and dynamics constraints, such as modulus bounds on velocity, cceleration, and force. A kinodynamic solution is a mapping from time to generalized forces or accelerations.
满足动力学约束的规划问题是生成一种受限于运动学的约束（比如避障）以及动力学的约束（比如速度、加速度与力的上下限约束）。
————Kinodynamic Motion Planning, Bruce Donald, Patrick Xavier, John Canny, John Reif

Avoiding obstracles
避障
Differentially constrained
受到高阶约束（微分约束，考虑到力）
Up to force (acceleration)

Why kinodynamic planning?

Straight-line connections between pairs of states are typically not valid trajectories due to the system's differential constraints.
由于系统的微分约束，折线段构成的轨迹通常是无效的。

Ask: We have the back-end optimization, why kinodynamic planning?
为什么有后端的轨迹优化，还要在前端考虑机器人的动力学约束呢？

在前端考虑一部分动力学约束，可以减轻后端的负担。

Coarse-to-fine process
Trajectory only optimizes locally
轨迹很可能只能在局部被优化

例如上图所示，不考虑动力学约束生成的轨迹如左侧实线所示，即使后端优化后也不如右侧轨迹。
Infeasible path means nothing to nonholonomic system

Examples

自行车模型：

其中包括自行车的速度，与转动车把的角速度。

上图列出了其动力学约束。

复杂一点：
考虑两个轮子：

http://planning.cs.uiuc.edu/node659.html

简化汽车模型：

http://planning.cs.uiuc.edu/node658.html

State Lattice Planning

Workflow

Basic Idea

Recall the search-based path finding method in L2
For planning, how to build a graph?
Is this graph doable for our real robot?

路径规划算法的核心：
根据任务需求，灵活地构建Search Tree（搜索树）。

有了搜索树之后，就可以利用各种各样的图搜索算法解决问题。

We have many weapons to attack graph search.
我们已经有很多图搜索算法储备。
Assume the robot a mass point is not satisfactory any more.
假设机器人的质点模型不再适用。
We now require a graph with feasible motion connections.
我们需要满足动力学约束（每个边都是对机器人运动可行的连接）的连接图。

We manually create (build) a graph with all edges executable by the robot.
因此，只需要构造一个图，其中每条边都是机器人可运动的。

Forward direction:discrete (sample) in control space
正向方法：离散机器人控制空间，驱动机器人运动，得到feasible motion connection。
Reverse direction:discrete (sample) in state space
反向方法：离散机器人状态空间，在周围环境产生多个机器人状态点，之后寻找一条从当前状态到某一状态的连接。
This is the basic motivation for all kinodynamic planning.
这就是Kinodynamic planning的基本方法。
State lattice planning is the most straight-forward one.
State Lattice Planning是最直接的一种方法。

Connection with previous lectures

Actually, this is a discretization of control space!
其实，这就是对机器人控制空间的离散。
We assume the robot can move in 4/8 directions
也就是说，我们假设机器人可以向4/8个方向移动。

Actually, this is a discretization of state space!
其实，这就是对机器人状态空间的离散。
Here the state is \(R^2\), only position (x, y) is considered here.
这里仅考虑二维空间，而且机器人仅作为质点。

Build the graph, sample in control vs. state space

For a robot model: \(\dot{s}=f(s,u)\)
s表示其状态，对于二维的机器人，可能包括\(s(x,y,\dot{x},\dot{y},\cdots)\)
u表示输入
等式表达的意思为，其状态的导数由当前状态和输入决定。

The robot is differentially driven.
We have an initial state \(S_0\) of the robot.
Generate feasible local motions by:
Select a u, fix a time duration T, forward simulate the system (numerical integration).
给不同的输入u，持续一定的时间，就可以得到不同的表现。
- Forward simulation
- Fixed u, T
- No mission guidance,
  没有任何导向性（不知道应该往哪个方向运动）
- low planning efficiency
  低效率（原因是没有导向性，并非指算法运行效率低）
- East to implement
  非常易于实现
Select a \(S_f\), find the connection (a trajectory) between \(S_0\) and \(S_f\).

已知两个状态，去解连接他们的边。
- Backward calculation
- Need calculate u, T
  需要计算出u与T，当然为了计算方便，可以简化为给定一个值，求解另一个值
- Good mission guidance
  有非常好的导向性
- High planning efficiency
  高效
- Hard to implement
  难于实现

Sample in Control Space

控制空间中采样的示例

给出一个无人机的模型：

该模型可以一般化地写为：

Several-order integrator
将状态方程写成一个一定阶数的积分器
A nilpotent
A是一个幂零矩阵，可以带来计算上的简化，具体的将，在某次方后，A的幂次方均为0
Very special, remember it

如何还原中间运行轨迹？

使用状态转移方程。
前面是0输入响应，后面是0状态响应。
\(e^{At}\)是状态转移矩阵。

对于维度不是很高的情况下，完全可以写成代数式。

Search-based Motion Planning for Quadrotors using Linear Quadratic Minimum Time Control, Sikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar

注意：并不需要构建整个图后再进行搜索，而是在启发函数的帮助下，有目标地向指定区域扩展。
此时使用在线构建的方法，可以节省很多时间和资源。

Note

During searching, the graph can be built when necessary.
Create nodes (state) and edges (motion primitive) when nodes are newly discovered.
Save computational time/space.

另一个模型：

For every \(s\in T\) from the search tree
Pick a control vector u
Integrate the equation over short duration
固定一个时间
Add collision-free motions to the search tree

Sample in State Space

在状态空间中离散化

Build a lattice graph: Given an origin.
for 8 neighbor nodes around the origin, feasible paths are found.
extend outward to 24 neighbors.
complete lattice.

Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces, Mihail Pivtoraiko and Alonzo Kelly

Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly

State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments, Thomas M. Howard, Colin J. Green, and Alonzo Kelly

Boundary Value Problem (BVP)

根据起点和终点，反算其路径。

BVP is the basis of state sampled lattice planning.
No general solution. Case by case design.
Often evolve complicated numerical optimization.

对于上图的起点与终点，如何求其x(t)呢？

首先，假设x(t)是5次多项式，
随后，根据边界条件，注意其中的速度加速度是无要求的，也即可以任意假设（不妨设为0）
最后，求解之。
注意：求解的方法是将边界条件带入x(t)的表达式，并将x(t)求导后带入速度，求二次导后带入加速度。

注意：求一个解并不复杂，但求一个最优解并不容易。

【难点】Optimal Boundary Value Problem (OBVP)

优化目标：在整个轨迹上，加速度的变化量的平方和最小。
为了简化，仅考虑一个轴（三轴分开考虑）。

方法：极小值原理(Minimum Principle)
解决最优控制问题

求解：

这里面应该注意：
costate（谐态），上面状态有几个量，就有几个谐态。
定义一个Hamiltonian Function。

参考书目：A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, Mark W. Mueller, Markus Hehn, and Raffaelo D' Andrea
Dynamic Programming and Optimal Control D. P. Bertsekas

Pontryagin's Minimum Principle

Generally,
一般地，一个最优控制的目标函数，由以下两项构成：

第一项：表示与末状态距离的惩罚项
第二项：从初状态到现在的能量损耗

写出Hamiltonian Function以及谐态项：

Hamiltonian Function是系统的状态、输入以及谐变量联合构成的形式。

需要求解：

由：

应用极小值原理，得：

具体案例

模型

求解

由极小值原理，设其谐态为\(\lambda_1,\lambda_2,\lambda_3\)

设其哈密尔顿方程为

\[\begin{aligned} H(s,u,\lambda)&=\frac{1}{T}j^2+\lambda^{\rm T}f_s(s,u)\\ &=\frac{1}{T}j^2+\lambda_1v+\lambda_2a+\lambda_3j \end{aligned} \]

根据极小值原理，\(\dot{\lambda}\)为

\[\dot{\lambda}=-\nabla_sH(s^*,u^*,\lambda)=(0,-\lambda_1,-\lambda_2) \]

由此，得：

此处需要留意，为什么没有边界条件

\[\lambda(T)=-\nablah(s^*(T)) \]

由于问题中要求最后状态必须到达，故不具备原边界条件（h不可导），在J中也没有关于末尾时刻的惩罚项。
事实上，h为

那么，本应利用边界条件求解的\(\lambda(T)\)应该如何求解呢？
求解方法，令

\[s(T)=s_f \]

求解最优控制

注意，由于此时\(s\)已经取导最优解\(s^*\)，因此

\[H(s,u,\lambda)=\frac{1}{T}j^2+\lambda_1v^*+\lambda_2a^*+\lambda_3j \]

即与\(s\)有关的已为最优，无需考虑。
与\(j\)有关的项为：

\[H(s,u,\lambda)=\frac{1}{T}j^2+\lambda_3j \]

选取\(u\)（\(u\)与\(j\)同含义）的值，使得\(H\)函数值最小，得

那么，最优的状态为\(u\)的多次积分可得

且满足初始值

轨迹代价：

待定系数\(\alpha,\beta,\gamma\)由末尾时刻状态得到

解出

J仅取决于T，并且边界状态（已知），因此我们甚至可以获得最佳T！

值得注意的是，如果在末尾时刻只指定部分量（如位置），那么边界条件对于不指定的量仍然成立。
此时，其不确定的量可用来优化J。

Another Example

汽车求OBVP问题

此处一般是Off-line，因为很难求得其闭式解（解析解），因此使用数值方法计算是较慢的。

Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly

状态空间采样：

控制空间采样：

离线的采样完成后，可以简化为在线的图搜索问题。

轨迹库（Trajectory Library）

单层晶格规划是避免局部碰撞的常见选择。
不进行图搜索，仅选择轨迹。（控制空间单层采样得到轨迹）
基于多个成本函数（碰撞风险，信息获取，舒适度，能量，...）对每个轨迹进行评级。（计算出来代价进行对比，不进行优化）
【动态窗口法】

Maximum Likelihood Path Planning for Fast Aerial Maneuvers and Collision Avoidance Ji Zhang, Chen Hu, Rushat Gupta Chadha, and Sanjiv Singh

Heuristic（启发性）

设计启发式函数的原理：解决一个简单的问题

假设没有障碍物存在
假设没有运动学模型存在

一般情况下，可能会不考虑运动学模型而只考虑障碍物的存在，进行启发式函数设计。

两种不同的启发式函数对比：

第二种启发式函数是不考虑障碍物仅考虑运动学下生成的OBVP的解作为启发函数。

Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, James Diebel

Planning in Frenet-serret Frame（在活动坐标系下的Lattice Planning）

Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun
Optimal trajectories for time-critical street scenarios using discretized terminal manifolds, Moritz Werling, Sören Kammel, Julius Ziegler and Lutz Gröll

沿车道的切向与径向的活动坐标系下进行

https://pythonrobotics.readthedocs.io/en/latest/modules/path_planning.html#optimal-trajectory-in-a-frenet-frame

Hybrid A*

Workflow

Basic Idea

在线生成密集的晶格花费太多时间，同时多条临近的轨迹可能同时失效。
那么，修剪一些节点怎么样？
定义修剪规则：使用网格图修剪。

Practical Search Techniques in Path Planning for Autonomous Driving, Dmitri Dolgov, Sebastian Thrun
Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments, Dmitri Dolgov, Sebastian Thrun

具体做法：选取不同的输入\(u\)向前积分，得到的state在每个栅格中只记录一个。
迭代原则：若新得到的state具有更小的累计代价（不仅是路径长度），则替代原记录。

Detial

\(g(n),f(n)\)的计算与普通A*不同，在Hybrid A*中要选取合适的启发式函数（与前面的准则不同）
寻找邻居依赖于输入\(u\)积分产生的状态
在栅格中要记录其状态
更新时要更新全部的状态

Heuristic design

（a）二维欧几里得距离
（b）不考虑障碍物考虑运动学
（c）不考虑障碍物考虑运动学，在死角处表现不佳
（d）非完整无障碍 + 考虑障碍物不考虑运动学（也即二维最短路径）
注意，此处的+不一定是普通意义的加法，也可以是\(\rm max()\)等操作。

Other tricks

Analytic Expansions: One shot heuristio
以一定的概率直接测试是否可直达障碍物

Applications

Practical Search Techniques in Path Planning for Autonomous Driving. Dmitri Dolgov, Sebastian Thrun

Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight Boyu Zhou, Fei Gao
https://github. com/HKUST-Aerial-Robotics/Fast-Planner