A rotor unmanned aerial vehicle physical interaction impedance control method based on active disturbance rejection

Abstract

The application discloses a kind of based on self-disturbance's rotor unmanned plane physical interaction impedance control method, it is related to multi-rotor unmanned plane impedance control field, including: establishing the dynamic model of unmanned plane translation and rotation separation and setting reference trajectory, design cascade structure basic bottom control law, by nonlinear attitude mapping and accurate control distribution conversion into execution instruction;Again construct linear extended state observer real-time estimate external disturbance and system unmodeled dynamics, establish including virtual mass, damping, stiffness matrix target impedance model;Finally, disturbance estimation and impedance model are fused, and the position loop control law of integrated characteristics is derived.The method does not need to install physical force sensor, reduces system mass and cost, solves the problem of sensor dependence in traditional control and underactuated system fusion, realizes high-precision trajectory tracking and adjustable compliant interaction, guarantees the safety and reliability of unmanned plane in unknown environment contact operation.

Description

Technical Field

[0001] This invention relates to the field of impedance control for multi-rotor unmanned aerial vehicles (UAVs), and more particularly to a physical interactive impedance control method for multi-rotor UAVs based on active disturbance rejection. Background Technology

[0002] Multirotor drones have achieved mature applications in free-space trajectory tracking, but traditional control architectures face new technical challenges in physical contact operations in unknown environments. These operations require drones to have compliant interaction capabilities with the environment, but existing control strategies are mostly based on cascaded rigid control designs of translation and rotation, which are difficult to adapt to the external forces generated during contact. With the expanding applications of drones in industrial inspection, material delivery, and close-range interaction, the demand for their safe operation capabilities in physical contact scenarios is becoming increasingly urgent. There is a pressing need for a technical solution that can achieve compliant control without additional hardware modifications to balance trajectory tracking accuracy and interaction safety.

[0003] The core drawbacks of existing technologies are mainly reflected in two aspects: First, compliant control is highly dependent on physical force sensors, requiring the installation of devices such as six-dimensional force sensors on the UAV to sense external contact forces. This not only increases the overall mass and manufacturing cost of the aircraft, but also introduces measurement noise, interferes with the dynamic response performance of the system, and limits the payload and operational flexibility of the UAV. Second, there is the challenge of control fusion in underactuated systems. As a strongly coupled system with six degrees of freedom but only four control inputs, the existing methods fail to deeply integrate external disturbance estimation with physical impedance parameters. This makes it impossible to achieve high-precision trajectory tracking and adjustable active yielding characteristics without force sensors, which can easily lead to system instability or aircraft damage during contact. Summary of the Invention

[0004] In order to overcome the shortcomings and deficiencies of the existing technology, the present invention provides a physical interactive impedance control method for rotary-wing unmanned aerial vehicles based on self-disturbance rejection.

[0005] The technical solution adopted in this invention is a physical interactive impedance control method for rotary-wing unmanned aerial vehicles based on self-disturbance rejection, comprising the following steps: S1. Establish a dynamic model that separates translation and rotation of a multi-rotor UAV. Construct translational dynamic equations in the inertial coordinate system based on the laws of motion, and establish rotational dynamic equations in the body coordinate system based on Euler's equations. At the same time, set the reference position trajectory and reference velocity trajectory. S2 is the basic control law for designing the cascade control structure. The position loop converts the position error and velocity deviation into the desired linear acceleration vector, and the attitude loop obtains the desired angular acceleration vector through the angle deviation and angular velocity deviation. S3 performs nonlinear attitude mapping and control allocation, determines the total tension command after deducting the influence of gravity, and calculates the desired roll angle and pitch angle. The control torque is obtained based on inverse dynamics feedforward compensation, and the motor speed square command is output by inverting the control efficiency matrix. S4, construct a linear extended state observer, classify unmodeled dynamics and external forces as total disturbance, and estimate velocity and total disturbance per unit mass in real time through state equations; S5. Based on the expected and actual errors, establish the target impedance dynamic equation including the virtual mass matrix, virtual damping matrix and virtual stiffness matrix; S6 replaces the actual acceleration in the target impedance dynamic equation with the desired acceleration command, performs feedforward compensation for the estimated disturbance of the extended state observer, and analyzes the position loop desired line acceleration vector control law of the fused impedance characteristics.

[0006] Furthermore, the translational dynamics equations satisfy: , in, For the total mass of the drone, Let be the position and acceleration vector in the inertial frame. This represents the total tensile force amplitude. The rotation matrix from the machine system to the inertial system. Let z be the unit vector of the inertial frame z-axis. Let gravitational acceleration be a scalar. This is the external contact force vector; The reference velocity trajectory satisfies: , in, For the desired velocity vector, For the initial desired speed, The desired acceleration trajectory.

[0007] Furthermore, the basic control law of the position loop satisfies: , in, Based on expected acceleration, This is the position scaling gain matrix. The velocity differential gain matrix is... As the reference position vector, This is the actual position vector. This is the actual velocity vector; The attitude loop basic control law satisfies: , in, Based on the expected angular acceleration, This is the attitude scaling gain matrix. Here is the attitude differential gain matrix. As the reference attitude angle vector, This is the actual attitude angle vector. This is the actual angular velocity vector.

[0008] Furthermore, the total tension command in the nonlinear attitude mapping satisfies: , in, For vector magnitude operations; Control torque distribution satisfies: , in, To control the torque vector, Here is the rotational inertia matrix. The time derivative of the moment of inertia matrix. This is the gyroscopic torque term; The command for the square of the motor speed satisfies: , in, This is the motor speed vector. To control the efficiency matrix, It is a vector formed by the squares of the rotational speeds.

[0009] Furthermore, the state equation of the extended state observer satisfies: ,in, This is a location estimate. This is a speed estimate. To control the gain coefficient, To control the input, For observer gain, This is a location measurement value; The total disturbance estimate satisfies: , in, This is the estimated total disturbance per unit mass. To extend the state estimate, To extend the state observer gain.

[0010] Furthermore, the target impedance dynamic equation satisfies: , in, For virtual mass matrix, For virtual damping matrix, This is a virtual stiffness matrix; The control law for the integrated impedance characteristics satisfies: , in, The final desired linear acceleration vector, It is the inverse of the virtual mass matrix.

