Visual inertial navigation calibration method

Abstract

The invention relates to the technical field of inertial navigation calibration, in particular to a visual inertial navigation calibration method. The method comprises the following steps: acquiring corresponding visual motion image data of a target carrier in a road environment through a visual sensor, and synchronously acquiring corresponding target carrier inertial data; performing motion feature point analysis on the visual motion image data to obtain a visual image feature point set; determining feature point pixel displacement between adjacent frames of visual images through the visual image feature point set, and carrying out motion trail optical flow tracking and three-dimensional camera pose conversion calculation to obtain a visual pose conversion matrix between adjacent frames of images under a camera coordinate system; adjacent frame pose pre-integration and vision-inertial navigation external reference calibration are performed on target carrier inertial data, and a corresponding vision-inertial navigation external reference calibration matrix from a camera coordinate system to an inertial coordinate system is generated. According to the invention, high-precision calibration of the inertial measurement unit can be realized, so that the corresponding navigation precision and reliability of the inertial measurement unit in a complex environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation calibration, and in particular to a visual inertial navigation calibration method. Background Art

[0002] Visual-Inertial Calibration (VIC) is an important technology that integrates computer vision and inertial measurement unit (IMU) data. It is widely used in autonomous driving, robot navigation, augmented reality (AR) and other fields. This technology can improve the accuracy of positioning and attitude estimation by fusing the data of visual sensors and inertial sensors, especially in scenes where GPS signals are missing or the environment is complex. At present, common VIN calibration methods mainly rely on calibration plates, known scenes or motion constraints, and use optimization algorithms to solve the camera's intrinsic parameters, extrinsic parameters, and the position and posture relationship between IMU and camera. Traditional inertial navigation calibration methods mainly rely on high-precision turntable equipment. Through multi-position and multi-attitude tests on the turntable, the known turntable motion information is used to estimate the sensor error parameters. However, due to the large differences in the sensor characteristics of cameras and IMUs, the calibration of the two often requires a lot of manual adjustments and meticulous operations, resulting in increasing navigation errors, which seriously affects navigation accuracy. Summary of the invention

[0003] Based on this, it is necessary for the present invention to provide a visual inertial navigation calibration method to solve at least one of the above technical problems.

[0004] To achieve the above purpose, a visual inertial navigation calibration method includes the following steps: Step S1: obtaining visual motion image data corresponding to the target carrier in the road environment through a visual sensor, and synchronously collecting corresponding target carrier inertial data through an inertial measurement unit built into the target carrier; performing motion feature point analysis on the visual motion image data to obtain a visual image feature point set; Step S2: determining the pixel displacement of feature points between adjacent frames of visual images through the visual image feature point set, and performing motion trajectory optical flow tracking between corresponding adjacent frame feature points in the visual image feature point set based on the pixel displacement of feature points between adjacent frames of visual images, so as to generate a motion trajectory of feature points between adjacent frame images; Step S3: calculating the visual motion transformation matrix between adjacent frame images according to the motion trajectory of the feature points between adjacent frame images, and performing three-dimensional camera pose transformation calculation according to the visual motion transformation matrix to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system; Step S4: Pre-integrate the adjacent frame poses of the target carrier inertial data to obtain the inertial pose change matrix between adjacent frame moments in the inertial coordinate system; perform visual-inertial navigation extrinsic parameter calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate the corresponding visual-inertial navigation extrinsic parameter calibration matrix from the camera coordinate system to the inertial coordinate system.

[0005] Further, step S1 includes the following steps: Step S11: acquiring visual motion image data corresponding to the target carrier in the road environment by deploying a visual sensor on the target carrier; Step S12: Sending a synchronization pulse through a synchronization signal generator to synchronously collect corresponding target carrier inertial data using an inertial measurement unit built into the target carrier; Step S13: performing histogram equalization processing on each frame of the visual motion image data to obtain visual motion contrast balanced image data; Step S14: performing pixel blur analysis on each frame of the visual motion contrast balanced image data to obtain pixel blur corresponding to each frame of the visual motion image; performing pixel blur filtering on each frame of the visual motion contrast balanced image data based on the pixel blur corresponding to each frame of the visual motion image to filter and remove corresponding pixel high-frequency noise and pixel abnormal values ​​to obtain visual motion filtered image data; Step S15: Input the visual motion filtered image data into the pre-trained convolutional neural network model to extract the corresponding feature key points and their descriptors in each frame image, and perform non-maximum suppression on the corresponding feature key points and their descriptors in each frame image to obtain a visual image feature point set.

[0006] Furthermore, the non-maximum suppression of the corresponding feature key points and their descriptors in each frame of the image includes: The deep feature analysis is performed on the corresponding key points and their descriptors in each frame of the image, so as to analyze the local neighborhood structure corresponding to the key points in the visual motion image, and the texture direction distribution characteristics of the key points are characterized by calculating the gradient direction histogram of the pixels in the neighborhood of the key points. For the descriptors, their numerical distribution laws and dimensional correlations in different color spaces are analyzed to obtain the deep feature data set of the key points and descriptors. Based on the feature key point and descriptor deep characteristic data set, the feature key points and their descriptors corresponding to each frame of the image are preliminarily screened, and the feature key points and their descriptors in each frame of the image are associated in the time dimension considering the temporal continuity between frames, from which the position changes corresponding to the feature key points of adjacent frames and the similarities corresponding to the descriptors are analyzed, and the feature key points with abnormal position changes or the descriptors with descriptor similarity lower than the threshold are marked as objects to be excluded for screening and exclusion, so as to obtain a set of feature key points and descriptors after the preliminary screening; A multi-scale local extreme value region is defined for each point in the feature key points and descriptor set after the initial screening, so as to determine the corresponding local extreme value region according to the pixel texture condition in its neighborhood, and obtain the multi-scale local extreme value region; Based on the multi-scale local extreme value region, non-maximum suppression is performed on the feature key points and descriptor sets after the initial screening to obtain the visual image feature point set.

[0007] Furthermore, the multi-scale local extreme value region specifically sets a 1x1 scale local extreme value region in the complex texture region to capture detail features, and sets a 5x5 scale local extreme value region in the smooth texture region to consider overall features.

[0008] Further, step S2 includes the following steps: Step S21: performing adjacent frame feature point matching on the visual image feature point set to obtain adjacent frame image feature point matching pairs; Step S22: obtaining the motion direction and speed range corresponding to the target carrier, and performing adjacent frame feature displacement constraint analysis according to the motion direction and speed range corresponding to the target carrier, so as to analyze the displacement direction and size constraint corresponding to the feature point in the adjacent frames, and obtain the adjacent frame feature point displacement constraint; Step S23: calculating the pixel displacement of feature points between each pair of feature points in the feature point matching pairs of the adjacent frame images based on the displacement constraint of the feature points of the adjacent frames, so as to obtain the pixel displacement of feature points between the adjacent frame visual images; Step S24: Based on the pixel displacement of feature points between adjacent frame visual images, optical flow tracking of motion trajectories is performed between corresponding adjacent frame feature points in the visual image feature point set to generate feature point motion trajectories between adjacent frame images.

[0009] Further, step S24 includes the following steps: Step S241: taking the pixel displacement of feature points between adjacent frame visual images as a spatial vector to generate a pixel displacement vector corresponding to each feature point in the adjacent frame images; Step S242: performing directional component decomposition on the pixel displacement vector corresponding to each feature point in the adjacent frame images to obtain the pixel displacement components corresponding to each feature point in the adjacent frame images in the horizontal, vertical and diagonal directions; Step S243: inversely solving the displacement angle between each feature point based on the pixel displacement components corresponding to each feature point in the horizontal, vertical and diagonal directions in the adjacent frame images to obtain the angle between the displacement vectors of adjacent frames; Step S244: Based on the angle between the displacement vectors of adjacent frames and in combination with the pixel displacement of the feature points between the adjacent frame visual images, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the feature point motion trajectory between the adjacent frame images.

[0010] Further, step S244 includes the following steps: Perform displacement change analysis on the pixel displacement of feature points between adjacent frames of visual images in the time dimension to obtain the change value of the pixel displacement of feature points of adjacent frames corresponding to time; The displacement time derivative is calculated according to the change value of the pixel displacement of the feature point of the adjacent frames corresponding to time, so as to obtain the pixel displacement speed and pixel displacement acceleration corresponding to the feature point of the adjacent frames; Based on the angle between the displacement vectors of adjacent frames, the pixel displacement speed and pixel displacement acceleration corresponding to the feature points of adjacent frames, and combined with the pyramid hierarchical optical flow method, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the motion trajectory of the feature points between adjacent frame images.

