Multi-modal sensor fused moving object positioning method and device

Abstract

The invention discloses a moving object positioning method and device based on multi-modal sensor fusion, and belongs to the technical field of multi-sensor fusion and computer vision. In order to solve the problems that a single IMU has drift errors and a single visual system is influenced by illumination and shielding, high-frequency dynamic data of the IMU and low-frequency but high-precision visual information provided by a fixed camera are effectively combined through the steps of time synchronization, coordinate system calibration, extended Kalman filtering fusion, error optimization and the like. The system firstly performs interpolation alignment on the asynchronously acquired IMU and image data, and then unifies the IMU and image data to a world coordinate system; then, the state is predicted through an IMU by means of an EKF, drifting is corrected through image feature points, and finally high-precision and high-robustness three-dimensional positioning and attitude estimation are achieved. The method is suitable for medical ultrasonic scanning assistance, robot navigation, augmented reality, unmanned driving and other application scenes with limited GPS or complex environment, and has the advantages of high real-time performance, high precision, wide adaptability and the like.

Description

Technical Field

[0001] This invention relates to the field of sensor fusion technology, and in particular to a method and apparatus for locating moving objects using multimodal sensor fusion. Background Technology

[0002] Sensor fusion technology, as a core support for intelligent sensing systems, is widely used in fields such as robot navigation, augmented reality, and medical imaging assistance. With the increasing demands for environmental perception accuracy from automated equipment, traditional positioning methods have evolved from single-sensor approaches to multimodal fusion systems. Among these, the collaborative operation of inertial measurement units (IMUs) and visual sensors has built a dynamic positioning technology ecosystem, encompassing the entire process from high-frequency motion data acquisition to 3D spatial modeling.

[0003] However, existing technologies that directly use independent data streams from the IMU and vision sensor fail to effectively address the spatiotemporal alignment errors (typical time deviations of ±50ms) caused by asynchronous sampling of cross-modal data. Specifically, while IMUs possess high sampling frequencies (hundreds to thousands of Hz) and environment-independent characteristics, long-term use leads to drift errors, causing position estimation bias to increase exponentially. Although vision sensors can provide 3D spatial positioning information, they are limited by illumination sensitivity (failure in scenes <50 lux or >100,000 lux) and occlusion issues. Based on this, traditional vision-inertial fusion (VIO) methods... Perform state prediction. While measurements are updated, the system still suffers from error amplification due to the coupling between organ movement and probe movement in medical scenarios. Furthermore, the covariance matrix divergence in nonlinear system modeling and the insufficient stability of feature point matching in dynamic environments significantly limit the system's long-term accuracy and robustness. Summary of the Invention

[0004] The present invention aims to at least partially solve one of the technical problems in the related art.

[0005] Therefore, the first objective of this invention is to propose a method for locating moving objects using multimodal sensor fusion.

[0006] Another objective of this invention is to provide a mobile object positioning device based on multimodal sensor fusion.

[0007] The third objective of this invention is to provide a computer device.

[0008] A fourth objective of this invention is to provide a non-transitory computer-readable storage medium.

[0009] To achieve the above objectives, a first aspect of the present invention proposes a method for locating a moving object using multimodal sensor fusion, comprising: S1: Acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and use an interpolation algorithm to timestamp-align the asynchronous sampled data to eliminate time deviations between sensors; S2, based on camera calibration parameters and IMU attitude information, transforms the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system, establishing spatial consistency of cross-modal data; S3 uses the extended Kalman filter algorithm to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system, predicts the position and attitude of objects through a nonlinear state transition model, and uses image feature points for error correction. S4 dynamically adjusts and optimizes the regularization coefficient of the objective function based on the object's motion state, balancing visual correction accuracy and motion trajectory smoothness, and achieving real-time suppression of drift error.

[0010] In one embodiment of the present invention, S1 includes: The IMU data is time-aligned using a linear interpolation algorithm, and the calculation formula is as follows:

[0011] Where R is the rotation matrix and T is the translation vector. This represents the object's position in the camera coordinate system; The sampling frequency of the inertial measurement unit is 250Hz-1000Hz, and the sampling frequency of the fixed camera is 20Hz-60Hz. After the high-frequency data of the IMU is cached by the data buffer module, interpolation mapping is performed according to the camera timestamp to achieve synchronization alignment with a time deviation of less than 1ms.

