基于三次参数样条的机器人路径平滑算法研究

乔亮 李泽文 胡杰 李照祥

摘要：本文针对机器人运动规划中必须解决的路径平滑问题，对常用的分段插值算法进行对比分析，紧密联系实际，说明其局限性，提出了基于三阶参数样条插值的机器人路径平滑的研究。解决了机器人在实际运动规划中出现的拐点及曲率不连续的问题，以机器人本身的硬件能力做参数调整。仿真实验表明调参后的路径平滑效果很好。

关键词：三次参数样条;路径平滑;平滑参数;移动机器人

中国分类号：TP301   文献标识码：A

文章编号：1009-3044（2019）22-0191-02

开放科学（资源服务）标识码（OSID）：

1 背景及研究意义

为了提高工作精度，在实际机器人开发中，运动过程不允许速度、加速度、航向角和曲率发生突变，在实现例如避障、抓取等功能的同时，保证机器人平稳运动，这对机器人在运动过程中的路径规划提出严格要求。三次样条插值，构造的曲线二次连续，且构造时只需事先给出很少的导数信息，如端点条件（这里我们选取自由边界，各区间首尾两端二次导为0）。但求解中不能出现轨迹平行或垂直于坐标轴[1]。B样条插值允许局部控制曲线的形状，又能保证曲率的连续性，但由于其原理导致不能经过所有的样点[2]。然而机器人在运动中，所在空间可能由于障碍物出现往返运动，导致轨迹图计算会出现垂直切线问题。为了满足机器人实际运动的要求，本文提出了基于三次参数样条的机器人路径平滑算法，并讨论如何根据硬件条件选择适合的参数。

2 三阶样条插值的算法改进

2.1 三阶样条插值

机器人在二维平面地图信息中包括地图中的坐标位置，以此将地图信息离散化，通过地图坐标信息机器人使用路径算法来获取可行的路径，在此过程中通过给定一系列的路径点，为了使轨迹更加平滑通过将带有坐标信息的路径点来进行插值[3]。样条插值法是根据可变样条连接样点的光滑曲线数学方法[4]。满足路径规划的条件需要：三次样条曲线在衔接点处是连续光滑的;选用自由边界的三次样条（Nature Cubic Spline），且边界二阶导数也是连续的。在任意两个路径点[SiSi+1]之间可以通过三次多项式生成一段平滑的曲线。该插值函数可以通过以下方程形式表示：

[Six=a i+b i（x-xi）+c i（x-xi）2+d i（x-xi）3]     （1）

各段擬合曲线系数不同，通过给定的n-1个公式和两个端点的限制，得到n+1个未知量的值。

2.2 三阶参数样条插值

