Multi-sensor fusion pose determination method and device, electronic equipment and storage medium

Abstract

The application relates to the computer technical field and provides a multi-sensor fusion pose determination method and device, electronic equipment and a storage medium. The method obtains sensor data at a current time, inputs the sensor data into multiple pose calculation algorithms, calculates multiple initial pose results and a confidence parameter corresponding to each initial pose result, maps each initial pose result to a target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result, generates time series data according to the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result, inputs the time series data into a pre-trained multi-sensor fusion model, processes the time series data by using a multi-head self-attention mechanism in the multi-sensor fusion model, and obtains a target pose result at the current time.

Description

Technical Field

[0001] This application relates to the field of computer technology, and in particular to a method, apparatus, electronic device and storage medium for multi-sensor fusion pose determination. Background Technology

[0002] With the rapid development of autonomous driving, drones, mobile robots, and intelligent surveying, high-precision and robust vehicle localization and attitude estimation (i.e., pose determination) have become core prerequisites for achieving autonomous navigation and control of systems. In complex and ever-changing application scenarios (such as urban canyons, tunnels, underground parking garages, and extreme weather), single sensors (such as pure global satellite navigation systems, pure visual cameras, or pure LiDAR) are often prone to failure due to signal obstruction, feature loss, or drastic changes in lighting. Therefore, multi-sensor fusion localization has become the mainstream technology in the industry.

[0003] Currently, most traditional multi-sensor fusion algorithms are based on Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), or Factor Graph Optimization (FGEM) architectures. However, when faced with anomalies or failures of different types of sensors due to external conditions, existing multi-source fusion localization and orientation algorithms typically can only adjust sensor weights according to preset rules or empirical thresholds. This rigid weight allocation mechanism has significant drawbacks: when some sensors malfunction or fail, the algorithm is prone to divergence or even collapse; simultaneously, traditional algorithms are severely inadequate in adapting to "data gaps" caused by heterogeneous sensors. Furthermore, multi-source fusion localization and orientation algorithms require precise weighting of each sensor during the fusion phase. As the number of sensors in the connected system increases, the coupling becomes exponentially more complex, making it extremely difficult for traditional algorithms, which rely on manual modeling, to perform real-time, dynamic optimization of sensor weights.

[0004] Therefore, it can be seen that the existing technical methods have insufficient adaptability when sensors are abnormal or fail, and it is difficult to dynamically adjust the weights of multiple sensors in real time. Summary of the Invention

[0005] In view of this, embodiments of this application provide a multi-sensor fusion pose determination method, apparatus, electronic device, and storage medium to solve the problem in the prior art that the adaptability is insufficient when sensors are abnormal or fail, and it is difficult to dynamically adjust the weights of multiple sensors in real time.

[0006] A first aspect of this application provides a multi-sensor fusion pose determination method. The method includes: acquiring sensor data corresponding to multiple heterogeneous sensors at the current time; inputting the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result; mapping each initial pose result to a target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result; generating time-series data based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result; inputting the time-series data into a pre-trained multi-sensor fusion model; and processing the time-series data using a multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time. A second aspect of this application provides a multi-sensor fusion pose determination device, comprising: a calculation module, configured to acquire sensor data corresponding to multiple heterogeneous sensors at the current time, and input the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result; a mapping module, configured to map each initial pose result to a target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result, and generate time series data based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result; and a determination module, configured to input the time series data into a pre-trained multi-sensor fusion model, and process the time series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time.

[0007] A third aspect of this application provides 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 computer program to implement the steps of the above-described method.

[0008] A fourth aspect of this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described method.

[0009] The beneficial effects of this application's embodiments compared to the prior art are as follows: The method of this application acquires sensor data at the current moment and inputs the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result; it maps each initial pose result to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result, and generates time series data based on multiple unified reference pose results and the confidence parameter corresponding to each initial pose result; it inputs the time series data into a pre-trained multi-sensor fusion model, and uses the multi-head self-attention mechanism in the multi-sensor fusion model to process the time series data to obtain the target pose result at the current moment. On the one hand, when calculating the initial pose result, this application not only independently solves the position and attitude in the spatial dimension, but also simultaneously outputs a confidence parameter characterizing the reliability of the initial pose result. This ensures that the time series data input to the multi-sensor fusion model not only includes the physical position of the unified reference pose result, but also carries the quality label of each unified reference pose result. This provides the multi-head self-attention mechanism in the multi-sensor fusion model with extremely accurate prior evaluation criteria when allocating feature weights, significantly reducing the illusion risk in the multi-modal fusion process. It allows the multi-sensor fusion model to dynamically set better feature weights based on the real-time state of each sensor, fundamentally avoiding the problem of insufficient adaptability in existing technologies when sensors are abnormal or fail, and the difficulty in dynamically adjusting the weights of multiple sensors in real time. Furthermore, when dealing with sensor data generated by heterogeneous sensors with dispersed physical locations, after generating the initial pose result, this application eliminates spatial lever errors through mapping the target carrier coordinate system, ensuring absolute alignment of spatial features. Subsequently, the spatially unified pose and confidence parameters are fused and processed in chronological order to generate time-series data of a unified dimension. This pre-processing mechanism of spatiotemporal unification provides a fair, standard, and distortion-free feature observation matrix for the backend multi-sensor fusion model, releasing the potential of the multi-sensor fusion model in fitting multi-source data and significantly improving the output accuracy of the final target pose result. This further avoids the problem of insufficient adaptability when sensors malfunction or fail, making it difficult to dynamically adjust the weights of multiple sensors in real time. Attached Figure Description

[0010] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0011] Figure 1This is a flowchart illustrating a multi-sensor fusion pose determination method provided in an embodiment of this application; Figure 2 This is a schematic diagram of another multi-sensor fusion pose determination device provided in the embodiments of this application; Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0012] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, multi-sensor fusion pose determination apparatuses, circuits, and methods are omitted to avoid unnecessary detail that may obscure the description of this application.

[0013] The following describes in detail, with reference to the accompanying drawings, a multi-sensor fusion pose determination method and a multi-sensor fusion pose determination device according to embodiments of this application.

[0014] Figure 1 This application provides a multi-sensor fusion pose determination method, such as... Figure 1 As shown, the method includes: S101. Obtain sensor data corresponding to multiple heterogeneous sensors at the current time, and input the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and the confidence parameter corresponding to each initial pose result.

[0015] It is understood that the multi-sensor fusion pose determination method and its steps provided in this application can be executed by a server or a terminal device alone, or by a server and a terminal device working together. The terminal device includes, but is not limited to, electronic devices with data processing capabilities such as in-vehicle computing platforms, autonomous driving domain controllers, UAV flight control motherboards, mobile robot main control boards, intelligent vehicle systems, or smartphones. For the sake of brevity and clarity, subsequent embodiments will use a terminal device with high computing power (e.g., an in-vehicle computing platform) as the execution subject to describe the method provided in this application.

[0016] For example, a target carrier (such as a vehicle or aircraft) carries multiple heterogeneous sensors, and the aforementioned terminal equipment is deployed inside the target carrier. The terminal equipment communicates with the multiple heterogeneous sensors via wired or wireless networks (such as carrier Ethernet, CAN bus, etc.). Specifically, the aforementioned heterogeneous sensors include, but are not limited to, at least two of the following: Global Navigation Satellite System (GNSS) receivers, inertial measurement units (IMUs), lidar, and visual cameras, gravity sensors, magnetic sensors, infrared sensors, etc. The sensor data in this application refers to the raw observation data collected by the aforementioned multiple sensors at the current moment, and the terminal equipment acquires this sensor data in real time through the aforementioned communication connection.

[0017] Understandably, to eliminate time misalignment at the hardware acquisition level for multi-source heterogeneous data, the terminal device first performs time synchronization on each sensor (e.g., hardware-level triggered synchronization or software-level timestamp alignment). For example, this application can utilize the pulse-of-seconds (PPS) signal or Precise Time Protocol (PTP) output by the Global Navigation Satellite System (GNSS) to assign a unified global timestamp to all received raw observation data, thereby ensuring that all sensor data input into the subsequent attitude calculation algorithm has physical consistency in time reference.

[0018] Furthermore, due to the different working principles of various sensors, this application employs multiple corresponding pose calculation algorithms in parallel to independently process the aforementioned sensor data. For example, inertial navigation algorithms can be used to process IMU data, while Simultaneous Localization and Mapping (SLAM) algorithms can be used to process laser point clouds or visual images. After independent processing by the aforementioned pose calculation algorithms, the terminal device can obtain the initial pose result calculated by each algorithm (i.e., the current position and attitude of the carrier from the perspective of that sensor).

[0019] Meanwhile, because sensors are subject to external environmental interference during operation (such as GNSS signal obstruction or camera sudden changes in lighting), each pose calculation algorithm, while outputting the pose result, also evaluates and outputs a "confidence parameter" based on its internal algorithm operation status. This confidence parameter characterizes the reliability of the initial pose result currently output by the algorithm, providing an evaluation basis for subsequent fusion.

[0020] S102. Map each initial pose result to the target carrier coordinate system to obtain the unified reference pose result corresponding to each initial pose result, and generate time series data based on multiple unified reference pose results and the confidence parameters corresponding to each initial pose result. In this embodiment, considering the actual physical installation of each sensor, the sensors are typically distributed at different locations on the carrier (for example, taking a vehicle as an example, the lidar is usually installed on the roof, and the IMU is usually installed on the chassis). Therefore, the initial pose results output by each algorithm have spatial lever errors or inconsistent origins. In order to enable the subsequent multi-sensor fusion model to compare and fuse data under a fair and unified benchmark, this application first aligns the spatial dimensions of each initial pose result.

[0021] Specifically, through coordinate mapping transformation, the original scattered initial pose results of all sensors are uniformly translated or rotated to the core reference point of the carrier (i.e., the target carrier coordinate system, such as the vehicle's center of mass), thereby obtaining a unified reference pose result that eliminates spatial physical differences.

[0022] After spatial unification, this application concatenates and fuses multiple unified reference pose results acquired at the same time with their corresponding confidence parameters (each unified reference pose result corresponds to an initial pose result, and each initial pose result corresponds to a confidence parameter; therefore, each unified reference pose result corresponds to a confidence parameter). Subsequently, these fused multidimensional features are continuously sampled or buffered in chronological order, thereby constructing time-series data with a certain historical length in the time dimension. This time-series data not only contains the spatial motion trend of the carrier but also includes dynamic changes in the reliability of each sensor (confidence parameters).

[0023] S103. Input the time series data into the pre-trained multi-sensor fusion model, and use the multi-head self-attention mechanism in the multi-sensor fusion model to process the time series data to obtain the target pose result at the current time.

[0024] In this embodiment, traditional fixed-weight fusion algorithms struggle to handle sudden sensor failures in complex scenarios. Therefore, this application introduces a pre-trained multi-sensor fusion model based on deep learning. Time-series data is input as the feature matrix into this multi-sensor fusion model. The core of this multi-sensor fusion model lies in its internally constructed multi-head self-attention mechanism. This mechanism overcomes the limitations of traditional local window filtering and extracts deep correlation features from the global spatiotemporal dimension of each unified reference pose result.

[0025] Specifically, since the deep learning network of the multi-sensor fusion model itself lacks the ability to process sequence order, the multi-sensor fusion model activates its core component, the multi-head self-attention mechanism. This mechanism, like a human expert, autonomously analyzes the data consistency of various unified reference pose results within the current timeframe and historical time windows in a high-dimensional feature space. In particular, by combining the confidence parameter in the accompanying time-series data, this multi-head attention mechanism can dynamically evaluate unified reference pose results currently experiencing data drift and the most reliable unified reference pose result. Based on this global context awareness capability, the multi-head self-attention mechanism can automatically assign optimal data feature weights to different unified reference pose results within milliseconds. Specifically, the multi-sensor fusion model assigns high weights to unified reference pose results with high confidence and good historical consistency, and extremely low weights to abnormal unified reference pose results.

[0026] Finally, through nonlinear regression calculations at the back end of the multi-sensor fusion model (e.g., via a fully connected network layer), the multi-sensor fusion model transforms the weighted multimodal features into a unique and highly accurate target pose result at the current moment, thereby achieving highly robust localization in complex dynamic environments.

[0027] According to the solution provided in this application, the method acquires sensor data at the current moment and inputs the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result. Each initial pose result is mapped to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result. Time series data is generated based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result. The time series data is input into a pre-trained multi-sensor fusion model, and the multi-head self-attention mechanism in the multi-sensor fusion model is used to process the time series data to obtain the target pose result at the current moment. On the one hand, when calculating the initial pose result, this application not only independently solves the position and attitude in the spatial dimension but also simultaneously outputs a confidence parameter characterizing the reliability of the initial pose result. This ensures that the time series data input to the multi-sensor fusion model not only includes the physical position of the unified reference pose result but also carries the quality label of each unified reference pose result. This provides the multi-head self-attention mechanism in the multi-sensor fusion model with extremely accurate prior evaluation criteria when allocating feature weights, significantly reducing the illusion risk in the multi-modal fusion process. It allows the multi-sensor fusion model to dynamically set better feature weights based on the real-time state of each sensor, fundamentally avoiding the problem of insufficient adaptability in existing technologies when sensors are abnormal or fail, and the difficulty in dynamically adjusting the weights of multiple sensors in real time. Furthermore, when dealing with sensor data generated by heterogeneous sensors with dispersed physical locations, after generating the initial pose result, this application eliminates spatial lever errors through mapping the target carrier coordinate system, ensuring absolute alignment of spatial features. Subsequently, the spatially unified pose and confidence parameters are fused and processed in chronological order to generate time-series data of a unified dimension. This pre-processing mechanism of spatiotemporal unification provides a fair, standard, and distortion-free feature observation matrix for the backend multi-sensor fusion model, releasing the potential of the multi-sensor fusion model in fitting multi-source data and significantly improving the output accuracy of the final target pose result. This further avoids the problem of insufficient adaptability when sensors malfunction or fail, making it difficult to dynamically adjust the weights of multiple sensors in real time.

[0028] To better illustrate the above pose calculation algorithm and the process of extracting confidence parameters, this application provides the following more specific examples for further explanation.

[0029] In some examples, the pose calculation algorithm includes: an integrated navigation algorithm; the sensor data includes: satellite navigation data and inertial measurement data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: S201. If the pose calculation algorithm is an integrated navigation algorithm, then the satellite navigation data and inertial measurement data are input into the integrated navigation algorithm to calculate the initial pose result corresponding to the integrated navigation algorithm. The inertial measurement data includes, but is not limited to, the triaxial acceleration of the target vehicle at the current moment. and triaxial angular velocity Satellite navigation data includes absolute position and absolute velocity observations. The initial pose output by the integrated navigation algorithm consists of longitude, latitude, and elevation, while the velocity includes northward velocity, eastward velocity, and azimuth (or downward) velocity. The attitude is represented using quaternions.

[0030] In this embodiment, the terminal device uses inertial measurement data to perform inertial navigation mechanical orchestration calculations, and recursively updates attitude, velocity, and position using the following formulas: The attitude update formula is as follows: ; The speed update formula is: ; The location update formula (including elevation, latitude, and longitude) is as follows: ; ; ; in, ; In the formula, n is the local navigation coordinate system, and b is the IMU coordinate system. for The attitude transformation relationship between the IMU frame and the local navigation coordinate system at any given time. for Local navigation coordinates relative to time The attitude transformation relationship in the local navigation coordinate system at any given time. for The attitude transformation relationship between the IMU frame at a given time and the local navigation coordinate system at the same time. for Time IMU system relative to Attitude transformation relationship of the IMU system at any time (based on three-axis angular velocity) (obtained by integral calculation) This is quaternion multiplication. and They are respectively Time and The speed in the local navigation coordinate system at any given time. for The proportional integral term and the force integral term at time (from triaxial acceleration) (obtained by integral calculation) for The gravity / Gorbachev integral term at time t. and They are respectively Time and Elevation at any moment and They are respectively Time and The dimension of time, and They are respectively Time and The northbound speed at any given moment, for The radius of the meridian at any given time. and They are respectively Time and Longitude of time and They are respectively Time and The eastward speed at any given moment, for The radius of the circle at time of day. yes Time and The average elevation at any given time. Time and The average value of latitude at any given time.

[0031] Based on the above formula, the terminal device can then use the previous time... The historical pose and state, combined with the current moment The collected inertial measurement data is used for calculus recursion to accurately calculate the prior pose result at the current moment. Subsequently, the terminal device uses satellite navigation data as observation data and employs fusion algorithms such as error state Kalman filtering to update and correct the prior pose result, ultimately obtaining the initial pose result of the integrated navigation algorithm at the current moment.

[0032] S202. Based on the initial pose result, update the state transition matrix and error matrix, and determine the confidence parameter corresponding to the initial pose result based on the updated state transition matrix and error matrix.

[0033] In this embodiment, the underlying framework of the integrated navigation algorithm employs error-state Kalman filtering. The terminal device uses the diagonal elements of the covariance matrix corresponding to the initial pose result to quantify the system's uncertainty and obtain the confidence parameter. Specifically, the confidence parameter output by the integrated navigation at the current moment... Elements taken from the main diagonal of the covariance matrix P during the Kalman filter prediction update process (i.e., the variance of each error state).

[0034] The time update (iterative prediction) formula for the covariance matrix P is as follows: ; In the formula, and They are respectively Time and The covariance matrix at time t, and They are respectively Time's up The state transition matrix at time t and its transpose. and They are respectively Error transition matrix at time step and its transpose. for Error matrix at time step.

[0035] It should be noted that the covariance matrix P and the state transition matrix... Error transition matrix and error matrix The initial values ​​(i.e., the initial calibration values ​​assigned during sensor cold start initialization or system restart and reset) are all parameters pre-calibrated and defined by the system. During the continuous recursive operation of the integrated navigation system, its iterative updates are closely dependent on the current physical motion state of the carrier.

[0036] Specifically, in the state transition matrix The preset partial derivative calculation formula (Jacobi matrix) contains elements composed of nonlinear variables such as the real-time position, velocity, and attitude of the carrier. Therefore, the terminal device uses the initial pose result calculated in the aforementioned step (S201) (such as the quaternion attitude and velocity at the current moment) as known independent variables, substituting them into the definition formulas of the state transition matrix and error matrix to accurately calculate the current position. The value (it is worth noting that in the formula) This is merely the transpose of the state transition matrix, obtained by directly transposing the matrix at the current time step.

[0037] Based on the updated state transition matrix, the terminal device recursively calculates the covariance matrix P at the current time using the formula described above. The terminal device further extracts the main diagonal elements of this covariance matrix. Since the diagonal element represents the variance of each pose error dimension, the smaller the value, the higher the accuracy of the system's current pose estimation. Therefore, the system directly uses the value of the main diagonal element (or its inverse mapping value) as the confidence parameter output by the integrated navigation algorithm, providing a reliable numerical basis for the dynamic weighting of the subsequent large model.

[0038] In the case of a pose calculation algorithm that is an integrated navigation algorithm, the above-mentioned scheme of this application inputs satellite navigation data and inertial measurement data into the integrated navigation algorithm to calculate the initial pose result corresponding to the integrated navigation algorithm. Based on the initial pose result, the state transition matrix and error matrix are updated, and the confidence parameter corresponding to the initial pose result is determined based on the updated state transition matrix and error matrix. This achieves accurate acquisition of the initial pose result and the corresponding confidence parameter under the integrated navigation algorithm.

[0039] In some examples, the pose calculation algorithm includes: a laser simultaneous localization and mapping algorithm; the sensor data includes: laser point cloud data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: S301. If the pose calculation algorithm is a laser real-time localization and mapping algorithm, then the laser point cloud data is input into the laser real-time localization and mapping algorithm to calculate the relative pose change between consecutive frames in the laser point cloud data, and the initial pose result corresponding to the laser real-time localization and mapping algorithm is obtained. Specifically, in step S301 above, the terminal device receives continuous frame point cloud data output by the high-frequency scanning of the lidar. This instantaneous lidar localization and mapping algorithm typically first preprocesses the lidar point cloud data (e.g., voxel filtering downsampling) and extracts features (e.g., extracting edge and planar feature points). Subsequently, a point cloud registration algorithm is used to calculate the optimal translation vector and rotation matrix between the current frame's feature point cloud and the previous frame's feature point cloud (or the constructed local target sub-map).

[0040] Commonly used point cloud registration algorithms include the Iterative Closest Point (ICP) algorithm and the Normal Distribution Transform (NDT) algorithm. This embodiment takes the Normal Distribution Transform method as an example. For each point in the current frame of the laser point cloud data... The formula for calculating its probability density function is: ; In the formula, For a point in the current frame's point cloud, use a 3D vector express, Let be the mean vector of the corresponding voxels in the target point cloud distribution where the point falls. Let be the covariance matrix of a voxel in the target point cloud distribution. Point The degree of matching with the target point cloud distribution.

[0041] During the registration process, the pose transformation T is optimized by minimizing the difference between the current frame point cloud and the target point cloud distribution. T is a 4×4 matrix that includes rotation and translation.

[0042] ; In the formula, It is a 3×3 rotation matrix representing the rotation from the current frame to the target frame, and the pose information of the current frame can be recursively derived. It is a 3×1 translation vector that can be used to recursively derive the position of the current frame.

[0043] The terminal device transforms the point cloud registration pose optimization problem between the current frame and the target frame into a problem of finding the maximum or minimum value of the following objective function: ; In the formula, The optimal pose matrix is... The coordinate system of the target point cloud is transformed from the point cloud of the previous frame to the point cloud coordinate system through pose transformation. For matrix multiplication, Let be the probability density function. The objective function is minimized by optimizing the pose change T.

[0044] After the above nonlinear optimization iteration converges, the optimal pose matrix is ​​obtained. (Minimized pose matrix), which is the laser pose result output by the laser real-time localization and mapping algorithm at the current moment. What is understandable is the optimal pose matrix. This indicates the relative pose change between consecutive frames in the laser point cloud data (i.e., the current frame point cloud acquired at the current moment, the previous frame point cloud acquired at the previous moment, or a local target point cloud map constructed based on historical frames).

[0045] S302. Based on the initial pose result, transform the current frame point cloud to the target point cloud coordinate system. Calculate the point cloud matching score based on the matching difference between the transformed current frame point cloud and the target point cloud distribution, and use the point cloud matching score as the confidence parameter corresponding to the initial pose result.

[0046] Specifically, after obtaining the optimal pose matrix (i.e., the initial pose result) through the above steps (S301), the terminal device uses this optimal pose matrix. All feature points in the current frame point cloud are uniformly mapped and projected to the relative spatial coordinate system corresponding to the target point cloud (such as a local environment sub-image or the previous frame point cloud).

[0047] Subsequently, the point cloud matching score is calculated based on the convergence state of the registration algorithm at the optimal pose. Taking the Normal Distribution Transform (NDT) algorithm mentioned above as an example, the point cloud matching score can be directly taken from the sum of the global probability density distributions when the objective function converges; or, if other registration algorithms such as Iterative Closest Point (ICP) are used, the average spatial Euclidean distance error or the inliers ratio between the target point and the mapped point after registration convergence can be extracted.

[0048] The point cloud matching score directly and sensitively reflects the quality of the current lidar observation data and the richness of the surrounding environment's geometric features at the physical level. For example, when the target vehicle travels through typical degraded scenarios such as long straight corridors, tunnels, or large open areas without features, the point cloud is prone to slip mismatch in specific directions. In this case, although the algorithm may barely output the pose matrix, its probability density score will drop significantly, or the spatial matching error will increase dramatically.

[0049] Therefore, the terminal device extracts the matching convergence score output by the algorithm as the point cloud matching score. If the score is in error / difference form, it is processed by taking the reciprocal or negative correlation mapping; if the score is in probability / overlap form, it is directly extracted or subjected to positive correlation normalization. Finally, the processed numerical result is directly used as the confidence parameter output by the laser real-time localization and mapping algorithm at the current moment.

[0050] According to the solution provided in the embodiments of this application, when the pose calculation algorithm is a laser simultaneous localization and mapping (LSTM) algorithm, the laser point cloud data is input into the LTM algorithm to calculate the relative pose change between consecutive frames in the laser point cloud data, thereby obtaining the initial pose result corresponding to the LTM algorithm. Then, based on the initial pose result, the current frame point cloud is transformed to the target point cloud coordinate system. The point cloud matching score is calculated based on the matching difference between the transformed current frame point cloud and the target point cloud distribution, and the point cloud matching score is used as the confidence parameter corresponding to the initial pose result. This achieves accurate acquisition of the initial pose result and the corresponding confidence parameter under the LTM algorithm.

[0051] In some examples, the pose calculation algorithm includes a visual real-time localization and mapping (VMR) algorithm; the sensor data includes a visible light image. When the pose calculation algorithm is a VMR algorithm, the visible light image is input into the VMR algorithm. By extracting visual feature points between consecutive frames and performing feature matching, the relative pose transformation of the camera is calculated, resulting in the initial pose result corresponding to the VMR algorithm. The confidence parameter corresponding to the initial pose result is determined based on the reprojection error or the number of tracked inliers during the visual feature matching process.

[0052] Specifically, in step S401 above, to overcome the degradation problem of pure visual SLAM in scenes with weak texture or fast motion, this embodiment adopts a tightly coupled visual-inertial joint optimization framework. The terminal device will use the triaxial acceleration data acquired at the current moment... Triaxial angular velocity and visible light images The visual SLAM algorithm for the data outputs the initial pose result. The number of tracked feature points , etc. as confidence parameters The algorithm constructs a joint optimization objective function based on a sliding window mechanism, and its core nonlinear optimization model is as follows: ; In the formula, As prior information, The Jacobian matrix corresponding to the prior information. For the variable to be optimized, for Time's up Pre-integral observations at time points The error (residual function) is constructed between the inertial observations and the variables to be optimized. Let the reprojection observation of the first spatial landmark on the j-th frame camera image be denoted as . The error (residual function) is constructed between the reprojected observations and the variables to be optimized.

[0053] in: ; ; In the formula, In the current sliding window The state vector to be optimized at any given time includes position, velocity, attitude, additive bias, and gyroscope bias. for It continuously outputs the position of the reference point in the world system. for Constantly output the velocity of the reference point in the world frame. for Constantly output the attitude of the reference point in the world system. for The triaxial accelerometer of the inertial measurement unit has zero bias at any given time. for The three-axis gyroscope of the instantaneous inertial measurement unit has zero bias. Let be the inverse depth of the m-th feature point.

[0054] The terminal device uses nonlinear optimization algorithms such as Gauss-Newton or Levenberg-Marquardt (LM) to solve for the optimal variables of the above objective function. ,in The included position, velocity, and attitude are The number of feature points visually tracked during the calculation process. , etc., are used as confidence parameters, denoted as .

[0055] In some examples, each initial pose result is mapped to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result, including: S401. Obtain the coordinate transformation matrix corresponding to each pose calculation algorithm. Through the coordinate transformation matrix corresponding to each pose calculation algorithm, map the reference point of the initial pose result corresponding to each pose calculation algorithm to the target carrier coordinate system to obtain the relative pose result. It is understandable that, because different pose calculation algorithms process different sensor data sources, the origin of the reference coordinate system established during internal calculation (i.e., the reference point for the initial pose result output by the pose calculation algorithm) will also be different. For example, visual real-time localization and mapping algorithms usually use the optical center of the camera as the reference point for pose calculation; laser real-time localization and mapping algorithms usually use the geometric center of the lidar as the calculation reference point; and integrated navigation algorithms usually use the measurement center of the inertial measurement unit (IMU) as the calculation reference point.

[0056] Therefore, the coordinate transformation matrix corresponding to each pose calculation algorithm here refers to the calibration extrinsic parameter matrix (including three-dimensional rotation matrix and translation vector) that maps the algorithm's unique solution reference point to the target vehicle's unique reference system (e.g., the target vehicle coordinate system with the vehicle's rear axle center or the aircraft's center of gravity as the origin). By calling the coordinate transformation matrix specific to each algorithm to perform geometric transformations on their respective initial pose results and performing strict lever compensation, the independent poses calculated by each pose calculation algorithm under different underlying reference systems are forcibly aligned and unified to the same physical reference point on the target vehicle. This completely eliminates the spatial geometric misalignment caused by differences in algorithm reference systems, resulting in a precise relative pose result mapped to the target vehicle coordinate system.

[0057] It should be noted that the coordinate transformation matrix corresponding to each pose calculation algorithm is pre-set based on the vehicle's factory physical parameters or offline calibration process. Taking the integrated navigation algorithm as an example, the calculation formula for uniformly mapping the initial pose result to the target vehicle's output reference point is: ; In the formula for The initial pose result output by the time-integrated navigation algorithm. This is the coordinate transformation matrix between the combined navigation coordinate system and the unified output reference point coordinate system. for The pose result output by the time-integrated navigation is transformed by matrix and mapped onto the unique output reference point of the target vehicle (i.e., the pose with the lever arm error eliminated).

[0058] Similarly, for the initial pose results of the LiDAR and visual camera, the system also uses their respective preset coordinate transformation matrices to perform the same mapping process, and combines the initial global anchor points to finally unify all pose results into the world absolute coordinate system in latitude and longitude format, which is denoted as the unified reference pose result standardized by the algorithm at the current moment. S402. Based on the geographic reference coordinates at the initial time, convert the relative pose result into a pose in a unified absolute coordinate system to obtain the unified reference pose result corresponding to each initial pose result.

[0059] Furthermore, although the physical reference points on the target vehicle are unified through step S401, the global coordinate system references output by different pose calculation algorithms still exhibit heterogeneity. Specifically, laser or visual SLAM algorithms typically output a local metric coordinate system (Local Cartesian Coordinate System) with the origin at system startup as the absolute reference, while the integrated navigation algorithm outputs a global latitude and longitude absolute coordinate system (such as WGS84). To enable the multi-sensor fusion model in the backend to align features within the same observation space, the terminal device extracts the absolute latitude and longitude and initial heading angle at system startup (or after initial alignment) and uses them as the geographic reference coordinates at the starting moment (i.e., the global absolute anchor point). Based on this global absolute anchor point, the terminal device uses matrix mapping operations such as rotation and translation to project and transform the local metric relative trajectories output by each SLAM algorithm to a global absolute coordinate system consistent with the integrated navigation (such as the unified Northeast ENU coordinate system).

[0060] For example, this application uses the absolute latitude and longitude of the starting frame location as the reference origin, and converts the metric relative coordinate system established by the above SLAM algorithm, with the eastward direction as the positive X-axis and the northward direction as the positive Y-axis, into global absolute latitude and longitude coordinates. The specific nonlinear projection mapping formula is as follows: ; ; In the formula, and They are respectively The latitude values ​​of the time and the start time. for The relative Y-axis coordinate of the time in the metric coordinate system for The radius of the meridian at any given time. and They are respectively The longitude values ​​of the time and the start time, for The x-axis coordinate relative to the metric coordinate system at any given time. for The radius of the circle at any given time.

[0061] Thus, the output results of various pose calculation algorithms not only eliminated the lever error in the physical space of the target carrier, but also achieved metric unification in the global absolute space, ultimately generating a unified reference pose result without any spatial geometric ambiguity, providing an extremely pure and fair feature observation benchmark for subsequent input into the multi-sensor fusion model.

[0062] In some examples, time series data are generated based on multiple unified reference pose results and the confidence parameter corresponding to each initial pose result, including: S501. Obtain the data output frequency of the sensor data, and determine the reference time axis based on the data output frequency of the sensor data; Specifically, due to the different sampling mechanisms of various heterogeneous sensor hardware, their data output frequencies vary significantly (for example, inertial measurement units typically output at high frequencies above 100Hz, visual cameras at mid-frequency frequencies around 30Hz, and LiDAR at low-frequency frequencies around 10Hz). To preserve the system's high-frequency dynamic information to the greatest extent possible without downsampling loss, the terminal device can extract the highest data output frequency from all running pose calculation algorithms (e.g., using the high-frequency timestamp of integrated navigation) as the system's global time resolution, thereby constructing a high-density reference time axis.

[0063] Considering the limitations of computing resources and system requirements in different application scenarios, terminal devices can also adopt any of the following frequency strategies to determine the reference time axis: Firstly, the median or arithmetic mean of all data output frequencies can be selected. By constructing a moderately dense baseline time axis, the matrix dimension of the time series data (Tensor) can be effectively controlled while preserving multi-source dynamic observation information as much as possible, thereby achieving an optimal balance between model prediction accuracy and the computing power overhead of edge computing hardware.

[0064] Secondly, the lowest frequency among all data output frequencies can be selected (e.g., 10Hz for LiDAR). Under this strategy, the system can force all features to align to the low-frequency time axis by performing local pre-integration or downsampling pooling on the high-frequency combined navigation data. The feature matrix generated in this way is the most lightweight and is extremely suitable for low-cost mobile robot platforms with extremely stringent requirements for power consumption and computing power.

[0065] Third, a preset core dominant sensor frequency or the target control frequency of the downstream system can be selected. For example, in a vision-driven autonomous driving domain controller, the 30Hz frequency of the vision camera can be directly used as the reference frequency; or, based on the actual control refresh rate required by the downstream trajectory planning and control module (such as a fixed 50Hz period), this 50Hz frequency can be directly used as the resolution of the reference time axis. This on-demand setting strategy can minimize the communication latency and resampling loss between the large model output and the downstream control module.

[0066] S502. Align the unified reference pose results and corresponding confidence parameters with the reference time axis using timestamps. In this embodiment, this is a crucial heterogeneous frequency fusion preprocessing step. The terminal device matches the unified reference pose results and their confidence parameters output by various pose calculation algorithms to the corresponding nodes on the reference time axis according to their respective timestamps.

[0067] It is understandable that during the alignment process with the high-frequency reference time axis, low-frequency sensors (such as LiDAR) will inevitably experience data gaps (no observation updates) at a large number of time points. To avoid traditional brute-force interpolation algorithms fabricating data out of thin air and inducing illusions in the backend AI model, this application abandons the conventional interpolation smoothing strategy. Specifically, for pose calculation algorithms that lack observation data at a certain reference time point, the terminal device directly fills the corresponding pose and confidence feature bits with preset special identifier values ​​(such as NaN invalid values, Inf infinity values, or all-zero placeholders). This absolute alignment mechanism of "filling in real observations and placing placeholders for missing moments" ensures that the data at each time point reflects the true measurement state of the physical world.

[0068] Conversely, if a mid-frequency or low-frequency frequency is used as the reference time axis, there will inevitably be redundancy issues with sensor data (such as high-frequency inertial measurement data) whose output frequency is higher than the reference time axis. For such high-frequency overflow data, the terminal device can use downsampling or aggregation methods for alignment processing.

[0069] Take downsampling or discarding as an example. The terminal device only retains the observation data closest to the reference timeline node (timestamp), while directly removing / discarding redundant high-frequency observation frames between the two reference nodes. This strategy greatly reduces the computational dimensionality of the data and is suitable for scenarios with limited computing power.

[0070] Taking averaging or aggregation as an example, to fully utilize the transient physical motion constraints inherent in high-frequency data and avoid the loss of high-frequency information (such as anti-aliasing loss) caused by direct discarding, the terminal device fuses multiple high-frequency data frames falling within the same reference time interval. For example, by calculating the arithmetic mean, weighted average, or using an inertial pre-integration algorithm, two or more high-frequency data (such as continuous poses at 100Hz) are dimensionality-reduced and aggregated into an equivalent low-frequency observation node (such as 50Hz). This strategy perfectly achieves timestamp alignment while preserving the physical motion continuity of the carrier to the greatest extent.

[0071] S503. The aligned unified reference pose result and confidence parameters are segmented according to the preset time step to generate time series data with unified dimensions.

[0072] Furthermore, to adapt to the standardized tensor input requirements of deep learning models, the terminal device transforms the data on the reference time axis into a feature matrix. Specifically, at the same time node, the terminal device concatenates and fuses the unified reference pose results calculated by various pose calculation algorithms with their built-in confidence parameters to form a joint feature vector at that moment. Subsequently, according to a pre-set time step (e.g., a sliding window size of N frames), the continuous feature vectors are segmented and packaged to finally generate a two-dimensional matrix with the shape of [time step, joint feature dimension], which is the time series data with a unified dimension.

[0073] Understandably, the generated time-series data perfectly matches the input paradigm of subsequent multi-sensor fusion models (such as the Transformer architecture). When time-series data containing special placeholders like NaN is input into the model, the multi-head self-attention mechanism within the model can automatically identify these special values ​​and force their corresponding weights to zero (mask them) when calculating the attention distribution. This not only completely solves the code tensor alignment error caused by inconsistent sampling frequencies of heterogeneous sensors, but also gives the multi-sensor fusion model a powerful ability to adaptively ignore missing data.

[0074] In some examples, before inputting time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time, the method also includes: S601. Obtain the sample time series data and the sample reference pose result corresponding to the sample time series data; Specifically, during the training phase, the terminal device (or cloud training server) collects historical sensor data equipped with various heterogeneous sensors. After undergoing the same preprocessing and timestamp alignment steps as described above, sample time series data is formed. Simultaneously, the terminal device acquires the absolute pose output by a high-precision integrated navigation reference system (e.g., a centimeter-level high-precision trajectory obtained by post-processing bidirectional adjustment using RTK-GNSS and high-precision laser / fiber inertial navigation), and uses it as the sample reference pose result (true pose label) that is strictly time-synchronized with the sample time series data.

[0075] S602. Input the sample time series into the multi-sensor fusion model to be trained to obtain the predicted pose result; Considering time series data (denoted as) The temporal dependence of the multi-sensor fusion model to be trained utilizes the core operator of the multi-head self-attention mechanism. Perform a linear mapping and calculate the attention score. The calculation formula for the single-head attention mechanism is as follows: ; ; In the formula, Let be the query matrix. For the key matrix, For the value matrix, , , These are the linear transformation coefficient matrices that the model can learn during training. The number of features for each form is input_dimensions. For the standardized function, This is the calculated single-head attention feature matrix.

[0076] The attention mechanism described above can be repeated multiple times at the input layer of the self-attention mechanism, and different attention methods can be used. , , The weight initialization combination involves repeatedly computing the above operators in parallel multiple times, thus forming a multi-head attention mechanism. This multi-head attention mechanism can be repeated in multiple layers. By stacking multiple layers of the multi-head attention mechanism, a deep fusion feature matrix containing global spatiotemporal correlations can be extracted. The data is then fed into the subsequent deep learning sub-network of the multi-sensor fusion model to be trained, yielding the predicted pose. Based on the characteristics and actual computing power requirements, the subsequent deep learning large model sub-network can choose different architectures such as convolutional neural network (CNN), recurrent neural network (RNN) or long short-term memory network (LSTM) for the final regression prediction.

[0077] S603. Construct a loss function based on the predicted pose result and the sample reference pose result, and take minimizing the loss function as the optimization objective. Update the multi-head self-attention mechanism of the multi-sensor fusion model to be trained and the network parameters of the multi-sensor fusion model to be trained through backpropagation until the preset convergence condition is met, and obtain the multi-sensor fusion model.

[0078] When training the multi-sensor fusion model, a loss function is constructed based on the predicted pose and the sample baseline pose. The weight matrix of the multi-head self-attention mechanism and the parameters of the neural network are trained with the goal of minimizing the loss function. The formula for calculating the objective function is as follows: ; In the formula, The overall optimization objective function (i.e., loss function) is constructed, where N is the total number of samples in the training batch. The true benchmark for the i-th sample Data (i.e., high-precision reference pose results). The target data (i.e., the predicted pose result) for the i-th sample. For regularization terms, This is a function that measures the error between the predicted data and the actual baseline data (such as mean square error, absolute error, etc.).

[0079] By calculating the gradient of the objective function and performing backpropagation, the system iteratively updates the parameters within the large model. When the objective function converges and stabilizes, it is determined that the preset convergence condition has been met, and training is complete.

[0080] The trained multi-sensor fusion model can then be deployed to the target carrier terminal. In actual operation, based on real-time acquired multi-source heterogeneous sensor time-series data, the trained multi-sensor fusion model uses a multi-head self-attention mechanism to weight and merge the poses of each sensor and their inherent confidence parameters in the sample time-series data, and outputs the target pose result at the current moment with high accuracy and robustness. .

[0081] In some examples, after inputting time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose at the current time, the method also includes: S701. The target pose result is used as a global observation reference value and synchronously fed back to each pose calculation algorithm; Specifically, to address the bottleneck issue of the infinite accumulation of errors in the underlying sensors caused by the unidirectional flow of data in the Bimina multi-sensor fusion framework, this embodiment adopts a closed-loop feedback architecture. The terminal device uses the target pose result at the current moment, which has extremely high global confidence and accuracy and is calculated and output by the multi-sensor fusion model (large model), as an equivalent high-precision absolute observation benchmark (Virtual Ground Truth), and synchronously transmits it back to the various pose calculation algorithm modules at the front end of the system through the feedback link.

[0082] S702. When the pose calculation algorithm is an integrated navigation algorithm, calculate the pose difference based on the target pose result and the initial pose result currently output by the integrated navigation algorithm, and construct the observation vector. In this embodiment, an integrated navigation algorithm employing Error State Kalman Filtering (ESKF) at the underlying level is used as an example. When the integrated navigation module (which runs the integrated navigation algorithm) receives the feedback signal, it performs a subtraction operation between its own prior pose (i.e., the aforementioned initial pose result) obtained through mechanical arrangement and recursion at the current moment and the target pose result fed back by the multi-sensor fusion model. This accurately quantifies the current calculation deviation of the underlying algorithm and constructs the observation vector for filter updates. The formula for constructing the observation vector is as follows: ; In the formula, for The observation vector at time t, for The prior initial pose result recursively output by the combined navigation algorithm at each time step. for The final target pose result output by the multi-sensor fusion model at any time. This indicates the difference between the calculated pose results.

[0083] S703. Using a preset filtering update algorithm and observation vector, perform a posteriori update on the error state vector inside the integrated navigation algorithm; wherein, the error state vector includes position and velocity error parameters, as well as device error parameters of the inertial sensor. Subsequently, the integrated navigation algorithm uses the standard Kalman filter update equation and the calculated Kalman gain to map the observation vector into a high-dimensional state space, and updates the internal error state vector with posterior measurements.

[0084] ; In the formula, Let be the posterior error state vector. The prior error state vector. for Gain matrix at time step for Measurement matrix at time.

[0085] During this process, the state vector It includes not only errors in the purely mathematical dimension, but more importantly, it includes error parameters of inertial sensor devices that characterize physical hardware defects, and state vectors. The complete matrix structure is represented as follows: ; In the formula, This is due to INS position error. For INS speed error, For INS attitude error, For zero bias of the gyroscope, To achieve zero bias in the accelerometer, This is the error of the gyroscope scaling factor. This is the accelerometer scaling factor error.

[0086] S704. The integrated navigation algorithm is calibrated using the updated error state vector to constrain the cumulative estimation error of the integrated navigation algorithm in subsequent time steps.

[0087] Finally, the terminal device injects the updated error state vector into the recursive reference of the integrated navigation algorithm. On the one hand, it directly compensates for the navigation parameters at the current moment using position, velocity, and attitude errors; on the other hand, it uses the updated device error parameters (zero bias) to reverse-calibrate the original high-frequency measurement values ​​output by the inertial measurement unit (IMU) at subsequent moments.

[0088] By using this closed-loop calibration mechanism that feeds back the multi-sensor fusion model to the hardware, when satellite navigation signals are lost or vision / laser signals are degraded, the underlying inertial navigation system can be ensured to perform calculus calculations based on clean IMU data that has been calibrated in real time by the multi-sensor fusion model. This effectively suppresses the error divergence caused by the calculus equations and significantly extends the system's high-precision passive calculation time in extremely harsh environments.

[0089] It is understood that steps S701 to S704 above only take the integrated navigation algorithm and its underlying error state Kalman filter framework as an example to elaborate on the closed-loop error feedback mechanism. For other pose calculation algorithms involved in this application (such as the aforementioned laser real-time localization and mapping algorithm, visual or visual-inertial real-time localization and mapping algorithm), the principle of feeding back the target pose result as a global absolute observation reference to its underlying optimization backend is exactly the same.

[0090] For example, after receiving the target pose result from synchronous feedback, laser or visual SLAM algorithms can use it as a high-weight global prior constraint node in Pose Graph Optimization (PGO) or Bundle Adjustment (BA) to perform deep backend joint calibration and update of the local map point cloud, visual landmarks, or sensor dynamic extrinsic parameters maintained within it. Since the underlying system architecture principle of this type of closed-loop feedback and error update is highly consistent with the aforementioned integrated navigation algorithm, it will not be elaborated upon here for the sake of brevity.

[0091] This application combines time-synchronized positioning and attitude determination sensor data from multiple sources and inputs it into different pose calculation algorithms. The initial pose results and confidence parameters from these algorithms are integrated, and a multi-head self-attention mechanism is used to preprocess the integrated data. The data and the self-attention structure are then synchronously fed into a deep learning model for training. During use, the trained multi-sensor fusion model outputs the final pose result and feeds it back to each pose calculation algorithm, adjusting the parameters within these algorithms to improve the pose accuracy and robustness of the multi-source fusion positioning and attitude determination system.

[0092] All of the above-mentioned optional technical solutions can be combined in any way to form the optional embodiments of this application, and will not be described in detail here.

[0093] Based on the same concept, this application also provides a multi-sensor fusion pose determination device, such as... Figure 2 As shown, the multi-sensor fusion pose determination device includes: The calculation module 201 is used to acquire sensor data corresponding to multiple heterogeneous sensors at the current moment, and input the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and the confidence parameter corresponding to each initial pose result. The mapping module 202 is used to map each initial pose result to the target carrier coordinate system to obtain the unified reference pose result corresponding to each initial pose result, and generate time series data based on multiple unified reference pose results and the confidence parameters corresponding to each initial pose result. The determination module 203 is used to input time series data into a pre-trained multi-sensor fusion model, and use the multi-head self-attention mechanism in the multi-sensor fusion model to process the time series data to obtain the target pose result at the current time.

[0094] In some examples, the pose calculation algorithm includes: an integrated navigation algorithm; the sensor data includes: satellite navigation data and inertial measurement data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: if the pose calculation algorithm is an integrated navigation algorithm, then the satellite navigation data and inertial measurement data are input into the integrated navigation algorithm to calculate the initial pose result corresponding to the integrated navigation algorithm; based on the initial pose result, the state transition matrix and error matrix are updated, and based on the updated state transition matrix and error matrix, the confidence parameter corresponding to the initial pose result is determined.

[0095] In some examples, the pose calculation algorithm includes a laser simultaneous localization and mapping (LSTM) algorithm; the sensor data includes laser point cloud data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: if the pose calculation algorithm is a laser simultaneous localization and mapping (LSTM) algorithm, the laser point cloud data is input into the laser simultaneous localization and mapping (LSTM) algorithm to calculate the relative pose change between consecutive frames in the laser point cloud data, and the initial pose result corresponding to the laser simultaneous localization and mapping (LSTM) algorithm is obtained; the current frame point cloud is transformed to the target point cloud coordinate system according to the initial pose result, the point cloud matching degree score is calculated based on the matching difference between the transformed current frame point cloud and the target point cloud distribution, and the point cloud matching degree score is used as the confidence parameter corresponding to the initial pose result.

[0096] In some examples, each initial pose result is mapped to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result. This includes: obtaining the coordinate transformation matrix corresponding to each pose calculation algorithm; mapping the reference point of the initial pose result corresponding to each pose calculation algorithm to the target carrier coordinate system using the coordinate transformation matrix corresponding to each pose calculation algorithm to obtain a relative pose result; and converting the relative pose result into a pose in a unified absolute coordinate system based on the geographic reference coordinates at the start time to obtain a unified reference pose result corresponding to each initial pose result.

[0097] In some examples, time series data is generated based on multiple unified reference pose results and the confidence parameters corresponding to each initial pose result. This includes: obtaining the data output frequency of the sensor data and determining the reference time axis based on the data output frequency of the sensor data; aligning each unified reference pose result and its corresponding confidence parameters to the reference time axis with timestamps; and segmenting the aligned unified reference pose results and confidence parameters according to a pre-set time step to generate time series data with a unified dimension.

[0098] In some examples, before inputting time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time, the device is also used to: acquire sample time-series data and the sample reference pose result corresponding to the sample time-series data; input the sample time-series data into the multi-sensor fusion model to be trained to obtain the predicted pose result; construct a loss function based on the predicted pose result and the sample reference pose result, and update the multi-head self-attention mechanism and network parameters of the multi-sensor fusion model to be trained through backpropagation with minimizing the loss function as the optimization objective, until the preset convergence condition is met, thus obtaining the multi-sensor fusion model.

[0099] In some examples, after inputting time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current moment, the device is also used to: use the target pose result as a global observation reference value and synchronously feed it back to each pose calculation algorithm; when the pose calculation algorithm is an integrated navigation algorithm, calculate the pose difference based on the target pose result and the initial pose result currently output by the integrated navigation algorithm, and construct an observation vector; use a preset filtering update algorithm and the observation vector to perform a posteriori update on the error state vector inside the integrated navigation algorithm; wherein, the error state vector includes position and velocity error parameters and device error parameters of the inertial sensor; and use the updated error state vector to calibrate the integrated navigation algorithm to constrain the cumulative estimation error of the integrated navigation algorithm at subsequent moments.

[0100] According to the solution provided in this application embodiment, the device acquires sensor data at the current moment and inputs the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result. Each initial pose result is mapped to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result. Time series data is generated based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result. The time series data is input into a pre-trained multi-sensor fusion model. The multi-head self-attention mechanism in the multi-sensor fusion model is first used to process the time series data to obtain the target pose result at the current moment. On the one hand, when calculating the initial pose result, this application not only independently solves the position and attitude in the spatial dimension but also simultaneously outputs a confidence parameter characterizing the reliability of the initial pose result. This ensures that the time series data input to the multi-sensor fusion model not only includes the physical position of the unified reference pose result but also carries the quality label of each unified reference pose result. This provides the multi-head self-attention mechanism in the multi-sensor fusion model with extremely accurate prior evaluation criteria when allocating feature weights, significantly reducing the illusion risk in the multi-modal fusion process. It allows the multi-sensor fusion model to dynamically set better feature weights based on the real-time state of each sensor, fundamentally avoiding the problem of insufficient adaptability in existing technologies when sensors are abnormal or fail, and the difficulty in dynamically adjusting the weights of multiple sensors in real time. Furthermore, when dealing with sensor data generated by heterogeneous sensors with dispersed physical locations, after generating the initial pose result, this application eliminates spatial lever errors through mapping the target carrier coordinate system, ensuring absolute alignment of spatial features. Subsequently, the spatially unified pose and confidence parameters are fused and processed in chronological order to generate time-series data of a unified dimension. This pre-processing mechanism of spatiotemporal unification provides a fair, standard, and distortion-free feature observation matrix for the backend multi-sensor fusion model, releasing the potential of the multi-sensor fusion model in fitting multi-source data and significantly improving the output accuracy of the final target pose result. This further avoids the problem of insufficient adaptability when sensors malfunction or fail, making it difficult to dynamically adjust the weights of multiple sensors in real time.

[0101] Figure 3 This is a schematic diagram of the electronic device 3 provided in an embodiment of this application. Figure 3As shown, the electronic device 3 in this embodiment includes a processor 301, a memory 302, and a computer program 303 stored in the memory 302 and executable on the processor 301. When the processor 301 executes the computer program 303, it implements the steps in the various method embodiments described above. Alternatively, when the processor 301 executes the computer program 303, it implements the functions of each module / unit in the various multi-sensor fusion pose determination device embodiments described above.

[0102] Electronic device 3 can be a desktop computer, laptop, handheld computer, cloud server, or other electronic device. Electronic device 3 may include, but is not limited to, processor 301 and memory 302. Those skilled in the art will understand that... Figure 3 This is merely an example of electronic device 3 and does not constitute a limitation on electronic device 3. It may include more or fewer components than shown, or different components.

[0103] The processor 301 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0104] The memory 302 can be an internal storage unit of the electronic device 3, such as a hard disk or memory of the electronic device 3. The memory 302 can also be an external storage device of the electronic device 3, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the electronic device 3. The memory 302 can also include both internal and external storage units of the electronic device 3. The memory 302 is used to store computer programs and other programs and data required by the electronic device.

[0105] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the multi-sensor fusion pose determination device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0106] If an integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program may include computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium may include: any entity capable of carrying computer program code, or a multi-sensor fusion pose determination device, recording medium, USB flash drive, portable hard drive, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to regional requirements and patent practice requirements. For example, in some regions, according to regional requirements and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0107] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for determining pose through multi-sensor fusion, characterized in that, The method includes: The sensor data corresponding to multiple heterogeneous sensors at the current moment is obtained, and the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and the confidence parameter corresponding to each initial pose result. Each initial pose result is mapped to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result. Time series data is generated based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result. The time series data is input into a pre-trained multi-sensor fusion model, and the multi-head self-attention mechanism in the multi-sensor fusion model is used to process the time series data to obtain the target pose result at the current time.

2. The method according to claim 1, characterized in that, The pose calculation algorithm includes: a combined navigation algorithm; the sensor data includes: satellite navigation data and inertial measurement data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: If the pose calculation algorithm is the integrated navigation algorithm, then the satellite navigation data and the inertial measurement data are input into the integrated navigation algorithm to calculate the initial pose result corresponding to the integrated navigation algorithm; Based on the initial pose result, the state transition matrix and error matrix are updated, and the confidence parameter corresponding to the initial pose result is determined based on the updated state transition matrix and error matrix.

3. The method according to claim 1, characterized in that, The pose calculation algorithm includes: a laser real-time localization and mapping algorithm; the sensor data includes: laser point cloud data; the sensor data is input into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result, including: If the pose calculation algorithm is the laser real-time localization and mapping algorithm, then the laser point cloud data is input into the laser real-time localization and mapping algorithm to calculate the relative pose change between consecutive frames in the laser point cloud data, and the initial pose result corresponding to the laser real-time localization and mapping algorithm is obtained. Based on the initial pose result, the current frame point cloud is transformed to the target point cloud coordinate system. The point cloud matching score is calculated based on the matching difference between the transformed current frame point cloud and the target point cloud distribution, and the point cloud matching score is used as the confidence parameter corresponding to the initial pose result.

4. The method according to claim 1, characterized in that, Mapping each initial pose result to the target carrier coordinate system yields a unified reference pose result corresponding to each initial pose result, including: Obtain the coordinate transformation matrix corresponding to each pose calculation algorithm, and map the reference point of the initial pose result corresponding to each pose calculation algorithm to the target carrier coordinate system through the coordinate transformation matrix corresponding to each pose calculation algorithm to obtain the relative pose result. Based on the geographic reference coordinates at the initial time, the relative pose results are converted into poses in a unified absolute coordinate system to obtain the unified reference pose result corresponding to each initial pose result.

5. The method according to claim 1, characterized in that, Based on the multiple unified reference pose results and the confidence parameter corresponding to each initial pose result, time series data is generated, including: Acquire the data output frequency of the sensor data, and determine the reference time axis based on the data output frequency of the sensor data; The unified reference pose results and the corresponding confidence parameters are timestamped to the reference time axis. The aligned unified reference pose result and the confidence parameter are segmented according to a pre-set time step to generate the time series data with a unified dimension.

6. The method according to claim 1, characterized in that, Before inputting the time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time, the method further includes: Obtain sample time series data and the sample reference pose results corresponding to the sample time series data; The sample time series is input into the multi-sensor fusion model to be trained to obtain the predicted pose result; Based on the predicted pose result and the sample reference pose result, a loss function is constructed. With minimizing the loss function as the optimization objective, the multi-head self-attention mechanism of the multi-sensor fusion model to be trained and the network parameters of the multi-sensor fusion model to be trained are updated through backpropagation until the preset convergence condition is met, and the multi-sensor fusion model is obtained.

7. The method according to claim 1, characterized in that, After inputting the time-series data into a pre-trained multi-sensor fusion model and processing the time-series data using the multi-head self-attention mechanism in the multi-sensor fusion model to obtain the target pose result at the current time, the method further includes: The target pose result is used as a global observation reference value and synchronously fed back to each of the pose calculation algorithms; When the pose calculation algorithm is an integrated navigation algorithm, the pose difference is calculated based on the target pose result and the initial pose result currently output by the integrated navigation algorithm, and an observation vector is constructed. Using a preset filtering update algorithm and the observation vector, the error state vector inside the integrated navigation algorithm is updated a posteriori; wherein, the error state vector includes position and velocity error parameters, as well as device error parameters of the inertial sensor; The integrated navigation algorithm is calibrated using the updated error state vector to constrain the cumulative estimation error of the integrated navigation algorithm at subsequent time points.

8. A multi-sensor fusion pose determination device, characterized in that, The device includes: The calculation module is used to acquire sensor data corresponding to multiple heterogeneous sensors at the current moment, and input the sensor data into multiple pose calculation algorithms to calculate multiple initial pose results and a confidence parameter corresponding to each initial pose result; The mapping module is used to map each initial pose result to the target carrier coordinate system to obtain a unified reference pose result corresponding to each initial pose result, and generate time series data based on multiple unified reference pose results and the confidence parameter corresponding to each initial pose result. The determination module is used to input time series data into a pre-trained multi-sensor fusion model, and use the multi-head self-attention mechanism in the multi-sensor fusion model to process the time series data to obtain the target pose result at the current time.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 7.

Images