Multi-source sensing vehicle fusion positioning method and system, and fusion filter

Abstract

A multi-source sensing vehicle fusion positioning method and system, and a fusion filter. The method comprises: by means of a visual odometer and a wheel speed odometer, acquiring initial state information of a vehicle and recursively predicting a subsequent state. Moreover, a timestamp difference between the predicted state and an observed state is compared: if the time difference is less than a threshold, the predicted state is corrected using the observed state; and if the time difference is too large, the calculation is reverted to an observation moment. In the method, historical state data is cached, and a fusion process is optimized by taking into account Kalman filtering, to ensure timeline consistency, thereby effectively improving positioning accuracy and stability and solving the problem of errors caused by the delay of a sensor (200) in a conventional fusion method.

Description

A multi-source sensing vehicle fusion localization method, system, and fusion filter Technical Field

[0001] This application relates to the field of vehicle positioning technology, specifically to a multi-source sensing vehicle fusion positioning method, system, and fusion filter. Background Technology

[0002] Currently, intelligent vehicles are generally equipped with wheel speed odometers and visual odometry. Wheel speed odometers offer high positioning accuracy in the short term, but the positional pose diverges over time and cannot be corrected. Visual odometry, on the other hand, provides global positioning within the visual map, preventing positional divergence. However, noise disturbances can cause slight positional fluctuations, which can lead to jitter in the control module and affect control performance. Therefore, wheel speed odometers and visual odometry are complementary, and fusing them in a certain way can combine their advantages to obtain stable, accurate, and non-divergent positioning results.

[0003] In existing technologies, traditional fusion methods simply weight the inputs from two sensors according to a certain ratio, and each measurement is considered to be the latest result. In reality, since the origins of visual odometry and wheel speed odometry are generally not the same point, and because image processing and transmission take a relatively long time, the pose obtained by visual odometry has a significant delay compared to wheel speed odometry. Ignoring this delay will cause certain errors. Summary of the Invention

[0004] To address the above technical issues, this application provides a method, device, and vehicle for fusing visual odometer and wheel speed odometer.

[0005] Firstly, this application proposes a multi-source perception vehicle fusion localization method, specifically including: S1: recursively deriving the predicted vehicle state at a first current moment based on the vehicle's baseline state; S2: receiving visual information from a visual odometer and wheel speed information from a wheel speed odometer at a second current moment; S3: deriving the observed vehicle state based on the visual information and the wheel speed information; S4: performing delay calibration processing on the predicted vehicle state based on the observed vehicle state according to the time difference between the second current moment and the first current moment, to obtain the current vehicle state, where the current vehicle state is the vehicle's current effective localization.

[0006] The fusion positioning method proposed in this application mainly involves fusing two aspects of vehicle state: the observed vehicle state and the predicted vehicle state. The observed vehicle state data is directly acquired from visual odometers and wheel speed odometers. The vehicle baseline state is the initial state of the vehicle upon startup, calculated from the first acquired visual and wheel speed information. The predicted vehicle state for subsequent time periods is then recursively derived from this initial state. The predicted vehicle state is based on the baseline state. In practical applications, the transmission and processing of data from both visual and wheel speed odometers require time, and the data received by the fusion filtering module is not constant. Furthermore, because different input sources require different processing times, the data received by the fusion filtering module at the same moment may not represent observations from the same moment. This application ensures the consistency of the observed and predicted vehicle states on the timeline by caching sensor data and historical states over a period of time, and by fusing them, improves the accuracy of vehicle state calculation.

[0007] Further, step S1 includes: S11: calculating the time interval between the reference time and the first current time; S12: substituting the time interval as a variable into the state transition matrix; S13: the vehicle predicted state is: the vehicle reference state multiplied by the state transition matrix.

[0008] The vehicle predicted state is derived recursively from the vehicle baseline state. This recursive prediction method can obtain the vehicle state at any time after the baseline time. It is necessary to know the time interval between the vehicle baseline time and the recursive time, as well as the state transition matrix. This part is the calculation of the vehicle predicted state. This application obtains the vehicle predicted state and the vehicle observed state, and then merges them after unifying them on the timeline to obtain more accurate vehicle state data.

