Visual inertial navigation fused unmanned aerial vehicle trajectory tracking control method and system

Abstract

The invention discloses a visual inertial navigation fused unmanned aerial vehicle trajectory tracking control method and system, and relates to the technical field of unmanned aerial vehicle navigation and control, and the method comprises the steps: obtaining multi-mode perception data, and carrying out the timestamp alignment; constructing a visual odometer network to extract a relative pose variation, and constructing an inertial integral network to extract a short-time motion estimator; a self-adaptive confidence fusion module is constructed, a visual modal weight is dynamically adjusted according to environment texture richness, an inertial navigation modal weight is dynamically adjusted according to maneuvering intensity, and self-adaptive weighted fusion is realized through a learnable attention mechanism to output high-precision pose estimation; and inputting fusion pose estimation into a nonlinear model prediction controller, and outputting an expected attitude angle and a thrust instruction to realize accurate trajectory tracking.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation and control technology, specifically to a UAV trajectory tracking and control method and system based on visual-inertial navigation fusion. Background Technology

[0002] UAV trajectory tracking and control is one of the core technologies for achieving autonomous flight of UAVs, and its performance directly determines the effectiveness of UAV applications in fields such as aerial surveying and mapping, logistics delivery, agricultural plant protection, and emergency rescue. Precise trajectory tracking and control relies on high-precision state estimation, and the accuracy of state estimation depends on the reliable fusion of sensor data. Currently, visual sensors and inertial measurement units (IMUs) are the two most commonly used types of sensors for UAV positioning and navigation. Visual sensors provide rich environmental information but are susceptible to changes in lighting and texture loss, while IMUs provide high-frequency motion measurements but suffer from cumulative drift. Effectively fusing information from these two complementary sensors is key to improving the accuracy of UAV trajectory tracking and control.

[0003] Chinese patent CN120406506A discloses a dynamic target tracking method for unmanned aerial vehicles (UAVs) based on end-to-end learning. This method collects multimodal input data, including visual images and IMU motion information, extracts visual and motion features through a dual-stream convolutional neural network, fuses multimodal features using a graph attention network, models the spatial relationship between the UAV, the target, and the environment, and generates the UAV tracking trajectory based on a deep reinforcement learning strategy. While this method achieves multimodal data fusion, it still has the following shortcomings: First, when using a graph attention network for feature fusion, it does not consider the impact of environmental changes on the reliability of different modalities. When the visual sensor degrades in scenes with weak texture or drastic lighting changes, the fixed fusion strategy cannot guarantee positioning accuracy. Second, the method uses deep reinforcement learning to generate the trajectory, but the training of its policy network requires a large number of samples and has limited generalization ability, making it difficult to guarantee tracking performance for unseen complex maneuvers. Third, this method does not establish a tight coupling between visual-inertial navigation fusion positioning and trajectory tracking control, and lacks an effective suppression mechanism for the transmission of positioning errors to the control system.

[0004] Furthermore, existing visual-inertial navigation fusion methods are mainly divided into two architectures: loosely coupled and tightly coupled. Loosely coupled methods operate visual odometry and inertial navigation as independent systems, then fuse their outputs through filters. This method is simple to implement but struggles to fully utilize the complementary characteristics of the two sensors. Tightly coupled methods jointly solve for visual feature points and inertial measurements within a unified optimization framework, achieving higher positioning accuracy, but with higher computational complexity and strict requirements for sensor synchronization accuracy. Regardless of whether the methods are loosely or tightly coupled, existing technologies generally employ fixed fusion weights or adaptive weight adjustment strategies based on the covariance matrix, failing to fully consider the direct impact of environmental conditions and motion states on sensor reliability. When a UAV flies over areas with weak textures or performs violent maneuvers, the measurement quality of either the visual or inertial sensors deteriorates significantly, and fixed-weight fusion strategies lead to a deterioration in positioning accuracy.

[0005] In trajectory tracking control, existing technologies mainly employ PID control, sliding mode control, and model predictive control. PID control has a simple structure but struggles to handle multivariable coupling and input-output constraints. Sliding mode control offers strong robustness but suffers from chattering. Model predictive control can explicitly handle constraints and optimize multi-step performance indicators. However, existing model predictive control methods typically separate state estimation and controller design, failing to establish a direct correlation between positioning accuracy and control performance. When the positioning system's accuracy degrades in specific scenarios, the controller cannot adjust its strategy in a timely manner to compensate for the impact of positioning errors. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a visual-inertial fusion-based UAV trajectory tracking control method. This method uses an adaptive confidence fusion module to dynamically adjust the fusion weights of the visual and inertial modes based on the richness of environmental texture and the intensity of maneuvering. It utilizes a learnable attention mechanism to achieve adaptive weighted fusion of the two modes and inputs the fused pose estimation result into a nonlinear model predictive controller to achieve accurate trajectory tracking. This effectively solves the problems of decreased positioning accuracy and deteriorated control performance in visual degradation scenarios in existing technologies.

[0007] The visual-inertial fusion-based UAV trajectory tracking and control method provided by this invention includes the following steps:

[0008] S1. Multimodal data synchronous acquisition: Acquire multimodal perception data during the flight of the UAV. The multimodal perception data includes environmental image sequences acquired by the forward-facing binocular camera and acceleration and angular velocity data acquired by the inertial measurement unit. Align the environmental image sequences and acceleration and angular velocity data according to timestamps to obtain a synchronous perception data stream for fusion positioning.

[0009] S2. Dual-modal feature extraction: For the environmental image sequence in the synchronous sensing data stream, a visual odometry network is constructed to extract the relative pose change between consecutive frames; for the acceleration and angular velocity data in the synchronous sensing data stream, an inertial integral network is constructed to extract the short-time motion estimate between adjacent moments.

