Unmanned aerial vehicle dynamic obstacle avoidance method and system based on interactive multiple model and unscented kalman filter

Abstract

The application discloses a kind of unmanned aerial vehicle dynamic obstacle avoidance method and system based on interactive multiple model and unscented Kalman filter, the method includes: collecting dynamic obstacle original position observation data and combining flight state and history cache to generate input data set;Multiple nonlinear motion models are filtered to obtain filtering results, the likelihood is calculated and the model probability is dynamically updated, the posterior state of each motion model at the current sampling time is weighted and mixed based on weight probability, to obtain the fusion estimation state containing position, velocity and acceleration, the safety control instruction is obtained by quadratic programming processing to the upper layer reference control instruction to drive unmanned aerial vehicle;Since the application eliminates nonlinear large maneuvering motion estimation delay and truncation error by multiple model interactive filtering, high confidence multi-dimensional boundary input is provided by dynamic weight mixing, and instruction online audit and filtering are realized by quadratic programming, which greatly improves the obstacle avoidance safety performance in complex environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and safety control of unmanned aerial vehicles (UAVs), and in particular to a dynamic obstacle avoidance method and system for UAVs based on interactive multi-model and unscented Kalman filtering. Background Technology

[0002] In the field of autonomous navigation and safety control of unmanned aerial vehicles (UAVs), ensuring absolute safety and accurate trajectory tracking in complex environments filled with dynamic obstacles is a current research hotspot. With the increasing density of UAV operating environments, ensuring dynamic safety and obstacle avoidance during autonomous flight has become a crucial practical need.

[0003] Existing UAV dynamic obstacle avoidance methods typically use a single motion model to estimate the motion state of obstacles, such as Kalman filtering based on a uniform velocity model or a uniform acceleration model, and then use the estimated obstacle position and velocity for safety control.

[0004] Existing filtering estimation methods based on a single motion model are difficult to adapt to the nonlinear large-scale maneuvering motion of dynamic obstacles. When a dynamic obstacle suddenly turns or changes lanes, the single-model filter requires multiple sampling periods to adapt to the change in motion state, resulting in lag in obstacle state estimation. At the same time, some nonlinear filtering methods based on linearization approximation are prone to truncation errors, making it difficult to provide the safety controller with high-confidence position, velocity, and acceleration estimation results, which in turn affects the accuracy and safety of generating secondary planning obstacle avoidance control commands. Summary of the Invention

[0005] The main objective of this invention is to provide a dynamic obstacle avoidance method and system for unmanned aerial vehicles (UAVs) based on interactive multi-model and unscented Kalman filtering, aiming to solve the technical problem that existing technologies cannot effectively cope with irregular nonlinear large-scale maneuvering motions, resulting in state estimation delays and subsequent lag in the underlying obstacle avoidance response.

[0006] To achieve the above objectives, this invention provides a dynamic obstacle avoidance method for unmanned aerial vehicles (UAVs) based on interactive multi-model and unscented Kalman filtering, the method comprising the following steps: Collect raw spatial position observation data of dynamic obstacles during the flight of the target UAV, and generate an input dataset based on the raw spatial position observation data, the flight status data of the target UAV, and the model estimation cache data of the previous sampling time. Based on the input dataset, a multi-model interactive filtering process is performed on a preset nonlinear motion model set to obtain the model filtering results of each motion model at the current sampling time. The preset nonlinear motion model set includes a uniform motion model, a left-turn cooperative turning motion model, and a right-turn cooperative turning motion model. The model filtering results include posterior state, posterior covariance, innovation residual, and innovation covariance. The model likelihood of each motion model is calculated based on the model filtering results, and the model probability of each motion model is dynamically updated based on the model likelihood to obtain the weight probability of each motion model at the current sampling time. Based on the weighted probabilities, the posterior states of each motion model at the current sampling time are dynamically weighted and fused to obtain the dynamic obstacle fusion estimation state, which includes the position component, velocity component and acceleration component of the dynamic obstacle. Based on the dynamic obstacle fusion estimation state, the upper-layer reference control command of the target UAV is subjected to secondary planning processing to obtain a safety control command, and the safety control command is sent to the control terminal of the target UAV so that the control terminal drives the target UAV based on the safety control command.

[0007] Furthermore, to achieve the above objectives, this invention also proposes a UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering, which applies the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described above. The system includes a perception input module, an interactive prediction module, a dynamic safety filtering module, and an actuator output module. The interactive prediction module includes an interactive filtering unit, a model probability update unit, and a combined output unit. The perception input module is used to collect the original spatial position observation data of dynamic obstacles during the flight of the target UAV, and generate an input dataset based on the original spatial position observation data, the flight status data of the target UAV, and the model estimation cache data of the previous sampling time. The interactive filtering unit is used to perform multi-model interactive filtering on a preset nonlinear motion model set based on the input dataset to obtain the model filtering results of each motion model at the current sampling time. The preset nonlinear motion model set includes a uniform motion model, a left-turn cooperative turning motion model, and a right-turn cooperative turning motion model. The model filtering results include posterior state, posterior covariance, innovation residual, and innovation covariance. The model probability update unit is used to calculate the model likelihood of each motion model based on the model filtering result, and dynamically update the model probability of each motion model based on the model likelihood to obtain the weight probability of each motion model at the current sampling time. The combined output unit is used to dynamically weight and fuse the posterior states of each motion model at the current sampling time based on the weighted probabilities to obtain a dynamic obstacle fusion estimation state, which includes the position component, velocity component and acceleration component of the dynamic obstacle. The dynamic safety filtering module is used to perform secondary planning processing on the upper-layer reference control command of the target UAV based on the dynamic obstacle fusion estimation state to obtain safety control command, and send the safety control command to the actuator output module; The actuator output module is used to drive the target drone based on the safety control command.

[0008] Furthermore, to achieve the above objectives, this application also proposes a dynamic obstacle avoidance device for unmanned aerial vehicles (UAVs). The device includes: a memory, a processor, and a UAV dynamic obstacle avoidance program stored in the memory. The processor is used to run the UAV dynamic obstacle avoidance program, and the computer program is configured to implement the steps of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described above.

[0009] In addition, to achieve the above objectives, this application also proposes a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described above.

[0010] This invention employs multi-model interactive filtering based on uniform motion models, left-turn coordinated turning motion models, and right-turn coordinated turning motion models. This allows different motion hypotheses to participate in state estimation in parallel around the same dynamic obstacle, avoiding the rigidity of estimation modes in a single motion model when the obstacle suddenly turns or changes lanes. The model likelihood is calculated using the innovation residuals and innovation covariance in the model filtering results, and the model probabilities are dynamically updated, enabling the motion model weights to adaptively switch according to the degree of observation matching. The posterior states of each motion model are dynamically weighted and fused using weight probabilities, allowing the position, velocity, and acceleration components of the dynamic obstacle to form a continuous and smooth fusion expression among the multi-model estimation results. Through dynamic obstacle fusion estimation state-driven quadratic programming, the upper-level reference control commands can be corrected into safe control commands under the constraint of obstacle maneuvering trends. This improves the synergy between state estimation and control command correction when the dynamic obstacle undergoes nonlinear large maneuvers, reducing the impact of estimation hysteresis and model mismatch on obstacle avoidance decisions. Attached Figure Description

[0011] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0012] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0013] Figure 1 This is a flowchart illustrating the first embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application. Figure 2 This is a flowchart illustrating the second embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application. Figure 3 This is a schematic diagram of the module structure of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering in an embodiment of this application; Figure 4 This is a schematic diagram of the module operation architecture in the UAV dynamic obstacle avoidance system example based on interactive multi-model and unscented Kalman filtering in this application; Figure 5 This is a schematic diagram of the obstacle avoidance interaction process in the UAV dynamic obstacle avoidance system example based on interactive multi-model and unscented Kalman filtering in this application; Figure 6 This is a schematic diagram of the device structure of the hardware operating environment involved in the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in the embodiments of this application.

[0014] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0015] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.

