Anti-disturbance control method for cable-driven flying robot

Abstract

An anti-disturbance control method for a cable-driven flying robot. The method comprises: 1. calculating a desired trajectory of a traveling mechanism, a joint angle of a robotic arm, a desired trajectory of the center of an unmanned aerial vehicle platform and an initial position of the traveling mechanism; 2. calculating a disturbance force and a disturbance torque exerted by the motion of the robotic arm on the unmanned aerial vehicle platform, and calculating a disturbance force and a disturbance torque induced by a cable, so as to obtain a total disturbance equation; 3. establishing a dynamic equation of a flying robot system, performing decoupling design to obtain a decoupled dynamic equation, and obtaining calculation formulas of a rotor lift, a rotor torque, a desired pitch angle value and a desired roll angle value; 4. constructing a system state error dynamic equation, and calculating the rotor lift and the rotor torque of a flying robot; and 5. designing a rotor rotational speed allocation matrix, and calculating a rotor rotational speed on the basis of the rotor lift, the rotor torque and the rotor rotational speed allocation matrix, so as to realize the anti-disturbance operation of the flying robot. The present invention solves the problems of stability and safety during the operation of a flying robot.

Description

Anti-disturbance control method of rope traction flying robot

[0001] The present application claims priority to the Chinese patent application filed on July 26, 2024, with the Chinese Patent Office, the number of which is 2024110099233, the title of which is "Anti-disturbance control method of rope traction flying robot", the whole content or part of which is incorporated herein by reference. TECHNICAL FIELD

[0002] The present application relates to the field of flying robot control, in particular to an anti-disturbance control method of rope traction flying robot. BACKGROUND

[0003] Major equipment in the fields of aerospace, marine vessels, rail transportation, etc. is the strategic guarantee for national defense security and national economic development, and its manufacturing level represents the core competitiveness of the national manufacturing industry. The efficient and high-quality manufacturing of large and complex components such as aircraft skin, ship cabin, and high-speed rail car body is the foundation for the development of major equipment in the fields of aerospace, marine vessels, rail transportation, etc. The above-mentioned large and complex components usually have the characteristics of super-large size, numerous processes, weak rigidity, and complex shape, and their manufacturing faces challenges such as large scale, multiple tasks, and high precision. These challenges require various processes in the entire manufacturing process to meet requirements such as large range, multi-scale, flexibility, and high precision. A rope traction flying robot system is composed of a support steel frame, a guide rail, a rope driving mechanism, a traveling mechanism, a UAV platform, a mechanical arm, and multiple ropes. Due to the multiple uncertainties and disturbances of the flight environment, as well as the limitations of sensors and other technologies, the flying robot system faces control problems such as stability, rotor speed precision control, and mechanical arm anti-disturbance during the working process, therefore, a robust anti-disturbance control method needs to be designed to improve its stability and precision during the working process. SUMMARY

[0004] The present application provides an anti-disturbance control method of rope traction flying robot to solve the technical problems mentioned in the background.

[0005] To achieve the above-mentioned purpose, the technical solution of the present application is as follows:

[0006] The present application provides an anti-disturbance control method of rope traction flying robot, comprising:

[0007] S1, planning a scanning range according to environmental information, calculating a traveling mechanism expected trajectory and a mechanical arm joint angle, a UAV platform center expected trajectory, and a traveling mechanism initial position;

[0008] S2, the migration mechanism is gradually moved from the initial position to the desired trajectory of the migration mechanism, and in this motion state, the disturbance force and disturbance torque brought by the motion of the mechanical arm to the unmanned aerial vehicle platform are calculated according to the joint angle of the mechanical arm, and the disturbance force and disturbance torque caused by the rope are calculated, and the total disturbance equation is obtained;

[0009] S3, the dynamics equation of the flight robot system is established, and the decoupling design is carried out considering the estimation error, and the decoupled dynamics equation is obtained;

[0010] S4, the system state error dynamics equation is constructed according to the decoupled dynamics equation, the total disturbance equation and the center desired trajectory of the unmanned aerial vehicle platform, and the rotor lift and rotor torque of the flight robot are calculated;

[0011] S5, the rotor speed distribution matrix is designed, the rotor speed is calculated according to the rotor lift, the rotor torque and the rotor speed distribution matrix, and the flight robot system is realized to resist disturbance when the mechanical arm and the migration mechanism move.

[0012] The beneficial effects of the present application are:

