Self-parking space attitude estimation method, device and equipment and readable storage medium

Abstract

The invention relates to the field of automatic driving perception and positioning, in particular to a self-parking attitude estimation method, device and equipment and a readable storage medium, and the self-parking attitude estimation method comprises the steps: determining a wheel slip rate according to a wheel collection wheel speed at a current moment and an average wheel speed in a set time period; determining the actual wheel speed at the current moment based on the wheel slip rate and the wheel acquisition wheel speed, and determining a preliminary prediction pose by combining the angular speed; determining Kalman filtering gain according to the wheel slip rate; and determining an estimated pose of the vehicle according to the preliminary prediction pose, the observation pose and the Kalman filtering gain. The problem that a positioning algorithm cannot actively correct a pose error through slippage information due to the fact that a wheel speed signal serves as a speed observation pose in the prior art can be solved.

Description

Technical Field

[0001] This application relates to the field of autonomous driving perception and localization, specifically to a method, apparatus, device, and readable storage medium for vehicle pose estimation. Background Technology

[0002] Vehicle pose estimation is one of the core technologies for achieving Level 3+ autonomous driving. It achieves real-time calculation of vehicle position and attitude by fusing data from multiple sensors, such as IMU, visual odometry, LiDAR, and wheel speed sensors. Existing solutions suffer from the following drawbacks: Traditional dead reckoning is based on the assumption of "no wheel slippage," calculating the vehicle's travel distance using wheel speed sensor signals and inferring its attitude by combining the heading angle. However, on low-traction surfaces, such as wet, snowy, or rapid acceleration / braking scenarios, wheels are prone to slippage, causing wheel speed signals to fail to accurately reflect the vehicle's actual motion. Dead reckoning errors accumulate rapidly, reaching 0.5 to 1 meter within 10 seconds, which cannot meet the requirements for high-precision positioning.

[0003] Wheel slip essentially reflects the contact mechanics characteristics between the vehicle and the ground, such as the coefficient of friction and normal force, and is a key state variable in the vehicle dynamics model. However, in existing technologies, positioning systems only use wheel speed signals as velocity observation poses, which prevents positioning algorithms from actively correcting pose errors using slip information. Summary of the Invention

[0004] This application provides a method, apparatus, device, and readable storage medium for estimating the pose of a vehicle, which can solve the problem in the prior art that only wheel speed signals are used as "velocity observation pose", resulting in the positioning algorithm being unable to actively correct the pose error through slip information.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: On one hand, the present invention provides a method for estimating the pose of a vehicle, the method comprising: The wheel slip ratio is determined based on the wheel speed collected at the current moment and the average wheel speed over a set time period. Based on the wheel slip ratio and the wheel speed collected, the actual wheel speed at the current moment is determined, and combined with the angular velocity, the preliminary predicted pose is determined; The Kalman filter gain is determined based on the wheel slip ratio. Based on the preliminary predicted pose and observed pose, as well as the Kalman filter gain, the estimated pose of the vehicle is determined.

[0006] In some alternative solutions, determining the wheel slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period includes: The initial slip ratio is determined based on the wheel speed collected at the current moment and the average wheel speed over a set time period. The wheel slip ratio at the current moment is determined based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment.

[0007] In some alternative solutions, determining the initial slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period includes: If the wheel speed collected by the wheel is less than a first set threshold, or the average wheel speed is less than a second set threshold, then the initial slip ratio is taken as a first set value that approaches 0. If the wheel speed collected by the wheel is greater than or equal to a first set threshold, and the average wheel speed is greater than or equal to a second set threshold, wherein the first set threshold is less than the second set threshold, then the initial slip ratio is determined based on the wheel speed collected by the wheel and the average wheel speed.

[0008] In some alternative solutions, determining the wheel slip ratio at the current moment based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment includes: Based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment, determine the time constant of the first-order hysteresis filter; Based on the initial slip ratio and time constant at the current moment, the slip ratio is updated through a first-order hysteresis filter to obtain the wheel slip ratio at the current moment.

[0009] In some alternative schemes, the discretization formula for the first-order hysteresis filter is: ,in, The wheel slip ratio at the current moment. The wheel slip ratio at the previous moment. τ Δ is the time constant. t The sampling period.

[0010] In some alternative schemes, determining the time constant based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment includes: If the initial slip ratio at the current moment is greater than the wheel slip ratio at the previous moment, then the time constant is taken as the first time constant value; If the initial slip ratio at the current moment is less than or equal to the wheel slip ratio at the previous moment, then the time constant is taken as the second time constant value, where the first time constant value is less than the second time constant value.

[0011] In some alternative solutions, determining the Kalman filter gain based on the wheel slip ratio includes: Based on the wheel slip ratio, determine the observation noise covariance matrix; The Kalman filter gain is determined based on the observed noise covariance matrix.

[0012] In a second aspect, the present invention also provides a vehicle pose estimation device, the vehicle pose estimation device comprising: The slip ratio determination module is used to determine the wheel slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period. The pose prediction module is used to determine the actual wheel speed at the current moment based on the wheel slip ratio and the wheel speed collected by the wheel, and to determine the preliminary predicted pose by combining the angular velocity. A gain determination module is used to determine the Kalman filter gain based on the wheel slip ratio; The pose estimation module is used to determine the estimated pose of the vehicle based on the preliminary predicted pose, the observed pose, and the Kalman filter gain.

[0013] Thirdly, the present invention provides a vehicle pose estimation device, the vehicle pose estimation device including a processor, a memory, and a vehicle pose estimation program stored in the memory and executable by the processor, wherein when the vehicle pose estimation program is executed by the processor, it implements the steps of the vehicle pose estimation method as described in any of the preceding claims.

[0014] Fourthly, the present invention provides a computer-readable storage medium storing a vehicle pose estimation program, wherein when the vehicle pose estimation program is executed by a processor, it implements the steps of the vehicle pose estimation method as described in any of the preceding claims.

[0015] Compared with existing technologies, the advantages of this invention are as follows: By collecting wheel speeds at the current moment and the average wheel speed over a set time period, the wheel slip ratio is determined, wheel slip features are accurately extracted, and the actual wheel speed at the current moment is determined based on the wheel slip ratio and the collected wheel speeds. Combined with angular velocity, a preliminary predicted pose is determined, and the Kalman filter gain is determined based on the wheel slip ratio. Based on the preliminary predicted pose, the observed pose, and the Kalman filter gain, the estimated pose of the vehicle is determined. By detecting the wheel slip ratio in real time and dynamically correcting the actual wheel speed and adjusting the Kalman filter gain, the vehicle pose estimation system can adapt to the wheel slip state: in slip scenarios, the weight contribution is automatically adjusted, thereby effectively suppressing the rapid accumulation of dead reckoning errors and significantly improving the positioning accuracy, robustness, and system reliability under complex working conditions. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a flowchart of the vehicle pose estimation method in an embodiment of the present invention; Figure 2 This is a flowchart illustrating the determination of wheel slip ratio in an embodiment of the present invention; Figure 3 This is a schematic diagram of the sliding state change in an embodiment of the present invention; Figure 4 This is a block diagram of the vehicle position estimation device in an embodiment of the present invention. Detailed Implementation

[0018] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present application.

[0019] First, some of the technical terms used in this application will be explained to help those skilled in the art understand this application.

[0020] Current pose estimation systems for autonomous vehicles generally employ dead reckoning, which relies on wheel speed sensor data to directly calculate the vehicle's motion state. This technology is based on the assumption of no wheel slippage, meaning that wheel speed is strictly proportional to the vehicle's actual speed, and dynamically updates the pose by integrating wheel speed signals. This approach performs stably on normal dry roads and, because it does not require external positioning signals such as GPS, possesses high real-time performance, thus being widely used in vehicle positioning systems.

[0021] However, this method has a fundamental flaw under complex operating conditions: when a vehicle enters a low-traction surface or encounters a rapid acceleration / braking scenario, the wheels are prone to slippage, leading to distortion of wheel speed sensor data. In this situation, the assumption of no wheel slippage completely fails, and the dead reckoning model accumulates errors due to input data deviations. Real-world data shows that in slippage scenarios, positioning errors can rapidly accumulate to 0.5–1 meter within 10 seconds, severely weakening the system's reliability in scenarios such as tunnel obstruction and GPS signal loss, and even posing safety risks.

[0022] To address the aforementioned problems, a solution is urgently needed that can dynamically sense wheel slippage and adaptively correct the positioning model. This invention detects wheel slippage rate in real time and uses it as a key constraint input for vehicle pose estimation, breaking through the reliance of traditional dead reckoning on the no-slip assumption. This significantly suppresses error accumulation under complex conditions such as low-adhesion surfaces, improving positioning accuracy and system robustness.

