线控四轮转向汽车自主代客泊车路径规划

2022-05-09张家旭周时莹李长龙

中国机械工程 2022年8期

张家旭周时莹李长龙郭崇赵健

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

0 引言

随着V2X技术和自动驾驶技术的高速发展，自主代客泊车系统已成为解决最后一公里自动驾驶问题的有效手段。在已知停车场静态环境地图的基础上，快速高效地规划出面向动态障碍物场景的可行路径是自主代客泊车系统的核心技术之一，且已得到了许多学者和主机厂商的广泛关注[1-2]。

自主代客泊车系统现有的路径规划算法依据搜索空间类型可划分为基于连续状态空间的规划算法和基于离散状态空间的规划算法。

文献[3]将多个车辆自主代客泊车过程描述成约束优化问题，并采用内点法求解该约束优化问题，得到了最优的自主代客泊车路径。文献[4-5]以最小化自主代客泊车过程的能量消耗和所需时间为优化目标，基于优化理论规划自主代客泊车路径。文献[6]借助V2X技术和云计算技术，基于优化理论规划满足约束要求的最优自主代客泊车路径。上述基于连续状态空间的规划算法具有完备性优势，但算法的高计算复杂度限制了其工程应用。

基于离散状态空间的规划算法通过降低完备性要求来提高算法的计算效率和实用性。文献[7]采用具有解析度完备的Dijkstra算法规划自主代客泊车路径。文献[8]基于车辆位姿信息离散化搜索空间，采用混合A*算法规划满足汽车运动学约束的自主代客泊车路径。文献[9]利用虚拟势能场思想建立自主代客泊车环境地图，并采用概率完备的快速扩展随机树搜索算法规划自主代客泊车路径。文献[10]采用将A*算法和改进的动态窗口法相结合的方式规划自主代客泊车路径。当汽车沿着基于离散状态空间的规划算法得到的自主代客泊车路径行驶时，遇到动态障碍物则需要重新规划路径，而上述算法的路径重规划效率较低。

鉴于此，本文针对线控四轮转向汽车自主代客泊车路径规划问题，提出一种对动态障碍物具有自适应能力的自主代客泊车路径规划方法。首先，采用栅格地图描述停车场环境信息，将连续状态空间中的自主代客泊车路径规划问题转化为离散状态空间中的自主代客泊车路径规划问题，并采用各向同性的圆形结构元素对栅格地图进行膨胀处理，使规划的自主代客泊车路径与障碍物之间保持一定的安全距离。随后，采用D*Lite算法规划出对动态障碍物具有自适应能力的全局自主代客泊车路径，并采用动态窗口法和圆弧-直线组合方式沿着全局路径规划出满足线控四轮转向汽车运动学和机械约束的局部自主代客泊车路径，引导汽车安全无碰撞地进入斜向泊车位。最后，通过仿真对所提方法的可行性进行了验证。所提出的方法可以在动态障碍物存在的自主代客泊车场景中，规划出安全无碰撞的可行路径，引导汽车进入目标斜向泊车位。

1 环境地图构建

快速、准确地表征停车场环境信息是线控四轮转向汽车实现自主代客泊车路径规划的必要前提[11]。本节采用栅格地图描述停车场环境信息，将连续状态空间中的自主代客泊车路径规划问题转化为离散状态空间中的自主代客泊车路径规划问题，以降低路径规划算法的计算复杂度。为了使规划的自主代客泊车路径与障碍物之间保持一定的安全距离，本节采用各向同性的圆形结构元素对栅格地图进行膨胀处理，膨胀算法的伪代码如图1所示。

图1 膨胀算法的伪代码

如图1所示，行1为利用原始栅格地图初始化膨胀栅格地图；在利用行2和行3遍历原始栅格地图过程中，若当前原始栅格节点为障碍物节点，且当前原始栅格节点的上下左右四个邻居节点中至少有一个为可行节点时，则利用行5将膨胀栅格地图中圆形结构元素覆盖的栅格节点设置为障碍物节点。相对于传统的膨胀算法，该算法仅需将障碍物边界区域包含的栅格节点重新设置为障碍物节点，很大程度上减少了栅格节点的访问次数，降低了算法的计算复杂度。同时，在算法实现过程中，采用数组保存圆形结构元素中所有栅格节点相对于中心栅格节点(锚点)的偏移量，提高了圆形结构元素中所有栅格节点的有效性判断效率。

