基于三次B样条曲线的叉车型AGV路径规划研究

钱东海，孙林林，赵 伟

(上海大学 机电工程与自动化学院 上海 200444)

0 引言

为提高工厂的运行效率，叉车型AGV得到了越来越广泛的使用。在工厂内，叉车型AGV通常沿规划好的路径行驶，但当叉车型AGV在运行时因避障或人为使用后会偏离规定参考路径，因此，研究路径规划技术实现叉车型AGV自主回到参考路径，提高叉车型AGV的作业效率具有重要意义。

路径规划技术[1-5]是移动智能装备的关键技术，受到国内外学者的大量关注。常见的路径规划算法，如A*算法[6]、Dijkstra算法[7]需要首先绘制栅格地图，栅格地图的精度决定了规划路径的质量，且搜索速度慢，效率低。遗传算法[8]具有良好的全局优化能力，但其收敛速度较慢，效率低，规划结果因为初始种群的随机性只能找出较优路径。L.Zhang[9]等提出了一种改进的粒子群算法，用以提高算法效率，避免陷入局部最优等问题，但离实际最优仍有一定差距。快速搜索随机树算法[10]是一种基于采样的搜索算法，搜索速度快，不需要对地图预处理，但搜索的盲目性较大，且路径不连续，不利于AGV跟踪。Rastelli J P[11]等使用贝塞尔曲线对城市转弯道路进行路径规划，通过动态调整控制点的位置实现路径平滑，提高乘坐的舒适性，但没有实车实验。另外，参数化曲线也被用来进行路径平滑[12-14]。

本文针对工厂环境中常用的叉车型AGV在偏离参考路径较大距离时，需重新回到参考路径的问题，研究相应的路径规划算法。论文以AGV的实时位姿为起点，以最短路径为优化目标，建立符合最小转弯半径要求的3次B样条路径曲线，并在参考路径上寻找出最优的路径终点。论文将路径规划问题转化为参数优化问题，并提出一种参数寻优算法，解决当AGV大幅度偏离参考路径时能以最短距离重新回到参考路径。论文安排如下：第一节给出了规划路径需要满足的约束；第二节给出了路径规划中3次B样条曲线的控制点的构造方法；第三节给出了路径规划算法的具体步骤；第四节通过Matlab仿真，验证本文算法的有效性。

1 路径描述

如图1所示，本文以大幅度偏离参考路径后的叉车型AGV的实时位姿作为全局路径规划的起点，将AGV从起点位置出发回到参考路径的过程中形成的一系列离散点序列p={P1,P2,P3,…Pn-2,Pn-1,Pn}称为规划路径,其中Pi(xi,yi,φi)为规划路径上一个点的全局位姿，(xi,yi)为全局坐标，φi为AGV的航向角。

图1 叉车型AGV规划路径示意图

本文将在满足多约束条件下形成的路径簇中选择最短路径作为规划的最优目标路径引导叉车型AGV回到参考路径。目标函数为：

(1)

1.1 规划路径中的约束

对于单舵轮驱动的叉车型AGV，要使规划路径能被跟踪，规划的路径曲线需要符合AGV运动学、结构参数约束，路径最大曲率约束、路径起点和终点位姿约束。

1.1.1 叉车型AGV运动学模型

叉车型AGV运动时速度较低且不易发生侧滑现象，所以路径规划时仅考虑AGV的运动学模型。假设车体为刚体且车轮不变形，车辆在平面上运动、不打滑，即车轮和地面之间为纯滚动。

叉车型AGV的运动学模型如图2所示，在世界坐标系XOY下，(xf,yf)代表小车前轮坐标，(xr,yr)代表后轮中心坐标，o为AGV小车的瞬时转动中心，L表示AGV的前后轮轴距，v表示舵轮线速度，δ和φ分别代表舵轮转角和小车航向角，R为车体的转弯半径。

图2 叉车型AGV运动学模型示意图

由图2可知，叉车型AGV的转弯半径为：

(2)

当舵轮转角最大时有最小转弯半径Rmin，即：

(3)

叉车型AGV工作时，需始终确保舵轮转角小于最大舵轮转角：

(4)

1.1.2 路径最大曲率约束

规划路径若曲率过大，会导致其转弯半径小于叉车的最小转弯半径，使得叉车型AGV无法准确跟踪，从而造成再次偏离规划路径。所以规划路径曲率κ要满足最大曲率约束。

路径的曲率为:

(5)

所以曲率约束为:

(6)

