Intelligent parking dead reckoning method and device and storage medium

Abstract

The invention provides an intelligent parking dead reckoning method and device and a storage medium. The method comprises the following steps: initializing a vehicle state vector, a system error covariance matrix, a system error matrix and an observation noise matrix of a vehicle at the current moment; according to the vehicle state vector at the previous moment, a predicted value of the vehicle state vector at the current moment is calculated through a vehicle kinematic model, and a state transition matrix is calculated based on a kinematic equation of the predicted value; calculating an observation matrix based on the front wheel driving distance, the rear axle center speed and the vehicle angular speed, and calculating a Kalman gain; on the basis of the predicted value, the Kalman gain and the observed value of Kalman filtering, updating the vehicle state vector to an optimal estimated value, and updating a system error covariance matrix; and performing iterative operation so as to continuously optimize the optimal estimation value of the vehicle state vector, and continuously outputting a dead reckoning result. According to the invention, the cost is reduced, and the precision and accuracy of the parking dead-reckoning positioning result are improved.

Description

Technical Field

[0001] The present application relates to the field of new energy vehicle technology, and in particular to an intelligent parking dead reckoning method, device and storage medium. Background Art

[0002] Parking is a challenging task in vehicle operation and has attracted widespread attention due to its high difficulty and high accident rate. The increase in the number of cars in cities has made the parking environment more complex, further increasing the difficulty of parking. To this end, the research and development of intelligent parking systems, aimed at improving safety and reducing driving burden, has become a research hotspot. The dead reckoning technology in intelligent parking, that is, calculating the coordinates and heading angle of the vehicle relative to the starting position through on-board sensors, is crucial to improving parking efficiency and safety.

[0003] In the prior art, there are various methods for dead reckoning, but there are a series of disadvantages. For example, some systems rely on high-precision inertial measurement units (IMUs) to provide acceleration and angular velocity data, which are expensive and not suitable for mass-produced vehicles. Other systems use on-board cameras to identify parking spaces, but their positioning accuracy is limited when there are no parking space lines or poor lighting conditions. The most mainstream method currently combines wheel speed pulses and on-board gyroscopes to perform dead reckoning through kinematic equations, but in practical applications, factors such as low-speed driving, large turns, and changes in the external environment during parking will affect the accuracy of dead reckoning. For example, the methods mentioned in the prior art literature do not fully consider the changes in vehicle dynamic parameters during parking, such as changes in wheelbase and wheelbase, as well as external factors such as road conditions, which lead to insufficient dead reckoning accuracy. The linear Kalman filter used in the prior art also fails to effectively consider the influence of the front wheel yaw angle error and vehicle speed error under the Ackerman steering model, as well as the problem that the gyroscope is prone to accumulate large errors when driving at low speeds, further limiting the realization of high-precision positioning of the intelligent parking system.

[0004] In summary, the existing dead reckoning technology has not been able to effectively cope with the complex dynamic changes in parking scenarios while maintaining cost-effectiveness, especially in terms of high-precision positioning. Therefore, there is an urgent need for a method that can accurately estimate the vehicle position during parking without adding additional costs to make up for the shortcomings of existing technologies. Summary of the invention

[0005] In view of this, the embodiments of the present application provide an intelligent parking dead reckoning method, device and storage medium to solve the problems of high equipment cost, insufficient track reckoning accuracy and inaccurate parking dead reckoning results in the prior art.

[0006] In a first aspect of an embodiment of the present application, a method for dead reckoning of an intelligent parking vehicle is provided, including: initializing a vehicle state vector, a system error covariance matrix, a system error matrix, and an observation noise matrix of a vehicle at a current moment, and using the initialized vehicle state vector as the initial state of the system; calculating a predicted value corresponding to the vehicle state vector at a current moment using a vehicle kinematic model according to the vehicle state vector output at a previous moment, and calculating a state transfer matrix based on a kinematic equation of the predicted value; acquiring wheel speed pulse sensor data and steering wheel angle data of the vehicle in real time, and calculating the front wheel travel distance, rear axle center The invention relates to a method for determining the vehicle state vector and the vehicle angular velocity; calculating the observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculating the Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix; updating the vehicle state vector to the optimal estimate value based on the predicted value, the Kalman gain and the observed value of the Kalman filter, and updating the system error covariance matrix; performing an iterative operation to continuously optimize the optimal estimate value of the vehicle state vector, and continuously outputting the estimated results of the vehicle position and heading angle in the global coordinate system, and using the estimated results as the dead reckoning results in vehicle parking.

[0007] In a second aspect of the embodiment of the present application, a smart parking dead reckoning device is provided, including: an initialization module, configured to initialize a vehicle state vector, a system error covariance matrix, a system error matrix, and an observation noise matrix of a vehicle at a current moment, and use the initialized vehicle state vector as the initial state of the system; a first calculation module, configured to calculate a predicted value corresponding to the vehicle state vector at a current moment using a vehicle kinematic model according to the vehicle state vector output at a previous moment, and calculate a state transfer matrix based on the kinematic equation of the predicted value; an acquisition module, configured to acquire wheel speed pulse sensor data and steering wheel angle data of the vehicle in real time, and calculate the front wheel travel distance, rear axle The invention relates to a method for controlling the vehicle state vector and the vehicle angular velocity; a second calculation module is configured to calculate the observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculate the Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix; an update module is configured to update the vehicle state vector to the optimal estimate value based on the predicted value, the Kalman gain and the observed value of the Kalman filter, and update the system error covariance matrix; an output module is configured to perform an iterative operation so as to continuously optimize the optimal estimate value of the vehicle state vector, and continuously output the estimated results of the vehicle position and heading angle in the global coordinate system, and use the estimated results as the dead reckoning results in vehicle parking.