[0012] In one embodiment of the present invention, S2 includes: Camera calibration parameters include intrinsic and extrinsic parameters. Intrinsic parameters include focal length and principal point position, while extrinsic parameters include the camera's rotation matrix R and displacement vector T relative to the world coordinate system. The transformation from the IMU coordinate system to the world coordinate system is achieved through the attitude matrix Aimu, and the transformation formula is as follows: Where Pimu is the measurement value in the local coordinate system of the IMU, and Pimu_world is the IMU data in the world coordinate system.

[0013] In one embodiment of the present invention, S3 includes: Define a 9-dimensional state vector ,in Indicates the position of an object. Indicates speed, Indicates attitude angle; Through state transition function and measurement function Modeling a nonlinear system, where the state update formula is:

[0014] The measurement update formula is:

[0015] Where zk represents the 3D coordinates of the feature points extracted by the camera, and vk represents the measurement noise.

[0016] In one embodiment of the present invention, S4 includes: Calculate the optimization objective function ,in, The location of the object extracted from the image. For the object position predicted by EKF, This is the regularization coefficient, ensuring the smoothness of the optimization process; Dynamically adjusted based on the rate of change of acceleration The value is given when the rate of change of acceleration exceeds 0.5 m / s². Decrease within the range of 0.1-1.0.

[0017] To achieve the above objectives, a second aspect of the present invention provides a moving object localization device based on multimodal sensor fusion, comprising: The asynchronous data alignment module is used to acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and to perform timestamp alignment on the asynchronous sampled data through an interpolation algorithm to eliminate time deviation between sensors. The cross-modal space transformation module is used to transform the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system based on the camera calibration parameters and IMU attitude information, thereby establishing spatial consistency of cross-modal data. The extended Kalman filter fusion module is used to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system using the extended Kalman filter algorithm. It predicts the position and attitude of objects through a nonlinear state transition model and uses image feature points for error correction. The dynamic regularization coefficient adjustment module is used to dynamically adjust the regularization coefficient of the objective function according to the motion state of the object, balance the visual correction accuracy and the smoothness of the motion trajectory, and achieve real-time suppression of drift error.

[0018] The present invention discloses a method and apparatus for locating moving objects by multimodal sensor fusion, which can effectively fuse data from an IMU and a fixed camera, significantly improve the positioning accuracy and system robustness of moving objects in dynamic and lighting changing environments, and resolve the contradiction between long-term IMU drift and the environmental dependence of visual sensors.

[0019] To achieve the above objectives, a third aspect of this application provides a computer device, including a processor and a memory; wherein the processor runs a program corresponding to the executable program code by reading executable program code stored in the memory, for implementing a multimodal sensor fusion method for locating moving objects as described in the first aspect embodiment.

[0020] To achieve the above objectives, a fourth aspect of this application provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a multimodal sensor fusion method for locating a moving object as described in the first aspect embodiment.

[0021] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0022] Figure 1 This is a flowchart of a moving object localization method based on multimodal sensor fusion according to an embodiment of the present invention; Figure 2 This is a flowchart of the Kalman filter fusion process according to an embodiment of the present invention; Figure 3 This is a framework diagram of a moving object localization system based on multimodal sensor fusion according to an embodiment of the present invention; Figure 4 This is a structural diagram of a moving object positioning device based on multimodal sensor fusion according to an embodiment of the present invention; Figure 5 It is a computer device according to an embodiment of the present invention. Detailed Implementation

[0023] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

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

[0025] The following description, with reference to the accompanying drawings, describes a method and apparatus for locating moving objects using multimodal sensor fusion according to an embodiment of the present invention.

[0026] Figure 1 This is a flowchart of a moving object localization method based on multimodal sensor fusion according to an embodiment of the present invention, such as... Figure 1 As shown, it includes: S1: Acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and use an interpolation algorithm to timestamp-align the asynchronous sampled data to eliminate time deviations between sensors; S2, based on camera calibration parameters and IMU attitude information, transforms the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system, establishing spatial consistency of cross-modal data; S3 uses the extended Kalman filter algorithm to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system, predicts the position and attitude of objects through a nonlinear state transition model, and uses image feature points for error correction. S4 dynamically adjusts and optimizes the regularization coefficient of the objective function based on the object's motion state, balancing visual correction accuracy and motion trajectory smoothness, and achieving real-time suppression of drift error.

[0027] Furthermore, this invention proposes another method for locating moving objects using multimodal sensor fusion, such as... Figure 2 As shown, this invention aims to improve the accuracy and robustness of object positioning, especially in dynamic environments and under conditions of significant lighting variations. Through reasonable data synchronization and sensor fusion techniques, this invention overcomes the limitations of single-sensor applications in complex environments, providing an efficient and reliable positioning solution, specifically as follows: Figure 3 As shown: Step 1, System Hardware and Data Acquisition, includes: IMU Sensor: An IMU sensor mounted on the ultrasonic probe acquires acceleration, angular velocity, and attitude data in real time. The IMU's sampling frequency is set to 250Hz to ensure sufficiently rapid data updates during high-dynamic motion. Fixed Camera: A fixedly mounted camera acquires 30 frames of image data per second, identifying object positions through feature points in the images. There is a certain time discrepancy between the camera and the IMU; therefore, precise time synchronization is used to align the IMU and camera data. Data Synchronization: Due to the different sampling frequencies of the IMU and camera, interpolation algorithms (such as linear interpolation or spline interpolation) are used to map the IMU data to the camera's timestamps, ensuring data synchronization.

[0028] Step 2, coordinate system calibration and data transformation, includes: Camera calibration with the world coordinate system: The camera calibration process yields its intrinsic and extrinsic parameters. Intrinsic parameters include the camera's focal length and principal point position, while extrinsic parameters represent the camera's rotation matrix and translation vector relative to the world coordinate system. Using the camera's extrinsic parameters, the 3D coordinates of the object in the camera coordinate system are transformed to the world coordinate system.

[0029]

[0030] Where R is the rotation matrix and T is the translation vector. It is the object's position in the camera coordinate system. It is the position of an object in the world coordinate system.

[0031] IMU to World Coordinate System Conversion: IMU sensors are typically based on the object's local coordinate system, therefore, it is necessary to convert the IMU measurements to the world coordinate system. The IMU's attitude matrix is ​​used to convert the IMU's acceleration and angular velocity information from the local coordinate system to physical quantities in the world coordinate system.

[0032] in, Let be the attitude matrix of the IMU, representing the rotational transformation from the IMU coordinate system to the world coordinate system.

[0033] Step 3, multi-sensor data fusion, includes: After data synchronization and coordinate system transformation, this invention fuses IMU and camera data using extended Kalman filtering (EKF), thereby improving the accuracy of object position and attitude estimation. State model: The system state is defined as the object's position, velocity, and attitude; the state vector is:

[0034] Where (x, y, z) represents the position of the object. Represents the velocity of an object. This represents the object's attitude (pitch angle, yaw angle, roll angle). Extended Kalman Filter (EKF): It uses the acceleration and angular velocity provided by the IMU for state prediction and update, and then corrects the state based on image feature points measured by the camera using the extended Kalman filter algorithm. The system's state update equation is: in, This is the state transition function. For control input (IMU data), This is process noise.

[0035] Image measurement update: Image data is updated by measuring the locations of feature points extracted from the camera. The measurement update equation is:

[0036] in, For image measurements, For the predicted object feature location, To measure noise, the EKF algorithm corrects the IMU's positioning drift by minimizing the error.

[0037] Step 4, Error Correction and Optimization, includes: Error Correction: Accumulated errors in the IMU sensor over time can lead to object position shifts. To correct these errors, this invention utilizes visual data from the camera. By comparing feature points in the image with a known environmental model, the IMU's position information is optimized in real time.

[0038] Optimize the objective function: Correct the IMU error by optimizing the objective function, which is as follows:

[0039] in, The location of the object extracted from the image. For the object position predicted by EKF, This is the regularization coefficient, ensuring the smoothness of the optimization process.

