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

Abstract

In conclusion, according to the multi-source sensing vehicle fusion positioning method and system and the fusion filter, the initial state information of the vehicle is obtained by obtaining the data information of the visual odometer and the data information of the wheel speed odometer, and the subsequent vehicle prediction state information is obtained through recursion; meanwhile, vehicle observation state information is obtained, the timestamp of the vehicle prediction state information and the timestamp of the vehicle observation state information are compared, and when the timestamps are smaller than a preset threshold value, the vehicle prediction state is restored through the vehicle observation state; when the timestamp is larger than a preset threshold value, the vehicle state is returned to the vehicle observation state moment, and the vehicle prediction state is restored according to the vehicle observation state information. According to the invention, the vehicle prediction state information within a period of time is cached and compared with the vehicle observation state information, and the corresponding strategy is executed based on the comparison result, so that the unification of the vehicle observation state and the vehicle prediction state on the timeline is ensured.

Description

Technical Field

[0001] The present application relates to the field of vehicle positioning technology, and in particular to a multi-source sensing vehicle fusion positioning method, system and fusion filter. Background Art

[0002] Nowadays, smart vehicles are generally equipped with wheel speed odometers and visual odometers. The positioning accuracy of the vehicle wheel speed odometer is relatively high in a short period of time, but the positioning posture will diverge over time and cannot be corrected. When using the visual odometer for positioning, it belongs to global positioning within the range of the visual map, and the positioned position will not diverge. However, due to noise disturbance and other reasons, the obtained posture will have slight jumps. If this jumping posture is input into the planning and control module, it will cause the control module to jitter. Affect the control effect. Therefore, the two positioning methods of speed odometer and visual odometer are obviously complementary. Fusion in a certain way can combine the advantages of the two positioning methods to obtain stable, accurate, and non-divergent positioning results.

[0003] In the prior art, the traditional fusion method simply weights the inputs of the two sensors in a certain ratio, and each measurement is considered to be the latest result. In fact, since the coordinate origins of the visual odometry and the wheel speed odometry are generally not the same point, and since it takes a relatively long time to process and transmit images, the position and posture obtained by the visual odometry has an obvious delay compared to the wheel speedometer. Ignoring this delay will cause certain errors. Summary of the invention

[0004] In order to solve the above technical problems, the present application provides a method, device and vehicle for integrating a visual odometer and a wheel speed odometer.

[0005] In the first aspect, the present application proposes a multi-source perception vehicle fusion positioning method, which specifically includes:

[0006] S1: Based on the vehicle reference state, recursively obtain the first vehicle predicted state at the current moment;

[0007] S2: receiving visual information of the visual odometer and wheel speed information of the wheel speed odometer at a second current moment;

[0008] S3: Determining the vehicle observation state according to the visual information and the wheel speed information;

[0009] 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 time and the first current time.

[0010] The fusion positioning method proposed in the present application mainly includes the content of fusing two aspects of vehicle status, one is the vehicle observation status, and the other is the vehicle prediction status. The vehicle status observation data is the vehicle observation status directly obtained by the visual odometer and the wheel speed odometer, and the vehicle prediction status is the vehicle prediction status recursively obtained based on the vehicle baseline status. In actual application, the transmission and processing of the data of the visual odometer and the wheel speed odometer require a certain amount of time, and the data received by the fusion filter module is not fixed. Moreover, since different input sources require different times to process data, the data received by the fusion filter module at the same time is not necessarily the observation at the same time. The present application ensures the unity of the vehicle observation status and the vehicle prediction status on the timeline by caching sensor data and historical status within a period of time, and performs fusion, thereby improving the accuracy of vehicle status calculation.

[0011] Furthermore, the step S1 includes:

[0012] S11: Calculate the time interval between the reference time and the first current time.

[0013] S12: Substitute the time interval as a variable into the state transfer matrix.

[0014] S13: The predicted vehicle state is: the vehicle reference state multiplied by the state transfer matrix.