[0023] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0024] like Figure 1 As shown, in one aspect, the present invention provides a method for estimating the pose of a vehicle, the method comprising: S1: Determine the wheel slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period.

[0025] To dynamically detect wheel slippage, such as spinning or locking, the slip ratio is used as a key constraint input to the vehicle's pose estimation system. This corrects for the error accumulation problem in the dead reckoning model under complex conditions such as low-adhesion surfaces, improving positioning accuracy and reliability. In this example, the wheel slip ratio is calculated by comparing the difference between the current wheel speed and the average wheel speed over a set time period in real time.

[0026] like Figure 2 As shown, step S1 specifically includes: S11: Determine the initial slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period.

[0027] In this example, the initial slip ratio is determined by comparing the wheel speed at the current moment with the average wheel speed over a set time period with the corresponding set threshold.

[0028] Preferably, step S11 includes the following steps: S111: If the wheel speed collected is less than the first set threshold, or the average wheel speed is less than the second set threshold, then the initial slip ratio is taken as the first set value that approaches 0.

[0029] In this example, the wheel speed is acquired through a wheel speed sensor. The average wheel speed is the average value of the wheel speeds collected over a set time period, and the absolute value is taken to ensure that it is non-negative, thus avoiding sign errors caused by sensor signal jumps.

[0030] In this example, the set time period is 40ms. The first set threshold is 0.5m / s, and the second set threshold is 1m / s.

[0031] If the wheel speed is less than the first set threshold, or the average wheel speed is less than the second set threshold, the wheel is considered not to have entered an effective rolling state. Therefore, the initial slip ratio is set to a first set value close to 0. In this example, the first set value is 0.01.

[0032] In this example, the wheel speed is collected when it is less than the first set threshold, or the average wheel speed is less than the second set threshold. At low speeds / stationary conditions (such as vehicle speed < 1 m / s), the slip ratio has no practical dynamic significance, so it is forcibly set to 0.01 (a small value close to 0) to suppress noise interference.

[0033] S112: If the wheel speed collected by the wheel is greater than or equal to the first set threshold and the average wheel speed is greater than or equal to the second set threshold, wherein the first set threshold is less than the second set threshold, the initial slip ratio is determined based on the wheel speed collected by the wheel and the average wheel speed.

[0034] In this example, if the wheel speed collected by the wheel is greater than or equal to the first preset threshold, and the average wheel speed is greater than or equal to the second preset threshold, then the wheel is considered to be in a normal rolling state. The formula is facRawinput = |ref - wheel speed collected by the wheel| / ref, where facRawinput is the initial slip ratio, ref is the average wheel speed, and ref - wheel speed collected by the wheel is the absolute value. During normal rolling, the initial slip ratio is defined as the ratio of "average wheel speed / wheel speed collected by the wheel" to "average wheel speed". The denominator is ref to avoid the unstable value of the denominator approaching 0 when the wheel locks up.

[0035] In this example, by performing the above step S11 on each wheel of the vehicle, the initial slip ratio of each wheel can be obtained.

[0036] S12: Determine the wheel slip ratio at the current moment based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment.

[0037] In this scheme, based on the initial slip ratio of each wheel, the wheel slip ratio of each wheel at the current moment is determined by an adaptive first-order hysteresis filtering algorithm (Pt1).

[0038] Preferably, step S12 includes: S121: Determine the time constant of the first-order hysteresis filter based on the initial slip ratio at the current moment and the wheel slip ratio at the previous moment.

[0039] This operation dynamically compares the current absolute value of the slip ratio with the magnitude of the filter output at the previous moment, and adaptively adjusts the time constant of the first-order hysteresis filter. This allows the filter to optimize its response characteristics in real time according to the rate of change of the slip state: it quickly tracks sudden changes in slip ratio when slipping is severe, avoiding hysteresis errors; and it smooths out noise interference when slipping is stable. This improves the real-time performance and accuracy of slip ratio estimation, providing a more reliable dynamic constraint input for the autonomous vehicle pose estimation system.

[0040] Specifically, step S121 includes: If the initial slip ratio at the current moment is greater than the wheel slip ratio at the previous moment, then the time constant is taken as the first time constant value.

