Visual inertial positioning method, system and device based on ArUco label and storage medium

By using ArUco tags to optimize the camera position and establishing adaptive global optimization functions in the VIO system, the problem of large cumulative errors in the long run of the VIO system is solved, and the positioning accuracy and system robustness are improved.

CN118913258APending Publication Date: 2024-11-08CHANGAN UNIV
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202410960813.9
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-17
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

The existing visual inertial odometer (VIO) system will produce large cumulative errors during long runs, especially in the absence of a loopback detection module, resulting in reduced positioning accuracy.

Method used

Using the visual inertial positioning method based on the ArUco tag, by arranging multiple ID-unique ArUco tags in the usage environment, obtaining the relative pose relationship of adjacent tags, establishing a VIO system, detecting the initial ArUco tag and defining it as an ArUco coordinate system. Optimize the camera's pose in the VIO coordinate system until the ArUco tag is detected, and an adaptive global optimization function is established to eliminate cumulative errors.

Benefits of technology

It effectively solves the problem of large cumulative errors in the VIO system without loopback detection module, improves positioning accuracy, and ensures the robustness and reliability of the system during long-term operation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913258A_ABST
    Figure CN118913258A_ABST
Patent Text Reader

Abstract

The invention relates to the field of computer positioning, in particular to a visual inertial positioning method, system and device based on an ArUco label and a storage medium. A visual inertial positioning method based on ArUco labels is characterized by comprising the steps that S1, a plurality of ArUco labels with unique IDs are arranged in a use environment, and the relative pose relation of the adjacent ArUco labels is obtained; s2, establishing a VIO system, obtaining an image and IMU data of a visual inertial odometer, and obtaining a VIO coordinate system according to the image and the IMU data; according to the method, the ArUco label is arranged, so that the problem of overlarge accumulative error in the system positioning process under the condition that the VIO system does not have a loopback detection module is solved according to the label.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of computer positioning, and in particular to a visual inertial positioning method, system, device and storage medium based on ArUco tags. Background Art

[0002] Visual Inertial Odometry (VIO) is a key technology that integrates information from visual sensors and inertial measurement units (IMUs) to achieve precise positioning and navigation functions. This technology is widely used in many fields such as robots, autonomous vehicles, and drones. Even in environments where GPS signals are missing or limited, VIO technology can help these devices accurately perceive their own position and motion status, providing a robust and reliable navigation solution.

[0003] However, the existing VIO will produce large cumulative errors during long-term operation, and the posture is usually corrected through a loop detection module. In the absence of a detection module, the positioning accuracy of the VIO will gradually decrease. In summary, a high-precision VIO positioning technology that does not rely on loop detection is proposed, which is a problem that existing technicians need to solve. Summary of the invention

[0004] In response to the problems mentioned in the prior art, the present invention proposes a visual inertial positioning method, system, device and storage medium based on ArUco tags, which can solve the problem that the existing VIO system has large cumulative errors when running for a long time and without a detection module.

[0005] The present invention provides a visual inertial positioning method based on ArUco tags, comprising:

[0006] S1: Arrange multiple ArUco tags with unique IDs in the use environment and obtain the relative position relationship of adjacent ArUco tags;

[0007] S2: Establish a VIO system, obtain images of the visual inertial odometer and IMU data, and obtain a VIO coordinate system according to the images and IMU data;

[0008] S3: Define the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtain the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtain the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system;

[0009] S4: Determine whether the ArUco tag in the image frame is detected during the VIO system operation. If not, optimize the camera's position in the VIO coordinate system until the VIO system detects the ArUco tag. If yes, establish an adaptive global optimization function to eliminate the accumulated error and complete the positioning.

[0010] Preferably, the specific process of optimizing the posture of the camera in the VIO coordinate system in step S4 is: using IMU data to obtain the starting posture of the image frame, and optimizing the posture of the camera in the VIO coordinate system according to the starting posture, the visual reprojection error of the image frame, the pre-integration error between IMU data frames, and the marginalized prior error, until the VIO system detects the ArUco tag.

[0011] Preferably, an optimization objective function is constructed based on the marginalized prior error, the IMU data inter-frame pre-integration error, and the visual reprojection error:

[0012]

[0013] Where: H p is the Hessian matrix; is the state of all cameras within the sliding window; is the set of all IMU measurements; is the pre-integrated quantity; is the visual observation; is the set of features observed at least twice in the current sliding window; ρ is the loss function; r p is the marginalized prior error; is the pre-integration error between IMU data frames; is the error function of the visual reprojection error.