[0009] Furthermore, the visual information includes visual pose information; the wheel speed information includes wheel speed pose information.

[0010] Visual information is the vehicle's visual pose information obtained by mathematical calculations from data acquired by a visual odometry system; wheel speed information is the vehicle's wheel speed pose information obtained by mathematical calculations from data acquired by a wheel speed odometry system. Although both vehicle visual pose and vehicle wheel speed pose are pose information, they have different advantages and disadvantages and are highly complementary. When the two are fused together for subsequent vehicle state calculations, the accuracy of the vehicle state calculations can be improved.

[0011] Furthermore, step S2 includes: S21: converting the wheel speed pose information into speed information, and optimizing the visual pose information with the speed information; S22: fusing the wheel speed pose information with the optimized visual pose information to obtain the vehicle observation state.

[0012] The two pose information are independent of each other. Simply fusing them would cause the system state to repeatedly switch between the two pose information, resulting in system oscillations. Since the relative pose calculated by wheel speed odometry is more accurate, and the absolute pose obtained by visual odometry within the map is more accurate, the strategy adopted is to convert the pose calculated by wheel speed odometry into velocity and fuse it with the visual pose to avoid system jumps.

[0013] Furthermore, step S3 includes: determining whether the time difference between the first current time and the second current time is less than a preset threshold; if so, repairing the vehicle prediction state based on the vehicle observation state; otherwise, recalculating the vehicle prediction state using the second current time as the new reference time and the vehicle observation state as the new reference state.

[0014] The step of repairing the vehicle prediction state based on the vehicle observation state specifically involves fusing the vehicle observation state with the vehicle prediction state to obtain a corrected vehicle prediction state.

[0015] When the difference between the predicted time and the first current time is less than a preset threshold, that is, the difference between the vehicle observation state and the vehicle prediction state on the timeline is very small, the vehicle prediction state can be obtained by using the vehicle observation state data to repair the vehicle prediction state data.

[0016] Furthermore, the second current time is any time that is less than or equal to the first current time.

[0017] Since the vehicle's predicted state is derived recursively from the vehicle's baseline state, it is always obtained before the vehicle's observed state. Therefore, the second current time is any time that is less than or equal to the first current time.

[0018] Furthermore, the vehicle observation state and the vehicle prediction state include at least: the vehicle's coordinates in the current coordinate system, the vehicle's lateral velocity and acceleration, the vehicle's longitudinal velocity and acceleration, and the vehicle's angular velocity.

[0019] Secondly, this application proposes an apparatus that can implement the fusion method of the multi-source sensing vehicle fusion positioning method described in the first aspect.

[0020] The system includes a domain controller and sensors. The domain controller further includes a memory and a processor. The processor communicates with the memory via a bus to execute each computer instruction of the functional modules stored in the memory.

[0021] The memory includes: a state recursion module for recursively deriving the predicted vehicle state at a first current moment based on the vehicle's baseline state; an information acquisition module for receiving visual information from a visual odometer and wheel speed information from a wheel speed odometer at a second current moment; a state calculation module for deriving the observed vehicle state based on the visual information and the wheel speed information; and a state correction module for performing delay calibration processing on the predicted vehicle state based on the observed vehicle state according to the time difference between the second current moment and the first current moment.

[0022] The state recursion module further includes: a time interval calculation unit, used to calculate the time difference between the reference time and the current time in real time, accurate to the microsecond level; a state transition matrix generation unit, which dynamically generates a state transition matrix based on the vehicle kinematics model, with the time difference as the core input variable; and a predicted state calculation unit, which outputs the predicted state of the vehicle at future times through matrix operations, including position, speed, and acceleration information.

[0023] The state calculation module further includes: a wheel speed visual optimization unit, used to convert wheel speed pose information into speed information and optimize the visual pose information with the speed information; and a multi-source observation fusion unit, used to fuse the wheel speed pose information with the optimized visual pose information to obtain the vehicle observation state.

