一种基于轨迹预判的垂直泊车路径规划算法研究

熊斌　钟益林　彭邦煌　白军明

【摘  要】为解决目前量产自动泊车系统泊车成功率低、泊车完成后车辆姿态偏斜等问题，研究搭建基于阿克曼转向几何学和车辆运动学的车辆运动模型，构建基于可行驶区域的栅格电子地图，在几何路径规划方法中融入基于轨迹预判的碰撞约束方法和路徑居中算法，采用车辆膨胀轮廓模型，最大程度利用电子地图可行驶区域和保证路径安全性，设计出一种基于轨迹预判的垂直泊车路径规划算法，并通过仿真测试和实车测试验证算法的可行性。该算法可大大提高泊车成功率，能帮助解决泊车完成后车辆偏斜不居中的问题。

【关键词】自动泊车；路径规划；几何法；轨迹预判

中图分类号：U469.72    文献标志码：A    文章编号：1003-8639（ 2023 ）09-0019-07

Research on Vertical Parking Path Planning Algorithm Based on Trajectory Prediction

XIONG Bin，ZHONG Yilin，PENG Banghuang，BAI Junming

（BYD Automotive Engineering Research Institute，Shenzhen 518118，China）

【Abstract】In order to solve the problems of the current mass-produced automatic parking system，such as the low parking success rate and the vehicle attitude skew after parking，a vehicle motion model based on Ackermann steering geometry and vehicle kinematics was built，and a network based on the drivable area was constructed. The grid electronic map integrates the collision constraint method based on trajectory prediction and the path centering algorithm into the geometric path planning method，and adopts the vehicle expansion contour model to maximize the use of the drivable area of the electronic map and ensure the safety of the path. Predictive vertical parking path planning algorithm. Simulation test and real vehicle test verify the feasibility of the algorithm，the algorithm greatly improves the success rate of parking，and solves the problem of vehicle deflection and non-centering after parking is completed.

【Key words】automatic parking；path planning；geometric method；trajectory prediction

作者简介

熊斌（1989—），主任控制算法工程师，主要从事新能源汽车整车智能控制算法开发工作；彭邦煌，男，软件测试开发部经理，研究方向为汽车算法研发。

随着电子元器件和现代控制理论方法快速发展，汽车自动化和智能化水平不断提升，汽车不仅仅是代步工具，用户对车辆安全性、自动化和科技感等要求也越来越高，各车企为提升市场竞争力，不断增加投入来提升汽车的智能自动化技术。

在国内，随着汽车数量不断增加，分配到每辆汽车的泊车空间越来越小，狭小的泊车位十分考验驾驶员的驾驶技术，泊车入库造成的交通事故在所有交通事故中的占比越来越大，因此自动泊车技术成为国内外学者重点研究对象。大众汽车在1992年首次在其IRVW概念车上搭载了自动泊车技术，但由于成本过高未量产；随着硬件成本降低，丰田汽车在2003年在普瑞斯车型上基于摄像头感知车位实现自动泊车，并实现量产；雪铁龙汽车在2007年毕加索车型上，通过6个测距传感器来感知车位实现自动泊车；2013年，奥迪汽车在CES上展示了代客泊车系统，即通过手机一键操作，车辆可以自主寻找车位并完成自动泊车；近十几年，一些国内外供应商如法雷奥、德赛西威、纵目科技、百度等纷纷推出自己的自动泊车系统，一些自主汽车品牌如长安、小鹏、吉利等都研制出自动泊车系统。

目前量产自动泊车系统存在许多问题，比如车位识别率较低、泊车成功率不高、揉库次数过多、泊车时间过长、泊车过程发生剐蹭、泊车轨迹不连续、泊车完成后车辆偏斜不居中等。路径规划模块是自动泊车系统的一部分，良好的路径规划算法不但能保证泊车路径轨迹安全性、可行性和连续性，还可以缩短泊车时间和实现泊车完成后车辆居中，因此研究路径规划算法具有重要意义。

1  自动泊车系统组成及工作原理

1.1  自动泊车系统介绍