[0040] Step 5, Applications and advantages include: Using the methods described above, the system can provide precise real-time positioning of the ultrasound probe, exhibiting strong robustness and adaptability, especially under varying lighting conditions and dynamic environments. This system has broad application prospects in fields such as gastric ultrasound scanning assistance.

[0041] This invention not only solves the IMU drift problem but also corrects IMU errors using camera image data, improving system accuracy and stability. This method is widely applicable to various fields such as automated equipment, robot navigation, autonomous driving, and augmented reality, and is particularly suitable for complex environments where GPS signals or traditional sensors cannot be relied upon. Through this multi-sensor fusion technology, this invention can provide more accurate and robust positioning and tracking in dynamic, occluded, or lighting-varying environments, especially in indoor environments assisted by medical ultrasound scanning, when assisting in the 3D reconstruction of organs.

[0042] To achieve the above embodiments, such as Figure 4 As shown, this embodiment also provides a multimodal sensor fusion-based mobile object positioning device 10, including: The asynchronous data alignment module 100 is used to acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and to perform timestamp alignment on the asynchronous sampled data through an interpolation algorithm to eliminate time deviation between sensors. The cross-modal space transformation module 200 is used to transform the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system based on the camera calibration parameters and IMU attitude information, thereby establishing the spatial consistency of cross-modal data. The extended Kalman filter fusion module 300 is used to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system using the extended Kalman filter algorithm. It predicts the position and attitude of objects through a nonlinear state transition model and uses image feature points for error correction. The dynamic regularization coefficient adjustment module 400 is used to dynamically adjust and optimize the regularization coefficient of the objective function according to the motion state of the object, balance the visual correction accuracy and the smoothness of the motion trajectory, and achieve real-time suppression of drift error.

[0043] Furthermore, the asynchronous data alignment module 100 is also used for: The IMU data is time-aligned using a linear interpolation algorithm, and the calculation formula is as follows:

[0044] Where R is the rotation matrix and T is the translation vector. This represents the object's position in the camera coordinate system; The sampling frequency of the inertial measurement unit is 250Hz-1000Hz, and the sampling frequency of the fixed camera is 20Hz-60Hz. After the high-frequency data of the IMU is cached by the data buffer module, interpolation mapping is performed according to the camera timestamp to achieve synchronization alignment with a time deviation of less than 1ms.

[0045] Furthermore, the cross-modal space conversion module 200 is also used for: Camera calibration parameters include intrinsic and extrinsic parameters. Intrinsic parameters include focal length and principal point position, while extrinsic parameters include the camera's rotation matrix R and displacement vector T relative to the world coordinate system. The transformation from the IMU coordinate system to the world coordinate system is achieved through the attitude matrix Aimu, and the transformation formula is as follows: Where Pimu is the measurement value in the local coordinate system of the IMU, and Pimu_world is the IMU data in the world coordinate system.

[0046] To implement the methods of the above embodiments, the present invention also provides a computer device, such as... Figure 5 As shown, the computer device 600 includes a memory 601 and a processor 602; wherein, the processor 602 reads the executable program code stored in the memory 601 to run a program corresponding to the executable program code, so as to implement the various steps of the multimodal sensor fusion moving object localization method described above.

[0047] To implement the above embodiments, this application also proposes a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a multimodal sensor fusion method for locating moving objects as described in the foregoing embodiments.

[0048] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0049] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

Claims

1. A method for locating a moving object using multimodal sensor fusion, characterized in that, include: S1: Acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and use an interpolation algorithm to timestamp-align the asynchronous sampled data to eliminate time deviations between sensors; S2, based on camera calibration parameters and IMU attitude information, transforms the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system, establishing spatial consistency of cross-modal data; S3 uses the extended Kalman filter algorithm to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system, predicts the position and attitude of objects through a nonlinear state transition model, and uses image feature points for error correction. S4 dynamically adjusts and optimizes the regularization coefficient of the objective function based on the object's motion state, balancing visual correction accuracy and motion trajectory smoothness, and achieving real-time suppression of drift error.