[0014] Preferably, all camera states within the sliding window The expression is as follows:

[0015]

[0016] Where: is the position of IMU in the VIO coordinate system; is the speed of IMU in the VIO coordinate system; is the direction of IMU in the VIO coordinate system; b a is the acceleration bias in the IMU coordinate system; b g is the gyroscope bias in the IMU coordinate system; n is the total number of key frames; m is the total number of feature points in the sliding window; λ l is the inverse depth of the lth feature point in the image frame observed for the first time; It is the external reference between the camera and IMU.

[0017] Preferably, the specific process of establishing the adaptive global optimization function in step S4 is: after the camera recognizes the ArUco tag, the ArUco tag ID is detected to determine the relative position relationship between the camera and the current ArUco tag;

[0018] The real pose of the camera in the VIO coordinate system is obtained according to the relative pose relationship of adjacent ArUco tags in step S1, the pose of the camera in the ArUco coordinate system in step S3, and the relative pose relationship between the VIO coordinate system and the ArUco coordinate system in step S3;

[0019] The objective function of adaptive global optimization is established based on the constraints between key frames and the label constraints generated after the current ArUco label positioning, as shown in the following formula:

[0020]

[0021] Where S is the set of all key frames; M is the set of camera poses obtained by label detection; r i,j is the inter-frame constraint of the key frame; r i,k It is the label constraint obtained by identifying the current ArUco label;

[0022] The objective function of adaptive global optimization determines the number of key frames for inter-frame constraints by evaluating the uncertainty of the pose. The uncertainty of the pose is as follows:

[0023]

[0024] Where P k represents the covariance matrix of the k-th frame pose, δT represents the relative pose between the k-th frame and the k-1 frame, Represents the covariance matrix of the k-th frame IMU;

[0025] The difference of the maximum eigenvalue between frames is compared with the set threshold, and the key frames with a value less than the threshold are brought into the objective function of the adaptive global optimization for constraint, so as to eliminate the accumulated error and complete the positioning.

[0026] Preferably, the r i,j The definition is as follows:

[0027]

[0028] Where: is the position of the i-th key frame and the j-th key frame in the VIO coordinate system; is the position difference between frames i and j in the i-th frame coordinate system; ψ i is the yaw angle of the i-th key frame in the VIO coordinate system; ψ jis the yaw angle of the j-th key frame in the VIO coordinate system; is the difference between the yaw angles of frames i and j; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame;

[0029] The r i,k The definition is as follows:

[0030]

[0031] Where: is the position of the i-th key frame in the VIO coordinate system; is the real pose of the camera in the VIO coordinate system; is the position difference between frames i and k in the i-th frame coordinate system; ψ i is the yaw angle of the camera in the VIO coordinate system; ψ k is the actual yaw angle of the camera in the VIO coordinate system; is the difference between the yaw angles of frames i and k; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame.

[0032] Preferably, the threshold is the average of the maximum eigenvalues ​​of the previous i key frames.

[0033] A visual inertial positioning system based on ArUco tags, comprising:

[0034] The first acquisition module arranges multiple ArUco tags with unique IDs in the use environment and obtains the relative position relationship between adjacent ArUco tags;

[0035] The second acquisition module establishes a VIO system, acquires images and IMU data of the visual inertial odometer, and obtains a VIO coordinate system according to the images and IMU data;

[0036] The third acquisition module defines the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtains the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtains the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system;

[0037] The positioning module determines whether the ArUco tag in the image frame is detected during the VIO system operation. If not, the camera's position in the VIO coordinate system is optimized until the VIO system detects the ArUco tag. If so, an adaptive global optimization function is established to eliminate the accumulated error and complete the positioning.

[0038] A visual inertial positioning device based on ArUco tags comprises a processor and a memory, wherein the processor implements the above-mentioned visual inertial positioning method based on ArUco tags when executing a computer program stored in the memory.

[0039] A computer-readable storage medium, characterized in that it is used to store a computer program, wherein when the computer program is executed by a processor, the above-mentioned visual inertial positioning method based on ArUco tags is implemented.

[0040] Compared with the prior art, the present invention has achieved the following technical effects:

[0041] The present invention arranges ArUco tags and solves the problem of excessive cumulative error in the system positioning process when the VIO system does not have a loop detection module. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is the overall flow chart of the present invention;