[0010] S3. Adaptive Confidence Fusion: An adaptive confidence fusion module is constructed. This module calculates the richness of environmental texture based on the environmental image sequence and dynamically adjusts the visual modality weights accordingly. It calculates the intensity of motion based on acceleration and angular velocity data and dynamically adjusts the inertial navigation modality weights accordingly. After normalizing the visual modality weights and inertial navigation modality weights through a learnable attention mechanism, it achieves adaptive weighted fusion of relative pose change and short-term motion estimate, and outputs a high-precision pose estimate.

[0011] S4. Model Predictive Trajectory Control: The high-precision attitude estimation is used as the state feedback input to the nonlinear model predictive controller. Based on the deviation between the desired trajectory and the high-precision attitude estimation, the nonlinear model predictive controller solves the optimal control sequence in the prediction time domain and outputs the desired attitude angle and thrust command to the UAV flight control system to achieve precise tracking control of the desired trajectory.

[0012] This invention also provides a visual-inertial fusion-based UAV trajectory tracking control system, comprising:

[0013] The multimodal data acquisition module is configured to acquire multimodal perception data during the flight of the UAV and perform timestamp alignment processing, and output synchronous perception data stream. The multimodal perception data includes environmental image sequences acquired by the forward-facing binocular camera and acceleration and angular velocity data acquired by the inertial measurement unit.

[0014] The dual-modal feature extraction module is configured to extract visual modal features and inertial navigation modal features from the synchronous sensing data stream, respectively. The dual-modal feature extraction module includes a visual odometry network and an inertial integral network. The visual odometry network is used to extract the relative pose change between consecutive frames, and the inertial integral network is used to extract the short-time motion estimate between adjacent time points.

[0015] The adaptive confidence fusion module is configured to dynamically adjust the visual modal weights based on the richness of the environmental texture and the inertial navigation modal weights based on the intensity of the maneuver. It achieves adaptive weighted fusion of relative pose change and short-term motion estimate through a learnable attention mechanism, and outputs a high-precision pose estimate.

[0016] The model predictive control module is configured to receive high-precision pose estimation as state feedback, solve for the optimal control sequence based on the desired trajectory, and output the desired attitude angle and thrust command. The model predictive control module uses a nonlinear model predictive controller to achieve accurate tracking control of the desired trajectory.

[0017] Compared with the prior art, the present invention has the following beneficial effects:

[0018] First, this invention achieves dynamic adaptive adjustment of the fusion weights of visual and inertial modes through environmental texture richness assessment and maneuver intensity assessment. When the reliability of the visual sensor decreases in a weak texture scene, the weight of the inertial mode is automatically increased. When the cumulative error of the inertial sensor increases during severe maneuvering, the weight of the visual mode is automatically increased, effectively solving the problem of performance degradation of a single mode in a specific scenario.

[0019] Second, this invention employs a learnable attention mechanism for multimodal fusion. Through end-to-end training, the fusion weights can adaptively match different environmental conditions and motion states. Compared with fixed weights or covariance-based fusion methods, this invention can better leverage the complementary advantages of the two sensors.

[0020] Third, this invention tightly couples adaptive confidence fusion positioning with nonlinear model predictive control. Changes in positioning accuracy can be directly reflected in the state feedback of the controller through pose estimation. The controller can adjust the control strategy accordingly, realizing the coordinated optimization of positioning and control. The position tracking error is reduced by 40% compared with the single-modal method, and stable control can still be maintained in visual degradation scenarios. Attached Figure Description

[0021] Figure 1 This is a flowchart of the UAV trajectory tracking and control method based on visual-inertial fusion of the present invention.

[0022] Figure 2 This is an architecture diagram of the UAV trajectory tracking control system based on the visual-inertial fusion of the present invention. Detailed Implementation

[0023] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. The following embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention.

[0024] This invention provides a visual-inertial fusion-based UAV trajectory tracking and control method, such as... Figure 1 As shown, this method includes four core steps: synchronous acquisition of multimodal data, dual-modal feature extraction, adaptive confidence fusion, and model predictive trajectory control. These four steps form a complete closed loop from sensor data acquisition to flight control command output. Among them, the adaptive confidence fusion module is the key to achieving high-precision positioning, and the model predictive controller is the core to achieving accurate trajectory tracking.

[0025] Step S1: Synchronous acquisition of multimodal data.

[0026] In one embodiment of the present invention, the sensor system carried by the UAV includes two core types of sensors: a forward-facing binocular camera and an inertial measurement unit (IMU). Preferably, the forward-facing binocular camera is a global shutter camera to avoid image distortion caused by a rolling shutter, with a baseline length of 12cm, a resolution of 640×480 pixels, and a frame rate of 30Hz. The IMU is a six-axis IMU, including a three-axis accelerometer and a three-axis gyroscope. The accelerometer has a range of ±16g, the gyroscope has a range of ±2000° / s, and the sampling frequency is 200Hz. In one embodiment of the present invention, there is an inherent deviation between the exposure time of the binocular camera and the sampling time of the IMU. This time deviation can introduce significant positioning errors during high-speed movement, therefore, precise timestamp alignment is required.

[0027] Specifically, the timestamp alignment process is as follows: First, at the hardware level, coarse synchronization between camera exposure and IMU sampling is achieved through GPIO trigger signals, keeping the time deviation between the two within 5ms. Then, at the software level, linear interpolation alignment of the camera image frame's exposure time is performed based on the IMU's high-frequency sampling time. Let the exposure time of the k-th frame be... The two most recent IMU sampling times before and after are respectively and The alignment factor is calculated as follows:

[0028] ,

