Energy tank-based flying operation robot compliant sampling method

Abstract

The application provides a kind of energy tank-based flying operation robot compliant sampling method.It includes the following steps: step S1, the position error of flying operation robot and the measured external force are used as the input of admittance model, and the variable admittance controller is designed by dividing parameters;Step S2, the adaptive rate of inertia and stiffness parameters of variable admittance controller is designed, and the damping parameter control rate is calculated based on the specified performance controller;Step S3, the passivity parameters of variable admittance controller are designed by combining the given energy tank rules and the changes of inertia, stiffness and damping parameters in admittance controller;Step S4, variable boundary energy tank is designed, boundary change adaptive rate and energy flow exchange rules are proposed, and safety supervision of flying operation robot sampling process is realized;The flying operation robot compliant sampling method proposed in the application performs well, the flying operation robot can complete compliant interaction and sampling operation in solid-liquid mixed environment, and can effectively meet the task requirements.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicles, and particularly relates to a compliant sampling method for a flight operation robot based on an energy tank. BACKGROUND

[0002] In recent years, the ability of flight operation robots to quickly reach high-altitude work areas and physically interact has been continuously enhanced. This has led to their widespread adoption in various applications, including inspection and maintenance tasks. The use of flight operation robots significantly reduces the time, cost, and safety risks of personnel, while improving the efficiency of task execution. The flexibility of the mechanical arm enables flight operation robots to actively perform complex tasks or react to environmental interactions during the task. However, this flexibility also introduces the risk of unstable physical interaction, including unstable contact, insufficient response speed to task parameters during interaction, and insufficient compliance in interactive behavior. At the same time, the complexity and uncertain dynamics of objects in solid-liquid mixed environments such as marshes and mudflats make it difficult for flight operation robots to successfully perform interactive 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 flight operation 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 the adjustment of stable contact and interactive compliance. During the process of compliant physical interaction with the environment, the behavior of the flight operation 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 under-damped and over-damped states caused by deviating from the critical damping state may have adverse effects on the sampling performance of the flight operation robot. The flight operation robot may exhibit jitter or oscillation under under-damped conditions. Over-damped conditions may reduce the response speed of the flight operation robot, resulting in tracking errors. Whether it is possible to enhance the response performance to changes in admittance parameters during physical interaction by precisely adjusting the damping state of the flight operation robot is a direction worthy of research. SUMMARY

[0004] In recent years, port Hamilton system, energy tank and damping distribution passive control and other passive-based methods are extended from network field to robot field. The flying operation robot and environment can be regarded as independent modules interacting through energy port. By applying passive standard, the robust stability of interactive behavior is ensured. In the process of compliant interaction, the compliance of commonly used admittance controller is fixed. Adjusting the admittance parameter to realize adjustable compliance is a non-passive operation. Considering the interconnection of energy tank and variable admittance controller, the passive-based control and admittance controller are decoupled, and the flying operation robot can adjust the compliance while keeping passive.

[0005] In practical application, the typical flying operation robot task usually needs to define appropriate energy in the energy tank in advance. If the predefined energy is too low, the available energy will be exhausted during the task process, which will cause the flying operation robot to repeatedly switch control modes, resulting in chattering and increasing the wear of the actuator. If the predefined energy is too much, the flying operation robot may extract a large amount of energy from the energy tank in a short time, which may cause it to perform dangerous sampling behavior. Therefore, during the compliant interaction between the flying operation robot and the environment, it is a research direction to safely adjust the energy in the energy tank.

[0006] The purpose of the present application is to provide a flying operation robot compliant sampling method based on an energy tank, which can improve the response speed of the admittance parameter, realize the adjustment of the compliance and the safe supervision of the interaction power in the interaction process, and thus realize the compliant sampling of the flying operation robot. The present application adopts the following technical solutions.

[0007] The present application provides a mobile robot motion planning method for unknown target collection. In the method, the mobile robot can quickly complete the exploration of unknown environment and the collection of targets in the task area, and can effectively meet the task demand. The present application adopts the following technical solutions.

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

[0009] Step S1, inputting the position error of the flying operation robot and the measured external force h f to the admittance model as input, and designing a variable admittance controller by dividing the parameters;

[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 the inertia, stiffness and damping parameters in the admittance controller, and designing the passivity parameter of the variable admittance controller;

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

[0013] The safety supervision of the sampling process of the flying operation robot includes connecting the energy tank to the variable admittance controller, and using the maximum rate of energy change in the energy tank to monitor the energy flow, which helps to understand and reduce the potential dangerous behavior of the flying operation robot;

[0014] The safety supervision of the sampling process of the flying operation robot also includes defining the safety energy condition of the flying operation robot through the maximum rate of energy change in the energy tank after completing the design of the variable boundary energy tank and proposing the boundary change adaptive rate and energy flow exchange rule, and adjusting the energy in the tank according to the change rate of the lower limit of the energy tank;

[0015] The safety supervision of the sampling process of the flying operation robot also includes adjusting the upper limit of the energy tank; this upper limit is defined by combining the current lower limit and the maximum kinetic energy allocated to the current admittance parameter:

[0016] Finally, as long as the energy level in the energy tank remains above the lower limit of the energy tank, the passivity of the variable admittance controller is maintained even if the upper and lower limits of the energy tank change over time; therefore, when the sampling process is in progress, if it is determined that the safety of the flying operation robot is threatened, the task will be interrupted; otherwise, the task will continue.

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

[0018] Step S11: define the input position error as Wherein, And respectively represent the desired position and the command position of the end effector. e = [x e y e z e ] T is the end effector position matrix, x e , y e , z e are the positions in the x direction, y direction and z direction in the Cartesian space respectively.

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

[0020]

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

[0022] Step S13: First, decompose the inertia, damping and stiffness matrices in the mobility model as follows:

[0023]

[0024] where M c , D c , K c are constant diagonal matrices, and M v , D v , K v are diagonal matrices with variable entries. Using Equations 1 and 2, the following variable mobility controller can be obtained:

[0025]

[0026] The damping parameter of the step S2 defines the performance control method specifically includes the following steps:

[0027] Step S21: The critical damping state refers to the system reaching the desired position without oscillation in the shortest time. When , the variable mobility controller is in this state; for easy calculation, square both sides to get D e 2 (t) = 4M e (t) K e (t). Then, the critical damping error, the auxiliary position error are defined as follows:

[0028]

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

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

[0031] where

[0032]

[0033] where exp(•) represents the exponential function with base e, κ i is a positive constant such that when , the value of holds, where represents the maximum allowable position error determined by the task parameters. The superscript i = {x, y, z} represents the component in the corresponding axis, and are positive constants.

[0034] Step S23: The critical damping controller is designed such that each error e(t) is able to converge quickly to a pre-specified region. In general, the prescribed performance can be expressed as follows:

[0035] - δ l ρ i (t) < e i (t) < δ r ρ i (t) (Equation Seven)

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

[0037]

[0038] where and l are positive constants satisfying Note that the rate of decrease of i is affected by the constant l. Then, an error transformation is employed to transform the error e i into an equivalent unconstrained error μ i . The transformation process is governed by the function T(μ i ) defined as

[0039] e i = T(μ i ) p i (Equation Nine) T(μ l ) is strictly increasing, its inverse always exists, and satisfies - δ i < T(μ r ) < δ i . The inverse of T(μ i ) is computed as follows

[0040]

[0041] where μ x is the i-th element of the transformed error μ, expressed as μ = [μ y , μy ,μ z ] T In addition, the first derivative of the transformation error μ can be derived as follows:

[0042]

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

[0044]

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

[0046] Step S24: Selecting the controller gain The following critically damped controller can be used to drive the critically damped error e to converge to 0 and guarantee the predetermined performance in Equation Seven:

[0047]

[0048] Therefore, the inertia, stiffness, and damping parameters in Equation Three are adaptively adjusted according to the changes in the position error and the critically damped error function e(t). By adjusting the decay factor The balance between stiffness and position error can be achieved. During the execution of the task, the system is always moving towards the critically damped state under the action of the adaptive gain The step S3 of the energy tank-based passivity method 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 no internal energy generation, so the following condition 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 the terms that can potentially generate energy are combined into one expression, and the dissipation terms are separated. where Ω contains all the terms that can potentially generate energy, such as the terms due to the variation of the controller's stiffness and inertia. The dissipation term is expressed as The dissipation term is always non-negative because it represents the energy lost due to damping. However, p can become positive due to the variation of the stiffness and inertia, which means that the controller can potentially generate energy, which violates the equilibrium condition in equation fourteen.

[0056] Step S33: To maintain the passivity of the system, it is necessary to ensure that p does not cause the system to generate net energy. By connecting the variable admittance controller equation three with an energy tank, it is ensured that the system does not generate energy on its own and remains passive at all times. The energy tank is a virtual storage that allows the dissipated energy to be accumulated for future use, and it is typically expressed as

[0057]

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

[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 where is the threshold to avoid the singularity in equation sixteen due to the depletion of energy.

[0062] Step S34: The power flow of the tank is calculated by equation sixteen and equation seventeen:

[0063]

[0064] where θ1 is used to keep the energy in the tank within a predefined range [T - ,T + ] and is designed as follows:

[0065]

[0066] where T- and T + These 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 + If this is violated, the energy connection between the tank and the variable admittance system will be shut down. 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 can be observed that only variable inertia and stiffness contribute energy to the variable admittance controller, which necessitates a conditional constraint on their variation:

[0068]

[0069] The condition variable θ2 is designed as follows:

[0070]

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

[0072] Step S36: This means that if the variable admittance controller formula twenty has a tank dynamic If the tank parameter θ1 and condition variable θ2 are designed as Equations 18, 19, and 21 respectively, then the controller will operate in a way that affects the input and output pairs. This aspect is passive.

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

[0074] Step S41: After connecting the energy tank to the variable admittance controller, changes in the energy flow within the tank will affect the admittance parameters. Monitoring the energy flow helps to understand and mitigate potentially hazardous behaviors of the aerial operation robot. According to Equation 18, the maximum rate of energy change in the energy tank is defined as...

[0075]

[0076] in, and These represent the maximum permissible position and velocity errors determined by the mission parameters, respectively, and are defined as follows:

[0077]

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

[0079] Step S42: The safety energy condition of the flying service robot can be understood by formula twenty-two, and the energy in the tank is 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 constant parameter steering control, the change of inertia and stiffness changes the kinetic energy, which needs to adjust the upper limit T + of the energy tank. This upper limit is defined by combining the current lower limit and the maximum kinetic energy that can be allocated to the current steering parameter:

[0082]

[0083] Step S44: According to step S36, it can be inferred that as long as the energy level in the tank is kept at its lower limit T - , the above, even if T + and T - change over time, the passivity of formula twenty is maintained. Therefore, when sampling, if it is realized that the safety of the flying service robot is threatened, the task will be interrupted; otherwise, the task will continue.

[0084] In summary, a flying service robot based on an energy tank adopts the above method; characterized in that the application condition of the variable steering controller based on the energy tank is: the flying service robot to which the variable steering controller is applied is a quadcopter; wherein the quadcopter has the following characteristics:

[0085] 1) The internal motion controller of the quadcopter can realize accurate tracking of the motion trajectory of the quadcopter;

[0086] 2) The end effector mechanism installed on the quadcopter is a 3-DOF mechanical arm, and the sampling force at the end thereof does not exceed the maximum load that can be applied by the quadcopter.

[0087] In addition, the flying service robot based on the energy tank is characterized in that the quadcopter has the following parameter characteristics:

[0088] The mass m b of the flight platform is in the range of: 2.05 ≦ m b ≦ 2.15; wherein the unit of m b is kg;

[0089] The length l1 of the robotic arm lever 1 is in the range of: 0.115 ≤ l1 ≤ 0.125; where the unit of l1 is meters.

[0090] The length l2 of the robotic arm lever 2 is in the range of: 0.06 ≤ l2 ≤ 0.12; where the unit of l2 is meters.

[0091] The range of the length l3 of the robotic arm lever 3 is: 0.165 ≤ l3 ≤ 0.215; where the unit of l3 is meters.

[0092] The mass m1 of the robotic arm 1 is in the range of 0.095 ≤ m1 ≤ 0.115; where m1 is in kg.

[0093] The mass m2 of the robotic arm lever 2 is in the range of 0.045 ≤ m2 ≤ 0.055; where m2 is in kg.

[0094] The range of the mass m3 of the robotic arm lever 3 is: 0.11 ≤ m3 ≤ 0.13; where the unit of m3 is kg.

[0095] Initial x-axis inertia tensor I x The range is: 0.45 ≤ I x ≤0.53; where 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; where 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; where I z The unit is kg·m -2 .

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

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

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

[0101] (3) The application 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 interaction. BRIEF DESCRIPTION OF DRAWINGS

[0102] The application will be further described in detail below in combination with the drawings and specific embodiments:

[0103] Figure 1 is a three-dimensional structure schematic diagram of a flying work robot of an embodiment of the application.

[0104] Figure 2 is a coordinate system schematic diagram of a flying work robot of an embodiment of the application.

[0105] Figure 3 is a control system structure schematic diagram of an embodiment of the application.

[0106] Figure 4 is a sampling trajectory tracking effect schematic diagram of a flying work robot of an embodiment of the application

[0107] Figure 5 is a sampling trajectory tracking error schematic diagram of a flying work robot of an embodiment of the application

[0108] Figure 6 is a variable admittance controller of a flying work robot of an embodiment of the application.

[0109] Figure 7 is a critical damping error schematic diagram of a variable admittance controller of a flying work robot of an embodiment of the application

[0110] Figure 8 is a flying work robot energy flow monitoring effect schematic diagram of an embodiment of the application

[0111] Figure 9 is an energy tank boundary and energy change schematic diagram of a flying work robot of an embodiment of the application. DETAILED DESCRIPTION

[0112] The technical solutions of the application will be specifically described below in combination with the drawings. A flying work robot compliant sampling method based on an energy tank comprises the following steps:

[0113] Step S1, input the position error of the flying work robot and the measured external force h f as the input of the admittance model, and design a variable admittance controller by dividing the parameters;

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

[0115] Step S3, design the passivity parameters of the variable admittance controller by combining the given energy tank rules with the changes of inertia, stiffness and damping parameters in the admittance controller;

[0116] Step S4, design the variable boundary energy tank, propose the boundary change adaptive rate and energy flow exchange rules, and realize the safety supervision of the sampling process of the flying operation robot;

[0117] Wherein the safety supervision of the sampling process of the flying operation robot includes connecting the energy tank to the variable admittance controller, and using the maximum rate of energy change in the energy tank to monitor the energy flow, which helps to understand and reduce the potential dangerous behavior of the flying operation robot;

[0118] Wherein the safety supervision of the sampling process of the flying operation robot also includes defining the safety energy condition of the flying operation robot through the maximum rate of energy change in the energy tank after completing the design of the variable boundary energy tank and proposing the boundary change adaptive rate and energy flow exchange rules, and adjusting the energy in the tank according to the change rate of the lower limit of the energy tank;

[0119] Wherein the safety supervision of the sampling process of the flying operation robot also includes adjusting the upper limit of the energy tank; this upper limit is defined by combining the current lower limit and the maximum kinetic energy allocated to the current admittance parameter:

[0120] Finally, as long as the energy level in the energy tank remains above the lower limit of the energy tank, the passivity of the variable admittance controller is maintained even if the upper and lower limits of the energy tank change over time; therefore, when the sampling process is in progress, if it is determined that the safety of the flying operation robot is threatened, the task will be interrupted; otherwise, the task will continue.

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

[0122] Step S11: define the input position error as Wherein, And respectively represent the desired position and the command position of the end effector. e = [x e y e z e ] T is the end effector position matrix, x e , y e , z e are the positions along the x, y and z directions in the Cartesian space respectively.

[0123] Step S12: During the physical interaction of the flying work robot with the environment, the dynamics of the end effector can be described by a mobility model:

[0124]

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

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

[0127]

[0128] where, M c , D c , K c are constant diagonal matrices, and M v , D v , K v are diagonal matrices with variable internal elements. Using Formula One and Formula Two, the following variable mobility controller can be obtained:

[0129]

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

[0131] Step S21: The critical damping state refers to the system reaching the desired position without oscillation in the shortest time. When , the variable mobility controller is in this state; for easy calculation, square both sides to get D e 2 (t) = 4M e (t)K e (t). Then, the critical damping error and the auxiliary position error are defined as follows:

[0132]

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

[0134] Step S22: The variations of the inertia parameter, the stiffness parameter and the damping parameter in Equation Three can be expressed as

[0135] where

[0136]

[0137] where exp(·) represents the exponential function with base e, κ i is a positive constant such that when , the value of holds, and represents the maximum allowable position error determined by the task parameter. The superscript i = {x, y, z} represents the component on the corresponding axis, and are positive constants.

[0138] Step S23: The critical damping controller is designed such that each error e(t) is able to converge to a pre-specified region quickly. In general, the prescribed performance can be expressed as follows:

[0139] - δ l ρ i (t) < e i (t) < δ r ρ i (t) (Equation Seven)

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

[0141]

[0142] where and l are positive constants satisfying Note that the rate of decrease of is affected by the constant l. Then, an error transformation is employed to transform the error e i into an equivalent unconstrained error μ i . The transformation process is governed by the function T(μ i ) defined as

