CN109703553A - 一种基于牵引点跟踪的自动泊车方法 - Google Patents

Abstract

本发明公开了一种基于牵引点跟踪的自动泊车方法，主要包括如下步骤：获取泊车环境信息，坐标变换，靠近车位入口，确定泊车方式，引导泊车。本发明通过车辆靠近车位时的位姿信息确定合适的泊车方式，用牵引点来引导车辆安全可靠的驶入车位。本发明突出的特点是该方法简单易于实现，可适用于多种泊车环境当中，并且相较于规划路径，然后跟踪路径的方法，本方法并不依赖于车辆模型的精确度，更符合人们的驾驶习惯。

Description

一种基于牵引点跟踪的自动泊车方法

技术领域

本发明属于无人驾驶汽车技术领域，尤其涉及一种基于牵引点跟踪的自动泊车方法。

背景技术

随着我国汽车产业的不断发展，我国机动车驾驶人数量已突破4亿。然而，由于泊车环境的复杂性，对很多新手驾驶员来说，泊车过程存在很大的风险，也极易发生碰撞及刮蹭事故，引发经济纠纷。自动泊车技术的出现，可以帮助人们安全可靠的解决泊车问题。目前已知的自动泊车方法一般包含两部分，即通过获取的车位位姿和车辆位姿对泊车路径的规划以及控制车辆跟随规划路径行驶完成泊车。这类方法主要存在以下几点问题：1、路径跟踪的精确度十分依赖建立的车辆模型的精确度；2、运算过程复杂；3、使用场景单一。

发明内容

本发明的目的在于提供一种基于牵引点跟踪的自动泊车方法，以解决上述背景技术中的问题。

为了实现上述目的，本发明所采用的技术方案为：一种基于牵引点跟踪的自动泊车方法，包括以下步骤：

S1，确定牵引点P，车辆在牵引点P的引导下靠近车位；牵引点P引导车辆的方法为：计算车辆至牵引点P的直线MP与车身航向角的角度差Δθ，通过比例控制得到目标前轮转角δ，所述目标前轮转角δ按照公式(1)确定，

式中，δ为车辆的前轮转角δ，δ_max为车辆的最大前轮转角，Δθ为车辆速度方向与直线MP的夹角，Δθ_max为预设的夹角边界；

其中，当牵引点P与车辆的距离大于车辆的最小转弯直径时，牵引点P为定点；否则，更新牵引点P的位置，更新方法为：保持车辆与牵引点P的距离恒定，调整牵引点P按照设定轨迹运动；

S2，车辆按照步骤S1所述的引导方法在牵引点P的引导下泊车。

进一步的，所述步骤S1中，

当车辆位于车位入口前方时，所述牵引点P位于车位入口的正前方，且牵引点P到车位入口前边界线的距离为车辆的最小转弯半径，用于引导车辆驶向车位入口；

当车辆位于车位入口后方时，所述牵引点P包括第一牵引点P1和第二牵引点P2，所述第一牵引点P1位于车位的侧方，所述第一牵引点P1位于车位前边界线的延长线上，且第一牵引点P1到同侧的车位边界的距离为车辆的最小转弯半径，用于引导车辆绕过车位边界线驶向车位入口前方；所述第二牵引点P2的位置与车辆位于车位入口前方时牵引点的位置相同，用于引导车辆驶向车位入口。

进一步的，当车辆位于车位入口后方时，所述第一牵引点P1动态更新的轨迹为：保持第一牵引点P1到同侧的车位边界线距离恒定，第一牵引点P1沿直线逐步向车位入口的前方运动。

进一步的，所述步骤S2中，牵引点P的位置随车辆的位置动态更新，且动态更新的方式为：保持车辆与牵引点P的距离恒定，调整牵引点P按照设定轨迹运动。

进一步的，所述步骤S2中，泊车方式包括前进式泊车、直线圆弧式泊车和“人”字型泊车三中方式；其中，

当车辆面向车位入口时，选择前进式泊车方式，车辆在牵引点的引导下前进直接驶入车位；

当车辆侧向车位入口时，选择直线圆弧式泊车方式，车辆在牵引点的引导下首先向前行驶至车位的侧方，然后倒车进入车位；

当车辆背向车位入口时，选择“人”字型泊车方式，车辆在牵引点的引导下首先向前行驶至车位的斜侧，然后倒车进入车位。

进一步的，泊车时，牵引点P的初始位置位于车位入口的正前方，且牵引点P到车位入口前边界线的距离为车辆的最小转弯半径；泊车过程中牵引点P动态更新的轨迹如下：

