[0001] 本发明涉及泊车方法，特别涉及一种垂直泊车方法。

[0002] 随着智能驾驶技术的发展，自动泊车技术的研发也高速发展，例如专利申请号为201810031452.4，申请日为2018年1月12日的发明专利申请《车辆在垂直停车位中的自主泊车》就公开了一种自动泊车技术。现有的机动车大多都配备了超声波传感器进行障碍物检测，帮助驾驶员消除盲点和视力模糊，提高驾驶安全性。由于超声波传感器经济实惠且多功能，现有的自动泊车技术多采用以超声波进行自动泊车位搜索。然而，受限于超声波传感器本身的特性，以及声波传播对外围环境强依懒性的特点，使得在利用超声波进行车位搜索时，往往会产生较大的车位位置估计误差，同时叠加车辆定位可能产生的偏离泊车预设轨迹误差，从而大大降低针对多复杂工况下泊车入位的成功率。

[0022] 下面将结合具体实施例及附图对本发明垂直泊车方法和及作进一步详细描述。

[0023] 可应用本发明方法的车辆应包括能够以简化的驾驶员输入来驾驶车辆的自动或半自动驾驶系统，包括用于收集车辆周围环境的信息的传感器，例如获取环境图像、车速、周围障碍物、温度、湿度等的传感器，驾驶系统基于收集到的信息来执行例如减速、转向、加速、制动、识别行人、识别停车位和自动或半自动泊车等功能。上述的自动或半自动泊车指的是一种自动或半自动驾驶状态，在自动驾驶状态下，车辆在不需要直接的驾驶员输入的情况下控制车辆的运动动作(例如，转向、转弯、加速、减速等)以将车辆停放在停车位中。在半自动驾驶状态下，车辆在某些情况下可能需要驾驶员输入某些确认口令来继续执行一些非常规的控制车辆的运动动作以将车辆停放在停车位中。

[0024] 本发明中，车辆四角装载有超声波传感器，在进行垂直停车位泊车时，可实现基于超声波传感器输出的信号进行垂直泊车位的重定位和垂直泊车。“垂直泊车”指的是目标停车位的泊车入口位于停车位的短边，与目标停车位的长边相邻的可能是其他停车位或墙、柱等障碍物。在此，为避免混淆，该停车位被称为垂直停车位。

[0025] 进一步地，本发明的垂直泊车方法和允许车辆正向前进或倒向行驶的方向来进行泊车。在车辆正向前进进行泊车时，主要使用车辆车头两个前角处的超声波传感器。在车辆倒向行驶进行泊车时，主要使用车辆车尾两个后角处的超声波传感器。车辆的“前角”指的是车辆的前表面（车头边界）和侧表面（车身侧边界）会聚到的车辆部分，通常为连接车辆的前表面和侧表面的弯曲表面，包括左前角和右前角。车辆的“后角”指的是车辆的后表面（车尾边界）和侧表面会聚到的车辆部分，通常为连接车辆的后表面和侧表面的弯曲表面，包括左后角和右后角。在自动或半自动驾驶系统中，车辆通常被表示为直角长方形俯视状态（如图1所示）或具有一定圆角的长方形俯视状态。为方便描述，本文以车辆被表示为直角长方形俯视状态为例进行说明，且车辆左前角、右前角、左后角和右后角分别被标注为Pb，Pa，Pc和Pd。当在自动或半自动驾驶系统中，车辆被表示为具有一定圆角的长方形俯视状态时，应默认圆角中的一点为角点，并与图1中的点Pb，Pa，Pc和Pd具有同样的功能。此外，车辆车尾边界的中点被标注为M1，车辆车头边界的中点被标注为M2。

[0026] 本实施例中，车辆每个角处均设置三颗超声波传感器，其他实施例中可采用功能类似的其他传感器组合或集成传感器。请参考图2，以左后角和右后角处为例（左前角和右前角与左后角和右后角相似，并基本对称），左后角设置超声波传感器SR_RL、LR_RL和SR_RML，右后角设置超声波传感器SR_RR、LR_RR和SR_RMR。相邻的超声波传感器可形成公共探测FOV（Field of View），同时基于超声波的单发多收机制，利用三角定位法对障碍物进行定位。其中超声波传感器SR_RL和SR_RR设置在角部更靠近车身两侧处，分别主要用于探测车尾两侧的障碍物。超声波传感器SR_RML和SR_RMR设置在角部更靠近车尾边界的中点M1，分别主要用于探测车尾后侧的障碍物，超声波传感器LR_RL和LR_RR靠近图1中的点Pc和Pd，分别主要用于探测车尾角部正对的障碍物。

