CN109733384B - 泊车路径设置方法及系统 - Google Patents

Abstract

本发明公开了一种泊车路径设置方法及系统，其中方法包括：确定目标泊车位信息；获取待泊车辆的泊车初始位姿信息；根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径。本发明不仅能实现实际泊车环境中无需每次在同一泊车起始位姿情况下进行泊车，并且在泊车过程中即使出现扰动偏差，本发明也可以通过更新泊车路径进行有效处置。

Description

泊车路径设置方法及系统

技术领域

本发明涉及自动泊车技术领域，尤其涉及一种泊车路径设置方法及系统。

背景技术

当前，汽车已经成为人们日常生活中便捷的交通工具。然而，对于很多驾驶员特别是新手来说，安全泊车依然具有很大挑战，特别是遇到两侧有障碍物的垂直车位或者侧方车位，泊车过程中极易发生碰撞、刮蹭等事故。目前，自动泊车技术可以大幅度提高泊车的入库率及安全性，其中包括起到辅助人工泊车的半自动泊车系统，或者无人参与的全自动泊车系统。

现有的半自动/全自动泊车系统通常分为三大模块，定位感知模块、运动规划和控制模块、人机交互模块。其中定位感知模块包括超声波模组等，运动规划和控制模块包括线控执行机构等，人机交互模块包括语音和图像界面提示等。但现有技术大多采用依据局部2D信息来定位车位，进而使用固定程式的双圆弧拼接法进行车辆运动规划。

但基于圆弧拼接的路径规划方案只适用于固定起止位置且车辆轨迹控制完全无误差的理想情况下的泊车操作，但实际使用过程中，难以确保每次泊车都是同一初始位置的条件约束，并且导致泊车偏向的因素繁多，而现有技术规划出的路径在车辆泊车过程中出现偏差时则较难调整回来。

发明内容

本发明提供了一种泊车路径设置方法及系统，用以克服上述问题。

本发明采用的技术方案如下：

一种泊车路径设置方法，包括：

确定目标泊车位信息；

获取待泊车辆的泊车初始位姿信息；

根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；

在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径。

可选地，所述在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径包括：

根据专家经验，为所述泊车路径中的各所述中间参考点预设浮动区域坐标及相应的航向角度阈值；

实时检测待泊车辆的当前坐标及当前航向角度；

根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径。

可选地，所述根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径包括：

根据所述当前坐标及所述浮动区域坐标，判断待泊车辆是否进入所述中间参考点的浮动区域；

若否，则确定所述泊车路径无效；若是，则判断所述当前航向角度是否小于或等于所述航向角度阈值；

若否，则确定所述泊车路径无效；若是，则根据所述当前坐标、所述当前航向角度以及所述目标泊车位信息，调整后续所述中间参考点的位置；

利用所述实时位姿信息、所述目标泊车位信息以及调整位置后的所述中间参考点，拟合出新的后续泊车路径。

可选地，

所述目标泊车位信息包括：有效泊车位的目标坐标；

所述确定目标泊车位信息包括：

根据待泊车辆的尺寸，确定有效泊车位；

获取由雷达检测的所述有效泊车位的第一坐标；

获取由拍摄装置检测的所述有效泊车位的第二坐标；

将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标。

可选地，所述将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标包括：

根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

利用所述权值，求取所述第一坐标与所述第二坐标的加权和，并将所述加权和与所述车位坐标累计误差值求和，得到所述目标坐标。

可选地，所述根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径包括：

根据所述目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

利用所述泊车初始位姿信息、所述目标泊车位信息以及所述中间参考点，拟合出多条候选路径；

计算各条所述候选路径的执行代价；

选取执行代价最小的所述候选路径作为所述泊车路径。

可选地，所述计算各条所述候选路径的执行代价包括：

根据所述候选路径中的曲线段的坐标平均变化量以及相应的航向角度平均变化量，得到所述候选路径的执行代价。

一种泊车路径设置系统，包括：

车位确定模块，用于确定目标泊车位信息；

初始位姿获取模块，用于获取待泊车辆的泊车初始位姿信息；

初始泊车路径生成模块，用于根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；

泊车路径更新模块，用于在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径。

可选地，所述泊车路径更新模块具体包括：

标准预设单元，用于根据专家经验，为所述泊车路径中的各所述中间参考点预设浮动区域坐标及相应的航向角度阈值；

实时位姿获取单元，用于实时检测待泊车辆的当前坐标及当前航向角度；

泊车路径更新单元，用于根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径。

可选地，所述泊车路径更新单元具体包括：

浮动区域检测子单元，用于根据所述当前坐标及所述浮动区域坐标，判断待泊车辆是否进入所述中间参考点的浮动区域；

航向角度检测子单元，用于在所述浮动区域检测子单元输出为是时，判断所述当前航向角度是否小于或等于所述航向角度阈值；

中间参考点调整子单元，用于在所述航向角度检测子单元输出为是时，根据所述当前坐标、所述当前航向角度以及所述目标泊车位信息，调整后续所述中间参考点的位置；

后续泊车路径拟合子单元，用于利用所述实时位姿信息、所述目标泊车位信息以及调整位置后的所述中间参考点，拟合出新的后续泊车路径；

路径无效确定子单元，用于在所述浮动区域检测子单元输出为否时或者在所述航向角度检测子单元输出为否时，确定所述泊车路径无效。

可选地，

所述目标泊车位信息包括：有效泊车位的目标坐标；

所述车位确定模块具体包括：

有效车位确定单元，用于根据待泊车辆的尺寸，确定有效泊车位；

第一坐标获取单元，用于获取由雷达检测的所述有效泊车位的第一坐标；

第二坐标获取单元，用于获取由拍摄装置检测的所述有效泊车位的第二坐标；