[0011] Further, step S3 includes the following steps: Step S31: projecting the motion trajectory of the feature points between adjacent frame images into the three-dimensional feature space, and determining the three-dimensional coordinates corresponding to the feature points of the adjacent frame images in the three-dimensional feature space to obtain the three-dimensional coordinates of the feature points of each adjacent frame image; Step S32: reprojecting the three-dimensional coordinates of the feature points of each adjacent frame image into the image plane coordinate system to obtain the reprojected coordinates of the feature points of each adjacent frame image; Step S33: obtaining the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane, and performing visual error calculation according to the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane and the reprojected coordinates of each adjacent frame image feature point, so as to obtain the visual reprojection error vector corresponding to each adjacent frame image feature point; Step S34: performing visual motion transformation calculation on the three-dimensional coordinates of the feature points of each adjacent frame image based on the visual reprojection error vector corresponding to the feature points of each adjacent frame image, so as to obtain a visual motion transformation matrix between adjacent frame images; Step S35: performing three-dimensional camera posture transformation calculation according to the visual motion transformation matrix between adjacent frame images to obtain the visual posture transformation matrix between adjacent frame images in the camera coordinate system.

[0012] Furthermore, the three-dimensional coordinates of the feature points of each adjacent frame image described in step S31 are specifically: Get the camera intrinsic parameter matrix corresponding to the visual sensor ,in and The camera is and The focal length corresponding to the direction, and They are the point-by-point coordinates of the visual motion image; Let the adjacent frame image be frame With frame , and in frame Get the corresponding feature point set , while in frame Get the matching feature point set ,in Represents the total number of feature points in adjacent frame images, Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are represents the transpose symbol; Based on the camera intrinsic parameter matrix For Frame feature point set Or Frame feature point set Perform normalized coordinate transformation to obtain Frame Normalized Coordinates Or Frame Normalized Coordinates ; The first Frame Normalized Coordinates Or Frame Normalized Coordinates Perform three-dimensional feature space mapping to obtain the three-dimensional coordinates of feature points of each adjacent frame image , among which 3D coordinates of feature points in adjacent frames .

[0013] Furthermore, the reprojection coordinates of the feature points of each adjacent frame image described in step S32 are specifically: Assume that the camera is The external parameters corresponding to the frames are the rotation matrices and the translation vector , and transform it from the three-dimensional coordinate system to the camera coordinate system according to the corresponding external parameters. The corresponding coordinates in the frame camera coordinate system are ; It is projected onto the image plane according to the projection formula corresponding to the camera, and the projection formula is specifically: ; in, Indicates The feature points of the adjacent frame images are The reprojected coordinates on the frame image plane, represents the scale factor; By solving the above projection formula, we can get the corresponding reprojection coordinates. , , and collect all the reprojection coordinates to obtain the reprojection coordinates of the feature points of each adjacent frame image .

[0014] Furthermore, the visual reprojection error vector described in step S33 is specifically: ,in Represents the total number of feature points in adjacent frame images, represents the transpose symbol, Indicates The visual reprojection error value between feature points of adjacent frame images, Indicates The actual observed coordinates corresponding to the feature points of the adjacent frames on the image plane , Indicates Reprojection coordinates of feature points in adjacent frames .

[0015] Further, step S35 includes the following steps: Perform singular value decomposition on the visual motion transformation matrix between adjacent frame images to obtain the rotation matrix and translation vector between the feature points of adjacent frame images; The three-dimensional camera pose transformation is calculated based on the rotation matrix and translation vector between the feature points of adjacent frame images to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system.

[0016] Furthermore, the visual posture conversion matrix is ​​specifically: ; in, Indicates the camera coordinate system Frame and The visual pose transformation matrix between frame images, Represents the rotation matrix between feature points of adjacent frame images, Represents the translation vector between feature points of adjacent frame images.

[0017] Further, step S4 includes the following steps: Step S41: pre-integrate the adjacent frame poses of the target carrier inertial data to obtain an inertial pose change matrix between adjacent frame moments in the inertial coordinate system; Step S42: Perform visual-inertial navigation extrinsic calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate a corresponding visual-inertial navigation extrinsic calibration matrix from the camera coordinate system to the inertial coordinate system.

[0018] Furthermore, the inertial posture change matrix described in step S41 is specifically: ; in, In the inertial coordinate system, Frame and The inertial pose change matrix between frame images, Indicates Frame and The rotation pre-integration quantity between frame images, Indicates Frame and Position pre-integration quantity between frame images.

[0019] Beneficial effects of the present invention: Compared with the prior art, the visual inertial navigation calibration method proposed in the present invention has the beneficial effect of obtaining visual motion image data in a road environment through a visual sensor, and synchronously collecting inertial data through an inertial measurement unit (IMU) built into the carrier. The image data provided by the visual sensor contains static and dynamic information of the environment, while the IMU can provide dynamic information such as acceleration and angular velocity. By synchronously collecting these two types of data, the motion state of the target carrier in a complex dynamic environment can be effectively captured, and the understanding of the motion trajectory of the target carrier can be enhanced. The image data obtained by the visual sensor provides a necessary basis for subsequent motion feature point analysis. By processing the image sequence, stable and representative visual feature points are extracted. These feature points can help us better capture the motion changes between adjacent frames, and the acceleration and angular velocity information provided by the IMU data can help us estimate the motion state of the carrier and fill the blind spots of the visual sensor in a specific environment (for example, in dynamic motion or rapid rotation, the visual sensor will produce blur or distortion). By synchronously collecting these two types of data, the accuracy and robustness of the positioning and navigation system can be effectively improved. Secondly, the pixel displacement of feature points between adjacent frames is determined through the set of visual image feature points. This process involves the extraction and matching of visual image feature points. By analyzing the changes between feature points of adjacent image frames and calculating the pixel displacement, the motion pattern of the image can be understood. The motion trajectory of feature points in adjacent frames is tracked and inferred through the optical flow tracking algorithm, which can further accurately understand the motion trajectory of the target carrier. Through this process, the motion trajectory of the feature points, namely the optical flow, can be constructed, because the optical flow of the feature points can not only reflect the translational motion of the target carrier, but also indirectly reflect the rotation information of the carrier. Compared with other traditional methods, the optical flow method has higher time resolution and real-time performance, and is suitable for scenes with high-speed motion or large dynamic changes. Through precise feature point tracking, it can also effectively reduce the interference caused by image noise or blur, thereby improving the accuracy and stability of motion estimation. Then, the visual motion transformation matrix is ​​calculated through the motion trajectory of the feature points between adjacent frame images. This matrix can describe the geometric transformation relationship between adjacent frames, that is, the visual motion change from the previous frame image to the current frame image. Based on this transformation matrix, the three-dimensional camera pose conversion calculation can be further performed. Through this step, the camera motion between adjacent frames can be accurately estimated, and then the spatial motion trajectory of the target carrier can be derived. The visual motion transformation matrix reflects the position and direction changes in the three-dimensional space calculated from the visual data, so that the motion trajectory of the target carrier can be continuously tracked in the global coordinate system, thereby providing accurate visual information for subsequent visual inertial navigation fusion.Finally, by pre-integrating the pose of adjacent frames of the inertial data of the target carrier, the pre-integration technology of inertial data can help estimate the motion trajectory of the carrier through the data of accelerometers and gyroscopes without relying on external signals. Unlike the processing of visual data, inertial data is usually not affected by problems such as changes in ambient lighting and motion blur. Therefore, they can provide more stable motion estimation in some special cases. By combining the inertial pose change matrix with the visual pose transformation matrix, visual-inertial navigation extrinsic parameter calibration can be performed. Visual-inertial navigation extrinsic parameter calibration is to align the coordinate systems of the visual sensor and the inertial measurement unit so that the two can work in coordination with each other. Through this calibration, the advantages of visual data and inertial data can be used at the same time to improve the accuracy and reliability of motion estimation. The calibrated extrinsic parameter matrix can perform accurate coordinate transformation in practical applications, which can achieve more accurate positioning and navigation, thereby reducing the corresponding navigation error of the inertial measurement unit. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments thereof made with reference to the following drawings: Figure 1 Schematic diagram of the steps of the visual inertial navigation calibration method of the present invention; Figure 2 for Figure 1 Detailed step flow diagram of step S1; Figure 3 for Figure 1 Detailed step flow chart of step S2 in FIG. DETAILED DESCRIPTION

[0021] The technical method of the present invention is described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by technicians in this field without creative work are within the scope of protection of the present invention.

[0022] In addition, the accompanying drawings are only schematic illustrations of the present invention and are not necessarily drawn to scale. The same reference numerals in the figures represent the same or similar parts, and their repeated description will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities. The functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor methods and / or microcontroller methods.

[0023] It should be understood that, although the terms "first", "second", etc. may be used herein to describe various units, these units should not be limited by these terms. These terms are used only to distinguish one unit from another unit. For example, without departing from the scope of the exemplary embodiments, the first unit may be referred to as the second unit, and similarly the second unit may be referred to as the first unit. The term "and / or" used herein includes any and all combinations of one or more of the listed associated items.

[0024] To achieve this, please refer to Figures 1 to 3 The present invention provides a visual inertial navigation calibration method, the method comprising the following steps: Step S1: obtaining visual motion image data corresponding to the target carrier in the road environment through a visual sensor, and synchronously collecting corresponding target carrier inertial data through an inertial measurement unit built into the target carrier; performing motion feature point analysis on the visual motion image data to obtain a visual image feature point set; Step S2: determining the pixel displacement of feature points between adjacent frames of visual images through the visual image feature point set, and performing motion trajectory optical flow tracking between corresponding adjacent frame feature points in the visual image feature point set based on the pixel displacement of feature points between adjacent frames of visual images, so as to generate a motion trajectory of feature points between adjacent frame images; Step S3: calculating the visual motion transformation matrix between adjacent frame images according to the motion trajectory of the feature points between adjacent frame images, and performing three-dimensional camera pose transformation calculation according to the visual motion transformation matrix to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system; Step S4: Pre-integrate the adjacent frame poses of the target carrier inertial data to obtain the inertial pose change matrix between adjacent frame moments in the inertial coordinate system; perform visual-inertial navigation extrinsic parameter calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate the corresponding visual-inertial navigation extrinsic parameter calibration matrix from the camera coordinate system to the inertial coordinate system.

[0025] In the embodiment of the present invention, please refer to Figure 1 FIG. 1 is a flow chart of the steps of the visual inertial navigation calibration method of the present invention. In this example, the visual inertial navigation calibration method includes the following steps: Step S1: obtaining visual motion image data corresponding to the target carrier in the road environment through a visual sensor, and synchronously collecting corresponding target carrier inertial data through an inertial measurement unit built into the target carrier; performing motion feature point analysis on the visual motion image data to obtain a visual image feature point set; In an embodiment of the present invention, a high-resolution, high-frame-rate visual sensor, such as an industrial-grade camera, is firmly installed on the target carrier so that its lens can cover a certain range of road environment around the target carrier. When the target carrier is running on the road, the visual sensor continuously captures images at a speed of 30 frames per second, converts the captured images into digital signals and stores them, thereby acquiring visual motion image data. At the same time, an inertial measurement unit (IMU) built into the target carrier is connected to the visual sensor through a synchronization signal line, and a synchronization pulse signal is sent by a synchronization signal generator to enable the IMU and the visual sensor to work synchronously, and inertial data such as acceleration and angular velocity of the target carrier are collected and stored in real time. For the acquired visual motion image data, each frame of the image is processed based on a scale-invariant feature transform (SIFT) algorithm. The SIFT algorithm first detects extreme points in the image, then screens and locates these extreme points, determines them as feature points, and calculates the descriptor of each feature point, and finally obtains a visual image feature point set containing a large number of feature points and their descriptors.

[0026] Step S2: determining the pixel displacement of feature points between adjacent frames of visual images through the visual image feature point set, and performing motion trajectory optical flow tracking between corresponding adjacent frame feature points in the visual image feature point set based on the pixel displacement of feature points between adjacent frames of visual images, so as to generate a motion trajectory of feature points between adjacent frame images; In an embodiment of the present invention, by using a feature point matching algorithm, such as a matching method based on the descriptor Hamming distance, the feature points of two adjacent frames of images in the visual image feature point set are matched. For each feature point in the first frame image, the Hamming distance between its descriptor and the descriptors of all feature points in the second frame image is calculated, and the feature point with the smallest distance and less than a specific threshold (such as 60) is determined as the matching point, thereby obtaining feature point matching pairs between adjacent frames of visual images. According to the pixel coordinates of the feature points in these matching pairs in the two frames of images, the pixel displacement of the feature points is calculated. For example, if the coordinates of a feature point in the first frame are , the pixel coordinates of the matching feature points in the second frame image are , then the pixel displacement of the pair of feature points is ,The Lucas-Kanade optical flow algorithm is used to track the feature points between adjacent frame images with these pixel displacements as initial conditions. During the tracking process, a search window is defined with the position of the feature point in the current frame as the center. The best matching position is found in subsequent frames through iterative calculation. The positions of feature points in adjacent frames are connected in sequence, and finally the motion trajectory of feature points between adjacent frame images is generated.

[0027] Step S3: calculating the visual motion transformation matrix between adjacent frame images according to the motion trajectory of the feature points between adjacent frame images, and performing three-dimensional camera pose transformation calculation according to the visual motion transformation matrix to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system; In the embodiment of the present invention, the motion trajectory of the feature points between the adjacent frame images is projected into the three-dimensional feature space, and the three-dimensional coordinates corresponding to the feature points of the adjacent frame images are determined in the three-dimensional feature space. , and reproject the three-dimensional coordinates of the feature points of each adjacent frame image to the image plane coordinate system to obtain the reprojected coordinates of the feature points of each adjacent frame image , by obtaining the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane , and reproject it with the feature points of the adjacent frame images The error metric is calculated to calculate the visual reprojection error vector, which is specifically At the same time, by constructing the previously obtained Frame No. The visual reprojection error vector corresponding to the image feature points Construct the corresponding error function , and by using a nonlinear optimization algorithm such as the Levenberg-Marquardt algorithm to minimize the error function , during the optimization process, constantly adjust and The value of the error function gradually decreases, and the error function is updated through iteration. ,in is the error function E about and The Jacobian matrix of is the damping factor, It is a vector composed of the visual reprojection error vectors of all feature points. After multiple iterations, when the error function converges to the minimum value, the visual motion transformation matrix between adjacent frame images is obtained. Then, the visual motion transformation matrix between adjacent frame images is decomposed by singular value to obtain the rotation matrix between the feature points of adjacent frame images. ,in and represents an orthogonal matrix, Represents a diagonal matrix. Since the rotation matrix is ​​an orthogonal matrix, take , as the rotation matrix between the feature points of adjacent frame images, and the translation vector is directly obtained from the visual motion transformation matrix. The pose matrix of the camera of the i-th frame is calculated by conversion in the camera coordinate system: , then the final visual pose transformation matrix between adjacent frame images in the camera coordinate system is ,in, Indicates the camera coordinate system Frame and The visual pose transformation matrix between frame images, Represents the rotation matrix between feature points of adjacent frame images, Represents the translation vector between feature points of adjacent frame images.

[0028] Step S34: performing visual motion transformation calculation on the three-dimensional coordinates of the feature points of each adjacent frame image based on the visual reprojection error vector corresponding to the feature points of each adjacent frame image, so as to obtain a visual motion transformation matrix between adjacent frame images; Step S35: performing three-dimensional camera posture transformation calculation according to the visual motion transformation matrix between adjacent frame images to obtain the visual posture transformation matrix between adjacent frame images in the camera coordinate system.

[0029] Step S4: Pre-integrate the adjacent frame poses of the target carrier inertial data to obtain the inertial pose change matrix between adjacent frame moments in the inertial coordinate system; perform visual-inertial navigation extrinsic parameter calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate the corresponding visual-inertial navigation extrinsic parameter calibration matrix from the camera coordinate system to the inertial coordinate system.

[0030] In an embodiment of the present invention, the inertial data corresponding to the target carrier is obtained, which is provided by an inertial measurement unit (IMU) and includes the angular velocity between adjacent frame times. and linear acceleration , for the angular velocity , at each sampling point The measured value is , considering the gyroscope bias , the real angular velocity is , and for the linear acceleration , at each sampling point The measured value is , considering the zero bias of the accelerometer and the acceleration due to gravity , the true linear acceleration in the inertial coordinate system is ,in It is at the moment The rotation matrix from the carrier coordinate system to the inertial coordinate system is obtained by integrating the true angular velocity: And the position pre-integrated quantity , the specific calculation uses the integration method of quaternion or rotation matrix, for example, the median integration method: ,in yes The antisymmetric matrix of is the sampling time interval. By double integrating the linear acceleration, the position pre-integration between adjacent frame times can be obtained. , the corresponding calculation formula is , the rotation pre-integrated quantity and the position pre-integrated quantity are combined into the inertial posture change matrix between adjacent frame times in the inertial coordinate system At the same time, the transformation matrix from the camera coordinate system to the inertial coordinate system is found by combining the previously calculated visual pose transformation matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system. , so that the visual and inertial data can be aligned in the same coordinate system, according to the coordinate transformation relationship , expand the above equation, and get and For the rotation part, we have ; For the translation part, we have , in order to solve and , collect multiple sets of adjacent frames and Data, construct a nonlinear least squares problem, and define the error function , using a nonlinear optimization algorithm (such as the Levenberg-Marquardt algorithm) to minimize the error function , during the optimization process, constantly adjust and The value of the error function gradually decreases. When the error function converges to the minimum value, the obtained and Combined into the corresponding visual-inertial navigation extrinsic calibration matrix from the camera coordinate system to the inertial coordinate system .

