Vehicle positioning method and device, self-moving robot and storage medium

Abstract

The embodiment of the invention discloses a vehicle positioning method and device, a self-moving robot and a storage medium. The method comprises the steps of obtaining a predicted speed and an observed speed of a target vehicle at a current moment, and a first vehicle pose parameter of the target vehicle at a previous moment; the first vehicle pose parameter is used for reflecting the pose of the target vehicle at the previous moment; determining target observation noise of the target vehicle at the current moment according to the predicted speed and the observation speed; the target observation noise is used for representing the confidence of the observation speed; according to the target observation noise, state updating is carried out on the first vehicle pose parameter, and a target vehicle pose parameter of the target vehicle at the current moment is determined; the target vehicle pose parameter is used for reflecting the pose of the target vehicle at the current moment. According to the embodiment of the invention, when the vehicle is positioned, the positioning precision can be improved.

Description

Technical Field

[0001] This invention relates to the field of vehicle technology, and more particularly to a vehicle positioning method, device, self-moving robot, and storage medium. Background Technology

[0002] Because single sensors are prone to positioning failures in scenarios with obstructions or few positioning features, current vehicle positioning technologies widely adopt multi-sensor fusion schemes, mainly combining navigation systems, inertial measurement units, wheel speed sensors, etc.

[0003] However, due to environmental factors, such as changes in road surface friction that may cause vehicles to skid, there is often an accumulation of positioning errors and mileage statistics errors when performing multi-sensor data fusion, which results in low positioning accuracy. Summary of the Invention

[0004] To address the aforementioned problems, embodiments of the present invention provide a vehicle positioning method, apparatus, self-moving robot, and storage medium, which can improve positioning accuracy when locating a vehicle.

[0005] In a first aspect, embodiments of the present invention provide a vehicle positioning method, including: The predicted speed and observed speed of the target vehicle at the current moment are obtained, as well as the first vehicle pose parameters of the target vehicle at the previous moment; the first vehicle pose parameters are used to reflect the pose of the target vehicle at the previous moment. Based on the predicted speed and the observed speed, the target observation noise of the target vehicle at the current moment is determined; the target observation noise is used to represent the confidence level of the observed speed. Based on the target observation noise, the first vehicle pose parameters are updated to determine the target vehicle pose parameters at the current time; the target vehicle pose parameters are used to reflect the pose of the target vehicle at the current time.

[0006] Secondly, embodiments of the present invention provide a vehicle positioning device, the device including an acquisition unit and a processing unit; The acquisition unit is used to acquire the predicted speed and observed speed of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment; the first vehicle pose parameters are used to reflect the pose of the target vehicle at the previous moment. The processing unit is configured to determine the target observation noise of the target vehicle at the current moment based on the predicted speed and the observed speed; the target observation noise is used to represent the confidence level of the observed speed. Based on the target observation noise, the first vehicle pose parameters are updated to determine the target vehicle pose parameters at the current time; the target vehicle pose parameters are used to reflect the pose of the target vehicle at the current time.

[0007] Thirdly, embodiments of the present invention provide a self-moving robot, the self-moving robot including a robot body, a processor and a memory, the processor being connected to the memory, the memory being used to store a computer program, and the processor being used to execute the computer program stored in the memory so that the self-moving robot performs the method as described in the first aspect.

[0008] Fourthly, embodiments of the present invention provide a computer-readable storage medium storing a computer program that is executed by a processor to implement the method described in the first aspect.

[0009] Fifthly, embodiments of this application provide a computer program product, the computer program product including a non-transitory computer-readable storage medium storing a computer program, the computer being operable to perform the method as described in the first aspect.

[0010] Implementing the embodiments of this application has the following beneficial effects: In this embodiment, the predicted and observed velocities of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment, are first obtained. The first vehicle pose parameters reflect the pose of the target vehicle at the previous moment. Then, based on the predicted and observed velocities, the target observation noise of the target vehicle at the current moment is determined. The target observation noise represents the confidence level of the observed velocity; a large target observation noise indicates distrust of the observation data, while a small target observation noise indicates greater trust in the observation data. Finally, based on the target observation noise, the state of the first vehicle pose parameters is updated to determine the target vehicle pose parameters of the target vehicle at the current moment. These target vehicle pose parameters reflect the pose of the target vehicle at the current moment. Thus, the observation confidence level is quantified based on the deviation between the predicted and observed velocities, allowing the target observation noise to reflect the reliability of the current observation data in real time. Different state update operations are performed on the first vehicle pose parameters according to different confidence levels of the observed velocity, making the target vehicle pose parameters at the current moment closer to the true value, reducing errors, and improving positioning accuracy. Attached Figure Description

[0011] To more clearly illustrate the technical solutions in the embodiments of the present invention or the background art, the drawings used in the embodiments of the present invention or the background art will be described below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 This is a schematic diagram of the architecture of a vehicle positioning system provided in an embodiment of this application; Figure 2 This is a flowchart of a vehicle positioning method provided in an embodiment of this application; Figure 3 This is a flowchart of a fusion wheel speed positioning method provided in an embodiment of this application; Figure 4 This is a flowchart of a target weight determination method provided in an embodiment of this application; Figure 5 This is a schematic diagram of IMU data and wheel speed data provided in an embodiment of this application; Figure 6 This is a flowchart of a slippage determination method provided in an embodiment of this application; Figure 7 This is a flowchart of a pose determination method provided in an embodiment of this application; Figure 8 This is a schematic diagram of the structure of a vehicle positioning device provided in an embodiment of this application; Figure 9 This is a schematic diagram of the structure of a self-moving robot provided in an embodiment of this application. Detailed Implementation

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

[0014] The terms "first," "second," "third," and "fourth," etc., used in the specification, claims, and accompanying drawings of this application are used to distinguish different objects, not to describe a specific order. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or modules is not limited to the listed steps or modules, but may optionally include steps or modules not listed, or may optionally include other steps or modules inherent to these processes, methods, products, or devices.