[0013] The present application patent application proposes a rope traction flight robot anti-disturbance control method aiming at the many challenges encountered in the work of the existing rope traction flight robot system for large and complex components, such as stability, insufficient rotor speed control accuracy and poor anti-interference ability, etc. The present application can effectively cope with multiple uncertainties and external disturbances of the flight environment, complete the work flow of the rope traction flight robot, thereby significantly improving the stability and accuracy of the control system in work. By optimizing the cooperative operation of the unmanned aerial vehicle platform and the mechanical arm, the present application not only improves the accuracy and efficiency of the work, but also enhances the reliability and flexibility of the whole system. BRIEF DESCRIPTION OF DRAWINGS

[0014] Fig. 1 is a flow chart of the control method in the present application;

[0015] Fig. 2 is a structural schematic diagram of the whole flight robot system in the present application;

[0016] Fig. 3 is a control principle diagram of the control method in the present application. DETAILED DESCRIPTION

[0017] In order to facilitate the understanding of the present application, the present application will be described more fully below with reference to the related drawings. The preferred embodiments of the present application are shown in the drawings. However, the present application can be realized in many other different forms, and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present application more thorough and comprehensive.

[0018] With reference to FIG. 1 and FIG. 2, the application provides a kind of rope traction flying robot anti-disturbance control method, comprising:

[0019] S1, according to the environment information planning scanning range, the expected trajectory of migration mechanism and the joint angle of mechanical arm, the expected trajectory of unmanned aerial vehicle platform center and the initial position of migration mechanism are calculated;

[0020] S2, migration mechanism is gradually moved from initial position to the expected trajectory of migration mechanism, in this motion state, the disturbance force and disturbance moment brought by mechanical arm motion to unmanned aerial vehicle platform are calculated according to the joint angle of mechanical arm, and the disturbance force and disturbance moment caused by rope are calculated, to obtain total disturbance equation;

[0021] S3, the dynamics equation of flying robot system is established, and decoupling design is carried out considering estimation error, to obtain decoupled dynamics equation, and the calculation formula of rotor lift, rotor moment, pitch angle expected value and roll angle expected value is obtained;

[0022] S4, system state error dynamics equation is constructed according to decoupled dynamics equation, total disturbance equation and the expected trajectory of unmanned aerial vehicle platform center, and the rotor lift and rotor moment of flying robot are solved out;

[0023] S5, rotor speed distribution matrix is designed, rotor speed is calculated according to rotor lift, rotor moment and rotor speed distribution matrix, to realize anti-disturbance operation of flying robot system when mechanical arm and migration mechanism move.

[0024] In some embodiments, the S1 specifically includes the following steps:

[0025] S11, first define north-east-terrestrial inertial coordinate system Σ I , unmanned aerial vehicle platform body coordinate system Σ B And the coordinate system Σ E Of mechanical arm end;

[0026] S12, according to the environment information planning scanning range and the projection of measured part scanning range on north-east-terrestrial inertial coordinate system Σ I The expected trajectory P t,d Of migration mechanism and the joint angle q m,d Of mechanical arm are calculated;

[0027] S13, and the expected trajectory p b,d Of unmanned aerial vehicle platform center and the initial position P t0 Of migration mechanism are calculated according to the expected trajectory P t,d Of migration mechanism;

[0028] S14, set the expected yaw angle ψ d Of unmanned aerial vehicle platform center.

[0029] In some embodiments, the S12 specifically comprises the following steps:

[0030] S121, acquiring the environment information of the measured object and its surroundings by using a camera, planning the scanning range required and the projection of the scanning range on the North-East-Geodetic inertial coordinate system Σ I ;

[0031] S122, determining the height of the UAV platform center according to the scanning range and the projection of the scanning range, so that the mechanical arm can make the scanning range of the scanner contain the scanning range planned in S1 in its working space;

[0032] S123, planning the scanning range according to the environment information and projecting the scanning range of the measured object on the North-East-Geodetic inertial coordinate system Σ I to calculate the expected trajectory P t,d of the moving mechanism, P t,d =[x t,d ,y t,d ,z t,d ] T ;

[0033] wherein x t,d represents a straight line (the default slope is 0.1) that increases uniformly from the initial position x t0 of the moving mechanism to the end position of the projection of the scanning range of the measured object on the X direction of the North-East-Geodetic inertial coordinate system Σ I over time; y t,d represents a corresponding fixed value set according to the projection of the scanning range of the measured object; z t,d represents a value that is always 0; and T represents the transpose of the matrix.

