融合B样条与改进APF的路径规划算法研究

夏长高,丁伟兵,韩江义

(江苏大学 汽车与交通工程学院， 江苏 镇江 212000)

0 引言

路径规划是智能车辆的核心技术，在一定要求下快速生成由初位置达到目标的安全无碰撞路径[1-2]。常用的路径规划算法有ACO、A-star、粒子群、RRT、人工势场法(APF)等。其中ACO、A-star、粒子群等算法在应用中存在计算量较大、迭代时间长，以及在未知环境下规划实时性较差等不足[3-5]。RRT算法基于概率采样向目标生成节点，存在规划路径不平滑、规划时间长的不足[6]。APF算法最早由Khatib[7]提出，应用于机器人避障领域，因其计算量小且具有较好的实时性等优点而被广泛运用。但是APF算法应用于智能车辆也存在一些不足，如易陷入局部最小值、目标不可达、路径曲率不连续、超出道路边界、不满足车辆动力学要求等[8-10]。张珂等[11]将汽车设置为椭圆形障碍物，有效降低路径曲率，但是所规划的路径局部出现曲率变化较大，会导致跟踪效果不理想。刘美辰等[12]在解决局部最小值时，通过给车辆方向盘一个转角，以摆脱局部最小值点。余腾伟等[13]基于RRT-APF的融合算法，通过RRT算法得到多个虚拟目标点，这样可以避免APF算法时陷入局部最小值点，但是因为需要应用RRT与APF的2次规划，需要的计算时间较长，实时性较低。

针对以上算法存在的不足，本文提出一种基于B样条-改进APF算法，首先加入车辆与目标距离的调节因子；设计可变边界斥力势场以及中心引力势场，以满足无人车应用于果园等非结构类型的道路，不仅可以防止车辆在避障中超出边界以及贴近中心线行驶，而且可以规划出平滑的路径；将3次准均匀B样条优化算法嵌入到改进的APF算法中，调用设计的B样条曲线优化算法，实现二阶导数的连续、路径曲率连续。

1 传统人工势场

1.1 传统人工势场法基本原理

人工势场法广泛应用于路径规划中，基本原理是在目标点周围构建引力势场，在障碍物周围构建斥力势场，通过引力与斥力的合力来控制车辆向目标点移动[14]，传统人工势场的受力示意图如图1。

图1 传统人工势场受力分析示意图

1.2 不足及改进思路

基于人工势场法原理以及结合相关文献，在传统人工势场中，只考虑了控制对象与障碍物、目标的欧几里得距离，距离障碍物越近，斥力越强，距离目标越远，引力越强[15]。但是在实际应用中存在一些问题：

1) 局部最小值。当障碍物的合斥力与目标所产生的引力在数值上大小相等，在同一直线上且方向相反，这时被控对象陷入局部最优值点，无法完成完整的路径规划。

2) 目标不可达。当在目标附近有障碍物存在时，因为靠近目标点，目标产生的引力变小，而此时障碍物产生的斥力超过目标的引力，使被控对象在目标点处抖动，无法到达目标点。

3) 超出道路边界、偏离行驶路径中心线。在复杂的道路上，避开多障碍物过程中由于多个障碍物的合斥力的作用会出现被控对象超出道路边界的情况；此外，在果园等作业道路中行驶，需要沿着作业道路的中间线行驶，但是传统人工势场法中主要依靠目标的吸引力行进，导致实际的路径无法沿着道路中心。

4) 规划避障路径曲率不连续。在靠近障碍物时，产生较大的斥力势场，而导致避障路径曲率不连续。

对于上述不足，本文提出了一种B样条-改进APF算法，可以很好地解决以上问题，该算法针对局部最小值引入虚拟障碍物；依据果园等非结构化道路对斥力场函数进行设计，并加入可变边界斥力势场、中心线引力势场；通过设计的3次B样条优化算法，确定避障控制点，对规划的路径进行拟合优化，得到曲率连续且满足车辆动力学要求的路径。