当车辆为前进式泊车方式时，牵引点P从初始位置沿车位纵轴线向车位内运动，引导车辆前进驶入车位，直至车辆驶入车位；

当车辆为直线圆弧式泊车方式时，牵引点P从初始位置沿垂直于车位纵轴线且远离车辆的方向运动，引导车辆行驶至垂直于车位入口的一侧，直至车辆到车位纵轴线的距离大于车辆的最小转弯半径；调整牵引点P的位置，牵引点P从初始位置沿车位纵轴线向车位内运动，直至车辆驶入车位；

当车辆为“人”字型泊车方式时，牵引点P从初始位置沿与车位纵轴线夹角为45°的直线远离车位和车辆的方向运动，引导车辆行驶至车位斜侧，直至车辆到车位纵轴线的距离大于车辆的最小转弯半径；调整牵引点P的位置，牵引点P从初始位置沿车位纵轴线向车位内运动，直至车辆驶入车位。

进一步的，所述步骤S1中，直线MP为车辆后轴中点M与牵引点P之间的直线。

与现有技术相比，本发明至少具有如下有益效果：本发明利用牵引点引导车辆泊车，泊车的效果主要受预设的牵引点运动轨迹影响，泊车过程就好像是人在驾驶一样，并不需要过高的车辆模型精度；其次，在控制车辆追踪牵引点时运算量很小，可以满足实时性的要求；并且该方法的鲁棒性很好，即使输出的控制量有较大的误差，最终也都可以轻松收敛到期望的位置。可适用于多种的泊车环境。

本发明方法在牵引点需要动态更新时，按照牵引点至车辆的距离保持不变的方法来控制车辆移动，这样便可以始终让车辆与牵引点的距离恒定，使得车辆按照牵引点的轨迹运动，直至驶入车位。本发明方法简单，运算量小，实现容易，具有很好的应用前景。

附图说明

图1为本发明实施例中车辆位姿状态量示意图；

图2为本发明实施例中车辆位于车位后方时的过渡牵引点示意图；

图3为本发明实施例中比例控制示意图；

图4为本发明实施例中车辆位于车位前方时牵引点示意图；

图5为本发明实施例中泊车方式选择示意图；

图6为本发明实施例中前进式泊车示意图；

图7为本发明实施例中直线圆弧式泊车示意图；

图8为本发明实施例中人字型泊车示意图；

图9为本发明实施例中前进式泊车时牵引点与车辆轨迹的示意图；

图10为本发明实施例中直线圆弧式泊车时牵引点与车辆轨迹的示意图；

图11为本发明实施例中人字型泊车时牵引点与车辆轨迹的示意图；

图12为本发明实施例提供的泊车逻辑流程示意图；

图13为本发明实施例提供的泊车流程简图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行详细的描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在本发明的实施例中，所有坐标向量遵循以下坐标系：

以车位入口前边界线中点为坐标原点0，过点0沿车位纵轴线指向车位内的方向为x轴，按右手系将x轴逆时针旋转90°的方向为y轴；坐标系如图1所示。

本发明一种基于牵引点跟踪的自动泊车方法的算法逻辑框图如图12所示，按照车辆与车位的初始关系分别为两个过程，具体为：当车辆位于车位的前方时，按照图13所示的流程选择合适的泊车方式停车；当车辆位于车位的后方时，此时需要将车辆移动到车位的前方，然后再按照图13所示的流程选择合适的方式进行泊车；在车辆移动和泊车的过程中，始终保持车辆与牵引点的距离保持恒定，以此实现车辆在牵引点的引导下进行泊车；

下面结合其他附图对发明方法做进一步的详细说明，具体如下：

步骤1、获得车辆位姿和车位位姿信息，具体包括：

方案一：通过全局摄像头获得车位相对于大地的位姿状态量(x₀，y₀，θ₀)，和车辆相对于大地的位姿状态量(x₁，y₁，θ₁)；其中x₀，y₀x₁，y₁，为车位和车辆在全局坐标系中的位置坐标，θ₀，θ₁为车位入口方向和车辆车头朝向与全局坐标系x轴方向的夹角，用来表示车位与车辆的姿态。

步骤2、经过坐标转换得到车辆相对于车位的位姿状态量(x，y，θ)，后文简称车辆状态量，其中x，y为车辆相对于坐标原点的位置，θ为车辆车头朝向与x轴的夹角，用来表示车辆的姿态。如图1所示，图中M点为车辆后轴中点，具体包括：

对于方案一：车辆状态量：(x，y，θ)按照下式确定，

对于方案二：车辆状态量(x，y，θ)按照下式确定，

