WO2020191977A1 - Dead reckoning method of automatic parking positioning system - Google Patents

Dead reckoning method of automatic parking positioning system Download PDF

Info

Publication number
WO2020191977A1
WO2020191977A1 PCT/CN2019/098634 CN2019098634W WO2020191977A1 WO 2020191977 A1 WO2020191977 A1 WO 2020191977A1 CN 2019098634 W CN2019098634 W CN 2019098634W WO 2020191977 A1 WO2020191977 A1 WO 2020191977A1
Authority
WO
WIPO (PCT)
Prior art keywords
vehicle
parking
moment
value
vehicle heading
Prior art date
Application number
PCT/CN2019/098634
Other languages
French (fr)
Chinese (zh)
Inventor
王德祥
Original Assignee
惠州市德赛西威汽车电子股份有限公司
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 惠州市德赛西威汽车电子股份有限公司 filed Critical 惠州市德赛西威汽车电子股份有限公司
Publication of WO2020191977A1 publication Critical patent/WO2020191977A1/en

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

Definitions

  • the present invention relates to the technical field of parking, in particular to a track estimation method of an automatic parking positioning system.
  • the automatic parking process is carried out in four steps. First, the parking space is detected. During the parking space detection process, the vehicle needs to be located and tracked. The position and track estimation system is triggered. When a parking space is obtained, the vehicle continues to move forward.
  • the parking space needs to track the parking space coordinates according to the previous position information and the current vehicle position information; the second step is to store the parking space coordinate information to the control In the ECU, the controller ECU obtains vehicle information and establishes the vehicle coordinate system; the third step, the controller ECU plans the target parking trajectory according to the vehicle’s position information, coordinate information, radar information and other conditions; finally, the parking control system passes real-time According to the target gear, target turning angle, target speed, target acceleration and target position output, the vehicle body is automatically parked and the ECU control unit completes the entire automatic parking process.
  • the parking track calculation is mainly based on the matching calculation of wheel speed pulse signals, wheel speed, and vehicle body parameters.
  • the wheel pulse signal is used to calculate the track for positioning. It calculates the body azimuth angle increment and the rear axle center position increment through the movement of the left and right wheels per unit time, and accumulates the body azimuth angle and position increment respectively. Get the attitude of the vehicle relative to the starting position.
  • track estimation algorithm is suitable for short-stroke vehicle pose positioning, but this method has some sources of error.
  • track calculation positioning error and non-systematic error track calculation positioning error includes the measurement error of the effective rolling radius of the wheel, the unequal rolling radius of the wheels on both sides, the error between the measured value of the track and the real value, and the resolution of the code disc The rate is not high; non-systematic errors include uneven road surface, wheel slip, car rolling phenomenon, tire load change, etc., which lead to a certain error in vehicle pose positioning, and the accuracy of track estimation is not high.
  • the present invention provides a method for calculating the track of an automatic parking positioning system.
  • the technical scheme of the present invention is as follows:
  • a method for estimating track of an automatic parking and positioning system includes the following steps:
  • the system matrix, measurement matrix and observation quantity of the vehicle heading positioning value at the second time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
  • the linear Kalman filter algorithm is used to filter and estimate the system matrix, the measurement matrix and the observation value of the vehicle heading at the second moment of parking to obtain the vehicle heading positioning value at the second time of parking;
  • the vehicle heading angle of the vehicle heading positioning value at the second time of parking is subtracted from the zero drift error value of the vehicle at the second time of parking to obtain the vehicle heading positioning value at the second time of parking.
  • the calculation of the system matrix, the measurement matrix, and the observation amount of the vehicle heading positioning value at the second moment specifically includes:
  • the system state vector and measurement vector of the vehicle heading positioning value at the first time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
  • the system state vector of the vehicle heading positioning value at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the first moment of parking, and the vehicle at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the second time System matrix of heading positioning value;
  • the vehicle motion information includes the yaw rate of the vehicle and the speed of the center of the rear axle of the vehicle;
  • the calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
  • the vehicle heading positioning value at the first moment of parking is respectively combined with the vehicle's yaw rate and the speed of the rear axle center to calculate the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking.
  • the vehicle motion information includes the yaw rate of the vehicle, the speed of the center of the rear axle of the vehicle, the speed of the two rear wheels of the vehicle, the turning radius of the center of the rear axle, and the steering wheel angle;
  • the calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
  • the vehicle heading positioning value at the first moment of parking is calculated by combining the vehicle's yaw rate and the speed of the vehicle's rear axle center to obtain the system state vector of the vehicle heading positioning value at the first time of parking;
  • the vehicle heading positioning value at the first moment of parking is calculated by combining the two rear wheel speeds of the vehicle, the turning radius of the rear axle center point and the steering wheel angle to obtain the measurement vector of the vehicle heading positioning value at the first time of parking.
  • system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
  • the vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle
  • the vehicle heading positioning value at the second moment of parking is calculated by the following formula
  • x, y, ⁇ are the vehicle heading positioning values at the first moment of parking
  • w is the yaw rate of the vehicle
  • v is the speed of the center of the rear axle of the vehicle
  • t is the first moment of parking
  • t+1 is the first moment of parking.
  • the observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
  • the vehicle's abscissa, ordinate or vehicle heading angle measurement vector in the vehicle heading positioning value is [w, v]T
  • the vehicle's abscissa and ordinate in the vehicle heading positioning value at the second moment of parking are calculated by the following formula Or the observation quantity Z 1(t+1) of the vehicle heading angle:
  • w is the yaw rate of the vehicle
  • v is the speed of the center of the rear axle of the vehicle
  • t is the first moment of parking
  • t+1 is the second moment of parking.
  • system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
  • the vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle
  • the vehicle heading positioning value at the second moment of parking is calculated by the following formula
  • x, y, ⁇ are the vehicle heading positioning values at the first moment of parking
  • w is the yaw rate of the vehicle
  • v is the speed of the center of the rear axle of the vehicle
  • t is the first moment of parking
  • t+1 is the first moment of parking.
  • the observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
  • V 3 and V 4 are the wheel speeds of the two rear wheels of the vehicle, Is the turning radius at the center of the rear axle, Is the steering wheel angle, t is the first moment of parking, and t+1 is the second moment of parking.
  • the calculation of the vehicle heading positioning value at the second moment of parking specifically includes:
  • the linear Kalman filter algorithm is used to filter and calculate the system matrix, measurement matrix and observations of the vehicle's horizontal coordinate, vertical coordinate, and vehicle heading angle of the vehicle heading position value at the second moment of parking through the following formulas to obtain the second parking time
  • t is the Jacobian matrix after f takes the first derivative of X, that is, the system matrix at time t+1, and W t is the system noise.
  • H t+1 is the Jacobian matrix obtained by calculating the first-order partial derivative of h with respect to X, that is, the measurement matrix at time t+1, and Z t+1 is the observation quantity at time t+1.
  • the calculation of the zero drift error value of the vehicle at the second moment of parking specifically includes:
  • An automatic parking positioning system includes a track estimation module, and the track estimation module adopts a track estimation method to calculate the track of a vehicle.
  • the result of the track estimation is used as the measurement quantity, combined with the yaw rate of the vehicle and the speed of the vehicle rear axle center and other parameters, and the linear Kalman filter algorithm is used to filter and estimate it to obtain the track estimation value of the vehicle real-time positioning. Improve the accuracy of track positioning;
  • the linear Kalman filter algorithm is used to filter to reduce the noise to the estimation. The impact of the result, thereby improving the accuracy of automatic parking positioning;
  • the calculation of the zero drift error value of the automatic parking system of the vehicle is added to eliminate the inherent error of the vehicle and improve the accuracy of the vehicle heading angle, so as to obtain the accurate real-time heading positioning value.
  • Figure 1 is a flow chart of the method of the present invention.
  • Figure 2 is a structural block diagram of the present invention.
  • Figure 3 is a schematic diagram of the coordinates of the present invention.
  • a method for estimating the track of an automatic parking positioning system includes the following steps:
  • the positioning value of the vehicle heading at the first time of parking includes the abscissa, the ordinate, and the heading angle of the vehicle at the first time of parking.
  • the vehicle heading positioning value at the first moment of parking is calculated through the wheel pulse signal combined with the vehicle's own parameters, and the wheel pulse signal combined with the vehicle's own parameters is used to calculate the vehicle's motion posture, which belongs to the prior art. I will not repeat them here.
  • the system matrix, the measurement matrix and the observation value of the vehicle heading at the second time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and the vehicle motion information.
  • This step specifically includes:
  • the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and the vehicle motion information.
  • the calculation of the system state vector and measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
  • the vehicle heading positioning value at the first moment of parking is respectively combined with the vehicle's yaw rate and the speed of the rear axle center to calculate the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking.
  • the calculation of the system state vector and measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
  • the vehicle heading positioning value at the first moment of parking is combined with the vehicle's yaw rate and the speed of the center of the rear axle to calculate the system state vector of the vehicle heading positioning value at the first time of parking; the vehicle heading positioning value at the first time of parking is combined with the vehicle two
  • the measurement vector of the vehicle heading at the first moment of parking is calculated by calculating the wheel speed of the rear wheels, the turning radius of the rear axle center point and the steering wheel angle.
  • the vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate and the vehicle heading angle. Therefore, in this step, the system matrix of the vehicle's abscissa, ordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are all calculated by the following formula:
  • the vehicle's abscissa, ordinate or vehicle heading angle system state vector X 1 in the vehicle heading positioning value is [x, y, ⁇ , w, v]T
  • the vehicle's abscissa in the vehicle heading positioning value at the first moment is used
  • the system state vector X 1(t) of the ordinate or the vehicle heading angle is calculated by the following formula to calculate the system state vector X 1(t+1) of the vehicle heading positioning value at the second moment of parking;
  • x, y, ⁇ are the vehicle heading positioning values at the first moment of parking
  • w is the yaw rate of the vehicle
  • v is the speed of the center of the rear axle of the vehicle
  • t is the first moment of parking
  • t+1 is the first moment of parking.
  • w is the yaw rate of the vehicle
  • v is the speed of the center of the rear axle of the vehicle
  • t is the first moment of parking
  • t+1 is the second moment of parking.
  • the vehicle motion information includes the speed of the two rear wheels of the vehicle, the turning radius of the center point of the rear axle, and the steering wheel angle, as shown in Figure 2-3:
  • the observation quantity Z 1(t) of the vehicle's abscissa, ordinate, or vehicle's heading angle in the vehicle heading positioning value at the first moment of parking is calculated by the following formula:
  • the observation quantity Z 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
  • V 3 and V 4 are the wheel speeds of the two rear wheels of the vehicle, Is the turning radius at the center of the rear axle, Is the steering wheel angle, t is the first moment of parking, and t+1 is the second moment of parking.
  • This step specifically includes:
  • the linear Kalman filter algorithm is used to filter and calculate the system matrix, measurement matrix and observations of the vehicle's horizontal coordinate, vertical coordinate, and vehicle heading angle of the vehicle heading position value at the second moment of parking through the following formulas to obtain the second parking time
  • t is the Jacobian matrix after f takes the first derivative of X, that is, the system matrix at time t+1, and W t is the system noise.
  • H t+1 is the Jacobian matrix obtained by calculating the first-order partial derivative of h with respect to X, that is, the measurement matrix at time t+1, and Z t+1 is the observation quantity at time t+1.
  • the system state vector value at time t+1 is obtained, that is, the vehicle horizontal coordinate X t+1 , the vertical coordinate Y t+1 and the vehicle heading angle ⁇ are obtained respectively for the vehicle heading positioning value at the second time of parking. t+1 .
  • This step is specifically:
  • this step is specifically as follows: after the pneumatic parking system, the vehicle body yaw rate sensor value within 1s is collected repeatedly at a frequency of 20ms while the vehicle is stationary A total of 50 values are sampled, and the average filter calculation is performed on the collected 50 values to obtain the zero drift error value of the vehicle at the second moment.
  • YawRate is the body yaw rate sensor value, which is accumulated 50 times
  • Yaw is the vehicle yaw rate sensor value after the body average filtering, that is, the zero drift error value.
  • the vehicle heading angle of the vehicle heading positioning value at the second time of parking is subtracted from the zero drift error value of the vehicle at the second time of parking to obtain the vehicle heading positioning value at the second time of parking.
  • the position information (X, Y, ⁇ ) of the determined vehicle at the second moment of parking is obtained.
  • the calculation of the zero drift error value of the automatic parking system of the vehicle is added to eliminate the inherent error of the vehicle, so as to improve the accuracy of the vehicle heading angle, thereby obtaining an accurate real-time heading positioning value.
  • An automatic parking positioning system includes a track estimation module, and the track estimation module adopts the track estimation method described in Embodiment 1 to calculate the track of the vehicle.

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

