Kalman filter-based trajectory fusion method, equipment, apparatus, and storage medium

Abstract

This application provides a Kalman filter-based trajectory fusion method, device, apparatus, and storage medium. In the vehicle motion prediction method, vehicle position observation data, vehicle monitoring data, and vehicle sensor data within a first time period are acquired. The vehicle monitoring data and vehicle sensor data are input into a vehicle motion state model to obtain a model-predicted trajectory. Based on the timestamp correspondence, the vehicle position observation data and the model-predicted trajectory are fused using Kalman filtering to obtain a corrected target trajectory. Data fusion based on the Kalman filter algorithm can help improve the accuracy of vehicle motion state, reduce the yaw effect caused by sensor or system errors, provide more accurate vehicle motion state information, and also predict subsequent vehicle motion states based on vehicle position observation data and the vehicle motion state model.

Description

Technical Field

[0001] This application relates to the field of data processing technology, and in particular to a method, device, apparatus and storage medium for trajectory fusion based on Kalman filter. Background Technology

[0002] With the development of intelligent and precise vehicle technologies, not only can the user's driving experience be effectively improved, but the driver's driving difficulty can also be significantly reduced. However, when actually outputting vehicle motion status or trajectory information, problems such as the disappearance of the Global Positioning System (GPS) signal or the low accuracy of the sensors themselves lead to the use of prediction to output the corresponding trajectory. However, the predicted trajectory results are not very accurate. Therefore, a new solution urgently needs to be proposed. Summary of the Invention

[0003] This application provides a Kalman filter-based trajectory fusion method, device, apparatus, and storage medium to accurately determine the distance between a vehicle body and obstacles, thereby reducing the probability of accidents.

[0004] This application provides a vehicle motion prediction method, including:

[0005] Acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period;

[0006] The vehicle monitoring data and the vehicle sensor data are input into the vehicle motion state model to obtain the model's predicted trajectory.

[0007] Based on the correspondence of timestamps, the vehicle position observation data and the model predicted trajectory are fused using Kalman filtering to obtain the corrected target trajectory.

[0008] Optionally, the step of fusing the vehicle position observation data with the model predicted trajectory using Kalman filtering based on the correspondence of timestamps to obtain the corrected target trajectory includes: determining the first timestamp of the corresponding vehicle position observation data based on the sampling frequency of the first timestamp of the model predicted trajectory;

[0009] According to the Kalman filter fusion algorithm, the vehicle position observation data with the same first time stamp are fused with the model predicted trajectory;

[0010] Output the corrected target trajectory corresponding to the trajectory predicted by the model.

[0011] Optionally, the step of fusing the vehicle position observation data with the model predicted trajectory according to the Kalman filter fusion algorithm includes: determining the corresponding map road data based on the vehicle position observation data;

[0012] Based on the first timestamp, the deviation information obtained by fusing the map road data and the model predicted trajectory is determined; the deviation information includes at least one of the following: offset direction, relative distance, and starting offset point;

[0013] The model observation trajectory is corrected based on the deviation information.

[0014] Optionally, after outputting the corrected target trajectory corresponding to the model-predicted trajectory, the method further includes: determining the cumulative deviation information between the map road data and the model-predicted trajectory; and optimizing the parameters in the vehicle motion state model based on the cumulative deviation information.

[0015] Optionally, optimizing the parameters in the vehicle motion state model based on the cumulative deviation information includes: training an error analysis model using the cumulative deviation information; and inputting the error offset output by the error analysis model into the vehicle motion state model as an optimization parameter of the vehicle motion state model.

[0016] Optionally, the step of calculating the model predicted trajectory by inputting the vehicle monitoring data and vehicle sensor data into the vehicle motion state model includes: determining the vehicle state decision rule based on the filtering and noise reduction results of the vehicle position observation data, the vehicle monitoring data, and the vehicle sensor data; and calculating the model predicted trajectory based on the vehicle state decision rule and the vehicle monitoring data and vehicle sensor data.

[0017] Optionally, the step of determining the vehicle state decision rule based on the filtering and noise reduction processing results of the vehicle position observation data, vehicle monitoring data, and vehicle sensor data includes: acquiring the position change amount in the vehicle position observation data, the vehicle's four-wheel wheel speed and vehicle gear in the vehicle monitoring data, and the driving speed and acceleration in the vehicle sensor data; if the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, and vehicle gear, then the vehicle state decision rule is determined to be a normal driving state; if the position variable matches the driving speed and acceleration, but does not match the vehicle's four-wheel wheel speed, then the vehicle state decision rule is determined to be an escape state; if the position variable matches the vehicle's four-wheel wheel speed, driving speed, and acceleration, but does not match the vehicle gear, then the vehicle state decision rule is determined to be a rollaway state.

