Discrete attitude resolving method for unmanned platform aided navigation

Abstract

The invention belongs to the field of unmanned platform aided navigation, particularly relates to a discrete attitude resolving method for unmanned platform aided navigation, and solves the problems that attitude errors are accumulated along with time and attitude resolving stability is poor in a traditional continuous attitude resolving method (such as a visual odometer, visual instant localization and map construction). And a rapid, discrete and stable posture can be provided for the unmanned vehicle. The method comprises the steps of attitude map library construction and attitude calculation which are in a dependency relationship. The method comprises the following steps: firstly, designing an attitude map library construction method based on a surveying and mapping vehicle, and obtaining an attitude map library containing an internal reference matrix, features and descriptors and integrated navigation information through four rules of integrated navigation horizontal attitude, semantic segmentation, feature number and distribution and position distribution in sequence; and secondly, designing an unmanned platform-oriented attitude resolving method based on the attitude map library and an RANSAC (Random Sample Consensus) algorithm. Experimental results show that the method provided by the invention has attitude calculation precision equivalent to that of a traditional method, and meanwhile, the calculation speed is higher and the stability is higher.

Description

Technical Field

[0001] This invention belongs to the field of unmanned platform assisted navigation, and specifically relates to a discrete attitude calculation method for unmanned platform assisted navigation. Background Technology

[0002] Currently, attitude calculation for ground-based unmanned platforms mainly relies on inertial navigation systems (INS), and the accuracy of attitude calculation depends on the accuracy of the INS. Since the accuracy of INS is directly proportional to its cost, the mainstream attitude calculation systems for existing ground-based unmanned platforms are relatively expensive. In recent years, vision-assisted navigation has emerged as a lower-cost and reasonably accurate attitude calculation method. These methods can be divided into two categories: visual odometry (VO) and simultaneous localization and mapping (SLAM). Both methods calculate attitude by continuously calculating and optimizing the attitude increments between frames. However, the errors of these methods accumulate over time, and the algorithms are prone to interruption when the inter-frame rotation is too large, resulting in very poor stability. This has led to vision-based assisted navigation remaining in the research stage, limiting its large-scale application in industry.

[0003] Analyzing the shortcomings of the aforementioned visual navigation technologies, both error accumulation and poor stability, can be attributed to the continuous solution process within the method. That is, the attitude calculation of the current frame depends on the matching effect between the current frame and the previous frame. Inspired by visual scene matching navigation methods, a discrete attitude calculation approach based on a visual library has emerged. Visual scene matching navigation is currently the mainstream navigation method for ground-based unmanned platforms in satellite-denied environments, such as the visual scene matching positioning of cruise missiles. This method primarily provides discrete positioning, with errors not accumulating over time and unaffected by the overlap of feature information between frames, exhibiting high stability. Intuitively, it seems to perfectly address the shortcomings of existing continuous visual navigation technologies. However, how to design a benchmark library and, based on this library, design an attitude calculation method remains unsolved.

[0004] In view of this, a method for preparing visual reference maps and attitude calculation for assisted navigation of unmanned platforms has been developed, which will fill the gap in discrete attitude calculation methods and overcome the two major drawbacks of continuous attitude calculation methods. Summary of the Invention

[0005] (a) Technical problems to be solved The technical problem to be solved by this invention is: to design a discrete attitude calculation method for the visual navigation process of unmanned platforms, to overcome the two problems of attitude error accumulation over time and very poor attitude calculation stability of traditional methods, and to provide stable attitude for unmanned platforms during long-term endurance. This invention includes two parts: (1) an attitude graph library construction method; (2) an attitude calculation method based on the attitude graph library. The two are dependent on each other, and (2) depends on (1).

[0006] (II) Technical Solution To address the aforementioned technical problems, this invention provides a discrete attitude calculation method for assisted navigation of unmanned platforms, the discrete attitude calculation method comprising: Step 1: Constructing the pose graph library; Step 2: Visual pose calculation.

