【转载】深蓝学院-运动规划重点笔记_advanced bit* (abit*): sampling-based planning wit

原文链接：https://blog.csdn.net/wqwqqwqw1231/article/details/107310965

基于图搜索的方法

配置空间：维度等于机器人的自由度，可以理解为一个点可以表示一个机器人的位姿。例如小车4自由度(x,y,z,θ)。在配置空间中，机器人表示为点。

在3维空间中，要做碰撞检测，就很麻烦。所以在配置空间中做规划，要对障碍物按照机器人的尺寸做膨胀。

基于图搜索的算法框架：

关键问题：
如果一个结点被弹出容器，就不再会被加入到容器中

BFS使用的容器是队列，DFS使用的是栈。在边的权重都为1的情况下，BFS能保证路径最短，所以搜索算法是基于BFS的。

Dijkstra

与BFS相比，Dijkstra从容器中弹出的规则不同。Dijkstra弹出的是从起点到某点的走过的距离最短的点。也就是说，容器不再使用队列，而是优先队列，弹出的总是走过路径最短的点。

优点：完备的，能保证最优解
缺点：没有启发，就是暴力搜

A*

相比于Dijkstra，A*的容器对元素的排序依赖的是（走过的路径的距离+启发距离）。某节点启发距离是指预测的从该节点到终点的距离。

当启发距离小于真实距离的时候，A*能找到最优解。

工程改进：
1）使用Diagonal Heuristic：用norm做的启发函数都过分的低估了实际距离，使得搜索的区域很大
2）Tie Breaker：可能存在很多条路都是最优解。打破路径的对称性，使得搜索更偏向于一条路

JPS（Jump Point Search）

Look Ahead Rule + Jumping Rule
相比于A*，visit一个结点后，使用Jumping Rule来扩展该节点，把从该节点jumping的后继结点加入到容器中

对于复杂环境，JPS总的来说要比A*好
JPS只能用于uniform grid使用

基于采样的方法

比较高效

三种完备性

• Complete Planner: always answers a path planning query correctly in bounded time.
• Probabilistic Complete Planner: if a solution exists, planner will eventually find it, using random sampling (e.g. Monte Carlo sampling)
• Resolution Complete Planner: same as above but based on a deterministic sampling (e.g sampling on a fixed grid).

概率路图

Learning Phase
先在空间中撒点，去掉与障碍物碰撞的点
然后构建kNN的图，图中的节点包括起始点、终止点和保留下来的撒点。然后去掉边长超出一定阈值的边、去掉与障碍物碰撞的边

Query Phase
使用图搜索的方法，就可以找到一条可行驶路径。

优点：概率完备的，相比于图搜索的比较高效
缺点：边界值问题，不够有效率

优化效率：
碰撞检测：lazy collision-checking
在撒点和构建图的时候，不管碰撞问题。在搜索到轨迹之后，再删去碰撞部分的轨迹，重新搜索部分轨迹。

RRT

Pros:
· Aims to find a path from the start to the goal
· More target-oriented than PRM
Cons:
· Not optimal solution
· Not efficient(leave room for improvement)
· Sample in the whole space

改进：
kd-tree做最近点搜索
Bidirectional RRT/Connect RRT：通过一次采样，完成两颗树的构建。

Note: 这里没讲是怎么构建kd-tree的。按照RRT的思路，采样点是逐渐加进去的，但加进去的过程要使用kd-tree找最近邻。所以，kd-tree也是增量式的构建吗？kd-tree其实本身是针对静态数据集的，在网上我也基本没有搜到kd-tree的增量式的构建方式。问题在于，插入一个点会影响整棵树的结构。所以我查到的方式是使用替罪羊树这种方法来完成增量式构建。或者kd-tree不是增量式构建，就是每一次查找都构建一棵树？这样子是不是有点太慢了。

RRT*

解决生成路径不最优的问题

通过撒点找到的新点xnew​不是直接连接到最近点上，而是先找一个领域内的点，选择一个连接使得xnew​到起始点最近，连接上去。然后更新领域内的点到起始点的距离。

RRT*在生成路径之后，继续采样可以优化路径

Kinodynamic-RRT*

连接xnew​与父节点不用直线，而是用曲线，使得更符合机器人的运动学约束

Anytime-RRT*

在机器人运动过程中，一直在更新RRT*

Advanced Sampling-based Methods

Informed RRT*

当路径生成之后，将采样的范围限定在与生成路径有关的椭圆内，从而完成路径的优化。

Cross-entropy motion planning

首先得到一个路径
然后以路径中的每个点作为一个高斯模型的中心，在多高斯模型中采样，得到多条路径。
然后对多条路径做均值，重新构建多高斯模型。

