阅读说明：本技术 搭载机械手和重力平衡装置的无人机自主平衡控制方法 (Unmanned aerial vehicle autonomous balance control method carrying manipulator and gravity balance device ) 是由 黄正宗 王雪燕 张玉江 王亮 陈霖 谭稳巧 公月 于 2021-09-01 设计创作，主要内容包括：本申请涉及一种搭载机械手和重力平衡装置的无人机自主平衡控制方法,通过调整配重坐标位置可实现无人机机械手组合体重心的动态调整,以防机械手工作时导致的无人机失稳。在正常使用时,它可以实现无人机重心的主动平衡,相较于目前在已造成机身倾侧后陀螺仪传感工作才能完成无人机平衡的被动技术,本发明由于其实时的对应重心调节功能,可降低飞控系统平衡机身的负担,增强无人机的飞控性能。若已知机械手的工作路径,则可计算出配重的移动路径,实时保持无人机稳定。(The application relates to an unmanned aerial vehicle autonomous balance control method carrying a manipulator and a gravity balancing device, which can realize dynamic adjustment of the gravity center of an unmanned aerial vehicle manipulator assembly by adjusting the position of a counterweight coordinate to prevent instability of the unmanned aerial vehicle caused by the operation of the manipulator. When the unmanned aerial vehicle is normally used, the active balance of the gravity center of the unmanned aerial vehicle can be realized, and compared with the passive technology that the unmanned aerial vehicle can only be balanced through the sensing work of a gyroscope after the unmanned aerial vehicle body tilts, the unmanned aerial vehicle has the advantages that the burden of the balance of the unmanned aerial vehicle body of the flight control system can be reduced due to the real-time corresponding gravity center adjusting function, and the flight control performance of the unmanned aerial vehicle is enhanced. If the working path of the manipulator is known, the moving path of the counterweight can be calculated, and the stability of the unmanned aerial vehicle can be kept in real time.)

1. An unmanned aerial vehicle autonomous balance control method carrying a manipulator and a gravity balancing device is characterized by comprising the following steps:

s1, firstly determining the gravity center position (x) of the manipulator_c1,y_c1) The gravity center position of the counterweight is (x)_c2,y_c2)。

S2, after the unmanned aerial vehicle is started, the unmanned aerial vehicle is initially in a vertical takeoff state, and the weight in the gravity center calculation control center 1The center position calculation and analysis unit starts to calculate the center of gravity position of the robot arm and the balance weight and calculates the center of gravity position in two-dimensional coordinates (x)_c,y_c) And (6) displaying. This two-dimensional coordinate uses unmanned aerial vehicle barycentric position as the initial point, and the level is the x coordinate right, and the level upwards is the y coordinate.

S3, if the center of gravity position of the whole assembly is at the center of gravity of the unmanned aerial vehicle, the x coordinate and the y coordinate of the center of gravity position of the unmanned aerial vehicle are set to be (0,0), namely x is_c＝0,y_cIf the unmanned aerial vehicle runs on the ground, the micro motor control chip in the center of gravity calculation control does not send driving instructions to the first micro motor, the second micro motor and the third micro motor, the first micro motor, the second micro motor and the third micro motor are self-locked, and the unmanned aerial vehicle continues to execute a flight program.

S4, if the gravity center position of the assembly is not at the gravity center position (0,0) of the unmanned aerial vehicle during takeoff, namely x_cNot equal to 0 or y_cAnd if not equal to 0, the micro motor control chip in the center of gravity calculation control center sends driving instructions to the first micro motor, the second micro motor and the third micro motor.

And S5, when the manipulator starts to work, the manipulator can stretch or rotate, and the gravity center position of the manipulator can change. The working path of the robot can be divided into a random path and a fixed path. The unmanned aerial vehicle autonomous balance control system executes different driving instructions according to the two conditions.

2. The unmanned aerial vehicle autonomy balance control method of claim 1, wherein (x) is described in S1_c1,y_c1) The calculation method is as follows:

setting the gravity center (x) of each moving unit of the manipulator₁,y₁)，(x₂,y₂)，(x₃,y₃) … …, then