[0015] The predicted state of the vehicle is obtained by recursive prediction based on the vehicle's baseline state. This recursive prediction method can obtain the vehicle state at any time subsequent to the baseline time. It is necessary to know the time interval between the vehicle's baseline state time and the recursive time and the state transfer matrix. This part is the calculation of the predicted state of the vehicle. This application obtains the predicted state of the vehicle and the observed state of the vehicle, and unifies them on the timeline and then fuses them to obtain more accurate vehicle state data.

[0016] Furthermore, the visual information includes visual posture information; and the wheel speed information includes wheel speed posture information.

[0017] Visual information is the vehicle visual posture information obtained by mathematical calculation after the visual odometer obtains data; wheel speed information is the vehicle wheel speed posture information obtained by mathematical calculation after the wheel speed odometer obtains data. Although both vehicle visual posture and vehicle wheel speed posture are posture information, they have different advantages and disadvantages and are highly complementary. After the two are integrated and used for subsequent vehicle state calculations, the accuracy of vehicle state calculations can be improved.

[0018] Furthermore, the step S2 comprises:

[0019] S21: Convert the wheel speed posture information into speed information, and optimize the visual posture information with the speed information.

[0020] S22: Fusing the wheel speed posture information with the optimized visual posture information to obtain a vehicle observation state.

[0021] The two types of posture information are independent of each other. If the two types of posture information are simply fused, the system state will repeatedly switch between the two types of posture information, causing system oscillation. Since the relative posture calculated by the wheel speed odometer is more accurate, and the absolute posture obtained by the visual odometer in the map is more accurate, the strategy adopted is to convert the posture calculated by the wheel speed odometer into speed and visual posture for fusion, so as to avoid system jumps.

[0022] Furthermore, the step S3 includes:

[0023] Determine whether the time difference between the first current moment and the second current moment is less than a preset threshold. If so, repair the vehicle predicted state with the vehicle observed state; otherwise, take the second current moment as the new reference moment and the vehicle observed state as the new reference state, and recalculate the vehicle predicted state.

[0024] The repairing of the predicted vehicle state by using the observed vehicle state is specifically as follows:

[0025] The vehicle observation state is integrated with the vehicle prediction state to obtain a corrected vehicle prediction state.

[0026] When the difference between the predicted moment and the first current moment is less than the preset threshold, that is, the difference between the vehicle observation state and the vehicle prediction state on the timeline is very small, at this time, the vehicle prediction state data can be repaired using the vehicle observation state data to obtain an accurate vehicle prediction state.

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

[0028] Since the predicted state of the vehicle is recursively derived from the reference state of the vehicle, it is always obtained before the observed state of the vehicle. Therefore, the second current moment is any moment that is less than or equal to the first current moment.

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

[0030] In a second aspect, the present application proposes a device, which can implement the fusion method of the multi-source perception vehicle fusion positioning method described in the first aspect.

[0031] The system comprises:

[0032] The state recursion module is used to recursively obtain the predicted state of the vehicle at the first current moment based on the vehicle reference state.

[0033] The information collection module is used to receive the visual information of the visual odometer and the wheel speed information of the wheel speed odometer at the second current moment.

[0034] The state calculation module is used to obtain the vehicle observation state based on the visual information and the wheel speed information.

[0035] A state correction module is used to perform delay calibration processing on the vehicle predicted state based on the vehicle observed state according to the time difference between the second current moment and the first current moment.

[0036] The state calculation module also includes:

[0037] The optimization unit is used to convert the wheel speed posture information into speed information, and optimize the visual posture information with the speed information.

[0038] The fusion unit is used to fuse the wheel speed posture information with the optimized visual posture information to obtain the vehicle observation state.

[0039] In a third aspect, the present application proposes a fusion filter, which includes the system described in the second aspect, and is used to implement a multi-source perception vehicle fusion positioning method.

