A grain-leveling robot for granary and its automatic height control method

Abstract

The present application discloses a grain-leveling robot for a granary and an automatic height control method thereof, and relates to the technical field of grain-leveling operation in a granary. When the granary grain-leveling robot is working, the robot working mode is determined by an external remote control signal. In the automatic working mode, the robot posture raw data is collected by a posture sensor, and the robot posture information is obtained by solving the main control chip, and the spiral drive component is controlled to automatically and quickly climb the slope according to the robot posture information and preset climbing information, and the Archimedean spiral is generated based on the data generated during the robot climbing process, and the spiral drive component is controlled to perform grain-leveling operation. The above-mentioned grain-leveling robot for a granary provided by the present application can improve the automation and intelligence level of the robot without increasing the cost of the robot. Compared with the current granary robots that are mainly operated by remote control, it solves the problems of low efficiency and complex operation of grain-leveling operation.

Description

Technical Field

[0001] The present application relates to the technical field of grain leveling operations in granaries, and in particular to a grain leveling robot in a granary and an automatic height control method thereof. Background Art

[0002] During grain storage operations, several uneven grain piles will be formed when grain is put into the warehouse. Uneven grain piles may cause abnormal local temperature and humidity, resulting in mold or insect problems, so it is necessary to level the grain piles. Using grain warehouse robots to replace manual grain leveling operations can greatly improve the efficiency of grain leveling operations and reduce the health hazards that grain leveling operations may bring to workers. In the special operating environment of the grain warehouse, it is difficult to use the existing intelligent sensing technology to achieve autonomous driving and autonomous operation. Therefore, most of the current grain warehouse robots are mainly remote-controlled, which has the problems of low efficiency and complex operation. Summary of the invention

[0003] The purpose of this application is to provide a grain-leveling robot for a granary and an automatic height control method thereof, which can efficiently and simply complete the automated grain-leveling operation.

[0004] To achieve the above objectives, this application provides the following solutions:

[0005] In a first aspect, the present application provides a grain-leveling robot for a granary, comprising: a robot chassis, a robot housing, a signal receiver, a posture sensor, a main control chip, and spiral drive components respectively fixed on both sides of the robot chassis.

[0006] The signal receiver, main control chip and attitude sensor are installed on the robot chassis, and the robot shell covers the robot chassis to protect the signal receiver, main control chip and attitude sensor, thereby playing a mechanical protection role; the robot chassis is in contact with the grain pile, and the robot chassis is used to flatten the surface protrusions of the grain pile; the signal receiver is used to receive external remote control signals and send them to the main control chip; the attitude sensor is used to collect the original data of the robot attitude and send them to the main control chip; the main control chip is used to determine the robot working mode according to the external remote control signal; when the robot working mode is manual control mode, the main control chip is used to control the spiral drive component to perform corresponding actions according to the external remote control signal; when the robot working mode is automatic working mode, the main control chip is used to obtain the robot attitude information according to the original data of the robot attitude, control the spiral drive component to automatically and quickly climb according to the robot attitude information and preset climbing information, and generate an Archimedean spiral based on the data generated during the robot climbing process, and control the spiral drive component to perform grain leveling operations.

[0007] Optionally, the robot posture raw data includes angular velocity around three axes and acceleration in three axes; the robot posture information includes the heading angle, pitch angle and roll angle of the robot; and obtaining the robot posture information by solving the robot posture raw data specifically includes the following steps:

[0008] Initialize the quaternion and get the initial quaternion.

[0009] According to the initial quaternion, the speed in three axes is calculated.

[0010] The attitude error is calculated based on the three-axis velocities and three-axis accelerations.

[0011] Based on the double-loop PID controller, the acceleration in the three-axis directions is corrected to obtain the corrected acceleration in the three-axis directions.

[0012] According to the modified three-axis acceleration, the initial quaternion is integrated and normalized to obtain the updated quaternion.

[0013] According to the updated quaternion, the heading angle, pitch angle and roll angle of the robot are calculated using inverse trigonometric functions.

[0014] Optionally, the heading angle of the robot can be obtained by solving the following formula:

[0015] .

[0016] The pitch angle of the robot is calculated according to the following formula:

[0017] .

[0018] The robot's roll angle is calculated according to the following formula:

[0019] .

