Autonomous escape control system and control method for buried granary grain leveling robot

Abstract

The invention discloses an autonomous escape control system and method for a barn grain leveling robot when the barn grain leveling robot is buried. The control method comprises the steps that shielding information of a photoelectric sensor, pressure information collected by a pressure sensor and yaw angle, pitch angle and roll angle information, collected by an attitude sensor, of the barn grain leveling robot are obtained; determining a buried state and a buried process of the robot according to the shielding information and the pressure information; wherein the buried state comprises whether the granary grain leveling robot is buried or not and the buried position; the burying process is a shielding triggering sequence of the photoelectric sensor; the moving direction of the granary grain leveling robot is controlled according to the buried state and the buried process of the robot; if only part of the position of the granary grain leveling robot is buried, the granary grain leveling robot is controlled to move in the direction opposite to the buried position; if the granary grain leveling robot is completely buried, the granary grain leveling robot is controlled to move in the direction where the finally triggered photoelectric sensor is located.

Description

Technical Field

[0001] The present invention belongs to the technical field of grain-leveling robot control in a granary, and in particular relates to a control system and a control method for a grain-leveling robot to autonomously escape from being buried. Background Art

[0002] Grain leveling is a labor-intensive task in the management and maintenance of large granaries. Traditionally, manual labor is employed, resulting in high workload and harsh working environments. Modern granary management and maintenance require automation and intelligent upgrades. While effective leveling can be achieved using grain leveling robots (such as the patented "A Segmented Variable-Pitch Screw-Drive Leveling Robot, Patent No. ZL 202110574759.0"), current leveling robots are still relatively inefficient in terms of automation and intelligence. While they can perform simple obstacle avoidance and navigation, they often require significant manual intervention.

[0003] When performing grain leveling operations in a granary, a grain-leveling robot needs to flatten the grain pile into a horizontal state as much as possible. However, the grain piles in large granaries are often relatively high and have large slopes. During the climbing operation, the grain-leveling robot can easily be buried by the self-flowing effect of grain, which is like an avalanche. This will cause the grain-leveling operation to be interrupted and affect the operation efficiency. It may even cause the robot to lose contact and be difficult to find, resulting in losses.

[0004] Existing methods for determining a robot's buried state typically use pressure sensors placed on the robot to detect the machine's buried state. However, when buried, the robot is typically not horizontal but tilted, and the pressure is not directed vertically downward. Consequently, determining the robot's buried state and finding the direction to escape are not accurate. Even when considering the robot's position, the judgment model is complex and ineffective. Therefore, there is an urgent need for an accurate and reliable control method that can help a buried grain-leveling robot autonomously escape from a buried position. This method would allow the robot to continue completing subsequent tasks after being freed, thereby improving the robot's efficiency in leveling grain. Summary of the Invention

[0005] The purpose of the present invention is to overcome the defects of the prior art and provide a control system for a granary grain leveling robot to escape autonomously when it is buried. The control system can accurately detect and judge the buried information of the granary grain leveling robot, so as to realize the autonomous escape of the granary grain leveling robot when it is buried.

[0006] The present invention also provides a method for controlling the autonomous escape of a grain silo leveling robot when it is buried. The method integrates the occlusion information, pressure information and posture information of the grain silo leveling robot's photoelectric sensor to control the escape mode of the grain silo leveling robot, thereby improving the reliability and efficiency of the escape of the grain silo leveling robot.

[0007] The technical solution provided by the present invention is:

[0008] A control system for a grain-leveling robot in a granary to escape autonomously when buried, comprising:

[0009] Four groups of photoelectric sensors are respectively arranged on the four sides of the shell of the grain silo leveling robot;

[0010] Each group of photoelectric sensors includes two photoelectric sensors, and the two photoelectric sensors are respectively arranged close to the two ends of the side surface, and the emitting ends of the photoelectric sensors are arranged toward the outside of the side surface;

[0011] The attitude sensor is installed on the chassis of the grain silo leveling robot and is used to detect the yaw angle, pitch angle and roll angle of the grain silo leveling robot;

[0012] Four pressure sensors are mounted on the top of the housing of the grain silo leveling robot and are respectively arranged near the four corners of the top of the grain silo leveling robot;

[0013] A main control chip is electrically connected to the photoelectric sensor, the posture sensor and the pressure sensor respectively; the main control chip determines the buried state and buried process information of the granary grain-leveling robot according to the status information of the photoelectric sensor, the posture sensor and the pressure sensor, and controls the movement of the granary grain-leveling robot according to the buried state and buried process information, so as to enable the granary grain-leveling robot to escape from trouble.

[0014] A method for controlling a grain-leveling robot to escape from a buried silo automatically, using the control system for controlling a grain-leveling robot to escape from a buried silo automatically, comprises the following steps:

[0015] Step 1: Obtain the occlusion information of the photoelectric sensor, the pressure information collected by the pressure sensor, and the yaw angle, pitch angle, and roll angle information of the grain silo leveling robot collected by the attitude sensor;

[0016] Step 2: determining the buried state and buried process of the robot according to the occlusion information and the pressure information;

[0017] The buried state includes: whether the grain leveling robot is buried, and the buried position; the buried process includes: the blocking triggering sequence of the photoelectric sensor;

[0018] Step 3: Control the moving direction of the grain-leveling robot in the granary according to the buried state and buried process of the robot;

[0019] Among them, if only part of the granary grain leveling robot is buried, the granary grain leveling robot is controlled to move in the opposite direction of the buried position; if the granary grain leveling robot is completely buried, the granary grain leveling robot is controlled to move in the direction of the last triggered photoelectric sensor.

[0020] Preferably, in the step 2, if a photoelectric sensor is blocked and triggered, and a pressure sensor is triggered, it is determined that the grain silo leveling robot is buried.

[0021] Preferably, in the step three, the method further comprises: using a PID strategy to control the moving speed of the grain leveling robot in the granary according to the following formula;

[0022] Δu(k)=K p [e(k)-e(k-1)]+K i e(k)+K d [e(k)-2e(k-1)+e(k-2)];

[0023] Wherein, e(k) represents the error at the current moment k, e(k) = v_target-v, v represents the current moving speed of the granary leveling robot, and v_target represents the current target moving speed of the granary leveling robot; K p is the proportionality coefficient, K i is the integral coefficient, K d is the differential coefficient, Δu(k) represents the control increment of k at the current moment, u(k)=u(k-1)+Δu(k), u(k) represents the control output of k at the current moment, u(k-1) represents the output at k-1 moment; e(k-1) and e(k-2) represent the error at k-1 moment and the error at k-2 moment, respectively.

[0024] Preferably, the target moving speed v_target of the grain leveling robot in the granary is determined according to the pressure information collected by the pressure sensor:

[0025]

[0026] Among them, λ is the adjustment coefficient, v0 is the preset standard escape speed, P is the current equivalent pressure of the granary leveling robot, P δ is the trigger pressure threshold of the pressure sensor, P max is the maximum pressure value.

[0027] Preferably, the calculation formula of the equivalent pressure is:

[0028]

[0029] Among them, R is the rotation matrix of the grain leveling robot in the granary, P B1 、PB2 、P B3 and P B4 These are the pressure values ​​collected by four pressure sensors.

[0030] Preferably, the rotation matrix is:

[0031]

[0032] Among them, ψ is the yaw angle of the granary grain leveling robot, θ is the pitch angle of the granary grain leveling robot, and φ is the roll angle of the granary grain leveling robot.

[0033] Preferably, the adjustment coefficient is set as:

[0034]

[0035] Among them, F T is the driving force of the granary grain leveling robot, σ is the driving force coefficient, and P is the equivalent pressure of the granary grain leveling robot.

[0036] Preferably, the autonomous escape control method for the granary grain-leveling robot when buried further includes:

[0037] The positioning module detects the position information of the grain leveling robot in the granary in real time and sends it to the host computer.

[0038] The beneficial effects of the present invention are:

[0039] The control system for the grain silo leveling robot to escape autonomously when buried provided by the present invention can accurately detect and determine the buried information of the grain silo leveling robot, thereby enabling the grain silo leveling robot to escape autonomously when buried.

[0040] The present invention provides an autonomous escape control method for a grain silo leveling robot when it is buried, which integrates the occlusion information, pressure information and posture information of the grain silo leveling robot's photoelectric sensor to control the escape mode of the grain silo leveling robot, thereby improving the reliability and efficiency of the grain silo leveling robot's escape.

[0041] The present invention provides a method for controlling a grain silo leveling robot to autonomously escape from a buried state. The method also determines the movement speed of the grain silo leveling robot when escaping based on the pressure under which the grain silo leveling robot is buried, thereby further improving the success rate and efficiency of the grain silo leveling robot in escaping from a buried state. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is a schematic structural diagram of a grain-leveling robot for use in a granary according to an embodiment of the present invention.

[0043] Figure 2 This is a front view of a robot leveling grain on a sloped surface of a grain pile in an embodiment of the present invention.

[0044] Figure 3 This is a left view of the robot leveling grain on the slope of a grain pile in an embodiment of the present invention.

[0045] Figure 4 Schematic diagram of various states of a robot partially buried by food in an embodiment of the present invention.

[0046] Figure 5 Schematic diagram of various states of a robot partially buried by grain and its escape directions in an embodiment of the present invention.

[0047] Figure 6 Schematic diagram of various states of a robot completely buried by food and its escape directions in an embodiment of the present invention.