步骤3、根据步骤2得到的车辆状态量确定牵引点的位置，具体包括：

当车辆位于车位入口后方时，即当车辆状态量x＞0时，确定位于车位侧面的第一牵引点如图2所示，用于引导车辆绕过车位边界线驶向车位入口前方；

其中：a为车位宽度，b为车位长度，c为车辆宽度；

当车辆位于车位入口前方时，即当车辆状态量x＜0时，确定位于车位正前方的牵引点P(-R_min，0)，其中R_min为车辆最小转弯半径；用于引导车辆驶向车位入口。

步骤4、车辆在牵引点的引导下靠近车位，当距离牵引点还较远时，即车辆后轴中点M与牵引点P距离d_MP＞d_min时，其中d_min为最小转弯直径，计算直线MP与车身航向角的角度差Δθ，通过比例控制得出目标前轮转角δ，控制车辆朝向车位入口行驶，如图3所示，目标前轮转角δ与角度差Δθ有如下关系：

其中，δ_max为前轮最大转角，Δθ_max为设定角度差边界；

步骤5、当车辆在牵引点的引导下已经靠近车位时，即车辆后轴中点M与牵引点P距离d_MP≤d_min时，此时包括以下两种情况：

若此时车辆状态量x＞0，即车辆仍位于车位后方，则此时车辆应在第一牵引点的引导下绕过车位边界，驶向车位入口前方。保持d_MP＝d_min，更新牵引点直至x≤0时，更新第二牵引点P(-R_min，0)这样车辆就绕过了车位边界到达了车位入口附近，如图4所示，其中：

式中：a为车位宽度，b为车位长度，Δd为避免车辆与车位边界刮蹭的安全距离；

若此时车辆状态量x＜0，即车辆已经位于车位入口的前方，则进入步骤6，选择泊车方式。

步骤6、选择泊车方式，具体包括：

根据车辆到达车位入口附近时车辆的状态量(x，y，θ)，选择最适合的泊车方式，如图5所示；

当车辆面向车位入口时，即|θ|≤θ₁时，选择前进式泊车方式，如图6所示；

当车辆侧向车位入口时，即-θ₂＜θ＜-θ₁或θ₁＜θ＜θ₂时，选择直线圆弧式泊车方式，如图7所示；

当车辆背向车位入口时，即-θ₃＜θ＜-θ₂或θ₂＜θ＜θ₃时，选择“人”字型泊车方式，如图8所示；

步骤7、根据所选择的泊车方式，车辆在牵引点的引导下驶入车位，三种泊车方式下车辆进入车位的具体方法为：

当车辆为前进式泊车方式时，车辆在牵引点的引导下前进直接驶入车位，牵引点从车位入口正前方的初始位置沿车位纵轴线向车位内运动，引导车辆前进驶入车位，如图9所示为车辆前进式泊车时牵引点与车辆的轨迹，令d_c＝d_MP，更新牵引点P(x^*，0)直至车辆驶入车位，其中：

当车辆为直线圆弧式泊车方式时，车辆在牵引点的引导下首先向前行驶至车位的侧方，然后倒车进入车位。牵引点从位于车位入口正前方的初始位置沿垂直于车位纵轴线方向运动，这里需要说明的是，当车辆位于x轴的负半轴时，车辆向着x轴的正半轴方向运动；相应地，当车辆位于x轴的正半轴时，车辆向着x轴的负半轴方向运动；引导车辆行驶至垂直于车位入口一侧；令d_c＝d_MP，更新牵引点P(-R_min，y^*)，其中，R_min为车辆的最小转弯半径直至车辆状态量y＜-R_min或y＞R_min时，再次调整牵引点P的位置，牵引点从位于车位入口正前方的初始位置沿车位纵轴线向车位内运动引导车辆倒车驶入车位，如图10所示为车辆直线圆弧式泊车时牵引点与车辆的轨迹，令更新牵引点P(x^*，0)，直至车辆驶入车位，其中：

当车辆为“人”字型泊车方式时，车辆在牵引点的引导下首先向前行驶至车位的斜侧，然后倒车进入车位。如图8所示，牵引点从位于车位入口正前方的初始位置沿与车位纵轴线夹角为45°的直线远离车位方向运动，同样地，当车辆位于x轴的负半轴时，车辆向着x轴的正半轴方向运动；相应地，当车辆位于x轴的正半轴时，车辆向着x轴的负半轴方向运动；引导车辆行驶至车位斜侧，