[0015] In this document, the term "embodiment" means that a particular feature, result, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0016] The following describes the relevant content, concepts, technical issues, technical solutions, and beneficial effects involved in the embodiments of this application.

[0017] When there is a large deviation between the predicted and observed values ​​of a vehicle's speed, there are usually two situations: First, wheel slippage causes a significant difference between the observed and predicted values, that is, the observed values ​​deviate; Second, an anomaly in the speed estimation during state estimation causes a significant difference between the predicted and observed values, that is, the predicted values ​​deviate.

[0018] When there is a large difference between the predicted and observed values, using fixed noise parameters cannot adapt to the influence of dynamic environmental factors, such as tire slippage, changes in road surface friction, and filter drift.

[0019] Therefore, this application provides a vehicle positioning method that adaptively determines the target observation noise of the target vehicle at the current moment, thereby reducing the deviation of the positioning result.

[0020] See Figure 1 , Figure 1 This is a schematic diagram of the architecture of a vehicle positioning system provided in an embodiment of this application. Figure 1 As shown, the vehicle positioning system includes an input module, a positioning module, and an output module. The input module includes a wheel speed sensor, an inertial measurement unit (IMU), and a state acquisition unit. The positioning module includes an adaptive noise estimation module and a state estimation module based on a filtering framework. The input module inputs wheel speed data from the wheel speed sensor, angular velocity data from the IMU, and the target vehicle's state at the previous moment from the state acquisition unit to the adaptive noise estimation module and the state estimation module based on the filtering framework. The target vehicle's state at the previous moment is the first vehicle pose parameter. The adaptive noise estimation module determines the target observation noise at the current moment. The state estimation module based on the filtering framework performs state estimation based on the target observation noise and the input data from the input module. The output module outputs the state estimation result from the state estimation module based on the filtering framework, obtaining the target vehicle's state at the current moment, which is the target vehicle pose parameter.

[0021] See Figure 2 , Figure 2 This is a flowchart illustrating a vehicle positioning method provided in an embodiment of this application. The vehicle positioning method provided in this application includes, but is not limited to, the following steps: Step S101: Obtain the predicted speed and observed speed of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment; Among them, the first vehicle pose parameter is used to reflect the pose of the target vehicle at the previous moment; Step S102: Determine the target observation noise of the target vehicle at the current moment based on the predicted speed and the observed speed; Among them, the target observation noise is used to represent the confidence level of the observation velocity; Step S103: Update the state of the first vehicle pose parameters based on the target observation noise to determine the target vehicle pose parameters at the current moment. Among them, the target vehicle pose parameters are used to reflect the pose of the target vehicle at the current moment.

[0022] Specifically, the predicted speed of the target vehicle at the current moment is an estimated speed value at the current moment based on the vehicle motion model and the target vehicle's motion state at the previous moment. The vehicle motion model can be a single-vehicle dynamics model, a uniform motion model, etc., and the target vehicle's motion state at the previous moment can be speed, acceleration, etc.

[0023] Specifically, the observed speed is the current speed data obtained directly or indirectly through the sensors of the target vehicle. These sensors may include wheel speed sensors, Global Navigation Satellite System (GNSS) receivers, Inertial Measurement Units (IMUs), etc. Wheel speed sensors calculate instantaneous speed by detecting the number of wheel rotations and the wheel circumference, while GNSS receivers measure vehicle speed through the Doppler effect.

[0024] Specifically, the pose includes the position and attitude information of the target vehicle in a preset coordinate system. The preset coordinate system can be a geodetic coordinate system, a vehicle body coordinate system, etc. The position information can be represented by the x and y coordinates in the Cartesian coordinate system. The attitude information can include the vehicle heading angle θ, which is the angle between the vehicle's direction of travel and the reference axis of the coordinate system.

[0025] Specifically, target observation noise is used to represent the confidence level of the observed speed. The magnitude of the target observation noise value is negatively correlated with the reliability of the observed speed. The larger the target observation noise value, the greater the influence of environmental interference or sensor error on the observed speed, and the lower the confidence level. The smaller the target observation noise value, the higher the accuracy of the observed speed, and the higher the confidence level. Environmental interference includes wheel speed sensor measurement deviation caused by road slippage and speed measurement error caused by GNSS signal obstruction.

[0026] In one possible embodiment, the residual between the predicted velocity and the observed velocity is first calculated, that is, the absolute value or square value of the difference between the two, which is used to reflect the degree of deviation between the observed velocity and the theoretically calculated velocity. Then, the value of the target observation noise is determined according to the magnitude of the residual. If the residual is greater than a preset threshold, it indicates that the observed velocity deviates significantly from the theoretically calculated result and the observation reliability is low. At this time, the target observation noise is adjusted to a larger value. If the residual is less than or equal to the preset threshold, it indicates that the observed velocity is in good agreement with the theoretically calculated result and the observation reliability is high. At this time, the target observation noise is adjusted to a smaller value.

[0027] Specifically, the state update is a process based on a filtering algorithm that combines target observation noise with the first vehicle pose parameters to correct the target vehicle's pose and obtain the current pose. The target vehicle pose parameters are used to store the target vehicle's pose information at the current moment, and their data structure is consistent with the first vehicle pose parameters, containing the current position and attitude information.

[0028] In a specific embodiment, see Figure 3 , Figure 3 This is a flowchart illustrating a fusion wheel speed positioning method provided in an embodiment of this application. For example... Figure 3 As shown, for each current moment, firstly, the state estimated by the filter at the previous moment, which is the first vehicle pose parameter, is obtained. Then, the current observation speed, which is the observation speed at the current moment, is obtained, and the adaptive noise of the observation speed, which is the target observation noise, is calculated. Finally, the filtering framework is used to calculate the fused state to obtain the target vehicle pose parameter at the current moment.

