Am_traj公式推导

    // Piece(t) = c5*t^5 + c4*t^4 + ... + c1*t + c0
    // The natural coefficient matrix = [c5,c4,c3,c2,c1,c0]
    // Any time in [0, T] is normalized into [0.0, 1.0]
    // Therefore, nCoeffMat = [c5*T^5,c4*T^4,c3*T^3,c2*T^2,c1*T,c0*1]
    // is used for better numerical stability

转为时间[0,1]；
则:

对求导，得

而将带入得


对比可知，

同理：

其中

//调整轨迹曲线以满足动力学要求
//调整方法：
//1.获取每段轨迹起始位置的位置速度加速度，强制使其小于(1-esp)*max
//2.对于每段轨迹，checkMaxAccRate/checkMaxVelRate，不满足条件，则时间*2，速度和加速度/2
//3.将新的时间，速度，加速度赋值给traj，迭代2-3，直到满足条件
bool enforceIniTrajFeasibility(Trajectory &traj, int tryOut) const
//检查是否超过最大速度
inline bool checkMaxVelRate(double maxVelRate) const
//检查是否超过最大加速度
//1.判断起点终点的加速度^2是否大于最大速度^2,
    TODO:小车的前后加速度不同，不能用^2的方式需要分别讨论
    TODO：起点有很大的加速度，以至于始终检测不通过
//2.对轨迹中间段，采取求极值的方法，极值点 - max,，判断是否
inline bool checkMaxAccRate(double maxAccRate) const

checkMaxAccRate()中求根公式解读

判断轨迹上是否存在点大于maxAcc

1. 已知
2. 由(2)式可知，maxAcc在上对应的根为
3. 公式转化为求是否存在根
4. 判断端点处是否有解，在处，acc的值是否小于maxAcc
5. 对于三维点，即求下式的根