[0041] In this example, if the initial slip ratio at the current moment is greater than the wheel slip ratio at the previous moment, it is considered that the slip ratio is rapidly increasing, such as during emergency braking and wheel lock-up. At this time, the first constant value τ is τ=0.02. If the initial slip ratio at the current moment is less than or equal to the wheel slip ratio at the previous moment, then the time constant is taken as the second time constant value, where the first time constant value is less than the second time constant value.

[0042] If the initial slip ratio at the current moment is less than or equal to the wheel slip ratio at the previous moment, it is considered to be in the stage of slow slip ratio decrease, such as after ABS adjustment. At this time, the second time constant value τ is τ=0.1.

[0043] S122: Based on the initial slip ratio and time constant at the current moment, the slip ratio is updated through a first-order hysteresis filter to obtain the wheel slip ratio at the current moment.

[0044] This operation dynamically smooths the initial slip ratio using a first-order hysteresis filter. Utilizing an adaptive time constant, it adjusts the filter response characteristics in real time based on the slip ratio's changing trend. This optimizes the filter response characteristics: during drastic slip changes, such as rapid acceleration / braking, it quickly tracks sudden changes to avoid hysteresis errors; and during stable slip, it effectively suppresses noise interference, thereby generating a smoother and more reliable wheel slip ratio at the current moment. This serves as a key constraint input for the autonomous vehicle's pose estimation system, directly suppressing the accumulation of errors in dead reckoning under slip scenarios such as low-adhesion surfaces, thus improving positioning accuracy and robustness.

[0045] Prior to this example, the discretization formula for the first-order hysteresis filter is: ,in, The wheel slip ratio at the current moment. Let τ be the wheel slip ratio at the previous moment, τ be the time constant, and Δt be the sampling period, which is 40ms in this example. The initial slip ratio at the current moment is the input value of the initial slip ratio calculated above, which is the unfiltered initial slip ratio. The filter gain is fixed at 1.0 to ensure that the amplitude is not attenuated.

[0046] like Figure 3 As shown, this graph represents 10 seconds of vehicle data, comprising three phases: normal driving (0-3 seconds), slippage (3-7 seconds), and recovery (7-10 seconds). In the normal driving phase (0-3 seconds): the slippage rate is low (<0.1), and no slippage detection is performed. In the slippage phase (3-7 seconds): the slippage rate increases (0.3), and acceleration abruptly changes (>1.5 m / s²), triggering slippage detection. In the recovery phase (7-10 seconds): the slippage rate decreases (0.05), acceleration stabilizes (<0.5 m / s²), and the slippage state ends.

[0047] S2: Based on the wheel slip ratio and the wheel speed collected by the wheel, determine the actual wheel speed at the current moment, and combine it with the angular velocity to determine the preliminary predicted pose.

[0048] In this example, the speed calculation based on dead reckoning is precisely corrected using wheel slip ratio. Specifically, the formula for calculating the actual speed is: vactual = vwheel speed × (1 / 2)2 In this model, *vwheel speed* represents the wheel speed acquired by the wheel speed sensor, *facRaw* is the wheel slip ratio (ranging from 0 to 1), and *vactual* is the corrected actual wheel speed. When *facRaw* = 0, it indicates no wheel slippage, and the actual speed equals the wheel speed; when *facRaw* > 0, it indicates wheel slippage, and the actual speed needs to be proportionally reduced. This model, by introducing the wheel slip ratio, effectively compensates for the distortion of wheel speed signals in slipping scenarios, providing a more reliable speed input for pose estimation.

[0049] In this example, the current pose state (position x, y and heading angle θ) is predicted based on historical pose and motion models (such as wheel speed and IMU data). Displacement increment via formula The calculation is performed, where Δt is the time step; the heading angle increment is calculated using Δθ = ω. z Calculate ×Δt, ω z The data is angular velocity. The preliminary pose prediction formula is as follows:

[0050] Among them, displacement increment Δt is the sampling period, and the actual wheel speed at the current moment. , Collect wheel speed for the current moment. The wheel slip ratio at the current moment. Let x be the x-coordinate at time k. Let k be the heading angle at time k. This is the heading angle increment.

[0051] S3: Adjust the weights corresponding to the preliminary predicted pose and the observed pose based on the wheel slip ratio.

[0052] S31: Determine the observation noise covariance matrix based on the wheel slip ratio.

