Mobile terminal track visualization system based on IMU and VINS fusion

Abstract

The invention discloses a mobile terminal track visualization system based on IMU and VINS fusion, and belongs to the technical field of mobile terminals, the mobile terminal track visualization system comprises an Xsens DOT module, a VINS algorithm module and a display analysis module, the Xsens DOT module and the VINS algorithm module are connected with the display analysis module, the Xsens DOT module comprises a Bluetooth data communication module, a data display module and a data storage module, the Bluetooth data communication module is connected with the VINS algorithm module, and the VINS algorithm module is connected with the display analysis module. The Bluetooth data communication module is connected with the data display module, the data display module is connected with the data storage module, the VINS algorithm module comprises an IMU integral track display sub-module and an algorithm state control sub-module, and the algorithm state control sub-module is connected with the IMU integral track display sub-module. By adopting the system, the motion trail output by the VINS and the foot motion data collected by the Xsens DOT can be presented in parallel, and the practicability and the collaboration of the Xsens DOT and the VINS system in motion analysis are improved.

Description

Technical Field

[0001] The present invention relates to the field of mobile terminal technology, and in particular to a mobile terminal trajectory visualization system based on the fusion of IMU and VINS. Background Art

[0002] In recent years, inertial measurement units (IMUs) and visual navigation technologies have been widely used in fields such as human motion perception, behavior analysis, and spatial positioning. Xsens DOT, a portable, high-precision six-axis IMU sensor, has been widely used in practical applications such as posture estimation, gait analysis, and sports rehabilitation due to its excellent dynamic response capabilities and Bluetooth communication interface. Furthermore, visual inertial navigation systems (VINS) achieve high-precision, low-drift three-dimensional trajectory estimation by jointly modeling camera image information with IMU data. This technology is widely used in areas such as drone navigation, robot SLAM, and augmented reality.

[0003] Currently, both the Xsens DOT and VINS algorithms support mobile platforms such as Android. Some researchers have deployed Xsens DOT data acquisition and VINS trajectory estimation functions on terminal devices for preliminary motion trajectory analysis. However, Xsens DOT data is typically stored independently or used for post-processing gait estimation, while VINS trajectories are used solely for spatial positioning. The two lack a unified display platform. This is particularly true in applications requiring joint analysis of an individual's full-body motion trajectory and foot movements. Current systems lack the integrated management and visual comparison of trajectory information, limiting their practical application in behavioral monitoring and motion analysis. Summary of the Invention

[0004] The purpose of this invention is to provide a mobile terminal trajectory visualization system based on the fusion of IMU and VINS, which can present the motion trajectory output by VINS and the foot motion data collected by Xsens DOT in parallel, thereby improving the practicality and synergy of Xsens DOT and VINS systems in motion analysis.

[0005] To achieve the above objectives, the present invention provides a mobile terminal trajectory visualization system based on the fusion of IMU and VINS, including an Xsens DOT module, a VINS algorithm module and a display and analysis module. The Xsens DOT module and the VINS algorithm module are respectively connected to the display and analysis module. The Xsens DOT module includes a Bluetooth data communication module, a data display module and a data storage module. The Bluetooth data communication module is connected to the data display module, and the data display module is connected to the data storage module. The VINS algorithm module includes an IMU integral trajectory display submodule and an algorithm state control submodule. The algorithm state control submodule is connected to the IMU integral trajectory display submodule.

[0006] Preferably, the Bluetooth data communication module uses the BLE protocol to establish a stable connection with the Xsens DOT device. The Xsens DOT device automatically scans and lists nearby sensor devices, and collects data measured by the sensor after connecting to the sensor device. The data is parsed in a fixed format, which includes a frame header, timestamp, and data body.

[0007] Preferably, after the Bluetooth data communication module is connected to the sensor device, the data display module compiles the acquired sensor data, and after the compilation is completed, the sensor data is displayed in a numerical form. At the same time, the Xsens DOT trajectory is obtained based on the integration of the acquired sensor data.

[0008] Preferably, the data storage module obtains the data collected by the Bluetooth data communication module and saves it in a CSV general format based on the timestamp.

[0009] Preferably, the algorithm state control submodule is based on multi-sensor state estimation theory, combined with IMU pre-integration, visual reprojection and sliding window optimization methods to estimate the spatial trajectory and generate a three-dimensional motion trajectory.

[0010] Preferably, the IMU integral trajectory display submodule displays the three-dimensional motion trajectory in real time.

[0011] Preferably, the display analysis module converts the Xsens DOT trajectory and the VINS trajectory into a unified coordinate system and aligns them with the time axis, and superimposes or independently displays the relevant trajectories in the 3D scene. At the same time, it compares the trajectory deviation and time step in the Xsens DOT module and the VINS algorithm module, and analyzes the trajectory error, motion period and offset rate based on the comparative data.

