High-precision indoor positioning method for UAV based on monocular camera, IMU and UWB multi-sensor fusion

Abstract

The present invention relates to a method for high-precision indoor positioning of unmanned aerial vehicles based on the fusion of monocular camera, IMU and UWB multi-sensor, comprising: arranging multiple ultra-wideband UWB anchor points in an indoor environment; calibrating the monocular camera and the ultra-wideband UWB anchor points; the flying unmanned aerial vehicle sends the video frame information recorded by the monocular camera, the ranging information of the ultra-wideband UWB, and the acceleration and angular velocity information recorded by the inertial measurement unit IMU to a server, and the server pre-processes the three received data; based on the result of the data pre-processing, a hybrid coupling optimization method is used to achieve high-precision positioning of the unmanned aerial vehicle. The present invention avoids the influence of the multipath effect of the indoor environment on the ultra-wideband UWB positioning, improves the accuracy of the ultra-wideband UWB positioning, designs a hybrid coupling collaborative positioning network framework to fuse multiple sensors such as the monocular camera, the inertial measurement unit IMU and the ultra-wideband UWB, and achieves high-precision positioning of the unmanned aerial vehicle in a complex indoor environment.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) positioning in complex indoor environments, and in particular to a high-precision indoor positioning method for UAV based on the fusion of a monocular camera, an IMU and a UWB multi-sensor. Background Art

[0002] Drones have been widely used in many fields such as civil applications, target tracking, search and rescue, industrial inspection, etc., showing great potential. In indoor applications, the real-time positioning of drones is a key component of guidance, navigation and control systems. Almost all drones are equipped with global navigation satellite system (GNSS) receivers for outdoor positioning. However, GNSS is unreliable or even unavailable in indoor environments due to signal attenuation caused by structures such as buildings and walls. Therefore, in the past decade, extensive research has been carried out in the field of indoor positioning of drones. Researchers have overcome the complexity of indoor environments and the limitations of GNSS unavailability and proposed various innovative positioning methods and technologies, such as WiFi, RFID, Bluetooth, lidar, inertial navigation, etc.

[0003] Among them, the visual inertial positioning method represented by the monocular camera and the inertial measurement unit (IMU) has high positioning accuracy, but the drift is very serious; the positioning method based on ultra-wideband (UWB) has high positioning accuracy, but there is also the influence of multipath effects in complex indoor environments, which will lead to a weakening of positioning accuracy. Therefore, it is urgent to develop a multi-sensor fusion positioning method. Summary of the invention

[0004] In order to solve the problems of serious drift and weak positioning accuracy in the prior art, the purpose of the present invention is to provide a high-precision indoor positioning method for unmanned aerial vehicles based on the fusion of monocular cameras, IMUs and UWB sensors, which realizes the complementary advantages among monocular cameras, inertial measurement units (IMUs) and ultra-wideband (UWB), achieves hybrid coupling between different data information, and has high positioning accuracy.

[0005] To achieve the above object, the present invention adopts the following technical solution: a high-precision indoor positioning method for UAV based on monocular camera, IMU and UWB multi-sensor fusion, the method comprising the following steps in order:

[0006] (1) Offline stage: multiple UWB anchor points are deployed in the indoor environment, and the positions of the UWB anchor points in the room (x i ,y i ,z i );

[0007] (2) Sensor calibration phase: calibrate the monocular camera to obtain the intrinsic parameter matrix K of the monocular camera; calibrate the ultra-wideband (UWB) anchor point to obtain the scale factor and deviation value of each ultra-wideband (UWB) anchor point;

[0008] (3) Data preprocessing stage: The flying drone sends the video frame information recorded by the monocular camera, the ranging information of the ultra-wideband UWB, and the acceleration and angular velocity information recorded by the inertial measurement unit IMU to the server. The server preprocesses the three types of data received and calculates the position of the ultra-wideband UWB anchor point in the room (x i ,y i ,z i ), obtain ultra-wideband UWB global positioning information, and use the dynamic random sampling consistency method to denoise its position estimate; extract Harris feature points from the video frame, judge the motion between two adjacent frames through the Harris feature point information, and obtain visual tracking information; pre-integrate the two adjacent frames through the acceleration and angular velocity information recorded by the inertial measurement unit IMU to obtain IMU pre-integration information;

[0009] (4) High-precision positioning stage of UAV: ​​Based on the results of data preprocessing, a hybrid coupling optimization method is used to achieve high-precision positioning of the UAV. First, a tight coupling method based on nonlinear optimization is used to obtain the six-degree-of-freedom information of the position and posture of the UAV in adjacent moments using the Harris feature point information, ultra-wideband UWB global positioning information between adjacent moments, and IMU pre-integration information. Then, a loose coupling method based on particle filtering is used to obtain the high-precision positioning information at each moment for positioning and tracking of the UAV.

