Pose estimation method and apparatus, storage medium, and electronic device

Abstract

The application discloses a pose estimation method and device, a storage medium and an electronic device. It relates to the technical field of positioning. The method is used for a multi-sensor positioning system, the multi-sensor positioning system comprising a first sensor, a second sensor and a third sensor, the second sensor being a laser radar; the method comprising: acquiring first data, second data and third data, the first data being data acquired by the first sensor, the second data being data acquired by the second sensor, and the third data being data acquired by the third sensor; determining first pose information based on the first data; determining Kalman gain information based on the second data and the third data; performing Kalman filtering based on the Kalman gain information and the first pose information to obtain new pose information. When the visual angle of the laser radar changes suddenly, the accuracy of pose estimation is improved, thereby improving the robustness and estimation accuracy of the system in challenging scenarios.

Description

Technical Field

[0001] This application relates to the field of positioning technology, specifically to a pose estimation method, a pose estimation device, a machine-readable storage medium, and an electronic device. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology in fields such as autonomous driving and robot navigation. It estimates the pose of a vehicle in real time and builds an environmental map by fusing data from multiple sensors.

[0003] In recursive mapping frameworks such as Error State Kalman Filtering (ESKF) based on the Markov assumption, the system is highly dependent on historical data, and the pose estimation error accumulates continuously with the recursive process. If, at any given moment, insufficient point cloud matching leads to unreliable observation updates, the impact is not limited to the pose estimation deviation at that moment. It also severely interferes with the fusion results of subsequent frames through state propagation, causing further amplification of the error and highlighting the severity of the problem.

[0004] Existing technologies commonly employ multi-sensor fusion schemes, such as fusing data from inertial measurement units (IMUs), lidar (LiDAR), and visual cameras. Many existing technologies utilize a essentially serial fusion process: the IMU provides predictions, LiDAR acts as the primary source for intermediate updates, and vision then provides auxiliary corrections. This architecture is overly reliant on the LiDAR component, making the performance of other sensors dependent on LiDAR output. If LiDAR performance deteriorates due to scene characteristics, a deviation occurs in the middle of the fusion chain, propagating to the vision correction stage and creating a single point of bottleneck in system robustness. In scenarios where the LiDAR observation model temporarily fails, such as sudden changes in perspective, the overall performance of this architecture will significantly degrade.

[0005] In summary, existing SLAM fusion schemes struggle to maintain high-precision pose estimation when faced with sudden changes in LiDAR viewpoint. Summary of the Invention

[0006] The purpose of this application is to provide a pose estimation method, a pose estimation device, a machine-readable storage medium, and an electronic device to solve the problem in the prior art that it is difficult to maintain high-precision pose estimation when faced with sudden changes in LiDAR viewpoint.

[0007] To achieve the above objectives, a first aspect of this application provides a pose estimation method for a multi-sensor positioning system, the multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the method includes:

[0008] Acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor;

[0009] Based on the first data, the first pose information is determined;

[0010] Based on the second data and the third data, the Kalman gain information is determined;

[0011] Based on the Kalman gain information and the first pose information, Kalman filtering is performed to obtain new pose information.

[0012] In this embodiment of the application, determining the Kalman gain information based on the second data and the third data includes:

[0013] Based on the second data, the residual term, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined to obtain the first state estimation parameters.

[0014] Based on the third data, the residual term of the corresponding observation model, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined, and the second state estimation parameters are obtained.

[0015] Based on the first state estimation parameters and the second state estimation parameters, the gain calculation parameters are determined;

[0016] Based on the aforementioned gain calculation parameters, the Kalman gain information is calculated.

[0017] In this embodiment of the application, the matrix dimension in the first state estimation parameter is the same as the matrix dimension in the second state estimation parameter;

[0018] The step of determining the gain calculation parameters based on the first state estimation parameters and the second state estimation parameters includes:

[0019] Obtain point cloud data weights;

[0020] Based on the point cloud data weights, a gain calculation parameter model is determined;

[0021] Substitute the first state estimation parameters and the second state estimation parameters into the gain calculation parameter model to obtain the gain calculation parameters.

[0022] In this embodiment of the application, the gain calculation parameter model is as follows:

[0023] ,

[0024] ,

[0025] in, For point cloud data weights, Let be the Jacobian matrix of the residual terms of the point cloud observation model with respect to the Kalman filter independent variables. The covariance matrix of the point cloud observation model is... For the residual term of the observation model corresponding to the third data, This is the Jacobian matrix of the residual terms of the observation model corresponding to the third data point, expressed as a function of the Kalman filter independent variables. The second covariance matrix, For the residual term of the observation model corresponding to the third data, The first gain calculation parameter, The second gain calculation parameter, Let be the total Jacobian matrix. Let be the total covariance matrix. This is the actual observation data from the sensor. To update the iteration count, For the first The nominal state of the next update iteration. For the observation equation, These are the observations predicted using the nominal state.

[0026] In this embodiment of the application, obtaining point cloud data weights includes:

[0027] Based on the second data, the number of point clouds to be processed and the preferred number of point clouds are determined;

[0028] Based on the number of point clouds processed and the preferred number of point clouds, the point cloud distribution characteristic values ​​are determined;

[0029] The point cloud data weights are determined based on the point cloud distribution feature values ​​and thresholds.

[0030] In this embodiment of the application, the threshold includes a first threshold and a second threshold;

[0031] The step of determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds includes:

[0032] If the point cloud distribution feature value is greater than the first threshold, the weight of the obtained point cloud data is determined to be 1;

[0033] If the point cloud distribution feature value is less than the second threshold, the weight of the obtained point cloud data is determined to be 0.

