一种基于直线圆弧路径的全自动泊车控制方法

但孝杰，郑 迎

一种基于直线圆弧路径的全自动泊车控制方法

但孝杰，郑 迎

（华域汽车系统股份有限公司，上海 200041）

为了实现车辆在全自动泊车过程中，能够安全稳定的沿着给定的直线圆弧路径行驶，文章结合经典比例-积分-微分（PID）控制思想，提出了一种依据提前转向距离和目标角速度进行方向盘角度控制的方法，再基于当前车辆后轴中心点位置及航向信息与目标路径点之间的偏差进行修正的控制方法。同时根据规划的泊车路径特点，提出了一种基于匀减速度来反推速度的规划方法。实验结果表明，该方法能够平稳控制车辆泊车，并且控制误差满足精度要求。

全自动泊车；路径跟踪；速度规划；直线圆弧路径

全自动泊车系统可以有效避免在人为泊车过程中的安全事故，也是未来自动驾驶最后一公里的关键技术。全自动泊车系统主要技术包括车位扫描、泊车路径规划及车辆控制。

车辆控制的目的是使车辆能够平稳地沿着泊车路径行驶，并最终停在指定的目标点。文献[1]使用传统比例-积分-微分（Proportional Integral Derivative, PID）控制器控制算法，将车辆当前的路径跟踪偏差作为输入量，对跟踪偏差进行PID控制得到转向控制量。文献[2]使用了一种纯追踪控制算法，基于当前车辆后轴中心位置，在规划路径上向前一定距离选择预瞄点。假设车辆后轴中心可以按照一定转弯半径行驶到该预瞄点，通过车辆的运动学公式计算前轮转角。文献[3]提出了一种基于非时间参考的无模型自适应路径跟踪转型控制策略，通过建立车辆非时间运动模型，来摆脱车辆加减速带来的时域约束。随着自动驾驶技术的发展，根据车辆动力学模型和最优控制理论，业界提出了线性二次型控制器（Linear Quadratic Regulator, LQR），该方法使用车辆的状态来使控制误差最小化[4]。此外，模型预测控制（Model Predictive Control, MPC）方法也不断地应用在车辆控制上，虽然能得到很好的控制效果，但需要有一定算力的控制器[5]。

考虑使用的嵌入式控制器算力有限，只生成一条由直线圆弧构成的路径。以上常用控制方法均需要光滑的路径，曲率突变会引起转向不平顺，甚至发散。因此，本文基于全自动泊车低速行驶的特点，以及直线圆弧构成路径的特征，提出一种先提前转向后自修正的控制方法，即在经过直线圆弧衔接点前一段距离，开始提前打方向盘至下一段轨迹的目标角度。当车辆行驶过衔接点后，以此目标方向盘角度为基准进行修正，从而提高路径跟踪的智能性和鲁棒性。此外，根据泊车路径在换挡点和终点处速度必为0的特点，通过每段轨迹长度反推计算目标车速，从而保证车辆平稳行驶。

1 车辆运动模型

1.1 泊车过程运动模型

图1 车辆运动学模型示意图

车辆泊车运动过程为低速运动过程，忽略车轮的侧向滑动，同时为方便计算，将四轮车简化为虚拟的两轮车运动模型，如图1所示。

图1中，为车辆车身与坐标系轴的夹角，即为车辆的航向角；为等效前轮的阿克曼转角；为车辆轴距；为车辆的转弯半径；()为车辆后轴中心点坐标。由此可得车辆运动学公式为

1.2 泊车过程误差模型

泊车车辆的运动包括平面移动和平面旋转，因此，可以利用车辆局部坐标系和大地坐标系来描述车辆在某一时刻的位置和方向，如图2所示。

图2 泊车过程误差模型示意图

图2中，为车辆的后轴中心点；为规划的直线圆弧路径；点为规划路径中与点距离最近的点；定义路径跟踪的位置偏差为点到点的距离；和d分别为车辆纵轴线和规划路径在点的切线与轴的夹角。定义路径跟踪的航向偏差为-d，假设车辆向后行驶，定义点到路径末点的长度为纵向距离偏差。

2 车辆控制器设计

2.1 控制器功能概述

图3为控制器总体框图，输入当前车辆位姿、速度、方向盘角度以及规划路径信息，其中路径信息包括直线圆弧段端点的坐标和航向、圆弧半径大小、直线圆弧段的长度以及规划的总段数等。目标转角控制模块计算目标方向盘角度，为了控制方向盘转速，同时防止电动助力转向系统（Electric Power Steering, EPS）因当前与目标角度差过大而失效，增加EPS转角控制模块，以一定的转向步长控制EPS转向。速度规划模块根据当前所处路径位置输出目标控制车速，将当前目标方向盘角度和目标车速通过控制器局域网络（Controller Area Network, CAN）总线发送给汽车底盘执行器，控制车辆行驶。

