CN114932559A

CN114932559A - Flying robot control method and device, electronic equipment and system

Info

Publication number: CN114932559A
Application number: CN202210854385.2A
Authority: CN
Inventors: 王豪; 杨鹏; 刘振; 黄秀韦
Original assignee: Ji Hua Laboratory
Current assignee: Ji Hua Laboratory
Priority date: 2022-07-20
Filing date: 2022-07-20
Publication date: 2022-08-23
Anticipated expiration: 2042-07-20
Also published as: CN114932559B

Abstract

The application belongs to the technical field of robot control, and discloses a flying robot control method, a flying robot control device, electronic equipment and a flying robot control system, wherein a flying robot is controlled to fly to a reference suspension point, and a mechanical arm is started to perform grabbing operation so as to grab an aerial object; in the grabbing operation process, acquiring first kinematic information of a multi-rotor flight platform and acquiring second kinematic information of a mechanical arm; calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information; calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque; controlling the six-degree-of-freedom pose compensation device to output compensation force to compensate pose disturbance of the mechanical arm base; therefore, the pose control precision of the tail end of the mechanical arm can be improved, and the aerial object can be reliably grabbed.

Description

Flying robot control method and device, electronic equipment and system

Technical Field

The application relates to the technical field of robot control, in particular to a flying robot control method, a flying robot control device, electronic equipment and a flying robot control system.

Background

At present, a flying robot generally consists of a multi-rotor flying platform, a multi-joint mechanical arm and a tail end executing mechanism, and has the characteristics of compact structure, large working space, flexible movement and the like. When the flying robot controls the operation of grabbing an object, the operation process can be divided into four stages: cruise flight, target approach, hover operation, and load return.

In the hovering operation process, due to the existence of multiple uncertain influence factors, such as height nonlinearity, time-varying property and uncertainty of a dynamic model of the multi-rotor flight platform, inertial parameter changes (such as gravity center changes) in the mechanical arm movement process, wind field disturbance and the like, the pose stability of the multi-rotor flight platform is poor, the base pose of the mechanical arm is unstable, the control precision of the tail end pose of the mechanical arm is further reduced, and the grabbing operation failure is easily caused.

Disclosure of Invention

The application aims to provide a flying robot control method, a flying robot control device, electronic equipment and a flying robot control system, which can improve the pose control precision of the tail end of a mechanical arm.

In a first aspect, the application provides a flying robot control method for controlling a flying robot to perform aerial object grabbing operation, where the flying robot includes a multi-rotor flying platform, a mechanical arm, and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the mechanical arm, and the six-degree-of-freedom pose compensation device is used to adjust the pose of a mechanical arm base of the mechanical arm; the flying robot control method comprises the following steps:

A1. controlling the flying robot to fly to a reference suspension point, and starting the mechanical arm to perform grabbing operation so as to grab the aerial object;

A2. in the grabbing operation process, acquiring first kinematic information of the multi-rotor flight platform and acquiring second kinematic information of the mechanical arm;

A3. calculating disturbance force and disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information;

A4. calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque;

A5. and controlling the six-degree-of-freedom pose compensation device to output the compensation force so as to compensate pose disturbance of the mechanical arm base.

According to the control method of the flying robot, the disturbance force and the disturbance moment received by the mechanical arm base are estimated, the compensation force required to be paid by the six-degree-of-freedom position and posture compensation device when the disturbance force and the disturbance moment are compensated is further calculated, then the six-degree-of-freedom position and posture compensation device is controlled to compensate the position and posture of the mechanical arm base, and therefore the position and posture stability of the mechanical arm base is guaranteed.

Preferably, step a1 includes:

controlling the flying robot to carry out cruise flight to search the aerial object;

after the aerial object is searched, controlling the flying robot to approach the aerial object;

and when the aerial object enters the operation range of the mechanical arm, the aerial object enters a hovering operation state, and the mechanical arm is started to perform grabbing operation so as to grab the aerial object.

By carrying out staged flight control, different navigation modes can be adopted for navigation according to the characteristics of different stages, so that the efficiency and the success rate of grabbing objects in the air are improved; when the aerial object enters the operation range of the mechanical arm, the aerial object enters a hovering operation state, so that the grabbing operation is performed while the pose stability control is started, and the reliability that the mechanical arm can successfully grab the aerial object is improved.

Preferably, step a3 includes:

calculating a first disturbance force and a first disturbance torque caused by the multi-rotor flying platform according to the first kinematic information;

and calculating a second disturbance force and a second disturbance torque caused by the movement of the mechanical arm according to the second kinematic information.

Preferably, said first kinematic information comprises a first attitude acceleration of said multi-rotor flying platform; the first attitude acceleration comprises three axial translational accelerations and three axial angular accelerations of the multi-rotor flying platform;

the step of calculating a first disturbance force and a first disturbance torque induced by the multi-rotor flying platform based on the first kinematic information comprises:

calculating the first disturbance force and the first disturbance torque according to the following formulas:

wherein,

as the first disturbance force, a force that is a disturbance force,

is the first disturbance torque and is,

for the mass of the multi-rotor flying platform,

is the inertia tensor of the multi-rotor flying platform,

for translational acceleration of the multi-rotor flying platform,

、

、

three axial translational accelerations of the multi-rotor flying platform,

for the angular acceleration of the multi-rotor flying platform,

、

、

the angular accelerations are respectively three axial directions of the multi-rotor flying platform.

Preferably, the second kinematic information includes rotation angles of joints of the robot arm;

the step of calculating a second disturbance force and a second disturbance torque caused by the movement of the mechanical arm according to the second kinematic information comprises:

calculating the mass center position vector of each joint of the mechanical arm relative to the mechanical arm base according to the rotation angle of each joint of the mechanical arm;

calculating a total center of mass position vector of the robot arm relative to the robot arm base according to the following formula:

wherein,

in order to be the mass of the robot arm,

is the mass of the robot arm base,

is the first of said robot arm

The mass of each joint is determined by the mass of the joint,

is the second of the said mechanical arm

A centroid position vector of each joint relative to the robot arm base;

a total centroid position vector for the robot arm relative to the robot arm base;

calculating the second disturbance force and the second disturbance torque according to the following formulas:

wherein,

as the second disturbance force, a force that disturbs the vibration,

is the second disturbance torque,

for the mass of the object currently being grasped by the robotic arm,

in order to be the acceleration of the gravity,

and calculating the center-of-mass position vector of the object currently grabbed by the mechanical arm relative to the mechanical arm base.

Preferably, the six-degree-of-freedom pose compensation device comprises a fixed table fixedly connected with the multi-rotor flight platform and six telescopic connecting rods connected between the fixed table and the mechanical arm base, and two ends of each telescopic connecting rod are respectively connected with the fixed table and the mechanical arm base through universal hinges;

the second kinematic information comprises a second attitude acceleration of the robot arm base;

step a4 includes:

acquiring the rod length of each telescopic connecting rod;

acquiring generalized gravity borne by the mechanical arm base;

acquiring the relative angular velocity of the mechanical arm base relative to the multi-rotor flight platform;

and calculating the driving force of each telescopic connecting rod according to the second attitude acceleration, the rod length, the first disturbance force moment, the second disturbance force moment, the generalized gravity and the relative angular velocity by adopting a dynamic model based on a Newton-Euler equation.

By means of the control of the driving force of each telescopic connecting rod, dynamic compensation of the pose of the mechanical arm base can be achieved, the tracking error of the tail end of the mechanical arm can be reduced, the purpose of stable operation is achieved, and operation accuracy, stability and environmental adaptability of the aerial mechanical arm can be effectively improved.