A dead reckoning method of an automatic parking positioning system, comprising steps of: calculating a heading positioning value of a vehicle at a first parking moment; calculating a system matrix, a measurement matrix, and an observation quantity of a heading positioning value of the vehicle at a second parking moment by combining the heading positioning value of the vehicle at the first parking moment and movement information of the vehicle; respectively subjecting the system matrix, the measurement matrix and the observation quantity to filtering estimation by a linear Kalman filtering algorithm to obtain the heading positioning value of the vehicle at the second parking moment; and subtracting a calculated zero drift error value of the vehicle at the second parking moment from a vehicle heading angle of the heading positioning value of the vehicle at the second parking moment to obtain the heading positioning value of the vehicle at the second parking moment. By taking the dead reckoning results as the measurement quantity and combining with parameters such as a yaw velocity and the velocity of a rear axle center of the vehicle, carrying out the filtering estimation by adopting the linear Kalman filter algorithm to obtain a dead reckoning value of real-time positioning of the vehicle, and then eliminating a zero drift error, an accurate and real-time heading positioning value is obtained.

Description

一种自动泊车定位***的航迹推算方法Method for calculating track of automatic parking positioning system 技术领域Technical field
本发明涉及泊车技术领域，特别是涉及一种自动泊车定位***的航迹推算方法。The present invention relates to the technical field of parking, in particular to a track estimation method of an automatic parking positioning system.
背景技术Background technique
随着汽车行业的蓬勃发展，自动泊车***目前已经应用到各大汽车厂商的高端配置车辆上，自动泊车过程分四步进行，首先，对车位进行检测，车位检测过程中需要定位跟踪车辆位置，航迹推算***触发，当得到可以停靠的车位后，车辆继续往前走，车位需要根据之前的位置信息跟当前车辆的位置信息跟踪车位坐标；第二步，将车位坐标信息存储到控制器ECU中，控制器ECU获取车辆信息并建立车辆坐标系；第三步，控制器ECU根据车辆的位置信息、坐标信息、雷达信息等条件规划目标泊车轨迹；最后，泊车控制***通过实时的航迹跟踪推算，根据目标挡位、目标转角、目标速度、目标加速度以及目标位置输出控制车身自动泊车，ECU控制单元完成整个自动泊车过程。With the vigorous development of the automobile industry, the automatic parking system has been applied to the high-end configuration vehicles of major automobile manufacturers. The automatic parking process is carried out in four steps. First, the parking space is detected. During the parking space detection process, the vehicle needs to be located and tracked. The position and track estimation system is triggered. When a parking space is obtained, the vehicle continues to move forward. The parking space needs to track the parking space coordinates according to the previous position information and the current vehicle position information; the second step is to store the parking space coordinate information to the control In the ECU, the controller ECU obtains vehicle information and establishes the vehicle coordinate system; the third step, the controller ECU plans the target parking trajectory according to the vehicle’s position information, coordinate information, radar information and other conditions; finally, the parking control system passes real-time According to the target gear, target turning angle, target speed, target acceleration and target position output, the vehicle body is automatically parked and the ECU control unit completes the entire automatic parking process.
在自动泊车过程中，泊车航迹推算主要通过轮速脉冲信号，轮速，以及车身参数来匹配运算。采用车轮脉冲信号进行航迹推算来定位，其通过单位时间内左右轮运动行程来计算车身方位角增量和后轴中心位置的增量，分别对车身方位角和位置的增量进行累加，可得出车辆相对于起始位置的姿态。In the process of automatic parking, the parking track calculation is mainly based on the matching calculation of wheel speed pulse signals, wheel speed, and vehicle body parameters. The wheel pulse signal is used to calculate the track for positioning. It calculates the body azimuth angle increment and the rear axle center position increment through the movement of the left and right wheels per unit time, and accumulates the body azimuth angle and position increment respectively. Get the attitude of the vehicle relative to the starting position.
航迹推算算法适用于短行程车辆位姿定位，但该方法存在一些误差来源。例如：航迹推算定位误差和非***误差，航迹推算定位误差包括，车轮有效滚动半径的测量误差、两侧车轮的滚动半径不等、轮距测量值与真实值存在误差及码盘的分辨率不高等；非***误差包括，路面不平、车轮打滑、溜车现象、轮胎载荷变化等，从而导致车辆位姿定位存在一定误差，进而航迹推算精度不高。The track estimation algorithm is suitable for short-stroke vehicle pose positioning, but this method has some sources of error. For example: track calculation positioning error and non-systematic error, track calculation positioning error includes the measurement error of the effective rolling radius of the wheel, the unequal rolling radius of the wheels on both sides, the error between the measured value of the track and the real value, and the resolution of the code disc The rate is not high; non-systematic errors include uneven road surface, wheel slip, car rolling phenomenon, tire load change, etc., which lead to a certain error in vehicle pose positioning, and the accuracy of track estimation is not high.
发明内容Summary of the invention
本发明为克服上述现有技术所述的不足，提供一种自动泊车定位***的航迹推算方法。In order to overcome the above-mentioned shortcomings of the prior art, the present invention provides a method for calculating the track of an automatic parking positioning system.
为解决上述技术问题，本发明的技术方案如下：To solve the above technical problems, the technical scheme of the present invention is as follows:
一种自动泊车定位***的航迹推算方法，包括如下步骤：A method for estimating track of an automatic parking and positioning system includes the following steps:
计算泊车第一时刻车辆航向定位值；Calculate the vehicle heading positioning value at the first moment of parking;
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量；The system matrix, measurement matrix and observation quantity of the vehicle heading positioning value at the second time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；The linear Kalman filter algorithm is used to filter and estimate the system matrix, the measurement matrix and the observation value of the vehicle heading at the second moment of parking to obtain the vehicle heading positioning value at the second time of parking;
通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；Calculate the zero drift error value of the vehicle at the second moment of parking based on the body yaw rate sensor value;
将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。The vehicle heading angle of the vehicle heading positioning value at the second time of parking is subtracted from the zero drift error value of the vehicle at the second time of parking to obtain the vehicle heading positioning value at the second time of parking.
进一步的，作为优选技术方案，第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量的计算具体包括：Further, as a preferred technical solution, the calculation of the system matrix, the measurement matrix, and the observation amount of the vehicle heading positioning value at the second moment specifically includes:
通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量；The system state vector and measurement vector of the vehicle heading positioning value at the first time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
通过泊车第一时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***状态向量，通过第二时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***矩阵；The system state vector of the vehicle heading positioning value at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the first moment of parking, and the vehicle at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the second time System matrix of heading positioning value;
通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。Calculate the observation value of the vehicle heading positioning value at the second moment of parking by calculating the measurement vector of the vehicle heading positioning value at the first moment of parking, and calculate the vehicle heading positioning value of the vehicle at the second moment of parking by calculating the observation volume of the vehicle heading positioning value at the second time Measurement matrix.
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；Further, as a preferred technical solution, the vehicle motion information includes the yaw rate of the vehicle and the speed of the center of the rear axle of the vehicle;
所述泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：The calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量。The vehicle heading positioning value at the first moment of parking is respectively combined with the vehicle's yaw rate and the speed of the rear axle center to calculate the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking.
进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；Further, as a preferred technical solution, the vehicle motion information includes the yaw rate of the vehicle, the speed of the center of the rear axle of the vehicle, the speed of the two rear wheels of the vehicle, the turning radius of the center of the rear axle, and the steering wheel angle;
所述泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：The calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量；The vehicle heading positioning value at the first moment of parking is calculated by combining the vehicle's yaw rate and the speed of the vehicle's rear axle center to obtain the system state vector of the vehicle heading positioning value at the first time of parking;
泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。The vehicle heading positioning value at the first moment of parking is calculated by combining the two rear wheel speeds of the vehicle, the turning radius of the rear axle center point and the steering wheel angle to obtain the measurement vector of the vehicle heading positioning value at the first time of parking.
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的***矩阵通过以下公式计算：Further, as a preferred technical solution, the system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；The vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle;
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)； Assuming that the system state vector X 1 of the vehicle's horizontal coordinate, vertical coordinate or vehicle's heading angle in the vehicle heading positioning value is [x, y, θ, w, v]T, the vehicle heading positioning value at the second moment of parking is calculated by the following formula The system state vector X 1(t+1) of the vehicle's abscissa, ordinate or vehicle heading angle in;
Figure PCTCN2019098634-appb-000001
Figure PCTCN2019098634-appb-000001
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)； Increment and linearize the system state vector X 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking to obtain the vehicle heading positioning value at the second moment of parking The system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate, or vehicle heading angle in;
Figure PCTCN2019098634-appb-000002
Figure PCTCN2019098634-appb-000002
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；Among them, x, y, θ are the vehicle heading positioning values at the first moment of parking, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the first moment of parking. Two moments
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：The observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)： Assuming that the vehicle's abscissa, ordinate or vehicle heading angle measurement vector in the vehicle heading positioning value is [w, v]T, the vehicle's abscissa and ordinate in the vehicle heading positioning value at the second moment of parking are calculated by the following formula Or the observation quantity Z 1(t+1) of the vehicle heading angle:
Figure PCTCN2019098634-appb-000003
Figure PCTCN2019098634-appb-000003
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H 1(t+1)： Calculate the vehicle in the vehicle heading positioning value at the second moment of parking based on the vehicle's horizontal coordinate, ordinate or vehicle heading angle observation Z 1(t+1) in the vehicle heading positioning value at the second moment of parking The measurement matrix of abscissa, ordinate or vehicle heading angle is H 1(t+1) :
Figure PCTCN2019098634-appb-000004
Figure PCTCN2019098634-appb-000004
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。Among them, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the second moment of parking.
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的***矩阵通过以下公式计算：Further, as a preferred technical solution, the system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；The vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle;
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)； Assuming that the system state vector X 1 of the vehicle's horizontal coordinate, vertical coordinate or vehicle's heading angle in the vehicle heading positioning value is [x, y, θ, w, v]T, the vehicle heading positioning value at the second moment of parking is calculated by the following formula The system state vector X 1(t+1) of the vehicle's abscissa, ordinate or vehicle heading angle in;
Figure PCTCN2019098634-appb-000005
Figure PCTCN2019098634-appb-000005
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)； Increment and linearize the system state vector X 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking to obtain the vehicle heading positioning value at the second moment of parking The system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate, or vehicle heading angle in;
Figure PCTCN2019098634-appb-000006
Figure PCTCN2019098634-appb-000006
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；Among them, x, y, θ are the vehicle heading positioning values at the first moment of parking, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the first moment of parking. Two moments
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：The observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z 1为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)： Assuming that the vehicle's abscissa, ordinate, or vehicle heading angle measurement vector Z 1 in the vehicle heading positioning value is [w, v]T, the vehicle's abscissa, Observation Z 1(t+1) of ordinate or vehicle heading angle:
Figure PCTCN2019098634-appb-000007
Figure PCTCN2019098634-appb-000007
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H 1(t+1)： Calculate the vehicle in the vehicle heading positioning value at the second moment of parking based on the vehicle's horizontal coordinate, ordinate or vehicle heading angle observation Z 1(t+1) in the vehicle heading positioning value at the second moment of parking The measurement matrix of abscissa, ordinate or vehicle heading angle is H 1(t+1) :
Figure PCTCN2019098634-appb-000008
Figure PCTCN2019098634-appb-000008
其中，V 3和V 4为车辆两个后轮的轮速，
Figure PCTCN2019098634-appb-000009
为后轴中心点转弯半径，
Figure PCTCN2019098634-appb-000010
为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。 Among them, V 3 and V 4 are the wheel speeds of the two rear wheels of the vehicle,
Figure PCTCN2019098634-appb-000009
Is the turning radius at the center of the rear axle,
Figure PCTCN2019098634-appb-000010
Is the steering wheel angle, t is the first moment of parking, and t+1 is the second moment of parking.
进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的计算具体包括：Further, as a preferred technical solution, the calculation of the vehicle heading positioning value at the second moment of parking specifically includes:
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的***矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；The linear Kalman filter algorithm is used to filter and calculate the system matrix, measurement matrix and observations of the vehicle's horizontal coordinate, vertical coordinate, and vehicle heading angle of the vehicle heading position value at the second moment of parking through the following formulas to obtain the second parking time The vehicle's abscissa, ordinate, and vehicle heading angle of the vehicle heading positioning value at the time;
线性卡尔曼滤波算法计算方程为：The linear Kalman filter algorithm calculation equation is:
Figure PCTCN2019098634-appb-000011
Figure PCTCN2019098634-appb-000011
Figure PCTCN2019098634-appb-000012
Figure PCTCN2019098634-appb-000012
其中，
Figure PCTCN2019098634-appb-000013
为t+1时刻的***状态向量值估计值，P t+1，t为t+1时刻协方差矩阵的估计值，Q t+1为t+1时刻的***噪声协方差矩阵，φ t+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的***矩阵，W t为***噪声。 among them,
Figure PCTCN2019098634-appb-000013
Is the estimated value of the system state vector value at t+1 , P t+1, t is the estimated value of the covariance matrix at t+1 , Q t+1 is the system noise covariance matrix at t+1, φ t+ 1. t is the Jacobian matrix after f takes the first derivative of X, that is, the system matrix at time t+1, and W t is the system noise.
进一步的，作为优选技术方案，线性卡尔曼滤波算法计算方程为：Further, as a preferred technical solution, the linear Kalman filter algorithm calculation equation is:
Figure PCTCN2019098634-appb-000014
Figure PCTCN2019098634-appb-000014
Figure PCTCN2019098634-appb-000015
Figure PCTCN2019098634-appb-000015
P t+1＝[1-K t+1H t+1]P t+1，t； P t+1 =[1-K t+1 H t+1 ]P t+1, t ;
其中，H t+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Z t+1为t+1时刻的观察量。 Among them, H t+1 is the Jacobian matrix obtained by calculating the first-order partial derivative of h with respect to X, that is, the measurement matrix at time t+1, and Z t+1 is the observation quantity at time t+1.
进一步的，作为优选技术方案，泊车第二时刻车辆零飘误差值的计算具体包括：Further, as a preferred technical solution, the calculation of the zero drift error value of the vehicle at the second moment of parking specifically includes:
以固定频率采集固定时间内的车身偏航率传感器值；Collect the body yaw rate sensor value within a fixed time at a fixed frequency;
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。Perform average filtering calculation on the collected multiple body yaw rate sensor values to obtain the vehicle zero drift error value at the second moment.
一种自动泊车定位***，包括航迹推算模块，所述航迹推算模块采用航迹推算方法进行车辆的航迹推算。An automatic parking positioning system includes a track estimation module, and the track estimation module adopts a track estimation method to calculate the track of a vehicle.
与现有技术相比，本发明技术方案的有益效果是：Compared with the prior art, the beneficial effects of the technical solution of the present invention are:
本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，大幅度提高了航迹定位精准度；In the present invention, the result of the track estimation is used as the measurement quantity, combined with the yaw rate of the vehicle and the speed of the vehicle rear axle center and other parameters, and the linear Kalman filter algorithm is used to filter and estimate it to obtain the track estimation value of the vehicle real-time positioning. Improve the accuracy of track positioning;
同时，由于本发明中所采用各车辆运动信息基于传感器采集，而传感器的信号中包含噪声，对算法计算结果带来很大影响，因此通过线性卡尔曼滤波算法进行滤波，以消弱噪声对估算结果的影响，进而提高自动泊车定位的精确度；At the same time, because the vehicle motion information used in the present invention is based on sensor collection, and the sensor signal contains noise, which has a great impact on the calculation result of the algorithm, the linear Kalman filter algorithm is used to filter to reduce the noise to the estimation. The impact of the result, thereby improving the accuracy of automatic parking positioning;
最后，增加对车辆自动泊车***零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。Finally, the calculation of the zero drift error value of the automatic parking system of the vehicle is added to eliminate the inherent error of the vehicle and improve the accuracy of the vehicle heading angle, so as to obtain the accurate real-time heading positioning value.
附图说明Description of the drawings
图1为本发明方法步骤流程图。Figure 1 is a flow chart of the method of the present invention.
图2为本发明结构框图。Figure 2 is a structural block diagram of the present invention.
图3为本发明坐标示意图。Figure 3 is a schematic diagram of the coordinates of the present invention.
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。The attached drawings are only for illustrative purposes and cannot be understood as a limitation of this patent; in order to better illustrate this embodiment, some parts of the attached drawings may be omitted, enlarged or reduced, and do not represent the size of the actual product; For the personnel, it is understandable that some well-known structures and their descriptions in the drawings may be omitted; the same or similar reference numerals correspond to the same or similar parts; the terms describing the positional relationship in the drawings are only for exemplary description and cannot be understood In order to limit this patent.
具体实施方式detailed description
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, and the protection scope of the present invention will be more clearly defined.
实施例1Example 1
一种自动泊车定位***的航迹推算方法，如图1所示，包括如下步骤：A method for estimating the track of an automatic parking positioning system, as shown in Figure 1, includes the following steps:
S10.计算泊车第一时刻车辆航向定位值。S10. Calculate the vehicle heading positioning value at the first moment of parking.
在本步骤中，泊车第一时刻车辆航向定位值包括泊车第一时刻车辆横坐标、纵坐标以及车辆航向角。In this step, the positioning value of the vehicle heading at the first time of parking includes the abscissa, the ordinate, and the heading angle of the vehicle at the first time of parking.
在本发明中，泊车第一时刻车辆航向定位值通过车轮脉冲信号结合车辆自身参数推到计算，而通过车轮脉冲信号结合车辆自身参数对车辆的运动姿态进行推到计算属于现有技术，在此不在进行赘述。In the present invention, the vehicle heading positioning value at the first moment of parking is calculated through the wheel pulse signal combined with the vehicle's own parameters, and the wheel pulse signal combined with the vehicle's own parameters is used to calculate the vehicle's motion posture, which belongs to the prior art. I will not repeat them here.
S20.通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量。S20. The system matrix, the measurement matrix and the observation value of the vehicle heading at the second time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and the vehicle motion information.
本步骤具体包括：This step specifically includes:
S201.通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量。S201. The system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and the vehicle motion information.
在本步骤中，当车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度时，泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：In this step, when the vehicle motion information includes the yaw rate of the vehicle and the speed of the center of the rear axle of the vehicle, the calculation of the system state vector and measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量。The vehicle heading positioning value at the first moment of parking is respectively combined with the vehicle's yaw rate and the speed of the rear axle center to calculate the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking.
当车辆运动信息还包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：When the vehicle motion information also includes the vehicle's two rear wheel speeds, the turning radius of the center point of the rear axle, and the steering wheel angle, the calculation of the system state vector and measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量；泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。The vehicle heading positioning value at the first moment of parking is combined with the vehicle's yaw rate and the speed of the center of the rear axle to calculate the system state vector of the vehicle heading positioning value at the first time of parking; the vehicle heading positioning value at the first time of parking is combined with the vehicle two The measurement vector of the vehicle heading at the first moment of parking is calculated by calculating the wheel speed of the rear wheels, the turning radius of the rear axle center point and the steering wheel angle.
S202.通过泊车第一时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***状态向量，通过第二时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***矩阵；S202. Calculate the system state vector of the vehicle heading positioning value at the second time of parking by calculating the system state vector of the vehicle heading positioning value at the first moment of parking, and obtain the second parking system by calculating the system state vector of the vehicle heading positioning value at the second time The system matrix of the vehicle heading positioning value at all times;
S203.通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。S203. Obtain the observed value of the vehicle heading positioning value at the second moment of parking by calculating the measurement vector of the vehicle heading positioning value at the first moment of parking, and calculate the vehicle heading at the second moment of parking by calculating the observed volume of the vehicle heading positioning value at the second moment of parking Measurement matrix of positioning values.
由于车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角。因此，在本步骤中，泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的***矩阵均通过以下公式计算：Because the vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate and the vehicle heading angle. Therefore, in this step, the system matrix of the vehicle's abscissa, ordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are all calculated by the following formula:
假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1为[x，y，θ，w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t)通过以下公式计算泊车第二时刻车辆航向定位值的***状态向量X 1(t+1)； Assuming that the vehicle's abscissa, ordinate or vehicle heading angle system state vector X 1 in the vehicle heading positioning value is [x, y, θ, w, v]T, the vehicle's abscissa in the vehicle heading positioning value at the first moment is used , The system state vector X 1(t) of the ordinate or the vehicle heading angle is calculated by the following formula to calculate the system state vector X 1(t+1) of the vehicle heading positioning value at the second moment of parking;
Figure PCTCN2019098634-appb-000016
Figure PCTCN2019098634-appb-000016
对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)； Increment and linearize the system state vector X 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking to obtain the vehicle heading positioning value at the second moment of parking The system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate, or vehicle heading angle in;
即，对
Figure PCTCN2019098634-appb-000017
时刻***状态进行增量计算，如下公式： That is, yes
Figure PCTCN2019098634-appb-000017
The system status is calculated incrementally at the moment, as follows:
Figure PCTCN2019098634-appb-000018
Figure PCTCN2019098634-appb-000018
由车辆运动学模型公式可知，上式微分方程写成差分形式：According to the formula of the vehicle kinematics model, the above differential equation is written in a difference form:
Figure PCTCN2019098634-appb-000019
Figure PCTCN2019098634-appb-000019
对上式线性化，得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)： Linearize the above formula to obtain the system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking:
Figure PCTCN2019098634-appb-000020
Figure PCTCN2019098634-appb-000020
其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；Among them, x, y, θ are the vehicle heading positioning values at the first moment of parking, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the first moment of parking. Two moments
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵 通过以下公式计算：The observation and measurement matrix of the vehicle's abscissa, ordinate, and vehicle heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
假设车辆航向定位值的测量向量Z 1为[w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t)通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)： Assuming that the measurement vector Z 1 of the vehicle heading positioning value is [w, v]T, using the vehicle's abscissa, ordinate or vehicle heading angle observation Z 1(t) in the vehicle heading positioning value at the first moment is calculated by the following formula Observation Z 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking:
Figure PCTCN2019098634-appb-000021
Figure PCTCN2019098634-appb-000021
以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H 1(t+1)： Calculate the vehicle in the vehicle heading positioning value at the second moment of parking based on the vehicle's horizontal coordinate, ordinate or vehicle heading angle observation Z 1(t+1) in the vehicle heading positioning value at the second moment of parking The measurement matrix of abscissa, ordinate or vehicle heading angle is H 1(t+1) :
Figure PCTCN2019098634-appb-000022
Figure PCTCN2019098634-appb-000022
其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。Among them, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the second moment of parking.
在上述计算过程中，计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)和测量矩阵车辆的横摆角速度和车辆后轴中心的速度。 In the above calculation process, calculate the vehicle's abscissa, ordinate or vehicle heading angle observation Z 1(t+1) and the measurement matrix of the vehicle's yaw rate and the rear axle of the vehicle in the vehicle heading positioning value at the second moment of parking The speed of the center.
而当车辆运动信息包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，如图2-3所示：When the vehicle motion information includes the speed of the two rear wheels of the vehicle, the turning radius of the center point of the rear axle, and the steering wheel angle, as shown in Figure 2-3:
Figure PCTCN2019098634-appb-000023
Figure PCTCN2019098634-appb-000023
Figure PCTCN2019098634-appb-000024
Figure PCTCN2019098634-appb-000024
因此，泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t)通过以下公式计算： Therefore, the observation quantity Z 1(t) of the vehicle's abscissa, ordinate, or vehicle's heading angle in the vehicle heading positioning value at the first moment of parking is calculated by the following formula:
Figure PCTCN2019098634-appb-000025
Figure PCTCN2019098634-appb-000025
泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)通过以下公式计算： The observation quantity Z 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
Figure PCTCN2019098634-appb-000026
Figure PCTCN2019098634-appb-000026
其中，V 3和V 4为车辆两个后轮的轮速，
Figure PCTCN2019098634-appb-000027
为后轴中心点转弯半径，
Figure PCTCN2019098634-appb-000028
为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。 Among them, V 3 and V 4 are the wheel speeds of the two rear wheels of the vehicle,
Figure PCTCN2019098634-appb-000027
Is the turning radius at the center of the rear axle,
Figure PCTCN2019098634-appb-000028
Is the steering wheel angle, t is the first moment of parking, and t+1 is the second moment of parking.
S30.通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值。S30. Perform filtering estimation on the system matrix, the measurement matrix and the observation value of the vehicle heading at the second moment of parking by using a linear Kalman filter algorithm to obtain the vehicle heading positioning value at the second time of parking.
本步骤具体包括：This step specifically includes:
分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的***矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；The linear Kalman filter algorithm is used to filter and calculate the system matrix, measurement matrix and observations of the vehicle's horizontal coordinate, vertical coordinate, and vehicle heading angle of the vehicle heading position value at the second moment of parking through the following formulas to obtain the second parking time The vehicle's abscissa, ordinate, and vehicle heading angle of the vehicle heading positioning value at the time;
线性卡尔曼滤波算法计算方程为：The linear Kalman filter algorithm calculation equation is:
Figure PCTCN2019098634-appb-000029
Figure PCTCN2019098634-appb-000029
Figure PCTCN2019098634-appb-000030
Figure PCTCN2019098634-appb-000030
其中，
Figure PCTCN2019098634-appb-000031
为t+1时刻的***状态向量值估计值，P t+1，t为t+1时刻协方差矩阵的估计值， Q t+1为t+1时刻的***噪声协方差矩阵，φ t+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的***矩阵，W t为***噪声。 among them,
Figure PCTCN2019098634-appb-000031
Is the estimated value of the system state vector value at t+1 , P t+1, t is the estimated value of the covariance matrix at t+1 , Q t+1 is the system noise covariance matrix at t+1, φ t+ 1. t is the Jacobian matrix after f takes the first derivative of X, that is, the system matrix at time t+1, and W t is the system noise.
进一步的，线性卡尔曼滤波算法计算方程为：Further, the linear Kalman filter algorithm calculation equation is:
Figure PCTCN2019098634-appb-000032
Figure PCTCN2019098634-appb-000032
Figure PCTCN2019098634-appb-000033
Figure PCTCN2019098634-appb-000033
P t+1＝[1-K t+1H t+1]P t+1，t； P t+1 =[1-K t+1 H t+1 ]P t+1, t ;
其中，H t+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Z t+1为t+1时刻的观察量。 Among them, H t+1 is the Jacobian matrix obtained by calculating the first-order partial derivative of h with respect to X, that is, the measurement matrix at time t+1, and Z t+1 is the observation quantity at time t+1.
通过本步骤的滤波计算，得到t+1时刻的***状态向量值，即分别得到泊车第二时刻车辆航向定位值的车辆横坐标X t+1、纵坐标Y t+1以及车辆航向角θ t+1。 Through the filtering calculation in this step, the system state vector value at time t+1 is obtained, that is, the vehicle horizontal coordinate X t+1 , the vertical coordinate Y t+1 and the vehicle heading angle θ are obtained respectively for the vehicle heading positioning value at the second time of parking. t+1 .
S40.通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值。S40. Calculate the zero drift error value of the vehicle at the second moment of parking based on the vehicle body yaw rate sensor value.
本步骤具体为：This step is specifically:
以固定频率采集固定时间内的车身偏航率传感器值；Collect the body yaw rate sensor value within a fixed time at a fixed frequency;
对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。Perform average filtering calculation on the collected multiple body yaw rate sensor values to obtain the vehicle zero drift error value at the second moment.
在本步骤中，采集频率为20ms每次，固定时间为1s，因此，本步骤具体为：气动泊车***后，在车辆静止过程中重复以20ms的频率采集1s以内的车身偏航率传感器值，共采样50个值，对采集的50个值进行均值滤波计算，以得到第二时刻车辆零飘误差值。In this step, the collection frequency is 20ms each time, and the fixed time is 1s. Therefore, this step is specifically as follows: after the pneumatic parking system, the vehicle body yaw rate sensor value within 1s is collected repeatedly at a frequency of 20ms while the vehicle is stationary A total of 50 values are sampled, and the average filter calculation is performed on the collected 50 values to obtain the zero drift error value of the vehicle at the second moment.
计算公式如下：Calculated as follows:
YawRate+＝YawRate；YawRate+=YawRate;
Yaw＝YawRate/50.0f；Yaw=YawRate/50.0f;
其中，YawRate为车身偏航率传感器值，累计叠加50次，Yaw为车身均值滤波后整车偏航率传感器值，即零飘误差值。Among them, YawRate is the body yaw rate sensor value, which is accumulated 50 times, and Yaw is the vehicle yaw rate sensor value after the body average filtering, that is, the zero drift error value.
S50.将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。S50. The vehicle heading angle of the vehicle heading positioning value at the second time of parking is subtracted from the zero drift error value of the vehicle at the second time of parking to obtain the vehicle heading positioning value at the second time of parking.
即，得到泊车第二时刻确定车辆位置信息(X，Y，θ)。That is, the position information (X, Y, θ) of the determined vehicle at the second moment of parking is obtained.
本步骤增加对车辆自动泊车***零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。In this step, the calculation of the zero drift error value of the automatic parking system of the vehicle is added to eliminate the inherent error of the vehicle, so as to improve the accuracy of the vehicle heading angle, thereby obtaining an accurate real-time heading positioning value.
实施例2Example 2
一种自动泊车定位***，包括航迹推算模块，所述航迹推算模块采用实施例1所述的航迹推算方法进行车辆的航迹推算。An automatic parking positioning system includes a track estimation module, and the track estimation module adopts the track estimation method described in Embodiment 1 to calculate the track of the vehicle.
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。Obviously, the above-mentioned embodiments of the present invention are merely examples to clearly illustrate the present invention, and are not intended to limit the embodiments of the present invention. For those of ordinary skill in the art, other changes or changes in different forms can be made on the basis of the above description. It is unnecessary and impossible to list all the implementation methods here. Any modification, equivalent replacement and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the claims of the present invention.