[0011] Furthermore, step S3 includes the following sub-steps: S31, in the calculation of total tension command, the desired linear acceleration vector output by the basic control law is extracted, and after superimposing the gravitational acceleration vector, the magnitude is calculated to obtain the total tension amplitude after deducting the influence of gravity. S32, combined with the preset desired yaw angle, solves the attitude angle relationship through the inverse operation of the rotation matrix, and rigorously derives the desired roll angle and pitch angle, eliminating the error caused by small angle approximation; S33, based on attitude dynamics equations, adopts an inverse dynamics feedforward compensation strategy to counteract the effects of rotational inertia coupling and gyro effect, and generates three-axis control torque commands that meet attitude tracking requirements. S34, for multi-rotor configuration, constructs a control efficiency matrix, and converts the total thrust command and control torque command into the corresponding motor speed square command through matrix inversion operation, and then allocates the control quantity.

[0012] Furthermore, step S4 includes the following sub-steps: S41. Position and velocity are selected as the basic state variables. External contact force, unmodeled dynamics and system noise are merged into the total disturbance and extended into new state variables to construct a complete state equation. S42 uses the bandwidth method to configure the observer gain parameters, sets the observation bandwidth according to the system dynamic response requirements, and determines the gain matrix of the linear extended state observer through the pole placement principle. S43: Collect the actual position and velocity signals output by the UAV's inertial measurement unit as input to the observer and update the state estimate in real time; S44, by extending the output of the state observer, obtains the total disturbance estimate per unit mass, which includes comprehensive information on external contact force and system disturbance, providing data support for subsequent compensation.

[0013] Furthermore, step S5 includes the following sub-steps: S51, based on the physical interaction requirements of UAVs, designs a virtual mass matrix. By adjusting the values ​​of the diagonal elements of the matrix, it shapes the equivalent inertial characteristics of the system and adapts to the dynamic response under different interaction scenarios. S52 is equipped with a virtual damping matrix. The matrix parameters are set according to the energy dissipation requirements to enable the UAV to generate a moderate damping effect during force-driven motion and avoid oscillation. S53, set the virtual stiffness matrix, and determine the restoring force characteristics of the UAV when it deviates from the reference trajectory by adjusting the proportional relationship parameters between position and force; S54, based on the deviation between the desired position, velocity, acceleration and the actual state, combines the virtual mass matrix, virtual damping matrix, virtual stiffness matrix and deviation term to construct a complete target impedance dynamic equation and determine the dynamic mapping relationship between position and force.

[0014] Furthermore, step S6 includes the following sub-steps: S61 replaces the actual acceleration term in the target impedance dynamic equation with the desired acceleration command required by the control system, reorganizes the equation structure, and highlights the relationship between the desired acceleration and the deviation and external force terms. S62, extract the total disturbance estimate from the extended state observer output, and introduce it as a feedforward compensation term into the rearranged equation to dynamically cancel the uncertainty between external disturbances and the system. S63, perform algebraic analysis on the equation including the compensation term, eliminate the external force variables, and derive the expression for the position loop desired linear acceleration control law that integrates impedance characteristics and self-disturbance compensation; S64 expands the matrix in the control law expression into a three-axis scalar form, determines the independent control parameters and calculation logic for each axis, and forms an instruction output form that can be directly used for low-level control.

[0015] Beneficial Effects: This invention proposes a physical interactive impedance control method for rotary-wing UAVs based on active disturbance rejection. Through dynamic modeling that separates translation and rotation, designing cascade basic control laws, nonlinear attitude mapping and precise control allocation, combined with real-time disturbance estimation of external contact forces and unmodeled system dynamics by a linear extended state observer, and further through deep fusion of mass, damping, and spring target impedance models with feedforward compensation, a position loop control law with integrated impedance characteristics is derived. This method can accurately sense external interactive forces without the need for physical force sensors, reducing aircraft mass and manufacturing costs, avoiding interference from measurement noise on the system's dynamic response, and solving the problem of dependence on force sensors in traditional impedance control. Meanwhile, considering the underactuated and strongly coupled system characteristics of multi-rotor UAVs, this method deeply integrates disturbance estimation with impedance parameters. By reshaping the system's equivalent inertia and feedback stiffness, the aircraft can maintain high-precision time-varying trajectory tracking capabilities while possessing adjustable active yielding characteristics when facing external forces. This transforms the resistant response of traditional rigid control into physical-level compliance, effectively avoiding the risk of system instability or aircraft damage during contact. It successfully overcomes the technical challenge of integrating disturbance estimation and impedance control in underactuated systems, providing reliable technical support for UAVs to safely perform physical contact operations in unknown environments. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating the overall process of the method of the present invention. Figure 2This is a flowchart of method step S3 of the present invention; Figure 3 This is a flowchart of method step S4 of the present invention; Figure 4 This is a flowchart of step S5 of the method of the present invention; Figure 5 This is a flowchart of step S6 of the method of the present invention. Detailed Implementation

[0017] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0018] like Figure 1 As shown, a physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on active disturbance rejection includes the following steps: S1. Establish a dynamic model that separates translation and rotation of a multi-rotor UAV. Construct translational dynamic equations in the inertial coordinate system based on the laws of motion, and establish rotational dynamic equations in the body coordinate system based on Euler's equations. At the same time, set the reference position trajectory and reference velocity trajectory. S2 is the basic control law for designing the cascade control structure. The position loop converts the position error and velocity deviation into the desired linear acceleration vector, and the attitude loop obtains the desired angular acceleration vector through the angle deviation and angular velocity deviation. S3 performs nonlinear attitude mapping and control allocation, determines the total tension command after deducting the influence of gravity, and calculates the desired roll angle and pitch angle. The control torque is obtained based on inverse dynamics feedforward compensation, and the motor speed square command is output by inverting the control efficiency matrix. S4, construct a linear extended state observer, classify unmodeled dynamics and external forces as total disturbance, and estimate velocity and total disturbance per unit mass in real time through state equations; S5. Based on the expected and actual errors, establish the target impedance dynamic equation including the virtual mass matrix, virtual damping matrix and virtual stiffness matrix; S6 replaces the actual acceleration in the target impedance dynamic equation with the desired acceleration command, performs feedforward compensation for the estimated disturbance of the extended state observer, and analyzes the position loop desired line acceleration vector control law of the fused impedance characteristics.

