Space-time synchronization high-precision positioning method and system based on multi-sensor fusion

Abstract

This invention discloses a spatiotemporal synchronous high-precision positioning method and system based on multi-sensor fusion, belonging to the field of positioning technology. The method includes the following steps: Step 1: Acquiring images from a visible light camera, a 3D point cloud coordinate image from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module; Step 2: Based on a pre-calibrated homogeneous transformation matrix, reprojecting the 3D point cloud coordinate image acquired by the ToF depth camera onto the image plane of the visible light camera to obtain spatially aligned coordinates; Step 3: Based on the signal strength indication received by the wireless signal module, calculating and correcting the distance observation value using a nonlinear ranging model to obtain the corrected distance observation value. This invention effectively solves the inherent defects of single-sensor positioning in complex environments through a complete spatiotemporal synchronization and multimodal data fusion framework.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and more specifically to a spatiotemporal synchronous high-precision positioning method and system based on multi-sensor fusion. Background Technology

[0002] In recent years, with the increasing demand for high-precision spatial perception in fields such as intelligent manufacturing, intelligent transportation, and smart grids, positioning technology has become one of the key supports. Especially in typical scenarios such as power drone inspection, path planning for indoor unmanned transport robots (AGVs), and automated warehousing and logistics, mobile devices need to complete high-frequency, accurate, and stable spatiotemporal positioning under complex environmental conditions. However, traditional single-sensor positioning systems (such as those based on a single GNSS module, monocular camera, or inertial measurement unit) often face problems such as occlusion, low light, and signal interference, leading to overall system failure or a significant decrease in accuracy.

[0003] Taking unmanned aerial vehicle (UAV) power line inspection as an example, its operational scenarios often include mountainous terrain, high-voltage tower obstructions, and complex electromagnetic interference, making GNSS signals susceptible to loss. Furthermore, during obstacle avoidance by mobile devices (indoor AGVs), static obstacle reflections or dynamic pedestrian occlusions create significant blind spots in vision-based navigation systems. In addition, ToF (Time-of-Flight) depth sensors experience ranging drift under strong sunlight or on reflective metallic surfaces, further impacting system availability. Therefore, there is an urgent need for a positioning solution with multimodal perception capabilities, capable of collaborative reasoning from different information sources, and able to perform high-precision spatiotemporal fusion.

[0004] Furthermore, existing multi-sensor fusion technologies have not yet achieved joint modeling of visible light images, depth information, and wireless signals. They also lack a spatiotemporal calibration method with mathematical interpretability from acquisition, calibration, fusion to output, resulting in compromised system robustness. Particularly during motion, such as drone jitter or AGV acceleration / deceleration, image blurring and IMU drift severely impact data synchronization accuracy, further limiting the effectiveness of high-precision fusion algorithms.

[0005] Based on this, the present invention designs a spatiotemporal synchronous high-precision positioning method and system based on multi-sensor fusion to solve the above problems. Summary of the Invention

[0006] In view of the above-mentioned shortcomings of the existing technology, the present invention provides a spatiotemporal synchronous high-precision positioning method and system based on multi-sensor fusion.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A high-precision spatiotemporal synchronization positioning method based on multi-sensor fusion includes the following steps:

[0009] Step 1: Acquire data from a visible light camera Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength;

[0010] Step 2: Based on the pre-calibrated homogeneous transformation matrix The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ;

[0011] Step 3: Based on the signal strength indication received by the wireless signal module, calculate and correct the distance observation value using a nonlinear ranging model to obtain the corrected distance observation value;

[0012] Step 4: Calculate the pixel displacement caused by the motion based on the linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ;

[0013] Step 5: Use a time interpolation algorithm to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time.

[0014] Step 6: Construct a state vector containing position, linear velocity, and attitude angles. The data processed through steps 2 to 5 are used as observation vectors. The extended Kalman filter model is input for state estimation and fusion, and the real-time pose information is output.

