CN110077392B - 一种自动泊车定位系统的航迹推算方法 - Google Patents

一种自动泊车定位系统的航迹推算方法 Download PDF

Info

Publication number
CN110077392B
CN110077392B CN201910243714.8A CN201910243714A CN110077392B CN 110077392 B CN110077392 B CN 110077392B CN 201910243714 A CN201910243714 A CN 201910243714A CN 110077392 B CN110077392 B CN 110077392B
Authority
CN
China
Prior art keywords
vehicle
parking
moment
value
time
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910243714.8A
Other languages
English (en)
Other versions
CN110077392A (zh
Inventor
王德祥
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huizhou Desay SV Automotive Co Ltd
Original Assignee
Huizhou Desay SV Automotive Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huizhou Desay SV Automotive Co Ltd filed Critical Huizhou Desay SV Automotive Co Ltd
Priority to CN201910243714.8A priority Critical patent/CN110077392B/zh
Priority to PCT/CN2019/098634 priority patent/WO2020191977A1/zh
Publication of CN110077392A publication Critical patent/CN110077392A/zh
Application granted granted Critical
Publication of CN110077392B publication Critical patent/CN110077392B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/06—Automatic manoeuvring for parking
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/06—Direction of travel
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/28—Wheel speed

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种自动泊车定位系统的航迹推算方法，包括步骤：计算泊车第一时刻车辆航向定位值；通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；通过线性卡尔曼滤波算法对其分别进行滤波估算以得到泊车第二时刻车辆航向定位值；将泊车第二时刻车辆航向定位值的车辆航向角与计算得到的泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，然后消除零飘误差，得到精准实时航向定位值。

Description

一种自动泊车定位系统的航迹推算方法
技术领域
本发明涉及泊车技术领域，特别是涉及一种自动泊车定位系统的航迹推算方法。
背景技术
随着汽车行业的蓬勃发展，自动泊车系统目前已经应用到各大汽车厂商的高端配置车辆上，自动泊车过程分四步进行，首先，对车位进行检测，车位检测过程中需要定位跟踪车辆位置，航迹推算系统触发，当得到可以停靠的车位后，车辆继续往前走，车位需要根据之前的位置信息跟当前车辆的位置信息跟踪车位坐标；第二步，将车位坐标信息存储到控制器ECU中，控制器ECU获取车辆信息并建立车辆坐标系；第三步，控制器ECU根据车辆的位置信息、坐标信息、雷达信息等条件规划目标泊车轨迹；最后，泊车控制系统通过实时的航迹跟踪推算，根据目标挡位、目标转角、目标速度、目标加速度以及目标位置输出控制车身自动泊车，ECU控制单元完成整个自动泊车过程。
在自动泊车过程中，泊车航迹推算主要通过轮速脉冲信号，轮速，以及车身参数来匹配运算。采用车轮脉冲信号进行航迹推算来定位，其通过单位时间内左右轮运动行程来计算车身方位角增量和后轴中心位置的增量，分别对车身方位角和位置的增量进行累加，可得出车辆相对于起始位置的姿态。
航迹推算算法适用于短行程车辆位姿定位，但该方法存在一些误差来源。例如：航迹推算定位误差和非系统误差，航迹推算定位误差包括，车轮有效滚动半径的测量误差、两侧车轮的滚动半径不等、轮距测量值与真实值存在误差及码盘的分辨率不高等；非系统误差包括，路面不平、车轮打滑、溜车现象、轮胎载荷变化等，从而导致车辆位姿定位存在一定误差，进而航迹推算精度不高。
发明内容
本发明为克服上述现有技术所述的不足，提供一种自动泊车定位系统的航迹推算方法。
为解决上述技术问题，本发明的技术方案如下：
一种自动泊车定位系统的航迹推算方法，包括如下步骤：
计算泊车第一时刻车辆航向定位值；
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；
通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；
通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；
将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。
进一步的，作为优选技术方案，第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量的计算具体包括：
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量；
通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；
通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；
所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；
所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；
泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)；
Figure GDA0002491534760000031
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ1(t+1，t)；
Figure GDA0002491534760000032
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)：
Figure GDA0002491534760000033
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H1(t+1)：
Figure GDA0002491534760000041
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)；
Figure GDA0002491534760000042
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ1(t+1，t)；
Figure GDA0002491534760000051
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z1为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)：
Figure GDA0002491534760000052
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H1(t+1)：
Figure GDA0002491534760000053
其中，V3和V4为车辆两个后轮的轮速，
Figure GDA0002491534760000054
为后轴中心点转弯半径，
Figure GDA0002491534760000055
为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的计算具体包括：
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；
线性卡尔曼滤波算法计算方程为：
Figure GDA0002491534760000061
Figure GDA0002491534760000062
其中，
Figure GDA0002491534760000063
为t+1时刻的系统状态向量值估计值，Pt+1，t为t+1时刻协方差矩阵的估计值，Qt+1为t+1时刻的系统噪声协方差矩阵，φt+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，Wt为系统噪声。
进一步的，作为优选技术方案，线性卡尔曼滤波算法计算方程为：
Figure GDA0002491534760000064
Figure GDA0002491534760000065
Pt+1＝[1-Kt+1 Ht+1]Pt+1，t；
其中，Ht+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Zt+1为t+1时刻的观察量。
进一步的，作为优选技术方案，泊车第二时刻车辆零飘误差值的计算具体包括：
以固定频率采集固定时间内的车身偏航率传感器值；
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用航迹推算方法进行车辆的航迹推算。
与现有技术相比，本发明技术方案的有益效果是：
本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，大幅度提高了航迹定位精准度；
同时，由于本发明中所采用各车辆运动信息基于传感器采集，而传感器的信号中包含噪声，对算法计算结果带来很大影响，因此通过线性卡尔曼滤波算法进行滤波，以消弱噪声对估算结果的影响，进而提高自动泊车定位的精确度；
最后，增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。
附图说明
图1为本发明方法步骤流程图。
图2为本发明结构框图。
图3为本发明坐标示意图。
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。
具体实施方式
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。
实施例1
一种自动泊车定位系统的航迹推算方法，如图1所示，包括如下步骤：
S10.计算泊车第一时刻车辆航向定位值。
在本步骤中，泊车第一时刻车辆航向定位值包括泊车第一时刻车辆横坐标、纵坐标以及车辆航向角。
在本发明中，泊车第一时刻车辆航向定位值通过车轮脉冲信号结合车辆自身参数推到计算，而通过车轮脉冲信号结合车辆自身参数对车辆的运动姿态进行推到计算属于现有技术，在此不在进行赘述。
S20.通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量。
本步骤具体包括：
S201.通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
在本步骤中，当车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
当车辆运动信息还包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。
S202.通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；
S203.通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。
由于车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角。因此，在本步骤中，泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的系统矩阵均通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1为[x，y，θ，w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t)通过以下公式计算泊车第二时刻车辆航向定位值的系统状态向量X1(t+1)；
Figure GDA0002491534760000091
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ1(t+1，t)；
即，对
Figure GDA0002491534760000092
时刻系统状态进行增量计算，如下公式：
Figure GDA0002491534760000093
由车辆运动学模型公式可知，上式微分方程写成差分形式：
Figure GDA0002491534760000094
对上式线性化，得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ1(t+1，t)：
Figure GDA0002491534760000095
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值的测量向量Z1为[w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t)通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)：
Figure GDA0002491534760000101
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H1(t+1)：
Figure GDA0002491534760000102
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。
在上述计算过程中，计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)和测量矩阵车辆的横摆角速度和车辆后轴中心的速度。
而当车辆运动信息包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，如图2-3所示：
Figure GDA0002491534760000103
Figure GDA0002491534760000104
因此，泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t)通过以下公式计算：
Figure GDA0002491534760000111
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)通过以下公式计算：
Figure GDA0002491534760000112
其中，V3和V4为车辆两个后轮的轮速，
Figure GDA0002491534760000113
为后轴中心点转弯半径，
Figure GDA0002491534760000114
为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。
S30.通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值。
本步骤具体包括：
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；
线性卡尔曼滤波算法计算方程为：
Figure GDA0002491534760000115
Figure GDA0002491534760000116
其中，
Figure GDA0002491534760000117
为t+1时刻的系统状态向量值估计值，Pt+1，t为t+1时刻协方差矩阵的估计值，Qt+1为t+1时刻的系统噪声协方差矩阵，φt+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，Wt为系统噪声。
进一步的，线性卡尔曼滤波算法计算方程为：
Figure GDA0002491534760000121
Figure GDA0002491534760000122
Pt+1＝[1-Kt+1 Ht+1]Pt+1，t；
其中，Ht+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Zt+1为t+1时刻的观察量。
通过本步骤的滤波计算，得到t+1时刻的系统状态向量值，即分别得到泊车第二时刻车辆航向定位值的车辆横坐标Xt+1、纵坐标Yt+1以及车辆航向角θt+1。
S40.通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值。
本步骤具体为：
以固定频率采集固定时间内的车身偏航率传感器值；
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
在本步骤中，采集频率为20ms每次，固定时间为1s，因此，本步骤具体为：气动泊车系统后，在车辆静止过程中重复以20ms的频率采集1s以内的车身偏航率传感器值，共采样50个值，对采集的50个值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
计算公式如下：
YawRate+＝YawRate；
Yaw＝YawRate/50.0f；
其中，YawRate为车身偏航率传感器值，累计叠加50次，Yaw为车身均值滤波后整车偏航率传感器值，即零飘误差值。
S50.将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。
即，得到泊车第二时刻确定车辆位置信息(X，Y，θ)。
本步骤增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。
实施例2
一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用实施例1所述的航迹推算方法进行车辆的航迹推算。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (5)