Preferably, the step of calculating the driving force of each telescopic link according to the second attitude acceleration, the rod length, the first disturbance force, the first disturbance torque, the second disturbance force, the second disturbance torque, the generalized gravity and the relative angular velocity by using a dynamic model based on a Newton-Euler equation comprises:

calculating a length direction vector of each telescopic connecting rod according to the rod length of each telescopic connecting rod;

the jacobian matrix is calculated according to the following formula:

；

wherein,

for the purpose of the jacobian matrix,

respectively are the length direction vectors of the first to the sixth telescopic connecting rods,

the six position vectors are respectively the position vectors of the hinge points of the telescopic connecting rods and the mechanical arm base relative to the mechanical arm base;

calculating a driving force of each of the telescopic links according to the following formula:

；

；

wherein,

the driving forces of the first to sixth telescopic links respectively,

in the form of a matrix of driving forces,

is composed of

The inverse of the matrix of (a) is,

is an identity matrix of order 3 x 3,

is the inertia tensor of the base of the mechanical arm,

is the mechanical arm baseSaid second attitude acceleration of the seat is,

for the generalized gravitational forces experienced by the robot arm base,

is the relative angular velocity.

In a second aspect, the present application provides a flying robot control device for controlling a flying robot to perform an aerial object grabbing operation, wherein the flying robot comprises a multi-rotor flying platform, a mechanical arm and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the mechanical arm, and the six-degree-of-freedom pose compensation device is used for adjusting the pose of a mechanical arm base of the mechanical arm; the flying robot control device includes:

the first execution module is used for controlling the flying robot to fly to a reference suspension point and starting the mechanical arm to perform grabbing operation so as to grab the aerial object;

the first acquisition module is used for acquiring first kinematic information of the multi-rotor flight platform and second kinematic information of the mechanical arm in the grabbing operation process;

the first calculation module is used for calculating the disturbance force and the disturbance torque of the mechanical arm base according to the first kinematic information and the second kinematic information;

the second calculation module is used for calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque;

and the first control module is used for controlling the six-degree-of-freedom pose compensation device to output the compensation force so as to compensate pose disturbance of the mechanical arm base.

According to the flying robot control device, the disturbance force and the disturbance torque received by the mechanical arm base are estimated, the compensation force required to be paid by the six-degree-of-freedom pose compensation device when the disturbance force and the disturbance torque are compensated is calculated, then the six-degree-of-freedom pose compensation device is controlled to compensate the pose of the mechanical arm base, so that the pose stability of the mechanical arm base is ensured, the mechanical arm is controlled to perform grabbing operation on the basis, the pose control precision of the tail end of the mechanical arm can be improved, and the aerial object can be reliably grabbed.

In a third aspect, the present application provides an electronic device comprising a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor executes the computer program to execute the steps of the flying robot control method.

In a fourth aspect, the application provides a flying robot control system, comprising a flying robot, a position tracker, and a ground station, wherein the flying robot and the position tracker are both in communication connection with the ground station;

the position tracker is used for measuring the position of the flying robot and sending the position to the flying robot through the ground station;

flying robot includes host system, many rotor flight platforms, arm and connects many rotor flight platforms with six degree of freedom position appearance compensation arrangement between the arm, six degree of freedom position appearance compensation arrangement are used for adjusting the position appearance of the arm base of arm, host system is used for:

controlling the flying robot to fly to a reference suspension point, and starting the mechanical arm to perform grabbing operation so as to grab an aerial object;

in the grabbing operation process, acquiring first kinematic information of the multi-rotor flight platform and acquiring second kinematic information of the mechanical arm;

calculating the disturbance force and the disturbance torque of the mechanical arm base according to the first kinematic information and the second kinematic information;

calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque;

and controlling the six-degree-of-freedom pose compensation device to output the compensation force to compensate pose disturbance of the mechanical arm base.

Advantageous effects

According to the flying robot control method, the flying robot control device, the flying robot control electronic equipment and the flying robot control system, the disturbance force and the disturbance moment received by the mechanical arm base are estimated, the compensation force required to be paid by the six-degree-of-freedom pose compensation device when the disturbance force and the disturbance moment are compensated is calculated, then the six-degree-of-freedom pose compensation device is controlled to compensate the pose of the mechanical arm base, so that the pose stability of the mechanical arm base is ensured, the mechanical arm is controlled to perform grabbing operation on the basis, the pose control precision of the tail end of the mechanical arm can be improved, and the aerial object can be reliably grabbed.

Additional features and advantages of the present application will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by the practice of the embodiments of the present application. The objectives and other advantages of the application may be realized and attained by the structure particularly pointed out in the written description and drawings.

Drawings

Fig. 1 is a flowchart of a flying robot control method according to an embodiment of the present application.

Fig. 2 is a schematic structural diagram of a flying robot control device according to an embodiment of the present application.

Fig. 3 is a schematic structural diagram of a flying robot control system according to an embodiment of the present application.

Fig. 4 is a schematic structural diagram of the flying robot.

Fig. 5 is a schematic structural view of the six-degree-of-freedom pose compensation device and the mechanical arm.

Fig. 6 is a schematic structural diagram of an electronic device according to an embodiment of the present application.

Description of reference numerals: 1. a first execution module; 2. a first acquisition module; 3. a first calculation module; 4. a second calculation module; 5. a first control module; 90. an airborne object; 301. a processor; 302. a memory; 303. a communication bus; 400. a flying robot; 401. a main control module; 402. a multi-rotor flying platform; 403. a mechanical arm; 4031. a two degree of freedom joint; 4032. a first link; 4033. a second link; 4034. a third link; 4035. a connecting rod rest; 4036. a first joint motor; 4037. an arm lever; 4038. a second joint motor; 404. a first IMU module; 405. a tracker target; 406. a pose compensation device with six degrees of freedom; 407. a fixed table; 408. a mechanical arm base; 409. a telescopic connecting rod; 4091. a universal hinge; 4092. a linear push rod; 4093. a DC servo motor; 410. a collapsible landing gear; 411. a flight controller module; 412. a GNSS positioning module; 500. a position tracker; 600. a ground station; 700. a vision device; 800. a second IMU module; 900. a clamping jaw.

Detailed Description

The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, as presented in the figures, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present application without making any creative effort, shall fall within the protection scope of the present application.

It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined or explained in subsequent figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not to be construed as indicating or implying relative importance.

Referring to fig. 1, fig. 1 illustrates a flying robot control method for controlling a flying robot to perform an aerial object grabbing operation according to some embodiments of the present application, where the flying robot includes a multi-rotor flying platform, a robot arm, and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the robot arm, where the six-degree-of-freedom pose compensation device is used to adjust a pose of a robot arm base of the robot arm; the flying robot control method includes the steps of:

A1. controlling the flying robot to fly to a reference suspension point, and starting the mechanical arm to perform grabbing operation so as to grab an aerial object;

A2. in the grabbing operation process, first kinematic information of the multi-rotor flight platform is obtained, and second kinematic information of the mechanical arm is obtained;

A3. calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information;

A5. and controlling the six-degree-of-freedom pose compensation device to output compensation force to compensate pose disturbance of the mechanical arm base.

According to the flying robot control method, the disturbance force and the disturbance torque received by the mechanical arm base are estimated, the compensation force required to be paid by the six-degree-of-freedom pose compensation device when the disturbance force and the disturbance torque are compensated is calculated, then the six-degree-of-freedom pose compensation device is controlled to compensate the pose of the mechanical arm base, so that the pose stability of the mechanical arm base is ensured, the mechanical arm is controlled to perform grabbing operation on the basis, the pose control precision of the tail end of the mechanical arm can be improved, and reliable grabbing of objects in the air is realized.

