Path planning and grain leveling operation control method for grain bin robot grain leveling operation

Abstract

The application relates to a path planning and grain flattening operation control method for a grain warehouse robot grain flattening operation, characterized by the following steps: 1. constructing a map of the grain warehouse through real-time positioning and map construction slam technology; 2. identifying the position coordinates, geometric shape and height of a convex area (grain pile) exceeding a set height threshold in the grain warehouse through a laser radar, and inflating the radius of the grain pile bottom surface approximated circle; 3. constructing a virtual 'potential field' through an artificial potential field method according to the environment where the robot is located; 4. planning a path so that the robot can drive to the grain pile; 5. making spiral line motion around the bottom of the grain pile until the robot reaches the center of the inflated bottom surface circle; and 6. repeating steps 1 to 5 until all the grain piles are below the given height threshold. The method realizes automatic grain flattening operation of a grain warehouse robot or a grain warehouse self-walking device, automatically drives in the grain warehouse according to a planned track, and safely and efficiently completes the grain warehouse operation.

Description

Technical Field

[0001] This invention relates to a path planning and control method for grain leveling operations using a grain warehouse robot. It is a method that plans the path for the robot to move towards the grain pile and controls the robot to perform spiral motion, thereby leveling the grain pile using the robot's walking mechanism. It belongs to the field of grain warehouse robot automation. Background Technology

[0002] In grain silo operations, grain transported to the silo by belt conveyors often forms uneven piles. After entering the silo, timely leveling of the grain surface and standardized management are necessary to ensure grain storage safety. Traditional grain silo storage methods involve manually leveling the grain piles, which is inefficient and prone to unevenness. With the continuous development of robotics technology, grain silo robots are now used for grain leveling. For robots to autonomously complete grain silo operations, they need to be able to identify the location of the grain piles, then plan their paths using algorithms and control their movement to reach the piles. Currently, most grain silo robots are controlled remotely, failing to achieve fully automated operation. Summary of the Invention

[0003] To address the aforementioned technical problems, the present invention aims to provide a path planning and control method for grain leveling operations using a grain warehouse robot. This method utilizes lidar technology to identify grain pile information with high accuracy, avoiding errors from manual operation. The robot can automatically move towards the grain pile, achieving a high degree of automation and reducing manual remote control operations. By controlling the robot to perform spiral motion and using its walking mechanism to quickly level the grain pile, work efficiency is improved.

[0004] The technical solution of this invention is implemented as follows: a path planning and leveling operation control method for grain warehouse robots, characterized by the following specific steps:

[0005] Step 1: Construct a map of the grain warehouse using SLAM (simultaneous localization and mapping) technology;

[0006] 1) Install sensors such as LiDAR, cameras, gyroscopes, and GPS on the robot to capture data of the surrounding environment and the robot's posture information;

[0007] 2) The robot is placed in a grain silo environment, where it needs to move in an unknown environment to collect environmental data. This data includes distance and orientation information from the LiDAR.

[0008] 3) After collecting the data, the SLAM algorithm built into ROS needs to be used to process the data and generate an environment map;

[0009] 4) After the map is generated, it is updated and optimized through data cleaning and algorithm optimization to improve accuracy and precision;

[0010] 5) Import the generated map into the robot's control system to achieve path planning.

[0011] Step 2: Identify the location coordinates, geometry, and height of the grain pile using lidar, and expand the radius of the approximately circular base of the grain pile.

[0012] 1) Use lidar to scan the grain pile to obtain its location coordinates, geometry, and height information;

[0013] 2) Approximate a circle is fitted to the bottom surface of the grain pile to obtain its radius;

[0014] 3) Expand the radius by 0.2 times based on the previous step, that is, multiply the original radius by 1.2;

[0015] 4) Combine the information of the bottom surface of the expanded grain pile with the original location, geometry and height information to obtain complete information about the grain pile.

[0016] Step 3: Based on the robot's environment, construct a virtual "potential field" using a variant of the artificial potential field method;

