CN102874252B - 辅助泊车轨迹规划及修正方法及系统 - Google Patents

Abstract

本发明公开了一种辅助泊车轨迹规划及修正方法及系统，所述方法包括：S1、根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹；S2、控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正；S3、结合线性泊车轨迹和轨迹误差修正完成泊车。本发明根据汽车初始位置不同，对轨迹规划进行线性分段，降低了控制器的运算负荷，交互轨迹长度修正能够提高泊车成功率和停泊位置的精确度。

Description

辅助泊车轨迹规划及修正方法及系统

技术领域

本发明属于辅助泊车系统技术领域，尤其涉及一种辅助泊车轨迹规划及修正方法及系统。

背景技术

辅助泊车系统根据汽车初始状态计算最佳行车轨迹，通过语音提示协助驾驶员将汽车准确地停放于目标车位中，与自动泊车系统的不同之处在于，语音提示转向、泊车等信息来辅助泊车系统，且不需要控制电动助力转向电机，也无需与整车网络通信，因此可以适用于更广泛的车型。

语音提示辅助驾驶员操作方向盘位置只有三种状态：左极限位置、回正位置、右极限位置，按照水平侧方泊车提出的一般流程，将驾驶员泊车动作分解为三段轨迹，每段轨迹的切换点即为提示驾驶员泊车转向的信息点，其轨迹和切换点决定了最终实际泊车位置准确度。

在设计时汽车的初始位置已经决定了泊车轨迹及切换点，理论上要求轮胎轨迹长度到达理论计算值后便切换方向盘的位置以便进入下一段行驶轨迹，但实践表明，由于汽车自身结构和尺寸原因，各段轨迹长度与汽车初始位置不是完全线性关系，尤其各轮行驶并非完全按照阿克曼定理，无法完全满足辅助泊车轨迹规划要求，且辅助泊车对实际泊车轨迹与理论路径偏差无直接反馈，属于典型的开环系统，降低了侧方泊车的成功率。

有鉴于此，有必要提供一种辅助泊车轨迹规划及修正方法及系统以解决上述问题。

发明内容

本发明的目的在于提供一种提高泊车精确度的辅助泊车轨迹规划及修正方法及系统。

本发明的一种辅助泊车轨迹规划及修正方法，所述方法包括：

S1、根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹；

S2、控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正；

S3、结合线性泊车轨迹和轨迹误差修正完成泊车。

作为本发明的进一步改进，所述步骤S2包括：

S21、控制方向盘至右极限位置，跟踪后左侧轮的泊车轨迹，同时记录后右侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

S22、控制方向盘至回正位置，跟踪后左侧轮或后右侧轮理论的泊车轨迹，同时记录后右侧轮或后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

S23、控制方向盘至左极限位置，跟踪右后侧轮理论的泊车轨迹，同时记录后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正。

作为本发明的进一步改进，所述线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置。

作为本发明的进一步改进，所述步骤S1具体为：

根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述不同的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；

根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。

作为本发明的进一步改进，所述泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式。

相应地，一种辅助泊车轨迹规划及修正系统，所述系统包括：

规划单元，用于根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹的单元；

修正单元，用于控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正的单元。

作为本发明的进一步改进，所述修正单元还用于：

控制方向盘至右极限位置时，跟踪后左侧轮的泊车轨迹，同时记录后右侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

控制方向盘至回正位置时，跟踪后左侧轮或后右侧轮理论的泊车轨迹，同时记录后右侧轮或后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

控制方向盘至左极限位置时，跟踪右后侧轮理论的泊车轨迹，同时记录后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正。

作为本发明的进一步改进，所述线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置。

作为本发明的进一步改进，所述规划单元还用于：

根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述不同的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；

根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。

作为本发明的进一步改进，所述泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式。

本发明的有益效果是：本发明根据汽车初始位置不同，对轨迹规划进行线性分段，降低了控制器的运算负荷，交互轨迹长度修正能够提高泊车成功率和停泊位置的精确度。

附图说明

图1为本发明辅助泊车轨迹规划及修正方法的总体流程图。

图2为本发明一实施方式中初始位置及方向盘右极限位置的泊车轨迹示意图。

图3为本发明一实施方式中方向盘回正位置的泊车轨迹示意图。

图4为本发明一实施方式中方向盘左极限位置的泊车轨迹示意图。

图5为本发明一实施方式中辅助泊车轨迹规划及修正方法的具体流程图。

具体实施方式