2 全局路径规划

全局路径规划的任务是规划出连接自主代客泊车起始点到目标点的无碰撞路径[12]。本节采用D*Lite算法规划线控四轮转向汽车自主代客泊车全局路径。采用c(M，N)表示节点M与节点N之间的当前代价，采用cold(M，N)表示节点M与节点N之间的历史代价，采用K值表示栅格估计量，采用g值表示任意节点与目标节点之间的历史代价，采用r值表示任意节点与目标节点之间的当前代价，则基于节点的g值和r值可将节点的状态划分为局部一致、局部过一致和局部欠一致。具体而言，若节点的g值等于r值，则节点的状态为局部一致；若节点的g值大于r值，则节点的状态为局部过一致；若节点的g值小于r值，则节点的状态为局部欠一致。D*Lite算法通过维护节点的g值和r值来实现动态障碍物场景的路径规划，包含三个主要模块的D*Lite算法的动态路径规划流程如图2所示[13]。

图2 基于D* Lite算法的动态路径规划流程

具体而言，D*Lite算法首先将自主代客泊车场景静态环境地图包含的所有节点的g值设置为极大值，将自主代客泊车场景静态环境地图包含的去除目标节点之外的所有节点的r值设置为极大值，随后采用从目标点向起始点反向搜索的方式来最小化各个节点的r值，进而将自主代客泊车场景静态环境地图转化为一个“静态路径场”，为汽车逐渐靠近目标点提供了择优依据。在已建立的“静态路径场”基础上，模块一可以实现汽车自主代客泊车过程中依据所在的当前节点信息贪婪地选择最佳下一个节点；模块二可以在汽车所在的当前节点的后继节点由可行节点变为障碍物节点或者由障碍物节点变为可行节点时，通过重新选择汽车所在的当前节点的最佳后继节点来更新当前节点的r值，进而将后继节点的变化情况传播到汽车所在的当前节点；模块三可以将汽车所在的当前节点的r值变化传播到其前继节点，优化当前节点及其前继节点到目标节点的代价。

图3为基于D*Lite算法的动态路径规划示意图。基于D*Lite算法建立的“静态路径场”可得到图3a所示的从起始节点13到目标节点8的最优路径为：节点13→节点10→节点7→节点8。如图3b所示，在汽车利用模块一贪婪地选择最优路径节点来逐渐靠近目标点的过程中，发现当前节点10的历史最佳节点7变为障碍物节点时，则激活模块二将当前节点10的最佳节点变为节点6，并且更新当前节点10的r值后将节点10插入到开放列表中。如图3c所示，从开放列表中取出节点10作为模块三的待处理节点，将节点10的g值设置为极大值，遍历节点10的前继节点，将节点10的前继节点13的历史最佳节点由节点10变为节点9，并且遍历节点10的后继节点来确定节点6是否依然为其历史最佳节点。如图3d所示，执行完模块三后再次激活模块一来将汽车从节点10移动到节点6，考虑到当前节点6的历史最佳节点7已经标记为障碍物节点，则再次激活模块一使汽车沿着路径“节点6→节点3→节点8”移动到目标节点8。

(a)相对位置1 (b)相对位置2

通过上述分析可知，D*Lite算法的核心是通过维护节点的g值和r值形成图4所示的以目标节点为稳定点的能量场，汽车沿着能量场的最速下降方向行驶，并且当能量场中障碍物信息发生变化时，通过快速修正能量场来规划出可行的动态路径。同时，D*Lite算法运行过程需要反复执行开放列表插入节点操作和提取最小节点操作，这两类操作的运行时间是影响D*Lite算法搜索效率的重要因素。为了提高D*Lite算法的搜索效率，本节采用优先队列数据结构构建D*Lite算法中的开放列表[14]。

(a)原始能量场

3 局部路径规划