[0031] Further, step S1 includes the following steps: Step S11: acquiring visual motion image data corresponding to the target carrier in the road environment by deploying a visual sensor on the target carrier; Step S12: Sending a synchronization pulse through a synchronization signal generator to synchronously collect corresponding target carrier inertial data using an inertial measurement unit built into the target carrier; Step S13: performing histogram equalization processing on each frame of the visual motion image data to obtain visual motion contrast balanced image data; Step S14: performing pixel blur analysis on each frame of the visual motion contrast balanced image data to obtain pixel blur corresponding to each frame of the visual motion image; performing pixel blur filtering on each frame of the visual motion contrast balanced image data based on the pixel blur corresponding to each frame of the visual motion image to filter and remove corresponding pixel high-frequency noise and pixel abnormal values ​​to obtain visual motion filtered image data; Step S15: Input the visual motion filtered image data into the pre-trained convolutional neural network model to extract the corresponding feature key points and their descriptors in each frame image, and perform non-maximum suppression on the corresponding feature key points and their descriptors in each frame image to obtain a visual image feature point set.

[0032] As an embodiment of the present invention, refer to Figure 2 As shown, Figure 1 Detailed step flow diagram of step S1 in FIG. 1 , in this embodiment, step S1 includes the following steps: Step S11: acquiring visual motion image data corresponding to the target carrier in the road environment by deploying a visual sensor on the target carrier; In the embodiment of the present invention, a visual sensor, such as a high-definition camera, is firmly installed at a suitable position on the target carrier. The visual sensor has the characteristics of high resolution and high frame rate, and can collect images at a speed of 30 frames per second. When the target carrier is driving in the road environment, the visual sensor continuously shoots the surrounding environment, and the viewing angle of the camera lens can cover a certain range of road scenes in front of and on both sides of the target carrier. The captured image data is transmitted to the data storage device in the form of digital signals, and finally forms the visual motion image data corresponding to the target carrier in the road environment.

[0033] Step S12: Sending a synchronization pulse through a synchronization signal generator to synchronously collect corresponding target carrier inertial data using an inertial measurement unit built into the target carrier; In an embodiment of the present invention, a high-precision pulse generating circuit is used in a synchronization signal generator to generate a stable and accurate synchronization pulse signal. When the target carrier starts to run, the synchronization signal generator is started to send synchronization pulses at fixed time intervals (such as every 10 milliseconds). The built-in inertial measurement unit (IMU) of the target carrier is connected to the synchronization signal generator. After receiving the synchronization pulse signal, the acceleration, angular velocity and other inertial data of the target carrier are immediately collected. The accelerometer and gyroscope inside the IMU can accurately measure the motion state of the target carrier in three-dimensional space. The collected inertial data is stored in a data storage module to ensure synchronization with the collection time of the visual motion image data, and finally obtain the corresponding target carrier inertial data.

[0034] Step S13: performing histogram equalization processing on each frame of the visual motion image data to obtain visual motion contrast balanced image data; In an embodiment of the present invention, a histogram equalization algorithm is used to process each frame of the visual motion image data. First, the grayscale histogram of the image is calculated to count the frequency of each grayscale level in the image. Then, the cumulative distribution function (CDF) is calculated based on the histogram, and the CDF is normalized. Next, the grayscale value of each pixel in the image is mapped based on the normalized CDF, and the original grayscale value is replaced with the mapped grayscale value. In this way, the contrast of the image is enhanced, so that the bright parts of the image are brighter and the dark parts are darker, and finally visual motion contrast balanced image data is obtained, which is convenient for subsequent analysis and processing of the image.

[0035] Step S14: performing pixel blur analysis on each frame of the visual motion contrast balanced image data to obtain pixel blur corresponding to each frame of the visual motion image; performing pixel blur filtering on each frame of the visual motion contrast balanced image data based on the pixel blur corresponding to each frame of the visual motion image to filter and remove corresponding pixel high-frequency noise and pixel abnormal values ​​to obtain visual motion filtered image data; In an embodiment of the present invention, a Laplace operator is used to perform pixel blur analysis on each frame of the visual motion contrast equalization image data to quantify the Laplace value of each pixel in the image. The larger the Laplace value, the more drastic the local change of the pixel, that is, the lower the blur. By performing a statistical analysis on the Laplace value of the entire frame of the image, the pixel blur corresponding to the frame of the image is obtained. Based on the pixel blur, a Gaussian filtering algorithm is used to perform pixel blur filtering on the image. The Gaussian filtering performs a convolution operation on the image through a two-dimensional Gaussian kernel function, and adjusts the standard deviation of the Gaussian kernel according to the pixel blur, so that the filtering process can adaptively remove pixel high-frequency noise and pixel outliers, and finally obtain visual motion filtered image data.

[0036] Step S15: Input the visual motion filtered image data into the pre-trained convolutional neural network model to extract the corresponding feature key points and their descriptors in each frame image, and perform non-maximum suppression on the corresponding feature key points and their descriptors in each frame image to obtain a visual image feature point set.

[0037] In an embodiment of the present invention, a pre-trained convolutional neural network model is used, which is a model trained on a large-scale image data set, such as a model based on a ResNet architecture, and the visual motion filter image data is input into the convolutional neural network model frame by frame. The model extracts features of the image through structures such as a convolution layer and a pooling layer. In the convolution layer, different convolution kernels are used to extract local features of the image. After multiple convolution and pooling operations, a high-level feature representation of the image is obtained. Representative feature key points are found from these high-level features, and corresponding descriptors are generated. The descriptors are used to describe the feature information of the feature key points. Then, non-maximum suppression is performed on the corresponding feature key points and their descriptors in each frame of the image. For each feature key point, its response value is compared with the response values ​​of other feature key points in the surrounding neighborhood. If the response value of the feature key point is not a local maximum, it is suppressed, and finally a set of visual image feature points is obtained.

[0038] Furthermore, the non-maximum suppression of the corresponding feature key points and their descriptors in each frame of the image includes: The deep feature analysis is performed on the corresponding key points and their descriptors in each frame of the image, so as to analyze the local neighborhood structure corresponding to the key points in the visual motion image, and the texture direction distribution characteristics of the key points are characterized by calculating the gradient direction histogram of the pixels in the neighborhood of the key points. For the descriptors, their numerical distribution laws and dimensional correlations in different color spaces are analyzed to obtain the deep feature data set of the key points and descriptors. In an embodiment of the present invention, a local neighborhood of a fixed size, such as a 16×16 pixel area, is defined with the feature key point in each frame of the image as the center. In this local neighborhood, the gradient direction and amplitude of each pixel are calculated, the gradient direction is divided into several intervals, and the sum of the gradient amplitudes in each interval is counted, so as to obtain the gradient direction histogram of the pixels in the neighborhood of the feature key point, so as to depict its texture direction distribution characteristics. For the descriptor, it is converted into different color spaces such as RGB and HSV, and the numerical distribution of the descriptor in different color spaces is counted, such as the mean, variance, etc. At the same time, the correlation coefficients between the dimensions of the descriptor are calculated, such as the Pearson correlation coefficient, and finally these information are integrated to form a dataset of feature key points and descriptor depth characteristics.

[0039] Preferably, based on the feature key point and descriptor depth characteristic data set, the feature key points and their descriptors corresponding to each frame of the image are preliminarily screened, so as to consider the temporal continuity between frames and associate the feature key points and their descriptors in each frame of the image in the time dimension, thereby analyzing the position changes corresponding to the feature key points of adjacent frames and the similarities corresponding to the descriptors, and marking the feature key points with abnormal position changes or the descriptors with descriptor similarity lower than a threshold as objects to be excluded for screening and exclusion, so as to obtain a set of feature key points and descriptors after the preliminary screening; In an embodiment of the present invention, feature key points and their descriptors in adjacent frame images are associated based on a dataset of feature key points and descriptor depth characteristics. For feature key points in adjacent frames, their position offsets on the image plane are calculated, and a reasonable position change threshold, such as 20 pixels, is set. If the position change of a feature key point between adjacent frames exceeds the threshold, it is marked as an object to be excluded. For descriptors, the cosine similarity algorithm is used to calculate the similarity between descriptors of adjacent frames, and the similarity threshold is set to 0.7. If the descriptor similarity is lower than the threshold, the corresponding descriptor is marked as an object to be excluded. Finally, all marked objects to be excluded are removed from the original set of feature key points and descriptors, and finally a set of feature key points and descriptors after preliminary screening is obtained.

