CN112550278A - 一种基于环视摄像头和超声波雷达检测的自动泊车方法和系统 - Google Patents

Abstract

本发明公开了一种基于环视摄像头和超声波雷达检测的自动泊车方法，其包括步骤：100：用环视摄像头和超声波雷达分别检测车位信息，并基于车位信息得到确定的停车位；200：测量车辆偏航角速率，用一阶卡尔曼滤波器对车辆偏航角速率进行常值偏差滤波，获得车辆偏航角速率的估计值；300：由确定的停车位，建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态；400：判断泊车起始区域；500：根据泊车起始区域，由车辆与车位的碰撞规避决策自动泊车，进入停车位，自动泊车过程中，实时根据车辆偏航角速率估计值和采集的车辆齿轮脉冲，估计车辆相对车位坐标系的位姿，根据环视摄像头测量值和/或超声波雷达测量值修正车辆相对车位坐标系的初始位置姿态。

Description

一种基于环视摄像头和超声波雷达检测的自动泊车方法和 系统

技术领域

本发明涉及一种控制方法和系统，尤其涉及一种自动泊车方法和系统。

背景技术

近年来，随着自动驾驶技术的快速发展，自动驾驶技术的逐步完善和改进，自动驾驶车辆运用在日常生活中的可能性也越来越大。

在现有的自动驾驶技术中，自动泊车技术是一种非常实用且重要的功能。自动泊车技术涉及非常多的传感器，其可以有效实现自动泊车的功能。采用自动泊车技术，可以进行合理的路径规划，使泊车姿态达到理想效果，实现车辆的自动泊车，其不仅可以防止与障碍物碰撞，保证泊车过程的安全性，还能为用户提供更好的驾驶体验。

基于此，本发明期望获得一种新的基于环视摄像头和超声波雷达检测的自动泊车方法和系统，采用该自动泊车方法和系统可以通过环视摄像头和超声波雷达获取车辆相对停车位的相对位置关系，建立车位坐标系，并基于环视和超声波雷达进行障碍物和行驶可通行区域检测，完成停车位选择，进行泊车路径的规划，实现自动泊车。

发明内容

本发明的目的之一在于提供一种基于环视摄像头和超声波雷达检测的自动泊车方法，该自动泊车方法可以通过环视摄像头和超声波雷达获取车辆相对停车位的相对位置关系，建立车位坐标系，并基于环视和超声波雷达进行障碍物和行驶可通行区域检测，完成停车位选择，进行泊车路径的规划，实现自动泊车，其具有良好的推广前景和应用价值。

为了实现上述目的，本发明提出了一种基于环视摄像头和超声波雷达检测的自动泊车方法，其包括步骤：

100：采用环视摄像头和超声波雷达分别检测车位信息，并基于车位信息得到确定的停车位；

200：采用车辆陀螺仪测量车辆偏航角速率，采用一阶卡尔曼滤波器对所述车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值；

300：基于所述确定的停车位，建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态；

400：判断泊车起始区域；

500：根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动泊车，直至进入所述确定的停车位，在自动泊车的过程中，实时根据所述车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正。

在本发明上述的技术方案中，本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法可以通过环视摄像头和超声波雷达获取车辆相对停车位的相对位置关系，建立车位坐标系，并基于环视和超声波雷达进行障碍物和行驶可通行区域检测，完成停车位选择，进行泊车路径的规划，实现自动泊车。

在本发明上述步骤200中，本发明所述的自动泊车方法采用了一阶卡尔曼滤波器对车辆偏航角速率进行常值偏差滤波，从而得到车辆偏航角速率的估计值，以实现车辆运动学的高精度定位。在自动泊车过程中，本发明所述的自动泊车方法可以实时根据车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正，使泊车姿态达到理想效果。

当采用本发明所述的自动泊车方法进行自动泊车时，对于垂直泊车，若停车通道宽度不足时，本发明可以控制进行动态调整，以使泊车规划不受障碍物的限制。

进一步地，在本发明所述的自动泊车方法中，在步骤400中，基于下述公式对车辆相对车位坐标系的位姿进行估计：