[0043] Figure 2 It is the principle diagram of the process of the present invention;

[0044] Figure 3 This is the ArUco tag layout diagram of the present invention;

[0045] Figure 4 This is the adaptive key frame inter-frame constraint graph of the present invention. DETAILED DESCRIPTION

[0046] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0047] In the description of the present invention, unless otherwise clearly specified and limited, the terms "connected", "connected", and "fixed" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0048] In the description of this embodiment, the terms "upper", "lower", "right", etc., directions or positional relationships are based on the directions or positional relationships shown in the drawings, and are only for the convenience of description and simplification of operation, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are only used to distinguish in the description and have no special meaning.

[0049] like Figure 1 and Figure 2 As shown, the present invention provides a visual inertial positioning method based on ArUco tags, comprising:

[0050] S1: Arrange multiple ArUco tags with unique IDs in the use environment and obtain the relative position relationship of adjacent ArUco tags;

[0051] S2: Establish a VIO system, obtain images of the visual inertial odometer and IMU data, and obtain a VIO coordinate system according to the images and IMU data;

[0052] S3: Define the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtain the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtain the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system;

[0053] S4: Determine whether the ArUco tag in the image frame is detected during the VIO system operation. If not, optimize the camera's position in the VIO coordinate system until the VIO system detects the ArUco tag. If yes, establish an adaptive global optimization function to eliminate the accumulated error and complete the positioning.

[0054] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments:

[0055] Step 1: If Figure 3 As shown, according to Figure 3 Arrange multiple ArUco tags in the usage scenario. In this embodiment, three ArUco tags are arranged. The ID of each ArUco tag is unique. The relative position relationship between the ArUco tags is obtained by measurement, such as Figure 3 Shown in and

[0056] Step 2: Obtain the image and IMU data of the visual inertial odometer, initialize the VIO system, and use the image and IMU data of the visual inertial odometer to determine the VIO coordinate system;

[0057] Step 3: The ArUco tag detected by the VIO system for the first time is the initial ArUco tag. The initial ArUco tag is defined as the ArUco coordinate system to obtain the position and pose of the camera in the ArUco coordinate system. Get the camera's position T in the VIO coordinate system vc , multiplying the two to get the relative pose relationship between the VIO coordinate system and the ArUco coordinate system

[0058] Step 4: Determine whether the ArUco tag in the image frame is detected when the VIO system is running. If not, optimize the camera's pose in the VIO coordinate system until the VIO system detects the ArUco tag.

[0059] In this embodiment, when the VIO system does not detect the ArUco tag in the image frame, the IMU data is used to obtain the starting pose of the image frame, and the pose of the camera in the VIO coordinate system is optimized according to the starting pose, the visual reprojection error of the image frame, the pre-integration error between IMU data frames, and the marginalized prior error until the VIO system detects the ArUco tag.

[0060] The following formula is to construct the optimization objective function based on the marginalization prior error, the IMU data inter-frame pre-integration error and the visual reprojection error:

[0061]

[0062] Where: H p is the Hessian matrix; is the state of all cameras within the sliding window; is the set of all IMU measurements; is the pre-integrated quantity; is the visual observation; is the set of features observed at least twice in the current sliding window; ρ is the loss function; r p is the marginalized prior error; is the pre-integration error between IMU data frames; is the error function of the visual reprojection error.

[0063] All camera states within the sliding window The expression is as follows:

[0064]

[0065] Where: is the position of IMU in the VIO coordinate system; is the speed of IMU in the VIO coordinate system; is the direction of IMU in the VIO coordinate system; b ais the acceleration bias in the IMU coordinate system; b g is the gyroscope bias in the IMU coordinate system; n is the total number of key frames; m is the total number of feature points in the sliding window; λ l is the inverse depth of the lth feature point in the image frame observed for the first time; It is the external reference between the camera and IMU.

[0066] In this embodiment, marginalization is used in combination with sliding windows. When a new pose enters the window, the earliest data will be removed from the window. In this process, the marginalization operation is responsible for processing the data of the removed window and retaining its influence on the remaining data in the form of constraints. The error e of the prior information is obtained by decomposing Hδx=b, where H=J T J,b=Je,J is the Jacobian of the error to the optimization variable. The marginalization operation solves the part of δx to be retained, rewriting Hδx=b as:

[0067]