[0007] Step 1 includes: Step 1.1: The construction process relies on a surveying vehicle platform. The surveying vehicle carries a high-precision inertial navigation system, satellite, and camera. The calibration relationships of these three components are known, and the sensor data can be aligned. The joint calibration matrix of the camera and the high-precision inertial navigation system is denoted as... Simultaneously, the camera's front-upper-right three axes are aligned with the front-upper-right three axes of the surveying vehicle. The high-precision inertial navigation system performs real-time integrated navigation with satellites and can output both positioning and attitude integrated navigation data. The surveying vehicle acquires image and integrated navigation data, obtaining a series of data pairs, denoted as... , For camera internal parameters, For the i-th frame image, Let the combined navigation data for the i-th frame be denoted as... The numbers represent, in sequence, the six-dimensional navigation data of the camera coordinate system relative to the navigation coordinate system: heading angle, roll angle, pitch angle, longitude, latitude, and elevation. The three attitude angles of the camera coordinate system relative to the navigation coordinate system are... Multiplying by the integrated navigation attitude matrix and decomposing it, we obtain the attitude matrix obtained from the integrated navigation in the i-th frame. Multiply by the joint calibration matrix get: Further, the rotation matrix is ​​converted into attitude angles: ; Step 1.2: Based on the integrated navigation results... Perform data set filtering and iterate through the data set. The pitch and roll angle data were processed by removing pitch angles exceeding ±1° and roll angles exceeding ±2°, and the dataset was then renumbered. ; Step 1.3: Process the dataset Perform semantic segmentation on the images in the dataset, and then perform semantic segmentation based on the results. Data was filtered to remove variable areas such as trees, and the dataset was renumbered. ; Step 1.4: Process the dataset ORB feature extraction and non-maximum suppression are performed on the image. The parameters for non-maximum suppression are set to A2*A3. Based on the results, [the following is applied to the image]. Data filtering was performed, removing data with fewer than A4 feature points and replacing them with ORB feature sets. In the I, the data set is renumbered to obtain , recorded as The ORB feature set is denoted as The following are the row, column, 1st dimension, 2nd dimension, and 128th dimension of the first feature, the row, column, 1st dimension, 2nd dimension, and 128th dimension of the second feature, and the row, column, 1st dimension, 2nd dimension, and 128th dimension of the Mth feature, respectively, after feature extraction and non-maximum suppression of the image in the i-th frame data. Step 1.5: Process the dataset The data is filtered by location, with one group retained for every A5 meters on straight sections and one group retained for every A6 meters on curves. The data is then renumbered. The attitude image library is obtained, including camera intrinsics, ORB features, and integrated navigation information, denoted as... .

[0008] In step 1.2, the ±first angle X1° is ±40°.

[0009] In step 1.2, the ±2° second angle is ±20°.

[0010] In step 1.4, A2*A3 is 10*10.

[0011] In step 1.4, A4 is 20.

[0012] In step 1.5, A5 is 40.

[0013] In step 1.5, A6 is 5.

[0014] Step 2 includes: Step 2.1: The attitude calculation method is designed for unmanned platforms. These platforms need to be equipped with cameras, whose front-upper-right axes are aligned with the vehicle's front-upper-right axes. The cameras have undergone intrinsic parameter calibration and are denoted as follows: During the real-time operation of the unmanned vehicle, the images perceived in real time by the onboard cameras are recorded as follows: , which represents the i-th frame image perceived in real time, and performs the following steps sequentially according to the frame number; Step 2.2: Based on the rough localization and attitude of the unmanned vehicle, search the attitude database to find the closest data set. ; Step 2.3: For ORB feature extraction and nonmaximum suppression were performed, and the results were consistent with... Perform a brute-force match to obtain a rough matching result. The brute-force matching method is a mature method in the field of vision. Step 2.4: Perform the RANSAC step on the coarse matching results to remove mismatches. The RANSAC interior point model is based on the fundamental matrix constraint, and the number of RANSAC iterations is set to Y1. The specific steps are as follows: Step 2.4.1: Input coarse matching point pairs The former refers to real-time features, while the latter corresponds to features in the pose database; the input parameters are the intrinsic parameters of the real-time camera. Input the intrinsic parameter K of the camera corresponding to the attitude graph library; Step 2.4.2: Execute iteratively: Select 4 non-collinear point pairs from the coarse matching point pairs, calculate the fundamental matrix using the 8-point method, and calculate the epipolar expression; Step 2.4.3: Calculate the distance of the remaining point pairs from the epipolar line. Set a threshold of 0.5. Points below the threshold are considered inliers, and points above or equal to the threshold are considered outliers. If the proportion of outliers exceeds 91%, the calculation ends and the corresponding basic matrix is ​​output. Otherwise, check if the number of iterations is greater than 200. If it is, output that the calculation failed. Otherwise, repeat step 2.4.2. Step 2.4.4: If the calculation is successful, calculate the rotation matrix based on the fundamental matrix. Then, proceed to step 2.4.5; if the calculation fails, output "Calculation failed for this frame" and proceed to step 2.1 for the next frame of data; the process of calculating the rotation matrix based on the fundamental matrix is ​​a mature method in the field of visual SLAM; Step 2.5: Based on the interior point model obtained in Step 2.4, calculate the rotation matrix of the current vehicle camera coordinate system relative to the image of the corresponding dataset in the pose library. Further The corresponding attitude angles are converted into attitude matrices: Furthermore, multiply the two together to obtain the attitude matrix of the current vehicle camera coordinate system relative to the navigation coordinate system: .

