基于库位跟踪的自动泊车决策规划系统

熊璐 严森炜 余卓平 张培志

Automobile Technology - - CONTENTS -

【摘要】针对自动泊车算法对自车初始位姿要求较高的问题,设计了一套自动泊车决策规划算法,对自车初始位姿无要求。设计了库位跟踪算法,在泊车过程中不断更新库位坐标,并以此在关键点进行后续规划,保证最终泊车姿态理想;针对平行库位长度不满足一次规划入库最短需求及垂直泊车中停车通道宽度不足的情况分别提出了多次规划算法和动态调整算法。通过Simulink 和 PreScan联合仿真及实车试验进行了验证,证明了上述算法的有效性。

主题词:自动泊车 库位跟踪 多次规划 动态调整 卡尔曼滤波U461 A 10.19620/j.cnki.1000-3703.20180456中图分类号: 文献标识码: DOI: Decision-Making and Planning System of Autonomous Parking Based on Closed-Loop Update of Parking Spot

Xiong Lu, Yan Senwei, Yu Zhuoping, Zhang Peizhi Tongji University, Shanghai 201804) ( Abstract An autonomous parking system was designed that had no requirement on vehicle's initial position. Parking【 】spot tracking algorithm was designed, which could continuously update coordinate of parking spot, and make subsequent planning at critical points, to ensure an ideal parking position. In case the actual length of parallel spot does not satisfy the minimum required length of planning path, and parking channel has insufficient width in vertical parking, the multiplanning algorithm and dynamic adjustment algorithm were proposed. These above algorithms were validated in Simulink and PreScan simulations as well as real vehicle tests.

Key words: Autonomous parking, Closed- loop update of parking spot, Multi- planning, Dynamic adjustment, Kalman filter 1 前言

自动泊车技术已经问世多年,但目前仍不能较好地解决如下问题:对不同库位环境和初始位姿的普适性、算法良好的实时性、算法的可靠性和可控性、最后停车

4的理想位姿。自动泊车算法主要分为 种:一是基于路径规划的控制算法[1- 6],即根据环境约束规划几何曲线(包括搜索采样算法),然后通过路径跟踪和底盘控制决定车辆的动作,优点是规划运算耗时较短、规划路径基本可控,缺点是路径跟踪和底盘控制存在误差,导致最后泊车位姿不理想;二是基于经验的控制算法[7-11],即利用模糊逻辑或神经网络,根据实时的环境输入得到航向角和车速的控制量,优点是闭环控制,缺点是控制参数难以确定、泊车路线不可控、实车应用效果不佳;三是基 于数值优化的算法[12- 13],通过建立系统运动学方程和结合参数约束进行路径最优化求解,优点是对不同环境适用性广,缺点是计算量大、实时性差;四是基于学习的算法[14- 16],利用行为克隆或强化学习进行从环境到车辆纵、侧向控制的映射,优点是可以将各模块的误差直接融合在算法中消除,缺点是学习算法训练难以收敛,容易陷入局部最优值,泛化能力一般。

Reeds-Shepp本文基于几何规划,结合 曲线路径最短[2]和螺旋线适用性广的优点设计了对自车初始位姿无要求的自动泊车决策规划算法。通过融合环视库位坐标信息和车辆底盘运动学信息对目标库位进行跟踪,同时对航位推算结果进行修正。对于路径跟踪和底盘执行器控制带来的误差,通过库位跟踪和多次规划的方法予以减小。此外,设计了多次规划和动态调整算法解决

泊车过程中由于空间狭窄难以入库的问题。本文设计的算法对各类库位的适应性强、实时性好、可靠性强,并且大幅改善了最后泊车姿态的理想程度。上述算法通过仿真和实车试验进行了验证。

2 方案设计2.1 自动泊车系统架构

1

自动泊车系统架构如图 所示。环境感知模块利用环视相机给出库位坐标,结合超声波雷达检测障碍物,将探测到的环境信息送到决策规划模块用以决策和规划泊车路径。运动控制模块中,底盘执行机构包括电EPS)