[0029] in: These are linear interpolation coefficients, with values ​​ranging from [0,1], representing the relative position of the image exposure time within adjacent IMU sampling intervals; The exposure time of the k-th frame image, in seconds, is obtained by the camera driver from the image metadata; and These are the closest IMU sampling times before and after the image exposure time, in seconds, recorded by the IMU driver. These interpolation coefficients are used to subsequently correlate image frames with the corresponding IMU measurements, achieving precise time alignment between the visual frame and the inertial navigation measurement, with a time deviation controllable within 1ms.

[0030] In one embodiment of the present invention, the data structure of the synchronous sensing data stream is designed as follows: each synchronous data frame includes a pair of binocular images, the corresponding IMU measurement value, and all IMU pre-integration results since the previous keyframe. Preferably, the keyframe selection strategy adopts a parallax-based method; when the average feature point displacement between consecutive frames exceeds 20 pixels or the time interval exceeds 0.5s, the current frame is set as the new keyframe. This selection strategy can control the computational load while ensuring positioning accuracy, enabling the system to run in real time on an embedded platform.

[0031] In one embodiment of the present invention, a data validity check mechanism is also provided to improve the reliability of data synchronization. Specifically, when the exposure time of an image frame deviates from the expected time by more than 10 ms, when there are frame drops or timestamp jumps in the IMU data, or when the timestamp deviation of the binocular image exceeds 1 ms, the data frame will be marked as invalid and a resynchronization process will be triggered. This mechanism can effectively avoid the decrease in positioning accuracy caused by data synchronization failure.

[0032] Step S2: Dual-modal feature extraction.

[0033] This step constructs a visual odometry network and an inertial integration network for the synchronous sensing data stream to extract features from the visual and inertial modes. The two networks run in parallel, outputting relative pose change and short-time motion estimates, respectively, providing input for subsequent adaptive confidence fusion.

[0034] In one embodiment of the present invention, the visual odometry network employs an encoder-decoder structure. The encoder uses ResNet-18 as its backbone network, feeding binocular image pairs into two weight-sharing encoder branches to extract multi-scale visual features. Preferably, the encoder outputs feature maps at four scales, with resolutions of 1 / 4, 1 / 8, 1 / 16, and 1 / 32 of the original image, and channel numbers of 64, 128, 256, and 512, respectively. This multi-scale feature extraction strategy can simultaneously capture detailed information and global semantic information of the image, improving the robustness of pose estimation.

[0035] In one embodiment of the present invention, the decoder first constructs a cost volume from the binocular feature maps, forming a four-dimensional cost volume in the disparity dimension by concatenating pixel-by-pixel features. Then, a 3D convolutional network is used to regularize the cost volume, outputting a disparity map and a confidence map. Preferably, the 3D convolutional network contains four residual blocks, each containing two layers of 3×3×3 3D convolutions, batch normalization, and ReLU activation functions. The disparity map is used for triangulation to calculate depth information, and the confidence map is used for subsequent feature weighting.

[0036] In one embodiment of the present invention, after obtaining depth information, the decoder establishes geometric constraints between consecutive frames through a differentiable reprojection operation. Let the current frame be... The reference frame is Pixels in the current frame The depth is Then its projection position in the reference frame is:

[0037] ,

[0038] in: These are the pixel coordinates in the current frame image, expressed in homogeneous coordinate form. , and These are the horizontal and vertical coordinates of a pixel, and their value range is determined by the image resolution. In this embodiment... , ; These are the projected pixel coordinates in the reference frame image, also in homogeneous coordinate form. This is the camera intrinsic parameter matrix, including focal length. , and optical center coordinates , In this embodiment Pixels Pixels Pixel; This is the inverse of the camera intrinsic parameter matrix; This represents the depth value of the current frame's pixels, in meters, obtained from the disparity map. The calculation yielded, where The baseline length of the binoculars. This represents the disparity value. Let be the rotation matrix from the current frame to the reference frame, which is a 3×3 orthogonal matrix belonging to the special orthogonal group SO(3); This is the translation vector from the current frame to the reference frame, a 3×1 column vector in meters.

[0039] Based on the aforementioned reprojection relationship, the visual odometry network estimates the relative pose change by minimizing a joint loss function of photometric and geometric errors. The photometric error is defined as the brightness difference between the projected and actual pixels in the reference frame, and the geometric error is defined as the reprojection distance of the feature points. Preferably, the photometric error weight in the joint loss function is 0.85, and the geometric error weight is 0.15. Finally, the fully connected layer of the decoder outputs the six-DOF relative pose change. The first three components are translations in meters, and the last three components are Euler angle increments in radians.

[0040] In one embodiment of the present invention, the inertial integrator network employs an IMU pre-integration method to extract short-time motion estimates. Pre-integration is a technique that incrementally accumulates IMU measurements between adjacent keyframes, effectively reducing the computational cost of repeated integration and facilitating error propagation during factor graph optimization.

[0041] Specifically, let adjacent keyframes be set and There are a total of The IMU measurement, the first Each measurement includes acceleration. and angular velocity The sampling time interval is Pre-integral rotational increment Speed ​​increment and position increment The recursive calculation formula is as follows:

[0042] ,

[0043] ,

[0044] ,