[0040] In summary, the present application proposes a multi-source perception vehicle fusion positioning method, system and fusion filter. The fusion positioning method obtains the data information of the visual odometer and the data information of the wheel speed odometer to obtain the initial state information of the vehicle, and recursively obtains the subsequent vehicle prediction state information; at the same time, the vehicle observation state information is also obtained, and the timestamp of the vehicle prediction state information and the timestamp of the vehicle observation state information are compared. When the timestamp is less than a preset threshold, the vehicle prediction state is repaired with the vehicle observation state; when the timestamp is greater than the preset threshold, the vehicle state is rolled back to the vehicle observation state time, and the vehicle observation state is repaired with the vehicle observation state information.

[0041] Compared with the prior art, the present invention has at least the following beneficial effects:

[0042] The present application caches the predicted vehicle status information for a period of time and compares it with the observed vehicle status information, and executes corresponding strategies based on the comparison results, thereby ensuring the consistency of the observed vehicle status and the predicted vehicle status on the timeline. The present application also repairs the visual posture information using the wheel speed posture information, thereby further improving the accuracy of the vehicle status. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a schematic diagram of the fusion method of the multi-source perception vehicle fusion positioning method shown in an embodiment of the present application.

[0044] Figure 2 This is a flow chart of visual odometer positioning shown in an embodiment of the present application.

[0045] Figure 3 It is a schematic diagram of a multi-source perception vehicle fusion positioning system shown in an embodiment of the present application.

[0046] Figure 4 It is a schematic diagram of the connection between the visual odometer, the wheel speed odometer and the fusion filter shown in the embodiment of the present application. DETAILED DESCRIPTION

[0047] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0048] The development of autonomous driving technology has promoted the rapid evolution of visual perception and sensor fusion technology. This application will introduce in detail a method for fusing a visual odometer (VO) with a wheel odometer (WO), and explain its specific implementation in an autonomous driving system. This method combines visual information and wheel speed information, and caches vehicle observation status data for a period of time and a status fallback mechanism to achieve accurate estimation of the vehicle's position, posture, and motion state, thereby improving the positioning accuracy and robustness of the autonomous driving system.

[0049] Embodiment 1:

[0050] As attached Figure 1 As shown, this application proposes a multi-source perception vehicle fusion positioning method, which specifically includes:

[0051] S1: Based on the vehicle reference state, recursively obtain the first vehicle predicted state at the current moment;

[0052] S2: receiving visual information of the visual odometer and wheel speed information of the wheel speed odometer at a second current moment;

[0053] S3: Determining the vehicle observation state according to the visual information and the wheel speed information;

[0054] 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 time and the first current time.

[0055] In the embodiment of the present invention, the vehicle reference state is optionally the initial state of the vehicle when the vehicle is started, and the initial state of the vehicle is calculated by the visual information and wheel speed information obtained for the first time, and the predicted state of the vehicle at the subsequent moment is recursively deduced from the initial state of the vehicle. In this process, the vehicle continuously observes the visual information and wheel speed information at the subsequent moment, and compares the second current moment with the first current moment, and makes a corresponding judgment according to the comparison result.

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

[0057] Gets the difference between the second current time and the first current time.

[0058] Determine whether the difference is greater than 100μs. If so, the difference between the first current moment and the second current moment is too large, and it is determined that the first current moment and the second current moment are not on the same timeline. At this time, the timeline is rewound to the second current moment, and the vehicle state at the second current moment is updated with the observed data information. Then recursively deduced to the vehicle state at the first current moment; if not, the first current moment is close to the second current moment, and it can be approximated that the first current moment is equal to the second current moment. The predicted vehicle state and the observed vehicle state are merged to obtain the vehicle current state.

[0059] In the embodiment of the present invention, optionally, the fusion / optimization process mentioned in the embodiment is implemented by Kalman filtering, which is a mathematical method for state estimation, particularly suitable for the situation where there is noise and uncertainty in dynamic systems. It can combine the dynamic model of the system and the measurement data from the sensor to provide the optimal estimate of the system state.

[0060] The basic principle of Kalman filtering involves the following key steps:

[0061] State model: Define the state variables of the system and build a mathematical model that describes how the state evolves over time. This is usually achieved through dynamic equations or state transition equations.

[0062] Measurement Model: A model that defines the measured variables of a system and how the measurements relate to the state. This is usually described by measurement equations or observation equations.

[0063] Prediction step: Based on the system's dynamic model and the state estimate at the previous moment, predict the state at the first current moment and its covariance (uncertainty of the state estimate).

[0064] Update step: Using the measurement data from the sensor, the predicted state estimate is fused with the actual measurement through the Kalman gain to obtain a more accurate state estimate.

[0065] State estimate update: Update the system’s predicted state estimate and its covariance based on the fused measurement data and predicted state estimate.

[0066] In the embodiment of the present invention, optionally, the step S1 includes:

[0067] S11: Calculate the time interval between the reference time and the first current time.

[0068] S12: Substitute the time interval as a variable into the state transfer matrix.

[0069] S13: The predicted vehicle state is: the vehicle reference state multiplied by the state transfer matrix.

[0070] The predicted state of the vehicle is obtained by recursive prediction based on the vehicle's baseline state. This recursive prediction method can obtain the vehicle state at any time subsequent to the baseline time. It is necessary to know the time interval between the vehicle's baseline state time and the recursive time and the state transfer matrix. This part is the calculation of the predicted state of the vehicle. This application obtains the predicted state of the vehicle and the observed state of the vehicle, and unifies them on the timeline and then fuses them to obtain more accurate vehicle state data.

[0071] State prediction is a means of predicting the state based on historical observations of the state. It can be expressed as follows:

[0072] X t =F(t)*X t-1 ,

[0073] Among them, F is called the state transfer matrix. According to the kinematic formula, F is:

[0074]

[0075] Wherein Δt represents the time interval from the previous moment to the first current moment.

[0076] Through the state transfer matrix, the state at any time after t-1 can be obtained.

[0077] In the embodiment of the present invention, optionally, the visual information includes visual posture information; and the wheel speed information includes wheel speed posture information.

[0078] Visual information is the vehicle visual posture information obtained by mathematical calculation after the visual odometer obtains data; wheel speed information is the vehicle wheel speed posture information obtained by mathematical calculation after the wheel speed odometer obtains data. Although both vehicle visual posture and vehicle wheel speed posture are posture information, they have different advantages and disadvantages and are highly complementary. After the two are integrated and used for subsequent vehicle state calculations, the accuracy of vehicle state calculations can be improved.

[0079] The data of the visual odometry can be obtained through the on-board camera. The visual odometry method extracts feature points from continuous image frames or uses a direct method to estimate the displacement and rotation of the vehicle in three-dimensional space. Common visual odometry methods include feature point-based methods (such as ORB-SLAM) or direct methods based on deep learning (such as DeepVO). The general steps include:

[0080] Feature extraction and matching: Extract key feature points, such as corners, edges, etc., from continuous image frames.

[0081] Motion estimation: Use the displacement information of feature points or direct methods to calculate the camera's displacement and rotation. This usually involves solving an optimization problem to minimize the matching error of feature points or pixel-level differences.

[0082] Scale estimation: Determine the scale factor of the motion, usually achieved through scene depth information or in combination with other sensors (such as IMU).

[0083] Cumulative error processing: The accumulated errors in the motion estimation process are processed through closed-loop correction or other techniques to maintain the long-term stability of the motion trajectory.

[0084] Wheel Odometry (WO) is a technology that uses the rotation information of vehicle tires to estimate vehicle motion. It infers the distance and direction of vehicle movement on the horizontal plane by measuring the rotation speed and direction change of vehicle tires.

[0085] The basic principle of the wheel speed odometer is to measure the rotation of each wheel through the encoder or sensor on the vehicle tire, so as to calculate the displacement and rotation angle of the vehicle. The main steps include:

[0086] Wheel speed sensor measurement: A wheel speed sensor or encoder is installed on each wheel to measure the rotation speed and direction changes of the wheel.

[0087] Kinematic integration: The rotational speed and direction changes of each wheel are integrated (or accumulated) to calculate the distance the vehicle has moved and the direction change.

[0088] Scale calibration: The wheel speed meter is calibrated based on the wheel size and other factors to ensure that the estimated vehicle displacement is consistent with the actual movement.

[0089] Accumulated error handling: Wheel speed odometers may accumulate errors over long periods of operation, especially under complex road conditions or tire wear. Therefore, closed-loop correction or other techniques are often required to handle the errors to maintain accurate position estimates.

[0090] Wheel speed odometry is mainly used in robot navigation, autonomous vehicles, indoor mobile robots, industrial automation, etc. It is usually combined with other sensors such as inertial measurement unit (IMU), global positioning system (GPS), and visual odometry to improve positioning accuracy and robustness.

[0091] In the embodiment of the present invention, optionally, step S2 includes:

[0092] S21: Convert the wheel speed posture information into speed information, and optimize the visual posture information with the speed information.

[0093] S22: Fusing the wheel speed posture information with the optimized visual posture information to obtain a vehicle observation state.

[0094] The two types of posture information are independent of each other. If the two types of posture information are simply combined, the system state will repeatedly switch between the two types of posture information, causing system oscillation. Since the relative posture calculated by the wheel speed odometer is more accurate, and the absolute posture obtained by the visual odometer in the map is more accurate, the strategy adopted is to convert the posture calculated by the wheel speed odometer into speed and visual posture information for fusion, so as to avoid system jumps.

[0095] In the embodiment of the present invention, optionally, as shown in the attached Figure 2 As shown in the figure, the visual odometer positioning process is shown in the figure, which specifically includes the following steps:

[0096] Extract the features of the acquired image.

[0097] Transform the map point to the camera coordinate system using the current pose T.

[0098] Project the map points in the camera coordinate system to the image coordinate system.

[0099] Match the projected features with the extracted image features.

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

[0101] The wheel speed odometer mainly affects the process of converting map points from the map coordinate system to the camera coordinate system. This process requires the current camera pose, but the camera pose is unknown at this time and can only be predicted by the uniform motion model. Due to the poor pose accuracy, the search matching range must be set relatively large after reprojecting the map points to have a high enough probability of covering the correct projection points. If the accurate pose results given by the wheel speed odometer are used to convert the coordinates of the map points at this time, more accurate projection coordinates can be obtained.

[0102] In the embodiment of the present invention, the method is optionally a loosely coupled fusion method, and the inputs of the fusion visual odometer and the wheel speed odometer are both posture information. Because the two measurements are independent of each other, if the two observations are simply input into the Kalman filter, the state of the system will repeatedly switch between the two observations, causing system oscillation. Since the relative posture calculated by the wheel speed odometer is relatively accurate, and the absolute posture obtained by the visual odometer in the map is relatively accurate, the strategy adopted by this method is to convert the posture calculated by the wheel speed odometer into speed and input it into the filtering system, thereby avoiding state jumps during the filtering process.

[0103] In the embodiment of the present invention, optionally, the posture calculated by the wheel speed odometer is converted into speed as follows:

[0104] The wheel speed odometer obtains the posture information of adjacent moments, such as position ((x,y)) and heading angle (\theta), so the speed information can be calculated by the following steps:

[0105] Assuming that the wheel speed odometer records the poses ((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), the time interval is (\Delta t=t_i-t_{i-1}).

[0106] Within the time interval (\Delta t), the displacement of the vehicle can be calculated by the following formula: [\Delta x=x_i-x_{i-1}][\Delta y=y_i-y_{i-1}].

[0107] The displacement and time interval are used to calculate the speed of the vehicle. If the vehicle speed is assumed to be in the direction of the heading angle (\theta), the vehicle speed (v) can be calculated by the following formula: [v = \sqrt{(\Delta x)^2+(\Delta y)^2} / \Delta t].

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

[0109] These calculations can be performed based on the position information and time interval provided by the wheel speed odometer. It should be noted that these calculations are based on the differentials between consecutive points, so for accurate speed estimation, the time interval (\Delta t) should be as small as possible to reduce errors.

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

[0111] Furthermore, the step S3 includes:

[0112] Determine whether the time difference between the first current moment and the second current moment is less than a preset threshold. If so, repair the vehicle predicted state with the vehicle observed state; otherwise, take the second current moment as the new reference moment and the vehicle observed state as the new reference state, and recalculate the vehicle predicted state.

[0113] The repairing of the predicted vehicle state by using the observed vehicle state is specifically as follows:

[0114] The vehicle observation state is integrated with the vehicle prediction state to obtain a corrected vehicle prediction state.

[0115] When the difference between the predicted moment and the first current moment is less than the preset threshold, that is, the difference between the vehicle observation state and the vehicle prediction state on the timeline is very small, at this time, the vehicle prediction state data can be repaired using the vehicle observation state data to obtain an accurate vehicle prediction state.

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

[0117] Since the predicted state of the vehicle is recursively derived from the reference state of the vehicle, it is always obtained before the observed state of the vehicle. Therefore, the second current moment is any moment that is less than or equal to the first current moment.

[0118] Since prediction always occurs earlier than observation, the second current moment is always less than or equal to the first current moment. For example, when 2S of data is observed, the data before 2S cannot be predicted.

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

[0120] The coordinate system used for final vehicle positioning is the coordinate system of the visual pose obtained by the visual odometer. The vehicle's speed and acceleration in the x and y directions and the vehicle's angular velocity can be obtained through the wheel speed odometer or IMU inertial unit.

[0121] In an embodiment of the present invention, optionally, after step 4, if the time difference between the second current moment and the first current moment 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.

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

[0123] Embodiment 2:

[0124] As attached Figure 3 As shown, the present application proposes a multi-source perception vehicle fusion positioning system, the system comprising:

[0125] A state recursion module, used for recursively obtaining a predicted state of the vehicle at a first current moment based on a vehicle reference state;

[0126] An information collection module, used for receiving visual information of the visual odometer and wheel speed information of the wheel speed odometer at a second current moment;

[0127] A state calculation module, used for obtaining a vehicle observation state based on the visual information and the wheel speed information;

[0128] A state correction module is used to perform delay calibration processing on the vehicle predicted state based on the vehicle observed state according to the time difference between the second current moment and the first current moment.

[0129] The state calculation module also includes:

[0130] An optimization unit, used to convert the wheel speed posture information into speed information, and optimize the visual posture information with the speed information;

[0131] The fusion unit is used to fuse the wheel speed posture information with the optimized visual posture information to obtain the vehicle observation state.

[0132] Embodiment three:

[0133] The present application proposes a fusion filter, which includes the system described in the second aspect and is used to implement a multi-source perception vehicle fusion positioning method.

[0134] In the embodiment of the present invention, optionally, as shown in the attached Figure 4 As shown, 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.

[0135] 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 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.

[0136] In an embodiment of the present invention, optionally, the fusion filter can be installed on a sedan, SUV, off-road vehicle and bus, but is not limited to it, and can be a vehicle that needs to implement the multi-source perception vehicle fusion positioning method described in the first aspect.

[0137] In summary, the present application proposes a multi-source perception vehicle fusion positioning method, system and fusion filter. The fusion positioning method obtains the data information of the visual odometer and the data information of the wheel speed odometer to obtain the initial state information of the vehicle, and recursively obtains the subsequent vehicle prediction state information; at the same time, the vehicle observation state information is also obtained, and the timestamp of the vehicle prediction state information and the timestamp of the vehicle observation state information are compared. When the timestamp is less than a preset threshold, the vehicle prediction state is repaired with the vehicle observation state; when the timestamp is greater than the preset threshold, the vehicle state is rolled back to the vehicle observation state time, and the vehicle observation state is repaired with the vehicle observation state information.

[0138] The present application caches the predicted vehicle status information for a period of time and compares it with the observed vehicle status information, and executes corresponding strategies based on the comparison results, thereby ensuring the consistency of the observed vehicle status and the predicted vehicle status on the timeline. The present application also repairs the visual posture information using the wheel speed posture information, thereby further improving the accuracy of the vehicle status.

[0139] In several embodiments provided in the present application, it is understood that each box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved.

[0140] If the functions are implemented in the form of software function 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 the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for enabling an electronic device to perform all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0141] The specific embodiments described above further describe the purpose, technical solutions and beneficial effects of the present application in detail. It should be understood that the above description is only a specific embodiment of the present application and is not intended to limit the scope of protection of the present application. It is particularly pointed out that for those skilled in the art, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of protection of the present application.

