详情

原文传递一种自动泊车定位系统的航迹推算方法

专利名称：	一种自动泊车定位系统的航迹推算方法
摘要：	本发明涉及一种自动泊车定位系统的航迹推算方法，包括步骤：计算泊车第一时刻车辆航向定位值；通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；通过线性卡尔曼滤波算法对其分别进行滤波估算以得到泊车第二时刻车辆航向定位值；将泊车第二时刻车辆航向定位值的车辆航向角与计算得到的泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，然后消除零飘误差，得到精准实时航向定位值。
专利类型：	发明专利
国家地区组织代码：	广东;44
申请人：	惠州市德赛西威汽车电子股份有限公司
发明人：	王德祥
专利状态：	有效
申请日期：	2019-03-28T00:00:00+0800
发布日期：	2019-08-02T00:00:00+0800
申请号：	CN201910243714.8
公开号：	CN110077392A
代理机构：	广州粤高专利商标代理有限公司
代理人：	陈卫;练逸夫
分类号：	B60W30/06(2006.01);B;B60;B60W;B60W30
申请人地址：	516006 广东省惠州市仲恺高新区和畅五路西103号
主权项：	1.一种自动泊车定位系统的航迹推算方法，其特征在于，包括如下步骤：计算泊车第一时刻车辆航向定位值；通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。 2.根据权利要求1所述的自动泊车定位系统的航迹推算方法，其特征在于，第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量的计算具体包括：通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量；通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。 3.根据权利要求2所述的自动泊车定位系统的航迹推算方法，其特征在于，所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。 4.根据权利要求2所述的自动泊车定位系统的航迹推算方法，其特征在于，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。 5.根据权利要求3所述的自动泊车定位系统的航迹推算方法，其特征在于，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量；；对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；；其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：；以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：；其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。 6.根据权利要求4所述的自动泊车定位系统的航迹推算方法，其特征在于，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量；；对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；；其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：；以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：；其中，和为车辆两个后轮的轮速，为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。 7.根据权利要求5或6任一项所述的自动泊车定位系统的航迹推算方法，其特征在于，泊车第二时刻车辆航向定位值的计算具体包括：分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；线性卡尔曼滤波算法计算方程为：； + ；其中，为t+1时刻的系统状态向量值估计值，为t+1时刻协方差矩阵的估计值，为t+1时刻的系统噪声协方差矩阵，是对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，为系统噪声。 8.根据权利要求7所述的自动泊车定位系统的航迹推算方法，其特征在于，线性卡尔曼滤波算法计算方程为：；；；其中，是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，为t+1时刻的观察量。 9.根据权利要求1所述的自动泊车定位系统的航迹推算方法，其特征在于，泊车第二时刻车辆零飘误差值的计算具体包括：以固定频率采集固定时间内的车身偏航率传感器值；对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。 10.一种自动泊车定位系统，其特征在于，包括航迹推算模块，所述航迹推算模块采用权利要求1~9中任一项所述的航迹推算方法进行车辆的航迹推算。
所属类别：	发明专利