Disclosure of Invention
The invention aims to provide a quadrotor autonomous tracking landing system and a quadrotor autonomous tracking landing method based on color block landmarks, and designs a square color block landmark containing four different colors, so that a central target value of the quadrotor is obtained, and the problem of follow-up landing of the quadrotor is solved.
The invention relates to a quad-rotor aircraft autonomous tracking landing system based on color block landmark identification, which comprises a central main control unit, a positioning identification module and a following landing module which are arranged on a quad-rotor aircraft body, and color block landmarks which are arranged on the ground, wherein: the positioning identification module is used for obtaining a central target value of the four-rotor aircraft, and an output signal of the positioning identification module is sent to the central main control unit; the central main control unit is used for reading sensing data information, ensuring the attitude control and operation of the four-rotor aircraft, and outputting a signal to the follow-up landing module;
and the following descending module adopts a PID algorithm to follow the positioning identification system to obtain a central target value of the color block landmark, and is used for realizing the adjustment of the machine head direction firstly when the tracking precision requirement is met and then carrying out automatic descending.
The invention discloses a quad-rotor aircraft autonomous tracking landing method based on color block landmark identification, which specifically comprises the following steps:
step 1, firstly, placing color block landmarks on a target place or a mobile platform with a flat surface, starting a four-rotor aircraft to reach a proper height, and ensuring the color block landmarks to be in a visual field range of a positioning and identifying system in the process;
step 2, detecting and identifying the color block, adopting a PID algorithm to follow the central target point of the color block landmark obtained by the positioning and identifying system,
calculating the PID controller output u (t), the expression is as follows:
where e (t) represents the difference between the target value and the current value, kpDenotes the proportional gain, kiRepresenting the integral gain, kdRepresents a differential gain;
step 3, when the number of the detected effective color blocks is more than or equal to 2, indicating that the color block landmark is successfully identified, and taking an average value of the detected color block coordinates as a target point to follow; along with the shortening of the landmark distance between the four-rotor aircraft and the color blocks, the positioning and identifying system completely identifies the color blocks with four different colors, and further obtains the central target point of the positioning and identifying system;
when the four-rotor aircraft flies right above the color block landmark and tracks to meet the precision, the four rotors rotate according to the requirement
The yaw angle of the aircraft is firstly adjusted in the aircraft head direction and then automatically landed, so that directional landing is realized; in the landing process, a constant-speed landing method is adopted to avoid overlarge ground impact caused by the fact that the four-rotor aircraft lands too fast.
Compared with the prior art, the four-rotor aircraft nose landing device can solve the problems and provide a direction reference for the four-rotor aircraft nose, so that the four-rotor aircraft can land accurately in a required direction.
Detailed Description
The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and examples.
Fig. 1 is a schematic diagram of an autonomous tracking and landing system of a quad-rotor aircraft based on color block landmarks according to the present invention. The system comprises a four-rotor aircraft body, a central main control unit, a color block landmark, a positioning identification module and a following landing module. The central main control unit, the positioning and identifying module and the following and landing module are all located on the four-rotor aircraft body, the color block landmarks are arranged on the ground, and the system realizes identification and tracking of the color block landmarks.
Specifically, the four-rotor aircraft body comprises a frame with a wheelbase not less than 360mm, a brushless motor with a propeller, an electronic speed regulator, a lithium battery power system, a laser height ranging module and an undercarriage. The specific modules of each section are not limited in the type of use, and most open source quad-rotor aircraft contain these sections.
Specifically, the central main control unit uses STM32 as a processor chip, and includes a peripheral working circuit and an attitude sensor module, and functions of the central main control unit include reading data information such as sensors and the like, and ensuring attitude control and related operations.
Specifically, a color block landmark is a square area, which includes four small square areas with different colors, and the detection background of the color block landmark cannot be the same as the four colors. Fig. 2 is a diagram of an example of a color block landmark designed by the present invention. Taking white background as an example, the upper left of the example graph of color block landmarks is red color block landmarks, the upper right is yellow color block landmarks, the lower left is blue color block landmarks, and the lower right is green color block landmarks. And obtaining the central target point of the whole color block landmark by judging the central coordinate of each color block landmark. Because the color block landmark has the specific direction of four colors, the four-rotor aircraft can judge the drift angle between the four-rotor aircraft and the color block landmark, and further provides a direction reference for the aircraft head of the four-rotor aircraft, and the control requirement of the four-rotor aircraft on the drift angle during landing is met. The problem can be solved well by taking the central target point in fig. 2 as a reference, and the color patches of the four colors are always in the camera view during the landing process. Note that the coordinate axes and the central target point in fig. 2 are only used to clearly express the working principle of the color patch landmark, and should not appear in the actual landmark. Since the actual size of the color patch landmarks is related to the quad-rotor aircraft altitude, the partial size reference is specifically shown in table 1.
TABLE 1
Color block landmark size
|
Flying height
|
200mm*200mm
|
30cm-100cm
|
500mm*500mm
|
100cm-200cm
|
1000mm*1000mm
|
200cm-500cm |
Specifically, the positioning identification module is an OpenMV camera module which is arranged in the center of the bottom of the four-rotor aircraft and is connected with the central main control unit through a serial port, and the module acquires a central target value of the four-rotor aircraft by identifying the central coordinate position of the landmarks of the four color blocks in the image. The OpenMV camera can quickly and accurately identify the required color block and obtain its center coordinates, thereby conveniently calculating the center target point described in fig. 2. In order to reduce the conservatism, the color block landmark is considered to be successfully recognized when only two color blocks with different colors are recognized initially.
Specifically, the following landing module is a controller located in a central main control unit, the function of the following landing module is to follow the central target value of the color block landmark obtained by a positioning and identifying system by adopting a PID algorithm, when the tracking precision requirement is met, the machine head direction is firstly adjusted, and then the automatic landing is carried out, wherein the PID algorithm is as shown in the specification
Where e (t) represents the difference between the target value and the current value, kpExpressing proportional gain, and playing a role in accelerating the response speed of the system and improving the regulation precision of the system, kiRepresenting integral gain, serving to eliminate residual errors and to regulate steady-state time, kdThe differential gain is expressed, and the functions of improving the dynamic performance of the system, predicting the error trend and correcting in advance are achieved. These parameters need to be adjusted according to the actual situation. In particular, when the following control is carried out, e (t) is the deviation of the central target point calculated by the positioning identification system and the position of the four-rotor aircraft; when the automatic landing is performed, e (t) is the difference between the desired landing speed and the landing speed. Different from the traditional height control method, the four-rotor aircraft keeps constant landing speed when landing, and avoids overlarge ground impact caused by the fact that the four-rotor aircraft lands too fast.
Fig. 3 is a flowchart illustrating an overall method for autonomous tracking and landing of a quad-rotor aircraft based on color patch landmark identification according to the present invention. The method specifically comprises the following steps:
step 1, firstly, placing color block landmarks on a target place or a mobile platform with a flat surface, starting a four-rotor aircraft to reach a proper height, and ensuring the color block landmarks to be in a visual field range of a positioning and identifying system in the process;
step 2, detecting and identifying the color block, adopting a PID algorithm to follow the central target point of the color block landmark obtained by the positioning and identifying system,
calculating the PID controller output u (t), the expression is as follows:
where e (t) represents the difference between the target value and the current value, kp、kiAnd kdRepresenting control parameters that need to be adjusted according to the actual situation. Particularly, when the following control is carried out, e (t) is the deviation between the central target point calculated by the positioning identification system and the self position of the four-rotor aircraft, and when the automatic landing is carried out, e (t) is the difference between the expected landing speed and the landing speed;
step 3, when the number of the detected effective color blocks is more than or equal to 2, indicating that the color block landmark is successfully identified, and taking an average value of the detected color block coordinates as a target point to follow; along with the shortening of the landmark distance between the four-rotor aircraft and the color blocks, the positioning and identifying system completely identifies the color blocks with four different colors, and further obtains the central target point of the positioning and identifying system;
when the four-rotor aircraft flies right above the color block landmarks and the tracking accuracy is met, rotating the yaw angle of the four-rotor aircraft according to the requirement, firstly adjusting the aircraft head direction, and then automatically landing, thereby realizing directional landing; in the landing process, a uniform-speed landing method is adopted, so that overlarge ground impact caused by the fact that the four-rotor aircraft lands too fast is avoided.
When the quad-rotor aircraft follows the color lump landmark, the problem that the color lump landmark is lost in the visual field of the positioning and identifying system due to the angle change is easy to occur. As shown in fig. 4, a representation of the problem of target loss that a quad-rotor aircraft is prone to following a patch landmark is shown. It can be seen that the chromotagous landmarks can be successfully identified when the quad-rotor aircraft is in a level position. After the central target point is calculated, the quad-rotor aircraft needs to change the posture of the quad-rotor aircraft to track the color block landmark, so that the problem of visual field loss occurs. Aiming at the problem, the following landing system is solved by using a posture decoupling method, and the method specifically comprises the following steps:
delaying for judgment, and considering the lost target after losing the color block landmark for one second;
attitude compensation, namely compensating the angle multiplied by the coefficient for e (t) when detecting that the target is lost due to the angle change of the quadrotor;
to avoid abrupt changes when the missing target is identified, e (t) slowly drops to zero in a second.
The invention is not the best known technology.
The foregoing detailed description is provided for the purpose of illustrating the technical concepts and features of the present invention, and is not intended to limit the scope of the present invention. All equivalent changes and modifications made according to the spirit of the present invention should be covered within the protection scope of the present invention.