[0040] Preferably, a multi-scale local extreme value region is defined for each point in the feature key points and descriptor set after the initial screening, so as to determine the corresponding local extreme value region according to the pixel texture status in its neighborhood, and obtain the multi-scale local extreme value region; In an embodiment of the present invention, neighborhoods of different scales, such as 1x1, 5x5, etc., are constructed for each point in the feature key points and descriptor set after the initial screening. In the neighborhood of each scale, the texture condition of the pixel is analyzed, for example, the complexity corresponding to the pixel texture is calculated. If the complexity corresponding to a point is greater than a threshold, it is determined to be a texture complex area, and a 1x1 scale local extreme value area is set in the area to capture detail features, and a 5x5 scale local extreme value area is set in the texture smooth area (that is, when it is less than or equal to the threshold) to consider the overall features. Such operations are performed on each point at different scales, and finally a multi-scale local extreme value area corresponding to each point is obtained.

[0041] Preferably, non-maximum suppression is performed on the feature key points and descriptor sets after the initial screening based on the multi-scale local extreme value regions to obtain a visual image feature point set.

[0042] In an embodiment of the present invention, a non-maximum suppression operation is performed on each point in the set of feature key points and descriptors after initial screening in its multi-scale local extreme value region. In each local extreme value region, the response value (such as feature intensity) of the point is compared with the response values ​​of other points in the neighborhood. If the response value of the point is not a local maximum, it is suppressed, that is, removed from the set. By performing such comparison and screening on all points in their multi-scale local extreme value regions, a set of feature key points and descriptors that are retained is finally obtained. This set is the visual image feature point set.

[0043] Furthermore, the multi-scale local extreme value region specifically sets a 1x1 scale local extreme value region in the complex texture region to capture detail features, and sets a 5x5 scale local extreme value region in the smooth texture region to consider overall features.

[0044] Further, step S2 includes the following steps: Step S21: performing adjacent frame feature point matching on the visual image feature point set to obtain adjacent frame image feature point matching pairs; Step S22: obtaining the motion direction and speed range corresponding to the target carrier, and performing adjacent frame feature displacement constraint analysis according to the motion direction and speed range corresponding to the target carrier, so as to analyze the displacement direction and size constraint corresponding to the feature point in the adjacent frames, and obtain the adjacent frame feature point displacement constraint; Step S23: calculating the pixel displacement of feature points between each pair of feature points in the feature point matching pairs of the adjacent frame images based on the displacement constraint of the feature points of the adjacent frames, so as to obtain the pixel displacement of feature points between the adjacent frame visual images; Step S24: Based on the pixel displacement of feature points between adjacent frame visual images, optical flow tracking of motion trajectories is performed between corresponding adjacent frame feature points in the visual image feature point set to generate feature point motion trajectories between adjacent frame images.

[0045] As an embodiment of the present invention, refer to Figure 3 As shown, Figure 1 Detailed step flow diagram of step S2 in FIG. 1 , in this embodiment, step S2 includes the following steps: Step S21: performing adjacent frame feature point matching on the visual image feature point set to obtain adjacent frame image feature point matching pairs; In an embodiment of the present invention, by adopting a feature descriptor matching algorithm, such as a matching algorithm based on the Hamming distance, adjacent frame feature point matching is performed on a visual image feature point set. For feature points in two adjacent frame images, their descriptors are respectively extracted, and the Hamming distance between the descriptor of each feature point in the first frame image and the descriptors of all feature points in the second frame image is calculated. If a feature point has a unique matching point in the second frame image with the minimum Hamming distance and less than a set threshold (such as 50), the two feature points are determined as a matching pair. Such matching operation is performed on all adjacent frame images in the visual image feature point set, and finally a set of adjacent frame image feature point matching pairs is obtained.

[0046] Step S22: obtaining the motion direction and speed range corresponding to the target carrier, and performing adjacent frame feature displacement constraint analysis according to the motion direction and speed range corresponding to the target carrier, so as to analyze the displacement direction and size constraint corresponding to the feature point in the adjacent frames, and obtain the adjacent frame feature point displacement constraint; In an embodiment of the present invention, the movement direction and speed range of the target carrier are obtained through the built-in positioning system and speed sensor of the target carrier. Assuming that the movement direction of the target carrier is forward and the speed range is 10-30 m / s, the possible displacement direction and size range of the feature points in adjacent frames are calculated according to the installation position and parameters of the camera and the motion information of the target carrier. For example, if the camera is installed in front of the target carrier and the target carrier moves forward, then the displacement direction of the feature points in adjacent frames is most likely forward, and the displacement size is related to the speed of the target carrier. By establishing a mathematical model and combining the internal and external parameters of the camera and the motion information of the target carrier, the corresponding displacement direction and size constraints of the feature points in adjacent frames are obtained, and finally the displacement constraints of the feature points in adjacent frames are formed.

[0047] Step S23: calculating the pixel displacement of feature points between each pair of feature points in the feature point matching pairs of the adjacent frame images based on the displacement constraint of the feature points of the adjacent frames, so as to obtain the pixel displacement of feature points between the adjacent frame visual images; In the embodiment of the present invention, for each pair of feature points in the adjacent frame image feature point matching pair, first check whether its displacement meets the adjacent frame feature point displacement constraint. If it meets, then calculate the pixel coordinate difference of the pair of feature points on the image plane. Assume that the pixel coordinate of a feature point in the first frame image is , the pixel coordinates of the matching feature points in the second frame image are , then the pixel displacement of the pair of feature points is , such calculation is performed on all matching pairs of feature points of adjacent frame images. If the displacement of a pair of feature points does not meet the displacement constraint, it will be excluded, and finally a set of feature point pixel displacements that meet the constraint conditions between adjacent frame visual images is obtained.

[0048] Step S24: Based on the pixel displacement of feature points between adjacent frame visual images, optical flow tracking of motion trajectories is performed between corresponding adjacent frame feature points in the visual image feature point set to generate feature point motion trajectories between adjacent frame images.

[0049] In an embodiment of the present invention, the Lucas-Kanade optical flow algorithm is used to track the motion trajectory optical flow between the corresponding adjacent frame feature points in the visual image feature point set. The algorithm is based on the assumption that the image grayscale value remains unchanged in a short period of time, and estimates the optical flow (i.e., the motion speed) of the feature point by solving a least squares problem. According to the pixel displacement of the feature point between the adjacent frames of visual images, it is input into the Lucas-Kanade optical flow algorithm as an initial estimate. For each feature point, a search window is defined in the adjacent frame image with its previous frame position as the center. The precise position of the feature point in the current frame is found through iterative calculation, and the positions of the feature points in the adjacent frames are connected in sequence to generate the motion trajectory of the feature points between the adjacent frame images. Such an optical flow tracking operation is performed on all feature points in the visual image feature point set, and finally a complete motion trajectory of the feature points between the adjacent frame images is obtained.

[0050] Further, step S24 includes the following steps: Step S241: taking the pixel displacement of feature points between adjacent frame visual images as a spatial vector to generate a pixel displacement vector corresponding to each feature point in the adjacent frame images; In the embodiment of the present invention, by shifting the pixel displacement of the feature points between the adjacent frames of visual images, It is regarded as a two-dimensional space vector, that is, a pixel displacement vector corresponding to the feature point is generated, and this operation is performed on each feature point in the adjacent frame image. For example, if there are 100 feature points in the adjacent frame image, the pixel displacement of these 100 feature points is calculated and converted into a vector form, and finally the pixel displacement vector corresponding to each feature point in the adjacent frame image is generated.

[0051] Step S242: performing directional component decomposition on the pixel displacement vector corresponding to each feature point in the adjacent frame images to obtain the pixel displacement components corresponding to each feature point in the adjacent frame images in the horizontal, vertical and diagonal directions; In the embodiment of the present invention, the pixel displacement vector corresponding to each feature point in the adjacent frame images is processed by a vector decomposition method, and the horizontal direction (x-axis), the vertical direction (y-axis) and the diagonal direction (45° and 135° to the x-axis) are used as reference directions. , the pixel displacement component in the horizontal direction is , the vertical pixel displacement component is For the diagonal direction, according to the trigonometric function relationship, the displacement component in the 45° direction is calculated as , the displacement component in the 135° direction is , the pixel displacement vector of each feature point in adjacent frame images is subjected to such directional component decomposition operation, and finally the pixel displacement components corresponding to each feature point in different directions are obtained.