[0024] Thirdly, this application proposes a fusion filter, which includes the system described in the second aspect, for implementing a multi-source sensing vehicle fusion positioning method.

[0025] In summary, this application proposes a multi-source perception vehicle fusion localization method, system, and fusion filter. The fusion localization method obtains the initial state information of the vehicle by acquiring data information from visual odometers and wheel speed odometers, and recursively derives the subsequent predicted state information of the vehicle. Simultaneously, it acquires the observed state information of the vehicle and compares the timestamps of the predicted state information and the observed state information. When the timestamp is less than a preset threshold, the predicted state of the vehicle is repaired using the observed state. When the timestamp is greater than the preset threshold, the vehicle state is reverted to the observed state time, and the observed state is repaired using the observed state information.

[0026] Compared with the prior art, this application has at least the following beneficial effects: This application caches the vehicle predicted state information over a period of time and compares it with the vehicle observed state information. Based on the comparison result, it executes the corresponding strategy, which ensures the consistency of the vehicle observed state and the vehicle predicted state on the timeline. Furthermore, this application uses wheel speed pose information to repair the visual pose information, which further improves the accuracy of the vehicle state. Attached Figure Description

[0027] Figure 1 is a schematic diagram of the fusion method of the multi-source sensing vehicle fusion positioning method shown in the embodiment of this application.

[0028] Figure 2 is a flowchart illustrating the visual odometry positioning process according to an embodiment of this application.

[0029] Figure 3 is a schematic diagram showing the connection between the visual odometer, wheel speed odometer and fusion filter in an embodiment of this application.

[0030] Figure 4 is a system based on a multi-source perception vehicle fusion localization method as shown in an embodiment of this application.

[0031] Figure 5 is a schematic diagram of the domain controller structure shown in an embodiment of this application.

[0032] Figure 6 is a schematic diagram of the memory structure shown in an embodiment of this application.

[0033] Figure 7 is a schematic diagram of the state calculation module submodule shown in an embodiment of this application.

[0034] Figure 8 is a schematic diagram of the state recursion module submodule shown in an embodiment of this application. Detailed Implementation

[0035] 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, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0036] The development of autonomous driving technology has driven the rapid evolution of visual perception and sensor fusion technologies. This application details a method for fusing visual odometry (VO) and wheel odometry (WO), and illustrates its specific implementation in an autonomous driving system. This method combines visual and wheel speed information, caches vehicle observation state data over a period of time, and incorporates a state backoff mechanism to achieve accurate estimation of the vehicle's position, attitude, and motion state, thereby improving the positioning accuracy and robustness of the autonomous driving system.

[0037] Example 1:

[0038] As shown in Figure 1, this application proposes a multi-source perception vehicle fusion localization method, which specifically includes: S1: recursively deriving the predicted vehicle state at the first current moment based on the vehicle's baseline state; S2: receiving visual information from the visual odometer and wheel speed information from the wheel speed odometer at the second current moment; S3: deriving the observed vehicle state based on the visual information and the wheel speed information; S4: performing delay calibration processing on the predicted vehicle state based on the observed vehicle state according to the time difference between the second current moment and the first current moment to obtain the current vehicle state, which is the current effective localization of the vehicle.

[0039] In this embodiment of the invention, optionally, the vehicle reference state is the initial state of the vehicle when it starts. The initial state is calculated from the visual information and wheel speed information acquired initially, and the predicted state of the vehicle at subsequent times is recursively derived from the initial state. During this process, the vehicle continuously observes the visual information and wheel speed information at subsequent times, compares the second current time with the first current time, and makes corresponding judgments based on the comparison results.

[0040] In an embodiment of the present invention, optionally, the second current time is compared with the first current time, specifically as follows:

[0041] Obtain the difference between the second current time and the first current time.

[0042] If the difference is greater than 100 μs, it is determined that the difference between the first and second current times is too large, indicating that the first and second current times are not on the same timeline. In this case, the timeline is rewound to the second current time, and the vehicle state at the second current time is updated with the observed data. This process is then repeated to determine the vehicle state at the first current time. If not, the first and second current times are close, and the first current time can be approximated as equal to the second current time. The predicted vehicle state and the observed vehicle state are then fused to obtain the current vehicle state.

