Following robot for mushroom shelf planting and automatic following method

Abstract

A following robot for mushroom shelf planting and an automatic following method belong to the field of agricultural mechanization, and the robot comprises a walking following system, a lifting mechanism and a ceiling clamping mechanism. The walking following system accurately positions and automatically follows a worker through the UWB technology, the lifting mechanism adopts a parallelogram mechanism to achieve stable lifting, the ceiling clamping mechanism completes grabbing, lifting and carrying of a turnover bin through cooperation of an electric push rod, the turnover bin stacking device is suitable for turnover bins of different sizes, and stable stacking and accurate butt joint are achieved. The robot supports a multi-label mode and can serve multiple workers at the same time, the cooperation efficiency is improved, and the equipment cost is reduced. According to the automatic following method, the distance and angle are calculated based on UWB, precise steering is achieved in combination with differential control, the robot adapts to the narrow and multi-obstacle environment, and the overall efficiency and safety of operation are improved. The mushroom planting and harvesting efficiency is improved, and the safety risk is reduced.

Description

Technical Field

[0001] This invention belongs to the field of agricultural mechanization and relates to a robot and control method for mushroom tiered planting and harvesting. Specifically, it relates to a robot for mushroom tiered planting that integrates following, lifting, and pushing functions, as well as an automatic following method. Background Technology

[0002] With the continuous growth in demand for mushrooms, traditional ground cultivation methods, due to the presence of many miscellaneous microorganisms in the soil and the limitation of only one crop per year, are gradually being replaced by the more efficient tiered cultivation technology. Tiered cultivation, by arranging the mushroom logs in layers, allows for year-round fruiting with high yields and good quality. However, tiered cultivation has the following drawbacks:

[0003] (1) During planting and harvesting, it is necessary to frequently move the trays and mushroom cultivation boxes, which disrupts the continuity of work and reduces efficiency.

[0004] (2) The height of the shelves usually exceeds two meters, and workers need to go up and down the ladder frequently, which poses a significant safety hazard;

[0005] (3) The lack of close connection between the planting, harvesting and transportation stages further reduced the overall efficiency.

[0006] Therefore, it is necessary to design an automated handling device with automatic following and lifting functions to effectively solve the above problems. Summary of the Invention

[0007] The purpose of this invention is to address the shortcomings of the existing technology by proposing a following robot and automatic following method for mushroom tiered planting, which has automatic following, lifting, and pushing functions to improve the efficiency of mushroom tiered planting and harvesting and reduce safety risks.

[0008] The following technical solution is provided in this application for a follower robot for mushroom shelf cultivation:

[0009] A follower robot for mushroom tiered cultivation includes tiered shelves installed on the ground; and a tag carried by the operator, characterized in that the follower robot comprises:

[0010] A walking and following system is installed at the bottom of the following robot, which enables the following robot to walk and automatically follow the staff.

[0011] A lifting mechanism is installed above the walking and following system to realize the lifting and lowering movements of the following robot;

[0012] The clamping and suction mechanism is connected to the walking and following system to realize the clamping and suction action of the turnover warehouse.

[0013] By adopting the above technical solution, the robot works in concert with the following system, lifting mechanism, and clamping ceiling mechanism to achieve automatic following of the operator, while simultaneously performing actions such as clamping, lifting, transporting, and stacking on the turnover warehouse. This solves the problems of low efficiency, high risk of high-altitude operations, and low automation in the mushroom shelf cultivation process, achieving flexible transport where the robot follows the operator wherever needed; the lifting and clamping mechanisms are adaptable to shelves of different heights; and it reduces human intervention, improving operational efficiency and safety.

[0014] Furthermore, the walking and following system consists of a base, omnidirectional wheels, drive wheels, and drive motors located at the bottom of the base; and a base station, motor controller, and power supply located at the top of the base; each drive wheel is independently driven by a drive motor, the drive motor is driven and controlled by the motor controller, and the power supply is connected to the motor controller.

[0015] By adopting the above technical solution, a four-point wheel system layout is used. The omnidirectional wheels provide support for the walking and following system and rotate freely in the direction of vehicle movement. The two drive wheels are controlled in coordination by the motor controller and the drive motor. The power supply provides power to the motor controller, which is driven by the motion commands issued by the base station. The structure is compact, anti-interference, and easy to maintain.