[0029] In this embodiment, by dynamically determining the target observation noise that matches the observation speed reliability, the problem of unreasonable observation weight allocation under the fixed noise model is avoided, which can improve the accuracy of vehicle pose estimation. At the same time, the vehicle state is adaptively updated based on the target observation noise, which can reduce the accumulation of positioning errors caused by dynamic changes in the environment and ensure the stability and reliability of vehicle positioning.

[0030] Optionally, step S102, determining the target observation noise of the target vehicle at the current moment based on the predicted speed and the observed speed, may include the following steps: Step S201: Obtain the initial observation noise of the target vehicle and the first weight at the previous time step; the first weight is the scaling factor of the initial observation noise of the target vehicle at the previous time step. Step S202: Determine the first distance based on the velocity difference between the predicted velocity and the observed velocity, the first weight, and the initial observation noise; the first distance is used to reflect the degree of deviation of the velocity difference. Step S203: Determine the target weight of the target vehicle at the current moment based on the first distance; Step S204: Determine the target observation noise of the target vehicle at the current moment based on the target weight and the initial observation noise.

[0031] Specifically, the initial observation noise is a preset basic observation noise parameter based on the inherent characteristics of the sensors mounted on the target vehicle, such as the factory-calibrated measurement error and static noise variance. Its value is a fixed constant. The initial observation noise can also be the observation noise of the target vehicle at the previous moment, reflecting the confidence level of the observed speed at the previous moment. For example, the initial observation noise of the wheel speed sensor can be preset to 0.2 m / s. The first weight is a coefficient calculated for the target vehicle at the previous moment and used to scale the initial observation noise. Its value range can be (0, +∞), reflecting the degree to which the confidence level of the observed speed at the previous moment affects the noise adjustment. For example, when the first weight is 1.2, it indicates that the initial observation noise needs to be amplified by 1.2 times to match the observation confidence level at that time. This first weight can be stored in the vehicle data storage module.

[0032] In one possible embodiment, a first distance is determined based on the velocity difference between the predicted velocity and the observed velocity, a first weight, and initial observation noise. The velocity difference between the predicted and observed velocities is used to visually characterize the degree of deviation between them; for example, when the predicted velocity is 30 m / s and the observed velocity is 31.5 m / s, the velocity difference is 1.5 m / s. The first distance is used to quantify the level of deviation of the velocity difference relative to the initial observation noise; a larger first distance indicates a more significant deviation of the current velocity difference relative to the initial observation noise.

[0033] Specifically, the first distance can be calculated using the following formula:

[0034] In the formula, The first distance, For observation speed, To predict speed, As the first weight, This represents the initial observation noise.

[0035] Furthermore, the target weight is a coefficient used to scale the initial observation noise at the current moment. Its value is determined by the first distance and is used to dynamically adapt the reliability of the current observation speed. Unlike the first weight, the target weight corresponds to the noise scaling requirements at the current moment.

[0036] In one possible implementation, the target observation noise of the target vehicle at the current moment is determined based on the target weight and the initial observation noise. Specifically, the target observation noise = target weight × initial observation noise.

[0037] In this embodiment, the first weight of the previous moment is combined with the influence of historical observation confidence, and the first distance is determined based on the current speed difference to quantify the degree of deviation. Then, the target weight and target observation noise are dynamically determined. This avoids the defect that the fixed noise model cannot adapt to the dynamic environment, and overcomes the problem that relying solely on the current data will cause excessive fluctuations in noise adjustment. This improves the accuracy of the target observation noise in adapting to the observation speed confidence, reduces the accumulation of positioning errors, and improves the vehicle positioning accuracy.

[0038] Optionally, step S203, determining the target weight of the target vehicle at the current moment based on the first distance, may include the following steps: Step S301: Obtain the function type of the kernel function; Step S302: Determine candidate weights based on the function type and the first distance; Step S303: If the candidate weights are within the preset weight range, determine the target weight as the first preset weight; Step S304: If the candidate weights are not within the preset weight range, determine whether the target vehicle is skidding; Step S305: If the target vehicle skids, perform the first operation on the first weight and the candidate weights to obtain the target weight; Step S306: If the target vehicle does not skid, perform a second operation on the first weight and the candidate weight to obtain the target weight.

[0039] Specifically, the kernel function type is used to establish the mapping relationship between the first distance and the weights. It can quantify the weight adjustment magnitude corresponding to the first distance through nonlinear mapping. Kernel function types include Huber, Cauchy, and Tukey kernels, among others. Different kernel functions have different tolerances for outliers in the first distance. For example, the Huber kernel uses quadratic mapping for small distances and linear mapping for large distances, while the Cauchy kernel uses a gentle logarithmic mapping for large distances. Pre-defined kernel function types can be called from the pre-configured algorithm library of the target vehicle's vehicle control system. These kernel function types can be pre-selected and stored based on the vehicle's usage scenario (e.g., urban roads, off-road roads), sensor characteristics (e.g., wheel speed sensor accuracy), or current environmental parameters (e.g., road friction coefficient).

[0040] In one possible implementation, the candidate weights are initial candidate values ​​for the weights at the current time, calculated based on the kernel function's function type and the first distance. These values ​​directly reflect the observation velocity reliability adjustment requirements corresponding to the first distance. Specifically, the mathematical expression corresponding to the function type is invoked, the first distance is substituted into the kernel function expression, and the candidate weights are calculated using a preset conversion rule between the kernel function output and the candidate weights.

[0041] In one possible embodiment, the preset weight range is pre-defined to constrain the weight values ​​and prevent excessively large or small weights from causing extreme adjustments to the target observation noise. This range can be determined based on the sensor's maximum and minimum measurement errors, such as a preset weight range of [0.5, 2.0]. The first preset weight is directly used as the benchmark value for the target weight when the candidate weight is within the preset weight range. For example, the first preset weight can be set to 1. If the candidate weight is greater than or equal to the lower limit of the preset weight range and less than or equal to the upper limit of the preset weight range, such as a candidate weight of 1.2 within [0.5, 2.0], then the target weight is directly determined as the first preset weight.