[0043] In this embodiment of the invention, optionally, the fusion / optimization process mentioned in this embodiment is implemented through Kalman filtering. Kalman filtering is a mathematical method for state estimation, particularly suitable for dynamic systems with noise and uncertainty. It can combine the dynamic model of the system and the measurement data from sensor 200 to provide an optimal estimate of the system state.

[0044] The basic principle of Kalman filtering involves the following key steps: State Model: Define the system's state variables and establish a mathematical model describing how the state evolves over time. This is typically achieved through dynamic equations or state transition equations; Measurement Model: Define the system's measurement variables and a model of how measurements relate to the state. This is typically described by measurement equations or observation equations; Prediction Step: Based on the system's dynamic model and the state estimate from the previous time step, predict the state at the first current time step and its covariance (the uncertainty of the state estimate); Update Step: Using measurement data from sensor 200, fuse the predicted state estimate with the actual measurements through Kalman gain to obtain a more accurate state estimate.

[0045] State estimation update: Update the predicted state estimate and its covariance of the system based on the fused measurement data and the predicted state estimate.

[0046] In an embodiment of the present invention, optionally, step S1 includes: S11: calculating the time interval between the reference time and the first current time; S12: substituting the time interval as a variable into the state transition matrix; S13: the vehicle predicted state is: the vehicle reference state multiplied by the state transition matrix;

[0047] The vehicle predicted state is derived recursively from the vehicle baseline state. This recursive prediction method can obtain the vehicle state at any time after the baseline time. It is necessary to know the time interval between the vehicle baseline time and the recursive time, as well as the state transition matrix. This part is the calculation of the vehicle predicted state. This application obtains the vehicle predicted state and the vehicle observed state, and then merges them after unifying them on the timeline to obtain more accurate vehicle state data.

[0048] State prediction is a method of predicting states based on historical observations of those states. It can be expressed as follows:

[0049] ,

[0050] in This is called the state transition matrix, which can be obtained from the kinematic formulas. for:

[0051]

[0052] in It represents the time interval between the previous moment and the first current moment.

[0053] The state transition matrix can be used to obtain the state at any time after t-1.

[0054] In this embodiment of the invention, optionally, the visual information includes visual pose information; the wheel speed information includes wheel speed pose information.

[0055] Visual information is the vehicle's visual pose information obtained by mathematical calculations from data acquired by a visual odometry system; wheel speed information is the vehicle's wheel speed pose information obtained by mathematical calculations from data acquired by a wheel speed odometry system. Although both vehicle visual pose and vehicle wheel speed pose are pose information, they have different advantages and disadvantages and are highly complementary. When the two are fused together for subsequent vehicle state calculations, the accuracy of the vehicle state calculations can be improved.

[0056] Visual odometry data can be acquired through onboard cameras. Visual odometry methods extract feature points from consecutive image frames or use direct methods to estimate the vehicle's displacement and rotation in three-dimensional space. Common visual odometry methods include feature-point based methods (such as ORB-SLAM) or deep learning-based direct methods (such as DeepVO). The general steps include:

[0057] Feature extraction and matching: Extracting key feature points, such as corners and edges, from consecutive image frames.

[0058] Motion estimation: Calculates camera displacement and rotation using displacement information from feature points or direct methods. This typically involves solving an optimization problem to minimize feature point matching errors or pixel-level differences.

[0059] Scale estimation: Determine the scale factor of motion, usually through scene depth information or by combining it with other sensors 200 (such as IMU).

[0060] Cumulative error handling: Cumulative errors in the motion estimation process are handled through closed-loop correction or other techniques to maintain the long-term stability of the motion trajectory.

[0061] Wheel odometer (WO) is a technique that uses information about the rotation of a vehicle's tires to estimate its motion. It infers the distance and direction of the vehicle's movement on a horizontal plane by measuring the rotational speed and direction changes of the tires.

[0062] The basic principle of a wheel speed odometer is to measure the rotation of each wheel using an encoder or sensor 200 on the vehicle's tires, thereby calculating the vehicle's displacement and rotation angle. The main steps include:

[0063] Wheel speed sensor 200 measurement: A wheel speed sensor 200 or encoder is installed on each wheel to measure the wheel's rotational speed and direction changes.

[0064] Motion integral: Integrating (or accumulating) the rotational speed and direction change of each wheel to calculate the distance the vehicle has traveled and the change in direction.

[0065] Scale calibration: The wheel speed odometer is calibrated according to the wheel size and other factors to ensure that the estimated vehicle displacement is consistent with the actual movement.

[0066] Accumulated Error Handling: Wheel speed odometers may accumulate errors over long periods of operation, especially under complex road conditions or with worn tires. Therefore, closed-loop calibration or other techniques are often needed to handle these errors and maintain accurate position estimation.

[0067] Wheel speed odometry is primarily used in fields such as robot navigation, autonomous vehicles, indoor mobile robots, and industrial automation. It is typically used in conjunction with other sensors, such as inertial measurement units (IMUs), global positioning systems (GPS), and visual odometry, to improve positioning accuracy and robustness.

[0068] In an embodiment of the present invention, optionally, step S2 includes: S21: converting the wheel speed pose information into speed information, and optimizing the visual pose information with the speed information; S22: fusing the wheel speed pose information with the optimized visual pose information to obtain the vehicle observation state.

[0069] The two pose information are independent of each other. Simply combining the two pose information will cause the system state to repeatedly switch between the two pose information, resulting in system oscillation. Since the relative pose calculated by wheel speed odometry is more accurate, and the absolute pose obtained by visual odometry within the map is more accurate, the strategy adopted is to convert the pose calculated by wheel speed odometry into velocity and fuse it with the visual pose information to avoid system jumps.

[0070] In an embodiment of the present invention, optionally, as shown in Figure 2, the visual odometry positioning process includes the following steps:

[0071] Extract the features of the acquired images.

[0072] Transform map points into camera coordinates using the current pose T.

[0073] Project map points from the camera coordinate system onto the image coordinate system.

[0074] The projected features are matched with the extracted image features.

[0075] The current pose is calculated by minimizing the reprojection error.

[0076] Wheel speed odometry primarily affects the process of transforming map points from the map coordinate system to the camera coordinate system. This process requires the current camera pose, but since the camera pose is unknown at this point, it can only be predicted using a uniform motion model. Due to the poor pose accuracy, the search matching range after reprojecting the map points needs to be set relatively large to ensure a sufficiently high probability of covering the correct projected points. If the precise pose results provided by wheel speed odometry are used to transform the coordinates of the map points, more accurate projected coordinates can be obtained.

[0077] In this embodiment of the invention, optionally, the method employs a loosely coupled fusion approach, fusing pose information from both visual odometry and wheel speed odometry inputs. Because the two measurements are independent, simply inputting both observations into a Kalman filter would cause the system state to repeatedly switch between the two observations, resulting in system oscillations. Since the relative pose calculated by wheel speed odometry is more accurate, and the absolute pose obtained by visual odometry within the map is more accurate, this method adopts a strategy of converting the pose calculated by wheel speed odometry into velocity input to the filtering system, thereby avoiding state jumps during the filtering process.

[0078] In an embodiment of the present invention, optionally, the pose calculated by the wheel speed odometer is converted into velocity, as follows:

[0079] The wheel speed odometer acquires pose information at adjacent moments, such as position (x, y) and heading angle (θ). The velocity information can then be calculated using the following steps:

[0080] Assuming the wheel speed odometer records the pose ((x_{i-1}, y_{i-1}, \theta_{i-1})) and ((x_i, y_i, \theta_i)) at consecutive time points (t_{i-1}) and (t_i), then the time interval is (\Delta t = t_i - t_{i-1}).

[0081] Within the time interval (\Delta t ), the vehicle's displacement can be calculated using the following formulas: [ \Delta x = x_i - x_{i-1} ] [ \Delta y = y_i - y_{i-1} ].