[0008] According to a third aspect of an embodiment of the present application, a computer-readable storage medium is provided, which stores a computer program. When the computer program is executed by a processor, the steps of the above method are implemented.

[0009] At least one of the above technical solutions adopted in the embodiments of the present application can achieve the following beneficial effects:

[0010] The vehicle state vector, system error covariance matrix, system error matrix and observation noise matrix of the vehicle at the current moment are initialized, and the initialized vehicle state vector is used as the initial state of the system; according to the vehicle state vector output at the previous moment, the predicted value corresponding to the vehicle state vector at the current moment is calculated using the vehicle kinematic model, and the state transfer matrix is ​​calculated based on the kinematic equation of the predicted value; the wheel speed pulse sensor data and steering wheel angle data of the vehicle are obtained in real time, and the front wheel travel distance, rear axle center speed and vehicle angular velocity are calculated; based on the front wheel travel distance, rear axle center speed and vehicle angular velocity, the observation matrix is ​​calculated, and based on the system error covariance matrix, the state transfer matrix and the observation matrix, the Kalman gain is calculated; based on the predicted value, the Kalman gain and the observed value of the Kalman filter, the vehicle state vector is updated to the optimal estimated value, and the system error covariance matrix is ​​updated; iterative operations are performed to continuously optimize the optimal estimated value of the vehicle state vector, and the estimated results of the vehicle position and heading angle in the global coordinate system are continuously output, and the estimated results are used as the dead reckoning results in vehicle parking. This application reduces costs and improves the precision and accuracy of parking position positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0012] Figure 1 is a flow chart of a smart parking dead reckoning method provided in an embodiment of the present application;

[0013] Figure 2 is a schematic diagram of the structure of the EKF dead reckoning algorithm involved in the embodiment of the present application;

[0014] Figure 3 is a schematic diagram of the principle of the vehicle kinematic model provided in the embodiment of the present application;

[0015] Figure 4 This is a schematic diagram of the actual vehicle straight line test trajectory error provided by the embodiment of the present application;

[0016] Figure 5 is a schematic diagram of the parking scenario test trajectory error provided by an embodiment of the present application;

[0017] Figure 6is a structural schematic diagram of an intelligent parking dead reckoning device provided in an embodiment of the present application;

[0018] Figure 7 It is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0019] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0020] In vehicle operation, parking is difficult and the accident rate is high. At the same time, the annual increase in the number of urban cars has made the parking environment increasingly complex, and the difficulty of parking has further increased. Intelligent parking systems that can significantly improve safety and reduce driving intensity have become a research hotspot in recent years. Vehicle dead reckoning refers to the use of on-board sensors to calculate the horizontal and vertical coordinates and heading angles of the vehicle relative to the zero position. The existing dead reckoning schemes include the following: 1. Use wheel speed to output vehicle speed, cooperate with high-precision IMU to output acceleration and angular velocity, and integrate acceleration and angular velocity for positioning, but the high-precision IMU has a high cost and is not suitable for mass-produced vehicles, and is not practical. 2. Rely on on-board cameras to identify parking spaces for dead reckoning, but cameras are not suitable for use in situations where there are no parking space lines or the parking space lines cannot be seen. At the same time, the positioning accuracy of this scheme is easily affected by conditions such as light intensity. 3. The most mainstream solution at present is to use wheel speed pulses, on-board gyroscopes, and the kinematic equations of the vehicle to achieve dead reckoning.

[0021] However, when parking, the vehicle is traveling at a low speed, and the signal-to-noise ratio of the wheel speed signal is relatively small; when parking, the vehicle turns sharply, and the insufficient rigidity of the vehicle body causes changes in parameters such as wheelbase and track; parking is affected by various external factors (such as road conditions, etc.). This makes the current mainstream traditional dead reckoning algorithm (such as based on the rear wheel speed signal) unable to meet the requirements of intelligent parking systems for high-precision vehicle positioning.

[0022] For example, in the prior art 1, “A dead reckoning method for an automatic parking positioning system” only the rear wheel speed signal and angular velocity are used for dead reckoning, and a linear Kalman filter is used for data fusion to calculate the dead position. The influence of the front wheel yaw angle error and vehicle speed error on the entire dead reckoning system under the Ackerman model is not considered.

[0023] For example, the method of eliminating invalid wheel speed pulses in the "optimization algorithm for automatic parking trajectory calculation" disclosed in the second prior art improves the accuracy, but does not fully consider the wheel speed error caused by the vehicle body stiffness, road conditions, tire pressure, etc. in the parking situation. At the same time, the application uses a gyroscope to obtain the angular velocity as the observation of the Kalman filter, which will inevitably introduce a relatively large cumulative error of the gyroscope in the process of dead reckoning integration.

[0024] In view of the deficiencies in the prior art, this application proposes an intelligent parking dead reckoning method based on extended Kalman filtering. The technical solution of this application uses the wheel speed pulses of the four wheels and the steering wheel angle signal combined with the vehicle kinematic model, and uses the nonlinear extended Kalman filter (EKF) as a data fusion algorithm to perform dead reckoning in the automatic parking scenario. Compared with the traditional dead reckoning algorithm, the advantages of the EKF algorithm are: fusing the signals of different independent sensors and improving the accuracy of dead reckoning by processing redundant information.