[0020] In the formula, yaw is the heading angle of the robot, pitch is the pitch angle of the robot, roll is the robot's rolling angle, q 0', q 1', q 2', q 3' is the updated quaternion.

[0021] Optionally, the preset climbing information includes: an offset angle threshold; according to the robot posture information and the preset climbing information, controlling the spiral drive component to automatically and quickly climb the slope specifically includes the following steps:

[0022] The robot's offset angle is determined based on the robot's driving direction and the angle between the bottom of the slope and the top of the slope.

[0023] If the absolute value of the offset angle of the robot is greater than the offset angle threshold, the spiral drive component of the robot is controlled according to the offset angle of the robot and the offset angle threshold until the absolute value of the offset angle of the robot is less than the offset angle threshold.

[0024] If the absolute value of the robot's offset angle is less than the offset angle threshold, the robot's orientation is determined based on the robot's pitch angle.

[0025] When the robot is facing the top of the slope, the spiral drive component is controlled in the positive direction to make the robot move toward the top of the slope.

[0026] When the robot is facing the bottom of the slope, the spiral drive component is controlled in the reverse direction to make the robot move back toward the top of the slope.

[0027] Optionally, the preset climbing information further includes: an uphill pitch angle threshold and a downhill pitch angle threshold; determining the orientation of the robot according to the pitch angle of the robot, specifically including the following steps:

[0028] If the pitch angle of the robot is greater than the uphill pitch angle threshold, it is determined that the robot is facing the top of the slope.

[0029] If the pitch angle of the robot is less than the downhill pitch angle threshold, it is determined that the robot is facing the bottom of the slope.

[0030] Optionally, when the screw driving component of the robot is controlled according to the offset angle of the robot and the offset angle threshold, the offset angle of the robot is adjusted based on a PID algorithm.

[0031] Adjust the robot's offset angle according to the following formula:

[0032] .

[0033] in, S is the offset angle of the robot, Kp is the proportional adjustment parameter, Ki is the integral adjustment parameter, Kd is the differential adjustment parameter, e ( k ) is the offset angle deviation of the robot adjusted this time, e ( k -1) is the offset angle deviation of the robot last adjusted. It is the accumulated value of the robot's offset angle deviation.

[0034] Optionally, an Archimedean spiral is generated based on data generated during the robot's climbing process, and a spiral drive component is controlled to perform a grain leveling operation, specifically including the following steps:

[0035] Determine the robot's climbing distance based on the robot's climbing time and climbing speed.

[0036] The height of the grain pile is determined based on the robot's climbing distance and the sine value of the robot's pitch angle.

[0037] The outer radius of the Archimedean spiral is determined according to the robot's climbing distance and the cosine value of the robot's pitch angle.

[0038] The spiral pitch of the Archimedean spiral is determined according to the height of the grain pile and the climbing distance of the robot.

[0039] An Archimedean spiral is generated based on the outer circle radius of the Archimedean spiral and the spiral pitch of the Archimedean spiral.

[0040] Control the spiral drive component of the robot to perform grain leveling operations along the Archimedean spiral.

[0041] Optionally, the robot's climbing distance is determined according to the following formula:

[0042] .

[0043] in, S is the climbing distance of the robot, v is the climbing speed of the robot, t is the climbing time of the robot.

[0044] Determine the grain pile height according to the following formula:

[0045] .

[0046] in, H is the grain pile height, pitch is the pitch angle of the robot.

[0047] The outer radius of the Archimedean spiral is determined according to the following formula:

[0048] .

[0049] in, Rs is the outer radius of the Archimedean spiral.

[0050] The spiral pitch of the Archimedean spiral is determined according to the following formula:

[0051] .

[0052] in, a is the spiral pitch of the Archimedean spiral, k is the coefficient, k >0.

[0053] Optionally, the granary grain leveling robot also includes: an OLED display module; the OLED display module is used to display the current working status of the granary grain leveling robot.

[0054] In a second aspect, the present application provides a method for automatically controlling the height of a grain leveling robot in a granary, comprising the following steps:

[0055] Receive external remote control signals and determine the robot working mode based on the external remote control signals.

[0056] When the robot working mode is the manual control mode, the spiral drive component is controlled to perform corresponding actions according to the external remote control signal.

[0057] When the robot is in automatic working mode, the original data of the robot's posture is collected and sent to the main control chip.

[0058] The robot posture information is obtained by solving the original data of the robot posture.

[0059] According to the robot posture information and preset climbing information, the spiral drive component is controlled to perform automatic and rapid climbing.

[0060] An Archimedean spiral is generated based on the data generated during the robot's climbing process, and the spiral drive components are controlled to perform grain leveling operations.

[0061] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0062] The present application provides a grain-leveling robot for a granary and a method for automatically controlling the leveling of grains in the granary. In the grain-leveling robot for a granary, the outer shell of the robot covers the chassis of the robot, and plays a mechanical protective role for the internal signal receiver, main control chip and attitude sensor. The plane of the robot chassis is in direct contact with the grain pile, and can push down the surface protrusions of the grain pile when moving. The working mode of the robot is determined by an external remote control signal. When the working mode of the robot is a manual control mode, the spiral drive component is controlled to perform corresponding actions according to the external remote control signal. When the working mode of the robot is an automatic working mode, the original posture data of the robot is collected by the attitude sensor, and then the main control chip calculates the robot posture information according to the original posture data of the robot, and according to the robot posture information and preset climbing information, the spiral drive component is controlled to automatically and quickly climb the slope, and an Archimedean spiral is generated based on the data generated during the robot climbing process, and the spiral drive component is controlled to perform grain-leveling operations. The above-mentioned grain silo grain leveling robot provided in the present application only needs to use the posture sensor installed on itself to obtain posture information, and can use the self-developed algorithm to automatically and quickly climb to the top of the grain pile, and perform grain leveling operations according to the generated Archimedean spiral. The automation and intelligence level of the robot can be improved without increasing the cost of the robot, and the problem of low efficiency and complex operation of grain leveling operations in current grain silo robots, which are mainly remote-controlled operations, is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0064] Figure 1 A schematic diagram of a grain-leveling robot in a granary provided in one embodiment of the present application.

[0065] Figure 2 A grain-leveling robot for a grain silo provided in one embodiment of the present application is a front view of the relationship between the robot and the grain pile when the pitch angle is greater than an uphill pitch angle threshold and the absolute value of the offset angle is less than the offset angle threshold.

[0066] Figure 3 A right view of the relationship between a grain-leveling robot in a granary provided in one embodiment of the present application and the robot and the grain pile when the pitch angle is greater than an uphill pitch angle threshold and the absolute value of the offset angle is less than the offset angle threshold.

[0067] Figure 4A grain-leveling robot for a grain silo provided in one embodiment of the present application is a front view of the relationship between the robot and the grain pile when the pitch angle is less than a downhill pitch angle threshold and the absolute value of the offset angle is less than the offset angle threshold.

[0068] Figure 5 A right view of the relationship between a grain-leveling robot in a granary provided in one embodiment of the present application and the robot and the grain pile when the pitch angle is less than a downhill pitch angle threshold and the absolute value of the offset angle is less than the offset angle threshold.

[0069] Figure 6 A schematic diagram of control feedback of a grain-leveling robot in a granary when adjusting the offset angle of the robot based on a PID algorithm is provided in one embodiment of the present application.

[0070] Figure 7 A schematic diagram of a grain-leveling robot in a granary performing grain-leveling trajectory planning based on an Archimedean spiral is provided in accordance with an embodiment of the present application.

[0071] Figure 8 A flowchart of an automatic up and down control method of a grain leveling robot in a granary provided in one embodiment of the present application. DETAILED DESCRIPTION

[0072] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0073] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0074] In an exemplary embodiment, Figure 1 As shown, a grain-leveling robot for a granary is provided, including: a robot chassis, a robot shell, a signal receiver, a posture sensor, a main control chip, and spiral drive components respectively fixed on both sides of the robot chassis. Specifically, the spiral drive components on both sides are a motor on each side connected to the spiral wheel through a transmission shaft to drive the spiral wheel to move. The direction and speed of the robot can be controlled by controlling the rotation direction and speed of the motor. It can be understood that the robot chassis is the installation base for the various components of the robot. In addition to the above necessary components, energy supply components such as batteries should also be arranged.