x_r(k)＝x_r(k-1)+l_r(k-1)*cos(yaw_r(k))

y_r(k)＝y_r(k-1)+l_r(k-1)*sin(yaw_r(k))

其中x_r、y_r表示车辆后轴中心点的位置坐标，yaw_r表示车辆航向角，

为车辆偏航角速率的估计值，l_r为根据车辆两后轮的齿轮脉冲计算得到的车辆在上个周期的行驶距离，Δt为采样周期，k、k-1分别表示k时刻和k-1时刻。

进一步地，在本发明所述的自动泊车方法中，在步骤200中，还采用一阶卡尔曼滤波器对车辆偏航角速率进行随机噪声滤波。

进一步地，在本发明所述的自动泊车方法中，在步骤200中，一阶卡尔曼滤波器的卡尔曼滤波状态预测方程为：

p[k]＝a*p[k-1]*a+q

卡尔曼滤波观测更新方程为：

kg＝p[k]*h/(h*p[k]*h+r)

p[k]＝(1-kg*h)*p[k-1]

其中，

为当前测量的车辆偏航角速率；

为当前车辆偏航角速率的估计值；a表示状态转移矩阵；p表示误差协方差矩阵；q表示状态噪声协方差矩阵；h表示观测矩阵；kg表示卡尔曼增益系数；r表示观测噪声协方差矩阵；k、k-1分别表示k时刻和k-1时刻。

进一步地，在本发明所述的自动泊车方法中，所述车辆与车位的碰撞规避决策包括：

车辆与车位右侧边界不发生碰撞的条件是：

(x_r-W_p/2)²+(R-y_r)²≤(R-L_b/2-L_safe1)²

车辆与车位左侧边界不发生碰撞的条件是：

其中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r表示车辆的后轴中心点的位置坐标，R表示车辆的最小转弯半径，L_safe1为车辆右侧边沿距离车位右上顶点的安全距离，L_safe2为车辆左后点与车位左边沿的安全距离。

相应地，本发明的另一目的在于提供一种基于环视摄像头和超声波雷达检测的自动泊车系统，该自动泊车系统可以用于实施本发明上述的自动泊车方法。

为了实现上述目的，本发明提出了一种基于环视摄像头和超声波雷达检测的自动泊车系统，其包括：环视摄像头、超声波雷达、车辆陀螺仪、一阶卡尔曼滤波器、停车位决策模块、车辆位姿估计模块、泊车起始区域判断模块和泊车执行模块，其中：

所述环视摄像头和超声波雷达分别检测车位信息；

停车位决策模块基于环视摄像头和超声波雷达分别检测得到的车位信息，得到确定的停车位；

车辆陀螺仪测量车辆偏航角速率；

一阶卡尔曼滤波器对所述车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值；

车辆位姿估计模块，其基于所述确定的停车位建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态；并且在自动泊车的过程中，实时根据所述车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正；

泊车起始区域判断模块，其判断车辆的泊车起始区域；

泊车执行模块，其根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动停泊至所述确定的停车位。

进一步地，在本发明所述的自动泊车系统中，所述车辆位姿估计模块基于下述公式对车辆相对车位坐标系的位姿进行估计：

x_r(k)＝x_r(k-1)+l_r(k-1)*cos(yaw_r(k))

y_r(k)＝y_r(k-1)+l_r(k-1)*sin(yaw_r(k))

其中x_r、y_r表示车辆后轴中心点的位置坐标，yaw_r表示车辆航向角，

为车辆偏航角速率的估计值，l_r为根据车辆两后轮的齿轮脉冲计算得到的车辆在上个周期的行驶距离，Δt为采样周期，k、k-1分别表示k时刻和k-1时刻。

进一步地，在本发明所述的自动泊车系统中，所述一阶卡尔曼滤波器还对车辆偏航角速率进行随机噪声滤波。

进一步地，在本发明所述的自动泊车系统中，所述一阶卡尔曼滤波器的卡尔曼滤波状态预测方程为：

p[k]＝a*p[k-1]*a+q

卡尔曼滤波观测更新方程为：

kg＝p[k]*h/(h*p[k]*h+r)