Claims (10)

  1. 一种自动泊车定位***的航迹推算方法，其特征在于，包括如下步骤：A method for estimating track of an automatic parking positioning system is characterized in that it comprises the following steps:
    计算泊车第一时刻车辆航向定位值；Calculate the vehicle heading positioning value at the first moment of parking;
    通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量；The system matrix, measurement matrix and observation quantity of the vehicle heading positioning value at the second time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
    通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；The linear Kalman filter algorithm is used to filter and estimate the system matrix, the measurement matrix and the observation value of the vehicle heading at the second moment of parking to obtain the vehicle heading positioning value at the second time of parking;
    通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；Calculate the zero drift error value of the vehicle at the second moment of parking based on the body yaw rate sensor value;
    将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。The vehicle heading angle of the vehicle heading positioning value at the second time of parking is subtracted from the zero drift error value of the vehicle at the second time of parking to obtain the vehicle heading positioning value at the second time of parking.
  2. 根据权利要求1所述的自动泊车定位***的航迹推算方法，其特征在于，第二时刻车辆航向定位值的***矩阵、测量矩阵以及观察量的计算具体包括：The method for estimating the track of the automatic parking positioning system according to claim 1, wherein the calculation of the system matrix, the measurement matrix and the observation amount of the vehicle heading positioning value at the second moment specifically includes:
    通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量；The system state vector and measurement vector of the vehicle heading positioning value at the first time of parking are calculated by combining the vehicle heading positioning value at the first time of parking and vehicle motion information;
    通过泊车第一时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***状态向量，通过第二时刻车辆航向定位值的***状态向量计算得到泊车第二时刻车辆航向定位值的***矩阵；The system state vector of the vehicle heading positioning value at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the first moment of parking, and the vehicle at the second time of parking is calculated by the system state vector of the vehicle heading positioning value at the second time System matrix of heading positioning value;
    通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。Calculate the observation value of the vehicle heading positioning value at the second moment of parking by calculating the measurement vector of the vehicle heading positioning value at the first moment of parking, and calculate the vehicle heading positioning value of the vehicle at the second moment of parking by calculating the observation volume of the vehicle heading positioning value at the second time Measurement matrix.
  3. 根据权利要求2所述的自动泊车定位***的航迹推算方法，其特征在于，The track estimation method of an automatic parking positioning system according to claim 2, wherein:
    所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；The vehicle motion information includes the yaw rate of the vehicle and the speed of the center of the rear axle of the vehicle;
    所述泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：The calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
    泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量和测量向量。The vehicle heading positioning value at the first moment of parking is respectively combined with the vehicle's yaw rate and the speed of the rear axle center to calculate the system state vector and the measurement vector of the vehicle heading positioning value at the first time of parking.
  4. 根据权利要求2所述的自动泊车定位***的航迹推算方法，其特征在于，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；The track estimation method of the automatic parking positioning system according to claim 2, wherein the vehicle motion information includes the yaw rate of the vehicle, the speed of the center of the vehicle's rear axle, the speed of the two rear wheels of the vehicle, and The turning radius of the shaft center point and the steering wheel angle;
    所述泊车第一时刻车辆航向定位值的***状态向量和测量向量的计算具体包括：The calculation of the system state vector and the measurement vector of the vehicle heading positioning value at the first moment of parking specifically includes:
    泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的***状态向量；The vehicle heading positioning value at the first moment of parking is calculated by combining the vehicle's yaw rate and the speed of the vehicle's rear axle center to obtain the system state vector of the vehicle heading positioning value at the first time of parking;
    泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。The vehicle heading positioning value at the first moment of parking is calculated by combining the two rear wheel speeds of the vehicle, the turning radius of the rear axle center point and the steering wheel angle to obtain the measurement vector of the vehicle heading positioning value at the first time of parking.
  5. 根据权利要求3所述的自动泊车定位***的航迹推算方法，其特征在于，泊车第二时刻车辆航向定位值的***矩阵通过以下公式计算：The track estimation method of the automatic parking positioning system according to claim 3, wherein the system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
    所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；The vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle;
    假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量x 1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)； Assuming that the system state vector x 1 of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value is [x, y, θ, w, v]T, the vehicle heading positioning value at the second moment of parking is calculated by the following formula The system state vector X 1(t+1) of the vehicle's abscissa, ordinate or vehicle heading angle in;
    Figure PCTCN2019098634-appb-100001
    Figure PCTCN2019098634-appb-100001
    对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)； Increment and linearize the system state vector X 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking to obtain the vehicle heading positioning value at the second moment of parking The system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate, or vehicle heading angle in;
    Figure PCTCN2019098634-appb-100002
    Figure PCTCN2019098634-appb-100002
    其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；Among them, x, y, θ are the vehicle heading positioning values at the first moment of parking, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the first moment of parking. Two moments
    泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：The observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
    假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z 1为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)： Assuming that the vehicle's abscissa, ordinate, or vehicle heading angle measurement vector Z 1 in the vehicle heading positioning value is [w, v]T, the vehicle's abscissa, Observation Z 1(t+1) of ordinate or vehicle heading angle:
    Figure PCTCN2019098634-appb-100003
    Figure PCTCN2019098634-appb-100003
    以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H 1(t+1)： Calculate the vehicle in the vehicle heading positioning value at the second moment of parking based on the vehicle's horizontal coordinate, ordinate or vehicle heading angle observation Z 1(t+1) in the vehicle heading positioning value at the second moment of parking The measurement matrix of abscissa, ordinate or vehicle heading angle is H 1(t+1) :
    Figure PCTCN2019098634-appb-100004
    Figure PCTCN2019098634-appb-100004
    其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。Among them, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the second moment of parking.
  6. 根据权利要求4所述的自动泊车定位***的航迹推算方法，其特征在于，The track estimation method of the automatic parking positioning system according to claim 4, wherein:
    泊车第二时刻车辆航向定位值的***矩阵通过以下公式计算：The system matrix of the vehicle heading positioning value at the second moment of parking is calculated by the following formula:
    所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；The vehicle heading positioning value includes the vehicle horizontal coordinate, the vertical coordinate, and the vehicle heading angle;
    假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量x 1为[x，y，θ，w，v]T，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)； Assuming that the system state vector x 1 of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value is [x, y, θ, w, v]T, the vehicle heading positioning value at the second moment of parking is calculated by the following formula The system state vector X 1(t+1) of the vehicle's abscissa, ordinate or vehicle heading angle in;
    Figure PCTCN2019098634-appb-100005
    Figure PCTCN2019098634-appb-100005
    对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的***状态向量X 1(t+1)进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的***矩阵φ 1(t+1，t)； Increment and linearize the system state vector X 1(t+1) of the vehicle's horizontal coordinate, vertical coordinate, or vehicle's heading angle in the vehicle heading positioning value at the second moment of parking to obtain the vehicle heading positioning value at the second moment of parking The system matrix φ 1(t+1,t) of the vehicle's abscissa, ordinate, or vehicle heading angle in;
    Figure PCTCN2019098634-appb-100006
    Figure PCTCN2019098634-appb-100006
    其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；Among them, x, y, θ are the vehicle heading positioning values at the first moment of parking, w is the yaw rate of the vehicle, v is the speed of the center of the rear axle of the vehicle, t is the first moment of parking, and t+1 is the first moment of parking. Two moments
    泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：The observation and measurement matrix of the vehicle's horizontal coordinate, vertical coordinate, and vehicle's heading angle in the vehicle heading positioning value at the second moment of parking are calculated by the following formula:
    假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量Z 1为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)： Assuming that the vehicle's abscissa, ordinate, or vehicle heading angle measurement vector Z 1 in the vehicle heading positioning value is [w, v]T, the vehicle's abscissa, Observation Z 1(t+1) of ordinate or vehicle heading angle:
    Figure PCTCN2019098634-appb-100007
    Figure PCTCN2019098634-appb-100007
    以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量Z 1(t+1)为基 础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为H 1(t+1)： Calculate the vehicle in the vehicle heading positioning value at the second moment of parking based on the vehicle's horizontal coordinate, ordinate or vehicle heading angle observation Z 1(t+1) in the vehicle heading positioning value at the second moment of parking The measurement matrix of abscissa, ordinate or vehicle heading angle is H 1(t+1) :
    Figure PCTCN2019098634-appb-100008
    Figure PCTCN2019098634-appb-100008
    其中，V 3和V 4为车辆两个后轮的轮速，
    Figure PCTCN2019098634-appb-100009
    为后轴中心点转弯半径，
    Figure PCTCN2019098634-appb-100010
    为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。     Among them, V 3 and V 4 are the wheel speeds of the two rear wheels of the vehicle,
    Figure PCTCN2019098634-appb-100009
    Is the turning radius at the center of the rear axle,
    Figure PCTCN2019098634-appb-100010
    Is the steering wheel angle, t is the first moment of parking, and t+1 is the second moment of parking.
  7. 根据权利要求5或6任一项所述的自动泊车定位***的航迹推算方法，其特征在于，The track estimation method of an automatic parking positioning system according to any one of claims 5 or 6, characterized in that,
    泊车第二时刻车辆航向定位值的计算具体包括：The calculation of the vehicle heading positioning value at the second moment of parking specifically includes:
    分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的***矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；The linear Kalman filter algorithm is used to filter and calculate the system matrix, measurement matrix and observations of the vehicle's horizontal coordinate, vertical coordinate, and vehicle heading angle of the vehicle heading position value at the second moment of parking through the following formulas to obtain the second parking time The vehicle's abscissa, ordinate, and vehicle heading angle of the vehicle heading positioning value at the time;
    线性卡尔曼滤波算法计算方程为：The linear Kalman filter algorithm calculation equation is:
    Figure PCTCN2019098634-appb-100011
    Figure PCTCN2019098634-appb-100011
    Figure PCTCN2019098634-appb-100012
    Figure PCTCN2019098634-appb-100012
    其中，
    Figure PCTCN2019098634-appb-100013
    为t+1时刻的***状态向量值估计值，P t+1，t为t+1时刻协方差矩阵的估计值，Q t+1为t+1时刻的***噪声协方差矩阵，φ t+1，t是f对X求一阶导数后的雅克比矩阵，即t+1时刻的***矩阵，W t为***噪声。     among them,
    Figure PCTCN2019098634-appb-100013
    Is the estimated value of the system state vector value at t+1 , P t+1, t is the estimated value of the covariance matrix at t+1 , Q t+1 is the system noise covariance matrix at t+1, φ t+ 1. t is the Jacobian matrix after f takes the first derivative of X, that is, the system matrix at time t+1, and W t is the system noise.
  8. 根据权利要求7所述的自动泊车定位***的航迹推算方法，其特征在于，线性卡尔曼滤波算法计算方程为：The track estimation method of the automatic parking positioning system according to claim 7, wherein the linear Kalman filter algorithm calculation equation is:
    Figure PCTCN2019098634-appb-100014
    Figure PCTCN2019098634-appb-100014
    Figure PCTCN2019098634-appb-100015
    Figure PCTCN2019098634-appb-100015
    P t+1＝[1-K t+1H t+1]P t+1，t； P t+1 =[1-K t+1 H t+1 ]P t+1, t ;
    其中，H t+1是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，Z t+1为t+1时刻的观察量。 Among them, H t+1 is the Jacobian matrix obtained by calculating the first-order partial derivative of h with respect to X, that is, the measurement matrix at time t+1, and Z t+1 is the observation quantity at time t+1.
  9. 根据权利要求1所述的自动泊车定位***的航迹推算方法，其特征在于，泊车第二时刻车辆零飘误差值的计算具体包括：The track estimation method of an automatic parking positioning system according to claim 1, wherein the calculation of the zero drift error value of the vehicle at the second moment of parking specifically includes:
    以固定频率采集固定时间内的车身偏航率传感器值；Collect the body yaw rate sensor value within a fixed time at a fixed frequency;
    对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。Perform average filtering calculation on the collected multiple body yaw rate sensor values to obtain the vehicle zero drift error value at the second moment.
  10. 一种自动泊车定位***，其特征在于，包括航迹推算模块，所述航迹推算模块采用权利要求1～9中任一项所述的航迹推算方法进行车辆的航迹推算。An automatic parking positioning system, characterized by comprising a track estimation module that uses the track estimation method according to any one of claims 1 to 9 to calculate the track of the vehicle.
PCT/CN2019/098634 2019-03-28 2019-07-31 Dead reckoning method of automatic parking positioning system WO2020191977A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201910243714.8 2019-03-28
CN201910243714.8A CN110077392B (en) 2019-03-28 2019-03-28 Dead reckoning method of automatic parking positioning system

Publications (1)

Publication Number Publication Date
WO2020191977A1 true WO2020191977A1 (en) 2020-10-01

Family

ID=67413825

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/098634 WO2020191977A1 (en) 2019-03-28 2019-07-31 Dead reckoning method of automatic parking positioning system

Country Status (2)

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

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110940345B (en) * 2019-12-19 2022-03-18 深圳南方德尔汽车电子有限公司 Vehicle body positioning device, computer equipment and storage medium
CN113291314B (en) * 2020-02-21 2022-06-14 亿咖通(湖北)技术有限公司 Method and system for calculating vehicle course information
CN112550278B (en) * 2020-12-03 2022-05-17 上汽大众汽车有限公司 Automatic parking method and system based on look-around camera and ultrasonic radar detection
CN113887060B (en) * 2021-10-13 2024-05-10 英博超算(南京)科技有限公司 Novel automatic parking system vehicle positioning method
CN114475581B (en) * 2022-02-25 2022-09-16 北京流马锐驰科技有限公司 Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion
CN114407877B (en) * 2022-02-25 2023-03-24 北京流马锐驰科技有限公司 Automatic parking positioning method based on wheel speed pulse and IMU (inertial measurement Unit)
CN115900695A (en) * 2022-09-30 2023-04-04 中国第一汽车股份有限公司 Intelligent parking vehicle positioning method applied to vehicle

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020055819A1 (en) * 2000-11-08 2002-05-09 Yasuhiro Shimizu Vehicle navigation apparatus providing rapid correction for excessive error in dead reckoning estimates of vehicle travel direction by direct application of position and direction information derived from GPS position measurement data
US20110077898A1 (en) * 2008-06-11 2011-03-31 Peter Van Wyck Loomis Altimeter with calibration
CN103453913A (en) * 2013-08-29 2013-12-18 镇江青思网络科技有限公司 Integrated positioning method of vehicle in parking lot
CN104316059A (en) * 2014-11-05 2015-01-28 中国科学院嘉兴微电子与***工程中心 Dead-reckoning navigation positioning method and system for acquiring longitude and latitude of automobile by using speedometer
CN105946853A (en) * 2016-04-28 2016-09-21 中山大学 Long-distance automatic parking system and method based on multi-sensor fusion
CN107246868A (en) * 2017-07-26 2017-10-13 上海舵敏智能科技有限公司 A kind of collaborative navigation alignment system and navigation locating method
CN108281041A (en) * 2018-03-05 2018-07-13 东南大学 A kind of parking space's detection method blended based on ultrasonic wave and visual sensor
CN109405837A (en) * 2018-11-14 2019-03-01 蔚来汽车有限公司 Object positioning method, application and vehicle

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101686646B1 (en) * 2009-04-29 2016-12-28 코닌클리케 필립스 엔.브이. 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
CN102372000B (en) * 2010-08-23 2014-02-05 北京经纬恒润科技有限公司 Device and system for acquiring parking lot parameters
DE102011077173A1 (en) * 2011-06-08 2012-12-13 Robert Bosch Gmbh Method for determining a parking trajectory
CN102556075B (en) * 2011-12-15 2015-04-01 东南大学 Vehicle operating state estimation method based on improved extended Kalman filter
JP2017024683A (en) * 2015-07-28 2017-02-02 株式会社ジェイテクト Vehicular steering device
US10029577B2 (en) * 2016-06-15 2018-07-24 Qualcomm Incorporated Methods and apparatus for positioning a vehicle
CN108357498B (en) * 2018-02-07 2019-12-27 北京新能源汽车股份有限公司 Vehicle state parameter determination method and device and automobile
CN108873038B (en) * 2018-09-10 2020-11-06 芜湖盟博科技有限公司 Autonomous parking positioning method and positioning system

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020055819A1 (en) * 2000-11-08 2002-05-09 Yasuhiro Shimizu Vehicle navigation apparatus providing rapid correction for excessive error in dead reckoning estimates of vehicle travel direction by direct application of position and direction information derived from GPS position measurement data
US20110077898A1 (en) * 2008-06-11 2011-03-31 Peter Van Wyck Loomis Altimeter with calibration
CN103453913A (en) * 2013-08-29 2013-12-18 镇江青思网络科技有限公司 Integrated positioning method of vehicle in parking lot
CN104316059A (en) * 2014-11-05 2015-01-28 中国科学院嘉兴微电子与***工程中心 Dead-reckoning navigation positioning method and system for acquiring longitude and latitude of automobile by using speedometer
CN105946853A (en) * 2016-04-28 2016-09-21 中山大学 Long-distance automatic parking system and method based on multi-sensor fusion
CN107246868A (en) * 2017-07-26 2017-10-13 上海舵敏智能科技有限公司 A kind of collaborative navigation alignment system and navigation locating method
CN108281041A (en) * 2018-03-05 2018-07-13 东南大学 A kind of parking space's detection method blended based on ultrasonic wave and visual sensor
CN109405837A (en) * 2018-11-14 2019-03-01 蔚来汽车有限公司 Object positioning method, application and vehicle