自动泊车系统构成示意如图1所示，其系统主要由软件系统和硬件系统组成。其中，硬件系统包括环境感知传感器、自动泊车控制器、转向制动挡位执行器等；软件系统包括底层操作系统、环境感知模块、路径规划模块、车辆控制模块、泊车主程序、人机交互界面等。

自动泊车控制器是感知算法、路径规划算法和车辆控制算法等软件的运行载体，并将一些自动泊车信息传递给人机交互界面进行显示；环境感知传感器包括超声波雷达、毫米波雷达、摄像头、IMU等传感器，负责采集周围环境信息以构建自动泊车电子地图；执行器由电动转向系统、电液制动系统和挡位系统等组成，负责执行车辆控制模块的指令信息来进行转向、制动或挡位切换等操作。

环境感知模块通过感知定位算法可以获取车辆位姿信息和识别车位、障碍物等目标；路径规划模块根据感知模块提供的环境信息，规划出一条车辆可行驶、无碰撞的路径轨迹；车辆控制模块根据路径规划模块给出的路径轨迹，控制转向系统、制动系统和挡位系统等执行机构，控制车辆沿着规划的路径轨迹运动；泊车主程序用于控制自动泊车过程状态切换等，泊车过程状态包括待机、搜车位、泊车准备就绪、泊车中、泊车暂停、泊车完成等；人机交互界面主要用于驾驶员操作和信息显示作用，驾驶员操作包括自动泊车开启关闭、泊入车位选择、车头车尾泊入选择等，信息显示包括车辆位姿、车位信息、障碍物信息、路径轨迹信息等显示。

1.2  自动泊车工作原理

自动泊车类型（图2）按照车辆停放位置主要分为水平泊车、垂直泊车和斜列泊车3种方式。水平泊车方式车辆停放位置与道路基准线平行，垂直泊车方式车辆停放位置与道路基准线垂直，斜列泊车方式车辆停放位置与道路基准线呈非水平垂直的一定角度。

文章针对垂直泊车路径规划算法进行研究，以垂直泊车方式为例，阐述自动泊车工作原理，如图3、图4所示。泊车系统激活后，开始搜车位，感知模块检测到车位有效后，语音提示“找到车位，请踩刹车”，驾驶员踩下制动踏板后在人机交互界面选择车位，感知模块将车位信息、障碍物信息和车辆位姿信息等环境信息发送给路径规划模块，路径规划模块根据接收到的环境信息规划出泊车的路径轨迹（图3中的垂直泊车路径轨迹由直线M0M1、圆弧M1M2、圆弧M2M3、圆弧M3M4和直线M4M5组成）发送给车辆控制模块，车辆控制模块根据路径轨迹计算出方向盘转角、速度和挡位等决策信息来控制车辆执行机构，驾驶员松开制动踏板后车辆按照路径轨迹行走到达泊车终点后完成泊车。

2  垂直泊车路径规划算法设计

2.1  车辆运动模型

要规划泊车路径轨迹，首先要建立车辆运动模型，得到车轮转向角和车辆路径轨迹点坐标、航向角的数学关系，故本文在阿克曼转向几何学基础上建立了车辆二自由度模型。

2.1.1  阿克曼转向模型

2.1.2  车辆运动学模型

本文研究对象为前轮转向汽车，泊车过程车速较低（一般在3km/h以下），因此车辆可以近似为一个刚体，假设车辆只做纯滚动运动，不发生侧滑，由此可确定如图6所示的车辆二自由度模型，即车辆运动学模型。

2.2  电子地图

进行自动泊车路径规划的前提是需要知道待泊入车位信息、车辆位姿信息和障碍物信息等环境信息，环境感知模块将这些环境信息需要按一定的格式输出至路径规划模块，车辆位姿信息和车位信息通过建立电子地图坐标系可以得到，障碍物信息（可行驶区域）通过网格划分和网格是否被占用来判定来得到。

2.2.1  坐标系建立