[0018] This application provides a vehicle device, including: a vehicle body, on which radar and image acquisition equipment are mounted, and a memory, a processor, a communication component, and a display component are also provided; the memory is used to store one or more computer instructions; the processor is used to execute the one or more computer instructions to: perform steps in the Kalman filter-based trajectory fusion method through the communication component and the display component.

[0019] This application provides a Kalman filter-based trajectory fusion device for use in vehicle equipment. The device includes:

[0020] The acquisition module is used to acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period.

[0021] The prediction module is used to input the vehicle monitoring data and the vehicle sensor data into the vehicle motion state model to obtain the model's predicted trajectory.

[0022] The fusion module is used to fuse the vehicle position observation data with the model predicted trajectory using Kalman filtering based on the correspondence of timestamps, so as to obtain the corrected target trajectory.

[0023] This application provides a computer-readable storage medium storing a computer program, which, when executed, can implement the steps in the Kalman filter-based trajectory fusion method.

[0024] The vehicle motion prediction method, device, apparatus, and storage medium provided in this application acquire vehicle position observation data, vehicle monitoring data, and vehicle sensor data within a first time period; input the vehicle monitoring data and vehicle sensor data into a vehicle motion state model to obtain a model-predicted trajectory; and, based on the timestamp correspondence, use Kalman filtering to fuse the vehicle position observation data and the model-predicted trajectory to obtain a corrected target trajectory. Data fusion using the Kalman filtering algorithm helps improve the accuracy of vehicle motion state, reduces the yaw effect caused by sensor or system errors, provides more accurate vehicle motion state information, and can also predict subsequent vehicle motion states based on vehicle position observation data and the vehicle motion state model. Attached Figure Description

[0025] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0026] Figure 1A flowchart illustrating a trajectory fusion method based on Kalman filtering is provided for an exemplary embodiment of this application.

[0027] Figure 2 A schematic diagram of Kalman filter-based data fusion provided for an exemplary embodiment of this application;

[0028] Figure 3 A schematic diagram of the structure of a vehicle device provided in an exemplary embodiment of this application;

[0029] Figure 4 This is a schematic diagram of a Kalman filter-based trajectory fusion device provided as an exemplary embodiment of this application. Detailed Implementation

[0030] To enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.

[0031] In some processes described in the specification, claims, and accompanying drawings of this invention, multiple operations appearing in a specific order are included. These operations may be executed out of order or in parallel. Operation numbers such as 101, 102, etc., are merely used to distinguish different operations and do not represent any execution order. Furthermore, these processes may include more or fewer operations, and these operations may be executed sequentially or in parallel. It should be noted that the terms "first," "second," etc., used herein are used to distinguish different messages, devices, modules, etc., and do not represent a chronological order, nor do they limit "first" and "second" to different types.

[0032] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0033] In existing technologies, vehicle motion status can be predicted by using vehicle monitoring data and sensor data collected from sensors installed on the vehicle. Alternatively, vehicle motion status and trajectory can be predicted using Global Positioning System (GPS) data and high-precision maps. However, the vehicle motion status or trajectory obtained through these methods is inaccurate and cannot meet the requirements for high-precision vehicle monitoring or control, nor can it meet the needs of autonomous driving / control scenarios. In some complex application scenarios, such as when a vehicle passes through a tunnel, signal loss may occur, further leading to inaccurate vehicle motion status or trajectory data. This application provides a vehicle motion prediction method in some embodiments.

[0034] In this application's technical solution, the data sources for predicting vehicle motion mainly consist of three aspects: vehicle position observation data, vehicle monitoring data, and vehicle sensor data. Vehicle position observation data is vehicle positioning data acquired via satellite using systems such as Global Positioning System (GPS) or BeiDou Navigation Satellite System. More specifically, it includes road position data, lane data, lane direction data, road condition data, etc. Vehicle monitoring data is data obtained based on vehicle attributes and status, such as vehicle weight, gear position, wheel speeds, and steering wheel angle. The vehicle sensor data mentioned here is data collected by sensors installed on the vehicle, such as driving speed, acceleration, and yaw rate.

[0035] After acquiring the aforementioned data, it undergoes preprocessing, such as filtering and noise reduction. Then, a pre-trained model is used to predict the vehicle's motion state based on this data. Specifically, after obtaining historical GPS data, a Hidden Markov Model combined with a deep learning model can be used to obtain vehicle position observation data as the observation data. After acquiring vehicle monitoring data and vehicle sensor data, the vehicle motion state model is used to obtain the model's predicted trajectory. The vehicle motion state data obtained based on the above approach is not accurate enough and requires further optimization.