[0010] In step (1), the multiple ultra-wideband (UWB) anchor points are on different straight lines.

[0011] The step (2) specifically comprises the following steps:

[0012] (2a) Calibrate the monocular camera: For a three-dimensional feature point P(X,Y,Z) in the world coordinate system, project this three-dimensional feature point P(X,Y,Z) onto the normalized imaging plane to obtain P'(X',Y',1):

[0013]

[0014] Where f is the focal length of the monocular camera. Suppose a pixel plane ouv is fixed on the physical plane, and the pixel coordinates of P' are obtained on the pixel plane: [u, v] T; The definition of the pixel coordinate system is: the origin o' is located in the upper left corner of the image, the u axis is parallel to the x axis to the right, and the v axis is parallel to the y axis downward; the difference between the pixel coordinate system and the imaging plane is a scaling and a translation of the origin; suppose the pixel coordinate is scaled by α times on the u axis and β times on the v axis, and the origin is translated by [c x ,c y ] T , then the coordinates of P' are the same as the pixel coordinates [u,v] T The relationship between them is:

[0015]

[0016] Substitute equation (1) into equation (2), and then combine αf into f x , merge βf into f y ,get:

[0017]

[0018] Among them, the unit of f is meter, and the unit of α and β is pixel / meter, so f x 、f y 、c x 、c y The unit is pixel, and formula (3) is written in matrix form:

[0019]

[0020] In formula (4), It is called the camera's intrinsic parameter matrix K;

[0021] (2b) Calibrate the UWB anchor point: Calibrate the measured data (x1, x2, x3, ... x m ) and the recorded real data (y1,y2,y3,...y m ), use the least squares algorithm to calculate the current ultra-wideband UWB anchor point U i The scaling factor a i and deviation value b i , obtain the scale factor and deviation value of all ultra-wideband UWB anchor points; the measurement data refers to each ultra-wideband UWB anchor point U i The measured distance between the ultra-wideband UWB tag and the real data refers to each ultra-wideband UWB anchor point U i The actual distance to the ultra-wideband UWB tag.

[0022] The step (3) specifically comprises the following steps:

[0023] (3a) Processing the video frame information: Using the Harris algorithm to extract corner points of the video frames transmitted to the server, and using the KLT optical flow algorithm to track the corner points, the movement direction and speed of the image pixels corresponding to the objects in the two frames are obtained, and the visual tracking information is obtained;

[0024] (3b) Processing the acceleration and angular velocity information recorded by the inertial measurement unit (IMU): using the pre-integration method to calculate the change in position measured by the inertial measurement unit (IMU) in two adjacent video frames Changes in speed Changes in attitude angle They are:

[0025]

[0026]

[0027]

[0028] in, It is the distance from time t to time b in the IMU coordinate system. k The rotation matrix at time; and n a are the measured value, zero bias value and Gaussian white noise of the accelerometer respectively; w t 、b wi 、n w They are the gyroscope’s measurement value, zero bias value, and Gaussian white noise; Indicates the gyroscope from time t to time b in the IMU coordinate system k The change of time; Ω represents the calculated angular velocity written in the form of a rotation matrix;

[0029] (3c) Processing the ranging information of the ultra-wideband UWB: Using the adaptive covariance weighted least squares method, assuming that the position of the ultra-wideband UWB anchor point (x i ,y i ,z i ), the position of the UWB tag on the drone is (x, y, z), and the distance between the drone tag to be located and the UWB anchor point is: Therefore, there is the following equation:

[0030]

[0031] Use the least squares method to estimate the unknown position: Aγ=b, where:

[0032]

[0033]

[0034]

[0035] Least squares estimation: γ=(A T A) -1 A T b, define the distance from the least square point at the current time t to the ultra-wideband UWB anchor point as m t , the distance from the tag point to the ultra-wideband UWB anchor point at time t is d t Now construct the absolute value L of the difference between the estimated 3D position and the distance between the ultra-wideband UWB anchor point i and the measured distance i :{|m1-d1|,|m2-d2|,……,|m t -d t |}, the sample L at the current time t i The variance of i , construct the adaptive covariance weight matrix at the current time t:

[0036]

[0037] Get the weighted position estimate: for ultra-wideband position estimation;

[0038] The use of the dynamic random sampling consistency method to denoise its position estimate specifically refers to:

[0039] For the position estimation of ultra-wideband UWB, M continuous positions are selected to form a short-term straight line trajectory using the measurement frequency of the ultra-wideband sensor; the dynamic random sampling consistency method establishes a corresponding straight line model based on these M positions:

[0040] (3c1) Randomly select two sample points to fit the straight line model;