[0034] S124, designing the joint angle q t,d of the mechanical arm according to the expected trajectory P m,d of the moving mechanism and the height of the UAV platform center, q m,d =[q 1,d ,q 2,d ,q 3,d ,q 4,d ,q 5,d ,q 6,d ] T , so that the scanner at the end of the mechanical arm can cover the entire scanning range; wherein q 1,d , q 2,d , q 3,d , q 4,d , q 5,d , and q 6,d represent the joint angles at different positions of the mechanical arm.

[0035] In some embodiments, the expected trajectory p b,d of the UAV platform center in the S13 is calculatedThe implementation method is as follows:

[0036] The drone platform center is designed based on the height of the drone platform center on the Z-axis. B The expected position z on dr,d And center the drone platform on the X-axis. B Desired position x in the direction dr,d And the center of the drone platform is on the Y-axis. B Desired position y in the direction dr,d Set respectively to the desired trajectory P of the moving mechanism t,d x in t,d ,y t,d Same; Obtain the desired trajectory p at the center of the drone platform. b,d p b,d =[x dr,d ,y dr,d ,z dr,d ] T ;

[0037] Wherein, the coordinate axis X B and Y B The coordinate axes represent the directions from the origin O to the nose and right side of the octagonal flying robot, respectively; coordinate axis Z... B This represents the coordinate axis pointing from the origin O to the direction of gravity of the UAV platform.

[0038] In some embodiments, S2 specifically includes the following steps:

[0039] S21, The moving mechanism reaches its initial position P. t0 After stabilization, the transfer mechanism begins to move along the desired trajectory P. t,d Moving forward, the robotic arm moves according to the joint angle q of the robotic arm. m,d The movement begins; at this point, the movement of the robotic arm and the transfer mechanism will cause disturbances to the drone platform, causing the drone's center to deviate from the desired position.

[0040] S22, Based on the joint angle q of the robotic arm m,d The perturbation force and perturbation torque exerted by the robotic arm's motion on the drone platform are calculated using the following formulas:

[0041] Among them, F dis,m In the northeast inertial coordinate system Σ I The disturbance force of the robotic arm on the drone platform, In the UAV platform body coordinate system Σ B The disturbance torque of the robotic arm on the drone platform; m s Let m be the total mass of the flying robot system. manthe mass of the manipulator, denotes the vector r OC in the UAV platform body coordinate system Σ B ; the vector r OC denotes the vector from the origin O of the UAV platform body coordinate system Σ B to the center of mass of the whole flying robot system; and are the first and second derivatives of , respectively, and are the angular velocity and its derivative of the UAV platform in the UAV platform body coordinate system Σ B ; is the derivative of the UAV platform velocity; g is the gravitational acceleration constant, and e3is the unit vector [0 0 1] T ;

[0042] denotes the rotation matrix from the UAV platform body coordinate system Σ B to the north-east- earth inertial coordinate system Σ I , and its specific form is:

[0043] where c and s are the trigonometric functions cos and sin, respectively; φ, θ, ψ represent the pitch angle, roll angle, and yaw angle of the UAV platform, respectively; the attitude of the UAV platform is represented by Z-Y-X Euler angles, i.e., the attitude of the UAV platform Φ b = [φ, θ, ψ] T ;

[0044] and denote the change in the moment of inertia of the flying robot system composed of the UAV platform and the manipulator system and its derivative, and the specific formula is as follows:

[0045] where and denote the position and velocity of the center of mass of the jth link in the UAV platform body coordinate system Σ B relative to the origin O of the UAV platform body coordinate system Σ B ; is the rotation matrix from the coordinate system Σ j to the UAV platform body coordinate system Σ B ; Σ j denotes the coordinate system fixed to the jth link of the manipulator; denotes the rotation matrix from the UAV platform body coordinate system Σ B to the coordinate system Σ j ; denotes the moment of inertia of the jth link about the link coordinate axis at its center of mass; skew(·) is a mapping function of the anti-symmetric matrix of a vector; denotes the angular velocity of the jth link of the robotic arm relative to the UAV platform; m j denotes the weight of the jth link of the robotic arm; I 3*3 is a three-dimensional identity matrix;

[0046] In addition, according to the definition of the center of mass, the center of mass variation of the flying robot system and its derivative can be obtained, and the specific formula is as follows:

[0047] where n is the number of links of the robotic arm;

[0048] S23, the disturbance to the UAV platform caused by the movement of the transfer mechanism is mainly caused by the change of the rope angle. Considering that the UAV platform will follow the movement of the transfer mechanism, and the UAV platform has a large weight, the rope angle will not change too much. The rope tension can be set to 0.95 times the size of the gravity in the opposite direction, so that the UAV platform can be regarded as a micro unmanned aerial vehicle, and the lift demand of the rotor is reduced. The disturbance force and disturbance torque caused by the rope are calculated, and the specific formula is as follows: F dis,l = 0.95*m s g

[0049] where F dis,l is the tension provided by the rope to the UAV platform in the north-east inertial coordinate system Σ I , that is, the disturbance force caused by the rope, is the torque provided by the rope tension to the UAV platform in the UAV platform body coordinate system Σ B ;

[0050] The disturbance force and disturbance torque caused by the movement of the robotic arm to the UAV platform, and the disturbance force and disturbance torque caused by the rope constitute the total disturbance equation.

[0051] In some embodiments, the S3 specifically comprises the following steps:

[0052] S31, a flying robot control system is constructed, and an initial dynamics equation of the flying robot system is established according to the momentum and momentum moment theorem of the particle system. The initial dynamics equation is specifically as follows:

[0053] where, is the first derivative of p b , is the first derivative of v b , and p b = [x, y, z]T and v b = [v x ,v y ,v z ] T are the absolute position and velocity of the UAV platform in the north-east-geodetic inertial frame Σ I , respectively; F t and τ are the rotor lift force and rotor moment force, is the first derivative of the UAV attitude Φ b , Φ b = [φ, θ, ψ] T , I b is the representation of the UAV platform's rotational inertia about the body axes of the UAV platform in the UAV platform body frame Σ B , and the specific expression of T(Φ b ) is as follows:

[0054] Considering the estimation error, the updated dynamic equation can be obtained:

[0055] wherein, and are the estimated values of the disturbance force and moment of the manipulator, and are the estimation errors of the disturbance force and moment of the manipulator, respectively; and are the estimated values of the tension force and tension moment provided by the cable, and are the disturbance errors of the tension force and tension moment provided by the cable, respectively;

[0056] S32, according to the updated dynamic equation, the updated dynamic equation is composed of two subsystems of a position control subsystem and an attitude control subsystem in cascade, and the two subsystems are coupled through a rotation matrix ; through decoupling design, the dynamic equation of the system after decoupling can be obtained:

[0057] wherein, is the second derivative of p b , is the second derivative of Φ b , v1 and v2 are the defined position loop virtual input and attitude loop virtual input, e Φb = Φ b - Φ b,d is the attitude tracking error, Φ b,d represents the desired attitude angle of the UAV platform, and Φ b,d= [φ d , θ d , ψ d ] T , φ d , θ d , ψ d are the desired pitch angle, roll angle, and yaw angle, respectively. is the nonlinear connection term of the inner and outer loops after decoupling by the virtual control input, is the desired rotation matrix determined by the desired attitude angle;

[0058] and the desired value calculation formulas of the rotor lift, rotor moment, pitch angle, and roll angle are constructed.

[0059] In some embodiments, the S32 specifically comprises the following steps:

[0060] S321, defining the position loop virtual input v1 = [v 1x , v 1y , v 1z ] T :

[0061] wherein Φ b,d = [φ d , θ d , ψ d ] T represents the desired attitude angle; v 1x , v 1y , v 1z represent different virtual variables used for decoupling calculation;

[0062] The position control subsystem can be obtained by bringing the position loop virtual input v1 into the updated dynamics equation:

[0063] The rotor lift F t , the desired pitch angle φ d , and the desired roll angle θ d can be solved as:

[0064] S322, defining the attitude loop virtual input v2:

[0065] The attitude control subsystem can be feedback linearized at and without considering the disturbance term, and the desired input of the rotor moment τ can be obtained by bringing the attitude loop virtual input v2 and the updated dynamics equation:

[0066] S323, the position loop virtual input v1 and the attitude loop virtual input v2 are brought into the updated dynamic equation, and a dynamic equation of the system after decoupling can be obtained.

[0067] In some embodiments, the S4 specifically comprises the following steps:

[0068] S41, the output of the flight robot system is the absolute position p of the 3-axis b and the attitude of the UAV Φ b ; thus the state error of the system can be defined:

[0069] wherein p b,d is the expected trajectory of the center of the UAV platform, v b,d is the expected speed of the center of the UAV platform, Φ b,d is the expected attitude angle of the UAV platform, is the first derivative of the expected attitude angle of the UAV platform, i.e. the attitude angular velocity; represents the position error of the center of the UAV platform; represents the speed error of the center of the UAV platform; represents the attitude error of the UAV platform; represents the attitude angular velocity error of the UAV platform; e ψ represents the yaw angle error of the UAV platform;

[0070] S42, considering the disturbance term, the dynamic equation of the system state error can be obtained: y=Cx

[0071] wherein A, B, C, D, E represent the coefficients of the dynamic equation of the system state error;

[0072] wherein x represents the system state error, u represents the output of the H ∞ controller, and Δ represents the disturbance term; e ψ =ψ-ψ d

[0073] wherein O 3*3 represents a three-dimensional unit matrix with all numerical values being 0;

[0074] S43, the output u of the H ∞ controller is solved according to the dynamic equation of the system state error, and the output u is specifically: u=Kx=WX -1 x;

[0075] wherein K represents the feedback gain matrix; X represents the positive definite matrix; and W represents the coefficient in the solving process;

[0076] S44, solving the rotor lift F and the rotor moment τ according to the system state error dynamics equation and the calculation formula of the rotor lift and the rotor moment, the pitch angle expectation value and the roll angle expectation value. t and the rotor moment τ.

[0077] In some embodiments, the S43 comprises the following steps:

[0078] S431, decoupling the nonlinear connection term of the inner and outer loops through the virtual control input It can be proved that there is a constant σ, such that

[0079] wherein Fx=[O 3*3 O 3*3 σI 3*3 O 3*3 ],

[0080] S432, for the linear controller u=Kx, if there is a positive definite symmetric matrix P satisfying:

[0081] wherein λ is a positive constant, then for the system state error dynamics equation, the system output y is finite L2 gain stable from the disturbance Δ, and the L2 gain is less than or equal to γ, and the proof process is as follows:

[0082] The closed-loop Lyapunov function of the system error dynamics equation is defined as: V(x)=x T Px

[0083] Then:

[0084] Since the positive definite symmetric matrix P satisfies the specific condition, it can be obtained that

[0085] Therefore, the system output y is finite L2 gain stable from the disturbance Δ, and the L2 gain is equal to or less than γ;

[0086] S433, then the feedback gain matrix K can be obtained by solving the following linear matrix inequality:

[0087] wherein X is a positive definite matrix. After X and W are solved, H ∞ The control rate u of the controller is u=Kx=WX -1 x.

[0088] In some embodiments, the S5 specifically comprises the following steps:

[0089] S51, design the rotor speed distribution matrix according to the rotor installation method of the aerial robot;

[0090] S52, calculate the rotor speed according to the rotor lift F t , the rotor torque τ and the rotor speed distribution matrix to realize the anti-disturbance operation of the aerial robot system when the manipulator and the traveling mechanism move.

[0091] In some embodiments, the rotor on the unmanned aerial vehicle platform is selected from a cross-shaped eight-rotor;

[0092] The rotor speed distribution matrix in S52 is as follows:

[0093] Wherein, c T and c M are the tension coefficient and torque coefficient of the propeller respectively, d is the distance between the center of the unmanned aerial vehicle body and any rotor motor, and are the square of the speed of each rotor in the cross-shaped eight-rotor.

[0094] The present application is directed to a rope traction aerial robot system, and proposes an anti-disturbance control method; the main advantages are:

[0095] 1: A rope traction aerial robot anti-disturbance control method is proposed for the work flow and environment of the aerial robot during task execution. This method enables the aerial robot to start from the parking position and travel according to the desired trajectory, ensures the smooth movement of the aerial robot during movement, reduces safety hazards, and effectively performs various tasks for large-scale complex components.

[0096] 2: The present application solves the problems of stability and safety during the operation of the aerial robot.