坐标融合单元，用于将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标。

可选地，所述坐标融合单元具体包括：

累计误差获取子单元，用于根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

预设权值调整子单元，用于根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

目标坐标获取子单元，用于利用所述权值，求取所述第一坐标与所述第二坐标的加权和，并将所述加权和与所述车位坐标累计误差值求和，得到所述目标坐标。

可选地，所述初始泊车路径生成模块具体包括：

中间参考点确定单元，用于根据所述目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

候选路径拟合单元，用于利用所述泊车初始位姿信息、所述目标泊车位信息以及所述中间参考点，拟合出多条候选路径；

代价计算单元，用于计算各条所述候选路径的执行代价；

泊车路径确定单元，用于选取执行代价最小的所述候选路径作为所述泊车路径。

可选地，所述代价计算单元具体用于：根据所述候选路径中的曲线段的坐标平均变化量以及相应的航向角度平均变化量，得到所述候选路径的执行代价。

本发明的构思是以设置至少两个中间参考点的方式规划出具有多个分段线路的泊车路径，具体是先确定目标泊车位信息以及待泊车辆的泊车初始位姿信息，在二者之间生成包含至少两个中间参考点的泊车路径，并且在泊车过程中，根据待泊车辆的实时位姿信息以及与中间参考点相关的预设标准，更新该泊车路径。本发明不仅能实现实际泊车环境中无需每次在同一泊车起始位姿情况下进行泊车，并且在泊车过程中即使出现扰动偏差，本规划方法也可以通过更新泊车路径进行有效处置。

在此基础上，进一步地本发明采用雷达检测和图像检测相融合的手段获取准确的车位信息；且进一步地，对前述至少两个中间参考点的确定，本发明还采用专家经验进行泊车路径采样自适应规划的方式。

附图说明

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步描述，其中：

图1为本发明提供的泊车路径设置方法的实施例的流程图；

图2为本发明提供的泊车位定位方法的实施例的流程图；

图2a为本发明提供的搭载检测设备的待泊车辆的示意图；

图3为本发明提供的第一坐标与第二坐标融合方法的实施例的流程图；

图4为本发明提供的泊车路径生成方法的实施例的流程图；

图5为本发明提供的泊车路径更新方法的实施例的流程图；

图6为本发明提供的步骤S43的具体实施例的流程图；

图7为本发明提供的已设置的泊车路径的实施例的示意图；

图8为本发明提供的泊车路径设置系统的实施例的方框示意图。

附图标记说明：

1双目视觉定位装置 2\4\5\6环视鱼眼摄像头 3超声波雷达

10车位确定模块 20初始位姿获取模块 30初始泊车路径生成模块

40泊车路径更新模块

具体实施方式

下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明提供了一种泊车路径设置方法的实施例，如图1所示，其中可以包括如下步骤：

步骤S1、确定目标泊车位信息；

在规划出泊车路径前，需要明确期待的泊车位置。在自动驾驶领域，通常包括全自动驾驶和辅助驾驶，前述“期待”可以是来自于全自动驾驶的算法，也可以是由辅助驾驶系统提供向驾驶员提供多个候选，由驾驶员从中指定，对此本发明不作限定。但需要说明的是，无论上述何种方式，皆是由机器算出目标泊车位信息，也因此，为了能够获得准确的目标泊车位信息，需要结合现有的汽车感知和联网技术，包括不限于雷达、视觉检测、图像分析以及V2X等。但是现有的单纯基于超声波或者激光雷达的车位定位技术，只适用于泊车位的两侧均有障碍物的情况。据此，本发明提出了一种结合雷达和图像分析技术的泊车位定位方法，如图2所示，该泊车位定位方法可以包括如下步骤：

步骤S11、根据待泊车辆的尺寸，确定有效泊车位；

对于泊车位的定位时机可以发生在进入车场后或驾驶员准备停靠路边时，通过设置触发机制，开启对泊车位的扫描，实施扫描的部件可以采用现有的雷达技术和/或摄像技术，例如通过分布待泊车辆周身的超声波雷达3以及环视鱼眼摄像头2、4、5、6等扫描出泊车位的尺寸，具体的硬件配置可参考图2a所示；但车身检测硬件的构成不是本实施例的重点，本步骤重点在于说明实际操作中，空余的泊车位不一定均适合待泊车辆，因此需要结合待泊车辆固有的尺寸属性，与雷达和/或拍摄装置检测到的泊车位的宽度和长度进行匹配，只有与待泊车辆尺寸匹配的泊车位才可称作“有效”的泊车位，后文各步骤、各实施例等皆是基于该有效泊车位开展。但需指出，虽然确定有效的泊车位是必要前提，但在某些场景中也可以是由驾驶员结合驾驶经验人为确定有效泊车位，之后再开启雷达或拍摄装置进行车位坐标的计算。

步骤S12、获取由雷达检测的有效泊车位的第一坐标；

例如图2a所示，可以通过待泊车辆周身的多个超声波雷达3对有效车位进行扫描，通过判断超声波的上升沿和下降沿的距离间隔从而确定第一坐标，所称第一坐标可以涵盖由雷达检测的有效泊车位的横纵坐标及规格。这里以P1＝[x1,y1,w,h]表示，其中x1、y1分别对应由雷达检测的有效泊车位的中心坐标,w，h则分别对应该有效泊车位的宽度和长度。

步骤S13、获取由拍摄装置检测的有效泊车位的第二坐标；

