An obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems

Abstract

This invention relates to the field of intelligent warehousing system technology, specifically disclosing an obstacle avoidance algorithm suitable for multi-wheel differential speed robot systems in warehousing environments. Based on the magnitude of the error obtained from obstacle position information, the algorithm sets collision probabilities for each location point in the environment, significantly improving the safety of the algorithm in practical applications. By outputting the optimal execution result in position space, it effectively eliminates errors in speed conversion, significantly improving the reliability and safety of obstacle avoidance, and providing a reliable obstacle avoidance solution for warehousing and logistics robots. The algorithm constructs an external contour polygon and obtains a position error shape polygon based on changes in the robot's movement position information; it combines these polygons to generate obstacle shapes containing position errors; it sets relative velocity obstacles and generates position obstacle information based on the relationship between the mass point and the combined polygon; and it sets the collision probability of the mobile robot's location area in the warehousing scenario based on the magnitude of the position obstacle and the error information in the environment.

Description

Technical Field

[0001] This invention relates to the field of intelligent warehousing system technology, and specifically to an obstacle avoidance algorithm for warehousing environments applicable to multi-dual-wheel differential speed robot systems. Background Technology

[0002] In recent years, the application of robots in the commercial sector has expanded rapidly, covering multiple fields such as outdoor inspection, service industry, healthcare, and warehousing and logistics. Efficient and intelligent "goods-to-person" warehousing systems have emerged, effectively alleviating the operational pressure on the logistics industry caused by labor shortages. Furthermore, through precise matching, automated operation, and intelligent scheduling, they have significantly reduced operating costs while achieving a qualitative leap in operational efficiency.

[0003] Currently, a common solution to obstacle avoidance problems involving robots coexisting with static obstacles is to integrate multiple algorithms. While some improved algorithms attempt to predict the positions of other robots even with communication delays, the predictions often deviate from reality due to limitations in the actual movement capabilities of robots, leading to the accumulation of positional errors. These errors can cause collisions between robots and obstacles in warehouse environments, resulting in economic losses. Finally, existing two-wheeled differential robots have significant limitations in execution capabilities. When the velocity vectors generated by velocity obstacle avoidance algorithms are converted into actual motion commands, errors in the mathematical formula conversion may cause collisions between multiple robots, further affecting the reliability and safety of the algorithm.

[0004] Therefore, in order to ensure that the fusion algorithm of speed obstacle and dynamic window can be effectively applied in the warehouse environment, it is necessary to further consider the robot shape, obstacle information acquisition error and execution capability of the two-wheel differential speed robot in the actual warehouse environment, and make corresponding improvements to the algorithm to enhance its applicability and stability in complex warehouse scenarios. Summary of the Invention

[0005] This invention addresses the challenges of obstacle avoidance in warehouse environments, including the shape of multi-wheeled differential speed robots, obstacle information acquisition errors, and robot execution capabilities. It proposes a fusion algorithm combining speed-obstacle and dynamic window methods to improve obstacle avoidance safety and efficiency in such environments. The algorithm optimizes the representation of obstacle shapes, enabling efficient robot movement in narrow passages and avoiding limitations imposed by excessively large obstacle shapes. Furthermore, it sets collision probabilities for each location point in the environment based on the magnitude of obstacle position information acquisition errors, significantly enhancing safety in practical applications. By outputting the optimal execution result in position space, the algorithm effectively eliminates errors in speed conversion, significantly improving obstacle avoidance reliability and safety, and providing a more reliable obstacle avoidance solution for warehouse logistics robots.

[0006] The objective of this invention can be achieved through the following technical solutions:

[0007] An obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems, the algorithm comprising:

[0008] Step 1: Collect the external contour information of the robot and obstacles to construct the external contour polygon;

[0009] Step 2: Obtain the position error shape polygon based on the changes in obstacle position information according to the robot's movement distance;

[0010] Step 3: Based on the outer contour polygons of the robot and the obstacle and the position error polygons, combine these polygons using the Minkowski algorithm to generate a polygon containing the obstacle shape with position error.

[0011] Step 4: Based on the representation polygons of the robot and obstacles, transform the obstacle avoidance of the robot and obstacles into obstacle avoidance of points and combined polygons;

[0012] Step 5: Based on the relationship between the mass point and the composite polygon, set the relative velocity obstacle and generate the position obstacle information;

[0013] Step 6: Based on the magnitude of location obstacles and error information in the environment, set the collision probability of the area where the mobile robot is located in the warehouse scenario;

[0014] Step 7: Sample from the robot's executable speed range to generate multiple robot movement trajectories, and calculate the collision information corresponding to each trajectory based on the probability information in the position space;

[0015] Step 8: Based on the velocity sampling information, define the evaluation function, and combine environmental factors, robot motion requirements, and safety assessment to evaluate the advantages and disadvantages of different paths and select the optimal path.

[0016] Preferably, the method for obtaining the position error shape polygon in step two is as follows:

[0017]

