平滑轨迹插值方法之多项式插值(附代码) | 您所在的位置:网站首页 › matlab直线拟合求斜率 › 平滑轨迹插值方法之多项式插值(附代码) |
前言 今天我们来聊聊轨迹插值,在机器人的运动规划和控制领域,参考轨迹的生成是一个历史悠久的问题,已经发展出了一系列的方法。今天我们就来聊一聊轨迹插值领域中最常见的轨迹插值方法:多项式插值。 说明:本文约5000字,全部看懂需要一定的时间,建议先收藏。本文所涉及的代码全部开源,提供Matlab和Python两个版本,仓库地址详见文末。 首先,我们定义一下问题: 如下图所示,给定一些离散的数据点,我们需要通过插值的方式生成一条能够通过所有给定数据点的参考曲线。 ![]() 并且,根据实际应用对于“平滑”的要求,通常会有以下不同的约束: 要求生成的参考曲线是连续的;在1的基础上,要求参考曲线的速度是连续的;在1和2的基础上,要参考曲线的加速度是连续的; 注意:速度是位置对时间的导数,加速度是速度对时间的导数,加速度对时间的导数我们称之为jerk。通常情况下,在机器人高速运动的时候,想要得到非常连续、平滑、噪音低的运动控制,第3个约束条件是必不可少的,有的甚至还要求加速度的导数jerk都是连续的。 在多项式插值里面,给定多项式的阶次越高,能拟合的函数曲线就越复杂,但越高阶次的多项式对于计算资源的要求越多。因此对于这3个要求,我们可以分别用不同阶次的多项式函数来拟合,实际应用时根据需求选择合适的方法。 1. 线性插值(一阶,恒定速度)线性插值,顾名思义,就是使用线性的方法来进行插值。即将给定的数据点依次用线段连起来,点与点之间运动的速度是恒定值。假设我们用
其中,
可以计算得到两个常量参数:
曲线的速度为:
线性插值的实验结果为: ![]() 红圈内为给定的中间点值,黑色实线为插值的结果。 从图中可以明显地看到,线性插值带来的最大问题就是在各个数据点交接处会出现一个急剧的“拐弯”,在这个拐弯处其速度不连续,因此对于运动控制来说,在这里会有一个速度的阶跃。例如,对于电机来说,这里会导致控制电流急剧变化,使得电机的输出不稳定,从而引发“抖动”问题,严重者还会损坏机构本身。因此,线性插值本身的问题导致其在控制领域应用范围受限。 2. 抛物线插值(二阶,恒定加速度)抛物线差值(Parabolic Spline)是二阶多项式插值方法。与线性插值法将各个数据点用线段连起来不同,抛物线插值方法是用二次曲线将各个数据点连接起来,在连接处使用平滑的曲线来过渡,而避免速度不连续导致的“急剧拐弯”。抛物线差值的特征是具有恒定的加速度/减速度,一般是由两个多项式的组合来得到。为什么是两个多项式呢?因为一个用于“加速阶段”,一个用于“减速阶段”。“加速阶段”和“减速阶段”的分割点叫flex point。 考虑2个数据点之间插值的情况。假设初始时刻是 ![]() 先来看一个特殊情况,flex point的位置是起点和终点的中间位置(也可以任意设定,后面我们会讨论,这里先讨论在中间位置的情况),即 对于加速阶段,其数学表达式为:
其中,
因此,常量参数可以计算为:
这样,我们可以计算得到当
在flex point的速度可以这样计算:
大家可能注意到了,与线性插值不同的是,除了指定初始时刻的位置,我们还需要给定初始时刻的速度 在flex point处,由加速阶段进入减速阶段,因此此时加速度的符号是会反转的,会导致加速度不连续。 2.2 减速阶段对于减速阶段,其数学表达式为:
其中,$a_3, a_4, a_5$为待定的常量参数。如果给定最终时刻的速度$v_1$,则有如下关系:
因此,我们可以计算得到: 这样,当
这里值得注意的是,如果 如果在
其中,
从图中我们可以看到,插值的结果中,加速度并不恒定,在 本文附带的代码里面实现了加速度对称的情况,但是留了设置不对称加速度的接口,非常容易实现。 下面我们来看一下加速度不对称的情况。 2.3 加速度不对称的情况直观上,加速度不对称也就是表示加速时间和减速时间不一致,也就是说
给定初始时刻
假定加速时间为
因此,在
在
值得注意的是,如果 ![]() 从图中可以看到,位置曲线是“平滑”的,速度曲线是连续的,加速度也是恒定的(在加速和减速阶段内保持恒定),但是加速度曲线在 三次多项式插值方法(Cubic Spline)是一种常用的插值方法,其位置和速度曲线是连续的,加速度是可变的,但加速度不一定连续。考虑2个数据点之间插值的情况,其数学表达式为:
其中, 考察给定2个数据点进行插值的情况,如果给定了在初始时刻
对于给定 如果我们只是通过给定一系列的位置信息(
其中, 三次多项式插值能够保证位置曲线和速度曲线是连续的,但加速度曲线不一定连续。虽然已经可以满足许多应用上对于“平滑”的要求了,但是在高速控制领域,一般要求加速度也要是连续的。因此,我们需要引入更高阶次的多项式插值方法。 实验结果如下: ![]() 从图中可以看到,位置曲线是“平滑”的,速度曲线是连续的,加速度曲线是可变的,但是不连续。这样,对于高速控制的场合来说,控制器的输入仍然会存在阶跃,导致不连续的情况。 4. 五次多项式插值(五阶,加速度连续)考虑2个数据点之间插值的情况,与其他阶次的多项式形式类似,五次多项式插值方法的数学表达式为:
其中, 设
对于具有 ![]() 从图中可以看出,位置、速度、加速度三条曲线都是连续的,并且位置和速度还是“平滑”的。到这里,我们已经满足了本文最开始所提到的三个要求。因此,五阶的多项式插值已经能够覆盖大多数应用场景。如果我们对加速度曲线也要求是平滑的,那么就需要更高阶次的多项式插值方法了,例如七阶多项式插值。 5. 七次及更高阶次的多项式插值理论上,多项式的阶次越高,我们可以获得越“平滑”的曲线,但是同时带来的是对运算资源要求的急剧上升,所以一般情况下,七次及更高阶次的多项式方法只用于某些特殊的场合,因此这里我们不再做深入的分析,有兴趣的读者请参考经典书籍《Trajectory Planning for Automatic Machines and Robots》,可在微信公众号【博士的沙漏】底部点击菜单栏或后台回复【TPA】获取下载链接。网上类似的书籍有: 6. 实验结果对比在实际的实验中,我们除了实现给定位置点,还给定了速度点和加速度点。这里我们放一张所有方法插值结果的对比图,从中可以直观地看到使用各个阶次多项式进行插值的结果差异。 ![]() 本文所涉及的代码全部开源,提供Matlab和Python两个版本: Github仓库更多问题,欢迎咨询: 参考《Trajectory Planning for Automatic Machines and Robots》 |
今日新闻 |
推荐新闻 |
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 |