1.一种自动泊车定位系统的航迹推算方法，其特征在于，包括如下步骤：
计算泊车第一时刻车辆航向定位值；
通过泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量，通过泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度或者通过泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量；
通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过泊车第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；
所述泊车第一时刻车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；
假设泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t)为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)；
Figure FDA0002504010320000011
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量X1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统矩阵φ1(t+1，t)；
Figure FDA0002504010320000012
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过泊车第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵；
假设泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z1(t)为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)：
Figure FDA0002504010320000021
或者，
Figure FDA0002504010320000022
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H1(t+1)：
Figure FDA0002504010320000023
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，V3和V4为车辆两个后轮的轮速，
Figure FDA0002504010320000024
为后轴中心点转弯半径，
Figure FDA0002504010320000025
为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻；
通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；
通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；
将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。
2.根据权利要求1所述的自动泊车定位系统的航迹推算方法，其特征在于，
泊车第二时刻车辆航向定位值的计算具体包括：
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；
线性卡尔曼滤波算法计算方程为：
Figure FDA0002504010320000026
Figure FDA0002504010320000027
其中，
Figure FDA0002504010320000031
为t时刻到t+1时刻系统状态向量值初步预估值，Pt+1，t为t+1时刻协方差矩阵初步预估值，Pt为t时刻的协方差矩阵，Qt+1为t+1时刻的系统噪声协方差矩阵，φt+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，Xt代表t时刻的系统状态变量值，Wt为系统噪声。
3.根据权利要求2所述的自动泊车定位系统的航迹推算方法，其特征在于，线性卡尔曼滤波算法计算方程为：
Figure FDA0002504010320000032
Figure FDA0002504010320000033
Pt+1＝[1-Kt+1Ht+1]Pt+1，t；
其中，
Figure FDA0002504010320000034
为t+1时刻的系统状态向量值估计值，
Figure FDA0002504010320000035
为t时刻到t+1时刻系统状态向量值初步预估值，Kt+1为t+1时刻卡尔曼滤波增益矩阵，Zt+1为t+1时刻的观察量，Pt+1，t为t+1时刻协方差矩阵初步预估值，Ht+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Rt+1为t+1时刻测量噪声协方差矩阵，Pt+1为t+1时刻协方差矩阵预估值。
4.根据权利要求1所述的自动泊车定位系统的航迹推算方法，其特征在于，泊车第二时刻车辆零飘误差值的计算具体包括：
以固定频率采集固定时间内的车身偏航率传感器值；
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
5.一种自动泊车定位系统，其特征在于，包括航迹推算模块，所述航迹推算模块采用权利要求1～4中任一项所述的航迹推算方法进行车辆的航迹推算。
CN201910243714.8A 2019-03-28 2019-03-28 一种自动泊车定位系统的航迹推算方法 Active CN110077392B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910243714.8A CN110077392B (zh) 2019-03-28 2019-03-28 一种自动泊车定位系统的航迹推算方法
PCT/CN2019/098634 WO2020191977A1 (zh) 2019-03-28 2019-07-31 一种自动泊车定位系统的航迹推算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910243714.8A CN110077392B (zh) 2019-03-28 2019-03-28 一种自动泊车定位系统的航迹推算方法

