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

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, device, electronic device and system

technical field

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

Background technique

At present, the flying robot is generally composed of a multi-rotor flight platform, a multi-joint mechanical arm and an end effector, which has the characteristics of compact structure, large working space and flexible movement. When the flying robot controls the operation of grasping objects, its operation process can be divided into four stages: cruise flight, target approach, hovering operation and load return.

During the hovering operation, due to the existence of a variety of uncertain factors, such as the highly nonlinear, time-varying and uncertainty of the dynamic model of the multi-rotor flight platform itself, the inertial parameter changes during the movement of the manipulator (such as Changes in the center of gravity), wind field disturbance, etc., will lead to poor posture stability of the multi-rotor flight platform, making the base seat posture of the robotic arm unstable, thereby reducing the control accuracy of the posture and posture of the end of the robotic arm, which may easily lead to the failure of the grasping operation. .

SUMMARY OF THE INVENTION

The purpose of the present application is to provide a control method, device, electronic device and system for a flying robot, which can improve the position and attitude control accuracy of the end of the robot arm.

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

A1. Control the flying robot to fly to a reference hovering point, and start the robotic arm to grab the air object;

A2. During the grabbing operation, obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the robotic arm;

A3. Calculate the disturbance force and disturbance moment received by the robotic arm base according to the first kinematics information and the second kinematics information;

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

A5. Control the six-degree-of-freedom pose compensation device to output the compensation force to compensate for the pose disturbance of the robotic arm base.

The flying robot control method calculates the compensation force that the six-degree-of-freedom pose compensation device needs to pay when compensating for the disturbance force and disturbance moment by estimating the disturbance force and disturbance moment received by the base of the manipulator, and then controls the six-degree-of-freedom degree of freedom. The pose compensation device compensates the pose of the manipulator base, thereby ensuring the pose stability of the manipulator base. On this basis, controlling the manipulator to perform grasping operations can improve the pose control accuracy of the end of the manipulator. , to achieve reliable grasping of objects in the air.

Preferably, step A1 includes:

controlling the flying robot to perform a cruising flight to search for the aerial object;

After searching for the aerial object, control the flying robot to approach the aerial object;

When the aerial object enters the working range of the robotic arm, it enters a hovering operation state, and the robotic arm is activated to perform a grasping operation to grasp the aerial object.

By performing flight control in stages, different navigation methods can be used for navigation according to the characteristics of different stages, which is beneficial to improve the efficiency and success rate of grasping aerial objects; In the stop operation state, the grasping operation is started while the pose stability control is performed, which is beneficial to improve the reliability of the robot arm to successfully grasp the aerial objects.

Preferably, step A3 includes:

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

A second disturbance force and a second disturbance moment caused by the motion of the manipulator are calculated according to the second kinematics information.

Preferably, the first kinematics information includes the first attitude acceleration of the multi-rotor flight platform; the first attitude acceleration includes the translation acceleration of the multi-rotor flight platform in three axial directions and the three axial directions angular acceleration;

The step of calculating the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematics information includes:

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

为所述第一扰动力，

为所述第一扰动力矩，

为所述多旋翼飞行平台的质量，

为所述多旋翼飞行平台的惯性张量，

为所述多旋翼飞行平台的平移加速度，

、

、

分别为所述多旋翼飞行平台的三个轴向的平移加速度，

为所述多旋翼飞行平台的角加速度，

、

、

分别为所述多旋翼飞行平台的三个轴向的角加速度。in,

is the first disturbance force,

is the first disturbance torque,

is the mass of the multi-rotor flight platform,

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

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

,

,

are the translational accelerations of the three axial directions of the multi-rotor flight platform, respectively,

is the angular acceleration of the multi-rotor flight platform,

,

,

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

Preferably, the second kinematics information includes the rotation angle of each joint of the robotic arm;

The described step of calculating the second disturbance force and the second disturbance moment caused by the motion of the robotic arm according to the second kinematics information includes:

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

Calculate the total centroid position vector of the manipulator relative to the manipulator base according to the following formula:

为所述机械臂的质量，

为所述机械臂基座的质量，

为所述机械臂的第

个关节的质量，

为所述机械臂的第

个关节相对所述机械臂基座的质心位置向量；

为所述机械臂相对所述机械臂基座的总质心位置矢量；in,

is the mass of the robotic arm,

is the mass of the robotic arm base,

for the first

the quality of a joint,

for the first

The position vector of the centroid of each joint relative to the base of the robotic arm;

is the total centroid position vector of the robotic arm relative to the robotic arm base;

The second disturbance force and the second disturbance moment are calculated according to the following formulas:

为所述第二扰动力，

为所述第二扰动力矩，

为所述机械臂当前抓取到的物体的质量，

为重力加速度，

为机械臂当前抓取到的所述物体相对所述机械臂基座的质心位置向量。in,

is the second disturbance force,

is the second disturbance torque,

is the mass of the object currently grasped by the robotic arm,

is the gravitational acceleration,

is the position vector of the center of mass of the object currently grasped by the robotic arm relative to the base of the robotic arm.

Preferably, the six-degree-of-freedom pose compensation device includes a fixed platform fixedly connected to the multi-rotor flight platform and six telescopic links connected between the fixed platform and the robotic arm base. Two ends of the connecting rod are respectively connected with the fixed platform and the base of the mechanical arm through universal hinges;

The second kinematics information includes the second pose acceleration of the robotic arm base;

Step A4 includes:

Obtain the rod length of each telescopic link;

Obtain the generalized gravity that the base of the robotic arm is subjected to;

Obtain the relative angular velocity of the manipulator base relative to the multi-rotor flight platform;

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 For the angular velocity, a dynamic model based on the Newton-Euler equation was used to calculate the driving force of each telescopic link.

By controlling the driving force of each telescopic link in this way, the dynamic compensation of the posture of the base of the manipulator can be realized, which is beneficial to reduce the tracking error of the end of the manipulator to achieve the purpose of stable operation, and can effectively improve the operation accuracy of the aerial manipulator. performance, stability and environmental adaptability.

Preferably, 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, using the dynamic model based on the Newton-Euler equation, the steps of calculating the driving force of each telescopic link include:

Calculate the direction vector of the length direction of each telescopic link according to the rod length of each telescopic link;

Calculate the Jacobian matrix according to the following formula:

；

;

为所述雅可比矩阵，

分别为第一个到第六个伸缩连杆的长度方向的方向矢量，

分别为六个所述伸缩连杆与所述机械臂基座的铰接点相对所述机械臂基座的位置矢量；in,

is the Jacobian matrix,

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

are respectively the position vectors of the hinge points of the six telescopic links and the manipulator base relative to the manipulator base;

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

；

;

；

;

分别为第一个到第六个伸缩连杆的所述驱动力，

为驱动力矩阵，

为

的逆矩阵，

为3×3阶的单位矩阵，

为所述机械臂基座的惯性张量，

为所述机械臂基座的所述第二位姿加速度，

为所述机械臂基座受到的所述广义重力，

为所述相对角速度。in,

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

is the driving force matrix,

for

The inverse matrix of ,

is an identity matrix of order 3 × 3,

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

is the second pose acceleration of the robotic arm base,

is the generalized gravitational force on the base of the robotic arm,

is the relative angular velocity.

In a second aspect, the present application provides a control device for a flying robot, which is used to control the flying robot to perform aerial object grabbing operations. The six-degree-of-freedom pose compensation device between the robotic arms is used to adjust the pose of the robotic arm base of the robotic arm; the flying robot control device includes:

a first execution module, configured to control the flying robot to fly to a reference hovering point, and start the robotic arm to perform a grasping operation to grasp the aerial object;

a first acquisition module, used for acquiring the first kinematic information of the multi-rotor flight platform and acquiring the second kinematic information of the robotic arm during the grabbing operation;

a first calculation module, configured to calculate the disturbance force and disturbance moment received by the manipulator base according to the first kinematics information and the second kinematics information;

a second calculation module, configured to calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque;

The first control module is configured to control the six-degree-of-freedom pose compensation device to output the compensation force to compensate for the pose disturbance of the robotic arm base.

The flying robot control device calculates the compensation force required by the six-degree-of-freedom pose compensation device to compensate the disturbance force and disturbance torque by estimating the disturbance force and disturbance moment received by the base of the manipulator, and then controls the six-degree-of-freedom control device. The pose compensation device compensates the pose of the manipulator base, thereby ensuring the pose stability of the manipulator base. On this basis, controlling the manipulator to perform grasping operations can improve the pose control accuracy of the end of the manipulator. , to achieve reliable grasping of objects in the air.

In a third aspect, the present application provides an electronic device, including a processor and a memory, where the memory stores a computer program executable by the processor, and when the processor executes the computer program, the operation is as described above Steps in a flying robot control method.

In a fourth aspect, the present application provides a control system for a flying robot, including a flying robot, a position tracker, and a ground station, wherein the flying robot and the position tracker are both communicatively connected to the ground station;

The position tracker is used to measure the position of the flying robot and send it to the flying robot through the ground station;