[0019] Step S1 involves constructing an accurate dynamic model and setting a reasonable reference trajectory to provide a foundation for control. In the inertial coordinate system, based on the laws of motion and considering the total mass parameters of the UAV (typically ranging from 1 to 5 kg), the translational dynamic equations are constructed, taking into account the effects of total tension, gravity, and external forces. These equations must accurately reflect the positional changes in three-dimensional space, ensuring a precise description of the relationship between the UAV's translational acceleration and the various forces acting upon it. In the body coordinate system, based on Euler's equations, the rotational inertia matrix parameters are input (the rotational inertia values ​​for each axis range from 0.01 to 0.1 kg). (square meters), combined with parameters such as angular velocity, angular acceleration, and external disturbance torque, a rotational dynamics equation is established to fully characterize the dynamic response characteristics of the three attitude angles: roll, pitch, and yaw. The reference trajectory setting must meet the requirements of smooth tracking. The reference position trajectory is based on the preset coordinates of the start point, waypoints, and end point according to the task, with the spacing between adjacent points controlled within the range of 0.5 to 5 meters. A smooth transition algorithm is used to avoid abrupt trajectory changes. The reference velocity trajectory is generated based on the time derivative of the reference position trajectory. Simultaneously, considering the dynamic performance limits of the UAV, the upper limit of speed is set to 3 to 15 meters per second, and the peak acceleration is controlled within 2 to 8 meters per second squared to ensure smooth speed changes. Through the above parameter settings and model construction, a definite and feasible tracking target is provided for subsequent control law design, ensuring the effectiveness and accuracy of the control strategy implementation.

[0020] Step S2 involves designing the underlying control law for the cascade control structure, achieving precise position and attitude control through layered regulation. In the position loop control law design, the actual position data is first collected in real-time by the positioning module onboard the UAV and compared with a preset reference position to obtain the position error. Simultaneously, the actual velocity data is acquired by the velocity measurement module and compared with a reference velocity to obtain the velocity deviation. A position proportional gain matrix and a velocity differential gain matrix are introduced. The parameters of the position proportional gain matrix range from 5 to 20, and the parameters of the velocity differential gain matrix range from 2 to 10. These gain parameters are used to weight the position error and velocity deviation, transforming them into a desired linear acceleration vector to ensure the speed and accuracy of position tracking. In the design of the attitude loop basic control law, actual attitude angle data is collected in real time by attitude sensors and compared with reference attitude angles to obtain the angle deviation. Combined with the deviation between the actual angular velocity data collected by angular velocity sensors and the reference angular velocity, an attitude proportional gain matrix (with parameters ranging from 10 to 50 for each axis) and an attitude differential gain matrix (with parameters ranging from 3 to 15 for each axis) are introduced. The angle deviation and angular velocity deviation are fused to obtain the desired angular acceleration vector, which can accurately guide attitude adjustment. This step, through the fusion calculation of determined gain parameter values ​​and deviations, constructs a stable cascade control foundation, providing reliable intermediate control commands for subsequent nonlinear mapping and control allocation, ensuring the accuracy and dynamic response performance of position and attitude control.

[0021] Step S3 completes nonlinear attitude mapping and control allocation, converting upper-level control commands into commands executable by the lower-level actuators. During the determination of the total thrust command, the desired linear acceleration vector output in step S2 is extracted, and the gravitational acceleration parameter (valued at 9.8 m / s²) is superimposed. The total thrust amplitude after deducting the gravitational influence is obtained through modulus calculation. The range of the total thrust amplitude is set to 10 to 50 N based on the UAV's mass and operational requirements. When calculating the desired attitude angle, the desired yaw angle (ranged from -180 to 180 degrees) is combined with the preset desired yaw angle. The desired roll and pitch angles are rigorously derived through the inverse operation of the rotation matrix. The range of both roll and pitch angles is controlled within -30 to 30 degrees, avoiding small-angle approximations to prevent errors. The control torque calculation adopts an inverse dynamics feedforward compensation strategy, inputting the rotational inertia matrix, the time derivative of the rotational inertia matrix, and the gyroscopic torque parameter (ranged from 0.001 to 0.01 N). (meters), to counteract the effects of rotational inertia coupling and gyroscopic effects, generating three-axis control torque commands, with each axis control torque ranging from 0.1 to 1 Newton. Meters. For multi-rotor configurations, a control efficiency matrix is ​​constructed. Matrix elements are set according to the motor mounting location and performance parameters, with values ​​ranging from 0.001 to 0.01 Newtons. The square of (revolutions per minute) is used to convert the total tension command and control torque command into the corresponding motor speed square command through matrix inversion. The value of the motor speed square ranges from 10,000 to 100,000 (revolutions per minute) square, realizing the precise allocation of control quantity and ensuring that the lower-level actuators act accurately according to the upper-level commands.

[0022] Step S4 constructs a linear extended state observer to achieve real-time and accurate estimation of the total disturbance. First, position and velocity are selected as the basic state variables. External contact forces, unmodeled dynamics, and system noise are merged into the total disturbance, extended into new state variables, and a complete state equation is constructed, ensuring that the equation comprehensively includes both known system states and unknown disturbances. The observer gain parameter is configured using the bandwidth method. The observation bandwidth is set according to the system's dynamic response requirements, ranging from 5 to 20 Hz. Using the pole placement principle and considering system stability requirements, the gain matrix of the linear extended state observer is determined. Each element of the gain matrix ranges from 10 to 100, ensuring the observer possesses characteristics of fast response and stable observation. In the data acquisition phase, the inertial measurement unit mounted on the UAV collects actual position and velocity signals in real time at a sampling frequency of 100 to 500 Hz, using these as inputs to the observer to ensure the real-time performance and accuracy of the input data. When the observer is working, it updates the state estimate in real time based on the input position and velocity signals through a preset observation algorithm. The error of the velocity estimate is controlled within 0.1 m / s, and the error of the position estimate is controlled within 0.05 m. At the same time, by extending the output of the state observer, the estimated value of the total disturbance per unit mass is obtained. The error of this estimate is controlled within 0.5 m / s². This includes comprehensive information on external contact force and system disturbance, providing accurate and reliable data support for feedforward compensation in the subsequent control law and ensuring the effective suppression of unknown disturbances by the system.

[0023] Step S5 establishes the target impedance dynamic equation, endowing the UAV with compliant interaction characteristics. The virtual mass matrix design, based on the UAV's physical interaction requirements, adopts a diagonal matrix form. The virtual mass parameters for each axis range from 1 to 10 kg. By adjusting the values ​​of each axis element, different equivalent inertial characteristics of the system are shaped to adapt to the dynamic response requirements under different interaction scenarios, such as light and heavy loads, ensuring that the UAV exhibits appropriate inertial response in various scenarios. The virtual damping matrix configuration, based on energy dissipation requirements, also adopts a diagonal matrix, with the virtual damping parameters for each axis ranging from 5 to 50 N. By setting parameters appropriately, the UAV generates a moderate damping effect during force-induced motion, controlling the oscillation decay time within 0.5 to 2 seconds to avoid continuous oscillation and ensure smooth motion. The virtual stiffness matrix is ​​set by adjusting the proportional relationship between position and force, using a diagonal matrix form. The virtual stiffness parameters for each axis range from 10 to 100 N / m, determining the restoring force characteristics of the UAV when deviating from the reference trajectory, ensuring a reasonable proportion between the restoring force and the deviation distance. Finally, based on the deviations between the desired position, velocity, acceleration, and the actual state, the designed virtual mass matrix, virtual damping matrix, virtual stiffness matrix, and deviation terms are combined with preset logic to construct a complete target impedance dynamic equation, determining the dynamic mapping relationship between position and force. This provides a foundation for the subsequent design of a control law integrating active disturbance rejection compensation, enabling the UAV to possess adjustable compliant interaction capabilities.