要构建电子地图，首先要建立参考坐标系，如图7所示，以车辆泊车开始时的后轴中心点O为坐标原点，车辆直线前进方向为X轴正方向，采用右手法则沿着X轴正方向逆时针旋转90°得到Y轴正方向，建立了电子地图坐标系，从而可以确定待泊入车位4个角点P0、P1、P2和P3坐标，车位4个角点坐标由摄像头感知得到，图中点S0为泊车过程中任意位置的车辆后轴中心点，点S0的坐标和航向角θ由IMU感知得到，航向角θ的定义为电子地图坐标系X轴正方向沿着逆时针方向旋转到车辆直线前进方向的角度，范围为0～360°。

2.2.2  网格划分

环境感知模块输出自动泊车电子地图可行驶区域格式一般通过划分网格占用情况进行输出，考虑到控制器算力、路径规划算法需求响应时间、环境感知模块输出范围及精度，电子地图选取25m×25m范围进行输出，网格大小为10cm×10cm。

电子地图网格划分示意如图8所示，以泊车开始时的车辆后轴中心点作为坐标原点构建坐标系，建立25m×25m区域范围的电子地图，电子地图X坐标和Y坐标的范围均为-12.5～12.5m，将地图均等划分成62500个10cm×10cm的网格，按从上到下、从右往左顺序对网格进行编号，环境感知模块通过网格变量来标记和记忆每个网格是否被障碍物占用来输出车辆可行驶区域，网格变量定义为当网格被障碍物占用时值为0，当网格未被占用时值为1，当网格不能判定是否被占用时值为2。

2.3  路径规划设计

常用的路径规划算法包括图搜法、几何法和人工智能算法等。图搜法包括Dijkstra、RRT、A*等算法，缺点是算法搜索速度慢、未考虑车辆运动约束、规划路径可能不可行驶等；几何法包括圆弧直线、微分平坦、B样条曲线、多项式曲线等算法，运算速度很快，缺点是规划路径曲率不一定連续；人工智能算法包括模糊控制、神经网络、遗传算法等算法，非常依赖专家经验系统建立和大量样本训练。

2.3.1  几何路径规划方法

考虑到算法的复杂度、计算效率和控制器算力，本文选用基于圆弧直线拼接的几何路径规划方法来对自动泊车路径进行规划，采用基于轨迹预判的碰撞约束方法来保证规划路径轨迹不与障碍物发生碰撞，利用基于轨迹预判的路径居中方法来保证泊车完成后车辆在车位居中无偏斜，几何法得到的路径轨迹按照一定的格式输出给车辆控制模块，车辆控制模块根据路径信息来进行路径跟踪控制。

2.3.2  碰撞约束方法

进行碰撞约束前，先得对车辆轮廓进行建模，由于车辆轮廓由不规则曲线组成，计算轮廓点坐标比较繁杂，因此常用规则形状的模型代替原轮廓形状，如图10所示的圆模型和图11所示的长方形模型，圆模型和长方形模型会占用部分可行驶区域，从而增加泊车所需空间，可能不满足狭小空间泊车要求，因此本文采用图12所示的基于膨胀法的轮廓模型。该模型保留了车辆原有轮廓形状，为减小计算量每间隔10cm取一个轮廓点，以后轴中心点为原点，点Ai为选取的任意轮廓点，点Ai极坐标为（Ri，θi），考虑到感知模块测量误差，采用膨胀法来保证车辆不发生碰撞，即车辆轮廓往径向扩张距离L，用膨胀轮廓点Bi（Ri+L，θi）来代替点Ai。