例如图2a所示，可以通过待泊车辆周身的多个环视鱼眼摄像头2、4、5、6对有效车位采集鸟瞰图，进一步可以通过CNN深度神经网络对泊车位的车位划线的角点进行分析，然后使用点对匹配方法实现车位的检出分类，进而确定第二坐标，所称第二坐标可以涵盖由拍摄装置检测的有效泊车位的横纵坐标及规格。这里以P2＝[x2,y2,w,h]表示，其中x2、y2分别对应由拍摄装置检测的有效泊车位的中心坐标,w，h则分别对应该有效泊车位的宽度和长度。

步骤S14、将第一坐标与第二坐标融合，得到有效泊车位的目标坐标。

最后，将由两种不同原理的检测设备所检测的多维度结果进行融合，从而获得相较采用单一设备检测的车位数据更为准确、可靠的车位定位信息。

关于将两个坐标数据进行融合的具体方式，本发明提供一种优选的融合方案，该融合方案的核心思路是基于泊车位周边的障碍物分布情况，对上述两种设备获得坐标信息进行加权求和操作，但其中还可以进一步包括对车位坐标检测误差的考虑，以此作为两个坐标融合的误差补偿，具体可如图3所示：

步骤S141、根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

步骤S142、根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

步骤S143、利用所述权值，求取第一坐标与第二坐标的加权和，并将该加权和与车位坐标累计误差值求和，得到目标坐标。

如前文所述，现有的单纯基于超声波或者激光雷达的车位定位技术，只适用于泊车位的两侧均有障碍物的情况，因而在该优选实施例中可以分别为两种检测方式预设初始的权重，例如分别设为0.5，针对不同场景的实际障碍物情况，调整该权重，例如对于车位划线不清且具有障碍物的场景时，由摄像头拍摄的鸟瞰图所检测的坐标数据参考性较低，则可以提升雷达的检测结果的权重例如设为0.8，而摄像头的检测结果的权重则为0.2；反之，当针对车位的两侧或一侧的障碍物难以被雷达准确探测到的泊车场景，那么将摄像头的检测结果的可信度相应提升，例如设其权重为0.7，而雷达检测结果的可信度相应地降为0.3。关于权重的具体调整方式，可结合现有的雷达检测技术以及与拍摄装置相关的图像处理技术，并根据两种检测手段的检测结果的偏差波动数据进行建模，再利用模型进行权重的自适应分配。接续上文，再结合上述优选实施例中提及的误差补偿，从而能够得到精确的目标泊车位信息，为后续设置泊车路径提供了可靠的基础条件。上述过程在具体计算时可以但不限于通过下述公式获得：

W＝[ω_x,ω_y,w,h]

其中，α₁为雷达检测结果的权值，α₂为拍摄装置的检测结果的权值；offset1（i,i-1)_x、offset1(i,i-1)_y分别为时刻i与上一时刻i-1雷达检测出的泊车位的横纵坐标方向的累计误差，offset2(i,i-1)_x、offset2(i,i-1)_y为时刻i与上一时刻i-1拍摄装置检测出的泊车位的横纵坐标方向的累计误差；W为将两项累计误差融合后的坐标误差补偿；P为结合了两种设备权重的第一坐标与第二坐标的加权和，并与累计误差值(误差补偿)的最终融合结果，可以表示为P＝[x,y,w,h]，据此得到了t时刻下准确的目标泊车位信息。这里需要说明两点，其一、上述公式中的P1和P2的权重系数，仅为表示归一化的权重分配结果，实际操作中可表达为其他形式；其二、当扫描到多个有效泊车位时，则可以将各泊车位的信息进行储存和展示，由驾驶员或者由机器择优而选。

接续图1所示实施例，步骤S2、获取待泊车辆的泊车初始位姿信息；

为了之后获得有效且可靠的泊车规划路线，上述步骤提供了终点目标的信息，而本步骤则关注的是获得起点的相关信息。众所周知，无论采用全自动驾驶还是有驾驶员干预的自主泊车，泊车路径均与待泊车辆的所处位置和行进线路息息相关，这里所称的位姿信息即可以是指包括表达所处位置的待泊车辆坐标数据以及与行进线路强相关的航向角度(表征待泊车辆的移动方向)。对于设置泊车路径的初始阶段，所获取的即是泊车初始位姿信息，更为可能的是准备泊车前的停车工况下的位姿信息，一方面是结合现实应用中的情况，另一方面是停车工况同时也可以作为触发下述生成泊车路径的启动条件，但本步骤并非限制生成泊车路径一定需要停车，下文中介绍了在行进中进行路径规划的方案。

由于待泊车辆的位姿信息同样是影响后续处理结果的关键数据，因而在一个具体实施方案中，如图2a中所示，可利用但不限于在前挡风玻璃处安装双目视觉定位装置1，并基于SLAM算法予以实现，具体地，可以通过双目视觉定位装置1获取左右相机的两个画面，利用离线标定的相机内外参数矩阵对图像进行实时校正，并拼接为一帧图像，并将给图像转化为灰度图后以避免光照明暗对成像效果的影响，然后经过双目匹配结果进行估算得到当前位置的位姿，以此获得精确的待泊车辆的坐标及航向角度(位姿信息)，尤其是采用了双目与SLAM算法相配合的方案能够准确获得车辆位姿状态信息，因而还可以满足针对不同尺寸车辆的通用性需求。但这里需说明两点：其一、获取待泊车辆的泊车初始位姿信息不限于采用布置在待泊车辆本体上的视觉检测装置，在其他实施例中还可以采用待泊车辆本体之外的外部检测设备，例如能够与待泊车辆(或泊车服务器)通信的路侧监测单元、车场内的摄像头、无人飞行器以及卫星拍摄系统等；其二、本发明不对上述步骤S1和步骤S2的次序进行限定，既可以是先确定目标泊车位后再获取待泊车辆的位姿信息，也可以先获取到待泊车辆的位姿信息再确定目标泊车位信息。

