Imu-based tracking and positioning method and apparatus, head-mounted device, and medium

Abstract

The application discloses an IMU-based tracking positioning method and device, a head-mounted device and a medium, and relates to the technical field of instant positioning. The method obtains IMU data between a previous frame and a current frame, and determines a rotation pose according to the IMU data. A first quaternion corresponding to the current frame is determined according to a quaternion of the previous frame and the rotation pose. The head-mounted device is positioned and tracked based on the first quaternion, and a current pose of the head-mounted device is obtained. A second quaternion corresponding to the current frame is determined according to the current pose, and the second quaternion is set as the quaternion of the current frame. The technical scheme can improve the credibility of the quaternion of each frame, and ensure that the positioning information accuracy of the output head-mounted device is not affected by zero bias error in the case of long-term work of the IMU and the gyroscope.

Description

Technical Field

[0001] This application belongs to the field of real-time positioning technology, and particularly relates to an IMU-based tracking and positioning method, device, head-mounted device, and computer-readable storage medium. Background Technology

[0002] In the fields of VR (Virtual Reality) and AR (Augmented Reality), when using SLAM (Simultaneous Localization and Mapping) algorithms for tracking in indoor scenarios, keyframe matching is required. Common keyframe tracking schemes include tracking constant-velocity motion models, tracking reference keyframes, and relocalization. Alternatively, a method can be employed that uses IMU and gyroscope data to calculate the quaternion of the current frame and then matches it with keyframes in the map. This effectively reduces the computational load of localization tracking and improves the real-time performance of localization information output.

[0003] However, considering the inherent bias problem of gyroscopes, when positioning and tracking head-mounted devices based on gyroscope data over a long period of time, the bias error generated by the gyroscope becomes larger and larger. As a result, the reliability of the calculated current frame quaternion gradually decreases over time, which in turn affects the accuracy of the final output positioning information of the head-mounted device.

[0004] The information disclosed in this background section is only for understanding the background technology of the present invention, and therefore may include information that does not constitute prior art. Summary of the Invention

[0005] The main objective of this application is to provide an IMU-based tracking and positioning method, apparatus, head-mounted device, and computer-readable storage medium. The aim is to improve the reliability of the quaternions in each frame through this IMU-based tracking and positioning method, ensuring that the accuracy of the positioning information output by the head-mounted device is not affected by zero-bias error even under long-term operation of the IMU and gyroscope.

[0006] To achieve the above objectives, this application provides an IMU-based tracking and positioning method, which includes:

[0007] Acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data;

[0008] Based on the quaternion of the previous frame and the rotation pose, determine the first quaternion corresponding to the current frame;

[0009] Based on the first quaternion, the head-mounted device is located and tracked to obtain the current pose of the head-mounted device;

[0010] The second quaternion corresponding to the current frame is determined based on the current pose, and the second quaternion is set as the quaternion of the current frame.

[0011] Optionally, the IMU data includes gyroscope data;

[0012] The step of acquiring IMU data between the previous frame and the current frame, and determining the rotation pose based on the IMU data, includes:

[0013] The current frame is constructed based on the image data acquired by the head-mounted device;

[0014] Acquire gyroscope data from the previous frame to the current frame, pre-integrate the gyroscope data, and obtain the rotation pose.

[0015] Optionally, the step of determining the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose includes:

[0016] Determine if a previous frame exists;

[0017] If a previous frame exists, then obtain the quaternion corresponding to the previous frame;

[0018] If there is no previous frame, the preset initial quaternion is set to the quaternion of the previous frame.

[0019] The product of the quaternion of the previous frame and the quaternion of the rotation pose is calculated to obtain the first quaternion corresponding to the current frame.

[0020] Optionally, the step of determining the second quaternion corresponding to the current frame based on the current pose includes:

[0021] Based on the current pose, obtain the relative pose of the head-mounted device with respect to the coordinate system of the previously stored map;

[0022] The second quaternion is obtained by calculating the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose.

[0023] Optionally, the step of performing positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device includes:

[0024] Based on the first quaternion, the target keyframe is selected from multiple keyframes of the map where the head-mounted device is located.

[0025] Matching feature points are determined by comparing the first bag-of-words model of the current frame with the second bag-of-words model of the target keyframe;

[0026] Based on the matched feature points, the current pose of the head-mounted device is located.

[0027] Optionally, the step of filtering the target keyframe from multiple keyframes of the map where the head-mounted device is located based on the first quaternion includes:

[0028] Traverse each of the keyframes and calculate the rotation angle difference and vector difference between each keyframe and the current frame based on the first quaternion and the quaternion of each keyframe;

[0029] Keyframes whose rotation angle difference and vector difference are less than preset angle difference threshold and preset vector difference threshold, respectively, are designated as candidate keyframes;

[0030] The candidate keyframe with the smallest difference in rotation angle among all the candidate keyframes is set as the target keyframe.

[0031] Optionally, the IMU-based tracking and localization method further includes:

[0032] If the location tracking fails, the first quaternion is set to the quaternion of the current frame;

[0033] When performing positioning and tracking of the head-mounted device in the next frame, the quaternion corresponding to the next frame is calculated based on the first quaternion and the IMU data between the current frame and the next frame.

[0034] In addition, to achieve the above objectives, this application also provides an IMU-based tracking and positioning device, which is applied to a head-mounted device;

[0035] The IMU-based tracking and positioning device includes:

[0036] The rotation pose module is used to acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data.

[0037] The quaternion calculation module is used to determine the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose.

[0038] The positioning and tracking module is used to perform positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device;

[0039] The quaternion update module is used to determine the second quaternion corresponding to the current frame based on the current pose, and set the second quaternion as the quaternion of the current frame.

[0040] In addition, to achieve the above objectives, this application also provides a head-mounted device, the head-mounted device comprising: a memory, a processor, and an IMU-based tracking and positioning program stored in the memory and executable on the processor, wherein the IMU-based tracking and positioning program, when executed by the processor, implements the steps of the IMU-based tracking and positioning method as described above.

[0041] In addition, to achieve the above objectives, this application also provides a computer-readable storage medium storing an IMU-based tracking and positioning program, which, when executed by a processor, implements the steps of the IMU-based tracking and positioning method as described above.

[0042] This application proposes an IMU-based tracking and positioning method, apparatus, head-mounted device, and computer-readable storage medium. First, it acquires IMU data between the previous frame and the current frame, and determines a rotational pose based on the IMU data. Then, based on the quaternion of the previous frame and the rotational pose, it determines a first quaternion corresponding to the current frame. Next, it performs positioning and tracking on the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device. Finally, it determines a second quaternion corresponding to the current frame based on the current pose and sets the second quaternion as the quaternion of the current frame.

[0043] Thus, in the technical solution of this application embodiment, when performing positioning and tracking of the head-mounted device, the quaternion of the current frame is mainly calculated by using the rotation pose determined by IMU data and the quaternion of the previous frame. This avoids the impact of zero bias caused by the long-term operation of the gyroscope on the accuracy of the quaternion. Furthermore, after obtaining the current pose, a more accurate quaternion is extracted based on the current pose to update the quaternion of the current frame. This makes the quaternion of the current frame called when calculating the quaternion of the next frame more accurate and unaffected by the zero bias error accumulated by the gyroscope in the early stage. The technical solution of this application improves the reliability of the quaternion corresponding to each frame through the above-mentioned quaternion update method, ensuring that the accuracy of the positioning information of the head-mounted device output is not affected under the long-term operation of the IMU and gyroscope. Attached Figure Description

[0044] Figure 1 This is a schematic diagram of the hardware operating environment of the head-mounted device involved in the embodiments of this application;

[0045] Figure 2 This is a flowchart illustrating the steps of the first embodiment of the IMU-based tracking and positioning method of this application;

[0046] Figure 3 This is a schematic diagram illustrating the overall inventive concept of the IMU-based tracking and positioning method of this application;

[0047] Figure 4 This is a flowchart illustrating steps S21 to S24 in the IMU-based tracking and positioning method of this application.

[0048] Figure 5 This is a schematic diagram of the functional modules of an embodiment of the IMU-based tracking and positioning device of this application.

[0049] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0050] It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit this application.

[0051] Reference Figure 1 , Figure 1 This is a schematic diagram of the hardware operating environment of the head-mounted device involved in the embodiments of this application.

[0052] It should be noted that the head-mounted device in this application embodiment can be a camera device, display device, and positioning device that executes the keyframe tracking method based on IMU (Inertial Measurement Unit) in this application. In addition, the terminal device in this application embodiment can also be a computing unit such as a PC, smartphone, or tablet connected to the above-mentioned terminal devices.

[0053] It should be understood that, based on different design needs of practical applications, the terminal device in the embodiments of this application may of course also be other devices or device modules. For example, the terminal device may specifically be a data storage control terminal or a portable computer, or other mobile or non-mobile terminal devices.

[0054] like Figure 1 As shown, the head-mounted device may include: a processor 1001, such as a CPU; a communication bus 1002; a user interface 1003; a network interface 1004; and a memory 1005. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface). The memory 1005 may be high-speed RAM or non-volatile memory, such as a disk drive. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0055] Those skilled in the art will understand that Figure 1 The head-mounted device structure shown does not constitute a limitation on the head-mounted device and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0056] like Figure 1 As shown, the memory 1005, which serves as a computer storage medium, may include an operating system, a network communication module, a user interface module, and an IMU-based tracking and positioning program.

[0057] exist Figure 1 In the terminal shown, network interface 1004 is mainly used to connect to the backend server and communicate data with it; user interface 1003 is mainly used to connect to the client and communicate data with it; and processor 1001 can be used to call the IMU-based tracking and positioning program stored in memory 1005 and perform the following operations:

[0058] Acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data;

[0059] Based on the quaternion of the previous frame and the rotation pose, determine the first quaternion corresponding to the current frame;

[0060] Based on the first quaternion, the head-mounted device is located and tracked to obtain the current pose of the head-mounted device;

[0061] The second quaternion corresponding to the current frame is determined based on the current pose, and the second quaternion is set as the quaternion of the current frame.