In the formula, m_iThe mass of each single body of the manipulator is (x) because the balance weight is a regular geometric body_c2,y_c2) I.e. the geometric center of the counterweight。

3. The unmanned aerial vehicle autonomy balance control method of claim 1, wherein (x) is described in S2_c,y_c) The calculation method comprises the following steps:

where m' is the total mass of the robot and m "is the mass of the counterweight.

4. The unmanned aerial vehicle autonomous balance control method of claim 1, wherein the driving command of S4 is:

s6, the gravity center position calculation and analysis unit in the gravity center calculation control center calculates the x coordinate and the y coordinate of the gravity center position of the manipulator, and the x coordinate and the y coordinate are set as (a)₁,b₁) And sends it to the micro-motor control chip;

s7, driving the micro motor I and the micro motor III to move to-b by the micro motor control chip₁Drive the micro motor to move to-a₁At least one of (1) and (b);

s8, after the micro motor I, the micro motor II and the micro motor III are moved, the balance weight is moved to (-a)₁,-b₁) At the moment, the gravity center position of the assembly is at (0,0), and the unmanned aerial vehicle can stably fly;

and S9, after the unmanned aerial vehicle flies stably, self-locking the first micro motor, the second micro motor and the third micro motor, and repeating the driving process if the gravity center calculation control center monitors the change of the gravity center position in the real-time calculation process.

5. The unmanned aerial vehicle autonomy balance control method of claim 1, wherein when the working path of S5 is random, the driving command is the driving command of S4; when the working path described in S5 is fixed, the working path of the robot is fixed, that is, the expansion and contraction or movement path of the robot is known, that is, the gravity center movement path of the robot is known, and the x-coordinate of the gravity center position of the robot is setAnd y coordinates (x ', y ') and x ' and y ' are functions of time t, i.e., x ' ═ f₂(t)，y′＝f₁(t), assuming that x and y coordinates of the gravity center position of the counterweight are (x ", y"), and x "and y" are time t, x ″ -f ", which is obtained by symmetry of the x 'and y' coordinates of the gravity center position of the manipulator and the x" and y "coordinates of the counterweight about the origin, is obtained as a function of time t₂(-t)，y″＝-f₁(-t), i.e. the weight center of gravity movement path is exactly opposite to the manipulator center of gravity movement path; when the position of the center of gravity of the manipulator is along x ═ f₂(t)，y′＝f₁(t) moving, the chip is controlled by the micro-motor according to x ″ -f ″₂(-t)，y″＝-f₁(-t) driving the first and third micro-motors to y ″ -f₁At (-t), the micro motor is driven to move to x ″ -f ″₂At (-t).

Technical Field

The application relates to the field of unmanned aerial vehicle application, in particular to an unmanned aerial vehicle autonomous balance control method carrying a manipulator and a gravity balancing device.

Background

Along with the rapid development and popularization of the unmanned aerial vehicle technology, the application scene is continuously widened, and the unmanned aerial vehicle is widely applied to the fields of industrial inspection, aerial photography, agricultural plant protection, national defense security and the like. Meanwhile, the upper limit of the load which can be borne by the unmanned aerial vehicle is continuously improved, the unmanned aerial vehicle serving as a platform with specific functions is often confronted with the situation of load size, weight and shape change, and when the unmanned aerial vehicle carries the pan-tilt camera, the camera rotates to enable the gravity center position to shift; when unmanned aerial vehicle transported the express delivery, transported the goods load condition and respectively differed, caused the weight distribution unbalanced.

Unmanned aerial vehicle's weight distribution is often symmetrical, but the weight distribution of load is total asymmetric for unmanned aerial vehicle's focus position breaks away from the position of rotor lift, and then makes unmanned aerial vehicle angle skew, makes unmanned aerial vehicle unable hover, produces and jolts. Carry on the unmanned aerial vehicle of manipulator in manipulator operation process, can change the holistic focus position of unmanned aerial vehicle system, cause focus skew and constantly change, lead to unmanned aerial vehicle wing control system to be difficult to make the adjustment in real time. Overweight, asymmetric and real-time load that changes cause unmanned aerial vehicle flight to lose stable incentive, if can not effectively solve unmanned aerial vehicle focus skew and the change that the manipulator action leads to, unmanned aerial vehicle wing control system will not maintain fuselage balance and the most likely because some minor disturbances take place to overturn and crash. At present, the method for eliminating gravity center asymmetry is to adjust the mounting position of the counterweight load temporarily before takeoff, the method is long in time consumption and not accurate enough, the gravity center offset position is fixed and not changed, the gravity center offset is known in advance, and the method is not suitable for scenes with continuously changed gravity centers of manipulators.