[0041] (3c2) Calculate the error between all data samples and the straight line model; samples with fitting errors less than a preset threshold are considered as inliers;

[0042] (3c3) Repeat steps (3c1) and (3c2) to obtain different straight line models until 1000 straight line models are obtained;

[0043] (3c4) evaluating the quality of each straight line model according to the number of inner points of the straight line model, and determining the straight line model with the largest number of inner points as the optimal local model;

[0044] (3c5) Each position estimation point is evaluated M times through the optimal local model and its corresponding inliers, and a confidence value is assigned according to the number of times it is evaluated as an inlier, which is used for the decision of the subsequent position estimation;

[0045] When using ultra-wideband UWB for positioning at different times, it is necessary to recalculate the weight matrix and use a dynamic random sampling consistency algorithm to eliminate outliers.

[0046] The step (4) specifically comprises the following steps:

[0047] (4a) A hybrid coupling optimization method is used to perform high-precision UAV positioning in a complex indoor environment. The hybrid coupling optimization method consists of a tight coupling method based on nonlinear optimization and a loose coupling method based on particle filtering. First, the tight coupling method based on nonlinear optimization is used to combine visual tracking information and inertial measurement unit (IMU) pre-integration information into a visual-inertial residual E. VI :

[0048]

[0049] where ρ(·) represents the Huber norm, ||r p -H p X VI || 2 represents a nonlinear least squares problem, r p and H p represents marginalized prior information; X VI Represents visual inertial information; and Represent the residual terms of vision and inertia respectively; and Represent the covariance matrices of the visual and inertial measurement unit IMU respectively;

[0050] (4b) At time k, the positioning information between adjacent UWB times is used to construct the ultra-wideband UWB ranging information residual:

[0051]

[0052] in, and Respectively represent the position of the UAV in the global coordinate system and the camera coordinate system; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U Represents the measurement covariance matrix of ultra-wideband UWB;

[0053] (4c) Through the visual inertial residual term E VI And the residual term of ultra-wideband UWB ranging information, the total residual of all sensors at each moment is obtained:

[0054] E=E VI +E U

[0055] The Cracked Berger-Marquardt algorithm is used to solve the six-degree-of-freedom pose transformation of the UAV at every moment in a complex indoor environment.

[0056] (4d) Using the six degrees of freedom and ultra-wideband UWB global positioning information, a loosely coupled method based on particle filtering is used for high-precision UAV positioning. The first step is to initialize the particles and generate a random position (x i ,y i ,z i ), random direction q i , random weight ρ i The particle set is denoted as P = {P1, P2, P3, ..., P N}, where P i =(x i ,y i ,z i ,q i ,ρ i );

[0057] (4e) The movement of the particle from the current moment to the next moment depends on the six degrees of freedom drive of the drone calculated above:

[0058]

[0059] in, is the state of the jth particle in the particle set at the current moment, is the state of the jth particle at the next moment, is the six-degree-of-freedom transformation from the kth moment to the k+1th moment;

[0060] The particle weight update strategy is as follows:

[0061]

[0062]

[0063] Among them, d k+1,j is the current position of particle j at time k+1 and ultra-wideband UWB for global positioning of drones The Euclidean distance is used to assign different weights to particles according to the distance range.

[0064] (4f) The particle resampling strategy is: first normalize the weights of all particles:

[0065]

[0066] In the formula, ρ k+1,j is the weight of the jth particle before normalization, is the weight of the jth particle after normalization, and N is the total number of particles;

[0067] Randomly generate a number m∈[0,1) such that 1≤j<n w j ≤m<∑ 1≤j<n+1 w j ; Generate a random position satisfying |(x n ,y n ,z n )-L VI |+|(x n ,y n ,z n )-L U |≤2D, the new particle is X n , X n =(x n ,y n ,z n ,q n ,ρ n ), where L VI , L U They are the position of the UAV positioned by the visual inertial odometer and the position of the UWB anchor point, and D is the maximum position error set;

[0068] Repeat steps (4d) to (4f) until the required number of resampled particles are generated, and then proceed to step (4g);

[0069] (4g) In the final stage of estimating the position of the UAV in the indoor environment, the position of the weighted particles is used to obtain the position estimate of the UAV, that is, the positioning in the global coordinate system:

[0070]

[0071] In the formula, (x n ,y n ,z n ) is the particle position, ρ n is the particle weight, and (x, y, z) is the position of the drone in the indoor global coordinate system.