[0097] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, any person skilled in the art can easily think of changes or replacements within the technical scope disclosed by the present application, which should be covered within the protection scope of the present application. Moreover, the technical solutions of each embodiment of the present application can be combined with each other, but it must be based on the realization of the ordinary skilled in the art, when the combination of technical solutions appears contradictory or unachievable, it should be considered that the combination of technical solutions does not exist, nor within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A rope-hauled flying robot disturbance rejection control method, characterized by, The application relates to a flight robot system, which comprises the following steps: S1, planning a scanning range according to environmental information, calculating a desired trajectory of a moving mechanism, a desired trajectory of a joint angle of a mechanical arm, a desired trajectory of a center of a UAV platform and an initial position of the moving mechanism; S2, the moving mechanism moves from the initial position along the desired trajectory of the moving mechanism, and in the moving state, disturbance force and disturbance torque caused by the mechanical arm to the UAV platform are calculated according to the joint angle of the mechanical arm, disturbance force and disturbance torque caused by a rope are calculated, and a total disturbance equation is obtained; S3, a dynamic equation of the flight robot system is established, and decoupling design is carried out by considering an estimation error, so that a decoupled dynamic equation is obtained; S4, a system state error dynamic equation is constructed according to the decoupled dynamic equation, the total disturbance equation and the desired trajectory of the center of the UAV platform, and rotor lift and rotor torque of the flight robot are calculated; S5, a rotor speed distribution matrix is designed, rotor speed is calculated according to the rotor lift, the rotor torque and the rotor speed distribution matrix, and the flight robot system can run against disturbance when the mechanical arm and the moving mechanism move.

2. The rope-hauled flying robot disturbance rejection control method of claim 1, wherein, The S1 specifically comprises the following steps: S11、First define the Northeast ground inertial coordinate system Σ I , unmanned aerial vehicle platform body coordinate system Σ B , and mechanical arm end coordinate system Σ E ; S12, plan the scanning range according to the environmental information and the projection of the scanning range of the measured object on the north-east inertial coordinate system Σ I P t,d and the joint angle q m,d of the robot arm S13, and according to the desired trajectory P of the transfer mechanism t,d calculating the desired trajectory p of the UAV platform center b,d and the initial position P of the transfer mechanism t0 ; S14, set the desired yaw angle ψ of the unmanned aerial vehicle platform center d .

3. The rope-hauled flying robot disturbance rejection control method of claim 2, wherein, The S12 specifically comprises the following steps: S121, using a camera to acquire the environmental information of the measured object and the environment near the measured object, planning the required scanning range and the projection of the scanning range on the north-east-geodetic inertial coordinate system Σ I ; S122, the height of the center of the UAV platform is determined according to the scanning range and the projection of the scanning range, so that the scanning range of the scanner in the working space of the mechanical arm can contain the scanning range planned in S1; S123, plan a scanning range according to environmental information and a projection of a scanning range of a measured piece on a north-east inertial coordinate system Σ I t,d t,d t,d t,d t,d T ;​​​​​​ wherein x t,d represents the initial position of the transition mechanism x t0 increases uniformly over time to the end position of the projection of the scanning range of the measured object in the X direction of the north-east inertial coordinate system Σ I ; y t,d represents the corresponding fixed value set according to the projection of the scanning range of the measured object; z t,d represents a value that is always 0; T represents the transpose of the matrix; S124, according to the desired trajectory P of the transfer mechanism t,d and the height of the center of the UAV platform design mechanical arm joint angle q m,d , m,d = [q 1,d , q 2,d , q 3,d , q 4,d , q 5,d , q 6,d ] T , so that the mechanical arm end scanner can cover the full scan range; wherein q 1,d , q 2,d , q 3,d , q 4,d , q 5,d , q 6,d indicates the joint angle of the mechanical arm at different positions.

4. The rope-hauled flying robot disturbance rejection control method of claim 3, wherein, The expected trajectory p of the center of the UAV platform is calculated in S13 b,d The implementation is as follows: The drone platform center is designed based on the height of the drone platform center on the Z-axis. B The expected position z on dr,d And center the drone platform on the X-axis. B Desired position x in the direction dr,d And the center of the drone platform is on the Y-axis. B Desired position y in the direction dr,d Set respectively to the desired trajectory P of the moving mechanism t,d x in t,d ,y t,d Same; Obtain the desired trajectory p at the center of the drone platform. b,d p b,d =[x dr,d ,y dr,d ,z dr,d ] T ; Wherein, the coordinate axis X B and Y B represent the coordinate axis pointing to the head and the right side of the octo-rotor flying robot from the coordinate origin O; the coordinate axis Z B represents the coordinate axis pointing to the gravity direction of the unmanned aerial vehicle platform from the coordinate origin O.

