Flight operation robot compliant sampling method based on energy tank

Abstract

The invention provides a flight operation robot compliant sampling method based on an energy tank. Comprising the following steps: S1, taking a position error of a flying operation robot and a measured external force as input of an admittance model, and dividing parameters to design a variable admittance controller; s2, designing self-adaptive rates of inertia and stiffness parameters of the variable admittance controller, and calculating a damping parameter control rate based on a specified performance controller; s3, designing passivity parameters of a variable admittance controller by combining a given energy tank rule and changes of inertia, rigidity and damping parameters in the admittance controller; s4, designing a variable boundary energy tank, providing a boundary change adaptive rate and an energy flow exchange rule, and realizing safety supervision of the sampling process of the flying operation robot; the flight operation robot compliant sampling method is good in performance, the flight operation robot can complete compliant interaction and sampling operation on the solid-liquid mixed environment, and task requirements can be effectively met.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned aerial vehicles, in particular to a flexible sampling method of a flying operation robot based on an energy tank. Background Art

[0002] In recent years, the ability of flying robots to quickly reach high-altitude working areas and conduct physical interactions has been continuously enhanced. This has led to their widespread adoption in various applications, including inspection and maintenance tasks. The use of flying robots significantly reduces personnel time, cost, and safety risks, while improving the efficiency of task execution. The flexibility of the manipulator enables flying robots to actively perform complex tasks or respond to environmental interactions during tasks. However, this flexibility also introduces the risk of unstable physical interactions, including unstable contacts, insufficient response speed to task parameters during interaction, and insufficient compliance in interactive behaviors. At the same time, the complexity and uncertain dynamics of objects in solid-liquid mixed environments such as swamps and mudflats make it difficult for flying robots to successfully perform interaction and sampling tasks. Therefore, it is necessary to design an interactive sampling control method with adjustable compliance and safety to improve the stability and safety of flying robots when interacting with the environment.

[0003] Many scholars have made various attempts and proposed a variety of methods including admittance control, impedance control, force-position hybrid control, etc. to achieve stable contact and interactive compliance. In the process of compliant physical interaction with the environment, the behavior of the flying robot is affected by different admittance parameters in the admittance controller, which can be adjusted as needed. The critical damping state refers to the system reaching the desired position without oscillation in the shortest time. When adjusting the admittance parameters, the underdamped and overdamped states caused by deviation from the critical damping state may have an adverse effect on the sampling performance of the flying robot. Underdamped conditions may cause jitter or oscillation in the flying robot. Overdamped conditions may reduce the response speed of the flying robot and cause tracking errors. Whether the response performance to changes in admittance parameters during physical interaction can be enhanced by accurately adjusting the damping state of the flying robot is a direction worth studying. Summary of the invention

[0004] In recent years, passivity-based methods such as port Hamiltonian systems, energy tanks, and damping distribution passivity control have been extended from the network field to the robotics field. The flying operation robot and the environment can be regarded as independent modules that interact through energy ports, and the robust stability of the interaction behavior can be ensured by applying the passivity standard. In compliant interaction, the compliance of the commonly used admittance controller is fixed, and adjusting the admittance parameters to achieve adjustable compliance is a non-passive operation. Consider decoupling the passivity-based control from the admittance controller by properly interconnecting the energy tank with the variable admittance controller, so that the flying operation robot can maintain passivity while adjusting the compliance.

[0005] In practical applications, performing typical flying robot tasks usually requires pre-defining an appropriate amount of energy in the energy tank. If the pre-defined energy is too low, the available energy will be exhausted during the task, causing the flying robot to repeatedly switch control modes, resulting in flutter and increased actuator wear. If the pre-defined energy is too high, the flying robot may extract a large amount of energy from the energy tank in a short period of time, which may cause it to perform dangerous sampling behaviors. Therefore, it is a worthy research direction to safely regulate the energy in the energy tank during the compliant interaction of the flying robot with the environment.

[0006] The purpose of the present invention is to provide a flexible sampling method for a flying operation robot based on an energy tank, which can improve the response speed of the admittance parameter, realize the adjustment of the flexibility during the interaction process and the safe supervision of the interaction power, thereby realizing the flexible sampling of the flying operation robot. The present invention adopts the following technical solutions.

[0007] The present invention proposes a mobile robot motion planning method for unknown target collection, in which the mobile robot can quickly complete the exploration of the unknown environment and collect targets in the task area, and can effectively meet the task requirements. The present invention adopts the following technical solutions.

[0008] A flexible sampling method for a flying operation robot based on an energy tank comprises the following steps:

[0009] Step S1: The position error of the flight operation robot and the measured external force h f As the input of the admittance model, the parameters are divided to design the variable admittance controller;

[0010] Step S2, designing the adaptive rate of the inertia and stiffness parameters of the variable admittance controller, and calculating the damping parameter control rate based on the specified performance controller;

[0011] Step S3, combining the given energy tank rule with the changes of inertia, stiffness and damping parameters in the admittance controller, designing the passive parameters of the variable admittance controller;

