Unmanned aerial vehicle light visual-inertial positioning method fusing magnetometer

Abstract

The application discloses a kind of unmanned aerial vehicle light visual inertial positioning methods of fusion magnetometer, including image point feature and line feature combination processing, geomagnetic field data correction yaw angle error, three degrees of freedom displacement graph optimization method, after multiple correction state quantity data, finally obtain higher precision displacement, velocity, rotation state quantity data, solve the feature point tracking robustness of visual inertial positioning method exists, and the yaw angle cannot be observed, resulting in the shortcomings of state quantity data precision drift, suitable for unmanned aerial vehicle light embedded platform for executing outdoor tasks such as traffic patrol.The unmanned aerial vehicle light visual inertial positioning method of fusion magnetometer provided by the application has very high positioning accuracy, robustness and real-time performance.

Description

TECHNICAL FIELD

[0001] The application relates to a unmanned aerial vehicle light visual inertial positioning method fusing a magnetometer, and belongs to the technical field of multi-sensor fusion positioning. BACKGROUND

[0002] In unmanned aerial vehicle applications, a positioning technology is one of key technologies for safe movement of a carrier. The positioning technology is of great significance to behavior decision-making such as speed control, path planning and collision avoidance.

[0003] At present, main positioning systems of multi-rotor aircrafts include a GPS satellite positioning system, a strapdown inertial positioning system and a laser and inertial navigation fusion positioning system, but these positioning systems are used for positioning on the premise that GPS or environment information is known, and state quantity data provided by the positioning systems loses reference value in the whole positioning process if there is great interference. A traditional positioning system improves positioning precision through redundancy design, and the method is accompanied by improvement of manufacturing cost.

[0004] In recent years, with rapid development of microelectronic technology, low-cost and small-size inertial sensors such as gyroscopes and magnetometers are applied to various fields. However, the sensors inevitably have the defect of poor anti-interference after highlighting the advantages of low cost and small size, and single sensors applied in a system inevitably lead to reduced measurement precision. In addition, the visual positioning system is widely applied in unmanned aerial vehicle positioning systems, and personnel skilled in the art urgently need to solve how to fuse unmanned aerial vehicle positioning information obtained by a camera and other inertial measurement sensors to optimize higher-precision displacement, speed and rotation state quantity data. SUMMARY

[0005] Objective: In order to overcome the problems of poor feature point tracking robustness and unobservable yaw angle in the visual inertial positioning method, so as to cause the defects of state quantity data drift and low precision, the application provides a unmanned aerial vehicle light visual inertial positioning method fusing a magnetometer.

[0006] Technical scheme: To solve the above technical problems, the application adopts the technical scheme that:

[0007] A unmanned aerial vehicle light visual inertial positioning method fusing a magnetometer comprises the following steps:

[0008] Step 1: acquiring image data of a dual-camera of the unmanned aerial vehicle, including a left view a right view and inertial measurement data, the inertial measurement data comprising acceleration data, angular velocity data and geomagnetic field data.

[0009] Step 2: respectively in each frame of the left view and the right view extracting image features.

[0010] Step 3: Integrate the angular velocity data and the acceleration data to obtain displacement and rotation relative state variables.

[0011] Step 4: Use the geomagnetic field data to calculate the geomagnetic yaw angle, extract the integrated yaw angle from the rotation relative state variables, correct the yaw angle error using the geomagnetic yaw angle and the integrated yaw angle, obtain the first yaw angle correction value, and obtain the first rotation state variable correction value according to the first yaw angle correction value.

[0012] Step 5: Use the direct method to match the current time image features with the next time image features, correct the first rotation state variable correction value according to the matching result, and obtain the second rotation state variable correction value.

[0013] Step 6: Associate the second state variable correction value with the key frame and perform nonlinear optimization to obtain the third rotation state variable correction value.

[0014] As a preferred solution, it further includes Step 7: When the UAV system detects a loop, construct a displacement graph optimization model, input the displacement state variables associated with the key frame into the displacement graph optimization model, and solve the displacement graph optimization model to obtain the displacement state variable correction value.

