路径规划是移动机器人领域的核心技术之一，主要分为全局路径规划和局部路径规划．全局路径规划是指机器人在有障碍物的静态环境中，规划出一条从起点到终点的无碰撞路径．典型的全局路径规划方法包括Dijkstra和A*等算法[1]．然而，由于许多路径规划算法规划出的路径存在尖角，并不符合机器人的运动学要求，因此在局部路径规划阶段进行路径平滑必不可少．当下应用在移动机器人中的路径平滑技术主要有多项式插值、贝塞尔曲线、B样条曲线和NURBS曲线．还有一些学者使用特殊曲线进行路径的平滑，如杜宾曲线和回旋曲线等[2]．其中，贝塞尔曲线和B样条曲线常用于轮式机器人的路径规划[3]．B样条曲线具备局部修改的特点，比贝塞尔曲线更加灵活，是机器人路径平滑最常用的数学工具．文献[4]在路径平滑阶段利用B样条曲线进行处理，成功消除了路径的折角问题．文献[5]在粒子群算法的基础上结合三次B样条曲线实现了规划路径的二阶导的连续性(C2)．文献[6]基于三次B样条曲线提出的平滑算法，保证路径除在个别孤立点外都可以满足路径的C2连续性．文献[7]用五次B样条曲线实现轨迹的C4连续性．通过以上研究分析，B样条曲线在实现路径连续性上具有显著优势，但上述研究并没有考虑路径曲率约束问题．文献[8]使用B样条曲线生成了曲率连续的路径，但并未考虑机器人最大曲率约束问题．文献[9]提出利用B样条曲线来修改不满足路径曲率约束的部分，以生成满足连续性和曲率约束的平滑路径，但并未给具体的路径曲率计算公式．文献[10]给出了受机器人转弯半径约束的曲率计算公式，并规划出多条待选路径，删除不符合曲率约束的路径，虽然满足了路径的曲率约束条件，但是多条路径的规划及其选优策略增加了系统的负荷．文献[11]在路径的曲率约束部分给出了段参数约束下的曲率计算公式，该公式由段夹角和段长约束，并未给出不符合曲率约束部分具体的修改策略．文献[12]则从曲线本身入手，设计满足机器人曲率约束的曲线设计公式，但该方法使得曲线的计算更加复杂．综上，目前国内外学者在利用B样条曲线平滑路径使得平滑后路径既满足C2连续和最大曲率约束的研究并不多，且对于不满足最大曲率约束部分的路径如何修改也并未给出明确的方法．为此，本研究基于三次准均匀B样条曲线，优化传统的三次B样条曲线递归方式，提高了所提出算法的整体运行效率．并在此基础上，对B样条曲线增加最大曲率约束条件，使得平滑后的路径可以实现C2连续及移动机器人的最大曲率约束．1 B样条曲线递归过程优化1.1　德布尔递推公式重复项问题描述由P0，P1，…，Pn共n+1个控制点以及一组非递减的连续变化的矢量节点ui可定义一k阶B样条曲线，其表达式为Pu=P0  P1  ⋯  PnB0,kuB1,ku    ⋮Bn,ku=∑i=0nPiBi,ku ，(1)式中：P为控制点坐标；u为矢量节点；n为控制点数量；k为B样条曲线阶次；i为第i个节点矢量、控制点或基函数；Bi,k(u)为第i个k阶B样条基函数，与控制点Pi相对应．式(1)中基函数由德布尔-考克斯递推公式构成，表达式为Bi,0(u)=1(ui≤uui+1),0(其他);Bi,k(u)=U1Bi,k-1(u)+U2Bi+1,k-1(u);U1=u-uiui+k-ui;U2=ui+k+1-uui+k+1-ui+1, (2)式中U1和U2为基函数Bi,k(u)的系数．图1为式(2)的递归过程，高阶基函数由两个低阶基函数构成，图中：[ui，ui+1)为矢量节点区间；Bi,k(u)为k阶基函数；a和b为红色扇形区域斜边．10.13245/j.hust.220511.F001图1德布尔算法递归过程根据式(2)可以得到图1中基函数Bi-2,2(u)和Bi-1,2(u)的表达式分别为：Bi-2,2(u)=U1Bi-2,1(u)+U2Bi-1,1(u);Bi-1,2(u)=U1Bi-1,1(u)+U2Bi,1(u). (3)根据式(3)可知：当求解Bi-2,2(u)和Bi-1,2(u)的表达式时，Bi-1,1(u)的表达式递归调用了两次．随着后续递归过程的深入，重复递归调用的现象会愈加严重，导致算法的整体运行效率降低．1.2　FA算法本研究基于文献[13]对于基函数的简化思路，提出了一种扇形输出算法(FA)，可以有效提高B样条曲线平滑路径的效率．根据式(2)可知，在区间[ui，ui+1)上仅有基函数Bi,1(u)非0．此时，仅在区间[ui，ui+1)上Bi-1,2(u)的表达式可简化为Bi-1,2(u)=U2Bi,1(u)．(4)根据式(4)的简化原理，在区间[ui，ui+1)上，图1中红色扇形区域的a边上基函数的表达式只由德布尔递推公式的第二项构成；同理，在b边上基函数的表达式只由递推公式的第一项构成．只有基函数Bi-1,3(u)由递推公式的两项构成，而决定每一项表达式的则是每项的系数U1或U2．基于以上结论，本研究在不同区间计算每个基函数的系数，并设置系数矩阵C来存储基函数的系数，采用上三角的形式存储，没有基函数的部分则设为0．合理设置下标以使得系数矩阵与基函数一一对应，这样当计算每个基函数时只须要调用相对应的系数即可．该优化算法可提高本研究后续算法的整体运行效率．FA算法主要思路为：初始化系数矩阵C；算法输入为B样条曲线的阶次k、节点数量i和节点向量u，算法输出为系数矩阵C；在系数矩阵C右上角存储基函数系数，矩阵C第1行存储如图1中a边的系数，矩阵C的对角线存储如图1中b边的系数，矩阵C右上角其他位置存储图1扇形区域内部的系数即可，系数矩阵C的具体存储方式为C=U1i      U1i-1              U1i-2    0             U2i       U1i-2+U2i-1    0                 0                         U2i．2 满足最大曲率约束的B样条曲线平滑算法2.1　问题描述全局路径规划算法规划出的路径通常是一条折线路径，如图2中折线段Z，这样的路径在拐点处曲率变化非常大且不连续．根据机器人的运动学可知，移动机器人无法在这样的路径上稳定行驶．因此可以采用B样条曲线等数学工具对初始折线路径进行平滑．10.13245/j.hust.220511.F002图2路径平滑过程中存在的问题但是B样条曲线在不同控制点数量下的拟合程度不同，如图2中曲线段E和N．通过全局路径规划算法得到的初始控制点数量一般较少，这样平滑后的路径拟合程度低，如图2中E所示，增大了机器人发生碰撞的危险．为此，本研究期望得到一条满足机器人最大曲率约束且拟合度高的平滑路径．2.2　控制点增加策略针对平滑后路径拟合度低的问题，采用最多的方法是增加控制点数量，这样平滑后路径更加逼近初始控制折线．图3展示了分别用7，13和23个控制点对同一条路径的平滑的结果．10.13245/j.hust.220511.F003图3不同控制点下的平滑的路径虽然控制点数量的增加提高了路径的逼近程度，但是导致了路径拐点处曲率的升高．这里给出新的控制点增加策略，以提高平滑后路径的拟合程度．目前对于B样条曲线控制点增加的主要策略是中点插入策略，如图4(b)中所示，其方法是在两个控制点的中点再插入一个控制点，以此来提高路径的拟合程度．而在实际的路径平滑过程中，平滑锐角路径所生成路径的曲率比平滑钝角路径所生成的曲线的曲率大很多，如图4中(a)和(c)所示．10.13245/j.hust.220511.F004图4不同角度下的路径平滑结果对锐角和钝角处路径分别采取不同的控制点增加策略，在满足最大曲率约束的条件下，可以使平滑后的钝角处路径比锐角处路径具有更高的拟合程度．因此，本研究提出以90°为分界线，对锐角和钝角分别设置不同的划分系数Ia=1/2和Ib=1/5，即在锐角段处采用中点插入策略，在钝角段处则在1/5处插入控制点．2.3　路径最大曲率约束条件在增加控制点后，平滑后的部分路径的曲率过大，因此本研究提出基于机器人最大曲率约束的路径修改策略．采用常见的轮式非完整约束移动机器人的运动学模型，用等效的自行车模型代替四轮机器人，简化了数学方程[14]，如图5所示，图中：{o}为全局坐标系；{w}为局部坐标系；xo和yo为全局坐标系的横纵坐标；xw和yw为局部坐标系的横纵坐标；β为转向角；θ为相对于全局坐标系xo轴的角度值；H为机器人的瞬心；M为机器人的轴距．当移动机器人转弯时有最大转向角βmax，限制了机器人可以通过的转弯半径，其表达式为R=M/tan β．(5)10.13245/j.hust.220511.F005图5运动学模型由式(5)推导得到最大曲率kmax的表达式为kmax=1/Rmin=(tan β)/M，(6)式中Rmin为机器人转弯半径的最小值．为满足移动机器人的平稳运动，平滑后的路径应当满足连续曲率和峰值曲率，而B样条曲线具有局部修改性，因此以节点为分界点将路径划分为数段，方便后续对不符合最大曲率约束的部分进行修改．为保证整条路径满足曲率要求，须要保证B样条曲线每一段的最大曲率kpartmax要小于最大曲率，即kpartmax≤kmax．(7)根据移动机器人的自身参数，由式(6)可以求解出平滑路径的最大曲率kmax．为了满足移动机器人的峰值曲率约束，须要计算每一段曲线的最大曲率值kpartmax，这是一个比较棘手的问题．本研究利用参数化方程给出了一种求解方法．文献[15]给出的三次准均匀B样条的参数化描述方程为x(u)=(-u3+3u2-3u+1+u3cos αi)Lj;y(u)=u3Ljsin αi, (8)式中：αi为第i个相邻两段路径之间的夹角；Lj为第j段路径的长度．式(8)可通过平面参数方程的曲率计算公式进一步推导曲率计算公式为k(u)=x'(u)y″(u)-x″(u)y'(u)(x'(u))2+(y'(u))23/2．(9)根据式(8)和(9)，易得到段参数下三次B样条曲线的曲率计算公式为    k=k(u)=(2u2-2u)sin αi/[2u4(1-cos αi)+4u3(cos αi-1)+2u2(3-cos αi)+1]. (10)在计算出各段路径的最大曲率值后，须要对不满足最大曲率约束的路径重新平滑．曲线的最大曲率受控制线的长度和夹角约束，其表达式为(sin αi)/(6Lj)[(1-cos αi)/8]-3/2≤kmax．(11)根据式(11)易得到与最大曲率相对应最小段长度Lmin为Lmin=(sin αi)/(6kmax)[(1-cos αi)/8]-3/2．(12)当某一段曲率值不符合式(7)的约束条件时，本研究给出了一个快速有效的解决方案．如图6所示，该路径由P1，P2，P3和P4四个初始控制点拟合而成，其中夹成α1角的段路径曲率不符合约束条件．根据式(12)可求得最小段长度Lmin，此时以Lmin为底做一个等腰三角形，可以得到两个新的控制点P5和P6．将原来的锐角α1化为两个钝角α2处理，这样再次拟合得到的路径便可以满足最大曲率要求．10.13245/j.hust.220511.F006图6最大曲率约束修改策略另外，还须要保证平滑后的曲线能够满足其速度和加速度连续性的要求，即须要保证曲线C2连续．因为B样条曲线在重复度m的节点上，所以基函数Bi,k(u)是Ck-m连续的．本研究采用三次B样条曲线，因此只须保证在节点处的重复度为1．3 实验结果与分析3.1　基于FA算法和德布尔算法的B样条曲线对比实验扇形输出算法(FA)算法旨在提高平滑算法的整体运行效率，在Visual Studio 2015上进行对比实验，两种算法均采用C++语言编写，调用计时函数分别记录两种算法的运行时间t，f为次数．分别用德布尔算法和FA算法为基础的B样条曲线平滑路径50次，记录每次算法的运行时间，结果如图7所示，再求解50次的平均运行时间tavg的值，结果如表1所示．10.13245/j.hust.220511.F007图7两种算法50次路径平滑运行时间对比10.13245/j.hust.220511.T001表1德布尔算法和FA算法的对比实验参数及结果算法阶次控制点数量ftavg/ms德布尔算法365012.561FA算法36507.848根据图7可知，FA算法与德布尔算法一样，运行时间的误差均能控制在10%以内．根据表2中的tavg可知，在相同阶次和控制点数量的情况下，FA算法相比德布尔算法提高了38%的运行效率．因此，相较德布尔算法，FA算法效率更高．10.13245/j.hust.220511.T002表2路径平滑算法对比结果算法最大曲率值/m-1路径长度/m连续性插值法0.536 026.837 7—贝塞尔曲线0.213 722.683 5G2B样条曲线0.184 121.783 1C2NURBS曲线本方法0.437 30.142 026.379 027.369 8C2C23.2　满足最大曲率约束的平滑算法仿真实验本研究所述平滑算法可保证平滑路径满足最大曲率约束要求和C2连续性．为验证该算法的有效性，在Matlab R2016a上进行数据分析．路径的最大曲率为0.3 m-1，使用本研究所述算法对路径进行平滑，平滑结果见图8，图中x和y为机器人路径的坐标值．为验证平滑路径的连续性，对路径进行求导计算，其求导结果见图9，图中：g为标准路径长度；p为路径值；d′为一阶导数；d″为二阶导数．根据式(10)计算路径曲率，其结果见图10，图中c为路径曲率值．10.13245/j.hust.220511.F008图8路径平滑结果10.13245/j.hust.220511.F009图9平滑后路径的C2连续性10.13245/j.hust.220511.F010图10平滑后路径的曲率图8(a)和图10(a)为基于初始控制点用三次B样条曲线平滑后的路径．平滑后的路径满足最大曲率约束，但是路径基本脱离了初始折线段的控制．图8(b)是在基于角度增加控制点后的平滑路径，平滑后的路径相对于初始控制折线段的拟合程度更高，且在钝角处的平滑效果比在锐角处更好．根据图10(b)可知：在增加控制点后，平滑后的部分路径的曲率已经超过了最大曲率约束，这样的路径是不希望得到的．图8(c)和图10(c)分别展示了经过最大曲率约束条件修改后的平滑路径和路径曲率．平滑后的路径不仅与初始折线段有很高的拟合程度，而且满足了机器人的最大曲率约束要求．图9分别展示了各个阶段路径的一、二阶导数的情况，可以看到：路径的一二阶导数均具有连续性，证明了路径的C2连续．上述仿真结果证明了采用本研究所提出平滑算法得到的路径可以满足路径的C2连续性和最大曲率约束要求．另外，为了证明本方法的有效性，与目前常用的路径平滑算法进行比较，对比结果如表2所示．所有方法求解路径长度和曲线曲率的计算公式均一致，平滑后路径的最大曲率要求为0.3 m-1．根据表2可知：贝塞尔曲线和传统B样条曲线的最大曲率值和路径长度均比其他方法低，其原因在仿真实验中也提到过，如图8(a)中所示平滑路径，由于算法的拟合效果并不好，导致平滑后的路径相对于初始控制折线段偏离较大．插值法采用的是赫尔米特插值法，由表2可知：插值法和NURBS曲线均能够得到较好的拟合初始控制折线段，但最大曲率值均超过了规定值0.3 m-1．在连续性方面，插值法并不能满足平滑后路径的C2连续，贝塞尔曲线也仅能满足路径的G2连续，而本研究方法是实现路径的C2连续最简便的方法，可同时保证最大曲率约束的要求和路径的C2连续，虽然拟合程度的提高也增加了路径长度，但是其偏差可控制在5%的范围内．3.3　实验室环境下的实物验证为了进一步验证平滑算法，将平滑算法移植到移动机器人平台上进行实验．配备光电编码器和惯性测量单元(IMU)来获取移动机器人的位置、速度和加速度等信息．操作系统层面采用ROS模块进行数据传输和运动控制，加载Ubuntu18.04系统．将本研究所述算法与其他必要的开源算法集成为一个局部路径规划器加载到Navigation导航框架上完成配置，实验要求的曲率上限为0.3 m-1．在获取数据后，通过Matlab和Origin软件进行分析．在实验室中搭建实验环境，采用经典的A*路径规划算法得到初始控制折线段，利用本研究所述三次B样条曲线对初始控制折线段进行平滑处理得到最终平滑路径．利用A*算法进行路径规划首先须要对环境进行栅格化．实验所采用的移动机器人大小为26 cm×21 cm×17 cm，故环境中每个栅格大小设置为30 cm×30 cm．栅格化后的环境如图11所示，图中：红色代表起点；蓝色代表终点；黑色代表有障碍物的区域；白色代表机器人可运行区域．10.13245/j.hust.220511.F011图11栅格化实验环境在A*算法得到初始的控制折线段路径后，分别用B样条曲线和本研究所述算法对路径进行平滑，分别得到图12中的绿色和红色两条路径．10.13245/j.hust.220511.F012图12按照本研究方法平滑后路径将获取到的路径位置信息进行处理，并在Origin2019上进行绘制，得到图13中的红色曲线，其他曲线利用Matlab计算绘制得到，作为对比．10.13245/j.hust.220511.F013图13本研究方法平滑路径过程根据图12和图13可知：由本方法平滑得到的路径相较传统三次B样条曲线具有更高的拟合度，在一定程度上降低了与障碍物发生碰撞的危险，且平滑路径相较只增加控制点的平滑路径，最终平滑路径不会出现局部曲率值过大的问题，使得机器人在实际跟踪过程中运行更加平稳．在得到移动机器人行走的路径信息后，根据式(10)，使用Matlab对路径的曲率进行计算并绘制，见图14中的绿色虚线，蓝色实线为仅增加控制点平滑后的路径曲率，作为对比．由图14可知：增加控制点后路径的最大曲率为0.390 m-1，满足最大曲率约束后路径最大曲率为0.296 m-1，最终得到的路径曲率相比增加控制点后的曲率降低了25.6%，满足最大曲率约束条件．10.13245/j.hust.220511.F014图14平滑前后路径曲率变化关于路径的连续性方面，根据获取到的速度与加速度信息绘制相应曲线，如图15所示，可以看到平滑后的路径保证了C2连续性．实验证明了本研究所述算法的有效性，且移动机器人在实际运行过程中可以稳定跟随这样的路径．10.13245/j.hust.220511.F015图15路径的C2连续性4 结语本研究提出一种满足路径最大曲率约束与C2连续性的路径平滑方法．在提高算法效率方面，基于传统的德布尔递推公式提出了FA算法，通过设置系数存储矩阵，减少了重复项的计算，提高了平滑算法的整体运行效率．在平滑算法设计方面，基于角度增加B样条曲线控制点的数量，使得平滑后的曲线比其他算法具有更高的拟合程度．针对增加控制点后不满足最大曲率约束的部分，建立基于路径长度和夹角约束下的控制折线路径修改策略，使得平滑后的路径可以满足移动机器人的最大曲率约束条件．结合B样条曲线的性质，保证了平滑的路径满足C2连续性．但是在避障层面尚未进行深入研究，路径修改后存在一定的碰撞危险，并且针对膨胀系数等问题也有待进一步研究．