Also Published As

Publication number Publication date
CN110077392B (en) 2020-08-25
CN110077392A (en) 2019-08-02

Similar Documents

Publication Publication Date Title
WO2020191977A1 (en) Dead reckoning method of automatic parking positioning system
CN108482379B (en) Wheel-hub motor driven vehicle coefficient of road adhesion and the synchronous real-time estimation system and method for road gradient
CN103407451B (en) A kind of road longitudinal and additional forces method of estimation
CN104354700B (en) A kind of vehicle parameter On-line Estimation method based on Unscented kalman filtering
CN111907516B (en) Full-automatic parking method and system
CN111547059A (en) Distributed driving electric automobile inertia parameter estimation method
CN107016157B (en) Pavement self-adaptive longitudinal speed estimation system and method for distributed driving electric automobile
CN108297872A (en) The full working scope vehicle-mounted road surface gradient estimates device and method
US20210240192A1 (en) Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery
CN110987463A (en) Multi-scene-oriented intelligent driving autonomous lane change performance test method
CN109606378A (en) Vehicle running state estimation method towards non-Gaussian noise environment
CN108819950B (en) Vehicle speed estimation method and system of vehicle stability control system
CN110530361B (en) Steering angle estimator based on agricultural machinery double-antenna GNSS automatic navigation system
CN110341714B (en) Method for simultaneously estimating vehicle mass center slip angle and disturbance
CN111381248A (en) Obstacle detection method and system considering vehicle bump
CN113247004A (en) Joint estimation method for vehicle mass and road transverse gradient
AU2020104234A4 (en) An Estimation Method and Estimator for Sideslip Angle of Straight-line Navigation of Agricultural Machinery
CN111806430B (en) Vehicle speed calculation method for automatic parking
CN103661398A (en) Vehicle non-steering left rear wheel linear speed estimation method based on sliding-mode observer
CN112744293A (en) Steering wheel zero offset angle measuring method and vehicle
CN109033017B (en) Vehicle roll angle and pitch angle estimation method under packet loss environment
CN114440881A (en) Unmanned vehicle positioning method integrating multi-source sensor information
CN110489807B (en) Local accurate positioning method for swing arm suspension structure inspection device
CN114475581B (en) Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion
CN116338719A (en) Laser radar-inertia-vehicle fusion positioning method based on B spline function

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19921538

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19921538

Country of ref document: EP

Kind code of ref document: A1