2 算法改进

2.1 障碍物斥力场的改进

传统APF的斥力场函数，在斥力场模型中，加入调节因子，使在靠近目标的过程中，斥力随着引力减小而减小，并同时减小为零，从而可以解决目标不可达的问题，并且增加一个由车辆指向目标的斥力，有助于使斥力矢量与引力矢量之间的角度减小，促进规划路径的平滑。改进的斥力场函数表示为[16]：

(1)

斥力函数为斥力场函数负梯度[17]：

(2)

(3)

(4)

式中：Freq1为障碍物指向车辆的斥力；Freq2为车辆指向目标点的斥力；Freq为合斥力；Vb为障碍物指向车辆斥力的单位向量；Vg为车辆指向目标斥力的单位向量。

解决局部最小值点问题，在本文的改进APF算法中，当控制对象处于局部最小值时，建立虚拟障碍物提供逃脱斥力；反之逃脱斥力为零。该逃脱斥力函数为：

(5)

式中：Freq3为虚拟障碍物指向车辆的斥力；Vy为虚拟障碍物指向车辆斥力的单位向量；Fsum为当前车辆所受合力大小。

2.2 可变边界斥力场的建立

农业无人车在进行避障的过程中很容易出现越过道路边界的问题，通过建立道路边界斥力场，不仅可以避免越出道路边界的问题，还可以促进车辆向中间区域行驶，使规划路径更加平缓。道路边界斥力场模型为：

(6)

式中：kedge为边界斥力势场增益系数；Yl、Yr分别为车辆质心到左右边界的距离;w为车辆宽度。

由式(6)知，当靠近边界时受到的斥力增大，可以防止无人车在避障中出现道路越界的问题[18]。

2.3 可变中心线引力势场的建立

中心线引力势场函数表示为：

(7)

2.4 3次B样条曲线优化规划路径

B样条曲线是B-样条基函数的线性组合，k阶B样条曲线可实现k-1次导数连续，因此，B样条曲线常应用于路径优化与轨迹规划中。应用B样条曲线对规划的路径进行优化，不仅可以指定阶次，而且改变控制点仅会对部分曲线形状产生改变，保留Bezier曲线优点的同时，弥补了Bezier曲线在局部无法进行修改的不足。

设存在n+1个控制点Pi(i=0,1,2,…,n)，节点向量T=[t0,t1,…,tn+k+1]，k阶B样条曲线可以定义如下[19]：

(8)

式中：Bi,k(t)是第i个k阶B样条基函数在参数t处的值，k为B样条基函数阶数(k=0,1,…,m)；下标i为B样条的序号；Pi为控制点；t为位置参数，0≤t≤1。

其中，Bi,k(t)表达式为[19]：

高等教育要符合大学生的身心发展和教育自身的规律。构建科学的渐进式专业认知教育体系需要吸取系统论、协同论、人的全面发展理论知识，全面充分的认识大学生的成长成才教育规律。

(9)

在综合优化效果和计算复杂度，采用3次准均匀B样条曲线[20-22]对改进APF规划的路径进行曲线优化，准均匀B样条曲线保留了Bezier曲线在2个端点处的性质，其节点矢量中两端节点具有重复度k+1，即t0=t1=…=tk，tn+1=tn+2=…=tn+k+1，如T=[0,0,0,0.2,0.4,0.6,0.8,1,1,1]。其他定义域在[tk,tn+1]内的节点均匀分布，重复度为1节点区间长度为Δi=常数>0(i=k,k+1,…,n)。

取k=3，则有3次B样条曲线表达式：

(10)

基函数表达式如下：

(11)

式中：t为位置参数。

将式 (11)代入式(10)，可得3次B样条曲线表达式：

t2(3P0-6P1+3P2)+

t(-3P0+3P2)+

(P0+4P1+P2)]

(12)

将式(11)用矩阵形式表达：

(13)

式中：P0、P1、P2、P3为控制点。