[0027] 在进行垂直泊车的过程中，车辆一般以近乎平行于初始垂直停车位的状态进入车位，则大坐标系x-o-y、车辆及初始垂直停车位的位置关系一般如图1所示（在不同的情景中，仅仅车辆左倾斜或右倾斜或车头进入还是车尾进入的不同），坐标系的X轴大体平行（实质上或视觉上基本平行）于初始垂直停车位的长边，一般偏差不超过6°。在泊车入位之前，车辆的自动或半自动驾驶系统应利用传感器检测得到一个四角用A、B、C和D标记、外边界（图中连接点A、B、C和D的为实线框）如图1所示的初始垂直停车位，并检测到四角在大坐标系x-o-y下的坐标信息分别为（A_x, A_y），(B_x, B_y)，(C_x, C_y)，(D_x, D_y)。此时，车辆的各角点Pb，Pa，Pc和Pd的坐标为(Pb_x, Pb_y)，（Pa_x, Pa_y），(Pc_x, Pc_y)和(Pd_x, Pd_y)，车辆车尾边界的中点M1的坐标信息为（M1_x, M1_y, Theta），车辆车头边界的中点M2的坐标信息为（M2_x, M2_y, Theta）。其中，上述坐标信息中，凡是跟着“_x”的坐标信息均为X轴坐标值，凡是跟着“_y”的坐标信息均为Y轴坐标值。Theta为车辆方向（M1-M2）与x轴方向所成的夹角。此外，定义一起始参考线L1、一中间参考线L2和一末端参考线L3，参考线L1、L2和L3均平行于初始垂直停车位的短边AB（入口短边）和CD（底部短边），且起始参考线L1位于初始垂直停车位的入口短边AB之外，并距离入口短边AB的距离为0.4 0.6m，该距离~与超声波传感器的触发范围有关，应大于超声波传感器探测距离的触发阈值，本实施例中为0.5米。中间参考线L2和末端参考线L3均位于初始垂直停车位内，且中间参考线L2位于入口短边AB和末端参考线L3之间，末端参考线L3位于中间参考线L2和短边CD之间。中间参考线L2与起始参考线L1之间的距离为2.3 2.6m，本实施例中为2.5米，末端参考线L3与中间参~
考线L2之间的距离为1.8 2.2m，本实施例中为2米。其中，起始参考线L1、中间参考线L2和初~
始垂直停车位的长边AD、BC的延长线围成的区域标记为D1，末端参考线L3、中间参考线L2和初始垂直停车位的长边AD、BC围成的区域标记为D2。可以理解的，起始参考线L1、中间参考线L2和末端参考线L3可全部或部分为曲线（非直线）。当起始参考线为曲线时，其位于初始垂直停车位的入口短边之外，且起始参考线上的各点到入口短边的最短距离（垂线段的长度）大于车辆的车角超声波传感器探测距离的触发阈值。当末端参考线为非直线时，其与中间参考线L2之间的距离应落在1.8 2.2m内。中间参考线用于触发第二车位重定位状态，此~
时车辆与车位基本呈平行状态，因此中间参考线实质或大体呈直线。

[0028] 为方便描述，本实施例中，设定在泊车的过程中最先越过起始参考线L1的车辆的车角为右后角Pd（第一车角）。

[0029] 请参考图3至图6，一实施例中，本发明的垂直泊车方法包括步骤：S001，进入泊车状态，且判断为倒车泊车状态，获取初始垂直停车位和初始垂直停车位的外边界的坐标信息；
S002，根据车辆车尾边界的中点M1的坐标信息为（M1_x, M1_y, Theta）判断点M1是否位于起始参考线L1上或已经进入区域D1（进入区域D1也即位于起始参考线L1和中间参考线L2之间），如果是，则进入第一车位重定位状态，执行步骤S003，否则执行步骤S014。

[0030] S003，当第一车角（本实施例中为右后角Pd）的超声波传感器SR_RR和LR_RR的探测障碍物距离Dist小于阈值DistTh_Up，也即Dist落在传感器探测盲区阈值DistTh_Down和探测距离的触发阈值DistTh_Up之间（Dist∈[DistTh_Down, DistTh_Up]）时，判断车辆在初始垂直停车位内有撞到障碍物的可能性，执行步骤S004，否则返回步骤S001。本实施例中，盲区阈值DistTh_Down为0.3m（小于或等于0.3m时是超声波传感器的探测盲区，且保持输出为0.3m不变）；超声波探测距离的触发阈值DistTh_Up为0.4m。