[0062] Optionally, the IMU data includes gyroscope data, and the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0063] The current frame is constructed based on the image data acquired by the head-mounted device;

[0064] Acquire gyroscope data from the previous frame to the current frame, pre-integrate the gyroscope data, and obtain the rotation pose.

[0065] Optionally, the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0066] Determine if a previous frame exists;

[0067] If a previous frame exists, then obtain the quaternion corresponding to the previous frame;

[0068] If there is no previous frame, the preset initial quaternion is set to the quaternion of the previous frame.

[0069] The product of the quaternion of the previous frame and the quaternion of the rotation pose is calculated to obtain the first quaternion corresponding to the current frame.

[0070] Optionally, the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0071] Based on the current pose, obtain the relative pose of the head-mounted device with respect to the coordinate system of the previously stored map;

[0072] The second quaternion is obtained by calculating the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose.

[0073] Optionally, the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0074] Based on the first quaternion, the target keyframe is selected from multiple keyframes of the map where the head-mounted device is located.

[0075] Matching feature points are determined by comparing the first bag-of-words model of the current frame with the second bag-of-words model of the target keyframe;

[0076] Based on the matched feature points, the current pose of the head-mounted device is located.

[0077] Optionally, the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0078] Traverse each of the keyframes and calculate the rotation angle difference and vector difference between each keyframe and the current frame based on the first quaternion and the quaternion of each keyframe;

[0079] Keyframes whose rotation angle difference and vector difference are less than preset angle difference threshold and preset vector difference threshold, respectively, are designated as candidate keyframes;

[0080] The candidate keyframe with the smallest difference in rotation angle among all the candidate keyframes is set as the target keyframe.

[0081] Optionally, the processor 1001 can also be used to call the IMU-based tracking and positioning program stored in the memory 1005 and perform the following operations:

[0082] If the location tracking fails, the first quaternion is set to the quaternion of the current frame;

[0083] When performing positioning and tracking of the head-mounted device in the next frame, the quaternion corresponding to the next frame is calculated based on the first quaternion and the IMU data between the current frame and the next frame.

[0084] Please refer to Figure 2 , Figure 2 This is a flowchart illustrating the first embodiment of the IMU-based tracking and positioning method of this application. In the first embodiment of the IMU-based tracking and positioning method of this application, the method includes:

[0085] Step S10: Obtain IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data;

[0086] In this embodiment of the application, it should be noted that the IMU-based tracking and positioning method is applied to head-mounted devices, such as VR devices or AR devices. The head-mounted device is equipped with a camera, which can construct each frame (previous frame, current frame, and next frame, etc.) by collecting image data. Specifically, between different frames, the pose of the head-mounted device itself is changing in the map of the scene, including changes in parameters such as rotation angle and coordinates. This change in pose can be collected by the IMU set on the head-mounted device. The IMU data includes at least gyroscope data. The gyroscope data can be used to represent the angular velocity of rotation around the x, y, z axes in the IMU coordinate system, and the rotation pose and the corresponding quaternion can also be obtained by predicting the rotation pose and the quaternion of the rotation pose through the gyroscope data.

[0087] Step S20: Determine the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose;

[0088] In this embodiment of the application, when the rotation pose has been determined, it is necessary to obtain the quaternion of the previous frame and calculate the first quaternion corresponding to the current frame by combining the quaternion of the previous frame and the quaternion corresponding to the rotation pose. The quaternion of the previous frame is a relatively accurate quaternion extracted based on the pose after the pose of the head-mounted device is obtained during the tracking and positioning process in the previous frame. It is not affected by the zero bias error generated by the long-term operation of the gyroscope, so the first quaternion obtained is also relatively accurate.

[0089] For example, the step of calculating the first quaternion of the current frame based on the quaternion of the previous frame and the rotation pose between the previous frame and the current frame specifically includes calculating the product of the quaternion of the previous frame and the quaternion of the rotation pose.

[0090] Step S30: Based on the first quaternion, perform positioning and tracking on the head-mounted device to obtain the current pose of the head-mounted device;

[0091] This application also provides a method for positioning and tracking a head-mounted device based on the quaternion of the current frame. First, a target key frame is selected from multiple key frames of the map where the head-mounted device is located based on the first quaternion. Then, matching feature points are determined by the first bag-of-words model of the current frame and the second bag-of-words model of the target key frame. Finally, the current pose of the head-mounted device is located based on the matching feature points.

[0092] Thus, in the technical solution of this application embodiment, when performing positioning and tracking on a head-mounted device, the key frame at that angle is quickly filtered out by the quaternion of the current frame. Compared with the traditional key frame matching scheme, the number of key frames that need to be calculated is effectively reduced, thereby reducing the computational complexity of the SLAM algorithm, reducing the amount of matching calculation, and enabling the key frame to be determined faster and more timely. This reduces the positioning time of the head-mounted device and improves the real-time performance of pose output while ensuring positioning accuracy.

[0093] Step S40: Determine the second quaternion corresponding to the current frame based on the current pose, and set the second quaternion as the quaternion of the current frame.