以下将结合附图所示的各实施方式对本发明进行详细描述。但这些实施方式并不限制本发明，本领域的普通技术人员根据这些实施方式所做出的结构、方法、或功能上的变换均包含在本发明的保护范围内。

参图1所示，本发明的一种辅助泊车轨迹规划及修正方法，其包括以下步骤：

S1、根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹。根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述不同的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式；

S2、控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正；

S3、结合线性泊车轨迹和轨迹误差修正完成泊车。

其中，步骤S2中线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置，具体为：

S21、控制方向盘至右极限位置，跟踪后左侧轮的泊车轨迹，同时记录后右侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

S22、控制方向盘至回正位置，跟踪后左侧轮或后右侧轮理论的泊车轨迹，同时记录后右侧轮或后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

S23、控制方向盘至左极限位置，跟踪右后侧轮理论的泊车轨迹，同时记录后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正。

相应地，一种辅助泊车轨迹规划及修正系统，其包括：

规划单元，用于根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹。根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述不同的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式；

计算单元，用于控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正。

其中，计算单元中线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置，具体为：

控制方向盘至右极限位置，跟踪后左侧轮的泊车轨迹，同时记录后右侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

控制方向盘至回正位置，跟踪后左侧轮或后右侧轮理论的泊车轨迹，同时记录后右侧轮或后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正；

控制方向盘至左极限位置，跟踪右后侧轮理论的泊车轨迹，同时记录后左侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半作为修正值对后一段的轨迹进行修正。

以下，参照附图2至4进行对本发明的一具体实施方式具体说明。

以某汽车为例说明具体实施方式，车辆尺寸：长4923mmX宽1845mm；泊车间距为0.5-1.5m。根据实际要求，分为近距泊车模式、中距泊车模式和远距泊车模式，对应汽车初始状态间距D是[0.50.8]、[0.81.2]、[1.21.5]，整个路径分为三步“弧1-直线段2-弧3”，见图2、图3、图4，即为三个轨迹切换点，切换点是通过语音提示驾驶员操作方向盘，与这三种模式对应的三段轨迹完全可以线性表示。

通过拟合可以得到三种模式下三段轨迹与初始位置的线性关系：

1.近距泊车模式X_n0∈[0.5，0.8]：

X_n1=1.1X_n0+1.5；X_n2=-1.5X_n0+5.1；X_n3=0.8X_n0+1.1。

2.中距泊车模式X_m0∈[0.8，1.2]：

X_m1=1.3X_m0+1.4；X_m2=-1.6X_m0+5.2；X_m3=0.87X_m0+0.99。

3.远距泊车模式X_f0∈[1.2，1.5]：

X_f1=2.5X_f0-0.14；X_f2=-3.2X_f0+7.1；X_f3=2X_f0-0.36。

其中X_n0、X_m0、X_f0分别是三种模式下汽车与其他车位的汽车横向间距，X_ni、X_mi、X_fi（i=1,2,3）分别为三种模式下三段轨迹长度。

结合图5所示，以右侧方泊车为例对本发明作进一步说明。

第一步：根据汽车在初始位置时，判断或选择泊车模式，按照当前模式理论计算后轮轨迹长度；

第二步：方向盘至右极限位置，以后左侧轮（外轮）理论计算行驶，同时记录后右侧轮的实际轨迹长度，并与理论轨迹长度比较计算，按差值的一半对第二段的轨迹进行修正；

第三步：方向盘至回正位置，按照修正轨迹行驶，仍以后左侧轮修正后的轨迹长度为目标，同时记录后右车轮的实际轨迹长度，并与第一次修正后轨迹比较计算，按差值的一半对第三段轨迹修正；

第四步：方向盘至左极限位置，按照二次修正后的轨迹行驶，此时以后右侧轮二次修正后轨迹长度为目标，同时记录后左车轮的实际轨迹长度，并与第二次修正后轨迹比较计算；

第五步：方向盘至回正位置，按照修正值提示驾驶员作前后调整。

综上所述，本发明将初始状态的横向间距分为高线性度的几个间距，根据车辆自身参数的不同，分段的数量亦不同，以达到目标位置精度要求为目的，这样泊车规划路径的每段行驶轨迹长度与初始状态成完全线性关系。另一方面，以后轮轨迹为跟踪，在各段行驶轨迹中均以外侧轮轨迹为目标，另一侧轮轨迹与理论计算轨迹长度作差，其差值的一半作为修正值，其值带符号，表示修正值的正负，带入修正下一段后轮的轨迹，即方向盘右极限位置时以左后轮轨迹为目标，右后轮为修正轨迹，方向盘左极限位置时则以右后轮轨迹为目标，左后轮为修正轨迹。在泊车过程中，跟踪的外侧轮是在左右变化的，因此后轮轨迹修正是交互进行的，可以消弱误差，提高泊车的精度。