[0018] Among them, (x i ,y i (h,k) represents the coordinates of the discretized points, (h,k) represents the position of the center point of the ellipse, a represents the major axis information of the ellipse, b represents the minor axis information of the ellipse, and N represents the total number of points to be generated, which are then connected to form an approximate polygon.

[0019] Preferably, in step four, obstacle avoidance of the robot and obstacles is transformed into obstacle avoidance of points and combined polygons based on the representation polygons of the robot and obstacles:

[0020] Assume the robot represents the center position coordinates of the polygon as follows: There are n vertices, and the vertices are represented as {P}. r1 ,P r2 ,......P rn}, vertex coordinates are (x ri ,y ri The center position of the polygon representing a certain obstacle is... There are m vertices, and their vertex representation is {P}. o1 ,P o2 ,......P om The coordinates of the vertex are (x...). oj ,y oj Using the robot's center position information, calculate the relative position information of the robot and the obstacles:

[0021]

[0022] Wherein, ΔC(x) Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x) Δri ,y Δri () represents the relative position coordinates of other robots; This represents the center coordinates of the polygon on the robot, (x... oj ,y oj (x) represents the vertex coordinates of the polygon representing the obstacle. ri ,y ri () represents the vertex coordinates of the polygon represented by other robots;

[0023] The combined results of using Minkowski and calculating the relative position information of the robot and the polygon vertices representing the obstacles:

[0024] S={(x Δri +x Δoj ,y Δri +y Δoj ) |i∈[1,n],j∈[1,m]}

[0025] Here, S is used to represent the combination result of the polygon vertices, (x Δri ,y Δri (x) represents the position coordinates of other robots in relative position. Δoj ,y Δoj ) represents the relative position coordinates of the obstacle; n is the number of vertices of the polygon represented by the other robots, and m is the number of vertices of the polygon represented by the obstacle;

[0026] Extract the outermost contour points {P1, P2, ..., P} kThe final combined result is described by}, resulting in an external shape polygon that can contain all the Minkowski equations and the result, serving as the combined polygon for the robot and the obstacle.

[0027] Preferably, in step five:

[0028] Based on the point mass and the combined polygon, a relative velocity obstacle is set. Then, based on the combined polygon obtained from the robot and the obstacle, the point set at time τ is obtained. Forming a new closed polygon;

[0029] Calculate the angle of each vertex in the new closed polygon, assuming the coordinates of each vertex are (a, b) and (c) and (d) respectively. ix ,a iy If i∈k, then:

[0030]

[0031] Where, θ i It is the angle corresponding to each vertex, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices in the new closed polygon; the vertices with the largest and smallest angles are obtained and used as tangent points, thus obtaining the tangent line from the origin of the velocity space to the polygon;

[0032] At time τ, the tangent point divides the polygon into two parts. The vertices of the two parts are placed into different sets of points, and the distance from each vertex to the origin is calculated:

[0033]

[0034] Where, d i It is the distance between each vertex and the origin, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices of the new closed polygon; determine the minimum distance at this time, connect the corresponding point sets in order, and use this as information between the two tangent points; the area enclosed by the tangent and the sequentially connected point sets is the relative velocity barrier;