[0052] Step S243: inversely solving the displacement angle between each feature point based on the pixel displacement components corresponding to each feature point in the horizontal, vertical and diagonal directions in the adjacent frame images to obtain the angle between the displacement vectors of adjacent frames; In the embodiment of the present invention, for any two feature points in adjacent frame images, the angle between their displacement vectors is calculated using the vector dot product formula according to their corresponding pixel displacement components in the horizontal, vertical and diagonal directions. Suppose the displacement vectors of the two feature points are and , whose components in the horizontal, vertical and diagonal directions are and , and according to the vector dot product formula (in is the angle between the two vectors), then, by first calculating , and then calculate and , and finally through The angle between the two vectors is inversely solved, and this calculation is performed between every two feature points in adjacent frame images, and finally the angle between the displacement vectors of adjacent frames is obtained.

[0053] Step S244: Based on the angle between the displacement vectors of adjacent frames and in combination with the pixel displacement of the feature points between the adjacent frame visual images, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the feature point motion trajectory between the adjacent frame images.

[0054] In an embodiment of the present invention, by adopting the Lucas-Kanade optical flow algorithm, combined with the angle between adjacent frame displacement vectors and the pixel displacement of feature points between adjacent frame visual images, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set. For each feature point, its pixel displacement in the adjacent frame is used as the initial estimate, and the angle information between the displacement vectors of adjacent frames is considered to optimize the optical flow calculation. In the Lucas-Kanade optical flow algorithm, a search window is defined in the adjacent frame image with the previous frame position of the feature point as the center, the search direction and range are adjusted using the pixel displacement and angle information, and the precise position of the feature point in the current frame is found by iterative calculation. The positions of the feature points in the adjacent frames are connected in sequence to generate the motion trajectory of the feature points between the adjacent frame images. Such an optical flow tracking operation is performed on all feature points in the visual image feature point set, and finally a complete motion trajectory of the feature points between the adjacent frame images is obtained.

[0055] Further, step S244 includes the following steps: Perform displacement change analysis on the pixel displacement of feature points between adjacent frames of visual images in the time dimension to obtain the change value of the pixel displacement of feature points of adjacent frames corresponding to time; In the embodiment of the present invention, by knowing the calculated pixel displacement of feature points between adjacent frames of visual images, and each frame of image has a corresponding acquisition time, for a feature point in adjacent frames, if the pixel coordinates in the nth frame of image are , the pixel coordinates in the n+1th frame image are , then the pixel displacement of the feature point between these two frames is , let the acquisition time of the nth frame image be , the acquisition time of the n+1th frame image is , time interval , compare the pixel displacement of the feature point in multiple sets of adjacent frame images (e.g., n to n+1, n+1 to n+2, etc.) to calculate the difference in the pixel displacement of the feature point in adjacent frames, such as , thereby obtaining the change value of the pixel displacement of the feature point corresponding to time, and performing such analysis operation on each feature point in the adjacent frame image, and finally obtaining the change value of the pixel displacement of the feature point of the adjacent frame corresponding to time.

[0056] Preferably, the displacement time derivative is calculated according to the change value of the pixel displacement of the feature point of the adjacent frames corresponding to time, so as to obtain the pixel displacement speed and pixel displacement acceleration corresponding to the feature point of the adjacent frames; In the embodiment of the present invention, the pixel displacement speed and pixel displacement acceleration are calculated by using the definition of derivatives for the change values ​​of the pixel displacement of feature points of adjacent frames over time. For a feature point in the horizontal direction, the change values ​​of the pixel displacement in the horizontal direction between adjacent frames are known. and time interval , then the horizontal pixel displacement speed Similarly, the pixel displacement speed in the vertical direction can be obtained , for the calculation of pixel displacement acceleration, the velocity is derived again, that is, the pixel displacement acceleration in the horizontal direction ,in is the change value of the horizontal pixel displacement speed in adjacent time intervals. Similarly, the vertical pixel displacement acceleration can be obtained. , such displacement time derivative calculation is performed on each feature point in adjacent frame images in the horizontal and vertical directions, and finally the pixel displacement speed and pixel displacement acceleration corresponding to each feature point are obtained.

[0057] Preferably, based on the angle between adjacent frame displacement vectors and the pixel displacement speed and pixel displacement acceleration corresponding to the adjacent frame feature points and combined with the pyramid hierarchical optical flow method, motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the feature point motion trajectory between adjacent frame images.

[0058] In an embodiment of the present invention, by adopting a pyramid hierarchical optical flow method, an image pyramid of adjacent frame images is first constructed, and the original image is continuously downsampled to obtain image layers of different resolutions. For each feature point in the visual image feature point set, on the bottom layer image (high-resolution image), the angle between the displacement vectors of adjacent frames, the pixel displacement speed, and the pixel displacement acceleration information are combined, and these information are used as initial conditions for optical flow calculation. On the basis of the Lucas-Kanade optical flow algorithm, the size and moving direction of the search window are adjusted according to the pixel displacement speed and acceleration, and the angle information between the displacement vectors of adjacent frames is used to optimize the angle of the search direction. The optical flow is calculated starting from the bottom layer image to obtain a preliminary displacement estimate of the feature point on the layer image, and then the estimated value is transferred to the upper layer (slightly lower resolution) image, and the optical flow calculation is continued and the displacement estimate is optimized, and so on until the top layer image. Finally, the displacement estimates of the feature points obtained on each layer of the image are combined to obtain an accurate motion trajectory of the feature point between adjacent frame images. Such an optical flow tracking operation is performed on all feature points in the visual image feature point set, thereby generating a complete motion trajectory of the feature points between adjacent frame images.

[0059] Further, step S3 includes the following steps: Step S31: projecting the motion trajectory of the feature points between adjacent frame images into the three-dimensional feature space, and determining the three-dimensional coordinates corresponding to the feature points of the adjacent frame images in the three-dimensional feature space to obtain the three-dimensional coordinates of the feature points of each adjacent frame image; In the embodiment of the present invention, by obtaining the camera intrinsic parameter matrix corresponding to the visual sensor ,in and The camera is and The focal length corresponding to the direction, and are the point-by-point coordinates of the visual motion image, and by assuming that the adjacent frame images are frame With frame , and in frame Get the corresponding feature point set , while in frame Get the matching feature point set ,in Represents the total number of feature points in adjacent frame images, Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are Represents the transposed symbol; based on the camera intrinsic parameter matrix For Frame feature point set Or Frame feature point set Perform normalized coordinate transformation to transform pixel coordinates into normalized plane coordinates to obtain Frame Normalized Coordinates Or Frame Normalized Coordinates ; Frame Normalized Coordinates Or Frame Normalized Coordinates Perform three-dimensional feature space mapping to obtain the three-dimensional coordinates of feature points of each adjacent frame image , among which 3D coordinates of feature points in adjacent frames .

[0060] Step S32: reprojecting the three-dimensional coordinates of the feature points of each adjacent frame image into the image plane coordinate system to obtain the reprojected coordinates of the feature points of each adjacent frame image; In the embodiment of the present invention, by assuming that the camera is The external parameters corresponding to the frames are the rotation matrices and the translation vector , and transform it from the three-dimensional coordinate system to the camera coordinate system according to the corresponding external parameters. The corresponding coordinates in the frame camera coordinate system are , project it onto the image plane according to the projection formula corresponding to the camera, the projection formula is specifically: ,in, Indicates The feature points of the adjacent frame images are The reprojected coordinates on the frame image plane, Represents the scale factor, and the corresponding reprojection coordinates are obtained by solving the above projection formula , , and collect all the reprojection coordinates to obtain the reprojection coordinates of the feature points of each adjacent frame image .

[0061] Step S33: obtaining the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane, and performing visual error calculation according to the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane and the reprojected coordinates of each adjacent frame image feature point, so as to obtain the visual reprojection error vector corresponding to each adjacent frame image feature point; In the embodiment of the present invention, the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane are obtained. , and reproject it with the feature points of the adjacent frame images The error metric is calculated to calculate the visual reprojection error vector, which is specifically ,in Represents the total number of feature points in adjacent frame images, represents the transpose symbol, Indicates The visual reprojection error value between feature points of adjacent frame images, Indicates The actual observed coordinates corresponding to the feature points of the adjacent frames on the image plane , Indicates Reprojection coordinates of feature points in adjacent frames .