[0034] When the point cloud distribution feature value is between the first threshold and the second threshold, the point cloud data weight is determined based on the first threshold, the second threshold and the point cloud distribution feature value.

[0035] In this embodiment of the application, determining the point cloud data weights based on the first threshold, the second threshold, and the point cloud distribution feature values ​​includes:

[0036] Calculate the difference between the point cloud distribution feature value and the second threshold to obtain the first difference;

[0037] Calculate the difference between the point cloud distribution feature value and the first threshold to obtain the second difference;

[0038] The ratio of the first difference to the second difference is calculated to obtain the point cloud data weight.

[0039] In this embodiment of the application, after determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds, the method further includes:

[0040] The threshold is adjusted based on the point cloud distribution feature values ​​to obtain a new threshold for use in the next round of pose estimation.

[0041] In this embodiment, the third sensor is an image sensor;

[0042] The acquisition of the first data, the second data, and the third data includes:

[0043] Acquire first initial data, second initial data, and third initial data, wherein the first initial data is data collected by the first sensor, the second initial data is data collected by the second sensor, and the third initial data is data collected by the third sensor;

[0044] Based on the aforementioned third initial data, the third data is determined;

[0045] Based on the collection interval of the third data, the first initial data and the second initial data are segmented and truncated respectively to obtain the first data and the second data.

[0046] A second aspect of this application provides a pose estimation device for a multi-sensor positioning system, the multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the device includes:

[0047] The acquisition module is used to acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor;

[0048] The determining module is used to determine the first pose information based on the first data;

[0049] The calculation module is used to determine the Kalman gain information based on the second data and the third data;

[0050] The filtering module is used to perform Kalman filtering based on the Kalman gain information and the first pose information to obtain new pose information.

[0051] A third aspect of this application provides an electronic device, the electronic device comprising:

[0052] At least one processor;

[0053] A memory connected to the at least one processor;

[0054] The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the pose estimation method described above by executing the instructions stored in the memory.

[0055] A fourth aspect of this application provides a machine-readable storage medium storing instructions that, when executed by a processor, configure the processor to perform the pose estimation method described above.

[0056] The above technical solution involves setting up a multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, where the second sensor is a lidar. First data, second data, and third data are acquired, where the first data is obtained through the first sensor, the second data through the second sensor, and the third data through the third sensor. Based on the first data, a first pose information is determined. Based on the second and third data, Kalman gain information is determined. Based on the Kalman gain information and the first pose information, Kalman filtering is performed to obtain new pose information. By fusing the second and third data in parallel to construct the Kalman gain and directly performing a single Kalman filter, a better posterior pose is obtained. This strategy fundamentally avoids the cascading failure risk of a cascaded architecture. When the lidar viewpoint changes abruptly, the third data can be used as compensation to calculate the Kalman gain information, improving the accuracy of pose estimation and thus enhancing the system's robustness and estimation accuracy in challenging scenarios. Furthermore, compared to a cascaded architecture, this method only requires one Kalman filter, improving efficiency.

[0057] Other features and advantages of the embodiments of this application will be described in detail in the following detailed description section. Attached Figure Description

[0058] The accompanying drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the following detailed description to explain the embodiments of this application, but do not constitute a limitation on the embodiments of this application. In the drawings:

[0059] Figure 1 The schematic diagram illustrates a flowchart of a pose estimation method according to an embodiment of this application;

[0060] Figure 2 This schematic diagram illustrates a structural block diagram of a pose estimation device according to an embodiment of the present application;

[0061] Figure 3 The diagram illustrates the internal structure of a computer device according to an embodiment of this application.

[0062] Explanation of reference numerals in the attached figures

[0063] 410 - Acquisition module; 420 - Determination module; 430 - Calculation module; 440 - Filtering module; A01 - Processor; A02 - Network interface; A03 - Internal memory; A04 - Display screen; A05 - Input device; A06 - Non-volatile storage medium; B01 - Operating system; B02 - Computer program. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only for illustration and explanation of the embodiments of this application and are not intended to limit the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0065] It should be noted that the acquisition, transmission, storage, use, and processing of data in the technical solution of this application all comply with relevant laws and regulations. In the embodiments of this application, certain existing industry solutions such as software, components, and models may be mentioned. These should be considered exemplary, intended only to illustrate the feasibility of implementing the technical solution of this application, and do not imply that the applicant has already used or necessarily used such solutions.

[0066] It should be noted that if the embodiments of this application involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0067] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0068] Figure 1 A schematic flowchart of a pose estimation method according to an embodiment of this application is shown. Figure 1 As shown, the pose estimation method provided in this embodiment can be widely applied in the field of Simultaneous Localization and Mapping (SLAM), specifically for pose estimation tasks based on the Kalman filter framework. This method is compatible with various variants of the Kalman filter, including the standard Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), error state Kalman filter (ESKF), and iterative extended Kalman filter (IEKF).

[0069] To clearly illustrate the core ideas and implementation details of this scheme, this embodiment mainly uses Error State Kalman Filtering (ESKF) as a specific example for detailed explanation. ESKF effectively handles rotation-related nonlinear problems by filtering in the error state space, and is widely used in fields such as Visual Inertial Odometry (VIO), making it a typical example. Its error state quantity remains consistently small, resulting in more accurate linearization approximations and facilitating theoretical analysis and understanding. This framework clearly separates the nominal state from the error state, helping to demonstrate the general design concepts of this method in state updates, covariance management, and other aspects.

[0070] This embodiment provides a pose estimation method for a multi-sensor positioning system, the multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the method includes the following steps:

[0071] Step 210: Acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor;

[0072] In this embodiment, a multi-sensor localization system can be used to construct SLAM. Multiple sensors can be configured to acquire localization data; specifically, the first, second, and third sensors are used for localization. The multi-sensor localization system can be applied to various scenarios, such as robots. This embodiment primarily uses a multi-sensor localization system on a robot as an example for SLAM construction. The first sensor can be an inertial measurement unit (IMU), real-time dynamic differential positioning (RTK), etc., which can collect localization information such as linear velocity and angular velocity in real time to obtain first data. The second sensor is a lidar (LiDAR) system, which collects point cloud data; therefore, the second data is point cloud data. The third sensor can be a camera, millimeter-wave radar, etc. It should be noted that there can be one or more third sensors. Multiple sensor combinations, including radar, are formed using the first, second, and third sensors.

[0073] It should be noted that, for ease of explanation, this embodiment primarily uses an IMU as the first sensor and a camera as the third sensor, and elaborates on a scenario combining the fusion of three sensors: IMU, LiDAR, and camera. For example, a robot is equipped with an IMU, LiDAR, and camera. The IMU acquires the first data, the LiDAR acquires the second data, and the camera acquires the third data.

[0074] Step 220: Based on the first data, determine the first pose information;

[0075] In this embodiment, prior state prediction can be performed based on the first data. The prior pose information, i.e., the first pose information, can be calculated from the first data. The first pose information includes the prior pose x. pred Prior covariance P pred Taking an IMU as the first sensor as an example, the first data includes angular velocity and acceleration measurements. The raw angular velocity and acceleration measurements can first undergo zero-bias correction and gravity elimination. Then, the prior pose is obtained recursively through integration (or pre-integration). Simultaneously, the covariance matrix is ​​propagated using an error state dynamics model to quantify the uncertainty of the prediction results. The final output prior pose and prior covariance provide a crucial initial estimate and confidence assessment basis for subsequent fusion of visual, laser, and other observation data. The process of calculating the prior pose information described above is existing technology and will not be elaborated further here.

[0076] In some embodiments, the third sensor is an image sensor; correspondingly, acquiring the first data, the second data, and the third data includes:

[0077] First, first initial data, second initial data, and third initial data are acquired. The first initial data is the data collected by the first sensor, the second initial data is the data collected by the second sensor, and the third initial data is the data collected by the third sensor.

[0078] Then, based on the third initial data, the third data is determined;

[0079] Finally, based on the collection interval of the third data, the first initial data and the second initial data are segmented and truncated respectively to obtain the first data and the second data.

[0080] In this embodiment, the image sensor may include a camera, a webcam, etc. The following explanation uses an IMU as the first sensor and a camera as the third sensor as an example. When the LiDAR viewpoint changes abruptly, the point clouds of adjacent LiDAR frames may have too small an overlap area due to the low scanning frequency (typically 10Hz) and significant viewpoint changes, making reliable point cloud registration difficult. However, image sensors, such as cameras, typically acquire images at higher frequencies (e.g., 30Hz). Even under rapid motion, adjacent image frames can maintain sufficient feature correlation and overlap, thus providing stable constraints for pose estimation. To ensure strict temporal consistency of multi-sensor data, this method can employ a sensor data synchronization strategy based on hardware triggering or software timestamps. Specifically, this process uses the acquisition time interval of the third data as a unified benchmark to perform time-aligned segmentation and truncation of the synchronized first and second initial data. That is, within a certain time window before and after the exposure time of each image frame, the first initial data is integrated or pre-integrated to obtain the relative motion increment within that time period. At the same time, the point cloud obtained by LiDAR scanning within this time period is uniformly projected onto the camera exposure time through motion compensation to eliminate the point cloud distortion caused during the scanning process. Thus, each camera frame triggers a data processing round, which includes the time-aligned image, IMU data, and the corrected LiDAR point cloud (i.e., the first, second, and third data), constituting a complete processing unit for multi-sensor fusion.

[0081] This synchronization mechanism ensures the temporal consistency of data from each sensor in subsequent fusion algorithms, providing a reliable data foundation for tightly coupled joint optimization.

[0082] Step 230: Based on the second data and the third data, determine the Kalman gain information;

[0083] In this embodiment, the second data and the third data can be fused to construct the Kalman gain. The second data is point cloud data, and the third data can be used as compensation for the point cloud data to obtain a more accurate Kalman gain.

[0084] In some embodiments, determining the Kalman gain information based on the second data and the third data includes:

[0085] First, based on the second data, the residual terms of the point cloud observation model, the Jacobian matrix of the residual terms with respect to the Kalman filter independent variables, and the covariance matrix are determined to obtain the first state estimation parameters.

[0086] In this embodiment, the second data mentioned above is point cloud data. The point cloud can be divided into voxel meshes to achieve standardized management. Within each voxel, principal component analysis (PCA) or least squares fitting algorithms are used to estimate the best-fit plane for that local point cloud distribution, obtaining the unit normal vector n and intercept parameter d of the plane, and the plane parameters. It is a 4-dimensional real vector, i.e., a plane parameter. Then, the Euclidean distances from all points within the voxel to the fitting plane are calculated, and the variance of these distance values ​​is obtained, which is the variance var of the distance from the point cloud within the voxel to the fitting plane. This variance var can be used as a quantitative indicator to evaluate the flatness of the point cloud within the voxel and the quality of the fit. This variance information can be used for subsequent feature selection, point cloud denoising, or adaptive resolution adjustment. The point cloud observation model refers to a mathematical model that associates the robot's state (mainly pose) with the raw point cloud data observed by LiDAR.

[0087] An observation error term can be constructed using point cloud registration methods (such as NDT, ICP, etc.) for observation updates in state estimation. Specifically, the point cloud of the current frame can be registered with reference to the previous frame or a local map to obtain observations of relative pose transformation. The difference between these observations and the predicted state constitutes the observation residual (error term), meaning the residual vector comes directly from the comparison between the registration result and the observation model. Covariance matrix estimation is used to assess the uncertainty of the registration result, and can be approximated using the Hessian matrix within the registration algorithm, the goodness of fit of the point cloud matching (such as the average distance variance of matched point pairs in ICP), or based on a sensor noise model. The Jacobian matrix of the residual term with respect to the Kalman filter independent variables can be calculated using the Jacobian matrix of the observation model with respect to the Kalman filter independent variables (such as robot pose).

[0088] The following example uses point-to-area ICP registration of point clouds. Let the number of point clouds registered in this round be... A certain point is After being transformed by the rotation matrix R and the translation vector t, it falls on a certain plane with parameters R. In the voxels, establish The error function between it and the plane formed by its nearest neighbors is:

[0089] ,

[0090] in, Let be a scalar, representing the directed distance from a point to the plane. This covariance matrix has a size of 1×1, and its values ​​are the variances of the distances from the point cloud within the voxel to the fitted plane. This is used to quantify the uncertainty of the point-surface matching.

[0091] We can define the state variables as x = [p, v, ... [bg, bt, g], where p is the position (corresponding to translation t) and v is the velocity. Let R be the rotation Lie algebra representation (corresponding to the rotation matrix R), bg be the gyroscope zero bias, bt be the accelerometer zero bias, and g be the gravity vector. Then the error... Jacobian matrix for state variable x It is The row vectors can be calculated using the perturbation model. Let the Jacobian of the residual term with respect to the Kalman filter independent variable be: In this Jacobian matrix, the first term corresponds to the derivative of the rotation Lie algebra, the second term corresponds to the derivative of the velocity, the third term corresponds to the derivative of the position, and the remaining terms correspond to the derivatives of the zero bias and gravity (all zero). This Jacobian matrix will be used to calculate the Kalman gain and update the state covariance in the Kalman filter update step.

[0092] The above calculation process can be achieved using existing technology, so it will not be elaborated here.

[0093] Then, based on the third data, the residual term of the corresponding observation model, the Jacobian matrix of the residual term with respect to the Kalman filter independent variable, and the covariance matrix are determined to obtain the second state estimation parameters.

[0094] In this embodiment, similar to the process of determining the first and second state estimation parameters, the second state estimation parameters can also be implemented using existing technologies. Taking a depth camera as the third sensor and image data as the third data, a visual error term can be constructed based on reprojection or optical flow methods, and its value, covariance matrix, and Jacobian matrix with respect to the Kalman filter independent variable can be solved to obtain the second state estimation parameters. The corresponding observation model refers to the mathematical model that associates the robot's state (mainly pose) with the third data.

[0095] The determination process will be explained in detail below using the optical flow method as an example:

[0096] Assume that the spatial coordinates of a certain pixel in the third data (depth camera data) are... Its grayscale value in the first frame image is ,in, For the camera intrinsic parameter matrix, For point The depth value in the camera coordinate system of the first frame. Considering the limited number of matched radar point clouds, this method... The coordinates are directly provided by the depth camera (i.e., the 3D point is obtained through backprojection of the depth image). Let... For the pose transformation of the camera from frame 1 to frame 2, then the point The grayscale value in the camera coordinate system of the second frame is ,in Points after transformation The depth value. Based on the assumption of gray-level invariance (i.e., the gray-level value of the same spatial point remains unchanged in adjacent images), the photometric error function can be established as:

[0097] ,

[0098] in, As a scalar, the covariance matrix value can be defined as the value of each image point. The variance of pixel intensity in the surrounding local area (the extent is determined by the step parameter, such as 3x3 pixels for step=1, totaling 9 pixels). This error function describes the error during pose transformation. The consistency of grayscale values ​​at corresponding points in two frames is considered. The camera pose transformation can be optimized by minimizing the sum of squared errors (or weighted sums) of all matching points.

[0099] Let u be The pixel coordinates of the second frame image, and their 3D coordinates in the camera coordinate system are: , For the Jacobian of the residual terms with respect to the transformation, we have:

[0100] ,

[0101] in, Let u be the pixel gradient. Let q = (X, Y, Z) be the 3D point in the camera coordinate system as projected by the equation. The derivative, This is the derivative of the transformed point with respect to the transformation.

[0102] set up For the residual term, the Kalman filter independent variables [p, v, ... Jacobian of pose transformation of [bg, bt, g] .

[0103] Then, based on the first state estimation parameters and the second state estimation parameters, the gain calculation parameters are determined;

[0104] In this embodiment, the above-mentioned gain calculation parameters include: and ), where H represents the Jacobian matrix and V represents the covariance matrix. The ) represents the residual term. Since the Kalman gain is processed by high-dimensional equivalent observations, , It can integrate the residual terms of the point cloud observation model, the Jacobian matrix and covariance matrix of the residual terms with respect to the Kalman filter independent variables, and the residual terms of the observation model corresponding to the third data, as well as the Jacobian matrix and covariance matrix of the residual terms with respect to the Kalman filter independent variables, to calculate the gain calculation parameters.

[0105] Finally, based on the gain calculation parameters, the Kalman gain information is calculated.

[0106] In this embodiment, the Kalman gain information includes the Kalman gain and the error state of the Kalman gain. In a Kalman filter, since the observation equation requires iteration, the Kalman gain and update process need to be recalculated with each iteration. Therefore, the quantities related to the update process also have a subscript k, indicating the number of update iterations. The Kalman gain can be denoted as... The Jacobian matrix can be written as: Let the error state of the k-th update iteration be denoted as... The covariance at this time is denoted as The nominal state is , Given the error in the observations, based on the Sherman-Morrison-Woodbury identity, the expression for the Kalman gain can be obtained as follows:

[0107] ,

[0108] The error state of the Kalman gain can be expressed as:

[0109] .

[0110] Substituting the Kalman gain expression into the error state expression yields the gain calculation parameters. Therefore, after obtaining the gain calculation parameters, the Kalman gain information can be obtained.

[0111] Based on the second data, the residual terms of the point cloud observation model, the Jacobian matrix of the residual terms with respect to the Kalman filter independent variables, and the covariance matrix are determined; based on the third data, the residual terms of the corresponding observation model, the Jacobian matrix of the residual terms with respect to the Kalman filter independent variables, and the covariance matrix are determined; thus, the gain calculation parameters are determined, and the Kalman gain information can be calculated quickly and accurately.

[0112] In some embodiments, the matrix dimension in the first state estimation parameters is the same as the matrix dimension in the second state estimation parameters; the gain calculation parameters can be obtained by weighted fusion of the first state estimation parameters and the second state estimation parameters. That is, determining the gain calculation parameters based on the first state estimation parameters and the second state estimation parameters includes:

[0113] The first step is to obtain the point cloud data weights;

[0114] In this embodiment, the point cloud data weights can be determined based on the number of matching point clouds in the second data. After the point cloud data weights are determined, the weights corresponding to the third data can be calculated using the fact that the sum of the weights is 1.

[0115] In some embodiments, obtaining point cloud data weights includes:

[0116] First, based on the second data, the number of point clouds to be processed and the optimal number of point clouds are determined;

[0117] In this embodiment, the number of point clouds processed refers to the total number of point clouds processed in the current round, which can be expressed as the number of point clouds processed. The optimal number of point clouds can refer to the number of point clouds that meet the requirements, which can be obtained by judging whether each processed point cloud meets a preset condition. The preset condition can be that the distance from the point cloud to the fitting plane is less than a preset distance threshold. The preset distance threshold is Then you can filter out The optimal number of point clouds is the number of point clouds that satisfy this condition, which can be denoted as: It should be noted that the preset distance threshold mentioned above can be set with reference to the voxel size, and can be set to be less than or equal to the voxel resolution, for example, =0.05 adapts to a voxel resolution of 0.05m.

[0118] Then, based on the number of point clouds processed and the preferred number of point clouds, the point cloud distribution characteristic values ​​are determined;

[0119] In this embodiment, the aforementioned point cloud distribution characteristic value can be obtained by calculating the ratio of the preferred number of point clouds to the number of processed point clouds.

[0120] Finally, the point cloud data weights are determined based on the point cloud distribution feature values ​​and thresholds.

[0121] In this embodiment, the point cloud data weights can be determined by comparing point cloud distribution feature values ​​with thresholds. Different comparison results can correspond to different point cloud data weights. The number of thresholds can be set according to actual conditions.

[0122] Based on the second data, the number of point clouds to be processed and the number of preferred point clouds are determined, and then the point cloud distribution characteristic value is determined. The point cloud data weight determined by the point cloud distribution characteristic value can better reflect the point cloud quality and help to obtain more reasonable gain calculation parameters.

[0123] In some embodiments, the threshold includes a first threshold and a second threshold; correspondingly, determining the point cloud data weight based on the point cloud distribution feature value and the threshold includes:

[0124] If the point cloud distribution feature value is greater than the first threshold, the weight of the obtained point cloud data is determined to be 1;

[0125] If the point cloud distribution feature value is less than the second threshold, the weight of the obtained point cloud data is determined to be 0.

[0126] When the point cloud distribution feature value is between the first threshold and the second threshold, the point cloud data weight is determined based on the first threshold, the second threshold and the point cloud distribution feature value.

[0127] The step of determining the point cloud data weights based on the first threshold, the second threshold, and the point cloud distribution feature values ​​includes:

[0128] Calculate the difference between the point cloud distribution feature value and the second threshold to obtain the first difference;

[0129] Calculate the difference between the point cloud distribution feature value and the first threshold to obtain the second difference;

[0130] The ratio of the first difference to the second difference is calculated to obtain the point cloud data weight.

[0131] In this embodiment, the first threshold and the second threshold can be preset, and can be expressed as follows: and The point cloud distribution characteristic value is the ratio of the number of preferred point clouds to the number of processed point clouds, which can be expressed as: .when At that time, point cloud data weight =1; when At that time, point cloud data weight =0; otherwise .

[0132] By comparing the point cloud distribution feature values ​​with the first and second thresholds respectively, different point cloud data weights can be set in different ranges, making the weight allocation more consistent with the actual point cloud distribution.

[0133] In some embodiments, after determining the point cloud data weights based on the point cloud distribution feature values ​​and the threshold, the method further includes: adjusting the threshold based on the point cloud distribution feature values ​​to obtain a new threshold for use in the next round of pose estimation.

[0134] In this embodiment, an initial threshold value can be preset, and then the threshold can be adjusted according to the point cloud distribution feature values ​​in each round. This allows for autonomous and efficient tracking of the dynamic changes in the point cloud distribution feature values ​​and natural separation. For example, setting an initial... =0.7, =0.3, if the point cloud distribution characteristic value is 0.9, that is, the point cloud distribution characteristic value is greater than 0.3. This indicates that the point cloud quality in the current frame (or even recent frames) is good, and you can increase the setting. , Specific adjustment strategies can be set according to the actual situation, for example... = +(Point cloud distribution characteristic value-) )×0.1, and The growth rate can be slower. = +(Point cloud distribution characteristic value-) )×0.05. If the point cloud distribution eigenvalue = 0.2, that is, less than This indicates that the point cloud quality in the current frame (or even recent frames) is poor and can be reduced. , Specific adjustment strategies can be set according to the actual situation, for example: = -( -point cloud distribution eigenvalue) × 0.1, while You can reduce it a bit. = -( -point cloud distribution characteristic value)×0.05.

[0135] By adjusting the threshold based on the point cloud distribution feature values, the threshold can be adaptively calculated according to the real-time features of the sensor data, which helps to obtain more accurate weights.

[0136] The second step is to determine the gain calculation parameter model based on the point cloud data weights.

[0137] The gain calculation parameter model is as follows:

[0138] ,

[0139] ,

[0140] in, For point cloud data weights, Let be the Jacobian matrix of the residual terms of the point cloud observation model with respect to the Kalman filter independent variables. The covariance matrix of the point cloud observation model is... For the residual term of the observation model corresponding to the third data, This is the Jacobian matrix of the residual terms of the observation model corresponding to the third data point, expressed as a function of the Kalman filter independent variables. The second covariance matrix, For the residual term of the observation model corresponding to the third data, The first gain calculation parameter, The second gain calculation parameter, Let be the total Jacobian matrix. Let be the total covariance matrix. This is the actual observation data from the sensor. To update the iteration count, For the first The nominal state of the next update iteration. For the observation equation, These are the observations predicted using the nominal state.

[0141] The third step is to substitute the first state estimation parameters and the second state estimation parameters into the gain calculation parameter model to obtain the gain calculation parameters.

[0142] In this embodiment, for example, when When ESKF only calculates the posterior pose based on radar point cloud computing, the Jacobian matrix, residual terms, and covariance matrix of the point cloud observation model can be substituted into the gain calculation parameter model. Since point-to-surface ICP can be used, and the residual terms are scalars, the covariance matrix size is 1×1, and the value is taken as the variance var of the distance from the point cloud within the voxel to the fitted plane. When ESKF calculates the posterior pose based solely on camera images, the Jacobian matrix, residual terms, and covariance matrix of the observation model corresponding to the third data are similarly substituted into the gain calculation parameter model.

[0143] By acquiring point cloud data weights, and based on these weights, a gain calculation parameter model is determined. The first state estimation parameter and the second state estimation parameter are then substituted into the gain calculation parameter model to obtain the gain calculation parameters. This allows the gain calculation parameters to be calculated through a weighted fusion of the first and second state estimation parameters, improving the accuracy of the gain calculation parameters.

[0144] Step 240: Based on the Kalman gain information and the first pose information, perform Kalman filtering to obtain new pose information.

[0145] In this embodiment, the first pose information is used as the prior pose state estimate. Combined with the Kalman gain information, a Kalman filter update step is performed to update the posterior pose for this round. , This yields the optimized posterior pose state estimate, which is the new pose information. The Kalman filtering process described above is existing technology and will not be elaborated further. The above steps can be continuously and iteratively executed until sensor data transmission ends or mapping is completed, ultimately outputting a map that maintains accuracy even during sudden and drastic changes in viewpoint.

[0146] In the above implementation, a multi-sensor positioning system is configured with a first sensor, a second sensor, and a third sensor, where the second sensor is a lidar. First data, second data, and third data are acquired, where the first data is obtained through the first sensor, the second data through the second sensor, and the third data through the third sensor. Based on the first data, a first pose information is determined. Based on the second and third data, Kalman gain information is determined. Based on the Kalman gain information and the first pose information, Kalman filtering is performed to obtain new pose information. By fusing the second and third data in parallel to construct the Kalman gain and directly performing a single Kalman filter, a better posterior pose is obtained. This strategy fundamentally avoids the cascading failure risk of a cascaded architecture. When the lidar viewpoint changes abruptly, the third data can be used as compensation to calculate the Kalman gain information, improving the accuracy of pose estimation and thus enhancing the system's robustness and estimation accuracy in challenging scenarios. Furthermore, compared to a cascaded architecture, this method only requires one Kalman filter, improving efficiency.

[0147] Figure 1 This is a flowchart illustrating a pose estimation method in one embodiment. It should be understood that, although... Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0148] Please refer to Figure 2 , Figure 2This schematically illustrates a structural block diagram of a pose estimation device according to an embodiment of the present application. This embodiment provides a pose estimation device for a multi-sensor positioning system, the multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the device includes an acquisition module 410, a determination module 420, a calculation module 430, and a filtering module 440, wherein:

[0149] The acquisition module 410 is used to acquire first data, second data and third data, wherein the first data is data acquired by the first sensor, the second data is data acquired by the second sensor and the third data is data acquired by the third sensor;

[0150] The determining module 420 is used to determine the first pose information based on the first data;

[0151] Calculation module 430 is used to determine Kalman gain information based on the second data and the third data;

[0152] The filtering module 440 is used to perform Kalman filtering based on the Kalman gain information and the first pose information to obtain new pose information.

[0153] The pose estimation device includes a processor and a memory. The acquisition module 410, determination module 420, calculation module 430 and filtering module 440 are all stored in the memory as program units. The processor executes the program modules stored in the memory to implement the corresponding functions.

[0154] The processor contains a kernel, which retrieves the corresponding program unit from memory. One or more kernels can be configured, and the pose estimation method can be implemented by adjusting the kernel parameters.

[0155] The memory may include non-permanent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0156] This application provides a machine-readable storage medium storing a program that, when executed by a processor, implements the aforementioned pose estimation method.

[0157] In one embodiment, a computer device is provided, which may be a terminal, and its internal structure diagram may be as follows: Figure 3As shown in the figure, the computer device includes a processor A01, a network interface A02, a display screen A04, an input device A05, and a memory (not shown) connected via a system bus. The processor A01 provides computing and control capabilities. The memory includes internal memory A03 and a non-volatile storage medium A06. The non-volatile storage medium A06 stores an operating system B01 and a computer program B02. The internal memory A03 provides an environment for the operation of the operating system B01 and the computer program B02 stored in the non-volatile storage medium A06. The network interface A02 is used for communication with external terminals via a network connection. When the computer program is executed by the processor A01, it implements a pose estimation method. The display screen A04 can be a liquid crystal display (LCD) or an e-ink display. The input device A05 can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.

[0158] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0159] In one embodiment, the pose estimation device provided in this application can be implemented as a computer program, which can be implemented in the form of, for example, Figure 3 The device runs on the computer shown. The computer's memory can store the various program modules that make up the pose estimation device, for example, Figure 2 The diagram shows an acquisition module 410, a determination module 420, a calculation module 430, and a filtering module 440. The computer program comprised of these modules causes the processor to execute the steps in the pose estimation methods of the various embodiments of this application described in this specification.

[0160] Figure 3 The computer equipment shown can be used as follows Figure 2 The acquisition module 410 in the pose estimation device shown executes step 210. The computer device executes step 220 through the determination module 420, step 230 through the calculation module 430, and step 240 through the filtering module 440.

[0161] This application provides a device including a processor, a memory, and a program stored in the memory and executable on the processor, for a multi-sensor positioning system. The multi-sensor positioning system includes a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar. When the processor executes the program, it performs the following steps:

[0162] Acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor;

[0163] Based on the first data, the first pose information is determined;

[0164] Based on the second data and the third data, the Kalman gain information is determined;

[0165] Based on the Kalman gain information and the first pose information, Kalman filtering is performed to obtain new pose information.

[0166] In one embodiment, determining the Kalman gain information based on the second data and the third data includes:

[0167] Based on the second data, the residual term, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined to obtain the first state estimation parameters.

[0168] Based on the third data, the residual term of the corresponding observation model, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined, and the second state estimation parameters are obtained.

[0169] Based on the first state estimation parameters and the second state estimation parameters, the gain calculation parameters are determined;

[0170] Based on the aforementioned gain calculation parameters, the Kalman gain information is calculated.

[0171] In one embodiment, the matrix dimension in the first state estimation parameters is the same as the matrix dimension in the second state estimation parameters;

[0172] The step of determining the gain calculation parameters based on the first state estimation parameters and the second state estimation parameters includes:

[0173] Obtain point cloud data weights;

[0174] Based on the point cloud data weights, a gain calculation parameter model is determined;

[0175] Substitute the first state estimation parameters and the second state estimation parameters into the gain calculation parameter model to obtain the gain calculation parameters.

[0176] In one embodiment, the gain calculation parameter model is as follows:

[0177] ,

[0178] ,

[0179] in, For point cloud data weights, Let be the Jacobian matrix of the residual terms of the point cloud observation model with respect to the Kalman filter independent variables. The covariance matrix of the point cloud observation model is... For the residual term of the observation model corresponding to the third data, This is the Jacobian matrix of the residual terms of the observation model corresponding to the third data point, expressed as a function of the Kalman filter independent variables. The second covariance matrix, For the residual term of the observation model corresponding to the third data, The first gain calculation parameter, The second gain calculation parameter, Let be the total Jacobian matrix. Let be the total covariance matrix. This is the actual observation data from the sensor. To update the iteration count, For the first The nominal state of the next update iteration. For the observation equation, These are the observations predicted using the nominal state.

[0180] In one embodiment, obtaining the point cloud data weights includes:

[0181] Based on the second data, the number of point clouds to be processed and the preferred number of point clouds are determined;

[0182] Based on the number of point clouds processed and the preferred number of point clouds, the point cloud distribution characteristic values ​​are determined;

[0183] The point cloud data weights are determined based on the point cloud distribution feature values ​​and thresholds.

[0184] In one embodiment, the threshold includes a first threshold and a second threshold;

[0185] The step of determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds includes:

[0186] If the point cloud distribution feature value is greater than the first threshold, the weight of the obtained point cloud data is determined to be 1;

[0187] If the point cloud distribution feature value is less than the second threshold, the weight of the obtained point cloud data is determined to be 0.

[0188] When the point cloud distribution feature value is between the first threshold and the second threshold, the point cloud data weight is determined based on the first threshold, the second threshold and the point cloud distribution feature value.

[0189] In one embodiment, determining the point cloud data weights based on the first threshold, the second threshold, and the point cloud distribution feature values ​​includes:

[0190] Calculate the difference between the point cloud distribution feature value and the second threshold to obtain the first difference;

[0191] Calculate the difference between the point cloud distribution feature value and the first threshold to obtain the second difference;

[0192] The ratio of the first difference to the second difference is calculated to obtain the point cloud data weight.

[0193] In one embodiment, after determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds, the method further includes:

[0194] The threshold is adjusted based on the point cloud distribution feature values ​​to obtain a new threshold for use in the next round of pose estimation.

[0195] In one embodiment, the third sensor is an image sensor;