[0016] Furthermore, the lifting mechanism is connected and installed above the walking and following system via a mounting plate; the lifting mechanism is composed of a top frame, a bottom frame, a first connecting rod, a second connecting rod, a slide groove, a roller, a middle connecting rod, and an electric push rod; the top frame and the bottom frame are arranged in parallel, and slide grooves are provided on their inner sides; the first connecting rod and the second connecting rod are arranged crosswise, one end of the first connecting rod is hinged to the top frame, and the other end is slidably connected to the bottom frame via a roller; one end of the second connecting rod is hinged to the bottom frame, and the other end is slidably connected to the top frame via a roller; the middle connecting rod is fixedly arranged between the two second connecting rods; one end of the electric push rod is hinged to the bottom frame, and the other end is hinged to the middle connecting rod.

[0017] By adopting the above technical solution, a symmetrical structure, guided rollers, and electric push rod drive achieve high stability, low noise, and smooth lifting. The electric push rod is hinged to the intermediate connecting rod, improving thrust efficiency and structural rigidity. The electric push rod extends and retracts, pushing the intermediate connecting rod to extend or retract the first and second connecting rods. The rollers slide in the groove, limiting freedom of movement and preventing swaying. The top frame enables platform lifting. This solution solves the problems of easy swaying and high noise in traditional lifting mechanisms (such as chains and lead screws), and has advantages such as large stroke, high stability lifting, strong structural self-locking, no slippage during power outages, adaptability to different shelf heights, and stepless adjustment.

[0018] Furthermore, the ceiling clamping mechanism is composed of a mounting base, a lifting electric push rod, a telescopic electric push rod, a positioning clamping plate, and a ceiling-mounted electric push rod connected together; the bottom of the lifting electric push rod is fixedly connected to the mounting plate, the mounting base is connected to the top of the lifting electric push rod, the telescopic electric push rod is connected to both sides of the mounting base, the positioning clamping plate is located on the inner side of the telescopic electric push rod to adjust the distance between the two positioning clamping plates, and the ceiling-mounted electric push rod is located in the middle of the mounting base.

[0019] By adopting the above technical solution, three electric push rods work together to complete the lifting, clamping, and ceiling-mounting actions, achieving integrated gripping, lifting, and transport. The spacing of the positioning clamping plates is adjustable to adapt to different sizes of turnover bins, providing flexibility and compatibility. The lifting electric push rod adjusts the overall height; the telescopic electric push rod drives the clamping plates to clamp the side wall of the turnover bin; the ceiling-mounting electric push rod extends to allow the turnover bin to smoothly enter the shelf. When it is necessary to remove the turnover bin from the shelf, the ceiling-mounting electric push rod is energized and forms an electromagnetic attraction with the magnet on the turnover bin, achieving precise gripping and stacking of the turnover bin, thus improving efficiency.

[0020] Furthermore, the turnover bin is composed of a bin body, a magnet, and a flange; the flange is located at the lower edge of the bin body and is used for limiting support when multiple turnover bins are stacked on top of each other; the magnet is located at the upper edge of the bin body.

[0021] By adopting the above technical solution, a stacking-positioning-adsorption closed loop is formed for the turnover warehouse. The flange of the upper turnover warehouse presses against the top edge of the lower warehouse to achieve mechanical positioning, which can realize the stable stacking of multiple turnover warehouses and save the stacking space. When the magnet on the turnover warehouse is energized, it forms electromagnetic adsorption with the electric push rod on the top, which facilitates precise docking and helps the robot to complete the handling operation.

[0022] The automatic following method for a following robot used in mushroom shelf cultivation provided in this application adopts the following technical solution:

[0023] An automatic following method for a robot used in mushroom shelf cultivation, characterized by the following steps:

[0024] (1) The distance S and angle between the base station and the tag are calculated by using a micro antenna array and ultra-wideband technology, and the coordinate values ​​x and y of the base station are calculated.

[0025] (2) Send the above coordinate values ​​x and y to the motor controller through the serial port on the base station;

[0026] (3) When the angle between the tag position and the base station is less than the set value, the motor controller controls the drive motor to move straight;

[0027] (4) When the angle between the tag position and the base station is greater than the set value, the motor controller controls the drive motor to reduce the angle in the corresponding direction until it is less than the set value and goes straight, thus realizing differential turning;

[0028] (5) When the value of y in the coordinates is less than the set following distance value, stop following.

[0029] By adopting the above technical solution, the base station calculates the tag distance S and angle α through the UWB antenna array; if the angle is less than the threshold, it moves straight; if the angle is greater than the threshold, it turns at differential speed; if the distance is less than the set value, it stops. This solves the problems of traditional infrared / Bluetooth tracking, such as susceptibility to obstruction, low accuracy, easy collision, jamming, and target loss during tracking, and improves positioning accuracy and tracking response time; it is suitable for the working environment of narrow, winding, and obstacle-ridden mushroom shelf.

[0030] Furthermore, the specific method for calculating the distance s and angle α between the base station and the tag in step (1), and calculating the coordinates x and y of the base station, is as follows:

[0031] By calculating the distance *s* between the tag and the base station using the signal's time-of-flight, and by relating the distance difference *r* between the two paths to the phase difference between the tag at locations A and B and the UWB signal wavelength *λ*, we can obtain:

[0032]

[0033] We obtain this through the relationships of trigonometric functions:

[0034]

[0035] The formula for calculating base station coordinates is as follows:

[0036]

[0037] Where: α is the angle between the tag position and the base station; d represents the distance between tag positions A and B.

[0038] By adopting the above technical solution, introducing a phase difference-wavelength model, and combining trigonometric function geometric calculation with a dual-path difference algorithm, multipath interference is suppressed, improving the robustness of positioning in complex environments. The distance S is obtained through TOF ranging; the path difference r is calculated using the phase difference θ and wavelength λ; and the coordinates x and y of the tag relative to the base station are calculated using trigonometric functions. This achieves two-dimensional positioning with a single base station, reducing deployment costs. The ranging accuracy between the base station and the tag is 10 cm, the angle measurement accuracy is 5 degrees, and the following distance is over 30 meters, effectively addressing the problem of insufficient following accuracy. This expands the robot's working range and enhances its applicability in large-scale planting.

[0039] Furthermore, the specific method for the motor controller to control the drive motor to move straight in step (3) is as follows:

[0040] Linear motion control method: The motor controller sends precisely matched synchronization pulse commands to the left and right drive wheels, ensuring that the two wheels always maintain a completely consistent angular velocity ω. L =ω R =ω0 and rotation angle By combining real-time encoder feedback and PID closed-loop control, the linear velocities of the two drive wheels are ensured to be strictly equal, v = rω0, thereby completely eliminating lateral slippage error and achieving high-precision, offset-free linear motion, where: ω L ω R θ represents the angular velocity of the left and right drive wheels, in rad / s. L θ R θ represents the rotation angle of the left and right drive wheels, in rad. L This is the target speed reference value.

[0041] By adopting the above technical solution, zero-slip linear motion is achieved through triple protection of dual-wheel angular velocity synchronization, encoder feedback, and PID closed-loop control; the controller sends synchronization pulses to the left and right wheel angular velocities ω. L =ω R Straight-line travel without deviation; improved docking accuracy and reduced number of retries.

[0042] Furthermore, the specific method for the motor controller in step (4) to control the drive motor to reduce the angle in the corresponding direction until it is less than the set value and proceeds straight, thus achieving differential turning, is as follows:

[0043] (1) In-situ rotation steering motion control method: By locking the pivot of a single drive wheel, the left wheel ω is frozen when turning left. L =0, θ L =0, only the right wheel is driven at ω R Rotate; freeze the right wheel ω when turning right. R =0, θ R =0, only the left wheel is driven at ω L Rotation, through the equations of geometric motion Precise control of vehicle body yaw angle The vehicle body deflection angle is expressed in rad; d is the diameter of the drive wheel; W is the track width.

[0044] (2) In-journey steering motion control method: Establishing the velocity gradient equation Δ v =v R -v L =k·R c Control the left and right wheels to generate a speed gradient, where: R c The target radius of curvature is k; k is the system coupling coefficient, dynamically distributing the angular velocities of the left and right wheels. and This allows the vehicle body to form a smooth arc trajectory with an adjustable radius of curvature around the instantaneous center of rotation.

[0045] By adopting the above technical solution, the "in-situ rotation + on-the-go turning" dual-mode integration achieves zero-radius turning and smooth trajectory switching; the velocity gradient equation + instantaneous rotation center model realizes a smooth arc trajectory with adjustable curvature radius. This solves the problems of traditional AGVs having large turning radii that prevent them from turning around; fixed curvature turning that easily leads to collisions with shelves; and uneven trajectories and cargo swaying during turning. It achieves zero-radius in-situ turning with a minimum aisle width of ≤1.2 meters; smooth, uninterrupted turning trajectory without cargo swaying; and adaptive curvature to suit complex aisle layouts.

[0046] In summary, the present invention has at least one of the following beneficial technical effects:

[0047] (1) In this invention, the automatic following, lifting and suction functions are realized through the coordinated operation of the walking following system, the lifting mechanism and the ceiling clamping mechanism. This solves the problems of low efficiency of manual handling and high risk of high-altitude operation in traditional mushroom cultivation, significantly improves the automation level of mushroom cultivation, and reduces labor intensity and safety risks.

[0048] (2) This invention, by employing a parallelogram mechanism and an electric actuator, achieves stable lifting, solving the problems of platform tilting and poor stability in high-altitude operations. The lifting mechanism supports a lifting range of 168mm to 690mm and a dynamic load capacity of 120 kg, precisely meeting the operational needs at different heights in mushroom shelf cultivation, and improving operational safety and efficiency. It effectively solves the safety hazards of high-altitude operations while improving the accuracy and adaptability of lifting operations.

[0049] (3) This invention supports a multi-label mode, which can provide services to multiple staff members simultaneously, solving the need for multi-person collaborative work, improving work efficiency and reducing equipment costs. It achieves seamless connection between planting, harvesting and transportation, and improves the overall efficiency of mushroom shelf cultivation. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the combined structure of the present invention and the shelf.

[0051] Figure 2 This is a schematic diagram of the overall structure of the follower robot in this invention.

[0052] Figure 3 This is a schematic diagram of the walking and following system structure in this invention.

[0053] Figure 4 This is a schematic diagram of the positioning algorithm between the base station and the tag in this invention.

[0054] Figure 5This is a schematic diagram illustrating the working principle of the base station and tag in this invention.

[0055] Figure 6 This is a schematic diagram of the wiring between the base station, the motor controller, and the motor in this invention.

[0056] Figure 7 This is a schematic diagram of the lifting mechanism in this invention.

[0057] Figure 8 This is a full sectional view of the lifting mechanism in this invention.

[0058] Figure 9 This is a schematic diagram showing the connection and installation of the lifting mechanism and the walking following system in this invention.

[0059] Figure 10 This is a schematic diagram of the ceiling clamping mechanism in this invention.

[0060] Figure 11 This is a schematic diagram of the turnover warehouse structure in this invention.

[0061] Figure 12 This is a schematic diagram of the bottom structure of the turnover warehouse in this invention.

[0062] Figure 13 This is a schematic diagram of the structure after multiple turnover warehouses are stacked in this invention.

[0063] In the diagram: 1. Ground; 2. Shelf; 3. Following robot; 4. Walking and following system; 4-1. Base; 4-2. Right drive wheel; 4-3. Left drive wheel; 4-4. Left drive motor; 4-5. Right drive motor; 4-6. Caster wheel; 4-7. Base station; 4-8. Motor controller; 4-9. Power supply; 5. Lifting mechanism; 5. Top frame; 5-1. Bottom frame; 5-2. First link; 5-3. Second link; 5-4. Slide; 5-5. Roller; 5-6. Middle link; 5-7. Electric push rod; 5-8. Ceiling clamping mechanism; 6. Mounting base; 6-1. Lifting electric push rod; 6-2. Telescopic electric push rod; 6-3. Positioning clamping plate; 6-4. Ceiling electric push rod; 6-5. Turnover bin; 7. Bin body; 7-1. Magnet; 7-2. Flange; 7-3. Reinforcing rib; 7-4. Bin top; 7-5. Bin bottom; 7-6. Label; 8. Mounting plate; 9. Platform; 10. Detailed Implementation

[0064] The present invention will be further described below with reference to the accompanying drawings and embodiments:

[0065] Example 1

[0066] like Figure 1-2As shown, a follower robot for mushroom shelf cultivation consists of a shelf 2 set on the ground 1, a tag 8 carried by the operator, and a follower robot 3. The follower robot is composed of a walking and following system 4, a lifting mechanism 5, a ceiling-mounted clamping mechanism 6, and a turnover bin 7. The walking and following system 4 is set at the bottom of the follower robot 3, enabling the follower robot 3 to walk and automatically follow the operator. The lifting mechanism 5 is set above the walking and following system 4 via a mounting plate 9. The top of the lifting mechanism is connected and fixed to a platform 10 for stacking the turnover bins, and the platform 10 moves up and down with the lifting mechanism 5. The clamping and ceiling-mounted mechanism 6 is connected and fixed to the mounting plate 9, realizing the clamping and ceiling-mounting action of the turnover bin 7.

[0067] like Figure 3 As shown, the walking and following system 4 consists of a base 4-1, omnidirectional wheels 4-6 located at the bottom of the base 4-1, a left drive wheel 4-3, a right drive wheel 4-2, a left drive motor 4-4, a right drive motor 4-5; and a base station 4-7, a motor controller 4-8, and a power supply 4-9 located at the top of the base 4-1. The left and right drive wheels are driven independently by the left and right drive motors, which are driven and controlled by the motor controller 4-8. The power supply 4-9 is connected to the motor controller 4-8 for power supply.