图13为垂直泊车过程几何碰撞约束示意图，点M0为车辆泊车起始位置，以点M0为原点建立了电子地图，泊车过程中后轴中心点坐标可由感知模块获取，从而可以求得车辆所有的膨胀轮廓点Bi笛卡尔坐标，图中空白网格为车辆可行驶区域，带阴影网格为车辆不可行驶区域，泊车开始后车辆沿着直线M0M1运动到达关键点M1，点M1坐标计算见2.3.1章节，然后车辆在点M1向顺时针方向打满方向盘以最小转弯半径沿着圆弧M1M2向后运动到关键点M2，车辆在点M2向逆时针方向打满方向盘以最小转弯半径沿着圆弧M2M3向前运动到关键点M3，关键点M2和M3的坐标分别由泊车过程中车辆膨胀轮廓点Bi与不可行驶区域发生碰撞（相交）条件得到，車辆在点M3向顺时针方向打满方向盘以最小转弯半径沿着圆弧M3M4向前运动到关键点M4，点M4坐标通过几何相切条件（车辆在点M4的航向角为90°）计算得到，此时车身摆正，然后车辆沿着直线M4M5到达关键点M5，然后泊车完成。假如，图13中泊入车位对面无障碍物，则车辆达到点M3后会继续向前转向运动，直到车身摆正，然后车辆往后直线运动将会碰撞到泊入车位右侧障碍物，因此需要增加车辆不碰撞车位右侧障碍物的约束条件，本文采用基于轨迹预判的碰撞约束方法来解决这个问题。

2.3.3  轨迹居中方法

根据2.3.2章节讲述的碰撞约束条件可以保证车辆能泊入车位，但是不能保证泊车完成后车辆在车位中左右居中，本文设计了一种基于轨迹预判的自动泊车路径居中方法来解决这个问题。

图15为基于轨迹预判的车辆自动泊车路径规划左侧居中算法示意图，其中点P0、P1、P2和P3为车位的4个角点，后轴中心点沿着圆弧A0A3以最小转弯半径向后运动来泊入车位，车辆达到点A3位置后停车，然后沿着圆弧A3B3以最小转弯半径向前运动，初始位置点A0坐标为已知的，点A3的位置需要求解，通过在点A3出预判下一段路径轨迹，通过居中条件来求解点A3坐标。

该算法具体求解过程是：泊车开始时控制车辆在点A0原地方向盘沿顺时针方向打满，后轴中心点以车辆最小转弯半径沿着圆弧A0A3向后运动，依次经过点A1、A2和A3。假设后轴中心点行进到点A1时，圆弧A1B1为下一段预判的路径轨迹，即假设车辆在点A1停车后，车辆将在点A1原地方向盘逆时针打满，沿着圆弧A1B1前进直到车身前进方向与车位中心线（或者直线P0P1）平行，此时B1在车位中心线右侧，未达到居中条件，因此车辆达到点A1位置不停车，继续向后运动；假设后轴中心点行进到点A2时，圆弧A2B2为下一段预判的路径轨迹，即假设车辆在点A2停车后，车辆将在点A2原地方向盘逆时针打满，沿着圆弧A2B2前进直到车身前进方向与车位中心线（或者直线P0P1）相切，此时B2正好在车位中心线上，达到居中条件，因此车辆达到点A2位置后停车，然后将沿着下一段路径圆弧A2B2前进运动；假设后轴中心点行进到点A3时，圆弧A3B3为下一段预判的路径轨迹，即假设车辆在点A3停车后，车辆将在点A3原地方向盘逆时针打满，沿着圆弧A3B3前进直到车身前进方向与车位中心线（或者直线P0P1）平行，此时B3在车位中心线左侧，未达到居中条件，因此车辆不会运动到点A3位置。故，基于轨迹预判的车辆泊车居中算法可以计算出居中关键点A2，车辆沿着圆弧A0A3运动，在达到点A2将停车并规划下一段路径圆弧A2B2，可以求解出点A2和B2的位置，假设沿着圆弧A0A2运动过程遇到障碍物未能达到点A2，则需要通过基于轨迹预判的路径规划右侧居中算法来对路径轨迹进行居中调整。

