具体实施方式

如图1和图2所示，本发明的基于直线圆弧路径的全自动泊车控制方法包括以下步骤：

步骤S1：如图1所示，根据规划路径信息中的分段的泊车路径，确定下一段泊车路径的期望方向盘角度和提前转向距离；

其中，整个泊车路径是分段的，同一个档位的情况下，同一个半径大小的轨迹为一段。在初始化(即车辆并未开始行驶)的状态下，下一段泊车路径为第一段泊车路径。需要说明的是，下一段泊车路径的期望方向盘角度和提前转向距离不需要反复计算，只需要在车辆刚到达一段路径的起点时，计算一次下一段泊车路径的期望方向盘角度和提前转向距离。

由此，本发明的全自动泊车控制方法可以先提前一段距离打方向盘至下一段泊车路径的所对应的期望方向盘角度，避免转向不足所导致的误差。

所述步骤S1具体包括：

步骤S11：预先根据车辆测试和阿克曼转角的计算公式，得到不同的方向盘角度所对应的阿克曼转角，以实现车辆标定；

所述步骤S11具体包括：

步骤S111：在空旷的平地上，驾驶员将方向盘角度α固定，让车辆怠速跑一圈，再用皮尺测量车辆转一圈行驶轨迹的半径作为泊车路径的圆弧的半径大小R，通过阿克曼转角的计算公式(即公式(1))计算得出该方向盘角度α对应的阿克曼转角

阿克曼转角的计算公式如下：

其中，R为一段泊车路径的圆弧的半径大小，L为车辆轴距，为阿克曼转角。其中，R是根据规划路径信息获得的；车辆轴距L是在车辆上测出来的，直接预先给定到算法中。

步骤S112：不断改变方向盘角度α并重复步骤S111，直到方向盘可转动范围内的所有方向盘角度α均测试完成，此时标定完成。由此，可以得到一系列方向盘角度α与阿克曼转角的对应关系。

其中，方向盘转动范围内的所有方向盘角度α包括，从左打死方向盘所对应的目标方向盘角度和从右打死方向盘所对应的目标方向盘角度之间等间隔的多个方向盘角度α。在本实施例中，相邻的两个方向盘角度α之间的间隔为30°，即每隔30°改变一次方向盘角度α。

步骤S12：当下一段泊车路径为直线，则下一段泊车路径的期望方向盘角度为0°；当下一段泊车路径为圆弧时，根据泊车路径的圆弧的半径大小和阿克曼转角的计算公式，得到阿克曼转角，并根据步骤S11的标定结果，得到阿克曼转角对应的方向盘角度作为下一段泊车路径的期望方向盘角度。

步骤S13：根据下一段泊车路径的期望方向盘角度、当前车辆速度、EPS控制器的迟滞响应时间以及EPS控制器的转角响应时间，确定下一段泊车路径的提前转向距离s。

此外，由于当规划路径信息中的某一段泊车路径为最大方向盘角度所对应的圆弧(即最小半径圆弧)时，车辆以最大方向盘角度转向，由于车辆的不足转向特性，车辆行驶过某弧长阈值l_sh后，跟踪误差(包括路径跟踪的位置偏差以及航向偏差)会超过容许范围，因方向盘已打到极限，无法通过自修正来减小误差。因此，本发明在提前转向距离计算公式中加入关于弧长阈值l_sh的额外距离计算项，在控制误差范围内，适当增加提前转向距离来抵消因不足转向产生的误差。

因此，在所述步骤S13中，在下一段泊车路径的期望方向盘角度为最大方向盘角度时，还根据弧长阈值l_sh来协助确定下一段泊车路径的提前转向距离s；其中，弧长阈值l_sh的值是通过实车测试得到的，测试方法为：把方向盘固定在左打死或者右打死的位置上不动，让车速从0逐步增加到最大泊车速度，车辆行驶一段弧长后，其轨迹转弯半径将会大于该方向盘转角对应的转弯半径，测量出从起点到转弯半径开始变大的实车轨迹点的弧长距离。

因此，下一段泊车路径的提前转向距离s的计算公式如下：

s＝v*(|σ_e-σ_c|/ω)+v*t_sr+C*K*(l-l_sh) (2)