[0094] In this embodiment, after successfully tracking and positioning the head-mounted device and obtaining the current pose corresponding to the current frame, the concept of visual inertial odometry is used to correct the zero-bias error generated by the gyroscope, thereby obtaining a more accurate quaternion for the current frame. Specifically, the step of using visual inertial odometry to correct the zero-bias error generated by the gyroscope includes extracting the corresponding second quaternion using the current pose. The second quaternion is calculated using the current pose, so it is more accurate than the first quaternion calculated using the quaternion of the previous frame and the rotation pose. Therefore, after determining the current true pose of the current frame, the corresponding second quaternion can be calculated based on the current pose to replace the original first quaternion, which can be used when calculating the quaternion in the next frame, making the first quaternion in the next frame more accurate. This process is repeated cyclically, so that the quaternion of each frame is obtained by calculating the quaternion of the pose of the previous frame and the rotation pose between the previous frame and the current frame, without being affected by the zero bias error accumulated by the gyroscope, thus improving the accuracy of the quaternion and the positioning information (current pose) of the output head-mounted device.

[0095] In the embodiments of this application, it should be noted that the embodiments of this application adopt a separate SLAM tracking method, that is, the map set of the current scene is preloaded and the tracking and positioning are performed based on the loaded map set. There is no need to perform loop closure detection and updating of the map set in real time, thereby saving the computing power of the built-in CPU (Central Processing Unit) of the head-mounted device, saving computing time, and improving the real-time output of positioning information.

[0096] Reference Figure 3 The inventive concept of this application involves the following steps: first, constructing the current frame based on the acquired image data; then, pre-integrating the gyroscope data in the IMU data between the previous frame and the current frame to obtain the quaternion q. d (i.e., rotation pose); then obtain the quaternion q from the previous frame. l Calculate the quaternion q of the current frame. m =q d q l Assign the matching quaternion q to the current frame. m (i.e., the first quaternion); based on the first quaternion, perform a split SLAM tracking process to calculate the current frame pose (i.e., the current position); extract the current frame quaternion q from the current pose. c (i.e., the second quaternion); based on the second quaternion, proceed to the tracking and positioning process of the next frame.

[0097] The technical solution of this application combines the pose of the visual inertial odometry and the gyroscope data in the IMU data to pre-integrate and obtain the quaternion corresponding to the rotation pose, thereby solving the zero bias problem of the IMU tracking algorithm in the separate SLAM deployment of head-mounted devices, and ensuring that the accuracy of matching key frames and outputting positioning information will not be affected when the separate SLAM scheme is executed for a long time.

[0098] Further, in step S10, the IMU data includes gyroscope data. The step of acquiring IMU data from the previous frame to the current frame and determining the rotation pose based on the IMU data may include:

[0099] Step S11: Construct the current frame based on the image data acquired by the head-mounted device;

[0100] Step S12: Obtain gyroscope data from the previous frame to the current frame, pre-integrate the gyroscope data, and obtain the rotation pose.

[0101] In this embodiment, the current frame is first constructed using the camera on the head-mounted device based on a preset positioning frequency and the currently acquired image data. Then, the quaternion of the rotation pose is calculated using gyroscope data from the IMU (Integrated Device Unit) on the head-mounted device. Here, a quaternion is a hypercomplex number; it can be understood that a quaternion consists of a real number a plus three imaginary units i, j, and k, and they have the following relationship: i 2 =j 2 =k 2 =-1, i°=j°=k°=1, each quaternion is a linear combination of 1, i, j and k, that is, a quaternion can generally be represented as a+bi+cj+dk, where a, b, c and d are real numbers.

[0102] In this embodiment, the geometric meaning of i, j, and k can be understood as a rotation, where i rotation represents a rotation from the positive Z-axis to the positive Y-axis in the plane where the Z-axis intersects the Y-axis, j rotation represents a rotation from the positive X-axis to the positive Z-axis in the plane where the X-axis intersects the Z-axis, and k rotation represents a rotation from the positive Y-axis to the positive X-axis in the plane where the Y-axis intersects the X-axis. -i, -j, and -k represent the opposite rotations of i, j, and k, respectively. Therefore, the first quaternion of the current frame can be used to characterize the current pose of the head-mounted device. Specifically, the IMU data consists of two parts: an accelerometer and a gyroscope. The gyroscope data is used to represent the angular velocity of rotation around the x, y, and z axes of the IMU coordinate system. In this embodiment, the gyroscope data can be integrated to obtain the first quaternion. The expression is as follows:

[0103]

[0104] Where, q d The quaternion corresponding to the rotation pose. For real numbers in quaternions, and It is an imaginary number in quaternions.

[0105] Furthermore, referring to Figure 4 In step S20, the step of determining the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose may include:

[0106] Step S21: Determine if a previous frame exists;

[0107] Step S22: If a previous frame exists, obtain the quaternion corresponding to the previous frame;

[0108] Step S23: If there is no previous frame, then set the preset initial quaternion to the quaternion of the previous frame.