Disclosure of Invention

An unmanned aerial vehicle autonomous balance control method carrying a manipulator and a gravity balancing device comprises the following steps:

s1, firstly determining the gravity center position (x) of the manipulator_c1,y_c1) The gravity center position of the counterweight is (x)_c2,y_c2)。

S2, after the unmanned aerial vehicle is started, the unmanned aerial vehicle is initially in a vertical takeoff state, and the gravity center position calculation analysis unit in the gravity center calculation control center 1 starts to calculate the manipulator and the balance weightPosition of center of gravity in two-dimensional coordinates (x)_c,y_c) And (6) displaying. This two-dimensional coordinate uses unmanned aerial vehicle barycentric position as the initial point, and the level is the x coordinate right, and the level upwards is the y coordinate.

S3, if the center of gravity position of the whole assembly is at the center of gravity of the unmanned aerial vehicle, the x coordinate and the y coordinate of the center of gravity position of the unmanned aerial vehicle are set to be (0,0), namely x is_c＝0,y_cIf the unmanned aerial vehicle runs on the ground, the micro motor control chip in the center of gravity calculation control does not send driving instructions to the first micro motor, the second micro motor and the third micro motor, the first micro motor, the second micro motor and the third micro motor are self-locked, and the unmanned aerial vehicle continues to execute a flight program.

S4, if the gravity center position of the assembly is not at the gravity center position (0,0) of the unmanned aerial vehicle during takeoff, namely x_cNot equal to 0 or y_cAnd if not equal to 0, the micro motor control chip in the center of gravity calculation control center sends driving instructions to the first micro motor, the second micro motor and the third micro motor.

And S5, when the manipulator starts to work, the manipulator can stretch or rotate, and the gravity center position of the manipulator can change. The working path of the robot can be divided into a random path and a fixed path. The unmanned aerial vehicle autonomous balance control system executes different driving instructions according to the two conditions.

Wherein (x) described in S1_c1,y_c1) The calculation method is as follows:

setting the gravity center (x) of each moving unit of the manipulator₁,y₁)，(x₂,y₂)，(x₃,y₃) … …, then

In the formula, m_iThe mass of each single body of the manipulator is (x) because the balance weight is a regular geometric body_c2,y_c2) Namely the geometric center of the counterweight.

Wherein (x) described in S2_c,y_c) The calculation method comprises the following steps:

where m' is the total mass of the robot and m "is the mass of the counterweight.

Wherein, the driving command of S4 is:

s6, the gravity center position calculation and analysis unit in the gravity center calculation control center calculates the x coordinate and the y coordinate of the gravity center position of the manipulator, and the x coordinate and the y coordinate are set as (a)₁,b₁) And sends it to the micro-motor control chip;

s7, driving the micro motor I and the micro motor III to move to-b by the micro motor control chip₁Drive the micro motor to move to-a₁At least one of (1) and (b);

s8, after the micro motor I, the micro motor II and the micro motor III are moved, the balance weight is moved to (-a)₁,-b₁) At the moment, the gravity center position of the assembly is at (0,0), and the unmanned aerial vehicle can stably fly;

and S9, after the unmanned aerial vehicle flies stably, self-locking the first micro motor, the second micro motor and the third micro motor, and repeating the driving process if the gravity center calculation control center monitors the change of the gravity center position in the real-time calculation process.