[0072] It can be seen from the above technical scheme that the beneficial effects of the present invention are as follows: first, the present invention avoids the influence of the multipath effect of the indoor environment on the ultra-wideband UWB positioning, improves the accuracy of the ultra-wideband UWB positioning, designs a hybrid coupling collaborative positioning network framework to integrate a monocular camera, an inertial measurement unit IMU and a variety of ultra-wideband UWB sensors, and realizes high-precision positioning of the UAV in a complex indoor environment; second, the present invention realizes the complementary advantages between a variety of sensors such as a monocular camera, an inertial measurement unit IMU and an ultra-wideband UWB sensor, realizes hybrid coupling between data, and further improves the positioning accuracy compared to the single coupling case, which can greatly reduce the range of errors and improve the reliability of the positioning of the UAV; third, the present invention has a wide range of uses, is easy to deploy, has high positioning accuracy, can achieve centimeter-level positioning, and uses sensors with low cost and is easy to promote; fourth, the present invention works in a complex indoor environment, such as a GPS-deficient area without texture, low light, and many obstacles, and can still provide a positioning result with high credibility when a certain sensor fails; fifth, while obtaining high-precision positioning, the present invention has a low algorithm complexity, can be implemented on a resource-constrained UAV, and can achieve real-time positioning performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 This is the monocular camera calibration map. DETAILED DESCRIPTION

[0074] like Figure 1 As shown, a high-precision indoor positioning method for UAV based on monocular camera, IMU and UWB multi-sensor fusion, the method includes the following steps in sequence:

[0075] (1) Offline stage: multiple UWB anchor points are deployed in the indoor environment, and the positions of the UWB anchor points in the room (x i ,y i ,z i );

[0076] (2) Sensor calibration phase: calibrate the monocular camera to obtain the intrinsic parameter matrix K of the monocular camera; calibrate the ultra-wideband (UWB) anchor point to obtain the scale factor and deviation value of each ultra-wideband (UWB) anchor point;

[0077] (3) Data preprocessing stage: The flying drone sends the video frame information recorded by the monocular camera, the ranging information of the ultra-wideband UWB, and the acceleration and angular velocity information recorded by the inertial measurement unit IMU to the server. The server preprocesses the three types of data received and calculates the position of the ultra-wideband UWB anchor point in the room (x i ,y i ,z i), obtain ultra-wideband UWB global positioning information, and use the dynamic random sampling consistency method to denoise its position estimate; extract Harris feature points from the video frame, judge the motion between two adjacent frames through the Harris feature point information, and obtain visual tracking information; pre-integrate the two adjacent frames through the acceleration and angular velocity information recorded by the inertial measurement unit IMU to obtain IMU pre-integration information;

[0078] (4) High-precision positioning stage of UAV: ​​Based on the results of data preprocessing, a hybrid coupling optimization method is used to achieve high-precision positioning of the UAV. First, a tight coupling method based on nonlinear optimization is used to obtain the six-degree-of-freedom information of the position and posture of the UAV in adjacent moments using the Harris feature point information, ultra-wideband UWB global positioning information between adjacent moments, and IMU pre-integration information. Then, a loose coupling method based on particle filtering is used to obtain the high-precision positioning information at each moment for positioning and tracking of the UAV.

[0079] In step (1), the multiple ultra-wideband (UWB) anchor points are on different straight lines.

[0080] The step (2) specifically comprises the following steps:

[0081] (2a) Calibrate the monocular camera: Figure 1 As shown, for a three-dimensional feature point P(X,Y,Z) in a world coordinate system, project this three-dimensional feature point P(X,Y,Z) onto the normalized imaging plane to obtain P'(X',Y',1):

[0082]

[0083] Where f is the focal length of the monocular camera. Suppose a pixel plane ouv is fixed on the physical plane, and the pixel coordinates of P' are obtained on the pixel plane: [u, v] T ; The definition of the pixel coordinate system is: the origin o' is located in the upper left corner of the image, the u axis is parallel to the x axis to the right, and the v axis is parallel to the y axis downward; the difference between the pixel coordinate system and the imaging plane is a scaling and a translation of the origin; suppose the pixel coordinate is scaled by α times on the u axis and β times on the v axis, and the origin is translated by [c x ,c y ] T , then the coordinates of P' are the same as the pixel coordinates [u,v] T The relationship between them is:

[0084]

[0085] Substitute equation (1) into equation (2), and then combine αf into f x, merge βf into f y ,get:

[0086]

[0087] Among them, the unit of f is meter, and the unit of α and β is pixel / meter, so f x 、f y 、c x 、c y The unit is pixel, and formula (3) is written in matrix form:

[0088]

[0089] In formula (4), It is called the camera's intrinsic parameter matrix K;

[0090] (2b) Calibrate the UWB anchor point: Calibrate the measured data (x1, x2, x3, ... x m ) and the recorded real data (y1,y2,y3,...y m ), use the least squares algorithm to calculate the current ultra-wideband UWB anchor point U i The scaling factor a i and deviation value b i , obtain the scale factor and deviation value of all ultra-wideband UWB anchor points; the measurement data refers to each ultra-wideband UWB anchor point U i The measured distance between the ultra-wideband UWB tag and the real data refers to each ultra-wideband UWB anchor point U i The actual distance to the ultra-wideband UWB tag.