[0082] The vehicle's speed is calculated using displacement and time interval. If we assume that the vehicle's speed is along the heading angle (\theta), then the vehicle's speed (v) can be calculated using the following formula: [v = \sqrt{(\Delta x)^2 + (\Delta y)^2} / \Delta t ].

[0083] If you need to obtain the vehicle's velocity (v_{\theta}) in the heading angle (\theta), you can use the following formula: [v_{\theta} = \frac{\Delta x \cdot \cos(\theta) + \Delta y \cdot \sin(\theta)}{\Delta t} ].

[0084] These calculations can be performed based on the pose information and time intervals provided by the wheel speed odometer. It should be noted that these calculations are based on the differentiation between consecutive points, therefore, for accurate speed estimation, the time interval (Δt) should be as small as possible to reduce errors.

[0085] It should be noted that the algorithms or formulas used in the remaining steps of the visual odometry positioning process are all well-known technologies and will not be elaborated on here.

[0086] Furthermore, step S3 includes:

[0087] If the time difference between the first current time and the second current time is less than a preset threshold, the vehicle prediction state is repaired based on the vehicle observation state; otherwise, the vehicle prediction state is recalculated using the second current time as the new reference time and the vehicle observation state as the new reference state.

[0088] The specific steps for repairing the predicted vehicle state based on the observed vehicle state are as follows:

[0089] The observed vehicle state and the predicted vehicle state are fused to obtain the corrected predicted vehicle state.

[0090] When the difference between the predicted time and the first current time is less than a preset threshold, that is, the difference between the vehicle observation state and the vehicle prediction state on the timeline is very small, the vehicle prediction state can be obtained by using the vehicle observation state data to repair the vehicle prediction state data.

[0091] In an embodiment of the present invention, optionally, the second current time is any time that is less than or equal to the first current time.

[0092] Since the vehicle's predicted state is derived recursively from the vehicle's baseline state, it is always obtained before the vehicle's observed state. Therefore, the second current time is any time that is less than or equal to the first current time.

[0093] Since predictions always occur before observations, the second current time is always less than or equal to the first current time. For example, if data is observed 2 seconds ago, it is impossible to predict data from 2 seconds ago.

[0094] In an embodiment of the present invention, optionally, the vehicle observation state and the vehicle prediction state include at least: the vehicle's coordinates in the current coordinate system, the vehicle's lateral velocity and acceleration, the vehicle's longitudinal velocity and acceleration, and the vehicle's angular velocity.

[0095] The final vehicle positioning coordinate system is the visual pose coordinate system obtained by visual odometry. The vehicle's velocity and acceleration in the x and y directions, as well as the vehicle's angular velocity, can be obtained through wheel speed odometry or IMU inertial unit.

[0096] In this embodiment of the invention, optionally, after step 4, if the time difference between the second current time and the first current time is greater than a preset threshold, the vehicle observation state is used as a new vehicle reference state to participate in subsequent multi-source perception vehicle fusion positioning.

[0097] After step 4, if the time difference between the second current time and the first current time is greater than a preset threshold, it means that the predicted time is not on the same timeline as the first current time. The vehicle observation state is used as the new vehicle reference state. At this time, the subsequent vehicle prediction states are all recursively derived from the new vehicle reference state to ensure the accuracy of the subsequent vehicle prediction states.

[0098] The current vehicle status is output to the vehicle control system for vehicle navigation or assisted / intelligent driving / parking. This current vehicle status may be displayed on the in-vehicle display screen for the driver's reference, or it may be used by the vehicle assisted / intelligent driving / parking system to assist in driving the vehicle or parking.

[0099] Example 2:

[0100] As shown in Figure 4, this application proposes a multi-source perception vehicle fusion positioning system. The system includes a domain controller 100 and a sensor 200. As shown in Figure 5, the domain controller 100 further includes a memory 110 and a processor 120. The processor 120 communicates with the memory 110 via a bus 130 and is used to execute each computer instruction of the functional module stored in the memory 110.