[0045] in: To extract from keyframes To keyframe The relative rotation matrix belongs to the special orthogonal group SO(3), which represents the attitude change of the UAV between two keyframes; This is the velocity increment vector, with units of m / s, representing the velocity change of the UAV in the body coordinate system; This is the position increment vector, in meters, representing the displacement change of the UAV in the body coordinate system; For the first The gyroscope measurement value at each sampling time, in rad / s, with a range of [-35, 35] rad / s (corresponding to a range of ±2000° / s). For the first The accelerometer measurement value at each sampling time, in m / s², with a range of [-157, 157] m / s² (corresponding to ±16g range); The gyroscope has zero bias, measured in rad / s. The initial value is set to zero and updated online. The typical value is within ±0.01 rad / s. The accelerometer is zero biased, with units of m / s². The initial value is set to zero and updated through online estimation. The typical value is within the range of ±0.1 m / s². The IMU sampling time interval is 5ms in this embodiment (corresponding to a 200Hz sampling rate). For the exponential mapping from Lie algebra to Lie group, the angular velocity vector is mapped to a rotation matrix; To extract from keyframes To the The cumulative rotation matrix at each sampling time; To extract from keyframes To the The cumulative velocity increment at each sampling time.

[0046] In one embodiment of the present invention, to account for the impact of IMU measurement noise on the pre-integration results, the inertial integrator network also calculates the pre-integration covariance matrix. The recursive update formula for the covariance matrix is:

[0047] ,

[0048] in: For the first The pre-integral covariance matrix at each sampling time point is a 9×9 symmetric positive definite matrix, and its diagonal elements reflect the degree of uncertainty in rotation, velocity, and position estimation. Let be the state transition Jacobian matrix, which is a 9×9 matrix, describing the propagation relationship of the pre-integration error from the previous time step to the current time step; Let be the noise Jacobian matrix, which is a 9×6 matrix, describing the propagation relationship of IMU measurement noise to pre-integration error; The IMU measurement noise covariance matrix is ​​a 6×6 diagonal matrix. The first three diagonal elements represent the gyroscope noise variance, with values ​​of [value missing]. (rad / s)², where the last three diagonal elements represent the accelerometer noise variance, with values ​​of (rad / s)². (m / s²)².

[0049] Preferably, the inertial integrator network resets the pre-integration results at each keyframe and performs a first-order correction to the pre-integration results based on the IMU zero bias estimated by the visual odometry network. This design effectively suppresses the cumulative drift of the pre-integration while maintaining computational efficiency. The final output is a short-time motion estimate. It includes pre-integrated pose increments and their covariance information, providing an uncertainty measure for subsequent adaptive confidence fusion.

[0050] Step S3: Adaptive confidence fusion.

[0051] This step is the core innovation of the present invention. By constructing an adaptive confidence fusion module, the fusion weights of the visual modality and the inertial navigation modality are dynamically adjusted according to environmental conditions and motion state. The optimal fusion of the two modalities is achieved by utilizing a learnable attention mechanism.

[0052] In one embodiment of the present invention, the evaluation of environmental texture richness is crucial for determining the reliability of visual modalities. When the environmental texture is rich, visual feature point matching is accurate, and the relative pose change output by the visual odometry network has high reliability; when the environmental texture is sparse, the number of feature points is insufficient and matching is prone to errors, reducing the reliability of the visual odometry network's output. This invention proposes a texture richness evaluation method based on corner response and distribution uniformity.