[0025] The technical solution of this application uses 4 front wheel speed pulses and steering wheel angle signals as EKF observations. The 4-wheel data can more comprehensively and accurately estimate the vehicle speed error caused by various reasons in the case of automatic parking. At the same time, the calibrated steering wheel angle signal does not have the integral cumulative error caused by zero bias like a gyroscope, which can greatly improve the positioning accuracy. Compared with the current non-mainstream technical solutions, the technical solution of this application will not introduce new sensors and increase costs, which makes the technical solution of this application adaptable to various high- and low-end models and more suitable for mass production.

[0026] The contents of the technical solution of the present application are described in detail below with reference to the accompanying drawings and specific embodiments.

[0027] Figure 1 It is a flowchart of the intelligent parking dead reckoning method provided in an embodiment of the present application. Figure 1 The intelligent parking dead reckoning method can be executed by the vehicle-side intelligent parking system. Figure 1 As shown, the smart parking dead reckoning method may specifically include:

[0028] S101, initializing the vehicle state vector, system error covariance matrix, system error matrix and observation noise matrix of the vehicle at the current moment, and using the initialized vehicle state vector as the initial state of the system;

[0029] S102, according to the vehicle state vector output at the previous moment, using the vehicle kinematic model to calculate the predicted value corresponding to the vehicle state vector at the current moment, and calculating the state transfer matrix based on the kinematic equation of the predicted value;

[0030] S103, acquiring wheel speed pulse sensor data and steering wheel angle data of the vehicle in real time, and calculating the front wheel travel distance, rear axle center speed and vehicle angular velocity;

[0031] S104, calculating an observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculating a Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix;

[0032] S105, based on the predicted value, the Kalman gain and the observed value of the Kalman filter, the vehicle state vector is updated to the optimal estimated value, and the system error covariance matrix is ​​updated;

[0033] S106, performing an iterative operation to continuously optimize the optimal estimated value of the vehicle state vector, and continuously outputting the estimated results of the position and heading angle of the vehicle in the global coordinate system, and using the estimated results as the dead reckoning results in the vehicle parking process.

[0034] This application uses the vehicle's four-wheel speed pulse signal and steering wheel angle signal as data input, and adopts the extended Kalman filter (formerly referred to as EKF) combined with the vehicle kinematic model for dead reckoning. The introduction of the four-wheel speed can more comprehensively estimate the vehicle speed error in the parking scenario. The calibrated steering wheel angle signal will not introduce cumulative errors. Using them as the observation quantity of the kinematic model can greatly improve the positioning accuracy of the algorithm.

[0035] The basic principle and implementation process of the EKF dead reckoning algorithm are described below in conjunction with the accompanying drawings and specific embodiments. Figure 2 Schematic diagram of the structure of the EKF dead reckoning algorithm involved in the embodiment of the present application. Figure 2 As shown, the EKF dead reckoning algorithm of the embodiment of the present application may include the following contents:

[0036] First, the algorithm selects the coordinates of the midpoint of the vehicle's rear axle in the global coordinate system (X RM , Y RM ), heading angle θ, heading angle change rate ω, and vehicle speed v are state quantities. The system state vector at time k is defined as:

[0037] X k =[X RM ,Y RM ,θ,ω,v] T (1)

[0038] Coordinate system definition: The position of the vehicle when it starts parking is defined as (X, Y) = (0, 0), the heading angle θ = 0, the upper left front of the vehicle body is the global coordinate system in the XYZ direction, and the vehicle coordinate system takes the center of the rear axle of the wheel as the origin and the upper left front of the vehicle body as the XYZ direction.

[0039] Furthermore, let the unit time be T0, and use the optimal state value X at time k-1 k－1 , calculate the estimated state value X at time k according to the kinematic model k , k－1 :

[0040]

[0041] Among them, ω is the angular velocity of the car, v is the center speed of the rear axle of the car (i.e., the vehicle speed), and θ is the heading angle.

[0042] Furthermore, the partial derivative of the above nonlinear function (2) is obtained to obtain the state transfer matrix

[0043]

[0044] Furthermore, the distance traveled per unit time by the left and right front wheels is selected as l FL , l FR (the product of wheel speed and unit time), vehicle heading angle change rate ω SW (calculated using the rear wheel speed and steering wheel angle sensor signal), rear axle midpoint speed v R (average value of rear wheel speed signal) is the observed quantity, then the observation equation is:

[0045] Z k =[l FL ,L FR ,ω SW , v R ] (4)

[0046] Among them l FL , l FR Obtained by the following formula:

[0047] l FL =T0*V FL ,l FR =T0*V FR , V FL ,V FR is the front left wheel speed and the front right wheel speed, which can be calculated by the wheel speed pulse formula:

[0048] V=πDN / pΔt (D is the wheel diameter, N is the number of pulses measured by the left and right wheel encoders within the Δt time, and p is the number of pulses output by the encoder per rotation).