[0109] Step S24: Calculate the product between the quaternion of the previous frame and the quaternion of the rotation pose to obtain the first quaternion corresponding to the current frame.

[0110] In this embodiment of the application, it should be noted that the existence of a previous frame means that the current frame is not the first frame for tracking and positioning the head-mounted device in this operation. If there is a previous frame before the current frame, that is, the positioning information (pose) of the head-mounted device was successfully output in the previous frame, then after SLAM constructs the current frame, the IMU data from the timestamp of the previous frame to the current frame can be obtained, and the rotation pose q can be obtained by pre-integrating the gyroscope data in the IMU. d Then assign the quaternion from the previous frame to q. l Then, the rotation pose increment of the previous frame value in the current frame is added to q. l Above, obtain the first quaternion q of the current frame. m If there is no previous frame, including cases where initialization or relocation has just been completed, then the preset initial quaternion q can be used. l The value (1,0,0,0) is assigned to the first quaternion to calculate the first quaternion q corresponding to the current frame. m For example, the calculation expression for the first quaternion is: q m =q d q l .

[0111] In this embodiment, the quaternion of the current frame is calculated both with and without a previous frame, for use in subsequent positioning of the head-mounted device. The current frame quaternion in this technical solution is calculated from the quaternion of the previous frame and the rotation pose from the previous frame to the current frame, and is independent of the quaternion at the initialization frame. As long as the quaternion corresponding to the previous frame is accurate, the accuracy of the current frame quaternion is also high. This is equivalent to providing a more accurate method for obtaining quaternions, which can effectively overcome the influence of the zero bias error in the gyroscope on the accuracy of quaternions, thereby improving the output accuracy of the positioning information of the head-mounted device.

[0112] Additionally, in this embodiment, step S40, the step of determining the second quaternion corresponding to the current frame based on the current pose, may include:

[0113] Step S41: Based on the current pose, obtain the relative pose of the head-mounted device with respect to the coordinate system of the previously stored map;

[0114] Step S42: Calculate the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose to obtain the second quaternion.

[0115] It should be noted that during the execution of step S30, two results may be obtained: one is that the head-mounted device is successfully tracked and positioned, in which case the current pose of the head-mounted device can be output smoothly; the other is that the tracking and positioning fails, in which case the current pose of the head-mounted device cannot be output successfully, and it is necessary to combine other keyframe matching methods or tracking and positioning methods for positioning and tracking. Step S40 is the step to be executed after the tracking and positioning is successful. In this case, visual inertial odometry can be used to correct the zero bias error in the gyroscope, thereby obtaining a more accurate second quaternion. In this embodiment, the second quaternion is the quaternion corresponding to the pose determined by visual inertial odometry. Compared with the first quaternion determined by the quaternion of the previous frame and the rotation pose, the second quaternion has higher accuracy. Therefore, in this embodiment, the second quaternion is used to replace the first quaternion, which improves the accuracy of the quaternion corresponding to the current frame, eliminates the influence of the gyroscope zero bias error, and provides more accurate parameters for the quaternion calculation of the next frame.

[0116] For example, after successful tracking, the coordinates corresponding to the current pose of the head-mounted device are projected back to the coordinate system where the IMU data is located to obtain the relative pose q of the head-mounted device relative to the coordinate system of the previously stored map. p Then calculate the quaternion q of the rotation pose. d The previous frame's quaternion q l And the product between the relative poses, to obtain the second quaternion q. n , where the second quaternion q n The calculation expression is: q n =q d q l q p Finally, for the second quaternion q n The solution is optimized to update the second quaternion, ultimately obtaining the second quaternion q corresponding to the current frame. c .

[0117] In one feasible embodiment, step S30, which involves performing positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device, includes:

[0118] Step S31: Based on the first quaternion, select the target keyframe from multiple keyframes of the map where the head-mounted device is located.

[0119] It should be noted that the storage unit of the head-mounted device in this embodiment has pre-stored a map atlas library consisting of multiple scene map atlases. When the user inputs SLAM commands, the image data collected by the head-mounted device is first matched with the map atlases in the map atlas library to obtain the map atlas that best matches the image data. This map atlas can be used in the SLAM algorithm for tracking and locating the head-mounted device. This map atlas includes keyframes corresponding to multiple viewpoints. The technical solution in this embodiment requires selecting the target keyframe that is closest to the current frame from each of the keyframes for locating the head-mounted device.

[0120] For example, in the technical solution of this embodiment, the rotation angle difference and vector difference between the first quaternion and the quaternion of each key frame are calculated, thereby selecting the key frame with the smallest difference as the target key frame.

[0121] Step S32: Determine matching feature points by using the first bag-of-words model of the current frame and the second bag-of-words model of the target keyframe;

[0122] In this embodiment, the first step is to determine the corresponding bag-of-words model for feature points based on the image data of the current frame. The bag-of-words model is used to vectorize the current frame; that is, it includes multiple feature point vectors corresponding to the current frame. Similarly, the target keyframe also has multiple corresponding feature point vectors. During the matching process, the feature point vectors in the two bag-of-words models are matched sequentially to obtain successfully matched feature point vectors, thereby determining the successfully matched feature points.