p[k]＝(1-kg*h)*p[k-1]

其中，

为当前测量的车辆偏航角速率；

为当前车辆偏航角速率的估计值；a表示状态转移矩阵；p表示误差协方差矩阵；q表示状态噪声协方差矩阵；h表示观测矩阵；kg表示卡尔曼增益系数；r表示观测噪声协方差矩阵；k、k-1分别表示k时刻和k-1时刻。

进一步地，在本发明所述的自动泊车系统中，所述车辆与车位的碰撞规避决策包括：

车辆与车位右侧边界不发生碰撞的条件是：

(x_r-W_p/2)²+(R-y_r)²≤(R-L_b/2-L_safe1)²

车辆与车位左侧边界不发生碰撞的条件是：

其中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r表示车辆的后轴中心点的位置坐标，R表示车辆的最小转弯半径，L_safe1为车辆右侧边沿距离车位右上顶点的安全距离，L_safe2为车辆左后点与车位左边沿的安全距离。

本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法和系统相较于现有技术具有如下所述的优点和有益效果：

本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法可以通过环视摄像头和超声波雷达获取车辆相对停车位的相对位置关系，建立车位坐标系，并基于环视和超声波雷达进行障碍物和行驶可通行区域检测，完成停车位选择，进行泊车路径的规划，实现自动泊车，其具有良好的推广前景和应用价值。

此外，本发明所述的自动泊车方法采用了一阶卡尔曼滤波器对车辆偏航角速率进行常值偏差滤波，从而得到车辆偏航角速率的估计值，以实现车辆运动学的高精度定位。在自动泊车过程中，本发明所述的自动泊车方法可以实时根据车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正，使泊车姿态达到理想效果。

相应地，本发明所述的基于环视摄像头和超声波雷达检测的自动泊车系统可以用于实施本发明上述自动泊车方法，其同样具有上述的优点以及有益效果。

附图说明

图1示意性地显示了本发明所述的自动泊车方法在一种实施方式下的步骤流程图。

图2显示了本发明所述的自动泊车方法在一种实施方式下建立的车辆坐标系示意图。

图3示意性地显示了本发明所述的自动泊车方法在一种实施方式下的泊车起始区域范围。

图4为本发明所述的自动泊车方法在一种实施方式下车辆与车位两侧碰撞规避示意图。

图5示意性地显示了本发明所述的自动泊车方法在一种实施方式下不同起始区域对应的泊车模式图。

图6示意性地显示了本发明所述的自动泊车方法在一种实施方式下泊车起始区域图。

图7示意性地显示了本发明所述的自动泊车方法在一种实施方式下泊车转向控制策略流程图。

图8为本发明所述的自动泊车方法在一种实施方式下车辆在Mode1和Mode2下的路径规划示意图。

图9为本发明所述的自动泊车方法在一种实施方式下车辆在Mode3下的路径规划示意图。

图10本发明所述的自动泊车方法在一种实施方式下的车辆在垂直动态调整示意图。

具体实施方式

下面将结合说明书附图和具体的实施例对本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法和系统做进一步的解释和说明，然而该解释和说明并不对本发明的技术方案构成不当限定。

在本发明中，本发明公开了一种基于环视摄像头和超声波雷达检测的自动泊车系统，该自动泊车系统可以实现自动泊车，其可以包括：环视摄像头、超声波雷达、车辆陀螺仪、一阶卡尔曼滤波器、停车位决策模块、车辆位姿估计模块、泊车起始区域判断模块和泊车执行模块。

其中，环视摄像头和超声波雷达可以分别检测车位信息；停车位决策模块能够基于环视摄像头和超声波雷达分别检测得到的车位信息，得到确定的停车位；车辆陀螺仪可以用于测量车辆偏航角速率；一阶卡尔曼滤波器可以对车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值；车辆位姿估计模块不仅可以基于确定的停车位建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态，其还可以在自动泊车的过程中，实时根据车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正；泊车起始区域判断模块可以判断车辆的泊车起始区域；泊车执行模块可以根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动停泊至确定的停车位。