对于三次样条算法不能解决具有垂直切线的问题以及[xi+1

[ti=sqrtxi+1-xi2+yi+1-yi2+ti-1]  （[t0]=0）             （2）

构建一系列样点在每两点之间可以[得到]参数三次多项式插值的形式：

[x=a i+b i（t-ti）+c i（t-ti）2+di（t-ti）3]           （3）

[y=a i+b i（t-ti）+c i（t-ti）2+d i（t-ti）3]           （4）

每一段曲线中，通过上述求解三阶样条的方法[6]，即通过公式（3）和（4）的求解过程可分别得到x、y两个关于t的三阶样条插值方程。每段自变量t的坐标系是不同的，但量化标准是一致的，与每段起始点到终止点的连线长度和方向有关。这样就很好地解决了原始坐标系的机器人垂直、水平及往返运动的情况。这里选取的边界条件为自由边界，因此可以得到每一段曲线的参数方程，其流程如下：

2.3 参数的选取

曲率就是针对曲线上某个点的切线方向角对弧长的转动率，数学上表明曲线在某一点的弯曲程度的数值。曲率的倒数就是曲率半径。参数t表示三次参数样条中要求单调递增的自变量的值，参数的变化决定了整个曲线的变化趋势，参数的选取是算法优化的重要一栏。其检验标准以曲率为主，包括连续性、变化趋势和最值。

在常规坐标系中，计算x、y的一阶二阶导数，得到的数据在物理属性上表示速度和加速度分量，本参数方程选取的自变量是基于分段转换后的坐标系，故本文中的参数方程得到的轨迹速度和加速度不能代表实际的情况。但计算的曲率是轨迹属性，与本文坐标系选取关系无关，得到的结果符合实际情况。

3 实验结果

3.1 三次参数样条插值实验结果

生成的平滑轨迹如图2所示：主要选取边界情况与异常点情况，进行测试主要考虑，水平点和垂直的点，以及点过少，步长的大小，以及曲线的抖动情况并对遇到的问题进行程序修改，保证程序稳定。这里输入为一串点包括三个连续垂直和水平的点。这里选取112个单位长的一段曲线，从中选取20个插值点，以0.5个单位的步长进行采样，共采得226个采样点。图2中的虚线表示之前通过路径规划算法直接连接的路径，红色的实线为平滑后的路径对比。

左侧为插值算法的测试结果;右侧为插值轨迹与样点连线对比图

调用构造和采样函数100次，计算所消耗的时间，并与三次B样条插值的时间进行对比，结果如下：

从图3可以看出在调用三阶参数样条算法计算100次所消耗的时间为89ms左右，而三阶B样条算法计算100次所消耗的时间为181ms左右，因此每次采用三阶参数样条算法处理更高效。

3.2 参数讨论

对于曲线轨迹平滑，参数调整主要体现在自变量选取。令[（xi-xi-1）2-（yi-yi-1）2]为[h]，下图4中，展示t为[h13]、[h12]（对应欧式距离）、[h23]、[h1]、[h2]时的三阶样条插值的对比图，样点满足自变量自由变换，二维平面取样点。这里以（-4，1.2）、（-2，0.6）、（1.5，1.3）、（-0.1，1.5）、（4，3.8）、（6，5）、（10，3）先后插值平滑。

图4表示t的指数选取2、1、2/3、1/2、1/3时的路径平滑图，单位为1。以参数t=[h23]为标准，由图可以看出，t的指数值越大则路径长度越长，但对于t指数为2时，路径过长且存在路径交叠，不可取。t的指數值越小，在路径长度方面更优。

图5中表示t的指数取1、2/3、1/2、1/3时的路径平滑轨迹以及以0.05个单位为间隔的曲率点，单位为1。由图中可见，在某些轨迹变化幅度较大的地方，各个t的指数值的曲率变化突显，最值之差较大。轨迹平滑的区域，曲率差别不大且变化平稳。参数t的指数值越小，对应轨迹的最大曲率值越大，且局部曲率变化增长幅度也越大。

减小轨迹长度，可以降低机器人运动的能源损耗;曲率半径（曲率倒数）参数值对机器人的转向能力提出了要求，降低曲率可以降低机器人在转向能力的要求，继而降低机器人硬件成本。因而，需要根据实际机器人的硬件的实际能力和硬件成本，考虑路径长度优化及转向能力选取最优t值。

4 总结

本文通过说明常用的三阶样条插值及B样条插值的实际应用优缺点，尤其针对机器人路径规划领域中的局限性进行说明。对于机器人领域的应用提出了三阶参数样条插值，实现了包含一阶二阶导数连续，曲率连续，不受单调性限制，拟合轨迹包含样点。并对选取的自变量参数t进行充分讨论分析，结合实际机器人性能选取最优的参数。

参考文献：

[1] 张小江，高秀华.三次样条插值在机器人轨迹规划应用中的改进研究[J].机械设计与制造，2008（9）：170-171.

[2] 任重，杨灿军，陈鹰.轨迹规划中的B样条插值算法[J].机电工程，2001（5）：38-39.

[3] Elbanhawi M ， Simic M ， Jazar R N . Continuous Path Smoothing for Car-Like Robots Using B-Spline Curves[J]. Journal of Intelligent and Robotic Systems， 2015， 80.

[4] Durrleman S， Simon R. Flexible regression models with cubic [J].Statistics in Medicine， 1989， 8（5）：551-561.

[5] 张彩明.高精度三次参数样条曲线的构造[J].计算机学报，2002，25（3）：262-268.

[6] 许小勇，钟太勇.三次样条插值函数的构造与Matlab实现[J].兵工自动化，2006，25（11）：76-78.

【通联编辑：梁书】