[0012] A mobile terminal trajectory visualization APP based on the fusion of IMU and VINS is also provided, which applies a mobile terminal trajectory visualization system based on the fusion of IMU and VINS, and the APP is installed on an Android device.

[0013] Therefore, the present invention adopts the above-mentioned mobile terminal trajectory visualization system based on the fusion of IMU and VINS, which has the following beneficial effects:

[0014] (1) By integrating the Xsens DOT inertial sensor and the VINS visual inertial navigation algorithm on the Android platform, a unified data acquisition, trajectory estimation, and visualization platform is provided, improving the collaborative processing capabilities of multi-source perception information;

[0015] (2) Through a unified time synchronization and coordinate alignment mechanism, the present invention can present the motion trajectory output by VINS and the foot motion data collected by Xsens DOT in parallel, thereby improving the practicality and synergy of Xsens DOT and VINS systems in motion analysis and providing users with a more intuitive and complete motion analysis reference;

[0016] (3) It can be used in fields such as rehabilitation medicine and gait recognition to meet diverse demands for trajectory accuracy, display efficiency, and analysis depth.

[0017] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 A module relationship diagram of an embodiment of the present invention;

[0019] Figure 2 This is a flowchart of the APP operation of an embodiment of the present invention. DETAILED DESCRIPTION

[0020] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings here can be arranged and designed in various different configurations. In the description of the present invention, it should be noted that the orientation or position relationship indicated by the terms "upper", "lower", "inside", "outside", etc. is based on the orientation or position relationship shown in the drawings, or is the orientation or position relationship in which the product of the invention is usually placed when in use. It is only for the convenience of describing the present invention and simplifying the description, and does not indicate or imply that the APP or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0021] Example

[0022] The present invention provides a mobile terminal trajectory visualization system based on the fusion of IMU and VINS. Figure 1As shown, the system includes an Xsens DOT module, a VINS algorithm module, and a display analysis module. The Xsens DOT module and the VINS algorithm module are connected to the display analysis module. The system runs on the Android platform and is developed in Java / Kotlin. Some algorithm modules integrate C++ libraries.

[0023] The Xsens DOT module is used to obtain high-precision, consistent wearable device data. It supports high-frequency acquisition and display of raw data such as acceleration, angular velocity, and magnetometers, and performs visualization, storage, and task-based management. The Xsens DOT module includes a Bluetooth data communication module, a data display module, and a data storage module. The Bluetooth data communication module is connected to the data display module, which in turn is connected to the data storage module. The implementation process of each module includes:

[0024] Bluetooth data communication module: Uses the BLE protocol to establish a stable connection with the Xsens DOT device. The Xsens DOT device automatically scans and lists nearby sensor devices. The user establishes a connection by selecting pairing. After the connection, the sensor data measured by the sensor is collected. The sensor device integrates sensors such as accelerometers, angular velocity sensors, and magnetometers. The data is parsed in a fixed format (frame header, timestamp, and data body). It should be noted that up to 7 IMU devices can be connected on the Android platform, and multi-device switching is supported to improve system robustness. The system sampling frequency is 60hz, and the recording duration and file naming rules can also be changed to ensure data integrity and organization.

[0025] After the Bluetooth data communication module is connected to the sensor device, the data display module compiles the acquired sensor data and displays it in numerical form. The data can display the three-axis acceleration from Xsens DOT. x ,a y ,a z , free acceleration, angular velocity and magnetometer data m x ,m y ,m z At the same time, the Xsens DOT foot trajectory is obtained by integrating the acceleration data. It is important to note that when measuring data, it is necessary to avoid highly magnetized areas as they can affect data accuracy and output.

[0026] The data storage module acquires data collected by the Bluetooth data communication module. Each data message from the Xsens DOT is accompanied by a packet counter and a timestamp. The packet counter starts with each new measurement and increments with each generated data message. The collected data is stored with a timestamp and saved in a common CSV format after stopping data collection.

[0027] The VINS algorithm module estimates and visualizes the device's spatial trajectory. Specifically, it uses the phone's built-in accelerometer, gyroscope, and camera to perform six-degree-of-freedom pose estimation and trajectory reconstruction, outputting a three-dimensional path in real time to facilitate observation of full-body motion trends. The VINS algorithm module includes an IMU integral trajectory display submodule and an algorithm status control submodule, which are connected to the IMU integral trajectory display submodule.

[0028] The algorithm state control submodule estimates the trajectory of a mobile device in space. Its input data includes IMU data and images from the phone itself, and its output is position and attitude information in continuous time. Its core concept is to estimate the spatial trajectory based on multi-sensor state estimation theory, combining IMU pre-integration, visual reprojection, and sliding window optimization methods to generate a three-dimensional motion trajectory. The algorithm state control submodule mainly includes the following:

[0029] Image preprocessing and feature tracking: Each time a frame of image arrives, it first performs dedistortion processing and extracts sparse corner points in the image as visual features. If inter-frame tracking fails or the number of tracking is insufficient, the features are extracted again. The tracking algorithm uses the optical flow method (KLT) to calculate the position of the corner points in the previous frame. Use Newton iteration to solve its matching point in the current frame The formula is:

[0030]

[0031] in, is the image gradient, and Δp is the offset vector, which is obtained by minimizing the brightness residual. This process ensures that the feature points are tracked continuously between different frames, which is beneficial for subsequent triangulation and reprojection optimization.

[0032] IMU pre-integration and error modeling: The IMU sensor may generate hundreds of sets of data between two frames of images. If the integration is repeated every time the optimization is performed, the computational complexity will increase significantly. Therefore, the IMU pre-integration method is used to calculate the error between the image interval [t k ,t k+1 ], the relative position, velocity and rotation within are:

[0033]

[0034] Where, Indicates the pre-integrated displacement (position change). Indicates the pre-integrated speed change. Represents the pre-integrated rotation change, which is the quaternion change obtained by integrating the angular velocity after removing the bias, and is used to update the posture. Indicates the conversion from the coordinate system of time t to time point b kThe rotation matrix of the (current keyframe) coordinate system. Ω represents the quaternion differentiation matrix. Represents the acceleration measured by the IMU (plus gravity and noise). Represents the angular velocity measured by the IMU (plus noise). Indicates the bias of the gyroscope. Indicates the bias of the accelerometer. Represents a rotation quaternion.

[0035] In addition, the Jacobian matrix and covariance are also calculated to facilitate the construction of IMU residual terms in subsequent nonlinear optimization.

[0036] Generate initial trajectory: Since the monocular system does not have an absolute scale, an initialization process is required to estimate the scale, IMU bias and velocity. The following steps are used:

[0037] The five-point method is used for feature point matching and triangulation, the camera pose between multiple frames is estimated, and the scale-free SFM (Structure from Motion, a computer vision technology that uses image sequences to estimate 3D scene structure and camera motion) trajectory is constructed. The formula is:

[0038] T i =[R i |t i ];

[0039] Where R i Represents the rotation matrix of the camera in the i-th frame. T i Represents the camera pose matrix of the i-th frame. i Represents the translation vector of the camera in frame i. This combination represents a rigid body transformation.

[0040] Construct initialization variables:

[0041]

[0042] Where: v i is the speed per frame; b a ,b g is the IMU acceleration and gyroscope bias; g is the gravity vector; s is the scale factor, is a set of parameters to be estimated during the initialization phase. The initial estimate is performed using linear regression and Gauss-Newton iteration by minimizing the difference between the IMU pre-integration and the visual SFM.

[0043] Sliding window nonlinear optimization: Optimize the state variables in the sliding window to minimize the residual of IMU and visual observation. The state variables in the window are the poses α of multiple key frames. k , speed v k , bias ba ,b g The objective function is:

[0044]

[0045] Among them, r imu is the IMU residual, which is the difference between the predicted position and the pre-integrated value; r proj is the image reprojection error, comparing the predicted position of the triangulated map point on the current image with the actual observation; r prior is the marginalized residual of the previous round. The optimization is solved using the Ceres solver and uses marginalization to preserve the sliding window size based on the Markov assumption.

[0046] Marginalization: In order to control computational complexity and maintain real-time performance, a sliding window optimization strategy is used to jointly optimize the most recent N keyframes. As time goes by, old keyframes must be "removed" from the window, but their observation information must still be retained to avoid loss of accuracy. For this purpose, a marginalization processing mechanism is introduced. The core idea of ​​marginalization is to integrate the variables to be removed (such as the old frame pose) into a "prior term" in the optimization problem, that is, to convert the influence of the old variables into a residual function r prior , and is retained as a constraint in the new optimization problem.

[0047] Consider that the variables in the original optimization problem can be split into the retained variables x r with the marginalized variable x m , its Hessian matrix and gradient are:

[0048]

[0049] Where b r 、b m Indicates that x r 、x m The associated gradient vector.

[0050] x m The variables are marginalized to obtain a new prior term, the formula is:

[0051]

[0052] The new (H', b') corresponds to a constrained error term r prior (x r ), which will be used in the next round of optimization.

[0053] Loop closure detection: The DBOW library is used to detect candidate loop keyframes based on bag-of-words vectors. To perform loop closure detection, 500 additional features are extracted based on the corner points of optical flow tracking and BRIEF descriptors are calculated. The image is then abstracted into bag-of-words vectors and saved. The similarity of the bag-of-words vectors for the new keyframes is calculated to determine the loop candidate frame. If a loop frame is detected, closed loop constraint edges are constructed:

[0054]

[0055] Where T i is the current frame pose, To match the relative transformation estimated in the historical frame, we introduce the residual edge and perform four-degree-of-freedom graph optimization, optimizing only the position and in-plane rotation:

[0056]

[0057] Where, Represents a set of pose variables; x, y, z are coordinates in three-dimensional space; θ represents the rotation angle in the plane.

[0058] This improves the global consistency of the trajectory and suppresses long-term drift.

[0059] The IMU integral trajectory display submodule displays the three-dimensional motion trajectory in real time.

[0060] The display and analysis module converts Xsens DOT trajectories (such as foot movements) and VINS trajectories into a unified coordinate system and aligns them with the time axis. It then overlays or independently displays the relevant trajectories in a 3D scene. It also compares the trajectory deviation and time step size between the Xsens DOT module and the VINS algorithm module, and analyzes the trajectory error, motion period, and offset rate based on the comparative data. The analysis results can be exported.

[0061] We also provide a mobile terminal trajectory visualization APP based on the fusion of IMU and VINS, which uses a mobile terminal trajectory visualization system based on the fusion of IMU and VINS. The APP is installed on Android devices. The usage process is as follows:

[0062] 1. Turn on Bluetooth and search for IMU devices near the wearable device;

[0063] 2. Connect to the searched IMU device;

[0064] 3. Compile the data collected by the IMU device. If the compilation is successful, it will be displayed in numerical form. If the compilation is not successful, recompile until it is successful.

[0065] 4. The VINS algorithm starts executing, generates a three-dimensional motion trajectory, and displays it;

[0066] 5. Save the relevant trajectory data after stopping the display.

[0067] Therefore, the present invention adopts the above-mentioned mobile terminal trajectory visualization system based on the fusion of IMU and VINS, which can present the motion trajectory output by VINS and the foot motion data collected by Xsens DOT in parallel, thereby improving the practicality and synergy of Xsens DOT and VINS systems in motion analysis.

[0068] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A mobile terminal trajectory visualization system based on the fusion of IMU and VINS, characterized by: It includes an Xsens DOT module, a VINS algorithm module and a display and analysis module. The Xsens DOT module and the VINS algorithm module are respectively connected to the display and analysis module. The Xsens DOT module includes a Bluetooth data communication module, a data display module and a data storage module. The Bluetooth data communication module is connected to the data display module, and the data display module is connected to the data storage module. The VINS algorithm module includes an IMU integral trajectory display submodule and an algorithm status control submodule. The algorithm status control submodule is connected to the IMU integral trajectory display submodule.

2. The mobile terminal trajectory visualization system based on the fusion of IMU and VINS according to claim 1 is characterized by: The Bluetooth data communication module uses the BLE protocol to establish a stable connection with the Xsens DOT device. The Xsens DOT device automatically scans and lists nearby sensor devices, and after connecting to the sensor device, collects the data measured by the sensor. The data is parsed in a fixed format, which includes a frame header, timestamp, and data body.

3. The mobile terminal trajectory visualization system based on the fusion of IMU and VINS according to claim 2 is characterized by: After the Bluetooth data communication module is connected to the sensor device, the data display module compiles the acquired sensor data. After the compilation is completed, the sensor data is displayed in a numerical form. At the same time, the XsensDOT trajectory is obtained based on the integration of the acquired sensor data.

4. The mobile terminal trajectory visualization system based on IMU and VINS fusion according to claim 3 is characterized by: The data saving module obtains the data collected by the Bluetooth data communication module and saves it in the CSV general format based on the timestamp.

5. The mobile terminal trajectory visualization system based on IMU and VINS fusion according to claim 1 is characterized by: The algorithm state control submodule is based on multi-sensor state estimation theory and combines IMU pre-integration, visual reprojection and sliding window optimization methods to estimate the spatial trajectory and generate a three-dimensional motion trajectory.

6. The mobile terminal trajectory visualization system based on IMU and VINS fusion according to claim 1 is characterized by: The IMU integral trajectory display submodule displays the three-dimensional motion trajectory in real time.

7. The mobile terminal trajectory visualization system based on IMU and VINS fusion according to claim 1 is characterized by: The display and analysis module converts the Xsens DOT and VINS trajectories into a unified coordinate system and aligns them with the time axis, overlaying or independently displaying the relevant trajectories in the 3D scene. At the same time, it compares the trajectory deviation and time step in the Xsens DOT module and the VINS algorithm module, and analyzes the trajectory error, motion period, and offset rate based on the comparative data.

Images