[0017] This step involves constructing an artificial potential field within the grain silo environment. This field includes repulsive and attractive poles. Areas or obstacles that the robot is not allowed to enter are defined as repulsive poles, while the target area and the area the robot is encouraged to enter are defined as attractive poles. Similar to an electromagnetic field in physics, this field causes the robot to move towards the target due to the combined effects of the target's attractive field and the surrounding repulsive field. The specific steps of the algorithm are as follows:

[0018] 1) Construct a gravitational potential field. The function of the gravitational potential field is:

[0019]

[0020] Where U(q) is the potential energy of the gravitational potential field, k is a proportionality coefficient, ρ(q,q0) is a vector pointing from the robot to the grain pile, and its magnitude is the Euclidean distance |q-q0| between the robot and the grain pile. ρ0 is a constant representing the maximum range of influence of the grain pile on the robot. The factor that determines the gravitational potential field of the grain pile is the distance between the robot and the grain pile. When the robot is not in the influence range of the grain pile, its potential energy is zero. After the robot enters the influence range of the grain pile, the smaller the distance between the two, the greater the potential energy of the robot, and the greater the distance, the smaller the potential energy of the robot.

[0021] The corresponding gravitational formula is:

[0022]

[0023] In the formula, F1 is the gravitational force acting on the robot, and its direction is that the robot is pointing towards the grain pile;

[0024] 2) Create boundary repulsion. Since there are no other obstacles in the grain silo environment by default, no obstacle repulsion potential field is established. Only the boundary repulsion of the grain silo is considered to restrict the robot's movement. The influence of the robot's speed on the repulsion force is also appropriately considered. The specific repulsion formula is as follows:

[0025]

[0026] Assuming the bottom of the grain silo is square, establish a rectangular coordinate system with the center of the bottom as the origin. In the formula, F2 is the repulsive force, η is a constant, v is the robot's velocity, x is the robot's lateral coordinate, y is the robot's longitudinal coordinate, a is half the width of the grain silo bottom, b is half the length of the grain silo bottom, L is the vehicle length, w is the vehicle width, and e... (x+y) With e -(x+y) This indicates that as the absolute values ​​of the horizontal and vertical coordinates increase, the repulsive force experienced by the robot increases exponentially, such as... Figure 3 As shown;

[0027] 3) Based on the fundamental principles of the artificial potential field method, the potential energy of the robot in the artificial potential field when it moves under these two potential field environments is determined by the sum of multiple potential energy vectors. Therefore, the expression for the resultant force of the resultant potential field acting on the robot is:

[0028] F = F1 + F2

[0029] In the formula: F is the net force acting on the robot in the potential field.

[0030] Step 4: Plan a path so that the robot can drive to the grain pile;

[0031] Based on the gravitational potential field function obtained in step 3, the potential energy of each grain pile is calculated. The grain pile with the largest potential energy is selected, and then the optimal path is calculated using the gradient descent method. Starting from the current point, the path moves along the negative gradient direction of the potential field until the target point is reached or the boundary is encountered. The specific steps are as follows:

[0032] Let the vehicle's position be (x, y) and the target position be (x, y). g ,y g If ), then the function of the gravitational potential field is

[0033]

[0034] In the formula: Represents the Euclidean distance between the robot and the target position.

[0035] After differentiation, the negative gradient of the gravitational potential field is obtained as follows:

[0036]

[0037] In the formula, For a negative gradient, -U' x (x,y) and -U' y (x,y) represents the negative gradient component. and Indicates the coordinate base;

[0038] in,

[0039]

[0040] After simplification, the negative gradient of the gravitational potential field is:

[0041]

[0042] The gradient descent algorithm outputs a sequence of path planning points. These sequences are transmitted to the robot's underlying control system, which then controls the robot to move to the target grain pile according to the planned path.

[0043] Step 5: Move the robot in a spiral around the bottom of the grain pile until it reaches the center of the expanded bottom circle;

[0044] Assuming the center of the target circle is at coordinates (x0, y0) and the radius is r, define a parameter t to represent the current angle, and use the following formula to calculate the coordinates of the point:

[0045] x = r·cos(t)

[0046] y = r·sin(t)

[0047] To achieve spiral motion around the outer edge of a circle, the value of t is updated cyclically, with the rate of change of t gradually increasing so that the increment of t increases proportionally to the current t value. This cycle repeats every fixed time interval T = 0.1 seconds. The specific formula is as follows:

[0048] initialization:

[0049] Δt = 0.01

[0050] k = 0.1

[0051] Cyclic Update:

[0052] Δt=Δt·(1+k·t)

[0053] t = t + Δt

[0054] r = r - 0.5

[0055] In the formula, Δt is the increment of t, and k is a fixed scaling factor. Each cycle yields a new coordinate (x, y) until the obtained coordinates approximate the center of the circle (x0, y0). The cycle ends, and the coordinate sequence is transmitted to the underlying control system, which can then control the robot to perform helical motion. By utilizing the interaction between the robot's walking mechanism and the grain surface, the grain pile can be flattened during the motion. Step 6: Repeat steps 2 to 5 until all grain piles are below the given height threshold;

[0056] During the operation of the robot in the grain warehouse, the lidar monitors the height of the grain pile in real time. Assuming that the height of the grain pile detected by the robot relative to the robot chassis is h, and the height of the lidar relative to the robot chassis is h0, the robot repeats the operation according to steps 2 to 5 in claim 1 until h ≤ h0, at which point it is considered that all grain piles have been flattened and the robot stops working.

[0057] Compared with the prior art, the positive effects of the present invention are:

[0058] 1. This invention utilizes lidar technology to identify grain pile information with high accuracy, avoiding errors caused by manual operation.

[0059] 2. This invention modifies the artificial potential field method in the path planning algorithm, changing the original repulsive potential field function to an attractive potential field function, enabling the robot to move towards the grain pile. This results in a high degree of automation and reduces manual remote control operations.

[0060] 3. Based on path planning, this invention adds a spiral motion algorithm to control the robot to move in a spiral and use the robot's walking mechanism to quickly flatten the grain pile, thereby improving work efficiency. Attached Figure Description

[0061] Figure 1 This is a flowchart of the method of the present invention.

[0062] Figure 2 Mapping for SLAM technology.

[0063] Figure 3 This is a dimensional drawing of the grain silo and the robot.

[0064] Figure 4 Create a spiral motion diagram for the robot. Detailed Implementation

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

[0066] Example 1

[0067] like Figure 1 As shown, a path planning and leveling operation control method for grain warehouse robots is characterized by the following specific steps:

[0068] Step 1: Construct a map of the grain warehouse using SLAM technology;

[0069] 1) Install sensors such as LiDAR, cameras, gyroscopes, and GPS on the robot to capture data of the surrounding environment and the robot's posture information;

[0070] 2) The robot is placed in a grain silo environment, where it needs to move in an unknown environment to collect environmental data. This data includes distance and orientation information from the LiDAR.

[0071] 3) After collecting the data, it is necessary to use the SLAM algorithm built into ROS to process the data and generate an environment map, such as... Figure 2 As shown;

[0072] 4) After the map is generated, it is updated and optimized through data cleaning and algorithm optimization to improve accuracy and precision;

[0073] 5) Import the generated map into the robot's control system to achieve path planning.

[0074] Step 2: Identify the location coordinates, geometry, and height of the grain pile using lidar, and expand the radius of the approximately circular base of the grain pile.

[0075] 1) Use lidar to scan the grain pile to obtain its location coordinates, geometry, and height information;

[0076] 2) Approximate a circle is fitted to the bottom surface of the grain pile to obtain its radius;

[0077] 3) Expand the radius by 0.2 times based on the previous step, that is, multiply the original radius by 1.2;

[0078] 4) Combine the information of the bottom surface of the expanded grain pile with the original location, geometry and height information to obtain complete information about the grain pile.

[0079] Step 3: Based on the robot's environment, construct a virtual "potential field" using a variant of the artificial potential field method;

[0080] This step involves constructing an artificial potential field within the grain silo environment. This field includes repulsive and attractive poles. Areas or obstacles that the robot is not allowed to enter are defined as repulsive poles, while the target area and the area the robot is encouraged to enter are defined as attractive poles. Similar to an electromagnetic field in physics, this field causes the robot to move towards the target due to the combined effects of the target's attractive field and the surrounding repulsive field. The specific steps of the algorithm are as follows:

[0081] 1) Construct a gravitational potential field. The function of the gravitational potential field is:

[0082]

[0083] Where U(q) is the potential energy of the gravitational potential field, k is a proportionality coefficient, ρ(q,q0) is a vector pointing from the robot to the grain pile, and its magnitude is the Euclidean distance |q-q0| between the robot and the grain pile. ρ0 is a constant representing the maximum range of influence of the grain pile on the robot. The factor that determines the gravitational potential field of the grain pile is the distance between the robot and the grain pile. When the robot is not in the influence range of the grain pile, its potential energy is zero. After the robot enters the influence range of the grain pile, the smaller the distance between the two, the greater the potential energy of the robot, and the greater the distance, the smaller the potential energy of the robot.

[0084] The corresponding gravitational formula is:

[0085]

[0086] In the formula, F1 is the gravitational force acting on the robot, and its direction is that the robot is pointing towards the grain pile;

[0087] 2) Create boundary repulsion. Since there are no other obstacles in the grain silo environment by default, no obstacle repulsion potential field is established. Only the boundary repulsion of the grain silo is considered to restrict the robot's movement. The influence of the robot's speed on the repulsion force is also appropriately considered. The specific repulsion formula is as follows:

[0088]

[0089] Assuming the bottom of the grain silo is square, establish a rectangular coordinate system with the center of the bottom as the origin. In the formula, F2 is the repulsive force, η is a constant, v is the robot's velocity, x is the robot's lateral coordinate, y is the robot's longitudinal coordinate, a is half the width of the grain silo bottom, b is half the length of the grain silo bottom, L is the vehicle length, w is the vehicle width, and e... (x+y) With e -(x+y) This indicates that as the absolute values ​​of the horizontal and vertical coordinates increase, the repulsive force experienced by the robot increases exponentially, such as... Figure 3 As shown;

[0090] 3) Based on the fundamental principles of the artificial potential field method, the potential energy of the robot in the artificial potential field when it moves under these two potential field environments is determined by the sum of multiple potential energy vectors. Therefore, the expression for the resultant force of the resultant potential field acting on the robot is:

[0091] F = F1 + F2

[0092] In the formula: F is the net force acting on the robot in the potential field.

[0093] Step 4: Plan a path so that the robot can drive to the grain pile;

[0094] Based on the gravitational potential field function obtained in step 3, the potential energy of each grain pile is calculated. The grain pile with the largest potential energy is selected, and then the optimal path is calculated using the gradient descent method. Starting from the current point, the path moves along the negative gradient direction of the potential field until the target point is reached or the boundary is encountered. The specific steps are as follows:

[0095] Let the vehicle's position be (x, y) and the target position be (x, y). g ,y g If ), then the function of the gravitational potential field is

[0096]

[0097] In the formula: Represents the Euclidean distance between the robot and the target position.

[0098] After differentiation, the negative gradient of the gravitational potential field is obtained as follows:

[0099]

[0100] In the formula, For a negative gradient, -U' x (x,y) and -U' y (x,y) represents the negative gradient component. and Indicates the coordinate base;

[0101] in,

[0102]

[0103] After simplification, the negative gradient of the gravitational potential field is:

[0104]

[0105] The gradient descent algorithm outputs a sequence of path planning points. These sequences are transmitted to the robot's underlying control system, which then controls the robot to move to the target grain pile according to the planned path.

[0106] Step 5: Move the robot in a spiral around the bottom of the grain pile until it reaches the center of the expanded bottom circle;

[0107] Assuming the center of the target circle is at coordinates (x0, y0) and the radius is r, define a parameter t to represent the current angle, and use the following formula to calculate the coordinates of the point:

[0108] x = r·cos(t)

[0109] y = r·sin(t)

[0110] To achieve spiral motion around the outer edge of a circle, the value of t is updated cyclically, with the rate of change of t gradually increasing so that the increment of t increases proportionally to the current t value. This cycle repeats every fixed time interval T = 0.1 seconds. The specific formula is as follows:

[0111] initialization:

[0112] Δt = 0.01

[0113] k = 0.1

[0114] Cyclic Update:

[0115] Δt=Δt·(1+k·t)

[0116] t = t + Δt

[0117] r = r - 0.5

[0118] In the formula, Δt is the increment of t, and k is a fixed scaling factor. Each cycle yields a new coordinate (x, y) until the obtained coordinates approximate the center of the circle (x0, y0). The cycle ends, and the coordinate sequence is transmitted to the underlying control system, which then controls the robot to perform helical motion. Utilizing the interaction between the robot's walking mechanism and the grain surface, the grain pile can be flattened during the motion. Figure 4 As shown.

[0119] Step 6: Repeat steps 2 through 5 until all grain piles are below the given height threshold;

[0120] During the operation of the robot in the grain warehouse, the lidar monitors the height of the grain pile in real time. Assuming that the height of the grain pile detected by the robot relative to the robot chassis is h, and the height of the lidar relative to the robot chassis is h0, the robot repeats the operation according to steps 2 to 5 in claim 1 until h ≤ h0, at which point it is considered that all grain piles have been flattened and the robot stops working.

Claims

1. A method for path planning and flat grain operation control of a grain bin robot, characterized in that Comprise the following steps: Step 1: build the map of the warehouse by instant positioning and map building slam technology; Step 2: identify the position coordinates, geometric shape and height of the grain pile higher than the set threshold by laser radar, and expand the radius of the grain pile bottom surface approximated circle; Step 3: according to the environment where the robot is located, a virtual "potential field" is constructed by artificial potential field method, that is, an artificial potential field is constructed in the warehouse environment, which includes repulsion poles and attraction poles, the areas where the robot is not expected to enter or obstacles are defined as repulsion poles, and the areas where the robot is expected to enter are defined as attraction poles, which is similar to the electromagnetic field in physics, so that the robot in the potential field is affected by the target pose attraction field and the surrounding repulsion field, and moves towards the target; The specific steps of the algorithm are as follows: 1) build the attractive potential field, the function of the attractive potential field is: Where U(q) is the potential energy of the attractive potential field, k is a positive proportional coefficient, p(q,q0) is a vector, the direction is from the robot to the grain pile, and the size is the Euclidean distance between the robot and the grain pile |q-q0|, p0 is a constant, representing the maximum influence range of the grain pile on the robot, the factor that determines the grain pile attractive potential field is the distance between the robot and the grain pile, when the robot does not enter the influence range of the grain pile, the potential energy value it receives is zero, when the robot enters the influence range of the grain pile, the smaller the distance between them, the greater the potential energy value the robot receives, the greater the distance, the smaller the potential energy value the robot receives; The corresponding attractive force formula is: Where F1 is the attractive force received by the robot, the direction is the robot pointing to the grain pile; 2) create a boundary repulsion, by default there is no other obstacle in the warehouse environment, so no obstacle repulsion potential field is set up, only the set warehouse boundary repulsion is considered, which limits the movement of the robot, and appropriately considers the influence of the speed of the robot on the repulsion, and the corresponding specific repulsion formula is as follows: Assuming the bottom surface of the granary is square, a rectangular coordinate system is established with the center of the bottom surface as the origin, where F2 is the repulsive force, η is a constant, v is the robot speed, x is the lateral coordinate of the robot, y is the longitudinal coordinate of the robot, a is half the width of the bottom surface of the granary, b is half the length of the bottom surface of the granary, L is the vehicle length, w is the vehicle width, e (x+y) and e -(x+y) represents that as the absolute values of the lateral and longitudinal coordinates increase, the repulsive force received by the robot increases exponentially; 3) according to the basic principle of artificial potential field method, the potential energy of the robot in the artificial potential field when moving in the environment of the two kinds of potential field is determined by the addition of multiple potential energy vectors, therefore, the expression of the resultant force of the combined potential field received by the robot is: F=F1+F2 Where: F is the resultant force received by the robot in the potential field; Step 4: plan a path to make the robot drive to the grain pile; Step 5: make spiral line motion around the bottom of the grain pile until the robot reaches the center of the expanded bottom surface circle; Step 6: repeat steps 2 to 5 until all grain piles are below the given height threshold.