[0012] Step S4, designing a variable boundary energy tank, proposing boundary change adaptation rate and energy flow exchange rules, and realizing safe supervision of the sampling process of the flying operation robot;

[0013] The variable admittance controller parameter division method of step S1 specifically comprises the following steps:

[0014] Step S11: Define the input position error as in, and represent the desired position and command position of the end effector respectively. is the end effector position matrix, x e ,y e 、z e are the positions along the x, y, and z directions in Cartesian space, respectively.

[0015] Step S12: During the physical interaction between the flying robot and the environment, the dynamics of the end effector can be described by an admittance model:

[0016]

[0017] in, is the force exerted by the environment on the end effector of the flying robot, M e ,D e and K e are the desired symmetric positive definite inertia, damping, and stiffness matrices.

[0018] Step S13: First, decompose the inertia, damping and stiffness matrices in the admittance model into:

[0019]

[0020] Among them, M c , D c , K c is a positive diagonal matrix, M v , D v , K v is a diagonal matrix with internal elements as variables. Using formula 1 and formula 2, the following variable admittance controller can be obtained:

[0021]

[0022] The damping parameter regulation performance control method of step S2 specifically comprises the following steps:

[0023] Step S21: The critical damping state means that the system reaches the desired position without oscillation in the shortest time. When the variable admittance controller is in this state, for the convenience of calculation, square both sides to get D e 2 (t)=4M e (t)K e (t). Then, the critical damping error and auxiliary position error are defined as follows:

[0024]

[0025] Where K a and are all constant symmetric matrices, e(t)=diag(e x ,e y ,e z ) is the critical damping error, is the auxiliary position error.

[0026] Step S22: The changes of inertia parameter, stiffness parameter and damping parameter in formula 3 can be expressed as

[0027]

[0028] in

[0029]

[0030]

[0031] where exp(·) represents the exponential function with base e, κ i is a positive constant such that when hour, The value of is established, represents the maximum allowable position error determined by the mission parameters. The superscript i = {x, y, z} represents the component on the corresponding axis, and They are all normal numbers.

[0032] Step S23: The critical damping controller is designed to enable each error e(t) to converge quickly to a preset region. Generally, the specified performance can be expressed as follows:

[0033] -δ l ρ i (t)<e i (t)<δ r ρ i (t) (Formula 7)

[0034] where e i is the i-th element of the critical damping error e(t), i = {x, y, z}, δ l and δ ris a positive constant, ρ(t) is a performance function, expressed as

[0035]

[0036] in and l are strictly positive constants satisfying Notice The rate of decrease is affected by the constant l. Then, the error conversion is used to convert the error e i Converted to the equivalent unconstrained error μ i The conversion process is determined by the function T(μ i ) control, defined as

[0037] e i =T(μ i )ρ i (Formula 9)

[0038] T(μ i ) is strictly increasing, its inverse function always exists and satisfies -δ l <T(μ i )<δ r . Calculate T(μ i ) is as follows

[0039]

[0040] Among them, μ i is the i-th element of the conversion error μ, expressed as In addition, the first-order derivative of the transformation error μ can be derived as follows

[0041]

[0042] in

[0043] P = diag (ρ x ,ρ y ,ρ z ),

[0044] r i (t) = (δ r +δ l ) / ((δ l ρ i (t)+e i )(δ r ρ i (t)-e i )), the superscript i = {x, y, z} represents the component on the corresponding coordinate axis.

[0045] Step S24: By selecting the controller gain The following critical damping controller can be used to drive the critical damping error e to converge to 0 and ensure the performance specified in Equation 7:

[0046]

[0047] Therefore, the inertia, stiffness and damping parameters in formula 3 are adaptively adjusted according to the changes in the position error and the critical damping error function e(t). A balance between stiffness and position error can be achieved. Under the action of , the system always moves towards the critical damping state.

[0048] The energy tank-based passivity method of step S3 specifically includes the following steps:

[0049] Step S31: Express the total energy of the variable admittance system with a natural non-negative energy storage function:

[0050]

[0051] The passivity condition requires that no internal energy is generated, so the following conditions must be met:

[0052]

[0053] Step S32: Calculate the time derivative of the energy storage function V(t), we get:

[0054]

[0055] In energy conservation and passivity analysis, all terms that may generate energy are often combined into one expression, and the dissipative terms are separated. where Ω includes all terms that may generate energy, such as those due to changes in the stiffness and inertia of the variable admittance controller. The dissipative term is then expressed as The dissipation term is always non-negative, since it represents the energy lost due to damping. However, due to changes in stiffness and inertia, p can become positive, which means that the controller may generate energy, violating the equilibrium condition in Equation 14.

[0056] Step S33: In order to maintain the passivity of the system, it is necessary to ensure that p does not cause the system to generate net energy. By interconnecting the variable admittance controller formula three with the energy tank, it is ensured that the system does not generate energy by itself and always maintains passivity. The energy tank is a virtual storage that allows the accumulation of dissipated energy for future use, usually expressed as:

[0057]

[0058] in A gain of 0 < η < 1 is used to control how much of the dissipated power is stored in the tank. It is the state of the tank. is the input-output pair that processes the tank power flow.

[0059] The energy stored in the energy tank is:

[0060]

[0061] The initial state of the tank is set to x t (0) to ensure that T(x t (0))≥ò, where ò is the threshold to avoid the singularity in Equation 16 due to energy depletion.

[0062] Step S34: Power flow of the tank Calculated by formula 16 and formula 17:

[0063]

[0064] Where θ1 is used to keep the energy in the tank within a predefined range [T - ,T + ], and T - >ò, its design is as follows:

[0065]

[0066] Where T - and T + They are the lower and upper limits of the energy tank respectively.

[0067] Step S35: The above definition means that if T(x t )∈[T - ,T + ] may be violated, then the energy connection between the tank and the variable admittance system will be closed. Having described the energy storage in the tank, the focus now shifts to utilizing this stored energy to achieve variable admittance behavior. From Equation 15, it is observed that only the variables inertia and stiffness contribute energy to the variable admittance controller, which requires a conditional restriction on their variation:

[0068]

[0069] The conditional variable θ2 is designed as:

[0070]

[0071] From the above conditions, it can be seen that if the energy dissipation is positive (ηd+p>0), the variable inertia and stiffness will fully work. Otherwise, the variable inertia and stiffness depend on θ1.

[0072] Step S36: This means that if the tank dynamics of the variable admittance controller formula 20 The tank parameter θ1 and conditional variable θ2 are designed as Formula 18, Formula 19 and Formula 21 respectively. Then the controller has the following input-output pair: Aspects are passive.

[0073] The variable boundary energy tank method for energy flow monitoring in step S4 specifically comprises the following steps:

[0074] Step S41: After the energy tank is connected to the variable admittance controller, the change of energy flow in the tank will affect the admittance parameter. Monitoring the energy flow helps to understand and mitigate the potential dangerous behavior of the flying operation robot. According to Formula 18, the maximum rate of energy change in the energy tank is defined as

[0075]

[0076] in, and Represent the maximum allowable position and velocity errors determined by the mission parameters, respectively, and are defined as follows

[0077]

[0078] where k r and k p is a constant symmetric matrix. t>t δ and t δ >0 represents the current time and the fixed time interval of the task. δ , the error of the expected task trajectory within a certain time range can be determined. is the maximum expected speed of the task.

[0079] Step S42: The safe energy condition of the flight operation robot can be understood through Formula 22, and the energy in the tank can be adjusted accordingly. Next, the rate of change of the lower limit of the energy tank can be defined as

[0080]

[0081] Step S43: Compared with the constant parameter admittance control, the change of inertia and stiffness changes the kinetic energy, and the upper limit T of the energy tank needs to be adjusted + This upper limit is defined by combining the current lower limit with the maximum kinetic energy that can be assigned to the current admittance parameter:

[0082]

[0083] Step S44: According to step S36, it can be inferred that as long as the energy level in the tank remains at its lower bound T - Above, even if T + and T - The passivity of Formula 20 is also maintained over time. Therefore, during the sampling process, if it is realized that the safety of the flying robot is threatened, the mission will be interrupted; otherwise, the mission will continue.

[0084] In summary, a flying operation robot based on an energy tank adopts the above method; it is characterized in that the application condition of the variable admittance controller based on the energy tank is: the flying operation robot applicable to the variable admittance controller is a quad-rotor drone; wherein the quad-rotor drone has the following characteristics:

[0085] 1) The internal motion controller of the quad-rotor drone can accurately track the motion trajectory of the quad-rotor drone;

[0086] 2) The end effector mechanism installed on the quadrotor drone is a 3-DOF robotic arm, and its end sampling force does not exceed the maximum load that the drone can apply.

[0087] In addition, the energy tank-based flying operation robot is characterized in that the quad-rotor drone also has the following parameter characteristics:

[0088] Flight platform mass m b The range is: 2.05≦m b ≦2.15; where m b The unit is kg;

[0089] The range of the length l1 of the robot arm 1 is: 0.115≦l1≦0.125; the unit of l1 is m;

[0090] The range of the length l2 of the robot arm 2 is: 0.06≦l2≦0.12; the unit of l2 is m;

[0091] The range of the length l3 of the robot arm 3 is: 0.165≦l3≦0.215; the unit of l3 is m;

[0092] The range of mass m1 of robot arm 1 is: 0.095≦m1≦0.115; the unit of m1 is kg;

[0093] The range of the mass m2 of the robot arm 2 is: 0.045≦m2≦0.055; the unit of m2 is kg;

[0094] The range of the mass m3 of the robot arm 3 is: 0.11≦m3≦0.13; the unit of m3 is kg;

[0095] Initial x-axis inertia tensor I x The range is: 0.45≦I x ≦0.53; among which I x The unit is kg·m -2 ;

[0096] Initial y-axis inertia tensor I y The range is: 0.45≦I y ≦0.53; among which I y The unit is kg·m -2 ;

[0097] Initial z-axis inertia tensor I z The range is: 0.05≦I z ≦0.09; among which I z The unit is kg·m -2 .

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

[0099] (1) The present invention proposes a variable admittance controller based on an energy tank, which realizes the adjustment of the compliance of the flying working robot when it interacts stably with the environment.

[0100] (2) The present invention proposes a critical damping controller with specified performance to enable the flying working robot to respond quickly to changes in admittance parameters when performing sampling operations in a swamp environment, thereby improving its performance in executing tasks.

[0101] (3) The present invention proposes a variable boundary energy tank, which can effectively weaken unsafe behaviors and improve the stability of sampling operations by monitoring energy flow when facing unknown disturbances in physical interactions. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0103] Figure 1 It is a schematic diagram of the three-dimensional structure of the flying working robot according to an embodiment of the present invention.

[0104] Figure 2 Schematic diagram of the coordinate system of the flying working robot according to the embodiment of the present invention.

[0105] Figure 3 Schematic diagram of the control system structure of an embodiment of the present invention.

[0106] Figure 4 Schematic diagram of the sampling trajectory tracking effect of the flying operation robot according to an embodiment of the present invention

[0107] Figure 5Schematic diagram of sampling trajectory tracking error of the flying operation robot according to an embodiment of the present invention

[0108] Figure 6 It is a schematic diagram of the change of the admittance parameters of the variable admittance controller of the flying working robot according to an embodiment of the present invention.

[0109] Figure 7 Schematic diagram of critical damping error of variable admittance controller of flying working robot according to an embodiment of the present invention

[0110] Figure 8 Schematic diagram of the energy flow monitoring effect of the flying operation robot according to an embodiment of the present invention

[0111] Fig. 9 It is a schematic diagram of the boundary and energy change of the energy tank of the flying operation robot according to an embodiment of the present invention. DETAILED DESCRIPTION

[0112] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings. A flexible sampling method for a flying operation robot based on an energy tank comprises the following steps:

[0113] Step S1, taking the position error and measured external force of the flying operation robot as inputs of the admittance model, and dividing the parameters to design a variable admittance controller;

[0114] Step S2, designing the adaptive rate of the inertia and stiffness parameters of the variable admittance controller, and calculating the damping parameter control rate based on the specified performance controller;

[0115] Step S3, combining the given energy tank rule with the changes of inertia, stiffness and damping parameters in the admittance controller, designing the passive parameters of the variable admittance controller;

[0116] Step S4, designing a variable boundary energy tank, proposing boundary change adaptation rate and energy flow exchange rules, and realizing safe supervision of the sampling process of the flying operation robot;

[0117] The variable admittance controller parameter division method of step S1 specifically comprises the following steps:

[0118] Step S11: Define the input position error as in, and represent the desired position and command position of the end effector respectively. is the end effector position matrix, x e ,y e 、z e are the positions along the x, y, and z directions in Cartesian space, respectively.

[0119] Step S12: During the physical interaction between the flying robot and the environment, the dynamics of the end effector can be described by an admittance model:

[0120]

[0121] in, is the force exerted by the environment on the end effector of the flying robot, M e ,D e and K e are the desired symmetric positive definite inertia, damping, and stiffness matrices.

[0122] Step S13: First, decompose the inertia, damping and stiffness matrices in the admittance model into:

[0123]

[0124] Among them, M c , D c , K c is a positive diagonal matrix, M v , D v , K v is a diagonal matrix with internal elements as variables. Using formula 1 and formula 2, the following variable admittance controller can be obtained:

[0125]

[0126] The damping parameter regulation performance control method of step S2 specifically comprises the following steps:

[0127] Step S21: The critical damping state means that the system reaches the desired position without oscillation in the shortest time. When the variable admittance controller is in this state, for the convenience of calculation, square both sides to get D e 2 (t)=4M e (t)K e (t). Then, the critical damping error and auxiliary position error are defined as follows:

[0128]

[0129] Where K a and are all constant symmetric matrices, e(t)=diag(e x ,e y ,e z ) is the critical damping error, is the auxiliary position error.

[0130] Step S22: The changes of inertia parameter, stiffness parameter and damping parameter in formula 3 can be expressed as

[0131]

[0132] in

[0133]

[0134]

[0135] where exp(·) represents the exponential function with base e, κ i is a positive constant such that when hour, The value of is established, represents the maximum allowable position error determined by the mission parameters. The superscript i = {x, y, z} represents the component on the corresponding axis, and They are all normal numbers.

[0136] Step S23: The critical damping controller is designed to enable each error e(t) to converge quickly to a preset region. Generally, the specified performance can be expressed as follows:

[0137] -δ l ρ i (t)<e i (t)<δ r ρ i (t) (Formula 7)

[0138] where e i is the i-th element of the critical damping error e(t), i = {x, y, z}, δ l and δ r is a positive constant, ρ(t) is a performance function, expressed as

[0139]

[0140] in and l are strictly positive constants satisfying Notice The rate of decrease is affected by the constant l. Then, the error conversion is used to convert the error e i Converted to the equivalent unconstrained error μ i The conversion process is determined by the function T(μ i ) control, defined as

[0141] e i =T(μ i )ρ i (Formula 9)

[0142] T(μi ) is strictly increasing, its inverse function always exists and satisfies -δ l <T(μ i )<δ r . Calculate T(μ i ) is as follows

[0143]

[0144] Among them, μ i is the i-th element of the conversion error μ, expressed as In addition, the first-order derivative of the transformation error μ can be derived as follows

[0145]

[0146] in

[0147] P = diag (ρ x ,ρ y ,ρ z ), r i (t) = (δ r +δ l ) / ((δ l ρ i (t)+e i )(δ r ρ i (t)-e i )), the superscript i = {x, y, z} represents the component on the corresponding coordinate axis.

[0148] Step S24: By selecting the controller gain The following critical damping controller can be used to drive the critical damping error e to converge to 0 and ensure the performance specified in Equation 7:

[0149]

[0150] Step S25: Combine Formulas 4 to 6 with Formula 11. It can be expressed as

[0151]

[0152] The Lyapunov function is defined as

[0153]

[0154] Then, using Formula 13, the derivative of V1 can be derived as

[0155]

[0156] By substituting Equation 12 into Equation 15, It can be expressed as

[0157]

[0158] Therefore, μ and e converge to zero asymptotically as t→∞.

[0159] According to the above definition, the inertia, stiffness and damping parameters in formula 3 are adaptively adjusted according to the changes of position error and critical damping error function e(t). A balance between stiffness and position error can be achieved. Under the action of , the system always moves towards the critical damping state.

[0160] The energy tank-based passivity method of step S3 specifically includes the following steps:

[0161] Step S31: Express the total energy of the variable admittance system with a natural non-negative energy storage function:

[0162]

[0163] The passivity condition requires that no internal energy is generated, so the following conditions must be met:

[0164]

[0165] Step S32: Calculate the time derivative of the energy storage function V(t), we get:

[0166]

[0167] In energy conservation and passivity analysis, all terms that may generate energy are often combined into one expression, and the dissipative terms are separated. where Ω includes all terms that may generate energy, such as those due to changes in the stiffness and inertia of the variable admittance controller. The dissipative term is then expressed as The dissipative term is always non-negative, since it represents the energy lost due to damping. However, due to changes in stiffness and inertia, p can become positive, which means that the controller may generate energy, violating the equilibrium condition in Equation 18.

[0168] Step S33: In order to maintain the passivity of the system, it is necessary to ensure that p does not cause the system to generate net energy. By interconnecting the variable admittance controller formula three with the energy tank, it is ensured that the system does not generate energy by itself and always maintains passivity. The energy tank is a virtual storage that allows the accumulation of dissipated energy for future use, usually expressed as:

[0169]

[0170] in A gain of 0 < η < 1 is used to control how much of the dissipated power is stored in the tank. It is the state of the tank. is the input-output pair that processes the tank power flow.

[0171] The energy stored in the energy tank is:

[0172]

[0173] The initial state of the tank is set to x t (0) to ensure that T(x t (0))≥ò, where ò is the threshold to avoid the singularity in Formula XX due to energy exhaustion.

[0174] Step S34: Power flow of energy tank Calculated by formula 20 and formula 21:

[0175]

[0176] Where θ1 is used to keep the energy in the tank within a predefined range [T - ,T + ], and T - >ò, its design is as follows:

[0177]

[0178] Where T - and T + They are the lower and upper limits of the energy tank respectively.

[0179] Step S35: The above definition means that if T(x t )∈[T - ,T + ] may be violated, then the energy connection between the tank and the variable admittance system will be closed. Having described the energy storage in the tank, the focus now shifts to utilizing this stored energy to achieve variable admittance behavior. From Equation 19, it is observed that only the variables inertia and stiffness contribute energy to the variable admittance controller, which requires a conditional restriction on their variation:

[0180]

[0181] The conditional variable θ2 is designed as:

[0182]

[0183] The above condition means that if the energy dissipation is positive (ηd+p>0), the variable inertia and stiffness will fully work. Otherwise, the variable inertia and stiffness depend on θ1.

[0184] Step S36: Prove that if the tank dynamics of the variable admittance controller formula 24 The tank parameter θ1 and conditional variable θ2 are designed as Formula 22, Formula 23 and Formula 25 respectively. Then the controller has the following input-output pair: Aspects are passive.

[0185] Step S37: Consider the following stored function

[0186]

[0187] Its time derivative can be calculated as

[0188]

[0189] According to the definitions of θ1 and θ2 in Formula 23 and Formula 25, we can derive

[0190]

[0191] Substituting Equation 28 into Equation 27 and noting that d>0, we can infer that

[0192]

[0193] This means that the passivity condition

[0194]

[0195] Therefore, the variable admittance controller always maintains passivity.

[0196] The variable boundary energy tank method for energy flow monitoring in step S4 specifically comprises the following steps:

[0197] Step S41: After the energy tank is connected to the variable admittance controller, the change of energy flow in the tank will affect the admittance parameter. Monitoring the energy flow helps to understand and mitigate the potential dangerous behavior of the flying operation robot. According to Formula 22, the maximum rate of energy change in the energy tank is defined as

[0198]

[0199] in, and Represent the maximum allowable position and velocity errors determined by the mission parameters, respectively, and are defined as follows

[0200]

[0201] where k r and k p is a constant symmetric matrix. t>t δ and t δ >0 represents the current time and the fixed time interval of the task. δ , the error of the expected task trajectory within a certain time range can be determined. is the maximum expected speed of the task.

[0202] Step S42: The safe energy condition of the flight operation robot can be understood through Formula 31, and the energy in the tank can be adjusted accordingly. Next, the rate of change of the lower limit of the energy tank can be defined as

[0203]

[0204] Step S43: Compared with the constant parameter admittance control, the change of inertia and stiffness changes the kinetic energy, and the upper limit T of the energy tank needs to be adjusted + This upper limit is defined by combining the current lower limit with the maximum kinetic energy that can be assigned to the current admittance parameter:

[0205]

[0206] Step S44: According to step S36, it can be inferred that as long as the energy level in the tank remains at its lower bound T - Above, even if T + and T - As time changes, the passivity of Formula 24 is also maintained. Therefore, during the sampling process, if it is realized that the safety of the flying robot is threatened, the mission will be interrupted; otherwise, the mission will continue.

[0207] Example:

[0208] The following is a detailed description of the implementation of the present invention using specific experiments. The present invention proposes a compliant sampling method for a flying operation robot based on an energy tank, which mainly studies how to improve the response speed of the admittance parameter and adjust the compliance to reduce the trajectory tracking error while compliantly tracking the expected sampling trajectory, and weaken the unsafe interactive behavior according to the energy flow supervision rules. The specific settings are as follows:

[0209] 1) The controller of the flight operation robot adopts an adaptive sliding mode controller, which takes into account the modeling error of the dynamic model and the impact of external disturbances on the flight operation robot.

[0210] 2) The system parameters of the flight operation robot are shown in Table 1:

[0211] Table 1 Flight operation robot system parameters

[0212]

[0213]

[0214] 3) The z-axis direction is set to receive the following interactive forces during the period from 5 seconds to 25 seconds:

[0215]

[0216] 4) The desired trajectory is set to 0.01 m per second along the z-axis.

[0217] 5) Application conditions of variable admittance controller based on energy tank: The internal motion controller of the quadrotor drone can calculate the smooth trajectory of the present invention. For precise tracking.

[0218] 6) Applicable scenarios: quadcopter drones in normal temperature and non-extreme weather.

[0219] 7) Structure and load limit of flying operation robot: The quadcopter is equipped with a 3-DOF robotic arm.

[0220] The terminal sampling force does not exceed the maximum load that the drone can exert.

[0221] like Figures 4 to 9 As shown, the controller further designed according to the safety sampling method of this embodiment can make the flight operation robot compliantly track the desired trajectory while minimizing the trajectory tracking error. A periodically changing external force is applied to the flight operation robot, thereby generating a trajectory tracking error that causes the admittance parameter to change, thereby achieving compliance adjustment. At the same time, the damping is adjusted so that the critical damping error is controlled within a specified range. The supervision of energy flow enables the flight operation robot to stop the change of the energy tank boundary when the energy extraction value exceeds the allowable value. Figures 4 to 9 The effectiveness and superiority of the present invention are demonstrated.

[0222] The above description is only a preferred embodiment of the present invention. For ordinary technicians in this field, according to the teachings of the present invention, designing different forms of flexible sampling algorithms based on energy tanks does not require creative labor. Without departing from the principles and spirit of the present invention, all equal changes, modifications, substitutions and variations made within the scope of the patent application of the present invention should fall within the scope of the present invention.

Claims

1. A flexible sampling method for a flying operation robot based on an energy tank, characterized in that: The following steps are involved: Step S1: The position error of the flight operation robot and the measured external force h f As the input of the admittance model, the parameters are divided to design the variable admittance controller; Step S2, designing the adaptive rate of the inertia and stiffness parameters of the variable admittance controller, and calculating the damping parameter control rate based on the specified performance controller; Step S3, combining the given energy tank rule with the changes of inertia, stiffness and damping parameters in the admittance controller, designing the passive parameters of the variable admittance controller; Step S4: design a variable boundary energy tank, propose boundary change adaptation rate and energy flow exchange rules, and realize safe supervision of the sampling process of the flying operation robot.

2. The flexible sampling method of the flying operation robot based on the energy tank according to claim 1 is characterized in that: Position Error of a Flying Robot as Design Input for a Variable Admittance Controller and the measured external force h f Specifically include the following: Step S11: Define the input position error as in, and represent the desired position and command position of the end effector respectively; is the end effector position matrix, x e ,y e 、z e are the positions along the x, y and z directions in Cartesian space respectively; Step S12: During the physical interaction between the flying robot and the environment, the dynamics of the end effector is described by an admittance model: in, is the force exerted by the environment on the end effector of the flying robot, M e ,D e and K e are the desired symmetric positive definite inertia, damping, and stiffness matrices.