[0042] When the target vehicle skids, there is relative sliding between the target vehicle's wheels and the ground, such as when the wheels lock up during emergency braking or when the wheels spin freely on a flooded road. This will cause a significant deviation between the observed speed measured by the wheel speed sensor and the actual driving speed of the vehicle.

[0043] Specifically, the first operation refers to the weighted calculation used to fuse the first weight and candidate weights when the target vehicle is skidding, in order to reduce the impact of unreliable candidate weights in skidding scenarios and enhance the stability of historical weights. The second operation refers to the weighted calculation used to fuse the first weight and candidate weights when the target vehicle is not skidding, in order to increase the impact of the current candidate weights while retaining the smoothing effect of historical weights and avoiding sudden weight changes.

[0044] If the target vehicle skids, a first operation is performed on the first weight and the candidate weights to obtain the target weight, where the target weight = first weight / candidate weight. If the target vehicle does not skid, a second operation is performed on the first weight and the candidate weights to obtain the target weight, where the target weight = first weight × candidate weight.

[0045] In a specific embodiment, see Figure 4 , Figure 4 This is a flowchart of a target weight determination method provided in an embodiment of this application. Figure 4As shown, the input data includes: prediction speed, IMU data queue, observation speed, initial observation noise, and first weight. First, based on the initial observation noise, first weight, prediction speed, and observation speed, a first distance is calculated. Then, based on the first distance and kernel function, candidate weights are calculated. Depending on the magnitude of the candidate weights, it is determined whether the candidate weights need to be reset. If the candidate weights are close to the target value, the filter is considered to have converged well, and no dynamic noise adjustment is needed; otherwise, dynamic noise adjustment is considered necessary. Specifically, when the candidate weights are within a preset range, the target weight = 1. When the candidate weights are outside the preset range, it is determined whether the vehicle is skidding based on the IMU data queue and observation speed. When the vehicle is skidding, the target weight = first weight / candidate weight. When the vehicle is not skidding, the target weight = first weight × candidate weight. After obtaining the target weight, the target observation noise = target weight × initial observation noise, and the first weight is updated to the target weight, so that the first weight obtained in the next time step is the target weight of the current time step. When a vehicle skids, the observation noise is amplified, resulting in higher uncertainty in the observation data; when the vehicle does not skid, the observation noise is reduced, resulting in lower uncertainty in the observation data.

[0046] In this embodiment, a robust mapping between the first distance and the weight is established through a kernel function. Combined with a preset weight range constraint, the weight is prevented from becoming extreme. At the same time, the historical first weight and the current candidate weight are fused in a differentiated manner for vehicle skidding scenarios. This ensures that the weight adjustment is adaptable to the current observation credibility and suppresses weight fluctuations in abnormal scenarios such as skidding, thereby improving the reliability of the target weight.

[0047] Optionally, step S302, determining candidate weights based on the function type and the first distance, may include the following steps: Step S401: Determine the hyperparameters of the kernel function based on the function type; Step S402: Determine the first derivative of the kernel function; Step S403: Substitute the hyperparameters and the first distance into the first derivative to obtain the candidate weights.

[0048] Specifically, the hyperparameters of a kernel function are preset fixed parameters used to adjust the robustness and mapping sensitivity of the kernel function. Their values ​​are strongly correlated with the function type of the kernel function and directly determine the response characteristics of the kernel function to the first distance. For example, the hyperparameter of the Huber kernel function is the threshold δ, which controls the critical point for switching the kernel function from quadratic mapping to linear mapping; the hyperparameter of the Cauchy kernel function is the scale σ, which controls the tolerance of the kernel function to large distances; and the hyperparameter of the Tukey kernel function is the truncation threshold c, which controls the suppression strength of the kernel function for distances exceeding the threshold.

[0049] Specifically, the first derivative of the kernel function is the function obtained by taking the first mathematical derivative of the kernel function with respect to the first distance d. It is used to convert the deviation of the first distance into a gradient quantization value. Compared to the kernel function itself, its first derivative can more directly reflect the weight adjustment magnitude corresponding to small changes in distance, avoiding abrupt changes in weight calculation.

[0050] In one possible implementation, the prior residual is the error predicted by the motion model, and the observation residual is the error measured by the sensor. The filtering objective is to minimize the weighted sum of squares of the prior and observation residuals. However, this is easily corrupted by outliers such as sudden sensor failures or abrupt changes in residuals caused by environmental interference. Outliers can cause the optimization results to deviate from the true state. To address this issue, the optimization objective can be modified using a kernel function ρ(·). The kernel function maintains an approximately squared growth for normal residuals, ensuring compatibility with the original optimization. It slows down the growth of abnormally large residuals, thereby reducing the weight of outliers and making the optimization results more reliable. Since two optimization problems have the same first derivative with respect to the residuals, their minimum points coincide. For example, for optimization problems using a kernel function... Its first derivative can be expressed as ,in, , For the standard weighted sum of squares problem, Its first derivative can be expressed as To ensure that the minimum points of the two problems are the same, we assume that the gradients are equal, and thus derive... ,in, The candidate weights are...

[0051] In one possible embodiment, candidate weights can be calculated based on the current observation residuals and applied to the noise matrix to adaptively adjust the observation noise, avoid the negative impact of outliers on state estimation, and improve the stability of state estimation.

[0052] In this embodiment, candidate weights can be determined by adapting the hyperparameters to the kernel function type and quantizing the distance deviation characteristics of the first derivative.