[0016] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.

[0017] It should be noted that the executing entity of this application embodiment can be a computing service device with data processing, network communication, and program execution functions, such as a computer or server, or an electronic device capable of performing the above functions. The following uses a drone dynamic obstacle avoidance device (hereinafter referred to as obstacle avoidance device) as an example to describe this embodiment and the following embodiments.

[0018] Based on this, the first embodiment of this application provides a dynamic obstacle avoidance method for unmanned aerial vehicles (UAVs) based on interactive multi-model and unscented Kalman filtering, referring to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application.

[0019] In this embodiment, the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering includes: Step S10: Collect the original spatial position observation data of dynamic obstacles during the flight of the target UAV, and generate an input dataset based on the original spatial position observation data, the flight status data of the target UAV, and the model estimation cache data of the previous sampling time.

[0020] It should be noted that the raw spatial position observation data refers to the dynamic obstacle position data, which contains measurement noise, obtained by the airborne sensing equipment at discrete sampling times. It can be two-dimensional planar coordinates or three-dimensional spatial coordinates. Dynamic obstacles can be other drones, aircraft, vehicles, pedestrians, or moving objects that can enter the flight path of the target drone.

[0021] Flight state data refers to data used to characterize the current motion state of the target UAV, which may include current position, flight speed, flight acceleration, attitude angle, angular velocity, and heading angle. Model estimation cache data refers to the state estimation information generated by each motion model and stored in the storage unit at the previous sampling time.

[0022] Step S20: Perform multi-model interactive filtering on the preset nonlinear motion model set based on the input dataset to obtain the model filtering results of each motion model at the current sampling time.

[0023] It should be noted that the pre-set nonlinear motion model set refers to a collection of state evolution models pre-established for different motion patterns that dynamic obstacles may exhibit. The pre-set nonlinear motion model set includes uniform motion models, left-turn coordinated turning motion models, and right-turn coordinated turning motion models.

[0024] Among them, the uniform motion model is used to describe the motion state of an obstacle that maintains a basically stable speed for a short period of time; the left-turn coordinated turning motion model is used to describe the motion state of an obstacle that coordinates its turn along the left-turn direction; and the right-turn coordinated turning motion model is used to describe the motion state of an obstacle that coordinates its turn along the right-turn direction.

[0025] It should be noted that the model filtering results include the posterior state, posterior covariance, innovation residual, and innovation covariance. The posterior state refers to the state estimate obtained after correction based on the current observation data; the posterior covariance is the uncertainty matrix of the posterior state estimation error; the innovation residual is the difference between the actual observed value and the predicted observed value; and the innovation covariance is the uncertainty matrix of the innovation residual.

[0026] It should be understood that using multiple motion models to describe straight, left-turn, and right-turn behaviors in parallel can reduce prediction bias caused by the mismatch between a single motion model and the actual motion pattern; by propagating nonlinear state distributions through unscented sampling, the linearization truncation error caused by relying on first-order Taylor expansion can be avoided, thereby improving the stability of position, velocity, and acceleration estimations during turning and lane changing.

[0027] In some embodiments, within each sampling period, the obstacle avoidance device forms the initial state of each filtering branch based on the state estimates and model probabilities of each model in the previous period. Then, it uses unscented Kalman filtering to perform nonlinear time prediction and observation correction, respectively, to obtain the posterior state, posterior covariance, innovation residual, and innovation covariance for the three models: constant speed, left turn, and right turn. For example, when the obstacle changes from straight flight to a left turn, the fitting error of the left turn model to the observed trajectory is usually smaller than that of the other two models, thus providing a basis for subsequent model probability adjustment.

[0028] Step S30: Calculate the model likelihood of each motion model based on the model filtering results, and dynamically update the model probability of each motion model based on the model likelihood to obtain the weight probability of each motion model at the current sampling time.

[0029] It should be noted that model likelihood refers to the relative probability of the current innovation residual occurring under the condition that the corresponding motion model holds true. Weight probability refers to the normalized probability that each motion model matches the actual motion pattern of the dynamic obstacle at the current sampling time, and the sum of the weight probabilities is 1.

[0030] In some embodiments, the obstacle avoidance device reads the innovation residuals and innovation covariance of each filtering branch, and calculates the model likelihood using a Gaussian probability density function or a residual matching function equivalent to Gaussian probability density. Then, combining the model probability at the previous sampling time and the transition relationships between models, the prior model probability at the current sampling time is obtained. The model likelihood is multiplied by the prior model probability and normalized to form the weighted probabilities of each motion model at the current sampling time. To avoid numerical underflow, log-likelihood calculation, likelihood lower bound truncation, or covariance regularization can be used. For example, when a dynamic obstacle suddenly turns right, the innovation residual corresponding to the right-turn model decreases, and its model likelihood and weighted probability increase with the recursive process, while the weight of the uniform velocity model decreases accordingly.

[0031] Step S40: Based on the weighted probabilities, dynamically weight and fuse the posterior states of each motion model at the current sampling time to obtain the dynamic obstacle fusion estimation state.

[0032] It should be noted that the dynamic obstacle fusion estimation state includes the position component, velocity component, and acceleration component of the dynamic obstacle.

[0033] It should be understood that fusing the state results of each motion model according to real-time weights can retain estimation information that matches the current motion pattern to a high degree, while suppressing the impact of short-term misjudgment or filtering fluctuations of a single model on the final state output, thus providing continuous obstacle motion parameters for safety boundary calculation.

[0034] In its implementation, the obstacle avoidance device transforms the posterior states of each motion model to the same coordinate system and extracts position, velocity, and acceleration according to the arrangement of state components. The fused state can be represented as: in, For the number of motion models, For the first The weighted probabilities of each motion model at the current sampling time. For the first The posterior state of each motion model. The obstacle avoidance device extracts the corresponding position, velocity, and acceleration components from the fused state, and adds obstacle identifiers, timestamps, and coordinate system identifiers to form a dynamic obstacle fusion estimation state.

[0035] Step S50: Based on the dynamic obstacle fusion estimation state, perform secondary planning processing on the upper-layer reference control command of the target UAV to obtain a safety control command, and send the safety control command to the control terminal of the target UAV so that the control terminal drives the target UAV based on the safety control command.

[0036] It should be noted that the upper-level reference control command refers to the desired control quantity generated by the track tracker, path planner, mission controller, or manual remote control module. This can be the desired acceleration, desired velocity, desired attitude, desired angular velocity, or equivalent motor control quantity. Secondary programming processing refers to solving for the control variables while preserving the reference control intent as much as possible, using safety constraints and the physical output range of the UAV as constraints. Safety control commands are the control quantities that can be sent to the flight controller after being checked or corrected for safety constraints.

[0037] It should be understood that this embodiment dynamically corrects the safety boundary by estimating the state through obstacle fusion and optimizes the constraints of the upper-level reference control command. It can generate control quantities that meet safety conditions when there is a collision risk and reduce intervention in the original trajectory tracking command when there is no collision risk, thereby taking into account dynamic obstacle avoidance safety, control continuity and trajectory tracking performance.

[0038] In some embodiments, the obstacle avoidance device calculates the relative motion relationship based on the target UAV's flight state and the dynamic obstacle fusion estimation state, and establishes a control barrier function to characterize the safe zone. Using the deviation between the safe control command and the upper-level reference control command as the optimization objective, the device solves for the safe control command online by considering the control barrier function constraints, maximum acceleration constraints, maximum attitude angle constraints, or motor output saturation constraints as the feasible region. When the reference control command satisfies the safety constraints, it can be directly output; when the reference control command may cause the UAV to enter the obstacle safety exclusion zone, the solver outputs a corrected control quantity that satisfies the safety constraints and has a small change relative to the reference control command. After receiving the safe control command, the control terminal drives the UAV to change its flight speed or heading through the attitude control loop, angular velocity control loop, and motor control loop, and re-executes the perception, estimation, and planning process in the next sampling cycle.