[0035] For a moving obstacle, obtain its velocity v(v x ,v y Using Minkowski's method, the relative velocity barrier R is transformed into the absolute velocity barrier A:

[0036] A=R⊕v

[0037] Where R represents relative speed obstacle, A represents absolute speed obstacle, and v represents the speed information of the obstacle;

[0038] Based on the robot's own position information p(px ,p y The absolute velocity obstacle A and the planned movement time t are used to convert the absolute velocity obstacle A into a positional obstacle L in the positional space.

[0039] L=A*t⊕p.

[0040] Preferably, the method for obtaining the collision probability in step six is ​​as follows:

[0041] Using the robot's current position as the center, the surrounding operating space is divided into a series of fine pixels, each representing a tiny area in the environment. Using the acquired obstacle information, the shortest distance from the center of each pixel to the boundary of the obstacle is calculated. For pixels directly inside the obstacle, their corresponding shortest distance is set to 0 to indicate that there is a high risk of collision in that area.

[0042] Preferably, based on the standard deviation σ of the error information in the environment, the collision probability of each pixel can be calculated using the following formula:

[0043]

[0044] Where p represents the collision probability of a pixel obtained based on the obstacle information of a certain obstacle or other robots, σ ​​is the standard deviation of the error information in the environment, and d represents the shortest distance from a pixel to the boundary of the obstacle.

[0045] For each obstacle, a position obstacle set is formed with the robot. When M obstacles exist, the probability information of each obstacle set on the mesh map is calculated, and the total collision probability information of each pixel is:

[0046]

[0047] Where, p all p represents the collision probability information of a pixel under the influence of all obstacles in the environment. j This represents the collision probability information of a pixel under the influence of an obstacle in the environment, where M represents the number of obstacles in the environment.

[0048] Preferably, the process of generating the movement trajectory in step seven is as follows:

[0049] In velocity space, the robot's linear velocity ranges from [0, v]. max The angular velocity range is [w min ,w max From this, we can obtain the velocity space:

[0050] S all ={(v,w)|v∈[0,v max ]∩w∈[wmin ,w max ]}

[0051] Among them, S all This is the set velocity space, where (v,w) represents the velocity combination, [0,v... max [w] indicates the range of linear velocity. min ,w max [] indicates the range of angular velocities;

[0052] When sampling velocity, linear velocity and angular velocity are set using different methods; for linear velocity, a uniform interval is set based on the overall interval length L of the linear velocity and the number of samples n to obtain a uniform increment:

[0053]

[0054] Where Δv represents the uniform increment of linear velocity, L is the overall interval length of linear velocity, and n is the number of samples within the linear velocity range;

[0055] For angular velocity, the calculation of the increment is different: for each index j, the increment is expressed as:

[0056]

[0057] Where Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, j represents the index information, which causes different results for the angular velocity increment under index changes; q is a set parameter used to adjust the trend of the increment. A smaller q value will result in a smooth increment region, while a larger q value will result in a larger difference in increment changes.

[0058] The results for linear velocity and angular velocity are:

[0059]

[0060]

[0061] v v (i) represents the linear velocity result under the action of the number of samples n and the uniform increment of linear velocity Δv, where v w (j) represents the angular velocity result under the influence of the number of samples and the index;

[0062] The velocity combination result is:

[0063] twist_combinations={(v v (i),v w (j))∣i∈[0,n-1],j∈[0,2n-1]}

[0064] The trajectory over a period of time is calculated based on the velocity and angular velocity.

[0065] Preferably, the collision information calculation process is as follows:

[0066] The movement time is divided into multiple time periods, and the location points reached in each time period are calculated. Then, the corresponding collision probability is calculated to obtain approximate collision information of the trajectory.

[0067] Suppose that the position coordinates of the mobile robot at time t are (x(t), y(t)), the linear velocity is v(t), the angular velocity is w(t), the orientation angle is θ(t), and the initial orientation quaternion is (x(t), y(t), z(t), w(t)). The sampling interval is set to Δt. Then the pose of the robot at time t is (x(t), y(t), θ(t)).

[0068] Assuming the robot's velocity remains constant between two adjacent time points [t, t+1), and that its motion is uniform linear motion when its angular velocity is 0, the robot's motion between two adjacent time points can be considered as a straight line. Therefore, the pose information for the next time point is:

[0069] x(t+1)=x(t)+v(t)×cosθ(t)×Δt

[0070] y(t+1)=y(t)+v(t)×sinθ(t)×Δt

[0071] θ(t+1)=θ(t)

[0072] Where v(t) is the linear velocity of the mobile robot at time t, x(t+1) is the coordinate information in the x-direction at time t+1, y(t+1) is the coordinate information in the y-direction at time t+1, θ(t+1) is the angle information at time t+1; Δt is the sampling interval time, and (x(t), y(t), θ(t)) is the pose information of the robot at time t.

[0073] When the robot's angular velocity and linear velocity are not zero, its trajectory is an arc; the radius of the arc is expressed as:

[0074] The position of the center of the rotating arc at this moment:

[0075]

[0076] center_x = x(t) - R × sinθ(t)

[0077] center_y = y(t) + R × cosθ(t)

[0078] Where v(t) is the linear velocity of the robot at time t, w(t) is the angular velocity of the robot at time t; (x(t), y(t), θ(t)) is the pose information of the robot at time t;

[0079] The rotation angle is:

[0080]

[0081] Where w(t) is the angular velocity of the robot at time t, and Δt is the sampling interval. Let be the rotation angle of the robot at time t;

[0082] The current location is:

[0083]

[0084] Where (x(t), y(t)) are the robot's position coordinates at time t, and (center_x, center_y) are the center positions of the robot's rotation trajectory at time t. Let be the rotation angle of the robot at time t;

[0085] The orientation quaternion at this point is:

[0086]

[0087] new_orientation.x(t+1)=x(t)×cosγ(t)+y×sinγ(t)

[0088] new_orientation.y(t+1)=y(t)×cosγ(t)-x×sinγ(t)

[0089] new_orientation.z(t+1)=z(t)×cosγ(t)+w×sinγ(t)

[0090] new_orientation.w(t+1)=w(t)×cosγ(t)-z×sinγ(t);

[0091] Where (x(t),y(t),z(t),w(t)) is the initial orientation quaternion, and γ(t) is the angle result calculated based on the quaternion and the rotation angle.

[0092] Preferably, the method for selecting the optimal path in step eight includes defining an evaluation function based on velocity sampling information, and evaluating the merits of different paths in conjunction with environmental factors, robot motion requirements, and safety assessments.

[0093] F(v,ω)=α*angle(v,w)+β*dist(v,w)+λ*obs(v,w)+μ*vel(v,w)

[0094] +γ*goal(v,w)

[0095] Here, angle(v,w) is the angle evaluation function; obs(v,w) is the collision information of the entire trajectory being detected and calculated; dist(v,w) is the distance evaluation function; vel(v,w) evaluates the speed of movement, ensuring efficient and smooth movement under the premise of meeting dynamic constraints; and goal(v,w) further enhances the robot's guidance towards the final target point, maintaining continuous approach to the endpoint even when avoiding obstacles or adjusting the path, thus enhancing the robustness and practicality of trajectory planning.

[0096] The beneficial effects of this invention are as follows: In the obstacle information acquisition stage, this algorithm, without significantly increasing computational cost or burden, effectively improves the robot's movement speed in narrow passages, particularly in warehouse environments, by optimizing the shape representation of obstacles. During robot movement, this algorithm solves the problem of obstacle position errors acquired by the robot. By setting initial error polygons and position collision probabilities, the safety of robot movement is greatly improved. Furthermore, considering the execution capabilities of two-wheeled differential robots in warehouse environments, this algorithm selects the optimal output in the position space based on the evaluation results of velocity sampling trajectory points, effectively avoiding the scraping of obstacles caused by the robot's limited mobility in traditional obstacle avoidance algorithms. In summary, this invention not only enhances the obstacle avoidance capabilities of two-wheeled differential robots but also improves operational efficiency in complex warehouse environments, providing a more reliable solution for robot obstacle avoidance in warehousing and logistics.

[0097] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0098] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0099] Figure 1 This is a general framework diagram for the method of fusing positional obstacles and dynamic windows, taking into account the shape representation of obstacles.

