张家旭，周时莹，刘晔，郭崇，赵健

（1.吉林大学汽车仿真与控制国家重点实验室，吉林长春 130022；2.中国第一汽车集团有限公司智能网联研发院，吉林长春 130011）

自主代客泊车系统被公认为是解决“最后一公里”自动驾驶问题的有效手段，其核心组件包括感知、定位、规划和控制，开发高计算效率且满足汽车非完整约束的路径规划方法是自主代客泊车系统落地的必要条件之一［1-2］.为了加速自主代客泊车系统落地，国内外众多学者对自主代客泊车路径规划问题展开了广泛研究.

目前，自主代客泊车路径规划问题研究可划分为基于优化的规划方法［3-6］、基于栅格的规划方法［7-8］和基于随机采样的规划方法［9-10］.文献［3］基于龙格-库塔法建立了包含汽车运动学约束和避障约束的自主代客泊车路径离散优化模型，并采用内点法求解该离散优化问题来获得最优的自主代客泊车路径.文献［4-6］以能量消耗最小和时间消耗最小为优化目标，基于优化理论获得自主代客泊车路径.尽管上述基于优化的规划方法可以得到最优的自主代客泊车路径，但优化问题求解过程所需的较大计算量限制了其工程应用.

相对于基于优化的规划方法，基于栅格的规划方法和基于随机采样的规划方法是通过离散化自主代客泊车路径搜索空间来提高计算效率，并分别通过精细化离散空间和增加采样点数量来提高算法的完备性.文献［7］和文献［8］采用栅格地图离散化自主代客泊车路径搜索空间，并分别采用D*Lite 算法和混合A*算法规划自主代客泊车路径，但规划的自主代客泊车路径精度受限于栅格地图的细化程度，而过于精细的栅格地图所需的计算量较大.文献［9］和文献［10］采用随机采样点离散化自主代客泊车路径搜索空间，并分别采用快速随机扩展树算法（Rap⁃idly Random Tree，RRT）和RRT*算法规划自主代客泊车路径，但规划的自主代客泊车路径难以满足汽车非完整约束.

鉴于此，本文将自主代客泊车路径细分为用于引导汽车安全行驶到泊车位附近的引导路径和用于引导汽车安全驶入泊车位的泊车路径，并分别基于改进快速行进树算法和Dubins曲线规划引导路径和泊车路径.首先，采用类广度优先搜索策略建立环境地图的“路径场”，并提出一种高计算效率的避障检测策略，基于环境地图“路径场”提出符合汽车非完整约束的远端参考点和近端参考点选取原则与路径节点更新原则，通过使路径节点逐渐靠近目标节点来完成自主代客泊车引导路径规划任务.随后，基于Dubins 曲线规划满足初始泊车方位角任意性要求和泊车位方位角的非唯一性要求的泊车路径，引导汽车安全驶入泊车位.最后，仿真验证所提自主代客泊车路径规划方法的可行性，结果表明：相对于基于传统快速行进算法的自主代客泊车路径规划方法，所提方法规划的自主代客泊车路径满足汽车非完整约束要求，可以安全引导汽车完成自主代客泊车任务.

1 引导路径规划

快速行进树算法采用类广度优先搜索策略建立环境地图的“路径场”，并沿着环境地图“路径场”梯度下降方向快速规划出成本最小的无碰撞路径.但环境地图“路径场”梯度下降方向变化率不满足汽车的非完整约束要求，使规划的无碰撞路径不可执行［11］.为此，本节在类广度优先搜索策略建立的环境地图“路径场”基础上改进快速行进树算法，并利用改进的快速行进树算法规划自主代客泊车引导路径，使规划的路径满足汽车非完整约束要求.因此，改进的快速行进树算法包括：环境地图“路径场”建立模块和路径规划模块.其中，环境地图“路径场”建立模块的流程如图1 所示，采用候选列表保存成本值未修正的节点，采用开放列表保存成本值已修正但未扩展的节点，以及采用封闭列表保存成本值已修正且已扩展的节点.并且从目标节点开始，采用类广度优先搜索策略维护候选列表、开放列表和封闭列表来建立环境地图“路径场”.