图16为基于轨迹预判的车辆自动泊车路径规划右侧居中算法示意图。泊车开始时控制车辆在点C0原地方向盘沿逆时针方向打满，后轴中心点以车辆最小转弯半径沿着圆弧C0C3向前运动，依次经过点C1、C2和C3。假设后轴中心点行进到点C1时，圆弧C1D1为下一段预判的路径轨迹，即假设车辆在点C1停车后，车辆将在点C1原地方向盘顺时针打满，沿着圆弧C1D1后退直到车身前进方向与车位中心线（或者直线P0P1）平行，此时D1在车位中心线左侧，未达到居中条件，因此车辆达到点C1位置不停车，继续向前运动；假设后轴中心点行进到点C2时，圆弧C2D2为下一段预判的路径轨迹，即假设车辆在点C2停车后，车辆将在点C2原地方向盘顺时针打满，沿着圆弧C2D2后退直到车身前进方向与车位中心线（或者直线P0P1）相切，此时D2正好在车位中心线上，达到居中条件，因此车辆达到点C2位置后停车，然后将沿着下一段路径圆弧C2D2向后运动；假设后轴中心点行进到点C3时，圆弧C3D3为下一段预判的路径轨迹，即假设车辆在点C3停车后，车辆将在点C3原地方向盘顺时针打满，沿着圆弧C3D3后退直到车身前进方向与车位中心线（或者直线P0P1）平行，此时D3在车位中心线左侧，未达到居中条件，因此车辆不会运动到点C3位置。因此，基于轨迹预判的车辆泊车居中算法可以计算出居中关键点C2坐标，车辆沿着圆弧C0C3运动，在达到点C2将停车并规划下一段路径圆弧C2D2，可以求解出点C2和D2的位置，假设沿着圆弧C0C2运动过程遇到障碍物未能达到点C2，则需要通过图15所示的基于轨迹预判的路径规划左侧居中算法来对路径轨迹进行居中调整。

3  自动泊车试验设计

3.1  垂直泊车仿真测试

3.1.1  仿真测试工装

如图17和图18所示，垂直泊车仿真测试工装包括感知模拟模块、路径规划模块和路径仿真显示模块，其中感知模拟模块和路径规划模块分别在虚拟机上命令窗口独立并行运行，感知模拟模块用于模拟摄像头、雷达等感知传感器发送垂直车位4个角点坐标和车辆初始坐标、航向角信息，路经规划模块收到车位及车辆位姿信息后进行路径规划，并按一定格式输出泊车路径信息并保存为.m文件，路径仿真显示模块是用MATLAB搭建的GUI界面，将路径.m文件导入GUI界面后可以显示泊车过程车辆轮廓包络轨迹，可以直观看出泊车过程车辆是否发生碰撞，并且可以输出泊车完成后车辆在车位中的偏移距离及偏斜角度数值。

3.1.2  仿真测试

通过仿真测试工装，分别模拟如图19所示垂直泊车场景，图中待泊入车位宽度W分别为2.6m、2.8m和3.0m的车位，车位长度L为6.0m，车辆行驶方向相对车位线P0P3的初始航向角为0°，车辆右侧到车位线P0P3的初始偏距H分别为0.5m、1m、1.5m和2.0m，组合总计12种垂直场景，保存虚拟机上得到的路径轨迹.m文件，将.m文件导入MATLAB仿真界面中显示泊车轨迹，记录泊车完成后在车位中的偏移距离和偏斜角度。

图20为虚拟机输出路径信息，模拟的是车位宽度为2.8m、初始偏距为2.0m的垂直泊车场景的路径规划，规划出的路径轨迹信息包括各轨迹点X坐标、Y坐标、曲率、航向角、挡位等信息。图21为接收图20的路径信息后MATLAB仿真界面显示结果，界面根据虚拟机输出的后轴中心点轨迹信息，计算出車辆各轮廓点坐标，并绘制出后轴中心点和车辆轮廓点曲线（即车辆运动包络线），界面还可以输出各垂直泊车场景泊车完成后车辆在车位中的偏移距离和偏斜角度，如表1所示。仿真结果表明，当车位宽度大于等于2.6m，车辆初始偏距离为0.5～2.0m范围时，设计的垂直泊车路径规划算法可以成功规划出泊车路径轨迹，路径轨迹与环境障碍物没有发生碰撞，满足自动泊车安全性要求，泊车完成后车辆在车位基本居中和车身基本摆正，车辆偏移距离小于等于5cm，车辆偏斜角度小于等于0.5°，满足泊车完成后车辆姿态要求。