The flying robot control method can be applied to a main control module 401 of a flying robot 400 of a flying robot control system shown in fig. 3 to 5 (the specific structure of the flying robot control system is described later) to control the flying robot 400 to grab an aerial object 90, wherein the aerial object 90 is an object fixedly placed in the air (for example, an object placed on the top of an overhead facility). The flying robot 400 may include one or more mechanical arms 403, for example, the flying robot 400 in fig. 4 includes two mechanical arms 403.

The reference suspension point can be set in advance according to the position of an aerial object, so that the flight track from the initial position to the reference suspension point can be planned in advance according to the initial position of the flying robot, the flying robot is controlled to fly to the reference suspension point according to the flight track, and the mechanical arm is started to perform grabbing operation. Thus, in some embodiments, step a1 includes:

controlling the flying robot to fly to a preset reference suspension point according to a preset flying track;

and entering a hovering operation state, and starting the mechanical arm to perform grabbing operation so as to grab the aerial object.

Wherein, at the flight in-process, the accessible sets up the position module at many rotor flight platforms and obtains many rotor flight platform's position appearance. For example, the flying robot control in fig. 3-5 includes a position tracker 500 and a ground station 600, a first IMU module 404 and a tracker target 405 are disposed on a multi-rotor flying platform 402 of a flying robot 400, wherein the first IMU module 404 is configured to measure a pose angle of the multi-rotor flying platform 402 (and in fact, a pose angular velocity and a pose angular acceleration of the multi-rotor flying platform 402), the position tracker 500 is configured to measure a position of the multi-rotor flying platform 402 in cooperation with the tracker target 405, and the measurement result is sent to the flying robot 400 through the ground station 600, so as to obtain a pose of the multi-rotor flying platform 402.

In other embodiments, step a1 includes:

controlling the flying robot to carry out cruise flight so as to search for an aerial object;

when the aerial object enters the operation range of the mechanical arm, the suspension operation state is entered, and the mechanical arm is started to carry out grabbing operation so as to grab the aerial object.

By carrying out staged flight control, different navigation modes can be adopted for navigation according to the characteristics of different stages, so that the efficiency and the success rate of grabbing objects in the air are improved; when the aerial object enters the operation range of the mechanical arm, the aerial object enters a hovering operation state, so that the grabbing operation is performed while the pose stability is controlled, and the reliability that the mechanical arm can successfully grab the aerial object is improved. For such an embodiment, the aircraft robot pose point at the time of entering the hover operational state may be taken as the reference hover point.

The flying robot is provided with a vision device, the flying robot can fly in a cruising manner according to a preset cruising route, and searches for an aerial object through the vision device in the cruising and flying process until the aerial object is searched, and the position of the aerial object is obtained through an image recognition method (the position of the object obtained through the image recognition method is the prior art, and the detailed description is not given here). Through the mode, the accurate position of the aerial object does not need to be known in advance, the position of the aerial object can be automatically acquired by the flying robot only by knowing the area where the aerial object is located and setting a proper cruising route (such as a snake-shaped route, a spiral route and the like) in the area, and then a basis is provided for the subsequent approaching and grabbing processes, so that the automation degree is higher, and the use is more convenient.

After the airborne object is searched, the position of the airborne object and the real-time position of the multi-rotor flying platform are obtained, then an approach route (generally, but not limited to, a straight route) from the real-time position to the position of the airborne object is planned, and then the flying robot is controlled to approach the airborne object along the approach route. In some embodiments, the process of controlling the flying robot to approach the airborne object along the approach path includes: the flying robot is controlled to approach the aerial object at a first speed (which can be set according to actual needs) until the distance between the flying robot and the aerial object is less than a first preset distance (which can be set according to actual needs), and then the flying robot is controlled to reduce the speed to continue approaching the aerial object. Namely, when the flying robot is close to the aerial object, the speed is reduced, so that the flying robot can enter a hovering state in time in the following process, and the collision with the aerial object is avoided. In the process of approaching the aerial object, the attitude angle of the flying robot can be adjusted according to the pose of the aerial object relative to the flying robot, so that the visual device of the flying robot faces the aerial object, and whether the aerial object enters the operation range of the mechanical arm or not can be determined subsequently.

In the flying robot having two arms, the working range of the arm refers to the cooperative working space range of the two arms (i.e., the overlapping region of the working space ranges of the two arms). Generally, the actual operation range of the mechanical arm can be expanded forward to obtain an expanded area, after the aerial object enters the expanded area, the flying robot gradually decelerates until hovering (the flying speed decreases to a preset speed threshold value, which indicates that the flying robot enters the hovering operation state), and the distance expanded forward can be set according to actual needs to ensure that the aerial object is located within the actual operation range of the mechanical arm when the flying robot enters the hovering operation state.

Wherein, the position appearance of the arm base after compensating through six degrees of freedom position appearance compensation arrangement has higher stability, when snatching the operation, can refer to the control process of the arm of arm base fixed position to the control process of arm, mainly includes: the method comprises the steps of acquiring the pose (hereinafter referred to as a first target pose) of a grabbing point (the grabbing point is identified by an existing image identification method) on an aerial object relative to a camera coordinate system of a vision device by using the vision device, calculating the pose (hereinafter referred to as a second target pose) of the grabbing point on a mechanical arm base coordinate system according to a pose transformation matrix (obtained by pre-calibration) between the camera coordinate system and the mechanical arm base coordinate system and the first target pose, calculating the rotation angle of each joint of the mechanical arm by a kinematic inverse transformation algorithm (which is the prior art and is not detailed here) according to the second target pose, and driving each joint of the mechanical arm to rotate to a corresponding rotation angle.

In this embodiment, step a3 includes:

calculating a first disturbance force and a first disturbance torque caused by the multi-rotor flight platform according to the first kinematic information;

In fact, when the flying robot grabs the target object, the disturbance force and the disturbance torque received by the mechanical arm base comprise two parts: one part is the pose change of the multi-rotor flying platform under the uncertain conditions of wind field disturbance and a nonlinear system, so that the force and moment change brought by the force and the moment change are transferred to the mechanical arm base; the other part is the gravity and moment of the mechanical arm and the load when the mechanical arm carries out grabbing operation. Correspondingly, the total disturbance of the mechanical arm base is composed of a flight platform disturbance item and a joint motion and load mass disturbance item. Therefore, the first disturbance force and the first disturbance torque caused by the multi-rotor flight platform can be calculated through the first kinematic information of the multi-rotor flight platform, and the second disturbance force and the second disturbance torque caused by the movement of the mechanical arm can be calculated through the second kinematic information of the mechanical arm.

Specifically, the first kinematic information includes a first attitude acceleration of the multi-rotor flying platform; first attitude accelerations include translational accelerations in three axes and angular accelerations in three axes of the multi-rotor flying platform (where the three axes refer to the three axes of the world coordinate system and the first attitude accelerations of the multi-rotor flying platform may be measured by a sensor, such as the first IMU module 404 for flying robot 400 in fig. 4);

the step of calculating a first disturbance force and a first disturbance torque caused by the multi-rotor flying platform according to the first kinematic information comprises:

the first disturbance force and the first disturbance torque are calculated according to the following formulas:

wherein,

as the first disturbing force,

is the first disturbance torque,

is the mass of the multi-rotor flying platform,