Wherein, when the working path of S5 is random, the driving command is the driving command of S4; when the working path described in S5 is fixed, the working path of the robot arm is fixed, that is, the expansion and contraction or movement path of the robot arm is known, that is, the gravity center movement path of the robot arm is known, the gravity center position of the robot arm is set to (x ', y ') in x-coordinate and y-coordinate, and x ' and y ' are functions of time t, that is, x ' ═ f₂(t)，y′＝f₁(t), assuming that x and y coordinates of the gravity center position of the counterweight are (x ", y"), and x "and y" are time t, x ″ -f ", which is obtained by symmetry of the x 'and y' coordinates of the gravity center position of the manipulator and the x" and y "coordinates of the counterweight about the origin, is obtained as a function of time t₂(-t)，y″＝-f₁(-t), i.e. the weight center of gravity movement path is exactly opposite to the manipulator center of gravity movement path; when the position of the center of gravity of the manipulator is along x ═ f₂(t)，y′＝f₁(t) during movement, the micro-motorThe control chip is as follows₂(-t)，y″＝-f₁(-t) driving the first and third micro-motors to y ″ -f₁At (-t), the micro motor is driven to move to x ″ -f ″₂At (-t).

Drawings

FIG. 1 is a block diagram of an autonomic balance system in accordance with an embodiment of the present invention;

FIG. 2 is a mechanical structure diagram of an autonomic balance system of an embodiment of the present application;

FIG. 3 is a diagram of a second embodiment of the autonomic balance system framework of the present application;

FIG. 4 is a mechanical structure diagram of a second embodiment of the present application;

FIG. 5 is a schematic diagram illustrating the calculation of barycentric coordinates of a center of gravity for the center of gravity calculation control of the present application;

FIG. 6 is a schematic diagram of a gravity center calculation driving process of the unmanned aerial vehicle-manipulator according to the present application;

fig. 7 is a schematic diagram of a driving process for calculating the center of gravity of the unmanned aerial vehicle-manipulator according to the present application.

Detailed Description

Reference will now be made in detail to embodiments of the present application, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are exemplary and intended to be used for explaining the present application and should not be construed as limiting the present application.

As shown in fig. 1 and 2, in the first embodiment, an autonomous balance control system of an unmanned aerial vehicle equipped with a manipulator includes a center of gravity calculation control center 1, a first moving module 2, a second moving module 3, a third moving module 4, a first guide rail groove 5, a second guide rail groove 6, a third guide rail groove 7, a first rack 8, a second rack 9, a third rack 10, a first micro motor 11, a second micro motor 12, a third micro motor 13, a counterweight 14, a first positioning baffle 15, a second positioning baffle 16, a third positioning baffle 17, and a fourth positioning baffle 18. The first moving module 2 is arranged on the first guide rail groove 5 and can move along the groove, the third moving module 4 is arranged on the third guide rail groove 7 and can move along the groove, two ends of the second guide rail groove 6 are fixedly connected with the first moving module 2 and the third moving module 4 respectively, and the second moving module 3 is arranged on the second guide rail groove 6 and can move along the groove; the first rack 8 is arranged on the first guide rail groove 5, the second rack 9 is arranged on the second guide rail groove 6, and the third rack 10 is arranged on the third guide rail groove 7; the first micro motor 11 is arranged on the first moving module 2, and a rotating shaft of the first micro motor 11 drives the first rack 8 to move; the second micro motor 12 is arranged on the second moving module 3, and a rotating shaft of the second micro motor 12 drives the second rack 9 to move; the third micro motor 13 is arranged on the third moving module 4, and a rotating shaft of the third micro motor 13 drives the third rack 10 to act; the counterweight 14 is fixedly arranged at the lower end of the second moving module 3; the unmanned aerial vehicle comprises a gravity center calculation control center 1, a first micro motor 11, a second micro motor 12 and a third micro motor 13, wherein the unmanned aerial vehicle is externally connected with a charging circuit to supply power through an internal power supply of the unmanned aerial vehicle, and a positive electrode lead and a negative electrode lead are respectively arranged on a first guide rail groove 5, a second guide rail groove 6 and a third guide rail groove 7; the first positioning baffle 15 and the second positioning baffle 16 are arranged at two ends of the first guide rail groove 5; and a second positioning baffle 16 and a third positioning baffle 17 are arranged at two ends of the third guide rail groove 7. The first positioning baffle 15, the second positioning baffle 16, the third positioning baffle 17 and the fourth positioning baffle 18 belong to limiting devices, and the second guide rail groove 6 is prevented from moving outwards to the first guide rail groove 5 and the third guide rail groove 7, so that the movement range of the movable counterweight is limited.

The unmanned aerial vehicle autonomous balance control system carrying the manipulator can be installed on the unmanned aerial vehicle through a fixed connection mechanism.

The gravity center calculation control center 1 comprises an overall gravity center position calculation and analysis unit and a micro motor control unit. The overall gravity center position calculation and analysis unit grasps the gravity center positions of the manipulator and the counterweight 14 through mechanical motion analysis or real-time state perception, the overall gravity center offset condition of the unmanned aerial vehicle assembly in the manipulator action process is solved, and the distance and the direction of the counterweight 14 required to move and the final coordinates to be located are obtained according to the offset condition.

The gravity center calculation control center 1 is in signal connection with the first micro motor 11, the second micro motor 12 and the third micro motor 13, namely, a micro motor control unit in the gravity center calculation control center 1 sends driving instructions to the first micro motor 11, the second micro motor 12 and the third micro motor 13 to drive the first micro motor 11, the second micro motor 12 and the third micro motor 13 to work. The driving instructions sent to the first micro motor 11 and the third micro motor 13 by the micro motor control unit are the same, so that the first micro motor 11 and the third micro motor 13 keep the same steering direction and rotating speed, and further the second guide rail groove 6 moves in parallel without torque, and after the first micro motor 11 and the third micro motor 13 work, the second guide rail groove 6, the second moving module 3, the second rack 9, the second micro motor 12 and the balance weight 14 arranged on the second guide rail groove 6 are driven to move on the y axis through the first rack 8 and the third rack 10; after the micro motor control unit sends a driving instruction to the second micro motor 12, the second micro motor 12 works, and the second moving module 3 and the balance weight 14 are driven to do x-axis motion through the second rack 9.

Furthermore, a self-locking device is arranged in the first micro motor 11, the second micro motor 12 and the third micro motor 13, so that the micro motor is prevented from abnormally moving a counterweight due to the inclination of the unmanned aerial vehicle under the condition that the driving instruction sent by the gravity center calculation control center 1 is not received, and the stability of the unmanned aerial vehicle is prevented from being influenced.

As shown in fig. 3 and 4, in the second embodiment, the autonomous balance control system for the unmanned aerial vehicle equipped with the manipulator includes a second center of gravity calculation control center 21, a fourth moving module 22, a fifth moving module 23, a sixth moving module 24, a fourth guide rail groove 25, a fifth guide rail groove 26, a sixth guide rail groove 27, a fourth rack 28, a fifth rack 29, a sixth rack 210, a fourth micro motor 211, a fifth micro motor 212, a second counterweight 214, a fifth positioning baffle 215, a sixth positioning baffle 216, a seventh positioning baffle 217, and an eighth positioning baffle 218. The four moving modules 22 are arranged on the four guide rail grooves 25 and can move along the grooves, the six moving modules 24 are arranged on the six guide rail grooves 27 and can move along the grooves, two ends of the five guide rail grooves 26 are fixedly connected with the four moving modules 22 and the six moving modules 24 respectively, and the five moving modules 23 are arranged on the five guide rail grooves 26 and can move along the grooves; a rack four 28 is arranged on the guide rail groove four 25, a rack five 29 is arranged on the guide rail groove five 26, and a rack six 210 is arranged on the guide rail groove six 27; the fourth micro motor 211 is arranged on the fourth moving module 22, a rotating shaft of the fourth micro motor 211 drives the fourth rack 28 to move, the fourth moving module 22 and the fourth guide rail groove 25 move in a limiting mode relatively, the sixth moving module 24 is not provided with the micro motor to drive to cause no locking, the sixth moving module 24 follows the fourth moving module 22 to move in a limiting mode relatively with the sixth guide rail groove 27, and the relative limiting movement process is synchronous with the fourth moving module 22; the micro motor five 212 is arranged on the moving module five 23, and a rotating shaft of the micro motor five 212 drives the rack five 29 to act; the second counter weight 214 is fixedly arranged at the lower end of the fifth moving module 23; the center of gravity calculation control center II 21, the micro motor IV 211 and the micro motor IV 212 are powered by an unmanned aerial vehicle external charging circuit through an unmanned aerial vehicle internal power supply, and a positive electrode lead and a negative electrode lead are respectively arranged on the guide rail groove IV 25, the guide rail groove IV 26 and the guide rail groove IV 27; a positioning baffle five 215 and a positioning baffle six 216 are arranged at two ends of the guide rail groove four 25; and a positioning baffle plate six 216 and a positioning baffle plate seven 217 are arranged at two ends of the guide rail groove six 27. The five positioning baffles 215, the six positioning baffles 216, the seven positioning baffles 217 and the eight positioning baffles 218 belong to limiting devices, and the five guide rail grooves 26, the four guide rail grooves 25 and the six guide rail grooves 27 are prevented from moving outwards, so that the moving range of the movable counterweight is limited.