3.2  垂直泊车实车测试

3.2.1  实车测试工装

垂直泊车测试工装由4个摄像头及12个超声波雷达组成的感知模块、主控制器、上位机等组成，搭载车辆为比亚迪某车型，其中感知模块负责获取待泊入车位4个角点坐标和车辆位姿信息，主控制器上运行泊车过程状态切换、路径规划及路径跟踪控制等软件算法，上位机用于软件编译下载及泊车信息显示打印等。

3.2.2  实车测试

垂直泊车实车测试时，用白胶条贴出车位宽度分别为2.6m、2.8m和3.0m的车位，车位长度为6.0m，车辆初始航向角为0°，通过卷尺测量控制车辆初始偏距分别为0.5m、1m、1.5m和2.0m，组合总计12种垂直泊车场景，泊车完成后用卷尺测量车辆在车位中的偏移距离，并根据前后轮偏移距离计算出车辆偏移角度，图22为垂直泊车完成后车辆姿态图，图23为总线采集的目标轨迹点和实际运动轨迹点曲线图，表2为泊车完成后车辆偏移距离和车辆偏斜角度数据统计表。实车测试结果表明，设计的路径规划算法在常见的垂直泊车场景都能在不发生碰撞前提下规划出路径并泊入车位中，泊车完成后车辆偏移距离小于等于10cm，车辆偏斜角度小于等于1.5°，由于车辆感知误差和车辆跟踪控制误差，车辆偏移距离和偏斜角度均比仿真测试结果要大，但基本满足泊车要求。

4  结论

本文为解决目前量产自动泊车系统泊车成功率不高、泊车完成后车辆姿态偏斜不居中等问题，研究了一种基于轨迹预判的垂直泊车路径规划算法。研究搭建了基于阿克曼转向几何学和车辆运动学方程的车辆运动模型，构建了基于可行驶区域的网格化电子地图，在几何法中融入基于轨迹预判的车位角点碰撞约束和路径居中算法，通过车辆膨胀轮廓模型最大程度利用电子地图可行驶区域和保证路径安全性，设计出一种基于轨迹预判的垂直泊车几何路径规划算法，通过仿真测试和实车测试验证了算法的可行性，该算法大大提高了泊车成功率，解决了泊车完成后车辆偏斜不居中问题。

参考文献：

[1] Gorinevsky D，Kapitanovsky A. Neural networkarchitecture for trajectory generation and control of automated car parking[J]. IEEE Transactions on Control Systems Technology，1996，4（1）：50-56.

[2] Daxwanger W A，Schmidt G K. Skill-based visual parking control using neural and fuzzy networks[J]. IEEE International Conference on Systems，1995（2）：1659-1664.

[3] Zhao Y，Collins Jr E G. Robust automatic parallel parking in tight spaces via fuzzylogic[J]. Robotics and AutonomousSystems，2005，51（2）：111-127.

[4] Chen Q Y，Chen T T，Yu H Y，et al. Lateral control for autonomous parking system with fractional order controller[J]. Journal of Software，2011（6）：1075-1078.

[5] Zheng G，Liang Z，Li J. Optimization of an intelligent controller for parallel autonomous parking[J]. Telkomnika Indonesian Journal of Electrical Engineering，2012，11（2）：1069-1075.

[6] 秦绪情. 自动平行泊车系统定速模糊控制算法研究[D]. 长春：吉林大学，2007.

[7] 宋金泽. 自主泊车系统关键技术研究[D]. 长沙：国防科技大学，2009.

[8] 姜辉. 自动平行泊车系统转向控制策略的研究[D]. 长春：吉林大学，2010.

[9] 郭孔辉，姜辉，张建伟，等. 基于路径规划的自动平行泊车转向控制器的研究[J].吉林大学学报（工学版），2011，41（2）：293-297.

[10] 李红，郭孔辉，宋晓琳. 基于样条理论的自动垂直泊车轨迹规划[J]. 湖南大学学报（自然科学版），2012，39（7）：25-30.

（编辑  凌  波）