[0036] It should be noted that the vehicle motion state model mentioned here includes the vehicle planar motion model, the vehicle spatial motion model, and the vehicle non-circular motion model.

[0037] The technical solutions provided by the various embodiments of this application are described in detail below with reference to the accompanying drawings.

[0038] Figure 1 A flowchart illustrating a Kalman filter-based trajectory fusion method provided as an exemplary embodiment of this application is shown below. Figure 1 As shown, the method includes:

[0039] 101: Acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period.

[0040] 102: Input the vehicle monitoring data and the vehicle sensor data into the vehicle motion state model to obtain the model's predicted trajectory.

[0041] 103: Based on the correspondence of timestamps, the vehicle position observation data and the model predicted trajectory are fused using Kalman filtering to obtain the corrected target trajectory.

[0042] In practical applications, vehicles are in motion, and due to factors such as the vehicle itself, GPS sensors, or interference from obstacles (e.g., tunnels), vehicle position data may be temporarily unavailable. Therefore, the vehicle's motion status cannot be directly determined in real-time after the inability to collect position data. Consequently, it is necessary to acquire vehicle position data, vehicle monitoring data, and vehicle sensor data from the first time period preceding the second time period. This data forms the basis for predicting the vehicle's motion status in the second time period and subsequent time periods. Specifically:

[0043] After acquiring vehicle monitoring data and vehicle sensor data for the first time period, the vehicle motion state model can be used to calculate the predicted trajectory. However, this predicted trajectory is calculated solely based on the vehicle's own data and requires further optimization and correction. As mentioned earlier, vehicle position observation data can be acquired before the second time period. Therefore, the prediction effect of the vehicle motion state model can be optimized using the vehicle position observation data and the predicted trajectory.

[0044] It should be noted that when fusing vehicle location observation data with model predicted trajectories, the fusion must be performed according to the correspondence of timestamps. For example... Figure 2 This is a schematic diagram illustrating Kalman filter-based data fusion as an exemplary embodiment of this application. Figure 2As can be seen, for example, suppose there are multiple predicted trajectories within one minute, namely A1, A2, A3, and A4, with corresponding timestamps TA1, TA2, TA3, and TA4, and the time interval between these timestamps is 15 seconds. Since the sampling frequency of vehicle position observation data (i.e., GPS data) is relatively high, for example, once per second, the resulting vehicle position observation data within one minute are B1, B2, B3, B4, B5…B15…B30…B45…B60, a total of 60 data points, with corresponding timestamps TB1, TB2, TB3, TB4, TB5…TB15…TB30…TB45…TB60. Therefore, according to the timestamp correspondence, TB1 corresponds to TA1, TB15 to TA2, TB30 to TA3, and TB60 to TA4. During fusion, A1, A2, A3, and A4 are fused with B1, B15, B30, and B60 to output the second motion state data at the second moment using the vehicle motion state model. This second motion state data includes vehicle speed, steering angle, yaw angle, pitch angle, acceleration, and spatial position. Based on this information, the vehicle's position and trajectory information for the next moment can be accurately obtained. Generally, updating the vehicle motion state model with GPS data (vehicle position observation data, etc.) obtained in the first moment or first time period, and repeating the calculation process of the vehicle motion state model, allows for the calculation of the vehicle's first motion state information within the first time period, the predicted second motion state information for the next tens of seconds (i.e., after the second moment), and the vehicle's trajectory after the second moment.

[0045] In one or more embodiments of this application, the vehicle position observation data and the model predicted trajectory are fused using Kalman filtering based on the correspondence of timestamps to obtain a corrected target trajectory. This includes: determining the first timestamp of the vehicle position observation data corresponding to the sampling frequency of the first timestamp of the model predicted trajectory; fusing the vehicle position observation data and the model predicted trajectory using a Kalman filtering fusion algorithm; and outputting the corrected target trajectory corresponding to the model predicted trajectory.

[0046] As mentioned earlier, since the sampling frequency of vehicle position observation data is higher than that of the model predicted trajectory, the vehicle position observation data should be determined based on the first timestamp corresponding to the model predicted trajectory when determining the timestamp. Then, a Kalman filter fusion algorithm is used to fuse the vehicle position observation data and the model predicted trajectory. For example, during fusion, the vehicle position observation data is used as the observation data, and the model predicted trajectory is used as the prediction data. The data fusion formula is as follows:

[0047]

[0048] Where x1 and x2 are the vehicle position observation data and model predicted trajectory to be fused, respectively, and α and β are coefficients. This is the merged data.

[0049] In one or more embodiments of this application, the step of fusing the vehicle position observation data with the model predicted trajectory according to the Kalman filter fusion algorithm includes: determining the corresponding map road data based on the vehicle position observation data; determining the deviation information between the map road data and the model predicted trajectory; the deviation information includes at least one of: offset direction, relative distance, and starting offset point; and correcting the model observation trajectory based on the deviation information.

[0050] In practical applications, vehicles can obtain approximate location information through satellite positioning systems such as the Global Positioning System (GPS). This location information then needs to be matched to roads on a map. However, GPS data is not entirely accurate; although it can match the nearest road, this road may not be entirely accurate, especially in situations with complex road conditions and dense road networks. Therefore, this solution employs a model-based trajectory prediction approach. Since the model-predicted trajectory also contains some errors, further correction is necessary. At this point, map road data can be used to correct the model-predicted trajectory. Specifically, the deviation information between the map road data and the model-predicted trajectory is determined, such as the direction of offset, relative distance, and starting offset point. Then, this deviation information is used to correct the model's observed trajectory, resulting in a more accurate corrected target trajectory.

[0051] In one or more embodiments of this application, after outputting the corrected target trajectory corresponding to the model predicted trajectory, the method further includes: determining the cumulative deviation information between the map road data and the model predicted trajectory; and optimizing the parameters in the vehicle motion state model based on the cumulative deviation information.

[0052] In practical applications, the calculated predicted trajectory accumulates errors over time, necessitating timely correction of the motion state data output by the vehicle motion state model. This can be achieved by calculating cumulative deviation information based on map road data and the model's predicted trajectory. This cumulative deviation information is then input into the vehicle motion state model to compensate and correct subsequent motion state data. This ensures that the vehicle motion state model provides more accurate trajectory predictions in later stages. It's important to note that when correcting the vehicle motion state model, one can either directly compensate by obtaining cumulative deviation information or utilize machine learning models to learn from the cumulative deviation information, enabling dynamic compensation of the vehicle motion state model. Specifically:

[0053] The step of optimizing the parameters in the vehicle motion state model based on the cumulative deviation information includes: training the error analysis model using the cumulative deviation information; and inputting the error offset output by the error analysis model into the vehicle motion state model as the optimization parameter of the vehicle motion state model.

[0054] Furthermore, to achieve better optimization results, offline high-precision map data can be used to optimize the vehicle motion state model. Specifically, the predicted trajectory of the vehicle model is accumulated over a certain period of time, and the cumulative deviation value is determined by comparing the map road data with the model's predicted trajectory. Then, at certain time intervals, the cumulative deviation value is matched with the offline high-precision map road data for data verification and cumulative error checking, and Euclidean distance and directional offset angle calculations are performed to obtain a more accurate vehicle error offset. This error offset is then used to compensate for the vehicle motion state model, resulting in a more accurate corrected target trajectory.

[0055] In one or more embodiments of this application, the step of inputting the vehicle monitoring data and the vehicle sensor data into a vehicle motion state model to obtain a model-predicted trajectory includes: determining a vehicle state decision rule based on the filtering and noise reduction processing results of the vehicle monitoring data and the vehicle sensor data; and calculating the model-predicted trajectory based on the vehicle state decision rule and the vehicle monitoring data and the vehicle sensor data.

[0056] In practical applications, due to environmental factors, normal vehicle vibrations, and sensor accuracy issues, noisy data is inevitable in vehicle monitoring and sensor data. Therefore, after acquiring the initial historical data, Gaussian filtering, bilateral filtering, and noise reduction based on clustering principles can be used. Users can choose appropriate filtering and noise reduction methods according to their specific needs to obtain effective processing results. Then, based on the filtering and noise reduction results, vehicle state decision rules are determined. These rules can include, for example, normal driving state, escape state, rollaway state, turning state, etc. This is because in complex driving environments, relying solely on single vehicle monitoring or sensor data is insufficient for accurate vehicle control.

[0057] For example, if a vehicle suddenly gets stuck in mud while driving normally, its wheels may slip, preventing it from moving forward or causing it to move very slowly. If the fact that the vehicle is stuck in mud cannot be detected in time, and the normal driving power output and drive mode are still used, it will be difficult for the vehicle to get out of the mud. Therefore, based on the collected vehicle monitoring data and vehicle sensor data, vehicle state decision rules can be output. This allows the vehicle to perceive its current state in a timely manner and adopt the corresponding power output mode. This enables more precise vehicle control and provides accurate feedback on the actual situation to the driver, better assisting the driver in driving safely and reliably.

[0058] In one or more embodiments of this application, the step of determining the vehicle state decision rule based on the filtering and noise reduction processing results of the vehicle position observation data, vehicle monitoring data, and vehicle sensor data includes:

[0059] The system acquires the position change data from the vehicle position observation data, the wheel speeds and gear positions of the vehicle's four wheels from the vehicle monitoring data, and the driving speed and acceleration from the vehicle sensor data. Of course, in practical applications, even more data can be acquired to more accurately determine the vehicle's decision-making state rules.

[0060] If the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, and vehicle gear, then the vehicle state decision rule determines it to be in normal driving condition. For example, if the currently detected vehicle's four-wheel wheel speed is basically the same and rotating at a constant speed, the obtained driving speed is a constant speed of 70 kilometers per hour, the acceleration is zero, the position change is approximately 1.16 kilometers per minute, and the gear is 7, then it can be determined that the vehicle is currently in normal driving condition.

[0061] If the position variable matches the driving speed and acceleration, but does not match the wheel speeds of the vehicle's four wheels, then the vehicle state decision rule is determined to be an escape state. Furthermore, based on more acquired data, the exact escape state can be determined more accurately. For example, the speed difference between the four wheels can be obtained to accurately determine which wheel has better traction and is more conducive to escape, allowing for the application of stronger power to that wheel. Additionally, vehicle monitoring data can include weather information. For instance, if the weather information indicates heavy snow, the vehicle state decision rule is a snow escape state, employing a crawling drive to extricate the vehicle; if the weather information indicates heavy rain, the vehicle state decision rule is a mud escape state, with the drive method being specifically strengthened based on the vehicle's traction performance to facilitate escape.

[0062] If the position variable matches the vehicle's four-wheel wheel speed, driving speed, and acceleration, but does not match the vehicle's gear position, then the vehicle state decision rule determines it to be in a rolling state. For example, if the vehicle is currently in neutral, but all four wheels are detected to be rotating in opposite directions, the vehicle's acceleration is not zero, and the vehicle's speed is gradually increasing, then it can be determined that the vehicle is in a rolling state, requiring active braking measures or notifying the driver to brake in time.

[0063] In one or more embodiments of this application, the vehicle motion state model includes a vehicle non-circular motion model. The step of outputting predicted second motion state data for a second time period based on the fused result input into the vehicle motion state model includes: decomposing the vehicle position observation data, vehicle monitoring data, and vehicle sensor data according to the data sampling frequency to obtain basic motion data; and processing multiple consecutive basic motion data using the vehicle non-circular motion model to obtain vehicle non-circular motion information.

[0064] In practical applications, during a vehicle's rotation, what appears to an observer as a relatively continuous and regular curved motion is actually an irregular trajectory composed of multiple curved lines, a phenomenon observed in reality. Therefore, to obtain more accurate and realistic vehicle motion state information, it is necessary to decompose the sampled vehicle position observation data, vehicle monitoring data, and vehicle sensor data to obtain basic motion data. Then, using this basic motion data and a vehicle non-circular motion model, non-circular motion information can be calculated. For example, the vehicle position observation data, vehicle monitoring data, and vehicle sensor data can be decomposed into millimeter-level intervals. Each small segment of motion is then numerically calculated using uniform acceleration or uniform deceleration. Using the vehicle motion state model, the vehicle's motion state from one moment to the next, as well as non-circular displacement calculated based on changes in steering wheel angle, can be obtained.

[0065] In some of the processes described in the above embodiments and figures, multiple operations are included in a specific order. However, it should be clearly understood that these operations may not be executed in the order they appear herein, or they may be executed in parallel. The operation numbers, such as 11, 12, etc., are merely used to distinguish different operations and do not represent any execution order. In addition, these processes may include more or fewer operations, and these operations may be executed sequentially or in parallel.

[0066] It should be noted that the terms "first" and "second" in this article are used to distinguish different messages, devices, modules, etc., and do not represent a chronological order, nor do they limit "first" and "second" to different types.

[0067] Figure 3 This is a schematic diagram of the structure of a terminal device provided in an exemplary embodiment of this application, as shown below. Figure 3 As shown, the terminal device includes a memory 301 and a processor 302.

[0068] Memory 301 is used to store computer programs and can be configured to store various other data to support operation on the terminal device. Examples of this data include instructions for any application or method used to operate on the terminal device, contact data, phone book data, messages, pictures, videos, etc.