[0049] In formula 4, ω SW Obtained by the following formula: SW =v R*tanψ / L, where ψ is the yaw angle of the front wheel center of the vehicle, which is obtained by dividing the steering wheel angle Ω by the transmission ratio g to obtain ψ=Ω / g; L is the center wheelbase of the front and rear axles of the vehicle, which can be obtained through actual vehicle measurement. At the same time, in order to improve the accuracy of the algorithm, the transmission ratio g can be calibrated to obtain the transmission ratio g corresponding to different steering wheel angles Ω in the case of low-speed parking.

[0050] Furthermore, according to the vehicle kinematic model and the established observation equation, the nonlinear function of the observation equation can be obtained as follows:

[0051] h(·)=[T0(ve·ω), T0(v+e·ω),ω,v] T (5)

[0052] Furthermore, the measurement matrix H is obtained by taking partial derivative of the observation equation h(·) k :

[0053]

[0054] Furthermore, according to the extended Kalman filter formula, the system error covariance matrix P at time k is k，k-1 The estimated values ​​are:

[0055]

[0056] Furthermore, the Kalman gain K at time k is k for:

[0057]

[0058] Furthermore, the optimal value of the state vector at time k is:

[0059]

[0060] Furthermore, the updated covariance matrix P of the system error is k for:

[0061] P k =[IK k H k ]P k，k-1 (10)

[0062] The above formulas 1 to 10 realize the vehicle state quantity X at time k k The optimal estimate of the vehicle state at any time can be obtained through the cyclic calculation and derivation of this process, that is, the position (x, y) and heading angle θ of the vehicle in the global coordinate system at any time.

[0063] Through the calculation steps of the EKF dead reckoning algorithm of the above embodiment, the position and heading angle of the vehicle in the global coordinate system can be accurately calculated at any time through the algorithm's cyclic calculation, thereby achieving high-precision positioning during the parking process. This process effectively integrates data from different sensors and improves positioning accuracy by dynamically correcting the prediction model, which is particularly suitable for dynamically changing parking environments.

[0064] In some embodiments, based on the vehicle state vector output at the previous moment, the following vehicle kinematics model is used to calculate the predicted value corresponding to the vehicle state vector at the current moment, and the vehicle kinematics model is:

[0065]

[0066] in, represents the predicted value of the vehicle state vector at the current moment, represents the estimated value of the vehicle state vector output at the previous moment, ω represents the angular velocity of the vehicle, v represents the center velocity of the rear axle of the vehicle, and θ represents the heading angle.

[0067] Specifically, when the vehicle state is initialized, the global coordinate system is first defined, with the vehicle parking start position as the origin (X RM , Y RM ), the initial heading angle θ is 0, and the coordinates of the center point of the rear axle of the vehicle are set (X RM , Y RM ), heading angle θ, heading angle change rate ω, and vehicle speed v are state quantities, forming a state vector. That is, the extended Kalman filter is initialized, and the system state quantity X k =[X RM ,Y RM ,θ,ω,v] T Initialized to [0,0,0,0,0] T , initialize the covariance matrix P of the system error k , initialize the system error matrix Q and the observation noise matrix R, and use the vehicle coordinate system in the initialization state as the global coordinate system.

[0068] Further, after completing the initialization operation of the extended Kalman filter, the vehicle motion model (see Formula 2) is used to calculate the system state estimate X after T0 = 0.01s. k,k-1 That is, using the vehicle state vector at the previous moment To predict the vehicle state vector at the current moment The state quantity is updated using the heading angle θ, the vehicle speed v and the angular velocity ω. The basic principle and implementation process of the vehicle kinematics model are described below in conjunction with the accompanying drawings and specific embodiments. Figure 3 Schematic diagram of the principle of the vehicle kinematic model provided in the embodiment of the present application. Figure 3As shown, the vehicle kinematics model of the embodiment of the present application may include the following contents:

[0069] Assume that the unit time is T0, and use the optimal state value X at time k-1 k－1 , calculate the estimated state value X at time k according to the kinematic model k，k－1 :

[0070]

[0071] Among them, ω is the angular velocity of the car, v is the center speed of the rear axle of the car (i.e., the vehicle speed), and θ is the heading angle.

[0072] In some embodiments, the state transfer matrix is ​​calculated based on the kinematic equation of the predicted value, including taking partial derivatives of the nonlinear kinematic equation using the following formula to obtain the state transfer matrix:

[0073]

[0074] Among them, Φ k，k-1 represents the state transfer matrix, which is used to characterize the change of vehicle state over time. Represents the estimated value of the vehicle state vector output at the previous moment.

[0075] Specifically, according to the content of the above embodiment, in the initialization stage, the vehicle state vector X is set k =[X RM ,Y RM ,θ,ω,v] T The initial state is zero. This state vector represents the coordinates of the rear axle center point of the vehicle (X RM , Y RM ), heading angle θ, heading angle change rate ω and vehicle speed v. At the same time, initialize the system error covariance matrix P k , system error matrix Q and observation noise matrix R.

[0076] Furthermore, in the prediction stage, according to the kinematic model of the vehicle, this embodiment uses a nonlinear function f(·) to convert the previous state vector Mapped to an estimate of the current state. This function takes into account the dynamic characteristics of the vehicle, such as steering, acceleration, etc.

[0077] Furthermore, in the update phase, the state transfer matrix Φ is calculated k,k-1 , which is the previous state vector The Jacobian matrix of the nonlinear function f(·) with respect to time k. As shown in Formula 2, the Jacobian matrix is ​​obtained by taking the derivative of the nonlinear function f(·), and its mathematical expression is