基于图2 所示的环境地图“路径场”建立实例，阐述图1 所示的改进快速行进树算法“路径场”建立模块流程.如图2 所示，成本值已修正且已扩展的目标节点A和节点B保存在封闭列表中，成本值已修正但未扩展的节点C、节点D和节点E保存在开放列表中，成本值未修正的节点F和节点G保存在候选列表中.从开放列表中取出成本值最小的节点D作为待扩展节点，将待扩展节点D圆邻域与候选列表交集作为待修正集合{节点F，节点G}.顺序取出待修正集合中的节点，将节点F圆邻域与开放列表交集中使其成本值最小的节点D作为其候选父节点，将节点G圆邻域与开放列表交集中使其成本值最小的节点D作为其候选父节点.若待修正集合中节点与其候选父节点构成的路径为无碰撞路径，则将此节点从候选列表中删除，并且将此节点标记为已修正节点，其候选父节点作为其真实父节点.当待修正集合为空时，更新开放列表和封闭列表，即将已修正节点插入开放列表中，将待扩展节点D从开放列表中删除，将待扩展节点D插入封闭列表中.

图1 环境地图“路径场”建立模块流程Fig.1 The building process of the“path field”of environmental map

图2 环境地图“路径场”建立实例Fig.2 The building instance of the“path field”of environmental map

在图1 所示的改进快速行进树算法环境地图“路径场”建立模块流程中，判断待修正集合中节点与其候选父节点构成的路径是否为无碰撞路径是一项需要较大计算量的工作［12］.为了提高算法的计算效率，本节采用多边形描述障碍物，并且将路径与多边形不相交问题转化为路径与多边形的边不相交问题，即两条线段不相交问题.如图3 所示，基于向量积运算性质，可得图3（a）中线段M1M2与线段N1N2不相交的充分条件为

图3 无碰撞路径判断策略Fig.3 The judgment strategy of collision-free path

式中：Lmax为点M1、M2、N1和N2之间的最大距离.

图3（b）～（d）中线段M1M2与线段N1N2不相交的充分条件为：

式中：×表示向量积运算；∥表示或运算.

在图1 所示的环境地图“路径场”建立模块流程的基础上，改进快速行进树算法.基于图4 所示的路径规划流程，规划满足汽车非完整约束的自主代客泊车引导路径.

图4 路径规划模块流程Fig.4 The process of the path planning module

如图4 所示，改进快速行进树算法路径规划模块主要依据选取的远端参考点和近端参考点来更新路径节点，使路径节点从起始节点开始逐渐靠近目标节点，进而规划出满足汽车非完整约束的自主代客泊车引导路径.因此，远端参考点和近端参考点的选取原则与路径节点的更新原则是路径规划模块的核心.如图5（a）所示，在当前路径节点P1的大圆邻域内选取成本值最小的节点Pfar作为圆心点，将以节点Pfar为圆心点的圆邻域内所有节点的横、纵坐标平均值构成的节点作为当前路径节点P1的远端参考点Pfar，以避免选取的远端参考点靠近障碍物.为了提高计算效率，将当前路径节点P1与远端参考点Pfar之间的路径作为汽车轮廓内边缘行驶轨迹，若当前路径节点P1与远端参考点Pfar之间的路径与障碍物相交，则采用同样的方式选取当前路径节点的近端参考点Pnear，并且以近端参考点Pnear作为当前路径节点P1更新的参考点.若当前路径节点P1与近端参考点Pnear之间的路径方向满足汽车非完整约束要求，则沿着当前路径节点P1与近端参考点Pnear之间的路径方向更新当前路径节点P1；否则，按照更新后的路径节点能够最大限度靠近近端参考点Pnear原则，基于汽车最小转弯半径更新当前路径节点P1.

图5 参考点选取原则和路径节点更新原则Fig.5 The principles of selecting reference point and updating path node

如图5（b）所示，若当前路径节点P1与远端参考点Pfar之间的路径与障碍物不相交，则远端参考点Pfar作为当前路径节点P1更新的参考点.若当前路径节点P1与远端参考点Pfar之间的路径方向满足汽车非完整约束要求，则沿着当前路径节点P1与远端参考点Pfar之间的路径方向更新当前路径节点P1；否则，按照更新后的路径节点能够最大限度靠近远端参考点Pfar原则，基于汽车最小转弯半径更新当前路径节点P1.