[0101] As shown in Figure 6, the memory 110 includes: a state recursion module 1103, used to recursively derive the predicted vehicle state at a first current moment based on the vehicle reference state; an information acquisition module 1101, used to receive visual information from the visual odometer and wheel speed information from the wheel speed odometer at a second current moment; a state calculation module 1102, used to derive the observed vehicle state based on the visual information and the wheel speed information; and a state correction module 1104, used to perform delay calibration processing on the predicted vehicle state based on the observed vehicle state according to the time difference between the second current moment and the first current moment.

[0102] As shown in Figure 7, the state recursion module 1103 further includes:

[0103] Time interval calculation unit 11031: used to calculate the time difference between the reference time and the current time in real time, accurate to the microsecond level;

[0104] State transition matrix generation unit 11032: dynamically generates state transition matrix based on vehicle kinematics model, with time difference as the core input variable; Predicted state calculation unit 11033: outputs the predicted state of the vehicle at future time through matrix operations, including position, velocity and acceleration information.

[0105] As shown in Figure 8, the state calculation module 1102 further includes: a wheel speed visual optimization unit 11021, used to convert wheel speed pose information into speed information and optimize the visual pose information with the speed information; and a multi-source observation fusion unit 11022, used to fuse the wheel speed pose information with the optimized visual pose information to obtain the vehicle observation state.

[0106] Example 3:

[0107] This application proposes a fusion filter, which includes the system described in the second aspect, for implementing a multi-source sensing vehicle fusion localization method.

[0108] In an embodiment of the present invention, optionally, as shown in Figure 3, both the visual odometer and the wheel speed odometer are unidirectionally linked to the fusion filter, and the wheel speed odometer is also unidirectionally connected to the visual odometer.

[0109] The visual odometry and wheel speed odometry send the acquired visual pose and wheel speed pose to the fusion filter. At the same time, the wheel speed odometry also converts the wheel speed pose information into speed information and sends it to the visual odometry to optimize the visual pose acquired by the visual odometry.

[0110] In this embodiment of the invention, optionally, the fusion filter can be installed on cars, SUVs, off-road vehicles and buses, but is not limited to them, and can be any vehicle that needs to implement the multi-source perception vehicle fusion positioning method described in the first aspect.

[0111] In summary, this application proposes a multi-source perception vehicle fusion localization method, system, and fusion filter. The fusion localization method obtains the initial state information of the vehicle by acquiring data information from visual odometers and wheel speed odometers, and recursively derives the subsequent predicted state information of the vehicle. Simultaneously, it acquires the observed state information of the vehicle and compares the timestamps of the predicted state information and the observed state information. When the timestamp is less than a preset threshold, the predicted state of the vehicle is repaired using the observed state. When the timestamp is greater than the preset threshold, the vehicle state is reverted to the observed state time, and the observed state is repaired using the observed state information.

[0112] This application caches vehicle predicted state information over a period of time and compares it with vehicle observed state information. Based on the comparison results, it executes corresponding strategies to ensure the consistency of vehicle observed state and vehicle predicted state on the timeline. Furthermore, this application uses wheel speed and pose information to repair visual pose information, which further improves the accuracy of vehicle state.

[0113] In the several embodiments provided in this application, it will be understood that each block in the flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those shown in the figures. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved.

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

[0115] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above descriptions are merely specific embodiments of this application and are not intended to limit the scope of protection of this application. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application for those skilled in the art.

Claims

1. A multi-source sensing vehicle fusion localization method, the fusion localization method comprising: S1: Based on the vehicle baseline state, the predicted vehicle state at the first current moment is recursively derived; S2: Receive visual information from the second current visual odometer and wheel speed information from the wheel speed odometer; S3: Determine the vehicle observation status based on the visual information and the wheel speed information; S4: Based on the time difference between the first current time and the second current time, perform delay calibration processing on the predicted state of the vehicle based on the observed state of the vehicle to obtain the current state of the vehicle, which is the current effective positioning of the vehicle.

2. The multi-source sensing vehicle fusion localization method according to claim 1, wherein the vehicle reference state includes: Collect visual and wheel speed information when the vehicle starts; The initial state of the vehicle at startup is calculated based on the visual information and wheel speed information. The initial state serves as the vehicle's baseline state.