[0015] In step 2.4, Y1 is 200 times.

[0016] (III) Beneficial Effects Compared with the prior art, the present invention has the following beneficial effects: (1) This invention provides a simple attitude graph library construction and discrete visual attitude calculation method for assisting unmanned vehicle navigation, providing a new and systematic attitude calculation method for unmanned vehicles.

[0017] (2) The attitude calculation method is very stable. Compared with the traditional method, which uses epipolar geometry to calculate the 3D feature position and further optimize the attitude, it overcomes the problem of interruption in the attitude calculation process caused by matching failure.

[0018] (3) The attitude calculation method is simple and has very high computational efficiency. Numerous experimental results show that the attitude calculation method of the present invention can reach 5Hz on industrial processors, which is twice as efficient as traditional methods such as ORBSLAM.

[0019] (4) The accuracy of the attitude calculation is higher than that of traditional methods. A large number of experimental results show that the positioning accuracy of the attitude calculation method of the present invention reaches 0.714° (RMSE).

[0020] (5) The posture map library is simple to construct. Compared with the mapping vehicles of map manufacturers such as Baidu and Google, the number of sensors and accuracy requirements are both lower, which makes the industrial application of the present invention possible in terms of cost. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating the offline pose library construction process of the present invention. Figure 2 This is a flowchart illustrating the use of the offline pose library of the present invention; Figure 3 This is the data storage method for the posture graph library of the present invention; Figure 4 This is a set of results from the accuracy analysis of the attitude calculation process of this invention. Detailed Implementation

[0022] To make the objectives, contents, and advantages of the present invention clearer, the specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples.

[0023] To address the problems of existing technologies, this invention provides a discrete attitude calculation method for assisted navigation of unmanned platforms, the discrete attitude calculation method comprising: Step 1: Constructing the pose graph library; Step 2: Visual pose calculation.

[0024] Step 1 includes: Step 1.1: The construction process relies on a surveying vehicle platform. The surveying vehicle carries a high-precision inertial navigation system, satellite, and camera. The calibration relationships of these three components are known, and the sensor data can be aligned. The joint calibration matrix of the camera and the high-precision inertial navigation system is denoted as... Simultaneously, the camera's front-upper-right three axes are aligned with the front-upper-right three axes of the surveying vehicle. The high-precision inertial navigation system performs real-time integrated navigation with satellites and can output both positioning and attitude integrated navigation data. The surveying vehicle acquires image and integrated navigation data, obtaining a series of data pairs, denoted as... , For camera internal parameters, For the i-th frame image, Let the combined navigation data for the i-th frame be denoted as... The numbers represent, in sequence, the six-dimensional navigation data of the camera coordinate system relative to the navigation coordinate system: heading angle, roll angle, pitch angle, longitude, latitude, and elevation. The three attitude angles of the camera coordinate system relative to the navigation coordinate system are... Multiplying by the integrated navigation attitude matrix and decomposing it, we obtain the attitude matrix obtained from the integrated navigation in the i-th frame. Multiply by the joint calibration matrix get: Further, the rotation matrix is ​​converted into attitude angles: ; Step 1.2: Based on the integrated navigation results... Perform data set filtering and iterate through the data set. The pitch and roll angle data were processed by removing pitch angles exceeding ±1° and roll angles exceeding ±2°, and the dataset was then renumbered. ; Step 1.3: Process the dataset Perform semantic segmentation on the images in the dataset, and then perform semantic segmentation based on the results. Data was filtered to remove variable areas such as trees, and the dataset was renumbered. ; Step 1.4: Process the dataset ORB feature extraction and non-maximum suppression are performed on the image. The parameters for non-maximum suppression are set to A2*A3. Based on the results, [the following is applied to the image]. Data filtering was performed, removing data with fewer than A4 feature points and replacing them with ORB feature sets. In the I, the data set is renumbered to obtain , recorded as The ORB feature set is denoted as The following are the row, column, 1st dimension, 2nd dimension, and 128th dimension of the first feature, the row, column, 1st dimension, 2nd dimension, and 128th dimension of the second feature, and the row, column, 1st dimension, 2nd dimension, and 128th dimension of the Mth feature, respectively, after feature extraction and non-maximum suppression of the image in the i-th frame data. Step 1.5: Process the dataset The data is filtered by location, with one group retained for every A5 meters on straight sections and one group retained for every A6 meters on curves. The data is then renumbered. The attitude image library is obtained, including camera intrinsics, ORB features, and integrated navigation information, denoted as... .

[0025] In step 1.2, the ±first angle X1° is ±40°.

[0026] In step 1.2, the ±2° second angle is ±20°.

[0027] In step 1.4, A2*A3 is 10*10.

[0028] In step 1.4, A4 is 20.

[0029] In step 1.5, A5 is 40.

[0030] In step 1.5, A6 is 5.

[0031] Step 2 includes: Step 2.1: The attitude calculation method is designed for unmanned platforms. These platforms need to be equipped with cameras, whose front-upper-right axes are aligned with the vehicle's front-upper-right axes. The cameras have undergone intrinsic parameter calibration and are denoted as follows: During the real-time operation of the unmanned vehicle, the images perceived in real time by the onboard cameras are recorded as follows: , which represents the i-th frame image perceived in real time, and performs the following steps sequentially according to the frame number; Step 2.2: Based on the rough localization and attitude of the unmanned vehicle, search the attitude database to find the closest data set. ; Step 2.3: For ORB feature extraction and nonmaximum suppression were performed, and the results were consistent with... Perform a brute-force match to obtain a rough matching result. The brute-force matching method is a mature method in the field of vision. Step 2.4: Perform the RANSAC step on the coarse matching results to remove mismatches. The RANSAC interior point model is based on the fundamental matrix constraint, and the number of RANSAC iterations is set to Y1. The specific steps are as follows: Step 2.4.1: Input coarse matching point pairs The former refers to real-time features, while the latter corresponds to features in the pose database; the input parameters are the intrinsic parameters of the real-time camera. Input the intrinsic parameter K of the camera corresponding to the attitude graph library; Step 2.4.2: Execute iteratively: Select 4 non-collinear point pairs from the coarse matching point pairs, calculate the fundamental matrix using the 8-point method, and calculate the epipolar expression; Step 2.4.3: Calculate the distance of the remaining point pairs from the epipolar line. Set a threshold of 0.5. Points below the threshold are considered inliers, and points above or equal to the threshold are considered outliers. If the proportion of outliers exceeds 91%, the calculation ends and the corresponding basic matrix is ​​output. Otherwise, check if the number of iterations is greater than 200. If it is, output that the calculation failed. Otherwise, repeat step 2.4.2. Step 2.4.4: If the calculation is successful, calculate the rotation matrix based on the fundamental matrix. Then, proceed to step 2.4.5; if the calculation fails, output "Calculation failed for this frame" and proceed to step 2.1 for the next frame of data; the process of calculating the rotation matrix based on the fundamental matrix is ​​a mature method in the field of visual SLAM; Step 2.5: Based on the interior point model obtained in Step 2.4, calculate the rotation matrix of the current vehicle camera coordinate system relative to the image of the corresponding dataset in the pose library. Further The corresponding attitude angles are converted into attitude matrices: Furthermore, multiply the two together to obtain the attitude matrix of the current vehicle camera coordinate system relative to the navigation coordinate system: .

[0032] In step 2.4, Y1 is 200 times.

[0033] Example 1 This embodiment includes: A simple method for constructing a pose graph library, the construction steps are as follows: 1.A. The construction process relies on a surveying vehicle platform. The surveying vehicle needs to be equipped with commonly used high-precision inertial navigation systems, satellite receivers, and cameras. The calibration relationships of these three components must be known, and the sensor data must be aligned. The joint calibration matrix of the camera and inertial navigation system is denoted as... Simultaneously, the camera's front-upper-right three axes are aligned with the vehicle's front-upper-right three axes. The high-precision inertial navigation system performs real-time integrated navigation with satellites and can output both positioning and attitude integrated navigation data. The mapping vehicle acquires images and integrated navigation data, obtaining a series of data pairs, denoted as... , For camera internal parameters, For the i-th frame image, Let the combined navigation data for the i-th frame be denoted as... The numbers represent, in sequence, the six-dimensional navigation data of the camera coordinate system relative to the navigation coordinate system: heading angle, roll angle, pitch angle, longitude, latitude, and elevation. The three attitude angles of the camera coordinate system relative to the navigation coordinate system are... Multiplying by the integrated navigation attitude matrix and decomposing it, we obtain the attitude matrix obtained from the integrated navigation in the i-th frame. Multiply by the calibration matrix get: (1) Further, the rotation matrix is ​​converted into attitude angles: 1.B. Based on the integrated navigation results... Perform data set filtering and iterate through the data set. The pitch and roll angle data were processed by removing pitch angles exceeding ±40° and roll angles exceeding ±20°, and the dataset was then renumbered. ; 1.C. For data sets Perform semantic segmentation on the images in the dataset, and then perform semantic segmentation based on the results. Data was filtered to remove variable areas such as trees, and the dataset was renumbered. ; 1.D. Regarding the data set ORB feature extraction and non-maximum suppression were performed on the image. The parameters for non-maximum suppression were set to 10*10. Based on the results, [the following was applied]. Data filtering was performed, removing data with fewer than 20 feature points and replacing them with ORB feature sets. In the I, the data set is renumbered to obtain , recorded as The ORB feature set is denoted as (2) The following are the row, column, 1st dimension, 2nd dimension, and 128th dimension of the first feature, the row, column, 1st dimension, 2nd dimension, and 128th dimension of the second feature, and the row, column, 1st dimension, 2nd dimension, and 128th dimension of the Mth feature, respectively, after feature extraction and non-maximum suppression of the image in the i-th frame data. 1.E. For data sets The data was filtered by location, with each group retained every 40 meters on straightaways and every 5 meters on curves. The data was then renumbered. The attitude image library is obtained, including camera intrinsics, ORB features, and integrated navigation information, denoted as... ; On the other hand, this embodiment provides a discrete attitude calculation method based on an attitude graph library, and the calculation steps are as follows: 2.A. The attitude calculation method is designed for unmanned platforms. These platforms need to be equipped with cameras whose front-upper-right axes are aligned with the vehicle's front-upper-right axes, and the cameras have undergone intrinsic parameter calibration. This is denoted as... During the real-time operation of the unmanned vehicle, the images perceived in real time by the onboard cameras are recorded as follows: , which represents the i-th frame image perceived in real time, and performs the following steps sequentially according to the frame number; 2.B. Based on the rough localization and attitude of the autonomous vehicle, search the attitude database to find the closest data set. ; 2.C. Correct ORB feature extraction and nonmaximum suppression were performed, and the results were consistent with... Perform a brute-force match to obtain a rough matching result. The brute-force matching method is a mature method in the field of vision. 2.D. Perform the RANSAC step on the coarse matching results to remove mismatches. The RANSAC interior point model is based on the fundamental matrix constraint, and the number of RANSAC iterations is set to 200. The specific steps are as follows: 2.D.1 Input coarse matching point pairs The former refers to real-time features, while the latter corresponds to features in the pose database; the input parameters are the intrinsic parameters of the real-time camera. Input the intrinsic parameter K of the camera corresponding to the attitude graph library; 2.D.2 Loop execution: Select 4 non-collinear point pairs from the coarse matching point pairs, calculate the fundamental matrix using the 8-point method, and calculate the epipolar expression; 2.D.3 Calculate the distance of the remaining point pairs from the epipolar line. Set a threshold of 0.5. Points below the threshold are considered inliers, and points above or equal to the threshold are considered outliers. If the proportion of outliers exceeds 91%, the calculation ends and the corresponding fundamental matrix is ​​output. Otherwise, check if the number of iterations is greater than 200. If it is, output that the calculation failed. Otherwise, repeat step 2.D.2. 2.D.4 If the calculation is successful, calculate the rotation matrix based on the fundamental matrix. And execute step 2.E; if the calculation fails, output that the calculation failed for this frame, and execute the next frame data in step 2.A; the process of calculating the rotation matrix based on the fundamental matrix is ​​a mature method in the field of visual SLAM; 2.E. Based on the interior point model obtained in step 2.D, calculate the rotation matrix of the current vehicle camera coordinate system relative to the image of the corresponding dataset in the pose library. Further The corresponding attitude angles are converted into attitude matrices: (3) Furthermore, multiply the two together to obtain the attitude matrix of the current vehicle camera coordinate system relative to the navigation coordinate system: (4).