[0078] Next, the wheel speed pulse sensor and steering wheel angle sensor data are obtained from the vehicle chassis in real time. The wheel speed and steering wheel angle are used in combination with vehicle geometric parameters (such as wheelbase, wheelbase) and transmission ratio to calculate the observations, which will be used to update the state vector.

[0079] In some embodiments, the wheel speed pulse sensor data and steering wheel angle data of the vehicle are acquired in real time, and the front wheel travel distance, the rear axle center speed and the vehicle angular velocity are calculated, including:

[0080] The wheel speed pulse sensor data and steering wheel angle data are read from the vehicle chassis in real time, and the wheel speed pulses are converted into wheel speeds. The front wheel travel distance is calculated based on the wheel speeds of the left front wheel and the right front wheel, and the rear axle center speed is calculated based on the speed of the vehicle's rear wheels; the vehicle's front wheel center yaw angle is calculated based on the steering wheel angle data, and the vehicle's angular velocity is calculated based on the rear axle center speed and the vehicle's front wheel center yaw angle.

[0081] Specifically, the wheel speed pulse sensor data is read from the chassis of the car in real time, and the wheel speeds of the four wheels are calculated by the formula V=πDN / pΔt, where N is the pulse measured by the left and right wheel encoders within the Δt time, p is the number of pulses output by the encoder per rotation, Δt is set to T0=0.01s, p=3000 is the sensor parameter, which can be measured in advance, and D=0.7m is the wheel diameter measured in advance. In this way, the wheel speeds V of the four wheels can be calculated FL ,V FR ,V RL ,V RR . Calculate the travel distance per unit time of the left and right front wheels l according to the left and right front wheel speeds respectively FL , l FR , l FL =V FL *T0,l FR =V FR *T0.

[0082] Furthermore, the steering wheel angle Ω is read from the vehicle chassis in real time, and the yaw angle ψ=Ω / g of the vehicle's front wheel center is calculated, wherein g is calibrated in advance, such as g=14.6. In particular, the transmission ratio may also vary with the steering wheel angle according to the wire-controlled steering strategy of different vehicles, presenting a one-to-one correspondence. The one-to-one correspondence may be established through calibration, and the corresponding transmission ratio g may be obtained by fitting a curve or looking up a table in actual use.

[0083] Further, the vehicle rear axle center speed V is calculated based on the wheel speeds of the left and right rear wheels. R =(V RR +V RL ) / 2, then, according to the front wheel center yaw angle ψ and the rear axle center speed V R Calculate the vehicle angular velocity ωSW =v R *tanψ / L, where L is the wheelbase between the front and rear axles, which can be 2.8m.

[0084] In some embodiments, based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, the observation matrix is ​​calculated, including:

[0085] The observation equation is established based on the observation quantity of Kalman filter. The nonlinear function of the observation equation is determined based on the vehicle kinematic model and the observation equation. The partial derivative of the nonlinear function of the observation equation is calculated to obtain the observation matrix:

[0086] Among them, the front wheel travel distance, vehicle heading angle change rate and rear axle midpoint speed are used as Kalman filter observations.

[0087] Specifically, the embodiment of the present application discloses a nonlinear observation model and its linearization: a nonlinear observation model h(·) is defined, which maps the predicted state to the observation space. The observation matrix H is calculated by the h(·) model. k , which is h(·) with respect to the vehicle state vector The Jacobian matrix of corresponds to formulas (5) and (6) provided in the above embodiment.

[0088] In one example, according to the vehicle kinematics model and the established observation equation, the nonlinear function of the observation equation is obtained as follows:

[0089] h(·)=[T0(ve·w), T0(v+e·ω),ω,v] T (5)

[0090] Furthermore, the observation equation After taking partial derivatives, we get the measurement matrix (i.e., observation matrix) H k :

[0091]

[0092] It should be noted that the observation equation in the embodiment of the present application is defined as: Select the unit time travel distance l of the left and right front wheels FL , l FR , vehicle heading angle change rate ω SW , rear axle midpoint speed V R As the observation quantity Zk. That is, the observation quantity Zk of EKF is calculated by using the formulas 3 to 6 provided in the above embodiment = [l FL ,l FR ,ω SW ,V R]. The wheel speed signal is read by the sensor to calculate the distance traveled by the front wheel. The heading angle change rate is calculated using the steering wheel angle and vehicle geometric parameters (gear ratio, wheel diameter, wheelbase, etc.).

[0093] In some embodiments, the Kalman gain is calculated based on the system error covariance matrix, the state transfer matrix and the observation matrix, including using the following extended Kalman filter formula to calculate the Kalman gain:

[0094]

[0095] Among them, K k represents the Kalman gain at the current moment, P k,k-1 Represents the estimated value of the system error covariance matrix at the current moment, H k Represents the estimated value of the observation matrix at the current moment.

[0096] Specifically, the embodiment of the present application uses the formula of extended Kalman filtering to calculate the estimated value P of the error covariance matrix k，k-1 and the Kalman gain K k This embodiment corresponds to the above formula 7 and formula 8, that is, according to the extended Kalman filter formula, the system error covariance matrix P at time k k,k-1 The estimated values ​​are:

[0097]

[0098] Furthermore, the Kalman gain K at time k is k for:

[0099]

[0100] In some embodiments, the vehicle state vector is updated to the optimal estimate using the following formula:

[0101]

[0102] in, represents the optimal estimate of the updated vehicle state vector, represents the predicted value of the vehicle state vector at the current moment, K k represents the Kalman gain at the current moment, Z k Represents the observation value of the Kalman filter.

[0103] Specifically, after calculating the predicted value of the vehicle state vector and the Kalman gain at the current moment, the embodiment of the present application will update the state vector according to the observed value and the predicted value. To its optimal estimate, and update the system error covariance matrix P kThe optimal estimated value of the vehicle state vector at time k is calculated according to Formula 9 provided in the above embodiment, that is, the optimal value of the state vector at time k is:

[0104]

[0105] In some embodiments, updating the system error covariance matrix includes updating the system error covariance matrix to an optimal estimate using the following formula:

[0106] P k =[IK k H k ]P k，k-1

[0107] Among them, P k represents the optimal estimate of the updated system error covariance matrix, K k represents the Kalman gain at the current moment, H k Represents the estimated value of the observation matrix at the current moment, P k，k-1 Represents the estimated value of the system error covariance matrix at the current moment.

[0108] Specifically, the embodiment of the present application will also update the system error covariance matrix P according to formula 10: k To its optimal estimate. That is, the updated covariance matrix P of the system error k for:

[0109] P k =[IK k H k ]P k,k-1 (10)

[0110] Furthermore, the steps of the above embodiment are repeated, and an iterative update is performed every unit time T0, so that the optimal position and heading angle estimation of the vehicle can be obtained in real time, that is, the position (x, y) and heading angle θ of the vehicle in the global coordinate system at any time. Through the method of the above embodiment, the technical solution of the present application allows the intelligent parking system to continuously track the position and direction of the vehicle during the parking process, thereby realizing high-precision automatic parking assistance.

[0111] According to the technical solution provided by the above-mentioned embodiment of the present application, the beneficial effects of the technical solution of the present application are:

[0112] This application proposes an intelligent parking dead reckoning method based on extended Kalman filtering, which uses the wheel speed pulses of the four wheels, the steering wheel angle signal combined with the vehicle kinematic model, and uses the nonlinear extended Kalman filter (EKF) as a data fusion algorithm to perform dead reckoning in the automatic parking scenario. Compared with the traditional dead reckoning algorithm, the advantages of the EKF algorithm are: it fuses the signals of different independent sensors and improves the accuracy of dead reckoning by processing redundant information.

[0113] This application uses 4 front and rear wheel speed pulses and steering wheel angle signals as EKF observations. The 4-wheel data can more comprehensively and accurately estimate the vehicle speed error caused by various reasons in the case of automatic parking. At the same time, the calibrated steering wheel angle signal does not have the integral cumulative error caused by zero bias like the gyroscope, which can greatly improve the positioning accuracy. Compared with the current non-mainstream solutions, no new sensors will be introduced to increase costs, which makes the method of this application adaptable to various high- and low-end models and more suitable for mass production.

[0114] In terms of positioning accuracy, two real vehicle tests were conducted using the positioning algorithm of the embodiment of the present application, and the test data are as follows:

[0115] 1. Straight line test

[0116] Table 1 Actual vehicle straight line test error table

[0117]

[0118] like Figure 4 As shown, Figure 4 It is a schematic diagram of the trajectory error of the actual vehicle straight line test provided by the embodiment of the present application. It can be seen that the straight line test EKF estimated data is compared with the true value system positioning data. When driving in a straight line for about 50m, the distance error is within 0.25m and the straight line error is less than 1%.

[0119] 2. Actual parking scenario test

[0120] Table 2 EKF estimated value and true value in vehicle X direction

[0121]

[0122] Table 3 EKF estimated value and true value of vehicle Y direction

[0123]

[0124] Table 4 Vehicle heading offset EKF estimated value and pointer

[0125]

[0126] like Figure 5 As shown, Figure 5 This is a schematic diagram of the trajectory error of the parking scenario test provided in the embodiment of the present application. It can be seen that when the EKF extrapolated data of the actual parking scenario test is compared with the true value system positioning data, when the vehicle's X and Y direction offset is about 10m, the X and Y errors are about 15cm, the error ratio is less than 2%, and the heading deviation is about 2°. The error of the EKF algorithm in the parking test is slightly larger than its error in the straight-line test experiment, especially when the vehicle makes a large-angle turn. At this time, the steering wheel angle and wheel speed signals change greatly, so the accuracy and calibration of the sensor have a greater impact on the accuracy of the dead reckoning than when driving in a straight line. Despite this, the EKF algorithm still controls the error within a relatively ideal range in the actual vehicle test.

[0127] The following is an embodiment of the device of the present application, which can be used to execute the embodiment of the method of the present application. For details not disclosed in the embodiment of the device of the present application, please refer to the embodiment of the method of the present application.

[0128] Figure 6 Schematic diagram of the structure of the intelligent parking dead reckoning device provided in the embodiment of the present application. Figure 6 As shown, the intelligent parking dead reckoning device includes:

[0129] Initialization module 601 is configured to initialize the vehicle state vector, system error covariance matrix, system error matrix and observation noise matrix of the vehicle at the current moment, and use the initialized vehicle state vector as the initial state of the system;

[0130] The first calculation module 602 is configured to calculate the predicted value corresponding to the vehicle state vector at the current moment using the vehicle kinematic model according to the vehicle state vector output at the previous moment, and calculate the state transfer matrix based on the kinematic equation of the predicted value;