[0053] Optionally, in step S304, determining whether the target vehicle is skidding may include the following steps: Step S501: Obtain the wheelbase of the target vehicle, the inertial measurement unit data during the target time period, and the wheel speed at the current moment; the wheel speed includes the left wheel speed and the right wheel speed; Step S502: Determine the average angular velocity of the target vehicle during the target time period based on the data from the inertial measurement unit; Step S503: Based on the average angular velocity, predict the first angular velocity of the target vehicle at the current moment; Step S504: Calculate the second angular velocity of the target vehicle at the current moment based on the wheel track, left wheel speed, and right wheel speed; Step S505: If the difference between the first angular velocity and the second angular velocity is greater than a preset threshold, it is determined that the target vehicle is skidding; Step S506: If the difference between the first angular velocity and the second angular velocity is less than or equal to a preset threshold, it is determined that the target vehicle is not skidding.

[0054] Specifically, the wheelbase is the horizontal distance between the center axes of the left and right drive wheels of the target vehicle, a fixed structural parameter preset at the factory; the inertial measurement unit (IMU) data consists of three-dimensional angular velocity and three-dimensional acceleration data output by the IMU on the target vehicle within the target time period, including roll rate around the x-axis, pitch rate around the y-axis, and yaw rate around the z-axis. The core data is the yaw rate around the z-axis, which is directly related to the vehicle's steering and slippage states; the target time period is the time period corresponding to the preset IMU sliding window, specifically a preset short time window adjacent to the current moment, such as 0.1s to 0.2s, used to collect IMU data with temporal continuity to reduce instantaneous noise interference; the left wheel speed and right wheel speed are the instantaneous linear velocities of the left and right wheels of the vehicle, respectively, measured by wheel speed sensors. The wheel speed sensors calculate the wheel speed by detecting the number of teeth rotating on the wheel and the wheel circumference.

[0055] For example, see Figure 5 , Figure 5 This is a schematic diagram of IMU data and wheel speed data provided in an embodiment of this application. Figure 5 As shown, the data includes the changes in IMU data over time and the changes in wheel speed data over time. The inertial measurement unit data during the target time period can be the IMU data corresponding to the IMU sliding window in the figure, and the wheel speed at the current moment can be the wheel speed data marked in the figure at the current moment.

[0056] Because IMUs are susceptible to instantaneous fluctuations due to vibration and temperature, averaging can reduce these fluctuations. The average angular velocity is the arithmetic mean of the angular velocity data output by the IMU within a target time period, used to smooth out instantaneous noise in the IMU data. Specifically, based on the IMU data, an angular velocity sampling sequence is determined. The sum of the values ​​at all sampling points in the sequence is calculated, and then divided by the number of sampling points to obtain the average angular velocity within the target time period. For example, if the sampling sequence is [0.3 rad / s, 0.32 rad / s, 0.28 rad / s], the average value is 0.3 rad / s.

[0057] Since the target time period is very close to the current time, the change in the vehicle's yaw rate is small within a short period of time. The angular velocity at the current time can be approximated by the average value. Based on the average angular velocity, the first angular velocity of the target vehicle at the current time is predicted. Specifically, the time interval between the target time period and the current time is determined. If the interval meets a preset condition, the calculated average angular velocity is taken as the first angular velocity at the current time.

[0058] When a vehicle is driving normally, the difference in wheel speed between the left and right wheels is caused by yaw motion. The yaw rate is directly proportional to the wheel speed difference and inversely proportional to the wheel track. Based on the wheel track, left wheel speed, and right wheel speed, the second angular velocity of the target vehicle at the current moment is calculated. Specifically, based on the wheel track L and the left wheel speed... Right wheel speed The second angular velocity is calculated using a preset formula. =( - ) / L, where > hour A positive value indicates that the vehicle is turning right. < hour A negative value indicates that the vehicle is turning left; for example, if the wheel track L = 1.6m and the left wheel speed is... =15m / s, right wheel speed At 15.8 m / s, the second angular velocity =(15.8-15) / 1.6=0.5rad / s.

[0059] Furthermore, the preset threshold is a critical value for the difference in angular velocity pre-set based on vehicle driving performance and the normal noise level of the road surface. It is used to distinguish between normal driving and slippage. During normal driving, the first angular velocity measured by the IMU and the second angular velocity calculated from the wheel speed are in good agreement, with a small difference. During slippage, the wheel speed measured by the wheel speed sensor deviates significantly from the actual vehicle speed, causing distortion in the second angular velocity, and the difference between it and the first angular velocity exceeds the threshold. Specifically, the first angular velocity is calculated... With the second angular velocity absolute value of the difference | - The absolute value of the difference is compared with a threshold. If the absolute value of the difference is greater than the preset threshold, the target vehicle is determined to be in a slipping state. If the absolute value of the difference is less than or equal to the preset threshold, it indicates that the wheel speed measured by the wheel speed sensor and the angular velocity measured by the IMU are in good agreement, the wheel speed is not distorted due to slippage, and the target vehicle is determined not to be slipping.

[0060] In a specific embodiment, see Figure 6 , Figure 6This is a flowchart of a slippage determination method provided in an embodiment of this application. Figure 6 As shown, the input data includes the IMU data queue and the current wheel speed, which includes the left wheel speed and the right wheel speed. First, all angular velocity data in the IMU data queue are traversed, and the average value of the angular velocity data is calculated to obtain the average angular velocity of the target vehicle in the target time period. Then, the average angular velocity is transferred from the IMU coordinate system to the rear axle rotation center of the vehicle to obtain the vehicle rotation angular velocity estimated by the IMU, which is the first angular velocity. Based on the speed difference between the left and right wheels of the vehicle and the wheelbase, the vehicle rotation angular velocity is calculated, which is the second angular velocity. Finally, it is determined whether the difference between the vehicle rotation angular velocity estimated by the IMU and the vehicle rotation angular velocity is greater than a given threshold. If yes, the vehicle state is slipping; otherwise, the vehicle state is not slipping.

[0061] In this embodiment, IMU data and wheel speed data are used to avoid the problem of misjudging slippage by a single sensor; at the same time, IMU noise is smoothed by the average value of the target time period and the preset threshold is adapted to the vehicle performance, so as to achieve accurate identification of slippage state.