[0100] Figure 2A flowchart illustrating the algorithm for setting the position obstacle probability based on obstacle and robot information in the environment;

[0101] Figure 3 This is a grayscale image showing the probability of a single obstacle being formed in this invention (the darker the color, the higher the probability of collision; the lighter the color, the lower the probability of collision).

[0102] Figure 4 Flowchart of an improved dynamic windowing method that incorporates obstacle probability into the dynamic windowing algorithm. Detailed Implementation

[0103] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0104] Please see Figure 1-4 As shown, this invention is an obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems, and the specific steps include:

[0105] Step 1: Collect the external contour information of the robot and obstacles to construct the external contour polygon;

[0106] Step 2: Obtain the position error shape polygon based on the changes in obstacle position information according to the robot's movement distance;

[0107] Step 3: Based on the outer contour polygons of the robot and the obstacle and the position error polygons, combine these polygons using the Minkowski algorithm to generate a polygon containing the obstacle shape with position error.

[0108] Step 4: Based on the representation polygons of the robot and obstacles, transform the obstacle avoidance of the robot and obstacles into obstacle avoidance of points and combined polygons;

[0109] Step 5: Based on the relationship between the mass point and the composite polygon, set the relative velocity obstacle and generate the position obstacle information;

[0110] Step 6: Based on the magnitude of location obstacles and error information in the environment, set the collision probability of the area where the mobile robot is located in the warehouse scenario;

[0111] Step 7: Sample from the robot's executable speed range to generate multiple robot movement trajectories, and calculate the collision information corresponding to each trajectory based on the probability information in the position space;

[0112] Step 8: Based on the velocity sampling information, define the evaluation function, and combine environmental factors, robot motion requirements, and safety assessment to evaluate the advantages and disadvantages of different paths and select the optimal path.

[0113] In the above technical solution:

[0114] Step 1: Data Acquisition and Polygon Construction. The robot collects the outline information of static obstacles (such as goods on shelves) and other robots in the warehouse environment; based on this data, it constructs the external outline polygons of the obstacles, simplifying the actual warehouse environment into a scene composed of multiple polygons, which facilitates more efficient handling of obstacle avoidance problems later.

[0115] Step Two: Setting the Position Error Shape Polygon. During robot movement, obstacle position information is acquired through two methods: sensors and communication transmission. However, in real-world warehouse environments, obstacles such as shelves can cause communication delays, and sensor scans can also contain errors. To improve robot movement safety, a position error polygon is set to account for errors in the acquired position information of other robots and obstacles. Considering the different impacts of errors in the robot's forward direction and lateral deviation directions, the position error polygon is designed as an ellipse to more accurately reflect the magnitude of the specific error. This elliptical shape effectively encompasses most of the position error information, thereby improving robot safety during movement.

[0116] To reduce computational cost, the ellipse is discretized into multiple points and connected to form an approximate ellipse polygon:

[0117]

[0118] Among them, (x i ,y i (h,k) represents the coordinates of the discretized points, (h,k) represents the position of the center point of the ellipse, a represents the major axis information of the ellipse, b represents the minor axis information of the ellipse, and N represents the total number of points to be generated, which are then connected to form an approximate polygon representation.

[0119] Step 3: Generation of polygon representations. Based on the outer contour polygons of the robot and obstacles in the environment, as well as the position error polygons generated during the robot's movement, the Minkowski Sum algorithm is used to combine and calculate these polygons to generate obstacle shapes that include position errors. For simplified calculation, only the outer points of the combined polygons are retained, and an extraction method is set.

[0120] Since the results of Minkowski sums are usually composed of multiple points and the initial shape may be a concave polygon, a concave polygon extraction strategy based on the k-nearest neighbor (k-NN) algorithm is adopted to ensure the accuracy of the final result. The outermost points are extracted to obtain a representation polygon that incorporates the positional error.

[0121] Step 4: Simplified handling of the obstacle avoidance problem. Based on the representation polygons of the robot and obstacles, the obstacle avoidance problem is transformed into obstacle avoidance between a point mass and a composite polygon. Assume the center coordinates of the robot's representation polygon are... There are n vertices, and the vertices are represented as {P}. r1 ,P r2 ,......P rn}, vertex coordinates are (x ri ,y ri The center position of the polygon representing a certain obstacle is... There are m vertices, and their vertex representation is {P}. o1 ,P o2 ,......P om The coordinates of the vertex are (x...). oj ,y oj Using the robot's center position information, calculate the relative position information of the robot and the obstacles:

[0122]

[0123] Wherein, ΔC(x) Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x) Δri ,y Δri () represents the relative position coordinates of other robots; This represents the center coordinates of the polygon on the robot, (x... oj ,y oj (x) represents the vertex coordinates of the polygon representing the obstacle. ri ,y ri ) are the vertex coordinates of the polygon represented by other robots.

[0124] Using Minkowski and Sum, the combined results of the polygon vertices representing the robot and obstacles with relative position information are calculated:

[0125] S={(x Δri +x Δoj ,y Δri +y Δoj )∣i∈[1,n],j∈[1,m]} (3)

[0126] Here, S is used to represent the combination result of the polygon vertices, (xΔri ,y Δri (x) represents the position coordinates of other robots in relative position. Δoj ,y Δoj ) represents the relative position coordinates of the obstacle. n is the number of vertices of the polygon represented by the other robots, and m is the number of vertices of the polygon represented by the obstacle.

[0127] Extract the outermost contour points {P1, P2, ..., P} k The final combined result is described by}, resulting in an external shape polygon that can contain all the Minkowski equations and the result, serving as the combined polygon for the robot and the obstacle.

[0128] At this stage, the robot's center point is treated as a point mass for obstacle avoidance; at this point, the problem of the robot and obstacles is transformed into an obstacle avoidance problem involving point masses and composite polygons.

[0129] Step 5: Setting up relative velocity obstacles and generating positional obstacle information; setting up relative velocity obstacles based on the mass point and the combined polygon; obtaining the point set at time τ based on the combined polygon obtained from the robot and the obstacle. This forms a new closed polygon.

[0130] Calculate the angle of each vertex in the new closed polygon, assuming the coordinates of each vertex are (a, b) and (c) and (d) respectively. ix ,a iy If i∈k, then

[0131]

[0132] Where, θ i It is the angle corresponding to each vertex, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices in the new closed polygon.

[0133] The vertices with the maximum and minimum angles are identified and used as tangent points, thus obtaining the tangent lines from the origin in velocity space to the polygon. At time τ, the tangent points divide the polygon into two parts. The vertices of these two parts are placed into different point sets, and the distance from each vertex to the origin is calculated.

[0134]

[0135] Where, d i It is the distance between each vertex and the origin, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices in the new closed polygon.

[0136] After determining which combination the minimum distance belongs to, the corresponding point sets are connected sequentially and used as information between the two tangent points. The area enclosed by the tangent and the sequentially connected point sets constitutes the relative velocity barrier.