接续前文，步骤S3、根据目标泊车位信息和泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；

当上述两个关键信息(起点和终点)确定后，就可以在二者之间构建泊车路径了，该路径可以满足阿克曼模型非完整性约束，这是对泊车过程中车辆控制的运动规划，因而也可称本步骤为泊车路径规划。现有的技术手段大多是将泊车过程中的车辆时变运动模型简化为纯刚体且无侧滑的时不变运动模型，并使用固定程式的双圆弧拼接法进行路径规划，因此只适用于固定起止位置且车辆轨迹控制完全无误差的理想情况下的泊车操作，但实际使用过程中无法完全满足每次泊车都是同一起止位置的条件约束，在车辆执行泊车操作时，如若出现较大偏差则难以通过对路线进行调整，致使无法准确到达目标位置。尤其地，采用理想圆弧拼接则需要精确计算以确定弧线上的切点所在位置，但这为实际泊车路径规划和按此路径的行车控制均带来较大的操作难度。

为了避免上述问题，本实施例提出了由路径中的多个(至少两个)关键参考点，拟合完整泊车路径的思路。本领域技术人员可以理解的是，本申请强调的多点路径，即是为了便于在中间参考点与中间参考点之间所形成的路径分段中实现局部规划和行车控制，这样在后续泊车过程中由参考点之间的各分段的实际“落实”情况，评判泊车是否到位或偏离，并且即使发生偏离也可以实现分段处置，例如不再关注已“落实”的路径分段，而可以对尚未经过的后续路径分段进行适应性调整，进而能够实现自适应规划泊车路径。

以上皆是由本实施例所称“生成至少两个中间参考点的泊车路径”能够直接或间接获得的技术效果，对于本领域技术人员而言，多点路径的优势和实现过程均不难理解，但为了更好地体现本发明相比现有技术的优势，在此提供一种优选的多点路径生成示例。该优选方案的构思是基于非线性可调得分点(NSP)以及能量代价函数生成多段连续路径，具体是在统计人类驾驶员的泊车路径(专家经验)的基础上，设定非线性变化的浮动得分点(中间参考点)，因此能够在保证车辆曲率连续情况下，同时保证规划结果的抗扰动性和路径点可达性。

归纳为图4所示的实施例，可以包括如下步骤：

步骤S31、根据目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

本实施例的核心在于由多条人类经验路径确定中间参考点，具体操作中可针对不同尺寸的车辆查找预先设置的相应的映射表，该映射表可由包含起点坐标、终点坐标规格以及起点处车辆航向角度的专家经验知识而来，但因为与所述目标泊车位信息、待泊车辆的泊车初始位姿信息相同或近似的专家经验可能较多，因而获得的是多条专家经验路径中的中间参考点。

步骤S32、利用泊车初始位姿信息、目标泊车位信息以及中间参考点，拟合出多条候选路径；

当前述中间参考点确定后，此时有起点信息、有终点信息还有中间各参考点信息，那么待泊车辆的规划路径也就足以确定了，但到此步骤可以得到的是多条候选路径。

步骤S33、计算各条候选路径的执行代价；

在多条候选中必然存在一条针对当前场景的最优路径，因此，在此步骤中本实施例提出通过计算执行代价的方式，识别各候选之间的优劣程度。对于执行代价的计算以及相应的计算基础，可借鉴现有的多种方式。这里本实施例提供一种考虑路径中的曲线段的能量消耗的代价计算策略，简而言之，是预判待泊车辆在候选路径中的曲线段执行泊车作业时，在整个曲线段上的坐标平均变化量以及相应的航向角度平均变化量，得到各候选路径的执行代价。这是因为在一条完整的多段路径中，直线段的规划和控制相对简明直接，仅可能需要考虑直线段的长度即可以方便地获得关于直线段的执行代价；但如果一条完整的泊车路径中，两个中间参考点构成的是具有一定曲度的弧线，那么在实际泊车过程中，从其中一个参考点到另一个参考点的能量消耗就与多种因素相关，例如用户在泊车前期可能会选定与车速、泊车时间相关的泊车模式(舒适泊车模式和快速泊车模式)，或者根据智能算法预测出的车内驾驶员的日常驾驶风格和习惯，自主选择不同侧重需求的泊车执行策略等。据此，在本实施例中以待泊车辆在执行相应曲线段的位姿信息的变化均值作为对所称执行代价的量化依据，例如可以采用下述公式计算每条候选路径的执行代价：

其中，Δx、Δy分别为途径曲线段时横纵坐标的平均变化量，Δθ为相应的航向角度的平均变化量，w₁为根据不同需求预设的权值，也即是如果采用不同泊车模式w₁可以不同，当然，如果不采用任何预设泊车模式，就无需考虑代价侧重点，则在计算执行代价时无需附带上述权值w₁。需说明的是，可以将前文提及的直线段的代价与J_cost融合，但由于直线段相对消耗较小，因此直线段的代价的权值占比可以是最小。

步骤S34、选取执行代价最小的候选路径作为泊车路径。

最后，从能量消耗最小的候选路径选取最优的作为所述泊车路径，这是显而易见的，无需赘言。

再结合图1所示的最后一步，步骤S4、在泊车过程中，根据待泊车辆的实时位姿信息以及与中间参考点相关的预设标准，更新泊车路径。