[0031] S004，基于超声波传感器SR_RR及LR_RR的公共探测FOV（Field of View）以及超声波的单发多收机制，利用三角定位法对障碍物进行定位，若超声波探测距离Dist∈[DistTh_Down, DistTh_Up]，可定位到障碍物，则执行步骤S005，否则执行步骤S013。

[0032] S005，计算在大坐标系x-o-y下障碍物的坐标（obj_x, obj_y），并按照时间以滑动窗口的形式（缓存一组固定大小的点，比如10个点；然后，每次刷新，就会更新一个点进入缓存，同时将最“旧”的点删除，这样在每个更新周期，都会有不断的新点进入缓存，不断的“旧”点删除，目的是始终保持的固定大小的数组，对历史数据进行处理，以便得到一个准确的结果），记录数据长度为N的障碍物坐标点集W={(obj_x(n), obj_y(n)) | N=1,2,3,...N}，N为自然整数，obj_x(n)为障碍物X轴坐标值，obj_y(n)为障碍物Y轴坐标值，N优选为5。

[0033] S006，对点集W进行最小二乘直线拟合，获得直线L：y=K*x+b。

[0034] S007，求解点集W中的各点到直线L距离的均方根Var：Var= 。

[0035] S008，判断Var大小，若Var大于预设值VarTh（Var>VarTh），则认为点集W不满足计算要求，则重新执行步骤S001，否则，执行步骤S009。预设值VarTh可选自[0.01, 0.03]，优选为0.01。

[0036] S009，确定点O的横坐标O_x=obj_x(N)，纵坐标O_y=obj_y(N)。

[0037] 上述步骤S005至S009实质为计算障碍物最有可能与车辆接触的点O的坐标信息。可以理解的其他实施例中，可以采用其他能确定最有可能与车辆接触的点O的坐标信息。

[0038] S010，计算初始垂直停车位的靠近障碍物的一条长边的斜率K1，本实施例中为长边BC的斜率K1，再获得穿过点O且斜率为-1/K1的第一直线Lo：y=(-1/K1)*(x-O_x)+O_y，以及穿过右后角Pd（第一车角）且斜率为K1的第二直线Lp：y=K1*(x–Pd_x)+Pd_y，计算第一直线Lo和第二直线Lp的交点Cr的坐标信息(Cr_x, Cr_y)。图5为图4中被虚线圆圈22圈住的部分的放大图，可以看出交点Cr形成的过程。

[0039] S011，根据点Cr的坐标信息(Cr_x, Cr_y)以及初始垂直停车位的外边界的坐标信息，计算目标垂直停车位（用A'、B'、C'、D'标记四角点，虚线标记四边）的相对于初始垂直停车位的偏移量。由于X轴大体平行于停车位长边则只需计算Y轴方向偏移量Delt_y=|B_y–Cr_y|即可。

[0040] S012，根据计算得到的偏移量求得目标垂直停车位的四角的坐标信息A'-B'-C'-D'，然后执行步骤S001。

[0041] 由于在测量初始垂直停车位时，对其入口处的短边所在直线的测量较为准确，且通常情况下初始垂直停车位和目标垂直停车位的长边基本平行于坐标系X轴，因此在计算得到初始垂直停车位的偏移量后，可对初始垂直停车位沿入口短边向远离障碍物的方向平移Y轴方向偏移量即可。

[0042] 因此，在可确定点Cr坐标的情况下，可直接根据点O、第一车角以及初始垂直停车位的外边界的坐标信息，计算目标垂直停车位的外边界的坐标信息。本实施例中B'的坐标值为（B_x, Cr_y），又因为Cr_y实质上等于Pd_y，B'的坐标值为（B_x, Pd_y）。也即，步骤S010和S011为在能够获得点O坐标信息下的一种计算车位偏移量的方法，其实质为先获得穿过第一车角（右后角Pd）且斜率为K1的第二直线Lp，再获得第二直线Lp与初始垂直停车位的入口短边AB的交点B'的坐标，然后将初始垂直停车位沿入口短边向远离障碍物的方向平移，使得目标垂直停车位的靠近第一车角的角点位于交点B'，从而获得目标垂直停车位的外边界的坐标信息A'-B'-C'-D'。