当有n个控制点Pi(i=0,1,…,n-1)，以P0、P1、P2、P3作为控制点得到首条3次B样条曲线；再以P1～P4绘制第2条曲线；类推以Pn-4～Pn-1绘制第n-3条曲线，绘制的曲线自然衔接，有二阶导数连续的特性。

本文设计的B样条优化算法对于控制点的选取如图2所示，该部分讲述的是局部路径控制点的选取，其余控制点也是依据该方法进行选取。其中P1-P4的控制点都是通过改进路径规划算法所得到的路径节点，控制点P1在规划路径的转向起始点；控制点P2为车辆行驶轴线方向距离障碍最近的点；控制点P3为控制点P2与P4的中间点；控制点P4为避障过程中车身回正后的质心位置；控制点P5是经过控制点P3且垂直于P3与障碍物连线的直线，与过控制点P5所在的水平轴线所形成的交点。

图2 局部路径控制点选取示意图

2.5 算法流程

本文设计的算法流程如下：

步骤1算法初始化，设置初始化参数，如初始位置Ps、目标位置Pg；给定相关参数，如引力、斥力的增益系数katt、kp、步长d0以及障碍物影响范围ρ0等。

步骤2更新车辆与障碍物、目标的位置向量以及计算距离。

步骤3计算障碍物斥力与边界斥力矢量和Freq、目标引力与中心线引力矢量和Fatt，获得作用于车辆的矢量和Fsum。

步骤4判断当前是否处于局部最优，如若判断是，设立虚拟障碍物提供逃脱斥力，并进行下一步；反之逃脱斥力为零，执行下一步。

步骤5计算车辆下一时刻的位置，沿着合力矢量方向行进一个步长，并将路径点更新在Path数组中。

步骤6获得路径数组中Bezier控制点并存储数组中，调用设计的B样条曲线优化算法模块对路径进行优化，并更新优化的路径。

步骤7判断与目标的距离是否小于常数值K(K数值设定小于步长，因为基于设置的步长难以正好到达目标点)，如果判断认定到达目标，则跳出循环，并将目标路径更新在Path数组中；反之，若与目标的距离大于K，跳到步骤2执行。

3 仿真及结果分析

3.1 针对局部最小值的改进仿真分析

为检验上述算法的可行性，本文将黄海某小型园艺电动拖拉机整车参数作为仿真检验对象，其轴距为1.6 m，宽度为1.2 m，前轮转角范围设置在(-π/4,π/4),最小转弯半径约为2 m,所以理论规划的路径曲率应低于0.5 m-1，设置障碍物的影响距离为2 m。接下来基于Matlab 2019b进行路径规划的仿真试验，分别使用本文设计的算法，与A-APF、B-APF、传统人工势场法进行对比分析，设置基础参数如表1所示。

表1 仿真基础参数设置

对于上文所提出的问题，分别将本文设计的改进算法与传统算法以及其他学者算法进行对比，其中文献[17]中所改进的人工势场算法(记作A-APF)、文献[9]中改进的人工势场算法(记作B-APF)、以及传统人工势场算法进行多次对比分析，从而验证本文提出的B样条-改进APF算法在路径规划有着良好的可行性。其中，B-APF算法的改进主要是在传统斥力势场中加入调节因子；A-APF算法在B-APF基础上，对斥力场函数改进，并加入了道路边界斥力势场。

针对局部最小值的仿真如图3所示。对于传统人工势场，在合力作用下，控制对象在X=6 m处陷入了局部最优，导致无法到达目标点；A-APF算法解决了局部最小值的问题，但避障路径曲率不连续；本文算法通过建立的虚拟障碍物提供的逃脱斥力，弥补局部最小值的不足，并基于设计的B样条优化模块，得到平滑且曲率连续的路径。

图3 局部最小值仿真曲线

对于多个障碍物的局部最小值问题，具体仿真结果如图4所示。由仿真结果可知，在传统APF算法中控制对象陷于局部最小值位置，无法到达目标位置；在改进APF算法中，增加由对象指向目标的斥力，可以顺利达到目标位置。