3. The flexible sampling method of the flying operation robot based on the energy tank according to claim 2 is characterized in that: The variable admittance controller design also includes the following steps: Step S13: After completing the position error and the measured external force h f After the matrix is ​​parameterized, the inertia, damping and stiffness matrices in the admittance model are first decomposed into: Among them, M c , D c , K c is a positive diagonal matrix, M v , D v , K v is a diagonal matrix with internal elements as variables; using formula 1 and formula 2, the following variable admittance controller is obtained: This formula is the state equation of the aircraft admittance controller.

4. The compliant sampling method of a flying operation robot based on an energy tank according to claim 1, characterized in that: The adaptive rate design of the inertia and stiffness parameters of the variable admittance controller required for calculating the damping parameter control rate of the variable admittance controller specifically includes the following steps: Step S21: The critical damping state means that the system reaches the desired position without oscillation in the shortest time; When the variable admittance controller is in this state, square both sides to get D e 2 (t)=4M e (t)K e (t); then, the critical damping error and auxiliary position error are defined as follows: Where K a and are all constant symmetric matrices, e(t)=diag(e x ,e y ,e z ) is the critical damping error, is the auxiliary position error.

5. The compliant sampling method of the flying operation robot based on the energy tank according to claim 4 is characterized in that: The adaptive rate design of the inertia and stiffness parameters of the variable admittance controller also includes the following steps: Step S22: The changes of inertia parameter, stiffness parameter and damping parameter in formula 3 are expressed as in where exp(·) represents the exponential function with base e, κ i is a positive constant such that when hour, The value of is established, represents the maximum allowable position error determined by the mission parameters, and the superscript i = {x, y, z} represents the component on the corresponding axis. and They are all normal numbers; Step S23: The critical damping controller is designed to enable each error e(t) to quickly converge to a preset region; The specified performance is expressed as follows: -δ l ρ i (t) < e i (t) < δ r ρ i (t) (Formula VII) where e i is the i-th element of the critical damping error e(t), i = {x, y, z}, δ l and δ r is a positive constant, ρ(t) is a performance function, expressed as in and l are strictly positive constants satisfying The rate of decrease is affected by the constant l, and then the error e is converted into i Converted to the equivalent unconstrained error μ i ; The conversion process is determined by the function T(μ i ) control, defined as e i =T(μ i )ρ i (Formula 9) T(μ i ) is strictly increasing, its inverse function always exists and satisfies -δ l <T(μ i )<δ r , calculate T(μ i ) is as follows Among them, μ i is the i-th element of the conversion error μ, expressed as In addition, the first-order derivative of the transformation error μ is derived as follows: where P = diag(ρ x , ρ y , ρ z ), r i (t) = (δ r +δ l ) / ((δ l ρ i (t)+e i )(δ r ρ i (t)-e i )), superscript i = {x, y, z} represents the component on the corresponding coordinate axis; Step S24: By selecting the controller gain The critical damping error e is driven to converge to 0 using the following critical damping controller and the performance specified in Equation 7 is guaranteed: Therefore, the inertia, stiffness and damping parameters in Formula 3 are adaptively adjusted according to the changes of position error and critical damping error function e(t); by adjusting the attenuation factor Achieve a balance between stiffness and position error, and in the process of performing tasks, in the adaptive gain Under the action of , the system always moves towards the critical damping state.

6. The compliant sampling method of a flying operation robot based on an energy tank according to claim 1, characterized in that: The design of the passive parameters of the variable admittance controller specifically includes the following steps: Step S31: Express the total energy of the variable admittance system with a natural non-negative energy storage function: The passivity condition requires that no internal energy is generated, so the following conditions must be met: V≤r e T h f (Formula 14) Step S32: Calculate the time derivative of the energy storage function V(t), we get: In the energy conservation and passivity analysis, all energy generating terms are combined into one expression, and the dissipative terms are separated. Formula 15 can be simplified to where Ω includes all energy-generating terms, such as those due to changes in the stiffness and inertia of the variable admittance controller; the dissipative terms are expressed as The dissipation term is always non-negative since it represents the energy lost due to damping; however, due to the changes in stiffness and inertia, p becomes positive, i.e., the controller generates energy, violating the equilibrium condition in Equation 14.