5. The rope-hauled flying robot disturbance rejection control method of claim 4, wherein, The S2 specifically comprises the following steps: S21, the translation mechanism moves along the desired trajectory P of the translation mechanism t,d travels while the robot arm follows the robot arm joint angles q m,d The motion starts; at this time, due to the motion of the robot arm and the motion of the translation mechanism, disturbances are brought to the UAV platform, making the UAV center deviate from the desired position; S22、 according to the mechanical arm joint angle q m,d The disturbance force and the disturbance torque brought by the motion of the mechanical arm to the unmanned aerial vehicle platform are calculated, and the specific formula is as follows: where F dis,m represents the disturbance force of the manipulator to the UAV platform in the north-east-geodetic inertial frame Σ I ​ represents the disturbance torque of the manipulator on the UAV platform in the UAV platform body coordinate system Σ B represents the disturbance torque of the manipulator on the UAV platform in the UAV platform body coordinate system Σ s m is the total mass of the flying robot system, man m is the total mass of the flying robot system, r denotes a vector OC in the UAV platform body coordinate system Σ B ; vector r OC denotes a vector from the origin O of the UAV platform body coordinate system Σ B to the center of mass of the entire flying robot system; and are respectively first and second derivatives of the function f(x) = x2+ 2x + 1, and For the unmanned aerial vehicle platform, the angular velocity of the unmanned aerial vehicle platform in the unmanned aerial vehicle platform body coordinate system Σ B and its derivative, is the derivative of the speed of the drone platform; g is the gravity acceleration constant, and e3is the unit vector [0 0 1] T ; denotes the rotation matrix from the UAV platform body coordinate system Σ B to the North-East-Geodetic-Inertial coordinate system Σ I with the specific form: where c and s are the trigonometric functions cos and sin, respectively; φ, θ, ψ represent the pitch angle, roll angle and yaw angle of the UAV platform, respectively; the attitude of the UAV platform is represented by Z-Y-X Euler angles, i.e. the attitude Φ of the UAV platform b = [φ, θ, ψ] T ; and The change in moment of inertia and its derivative of a flying robot system consisting of a drone platform and a robotic arm system are represented by the following specific formulas: wherein, and respectively represent the position and velocity of the center of mass of the jth link with respect to the origin O of the UAV platform body coordinate system Σ B respectively represent the position and velocity of the center of mass of the jth link with respect to the origin O of the UAV platform body coordinate system Σ B respectively represent the position and velocity of the center of mass of the jth link with respect to the origin O of the UAV platform body coordinate system Σ The coordinate system Σ j to the unmanned aerial vehicle platform body coordinate system Σ B is a rotation matrix; Σ j represents a coordinate system fixed to the jth link of the robot arm; represents the UAV platform body coordinate system B to the coordinate system j rotation matrix; Ijdenotes the moment of inertia of the jth link about its center of mass with respect to the link coordinate axes; skew( ) is a mapping function of the anti-symmetric matrix of a vector; denotes the rotational angular velocity of the jth link of the robotic arm with respect to the drone platform; m j denotes the weight of the jth link of the robotic arm; I 3*3 is a three-dimensional identity matrix; In addition, according to the definition of the center of mass, the center of mass variation of the flying robot system and its derivative can be obtained, and the specific formula is as follows: Wherein, n is the number of the connecting rods of the mechanical arm; S23, disturbance force and disturbance torque caused by the rope are calculated, and the specific formula is as follows: F dis,l = 0.95 * m s g where F dis,l is the tension force provided by the tether to the UAV platform in the North-East-Geodetic inertial frame Σ I , i.e. the disturbance force caused by the tether, To provide the moment to the UAV platform in the UAV platform body coordinate system Σ B by the cable tension; The disturbance force and disturbance torque caused by the mechanical arm to the UAV platform and the disturbance force and disturbance torque caused by the rope constitute the total disturbance equation.