The flying robot includes a main control module, a multi-rotor flight platform, a mechanical arm, and a six-degree-of-freedom pose compensation device connected between the multi-rotor flight platform and the mechanical arm. The six-degree-of-freedom pose compensation device For adjusting the pose of the robotic arm base of the robotic arm, the main control module is used for:

Controlling the flying robot to fly to a reference hovering point, and starting the robotic arm to grab an object in the air;

During the grabbing operation, obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the mechanical arm;

Calculate the disturbance force and disturbance moment received by the manipulator base according to the first kinematics information and the second kinematics information;

Calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance moment;

The six-degree-of-freedom pose compensation device is controlled to output the compensation force to compensate for the pose disturbance of the robotic arm base.

beneficial effect

The flying robot control method, device, electronic device and system provided by this application, by estimating the disturbance force and disturbance moment received by the base of the manipulator, and then calculating the required six-degree-of-freedom pose compensation device when compensating for the disturbance force and disturbance moment. The compensation force is paid, and then the six-degree-of-freedom pose compensation device is controlled to compensate the pose of the manipulator base, thereby ensuring the pose stability of the manipulator base. It can improve the pose control accuracy of the end of the robotic arm and achieve reliable grasping of objects in the air.

Other features and advantages of the present application will be set forth in the description which follows, and, in part, will be apparent from the description, or may be learned by 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.

Description of drawings

FIG. 1 is a flowchart of a method for controlling a flying robot provided by an embodiment of the present application.

FIG. 2 is a schematic structural diagram of a flying robot control device provided by an embodiment of the present application.

FIG. 3 is a schematic structural diagram of a flying robot control system provided by an embodiment of the present application.

FIG. 4 is a schematic structural diagram of a flying robot.

FIG. 5 is a schematic structural diagram of a six-degree-of-freedom pose compensation device and a robotic arm.

FIG. 6 is a schematic structural diagram of an electronic device provided by an embodiment of the present application.

Numeral description: 1, first execution module; 2, first acquisition module; 3, first calculation module; 4, second calculation module; 5, first control module; 90, aerial object; 301, processor; 302, memory; 303, communication bus; 400, flying robot; 401, main control module; 402, multi-rotor flight platform; 403, robotic arm; 4031, two degree of freedom joint; 4032, first link; 4033, second link ;4034, the third link; 4035, the link frame; 4036, the first joint motor; 4037, the arm; 4038, the second joint motor; 404, the first IMU module; 405, the tracker target; 406, the six freedom Degree and posture compensation device; 407, fixed table; 408, manipulator base; 409, telescopic link; 4091, universal hinge; 4092, linear push rod; 4093, DC servo motor; 410, foldable landing gear; 411 , flight controller module; 412, GNSS positioning module; 500, position tracker; 600, ground station; 700, vision device; 800, second IMU module; 900, gripper.

Detailed ways

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. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations. Thus, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the application as claimed, but is merely representative of selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

It should be noted that like numerals and letters refer to like items in the following figures, so once an item is defined in one figure, it does not require further definition and explanation in subsequent figures. Meanwhile, in the description of the present application, the terms "first", "second", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.

Please refer to FIG. 1. FIG. 1 shows a method for controlling a flying robot in some embodiments of the present application, which is used to control the flying robot to perform aerial object grabbing operations. The flying robot includes a multi-rotor flight platform, a mechanical arm, and a A six-degree-of-freedom pose compensation device between a rotor flight platform and a robotic arm, the six-degree-of-freedom pose compensation device is used to adjust the pose of the robotic arm base of the robotic arm; the control method of the flying robot includes the steps:

A1. Control the flying robot to fly to the reference hovering point, and start the robotic arm to grab the object in the air;

A2. During the grasping operation, obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the manipulator;

A3. Calculate the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematics information and the second kinematics information;

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

A5. Control the six-degree-of-freedom pose compensation device to output a compensation force to compensate for the pose disturbance of the robotic arm base.

The flying robot control method calculates the compensation force that the six-degree-of-freedom pose compensation device needs to pay when compensating for the disturbance force and disturbance moment by estimating the disturbance force and disturbance moment received by the base of the manipulator, and then controls the six-degree-of-freedom degree of freedom. The pose compensation device compensates the pose of the manipulator base, thereby ensuring the pose stability of the manipulator base. On this basis, controlling the manipulator to perform grasping operations can improve the pose control accuracy of the end of the manipulator. , to achieve reliable grasping of objects in the air.

Wherein, the flying robot control method can be applied to the main control module 401 of the flying robot 400 of the flying robot control system shown in FIG. 3 to FIG. The flying robot 400 is controlled to grab the aerial object 90 , wherein the aerial object 90 is an object that is fixedly placed in the air (for example, an object placed on top of a high-altitude facility). The flying robot 400 may include one or more robotic arms 403 , for example, the flying robot 400 in FIG. 4 includes two robotic arms 403 .

The reference hovering point can be set in advance according to the position of the air object, so that the flight trajectory from the initial position to the reference hovering point can be planned in advance according to the initial position of the flying robot, and the flying robot can be controlled to fly to the reference according to the flight trajectory. Hover point, and then start the robotic arm to grab. Thus, in some embodiments, step A1 includes:

Control the flying robot to fly to the preset reference hovering point according to the preset flight trajectory;

Enter the hovering operation state, and start the robotic arm for grabbing operations to grab objects in the air.

Wherein, during the flight, the position and attitude of the multi-rotor flight platform can be acquired through the positioning module arranged on the multi-rotor flight platform. For example, the control of the flying robot in FIG. 3 to FIG. 5 includes a position tracker 500 and a ground station 600. The multi-rotor flight platform 402 of the flying robot 400 is provided with a first IMU module 404 and a tracker target 405, wherein the first IMU module 404 can be used to measure the attitude angle of the multi-rotor flying platform 402 (actually, it can also measure the attitude angular velocity and attitude angular acceleration of the multi-rotor flying platform 402), and the position tracker 500 is used to cooperate with the tracker target 405 to measure the multi-rotor flying platform 402. The position and the measurement result are sent to the flying robot 400 through the ground station 600 , so as to obtain the pose of the multi-rotor flying platform 402 .

In other embodiments, step A1 includes:

Control the flying robot for cruising flight to search for air objects;

After searching for an air object, control the flying robot to approach the air object;

When the aerial object enters the working range of the robotic arm, it enters the hovering operation state, and starts the robotic arm to grasp the aerial object.

Through staged flight control, different navigation methods can be used for navigation according to the characteristics of different stages, which is beneficial to improve the efficiency and success rate of grasping aerial objects; when aerial objects enter the working range of the robotic arm, the hovering operation is performed state, so that the grasping operation is started while the pose stability control is performed, which is beneficial to improve the reliability of the robotic arm being able to successfully grasp the objects in the air. For this embodiment, the pose point of the flying robot at the moment of entering the hovering operation state can be used as the reference hovering point.

Among them, the flying robot is provided with a visual device, and the flying robot can cruise and fly according to the preset cruise route, and search for air objects through the visual device during the cruise flight, until the air object is searched, and obtain the air object through the image recognition method. (obtaining the position of an object by an image recognition method is the prior art, which will not be described in detail here). In this way, there is no need to know the exact position of the air object in advance, just know the area where the air object is located, and set a suitable cruising route (such as snake route, spiral route, etc.) in this area, and the flying robot can automatically obtain the air The position of the object, which in turn provides a basis for the subsequent approach and grasping process, has a higher degree of automation and is more convenient to use.

After searching for the air object, obtain the position of the air object and the real-time position of the multi-rotor flight platform, and then plan the approach route (generally a straight line, but not limited to this) from the real-time position to the position of the air object, and then control the flight The robot approaches the airborne object along this approach route. In some embodiments, the process of controlling the flying robot to approach the aerial object along the approaching route includes: controlling the flying robot 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 predetermined speed. Set the distance (can be set according to actual needs), and then control the flying robot to reduce the speed and continue to approach the air object. That is, when the distance between the flying robot and the air object is relatively close, the speed is reduced so that it can enter the hovering state in time to avoid collision with the air object. Among them, 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, so as to determine whether the aerial object enters the working range of the robotic arm. Inside.

Among them, for a flying robot with two robotic arms, the working range of the robotic arms refers to the collaborative workspace range of the two robotic arms (that is, the overlapping area of the respective workspace ranges of the two robotic arms). Generally, the actual operating range of the robotic arm can be extended forward to obtain an extended area. When the aerial object enters the extended area, the flying robot gradually decelerates until it hovers (the flight speed drops to the preset speed threshold, which means it enters the hovering operation state. ), the forward expansion distance can be set according to actual needs to ensure that when the flying robot enters the hovering operation state, the aerial object is located within the actual working range of the robotic arm.

Among them, the pose of the manipulator base after being compensated by the six-degree-of-freedom pose compensation device has high stability. During the grasping operation, the control process of the manipulator can refer to the mechanical arm base with a fixed position. The control process of the arm mainly includes: using the visual device to obtain the grasping point on the air object (identifying the grasping point through the existing image recognition method) relative to the camera coordinate system of the visual device (hereinafter referred to as the first target). pose), according to the pose transformation matrix (pre-calibrated) between the camera coordinate system and the manipulator base coordinate system and the first target pose, calculate the pose of the grasping point in the manipulator base coordinate system ( Hereinafter referred to as the second target pose), according to the second target pose, the rotation angle of each joint of the manipulator is calculated through the kinematic inverse transformation algorithm (this is the prior art, which will not be described in detail here). And drive each joint of the manipulator to rotate to the corresponding rotation angle.