2. The method as described in claim 1, characterized in that, S1 includes: The IMU data is time-aligned using a linear interpolation algorithm, and the calculation formula is as follows: Where R is the rotation matrix and T is the translation vector. This represents the object's position in the camera coordinate system; The sampling frequency of the inertial measurement unit is 250Hz-1000Hz, and the sampling frequency of the fixed camera is 20Hz-60Hz. After the high-frequency data of the IMU is cached by the data buffer module, interpolation mapping is performed according to the camera timestamp to achieve synchronization alignment with a time deviation of less than 1ms.

3. The method as described in claim 1, characterized in that, The S2 includes: Camera calibration parameters include intrinsic and extrinsic parameters. Intrinsic parameters include focal length and principal point position, while extrinsic parameters include the camera's rotation matrix R and displacement vector T relative to the world coordinate system. The transformation from the IMU coordinate system to the world coordinate system is achieved through the attitude matrix Aimu, and the transformation formula is as follows: Where Pimu is the measurement value in the local coordinate system of the IMU, and Pimu_world is the IMU data in the world coordinate system.

4. The method as described in claim 1, characterized in that, The S3 includes: Define a 9-dimensional state vector ,in Indicates the position of an object. Indicates speed, Indicates attitude angle; Through state transition function and measurement function Modeling a nonlinear system, where the state update formula is: The measurement update formula is: Where zk represents the 3D coordinates of the feature points extracted by the camera, and vk represents the measurement noise.

5. The method as described in claim 1, characterized in that, The S4 includes: Calculate the optimization objective function ,in, The location of the object extracted from the image. For the object position predicted by EKF, This is the regularization coefficient, ensuring the smoothness of the optimization process; Dynamically adjusted based on the rate of change of acceleration The value is given when the rate of change of acceleration exceeds 0.5 m / s². Decrease within the range of 0.1-1.

0.

6. A moving object positioning device based on multimodal sensor fusion, characterized in that, include: The asynchronous data alignment module is used to acquire high-frequency dynamic data from the inertial measurement unit and low-frequency image data from the fixed camera, and to perform timestamp alignment on the asynchronous sampled data through an interpolation algorithm to eliminate time deviation between sensors. The cross-modal space transformation module is used to transform the local coordinate system data of the IMU and the image coordinate system data of the camera to a unified world coordinate system based on the camera calibration parameters and IMU attitude information, thereby establishing spatial consistency of cross-modal data. The extended Kalman filter fusion module is used to fuse IMU dynamic prediction data and camera visual correction data in the world coordinate system using the extended Kalman filter algorithm. It predicts the position and attitude of objects through a nonlinear state transition model and uses image feature points for error correction. The dynamic regularization coefficient adjustment module is used to dynamically adjust the regularization coefficient of the objective function according to the motion state of the object, balance the visual correction accuracy and the smoothness of the motion trajectory, and achieve real-time suppression of drift error.

7. The apparatus as claimed in claim 6, characterized in that, The asynchronous data alignment module is also used for: The IMU data is time-aligned using a linear interpolation algorithm, and the calculation formula is as follows: Where R is the rotation matrix and T is the translation vector. This represents the object's position in the camera coordinate system; The sampling frequency of the inertial measurement unit is 250Hz-1000Hz, and the sampling frequency of the fixed camera is 20Hz-60Hz. After the high-frequency data of the IMU is cached by the data buffer module, interpolation mapping is performed according to the camera timestamp to achieve synchronization alignment with a time deviation of less than 1ms.

8. The apparatus as claimed in claim 6, characterized in that, The cross-modal space conversion module is also used for: Camera calibration parameters include intrinsic and extrinsic parameters. Intrinsic parameters include focal length and principal point position, while extrinsic parameters include the camera's rotation matrix R and displacement vector T relative to the world coordinate system. The transformation from the IMU coordinate system to the world coordinate system is achieved through the attitude matrix Aimu, and the transformation formula is as follows: Where Pimu is the measurement value in the local coordinate system of the IMU, and Pimu_world is the IMU data in the world coordinate system.

9. A computer device, characterized in that, Including processor and memory; The processor reads executable program code stored in the memory to run a program corresponding to the executable program code, so as to implement a multimodal sensor fusion method for locating moving objects as described in any one of claims 1-5.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by the processor, the program implements a moving object localization method based on multimodal sensor fusion as described in any one of claims 1-5.

Images