[0069] The memory 301 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random-access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), electrically programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk or optical disk.

[0070] The vehicle equipment also includes: a display component 303; and a processor 302, coupled to a memory 301, for executing computer programs in the memory 301 for:

[0071] Acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period;

[0072] The vehicle monitoring data and the vehicle sensor data are input into the vehicle motion state model to obtain the model's predicted trajectory.

[0073] Based on the correspondence of timestamps, the vehicle position observation data and the model predicted trajectory are fused using Kalman filtering to obtain the corrected target trajectory.

[0074] Further optionally, the processor 302 is also configured to: determine the first timestamp of the corresponding vehicle position observation data based on the sampling frequency of the first timestamp of the model-predicted trajectory;

[0075] According to the Kalman filter fusion algorithm, the vehicle position observation data with the same first time stamp are fused with the model predicted trajectory;

[0076] Output the corrected target trajectory corresponding to the trajectory predicted by the model.

[0077] Further optionally, the processor 302 is also used to: determine the corresponding map road data based on the vehicle position observation data;

[0078] Based on the first timestamp, the deviation information obtained by fusing the map road data and the model predicted trajectory is determined; the deviation information includes at least one of the following: offset direction, relative distance, and starting offset point;

[0079] The model observation trajectory is corrected based on the deviation information.

[0080] Further optionally, the processor 302 is also configured to: determine the cumulative deviation information between the map road data and the model predicted trajectory; and optimize the parameters in the vehicle motion state model based on the cumulative deviation information.

[0081] Further optionally, the processor 302 is also configured to: train the error analysis model using the accumulated deviation information; and input the error offset output by the error analysis model into the vehicle motion state model as an optimization parameter of the vehicle motion state model.

[0082] Further optionally, the processor 302 is also configured to: determine vehicle state decision rules based on the filtering and noise reduction processing results of the vehicle monitoring data and vehicle sensor data; and calculate the model prediction trajectory based on the vehicle state decision rules, the vehicle monitoring data, and the vehicle sensor data.

[0083] Optionally, the processor 302 is further configured to: acquire the position change in the vehicle position observation data, the vehicle's four-wheel wheel speed and gear position in the vehicle monitoring data, and the driving speed and acceleration in the vehicle sensor data; if the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, and gear position, then determine the vehicle state decision rule as normal driving state; if the position variable matches the driving speed and acceleration, but does not match the vehicle's four-wheel wheel speed, then determine the vehicle state decision rule as extrication state; if the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, but does not match the vehicle's gear position, then determine the vehicle state decision rule as slipping state.

[0084] Further optionally, the vehicle motion state model includes a vehicle non-circular motion model. The processor 302 is also configured to: decompose the vehicle position observation data, vehicle monitoring data, and vehicle sensor data according to the data sampling frequency to obtain basic motion data; and process multiple consecutive basic motion data using the vehicle non-circular motion model to obtain vehicle non-circular motion information.

[0085] The above Figure 3The memory in the memory can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random-Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Electrically Programmable Read Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0086] The above Figure 3 The display component 303 includes a screen, which may include a liquid crystal display (LCD) and a touch panel (TP). If the screen includes a touch panel, the screen can be implemented as a touchscreen to receive input signals from the user. The touch panel includes one or more touch sensors to sense touches, swipes, and gestures on the touch panel. The touch sensors can sense not only the boundaries of the touch or swipe action but also the duration and pressure associated with the touch or swipe operation.

[0087] superior Figure 3 The audio component 304 can be configured to output and / or input audio signals. For example, the audio component includes a microphone (MIC) configured to receive external audio signals when the device containing the audio component is in an operating mode, such as call mode, recording mode, and voice recognition mode. The received audio signals can be further stored in memory or transmitted via a communication component. In some embodiments, the audio component also includes a speaker for outputting audio signals.

[0088] Furthermore, such as Figure 3 As shown, the vehicle equipment also includes other components such as a communication component 305 and a power supply component 306. Figure 3 The diagram only shows some components and does not mean that the vehicle equipment includes only these components. Figure 3 The components shown.

[0089] The above Figure 3The communication component 305 is configured to facilitate wired or wireless communication between the device containing the communication component and other devices. The device containing the communication component can access wireless networks based on communication standards, such as WiFi, 2G, 3G, 4G, or 5G, or combinations thereof. In one exemplary embodiment, the communication component receives broadcast signals or broadcast-related information from an external broadcast management system via a broadcast channel. In one exemplary embodiment, the communication component may be implemented based on Near Field Communication (NFC), Radio Frequency Identification (RFID), Infrared Data Association (IrDA), Ultra Wideband (UWB), Bluetooth, and other technologies.