[0024] Step S6 integrates the impedance characteristics and active disturbance rejection compensation (ADRC) position loop control law to achieve a unified high-precision tracking and compliant interaction. First, the target impedance dynamics equation established in Step S5 is structurally adjusted by replacing the actual acceleration term with the desired acceleration command required by the control system. The order of the equation terms is rearranged to determine the relationship between the desired acceleration and position deviation, velocity deviation, acceleration deviation, and external force terms, laying the structural foundation for the subsequent introduction of compensation terms. Then, the total disturbance estimate output by the linear extended state observer in Step S4 is extracted. This estimate includes comprehensive information from external contact forces and unmodeled system dynamics. It is introduced as a feedforward compensation term with preset weights ranging from 0.8 to 1.0, achieving dynamic cancellation of external disturbances and system uncertainties, and improving the system's anti-interference capability. Algebraic analysis is performed on the equation including the compensation term. Through operations such as rearranging terms and merging like terms, external force variables that cannot be directly measured are eliminated, deriving the expression for the position loop desired linear acceleration control law integrating impedance characteristics and ADRC. This expression must completely include impedance parameters and disturbance compensation information. Finally, the matrix in the control law expression is expanded in a three-axis scalar form to determine the independent control parameters (including the specific values ​​of parameters such as position proportional, velocity derivative, virtual mass, virtual damping, and virtual stiffness) and calculation logic for each axis. The parameter values ​​for each axis are consistent with the settings in the previous text, forming an instruction output form that can be directly used for the underlying control. This ensures that the control law can be recognized and executed by the underlying control system, ultimately achieving high-precision time-varying trajectory tracking of the UAV and adjustable active yielding characteristics when facing external forces.

[0025] Preferably, the translational dynamics equations satisfy: , in, For the total mass of the drone, Let be the position and acceleration vector in the inertial frame. This represents the total tensile force amplitude. The rotation matrix from the machine system to the inertial system. Let z be the unit vector of the inertial frame z-axis. Let gravitational acceleration be a scalar. This is the external contact force vector; The reference velocity trajectory satisfies: , in, For the desired velocity vector, For the initial desired speed, The desired acceleration trajectory.

[0026] Specifically, the translational dynamics equations and the specific forms of the reference velocity trajectory provide a quantitative basis for describing the motion state and planning the trajectory of the UAV. In the translational dynamics equations, the total mass parameter of the UAV is set according to the actual model, ranging from 1 to 5 kg. This parameter directly affects the quantitative relationship between acceleration and force. The position acceleration vector in the inertial frame is calculated by collecting data from the inertial measurement unit, with the sampling frequency set to 100 to 500 Hz to ensure real-time data. The total thrust amplitude is dynamically adjusted according to the flight state, ranging from 10 to 50 N, and is obtained by subsequent control law calculation. The rotation matrix from the machine frame to the inertial frame is updated in real time through attitude angle data, used to realize the conversion of force and motion parameters in different coordinate systems. The z-axis unit vector of the inertial frame is an inherent parameter of the coordinate system, used to determine the direction of the force. The gravitational acceleration scalar is uniformly set to 9.8 m / s², ensuring accurate quantification of the influence of gravity. The external contact force vector is an unknown quantity, which is estimated later by the extended state observer. This equation fully establishes the quantitative relationship between translational acceleration, total thrust, gravity, and external contact force, providing a theoretical basis for translational control. In the reference velocity trajectory, the initial expected velocity is set according to the start state of the operation, with a value range of 0 to 3 meters per second; the expected acceleration trajectory is planned in conjunction with the requirements of the operation task, with the peak value controlled between 2 and 8 meters per square second to avoid sudden speed changes; by performing time integration on the expected acceleration trajectory and superimposing the initial expected velocity, a complete expected velocity trajectory is obtained. This trajectory is smooth and continuous, meets the dynamic performance limits of the UAV, provides a definite speed tracking target for the position loop control, and ensures the stability and accuracy of trajectory tracking.

[0027] Preferably, the basic control law of the position loop satisfies: , in, Based on expected acceleration, This is the position scaling gain matrix. The velocity differential gain matrix is... As the reference position vector, This is the actual position vector. This is the actual velocity vector; The attitude loop basic control law satisfies: , in, Based on the expected angular acceleration, This is the attitude scaling gain matrix. Here is the attitude differential gain matrix. As the reference attitude angle vector, This is the actual attitude angle vector. This is the actual angular velocity vector.

[0028] Specifically, the quantitative relationship between the basic control laws of the position loop and attitude loop is precisely corrected for deviations through the configuration of gain matrix parameters. In the basic control law of the position loop, the basic desired acceleration, as the output of the position loop, is the core basis for subsequent total tension and attitude calculations. The parameters of each axis of the position proportional gain matrix are set according to the trajectory tracking accuracy requirements, with values ​​ranging from 5 to 20. The larger the parameter, the faster the position tracking response, but excessive values ​​should be avoided to prevent system oscillations. The parameters of each axis of the velocity differential gain matrix range from 2 to 10, used to suppress velocity overshoot and improve system stability. The deviation between the reference position vector and the actual position vector is calculated from the positioning module data, with the error controlled within 0.05 meters. The deviation between the reference velocity vector and the actual velocity vector is obtained from the velocity measurement module data, with the error controlled within 0.1 meters per second. This control law obtains the basic desired acceleration by weighting and summing the position deviation and velocity deviation according to the gain parameters, and then superimposing the desired acceleration trajectory, thus achieving dual correction of position and velocity. In the attitude loop's basic control law, the basic desired angular acceleration is the core output of the attitude loop, used to derive the control torque. The parameters of each axis of the attitude proportional gain matrix range from 10 to 50, ensuring the speed of attitude angle tracking. The parameters of each axis of the attitude differential gain matrix range from 3 to 15, used to suppress attitude angular velocity overshoot. The reference attitude angle vector is calculated by the position loop, and the actual attitude angle vector is acquired by the attitude sensor, with the error controlled within 0.5 degrees. The deviation between the reference angular velocity vector and the actual angular velocity vector is calculated using angular velocity sensor data, with the error controlled within 1 degree per second. This control law obtains the basic desired angular acceleration by weighted fusion of angle deviation and angular velocity deviation, superimposed with the desired angular acceleration trajectory, providing quantitative instructions for attitude adjustment and ensuring the accuracy of attitude tracking and dynamic response performance.

[0029] Preferably, the total tension command in the nonlinear attitude mapping satisfies: , in, For vector magnitude operations; Control torque distribution satisfies: , in, To control the torque vector, Here is the rotational inertia matrix. The time derivative of the moment of inertia matrix. This is the gyroscopic torque term; The command for the square of the motor speed satisfies: , in, This is the motor speed vector. To control the efficiency matrix, It is a vector formed by the squares of the rotational speeds.

[0030] Specifically, the quantitative calculation logic for nonlinear attitude mapping and control allocation enables precise conversion from upper-level control commands to lower-level execution quantities. In the calculation of the total pull command, the basic expected acceleration vector output from the position loop is extracted, superimposed with the gravitational acceleration vector, and then subjected to magnitude calculation. This calculation process eliminates the influence of gravity on the total pull, ensuring that the total pull only reflects translational control requirements. The total pull amplitude ranges from 10 to 50 Newtons, matching the UAV's mass and acceleration requirements. In the control torque allocation, the moment of inertia matrix is ​​pre-calibrated based on the UAV's structural parameters, with values ​​for each axis ranging from 0.01 to 0.1 kg. The square meter value, whose time derivative is calculated using the rate of change of attitude angle, is used to compensate for the coupling effect of rotational inertia; the gyroscopic torque term ranges from 0.001 to 0.01 Newtons. The torque term, measured in Newtons (N), is used to counteract the inter-axis coupling effects during high-speed rotation of the machine body. In practical applications, this torque term can be retained or not depending on the system's accuracy requirements. Through the fusion calculation of the above parameters with the basic desired angular acceleration and actual angular velocity, the three-axis control torque command is obtained, with each axis ranging from 0.1 to 1 N. Meters. In the calculation of the square command for motor speed, for multi-rotor layouts, the control efficiency matrix is ​​preset according to the motor installation position and performance parameters, and the matrix element values ​​range from 0.001 to 0.01 Newtons. The matrix (revolutions per minute) squared reflects the quantitative relationship between motor speed and pulling force and torque. By inverting the control efficiency matrix, the total pulling force command and the three-axis control torque command are converted into the corresponding motor speed squared command. The speed squared value ranges from 10,000 to 100,000 (revolutions per minute) squared, ensuring that the motor output is accurately matched with the upper-level control requirements and realizing the efficient allocation of control quantities.

[0031] Preferably, the state equation of the extended state observer satisfies: , in, This is a location estimate. This is a speed estimate. To control the gain coefficient, To control the input, For observer gain, This is a location measurement value; The total disturbance estimate satisfies: , in, This is the estimated total disturbance per unit mass. To extend the state estimate, To extend the state observer gain.

[0032] Specifically, a quantitative model of a linear extended state observer is constructed to provide technical support for real-time estimation of the total disturbance. In the state equation, the position estimate and velocity estimate are the core observation variables, updated in real time through the observer algorithm. The position estimate error is controlled within 0.05 meters, and the velocity estimate error is controlled within 0.1 meters per second. The control gain coefficient is set according to the dynamic characteristics of the system, with a value range of 0.1 to 1.0, and is used to adjust the degree of influence of the control input on the state variables. The control input is the basic expected acceleration output of the position loop, ensuring the linkage between the observer and the control system. The observer gain is configured using the bandwidth method, with the observation bandwidth ranging from 5 to 20 Hz, set according to the system response speed requirements. The observer gain matrix, determined by the pole placement principle, has each element ranging from 10 to 100, ensuring that the observer has the characteristics of fast convergence and stable observation. The position measurement value is acquired in real time through the positioning module, with a sampling frequency of 100 to 500 Hz, providing accurate input data for the observer. This state equation completely describes the dynamic working process of the observer, realizing accurate estimation of position and velocity. In the total disturbance estimation, the extended state estimate is a new state variable added to the observer, specifically used to capture unmodeled dynamics of the system and external disturbances, with its error controlled within 0.5 meters per second squared. The extended state observer gain is configured in coordination with the observer gain mentioned above to ensure the accuracy of the total disturbance estimation. By calculating the extended state estimate, the position estimation error, and the control gain coefficient, the total disturbance estimate per unit mass is obtained. This estimate includes comprehensive information on external contact forces and system disturbances, providing accurate quantitative data for feedforward compensation in the subsequent control law, enabling the system to have active disturbance rejection capability.

[0033] Preferably, the target impedance dynamic equation satisfies: , in, For virtual mass matrix, For virtual damping matrix, This is a virtual stiffness matrix; The control law for the integrated impedance characteristics satisfies: , in, The final desired linear acceleration vector, It is the inverse of the virtual mass matrix.

[0034] Specifically, the quantitative relationship between the target impedance dynamic equation and the control law of the fusion impedance characteristic is established to achieve the unification of trajectory tracking and compliant interaction. In the target impedance dynamic equation, the virtual mass matrix adopts a diagonal matrix form, with the parameters of each axis ranging from 1 to 10 kg. By adjusting the parameters, the equivalent inertia of the system is shaped to adapt to different interaction scenarios. The virtual damping matrix is ​​also a diagonal matrix, with the parameters of each axis ranging from 5 to 50 N. The value per second per meter is used to dissipate energy during the interaction process and avoid system oscillation. The oscillation decay time is controlled within 0.5 to 2 seconds. The virtual stiffness matrix is ​​a diagonal matrix, and the values ​​of the parameters of each axis range from 10 to 100 N / m to determine the quantitative relationship between position deviation and restoring force. The expected and actual position, velocity, and acceleration deviations are calculated by comparing sensor data with the reference trajectory. The external contact force vector is an unknown input during the system interaction process. This equation establishes a dynamic mapping between position deviation, velocity deviation, acceleration deviation, and external contact force, providing a theoretical basis for compliant control. In the control law that integrates impedance characteristics, the final desired linear acceleration vector is the final output command of the position loop, directly guiding the underlying control. The inverse of the virtual mass matrix is ​​obtained through matrix inversion, ensuring the reverse adjustment logic of the impedance parameters. By summing the products of the virtual damping matrix and the velocity deviation, and the virtual stiffness matrix and the position deviation, and then subtracting the product of the total disturbance estimate and the total mass of the UAV, the impedance compensation term is obtained. The weight coefficient of this compensation term ranges from 0.8 to 1.0. After being superimposed with the desired acceleration trajectory, a complete control law expression is formed. This control law deeply integrates active disturbance rejection compensation and impedance characteristics, ensuring high accuracy of trajectory tracking and giving the system adjustable compliant interaction capabilities, thus achieving a quantitative balance between the two control requirements.