[0039] This embodiment employs multi-model interactive filtering based on uniform motion, left-turn coordinated turning, and right-turn coordinated turning motion models. This allows different motion hypotheses to participate in state estimation in parallel around the same dynamic obstacle, avoiding the rigidity of estimation modes in a single motion model when the obstacle suddenly turns or changes lanes. The model likelihood is calculated using the innovation residuals and innovation covariance in the model filtering results, and the model probabilities are dynamically updated, enabling the motion model weights to adaptively switch according to the degree of observation matching. The posterior states of each motion model are dynamically weighted and fused using weight probabilities, allowing the position, velocity, and acceleration components of the dynamic obstacle to form a continuous and smooth fusion expression among the multi-model estimation results. Through dynamic obstacle fusion estimation state-driven quadratic programming, the upper-level reference control commands can be corrected into safe control commands under the constraint of obstacle maneuvering trends. This improves the synergy between state estimation and control command correction when the dynamic obstacle undergoes nonlinear large maneuvers, reducing the impact of estimation hysteresis and model mismatch on obstacle avoidance decisions.

[0040] Reference Figure 2 , Figure 2 This is the second embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application.

[0041] Based on the first embodiment described above, in this embodiment, the model estimation buffer data at the previous sampling time includes the posterior state, posterior covariance, and model probability of each motion model; step S20 may include: Step S201: Calculate the interaction and mixing probability between each motion model based on the model probability and the preset model transition probability matrix.

[0042] It should be noted that the model transition probability matrix is ​​a matrix used to describe the probability of switching between different motion models at adjacent sampling times, where the matrix elements of the model transition probability matrix are... This represents the probability that the motion pattern changes from the i-th model to the j-th model.

[0043] It should be understood that by calculating the interactive hybrid probability through the model transition probability and the model probability at the previous sampling time, state information can be transferred between different motion models, and the time required for a single filter branch to reconverge during motion mode switching can be reduced.

[0044] In practice, the obstacle avoidance device adjusts according to the previous moment. Posterior state estimates for each model Covariance and model probability Combined with the preset model transition probability matrix Calculate the interaction mixture probability : in, This is the normalization constant. Then, the initial state after mixing is calculated. and initial covariance As input to each parallel filter.

[0045] Step S202: Based on the interaction mixing probability, the posterior state and the posterior covariance are mixed to obtain the mixed initial state and mixed initial covariance corresponding to each motion model.

[0046] It should be noted that the mixed initial state refers to the initial state provided to the current model's filtering branch after combining the posterior states of each model at the previous sampling time according to the interactive mixture probability. The mixed initial covariance refers to the initial error covariance formed by simultaneously considering the internal estimation errors of each model and the differences in the mean of the model states.

[0047] It should be understood that state mean mixing can introduce state information from other motion modes into the current model, while covariance mixing can retain the uncertainty caused by state differences between models, thereby reducing the estimation abruptness caused by motion mode switching.

[0048] Step S203: Generate unmarked sampling points for each motion model based on the mixed initial state and the mixed initial covariance.

[0049] It should be noted that unscented sampling points refer to a set of deterministic state samples determined around the mixed initial state according to the mixed initial covariance, also known as Sigma points, which are used to approximate the probability distribution of the state variables of a nonlinear system.