2. The path planning and grain leveling operation control method for a grain barn robot to perform a grain leveling operation according to claim 1, characterized in that The map of the warehouse is constructed by slam technology, which comprises the following steps: 1) install laser radar, camera, gyroscope and GPS on the robot to capture the surrounding environment data and the attitude information of the robot; 2) put the robot into the warehouse environment, the robot needs to move in the unknown environment to collect environmental data; These data include the distance and direction information of the laser radar; 3) after collecting the data, the slam algorithm in ros needs to be used to process the data and generate an environment map; 4) after the map is generated, the map is updated and optimized through data cleaning and algorithm optimization to improve the accuracy and accuracy; 5) The generated map is imported into the control system of the robot to realize the path planning function.

3. The path planning and grain leveling operation control method for a grain barn robot grain leveling operation according to claim 1, characterized in that The position coordinates, geometric shape and height of the grain pile are recognized by the laser radar, and the radius of the approximately circular bottom surface of the grain pile is inflated, and the specific steps include the following steps: 1) The laser radar is used to scan the grain pile to obtain its position coordinates, geometric shape and height information; 2) The bottom surface of the grain pile is approximately circularly fitted to obtain its radius; 3) The radius is inflated by 0.2 times based on the previous step, that is, the original radius is multiplied by 1.2; 4) The inflated bottom surface information of the grain pile is combined with the original position, geometric shape and height information to obtain complete grain pile information.

4. The path planning and grain leveling operation control method for a grain barn robot grain leveling operation according to claim 1, characterized in that The path is planned to enable the robot to drive to the grain pile, which is according to the potential energy of each grain pile obtained by the gravitational potential field function, the grain pile with the maximum potential energy is screened out, and then the optimal path is calculated by the gradient descent method, and the robot starts from the current point and moves along the negative gradient direction of the potential field until it reaches the target point or encounters the boundary, and the specific steps are as follows: Let the vehicle position be (x, y), and the target position be (x g ,y g ), then the function of the attractive potential field is In the formula: represents the Euclidean distance between the robot and the target position; The negative gradient of the gravitational potential field is obtained after derivation: where -▽U(x,y) is the negative gradient, -U' x (x,y) and -U' y (x,y) is the negative gradient component, and denotes the coordinate basis; wherein, The negative gradient of the gravitational potential field is obtained after simplification: The output of the gradient descent algorithm is a path planning point sequence, which is transmitted to the robot bottom control system to control the robot to move to the target grain pile according to the planned path. 5.The method according to claim 1, wherein The specific steps of the spiral line motion around the bottom of the grain pile until the robot reaches the center of the inflated bottom surface circle are as follows: Assuming that the coordinates of the target center are (x0, y0) and the radius is r, a parameter t is defined to represent the current angle, and the following formula is used to calculate the coordinates of the point: x = r cos (t) y = r sin (t) In order to make spiral line motion around the periphery of the circle, the value of t is updated in a loop, and the change speed of t is gradually increased, so that the increment of t is proportional to the current value of t, and the loop is executed every fixed time T = 0.1 s, and the specific formula is as follows: Initialization: Delta t = 0.01 k＝0.1 Loop update: Delta t = Delta t * (1 + k * t) t = t + Delta t r＝r-0.5 In the formula, Delta t is the increment of t, and k is a fixed proportional factor. A new coordinate (x, y) is obtained every time the loop is executed until the obtained coordinate is approximately equal to the center (x0, y0) of the circle. The loop ends, and the coordinate sequence is transmitted to the bottom control system to control the robot to make spiral line motion. The interaction between the robot walking mechanism and the grain surface can flatten the grain pile during the motion.

6. The path planning and grain leveling operation control method for a grain barn robot grain leveling operation according to claim 1, characterized in that The step 6 repeats steps 2 to 5 until all grain piles are below the given height threshold, which means that the laser radar monitors the height of the grain pile in real time during the work of the robot. Assuming that the height of the grain pile monitored by the robot relative to the robot chassis is h, and the height of the laser radar relative to the robot chassis is h0, the robot repeats the work according to steps 2 to 5 until h <= h0, then it is considered that all grain piles have been flattened, and the robot stops working.

Images