[0015] As a preferred solution, the step 2 specifically includes the following steps:

[0016] Step 2.1: Extract τ1 point features from the left view and the right view image.

[0017] Step 2.2: After removing bad points, use a line feature detection algorithm to extract τ2 line features from the left view and the right view image.

[0018] Step 2.3: After removing line features with a length less than a length threshold, obtain the start and end points of the removed line features and add them to the point features to obtain updated point features.

[0019] As a preferred solution, it further includes Step 2.4:

[0020] Step 2.4: When the updated point features are less than a quantity threshold, select random points on the image and add them to the updated point features.

[0021] As a preferred solution, the displacement and rotation relative state variable calculation formula is as follows:

[0022]

[0023]

[0024] wherein, are the displacement, rotation relative state variables of the body coordinate system b at time k+1 with respect to the body coordinate system b at time k, b a is the acceleration bias, b ω is the gyroscope bias, n a , n ω are the Gaussian white noise of the acceleration and angular velocity sensors, are the acceleration data measurements, are the angular velocity data measurements, is the rotation matrix of the body coordinate system b at time t, is the quaternion of the rotation of the body coordinate system b at time t, and Ω represents the conversion from vector form to quaternion form.

[0025] As a preferred solution, the step 4 specifically comprises the following steps:

[0026] Step 4.1: According to the geomagnetic field data, the geomagnetic yaw angle Yaw in the rotation state variable is calculated, and the calculation formula is as follows:

[0027]

[0028]

[0029]

[0030] wherein, the superscript b represents in the body coordinate system, and the superscript n represents in the world coordinate system, respectively represent the geomagnetic field in the x and y axis directions in the world coordinate system, respectively represent the geomagnetic field in the x, y and z axis directions in the body coordinate system, Pitch is the pitch angle, and Roll is the roll angle.

[0031] Step 4.2: The integral yaw angle is extracted from the rotation relative state variable The geomagnetic yaw angle Yaw and the integral yaw angle are fused by using the Kalman filtering method to obtain the first yaw angle correction value

[0032] Step 4.3: The first yaw angle correction value is used to replace the yaw angle in the rotation state variable to obtain the first rotation state variable correction value.

[0033] As a preferred solution, the step 4.2 specifically comprises the following steps:

[0034] Step 4.2.1: Integrate the yaw angle As the prior estimate at the current time, denoted as Take the magnetic yaw angle Yaw as the observation at the current time, denoted as Yaw t .

[0035] Step 4.2.2: According to the prior estimate The observation Yaw t and the state weight K t , calculate the first yaw angle correction value at the current time The calculation formula is as follows:

[0036]

[0037] Wherein, the state weight K t The calculation formula is as follows:

[0038]

[0039] Wherein, The prior estimation covariance at the current time, H is the observation transition matrix, is the unit matrix, H T The transpose of H, R is the observation noise variance.

[0040] Wherein, The calculation formula is as follows:

[0041]

[0042] Wherein, P t-1 The posterior estimation covariance at the last time, F is the state transition matrix, is a constant, Q is the process noise variance.

[0043] As a preferred solution, the step 6 specifically comprises the following steps:

[0044] Step 6.1: Select the second rotation state correction value corresponding to the image pair with a parallax greater than τ3 pixels and more than τ4 image features as the key frame

[0045] Step 6.2: Store the key frame Into the window, and store τ5 windows continuously.

[0046] Step 6.3: Put the second rotation state correction value in all windows into the optimization model for optimization until the relative rotation error is minimized, and output the third rotation state correction value.

[0047] As a preferred solution, the optimization model comprises: visual residual, inertial residual, and edge prior information.

[0048] As a preferred solution, the relative rotation amount error is calculated according to the inertial residual error, and the calculation formula is as follows:

[0049]

[0050] wherein, is the rotation amount of the second rotation state quantity correction value in the body coordinate system b in the s-th window, is the rotation amount of the second rotation state quantity correction value in the body coordinate system b in the s+1-th window, is the relative rotation amount between the s-th window and the s+1-th window in the body coordinate system b, is the relative rotation amount error between the s-th window and the s+1-th window.

[0051] As a preferred solution, the displacement map optimization model calculation formula is as follows:

[0052]

[0053] wherein, are respectively the displacement state quantity of the two consecutive key frames in the body coordinate system b, r i,j (·) is the residual function, and h(·) is the loss function.

[0054] As a preferred solution, the calculation formula of the residual function is as follows:

[0055]

[0056] wherein, are respectively the Pitch, Roll and Yaw of the third rotation state quantity correction value; R(·) is the matrix expression of the rotation amount, is the relative displacement amount between the key frames in the body coordinate system b at the k+1 time.

[0057] As a preferred solution, τ1=500, τ2=100, and the length threshold is 50 pixels.

[0058] As a preferred solution, τ3=40, τ4=100, and τ5=10.

[0059] Beneficial effects: the unmanned aerial vehicle light visual inertial positioning method fusing magnetometer provided by the application includes image point feature and line feature combined processing, magnetic field data correction yaw angle error, three degrees of freedom displacement graph optimization method, after multiple correction state quantity data, finally, higher precision displacement, speed, rotation state quantity data are obtained, the shortcomings of poor feature point tracking robustness and state quantity data precision drift caused by unobservable yaw angle in visual inertial positioning method are solved, the method is suitable for unmanned aerial vehicle light embedded platform for executing outdoor tasks such as traffic patrol, the method has high positioning precision, robustness and real-time performance. BRIEF DESCRIPTION OF DRAWINGS

[0060] Figure 1 The system flow chart of the unmanned aerial vehicle light visual inertial positioning method fusing magnetometer is shown.

[0061] Figure 2 The image feature extraction method flow chart is shown.

[0062] Figure 3 The yaw angle correction method flow chart is shown. DETAILED DESCRIPTION

[0063] The application will be further described below in combination with specific embodiments.

[0064] As shown in the figure, an unmanned aerial vehicle light visual inertial positioning method fusing magnetometer includes the following steps: Figure 1

[0065] Step 1: image data is obtained from a binocular camera, including left view and right view Inertial measurement data is obtained from a nine-axis IMU sensor, including acceleration data a t ={a x a y a z}, angular velocity data ω t ={ω x ω y ω z}, and magnetic field data m t ={m x m y m z}, wherein a x , a y , a z represent the acceleration of x, y and z axis directions respectively, ω x , ω y , ω z represent the angular velocity of x, y and z axis directions respectively, and m x ​m y m z These represent the geomagnetic fields along the x, y, and z axes, respectively.

[0066] The binocular camera outputs image data at a frequency of 25Hz. Acceleration data, angular velocity data, and geomagnetic field data are aligned with the three data timestamps using a linear interpolation method, maintaining an output frequency of 100Hz.

[0067] This method incorporates geomagnetic field data into the traditional visual-inertial positioning method. Therefore, by acquiring geomagnetic field data through a magnetometer and fusing the geomagnetic field data in a loosely coupled manner, the yaw angle can be corrected, thereby improving the positioning robustness of the system.

[0068] Step 2: View the left side of each frame of image data. and right view Extract image features.

[0069] like Figure 2 As shown, firstly, 500 point features P are extracted from the image. i ={x i ,y i After removing bad pixels, a line feature detection algorithm is used to extract 100 line features from the image. The extracted line features contain three basic pieces of information: starting point end and length l j After removing lines with a length less than 50, extract the starting point of each line feature. and the end point As a new point feature.

[0070] This method extracts two types of features from the image: point features and edge features. Since the actual number of point features is less than the system requirement after each extraction and removal of low-quality points, edge features are used to compensate for this deficiency and improve the overall feature richness of the image. If the final number of features after compensating for point features with edge features still does not meet the system requirements, random points will be selected from the image as image features.

[0071] Step 3: Integrate the angular velocity and acceleration data to obtain the displacement, velocity, and rotational relative state variables. The calculation formulas are as follows:

[0072]

[0073]

[0074]

[0075] in, are displacement, velocity, rotation relative state quantities of the body coordinate system b at time k + 1 with respect to the body coordinate system b at time k, b a is the acceleration bias, b ω is the gyroscope bias, n a , n ω are the Gaussian white noises of the acceleration and angular velocity sensors. are the acceleration data measurements, are the angular velocity data measurements, is the rotation matrix of the body coordinate system b at time t, is the quaternion of the rotation of the body coordinate system b at time t, and Ω represents the conversion from the vector form to the quaternion form.

[0076] As shown in Figure 3 Step 4: using the geomagnetic field data, calculating the geomagnetic yaw angle, extracting the integral yaw angle from the rotation relative state quantity, correcting the yaw angle error using the geomagnetic yaw angle and the integral yaw angle, obtaining the first yaw angle correction value, and obtaining the first rotation state quantity correction value according to the first yaw angle correction value.

[0077] According to the geomagnetic field data, the geomagnetic yaw angle Yaw in the rotation state quantity is calculated as follows:

[0078]

[0079]

[0080]

[0081] wherein the superscript b represents in the body coordinate system, and the superscript n represents in the world coordinate system, respectively represent the geomagnetic field in the x and y axis directions in the world coordinate system, respectively represent the geomagnetic field in the x, y and z axis directions in the body coordinate system, Pitch is the pitch angle, and Roll is the roll angle.

[0082] extracting the integral yaw angle from the rotation relative state quantity combining the geomagnetic yaw angle Yaw and the integral yaw angle using the Kalman filtering method to obtain the first yaw angle correction value The specific fusion method is as follows:

[0083] taking the integral yaw angle as the prior estimate at the current time, denoted as taking the geomagnetic yaw angle Yaw as the observation at the current time, denoted as Yaw t .

[0084] according to the prior estimate Observation Yaw t And state quantity weight K t , calculate the first time yaw angle correction value at the current time The calculation formula is as follows:

[0085]

[0086] Wherein, state quantity weight K t The calculation formula is as follows:

[0087]

[0088] Wherein, The prior estimation covariance at the current time, H is the observation transition matrix, is the unit matrix, H T Is the transpose of H, and R is the observation noise variance.

[0089] Wherein, The calculation formula is as follows:

[0090]

[0091] Wherein, P t-1 The posterior estimation covariance at the last time, F is the state transition matrix, is a constant, and Q is the process noise variance.

[0092] Replace the yaw angle in the rotation state quantity with the first time yaw angle correction value Obtain the first rotation state quantity correction value.

[0093] Step 5: use the direct method to match the image features at the current time with the image features at the next time, and correct the first rotation state quantity correction value according to the matching result to obtain the second rotation state quantity correction value.

[0094] Step 6: associate the second state quantity correction value with the key frame And carry out nonlinear optimization to obtain the third rotation state quantity correction value, further reduce the error of the rotation state quantity. The specific steps are as follows:

[0095] Nonlinear optimization includes: using sliding window method and beam square difference (BA) method.

[0096] Select the second rotation state quantity correction value corresponding to the image pair with a parallax greater than 40 pixels and more than 100 image features as the key frame

[0097] Store the key frame Into the window, store 10 windows continuously.

[0098] The second rotation state quantity correction value in all windows is put into the same optimization model for optimization until the relative rotation quantity error is minimized, and the third rotation state quantity correction value is output.

[0099] The optimization model comprises visual residual error, inertial residual error and marginalized prior information.

[0100] The inertial residual error is used to calculate the relative rotation quantity error, and the rotation quantity error calculation formula is as follows:

[0101]

[0102] Wherein, is the rotation quantity of the second rotation state quantity correction value in the s-th window in the body coordinate system b, is the rotation quantity of the second rotation state quantity correction value in the s+1-th window in the body coordinate system b, is the relative rotation quantity between the s-th window and the s+1-th window in the body coordinate system b, is the relative rotation quantity error between the s-th window and the s+1-th window.

[0103] Therefore, finally in the optimization, the corrected rotation state quantity can make the rotation quantity error converge more quickly, and when the minimum value is reached, the third rotation state quantity correction value can be obtained.

[0104] Step 7: When the unmanned aerial vehicle system detects a loop, a displacement graph optimization model is constructed, and the key frames associated displacement state quantity is input into the displacement graph optimization model to obtain the displacement state quantity correction value.

[0105] The displacement graph optimization model calculation formula is as follows:

[0106]

[0107] Wherein, are the displacement state quantities of the two consecutive key frames in the body coordinate system b, r i,j (·) is a residual error function, and h(·) is a loss function.

[0108] Wherein, the calculation formula of the residual error function is as follows:

[0109]

[0110] Wherein, are the Pitch, Roll and Yaw angles of the third rotation quantity state quantity correction value, respectively; R(·) is the matrix expression of the rotation quantity, is the key frame The relative displacement amount of the body coordinate system b between the k+1 moment.

[0111] The UAV system detects the loop back, which means that the second time runs to the same position. The moment when the first time reaches the position is called the loop back moment, and the moment when the second time reaches the position is called the current moment. At this time, the relative displacement state amount of all key frames between the two moments needs to be optimized. The displacement amount after optimization can obtain the displacement state amount correction value, and the third rotation state amount correction value and the displacement state amount correction value are used as the final state amount output of the UAV system.

[0112] The above only describes the preferred embodiments of the present application, and it should be pointed out that for ordinary skilled in the art, without departing from the principles of the present application, a number of improvements and refinements can be made, and these improvements and refinements should be considered as the protection scope of the present application.

Claims

1. A method for unmanned aerial vehicle light vision inertial positioning with fusion magnetometer, characterized in that: Comprising the following steps: Step 1: Obtain image data of a drone binocular camera, including left view right view and inertial measurement data, including: acceleration data, angular velocity data and geomagnetic field data; Step 2: Extract image features from the left view and right view of each frame, respectively. Step 3: Integrate the angular velocity data and the acceleration data to obtain displacement and rotation relative state variables; Step 4: Use the geomagnetic field data to calculate the geomagnetic yaw angle, extract the integral yaw angle from the rotation relative state variables, correct the yaw angle error using the geomagnetic yaw angle and the integral yaw angle, obtain the first yaw angle correction value, and obtain the first rotation state variable correction value according to the first yaw angle correction value; Step 5: Use the direct method to match the current time image features with the next time image features, correct the first rotation state variable correction value according to the matching result, and obtain the second rotation state variable correction value; Step 6: The second state quantity correction value is associated with the key frame and nonlinear optimization is performed to obtain a third rotation state quantity correction value. 2.The unmanned aerial vehicle light vision inertial positioning method with fused magnetometer, according to claim 1, wherein: Also comprising step 7: when the UAV system detects a loop, constructing a displacement graph optimization model, and solving the displacement graph optimization model to obtain a displacement state quantity correction value. The associated displacement state quantity is input into the displacement graph optimization model, and the displacement graph optimization model is solved to obtain a displacement state quantity correction value.

3. The unmanned aerial vehicle light vision-inertial positioning method of fusing magnetometers according to claim 1 or 2, characterized in that: The step 2 specifically comprises the following steps: Step 2.1: In the left view right view Up-sampling τ1 point features; Step 2.2: After bad points are removed, use line feature detection algorithm to extract τ2 line features in left view right view image τ2 line features Step 2.3: After removing the line features with a length less than the length threshold, the starting point and the ending point of the removed line features are added to the point features to obtain updated point features.

4. The unmanned aerial vehicle light vision-inertial positioning method of claim 3, wherein: Further comprising step 2.4: Step 2.4: When the updated point features are less than the number threshold, randomly select points on the image and add them to the updated point features.

5. The unmanned aerial vehicle light vision-inertial positioning method of fusing magnetometers according to claim 1 or 2, characterized in that: The displacement and rotation relative state variable calculation formula is as follows: where, are the displacement, rotation relative state quantities of the body coordinate system b at time k + 1 with respect to the body coordinate system b at time k, respectively, b a is the acceleration bias, b ω is the gyroscope bias, n a , n ω are the Gaussian white noise of the acceleration and angular velocity sensors, is the acceleration data measurement value, is the angular velocity data measurement value, is the matrix of the rotation quantity of the body coordinate system b at time t, is the quaternion of the rotation quantity of the body coordinate system b at time t, and Ω represents the conversion from a vector form to a quaternion form.

6. The unmanned aerial vehicle light vision-inertial positioning method of fusing magnetometers according to claim 5, characterized in that: The step 4 specifically comprises the following steps: Step 4.1: According to the geomagnetic field data, calculate the geomagnetic yaw angle Yaw in the rotation state variable, and the calculation formula is as follows: wherein: superscript b represents in the body coordinate system, superscript n represents in the world coordinate system, respectively represent the geomagnetic field in the x, y axis direction under the world coordinate system, respectively represent the geomagnetic field in the x, y, z axis direction under the body coordinate system, Pitch is the pitch angle, and Roll is the roll angle. Step 4.2: from Extracting the integral yaw angle from the rotational relative state vector Combining the geomagnetic yaw angle Yaw and the integral yaw angle Fusion using Kalman filtering method to obtain the first yaw angle correction value Step 4.3: Replacing the yaw angle in the rotation state quantity, a first rotation state quantity correction value is obtained. Replacing the yaw angle in the rotation state quantity, a first rotation state quantity correction value is obtained.

7. The unmanned aerial vehicle light vision-inertial positioning method of claim 6, wherein: The step 4.2 specifically comprises the following steps: Step 4.2.1: Integrate the yaw angle Let the prior estimate at the current time be denoted as Let the magnetic yaw angle Yaw be the observation at the current time, denoted as Yaw t ; Step 4.2.2: According to the prior estimate Observation Yaw t And state quantity weight K t Calculate the first yaw angle correction value at the current time The calculation formula is as follows: Wherein, the state quantity weight K t The calculation formula is as follows: wherein is the prior estimation covariance at the current time instant, H is the observation transition matrix, I is the identity matrix, H T is the transpose of H, and R is the observation noise variance; wherein The calculation formula is as follows: where P t-1 is the posterior covariance at the previous time step, F is the state transition matrix, is a constant, and Q is the process noise variance.

8. The unmanned aerial vehicle light vision inertial positioning method of fusing magnetometers according to claim 5, wherein: The step 6 specifically comprises the following steps: Step 6.1: Select the second rotation state correction value corresponding to the image pair whose two image frames have a disparity greater than τ3 pixels and whose number of common image features is more than τ4 as the key frame Step 6.2: Store the key frame into the window, and store τ5 windows successively; Step 6.3: Put all the second rotation state variable correction values in the window into the optimization model for optimization until the relative rotation error is minimized, and output the third rotation state variable correction value.

9. The unmanned aerial vehicle light vision-inertial positioning method of claim 8, wherein: The optimization model includes: visual residual, inertial residual, and edge prior information; the relative rotation error is calculated according to the inertial residual, and the calculation formula is as follows: wherein, is a rotation amount of the second rotation state amount correction value in the body coordinate system b in the s-th window, is a rotation amount of the second rotation state amount correction value in the body coordinate system b in the s+1-th window, is a relative rotation amount between the s-th window and the s+1-th window in the body coordinate system b, is a relative rotation amount error between the s-th window and the s+1-th window.

10. The unmanned aerial vehicle light vision inertial positioning method of fusing magnetometers according to claim 2, wherein: The displacement map optimization model calculation formula is as follows: wherein, are consecutive two key frames, respectively displacement state quantity in the body coordinate system b, r i,j (·) is a residual function, h(·) is a loss function.

11. The unmanned aerial vehicle light vision inertial positioning method of claim 10, wherein: The calculation formula of the residual function is as follows: wherein, Pitch, Roll, Yaw are the third-rotation-amount state-quantity correction values of the pitch angle, the roll angle, and the yaw angle, respectively; R(·) is a matrix expression of the rotation amount, is a key frame is the relative displacement amount of the body coordinate system b at the k+1 time.

Images