基于三次B样条的移动机器人实时轨迹规划研究

郑天江，李俊杰，陈庆盈，杨桂林，张 驰

（中科院宁波材料技术与工程研究所，宁波 315201）

基于三次B样条的移动机器人实时轨迹规划研究

郑天江，李俊杰，陈庆盈，杨桂林，张 驰

（中科院宁波材料技术与工程研究所，宁波 315201）

基于B样条曲线生成的轨迹曲线具有几何不变性和连续性等优点，被广泛应用于数控机床设备或机器人的轨迹规划算法中，然而传统的轨迹规划基本上是在离线的情况下进行的，即所有的型值点已知，或者控制点已知，然而这种方法很难运用到移动机器人的轨迹规划中，此外如果简单地将B样条轨迹规划方法应用于移动机器人的轨迹规划，即通过给定控制点，进行插补生成一条新的轨迹曲线，但是这种方法通常不会经过所有的控制点，这样会导致实际运行路径与规划的路径有偏差，提出了一种基于三次B样条的移动机器人实时轨迹规划算法，在移动机器人的运动过程中能够实时生成插补点，不需要预先输入所有轨迹点（型值点），并利用反求控制点算法保证机器人能够经过所有的（型值点），从而保证机器人既能经过所有预先规定的点又能保证机器人运行的连续性和平稳性，通过仿真验证了其有效性。

B样条；实时轨迹规划；移动机器人；插补算法；样条函数

0 引言

轨迹规划问题是运动控制的核心问题，从数控机床设备到机器人设备，轨迹规划都是其中必不可少的关系着系统性能的关键因素。一般的数控机床或者机器人搭载的运动控制器均具有简单的轨迹规划和插补功能，其中梯形曲线、S曲线[1]、三次多项式[2]以及高次多项式算法[3]等最为常见，样条函数与轨迹规划的原理最早出现在凸轮轮廓设计上，随着自动化技术的发展，其在数控机床和机器人领域有了广泛的应用。其中B-Spline[4]以及PH（速端螺旋线）[5]等样条函数被广泛采纳与应用。其中采用B样条曲线的轨迹规划方法可以实现对时间的一阶二阶导数连续，并且形成轨迹的局部支撑性，被较多地应用到了数控机床上。近年来，随着工业机器人的性能不断提高，除了简单的PTP（点对点）运动以外，机器人还需要高性能、复杂轨迹的运动模式。B样条等轨迹规划技术也越来越多地被应用于机器人的轨迹规划[6]中，现有的用于机器人的B样条大多是集中在离线的轨迹规划研究，也就是说，其全局的运动轨迹点（型值点）已经给定，轨迹规划的任务就是通过B样条算法以轨迹点为参考生成一条优化的曲线，然后将优化的曲线坐标和速度等参数输入机器人控制器。

Koch[7]等人最早将B样条曲线应用于机器人的轨迹规划中，主要是针对关节类型的机器人，实现了离线的轨迹规划；Ling Chen等人利用Bezier曲线进行了轮式机器人的轨迹规划[8]，而Bezier曲线进行轨迹规划时需要知道全部控制点的坐标，并且控制点的修改会影响全局的曲线轨迹。上述的方法对于移动机器人来说很难适用，原因是移动机器人的工作环境往往是变化的，机器人很难制造工作环境中的所有障碍物及即将通过的路径的环境动态障碍物，这些信息一般需要利用机器人自身的传感器检测得来，也就是说这些信息是实时改变的，因此对于移动机器人来说，其轨迹规划必须是实时计算的。此外虽然有学者[9]也采用了B样条的轨迹规划应用于移动机器人，但是他们的研究中尚没有考虑到让轨迹曲线经过路径点，并且也没有考虑到移动机器人实时性的要求。

本文针对B样条生成原理进行了分析，并且提出了一种基于三次B样条的实时快速算法，用来进行移动机器人的轨迹生成，该方法基于分段B样条的思想，在每一个时钟步长时取历史的路径点和下一个即将路过的路径点作为样条的一段，采用滑动的方法在不同时刻均采用均匀的三次B样条函数进行轨迹规划，并且应用反求控制点的算法来保证机器人的轨迹路过给定的路径点。

1 传统B样条曲线介绍及分析

B样条基函数公式由下式给出：