在某些实施方式中，当驾驶员将具有上述自动泊车系统的车辆驶入停车场或沿停车位行驶时，当开启自动泊车功能后，自车会接管车辆控制，并基于自动泊车系统中的环视摄像头和超声波雷达检测这些传感器进行停车位检测并自动判别停车位类型，实现自动泊车。

由此可见，本发明所述的自动泊车系统可以用于执行本发明基于环视摄像头和超声波雷达检测的自动泊车方法，其具体步骤，如图1所示。

图1示意性地显示了本发明所述的自动泊车方法在一种实施方式下的步骤流程图。

在本发明中，本发明所述的自动泊车方法主要通过环视摄像头和超声波雷达获得车辆相对车位的相对位置关系，建立车辆坐标系，并基于环视摄像头和超声波雷达进行障碍物和行驶可通行区域监测，选择泊车区域，规划泊车路径，并通过偏航角速率的一维卡尔曼滤波估计，实现车辆运动学的高精度定位。

如图1所示，在图1所示的实施方式中，本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法可以包括如下步骤：

100：采用环视摄像头和超声波雷达分别检测车位信息，并基于车位信息得到确定的停车位。

200：采用车辆陀螺仪测量车辆偏航角速率，采用一阶卡尔曼滤波器对车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值。

需要说明的是，在本发明中，车辆本身就具有车辆陀螺仪，车辆陀螺仪可以测量车辆偏航角速率，但此时车辆陀螺仪给出的测量值存在常值偏差和随机噪声，因此在本实施方式中，不仅需要对车辆偏航角速率进行常规偏差滤波估计，还需要采用一阶卡尔曼滤波器对车辆偏航角速率进行随机噪声滤波。

在本实施方式中，卡尔曼滤波状态预测方程可以为：

p[k]＝a*p[k-1]*a+q

卡尔曼滤波观测更新方程可以为：

kg＝p[k]*h/(h*p[k]*h+r)

p[k]＝(1-kg*h)*p[k-1]

上式中，

表示当前测量的偏航角速率；

表示当前偏航角速率估计值；p表示误差协方差矩阵；q表示状态噪声协方差矩阵；a表示状态转移矩阵；h表示观测矩阵；kg表示卡尔曼增益系数；

表示当前测量观测值；r表示观测噪声协方差矩阵；k、k-1分别表示k时刻和k-1时刻。

300：基于确定的停车位，建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态。

400：判断泊车起始区域。

500：根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动泊车，直至进入确定的停车位，在自动泊车的过程中，实时根据车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正。

需要说明的是，在本发明上述步骤100中，当采用环视摄像头和超声波雷达进行检测，发现可用泊车库有清晰的车位线标记时，本发明基于环视摄像头检测得到的车位信息得到确定的停车位，其具体可以包括以下两种情况：(1)可用泊车库有完整的车位线标记；(2)可用泊车库一边没有车位线标记。

相应地，当采用环视摄像头和超声波雷达进行检测，发现可用泊车库车位线标记不清晰或没有车位线标记时，本发明基于超声波雷达检测得到的车位信息得到确定的停车位，其具体可以包括以下两种情况：(1)可用泊车库相邻两边车库都停有车辆；(2)可用泊车库只有相邻一边车库停有车辆。

而当可用泊车库有车位线标记，且车库相邻边上停有车辆时，则需要将环视摄像头和超声波雷达分别检测得到的车位信息进行融合，进而得到确定的停车位。需要注意的是，在环视摄像头和超声波雷达分别检测得到的车位信息进行融合前，若两者检测的车位信息偏差大于Δλm，则以超声波雷达检测的车位信息为参考值，否则以环视检测的车位信息为参考值。其中，上述Δλ的值可以根据传感器的车库检测性能参数进行标定。

图2显示了本发明所述的自动泊车方法在一种实施方式下的车辆坐标系示意图。

在本发明中上述的步骤200中，根据环视摄像头获取的车位角点位置信息或超声波雷达给出的相邻车位车辆相对本车的位置信息，通过坐标平移和转换，可以将车辆坐标系建立如图2。