其中，v为当前车辆速度，t_sr为EPS控制器的迟滞响应时间，由EPS实车测试得到；σ_e为下一段泊车路径的期望方向盘角度，其根据规划路径信息获得的；σ_c为当前周期的实际方向盘角度，ω为方向盘角速度，此处的方向盘角速度直接采用方向盘最大角速度值，方向盘最大角速度的值可以通过实车采集方向盘角度的信号，再进行微分得到；C为选择参数，当下一段泊车路径的期望方向盘角度为最大方向盘角度且其弧长超过弧长阈值l_sh时，C置为1，否则C置为0；l为期望方向盘角度为最大方向盘角度时的下一段泊车路径的弧长；l_sh为弧长阈值；K为标定参数。该标定参数通过实车测试过程中不断调整得到。

步骤S2：在每个固定的周期，基于车辆在当前周期的路径跟踪的位置偏差和航向偏差，对当前段的泊车路径的期望方向盘角度进行修正，得到当前周期的目标方向盘角度σ_t；直到车辆在当前段的泊车路径的剩余距离小于提前转向距离，则将下一段的泊车路径更新作为当前段的泊车路径，并回到步骤S1；

当车辆后轴中心点行驶过路径衔接点后，需要相应的控制策略保证车辆行驶在轨迹误差范围内，因此，本发明的基于直线圆弧路径的全自动泊车控制方法的步骤S2基于车辆在当前周期的路径跟踪的位置偏差和航向偏差，在期望方向盘角度附近进行修正。

需要说明的是，本发明的整个基于直线圆弧路径的全自动泊车控制方法是周期化运行的，也就是隔一段时间(即周期)输出一次包含多个参数的结果，而不是仅仅某个参数是周期性输出的。在本实施例中，所述周期是在实践中经验性地确定的，其数值范围在10～50ms比较合适。

泊车过程的误差模型如图3所示。

图3中，G为车辆的后轴中心点，AB为规划的包含直线和圆弧的泊车路径，O点为泊车路径中与G点距离最近的点，即泊车路径上的控制目标点，定义路径跟踪的位置偏差为车辆的后轴中心点G到控制目标点O的距离，θ和θ_d分别表示车辆纵轴线和泊车路径在控制目标点O的切线与X轴的夹角，定义路径跟踪的航向偏差为θ-θ_d，假设车辆向后行驶，定义控制目标点O到路径末点A(即换挡点或终点)的长度为纵向距离偏差s_e；路径末点A是某一段泊车路径的末点；纵向距离偏差的作用是用于速度规划，计算目标车速。

在所述步骤S2中，对当前段的泊车路径的期望方向盘角度进行修正，具体包括：

步骤S21：在每个周期，判断车辆当前的路径跟踪的位置偏差和航向偏差是否均小于控制误差阈值；控制误差阈值均是人为设定的。

步骤S22：根据步骤S21的判断结果确定并输出当前周期的目标方向盘角度；

在所述步骤S22中，若车辆当前的路径跟踪的位置偏差、航向偏差中的至少一个大于控制误差阈值时，则当前周期的目标方向盘角度为：

σ＝σ_e+K_p*e_pos+K_h*e_head (3)

式中，σ是当前周期的目标方向盘角度σ_t的中间变量，公式(4)的作用是限幅；σ_t为当前周期的目标方向盘角度，σ_e为当前段的泊车路径的期望方向盘角度，e_pos为车辆在当前周期的位置偏差，用于控制车辆靠近参考路径行驶，e_head为车辆在当前周期的航向偏差，用于控制车辆航向角与参考路径控制点航向角一致，同时避免系统因超调而造成车辆蛇形行驶，K_p为位置偏差项系数，K_h为航向偏差项系数，σ_max为最大方向盘转角。

在所述步骤S22中，当车辆在当前周期的路径跟踪的位置偏差、航向偏差均小于设定阈值时，不进行控制(即不需要对方向盘角度再进行修正)，直接输出当前段的泊车路径的期望方向盘角度或者上一个周期的目标方向盘角度作为当前周期的目标方向盘角度。其中，在当前段的泊车路径中存在上一个周期的目标方向盘角度时，输出上一个周期的目标方向盘角度；否则，输出当前段的泊车路径的期望方向盘角度。由此，保证了方向盘控制的稳定性。

上述的步骤S1、S2均由目标转角控制模块100执行，从而由目标转角控制模块100来输出当前周期的目标方向盘角度，从而通过目标转角控制模块100在每个周期均输出目标方向盘角度。