[0123] Step S33: Based on the matching feature points, locate the current pose of the head-mounted device.

[0124] In this embodiment of the application, it should be noted that after the matching feature points are determined, the correspondence between 3D and 2D points can be solved by the preset PNP (Perspective-n-Point) algorithm, and the current pose of the head-mounted device can be obtained by optimizing the 3D-2D reprojection error.

[0125] Optionally, during the pose localization process described above, each matching feature point needs to be screened and deleted, categorized into outer points and inner points. Inner points are the feature points used for pose localization, while outer points are those that need to be discarded or deleted, thus obtaining the number of deleted outer points. After obtaining the number of outer points, the success of keyframe matching can be determined based on this number. In this embodiment, the success of keyframe tracking is primarily determined by the number of deleted outer points. A larger number of deleted outer points during pose localization via matching feature points indicates lower accuracy of the selected matching feature points, meaning the strategy for feature point matching using the target keyframe is ill-suited to the current tracking and localization process. The conclusion is that the target keyframe selection has failed. In this case, the target keyframe matching should be performed again.

[0126] On the other hand, the smaller the number of deleted outliers and the less than the preset number, the higher the accuracy of the matched feature points, which meets the tracking and positioning requirements, and the target keyframe is successfully tracked. The preset number can be set according to the positioning accuracy requirements in actual applications; a higher preset number results in lower positioning accuracy, and a lower preset number results in higher positioning accuracy.

[0127] In another feasible embodiment, the success of keyframe tracking can be measured by the number of inliers determined during pose localization. Specifically, an inlier number threshold (e.g., 10) can be set. If the number of inliers in the matched feature points is greater than or equal to 10, the keyframe tracking is considered successful; if the number of inliers is less than 10, the keyframe tracking is considered unsuccessful.

[0128] It should be noted that after keyframe tracking fails, other preset traditional keyframe tracking methods can be used to perform keyframe tracking, thereby ensuring that the target keyframe with the required accuracy can be tracked when keyframe tracking through IMU data fails. Therefore, the technical solution of this application embodiment can combine the IMU-based keyframe tracking method proposed in this application with other traditional keyframe tracking methods to improve the real-time performance of the output positioning information while ensuring the accuracy of the output positioning information.

[0129] Further, in step S31, the step of filtering out the target keyframe from multiple keyframes of the map where the head-mounted device is located based on the first quaternion may include:

[0130] Step S311: Traverse each of the keyframes and calculate the rotation angle difference and vector difference between each keyframe and the current frame based on the first quaternion and the quaternion of each keyframe.

[0131] Step S312: Set keyframes whose rotation angle difference and vector difference are less than preset angle difference threshold and preset vector difference threshold, respectively, as candidate keyframes;

[0132] Step S313: Set the candidate keyframe with the smallest rotation angle difference among all the candidate keyframes as the target keyframe.

[0133] This application provides a method for filtering target keyframes based on the first quaternion of the current frame and the quaternions of each keyframe. Specifically, the quaternion can generally be represented as a+bi+cj+dk, where a, b, c, and d are real numbers.

[0134] In this embodiment, the geometric meaning of i, j, and k can be understood as a rotation, where i rotation represents a rotation from the positive Z-axis to the positive Y-axis in the plane where the Z-axis intersects the Y-axis, j rotation represents a rotation from the positive X-axis to the positive Z-axis in the plane where the X-axis intersects the Z-axis, and k rotation represents a rotation from the positive Y-axis to the positive X-axis in the plane where the Y-axis intersects the X-axis. -i, -j, and -k represent the opposite rotations of i, j, and k, respectively. Therefore, the quaternion of the current frame can be used to characterize the current pose of the head-mounted device. Specifically, the IMU data consists of two parts: an accelerometer and a gyroscope. The gyroscope data is used to represent the angular velocity of rotation around the x, y, and z axes of the IMU coordinate system. In this embodiment, the gyroscope data can be integrated to obtain the first quaternion. The expression for the first quaternion is as follows:

[0135]

[0136] Where, q 当前帧 The first quaternion corresponding to the current frame. It is a real number in the first quaternion. and It is an imaginary number in the first quaternion.

[0137] For example, the quaternion expression of the keyframe is as follows:

[0138]

[0139] Where q 关键帧 The quaternion of the keyframe. Let be a real number in the quaternion of the keyframe. as well as It is an imaginary number in the quaternion of the keyframe.

[0140] Furthermore, in this embodiment of the application, under the same coordinate system, the angle difference can be calculated using the real number in the first quaternion and the quaternion of the key frame, and the vector difference can be calculated using the imaginary number in the first quaternion and the quaternion of the key frame.

[0141] For example, the expression for calculating the angle difference is:

[0142] The expression for calculating the vector difference is:

[0143] After calculating the rotation angle difference and vector difference between each keyframe and the current frame, keyframes whose rotation angle difference and vector difference are both less than the preset angle difference threshold and preset vector difference can be selected to obtain candidate keyframes. Finally, based on the rotation angle difference, the target keyframe with the highest quaternion similarity to the current frame among the candidate keyframes is selected, achieving rapid selection of target keyframes. The technical solution of this application embodiment does not require pose calculation for each keyframe separately, nor does it require a large number of tedious and complex calculations, effectively reducing the computational resource consumption and computation time of the positioning and tracking process, and improving the real-time performance of the positioning information output by the head-mounted device.