[0143] e i = T(μ i ) p i (Equation Nine) T(μ i ) is strictly increasing, its inverse function always exists, and satisfies - δ l < T(μ i ) < δ r . T(μi The inverse function of (7) is as follows

[0144]

[0145] where μ i is the i-th element of the transformation error μ, denoted as μ = [μ x , μ y , μ z ] T Moreover, the first derivative of the transformation error μ can be derived as follows:

[0146]

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

[0148]

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

[0150] Step S24: Selecting the controller gain The following critically damped controller can be used to drive the critically damped error e to converge to 0 and guarantee the predetermined performance in Equation (7):

[0151]

[0152] Step S25: Combining Equations (4) to (6) and (11), which can be expressed as

[0153]

[0154] Define the Lyapunov function as

[0155]

[0156] Then, the derivative of V1 can be derived using Equation (13) as

[0157]

[0158] By substituting Equation (12) into Equation (15), may be expressed as

[0159]

[0160] Thus, μ and e asymptotically converge to zero as t→∞.

[0161] According to the above definition, the inertia, stiffness, and damping parameters in equation three are adaptively adjusted according to the change of position error and critical damping error function e(t). By adjusting the decay factor The balance between stiffness and position error can be achieved. During the process of performing tasks, the system is always moving towards the critical damping state under the action of adaptive gain

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

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

[0164]

[0165] The passivity condition requires no internal energy generation, so the following condition must be met:

[0166]

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

[0168]

[0169] In energy conservation and passivity analysis, all possible energy generating terms are combined into one expression, and the dissipation term is separated alone. That is, where Ω contains all possible energy generating terms, such as terms due to changes in the stiffness and inertia of the variable admittance controller. The dissipation term is expressed as The dissipation term is always non-negative because it represents the energy lost due to damping. However, due to changes in stiffness and inertia, p can become positive, which means the controller can generate energy, which violates the balance condition in equation eighteen.

[0170] Step S33: To maintain the passivity of the system, it is necessary to ensure that p does not cause the system to generate net energy. By connecting the variable admittance controller equation three with the energy tank, it is ensured that the system does not generate energy on its own, always maintaining passivity. The energy tank is a virtual storage that allows the accumulation of dissipated energy for future use, and is usually expressed as:

[0171] ​

[0172] where The gain 0 < η < 1 is used to control how much of the dissipated power is stored into the tank. is the state of the tank, is the input-output pair handling the power flow of the tank.

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

[0174]

[0175] The initial state of the tank is set to x t (0) to ensure where θ0is a threshold value to avoid the singularity in equation twenty due to energy depletion.

[0176] Step S34: Power flow of the energy tank is computed by equations twenty and twenty-one:

[0177]

[0178] where θ1is used to keep the energy in the tank within a predefined range [T - ,T + ] and is designed as follows:

[0179]

[0180] where T - and T + are the lower and upper bounds of the energy tank, respectively.

[0181] Step S35: The above definitions imply that if T(x t ) ∈ [T - ,T + ] can be violated, then the energy link between the tank and the variable admittance system will be shut down. Having described the energy storage in the tank, the focus now shifts to exploiting these stored energies to achieve the variable admittance behavior. From equation nineteen, it is observed that only the variable inertia and stiffness contribute energy to the variable admittance controller, which requires a conditional restriction on their variations:

[0182]

[0183] where the conditional variable θ2is designed as:

[0184]

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

[0186] Step S36: Prove that the tank dynamics of the variable admittance controller formula twenty-four are stable if The tank parameters θ1 and the condition variable θ2 are designed as formula twenty-two, formula twenty-three and formula twenty-five, respectively, such that the controller is passive with respect to the input-output pair .

[0187] Step S37: Consider the following storage function

[0188]

[0189] The time derivative of which can be calculated as

[0190]

[0191] From the definitions of θ1 and θ2 in formula twenty-three and formula twenty-five, it follows that

[0192]

[0193] Substitute formula twenty-eight into formula twenty-seven and note that d > 0, it can be concluded that

[0194]

[0195] This means that the passivity condition

[0196]

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

[0198] The variable boundary energy tank method of the energy flow monitoring of the step S4 specifically comprises the following steps:

[0199] Step S41: After connecting the energy tank to the variable admittance controller, the change of energy flow in the tank will affect the admittance parameters. Monitoring the energy flow helps to understand and mitigate the potential dangerous behavior of the flying operating robot. According to formula twenty-two, the maximum rate of energy change in the energy tank is defined as

[0200]

[0201] Where, and represent the maximum allowable position and velocity errors determined by the task parameters, respectively, and are defined as follows

[0202]

[0203] where k r and k p are constant symmetric matrices. t > t δ and t δ > 0 represent the current time and the fixed time interval of the task, respectively. By choosing t δ , the error of the desired trajectory of the task within a certain time range can be determined, is the maximum desired velocity of the task.

[0204] Step S42: The safety energy condition of the flying service robot can be understood by formula thirty-one, and the energy in the tank is adjusted accordingly. Next, the rate of change of the lower limit of the energy tank can be defined as

[0205]

[0206] Step S43: Compared with constant parameter steering control, the change of inertia and stiffness changes the kinetic energy, which requires adjusting the upper limit T + of the energy tank. This upper limit is defined by combining the current lower limit and the maximum kinetic energy that can be allocated to the current steering parameter:

[0207]

[0208] Step S44: According to step S36, it can be inferred that as long as the energy level in the tank is kept at its lower limit T - above, the passivity of formula twenty-four is maintained even if T + and T - change over time. Therefore, if it is realized during the sampling process that the safety of the flying service robot is threatened, the task will be interrupted; otherwise, the task will continue.

[0209] Embodiment:

[0210] The following will be described in detail with specific experiments. The present application proposes a flying service robot compliant sampling method based on energy tank, mainly studies how to improve the response speed of steering parameters and adjust compliance to reduce trajectory tracking error while compliantly tracking the desired sampling trajectory, and weakens unsafe interaction behavior according to energy flow supervision rules. The specific settings are as follows:

[0211] 1) The flying service robot controller uses an adaptive sliding mode controller, which takes into account the influence of modeling errors of the dynamic model and external disturbances received on the flying service robot.