Publications (2)

Publication Number Publication Date
CN110077392A CN110077392A (zh) 2019-08-02
CN110077392B true CN110077392B (zh) 2020-08-25

Family

ID=67413825

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910243714.8A Active CN110077392B (zh) 2019-03-28 2019-03-28 一种自动泊车定位系统的航迹推算方法

Country Status (2)

Country Link
CN (1) CN110077392B (zh)
WO (1) WO2020191977A1 (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110940345B (zh) * 2019-12-19 2022-03-18 深圳南方德尔汽车电子有限公司 车身定位、装置、计算机设备和存储介质
CN113291314B (zh) * 2020-02-21 2022-06-14 亿咖通(湖北)技术有限公司 一种车辆航向信息的计算方法及系统
CN112550278B (zh) * 2020-12-03 2022-05-17 上汽大众汽车有限公司 一种基于环视摄像头和超声波雷达检测的自动泊车方法和系统
CN113887060B (zh) * 2021-10-13 2024-05-10 英博超算(南京)科技有限公司 一种新型的自动泊车系统车辆定位方法
CN114475581B (zh) * 2022-02-25 2022-09-16 北京流马锐驰科技有限公司 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法
CN114407877B (zh) * 2022-02-25 2023-03-24 北京流马锐驰科技有限公司 一种基于轮速脉冲和imu的自动泊车定位方法
CN115900695A (zh) * 2022-09-30 2023-04-04 中国第一汽车股份有限公司 一种应用于车辆的智能泊车车辆定位的方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102372000A (zh) * 2010-08-23 2012-03-14 北京经纬恒润科技有限公司 获取泊车位参数的装置和系统
CN108357498A (zh) * 2018-02-07 2018-08-03 北京新能源汽车股份有限公司 一种车辆状态参数确定方法、装置及汽车

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3570372B2 (ja) * 2000-11-08 2004-09-29 株式会社デンソー 車両用現在位置検出装置、車両用現在位置表示装置、ナビゲーション装置および記録媒体
US9157737B2 (en) * 2008-06-11 2015-10-13 Trimble Navigation Limited Altimeter with calibration
EP2425274B1 (en) * 2009-04-29 2017-10-18 Koninklijke Philips N.V. Laser diode based self-mixing sensor for a vehicle electronic stability program
US9403415B2 (en) * 2009-10-12 2016-08-02 Ford Global Technologies GPS based pitch sensing for an integrated stability control system
DE102011077173A1 (de) * 2011-06-08 2012-12-13 Robert Bosch Gmbh Verfahren zur Bestimmung einer Einparktrajektorie
CN102556075B (zh) * 2011-12-15 2015-04-01 东南大学 一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法
CN103453913B (zh) * 2013-08-29 2016-04-06 镇江青思网络科技有限公司 停车场中车辆的组合定位方法
CN104316059B (zh) * 2014-11-05 2017-08-25 中国科学院嘉兴微电子与系统工程中心 由里程计获得车辆经纬度的航位推算导航定位方法及系统
JP2017024683A (ja) * 2015-07-28 2017-02-02 株式会社ジェイテクト 車両用操舵装置
CN105946853B (zh) * 2016-04-28 2018-05-29 中山大学 基于多传感器融合的长距离自动泊车的系统及方法
US10029577B2 (en) * 2016-06-15 2018-07-24 Qualcomm Incorporated Methods and apparatus for positioning a vehicle
CN107246868B (zh) * 2017-07-26 2021-11-02 上海舵敏智能科技有限公司 一种协同导航定位系统及导航定位方法
CN108281041A (zh) * 2018-03-05 2018-07-13 东南大学 一种基于超声波和视觉传感器相融合的泊车车位检测方法
CN108873038B (zh) * 2018-09-10 2020-11-06 芜湖盟博科技有限公司 自主泊车定位方法及定位系统
CN109405837B (zh) * 2018-11-14 2021-06-08 蔚来(安徽)控股有限公司 物体定位方法、应用和车辆

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102372000A (zh) * 2010-08-23 2012-03-14 北京经纬恒润科技有限公司 获取泊车位参数的装置和系统
CN108357498A (zh) * 2018-02-07 2018-08-03 北京新能源汽车股份有限公司 一种车辆状态参数确定方法、装置及汽车

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于改进UKF的自动泊车车辆位姿估计算法研究;谢有浩等;《汽车技术》;20190225(第03期);第8-12页 *

Also Published As

Publication number Publication date
WO2020191977A1 (zh) 2020-10-01
CN110077392A (zh) 2019-08-02

Similar Documents

Publication Publication Date Title
CN110077392B (zh) 一种自动泊车定位系统的航迹推算方法
CN108482379B (zh) 轮毂电机驱动车辆路面附着系数及路面坡度同步实时估算系统及方法
Bevly et al. The use of GPS based velocity measurements for measurement of sideslip and wheel slip
US6522956B2 (en) Method and device for estimating a transverse acceleration at an axle of a semitrailer or a trailer of a vehicle combination
CN109916431B (zh) 一种针对四轮移动机器人的车轮编码器标定算法
CN111907516B (zh) 一种全自动泊车方法和系统
CN108297872A (zh) 全工况车载路面坡度估算装置和方法
CN110987463A (zh) 面向多场景的智能驾驶自主车道变换性能测试方法
US11079237B2 (en) Method for determining a relative position of a motor vehicle, position determination system for a motor vehicle and motor vehicle
CN108819950B (zh) 汽车稳定性控制系统的车速估计方法及系统
CN110341714B (zh) 一种同时估计车辆质心侧偏角和扰动的方法
CN111547059A (zh) 一种分布式驱动电动汽车惯性参数估计方法
CN110530361B (zh) 基于农业机械双天线gnss自动导航系统的转向角度估计器
CN105172793A (zh) 自动驾驶汽车的位姿估算方法
CN103661398B (zh) 一种基于滑模观测器的车辆非转向左后轮线速度估计方法
CN111806430B (zh) 一种用于自动泊车的车速计算方法
CN102582626A (zh) 重型半挂车状态估计方法
CN111751857A (zh) 一种车辆位姿的估算方法、装置、存储介质及系统
CN111006884B (zh) 基于傅立叶变换的车轮车轴侧偏角与侧偏刚度的测量方法
CN113247004A (zh) 一种车辆质量与道路横向坡度的联合估计方法
CN114475581B (zh) 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法
CN112278072A (zh) 一种智能车转向安全控制系统及控制方法
CN111504309B (zh) 低速运动中汽车位姿的计算方法
Kochem et al. Accurate local vehicle dead-reckoning for a parking assistance system
CN111665530A (zh) 一种基于车辆状态的gps诊断方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant