Vehicle multi-source sensing fusion positioning method, system and equipment

Abstract

The invention provides a vehicle multi-source sensing fusion positioning method, system and device, and belongs to the technical field of automatic driving positioning. The method comprises the following steps: under an error state Kalman filtering framework based on a manifold space, decomposing a real state quantity of a vehicle into a nominal state quantity and an error state quantity; using IMU data to predict a state through median integration; and asynchronously fusing multi-source observation such as an encoder, a GNSS (Global Navigation Satellite System) and a lane line, updating an error state through a linearized observation model and a Kalman gain, and finally merging and outputting a calibrated vehicle pose. According to the method, deep fusion and incremental updating of multi-source data are realized through a unified ESKF framework, error accumulation of pure inertial navigation and positioning jump during mode switching are effectively inhibited, and the real-time performance, output continuity and overall robustness of the system are remarkably improved while the positioning precision is ensured.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic driving positioning, and more particularly relates to a vehicle multi-source perception fusion positioning method, system and device. BACKGROUND

[0002] Automatic driving technology is becoming the core of modern transportation systems, especially in the context of trunk logistics, which requires high speed and high precision. Vehicles need to achieve centimeter-level lateral positioning capability to accurately distinguish adjacent lanes and meet real-time decision-making needs at high speed. As an upstream basic module of the automatic driving system, the accuracy and response speed of the positioning technology directly determine the safety and reliability of vehicle planning and control.

[0003] Currently, there are various technical solutions to try to solve the problem of automatic driving positioning, but all have obvious limitations. For example, some positioning systems based on multi-sensor fusion use a state machine mechanism to switch different positioning modes. Although it can maintain some positioning capability in signal-shielded sections, the independent operation of each positioning module makes it prone to positioning jumps during mode switching, which seriously affects the continuity and stability of the positioning output.

[0004] Some other methods focus on using multi-source data such as lidar and IMU for fusion positioning to improve pose estimation accuracy through point cloud matching and other technologies. However, this type of method generally relies on a large amount of point cloud data processing, resulting in excessive computational burden, making it difficult to complete positioning calculation within the time constraints required in high-speed scenarios, limiting the real-time performance of the system.

[0005] In addition, some improved solutions attempt to achieve dynamic switching of data sources by monitoring sensor confidence to enhance system reliability, but the core positioning algorithm is still based on the computationally intensive lidar-IMU coupling method, and does not fundamentally solve the conflict between computational efficiency and real-time response. In summary, existing positioning technologies are difficult to achieve effective coordination between accuracy and delay, and cannot fully meet the application requirements of high-performance scenarios such as trunk logistics. SUMMARY

[0006] To solve the above problems, the purpose of the present application is to provide a vehicle multi-source perception fusion positioning method, system and device based on a manifold space error state Kalman filter framework, which realizes high-precision, low-delay and smooth and stable vehicle positioning through multi-source sensor asynchronous fusion and error state incremental update.

[0007] To achieve the above purpose, the following technical solutions are used: In a first aspect, the present application provides a vehicle multi-source perception fusion positioning method, comprising: In the localization framework constructed based on the error state Kalman filter in manifold space, the real state quantity containing vehicle localization information is defined as being decomposed into a nominal state quantity that does not consider noise and an error state quantity that includes noise. Acquire vehicle acceleration and angular velocity data measured by IMU sensors, adjust the data using median integration, and generate target acceleration and target angular velocity; Based on the target acceleration and target angular velocity, the nominal state quantity and the error state quantity are predicted, and the predicted nominal state quantity and the predicted error state quantity are generated. The sensor's observation function is constructed. Based on the real-time acquired observation data, the observation function is linearized using the Jacobian matrix of the corresponding observation function, and the error state quantity is updated by introducing Kalman gain. The calibrated true vehicle pose is generated by merging the predicted nominal state variables with the updated error state variables.

[0008] In an optional implementation, the localization framework constructed using an error-state Kalman filter based on manifold space is defined by decomposing the true state quantity containing vehicle localization information into a nominal state quantity that does not consider noise and an error state quantity that includes noise, including: In the localization framework constructed using an error-state Kalman filter based on manifold space, the nominal state quantity is defined as:

[0009] in This refers to the nominal vehicle attitude state quantity. This refers to the nominal vehicle location status quantity. This refers to the nominal vehicle speed state quantity. For the random walk state variables of the IMU gyroscope, Random walk state quantities added to the IMU; The error state quantity is defined as:

[0010] in The attitude error state variable is represented by a Lie algebra. This is the position error state quantity. For velocity error state quantity, This refers to the random walk error state quantity of the IMU gyroscope. The random walk error state quantity added to the IMU.

[0011] In an optional implementation, the step of acquiring vehicle acceleration and angular velocity data measured by the IMU sensor, adjusting the data using median integration, and generating target acceleration and target angular velocity includes: Acquire the vehicle acceleration measured by the IMU sensor at time k and time k+1 respectively. , and vehicle angular velocity , The target acceleration is calculated using the following formula, combining the relevant nominal state variables. and target angular velocity :

[0012] in, Let be the random walk state variable of the IMU gyroscope at time k. Let be the nominal vehicle attitude state at time k. Let be the nominal vehicle attitude state at time k+1. The random walk state quantity added to the IMU at time k.

[0013] In an optional implementation, the step of predicting the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and generating the predicted nominal state quantity and the predicted error state quantity, includes: Based on target acceleration and target angular velocity The nominal state quantity at time k+1 is calculated using the following formula and used as the predicted nominal state quantity. :

[0014] in, It represents the time difference between time k and time k+1.

[0015] In an optional implementation, the step of predicting the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and generating the predicted nominal state quantity and the predicted error state quantity, further includes: Linearizing the continuous-time kinematic equations of the error state variables yields the following linearized equations:

[0016] in, For noise vectors, and These are the vehicle angular velocity and acceleration noise measured by the IMU sensor, respectively. and They are respectively and noise; The linearized equation can be simplified as follows:

[0017] The prediction equation for the error state is then:

[0018] in, ; Since the error state quantity contains noise, through the formula Calculate the noise covariance at time k+1 to assess the uncertainty included in the current state estimate; where, the covariance... It is initialized to the identity matrix. Here is the noise covariance matrix of the IMU; The error state quantity at time k+1 is calculated using the prediction equation for the error state. , which serves as the error state quantity for prediction.

[0019] In an optional implementation, the construction of the sensor's observation function, based on real-time acquired observation data, linearizes the observation function using the Jacobian matrix of the corresponding observation function, and updates the error state quantity by introducing Kalman gain, including: The sensor's observation function is defined as:

[0020] Where z is the sensor reading. To measure noise, This is a nonlinear function that converts pose physical quantities into sensor readings. When using observation functions, the appropriate function should be selected based on the type of observation data. The corresponding Jacobian matrix H will Transform it into a linear function to complete the linearization of the observation function; The Kalman gain is calculated using the following formula:

[0021] Where R0 is the observation noise matrix and H is the Jacobian matrix in the linearization process; Update the error state variables using the following formula:

[0022] in, This is the updated error state quantity at time k+1.

[0023] In an optional implementation, when the observed data is encoder observation data, assuming the vehicle's forward direction is the positive x-axis direction, the vehicle velocity state variable can be expressed as: The speed component in the direction of vehicle movement is observed through the encoder. , The corresponding Jacobian matrix is:

[0024] in, and These are the values ​​of the z-axis and y-axis velocity components after transformation to the vehicle body coordinate system, respectively. Vehicle attitude state quantity The first row elements of the corresponding rotation matrix; When the observation data is GNSS observation data, the data includes the absolute position of the vehicle. Vehicle attitude state quantity , The corresponding Jacobian matrix is:

[0025] When the observation data is fused lane line observation data The corresponding Jacobian matrix is:

[0026] in, and 3 respectively The second and third rows of the 3 identity matrix.

[0027] In an optional implementation, generating the calibrated true vehicle pose by merging the predicted nominal state quantities with the updated error state quantities includes: The calibrated pose is obtained by merging the predicted nominal state variables with the updated error state variables using the following formula:

[0028] in, For addition on a manifold, This represents the vehicle's true pose at time k+1.

[0029] Secondly, embodiments of this application also provide a vehicle multi-source perception fusion positioning system, including: The state quantity definition module is used to define, in the localization framework constructed based on the error state Kalman filter of manifold space, the decomposition of the real state quantity containing vehicle localization information into a nominal state quantity that does not consider noise and an error state quantity that includes noise. The data adjustment module is used to acquire vehicle acceleration and angular velocity data measured by IMU sensors, and to adjust the data using median integration to generate target acceleration and target angular velocity. The state quantity prediction module is used to predict the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and generate the predicted nominal state quantity and the predicted error state quantity. The state quantity update module is used to construct the sensor's observation function. Based on the real-time acquired observation data, the observation function is linearized using the Jacobian matrix of the corresponding observation function, and the error state quantity is updated by introducing Kalman gain. The data fusion module is used to generate the calibrated true vehicle pose by merging the predicted nominal state variables with the updated error state variables.

[0030] Thirdly, embodiments of this application also provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the vehicle multi-source perception fusion localization method as described in any of the above.

[0031] As can be seen from the above technical solutions, the present invention has the following advantages: The vehicle multi-source perception fusion localization method provided in this application constructs an error state Kalman filter framework based on manifold space, decomposing the state variables into nominal states and error states. Median integration is used to process IMU data to improve prediction accuracy. The error states and their covariance are linearized, and multi-source observation data from encoders, GNSS, IMU, and lane lines are asynchronously fused for incremental updates and calibration of the error states. Finally, the states are merged on the manifold to output the pose. This method effectively unifies multi-sensor information, significantly improving the system's computational efficiency and real-time performance while ensuring lateral positioning accuracy. It overcomes the positioning jumps caused by mode switching and the latency issues arising from reliance on complex point cloud matching in traditional solutions, meeting the stringent requirements for high accuracy, high robustness, and low latency in high-speed scenarios such as trunk logistics.

[0032] This application decomposes the state variables into nominal states and error states, and uses Lie algebras on manifolds to represent the attitude. This enables asynchronous and incremental optimal fusion of the encoder's high-frequency relative motion information, the GNSS absolute pose reference, and the lateral geometric constraints provided by the lane lines in the error state space. This fusion mechanism effectively combines the advantages of various sensors, exerting strong constraints on vehicle pose from multiple dimensions, thereby stabilizing the lateral positioning accuracy at the centimeter level and meeting the stringent requirement of accurately distinguishing adjacent lanes.

[0033] This application significantly improves the real-time response capability of the system through a mechanism of high-frequency inertial prediction and asynchronous observation updates. The prediction stage utilizes IMU data, continuously calculating the vehicle pose using median integration at the IMU sensor's natural frequency, ensuring the instantaneous nature and low latency of state estimation. The update stage allows asynchronous injection of observation data from sensors of different frequencies, performing state calibration without blocking the high-frequency prediction stream. This algorithmically balances the real-time requirements and fusion accuracy in high-speed scenarios.

[0034] This application utilizes error state separation and covariance management to ensure the smoothness and stability of the positioning output. All sensor observations are used to update the same set of error state variables, which are then combined with the nominal state variables for output, avoiding the inherent state transition problems in traditional multi-mode switching schemes. Simultaneously, by continuously predicting and updating the error state covariance, uncertainty can be quantified, and the Kalman gain can be dynamically adjusted to assign trust weights to different sensors, thereby outputting a continuous, smooth, and highly reliable pose trajectory.

[0035] This application linearizes the nonlinear motion equations and observation equations in the error state space, constraining the core filtering operations to a local linear space, thus significantly reducing computational complexity while maintaining accuracy. The Jacobian matrices designed separately for the encoder, GNSS, and lane lines ensure that various types of observation information can be correctly and efficiently incorporated into the filtering system, enabling the system to maintain reliable positioning using the remaining sensors even in complex scenarios such as limited GNSS signals and changing weather.

[0036] This application uses SO(3) manifolds and their corresponding 3D Lie algebras to represent attitude, which significantly reduces the dimensionality of state variables compared to traditional rotation matrices or quaternions. Combined with the linearization of motion and observation equations, the core operations of the entire prediction and update process are all efficient matrix operations, fundamentally reducing the computational burden of the system. This allows it to meet the stringent limitations of computing power and power consumption on automotive embedded platforms, providing feasibility for deploying high-performance positioning algorithms in mass-produced vehicles. Attached Figure Description

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

[0038] Figure 1 This is a flowchart illustrating the vehicle multi-source perception fusion localization method provided in this application.

[0039] Figure 2This is a schematic diagram of the structure of the vehicle multi-source perception fusion positioning system provided in this application.

[0040] Figure 3 A schematic diagram of the structure of the electronic device provided in this application. Detailed Implementation

[0041] The various embodiments of this disclosure will be described more fully in the detailed steps of the vehicle multi-source perception fusion localization method described below. This disclosure may have various embodiments, and adjustments and changes may be made therein. However, it should be understood that there is no intention to limit the various embodiments of this disclosure to the specific embodiments disclosed herein, but rather this disclosure should be understood to cover all adjustments, equivalents, and / or alternatives falling within the spirit and scope of the various embodiments of this disclosure.

[0042] In the following, the terms “comprising” or “may include”, which may be used in various embodiments of this disclosure, indicate the presence of the disclosed functions, operations, or elements, and do not limit the addition of one or more functions, operations, or elements. Furthermore, as used in various embodiments of this disclosure, the terms “comprising,” “having,” and their cognates are intended only to indicate a particular feature, number, step, operation, element, component, or combination of the foregoing, and should not be construed as primarily excluding the presence of one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing, or the possibility of adding one or more combinations of the foregoing.

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

[0044] Please see Figure 1The diagram illustrates a flowchart of a vehicle multi-source perception fusion localization method in a specific embodiment. This method uses the ESKF framework to solve for vehicle state variables, comprising two processes: prediction and update. In the prediction process, acceleration and angular velocity data measured by the IMU are integrated to calculate the vehicle's velocity, attitude, and position information. This process outputs real-time pose estimation results consistent with the IMU data frequency. However, the IMU integration process accumulates sensor noise, leading to increased pose estimation errors and decreased accuracy. To correct these errors, ESKF introduces an update process, fusing observation data from other sensors to calibrate the pose. These observations constrain the vehicle's pose from different dimensions, effectively reducing pose estimation errors. By continuously fusing multi-source observation information, ESKF can maintain the vehicle's pose within a high-precision range, ensuring the accuracy and stability of the localization results.

[0045] The vehicle multi-source perception fusion localization method disclosed in this embodiment specifically includes the following steps: S1: In the localization framework constructed based on the error state Kalman filter of manifold space, the real state quantity containing vehicle localization information is defined as being decomposed into a nominal state quantity that does not consider noise and an error state quantity that includes noise.

[0046] In a specific implementation, in ESKF, the real state quantity containing vehicle positioning information is decomposed into a nominal state quantity that does not consider noise and an error state quantity that includes noise, wherein the nominal state quantity is defined as:

[0047] in, The nominal vehicle attitude state is usually represented by 3. 3. The rotation matrix or quaternion representation corresponds to 9 and 4 variables to be solved, respectively. In order to reduce the number of variables to be solved, this method defines the attitude on the manifold and uses the SO(3) group in the Lie group. In this way, only 3 variables are needed to represent the vehicle attitude, reducing the amount of computation. This refers to the nominal vehicle location status quantity. This refers to the nominal vehicle speed state quantity. For the random walk state variables of the IMU gyroscope, Random walk state quantities added to the IMU.

[0048] The error state quantity is defined as:

[0049] in, The attitude error state variable is represented by a Lie algebra. This is the position error state quantity. For velocity error state quantity, This refers to the random walk error state quantity of the IMU gyroscope. The random walk error state quantity added to the IMU.

[0050] After defining the nominal state variables and the error state variables, the vehicle's true state variables can be represented as:

[0051] Right now:

[0052] Here, the subscript "true" represents the true state quantity, and exp represents the exponential mapping between Lie algebras and Lie groups. Through this mapping function, Lie algebras can be converted into Lie groups.

[0053] It should be noted that the prediction phase of ESKF is divided into the prediction of nominal state quantities and the prediction of error state quantities. The prediction of nominal state quantities depends on the dynamic equation, which conforms to the following differential equation:

[0054] in, It is the gravity vector. and These are the IMU angular velocity and acceleration measurements, respectively. The subscript "m" indicates the raw measurement from the sensor. and The values ​​are the IMU angular velocity and acceleration noise, respectively, and both follow a Gaussian distribution. and Biased random walk and The noise. The prediction of nominal state variables is essentially the process of integrating IMU measurement data and solving for state variables such as attitude, velocity, and position.

[0055] S2: Acquire vehicle acceleration and angular velocity data measured by IMU sensors, adjust the data using median integration, and generate target acceleration and target angular velocity.

[0056] In a specific implementation, the vehicle acceleration measured by the IMU sensor is acquired at time k and time k+1, respectively. , and vehicle angular velocity , The target acceleration is calculated using the following formula, combining the relevant nominal state variables. and target angular velocity :

[0057] To improve the accuracy of state variable recursion, this method employs the more accurate median integral when processing IMU data. Let be the random walk state variable of the IMU gyroscope at time k. Let be the nominal vehicle attitude state at time k. Let be the nominal vehicle attitude state at time k+1. The random walk state quantity added to the IMU at time k.

[0058] S3: Based on the target acceleration and target angular velocity, predict the nominal state quantity and the error state quantity, and generate the predicted nominal state quantity and the predicted error state quantity.

[0059] In a specific implementation, based on target acceleration and target angular velocity The nominal state quantity at time k+1 is calculated using the following formula and used as the predicted nominal state quantity. :

[0060] in, It represents the time difference between time k and time k+1.

[0061] Unlike the prediction of nominal state variables, the prediction of error state variables requires first linearizing the kinematic equations. This is because linearization simplifies computation, and since error state variables are typically small, linearization does not significantly reduce accuracy. Furthermore, linearization ensures that all state variable derivations are performed within the tangent space of the manifold, conforming to the Gaussianity assumption of the noise. The final result of linearization is given below:

[0062] in Let be the noise vector. For ease of description, the linearized equation is simplified as follows:

[0063] The prediction equation for the error state is then:

[0064] in Since the error state variables contain noise, it is also necessary to calculate the noise covariance to assess the uncertainty contained in the current state estimate. The calculation method is as follows: Covariance It was initialized as an identity matrix. This is the noise covariance matrix of the IMU, which is related to the physical characteristics of the IMU.

[0065] As can be seen, the prediction phase of ESKF utilizes IMU median integration technology to estimate the vehicle's pose at high frequency. However, due to the presence of measurement noise, simple IMU integration can cause the pose to diverge rapidly. Therefore, ESKF introduces an update phase, using multiple sensors other than the IMU to provide pose constraints to calibrate the pose, thereby ensuring that the pose remains highly accurate at all times.

[0066] S4: Construct the sensor's observation function. Based on the real-time acquired observation data, linearize the observation function using the Jacobian matrix of the corresponding observation function, and update the error state quantity by introducing Kalman gain.

[0067] In a specific implementation, during the ESKF update phase, the sensor's observation function is defined as:

[0068] In the formula, z is the sensor reading. To measure noise, This is a function that converts pose physical quantities into sensor readings, and is typically a nonlinear function. In practical applications of the observation model in ESKF, to simplify calculations, a more complex function is usually used. The corresponding Jacobian matrix H will Convert to a linear function.

[0069] When using observation functions, the appropriate function should be selected based on the type of observation data. The corresponding Jacobian matrix H will Transform it into a linear function to complete the linearization of the observation function; After linearizing the observation function, in order to update the pose state, a Kalman gain K needs to be introduced, which is calculated using the following formula:

[0070] Where R0 is the observation noise matrix, which is related to the physical characteristics of the sensor, and H is the Jacobian matrix in the linearization process; Update the error state variables using the following formula:

[0071] in, This is the updated error state quantity at time k+1.

[0072] In this step, different sensors correspond to different observation functions, and therefore there are differences in the linearization of the observation functions; that is, different sensors correspond to different Jacobian matrices H. Therefore, based on the following three types of observation data, the corresponding Jacobian matrices are as follows: 1. Encoder observation data: Assuming the vehicle's forward direction is the positive x-axis, the vehicle's velocity state variable can be expressed as: The encoder can observe the velocity component in the direction of vehicle movement. When linearizing the observation function corresponding to the encoder, its corresponding Jacobian matrix is:

[0073] in, and These are the values ​​of the z-axis and y-axis velocity components after transformation to the vehicle body coordinate system, respectively. For posture The first row element of the corresponding rotation matrix.

[0074] 2. GNSS observation data: GNSS observed the absolute position of the vehicle. If it's GNSS observation with RTK signal, the vehicle's attitude can also be observed. Taking GNSS observations with RTK signals as an example, the corresponding Jacobian matrix when linearizing the GNSS observation function is:

[0075] 3. Integrate lane line observation data: The vehicle's built-in high-precision map and perception algorithm both provide lane markings for the road. These two can be fused to obtain a final set of lane markings. The fusion process also provides observations of the vehicle's position and attitude. When linearizing this fused observation function, its corresponding Jacobian matrix is:

[0076] in, and 3 respectively The second and third rows of the 3 identity matrix.

[0077] S5: Generate the calibrated true vehicle pose by merging the predicted nominal state variables with the updated error state variables.

[0078] In a specific implementation, within the ESKF framework, for each sensor observation acquired, the corresponding Jacobian matrix is ​​substituted into the above equation to obtain the updated error state quantity. Then, the error state quantity is merged with the nominal state quantity to obtain the calibrated pose.

[0079] in, For addition on a manifold, the following computational rules apply:

[0080] This step combines the predicted nominal state variables with the updated error state variables to obtain the calibrated pose, which is the final vehicle positioning result.

[0081] In this embodiment, firstly, the median integral method is used to process the inertial measurement unit (IMU) data to obtain a high-frequency initial estimate of the vehicle pose. This estimate has high accuracy in the short term, but accumulates errors over time. Subsequently, vehicle velocity information provided by the encoder is introduced to indirectly constrain the vehicle's longitudinal displacement, thereby calibrating the longitudinal translation component of the pose. Further, GNSS observation data is fused to provide lateral and longitudinal constraints on the vehicle position, significantly improving the accuracy of the translation component of the positioning. Finally, by fusing road geometry information provided by high-precision maps with lane line information perceived in real-time by onboard sensors, precise constraints on the vehicle's lateral position and heading are achieved, thus comprehensively improving the overall accuracy of the positioning system. This method, through the collaborative fusion of multi-level, multi-source heterogeneous sensors, effectively overcomes the limitations of a single sensor and can meet the high-precision, high-robust positioning requirements of commercial vehicles in long-haul logistics scenarios.

[0082] like Figure 2 As shown, the following are embodiments of the vehicle multi-source perception fusion positioning system provided in this disclosure. This system and the vehicle multi-source perception fusion positioning method in the above embodiments belong to the same inventive concept. For details not described in detail in the embodiments of the vehicle multi-source perception fusion positioning system, please refer to the embodiments of the vehicle multi-source perception fusion positioning method described above.

[0083] A vehicle multi-source perception fusion positioning system, comprising: The state quantity definition module is used to define, in the localization framework constructed based on the error state Kalman filter in manifold space, the decomposition of the real state quantity containing vehicle positioning information into a nominal state quantity that does not consider noise and an error state quantity that includes noise.

[0084] The data adjustment module is used to acquire vehicle acceleration and angular velocity data measured by IMU sensors, and to adjust the data using median integration to generate target acceleration and target angular velocity.

[0085] The state quantity prediction module is used to predict the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and generate the predicted nominal state quantity and the predicted error state quantity.

[0086] The state update module is used to construct the sensor's observation function. Based on the real-time acquired observation data, it linearizes the observation function using the Jacobian matrix of the corresponding observation function and updates the error state variable by introducing Kalman gain.

[0087] The data fusion module is used to generate the calibrated true vehicle pose by merging the predicted nominal state variables with the updated error state variables.

[0088] The vehicle multi-source perception fusion positioning system provided in this embodiment constructs a unified error state Kalman filter framework based on manifold space, deeply collaborates and asynchronously fuses multiple source sensors, uses median integral to improve inertial prediction accuracy, and performs incremental observation updates and covariance management in the error state space. Thus, at the algorithm level, it simultaneously achieves the comprehensive advantages of centimeter-level lateral positioning accuracy, high-frequency real-time response, smooth and stable output trajectory, strong adaptability to complex environments, and high overall computational efficiency, effectively meeting the stringent requirements of high precision, high robustness, and low latency in high-speed autonomous driving scenarios such as trunk logistics.

[0089] Figure 3 A schematic diagram of the hardware structure of an electronic device for implementing various embodiments of the present invention.

[0090] The vehicle multi-source perception fusion localization method provided in this application can be applied to electronic devices. Those skilled in the art will understand that the electronic device structure involved in the embodiments of this invention does not constitute a limitation on the electronic device. An electronic device may include more or fewer components than illustrated, or combine certain components, or have different component arrangements. In the embodiments of this invention, the electronic device includes, but is not limited to, laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the embodiments of this application described and / or claimed herein.

[0091] Electronic devices may include processors, external memory interfaces, internal memory, universal serial bus (USB) interfaces, charging management modules, power management modules, batteries, wireless communication modules, audio modules, speakers, microphones, sensor modules, buttons, cameras, displays, and SIM card interfaces, etc.

[0092] A processor may include one or more processing units, such as: a central processing unit (CPU), an application processor (AP), a modem processor, a graphics processing unit (GPU), an image signal processor (ISP), a controller, memory, a video codec, a digital signal processor (DSP), a baseband processor, and / or a neural network processing unit (NPU). Different processing units may be independent devices or integrated into one or more processors.

[0093] The processor can serve as the nerve center and command center of an electronic device. The controller can generate operation control signals based on the instruction opcode and timing signals to control the fetching and execution of instructions.

[0094] The processor may also include memory for storing instructions and data. In some embodiments, the memory in the processor is a cache memory. This memory can store instructions or data that the processor has just used or that are used repeatedly. If the processor needs to use the instruction or data again, it can retrieve it directly from this memory. This avoids repeated accesses, reduces processor latency, and thus improves system efficiency.

[0095] An external storage interface (ESI) can be used to connect external memory cards, such as microSD cards, to expand the storage capacity of electronic devices. The external memory card communicates with the processor through the ESI to perform data storage functions, such as saving music and video files on the external memory card.

[0096] Internal memory can be used to store computer executable program code, which includes instructions. The processor executes various functional applications and data processing of electronic devices by running the instructions stored in internal memory. Internal memory can include a program storage area and a data storage area. Internal memory can include high-speed random access memory, and can also include non-volatile memory, such as at least one disk storage device, flash memory device, universal flash storage (UFS), etc.

[0097] Wireless communication functionality in electronic devices can be achieved through antennas, wireless communication modules, modem processors, and baseband processors.

[0098] Wireless communication modules can provide solutions for wireless communication applications in electronic devices, including wireless local area networks (WLANs) (such as wireless fidelity (Wi-Fi) networks), Bluetooth (BT), global navigation satellite system (GNSS), frequency modulation (FM), near field communication (NFC), and infrared (IR) technologies.

[0099] Electronic devices can implement audio functions through audio modules, speakers, receivers, microphones, headphone jacks, and application processors.

[0100] Electronic devices can achieve shooting functions through ISPs, cameras, video codecs, GPUs, displays, and application processors.

[0101] Electronic devices can achieve display functions through GPUs, displays, and application processors.

[0102] A GPU is a microprocessor for image processing, connected to the display screen and application processor. GPUs perform mathematical and geometric calculations for graphics rendering. A processor may include one or more GPUs, which execute program instructions to generate or modify display information.

[0103] A display screen is used to display images, videos, etc. A display screen includes a display panel.

[0104] The aforementioned electronic device realizes the vehicle multi-source perception fusion positioning method of this application by constructing an error state Kalman filter framework based on manifold space and adopting an asynchronous incremental fusion mechanism of multi-source sensor observations, thereby achieving the beneficial effects of improving positioning accuracy while ensuring system real-time performance, output continuity and environmental adaptability.

[0105] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A vehicle multi-source perception fusion positioning method, characterized in that, Comprise: In the positioning framework constructed based on the manifold space error state Kalman filter, the real state quantity containing vehicle positioning information is defined to be decomposed into a nominal state quantity not considering noise and an error state quantity containing noise; Obtain the vehicle acceleration and angular velocity data measured by the IMU sensor, adjust the data by using median integration, and generate target acceleration and target angular velocity; Based on the target acceleration and target angular velocity, the prediction of the nominal state quantity and the error state quantity is carried out, and the predicted nominal state quantity and the predicted error state quantity are generated; Construct the observation function of the sensor, linearize the observation function by using the Jacobian matrix of the corresponding observation function based on the real-time obtained observation data, and update the error state quantity by introducing the Kalman gain; By merging the predicted nominal state quantity and the updated error state quantity, the calibrated real pose of the vehicle is generated.

2. The vehicle multi-source perception fusion positioning method according to claim 1, characterized in that, In the positioning framework constructed based on the manifold space error state Kalman filter, the real state quantity containing vehicle positioning information is defined to be decomposed into a nominal state quantity not considering noise and an error state quantity containing noise, comprising: In the positioning framework constructed based on the manifold space error state Kalman filter, the nominal state quantity is defined as: wherein is a nominal vehicle pose state quantity, is a nominal vehicle position state quantity, is a nominal vehicle velocity state quantity, is a random walk state quantity for the IMU gyroscope, is a random walk state quantity for the IMU accelerometer; The error state quantity is defined as: wherein for the attitude error state quantity, using Lie algebra; for the position error state quantity, for the velocity error state quantity, for the random walk error state quantity of the IMU gyroscope, for the random walk error state quantity of the IMU accelerometer.

3. The vehicle multi-source perception fusion positioning method according to claim 2, characterized in that, The acquisition of the vehicle acceleration and angular velocity data measured by the IMU sensor, the data adjustment by using median integration, and the generation of target acceleration and target angular velocity, comprising: At the k-th time instant and the k+1-th time instant, respectively, the vehicle acceleration measured by the IMU sensor is acquired , , and the vehicle angular velocity , , are combined with the relevant nominal state quantities to calculate the target acceleration and the target angular velocity by the following equations: wherein, is the random walk state quantity of the IMU gyroscope at the kth time instant, is the nominal vehicle attitude state quantity at the kth time instant, is the nominal vehicle attitude state quantity at the k+1th time instant, is the IMU integrated random walk state quantity at the kth time instant.