In this embodiment, step A3 includes:

Calculate the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematic information;

The second disturbance force and the second disturbance moment caused by the motion of the manipulator are calculated according to the second kinematics information.

In fact, when the flying robot grabs the target, the disturbance force and disturbance moment on the base of the manipulator include two parts: one part is the multi-rotor flying platform due to the wind field disturbance and the uncertainty of the nonlinear system. The pose changes, and thus the force and torque transmitted to the base of the manipulator; the other part is the gravity and moment of the manipulator itself and the load when the manipulator is grabbing. Correspondingly, the total disturbance of the manipulator base consists of two parts: the disturbance term of the flight platform and the disturbance term of the joint motion and load mass. Therefore, the first disturbance force and the first disturbance moment caused by the multi-rotor flying platform can be calculated through the first kinematic information of the multi-rotor flying platform, and the second disturbance caused by the motion of the manipulator can be calculated through the second kinematic information of the manipulator. power and second disturbance torque.

Specifically, the first kinematics information includes the first attitude acceleration of the multi-rotor flight platform; the first attitude acceleration includes the translational accelerations in three axial directions and the angular accelerations in the three axial directions of the multi-rotor flight platform (here three The three axes refer to the three axes of the world coordinate system, and the first attitude acceleration of the multi-rotor flight platform can be measured by a sensor, for example, for the flying robot 400 in FIG. 4 , it can be measured by the first IMU module 404 );

The step of calculating the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematic information includes:

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

为第一扰动力，

为第一扰动力矩，

为多旋翼飞行平台的质量，

为多旋翼飞行平台的惯性张量，

为多旋翼飞行平台的平移加速度，

、

、

分别为多旋翼飞行平台的三个轴向的平移加速度，

为多旋翼飞行平台的角加速度，

、

、

分别为多旋翼飞行平台的三个轴向的角加速度。in,

is the first disturbance force,

is the first disturbance torque,

is the mass of the multi-rotor flight platform,

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

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

,

,

are the translational accelerations of the three axial directions of the multi-rotor flight platform, respectively,

is the angular acceleration of the multi-rotor flight platform,

,

,

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

Specifically, the second kinematics information includes the rotation angle of each joint of the robotic arm;

The step of calculating the second disturbance force and the second disturbance moment caused by the motion of the manipulator according to the second kinematics information includes:

Calculate the position vector of the center of mass of each joint of the manipulator relative to the base of the manipulator according to the rotation angle of each joint of the manipulator (the specific calculation method is in the prior art, which will not be described in detail here);

Calculate the total centroid position vector of the manipulator relative to the base of the manipulator according to the following formula:

为机械臂的质量，

为机械臂基座的质量，

为机械臂的第

个关节的质量，

为机械臂的第

个关节相对机械臂基座的质心位置向量，

为机械臂相对机械臂基座的总质心位置矢量；in,

is the mass of the robotic arm,

is the mass of the manipulator base,

the first of the robotic arm

the quality of a joint,

the first of the robotic arm

The position vector of the center of mass of each joint relative to the base of the manipulator,

is the total centroid position vector of the manipulator relative to the base of the manipulator;

The second disturbance force and second disturbance moment are calculated according to the following formulas:

为第二扰动力，

为第二扰动力矩，

为机械臂当前抓取到的物体的质量，

为重力加速度，

为机械臂当前抓取到的物体相对机械臂基座的质心位置向量。in,

is the second disturbance force,

is the second disturbance torque,

is the mass of the object currently grasped by the robotic arm,

is the gravitational acceleration,

It is the position vector of the center of mass of the object currently grasped by the robotic arm relative to the base of the robotic arm.

为0，

为零向量，在抓取到空中物体后（即在机械臂缩回阶段），

为该空中物体的质量，

为该空中物体相对机械臂基座的质心位置向量。It should be noted that before starting the grasping operation, the robotic arm is in a folded state, so that the center of mass of the robotic arm is closest to the multi-rotor flight platform, which reduces the interference of the flying robot during the movement process. The grasping operation process includes mechanical The arm extension stage and the robotic arm retraction stage. In the robotic arm extension stage, the robotic arm starts to stretch toward the air object from the retracted state. In the robotic arm retraction stage, the robotic arm drives the grasped air object to retract. During the entire grasping process, the pose of the robotic arm base needs to be stably controlled. Therefore, before grasping an object in the air (i.e. in the arm extension stage),

is 0,

The zero vector, after grabbing the air object (i.e. during the retraction phase of the manipulator),

is the mass of the airborne object,

is the position vector of the center of mass of the air object relative to the base of the manipulator.

等于该已知值。若空中物体的质量未知，则可以用机械臂的最大负载质量作为该空中物体的质量的估计值，令

等于该估计值，或者，通过视觉装置识别空中物体的类型和体积，然后根据空中物体的类型对应的平均密度和空中物体的体积，计算空中物体的质量，得到质量估算值，并令

等于该质量估算值。其中，可通过视觉装置识别空中物体的质心位置，进而计算该质心位置相对机械臂基座的位置向量，作为

的值。Among them, if the mass of the airborne object is a known value (for example, the mass of the airborne object is known in advance, and the known value is sent to the flying robot when the task is assigned to the flying robot), then when the manipulator retracts stage,

equal to this known value. If the mass of the air object is unknown, the maximum load mass of the manipulator can be used as the estimated value of the mass of the air object, let

is equal to the estimated value, or, identify the type and volume of the airborne object through a visual device, and then calculate the mass of the airborne object according to the corresponding average density of the airborne object type and the volume of the airborne object, obtain the mass estimation value, and let

equal to this quality estimate. Among them, the position of the center of mass of the object in the air can be recognized by the visual device, and then the position vector of the position of the center of mass relative to the base of the robot arm can be calculated as

value of .

In some embodiments, the 6DOF pose compensation device is the 6DOF pose compensation device 406 shown in FIG. and six telescopic links 409 connected between the fixed table and the robotic arm base 408, the two ends of the telescopic links 409 are respectively connected with the fixed table 407 and the robotic arm base 408 through universal hinges 4091; hereinafter, based on this Six-DOF Pose Compensation Device Step A4 will be described.

Among them, in the cruising flight stage and the stage of approaching the air object, the telescopic link 409 can be completely retracted (that is, retracted to the shortest point), and the mechanical arm 403 is in a retracted state, thereby improving the operational performance (or called maneuverability) of the flying robot, At the same time, it can reduce the windward area and reduce wind interference.

Wherein, the second kinematics information also includes the second pose acceleration of the manipulator base; the second pose acceleration includes the three-axis translational acceleration and three-axis angular acceleration of the manipulator base (here three The three axes refer to the three axes of the world coordinate system, and the second pose acceleration of the manipulator base can be measured by sensors, for example, for the flying robot 400 in FIG. 4 , it can be measured by the second IMU module 800 )

Step A4 includes:

Obtain the rod length of each telescopic link;

Obtain the generalized gravity on the base of the robotic arm;

Obtain the relative angular velocity of the manipulator base relative to the multi-rotor flight platform;

According to the second pose acceleration, rod length, first disturbance force, first disturbance moment, second disturbance force, second disturbance moment, generalized gravity and relative angular velocity, the dynamic model based on the Newton-Euler equation is used to calculate each telescopic The driving force of the connecting rod (that is, the compensation force required by the six-degree-of-freedom pose compensation device includes the driving force of each telescopic link).

By controlling the driving force of each telescopic link in this way, the dynamic compensation of the posture of the base of the manipulator can be realized, which is beneficial to reduce the tracking error of the end of the manipulator to achieve the purpose of stable operation, and can effectively improve the operation accuracy of the aerial manipulator. performance, stability and environmental adaptability.

Wherein, each telescopic link has a sensor for measuring the length of the telescopic link (for example, the telescopic link 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, and the DC servo motor 4093 is provided with a rotary encoder. The motor 4093 is used to drive the linear push rod 4092 to extend and retract, and the rotary encoder is the sensor used to measure the length of the telescopic link), and the current rod length of the telescopic link can be measured through the sensor.

是用于机械臂基座的广义质量乘以重力加速度得到，其中机械臂基座的广义质量是包括机械臂基座本身的质量

和安装在机械臂基座上的传感器模块（例如视觉传感器、定位传感器等）的质量等在内的总质量。Among them, the generalized gravity of the base of the manipulator

is the generalized mass for the base of the manipulator multiplied by the acceleration of gravity, where the generalized mass of the base of the manipulator is the mass including the base of the manipulator itself

The total mass including the mass of the sensor modules (such as vision sensors, positioning sensors, etc.) mounted on the base of the robot arm.

。The first kinematic information may also include the first attitude angular velocity of the multi-rotor flight platform (including the angular velocity of the three axial directions of the world coordinate system, which can be measured by sensors, such as the first IMU module), the second kinematic information It can also include the second attitude angular velocity of the manipulator base (including the angular velocity of the three axial directions of the world coordinate system, which can be measured by sensors, such as the second IMU module); thus, the second attitude angular velocity and the first attitude angular velocity are used. Subtraction, the relative angular velocity of the manipulator base relative to the multi-rotor flight platform can be obtained

.

Further, 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, a dynamic model based on the Newton-Euler equation is adopted, The steps of calculating the driving force of each telescopic link include:

Calculate the direction vector in the length direction of each telescopic link according to the rod length of each telescopic link;

Calculate the Jacobian matrix according to the following formula:

；

;

为雅可比矩阵，

分别为第一个到第六个伸缩连杆的长度方向的方向矢量，

分别为六个伸缩连杆与机械臂基座的铰接点相对机械臂基座的位置矢量；in,

is the Jacobian matrix,

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

are the position vectors of the hinge points of the six telescopic links and the base of the manipulator relative to the base of the manipulator;

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

（1）；

(1);

；

;

分别为第一个到第六个伸缩连杆的驱动力，

为驱动力矩阵，

为

的逆矩阵，

为3×3阶的单位矩阵，

为机械臂基座的惯性张量，

为机械臂基座的第二位姿加速度，

为机械臂基座受到的广义重力，

为相对角速度。in,

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

is the driving force matrix,

for

The inverse matrix of ,

is an identity matrix of order 3 × 3,

is the inertia tensor of the base of the manipulator,

is the second pose acceleration of the manipulator base,

is the generalized gravity on the base of the manipulator,

is the relative angular velocity.

个伸缩连杆，其上下两个铰接点在机械臂基座坐标系下的位置分别为

、

，

，其中，由于下铰接点在机械臂基座上的位置是固定且已知的，因此

是已知的，其中上铰接点在多旋翼飞行平台上的位置是固定且已知的，可根据各伸缩连杆的长度计算多旋翼飞行平台和机械臂基座坐标系之间的位姿转换矩阵，进而根据该位姿转换矩阵和上铰接点在多旋翼飞行平台上的位置计算得到

，进而可根据以下公式计算各伸缩连杆的长度方向的方向矢量：Among them, for the sixth degree of freedom pose compensation device

A telescopic link, the positions of its upper and lower hinge points in the coordinate system of the manipulator base are:

,

,

, where, since the position of the lower hinge point on the arm base is fixed and known, so

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

, and then the direction vector in the length direction of each telescopic link can be calculated according to the following formula:

；

;

为第

个伸缩连杆的上铰接点在机械臂基座坐标系下的位置矢量，

为第

个伸缩连杆的下铰接点在机械臂基座坐标系下的位置矢量，

为第

个伸缩连杆的上下两个铰接点之间的距离。in,

for the first

The position vector of the upper hinge point of each telescopic link in the coordinate system of the manipulator base,

for the first

The position vector of the lower hinge point of each telescopic link in the coordinate system of the manipulator base,

for the first

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

。Among them, the lower hinge point of the telescopic link is the hinge point with the base of the manipulator. Therefore,

.

Among them, the Newton-Euler equation of the manipulator base is:

（2）；

(2);

为机械臂基座的平移加速度（其包括世界坐标系的三个轴向的平移加速度），

为机械臂基座受到的力矩，

为机械臂基座相对多旋翼飞行平台的角加速度，

为第

个伸缩连杆的驱动力。in,

is the translational acceleration of the manipulator base (which includes the translational accelerations of the three axes of the world coordinate system),

is the moment received by the base of the manipulator,

is the angular acceleration of the manipulator base relative to the multi-rotor flight platform,

for the first

The driving force of a telescopic link.

The dynamic model of the manipulator base is:

（3）；

(3);

为机械臂基座的位姿变化速度（可通过机械臂基座上的传感器测得，例如第二IMU模块），

为机械臂基座的向心力和科氏力的系数矩阵；在实际应用中，由于多旋翼飞行平台的转动角速度较小，机械臂基座的向心力和科氏力可忽略不计，从而公式（3）退化为：in,

is the pose change speed of the manipulator base (which can be measured by sensors on the manipulator base, such as the second IMU module),

is the coefficient matrix of the centripetal force and Coriolis force of the manipulator base; in practical applications, due to the small rotational angular velocity of the multi-rotor flight platform, the centripetal force and the Coriolis force of the manipulator base can be ignored, so formula (3) degenerates to:

（4）；

(4);

Combining formulas (2) and (4), formula (1) can be obtained.

Through the above method, during the operation of the robotic arm, the driving force (including the size and direction) of each telescopic link is adjusted in real time to control the telescopic link. In essence, the driving force is controlled to adjust each telescopic link. The telescopic length of the rod makes the manipulator base move opposite to the disturbance, thereby minimizing the deviation of the actual output pose of the manipulator base from the desired pose, avoiding the influence of external disturbances on the manipulator base pose, and improving the performance of the manipulator base. The error between the trajectory of the end effector of the robotic arm and the planned trajectory (that is, the trajectory planned in real time according to the position of the air object during the grasping operation).

In some preferred embodiments, after step A2, it also includes steps:

（

可根据实际需要设置）内的姿态角变化总量和位置变化总量；Get the preset time period of the multi-rotor flight platform with the current moment as the termination point