[0091] The step (3) specifically comprises the following steps:

[0092] (3a) Processing the video frame information: Using the Harris algorithm to extract corner points of the video frames transmitted to the server, and using the KLT optical flow algorithm to track the corner points, the movement direction and speed of the image pixels corresponding to the objects in the two frames are obtained, and the visual tracking information is obtained;

[0093] (3b) Processing the acceleration and angular velocity information recorded by the inertial measurement unit (IMU): using the pre-integration method to calculate the change in position measured by the inertial measurement unit (IMU) in two adjacent video frames Changes in speed Changes in attitude angle They are:

[0094]

[0095]

[0096]

[0097] in, It is the distance from time t to time b in the IMU coordinate system. k The rotation matrix at time; and n a They are the measurement value, zero bias value and Gaussian white noise of the accelerometer respectively; They are the gyroscope’s measurement value, zero bias value, and Gaussian white noise; Indicates the gyroscope from time t to time b in the IMU coordinate system k The change of time; Ω represents the calculated angular velocity written in the form of a rotation matrix;

[0098] (3c) Processing the ranging information of the ultra-wideband UWB: Using the adaptive covariance weighted least squares method, assuming that the position of the ultra-wideband UWB anchor point (x i ,y i ,z i ), the position of the UWB tag on the drone is (x, y, z), and the distance between the drone tag to be located and the UWB anchor point is: Therefore, there is the following equation:

[0099]

[0100] Use the least squares method to estimate the unknown position: Aγ=b, where:

[0101]

[0102]

[0103]

[0104] Least squares estimation: γ=(A T A) -1 A T b, define the distance from the least square point at the current time t to the ultra-wideband UWB anchor point as m t , the distance from the tag point to the ultra-wideband UWB anchor point at time t is d t Now construct the absolute value L of the difference between the estimated 3D position and the distance between the ultra-wideband UWB anchor point i and the measured distance i :{|m1-d1|,|m2-d2|,……,|m t -d t |}, the sample L at the current time t i The variance of i , construct the adaptive covariance weight matrix at the current time t:

[0105]

[0106] Get the weighted position estimate: for ultra-wideband position estimation;

[0107] The use of the dynamic random sampling consistency method to denoise its position estimate specifically refers to:

[0108] For the position estimation of ultra-wideband UWB, M continuous positions are selected to form a short-term straight line trajectory using the measurement frequency of the ultra-wideband sensor; the dynamic random sampling consistency method establishes a corresponding straight line model based on these M positions:

[0109] (3c1) Randomly select two sample points to fit the straight line model;

[0110] (3c2) Calculate the error between all data samples and the straight line model; samples with fitting errors less than a preset threshold are considered as inliers;

[0111] (3c3) Repeat steps (3c1) and (3c2) to obtain different straight line models until 1000 straight line models are obtained;

[0112] (3c4) evaluating the quality of each straight line model according to the number of inner points of the straight line model, and determining the straight line model with the largest number of inner points as the optimal local model;

[0113] (3c5) Each position estimation point is evaluated M times through the optimal local model and its corresponding inliers, and a confidence value is assigned according to the number of times it is evaluated as an inlier, which is used for the decision of the subsequent position estimation;

[0114] When using ultra-wideband UWB for positioning at different times, it is necessary to recalculate the weight matrix and use a dynamic random sampling consistency algorithm to eliminate outliers.

[0115] The step (4) specifically comprises the following steps:

[0116] (4a) A hybrid coupling optimization method is used to perform high-precision UAV positioning in a complex indoor environment. The hybrid coupling optimization method consists of a tight coupling method based on nonlinear optimization and a loose coupling method based on particle filtering. First, the tight coupling method based on nonlinear optimization is used to combine visual tracking information and inertial measurement unit (IMU) pre-integration information into a visual-inertial residual E. VI :

[0117]

[0118] where ρ(·) represents the Huber norm, ||rp -H p X VI || 2 represents a nonlinear least squares problem, r p and H p represents marginalized prior information; X VI Represents visual inertial information; and Represent the residual terms of vision and inertia respectively; and Represent the covariance matrices of the visual and inertial measurement unit IMU respectively;

[0119] (4b) At time k, the positioning information between adjacent UWB times is used to construct the ultra-wideband UWB ranging information residual:

[0120]

[0121] in, and Respectively represent the position of the UAV in the global coordinate system and the camera coordinate system; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U Represents the measurement covariance matrix of ultra-wideband UWB;

[0122] (4c) Through the visual inertial residual term E VI And the residual term of ultra-wideband UWB ranging information, the total residual of all sensors at each moment is obtained:

[0123] E=E VI +E U

[0124] The Cracked Berger-Marquardt algorithm is used to solve the six-degree-of-freedom pose transformation of the UAV at every moment in a complex indoor environment.

[0125] (4d) Using the six degrees of freedom and ultra-wideband UWB global positioning information, a loosely coupled method based on particle filtering is used for high-precision UAV positioning. The first step is to initialize the particles and generate a random position (x i ,y i ,z i ), random direction q i , random weight ρ i The particle set is denoted as P = {P1, P2, P3, ..., P N}, where P i =(x i ,y i ,z i ,q i ,ρ i );

[0126] (4e) The movement of the particle from the current moment to the next moment depends on the six degrees of freedom drive of the drone calculated above:

[0127]

[0128] in, is the state of the jth particle in the particle set at the current moment, is the state of the jth particle at the next moment, is the six-degree-of-freedom transformation from the kth moment to the k+1th moment;

[0129] The particle weight update strategy is as follows:

[0130]

[0131] ρ k+1,j =1 / d k+1,j

[0132] Among them, d k+1,j is the current position of particle j at time k+1 and ultra-wideband UWB for global positioning of drones The Euclidean distance is used to assign different weights to particles according to the distance range.

[0133] (4f) The particle resampling strategy is: first normalize the weights of all particles:

[0134]

[0135] In the formula, ρ k+1,j is the weight of the jth particle before normalization, is the weight of the jth particle after normalization, and N is the total number of particles;

[0136] Randomly generate a number m∈[0,1) such that 1≤j<n w j ≤m<∑ 1≤j<n+1 w j ; Generate a random position satisfying |(x n ,y n ,z n )-L VI |+|(x n ,y n ,z n )-L U |≤2D, the new particle is X n , X n =(x n ,y n ,z n ,q n ,ρn ), where L VI , L U They are the position of the UAV positioned by the visual inertial odometer and the position of the UWB anchor point, and D is the maximum position error set;

[0137] Repeat steps (4d) to (4f) until the required number of resampled particles are generated, and then proceed to step (4g);

[0138] (4g) In the final stage of estimating the position of the UAV in the indoor environment, the position of the weighted particles is used to obtain the position estimate of the UAV, that is, the positioning in the global coordinate system:

[0139]

[0140] In the formula, (x n ,y n ,z n ) is the particle position, ρ n is the particle weight, and (x, y, z) is the position of the drone in the indoor global coordinate system.

[0141] In summary, the present invention avoids the influence of the multipath effect of the indoor environment on the ultra-wideband UWB positioning, improves the accuracy of the ultra-wideband UWB positioning, designs a hybrid coupling collaborative positioning network framework to integrate a monocular camera, an inertial measurement unit IMU and an ultra-wideband UWB sensor to realize high-precision positioning of the UAV in a complex indoor environment; the present invention realizes the complementary advantages between the monocular camera, the inertial measurement unit IMU and the ultra-wideband UWB sensor, realizes the hybrid coupling between the data, and further improves the positioning accuracy compared with the single coupling case, can greatly reduce the range of the error, and improves the reliability of the positioning of the UAV; the present invention has a wide range of use, is easy to deploy, has high positioning accuracy, can achieve centimeter-level positioning, and uses sensors with low cost and is easy to promote; the present invention works in a complex indoor environment, such as indoor GPS-deficient areas without texture, low light, and many obstacles, and can still provide a positioning result with high credibility when a certain sensor fails; while obtaining high-precision positioning, the present invention has a low algorithm complexity, can be implemented on a resource-constrained UAV, and can achieve real-time positioning performance.

Claims

1. A high-precision indoor positioning method for UAV based on monocular camera, IMU and UWB multi-sensor fusion, characterized by: The method comprises the following steps in order: (1) Offline stage: multiple UWB anchor points are deployed in the indoor environment, and the positions of the UWB anchor points in the room (x i ,y i ,z i ); (2) Sensor calibration phase: calibrate the monocular camera to obtain the intrinsic parameter matrix K of the monocular camera; calibrate the ultra-wideband (UWB) anchor point to obtain the scale factor and deviation value of each ultra-wideband (UWB) anchor point; (3) Data preprocessing stage: The flying drone sends the video frame information recorded by the monocular camera, the ranging information of the ultra-wideband UWB, and the acceleration and angular velocity information recorded by the inertial measurement unit IMU to the server. The server preprocesses the three types of data received and calculates the position of the ultra-wideband UWB anchor point in the room (x i ,y i ,z i ), obtain ultra-wideband UWB global positioning information, and use the dynamic random sampling consistency method to denoise its position estimate; extract Harris feature points from video frames, determine the motion between two adjacent frames through Harris feature point information, and obtain visual tracking information; Pre-integrate two adjacent frames using the acceleration and angular velocity information recorded by the inertial measurement unit (IMU) to obtain IMU pre-integration information; (4) UAV high-precision positioning stage: Based on the results of data preprocessing, a hybrid coupling optimization method is used to achieve high-precision positioning of the UAV. First, the Harris feature point information, the ultra-wideband UWB global positioning information between adjacent moments, and the IMU pre-integration information are tightly coupled using a nonlinear optimization method to obtain the six-degree-of-freedom information of the UAV's position and attitude at adjacent moments. Then, a loose coupling method based on particle filtering is used to obtain the high-precision positioning information at each moment for positioning and tracking of the UAV. The use of the dynamic random sampling consistency method to denoise its position estimate specifically refers to: For the position estimation of ultra-wideband UWB, M continuous positions are selected to form a short-term straight line trajectory using the measurement frequency of the ultra-wideband sensor; the dynamic random sampling consistency method establishes a corresponding straight line model based on these M positions: (3c1) Randomly select two sample points to fit the straight line model; (3c2) Calculate the error between all data samples and the straight line model; samples with fitting errors less than a preset threshold are considered as inliers; (3c3) Repeat steps (3c1) and (3c2) to obtain different straight line models until 1000 straight line models are obtained; (3c4) evaluating the quality of each straight line model according to the number of inner points of the straight line model, and determining the straight line model with the largest number of inner points as the optimal local model; (3c5) Each position estimation point is evaluated M times through the optimal local model and its corresponding inliers, and a confidence value is assigned according to the number of times it is evaluated as an inlier, which is used for the decision of the subsequent position estimation; When using ultra-wideband UWB for positioning at different times, it is necessary to recalculate the weight matrix and use a dynamic random sampling consensus algorithm to remove outliers; The step (4) specifically comprises the following steps: (4a) A hybrid coupling optimization method is used to perform high-precision UAV positioning in a complex indoor environment. The hybrid coupling optimization method consists of a tight coupling method based on nonlinear optimization and a loose coupling method based on particle filtering. First, the tight coupling method based on nonlinear optimization is used to combine visual tracking information and inertial measurement unit (IMU) pre-integration information into a visual-inertial residual E. VI : where ρ(·) represents the Huber norm, ||r p -H p X VI || 2 represents a nonlinear least squares problem, r p and H p represents marginalized prior information; X VI Represents visual inertial information; and Represent the residual terms of vision and inertia respectively; and Represent the covariance matrices of the visual and inertial measurement unit IMU respectively; (4b) At time k, the positioning information between adjacent UWB times is used to construct the ultra-wideband UWB ranging information residual: in, and Respectively represent the position of the UAV in the global coordinate system and the camera coordinate system; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U Represents the measurement covariance matrix of ultra-wideband UWB; (4c) Through the visual inertial residual term E VI And the residual term of ultra-wideband UWB ranging information, the total residual of all sensors at each moment is obtained: E=E VI +E U The Cracked Berger-Marquardt algorithm is used to solve the six-degree-of-freedom pose transformation of the UAV at every moment in a complex indoor environment. (4d) Using the six degrees of freedom and ultra-wideband UWB global positioning information, a loosely coupled method based on particle filtering is used for high-precision UAV positioning. The first step is to initialize the particles and generate a random position (x i ,y i ,z i ), random direction q i , random weight ρ i The particle set is denoted as P = {P1, P2, P3, ..., P N }, where P i =(x i ,y i ,z i ,q i ,ρ i ); (4e) The movement of the particle from the current moment to the next moment depends on the six degrees of freedom drive of the drone calculated above: in, is the state of the jth particle in the particle set at the current moment, is the state of the jth particle at the next moment, is the six-degree-of-freedom transformation from the kth moment to the k+1th moment; The particle weight update strategy is as follows: r k+1,j =1 / d k+1,j Among them, d k+1,j is the current position of particle j at time k+1 and ultra-wideband UWB for global positioning of drones The Euclidean distance is used to assign different weights to particles according to the distance range. (4f) The particle resampling strategy is: first normalize the weights of all particles: In the formula, ρ k+1,j is the weight of the jth particle before normalization, is the weight of the jth particle after normalization, and N is the total number of particles; Randomly generate a number m∈[0,1) such that 1≤j＜n w j ≤m<∑ 1≤j＜n+1 w j ; Generate a random position satisfying |(x n ,y n ,z n )-L VI |+|(x n ,y n ,z n )-L U |≤2D, the new particle is X n , X n =(x n ,y n ,z n ,q n ,ρ n ), where L VI , L U They are the position of the UAV positioned by the visual inertial odometer and the position of the UWB anchor point, and D is the maximum position error set; Repeat steps (4d) to (4f) until the required number of resampled particles are generated, and then proceed to step (4g); (4g) In the final stage of estimating the position of the UAV in the indoor environment, the position of the weighted particles is used to obtain the position estimate of the UAV, that is, the positioning in the global coordinate system: In the formula, (x n ,y n ,z n ) is the particle position, ρ n is the particle weight, and (x, y, z) is the position of the drone in the indoor global coordinate system.