结合式(3)、(6)可得:

(7)

1.1.3 曲率连续约束

为了避免规划路径的曲率不连续而造成跟踪失败，故路径要满足曲率连续，即规划的路径要满足C2连续。

B样条曲线是受控制点影响，并由多段连续曲线构成，且可以通过改变控制点的位置而局部修改整条曲线。所以本文选择满足曲率约束条件的3次B样条曲线进行路径规划。

1.1.4 起点和终点位姿约束

为避免AGV在起点和终点出现就地转向现象，需要对规划路径的起点和终点进行位姿约束。路径的起点为该时刻AGV在全局坐标系下的位姿，即p1(x1,y1,φ1)；路径的终点是AGV进入参考路径的点，记为pn(xn,yn,φn)。3次B样条曲线在其起点及终点需满足：

(8)

2 3次B样条曲线

3次B样条曲线[15]的表达式为：

(9)

式中，s为参数，s∈[0,1]，Bj,3(s)为3次B样条基函数；Ci+j为第i段中的第j个控制点。

3次B样条的基函数的矩阵形式为：

(10)

其中:

将式(9)代入式(8)可得3次B样条曲线上的点为：

(11)

相应的一阶导数和二阶导数为：

(12)

(13)

式中,

由上式可得3次B样条曲线的曲率为：

(14)

由于B样条曲线本身不通过首末控制点，为了使B样条曲线通过规划路径的起点和终点并满足起点和终点的航向角约束，以起点和终点为中点，在其两端各增加一个控制点。

图3 B样条曲线与控制点的关系

设|p0p2|=2L1，|p3p5|=2L2，L1、L2称为起点和终点处的构造距离，所以增加的各控制点坐标为:

(15)

(16)

(17)

(18)

3 路径规划算法

要规划出满足约束条件的最短路径，不仅跟B样条曲线的控制点的位置有关，还和选择的终点位置有关。假设该时刻参考路径上距离AGV的最近点为pN(xN,yN,θN)，pN点为最近点，但并不满足曲率要求，所以终点需以pN点为起点，沿参考路径方向寻找。令l1为自pN点出发，沿参考路径行进的距离。在本算法中，再令构造距离L1=L2=l2，这样只需要优化求解l1和l2，即可确定3次B样条曲线，从而寻找出最短路径。

3.1 路径规划过程

路径规划过程如图4所示。传感器检测到此时刻叉车型AGV的位姿p1(x1,y1,φ1)，并通过计算得知已偏离参考路径；然后找到参考路径上距离AGV最近的坐标点pN(xN,yN,φN)；在最大曲率约束、起点和终点位姿约束的条件下，对于给定的初始构造距离l2(通常取一个很小的值)，找到此时最短路径对应的沿参考路径的行进距离l1；然后以此l1为基础，优化构造距离l2，同时优化行进距离l1，循环往复，最终寻找到符合多约束条件的最优目标路径，从而完成路径规划目标。上述优化求解本质上由两个一维搜索构成，其中行进距离l1的一维搜索内嵌在构造距离l2一维搜索的内部，即l2每移动一个步长，都将通过一维搜索求得最优的行进距离l1。

图4 路径规划过程

3.2 验证搜索区间内的收敛性

利用枚举法分别求取直线和圆弧参考路径在不同的行进距离l1和构造距离l2时3次B样条曲线的路径长度。图5和图6中下方的线条代表不考虑规划路径曲率约束时，不同构造距离l2形成的B样条曲线中，路径长度为最短时对应的终点位置；上方的线条代表同时考虑规划路径曲率约束时各B样条曲线中，路径取长度最短时对应的终点位置。

图5 直线路径中各因素对路径长度的影响

图6 圆弧路径中各因素对路径长度的影响

从图中可以看出以l1、l2为设计变量，构造设计空间，求得的3次B样条曲线路径长度函数的曲面为凸的，在考虑规划路径曲率约束时，形成的约束边界也是凸的，故可用优化设计的方法求得满足约束条件的全局最优解。

3.3 路径规划算法

本文采用加速步长法和二分法对行进距离l1和构造距离l2进行优化，具体步骤如下：

1)找到参考路径上距离AGV的几何最近点pN，并求其位置信息；

2)给定初始构造距离l2；

