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

权利要求书

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]

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

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所述的自动泊车方法，其特征在于，所述车辆与车位的碰撞规避决策包括：

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

(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为车辆左后点与车位左边沿的安全距离。