[0131] The acquisition module 603 is configured to acquire the wheel speed pulse sensor data and steering wheel angle data of the vehicle in real time, and calculate the front wheel travel distance, the rear axle center speed and the vehicle angular velocity;

[0132] The second calculation module 604 is configured to calculate the observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculate the Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix;

[0133] An updating module 605 is configured to update the vehicle state vector to an optimal estimate value based on the predicted value, the Kalman gain and the observed value of the Kalman filter, and to update the system error covariance matrix;

[0134] The output module 606 is configured to perform an iterative operation to continuously optimize the optimal estimated value of the vehicle state vector and continuously output the estimated results of the position and heading angle of the vehicle in the global coordinate system, and use the estimated results as the dead reckoning results in vehicle parking.

[0135] In some embodiments, Figure 6 The first calculation module 602 uses the following formula to calculate the partial derivative of the nonlinear kinematics equation to obtain the state transfer matrix:

[0136]

[0137] Among them, Φ k，k-1 represents the state transfer matrix, which is used to characterize the change of vehicle state over time. Represents the estimated value of the vehicle state vector output at the previous moment.

[0138] In some embodiments, Figure 6 The acquisition module 603 reads the wheel speed pulse sensor data and the steering wheel angle data from the vehicle chassis in real time, and converts the wheel speed pulse into the wheel speed of the wheel, calculates the front wheel travel distance according to the wheel speed of the left front wheel and the wheel speed of the right front wheel, and calculates the rear axle center speed according to the speed of the rear wheel of the vehicle; calculates the vehicle front wheel center yaw angle based on the steering wheel angle data, and calculates the vehicle angular velocity based on the rear axle center speed and the vehicle front wheel center yaw angle.

[0139] In some embodiments, Figure 6 The second calculation module 604 establishes an observation equation according to the observation quantity of the Kalman filter, determines the nonlinear function of the observation equation according to the vehicle kinematic model and the observation equation, calculates the partial derivative of the nonlinear function of the observation equation, and obtains the observation matrix: wherein the front wheel travel distance, the vehicle heading angle change rate and the rear axle midpoint speed are used as the observation quantities of the Kalman filter.

[0140] In some embodiments, Figure 6 The second calculation module 604 uses the following extended Kalman filter formula to calculate the Kalman gain:

[0141]

[0142] Among them, K k represents the Kalman gain at the current moment, P k，k-1 Represents the estimated value of the system error covariance matrix at the current moment, H k Represents the estimated value of the observation matrix at the current moment.

[0143] In some embodiments, Figure 6 The updating module 605 updates the vehicle state vector to the optimal estimated value using the following formula:

[0144]

[0145] in, represents the optimal estimate of the updated vehicle state vector, represents the predicted value of the vehicle state vector at the current moment, K k represents the Kalman gain at the current moment, Z k Represents the observation value of the Kalman filter.

[0146] In some embodiments, Figure 6 The updating module 605 updates the system error covariance matrix to the optimal estimate using the following formula:

[0147] P k =[IK k H k ]P k,k-1

[0148] Among them, P k represents the optimal estimate of the updated system error covariance matrix, H k represents the Kalman gain at the current moment, H k Represents the estimated value of the observation matrix at the current moment, P k，k-1 Represents the estimated value of the system error covariance matrix at the current moment.

[0149] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0150] Figure 7 Schematic diagram of the structure of the electronic device 7 provided in the embodiment of the present application. Figure 7 As shown, the electronic device 7 of this embodiment includes: a processor 701, a memory 702, and a computer program 703 stored in the memory 702 and executable on the processor 701. When the processor 701 executes the computer program 703, the steps in the above-mentioned method embodiments are implemented. Alternatively, when the processor 701 executes the computer program 703, the functions of the modules / units in the above-mentioned device embodiments are implemented.

[0151] Exemplarily, the computer program 703 may be divided into one or more modules / units, which are stored in the memory 702 and executed by the processor 701 to complete the present application. The one or more modules / units may be a series of computer program instruction segments capable of completing specific functions, which are used to describe the execution process of the computer program 703 in the electronic device 7.

[0152] The electronic device 7 may be a desktop computer, a notebook, a PDA, a cloud server, or other electronic device. The electronic device 7 may include, but is not limited to, a processor 701 and a memory 702. Those skilled in the art will appreciate that Figure 7 It is only an example of the electronic device 7 and does not constitute a limitation of the electronic device 7. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the electronic device may also include input and output devices, network access devices, buses, etc.

[0153] The processor 701 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc.

[0154] The memory 702 may be an internal storage unit of the electronic device 7, for example, a hard disk or memory of the electronic device 7. The memory 702 may also be an external storage device of the electronic device 7, for example, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device 7. Further, the memory 702 may also include both an internal storage unit of the electronic device 7 and an external storage device. The memory 702 is used to store computer programs and other programs and data required by the electronic device. The memory 702 may also be used to temporarily store data that has been output or is to be output.

[0155] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0156] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0157] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0158] In the embodiments provided in the present application, it should be understood that the disclosed devices / computer equipment and methods can be implemented in other ways. For example, the device / computer equipment embodiments described above are only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation. Multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0159] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0160] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0161] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. The computer program may include computer program code, which may be in source code form, object code form, executable file or some intermediate form. Computer-readable media may include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc.