[0053] Specifically, firstly, Harris corner detection or Shi-Tomasi corner detection is performed on the environmental image to obtain a set of corners. ,in This represents the number of corner points detected. Then, the image is divided into... The grid is used to count the number of corner points in each grid cell. ( , Environmental texture richness index The calculation formula is as follows:

[0054] ,

[0055] in: This is an indicator of environmental texture richness, with a value range of (0,1). The larger the value, the richer the texture and the more reliable the visual modality. The Sigmoid activation function maps the input to the (0,1) interval, as shown in the formula. ; The number of corner points detected in the current frame is a dimensionless integer with a value range of [0, +∞), typically 50 to 2000. The reference corner number threshold is set to 500, which represents the typical number of corners in a scene with good texture. This value is determined based on the image resolution and scene type. The standard deviation of the number of corner points in each grid reflects the dispersion of the corner point distribution and is dimensionless. This is the average number of corner points in each grid, reflecting the average density of corner point distribution, and is dimensionless. To prevent small constants from being divided by zero, the value is set to 1; and These are weighting coefficients, controlling the contribution of corner point quantity and distribution uniformity to texture richness, respectively. Set to 1.5. The ratio is set to 0.5, and the sum of the two is 2.0. This ratio is calibrated based on a large amount of flight data and can obtain stable texture evaluation results in various scenarios.

[0056] In one embodiment of the present invention, the mesh generation parameters and The resolution is set to 8×6, dividing a 640×480 image into 48 grids of 80×80 pixels each. When the number of corner points in a grid is zero or significantly lower than the mean, it indicates that the texture in that area is sparse, which may affect the local estimation accuracy of visual odometry. By introducing a distribution uniformity term, we can distinguish between corner points concentrated in local areas and those evenly distributed throughout the image. The former may meet the total number requirement, but its spatial coverage is insufficient, while the latter represents the ideal texture condition.

[0057] In one embodiment of the present invention, assessing the severity of maneuvers is crucial for determining the reliability of inertial navigation modes. When the UAV flies smoothly, IMU measurements change slowly, pre-integration accumulation error is small, and the short-time motion estimate output by the inertial integration network has high reliability. When the UAV performs severe maneuvers, IMU measurements change rapidly, pre-integration accumulation error increases rapidly, and the reliability of the output of the inertial integration network decreases. The present invention proposes a method for assessing the severity of maneuvers based on the standard deviation of IMU measurements.

[0058] Specifically, in the sliding time window Internal, statistical acceleration measurements and angular velocity measurement value The standard deviation of the maneuver intensity index. The calculation formula is as follows:

[0059] ,

[0060] in: This is the inverse expression of the severity of the maneuver, with a value range of (0,1). The larger the value, the smoother the maneuver and the more reliable the inertial navigation mode. is the standard deviation vector of the acceleration measurements within the sliding window, which is a 3×1 vector with units of m / s². is the standard deviation vector of the angular velocity measurements within the sliding window, which is a 3×1 vector with units of rad / s; Let be the Euclidean norm of the vector; The standard deviation of acceleration is set to a reference threshold of 2.0 m / s², representing the dividing point between stable flight and violent maneuvering. This value is determined based on typical flight data. The reference threshold for the standard deviation of angular velocity is set to 0.5 rad / s (approximately 28.6° / s), representing the dividing point between stable flight and violent maneuvers; and These are weighting coefficients, which control the contributions of acceleration and angular velocity to the degree of maneuverability, respectively. Set to 1.0. The value is set to 1.0, meaning that acceleration and angular velocity contribute equally to the degree of maneuverability. The number of IMU samples within the sliding window depends on the window length. And sampling frequency, in this embodiment The setting is 0.1s, and the sampling frequency is 200Hz, therefore .

[0061] In one embodiment of the present invention, the environmental texture richness index and the intensity of maneuvering indicators As input to the adaptive confidence fusion module, the final fusion weights are generated after processing by a learnable attention mechanism. The design principle of the attention mechanism is as follows: the two confidence indicators are mapped to query vectors and key vectors respectively, the interaction relationship between the two modalities is calculated through dot product operation, and then the fusion weights are obtained after Softmax normalization.

[0062] Specifically, visual modality confidence The query vector is obtained through a learnable linear transformation. and key vector Inertial navigation mode confidence The query vector is obtained through a learnable linear transformation. and key vector The formula for calculating the cross-attention score is as follows:

[0063] ,

[0064] ,

[0065] in: The final fusion weights for the visual modal are defined, with values ​​ranging from (0,1). The final fusion weights for the inertial navigation modes are defined, with values ​​ranging from (0,1), and are related to... The sum of them is always 1; The query vector for the visual modality has dimensions of [dimensionality]. ,Depend on Learnable parameter matrix Obtained by linear transformation; The key vector of the visual modality has a dimension of ,Depend on Learnable parameter matrix Obtained by linear transformation; The query vector for the inertial navigation mode has a dimension of ,Depend on Learnable parameter matrix Obtained by linear transformation; Let be the key vector of the inertial navigation mode, with dimension . ,Depend on Learnable parameter matrix Obtained by linear transformation; The key vector dimension is set to 64. This dimension determines the expressive power of the attention mechanism. Too small a dimension will limit the model capacity, while too large a dimension will increase the computational cost. This is a scaling factor used to prevent the softmax gradient from vanishing due to an excessively large dot product result; this is a standard scaling dot product attention design.

[0066] In one embodiment of the present invention, a learnable parameter matrix , , , During the training phase, end-to-end optimization is performed through backpropagation. The training data includes real flight data under various environmental conditions and motion states, enabling the attention mechanism to learn the optimal fusion strategy. Preferably, the training loss function is defined as the weighted error between the fused pose estimate and the ground truth pose, with a position error weight of 1.0 and a pose error weight of 0.5.

[0067] After obtaining the fusion weights, the adaptive confidence fusion module adjusts the relative pose change output by the visual odometry network. Short-time motion estimate output by inertial integral network Weighted fusion is performed. The resulting high-precision pose estimation... The calculation uses a manifold weighted average method:

[0068] ,

[0069] in: For high-precision pose estimation after fusion, it belongs to the special Euclidean group SE(3), which includes 3 degrees of freedom rotation and 3 degrees of freedom translation; The relative pose change output by the visual odometry network belongs to SE(3); The short-time motion estimate output by the inertial integral network belongs to SE(3); The fusion weights for the inertial navigation modes range from (0,1); For addition operations on the SE(3) manifold, add the Lie algebra vector to the elements of the Lie group; For subtraction operations on the SE(3) manifold, calculate the Lie algebra difference between two Lie group elements. This manifold weighted average formula ensures that the fusion result still lies on the SE(3) manifold, avoiding the non-orthogonality problem that may arise from direct linear interpolation of the rotation matrix.

[0070] In one embodiment of the present invention, when the visual environment is good and the maneuver is stable, and Approaching 0.5, the contributions of both modalities are comparable; however, when the visual environment degrades (e.g., weak texture, strong lighting changes), Reduce As the inertial navigation system (INS) mode increases, the fusion results become more dependent on the INS modes; when severe maneuvers lead to an increase in the cumulative INS error, the fusion results become more dependent on the INS modes. Enlarge The fusion result relies more on the visual modality, which is reduced. This adaptive adjustment mechanism enables the fusion system to maintain stable positioning accuracy in various complex scenarios.

[0071] Step S4: Model-predicted trajectory control.

[0072] This step uses the high-precision pose estimate output by the adaptive confidence fusion module as state feedback, and solves the optimal control sequence through a nonlinear model predictive controller to achieve accurate tracking control of the desired trajectory.

[0073] In one embodiment of the present invention, the dynamic model of the UAV adopts a standard quadcopter model. Let the position of the UAV in the inertial coordinate system be... The speed is The attitude Euler angles are (representing roll angle, pitch angle, and yaw angle), angular velocity is The equation of motion for the drone is:

[0074] ,

[0075] ,

[0076] ,

[0077] in: This is the position vector of the UAV in the inertial coordinate system, in meters (m). This is the velocity vector of the UAV in the inertial coordinate system, in m / s; Let be the Euler angle vector of the UAV's attitude, in rad. The roll angle has a range of values. , The pitch angle has a range of values. , The yaw angle has a range of values. ; This is the angular velocity vector of the UAV in the body coordinate system, in rad / s; The mass of the drone is expressed in kg, and is set to 1.5 kg in this embodiment. The rotation matrix from the body coordinate system to the inertial coordinate system is given by Euler angles. Sure; The total thrust generated by the four rotors, expressed in Newton-meters (N), with a range of values ​​of [value missing]. Right now N; The vector of gravitational acceleration. m / s²; Let be the rotational inertia matrix of the UAV about its center of mass. It is a 3×3 diagonal matrix with diagonal elements as follows: kg·m² kg·m² kg·m²; This is a three-axis torque vector, with units of N·m.

[0078] In one embodiment of the present invention, the nonlinear model predictive controller employs a rolling time-domain optimization strategy. In this invention, the prediction time domain ranges from 10 to 30 sampling periods, the control time domain ranges from 5 to 15 sampling periods, and the sampling period ranges from 10 ms to 50 ms. A longer prediction time domain allows the controller to anticipate future state changes further in the future, but the computational load increases accordingly; a longer control time domain grants higher degrees of freedom for the controller, but the complexity of the optimization problem also increases; a shorter sampling period results in higher control accuracy, but also imposes stricter real-time requirements on the computing platform. In practical applications, the above parameters need to be comprehensively selected based on the dynamic characteristics of the UAV, computing resources, and accuracy requirements. In each control cycle, the controller solves for the optimal control sequence within the prediction time domain based on the current state (derived from high-precision pose estimation) and the desired trajectory. The control variables include the desired roll angle, desired pitch angle, and desired total thrust; the yaw angle is independently provided by the heading planning module.

[0079] The cost function of the nonlinear model predictive controller is defined as follows:

[0080] ,

[0081] Where J is the total cost function value, which is dimensionless, and the optimization objective is to minimize J. For the prediction time domain, a sampling period of 20 is set, corresponding to a prediction window of 0.4s (sampling period of 20ms). To control the time domain, a sampling period of 10 is set, corresponding to a control window of 0.2s. The above parameter values ​​are respectively within the preferred range of 10 to 30 sampling periods in the prediction time domain, 5 to 15 sampling periods in the control time domain, and 10ms to 50ms in the sampling period. This set of parameters has been verified by a large number of flight experiments on a typical quadcopter UAV platform, and can achieve a good balance between positioning accuracy and computational efficiency. For the predicted first Step position, For the desired position; For the predicted first Step speed, For the desired speed; For the predicted first gait, The desired posture; The position tracking error weight matrix is ​​a 3×3 diagonal matrix with diagonal elements set to [10, 10, 20]. The weight in the z-direction is larger to suppress height fluctuations. The speed tracking error weight matrix is ​​a 3×3 diagonal matrix with diagonal elements set to [5, 5, 5]. The attitude tracking error weight matrix is ​​a 3×3 diagonal matrix with diagonal elements set to [3, 3, 1]. The yaw angle weight is relatively small to allow for a certain heading adjustment margin. R is the control increment; R is the control change rate penalty matrix, a 3×3 diagonal matrix with diagonal elements set to [0.1, 0.1, 0.01], used to suppress drastic changes in control commands and improve flight stability.

[0082] In one embodiment of the present invention, the nonlinear model predictive controller is solved using a sequential quadratic programming method. At each optimization iteration step, the nonlinear dynamic model is first linearized at the current state point, resulting in the linearized state equation:

[0083] ,

[0084] in: For the first The state vector of a step contains nine components: position, velocity, and attitude. The state transition matrix is ​​a 9×9 matrix, obtained by discretizing the dynamic equation at the current point using the Jacobian matrix. The control matrix is ​​a 9×3 matrix. Let be a constant offset vector, a 9×1 vector. Then, the quadratic programming problem is transformed into a standard form for solution, iteratively updating the control sequence until convergence. Preferably, the maximum number of iterations is set to 10, and the convergence tolerance is set to . .

[0085] In one embodiment of the present invention, the nonlinear model predictive controller further includes processing of control constraints and state constraints. The control constraints include desired attitude angle constraints. , and expected thrust constraint State constraints include velocity constraints. ( These constraints are addressed in the optimization solution using interior point methods or effective set methods to ensure that the output control commands meet physical limitations and safety requirements.

[0086] In one embodiment of the present invention, the desired attitude angle output by the nonlinear model predictive controller is... and expected total thrust The information is fed into the attitude inner loop of the UAV flight control system. The attitude inner loop uses a PID controller to convert the deviation between the desired attitude angle and the actual attitude angle obtained from high-precision attitude estimation into speed commands for the four motors, ultimately achieving precise tracking control of the desired trajectory.

[0087] Preferably, the parameters of the attitude inner loop PID controller are set as follows: the proportional coefficient of the roll / pitch channel. Integral coefficient Differential coefficients ; yaw channel ratio coefficient Integral coefficient Differential coefficients These parameters are tuned based on the UAV's dynamic characteristics and attitude response bandwidth requirements.

[0088] like Figure 2 As shown, this invention also provides a visual-inertial fusion UAV trajectory tracking control system, which includes a multimodal data acquisition module 1, a dual-modal feature extraction module 2, an adaptive confidence fusion module 3, and a model prediction control module 4. These four modules correspond one-to-one with the four steps in the method embodiment, forming a complete trajectory tracking control system.

[0089] In one embodiment of the present invention, the multimodal data acquisition module 1 is configured to acquire multimodal perception data during the flight of the UAV, perform timestamp alignment processing, and output a synchronized perception data stream. The multimodal data acquisition module includes a sensor interface submodule 11 and a time synchronization submodule 12. The sensor interface submodule 11 is responsible for communicating with the forward-facing binocular camera and the inertial measurement unit (IMU) to acquire raw sensor data. Preferably, the sensor interface submodule 11 is implemented using the ROS2 framework, the binocular camera is connected via a USB 3.0 interface, and the IMU is connected via an SPI interface. The time synchronization submodule 12 is responsible for performing timestamp alignment processing on data from different sensors, using the linear interpolation alignment method described in step S1 of the method embodiment to ensure that the time deviation between the visual frame and the inertial measurement does not exceed 1 ms.

[0090] In one embodiment of the present invention, the dual-modal feature extraction module 2 is configured to extract visual modal features and inertial navigation modal features from the synchronous sensing data stream, respectively, and output relative pose change and short-time motion estimate. The dual-modal feature extraction module 2 includes two sub-networks: a visual odometry network 21 and an inertial integration network 22. The structure and parameters of the visual odometry network 21 are described in detail in step S2 of the method embodiment, employing an encoder-decoder structure. The encoder is based on a ResNet-18 backbone network, and the decoder outputs a six-DOF relative pose change. The inertial integration network employs an IMU pre-integration method, as described in detail in step S2 of the method embodiment, and outputs a short-time motion estimate including relative rotation, relative translation, relative velocity, and their covariance. Preferably, the visual odometry network and the inertial integration network run on the NVIDIA Jetson AGX Orin platform, with an inference frame rate of up to 50Hz.

[0091] In one embodiment of the present invention, the adaptive confidence fusion module 3 is configured to dynamically adjust the visual modal weights based on the richness of the environmental texture and dynamically adjust the inertial navigation modal weights based on the intensity of the maneuver. It achieves adaptive weighted fusion of relative pose change and short-term motion estimation through a learnable attention mechanism, outputting a high-precision pose estimate. The adaptive confidence fusion module 3 includes a texture evaluation submodule 31, a maneuver evaluation submodule 32, and an attention fusion submodule 33. The texture evaluation submodule 31 implements the environmental texture richness calculation method described in step S3 of the embodiment, and outputs the visual modal confidence score. The method for calculating the severity of maneuvering, as described in step S3 of the implementation method embodiment for the maneuvering evaluation submodule 32, outputs the confidence level of the inertial navigation mode. The attention fusion submodule 33 implements the learnable attention mechanism described in step S3 of the implementation method embodiment, which converts the two confidence indices into fusion weights and performs weighted fusion of the pose estimates of the visual and inertial modes.

[0092] In one embodiment of the present invention, the model predictive control module 4 is configured to receive high-precision attitude estimation as state feedback, solve for the optimal control sequence based on the desired trajectory, and output the desired attitude angle and thrust command. The model predictive control module 4 includes a trajectory planning submodule 41, an NMPC solving submodule 42, and an attitude inner loop submodule 43. The trajectory planning submodule 41 is responsible for generating the desired trajectory according to mission requirements, supporting multiple modes such as waypoint tracking, circular trajectory, and figure-eight trajectory. The NMPC solving submodule 42 implements the nonlinear model predictive control algorithm described in step S4 of the method embodiment, using a sequential quadratic programming method to solve for the optimal control sequence online. Preferably, the control cycle of the NMPC solving submodule is 20ms, which can meet the real-time control requirements. The attitude inner loop submodule 43 uses a PID controller to convert the desired attitude angle into a motor speed command, which is then output to the flight control system for execution.

[0093] In one embodiment of the present invention, the system's hardware and software architecture is as follows: The hardware platform uses NVIDIA Jetson AGX Orin as the onboard computing unit, equipped with 32GB of memory and 275 TOPSAI computing power, which can meet the real-time computing requirements of the visual odometry network and the adaptive confidence fusion module. The sensor configuration includes an Intel RealSense D455 binocular camera (30Hz frame rate, 640×480 resolution) and a BMI088 inertial measurement unit (200Hz sampling rate). The flight control system uses the PX4Autopilot open-source flight controller, which communicates with the onboard computing unit through MAVROS. The software framework is built on ROS2Humble, with each module running as a node and data transmission through a topic mechanism.

[0094] In one embodiment of the present invention, system verification experiments were conducted on the EuRoCMAV dataset and a self-built indoor and outdoor flight dataset. The EuRoCMAV dataset contains various flight scenarios, where V1_01_easy to V2_03_difficult are indoor flight sequences, and MH_01_easy to MH_05_difficult are factory environment flight sequences. The self-built dataset contains three challenging scenarios: a weakly textured corridor, an outdoor environment with strong lighting changes, and a high-intensity maneuvering scenario.

[0095] Experimental results show that, compared with visual odometry alone, the root mean square error (RMSE) of position tracking in this invention is reduced by approximately 25% in normal scenarios and by approximately 55% in weakly textured scenarios. Compared with inertial integration alone, the method of this invention significantly reduces cumulative drift during long-distance flight, with the position error decreasing from 2.1m to 0.3m after 30s of flight. Compared with fixed-weight fusion methods, the adaptive confidence fusion strategy of this invention can automatically adjust weights according to environmental conditions, improving position tracking accuracy by approximately 30% in mixed scenarios. In terms of trajectory tracking control, the position tracking error of this invention is reduced by 40% compared with single-modal methods, and it can still maintain stable control in visual degradation scenarios, with a maximum position deviation not exceeding 0.3m.

[0096] The embodiments of the present invention are not limited to the specific embodiments described above. Those skilled in the art can make various equivalent changes or substitutions based on the technical solutions of the present invention, and all such changes or substitutions should be included within the protection scope of the present invention.

Claims

1. A visual-inertial navigation fusion-based UAV trajectory tracking and control method, characterized in that, Includes the following steps: S1. Multimodal data synchronous acquisition: Acquire multimodal perception data during the flight of the UAV. The multimodal perception data includes environmental image sequences acquired by the forward-facing binocular camera and acceleration and angular velocity data acquired by the inertial measurement unit. Align the environmental image sequences and acceleration and angular velocity data according to timestamps to obtain a synchronous perception data stream for fusion positioning. S2. Dual-modal feature extraction: For the environmental image sequence in the synchronous sensing data stream, a visual odometry network is constructed to extract the relative pose change between consecutive frames; for the acceleration and angular velocity data in the synchronous sensing data stream, an inertial integral network is constructed to extract the short-time motion estimate between adjacent moments. S3. Adaptive Confidence Fusion: An adaptive confidence fusion module is constructed. This module calculates the richness of environmental texture based on the environmental image sequence and dynamically adjusts the visual modality weights accordingly. It calculates the intensity of motion based on acceleration and angular velocity data and dynamically adjusts the inertial navigation modality weights accordingly. After normalizing the visual modality weights and inertial navigation modality weights through a learnable attention mechanism, it achieves adaptive weighted fusion of relative pose change and short-term motion estimate, and outputs a high-precision pose estimate. S4. Model Predictive Trajectory Control: The high-precision attitude estimation is used as the state feedback input to the nonlinear model predictive controller. Based on the deviation between the desired trajectory and the high-precision attitude estimation, the nonlinear model predictive controller solves the optimal control sequence in the prediction time domain and outputs the desired attitude angle and thrust command to the UAV flight control system to achieve precise tracking control of the desired trajectory.

2. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S1, the timestamp alignment process includes: using the sampling time of the inertial measurement unit as a reference, performing linear interpolation alignment on the exposure time of the environmental image sequence so that the time deviation between the visual frame and the inertial navigation measurement does not exceed 1ms.

3. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S2, the visual odometry network adopts an encoder-decoder structure. The encoder part uses a residual convolutional network to extract multi-scale visual features, and the decoder part uses a fully connected layer to regress and output the six-degree-of-freedom relative pose change.

4. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S2, the inertial integrator network uses the IMU pre-integration method to integrate the acceleration and angular velocity data between adjacent keyframes, and outputs a short-time motion estimate that includes relative rotation, relative translation and relative velocity.

5. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S3, the method for calculating the richness of environmental texture is as follows: extract corner features from the environmental image, count the number and distribution uniformity of the corner features, and when the number of corners is greater than a set threshold and the distribution uniformity is higher than a set ratio, the richness of environmental texture is determined to be high, and the visual modality weight is increased accordingly; when the number of corners is less than a set threshold or the distribution uniformity is lower than a set ratio, the richness of environmental texture is determined to be low, and the visual modality weight is decreased accordingly.

6. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S3, the method for calculating the severity of the maneuver is as follows: calculate the standard deviation of the acceleration data and angular velocity data within the sliding time window. When the standard deviation is within the set stable range, it is determined that the severity of the maneuver is low and the cumulative error of the inertial integral is small, and the inertial navigation mode weight is increased accordingly. When the standard deviation exceeds the stable range, it is determined that the severity of the maneuver is high and the cumulative error of the inertial integral is large, and the inertial navigation mode weight is decreased accordingly.

7. The visual-inertial fusion UAV trajectory tracking and control method according to claim 1, characterized in that, In step S3, the learnable attention mechanism includes: mapping the visual modal weights and inertial mode weights to query vectors and key vectors respectively; calculating the correlation score between the two modalities through dot product attention; performing Softmax normalization on the correlation score to obtain the final fusion weights; and using the final fusion weights to perform a weighted summation of the relative pose change and the short-term motion estimate to obtain a high-precision pose estimate.

8. The visual-inertial fusion-based UAV trajectory tracking and control method according to claim 1, characterized in that, In step S4, the prediction time domain of the nonlinear model prediction controller is 10 to 30 sampling periods, the control time domain is 5 to 15 sampling periods, and the sampling period is 10ms to 50ms.

9. The visual-inertial fusion-based UAV trajectory tracking and control method according to claim 1, characterized in that, In step S4, the cost function of the nonlinear model predictive controller includes a position tracking error term, an attitude tracking error term, and a control variable change rate penalty term, wherein the weight of the position tracking error term is greater than the weight of the attitude tracking error term, and the control variable change rate penalty term is used to suppress drastic changes in control commands.

10. A visual-inertial navigation fusion-based UAV trajectory tracking control system, used to implement the visual-inertial navigation fusion-based UAV trajectory tracking control method according to any one of claims 1-9, characterized in that, include: The multimodal data acquisition module is configured to acquire multimodal perception data during the flight of the UAV and perform timestamp alignment processing, and output synchronous perception data stream. The multimodal perception data includes environmental image sequences acquired by the forward-facing binocular camera and acceleration and angular velocity data acquired by the inertial measurement unit. The dual-modal feature extraction module is configured to extract visual modal features and inertial navigation modal features from the synchronous sensing data stream, respectively. The dual-modal feature extraction module includes a visual odometry network and an inertial integral network. The visual odometry network is used to extract the relative pose change between consecutive frames, and the inertial integral network is used to extract the short-time motion estimate between adjacent time points. The adaptive confidence fusion module is configured to dynamically adjust the visual modal weights based on the richness of the environmental texture and the inertial navigation modal weights based on the intensity of the maneuver. It achieves adaptive weighted fusion of relative pose change and short-term motion estimate through a learnable attention mechanism, and outputs a high-precision pose estimate. The model predictive control module is configured to receive high-precision pose estimation as state feedback, solve for the optimal control sequence based on the desired trajectory, and output the desired attitude angle and thrust command. The model predictive control module uses a nonlinear model predictive controller to achieve accurate tracking control of the desired trajectory.

Images