学习笔记4：Minimum Jerk轨迹跟踪实验（算法原理及代码框架）

一、算法原理

初始在a点，T时刻在b点，需要找到从a到b的平滑轨迹。

轨迹可由5阶多项式表示：

将t=0和t=T代入，并求一阶、二阶导；

x一阶导为速度，二阶导为加速度；

写成矩阵形式。

为求得5阶多项式轨迹，需要求得多项式系数p。

对A矩阵求逆，可以计算出系数p。

同理，使用3阶多项式表示yaw角轨迹；

并求得yaw角多项式轨迹的系数。

二、代码架构

1、matlab文件line_tracker_min_jerk_numerical_issues.m计算minimum jerk轨迹多项式系数。

2、line_tracker_min_jerk_server.cpp为轨迹生成的主体服务；

其中，轨迹生成函数如下：