Unmanned aerial vehicle navigation positioning method and system based on visual inertial odometer

Abstract

The invention provides an unmanned aerial vehicle navigation positioning method and system based on a visual inertial odometer, and relates to the technical field of unmanned aerial vehicle navigation, and the method comprises the steps: synchronously obtaining image data collected by an airborne visual sensor, inertial data output by an inertial measurement unit, and a motor control instruction signal generated by a flight control system; synchronously executing noise compensation on the inertial data by combining a preset thrust-rotating speed mapping relation according to the motor control instruction signal to obtain compensated inertial data; in a preset sliding window optimization framework, solving an optimal state estimation value based on the image data, the motor control instruction signal and the compensated inertial data; according to the method, through synchronous acquisition, noise compensation, state estimation optimization and navigation control strategies, the navigation positioning precision and the anti-interference capability of the unmanned aerial vehicle in a complex environment are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) navigation technology, and in particular to a UAV navigation and positioning method and system based on a visual inertial odometry. Background Art

[0002] With the rapid development of drone technology, drones have been widely used in numerous fields, including aerial photography, logistics, agriculture, and surveying and mapping. However, the navigation and positioning accuracy of drones is directly related to the safety and efficiency of their mission execution. Traditional drone navigation and positioning methods rely primarily on the Global Positioning System (GPS). However, in environments where GPS signals are blocked or weakened, such as indoors, in urban canyons, or in complex scenes like forests, GPS positioning accuracy can drop significantly or even fail. Therefore, developing a high-precision navigation and positioning method that does not rely on GPS is of great significance for improving drones' environmental adaptability and mission execution capabilities.

[0003] In recent years, visual-inertial odometry (VIO) technology has attracted considerable attention due to its ability to fuse data from visual and inertial sensors to achieve high-precision navigation and positioning. However, existing VIO methods still suffer from significant noise interference and inaccurate state estimation when handling complex conditions such as high-speed UAV motion and strong vibration.

[0004] Therefore, it is necessary to provide a UAV navigation and positioning method and system based on visual inertial odometry to solve the above technical problems. Summary of the Invention

[0005] To solve the above technical problems, the present invention provides a UAV navigation and positioning method and system based on visual inertial odometry, which significantly improves the navigation and positioning accuracy and anti-interference ability of the UAV in complex environments through synchronous acquisition, noise compensation, state estimation optimization and navigation control strategy. The present invention provides a method for navigation and positioning of an unmanned aerial vehicle (UAV) based on a visual inertial odometry (VIIO), the method comprising the following steps: Synchronously acquire image data collected by the onboard visual sensor, inertial data output by the inertial measurement unit, and motor control command signals generated by the flight control system; Synchronously performing noise compensation on the inertial data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertial data; In a preset sliding window optimization framework, based on the image data, the motor control command signal and the compensated inertial data, the following steps are performed progressively: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. Solving the optimal state estimation value by nonlinear optimization for the state vector combined with the constraint term, wherein the optimal state estimation value includes a position estimation value, a velocity estimation value, and a power model parameter estimation value; Navigation control is performed based on the optimal state estimation value.

[0006] Preferably, the synchronous acquisition of image data collected by an onboard visual sensor, inertial data output by an inertial measurement unit, and motor control command signals generated by a flight control system includes: Achieving time stamp synchronization of the onboard visual sensor, inertial measurement unit, and flight control system through a hardware trigger signal; Analyzing the motor control command signal to generate a real-time speed sequence corresponding to each motor; The real-time rotational speed sequence is matched with a preset thrust-rotational speed mapping database, and a vibration spectrum characteristic value is output.

[0007] Preferably, the synchronously performing noise compensation on the inertial data according to the motor control command signal in combination with a preset thrust-speed mapping relationship includes: Based on the vibration spectrum characteristic value, separating the high-frequency noise base component through a resonance frequency identification algorithm; Based on the high-frequency noise floor component and the acceleration spectrum characteristics of the inertial measurement unit, a dual-source nonlinear compensation manipulator is constructed; The dual-source nonlinear compensation operator acts on the acceleration and angular velocity of the inertial data to output compensated inertial data.

[0008] Preferably, the definition includes a state vector of the dynamic model parameters, including: extracting the angular velocity component of the compensated inertial data and calculating the instantaneous angular acceleration in the body coordinate system; Derived thrust gradient parameters of each rotor blade in combination with the motor control command signal; The instantaneous angular acceleration and the thrust change gradient parameter are integrated to generate a state vector.

[0009] Preferably, the construction of the constraint item includes: Calculating a theoretical angular acceleration prediction value based on the state vector; Performing time difference calculation on the angular velocity of the compensated inertial data to obtain an actual angular acceleration observation value; A residual term between the theoretical angular acceleration prediction value and the actual angular acceleration observation value is constructed as a constraint term.

[0010] Preferably, the step of combining the state vector with the constraint term to solve the optimal state estimate through nonlinear optimization includes: Based on the residual term, constructing an objective function including a dynamic weighting factor; extracting a thrust change gradient parameter from the state vector and calculating a credibility coefficient based on the vibration spectrum characteristic value; The dynamic weighting factor is adjusted according to the credibility coefficient, the objective function after the dynamic weighting factor adjustment is minimized using the LM optimization algorithm, and an optimal solution including a position estimate, a speed estimate, and a power model parameter estimate is output.

[0011] Preferably, performing navigation control based on the optimal state estimation value includes: Generate obstacle avoidance trajectory correction instructions based on the position estimate and speed estimate, combined with the obstacle spatial distribution map constructed in real time; The thrust fluctuation coefficient component in the power model parameter estimation value is extracted, the motor torque compensation amount is calculated in real time, the motor torque compensation amount is added to the original torque instruction of the flight control system, and an anti-disturbance motor control signal is output.

[0012] The present invention also provides a UAV navigation and positioning system based on visual inertial odometer, for executing the UAV navigation and positioning method based on visual inertial odometer, the system comprising; The data synchronization acquisition module is used to synchronously acquire image data collected by the onboard visual sensor, inertial data output by the inertial measurement unit, and motor control command signals generated by the flight control system; a noise compensation module, configured to synchronously perform noise compensation on the inertia data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertia data; A state estimation optimization module is configured to progressively perform the following steps based on the image data, the motor control command signal, and the compensated inertial data within a preset sliding window optimization framework: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. Solving the optimal state estimation value by nonlinear optimization for the state vector combined with the constraint term, wherein the optimal state estimation value includes a position estimation value, a velocity estimation value, and a power model parameter estimation value; A navigation control execution module is configured to execute navigation control based on the optimal state estimation value.

[0013] Compared with related technologies, the UAV navigation and positioning method and system based on visual inertial odometry provided by the present invention has the following beneficial effects: The present invention effectively improves the accuracy and reliability of data by synchronously collecting visual, inertial and motor control command signals, and using the thrust-speed mapping relationship to compensate for noise in the inertial data. In the preset sliding window optimization framework, by defining a state vector containing the dynamic model parameters and constructing constraint terms for nonlinear optimization solution, the position, speed and dynamic model parameters of the UAV can be estimated more accurately. Performing navigation control based on the optimal state estimate not only achieves high-precision obstacle avoidance trajectory planning, but also effectively suppresses the impact of external disturbances on the flight stability of the UAV by calculating the motor torque compensation in real time. This method significantly improves the navigation and positioning accuracy and robustness of UAVs in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 A flowchart of a UAV navigation and positioning method based on visual inertial odometry provided by the present invention; Figure 2 This is a module structure diagram of a UAV navigation and positioning system based on visual inertial odometry provided by the present invention. DETAILED DESCRIPTION

[0015] The present invention will be further described in detail below with reference to the accompanying drawings and examples. It will be understood that the specific embodiments described herein are intended only to illustrate the present invention and are not intended to limit the present invention. It should also be noted that, for ease of description, the accompanying drawings only illustrate portions relevant to the present invention, not all of the structures. Furthermore, the embodiments of the present invention and the features of the embodiments may be combined with one another unless there is a conflict.

[0016] It should also be noted that, for ease of description, only portions relevant to the present invention are shown in the accompanying drawings, rather than all of the contents. Before discussing the exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe the various operations (or steps) as sequential processes, many of the operations can be performed in parallel, concurrently, or simultaneously. In addition, the order of the various operations can be rearranged. The process can be terminated when its operations are completed, but may also have additional steps not included in the accompanying drawings. The process can correspond to a method, function, procedure, subroutine, subprogram, etc.

[0017] Example 1 The present invention provides a UAV navigation and positioning method based on visual inertial odometry. Figure 1 As shown, the method includes the following steps: S1: Synchronously acquire the image data collected by the onboard visual sensor, the inertial data output by the inertial measurement unit, and the motor control command signal generated by the flight control system.

[0018] Specifically, step S1 includes the following steps: S11: Achieving time stamp synchronization of the onboard visual sensor, inertial measurement unit, and flight control system through a hardware trigger signal.

[0019] In this embodiment, a reference clock module is configured on the main controller of the drone's onboard system, generating a trigger signal through a dedicated hardware circuit. The clock signal is connected to the trigger pin of the vision sensor, the synchronization input interface of the inertial measurement unit, and the signal output port of the flight control system, forming a star topology.

[0020] Each device has a built-in timestamp register, using a microsecond counter to precisely mark the moment of signal reception. When the system is running, the main controller uses a hardware interrupt mechanism to issue trigger pulses at a fixed interval: first, a 200-microsecond rising edge signal activates all devices, followed by an encoded instruction containing absolute time information. The vision sensor immediately initiates image exposure upon detecting the rising edge and writes a timestamp into the image data packet header upon completion. The inertial measurement unit locks the current sample value and binds it to the timestamp. The flight control system simultaneously records a snapshot of the motor control command status.

[0021] S12: Analyze the motor control command signal to generate a real-time speed sequence corresponding to each motor.

[0022] In this embodiment, the flight control system firmware has a built-in command decoding module that performs protocol parsing on received motor control commands and extracts the duty cycle parameters of the four independent motor channels based on common PPM or S.BUS control protocols.

[0023] Based on the KV value (speed coefficient per volt) in the motor performance parameter library and real-time battery voltage data, the duty cycle is converted to the theoretical speed value of each motor. A temperature compensation mechanism is introduced: a temperature sensor installed on the motor obtains real-time winding temperature. The temperature's effect on magnetic flux is calculated in combination with the material expansion coefficient, dynamically correcting the theoretical speed value. Finally, a sequence of four motor speeds within a continuous time window is constructed. This sequence contains the timestamps and the actual speed values ​​of the four motors at the corresponding moments, forming a standard time series data matrix for subsequent processing.

[0024] S13: Matching the real-time rotational speed sequence with a preset thrust-rotational speed mapping database, and outputting vibration spectrum characteristic values.

[0025] In this embodiment, a thrust-speed mapping relationship database stored in an onboard memory is called, and the database stores speed ranges, thrust coefficients, and vibration characteristic parameters classified by motor model.

[0026] The real-time motor speed sequence is extracted for feature extraction, calculating the average speed within a specific time window, the speed fluctuation variance, and the energy distribution within a preset frequency band. Based on these statistical features, a similarity match is performed within the mapping database, selecting the database entry with the closest vibration model parameters. Based on the matching results, the corresponding physical model parameter set is loaded, including the main vibration frequency range, fundamental wave amplitude coefficient, and harmonic amplification factor.

[0027] The vibration equation is applied to the actual motor speed to calculate the characteristic amplitudes of the main vibration frequency and harmonic components. The final output is a structured vibration eigenvector containing the main vibration frequency, harmonic energy percentage, fundamental acceleration amplitude, and resonance index. When the speed change exceeds a preset threshold, an online parameter fine-tuning mechanism is automatically triggered to update the eigenvalues.

[0028] S2: Synchronously performing noise compensation on the inertia data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertia data.

[0029] Specifically, step S2 includes the following steps: S21: Based on the vibration spectrum characteristic value, separate the high-frequency noise base component through a resonance frequency identification algorithm.

[0030] After reading the vibration spectrum eigenvalues ​​output in step S13, the resonance frequency analysis module is activated to perform noise separation. First, the vibration spectrum eigenvalues ​​undergo a three-stage decomposition: A multi-scale wavelet transform is used to segment the original spectrum into frequency bands, focusing on high-frequency components above 100 Hz. The characteristic resonance peaks are located within the target frequency band, using the current UAV's airframe structural natural frequency model (previously determined through modal testing). Based on the identified resonance peak center frequency and bandwidth parameters, the original spectrum eigenvalues ​​are adaptively filtered using a Gaussian band-stop filter. This process dynamically tracks frequency offsets, and a real-time calibration mechanism is automatically initiated when a frequency deviation exceeding 10% is detected. Ultimately, three core high-frequency noise components are output: main rotor resonance noise, structural harmonic noise, and motor electromagnetic noise, forming a complete set of high-frequency noise floor components.

[0031] S22: Constructing a dual-source nonlinear compensation operator based on the high-frequency noise floor component and the acceleration spectrum characteristics of the inertial measurement unit.

[0032] In this embodiment, the raw acceleration spectrum characteristics of the inertial measurement unit (IMU) are first analyzed: a short-time Fourier transform is performed on the three-axis acceleration data output by the IMU to generate a 0-1000Hz frequency domain feature distribution map. Spectral envelope parameters of the vibration-sensitive region are extracted, including the main energy concentration frequency band, spectral flatness, and peak signal-to-noise ratio. The high-frequency noise floor components obtained in step S21 are then integrated to establish a dual-source joint impact model: the structural resonance noise component is mapped to the vibration effect of the X / Y axes of the body coordinate system, and the motor electromagnetic noise component is mapped to the vibration effect of the Z axis. Based on the coupling analysis results, a composite nonlinear compensator is constructed: a variable-order Kalman filter is designed in the time domain, with differentiated update gains configured for the vibration characteristics of different axes. In the frequency domain, a multi-feedback loop notch filter group is established, with independent center frequency and attenuation parameters set for each identified noise component. Finally, the time-frequency domain processing modules are integrated and packaged into a unified dual-source nonlinear compensation operator, which contains 17 tunable compensation nodes.

[0033] S23: Applying the dual-source nonlinear compensation operator to the acceleration and angular velocity of the inertial data to output compensated inertial data.

[0034] In this embodiment, the dual-source nonlinear compensation operator operates according to a two-stage processing flow: the first stage processes acceleration data and inputs the original three-axis acceleration into three signal channels for parallel processing.

[0035] Each channel performs independent time-frequency hybrid noise reduction: baseline drift is first removed within a sliding window (window length 10ms), then a dedicated notch filter bank configured within the operator is applied, and finally residual pulse interference is eliminated through adaptive threshold blanking technology.

[0036] The second stage processes the angular velocity data using the rotating coordinate system decoupling technology: first, the original angular velocity in the body coordinate system is transformed into the rotor rotating coordinate system, and then decorrelation filtering based on vibration phase compensation is performed in this coordinate system, and then the data is inversely transformed back to the body coordinate system.

[0037] Finally, dynamic amplitude normalization is performed: the output gain is dynamically adjusted based on the signal-to-noise ratio of the preprocessor output to ensure that the compensation accuracy remains within ±16g and the angular velocity range is controlled within ±2000dps. A compensation quality indicator field is added to the output data, including signal distortion and noise suppression ratio indicators.

[0038] S3: In a preset sliding window optimization framework, based on the image data, the motor control command signal, and the compensated inertial data, the following steps are performed progressively: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. Solving the optimal state estimation value by nonlinear optimization for the state vector combined with the constraint term, wherein the optimal state estimation value includes a position estimation value, a velocity estimation value, and a power model parameter estimation value; Specifically, in step S3, the definition of the state vector includes the following steps: First, the angular velocity component of the compensated inertial data is extracted to calculate the instantaneous angular acceleration in the body coordinate system.

[0039] In this embodiment, differential processing is performed on each of the three body axes (X, Y, and Z) based on the compensated angular velocity data. A fifth-order central difference algorithm is used for the angular velocity sequence along each axis: five symmetrical sample points are selected within a time window to construct a least-squares fit curve, and its slope is calculated as the instantaneous angular acceleration value. To eliminate high-frequency noise interference, a motion trend detection mechanism is also implemented: when the rate of change of angular acceleration exceeds 15 rad / s³ for five consecutive cycles, an anti-shake range limiter is triggered, limiting the output to a range of ±800 rad / s². The final output is a three-axis angular acceleration vector with a confidence level.

[0040] Secondly, the thrust change gradient parameters of each rotor are derived in combination with the motor control command signal.

[0041] In this embodiment, the dynamic characteristics of the motor control command signal are analyzed: the rate of change of each motor command within a 10 ms time window is tracked in real time, and its normalized gradient value is calculated.

[0042] For quadrotor drones, a torque conversion matrix is ​​constructed based on the rotor arm geometry. This matrix projects the thrust variation of each motor onto the three rotation axes (pitch, roll, and yaw) of the aircraft coordinate system. Dynamic gain adjustment is introduced: Based on the current flight altitude, the air density table is used to obtain the pressure compensation coefficient, which is used to adjust the thrust-gradient conversion factor in real time. The output is a thrust gradient parameter matrix containing three degrees of freedom, along with a noise interference intensity indicator for each dimension.

[0043] Finally, the instantaneous angular acceleration and the thrust change gradient parameter are integrated to generate a state vector.

[0044] In this embodiment, a 15-dimensional state vector container is created to accommodate nine fundamental state variables: position, attitude, and velocity, plus six dynamic model parameters consisting of instantaneous angular acceleration and thrust gradient parameters. A covariance-based data fusion algorithm is employed: weights are assigned based on the accuracy of the sensor sources (angular velocity data is assigned a weight of 0.6, motor command data is assigned a weight of 0.4). When a parameter conflict is detected, a conflict resolution mechanism is activated: if the difference between two parameters exceeds three times the standard deviation, the higher-confidence source data is automatically selected to overwrite the lower-quality data. This ultimately generates a compressed state vector package with complete covariance information.

[0045] Specifically, in step S3, the construction of the constraint item includes the following steps: First, based on the state vector, the theoretical angular acceleration prediction value is calculated.

[0046] In this embodiment, angular acceleration is predicted using rigid-body dynamics equations based on the attitude and power parameters in the current state vector. First, a motor torque model is constructed based on the thrust gradient parameters, and the theoretical moment of inertia is calculated using the moment of inertia tensor. The aerodynamic drag coupling effect is then considered: a preset air drag coefficient table is consulted based on flight speed to construct a drag torque component that is a square of the speed. Finally, a gyroscopic effect term is added: a Coriolis force compensation equation is applied to the high-speed rotating rotor. All these components are vector-synthesized in the body coordinate system to generate a three-dimensional theoretical angular acceleration prediction.

[0047] Secondly, the angular velocity of the compensated inertial data is subjected to time difference calculation to obtain an actual angular acceleration observation value.

[0048] In this embodiment, time series processing is performed on the compensated angular velocity data: differential numerical integration is performed using the fourth-order Runge-Kutta method at a 100Hz sampling rate. An adaptive window mechanism is implemented: when the angular velocity change rate exceeds 200 rad / s², a small 1ms window mode is switched to improve transient response. A baseline correction technique is introduced: the historical drift is subtracted from the average value within the sliding window to eliminate long-term drift errors. The observation results are also labeled with quality assessment: if the angular acceleration exceeds a threshold (greater than 700 rad / s²) for five consecutive frames, an outlier warning flag is triggered.

[0049] Finally, a residual term between the theoretical angular acceleration prediction value and the actual angular acceleration observation value is constructed as a constraint term.

[0050] In this example, a two-factor residual model is designed: the base difference is the Euclidean distance between the theoretical prediction and the actual observation. A directional consistency constraint is added: the cosine of the angle between two vectors is calculated, and a penalty factor is applied when the angle is greater than 45°. A dynamic residual scaling mechanism is implemented: the residual sensitivity is automatically adjusted based on the resonance index in the vibration spectrum eigenvalues, significantly reducing the weight under strong resonance conditions (resonance index > 80). Finally, the Tukey robust function is applied to encapsulate the residuals to enhance the ability to resist outliers.

[0051] Specifically, in step S3, the process of solving the optimal state estimation value includes the following steps: Based on the residual term, an objective function including a dynamic weighting factor is constructed.

[0052] In this embodiment, the system uses a structured process to construct the objective function, first performing specialized preprocessing on the multi-source residuals. Visual residuals are resolution-normalized to eliminate dimensional differences, making data from different image sensors comparable. IMU residuals are smoothed and denoised using a second-order Butterworth filter with a 50Hz cutoff frequency. Dynamic residuals undergo coordinate system calibration to ensure that the vector direction is precisely aligned with the body coordinate system. The preprocessed residuals enter a labeling management module. Visual residuals are labeled as "V" sequences and record feature point association information. IMU residuals are labeled as "I" sequences with temperature and timestamps. Dynamic residuals are labeled as "D" sequences with confidence indicators, forming a clearly categorized residual source library.

[0053] A tiered strategy is implemented for weight distribution: the visual constraint maintains a fixed weight of 1.0 to ensure basic stability for visual positioning. The IMU constraint utilizes a temperature-adaptive mechanism, with a base weight of 0.8 that decays linearly when the temperature exceeds 40°C, decreasing by 1% for every 1°C increase, while maintaining a minimum threshold of 0.5. The dynamic constraint is fixed at a weight of 0.6 to ensure a consistent impact on the dynamic model. A matrix-based control system monitors weight distribution in real time: a red alert is triggered when the dynamic constraint's weight falls below 25% for three consecutive frames, and the event log records the abnormal status. A three-dimensional weight space locks the visual constraint to a global scope, the IMU constraint to the pose estimation domain, and the dynamic constraint for global domain monitoring.

[0054] The scale balance control system operates in two phases: Initialization collects the first frame data sample, calculates the initial mean of various residuals, sets a baseline scale factor, and verifies that the mean falls within the ideal range of 0.5-1.5. During runtime, dynamic adjustments are made, performing window checks every 10 frames. When the residual mean exceeds a limit, a gradient adjustment of the scale factor is initiated (maximum reduction of 2% / frame above the upper limit, maximum increase of 3% / frame below the lower limit). Special operating conditions activate a rapid response mechanism: image overexposure (brightness > 220) triggers a 5% adjustment rate mode, and intense maneuvers (angular velocity > 300° / s) activate a three-frame weight multiplication cycle for the dynamic residual.

[0055] The quality assurance system establishes a three-tiered control system: Hard thresholds are set for residual validity verification. Visual residual pixel deviations exceeding 15px are flagged as low confidence. IMU residuals are degraded when the temperature exceeds 50°C or the rate of change of angular acceleration exceeds 100 rad / s³. Power residual angular acceleration exceeding 800 rad / s² triggers protective limiting. The exception handling process utilizes a tree-like decision-making process: Prioritizing visual source failure increases the IMU weight to 0.9. Secondary to detecting power source anomalies, the weight is temporarily reduced to 0.3. Ultimately, an exception report containing event parameters is generated.

[0056] The thrust change gradient parameter in the state vector is extracted and combined with the vibration spectrum eigenvalue to calculate the credibility coefficient.

[0057] In this embodiment, the core parameter credibility is calculated by extracting the thrust variation gradient parameter as the main input source; calculating the signal integrity factor based on the main vibration frequency amplitude of the vibration spectrum eigenvalue; and performing a sliding window statistical analysis of the parameter's short-term stability. The final credibility coefficient is the geometric mean of the three indicators, converted into an exponential form, specifically expressed as: in, is the temperature influence coefficient, is the frequency offset, is the standard deviation of parameter fluctuation, and the result value range is limited to 0.05-0.95 to avoid extreme weight conditions.

[0058] The dynamic weighting factor is adjusted according to the credibility coefficient, the objective function after the dynamic weighting factor adjustment is minimized using the LM optimization algorithm, and an optimal solution including a position estimate, a speed estimate, and a power model parameter estimate is output.

[0059] After obtaining the credibility coefficient, the dynamic optimization phase begins. First, a pattern is divided based on the credibility value: when the coefficient is below 0.3, a conservative strategy is initiated, reducing the dynamic constraint weight to 30% of the baseline value and increasing the iterative damping; when the coefficient is above 0.7, an accelerated strategy is implemented, increasing the dynamic constraint weight and relaxing the convergence conditions. During the objective function reconstruction phase, vibration spectrum data is introduced to construct a penalty term, whose weight is negatively correlated with the credibility, forming an optimization framework that adapts to the vibration environment.

[0060] During the Levenberg-Marquardt algorithm iteration phase, a three-loop linkage mechanism is established: the inner loop performs standard iterative calculations to solve for the state vector increment; the middle loop dynamically adjusts the damping factor, significantly reducing the damping to accelerate convergence when the iteration gain exceeds 0.75, and increasing the damping to prevent divergence when it falls below 0.25; the outer loop continuously monitors the Jacobian matrix state, updating the matrix elements every 10 confidence intervals. The entire iteration process is terminated by three conditions: the residual modulus reaches the confidence scaling threshold, the matrix condition number exceeds the limit, or the dynamically adjusted upper limit on the number of iterations is set.

[0061] After optimization is complete, a four-level validation mechanism is implemented: the Mahalanobis distance is used to detect the degree of deviation in the estimated value. When it exceeds the 95% confidence interval, a hierarchical reoptimization is triggered: high confidence (greater than 0.6) initiates global reoptimization, medium confidence (0.3-0.6) adjusts only the dynamic parameters, and low confidence (less than 0.3) freezes the depth of the visual feature points. When the final output package containing position, velocity, and dynamic parameters is generated, a quality identification code is added to each data field: Level 0 (optimal solution) is directly output, Level 1 (minor deviation) adds a compensation factor, Level 2 (warning) activates the dynamic model, and Level 3 (serious anomaly) switches to a purely visual backup mode.

[0062] S4: Execute navigation control based on the optimal state estimation value.

[0063] Specifically, step S4 includes the following steps: S41: Generate an obstacle avoidance trajectory correction instruction based on the position estimation value and the speed estimation value in combination with the obstacle space distribution map constructed in real time.

[0064] In this embodiment, an octree spatial map is constructed in real time using an onboard ToF sensor, and global coordinate system transformations are performed in conjunction with position estimates. The collision risk sector within the next two seconds is predicted based on the velocity vector direction. When the obstacle density in a specific direction exceeds a threshold, a sinusoidal curve is superimposed on the target path to correct the trajectory. The trajectory optimizer design utilizes a two-layer planning structure: an upper-layer global A* algorithm for path planning and a lower-layer local DWA algorithm for obstacle avoidance. The output trajectory points are labeled with safety levels, ranging from Level 1 (safe) to Level 3 (forced avoidance) warning signs.

[0065] S42: extracting the thrust fluctuation coefficient component from the power model parameter estimation value, calculating the motor torque compensation in real time, adding the motor torque compensation to the original torque instruction of the flight control system, and outputting an anti-disturbance motor control signal.

[0066] In this embodiment, the thrust fluctuation coefficient component in the power model parameters is extracted and its time derivative is analyzed as the main compensation input source. Compensation control law design: is the torque compensation amount, is the proportional gain coefficient, is the thrust fluctuation rate, is the differential gain coefficient, is the thrust fluctuation acceleration, where Take 0.6N·m / rpm, The value is 0.2 N·m·s / rpm. Smoothing is performed before superposition: a digital ramp function is applied to limit the torque change rate to no more than 10 N·m / s. The compensation result is integrated into the flight control system: the compensation amount is superimposed on the basic PID output to generate the final PWM waveform control signal.

[0067] Example 2 The present invention also provides a UAV navigation and positioning system based on visual inertial odometry, which is used to execute the UAV navigation and positioning method based on visual inertial odometry. Figure 2 As shown, the system comprises; The data synchronization acquisition module 100 is used to synchronously acquire image data collected by the onboard visual sensor, inertial data output by the inertial measurement unit, and motor control command signals generated by the flight control system.

[0068] The noise compensation module 200 is configured to synchronously perform noise compensation on the inertia data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertia data.

[0069] The state estimation optimization module 300 is configured to progressively perform the following steps based on the image data, the motor control command signal, and the compensated inertial data within a preset sliding window optimization framework: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. The state vector is combined with the constraint item to obtain an optimal state estimation value through nonlinear optimization, wherein the optimal state estimation value includes a position estimation value, a speed estimation value and a power model parameter estimation value.

[0070] The navigation control execution module 400 is configured to execute navigation control based on the optimal state estimation value.

[0071] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems) and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0072] Those skilled in the art will appreciate that all or part of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program. The program can be stored in a computer-readable storage medium, including a read-only memory (ROM), a random access memory (RAM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), a one-time programmable read-only memory (OTPROM), an electronically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM), or other optical disc storage, magnetic disk storage, or magnetic tape storage, or any other computer-readable medium capable of carrying or storing data.

[0073] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

Claims