动助力转向系统( 以及驱动、制动执行器。

2.2 决策规划系统流程

自动泊车系统的目标功能为:驾驶员将车辆驶入停车场或沿停车位行驶时,启动自动泊车功能后,自车接管车辆控制,前进过程中自动判别库位类型,实现自动泊车。在泊车过程中,自车通过多次更新库位坐标减小库位坐标检测误差和航位推算累积误差,使泊车姿态更理想。对于平行泊车,设计了多次规划模块,即库位长度无法满足一次倒车入库时,自动增加调整次数,直至能够泊入车位,理论上只需车位长度大于车长即可入库;对于垂直泊车,停车通道宽度不足时,进行动态调整,

2使泊车规划不受障碍物的限制。简要流程如图 所示。

2.3 车辆模型

本文使用阿克曼转向性质的车辆模型。将车辆视

3

为平面刚体,具有 个自由度,选取后轴中心作为参考

( x,y),点,车辆在世界坐标系下的坐标为 航向角为φ,则

3 3车辆位置唯一确定,如图 所示。图 中, γ为转向轮转角,、rκ分别为车辆转弯半径和曲率, L为轴距。车辆的状态方程为:

3 基础路径规划 3.1 平行泊车

自车起始时(首次获得环视系统得到的库位角点P0、P1时,库位宽度通过超声波雷达测算或按国标推算),后轴中点位于S点,将S点设为规划的原点,建立的4坐标系和基础路径规划结果如图 所示。车辆沿X轴方向前进,到达某点A时,以最小转弯半径Rmin倒车,再以 Rmin为半径反向倒车,后轴中心点轨迹分别为AB 和 BC ,此时车身与库位平行,然后前进至库位中心位置。

实际泊车过程中,库位长度可能不满足一次规划的8) 9)