Claims

1. A multi-source perception vehicle fusion positioning method, which is characterized by: The fusion positioning method comprises: S1: Based on the vehicle reference state, recursively obtain the first vehicle predicted state at the current moment; S2: receiving visual information of the visual odometer and wheel speed information of the wheel speed odometer at a second current moment; S3: Determining the vehicle observation state according to the visual information and the wheel speed information; S4: According to the time difference between the first current moment and the second current moment, delay calibration processing is performed on the predicted vehicle state based on the vehicle observation state to obtain the current state of the vehicle.

2. The multi-source perception vehicle fusion positioning method according to claim 1 is characterized in that: The step S1 comprises: 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 transfer matrix; S13: The predicted vehicle state is: the vehicle reference state multiplied by the state transfer matrix.

3. The multi-source perception vehicle fusion positioning method according to claim 1 is characterized in that: The visual information includes visual pose information; The wheel speed information includes wheel speed posture information.

4. The multi-source perception vehicle fusion positioning method according to claim 3 is characterized in that: The step S3 comprises: S31: converting the wheel speed posture information into speed information, and optimizing the visual posture information with the speed information; S32: Fusing the wheel speed posture information with the optimized visual posture information to obtain a vehicle observation state.

5. The multi-source perception vehicle fusion positioning method according to claim 1 is characterized in that: The step S4 comprises: Determine whether the time difference between the first current moment and the second current moment is less than a preset threshold. If so, repair the vehicle predicted state with the vehicle observed state to obtain the vehicle current state; otherwise, take the second current moment as the new reference moment and the vehicle observed state as the new reference state, recalculate the vehicle predicted state and obtain the vehicle current state.

6. The multi-source perception vehicle fusion positioning method according to claim 5 is characterized in that: The second current time is any time that is less than or equal to the first current time.

7. The multi-source perception vehicle fusion positioning method according to claim 1 is characterized in that: The vehicle observed state, the vehicle predicted state and the vehicle current state include at least: the coordinates of the vehicle in the coordinate system, the vehicle lateral speed and acceleration, the vehicle longitudinal speed and acceleration and the vehicle angular velocity.

8. A system based on a multi-source perception vehicle fusion positioning method according to any one of claims 1 to 7, characterized in that: The system comprises: A state recursion module, used for recursively obtaining a predicted state of the vehicle at a first current moment based on a vehicle reference state; An information collection module, used for receiving visual information of the visual odometer and wheel speed information of the wheel speed odometer at a second current moment; A state calculation module, used for obtaining a vehicle observation state based on the visual information and the wheel speed information; A state correction module is used to perform delay calibration processing on the vehicle predicted state based on the vehicle observed state according to the time difference between the second current moment and the first current moment.

9. The system according to claim 8, characterized in that: The state calculation module also includes: An optimization unit, used to convert the wheel speed posture information into speed information, and optimize the visual posture information with the speed information; The fusion unit is used to fuse the wheel speed posture information with the optimized visual posture information to obtain the vehicle observation state.

10. A fusion filter, characterized in that the fusion filter comprises the system according to claim 9, which is used to implement a multi-source perception vehicle fusion positioning method.

Images