[0075] The signal receiver, main control chip and attitude sensor are installed on the robot chassis, and the robot shell covers the robot chassis to protect the signal receiver, main control chip and attitude sensor, thereby playing a mechanical protection role; the robot chassis is in contact with the grain pile, and the robot chassis is used to flatten the surface protrusions of the grain pile; the signal receiver is used to receive external remote control signals and send them to the main control chip; the attitude sensor is used to collect the original data of the robot attitude and send them to the main control chip; the main control chip is used to determine the robot working mode according to the external remote control signal; when the robot working mode is manual control mode, the main control chip is used to control the spiral drive component to perform corresponding actions according to the external remote control signal; when the robot working mode is automatic working mode, the main control chip is used to obtain the robot attitude information according to the original data of the robot attitude, control the spiral drive component to automatically and quickly climb according to the robot attitude information and preset climbing information, and generate an Archimedean spiral based on the data generated during the robot climbing process, and control the spiral drive component to perform grain leveling operations.

[0076] Specifically in this embodiment, the robot posture raw data includes the angular velocity around three axes and the acceleration in three axis directions; the robot posture information includes the heading angle, pitch angle and roll angle of the robot; and the robot posture information is obtained by solving the robot posture raw data, which specifically includes the following steps:

[0077] Initialize the quaternion to get the initial quaternion. To represent the rotation information of a three-dimensional object, it is necessary to add the third-dimensional vector information on the basis of the traditional description of the rotation information of the two-dimensional plane object. is a one-dimensional vector, is a three-dimensional vector, θ is the angle with the unit plane, then the following equation exists:

[0078] .

[0079] .

[0080] After simplification, we get: , in this formula, q 0, q 1, q 2, q 3 is a quaternion, all real numbers, are mutually orthogonal vectors.

[0081] According to the initial quaternion, the three-axis direction speed is calculated. Assuming the obtained angular speed around the three axes is gx, gy, gz, and the three-axis direction acceleration is ax, ay, az, the three-axis direction speed Vx, Vy, Vz can be calculated according to the following formula:

[0082] Vx=2×( q 1× q3- q 0× q 2).

[0083] Vy=2×( q 0× q 1+ q 2× q 3).

[0084] Vz= q 0× q 0- q 1× q 1- q 2× q 2+ q 3× q 3.

[0085] The attitude error is calculated based on the three-axis velocity and three-axis acceleration. Specifically, the attitude error is the cross product of the sensor direction and the reference direction, which can be expressed by the following formula:

[0086] ex=(ay×Vz-az×Vy).

[0087] ey=(az×Vx-ax×Vz).

[0088] ez=(ax×Vy-ay×Vx).

[0089] Based on the dual-loop PID controller, the three-axis acceleration is corrected to obtain the corrected three-axis acceleration. In this embodiment, by adjusting the PID speed loop parameters, the proportional constant Kp and the integral constant Ki, the speed at which the accelerometer corrects the gyroscope integral attitude can be controlled, as shown in the following formula:

[0090] ex·Int=ex·Int+ex×Ki.

[0091] ey·Int=ey·Int+ey×Ki.

[0092] ez·Int=ez·Int+ez×Ki.

[0093] Then adjust the three-axis acceleration measured by the gyroscope using the following formula:

[0094] gx=gx+Kp×ex+ex·Int.

[0095] gy=gy+Kp×ey+ey·Int.

[0096] gz=gz+Kp×ez+ez·Int.

[0097] According to the modified three-axis acceleration, the initial quaternion is integrated and normalized to obtain an updated quaternion. In this embodiment, the quaternion is integrated and normalized by the following formula:

[0098] q 0= q 0+(- q 1×gx- q 2×gy- q 3×gz)×T / 2.

[0099] q 1= q 1+( q 0×gx+ q 2×gz- q 3×gy)×T / 2.

[0100] q 2= q 2+( q 0×gy- q 1×gz+ q 3×gx)×T / 2.

[0101] q 3= q 3+( q 0×gz+ q 1×gy- q 2×gx)×T / 2.

[0102] Where T is the measurement period, gx, gy, and gz are the three-axis angular velocities.

[0103] Then, the variable norm is taken by the following formula and the quaternion is normalized:

[0104] norm=sqrt( q 0× q 0+ q 1× q 1+ q 2× q 2+ q 3× q 3).

[0105] q 0= q 0 / norm.

