自动驾驶Motion Plan之轨迹生成代码实现

自动驾驶Motion Plan之轨迹生成代码实现

文章开始前先回顾下上次的带约束的轨迹生成，轨迹生成本质就是曲线拟合。曲线拟合常用的方法有：多项式、贝赛尔曲线、三次样条差值…下面的例子我们会选择多项式曲线拟合来做讲解。
有了曲线拟合的方法，理论就能够在通过高路径规划得到的wayponit点就能够生成一条可以通行的路径了。然而如果只是生成路径就有点粗糙了，生成路径是需要满足一些约束条件的，比如生成的路径轨迹整体要光滑、路径必须要经过某些指定点、不能碰到障碍物、连续性要求、考虑到移动机器人的物理限制（比如加速度小于20m/s）…
接着就是考虑了这么多约束求解如果太复杂也不行，太复杂了你就跑不快、太复杂了消耗能量大、太复杂了出了问题差问题很难。所以有没可能把上面约束问题都统一到一个求解框架、一步就求解完又成了科研人员研究新方向。

附赠自动驾驶最全的学习资料和量产经验：链接

多项式轨迹拟合

由于四旋翼无人机以及双轮差速轮机器人(不准确的简化)，它们轨迹在各个坐标轴上是独立的，因此我们可以对其分别进行轨迹拟合。也就是说我们可以分别对它们在x , y , z x,y,zx,y,z轴上进行路径生成，然后直接将三个轴合成就可以得到一个完整的空间轨迹。
因此后续算法细节介绍中，将不再强调轨迹是一维、二维还是三维，均以一维来进行介绍，可以很容易将其推广到任意维度。

五次多项式曲线表示

求解

轨迹的表达只与时间$t$有关，也就是说我们如果知道机器人的起始点位置、位置的一阶倒数（速度），二阶倒数（加速度）和终止位置、位置的一阶倒数（速度），二阶倒数（加速度）方程的参数KaTeX parse error: Undefined control sequence: \[ at position 1: \̲[̲p\_5,p\_4,p\_3,…可以求解出来。那么这段轨迹拟合就能够表示出来了。下面