CN110077392A - 一种自动泊车定位系统的航迹推算方法 - Google Patents
一种自动泊车定位系统的航迹推算方法 Download PDFInfo
- Publication number
- CN110077392A CN110077392A CN201910243714.8A CN201910243714A CN110077392A CN 110077392 A CN110077392 A CN 110077392A CN 201910243714 A CN201910243714 A CN 201910243714A CN 110077392 A CN110077392 A CN 110077392A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- moment
- parking
- locator value
- vehicle course
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims abstract description 70
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 239000000654 additive Substances 0.000 claims abstract description 4
- 230000000996 additive effect Effects 0.000 claims abstract description 4
- 238000001914 filtration Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000000386 athletic effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
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控制单元完成整个自动泊车过程。
在自动泊车过程中，泊车航迹推算主要通过轮速脉冲信号，轮速，以及车身参数来匹配运算。采用车轮脉冲信号进行航迹推算来定位，其通过单位时间内左右轮运动行程来计算车身方位角增量和后轴中心位置的增量，分别对车身方位角和位置的增量进行累加，可得出车辆相对于起始位置的姿态。
航迹推算算法适用于短行程车辆位姿定位，但该方法存在一些误差来源。例如：航迹推算定位误差和非系统误差，航迹推算定位误差包括，车轮有效滚动半径的测量误差、两侧车轮的滚动半径不等、轮距测量值与真实值存在误差及码盘的分辨率不高等；非系统误差包括，路面不平、车轮打滑、溜车现象、轮胎载荷变化等，从而导致车辆位姿定位存在一定误差，进而航迹推算精度不高。
发明内容
本发明为克服上述现有技术所述的不足，提供一种自动泊车定位系统的航迹推算方法。
为解决上述技术问题，本发明的技术方案如下：
一种自动泊车定位系统的航迹推算方法，包括如下步骤：
计算泊车第一时刻车辆航向定位值；
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；
通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；
通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；
将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。
进一步的，作为优选技术方案，第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量的计算具体包括：
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量；
通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；
通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；
所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；
所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；
泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量 ；
；
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；
；
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：
；
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为 ：
；
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量 ；
；
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；
；
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：
；
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：
；
其中，和为车辆两个后轮的轮速， 为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的计算具体包括：
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；
线性卡尔曼滤波算法计算方程为：
；
+；
其中，为t+1时刻的系统状态向量值估计值，为t+1时刻协方差矩阵的估计值，为t+1时刻的系统噪声协方差矩阵，是对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，为系统噪声。
进一步的，作为优选技术方案，线性卡尔曼滤波算法计算方程为：
；
；
；
其中，是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，为t+1时刻的观察量。
进一步的，作为优选技术方案，泊车第二时刻车辆零飘误差值的计算具体包括：
以固定频率采集固定时间内的车身偏航率传感器值；
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用航迹推算方法进行车辆的航迹推算。
与现有技术相比，本发明技术方案的有益效果是：
本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，大幅度提高了航迹定位精准度；
同时，由于本发明中所采用各车辆运动信息基于传感器采集，而传感器的信号中包含噪声，对算法计算结果带来很大影响，因此通过线性卡尔曼滤波算法进行滤波，以消弱噪声对估算结果的影响，进而提高自动泊车定位的精确度；
最后，增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。
附图说明
图1为本发明方法步骤流程图。
图2为本发明结构框图。
图3为本发明坐标示意图。
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。
具体实施方式
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。
实施例1
一种自动泊车定位系统的航迹推算方法，如图1所示，包括如下步骤：
S10. 计算泊车第一时刻车辆航向定位值。
在本步骤中，泊车第一时刻车辆航向定位值包括泊车第一时刻车辆横坐标、纵坐标以及车辆航向角。
在本发明中，泊车第一时刻车辆航向定位值通过车轮脉冲信号结合车辆自身参数推到计算，而通过车轮脉冲信号结合车辆自身参数对车辆的运动姿态进行推到计算属于现有技术，在此不在进行赘述。
S20. 通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量。
本步骤具体包括：
S201. 通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
在本步骤中，当车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。
当车辆运动信息还包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。
S202. 通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；
S203. 通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。
由于车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角。因此，在本步骤中，泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的系统矩阵均通过以下公式计算：
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量通过以下公式计算泊车第二时刻车辆航向定位值的系统状态向量；
；
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；
即，对时刻系统状态进行增量计算，如下公式：
；
由车辆运动学模型公式可知，上式微分方程写成差分形式：
；
对上式线性化，得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵：
；
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：
假设车辆航向定位值的测量向量为[w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量 ：
；
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：
；
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。
在上述计算过程中，计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量和测量矩阵车辆的横摆角速度和车辆后轴中心的速度。
而当车辆运动信息包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，如图2-3所示：
；
；
因此，泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算：
；
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算：
；
其中，和为车辆两个后轮的轮速，为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。
S30. 通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值。
本步骤具体包括：
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；
线性卡尔曼滤波算法计算方程为：
；
+；
其中，为t+1时刻的系统状态向量值估计值，为t+1时刻协方差矩阵的估计值，为t+1时刻的系统噪声协方差矩阵，是对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，为系统噪声。
进一步的，线性卡尔曼滤波算法计算方程为：
；
；
；
其中，是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，为t+1时刻的观察量。
通过本步骤的滤波计算，得到t+1时刻的系统状态向量值，即分别得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角。
S40. 通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值。
本步骤具体为：
以固定频率采集固定时间内的车身偏航率传感器值；
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
在本步骤中，采集频率为20 ms每次，固定时间为1s，因此，本步骤具体为：气动泊车系统后，在车辆静止过程中重复以20 ms的频率采集1s以内的车身偏航率传感器值，共采样50个值，对采集的50个值进行均值滤波计算，以得到第二时刻车辆零飘误差值。
计算公式如下：
；
；
其中，YawRate 为车身偏航率传感器值，累计叠加50次，Yaw 为车身均值滤波后整车偏航率传感器值，即零飘误差值。
S50. 将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。
即，得到泊车第二时刻确定车辆位置信息（X，Y，θ）。
本步骤增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。
实施例2
一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用实施例1所述的航迹推算方法进行车辆的航迹推算。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
Claims (10)
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中任一项所述的航迹推算方法进行车辆的航迹推算。
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 true CN110077392A (zh) | 2019-08-02 |
| CN110077392B 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) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110940345A (zh) * | 2019-12-19 | 2020-03-31 | 深圳南方德尔汽车电子有限公司 | 车位定位、装置、计算机设备和存储介质 |
| CN112550278A (zh) * | 2020-12-03 | 2021-03-26 | 上汽大众汽车有限公司 | 一种基于环视摄像头和超声波雷达检测的自动泊车方法和系统 |
| CN113291314A (zh) * | 2020-02-21 | 2021-08-24 | 湖北亿咖通科技有限公司 | 一种车辆航向信息的计算方法及系统 |
| CN113887060A (zh) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车系统车辆定位算法 |
| CN114407877A (zh) * | 2022-02-25 | 2022-04-29 | 北京流马锐驰科技有限公司 | 一种基于轮速脉冲和imu的自动泊车定位方法 |
| CN114475581A (zh) * | 2022-02-25 | 2022-05-13 | 北京流马锐驰科技有限公司 | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 |
| WO2024066943A1 (zh) * | 2022-09-30 | 2024-04-04 | 中国第一汽车股份有限公司 | 应用于车辆的智能泊车车辆定位的方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN202106962U (zh) * | 2009-10-12 | 2012-01-11 | 福特全球技术公司 | 用于集成稳定控制系统的俯仰感测系统 |
| CN102372000A (zh) * | 2010-08-23 | 2012-03-14 | 北京经纬恒润科技有限公司 | 获取泊车位参数的装置和系统 |
| CN102414576A (zh) * | 2009-04-29 | 2012-04-11 | 皇家飞利浦电子股份有限公司 | 用于车辆电子稳定程序的基于激光二极管的自混合传感器 |
| CN102556075A (zh) * | 2011-12-15 | 2012-07-11 | 东南大学 | 一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法 |
| DE102011077173A1 (de) * | 2011-06-08 | 2012-12-13 | Robert Bosch Gmbh | Verfahren zur Bestimmung einer Einparktrajektorie |
| CN106394665A (zh) * | 2015-07-28 | 2017-02-15 | 株式会社捷太格特 | 车辆用转向操纵装置 |
| US10029577B2 (en) * | 2016-06-15 | 2018-07-24 | Qualcomm Incorporated | Methods and apparatus for positioning a vehicle |
| CN108357498A (zh) * | 2018-02-07 | 2018-08-03 | 北京新能源汽车股份有限公司 | 一种车辆状态参数确定方法、装置及汽车 |
| CN108873038A (zh) * | 2018-09-10 | 2018-11-23 | 芜湖盟博科技有限公司 | 自主泊车定位方法及定位系统 |
| CN109405837A (zh) * | 2018-11-14 | 2019-03-01 | 蔚来汽车有限公司 | 物体定位方法、应用和车辆 |
Family Cites Families (7)
| 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 |
| CN103453913B (zh) * | 2013-08-29 | 2016-04-06 | 镇江青思网络科技有限公司 | 停车场中车辆的组合定位方法 |
| CN104316059B (zh) * | 2014-11-05 | 2017-08-25 | 中国科学院嘉兴微电子与系统工程中心 | 由里程计获得车辆经纬度的航位推算导航定位方法及系统 |
| CN105946853B (zh) * | 2016-04-28 | 2018-05-29 | 中山大学 | 基于多传感器融合的长距离自动泊车的系统及方法 |
| CN107246868B (zh) * | 2017-07-26 | 2021-11-02 | 上海舵敏智能科技有限公司 | 一种协同导航定位系统及导航定位方法 |
| CN108281041A (zh) * | 2018-03-05 | 2018-07-13 | 东南大学 | 一种基于超声波和视觉传感器相融合的泊车车位检测方法 |
-
2019
- 2019-03-28 CN CN201910243714.8A patent/CN110077392B/zh active Active
- 2019-07-31 WO PCT/CN2019/098634 patent/WO2020191977A1/zh active Application Filing
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102414576A (zh) * | 2009-04-29 | 2012-04-11 | 皇家飞利浦电子股份有限公司 | 用于车辆电子稳定程序的基于激光二极管的自混合传感器 |
| CN202106962U (zh) * | 2009-10-12 | 2012-01-11 | 福特全球技术公司 | 用于集成稳定控制系统的俯仰感测系统 |
| CN102372000A (zh) * | 2010-08-23 | 2012-03-14 | 北京经纬恒润科技有限公司 | 获取泊车位参数的装置和系统 |
| DE102011077173A1 (de) * | 2011-06-08 | 2012-12-13 | Robert Bosch Gmbh | Verfahren zur Bestimmung einer Einparktrajektorie |
| CN102556075A (zh) * | 2011-12-15 | 2012-07-11 | 东南大学 | 一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法 |
| CN106394665A (zh) * | 2015-07-28 | 2017-02-15 | 株式会社捷太格特 | 车辆用转向操纵装置 |
| US10029577B2 (en) * | 2016-06-15 | 2018-07-24 | Qualcomm Incorporated | Methods and apparatus for positioning a vehicle |
| CN108357498A (zh) * | 2018-02-07 | 2018-08-03 | 北京新能源汽车股份有限公司 | 一种车辆状态参数确定方法、装置及汽车 |
| CN108873038A (zh) * | 2018-09-10 | 2018-11-23 | 芜湖盟博科技有限公司 | 自主泊车定位方法及定位系统 |
| CN109405837A (zh) * | 2018-11-14 | 2019-03-01 | 蔚来汽车有限公司 | 物体定位方法、应用和车辆 |
Non-Patent Citations (1)
| Title |
|---|
| 谢有浩等: "基于改进UKF的自动泊车车辆位姿估计算法研究", 《汽车技术》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110940345A (zh) * | 2019-12-19 | 2020-03-31 | 深圳南方德尔汽车电子有限公司 | 车位定位、装置、计算机设备和存储介质 |
| CN113291314A (zh) * | 2020-02-21 | 2021-08-24 | 湖北亿咖通科技有限公司 | 一种车辆航向信息的计算方法及系统 |
| CN112550278A (zh) * | 2020-12-03 | 2021-03-26 | 上汽大众汽车有限公司 | 一种基于环视摄像头和超声波雷达检测的自动泊车方法和系统 |
| CN113887060A (zh) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车系统车辆定位算法 |
| CN113887060B (zh) * | 2021-10-13 | 2024-05-10 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车系统车辆定位方法 |
| CN114407877A (zh) * | 2022-02-25 | 2022-04-29 | 北京流马锐驰科技有限公司 | 一种基于轮速脉冲和imu的自动泊车定位方法 |
| CN114475581A (zh) * | 2022-02-25 | 2022-05-13 | 北京流马锐驰科技有限公司 | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 |
| CN114475581B (zh) * | 2022-02-25 | 2022-09-16 | 北京流马锐驰科技有限公司 | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 |
| WO2024066943A1 (zh) * | 2022-09-30 | 2024-04-04 | 中国第一汽车股份有限公司 | 应用于车辆的智能泊车车辆定位的方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2020191977A1 (zh) | 2020-10-01 |
| CN110077392B (zh) | 2020-08-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110077392A (zh) | 一种自动泊车定位系统的航迹推算方法 | |
| CN108482379B (zh) | 轮毂电机驱动车辆路面附着系数及路面坡度同步实时估算系统及方法 | |
| CN111907516B (zh) | 一种全自动泊车方法和系统 | |
| CN103661599B (zh) | 一种车辆转弯轨迹预测系统和方法 | |
| CN101568798B (zh) | 用于确定车轮旋转轴线与旋转中心的方法 | |
| CN110307850A (zh) | 航迹推算定位方法及自动泊车系统 | |
| CN104590414A (zh) | 一种具有相对位姿检测功能的全方位轮式移动机器人 | |
| CN109823393A (zh) | 一种智能驾驶车辆轨迹跟踪控制方法 | |
| CN103661398B (zh) | 一种基于滑模观测器的车辆非转向左后轮线速度估计方法 | |
| CN108819950B (zh) | 汽车稳定性控制系统的车速估计方法及系统 | |
| CN109143255B (zh) | 一种铰接轮式装载机姿态预测方法 | |
| CN102416956A (zh) | 汽车质心侧偏角和轮胎侧偏角软测量方法 | |
| CN113247004A (zh) | 一种车辆质量与道路横向坡度的联合估计方法 | |
| CN109374315B (zh) | 车辆车轮瞬时滑转率的获取方法、控制方法、系统、装置 | |
| CN112097792B (zh) | 一种阿克曼模型移动机器人里程计标定方法 | |
| CN110530361A (zh) | 一种基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
| CN110001630A (zh) | 一种分布式驱动电动汽车自动泊车行车轨迹差动修正系统及方法 | |
| CN102582626A (zh) | 重型半挂车状态估计方法 | |
| CN104990563A (zh) | 车辆行驶里程计算方法 | |
| CN111806430B (zh) | 一种用于自动泊车的车速计算方法 | |
| CN111076951B (zh) | 一种履带式车辆转向滑转率的测试方法 | |
| CN109591827B (zh) | 一种基于侧向速度估计的汽车质量辨识方法 | |
| CN112389438A (zh) | 一种车辆转向系统传动比的确定方法及装置 | |
| Chaichaowarat et al. | Kinematics-based analytical solution for wheel slip angle estimation of a RWD vehicle with drift | |
| CN103253272B (zh) | 一种汽车极限工况下轮胎侧偏角测试方法 |
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 |