如图2所示，在本实施方式中，设置点O为原点，原点O点横坐标为目标车位中轴线位置，O点纵坐标为目标车位的上边界位置，建立直角坐标系。建立车位坐标系后，可以得到本车相对车位坐标系的初始位置姿态(x0、y0、yaw0)。

图3示意性地显示了本发明所述的自动泊车方法在一种实施方式下的泊车起始区域范围。

如图3所示，同时结合参考图1，在本实施方式中，以停车位位于车辆的右侧进行分析研究，可以判断泊车起始区域。车辆泊车的起始位置通常是如图3所示泊车起始区域范围的左下极限位置和右上极限位置之间，水平方向上是从车辆的尾部在目标车位的右侧边界位置到车辆尾部到Y轴2倍车位宽度；在垂直方向上，从车辆的下边界与车辆上边界安全距离是ε，到车辆的上边界与车位上边界距离是Δr之间。其中，Δr的单位参量可以为m，且Δr值根据超声波雷达和环视传感器的有效探测范围确定。由此可以求出得到：

W_p/2≤(x_r-L_r)≤2W_p

上式中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r车辆的后轴中心坐标。

图4为本发明所述的自动泊车方法在一种实施方式下车辆与车位两侧碰撞规避示意图。

如图4所示，同时结合参考图1，在本发明所述自动泊车方法的步骤500中，根据上述求得的泊车起始区域，基于车辆与车位的碰撞规避决策，可以控制车辆自动泊车，直至进入确定的停车位。

需要说明的是，在本实施方式中，车辆与车位之间的碰撞规避决策可以包括：

车辆与车位右侧边界不发生碰撞的条件是：

(x_r-W_p/2)²+(R-y_r)²≤(R-L_b/2-L_safe1)²

车辆与车位左侧边界不发生碰撞的条件是：

其中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r表示车辆的后轴中心点的位置坐标，R表示车辆的最小转弯半径，L_safe1为车辆右侧边沿距离车位右上顶点的安全距离，L_safe2为车辆左后点与车位左边沿的安全距离。

此外，需要注意的是，在驾驶车辆进行车位搜索和自动泊车过程中，由于环视摄像头和超声波雷达的视场约束，有效车位并不是一直处于可检测状态，需要知道车辆相对所选有效车位的实时位姿关系，并进行路径跟踪控制，故需要实时根据车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正。

在本实施方式中，本发明所述自动泊车方法的步骤500基于下述公式对车辆相对车位坐标系的位姿进行估计：

x_r(k)＝x_r(k-1)+l_r(k-1)*cos(yaw_r(k))

y_r(k)＝y_r(k-1)+l_r(k-1)*sin(yaw_r(k))

其中，x_r、y_r表示车辆后轴中心点的位置坐标，yaw_r表示车辆航向角，

为车辆偏航角速率的估计值，l_r为根据车辆两后轮的齿轮脉冲计算得到的车辆在上个周期的行驶距离，Δt为采样周期，k、k-1分别表示k时刻和k-1时刻。

图5示意性地显示了本发明所述的自动泊车方法在一种实施方式下不同起始区域对应的泊车模式图。

图6示意性地显示了本发明所述的自动泊车方法在一种实施方式下泊车起始区域图。

需要说明的是，在本实施方式中，采用本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法进行自动泊车过程时，可以具有三种不同的泊车操作模式，例如：

模式1(Mode1)：挂入倒挡，将方向盘打到右极限位置，将车辆一次倒入停车位。

模式2(Mode2)：挂入倒挡，将方向盘打在某一中间位置，将车辆一次倒入停车位。

模式3(Mode3)：首先，将车辆挂入倒档，方向盘打到右极限位置；当车辆运动到快要与左侧车辆碰撞时，换成前进挡，方向盘打到左极限位置；车辆向前运动到某一位置，然后再换入倒档，方向盘打到右极限位置；当车辆进入停车位并且车身与两侧车辆平行时，方向盘回正，向后倒入停车位内。

本发明所述的基于环视摄像头和超声波雷达检测的自动泊车方法可以根据车辆与两侧车位的碰撞分析确定针对不同泊车起始位置的转向控制策略，并根据不同的转向控制策略可以得到图5。

如图5所示，在本实施方式中，可以将整个泊车起始区域分为：A和B。其中，A为可以将车辆一次性倒入车位的起始区域，B为不能将车辆一次倒入车位的起始区域。当车辆的起始位置位于A区域时，则采用上述泊车操作模式1(Mode1)，车辆挂入倒挡，车辆一次倒入停车位。而当车辆的起始位置位于B区域时，则车辆可以根据情况采用上述泊车操作模式2(Mode2)或模式3(Mode3)，车辆需要中间变换档位，才能成功泊车。

需要说明的是，在本实施方式中，根据不同的转向控制策略可以将图5对应的泊车起始区域进一步分解细化，细化后的泊车起始区域图如图6所示。

相应地，图6所示的泊车起始区域与车辆的泊车模式的对应关系列于表1中。

表1.

图7示意性地显示了本发明所述的自动泊车方法在一种实施方式下泊车转向控制策略流程图。

如图7所示，同时结合参考图5和图6，当车辆停在某一起始位置点后，车辆后轴中心点(车位坐标系起始坐标)的坐标是X，判断X是否在A区域内，如果X在A区域中，则需要进一步判断X是否在A1区域中，如果在A1区域中，就采用Mode1泊车模式，如果不在A1区域中则在A2区域中，就采用Mode2泊车模式。

而若一开始便判断X不在A区域中，则可知X在B区域中，如果X在B1区域中，则车辆向前开进入A1区域，然后按照Mode1泊车模式倒车；如果X不在B1区域中，X在B2区域中，则车辆向后开入A2区域，然后按照Mode2泊车模式倒车；如果X在B3区域中，则车辆采用Mode3泊车模式；如果X在区域B4中，则车辆向后行驶进入B3区域，然后按照Mode3泊车模式泊车。

图8为本发明所述的自动泊车方法在一种实施方式下车辆在Mode1和Mode2下的路径规划示意图。

如图8所示，在本发明所述的自动泊车方法中，Mode1泊车操作为：挂入倒挡，将方向盘打到右极限位置。在图8中，具有O、O1和O2三个点，其中O点横坐标为目标车位中轴线位置，纵坐标为目标车位的上边界位置，以O点为原点建立创建直角坐标系；相应地，O1点的横坐标由OO1＝R_min确定，纵坐标为目标车位的上边界位置。

进一步参阅图8可知，图8中圆O1为车辆在Mode1泊车模式位置处后轴中心点的轨迹圆；圆O2为车辆在Mode2泊车模式位置处后轴中心点的轨迹圆。其中，圆O2的半径可以根据车辆的纵向坐标进行计算。

图9为本发明所述的自动泊车方法在一种实施方式下车辆在Mode3下的路径规划示意图。

如图9所示，图9示意性地显示了车辆采用Mode3泊车模式下的路径规划的三段轨迹三角变化关系。

在本实施方式中，SABCD轨迹是车辆后轴中心点的轨迹线。其中，车辆起始位置点在S位置，车身方向沿S点处的箭头方向，且车辆的操作是：车辆挂入倒档，方向盘打到右极限位置，后轴中心点轨迹是沿着轨迹圆SA将车辆从S点倒到A点；相应地，车辆在A点变换成前进挡，方向盘打到左极限位置，后轴中心点轨迹是沿着轨迹圆AB将车辆从A点向前开到B点；此时，车辆在B点再变换成倒档，方向盘打到左极限位置，后轴中心点轨迹是沿着轨迹圆BC将车辆从B点倒到C点；在C点将方向盘转角调整为零，车辆后轴中心点轨迹是CD，以直线的形式将车辆倒到D点，最终泊车过程结束，完成自动泊车过程。

需要说明的是，在图9中，O为最小转向半径圆最下方位置；O₁第一段弧SA的圆心；O₂为第二段弧AB的圆心；O₃第三段弧BC的圆心；O₂的坐标(x_O2,y_O2)；O₃的坐标(x_O3,y_O3)；R₂为圆O₂的半径，根据车辆参数进行设计；R₃为圆O₃的半径，其根据车辆参数进行设计；φo2o3表示线段轴正方向的夹角；φ₂表示第二段弧AB对应的角度；φ₃表示第三段弧BC对应的角度；C’点为圆O与y轴的切点。

图10本发明所述的自动泊车方法在一种实施方式下的车辆在垂直动态调整示意图。

图10中所有的轨迹线均表示为：车辆在垂直动态调整过程中，车辆后轴中心点的轨迹。在本实施方式中，采用本发明所述的自动泊车方法进行自动泊车时，在垂直泊车工况中，常遇到泊车通道宽度不足的问题。例如：较窄的停车通道或泊车规划路径上存在墙或其他车辆。这些问题均会导致车辆无法按照原规划路径行驶，此时便需要进行如图10所示的动态调整。

继续参阅图10可知，当车辆按原规划路径行驶过程中，车前方超声波雷达探测到障碍物距离小于某一阈值时，则进行动态调整，将车辆转向盘转至右极限位置，路径为B1A2，到达A2点后参考图10的三角关系按A1B1和B1A2的规划方法，规划A2B2和B2D路径，而后再按新的规划路径泊车入库。若仍检测到障碍物距离小于安全阈值，则重复动态调整，直至顺利入库。

需要说明的是，本发明保护范围中现有技术部分并不局限于本申请文件所给出的实施例，所有不与本发明的方案相矛盾的现有技术，包括但不局限于在先专利文献、在先公开出版物，在先公开使用等等，都可纳入本发明的保护范围。

此外，本案中各技术特征的组合方式并不限本案权利要求中所记载的组合方式或是具体实施例所记载的组合方式，本案记载的所有技术特征可以以任何方式进行自由组合或结合，除非相互之间产生矛盾。

还需要注意的是，以上所列举的实施例仅为本发明具体实施例。显然本发明不局限于以上实施例，随之做出的类似变化或变形是本领域技术人员能从本发明公开的内容直接得出或者很容易便联想到的，均应属于本发明的保护范围。

Claims (10)

1.一种基于环视摄像头和超声波雷达检测的自动泊车方法，其特征在于，包括步骤：

100：采用环视摄像头和超声波雷达分别检测车位信息，并基于二者检测的车位信息得到确定的停车位；

200：采用车辆陀螺仪测量车辆偏航角速率，采用一阶卡尔曼滤波器对所述车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值；

300：基于所述确定的停车位，建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态；

400：判断泊车起始区域；

500：根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动泊车，直至进入所述确定的停车位，在自动泊车的过程中，实时根据所述车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正。

2.如权利要求1所述的自动泊车方法，其特征在于，在步骤500中，基于下述公式对车辆相对车位坐标系的位姿进行估计：

x_r(k)＝x_r(k-1)+l_r(k-1)*cos(yaw_r(k))

y_r(k)＝y_r(k-1)+l_r(k-1)*sin(yaw_r(k))

其中x_r、y_r表示车辆后轴中心点的位置坐标，yaw_r表示车辆航向角，

为车辆偏航角速率的估计值，l_r为根据车辆两后轮的齿轮脉冲计算得到的车辆在上个周期的行驶距离，Δt为采样周期，k、k-1分别表示k时刻和k-1时刻。

3.如权利要求1所述的自动泊车方法，其特征在于，在步骤200中，还采用一阶卡尔曼滤波器对车辆偏航角速率进行随机噪声滤波。

4.如权利要求1所述的自动泊车方法，其特征在于，在步骤200中，一阶卡尔曼滤波器的卡尔曼滤波状态预测方程为：

p[k]＝a*p[k-1]*a+q

卡尔曼滤波观测更新方程为：

kg＝p[k]*h/(h*p[k]*h+r)

p[k]＝(1-kg*h)*p[k-1]

其中，

为当前测量的车辆偏航角速率；

为当前车辆偏航角速率的估计值；a表示状态转移矩阵；p表示误差协方差矩阵；q表示状态噪声协方差矩阵；h表示观测矩阵；kg表示卡尔曼增益系数；r表示观测噪声协方差矩阵；k、k-1分别表示k时刻和k-1时刻。

5.如权利要求1所述的自动泊车方法，其特征在于，所述车辆与车位的碰撞规避决策包括：

车辆与车位右侧边界不发生碰撞的条件是：

(x_r-W_p/2)²+(R-y_r)²≤(R-L_b/2-L_safe1)²

车辆与车位左侧边界不发生碰撞的条件是：

其中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r表示车辆的后轴中心点的位置坐标，R表示车辆的最小转弯半径，L_safe1为车辆右侧边沿距离车位右上顶点的安全距离，L_safe2为车辆左后点与车位左边沿的安全距离。

6.一种基于环视摄像头和超声波雷达检测的自动泊车系统，其特征在于，包括：环视摄像头、超声波雷达、车辆陀螺仪、一阶卡尔曼滤波器、停车位决策模块、车辆位姿估计模块、泊车起始区域判断模块和泊车执行模块，其中：

所述环视摄像头和超声波雷达分别检测车位信息；

停车位决策模块基于环视摄像头和超声波雷达分别检测得到的车位信息，得到确定的停车位；

车辆陀螺仪测量车辆偏航角速率；

一阶卡尔曼滤波器对所述车辆偏航角速率进行常值偏差滤波，得到车辆偏航角速率的估计值；

车辆位姿估计模块，其基于所述确定的停车位建立车位坐标系，得到车辆相对车位坐标系的初始位置姿态；并且在自动泊车的过程中，实时根据所述车辆偏航角速率的估计值和采集到的车辆齿轮脉冲，对车辆相对车位坐标系的位姿进行估计，并根据环视摄像头的测量值和/或超声波雷达的测量值对车辆相对车位坐标系的初始位置姿态进行修正；

泊车起始区域判断模块，其判断车辆的泊车起始区域；

泊车执行模块，其根据泊车起始区域，基于车辆与车位的碰撞规避决策，控制车辆自动停泊至所述确定的停车位。

7.如权利要求6所述的自动泊车系统，其特征在于，所述车辆位姿估计模块基于下述公式对车辆相对车位坐标系的位姿进行估计：

x_r(k)＝x_r(k-1)+l_r(k-1)*cos(yaw_r(k))

y_r(k)＝y_r(k-1)+l_r(k-1)*sin(yaw_r(k))

其中x_r、y_r表示车辆后轴中心点的位置坐标，yaw_r表示车辆航向角，

为车辆偏航角速率的估计值，l_r为根据车辆两后轮的齿轮脉冲计算得到的车辆在上个周期的行驶距离，Δt为采样周期，k、k-1分别表示k时刻和k-1时刻。

8.如权利要求6所述的自动泊车系统，其特征在于，所述一阶卡尔曼滤波器还对车辆偏航角速率进行随机噪声滤波。

9.如权利要求6所述的自动泊车系统，其特征在于，所述一阶卡尔曼滤波器的卡尔曼滤波状态预测方程为：

p[k]＝a*p[k-1]*a+q

卡尔曼滤波观测更新方程为：

kg＝p[k]*h/(h*p[k]*h+r)

p[k]＝(1-kg*h)*p[k-1]

其中，

为当前测量的车辆偏航角速率；

为当前车辆偏航角速率的估计值；a表示状态转移矩阵；p表示误差协方差矩阵；q表示状态噪声协方差矩阵；h表示观测矩阵；kg表示卡尔曼增益系数；r表示观测噪声协方差矩阵；k、k-1分别表示k时刻和k-1时刻。

10.如权利要求6所述的自动泊车系统，其特征在于，所述车辆与车位的碰撞规避决策包括：

车辆与车位右侧边界不发生碰撞的条件是：

(x_r-W_p/2)²+(R-y_r)²≤(R-L_b/2-L_safe1)²

车辆与车位左侧边界不发生碰撞的条件是：

其中，W_p为车位宽度，L_b是车辆宽度，L_r车辆的后悬长度，x_r、y_r表示车辆的后轴中心点的位置坐标，R表示车辆的最小转弯半径，L_safe1为车辆右侧边沿距离车位右上顶点的安全距离，L_safe2为车辆左后点与车位左边沿的安全距离。

Images