is the inertia tensor of the multi-rotor flying platform,

is the translational acceleration of the multi-rotor flying platform,

、

、

three axial translational accelerations of the multi-rotor flying platform respectively,

for the angular acceleration of a multi-rotor flying platform,

、

、

the angular accelerations are respectively the three axial directions of the multi-rotor flight platform.

Specifically, the second kinematic information includes rotation angles of joints of the mechanical arm;

the step of calculating a second disturbance force and a second disturbance torque caused by the movement of the mechanical arm according to the second kinematic information comprises the following steps:

calculating the centroid position vector of each joint of the mechanical arm relative to the mechanical arm base according to the rotation angle of each joint of the mechanical arm (the specific calculation method is prior art, and the detailed description is not given here);

calculating the total mass center position vector of the mechanical arm relative to the mechanical arm base according to the following formula:

wherein,

in order to provide the mass of the robot arm,

in order to provide the quality of the mechanical arm base,

is the first of a robot arm

The mass of each joint is determined by the mass of the joint,

is the first of a robot arm

The centroid position vector of each joint relative to the mechanical arm base,

is the total mass center position vector of the mechanical arm relative to the mechanical arm base;

calculating a second disturbance force and a second disturbance torque according to the following formulas:

wherein,

as a second disturbance force, the first disturbance force,

is the second disturbance torque,

for the mass of the object currently being grasped by the robotic arm,

in order to be the acceleration of the gravity,

the center of mass position vector of the object currently grabbed by the mechanical arm relative to the mechanical arm base is obtained.

Before the grabbing operation is started, the mechanical arm is in a folding state, so that the mass center of the mechanical arm is closest to the multi-rotor flight platform, the interference of the flying robot in the moving process is reduced, the grabbing operation process comprises a mechanical arm extending stage and a mechanical arm retracting stage, the mechanical arm extends to an aerial object from the folding state in the mechanical arm extending stage, the mechanical arm drives the grabbed aerial object to retract in the mechanical arm retracting stage, and the pose of the mechanical arm base needs to be stably controlled in the whole grabbing operation process. Thus, before the aerial object is grabbed (i.e. in the robot arm protraction phase),

is a non-volatile organic compound (I) with a value of 0,

zero vector, after the airborne object is grabbed (i.e. in the robot retraction phase),

as to the mass of the airborne object(s),

is the centroid position vector of the air object relative to the mechanical arm base.

Wherein if the mass of the airborne object is a known value (e.g. the mass of the airborne object is known in advance and the known value is sent to the flying robot when assigning a task to the flying robot), then in the robot retraction phase,

equal to the known value. If the mass of the aerial object is unknown, the maximum load mass of the mechanical arm can be used as the estimated value of the mass of the aerial object, so that

Or identifying the type and the volume of the aerial object through a vision device, calculating the mass of the aerial object according to the average density corresponding to the type of the aerial object and the volume of the aerial object to obtain a mass estimated value, and controlling the vision device to obtain the mass estimated value

Equal to the quality estimate. Wherein, the centroid position of the aerial object can be identified through the vision device, and then the position vector of the centroid position relative to the mechanical arm base is calculated and used as

The value of (c).

In some embodiments, the six-degree-of-freedom pose compensation device is the six-degree-of-freedom pose compensation device 406 shown in fig. 5, the six-degree-of-freedom pose compensation device 406 includes a fixed platform 407 fixedly connected to the multi-rotor flight platform 402 and six telescopic links 409 connected between the fixed platform and the robot arm base 408, and both ends of the telescopic links 409 are respectively connected to the fixed platform 407 and the robot arm base 408 through universal hinges 4091; hereinafter, step a4 will be described based on this six-degree-of-freedom pose compensation apparatus.

In the cruising flight stage and the stage of approaching an aerial object, the telescopic connecting rod 409 can be completely contracted (namely, contracted to the shortest), and meanwhile, the mechanical arm 403 is in a folding state, so that the operation performance (or maneuvering performance) of the flying robot is improved, the windward area can be reduced, and the wind interference is reduced.

Wherein the second kinematic information further includes a second attitude acceleration of the robot arm base; the second attitude acceleration includes translational acceleration of the robot base in three axes and angular acceleration in three axes (the three axes are three axes of the world coordinate system, and the second attitude acceleration of the robot base can be measured by a sensor, such as the second IMU module 800 in the case of the flying robot 400 in FIG. 4)

Step a4 includes:

acquiring the rod length of each telescopic connecting rod;

acquiring generalized gravity borne by a mechanical arm base;

acquiring the relative angular speed of the mechanical arm base relative to the multi-rotor flight platform;

and calculating the driving force of each telescopic connecting rod (namely the compensation force required to be paid by the six-degree-of-freedom pose compensation device comprises the driving force of each telescopic connecting rod) by adopting a dynamic model based on a Newton-Euler equation according to the second pose acceleration, the rod length, the first disturbance force, the first disturbance moment, the second disturbance force, the second disturbance moment, the generalized gravity and the relative angular velocity.

By means of the mode, the driving force of each telescopic connecting rod is controlled, dynamic compensation of the pose of the mechanical arm base can be achieved, the tracking error of the tail end of the mechanical arm can be reduced, the purpose of stable operation is achieved, and operation accuracy, stability and environmental adaptability of the aerial mechanical arm can be effectively improved.

Each telescopic connecting rod is provided with a sensor for measuring the length of the telescopic connecting rod (for example, the telescopic connecting rod 409 in fig. 5 comprises a linear push rod 4092 and a direct current servo motor 4093, the direct current servo motor 4093 is provided with a rotary encoder, the direct current servo motor 4093 is used for driving the linear push rod 4092 to stretch and retract, and the rotary encoder is a sensor for measuring the length of the telescopic connecting rod), and the rod length of the telescopic connecting rod at the current moment can be measured by the sensor.

Wherein the mechanical arm base is subjected to generalized gravity

Is the generalized mass for the robot arm base multiplied by the gravitational acceleration, wherein the generalized mass of the robot arm base is the mass comprising the robot arm base itself

And mounted on machinesThe mass of the sensor modules (e.g., vision sensor, positioning sensor, etc.) on the arm base, and the like.

Wherein the first kinematic information may further include a first attitude angular velocity of the multi-rotor flying platform (including three axial angular velocities of the world coordinate system, measurable by a sensor, such as the first IMU module), and the second kinematic information may further include a second attitude angular velocity of the robot arm base (including three axial angular velocities of the world coordinate system, measurable by a sensor, such as the second IMU module); therefore, the relative angular velocity of the mechanical arm base relative to the multi-rotor flight platform can be obtained by subtracting the first attitude angular velocity from the second attitude angular velocity

。

Further, the step of calculating the driving force of each telescopic link according to the second attitude acceleration, the rod length, the first disturbance force, the first disturbance moment, the second disturbance force, the second disturbance moment, the generalized gravity and the relative angular velocity by using a dynamical model based on a Newton-Euler equation comprises the following steps of:

calculating the length direction vector of each telescopic connecting rod according to the rod length of each telescopic connecting rod;

the jacobian matrix is calculated according to the following formula:

；

wherein,

in the form of a jacobian matrix,

respectively six telescopic connecting rods and mechanical arm baseThe position vector of the hinge point relative to the mechanical arm base;

the driving force of each telescopic link is calculated according to the following formula:

（1）；

；

wherein,

the driving forces of the first to sixth telescopic links respectively,

in the form of a matrix of driving forces,

is composed of

The inverse of the matrix of (a) is,