[0043] S013，利用右后角Pd（第一车角）的坐标信息(Pd_x, Pd_y)、靠近右后角Pd（第一车角）的初始垂直停车位的入口短边的角点B的坐标信息(B_x, B_y)、以及右后角Pd（第一车角）的超声波传感器SR_RR和LR_RR的探测障碍物距离Dist计算目标垂直停车位相对于初始垂直停车位的Y轴方向的偏移量Delt=|B_y–Pd_y|-sqrt(Dist -|B_x–Pd_x| )，然后执行步骤S012。

[0044] 也即，在无法定位到障碍物的情况下，根据第一车角的超声波传感器探测障碍物距离Dist、第一车角以及靠近第一车角的初始垂直停车位的入口短边的角点的坐标信息，计算目标垂直停车位的外边界的坐标信息。具体利用了Dist，右后角Pd（第一车角）和角点B的坐标值，将Dist作为一直角三角形的斜边的长度，将右后角Pd和角点B的X轴方向的差值作为直角三角形的平行于X轴的直角边的长度，求取平行于Y轴的直角三角形的直角边的长度，并将该长度作为右后角Pd和目标垂直停车位的角点B'在Y轴方向的差值。

[0045] S014，根据车辆车尾边界的中点M1的坐标信息为（M1_x, M1_y, Theta）判断点M1是否位于中间参考线L2或末端参考线L3上或已经进入区域D2（也即位于中间参考线L2和末端参考线L3之间），如果是，从第一车位重定位状态转入第二车位重定位状态，执行步骤S015，否则执行步骤S200。

[0046] S015，定义车辆右后角的超声波传感器SR_RR的探测障碍物距离为DistR，左后角的超声波传感器SR_RL的探测障碍物距离为DistL，则判断（DistL + DistR）是否大于2* DistTh_Up，如果是，执行步骤S016，否则执行步骤S021。

[0047] S016，判断是否DistL> DistTh_Up且DistR> DistTh_Up，如果是则判断可继续泊车，执行步骤S001，否则执行步骤S017。

[0048] S017，判断是否DistL> DistTh_Up且DistR≤DistTh_Up，如果是则判断车辆右侧存在障碍物(请参考图6中扇形的超声波辐射范围)，则执行步骤S018，否则执行步骤S019。

[0049] S018，计算车位调整偏移量为Delt= DistTh_Up–DistR，执行步骤S019。

[0050] S019，判断是否DistL≤DistTh_Up且DistR> DistTh_Up，如果是，则判断车辆左侧存在障碍物，则执行步骤S020，否则执行步骤S001。

[0051] S020，确定需要调整的偏移量为Delt= DistTh_Up–DistL，执行步骤S012。

[0052] S021，确定不可泊入车位并输出提醒信息。

[0053] 步骤S015至S021实质为根据DistR、DistL和车辆的车角超声波传感器探测距离的触发阈值DistTh_Up判断车辆至少一侧是否有足够的位置可供人员上下车，并根据判断结果输出不可泊入车位的提醒信息或计算目标垂直停车位的外边界的坐标信息，以使得泊车结束后车辆至少一侧可供人员下车。可以理解的，可以用其他预设值取代DistTh_Up进行判断，例如根据车辆销售区域考虑到不同人种不同体型或不过地区车辆标准车位不同采用不同数值。只要判断的结果导向车辆至少一侧有足够的空间可供人员上下车即可。

[0054] S200，判断车辆车尾边界的中点M1的坐标信息是否到达末端参考线L3，如果是，泊车结束，否则执行步骤S001。其他实施例的步骤S200中，还可以通过判定车头是否进入车位入口短边之内来确定是否泊车结束，此时不需要判断点M1与末端参考线L3的位置关系，则末端参考线L3可取消。例如，判断车头点M2或Pa和Pb中所有点均进入车位入口短边之内即确定泊车结束，或车头点Pa和Pb在车位入口短边之内且距离入口短边的距离均大于0.1m则泊车结束。

[0055] 通过执行上述垂直泊车方法，可达到如下效果：（1）在车辆泊入区域D1时，可通过障碍定位的方法预测真实车位位置，并得到真实有效的车位，可提高泊车入位准确性，也可改善用户用体验；
（2）在车辆泊入区域D2时，由于垂直停车位两侧车辆倾斜停放，导致车位尾部相对较窄，可利用超声波探测两侧障碍物距离，得到真实有效地车位，并保证泊车入位后，可正常开门下车；
（3）可减小车辆利用超声波定位所带来的车位检测误差，提高泊车入位成功率。