[0048] Figure 7 Schematic diagram of different postures and orientations of the robot buried below the grain surface in an embodiment of the present invention.

[0049] Figure 8 This is a schematic diagram of a robot being completely buried in grain and rolling up and down in an embodiment of the present invention.

[0050] Figure 9-10 This is a schematic diagram of the escape direction of a robot partially buried by grain in an embodiment of the present invention.

[0051] Figure 11 (a) and (b) are schematic diagrams of the robot being completely buried by food in an embodiment of the present invention, but with different directions of escape during the burial process.

[0052] Figure 12 Schematic diagram of the posture calculation model of the robot in the completely buried state in an embodiment of the present invention.

[0053] Figure 13 Flowchart of a method for controlling a robot to escape autonomously when buried in an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The present invention will be described in further detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

[0055] The present invention provides a control system for a grain-leveling robot in a granary to escape autonomously when buried. The control system mainly comprises four groups of photoelectric sensors, a posture sensor, four pressure sensors and a main control chip.

[0056] Four groups of photoelectric sensors are respectively arranged on the four sides of the shell of the grain-leveling robot; each group of photoelectric sensors includes two photoelectric sensors, and the two photoelectric sensors in each group are respectively arranged near the two ends of the side on which they are located, with the emitting ends of the photoelectric sensors facing the outside of the side on which they are located. For example, the two photoelectric sensors arranged on the front (side) surface of the grain-leveling robot are respectively arranged near the left and right ends of the front (side) surface, and the emitting ends of the two photoelectric sensors are arranged facing straight ahead. The photoelectric sensors detect whether there are obstacles. When a certain position of the grain-leveling robot is buried and blocked by grain, the photoelectric sensor signal at that position will be triggered.

[0057] An attitude sensor is mounted on the robot's chassis to monitor its position, including its yaw, pitch, and roll angles. Four pressure sensors are mounted on the robot's top shell, one near each of its four corners. When a portion of the robot is submerged in grain, the corresponding pressure sensor detects the pressure.

[0058] The main control chip is electrically connected to the photoelectric sensor, the posture sensor, and the pressure sensor, respectively; It is also electrically connected to the drive control mechanism of the grain silo leveling robot, enabling drive control of the grain silo leveling robot. The main control chip determines the buried state and buried process information of the grain silo leveling robot based on the signals transmitted by the photoelectric sensor, the posture sensor, and the pressure sensor. It then controls the movement (escape) process of the grain silo leveling robot based on this buried state and buried process information, thereby enabling the grain silo leveling robot to escape. In this embodiment, the main control chip is mounted on the chassis of the grain silo leveling robot.

[0059] Preferably, the autonomous escape control system further comprises: a positioning module, which is mounted on the outer shell of the grain silo leveling robot. The positioning module is capable of collecting the position information of the grain silo leveling robot in real time. The positioning module is electrically connected to the main control chip, and the main control chip is capable of controlling the positioning module to upload the position information of the grain silo leveling robot to the host computer. When the grain silo leveling robot is buried, especially when the grain silo leveling robot is completely buried and unsupervised, if the grain silo leveling robot is buried too deep and cannot escape through its own power, the uploaded position information can provide a basis for manual retrieval.

[0060] The present invention also provides a method for controlling a grain-leveling robot in a granary to escape autonomously when buried, and the specific implementation process is as follows.

[0061] 1. The main control chip obtains in real time the occlusion information of the photoelectric sensor, the pressure information collected by the pressure sensor, and the yaw angle, pitch angle and roll angle information of the grain silo leveling robot collected by the attitude sensor.

[0062] 2. The main control chip uses the occlusion information and the pressure information to determine the buried state and buried process of the robot.

[0063] The buried state includes: whether the granary grain leveling robot is buried, and the buried position; the buried process is: the blocking triggering sequence of the photoelectric sensor.

[0064] First, the occlusion-pressure model of the robot's burial is derived by fusing information from photoelectric and pressure sensors. This model has four states, with T representing occlusion and F representing absence, namely TT, TF, FT, and FF. The TT state indicates that at least one photoelectric sensor is occluded and at least one pressure sensor detects pressure; the TF state indicates that at least one photoelectric sensor is occluded, but no pressure sensor detects pressure; the FT state indicates that no photoelectric sensor is occluded, but at least one pressure sensor detects pressure; and the FF state indicates that no photoelectric sensor is occluded and no pressure sensor detects pressure.

[0065] TT means there is both pressure and obstruction, and the robot is judged to be partially or completely buried. The specific state needs to be subdivided according to the position distribution and trigger state of the photoelectric sensor; TF means there is obstruction but no pressure, and the robot is judged to have encountered an obstacle, and the robot's obstacle avoidance program is activated; FT means there is pressure but no obstruction, and the robot is in a state where objects are placed on the upper cover or the pressure sensor fails, and the robot's warning alarm program is activated; FF means there is no obstruction and no pressure, and the robot is in a state with no obstacles around it, and continues to execute the grain leveling task program normally.

[0066] If the robot's "occlusion-pressure" model determines that it is in the TT state at this time, then the position of the blocked photoelectric sensor and the pressure sensor that detects pressure will be used to determine whether the robot is completely buried or partially buried. If it is partially buried, it will be determined whether it is part of the front, back, left, or right, or a combination of several parts. Based on the orientation information and trigger sequence of the photoelectric sensor and the value of the pressure sensor, combined with the posture sensor information, a model of the robot being buried can be constructed, and then an escape strategy can be formulated based on the trapped model. Figure 1As shown, for example, if the fusion information is in the TT state, and only sensor A1 installed at the front left side of the robot and sensor A2 installed at the left end of the front side of the robot are blocked, it can be inferred that the left front of the robot is buried by grain; if the fusion information is in the TT state, and only A1, A2, A7, and A8 of the photoelectric sensors are blocked, it can be inferred that the left side of the robot is buried by grain; if the fusion information is in the TT state and all the photoelectric sensors are blocked, it is necessary to use the triggering sequence of the photoelectric sensors to infer the process of the robot being buried by grain. If the triggering sequence is A4→A3→A5→A1→A2→A6→A7→A8, it can be known that the robot was buried from the right front and finally buried by the left rear.

[0067] If the robot is completely buried, or in extreme cases, it may have rolled under the grain surface, the photoelectric sensor is always triggered, making it impossible to determine the rolling process. In this extreme case, the pressure sensor and attitude sensor information are used as the primary information to determine the robot's buried status.

[0068] 3. Control the moving direction of the granary leveling robot according to the robot's buried state and buried process.

[0069] If the robot is judged to be partially buried, the robot is controlled to move in the direction opposite to the buried part. For example, if the left front of the robot is buried by grain, the robot is controlled to escape by retreating, moving to the right, or moving to the right rear.

[0070] If it is determined that the robot is buried on the left side, the robot is controlled to move to the right to escape.

[0071] If the robot is completely buried, and the order in which the photoelectric sensors are blocked indicates that the robot was buried starting from the right front and ending at the left rear, if the robot does not roll over after being buried, the robot can escape by moving to the left or right, that is, moving toward the direction in which it was last buried.

[0072] If the robot is completely buried and rolls over, the direction of escape movement is determined based on the post-rolling posture and the pressure of the pressure sensor.

[0073] In addition, if the robot's "occlusion-pressure" model determines that it is in the TT state at this time, the main control chip immediately controls the positioning module to send the robot's current positioning information to the outside, ensuring that the positioning information is sent before the robot is completely buried.

[0074] After the robot is completely buried by grain, it will automatically execute the automatic escape program. However, if there is no one on site to supervise the robot when it is completely buried and the robot is buried too deep and cannot escape by its own power, the location information sent when it is buried will provide important basis information for manual search.

[0075] Preferably, the method for controlling the autonomous escape of a buried grain-leveling robot provided by the present invention further includes determining the robot's escape direction and escape speed, and performing feedback control. If the pressure is too high, the escape speed should be slow; if the pressure is too low, the escape speed can be fast. In the extreme case of complete burial, the correct escape direction is verified using the pressure sensor. Specifically, during a correct escape, the pressure should gradually decrease to increase the success rate of the escape.

[0076] Since the robot is not horizontal in most cases when buried, and the pressure sensor measures the positive pressure on the installation surface, it is necessary to convert the pressure into a horizontal model to determine the buried depth of the robot's four corner points, such as Figure 12 The pressure change is combined with the robot's motion description method, where ψ is the yaw angle, θ is the pitch angle, and φ is the roll angle, and is calculated using the Euler angle ZYX model, where:

[0077] Rotation matrix around the X axis (roll angle φ):

[0078]

[0079] Rotation matrix around the Y axis (pitch angle θ)

[0080]

[0081] The rotation matrix around the Z axis (yaw angle ψ):

[0082]

[0083] The final rotation matrix R is:

[0084]

[0085] The setting of the escape speed is related to the pressure of the sensor. If the pressure is too high, the escape speed will be lower. If the pressure is too low, the escape speed can be higher. The relationship is:

[0086]

