CN114475581B - Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion - Google Patents

Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion Download PDF

Info

Publication number
CN114475581B
CN114475581B CN202210181521.6A CN202210181521A CN114475581B CN 114475581 B CN114475581 B CN 114475581B CN 202210181521 A CN202210181521 A CN 202210181521A CN 114475581 B CN114475581 B CN 114475581B
Authority
CN
China
Prior art keywords
imu
vehicle
value
yaw
wheel
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210181521.6A
Other languages
Chinese (zh)
Other versions
CN114475581A (en
Inventor
秦明玉
于宏啸
夏天
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Liu Ma Chi Chi Technology Co ltd
Original Assignee
Beijing Liu Ma Chi Chi Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Liu Ma Chi Chi Technology Co ltd filed Critical Beijing Liu Ma Chi Chi Technology Co ltd
Priority to CN202210181521.6A priority Critical patent/CN114475581B/en
Publication of CN114475581A publication Critical patent/CN114475581A/en
Application granted granted Critical
Publication of CN114475581B publication Critical patent/CN114475581B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • 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
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • 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
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • 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
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/105—Speed
    • 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
    • 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

Abstract

The invention belongs to the technical field of automatic driving, and particularly relates to a real-time positioning method of an automatic driving parking system. An automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion comprises the following steps: s1, acquiring a pulse rotating speed signal of a vehicle wheel and a yaw angular speed signal of an IMU sensor; initializing and calculating an IMU yaw velocity null shift value; s2, compensating the yaw angular speed of the IMU sensor based on a zero drift value; s3, calculating the yaw angular velocity and the vehicle velocity of the vehicle; s4, fusing the obtained vehicle yaw velocity value and the yaw velocity value measured in real time by the IMU through Kalman filtering; s5, performing sliding window filtering again; and S6, carrying out dead reckoning to obtain a real-time coordinate value of the vehicle. The method is utilized to enable the real-time pose of the obtained vehicle to be more accurate and realize accurate parking and exiting.

Description

Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion
Technical Field
The invention belongs to the technical field of automatic driving, and particularly relates to a real-time positioning method of an automatic driving parking system.
Background
One of the main development directions of future automobiles is intellectualization, and an automatic parking system is used as an important component of an intelligent automobile and is more and more concerned by scientific research institutions and automobile manufacturers. However, due to the complexity of parking scenes and the characteristic of low-speed running, the real-time accurate positioning of the vehicle has been a technical difficulty of automatic parking.
At present, in the field of automatic parking, information such as vision, ultrasonic radar and the like is mostly adopted to position vehicles in real time. One of the automatic parking positioning methods is based on a space map, and the method calculates the relative position of the self-vehicle through real-time vehicle speed and steering wheel angle information, so that a two-dimensional space map taking the self-vehicle as a reference is constructed, meanwhile, sensing data of a self-vehicle sensor is projected and mapped into the space map, synchronous association of the self-vehicle and the surrounding environment is realized, and the positioning problem in the parking process is solved.
On one hand, the positioning method based on the visual and ultrasonic information mapping consumes larger computing resources and has higher cost; on the other hand, due to cost and platform computing power, most of the vision and ultrasonic information of the vehicles in mass production at present can not accurately identify various complex working conditions, and only can play an auxiliary role.
Disclosure of Invention
The purpose of the invention is: the invention discloses an automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion, and solves the problem of low positioning accuracy in the current automatic parking scene.
The technical scheme of the invention is as follows: an automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion comprises the following steps:
s1, acquiring a pulse rotating speed signal omega of a left rear wheel of a vehicle L (i) And a right rear wheel pulse rotating speed signal omega R (i) IMU sensor yaw rate signal ω IMU (i) (ii) a And initializing and calculating the zero drift value of the yaw velocity of the IMU sensor:
Figure BDA0003521274050000011
b is the initialization time sequence length of the IMU sensor yaw angular velocity, and i is the frame number of signal acquisition;
s2, compensating the yaw angular velocity of the IMU sensor in real time based on the zero drift value to obtain a vehicle yaw angular velocity estimated value based on the IMU
Figure BDA0003521274050000021
Figure BDA0003521274050000022
S3, observing the pulse signals of the left rear wheel and the right rear wheel so as to calculate the yaw angular velocity theta' (i) and the vehicle velocity v (i),
Figure BDA0003521274050000023
Figure BDA0003521274050000024
wherein N is R (i) And N L (i) The pulse signals are right rear wheel pulse signals and left rear wheel pulse signals respectively, P is the number of teeth, d is the diameter of a wheel, d is the wheel track, w is the wheel speed, and delta t is sampling time;
s4, calculating a yaw angular velocity value theta' (i) obtained by wheel speed pulse and performing null shift processing on the IMU yaw angular velocity value
Figure BDA0003521274050000025
Performing Kalman filtering to obtain a fused yaw angular velocity value, and performing integral calculation to obtain a real-time yaw angular value of the vehicle
Figure BDA0003521274050000026
The kalman filtering includes two processes of prediction and update:
and (3) prediction process:
Figure BDA0003521274050000027
Figure BDA0003521274050000028
and (3) updating:
Figure BDA0003521274050000029
Figure BDA00035212740500000210
Figure BDA00035212740500000211
wherein z (i) ═ θ' (i) ω IMU (i)]A is a measurement matrix here [1 ]]Q is the measurement error, H is the state input matrix, here [1, 1 ]]R is state input noise, and P is a covariance matrix;
s5, carrying out sliding window filtering on the yaw velocity X (i) obtained in the step S4 to obtain the smoothed yaw velocity X V (i)；
Figure BDA00035212740500000212
Wherein c is the width of the sliding window;
and S6, carrying out dead reckoning by utilizing a kinematic formula, the calculated yaw angle value and the filtered speed value to obtain a real-time coordinate value of the vehicle.
Has the advantages that: according to the invention, Kalman filtering fusion is carried out by utilizing various signals acquired by the existing real vehicle and IMU output data, and the fused result is filtered again to calculate the position and posture of the vehicle body in real time, so that the obtained real-time position and posture of the vehicle are more accurate, and accurate parking in and out can be realized under more complicated working conditions.
Drawings
FIG. 1 is a flow chart of the present invention.
Detailed Description
Embodiment 1, referring to fig. 1, an automatic parking positioning method based on wheel speed pulse and IMU kalman filter fusion includes the following steps:
s1, acquiring a pulse rotating speed signal omega of a left rear wheel of a vehicle L (i) And a right rear wheel pulse rotating speed signal omega R (i) IMU sensor yaw rate signal ω IMU (i) (ii) a And initializing and calculating a zero drift value of the yaw velocity of the IMU sensor:
Figure BDA0003521274050000031
b is the initialization time sequence length of the IMU sensor yaw angular velocity, and i is the frame number of signal acquisition;
for the yaw angular velocity value input by the IMU in real time, a certain value still exists when the vehicle speed is 0, the value is zero drift, the zero drift of the data needs to be calculated by adopting data under a certain number of frames, and the zero drift is added into the calculation when the vehicle runs. In this example, when the vehicle speed is 0, the IMU yaw rate input for 100 frames is taken, the average value is calculated and taken as the null shift value in the test, and when the vehicle speed is not 0, the value needs to be added to the compensation of the yaw rate value input by the IMU, so that the processed value is closer to the true value;
s2, compensating the yaw angular velocity of the IMU sensor in real time based on the zero drift value to obtain a vehicle yaw angular velocity estimated value based on the IMU
Figure BDA0003521274050000032
Figure BDA0003521274050000033
S3, observing the pulse signals of the left rear wheel and the right rear wheel so as to calculate the yaw angular velocity theta' (i) and the vehicle velocity v (i),
Figure BDA0003521274050000034
Figure BDA0003521274050000035
wherein N is R (i) And N L (i) The pulse signals of the right rear wheel and the left rear wheel are respectively, P is the tooth number, d is the diameter of the wheel, d is the wheel track, w is the wheel speed, and delta t is sampling time;
the reason why the two rear wheels are used for calculation instead of the two front wheels is that the front wheels are steering wheels, redundant errors are generated in the steering process, the front wheels are used for calculating the yaw angular speed of the vehicle by using the wheel speed difference, the front wheel rotating angles are required to be involved, and the values only have static calibration data at present and are not accurate enough.
S4, calculating a yaw angular velocity value theta' (i) obtained by wheel speed pulse and performing null shift processing on the IMU yaw angular velocity value
Figure BDA0003521274050000041
Performing Kalman filtering to obtain a fused yaw angular velocity value, and performing integral calculation to obtain a real-time yaw angular value of the vehicle
Figure BDA0003521274050000042
The kalman filtering includes two processes of prediction and update:
and (3) prediction process:
Figure BDA0003521274050000043
Figure BDA0003521274050000044
and (3) updating:
Figure BDA0003521274050000045
Figure BDA0003521274050000046
Figure BDA0003521274050000047
wherein z (i) ═ θ' (i) ω IMU (i)]A is a measurement matrix here [1 ]]Q is the measurement error, H is the state input matrix, here [1, 1 ]]R is state input noise, and P is a covariance matrix;
s5, carrying out sliding window filtering on the yaw velocity X (i) obtained in the step S4 to obtain the smoothed yaw velocity X V (i)；
Figure BDA0003521274050000048
Wherein c is the width of the sliding window; in this example, the sliding window width c is 20;
s6, carrying out dead reckoning by utilizing a kinematic formula, the calculated yaw angle value and the filtered speed value to obtain a real-time coordinate value of the vehicle;
the method for dead reckoning comprises the following steps:
Figure BDA0003521274050000049
Figure BDA00035212740500000410
wherein x '(i), y' (i) are respectively the updated values of the current real-time horizontal and vertical coordinates of the vehicle, r is the rolling radius of the wheel, and omega R (i) And ω L (i) And respectively controlling the speed of the right rear wheel and the left rear wheel, and integrating the state parameters to obtain real-time vehicle position coordinates and a real-time course value.
Although the invention has been described in detail above with reference to a general description and specific examples, it will be apparent to one skilled in the art that modifications or improvements may be made thereto based on the invention. Accordingly, such modifications and improvements are intended to be within the scope of the invention as claimed.

Claims (3)

1. The automatic parking positioning method based on the fusion of wheel speed pulse and IMU Kalman filtering is characterized by comprising the following steps:
s1, acquiring a pulse rotating speed signal omega of a left rear wheel of a vehicle L (i) And a right rear wheel pulse rotating speed signal omega R (i) IMU sensor yaw rate signal ω IMU (i) (ii) a And initializing and calculating a zero drift value of the yaw velocity of the IMU sensor:
Figure FDA0003775368480000011
b is the initialization time sequence length of the IMU sensor yaw angular velocity, and i is the frame number of signal acquisition;
s2, compensating the yaw angular velocity of the IMU sensor in real time based on the zero drift value to obtain a vehicle yaw angular velocity estimated value based on the IMU
Figure FDA0003775368480000012
Figure FDA0003775368480000013
S3, observing the pulse signals of the left rear wheel and the right rear wheel so as to calculate the yaw angular velocity theta' (i) and the vehicle velocity v (i),
Figure FDA0003775368480000014
Figure FDA0003775368480000015
wherein N is R (i) And N L (i) Respectively the right rear and left rear wheel pulse signals, P g Is the number of teeth, d is the diameter of the wheel, b is the wheel track, w is the wheel speed, Δ t is the sampling time;
s4, calculating a yaw angular velocity value theta' (i) obtained by wheel speed pulse and performing null shift processing on the IMU yaw angular velocity value
Figure FDA0003775368480000016
Performing Kalman filtering to obtain a fused yaw angular velocity value, and performing integral calculation to obtain a real-time yaw angular value of the vehicle
Figure FDA0003775368480000017
The kalman filtering includes two processes of prediction and update:
and (3) prediction process:
Figure FDA0003775368480000018
Figure FDA0003775368480000019
and (3) updating:
Figure FDA0003775368480000021
Figure FDA0003775368480000022
Figure FDA0003775368480000023
wherein z (i) ═ θ' (i) ω IMU (i)]A is a measurement matrix here [1 ]]Q is the measurement error, H is the state input matrix, here [1, 1 ]]R is state input noise, P is a covariance matrix, and K is Kalman gain;
s5, carrying out sliding window filtering on the yaw velocity X (i) obtained in the step S4 to obtain the smoothed yaw velocity X V (i)；
Figure FDA0003775368480000024
Wherein c is the width of the sliding window;
and S6, carrying out dead reckoning by utilizing a kinematic formula, the calculated yaw angle value and the filtered speed value to obtain a real-time coordinate value of the vehicle.
2. The method for automatic parking positioning based on wheel speed pulse and IMU kalman filter fusion of claim 1, wherein in S5, the sliding window width c is 20.
3. The automatic parking positioning method based on the fusion of wheel speed pulse and IMU Kalman filter as claimed in claim 1, wherein in S6, the dead reckoning method is:
Figure FDA0003775368480000025
Figure FDA0003775368480000026
wherein x '(i), y' (i) are respectively the updated values of the current real-time horizontal and vertical coordinates of the vehicle, r is the rolling radius of the wheel, theta is the yaw angle of the vehicle, and omega is R (i) And omega L (i) And integrating the state parameters to obtain real-time vehicle position coordinates and a real-time vehicle course value respectively for the wheel speeds of the right rear wheel and the left rear wheel.
CN202210181521.6A 2022-02-25 2022-02-25 Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion Active CN114475581B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210181521.6A CN114475581B (en) 2022-02-25 2022-02-25 Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210181521.6A CN114475581B (en) 2022-02-25 2022-02-25 Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion

Publications (2)

Publication Number Publication Date
CN114475581A CN114475581A (en) 2022-05-13
CN114475581B true CN114475581B (en) 2022-09-16

Family

ID=81483671

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210181521.6A Active CN114475581B (en) 2022-02-25 2022-02-25 Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion

Country Status (1)

Country Link
CN (1) CN114475581B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115959122B (en) * 2023-03-10 2023-06-02 杭州枕石智能科技有限公司 Method and device for positioning vehicle in parking scene, electronic equipment and storage medium

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110077392A (en) * 2019-03-28 2019-08-02 惠州市德赛西威汽车电子股份有限公司 A kind of reckoning method of automatic parking positioning system
CN111220154A (en) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 Vehicle positioning method, device, equipment and medium
CN112566828A (en) * 2018-08-17 2021-03-26 罗伯特·博世有限公司 Driving assistance method for vehicle, control unit, driving assistance system, and vehicle
CN113147738A (en) * 2021-02-26 2021-07-23 重庆智行者信息科技有限公司 Automatic parking positioning method and device
CN113503873A (en) * 2021-07-14 2021-10-15 北京理工大学 Multi-sensor fusion visual positioning method
CN114013427A (en) * 2021-11-13 2022-02-08 安徽江淮汽车集团股份有限公司 Parking data processing method for automatic parking test

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9238483B2 (en) * 2011-04-19 2016-01-19 Ford Global Technologies, Llc Trailer backup assist system with trajectory planner for multiple waypoints
US20170267233A1 (en) * 2016-03-15 2017-09-21 Cruise Automation, Inc. Method for autonomous vehicle parking

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112566828A (en) * 2018-08-17 2021-03-26 罗伯特·博世有限公司 Driving assistance method for vehicle, control unit, driving assistance system, and vehicle
CN110077392A (en) * 2019-03-28 2019-08-02 惠州市德赛西威汽车电子股份有限公司 A kind of reckoning method of automatic parking positioning system
CN111220154A (en) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 Vehicle positioning method, device, equipment and medium
CN113147738A (en) * 2021-02-26 2021-07-23 重庆智行者信息科技有限公司 Automatic parking positioning method and device
CN113503873A (en) * 2021-07-14 2021-10-15 北京理工大学 Multi-sensor fusion visual positioning method
CN114013427A (en) * 2021-11-13 2022-02-08 安徽江淮汽车集团股份有限公司 Parking data processing method for automatic parking test

Also Published As

Publication number Publication date
CN114475581A (en) 2022-05-13

Similar Documents

Publication Publication Date Title
CN112083726B (en) Park-oriented automatic driving double-filter fusion positioning system
JP2022019642A (en) Positioning method and device based upon multi-sensor combination
CN110077392B (en) Dead reckoning method of automatic parking positioning system
CN113819914A (en) Map construction method and device
CN112014122B (en) Wheel corner measuring device and operation method
CN106568449B (en) A kind of GNSS/INS Combinated navigation method of auto model auxiliary and constraint based on MEMS
CN107063305B (en) Method for correcting downhill suspended rear wheel odometer error by using inertial navigation and pressure sensors
CN108759822B (en) Mobile robot 3D positioning system
CN112433531A (en) Trajectory tracking method and device for automatic driving vehicle and computer equipment
CN113503873B (en) Visual positioning method for multi-sensor fusion
CN113819905A (en) Multi-sensor fusion-based odometer method and device
CN114475581B (en) Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion
CN114966629A (en) Vehicle body laser radar external reference calibration method based on EKF algorithm framework
CN112326990A (en) Method and system for measuring speed of working vehicle
CN110426011A (en) Vehicular turn angle measuring system and method
CN115060257A (en) Vehicle lane change detection method based on civil-grade inertia measurement unit
CN111806430A (en) Vehicle speed calculation method for automatic parking
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN112710301B (en) High-precision positioning method and system for automatic driving vehicle
CN115993089B (en) PL-ICP-based online four-steering-wheel AGV internal and external parameter calibration method
CN114440881B (en) Unmanned vehicle positioning method integrating multi-source sensor information
CN116338719A (en) Laser radar-inertia-vehicle fusion positioning method based on B spline function
CN113048987A (en) Vehicle navigation system positioning method
Kochem et al. Accurate local vehicle dead-reckoning for a parking assistance system
CN111469855A (en) Vehicle motion parameter calculation method

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