[0068] Let δx b For the retained part, by eliminating the equation, we get

[0069]

[0070] Through Decompose and obtain the marginalized prior error.

[0071] The error measured by the IMU in this embodiment is defined as:

[0072]

[0073] Where: [g] xyz Represents the operation of extracting the imaginary part of the quaternion; [t p ,r v ,r q ,] respectively represent the difference between the position, velocity, and posture of the two frames and the error of the pre-integration of the two frames, [r ba ,t bg ] represents the zero bias error of the IMU in two consecutive frames; is the rotation matrix of the kth frame; is the position of the k+1th frame; is the position of the kth frame; g v is the acceleration due to gravity; Δt k is the time difference between the kth frame and the k+1th frame; is the speed of the kth frame; is the position integral between the kth frame and the k+1th frame; is the velocity integral between the kth frame and the k+1th frame; is the rotation integral between the kth frame and the k+1th frame; is the quaternion of the kth frame; is the quaternion of the k+1th frame; is the accelerometer zero bias of the kth frame; is the accelerometer zero bias of the k+1 frame; is the gyro bias of the kth frame; is the gyro bias of the k+1 frame.

[0074] The visual reprojection error of this embodiment is defined as:

[0075]

[0076] Where: It represents the coordinates of the feature points of the i-th frame projected to the camera coordinate system of the j-th frame after a series of coordinate transformations, u c ,v c Represents the pixel coordinates, T bc is the external parameter matrix between the camera and IMU, is the inverse matrix of the extrinsic matrix between the camera and IMU, is the inverse matrix of the pose matrix of the jth frame in the VIO coordinate system, is the pose matrix of the i-th frame in the VIO coordinate system.

[0077] This embodiment continuously optimizes the posture T in the VIO coordinate system by eliminating the above three errors in real time. vc Until the VIO system can detect the ArUco tag, the relative pose relationship between the VIO coordinate system and the ArUco coordinate system is obtained through the process of step 2

[0078] If the ArUco tag in the image frame can be detected, an adaptive global optimization function is established to eliminate the accumulated error and complete the positioning.

[0079] In this embodiment, after the camera recognizes the ArUco tag, it detects the ArUco tag ID and determines the relative position relationship between the camera and the current ArUco tag. According to the relative position relationship between the current label and the initial label in step 1 The position of the camera in the ArUco coordinate system Relative pose relationship between VIO coordinate system and ArUco coordinate system Get the real pose T of the camera in the VIO coordinate system vc .

[0080] according to Figure 4 As shown in the figure, the objective function of adaptive global optimization is established based on the constraints between key frames and the label constraints generated after ArUco label positioning:

[0081]

[0082] Where S is the set of all key frames; M is the set of camera poses obtained by label detection; r i,j is the inter-frame constraint of the key frame; r i,k It is the label constraint obtained by identifying the current ArUco label.

[0083]

[0084] Where: is the position of the i-th key frame and the j-th key frame in the VIO coordinate system; is the position difference between frames i and j in the i-th frame coordinate system; ψ i is the yaw angle of the i-th key frame in the VIO coordinate system; ψ j is the yaw angle of the j-th key frame in the VIO coordinate system; is the difference between the yaw angles of frames i and j; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame.

[0085] r i,k The definition is as follows:

[0086]

[0087] Where: is the position of the i-th key frame in the VIO coordinate system; is the real pose of the camera in the VIO coordinate system; is the position difference between frames i and k in the i-th frame coordinate system; ψ i is the yaw angle of the camera in the VIO coordinate system; ψ k is the actual yaw angle of the camera in the VIO coordinate system; is the difference between the yaw angles of frames i and k; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame.

[0088] The adaptive optimization function determines the number of key frames for inter-frame constraints by evaluating the uncertainty of the pose, where the uncertainty of the pose is represented by the covariance matrix, and the transfer formula of the covariance matrix is:

[0089]

[0090] Among them, P krepresents the covariance matrix of the k-th frame pose, δT represents the relative pose between the k-th frame and the k-1 frame, Represents the covariance matrix of the IMU of the kth frame.