步骤S3：根据当前周期的实际方向盘角度和所述步骤S2中的当前周期的目标方向盘角度σ_t来得到当前周期的当前目标角度δ_out，并将当前周期的当前目标角度δ_out输出。其中，将当前周期的当前目标角度δ_out发送给EPS控制器(电动助力转向系统控制器)，从而控制车辆以当前目标角度δ_out进行转向来行驶。

其中，当前周期的实际方向盘角度由车辆自带的转角传感器提供。由此，可以将方向盘转速度控制在一定的转向步长范围内，同时防止EPS因当前与目标角度差过大而失效。在本实施例中，所述步骤S3是由一EPS转角控制模块2来执行的。

当前周期的当前目标角度δ_out为：

式中：δ_out为当前周期的当前目标角度，该参数也是整个基于直线圆弧路径的全自动泊车控制方法的输出结果；δ_c为当前周期的实际方向盘角度，δ_t为当前周期的目标方向盘角度；Δ为控制步长。

其中，控制步长Δ需要尽可能大，同时必须满足当前车辆所使用EPS的规格参数。因为控制步长Δ越大越能够保证控制的实时性，但是越大越容易导致EPS模块宕机，所以需要在二者之间取个平衡，在保证EPS模块不宕机的情况下，此值越大越好，一般会预先根据实车测试确定出控制步长Δ的值。

控制步长Δ的测试方法如下：通过CANoe(一种CAN通讯工具)向EPS模块周期性地发送目标方向盘角度，发送周期与本发明的基于直线圆弧路径的全自动泊车控制方法的周期的值相同，每次发送的目标方向盘角度都在上一次发送的基础上增加一控制步长Δ，且控制步长Δ从10°逐步增加，直到EPS模块宕机，则宕机前的那一次控制步长Δ即为所求。

需要说明的是，控制步长Δ对提前转向距离s的影响已经由方向盘角速度ω考虑了，所以提前转向距离s的计算不再单独添加关于控制步长Δ的修正项。

上述的步骤S3由EPS转角控制模块2来执行，从而通过EPS转角控制模块2来输出当前周期的当前目标角度δ_out。

由此，通过步骤S1、步骤S2和步骤S3共同实现了方向盘转角控制设计。

此外，本发明的基于直线圆弧路径的全自动泊车控制方法还可以包括步骤S1’：如图2所示，根据当前车辆位姿和规划路径信息中的泊车路径来确定纵向距离偏差s_e，并根据纵向距离偏差s_e输出当前周期的目标车速。由此，实现了速度规划设计。

所述步骤S1’由一速度规划模块3执行，并且与上文的步骤S1-步骤S3同时执行。所述步骤S1’具体包括：

步骤S11’：在每个周期，根据车辆底盘控制系统的延迟响应时间以及当前开始制动车速，确定提前制动距离s_f；

本发明根据泊车路径在换挡点(即前进档切换为倒退档的位置，或者倒退档切换为前进档的位置)和终点处速度必为0的特点，将泊车行驶视为匀减速度的过程，通过每段轨迹长度反推计算目标速度。为了保证较精确的停在目标点，必须考虑车辆底盘控制系统的延迟响应时间，提前一段距离开始制动。

因此，提前制动距离s_f为：

式中，s_f为提前制动距离，t_r为车辆底盘控制系统的延迟响应时间，其通过车辆测试得到；v_d为当前开始制动车速，当前开始制动车速v_d是在每个周期由车辆通过CAN将当前车辆速度传给速度规划模块3作为当前开始制动车速来得到的，当前车辆速度是控制器的其中一个输入参数；a_d为目标控制减速度。目标控制减速度a_d用于发送给底盘，其是在保证刹车平顺性的情况下采用的最大制动减速度，是一个标定值，由驾驶员在车上测试得到的，标定时，驾驶员在车上，让CANoe向车辆底盘ESC模块发送不同的减速度(由小到大)驾驶员感觉比较平顺且能够刹住车的最大减速度即为所求。

步骤S12’：在每个周期中计算判断纵向距离偏差s_e与提前制动距离s_f大小；当纵向距离偏差s_e≤提前制动距离s_f时，将当前周期的目标车速记为0，输出当前周期的目标车速和目标控制减速度a_d，控制车辆制动；否则，纵向距离偏差s_e>提前制动距离s_f，根据泊车最大车速和泊车最小车速确定并输出当前周期的目标车速v_t，以控制车辆行驶。