当由前述步骤获得一条最优的从停车的位置到有效泊车位的泊车路径后，就可以采用全自动泊车或辅助泊车方式，控制待泊车辆按照该泊车路径执行泊车作业。例如将该泊车路径显示在人机交互界面，驾驶员可依需要选择通过语言或者文字提醒方式进行人工泊车操作；或者由车辆的智能操控部件接入控制，通过线控总线等执行方向盘的转角、油门和刹车等动作，自动完成泊车入位的操作。本发明的重点不在于泊车过程中如何对车辆的运行进行控制，而是考虑到在实际操控中会由于多种因素导致待泊车辆不能按照已规划的泊车路径完成泊车，例如电控信号被干扰、受控部件的执行力不足、突发工况导致偏向，特别是由人工执行泊车操作时驾驶员的驾驶经验等个人因素，均可能导致车辆的实际行进路线发生偏差。这对于前述现有的泊车手段则难以解决，最终可能导致车辆所停位置与期待的泊车位置产生较大偏差。据此，本实施例提出可以采用更新泊车路径的方式以解决该问题，这里涉及的被更新的泊车路径是指前述已从多个规划的候选路径中择优而选的泊车路径；而且对于更新泊车路径的依据，本实施例提出依赖于待泊车辆的实时位姿信息以及与中间参考点相关的预设标准。其中，待泊车辆的实时位姿信息可以是前文提到的采用诸如双目SLAM算法获得的精准位姿信息，而这里所称的与中间参考点相关的预设标准，则可以有多种方案予以实现。下文中本发明将提供一种基于专家经验的优选实施示例，当然，多点路径中的多个参考点若是根据其他条件获得，则相应地会有与该条件相匹配的预设标准，这在实际操作中可依需求而定。

具体可如图5所示，包括如下步骤：

步骤S41、根据专家经验，为泊车路径中的各中间参考点预设浮动区域坐标及相应的航向角度阈值；

如上文提及的，本发明可以基于非线性可调得分点(NSP)以及能量代价函数生成多段连续路径，因此在统计人类驾驶员的泊车路径(专家经验)的基础上，能够得到非线性变化的浮动得分点(中间参考点)。因而，本实施例考虑为中间参考点预设浮动区域坐标及相应的航向角度阈值。这里所称浮动区域是由专家经验而来，即给定中间参考点一个浮动范围，车辆如果处于某个中间参考点的浮动范围，并且符合航向角度的要求，在专家经验数据中均能够获得至少一条继续到达目标泊车位的路径，换言之，也即是在泊车规划中提供了一种容错能力，但需指明，这里所称容错能力并不是针对车辆的运行控制，针对的客体是路径本身。继而所称浮动区域坐标是指在确定了容错区域的范围后，该区域的边沿的坐标也就相应确定了，当然，这里所称浮动区域并不限定区域的形状，可能是长方形、可能是椭圆形、可能是扇形等等，计算出的区域形状均可以根据专家经验数据以及实际场景而定，例如计算出的浮动区域是长方形，那么长方形四条边的横纵坐标就能相应确定下来。此外，关于相应的航向角度阈值，这是考虑到在实际操作中，泊车的动作是车辆运动的完整过程，并且航向角度是随着车辆在运动过程中所处的不同的坐标位置而有所变化，因此较为优选地是需要先从坐标位置上考察车辆是否进入到浮动区域内，之后再考察车辆的行驶方向能够符合继续泊车入位的需求，所以在此情形下航向角度的阈值取决于所处具体的位置，因而这里采用“相应的”表述，实际操作中专家经验数据也能够提供与坐标位置相关的航向角度的要求。

步骤S42、实时检测待泊车辆的当前坐标及当前航向角度；

显而易见，待泊车辆的实时位姿决定了后续所更新的泊车路径的指引方向，这里所称实时位姿是指待泊车辆的当前坐标及当前航向角度。

步骤S43、根据当前坐标与浮动区域坐标的关系以及当前航向角度与航向角度阈值的关系，更新泊车路径。

在获得上述两组数据后，至本步骤分别执行比对，用以判定待泊车辆的位置是否符合重新构建的要求，同时，待泊车辆的航向是否符合重新构建的要求，二者缺一不可。当两项指标均满足后，则可以通过更新前述步骤S3获得的泊车路径，采用另一条新的路径，具体关于新的路径的生成方式可以借鉴前文提及的基于专家经验获得相应的中间参考点，将各中间参考点拟合成多段路径，在此不在赘述。但可以于此补充说明的是，所称更新泊车路径，优选是指得到一条与原始规划的泊车路径不同的线路，但不排除在实际操作中车辆能够准确到达各中间参考点并且角度也符合原始路径的要求，则更新后的路径也可以与原始规划的泊车路径重叠，下文中对此还提供了一种不对原始的泊车路径进行更新的实施例，可以根据专家经验，为原始的泊车路径中的各中间参考点预设有效标准，当检测到待泊车辆的实时位姿信息符合该有效标准，则继续沿用原始的泊车路径。这里所称有效标准同样是针对各中间参考点而言，例如可以基于一个更为严格的标准为各中间参考点设定一个“有效区域”以及相应严格的有效航向角度阈值，本领域技术人员可以理解的是，该“有效区域”坐标及有效航向角度阈值必然小于或等于前述“浮动区域”及相应的航向角度阈值，有效即意味着之前的分段路径中待泊车辆到达了期待的位置并具有期待的角度，无需对后续中间参考点进行更新，依然可以按照最初规划的泊车路径继续泊车。这里可以补充的是，前述两套标准——有效和浮动，可以同时应用在整个泊车过程中，一段一段地判定泊车是否有效，如不再有效区域内进而判定是否在浮动区域，并据此可以执行更新路径的操作，以保证准确的泊车入位作业，由此可见，本发明所称浮动区域与有效区域虽然可能存在“子集”关系(有效区域是浮动区域的“子集”)，但在理解上应区分当待泊车辆驶入不同区域后，浮动区域与有效区域分别对后续路径的更新规划所起的作用，浮动即意味着大概率上在该区域内若对后续路径进行更新，新的路径与原始的路径会有所不同；有效即意味着在该区域内若对后续路径进行更新，新的路径在大概率上可能会与原始的路径重叠。本发明在下文中提供了一种具体的如何依据“浮动”标准，即所称根据当前坐标与浮动区域坐标的关系以及当前航向角度与航向角度阈值的关系，更新泊车路径的实施例，如图6所示，可以包括如下步骤：