[0106] q 1= q 1 / norm.

[0107] q 2= q 2 / norm.

[0108] q 3= q 3 / norm.

[0109] According to the updated quaternion, the heading angle, pitch angle and roll angle of the robot are calculated using inverse trigonometric functions. In this embodiment, the heading angle, pitch angle and roll angle of the robot are calculated according to the following formula:

[0110] .

[0111] .

[0112] .

[0113] In the formula, yaw is the heading angle of the robot, pitch is the pitch angle of the robot, roll is the robot's rolling angle, q 0', q 1', q 2', q 3' is the updated quaternion.

[0114] In an exemplary embodiment of the present application, the preset climbing information includes: an offset angle threshold; according to the robot posture information and the preset climbing information, controlling the spiral drive component to automatically and quickly climb the slope specifically includes the following steps:

[0115] The robot's offset angle is determined based on the robot's driving direction and the angle between the bottom of the slope and the top of the slope.

[0116] If the absolute value of the offset angle of the robot is greater than the offset angle threshold, the spiral drive component of the robot is controlled according to the offset angle of the robot and the offset angle threshold until the absolute value of the offset angle of the robot is less than the offset angle threshold.

[0117] If the absolute value of the robot's offset angle is greater than the offset angle threshold, there are two situations: the front of the vehicle is biased to the right or left relative to the vertical line from the top of the slope to the ground. If it is biased to the right, the vehicle body rotates to the left on the spot; if it is biased to the left, the vehicle body rotates to the right on the spot until the driving direction and the direction pointing to the top of the slope are less than the offset angle threshold.

[0118] If the absolute value of the robot's offset angle is less than the offset angle threshold, the robot's orientation is determined based on the robot's pitch angle.

[0119] When the robot is facing the top of the slope, the spiral drive component is controlled in the positive direction to make the robot move toward the top of the slope.

[0120] When the robot is facing the bottom of the slope, the spiral drive component is controlled in the reverse direction to make the robot move back toward the top of the slope.

[0121] In another exemplary embodiment of the present application, the preset climbing information further includes: an uphill pitch angle threshold and a downhill pitch angle threshold; determining the orientation of the robot according to the pitch angle of the robot, specifically including the following steps:

[0122] If the robot's pitch angle is greater than the uphill pitch angle threshold, the robot is determined to be facing the top of the slope. Figure 2~Figure 3 At this time, the spiral drive component is positively controlled to make the robot move toward the top of the slope.

[0123] If the robot's pitch angle is less than the downhill pitch angle threshold, the robot is determined to be facing the bottom of the slope. Figure 4~Figure 5 At this time, the spiral drive component is controlled in the reverse direction to make the robot retreat toward the top of the slope.

[0124] In an exemplary embodiment of the present application, considering that the robot may slip due to its own speed and the looseness of the grain pile when traveling, the ideal state should reduce the swing and accurately find the correct driving direction, so the PID algorithm is used to adjust the driving angle, that is, when controlling the spiral drive component of the robot according to the offset angle and the offset angle threshold of the robot, the PID algorithm is used. Figure 6 In the manner shown, the offset angle of the robot is adjusted based on the PID algorithm.

[0125] Specifically, the robot's offset angle is adjusted according to the following formula:

[0126] .

[0127] in, S is the offset angle of the robot, Kp is the proportional adjustment parameter, Ki is the integral adjustment parameter, Kd is the differential adjustment parameter, e ( k ) is the offset angle deviation of the robot adjusted this time, e ( k -1) is the offset angle deviation of the robot last adjusted. It is the accumulated value of the robot's offset angle deviation.

[0128] In an exemplary embodiment of the present application, an Archimedean spiral is generated based on data generated during the robot climbing process, and a spiral drive component is controlled to perform a grain leveling operation, specifically including the following steps:

[0129] The climbing distance of the robot is determined according to the climbing time and climbing speed of the robot. In this embodiment, the climbing distance of the robot is determined according to the following formula:

[0130] .

[0131] in, S is the climbing distance of the robot, v is the climbing speed of the robot, t is the climbing time of the robot.

[0132] The height of the grain pile is determined according to the climbing distance of the robot and the sine value of the pitch angle of the robot. In this embodiment, the height of the grain pile is determined according to the following formula:

[0133] .