Note：具体的如何做均值的，可以使用k-means这种方式。但具体的还得看论文

Kinodynamic Path Finding

State Lattice Planning

对空间采样的两种方法：
前向模拟：通过采样控制量，推算下一步车的位置。优点是容易实现，缺点是非任务导向的、效率低
反向计算：通过采样空间中的位置，反算两点之间的控制量，反算边，有点是任务导向，缺点是难实现

在反向计算中，给出采样空间的位置，如何生成一条满足动力学要求的边呢？这就是个Boundary Value Problem

BVP

如何获得一个BVP最好的解

使用经典套路，Pontryain’s minimum principle来求解。

在做好啊lattice之后，还是要搜索的。也可以用一些启发式的方法。启发函数可以考虑动力学约束，也可以考虑障碍物约束。

Hybrid A*

A*不考虑运动学约束，而lattice又太过于稠密。那使用grid来剪枝lattice生成的轨迹，就可以既符合运动学约束，又不至于太过于稠密。

核心思想就是，每个grid中只能有一个点。

具体的操作区别于A*的有：
1）在从容器中拿出一个结点，要扩展这个结点，不是通过4连接或者8连接，而是通过生成lattice来扩展这个结点。
2）扩展结点之后，生成的新的点可能已经落在已经有点的格子里，这就要比较新生成的点的cost和之前的cost，如果新生成的cost少的话，还需要更新这个格子中点的信息。
3）在容器中找到一个结点时，要利用启发函数，这个启发函数不仅仅是欧式距离这么简单，要考虑non-holonomic和obstacles的约束。

obstacle的约束，可以从终点Dijkstra搜一遍每个格子最短路径，然后存下来，在启发的时候用。
non-nolonomic，

Analytic Expansions：以一定概率，在某次扩展树之后，直接尝试生成一条由当前节点联通目标位置的轨迹，如果该轨迹不碰撞障碍物，就可以结束了。

Kinodynamic RRT*

与RRT_不同的是：
1）采样新的点不是在坐标空间中，而是在状态空间中。例如，如果状态空间包括(p,v)，位置和速度，那么就是6维空间。
2）计算xnew​的邻域不是使用欧式距离，而是一定的cost之内。在RRT_中，在采样到一个xnew​，要找个一个邻域内所有点。在Kinodynamic RRT*中，这个邻域是用cost计算，计算这个邻域是有标准的方法的。

Minimum Snap

边界条件：起始状态，目标状态
中继状态
光滑准则

微分平坦

一个全维度的状态空间可以被一组精心挑选的变量及其导数的代数变换所表示。由此，路径规划，就可以在这组精心挑选的变量的空间所进行。

对于四旋翼飞机来说，状态空间是12维度，包括3个位置，3个角度，3个线速度，3个角速度。可以用3个位置和偏航角这4个量及其导数表示。所以，路径规划可以在4维空间中进行。

用多项式来表示轨迹的好处：
1）多项式的光滑准则很好，例如某阶导数的平方的积分
2）求导好求
3）3维度方向上都是多项式，可以解耦地规划估计

用minimun snap求解分段规划中，中继点的速度和加速度的条件

问题定义：

1、确定分段路径，每一段路径的多项式的阶次
如果优化jerk，路径选5阶
如果优化snap，路径选7阶

2、写出优化的cost，每一段路径可以写成二次型的形式

3、写出每一段路径的边界条件，每一段路径的边界条件只有位置（初始和目标有多次导数），也可以写成矩阵的形式

4、写出两段路径连接处连续的约束，高阶导数连续，也能写成矩阵的形式

5、把上述整合起来，是一个QP问题。

6、QP问题的数值解有很多方法可以求解

求minimun snap的数值解

1、先把每段路径的参数转为边界的导数值，相当于换了优化问题的自变量。

2、整理自变量，通过选择矩阵，将两段路径共用的边界值消掉，其实这一步就消去了约束

3、消去了约束，就是接二次规划问题，直接求导等于零，得到极值解

然而这种优化存在超调，可能会引起轨迹不安全

工程实际

time normalization & scale normalization

时间分配

轨迹优化

硬约束

corridor-based

1、首先用八叉树来描述整个地图

2、在八叉树中搜索从起点到终点的一条轨迹
Note：需要在八叉树中寻找一个格子的相邻的格子，就是怎么扩展结点

3、将轨迹中的格子沿每个轴进行膨胀，膨胀到遇到障碍物或者到达地图边缘，并去掉被多余的格子。剩余格子的数量就是轨迹的段数。
Note：膨胀的算法