[0015] Furthermore, for The specific operation of establishing a unified timestamp reference system for images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength is to use a master clock to stamp each frame / data packet acquired by the visible light camera, ToF depth camera, inertial measurement unit, and wireless signal module with a precise timestamp based on a unified reference.

[0016] Furthermore, spatially aligned coordinates The calculation formula is as follows:

[0017]

[0018] in, This is an RGB intrinsic parameter matrix. The coordinate image of the 3D point cloud acquired by the ToF depth camera. These are spatially aligned coordinates, which are the coordinates of the corresponding projection point on the two-dimensional image plane of the visible light camera.

[0019] Furthermore, the nonlinear ranging model is calculated as follows:

[0020]

[0021] in, The corrected distance observations are the distance observations from the nonlinear ranging model. The signal strength at the reference point; This refers to the environmental path loss factor. The term represents Gaussian noise with a mean of 0 and a variance of . ; This indicates the signal strength, specifically the actual signal strength received at the node under test.

[0022] Furthermore, the specific steps in step 4 are as follows:

[0023] Step 41: Perform motion blur correction on the 3D point cloud coordinate image from the ToF depth camera;

[0024] Step 42: Attitude change based on the output of the inertial measurement unit Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation.

[0025] Furthermore, step 41 is performed as follows:

[0026] Step 411: Measure the linear acceleration using the inertial measurement unit. Perform a second integral to calculate the time during the exposure period. Within, the pixel displacement generated by the ToF depth camera on the imaging plane. ;

[0027] Step 412: Pixel displacement Synchronize with the acquisition time of the 3D point cloud coordinate image from the ToF depth camera, and perform reverse displacement correction on the 3D point cloud coordinate image to obtain the corrected image.

[0028] Furthermore, pixel displacement The specific calculations are as follows:

[0029]

[0030] The angular velocity measured by the inertial measurement unit at the current time (t). Let be the linear acceleration at the current time (t).

[0031] Furthermore, step 42 is performed as follows:

[0032] Step 421: Read the attitude change ΔT(t) output by the inertial measurement unit;

[0033] Step 422: Using the image correction function The above attitude change amount Acting on the Images captured by each band Above, apply registration transformation to obtain

[0034] Furthermore, it also includes: Step 7: Evaluating the performance of the localization method using the average covariance as an indicator:

[0035] Step 71: Model the overall reconstruction error and calculate the prediction error vector. ;

[0036] Step 72: Facilitating the prediction of the error vector Calculate the covariance matrix;

[0037] Step 73: Calculate the mean covariance of the covariance matrix, and use the mean covariance as an indicator to evaluate the performance of the positioning method.

[0038] A high-precision spatiotemporal synchronization positioning system based on multi-sensor fusion includes:

[0039] Multi-source data acquisition and processing module: used to acquire data from a visible light camera. Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength. A time interpolation algorithm is used to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time.

[0040] Spatial registration module: used for registration based on pre-calibrated homogeneous transformation matrices. The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ;

[0041] RSSI strength and spatial distance module: used to calculate and correct distance observations based on the signal strength indication received by the wireless signal module, and obtain corrected distance observations using a nonlinear ranging model;

[0042] Auxiliary motion compensation module: Used to calculate pixel displacement caused by motion based on linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ;

[0043] Fusion state estimation module: used to implement iterative state updates based on extended Kalman filtering.

[0044] Beneficial Effects: This invention effectively addresses the inherent limitations of single-sensor positioning in complex environments by employing a complete spatiotemporal synchronization and multimodal data fusion framework. First, a unified timestamp reference is established for multi-source heterogeneous data using a master clock, and combined with temporal interpolation and spatial reprojection techniques, fundamentally ensuring data consistency across the spatiotemporal dimensions. Second, an innovative motion blur correction and image registration compensation algorithm based on an inertial measurement unit (IMU) is introduced, significantly suppressing the negative impact of device motion on depth and image data quality. Finally, the spatiotemporally aligned and motion-compensated multi-source observation data is input into an extended Kalman filter model for optimal state estimation, achieving highly robust and accurate real-time pose calculation. This invention ensures accurate fusion of data from various sensors within the same spatiotemporal reference frame, effectively avoiding measurement deviations caused by sensor asynchrony and motion distortion, achieving millimeter-level positioning accuracy while meeting real-time response requirements in highly dynamic scenarios. Attached Figure Description

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