[0134] in, H is the grain pile height, pitch is the pitch angle of the robot.

[0135] The outer radius of the Archimedean spiral is determined according to the climbing distance of the robot and the cosine value of the pitch angle of the robot. In this embodiment, the outer radius of the Archimedean spiral is determined according to the following formula:

[0136] .

[0137] in, Rs is the outer radius of the Archimedean spiral.

[0138] The spiral pitch of the Archimedean spiral is determined according to the height of the grain pile and the climbing distance of the robot. In this embodiment, the spiral pitch of the Archimedean spiral is determined according to the following formula:

[0139] .

[0140] in, a is the spiral pitch of the Archimedean spiral, k is the coefficient, k > 0. This formula controls the spiral pitch so that it is inversely proportional to the sine value of the grain surface slope, that is, the steeper the slope, the smaller the spiral pitch.

[0141] Based on the outer radius of the Archimedean spiral and the spiral pitch of the Archimedean spiral, an Archimedean spiral is generated. In this embodiment, the generated Archimedean spiral is as follows: Figure 7 shown.

[0142] The spiral drive component of the robot is controlled to perform grain leveling operation along the Archimedean spiral. As an example, when the Archimedean spiral is used for grain leveling operation, x( θ ), y ( θ ) is the calculation formula for converting polar coordinates to Cartesian coordinates, which represents the coordinates in the plane. The calculation formula is as follows:

[0143] .

[0144] .

[0145] in, θ Indicates the angle, a is the spiral pitch of the Archimedean spiral.

[0146] In another exemplary embodiment of the present application, the granary grain leveling robot further includes: an OLED display module; the OLED display module is used to display the current working status of the granary grain leveling robot.

[0147] In another exemplary embodiment of the present application, the above-mentioned grain silo leveling robot applies the FreeRTOS framework to better allocate the computing resources of the equipment. Four tasks are created, namely: chassis drive task, OLED parameter display task, posture detection task and serial communication task. The stack size of the posture detection task is 512 bytes, and the task priority is 3, which is higher than the chassis drive task and serial communication task.

[0148] Start the grain silo leveling robot, and it will go through a series of initialization processes. Initialize the interrupt priority to facilitate the FreeRTOS operating system to assign tasks; initialize ADC to read the robot's power and control mode; initialize IIC to achieve subsequent communication with OLED; after initialization, it will wait for external remote control signals. When the aircraft model remote control pushes the joystick lightly, an external remote control signal is sent. In addition, other communication control modes can be entered, such as handles, keyboards, ROS, and Bluetooth communications. After determining to use the aircraft model remote control, the grain silo leveling robot will wait for instructions from the aircraft model remote control. When the aircraft model remote control sends an external remote control signal for manual control, the robot will execute the driving command according to the operator's command to the robot. After receiving an external remote control signal for automatic driving, it will automatically execute the driving command suitable for itself according to the posture information collected by its own posture sensor.

[0149] When the automatic climbing algorithm is turned on, the robot will first determine the robot's offset angle based on the robot's driving direction and the angle between the bottom of the slope and the top of the slope. If the absolute value of the offset angle is greater than δ° (the user can customize the ideal threshold of the angle between the robot's driving direction and the bottom of the slope and the top of the slope based on experience in advance), it means that the robot's own posture, that is, the straight line where the driving direction is located is not parallel to the vertical line along the slope to the ground, and the angle is large. At this time, the robot needs to adjust its own posture on the spot and rotate to a suitable angle, that is, the straight line where the driving direction is located is parallel to the vertical line of the slope. Driving straight on this straight line, it either climbs to the top of the slope or climbs to the bottom of the slope. At this time, it is necessary to judge its own pitch angle. If the angle is greater than the uphill pitch angle threshold, that is, the robot's front is facing upward, then it can reach the top of the slope by walking directly upward; if the robot's pitch angle is less than the downhill pitch angle threshold, that is, the front is facing downward, then it needs to walk backwards to reach the top of the slope. On the way to the top of the slope, it will continuously collect its own posture information to ensure that it keeps driving to the top of the slope under the premise that the absolute value of the offset angle is less than δ°. In this way, the grain silo leveling robot can quickly climb to the top of the grain pile and quickly start the grain leveling operation. Figure 7 The robot can perform grain leveling operations according to a given driving trajectory. In this embodiment, the robot drives in circles according to the Archimedean spiral, so that the grain pile can be leveled.

[0150] Based on the same inventive concept as the above robot embodiment, the present application embodiment also provides an automatic height control method for a grain leveling robot in a granary, such as Figure 8 As shown in the flowchart, the automatic high and low control method comprises the following steps:

[0151] Receive external remote control signals and determine the robot's working mode based on the external remote control signals. After the grain silo leveling robot is started, it waits for the aircraft model remote control to send the remote control signal to its signal receiver. The signal receiver is connected to the main control chip and sends the external remote control signal in PWM form to the main control chip, which performs logic processing.

[0152] When the robot working mode is the manual control mode, the spiral drive component is controlled to perform corresponding actions according to the external remote control signal.

[0153] When the robot is in automatic working mode, the original data of the robot's posture is collected and sent to the main control chip.

[0154] The robot posture information is obtained by solving the original data of the robot posture.

[0155] According to the robot posture information and preset climbing information, the spiral drive component is controlled to perform automatic and rapid climbing.

[0156] An Archimedean spiral is generated based on the data generated during the robot's climbing process, and the spiral drive components are controlled to perform grain leveling operations.

[0157] The above-mentioned grain silo leveling robot and its automatic height control method provided in the present application only need to use the posture sensor installed on itself to obtain posture information, and can use the self-developed algorithm to automatically and quickly climb to the top of the grain pile, and perform grain leveling operations according to the generated Archimedean spiral. The automation and intelligence level of the robot can be improved without increasing the cost of the robot, and solves the problems of low efficiency and complex operation of grain leveling operations of the current grain silo robots that are mainly operated by remote control.

[0158] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0159] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A grain leveling robot in a granary, characterized in that: include: A robot chassis, a robot shell, a signal receiver, a posture sensor, a main control chip, and spiral drive components respectively fixed on both sides of the robot chassis; The signal receiver, the main control chip and the attitude sensor are installed on the robot chassis, and the robot shell covers the robot chassis to protect the signal receiver, the main control chip and the attitude sensor, thereby playing a mechanical protection role; the robot chassis is in contact with the grain pile, and the robot chassis is used to flatten the surface protrusions of the grain pile; the signal receiver is used to receive an external remote control signal and send it to the main control chip; the attitude sensor is used to collect the original data of the robot attitude and send it to the main control chip; the main control chip is used to determine the robot working mode according to the external remote control signal; when the robot working mode is the manual control mode, the main control chip is used to control the spiral drive component to perform corresponding actions according to the external remote control signal; when the robot working mode is the automatic working mode, the main control chip is used to obtain the robot attitude information according to the original data of the robot attitude, control the spiral drive component to automatically and quickly climb according to the robot attitude information and preset climbing information, and generate an Archimedean spiral based on the data generated during the robot climbing process, and control the spiral drive component to perform grain leveling operations; The Archimedean spiral is generated based on the data generated during the robot climbing process, and the spiral drive component is controlled to perform a grain leveling operation, specifically including: Determine the robot's climbing distance based on the robot's climbing time and climbing speed; The height of the grain pile is determined according to the robot's climbing distance and the sine value of the robot's pitch angle; Determine the outer radius of the Archimedean spiral according to the robot's climbing distance and the cosine value of the robot's pitch angle; Determine the spiral pitch of the Archimedean spiral according to the height of the grain pile and the climbing distance of the robot; Generate an Archimedean spiral based on the outer radius of the Archimedean spiral and the spiral pitch of the Archimedean spiral; The spiral driving component of the robot is controlled to perform grain leveling operation along the Archimedean spiral line.

2. The grain leveling robot in a granary according to claim 1, characterized in that: The robot posture raw data includes angular velocity around three axes and acceleration in three axes; the robot posture information includes the heading angle, pitch angle and roll angle of the robot; the robot posture information is obtained by solving the robot posture raw data, specifically including: Initialize the quaternion and obtain the initial quaternion; According to the initial quaternion, the speeds in three axes are calculated; Calculating the attitude error according to the three-axis direction velocities and the three-axis direction accelerations; Based on a dual-loop PID controller, the three-axis acceleration is corrected to obtain a corrected three-axis acceleration; Integrate and normalize the initial quaternion according to the corrected three-axis acceleration to obtain an updated quaternion; According to the updated quaternion, the heading angle, pitch angle and roll angle of the robot are obtained by using inverse trigonometric functions.