要求,即式( 、式( 不成立。参考人类驾驶员的泊车 经验,本文提出多次规划算法,即降低一次规划的目标,使车身部分停入库位,再通过不断规划调整,最终停入

6,

库位中心。如图 一次规划计算发现会发生碰撞,因此逐渐增加目标终点从库位中心向外侧的偏移值,直至能够满足无碰撞条件。当行驶完一次规划路径后,根据

S′A′B′C′D′,

与库位的相对位置再次规划路径 直至自车停在库位中心。库位长度越短,单次调整纵向距离受限,因此横向距离变化也越小,所需调整次数越多。

3.2 垂直泊车

自车起始时(首次获得环视系统得到的库位角点P0、P1坐标时),后轴中点位于S点,将S点设为规划原7

点,如图 所示。车辆沿X轴方向前进,到达某点A时,以Rmin为半径倒车,车辆后轴中心点轨迹为AB 。

16)

反之,则需多次规划入库。当式( 取等号时停车,车辆后轴中心位于点B,根据此时自车相对库位的位姿规划后续路径。为便于表述,将坐标系原点重设为

9泊车目标点E,如图 所示。E点的X坐标为:

在垂直泊车工况中,常遇到泊车通道宽度不足的问题,如较窄的停车通道或泊车规划路径上存在墙或其他车辆,导致自车无法按照原规划路径行驶,此时需要进

10

行如图 所示的动态调整。自车按原规划路径行驶过程中,车前方超声波雷达探测到障碍物距离小于某一阈值,则进行动态调整,将转向盘转至右极限位置,路径为  C1B2 ,到达B2点后按1C1 和C1B2 的规划方法,规划2C2 和C2D ,按规划路径泊车入库。若仍检测到障碍物距离小于安全阈值,则重复动态调整,直至顺利入库。

4 垂直泊车规划算法优化

在实车试验中发现,由于垂直泊车的侧向空间小,对路径跟踪、底盘执行器控制和航位推算误差敏感,因此考虑在C点增加一次库位跟踪规划,即在C点根据自车和库位的相对位姿进行重新规划。

为了继承基础路径规划的思路,将CD段(此时基础路径规划的BC段和原CD段圆弧半径稍大于Rmin)路径

3 11

调整为 段曲线,如图 所示,中间段仍采用圆弧曲线,前后两段采用二次螺旋线便于位姿调整。二次螺旋线的表达式为: 1

设第 段螺旋线的长度为s1,终点处的车辆位姿为X1=( X1,Y1,θ1), ( XR,YR),

中间段圆弧的圆心坐标是 圆弧结束

X2=( X2,Y2,θ2), 2

点处车辆位姿为 第 段螺旋线的长度为s2。设两组螺旋线方程的未知参数分别为( a1,b1,c1)

( a2,b2,c2) X0=( X0,Y0,θ0)=(Xi,Yi,θi),

。初始点位姿为 终点位姿

X3=( X3,Y3,θ3)=(Xg,Yg,θg),κ0

为 、κ3分别为起点和终点的曲

式中, sg为积分区间的右边界。

MIDACO

考虑到初值的选择对于 求解有影响,因此

MIDACO

考虑用成功生成的算例来训练神经网络,作为

( Xi,Yi,θi,ki) ( Xg,Yg,θg,kg),

求解的初值。流程如下:首先已知 和

MIDACO

通过 求解待求参数a1、s1、a2、(在现有设备上s

1.2 s),

的平均计算周期为 用求解参数和初始条件生成曲线,二次验证筛选掉可能发生碰撞的结果,使用筛选

BP 5

后的数据集训练 神经网络,含 层隐藏层,神经元数

200 100 50 20 10, Leaky ReLU,

分别为 、 、 、 、 激活函数选择

Mini- Batch

输入、输出均归一化参与训练,选择 和

AdaGrad 4

作为优化方法。最终训练结果中 个参数的平

10%

均误差约 。

4

使用训练好的神经网络输出的 个参数作为

MIDACO 0.8 s 12

求解的初值,平均计算周期降低为 。图

30°~90°(

所示为选取的一定区域内,初始航向角 为了显

示清晰每 采样一次)的路径库。 MIDACO在实车试验中若出现 求解的结果不符合( Xg,Yg,θg,kg) MIDACO要求的情况,可微调目标点 再次运行求解。若多次微调目标点的结果均不符合要求,则进行自车位姿微调后根据新的相对位姿进行求解。

5 库位跟踪

因传统自动泊车规划算法采用航位推算来估计自车的位姿,其使用的惯性导航系统或车辆底盘信息存在噪声,同时,泊车工况的前进、倒车切换频繁,导致实际泊车中的航位推算累积误差达到了分米级以上,这对于泊车工况来说是难以接受的。环视系统的标定、拼接、光照、阴影、遮挡等具有随机性,此外,在检测库位时车辆非静止而影像系统帧数有限,导致测量误差的存在。

将航位推算信息和环视信息融合能够有效地解决上述问题。因方程非线性,采用拓展卡尔曼滤波算法。世界坐标系XwOwYw、车辆坐标系XvOvYv和状态量的定义

13

如图 所示。

2

对于垂直泊车,在同一初始位姿进行 次试验。首

1

先通过测量得到库位相对自车初始位置的真值。第次不使用库位跟踪,在首次收到库位坐标后仅用航位推算更新自车相对库位的位姿,以此为基础进行规

2

划。第 次使用库位跟踪,用卡尔曼滤波实时更新自车的相对位姿,以此为基础进行规划,同时记录载波相位

Real-Time Kinematic,RTK)

差分( 定位结果。结果如图

23

所示,基于库位闭环的决策规划算法能够明显改善泊车位姿。

7 结束语

本文设计的自动泊车决策规划系统能够较大限度地发挥几何规划路径可靠、算法实时性好的优点,同时对自车初始位姿无要求。通过库位跟踪,能够有效降低航位推算和环视系统库位检测的误差,提高车辆与库位的相对定位精度。将库位跟踪和本文所设计的自动泊车决策规划算法结合起来,能够较大幅度提升自动泊车最终泊车位姿的理想程度。此外,对于自动泊车场景中常见的平行泊车长度不足或垂直泊车可行通道较窄的问题设计了多次规划和动态调整算法,能够有效地解决上述问题。

参考文献

[1] Vorobieva H, Glaser S, Minoiu- Enache N, et al. Automatic Parallel Parking with Geometric Continuous- Curvature Path Planning[C]// Intelligent Vehicles Symposium Proceedings. IEEE, 2014: 465-471. [2] Reeds J A, Shepp L A. Optimal Paths for a Car that Goes Both Forward and Backward[J]. Pacific J Math, 1990, 145 (2): 367-393. [3] Schwesinger U, Bürki M, Timpner J, et al. Automated Valet Parking and Charging for E- Mobility[C]// Intelligent Vehicles Symposium. IEEE, 2016: 157-164. [4] Han L, Do Q H, Mita S. Unified Path Planner for Parking an Autonomous Vehicle Based on RRT[C]// IEEE International Conference on Robotics and Automation. IEEE, 2011: 56225627.

[5] Scheuer A, Fraichard T. Continuous- Curvature Path Planning for Car- Like Vehicles[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2005: 997-1003. [6] Fraichard T, Scheuer A. From Reeds and Shepp's to Continuous- Curvature Paths[J]. IEEE Transactions on Robotics, 2004, 20(6): 1025-1035. [7] Gorinevsky D, Kapitanovsky A, Goldenberg A. Neural Network Architecture for Trajectory Generation and Control of Automated Car Parking[J]. IEEE Transactions on Control Systems Technology, 1996, 4(1): 50-56. [8] Chiu C S, Lian K Y, Liu P. Fuzzy Gain Scheduling for Parallel Parking a Car- Like Robot[J]. IEEE Transactions on Control Systems Technology, 2005, 13(6): 1084-1092. [9] Yasunobu S, Murai Y. Parking Control Based on Predictive Fuzzy Control[C]// IEEE World Congress on Computational Intelligence. Proceedings of the Third IEEE Conference on Fuzzy Systems. IEEE, 2002: 1338-1341. [10] Lee C K, Lin C L, Shiu B M. Autonomous Vehicle Parking using Artificial Intelligent Approach[J]. Journal of Intelligent & Robotic Systems, 2009, 56(3): 319-343. [11] Li T H S, Chang S J. Autonomous Fuzzy Parking Control of a Car- Like Mobile Robot[J]. IEEE Transactions on Systems Man & Cybernetics Part A: Systems & Humans, 2003, 33(4): 451-465. [12] Zhang X, Liniger A, Borrelli F. Optimization- Based Colli⁃ sion Avoidance[DB/OL]. [2017- 11- 9]. https://arxiv.org/abs/ 1711.03449. [13] Li B, Shao Z. A Unified Motion Planning Method for Parking an Autonomous Vehicle in the Presence of Irregularly Placed Obstacles[J]. Knowledge- Based Systems, 2015, 86: 11-20. [14] Liu W, Li Z, Li L, et al. Parking Like a Human: A Direct Trajectory Planning Solution[J]. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(12): 33883397. [15] Vijay J, Swarn R. Vision and Dead Reckoning- based Endto- End Parking for Autonomous Vehicles[C]// Intelligent Vehicles Symposium. IEEE, 2018. [16] Nakamura H, Yafuso Y, Watanabe K, et al. A Study on Automatic Parking for Automobiles using Rational Policy Making Method[C]// IEEE Conference on Soft Computing in Industrial Applications. IEEE, 2008: 7-12.

Newspapers in Chinese (Simplified)

Newspapers from China

© PressReader. All rights reserved.