[0062] Optionally, step S503, predicting the first angular velocity of the target vehicle at the current moment based on the average angular velocity, may include the following steps: Step S601: Determine the vehicle coordinate system corresponding to the target vehicle; Step S602: Determine the coordinate transformation relationship between the vehicle coordinate system and the inertial measurement unit coordinate system corresponding to the average angular velocity; Step S603: Based on the coordinate transformation relationship, transfer the average angular velocity to the vehicle coordinate system to obtain the first angular velocity.

[0063] Specifically, the vehicle coordinate system is a right-handed Cartesian coordinate system used to describe the motion state of the target vehicle. Its origin can be set as the geometric center or key structural point of the vehicle, such as the rear axle rotation center. The x-axis points longitudinally to the front of the vehicle, which is the direction of travel. The y-axis points laterally to the right side of the vehicle. The z-axis is perpendicular to the vehicle chassis and points upward. This coordinate system translates or rotates synchronously with the vehicle's movement and is used to intuitively represent the vehicle's own posture and motion parameters, such as yaw rate and longitudinal velocity.

[0064] Specifically, the inertial measurement unit coordinate system, also known as the IMU coordinate system, is the sensor coordinate system of the inertial measurement unit itself mounted on the target vehicle, with its origin at the geometric center of the IMU hardware. The coordinate transformation relationship is a mathematical relationship used to transform the angular velocity in the IMU coordinate system to the vehicle coordinate system. Specifically, the coordinate transformation relationship corresponds to a rotation matrix, the elements of which are calculated from the Euler angles between the IMU coordinate system and the vehicle coordinate system, used to quantify the axial direction deviation between the two coordinate systems.

[0065] Furthermore, by using coordinate transformation relationships, the average angular velocity in the IMU coordinate system is converted into an equivalent angular velocity in the vehicle coordinate system to obtain the first angular velocity, thereby eliminating the numerical deviation of angular velocity caused by coordinate system mismatch. First, the average angular velocity is represented as a three-dimensional column vector in the IMU coordinate system. Then, according to the vector coordinate system transformation rules, this vector is multiplied on the left by the rotation matrix to obtain the angular velocity vector in the vehicle coordinate system, which is the first angular velocity.

[0066] In this embodiment, the angular velocity deviation caused by the mismatch between the sensor coordinate system and the vehicle motion representation coordinate system is eliminated by the transformation relationship between the vehicle coordinate system and the IMU coordinate system, ensuring that the first angular velocity can accurately reflect the actual motion state of the target vehicle.

[0067] Optionally, step S103, updating the state of the first vehicle pose parameters based on the target observation noise to determine the target vehicle pose parameters at the current moment, may include the following steps: Step S701: Calculate the predicted state value based on the state transition model of the target vehicle and the pose parameters of the first vehicle; Step S702: Calculate the first gain based on the target observation noise; the first gain is used to correct the error of the state prediction value; Step S703: Determine the target vehicle pose parameters based on the state prediction value and the first gain.

[0068] Specifically, the state transition model is a mathematical model used to describe the motion law of the target vehicle from the previous state to the predicted state at the current time. The state prediction value is the estimated value of the vehicle state at the current time, which is calculated by the state transition model based on the first vehicle pose parameters at the previous time. Its data dimension is consistent with the first vehicle pose parameters. If the first vehicle pose parameters include position (x,y), heading angle θ, and linear velocity v, then the state prediction value also includes the prediction results of these four types of parameters.

[0069] Specifically, the first gain is used to quantify the correction weight of the observation information on the state prediction value. Its value is negatively correlated with the target observation noise. The smaller the target observation noise, the larger the first gain, indicating that the observation information is more reliable and the correction magnitude of the state prediction value is larger; conversely, the correction magnitude is smaller.

[0070] Furthermore, the state prediction covariance matrix, used to characterize the uncertainty of the state prediction values, is denoted as... The state covariance matrix from the previous time step is calculated from the process noise matrix of the state transition model. The observation matrix, denoted as H, is used to establish the mapping relationship between state parameters and observation parameters. If the observation parameter is linear velocity v, then H is [0,0,0,1], associating only the linear velocity component in the predicted state value. Specifically, the state covariance matrix from the previous time step is first obtained. The process noise matrix Q of the state transition model is a preset parameter used to reflect the uncertainty of the motion model. The state prediction covariance matrix is ​​then calculated, and the observation matrix H is determined based on the type of observation parameters. For example, if wheel speed observation corresponds to linear velocity, H only assigns a value of 1 to the velocity component in the state, and 0 to the rest. Finally, the state prediction covariance matrix is ​​used as the basis for the calculation. The first gain is obtained by calculating the observation matrix H and the target observation noise R.

[0071] In one possible embodiment, the target vehicle pose parameters are determined based on the state prediction value and a first gain. The target vehicle pose parameters are determined by adjusting the predicted value based on the first gain, balancing the state prediction value with the observation information, and then correcting the error. First, the observation residual is calculated, which is the difference between the current observation value and the observed prediction value based on the state prediction value, reflecting the deviation between the predicted value and the actual observation. Then, the residual is converted into a state correction amount using the first gain, and finally superimposed on the state prediction value to obtain the target vehicle pose parameters. Specifically, the current observation value is first obtained, and the observed prediction value is calculated based on the state prediction value and the observation matrix. Then, the observation residual is calculated, and the first gain is multiplied by the observation residual to obtain the state correction amount. Finally, the state correction amount is superimposed on the state prediction value to obtain the target vehicle pose parameters at the current moment.

[0072] In a specific embodiment, for the motion of the target vehicle in a two-dimensional plane, the pose parameters of the target vehicle can be obtained through a state vector. To express, Specifically:

[0073] Where x and y represent the position of the target vehicle in the global coordinate system, The vehicle's heading angle. Let be the linear velocity in the direction the vehicle is moving.

[0074] The state transition model is as follows:

[0075] in, For time intervals, This is process noise.

[0076] For state transition function Taking the derivative, we obtain the Jacobian matrix. :

[0077] Since the wheel speedometer provides the linear velocity observation of the target vehicle, the observation function is:

[0078] in, To observe noise.

[0079] For the observation function By taking the derivative, we can obtain the observation Jacobian matrix. :

[0080] Furthermore, state estimation includes a prediction phase and an update phase.

[0081] For the prediction phase, state prediction Covariance prediction .in, It is the prior state estimate at time k, which is predicted only by the motion model and does not incorporate the current observation data; It is the kth The posterior state estimate at time 1 is a fusion of k The final state determined after observing the data at time 1. It is the covariance matrix of the prior state at time k, used to describe the uncertainty of the prior state. The larger the matrix element, the higher the uncertainty of the corresponding state dimension. It is the kth The covariance matrix of the posterior state at time 1 is the fusion k The final description of state uncertainty after observation at time 1.

[0082] For the update phase, the first gain Status update Covariance update .in, Target observation noise; It is the posterior state estimate at time k, which is the target vehicle pose parameter after fusing the observation data; It is the observation value at time k, such as the sensor measurement data; These are observational predictions based on prior states; It is the covariance matrix of the posterior state at time k, used to describe the uncertainty of the posterior state. Since it incorporates observation data, the uncertainty is usually reduced. It is an identity matrix.

[0083] In this embodiment, state prediction is achieved through a state transition model, and the first gain is calculated by combining dynamic target observation noise. This avoids the problem of unbalanced observation weight allocation caused by fixed gain, ensures that reliable observations fully correct prediction deviations and unreliable observations reduce interference, effectively improves the accuracy of target vehicle pose parameters, suppresses the accumulation of positioning errors, and ensures the stability and accuracy of vehicle positioning.

[0084] In a specific embodiment, see Figure 7 , Figure 7 This is a flowchart of a pose determination method provided in an embodiment of this application. Figure 7 As shown, the input data includes wheel speed sensor data, IMU data, and the state variables from the previous moment. The wheel speed sensor data includes the speeds of the left and right wheels and the vehicle body. The state variables from the previous moment can include position, attitude, velocity, bias, gravity, etc. The adaptive noise estimation module includes a first distance calculation unit, a weighting function calculation module, a slippage detection module, and a noise dynamic adjustment unit. Through the input data and the adaptive noise estimation module, a weighted noise matrix, i.e., the target observation noise, can be obtained. The state estimation module based on the filtering framework is used to perform state estimation based on the input data and the weighted noise matrix to calculate the latest state variables. Similarly, the latest state variables can also include position, attitude, velocity, bias, gravity, etc. The latest state variables are used to provide position and attitude to the planning and control module, enabling the planning and control module to generate vehicle path planning and motion control commands based on the latest state variables.

[0085] In summary, in this embodiment, the predicted and observed velocities of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment, are first obtained. The first vehicle pose parameters reflect the pose of the target vehicle at the previous moment. Then, based on the predicted and observed velocities, the target observation noise of the target vehicle at the current moment is determined. The target observation noise represents the confidence level of the observed velocity; a large target observation noise indicates distrust of the observation data, while a small target observation noise indicates greater trust in the observation data. Finally, based on the target observation noise, the state of the first vehicle pose parameters is updated to determine the target vehicle pose parameters of the target vehicle at the current moment, which reflect the pose of the target vehicle at the current moment. Thus, the observation confidence level is quantified based on the deviation between the predicted and observed velocities, allowing the target observation noise to reflect the reliability of the current observation data in real time. Different state update operations are performed on the first vehicle state according to different confidence levels of the observed velocity, making the current target vehicle state closer to the true value, reducing errors, and improving positioning accuracy.

[0086] The methods of the embodiments of the present invention have been described in detail above, and the apparatus of the embodiments of the present invention is provided below.

[0087] See Figure 8 , Figure 8 This is a structural schematic diagram of a vehicle positioning device provided in an embodiment of this application. Figure 8 As shown, the vehicle positioning device 800 includes an acquisition unit 801 and a processing unit 802. The acquisition unit 801 is used to acquire the predicted speed and observed speed of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment. The first vehicle pose parameters are used to reflect the pose of the target vehicle at the previous moment. The processing unit 802 is used to determine the target observation noise of the target vehicle at the current moment based on the predicted speed and observed speed. The target observation noise is used to represent the confidence level of the observed speed. Based on the target observation noise, the first vehicle pose parameters are updated to determine the target vehicle pose parameters of the target vehicle at the current moment. The target vehicle pose parameters are used to reflect the pose of the target vehicle at the current moment.

[0088] In specific implementations, the acquisition unit 801 and the processing unit 802 in this application embodiment may also execute other implementation methods described in the vehicle positioning method of this application embodiment, which will not be repeated here.

[0089] See Figure 9 , Figure 9 This is a structural schematic diagram of a self-moving robot provided in an embodiment of this application. For example... Figure 9 As shown, the self-moving robot includes a robot body, which comprises a motion module, a sensor module, and a frame. The motion module enables the robot to move autonomously and can be wheeled, tracked, or legged, providing power and allowing it to move on the ground, in complex terrain, or in specific environments. The sensor module collects the robot's movement parameters, including its pose and speed. The frame integrates and secures the modules, ensuring structural stability and providing physical protection for the internal electronic components.

[0090] The self-propelled robot also includes a transceiver, a processor, and a memory, which are connected via a bus. The memory stores computer programs and data, and can transmit the data stored in the memory to the processor. The processor can be the aforementioned acquisition unit 801 and processing unit 802. In this embodiment, the processor reads the computer program from the memory and executes some or all of the steps of the vehicle positioning method described above.