[0068] like Figure 4-6 As shown, base stations 4-7 and tag 8 employ UWB-X2-AOA modules. Through a micro-antenna array and ultra-wideband technology, tag 8 transmits positioning signals. By calculating the phase difference of the signal received at different locations, the round-trip propagation time is determined, and the round-trip distance is calculated. Ultimately, the distance and angle between tag 8 and base station 4-7 are accurately measured. The ranging accuracy between the base station and the tag is 10 cm, the angle measurement accuracy is 5 degrees, and the following distance is over 30 meters. Data is transmitted via the serial port on the base station to the motor controller, which then controls the drive motors to operate, enabling the following robot 3 to automatically follow the operator.

[0069] To achieve stable lifting and lowering, and to address the issues of platform tilting and poor stability during high-altitude operations, this embodiment improves the safety and accuracy of lifting operations, adapting to different height requirements. Specifically, a parallelogram-shaped lifting mechanism is used in this embodiment. Figure 7-8As shown, the lifting mechanism 5 is connected and installed above the walking and following system 4 via the mounting plate 8. The lifting mechanism 5 is composed of a top frame 5-1, a bottom frame 5-2, a first connecting rod 5-3, a second connecting rod 5-4, a slide groove 5-5, a roller 5-6, a middle connecting rod 5-7, and an electric push rod 5-8. The top frame 5-1 and the bottom frame 5-2 are arranged in parallel, and slide grooves 5-5 are provided on their inner sides. The first connecting rod 5-3 and the second connecting rod 5-4 are arranged crosswise. One end of the first connecting rod 5-3 is hinged to the top frame 5-1, and the other end is slidably connected to the bottom frame 5-2 via the roller 5-6. One end of the second connecting rod 5-4 is hinged to the bottom frame 5-2, and the other end is slidably connected to the top frame 5-1 via the roller 5-6. The middle connecting rod 5-7 is fixedly arranged between the two second connecting rods 5-4. One end of the electric push rod 5-8 is hinged to the bottom frame 5-1, and the other end is hinged to the middle connecting rod 5-7. When the electric push rod 5-8 extends, it pushes the intermediate connecting rod 5-7 upward. The intermediate connecting rod 5-7 then pushes the second connecting rod 5-4 outward, widening the intersection angle between the first connecting rod 5-3 and the second connecting rod 5-4. At this time, the roller of the first connecting rod 5-3 slides outward along the groove of the bottom frame 5-2, while the roller of the second connecting rod 5-4 slides inward along the groove of the top frame 5-1, causing the top frame 5-1 to rise relative to the bottom frame 5-1. Conversely, when the electric push rod 5-8 shortens, the intersection angle of the two connecting rods closes, and the top frame lowers relative to the bottom frame. The entire process, through the cooperation of the rollers and the groove, ensures the linear movement of the frame, and the opening and closing of the connecting rods achieves the lifting function. Ultimately, this achieves the lifting operation of the turnover warehouse.

[0070] To achieve precise pushing between the turnover warehouse 7 and the shelf 2, and to solve the problems of low efficiency and poor accuracy of manual operation in traditional handling, this embodiment uses a ceiling-mounted clamping mechanism 6. Specifically, as shown... Figure 9-10 As shown, the ceiling-mounted clamping mechanism 6 is composed of a mounting base 6-1, a lifting electric push rod 6-2, a telescopic electric push rod 6-3, a positioning clamping plate 6-4, and a ceiling-mounted electric push rod 6-5. The bottom of the lifting electric push rod 6-2 is fixedly connected to the mounting plate 8. The mounting base 6-1 is connected to the top of the lifting electric push rod 6-2. The telescopic electric push rod 6-3 is connected to both sides of the mounting base 6-1. The positioning clamping plate 6-4 is located inside the telescopic electric push rod 6-3 to adjust the distance between the two positioning clamping plates 6-4. The ceiling-mounted electric push rod 6-5 is located in the middle of the mounting base 6-1. After the turnover bins are stacked, the lifting electric push rod 6-2 moves first to raise the entire device to the upper shelf. At this time, the telescopic electric push rod 6-3 adjusts the distance between the positioning clamping plates 6-4 to clamp the turnover bins. The ceiling-mounted electric push rod 6-5 then moves to push the turnover bins onto the shelf, reducing manual intervention and improving handling efficiency.

[0071] To achieve stable multi-layer stacking of the turnover warehouse, saving stacking space and facilitating precise docking for robotic handling, this embodiment improves the turnover warehouse structure. Specifically, as follows: Figure 11-13 The turnover bin 7 consists of a bin body 7-1, a magnet 7-2, and a flange 7-3. The flange 7-3 is located at the lower edge of the bin body 7-1 and serves as a limiting support when multiple turnover bins 7 are stacked. The magnet 7-2 is located at the upper edge of the bin body 7-1. This forms a stacking-positioning-adsorption closed loop for the turnover bins. The flange of the upper turnover bin presses against the top edge of the lower bin, achieving mechanical limiting. The magnet on the turnover bin forms an electromagnetic adsorption when energized with the electric push rod. This works well with the pushing mechanism, increasing the stability of the turnover bins during handling and improving handling efficiency.

[0072] Example 2

[0073] Unlike Implementation 1 described above, this embodiment aims to meet the actual needs of mushroom cultivation, precisely address the adaptability issues of working at different heights, improve the robot's practicality and operational efficiency, and satisfy diverse cultivation scenarios. In this embodiment, the lifting mechanism's lifting range is set to 168mm–690mm, its dynamic load capacity is 120 kg, and its travel time is 26 seconds. To effectively address the needs of multi-person collaborative work, improve the robot's work efficiency, and reduce equipment costs, this embodiment supports a multi-tag mode, allowing it to provide services to multiple workers simultaneously. To address the impact issues of instantaneous motor start-stop and the safety hazards of high-altitude operations, this embodiment extends motor life and improves operational safety.

[0074] like Figure 1-13 As shown, the steps of an automatic following method for a following robot used in mushroom shelf cultivation are as follows:

[0075] (1) The distance S and angle α between the base station and the tag are calculated by using a micro antenna array and ultra-wideband technology, and the coordinates x and y of the base station are calculated.

[0076] (2) Send the distance S and angle α to the motor controller via the serial port on the base station;

[0077] (3) When the angle is less than the set value, the motor controller controls the drive motor to go straight;

[0078] (4) When the angle is greater than the set value, the motor controller controls the drive motor to reduce the angle in the corresponding direction until it is less than the set value and goes straight, thus realizing differential turning;

[0079] (5) Stop following when the distance is less than the set following distance value.

[0080] The specific method for calculating the distance s and angle α between the base station and the tag in step (1) above, and for calculating the coordinates x and y of the base station, is as follows:

[0081] By calculating the distance *s* between the tag and the base station using the signal's time-of-flight, and by relating the distance difference *r* between the two paths to the phase difference between the tag at locations A and B and the UWB signal wavelength *λ*, we can obtain:

[0082]

[0083] We obtain this through the relationships of trigonometric functions:

[0084]

[0085] The formula for calculating base station coordinates is as follows:

[0086]

[0087] The specific method for the motor controller to control the drive motor to move straight in step (3) above is as follows:

[0088] Linear motion control method: The motor controller sends precisely matched synchronization pulse commands to the left and right drive wheels, ensuring that the two wheels always maintain a completely consistent angular velocity ω. L =ω R =ω0 and rotation angle By combining real-time encoder feedback and PID closed-loop control, the linear velocities of the two drive wheels are ensured to be strictly equal, v = rω0, thereby completely eliminating lateral slippage error and achieving high-precision, offset-free linear motion, where: ω L ω R θ represents the angular velocity of the left and right drive wheels, in rad / s. L θ R θ represents the rotation angle of the left and right drive wheels, in rad. L This is the target speed reference value. Specifically, to achieve linear motion of the walking and following system, whether moving forward or backward, the control system sends a synchronous drive command to the motor controller. The motor controller 4-8 sends identical control signals to the two drive motors 4-4 and 4-5. This signal can be represented in the following two equivalent forms:

[0089] a) Angle control mode: Instructs the two motors to rotate through the exact same target angle within the same time.

[0090] b) Speed ​​control mode: Instructs both motors to run at the same speed for the same amount of time.

[0091] Since the two drive wheels 4-2 and 4-3 are of the same specification and operate synchronously, the driving torque they generate is equal in magnitude and in the same direction, which enables the trolley to overcome resistance and move along a straight trajectory. The omnidirectional wheel 4-6 rolls freely to adapt to this straight movement.

[0092] In step (4) above, the motor controller controls the drive motor to reduce the angle in the corresponding direction until it is less than the set value and moves straight, and the specific method for achieving differential turning is as follows:

[0093] (1) In-situ rotation steering motion control method: By locking the pivot of a single drive wheel, the left wheel ω is frozen when turning left. L =0, θ L =0, only the right wheel is driven at ω R Rotate; freeze the right wheel ω when turning right. R =0, θ R =0, only the left wheel is driven at ω L Rotation, through the equations of geometric motion Precise control of vehicle body yaw angle This indicates the vehicle's deflection angle, in rad; d is the diameter of the drive wheel; W is the track width. Specifically, for stationary rotation: when the vehicle needs to make a very small range of directional adjustments, the motor controller 4-8 instructs one of the two drive motors to freeze and the other to rotate. Taking a stationary left turn as an example: it instructs the right drive motor 4-5 of the right drive wheel 4-2 to rotate forward at a speed of +ω. It instructs the left drive motor 4-4 of the left drive wheel 4-3 to freeze at 0 speed. A stationary right turn can be achieved by instructing the right wheel to freeze and the left wheel to rotate forward.

[0094] (2) In-journey steering motion control method: Establishing the velocity gradient equation Δ v =v R -v L =k·R c Control the left and right wheels to generate a speed gradient, where: R c The target radius of curvature is k; k is the system coupling coefficient, dynamically distributing the angular velocities of the left and right wheels. and This allows the vehicle body to form a smooth arc trajectory with an adjustable radius of curvature around its instantaneous center of rotation. Specifically, when the vehicle needs to change direction while moving, such as turning left from a specific position, the control system employs a differential control strategy. The motor controller 4-8 receives an asymmetric speed command set. Taking a left turn as an example: it commands the right drive motor 4-5 of the right drive wheel 4-2 to operate at a higher speed ω. high Run. Instruct the left drive motor 4-4 of the left drive wheel 4-3 to operate at a lower speed ω. low Run (where ω) low <ω high Because of the difference in linear velocity between the two wheels, the right wheel travels a greater distance than the left wheel. The vehicle will then move in an arc around an instantaneous center of rotation (ICR), achieving a smooth left turn. The principle for turning right is exactly the same; simply interchange the rotational speeds of the left and right wheels (i.e., left wheel ω). high >Right wheel ωlow The radius of curvature during a turn is determined by the speed difference between the two wheels, Δω = |ω_t|. high -ω low The greater the speed difference, the greater the curvature of the bend.

[0095] In follow mode, a safe distance of 100cm is set. The base station monitors the distance to the tag in real time. When the distance to the tag's center is within 100cm, the follow stops. When the distance between the base station and the tag exceeds 100cm, the base station sends the distance and angle differences to the motor controller, which then controls the motor to move straight or turn. In addition, the control system also features a deceleration stop function. By monitoring the distance and angle differences between the base station and the tag in real time, a deceleration distance is set. When the distance exceeds this distance, the vehicle moves at full speed to follow the target. Different speeds are set for different deceleration distances. For example, 100cm is set as the safe distance, and 300cm as the deceleration distance; the closer to the safe distance, the slower the speed. When the tag is more than 300cm from the base station, the vehicle speed is full speed; when the tag is 200cm away, the speed is 2 / 3 of full speed; when the tag is 150cm away, the speed is 1 / 3 of full speed; and when the tag is 100cm away, the vehicle dynamically stops, continuously following the target.

Claims

1. A follower robot for mushroom shelf cultivation, comprising shelves (2) set on the ground (1); and a tag (8) carried by the operator, characterized in that, The follower robot (3) includes: A walking follow system (4) is installed at the bottom of the following robot (3). The walking follow system enables the following robot (3) to walk and automatically follow the staff. The lifting mechanism (5) is located above the walking and following system (4) to realize the lifting and lowering action of the following robot (3); The clamping and suction mechanism (6) is connected to the walking and following system (4) to realize the clamping and suction action of the turnover warehouse (7).

2. The following robot for mushroom tiered cultivation according to claim 1, characterized in that: The walking and following system (4) consists of a base (4-1), omnidirectional wheels, drive wheels and drive motors located at the bottom of the base (4-1); and a base station (4-7), a motor controller (4-8) and a power supply (4-9) located at the top of the base (4-1); each drive wheel is driven independently by a drive motor, the drive motor is driven and controlled by the motor controller (4-8), and the power supply (4-9) is connected to the motor controller (4-8) for power supply.

3. The following robot for mushroom tiered cultivation according to claim 1, characterized in that: The lifting mechanism (5) is connected and installed above the walking and following system (4) via a mounting plate (8); the lifting mechanism (5) is composed of a top frame (5-1), a bottom frame (5-2), a first connecting rod (5-3), a second connecting rod (5-4), a slide groove (5-5), a roller (5-6), a middle connecting rod (5-7), and an electric push rod (5-8); the top frame (5-1) and the bottom frame (5-2) are arranged parallel to each other, and slide grooves (5-5) are provided on their inner sides; the first connecting rod (5-3) and the second connecting rod (5-4) are connected... 5-4) The first connecting rod (5-3) is hinged at one end to the top frame (5-1) and slidably connected to the bottom frame (5-2) via a roller (5-6); the second connecting rod (5-4) is hinged at one end to the bottom frame (5-2) and slidably connected to the top frame (5-1) via a roller (5-6); the middle connecting rod (5-7) is fixedly set between the two second connecting rods (5-4); the electric push rod (5-8) is hinged at one end to the bottom frame (5-1) and at the other end to the middle connecting rod (5-7).

4. The following robot for mushroom tiered cultivation according to claim 1, characterized in that: The clamping ceiling-mounting mechanism (6) is composed of a mounting base (6-1), a lifting electric push rod (6-2), a telescopic electric push rod (6-3), a positioning clamping plate (6-4), and a ceiling-mounting electric push rod (6-5). The bottom of the lifting electric push rod (6-2) is fixedly connected to the mounting plate (8). The mounting base (6-1) is connected to the top of the lifting electric push rod (6-2). The telescopic electric push rod (6-3) is connected to both sides of the mounting base (6-1). The positioning clamping plate (6-4) is located inside the telescopic electric push rod (6-3) to adjust the distance between the two positioning clamping plates (6-4). The ceiling-mounting electric push rod (6-5) is located in the middle of the mounting base (6-1).

5. The following robot for mushroom tiered cultivation according to claim 1, characterized in that: The turnover bin (7) is composed of a bin body (7-1), a magnet (7-2) and a flange (7-3); the flange (7-3) is located at the lower edge of the bin body (7-1) and is used for limiting support when multiple turnover bins (7) are stacked together; the magnet (7-2) is located at the upper edge of the bin body (7-1).

6. An automatic following method for a following robot used in mushroom tiered cultivation, characterized in that, Using the follow-up robot for mushroom tiered cultivation according to any one of claims 1-5, the steps are as follows: (1) The distance S and angle between the base station and the tag are calculated by using a micro antenna array and ultra-wideband technology, and the coordinate values ​​x and y of the base station are calculated. (2) Send the above coordinate values ​​x and y to the motor controller through the serial port on the base station; (3) When the angle between the tag position and the base station is less than the set value, the motor controller controls the drive motor to move straight; (4) When the angle between the tag position and the base station is greater than the set value, the motor controller controls the drive motor to reduce the angle in the corresponding direction until it is less than the set value and goes straight, thus realizing differential turning; (5) When the value of y in the coordinates is less than the set following distance value, stop following.

7. The automatic following method for a following robot used in mushroom tiered cultivation according to claim 6, characterized in that, The specific method for calculating the distance s and angle between the base station and the tag, and calculating the coordinates x and y of the base station in step (1) is as follows: By calculating the distance *s* between the tag and the base station using the signal's time-of-flight, and by relating the distance difference *r* between the two paths to the phase difference between the tag at locations A and B and the UWB signal wavelength *λ*, we can obtain: We obtain this through the relationships of trigonometric functions: The formula for calculating base station coordinates is as follows: Where: α is the angle between the tag position and the base station; d represents the distance between tag positions A and B.

8. The automatic following method for a following robot used in mushroom tiered cultivation according to claim 6, characterized in that, The specific method for controlling the drive motor to move straight in step (3) is as follows: Linear motion control method: The motor controller sends precisely matched synchronization pulse commands to the left and right drive wheels, ensuring that the two wheels always maintain a completely consistent angular velocity ω. L =ω R =ω0 and rotation angle By combining real-time encoder feedback and PID closed-loop control, the linear velocities of the two drive wheels are ensured to be strictly equal, v = rω0, thereby completely eliminating lateral slippage error and achieving high-precision, offset-free linear motion, where: ω L ω R θ represents the angular velocity of the left and right drive wheels, in rad / s. L θ R θ represents the rotation angle of the left and right drive wheels, in rad. L This is the target speed reference value.

9. The automatic following method for a following robot used in mushroom tiered cultivation according to claim 6, characterized in that, The specific method for controlling the drive motor to reduce the angle in the corresponding direction until it is less than the set value and then moving straight, as described in step (4), is as follows: (1) In-situ rotation steering motion control method: By locking the pivot of a single drive wheel, the left wheel ω is frozen when turning left. L =0, θ L =0, only the right wheel is driven at ω R Rotate; freeze the right wheel ω when turning right. R =0, θ R =0, only the left wheel is driven at ω L Rotation, through the equations of geometric motion Precise control of vehicle body yaw angle Indicates the vehicle body deflection angle, unit: rad; d is the diameter of the drive wheel; W is the wheel track. (2) In-journey steering motion control method: Establishing the velocity gradient equation Δ v =v R -v L =k·R c Control the left and right wheels to generate a speed gradient, where: R c The target radius of curvature is k; k is the system coupling coefficient, dynamically distributing the angular velocities of the left and right wheels. and This allows the vehicle body to form a smooth arc trajectory with an adjustable radius of curvature around the instantaneous center of rotation.

Images