[0212] 2) The system parameters of the flying service robot are shown in Table 1:

[0213] Table 1 System parameters of flying service robot

[0214]

[0215]

[0216] 3) The following interaction forces are applied in the z-axis direction during the period from 5 to 25 seconds:

[0217]

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

[0219] 5) Application conditions of the variable admittance controller based on the energy tank: The motion controller inside the quadcopter UAV can perform the compliant trajectory calculated by this invention. To perform precise tracking.

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

[0221] 7) Structural and load limitations of the flying operation robot: The quadcopter drone is equipped with a 3-DOF robotic arm, and the end-effector sampling force does not exceed the maximum load that the drone can apply.

[0222] like Figures 4 to 9 As shown, the controller further designed according to the safety sampling method of this embodiment can enable the flying operation robot to smoothly track the desired trajectory while minimizing trajectory tracking error. A periodically varying external force is applied to the flying operation robot, thereby generating trajectory tracking error and causing changes in admittance parameters, thus adjusting compliance. Simultaneously, damping adjustment ensures that the critical damping error is controlled within a specified range. Monitoring of energy flow causes the flying operation robot to stop changing the energy tank boundary when the energy extraction value exceeds the allowable value. Figures 4 to 9 This demonstrates the effectiveness and superiority of the present invention.

[0223] The above description is only a preferred embodiment of the present invention. For those skilled in the art, designing different forms of compliant sampling algorithms based on energy tanks does not require creative labor according to the teachings of the present invention. All equivalent changes, modifications, substitutions and variations made in accordance with the scope of the patent application of the present invention without departing from the principles and spirit of the present invention shall be covered by the present invention.

Claims

1. An energy tank based aerial work machine robot compliant sampling method, characterized by: The method comprises the following steps: Step S1, obtaining a position error of the flying operation robot and a measured external force h f as an input of the admittance model, and performing a partition design on the parameters to obtain a variable admittance controller Step S2, adaptive rate of inertia and stiffness parameters of the variable admittance controller is designed, and damping parameter control rate of the variable admittance controller is calculated based on a prescribed performance controller; Step S3, passive parameter of the variable admittance controller is designed by combining given energy tank rules and changes of inertia, stiffness and damping parameters in the admittance controller; Step S4, a variable boundary energy tank is designed, a boundary change adaptive rate and an energy flow exchange rule are proposed, and safety supervision on a sampling process of the flight operation robot is realized; The safety supervision on the sampling process of the flight operation robot comprises the following steps: The safety supervision on the sampling process of the flight operation robot further comprises the following steps: The safety supervision on the sampling process of the flight operation robot further comprises the following steps: The safety supervision on the sampling process of the flight operation robot further comprises the following steps:

2. The energy tank based aerial work robot compliant sampling method according to claim 1, characterized in that: Position error of a flight operating robot as a design input of a variable admittance controller and a measured external force h f Specifically includes the following content: Step S11: define the input position error as wherein, and denote the desired position and the commanded position of the end effector, respectively; r e = [x e y e z e ] T is the end effector position matrix, x e , y e , z e are the positions in the x-direction, y-direction and z-direction in Cartesian space, respectively; As long as the energy level in the energy tank is kept above the lower limit of the energy tank, the passivity of the variable admittance controller is maintained even if the upper limit and the lower limit of the energy tank change over time; therefore, when the sampling process is in progress, if it is determined that the safety of the flight operation robot is threatened, the task will be interrupted; otherwise, the task will continue. wherein, is the force exerted by the environment on the flying robot end effector, M e ,D e and K e are the desired symmetric positive definite inertia, damping and stiffness matrices.

