Unmanned aerial vehicle rear-end loopback detection method and device based on multi-sensor fusion extended Kalman filtering

Abstract

The invention discloses an unmanned aerial vehicle rear-end loopback detection method and device based on multi-sensor fusion extended Kalman filtering. The method comprises the following steps: constructing a linearization state equation and a linearization observation equation of a target unmanned aerial vehicle; collecting visual feature points and laser radar feature points; performing target unmanned aerial vehicle rear-end loopback detection based on the visual feature points and the laser radar feature points to obtain a visual matching degree and a laser radar matching degree; calculating a visual proportion coefficient and a laser radar proportion coefficient in the loopback detection fusion process; taking the linearized state equation and the linearized observation equation as initial pose states of the target unmanned aerial vehicle, and iteratively updating a Kalman filtering gain by using a visual proportion coefficient and a laser radar proportion coefficient; the pose state of the target unmanned aerial vehicle is corrected and updated according to the Kalman filtering gain, and back-end loopback detection is carried out on the target unmanned aerial vehicle again based on the pose state of the target unmanned aerial vehicle; according to the invention, the error of back-end loopback detection in the mapping process of the unmanned aerial vehicle is reduced.

Description

Technical Field

[0001] The present application relates to the field of multi-sensor information fusion, and in particular to a method and device for back-end loop detection of unmanned aerial vehicles (UAVs) based on multi-sensor fusion extended Kalman filtering. Background Art

[0002] In recent years, with the rapid development and widespread application of technologies such as drones, robots, and autonomous driving, unmanned systems (UAVs) have played an increasingly important role in daily life, industrial automation, and intelligent transportation. The autonomous navigation and mission execution capabilities of UAVs rely on accurate and reliable environmental perception, mapping, and positioning technologies. Environmental perception and modeling, as well as positioning and mapping, provide UAVs with information about their surroundings and their own status. These modules are the prerequisites for achieving complex autonomous control functions and are the core and foundation of UAV technology.

[0003] Traditional single-sensor perception technology only provides limited information and is subject to its own quality and performance limitations, making it unable to meet the requirements of high-precision positioning and mapping in complex scenarios. Using only one sensor for back-end loop detection can result in loop failures. For example, a loop may occur in the real environment but not be detected during mapping, or vice versa. Therefore, mapping and localization based on multi-sensor information fusion has become a hot topic and a major challenge in current research. Loop detection is crucial for the global consistency of SLAM (Simultaneous Localization and Mapping) systems. The changing environmental factors during sensor mapping significantly impact both binocular cameras and lidar. When performing back-end loop detection for lidar mapping and binocular visual mapping, factors such as lighting, weather, surface characteristics, and external interference can lead to inconsistencies in loop detection between the two, resulting in significant errors in back-end loop detection for drones. Summary of the Invention

[0004] The purpose of this application is to provide a UAV back-end loop detection method and device based on multi-sensor fusion extended Kalman filtering, which can reduce the error of back-end loop detection during the UAV mapping process.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] In the first aspect, the present application provides a method for loop closure detection of the back-end of a UAV based on a multi-sensor fusion extended Kalman filter, wherein the sensors include at least: a visual sensor and a lidar sensor, and the method for loop closure detection of the back-end of a UAV based on a multi-sensor fusion extended Kalman filter includes: constructing a linearized state equation and a linearized observation equation of the target UAV; collecting visual feature points and lidar feature points according to the visual posture state and lidar posture state of the target UAV respectively; the visual posture state is the posture state of the UAV obtained by the visual sensor during the mapping process; the lidar posture state is the posture state of the UAV obtained by the lidar sensor in the The posture state of the UAV is obtained during the mapping process; the back-end loop detection of the target UAV is performed based on the visual feature points and the lidar feature points to obtain the visual matching degree and the lidar matching degree; the visual matching degree and the lidar matching degree are used to calculate the visual ratio coefficient and the lidar ratio coefficient in the loop detection fusion process; the linearized state equation and the linearized observation equation are used as the initial posture state of the target UAV, and the Kalman filter gain is iteratively updated using the visual ratio coefficient and the lidar ratio coefficient; the posture state of the target UAV is corrected and updated according to the Kalman filter gain, and the back-end loop detection of the target UAV is re-performed based on the posture state of the target UAV.

[0007] In the second aspect, the present application provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above-mentioned drone back-end loop detection method based on multi-sensor fusion extended Kalman filtering.

[0008] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0009] This application calculates the visual proportion coefficient and the lidar proportion coefficient in the loop detection fusion process by using the visual matching degree and the lidar matching degree; and uses the linearized state equation and the linearized observation equation as the initial posture state of the target drone, and iteratively updates the Kalman filter gain by using the visual proportion coefficient and the lidar proportion coefficient; updates the target drone posture state according to the Kalman filter gain correction, and re-performs back-end loop detection on the target drone based on the target drone posture state. This application introduces adaptive variables, uses visual feature point matching and lidar feature point matching to change the adaptive variables to affect the weights of visual loop detection and lidar loop detection in the fusion process, thereby reducing the error of back-end loop detection in the drone mapping process. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] In order to more clearly illustrate the technical solutions in the embodiments of the present application or related technologies, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0011] Figure 1 A schematic diagram of a back-end loop detection method for drones based on multi-sensor fusion extended Kalman filtering provided in an embodiment of the present application Figure 1 .

[0012] Figure 2 A schematic diagram of a back-end loop detection method for drones based on multi-sensor fusion extended Kalman filtering provided in an embodiment of the present application Figure 2 .

[0013] Figure 3 The embodiment of the present application provides a laser radar sensor and a binocular vision sensor installed on the same target drone, which are fused to output a key frame trajectory map.

[0014] Figure 4 A schematic diagram of the structure of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0015] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0016] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0017] Example 1, as Figure 1 As shown, this embodiment provides a drone backend loop detection method based on multi-sensor fusion extended Kalman filtering, wherein the sensors include at least: a visual sensor and a lidar sensor, and the drone backend loop detection method based on multi-sensor fusion extended Kalman filtering includes:

[0018] S1. Construct the linearized state equation and linearized observation equation of the target UAV;

[0019] Step S1 specifically includes:

[0020] S11. Construct the state equation and observation equation of the target UAV;

[0021] S12. Linearize the state equation and the observation equation to obtain a linearized state equation and a linearized observation equation.

[0022] In actual application, the process of modeling the target drone is as follows:

[0023] Get the state vector of the target drone at the kth moment

[0024] x k =f(x k-1 ,u k )+ω k (1).

[0025] Among them, x k is the state vector at the kth moment, x k-1 is the state vector at the k-1th moment, u k is the control vector at the kth moment, ω k is the interference at the kth moment, and f is the functional relationship.

[0026] According to formula (1), the state transfer equation of the UAV motion model is expanded by the first-order Taylor:

[0027]

[0028] in, is the posterior estimate at the kth moment, Δf x is the Jacobian matrix of the state space model, and the Jacobian expression is:

[0029]

[0030] Therefore, the nonlinear state transfer equation can be approximately linearized into a linear state equation through equations (2) and (3):

[0031] x k ≈F x x k-1 +ω k (4).

[0032] Among them, F x =Δf x is the state transition matrix.

[0033] The observation equation of the Kalman filter is used to describe how the observation of the system is related to the true state of the system. k Get the actual observation data z k , and through the noise v k The relationship between the calibration and estimation of the observation model is defined as follows:

[0034] z k =h(x k ,v k )=h k (x k )+v k (5).

[0035] Among them, z k The observation vector at the kth moment, v k is the observation noise at the kth moment, H k is the observation matrix at the kth moment, and h is the functional relationship.

[0036] According to formula (5), the observation equation of the UAV motion model is expanded into a first-order Taylor expansion:

[0037]

[0038] in, is the prior estimate at time k, is the Jacobian matrix of the observation equation.

[0039] The Jacobian expression is:

[0040]

[0041] Therefore, the nonlinear observation equation can be approximately linearized into a linear observation equation through equations (6) and (7):

[0042] z k ≈H x x k-1 +v k (8).

[0043] Among them, H x =Δh x is the observation matrix.

[0044] According to formula (4), after linearizing the state transfer equation, the state estimation equation of the UAV system is established:

[0045]

[0046] in, Indicates that x is k The pose state estimation of the target UAV.

[0047] According to formula (8), after linearizing the observation equation, the observation estimation equation of the UAV system is established:

[0048]

[0049] in, It means that the estimated value of x at the k-1th moment is k Observation.

[0050] According to formula (9), the UAV system state estimation error is established:

[0051]

[0052] in, It represents the estimation error of time k to time k-1.

[0053] According to formula (10), the observation estimation error is established:

[0054]

[0055] in, It represents the observation error from moment k to moment k-1.

[0056] The uncertainty relationship between sensor states is associated by constructing a covariance matrix to describe the correlation between errors.

[0057] The state error covariance matrix is established by formula (11):

[0058]

[0059] Among them, P k|k-1 Represents the state error covariance matrix at time k-1.

[0060] Initialize the covariance matrix:

[0061]

[0062] The error covariance matrix is established by formula (12):

[0063]

[0064] Among them, S K represents the observation error covariance matrix.

[0065] From the above formula (10) and formula (15), the relationship between the observation error covariance matrix and the state error covariance matrix can be obtained as follows:

[0066]

[0067] Among them, R k is the observation noise covariance matrix, H k is the observation matrix.

[0068] According to formula (16), the Kalman filter gain is established.

[0069] S2. Collect visual feature points and lidar feature points based on the visual pose state and lidar pose state of the target drone, respectively; the visual pose state is the pose state of the drone obtained by the visual sensor during the mapping process; the lidar pose state is the pose state of the drone obtained by the lidar sensor during the mapping process;

[0070] S3. Perform back-end loop detection on the target drone based on visual feature points and lidar feature points to obtain visual matching and lidar matching;

[0071] Furthermore, step S3 specifically includes:

[0072] S31. Encode the visual feature points into bag-of-words vectors, filter them using TF-IDF (Term Frequency-Inverse Document Frequency), and then perform feature comparison to obtain the visual matching degree, which specifically includes:

[0073] S311. Perform visual vocabulary semantic representation on the visual feature points to obtain visual words.

[0074] S312. Use the bag-of-words model to convert visual words into bag-of-words vectors.

[0075] S313. Use the TF-IDF method to filter the bag-of-words vector to obtain the filtered bag-of-words vector.

[0076] S314. Based on the filtered bag-of-words vector, the image difference scores of the visual feature points and the historical visual feature points are scored, and the difference scores exceeding the threshold are selected as the visual matching degree.

[0077] Furthermore, the calculation formula of the image difference score is as follows:

[0078]

[0079] Among them, s(V A -V B ) is the visual feature point V A and historical visual feature points V B The difference score of V A is the visual feature point; V B is the historical visual feature point; is the number of words; V Ai Word i representing a visual feature point; V Bi The word i represents a historical visual feature point.

[0080] In actual applications, when the visual sensor builds a map, it constructs a bag-of-words model based on the feature points extracted by vision. The words in the bag-of-words model are then similarity calculated. After using the TF-IDF method, some words that are not very useful for loop detection are eliminated. The specific calculation and elimination process is as follows:

[0081] 1) Extract visual feature points and represent images with visual words.

[0082] 2) Build a bag-of-words model, where a large number of visual words form bag-of-words vectors.

[0083] 3) Use TF-IDE for weighted optimization.

[0084]

[0085] 4) For words whose TF-IDF weight is lower than a certain threshold, such as frequently occurring words or irrelevant words, they are removed from the bag-of-words model.

[0086] 5) For the visually extracted feature points of the first image A1, we can use a vector V A1 To represent it, for the j-th image A j We can use a vector V to extract the feature points of vision Aj To represent it, for the feature points visually extracted from the current image B we can use a vector V B To represent it, and then calculate the difference of the picture in a paradigm form to get the difference score s sj .

[0087] S32. Perform feature point matching analysis on the laser radar feature points and the historical laser radar feature points to obtain the laser radar matching degree.

[0088] Step S32 specifically includes:

[0089] S321. Perform feature point matching on the laser radar feature points and the historical laser radar feature points to obtain laser radar matching feature points.

[0090] S322. Calculate the laser radar matching degree using the number of laser radar matching feature points and the total number of laser radar feature points; the total number of laser radar feature points is the sum of the number of laser radar feature points and the number of historical laser radar feature points.

[0091] Furthermore, the calculation formula of the lidar matching degree is as follows:

[0092]

[0093] Where s jg is the laser radar matching degree, Q match Q is the feature point matching of the laser radar,jg is the number of lidar feature points, Q h，jg is the number of historical lidar feature points.

[0094] In actual application, lidar loop detection determines whether there may be a loop based on the current position information. If the current position and the historical position are within a certain range and there are historical trajectory points, they are matched with the corresponding key frames. If the number of matched feature points exceeds a certain percentage, the lidar feature point ratio s is obtained. jg .

[0095] S4. Use the visual matching degree and the lidar matching degree to calculate the visual proportion coefficient and the lidar proportion coefficient in the loop detection fusion process.

[0096] Furthermore, the calculation formulas for the visual ratio and the lidar ratio are as follows:

[0097]

[0098] Where λ k is the visual ratio; q is the laser radar ratio; s sj is the visual matching degree; s jg is the laser radar matching degree.

[0099] S5. Use the linearized state equation and the linearized observation equation as the initial pose state of the target UAV, and iteratively update the Kalman filter gain using the vision ratio and the lidar ratio.

[0100] Step S5 specifically includes:

[0101] S51. Use the linearized state equation and the linearized observation equation as the initial posture state of the target UAV.

[0102] S52. Calculate a first error function based on the linearized state equation, the linearized observation equation, and the visual pose state.

[0103] S53. Update the error covariance matrix and Kalman filter gain using the first error function and the visual ratio coefficient.

[0104] S54. Update the target UAV posture state according to the Kalman filter gain correction.

[0105] S55. Calculate a second error function using the target UAV posture state and the lidar posture state.

[0106] S56. Use the second error function and the lidar ratio coefficient to update the error covariance matrix and the Kalman filter gain.

[0107] In a specific implementation process, the calculation formula of the Kalman filter gain after two error function updates in step S56 is as follows:

[0108]

[0109] Among them, P k|k-1 represents the state error covariance matrix at time k-1, P k|k-1 / t265 represents the error covariance matrix of the visual sensor, P k|k-1 / laser Represents the error covariance matrix of the lidar sensor.

[0110] The error covariance matrix is calculated as follows:

[0111]

[0112] Among them, P k|k represents the state error covariance matrix at time k, P k|k-1 represents the state error covariance matrix at time k-1, H k is the observation matrix, is the Kalman filter gain.

[0113] S6. Update the target UAV's posture state according to the Kalman filter gain correction, and re-perform back-end loopback detection on the target UAV based on the target UAV's posture state.

[0114] In the specific implementation process, the following formula is used to correct and update the target UAV posture state according to the Kalman filter gain:

[0115]

[0116] in, Indicates that x is k The target UAV pose state estimation, represents the observation error at time k to time k-1, is the Kalman filter gain.

[0117] Optionally, step S6 further includes updating the posterior probability distribution.

[0118] The technical effects of this application are as follows:

[0119] This application first uses the target drone for modeling, and uses the extended Kalman filter as a premise, adding adaptive variables to the error covariance matrix to change the proportion of visual loop detection and lidar loop detection in the Kalman filter gain. By introducing adaptive variables, the adaptive variables are changed by using visual feature point matching and lidar feature point matching to affect the weights of visual loop detection and lidar loop detection in the fusion process, thereby reducing the error of back-end loop detection in the drone mapping process.

[0120] Example 2: This application also provides a computer device, which can be a server or a terminal, and its internal structure diagram can be as follows: Figure 4 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O) and a communication interface. The processor, memory and input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and computer program in the non-volatile storage medium. The database of the computer device is used to store processed data. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a method for back-end loop detection of a drone based on multi-sensor fusion extended Kalman filtering is implemented.

[0121] Those skilled in the art will understand that Figure 4 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0122] Those skilled in the art will understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, database or other media used in the embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory may include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0123] All actions of acquiring signals, information or data in this application are carried out in compliance with the relevant data protection laws and policies of the country where they are located and with the authorization given by the owner of the corresponding device.

[0124] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0125] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for back-end loop detection of unmanned aerial vehicles based on multi-sensor fusion extended Kalman filtering, wherein the sensors include at least: The visual sensor and the lidar sensor are characterized in that the UAV back-end loop detection method based on multi-sensor fusion extended Kalman filter includes: Construct the linearized state equation and linearized observation equation of the target UAV; Collect visual feature points and lidar feature points according to the visual pose state and lidar pose state of the target drone, respectively; the visual pose state is the pose state of the drone obtained by the visual sensor during the mapping process; the lidar pose state is the pose state of the drone obtained by the lidar sensor during the mapping process; Perform back-end loop detection on the target drone based on visual feature points and lidar feature points to obtain visual matching and lidar matching; The visual matching degree and the lidar matching degree are used to calculate the visual ratio and the lidar ratio in the loop detection fusion process; The linearized state equation and the linearized observation equation are used as the initial pose state of the target UAV, and the Kalman filter gain is iteratively updated using the visual ratio coefficient and the lidar ratio coefficient. The target UAV's posture state is updated according to the Kalman filter gain correction, and the target UAV is re-performed with back-end loop detection based on the target UAV's posture state.

2. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 1 is characterized in that: Construct the linearized state equation and linearized observation equation of the target UAV, including: Construct the state equation and observation equation of the target UAV; The state equation and the observation equation are linearized to obtain the linearized state equation and the linearized observation equation.

3. The UAV back-end loop detection method based on multi-sensor fusion extended Kalman filter according to claim 1 is characterized in that: Based on the visual feature points and lidar feature points, the target drone backend loop detection is performed to obtain the visual matching degree and lidar matching degree, which specifically includes: Encode the visual feature points into bag-of-words vectors, perform feature comparison after TF-IDF filtering, and obtain the visual matching degree; The lidar feature points are matched with the historical lidar feature points to obtain the lidar matching degree.

4. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 3 is characterized in that: The visual feature points are encoded into bag-of-words vectors, and feature comparison is performed after TF-IDF filtering to obtain the visual matching degree, which includes: Perform visual vocabulary semantic representation on visual feature points to obtain visual words; Use the bag-of-words model to convert visual words into bag-of-words vectors; Use the TF-IDF method to filter the word bag vector to obtain the filtered word bag vector; Based on the filtered bag-of-words vector, the image difference scores of visual feature points and historical visual feature points are scored, and the difference scores exceeding the threshold are selected as the visual matching degree.

5. The UAV back-end loop detection method based on multi-sensor fusion extended Kalman filter according to claim 4 is characterized in that: The calculation formula of the image difference score is as follows: Among them, s(V A -V B ) is the visual feature point V A and historical visual feature points V B The difference score of V A is the visual feature point; V B is the historical visual feature point; N is the number of words; V Ai Word i representing a visual feature point; V Bi The word i represents a historical visual feature point.

6. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 3 is characterized in that: Perform feature point matching analysis on the LiDAR feature points and historical LiDAR feature points to obtain the LiDAR matching degree, which includes: Perform feature point matching on the lidar feature points and the historical lidar feature points to obtain lidar matching feature points; The lidar matching degree is calculated using the number of lidar matching feature points and the total number of lidar feature points; the total number of lidar feature points is the sum of the number of lidar feature points and the number of historical lidar feature points.

7. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 1 is characterized in that: The calculation formula for the lidar matching degree is as follows: Where s jg is the laser radar matching degree, Q match Q is the feature point matching of the laser radar, jg is the number of lidar feature points, Q h，jg is the number of historical lidar feature points.

8. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 1 is characterized in that: The linearized state equation and the linearized observation equation are used as the initial pose state of the target UAV, and the Kalman filter gain is iteratively updated using the visual ratio coefficient and the lidar ratio coefficient. Specifically, The linearized state equation and the linearized observation equation are used as the initial posture state of the target UAV; Calculate a first error function based on the linearized state equation, the linearized observation equation, and the visual pose state; Update the error covariance matrix and Kalman filter gain using the first error function and the visual ratio coefficient; Update the target UAV's posture state based on the Kalman filter gain correction; Calculating a second error function using the target UAV pose state and the lidar pose state; The error covariance matrix and Kalman filter gain are updated using the second error function and the lidar ratio coefficient.

9. The UAV backend loop detection method based on multi-sensor fusion extended Kalman filter according to claim 1 is characterized in that: The calculation formulas for the visual ratio and the laser radar ratio are as follows: l q =1-λ k ; Where λ k is the visual ratio; q is the laser radar ratio; s sj is the visual matching degree; s jg is the laser radar matching degree.

10. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the UAV back-end loop detection method based on multi-sensor fusion extended Kalman filtering according to any one of claims 1 to 9.

Images