3. The grain leveling robot in a granary according to claim 2, characterized in that: The robot's heading angle is calculated according to the following formula: ； The pitch angle of the robot is calculated according to the following formula: ； The robot's roll angle is calculated according to the following formula: ； In the formula, yaw is the heading angle of the robot, pitch is the pitch angle of the robot, roll is the robot's rolling angle, q 0', q 1', q 2', q 3' is the updated quaternion.

4. The grain leveling robot in a granary according to claim 1, characterized in that: The preset climbing information includes: an offset angle threshold; according to the robot posture information and the preset climbing information, controlling the spiral drive component to automatically and quickly climb the slope, specifically includes the following steps: Determine the robot's offset angle based on the robot's travel direction and the angle between the bottom of the slope and the top of the slope; If the absolute value of the offset angle of the robot is greater than the offset angle threshold, the spiral drive component of the robot is controlled according to the offset angle of the robot and the offset angle threshold until the absolute value of the offset angle of the robot is less than the offset angle threshold; If the absolute value of the offset angle of the robot is less than the offset angle threshold, determining the orientation of the robot according to the pitch angle of the robot; When the direction of the robot is the top of the slope, the spiral driving component is positively controlled to make the robot move toward the top of the slope; When the robot is facing the bottom of the slope, the spiral driving component is controlled in the reverse direction to make the robot retreat toward the top of the slope.

5. The grain leveling robot in a granary according to claim 4, characterized in that: The preset climbing information also includes: an uphill pitch angle threshold and a downhill pitch angle threshold; determining the orientation of the robot according to the pitch angle of the robot, specifically including: If the pitch angle of the robot is greater than the uphill pitch angle threshold, it is determined that the robot is facing the top of the slope; If the pitch angle of the robot is less than the downhill pitch angle threshold, it is determined that the direction of the robot is the bottom of the slope.

6. The grain leveling robot in a granary according to claim 4, characterized in that: When controlling the screw drive component of the robot according to the offset angle of the robot and the offset angle threshold, adjusting the offset angle of the robot based on a PID algorithm; Adjust the robot's offset angle according to the following formula: ； in, S is the offset angle of the robot, Kp is the proportional adjustment parameter, Ki is the integral adjustment parameter, Kd is the differential adjustment parameter, e ( k ) is the offset angle deviation of the robot adjusted this time, e ( k -1) is the offset angle deviation of the robot last adjusted. It is the accumulated value of the robot's offset angle deviation.

7. The grain leveling robot in a granary according to claim 1, characterized in that: Determine the robot's climbing distance according to the following formula: ； in, S is the climbing distance of the robot, v is the climbing speed of the robot, t is the climbing time of the robot; Determine the grain pile height according to the following formula: ； in, H is the grain pile height, pitch is the pitch angle of the robot; The outer radius of the Archimedean spiral is determined by the following formula: ； in, Rs is the outer radius of the Archimedean spiral; The spiral pitch of the Archimedean spiral is determined according to the following formula: ； in, a is the spiral pitch of the Archimedean spiral, k is the coefficient, k >0.

8. The grain leveling robot in a granary according to claim 1, characterized in that: Also includes: OLED display module; The OLED display module is used to display the current working status of the granary grain leveling robot.

9. A method for automatically controlling the height of a grain leveling robot in a granary, characterized in that: The grain-leveling robot for a granary is applied as claimed in any one of claims 1 to 8, wherein the automatic height control method of the grain-leveling robot for a granary comprises: Receiving an external remote control signal, and determining a robot operating mode according to the external remote control signal; When the robot working mode is the manual control mode, the spiral driving component is controlled to perform corresponding actions according to the external remote control signal; When the robot working mode is the automatic working mode, collecting the original data of the robot posture and sending it to the main control chip; Obtaining robot posture information by solving the robot posture raw data; According to the robot posture information and preset climbing information, controlling the spiral drive component to automatically and quickly climb the slope; An Archimedean spiral is generated based on data generated during the robot's climbing process, and the spiral driving component is controlled to perform grain leveling operations.

Images