CN118209137B - VIO initialization method, device and equipment assisted by ArUco codes - Google Patents
VIO initialization method, device and equipment assisted by ArUco codes Download PDFInfo
- Publication number
- CN118209137B CN118209137B CN202410641281.2A CN202410641281A CN118209137B CN 118209137 B CN118209137 B CN 118209137B CN 202410641281 A CN202410641281 A CN 202410641281A CN 118209137 B CN118209137 B CN 118209137B
- Authority
- CN
- China
- Prior art keywords
- aruco
- coordinate system
- coordinates
- camera
- codes
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000011423 initialization method Methods 0.000 title claims description 19
- 238000006243 chemical reaction Methods 0.000 claims abstract description 62
- 238000000034 method Methods 0.000 claims abstract description 54
- 230000008569 process Effects 0.000 claims abstract description 22
- 230000001133 acceleration Effects 0.000 claims description 44
- 239000013598 vector Substances 0.000 claims description 25
- 230000005484 gravity Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000003068 static effect Effects 0.000 claims description 10
- 238000010606 normalization Methods 0.000 claims description 3
- 230000005284 excitation Effects 0.000 abstract description 8
- 238000001514 detection method Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 230000000007 visual effect Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000012935 Averaging Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003190 augmentative effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000012467 final product Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000047 product Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Remote Sensing (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Data Mining & Analysis (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Closed-Circuit Television Systems (AREA)
- Image Analysis (AREA)
Abstract
The invention discloses a method, a device and equipment for initializing VIO assisted by ArUco codes, which relate to the field of unmanned aerial vehicle navigation, and the method comprises the following steps: after the motion state of the IMU meets the initialization requirement, determining the coordinates of the angular point of ArUco codes with the real size under the camera coordinate system according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes; according to the coordinates of the angular point of ArUco codes with real size in the camera coordinate system and the coordinates of the angular point of ArUco codes in the ArUco coordinate system, determining the conversion relation from ArUco coordinates to camera coordinates; and determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing the initialization process. By introducing ArUco codes, the problem that the conventional VIO initialization algorithm has higher requirement on motion excitation of equipment in the initialization process is solved, the initialization time is shortened while the initialization precision is ensured, better robustness is achieved, and a more stable initialization result is provided for engineering application.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle navigation, in particular to a method, a device and equipment for initializing VIO assisted by ArUco codes.
Background
With the rapid development of computer technology and sensor technology, research layers such as open source frameworks of VINS-Mono, VINS-Fusion and OpenVINS, which are formed by integrating an inertial measurement unit (Inertial Measurement Unit, IMU) with a visual inertial measurement unit (Visual Inertial Odometry, IO) in visual inertial navigation (visual-INERTIAL SYSTEM), have become the foundation of many engineering applications and academic researches.
In the VIO algorithm, according to the number of vision sensors (usually, cameras), the two conditions of monocular (only one camera is used) and binocular (two cameras are used) can be simply divided, wherein the vision observation under the monocular condition lacks absolute scale information, so that the pose obtained by direct tracking and positioning and the depth of map points and the like cannot be aligned with the real scale, the existing algorithm is strictly designed for the VIO initialization part, but the existing initialization process is strictly required for motion excitation, the phenomenon that the initialization is difficult to realize and easy to fail occurs for large-scale equipment while the required time is longer, and the more stable initialization process is important for the requirement of higher stability in engineering use.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a method, a device and equipment for initializing VIO by utilizing ArUco code assistance, so as to solve the problems of slower and unstable VIO initialization flow in the prior art.
In order to achieve the above purpose, the invention adopts the following technical scheme:
In a first aspect, the present application provides a method for initializing VIO with ArUco code assistance, comprising the steps of:
After the motion state of the IMU meets the initialization requirement, determining the coordinates of the angular point of ArUco codes with the real size under the camera coordinate system according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes;
According to the coordinates of the angular point of ArUco codes with real size in the camera coordinate system and the coordinates of the angular point of ArUco codes in the ArUco coordinate system, determining the conversion relation from ArUco coordinates to camera coordinates;
And determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing the initialization process.
In some alternative embodiments, determining that the motion state of the IMU meets the initialization requirement includes:
Determining an average value of the original acceleration observation values according to the IMU acceleration;
Determining the variance of the original acceleration observation value according to the average value of the original acceleration observation value;
And comparing the variance of the original acceleration observation value with an acceleration variance threshold value, and if the variance of the original acceleration observation value is smaller than the acceleration variance threshold value, enabling the IMU equipment to be in a static state and meeting the initialization requirement.
In some alternative embodiments, determining that the angular point of ArUco codes has the coordinates of the real size in the camera coordinate system according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes includes:
according to pixel coordinates of angular points of ArUco codes detected by a camera, determining coordinates of angular points of ArUco codes under a camera normalization coordinate system by combining camera internal parameters;
and determining the coordinates of the corner of the ArUco code with the real size in the camera coordinate system according to the coordinates of the corner of the ArUco code in the camera normalized coordinate system and combining the known size of the ArUco code.
In some alternative embodiments, determining the coordinates of the corner of code ArUco with the real dimensions in the camera coordinate system based on the coordinates of the corner of code ArUco in the camera normalized coordinate system, in combination with the known dimensions of code ArUco, includes:
Presetting depth values of corner points of the ArUco codes according to coordinates of the corner points of the ArUco codes under a camera normalized coordinate system, and determining coordinates of four corner points of the ArUco codes under the camera coordinate system;
according to the coordinates of four corner points of ArUco codes under the camera coordinate system, the coordinates of the corner points of ArUco codes with real sizes under the camera coordinate system are determined by combining the known sizes of ArUco codes.
In some alternative embodiments, when ArUco codes are multiple, the accuracy of the depth value is improved in combination with the distance between two ArUco codes.
In some alternative embodiments, determining the conversion relationship from ArUco coordinates to camera coordinates based on the coordinates of the ArUco-code corner with the real size in the camera coordinate system and the coordinates of the ArUco-code corner in the ArUco coordinate system includes:
according to the coordinate of the angular point of ArUco codes with the real size under the camera coordinate system and the coordinate of the angular point of ArUco codes in the ArUco coordinate system, obtaining the conversion relation between the two coordinate systems, and determining the conversion residual error of the angular point of each ArUco codes;
And determining the conversion relation from ArUco coordinates to camera coordinates according to the sum of conversion residuals of all the ArUco code corner points and minimizing the sum of all the conversion residuals.
In some alternative embodiments, according to the conversion relation from ArUco coordinates to camera coordinates, determining the conversion relation from the global coordinate system to ArUco code coordinate system, and completing the initialization process, including:
Determining an average value of the original acceleration observation values according to the IMU acceleration;
determining a rotation relation from the IMU coordinate system to the global coordinate system when the vehicle is stationary according to the average value of the original acceleration observation values and a gravity vector under a specified global coordinate system;
And determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the rotation relation from the IMU coordinate system to the global coordinate system and the conversion relation from the ArUco code coordinate system to the camera coordinate system when in rest and combining the conversion matrix from the camera to the IMU coordinate system, thereby completing the initialization process.
In some alternative embodiments, determining the rotational relationship of the IMU coordinate system to the global coordinate system at rest from the average of the raw acceleration observations and the gravity vector at the prescribed global coordinate system includes:
Normalizing the gravity vector according to the average value of the original acceleration observation values and the gravity vector under the specified global coordinate system, and determining ；
According toAnd constructing unit vectors under ideal conditions, combining with Schmidt orthogonalization, constructing；
According toAnd cross-multiplying byObtaining；
According to、、Determining a rotation matrix；
To the rotation matrixTranspose, determining the rotational relationship of IMU coordinate system to global coordinate system at restWherein, the method comprises the steps of, wherein,Is a unit vector of the x-axis,Is the unit vector of the y-axis,Is the unit vector of the z-axis.
In a second aspect, the present application further provides an apparatus for using a ArUco code-assisted VIO initialization method, including:
The coordinate determining module is used for determining the coordinates of the angular point of ArUco codes with the real size under the camera coordinate system according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes after the motion state of the IMU meets the initialization requirement;
the coordinate conversion module is used for determining the conversion relation from ArUco coordinates to camera coordinates according to the coordinates of the angular points of ArUco codes with real dimensions in the camera coordinate system and the coordinates of the angular points of ArUco codes in the ArUco coordinate system;
And the initialization completion module is used for determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing an initialization process.
In a third aspect, the present application also provides an apparatus for using a ArUco code assisted VIO initialization method, where the apparatus includes a processor, a memory, and a program stored on the memory and executable by the processor for using a ArUco code assisted initialization method, where the program for using a ArUco code assisted initialization method implements any of the steps of the ArUco code assisted VIO initialization method when executed by the processor.
Compared with the prior art, the invention has the advantages that:
By introducing ArUco codes, a static initialization flow is designed, motion excitation is not needed, the actual engineering scene is more fit, the problem that in the traditional VIO initialization algorithm, the motion excitation of equipment in the initialization process has higher requirements, and in the actual engineering application scene, the equipment characteristics often bring partial freedom degree fixity, so that the requirement of omnidirectional motion excitation is difficult to meet is solved; the method effectively shortens the initialization time while ensuring the initialization precision, has better robustness, and can provide a more stable initialization result for engineering application.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a method for initializing VIO using ArUco code assistance in accordance with the present invention;
Fig. 2 is a schematic diagram of a coordinate system and a conversion relationship constructed in a VIO initialization method using ArUco codes.
In the figure: m, arUco code coordinate system; f. each frame of camera coordinate system; b. an IMU coordinate system at each moment; g. global reference coordinate system.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present application more apparent, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application, and it is apparent that the described embodiments are some embodiments of the present application, but not all embodiments of the present application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
First, some technical terms in the present application are explained so as to facilitate understanding of the present application by those skilled in the art.
The ArUco code is a two-dimensional bar code for computer vision and augmented reality, is easy to identify and detect, has been realized based on ArUco codes in the instant positioning and map construction (SLAM, simultaneous localization AND MAPPING) technology, such as ArUco-SLAM, and uses ArUco codes as marks in many projects to provide absolute position information for positioning processes.
Therefore, the method designs a VIO initialization method assisted by ArUco codes based on ArUco codes detection and tracking, and provides a rapid and stable initialization method for engineering.
The coordinate system and conversion relation constructed by the method are shown in figure 2 of the specification, wherein an m coordinate system represents ArUco code coordinate system, an f coordinate system represents each frame of camera coordinate system, a b coordinate system represents IMU coordinate system at each moment, a g coordinate system represents global reference coordinate system, and T represents conversion relation among coordinate systems, whereinRepresenting the conversion relation from ArUco code coordinate system to camera coordinate system,Representing the conversion relation from the IMU coordinate system to the camera coordinate system,Representing the conversion relation from the global coordinate system to the IMU coordinate system,Representing the conversion relation from the global coordinate system to the ArUco code coordinate system.
Specifically, as shown in fig. 1, in one aspect, the present application provides a VIO initialization method using ArUco codes for assistance, including the following steps:
s1: when the motion state of the IMU meets the initialization requirement, determining the coordinates of the angular point of ArUco codes with the real size under the camera coordinate system according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes.
It can be understood that, before step S1, the visual original observation information and the IMU original observation data are collected by the information collecting device, that is, the image information of ArUco codes is collected by using the monocular camera, and the acceleration, the angular speed and the like of the IMU are collected. And detecting angular points of ArUco codes during acquisition.
For detection and identification of ArUco codes, there are many mature open source algorithms, such as ar_track_alvar, aruco module of Opencv, etc., where the latter can effectively detect a plurality of ArUco codes under the same scene and has higher robustness and angular point detection precision, so the method uses aruco module of Opencv to extract ArUco code angular points, and records the number of detected ArUco codes as followsDetected ArUco code corner coordinate sequenceThe method comprises the following steps:
Wherein the pixel coordinate system coordinates of each corner point Can be expressed as。
It should be noted that, when a plurality of ArUco codes are used, four corner coordinates corresponding to each ArUco code should be ensuredFour corner points corresponding to the left lower corner, the right upper corner and the left upper corner of ArUco codes respectively; and it should be ensured that in the case of a plurality of ArUco codes, the coordinate sequence should be a horizontal arrangement relationship of ArUco codes corresponding from left to right.
In some alternative embodiments, whether the motion state of the IMU meets the initialization requirement is determined according to whether the IMU is in a stationary state, and the initialization is started after the IMU is in the stationary state.
It can be understood that the initialization method designed in the present invention is an initialization method in a stationary state, so that it is necessary to ensure that the motion state determined in the first step is a stationary state to continue the initialization step.
Specifically, determining that the motion state of the IMU meets the initialization requirement includes:
A: and determining the average value of the original acceleration observation value according to the IMU acceleration.
It will be appreciated that there are many ways of detecting zero speed of an IMU, and that the method uses acceleration slip variance detection to determine if the device is in a stationary state, as it requires the device to remain stationary during the initialization phase. The average value of the original acceleration observations is:
wherein, As a total number of observations within a window,Represent the firstThe accelerometer observations, consisting of three-axis accelerations, can be expressed as，The average value of the original acceleration observed values obtained by calculation is used.
B: determining the variance of the original acceleration observation according to the average value of the original acceleration observation：
C: and comparing the variance of the original acceleration observation value with an acceleration variance threshold, and if the variance of the original acceleration observation value is smaller than the acceleration variance threshold, enabling the IMU equipment to be in a static state and meeting the initialization requirement.
It will be appreciated that setting the acceleration variance threshold empirically is noted asThe calculated variance valueAnd (3) withIn contrast, ifThe device is considered to be in a stationary state; otherwise, the device is considered to be in motion.
When the motion state of the IMU meets the initialization requirement, the angular point coordinates of ArUco codes can be determined.
Under ArUco codes, the coordinates of ArUco codes are manually specified, the first ArUco codes have the left lower corner point of ArUco codes as the origin of the coordinate system, and the coordinates of (0, 0), and the other ArUco codes have the coordinates of (S, 0), (S, S, 0) and (0, S, 0) respectively; if the ArUco code space is D, the coordinates of each corner point of the next ArUco codes under the ArUco code coordinate system are as follows: (D, 0), (S+D, 0), (S+D, S, 0) and (D, S, 0), and so on, all ArUco corner points are obtained to be coordinates under the ArUco coordinate system. Wherein S is the side length of ArUco codes.
Since the actual size of ArUco codes is known by measurement, in some alternative embodiments, step S1 above further comprises:
S11: and determining the coordinates of the angular points of the ArUco codes under a camera normalized coordinate system according to the pixel coordinates of the angular points of the ArUco codes detected by the camera and the camera internal parameters.
It can be understood that, according to the detected pixel coordinates of the corner points, the coordinates of the normalized coordinate system of the corner point camera are obtained by using the camera internal parameters K:
wherein, For the detected corner pixel coordinates,Normalizing the coordinates of the corner camera under the coordinate system, i= [1,2,3 … … n ], j= [1,2,3,4], namelyRepresents the jth corner of the ith ArUco th code.
At this time, the coordinates of the corner point do not include depth information, and the process proceeds to S12 in order to acquire the three-dimensional space conversion relationship.
S12: and determining the coordinates of the corner of the ArUco code with the real size in the camera coordinate system according to the coordinates of the corner of the ArUco code in the camera normalized coordinate system and combining the known size of the ArUco code.
It will be appreciated that using the known size of ArUco yards (i.e., arUco yards side length S), for one of the ArUco yards, the depth value of the unknown corner point is set to beAnd obtaining the coordinates under the camera coordinate system according to the obtained coordinates of the normalized coordinate system, wherein the coordinates under the camera coordinate system are as follows:
The distance between adjacent angular points represents ArUco yards of side length, namely:
in the formula, the sum of squares of the coordinate difference between two points is calculated, and the specific calculation formula is as follows:
similarly, the distance between two corner points on a diagonal can be expressed as:
For four corner points of ArUco codes, 6 equations, namely four side length equations and two diagonal equations of ArUco codes, can be obtained by utilizing the two conditions, wherein 4 unknown parameters are obtained, and after joint solution, coordinates of the corner points with real scale information under a camera coordinate system are obtained, namely 。
In some alternative embodiments, when there are multiple ArUco codes, a new residual relationship for coordinate transformation, such as the distance between two ArUco codes, may be addedThe distance relation between the corresponding angular points of two adjacent ArUco codes is as follows:
similarly, the distance between two corner points of two adjacent ArUco code diagonal relations is as follows:
And the accuracy of the depth information is improved by utilizing the distance relation between ArUco codes. Wherein:
s2: and determining the conversion relation from ArUco coordinates to camera coordinates according to the coordinates of the angular point of ArUco codes with real dimensions in the camera coordinate system and the coordinates of the angular point of ArUco codes in the ArUco coordinate system.
Specifically, according to the fact that the angular point of ArUco codes has the real-size coordinates under the camera coordinate system and the coordinate of the angular point of ArUco codes in the ArUco coordinate system, the conversion relation between the two coordinate systems is obtained, and the conversion residual error of each angular point of ArUco codes is determined.
Wherein the residual of the transform for each corner is:
In the method, in the process of the invention, And (3) withThe rotation matrix and the translation vector which respectively represent the angular points and are converted from a camera coordinate system to a ArUco code coordinate system are unknown,Is the coordinates of the corner point in ArUco yards,The residual is transformed for the corner of ArUco codes.
And listing error square products of all corner points, and determining a conversion relation from ArUco coordinates to camera coordinates according to the sum of conversion residual errors of all the corner points of ArUco codes and minimizing the sum of all the conversion residual errors.
Here, it can be solved according to the least square methodAnd (3) withThe least squares method for such problems has been described in a great deal of literature, and is not repeated here, namely the conversion relation from the obtained camera coordinate system to the ArUco code coordinate system is thatThe conversion of ArUco code coordinate system to camera coordinate can be expressed asWherein:
s3: and determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing the initialization process.
In some alternative embodiments, the step S3 includes:
s31: and determining the average value of the original acceleration observation value according to the IMU acceleration.
S32: and determining the rotation relation from the IMU coordinate system to the global coordinate system when the IMU coordinate system is stationary according to the average value of the original acceleration observation values and the gravity vector under the specified global coordinate system.
It can be understood that the gravity vector under the world coordinate system is (0, G), namelyAnd (0, G) to obtain the rotation relation from the IMU to the world coordinate system under the static condition, wherein the characteristic that the 3-axis acceleration of the IMU is mutually orthogonal is utilized, and the Z-axis direction, namely the gravity vector is normalized:
Because in an ideal case the 3-axis direction should be (1,0,0)，(0,1,0)，(0, 1), So that the ideal unit vector is constructed firstThen according to Schmitt orthogonalization, construct：
Then according to the cross relation, calculate to get：
From the three unit vectors obtained, a rotation matrix can be obtained:
Then transpose the matrix to obtain the rotation relation from the IMU coordinate system to the world coordinate system Wherein, the method comprises the steps of, wherein,Is a unit vector of the x-axis,Is the unit vector of the y-axis,Is the unit vector of the z-axis.
Further, it is also possible to calculate from the calculatedTo determine the zero offset of the accelerometer in preparation for further steps of subsequent initialisation.
The initial zero offset of the gyroscope is obtained by directly averaging and calculating the original observation value of the gyroscope:
Zero offset of the accelerometer then takes into account the initial rotation:
wherein, The original observations of the gyroscope are: angular velocity.
The global reference coordinate system is completed by initializing the static time period, and a matrix sequence of converting ArUco code coordinate systems under the condition of M static frames into a camera coordinate system is obtained according to the static timeWherein the camera is transformed into the IMU coordinate systemFrom the prior external parameter calibration, the conversion relationship between the global coordinate system and the ArUco code coordinate system can be expressed as follows:
Pair sequence Averaging to obtain the final productAnd finishing the initialization flow.
It should be noted that, the length of the initialization time can be adjusted according to the experience value and the actual application scenario, and the initialization precision can be improved by properly increasing the rest time.
If the initialization is successfully completed, the VIO algorithm is normally executed afterwards, otherwise, S1-S3 are executed again, and the initialization is restarted.
In a second aspect, the present application further provides an apparatus for using the ArUco code-assisted VIO initialization method.
Specifically, the initializing device comprises a coordinate determining module, a coordinate converting module and an initializing completion module. The coordinate determining module is used for determining the coordinate of the angular point of ArUco codes with the real size under the camera coordinate system according to the coordinate of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes after the motion state of the IMU meets the initialization requirement; the coordinate conversion module is used for determining the conversion relation from ArUco coordinates to camera coordinates according to the coordinates of the angular points of ArUco codes with real dimensions in the camera coordinate system and the coordinates of the angular points of ArUco codes in the ArUco coordinate system; the initialization completion module is used for determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing the initialization process.
In a third aspect, the present application also provides an apparatus for using a ArUco code aided VIO initialization method, including a processor, a memory, and a ArUco code aided initialization method program stored on the memory and executable by the processor, wherein the ArUco code aided initialization method program, when executed by the processor, implements any of the steps of the ArUco code aided VIO initialization method described above.
According to the method, the device and the equipment for initializing the VIO by utilizing ArUco codes, the ArUco codes are introduced, so that a static initialization flow is designed, motion excitation is not needed, the method is more fit with an actual engineering scene, the problem that in a traditional VIO initialization algorithm, the motion excitation of equipment in the initialization process has higher requirements, and in the actual engineering application scene, the equipment characteristics often bring partial freedom degree fixity, so that the requirement of omnidirectional motion excitation is difficult to meet is solved; the method effectively shortens the initialization time while ensuring the initialization precision, has better robustness, and can provide a more stable initialization result for engineering application.
In the description of the present application, it should be noted that the azimuth or positional relationship indicated by the terms "upper", "lower", etc. are based on the azimuth or positional relationship shown in the drawings, and are merely for convenience of describing the present application and simplifying the description, and are not indicative or implying that the apparatus or element in question must have a specific azimuth, be constructed and operated in a specific azimuth, and thus should not be construed as limiting the present application. Unless specifically stated or limited otherwise, the terms "mounted," "connected," and "coupled" are to be construed broadly, and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present application can be understood by those of ordinary skill in the art according to the specific circumstances.
It should be noted that in the present application, relational terms such as "first" and "second" and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The foregoing is only a specific embodiment of the application to enable those skilled in the art to understand or practice the application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the application. Thus, the present application is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (8)
1. A method for initialization of VIO with ArUco code assistance, comprising the steps of:
When the motion state of the IMU meets the stationary initialization requirement, determining, according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of ArUco codes, the coordinates of the angular point of ArUco codes with the actual size in the camera coordinate system includes:
according to pixel coordinates of angular points of ArUco codes detected by a camera, determining coordinates of angular points of ArUco codes under a camera normalization coordinate system by combining camera internal parameters;
According to the coordinates of the angular points of ArUco codes under the camera normalized coordinate system, combining the known size of ArUco codes, determining the coordinates of the angular points of ArUco codes with real sizes under the camera coordinate system;
According to the coordinates of the angular point of ArUco codes with real size in the camera coordinate system and the coordinates of the angular point of ArUco codes in the ArUco coordinate system, determining the conversion relation from ArUco coordinates to camera coordinates;
according to the conversion relation from ArUco coordinates to camera coordinates, determining the conversion relation from a global coordinate system to a ArUco code coordinate system, and completing an initialization process;
according to the conversion relation from ArUco coordinates to camera coordinates, determining the conversion relation from a global coordinate system to a ArUco code coordinate system, and completing an initialization process, wherein the method comprises the following steps:
Determining an average value of the original acceleration observation values according to the IMU acceleration;
determining a rotation relation from the IMU coordinate system to the global coordinate system when the vehicle is stationary according to the average value of the original acceleration observation values and a gravity vector under a specified global coordinate system;
And determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the rotation relation from the IMU coordinate system to the global coordinate system and the conversion relation from the ArUco code coordinate system to the camera coordinate system when in rest and combining the conversion matrix from the camera to the IMU coordinate system, thereby completing the initialization process.
2. The method of claim 1, wherein determining that the motion state of the IMU meets an initialization requirement comprises:
Determining an average value of the original acceleration observation values according to the IMU acceleration;
Determining the variance of the original acceleration observation value according to the average value of the original acceleration observation value;
And comparing the variance of the original acceleration observation value with an acceleration variance threshold, and if the variance of the original acceleration observation value is smaller than the acceleration variance threshold, enabling the IMU equipment to be in a static state and meeting the initialization requirement.
3. The method of claim 1, wherein determining that the corner point of code ArUco has the coordinates of the true size in the camera coordinate system based on the coordinates of the corner point of code ArUco in the camera normalized coordinate system in combination with the known size of code ArUco, comprises:
Presetting depth values of corner points of the ArUco codes according to coordinates of the corner points of the ArUco codes under a camera normalized coordinate system, and determining coordinates of four corner points of the ArUco codes under the camera coordinate system;
according to the coordinates of four corner points of ArUco codes under the camera coordinate system, the coordinates of the corner points of ArUco codes with real sizes under the camera coordinate system are determined by combining the known sizes of ArUco codes.
4. A method as claimed in claim 3, wherein when there are a plurality of ArUco codes, the accuracy of the depth value is improved in combination with the distance between two ArUco codes.
5. The method of claim 1, wherein determining ArUco the conversion relationship of the coordinates to the camera coordinates based on the coordinates of the corner of ArUco codes with the real size in the camera coordinate system and the coordinates of the corner of ArUco codes in the ArUco coordinate system, comprises:
according to the coordinate of the angular point of ArUco codes with the real size under the camera coordinate system and the coordinate of the angular point of ArUco codes in the ArUco coordinate system, obtaining the conversion relation between the two coordinate systems, and determining the conversion residual error of the angular point of each ArUco codes;
And determining the conversion relation from ArUco coordinates to camera coordinates according to the sum of conversion residuals of all the ArUco code corner points and minimizing the sum of all the conversion residuals.
6. The method of claim 1, wherein determining the rotational relationship of the IMU coordinate system to the global coordinate system at rest based on the average of the raw acceleration observations and the gravity vector at the specified global coordinate system comprises:
Normalizing the gravity vector according to the average value of the original acceleration observation values and the gravity vector under the specified global coordinate system, and determining ；
According toAnd constructing unit vectors under ideal conditions, combining with Schmidt orthogonalization, constructing；
According toAnd cross-multiplying byObtaining；
According to、、Determining a rotation matrix；
To the rotation matrixTranspose, determining the rotational relationship of IMU coordinate system to global coordinate system at restWherein, the method comprises the steps of, wherein,Is a unit vector of the x-axis,Is the unit vector of the y-axis,Is the unit vector of the z-axis.
7. An apparatus for implementing the method of any one of claims 1-6 using a ArUco code-assisted VIO initialization method, comprising:
The coordinate determining module is configured to determine, according to the coordinates of the angular point of ArUco codes in the ArUco coordinate system and the actual size of the ArUco codes, the coordinates of the angular point of ArUco codes with the actual size in the camera coordinate system after the motion state of the IMU meets the static initialization requirement, where the coordinate determining module includes: according to pixel coordinates of angular points of ArUco codes detected by a camera, determining coordinates of angular points of ArUco codes under a camera normalization coordinate system by combining camera internal parameters; according to the coordinates of the angular points of ArUco codes under the camera normalized coordinate system, combining the known size of ArUco codes, determining the coordinates of the angular points of ArUco codes with real sizes under the camera coordinate system;
the coordinate conversion module is used for determining the conversion relation from ArUco coordinates to camera coordinates according to the coordinates of the angular points of ArUco codes with real dimensions in the camera coordinate system and the coordinates of the angular points of ArUco codes in the ArUco coordinate system;
And the initialization completion module is used for determining the conversion relation from the global coordinate system to the ArUco code coordinate system according to the conversion relation from ArUco coordinates to camera coordinates, and completing an initialization process.
8. An apparatus for utilizing a ArUco code assisted VIO initialization method, the apparatus comprising a processor, a memory, and a ArUco code assisted initialization method program stored on the memory and executable by the processor, wherein the ArUco code assisted initialization method program, when executed by the processor, implements the steps of the ArUco code assisted VIO initialization method of any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410641281.2A CN118209137B (en) | 2024-05-22 | 2024-05-22 | VIO initialization method, device and equipment assisted by ArUco codes |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410641281.2A CN118209137B (en) | 2024-05-22 | 2024-05-22 | VIO initialization method, device and equipment assisted by ArUco codes |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118209137A CN118209137A (en) | 2024-06-18 |
| CN118209137B true CN118209137B (en) | 2024-08-02 |
Family
ID=91450935
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410641281.2A Active CN118209137B (en) | 2024-05-22 | 2024-05-22 | VIO initialization method, device and equipment assisted by ArUco codes |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118209137B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114910069A (en) * | 2022-06-01 | 2022-08-16 | 深圳市边界智控科技有限公司 | Fusion positioning initialization system and method for unmanned aerial vehicle |
| CN115930959A (en) * | 2022-12-20 | 2023-04-07 | 广东汇天航空航天科技有限公司 | Vision initialization method and device and hovercar |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117333551A (en) * | 2023-08-17 | 2024-01-02 | 湖北星纪魅族科技有限公司 | Initialization method for monocular VIO system, electronic device and storage medium |
| CN117765070A (en) * | 2023-11-20 | 2024-03-26 | 东南大学 | Method for estimating traffic sign position and posture information in vision/inertial odometer |
-
2024
- 2024-05-22 CN CN202410641281.2A patent/CN118209137B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114910069A (en) * | 2022-06-01 | 2022-08-16 | 深圳市边界智控科技有限公司 | Fusion positioning initialization system and method for unmanned aerial vehicle |
| CN115930959A (en) * | 2022-12-20 | 2023-04-07 | 广东汇天航空航天科技有限公司 | Vision initialization method and device and hovercar |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118209137A (en) | 2024-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109991636B (en) | Map construction method and system based on GPS, IMU and binocular vision | |
| CN110009681B (en) | IMU (inertial measurement unit) assistance-based monocular vision odometer pose processing method | |
| CN111795686B (en) | Mobile robot positioning and mapping method | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN110345944A (en) | Merge the robot localization method of visual signature and IMU information | |
| CN111156984A (en) | Monocular vision inertia SLAM method oriented to dynamic scene | |
| CN103759716A (en) | Dynamic target position and attitude measurement method based on monocular vision at tail end of mechanical arm | |
| CN110411457B (en) | Positioning method, system, terminal and storage medium based on stroke perception and vision fusion | |
| CN112734841B (en) | Method for realizing positioning by using wheel type odometer-IMU and monocular camera | |
| CN112649016A (en) | Visual inertial odometer method based on point-line initialization | |
| CN112815939B (en) | Pose estimation method of mobile robot and computer readable storage medium | |
| CN114234967B (en) | Six-foot robot positioning method based on multi-sensor fusion | |
| CN110458885B (en) | Positioning system and mobile terminal based on stroke perception and vision fusion | |
| CN115371665B (en) | Mobile robot positioning method based on depth camera and inertial fusion | |
| CN111609868A (en) | Visual inertial odometer method based on improved optical flow method | |
| CN113503873B (en) | Visual positioning method for multi-sensor fusion | |
| CN108053445A (en) | The RGB-D camera motion methods of estimation of Fusion Features | |
| CN114485640A (en) | Monocular vision inertia synchronous positioning and mapping method and system based on point-line characteristics | |
| Horanyi et al. | Generalized pose estimation from line correspondences with known vertical direction | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| CN112179373A (en) | Measuring method of visual odometer and visual odometer | |
| CN108827287B (en) | Robust visual SLAM system in complex environment | |
| Ling et al. | RGB-D inertial odometry for indoor robot via keyframe-based nonlinear optimization | |
| CN116721166B (en) | Binocular camera and IMU rotation external parameter online calibration method, device and storage medium | |
| CN118209137B (en) | VIO initialization method, device and equipment assisted by ArUco codes |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |