基于分层架构的自主代客泊车路径规划

张家旭 王志伟 郭 崇 赵 健 周时莹

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

高级驾驶辅助技术的推广应用提高了汽车行驶的安全性、舒适性和经济性,泊车辅助系统作为高级驾驶辅助技术的重要组成部分,已有近30年研究历史[1],其意在帮助驾驶员解决棘手的泊车难问题.相对于传统的泊车辅助系统,自主代客泊车系统能够代替驾驶员自主寻找车位并停放汽车,具有更高的自动化程度,彻底解决汽车最后一公里自动驾驶问题[2].

路径规划是自主代客泊车系统的重要环节,目前最具代表性的路径规划方法有基于优化的方法、基于采样的方法和基于图搜索的方法.文献[3]将自主代客泊车路径规划问题转化为非线性规划问题,并将客观数据作为初始解来加快非线性规划问题的求解效率.文献[4]综合考虑汽车运动学约束、机械约束和碰撞约束等,将泊车路径规划转化为最优化问题,并利用同步动态优化方法求解该最优优化问题.文献[5]采用自适应伪谱法求解泊车路径规划问题,相对于传统伪普法具有更快的收敛速度.基于优化的路径规划方法得到的路径满足汽车运动学、汽车动力学和碰撞约束,并且具有较好的可跟随性,但优化问题求解过程复杂,对计算性能要求较高,难以工程应用.

相对于基于优化的方法,基于采样的方法以快速扩展随机树算法为代表,有一定的随机性,具有迭代快、计算量小等特点,而基于图搜索的方法以A*算法为代表,通过设置不同的启发函数和节点扩展方式,可实现全局最优或近似最优.文献[6]提出利用停车场内摄像头识别空车位,再利用Dijkstra算法规划全局自主代客泊车路径,并通过平行换道策略躲避停放不标准的车辆.文献[7]首先采用混合A*算法规划出满足汽车运动学约束的全局自主代客泊车路径,随后利用共轭梯度方法对全局自主代客泊车路径的曲率、与障碍物距离等因素进行优化.文献[8]改进了混合A*算法的节点扩展方式和代价函数,并利用杜宾斯曲线直接连接自主代客泊车路径的当前节点与目标点的方式来提高路径规划的效率.文献[9]从目标偏向性、转角约束和路径平滑性3个角度对传统快速扩展随机树算法进行优化改进,提高了快速扩展随机树算法对自主代客泊车路径的规划效率,并使规划出的路径具有更好的可跟随性.文献[10]将最优泊车位姿融入到混合A*算法的启发值中来优化全局路径搜索方向,减少时间消耗.文献[11]利用可行驶区域场构建快速扩展随机树算法的概率密度函数来优化随机树的生长方向,并利用B样条曲线平滑规划出的路径.上述2类方法生成的路径一般难以满足汽车运动学约束,通常作为全局路径应用.

综上分析可知,单一的路径规划方法难以应对复杂的自主代客泊车工况.鉴于此,本文提出一种基于分层架构的自主代客泊车路径规划算法.首先,利用栅格扫描算法快速准确地将自主代客泊车环境地图转化成为Voronoi图,量化自主代客泊车环境中任意栅格格网区域与其最近障碍物距离.随后,利用A*算法规划出全局自主代客泊车路径,并采用优先队列数据实现A*算法的开放列表来提升其计算效率.最后,基于自主代客泊车环境Voronoi图实现汽车碰撞检测,并采用改进动态窗口法沿着全局自主代客泊车路径规划出满足汽车非完整约束和机械约束的无碰撞路径,扩展传统动态窗口法的可行解空间和降低其保守性.

1 环境地图构建

准确高效获取环境地图信息是快速规划出可执行的自主代客泊车路径的重要基础.本节利用Voronoi图描述自主代客泊车环境地图信息,并采用具有离散计算特征的栅格扫描算法生成Voronoi图,快速准确地量化自主代客泊车环境中任意栅格格网区域与其最近障碍物距离.如图1所示,采用q1,q2,…,q8表示栅格格网p的8个邻近格网,由这8个邻近格网构成栅格格网p的邻域模板.栅格扫描算法利用此模板对所有栅格格网分别进行从左到右的逐行正向扫描,从右到左逐行反向扫描,从上到下逐列正向扫描和从下到上逐列反向扫描来生成Voronoi图[12].

图1 栅格格网p的邻域模板

栅格格网p的8个邻近格网的相对坐标为C(q)=(Cx(q),Cy(q)),q∈{q1,q2,…,q8},则栅格格网p与8个邻近格网之间的平方欧氏距离偏差d(p,q)和相对坐标偏差M(p,q)可分别表示为[13]

