Agricultural robot control method based on ROS and infrared obstacle avoidance

Abstract

The invention discloses an agricultural robot control method based on ROS and infrared obstacle avoidance. The method comprises the following steps: acquiring an environment image and an infrared signal around an agricultural robot and initial state data of an end effector; obtaining target data with obstacle information according to the environment image and the infrared signal; according to the target data with the obstacle information and the initial state data, through inverse Cartesian kinematics and motion trail measurement and calculation, a joint control instruction and a driving control instruction are generated respectively so as to control the agricultural robot to work; wherein the joint control instruction and the driving control instruction are respectively transmitted to the joint steering engine and the driving steering engine through the ROS. On one hand, based on mutual cooperation of the environment image and the infrared signal, image parameters can be further adjusted, and the accuracy and reliability of obstacle avoidance are improved; on the other hand, inverse Cartesian kinematics and motion trail calculation are adopted, the accuracy of coordinate analysis is improved, and the problems of motion trail disorder and the like are avoided.

Description

Technical Field

[0001] The invention relates to an agricultural robot control method based on ROS and infrared obstacle avoidance, and belongs to the technical field of agricultural robot control. Background Art

[0002] With the rapid development of science and technology, intelligent robot technology is gradually showing its great potential in agricultural production operations. In view of the problem that traditional manual operations are prone to errors and low efficiency, modern agricultural robots have significantly improved this situation with their high-precision positioning and navigation systems, and can adapt to environmental changes and different types of crops, and have broad development prospects.

[0003] However, the existing agricultural robots on the market often have the following problems, such as errors in matching parameters between the visual module and the agricultural robot, image acquisition affected by environmental factors, incomplete global camera shooting angles, or incomplete signals received by the servo, resulting in inaccurate image information transmitted to the agricultural robot's main control board, causing coordinate parsing errors and motion trajectory confusion when the agricultural robot plans the motion trajectory and controls the rotation of the servo. Summary of the invention

[0004] The purpose of the present invention is to overcome the shortcomings of the prior art and provide an agricultural robot control method based on ROS and infrared obstacle avoidance. On the one hand, based on the mutual cooperation of environmental images and infrared signals, the image parameters can be further adjusted to improve the accuracy and reliability of obstacle avoidance; on the other hand, the inverse solution of Cartesian kinematics and motion trajectory calculation are used to improve the accuracy of coordinate analysis and avoid problems such as confusion of motion trajectory.

[0005] To achieve the above object, the present invention is implemented by adopting the following technical solutions:

[0006] The present invention discloses an agricultural robot control method based on ROS and infrared obstacle avoidance, comprising the following steps:

[0007] Acquire the environmental image, infrared signal and initial state data of the end effector of the agricultural robot;

[0008] According to the environmental image and the infrared signal, feature extraction and target recognition are performed to obtain target data with obstacle information;

[0009] According to the target data and initial state data with obstacle information, joint control instructions and drive control instructions are generated respectively through Cartesian kinematics inverse solution and motion trajectory measurement to control the work of the agricultural robot;

[0010] Among them, the joint control instructions are transmitted to the joint servos through ROS to realize the angle control of each joint of the agricultural robot mechanical arm; the drive control instructions are transmitted to the drive servos through ROS to realize the control of the end effector of the agricultural robot mechanical arm.

[0011] Further, obtaining the target data with obstacle information includes the following steps:

[0012] According to the environmental image, image transformation, image enhancement and image filtering are performed to obtain a processed environmental image;

[0013] Extracting edge information by detecting grayscale values ​​according to the processed environment image to obtain edge data;

[0014] According to the edge data, the processed environment image is segmented to obtain a plurality of mutually non-intersecting area images;

[0015] For each area image, perform object recognition and feature extraction to obtain object features; calculate the matching degree between each object feature and the preset target reference feature;

[0016] When the matching degree of the object feature is greater than a preset matching degree threshold, the corresponding object is a target object; otherwise, it is an obstacle object;

[0017] Based on the monocular ranging method, the relative positions of the target object and the obstacle to the agricultural robot are calculated respectively, and then the pose estimation algorithm is used to obtain the pose information of the target object and the obstacle respectively, and the target data with obstacle information is fused.

[0018] Furthermore, the image transformation includes image translation, grayscale transformation, rotation and image mirroring operations; the image enhancement includes grayscale correction, color enhancement, smoothing and sharpening operations; and the image filtering is to perform pixel-by-pixel weighted summation processing through a filter to achieve image denoising.

[0019] Furthermore, the expression of the matching degree is as follows:

[0020] M(q)=match(F(q), F');

[0021] Wherein, M(q) represents the matching degree between the object feature of the qth object and the preset target reference feature; match represents the matching function; F(q) represents the object feature of the qth object; and F' represents the preset target reference feature.

[0022] Further, the agricultural robot comprises a base, a first mechanical arm, a second mechanical arm, a third mechanical arm and a fourth mechanical arm, the base is connected to the first mechanical arm, the first mechanical arm and the second mechanical arm are connected via a first joint, the second mechanical arm and the third mechanical arm are connected via a second joint, the third mechanical arm and the fourth mechanical arm are connected via a third joint, the fourth mechanical arm is provided with a fourth joint at an end away from the third mechanical arm, and an end effector is provided on the fourth joint;

[0023] The inverse solution of Cartesian kinematics and motion trajectory calculation include the following steps:

[0024] According to the initial state data of the end effector, initial coordinates and posture data of the end effector in a preset world coordinate system are obtained;

[0025] Obtaining initial coordinates and position data of the obstacle object and the target object in a preset world coordinate system according to the target data with obstacle information;

[0026] According to the initial coordinates and posture data of the end effector, the obstacle object and the target object in the preset world coordinate system, under the constraint of not touching the obstacle object, the coordinates and posture data of the end effector at each preset time point are calculated based on Cartesian space trajectory planning and linear interpolation;

[0027] The inverse kinematics solution is performed for the coordinates and posture data of the end effector at any preset time point to obtain the angle data of each joint of the agricultural robot, and then generate joint control instructions and drive control instructions.

[0028] Furthermore, the posture data of the end effector in a preset world coordinate system includes a homogeneous transformation matrix of the first joint relative to the origin of the preset world coordinate system, a homogeneous transformation matrix of the second joint relative to the first joint, a homogeneous transformation matrix of the third joint relative to the second joint, and a homogeneous transformation matrix of the fourth joint relative to the third joint;

[0029] The expression of the posture data is as follows:

[0030]

[0031] In the formula, Represents the position data of the end effector in the preset world coordinate system; T1 represents the homogeneous transformation matrix of the first joint relative to the origin of the preset world coordinate system; T2 represents the homogeneous transformation matrix of the second joint relative to the first joint; T3 represents the homogeneous transformation matrix of the third joint relative to the second joint; T4 represents the homogeneous transformation matrix of the fourth joint relative to the third joint.

[0032] Furthermore, the homogeneous transformation matrix T1 of the first joint relative to the origin of the preset world coordinate system is expressed as follows:

[0033]

[0034] In the formula, represents the rotation angle of the first joint relative to the origin of the world coordinate system; d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis;

[0035] The expression of the homogeneous transformation matrix T2 of the second joint relative to the first joint is as follows:

[0036]

[0037] In the formula, represents the rotation angle of the second joint relative to the first joint; l2 represents the length of the connecting rod between the first joint and the second joint along the x-axis.

[0038] The expression of the homogeneous transformation matrix T3 of the third joint relative to the second joint is as follows:

[0039]

[0040] In the formula, represents the rotation angle of the third joint relative to the second joint; l3 represents the length of the connecting rod between the second joint and the third joint along the x-axis;

[0041] The expression of the homogeneous transformation matrix T4 of the fourth joint relative to the third joint is as follows:

[0042]

[0043] In the formula, It represents the rotation angle of the fourth joint relative to the third joint; d4 represents the connecting rod offset of the fourth joint relative to the third joint along the z-axis.

[0044] Furthermore, the expressions of the angle data of each joint of the agricultural robot are as follows:

[0045]

[0046] In the formula, Represents the rotation angle of the first joint relative to the origin of the world coordinate system; represents the rotation angle of the second joint relative to the first joint; represents the rotation angle of the third joint relative to the second joint; represents the rotation angle of the fourth joint relative to the third joint;

[0047] p xrepresents the coordinate of the end effector along the x-axis; p y represents the coordinate of the end effector along the y-axis; p z represents the coordinate of the end effector along the z-axis;

[0048] d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis; l2 represents the connecting rod length between the first joint and the second joint along the x-axis; l3 represents the connecting rod length between the second joint and the third joint along the x-axis;

[0049] Furthermore, the expression of the coordinates of the end effector at each preset time point is as follows:

[0050]

[0051]

[0052]

[0053] In the formula, x n represents the coordinate of the end effector along the x-axis at the nth preset time point; y n represents the coordinate of the end effector along the y-axis at the nth preset time point; z n represents the coordinate of the end effector along the z-axis at the nth preset time point; N represents the total number of preset time points;

[0054] x0 represents the initial coordinate of the end effector along the x-axis; y0 represents the initial coordinate of the end effector along the y-axis; z0 represents the initial coordinate of the end effector along the z-axis;

[0055] x f represents the initial coordinate of the target object along the x-axis; y f represents the initial coordinate of the target object along the y-axis; z f Indicates the initial coordinate of the target object along the z-axis.

[0056] Furthermore, the joint servo is connected in parallel with the driving servo and is located in the same power supply channel. The rotation direction of the joint servo is opposite to that of the driving servo, so that when the joint servo starts to rotate forward, the positive steering amount is transmitted to the driving servo, thereby prompting the driving servo to rotate forward.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] The agricultural robot control method based on ROS and infrared obstacle avoidance of the present invention, firstly, can further adjust the image parameters based on the mutual cooperation of the environmental image and the infrared signal, improve the accuracy and reliability of obstacle avoidance, and can also quickly respond to obstacles in complex environments to ensure the safety of the system. Secondly, the inverse solution of Cartesian kinematics and motion trajectory calculation are adopted to improve the accuracy of coordinate analysis and avoid problems such as confusion of motion trajectory. Finally, the joint control instructions and drive control instructions are transmitted through ROS, which ensures the stability of signal transmission and improves the operation efficiency of the agricultural robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 is a flow chart of the agricultural robot control method based on ROS and infrared obstacle avoidance provided in Example 1;

[0060] Figure 2 is a structural diagram of the agricultural robot provided in Example 1;

[0061] Figure 3 is a schematic diagram of each mechanical arm and each joint provided in Example 1;

[0062] Figure 4 is a schematic diagram of the ROS architecture provided in Example 1;

[0063] Figure 5 This is a schematic diagram of Cartesian space straight line trajectory planning provided in Example 1;

[0064] In the figure: 1. Stepper motor; 2. Main control chip; 3. DC-DC buck module; 4. Motor drive board; 5. Motor driver; 6. Motor bracket; 7. Power supply; 8. Joint servo; 9. Drive servo; 10. Infrared transmitter; 11. Infrared receiver; 12. Global camera; 13. End effector; 14. Image processor;

[0065] 15. Base; 16. First mechanical arm; 17. Second mechanical arm; 18. Third mechanical arm; 19. Fourth mechanical arm;

[0066] 20. First joint; 21. Second joint; 22. Third joint; 23. Fourth joint. DETAILED DESCRIPTION

[0067] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

[0068] This embodiment discloses an agricultural robot control method based on ROS and infrared obstacle avoidance, comprising the following steps:

[0069] Acquire the environmental image, infrared signal and initial state data of the end effector of the agricultural robot;

[0070] Based on the environmental image and infrared signal, feature extraction and target recognition are performed to obtain target data with obstacle information;

[0071] According to the target data and initial state data with obstacle information, joint control instructions and drive control instructions are generated respectively through Cartesian kinematics inverse solution and motion trajectory measurement to control the work of the agricultural robot;

[0072] Among them, the joint control instructions are transmitted to the joint servo 8 through ROS to realize the angle control of each joint of the agricultural robot arm; the drive control instructions are transmitted to the drive servo 9 through ROS to realize the control of the end effector of the agricultural robot arm.

[0073] The technical concept of the present invention is as follows: First, based on the mutual cooperation of environmental images and infrared signals, the image parameters can be further adjusted to improve the accuracy and reliability of obstacle avoidance, and obstacles can be quickly responded to in complex environments to ensure the safety of the system. Secondly, the inverse solution of Cartesian kinematics and motion trajectory calculation are used to improve the accuracy of coordinate analysis and avoid problems such as motion trajectory confusion. Finally, the joint control instructions and drive control instructions are transmitted through ROS to ensure the stability of signal transmission and improve the operating efficiency of the agricultural robot.

[0074] like Figure 2 and Figure 3 As shown, the agricultural robot includes a stepper motor 1, a main control chip 2, a DC-DC step-down module 3, a motor drive board 4, a motor driver 5, a motor bracket 6, a power supply 7, a joint servo 8, a drive servo 9, an infrared transmitting tube 10, an infrared receiving tube 11, a global camera 12, an end effector 13, an image processor 14, a base 15, a first robotic arm 16, a second robotic arm 17, a third robotic arm 18 and a fourth robotic arm 19.

[0075] The base 15 is connected to the first robotic arm 16, the first robotic arm 16 and the second robotic arm 17 are connected via a first joint 20, the second robotic arm 17 and the third robotic arm 18 are connected via a second joint 21, the third robotic arm 18 and the fourth robotic arm 19 are connected via a third joint 22, and the fourth robotic arm 19 is provided with a fourth joint 23 at the end away from the third robotic arm 18, and the fourth joint 23 is provided with an end effector 13.

[0076] The base 15 is provided with a stepper motor 1, a main control chip 2, a DC-DC step-down module 3, a motor drive board 4, a motor driver 5, a motor bracket 6, a power supply 7, an infrared transmitting tube 10, and an infrared receiving tube 11. The model of the main control chip 2 in this embodiment is STM32F103RCT6; the model of the motor drive board 4 is A498.

[0077] The infrared transmitting tube 10 and the infrared receiving tube 11 are the transmission channels for real-time communication between the upper computer and the lower computer. Infrared communication is a one-line communication, that is, only one line is needed for communication. Infrared information is composed of different infrared high and low level pulses. When the infrared receiving head captures the infrared signal, the communication line will show different high and low levels. An IO pin of the chip is connected to this communication line. The high and low level information on this line is read out by query. The chip converts the received high and low signals into numerical values ​​according to a unified communication protocol. Based on the infrared transmitting tube 10 and the infrared receiving tube 11, the infrared signal around the agricultural robot is obtained and transmitted to the main control chip 2.

[0078] The infrared transmitting tube 10 continuously transmits infrared rays of a certain frequency to the front. When the infrared rays encounter an obstacle in front, the rays return and are received by the infrared receiving tube 11. If there is no obstacle in front, the rays are not reflected. The infrared transmitting tube 10 and the infrared receiving tube 11 are carefully arranged on the left side of the joint servo 8. The object distance is converted by measuring the time difference between the emission and reception of the characteristic frequency infrared rays. This technology not only improves the accuracy and reliability of obstacle avoidance, but also can quickly respond to obstacles in complex environments to ensure the safety of the system.

[0079] The control of the joint servo 8 and the drive servo 9 is based on the main control chip 2, and the servo is controlled by outputting PWM pulse width modulation to the signal line of the servo. The period and duty cycle of PWM are controllable, and the duty cycle determines the position of the output shaft. When a pulse width of 1.5ms is sent to the servo, the output shaft of the servo will move to the middle position of 90 degrees, when the pulse width is 1ms, the output shaft will move to the minimum position of 0 degrees, and when the pulse width is 2ms, the output shaft will move to the maximum position of 180 degrees.

[0080] The joint servo 8 and the driving servo 9 are connected in parallel and located in the same power supply channel. The rotation direction of the joint servo 8 and the driving servo 9 is opposite, so that when the joint servo 8 starts to rotate forward, the positive steering amount is transmitted to the driving servo 9 to cause the driving servo 9 to rotate forward. The rotation direction is opposite during installation, and the speed and angle of forward and reverse rotation can be set before starting. This configuration allows the system to control the movement of the servo more flexibly. By utilizing the speed difference between the two, more complex motion trajectories and more precise positioning can be achieved.

[0081] The power supply 7 and the stepper motor 1 are used as drivers. The power supply 7 provides electrical energy, power and stable voltage for the entire module. The stepper motor 1 and other components convert mechanical energy into electrical energy and generate driving torque, which serves as the overall power source.

[0082] The end effector 13 is an electric end effector equipped with a force / torque sensor to facilitate the servo to perform force-sensitive tasks. It can accurately detect and measure the contact device of the servo, thereby improving the control of the servo and simplifying software programming.

[0083] The image processor 14 is related to the quality and speed of image output, and even the operating environment of the entire system. It can be said to be the system core of color image imaging. Its main function is to convert various image information obtained by the host computer, that is, the image captured by the global camera 12, into dot matrix and coordinate information that can be recorded by the main control board, and then control the main control board to record the image dot matrix information and send it to the joint servo 8 and the drive servo 9 to form a motion trajectory. During the processing, the image processor 14 converts the image information into data that can be directly used for imaging according to the properties, parameters, status and data format of the imaging device. Based on the global camera 12 and the image processor 14, the environmental image of the periphery of the agricultural robot is obtained and transmitted to the main control chip 2.

[0084] The DC-DC buck module 3 is part of the power supply 7. It integrates an asynchronous rectifier buck regulator with a MOSFET field effect tube. The input voltage range is 4.75V-40V, the output current is up to 1A, the operating frequency is 550KH, and the maximum efficiency can reach 90%. The buck module adopts PWM control mode, which has good transient response and cycle-by-cycle current limiting function. Under light load conditions, the system automatically switches to PF image tracking and left and right following mode to ensure high light load efficiency. It has built-in soft start and loop compensation, as well as a fixed operating frequency of 550K, which ensures performance while significantly reducing the energy consumption required for practical applications.

[0085] It should be noted that in this embodiment, the global camera 12 and the image processor 14, the image processor 14 and the main control chip 2, and the main control chip 2 and the infrared receiving tube 11, the joint servo 8 and the drive servo 9 are connected through the agricultural robot operating system ROS (Robot Operating System), so that different parts of the system can discover each other and send and receive data between them.

[0086] like Figure 4As shown, this embodiment uses Matlab for visual processing. MATLAB supports the agricultural robot operating system ROS through a function library and can exchange data with the physical interface of the agricultural robot operating system ROS. The ROS toolbox provides an interface to connect MATLAB and Simulink to ROS to create a ROS node network. The toolbox includes MATLAB functions and Simulink models for importing, analyzing, and replaying recorded ROS data. At the same time, the ROS toolbox supports C++ code generation, which enables ROS nodes to be automatically generated from Simulink models and deployed to simulation or physical hardware.

[0087] The present embodiment uses Keil uVision5 to write the main control board program. Keil uVision5 is a single-chip C language software development system, which provides a complete development solution including C compiler, macro assembly, linker, library management and simulation debugger, etc., and combines these parts together through an integrated development environment. It supports ARM microcontrollers and microprocessors with multiple cores, writes code in Keil5, compiles and debugs the software, and generates an executable target file, which is downloaded to the specific chip of 51 or 32 series, and can be used to simulate the functions of various peripherals. Using Keil5 software simulation and hardware debugging can clearly plan and control the motion trajectory of the agricultural robot, continuously perform local detection and calibration and management, and manipulate the operation of the agricultural robot. For easy operation, this module mainly uses GPIO and AD to collect and debug.

[0088] like Figure 1 As shown, the specific steps of the agricultural robot control method are as follows:

[0089] Step 1: Obtain the environmental image, infrared signal and initial state data of the end effector around the agricultural robot.

[0090] Step 2: Based on the environmental image and infrared signal, feature extraction and target recognition are performed to obtain target data with obstacle information.

[0091] The specific steps include:

[0092] 2.1. Perform image transformation, image enhancement and image filtering according to the environment image to obtain the processed environment image.

[0093] Image transformation includes image translation, grayscale transformation, rotation, and image mirroring operations; image enhancement includes grayscale correction, color enhancement, smoothing, and sharpening operations; image filtering is to perform pixel-by-pixel weighted summation processing through filters to achieve image denoising.

[0094] Specifically, image transformation is to convert the image originally defined in the image space to another space in some form, and use the characteristics of the space to perform certain processing to obtain the desired effect. It is often divided into geometric transformation and two-dimensional orthogonal transformation. The present invention selects common transformation methods such as image translation, grayscale transformation, rotation, and image mirroring.

[0095] Image enhancement mainly emphasizes the important parts of the image or masks some unnecessary features in the image to make the image match the visual response characteristics. The main functions of this module are grayscale correction, color enhancement, smoothing and sharpening.

[0096] The purpose of image filtering is to remove noise from an image or perform specific image enhancement. Filtering can be seen as using a filter template, or filter, or convolution kernel, to scan each pixel in an image, and obtain the pixel value of the corresponding position in the output image through some operation between the template and the image pixel, such as weighted summation.

[0097] 2.2. Based on the processed environmental image, edge information is extracted by detecting the grayscale value to obtain edge data.

[0098] Edge detection is an important step in image processing, which is used to extract edge information in an image. An edge is a place in an image where the grayscale value changes dramatically, usually corresponding to the outline of an object or the boundary between different regions.

[0099] 2.3. According to the edge data, the processed environment image is segmented to obtain multiple non-intersecting area images.

[0100] Image segmentation is the process of dividing an image into several non-overlapping regional images, each of which satisfies certain specific properties or conditions and is the basis for subsequent image analysis and understanding.

[0101] 2.4. For each regional image, perform object recognition and feature extraction to obtain object features; calculate the matching degree between each object feature and the preset target reference feature.

[0102] The expression of matching degree is as follows:

[0103] M(q)=match(F(q), F');

[0104] Wherein, M(q) represents the matching degree between the object feature of the qth object and the preset target reference feature; match represents the matching function; F(q) represents the object feature of the qth object; and F' represents the preset target reference feature.

[0105] 2.5. When the matching degree of the response object feature is greater than the preset matching degree threshold, the corresponding object is the target object; otherwise, it is an obstacle object.

[0106] The preset matching degree threshold in this embodiment is determined according to actual conditions.

[0107] 2.6. Based on the monocular ranging method, according to the geometric information in the image, the relative positions of the target object and the obstacle to the agricultural robot are calculated respectively, and then the pose estimation algorithm is used to obtain the pose information of the target object and the obstacle respectively, and the target data with obstacle information is obtained by fusion.

[0108] Step 3: Based on the target data and initial state data with obstacle information, joint control instructions and drive control instructions are generated respectively through Cartesian kinematics inverse solution and motion trajectory measurement to control the work of the agricultural robot.

[0109] In this embodiment, the method of solving the angle of motion of each joint when the end effector position is known is called solving the inverse kinematic problem of agricultural robots. Inverse kinematics solution is the basis for realizing multi-degree-of-freedom steering gear control, and it is also a necessary condition for subsequent trajectory planning control. In inverse kinematics solution, the end effector position is a known condition, and the matrix calculation method is used to solve the joint angle.

[0110] The specific steps include:

[0111] 3.1. According to the initial state data of the end effector, the initial coordinates and posture data of the end effector in the preset world coordinate system are obtained.

[0112] The pose data of the end effector in a preset world coordinate system includes a homogeneous transformation matrix of the first joint relative to the origin of the preset world coordinate system, a homogeneous transformation matrix of the second joint relative to the first joint, a homogeneous transformation matrix of the third joint relative to the second joint, and a homogeneous transformation matrix of the fourth joint relative to the third joint;

[0113] The expression of pose data is as follows:

[0114]

[0115] In the formula, Represents the position data of the end effector in the preset world coordinate system; T1 represents the homogeneous transformation matrix of the first joint relative to the origin of the preset world coordinate system; T2 represents the homogeneous transformation matrix of the second joint relative to the first joint; T3 represents the homogeneous transformation matrix of the third joint relative to the second joint; T4 represents the homogeneous transformation matrix of the fourth joint relative to the third joint.

[0116] The expression of the homogeneous transformation matrix T1 of the first joint relative to the origin of the preset world coordinate system is as follows:

[0117]

[0118] In the formula, represents the rotation angle of the first joint relative to the origin of the world coordinate system; d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis;

[0119] The expression of the homogeneous transformation matrix T2 of the second joint relative to the first joint is as follows:

[0120]

[0121] In the formula, represents the rotation angle of the second joint relative to the first joint; l2 represents the length of the connecting rod between the first joint and the second joint along the x-axis;

[0122] The expression of the homogeneous transformation matrix T3 of the third joint relative to the second joint is as follows:

[0123]

[0124] In the formula, represents the rotation angle of the third joint relative to the second joint; l3 represents the length of the connecting rod between the second joint and the third joint along the x-axis;

[0125] The expression of the homogeneous transformation matrix T4 of the fourth joint relative to the third joint is as follows:

[0126]

[0127] In the formula, It represents the rotation angle of the fourth joint relative to the third joint; d4 represents the connecting rod offset of the fourth joint relative to the third joint along the z-axis.

[0128] 3.2. Based on the target data with obstacle information, the initial coordinates and posture data of the obstacle object and the target object in the preset world coordinate system are obtained.

[0129] 3.3. According to the initial coordinates and posture data of the end effector, obstacle and target object in the preset world coordinate system, under the constraint of not touching the obstacle, the coordinates and posture data of the end effector at each preset time point are calculated based on Cartesian space trajectory planning and linear interpolation.

[0130] The schematic diagram of Cartesian space straight line trajectory planning is as follows Figure 5 As shown, the expression of the coordinates of the end effector at each preset time point is as follows:

[0131]

[0132]

[0133]

[0134] In the formula, x n represents the coordinate of the end effector along the x-axis at the nth preset time point; y n represents the coordinate of the end effector along the y-axis at the nth preset time point; z n represents the coordinate of the end effector along the z-axis at the nth preset time point; N represents the total number of preset time points;

[0135] x0 represents the initial coordinate of the end effector along the x-axis; y0 represents the initial coordinate of the end effector along the y-axis; z0 represents the initial coordinate of the end effector along the z-axis;

[0136] x f represents the initial coordinate of the target object along the x-axis; y f represents the initial coordinate of the target object along the y-axis; z f Indicates the initial coordinate of the target object along the z-axis.

[0137] 3.4. For the coordinates and posture data of the end effector at any preset time point, inverse kinematics is solved to obtain the angle data of each joint of the agricultural robot, and then generate joint control instructions and drive control instructions.

[0138] The expressions of angle data of each joint of the agricultural robot are as follows:

[0139]

[0140] In the formula, Represents the rotation angle of the first joint relative to the origin of the world coordinate system; represents the rotation angle of the second joint relative to the first joint; represents the rotation angle of the third joint relative to the second joint; represents the rotation angle of the fourth joint relative to the third joint;

[0141] p x represents the coordinate of the end effector along the x-axis; p y represents the coordinate of the end effector along the y-axis; p z represents the coordinate of the end effector along the z-axis;

[0142] d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis; l2 represents the connecting rod length between the first joint and the second joint along the x-axis; l3 represents the connecting rod length between the second joint and the third joint along the x-axis;

[0143] Step 4: Control the agricultural robot to work according to the joint control instructions and drive control instructions.

[0144] If during the movement, the agricultural robot moves with deviation or the target object moves so that there is a deviation between the position of the end effector of the agricultural robot after movement and the position of the target object, then return to step one and generate a new control instruction by adjusting the parameters in the control instruction queue, such as speed, acceleration or joint angle.

[0145] These new control instructions will be transmitted to the joint servo 8 and the driving servo 9 through ROS again, driving the joint servo 8 and the driving servo 9 to perform fine adjustments to reduce or eliminate errors.

[0146] This process will be repeated repeatedly until the end effector reaches the target position accurately, meeting the accuracy requirements of visual servo control. At the same time, the main control board will continue to monitor the potential feedback of the joint servo 8 to ensure that the posture of each joint is stable and in line with expectations, thereby achieving precise control and stable operation of the entire motion control system based on visual servo feedback.

[0147] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0148] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0149] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0150] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0151] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. The agricultural robot control method based on ROS and infrared obstacle avoidance is characterized by: The steps include: Acquire the environmental image, infrared signal and initial state data of the end effector of the agricultural robot; According to the environmental image and the infrared signal, feature extraction and target recognition are performed to obtain target data with obstacle information; According to the target data and initial state data with obstacle information, joint control instructions and drive control instructions are generated respectively through Cartesian kinematics inverse solution and motion trajectory measurement to control the work of the agricultural robot; Among them, the joint control instructions are transmitted to the joint servos through ROS to realize the angle control of each joint of the agricultural robot mechanical arm; the drive control instructions are transmitted to the drive servos through ROS to realize the control of the end effector of the agricultural robot mechanical arm.

2. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 1 is characterized in that: The step of obtaining target data with obstacle information comprises the following steps: According to the environmental image, image transformation, image enhancement and image filtering are performed to obtain a processed environmental image; Extracting edge information by detecting grayscale values ​​according to the processed environment image to obtain edge data; According to the edge data, the processed environment image is segmented to obtain a plurality of mutually non-intersecting area images; For each regional image, object recognition and feature extraction are performed to obtain object features; Calculate the matching degree between each object feature and the preset target reference feature; When the matching degree of the object feature is greater than a preset matching degree threshold, the corresponding object is the target object; Otherwise it is an obstacle; Based on the monocular ranging method, the relative positions of the target object and the obstacle to the agricultural robot are calculated respectively, and then the pose estimation algorithm is used to obtain the pose information of the target object and the obstacle respectively, and the target data with obstacle information is fused.

3. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 2 is characterized in that: The image transformation includes image translation, grayscale transformation, rotation and image mirroring operations; the image enhancement includes grayscale correction, color enhancement, smoothing and sharpening operations; the image filtering is to perform pixel-by-pixel weighted summation processing through a filter to achieve image denoising.

4. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 2 is characterized in that: The expression of the matching degree is as follows: M(q)=match(F(q), F'); Wherein, M(q) represents the matching degree between the object feature of the qth object and the preset target reference feature; match represents the matching function; F(q) represents the object feature of the qth object; and F' represents the preset target reference feature.

5. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 1 is characterized in that: The agricultural robot comprises a base, a first mechanical arm, a second mechanical arm, a third mechanical arm and a fourth mechanical arm, wherein the base is connected to the first mechanical arm, the first mechanical arm is connected to the second mechanical arm via a first joint, the second mechanical arm is connected to the third mechanical arm via a second joint, the third mechanical arm is connected to the fourth mechanical arm via a third joint, the fourth mechanical arm is provided with a fourth joint at an end away from the third mechanical arm, and an end effector is provided on the fourth joint; The inverse solution of Cartesian kinematics and motion trajectory calculation include the following steps: According to the initial state data of the end effector, initial coordinates and posture data of the end effector in a preset world coordinate system are obtained; Obtaining initial coordinates and position data of the obstacle object and the target object in a preset world coordinate system according to the target data with obstacle information; According to the initial coordinates and posture data of the end effector, the obstacle object and the target object in the preset world coordinate system, under the constraint of not touching the obstacle object, the coordinates and posture data of the end effector at each preset time point are calculated based on Cartesian space trajectory planning and linear interpolation; The inverse kinematics solution is performed for the coordinates and posture data of the end effector at any preset time point to obtain the angle data of each joint of the agricultural robot, and then generate joint control instructions and drive control instructions.

6. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 5 is characterized in that: The pose data of the end effector in a preset world coordinate system includes a homogeneous transformation matrix of the first joint relative to the preset world coordinate system origin, a homogeneous transformation matrix of the second joint relative to the first joint, a homogeneous transformation matrix of the third joint relative to the second joint, and a homogeneous transformation matrix of the fourth joint relative to the third joint; The expression of the posture data is as follows: ； In the formula, Represents the position data of the end effector in the preset world coordinate system; T1 represents the homogeneous transformation matrix of the first joint relative to the origin of the preset world coordinate system; T2 represents the homogeneous transformation matrix of the second joint relative to the first joint; T3 represents the homogeneous transformation matrix of the third joint relative to the second joint; T4 represents the homogeneous transformation matrix of the fourth joint relative to the third joint.

7. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 6 is characterized in that: The homogeneous transformation matrix T1 of the first joint relative to the origin of the preset world coordinate system is expressed as follows: ； In the formula, represents the rotation angle of the first joint relative to the origin of the world coordinate system; d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis; The expression of the homogeneous transformation matrix T2 of the second joint relative to the first joint is as follows: ； In the formula, represents the rotation angle of the second joint relative to the first joint; l2 represents the length of the connecting rod between the first joint and the second joint along the x-axis; The expression of the homogeneous transformation matrix T3 of the third joint relative to the second joint is as follows: ； In the formula, represents the rotation angle of the third joint relative to the second joint; l3 represents the length of the connecting rod between the second joint and the third joint along the x-axis; The expression of the homogeneous transformation matrix T4 of the fourth joint relative to the third joint is as follows: ； In the formula, It represents the rotation angle of the fourth joint relative to the third joint; d4 represents the connecting rod offset of the fourth joint relative to the third joint along the z-axis.

8. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 5 is characterized in that: The expression of the angle data of each joint of the agricultural robot is as follows: ； In the formula, Represents the rotation angle of the first joint relative to the origin of the world coordinate system; represents the rotation angle of the second joint relative to the first joint; represents the rotation angle of the third joint relative to the second joint; represents the rotation angle of the fourth joint relative to the third joint; p x represents the coordinate of the end effector along the x-axis; p y represents the coordinate of the end effector along the y-axis; p z represents the coordinate of the end effector along the z-axis; d1 represents the connecting rod offset of the first joint relative to the origin of the world coordinate system along the z-axis; l2 represents the connecting rod length between the first joint and the second joint along the x-axis; l3 represents the connecting rod length between the second joint and the third joint along the x-axis.

9. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 5 is characterized in that: The expression of the coordinates of the end effector at each preset time point is as follows: ； ； ； In the formula, x n represents the coordinate of the end effector along the x-axis at the nth preset time point; y n represents the coordinate of the end effector along the y-axis at the nth preset time point; z n represents the coordinate of the end effector along the z-axis at the nth preset time point; N represents the total number of preset time points; x0 represents the initial coordinate of the end effector along the x-axis; y0 represents the initial coordinate of the end effector along the y-axis; z0 represents the initial coordinate of the end effector along the z-axis; x f represents the initial coordinate of the target object along the x-axis; y f represents the initial coordinate of the target object along the y-axis; z f Indicates the initial coordinate of the target object along the z-axis.

10. The agricultural robot control method based on ROS and infrared obstacle avoidance according to claim 1 is characterized in that: The joint servo is connected in parallel with the driving servo and is located in the same power supply channel. The rotation direction of the joint servo is opposite to that of the driving servo, so that when the joint servo starts to rotate forward, the positive steering amount is transmitted to the driving servo, thereby causing the driving servo to rotate forward.

Images