[0137] For a moving obstacle, obtain its velocity v(v x ,v y Using Minkowski's method, the relative velocity barrier R is transformed into the absolute velocity barrier A.

[0138] A=R⊕v (6)

[0139] Where R represents the relative speed obstacle, A represents the absolute speed obstacle, and v represents the speed information of the obstacle.

[0140] Based on the robot's own position information p(p x ,p y The absolute velocity obstacle A and the planned movement time t are used to convert the absolute velocity obstacle A into a positional obstacle L in the positional space.

[0141] L=A*t⊕p (7)

[0142] Where A represents the absolute velocity obstacle, L represents the positional obstacle in the location space, p represents the robot's own position information, and t represents the robot's planned movement time.

[0143] Step Six: Setting the Collision Probability. Based on the magnitude of location obstacles and environmental error information, the collision probability of the mobile robot's location area in the warehouse scenario is set. To ensure high utilization of location space and stable computational costs in the warehouse environment, a fine mesh map with small pixels is designed. Specifically, the robot's current position is taken as the center, and the surrounding operating space is divided into a series of fine pixels, each representing a small area in the environment. Using the acquired obstacle information, the shortest distance from the center of each pixel to the boundary of the location obstacle is calculated. For pixels directly inside the location obstacle, their corresponding shortest distance is set to 0 to indicate a high collision risk in that area.

[0144] To accurately reflect changes in collision probability, the collision probability of each pixel can be calculated using the following formula, based on the standard deviation σ of the error information in the environment:

[0145]

[0146] Where p represents the collision probability of a pixel obtained based on obstacle information from an obstacle or other robots, σ ​​is the standard deviation of the error information in the environment, and d represents the shortest distance from a pixel to the boundary of the obstacle.

[0147] For each obstacle, a position obstacle set is formed with the robot. When M obstacles exist, the probability information of each obstacle set on the mesh map can be calculated. Then, the total collision probability information of each pixel is...

[0148]

[0149] Where, p all p represents the collision probability information of a pixel under the influence of all obstacles in the environment. j This represents the collision probability information of a pixel under the influence of an obstacle in the environment, where M represents the number of obstacles in the environment.

[0150] Step 7: Trajectory Generation and Collision Information Calculation. Multiple robot movement trajectories are generated by sampling from the robot's executable velocity range. Collision information is calculated for each trajectory based on probability information in the position space. In the velocity space, the robot's linear velocity ranges from [0, v]. max The angular velocity range is [w min ,w max [This allows us to obtain the velocity space.]

[0151] S all ={(v,w)|v∈[0,v max ]∩w∈[w min ,w max ]} (10)

[0152] Among them, S all This is the set velocity space, where (v,w) represents the velocity combination, [0,v... max [w] indicates the range of linear velocity. min ,w max [] indicates the range of angular velocities.

[0153] When sampling velocity, linear velocity and angular velocity are set using different methods. For linear velocity, a uniform interval is set based on the overall interval length L of the linear velocity and the number of samples n to obtain a uniform increment.

[0154]

[0155] Where Δv represents the uniform increment of linear velocity, L is the overall interval length of linear velocity, and n is the number of samples within the linear velocity range.

[0156] For angular velocity, the calculation of the increment is different: for each index j, the increment is expressed as...

[0157]

[0158] Where Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, and j represents the index information, causing different results for the angular velocity increment under index changes. q is a set parameter used to adjust the trend of the increment; a smaller q value results in a smoother increment region, while a larger q value results in a larger difference in increment variation; therefore, the results for linear velocity and angular velocity are:

[0159]

[0160] v v (i) represents the linear velocity result under the action of the number of samples n and the uniform increment of linear velocity Δv, where v w (j) represents the angular velocity result under the influence of the number of samples and the index.

[0161] The result of the velocity combination is

[0162] twist_combinations={(v v (i),v w (j))∣i∈[0,n-1],j∈[0,2n-1]} (15)

[0163] The trajectory is calculated over a period of time based on velocity and angular velocity. To facilitate the calculation of collision probabilities on the trajectory, the movement time is divided into multiple time intervals, and the position points reached in each time interval are calculated. The corresponding collision probabilities are then calculated to obtain approximate collision information for the trajectory.

[0164] Suppose that the position coordinates of the mobile robot at time t are (x(t), y(t)), the linear velocity is v(t), the angular velocity is w(t), the orientation angle is θ(t), and the initial orientation quaternion is (x(t), y(t), z(t), w(t)). The sampling interval is set to Δt. Then the pose of the robot at time t is (x(t), y(t), θ(t)).

[0165] Assuming the robot's velocity remains constant between two adjacent time points [t, t+1), and that its motion is uniform linear motion when its angular velocity is 0, the robot's motion between two adjacent time points can be considered as a straight line. Therefore, the pose information for the next time point is:

[0166] x(t+1)=x(t)+v(t)×cosθ(t)×Δt

[0167] y(t+1)=y(t)+v(t)×sinθ(t)×Δt

[0168] θ(t+1)=θ(t) (11)

[0169] Where v(t) is the linear velocity of the mobile robot at time t, x(t+1) is the x-coordinate information at time t+1, y(t+1) is the y-coordinate information at time t+1, and θ(t+1) is the angle information at time t+1. Δt is the sampling interval, and (x(t), y(t), θ(t)) is the pose information of the robot at time t.

[0170] When the robot's angular velocity and linear velocity are not zero, its trajectory is an arc. The radius of the arc is denoted as...

[0171] The position of the center of the rotating arc at this moment:

[0172]

[0173] center_x = x(t) - R × sinθ(t)

[0174] center_y=y(t)+R×cosθ(t) (13)

[0175] Where v(t) is the linear velocity of the robot at time t, and w(t) is the angular velocity of the robot at time t. (x(t), y(t), θ(t)) represents the pose information of the robot at time t.

[0176] The rotation angle is:

[0177]

[0178] Where w(t) is the angular velocity of the robot at time t, and Δt is the sampling interval. Let t be the rotation angle of the robot at time t.

[0179] The current location is:

[0180]

[0181] Where (x(t), y(t)) are the robot's position coordinates at time t, and (center_x, center_y) are the center positions of the robot's rotation trajectory at time t. Let t be the rotation angle of the robot at time t.

[0182] The orientation quaternion at this point is:

[0183]

[0184] new_orientation.x(t+1)=x(t)×cosγ(t)+y×sinγ(t)

[0185] new_orientation.y(t+1)=y(t)×cosγ(t)-x×sinγ(t)

[0186] new_orientation.z(t+1)=z(t)×cosγ(t)+w×sinγ(t)

[0187] new_orientation.w(t+1)=w(t)×cosγ(t)-z×sinγ(t) (16)

[0188] Where (x(t),y(t),z(t),w(t)) is the initial orientation quaternion, and γ(t) is the angle result calculated based on the quaternion and the rotation angle.

[0189] Based on the calculated location information, the corresponding pixel is found in the grid map storing the obstacle probabilities to quickly obtain its collision probability value. At the same time, in order to reduce the computational burden while ensuring that the running effect does not change much, only the maximum collision probability among all discrete points on the trajectory is considered as the collision probability of the entire trajectory.

[0190] Step 8: Optimal Path Selection. Based on the velocity sampling information, define an evaluation function and, considering environmental factors, robot motion requirements, and safety assessments, evaluate the merits of different paths.

[0191] F(v,ω)=α*angle(v,w)+β*dist(v,w)+λ*obs(v,w)+μ*vel(v,w)

[0192] +γ*goal(v,w) (16)

[0193] In this evaluation function, angle(v,w) is the angle evaluation function: it evaluates the angular difference between the calculated trajectory end orientation and the desired trajectory orientation under the current velocity combination, ensuring high consistency of the motion direction. obs(v,w) detects collision information for the entire calculated trajectory segment; based on the settings in the previous step, the collision probability for each trajectory corresponding to the velocity sampling can be obtained. dist(v,w) is the distance evaluation function, evaluating the distance difference between the trajectory end and the desired trajectory, thus ensuring that the robot can move well along the given path. vel(v,w) evaluates the speed of movement, ensuring efficient and smooth movement under the premise of meeting dynamic constraints. goal(v,w) further enhances the robot's guidance towards the final target point, maintaining continuous approach to the endpoint even when avoiding obstacles or adjusting the path, thus enhancing the robustness and practicality of trajectory planning.

[0194] Finally, by comparing the evaluation values ​​of different trajectories, the optimal path is selected as the robot's final motion plan. The comparison and comparison outputs the best result; this optimal decision after comprehensive evaluation will serve as the robot's execution command, guiding it to complete tasks safely and efficiently in the real environment.

[0195] The above description is merely an example and illustration of the concept of the present invention. Those skilled in the art can make various modifications or additions to the specific embodiments described or use similar methods to replace them, as long as they do not deviate from the concept of the invention or exceed the scope defined in the claims, they should all fall within the protection scope of the present invention.

Claims

1. A warehouse environment obstacle avoidance algorithm suitable for multi-wheel differential speed robot systems, characterized in that, The algorithm includes: Step 1: Collect the external contour information of the robot and obstacles to construct the external contour polygon; Step 2: Obtain the position error shape polygon based on the changes in obstacle position information according to the robot's movement distance; Step 3: Based on the outer contour polygons of the robot and the obstacle and the position error polygons, combine these polygons using the Minkowski algorithm to generate a polygon containing the obstacle shape with position error. Step 4: Based on the representation polygons of the robot and obstacles, transform the obstacle avoidance of the robot and obstacles into obstacle avoidance of points and combined polygons; Step 5: Based on the relationship between the mass point and the composite polygon, set the relative velocity obstacle and generate the position obstacle information; Step 6: Based on the magnitude of location obstacles and error information in the environment, set the collision probability of the area where the mobile robot is located in the warehouse scenario; Step 7: Sample from the robot's executable speed range to generate multiple robot movement trajectories, and calculate the collision information corresponding to each trajectory based on the probability information in the position space; Step 8: Based on the velocity sampling information, define the evaluation function, and combine environmental factors, robot motion requirements, and safety assessment to evaluate the advantages and disadvantages of different paths and select the optimal path.

2. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 1, characterized in that, The method for obtaining the position error shape polygon in step two is as follows: Among them, (x i ,y i () represents the coordinates of the discretized points, (h,k) represents the center point of the ellipse, indicating the mean error, a represents the major axis information of the ellipse, indicating the error information of the robot's forward direction, b represents the minor axis information of the ellipse, indicating the error of the robot's left and right offset direction, and N represents the total number of points to be generated, which is represented by connecting these points into an approximate polygon.

3. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 1, characterized in that, In step four, obstacle avoidance is transformed into obstacle avoidance using points and combined polygons, based on the polygon representations of the robot and obstacles. Assume the robot represents the center position coordinates of the polygon as follows: There are n vertices, and the vertices are represented as {P}. r1 ,P r2 ,......P rn }, vertex coordinates are (x ri ,y ri The center position of the polygon representing a certain obstacle is... There are m vertices, and their vertex representation is {P}. o1 ,P o2 ,......P om The coordinates of the vertex are (x...). oj ,y oj Using the robot's center position information, calculate the relative position information of the robot and the obstacles: Wherein, ΔC(x) Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x) Δri ,y Δri () represents the relative position coordinates of other robots; This represents the center coordinates of the polygon on the robot, (x... oj ,y oj (x) represents the vertex coordinates of the polygon representing the obstacle. ri ,y ri () represents the vertex coordinates of the polygon represented by other robots; The combined results of using Minkowski and calculating the relative position information of the robot and the polygon vertices representing the obstacles: S={(x Δri +x Δoj ,y Δri +y Δoj )∣i∈[1,n],j∈[1,m]} Here, S is used to represent the combination result of the polygon vertices, (x Δri ,y Δri (x) represents the position coordinates of other robots in relative position. Δoj ,y Δoj ) represents the relative position coordinates of the obstacle; n is the number of vertices of the polygon represented by the other robots, and m is the number of vertices of the polygon represented by the obstacle; Extract the outermost contour points {P1, P2, ..., P} k The final combined result is described by}, resulting in an external shape polygon that can contain all the Minkowski equations and the result, serving as the combined polygon for the robot and the obstacle.

4. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 3, characterized in that, In step five: Based on the point mass and the combined polygon, a relative velocity obstacle is set. Then, based on the combined polygon obtained from the robot and the obstacle, the point set at time τ is obtained. Forming a new closed polygon; Calculate the angle of each vertex in the new closed polygon, assuming the coordinates of each vertex are (a, b) and (c) and (d) respectively. ix ,a iy If i∈k, then: Where, θ i It is the angle corresponding to each vertex, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices in the new closed polygon; The vertices with the largest and smallest angles are obtained and used as tangent points to obtain the tangent lines from the origin of the velocity space to the polygon. At time τ, the tangent point divides the polygon into two parts. The vertices of the two parts are placed into different sets of points, and the distance from each vertex to the origin is calculated: Where, d i It is the distance between each vertex and the origin, (a ix ,a iy ) represents the coordinates of each vertex, and k is the number of vertices in the new closed polygon; Determine the minimum distance at this point, connect the corresponding point sets in sequence, and use this as information between the two tangent points; the area enclosed by the tangent and the sequentially connected point sets is the relative velocity obstacle; For a moving obstacle, obtain its velocity v(v x ,v y Using Minkowski's method, the relative velocity barrier R is transformed into the absolute velocity barrier A: Where R represents relative speed obstacle, A represents absolute speed obstacle, and v represents the speed information of the obstacle; Based on the robot's own position information p(p x ,p y The absolute velocity obstacle A and the planned movement time t are used to convert the absolute velocity obstacle A into a positional obstacle L in the positional space.

5. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 1, characterized in that, The method for obtaining the collision probability in step six is ​​as follows: Using the robot's current position as the center, the surrounding operating space is divided into a series of fine pixels, each representing a tiny area in the environment. Using the acquired obstacle information, the shortest distance from the center of each pixel to the boundary of the obstacle is calculated. For pixels directly inside the obstacle, their corresponding shortest distance is set to 0 to indicate that there is a high risk of collision in that area.

6. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 5, characterized in that, Based on the standard deviation σ of the error information in the environment, the collision probability of each pixel can be calculated using the following formula: Where p represents the collision probability of a pixel obtained based on the obstacle information of a certain obstacle or other robots, σ ​​is the standard deviation of the error information in the environment, and d represents the shortest distance from a pixel to the boundary of the obstacle. For each obstacle, a position obstacle set is formed with the robot. When M obstacles exist, the probability information of each obstacle set on the mesh map is calculated, and the total collision probability information of each pixel is: Where, p all p represents the collision probability information of a pixel under the influence of all obstacles in the environment. j This represents the collision probability information of a pixel under the influence of an obstacle in the environment, where M represents the number of obstacles in the environment.

7. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 1, characterized in that, The process of generating the movement trajectory in step seven is as follows: In velocity space, the robot's linear velocity ranges from [0, v]. max The angular velocity range is [w min ,w max From this, we can obtain the velocity space: S all ={(v,w)|v∈[0,v max ]∩w∈[w min ,w max ]} Among them, S all This is the set velocity space, where (v,w) represents the velocity combination; When sampling velocity, linear velocity and angular velocity are set using different methods; for linear velocity: Where Δv represents the uniform increment of linear velocity, L is the overall interval length of linear velocity, and n is the number of samples within the linear velocity range; For angular velocity: Where Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, j represents the index information, which causes different results for the angular velocity increment under index changes; q is a set parameter used to adjust the trend of the increment. A smaller q value will result in a smooth increment region, while a larger q value will result in a larger difference in increment changes. The results for linear velocity and angular velocity are: v v (i) represents the linear velocity result under the action of the number of samples n and the uniform increment of linear velocity Δv, where v w (j) represents the angular velocity result under the influence of the number of samples and the index; The velocity combination result is: twist_combinations={(v v (i),v w (j))∣i∈[0,n-1],j∈[0,2n-1]} twist_combinations are combinations of speeds (v) v (i),v w The combined result of all velocities and angular velocities under the action of (j) is used to calculate and obtain the trajectory over a period of time based on the velocity and angular velocity.

8. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 7, characterized in that, The calculation process for the collision information is as follows: The movement time is divided into multiple time periods, and the location points reached in each time period are calculated. Then, the corresponding collision probability is calculated to obtain approximate collision information of the trajectory. Suppose that the position coordinates of the mobile robot at time t are (x(t), y(t)), the linear velocity is v(t), the angular velocity is w(t), the orientation angle is θ(t), and the initial orientation quaternion is (x(t), y(t), z(t), w(t)). The sampling interval is set to Δt. Then the pose of the robot at time t is (x(t), y(t), θ(t)). Assuming the robot's velocity remains constant between two adjacent time points [t, t+1), and that its motion is uniform linear motion when its angular velocity is 0, the robot's motion between two adjacent time points can be considered as a straight line. Therefore, the pose information for the next time point is: x(t+1)=x(t)+v(t)×cosθ(t)×Δt y(t+1)=y(t)+v(t)×sinθ(t)×Δt θ(t+1)=θ(t) Where v(t) is the linear velocity of the mobile robot at time t, x(t+1) is the coordinate information in the x-direction at time t+1, y(t+1) is the coordinate information in the y-direction at time t+1, θ(t+1) is the angle information at time t+1; Δt is the sampling interval time, and (x(t), y(t), θ(t)) is the pose information of the robot at time t. When the robot's angular velocity and linear velocity are not zero, its trajectory is an arc; the radius of the arc is expressed as: The position of the center of the rotating arc at this moment: center_x = x(t) - R × sinθ(t) center_y = y(t) + R × cosθ(t) Where v(t) is the linear velocity of the robot at time t, w(t) is the angular velocity of the robot at time t; (x(t), y(t), θ(t)) is the pose information of the robot at time t; The rotation angle is: w(t) is the angular velocity of the robot at time t, and Δt is the sampling interval. Let be the rotation angle of the robot at time t; The current location is: Where (x(t), y(t)) are the robot's position coordinates at time t, and (center_x, center_y) are the center positions of the robot's rotation trajectory at time t. Let be the rotation angle of the robot at time t; The orientation quaternion at this point is: new_orientation.x(t+1)=x(t)×cosγ(t)+y×sinγ(t) new_orientation.y(t+1)=y(t)×cosγ(t)-x×sinγ(t) new_orientation.z(t+1)=z(t)×cosγ(t)+w×sinγ(t) new_orientation.w(t+1)=w(t)×cosγ(t)-z×sinγ(t) (x(t),y(t),z(t),w(t)) represents the initial orientation quaternion, and γ(t) represents the angle result calculated based on the quaternion and the rotation angle.

9. The obstacle avoidance algorithm for warehouse environments applicable to multi-wheel differential speed robot systems according to claim 1, characterized in that, The method for selecting the optimal path in step eight includes defining an evaluation function based on velocity sampling information, and evaluating the merits of different paths by combining environmental factors, robot motion requirements, and safety assessments. F(v,ω)=α*angle(v,w)+β*dist(v,w)+λ*obs(v,w)+μ*vel(v,w)+γ*goal(v,w) Where angle(v,w) is the angle evaluation function; obs(v,w) is the collision information of the entire trajectory calculated by detection; dist(v,w) is the path distance evaluation function; vel(v,w) is the motion speed evaluation function; and goal(v,w) is the distance evaluation function to the target point.

Images