[0053] In this example, the observation noise covariance matrix is: ,in, =σvbase²·(1+α·facRaw), For the velocity noise variance, The variance of the yaw rate noise. Let σvbase² be the variance of acceleration noise, and σvbase² be the variance of basic velocity.

[0054] S32: Determine the Kalman filter gain K based on the observed noise covariance matrix.

[0055] The observation noise covariance matrix Rk determines the "reliability" of each observation at this stage: the larger the variance value (such as σv²) in Rk, the greater the noise in the corresponding observation data, and the lower its weight in the fusion. The observation noise covariance matrix Rk determines the reliability of the observation: the diagonal elements of Rk, velocity noise variance σv², yaw rate noise variance σωz², and acceleration noise variance σa², are the variances of each observation noise—the larger the variance, the higher the noise in the observation data, and the lower its weight in the fusion.

[0056] In this example, step S32 specifically includes: According to the formula: Determine the Kalman filter gain ,in, To predict the covariance matrix, representing the uncertainty of the state estimation, it is updated from the prediction step. The initial value is a diagonal matrix, and H is the observation matrix, which represents mapping the state vector (position, velocity, heading) to the observation space (yaw rate, acceleration). The observation noise covariance matrix, where T is the transpose.

[0057] As the observation noise covariance increases and the Kalman gain K decreases, the contribution of the observation to the pose correction weakens.

[0058] When the slip ratio facRaw exceeds a threshold, such as 0.3, the velocity observation variance is increased to reduce its reliability.

[0059] In this example: σv²=0.1m² / s², σωz²=0.05rad² / s², σa 2 =0.2m 2 / s4 .

[0060] Dynamic adjustment formula: σv²=σvbase²·(1+α·facRaw), where σvbase²=0.1m² / s² base variance, α=2.0 gain coefficient, and facRaw is the wheel slip ratio at the current moment.

[0061] Adjustment effect: If facRaw=0.5 and α=2.0, then σv²=2.0×σvbase², the velocity observation weight is halved, and the attitude correction depends more on the yaw rate, verifying the heading and acceleration.

[0062] The slip anomaly protection mechanism monitors the slip rate in real time and triggers a dynamic response to ensure positioning safety. When the slip rate is detected to continuously exceed the safety threshold, such as facRaw > 0.3, indicating severe slip, the system immediately performs the following protective actions: Reduce the weight of dead reckoning, for example, from 0.7 to 0.3, while increasing the weight of observation and positioning results to reduce the impact of slip error.

[0063] Send a "low traction road surface warning" to the decision-making level, suggesting a switch to a more conservative control strategy, such as limiting maximum acceleration or activating stability control.

[0064] This mechanism ensures the failure tolerance of pose estimation under extreme conditions through multi-sensor weight adjustment and decision interaction.

[0065] S4: Determine the estimated pose of the vehicle based on the preliminary predicted pose and observed pose, as well as the Kalman filter gain.

[0066] In this example, yaw rate and acceleration sensor data are used as observations to correct the initial predicted pose. Specifically, the correction is calculated by comparing the initial predicted pose with the observed pose and combining the Kalman filter gain (K), and the optimal pose is output.

[0067] Specifically, based on the estimated pose (k+1) =Preliminary pose prediction (k+1) +K·(Observation pose) k -H·Preliminary Pose Prediction (k+1) ).

[0068] The observation matrix H is:

[0069] Preliminary pose prediction The information has already been obtained in step S2, so it will not be repeated here.

[0070] Kalman filter gain is .

[0071] The observations are actually collected at time k and are used to correct the prediction at time k+1. The observed pose is obtained through sensors, such as GPS data, to calculate the position and heading angle at time k.

[0072] Through the coordinated efforts of the above steps, the effective fusion and application of skid information is achieved, which not only solves the error accumulation problem of traditional dead reckoning but also provides autonomous driving systems with adaptive and high-precision pose estimation capabilities. The entire design emphasizes real-time performance and robustness, and can be widely applied to L3+ level autonomous driving scenarios.

[0073] Send a "low traction road surface warning" to the decision-making level, suggesting a switch to a more conservative control strategy, such as limiting maximum acceleration.

