本篇承接上一篇,介绍PX4里另一个“给定总时间”的S曲线规划,本篇会多次引用上一篇的推导公式,建议两篇文章交换看一下。

void updateDurationsGivenTotalTime(float T123); 根据给定时间规划S曲线

目标:已知当前状态和目标速度,给定总的规划时间,如何设计S曲线路径达到目标设定

整个推导过程可以参考上一篇文章,只是最后的最优化问题,转化为了等式求解。

提取总时间:

[公式]

[公式]

进一步化简:

[公式]

二次方程求解:

[公式]

T1时间已知,那么T1时刻达到的加速度为:

[公式]

[公式]

[公式] 如果没有超限,那么

[公式]

按照推导的公式,T3和T1联系较大,区别在于初始加速度 [公式] 

[公式]

T2呢,主要取决于规划要求了:

如果是时间最短的要求:

[公式]

[公式]

如果给定总时间:

[公式] 

几点结论:

① 这两章原理篇主要介绍了PX4里S曲线相关的唯二的规划方案对应的原理。一个是时间最短规划,一个是给定时间规划。S曲线开放性很高,一定要设计好完整的约束条件,不然可能存在多个解的情况,程序设计就会存在bug。

② 其实整个求解过程,就是根据约束求解三个时间段的时间长度,再结合已知的初始状态,限幅条件,目标状态,最终的唯一曲线就设计完成了。

③ 公式的推导主要是高中数学的知识点,下一篇会做PX4源码分析,对照一下,就会发现PX4只是把数学推导用C语言实现了。