[0144] Furthermore, after step S30, if the positioning and tracking of the head-mounted device fails and the current pose of the head-mounted device in the current frame is not obtained, the IMU-based tracking and positioning method further includes:

[0145] Step S50: If the positioning and tracking fails, the first quaternion is set to the quaternion of the current frame;

[0146] Step S60: When performing positioning and tracking of the head-mounted device in the next frame, calculate the quaternion corresponding to the next frame based on the first quaternion and the IMU data between the current frame and the next frame.

[0147] In the technical solution of this application embodiment, if tracking of the head-mounted device fails, the first quaternion q corresponding to the current frame will not be... m Modify or update the quaternion q from the previous frame after the next frame enters the SLAM algorithm execution step. m Based on the IMU data between the current frame and the next frame, the corresponding quaternion for the next frame is calculated, so that the next frame can use the quaternion for keyframe matching and tracking and positioning.

[0148] Optionally, if the tracking and positioning method of the head-mounted device fails to track the head-mounted device, the constant velocity motion model algorithm, tracking keyframe algorithm or relocation method in the traditional tracking and positioning method can be used to perform the tracking and positioning steps of the head-mounted device, thereby outputting the current pose of the head-mounted device.

[0149] The technical solution of this application, when performing positioning and tracking on a head-mounted device, mainly calculates the quaternion of the current frame using the rotational pose determined by IMU data and the quaternion of the previous frame. This avoids the impact of zero bias caused by the long-term operation of the gyroscope on the accuracy of the quaternion. Furthermore, after obtaining the current pose, a more accurate quaternion is extracted based on the current pose to update the quaternion of the current frame. This makes the quaternion of the current frame used when calculating the quaternion of the next frame more accurate and unaffected by the zero bias error accumulated by the gyroscope in the early stage. The technical solution of this application improves the reliability of the quaternion corresponding to each frame through the above-mentioned quaternion update method, ensuring that the accuracy of the positioning information of the head-mounted device output is not affected under the long-term operation of the IMU and gyroscope.

[0150] In addition, this application also provides an IMU-based tracking and positioning device, please refer to... Figure 5 , Figure 5 This is a functional module diagram of an embodiment of the IMU-based tracking and positioning device of this application, as shown below. Figure 5 As shown, the IMU-based tracking and positioning device of this application includes:

[0151] The rotation pose module 10 is used to acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data.

[0152] Quaternion calculation module 20 is used to determine the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose;

[0153] The positioning and tracking module 30 is used to perform positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device;

[0154] Quaternion update module 40 is used to determine the second quaternion corresponding to the current frame based on the current pose, and set the second quaternion as the quaternion of the current frame.

[0155] Optionally, the rotation pose module 10 is further used for:

[0156] The current frame is constructed based on the image data acquired by the head-mounted device;

[0157] Acquire gyroscope data from the previous frame to the current frame, pre-integrate the gyroscope data, and obtain the rotation pose.

[0158] Optionally, the quaternion calculation module 20 is further configured to:

[0159] Determine if a previous frame exists;

[0160] If a previous frame exists, then obtain the quaternion corresponding to the previous frame;

[0161] If there is no previous frame, the preset initial quaternion is set to the quaternion of the previous frame.

[0162] The product of the quaternion of the previous frame and the quaternion of the rotation pose is calculated to obtain the first quaternion corresponding to the current frame.

[0163] Optionally, the quaternion update module 40 is further configured to:

[0164] Based on the current pose, obtain the relative pose of the head-mounted device with respect to the coordinate system of the previously stored map;

[0165] The second quaternion is obtained by calculating the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose.

[0166] Optionally, the tracking and positioning module 30 is further configured to:

[0167] Based on the first quaternion, the target keyframe is selected from multiple keyframes of the map where the head-mounted device is located.

[0168] Matching feature points are determined by comparing the first bag-of-words model of the current frame with the second bag-of-words model of the target keyframe;

[0169] Based on the matched feature points, the current pose of the head-mounted device is located.

[0170] Optionally, the tracking and positioning module 30 is further configured to:

[0171] Traverse each of the keyframes and calculate the rotation angle difference and vector difference between each keyframe and the current frame based on the first quaternion and the quaternion of each keyframe;

[0172] Keyframes whose rotation angle difference and vector difference are less than preset angle difference threshold and preset vector difference threshold, respectively, are designated as candidate keyframes;

[0173] The candidate keyframe with the smallest difference in rotation angle among all the candidate keyframes is set as the target keyframe.

[0174] Optionally, the quaternion update module 40 is further configured to:

[0175] If the location tracking fails, the first quaternion is set to the quaternion of the current frame;

[0176] When performing positioning and tracking of the head-mounted device in the next frame, the quaternion corresponding to the next frame is calculated based on the first quaternion and the IMU data between the current frame and the next frame.