[0062] Step S34: performing visual motion transformation calculation on the three-dimensional coordinates of the feature points of each adjacent frame image based on the visual reprojection error vector corresponding to the feature points of each adjacent frame image, so as to obtain a visual motion transformation matrix between adjacent frame images; In the embodiment of the present invention, by constructing the first Frame No. The visual reprojection error vector corresponding to the image feature points Construct the corresponding error function ,in is the rotation matrix between adjacent frame images, is the translation vector, and the error function is minimized by using a nonlinear optimization algorithm such as the Levenberg-Marquardt algorithm , during the optimization process, constantly adjust and The value of the error function gradually decreases. Let the initial rotation matrix and translation vector , through iterative update ,in is the error function E about and The Jacobian matrix of is the damping factor, It is a vector composed of the visual reprojection error vectors of all feature points. After multiple iterations, when the error function converges to the minimum value, the visual motion transformation matrix between adjacent frame images is finally obtained. .

[0063] Step S35: performing three-dimensional camera posture transformation calculation according to the visual motion transformation matrix between adjacent frame images to obtain the visual posture transformation matrix between adjacent frame images in the camera coordinate system.

[0064] In the embodiment of the present invention, the visual motion transformation matrix between adjacent frame images is subjected to singular value decomposition to obtain the rotation matrix between the feature points of adjacent frame images. ,in and represents an orthogonal matrix, Represents a diagonal matrix. Since the rotation matrix is ​​an orthogonal matrix, take , as the rotation matrix between the feature points of adjacent frame images, and the translation vector is directly obtained from the visual motion transformation matrix. The pose matrix of the camera of the i-th frame is calculated by conversion in the camera coordinate system: , then the final visual pose transformation matrix between adjacent frame images in the camera coordinate system is ,in, Indicates the camera coordinate system Frame and The visual pose transformation matrix between frame images, Represents the rotation matrix between feature points of adjacent frame images, Represents the translation vector between feature points of adjacent frame images.

[0065] Further, step S4 includes the following steps: Step S41: pre-integrate the adjacent frame poses of the target carrier inertial data to obtain an inertial pose change matrix between adjacent frame moments in the inertial coordinate system; In an embodiment of the present invention, the inertial data corresponding to the target carrier is obtained, which is provided by an inertial measurement unit (IMU) and includes the angular velocity between adjacent frame times. and linear acceleration , assuming that the adjacent frame times are and , in this time interval Inertial data is discretized and the sampling frequency is set to , then within this time interval there is sampling points, for the angular velocity , at each sampling point The measured value is , considering the gyroscope bias , the real angular velocity is , and for the linear acceleration , at each sampling point The measured value is , considering the zero bias of the accelerometer and the acceleration due to gravity , the true linear acceleration in the inertial coordinate system is ,in It is at the moment The rotation matrix from the carrier coordinate system to the inertial coordinate system is obtained by integrating the true angular velocity: , the specific calculation uses the integration method of quaternion or rotation matrix, for example, the median integration method: ,in yes The antisymmetric matrix of is the sampling time interval. By double integrating the linear acceleration, the position pre-integration between adjacent frame times can be obtained. and velocity pre-integrated quantity , taking the position pre-integrated quantity as an example, the corresponding calculation formula is , the rotation pre-integrated quantity and the position pre-integrated quantity are combined into the inertial posture change matrix between adjacent frame times in the inertial coordinate system , and finally the inertial posture change matrix between adjacent frame times in the inertial coordinate system is obtained.

[0066] Step S42: Perform visual-inertial navigation extrinsic calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate a corresponding visual-inertial navigation extrinsic calibration matrix from the camera coordinate system to the inertial coordinate system.

[0067] In the embodiment of the present invention, the visual pose transformation matrix between adjacent frame images in the camera coordinate system obtained previously is And the inertial pose change matrix between adjacent frame times in the inertial coordinate system Find the transformation matrix from the camera coordinate system to the inertial coordinate system in , so that the visual and inertial data can be aligned in the same coordinate system, according to the coordinate transformation relationship , expand the above equation, and get and For the rotation part, we have ; For the translation part, we have , in order to solve and , collect multiple sets of adjacent frames and Data, construct a nonlinear least squares problem, and define the error function , using a nonlinear optimization algorithm (such as the Levenberg-Marquardt algorithm) to minimize the error function , during the optimization process, constantly adjust and The value of the error function gradually decreases. When the error function converges to the minimum value, the obtained and Combined into the corresponding visual-inertial navigation extrinsic calibration matrix from the camera coordinate system to the inertial coordinate system .

[0068] Therefore, the embodiments should be regarded as illustrative and non-restrictive from all points, and the scope of the present invention is limited by the appended claims rather than the above description, and it is therefore intended that all changes falling within the meaning and range of equivalent elements of the application documents are included in the present invention.

[0069] The above description is only a specific embodiment of the present invention, so that those skilled in the art can understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but should conform to the widest scope consistent with the principles and novel features invented herein.

Claims

1. A visual inertial navigation calibration method, characterized in that: The following steps are involved: Step S1: obtaining visual motion image data corresponding to the target carrier in the road environment through a visual sensor, and synchronously collecting corresponding target carrier inertial data through an inertial measurement unit built into the target carrier; performing motion feature point analysis on the visual motion image data to obtain a visual image feature point set; Step S2: determining the pixel displacement of feature points between adjacent frames of visual images through the visual image feature point set, and performing motion trajectory optical flow tracking between corresponding adjacent frame feature points in the visual image feature point set based on the pixel displacement of feature points between adjacent frames of visual images, so as to generate a motion trajectory of feature points between adjacent frame images; Step S3: calculating the visual motion transformation matrix between adjacent frame images according to the motion trajectory of the feature points between adjacent frame images, and performing three-dimensional camera pose transformation calculation according to the visual motion transformation matrix to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system; Step S4: Pre-integrate the adjacent frame poses of the target carrier inertial data to obtain the inertial pose change matrix between adjacent frame moments in the inertial coordinate system; perform visual-inertial navigation extrinsic parameter calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate the corresponding visual-inertial navigation extrinsic parameter calibration matrix from the camera coordinate system to the inertial coordinate system.

2. The visual inertial navigation calibration method according to claim 1, characterized in that: Step S1 includes the following steps: Step S11: acquiring visual motion image data corresponding to the target carrier in the road environment by deploying a visual sensor on the target carrier; Step S12: Sending a synchronization pulse through a synchronization signal generator to synchronously collect corresponding target carrier inertial data using an inertial measurement unit built into the target carrier; Step S13: performing histogram equalization processing on each frame of the visual motion image data to obtain visual motion contrast balanced image data; Step S14: performing pixel blur analysis on each frame of the visual motion contrast balanced image data to obtain pixel blur corresponding to each frame of the visual motion image; performing pixel blur filtering on each frame of the visual motion contrast balanced image data based on the pixel blur corresponding to each frame of the visual motion image to filter and remove corresponding pixel high-frequency noise and pixel abnormal values ​​to obtain visual motion filtered image data; Step S15: Input the visual motion filtered image data into the pre-trained convolutional neural network model to extract the corresponding feature key points and their descriptors in each frame image, and perform non-maximum suppression on the corresponding feature key points and their descriptors in each frame image to obtain a visual image feature point set.

3. The visual inertial navigation calibration method according to claim 2, characterized in that: The non-maximum suppression of the corresponding feature key points and their descriptors in each frame of image comprises: The deep feature analysis is performed on the corresponding key points and their descriptors in each frame of the image, so as to analyze the local neighborhood structure corresponding to the key points in the visual motion image, and the texture direction distribution characteristics of the key points are characterized by calculating the gradient direction histogram of the pixels in the neighborhood of the key points. For the descriptors, their numerical distribution laws and dimensional correlations in different color spaces are analyzed to obtain the deep feature data set of the key points and descriptors. Based on the feature key point and descriptor deep characteristic data set, the feature key points and their descriptors corresponding to each frame of the image are preliminarily screened, and the feature key points and their descriptors in each frame of the image are associated in the time dimension considering the temporal continuity between frames, from which the position changes corresponding to the feature key points of adjacent frames and the similarities corresponding to the descriptors are analyzed, and the feature key points with abnormal position changes or the descriptors with descriptor similarity lower than the threshold are marked as objects to be excluded for screening and exclusion, so as to obtain a set of feature key points and descriptors after the preliminary screening; A multi-scale local extreme value region is defined for each point in the feature key points and descriptor set after the initial screening, so as to determine the corresponding local extreme value region according to the pixel texture condition in its neighborhood, and obtain the multi-scale local extreme value region; Based on the multi-scale local extreme value region, non-maximum suppression is performed on the feature key points and descriptor sets after the initial screening to obtain the visual image feature point set.