1. A UAV navigation and positioning method based on visual inertial odometry, characterized in that: The method comprises the following steps: Synchronously acquire image data collected by the onboard visual sensor, inertial data output by the inertial measurement unit, and motor control command signals generated by the flight control system; Synchronously performing noise compensation on the inertial data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertial data; In a preset sliding window optimization framework, based on the image data, the motor control command signal and the compensated inertial data, the following steps are performed progressively: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. Solving the optimal state estimation value by nonlinear optimization for the state vector combined with the constraint term, wherein the optimal state estimation value includes a position estimation value, a velocity estimation value, and a power model parameter estimation value; Navigation control is performed based on the optimal state estimation value.

2. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 1, characterized in that: The synchronous acquisition of image data collected by an onboard visual sensor, inertial data output by an inertial measurement unit, and motor control command signals generated by a flight control system includes: Achieving time stamp synchronization of the onboard visual sensor, inertial measurement unit, and flight control system through a hardware trigger signal; Analyzing the motor control command signal to generate a real-time speed sequence corresponding to each motor; The real-time rotational speed sequence is matched with a preset thrust-rotational speed mapping database, and a vibration spectrum characteristic value is output.

3. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 2, characterized in that: The noise compensation is synchronously performed on the inertial data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship, including: Based on the vibration spectrum characteristic value, separating the high-frequency noise base component through a resonance frequency identification algorithm; Based on the high-frequency noise floor component and the acceleration spectrum characteristics of the inertial measurement unit, a dual-source nonlinear compensation manipulator is constructed; The dual-source nonlinear compensation operator acts on the acceleration and angular velocity of the inertial data to output compensated inertial data.

4. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 3, wherein: The definition includes a state vector of dynamic model parameters, including: extracting the angular velocity component of the compensated inertial data and calculating the instantaneous angular acceleration in the body coordinate system; Derived thrust gradient parameters of each rotor blade in combination with the motor control command signal; The instantaneous angular acceleration and the thrust change gradient parameter are integrated to generate a state vector.

5. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 4, characterized in that: The construction of the constraint items includes: Calculating a theoretical angular acceleration prediction value based on the state vector; Performing time difference calculation on the angular velocity of the compensated inertial data to obtain an actual angular acceleration observation value; A residual term between the theoretical angular acceleration prediction value and the actual angular acceleration observation value is constructed as a constraint term.

6. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 5, characterized in that: The step of combining the state vector with the constraint term to solve the optimal state estimation value through nonlinear optimization includes: Based on the residual term, constructing an objective function including a dynamic weighting factor; extracting a thrust change gradient parameter from the state vector and calculating a credibility coefficient based on the vibration spectrum characteristic value; The dynamic weighting factor is adjusted according to the credibility coefficient, the objective function after the dynamic weighting factor adjustment is minimized using the LM optimization algorithm, and an optimal solution including a position estimate, a speed estimate, and a power model parameter estimate is output.

7. The method for navigation and positioning of an unmanned aerial vehicle based on visual inertial odometry according to claim 6, characterized in that: The performing navigation control based on the optimal state estimation value includes: Generate obstacle avoidance trajectory correction instructions based on the position estimate and speed estimate, combined with the obstacle spatial distribution map constructed in real time; The thrust fluctuation coefficient component in the power model parameter estimation value is extracted, the motor torque compensation amount is calculated in real time, the motor torque compensation amount is added to the original torque instruction of the flight control system, and an anti-disturbance motor control signal is output.

8. A UAV navigation and positioning system based on visual inertial odometry, used to execute the UAV navigation and positioning method based on visual inertial odometry according to any one of claims 1 to 7, characterized in that: The system includes: The data synchronization acquisition module is used to synchronously acquire image data collected by the onboard visual sensor, inertial data output by the inertial measurement unit, and motor control command signals generated by the flight control system; a noise compensation module, configured to synchronously perform noise compensation on the inertia data according to the motor control command signal and in combination with a preset thrust-speed mapping relationship to obtain compensated inertia data; A state estimation optimization module is configured to progressively perform the following steps based on the image data, the motor control command signal, and the compensated inertial data within a preset sliding window optimization framework: Define the state vector containing the dynamic model parameters, Based on the state vector, a constraint term is constructed. Solving the optimal state estimation value by nonlinear optimization for the state vector combined with the constraint term, wherein the optimal state estimation value includes a position estimation value, a velocity estimation value, and a power model parameter estimation value; A navigation control execution module is configured to execute navigation control based on the optimal state estimation value.

Images