[0050] In the specific implementation, for each nonlinear model in the model set... Using the mixed initial state, perform an unscented transform (UT) to generate Sigma points ( (where is the state dimension).

[0051] Furthermore, in order to reduce linearization error, step S203 above may include: Step S2031: Determine the sampling center state corresponding to each motion model based on the mixed initial state.

[0052] It should be noted that the sampling center state refers to the center of the distribution of unscented sampling points, corresponding to the mixed initial state of the current motion model.

[0053] It should be understood that using the mixed initial state as the sampling center enables the unscented sampling points to be distributed around the mean of the current model's state, keeping the sampling results consistent with the model's interactive output.

[0054] Step S2032: Generate the covariance decomposition matrix corresponding to each motion model based on the hybrid initial covariance and the preset sampling scale parameter.

[0055] It should be noted that the preset sampling scale parameter refers to the parameter used to adjust the dispersion of unscented sampling points relative to the sampling center, and can be determined jointly by the state dimension and the unscented transformation parameters α, β, and κ. For example, α is used to adjust the distribution range of sampling points, β is used to introduce prior information about the state distribution, and κ is used to assist in adjusting the scaling ratio. For example, setting a proportionally symmetric sampling parameter... , , Generate for each model Sigma points.

[0056] Step S2033: Generate a set of positive offset sampling points corresponding to each motion model based on the sampling center state and the covariance decomposition matrix.

[0057] It should be understood that positive offset sampling points can characterize the state distribution characteristics when the state mean changes positively along the principal directions of each covariance.

[0058] Step S2034: Generate the back offset sampling point set corresponding to each motion model based on the sampling center state and the covariance decomposition matrix.

[0059] It should be understood that the reverse offset sampling points and the forward offset sampling points form a symmetrical distribution about the sampling center, which can simultaneously characterize the changes of the state variables on both sides of the principal direction of the covariance.

[0060] Step S2035: Combine the sampling center state, the positive offset sampling point set, and the negative offset sampling point set to obtain the unmarked sampling points corresponding to each motion model.

[0061] It should be understood that combining the central sampling point and the bidirectional offset sampling point can approximate the mean, covariance, and nonlinear propagation characteristics of the mixed initial state with a finite number of deterministic samples.

[0062] Step S204: Update the unscented sampling points in time based on the nonlinear state equations corresponding to each motion model to obtain the predicted state and prediction covariance corresponding to each motion model.

[0063] In the specific implementation, the Sigma points are substituted into the nonlinear state equations of each model (such as the nonlinear kinematic equations of the CT turning model), the predicted Sigma points are calculated and weighted summation is performed to obtain the model. One-step prediction of state and predicted covariance .

[0064] Step S205: Extract spatial location observation sequences from the input dataset.

[0065] It should be noted that a spatial location observation sequence refers to location observations associated with the same dynamic obstacle and arranged according to sampling time, which may include the current observation and several location observations prior to the current sampling time.

[0066] Step S206: Based on the spatial location observation sequence, measure and update the predicted state and the predicted covariance to obtain the model filtering results of each motion model at the current sampling time.

[0067] It should be noted that measurement update refers to the process of correcting the predicted state and predicted covariance using the current spatial location observations, including predicted observation calculation, innovation calculation, unscented Kalman gain calculation, and posterior state update.

[0068] In practical implementation, the obstacle avoidance device calculates the predicted observed values ​​and the actual sensor observed values ​​using observation equations. The residual (new information) and the corresponding new information covariance The independent posterior states of each model are obtained by updating the unscented Kalman gain. and posterior covariance .

[0069] This embodiment establishes prior switching relationships between motion modes through model transition probabilities, retains historical estimation information of different models through state and covariance hybridization, obtains prediction results under each motion assumption through unscented sampling and nonlinear time updates, and implements measurement correction through spatial position observation after target association. This enables multi-model interaction and nonlinear filtering to form a continuous recursive process, taking into account both motion mode switching response capability and nonlinear state estimation accuracy.

[0070] Based on the above embodiments, in the third embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application, step S30 may include: Step S301: Calculate the innovation matching degree corresponding to each motion model based on the innovation residual and the innovation covariance.

[0071] It should be noted that the information matching degree is a statistic used to measure the degree of agreement between current actual observations and the predicted observations of a certain motion model.

[0072] It should be understood that considering both the magnitude of the innovation residual and the innovation covariance simultaneously can distinguish between large residuals caused by model mismatch and residual changes caused by increased observation noise, thereby improving the rationality of model matching judgment.

[0073] Step S302: Generate the model likelihood corresponding to each motion model based on the information matching degree; Step S303: Calculate the unnormalized model probability of each motion model at the current sampling time based on the model likelihood and the model probability of each motion model at the previous sampling time; Step S304: Normalize the unnormalized model probabilities to obtain the weight probabilities of each motion model at the current sampling time.

[0074] In practical implementation, the obstacle avoidance device uses the new information residual... and new information covariance Construct a Gaussian likelihood function to calculate the likelihood of each model. : in, This represents the model likelihood of the j-th motion model at the k-th sampling time, which characterizes the degree to which the current observation data matches the prediction result of the j-th motion model. Indicates the current discrete sampling time; T represents the transpose; This represents the innovation residual vector of the j-th motion model at the k-th sampling time; Let represent the innovation covariance matrix of the j-th motion model at the k-th sampling time, used to characterize the uncertainty of the innovation residual.

[0075] Then update the current time. Weight probabilities of each model : in, When the obstacle begins to turn, the model or The new information residual decreases, and its corresponding likelihood is reduced. and probability It will climb rapidly within 1-2 cycles.

[0076] This embodiment determines the observation-model matching degree through the innovation residual and innovation covariance, forms a comparable model likelihood through probability density mapping, and maintains the temporal continuity of motion patterns by combining historical model probabilities and model transition relationships. Finally, it obtains weight probabilities that can be directly involved in fusion through normalization, and can adaptively switch the model contribution ratio according to the real-time motion changes of dynamic obstacles.

[0077] Based on the above embodiments, in the fourth embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application, step S40 may include: Step S401: Based on the posterior state and the weighted probability, the position state components of each motion model are weighted to obtain the dynamic obstacle fusion position components.

[0078] It should be noted that the position state component refers to the coordinate component in the posterior state of each motion model used to characterize the current position of the dynamic obstacle.

[0079] It should be understood that by fusing position components according to model weights, position estimates under different motion assumptions can be integrated, reducing the impact of position deviation of a single filter branch on obstacle avoidance distance calculation.

[0080] Step S402: Based on the posterior state and the weighted probability, the velocity state components of each motion model are weighted to obtain the dynamic obstacle fusion velocity components.

[0081] It should be noted that the velocity state component refers to the vector component in the posterior state of each motion model used to characterize the speed and direction of motion of the dynamic obstacle.

[0082] It should be understood that fusing velocity components can provide continuous inputs for calculating relative approach speed, potential collision time, and look-ahead safety distance.

[0083] Step S403: Based on the posterior state and the weighted probability, the acceleration state components of each motion model are weighted to obtain the dynamic obstacle fusion acceleration components.

[0084] It should be noted that the acceleration state component refers to the vector component used to characterize the rate of change of velocity of a dynamic obstacle. It can be directly estimated from the motion model or obtained from the internal state of the model through deterministic transformation.

[0085] It should be understood that the fusion of acceleration components can characterize the current maneuvering trend of the obstacle, so that safety control calculations can not only rely on the current position and speed, but also take into account the motion changes caused by sharp turns, lane changes or braking.

[0086] Step S404: Generate a dynamic obstacle fusion estimation state based on the dynamic obstacle fusion position component, the dynamic obstacle fusion velocity component, and the dynamic obstacle fusion acceleration component.

[0087] It should be understood that combining the fused position, fused velocity, and fused acceleration according to a unified data structure can provide obstacle state input with consistent timestamps and coordinate systems for subsequent relative motion calculations and dynamic control barrier functions.

[0088] In its implementation, the obstacle avoidance device performs a weighted summation of the model states from all parallel processing branches and outputs the final fused state estimate (i.e., the dynamic obstacle fusion estimate state). and its acceleration components : in, Indicates the dynamic obstacle fusion estimation state; This represents the fused acceleration estimate vector of the dynamic obstacle at the k-th sampling time; This represents the posterior acceleration estimate vector output by the j-th motion model at the k-th sampling time. Represents the fused location estimation vector; Represents the fusion velocity estimation vector; Let represent the posterior position estimation vector of the j-th motion model; Let represent the posterior velocity estimate vector of the j-th motion model; This represents the position component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the velocity component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the acceleration component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the position component of the dynamic obstacle in the y-direction estimated by the j-th motion model; This represents the velocity component of the dynamic obstacle in the y-direction estimated by the j-th motion model; This represents the acceleration component of the dynamic obstacle in the y-direction estimated by the j-th motion model.

[0089] This embodiment reduces the degree to which different physical state components are affected by model mismatch by fusing position, velocity and acceleration separately, and maintains the consistency of time and coordinates of each component through a unified state structure. This enables the formation of a state estimate that simultaneously reflects the current position, direction of motion and maneuvering trend of the obstacle, providing a complete input for collision risk prediction and dynamic safety constraint construction.

[0090] Based on the above embodiments, in the fifth embodiment of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in this application, step S50 may include: Step S501: Determine the relative position state, relative velocity state, and obstacle acceleration state between the target UAV and the dynamic obstacle based on the dynamic obstacle fusion estimation state and the flight state data.

[0091] It should be noted that the relative position state refers to the difference vector between the position of the dynamic obstacle and the position of the target UAV; the relative velocity state refers to the difference vector between the velocity of the dynamic obstacle and the velocity of the target UAV; and the obstacle acceleration state refers to the acceleration vector in the dynamic obstacle fusion estimation state. When the acceleration of the target UAV itself needs to be considered, the relative acceleration can also be formed by the difference between the obstacle acceleration and the target UAV acceleration.

[0092] It should be understood that this embodiment converts the obstacle state into a relative motion state under the reference of the target UAV, which can directly describe the changes in distance and approach trend between the two parties, and reduce the impact of absolute coordinate changes on collision risk assessment.

[0093] Step S502: Construct a relative safe distance function based on the relative position state, the relative velocity state, the obstacle acceleration state, the preset safe repulsion radius, and the preset look-ahead time constant.

[0094] It should be noted that the preset safety repulsion radius refers to the minimum basic distance that the target UAV and the dynamic obstacle need to maintain, and it can be set according to the UAV's dimensions, the obstacle's dimensions, the positioning error, and the control response margin. The preset look-ahead time constant is a time parameter that maps the relative speed or obstacle maneuvering trend to the look-ahead safety margin, and it can be set according to the UAV's maximum braking capacity, control cycle, and flight speed. For example, the safety repulsion radius can be a value greater than the sum of the outer envelope radii of both sides, and the look-ahead time constant can be a time length covering several control cycles.

[0095] It should be understood that introducing relative velocity and obstacle acceleration information into the geometric safety distance allows the safety boundary to change dynamically with the approach direction and maneuvering trend of the obstacle, reducing the problem of fixed circular or spherical boundaries being too conservative or insufficient in response.

[0096] in, This represents the relative safety distance function value, used to characterize whether the relative motion state between the target UAV and dynamic obstacles meets the safety distance requirements; when the safety set is a positive value region, it is usually expressed as... This indicates that the safety conditions are met. This indicates that it is located within a safe boundary.

[0097] This represents the relative position vector between the target UAV and the dynamic obstacle; This represents the relative velocity vector between the target UAV and the dynamic obstacle; This indicates the preset safety rejection radius, which can be determined based on the target UAV's external dimensions, the dynamic obstacle's external dimensions, the positioning error, and the control response margin. The look-ahead time constant is used to adjust the degree of influence of the dynamic obstacle's motion trend on the safety boundary. This represents a continuous function used to characterize the approach trend between the target UAV and the dynamic obstacle, as well as the maneuvering direction of the dynamic obstacle.

[0098] Step S503: Construct dynamic control barrier function constraints based on the relative safety distance function and the obstacle acceleration state.

[0099] It should be noted that the dynamic control barrier function constraint refers to the safety inequality constructed based on the relative safe distance function and its rate of change along the system dynamics, which is used to limit the control variables so that the state of the target UAV remains within the safe set or tends towards the safe set.

[0100] Step S504: Construct a quadratic programming model based on the upper-layer reference control command, physical output constraints, and dynamic control barrier function constraints of the target UAV, and solve the quadratic programming model to obtain the safety control command.

[0101] It should be noted that physical output constraints refer to the feasible range of control variables determined by motor thrust, flight speed, flight acceleration, attitude angle, angular velocity, or other actuator capabilities.

[0102] In the specific implementation, the optimal estimated state of the obstacle (including position, velocity, and acceleration) is input into the dynamic control barrier function constraints in real time. The dynamic barrier function is defined. Its safety boundary expands asymmetrically with the predicted acceleration direction and turning trend of the obstacle. Construct a quadratic programming (QP) problem: in, Pre-set the initial control commands for drone trajectory tracking. It is a linear function. When the original instruction poses a collision risk, this quadratic programming method solves for a safe instruction that is closest to the original course and can absolutely avoid sudden obstacles within microseconds. It is fed to the motor for execution.

[0103] Furthermore, in order to improve the obstacle avoidance performance and control accuracy of the UAV, step S504 above may include: Step S5041: Determine physical output constraints based on the actuator parameters of the target UAV, wherein the physical output constraints include a minimum physical output boundary and a maximum physical output boundary.

[0104] It should be noted that actuator parameters refer to parameters characterizing the target UAV's power and control execution capabilities, and may include maximum motor speed, minimum stable speed, maximum thrust, minimum thrust, maximum roll angle, maximum pitch angle, maximum angular velocity, and maximum permissible acceleration. The minimum physical output boundary and the maximum physical output boundary respectively define the lower and upper limits of the control variables to be planned.

[0105] In practical implementation, the obstacle avoidance device can read the UAV model configuration file and flight controller parameter table, and correct the actuator boundaries based on the current battery voltage, payload weight, and flight mode. When the control variable to be planned is acceleration, the acceleration boundaries in each direction can be determined based on the maximum thrust, airframe mass, and attitude limits; when the control variables to be planned are total thrust and torque, the corresponding upper and lower limits can be determined based on the motor speed range and rotor hybrid control matrix.

[0106] Step S5042: Determine reference control variables based on the upper-level reference control commands of the target UAV, and determine control deviation variables based on the control variables to be planned and the reference control variables.

[0107] It should be noted that the reference control variable refers to the control vector obtained from the upper-level reference control command, which has the same physical meaning and dimension as the control variable to be planned. The control deviation variable refers to the difference between the control variable to be planned and the reference control variable.

[0108] It should be understood that constructing control deviation variables can quantify the degree to which safety filtering modifies the original track tracking control intent, providing an optimization target for maintaining track smoothness.

[0109] Step S5043: Construct the control deviation target term based on the control deviation variable and the preset positive definite weight matrix.

[0110] It should be noted that the preset positive definite weight matrix refers to the positive definite matrix used to determine the deviation cost of different control directions or different control components. The matrix can be preset according to the trajectory tracking accuracy, lateral obstacle avoidance priority, vertical maneuver energy consumption and control smoothness requirements.

[0111] Step S5044: Construct a set of constraint conditions based on the dynamic control barrier function constraint and the physical output constraint.

[0112] It should be noted that the set of constraints refers to the set of inequalities or equality that jointly limit the feasible domain of the control variables to be planned, including dynamic control barrier function constraints and physical output constraints; in multi-obstacle scenarios, it can include multiple safety constraints corresponding to each dynamic obstacle.

[0113] Step S5045: Construct a quadratic programming model based on the control deviation target term and the set of constraints.

[0114] It should be understood that by combining the control deviation objective term and the set of constraints into a standard quadratic programming form, feasible safety control quantities with small deviations from the reference command can be obtained in real time using a mature convex optimization solver.

[0115] This embodiment extracts the proximity relationship between the UAV and obstacles through relative state transformation, establishes a dynamic safety distance related to the motion trend through the safety repulsion radius and look-ahead time constant, corrects the control barrier function constraint through obstacle acceleration, and optimizes the control deviation within the physical output range through quadratic programming. This enables the obstacle state estimation to be converted into a real-time solvable safety control constraint, thereby taking into account the foresight of dynamic obstacle avoidance, control executability, and reference track maintenance capability.

[0116] This application also provides a dynamic obstacle avoidance system for unmanned aerial vehicles (UAVs) based on interactive multi-model and unscented Kalman filtering. Please refer to [link / reference]. Figure 3 , Figure 3 This is a schematic diagram of the module structure of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering in this application. The system includes a perception input module 10, an interactive prediction module 20, a dynamic safety filtering module 30, and an actuator output module 40. The interactive prediction module includes an interactive filtering unit, a model probability update unit 23, and a combined output unit 24. The interactive filtering unit includes an interactive action unit 21 and a parallel filtering unit 22. The perception input module 10 is used to collect the original spatial position observation data of dynamic obstacles during the flight of the target UAV, and generate an input dataset based on the original spatial position observation data, the flight status data of the target UAV, and the model estimation cache data of the previous sampling time. The interaction unit 21 and the parallel filtering unit 22 are used to perform multi-model interactive filtering on the preset nonlinear motion model set based on the input dataset to obtain the model filtering results of each motion model at the current sampling time. The preset nonlinear motion model set includes a uniform motion model, a left-turn cooperative turning motion model, and a right-turn cooperative turning motion model. The model filtering results include posterior state, posterior covariance, innovation residual, and innovation covariance. The model probability update unit 23 is used to calculate the model likelihood of each motion model based on the model filtering result, and dynamically update the model probability of each motion model based on the model likelihood to obtain the weight probability of each motion model at the current sampling time. The combined output unit 24 is used to dynamically weight and fuse the posterior states of each motion model at the current sampling time based on the weighted probabilities to obtain a dynamic obstacle fusion estimation state, which includes the position component, velocity component and acceleration component of the dynamic obstacle. The dynamic safety filtering module 30 is used to perform secondary planning processing on the upper-layer reference control command of the target UAV based on the dynamic obstacle fusion estimation state to obtain safety control command, and send the safety control command to the actuator output module; The actuator output module 40 is used to drive the target drone based on the safety control command.

[0117] In some embodiments, the sensing input module 10 acquires the original spatial position observation data of the obstacle with noise collected by the sensor, and defines a set of nonlinear models containing various typical motion characteristics. .in For a uniform velocity (CV) model, This is a Cooperative Left Turn (CT-L) model. This is a right-turn cooperative turning (CT-R) model.

[0118] The interactive prediction module 20 uses unscented transformation (UT) to avoid linearization error and dynamically adjusts the weight ratio of each motion model through state mixing interaction between various models and likelihood probability calculation.

[0119] The dynamic safety filtering module 30 receives the optimal combination state of obstacles (including the fused position, velocity, and acceleration prediction values) output by the interactive prediction module, and dynamically constructs a control barrier function that considers the multi-dimensional maneuvering characteristics of obstacles. The system performs online secondary planning (QP) auditing and filtering of upper-level reference control commands, and outputs physically executable safety control commands.

[0120] The following description, in conjunction with the accompanying drawings and examples, further illustrates the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering in this application.

[0121] System Example 1: This example uses a low-altitude, densely populated UAV cross-logistics operation environment as an application background to verify the feasibility and effectiveness of the system of the present invention. (Refer to...) Figure 4 , Figure 4 This is a schematic diagram of the module operation architecture in the first example of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering in this application.

[0122] When the main drone is traveling on a preset straight trajectory, an interfering drone (hereinafter referred to as a dynamic obstacle) that suddenly malfunctions or changes course crosses the main drone's trajectory on a non-linear path. At this time, the coordinated operation mechanism of the various modules of the system is as follows: The sensing input module 10, through airborne lidar or millimeter-wave radar, ... The sampling frequency continuously captures the spatial three-dimensional coordinates of the dynamic obstacle, and the original position dataset with observation noise is then used. The data is input in real time to the interactive prediction module 20. In the interactive prediction module 20, the preset model set includes three nonlinear kinematic models: a uniform velocity model... Left-turn cooperative turning model and right turn cooperative turning model .

[0123] ① During the first 3 seconds of the dynamic obstacle's linear motion, the interaction unit 21 and the parallel filtering unit 22 process the observed data, and the model probability update unit 23 calculates the residual likelihood to make the uniform velocity model... Model probability It remains at a high level of 92%. Based on this, the combined output unit 24 outputs a high-confidence linear motion state.

[0124] ② At 3.2 seconds, the dynamic obstacle suddenly makes a sharp left turn to avoid it (the lateral angular velocity increases). At this time, the parallel filtering unit 22, targeting the left-turn model... Observational residuals A sharp drop, corresponding to the new information covariance The optimal convergence state is reached.

[0125] ③ At the 3.4-second mark, the model probability update unit 23 calculates the model... The likelihood of the model has increased explosively, and the model probability has increased. Within two sampling periods, the percentage rapidly jumped from 4% to 85%. The combined output unit 24 immediately completed the calculation and output the fused optimal obstacle state estimate. This state includes precise instantaneous leftward yaw velocity and lateral acceleration components. .

[0126] The dynamic safety filtering module 30 receives the instantaneous velocity and lateral acceleration components in real time. The underlying control loop uses... During high-frequency operation, the dynamic safety filtering module 30 predicts the leftward trajectory of the pipeline based on the predicted dynamic obstacle within the next second. Before the safety critical point arrives, it proactively raises an outward-protruding "asymmetric safety barrier" (i.e., dynamic control barrier function) on the left side of the machine. (physical boundary).

[0127] When the original PD trajectory tracking control command of the machine When an attempt to intrude into the security barrier is detected, the quadratic programming (QP) solver within the dynamic security filtering module 30 is activated. It solves for the barrier function condition within 1.5 milliseconds. And the corrective acceleration control command that is closest to the original heading. Because the leftward acceleration of the obstacle was pre-calculated, the command instructed the actuator output module 40 to control the motor, allowing the machine to smoothly tangentially veer to the right. Since the entire "recognition-prediction-intervention" process was completed within 0.2 seconds, the machine did not experience any severe trajectory swaying or overshooting during obstacle avoidance, safely passing the nonlinear maneuver opportunity period.

[0128] Example System Example 2: See Figure 5 , Figure 5 This diagram illustrates the obstacle avoidance interaction process of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering in this application. It provides a detailed explanation of the specific mathematical calculations and logical switching processes for the obstacle avoidance interaction in the first example. The specific execution steps include: Step 1: Environmental perception and multi-model initialization.

[0129] The sensing input module 10 initializes the sensor parameters and sets the sampling period. Define the state vector. The interactive prediction module 20 loads the model transition probability matrix. : The interaction unit 21 determines the model probability based on the previous time step. Assign mixing weights and calculate the initial state after interactive mixing. .

[0130] Step 2: Sigma point sampling and parallel nonlinear filtering solution.

[0131] Parallel filtering unit 22 introduces unscented transform (UT) and sets proportionally symmetric sampling parameters. , , Generate for each model Sigma points.

[0132] I. Time Update: Substitute the Sigma point into the nonlinear state equations of their respective models: For turning models and Using turning angular velocity The cooperative turning (CT) state transition matrix is ​​used for one-step prediction to obtain the predicted state. .

[0133] II. Measurement Update: Through the observation matrix Mapping to location space and calculating actual observations The difference between the observed and predicted values ​​is used to obtain the innovation residual. and the corresponding new information covariance By updating with unscented gain, the posterior state of each branch is calculated.

[0134] Step 3: Model probability transformation and weight fusion output.

[0135] Model probability update unit 23 constructs a Gaussian probability density function based on the residual vector and solves for the likelihood of each model. : Update the weight probabilities of each model at the current time step. ,in This is a normalization constant. The combined output unit 24 performs a weighted summation of the states from the three branches, outputting the final fused estimation vector of obstacle position, velocity, and acceleration. .

[0136] Step 4: Dynamic CBF instruction auditing based on quadratic programming.

[0137] The dynamic CBF safety filtering module 30 inputs the fused estimation vector into the control barrier function. The relative safety distance function is defined as follows: In the formula, The relative safe distance function value at the k-th sampling time is used to characterize whether the relative motion state between the target UAV and the dynamic obstacle meets the safe distance condition. This represents the combined state of the target UAV and dynamic obstacles at the k-th sampling time. This represents the position vector of the target UAV at the k-th sampling time. This represents the dynamic obstacle position vector estimated using observation data up to the k-th sampling time. This represents the velocity vector of the target UAV at the k-th sampling time. This represents the dynamic obstacle velocity vector estimated using observation data up to the k-th sampling time. This represents the dynamic obstacle acceleration vector estimated using observation data up to the k-th sampling time. For safety repulsion radius (set to 1.2 meters in this example), As the look-ahead time constant (set to 0.5 seconds), this term explicitly incorporates the obstacle acceleration predicted in step three.

[0138] Construct a constrained quadratic programming (QP) mathematical model: in, It is a positive definite weight symmetric matrix; and This is a physical output saturation limit constraint for the drone motor.

[0139] Step 5: Issuance of security instructions and rolling feedback.

[0140] The QP solver performs an online evaluation of the inequality constraints from the previous step. If the original reference instruction... If the constraint inequality holds, it means the current trajectory is within a safe region, and the QP solver outputs the result directly. If the constraint inequalities do not hold, the solver will satisfy the physical limits of the motor. Within the boundary, calculate the control quantity that optimizes the system objective function (i.e., minimizes the change in the original control heading). The actuator output module 40 drives the drone hardware to complete obstacle avoidance maneuvers. The time step is then updated. The control flow returns to step one, forming a closed-loop rolling control.

[0141] It should be noted that the above examples are only for understanding this application and do not constitute a limitation on the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering. Any simple modifications based on this technical concept are within the protection scope of this application.

[0142] The UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering provided in this application, employing the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in the above embodiments, can solve the technical problems to be addressed by the aforementioned UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering. Compared with the prior art, the beneficial effects of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering provided in this application are the same as the beneficial effects of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering provided in the above embodiments, and other technical features of the UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0143] This application provides a dynamic obstacle avoidance device for unmanned aerial vehicles (UAVs). The UAV dynamic obstacle avoidance device includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in the first embodiment described above.

