UAV indoor positioning method based on VIO and UWB elastic evolution fusion

Abstract

The present invention relates to a method for indoor positioning of unmanned aerial vehicles (UAVs) based on the elastic evolutionary fusion of VIO and UWB, comprising: equipping a UAV with a monocular camera, an inertial sensor, and a UWB tag module, installing the UAV on the UAV, and deploying UWB anchor points in an indoor environment; calibrating the monocular camera, inertial sensor, and UWB device; obtaining a global positioning result of the UWB and a residual term of the VIO system; fusing the positioning information of the VIO system and the UWB device to obtain elastic adjustment coefficients λ and μ, and adjusting the elastic adjustment coefficients λ and μ based on real-time feedback to obtain an accurate indoor positioning estimation result. The present invention avoids the interference of the multipath effect of the indoor environment on UWB positioning, significantly improves the positioning accuracy of UWB, and designs an elastic evolutionary fusion framework that can effectively integrate multimodal information such as the monocular camera, IMU, and UWB, achieving a strong anti-interference capability and high-precision positioning effect when the UAV faces a complex indoor environment.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor positioning of unmanned aerial vehicles (UAVs), and in particular to an indoor positioning method for UAVs based on the flexible evolutionary fusion of VIO and UWB. Background Art

[0002] With the continuous advancement of technology, drones have been widely used in various industries, providing possibilities for innovation and convenience. In particular, drone positioning technology plays an important role in indoor scenarios. Equipped with six-degree-of-freedom attitude tracking sensors, drones can achieve highly accurate three-dimensional positioning, attitude estimation, and indoor motion control. However, achieving precise positioning in indoor scenarios still faces complex challenges, because GPS signals are easily blocked by buildings indoors, resulting in signal weakening or failure to receive normally, making it impossible to provide reliable positioning information. There are also a large number of objects and structures in indoor environments, and the movement of objects (such as people and furniture) may cause the positioning system to be poorly adaptable to environmental changes, so in-depth research and innovation are needed to overcome this. Scholars have proposed a variety of sensor solutions, including wireless signals, ultrasonic waves, infrared sensors, lidar, lasers, motion capture, and vision technologies.

[0003] Traditional multimodal fusion methods can lead to a decline in the positioning performance of the fusion system due to measurement errors and uncertainties between different sensors, and multimodal systems may face challenges when facing complex and dynamic environments, such as real-time noise changes. The elastic evolutionary fusion positioning of UWB and VIO is of great value in these based schemes. UWB provides global position information, while VIO provides real-time relative position tracking. The application of UWB data effectively corrects the drift problem of VIO, and the high-frequency relative position updates from VIO further improve the positioning accuracy. This multimodal elastic evolutionary fusion strategy not only ensures reliable relative position tracking and global positioning, but also optimizes according to the measurement errors between different sensors and flexibly adjusts to the noise changes of the scene at different times in the scene, meeting the needs of diverse indoor applications and providing a significant improvement in the overall positioning performance. Therefore, it is necessary to propose a multimodal elastic evolutionary fusion positioning method. Summary of the Invention

[0004] In order to solve the problem of low positioning accuracy of drones in indoor scenarios, the purpose of the present invention is to provide a drone indoor positioning method based on the elastic evolutionary fusion of VIO and UWB based on the optimized extended Kalman filter, which realizes the complementary advantages between VIO and UWB, realizes the elastic evolutionary fusion of multimodal data, and obtains accurate indoor positioning estimation results.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a method for indoor positioning of unmanned aerial vehicles based on the flexible evolutionary fusion of VIO and UWB, the method comprising the following steps in sequence:

[0006] (1) A UAV is equipped with a monocular camera, an inertial sensor, and a UWB tag module. The monocular camera and inertial sensor form a VIO system, and UWB anchors are deployed in an indoor environment.

[0007] (2) Calibrate the internal and external parameters of the monocular camera, calibrate the gyroscope and accelerometer of the inertial sensor, and calibrate the transmission delay and path loss of the UWB device;

[0008] (3) Start the drone and collect data from the VIO system and UWB devices simultaneously. The drone moves indoors, obtains measurement values ​​from visual, ultra-wideband, and inertial sensors, and transmits the real-time data to the server. The server preprocesses the multimodal data to obtain the UWB global positioning result and the residual term of the VIO system.

[0009] (4) Based on the global positioning results of UWB and the residual term of the VIO system, the positioning information of the VIO system and the UWB device are fused to obtain the elastic adjustment coefficients λ and μ. The elastic adjustment coefficients λ and μ are adjusted according to real-time feedback to obtain accurate indoor positioning estimation results.

[0010] In step (1), the deployment of UWB anchor points in an indoor environment specifically refers to: selecting at least three known locations in the indoor scene to place UWB anchor points, the UWB anchor points are on different straight lines, and recording the positions of the UWB anchor points (x i ,y i ,z i ), where i is the number of UWB anchor points.

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

[0012] (2a) Calibrate the monocular camera and obtain the intrinsic parameter matrix and extrinsic parameter matrix after calibration;

[0013] The relationship between the object point M and its imaging point M′ on the imaging plane is expressed by formula (1):

[0014]

[0015] Where X, Y, and Z are the three-dimensional coordinates of the object point M, X′ and Y′ are the two-dimensional coordinates of the imaging point M′, and f is the focal length of the monocular camera.

[0016] Map the imaging point M′ to the pixel coordinate system. Assume that the coordinates of the imaging point M′ in the pixel coordinate system are [u, v]. The pixel coordinate system is scaled by α times on the u axis and β times on the v axis. At the same time, the origin is translated by [c x ,c y ], so the relationship between the imaging point M′ and the pixel coordinates is obtained:

[0017]

[0018] Use f x ,f y Replace αf and βf, f x 、f y The unit is pixel, using homogeneous coordinates, write equation (2) in matrix form:

[0019]

[0020] Where K is the intrinsic parameter matrix of the monocular camera;

[0021] For the camera external parameters, transform the point Mw in the world coordinate system to the point M in the camera coordinate system. The pose of the monocular camera is represented by R and t, and we get:

[0022] M=RM w +t(4)

[0023] Substituting into the internal parameter matrix K, we get:

[0024] ZM uv =K(RM w +t)=KTM w (5)

[0025] Among them, R is the rotation matrix, which represents the rotation from the world coordinate system to the camera coordinate system; t is the translation vector, which represents the translation from the world coordinate system to the camera coordinate system; T is the external parameter matrix of the monocular camera, which contains the rotation matrix R and the translation vector t, which represents the transformation from the world coordinate system to the camera coordinate system; ZM uv The homogeneous form of the pixel coordinates is transformed from the world coordinate system to the pixel coordinate system under the influence of the scale factor Z;

[0026] After obtaining the intrinsic parameter matrix K and extrinsic parameter matrix T of the monocular camera, the newly input image is corrected;

[0027] (2b) Calibrate the inertial sensor: The measured value A of the accelerometer after calibration measured for:

[0028] A measured =A true +B a +S a ·A true+θ (6)

[0029] Among them, A true is the actual acceleration value, B a Indicates the zero bias of the accelerometer, S a represents the scale factor of the accelerometer, θ represents the noise of the accelerometer;

[0030] When the inertial sensor is stationary, record the accelerometer output for a period of time and calculate the average value of the accelerometer output, which is the zero bias. In subsequent measurements, subtract the zero bias from the accelerometer measurement to eliminate static error. When the inertial sensor is in a constant acceleration state, record the accelerometer output, calculate the scale factor between the actual acceleration and the measured value, and use the scale factor to adjust the accelerometer measurement to eliminate the scale error.

[0031] At the same time, the gyroscope of the inertial sensor is calibrated, and the measured value of the gyroscope after calibration is ω measured for:

[0032] ω measured =ω true +B g +S g ·ω true +τ (7)

[0033] Among them, ω true Indicates the true acceleration value, B g Indicates gyroscope zero bias, S g τ represents the gyroscope scale factor, and τ represents the gyroscope noise. When the inertial sensor is stationary, the gyroscope output is recorded for a period of time, and the average value of the gyroscope output is calculated. This value is the zero bias. In subsequent measurements, the zero bias is subtracted from the gyroscope measurement to eliminate static error. When the inertial sensor is at a constant rotation speed, the gyroscope output is recorded, and the scale factor between the actual rotation speed of the gyroscope and the measured value is calculated. The scale factor is used to adjust the gyroscope measurement to eliminate the scale error.

[0034] (2c) Use the synchronization signal and time tag to calibrate the transmission delay of the UWB device. The timestamp of the synchronization signal is recorded at the transmitter and receiver respectively. The transmission delay T delay is the receiving time T received and sending time T tansmitted The difference between the two times is used to calculate the transmission delay by comparing the timestamps:

[0035] T delay =T received -T tansmitted (8)

[0036] The measured transmission delay is mapped to the actual distance through formula (8), that is, the transmission delay time is 3×10 8 Multiply meters per second;

[0037] By measuring the signal strength at different distances, a path loss model is established. The path loss model adopts a free space propagation model. The formula of the path loss model is:

[0038]

[0039] Where L0 is the path loss at the reference distance d0, is the path loss index. Using the established path loss model, the measured received power is mapped to the actual distance d, L loss is the path loss.

[0040] The step (3) specifically includes the following steps in order:

[0041] (3a) After the UWB device is calibrated, the data is processed and the position of the i-th UWB anchor point is B i =(x i ,y i ,z i ), assuming that the UWB tag carried by the drone sends a wireless pulse signal at time t0, and the time when the i-th UWB anchor point receives the signal is t k , then the flight time is t k -t0, according to the TOA method (time of arrival method), the distance d between the UWB anchor point and the UWB tag i Expressed as:

[0042] d i =c(t k -t0) (10)

[0043] Where c is the speed of light;

[0044] Then the modulus of the coordinate vector of the i-th UWB anchor point is expressed as:

[0045]

[0046] The distance from the UWB tag to the UWB anchor point is expressed as:

[0047]

[0048] Converting formula (12) into matrix form yields:

[0049]

[0050] set up:

[0051]

[0052] Then we get:

[0053] AP=b(15)

[0054] The pseudo-inverse of the matrix is ​​used to solve Equation (15) to obtain the position P of the UWB tag. In order to describe the continuous motion process in a continuous form, a hybrid threshold wavelet denoising method is proposed. First, the discrete data is transformed into the wavelet domain through wavelet transformation, and the signal is decomposed into wavelet coefficients of different scales and frequencies:

[0055]

[0056] Where P(k) is the discrete UWB positioning data, ψ * (·) is the discrete wavelet basis function, s0 and τ0 are the step sizes of scale and displacement, respectively, and m and n are the discrete indices representing scale and displacement, respectively;

[0057] The new mixed threshold function in the mixed threshold wavelet denoising method is constructed as follows:

[0058]

[0059] Where w(m,n) is the wavelet coefficient, is the wavelet coefficient after mixed threshold denoising, γ is the threshold, sgn(·) is the sign function; η is the adjustment factor, which is greater than 0;

[0060] Finally, use Reconstruct and obtain the denoised UWB positioning data P′(k):

[0061]

[0062] Among them, C ψ is the normalization constant ψ(·) of the wavelet basis; the denoised UWB positioning data P′(k) is used as the global positioning result of UWB;

[0063] (3b) The pre-integrated information of the inertial sensor is combined with the visual information to form the residual term of the VIO system. The maximum likelihood estimation problem is solved by constructing the residual term based on the visual and inertial sensor measurements, and the optimal solution of the entire optimization is used as an accurate state estimate:

[0064]

[0065] Among them, {r p ,H p} is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; is the complete state vector to be optimized; and Represent the covariance matrices of the visual and inertial sensors respectively; through optimization, the motion process of the drone's monocular camera and its position estimation in space are obtained.

[0066] The step (4) specifically includes the following steps:

[0067] (4a) Construct the residual term E of UWB U :

[0068]

[0069] in, and Represent the position of the UAV in the global coordinate system and the camera coordinate system respectively; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U represents the measurement covariance matrix of UWB; p represents the UWB measurement value at time k, and γ is the set of all UWB measurement values;

[0070] After obtaining the UWB residual term, the UWB residual term is combined with the cost function of the residual term of the VIO system to construct a new nonlinear cost function. The nonlinear cost function uses all inertial sensor measurements, local visual measurements, retrieved feature correspondences, and the UWB residual term E U To jointly optimize the sliding window:

[0071]

[0072] In the formula, {r p ,H p} is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; is the complete state vector to be optimized; and denote the covariance matrices of the visual and inertial sensors respectively;

[0073] Use Ceres solver to solve nonlinear problems and obtain local tracking results;

[0074] (4b) Using the local tracking results and the global positioning results of UWB, a flexible evolutionary fusion framework is proposed to fuse multi-sensor data through the optimized EKF method;

[0075] First predict the state covariance matrix:

[0076]

[0077] Among them, P k|k-1 is the posterior estimation error covariance matrix at time k-1; F k is the Jacobian matrix; μQ k-1 is the process noise covariance matrix at time k-1; μ is the elastic adjustment coefficient;

[0078] The Kalman gain is:

[0079]

[0080] Among them, k k is the Kalman gain; H k is the Jacobian matrix of the observation model; R k is the observation noise covariance matrix; λ is the elastic adjustment coefficient;

[0081] Update the state equation:

[0082]

[0083] in, is the positioning result of the state estimation at time k, Z k is the distance between the UAV and the UWB anchor point at time k; is the observation value calculated by the observation function h(·) at time k;

[0084] Finally update the state covariance matrix:

[0085] P k|k =(I9-k k H k )P k|k-1 (27)

[0086] Among them, P k|k is the posterior estimation error covariance matrix at time k, I9 is ​​the unit matrix, and the elastic evolutionary fusion framework established uses λ and μ as elastic adjustment coefficients. Based on the above optimized EKF method, it is used to update the residual η of the state estimation k The calculation formula is as follows:

[0087]

[0088] The theoretical covariance obtained from this is for:

[0089]

[0090] Actual covariance of the residuals for:

[0091]

[0092] Deviation e of the constructed variance at time k k is the actual covariance Subtract the theoretical covariance The given e k As shown below:

[0093]

[0094] Based on the deviation e k The square of , constructs the fitness function:

[0095]

[0096] According to the fitness function, an initial set of elastic adjustment coefficients of λ and μ is randomly generated, and its value range is [0,10]. Then, the fitness of each parameter in the initial set is calculated, and the parameters of the next generation are selected using the roulette strategy. The selected parameters will generate new individual parameters through the crossover and mutation operations in the genetic algorithm, and the fitness is calculated again, and the next generation parameters are selected. This cycle is repeated until the value of the fitness function reaches the maximum value and stops. Finally, the parameters with the highest fitness are obtained as the optimal solution, that is, the optimal elastic adjustment coefficients λ and μ are obtained. Subsequently, the optimal elastic adjustment coefficients λ and μ are substituted into formula (26) for iteration, and then continuous monitoring is performed to elastically adjust the noise at each moment, and then elastic evolution fusion is performed to obtain accurate indoor positioning estimation results.

[0097] It can be seen from the above technical solution that the beneficial effects of the present invention are: First, the present invention avoids the interference of the multipath effect of the indoor environment on UWB positioning, significantly improves the positioning accuracy of UWB, and designs a flexible evolutionary fusion framework that can effectively integrate multimodal information such as monocular cameras, IMUs and UWBs, so as to achieve strong anti-interference ability and high-precision positioning effect when the drone faces a complex indoor environment; Second, the present invention successfully realizes the complementary advantages between monocular cameras, IMUs and UWB sensors by means of multimodal elastic evolutionary fusion, gives full play to the unique advantages of each sensor, and combines their data with each other through the elastic evolutionary fusion framework, thereby creating a more comprehensive and accurate perception framework; Third, the present invention has a wide range of applications, is easy to deploy, has high positioning accuracy, can achieve centimeter-level positioning, and the sensors used are low-cost and easy to promote; Fourth, the present invention works in complex indoor environments, such as indoor areas lacking GPS, such as those without textures, low light, and many obstacles, and can still provide highly reliable indoor positioning results when a sensor fails. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Figure 1 This is a schematic diagram of multimodal information fusion positioning;

[0099] Figure 2 Schematic diagram of the trajectory using the mixed threshold wavelet denoising method. DETAILED DESCRIPTION

[0100] A method for indoor positioning of unmanned aerial vehicles based on the flexible evolutionary fusion of VIO and UWB includes the following steps:

[0101] (1) A monocular camera, an inertial sensor, and a UWB tag module are installed on the drone. The monocular camera and the inertial sensor form a VIO system to ensure that the data of the inertial sensor and the UWB tag module can be acquired by the drone system at the same time. At the same time, UWB anchor points are deployed in the indoor environment.

[0102] (2) Calibrate the internal and external parameters of the monocular camera, and calibrate the gyroscope and accelerometer of the inertial sensor to improve the estimation accuracy of the camera attitude; calibrate the transmission delay and path loss of the UWB device to ensure the accuracy of the UWB data;

[0103] (3) Start the drone and collect data from the VIO system and UWB devices at the same time. The drone moves indoors and obtains measurement values ​​from visual, ultra-wideband, and inertial sensors. The real-time data is transmitted to the server. The server preprocesses the multimodal data to obtain the UWB global positioning result and the residual term of the VIO system; the UWB data is used to calculate the distance between the drone and the UWB tag, and the hybrid threshold wavelet denoising method is used to effectively denoise the UWB, thereby achieving accurate global positioning.

[0104] (4) Based on the global positioning results of UWB and the residual term of the VIO system, the positioning information of the VIO system and the UWB device are fused to obtain elastic adjustment coefficients λ and μ. The elastic adjustment coefficients λ and μ are adjusted according to real-time feedback to obtain accurate indoor positioning estimation results. The positioning information of VIO and UWB are fused, and the weight parameters of real-time noise are optimized through elastic evolution to achieve seamless integration of global positioning and local tracking. The present invention realizes multi-sensor data fusion and enhances the system's adaptability to environmental changes and sensor noise, thereby improving the accuracy and robustness of positioning.

[0105] In step (1), the deployment of UWB anchor points in an indoor environment specifically refers to: selecting at least three known locations in the indoor scene to place UWB anchor points, the UWB anchor points are on different straight lines, and recording the positions of the UWB anchor points (x i ,y i ,z i ), where i is the number of UWB anchor points.

[0106] The step (2) specifically includes the following steps:

[0107] (2a) Calibrate the monocular camera and obtain the intrinsic parameter matrix and extrinsic parameter matrix after calibration;

[0108] The relationship between the object point M and its imaging point M′ on the imaging plane is expressed by formula (1):

[0109]

[0110] Where X, Y, and Z are the three-dimensional coordinates of the object point M, X′ and Y′ are the two-dimensional coordinates of the imaging point M′, and f is the focal length of the monocular camera.

[0111] Map the imaging point M′ to the pixel coordinate system. Assume that the coordinates of the imaging point M′ in the pixel coordinate system are [u, v]. The pixel coordinate system is scaled by α times on the u axis and β times on the v axis. At the same time, the origin is translated by [c x ,c y ], so the relationship between the imaging point M′ and the pixel coordinates is obtained:

[0112]

[0113] Use f x ,f y Replace αf and βf, f x 、f y The unit is pixel, using homogeneous coordinates, write equation (2) in matrix form:

[0114]

[0115] Where K is the intrinsic parameter matrix of the monocular camera;

[0116] For the camera external parameters, transform the point Mw in the world coordinate system to the point M in the camera coordinate system. The pose of the monocular camera is represented by R and t, and we get:

[0117] M=RM w +t(4)

[0118] Substituting into the internal parameter matrix K, we get:

[0119] ZM uv =K(RM w +t)=KTM w (5)

[0120] Among them, R is the rotation matrix, which represents the rotation from the world coordinate system to the camera coordinate system; t is the translation vector, which represents the translation from the world coordinate system to the camera coordinate system; T is the external parameter matrix of the monocular camera, which contains the rotation matrix R and the translation vector t, which represents the transformation from the world coordinate system to the camera coordinate system; ZM uv The homogeneous form of the pixel coordinates is transformed from the world coordinate system to the pixel coordinate system under the influence of the scale factor Z;

[0121] After obtaining the camera's intrinsic and extrinsic parameter matrices, the new image is corrected. The goal of correction is to eliminate the camera's lens distortion so that straight lines in the image remain straight lines in the corrected image. This correction process is achieved through geometric transformations based on the previously calculated intrinsic and extrinsic parameters, resulting in a more accurate and realistic image.

[0122] (2b) Calibrate the inertial sensor:

[0123] Place the IMU on a stationary surface and record the accelerometer output. Use the difference between the measured output and the actual acceleration to estimate the calibration parameters.

[0124] The measured value A of the accelerometer after calibration measured for:

[0125] A measured =Atrue +B a +S a ·A true +θ (6)

[0126] Among them, A true is the actual acceleration value, B a Indicates the zero bias of the accelerometer, S a represents the scale factor of the accelerometer, θ represents the noise of the accelerometer;

[0127] When the inertial sensor is stationary, record the accelerometer output for a period of time and calculate the average value of the accelerometer output, which is the zero bias. In subsequent measurements, subtract the zero bias from the accelerometer measurement to eliminate static error. When the inertial sensor is in a constant acceleration state, record the accelerometer output, calculate the scale factor between the actual acceleration and the measured value, and use the scale factor to adjust the accelerometer measurement to eliminate the scale error.

[0128] At the same time, the gyroscope of the inertial sensor is calibrated. The IMU is placed on a rotating plane and the gyroscope output at this time is recorded. The measured value of the gyroscope after calibration is ω measured for:

[0129] ω measured =ω true +B g +S g ·ω true +τ (7)

[0130] Among them, ω true Indicates the true acceleration value, B g Indicates gyroscope zero bias, S g τ represents the gyroscope scale factor, and τ represents the gyroscope noise. When the inertial sensor is stationary, the gyroscope output is recorded for a period of time, and the average value of the gyroscope output is calculated. This value is the zero bias. In subsequent measurements, the zero bias is subtracted from the gyroscope measurement to eliminate static error. When the inertial sensor is at a constant rotation speed, the gyroscope output is recorded, and the scale factor between the actual rotation speed of the gyroscope and the measured value is calculated. The scale factor is used to adjust the gyroscope measurement to eliminate the scale error.

[0131] Incorporating calibration parameters for the gyroscope and accelerometer into the system ensures that the entire IMU system provides accurate output under various motion conditions.

[0132] (2c) Use the synchronization signal and time tag to calibrate the transmission delay of the UWB device. The timestamp of the synchronization signal is recorded at the transmitter and receiver respectively. The transmission delay T delay is the receiving time Treceived and sending time T tansmitted The difference between the two times is used to calculate the transmission delay by comparing the timestamps:

[0133] T delay =T received -T tansmitted (8)

[0134] The measured transmission delay is mapped to the actual distance through formula (8), that is, the transmission delay time is 3×10 8 Multiply meters per second;

[0135] By measuring the signal strength at different distances, a path loss model is established. The path loss model adopts a free space propagation model. The formula of the path loss model is:

[0136]

[0137] Where L0 is the path loss at the reference distance d0, is the path loss index. Using the established path loss model, the measured received power is mapped to the actual distance d, L loss is the path loss.

[0138] The goal of these two calibration processes, calibration of transmission delay and path loss, is to accurately estimate the impact of transmission delay and path loss in the UWB system through measurement and model fitting, combine the information of transmission delay and path loss for joint calculation, and fuse the information of transmission delay and path loss together to make corrections in the positioning calculation, which can improve the accuracy and reliability of the UWB positioning system.

[0139] The step (3) specifically includes the following steps in order:

[0140] (3a) After the UWB device is calibrated, the data is processed and the position of the i-th UWB anchor point is B i =(x i ,y i ,z i ), assuming that the UWB tag carried by the drone sends a wireless pulse signal at time t0, and the time when the i-th UWB anchor point receives the signal is t k , then the flight time is t k -t0, according to the TOA method (time of arrival method), the distance d between the UWB anchor point and the UWB tag i Expressed as:

[0141] d i =c(t k -t0) (10)

[0142] Where c is the speed of light;

[0143] Then the modulus of the coordinate vector of the i-th UWB anchor point is expressed as:

[0144]

[0145] The distance from the UWB tag to the UWB anchor point is expressed as:

[0146]

[0147] Converting formula (12) into matrix form yields:

[0148]

[0149] set up:

[0150]

[0151] Then we get:

[0152] AP=b(15)

[0153] The pseudo-inverse of the matrix is ​​used to solve Equation (15) to obtain the position P of the UWB tag. In order to describe the continuous motion process in a continuous form, a hybrid threshold wavelet denoising method is proposed. First, the discrete data is transformed into the wavelet domain through wavelet transformation, and the signal is decomposed into wavelet coefficients of different scales and frequencies:

[0154]

[0155] Where P(k) is the discrete UWB positioning data, ψ * (·) is the discrete wavelet basis function, s0 and τ0 are the step sizes of scale and displacement, respectively, and m and n are the discrete indices representing scale and displacement, respectively;

[0156] The new mixed threshold function in the mixed threshold wavelet denoising method is constructed as follows:

[0157]

[0158] Where w(m,n) is the wavelet coefficient, is the wavelet coefficient after mixed threshold denoising, γ is the threshold, sgn(·) is the sign function; η is the adjustment factor, which is greater than 0;

[0159] Finally, use Reconstruct and obtain the denoised UWB positioning data P′(k):

[0160]

[0161] Among them, C ψis the normalization constant ψ(·) of the wavelet basis; the denoised UWB positioning data P′(k) is used as the global positioning result of UWB;

[0162] (3b) VIO uses both visual and inertial sensors for positioning. Its working principle is based on combining the visual information of the camera with the accelerometer and gyroscope data of the inertial sensors. The key frames captured by the camera during motion are selected for motion estimation. Points with significant features in the key frames are used as feature points. VIO first tracks the relative motion between adjacent key frames through feature point matching and obtains the relative motion estimate of the drone by solving the three-dimensional position of the feature points. At the same time, the inertial information provided by the IMU is used to estimate the camera's motion state, making up for the lack of visual information in fast motion or feature loss.

[0163] The pre-integrated information of the inertial sensor is combined with the visual information to form the residual term of the VIO system. The maximum likelihood estimation problem is solved by constructing the residual term based on the visual and inertial sensor measurements, and the optimal solution of the entire optimization is used as an accurate state estimate:

[0164]

[0165] Among them, {r p ,H p} is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; is the complete state vector to be optimized; and Represent the covariance matrices of the visual and inertial sensors respectively; through optimization, the motion process of the drone's monocular camera and its position estimation in space are obtained.

[0166] The step (4) specifically includes the following steps:

[0167] (4a) The introduction of UWB can effectively suppress the VIO drift problem, improve the stability and accuracy of the positioning system, and provide a more reliable reference for overall positioning and ensuring accuracy. Therefore, the residual term E of UWB is constructed. U , to better integrate global and local information and improve positioning performance:

[0168]

[0169] in, and Represent the position of the UAV in the global coordinate system and the camera coordinate system respectively; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U represents the measurement covariance matrix of UWB; p represents the UWB measurement value at time k, and γ is the set of all UWB measurement values;

[0170] After obtaining the UWB residual term, the UWB residual term is combined with the cost function of the residual term of the VIO system to construct a new nonlinear cost function. The nonlinear cost function uses all inertial sensor measurements, local visual measurements, retrieved feature correspondences, and the UWB residual term E U To jointly optimize the sliding window:

[0171]

[0172] In the formula, {r p ,H p} is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; χ is the complete state vector to be optimized; and denote the covariance matrices of the visual and inertial sensors respectively;

[0173] Use Ceres solver to solve nonlinear problems and obtain local tracking results;

[0174] (4b) Using the local tracking results and the global positioning results of UWB, a flexible evolutionary fusion framework is proposed to fuse multi-sensor data through the optimized EKF method to flexibly adapt to changes in complex environments and dynamic scenes;

[0175] First predict the state covariance matrix:

[0176]

[0177] Among them, P k|k-1 is the posterior estimation error covariance matrix at time k-1; Fk is the Jacobian matrix; μQ k-1 is the process noise covariance matrix at time k-1; μ is the elasticity adjustment coefficient; the elasticity adjustment coefficient is used to optimize the process noise to better adapt to the complex situation of real-time changes and make it more flexible.

[0178] The Kalman gain is:

[0179]

[0180] Among them, k k is the Kalman gain; H k is the Jacobian matrix of the observation model; R k is the observation noise covariance matrix; λ is the elastic adjustment coefficient; the same elastic evolutionary optimization process is adopted for the observation noise to better adapt to the real-time changing measurement results.

[0181] Update the state equation:

[0182]

[0183] in, is the positioning result of the state estimation at time k, Z k is the distance between the UAV and the UWB anchor point at time k; is the observation value calculated by the observation function h(·) at time k;

[0184] Finally update the state covariance matrix:

[0185] P k|k =(I9-k k H k )P k|k-1 (27)

[0186] Among them, P k|k is the posterior estimation error covariance matrix at time k, I9 is ​​the unit matrix, and the elastic evolutionary fusion framework established uses λ and μ as elastic adjustment coefficients. Based on the above optimized EKF method, it is used to update the residual η of the state estimation k The calculation formula is as follows:

[0187]

[0188] The theoretical covariance obtained from this is for:

[0189]

[0190] Actual covariance of the residuals for:

[0191]

[0192] Deviation e of the constructed variance at time k k is the actual covariance Subtract the theoretical covariance The given e k As shown below:

[0193]

[0194] Based on the deviation e k The square of , constructs the fitness function:

[0195]

[0196] According to the fitness function, an initial set of elastic adjustment coefficients of λ and μ is randomly generated, and its value range is [0,10]. Then, the fitness of each parameter in the initial set is calculated, and the parameters of the next generation are selected using the roulette strategy. The selected parameters will generate new individual parameters through the crossover and mutation operations in the genetic algorithm, and the fitness is calculated again, and the next generation parameters are selected. This cycle is repeated until the value of the fitness function reaches the maximum value and stops. Finally, the parameters with the highest fitness are obtained as the optimal solution, that is, the optimal elastic adjustment coefficients λ and μ are obtained. Subsequently, the optimal elastic adjustment coefficients λ and μ are substituted into formula (26) for iteration, and then continuous monitoring is performed to elastically adjust the noise at each moment, and then elastic evolution fusion is performed to obtain accurate indoor positioning estimation results.

[0197] like Figure 1 As shown, Figure 1 The paper demonstrates the introduction of UWB as an additional constraint, using four anchor points to obtain the global positioning results of the drone, in order to optimize the VIO local tracking in the drone positioning trajectory segment. VIO first tracks the relative motion between adjacent key frames through feature point matching, and obtains the relative motion estimate of the drone by solving the three-dimensional position of the feature points, thereby realizing the elastic evolutionary fusion of global positioning and local tracking to obtain accurate indoor positioning results.

[0198] like Figure 2 The figure shows the two-dimensional flight trajectory of a UAV, with the dashed line representing the actual trajectory. The dots represent the calculated positions of the UWB anchor points, and the solid line represents the UWB positioning trajectory obtained using a hybrid threshold wavelet denoising method. This hybrid threshold wavelet denoising method not only enhances the overall continuity of the UWB positioning data but also makes the positioning trajectory closer to the actual trajectory.

[0199] In summary, the present invention avoids the interference of the multipath effect of the indoor environment on UWB positioning, significantly improves the positioning accuracy of UWB, and designs a flexible evolutionary fusion framework that can effectively integrate multimodal information such as monocular cameras, IMUs and UWBs, so as to achieve strong anti-interference ability and high-precision positioning effect when the drone faces a complex indoor environment; the present invention successfully realizes the complementary advantages between monocular cameras, IMUs and UWB sensors by means of multimodal flexible evolutionary fusion, gives full play to the unique advantages of each sensor, and combines their data with each other through the flexible evolutionary fusion framework, thereby creating a more comprehensive and accurate perception framework; the present invention has a wide range of uses, is easy to deploy, has high positioning accuracy, can achieve centimeter-level positioning, and the sensors used are low-cost and easy to promote; the present invention works in complex indoor environments, such as indoor areas lacking GPS, such as those without textures, low light, and many obstacles, and can still provide highly reliable indoor positioning results when a sensor fails.

Claims

1. A method for indoor positioning of unmanned aerial vehicles based on the flexible evolutionary fusion of VIO and UWB, characterized by: The method comprises the following steps in sequence: (1) A UAV is equipped with a monocular camera, an inertial sensor, and a UWB tag module. The monocular camera and inertial sensor form a VIO system, and UWB anchors are deployed in an indoor environment. (2) Calibrate the internal and external parameters of the monocular camera, calibrate the gyroscope and accelerometer of the inertial sensor, and calibrate the transmission delay and path loss of the UWB device; (3) Start the drone and collect data from the VIO system and UWB devices simultaneously. The drone moves indoors, obtains measurement values ​​from visual, ultra-wideband, and inertial sensors, and transmits the real-time data to the server. The server preprocesses the multimodal data to obtain the UWB global positioning result and the residual term of the VIO system. (4) Based on the global positioning results of UWB and the residual term of the VIO system, the positioning information of the VIO system and the UWB device are fused to obtain the elastic adjustment coefficients λ and μ. The elastic adjustment coefficients λ and μ are adjusted according to real-time feedback to obtain accurate indoor positioning estimation results; According to the fitness function, an initial set of elastic adjustment coefficients of λ and μ is randomly generated, with a value range of [0,10]. Then, the fitness of each parameter in the initial set is calculated, and the parameters of the next generation are selected using the roulette strategy. The selected parameters will generate new individual parameters through the crossover and mutation operations in the genetic algorithm, and the fitness will be calculated again, and the next generation parameters will be selected. This cycle will be iterated until the value of the fitness function reaches the maximum value, and finally the parameters with the highest fitness are obtained as the optimal solution, that is, the optimal elastic adjustment coefficients λ and μ are obtained.

2. The UAV indoor positioning method based on VIO and UWB elastic evolution fusion according to claim 1 is characterized by: In step (1), the deployment of UWB anchor points in an indoor environment specifically refers to: selecting at least three known locations in the indoor scene to place UWB anchor points, the UWB anchor points are on different straight lines, and recording the positions of the UWB anchor points (x i ,y i ,z i ), where i is the number of UWB anchor points.

3. The UAV indoor positioning method based on VIO and UWB elastic evolution fusion according to claim 1 is characterized by: The step (2) specifically includes the following steps: (2a) Calibrate the monocular camera and obtain the intrinsic parameter matrix and extrinsic parameter matrix after calibration; The relationship between the object point M and its imaging point M′ on the imaging plane is expressed by formula (1): Where X, Y, and Z are the three-dimensional coordinates of the object point M, X′ and Y′ are the two-dimensional coordinates of the imaging point M′, and f is the focal length of the monocular camera. Map the imaging point M′ to the pixel coordinate system. Assume that the coordinates of the imaging point M′ in the pixel coordinate system are [u, v]. The pixel coordinate system is scaled by α times on the u axis and β times on the v axis. At the same time, the origin is translated by [c x ,c y ], so the relationship between the imaging point M′ and the pixel coordinates is obtained: Use f x ,f y Replace αf and βf, f x 、f y The unit is pixel, using homogeneous coordinates, write equation (2) in matrix form: Where K is the intrinsic parameter matrix of the monocular camera; For the camera external parameters, transform the point Mw in the world coordinate system to the point M in the camera coordinate system. The pose of the monocular camera is represented by R and t, and we get: M=RM w +t(4) Substituting into the internal parameter matrix K, we get: ZM uv =K(RM w +t)=KTM w (5) Among them, R is the rotation matrix, which represents the rotation from the world coordinate system to the camera coordinate system; t is the translation vector, which represents the translation from the world coordinate system to the camera coordinate system; T is the external parameter matrix of the monocular camera, which contains the rotation matrix R and the translation vector t, which represents the transformation from the world coordinate system to the camera coordinate system; ZM uv The homogeneous form of the pixel coordinates is transformed from the world coordinate system to the pixel coordinate system under the influence of the scale factor Z; After obtaining the intrinsic parameter matrix K and extrinsic parameter matrix T of the monocular camera, the newly input image is corrected; (2b) Calibrate the inertial sensor: The measured value A of the accelerometer after calibration measured for: A measured =A true +B a +S a ·A true +θ (6) Among them, A true is the actual acceleration value, B a Indicates the zero bias of the accelerometer, S a represents the scale factor of the accelerometer, and θ represents the noise of the accelerometer; When the inertial sensor is stationary, record the accelerometer output for a period of time and calculate the average value of the accelerometer output, which is the zero bias. In subsequent measurements, subtract the zero bias from the accelerometer measurement to eliminate static error. When the inertial sensor is in a constant acceleration state, record the accelerometer output, calculate the scale factor between the actual acceleration and the measured value, and use the scale factor to adjust the accelerometer measurement to eliminate the scale error. At the same time, the gyroscope of the inertial sensor is calibrated, and the measured value of the gyroscope after calibration is ω measured for: oh measured =ω true +B g +S g ·oh true +t (7) Among them, ω true Indicates the true acceleration value, B g Indicates gyroscope zero bias, S g τ represents the gyroscope scale factor, and τ represents the gyroscope noise. When the inertial sensor is stationary, the gyroscope output is recorded for a period of time, and the average value of the gyroscope output is calculated. This value is the zero bias. In subsequent measurements, the zero bias is subtracted from the gyroscope measurement to eliminate static error. When the inertial sensor is at a constant rotation speed, the gyroscope output is recorded, and the scale factor between the actual rotation speed of the gyroscope and the measured value is calculated. The scale factor is used to adjust the gyroscope measurement to eliminate the scale error. (2c) Use the synchronization signal and time tag to calibrate the transmission delay of the UWB device. The timestamp of the synchronization signal is recorded at the transmitter and receiver respectively. The transmission delay T delay is the receiving time T received and sending time T tansmitted The difference between the two times is used to calculate the transmission delay by comparing the timestamps: T delay =T received -T tansmitted (8) The measured transmission delay is mapped to the actual distance through formula (8), that is, the transmission delay time is 3×10 8 Multiply meters per second; By measuring the signal strength at different distances, a path loss model is established. The path loss model adopts a free space propagation model. The formula of the path loss model is: Where L0 is the path loss at the reference distance d0, is the path loss index. Using the established path loss model, the measured received power is mapped to the actual distance d, L loss is the path loss.

4. The UAV indoor positioning method based on VIO and UWB elastic evolution fusion according to claim 1 is characterized by: The step (3) specifically includes the following steps in order: (3a) After the UWB device is calibrated, the data is processed and the position of the i-th UWB anchor point is B i =(x i ,y i ,z i ), assuming that the UWB tag carried by the drone sends a wireless pulse signal at time t0, and the time when the i-th UWB anchor point receives the signal is t k , then the flight time is t k -t0, according to the TOA method (time of arrival method), the distance d between the UWB anchor point and the UWB tag i Expressed as: d i =c(t k -t0) (10) Where c is the speed of light; Then the modulus of the coordinate vector of the i-th UWB anchor point is expressed as: The distance from the UWB tag to the UWB anchor point is expressed as: Converting formula (12) into matrix form yields: set up: Then we get: AP=b(15) The pseudo-inverse of the matrix is ​​used to solve Equation (15) to obtain the position P of the UWB tag. In order to describe the continuous motion process in a continuous form, a hybrid threshold wavelet denoising method is proposed. First, the discrete data is transformed into the wavelet domain through wavelet transformation, and the signal is decomposed into wavelet coefficients of different scales and frequencies: Where P(k) is the discrete UWB positioning data, ψ * (·) is the discrete wavelet basis function, s0 and τ0 are the step sizes of scale and displacement, respectively, and m and n are the discrete indices representing scale and displacement, respectively; The new mixed threshold function in the mixed threshold wavelet denoising method is constructed as follows: Where w(m,n) is the wavelet coefficient, is the wavelet coefficient after mixed threshold denoising, γ is the threshold, sgn(·) is the sign function; η is the adjustment factor, which is greater than 0; Finally, use Reconstruct and obtain the denoised UWB positioning data P′(k): Among them, C ψ is the normalization constant ψ(·) of the wavelet basis; the denoised UWB positioning data P′(k) is used as the global positioning result of UWB; (3b) The pre-integrated information of the inertial sensor is combined with the visual information to form the residual term of the VIO system. The maximum likelihood estimation problem is solved by constructing the residual term based on the visual and inertial sensor measurements, and the optimal solution of the entire optimization is used as an accurate state estimate: Among them, {r p ,H p } is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; χ is the complete state vector to be optimized; and Represent the covariance matrices of the visual and inertial sensors respectively; through optimization, the motion process of the drone's monocular camera and its position estimation in space are obtained.

5. The UAV indoor positioning method based on VIO and UWB elastic evolution fusion according to claim 1 is characterized by: The step (4) specifically includes the following steps: (4a) Construct the residual term E of UWB U : in, and Represent the position of the UAV in the global coordinate system and the camera coordinate system respectively; Represents the transformation matrix from the camera coordinate system to the global coordinate system; P U represents the measurement covariance matrix of UWB; p represents the UWB measurement value at time k, and γ is the set of all UWB measurement values; After obtaining the UWB residual term, the UWB residual term is combined with the cost function of the residual term of the VIO system to construct a new nonlinear cost function. The nonlinear cost function uses all inertial sensor measurements, local visual measurements, retrieved feature correspondences, and the UWB residual term E U To jointly optimize the sliding window: In the formula, {r p ,H p } is the prior information related to marginalization; and are the residuals of inertial sensor and visual measurement respectively; Two consecutive frames b in the sliding window k and b k+1 Inertial sensor measurements within; is the first l-th feature observed in the current image of the sliding window; ρ is the Huber norm; δ is the set of all inertial sensor measurements; is the feature set in the sliding window; χ is the complete state vector to be optimized; and denote the covariance matrices of the visual and inertial sensors respectively; Use Ceres solver to solve nonlinear problems and obtain local tracking results; (4b) Using the local tracking results and the global positioning results of UWB, a flexible evolutionary fusion framework is proposed to fuse multi-sensor data through the optimized EKF method; First predict the state covariance matrix: Among them, P k|k-1 is the posterior estimation error covariance matrix at time k-1; F k is the Jacobian matrix; μQ k-1 is the process noise covariance matrix at time k-1; μ is the elastic adjustment coefficient; The Kalman gain is: Among them, k k is the Kalman gain; H k is the Jacobian matrix of the observation model; R k is the observation noise covariance matrix; λ is the elastic adjustment coefficient; Update the state equation: in, is the positioning result of the state estimation at time k, Z k is the distance between the UAV and the UWB anchor point at time k; is the observation value calculated by the observation function h(·) at time k; Finally update the state covariance matrix: P k|k =(I9-k k H k )P k|k-1 (27) Among them, P k|k is the posterior estimation error covariance matrix at time k, I9 is ​​the unit matrix, and the elastic evolutionary fusion framework established uses λ and μ as elastic adjustment coefficients. Based on the above optimized EKF method, it is used to update the residual η of the state estimation k The calculation formula is as follows: The theoretical covariance obtained from this is for: Actual covariance of the residuals for: Deviation e of the constructed variance at time k k is the actual covariance Subtract the theoretical covariance The given e k As shown below: Based on the deviation e k The square of , constructs the fitness function: Then, the optimal elastic adjustment coefficients λ and μ are substituted into formula (26) for iteration, and then continuous monitoring is performed to elastically adjust the noise at each moment, and then elastic evolution fusion is performed to obtain accurate indoor positioning estimation results.

Images