An active contact detection and control method for aerial working robots in uncertain environments
Through dynamic modeling and additional bending moment estimator design of the aerial operation robot system, combined with the force tracking impedance control strategy, the active contact detection problem of the aerial operation robot in an uncertain environment is solved, and stable and efficient contact detection capabilities are achieved. .
Patent Information
- Application Number
- CN202211295356.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-10-21
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2042-10-21
AI Technical Summary
It is difficult for aerial operation robots to achieve active contact detection in uncertain environments, and there are problems with system stability and contact force adjustment, so they cannot effectively complete infrastructure inspection and maintenance tasks.
By constructing a dynamic model of the aerial operation robot system, an additional bending moment estimator and a force tracking impedance control strategy based on the time-varying stiffness coefficient are designed, and combined with the posture controller, the active contact detection of the aerial operation robot in an uncertain environment is realized.
In the case of powerless/torque sensors, it ensures safe and reliable contact detection of air operation robots in uncertain environments, has robust interaction capabilities and low cost advantages, and achieves stable contact detection behavior and good force tracking performance.
Smart Images

Figure CN115533915B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of aerial working robots, and in particular relates to an active contact detection control method for an aerial working robot in an uncertain environment. Background Art
[0002] In the past, aerial robots were considered to lack active operational capabilities and were mainly used for passive monitoring tasks. With the advancement of aerial robot technology, the research field of aerial robots has gradually expanded to achieve active contact detection operations with the environment. This new type of aerial robot system for aerial contact detection is often called an aerial work robot, and it has attracted the exploration of many researchers. Aerial work robots have many potential applications, such as inspecting infrastructure such as bridges or manufacturing plants, contact detection tasks to press emergency switches in industrial accidents, and performing maintenance work in places that are difficult to reach. In fact, research challenges include system stability and regulation of the required contact force during the entire interaction process. In order to meet the needs of aerial work robots for infrastructure inspection and maintenance tasks, it is urgent to develop the ability of aerial work robots to actively detect contact with uncertain environments. Summary of the invention
[0003] In view of the above technical problems, the present invention provides an active contact detection control method for an aerial working robot in an uncertain environment, the purpose of which is to solve the technical problem of active contact detection between an aerial working robot and an uncertain environment in the background technology.
[0004] The technical solution adopted by the present invention to solve the technical problem is:
[0005] An active contact detection and control method for an aerial working robot in an uncertain environment, the method comprising the following steps:
[0006] S100: construct an aerial work robot system that actively contacts and detects the environment, and perform dynamic modeling analysis on the aerial work robot system to obtain a dynamic model of the aerial work robot;
[0007] S200: obtaining a desired roll and pitch angle and a conversion expression between a rotor speed and thrust / torque in the aerial work robot system according to a generalized input in a dynamic model of the aerial work robot;
[0008] S300: Design additional bending moment estimator to estimate contact interaction forces / moments;
[0009] S400: A second-order spring-mass-damper model is introduced to establish the dynamic relationship between the ideal contact force and the position of the aerial working robot, and then a force tracking impedance control strategy based on a time-varying stiffness coefficient is designed to obtain the calculated corrected position command trajectory;
[0010] S500: Obtain a command attitude according to the expected roll and pitch angles, obtain a state error according to the corrected position command trajectory and the command attitude, design an expression for the aerial working robot's posture control rate according to the state error, the aerial working robot's dynamic model, and the extra bending moment estimator, and obtain the aerial robot's posture control rate;
[0011] S600: Obtain the rotor speed according to the aerial robot posture control rate and the conversion expression between the rotor speed and thrust / torque in the aerial working robot system, so that the aerial working robot can complete the active contact detection task in the uncertain environment.
[0012] Preferably, S100 includes:
[0013] S110: construct an aerial work robot system for active contact detection with the environment, the aerial work robot interaction system includes a quadrotor drone and a rigidly mounted contact tool;
[0014] S120: Using the Newton-Euler equation method, the dynamic model of the aerial work robot is described as follows:
[0015]
[0016] in, and They are the position of the aerial work robot’s center of mass in the world inertial coordinate system and the angular velocity in the body coordinate system. Represents an entity set, m u is the total mass of the aerial work robot, Denote the rotation matrix from the body coordinate system to the world inertial coordinate system, J = diag(I φ , I θ , I ψ ) is the inertia matrix, where I φ , I θ , I ψ is the inertia constant; g is the gravity constant, e3=[0,0,1] T , the operator “×” represents the cross product; is the total lift of the aerial work robot, where T i , i = 1, ..., 4 represents the lift of each rotor; is the torque vector of the aerial working robot; and It is the external force and torque generated on the center of mass of the aerial working robot by the force and torque applied by the external environment to the tool;
[0017] S130: Define the aerial work robot posture Φ B =[φ,θ,ψ] T , the angular velocity in the world inertial coordinate system Euler angle change rate With ω B The conversion relationship between them is:
[0018]
[0019] in,
[0020] Substituting equation (2) and its time derivative into equation (1), the following kinetic model can be obtained:
[0021]
[0022] in, represents the inertia matrix, is the Coriolis and eccentric matrices, where S(·) represents the skew-symmetric matrix operator, Q(Φ B ), and
[0023] Step S140: define variables The dynamic model of the aerial work robot can be expressed in the following matrix form:
[0024]
[0025] in, represents the positive definite inertia matrix, are the Coriolis and centrifugal terms, is the gravity vector, u=[(TR B e3) T ,(Q(Φ B )TM B ) T ] T is a generalized input, is the additional force / torque acting on the center of mass of the aerial working robot, which can be expressed as:
[0026]
[0027] in, represents the interaction force / torque between the environment and the tool tip, is the tool end position in the body coordinate system;
[0028] Preferably, S200 includes:
[0029] Step S210: In view of the underactuated characteristics of the aerial working robot, the expected roll and pitch angles are calculated using the generalized inputs u(1), u(2) and u(3) in the dynamic model of the aerial working robot as follows:
[0030]
[0031] Among them, φ d ,θ d , ψ d Represent the desired roll, pitch, and yaw angles, respectively. and They represent the trigonometric function cos(ψ d ) and sin(ψ d )ψ d represents the desired yaw angle;
[0032] Step S220: Since the rotor rotation generates lift and torque, the conversion expression between the rotor speed and thrust / torque in the aerial work robot system is obtained according to the generalized input in the dynamic model of the aerial work robot:
[0033]
[0034] Among them, Ω i , i = 1, ..., 4 is the rotor speed, c T and c M Denote as lift and moment coefficients, and d represents the wheelbase length of the aerial work robot.
[0035] Preferably, S300 includes:
[0036] Step S310: construct an additional bending moment estimator intermediate auxiliary variable as:
[0037]
[0038] in, W B The estimated value of , a is the positive constant gain of the estimator;
[0039] Step S320: Ignoring the linear acceleration information measurement, by constructing the Lyapunov function, the differential form of the intermediate auxiliary variable of the estimator is designed as follows:
[0040]
[0041] Step S330: According to equations (8) and (9), the additional bending moment estimator is designed as:
[0042]
[0043] Preferably, S400 includes:
[0044] Step S410: Consider the linear stiffness as k e The environmental model of the contact force can be expressed as:
[0045]
[0046] Among them, x e It represents the x-direction position when the tool just touches the target without external force;
[0047] Step S420: According to the contact force of formula (11), the x-direction force error is defined as follows:
[0048]
[0049] in, is the expected contact force in the x-direction in the body coordinates, where f E,d is the desired force in the end tool coordinate system,
[0050] Step S430: Simulate the behavior of a human hand contacting an uncertain environment, and define a universal one-dimensional force tracking impedance control strategy with a time-varying stiffness coefficient as follows:
[0051]
[0052]
[0053] Among them, m d , b d , Respectively represent the mass, damping, and stiffness coefficients of the impedance model, and the correction value x dc Defined as x dc =x d -x c , where x d is the expected position calculated by the offline trajectory generator, x c is the corrected command position trajectory, k p and k v are the normal PD control gain, and k0 is the initial stiffness of the impedance model.
[0054] Preferably, S500 includes:
[0055] Step S510: Calculate the command attitude Φ according to the expected roll and pitch angles of equation (6): B,c , according to the command posture Φ B,c and the corrected position command trajectory x c Define the position error e p =pp c and attitude error According to the position error and attitude error, the state error is:
[0056] e=qq c (15)
[0057] in, and p c =[x c ,y c , z c ] T .
[0058] Step S520: By using the performance function ρ k (t) Define the boundaries of the error vector:
[0059]
[0060] in, δ k is a positive constant, and the performance function is defined as follows:
[0061]
[0062] Among them, the positive constant ρ ∞,k and l k are the final bound of the error and the convergence rate, respectively;
[0063] Step S530: Map the state error of the constrained space to the unconstrained space through an equivalent transfer function, and define the error transformation with predefined performance specifications as follows:
[0064]
[0065] Among them, ε k is the transmission error ε = [ε1,…,ε6] T The kth element of ;
[0066] Step S540: Construct the following terminal sliding surface to stabilize the variable ε in a finite time:
[0067]
[0068] Among them, σ and is a positive constant, satisfying σ>0 and , where sgn(·) and ||·|| represent the standard sign function and the Euclidean norm, respectively;
[0069] Step S550: Definition Given the following assumptions:
[0070]
[0071] Among them, β is an unknown positive constant;
[0072] Step S560: Combine the additional bending moment estimator of formula (10) to design the following aerial working robot posture control rate:
[0073]
[0074]
[0075]
[0076]
[0077] Among them, K0 is a positive diagonal matrix, represents the estimated value of β, λ and b are positive constants, is a diagonal matrix.
[0078] The above method can ensure the safe and reliable contact detection behavior of the aerial working robot in an uncertain contact environment without the need for force / torque sensors, and has the advantages of robust interaction capability, safety and reliability, and low cost. BRIEF DESCRIPTION OF THE DRAWINGS
[0079] Figure 1 This is a flow chart of an active contact detection control method for an aerial working robot in an uncertain environment in one embodiment of the present invention;
[0080] Figure 2 A schematic diagram of a model of an active contact detection control method for an aerial working robot in an uncertain environment in one embodiment of the present invention;
[0081] Figure 3 It is an algorithm block diagram of an active contact detection control method of an aerial working robot in an uncertain environment in one embodiment of the present invention;
[0082] Figure 4 is a track diagram of the position tracking in the xyz direction of the aerial working robot in one embodiment of the present invention in the simulation;
[0083] Figure 5 is a posture tracking trajectory diagram of the aerial working robot in one embodiment of the present invention in simulation;
[0084] Figure 6 is a schematic diagram of position tracking error in the xyz direction of an aerial working robot with predefined performance in simulation in one embodiment of the present invention;
[0085] Figure 7 is a schematic diagram of posture tracking error of an aerial working robot having predefined performance in simulation in one embodiment of the present invention;
[0086] Figure 8It is a schematic diagram of the force tracking trajectory of the aerial working robot in simulation in one embodiment of the present invention. DETAILED DESCRIPTION
[0087] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.
[0088] In one embodiment, Figures 1 to 3 As shown, an active contact detection and control method for an aerial working robot in an uncertain environment comprises the following steps:
[0089] S100: construct an aerial work robot system that actively contacts and detects the environment, and perform dynamic modeling analysis on the aerial work robot system to obtain a dynamic model of the aerial work robot.
[0090] In one embodiment, S100 includes:
[0091] S110: construct an aerial work robot system for active contact detection with the environment, the aerial work robot interaction system includes a quadrotor drone and a rigidly mounted contact tool;
[0092] S120: Using the Newton-Euler equation method, the dynamic model of the aerial work robot is described as follows:
[0093]
[0094] in, and They are the position of the aerial work robot’s center of mass in the world inertial coordinate system and the angular velocity in the body coordinate system. Represents an entity set, m u is the total mass of the aerial work robot, Denote the rotation matrix from the body coordinate system to the world inertial coordinate system, J = diag(I φ , I θ , I ψ ) is the inertia matrix, where I φ , I θ , I ψ is the inertia constant; g is the gravity constant, e3=[0,0,1] T , the operator “×” represents the cross product; is the total lift of the aerial work robot, where T i , i = 1, ..., 4 represents the lift of each rotor; is the torque vector of the aerial working robot; and It is the external force and torque generated on the center of mass of the aerial working robot by the force and torque applied by the external environment to the tool;
[0095] S130: Define the aerial work robot posture Φ B =[φ,θ,ψ] T , the angular velocity in the world inertial coordinate system Euler angle change rate With ω B The conversion relationship between them is:
[0096]
[0097] in,
[0098] Substituting equation (2) and its time derivative into equation (1), the following kinetic model can be obtained:
[0099]
[0100] in, represents the inertia matrix, is the Coriolis and eccentric matrices, where S(·) represents the skew-symmetric matrix operator, Q(Φ B ), and
[0101] Step S140: define variables The dynamic model of the aerial work robot can be expressed in the following matrix form:
[0102]
[0103] in, represents the positive definite inertia matrix, are the Coriolis and centrifugal terms, is the gravity vector, u=[(TR B e3) T ,(Q(Φ B ) T M B ) T ] T is a generalized input, is the additional force / torque acting on the center of mass of the aerial working robot, which can be expressed as:
[0104]
[0105] in, represents the interaction force / torque between the environment and the tool tip, is the tool end position in the body coordinate system.
[0106] S200: Obtain the desired roll and pitch angles and the conversion expressions between the rotor speed and thrust / torque in the aerial work robot system according to the generalized input in the dynamic model of the aerial work robot.
[0107] In one embodiment, S200 includes:
[0108] Step S210: In view of the underactuated characteristics of the aerial working robot, the expected roll and pitch angles are calculated using the generalized inputs u(1), u(2) and u(3) in the dynamic model of the aerial working robot as follows:
[0109]
[0110] Among them, φ d ,θ d , ψ d Represent the desired roll, pitch, and yaw angles, respectively. and They represent the trigonometric function cos(ψ d ) and sin(ψ d );
[0111] Step S220: Since the rotor rotation generates lift and torque, the conversion expression between the rotor speed and thrust / torque in the aerial work robot system is obtained according to the generalized input in the dynamic model of the aerial work robot:
[0112]
[0113] Among them, Ω i , i = 1, ..., 4 is the rotor speed, c T and c M Denote as lift and moment coefficients, and d represents the wheelbase length of the aerial work robot.
[0114] S300: Design additional bending moment estimator to estimate contact interaction forces / moments.
[0115] In one embodiment, S300 includes:
[0116] Step S310: construct an additional bending moment estimator intermediate auxiliary variable as:
[0117]
[0118] in, W B The estimated value of , a is the positive constant gain of the estimator;
[0119] Step S320: Ignoring the linear acceleration information measurement, by constructing the Lyapunov function, the differential form of the intermediate auxiliary variable of the estimator is designed as follows:
[0120]
[0121] Step S330: According to equations (8) and (9), the additional bending moment estimator is designed as:
[0122]
[0123] S400: A second-order spring-mass-damper model is introduced to establish the dynamic relationship between the ideal contact force and the position of the aerial working robot, and then a force tracking impedance control strategy based on time-varying stiffness coefficient is designed to obtain the calculated and corrected position command trajectory.
[0124] In one embodiment, S400 includes:
[0125] Step S410: Consider the linear stiffness as k e The environmental model of the contact force can be expressed as:
[0126]
[0127] Among them, x e It represents the x-direction position when the tool just touches the target without external force;
[0128] Step S420: According to the contact force of formula (11), the x-direction force error is defined as follows:
[0129]
[0130] in, is the expected contact force in the x-direction in the body coordinates, where f E,d is the desired force in the end tool coordinate system,
[0131] Step S430: Simulate the behavior of a human hand contacting an uncertain environment, and define a universal one-dimensional force tracking impedance control strategy with a time-varying stiffness coefficient as follows:
[0132]
[0133]
[0134] Among them, m d , b d , Respectively represent the mass, damping, and stiffness coefficients of the impedance model, and the correction value x dc Defined as x dc =x d -x c , where x d is the expected position calculated by the offline trajectory generator, xc is the corrected command position trajectory, k p and k v are the normal PD control gain, and k0 is the initial stiffness of the impedance model.
[0135] S500: Obtain a command attitude according to the expected roll and pitch angles, obtain a state error according to the corrected position command trajectory and command attitude, design an expression for the aerial working robot's posture control rate according to the state error, the aerial working robot's dynamic model, and an additional bending moment estimator, and obtain the aerial robot's posture control rate.
[0136] In one embodiment, S500 includes:
[0137] Step S510: Calculate the command attitude Φ according to the expected roll and pitch angles of equation (6): B,c , according to the command posture Φ B,c and the corrected position command trajectory x c Define the position error e p =pp c and attitude error According to the position error and attitude error, the state error is:
[0138] e=qq c (15)
[0139] in, and p c =[x c ,y c , z c ] T .
[0140] Step S520: By using the performance function ρ k (t) Define the boundaries of the error vector:
[0141]
[0142] in, δ k is a positive constant, and the performance function is defined as follows:
[0143]
[0144] Among them, the positive constant ρ ∞,k and l k are the final bound of the error and the convergence rate, respectively;
[0145] Step S530: Map the state error of the constrained space to the unconstrained space through an equivalent transfer function, and define the error transformation with predefined performance specifications as follows:
[0146]
[0147] Among them, ε k is the transmission error ε = [ε1,…,ε6] T The kth element of ;
[0148] Step S540: Construct the following terminal sliding surface to stabilize the variable ε in a finite time:
[0149]
[0150] Among them, σ and is a positive constant, satisfying σ>0 and where sgn(·) and ||·|| represent the standard sign function and the Euclidean norm, respectively;
[0151] Step S550: Definition Given the following assumptions:
[0152]
[0153] Among them, β is an unknown positive constant;
[0154] Step S560: Combine the additional bending moment estimator of formula (10) to design the following aerial working robot posture control rate:
[0155]
[0156]
[0157]
[0158]
[0159] Among them, K0 is a positive diagonal matrix, represents the estimated value of β, λ and b are positive constants, is a diagonal matrix.
[0160] S600: Obtain the rotor speed according to the aerial robot posture control rate and the conversion expression between the rotor speed and thrust / torque in the aerial working robot system, so that the aerial working robot can complete the active contact detection task in the uncertain environment.
[0161] In the present invention, the quadcopter is equipped with a rigidly connected tool. In order to obtain position feedback in an environment where GPS cannot be used, a monocular camera with a resolution of 640×480 installed on the aerial robot is used to calculate the relative position between the aerial robot and the tag. The parameters of the aerial robot are set as: m u =0.85kg, J=diag(0.0820,0.0845,0.1377)kg·m 2 , g = 9.81 kg·m / s 2 ; Select the additional moment estimator parameter as a=6, and set the initial value of the estimator auxiliary variable γ to γ(0)=0 6×1 ; Impedance control parameters are set to: m d =0.85, b d =30, k0=50, k p =1.5, k v =0.1; in the posture controller, the predefined performance parameters are set as: l k = 0.5, for k = 1, 2, 3, ρ 0,k =1.0,ρ ∞,k =0.15, for k=4, 5, 6, ρ 0,k =1.5,ρ ∞,k =0.5, the controller parameters are: σ=1, K0=diag(8,3,3,1,2,1), λ=0.001, b=2; The initial value is set to
[0162] The expected position trajectory is set as:
[0163]
[0164] The desired yaw angle is always zero, and the desired contact force is set to:
[0165]
[0166] Environmental model stiffness coefficient k e Assume that at t = 25s, the force suddenly changes from 100 N / m to 200 N / m, and at t = 65s, the force suddenly changes from 200 N / m to 100 N / m. In the initial state, the aerial working robot hovers at point p(0) = [0.4, -0.3, 1.2] T m.
[0167] like Figure 4-8 As shown, Figure 4 is a track diagram of the position tracking in the xyz direction of the aerial working robot in the simulation according to an embodiment of the present invention, Figure 5This is a posture tracking trajectory diagram of the aerial working robot in the simulation according to an embodiment of the present invention. It can be seen that the aerial working robot has good convergence speed and tracking accuracy; Figure 6 Schematic diagram of position tracking error in the xyz direction of an aerial working robot with predefined performance in simulation according to an embodiment of the present invention, Figure 7 A schematic diagram of posture tracking error of an aerial working robot with predefined performance in simulation according to an embodiment of the present invention, wherein all state errors of the system are always within the predefined performance boundary in the transient and steady-state stages, and the steady-state error is always less than the final boundary of the performance function; Figure 8 FIG. 1 is a schematic diagram of the force tracking trajectory of the aerial working robot in the simulation of the embodiment of the present invention. It can be seen that the additional bending moment estimator has a good estimation effect on the required drastic change contact force, thereby completing the active contact detection task with the environment. The interaction force has a significant effect at t = 25s and t = 65s. This is because when the environmental stiffness k e During a sudden change, the tool tip is still in the position it was at the previous moment, but it stabilizes to the required contact force in a very short time.
[0168] Compared with the prior art, the present invention has the following beneficial effects:
[0169] 1) An active contact detection control system consisting of an additional bending moment estimator, variable stiffness force tracking impedance control and posture controller is proposed, which can realize stable flight motion and active contact detection behavior of the aerial working robot;
[0170] 2) Compared with the existing schemes, the proposed additional bending moment estimator takes advantage of the design concept of minimum sensor conditions and does not use acceleration and additional force / torque measurements. Since the measurement of acceleration is usually unreliable, this method is relatively suitable for practical applications;
[0171] 3) Aiming at the problem of uncertain contact environment, the proposed variable stiffness force tracking impedance control strategy can ensure the contact stability and force tracking performance of the aerial working robot;
[0172] 4) By utilizing the knowledge of the predetermined performance and the terminal sliding surface, the predefined transient / steady-state performance and convergence in a finite time during the active contact detection process are achieved, ensuring the dynamic response speed and control accuracy of the aerial work robot system.
[0173] The above is a detailed introduction to the active contact detection control method for an aerial working robot in an uncertain environment provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.
Claims
1. An active contact detection and control method for an aerial working robot in an uncertain environment, characterized in that: The method comprises the following steps: S100: constructing an aerial working robot system capable of active contact detection with the environment, and performing dynamic modeling analysis on the aerial working robot system to obtain a dynamic model of the aerial working robot; S200: Obtaining a desired roll and pitch angle and a conversion expression between a rotor speed and thrust / torque in an aerial working robot system according to a generalized input in a dynamic model of the aerial working robot; S300: Design additional force / torque estimator to estimate contact interaction forces / torques; S400: A second-order spring-mass-damper model is introduced to establish the dynamic relationship between the ideal contact force and the position of the aerial working robot, and then a force tracking impedance control strategy based on a time-varying stiffness coefficient is designed to obtain the calculated corrected position command trajectory; S500: obtaining a command posture according to the desired roll and pitch angles, obtaining a state error according to the corrected position command trajectory and the command posture, designing an aerial working robot posture control rate expression according to the state error, the dynamic model of the aerial working robot, and the additional force / torque estimator, and obtaining the aerial robot posture control rate; S600: Obtaining the rotor speed according to the aerial robot posture control rate and the conversion expression between the rotor speed and thrust / torque in the aerial working robot system, so that the aerial working robot can complete the active contact detection task in an uncertain environment; S100 includes: S110: construct an aerial work robot system for active contact detection with the environment, the aerial work robot interaction system includes a quadrotor drone and a rigidly mounted contact tool; S120: Using the Newton-Euler equation method, the dynamic model of the aerial work robot is described as follows: in, and They are the position of the aerial work robot’s center of mass in the world inertial coordinate system and the angular velocity in the body coordinate system. Represents an entity set, m u is the total mass of the aerial work robot, Denote the rotation matrix from the body coordinate system to the world inertial coordinate system, J = diag(I φ ,I θ ,I ψ ) is the inertia matrix, where I φ , I θ , I ψ is the inertia constant; g is the gravity constant, e3=[0,0,1] T , the operator "×" represents the cross product; is the total lift of the aerial work robot, where T i , i=1,…,4 represents the lift of each rotor; is the torque vector of the aerial working robot; and It is the external force and torque generated on the center of mass of the aerial working robot by the force and torque applied by the external environment to the tool; S130: Define the aerial work robot posture Φ B =[φ,θ,ψ] T , the angular velocity in the world inertial coordinate system Euler angle change rate With ω B The conversion relationship between them is: in, Substituting equation (2) and its time derivative into equation (1), the following kinetic model can be obtained: in, represents the inertia matrix, is the Coriolis and eccentric matrices, where S(·) represents the skew-symmetric matrix operator, Q(Φ B ), and Step S140: define variables The dynamic model of the aerial work robot can be expressed in the following matrix form: in, represents the positive definite inertia matrix, are the Coriolis and centrifugal terms, is the gravity vector, is a generalized input, is the additional force / torque acting on the center of mass of the aerial working robot, which can be expressed as: in, represents the interaction force / torque between the environment and the tool tip, is the tool end position in the body coordinate system; S300 includes: Step S310: construct the intermediate auxiliary variables of the additional force / torque estimator as: in, W B The estimated value of , a is the positive constant gain of the estimator; Step S320: Ignoring the linear acceleration information measurement, by constructing the Lyapunov function, the differential form of the intermediate auxiliary variable of the estimator is designed as follows: Step S330: According to equations (8) and (9), the additional force / torque estimator is designed as:
2. The method according to claim 1, characterized in that S200 includes: Step S210: In view of the underactuated characteristics of the aerial working robot, the expected roll and pitch angles are calculated using the generalized inputs u(1), u(2) and u(3) in the dynamic model of the aerial working robot as follows: Among them, φ d ,θ d , ψ d Represent the desired roll, pitch, and yaw angles, respectively. and They represent the trigonometric function cos(ψ d ) and sin(ψ d ); Step S220: Since the rotor rotation generates lift and torque, the conversion expression between the rotor speed and thrust / torque in the aerial work robot system is obtained according to the generalized input in the dynamic model of the aerial work robot: Among them, Ω i , i=1,…,4 is the rotor speed, c T and c M Denote as lift and moment coefficients, and d represents the wheelbase length of the aerial work robot.
3. The method according to claim 2, characterized in that S400 includes: Step S410: Consider the linear stiffness as k e The environmental model of the contact force can be expressed as: Among them, x e It represents the x-direction position when the tool just touches the target without external force; Step S420: According to the contact force of formula (11), the x-direction force error is defined as follows: in, is the expected contact force in the x-direction in the body coordinates, where f E,d is the desired force in the end-tool coordinate system, Step S430: Simulate the behavior of a human hand contacting an uncertain environment, and define a universal one-dimensional force tracking impedance control strategy with a time-varying stiffness coefficient as follows: in, Respectively represent the mass, damping, and stiffness coefficients of the impedance model, and the correction value x dc Defined as x dc =x d -x c , where x d is the expected position calculated by the offline trajectory generator, x c is the corrected command position trajectory, k p and k v are the normal PD control gain, and k0 is the initial stiffness of the impedance model.
4. The method according to claim 3, characterized in that S500 includes: Step S510: Calculate the command attitude Φ according to the desired roll and pitch angles of equation (6): B,c , according to the command posture Φ B,c and the corrected position command trajectory x c Define the position error e p =pp c and attitude error Then the state error is obtained according to the position error and the attitude error: e=q-q c (15) in, and p c =[x c ,y c ,z c ] T ; Step S520: By using the performance function ρ k (t) Define the boundaries of the error vector: in, δ k is a positive constant, and the performance function is defined as follows: Among them, the positive constant ρ ∞,k and l k are the final bound of the error and the convergence rate, respectively; Step S530: Map the state error of the constrained space to the unconstrained space through an equivalent transfer function, and define the error transformation with predefined performance specifications as follows: Among them, ε k is the transmission error ε=[ε1,…,ε6] T The kth element of ; Step S540: Construct the following terminal sliding surface to stabilize the variable ε in a finite time: Among them, σ and is a positive constant, satisfying σ>0 and where sgn(·) and ‖·‖ represent the standard sign function and the Euclidean norm, respectively; Step S550: Definition Given the following assumptions: Among them, β is an unknown positive constant; Step S560: Combine the additional force / torque estimator described in formula (10) to design the following aerial working robot posture control rate: Among them, K0 is a positive diagonal matrix, represents the estimated value of β, λ and b are positive constants, is a diagonal matrix.