令d_c＝d_MP，更新牵引点P(x^*，y^*)，直至车辆状态量y＜-R_min或y＞R_min时；再次调整牵引点P的位置，牵引点从位于车位入口正前方的初始位置沿车位纵轴线向车位内运动引导车辆倒车驶入车位，如图11所示为车辆人字型泊车时牵引点与车辆的轨迹，令更新牵引点直至车辆驶入车位，其中：

以上结合附图对本发明的实施方式进行了阐述，但本发明不局限于上述具体的实施方式，上述具体的实施方式仅仅是示意性的，指导性的，而非限制性的，在不脱离本发明精神和范围前提下，本发明会有各种变化和改进，这些变化和改进都属于本发明要求保护范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (7)

1.一种基于牵引点跟踪的自动泊车方法，其特征在于，包括以下步骤：

S1，确定牵引点P，车辆在牵引点P的引导下靠近车位；牵引点P引导车辆的方法为：计算车辆至牵引点P的直线MP与车身航向角的角度差Δθ，通过比例控制得到目标前轮转角δ，所述目标前轮转角δ按照公式(1)确定，

式中，δ为车辆的前轮转角δ，δ_max为车辆的最大前轮转角，Δθ为车辆速度方向与直线MP的夹角，Δθ_max为预设的夹角边界；

其中，当牵引点P与车辆的距离大于车辆的最小转弯直径时，牵引点P为定点；否则，更新牵引点P的位置，更新方法为：保持车辆与牵引点P的距离恒定，调整牵引点P按照设定轨迹运动；

S2，车辆按照步骤S1所述的引导方法在牵引点P的引导下泊车。

2.如权利要求1所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，所述步骤S1中，

当车辆位于车位入口前方时，所述牵引点P位于车位入口的正前方，且牵引点P到车位入口前边界线的距离为车辆的最小转弯半径，用于引导车辆驶向车位入口；

当车辆位于车位入口后方时，所述牵引点P包括第一牵引点P1和第二牵引点P2，所述第一牵引点P1位于车位的侧方，所述第一牵引点P1位于车位前边界线的延长线上，且第一牵引点P1到同侧的车位边界的距离为车辆的最小转弯半径，用于引导车辆绕过车位边界线驶向车位入口前方；所述第二牵引点P2的位置与车辆位于车位入口前方时牵引点的位置相同，用于引导车辆驶向车位入口。

3.如权利要求2所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，当车辆位于车位入口后方时，所述第一牵引点P1动态更新的轨迹为：保持第一牵引点P1到同侧的车位边界线距离恒定，第一牵引点P1沿直线逐步向车位入口的前方运动。

4.如权利要求1所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，所述步骤S2中，牵引点P的位置随车辆的位置动态更新，且动态更新的方式为：保持车辆与牵引点P的距离恒定，调整牵引点P按照设定轨迹运动。

5.如权利要求1或4所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，所述步骤S2中，泊车方式包括前进式泊车、直线圆弧式泊车和“人”字型泊车三中方式；其中，

当车辆面向车位入口时，选择前进式泊车方式，车辆在牵引点的引导下前进直接驶入车位；

当车辆侧向车位入口时，选择直线圆弧式泊车方式，车辆在牵引点的引导下首先向前行驶至车位的侧方，然后倒车进入车位；

当车辆背向车位入口时，选择“人”字型泊车方式，车辆在牵引点的引导下首先向前行驶至车位的斜侧，然后倒车进入车位。

6.如权利要求5所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，泊车时，牵引点P的初始位置位于车位入口的正前方，且牵引点P到车位入口前边界线的距离为车辆的最小转弯半径；泊车过程中牵引点P动态更新的轨迹如下：

当车辆为前进式泊车方式时，牵引点P从初始位置沿车位纵轴线向车位内运动，引导车辆前进驶入车位，直至车辆驶入车位；

当车辆为直线圆弧式泊车方式时，牵引点P从初始位置沿垂直于车位纵轴线且远离车辆的方向运动，引导车辆行驶至垂直于车位入口的一侧，直至车辆到车位纵轴线的距离大于车辆的最小转弯半径；调整牵引点P的位置，牵引点P从初始位置沿车位纵轴线向车位内运动，直至车辆驶入车位；

当车辆为“人”字型泊车方式时，牵引点P从初始位置沿与车位纵轴线夹角为45°的直线远离车位和车辆的方向运动，引导车辆行驶至车位斜侧，直至车辆到车位纵轴线的距离大于车辆的最小转弯半径；调整牵引点P的位置，牵引点P从初始位置沿车位纵轴线向车位内运动，直至车辆驶入车位。

7.如权利要求1所述的一种基于牵引点跟踪的自动泊车方法，其特征在于，所述步骤S1中，直线MP为车辆后轴中点M与牵引点P之间的直线。