[0196] The acquisition of the first data, the second data, and the third data includes:

[0197] Acquire first initial data, second initial data, and third initial data, wherein the first initial data is data collected by the first sensor, the second initial data is data collected by the second sensor, and the third initial data is data collected by the third sensor;

[0198] Based on the aforementioned third initial data, the third data is determined;

[0199] Based on the collection interval of the third data, the first initial data and the second initial data are segmented and truncated respectively to obtain the first data and the second data.

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

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

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

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

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

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

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

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

[0208] The above are merely embodiments of this application and are not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A pose estimation method, characterized in that, A multi-sensor positioning system is used, the multi-sensor positioning system including a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the method includes: Acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor; Based on the first data, the first pose information is determined; Based on the second data and the third data, the Kalman gain information is determined; Based on the Kalman gain information and the first pose information, Kalman filtering is performed to obtain new pose information.

2. The method according to claim 1, characterized in that, The determination of Kalman gain information based on the second data and the third data includes: Based on the second data, the residual term, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined to obtain the first state estimation parameters. Based on the third data, the residual term of the corresponding observation model, the Jacobian matrix and covariance matrix of the residual term with respect to the Kalman filter independent variable are determined, and the second state estimation parameters are obtained. Based on the first state estimation parameters and the second state estimation parameters, the gain calculation parameters are determined; Based on the aforementioned gain calculation parameters, the Kalman gain information is calculated.

3. The method according to claim 2, characterized in that, The matrix dimension in the first state estimation parameter is the same as the matrix dimension in the second state estimation parameter; The step of determining the gain calculation parameters based on the first state estimation parameters and the second state estimation parameters includes: Obtain point cloud data weights; Based on the point cloud data weights, a gain calculation parameter model is determined; Substitute the first state estimation parameters and the second state estimation parameters into the gain calculation parameter model to obtain the gain calculation parameters.

4. The method according to claim 3, characterized in that, The gain calculation parameter model is as follows: ， ， in, For point cloud data weights, Let be the Jacobian matrix of the residual terms of the point cloud observation model with respect to the Kalman filter independent variables. The covariance matrix of the point cloud observation model is... For the residual term of the observation model corresponding to the third data, This is the Jacobian matrix of the residual terms of the observation model corresponding to the third data point, expressed as a function of the Kalman filter independent variables. The second covariance matrix, For the residual term of the observation model corresponding to the third data, The first gain calculation parameter, The second gain calculation parameter, Let be the total Jacobian matrix. Let be the total covariance matrix. This is the actual observation data from the sensor. To update the iteration count, For the first The nominal state of the next update iteration. For the observation equation, These are the observations predicted using the nominal state.

5. The method according to claim 3, characterized in that, The acquisition of point cloud data weights includes: Based on the second data, the number of point clouds to be processed and the preferred number of point clouds are determined; Based on the number of point clouds processed and the preferred number of point clouds, the point cloud distribution characteristic values ​​are determined; The point cloud data weights are determined based on the point cloud distribution feature values ​​and thresholds.

6. The method according to claim 5, characterized in that, The threshold includes a first threshold and a second threshold; The step of determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds includes: If the point cloud distribution feature value is greater than the first threshold, the weight of the obtained point cloud data is determined to be 1; If the point cloud distribution feature value is less than the second threshold, the weight of the obtained point cloud data is determined to be 0. When the point cloud distribution feature value is between the first threshold and the second threshold, the point cloud data weight is determined based on the first threshold, the second threshold and the point cloud distribution feature value.

7. The method according to claim 6, characterized in that, The step of determining the point cloud data weights based on the first threshold, the second threshold, and the point cloud distribution feature values ​​includes: Calculate the difference between the point cloud distribution feature value and the second threshold to obtain the first difference; Calculate the difference between the point cloud distribution feature value and the first threshold to obtain the second difference; The ratio of the first difference to the second difference is calculated to obtain the point cloud data weight.

8. The method according to claim 5, characterized in that, After determining the point cloud data weights based on the point cloud distribution feature values ​​and thresholds, the method further includes: The threshold is adjusted based on the point cloud distribution feature values ​​to obtain a new threshold for use in the next round of pose estimation.

9. The method according to claim 1, characterized in that, The third sensor is an image sensor; The acquisition of the first data, the second data, and the third data includes: Acquire first initial data, second initial data, and third initial data, wherein the first initial data is data collected by the first sensor, the second initial data is data collected by the second sensor, and the third initial data is data collected by the third sensor; Based on the aforementioned third initial data, the third data is determined; Based on the collection interval of the third data, the first initial data and the second initial data are segmented and truncated respectively to obtain the first data and the second data.

10. A pose estimation device, characterized in that, For a multi-sensor positioning system, the multi-sensor positioning system includes a first sensor, a second sensor, and a third sensor, wherein the second sensor is a lidar; the device includes: The acquisition module is used to acquire first data, second data, and third data, wherein the first data is data acquired through the first sensor, the second data is data acquired through the second sensor, and the third data is data acquired through the third sensor; The determining module is used to determine the first pose information based on the first data; The calculation module is used to determine the Kalman gain information based on the second data and the third data; The filtering module is used to perform Kalman filtering based on the Kalman gain information and the first pose information to obtain new pose information.

11. A machine-readable storage medium storing instructions thereon, characterized in that, This instruction is used to cause the machine to perform the method according to any one of claims 1 to 9.

12. An electronic device, characterized in that, The electronic device includes: At least one processor; A memory connected to the at least one processor; The memory stores instructions executable by the at least one processor, which implements the method of any one of claims 1 to 9 by executing the instructions stored in the memory.

Images