自主代客泊车系统可以有效解决汽车“最后一公里”自动驾驶问题，快速高效的路径规划方法是制约自主代客泊车系统产品落地的重要因素之一[1-2]．现有的自主代客泊车路径规划方法依据搜索空间不同的处理方式可划分为基于优化的规划方法[3-6]、基于栅格的规划方法[7-8]和基于随机采样的规划方法[9-10]．基于优化的自主代客泊车路径规划方法综合考虑汽车运动学约束、避障约束和机械约束等，在连续空间建立自主代客泊车路径约束优化模型，采用伪谱法或龙格库塔法将连续空间的自主代客泊车路径约束优化模型转化为离散空间的自主代客泊车路径约束优化模型，并采用非线性规划方法或智能优化方法求解得到最优的自主代客泊车路径．虽然基于优化的自主代客泊车路径规划方法可以得到的最优的自主代客泊车路径，但高计算量限制了其工程应用．基于栅格的自主代客泊车路径规划方法采用栅格地图描述自主代客泊车场景，并采用A*，D*和D* Lite等启发式搜索算法规划自主代客泊车路径，但栅格地图的细化程度决定了自主代客泊车路径规划的精度，过于精细的栅格地图增加了算法的计算量．基于随机采样的自主代客泊车路径规划方法通过离散化采样搜索空间和采用渐近递增的搜索方式实现自主代客泊车路径规划，但传统快速扩展随机树(rapidly exploring random tree，RRT)、RRT*算法搜索效率较低，仅能获得近似最优路径．鉴于此，针对室内结构化场景的自主代客泊车路径规划问题，基于批处理先验知识树(batch informed tree，BIT*)算法和滚动优化思想提出一种自主代客泊车路径规划方法．首先，基于BIT*算法规划全局自主代客泊车路径，并采用数组实现影响BIT*算法搜索效率的树节点优先队列和树边优先队列，提高全局自主代客泊车路径规划效率．随后，基于滚动优化思想和圆弧-直线组合方法规划满足汽车运动学约束和避障约束的局部自主代客泊车路径．最后，仿真验证所提出的自主代客泊车路径规划方法的可行性和有效性，结果表明：相对于基于传统RRT算法的自主代客泊车路径规划方法，本文方法搜索效率更高，并且规划的自主代客泊车路径满足汽车运动学约束和避障约束．1 全局路径规划自主代客泊车系统全局路径规划的任务是建立从起始点到目标点的无碰撞路径[11]，采用BIT*算法规划全局自主代客泊车路径．图1所示的BIT*算法流程图包含3个模块[12]，模块1主要用于实现剪枝操作和采样操作；模块2主要用于实现树节点扩展操作；模块3主要用于实现重布线操作和树边扩展操作．采用g值表示起始点到当前节点的实际成本，结合图1所示的BIT*算法流程图，BIT*算法对可行路径的优化过程如图2所示．如图2(a)所示，当前时刻采用BIT*算法得到的从起始点A1到目标点A8的可行路径为：“节点A1→节点A2→节点A3→节点A4→节点A5→节点A6→节点A7→节点A8”．激活模块1中的剪枝操作，从树节点列表中删除椭圆域之外的节点B3，B4和B5，从树边列表中删除与椭圆域边界有交点的边A6B3，B2B4，B4B5和B5B6，将因删除边B5B6导致g值变为无穷大的节点B6插入到采样点列表中，并将其从树节点列表中删除．10.13245/j.hust.210914.F001图1BIT*算法流程图如图2(b)所示，激活模块1中的采样操作为将新的采样节点C1，C2，C3和C4插入到采样节点列表中．至此，采样节点列表中包含节点C1，C2，C3，C4和B6，树节点优先队列中包含节点A1，A2，A3，A4，A5，A6，A7，A8，B1和B2，树边优先队列为空．考虑空树边优先队列返回的最小估计成本为无穷大，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点A1作为待扩展边的一端，将采样节点列表中位于节点A1圆邻域内的节点C1和C2作为待扩展边的另一端，并且将可以改善当前路径成本的待扩展边A1C1和A1C2插入到树边优先队列中．考虑待扩展边A1C1和A1C2未插入到树边列表中，将节点C1和C2的g值设置为无穷大．10.13245/j.hust.210914.F002图2BIT*算法路径优化示意图如图2(c)所示，考虑树节点优先队列中最优节点A2的估计成本大于树边优先队列中最优待扩展边A1C1的估计成本，激活模块3来从树边优先队列中取出最优待扩展边A1C1，将节点C1从采样节点列表中删除，并插入到树节点列表中和树节点优先队列中，将待扩展边A1C1插入到树边列表中，正式成为树的一部分．考虑树节点优先队列中最优节点C1的估计成本小于树边优先队列中最优待扩展边A1C2的估计成本，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点C1作为待扩展边的一端，树节点列表与节点C1的圆邻域交集中不存在节点可以作为待扩展边的另一端，而采样节点列表中位于节点C1圆邻域内的节点C2可以作为待扩展边的另一端，将待扩展边C1C2插入到树边优先队列中．考虑树节点优先队列中最优节点A2的估计成本大于树边优先队列中最优待扩展边A1C2的估计成本，激活模块3来从树边优先队列中取出最优待扩展边A1C2，将节点C2从采样节点列表中删除，并插入到树节点列表和树节点优先队列中，将待扩展边A1C2插入到树边列表中，正式成为树的一部分．考虑以节点C2为终端点的待扩展边C1C2估计成本大于节点C2的g值，将其从树边优先队列中删除．考虑空树边优先队列返回的最小估计成本为无穷大，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点C2作为待扩展边的一端，由于树节点列表与节点C2的圆邻域交集中不存在节点可以作为待扩展边的另一端，因此不存在待扩展边．如图2(d)所示，考虑空树边优先队列返回的最小估计成本为无穷大，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点A2作为待扩展边的一端，将采样节点列表中位于节点A2圆邻域内的节点C3和C4作为待扩展边的另一端，并且将可以改善当前路径成本的待扩展边A2C3和A2C4插入到树边优先队列中．考虑待扩展边A2C3和A2C4未插入到树边列表中，将节点C3和C4的g值设置为无穷大．考虑树节点优先队列中最优节点A3的估计成本大于树边优先队列中最优待扩展边A2C3的估计成本，激活模块3来从树边优先队列中取出最优待扩展边A2C3，将节点C3从采样节点列表中删除，并插入到树节点列表中和树节点优先队列中，将待扩展边A2C3插入到树边列表中，正式成为树的一部分．考虑树节点优先队列中最优节点C3的估计成本小于树边优先队列中最优待扩展边A2C4的估计成本，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点C3作为待扩展边的一端，将树节点列表中位于节点C3圆邻域内的节点A4作为待扩展边的另一端，并且将可以改善当前路径成本的待扩展边C3A4插入到树边优先队列中．同时，采样节点列表中位于节点C3圆邻域内的节点C4也可以作为待扩展边的另一端，将待扩展边C3C4插入到树边优先队列中．考虑树节点优先队列中最优节点A3的估计成本大于树边优先队列中最优待扩展边A2C4的估计成本，激活模块3来从树边优先队列中取出最优待扩展边A2C4，将节点C4从采样节点列表中删除，并插入到树节点列表中和树节点优先队列中，将待扩展边A2C4插入到树边列表中，正式成为树的一部分．考虑以节点C4为终端点的待扩展边C3C4估计成本大于节点C4的g值，将其从树边优先队列中删除．考虑树节点优先队列中最优节点C4的估计成本小于树边优先队列中最优待扩展边C3A4的估计成本，激活模块2的扩展操作，从树节点优先队列中取出估计成本最小的节点C4作为待扩展边的一端，将树节点列表中位于节点C4圆邻域内的节点A4作为待扩展边的另一端，并且将可以改善当前路径成本的待扩展边C4A4插入到树边优先队列中．如图2(e)所示，考虑树节点优先队列中最优节点A3的估计成本大于树边优先队列中最优待扩展边C3A4的估计成本，激活模块3来从树边优先队列中取出最优待扩展边C3A4，将以节点A4为终端点的边A3A4从树边列表中删除，将待扩展边C3A4插入到树边列表中，正式成为树的一部分，并且将以节点A4为终端点的待扩展边C4A4从树边优先队列中删除．至此，原始路径片段“节点A2→节点A3→节点A4”被优化为成本更低的路径片段“节点A2→节点C3→节点A4”，并且在树边列表中插入具有改善路径成本能力的边A1C1，A1C2和A2C4，BIT*算法不断迭代运行实现全局路径持续优化．BIT*算法运行过程须要反复执行树节点优先队列和树边优先队列插入操作和提取操作，为了提高BIT*算法搜索效率，采用数组实现树节点优先队列和树边优先队列，使得插入操作和提取操作最坏情形运行时间均为O(logN)[13]．2 局部路径规划自主代客泊车系统局部路径规划的任务是规划出满足汽车运动学约束的无碰撞路径．如图3所示，汽车矩形包络ABCD的顶点B不在障碍物矩形包络EFGH内，约束条件可等价为如下不等式约束条件SΔBEH+SΔBHG+SΔBGF+SΔBFESEFGH，(1)式中SΔBEH，SΔBHG，SΔBGF，SΔBFE和SEFGH分别为三角形ΔBEH，ΔBHG，ΔBGF，ΔBFE和障碍物矩形包络EFGH的面积．由式(1)描述的等价条件可知，汽车矩形包络ABCD与障碍物矩形包络EFGH无碰撞约束条件可等价为：SΔAEH+SΔAHG+SΔAGF+SΔAFESEFGH;SΔBEH+SΔBHG+SΔBGF+SΔBFESEFGH;SΔCEH+SΔCHG+SΔCGF+SΔCFESEFGH;SΔDEH+SΔDHG+SΔDGF+SΔDFESEFGH;SΔEAB+SΔEBC+SΔECD+SΔEDASABCD;SΔFAB+SΔFBC+SΔFCD+SΔFDASABCD;SΔGAB+SΔGBC+SΔGCD+SΔGDASABCD;SΔHAB+SΔHBC+SΔHCD+SΔHDASABCD. (2)如图3所示，已知汽车轴距、宽度、前悬和后悬长度分别为L，W，Lf和Lr，则采用汽车后轴中点坐标(xr,yr)将汽车矩形包络ABCD的顶点坐标表示为：        A=(Ax,Ay) =(xr+(L+Lf)cos φ-(W/2)sin φ,yr+(L+Lf)sin φ+(W/2)cos φ);        B=(Bx,By)=(xr+(L+Lf)cos φ+(W/2)sin φ,yr+(L+Lf)sin φ-(W/2)cos φ);C=(Cx,Cy)=(xr-Lfcos φ+(W/2)sin φ,yr-Lfsin φ-(W/2)cos φ);D=(Dx,Dy)=(xr-Lfcos φ-(W/2)sin φ,yr-Lfsin φ+(W/2)cos φ). (3)由式(3)可知，式(2)描述的约束条件可转化为汽车后轴中点坐标约束，进而获得汽车无碰撞约束等价条件．下面建立运动学约束等价条件，已知三个相邻路径点构成的向量V1和V2，则向量V1和V2的夹角可表示为θ=arccosV1⋅V2/V1V2．(4)当向量V1和V2不平行时，汽车运动学约束条件等价于与向量V1和V2相切的圆弧曲率约束ρ=tanθ/2/minV1,V2≤ρmax，(5)式中ρmax为汽车最小转弯半径决定的最大曲率．综合考虑式(2)描述的汽车无碰撞约束等价条件和式(5)描述的汽车运动学约束等价条件，基于滚动优化思想将自主代客泊车系统局部路径规划问题转化成约束优化问题．如图4所示，每次滚动求解约束优化问题时，仅优化三个连续的路径片段．10.13245/j.hust.210914.F003图4滚动优化思想如图4所示，已知约束优化问题在当前求解周期内得到的最优解为路径片段NP，PQ和QW，将路径片段NP作为当前求解周期内局部最优路径．在约束优化问题的下一个求解周期内，将连续的三个全局路径片段N*P*，P*Q*和Q*W*作为约束优化问题的初始值，将路径片段NP与待优化路径片段N*P*相等作为约束优化问题相邻求解周期解连续性约束条件，由此得到约束优化问题表达形式为：J=minV3=minV2-V1;s.t.式(2)描述的汽车运动学约束,式(5)描述的汽车避障约束,解连续性约束条件, (6)式中J为约束优化问题的优化目标，即最小化三个相邻路径点构成的向量V1和V2的变化量．当汽车沿着基于滚动优化思想规划的局部自主代客泊车路径行驶到泊车位附近时，基于圆弧-直线组合方法规划局部自主代客泊车路径，使汽车安全驶入泊车位[15]．如图5所示，已知泊车起始点和目标点的坐标分别为(xG1,yG1)和(xG3,yG3)，则有G2G3sin θ2+R1(1-cos θ2)=yG3-yG1*;G2G3cos θ2+R1sin θ2=xG3-xG1*, (7)式中：θ2为泊车位倾斜角度；R1为圆弧半径；(xG1*,yG1*)为虚拟泊车起始点坐标，可表示为(xG1*,yG1*)=(xG1-R1sin θ1,yG1+R1(1-cos θ1)), (8)式中θ1为泊车起始角度．由式(7)和式(8)可得圆弧半径表达式为R1=(xG3-xG1)sin θ2-(yG3-yG1)cos θ21+cos(θ1+θ2)-2cos θ2．(9)基于泊车起始点坐标和圆弧半径可得圆弧段和直线段的交点坐标为(xG2,yG2)=(xG1+R1(sin θ2-sin θ1),yG1+R1(cos θ2-cos θ1)).(10)至此，规划出由圆弧段G1G2̑和直线段G2G3组成的泊车路径．10.13245/j.hust.210914.F004图5泊车路径规划3 仿真分析在VC++6.0环境中仿真验证所提出的方法．仿真过程中，汽车轴距、宽度、前悬和后悬长度分别设置为2.405，1.523，0.800和0.950 m，并且为了避免车辆轮廓对局部自主代客泊车路径规划过程的影响，在全局自主代客泊车路径规划过程中对障碍物进行了膨胀处理．仿真结果如图6所示，蓝色区域为障碍物，黄色区域为障碍物膨胀区域．如图6(a)~(c)所示，BIT*算法得到的初始全局路径长度和优化后的全局路径长度分别为63.394和59.478 m，RRT算法得到的全局路径长度为90.103 m，说明BIT*算法得到的全局路径更优，并且优化过程仅在约束椭圆区域内采样，避免了无效采样点降低算法的搜索效率．如图6(d)~(e)所示，基于滚动优化思想和圆弧-直线组合方法规划出的局部自主代客泊车路径满足汽车运动学约束，并且基于滚动优化思想规划出的局部自主代客泊车路径可以将汽车引导到泊车位附近，而基于圆弧-直线组合方法规划出的局部自主代客泊车路径可以使汽车安全驶入泊车位．10.13245/j.hust.210914.F005图6自主代客泊车路径规划结果4 结语仿真验证了所提出的路径规划方法的可行性和有效性，结果表明：相对于基于传统RRT算法的自主代客泊车路径规划方法，本文方法搜索效率更高，并且规划出的自主代客泊车路径满足汽车运动学约束和避障约束．