Near-wall quadrotor safety control method based on model compensation

Abstract

The invention discloses a near-wall quadrotor safety control method based on model compensation, which belongs to the technical field of near-wall quadrotor safety control, improves a near-wall dynamic model in an MPC system by introducing suction compensation, and constructs an MPC into a factor graph comprising dynamic control, a reference trajectory, a control rate and a control limiting factor. The factor graph optimization FGO can solve and control input thrust and angular velocity under multiple constraints; the method specifically comprises three parts of dynamic modeling, suction compensation model prediction control and dynamic identification. A near-wall dynamic model in an MPC system is improved by introducing suction compensation, and the MPC is constructed into a factor graph FGO comprising dynamic control, a reference trajectory, a control rate and a control limiting factor; the SC-MPC is derived from force measurement data at different distances and rotating speeds, and the effectiveness of the SC-MPC is verified through a near-wall trajectory tracking experiment; compared with cascade proportion-integration-differentiation (PID) and manifold model predictive control (MPC), the tracking precision is remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of data communication in data centers, and particularly relates to a near-wall quadrotor safety control method based on model compensation. Background Technology

[0002] Multi-rotor drones (UAVs) are increasingly being used in confined urban spaces or indoor environments due to their mobility and versatility. Operations such as firefighting, search and rescue, window cleaning, and tunnel and wall inspection often require close-to-wall flight. However, in these applications, the near-wall effect causes airflow disturbances that severely threaten the flight stability of the drones.

[0003] The study found significant airflow disturbances between the quadcopter and the wall. Due to these aerodynamic disturbances, proportional-integral-derivative (PID) and model predictive control (MPC) methods could not prevent collisions. When the quadcopter approached the wall, it was pulled towards the wall. Due to the near-wall effect, the quadcopter continuously oscillated. Figure 1 As shown in (a), we first observed the airflow direction through smoke. Furthermore, force measurement experiments conducted on the test bench helped reveal the quantitative relationship of suction, such as... Figure 1 As shown in (b).

[0004] Suction modeling is crucial for compensating for dynamic model errors and maintaining trajectory accuracy when quadcopter drones approach walls. For example... Figure 1 As shown in -c, to fill this gap, this invention employs a more complete dynamic model that enhances the control method based on observational information about the distance between the duct and the wall, thereby achieving safer trajectory tracking during near-wall operations. However, offline parameters obtained from test bench measurements can change during flight. Therefore, this invention uses a flight data parameter identification method based on the Levenberg-Marquardt algorithm to estimate the suction model parameters. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a near-wall quadrotor safety control method based on model compensation to address the shortcomings of the prior art.

[0006] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0007] The near-wall quadrotor safety control method based on model compensation improves the near-wall dynamic model in the MPC system by introducing suction compensation, and constructs the MPC as a factor graph including dynamic control, reference trajectory, control rate and control constraint factors. The factor graph optimization FGO can solve the control input thrust and angular velocity under multiple constraints. Specifically, it includes three parts: dynamic modeling, suction compensation model predictive control and dynamic identification.

[0008] As a further preferred embodiment of the model-compensation-based near-wall quadrotor safety control method of the present invention, dynamic modeling includes:

[0009] The nominal dynamic model of the quadcopter is as follows:

[0010]

[0011] in, These represent the position, rotation matrix, velocity, and angular velocity of the quadcopter, respectively; e3 = [0, 0, 1] T ;m b It is mass; g is the acceleration due to gravity; T is the acceleration due to gravity. b It is thrust; Indicates air resistance; I b It is the inertial tensor; M b It is the aerodynamic torque; in the near-wall situation, the suction force has a significant impact on control stability and accuracy; considering the suction force, the acceleration model becomes:

[0012]

[0013] Among them, F s This indicates the suction force caused by the near-wall effect;

[0014] The formula for calculating air resistance is as follows:

[0015]

[0016] Where D = diag([d1, d2, d3]) T ) represents the resistance matrix.

[0017] As a further preferred embodiment of the model-compensated near-wall quadrotor safety control method of the present invention, dynamic modeling also includes: near-wall surface force modeling, as follows:

[0018] Let the k-th plane coordinate system, the body coordinate system, and the world coordinate system be respectively; where, the plane coordinate system... The x-axis is perpendicular to the wall, and the z-axis is opposite to the direction of gravity.