7. The compliant sampling method of a flying operation robot based on an energy tank according to claim 6, characterized in that: The design of the passivity parameters of the variable admittance controller also includes the following steps: Step S33: After completing the energy conservation and passivity analysis of the system, in order to maintain the passivity of the system, it is necessary to ensure that p does not cause the system to generate net energy; Therefore, by interconnecting the variable admittance controller Formula 3 with the energy tank, it is ensured that the system does not generate energy by itself and always remains passive; the energy tank is a virtual storage that allows the accumulation of dissipated energy for future use, expressed as: in The gain 0 < η < 1 is used to control how much of the dissipated power is stored in the tank; It is the state of the tank. is the input-output pair that processes the tank power flow; The energy stored in the energy tank is: The initial state of the tank is set to x t (0) to ensure that T(x t (0))≥ò, where ò is the threshold to avoid the singularity in equation 16 due to energy depletion; Step S34: Power flow of the tank Calculated by formula 16 and formula 17: Where θ1 is used to keep the energy in the tank within a predefined range [T - ,T + ], and T - >ò, its design is as follows: Where T - and T + They are the lower and upper limits of the energy tank respectively; Step S35: Step S34 shows that if T(x t )∈[T - ,T + ] is violated, then the energy connection between the tank and the variable admittance system will be closed; Having described the energy storage in the tank, the focus now shifts to utilizing this stored energy to achieve variable admittance behavior; From Equation 15, it is observed that only the variables inertia and stiffness contribute energy to the variable admittance controller, which requires a conditional restriction on their variation: The conditional variable θ2 is designed as: From the above conditions, it can be seen that if the energy dissipation is positive (ηd+p>0), the variable inertia and stiffness will fully work; otherwise, the variable inertia and stiffness depend on θ1; Step S36: Based on step S35, if the tank dynamics of the variable admittance controller formula 20 The tank parameter θ1 and conditional variable θ2 are designed as Formula 18, Formula 19 and Formula 21 respectively. Then the controller has the following input-output pair: Aspects are passive.

8. The compliant sampling method of a flying operation robot based on an energy tank according to claim 1, characterized in that: The variable boundary energy tank and boundary change adaptation rate and energy flow exchange rules required for the implementation of the safety supervision of the sampling process of the flying operation robot specifically include the following steps: Step S41: After the energy tank is connected to the variable admittance controller, the change of energy flow in the tank will affect the admittance parameter; monitoring the energy flow helps to understand and mitigate the potential dangerous behavior of the flying operation robot; according to Formula 18, the maximum rate of energy change in the energy tank is defined as: in, and Represent the maximum allowable position and velocity errors determined by the mission parameters, respectively, and are defined as follows where k r and k p is a constant symmetric matrix; t>t δ and t δ >0 represents the current time and the fixed time interval of the task respectively; by selecting t δ , determine the error of the expected task trajectory within a certain time range, is the maximum expected speed of the task; Step S42: After completing the design of the variable boundary energy tank and proposing the boundary change adaptation rate and energy flow exchange rule, the safe energy condition of the flight operation robot is obtained through Formula 22, and the energy in the tank is adjusted accordingly; next, the change rate of the lower limit of the energy tank is defined as: Step S43: Compared with the constant parameter admittance control, the change of inertia and stiffness changes the kinetic energy, and the upper limit T of the energy tank needs to be adjusted + ; This upper limit is defined by combining the current lower limit and the maximum kinetic energy assigned to the current admittance parameter: Step S44: According to step S43, as long as the energy level in the tank remains at its lower limit T - Above, even if T + and T - As time changes, the passivity of Formula 20 is also maintained; therefore, during the sampling process, if it is determined that the safety of the flying operation robot is threatened, the mission will be interrupted; otherwise, the mission will continue.

9. A flying operation robot based on an energy tank, implemented by the method described in any one of claims 1 to 9; characterized in that: The application conditions of the variable admittance controller based on the energy tank are: the flight operation robot suitable for the variable admittance controller is a quad-rotor drone; the quad-rotor drone has the following characteristics: 1) The internal motion controller of the quad-rotor drone can accurately track the motion trajectory of the quad-rotor drone; 2) The end effector mechanism installed on the quadrotor drone is a 3-DOF robotic arm, and its end sampling force does not exceed the maximum load that the drone can apply.

10. The flying operation robot based on an energy tank according to claim 9, characterized in that: The quad-rotor drone also has the following parameter characteristics: Flight platform mass m b The range is: 2.05≦m b ≦2.15; where m b The unit is kg; The range of the length l1 of the robot arm 1 is: 0.115≦l1≦0.125; the unit of l1 is m; The range of the length l2 of the robot arm 2 is: 0.06≦l2≦0.12; the unit of l2 is m; The range of the length l3 of the robot arm 3 is: 0.165≦l3≦0.215; the unit of l3 is m; The range of mass m1 of robot arm 1 is: 0.095≦m1≦0.115; the unit of m1 is kg; The range of the mass m2 of the robot arm 2 is: 0.045≦m2≦0.055; the unit of m2 is kg; The range of the mass m3 of the robot arm 3 is: 0.11≦m3≦0.13; the unit of m3 is kg; Initial x-axis inertia tensor I x The range is: 0.45≦I x ≦0.53; among which I x The unit is kg·m -2 ; Initial y-axis inertia tensor I y The range is: 0.45≦I y ≦0.53; among which I y The unit is kg·m -2 ; Initial z-axis inertia tensor I z The range is: 0.05≦I z ≦0.09; among which I z The unit is kg·m -2 .

Images