4. The visual inertial navigation calibration method according to claim 3, characterized in that: The multi-scale local extreme value region specifically sets a 1x1 scale local extreme value region in a complex texture region to capture detail features, and sets a 5x5 scale local extreme value region in a smooth texture region to consider overall features.

5. The visual inertial navigation calibration method according to claim 1, characterized in that: Step S2 includes the following steps: Step S21: performing adjacent frame feature point matching on the visual image feature point set to obtain adjacent frame image feature point matching pairs; Step S22: obtaining the motion direction and speed range corresponding to the target carrier, and performing adjacent frame feature displacement constraint analysis according to the motion direction and speed range corresponding to the target carrier, so as to analyze the displacement direction and size constraint corresponding to the feature point in the adjacent frames, and obtain the adjacent frame feature point displacement constraint; Step S23: calculating the pixel displacement of feature points between each pair of feature points in the feature point matching pairs of the adjacent frame images based on the displacement constraint of the feature points of the adjacent frames, so as to obtain the pixel displacement of feature points between the adjacent frame visual images; Step S24: Based on the pixel displacement of feature points between adjacent frame visual images, optical flow tracking of motion trajectories is performed between corresponding adjacent frame feature points in the visual image feature point set to generate feature point motion trajectories between adjacent frame images.

6. The visual inertial navigation calibration method according to claim 5, characterized in that: Step S24 includes the following steps: Step S241: taking the pixel displacement of feature points between adjacent frame visual images as a spatial vector to generate a pixel displacement vector corresponding to each feature point in the adjacent frame images; Step S242: performing directional component decomposition on the pixel displacement vector corresponding to each feature point in the adjacent frame images to obtain the pixel displacement components corresponding to each feature point in the adjacent frame images in the horizontal, vertical and diagonal directions; Step S243: inversely solving the displacement angle between each feature point based on the pixel displacement components corresponding to each feature point in the horizontal, vertical and diagonal directions in the adjacent frame images to obtain the angle between the displacement vectors of adjacent frames; Step S244: Based on the angle between the displacement vectors of adjacent frames and in combination with the pixel displacement of the feature points between the adjacent frame visual images, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the feature point motion trajectory between the adjacent frame images.

7. The visual inertial navigation calibration method according to claim 6, characterized in that: Step S244 includes the following steps: Perform displacement change analysis on the pixel displacement of feature points between adjacent frames of visual images in the time dimension to obtain the change value of the pixel displacement of feature points of adjacent frames corresponding to time; The displacement time derivative is calculated according to the change value of the pixel displacement of the feature point of the adjacent frames corresponding to time, so as to obtain the pixel displacement speed and pixel displacement acceleration corresponding to the feature point of the adjacent frames; Based on the angle between the displacement vectors of adjacent frames, the pixel displacement speed and pixel displacement acceleration corresponding to the feature points of adjacent frames, and combined with the pyramid hierarchical optical flow method, the motion trajectory optical flow tracking is performed between the corresponding adjacent frame feature points in the visual image feature point set to generate the motion trajectory of the feature points between adjacent frame images.

8. The visual inertial navigation calibration method according to claim 1, characterized in that: Step S3 includes the following steps: Step S31: projecting the motion trajectory of the feature points between adjacent frame images into the three-dimensional feature space, and determining the three-dimensional coordinates corresponding to the feature points of the adjacent frame images in the three-dimensional feature space to obtain the three-dimensional coordinates of the feature points of each adjacent frame image; Step S32: reprojecting the three-dimensional coordinates of the feature points of each adjacent frame image into the image plane coordinate system to obtain the reprojected coordinates of the feature points of each adjacent frame image; Step S33: obtaining the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane, and performing visual error calculation according to the actual observed coordinates corresponding to the feature points of each adjacent frame image on the image plane and the reprojected coordinates of each adjacent frame image feature point, so as to obtain the visual reprojection error vector corresponding to each adjacent frame image feature point; Step S34: performing visual motion transformation calculation on the three-dimensional coordinates of the feature points of each adjacent frame image based on the visual reprojection error vector corresponding to the feature points of each adjacent frame image, so as to obtain a visual motion transformation matrix between adjacent frame images; Step S35: performing three-dimensional camera pose transformation calculation according to the visual motion transformation matrix between adjacent frame images to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system.

9. The visual inertial navigation calibration method according to claim 8, characterized in that: The three-dimensional coordinates of the feature points of each adjacent frame image described in step S31 are specifically: Get the camera intrinsic parameter matrix corresponding to the visual sensor ,in and The camera is and The focal length corresponding to the direction, and They are the point-by-point coordinates of the visual motion image; Let the adjacent frame image be frame With frame , and in frame Get the corresponding feature point set , while in frame Get the matching feature point set ,in Represents the total number of feature points in adjacent frame images, Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are Indicates frame The corresponding adjacent frame image feature points, specifically: ,in Indicates frame Neidi The horizontal coordinates of the feature points of the adjacent frame images are Indicates frame Neidi The vertical coordinates of the feature points of the adjacent frame images are represents the transpose symbol; Based on the camera intrinsic parameter matrix For Frame feature point set Or Frame feature point set Perform normalized coordinate transformation to obtain Frame Normalized Coordinates Or Frame Normalized Coordinates ; The first Frame Normalized Coordinates Or Frame Normalized Coordinates Perform three-dimensional feature space mapping to obtain the three-dimensional coordinates of feature points of each adjacent frame image , among which 3D coordinates of feature points in adjacent frames .

10. The visual inertial navigation calibration method according to claim 8, characterized in that: The reprojection coordinates of the feature points of each adjacent frame image described in step S32 are specifically: Assume that the camera is The external parameters corresponding to the frames are the rotation matrices and the translation vector , and transform it from the three-dimensional coordinate system to the camera coordinate system according to the corresponding external parameters. The corresponding coordinates in the frame camera coordinate system are ; It is projected onto the image plane according to the projection formula corresponding to the camera, and the projection formula is specifically: ； in, Indicates The feature points of the adjacent frame images are The reprojected coordinates on the frame image plane, represents the scale factor; By solving the above projection formula, we can get the corresponding reprojection coordinates. , , and collect all the reprojection coordinates to obtain the reprojection coordinates of the feature points of each adjacent frame image .

11. The visual inertial navigation calibration method according to claim 8, characterized in that: The visual reprojection error vector described in step S33 is specifically: ,in Represents the total number of feature points in adjacent frame images, represents the transpose symbol, Indicates The visual reprojection error value between feature points of adjacent frame images, Indicates The actual observed coordinates corresponding to the feature points of the adjacent frames on the image plane , Indicates Reprojection coordinates of feature points in adjacent frames .

12. The visual inertial navigation calibration method according to claim 8, characterized in that: Step S35 includes the following steps: Perform singular value decomposition on the visual motion transformation matrix between adjacent frame images to obtain the rotation matrix and translation vector between the feature points of adjacent frame images; The three-dimensional camera pose transformation is calculated based on the rotation matrix and translation vector between the feature points of adjacent frame images to obtain the visual pose transformation matrix between adjacent frame images in the camera coordinate system.

13. The visual inertial navigation calibration method according to claim 12, characterized in that: The visual pose conversion matrix is ​​specifically: ； in, Indicates the camera coordinate system Frame and The visual pose transformation matrix between frame images, Represents the rotation matrix between feature points of adjacent frame images, Represents the translation vector between feature points of adjacent frame images.

14. The visual inertial navigation calibration method according to claim 1, characterized in that: Step S4 includes the following steps: Step S41: pre-integrate the adjacent frame poses of the target carrier inertial data to obtain an inertial pose change matrix between adjacent frame moments in the inertial coordinate system; Step S42: Perform visual-inertial navigation extrinsic calibration according to the visual pose conversion matrix between adjacent frame images in the camera coordinate system and the inertial pose change matrix between adjacent frame moments in the inertial coordinate system to generate a corresponding visual-inertial navigation extrinsic calibration matrix from the camera coordinate system to the inertial coordinate system.

15. The visual inertial navigation calibration method according to claim 14, characterized in that: The inertial posture change matrix described in step S41 is specifically: ； in, In the inertial coordinate system, Frame and The inertial pose change matrix between frame images, Indicates Frame and The rotation pre-integration quantity between frame images, Indicates Frame and Position pre-integration quantity between frame images.

Images