Control system and method of multifunctional field planting robot
Technical Field
The invention belongs to the field of automation, and particularly relates to a control system and method of a multifunctional field planting robot.
Background
The embedded development control system plays an important role in the fields of robots, automation industry, aerospace, medical appliances and the like, wherein the embedded development control system is a widely applied control system, and the single chip microcomputer is widely applied to various engineering fields and is an important core which can not be lacked by various robots, intelligent equipment and automation equipment; compared with the traditional embedded equipment, the MCU chip of the single chip microcomputer has the advantages of small volume, small power consumption, multiple interfaces, high timeliness, easiness in module expansion, convenience in external debugging and the like, is concerned and widely applied, and is mainly used for robots, unmanned planes, automation equipment and the like; the control system is built by an STM32F4 series-based single chip microcomputer and a LINUX in an embedded mode.
The common directional tree planting robot cannot complete the directional tree planting task quickly and reliably under various specific conditions, and the problem of low universality exists in different scenes.
Disclosure of Invention
The purpose of the invention is as follows: the invention can quickly and reliably complete the directional tree planting task based on the mechanical innovation directional tree planting robot under various specific conditions, and execute different tree planting schemes in different scenes.
The technical scheme is as follows: a control system and a method of a multifunctional planting robot comprise a planting robot and a control system, wherein the planting robot comprises a mechanical soil covering module, a drilling module, a mechanical arm module, a roller conveying module, a crawler chassis moving module and a motor; the control system comprises a single chip microcomputer module, an external module, a control module and a power supply, wherein the single chip microcomputer is distributed with a plurality of different power supply interfaces and signal interfaces, and the external module comprises a positioning module, a communication module, a motion module and a feedback module; the positioning module is used for positioning through SLAM, UWB, odometer and IMU; the communication module carries out communication through the SUBS in a Bluetooth or Wi-Fi mode; the motion module and the feedback module perform motion control through a CAN bus and a Hall element;
the control method of the multifunctional planting robot comprises the following steps:
(1) And executing the moving task: the movement process from the throwing point of the planting robot to the different movement states of the hole fixing point of the seedling is indicated;
(2) And (3) executing a hole positioning task: the planting robot is appointed to control the process of digging a hole by a drill bit above a hole fixing point;
(3) And (3) executing a supply task: a supplying process that the planting robot conveys the saplings in the storage cylinder to a mechanical arm is indicated;
(4) Executing a constant value task: the method comprises the following steps that a mechanical arm on a working turntable of a planting robot receives a tree seedling and vertically places the tree seedling into a planting hole;
(5) And (3) executing a soil burying task: the method comprises the following steps that a planting robot buries soil in a sapling through a soil pushing device fixed on a chassis;
(6) And (3) executing a return voyage task: and the process that the planting robot returns to the robot release point through the path planning of the odometer and the LINUX is shown.
The step (1) of the control method is specifically as follows: the movement process is completed by the rotation of the crawler belt, the different movement states are completed by the rotation of the differential crawler belt, the control of the rotation of the crawler belt is completed by the control of a motor, and the motor controls the transmission and the reception of data through a CAN1 bus in the embedded single chip microcomputer; the single chip takes the target speed as input, the real-time speed of the chassis as feedback, and outputs the output current of the chassis motor to form a PID closed loop, and the output current of the chassis motor is controlled by a PID algorithm.
The step (2) of the control method is specifically as follows: the hole digging process is divided into two parts of controlling the drill to descend and controlling the drill to rotate, the drill descends and the drill rotates through mechanical design, the ratio of the height number of circles of a descending screw rod to the drill drilling speed of the drill is 1:3, the mechanical design is connected with an output shaft of a motor, and the motor controls the CAN2 bus in the embedded single chip microcomputer to send and receive data; the single chip microcomputer takes a target rotating speed as input, the real-time speed of the drill bit is taken as feedback through a TD filter, the output current of the chassis motor is output to form an NLPID closed loop, and the motor speed is controlled by using an NLPID algorithm; the locked-rotor detection of the drill bit in the rotating process aims to prevent the drill bit from being damaged due to the locked-rotor of a hard object; the locked rotor detection scheme is characterized in that the current and speed changes of the motor are detected in real time in a data packet of the CAN2 bus, and weighted proportion operation is carried out through kalman filtering to reverse when large amplitude vibrates.
The step (3) of the control method is specifically as follows: the supply process comprises two parts, namely rotation of an inner rotary disc of the storage cylinder and rotation of the working rotary disc, wherein the rotation of the rotary disc of the storage cylinder and the rotation of the working rotary disc are simultaneously controlled by a gear set rotary disc containing ratchet wheels, the gear set containing the ratchet wheels rotates the working rotary disc and the storage cylinder rotary disc driven by the ratchet wheels in proportion to ensure that the working rotary disc rotates for a circle, and the input end of the gear set rotary disc containing the ratchet wheels, which is supplied by the supply rotary disc, is linked to the output shaft of the motor; the motor control sends and receives data through a CAN2 bus in the embedded single chip microcomputer;
the single chip microcomputer takes a target angle as input, a turntable real-time angle as feedback and outputs the ideal speed of a turntable motor to form a PID angle outer closed loop, the ideal speed of the turntable motor which is subjected to TD filter, namely low-pass filtering, is taken as input, the turntable real-time angle is taken as ESO expansion observer feedback, and the turntable motor current is output through a PD combination scheme of NLSEF nonlinear combination to form an ADRC closed loop, the angle of a turntable part motor is accurately controlled by using an ADRC algorithm, and compared with gyroscope data, the relative angle of a mechanical arm on the turntable is ensured to be accurate, and the accurate hole-locating parameter is ensured.
The step (4) of the control method specifically comprises the following steps: the mechanical arm is controlled by 4 large-torque steering engines, the large-torque steering engine control means that high-frequency PWM square waves are input into each steering engine of the mechanical arm through the embedded single chip microcomputer, attitude simulation is carried out in an upper computer through DEBUG, after PWM duty ratios are determined, the PWM duty ratios are assigned in sequence through a priority ranking algorithm, and the effect that the mechanical arm moves in sequence is achieved.
The step (5) of the control method is specifically as follows: the soil pushing device is fixed at the front end of the chassis; the soil burying task is realized by moving the chassis back and forth to bury soil.
The step (6) of the control method is specifically as follows: the odometer is a mode of mixing data into LINUX path planning after accumulating through kinematics by recording the accumulated number of turns of the motor, and the robot for field planting returns to a throwing point through a chassis moving task.
Has the beneficial effects that: compared with the prior art, the invention combines a plurality of modules, greatly reduces the complexity and the difficulty of parameter adjustment, module addition and strategy change, provides more application modes and application occasions for the planting robot, and ensures that the planting robot has wider application prospect.
Drawings
Fig. 1 is a three-dimensional model diagram of a planting robot actually controlled by the control system in the embodiment of the invention;
FIG. 2 is a flowchart illustrating overall task control according to an embodiment of the present invention;
FIG. 3 is a diagram of the PID algorithm according to an embodiment of the invention;
FIG. 4 is an application diagram of the NLPID algorithm in the embodiment of the present invention;
FIG. 5 is a graph of the speed control result of the embodiment of the present invention using NLPID algorithm;
FIG. 6 is a current diagram of the embodiment of the present invention applying NLPID algorithm;
fig. 7 is the locked rotor detection algorithm according to the embodiment of the present invention;
FIG. 8 is a diagram of the ADRC algorithm application in an embodiment of the present invention;
FIG. 9 is a graph of the timing result of the ADRC algorithm;
FIG. 10 is a current diagram illustrating the application of the ADRC algorithm in accordance with an embodiment of the present invention;
FIG. 1: the device comprises a mechanical soil covering module, a 2-soil drilling module, a 3-mechanical arm module, a 4-roller conveying module and a 5-crawler moving platform module.
Detailed Description
The technical scheme of the invention is further explained by combining the attached drawings.
FIG. 1 is a three-dimensional model diagram of a planting robot actually controlled by the control system in the embodiment of the invention, the planting robot comprises a mechanical soil covering module 1, a drilling module 2, a mechanical arm module 3, a roller conveying module 4, a crawler moving platform module 4 and a motor,
based on the embedded development control system of the planting robot, the invention provides an overall control scheme method which is used for controlling different modules to coordinate and efficiently complete different tasks, and the Huhu control system needs to control 5M 3508 motors and one GM6020 motor (4M 3508 motors are used for chassis control, and 1M 3508 motor is used for realizing the functions of seedling conveying of the roller conveying module and conversion of the mechanical arm module and the earth drilling module).
The control method comprises the following specific processes:
firstly, setting LINUX and an external module to set CH340 serial port data to be sent to an embedded single chip microcomputer, so as to realize communication between the embedded single chip microcomputer and the embedded LINUX, and specifically, with reference to fig. 2, a schematic diagram of main control interconnection of the embedded LINUX, the embedded single chip microcomputer and an upper computer is shown; and secondly, starting the upper computer to be connected with the embedded single chip microcomputer to carry out mutual communication through a 2.4 GWIFII signal, and preparing for the next corresponding parameter adjustment and strategy change. Under the condition of being connected to the LINUX and the upper computer, a FreeRTOS system is operated in the singlechip to start task scheduling.
The FreeRTOS has the function of making up the disadvantage that the single-chip microcomputer only runs in a single thread, simulating multiple threads to run simultaneously, performing message transmission between tasks through semaphores and events, and performing corresponding tasks when the corresponding semaphores trigger (refer to a task list in FIG. 2).
When a mobile task is executed, the single chip sends CAN data to control a chassis motor, and sends the magnitude of the output current of the motor through a PID algorithm (refer to FIG. 3). According to the method shown in fig. 3, the difference value between the set rotation speed and the real-time feedback speed value of the motor is used as the input of the current, and the current is calculated by three weighted addition modes of proportional integral derivative.
When the hole-fixing task is executed, the single chip microcomputer sends CAN data to control a drill motor, the magnitude of the output current of the motor is sent through an NLPID algorithm (refer to fig. 4), a TD tracking differentiator is added into the difference value of the set rotating speed and the real-time feedback speed value of the motor to serve as a low-pass filter, and the following weighting is the same as the PID. According to fig. 5, 6, after the addition of the filter, the transition is arranged and the tracking differentiation is improved. The power change of the motor is stabilized, the overshoot is improved, and the stable rotating speed is ensured. In order to avoid the damage of the drill bit, the locked rotor detection of the rotation process of the drill bit is carried out. The locked rotor detection scheme is characterized in that the current and speed changes of the motor are detected in real time in a data packet of the can2 bus, and the current and speed changes are reversed when the amplitude is oscillated by performing weighted proportion operation through Kalman filtering. The algorithm is shown in fig. 7.
When the supply task is executed, the single chip microcomputer sends CAN data to control the drill motor, the magnitude of the output current of the motor (refer to fig. 8) is sent through a PD algorithm serial ADRC algorithm, the speed angle of the motor target and the real-time feedback angle difference value are subjected to proportional and differential weighting operation, and the motor target speed value is calculated. According to the graph shown in fig. 7, after the target speed value is obtained, the effective value of the current is output after complex weighting and feedback are carried out through a TD differential tracker and NLSEF nonlinear configuration and an ESO extended observer, the current output is accurately controlled, and the angle closed loop is achieved. Specific effects can be seen in fig. 9 and 10. According to the graph shown in fig. 8 and fig. 9, the tracking differentiator, the nonlinear combination and the extended state observer are arranged to be combined, so that the problems of overshoot oscillation, overlarge static error, overlarge error and the like are solved.
When a planting task is executed, the mechanical arm sequentially acts through the posture pre-corrected by the upper computer, and other factors such as different lengths of the seedlings, the fixed hole depth and the like are considered in a specific implementation scheme. When the earth burying and return voyage tasks are executed, the essence of the method is that the LINUX carries out path planning on the customized robot, and the disclosed source code is added into a communication protocol customized according to the data length to control the ground motion of the robot. And finishing a complete directional tree planting engineering operation.
Compared with the traditional control system, the control system has the characteristics of high timeliness and easiness in changing, the data packet format of the data in the transmission process can be added according to the expansion of the module, more tasks can be completed in a single time period, and the control system has wider application prospect.
Although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that modifications may be made to the embodiments described in the foregoing detailed description, or equivalent changes may be made in some of the features of the embodiments described above. All equivalent structures made by using the contents of the specification and the attached drawings of the invention can be directly or indirectly applied to other related technical fields, and all the equivalent structures are within the protection scope of the invention.