图3 控制器总体框图

2.2 目标转角控制模块设计

泊车规划的轨迹由直线圆弧段构成，实现较好跟随轨迹，需要在衔接点处停车并原地调整转角姿态，这不符合正常泊车驾驶习惯且乘坐体验很差。因此，本文先提前一段距离打方向盘至下一段路径的目标角度。

当下段路径为直线，则输出目标方向盘转角为0°；当下段路径为圆弧时，通过圆弧半径大小以及车辆运动学公式，计算得到目标阿克曼转角，通过车辆标定，得到相应阿克曼转角对应的目标方向盘转角。计算公式为

式中，为下一段圆弧路径的半径大小；为车辆轴距；为阿克曼转角。

(一)工资、薪金所得，是指个人因任职或者受雇而取得的工资、薪金、奖金、年终加薪、劳动分红、津贴、补贴以及与任职或者受雇有关的其他所得。

考虑EPS的迟滞响应时间、转角响应时间及车速等因素，计算提前转向距离。当规划的某段路径为最小半径圆弧时，车辆以最大方向盘角度转向，由于车辆的不足转向特性，车辆行驶过某弧长阈值后，跟踪误差会超过容许范围。因方向盘已打到极限，无法通过自修正来减小误差，因此，本文在提前转向距离计算公式中加入额外距离计算项，在控制误差范围内，适当增加提前转向距离来抵消因不足转向产生的误差。提前转向距离为

式中，为当前车速；sr为EPS迟滞响应时间，由EPS实车测试得到；σe为下一路径段期望方向盘转角；σc为当前方向盘转角；为方向盘角速度，由当前车速和EPS控制步长计算标定得到；为选择参数，当下段路径为最小半径圆弧且弧长超过阈值sh时，置为1，否则为0；为规划的最小半径圆弧弧长；为标定参数。

当车辆后轴中心点行驶过路径衔接点后，需要相应的控制策略保证车辆行驶在轨迹误差范围内。PID控制是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，被广泛应用于工业过程控制[5]。传统的离散型位置式PID为

式中，()为控制器输出；()当前周期误差；(-1)为上一周期误差；()为历史累计误差；p为比例项系数；i为积分项系数；d为微分项系数。

传统PID控制只针对一个误差量进行控制，无法较好地同时保证泊车过程中的位置和航向误差。其次，误差数量级远小于控制量，容易造成PID参数过大且不易整定。因此，本文在PID控制思想基础上，提出基于车辆当前位置和航向偏差，在目标方向盘转角附近进行修正的控制方法，计算公式为

=e+p×pos+h×head（5）

式中，t为输出方向盘转角；e为当前路径段的期望方向盘转角；pos为位置偏差项，控制车辆靠近参考路径行驶；head为航向偏差，控制车辆航向角与参考路径控制点航向角一致，同时避免系统因超调而造成车辆蛇形行驶；p为位置偏差项系数；h为航向偏差项系数；max为最大方向盘转角。为了进一步保证方向盘控制的稳定，当位置航向误差小于设定阈值时，不进行控制，直接输出上一周期方向盘控制转角。

2.3 EPS转角控制模块设计

为了控制方向盘转速度，同时防止EPS因当前与目标角度差过大而失效，增加EPS转角控制模块，即以一定的转向步长控制EPS转向。当前周期输出目标方向盘转角为

2.4 速度规划模块设计

依据泊车路径[6-8]在换挡点和终点处速度必为0的特点，可将泊车行驶视为匀减速度的过程，通过每段轨迹长度反推计算目标速度。为了保证较精确地停在目标点，必须考虑车辆底盘控制系统延迟响应时间和控制响应时间，提前一段距离开始制动。提前制动距离为

式中，f为提前制动距离；r为控制系统延迟响应时间，通过车辆测试得到；d为当前开始制动车速；d为发送给底盘的目标控制减速度。计算目标车速为

式中，t为目标车速；p为标定加速度；e为纵向距离误差，即车辆规划路径上的控制目标点到换挡点或终点的距离；max为泊车最大车速；min为泊车最小车速。

泊车过程中，目标车速通过CAN总线发送给汽车底盘，控制车辆行驶。同时在每个控制周期中计算判断e与f大小，当e≤f时，发送目标减速度，控制车辆制动。