4、描述路径的cost，与minimun snap中一样，能写成二次型；两段轨迹断点高阶导数连续，能写成线性约束；两段轨迹的连接点在两个格子的交区域，能写成不等式约束。

优点：凸优化问题；限定中继点在一个区域，有效地解决了时间分配问题
缺点：路径不能保证是安全的

5、求解轨迹之后，检查约束。如果发生碰撞，就把极值点往下压，在极值上施加新的约束。通过迭代，找到一条轨迹。

贝塞尔曲线优化

用bernstein多项式代替普通的多项式

贝塞尔曲线是与多项式一一对应的。贝塞尔曲线的贝塞尔曲线拥有以下性质：
1）必定过第一个控制点和最后一个控制点
2）曲线被控制点所组成的凸包包含
3）曲线的导数非常容易求解
4）缺点是，t只能是[0,1]，所以需要对轨迹进行缩放

使用贝塞尔曲线，可以重新写cost和约束：
1）cost仍然是二次型
2）端点约束是线性等式约束
3）连续性约束也是等式约束
4）安全约束可以写成控制点在安全走廊内，是一个线性不等式约束

总体的来说，QP问题。

软约束

硬约束认为安全走廊中所有区域是等价的。所以，可能会生成一条轨迹，沿着走廊的边，这时，这个轨迹就与障碍物很近。

软约束则是把安全约束作为cost加入优化中。所以，不能保证安全性。

cost分为三项：

Smoothness cost

其中Smoothness与Minimum Snap方法一样，首先可以写成二次型，然后用中继点的(p,v,a)来代替路径多项式的参数，然后将确定变量和不定变量分开。值得注意的是，在Minimum Snap中，是要确定中继点的，所以中继点的位置是不变量，但在本cost中，中继点的位置也是可变量。

要优化的变量就变成了dp​，那么对dp​求导，其中μ代表xyz中的一个维度。

Collision cost

p(t)代表t时刻的位置，c(p(t))代表障碍物对该位置的惩罚，然后沿着路径积分。

c(p(t))的确定方式，首先可以构建一个Euclidean signed distance field（ESDF），每个格子储存其到障碍物的最小距离。然后再经过一个幂次函数，或者barrier function，使得当里障碍物特别近的时候，惩罚急剧上升。

由于是对路径积分，求导不太好求，所以把路径离散化掉，再对dpμ​求导。其中，c(p(Tk​))和∣∣v(t)∣∣是与dpμ​有关，δt是人为设定的，常量，无关。

所以

Dynamical cost

形式其实是和collision cost一样，只不过，cv​不用ESDF，而是一个barrier function。

求解

以上，得到了所有的cost和其导数。那么使用梯度下降的方式就可以进行优化。在某一次迭代过程中，有了dp​，可以计算路径，给定δt，可以将路径离散，求cost的导数。然后利用导数更新dp​

工程实践

只优化前面一段的路径，间隔一段时间优化一次，保持车辆或者无人机行驶的稳定性。
两步走的优化，先只考虑collision cost，然后再加上其他优化

MDP-based Planning

uncertainties

nondeterministic：完全不知道会受什么样的干扰
probalistic：通过经验积累，机器人可能能知道收到干扰的概率等信息

Minimax Cost Planning

对于nondeterministic的干扰，一般采取worst case analysis来分析，如下
总认为环境会把机器人向最坏的方向干扰，从中，我们选取一个最好的策略。

根据worst case的cost-to-goal，我们可以得到反向求解，知道k+1步的cost-to-goal，可以计算k步的cost-to-goal。

因此，给定一个graph表示地图，令终点的cost为0，其他节点为inf，然后反向搜索，更新每个节点的cost。其实就是一个从终点散开的dijstra搜索。不同的是，扩展节点时，是扩展其父节点，而且只有父节点的cost被计算小于inf才会被加入容器中。

优缺点：

地图很难用图来表示出来，所以dijkstra也不是总是适用。

Expected cost planning

由于环境的影响是用一个分布表示的，所以使用expected cost analysis，如下：

同样，也可以找到递归关系，

从而可以想一下，同样也可以用dijkstra来搜索，方式是一样的。只不过，里面一层的最大换成期望相加。

Real Time Dynamic Programming

MPC

MPC主要包含上面几个部分：
System model：系统模型，x˙=f(x,u)
Problem model：优化目标，例如要使系统状态到0，min xTx
state space：x
input space：u
parameter space: 无法优化连续的无限维空间，只能将要离散化输入
control：通过x,u计算x˙从而演化下一步的x

Linear MPC

考虑一个路径规划问题，system model是：

可以写成矩阵形式

优化目标为：

这是一个二次规划问题，可以直接调用matlab求解