[0056] 如此，车辆在自动或半自动泊车的过程中，不断根据周围障碍物状态进行车位重定位，消除初始车位预估误差，避免车辆泊车入位后，由于车辆与左侧或右侧参考车辆距离较小，从而导致乘客无法正常下车的情况，也可避免在车辆泊车入位的最后一步由于车位误差使得车辆行进过程中，因遇到到障碍物而中断刹停，影响用户体验的情况。

[0057] 在其他实施例中，可以理解的，可将步骤S016和S017对调。

[0058] 其他实施例中，可以理解的，初始垂直停车位的位置（外边界的坐标信息）可由第三方获得，并由车辆的驾驶系统转换为大坐标系x-o-y下的坐标。

[0059] 上述实施例中，考虑到在测量初始垂直停车位时对其入口处的短边所在位置的测量较为准确，因此在计算目标垂直停车位的最终坐标信息时，是直接对初始垂直停车位沿入口短边向远离障碍物的方向平移来实现。可以理解的，在计算出偏移量的情况下，可采用其他算法获得目标垂直停车位的最终坐标。

[0060] 其他实施例中，可省略末端参考线L3，也即无需定义末端参考线L3也可实现本发明方案。

[0061] 其他实施例中，步骤S002中，只要车辆上一个预设的点，只要是位于车辆中轴线（垂直于车辆前轴和后轴的中轴）上，且靠近车头部或车尾部即可，的坐标信息位于起始参考线L1上或已经进入起始参考线L1和中间参考线L2之间的区域即可执行。当该预设的点不是车辆车头边界的中点和车辆车尾边界的中点时，应对照修改起始参考线和中间参考线相对入口短边的位置。

[0062] 上述实施例中，设定在泊车的过程中最先越过起始参考线L1的车辆的车角为右后角Pd（作为第一车角）。可以理解的，当第一车角为左后角或左前角或右前角时，只要对应调整垂直泊车方法中与真正的“第一车角”对应的车角及靠近该“第一车角”或其对应的初始垂直停车位和目标垂直停车位的角点即可。例如，当第一车角为左后角时，将步骤S003中的“判断右后角Pd（第一车角）的超声波传感器SR_RR和LR_RR的探测障碍物距离”修改为“判断左后角Pc（第一车角）的超声波传感器SR_RL和LR_RL的探测障碍物距离”；将步骤S004中的“超声波传感器SR_RR及LR_RR”修改为“超声波传感器SR_RL及LR_RL”；将步骤S010中计算长边BC的斜率，修改为“计算长边AD的斜率”，将“穿过右后角Pd（第一车角）且斜率为……”修改为“穿过左后角Pc（第一车角）且斜率为……”；将步骤S011中“特别是右后角Pd（第一车角）的坐标信息(Pd_x, Pd_y)”修改为“特别是左后角Pc（第一车角）的坐标信息(Pc_x, Pc_y)”；将步骤S013中的“利用右后角Pd（第一车角）的坐标信息(Pd_x, Pd_y)、靠近右后角Pd（第一车角）的初始垂直停车位的入口短边的角点B的坐标信息(B_x, B_y)、以及右后角Pd（第一车角）的超声波传感器SR_RR和LR_RR的探测障碍物距离Dist计算目标垂直停车位相对于初始垂直停车位的偏移量Delt=|B_y–Pd_y|-sqrt(Dist -|B_x–Pd_x| )”修改为“利用左后角Pc（第一车角）的坐标信息(Pc_x, Pc_y)、靠近左后角Pc（第一车角）的初始垂直停车位的入口短边的角点A的坐标信息(A_x, A_y)、以及左后角Pc（第一车角）的超声波传感器SR_RL和LR_RL的探测障碍物距离Dist计算目标垂直停车位相对于初始垂直停车位的偏移量Delt=|A_y–Pc_y|-sqrt(Dist -|A_x–Pc_x| )”即可。但步骤S014至步骤S020中的参数不随第一车角变换为左后角或左前角或右前角而变化。

[0063] 在本发明的描述中，需要理解的是，术语诸如“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

[0064] 此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

[0065] 虽然对本发明的描述是结合以上具体实施例进行的，但是，熟悉本技术领域的人员能够根据上述的内容进行许多替换、修改和变化、是显而易见的。因此，所有这样的替代、改进和变化都包括在附后的权利要求的精神和范围内。