与现有技术相比，本发明根据汽车初始位置不同，对轨迹规划进行线性分段，降低了控制器的运算负荷，交互轨迹长度修正能够提高泊车成功率和停泊位置的精确度。

应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施方式中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更均应包含在本发明的保护范围之内。

Claims (8)

1.一种辅助泊车轨迹规划及修正方法，其特征在于，所述方法包括：

步骤S1、根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹；

步骤S2、控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正；具体包括：

步骤S21、根据汽车在初始位置时，判断或选择泊车模式，按照当前模式理论计算后轮轨迹长度；

步骤S22、方向盘至右极限位置，以后左侧轮理论轨迹行驶，同时记录后右侧轮的实际轨迹长度，并与后右侧轮的理论轨迹长度比较计算，按差值的一半对第二段后轮理论轨迹进行第一次修正；

步骤S23、方向盘至回正位置，按照第一次修正后的后轮轨迹行驶，以后左侧轮第一次修正后的轨迹长度为目标，同时记录后右车轮的实际轨迹长度，并与第一次修正后右侧轮的理论轨迹比较计算，按差值的一半对第三段后轮理论轨迹进行第二次修正；

步骤S24、方向盘至左极限位置，按照第二次修正后的后轮轨迹行驶，以后右侧轮第二次修正后的轨迹长度为目标，同时记录后左车轮的实际轨迹长度，并与第二次修正后左车轮的理论轨迹比较计算，按差值的一半作为下一段后轮理论轨迹修正值；

步骤S25、方向盘至回正位置，按照所述下一段后轮理论轨迹修正值提示驾驶员作前后位置调整；

步骤S3、结合线性泊车轨迹和轨迹误差修正完成泊车。

2.根据权利要求1所述的方法，其特征在于，所述线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置。

3.根据权利要求1所述的方法，其特征在于，所述步骤S1具体为：

根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述若干高线性度的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；

根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。

4.根据权利要求3所述的方法，其特征在于，所述泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式。

5.一种辅助泊车轨迹规划及修正系统，其特征在于，所述系统包括：

规划单元，用于根据汽车的初始位置和最终位置拟合得到若干段连续的线性泊车轨迹的单元；

修正单元，用于控制方向盘至右极限位置、回正位置或左极限位置按照线性泊车轨迹行驶，通过跟踪、比较、计算前一段的轨迹进行后一段的轨迹误差修正的单元；所述修正单元还包括：

根据汽车在初始位置时，判断或选择泊车模式，按照当前模式理论计算后轮轨迹长度的模块；

方向盘至右极限位置，以后左侧轮理论轨迹行驶，同时记录后右侧轮的实际轨迹长度，并与后右侧轮的理论轨迹长度比较计算，按差值的一半对第二段后轮理论轨迹进行第一次修正的模块；

方向盘至回正位置，按照第一次修正后的后轮轨迹行驶，以后左侧轮第一次修正后的轨迹长度为目标，同时记录后右车轮的实际轨迹长度，并与第一次修正后右侧轮的理论轨迹比较计算，按差值的一半对第三段后轮理论轨迹进行第二次修正的模块；

方向盘至左极限位置，按照第二次修正后的后轮轨迹行驶，以后右侧轮第二次修正后的轨迹长度为目标，同时记录后左车轮的实际轨迹长度，并与第二次修正后左车轮的理论轨迹比较计算，按差值的一半作为下一段后轮理论轨迹修正值的模块；

方向盘至回正位置，按照所述下一段后轮理论轨迹修正值提示驾驶员作前后位置调整的模块。

6.根据权利要求5所述的系统，其特征在于，所述线性泊车轨迹分为三段，分别对应方向盘右极限位置、回正位置和左极限位置。

7.根据权利要求5所述的系统，其特征在于，所述规划单元还用于：

根据汽车自身参数将初始的横向间距分为若干高线性度的泊车间距，所述若干高线性度的泊车间距对应不同的泊车模式，拟合不同泊车模式下的线性泊车轨迹；

根据泊车间距确定汽车的泊车模式及对应泊车模式下的线性泊车轨迹。

8.根据权利要求7所述的系统，其特征在于，所述泊车模式包括近距泊车模式、中距泊车模式和远距泊车模式。