4. The vehicle multi-source perception fusion positioning method according to claim 3, characterized in that, The prediction of the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and the generation of the predicted nominal state quantity and the predicted error state quantity, comprising: Based on the target acceleration and the target angular velocity , the nominal state quantity at the k+1 time is calculated as a predicted nominal state quantity by the following equation : wherein, is the time difference from the kth moment to the k+1th moment.

5. The vehicle multi-source perception fusion positioning method according to claim 4, characterized in that, The prediction of the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and the generation of the predicted nominal state quantity and the predicted error state quantity, further comprising: The linearization processing of the continuous time kinematics equation of the error state quantity is carried out, and the following linearization equation is generated: wherein, is a noise vector, and are vehicle angular velocity and acceleration noise measured by the IMU sensors, respectively, and are and noise; The linearization equation is simply recorded as: The prediction equation of the error state is: wherein ; Since the error state quantity contains noise, the noise covariance at the k+1 time is calculated by the formula to evaluate the uncertainty contained in the current state estimate; where the covariance is initialized as an identity matrix, is the noise covariance matrix of the IMU; The error state quantity at time k+1 is calculated using the prediction equation for the error state. , which serves as the error state quantity for prediction.

6. The vehicle multi-source perception fusion positioning method according to claim 5, characterized in that, The construction of the observation function of the sensor, the linearization of the observation function by using the Jacobian matrix of the corresponding observation function based on the real-time obtained observation data, and the updating of the error state quantity by introducing the Kalman gain, comprising: The observation function of the sensor is defined as: where z is a sensor reading, is a measurement noise, is a non-linear function converting the pose physical quantity into a sensor reading; In using the observation function, based on the kind of observation data, use The corresponding Jacobian matrix H will Convert to a linear function, complete the linearization of the observation function; The Kalman gain is calculated by the following formula: Wherein, R0 is the observation noise matrix, and H is the Jacobian matrix in the linearization process; The error state quantity is updated by the following formula: wherein, is the updated error state quantity at time k+1.

7. The vehicle multi-source perception fusion positioning method according to claim 6, characterized in that: When the observation data is the encoder observation data, assuming that the vehicle forward direction is the positive direction of the x-axis, the vehicle speed state quantity can be expressed as , the speed component in the vehicle forward direction is observed by the encoder , The corresponding Jacobian matrix is: wherein and are the values of the z-axis and y-axis velocity components after conversion to the vehicle body coordinate system, respectively, is the vehicle attitude state quantity is the first row element of the corresponding rotation matrix. When the observation data is GNSS observation data, the data comprises absolute position of the vehicle and vehicle attitude state quantities , The corresponding Jacobian matrix is: When the observation data is the fused lane line observation data, The corresponding Jacobian matrix is: wherein and are 3 the 2nd and 3rd rows of the 3x3 identity matrix.

8. The vehicle multi-source perception fusion positioning method according to claim 6, characterized in that, The merging of the predicted nominal state quantity and the updated error state quantity to generate the calibrated real pose of the vehicle, comprising: The predicted nominal state quantity and the updated error state quantity are merged to obtain the calibrated pose by the following formula: wherein, is the addition on the manifold, is the real pose of the vehicle at the k+1 time instant. 9.A vehicle multi-source perception fusion positioning system, characterized in that, The system adopts the vehicle multi-source perception fusion positioning method according to any one of claims 1 to 8; The system comprises: A state quantity definition module is used to define the real state quantity containing vehicle positioning information to be decomposed into a nominal state quantity not considering noise and an error state quantity containing noise in the positioning framework constructed based on the manifold space error state Kalman filter. a data adjustment module, configured to acquire vehicle acceleration and angular velocity data measured by an IMU sensor, perform data adjustment by using median integration, and generate target acceleration and target angular velocity; a state quantity prediction module, configured to perform prediction of the nominal state quantity and the error state quantity based on the target acceleration and the target angular velocity, and generate a predicted nominal state quantity and a predicted error state quantity; a state quantity update module, configured to construct an observation function of the sensor, linearize the observation function by using a Jacobian matrix of the corresponding observation function based on real-time acquired observation data, and update the error state quantity by introducing a Kalman gain; a data fusion module, configured to generate a calibrated real vehicle pose by merging the predicted nominal state quantity and the updated error state quantity.

10. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor implements the steps of the vehicle multi-source perception fusion positioning method according to any one of claims 1 to 8 when executing the program. The processor implements the steps of the vehicle multi-source perception fusion positioning method according to any one of claims 1 to 8 when executing the program.

Images