[发明专利]一种自动泊车定位系统的航迹推算方法

权利要求书

1.一种自动泊车定位系统的航迹推算方法，其特征在于，包括如下步骤：

计算泊车第一时刻车辆航向定位值；

通过泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量，通过泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度或者通过泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量；

通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过泊车第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；

所述泊车第一时刻车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；

假设泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X_1(t)为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X_1(t+1)；

对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X_1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ_1(t+1，t)；

其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；

通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过泊车第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵；

假设泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z_1(t)为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z_1(t+1)：

或者，

以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z_1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H_1(t+1)：

其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，V₃和V₄为车辆两个后轮的轮速，为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻；

通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；

通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；

将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。

2.根据权利要求1所述的自动泊车定位系统的航迹推算方法，其特征在于，

泊车第二时刻车辆航向定位值的计算具体包括：

分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；

线性卡尔曼滤波算法计算方程为：

其中，为t时刻到t+1时刻系统状态向量值初步预估值，P_t+1，t为t+1时刻协方差矩阵初步预估值，P_t为t时刻的协方差矩阵，Q_t+1为t+1时刻的系统噪声协方差矩阵，φ_t+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，X_t代表t时刻的系统状态变量值，W_t为系统噪声。