[0034] Example 2 On the one hand, this embodiment provides a simple method for constructing a pose graph library, and the system flow is as follows: Figure 1 As shown, the specific construction steps are as follows: 1.A. The construction process relies on a surveying vehicle platform. The surveying vehicle needs to be equipped with commonly used high-precision inertial navigation systems, satellite receivers, and cameras. The calibration relationships of these three components must be known, and the sensor data must be aligned. The joint calibration matrix of the camera and inertial navigation system is denoted as... Simultaneously, the camera's front-upper-right three axes are aligned with the vehicle's front-upper-right three axes. The high-precision inertial navigation system performs real-time integrated navigation with satellites and can output both positioning and attitude integrated navigation data. The mapping vehicle acquires images and integrated navigation data, obtaining a series of data pairs, denoted as... , For camera internal parameters, For the i-th frame image, Let the combined navigation data for the i-th frame be denoted as... These represent the six-dimensional navigation data of the camera coordinate system relative to the navigation coordinate system: heading angle, roll angle, pitch angle, longitude, latitude, and elevation. The three attitude angles of the camera coordinate system relative to the navigation coordinate system are... Multiplying by the integrated navigation attitude matrix and decomposing it, we obtain the attitude matrix obtained from the integrated navigation in the i-th frame. Multiply by the calibration matrix get: (1) Further, the rotation matrix is ​​converted into attitude angles: 1.B. Based on the integrated navigation results... Perform data set filtering and iterate through the data set. The pitch and roll angle data were processed by removing pitch angles exceeding ±40° and roll angles exceeding ±20°, and the dataset was then renumbered. .

[0035] 1.C. For data sets Perform semantic segmentation on the images in the dataset, and then perform semantic segmentation based on the results. Data was filtered to remove variable areas such as trees, and the dataset was renumbered. .

[0036] 1.D. Regarding the data set ORB feature extraction and non-maximum suppression were performed on the image. The parameters for non-maximum suppression were set to 10*10. Based on the results, [the following was applied]. Data filtering was performed, removing data with fewer than 20 feature points and replacing them with ORB feature sets. In the I, the data set is renumbered to obtain , recorded as The ORB feature set is denoted as (2) The following are the row, column, 1st dimension, 2nd dimension, and 128th dimension of the first feature, the row, column, 1st dimension, 2nd dimension, and 128th dimension of the second feature, and the row, column, 1st dimension, 2nd dimension, and 128th dimension of the Mth feature, respectively, after feature extraction and non-maximum suppression of the image in the i-th frame data.

[0037] 1.E. For data sets The data was filtered by location, with each group retained every 40 meters on straightaways and every 5 meters on curves. The data was then renumbered. The attitude image library is obtained, including camera intrinsics, ORB features, and integrated navigation information, denoted as... Specific forms are as follows: Figure 3 As shown.

