本发明涉及一种基于视觉与超声波感知融合的自动泊车方法，包括下列步骤：视觉车位检测，超声波雷达车位检测，两种车位检测结果融合，车位跟踪，车位显示；确定目标泊车位，结合当前场景地图，规划泊车速度及泊车轨迹；根据视觉检测以及超声波雷达检测，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，并且对障碍物进行运动估计；计算自身车辆和障碍物发生碰撞的可能性，若碰撞可能性大于预设阈值，则触发避障功能；通过低速控制器进行泊车轨迹跟踪，实现沿泊车轨迹的档位控制、车速控制、方向盘转向控制和避障控制，完成自动泊车。本发明提升车辆周围车位以及障碍物检测效果，提升鲁棒性，增加运动障碍物轨迹估计模块，提升避障可靠性。
1.一种基于视觉与超声波感知融合的自动泊车方法，其特征在于包括下列步骤：①检测泊车位：视觉车位检测，超声波雷达车位检测，两种车位检测结果融合，车位跟踪，车位显示；
②泊车轨迹规划：确定目标泊车位，结合当前场景地图，规划泊车速度及泊车轨迹；
③可行驶区域检测：根据视觉检测以及超声波雷达检测，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，并且对障碍物进行运动估计；
④判断是否避障：计算自身车辆和障碍物发生碰撞的可能性，若碰撞可能性大于预设阈值，则触发避障功能；
⑤泊车轨迹跟踪：通过低速控制器进行泊车轨迹跟踪，实现沿泊车轨迹的档位控制、车速控制、方向盘转向控制和避障控制，完成自动泊车;所述的步骤⑤中方向盘转向控制，采用前馈结合反馈控制进行轨迹跟踪，目标方向盘转角为：其中， 为规划路径的曲率所对应的静态标定车辆前轮转角； 为航向偏
差补偿项，K1为控制增益， 为横摆角积分测量值， 为路径上行驶点处理想横摆角估计值； 为路径偏差补偿项，K2为控制增益， 为实际行驶路线与目标路径在车辆坐标系下的侧向偏差值。
2.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤①包括下列步骤：（1.1）视觉车位检测：在左右环视原始鱼眼场景输入下，利用泊车位检测卷积神经网络进行泊车位角点检测，将原始视角下检测结果按环视系统标定参数，逆透视变换至地面坐标系下，按预定义车位宽度或长度进行泊车位角点匹配；对成功匹配的角点所生成的车位候选区域，利用车位可用性分类卷积神经网络进行初步车位可泊性判断；最终在视觉车位寄存器中存放包括车位ID、车位入口中点坐标和车位长度及车位宽度的车位信息；
（1.2）超声波雷达车位检测：基于侧向超声波雷达、车速以及横摆角速度信号输入，通过行驶里程积分生成车辆两侧可行空间栅格；车辆坐标系下，更新车辆两侧可行驶区域边界坐标，车辆右侧可行驶区域边界坐标更新如下：其中， 为横摆角速度，为车速， 为超声波雷达传感器测量值， 为超声波雷达传感器与前轴的纵向位置偏移量, W为车辆宽度；
（1.3）车位检测结果融合：若视觉系统与超声波雷达系统均正常工作，将步骤（1.1）中视觉系统检测所得的可泊车位与步骤（1.2）中超声波雷达系统检测所得可行空间格栅进行融合，对于每一个车位可泊性 的计算方式如下：其中， 为可行空间格栅输出与输入映射关系，输入为地面坐标系下坐标，输出为二进制区域可行值；a为搜索采样步长；若P大于预设阈值P0，则该车位为可泊车位；反之，则为不可泊车位；
（1.4）车位跟踪：利用自身车辆运动学信息，更新车辆坐标系原点位置( , )：对于任意一个车位，利用俯视平面重合度，计算其时序关联性 ：
若 大于预设阈值，则该车位的生命周期累加1，若某车位生命周期大于预设阈值，则确认为真实存在车位；当车位处于感知系统测量范围内时，采用如下公式更新：其中， 为当前时刻时序跟踪后的车位输出结果， 为
当前时刻车位测量结果， 为上一时刻时序跟踪后的车位输出结
当车位超出感知系统测量范围时，则根据车辆运动学信息，按最后检测到车位的历史位置进行更新；
（1.5）车位显示：在自动泊车控制器上显示车位及车位信息，车位信息包括车位编号、车位入口中点坐标、车位长度及宽度、车位属性。
3.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤②中，泊车轨迹规划方法为：对于侧方位泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-圆弧-直线的轨迹规划方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线的轨迹方案；
对于入库泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-直线的轨迹方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线-圆弧-直线的轨迹规划方案。
4.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤③包括下列步骤：根据视觉检测以及超声波雷达检测，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，利用低速车辆运动学模型，根据轮速以及车辆横摆角速度信号，实时更新车辆坐标系信息，在世界坐标系下将视觉系统与超声波雷达系统所生成的鸟瞰可行驶区域视图取交集进行融合；并且对障碍物进行运动估计，障碍物的运动轨迹预测方法为：基于匀速运动学模型，利用卡尔曼滤波对障碍物的运动进行估计，公式如下：其中，( )为障碍物的相对位置，( )为障碍物的相对速度。
5.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤④中计算自身车辆和障碍物发生碰撞的可能性的方法为：计算自身车辆和障碍物的预碰撞时间TTC：其中，Di为自身车辆到障碍物i的距离，vi为自身车辆与障碍物i的相对速度；若TTCi小于预设阈值，则向自动泊车控制器发送避障控制触发信号，车辆按指定控制算法进行障碍物避让。
6.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤⑤中车速控制方法为：在启动的工况下 ，即v-3＜-0 .1km/h且 时 ，目标车速以及目标加速度 发送给汽车电机控制单元;
在稳态工况下，即|v-3|＜0.1km/h且 时，目标车速 以及目标加速度 发送给汽车电机控制单元；
在减速的工况下，即v-3＜-0 .1km/h且 时，目标车速
7.根据权利要求1或6所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤⑤中档位控制方法为：若目标车速大于零，则置于D档；若目标车速小于零，则置于R档；自动泊车结束时，将档位切换为P档。
8.根据权利要求1或6所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于所述的步骤⑤中避障控制方法，采用纵向定减速制动方式进行避障操作：将目标车速以及目标加速度 发送给制动控制单元。
9.根据权利要求1所述的一种基于视觉与超声波感知融合的自动泊车方法，其特征在于包括包含泊车位显示、泊车位选择和泊车过程提示的泊车交互方法，其中泊车过程提示方法为：获取并解析CAN总线上各超声波雷达信号，按障碍物和车辆之间的距离远近，分远、中、近三档在自动泊车过程中实时显示车辆周围障碍物，当近档障碍物和车辆之间的距离小于预设阈值时发出声音告警以提示操作者。
一种基于视觉与超声波感知融合的自动泊车方法
技术领域
[0001] 本发明涉及汽车自动驾驶技术领域，尤其涉及一种基于视觉与超声波感知融合的自动泊车方法。
背景技术
[0002] 智能化是如今汽车行业发展的重要趋势之一，预计2020～2030期间智能驾驶技术与系统将在世界范围内快速发展。目前自动驾驶技术与系统的研究与开发大多基于高速公路与泊车两种应用场景。基于泊车场景的辅助驾驶应用在汽车行业内已经发展多年，功能应用从最初的碰撞预警发展到后来的泊车辅助、泊车引导以及自动泊车；系统形态从最初的泊车雷达系统发展到后来的泊车影像系统以及近期的车载环视系统。
[0003] 现有泊车相关智能系统以泊车辅助系统为主，泊车辅助系统自动化程度较低，其作用仅限于在泊车过程中警示驾驶者相应潜在危险状况。车辆操作需要完全由驾驶者掌控，无法从本质上降低泊车场景驾驶负担。
[0004] 目前已有极少量产车型具备自主泊车能力，大部分此类系统基于超声波雷达系统的车位检测以及目标探测结果，进行相应的泊车操作。然而，超声波雷达传感器存在测量距离短、相邻传感器之间易产生信号干涉等缺点，并且无法区别障碍物尺寸与类别。因此，其在某些场景下无法准确获得有效泊车位信息（如泊车位区域有暗坑或泊车位区域为出入通道等），应用场景具有一定的局限性，需要驾驶员人为确认泊车区域的正确性。其余部分此类系统综合考虑环视以及超声波雷达系统输入进行泊车位检测，此类系统相较于单独基于超声波雷达车位感知的系统，车位检测的准确性显著提升。然而已有融合方法需视觉与超声波雷达两系统都正常工作的情况下才能正常进行相应泊车位检测（如视觉进行泊车位模板检测、超声波雷达进行泊车位区域障碍物探测），不具备多模态感知系统潜在的感知鲁棒性。此外，现有系统未对移动障碍物轨迹进行预测，因而达不到很好的避障效果，影响自动泊车的顺利完成。
发明内容
[0005] 本发明为了解决上述技术问题，提供一种基于视觉与超声波感知融合的自动泊车方法，视觉系统与超声波雷达系统均可独立进行自动泊车过程中的泊车位检测以及障碍物探测。在两系统正常工作的情况下，能够有效进行系统输出融合；在单一系统异常的情况下，剩余系统仍能够独立工作，从而提升自动泊车的可靠性与鲁棒性。
[0006] 本发明的另一目的是，提供一种基于视觉与超声波感知融合的自动泊车方法，增加车位跟踪模块，从而支持多可泊车位选择；增加运动障碍物轨迹估计模块，提升避障可靠性，确保自动泊车顺利地、可靠地完成。
[0007] 本发明的上述技术问题主要是通过下述技术方案得以解决的：本发明一种基于视觉与超声波感知融合的自动泊车方法，包括下列步骤：
[0008] ①检测泊车位：视觉车位检测，超声波雷达车位检测，两种车位检测结果融合，车位跟踪，车位显示；
[0009] 泊车位检测，基于视觉系统以及超声波雷达系统输入，进行泊车位检测，并根据自身车辆运动学轨迹，对泊车位进行时序跟踪，输出包括潜在可停车位（空车位）以及不可停车位（被占车位）。
[0010] ②泊车轨迹规划：确定目标泊车位，结合当前场景地图，规划泊车速度及泊车轨迹；
[0011] 基于潜在可停车位检测结果输入，计算该泊车位所对应的泊车代价函数，输出可停车位以及相应所规划的泊车轨迹。
[0012] ③可行驶区域检测：根据视觉检测以及超声波雷达检测，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，并且对障碍物进行运动估计；
[0013] 基于视觉系统以及超声波雷达系统输入，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，并且对障碍物进行运动估计，输出传入避障模块进行相应车辆避障操作。
[0014] ④判断是否避障：计算自身车辆和障碍物发生碰撞的可能性，若碰撞可能性大于预设阈值，则触发避障功能；
[0015] ⑤泊车轨迹跟踪：通过低速控制器（PID）进行泊车轨迹跟踪，实现沿泊车轨迹的档位控制、车速控制、方向盘转向控制和避障控制，完成自动泊车。
[0016] 在自动泊车过程中自动泊车系统与车辆驾驶者交互必要系统信息，包括车位显示、泊车位选取、泊车功能触发、系统状态交互和人为操作干预等。
[0017] 本发明深度融合超声波雷达系统与视觉系统探测特性，提升车辆周围的车位以及障碍物的检测效果，确保两系统能独立支撑系统完成自主泊车任务，从而提升系统在部分感知器件故障情况下的鲁棒性；增加车位跟踪模块，从而支持多可泊车位选择；增加运动障碍物轨迹估计模块，提升避障可靠性，确保自动泊车顺利地、可靠地完成。
[0018] 作为优选，所述的步骤①包括下列步骤：
[0019] （1.1）视觉车位检测：在左右环视原始鱼眼场景输入下，利用泊车位检测卷积神经网络进行泊车位角点检测，将原始视角下检测结果按环视系统标定参数，逆透视变换至地面坐标系下，按预定义车位宽度或长度进行泊车位角点匹配；对成功匹配的角点所生成的车位候选区域，利用车位可用性分类卷积神经网络进行初步车位可泊性判断；最终在视觉车位寄存器中存放包括车位ID、车位入口中点坐标和车位长度及车位宽度的车位信息；
[0020] （1.2）超声波雷达车位检测：基于侧向超声波雷达、车速以及横摆角速度信号输入，通过行驶里程积分生成车辆两侧可行空间栅格；车辆坐标系下，更新车辆两侧可行驶区域边界坐标，车辆右侧可行驶区域边界坐标更新如下：
[0021]
[0022]
[0023]
[0024] 其中，为横摆角速度，为车速， 为超声波雷达传感器测量值， 为超声波雷达传感器与前轴的纵向位置偏移量, W为车辆宽度；
[0025] （1.3）车位检测结果融合：若视觉系统与超声波雷达系统均正常工作，将步骤（1.1）中视觉系统检测所得的可泊车位与步骤（1.2）中超声波雷达系统检测所得可行空间格栅进行融合，对于每一个车位可泊性 的计算方式如下：
[0026]
[0027] 其中, 为可行空间格栅输出与输入映射关系，输入为地面坐标系下坐标，输出为二进制区域可行值；a为搜索采样步长；若P大于预设阈值P0，则该车位为可泊车位；反之，则为不可泊车位；
[0028] （1.4）车位跟踪：利用自身车辆运动学信息，更新车辆坐标系原点位置( , )：
[0029]
[0030]
[0031]
[0032] 对于任意一个车位，利用俯视平面重合度，计算其时序关联性 ：
[0033]
[0034] 若 大于预设阈值，则该车位的生命周期累加1，若某车位生命周期大于预设阈值，则确认为真实存在车位；当车位处于感知系统测量范围内时，采用如下公式更新：
[0035]
[0036] 其中 ， 为当前时刻时 序跟踪后的 车位输出结果 ，为当前时刻车位测量结果， 为上一时刻时序跟
踪后的车位输出结果，k1和k2为可配置参数；
[0037] 当车位超出感知系统测量范围时，则根据车辆运动学信息，按最后检测到车位的历史位置进行更新；
[0038] （1.5）车位显示：在自动泊车控制器上显示车位及车位信息，车位信息包括车位编号、车位入口中点坐标、车位长度及宽度、车位属性。
[0039] 本技术方案支持多可泊车位选择，通过深度融合超声波雷达系统与视觉系统探测特性，提升车辆周围车位检测效果，确保两系统能独立支撑系统完成自主泊车任务，同时提升系统在部分感知器件故障情况下的鲁棒性。
[0040] 作为优选，所述的步骤②中，泊车轨迹规划方法为：
[0041] 对于侧方位泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-圆弧-直线的轨迹规划方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线的轨迹方案；
[0042] 对于入库泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-直线的轨迹方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线-圆弧-直线的轨迹规划方案。
[0043] 作为优选，所述的步骤③包括下列步骤：根据视觉检测以及超声波雷达检测，进行车辆周围可行驶区域以及可行驶区域边缘障碍物检测，利用低速车辆运动学模型，根据轮速以及车辆横摆角速度信号，实时更新车辆坐标系信息，在世界坐标系下将视觉系统与超声波雷达系统所生成的鸟瞰可行驶区域视图取交集进行融合；并且对障碍物进行运动估计，障碍物的运动轨迹预测方法为：基于匀速运动学模型，利用卡尔曼滤波对障碍物的运动进行估计，公式如下：
[0044]
[0045]
[0046]
[0047]
[0048] 其中，( )为障碍物的相对位置，( )为障碍物的相对速度。
[0049] 本技术方案深度融合超声波雷达系统与视觉系统探测特性，确保两系统能独立支撑泊车系统，提升在部分感知器件故障情况下的鲁棒性，提高车辆周围障碍物检测效果。能够准确的估计场景中低速运动物体的运动轨迹，从而更有效的实现避障功能。
[0050] 作为优选，所述的步骤④中计算自身车辆和障碍物发生碰撞的可能性的方法为：计算自身车辆和障碍物的预碰撞时间TTC：
[0051]
[0052] 其中，Di为自身车辆到障碍物i的距离，vi为自身车辆与障碍物i的相对速度；若TTCi小于预设阈值，则向自动泊车控制器发送避障控制触发信号，车辆按指定控制算法进行障碍物避让。
[0053] 作为优选，所述的步骤⑤中车速控制方法为：
[0054] 在启动的工况下，即v-3＜-0.1km/h且 时，目标车速以及目标加速度 发送给汽车电机控制单元;
[0055] 在稳态工况下，即|v-3|＜0.1km/h且 时，目标车速 以及目标加速度 发送给汽车电机控制单元；
[0056] 在减速的工况下，即v-3＜-0.1km/h且 时，目标车速以及目标加速度 发送给制动控制单元。
[0057] 作为优选，所述的步骤⑤中方向盘转向控制，采用前馈结合反馈控制进行轨迹跟踪，目标方向盘转角为：
[0058]
[0059] 其中， 为规划路径的曲率所对应的静态标定车辆前轮转角； 为航向偏差补偿项，K1为控制增益， 为横摆角积分测量值， 为路径上该点处理想横摆角估计值； 为路径偏差补偿项，K2为控制增益， 为实际行驶路线与目标路径在车辆坐标系下的侧向偏差值。
[0060] 作为优选，所述的步骤⑤中档位控制方法为：若目标车速大于零，则置于D档；若目标车速小于零，则置于R档；自动泊车结束时，将档位切换为P档。
[0061] 作为优选，所述的步骤⑤中避障控制方法，采用纵向定减速制动方式进行避障操作：将目标车速 以及目标加速度 发送给制动控制单元。
[0062] 沿泊车轨迹自动完成档位切换、车速控制、方向盘转向控制和避障控制，顺利完成自动泊车。
[0063] 作为优选，本发明还包括包含泊车位显示、泊车位选择和泊车过程提示的泊车交互方法，其中泊车过程提示方法为：获取并解析CAN总线上各超声波雷达信号，按障碍物和车辆之间的距离远近，分远、中、近三档在自动泊车过程中实时显示车辆周围障碍物，当近档障碍物和车辆之间的距离小于预设阈值时发出声音告警以提示操作者。及时提醒驾驶员可能发生的潜在碰撞危险，提高自动泊车的可靠性。泊车交互可通过车载控制终端实现，亦可通过移动控制终端实现，交互界面共分三部分，即车位选择界面、泊车过程界面以及泊车完成界面。本技术方案优化自动泊车系统的交互方式与交互内容，从而改善自动泊车系统的用户使用体验。
[0064] 本发明的有益效果是：深度融合超声波雷达系统与视觉系统探测特性，提升车位以及车辆周围障碍物检测效果；确保两系统能独立支撑系统完成自主泊车任务，从而提升自动泊车系统在部分感知器件故障情况下的鲁棒性；增加车位跟踪模块，从而支持多可泊车位选择；增加运动障碍物轨迹估计模块，提升避障可靠性；优化系统交互方式与内容，改善用户使用体验。
附图说明
[0065] 图1是本发明中汽车的一种俯视结构示意图。
[0066] 图2是本发明的一种流程图。
[0067] 图3是本发明中卷积神经网络的一种网络架构示意图。
具体实施方式
[0068] 下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
[0069] 实施例：本实施例的一种基于视觉与超声波感知融合的自动泊车方法，能够在特定场景下完成泊车位搜索并按照使用者所选择泊车位完成自动泊车操作，所基于的系统硬件架构如图1所示，主要包括360°环视系统、超声波雷达系统、自动泊车控制器、视觉协处理器以及执行机构。其中，环视系统共包含4个鱼眼相机，各相机水平视场角为180°，通过HD-CVI协议（或LVDS）传输到视觉协处理器（可与自动泊车控制器集成为一体）中，经处理后将相关信息（包括视觉泊车位、目标泊车位以及可行驶区域）送入自动泊车控制器。超声波雷达系统共包含12个超声波雷达传感器，各传感器视场角为60°，最远探测距离为5米，信号由CAN总线传输到自动泊车控制器内与视觉结果融合经轨迹规划模块生成相应泊车轨迹。自动泊车控制器所输出的车辆控制信号分别通过电机控制器（MCU）、整车控制器（VCU）、制动系统控制器（ESC）以及转向系统控制器（EPS）发送至相应执行机构。
[0070] 基于视觉与超声波感知融合的自动泊车方法，如图2所示，输入为环视系统场景图像、超声波雷达系统信号以及轮速、方向盘转角传感器，输出为目标车速、目标加速度、目标方向盘转角以及档位控制信号，具体包括下列步骤：
[0071] ①泊车位检测：基于视觉系统与超声波雷达系统输入，实时检测车辆周围指定区域泊车位并依据车辆自身运动，跟踪历史泊车位检测结果轨迹；主要包括基于视觉输入泊车位检测、基于超声波雷达信号泊车位检测以及泊车位结果融合与跟踪。详细内容如下：
[0072] （1.1）视觉车位检测：在左右环视原始鱼眼场景输入下，如图3所示，利用泊车位检测卷积神经网络进行泊车位角点检测，将原始视角下检测结果按环视系统标定参数，逆透视变换至地面坐标系下，按预定义车位宽度或长度进行泊车位角点匹配；对成功匹配的角点所生成的车位候选区域，利用车位可用性分类卷积神经网络（如图3所示）进行初步可泊性判断（即判断车位是否被占据），最终在视觉车位寄存器中存放如下车位信息：车位ID，车位入口中点坐标（X0, Y0）, 车位长度和车位宽度（W，H）；
[0073] （1.2）超声波雷达车位检测：基于侧向超声波雷达、车速以及横摆角速度信号输入，通过行驶里程积分生成车辆两侧可行空间栅格。车辆坐标系下，车辆两侧可行驶区域边界坐标更新如下（以车辆右侧为例）：
[0074]
[0075]
[0076]
[0077] 其中， 为横摆角速度（可由ESC提供或由方向盘转角推算），为车速， 为超声波雷达传感器测量值， 为超声波雷达传感器与前轴的纵向位置偏移量, W为车宽。
[0078] 若视觉系统故障，则生成车辆两侧可行空间栅格后，利用预设泊车位几何限制（宽度、深度限制）以及相邻参考车辆姿态（利用检测到的参考车宽度判断车辆姿态），生成可行空间相应泊车位信息（X0, Y0, W, H）。若视觉系统正常工作，则跳过此段所述步骤。
[0079] （1.3）车位检测结果融合：主要包括视觉系统与超声波雷达系统车位检测结果匹配以及其他限制条件可泊车位判断。
[0080] 若视觉系统与超声波雷达系统均正常工作，将（1.1）中视觉系统检测所得的可泊车位与（1.2）中超声波雷达系统检测所得可行空间格栅进行融合，对于每一个车位可停性的计算方式如下：
[0081]
[0082] 其中， 为可行空间格栅输出与输入映射关系，输入为地面坐标系下坐标，输出为二进制区域可行值（1代表可行，0代表不可行）；a为搜索采样步长。若P大于预设阈值P0，则该车位为可泊车位；反之，则为不可泊车位。
[0083] 若超声波雷达系统故障，则将（1.1）中所判断的初步可泊车位作为最终可泊车位进行后续判断。融合后的车位结果经其他限制条件判断后（可选，与泊车规划控制算法模块相关联），方可最终确认为可泊车位。判断条件主要包括泊车位与自身车辆的距离以及角度限制、道路两侧障碍物与自身车辆的距离限制、最小车位宽度限制以及最小车位深度限制等。
[0084] （1.4）车位跟踪：利用自身车辆运动学信息，更新车辆坐标系原点位置( , )：
[0085]
[0086]
[0087]
[0088] 对于任意一个车位，利用俯视平面重合度，计算其时序关联性 ：
[0089]
[0090] 若 大于预设阈值，则该车位的生命周期累加1，若某车位生命周期大于预设阈值，则确认为真实存在车位。当车位处于感知系统测量范围内时，采用如下公式更新：
[0091]
[0092] 其中 ， 为当前时刻时序跟踪后的车位输出结果，为当前时刻车位测量结果， 为上一时刻
时序跟踪后的车位输出结果，k1和k2为可配置参数默认值为0.2与0.8。
[0093] 当车位超出感知系统测量范围时，则根据车辆运动学信息，按最后检测到车位的历史位置进行更新。
[0094] （1.5）车位结果输出：一方面显示车位信息用于与用户交互，另一方面输出车位信息送入后续轨迹规划算法模块，输出泊车位寄存器空间上限20个，每个车位信息包括车位编号（ID）、车位入口中点坐标（X0, Y0）、车位长度和宽度（W，H）、车位占用属性（0-已占据，1-未占但不可泊， 2-可泊）。上述信息通过CAN总线与其他模块交互，每个CAN信息包含1个车位详细信息。
[0095] ②泊车轨迹规划：可泊车位经车载中控系统显示展示给驾驶者，由驾驶者选择目标泊车位并确认后，返回目标泊车位位置与当前场景下的可行区域格栅地图一同作为轨迹规划模块输入。泊车轨迹规划主要包含两部分，即泊车速度规划以及泊车路径（轨迹）规划：
[0096] 泊车速度规划：采用预设目标车速的规划方案，综合考虑感知系统探测时间以及系统响应迟滞时间，默认目标车速设置为3km/h。
[0097] 泊车路径规划：可采用基于规则的规划方法（如圆弧直线法）或基于机器学习的规划方法（如强化学习）等方法。本实施例采用直线与圆弧的路径组合方式，规划泊车起始点至泊车终点间的轨迹：
[0098] 对于侧方位泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-圆弧-直线的轨迹方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线的轨迹方案；
[0099] 对于入库泊车：若所选车位入口与泊车起点的侧向距离大于2.5m，采用直线-圆弧-直线的轨迹方案；若所选车位入口与泊车起点的侧向距离小于2.5m，采用直线-圆弧-直线-圆弧-直线的轨迹方案。
[0100] ③可行驶区域检测：利用视觉系统以及超声波雷达系统进行场景动态目标以及可行驶区域检测，用于后续避障算法模块。
[0101] （3.1）视觉可行驶区域检测：利用多任务卷积神经网络（如图3所示）进行场景可行驶区域以及可移动目标联合分析。输入为4个车载环视相机RGB图像，输出为车辆周围可行驶区域以及可移动目标序列。
[0102] （3.2）超声波雷达可行驶区域检测：同（1.2）。
[0103] （3.3）可行域融合：利用低速车辆运动学模型，根据轮速以及车辆横摆角速度信号输入，实时更新车辆坐标系信息。由于超声波雷达系统与视觉系统采集频率不同，在世界坐标系下将（3.1）与（3.2）所生成的鸟瞰可行驶区域视图取交集进行融合。
[0104] （3.4）障碍物运动轨迹预测：基于匀速运动学模型，利用卡尔曼滤波对障碍物的运动进行估计，公式如下：
[0105]
[0106]
[0107]
[0108]
[0109] 其中，( )为障碍物的相对位置，( )为障碍物的相对速度。
[0110] ④障碍物避让：根据车辆周围障碍物运动轨迹预测结果，计算自身车辆和障碍物发生碰撞的可能性，通过计算自身车辆和障碍物的预碰撞时间TTC实现：
[0111]
[0112] 其中，Di为自身车辆到障碍物i的距离，vi为自身车辆与障碍物i的相对速度。若TTCi小于预设阈值，则向自动泊车控制器发送避障控制触发信号，车辆按指定控制算法进行障碍物避让。
[0113] ⑤泊车轨迹跟踪：根据步骤②中泊车轨迹表达方式，利用纵向车速控制器以及横侧向路径控制器对泊车轨迹进行跟踪。
[0114] （5.1） 泊车过程车速控制：由于自身车辆车速为3km/h，故对于起停工况采用定加减速控制，稳态工况采用定速控制。
[0115] 在启动的工况下，即v-3<-0.1km/h且 时，目标车速以及目标加速度 发送给电机控制单元（MCU）;
[0116] 在稳态工况下，即|v-3|<0.1km/h且 时，目标车速 以及目标加速度 发送给电机控制单元（MCU）；
[0117] 在减速的工况下，即v-3<-0.1km/h且 时，目标车速以及目标加速度 发送给制动控制单元（ESC）。
[0118] （5.2） 泊车过程方向盘转向控制：采用前馈结合反馈控制进行路径跟踪，输出的方向盘转角包括前馈项、路径偏离补偿项以及姿态偏离补偿项，具体如下：
[0119]
[0120] 其中， 为规划路径的曲率所对应的静态标定车辆前轮转角； 为航向偏差补偿项，K1为控制增益， 为横摆角积分测量值， 为路径上该点处理想横摆角估计值； 为路径偏差补偿项，K2为控制增益， 为实际行驶路线与目标路径在车辆坐标系下的侧向偏差值，可由（1.4）中的运动学公式计算所得。
[0121] （5.3） 档位切换：若目标车速大于零，则置于D档；若目标车速小于零，则置于R档；自动泊车结束时，将档位切换为P档。档位自动切换通过将目标档位信号发送给VCU实现，在车速非零时，档位切换信号失效。
[0122] （5.4）避障控制：对于步骤④中给出的避障触发信号，采用纵向定减速制动方式进行避障操作：即将目标车速 以及目标加速度 发送给制动控制单元（ESC）。
[0123] 整个自动泊车过程中进行泊车交互，泊车交互可通过车载控制终端实现，亦可通过移动控制终端实现，交互界面共分三部分，即车位选择界面、泊车过程界面以及泊车完成界面，主要包括泊车位显示、泊车位选择以及泊车过程提示模块。详细内容如下：
[0124] 泊车位显示：获取CAN总线上车位信息，按定义协议解析车位结构化数据。在自身车辆坐标系下，绘制最近10米历史轨迹范围周边的车位信息，以不同的方式显示已占车、空闲以及可选三种属性的车位。
[0125] 泊车位选择：向用户提供按键（上、下、左、右）或者触摸屏输入作为泊车位选择接口，用户选择目标泊车位并确认后，将目标泊车位信息通过CAN总线返回自动泊车控制器。
[0126] 泊车过程提示：获取并解析CAN总线上各超声波雷达信号，按障碍物距离远近，分远、中、近三档在自动泊车过程中实时显示车辆周围障碍物情况，当近档障碍物和车辆之间的距离小于预设阈值时发出声音告警以提示操作者。
[0127] 在泊车完成后，交互界面切换至泊车完成界面。
[0128] 本发明的优点在于：（i）.适用于不同场景（标准停车场、道路边泊车位等）的各类车位模板（水平、垂直以及其他车位角度）的自动泊车；（ii）.视觉与超声波雷达系统具备独立支持泊车过程的感知能力，系统能够在单一感知系统失效的情况下鲁棒的工作；（iii）.能够准确的估计场景中低速运动物体的运动轨迹，从而更有效的实现避障功能；（iv）.能够实时显示系统工作状态并提示驾驶员自动泊车过程中的潜在碰撞危险，驾驶者可通过多种方式暂停/接管泊车过程，从而确保系统使用的安全性；（v）.能够自行检测到意外情况发生的轻微碰撞，从而降低因此造成的人员、财产损失。