其中，n可以看作为B样条的次数，那么三次均匀B样条基函数可以写成：

三次B样条曲线段P0,3(t)为：

其中Pi为控制点，当给定控制点Pi的时候，利用式(3)就可以求出一系列的满足三次B样条的曲线点。

图1 通过控制点求曲线点

从图1可以看出，如果把期望的轨迹点作为控制点来求得轨迹曲线，很明显，轨迹曲线将不通过控制点，因此，本文需要将轨迹点看成是型值点，通过型值点反求控制点来保证生成的曲线经过给定的轨迹点。

2 B样条实时曲线生成方法

1）单线段反求控制点算法

从而控制该线段的四个控制点可写为Pi-1，Pi-1，Pi，Pi，显然，对于单段线段而言，再通过控制点求出来的曲线点，仅仅是过轨迹点的直线插值而已。对于移动机器人而言，通常我们会已知移动机器人走过的历史路径，并且通过实时的障碍物传感器能够得当局部范围内的一个或者几个障碍物信息，因此，移动机器人即将路过的一个或数个路径点是可以实时得到的，因而由历史轨迹点和下一时刻的轨迹点可以生成一组B样条曲线。

2）多线段反求控制点算法

由多段线段反求控制点的方程可以通过下式给出，对于n个点组成的n-1个线段，可以列出n-2个独立的端点条件方程，即：

在这里需要假设第i条线段与第i-1条线段的控制点有三个是重合的，从而保证第i条线段与第i-1条线段的二阶连续性。为了保证方程能够求解，需要在最左端线段的左端点和最右端线段的右端点设置重节点条件，即增加两个控制点，P0=P1，Pn+1=Pn，从而可以得出由矩阵形式组成的方程组：

上式可解出n个控制点，再加上P0和Pn+1共有n+2个控制点，将上述n+2个控制点分别作为这n-1个线段的控制点，每段曲线由四个控制点控制，分配如图2所示。

图2 控制点分配

利用上述控制点及公式(3)便可生成一系列平滑的曲线，由于第i个线段与第i-1个线段有三个控制点是重叠的，因此这些曲线是二阶连续的，即速度和加速度是平滑的。

3）滑动实时轨迹生成方法

上述方法可以在已知机器人所需要经过的轨迹点情况下生成一条连续的轨迹曲线，但是此种方法需要一次读入多个轨迹点，导致系统在生成一条轨迹曲线之后需要等待多个时间间隔，例如，当n=5时，每次运算完成后需要等待5个时间间隔，当选择的n增大时，这个时间间隔需要更大，从而不能保证机器人轨迹规划的实时性。

本文利用滑动的方法，先将一定数量的轨迹点存储起来，然后在机器人运行的每一时刻丢弃上一时刻存储区域中的最早一个轨迹点，然后加入最新读入的机器人即将经过的轨迹点，例如对于第si时刻，假设存储区域中的轨迹点为Ti，Ti+1，…，Ti+n-1，那么可利用这些点求得对应的控制点，生成n-1条轨迹曲线，然后通过优化方法选择其中的某一条轨迹曲线作为机器人第si时刻规划出来的路线，而在si+1时刻，我们则可以取轨迹点为Ti+1，Ti+2，…，Ti+n-1，Ti+1，其中Ti+n为最后一次机器人读入的轨迹点。在si+1时刻，我们同样可以规划出n-1条轨迹曲线，而取其中的某一条曲线作为第si+1时刻规划出来的路线。利用这种方法，便可以实现在机器人每读到一个新的轨迹点时便马上生成一条新的轨迹曲线点作为机器人的轨迹插值点，从而保证了机器人运行的实时性。

然而，通过研究发现，由于在si和si+1时刻用于规划B样条曲线的轨迹点（型值点）发生了变化，因此在si和si+1时刻生成的轨迹曲线在交接触不能保证二阶连续性，需要后续的算法探索来改进。

3 仿真结果分析

本文针对给定的轨迹点利用B样条函数进行了仿真，给定平面上轨迹点x,y如表1所示。

表1 给定平面上轨迹点x,y值