[0177] The specific implementations of the various functional modules of the IMU-based tracking and positioning device in this application are basically the same as the implementations of the IMU-based tracking and positioning method described above, and will not be repeated here.

[0178] This application also provides a computer storage medium storing an IMU-based tracking and positioning program, which, when executed by a processor, implements the steps of the IMU-based tracking and positioning program method as described in any of the above embodiments.

[0179] The specific embodiments of the computer storage medium in this application are basically the same as the embodiments of the IMU-based tracking and positioning program method described above, and will not be repeated here.

[0180] This application also provides a computer program product, which includes a computer program. When the computer program is executed by a processor, it implements the steps of the IMU-based tracking and positioning method of this application as described in any of the above embodiments, which will not be repeated here.

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

[0182] The sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0183] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a head-mounted device (such as TWS earphones, etc.) to execute the methods described in the various embodiments of this application.

[0184] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A tracking and positioning method based on IMU, characterized in that, The IMU-based tracking and localization method includes: Acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data; Based on the quaternion of the previous frame and the rotation pose, determine the first quaternion corresponding to the current frame; Based on the first quaternion, the head-mounted device is located and tracked to obtain the current pose of the head-mounted device; The second quaternion corresponding to the current frame is determined based on the current pose, and the second quaternion is set as the quaternion of the current frame; The step of determining the second quaternion corresponding to the current frame based on the current pose includes: Based on the current pose, obtain the relative pose of the head-mounted device with respect to the coordinate system of the previously stored map; The second quaternion is obtained by calculating the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose.

2. The IMU-based tracking and positioning method as described in claim 1, characterized in that, The IMU data includes gyroscope data; The step of acquiring IMU data between the previous frame and the current frame, and determining the rotation pose based on the IMU data, includes: The current frame is constructed based on the image data acquired by the head-mounted device; Acquire gyroscope data from the previous frame to the current frame, pre-integrate the gyroscope data, and obtain the rotation pose.

3. The IMU-based tracking and positioning method as described in claim 1, characterized in that, The step of determining the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose includes: Determine if a previous frame exists; If a previous frame exists, then obtain the quaternion corresponding to the previous frame; If there is no previous frame, the preset initial quaternion is set to the quaternion of the previous frame. The product of the quaternion of the previous frame and the quaternion of the rotation pose is calculated to obtain the first quaternion corresponding to the current frame.

4. The IMU-based tracking and positioning method as described in claim 1, characterized in that, The step of performing positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device includes: Based on the first quaternion, the target keyframe is selected from multiple keyframes of the map where the head-mounted device is located. Matching feature points are determined by comparing the first bag-of-words model of the current frame with the second bag-of-words model of the target keyframe; Based on the matched feature points, the current pose of the head-mounted device is located.

5. The IMU-based tracking and positioning method as described in claim 4, characterized in that, The step of filtering out target keyframes from multiple keyframes of the map where the head-mounted device is located based on the first quaternion includes: Traverse each of the keyframes and calculate the rotation angle difference and vector difference between each keyframe and the current frame based on the first quaternion and the quaternion of each keyframe; Keyframes whose rotation angle difference and vector difference are less than preset angle difference threshold and preset vector difference threshold, respectively, are designated as candidate keyframes; The candidate keyframe with the smallest difference in rotation angle among all the candidate keyframes is set as the target keyframe.

6. The IMU-based tracking and positioning method as described in any one of claims 1-5, characterized in that, The IMU-based tracking and localization method also includes: If the location tracking fails, the first quaternion is set to the quaternion of the current frame; When performing positioning and tracking of the head-mounted device in the next frame, the quaternion corresponding to the next frame is calculated based on the first quaternion and the IMU data between the current frame and the next frame.

7. A tracking and positioning device based on an IMU, characterized in that, The IMU-based tracking and positioning device includes: The rotation pose module is used to acquire IMU data between the previous frame and the current frame, and determine the rotation pose based on the IMU data. The quaternion calculation module is used to determine the first quaternion corresponding to the current frame based on the quaternion of the previous frame and the rotation pose; The positioning and tracking module is used to perform positioning and tracking of the head-mounted device based on the first quaternion to obtain the current pose of the head-mounted device; The quaternion update module is used to determine the second quaternion corresponding to the current frame based on the current pose, and set the second quaternion as the quaternion of the current frame; The quaternion update module is further configured to: obtain the relative pose of the head-mounted device relative to the coordinate system of the previously stored map based on the current pose; calculate the product of the quaternion of the rotation pose, the quaternion corresponding to the previous frame, and the quaternion corresponding to the relative pose to obtain a second quaternion.

8. A head-mounted device, characterized in that, The head-mounted device includes: a memory, a processor, and an IMU-based tracking and positioning program stored in the memory and executable on the processor, wherein the IMU-based tracking and positioning program, when executed by the processor, implements the steps of the IMU-based tracking and positioning method as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores an IMU-based tracking and positioning program, which, when executed by a processor, implements the steps of the IMU-based tracking and positioning method as described in any one of claims 1 to 6.

Images