[0035] Preferred, such as Figure 2 As shown, step S3 includes the following sub-steps: S31, in the calculation of total tension command, the desired linear acceleration vector output by the basic control law is extracted, and after superimposing the gravitational acceleration vector, the magnitude is calculated to obtain the total tension amplitude after deducting the influence of gravity. S32, combined with the preset desired yaw angle, solves the attitude angle relationship through the inverse operation of the rotation matrix, and rigorously derives the desired roll angle and pitch angle, eliminating the error caused by small angle approximation; S33, based on attitude dynamics equations, adopts an inverse dynamics feedforward compensation strategy to counteract the effects of rotational inertia coupling and gyro effect, and generates three-axis control torque commands that meet attitude tracking requirements. S34, for multi-rotor configuration, constructs a control efficiency matrix, and converts the total thrust command and control torque command into the corresponding motor speed square command through matrix inversion operation, and then allocates the control quantity.

[0036] Specifically, the nonlinear attitude mapping and control allocation process in step S3 achieves precise conversion from upper-level control commands to lower-level execution quantities through four sub-steps. In step S31, during the calculation of the total thrust command, the basic desired linear acceleration vector output in step S2 is first extracted. This vector is obtained through a weighted calculation of position error and velocity deviation. After superimposing the gravitational acceleration parameter (valued at 9.8 m / s²), a vector magnitude calculation method is used to eliminate the interference of gravity on the total thrust, ultimately obtaining the total thrust amplitude. Its value range is set to 10 to 50 N based on the UAV's mass and operational requirements, ensuring that the total thrust only responds to translational control needs. In step S32, during the calculation of the desired attitude angle, combined with the preset desired yaw angle (value ranged from -180 to 180 degrees), the desired roll angle and pitch angle are obtained through a rigorous derivation process using the inverse operation of the rotation matrix. Both values ​​are controlled within the range of -30 to 30 degrees, completely eliminating errors caused by small-angle approximations and ensuring the accuracy of the attitude angle calculation. In generating the control torque, S33 employs an inverse dynamics feedforward compensation strategy based on the attitude dynamics equations, inputting a pre-calibrated rotational inertia matrix (with values ​​ranging from 0.01 to 0.1 kg for each axis). (square meters), the time derivative of the moment of inertia matrix, and the gyroscopic torque term (ranging from 0.001 to 0.01 Newtons). (meters), effectively counteracting the effects of rotational inertia coupling and gyroscopic effects, generating three-axis control torque commands, with values ​​ranging from 0.1 to 1 Newton for each axis. For multi-rotor configurations, the S34 first constructs a control efficiency matrix. The matrix elements are set according to the motor mounting location and performance parameters, with values ​​ranging from 0.001 to 0.01 Newtons. The square of (revolutions per minute) is then used to convert the total tension command and the three-axis control torque command into the corresponding motor speed square command through matrix inversion. The speed square value ranges from 10,000 to 100,000 (revolutions per minute) square, realizing the precise allocation of control quantity in the bottom-level actuator and ensuring a high degree of matching between motor output and upper-level control requirements.

[0037] Preferred, such as Figure 3 As shown, step S4 includes the following sub-steps: S41. Position and velocity are selected as the basic state variables. External contact force, unmodeled dynamics and system noise are merged into the total disturbance and extended into new state variables to construct a complete state equation. S42 uses the bandwidth method to configure the observer gain parameters, sets the observation bandwidth according to the system dynamic response requirements, and determines the gain matrix of the linear extended state observer through the pole placement principle. S43: Collect the actual position and velocity signals output by the UAV's inertial measurement unit as input to the observer and update the state estimate in real time; S44, by extending the output of the state observer, obtains the total disturbance estimate per unit mass, which includes comprehensive information on external contact force and system disturbance, providing data support for subsequent compensation.

[0038] Specifically, the linear extended state observer construction and disturbance estimation process in step S4 achieves real-time and accurate capture of the total disturbance through four sub-steps. In step S41, when constructing the state equation, position and velocity are selected as the basic state variables. The position variable is acquired through a positioning module with an error controlled within 0.05 meters, and the velocity variable is acquired through a velocity measurement module with an error controlled within 0.1 meters per second. Simultaneously, external contact force, unmodeled dynamics, and system noise are merged into the total disturbance, expanded into new state variables, and form a complete state equation that comprehensively includes both known system states and unknown disturbance factors. In step S42, in the observer gain configuration, the bandwidth method is used to set the observation bandwidth, with a value range of 5 to 20 Hz. Combined with system stability requirements, the gain matrix of the linear extended state observer is determined using the pole placement principle, with each element ranging from 10 to 100, ensuring the observer possesses both fast response and stable observation characteristics. In data acquisition and state update, S43 utilizes the inertial measurement unit onboard the UAV to collect real-time position and velocity signals at a sampling frequency of 100 to 500 Hz. These signals are used as input to the observer, and the position and velocity estimates are continuously updated based on a preset observation algorithm, ensuring the real-time performance and accuracy of the state estimation. In total disturbance separation, S44 extends the output of the state observer to separate the estimated total disturbance per unit mass. This estimate has an error controlled within 0.5 meters per second squared, comprehensively including information on external contact forces and system disturbances. This provides accurate and reliable data support for feedforward compensation in subsequent control laws, enhancing the system's anti-interference capability.

[0039] Preferred, such as Figure 4 As shown, step S5 includes the following sub-steps: S51, based on the physical interaction requirements of UAVs, designs a virtual mass matrix. By adjusting the values ​​of the diagonal elements of the matrix, it shapes the equivalent inertial characteristics of the system and adapts to the dynamic response under different interaction scenarios. S52 is equipped with a virtual damping matrix. The matrix parameters are set according to the energy dissipation requirements to enable the UAV to generate a moderate damping effect during force-driven motion and avoid oscillation. S53, set the virtual stiffness matrix, and determine the restoring force characteristics of the UAV when it deviates from the reference trajectory by adjusting the proportional relationship parameters between position and force; S54, based on the deviation between the desired position, velocity, acceleration and the actual state, combines the virtual mass matrix, virtual damping matrix, virtual stiffness matrix and deviation term to construct a complete target impedance dynamic equation and determine the dynamic mapping relationship between position and force.

[0040] Specifically, the target impedance dynamic equation construction process in step S5 endows the UAV with adjustable compliant interaction characteristics through four sub-steps. In S51, the virtual mass matrix design adopts a diagonal matrix structure based on the physical interaction scenario requirements of the UAV. The virtual mass parameters for each axis range from 1 to 10 kg. By adjusting the parameter values ​​of different axes, differentiated equivalent inertial characteristics of the system are shaped to adapt to the dynamic response requirements of different operating scenarios such as light and heavy loads, ensuring that the UAV can exhibit appropriate inertial performance in various interaction scenarios. In S52, the virtual damping matrix configuration also adopts a diagonal matrix form, with the virtual damping parameters for each axis ranging from 5 to 50 N. The parameters are precisely set according to energy dissipation requirements to generate a moderate damping effect during the UAV's force-driven motion, controlling the oscillation decay time within 0.5 to 2 seconds, effectively avoiding continuous oscillations and ensuring motion stability. In the virtual stiffness matrix setting (S53), the proportional relationship parameters between position and force are adjusted using a diagonal matrix structure, with virtual stiffness parameters for each axis ranging from 10 to 100 N / m. This determines the restoring force characteristics of the UAV when deviating from the reference trajectory, ensuring a reasonable linear correlation between the magnitude of the restoring force and the deviation distance. In the impedance equation construction (S54), based on the deviation data between the desired position, velocity, acceleration, and actual state, the designed virtual mass matrix, virtual damping matrix, virtual stiffness matrix, and deviation terms are fused and calculated according to preset logic to construct a complete target impedance dynamic equation. This clearly defines the dynamic mapping relationship between position and force, laying the foundation for the subsequent design of the control law integrating active disturbance rejection compensation.

[0041] Preferred, such as Figure 5 As shown, step S6 includes the following sub-steps: S61 replaces the actual acceleration term in the target impedance dynamic equation with the desired acceleration command required by the control system, reorganizes the equation structure, and highlights the relationship between the desired acceleration and the deviation and external force terms. S62, extract the total disturbance estimate from the extended state observer output, and introduce it as a feedforward compensation term into the rearranged equation to dynamically cancel the uncertainty between external disturbances and the system. S63, perform algebraic analysis on the equation including the compensation term, eliminate the external force variables, and derive the expression for the position loop desired linear acceleration control law that integrates impedance characteristics and self-disturbance compensation; S64 expands the matrix in the control law expression into a three-axis scalar form, determines the independent control parameters and calculation logic for each axis, and forms an instruction output form that can be directly used for low-level control.

[0042] Specifically, step S6 achieves deep integration of active disturbance rejection compensation and impedance characteristics through four sub-steps. In S61, during equation structure adjustment, the target impedance dynamic equation established in step S5 is first obtained. The actual acceleration term in the equation is replaced with the desired acceleration command required by the control system. The order of the equation terms is rearranged, and the mathematical relationship between the desired acceleration and position deviation, velocity deviation, acceleration deviation, and external force terms is determined, thus building a reasonable equation structure for the subsequent introduction of disturbance compensation terms. In S62, during the introduction of compensation terms, the total disturbance estimate output by the linear extended state observer in step S4 is extracted. This estimate includes comprehensive information on external contact force and unmodeled system dynamics. It is used as a feedforward compensation term and introduced into the adjusted equation with a preset weight (ranging from 0.8 to 1.0) to achieve dynamic cancellation of external disturbances and system uncertainties, thereby enhancing the system's disturbance rejection capability. In the control law analysis in S63, a systematic algebraic analytical operation is performed on the equations including compensation terms. Through operations such as rearranging terms and merging like terms, external force variables that cannot be directly measured are eliminated. Finally, the position loop desired linear acceleration control law expression that integrates impedance characteristics and active disturbance rejection compensation is derived. This expression completely includes impedance parameters and disturbance compensation information. In the control law expansion in S64, the matrix in the control law expression is decomposed into a three-axis scalar form to determine the independent control parameters (including position proportional, velocity derivative, virtual mass, virtual damping, virtual stiffness, etc.) and calculation logic for each axis. The parameter values ​​for each axis are consistent with the settings in the previous text, forming a command output form that can be directly recognized and executed by the underlying control system. Ultimately, this achieves the unification of high-precision trajectory tracking and compliant interaction characteristics of the UAV.

[0043] A physical interactive impedance control method for rotary-wing unmanned aerial vehicles (UAVs) based on active disturbance rejection (ADRROR) addresses the problem of traditional impedance control relying on physical force sensors. By extending the state observer, it estimates external contact forces and system disturbances in real time from the UAV's own motion state, eliminating the need for additional force sensing equipment. This reduces the aircraft's mass and manufacturing cost, avoids interference from measurement noise on the system's dynamic response, and improves payload and operational flexibility. For the underactuated and strongly coupled system characteristics of multi-rotor UAVs, this method deeply integrates disturbance estimation with impedance parameters. Through the coordination of cascade control structures, nonlinear attitude mapping, and precise control allocation, it achieves efficient connection between the control law and the underlying actuators, solving the technical challenge of existing methods that struggle to balance trajectory tracking accuracy and compliant interaction.

[0044] This method provides reliable support for physical contact operations using unmanned aerial vehicles (UAVs). By constructing a target impedance model that includes virtual mass, damping, and stiffness matrices, it reshapes the system's equivalent inertia and feedback stiffness, enabling the aircraft to exhibit adjustable active yielding characteristics when facing external forces. This transforms the "resistive" response of traditional rigid control into physical-level compliance, significantly improving interaction safety and effectively avoiding the risk of system instability or aircraft damage. Simultaneously, through the deep integration of dynamic modeling, real-time disturbance estimation, and feedforward compensation, it ensures that the UAV maintains high-precision time-varying trajectory tracking capabilities even in complex and unknown environments. This achieves dual optimization of compliant interaction and trajectory tracking, providing a more adaptable and reliable control solution for contact-based operation scenarios such as industrial inspection and material delivery.