[0046] Figure 1 This is a flowchart of the spatiotemporal synchronization high-precision positioning system based on multi-sensor fusion of the present invention. Detailed Implementation

[0047] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0048] The present invention will be further described below with reference to embodiments.

[0049] Example 1: Please refer to Figure 1 A high-precision spatiotemporal synchronous positioning method based on multi-sensor fusion includes the following steps:

[0050] Step 1: Acquire data from a visible light camera Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength;

[0051] Step 2: Based on the pre-calibrated homogeneous transformation matrix The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ;

[0052] Step 3: Based on the signal strength indication received by the wireless signal module, calculate and correct the distance observation value using a nonlinear ranging model to obtain the corrected distance observation value;

[0053] Step 4: Calculate the pixel displacement caused by the motion based on the linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ;

[0054] Step 5: Use a time interpolation algorithm to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time.

[0055] Step 6: Construct a state vector containing position, linear velocity, and attitude angles. The data processed through steps 2 to 5 are used as observation vectors. The extended Kalman filter model is input for state estimation and fusion, and the real-time pose information is output.

[0056] for The specific operation of establishing a unified timestamp reference system for images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength is to use a master clock to stamp each frame / data packet acquired by the visible light camera, ToF depth camera, inertial measurement unit, and wireless signal module with a precise timestamp based on a unified reference.

[0057] Spatial alignment coordinates The calculation formula is as follows:

[0058]

[0059] in, This is an RGB intrinsic parameter matrix. The coordinate image of the 3D point cloud acquired by the ToF depth camera. These are spatially aligned coordinates, which are the coordinates of the corresponding projection point on the two-dimensional image plane of the visible light camera.

[0060] It is a predetermined 4x4 homogeneous transformation matrix;

[0061] The ToF depth camera and the visible light camera are two sensors with different physical locations. When they see the same object, the coordinates of the object in their respective sensor coordinate systems are different. Step 2 is to unify the two "views" into one "view" (usually the RGB view of the main camera).

[0062] Only by precisely aligning the depth (3D position) information provided by ToF with the color and texture (2D image) information provided by the RGB camera at the pixel level can subsequent algorithms know "what is the depth of this red pixel in the real space" and thus perform effective fusion.

[0063] After the transformation in step 2, a "color image with accurate three-dimensional spatial location information" or a "three-dimensional point cloud with color information" is obtained, which provides accurate input data for subsequent calculations.

[0064] The specific calculations for the nonlinear ranging model are as follows:

[0065]

[0066] in, The corrected distance observations are the distance observations from the nonlinear ranging model. The signal strength at the reference point; This refers to the environmental path loss factor. The term represents Gaussian noise with a mean of 0 and a variance of . ; This indicates the signal strength, specifically the actual signal strength received at the node under test.

[0067] If GNSS signals are unusable (e.g., indoors, underground, or blocked by objects), visual sensors and inertial measurement units can only detect relative movement, leading to an accumulation of errors (commonly known as "drift"). Wireless signal ranging, on the other hand, can provide an absolute distance without drift, which precisely covers the accumulated error of the entire positioning system.

[0068] If the ToF depth camera and visible light camera are temporarily unusable due to obstruction, poor lighting, or other reasons, the wireless signal, although its accuracy may be slightly lower, can still provide location information, ensuring that the system does not completely lose its positioning.