2 泊车路径规划

当汽车沿着自主代客泊车引导路径行驶到斜向泊车位附近时，泊车路径用于将汽车安全地引导到斜向泊车位，进而完成自主代客泊车任务［13］.考虑初始泊车方位角的任意性，以及斜向泊车位方位角的非唯一性，基于Dubins曲线，本节规划针对斜向泊车位的泊车路径.

Dubins 曲线是在满足汽车非完整约束和边界约束条件下，连接泊车起始点和目标点的最短路径.采用R表示汽车以最小转弯半径右转，采用S表示汽车沿着直线行驶，采用L 表示汽车以最小转弯半径左转，则连接泊车起始点和目标点的最短路径是图6所示的四种Dubins 曲线之一，即RSR，RSL，LSR 和LSL.本节基于文献［14］提出的方法，计算连接泊车起始点和目标点的四种Dubins 曲线，并将无碰撞且最短的Dubins曲线作为泊车路径.

图6 基于Dubins曲线的泊车路径Fig.6 The parking path based on Dubins curves

3 仿真分析

在VC++6.0 环境中，仿真验证本文提出的基于改进快速行进树算法的自主代客泊车路径规划方法.仿真验证过程采用的汽车参数为：汽车轴距L=2.405 m、汽车宽度W=1.523 m、汽车前悬长度Lf=0.800 m、汽车后悬长度Lr=0.950 m 和汽车最小转弯半径Rmin=4.200 m，仿真结果如图7 所示.考虑汽车轮廓的影响和后续路径跟踪控制模块容差要求，对障碍物进行了膨胀处理，图7 中的虚线封闭区域为障碍物膨胀区域.

图7 自主代客泊车路径规划结果Fig.7 The planning results of autonomous valet parking path

图7（a）为采用类广度优先搜索策略建立的环境地图“路径场”，圆圈和五角星分别表示自主代客泊车起始点和目标点；图7（b）为快速行进树算法沿着图7（a）所示的环境地图“路径场”梯度下降方向规划出的引导路径，粗实线表示引导路径，细实线表示当前节点的梯度下降方向；图7（c）、图7（d）为本文提出的改进快速行进树算法规划出的引导路径，粗实线表示改进快速行进树算法规划出的引导路径，虚线表示快速行进树算法规划出的引导路径；图7（e）为汽车沿自主代客泊车路径行驶产生的包络.由图7（b）～（e）可知，快速行进树算法和本文提出的改进快速行进树算法规划出的引导路径均为无碰撞路径.并且两种算法规划的路径长度相当，但快速行进树算法规划出的引导路径存在方向变化率较大的折点A和B，而本文提出的改进快速行进树算法规划的引导路径更平滑，满足汽车非完整约束要求.同时，由图7（e）可知，在初始泊车方位角不为零时，本文基于Dubins曲线规划出的针对斜向泊车位的泊车路径可以安全引导汽车驶入泊车位.

4 结论

1）基于改进快速行进树算法规划出自主代客泊车系统的引导路径.采用类广度优先搜索策略建立了环境地图的“路径场”，并提出了一种高计算效率的避障检测策略，以及基于环境地图“路径场”提出了一种符合汽车非完整约束的远端参考点和近端参考点选取原则与路径节点更新原则.在上述策略和原则的加持下，通过使路径节点逐渐靠近目标节点来完成自主代客泊车引导路径规划任务.

2）基于Dubins曲线规划出自主代客泊车系统的泊车路径.充分考虑初始泊车方位角任意性要求和泊车位方位角的非唯一性要求，计算连接泊车起始点和目标点的四种Dubins曲线，即RSR，RSL，LSR和LSL，将无碰撞且最短的Dubins 曲线作为泊车路径，进而完成泊车路径规划任务.

3）在VC++6.0 环境中，仿真验证本文提出的自主代客泊车路径规划方法的可行性.结果表明：相对于传统的快速行进算法，所提方法规划的自主代客泊车路径满足汽车非完整约束要求，可以安全引导汽车完成自主代客泊车任务.