3. The energy tank based aerial work robot compliant sampling method according to claim 2, characterized in that: Step S12: in the physical interaction process between the flight operation robot and the environment, the dynamics of the end effector is described by an admittance model: Step S13: After completion of the position error and the measured external force h f After matrix parameterization of the inertia, damping and stiffness matrices in the admittance model, one first decomposes them into: where M c , D c , K c are constant diagonal matrices, M v , D v , K v are diagonal matrices with variable entries; using equation one and equation two, the following variable admittance controller is obtained: The variable admittance controller design further comprises the following steps:

4. The energy tank based aerial work robot compliant sampling method according to claim 1, characterized in that: The formula is the state equation of the aircraft admittance controller. Step S21: Critical damping state refers to the system reaching the desired position without oscillation in the shortest time; when the variable admittance controller is in this state; the square of both sides gives D e 2 (t) = 4M e (t) K e (t); then the critical damping error, the auxiliary position error is defined as follows: where K a and are constant symmetric matrices, e(t) = diag(e x ,e y ,e z ) is the critical damping error, is the auxiliary position error.

5. The energy tank based aerial work robot compliant sampling method according to claim 4, 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 comprises the following steps: Step S22: The changes in the inertia parameter, the stiffness parameter, and the damping parameter in Equation Three are expressed as The adaptive rate design of the inertia and stiffness parameters of the variable admittance controller further comprises the following steps: where exp(•) represents the exponential function with base e, K i is a positive constant such that when , the value of represents the maximum allowed position error determined by the task parameters, the superscript i = {x, y, z} represents the component in the respective axis, and are positive constants; Wherein Step S23: a critical damping controller is designed so that each error e(t) can quickly converge to a preset region; - δ l p i (t) < e i (t) < δ r p i (t) (Equation Seven) where e i is the i-th element of the critically damped error e(t), i = {x, y, z}, δ l and δ r are normal numbers, and p(t) is a performance function, expressed as wherein and l is a strictly positive constant satisfying The rate of decrease of e is influenced by the constant l, and then the error e i is converted to an equivalent unconstrained error μ i using an error transformation; The conversion process is controlled by a function T(μ i ) defined as: e i = T(μ i )ρ i (Equation Nine) T(μ i ) is strictly increasing, its inverse function always exists and satisfies -δ l < T(μ i ) < δ r The inverse function of T(μ i ) is calculated as follows where μ i is the i-th element of the transform error μ, denoted as μ x = [μ y , μ z ] T ; furthermore, the first derivative of the transform error μ is derived as follows: where P = diag(p x , p y , p z ), r i (t) = (δ r + δ l ) / ((δ l ρ i (t) + e i )(δ r ρ i (t) - e i )), with the superscript i = {x, y, z} representing the component on the respective coordinate axis; Step S24: Selecting controller gains The critical damping error e is driven to convergence to zero using the following critically damped controller, which guarantees the predetermined performance in Equation Seven: Thus, the inertia, stiffness and damping parameters in equation three are adaptively adjusted according to the changes of the position error and the critical damping error function e(t); the attenuation factor is adjusted The balance between the stiffness and the position error is achieved, and in the process of performing the task, under the action of the adaptive gain the system always moves towards the critical damping state.

6. The energy tank based aerial work robot compliant sampling method according to claim 1, characterized in that: The prescribed performance is expressed as follows: The passive parameter design of the variable admittance controller comprises the following steps: Step S31: the total energy of the variable admittance system is expressed by a natural non-negative energy storage function: V < r e T h f (Formula Fourteen) The passivity condition requires that no internal energy is generated, and therefore the following condition must be met: In the energy conservation and passivity analysis, all the energy producing terms are combined into one expression and the dissipative terms are separated, and the formula fifteen is simplified as where Ω contains all the energy producing terms, due to the changes in the variable admittance controller stiffness and inertia; the dissipative term is represented as The dissipative term is always non-negative, because it represents the energy lost due to damping; however, due to the changes in the stiffness and inertia, p becomes positive, i.e., the controller produces energy, violating the balance condition in the formula fourteen.