局部路径规划的任务是在全局路径的基础上规划出满足线控四轮转向汽车运动学约束的无碰撞路径。本节采用动态窗口算法规划线控四轮转向汽车自主代客泊车局部路径[15]。如图5所示，已知线控四轮转向汽车的前轮转向角为δf、后轮转向角为δr、方位角为φ、轴距为L以及后轴中点横坐标、纵坐标和速度分别为xr、yr和vr，则线控四轮转向汽车运动学模型可描述为[16]

图5 线控四轮转向汽车运动学模型

(1)

若采用x=(xr，yr，φ)T表示系统状态向量，采用u=(vr,δf,δr)T表示系统控制向量，则利用四阶龙格库塔积分法可将线控四轮转向汽车运动学模型离散化为

(2)

其中，h为计算步长，k为离散时刻，K1、K2、K3和K4为四阶龙格库塔积分法系数，可表示为[17]

(3)

图6 栅格空间覆盖枚举方法原理

(4)

其中，T为旋转变换矩阵，可表示为

(5)

至此，利用式(4)和式(5)，并结合汽车后轴中点坐标可快速定位需要栅格空间覆盖枚举方法检测的栅格节点。

若汽车预测轨迹经栅格空间覆盖枚举方法检测后为无碰撞轨迹，则采用评价函数J从无碰撞轨迹中筛选出最优的汽车预测轨迹，并将最优的汽车预测轨迹片段作为当前时刻与下一时刻之间的线控四轮转向汽车自主代客泊车局部路径[18]，其表达式为

J=αJ1+βJ2

(6)

式中，J1、α分别为预测轨迹终端处汽车横摆角与全局路径目标点处切线方向之间的角度偏差惩罚项及加权系数；J2、β分别为汽车预测轨迹与障碍物之间最近距离惩罚项及加权系数。

当汽车位于斜向泊车位附近时，基于圆弧-直线组合方式规划连接泊车起始点和目标点的泊车路径[19]。如图7所示，已知泊车起始点的坐标为(xG1，yG1)，泊车目标点的坐标为(xG3，yG3)，斜向泊车位的倾斜角度为θ1，则有

图7 泊车路径规划

(7)

其中，R1为线控四轮转向汽车转弯半径，由式(7)可得其表达式为

(8)

由式(8)可得泊车路径包含的圆弧段和直线段的交点坐标为

(xG2，yG2)=(xG1-R1sinθ1，yG1-R1(1-cosθ1))

(9)

至此，已规划出由圆弧段和直线段组成的泊车路径。

4 仿真分析

图8 自主代客泊车场景静态环境地图

表1 仿真参数

在采用膨胀算法建立的图9a所示的自主代客泊车场景静态环境栅格地图基础上，基于D*Lite算法规划出的无碰撞原始全局路径如图9b所示。在汽车行驶过程中，当自主代客泊车场景地图中出现图9c中红色区域标识的动态障碍物时，再次激活膨胀算法可得到图9d所示的存在动态障碍物的自主代客泊车场景栅格地图，并且D*Lite算法仅需遍历图9e所示的障碍物周围较少的节点便可重规划出图9f所示的无碰撞全局路径，提高了全局路径重规划的效率。如图9g～图9i所示，在重规划的全局路径基础上，基于动态窗口法得到的局部路径可以安全无碰撞地引导汽车行驶到目标斜向泊车位附近，而基于圆弧-直线组合方式得到的泊车路径可以安全无碰撞地引导汽车进入目标斜向泊车位。

(a)自主代客泊车场景静态环境栅格地图 (b)规划的原始全局路径 (c)存在动态障碍物的自主代客泊车场景地图

5 硬件在环试验验证

本节基于图10所示的硬件在环试验平台对自主代客泊车路径规划方法的有效性进行验证。硬件在环试验平台由自主代客泊车系统感知模块、待验证的规划模块、控制模块和虚拟车辆组成。其中，自主代客泊车系统感知模块用于回放实车测试数据，待验证的规划模块基于感知回放数据规划自主代客泊车路径，控制模块基于规划路径与实际路径的偏差控制虚拟车辆完成自主代客泊车任务。