(

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

If the total amount of attitude angle change exceeds the preset angle change threshold and/or the total position change exceeds the preset position change threshold, the flying robot will be controlled to fly to a new reference hovering point, and the robotic arm will be restarted for grasping operations , to grab objects in the air.

The preset angle change threshold and the preset position change threshold can be set according to the telescopic length and telescopic speed constraints of each telescopic link (that is, the range of telescopic length and the range of telescopic speed). If the set angle change threshold and/or the total position change exceeds the preset position change threshold, it means that the pose change of the flying robot is likely or has exceeded the telescopic length and telescopic speed constraints of each telescopic link, which will lead to the failure of grasping , so the reference hovering point is re-planned to restart the grabbing operation to further ensure the reliability of the grabbing operation.

内的姿态角变化总量为：

，

为姿态角变化总量，

为

时刻的多旋翼飞行平台的第一姿态角速度，

为以当前时刻为终止点的预设时间段的起始时刻。Among them, the multi-rotor flight platform is in the preset time period with the current moment as the termination point

The total amount of attitude angle change within is:

,

is the total amount of attitude angle change,

for

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

is the start time of the preset time period with the current time as the end point.

内的位置变化总量为：

，

为位置变化总量，

为

时刻的多旋翼飞行平台的第一平移速度（第一运动学信息可包括多旋翼飞行平台的第一平移速度，可通过传感器测得，例如第一IMU模块或位置跟踪仪500）。Among them, the multi-rotor flight platform is in the preset time period with the current moment as the termination point

The total amount of position change within is:

,

is the total amount of position change,

for

The first translation speed of the multi-rotor flight platform at the moment (the first kinematic information may include the first translation speed of the multi-rotor flight platform, which may be measured by a sensor, such as the first IMU module or the 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 flight platform will be compensated separately according to the existing pose compensation method, so as to keep the multi-rotor flight platform with a certain stability. In the case of various uncertain factors, the pose of the multi-rotor flight platform after compensation according to the existing pose compensation method generally has a relatively large deviation, which is the need to compensate the manipulator base. Although the pose stability of the multi-rotor flight platform itself cannot be improved by compensating the manipulator base, it can improve the pose stability of the manipulator base, thereby improving the pose control of the manipulator end. accuracy.

It can be seen from the above that the control method of the flying robot controls the flying robot to fly to the reference hovering point, and starts the mechanical arm to grasp the objects in the air; 1 kinematic information, and obtain the second kinematic information of the manipulator; calculate the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematic information and the second kinematic information; calculate the six freedoms according to the disturbance force and disturbance torque The compensation force required by the 6-degree pose compensation device is controlled; the 6-DOF pose compensation device is controlled to output the compensation force to compensate for the pose disturbance of the manipulator base; thus, the pose control accuracy of the end of the manipulator can be improved, and the control of air objects can be realized. Crawl reliably.

Referring to FIG. 2, the present application provides a flying robot control device for controlling the flying robot to perform aerial object grabbing operations. The flying robot includes a multi-rotor flying platform, a mechanical arm, and a multi-rotor flying platform and a mechanical arm. The six-degree-of-freedom pose compensation device is used to adjust the pose of the robotic arm base of the robotic arm; the flying robot control device includes:

The first execution module 1 is used to control the flying robot to fly to the reference hovering point, and start the mechanical arm to carry out the grasping operation to grasp the aerial object;

The first acquisition module 2 is used for acquiring the first kinematic information of the multi-rotor flight platform and acquiring the second kinematic information of the manipulator during the grasping operation;

The first calculation module 3 is used to calculate the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematics information and the second kinematics information;

The second calculation module 4 is used to calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance moment;

The first control module 5 is used to control the six-degree-of-freedom pose compensation device to output a compensation force to compensate for the pose disturbance of the base of the robotic arm.

The flying robot control device calculates the compensation force required by the six-degree-of-freedom pose compensation device to compensate the disturbance force and disturbance torque by estimating the disturbance force and disturbance moment received by the base of the manipulator, and then controls the six-degree-of-freedom control device. The pose compensation device compensates the pose of the manipulator base, thereby ensuring the pose stability of the manipulator base. On this basis, controlling the manipulator to perform grasping operations can improve the pose control accuracy of the end of the manipulator. , to achieve reliable grasping of objects in the air.

Wherein, the flying robot control device can be applied to the main control module 401 of the flying robot 400 of the flying robot control system shown in FIG. 3 to FIG. The flying robot 400 is controlled to grab the aerial object 90 , wherein the aerial object 90 is an object that is fixedly placed in the air (for example, an object placed on top of a high-altitude facility). The flying robot 400 may include one or more robotic arms 403 , for example, the flying robot 400 in FIG. 4 includes two robotic arms 403 .

The reference hovering point can be set in advance according to the position of the air object, so that the flight trajectory from the initial position to the reference hovering point can be planned in advance according to the initial position of the flying robot, and the flying robot can be controlled to fly to the reference according to the flight trajectory. Hover point, and then start the robotic arm to grab. Therefore, in some embodiments, the first execution module 1 is used to control the flying robot to fly to the reference hovering point and start the mechanical arm to perform the grabbing operation to grab the air object, to execute:

Control the flying robot to fly to the preset reference hovering point according to the preset flight trajectory;

Enter the hovering operation state, and start the robotic arm for grabbing operations to grab objects in the air.

Wherein, during the flight, the position and attitude of the multi-rotor flight platform can be acquired through the positioning module arranged on the multi-rotor flight platform. For example, the control of the flying robot in FIG. 3 to FIG. 5 includes a position tracker 500 and a ground station 600. The multi-rotor flight platform 402 of the flying robot 400 is provided with a first IMU module 404 and a tracker target 405, wherein the first IMU module 404 can be used to measure the attitude angle of the multi-rotor flying platform 402 (actually, it can also measure the attitude angular velocity and attitude angular acceleration of the multi-rotor flying platform 402), and the position tracker 500 is used to cooperate with the tracker target 405 to measure the multi-rotor flying platform 402. The position and the measurement result are sent to the flying robot 400 through the ground station 600 , so as to obtain the pose of the multi-rotor flying platform 402 .

In other embodiments, the first execution module 1 is used to control the flying robot to fly to the reference hovering point, and start the robotic arm to carry out the grabbing operation to grab the air object, to execute:

Control the flying robot for cruising flight to search for air objects;

After searching for an air object, control the flying robot to approach the air object;

When the aerial object enters the working range of the robotic arm, it enters the hovering operation state, and starts the robotic arm to grasp the aerial object.

Through staged flight control, different navigation methods can be used for navigation according to the characteristics of different stages, which is beneficial to improve the efficiency and success rate of grasping aerial objects; when aerial objects enter the working range of the robotic arm, the hovering operation is performed state, so that the grasping operation is started while the pose stability control is performed, which is beneficial to improve the reliability of the robotic arm being able to successfully grasp the objects in the air. For this embodiment, the pose point of the flying robot at the moment of entering the hovering operation state can be used as the reference hovering point.

Among them, the flying robot is provided with a visual device, and the flying robot can cruise and fly according to the preset cruise route, and search for air objects through the visual device during the cruise flight, until the air object is searched, and obtain the air object through the image recognition method. (obtaining the position of an object by an image recognition method is the prior art, which will not be described in detail here). In this way, there is no need to know the exact position of the air object in advance, just know the area where the air object is located, and set a suitable cruising route (such as snake route, spiral route, etc.) in this area, and the flying robot can automatically obtain the air The position of the object, which in turn provides a basis for the subsequent approach and grasping process, has a higher degree of automation and is more convenient to use.

After searching for the air object, obtain the position of the air object and the real-time position of the multi-rotor flight platform, and then plan the approach route (generally a straight line, but not limited to this) from the real-time position to the position of the air object, and then control the flight The robot approaches the airborne object along this approach route. In some embodiments, the process of controlling the flying robot to approach the aerial object along the approaching route includes: controlling the flying robot 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 predetermined speed. Set the distance (can be set according to actual needs), and then control the flying robot to reduce the speed and continue to approach the air object. That is, when the distance between the flying robot and the air object is relatively close, the speed is reduced so that it can enter the hovering state in time to avoid collision with the air object. Among them, 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, so as to determine whether the aerial object enters the working range of the robotic arm. Inside.

Among them, for a flying robot with two robotic arms, the working range of the robotic arms refers to the collaborative workspace range of the two robotic arms (that is, the overlapping area of the respective workspace ranges of the two robotic arms). Generally, the actual operating range of the robotic arm can be extended forward to obtain an extended area. When the aerial object enters the extended area, the flying robot gradually decelerates until it hovers (the flight speed drops to the preset speed threshold, which means it enters the hovering operation state. ), the forward expansion distance can be set according to actual needs to ensure that when the flying robot enters the hovering operation state, the aerial object is located within the actual working range of the robotic arm.

Among them, the pose of the manipulator base after being compensated by the six-degree-of-freedom pose compensation device has high stability. During the grasping operation, the control process of the manipulator can refer to the mechanical arm base with a fixed position. The control process of the arm mainly includes: using the visual device to obtain the grasping point on the air object (identifying the grasping point through the existing image recognition method) relative to the camera coordinate system of the visual device (hereinafter referred to as the first target). pose), according to the pose transformation matrix (pre-calibrated) between the camera coordinate system and the manipulator base coordinate system and the first target pose, calculate the pose of the grasping point in the manipulator base coordinate system ( Hereinafter referred to as the second target pose), according to the second target pose, the rotation angle of each joint of the manipulator is calculated through the kinematic inverse transformation algorithm (this is the prior art, which will not be described in detail here). And drive each joint of the manipulator to rotate to the corresponding rotation angle.

In this embodiment, the first calculation module 3 is configured to perform: when calculating the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematics information and the second kinematics information:

Calculate the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematic information;

The second disturbance force and the second disturbance moment caused by the motion of the manipulator are calculated according to the second kinematics information.

In fact, when the flying robot grabs the target, the disturbance force and disturbance moment on the base of the manipulator include two parts: one part is the multi-rotor flying platform due to the wind field disturbance and the uncertainty of the nonlinear system. The pose changes, and thus the force and torque transmitted to the base of the manipulator; the other part is the gravity and moment of the manipulator itself and the load when the manipulator is grabbing. Correspondingly, the total disturbance of the manipulator base consists of two parts: the disturbance term of the flight platform and the disturbance term of the joint motion and load mass. Therefore, the first disturbance force and the first disturbance moment caused by the multi-rotor flying platform can be calculated through the first kinematic information of the multi-rotor flying platform, and the second disturbance caused by the motion of the manipulator can be calculated through the second kinematic information of the manipulator. power and second disturbance torque.

Specifically, the first kinematics information includes the first attitude acceleration of the multi-rotor flight platform; the first attitude acceleration includes the translational accelerations in three axial directions and the angular accelerations in the three axial directions of the multi-rotor flight platform (here three The three axes refer to the three axes of the world coordinate system, and the first attitude acceleration of the multi-rotor flight platform can be measured by sensors, for example, for the flying robot 400 in FIG. 4 , it can be measured by the first IMU module 404 ) ;

When calculating the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematic information, the first calculation module 3 executes:

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

为第一扰动力，

为第一扰动力矩，

为多旋翼飞行平台的质量，

为多旋翼飞行平台的惯性张量，

为多旋翼飞行平台的平移加速度，

、

、

分别为多旋翼飞行平台的三个轴向的平移加速度，

为多旋翼飞行平台的角加速度，

、

、

分别为多旋翼飞行平台的三个轴向的角加速度。in,

is the first disturbance force,

is the first disturbance torque,

is the mass of the multi-rotor flight platform,

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

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

,

,

are the translational accelerations of the three axial directions of the multi-rotor flight platform, respectively,

is the angular acceleration of the multi-rotor flight platform,

,

,

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

Specifically, the second kinematics information includes the rotation angle of each joint of the robotic arm;

The first calculation module 3, when calculating the second disturbance force and the second disturbance moment caused by the motion of the manipulator according to the second kinematics information, executes:

Calculate the position vector of the center of mass of each joint of the manipulator relative to the base of the manipulator according to the rotation angle of each joint of the manipulator (the specific calculation method is in the prior art, which will not be described in detail here);

Calculate the total centroid position vector of the manipulator relative to the base of the manipulator according to the following formula:

为机械臂的质量，

为机械臂基座的质量，

为机械臂的第

个关节的质量，

为机械臂的第

个关节相对机械臂基座的质心位置向量，

为机械臂相对机械臂基座的总质心位置矢量；in,

is the mass of the robotic arm,

is the mass of the manipulator base,

the first of the robotic arm

the quality of a joint,

the first of the robotic arm

The position vector of the center of mass of each joint relative to the base of the manipulator,

is the total centroid position vector of the manipulator relative to the base of the manipulator;

The second disturbance force and second disturbance moment are calculated according to the following formulas:

为第二扰动力，

为第二扰动力矩，

为机械臂当前抓取到的物体的质量，

为重力加速度，

为机械臂当前抓取到的物体相对机械臂基座的质心位置向量。in,

is the second disturbance force,

is the second disturbance torque,

is the mass of the object currently grasped by the robotic arm,

is the gravitational acceleration,

It is the position vector of the center of mass of the object currently grasped by the robotic arm relative to the base of the robotic arm.

为0，

为零向量，在抓取到空中物体后（即在机械臂缩回阶段），

为该空中物体的质量，

为该空中物体相对机械臂基座的质心位置向量。It should be noted that before starting the grasping operation, the robotic arm is in a folded state, so that the center of mass of the robotic arm is closest to the multi-rotor flight platform, which reduces the interference of the flying robot during the movement process. The grasping operation process includes mechanical The arm extension stage and the robotic arm retraction stage. In the robotic arm extension stage, the robotic arm starts to stretch toward the air object from the retracted state. In the robotic arm retraction stage, the robotic arm drives the grasped air object to retract. During the entire grasping process, the pose of the robotic arm base needs to be stably controlled. Therefore, before grasping an object in the air (i.e. in the arm extension stage),

is 0,

The zero vector, after grabbing the air object (i.e. during the retraction phase of the manipulator),

is the mass of the airborne object,

is the position vector of the center of mass of the air object relative to the base of the manipulator.

等于该已知值。若空中物体的质量未知，则可以用机械臂的最大负载质量作为该空中物体的质量的估计值，令

等于该估计值，或者，通过视觉装置识别空中物体的类型和体积，然后根据空中物体的类型对应的平均密度和空中物体的体积，计算空中物体的质量，得到质量估算值，并令

等于该质量估算值。其中，可通过视觉装置识别空中物体的质心位置，进而计算该质心位置相对机械臂基座的位置向量，作为

的值。Among them, if the mass of the airborne object is a known value (for example, the mass of the airborne object is known in advance, and the known value is sent to the flying robot when the task is assigned to the flying robot), then when the manipulator retracts stage,

equal to this known value. If the mass of the air object is unknown, the maximum load mass of the manipulator can be used as the estimated value of the mass of the air object, let

is equal to the estimated value, or, identify the type and volume of the airborne object through a visual device, and then calculate the mass of the airborne object according to the corresponding average density of the airborne object type and the volume of the airborne object, obtain the mass estimation value, and let

equal to this quality estimate. Among them, the position of the center of mass of the object in the air can be recognized by the visual device, and then the position vector of the position of the center of mass relative to the base of the robot arm can be calculated as

value of .

In some embodiments, the 6DOF pose compensation device is the 6DOF pose compensation device 406 shown in FIG. and six telescopic links 409 connected between the fixed table and the robotic arm base 408, the two ends of the telescopic links 409 are respectively connected with the fixed table 407 and the robotic arm base 408 through universal hinges 4091; hereinafter, based on this The six-degree-of-freedom pose compensation device will be described.

Among them, in the cruising flight stage and the stage of approaching the air object, the telescopic link 409 can be completely retracted (that is, retracted to the shortest point), and the mechanical arm 403 is in a retracted state, thereby improving the operational performance (or called maneuverability) of the flying robot, At the same time, it can reduce the windward area and reduce wind interference.

Wherein, the second kinematics information also includes the second pose acceleration of the manipulator base; the second pose acceleration includes the three-axis translational acceleration and three-axis angular acceleration of the manipulator base (here three The three axes refer to the three axes of the world coordinate system, and the second pose acceleration of the manipulator base can be measured by sensors, for example, for the flying robot 400 in FIG. 4 , it can be measured by the second IMU module 800 )

The second calculation module 4 is configured to perform: when calculating the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque:

Obtain the rod length of each telescopic link;

Obtain the generalized gravity on the base of the robotic arm;

Obtain the relative angular velocity of the manipulator base relative to the multi-rotor flight platform;

According to the second pose acceleration, rod length, first disturbance force, first disturbance moment, second disturbance force, second disturbance moment, generalized gravity and relative angular velocity, the dynamic model based on the Newton-Euler equation is used to calculate each telescopic The driving force of the connecting rod (that is, the compensation force required by the six-degree-of-freedom pose compensation device includes the driving force of each telescopic link).

By controlling the driving force of each telescopic link in this way, the dynamic compensation of the posture of the base of the manipulator can be realized, which is beneficial to reduce the tracking error of the end of the manipulator to achieve the purpose of stable operation, and can effectively improve the operation accuracy of the aerial manipulator. performance, stability and environmental adaptability.

Wherein, each telescopic link has a sensor for measuring the length of the telescopic link (for example, the telescopic link 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, and the DC servo motor 4093 is provided with a rotary encoder. The motor 4093 is used to drive the linear push rod 4092 to extend and retract, and the rotary encoder is the sensor used to measure the length of the telescopic link), and the current rod length of the telescopic link can be measured through the sensor.

是用于机械臂基座的广义质量乘以重力加速度得到，其中机械臂基座的广义质量是包括机械臂基座本身的质量

和安装在机械臂基座上的传感器模块（例如视觉传感器、定位传感器等）的质量等在内的总质量。Among them, the generalized gravity of the base of the manipulator

is the generalized mass for the base of the manipulator multiplied by the acceleration of gravity, where the generalized mass of the base of the manipulator is the mass including the base of the manipulator itself

The total mass including the mass of the sensor modules (such as vision sensors, positioning sensors, etc.) mounted on the base of the robot arm.

。The first kinematic information may also include the first attitude angular velocity of the multi-rotor flight platform (including the angular velocity of the three axial directions of the world coordinate system, which can be measured by sensors, such as the first IMU module), the second kinematic information It can also include the second attitude angular velocity of the manipulator base (including the angular velocity of the three axial directions of the world coordinate system, which can be measured by sensors, such as the second IMU module); thus, the second attitude angular velocity and the first attitude angular velocity are used. Subtraction, the relative angular velocity of the manipulator base relative to the multi-rotor flight platform can be obtained

.

Further, the second calculation module 4 adopts a Newton-Euler-based method based on the second pose acceleration, rod length, first disturbance force, first disturbance moment, second disturbance force, second disturbance moment, generalized gravity and relative angular velocity. The dynamic model of the equation, when calculating the driving force of each telescopic link, execute:

Calculate the direction vector in the length direction of each telescopic link according to the rod length of each telescopic link;

Calculate the Jacobian matrix according to the following formula:

；

;

为雅可比矩阵，

分别为第一个到第六个伸缩连杆的长度方向的方向矢量，

分别为六个伸缩连杆与机械臂基座的铰接点相对机械臂基座的位置矢量；in,

is the Jacobian matrix,

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

are the position vectors of the hinge points of the six telescopic links and the base of the manipulator relative to the base of the manipulator;

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

（1）；

(1);

；

;

分别为第一个到第六个伸缩连杆的驱动力，

为驱动力矩阵，

为

的逆矩阵，

为3×3阶的单位矩阵，

为机械臂基座的惯性张量，

为机械臂基座的第二位姿加速度，

为机械臂基座受到的广义重力，

为相对角速度。in,

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

is the driving force matrix,

for

The inverse matrix of ,

is an identity matrix of order 3 × 3,

is the inertia tensor of the base of the manipulator,

is the second pose acceleration of the manipulator base,

is the generalized gravity on the base of the manipulator,

is the relative angular velocity.

个伸缩连杆，其上下两个铰接点在机械臂基座坐标系下的位置分别为

、

，

，其中，由于下铰接点在机械臂基座上的位置是固定且已知的，因此

是已知的，其中上铰接点在多旋翼飞行平台上的位置是固定且已知的，可根据各伸缩连杆的长度计算多旋翼飞行平台和机械臂基座坐标系之间的位姿转换矩阵，进而根据该位姿转换矩阵和上铰接点在多旋翼飞行平台上的位置计算得到

，进而可根据以下公式计算各伸缩连杆的长度方向的方向矢量：Among them, for the sixth degree of freedom pose compensation device

A telescopic link, the positions of its upper and lower hinge points in the coordinate system of the manipulator base are:

,

,

, where, since the position of the lower hinge point on the arm base is fixed and known, so

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

, and then the direction vector in the length direction of each telescopic link can be calculated according to the following formula:

；

;

为第

个伸缩连杆的上铰接点在机械臂基座坐标系下的位置矢量，

为第

个伸缩连杆的下铰接点在机械臂基座坐标系下的位置矢量，

为第

个伸缩连杆的上下两个铰接点之间的距离。in,

for the first

The position vector of the upper hinge point of each telescopic link in the coordinate system of the manipulator base,

for the first

The position vector of the lower hinge point of each telescopic link in the coordinate system of the manipulator base,

for the first

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

。Among them, the lower hinge point of the telescopic link is the hinge point with the base of the manipulator. Therefore,

.

Among them, the Newton-Euler equation of the manipulator base is:

（2）；

(2);

为机械臂基座的平移加速度（其包括世界坐标系的三个轴向的平移加速度），

为机械臂基座受到的力矩，

为机械臂基座相对多旋翼飞行平台的角加速度，

为第

个伸缩连杆的驱动力。in,

is the translational acceleration of the manipulator base (which includes the translational accelerations of the three axes of the world coordinate system),

is the moment received by the base of the manipulator,

is the angular acceleration of the manipulator base relative to the multi-rotor flight platform,

for the first

The driving force of a telescopic link.

The dynamic model of the manipulator base is:

（3）；

(3);

为机械臂基座的位姿变化速度（可通过机械臂基座上的传感器测得，例如第二IMU模块），

为机械臂基座的向心力和科氏力的系数矩阵；在实际应用中，由于多旋翼飞行平台的转动角速度较小，机械臂基座的向心力和科氏力可忽略不计，从而公式（3）退化为：in,

is the pose change speed of the manipulator base (which can be measured by sensors on the manipulator base, such as the second IMU module),

is the coefficient matrix of the centripetal force and Coriolis force of the manipulator base; in practical applications, due to the small rotational angular velocity of the multi-rotor flight platform, the centripetal force and the Coriolis force of the manipulator base can be ignored, so formula (3) degenerates to:

（4）；

(4);

Combining formulas (2) and (4), formula (1) can be obtained.

Through the above method, during the operation of the robotic arm, the driving force (including the size and direction) of each telescopic link is adjusted in real time to control the telescopic link. In essence, the driving force is controlled to adjust each telescopic link. The telescopic length of the rod makes the manipulator base move opposite to the disturbance, thereby minimizing the deviation of the actual output pose of the manipulator base from the desired pose, avoiding the influence of external disturbances on the manipulator base pose, and improving the performance of the manipulator base. The error between the trajectory of the end effector of the robotic arm and the planned trajectory (that is, the trajectory planned in real time according to the position of the air object during the grasping operation).

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

（

可根据实际需要设置）内的姿态角变化总量和位置变化总量；The second acquisition module is used to acquire the preset time period of the multi-rotor flight platform with the current moment as the termination point

(

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

The second execution module is configured to control the flying robot to fly to a new reference hovering point when the total amount of attitude angle change exceeds the preset angle change threshold and/or the total position change exceeds the preset position change threshold, and restarts Start the robotic arm for grabbing work to grab objects in the air.

The preset angle change threshold and the preset position change threshold can be set according to the telescopic length and telescopic speed constraints of each telescopic link (that is, the range of telescopic length and the range of telescopic speed). If the set angle change threshold and/or the total position change exceeds the preset position change threshold, it means that the pose change of the flying robot is likely or has exceeded the telescopic length and telescopic speed constraints of each telescopic link, which will lead to the failure of grasping , so the reference hovering point is re-planned to restart the grabbing operation to further ensure the reliability of the grabbing operation.

内的姿态角变化总量为：

，

为姿态角变化总量，

为

时刻的多旋翼飞行平台的第一姿态角速度，

为以当前时刻为终止点的预设时间段的起始时刻。Among them, the multi-rotor flight platform is in the preset time period with the current moment as the termination point

The total amount of attitude angle change within is:

,

is the total amount of attitude angle change,

for

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

is the start time of the preset time period with the current time as the end point.

内的位置变化总量为：

，

为位置变化总量，

为

时刻的多旋翼飞行平台的第一平移速度（第一运动学信息可包括多旋翼飞行平台的第一平移速度，可通过传感器测得，例如第一IMU模块或位置跟踪仪500）。Among them, the multi-rotor flight platform is in the preset time period with the current moment as the termination point

The total amount of position change within is:

,

is the total amount of position change,

for

The first translation speed of the multi-rotor flight platform at the moment (the first kinematic information may include the first translation speed of the multi-rotor flight platform, which may be measured by a sensor, such as the first IMU module or the 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 flight platform will be compensated separately according to the existing pose compensation method, so as to keep the multi-rotor flight platform with a certain stability. In the case of various uncertain factors, the pose of the multi-rotor flight platform after compensation according to the existing pose compensation method generally has a relatively large deviation, which is the need to compensate the manipulator base. Although the pose stability of the multi-rotor flight platform itself cannot be improved by compensating the manipulator base, it can improve the pose stability of the manipulator base, thereby improving the pose control of the manipulator end. accuracy.

It can be seen from the above that the flying robot control device controls the flying robot to fly to the reference hovering point, and activates the mechanical arm to grasp the objects in the air; 1 kinematic information, and obtain the second kinematic information of the manipulator; calculate the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematic information and the second kinematic information; calculate the six freedoms according to the disturbance force and disturbance torque The compensation force required by the 6-degree pose compensation device is controlled; the 6-DOF pose compensation device is controlled to output the compensation force to compensate for the pose disturbance of the manipulator base; thus, the pose control accuracy of the end of the manipulator can be improved, and the control of air objects can be realized. Reliable crawl.

Please refer to FIG. 6 . FIG. 6 is a schematic structural diagram of an electronic device provided by an embodiment of the present application. The present application provides an electronic device including: a processor 301 and a memory 302 , and the processor 301 and the memory 302 pass through the communication bus 303 and /or other forms of connection mechanisms (not shown) are interconnected and communicate with each other, and the memory 302 stores a computer program executable by the processor 301. When the electronic device is running, the processor 301 executes the computer program to execute the above embodiments. The control method of the flying robot in any optional implementation manner of the above, so as to realize the following functions: control the flying robot to fly to the reference hovering point, and start the mechanical arm to carry out the grasping operation to grasp the aerial objects; in the grasping operation process , obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the manipulator; calculate the disturbance force and disturbance moment received by the base of the manipulator according to the first kinematic information and the second kinematic information; According to the disturbance force and disturbance torque, the compensation force to be paid by the six-degree-of-freedom pose compensation device is calculated; the six-degree-of-freedom pose compensation device is controlled to output the compensation force to compensate the pose disturbance of the manipulator base.

3-5, the present application provides a control system for a flying robot, including a flying robot 400, a position tracker 500 and a ground station 600, and both the flying robot 400 and the position tracker 500 are connected in communication with the ground station 600;

The position tracker 500 is used to measure the position of the flying robot 400 and send it to the flying robot 400 through the ground station 600;

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

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

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

Calculate the disturbance force and disturbance moment received by the manipulator base 408 according to the first kinematic information and the second kinematic information;

Calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device 406 according to the disturbance force and the disturbance moment;

The six-DOF pose compensation device 406 is controlled to output a compensation force to compensate for the pose disturbance of the robotic arm base 408 .

For the specific function realization process of the main control module 401, refer to the steps of the above-mentioned flying robot control method.

In this embodiment, see FIG. 5 , the six-degree-of-freedom pose compensation device 406 includes a fixed platform 407 fixedly connected with the multi-rotor flight platform 402 and six telescopic links connected between the fixed platform 407 and the robotic arm base 408 409 , the two ends of the telescopic link 409 are respectively connected to the fixed table 407 and the robotic arm base 408 through the universal hinge 4091 .

In some embodiments, the six telescopic links 409 are divided into three groups, each group includes two telescopic links 409, and the two telescopic links 409 in the same group have an included angle. The fixed platform 407 and the multi-rotor flight platform 402 are connected by a detachable connection, and the manipulator base 408 includes a connecting ring and a base frame (refer to FIG. The mechanical arm 403 is fixedly connected to the base frame, and the base frame and the connecting ring are connected by a detachable connection, so that the six-degree-of-freedom pose compensation device of different sizes and driving capabilities can be replaced according to actual needs. 406 (the fixing table 407, the telescopic link 409 and the connecting ring are replaced together).

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

The multi-rotor flight platform 402 is provided with a first IMU module 404 for measuring the attitude angle of the multi-rotor flight platform 402 (actually, the attitude angular velocity and attitude angular acceleration of the multi-rotor flight platform 402 can also be measured).

In this 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 set on the fixed platform 407 , as shown in FIG. 5 .

5, the telescopic link 409 includes a linear push rod 4092 and a DC servo motor 4093. The DC servo motor 4093 is provided with a rotary encoder, and the DC servo motor 4093 is used to drive the linear push rod 4092 to expand and contract. The rotary encoder is It is a sensor for measuring the length of the telescopic link 409 .

In some embodiments, the manipulator base 408 is further provided with a second IMU module 800 (see FIG. 5 ), the second IMU module 800 is used to detect the attitude angle of the manipulator base 408 (actually, it can also detect the mechanical Attitude Angular Velocity and Attitude Angular Acceleration of Arm Base 408).

The number of the robotic arms 403 may be one or more. As shown in FIG. 5 , there are two robotic arms 403 ; the robotic arms 403 may be the robotic arms in the prior art, or the robotic arms 403 in FIG. 5 may be used. , the manipulator 403 includes a two-degree-of-freedom joint 4031 connected with the manipulator 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 frame 4035 , the first joint motor 4036 for driving the third link 4034 to swing, the arm rod 4037 and the second joint motor 4038 arranged on the link frame 4035 for driving the arm rod 4037 to rotate around its own axis; 4031 is used to drive the four-bar linkage mechanism to swing; when the first joint motor 4036 drives the third link 4034 to swing, the link frame 4035 can swing correspondingly, thereby driving the arm rod 4037 to swing; the end of the arm rod 4037 is provided with a clamping jaw 900 .

In some embodiments, the multi-rotor flight platform 402 includes at least two foldable landing gears 410 for supporting the multi-rotor flight platform 402 when the flying robot 400 is landing, so as to prevent the robotic arms 403 from touching the ground .

Wherein, the multi-rotor flight platform 402 is provided with a flight controller module 411, the flight controller module 411 is connected to the main control module 401 in communication, and the flight controller module 411 is used to control the multi-rotor according to the control instructions of the main control module 401. The flight platform 402 flies.

Preferably, a GNSS positioning module 412 (ie, a satellite positioning module) can also be set on the multi-rotor flight platform 402, and the position of the multi-rotor flight platform 402 can be measured through the GNSS positioning module 412, and the position measurement results of the GNSS positioning module 412 can be integrated and the position measurement result of the position tracker 500 to obtain valid position data of the multi-rotor flight platform 402 (for example, when the GNSS signal is good, use the average or weighted average of the two as valid position data, when the GNSS signal is poor , use the position measurement result of the position tracker 500 as valid position data) to further improve the accuracy of the position data.

In this document, relational terms such as first and second, etc. are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such existence between these entities or operations. The actual relationship or sequence.

The above descriptions are merely examples of the present application, and are not intended to limit the protection scope of the present application. For those skilled in the art, various modifications and changes may be made to the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of this application shall be included within the protection scope of this application.

Claims (10)

1. a method for controlling a flying robot, for controlling a flying robot to carry out an aerial object grasping operation, it is characterized in that, the flying robot comprises a multi-rotor flying platform, a mechanical arm and is connected to the multi-rotor flying platform and the mechanical A six-degree-of-freedom pose compensation device between arms, the six-degree-of-freedom pose compensation device is used to adjust the pose of the robotic arm base of the robotic arm; the flying robot control method includes the steps: A1. Control the flying robot to fly to a reference hovering point, and start the robotic arm to grab the air object; A2. During the grabbing operation, obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the robotic arm; A3. Calculate the disturbance force and disturbance moment received by the robotic arm base according to the first kinematics information and the second kinematics information; A4. Calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance moment; A5. Control the six-degree-of-freedom pose compensation device to output the compensation force to compensate for the pose disturbance of the robotic arm base. 2. flying robot control method according to claim 1, is characterized in that, step A1 comprises: controlling the flying robot to perform a cruising flight to search for the aerial object; After searching for the aerial object, control the flying robot to approach the aerial object; When the aerial object enters the working range of the robotic arm, it enters a hovering operation state, and the robotic arm is activated to perform a grasping operation to grasp the aerial object. 3. flying robot control method according to claim 1, is characterized in that, step A3 comprises: calculating a first disturbance force and a first disturbance moment caused by the multi-rotor flight platform according to the first kinematics information; A second disturbance force and a second disturbance moment caused by the motion of the manipulator are calculated according to the second kinematics information. 4 . The method for controlling a flying robot according to claim 3 , wherein the first kinematic information comprises the first attitude acceleration of the multi-rotor flying platform; the first attitude acceleration comprises the multi-rotor flying platform. The translational acceleration and the angular acceleration of the three axial directions of the rotor flight platform; The step of calculating the first disturbance force and the first disturbance moment caused by the multi-rotor flight platform according to the first kinematics information includes: The first disturbance force and the first disturbance moment are calculated according to the following formulas:

in,

is the first disturbance force,

is the first disturbance torque,

is the mass of the multi-rotor flight platform,

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

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

,

,

are the translational accelerations of the three axial directions of the multi-rotor flight platform, respectively,

is the angular acceleration of the multi-rotor flight platform,

,

,

are the angular accelerations of the three axial directions of the multi-rotor flight platform. 5. The method for controlling a flying robot according to claim 3, wherein the second kinematics information comprises the rotation angle of each joint of the robotic arm; The described step of calculating the second disturbance force and the second disturbance moment caused by the motion of the robotic arm according to the second kinematics information includes: Calculate the position vector of the center of mass of each joint of the robotic arm relative to the base of the robotic arm according to the rotation angle of each joint of the robotic arm; Calculate the total centroid position vector of the manipulator relative to the manipulator base according to the following formula:

in,

is the mass of the robotic arm,

is the mass of the robotic arm base,

for the first

the quality of a joint,

for the first

The position vector of the center of mass of each joint relative to the base of the robotic arm,

is the total centroid position vector of the robotic arm relative to the robotic arm base; The second disturbance force and the second disturbance moment are calculated according to the following formulas:

in,

is the second disturbance force,

is the second disturbance torque,

is the mass of the object currently grasped by the robotic arm,

is the gravitational acceleration,

is the position vector of the center of mass of the object currently grasped by the robotic arm relative to the base of the robotic arm. 6 . The flying robot control method according to claim 3 , wherein the six-degree-of-freedom pose compensation device comprises a fixed platform fixedly connected with the multi-rotor flying platform and six cables connected to the fixed platform and the fixed platform. 7 . The telescopic link between the bases of the robotic arm, the two ends of the telescopic link are respectively connected with the fixed platform and the base of the robotic arm through a universal hinge; The second kinematics information includes the second pose acceleration of the robotic arm base; Step A4 includes: Obtain the rod length of each telescopic link; Obtain the generalized gravity that the base of the robotic arm is subjected to; Obtain the relative angular velocity of the manipulator base relative to the multi-rotor flight platform; 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 For the angular velocity, a dynamic model based on the Newton-Euler equation was used to calculate the driving force of each telescopic link. 7 . The method for controlling a flying robot according to claim 6 , wherein, according to the second pose acceleration, the rod length, the first disturbance force, the first disturbance moment, the The second disturbance force, the second disturbance moment, the generalized gravity and the relative angular velocity are based on the dynamic model of the Newton-Euler equation, and the steps of calculating the driving force of each telescopic link include: Calculate the direction vector of the length direction of each telescopic link according to the rod length of each telescopic link; Calculate the Jacobian matrix according to the following formula:

; in,

is the Jacobian matrix,

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

are respectively the position vectors of the hinge points of the six telescopic links and the manipulator base relative to the manipulator base; The driving force of each of the telescopic links is calculated according to the following formula:

;

; in,

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

is the driving force matrix,

for

The inverse matrix of ,

is an identity matrix of order 3 × 3,

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

is the second pose acceleration of the robotic arm base,

is the generalized gravitational force on the base of the robotic arm,

is the relative angular velocity,

is the mass of the robotic arm,

is the mass of the robotic arm base,

is the first disturbance force,

is the first disturbance torque,

is the second disturbance force,

is the second disturbance torque. 8. A flying robot control device for controlling the flying robot to carry out aerial object grasping operations, wherein the flying robot comprises a multi-rotor flying platform, a mechanical arm, and is connected to the multi-rotor flying platform and the mechanical A six-degree-of-freedom pose compensation device between arms, the six-degree-of-freedom pose compensation device is used to adjust the pose of the robotic arm base of the robotic arm; the flying robot control device includes: a first execution module, configured to control the flying robot to fly to a reference hovering point, and start the robotic arm to perform a grasping operation to grasp the aerial object; a first acquisition module, used for acquiring the first kinematic information of the multi-rotor flight platform and acquiring the second kinematic information of the robotic arm during the grabbing operation; a first calculation module, configured to calculate the disturbance force and disturbance moment received by the manipulator base according to the first kinematics information and the second kinematics information; a second calculation module, configured to calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance torque; The first control module is configured to control the six-degree-of-freedom pose compensation device to output the compensation force to compensate for the pose disturbance of the robotic arm base. 9. An electronic device, characterized in that it comprises a processor and a memory, wherein the memory stores a computer program executable by the processor, and when the processor executes the computer program, the processor executes the steps of claims 1-7. Any one of the steps in the flying robot control method. 10. A flying robot control system, characterized in that it comprises a flying robot, a position tracker and a ground station, wherein the flying robot and the position tracker are all connected in communication with the ground station; The position tracker is used to measure the position of the flying robot and send it to the flying robot through the ground station; The flying robot includes a main control module, a multi-rotor flight platform, a mechanical arm, and a six-degree-of-freedom pose compensation device connected between the multi-rotor flight platform and the mechanical arm. The six-degree-of-freedom pose compensation device For adjusting the pose of the robotic arm base of the robotic arm, the main control module is used for: Controlling the flying robot to fly to a reference hovering point, and starting the robotic arm to grab an object in the air; During the grabbing operation, obtain the first kinematic information of the multi-rotor flight platform, and obtain the second kinematic information of the mechanical arm; Calculate the disturbance force and disturbance moment received by the manipulator base according to the first kinematics information and the second kinematics information; Calculate the compensation force to be paid by the six-degree-of-freedom pose compensation device according to the disturbance force and the disturbance moment; The six-degree-of-freedom pose compensation device is controlled to output the compensation force to compensate for the pose disturbance of the robotic arm base.

Images