[0087] Among them, v_target represents the target moving speed of the granary leveling robot, λ is the adjustment coefficient, v0 is the preset standard escape moving speed, P is the current equivalent pressure of the granary leveling robot, P δ is the trigger pressure threshold of the pressure sensor, P maxis the maximum pressure value (i.e. the upper limit of pressure sensor detection); F T is the driving force of the robot (the driving force of the robot's driving motor at the rated speed), σ is the driving force coefficient of the robot, and the recommended value is [0.5~1.0]; R is the rotation matrix of the granary leveling robot, P B1 、P B2 、P B3 and P B4 These are the pressure values ​​collected by four pressure sensors.

[0088] The speed regulation during the escape process adopts discrete PID control strategy, and its control method is as follows:

[0089] Δu(k)=K p [e(k)-e(k-1)]+K i e(k)+K d [e(k)-2e(k-1)+e(k-2)];

[0090] Wherein, e(k) represents the error at the current moment k, e(k) = v_target-v, v represents the current moving speed of the granary leveling robot, and v_target represents the current target moving speed of the granary leveling robot; K p is the proportionality coefficient, K i is the integral coefficient, K d is the differential coefficient, Δu(k) represents the control increment of k at the current moment, u(k)=u(k-1)+Δu(k), u(k) represents the control output of k at the current moment, u(k-1) represents the output at k-1 moment; e(k-1) and e(k-2) represent the error at k-1 moment and the error at k-2 moment, respectively.

[0091] The robot determines the corresponding escape direction and escape speed parameters according to different buried states and executes the automatic escape program. The program drives the robot's walking mechanism to move. At the same time, it detects whether the escape is completed during the escape process. If the robot completes the autonomous escape, the autonomous escape program ends. If not, a new round of state detection is carried out. The robot will generate a new escape strategy according to the new buried state until the robot completes the autonomous escape task.

[0092] The control system and control method for the grain silo leveling robot to escape autonomously when buried provided by the present invention will be further described below with reference to specific embodiments.

[0093] Example

[0094] like Figure 1As shown, the grain-leveling robot used in this embodiment includes a robot chassis, a spiral wheel 1, a power battery 2, a housing 3, a drive shaft 4, a frame 5, a motor, and a reducer 6. The frame 5 of the grain-leveling robot is mounted on the robot chassis, the housing 3 covers the frame 5, the power battery 2 is mounted on the robot chassis, and the spiral wheel 1 and drive components are mounted on both sides of the robot chassis. The specific structure of the robot can be found in Patent ZL 202110574759.0.

[0095] The main control chip and posture sensor 9 are installed on the robot chassis, and the robot shell protects the main control chip and the posture sensor to play a mechanical protection role; the pressure sensor 7, photoelectric sensor 8, and positioning module are installed on the robot shell; the main control chip can receive the control signal received by the signal receiver to control the manual / automatic mode of the robot and adjust the speed and direction of the robot; the main control chip can judge the buried state of the robot according to the comprehensive status information of the pressure sensor, photoelectric sensor, and axis posture sensor during automatic grain leveling operation, and construct a trapped model and automatically execute the escape function program according to the trapped model; the positioning module will promptly send the robot's position information to the outside when the main control chip determines that the robot is in a buried state; the robot's autonomous escape control method captures the moment and final state of the robot being buried according to the comprehensive status information of the pressure sensor, photoelectric sensor, and axis posture sensor, and constructs an escape control method through the main control chip program.

[0096] The specific process of the escape control method is as follows: Figure 13 As shown in the figure, when the grain-leveling robot has already reached a suitable position in the granary in manual mode and the signal receiver receives the automatic grain-leveling task command, the robot switches to automatic control mode. At this point, the robot performs the grain-leveling movement according to the trajectory planned by the control program, while simultaneously monitoring the status values ​​of the photoelectric sensors, pressure sensors, and attitude sensors in real time.

[0097] The robot controller divides the pressure sensor into two states: pressure and no pressure. The photoelectric sensor indicates two states: triggered (occlusion) and untriggered (no occlusion). The fused information from the two sensors represents four states, with T representing "active" and F representing "no occlusion," namely TT, TF, FT, and FF. This represents a four-state "occlusion-pressure" model. TT indicates both pressure and occlusion, indicating the robot is partially or completely buried. The specific state is further subdivided based on the location and trigger status of the photoelectric sensors. TF indicates occlusion but no pressure, indicating the robot is encountering an obstacle and initiating the robot's obstacle avoidance program. FT indicates pressure but no occlusion, indicating the robot is in an obstacle-free situation or a pressure sensor failure, initiating the robot's warning alarm program. FF indicates no occlusion and no pressure, indicating the robot is clear and continues to perform its grain-leveling task normally.

[0098] Once the robot's "occlusion-pressure" model determines the current TT condition, the main control chip immediately controls the positioning module to transmit the robot's current location information, ensuring that this information is transmitted before the robot is completely buried. Once the robot is completely buried, it will automatically execute the self-rescue sequence. However, if the robot is completely buried and there is no one on site to supervise it, and the burial is too deep to escape by its own power, the location information transmitted during the burial period is crucial for manual recovery.

[0099] like Figure 1 As shown, in this embodiment, the eight photoelectric sensors are A1, A2, A3, A4, A5, A6, A7, and A8, and the four pressure sensors are B1, B2, B3, and B4. A2 and A3 are mounted on the left and right ends of the front of the robot, respectively; A4 and A5 are mounted on the front and rear ends of the right side of the robot, respectively; A6 and A7 are mounted on the right and left ends of the rear side of the robot, respectively; and A1 and A8 are mounted on the front and rear ends of the left side of the robot. B1, B2, B3, and B4 are mounted on the left front, right front, right rear, and left rear of the top of the robot, respectively.

[0100] Different states correspond to different control methods. Different positions and numbers of sensors triggered in the same state also correspond to different control methods. The subdivided state can determine whether the robot is completely buried or partially buried, and when partially buried, whether it is part of the front, back, left, or right, or a combination of several parts. Based on the orientation information and triggering sequence of the photoelectric sensor and the numerical value of the pressure sensor, combined with the posture sensor information, a model of the buried robot can be constructed, and then an escape strategy can be formulated based on the trapped model.

[0101] Figure 2-8 The moving direction of the granary grain-leveling robot in different working conditions and when being buried and rescued is demonstrated. Figure 2This is a front view of the robot leveling grain on the slope of a grain pile; Figure 3 This is the left view of the robot leveling grain on the slope of a grain pile; Figure 4 Schematic diagram of the robot in various states (rotated, tilted downward, tilted upward, horizontal) when partially buried by grain; Figure 5 A schematic diagram showing the robot in various states of being partially buried by grain and the corresponding escape directions; Figure 6 This is a diagram showing the robot's various states and escape directions when it is completely buried by grain but does not roll over. Figure 7 Schematic diagram of different postures and orientations of the robot when it is completely buried. Figure 8 A schematic diagram of a robot being completely buried in food and rolling up and down.

[0102] There are many subdivisions of robot partial burial. Here are some typical robot escape control methods:

[0103] For example, the fusion information is in the TT state, and only A2 of the photoelectric sensor is blocked, such as Figure 9 As shown in state 1, it can be inferred that the robot is buried in the food in front of the left side, and the direction of escape can be backward or moving to the right.

[0104] For example, if the fusion information is in the TT state, and only A1, A2, A7, and A8 of the photoelectric sensor are blocked, such as Figure 9 From the process from state 1 to state 2, it can be inferred that the left side of the robot is buried by food, and the direction of escape can be moving to the right.

[0105] For example, if the fusion information is in the TT state, and only A1, A2, A3, A4, A5, and A5 of the photoelectric sensors are blocked, and the blocking order is A2→A1→A3→A4→A5→A6, then Figure 10 As shown, the process of the photoelectric sensor being blocked is the process of developing from state 1 to state 4. It can be inferred that the robot's left front, right front, and right rear are all buried by food, and the order of burial is left front → right front → right rear. The direction of escape can be moving to the right.

[0106] There are many different situations where a robot is completely buried. Here are two typical methods for escaping a completely buried robot:

[0107] For example, when the robot is completely buried, it does not roll under the grain surface, the fusion information is in the TT state, and all the photoelectric sensors are blocked. In this case, the triggering sequence of the photoelectric sensors is needed to infer the state of the robot being buried by grain. Figure 11As shown in (a), the triggering order of the robot's pressure sensor is A2→A1→A3→A4→A5→A6→A7→A8, so we know that the robot was buried from the left front, and the order of burial is left front→right front→right back→left back, so the escape direction is to move to the left back. Figure 11 As shown in (b), the triggering order of the robot's pressure sensors is A8→A7→A1→A2→A4→A3→A5→A6. It can be known that the robot was buried from the left front, and the buried order is left rear→left front→right front→right rear, so the direction of escape can be moving towards the right rear.

[0108] In the process of the machine being completely buried, in extreme cases, it is rolled under the grain surface at the same time (such as Figure 8 (As shown in the figure), at this point, because the photoelectric sensor is always triggered, it is impossible to determine the robot's tumbling progress. In this extreme case, the pressure sensor and attitude sensor information are used as the primary information to determine the robot's buried status. When the robot rolls to a stop, the pressure sensor and attitude sensor are used to correct the robot's position, and the robot then moves toward the direction of less pressure based on this corrected position to escape.

[0109] The robot's escape direction and speed are determined and feedback is adjusted. If the pressure is too high, the escape speed should be slow, while if the pressure is low, the escape speed can be fast. In the extreme case of complete burial, the correctness of the escape direction is verified by the pressure sensor. That is, during a correct escape, the pressure should gradually decrease.

[0110] Because the robot is not horizontal in most cases when buried, the pressure sensor measures the positive pressure on the installation surface. Therefore, the pressure needs to be converted to a horizontal model to determine the buried depth of the robot's four corner points, such as Figure 12 As shown, for example, h1 is the buried depth of pressure sensor B3, and h2 is the buried depth of pressure sensor B2. The pressure change is combined with the robot's motion description method, where ψ is the yaw angle, θ is the pitch angle, and φ is the roll angle, and the calculation is performed using the Euler angle ZYX model, where:

[0111] Rotation matrix around the X axis (roll angle φ):

[0112]

[0113] Rotation matrix around the Y axis (pitch angle θ)

[0114]

[0115] The rotation matrix around the Z axis (yaw angle ψ):

[0116]

[0117] The final rotation matrix R is:

[0118]

[0119] The setting of the escape speed is related to the pressure of the sensor. If the pressure is too high, the escape speed will be lower. If the pressure is too low, the escape speed can be higher. The relationship is:

[0120]

[0121] Among them, v_target represents the target moving speed of the granary leveling robot, λ is the adjustment coefficient, v0 is the preset standard escape moving speed, P is the current equivalent pressure of the granary leveling robot, P δ is the trigger pressure threshold of the pressure sensor, P max is the maximum pressure value; F T is the driving force of the robot, σ is the driving force coefficient of the robot, and the recommended value is [0.5~1.0]; R is the rotation matrix of the granary leveling robot, P B1 、P B2 、P B3 and P B4 These are the pressure values ​​collected by four pressure sensors.

[0122] The speed regulation during the escape process adopts discrete PID control strategy, and its control method is as follows:

[0123] Δu(k)=K p [e(k)-e(k-1)]+K i e(k)+K d [e(k)-2e(k-1)+e(k-2)];

[0124] Wherein, e(k) represents the error at the current moment k, e(k) = v_target-v, v represents the current moving speed of the granary leveling robot, and v_target represents the current target moving speed of the granary leveling robot; K p is the proportionality coefficient, K i is the integral coefficient, K d is the differential coefficient, Δu(k) represents the control increment of k at the current moment, u(k)=u(k-1)+Δu(k), u(k) represents the control output of k at the current moment, u(k-1) represents the output at k-1 moment; e(k-1) and e(k-2) represent the error at k-1 moment and the error at k-2 moment, respectively.

[0125] The robot determines the corresponding escape direction and escape speed parameters according to different buried states and executes the automatic escape program. The program drives the robot's walking mechanism to move. At the same time, it detects whether the escape is completed during the escape process. If the robot completes the autonomous escape, the autonomous escape program ends. If not, a new round of state detection is carried out. The robot will generate a new escape strategy according to the new buried state until the robot completes the autonomous escape task.

[0126] After the robot has escaped from the burial, it will send a message of successful escape through the signal sensor and continue to perform the previous grain leveling task according to the new granary status and environment.

[0127] In this embodiment, a currently commonly used granary grain-leveling robot with a spiral wheel is adopted as the control object. In practical applications, the control system and control method for the granary grain-leveling robot to autonomously escape when buried provided by the present invention can be applied to many different types of granary grain-leveling robots.

[0128] The present invention does not rely solely on pressure sensors to determine the robot's buried state. Instead, it uses a combination of photoelectric sensors and pressure sensor detection results, as well as the photoelectric sensor triggering sequence, to determine the robot's buried state. The photoelectric sensor triggering sequence and triggering state simultaneously detect the instantaneous process and final state of the grain-leveling robot being buried by the avalanche-like flow of grain during its climbing operation, providing a more accurate and reliable judgment of the robot's trapped state. The grain-leveling robot will not trigger its escape mode simply due to obstruction or pressure detected by the pressure sensor.

[0129] The autonomous escape control method for a grain-leveling robot provided by the present invention can simultaneously control both the escape direction and the escape speed, enabling efficient and reliable escape. The existing fixed escape speed is not reasonable, as the robot can be buried at varying depths, and escape is a dynamic process. The order in which the photoelectric sensors of the grain-leveling robot are triggered when buried by grain is used to calculate the robot's escape direction. Position and posture sensors and pressure sensors are used to verify and correct the robot's escape direction, and the escape speed is adaptively set based on the corresponding sensor values, thereby improving the robot's escape success rate and efficiency.

[0130] The autonomous robot escape control method provided by this invention can handle a variety of buried situations. During grain-leveling operations, a robot can be buried from all directions, including front-to-back, left-to-right, and even at an angle, potentially causing the robot to roll up and down beneath the grain surface. If this happens, the robot utilizes posture and pressure sensors as primary sensors, and then determines an escape method based on the "obstruction-pressure" model of the entrapment.

[0131] The present invention is simple and easy to implement in upgrading an ordinary grain-leveling robot to add an autonomous escape plan when buried. It mainly involves adding photoelectric sensors and pressure sensors. It increases the functions of the grain-leveling robot without increasing the cost too much, has a good cost-effectiveness ratio, and the transformation plan has a low cost and is easy to promote.

[0132] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the description and implementation methods. They can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to the specific details and illustrations shown and described herein.

Claims

1. A control system for a grain silo leveling robot to escape from a buried situation, characterized in that: include: Four groups of photoelectric sensors are respectively arranged on the four sides of the shell of the grain silo leveling robot; Each group of photoelectric sensors includes two photoelectric sensors, and the two photoelectric sensors are respectively arranged close to the two ends of the side surface, and the emitting ends of the photoelectric sensors are arranged toward the outside of the side surface; The attitude sensor is installed on the chassis of the grain silo leveling robot and is used to detect the yaw angle, pitch angle and roll angle of the grain silo leveling robot; Four pressure sensors are mounted on the top of the housing of the grain silo leveling robot and are respectively arranged near the four corners of the top of the grain silo leveling robot; A main control chip is electrically connected to the photoelectric sensor, the posture sensor and the pressure sensor respectively; the main control chip determines the buried state and buried process information of the granary grain-leveling robot according to the status information of the photoelectric sensor, the posture sensor and the pressure sensor, and controls the movement of the granary grain-leveling robot according to the buried state and buried process information, so as to enable the granary grain-leveling robot to escape from trouble.

2. A method for controlling a grain silo leveling robot to escape from a buried state, characterized in that: The autonomous escape control system for the buried grain-leveling robot in a granary according to claim 1 comprises the following steps: Step 1: Obtain the occlusion information of the photoelectric sensor, the pressure information collected by the pressure sensor, and the yaw angle, pitch angle, and roll angle information of the grain silo leveling robot collected by the attitude sensor; Step 2: determining the buried state and buried process of the robot according to the occlusion information and the pressure information; The buried state includes: whether the grain leveling robot is buried, and the buried position; the buried process includes: the blocking triggering sequence of the photoelectric sensor; Step 3: Control the moving direction of the grain-leveling robot in the granary according to the buried state and buried process of the robot; Among them, if only part of the granary grain leveling robot is buried, the granary grain leveling robot is controlled to move in the opposite direction of the buried position; if the granary grain leveling robot is completely buried, the granary grain leveling robot is controlled to move in the direction of the last triggered photoelectric sensor.

3. The autonomous escape control method for a buried grain leveling robot in a granary according to claim 2 is characterized in that: In the step 2, if a photoelectric sensor is blocked and triggered, and a pressure sensor is triggered, it is determined that the grain silo leveling robot is buried.

4. The autonomous escape control method for a buried grain-leveling robot in a granary according to claim 3 is characterized in that: In the step three, it also includes: using a PID strategy to control the moving speed of the grain leveling robot in the granary according to the following formula; Δu(k)=K p [e(k)-e(k-1)]+K i e(k)+K d [e(k)-2e(k-1)+e(k-2)]; Wherein, e(k) represents the error at the current moment k, e(k) = v_target-v, v represents the current moving speed of the granary leveling robot, and v_target represents the current target moving speed of the granary leveling robot; K p is the proportionality coefficient, K i is the integral coefficient, K d is the differential coefficient, Δu(k) represents the control increment of k at the current moment, u(k)=u(k-1)+Δu(k), u(k) represents the control output of k at the current moment, u(k-1) represents the output at k-1 moment; e(k-1) and e(k-2) represent the error at k-1 moment and the error at k-2 moment, respectively.

5. The autonomous escape control method for a buried grain-leveling robot in a granary according to claim 4 is characterized in that: Determine the target moving speed v_target of the grain silo leveling robot based on the pressure information collected by the pressure sensor: Among them, λ is the adjustment coefficient, v0 is the preset standard escape speed, P is the current equivalent pressure of the granary leveling robot, P δ is the trigger pressure threshold of the pressure sensor, P max is the maximum pressure value.

6. The autonomous escape control method for a buried grain-leveling robot in a granary according to claim 5, characterized in that: The calculation formula of the equivalent pressure is: Among them, R is the rotation matrix of the grain leveling robot in the granary, P B1 、P B2 、P B3 and P B4 These are the pressure values ​​collected by four pressure sensors.

7. The autonomous escape control method for a buried grain leveling robot in a granary according to claim 6, characterized in that: The rotation matrix is: Among them, ψ is the yaw angle of the granary grain leveling robot, θ is the pitch angle of the granary grain leveling robot, and φ is the roll angle of the granary grain leveling robot.

8. The method for controlling the autonomous escape of a buried grain-leveling robot in a granary according to claim 6 or 7, characterized in that: The adjustment coefficient is set as: Among them, F T is the driving force of the granary grain leveling robot, σ is the driving force coefficient, and P is the equivalent pressure of the granary grain leveling robot.

9. The method for controlling the autonomous escape of a buried grain-leveling robot in a granary according to claim 8, characterized in that: Also includes: The positioning module detects the position information of the grain leveling robot in the granary in real time and sends it to the host computer.

Images