[0074] In summary, this invention determines the wheel slip ratio by collecting the wheel speed at the current moment and the average wheel speed over a set time period, accurately extracting wheel slip features. Based on the wheel slip ratio and the collected wheel speed, the actual wheel speed at the current moment is determined, and combined with the angular velocity, a preliminary predicted pose is determined. The Kalman filter gain is determined based on the wheel slip ratio. Finally, the estimated vehicle pose is determined based on the preliminary predicted pose, the observed pose, and the Kalman filter gain. By real-time detection of the wheel slip ratio and dynamic correction of the actual wheel speed and adjustment of the Kalman filter gain, the vehicle pose estimation system can adapt to wheel slip states: in slip scenarios, it automatically adjusts the weight contributions of each sensor, thereby effectively suppressing the rapid accumulation of dead reckoning errors and significantly improving positioning accuracy, robustness, and system reliability under complex conditions.

[0075] Secondly, the present invention also provides a vehicle pose estimation device, comprising: a slip ratio determination module, a preliminary pose prediction module, a gain determination module, and a pose estimation module. The slip ratio determination module determines the wheel slip ratio based on the wheel speed collected at the current moment and the average wheel speed over a set time period; the preliminary pose prediction module determines the actual wheel speed at the current moment based on the wheel slip ratio and the collected wheel speed, and determines the preliminary predicted pose by combining the wheel slip ratio and the wheel speed; the gain determination module determines the Kalman filter gain based on the wheel slip ratio; and the pose estimation module determines the estimated pose of the vehicle based on the preliminary predicted pose, the observed pose, and the Kalman filter gain.

[0076] This solution determines the wheel slip ratio by collecting the wheel speed at the current moment and the average wheel speed over a set time period, accurately extracting wheel slip features. Based on the wheel slip ratio and the collected wheel speed, the actual wheel speed at the current moment is determined, and combined with the angular velocity, a preliminary predicted pose is determined. By detecting the wheel slip ratio in real time and dynamically correcting the actual wheel speed and adjusting the Kalman filter gain, the vehicle pose estimation system can adapt to the wheel slip state. In slip scenarios, it automatically adjusts the weight contribution, thereby effectively suppressing the rapid accumulation of dead reckoning errors and significantly improving the positioning accuracy, robustness, and system reliability under complex working conditions.

[0077] The functions of each module in the above-mentioned autonomous vehicle pose estimation device correspond to the steps in the above-mentioned autonomous vehicle pose estimation method embodiment, and their functions and implementation processes will not be described in detail here.

[0078] Thirdly, embodiments of this application provide a vehicle pose estimation device, which can be a personal computer (PC), laptop computer, server, or other device with data processing capabilities.

[0079] Reference Figure 4 , Figure 4 This is a schematic diagram of the hardware structure of the vehicle pose estimation device involved in the embodiments of this application. In the embodiments of this application, the vehicle pose estimation device may include a processor, a memory, a communication interface, and a communication bus.

[0080] The communication bus can be of any type and is used to interconnect the processor, memory, and communication interface.

[0081] The communication interface includes input / output (I / O) interfaces, physical interfaces, and logical interfaces used for interconnecting internal components of the autonomous vehicle pose estimation device, as well as interfaces used for interconnecting the autonomous vehicle pose estimation device with other devices (such as other computing devices or user equipment). Physical interfaces can be Ethernet interfaces, fiber optic interfaces, ATM interfaces, etc.; user equipment can be displays, keyboards, etc.

[0082] Memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0083] The processor can be a general-purpose processor, which can call the vehicle pose estimation program stored in the memory and execute the vehicle pose estimation method provided in the embodiments of this application. For example, the general-purpose processor can be a central processing unit (CPU). The method executed when the vehicle pose estimation program is called can be referred to in the various embodiments of the vehicle pose estimation method of this application, and will not be repeated here.

[0084] Those skilled in the art will understand that Figure 4 The hardware structure shown does not constitute a limitation of this application and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0085] Fourthly, embodiments of this application also provide a computer-readable storage medium.

[0086] The present application provides a computer-readable storage medium storing a vehicle pose estimation program, wherein when the vehicle pose estimation program is executed by a processor, it implements the steps of the vehicle pose estimation method described above.

[0087] The method implemented when the vehicle pose estimation program is executed can be referred to in various embodiments of the vehicle pose estimation method of this application, and will not be repeated here.

[0088] It should be noted that the sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0089] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus. The terms "first," "second," and "third," etc., are used to distinguish different objects, etc., and do not indicate a sequence, nor do they limit "first," "second," and "third" to different types.