[0069] Calculate nonlinear distance using a nonlinear ranging model The coordinates will be aligned with the space obtained in the second step. The results from step 4, together with the results from step 5, become part of the "observation data" in step 6, the extended Kalman filter. These different observation data complement each other, and the filtering algorithm can calculate the most accurate position state.

[0070] Step 4:

[0071] Step 41: Perform motion blur correction on the 3D point cloud coordinate image from the ToF depth camera;

[0072] Step 42: Attitude change based on the output of the inertial measurement unit Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation.

[0073] Step 41 is performed as follows:

[0074] Step 411: Measure the linear acceleration using the inertial measurement unit. Perform a second integral to calculate the time during the exposure period. Within, the pixel displacement generated by the ToF depth camera on the imaging plane. ;

[0075] Step 412: Pixel displacement Synchronize with the acquisition time of the 3D point cloud coordinate image from the ToF depth camera, and perform reverse displacement correction on the 3D point cloud coordinate image to obtain the corrected image.

[0076] To counteract the blurring effect caused by motion and obtain clearer depth information.

[0077] Pixel displacement The specific calculations are as follows:

[0078]

[0079] The angular velocity measured by the inertial measurement unit at the current time (t). Let be the linear acceleration at the current time (t).

[0080] Step 42 is performed as follows:

[0081] Step 421: Read the attitude change ΔT(t) output by the inertial measurement unit;

[0082] Step 422: Using the image correction function The above attitude change amount Acting on the Images captured by each band Above, apply registration transformation to obtain .

[0083] attitude change The specific calculations are as follows:

[0084]

[0085] in, This is a three-dimensional rotation matrix, representing the pose change of the ToF depth camera; Let be the translation vector, representing displacement.

[0086] Motion-compensated, aligned image frames The specific calculations are as follows:

[0087]

[0088] Paired observation data at the same time The specific calculations are as follows:

[0089]

[0090] At RGB image time The corresponding ToF data after interpolation and alignment; In order to be in 3D point cloud coordinate image of a ToF depth camera at a given time; For RGB time; and ToF depth camera in nearest neighbor The acquisition times of the two frames of data before and after the time (i.e. < < ).

[0091] Needs to be processed Moment When processing images, the system searches for the two most recent frames in the data stream of the 3D point cloud coordinate image from the ToF depth camera. Then, based on the time ratio, it uses mathematical methods to "estimate" the time frame if the ToF camera were to be in the exact same position. If the camera is taken at this moment, what kind of data will it see? This estimated data... Just with The images are perfectly aligned in time.

[0092] Step 2 (spatial projection based on the extrinsic parameter matrix) assumes that the two frames of data involved in the calculation (one visible light frame and one ToF frame) must describe the same physical instant. Step 5 ensures this, thus making the spatial transformation calculation in Step 2 accurate and meaningful.

[0093] Step 6 is performed as follows:

[0094] Step 61: Construct a state vector containing position, linear velocity, and attitude angle. ;

[0095] Let be the absolute coordinates of the mobile device in three-dimensional space at the current time (t). The linear velocity components of the mobile device in the three-axis directions in three-dimensional space; The pitch angle of a mobile device describes the rotation of an object around the Y-axis (lateral axis), i.e., the "nodding" motion. The roll angle of a mobile device describes the rotation of an object around the X-axis (vertical axis), i.e., the "roll" motion. The yaw angle of the mobile device describes the rotation of an object around the Z-axis (vertical axis), i.e., the "turning head" motion;

[0096] Step 62: Align the coordinates of the space after processing in steps 2 to 5. Corrected distance observations, corrected images, motion-compensated, aligned image frames Paired observation data at the same time are used as observation vectors The extended Kalman filter model is input for state estimation and fusion, and the real-time pose information is output.

[0097] The extended Kalman filter model is calculated in detail below:

[0098]

[0099]