[0144] The following is for reference. Figure 6 , Figure 6 A schematic diagram of a dynamic obstacle avoidance device for unmanned aerial vehicles (UAVs) suitable for implementing embodiments of this application is shown. The UAV dynamic obstacle avoidance device can be an onboard computing device, flight controller, navigation controller, mission computer, or onboard control device integrating perception processing and flight control functions installed on the target UAV, or it can be a ground control device communicatively connected to the target UAV. The onboard computing device can communicate with lidar, millimeter-wave radar, visual camera, global navigation satellite system receiver, inertial measurement unit, and the actuator control module of the target UAV to obtain raw spatial position observation data of dynamic obstacles and flight status data of the target UAV, and send safety control commands to the flight controller or actuator control module. Figure 6 The drone dynamic obstacle avoidance device shown is merely an example and should not be construed as limiting the functionality and scope of application of the embodiments in this application.

[0145] like Figure 6As shown, the UAV dynamic obstacle avoidance device may include a processing unit 1001. The processing unit 1001 may include a central processing unit, graphics processing unit, digital signal processor, microcontroller, field-programmable gate array, application-specific integrated circuit, or other processing units with data processing and control computation capabilities. The processing unit 1001 can perform various computations and control processes according to a program stored in ROM 1002 (Read Only Memory) or a program loaded from storage device 1003 into RAM 1004 (Random Access Memory). For example, the processing unit 1001 can perform dynamic obstacle observation data preprocessing, multi-motion model interaction, unscented Kalman filtering, model probability updating, obstacle state weighted fusion, dynamic control barrier function constraint construction, and quadratic programming solution to obtain safety control commands for driving the target UAV to perform dynamic obstacle avoidance.

[0146] ROM1002 can store the startup program, basic control program, and fixed configuration parameters of the UAV dynamic obstacle avoidance device. RAM1004 can store the programs and data used during the operation of the UAV dynamic obstacle avoidance device, such as the original spatial position observation data of dynamic obstacles, the flight status data of the target UAV, the model estimation cache data of the previous sampling time, the model transition probability matrix, the posterior state and posterior covariance of each motion model, model probability, unmarked sampling points, innovation residuals, innovation covariance, dynamic obstacle fusion estimation state, and intermediate data in the quadratic programming process. Storage device 1003 can store the UAV dynamic obstacle avoidance program, motion model parameters, sensor calibration parameters, flight logs, historical obstacle trajectories, and safety control parameters.

[0147] Processing unit 1001, ROM 1002, and RAM 1004 can be connected to each other via bus 1005, and I / O interface 1006 can also be connected to bus 1005. Bus 1005 can be an internal communication bus for transmitting data, addresses, and control signals, such as a serial bus, parallel bus, or on-chip system bus. I / O interface 1006 can be used to connect sensing devices, flight status detection devices, control output devices, storage devices, and communication devices in the UAV dynamic obstacle avoidance device.

[0148] The input device 1007 may include a lidar, millimeter-wave radar, visual camera, depth camera, ultrasonic sensor, global navigation satellite system receiver, inertial measurement unit, altimeter, airspeed meter, and other devices for acquiring environmental information or flight status information. The lidar, millimeter-wave radar, visual camera, or depth camera can be used to collect raw spatial position observation data of dynamic obstacles; the global navigation satellite system receiver, inertial measurement unit, altimeter, or airspeed meter can be used to collect flight status data such as the target UAV's position, velocity, acceleration, attitude angle, angular velocity, altitude, or airspeed. In some embodiments, the input device 1007 may also include a human-machine interface for inputting flight mission, trajectory planning parameters, safety exclusion radius, look-ahead time constant, and model configuration parameters.

[0149] The output device 1008 may include a flight controller, an actuator control interface, an electronic speed controller, a motor control interface, a servo control interface, a display device, an audible and visual alarm device, or other control output devices. After receiving a safety control command, the processing device 1001 can send the safety control command to the flight controller via the I / O interface 1006. The flight controller can generate motor speed commands, rotor thrust commands, attitude control commands, speed control commands, or acceleration control commands based on the safety control command, and drive the target UAV to perform obstacle avoidance actions such as deceleration, hovering, turning, climbing, or descending via the electronic speed controller, motors, or servos. The display device or audible and visual alarm device can be used to display the dynamic obstacle status, collision risk information, obstacle avoidance trajectory, or equipment operating status.

[0150] Storage device 1003 may include solid-state memory, flash memory, memory card, hard disk, or other non-volatile storage media. Storage device 1003 may be used to store computer programs implementing a dynamic obstacle avoidance method for UAVs based on interactive multi-model and unscented Kalman filtering, and may also be used to store uniform motion models, left-turn cooperative turning motion models, right-turn cooperative turning motion models, process noise parameters, measurement noise parameters, model transition probability matrices, unscented transformation parameters, control barrier function parameters, and actuator physical output boundaries.

[0151] The communication device 1009 may include a wireless communication module and / or a wired communication interface. The wireless communication module may employ wireless local area network (WLAN), cellular mobile communication, Bluetooth, ultra-wideband (UWB), data radio, telemetry link, or other communication methods suitable for UAV communication; the wired communication interface may include a serial communication interface, a controller area network (Controller Area Network) interface, an Ethernet interface, or other airborne communication interfaces. The communication device 1009 can be used to exchange data between the UAV's dynamic obstacle avoidance device and sensors, flight controllers, ground control stations, other UAVs, or external servers. For example, the communication device 1009 can receive dynamic obstacle observation data and upper-level reference control commands, and send dynamic obstacle fusion estimation status, safety control commands, obstacle avoidance execution status, or flight logs.

[0152] In some embodiments, the processing device 1001 may be integrated with the input device 1007, output device 1008, storage device 1003, and communication device 1009 into the onboard mission computer of the target UAV. In other embodiments, the processing device 1001 may be located in a separate onboard computing module and communicate with the flight controller and various sensing devices via the communication device 1009. In other embodiments, some state estimation or path planning calculations may also be performed by ground control equipment, and the calculation results may be sent to the control terminal of the target UAV via a communication link.

[0153] It should be understood that, Figure 6 The devices and their connections shown are merely exemplary hardware structures and do not require the UAV dynamic obstacle avoidance device to possess all the devices shown in the figure. Depending on the target UAV's model, payload capacity, perception configuration, and control architecture, some devices can be added, removed, combined, or separated. For example, when the flight controller has integrated an inertial measurement unit and a global navigation satellite system receiver, the flight controller can provide flight status data to the processing device 1001; when the processing device 1001 integrates non-volatile memory, a separate ROM 1002 or storage device 1003 may not be necessary.

[0154] The UAV dynamic obstacle avoidance device provided in this application employs the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in the above embodiments, and can solve the technical problems to be solved by the aforementioned UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering. Compared with the prior art, the beneficial effects of the UAV dynamic obstacle avoidance device provided in this application are the same as those of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering provided in the above embodiments, and other technical features in this UAV dynamic obstacle avoidance device are the same as those disclosed in the previous embodiment method, and will not be repeated here.

[0155] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0156] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0157] This application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, which are used to execute the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering in the above embodiments.

[0158] The computer-readable storage medium provided in this application can be a tangible medium capable of storing computer programs, such as a hard disk, random access memory, read-only memory, flash memory, optical storage device, or magnetic storage device. When the computer program is executed by a processor, it implements the above-described UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering.

[0159] The aforementioned computer-readable storage medium may be included in the UAV dynamic obstacle avoidance device; or it may exist independently and not be installed in the UAV dynamic obstacle avoidance device.

[0160] The aforementioned computer-readable storage medium carries one or more programs. When the aforementioned one or more programs are executed by the UAV dynamic obstacle avoidance device, the UAV dynamic obstacle avoidance device performs the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in the above embodiments.

[0161] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including LAN (Local Area Network) or WAN (Wide Area Network)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0162] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.

[0163] The readable storage medium provided in this application is a computer-readable storage medium that stores computer-readable program instructions (i.e., a computer program) for executing the aforementioned UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering. This program is capable of solving the technical problems addressed by the aforementioned UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as those of the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering provided in the above embodiments, and will not be elaborated upon here.

[0164] The above description is only a part of the embodiments of this application and does not limit the patent scope of this application. All equivalent structural transformations made under the technical concept of this application and using the contents of the specification and drawings of this application, or direct / indirect applications in other related technical fields, are included in the patent protection scope of this application.

Claims

1. A dynamic obstacle avoidance method for unmanned aerial vehicles (UAVs) based on interactive multi-model and unscented Kalman filtering, characterized in that, The method includes: Collect raw spatial position observation data of dynamic obstacles during the flight of the target UAV. Generate an input dataset based on the raw spatial position observation data, the flight status data of the target UAV, and the model estimation cache data at the previous sampling time. The model estimation cache data at the previous sampling time includes the posterior state, posterior covariance, and model probability of each motion model. Based on the model probabilities and the preset model transition probability matrix, the interaction and mixing probabilities among the motion models in the preset nonlinear motion model set are calculated. The preset nonlinear motion model set includes a uniform motion model, a left-turn coordinated turning motion model, and a right-turn coordinated turning motion model. The uniform motion model describes the motion state of an obstacle maintaining a stable speed; the left-turn coordinated turning motion model describes the motion state of an obstacle coordinating a turn along the left-turn direction; and the right-turn coordinated turning motion model describes the motion state of an obstacle coordinating a turn along the right-turn direction. The interaction and mixing probabilities are calculated using the following formula: in, for The probability of interaction mixture at any given time; This is a normalization constant; This represents the probability that the motion pattern transitions from the i-th model to the j-th model; For model probabilities; The posterior state and posterior covariance are mixed based on the interactive mixture probability to obtain the mixed initial state and mixed initial covariance for each motion model. Unscented sampling points are generated for each motion model based on the mixed initial state and mixed initial covariance. The unscented sampling points are updated over time based on the nonlinear state equations corresponding to each motion model to obtain the predicted state and predicted covariance for each motion model. Spatial location observation sequences are extracted from the input dataset. Based on the spatial location observation sequences, nonlinear time prediction and observation correction are performed using unscented Kalman filtering to obtain the model filtering results for each motion model at the current sampling time. The model filtering results include the posterior state, posterior covariance, innovation residual, and innovation covariance. The posterior state refers to the state estimate obtained after correction using the current observation data. The posterior covariance is the uncertainty matrix of the posterior state estimation error. The innovation residual is the difference between the actual observed value and the predicted observed value. The innovation covariance is the uncertainty matrix of the innovation residual. The model likelihood of each motion model is calculated based on the model filtering results, and the model probability of each motion model is dynamically updated based on the model likelihood to obtain the weight probability of each motion model at the current sampling time. The positional components of each motion model are weighted based on the posterior state and the weighted probabilities to obtain the dynamic obstacle fusion positional component; the velocity components of each motion model are weighted based on the posterior state and the weighted probabilities to obtain the dynamic obstacle fusion velocity component; the acceleration components of each motion model are weighted based on the posterior state and the weighted probabilities to obtain the dynamic obstacle fusion acceleration component; a dynamic obstacle fusion estimation state is generated based on the dynamic obstacle fusion positional component, the dynamic obstacle fusion velocity component, and the dynamic obstacle fusion acceleration component. The dynamic obstacle fusion estimation state includes the positional component, velocity component, and acceleration component of the dynamic obstacle, and is generated with reference to the following formula: in, Indicates the dynamic obstacle fusion estimation state; This represents the fused acceleration estimate vector of the dynamic obstacle at the k-th sampling time; This represents the posterior acceleration estimate vector output by the j-th motion model at the k-th sampling time. Represents the fused location estimation vector; Represents the fusion velocity estimation vector; Let represent the posterior position estimation vector of the j-th motion model; Let represent the posterior velocity estimate vector of the j-th motion model; This represents the position component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the velocity component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the acceleration component of the dynamic obstacle in the x-direction estimated by the j-th motion model; This represents the position component of the dynamic obstacle in the y-direction estimated by the j-th motion model; This represents the velocity component of the dynamic obstacle in the y-direction estimated by the j-th motion model; This represents the acceleration component of the dynamic obstacle in the y-direction estimated by the j-th motion model; Based on the dynamic obstacle fusion estimation state and the flight state data, the relative position state, relative velocity state, and obstacle acceleration state between the target UAV and the dynamic obstacle are determined; A relative safety distance function is constructed based on the relative position state, the relative velocity state, the obstacle acceleration state, the preset safety repulsion radius, and the preset look-ahead time constant. The relative safety distance function is based on the following formula: in, The relative safety distance function value is used to characterize whether the relative motion state between the target UAV and dynamic obstacles meets the safety distance requirements. This represents the relative position vector between the target UAV and the dynamic obstacle; This represents the relative velocity vector between the target UAV and the dynamic obstacle; Indicates the preset safety repulsion radius; The look-ahead time constant is used to adjust the degree of influence of the dynamic obstacle's motion trend on the safety boundary; This represents a continuous function used to characterize the approach trend between the target UAV and dynamic obstacles, as well as the maneuvering direction of the dynamic obstacles; A dynamic control barrier function constraint is constructed based on the relative safety distance function and the obstacle acceleration state, and the safety boundary range corresponding to the dynamic control barrier function constraint is asymmetrically expanded with the acceleration direction and turning trend of the dynamic obstacle. A quadratic programming model is constructed based on the upper-layer reference control command, physical output constraint, and dynamic control barrier function constraint of the target UAV, and the quadratic programming model is solved to obtain the safety control command. The safety control command is then sent to the control terminal of the target UAV so that the control terminal drives the target UAV based on the safety control command.

2. The UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in claim 1, characterized in that, The process of generating unscented sampling points for each motion model based on the mixed initial state and the mixed initial covariance includes: Based on the aforementioned mixed initial state, the sampling center state corresponding to each motion model is determined; Based on the hybrid initial covariance and the preset sampling scale parameters, generate the covariance decomposition matrix corresponding to each motion model; Based on the sampling center state and the covariance decomposition matrix, a set of positive offset sampling points corresponding to each motion model is generated; Based on the sampling center state and the covariance decomposition matrix, generate the back offset sampling point set corresponding to each motion model; The sampling center state, the positive offset sampling point set, and the negative offset sampling point set are combined to obtain the unmarked sampling points corresponding to each motion model.

3. The UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in claim 1, characterized in that, The process of calculating the model likelihood of each motion model based on the model filtering results, and dynamically updating the model probability of each motion model based on the model likelihood to obtain the weight probability of each motion model at the current sampling time, includes: Calculate the innovation matching degree for each motion model based on the innovation residual and the innovation covariance; Based on the information matching degree, generate the model likelihood corresponding to each motion model; The unnormalized model probability of each motion model at the current sampling time is calculated based on the model likelihood and the model probability of each motion model at the previous sampling time. The unnormalized model probabilities are normalized to obtain the weight probabilities of each motion model at the current sampling time.

4. The UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in claim 1, characterized in that, The construction of a quadratic programming model based on the upper-layer reference control commands, physical output constraints, and dynamic control barrier function constraints of the target UAV includes: The physical output constraints are determined based on the actuator parameters of the target UAV, wherein the physical output constraints include a minimum physical output boundary and a maximum physical output boundary; Reference control variables are determined based on the upper-level reference control commands of the target UAV, and control deviation variables are determined based on the control variables to be planned and the reference control variables. The control deviation target term is constructed based on the control deviation variable and the preset positive definite weight matrix; A set of constraint conditions is constructed based on the dynamic control barrier function constraint and the physical output constraint; A quadratic programming model is constructed based on the control deviation target term and the set of constraints.

5. A UAV dynamic obstacle avoidance system based on interactive multi-model and unscented Kalman filtering, applying the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in any one of claims 1 to 4, characterized in that, The system includes: a perception input module, an interactive prediction module, a dynamic safety filtering module, and an actuator output module. The interactive prediction module includes an interactive filtering unit, a model probability update unit, and a combined output unit. The perception input module is used to collect the original spatial position observation data of dynamic obstacles during the flight of the target UAV, and generate an input dataset based on the original spatial position observation data, the flight status data of the target UAV, and the model estimation cache data of the previous sampling time. The interactive filtering unit is used to perform multi-model interactive filtering on a preset nonlinear motion model set based on the input dataset to obtain the model filtering results of each motion model at the current sampling time. The preset nonlinear motion model set includes a uniform motion model, a left-turn cooperative turning motion model, and a right-turn cooperative turning motion model. The model filtering results include posterior state, posterior covariance, innovation residual, and innovation covariance. The model probability update unit is used to calculate the model likelihood of each motion model based on the model filtering result, and dynamically update the model probability of each motion model based on the model likelihood to obtain the weight probability of each motion model at the current sampling time. The combined output unit is used to dynamically weight and fuse the posterior states of each motion model at the current sampling time based on the weighted probabilities to obtain a dynamic obstacle fusion estimation state, which includes the position component, velocity component and acceleration component of the dynamic obstacle. The dynamic safety filtering module is used to perform secondary planning processing on the upper-layer reference control command of the target UAV based on the dynamic obstacle fusion estimation state to obtain safety control command, and send the safety control command to the actuator output module; The actuator output module is used to drive the target drone based on the safety control command.

6. A dynamic obstacle avoidance device for unmanned aerial vehicles (UAVs), characterized in that, The UAV dynamic obstacle avoidance device includes: a memory, a processor, and a UAV dynamic obstacle avoidance program stored in the memory. The processor is used to run the UAV dynamic obstacle avoidance program, which is configured to implement the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a UAV dynamic obstacle avoidance program, which, when executed by a processor, implements the UAV dynamic obstacle avoidance method based on interactive multi-model and unscented Kalman filtering as described in any one of claims 1 to 4.

Images