d(p,q)=

(1)

(2)

定义从左到右逐行正向扫描集合为N1(p)={q1,q2,q3,q4},从右到左逐行反向扫描集合为N2(p)={q5,q6,q7,q8},从上到下逐列正向扫描集合为N3(p)={q3,q2,q1,q8},从下到上逐列反向扫描集合为N4(p)={q7,q6,q5,q4}.采用f(q),q∈{q1,q2,…,q8}表示栅格格网p的8个邻近格网的平方欧氏距离,则栅格扫描算法计算任意栅格格网p的平方欧氏距离f(p)和相对坐标C(p)的具体流程如下:

① 从左到右逐行正向扫描栅格格网p.设置f(p)=∞,对于任意的邻近格网q∈N1(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

② 从右到左逐行反向扫描栅格格网p.对于任意的邻近格网q∈N2(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

③ 从上到下逐列正向扫描栅格格网p.对于任意的邻近格网q∈N3(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

④ 从下到上逐列反向扫描栅格格网p.对于任意的邻近格网q∈N4(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

2 全局路径规划

全局路径规划的任务是根据环境地图信息规划出从起始点到目标点的无碰撞全局自主代客泊车路径.本节采用A*算法规划全局自主代客泊车路径.利用欧氏距离计算公式定义A*算法中从起始点到当前节点的实际代价G,从当前节点到目标点的预估代价H,以及当前节点的总代价F=G+H,则A*算法通过对保存待检测节点的开放列表反复执行插入节点和提取最小节点操作,并采用封闭列表保存已检测的节点,实现全局自主代客泊车路径的规划,其具体流程如下:

① 将起始点加入开放列表.

② 重复如下过程直到目标点加入开放列表或开放列表为空.

③ 将开放列表中最小F值节点取出作为当前节点,将其移至封闭列表.

④ 检查当前节点的8个邻近节点,若邻近节点平方欧氏距离小于给定阈值或已在封闭列表中,不操作;若邻近节点不在开放列表中,将其加入开放列表,将当前节点作为其父节点;若邻近节点在开放列表中且当前节点能减小邻近节点G值,更新邻近节点G值和F值,将当前节点作为其父节点.

开放列表插入节点操作和提取最小节点操作的运行时间是影响A*算法搜索效率的重要因素.本节采用插入节点操作和提取最小节点操作最坏情形运行时间均为O(logN)的优先队列数据结构实现A*算法中的开放列表.同时,为了获取A*算法规划的全局自主代客泊车路径,需要从目标点开始,沿着每个节点的父节点移动直到起始点.因此,为了便于快速高效地从起始点开始访问规划出的全局自主代客泊车路径,本节采用具有后进先出特征的栈数据结构保存全局自主代客泊车路径.

3 局部路径规划

局部路径规划的任务是沿着全局自主代客泊车路径规划出满足汽车非完整约束和机械约束的无碰撞路径.本节采用改进动态窗口法规划局部自主代客泊车路径.传统的动态窗口法采用精度较低的欧拉法离散化汽车运动学方程,并且将第1个计算步长内控制量允许的变化量作为整个预测路径过程的约束条件,限制了可行解空间[14].为此,本节采用精度更高的四阶龙格库塔积分法离散化汽车运动学方程,并在汽车预测路径计算过程中持续更新控制量,扩大可行解空间.同时,本节采用3个相同半径的圆表示汽车外轮廓,并借助Voronoi图栅格格网的平方欧氏距离实现汽车碰撞快速检测,避免了传统动态窗口法将汽车简化为质点来检测碰撞而产生的较大保守性.

汽车自主代客泊车过程中处于低速大转角行驶状态,可认为4个车轮无侧偏地绕同一瞬时圆心作圆周运动.如图2所示,将汽车后轴中点横坐标xr、纵坐标yr和汽车横摆角φ作为系统状态量x=[xryrφ]T,将汽车后轴中点速度vr和汽车前轮转向角δf作为系统控制量u=[vrδf]T,由此建立汽车运动学方程为

图2 汽车运动学模型

(3)

式中,L为汽车轴距.

利用四阶龙格库塔积分法将式(3)离散化为[15]

(4)

式中,Δt为计算步长;K1、K2、K3和K4为四阶龙格库塔积分法的系数,可表示为

(5)

若k时刻汽车后轴中点速度和汽车前轮转向角的设定值分别为vr,set∈[Vmin,Vmax]和δf,set∈[-δmax,δmax],综合考虑k-1时刻汽车后轴中点速度和汽车前轮转向角实际值与汽车机械约束,可得

vr(k)=

(6)

δf(k)=

(7)

式中,amax和ωmax分别为汽车后轴中点加速度最大值和汽车前轮转向角速度最大值.

为了提高局部自主代客泊车路径规划过程中汽车碰撞检测效率,采用3个相同半径的圆包含汽车外轮廓,通过3个圆圆心所在Voronoi图栅格格网的平方欧氏距离实现汽车碰撞快速检测.如图3所示,已知汽车的长度、宽度和后悬长度分别为Lc、W和Lr,则3个圆半径R和k时刻圆心坐标可分别表示为

图3 汽车圆包络

(8)

(9)

(10)

(11)

式中,T为旋转变换矩阵,可表示为

(12)

因此,若k时刻3个圆圆心所在Voronoi图栅格格网的平方欧氏距离大于R2,则汽车在k时刻与障碍物不相交.

在汽车后轴中点速度可行空间[Vmin,Vmax]和汽车前轮转向角可行空间[-δmax,δmax]均匀采样,得到k时刻一簇汽车后轴中点速度和汽车前轮转向角的设定值,利用式(3)～(7)预测k时刻到k+N时刻的汽车行驶轨迹.为了实现局部自主代客泊车路径规划,本节设计如下评价函数从无碰撞的汽车预测轨迹中筛选出最优轨迹:

J(vr,set,δf,set)=αJ1(vr,set,δf,set)+βJ2(vr,set,δf,set)

(13)

式中,J1(vr,set,δf,set)为汽车k+N时刻横摆角与全局路径切线方向之间的角度偏差;J2(vr,set,δf,set)为汽车预测轨迹与障碍物之间最近距离;α和β为加权系数.

在采用改进动态窗口法规划出局部自主代客泊车路径来引导汽车到达泊车位附近后,再利用文献[5]提出的方法规划泊车路径,引导汽车进入泊车位,进而实现汽车自主代客泊车功能.

4 仿真分析

本节在VC++6.0编辑和编译环境中实现自主代客泊车路径规划算法.针对图4(a)所示的停车场原始地图,所提出的自主代客泊车路径规划算法参数设置为:Δt=0.1 s,Lc=4.155 m,L=2.405 m,Lf=0.800 m,Lr=0.950 m,W=1.645 m,δmax=0.53 rad,Vmin=1 m/s,Vmax=3 m/s,amax=2 m/s2,ωmax=0.53 rad/s,栅格格网距离为0.5 m,泊车位长度和宽度分别为4.5和2.5 m.仿真计算结果如图4(b)～(d)所示.

(a) 停车场原始地图

图4(b)为栅格扫描算法生成的停车场Voronoi图,图4(c)为A*算法规划的全局自主代客泊车路径,图4(d)为改进动态窗口法规划的以圆形为起始点、五角星为目标点的局部自主代客泊车路径和文献[5]方法规划的连接2个五角星的垂直泊车路径.如图4(b)所示,基于栅格扫描算法将停车场原始地图转换为包含势能场信息的停车场Voronoi图,利用该信息可以更易判断车辆与障碍物之间的距离,为解决自动代客泊车路径规划的避障问题奠定了基础.如图4(c)所示,本文将平方欧氏距离小于一定阈值的栅格格网考虑成不可行区域,使得规划的全局自主代客泊车路径与障碍物保持一定的安全距离,提升自主代客泊车过程的安全性,并且便于局部路径规划过程中障碍物处理.如图4(d)所示,采用改进动态窗口法规划的局部自主代客泊车路径满足汽车运动学约束和避障约束,可以安全地引导汽车到达目标泊车位附近.当汽车到达目标泊车位附近时,采用文献[5]提出的泊车路径方法规划垂直泊车路径,引导汽车进入泊车位,进而实现汽车自主代客泊车功能.

5 结论

1) 基于分层架构提出了一种自主代客泊车路径规划算法.采用Voronoi图描述自主代客泊车环境地图信息,量化自主代客泊车环境中任意栅格格网区域与其最近障碍物距离.基于自主代客泊车环境Voronoi图,利用A*算法规划出了全局自主代客泊车路径,并采用优先队列数据实现A*算法的开放列表来提升其计算效率.

2) 在全局自主代客泊车路径的基础上,采用改进动态窗口法规划出了满足汽车非完整约束和机械约束的无碰撞路径,有效地扩展了传统动态窗口法的可行解空间,并且降低了其保守性.

3) 在VC++6.0环境中验证所提出的自主代客泊车路径规划算法的可行性和有效性,结果表明，所提出的自主代客泊车路径规划算法可以安全、快速地引导汽车到达目标泊车位附近,为汽车后续执行泊车操作奠定基础.