[0100] in, For the multi-source observations at the current time t, Let be the Jacobian matrix of the observation function at the current time t. To observe the covariance matrix, The Kalman gain coefficient at the current time t. Let be the state covariance matrix at the previous time step (t-1).

[0101] This also includes: Step 7: Evaluating the performance of the localization method using the average covariance as an indicator.

[0102] Step 71: Model the overall reconstruction error and calculate the prediction error vector. ;

[0103]

[0104] In the band The true value or ground truth value, Indicates the predicted or reconstructed value in the band. The value below, The prediction error vector is the difference between the system's predicted value and the actual value. It is used to measure the system's performance in a specific frequency band. The accuracy of the predictions;

[0105] Step 72: Facilitating the prediction of the error vector Calculate the covariance matrix;

[0106] The covariance matrix is ​​calculated as follows:

[0107]

[0108] For the prediction error vector The covariance matrix;

[0109] Step 73: Calculate the mean covariance of the covariance matrix, and use the mean covariance as an indicator to evaluate the performance of the positioning method.

[0110]

[0111] in, The total number of target bands that need to be processed or reconstructed. It is the trace operation of a matrix. For all target bands ( arrive trace of covariance matrix The average value, The smaller the value, the more stable the localization method's predictions.

[0112] A high-precision spatiotemporal synchronization positioning system based on multi-sensor fusion includes:

[0113] Multi-source data acquisition and processing module: used to acquire data from a visible light camera. Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength. A time interpolation algorithm is used to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time.

[0114] Spatial registration module: used for registration based on pre-calibrated homogeneous transformation matrices. The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ;

[0115] RSSI strength and spatial distance module: used to calculate and correct distance observations based on the signal strength indication received by the wireless signal module, and obtain corrected distance observations using a nonlinear ranging model;

[0116] Auxiliary motion compensation module: Used to calculate pixel displacement caused by motion based on linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ;

[0117] Fusion state estimation module: used to implement iterative state updates based on extended Kalman filtering.

[0118] This invention effectively addresses the inherent limitations of single-sensor positioning in complex environments by employing a complete spatiotemporal synchronization and multimodal data fusion framework. First, a unified timestamp reference is established for multi-source heterogeneous data using a master clock, and combined with temporal interpolation and spatial reprojection techniques, fundamentally ensuring data consistency across the spatiotemporal dimensions. Second, an innovative motion blur correction and image registration compensation algorithm based on an inertial measurement unit (IMU) is introduced, significantly suppressing the negative impact of device motion on depth and image data quality. Finally, the spatiotemporally aligned and motion-compensated multi-source observation data is input into an extended Kalman filter model for optimal state estimation, achieving highly robust and accurate real-time pose calculation. This invention ensures accurate fusion of data from various sensors within the same spatiotemporal reference frame, effectively avoiding measurement deviations caused by sensor asynchrony and motion distortion, achieving millimeter-level positioning accuracy while meeting the real-time response requirements of highly dynamic scenarios.

[0119] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention 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 will not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A high-precision spatiotemporal synchronous positioning method based on multi-sensor fusion, characterized in that, Includes the following steps: Step 1: Acquire data from a visible light camera Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength; Step 2: Based on the pre-calibrated homogeneous transformation matrix The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ; Step 3: Based on the signal strength indication received by the wireless signal module, calculate and correct the distance observation value using a nonlinear ranging model to obtain the corrected distance observation value; Step 4: Calculate the pixel displacement caused by the motion based on the linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ; Step 5: Use a time interpolation algorithm to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time. Step 6: Construct a state vector containing position, linear velocity, and attitude angles. The data processed through steps 2 to 5 are used as observation vectors. The extended Kalman filter model is input for state estimation and fusion, and the real-time pose information is output.

2. The positioning method according to claim 1, characterized in that, for The specific operation of establishing a unified timestamp reference system for images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength is to use a master clock to stamp each frame / data packet acquired by the visible light camera, ToF depth camera, inertial measurement unit, and wireless signal module with a precise timestamp based on a unified reference.

3. The positioning method according to claim 2, characterized in that, Spatial alignment coordinates The calculation formula is as follows: ； in, This is an RGB intrinsic parameter matrix. The coordinate image of the 3D point cloud acquired by the ToF depth camera. These are spatially aligned coordinates, which are the coordinates of the corresponding projection point on the two-dimensional image plane of the visible light camera.

4. The positioning method according to claim 3, characterized in that, The specific calculations for the nonlinear ranging model are as follows: ； in, The corrected distance observations are the distance observations from the nonlinear ranging model. The signal strength at the reference point; This refers to the environmental path loss factor. The term represents Gaussian noise with a mean of 0 and a variance of . ; This indicates the signal strength, specifically the actual signal strength received at the node under test.

5. The positioning method according to claim 4, characterized in that, Step 4: Step 41: Perform motion blur correction on the 3D point cloud coordinate image from the ToF depth camera; Step 42: Attitude change based on the output of the inertial measurement unit Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation.

6. The positioning method according to claim 5, characterized in that, Step 41 is performed as follows: Step 411: Measure the linear acceleration using the inertial measurement unit. Perform a second integral to calculate the time during the exposure period. Within, the pixel displacement generated by the ToF depth camera on the imaging plane. ; Step 412: Pixel displacement Synchronize with the acquisition time of the 3D point cloud coordinate image from the ToF depth camera, and perform reverse displacement correction on the 3D point cloud coordinate image to obtain the corrected image.

7. The positioning method according to claim 6, characterized in that, Pixel displacement The specific calculations are as follows: ； The angular velocity measured by the inertial measurement unit at the current time (t). Let be the linear acceleration at the current time (t).

8. The positioning method according to claim 2, characterized in that, Step 42 is performed as follows: Step 421: Read the attitude change ΔT(t) output by the inertial measurement unit; Step 422: Using the image correction function The above attitude change amount Acting on the Images captured by each band Above, apply registration transformation to obtain 9. The positioning method according to claim 2, characterized in that, This also includes: Step 7: Evaluating the performance of the localization method using the average covariance as an indicator. Step 71: Model the overall reconstruction error and calculate the prediction error vector. ; Step 72: Facilitating the prediction of the error vector Calculate the covariance matrix; Step 73: Calculate the mean covariance of the covariance matrix, and use the mean covariance as an indicator to evaluate the performance of the positioning method.

10. A high-precision spatiotemporal synchronous positioning system based on multi-sensor fusion, characterized in that, include: Multi-source data acquisition and processing module: used to acquire data from a visible light camera. Images, 3D point cloud coordinate images from a ToF depth camera, angular velocity and acceleration from an inertial measurement unit, and wireless signal strength from a wireless signal module, and for... A unified timestamp reference system is established using images, 3D point cloud coordinate images, angular velocity, acceleration, and wireless signal strength. A time interpolation algorithm is used to synchronize the raw data streams of the visible light camera and the ToF depth camera to obtain paired observation data at the same time. Spatial registration module: used for registration based on pre-calibrated homogeneous transformation matrices. The 3D point cloud coordinate image acquired by the ToF depth camera is reprojected onto the image plane of the visible light camera to obtain spatially aligned coordinates. ; RSSI strength and spatial distance module: used to calculate and correct distance observations based on the signal strength indication received by the wireless signal module, and obtain corrected distance observations using a nonlinear ranging model; Auxiliary motion compensation module: Used to calculate pixel displacement caused by motion based on linear acceleration and angular velocity measured by the inertial measurement unit. Motion blur correction is performed on the 3D point cloud coordinate image from the ToF depth camera to obtain the corrected image; and the attitude change is based on the output of the inertial measurement unit. Registration and compensation are performed on multi-band images to obtain aligned image frames after motion compensation. ; Fusion state estimation module: used to implement iterative state updates based on extended Kalman filtering.

Images