步骤S431、根据当前坐标及浮动区域坐标，判断待泊车辆是否进入中间参考点的浮动区域；

若否，则执行步骤S5；若是，则执行步骤S432；

步骤S432、判断当前航向角度是否小于或等于航向角度阈值；

若否，则执行步骤S5；若是，则执行步骤S433；

步骤S433、根据当前坐标、当前航向角度以及目标泊车位信息，调整后续中间参考点的位置；

步骤S434、利用实时位姿信息、目标泊车位信息以及调整位置后的中间参考点，拟合出新的后续泊车路径；

步骤S5、确定泊车路径无效。

为便于理解，参考图7所示的路径规划示意图，假设起点(待泊车辆所停位置)位于某个纵向车位的右前方，且起点时车头朝右，待泊车辆期待倒入该纵向车位。设定起点为S，中间参考点为C、B、A，终点也即是该纵向车位的中心为O，那么经由前述步骤，已经在S位置获得了一条最优的S-C-B-A-O的泊车路径，也可以称此为原始泊车路径。而基于专家经验为各个中间参考点为C、B、A设定的均是长方形的浮动区域并附带相应的航向角度阈值(图中未示出)。

在执行泊车作业时，当通过待泊车辆的当前坐标以及C点的浮动区域(此处简称C区)坐标，能够判定出待泊车辆是否已经进入到C区。但在实际操作中，该过程可能具体如下：通过横坐标先判定待泊车辆已进入C区的右边沿，进而通过纵坐标判定待泊车辆是否在C区的上下边沿内，若是则确定待泊车辆进入到C区，当然该过程中如果判定出待泊车辆超出了C区的上下边沿，则表明待泊车辆的偏向已超出任何能倒入O的专家路径的范围，即当前位置已不可能继续执行泊车，因而可以确定前述泊车路径已无效。

如若经上述过程判定在C区内，则还要进一步考察当前航向角度是否在相应的航向角度阈值内。理由已在前文阐明，此处不再赘述。同理地，若不符合航向角度的需求，则表明待泊车辆的位置虽满足后续处理条件，但角度已超出任何能倒入O的专家路径的范围，即以当前角度已不可能继续执行泊车，因而也可以确定此时前述的泊车路径已无效。

进而，如果也满足了航向角度阈值的要求，则表明S-C段的泊车作业有效，但因为前文说明了，本发明侧重关注的情况是泊车偏移的情况，且介绍了C区为浮动区域亦即是容错区域，因而不对车辆完美驶入C点的有效区域进行讨论，在该示例中，由于车辆没有完美驶入C点，而是到达了C点的下方，导致原始的C-B-A-O可能无效或部分无效，因为此时车辆的位置实质上是C’，所以才引出对后续路径进行更新。在本实施例中更新的方式是根据当前坐标、当前航向角度以及目标泊车位信息，调整后续中间参考点的位置，也即是由待泊车辆当前的位姿信息以及目标泊车位信息重新规划路径，实际原理与前述基于泊车初始位姿信息和目标泊车位信息进行泊车路径规划的方式相似，只不过由于前述步骤已经确定了中间参考点及其可浮动的区域，因而在本示例中只需要调整B和/或A的位置，更优选地可以先在其各自的浮动区域内调整，例如B点在B区调整到B’，A点在A区调整到A’；但不排除重新调整位置的中间参考点有可能已位于原浮动区域外，此时可以根据实际的位置重新给定一个浮动区域范围，例如，调整后的B’点在B区外，那么可以基于专家经验知识为新位置上的B’点设置B’区；但具体是否需要对每个后续的中间参考点进行位置调整，则需由实际场景结合专家知识而定。最后，则利用待泊车辆的实时位姿信息、目标泊车位信息以及调整位置后的中间参考点，拟合出新的后续泊车路径，这里需再次指出的是，无论是基于泊车初始位姿信息还是泊车过程中的实时位姿信息，在拟合泊车路径时优选参考起点、终点以及中间参考点各自的坐标和相应的航向角度的要求，对于终点的航向角度的考虑下文中将作说明；最终，该新的后续泊车路径依然是多段的，例如C’-B’-A-O，C’-B-A’-O或C’-B’-A’-O，在某些场景中，也有可能依专家经验相应地增减中间参考点的数量。在图7的示例中，更新后的泊车路径即为C’-B’-A-O。

至此，本发明完成了泊车路径的更新作业，在之后的泊车过程中同理，分段地判定，并分段地实现路径更新，原理皆同，不再赘述。而至于前文提及的当判定出泊车路径无效后(包括在任一中间参考点判定无效)如何克服该问题以重新执行泊车，则不在本发明的技术范畴内，虽可以采用诸如返回起点S或返回上一中间参考点或小范围地原地调整航向角度等等，但本发明均不对此做出限定。

而对于当待泊车辆驶入O点后的停止操作，如提示踩刹车或自动控制刹车，或者根据待泊车辆后方的倒车雷达判断已接近障碍据此执行刹车，也属于对车辆运行的控制范畴，本发明不予限定。但可以说明的是，为了避免泊车后待泊车辆对邻车造成干扰，对于终点O同样也可以预先设定较为严苛的航向角度要求，即最优的泊车结果是，待泊车辆入位后的位姿均符合预设的坐标和角度的要求。