is an identity matrix of order 3 x 3,

is the inertia tensor of the base of the mechanical arm,

is the second attitude acceleration of the robot arm base,

in order to realize the generalized gravity to which the mechanical arm base is subjected,

is the relative angular velocity.

Wherein, it is toIn the sixth degree of freedom posture compensation device

The positions of two upper and lower hinged points of each telescopic connecting rod under the coordinate system of the mechanical arm base are respectively

、

，

Wherein the position of the lower hinge point on the robot arm base is fixed and known

The position of the upper hinge point on the multi-rotor flight platform is fixed and known, a pose transformation matrix between the multi-rotor flight platform and a mechanical arm base coordinate system can be calculated according to the length of each telescopic link, and then the pose transformation matrix and the position of the upper hinge point on the multi-rotor flight platform are calculated to obtain

And then, the direction vector of the length direction of each telescopic connecting rod can be calculated according to the following formula:

；

wherein,

is as follows

The upper hinge point of each telescopic connecting rod is a position vector under a mechanical arm base coordinate system,

is as follows

The lower hinge point of each telescopic connecting rod is a position vector under the coordinate system of the mechanical arm base,

is a first

The distance between the upper and lower hinge points of each telescopic connecting rod.

Wherein, the lower hinge point of the telescopic connecting rod is the hinge point with the mechanical arm base, therefore,

。

wherein the Newton-Euler equation of the mechanical arm base is as follows:

（2）；

wherein,

is the translational acceleration of the base of the mechanical arm (which comprises the translational acceleration of three axial directions of a world coordinate system),

is the moment that the base of the mechanical arm is subjected to,

for angular acceleration of the mechanical arm base relative to the multi-rotor flying platform,

is as follows

The driving force of the telescopic connecting rod.

The dynamic model of the mechanical arm base is as follows:

（3）；

wherein,

the pose change speed of the robot arm base (measurable by a sensor on the robot arm base, such as a second IMU module),

the coefficient matrix of the centripetal force and the Coriolis force of the mechanical arm base is obtained; in practical application, because the rotating angular velocity of the multi-rotor flight platform is small, the centripetal force and the coriolis force of the mechanical arm base can be ignored, and thus the formula (3) is degenerated as follows:

（4）；

combining equations (2) and (4), equation (1) can be obtained.

Through the mode, in the action process of the mechanical arm, the driving force (including the size and the direction) of each telescopic connecting rod is adjusted in real time to control the telescopic connecting rods to stretch, the stretching length of each telescopic connecting rod is adjusted by controlling the driving force substantially, the mechanical arm base generates the motion opposite to disturbance, the deviation of the actual output pose and the expected pose of the mechanical arm base is minimized, the influence of external interference on the pose of the mechanical arm base is avoided, and the error between the track of the mechanical arm end effector and the planning track (namely the track obtained by real-time planning according to the position of an object in the air during grabbing operation) is improved.

In some preferred embodiments, after step a2, the method further comprises the steps of:

obtaining a preset time of a multi-rotor flight platform taking the current time as a termination pointTime section

（

Can be set according to actual needs) the total attitude angle change amount and the total position change amount;

and if the total attitude angle change amount exceeds a preset angle change threshold value and/or the total position change amount exceeds a preset position change threshold value, controlling the flying robot to fly to a new reference suspension point, and restarting the mechanical arm to perform grabbing operation so as to grab the aerial object.

If the total attitude angle change amount exceeds the preset angle change threshold and/or the total position change amount exceeds the preset position change threshold, it indicates that the attitude change of the flying robot is likely to exceed the telescopic length and telescopic speed constraints of each telescopic connecting rod or exceeds the telescopic length and telescopic speed constraints of each telescopic connecting rod, and the grabbing failure is caused, so that the grabbing operation is restarted by replanning the reference suspension point, and the reliability of the grabbing operation is further ensured.

Wherein, the multi-rotor flight platform is in the preset time period taking the current time as the termination point

The total change amount of the inner attitude angles is as follows:

，

the total amount of the change of the attitude angle,

is composed of

The first attitude angular velocity of the multi-rotor flying platform at the moment,

is the starting time of the preset time period taking the current time as the ending point.

The total amount of position change in (a) is:

，

in order to be the total amount of position change,

is composed of

A first translational velocity of the multi-rotor flying platform at a time (the first kinematic information may include the first translational velocity of the multi-rotor flying platform, which may be measured by a sensor, such as the first IMU module or position tracker 500).

It should be noted that, in the whole process of the control of the flying robot, the pose of the multi-rotor flying platform can be compensated according to the existing pose compensation method to keep the multi-rotor flying platform stable to a certain extent, under the condition of various uncertain influence factors, the pose of the multi-rotor flying platform compensated according to the existing pose compensation method generally has relatively large deviation, the deviation is the pose disturbance amount required to be compensated for the mechanical arm base, and the pose stability of the mechanical arm base can be improved by compensating the mechanical arm base, so that the accuracy of the control of the pose at the tail end of the mechanical arm can be improved.

According to the flying robot control method, the flying robot is controlled to fly to the reference suspension point, and the mechanical arm is started to perform grabbing operation so as to grab an aerial object; in the grabbing operation process, acquiring first kinematic information of a multi-rotor flight platform and acquiring second kinematic information of a mechanical arm; calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information; calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque; controlling the six-degree-of-freedom pose compensation device to output compensation force to compensate pose disturbance of the mechanical arm base; therefore, the pose control precision of the tail end of the mechanical arm can be improved, and the aerial object can be reliably grabbed.

Referring to fig. 2, the present application provides a flying robot control device for controlling a flying robot to perform an aerial object grabbing operation, the flying robot comprising a multi-rotor flying platform, a mechanical arm, and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the mechanical arm, the six-degree-of-freedom pose compensation device being configured to adjust the pose of a mechanical arm base of the mechanical arm; the flying robot control device includes:

the first execution module 1 is used for controlling the flying robot to fly to a reference suspension point and starting a mechanical arm to perform grabbing operation so as to grab an aerial object;

the first acquisition module 2 is used for acquiring first kinematic information of the multi-rotor flight platform and acquiring second kinematic information of the mechanical arm in the grabbing operation process;

the first calculation module 3 is used for calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information;

the second calculation module 4 is used for calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque;

and the first control module 5 is used for controlling the six-degree-of-freedom pose compensation device to output compensation force so as to compensate pose disturbance of the mechanical arm base.

The flying robot control device can be applied to a main control module 401 of a flying robot 400 of a flying robot control system shown in fig. 3 to 5 (the specific structure of the flying robot control system is described later) to control the flying robot 400 to grab an aerial object 90, wherein the aerial object 90 is an object fixedly placed in the air (for example, an object placed on the top of an overhead facility). The flying robot 400 may include one or more mechanical arms 403, for example, the flying robot 400 in fig. 4 includes two mechanical arms 403.

The reference suspension point can be set in advance according to the position of an aerial object, so that the flight track from the initial position to the reference suspension point can be planned in advance according to the initial position of the flying robot, the flying robot is controlled to fly to the reference suspension point according to the flight track, and the mechanical arm is started to perform grabbing operation. Thus, in some embodiments, the first execution module 1 is configured to, when controlling the flying robot to fly to the reference suspension point and starting the mechanical arm to perform a grabbing operation to grab an aerial object, execute:

In other embodiments, the first execution module 1 is configured to execute, when controlling the flying robot to fly to the reference suspension point and starting the mechanical arm to perform a grabbing operation to grab an aerial object:

when the aerial object enters the operation range of the mechanical arm, the robot enters a hovering operation state, and the mechanical arm is started to perform grabbing operation so as to grab the aerial object.

By carrying out staged flight control, different navigation modes can be adopted for navigation according to the characteristics of different stages, so that the efficiency and the success rate of grabbing objects in the air are improved; when the aerial object enters the operation range of the mechanical arm, the robot enters a hovering operation state, so that the robot starts to perform grabbing operation while performing pose stability control, and the reliability that the mechanical arm can successfully grab the aerial object is improved. For such an embodiment, the aircraft robot pose point at the time of entering the hover operational state may be taken as the reference hover point.

The flying robot is provided with a vision device, the flying robot can cruise and fly according to a preset cruising route, and searches for an aerial object through the vision device in the cruising and flying process until the aerial object is searched, and the position of the aerial object is obtained through an image recognition method (the position of the object obtained through the image recognition method is the prior art, and the detailed description is not given here). Through this kind of mode, need not to learn the accurate position of aerial object in advance, only need know the region that aerial object was located to set up suitable cruising route (for example snakelike route, spiral route etc.) in this region and can obtain the position of aerial object by flying robot is automatic, and then provide the basis for subsequent approaching and snatching the process, degree of automation is higher, and it is more convenient to use.

Wherein, the position appearance of the arm base after compensating through six degrees of freedom position appearance compensation arrangement has higher stability, when snatching the operation, can refer to the control process of the arm of arm base fixed position to the control process of arm, mainly includes: the method comprises the steps of acquiring the pose (hereinafter referred to as a first target pose) of a grabbing point (the grabbing point is identified by an existing image identification method) on an aerial object relative to a camera coordinate system of a vision device by using the vision device, calculating the pose (hereinafter referred to as a second target pose) of the grabbing point on a mechanical arm base coordinate system according to a pose transformation matrix (obtained by pre-calibration) between the camera coordinate system and the mechanical arm base coordinate system and the first target pose, calculating the rotation angle of each joint of a mechanical arm by a kinematic inverse transformation algorithm (the prior art, which is not detailed here) according to the second target pose, and driving each joint of the mechanical arm to rotate to the corresponding rotation angle.

In this embodiment, the first calculating module 3 is configured to, when calculating the disturbance force and the disturbance torque received by the robot arm base according to the first kinematic information and the second kinematic information, perform:

In fact, when the flying robot grabs the target object, the disturbance force and the disturbance torque received by the mechanical arm base comprise two parts: one part is the pose change of the multi-rotor flying platform under the uncertain conditions of wind field disturbance and a nonlinear system, so that the force and moment change brought by the force and moment change are transmitted to a mechanical arm base; the other part is the gravity and moment of the mechanical arm and the load when the mechanical arm carries out grabbing operation. Correspondingly, the total disturbance of the mechanical arm base is composed of a flight platform disturbance item and a joint motion and load mass disturbance item. Therefore, the first disturbance force and the first disturbance torque caused by the multi-rotor flight platform can be calculated through the first kinematic information of the multi-rotor flight platform, and the second disturbance force and the second disturbance torque caused by the movement of the mechanical arm can be calculated through the second kinematic information of the mechanical arm.

Specifically, the first kinematic information includes a first attitude acceleration of the multi-rotor flying platform; the first attitude accelerations include three axial translational accelerations and three axial angular accelerations of the multi-rotor flying platform (where the three axial accelerations refer to the three axial accelerations of the world coordinate system, and the first attitude accelerations of the multi-rotor flying platform can be measured by a sensor, such as the first IMU module 404 for the flying robot 400 in fig. 4);

the first calculation module 3, when calculating the first disturbance force and the first disturbance torque caused by the multi-rotor flying platform according to the first kinematic information, performs:

wherein,

as the first disturbing force,

is the first disturbance torque,

is the mass of the multi-rotor flying platform,

is the inertia tensor of the multi-rotor flying platform,

is the translational acceleration of the multi-rotor flying platform,

、

、

for the angular acceleration of a multi-rotor flying platform,

、

、

three axial angular accelerations of the multi-rotor flying platform, respectively.

the first calculating module 3 performs, when calculating the second disturbance force and the second disturbance torque caused by the movement of the robot arm according to the second kinematic information:

wherein,

in order to provide the mass of the robot arm,

in order to provide the quality of the mechanical arm base,

is the first of a robot arm

The mass of each joint is determined by the mass of the joint,

is the first of a robot arm

The centroid position vector of each joint relative to the mechanical arm base,

wherein,

as a second disturbance force, the first disturbance force,

is the second disturbance torque,

for the mass of the object currently being grasped by the robotic arm,

in order to be the acceleration of the gravity,

is a group of a number of 0 s,

is the mass of the object in the air,

Equal to this estimate, or, by visual means, identify the type and volume of the airborne object,then, calculating the mass of the aerial object according to the average density corresponding to the type of the aerial object and the volume of the aerial object to obtain a mass estimation value, and enabling the mass estimation value to be the same as the mass of the aerial object

Is equal to the quality estimate. Wherein, the centroid position of the aerial object can be identified through the vision device, and then the position vector of the centroid position relative to the mechanical arm base is calculated and used as

The value of (c).

In some embodiments, the six-degree-of-freedom pose compensation device is the six-degree-of-freedom pose compensation device 406 shown in fig. 5, the six-degree-of-freedom pose compensation device 406 includes a fixed platform 407 fixedly connected to the multi-rotor flight platform 402 and six telescopic links 409 connected between the fixed platform and the robot arm base 408, and both ends of the telescopic links 409 are respectively connected to the fixed platform 407 and the robot arm base 408 through universal hinges 4091; hereinafter, a description will be given based on such a six-degree-of-freedom pose compensation apparatus.

Wherein the second kinematic information further includes a second attitude acceleration of the robot arm base; the second attitude acceleration includes the translational acceleration of the robot arm base in three axial directions and the angular acceleration in three axial directions (the three axial directions are the three axial directions of the world coordinate system, and the second attitude acceleration of the robot arm base can be measured by a sensor, such as the second IMU module 800 in the case of the flying robot 400 in FIG. 4)

The second calculation module 4 is configured to, when calculating a compensation force that needs to be paid by the six-degree-of-freedom pose compensation apparatus according to the disturbance force and the disturbance torque, execute:

acquiring the rod length of each telescopic connecting rod;

acquiring generalized gravity borne by a mechanical arm base;

Each telescopic connecting rod is provided with a sensor for measuring the length of the telescopic connecting rod (for example, the telescopic connecting rod 409 in fig. 5 includes a linear push rod 4092 and a dc servo motor 4093, the dc servo motor 4093 is provided with a rotary encoder, the dc servo motor 4093 is used for driving the linear push rod 4092 to stretch and retract, and the rotary encoder is a sensor for measuring the length of the telescopic connecting rod), and the rod length of the telescopic connecting rod at the current moment can be measured by the sensor.

Wherein, the mechanical arm base is subjected to generalized gravity

Is obtained by multiplying the generalized mass of the mechanical arm base by the gravity acceleration, wherein the generalized mass of the mechanical arm base comprises the mass of the mechanical arm base

And the mass of the sensor module (e.g., vision sensor, positioning sensor, etc.) mounted on the base of the robot arm.

Wherein the first kinematic information may further include a first attitude angular velocity (including world coordinate system) of the multi-rotor flying platformThree axial angular velocities measurable by a sensor, such as the first IMU module), and the second kinematic information may further include a second pose angular velocity of the robot arm base (including three axial angular velocities of a world coordinate system measurable by a sensor, such as the second IMU module); therefore, the relative angular velocity of the mechanical arm base relative to the multi-rotor flight platform can be obtained by subtracting the first attitude angular velocity from the second attitude angular velocity