[0091] The maximum eigenvalue of the pose covariance matrix is ​​used as the uncertainty of the pose. The difference between the maximum eigenvalues ​​between frames is compared with the set threshold. If it is less than the threshold, it is considered that the two frames can form an inter-frame constraint. If it is less than the threshold, it is considered that the two frames cannot form a constraint. Figure 3 As shown, the dotted lines in the figure represent constraints established between key frames. The threshold in this embodiment is the mean of the maximum eigenvalues ​​of the key frames of the previous i frames. The key frames smaller than the threshold are brought into the objective function of the above-mentioned adaptive global optimization for constraints, eliminating the accumulated errors and completing the positioning.

[0092] Based on the same inventive concept, an embodiment of the present invention also provides a visual-inertial positioning system based on ArUco tags. Since the principle of solving the problem by the visual-inertial positioning system based on ArUco tags is similar to that of the aforementioned visual-inertial positioning method based on ArUco tags, the implementation of the visual-inertial positioning system based on ArUco tags can refer to the implementation of the visual-inertial positioning method based on ArUco tags, and the repeated parts will not be repeated.

[0093] In a specific implementation, the visual inertial positioning system based on ArUco tags provided in an embodiment of the present invention specifically includes:

[0094] The first acquisition module arranges multiple ArUco tags with unique IDs in the use environment and obtains the relative position relationship between adjacent ArUco tags;

[0095] The second acquisition module establishes a VIO system, acquires images and IMU data of the visual inertial odometer, and obtains a VIO coordinate system according to the images and IMU data;

[0096] The third acquisition module defines the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtains the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtains the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system;

[0097] The positioning module determines whether the ArUco tag in the image frame is detected during the VIO system operation. If not, the camera's position in the VIO coordinate system is optimized until the VIO system detects the ArUco tag. If so, an adaptive global optimization function is established to eliminate the accumulated error and complete the positioning.

[0098] Correspondingly, an embodiment of the present invention further provides a visual inertial positioning device based on ArUco tags, comprising a processor and a memory, wherein when the processor executes a computer program stored in the memory, the visual inertial positioning method based on ArUco tags provided in an embodiment of the present invention is implemented.

[0099] For more specific processes of the above method, please refer to the corresponding contents disclosed in the aforementioned embodiments, which will not be repeated here.

[0100] Correspondingly, an embodiment of the present invention further provides a computer-readable storage medium for storing a computer program, wherein when the computer program is executed by a processor, the visual-inertial positioning method based on ArUco tags as provided in the embodiment of the present invention is implemented.

[0101] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the systems, devices, and storage media disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method part.

[0102] Professionals may further appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in the above description according to function. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians may use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0103] The steps of the method or algorithm described in conjunction with the embodiments disclosed herein may be implemented directly using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

[0104] Finally, it should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0105] The visual inertial positioning method, system, device and storage medium based on ArUco tags provided by the present invention are introduced in detail above. Specific examples are used in this article to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A visual inertial positioning method based on ArUco tags, characterized in that: include: S1: Arrange multiple ArUco tags with unique IDs in the use environment and obtain the relative position relationship of adjacent ArUco tags; S2: Establish a VIO system, obtain images of the visual inertial odometer and IMU data, and obtain a VIO coordinate system according to the images and IMU data; S3: Define the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtain the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtain the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system; S4: Determine whether the ArUco tag in the image frame is detected during the VIO system operation. If not, optimize the camera's position in the VIO coordinate system until the VIO system detects the ArUco tag. If yes, establish an adaptive global optimization function to eliminate the accumulated error and complete the positioning.

2. According to claim 1, a visual inertial positioning method based on ArUco tags is characterized in that: The specific process of optimizing the camera's posture in the VIO coordinate system in step S4 is: using the IMU data to obtain the starting posture of the image frame, and optimizing the camera's posture in the VIO coordinate system according to the starting posture, the visual reprojection error of the image frame, the pre-integration error between IMU data frames, and the marginalized prior error until the VIO system detects the ArUco tag.

3. According to claim 2, a visual inertial positioning method based on ArUco tags is characterized in that: The optimization objective function is constructed based on the marginalized prior error, the IMU data inter-frame pre-integration error, and the visual reprojection error: Where: H p is the Hessian matrix; is the state of all cameras within the sliding window; is the set of all IMU measurements; is the pre-integrated quantity; is the visual observation; is the set of features observed at least twice in the current sliding window; ρ is the loss function; r p is the marginalized prior error; is the pre-integration error between IMU data frames; is the error function of the visual reprojection error.