2. The method for high-precision indoor positioning of unmanned aerial vehicles based on the fusion of monocular camera, IMU and UWB multi-sensor according to claim 1 is characterized in that: In step (1), the multiple ultra-wideband (UWB) anchor points are on different straight lines.

3. The method for high-precision indoor positioning of unmanned aerial vehicles based on the fusion of monocular camera, IMU and UWB multi-sensor according to claim 1 is characterized in that: The step (2) specifically comprises the following steps: (2a) Calibrate the monocular camera: For a three-dimensional feature point P(X, Y, Z) in the world coordinate system, project this three-dimensional feature point P(X, Y, Z) into the normalized imaging plane to obtain P′(X′, Y′, 1): Where f is the focal length of the monocular camera. Suppose a pixel plane ouv is fixed on the physical plane, and the pixel coordinates of P′ are obtained on the pixel plane: [u, v] T ; The definition of the pixel coordinate system is: the origin o′ is located in the upper left corner of the image, the u axis is parallel to the x axis to the right, and the v axis is parallel to the y axis downward; the pixel coordinate system differs from the imaging plane by a scaling and a translation of the origin; suppose the pixel coordinate is scaled by α times on the u axis and β times on the v axis, and the origin is translated by [c x ,c y ] T , then the coordinates of P′ and the pixel coordinates [u,v] T The relationship between them is: Substitute equation (1) into equation (2), and then combine αf into f x , merge βf into f y ,get: Among them, the unit of f is meter, and the unit of α and β is pixel / meter, so f x 、f y 、c x 、c y The unit is pixel, and formula (3) is written in matrix form: In formula (4), It is called the camera's intrinsic parameter matrix K; (2b) Calibrate the UWB anchor point: Calibrate the measured data (x1, x2, x3, ... x m ) and the recorded real data (y1,y2,y3,...y m ), use the least squares algorithm to calculate the current ultra-wideband UWB anchor point U i The scaling factor a i and deviation value b i , obtain the scale factor and deviation value of all ultra-wideband UWB anchor points; the measurement data refers to each ultra-wideband UWB anchor point U i The measured distance between the ultra-wideband UWB tag and the real data refers to each ultra-wideband UWB anchor point U i The actual distance to the ultra-wideband UWB tag.

4. The method for high-precision indoor positioning of unmanned aerial vehicles based on the fusion of monocular camera, IMU and UWB multi-sensor according to claim 1, characterized in that: The step (3) specifically comprises the following steps: (3a) Processing the video frame information: Using the Harris algorithm to extract corner points of the video frames transmitted to the server, and using the KLT optical flow algorithm to track the corner points, the movement direction and speed of the image pixels corresponding to the objects in the two frames are obtained, and the visual tracking information is obtained; (3b) Processing the acceleration and angular velocity information recorded by the inertial measurement unit (IMU): using the pre-integration method to calculate the change in position measured by the inertial measurement unit (IMU) in two adjacent video frames Changes in speed Changes in attitude angle They are: in, It is the distance from time t to time b in the IMU coordinate system. k The rotation matrix at time; and n a are the measured value, zero bias value and Gaussian white noise of the accelerometer respectively; w t , n w They are the gyroscope’s measurement value, zero bias value, and Gaussian white noise; Indicates the gyroscope from time t to time b in the IMU coordinate system k The change of time; Ω represents the calculated angular velocity written in the form of a rotation matrix; (3c) Processing the ranging information of the ultra-wideband UWB: Using the adaptive covariance weighted least squares method, assuming that the position of the ultra-wideband UWB anchor point (x i ,y i ,z i ), the position of the UWB tag on the drone is (x, y, z), and the distance between the drone tag to be located and the UWB anchor point is: Therefore, there is the following equation: Use the least squares method to estimate the unknown position: Aγ=b, where: Least squares estimation: γ=(A T A) -1 A T b, define the distance from the least square point at the current time t to the ultra-wideband UWB anchor point as m t , the distance from the tag point to the ultra-wideband UWB anchor point at time t is d t Now construct the absolute value L of the difference between the estimated 3D position and the distance between the ultra-wideband UWB anchor point i and the measured distance i :{|m1-d1|,|m2-d2|,……,|m t -d t |}, the sample L at the current time t i The variance of i , construct the adaptive covariance weight matrix at the current time t: Get the weighted position estimate: for ultra-wideband position estimation.

Images