6. The rope-hauled flying robot disturbance rejection control method of claim 5, wherein, The S3 specifically comprises the following steps: S31, construct the flight robot control system, according to the momentum and momentum theorem of particle system, establish the initial dynamics equation of the flight robot system, the initial dynamics equation is as follows: wherein, For p b the first derivative, For v b the first derivative of p b and v b are the absolute position and velocity of the UAV platform in the North-East-Down inertial frame of reference Σ I F t and τ are the rotor lift and rotor moment, the first derivative of the attitude Φ of the UAV b , I b is the moment of inertia of the UAV platform about the body axes at the origin O, represented in the body frame of the UAV platform Σ B , T(Φ b ) is given by: Taking into account the estimation error, the updated dynamics equation can be obtained: wherein and for the estimated values of the disturbance forces and torques of the robot arm, and respectively, the estimation errors of the disturbance force and torque of the robot arm; and an estimated value of the pulling force and the pulling moment provided to the rope, and Disturbance errors of the tension and the tension torque provided by the rope; S32、According to the updated dynamic equation, the updated dynamic equation is composed of a position control subsystem and a posture control subsystem, and the two subsystems are connected through a rotation matrix Coupling; through decoupling design can get the system decoupling dynamics equation: wherein For p b the second derivative of p, For the second derivative of Φ b v1 and v2 are defined position loop virtual inputs and attitude loop virtual inputs, for the attitude tracking error, Φ b,d denotes the desired attitude angle of the UAV platform, and Φ b,d = [φ d , θ d , ψ d ] T , φ d , θ d , ψ d are the desired values of the pitch angle, the roll angle, and the desired yaw angle, respectively. to decouple the nonlinear connection terms of the inner and outer loops by the virtual control input, is a desired rotation matrix determined by a desired attitude angle; And an expected value calculation formula of the rotor lift, the rotor torque, the pitch angle and the roll angle is constructed.

7. The rope-hauled flying robot disturbance rejection control method of claim 6, wherein, The S32 specifically comprises the following steps: S321, define position loop virtual input v1 = [v 1x ,v 1y ,v 1z ] T : where Φ b,d = [φ d , θ d , ψ d ] T denotes the desired attitude angles; v 1x , v 1y , v 1z denote different dummy variables used for decoupling the calculations. Substituting the virtual input v1 of the position loop into the updated dynamics equation gives the position control subsystem: The rotor lift force F can be solved t , the pitch angle desired value φ d and the roll angle desired value θ d are: S322, define the gesture ring virtual input v2: The input is used by the attitude control subsystem to determine the attitude control command in the absence of the disturbance term and When the feedback linearization is implemented, the attitude loop virtual input v2 and the updated dynamics equation can be used to obtain the desired input of the rotor moment τ: S323, the position loop virtual input v1 and the attitude loop virtual input v2 are brought into the updated dynamic equation, so that the decoupled dynamic equation of the system can be obtained.

8. The rope-hauled flying robot disturbance rejection control method of claim 7, wherein, The S4 specifically comprises the following steps: S41, the output quantity of the flight robot system is the absolute position p of 3 axes b and the attitude of the UAV Φ b ; thus the state error of the system can be defined: where p b,d is the desired trajectory of the center of the UAV platform, v b,d is the desired velocity of the center of the UAV platform, Φ b,d is the desired attitude angle of the UAV platform, and a first derivative of the desired attitude angle of the drone platform, i.e. the attitude angular velocity; represents a position error of the center of the drone platform; a velocity error representing a center of the drone platform; represents an attitude error of the drone platform; represents an error in the yaw angle of the UAV platform; and ψ represents an error in the yaw angle of the UAV platform; and S42、Considering the disturbance term, the system state error dynamics equation can be obtained: y=Cx wherein A, B, C, D, E represent coefficients of the system state error dynamics equation; where x represents a system state error, u represents H ∞ the output of the controller, Δ represents a disturbance term; e ψ = ψ - ψ d where O 3*3 denotes a three-dimensional identity matrix with all values equal to 0; S43, according to the system state error dynamics equation, H is solved out ∞ The output u of the controller is specifically: u = Kx = Wx -1 x; Wherein, K represents a feedback gain matrix; X represents a positive definite matrix; W represents a coefficient in the calculation process; S44, according to the system state error dynamics equation, the rotor lift F is solved t and the rotor torque τ.

9. The rope-hauled flying robot disturbance rejection control method of claim 8, wherein, The S5 specifically comprises the following steps: S51, a rotor speed distribution matrix is designed according to the rotor installation method of the flight robot; S52, according to the rotor lift F t , the rotor torque τ and the rotor speed distribution matrix calculate the rotor speed to achieve the anti-disturbance operation of the flying robot system when the manipulator and the traveling mechanism move.

10. The rope-hauled flying robot disturbance rejection control method of claim 9, wherein, The cross-shaped eight rotors on the UAV platform are selected; The rotor speed distribution matrix in S52 is specified as follows: wherein c T and c M are the drag coefficient and the torque coefficient of the propeller, respectively, and d is the distance between the center of the UAV body and any rotor motor, and The square of the speed of each rotor in the cross-shaped eight rotors is calculated.

Images