。

Further, the second calculating module 4 executes when calculating the driving force of each telescopic link according to the second attitude acceleration, the rod length, the first disturbance force, the first disturbance moment, the second disturbance force, the second disturbance moment, the generalized gravity and the relative angular velocity by using a dynamical model based on Newton-Euler equation:

the jacobian matrix is calculated according to the following formula:

；

wherein,

in the form of a jacobian matrix,

the position vectors of the hinge points of the six telescopic connecting rods and the mechanical arm base relative to the mechanical arm base are respectively;

（1）；

；

wherein,

the driving forces of the first to sixth telescopic links respectively,

in the form of a matrix of driving forces,

is composed of

The inverse of the matrix of (a) is,

is an identity matrix of order 3 x 3,

is the inertia tensor of the base of the mechanical arm,

is the second attitude acceleration of the mechanical arm base,

is the relative angular velocity.

Wherein for the pose compensation device with six degrees of freedom

A telescopic connecting rod, two upper and lower hinged points of which are arranged on a mechanical arm baseThe positions in the coordinate system are respectively

、

，

；

wherein,

is as follows

The upper hinge point of each telescopic connecting rod is positioned at the position vector of the mechanical arm base coordinate system,

is as follows

Lower hinge joint of telescopic connecting rodThe position vector of the point under the coordinate system of the mechanical arm base,

is as follows

。

wherein the Newton-Euler equation of the mechanical arm base is as follows:

（2）；

wherein,

the translational acceleration of the mechanical arm base (which comprises the translational acceleration of three axial directions of a world coordinate system),

is the moment that the base of the mechanical arm is subjected to,

is as follows

The driving force of each telescopic link.

The dynamic model of the mechanical arm base is as follows:

（3）；

wherein,

the pose change speed of the base of the mechanical arm (which can be measured by a sensor on the base of the mechanical arm, such as a second IMU module),

the coefficient matrix of the centripetal force and the Coriolis force of the mechanical arm base is obtained; in practical application, because the rotational angular velocity of the multi-rotor flight platform is small, the centripetal force and the coriolis force of the mechanical arm base can be ignored, and thus the formula (3) is degenerated as follows:

（4）；

combining equations (2) and (4), equation (1) can be obtained.

In some preferred embodiments, the flying robot control device further includes:

a second acquisition module for acquiring a preset time period of the multi-rotor flight platform taking the current moment as a termination point

（

Can be set according to actual needs) the total amount of attitude angle change and the total amount of position change;

and the second execution module is used for controlling the flying robot to fly to a new reference suspension point and restarting the mechanical arm to grab the aerial object when the total attitude angle change amount exceeds a preset angle change threshold and/or the total position change amount exceeds a preset position change threshold.

The total change amount of the inner attitude angles is as follows:

，

is the total amount of the change of the attitude angle,

is composed of

A first attitude angular velocity of the multi-rotor flying platform at a time,

is the starting time of the preset time period taking the current time as the end point.

The total amount of position change in (a) is:

，

in order to be the total amount of position change,

is composed of

Therefore, the flying robot control device controls the flying robot to fly to the reference suspension point and starts the mechanical arm to grab the aerial object; in the grabbing operation process, first kinematic information of the multi-rotor flight platform is obtained, and second kinematic information of the mechanical arm is obtained; calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information; calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque; controlling the six-degree-of-freedom pose compensation device to output compensation force to compensate pose disturbance of the mechanical arm base; therefore, the pose control precision of the tail end of the mechanical arm can be improved, and the aerial object can be reliably grabbed.

Referring to fig. 6, fig. 6 is a schematic structural diagram of an electronic device according to an embodiment of the present disclosure, where the present disclosure provides an electronic device including: a processor 301 and a memory 302, the processor 301 and the memory 302 being interconnected and communicating with each other via a communication bus 303 and/or other form of connection mechanism (not shown), the memory 302 storing a computer program executable by the processor 301, the processor 301 executing the computer program when the electronic device is running to perform the method of controlling an aircraft robot in any of the alternative implementations of the above embodiments to implement the following functions: controlling the flying robot to fly to a reference suspension point, and starting the mechanical arm to perform grabbing operation so as to grab an aerial object; in the grabbing operation process, acquiring first kinematic information of a multi-rotor flight platform and acquiring second kinematic information of a mechanical arm; calculating the disturbance force and the disturbance torque applied to the mechanical arm base according to the first kinematic information and the second kinematic information; calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque; and controlling the six-degree-of-freedom pose compensation device to output compensation force to compensate pose disturbance of the mechanical arm base.

Referring to fig. 3-5, the present application provides an aircraft robot control system comprising a flying robot 400, a position tracker 500, and a ground station 600, both the flying robot 400 and the position tracker 500 being communicatively coupled to the ground station 600;

the position tracker 500 is used for measuring the position of the flying robot 400 and sending the position to the flying robot 400 through the ground station 600;

the flying robot 400 comprises a main control module 401, a multi-rotor flying platform 402, a mechanical arm 403 and a six-degree-of-freedom pose compensation device 406 connected between the multi-rotor flying platform 402 and the mechanical arm 403, wherein the six-degree-of-freedom pose compensation device 406 is used for adjusting the pose of a mechanical arm base 408 of the mechanical arm 403, and the main control module 401 is used for:

controlling the flying robot 400 to fly to a reference suspension point (specifically, controlling the multi-rotor flying platform 402 to fly to the reference suspension point), and starting the mechanical arm 403 to perform grabbing operation to grab the aerial object 90;

during the grabbing operation, acquiring first kinematic information of the multi-rotor flight platform 402 and acquiring second kinematic information of the mechanical arm 403;

calculating the disturbance force and the disturbance torque applied to the mechanical arm base 408 according to the first kinematic information and the second kinematic information;

calculating the compensation force required to be paid by the six-degree-of-freedom pose compensation device 406 according to the disturbance force and the disturbance torque;

the six-degree-of-freedom pose compensation device 406 is controlled to output a compensation force to compensate for the pose disturbance of the robot arm base 408.

The specific function implementation process of the main control module 401 refers to the foregoing steps of the flying robot control method.

In this embodiment, referring to fig. 5, the six-degree-of-freedom pose compensation apparatus 406 includes a fixed stage 407 fixedly connected to the multi-rotor flying platform 402, and six telescopic links 409 connected between the fixed stage 407 and the robot arm base 408, and both ends of the telescopic links 409 are connected to the fixed stage 407 and the robot arm base 408 via universal hinges 4091, respectively.

In some embodiments, the six pantograph linkages 409 are divided into three groups, each group including two pantograph linkages 409, the two pantograph linkages 409 of the same group having an included angle. The fixed platform 407 is connected with the multi-rotor flying platform 402 in a detachable connection manner, the mechanical arm base 408 includes a connection ring and a base frame (refer to fig. 5, where the annular portion of the outer periphery is the connection ring, the middle T-shaped piece is the base frame), the mechanical arm 403 is fixedly connected with the base frame, and the base frame is connected with the connection ring in a detachable connection manner, so that the six-degree-of-freedom pose compensation device 406 (the fixed platform 407, the telescopic link 409, and the connection ring are replaced together) with different sizes and driving capabilities can be replaced according to actual needs.

Further, a vision device 700 (such as, but not limited to, a binocular camera) is disposed on the robot arm base 408.

Wherein, multi-rotor flying platform 402 is provided with a first IMU module 404 for measuring an attitude angle of multi-rotor flying platform 402 (and in fact, an attitude angular velocity and an attitude angular acceleration of multi-rotor flying platform 402 may also be measured).

In the present embodiment, the position tracker 500 is a laser tracker, and the flying robot 400 further includes a tracker target 405, and the tracker target 405 is disposed on a fixed stage 407, as shown in fig. 5.

Referring to fig. 5, the telescopic connecting rod 409 includes a linear push rod 4092 and a dc servo motor 4093, the dc servo motor 4093 is provided with a rotary encoder, the dc servo motor 4093 is used for driving the linear push rod 4092 to extend and retract, and the rotary encoder is a sensor for measuring the length of the telescopic connecting rod 409.

In some embodiments, a second IMU module 800 (see fig. 5) is also disposed on the robot base 408, the second IMU module 800 being configured to detect the attitude angle of the robot base 408 (and in fact the attitude angular velocity and attitude angular acceleration of the robot base 408).

Wherein, the number of the mechanical arms 403 may be one or more, as in fig. 5, two mechanical arms 403 are provided; the robot arm 403 may adopt a robot arm in the prior art, or adopt the robot arm 403 in fig. 5, the robot arm 403 includes a two-degree-of-freedom joint 4031 connected to the robot arm base 408, a parallel four-bar linkage mechanism composed of a first link 4032, a second link 4033, a third link 4034, and a link 4035, a first joint motor 4036 for driving the third link 4034 to swing, an arm 4037, and a second joint motor 4038 provided on the link 4035 for driving the arm 4037 to rotate about its axis; the two-degree-of-freedom joint 4031 is used for driving the four-bar linkage to swing; when the first joint motor 4036 drives the third connecting rod 4034 to swing, the connecting rod support 4035 can correspondingly swing, so that the arm 4037 is driven to swing; the distal end of arm 4037 is provided with a clamping jaw 900.

In some embodiments, multi-rotor flight platform 402 includes at least two collapsible landing gears 410, where collapsible landing gears 410 are used to support multi-rotor flight platform 402 when flying robot 400 is landing to avoid mechanical arms 403 touching the ground.

Wherein, be provided with flight controller module 411 on many rotor flight platform 402, this flight controller module 411 and host system 401 communication connection, this flight controller module 411 is used for controlling many rotor flight platform 402 according to host system 401's control command, and the flight.

A GNSS positioning module 412 (i.e., a satellite positioning module) is preferably also disposed on multi-rotor platform 402, and the position of multi-rotor platform 402 can be measured by GNSS positioning module 412, and the position measurement of GNSS positioning module 412 and the position measurement of position tracker 500 can be combined to obtain valid position data of multi-rotor platform 402 (e.g., when the GNSS signals are good, the average or weighted average of the two is used as valid position data, and when the GNSS signals are poor, the position measurement of position tracker 500 is used as valid position data), so as to further improve the accuracy of the position data.

In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.

The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and changes may be made to the present application by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A flying robot control method is used for controlling a flying robot to carry out aerial object grabbing operation and is characterized in that the flying robot comprises a multi-rotor flying platform, a mechanical arm and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the mechanical arm, wherein the six-degree-of-freedom pose compensation device is used for adjusting the pose of a mechanical arm base of the mechanical arm; the flying robot control method comprises the following steps:

A3. calculating the disturbance force and the disturbance torque of the mechanical arm base according to the first kinematic information and the second kinematic information;

A5. and controlling the six-degree-of-freedom pose compensation device to output the compensation force to compensate pose disturbance of the mechanical arm base.

2. The flying robot control method according to claim 1, wherein step a1 includes:

3. The method according to claim 1, wherein step a3 includes:

4. The method of flying robot control of claim 3, wherein the first kinematic information comprises a first attitude acceleration of the multi-rotor flying platform; the first attitude acceleration comprises three axial translational accelerations and three axial angular accelerations of the multi-rotor flying platform;

wherein,

as the first disturbing force,

in order to obtain the first disturbance torque,

for the mass of the multi-rotor flying platform,

is the inertia tensor of the multi-rotor flying platform,

is the translational acceleration of the multi-rotor flying platform,

、

、

three axial translational accelerations of the multi-rotor flying platform,

for the angular acceleration of the multi-rotor flying platform,

、

、

5. The flying robot control method according to claim 3, wherein the second kinematic information includes a rotation angle of each joint of the robot arm;

wherein,

in order to be the mass of the robot arm,

is the mass of the robot arm base,

is the second of the said mechanical arm

The mass of each joint is determined by the mass of the joint,

is the second of the said mechanical arm

A center of mass position vector of each joint relative to the base of the mechanical arm,

wherein,

as the second disturbing force,

is the second disturbance torque,

for the mass of the object currently being grasped by the robotic arm,

in order to be the acceleration of the gravity,

and the center of mass position vector of the object currently grabbed by the mechanical arm relative to the mechanical arm base is obtained.

6. The flying robot control method according to claim 3, wherein the six-degree-of-freedom pose compensation device comprises a fixed stage fixedly connected to the multi-rotor flying platform and six telescopic links connected between the fixed stage and the arm base, and both ends of the telescopic links are connected to the fixed stage and the arm base through universal hinges, respectively;

step a4 includes:

acquiring the rod length of each telescopic connecting rod;

acquiring generalized gravity borne by the mechanical arm base;

and calculating the driving force of each telescopic connecting rod by adopting a dynamic model based on a Newton-Euler equation according to the second attitude acceleration, the rod length, the first disturbance force moment, the second disturbance force moment, the generalized gravity and the relative angular velocity.

7. The flying robot control method according to claim 6, wherein the step of calculating the driving force of each telescopic link from the second attitude acceleration, the rod length, the first disturbance force, the first disturbance torque, the second disturbance force, the second disturbance torque, the generalized gravity and the relative angular velocity using a dynamical model based on the Newton-Euler equation comprises:

calculating a direction vector of the length direction of each telescopic connecting rod according to the rod length of each telescopic connecting rod;

the jacobian matrix is calculated according to the following formula:

；

wherein,

for the purpose of the jacobian matrix,

respectively are the length direction vectors of the first to the sixth expansion link,

；

；

wherein,

the driving forces of the first to sixth telescopic links, respectively，

In the form of a matrix of driving forces,

is composed of

The inverse of the matrix of (a) is,

is an identity matrix of order 3 x 3,

is the inertia tensor of the base of the mechanical arm,

is the second attitude acceleration of the robot arm base,

for the generalized gravitational force experienced by the robot arm base,

for the purpose of the relative angular velocity,

in order to be the mass of the robot arm,

is the mass of the robot arm base,

as the first disturbing force,

is the first disturbance torque and is,

as the second disturbing force,

is the second disturbance torque.

8. A flying robot control device is used for controlling a flying robot to grab objects in the air, and is characterized in that the flying robot comprises a multi-rotor flying platform, a mechanical arm and a six-degree-of-freedom pose compensation device connected between the multi-rotor flying platform and the mechanical arm, wherein the six-degree-of-freedom pose compensation device is used for adjusting the pose of a mechanical arm base of the mechanical arm; the flying robot control device includes:

9. An electronic device comprising a processor and a memory, the memory storing a computer program executable by the processor, the processor executing the computer program to perform the steps of the flying robot control method according to any one of claims 1-7.

10. The flying robot control system is characterized by comprising a flying robot, a position tracker and a ground station, wherein the flying robot and the position tracker are in communication connection with the ground station;