3. The multi-source sensing vehicle fusion localization method according to claim 1, wherein step S1 includes: S11: Calculate the time interval between the reference time and the first current time; S12: Substitute the time interval as a variable into the state transition matrix; S13: The predicted vehicle state is: the vehicle baseline state multiplied by the state transition matrix.

4. The multi-source perception vehicle fusion localization method according to claim 1, wherein the visual information includes visual pose information; and the wheel speed information includes wheel speed pose information.

5. The multi-source sensing vehicle fusion localization method according to claim 1, wherein step S3 includes: S31: Convert the wheel speed pose information into speed information, and optimize the visual pose information using the speed information; S32: The wheel speed and pose information is fused with the optimized visual pose information to obtain the vehicle observation state.

6. The multi-source sensing vehicle fusion localization method according to claim 5, wherein the specific steps for converting wheel speed and pose information into speed information include: Based on the pose data and time intervals of adjacent moments recorded by the wheel speed odometer, the vehicle displacement and speed are calculated, and the speed information is used to optimize the visual pose information.

7. The multi-source sensing vehicle fusion localization method according to claim 1, wherein step S4 includes: Determine whether the time difference between the first current time and the second current time is less than a preset threshold. If so, repair the predicted state of the vehicle based on the observed state of the vehicle to obtain the current state of the vehicle. Otherwise, the second current time is used as the new reference time, and the vehicle observation state is used as the new reference state to recalculate the vehicle prediction state and obtain the current vehicle state.

8. The multi-source sensing vehicle fusion positioning method according to claim 7, wherein the second current time is any time less than or equal to the first current time.

9. The multi-source perception vehicle fusion localization method according to claim 8, wherein the vehicle observation state, the vehicle prediction state, and the vehicle current state at least include: The vehicle's coordinates in the coordinate system, its lateral velocity and acceleration, its longitudinal velocity and acceleration, and its angular velocity. 10.10 A system based on a multi-source perception vehicle fusion positioning method, further comprising a domain controller (100) and a sensor (200), wherein the domain controller (100) further comprises a memory (110) and a processor (120), wherein the processor (120) communicates with the memory (110) via a bus (130) for executing each computer instruction of the functional module stored in the memory (110).

11. The system based on the multi-source perception vehicle fusion localization method according to claim 10, wherein the memory (120) comprises: State recursion module (1103), information acquisition module (1101), state calculation module (1102), state correction module (1104). The state recursion module (1103) is used to recursively derive the predicted state of the vehicle at the first current moment based on the vehicle's baseline state. The information acquisition module (1101) is used to receive visual information from the visual odometer at the second current moment and wheel speed information from the wheel speed odometer. The state calculation module (1102) is used to determine the observed state of the vehicle based on the visual information and the wheel speed information; Furthermore, the state correction module (1104) is used to perform delay calibration processing on the vehicle prediction state based on the vehicle observation state according to the time difference between the first current time and the second current time.

12. The system based on the multi-source perception vehicle fusion localization method according to claim 10, wherein the state recursion module (1103) comprises: Time interval calculation unit (11031): calculates the time interval between the reference time and the first current time; State transition matrix generation unit (11032): Substitutes the time interval as a variable into the state transition matrix; Predicted state calculation unit (11033): The predicted state of the vehicle is: the vehicle reference state multiplied by the state transition matrix.

13. The system based on the multi-source perception vehicle fusion localization method according to claim 10, wherein the state calculation module (1102) comprises: Wheel speed visual optimization unit (11021): converts the wheel speed pose information into speed information, and optimizes the visual pose information with the speed information; Multi-source observation fusion unit (11022): fuses the wheel speed and pose information with the optimized visual pose information to obtain the vehicle observation state.

14. The system based on the multi-source perception vehicle fusion positioning method according to claim 10, wherein the state correction module (1104) implements delay calibration processing through the Kalman filter algorithm.

15. A fusion filter comprising a system based on a multi-source sensing vehicle fusion localization method for implementing the multi-source sensing vehicle fusion localization method as described in claim 1.

Images