[0091] This application also provides a computer-readable storage medium storing a computer program that is executed by a processor to implement some or all of the steps of any of the vehicle positioning methods described in the above method embodiments.

[0092] This application also provides a computer program product, which includes a non-transitory computer-readable storage medium storing a computer program operable to cause a computer to perform some or all of the steps of any of the vehicle positioning methods described in the above method embodiments.

[0093] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that this application is not limited to the described order of actions, as some steps may be performed in other orders or simultaneously according to this application. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are all optional embodiments, and the actions and modules involved are not necessarily essential to this application.

[0094] In the above embodiments, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0095] In the several embodiments provided in this application, it should be understood that the disclosed apparatus can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or modules may be electrical or other forms.

[0096] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical modules; that is, they may be located in one place or distributed across multiple network modules. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0097] Furthermore, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module. The integrated modules described above can be implemented in hardware or as software program modules.

[0098] If the integrated module is implemented as a software program module and sold or used as an independent product, it can be stored in a computer-readable storage device (CMD). Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a memory and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned memory includes various media capable of storing program code, such as USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks.

[0099] The embodiments of this application have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of this application. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of this application. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this application. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A vehicle positioning method, characterized in that, include: Obtain the predicted speed and observed speed of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment; The first vehicle pose parameter is used to reflect the pose of the target vehicle at the previous moment; Based on the predicted speed and the observed speed, the target observation noise of the target vehicle at the current moment is determined; The target observation noise is used to represent the confidence level of the observation velocity; Based on the target observation noise, the first vehicle pose parameters are updated to determine the target vehicle pose parameters at the current time; the target vehicle pose parameters are used to reflect the pose of the target vehicle at the current time.

2. The method as described in claim 1, characterized in that, The step of determining the target observation noise of the target vehicle at the current moment based on the predicted speed and the observed speed includes: The initial observation noise of the target vehicle and the first weight at the previous time are obtained; the first weight is the scaling factor of the initial observation noise of the target vehicle at the previous time. A first distance is determined based on the velocity difference between the predicted velocity and the observed velocity, the first weight, and the initial observation noise; the first distance is used to reflect the degree of deviation of the velocity difference. Based on the first distance, determine the target weight of the target vehicle at the current time; The target observation noise of the target vehicle at the current time is determined based on the target weight and the initial observation noise.

3. The method as described in claim 2, characterized in that, Determining the target weight of the target vehicle at the current time based on the first distance includes: Get the function type of the kernel function; Based on the function type and the first distance, determine the candidate weights; If the candidate weights are within a preset weight range, the target weight is determined to be the first preset weight; If the candidate weight is not within the preset weight range, determine whether the target vehicle is skidding; If the target vehicle skids, a first operation is performed on the first weight and the candidate weight to obtain the target weight; If the target vehicle does not skid, a second operation is performed on the first weight and the candidate weight to obtain the target weight.

4. The method as described in claim 3, characterized in that, The step of determining candidate weights based on the function type and the first distance includes: Determine the hyperparameters of the kernel function based on the function type; Determine the first derivative of the kernel function; Substituting the hyperparameters and the first distance into the first derivative yields the candidate weights.

5. The method as described in claim 3, characterized in that, Determining whether the target vehicle is skidding includes: The wheelbase of the target vehicle, the inertial measurement unit data during the target time period, and the wheel speed at the current moment are obtained; the wheel speed includes the left wheel speed and the right wheel speed. Based on the data from the inertial measurement unit, the average angular velocity of the target vehicle during the target time period is determined; Based on the average angular velocity, predict the first angular velocity of the target vehicle at the current moment; Calculate the second angular velocity of the target vehicle at the current moment based on the wheel track, the left wheel speed, and the right wheel speed; If the difference between the first angular velocity and the second angular velocity is greater than a preset threshold, it is determined that the target vehicle is skidding. If the difference between the first angular velocity and the second angular velocity is less than or equal to a preset threshold, it is determined that the target vehicle is not skidding.

6. The method as described in claim 5, characterized in that, The step of predicting the first angular velocity of the target vehicle at the current moment based on the average angular velocity includes: Determine the vehicle coordinate system corresponding to the target vehicle; Determine the coordinate transformation relationship between the vehicle coordinate system and the inertial measurement unit coordinate system corresponding to the average angular velocity; Based on the coordinate transformation relationship, the average angular velocity is transferred to the vehicle coordinate system to obtain the first angular velocity.

7. The method according to any one of claims 1-6, characterized in that, The step of updating the state of the first vehicle pose parameters based on the target observation noise, and determining the target vehicle pose parameters at the current time, includes: Calculate the state prediction value based on the state transition model of the target vehicle and the pose parameters of the first vehicle; Calculate a first gain based on the target observation noise; the first gain is used to correct the error of the state prediction value. The target vehicle pose parameters are determined based on the predicted state value and the first gain.

8. A vehicle positioning device, characterized in that, The device includes an acquisition unit and a processing unit; The acquisition unit is used to acquire the predicted speed and observed speed of the target vehicle at the current moment, as well as the first vehicle pose parameters of the target vehicle at the previous moment; the first vehicle pose parameters are used to reflect the pose of the target vehicle at the previous moment. The processing unit is configured to determine the target observation noise of the target vehicle at the current moment based on the predicted speed and the observed speed. The target observation noise is used to represent the confidence level of the observation velocity; Based on the target observation noise, the first vehicle pose parameters are updated to determine the target vehicle pose parameters at the current time; the target vehicle pose parameters are used to reflect the pose of the target vehicle at the current time.

9. A self-moving robot, characterized in that, include: The robot body comprises a mobile module and a sensor module, the sensor module being used to collect the mobile parameters of the self-moving robot, the processor being connected to the memory, the memory being used to store a computer program, and the processor being used to execute the computer program stored in the memory to cause the self-moving robot to perform the method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, the computer program including program instructions that, when executed by a processor, cause the processor to perform the method as described in any one of claims 1-7.

Images