The unmanned aerial vehicle autonomous balance control system carrying the manipulator can be installed on the unmanned aerial vehicle through a fixed connection mechanism.

The second gravity center calculation control center 21 comprises an overall gravity center position calculation and analysis unit and a micro motor control unit. The overall gravity center position calculation and analysis unit grasps the gravity center positions of the manipulator and the second counter weight 214 through mechanical motion analysis or real-time state perception, obtains the overall gravity center offset condition of the unmanned aerial vehicle assembly in the manipulator action process, and obtains the required moving distance and direction of the second counter weight 214 and the final coordinates to be located according to the offset condition.

The second center of gravity calculation control center 21 is in signal connection with the fourth micro motor 211 and the fifth micro motor 212, that is, the micro motor control unit in the second center of gravity calculation control center 21 sends driving instructions to the fourth micro motor 211 and the fifth micro motor 212 to drive the fourth micro motor 211 and the fifth micro motor 212 to work. After the four micro motors 211 and the five micro motors 212 work, the five guide rail grooves 26 are driven through the four racks 28, and the five moving modules 23, the five racks 29, the five micro motors 212, the two counter weights 214 arranged on the five guide rail grooves 26 and the six moving modules 24 fixedly connected with the five guide rail grooves 26 move along the y axis; after the micro motor control unit sends a driving instruction to the five micro motor 212, the five micro motor 212 works, and the five rack 29 drives the five moving module 23 and the two counter weights 214 to move along the x axis.

Furthermore, self-locking devices are arranged in the four micro motors 211 and the five micro motors 212, so that the micro motors are prevented from being influenced by abnormal movement of the counter weight due to inclination of the unmanned aerial vehicle under the condition that the driving instructions sent by the center of gravity calculation control center II 21 are not received, and stability of the unmanned aerial vehicle is prevented.

An unmanned aerial vehicle self-balancing control method carrying a manipulator is as follows, and the method is to complete the gravity balance when the manipulator acts by adjusting the rotating speed of each wing:

s1, the system uses the body of the unmanned aerial vehicle as a base point to determine a horizontal plane coordinate system and a coordinate system origin point, and the center of gravity (x) of each moving body of the manipulator is determined according to the center of gravity of each moving body of the manipulator₁,y₁)，(x₂,y₂)，(x₃,y₃) … …, determining the coordinates (x) of the center of gravity of the initial robot assembly_c1,y_c1) Determining initial unmanned aerial vehicle body barycentric coordinates (x)_c2,y_c2) Obtaining the gravity center coordinate (x) of the manipulator unmanned plane assembly_c,y_c)。

S2, when the unmanned aerial vehicle manipulator control system is ready to issue motion instructions to each joint of the manipulator, calculating to obtain the gravity center (x ') of each motion monomer after the joints execute the motion instructions according to the motion instructions'₁,y′₁)， (x′₂,y′₂)，(x′₃,y′₃) … …, determining coordinates (x ') of center of gravity of the manipulator assembly after the movement command is executed on the joint'_c1,y′_c1) And calculating to obtain the gravity center deviation (delta x) of the mechanical arm₁,Δy₁) And further obtaining the gravity center deviation (delta x, delta y) of the manipulator unmanned aerial vehicle assembly.

And S3, calculating to obtain wing adjustment quantity of the unmanned aerial vehicle according to the gravity center deviation quantity (delta x, delta y) of the manipulator unmanned aerial vehicle assembly, and determining the adjustment distribution quantity of each wing according to the quantity of the wings.