本发明的构思是以设置至少两个中间参考点的方式规划出具有多个分段线路的泊车路径，具体是先确定泊车位的目标泊车位信息以及待泊车辆的泊车初始位姿信息，在二者之间生成包含至少两个中间参考点的泊车路径，并且在泊车过程中，根据待泊车辆的实时位姿信息以及与中间参考点相关的预设标准，更新该泊车路径。本发明不仅能实现实际泊车环境中无需每次在同一泊车起始位姿情况下进行泊车，并且在泊车过程中即使出现扰动偏差，本规划方法也可以及时更新泊车路径。

在此基础上，进一步地本发明采用雷达检测和图像检测相融合的手段获取准确的车位信息；且进一步地，对前述至少两个中间参考点的确定，本发明还采用专家经验进行泊车路径采样自适应规划的方式。

相应于前述各实施例及优选方案，本发明还提供了一种泊车路径设置系统的实施例，如图8所示，该系统可以包括至少一个用于存储相关指令的存储器以及至少一个与所述存储器连接且用于执行下述各模块的处理器(在其他实施例中一个或多个处理器也可以直接执行相应的步骤动作，而无需通过下述模块执行，例如处理器直接执行待泊车辆定位、目标定位、生成路径、更新路径等操作)：

车位确定模块10，用于确定目标泊车位信息；

初始位姿获取模块20，用于获取待泊车辆的泊车初始位姿信息；

初始泊车路径生成模块30，用于根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；

泊车路径更新模块40，用于在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径。

进一步地，所述泊车路径更新模块具体包括：

标准预设单元，用于根据专家经验，为所述泊车路径中的各所述中间参考点预设浮动区域坐标及相应的航向角度阈值；

实时位姿获取单元，用于实时检测待泊车辆的当前坐标及当前航向角度；

泊车路径更新单元，用于根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径。

进一步地，所述泊车路径更新单元具体包括：

浮动区域检测子单元，用于根据所述当前坐标及所述浮动区域坐标，判断待泊车辆是否进入所述中间参考点的浮动区域；

航向角度检测子单元，用于在所述浮动区域检测子单元输出为是时，判断所述当前航向角度是否小于或等于所述航向角度阈值；

中间参考点调整子单元，用于在所述航向角度检测子单元输出为是时，根据所述当前坐标、所述当前航向角度以及所述目标泊车位信息，调整后续所述中间参考点的位置；

后续泊车路径拟合子单元，用于利用所述实时位姿信息、所述目标泊车位信息以及调整位置后的所述中间参考点，拟合出新的后续泊车路径；

路径无效确定子单元，用于在所述浮动区域检测子单元输出为否时或者在所述航向角度检测子单元输出为否时，确定所述泊车路径无效。

进一步地，

所述目标泊车位信息包括：有效泊车位的目标坐标；

所述车位确定模块具体包括：

有效车位确定单元，用于根据待泊车辆的尺寸，确定有效泊车位；

第一坐标获取单元，用于获取由雷达检测的所述有效泊车位的第一坐标；

第二坐标获取单元，用于获取由拍摄装置检测的所述有效泊车位的第二坐标；

坐标融合单元，用于将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标。

进一步地，所述坐标融合单元具体包括：

累计误差获取子单元，用于根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

预设权值调整子单元，用于根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

目标坐标获取子单元，用于利用所述权值，求取所述第一坐标与所述第二坐标的加权和，并将所述加权和与所述车位坐标累计误差值求和，得到所述目标坐标。

进一步地，所述初始泊车路径生成模块具体包括：

中间参考点确定单元，用于根据所述目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

候选路径拟合单元，用于利用所述泊车初始位姿信息、所述目标泊车位信息以及所述中间参考点，拟合出多条候选路径；

代价计算单元，用于计算各条所述候选路径的执行代价；

泊车路径确定单元，用于选取执行代价最小的所述候选路径作为所述泊车路径。

进一步地，所述代价计算单元具体用于：根据所述候选路径中的曲线段的坐标平均变化量以及相应的航向角度平均变化量，得到所述候选路径的执行代价。

虽然上述系统实施例及优选方案的工作方式以及技术原理皆记载于前文，但仍需指出的是，本发明的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，也可以把它们分成多个子模块或子单元或子组件予以实施。

以及，本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上依据图式所示的实施例详细说明了本发明的构造、特征及作用效果，但以上仅为本发明的较佳实施例，需要言明的是，上述实施例及其优选方式所涉及的技术特征，本领域技术人员可以在不脱离、不改变本发明的设计思路以及技术效果的前提下，合理地组合搭配成多种等效方案；因此，本发明不以图面所示限定实施范围，凡是依照本发明的构想所作的改变，或修改为等同变化的等效实施例，仍未超出说明书与图示所涵盖的精神时，均应在本发明的保护范围内。

Claims (14)

1.一种泊车路径设置方法，其特征在于，包括：

确定目标泊车位信息；

获取待泊车辆的泊车初始位姿信息；

根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；所述泊车路径中包含至少两个参考点的作用是：通过多个中间参考点形成路径分段；

在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径；所述更新所述泊车路径包括：在泊车过程中，对尚未经过的所述路径分段进行适应性调整。

2.根据权利要求1所述的泊车路径设置方法，其特征在于，所述在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径包括：

根据专家经验，为所述泊车路径中的各所述中间参考点预设浮动区域坐标及相应的航向角度阈值；

实时检测待泊车辆的当前坐标及当前航向角度；

根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径。

3.根据权利要求2所述的泊车路径设置方法，其特征在于，所述根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径包括：

根据所述当前坐标及所述浮动区域坐标，判断待泊车辆是否进入所述中间参考点的浮动区域；

若否，则确定所述泊车路径无效；若是，则判断所述当前航向角度是否小于或等于所述航向角度阈值；

