论文：《Efficient Trajectory Planning for Multiple Non-holonomic Mobile Robots via Prioritized Trajectory Optimization》 Juncheng Li
最近看到了好多用这种套路作轨迹优化的，无人机也有，无人车也有，单车也有，多车也有：
初始路径-->安全走廊-->非线性/凸优化问题-->求解器求解
Introduction
本文提出了一种创新的方法，在富含障碍物的环境，为non-holonomic移动机器人多车生成避障的最优轨迹。
为了提高计算效率，decoupled planning方法被提出[6]，就是序列优先级。每个agent的轨迹需要避免与之前的agent的轨迹发生冲突。
文献[7][8]提出的轨迹优化方法，利用sequential planning，显著提高了计算效率。这些decoupled planning methods都很快，但是存在
的问题是--不完备。有些场景下的可行解，可能发现不了。
在富含障碍物的环境，单机轨迹规划有大量的研究，套路分为前端路径规划和后端轨迹优化。（这里提到了很多高飞老师的研究。）
多机轨迹规划，也采用两段的套路（可以保证完备），（这里提到的研究还是无人机的，无人车的研究这么少么？）
多无人机可以用quadratic program (QP) problem表示，然后优化。
无人机的多车轨迹规划，是作用在线性动力学linear dynamics的，因此轨迹优化可以表示为QP问题，这个是凸优化，容易求解。
但是在现代工业，很多差速驱动的机器人，是非线性动力学nonlinear dynamics，轨迹优化只能表示为非凸的非线性问题（NLP）。
分布式的规划方法，计算效率高，但是不能保证无死锁并且在迷宫形的环境性能很差。基于上述问题的考虑，我们的方法可以生成安全，可行，接近最优的多机器人轨迹。
在前端和后端两个阶段，机器人的非线性运动模型都被考虑进去，以保证轨迹的可行。我们提出了一种优先级轨迹优化方法，使得计算效率提高，可以用于大规模的机器人群体。
本文是在连续空间集中式多non-holonomic机器人轨迹规划的第一次尝试，主要贡献如下：
- 大规划无滑移多机器人轨迹规划。
- 优先级优化方法提高了计算效率。（这个在李柏的书里也有专门的介绍）
问题描述：
0.碰撞模型
每个robot定义为一个半径为R的圆形，根据圆形占据的栅格是否碰撞来确定。
如果一个圆形不够精确，那可以用两个圆形，这些是无人车里常用的方法。
1.初始路径构成waypoints
这里初始路径是（x, y, theta）的集合。
2.kinematic model
本文采用差速轮的unicycle model，状态为(x, y, theta)，机器人通过线性速度和角速度控制。
3.最大速度，最大角速度
看到这里，真正的差速轮是没有转弯半径限制的，如果想限制，那只能在这里加了。v / w > R。
具体方法：
A. Discrete Path Planning
将占据栅格地图转化为一种2-D grid graph。相比于传统的栅格地图，我们考虑了位置和朝向。x,y变成了x, y, theta。
首先就要构建这个图了。假设图的栅格尺寸为D，dT表示机器人移动一个边需要的时间。考虑到机器人的运动学约束：
确定了D和dT，就可以构建这个图。
离散路径的冲突处理，采用ECBS来处理。这里冲突定义两类，一个是节点之间，一个是边之间。（不是优先级吗？怎么初始路径还需要ECBS呢？可能优先级是针对轨迹优化的）
采用ECBS获取无冲突的离散路径（那就是说，作者把ECBS的下层搜索修改了？）
在轨迹优化过程，两个robots之间每个时间步的路径点距离都会被检测，所以每一个robot的离散路径长度都相等（这里想表达的应该是路径短的robot到达终点后，持续存在！持续到跟最长的那个路径相等）。
B.安全走廊构建：
这个安全走廊构建方法有很多，刘思康，高飞，李柏，港科大，等等，有很多构建方法。
每个robot的离散路径，里边即waypoints rk，每两个waypoints之间的线段用Ik = < rk-1----->rk >表示。在每个线段附近生成无碰撞的凸多面体Sk。
需要满足以下条件：
凸多面体跟障碍物没有交集
相邻多面体之间必须有交集
本文参考了《Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial》中关于走廊生成的方法。
分别在x轴和y轴方向扩张到最大空间。但是对角线的线段，是有可能没有凸多面体的，需要加密waypoints，满足一下条件即可确保走廊连续（即确定最大waypoints间隔）：
每一段重新分割成h小段, 路径长度为M, 新的采样包含waypoints个数H = 1 + h * ( M - 1 )
Ri是机器人碰撞半径。
如果下一段线段被上一个凸多面体完全包围，那就不用再求了。
C.轨迹优化问题：
问题描述：
一些参数：
简化问题：
目标函数分为两部分：
1.连个连续的控制输入之间的差值需要惩罚；应该是这里是差分驱动，控制输入为速度，所以没有用比较常用的minimum snap。
2.最优轨迹与参考轨迹之间的差值。这里作者说的是因为初始的参考轨迹已经是可行路径啦，所以要惩罚这个偏离。额，感觉这里有问题，没必要贴近初始轨迹。
4.Prioritized Trajectory Optimization
对于大规模的机器人队伍，上边的问题求解是不够有效的。我们提出了优先级的轨迹优化方法。
robots分为一些groups,优先级低的求解时，把高优先级的轨迹作为障碍物要避免碰撞。
如果优先级排的不好，可能导致低级的robot轨迹优化失败，我们提出了分组和优先级排序的方法：
略
仿真：
A.实施细节
Ubuntu 16.04 with Intel i5-6300HQ @2.30GHz CPU and 12GB of RAM
地图：OctoMap地图表示占据栅格地图
求解器：内点法非线性求解器IPOPT
参数：碰撞模型半径0.15m，栅格图尺寸D=1m（在这么大的栅格上是必须的吗），移动一个edge耗时?T=1.6s，最大线速度1m/s，最大角速度1rad/s。构建安全走廊时，每一段线分为h=5个小段。
B.计算效率和解质量
环境大下：10 m X 12 m,包含6个3 X 0.6m的货架
ECBS次优系数：1.5
与coupled方法（不分优先级）作对比，可以看出，求解时间减少，但是最优性依然保证！！
C.成功率
实验：
碰撞模型半径：R=0.3m
最大线速度：0.6m/s
最大角速度：0.6m/s
移动一个edge的时间间隔：?T=2.65s
其他参数同仿真
将优化的轨迹上传，使用MPC-based 轨迹跟踪控制方法来执行轨迹。