[0090] In the description of the embodiments of this application, terms such as "exemplary," "for example," or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "exemplary," "for example," or "for instance" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of terms such as "exemplary," "for example," or "for instance" is intended to present the relevant concepts in a concrete manner.

[0091] In the description of the embodiments of this application, unless otherwise stated, " / " means "or". For example, A / B can mean A or B. The "and / or" in the text is merely a description of the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist simultaneously, and B exists alone. In addition, in the description of the embodiments of this application, "multiple" means two or more.

[0092] In some processes described in the embodiments of this application, multiple operations or steps are included in a specific order. However, it should be understood that these operations or steps may not be executed in the order they appear in the embodiments of this application, or they may be executed in parallel. The sequence number of the operation is only used to distinguish different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed sequentially or in parallel, and these operations or steps may be combined.

[0093] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device to execute the methods described in the various embodiments of this application.

[0094] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for estimating the pose of a vehicle, characterized in that, The method comprises: determining a wheel slip ratio according to a wheel collected speed at a current time and an average wheel speed in a set time period; determining an actual wheel speed at the current time based on the wheel slip ratio and the wheel collected speed, and determining a preliminary predicted pose in combination with an angular velocity; determining a Kalman filter gain according to the wheel slip ratio; determining an estimated pose of the vehicle according to the preliminary predicted pose and an observed pose, and the Kalman filter gain.

2. The ego vehicle pose estimation method of claim 1, wherein: The method comprises: determining an initial slip ratio according to a wheel collected speed at a current time and an average wheel speed in a set time period; determining a wheel slip ratio at the current time based on the initial slip ratio at the current time and a wheel slip ratio at a previous time.

3. The ego vehicle pose estimation method of claim 2, wherein, The method comprises: if the wheel collected speed is less than a first set threshold value or the average wheel speed is less than a second set threshold value, the initial slip ratio takes a first set value which tends to 0; if the wheel collected speed is greater than or equal to the first set threshold value and the average wheel speed is greater than or equal to the second set threshold value, wherein the first set threshold value is less than the second set threshold value, the initial slip ratio is determined according to the wheel collected speed and the average wheel speed.

4. The ego vehicle pose estimation method of claim 2, wherein, The method comprises: determining a time constant of a first-order lag filter based on the initial slip ratio at the current time and the wheel slip ratio at the previous time; updating the slip ratio through the first-order lag filter based on the initial slip ratio at the current time and the time constant to obtain the wheel slip ratio at the current time.

5. The ego vehicle pose estimation method of claim 4, wherein, The discrete formula of the first-order lag filter is: wherein, is the wheel slip ratio at the current time, is the wheel slip ratio at the previous time, The method comprises: is a time constant, Δ t is a sampling period.

6. The ego vehicle pose estimation method of claim 5, wherein, if the initial slip ratio at the current time is greater than the wheel slip ratio at the previous time, the time constant takes a first time constant value; if the initial slip ratio at the current time is less than or equal to the wheel slip ratio at the previous time, the time constant takes a second time constant value, wherein the first time constant value is less than the second time constant value. The method comprises:

7. The ego vehicle pose estimation method of claim 1, wherein, determining an observation noise covariance matrix according to the wheel slip ratio; determining the Kalman filter gain according to the observation noise covariance matrix. The self-vehicle pose estimation device comprises:

8. A self-vehicle pose estimation device, characterized by comprising: a slip ratio determination module configured to determine a wheel slip ratio according to a wheel collected speed at a current time and an average wheel speed in a set time period; a pose prediction module configured to determine an actual wheel speed at the current time based on the wheel slip ratio and the wheel collected speed, and determine a preliminary predicted pose in combination with an angular velocity; a gain determination module configured to determine a Kalman filter gain according to the wheel slip ratio; a pose estimation module configured to determine an estimated pose of the vehicle according to the preliminary predicted pose and an observed pose, and the Kalman filter gain. ​ 9. A self vehicle pose estimation apparatus characterized by comprising: The ego vehicle pose estimation device includes a processor, a memory, and an ego vehicle pose estimation program stored on the memory and executable by the processor, wherein the ego vehicle pose estimation program, when executed by the processor, implements the steps of the ego vehicle pose estimation method of any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium has stored thereon an ego vehicle pose estimation program, wherein the ego vehicle pose estimation program, when executed by a processor, implements the steps of the ego vehicle pose estimation method of any one of claims 1 to 7.

Images