图4 局部最小值仿真曲线

3.2 针对目标不可达改进仿真分析

对于解决目标不可达的问题，如图5所示。在仿真结果中可以看出，应用传统APF算法由于在目标点处受到的斥力大于引力，导致无法到达目标点。本文算法在斥力场函数中加入调节因子，在接近目标点附近时，受到的障碍物斥力也会同时减小，可以达到目标点。

图5 目标不可达问题仿真曲线

3.3 加入中心线引力场模型的仿真分析

车辆在果园等非结构化道路环境中进行，作业行驶路线应沿着道路中间线，如图6所示。可见，本文改进的算法可以使车辆在无障碍行驶中，沿着中心线行进作业。

图6 无障碍路径规划算法曲线

3.4 针对超出边界的仿真分析

针对超出边界的问题，仿真结果如图7所示，在所设置的道路环境中，利用传统APF算法所规划的路径在X=15～20 m出现较大的纵向偏离现象；而通过引入可变道路边界斥力势场和中心线引力势场，规划的路径没有出现越过道路边界的现象，障碍物斥力较小的情况下，可以贴近中间线行驶，此外，所规划的路径相比较传统APF算法更加连续、平滑。

图7 针对超出边界问题的算法曲线

3.5 不同道路环境下路径规划算法对比分析

表2为各算法在不同道路环境仿真时间，本文设计的算法在复杂道路总运算时间仅为6.982 ms，依然有着APF算法运算快捷的特性。

表2 不同道路环境算法仿真时间

针对单一障碍物环境的路径规划，设置起点坐标为(0,0)，目标坐标为(12,0)，将各算法进行对比分析，仿真结果如图8所示。其中，图8(a)为路径规划仿真图，可以直观看出，本文设计的算法所规划的避障路径更加平滑。图8(b)、8(c)为路径的切向斜率、曲率，与A-APF算法相比，本文所设计的算法在路径的最大切向斜率、平均切向斜率分别降低30.64%、34.24%，在路径的最大曲率、平均曲率分别降低44.80%、39.13%；与B-APF算法相比，路径的最大切向斜率、平均切向斜率分别降低49.79%、49.00%，而B-APF规划路径的曲率不连续。

图8 单一障碍物障碍路径规划仿真结果曲线

针对复杂障碍物环境的路径规划，设置起点坐标为(0,0)，目标坐标为(27.5,0)，将各算法进行对比分析，仿真结果如图9所示。与A-APF算法相比，本文设计算法在路径最大切向斜率、平均切向斜率分别降低71.25%、8.80%，在路径最大曲率、平均曲率分别降低58.64%、33.47%；与B-APF算法相比，在路径的最大切向斜率、平均切向斜率分别降低73.23%%、34.41%，而B-APF规划的路径曲率不连续。

图9 多离散障碍物路径规划仿真结果曲线

4 结论

针对传统APF在果园等非结构化道路路径规划中存在的不足，提出了一种B样条优化与改进APF的融合算法，加入了车辆与目标距离的调节因子；对斥力势场函数进行了改进，引入了可变边界斥力势场以及中心引力斥力势场，满足无人作业车应用于果园等非结构类型的道路；将3次准均匀B样条优化算法嵌入到改进的APF算法中，对规划的路径进行优化，规划平滑并曲率符合车辆动力学约束的路径。

仿真实验表明，所设计的B样条-改进APF算法相比其他改进的路径规划算法，曲率连续，在复杂路径下的最大曲率与平均曲率为0.230 2、0.065 2 m-1，满足仿真对象黄海某小型园艺电动拖拉机的动力学约束。此外，所设计的算法在复杂路径下运算时间仅为6.982 ms，依然有着人工势场法计算快速的特性，该算法可以应用于在线轨迹规划。本文设计主要针对果园等非结构化道路的避障路径规划，下一步会融入自适应模块，使算法可以兼容多种类型的道路。