这个梯形速度曲线进行时间分割就相当于启发式函数，估计了一个相对较为合理的time allocation

这个问题有点复杂，还真没遇到过执行器延迟大到影响这么多的情况。
MPC里的滚动优化概念你可以尝试一下，不过感觉对于你的情况可能帮助不大

运行rviz之前source devel/setup.bash

看这个变量名，存的是每段轨迹x坐标分量的多项式函数系数

没错~

目前是这样的，我个人觉得从学习的角度这些解析已经足够清晰，不给参考代码也是为了逼迫大家能够自己去实现，加深理解才能有所收获

第五章作业没有需要用到rrt的地方啊

不是，代码里的_time_interval只是用于trajectory library前向积分的时间间隔

minimum snap算出来的是期望轨迹，具体的控制器需要另行设计

h函数是终端代价函数，根据你的需求来确定的，通俗举个例子，如果你只有g函数（即过程代价函数），当你以时间最优为优化指标且不设置速度加速度约束时，计算出来的轨迹就是加速度无穷大的轨迹，如果加入了终端代价函数，则会惩罚到达目标点的速度，即可避免这个问题，所以优化问题中，终端代价函数和过程代价函数都很重要，只不过课程里所讲的部分，由于终端状态固定，已经是一个等式约束了，就不要通过优化问题来求解终端状态。

你定义的那个h函数可以的，只要系数大于0就好

你是不是count没随着循环数的增加而增加，导致给同一个节点重复赋值

@zgrobottoday “用障碍物地图生成一个ESDF地图，再从ESDF地图里评估每个位置的障碍物推力cost。”本质上也是构建了一个costmap呀，“地图更新频繁”，我理解为环境中会不断有新的障碍物加入或者离开，一般来说不会更新整个ESDF，只会更新障碍物附近的值

解决的问题都不一样吧，Pontryain’s minimum principle用于解obvp，obvp是端点固定了的问题，本质上是单段轨迹优化；

但是Minimum-snap用于解一系列路径点下的轨迹优化，本质上是多段轨迹优化，需要考虑的约束相比obvp多了中间点的约束和连续性约束。

我把你的“最小化”理解为平方和积分最小，理性上不严谨的认识是，acc最小化代表运动时的姿态变化最小，jerk最小化代表运动变化最小（适用于无人机拍摄），snap最小化代表能量消耗最小。

严谨一点的讨论是：

实际建模时，无人机状态量表示为：

而无人机模型由于满足Differential Flatness，可以用x,y,z,yaw来间接表示上述12个状态量，方便进行minimum snap trajetcory generation and control

minimum snap: 代表最小化Differential thrust，一般规划时给定yaw角朝前，末状态也可以任意指定，由于轨迹经过了优化，yaw的变化也同样符合能量最小的优化原则，不会不停转头。

既然你要设置p free, 那么位置p为何还有末状态呢？

来自王超群老师的回答“适用于不同的场景，不同的算法表现不一定一样，比如特别小的环境，RRT之类的不一定比A*好。另外，inform rrt* 不考虑初始路径，如果给定同样的初始路径，他肯定比RRT*好，相对RRT*没有缺点，椭圆内采样的设计在路径最短的要求下，这种采样方式也是合理而且最优的。但是万一有人搞了个基于深度学习的采样方式，是不是能超过inform rrt这种在椭圆内采样的方式，也很难说。况且动力学约束也是很重要需要考虑的。”

编程题为了简单起见，做的是末状态固定的obvp

@wudi9545 末状态固定的意思是指末时刻位置、速度、加速度已知，并不一定等于0，不过编程时为了简单，可以设置末时刻速度和加速度为0，末时刻位置就是你指定的终点坐标