[0038] On the other hand, this embodiment provides a discrete attitude calculation method based on an attitude graph library, and the system flow is as follows: Figure 2 As shown, the specific solution steps are as follows: 2.A. The attitude calculation method is designed for unmanned platforms. These platforms need to be equipped with cameras whose front-upper-right axes are aligned with the vehicle's front-upper-right axes, and the cameras have undergone intrinsic parameter calibration. This is denoted as... During the real-time operation of the autonomous vehicle, the images perceived by its onboard cameras are recorded as follows: , which represents the i-th frame image perceived in real time, and the following steps are executed sequentially according to the frame number.

[0039] 2.B. Based on the rough localization and attitude of the autonomous vehicle, search the attitude database to find the closest data set. .

[0040] 2.C. Correct ORB feature extraction and nonmaximum suppression were performed, and the results were consistent with... Perform a brute-force match to obtain a rough matching result. The brute-force matching method is a mature method in the field of vision. 2.D. Perform the RANSAC step on the coarse matching results to remove mismatches. The RANSAC interior point model is based on the fundamental matrix constraint, and the number of RANSAC iterations is set to 200. The specific steps are as follows: 2.D.1 Input coarse matching point pairs The former refers to real-time features, while the latter corresponds to features in the pose database. Input the intrinsic parameters of the real-time camera. Input the intrinsic parameter K of the camera corresponding to the attitude graph library.

[0041] 2.D.2 Loop execution: Select 4 non-collinear point pairs from the coarse matching point pairs, calculate the fundamental matrix using the 8-point method, and calculate the epipolar expression.

[0042] 2.D.3 Calculate the distance of the remaining point pairs from the epipolar line, setting a threshold of 0.5. Points below the threshold are considered inliers, and those above or equal to the threshold are considered outliers. If the proportion of outliers exceeds 91%, the calculation ends, and the corresponding fundamental matrix is ​​output as a success. Otherwise, check if the loop count is greater than 200. If it is, output "Calculation failed"; otherwise, repeat step 2.D.2.

[0043] 2.D.4 If the calculation is successful, calculate the rotation matrix based on the fundamental matrix. Then, proceed to step 2.E. If the calculation fails, output "Calculation Failed for this Frame," and proceed to the next frame of data in step 2.A. The process of calculating the rotation matrix based on the fundamental matrix is ​​a mature method in the field of visual SLAM.

[0044] 2.E. Based on the interior point model obtained in step 2.D, calculate the rotation matrix of the current vehicle camera coordinate system relative to the image of the corresponding dataset in the pose library. Further The corresponding attitude angles are converted into attitude matrices: (3) Furthermore, multiply the two together to obtain the attitude matrix of the current vehicle camera coordinate system relative to the navigation coordinate system: (4) A set of analysis results as follows Figure 4 As shown, the attitude estimation error is better than 1.2341° (RMSE).

[0045] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A discrete attitude calculation method for assisted navigation of unmanned platforms, characterized in that the discrete attitude calculation method includes: Step 1: Constructing the pose graph library; Step 2: Visual pose calculation.

2. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 1, characterized in that, Step 1 includes: Step 1.1: The construction process relies on a surveying vehicle platform. The surveying vehicle carries a high-precision inertial navigation system, satellite, and camera. The calibration relationships of these three components are known, and the sensor data can be aligned. The joint calibration matrix of the camera and the high-precision inertial navigation system is denoted as... Simultaneously, the camera's front-upper-right three axes are aligned with the front-upper-right three axes of the surveying vehicle. The high-precision inertial navigation system performs real-time integrated navigation with satellites and can output both positioning and attitude integrated navigation data. The surveying vehicle acquires image and integrated navigation data, obtaining a series of data pairs, denoted as... , For camera internal parameters, For the i-th frame image, Let the combined navigation data for the i-th frame be denoted as... The numbers represent, in sequence, the six-dimensional navigation data of the camera coordinate system relative to the navigation coordinate system: heading angle, roll angle, pitch angle, longitude, latitude, and elevation. The three attitude angles of the camera coordinate system relative to the navigation coordinate system are... Multiplying by the integrated navigation attitude matrix and decomposing it, we obtain the attitude matrix obtained from the integrated navigation in the i-th frame. Multiply by the joint calibration matrix get: Further, the rotation matrix is ​​converted into attitude angles: ； Step 1.2: Based on the integrated navigation results... Perform data set filtering and iterate through the data set. The pitch and roll angle data were processed by removing pitch angles exceeding ±1° and roll angles exceeding ±2°, and the dataset was then renumbered. ; Step 1.3: Process the dataset Perform semantic segmentation on the images in the dataset, and then perform semantic segmentation based on the results. Data was filtered to remove variable areas such as trees, and the dataset was renumbered. ; Step 1.4: Process the dataset ORB feature extraction and non-maximum suppression are performed on the image. The parameters for non-maximum suppression are set to A2*A3. Based on the results, [the following is applied to the image]. Data filtering was performed, removing data with fewer than A4 feature points and replacing them with ORB feature sets. In the I, the data set is renumbered to obtain , recorded as The ORB feature set is denoted as The following are the row, column, 1st dimension, 2nd dimension, and 128th dimension of the first feature, the row, column, 1st dimension, 2nd dimension, and 128th dimension of the second feature, and the row, column, 1st dimension, 2nd dimension, and 128th dimension of the Mth feature, respectively, after feature extraction and non-maximum suppression of the image in the i-th frame data. Step 1.5: Process the dataset The data is filtered by location, with one group retained for every A5 meters on straight sections and one group retained for every A6 meters on curves. The data is then renumbered. The attitude image library is obtained, including camera intrinsics, ORB features, and integrated navigation information, denoted as... .

3. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.2, the ±first angle X1° is ±40°.

4. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.2, ±2° is ±20°.

5. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.4, A2*A3 is 10*10.

6. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.4, A4 is 20.

7. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.5, A5 is 40.

8. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, In step 1.5, A6 is 5.

9. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 2, characterized in that, Step 2 includes: Step 2.1: The attitude calculation method is designed for unmanned platforms. These platforms need to be equipped with cameras, whose front-upper-right axes are aligned with the vehicle's front-upper-right axes. The cameras have undergone intrinsic parameter calibration and are denoted as follows: During the real-time operation of the unmanned vehicle, the images perceived in real time by the onboard cameras are recorded as follows: , which represents the i-th frame image perceived in real time, and performs the following steps sequentially according to the frame number; Step 2.2: Based on the rough localization and attitude of the unmanned vehicle, search the attitude database to find the closest data set. ; Step 2.3: For ORB feature extraction and nonmaximum suppression were performed, and the results were consistent with... Perform a brute-force match to obtain a rough matching result. The brute-force matching method is a mature method in the field of vision. Step 2.4: Perform the RANSAC step on the coarse matching results to remove mismatches. The RANSAC interior point model is based on the fundamental matrix constraint, and the number of RANSAC iterations is set to Y1. The specific steps are as follows: Step 2.4.1: Input coarse matching point pairs The former refers to real-time features, while the latter corresponds to features in the pose database; the input parameters are the intrinsic parameters of the real-time camera. Input the intrinsic parameter K of the camera corresponding to the attitude graph library; Step 2.4.2: Execute iteratively: Select 4 non-collinear point pairs from the coarse matching point pairs, calculate the fundamental matrix using the 8-point method, and calculate the epipolar expression; Step 2.4.3: Calculate the distance of the remaining point pairs from the epipolar line. Set a threshold of 0.

5. Points below the threshold are considered inliers, and points above or equal to the threshold are considered outliers. If the proportion of outliers exceeds 91%, the calculation ends and the corresponding basic matrix is ​​output. Otherwise, check if the number of iterations is greater than 200. If it is, output that the calculation failed. Otherwise, repeat step 2.4.

2. Step 2.4.4: If the calculation is successful, calculate the rotation matrix based on the fundamental matrix. Then, proceed to step 2.4.5; if the calculation fails, output "Calculation failed for this frame" and proceed to step 2.1 for the next frame of data; the process of calculating the rotation matrix based on the fundamental matrix is ​​a mature method in the field of visual SLAM; Step 2.5: Based on the interior point model obtained in Step 2.4, calculate the rotation matrix of the current vehicle camera coordinate system relative to the image of the corresponding dataset in the pose library. Further The corresponding attitude angles are converted into attitude matrices: Furthermore, multiply the two together to obtain the attitude matrix of the current vehicle camera coordinate system relative to the navigation coordinate system: 。 10. The discrete attitude calculation method for assisted navigation of unmanned platforms as described in claim 9, characterized in that, In step 2.4, Y1 is 200 times.

Images