论文《An Admissible Heuristic to Improve Convergence in Kinodynamic Planners Using Motion Primitives》
最近遇到的问题是，lattice plannner很难到点，虽然加上了dubins shot，但是还是有些情况下一直在终点附近扩展，却到达不了。
归根到底，因为用Dijkstra作为启发值，本身就不合适，那我模仿混合A*的做法，取max(Dijkstra， dubins)，在终点附近又不敢用，因为可能转圈。
本文提出了一种基于motion primitives的运动规划的Admissible Heuristic，这种启发值可以用于任何基于motion primitives的运动规划算法，并且是admissible的。这么厉害的么。
运动规划问题描述
系统方程：
轨迹：
cost criterion：
平移不变性：
主要就是根据这个性质，来生成motion primitives库的。
启发式函数：
A. 轨迹库
在box内，生成一些motion primitives
B.启发式函数
没看懂，大概就是连接当前点到终点，形成了很多个box，而box里边有轨迹。怎么计算一下。
different cost metrics：
虽然文章核心没看懂，但是这里还是有收获的。不同的cost metrics，可以体现不同的heuristic，文中介绍了两种：
A.最小时间 最小能量
独轮模型，适合差速底盘，状态空间（x, y, theta, v）:
控制输入:
cost function, 这个就是常规用的啦，最小化输入的平方:
其中
, 
两点之间的轨迹就是motion primitives，属于两点边界值问题，TPBVP，但是没有讲如何求解。现在需要的就是这个，如何求解汽车底盘或者差速底盘的BVP问题
B.乘客舒适
car-like 模型：
根据IOS 2631-1标准的定义，乘客舒适度与r.m.s acceleration aw有关(加速度均方根)
目标函数：