7. The energy tank based aerial work robot compliant sampling method according to claim 6, characterized in that: Step S32: the time derivative of the energy storage function V(t) is calculated, and we obtain: The passive parameter design of the variable admittance controller further comprises the following steps: Step S33: after completing the analysis of system energy conservation and passivity, 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 connecting the variable admittance controller formula three with the energy tank, it is ensured that the system will not generate energy by itself, and always remains passive; the energy tank is a virtual storage, allowing the accumulation of dissipated energy for future use, represented by: wherein gain 0 < η < 1 is used to control how much of the dissipated power is stored into the tank; is the state of the tank, is the input-output pair handling 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)) ≥ 0, where 0 is a threshold value to avoid singularities in equation sixteen due to energy depletion. Step S34: Power flow of the tank The calculations by equation sixteen and equation seventeen give: wherein to keep the energy in the tank within a predefined range [T - ,T + ] and T - > 0, is designed as follows: where T - and T + are the lower and upper limits of the energy tank, respectively; Step S35: Step S34 indicates that if T(x t )∈[T - ,T + ] is violated, then the energy link between the tank and the variable admittance system will be closed; after describing the energy storage in the tank, the focus now shifts to utilizing these stored energies to achieve the variable admittance behavior; it is observed from Equation 15 that only the variable inertia and stiffness contribute energy to the variable admittance controller, which requires a conditional restriction on their variations: where the conditional variable is designed to: From the above conditions, if the energy dissipation is positive (ηd+p > 0), then the variable inertia and stiffness will act in full; otherwise, the variable inertia and stiffness depend on Step S36: Based on step S35, if the tank dynamics tank parameters and conditional variables are designed as equations eighteen, nineteen and twenty-one, respectively, then the controller is passive in terms of input-output pairs ​ 8. The energy tank based aerial work robot compliant sampling method according to claim 1, characterized in that: Step S4 specifically includes the following content: Step S41: After connecting the energy tank to the variable admittance controller, the change of energy flow in the tank will affect the admittance parameters; monitoring the energy flow helps to understand and mitigate potential dangerous behaviors of the flying operation robot; according to formula eighteen, the maximum rate of change of energy in the energy tank is defined as: wherein, and respectively represent the maximum allowed position and velocity errors determined by the task parameters, defined as follows where k r and k p are constant symmetric matrices; t > t δ and t δ > 0 represent the current time and a fixed time interval of the task, respectively; by choosing t δ , the error of the desired trajectory of the task within a certain time range is determined, is the maximum desired velocity of the task; Step S42: After completing the design of the variable boundary energy tank and proposing the boundary change adaptive rate and energy flow exchange rule, the safe energy condition of the flying operation robot is obtained by formula twenty-two, 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 to the constant parameter admittance control, the change in inertia and stiffness changes the kinetic energy, which requires adjusting the upper limit T of the energy tank + ; this upper limit is defined by combining the current lower limit and the maximum kinetic energy allocated to the current admittance parameter: Step S44: According to step S43, as long as the energy level in the energy tank remains above the lower limit of the energy tank T - Above, even if T + and T - The passivity of equation twenty is maintained over time; Therefore, when sampling, if it is determined that the safety of the flying operation robot is threatened, the task will be interrupted; otherwise, the task will continue.

9. An energy tank based aerial work robot implemented with the method of any one of claims 1 to 8; characterized by, The application conditions of the variable admittance controller based on the energy tank are: the flying operation robot suitable for the variable admittance controller is a quadrotor unmanned aerial vehicle; The quadrotor unmanned aerial vehicle has the following characteristics: 1) The internal motion controller of the quadrotor unmanned aerial vehicle can realize accurate tracking of the motion trajectory of the quadrotor unmanned aerial vehicle; 2) The end effector mechanism installed on the quadrotor unmanned aerial vehicle is a 3-DOF mechanical arm, and the sampling force at the end thereof does not exceed the maximum load that the unmanned aerial vehicle can exert.

10. The energy tank based aerial work robot according to claim 9, characterized in that, The quadrotor unmanned aerial vehicle also has the following parameter characteristics: mass of the flight platform m b The range is: 2.05 ≦ m b ≦ 2.15; wherein m b is in kg; The length l1 of the arm 1 is in the range of 0.115≦l1≦0.125; wherein the unit of l1 is m; The length l2 of the arm 2 is in the range of 0.06≦l2≦0.12; wherein the unit of l2 is m; The length l3 of the arm 3 is in the range of 0.165≦l3≦0.215; wherein the unit of l3 is m; The mass m1 of the arm 1 is in the range of 0.095≦m1≦0.115; wherein the unit of m1 is kg; The mass m2 of the arm 2 is in the range of 0.045≦m2≦0.055; wherein the unit of m2 is kg; The mass m3 of the arm 3 is in the range of 0.11≦m3≦0.13; wherein the unit of m3 is kg; initial x-axis inertia tensor I x is in the range: 0.45 ≦ I x ≦ 0.53; wherein I x is in kg·m -2 ; Initial y-axis inertia tensor I y is in the range: 0.45 ≦ I y ≦ 0.53; wherein I y is in kg·m -2 ; Initial z-axis inertia tensor I z The range of I z z-axis inertia tensor I z The units of I -2 z-axis inertia tensor I

Images