3 实车测试及分析

为验证本控制器在真实泊车环境中的有效性和可靠性，开展了实车测试。采用上汽荣威MARVELX为测试平台，车上搭载公司自研的6个高分辨率多模态毫米波雷达和4个摄像头感知获取车位信息。泊车测试场景包括平行泊车和垂直泊车，其中平行车位大小为车长+1.5 m，垂直车位大小为车宽+1.2 m。实验时泊车最高车速设为 3 km/h，最低车速设为1 km/h。

图4为车辆在平行泊车和垂直泊车时的方向盘转角控制图。由图4可见，车辆当前方向盘转角能够很好跟随目标方向盘度。

图5为车辆在平行泊车和垂直泊车时的位置偏差示意图。平行泊车过程中位置偏差始终控制在5 cm范围内，泊车完成时距离终点的位置偏差在4 cm左右。垂直泊车过程中，由于第一步泊车最小转向半径圆弧路径的存在，方向盘无法通过自修正来减小误差，目前通过标定提前转向距离，使最大位置偏差不超过10 cm，泊车完成时距离终点的位置偏差在2 cm左右。

图5 位置偏差图

图6为车辆在平行泊车和垂直泊车时的航向角偏差示意图。平行泊车和垂直泊车过程中，航向偏差始终控制在3°范围内，泊车完成时距离终点的航向偏差在1°左右。

图6 航向角偏差图

图7为车辆在平行泊车和垂直泊车时的速度控制图。车辆起步时，系统延迟响应时间在1 s左右；车辆刹车制动时，系统延迟响应时间在0.5 s左右。在平行泊车和垂直泊车过程中，速度偏差始终控制在0.1 m/s范围内。

4 结束语

本文针对全自动泊车规划直线圆弧路径曲率不连续特点提出了一种先提前转向后自修正的转向控制方法，同时根据换挡点和终点处速度必为0的特点，提出通过每段路径长度反推目标车速的速度规划方法，并通过实车测试对控制器的可靠性和有效性进行了验证。测试结果表明，上述控制方法可保证控制误差能快速地收敛到合理范围内，保证车辆能够有效准确实现路径跟踪和平稳行驶，具有一定的工程实用价值。

[1] 郑平平.无人驾驶拖拉机路径跟踪与转向控制研究[D].合肥:合肥工业大学,2020.

[2] 王雪犇.自动驾驶车辆垂直车位自动泊车控制算法研究[D].长春:吉林大学,2021.

[3] 张持.基于超声波的自动泊车控制策略研究[D].镇江:江苏科技大学,2018.

[4] 陶冰冰,周海鹰,王思山.自动驾驶车辆LQR轨迹跟踪控制器设计[J].湖北汽车工业学院学报,2017,31 (4):1-6.

[5] 黄颖涛.自动驾驶车辆轨迹跟踪模型预测控制方法研究[D].西安:长安大学,2019.

[6] 赵林峰,徐磊,陈无畏.基于自抗扰控制的自动泊车路径跟踪[J].中国机械工程,2017,28(8):966-973.

[7] 徐磊.基于EPS的自动泊车路径规划及跟踪控制研究[D].合肥:合肥工业大学,2017.

[8] 张刚.基于模糊控制汽车自动泊车关键技术设计[J].微型电脑应用,2019,35(6):123-125.

An Automatic Parking Control Method Based on Straight and Arc Path

DAN Xiaojie, ZHENG Ying

( Huayu Automotive Systems Company Limited, Shanghai 200041, China )

In order to realize safely and stably control the vehicle driving along the given straight and arc path in the process of automatic parking, combined with the classical proportion integration differentiation (PID) idea, a new control method is proposed, which reach the target steering wheel angle according to the advance steering distance and target angular speed, and then based on the current vehicle rear axle center position and heading information and the deviation between the target path point correction control method in this paper. Meanwhile according to the characteristics of the planned path, this paper also proposes a speed planning method, which is based on the uniform deceleration. The experiment result shows that the method can control the parking smoothly, and the control error meets the accuracy requirements.

Automatic parking; Path tracking; Speed planning; Straight and arc path

U495

A

1671-7988(2023)03-49-05

10.16638/j.cnki.1671-7988.2023.03.009

但孝杰（1995—），男，助理工程师，研究方向为自动驾驶技术，E-mail:xiaojied@hasco-group.com。

上海市2020年度“科技创新行动计划”高新技术领域项目（20511104400）。