[0019] Based on a desktop mechanical measurement experiment: when the rotor speed is constant, the suction force F s With actual distance d and distance threshold d thr The difference between them is directly proportional, and its mathematical expression is:

[0020] F s =k s (dd thr (4)

[0021] Where, ks and d thr These are model parameters;

[0022] A quadcopter is equipped with four rotors, where the position of rotor j in the body coordinate system is defined as follows: The rotor in the k-plane coordinate system The position in the middle is represented as:

[0023]

[0024] The suction model of a quadcopter due to the near-wall effect is as follows:

[0025]

[0026] When the distance between the duct and the wall exceeds the threshold distance d thr At that time, the suction force F s It will decay to near zero.

[0027] As a further preferred embodiment of the near-wall quadrotor safety control method based on model compensation of the present invention, suction compensation model predictive control includes:

[0028] The core objective of Model Predictive Control (MPC) is to minimize the quadratic penalty term of the error between the predicted state and the input and their respective reference values ​​over the next N time steps. The traditional MPC problem is expressed as:

[0029]

[0030] in, and It is a set of polyhedra. For the terminal polyhedral region; Q≥0, G>0 (where 0≤k≤N-1) and P≥0 are the weight matrices of the state variable, control variable, and terminal state, respectively; u k and Where 0 ≤ k ≤ N-1) corresponds to the control input and its reference value. and For the reference state sequence, This represents the initial state from the state estimator;

[0031] Using FGO[27,31] to optimize the solution of MPC, (7) is restated as follows:

[0032]

[0033] in, and Q lim These represent different weighted matrices.

[0034] As a further preferred embodiment of the near-wall quadrotor safety control method based on model compensation of the present invention, various penalty terms are uniformly defined in the cost function, including trajectory tracking penalty terms and smoothness penalty terms, for the rate of change of control input u. t -u t+1 Apply constraints to prevent exceeding the bearing limit; simultaneously, the control quantity must satisfy the boundary constraint function h(u j The control module contains two types of residual factors: the reference trajectory residual r ref =xx ref Residuals of the dynamic model

[0035] Dynamic control factor:

[0036] The nominal dynamic model is updated according to formula (2), and the model specifically considers the suction effect when the quadcopter flies near the wall; the control framework adopts a two-layer structure, in which the upper controller is the model predictive control (MPC), which uses thrust and angular velocity as control inputs. State variables are defined as in For the corresponding rotation matrix The rotation vector, i.e. The underlying controller is responsible for tracking and controlling the output thrust and angular velocity of the MPC. The dynamic model used in the MPC... The expression is as follows:

[0037]

[0038] The discrete state propagation function in the dynamic factor residual is as follows:

[0039]

[0040] The dynamic factor residuals are as follows:

[0041]

[0042] For x i The Jacobian matrix is ​​as follows:

[0043]

[0044] in For detailed expressions, please refer to the appendix. Mathematical operator (θ) ∧ =logmap(R) can be equivalently represented as θ = Logmap(R);

[0045] For u i The Jacobian matrix is ​​as follows:

[0046]

[0047] For x i+1 The Jacobian matrix is ​​as follows:

[0048]

[0049] Reference trajectory factor:

[0050] The residuals of the reference trajectory factor are as follows:

[0051]

[0052] Among them, operators Defined as a pose subtraction operator, used to calculate the residuals of position, velocity, and rotation; specifically including: position residual. velocity residual and rotational residuals The reference trajectory residual vector can be obtained. The covariance matrix Q of the reference trajectory factor k (1≤k≤N) can be configured by the user;

[0053] Control rate factor and control constraint factor:

[0054] The control boundary factor aims to ensure that the control inputs satisfy the actuator's constraints; the hinge loss cost function E = h(u) is defined as the control input inequality. j Its description is as follows:

[0055]

[0056] in, and These represent the lower and upper limits of the input quantity, respectively; Q lim The weight matrix is ​​used to control the boundary factors. The matrix determines the rate of error growth as the input approaches the limit. Represents vector u j The nth element, E n The nth element of the corresponding vector E;

[0057] To smooth the trajectory, a control rate factor is used to limit the rate of change of the control input, and its mathematical expression is as follows:

[0058]

[0059] Solve the MPC problem using the Levenberg-Marquardt algorithm.

[0060] As a further preferred embodiment of the model-compensation-based near-wall quadrotor safety control method of the present invention, dynamic identification specifically includes:

[0061] The parameters of the dynamic model are accurately estimated using a parameter identification method, where the distance threshold d is used. thr and suction coefficient k s These are key suction parameters that need to be identified online; based on formulas (9) and (11), the force residual equation is defined as:

[0062]

[0063] Visual inertial odometry (VIO) can estimate acceleration in real time. speed and rotation matrix Based on a continuous observation data and control input sequence under suction conditions, multiple sets of observation equations are constructed using formula (18), thereby enabling the Levenberg-Marquardt algorithm to identify parameter d from the flight data. thr and k s ;

[0064] Force residual e F For coefficient k s The derivative is:

[0065]

[0066] e F For d thr The derivative is:

[0067] if

[0068]

[0069] otherwise,

[0070]

[0071] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0072] This invention presents a near-wall quadrotor safety control method based on model compensation. It improves the near-wall dynamic model in the MPC system by introducing suction compensation and constructs the MPC as a factor graph including dynamic control, reference trajectory, control rate, and control constraint factors. Factor graph optimization (FGO) can solve for the control input thrust and angular velocity under multiple constraints. Specifically, it includes three parts: dynamic modeling, suction-compensated model predictive control, and dynamic identification, ensuring the accuracy and safety of near-wall trajectory tracking. The method is derived from force measurement data at different distances and rotational speeds, and the effectiveness of the proposed SC-MPC is verified through near-wall trajectory tracking experiments. The quadrotor used in these experiments was equipped with four ducts to protect the rotor from collisions. Compared with cascaded proportional-integral-derivative PID and model predictive control MPC on manifolds, the tracking accuracy is significantly improved. Attached Figure Description