其中，当纵向距离偏差s_e>提前制动距离s_f时，当前周期的目标车速v_t为：

式中：v_t为目标车速，a_p为标定加速度，标定加速度a_p的值与上文中的目标控制减速度a_d的值相同，s_e为纵向距离偏差，即泊车路径上的控制目标点O到路径末点A(即换挡点或终点)的距离；v_max为泊车最大车速，v_min为泊车最小车速，泊车最大车速和泊车最小车速是人为给定的；v_o是目标车速v_t的中间变量，公式(8)是为了限幅。

泊车过程中，当前周期的目标车速和目标控制减速度a_d通过CAN总线输出给汽车底盘的ESC控制器(车身电子稳定性控制器)，以控制车辆行驶。实际加速度与目标加速度会有一定的出入，但是在加速过程中，这种出入的影响不大，在减速过程中，减速度由车辆的ESC模块保证。

如图4所示为基于上文所述的基于直线圆弧路径的全自动泊车控制方法，所实现的本发明基于直线圆弧路径的全自动泊车控制器100。

该全自动泊车控制器100的总体框图如图1所示，全自动泊车控制器100的输入参数为当前车辆位姿、当前车辆速度、当前周期的实际方向盘角度以及规划路径信息。其中，所述规划路径信息包括包含泊车路径和对应于每一段泊车路径的期望方向盘角度。

所述全自动泊车控制器100包括目标转角控制模块1、与所述目标转角控制模块1相连的EPS转角控制模块2以及速度规划模块3。

目标转角控制模块1的输入参数包括当前车辆位姿、当前车辆速度、当前周期的实际方向盘角度以及规划路径信息。

目标转角控制模块1设置为根据当前车辆位姿、当前车辆速度、当前周期的实际方向盘角度以及规划路径信息来确定当前周期的目标方向盘角度σ_t。具体来说，所述目标转角控制模块1设置为执行如下步骤：步骤S1：根据规划路径信息中的泊车路径，确定下一段泊车路径的期望方向盘角度和提前转向距离；步骤S2：在每个周期，基于车辆在当前周期的路径跟踪的位置偏差和航向偏差，对当前段的泊车路径的期望方向盘角度进行修正，得到当前周期的目标方向盘角度；直到车辆在当前段的泊车路径的剩余距离小于提前转向距离，则将下一段的泊车路径更新作为当前段的泊车路径，回到步骤S1。其中，目标方向盘角度σ_t的具体的计算方式如上文所述，其计算公式为上文的公式(3)、公式(4)。

所述EPS转角控制模块2的输入参数是当前周期的实际方向盘角度和当前周期的目标方向盘角度，输出参数是当前目标角度。

所述EPS转角控制模块2设置为根据当前周期的实际方向盘角度和当前周期的目标方向盘角度σ_t来得到当前目标角度δ_out，并将当前周期的当前目标角度δ_out输出。当前目标角度δ_out的具体计算方式已在上文详述，其计算公式如上文的公式(5)所示。

由此，通过增加EPS转角控制模块2，来在一定的转向步长(即控制步长Δ)的范围内控制EPS控制器转向，进而控制方向盘转速同时防止EPS控制器因当前周期的实际方向盘角度与目标转角控制模块1所提供的目标方向盘角度的差值过大而失效。

速度规划模块3设置为根据当前车辆位姿和规划路径信息中的泊车路径，输出当前周期的目标车速，从而进行速度规划设计。

由此，本发明的全自动泊车控制器100可以将当前目标角度δ_out和当前目标车速通过CAN总线发送给汽车底盘，控制车辆行驶。具体来说，全自动泊车控制器100将当前目标角度δ_out发送给汽车底盘的EPS控制器，EPS控制器是车辆自带的一种控制器，负责把方向盘转到当前目标角度上；全自动泊车控制器100将当前目标车速发送给汽车底盘的ESC控制器(车身电子稳定性控制器)，ESC控制器是车辆自带的模块，负责让车辆按照当前目标车速行驶。

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

以上所述的，仅为本发明的较佳实施例，并非用以限定本发明的范围，本发明的上述实施例还可以做出各种变化。凡是依据本发明申请的权利要求书和说明书内容所做的简单、等效变化与修饰，皆落入本发明专利的权利要求保护范围。本发明未详尽叙述的均为常规技术内容。