[0162] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A method for intelligent parking dead reckoning, characterized in that: include: Initialize the vehicle state vector, system error covariance matrix, system error matrix and observation noise matrix of the vehicle at the current moment, and use the initialized vehicle state vector as the initial state of the system; According to the vehicle state vector output at the previous moment, a prediction value corresponding to the vehicle state vector at the current moment is calculated using a vehicle kinematic model, and a state transfer matrix is ​​calculated based on the kinematic equation of the prediction value; Acquire the vehicle's wheel speed pulse sensor data and steering wheel angle data in real time, and calculate the front wheel travel distance, rear axle center speed and vehicle angular velocity; Calculating an observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculating a Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix; Based on the predicted value, the Kalman gain and the observed value of the Kalman filter, the vehicle state vector is updated to an optimal estimated value, and the system error covariance matrix is ​​updated; An iterative operation is performed to continuously optimize the optimal estimated value of the vehicle state vector, and the estimated results of the position and heading angle of the vehicle in the global coordinate system are continuously output, and the estimated results are used as the dead reckoning results in vehicle parking.

2. The method according to claim 1, characterized in that: According to the vehicle state vector output at the previous moment, the prediction value corresponding to the vehicle state vector at the current moment is calculated using the following vehicle kinematics model, and the vehicle kinematics model is: in, represents the predicted value of the vehicle state vector at the current moment, represents the estimated value of the vehicle state vector output at the previous moment, ω represents the angular velocity of the vehicle, v represents the center velocity of the rear axle of the vehicle, and θ represents the heading angle.

3. The method according to claim 1, characterized in that The calculating of the state transfer matrix based on the kinematic equation of the predicted value includes obtaining the state transfer matrix by taking partial derivatives of the nonlinear kinematic equation using the following formula: Among them, Φ k，k-1 represents a state transfer matrix, which is used to characterize the change of vehicle state over time. Represents the estimated value of the vehicle state vector output at the previous moment.

4. The method according to claim 1, characterized in that: The real-time acquisition of the vehicle's wheel speed pulse sensor data and steering wheel angle data, and calculation of the front wheel travel distance, rear axle center speed and vehicle angular velocity, includes: The wheel speed pulse sensor data and the steering wheel angle data are read from the vehicle chassis in real time, and the wheel speed pulses are converted into wheel speeds. The front wheel travel distance is calculated according to the wheel speeds of the left front wheel and the right front wheel, and the rear axle center speed is calculated according to the speed of the vehicle's rear wheels; the vehicle's front wheel center yaw angle is calculated based on the steering wheel angle data, and the vehicle angular velocity is calculated based on the rear axle center speed and the vehicle's front wheel center yaw angle.

5. The method according to claim 1, characterized in that The calculation of the observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity includes: An observation equation is established according to the observed quantity of Kalman filtering, a nonlinear function of the observation equation is determined according to the vehicle kinematic model and the observation equation, and a partial derivative of the nonlinear function of the observation equation is obtained to obtain the observation matrix: Among them, the front wheel travel distance, the vehicle heading angle change rate and the rear axle midpoint speed are used as the observed quantities of the Kalman filter.

6. The method according to claim 1, characterized in that The calculating of the Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix includes calculating the Kalman gain using the following extended Kalman filter formula: Among them, K k represents the Kalman gain at the current moment, P k，k-1 Represents the estimated value of the system error covariance matrix at the current moment, H k Represents the estimated value of the observation matrix at the current moment.

7. The method according to claim 6, characterized in that The vehicle state vector is updated to the optimal estimate using the following formula: in, represents the optimal estimate of the updated vehicle state vector, represents the predicted value of the vehicle state vector at the current moment, K k represents the Kalman gain at the current moment, Z k Represents the observation value of the Kalman filter.

8. The method according to claim 7, characterized in that The updating of the system error covariance matrix includes updating the system error covariance matrix to an optimal estimate value using the following formula: P k =[I-K k H k ]P k，k-1 Among them, P k represents the optimal estimate of the updated system error covariance matrix, K k represents the Kalman gain at the current moment, H k Represents the estimated value of the observation matrix at the current moment, P k，k-1 Represents the estimated value of the system error covariance matrix at the current moment.

9. An intelligent parking dead reckoning device, characterized in that: include: An initialization module is configured to initialize a vehicle state vector, a system error covariance matrix, a system error matrix, and an observation noise matrix of the vehicle at a current moment, and use the initialized vehicle state vector as an initial state of the system; A first calculation module is configured to calculate a predicted value corresponding to the vehicle state vector at a current moment using a vehicle kinematic model according to the vehicle state vector output at a previous moment, and calculate a state transfer matrix based on a kinematic equation of the predicted value; An acquisition module is configured to acquire wheel speed pulse sensor data and steering wheel angle data of the vehicle in real time, and calculate the front wheel travel distance, rear axle center speed and vehicle angular velocity; A second calculation module is configured to calculate an observation matrix based on the front wheel travel distance, the rear axle center speed and the vehicle angular velocity, and calculate a Kalman gain based on the system error covariance matrix, the state transfer matrix and the observation matrix; An updating module is configured to update the vehicle state vector to an optimal estimated value based on the predicted value, the Kalman gain and the observed value of the Kalman filter, and to update the system error covariance matrix; The output module is configured to perform an iterative operation so as to continuously optimize the optimal estimated value of the vehicle state vector and continuously output the estimated results of the position and heading angle of the vehicle in the global coordinate system, and use the estimated results as the dead reckoning results in vehicle parking.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images