若否，则确定所述泊车路径无效；若是，则根据所述当前坐标、所述当前航向角度以及所述目标泊车位信息，调整后续所述中间参考点的位置；

利用所述实时位姿信息、所述目标泊车位信息以及调整位置后的所述中间参考点，拟合出新的后续泊车路径。

4.根据权利要求1所述的泊车路径设置方法，其特征在于，

所述目标泊车位信息包括：有效泊车位的目标坐标；

所述确定目标泊车位信息包括：

根据待泊车辆的尺寸，确定有效泊车位；

获取由雷达检测的所述有效泊车位的第一坐标；

获取由拍摄装置检测的所述有效泊车位的第二坐标；

将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标。

5.根据权利要求4所述的泊车路径设置方法，其特征在于，所述将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标包括：

根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

利用所述权值，求取所述第一坐标与所述第二坐标的加权和，并将所述加权和与所述车位坐标累计误差值求和，得到所述目标坐标。

6.根据权利要求1～5任一项所述的泊车路径设置方法，其特征在于，所述根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径包括：

根据所述目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

利用所述泊车初始位姿信息、所述目标泊车位信息以及所述中间参考点，拟合出多条候选路径；

计算各条所述候选路径的执行代价；

选取执行代价最小的所述候选路径作为所述泊车路径。

7.根据权利要求6所述的泊车路径设置方法，其特征在于，所述计算各条所述候选路径的执行代价包括：

根据所述候选路径中的曲线段的坐标平均变化量以及相应的航向角度平均变化量，得到所述候选路径的执行代价。

8.一种泊车路径设置系统，其特征在于，包括：

车位确定模块，用于确定目标泊车位信息；

初始位姿获取模块，用于获取待泊车辆的泊车初始位姿信息；

初始泊车路径生成模块，用于根据所述目标泊车位信息和所述泊车初始位姿信息，生成包含至少两个中间参考点的泊车路径；所述泊车路径中包含至少两个参考点的作用是：通过多个中间参考点形成路径分段；

泊车路径更新模块，用于在泊车过程中，根据待泊车辆的实时位姿信息以及与所述中间参考点相关的预设标准，更新所述泊车路径；所述更新所述泊车路径包括：在泊车过程中，对尚未经过的所述路径分段进行适应性调整。

9.根据权利要求8所述的泊车路径设置系统，其特征在于，所述泊车路径更新模块具体包括：

标准预设单元，用于根据专家经验，为所述泊车路径中的各所述中间参考点预设浮动区域坐标及相应的航向角度阈值；

实时位姿获取单元，用于实时检测待泊车辆的当前坐标及当前航向角度；

泊车路径更新单元，用于根据所述当前坐标与所述浮动区域坐标的关系以及所述当前航向角度与所述航向角度阈值的关系，更新所述泊车路径。

10.根据权利要求9所述的泊车路径设置系统，其特征在于，所述泊车路径更新单元具体包括：

浮动区域检测子单元，用于根据所述当前坐标及所述浮动区域坐标，判断待泊车辆是否进入所述中间参考点的浮动区域；

航向角度检测子单元，用于在所述浮动区域检测子单元输出为是时，判断所述当前航向角度是否小于或等于所述航向角度阈值；

中间参考点调整子单元，用于在所述航向角度检测子单元输出为是时，根据所述当前坐标、所述当前航向角度以及所述目标泊车位信息，调整后续所述中间参考点的位置；

后续泊车路径拟合子单元，用于利用所述实时位姿信息、所述目标泊车位信息以及调整位置后的所述中间参考点，拟合出新的后续泊车路径；

路径无效确定子单元，用于在所述浮动区域检测子单元输出为否时或者在所述航向角度检测子单元输出为否时，确定所述泊车路径无效。

11.根据权利要求8所述的泊车路径设置系统，其特征在于，

所述目标泊车位信息包括：有效泊车位的目标坐标；

所述车位确定模块具体包括：

有效车位确定单元，用于根据待泊车辆的尺寸，确定有效泊车位；

第一坐标获取单元，用于获取由雷达检测的所述有效泊车位的第一坐标；

第二坐标获取单元，用于获取由拍摄装置检测的所述有效泊车位的第二坐标；

坐标融合单元，用于将所述第一坐标与所述第二坐标融合，得到所述有效泊车位的目标坐标。

12.根据权利要求11所述的泊车路径设置系统，其特征在于，所述坐标融合单元具体包括：

累计误差获取子单元，用于根据不同时刻检测到的坐标值，得到雷达和拍摄装置所检测的车位坐标累计误差值；

预设权值调整子单元，用于根据由雷达和/或拍摄装置检测的泊车位周边的障碍物分布情况，调整预设的权值；

目标坐标获取子单元，用于利用所述权值，求取所述第一坐标与所述第二坐标的加权和，并将所述加权和与所述车位坐标累计误差值求和，得到所述目标坐标。

13.根据权利要求8～12任一项所述的泊车路径设置系统，其特征在于，所述初始泊车路径生成模块具体包括：

中间参考点确定单元，用于根据所述目标泊车位信息、待泊车辆的泊车初始位姿信息，从多条专家经验路径中获取中间参考点；

候选路径拟合单元，用于利用所述泊车初始位姿信息、所述目标泊车位信息以及所述中间参考点，拟合出多条候选路径；

代价计算单元，用于计算各条所述候选路径的执行代价；

泊车路径确定单元，用于选取执行代价最小的所述候选路径作为所述泊车路径。

14.根据权利要求13所述的泊车路径设置系统，其特征在于，所述代价计算单元具体用于：根据所述候选路径中的曲线段的坐标平均变化量以及相应的航向角度平均变化量，得到所述候选路径的执行代价。

Images