[0090] The power supply component 306 provides power to various components of the device in which it resides. The power supply component may include a power management system, one or more power supplies, and other components associated with generating, managing, and distributing power to the device in which it resides.

[0091] Figure 4 A schematic diagram of a vehicle motion prediction device provided in an exemplary embodiment of this application is shown below. Figure 4 As shown, the vehicle motion prediction device includes:

[0092] The acquisition module 41 is used to acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period.

[0093] The prediction module 42 is used to input the vehicle monitoring data and the vehicle sensor data into the vehicle motion state model to obtain the model predicted trajectory.

[0094] The fusion module 43 is used to fuse the vehicle position observation data with the model predicted trajectory using Kalman filtering based on the correspondence of timestamps, so as to obtain the corrected target trajectory.

[0095] Further optionally, the fusion module 43 is used to determine the first timestamp of the corresponding vehicle position observation data according to the sampling frequency of the first timestamp of the model predicted trajectory; to fuse the vehicle position observation data with the same first timestamp with the model predicted trajectory according to the Kalman filter fusion algorithm; and to output the corrected target trajectory corresponding to the model predicted trajectory.

[0096] Further optionally, the determining module 44 is used to determine the corresponding map road data based on the vehicle position observation data; determine the deviation information obtained by fusing the map road data and the model predicted trajectory based on the first timestamp; the deviation information includes at least one of: offset direction, relative distance, and starting offset point; and correct the model observation trajectory according to the deviation information.

[0097] Further optionally, the determining module 44 is used to determine the cumulative deviation information between the map road data and the model predicted trajectory; and to optimize the parameters in the vehicle motion state model based on the cumulative deviation information.

[0098] Further optionally, the determination module 44 is used to train the error analysis model using the accumulated deviation information; and to input the error offset output by the error analysis model into the vehicle motion state model as an optimization parameter of the vehicle motion state model.

[0099] Optionally, the prediction module 42 is used to determine a vehicle state decision rule based on the filtering and noise reduction results of the vehicle monitoring data and vehicle sensor data. Based on the vehicle state decision rule and the vehicle monitoring data and vehicle sensor data, a model prediction trajectory is calculated.

[0100] Further optionally, the prediction module 42 is used to acquire the position change in the vehicle position observation data, the wheel speed of the four wheels and the gear of the vehicle in the vehicle monitoring data, and the driving speed and acceleration in the vehicle sensor data;

[0101] If the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, and vehicle gear, then the vehicle state decision rule is determined to be a normal driving state.

[0102] If the position variable matches the driving speed and the acceleration, but does not match the wheel speed of the four wheels of the vehicle, then the vehicle state decision rule is determined to be an extrication state.

[0103] If the position variable matches the vehicle's four-wheel speed, driving speed, and acceleration, but does not match the vehicle's gear position, then the vehicle state decision rule is determined to be a slippery state.

[0104] Optionally, the vehicle motion state model includes a vehicle non-circular motion model. The determination module 44 is used to decompose the vehicle position observation data, vehicle monitoring data, and vehicle sensor data according to the data sampling frequency to obtain basic motion data; and to process multiple consecutive basic motion data using the vehicle non-circular motion model to obtain vehicle non-circular motion information.

[0105] In this embodiment, vehicle position observation data, vehicle monitoring data, and vehicle sensor data are acquired within a first time period. Based on the vehicle monitoring data and vehicle sensor data input into the vehicle motion state model, a model-predicted trajectory is calculated. According to the correspondence of timestamps, the vehicle position observation data and the model-predicted trajectory are fused using Kalman filtering to obtain a corrected target trajectory. Data fusion using the Kalman filtering algorithm helps improve the accuracy of vehicle motion state, reduces the yaw effect caused by sensor or system errors, provides more accurate vehicle motion state information, and can also predict subsequent vehicle motion states based on vehicle position observation data and the vehicle motion state model.

[0106] Accordingly, embodiments of this application also provide a computer-readable storage medium storing a computer program, which, when executed, can perform the steps that can be executed by a terminal device in the above method embodiments.

[0107] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0108] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0109] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0110] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0111] In a typical configuration, a computing device includes one or more processors (central processing unit, CPU), input / output interfaces, network interfaces, and memory.

[0112] Memory may include non-persistent storage in computer-readable media, such as random-access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash random-access memory (flash RAM). Memory is an example of computer-readable media.

[0113] Computer-readable media, including both permanent and non-permanent, removable and non-removable media, can store information using any method or technology. Information can be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include, but are not limited to, phase-change random-access memory (PRAM), static random-access memory (SRAM), dynamic random-access memory (DRAM), other types of random-access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0114] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0115] The above description is merely an embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principle of this application should be included within the scope of the claims of this application.

Claims

1. A trajectory fusion method based on Kalman filter, applied to vehicle equipment, characterized in that, The method includes: Acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period; The vehicle monitoring data and the vehicle sensor data are input into the vehicle motion state model to obtain the model's predicted trajectory. Based on the correspondence of timestamps, the vehicle position observation data and the model predicted trajectory are fused using Kalman filtering to obtain the corrected target trajectory; Based on the cumulative deviation information, the parameters in the vehicle motion state model are optimized; wherein, the cumulative deviation information is determined by map road data and the model's predicted trajectory.

2. The method according to claim 1, characterized in that, The step of fusing the vehicle position observation data with the model-predicted trajectory using Kalman filtering based on the timestamp correspondence to obtain the corrected target trajectory includes: Based on the sampling frequency of the first timestamp of the trajectory predicted by the model, the corresponding vehicle position observation data is determined. First timestamp; According to the Kalman filter fusion algorithm, the vehicle position observation data with the same first time stamp are fused with the model predicted trajectory; Output the corrected target trajectory corresponding to the trajectory predicted by the model.

3. The method according to claim 2, characterized in that, The step of fusing the vehicle position observation data with the same first time stamp with the model predicted trajectory according to the Kalman filter fusion algorithm includes: Based on vehicle location observation data, determine the corresponding map road data; Based on the first timestamp, the deviation information obtained by fusing the map road data and the model predicted trajectory is determined; the deviation information includes at least one of the following: offset direction, relative distance, and starting offset point; The model observation trajectory is corrected based on the deviation information.

4. The method according to claim 3, characterized in that, The optimization of parameters in the vehicle motion state model based on cumulative deviation information includes: The cumulative deviation information is used to train the error analysis model; The error offset output by the error analysis model is input into the vehicle motion state model as an optimization parameter of the vehicle motion state model.

5. The method according to claim 1, characterized in that, The step of inputting the vehicle monitoring data and the vehicle sensor data into the vehicle motion state model to obtain the model's predicted trajectory includes: Based on the vehicle location observation data, the vehicle monitoring data, and the vehicle sensor data, the vehicle state decision rules are determined through filtering and noise reduction processing. Based on the vehicle state decision rules and the vehicle monitoring data and vehicle sensor data, the model prediction is calculated. Tracking.

6. The method according to claim 5, characterized in that, The step of determining vehicle state decision rules based on the filtered noise reduction results of the vehicle position observation data, vehicle monitoring data, and vehicle sensor data includes: The vehicle position observation data includes the position change, the vehicle four-wheel speed and gear position in the vehicle monitoring data, and the driving speed and acceleration in the vehicle sensor data. If the position variable matches the vehicle's four-wheel wheel speed, driving speed, acceleration, and vehicle gear, then the vehicle state decision rule is determined to be a normal driving state. If the position variable matches the driving speed and the acceleration, but does not match the wheel speed of the four wheels of the vehicle, then the vehicle state decision rule is determined to be an extrication state. If the position variable matches the vehicle's four-wheel speed, driving speed, and acceleration, but does not match the vehicle's gear position, then the vehicle state decision rule is determined to be a slippery state.

7. A vehicle device, characterized in that, Includes: a vehicle body, on which radar and image acquisition equipment are installed, and also includes a memory, processor, communication components and display components; The memory is used to store one or more computer instructions; The processor is configured to execute one or more computer instructions for performing the steps of the method according to any one of claims 1-6 via the communication component and the display component.

8. A trajectory fusion device based on Kalman filter, applied to vehicle equipment, characterized in that, The device includes: The acquisition module is used to acquire vehicle location observation data, vehicle monitoring data, and vehicle sensor data within the first time period. The prediction module is used to input the vehicle monitoring data and the vehicle sensor data into the vehicle motion state model to obtain the model's predicted trajectory. The fusion module is used to fuse the vehicle position observation data with the model predicted trajectory using Kalman filtering based on the correspondence of timestamps, so as to obtain the corrected target trajectory. An optimization module is used to optimize the parameters in the vehicle motion state model based on cumulative deviation information; wherein, the cumulative deviation information is determined by map road data and the model's predicted trajectory.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed, it can perform the steps of the method as described in any one of claims 1-6.

Images