[0073] Figure 1 This is a schematic diagram of the rotor control and quantifiable aerodynamic effect research of the present invention;

[0074] Figure 2 This is a schematic diagram of the near-wall force modeling of the present invention;

[0075] Figure 3 This is a factor graph of the MPC problem in this invention. Detailed Implementation

[0076] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

[0077] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention. The present invention will be described in detail below with reference to the accompanying drawings and preferred embodiments. The purpose and effects of the present invention will become clearer. It should be understood that the specific embodiments described herein are merely illustrative of the present invention and are not intended to limit the present invention.

[0078] In compact urban or indoor environments, near-wall flight of quadcopters requires extremely high safety. However, achieving near-wall safety control is challenging due to unidentifiable airflow interactions (vortices exert suction on the aircraft, leading to vibration or even collisions). This invention reveals a suction model related to rotational speed and distance from the wall. To address this issue, we propose a suction-compensated model predictive control (SC-MPC) method to ensure both accuracy and safety in near-wall trajectory tracking. By introducing suction compensation, we improve the near-wall dynamic model in the MPC system and construct the MPC as a factor graph containing dynamic control, a reference trajectory, control rate, and control constraint factors. Factor graph optimization (FGO) solves for the control inputs (thrust and angular velocity) under multiple constraints. Our force model is derived from force measurement data at different distances and rotational speeds, and the effectiveness of the proposed SC-MPC is verified through near-wall trajectory tracking experiments. The quadcopter used in these experiments was equipped with four ducts to protect the rotor from collisions. Tracking accuracy is significantly improved compared to cascaded proportional-integral-derivative (PID) and model predictive control (MPC) on a manifold.

[0079] This invention, through airflow visualization and empirical tabletop experiments, specifically reveals the rotor's wall-attaching effect, ultimately establishing a suction model caused by this effect. Furthermore, the suction-compensated dynamic model can also be applied to surface-based dynamic control (MPC). During dynamic flight near the wall, due to differences in aerodynamic configurations between actual flight experiments and tabletop experiments, a more accurate model can be determined from flight data.

[0080] Specifically, the main contributions of this invention include:

[0081] 1) Through smoke visualization experiments and tabletop mechanical experiments, a detailed explanation of the near-wall effect and a suction model are proposed.

[0082] 2) A method for identifying suction parameters in dynamic models is proposed.

[0083] 3) Based on manifold model predictive control (MPC), a dynamic model compensation strategy is proposed for near-wall safety control.

[0084] Figure 1 This study focuses on rotor control and quantifiable aerodynamic effects. Specifically, (a) suction mechanism research: (a-1) schematic diagram shows the airflow around the rotor duct when the ducted rotor is close to a wall. (a-2) and (a-3) show the smoke flow paths under wall-free and wall-near conditions, respectively. (b) Force measurement experiments were conducted on a test platform. (c) The proposed control method achieves accurate trajectory tracking and avoids collisions with the wall.

[0085] A. Dynamic modeling:

[0086] The nominal dynamic model of the quadcopter is as follows:

[0087]

[0088] in These represent the position, rotation matrix, velocity, and angular velocity of the quadcopter, respectively. e3 = [0, 0, 1] T m b It's mass. g is the acceleration due to gravity. T b It is thrust. Indicates air resistance. I b It is the inertial tensor. M b This is the aerodynamic torque. However, in the near-wall situation, the suction force has a significant impact on control stability and accuracy. Considering the suction force, the acceleration model becomes:

[0089]

[0090] Where F s This indicates the suction force caused by the near-wall effect.

[0091] The formula for calculating air resistance is as follows:

[0092]

[0093] Where D = diga([d1,d2,d3]) T ) represents the resistance matrix.

[0094] Let represent the k-th plane coordinate system, the body coordinate system, and the world coordinate system, respectively. Among them, the plane coordinate system... The x-axis is perpendicular to the wall, and the z-axis is opposite to the direction of gravity.

[0095] like Figure 2 As shown, when the quadcopter drone approaches plane i, its rotors 2 and 3 will experience a specific suction force. This mechanism is specifically manifested as follows: Figure 1 As shown in (a), the airflow between the wall and the ducted rotor forms a ring-shaped backflow, and the high-speed airflow between the aircraft and the wall will generate a negative pressure effect.

[0096] like Figure 1 As shown in (b), this study quantifies the suction model through mechanical measurement experiments. Details of the relevant experimental procedures will be described in Section 4. Based on the desktop mechanical measurement experiments, the key conclusion is: when the rotor speed is constant, the suction force F... s With actual distance d and distance threshold d thr The difference between them is directly proportional, and its mathematical expression is:

[0097] F s =k s (ddthr (4)

[0098] Where k s and d thr These are model parameters.

[0099] A quadcopter is equipped with four rotors, where the position of rotor j in the body coordinate system is defined as follows: The rotor is in the k-plane coordinate system The position in can be represented as:

[0100]

[0101] Therefore, the suction force model generated by the near-wall effect in a quadcopter is as follows:

[0102]

[0103] When the distance between the duct and the wall exceeds the threshold distance d thr At that time, the suction force F s It will decay to near zero.

[0104] B. Suction Compensation Model Predictive Control The core objective of model predictive control (MPC) is to minimize the quadratic penalty term of the error between the predicted state and the input and their respective reference values ​​over the next N time steps [6]. The traditional MPC problem can be expressed as:

[0105]

[0106] in, and It is a set of polyhedra. Let be the terminal polyhedral region. Q≥0, G>0 (0≤k≤N-1) and P≥0 are the weight matrices for the state variables, control variables, and terminal states, respectively. k and Corresponding control inputs and their reference values, and For the reference state sequence, This represents the initial state from the state estimator.

[0107] In order to optimize the solution of MPC using FGO[27,31], (7) can be restated as follows:

[0108] in and Q lim These represent different weighted matrices.

[0109] This study uniformly defines various penalty terms in the cost function, including trajectory tracking penalty terms and smoothness penalty terms. To ensure the safe operation of the actuator, the rate of change of the control input u is... t -u t+1 Constraints are applied to prevent exceeding the bearing limit. Simultaneously, the control variables must satisfy the boundary constraint function h(u). j The control module mainly includes two types of residual factors: the reference trajectory residual r. ref =xx ref Residuals of the dynamic model MPC factor graph construction as follows Figure 3 As shown.

[0110] 4) The dynamic control factor nominal dynamic model is updated according to formula (2). This model specifically considers the suction effect of the quadcopter when flying near the wall. The control framework adopts a two-layer structure design, in which the upper controller is a model predictive control (MPC), which uses thrust and angular velocity as control inputs. Furthermore, the state quantity is defined as in For the corresponding rotation matrix The rotation vector, i.e. The underlying controller is responsible for tracking and controlling the output thrust and angular velocity of the MPC. Therefore, the dynamic model used in the MPC... This can be expressed as:

[0111]

[0112] The discrete state propagation function in the dynamic factor residual is as follows:

[0113]

[0114] The dynamic factor residuals are as follows:

[0115]

[0116] For x i The Jacobian matrix is ​​as follows:

[0117]

[0118] in For detailed expressions, please refer to the appendix. Mathematical operator (θ) ∧ =logmap(R) can be equivalently represented as θ = Logmap(R).

[0119] For u i The Jacobian matrix is ​​as follows:

[0120]

[0121] For x i+1 The Jacobian matrix is ​​as follows:

[0122]

[0123] 5) Reference trajectory factor: The residuals of the reference trajectory factor are as follows:

[0124]

[0125] Among them, operators Defined as a pose subtraction operator, it is used to calculate the residuals of position, velocity, and rotation. Specifically, it includes: position residual. velocity residual and rotational residuals From this, the reference trajectory residual vector can be obtained. Furthermore, the covariance matrix Q of the reference trajectory factor k (1≤k≤N) can be configured by the user.

[0126] 6) Control rate factor and control limit factor. The control boundary factor aims to ensure that the control input meets the actuator's limits. The hinge loss cost function E = h(u) is defined as the control input inequality. j Its description is as follows:

[0127]

[0128] in and These represent the lower and upper limits of the input quantity, respectively. Q lim The weight matrix is ​​used to control the boundary factors, which determines the rate of error growth as the input approaches its limit. Represents vector u j The nth element, E n It corresponds to the nth element of vector E.

[0129] To smooth the trajectory, a control rate factor is used to limit the rate of change of the control input, and its mathematical expression is as follows:

[0130]

[0131] Finally, the Levenberg-Marquardt algorithm is used to solve the MPC problem.

[0132] C. Dynamic Recognition

[0133] Although the principle and basic model of suction were obtained through desktop mechanical measurement experiments, the aerodynamic configuration of actual aircraft differs from the experimental conditions. Therefore, a parameter identification method was employed to accurately estimate the parameters of the dynamic model, including the distance threshold d. thr and suction coefficient k s These are the key suction parameters that need to be identified online. Based on formulas (9) and (11), the force residual equation is defined as:

[0134]

[0135] Visual inertial odometry (VIO) can estimate acceleration in real time. speed and rotation matrix Based on a continuous observation data and control input sequence under suction conditions, multiple sets of observation equations are constructed using formula (18), thereby enabling the Levenberg-Marquardt algorithm to identify parameter d from the flight data. thr and k s .

[0136] Force residual e F For coefficient k s The derivative is:

[0137]

[0138] e F For d thr The derivative is:

[0139] if

[0140]

[0141] otherwise,

[0142]

[0143] It will be understood by those skilled in the art that the above descriptions are merely preferred examples of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the invention should be included within the scope of protection of the invention. All technical features in this embodiment can be freely combined according to actual needs.

[0144] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A near-wall quadrotor safety control method based on model compensation, characterized in that: The near-wall dynamic model in the MPC system was improved by introducing suction compensation, and the MPC was constructed as a factor graph containing dynamic control, reference trajectory, control rate and control constraint factors. The factor graph optimization FGO can solve the control input thrust and angular velocity under multiple constraints; specifically, it includes three parts: dynamic modeling, suction compensation model predictive control and dynamic identification.

2. The near-wall quadrotor safety control method based on model compensation according to claim 1, characterized in that: Dynamic modeling, including: The nominal dynamic model of the quadcopter is as follows: in, ω b These represent the position, rotation matrix, velocity, and angular velocity of the quadcopter, respectively; e3 = [0, 0, 1] T m b It is mass; g is the acceleration due to gravity; T b It is thrust; Indicates air resistance; I b It is the inertial tensor; M b It is the aerodynamic torque; in the near-wall situation, the suction force has a significant impact on control stability and accuracy; considering the suction force, the acceleration model becomes: Among them, F s This indicates the suction force caused by the near-wall effect; The formula for calculating air resistance is as follows: Where D = diag([d1, d2, d3]) T ) represents the resistance matrix.

3. The near-wall quadrotor safety control method based on model compensation according to claim 1, characterized in that: Dynamic modeling also includes: near-wall force modeling, as detailed below: F b ,F w Let the k-th plane coordinate system, the body coordinate system, and the world coordinate system be respectively; where, the plane coordinate system... The x-axis is perpendicular to the wall, and the z-axis is opposite to the direction of gravity. Based on a desktop mechanical measurement experiment: when the rotor speed is constant, the suction force F s With actual distance d and distance threshold d thr The difference between them is directly proportional, and its mathematical expression is: F s =k s (d-d thr ) (4) Where, k s and d thr These are model parameters; A quadcopter is equipped with four rotors, where the position of rotor j in the body coordinate system is defined as follows: The rotor in the k-plane coordinate system The position in the middle is represented as: The suction model of a quadcopter due to the near-wall effect is as follows: with e1=[1,0,0] T , if else, When the distance between the duct and the wall exceeds the threshold distance d thr At that time, suction force F s It will decay to near zero.

4. The near-wall quadrotor safety control method based on model compensation according to claim 1, characterized in that: Suction compensation model predictive control, including: The core objective of Model Predictive Control (MPC) is to minimize the quadratic penalty term of the error between the predicted state and the input and their respective reference values ​​over the next N time steps. The traditional MPC problem is expressed as: in, and It is a set of polyhedra. For the terminal polyhedral region; Q≥0, G>0 (where 0≤k≤N-1) and P≥0 are the weight matrices of the state variable, control variable, and terminal state, respectively; u k and Where 0 ≤ k ≤ N-1) corresponds to the control input and its reference value. and For the reference state sequence, This represents the initial state from the state estimator; Using FGO[27,31] to optimize the solution of MPC, (7) is restated as follows: Among them, Q k , G t and Q lim These represent different weighted matrices.

5. The near-wall quadrotor safety control method based on model compensation according to claim 4, characterized in that: In the cost function, various penalty terms are uniformly defined, including trajectory tracking penalty terms and smoothness penalty terms, for the rate of change of control input u. t -u t+1 Apply constraints to prevent exceeding the bearing limit; simultaneously, the control quantity must satisfy the boundary constraint function h(u j The control module contains two types of residual factors: the reference trajectory residual r ref =xx ref Residuals of the dynamic model 1) Dynamic control factor: The nominal dynamic model is updated according to formula (2), and the model specifically considers the suction effect when the quadcopter flies near the wall; the control framework adopts a two-layer structure, in which the upper controller is the model predictive control (MPC), which uses thrust and angular velocity as control inputs. State variables are defined as in For the corresponding rotation matrix The rotation vector, i.e. The underlying controller is responsible for tracking and controlling the output thrust and angular velocity of the MPC. The dynamic model used in the MPC... The expression is as follows: The discrete state propagation function in the dynamic factor residual is as follows: The dynamic factor residuals are as follows: For x i The Jacobian matrix is ​​as follows: in The mathematical operator (θ)^=logmap(R) can be equivalently represented as θ=Logmap(R); For u i The Jacobian matrix is ​​as follows: For x i+1 The Jacobian matrix is ​​as follows: 2) Reference trajectory factor: The residuals of the reference trajectory factor are as follows: Among them, operators Defined as a pose subtraction operator, used to calculate the residuals of position, velocity, and rotation; specifically including: position residual. velocity residual and rotational residuals The reference trajectory residual vector can be obtained. The covariance matrix Q of the reference trajectory factor k (1≤k≤N) can be configured by the user; 3) Control rate factor and control constraint factor: The control boundary factor aims to ensure that the control inputs satisfy the actuator's constraints; the hinge loss cost function E = h(u) is defined as the control input inequality. j Its description is as follows: in, and These represent the lower and upper limits of the input quantity, respectively; Q lim The weight matrix is ​​used to control the boundary factors. The matrix determines the rate of error growth as the input approaches the limit. Represents vector u j The nth element, E n The corresponding element is the nth element of vector E; To smooth the trajectory, a control rate factor is used to limit the rate of change of the control input, and its mathematical expression is as follows: Solve the MPC problem using the Levenberg-Marquardt algorithm.

6. The near-wall quadrotor safety control method based on model compensation according to claim 4, characterized in that: Dynamic recognition, specifically includes: The parameters of the dynamic model are accurately estimated using a parameter identification method, where the distance threshold d is used. thr and suction coefficient k s These are key suction parameters that need to be identified online; based on formulas (9) and (11), the force residual equation is defined as: Visual inertial odometry (VIO) can estimate acceleration in real time. speed and rotation matrix Based on a continuous observation data and control input sequence under suction conditions, multiple sets of observation equations are constructed using formula (18), thereby enabling the Levenberg-Marquardt algorithm to identify parameter d from the flight data. thr and k s ; Force residual e F For coefficient k s The derivative is: e F For d thr The derivative is: if otherwise,

Images