4. The visual inertial positioning method based on ArUco tags according to claim 3, characterized in that: All camera states within the sliding window The expression is as follows: Where: is the position of IMU in the VIO coordinate system; is the speed of IMU in the VIO coordinate system; is the direction of IMU in the VIO coordinate system; b a is the acceleration bias in the IMU coordinate system; b g is the gyroscope bias in the IMU coordinate system; n is the total number of key frames; m is the total number of feature points in the sliding window; λ l is the inverse depth of the lth feature point in the image frame observed for the first time; It is the external reference between the camera and IMU.

5. The visual inertial positioning method based on ArUco tags according to claim 1, characterized in that: The specific process of establishing the adaptive global optimization function in step S4 is: after the camera recognizes the ArUco tag, the ArUco tag ID is detected to determine the relative position relationship between the camera and the current ArUco tag; The real pose of the camera in the VIO coordinate system is obtained according to the relative pose relationship of adjacent ArUco tags in step S1, the pose of the camera in the ArUco coordinate system in step S3, and the relative pose relationship between the VIO coordinate system and the ArUco coordinate system in step S3; The objective function of adaptive global optimization is established based on the constraints between key frames and the label constraints generated after the current ArUco label positioning, as shown in the following formula: Where S is the set of all key frames; M is the set of camera poses obtained by label detection; r i,j is the inter-frame constraint of the key frame; r i,k It is the label constraint obtained by identifying the current ArUco label; The objective function of adaptive global optimization determines the number of key frames for inter-frame constraints by evaluating the uncertainty of the pose. The uncertainty of the pose is as follows: Where P k represents the covariance matrix of the k-th frame pose, δT represents the relative pose between the k-th frame and the k-1 frame, Represents the covariance matrix of the k-th frame IMU; The difference of the maximum eigenvalue between frames is compared with the set threshold, and the key frames with a value less than the threshold are brought into the objective function of the adaptive global optimization for constraint, so as to eliminate the accumulated error and complete the positioning.

6. The visual inertial positioning method based on ArUco tags according to claim 5, characterized in that: The r i,j The definition is as follows: Where: is the position of the i-th key frame and the j-th key frame in the VIO coordinate system; is the position difference between frames i and j in the i-th frame coordinate system; ψ i is the yaw angle of the i-th key frame in the VIO coordinate system; ψ j is the yaw angle of the j-th key frame in the VIO coordinate system; is the difference between the yaw angles of frames i and j; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame; The r i,k The definition is as follows: Where: is the position of the i-th key frame in the VIO coordinate system; is the real pose of the camera in the VIO coordinate system; is the position difference between frames i and k in the i-th frame coordinate system; ψ i is the yaw angle of the camera in the VIO coordinate system; ψ k is the actual yaw angle of the camera in the VIO coordinate system; is the difference between the yaw angles of frames i and k; R is the rotation matrix of the i-th key frame in the VIO coordinate system; is the pitch angle of the i-th frame; is the roll angle of the i-th frame.

7. The visual inertial positioning method based on ArUco tags according to claim 5, characterized in that: The threshold is the average of the maximum eigenvalues ​​of the key frames of the previous i frames.

8. A visual inertial positioning system based on ArUco tags, characterized in that: include: The first acquisition module arranges multiple ArUco tags with unique IDs in the use environment and obtains the relative position relationship between adjacent ArUco tags; The second acquisition module establishes a VIO system, acquires images and IMU data of the visual inertial odometer, and obtains a VIO coordinate system according to the images and IMU data; The third acquisition module defines the initial ArUco tag detected by the VIO system as the ArUco coordinate system, obtains the position and pose of the camera in the ArUco coordinate system and the position and pose of the camera in the VIO coordinate system, and obtains the relative position and pose relationship between the VIO coordinate system and the ArUco coordinate system; The positioning module determines whether the ArUco tag in the image frame is detected during the VIO system operation. If not, the camera's position in the VIO coordinate system is optimized until the VIO system detects the ArUco tag. If so, an adaptive global optimization function is established to eliminate the accumulated error and complete the positioning.

9. A visual inertial positioning device based on ArUco tags, characterized in that: The method comprises a processor and a memory, wherein when the processor executes the computer program stored in the memory, the visual inertial positioning method based on the ArUco tag as claimed in any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: Used to store a computer program, wherein when the computer program is executed by a processor, the visual inertial positioning method based on ArUco tags according to any one of claims 1 to 7 is implemented.

Citation Information

Cited By

  • Multi-label fusion positioning method based on geometric constraint

    CN120991855A