[0045] In the description of this invention, it should be noted that, unless otherwise specified and limited, the terms "set," "install," "connect," "link," and "fix" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0046] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various equivalent changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for controlling the physical interactive impedance of a rotary-wing unmanned aerial vehicle based on active disturbance rejection, characterized in that, Includes the following steps: S1. Establish a dynamic model that separates translation and rotation of a multi-rotor UAV. Construct translational dynamic equations in the inertial coordinate system based on the laws of motion, and establish rotational dynamic equations in the body coordinate system based on Euler's equations. At the same time, set the reference position trajectory and reference velocity trajectory. S2 is the basic control law for designing the cascade control structure. The position loop converts the position error and velocity deviation into the desired linear acceleration vector, and the attitude loop obtains the desired angular acceleration vector through the angle deviation and angular velocity deviation. S3 performs nonlinear attitude mapping and control allocation, determines the total tension command after deducting the influence of gravity, and calculates the desired roll angle and pitch angle. The control torque is obtained based on inverse dynamics feedforward compensation, and the motor speed square command is output by inverting the control efficiency matrix. S4, construct a linear extended state observer, classify unmodeled dynamics and external forces as total disturbance, and estimate velocity and total disturbance per unit mass in real time through state equations; S5. Based on the expected and actual errors, establish the target impedance dynamic equation including the virtual mass matrix, virtual damping matrix and virtual stiffness matrix; S6 replaces the actual acceleration in the target impedance dynamic equation with the desired acceleration command, performs feedforward compensation for the estimated disturbance of the extended state observer, and analyzes the position loop desired line acceleration vector control law of the fused impedance characteristics.

2. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 1, characterized in that, The translational dynamics equations satisfy: , in, The total mass of the drone, Let be the position and acceleration vector in the inertial frame. This represents the total tensile force amplitude. The rotation matrix from the machine system to the inertial system. Let z be the unit vector of the inertial frame z-axis. Let gravitational acceleration be a scalar. This is the external contact force vector; The reference velocity trajectory satisfies: , in, For the desired velocity vector, For the initial desired speed, The desired acceleration trajectory.

3. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 2, characterized in that, The basic control law of the position loop satisfies: , in, Based on expected acceleration, This is the position scaling gain matrix. Here is the velocity differential gain matrix. As the reference position vector, This is the actual position vector. This is the actual velocity vector; The attitude loop basic control law satisfies: , in, Based on the expected angular acceleration, This is the attitude scaling gain matrix. Here is the attitude differential gain matrix. As the reference attitude angle vector, This is the actual attitude angle vector. This is the actual angular velocity vector.

4. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 3, characterized in that, The total tension command in nonlinear attitude mapping satisfies: , in, For vector magnitude operations; Control torque distribution satisfies: , in, To control the torque vector, Here is the rotational inertia matrix. The time derivative of the moment of inertia matrix. This is the gyroscopic torque term; The command for the square of the motor speed satisfies: , in, This is the motor speed vector. To control the efficiency matrix, It is a vector formed by the squares of the rotational speeds.

5. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 4, characterized in that, The state equation of the extended state observer satisfies: , in, This is a location estimate. This is a speed estimate. To control the gain coefficient, To control the input, For observer gain, This is a location measurement value; The total disturbance estimate satisfies: , in, This is the estimated total disturbance per unit mass. To extend the state estimate, To extend the state observer gain.

6. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 5, characterized in that, The target impedance dynamic equation satisfies: ， in, For virtual mass matrix, For virtual damping matrix, This is a virtual stiffness matrix; The control law for the integrated impedance characteristics satisfies: ， in, The final desired linear acceleration vector, It is the inverse of the virtual mass matrix.

7. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 6, characterized in that, S3 includes the following steps: S31, in the calculation of total tension command, the desired linear acceleration vector output by the basic control law is extracted, and after superimposing the gravitational acceleration vector, the magnitude is calculated to obtain the total tension amplitude after deducting the influence of gravity. S32, combined with the preset desired yaw angle, solves the attitude angle relationship through the inverse operation of the rotation matrix, and derives the desired roll angle and pitch angle, thus eliminating the error caused by small angle approximation; S33, based on attitude dynamics equations, adopts an inverse dynamics feedforward compensation strategy to counteract the effects of rotational inertia coupling and gyro effect, and generates three-axis control torque commands that meet attitude tracking requirements. S34, for multi-rotor configuration, constructs a control efficiency matrix, and converts the total thrust command and control torque command into the corresponding motor speed square command through matrix inversion operation, and then allocates the control quantity.

8. The physical interactive impedance control method for a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 7, characterized in that, S4 includes the following sub-steps: S41. Position and velocity are selected as the basic state variables. External contact force, unmodeled dynamics and system noise are merged into the total disturbance and extended into new state variables to construct a complete state equation. S42 uses the bandwidth method to configure the observer gain parameters, sets the observation bandwidth according to the system dynamic response requirements, and determines the gain matrix of the linear extended state observer through the pole placement principle. S43: Collect the actual position and velocity signals output by the UAV's inertial measurement unit as input to the observer and update the state estimate in real time; S44, by extending the output of the state observer, obtains the total disturbance estimate per unit mass, which includes comprehensive information on external contact force and system disturbance, providing data support for subsequent compensation.

9. A method for controlling the physical interactive impedance of a rotary-wing unmanned aerial vehicle based on self-disturbance rejection as described in claim 8, characterized in that, S5 includes the following steps: S51, based on the physical interaction requirements of UAVs, designs a virtual mass matrix. By adjusting the values ​​of the diagonal elements of the matrix, it shapes the equivalent inertial characteristics of the system and adapts to the dynamic response under different interaction scenarios. S52 is equipped with a virtual damping matrix. The matrix parameters are set according to the energy dissipation requirements to enable the UAV to generate a moderate damping effect during force-driven motion and avoid oscillation. S53, set the virtual stiffness matrix, and determine the restoring force characteristics of the UAV when it deviates from the reference trajectory by adjusting the proportional relationship parameters between position and force; S54, based on the deviation between the desired position, velocity, acceleration and the actual state, combines the virtual mass matrix, virtual damping matrix, virtual stiffness matrix and deviation term to construct a complete target impedance dynamic equation and determine the dynamic mapping relationship between position and force.

10. A method for controlling the physical interactive impedance of a rotary-wing unmanned aerial vehicle based on active disturbance rejection as described in claim 9, characterized in that, S6 includes the following sub-steps: S61 replaces the actual acceleration term in the target impedance dynamic equation with the desired acceleration command required by the control system, reorganizes the equation structure, and highlights the relationship between the desired acceleration and the deviation and external force terms; S62, extract the total disturbance estimate from the extended state observer output, and introduce it as a feedforward compensation term into the simplified equation to dynamically cancel the uncertainty of external disturbances and the system. S63, perform algebraic analysis on the equation including the compensation term, eliminate the external force variables, and derive the expression for the position loop desired linear acceleration control law that integrates impedance characteristics and self-disturbance compensation; S64 expands the matrix in the control law expression into a three-axis scalar form, determines the independent control parameters and calculation logic for each axis, and forms an instruction output form that can be directly used for low-level control.

Images