S4, when the robot control system of the unmanned aerial vehicle gives a motion instruction to each joint of the robot, a gravity center calculation control unit in the unmanned aerial vehicle body control system synchronously gives an instruction of adjusting distribution amount to an unmanned aerial vehicle wing motor, and the gravity center stability and balance in the operation process of the robot are completed.

As shown in fig. 5, the barycentric coordinates (x) of the initial robot assembly described in S1_c1,y_c1) The calculation formula of (a) may specifically be:

in the formula, m_iThe mass of each individual body of the robot arm, (x)_i,y_i) Are their corresponding barycentric coordinate values.

Wherein, manipulator unmanned aerial vehicle assembly barycentric coordinate (x) in S1_c,y_c) The calculation formula of (a) may specifically be:

in the formula, m' is the total mass of manipulator, and m "is the mass of unmanned aerial vehicle body.

Wherein the barycentric coordinate (x ') of the manipulator assembly in S2 after the joint has executed the motion command'_c1,y′_c1) The calculation formula of (a) may specifically be:

the robot center of gravity deviation (Δ x) described in S2₁,Δy₁) The calculation formula of (a) may specifically be:

Δx₁＝x′_c1-x_c1,Δy₁＝y′_c1-y_c1。

wherein, the formula for calculating the deviation of the center of gravity (Δ x, Δ y) of the robot-drone assembly in S2 may specifically be:

the calculation formula of the distribution adjustment amount of each wing specifically may be: f ═ b · ω²When unmanned aerial vehicle is four rotor unmanned aerial vehicle, in the formula, b is four rotor unmanned aerial vehicle's lift parameter, and omega is the rotational speed of rotor. When unmanned aerial vehicle focus was close to certain rotor, then this rotor increase rotational speed promotes this rotor lift increase.

An unmanned aerial vehicle self-balancing control method for carrying a manipulator is as follows, and the method is to complete the gravity balance when the manipulator acts by adjusting a balance weight loaded on a machine body:

s5, firstly determining the gravity center position (x) of the mechanical arm_c1,y_c1) The gravity center position of the counterweight is (x)_c2,y_c2)。

S6, after the unmanned aerial vehicle is started, the unmanned aerial vehicle is initially in a vertical takeoff state, the gravity center position calculation and analysis unit in the gravity center calculation control center 1 starts to calculate the gravity center positions of the manipulator and the balance weight, and two-dimensional coordinates (x) are used_c,y_c) And (6) displaying. This two-dimensional coordinate uses unmanned aerial vehicle barycentric position as the initial point, and the level is the x coordinate right, and the level upwards is the y coordinate.

S7, if the gravity center position of the whole assembly is at the gravity center of the unmanned aerial vehicle, the x coordinate and the y coordinate of the gravity center position of the unmanned aerial vehicle are set to be (0,0), namely x_c＝0,y_cIf the unmanned aerial vehicle runs on the ground, the micro motor control chip in the center of gravity calculation control does not send driving instructions to the first micro motor, the second micro motor and the third micro motor, the first micro motor, the second micro motor and the third micro motor are self-locked, and the unmanned aerial vehicle continues to execute a flight program.

S8, if the gravity center position of the assembly is not at the gravity center position (0,0) of the unmanned aerial vehicle during takeoff, namely x_cNot equal to 0 or y_cAnd if not equal to 0, the micro motor control chip in the center of gravity calculation control center sends driving instructions to the first micro motor, the second micro motor and the third micro motor.

And S9, when the manipulator starts to work, the manipulator can stretch or rotate, and the gravity center position of the manipulator can change. The working path of the robot can be divided into a random path and a fixed path. The unmanned aerial vehicle autonomous balance control system executes different driving instructions according to the two conditions.

Wherein, as shown in FIG. 5, (x) is as described in S5_c1,y_c1) The calculation method is as follows: setting the gravity center (x) of each moving unit of the manipulator₁,y₁)，(x₂,y₂)，(x₃,y₃) … …, then:

in the formula, m_iThe mass of each single body of the manipulator. Since the counterweight has a regular geometry, (x)_c2,y_c2) Namely the geometric center of the counterweight.

Wherein (x) in S6_c,y_c) The calculation method comprises the following steps:

where m' is the total mass of the robot and m "is the mass of the counterweight.

Wherein, the driving command of S8 is: the gravity center position calculation and analysis means in the gravity center calculation control center calculates the x-coordinate and the y-coordinate of the gravity center position of the manipulator, and the value is (a)₁,b₁) And sends it to the micro-motor control chip. The micro motor control chip drives the micro motor I and the micro motor III to move to-b₁Drive the micro motor to move to-a₁To (3). After the micro motor I, the micro motor II and the micro motor III are moved, the balance weight is moved to (-a)₁,-b₁) Department, the assembly focus position is in (0,0) department this moment, and unmanned aerial vehicle can stably fly. After the unmanned aerial vehicle flies stably, the first micro motor, the second micro motor and the third micro motor are self-locked. If the center of gravity is in calculation controlIf the heart monitors the change of the position of the center of gravity in the real-time calculation process, the above driving process is repeated, and the driving process is as shown in fig. 6.

When the operation path in S9 is random, the driving command is as in S8.

When the operation path described in S9 is fixed, the drive command is as follows: the working path of the manipulator is fixed, i.e. the telescopic or moving path of the manipulator is known, i.e. the moving path of the centre of gravity of the manipulator is known. Let the center of gravity position of the robot be (x ', y ') in x and y coordinates, and x ' and y ' be a function of time t, i.e., x ' ═ f₂(t)，y′＝f₁(t), assuming that x and y coordinates of the gravity center position of the counterweight are (x ", y"), and x "and y" are time t, x ″ -f ", which is obtained by symmetry of the x 'and y' coordinates of the gravity center position of the manipulator and the x" and y "coordinates of the counterweight about the origin, is obtained as a function of time t₂(-t)，y″＝-f₁(-t) i.e. the path of movement of the center of gravity of the counterweight is exactly opposite to the path of movement of the center of gravity of the robot. When the position of the center of gravity of the manipulator is along x ═ f₂(t)，y′＝f₁(t) moving, the chip is controlled by the micro-motor according to x ″ -f ″₂(-t)，y″＝-f₁(-t) driving the first and third micro-motors to y ″ -f₁At (-t), the micro motor is driven to move to x ″ -f ″₂At (-t), the driving process is shown in FIG. 7.

The first micro motor, the second micro motor and the third micro motor move in the process, the gravity center of the assembly can be kept stable in real time, and the unmanned aerial vehicle can fly stably.

In the two methods, because the flight balance of the unmanned aerial vehicle cannot be influenced in the direction of the gravity z axis, only the gravity center offset on the horizontal plane is calculated and balanced, but the variation on the z axis may cause the unmanned aerial vehicle to fluctuate, and further, the two-dimensional coordinate is converted into a three-dimensional coordinate to control the offset of the balance z axis through the rotating speed of each wing; furthermore, the mechanical sliding groove in the z-axis direction is added with the counterweight, the degree of freedom in the z-axis direction is increased, and the offset of the z-axis is balanced by the movement of the counterweight in the z-axis direction.

The invention can realize the dynamic adjustment of the gravity center of the unmanned aerial vehicle manipulator assembly, so as to prevent the instability of the unmanned aerial vehicle caused by the operation of the manipulator. When the unmanned aerial vehicle is normally used, the active balance of the gravity center of the unmanned aerial vehicle can be realized, and compared with the passive technology that the balance of the unmanned aerial vehicle can be completed only after the unmanned aerial vehicle body tilts, the unmanned aerial vehicle has the advantages that the burden of the balance of the unmanned aerial vehicle body can be reduced due to the real-time corresponding gravity center adjusting function, and the flight control performance of the unmanned aerial vehicle is enhanced. If the working path of the manipulator is known, the moving path of the counterweight can be calculated, and the stability of the unmanned aerial vehicle can be kept in real time.

In the description herein, reference to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the application. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.

Although embodiments of the present application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application, and that variations, modifications, substitutions and alterations may be made to the above embodiments by those of ordinary skill in the art within the scope of the present application.