在轨迹点已知的情况下，首先利用反求控制点方法求得相应的控制点，然后将控制点进行分段，求出每段曲线的曲线点，如图3所示。速度和加速度分别如图4和图5所示，可以看出，如果是轨迹点已知的离线规划，那么其求出的多段轨迹曲线的速度和加速度也是连续的。

此外，本文采用滑动轨迹生成的方法，假设总共的轨迹点仍然是所设定表1中的内容，但是每次只读入5个轨迹点，每次都选择所生成的5端曲线中的第3段，按照不同时刻拼接成一个曲线，由于每个时刻都需要5个数据点，因此这里总共规划出来的曲线为6段。仿真结果如图6、图7和图8所示。

图3 控制点及B样条曲线

图4 x,y方向的速度曲线

图5 x,y方向的加速度曲线

图6 滑动法求得控制点及B样条曲线

图7 滑动法求得x,y方向的速度曲线

图8 滑动法求得x,y方向的加速度曲线

采用滑动方法可以在机器人每扫描到一个轨迹点时便生成一段B样条曲线，能够保证机器人路径的实时规划，并且在每一段规划出来的曲线中间具有二阶连续性，但是其轨迹曲线的速度和加速度在两段曲线的拼接处存在一定的波动，可将生成的曲线数据点进行滤波处理来降低波动。除此之外在研究过程中还发现，如果采用的轨迹点数越多采用此方法拼接出来的实时曲线越平滑，并且曲线是否平滑也跟拼接时选择曲线中的第几段有关，为了进一步高其实用性，下一步可以考虑采用优化算法来确定最优的选择方法和轨迹点选择方法。

4 结束语

本文提出了一种基于三次均匀B样条函数的适用于移动机器人的实时轨迹规划方法，仿真研究表明，该方法能够依据机器人的历史路径点和利用传感器扫描到的路径点生成基于B样条曲线的机器人实时路径，在曲线的中间具有二阶的连续性，但是在曲线的拼接处有少量的抖动情况，因此该方法基本上能够应用于移动机器人的实时轨迹规划，通过滤波处理或者利用优化算法选择最优方案，可以降低曲线之间的跳动，实现移动机器人的平滑、平稳运行。

[1] 潘海鸿,杨微,陈琳,等.全程S曲线加减速控制的自适应分段NURBS曲线插补算法[J].中国机械工程,2010,21(2)：190-195.

[2] Kee-Whan K, Hyun-Sik K, Young-Kiu C, et al. Optimization of cubic polynomial joint trajectories and sliding mode controllers for robots using evolution strategy： Industrial Electronics, Control and Instrumentation[A].IECON 97.23rd International Conference [C].1997.

[3] 高岩.工业机器人轨迹规划算法的研究与实现[D].2014.

[4] Arney T.Dynamic path planning and execution using B-Splines：Information and Automation for Sustainability[A].Third International Conference on,Melbourne,VIC, 2007[C].20074-6 Dec.2007.

[5] Liu IH, Tsai Mc,Cheng MY,et al.Planning and implementation of motion trajectory based on C2 PH spline[A],word[C].2012.

[6] 钱东海,谭伟,赵锡芳.基于B样条路径的机器人时间最优轨迹规划[J].上海交通大学学报,1998(12)：31-35.

[7] Koch P E, Wang K.Introduction of b-splines to trajectory planning for robot manipulators[J].Modeling,Identification and Control：A Norwegian Research Bulletin,1988,9(2)：69-80.

[8] Ling C, Sen W, Huosheng H, et al.Bezier curve based trajectory planning for an intelligent wheelchair to pass a doorway：Control (CONTROL)[A].2012 UKACC International Conference [C].2012.

[9] Sprunk C.Planning Motion Trajectories for Mobile Robots Using Splines[D].Albert Ludwigs University,2008.

A study of robot motion controller based on pure network

ZHENG Tian-jiang, LI Jun-jie, CHEN Qing-ying, YANG Gui-lin, ZHANG Chi

TP242

：A

：1009-0134(2017)05-0004-04

2017-01-20

浙江省“钱江人才”计划（QJD1602033）；宁波市数控一代机械产品创新应用示范工程计划（2014BAF10B00）；湖北省省院合作项目：工业自动化开放式智能控制系统研发及产业化

郑天江（1984 -），男，湖南永州人，博士，研究方向为多轴运动控制系统和软体机器人建模仿真与系统控制等。