3)在给定构造距离l2下，寻找最优路径对应的行进距离l1：用一个试探步长t0沿参考路径的行进方向移动一步，若不满足约束条件，则取上一次步长的β倍来递增步长，即tj=βtj-1，所以l1,j=l1,j-1+β·tj-1。通过加速步长的方法快速求得搜索区间，以最后一次不满足约束的位置和第一次满足约束的位置为搜索区间进行二分法搜索，直至l1的变化已小于给定计算精度ε1，从而求得该构造距离下满足约束条件的最短路径和对应的行进距离l1；

4)以上一步骤求得的行进距离l1为基础，优化构造距离l2：在初始构造距离l2的基础上加上试探步长t0，按步骤3)中同样的方法，求取最短路径；若本次所得到的最优路径长度小于上一次，则取上一次步长的β倍作为递增步长，即ti=βti-1，所以l2,j=l2,j-1+β·ti-1；若该次所得到的最优路径长度大于上一次，以该次最优路径对应的构造距离l2和上一次最优路径对应的构造距离l2为区间进行二分法搜索，直至l2的变化已小于给定计算精度ε2。

至此，得到最优参数l1和l2，从而得到满足多约束条件的最短3次B样条曲线路径。

4 仿真实验与分析

为了验证提出的路径规划算法，本文利用Matlab建立了仿真验证程序。AGV的前后轴距为1.44，舵轮最大转角限定为，所以最大曲率为2.592 m-1，l1和l2精度ε1=ε2=0.02。由图5和图6可知，当优化参数过小时，找不到符合约束条件的路径，经多次仿真实验，选取直线段构造距离l1最小值为0.5 m，圆弧段构造距离l1最小值为0.3 m，在此约束条件下能够较快地找到符合约束条件的最优目标路径。

4.1 偏离直线路径

以不同的初始位姿规划·到直线的最短路径，AGV在y轴初始偏差分别为1 m、2 m、3 m的情况下，航向角分别为-45°、0、45°，仿真结果如表1所示。

表1 直线段不同位姿下得到的参数l1、l2

选取表中第1、5、7三种情况进行分析，结果如图7所示。在图7(a)中，叉车型AGV在3种大偏差下都能满足多约束条件，规划出一条最短路径；在图7(b)中，AGV在不同起始位姿时，规划路径的最大路径曲率均在最大曲率要求范围内、曲率连续，且起点和终点位置的曲率趋近于0，使规划路径具有良好的跟踪性能，曲率的正负表示路径的凹凸性；在图7(c)反映了行进距离l1迭代次数随构造距离l2迭代步数(迭代的推进)的变化，可以看出通过十多次构造距离l2的迭代，每次l2迭代中嵌套的十多次的行进距离l1迭代，可快速求得最优路径。

对比表1和图7(a)、图7(b)可以看出，当AGV位于直线参考路径上方时，此时若AGV初始航向与参考路径方向之间夹角为负值，如图中的-45°，AGV能以更短的路径距离回到参考路径；相反，AGV初始航向与参考路径方向之间夹角为正值，如图中的45°，AGV规划路径长度较长，但曲率变化较为平缓，这有利于AGV的路径跟踪。

图7 直线段不同位姿下规划路径结果

4.2 偏离圆弧路径

以不同的初始位姿规划重回圆弧的最短路径，圆弧路径的半径为1.44 m，假设圆弧路径起点为(0，0)，圆心为(0，1.44)。AGV在圆弧路径外部时，假设全局坐标为(0，-1)和(1，-1)，航向角分别为-15°、0、45°，当AGV再圆弧路径内部时，全局坐标为(0，0.5)，航向角分别为-45°、0、15°,仿真结果如表2所示。

表2 直线段不同位姿下得到的参数l1、l2

选取表中第1、6、8三种情况进行分析，结果如图8所示。在图8 (a)、图8 (b)中可以看出，叉车型AGV在圆弧路径内外都能在多约束条件下规划出一条曲率连续、具有良好的跟踪性能规划路径；在图8 (c)中，可以看出通过十多次构造距离l2迭代，每次l2迭代中嵌套的十多次的行进距离l1迭代，可快速求得最优路径。

图8 圆弧段不同位姿下规划路径结果

5 结束语

本文针对工厂环境下叉车型AGV因避障或人为使用后大幅度偏离参考路径后需重新自动回到参考路径时的路径规划问题进行了研究，设计了基于3次B样条曲线的路径规划算法，从而实现了AGV在多约束条件下，规划出回到参考路径的最短路径。最后通过Matlab对直线和圆弧参考路段进行了仿真实验，实验表明本文提出的轨迹规划算法能够快速、高质量地规划出具有良好跟踪效果的最短路径。