Storage environment obstacle avoidance algorithm suitable for multi-double-wheel differential robot system

Abstract

The invention relates to the technical field of intelligent warehousing systems, and particularly discloses a warehousing environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system, the size of an error is obtained according to obstacle position information, a collision probability is set for each position point in the environment, and the safety of the algorithm in practical application is greatly improved; the optimal execution result is output in the position space, errors in speed conversion are effectively eliminated, the reliability and safety of obstacle avoidance are remarkably improved, and a reliable warehouse logistics robot obstacle avoidance solution is provided. An external contour polygon is constructed, and a position error shape polygon is obtained according to the movement position information change of the robot; performing combined calculation to generate a polygon containing the obstacle shape of the position error; setting a relative speed obstacle and generating position obstacle information according to the relationship between the mass point and the combined polygon; and the collision probability of the position area where the mobile robot is located in the storage scene is set according to the position obstacle and the error information in the environment.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent warehousing systems, and in particular to an obstacle avoidance algorithm for a warehousing environment suitable for a multi-double-wheel differential robot system. Background Art

[0002] In recent years, the application of robots in the commercial field has expanded rapidly, covering outdoor inspection, service industry, medical health, warehousing and logistics, etc. The efficient and intelligent "goods to person" warehousing system model has emerged, which not only effectively alleviates the operational pressure caused by the shortage of human resources in the logistics industry, but also greatly reduces the operating costs through precise matching, automated operation and intelligent scheduling, while achieving a qualitative leap in operating efficiency.

[0003] At present, the common solution to the obstacle avoidance problem of robots coexisting with static obstacles is to integrate multiple algorithms. Although some improved algorithms try to estimate the positions of other robots in the case of communication delays, due to the limitations of the actual mobility of the robots, the estimated results are often inconsistent with the actual situation, resulting in the accumulation of position errors. This error may cause collisions between robots and obstacles in a warehousing environment, resulting in economic losses. Finally, the existing two-wheel differential robots have significant limitations in their execution capabilities. When the speed vector generated by the speed obstacle algorithm is converted into actual motion instructions, it may cause collisions between multiple robots due to errors in mathematical formula conversion, further affecting the reliability and safety of the algorithm.

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

[0005] Aiming at the shapes of multiple double-wheel differential robots in warehousing environments, the error in obtaining obstacle information, and the execution capability of robots, the present invention proposes a fusion algorithm that integrates speed obstacles and dynamic windows, aiming to improve the safety and efficiency of obstacle avoidance in warehousing environments. The algorithm optimizes the representation of obstacle shapes, allowing the robot to move efficiently in narrow passages and avoid being restricted by the excessively large representation of obstacle shapes. In addition, the algorithm sets the collision probability for each position point in the environment according to the size of the error in obtaining obstacle position information, which greatly improves the safety of the algorithm in practical applications. By outputting the best execution result in the position space, the algorithm effectively eliminates the error in speed conversion, significantly improves the reliability and safety of obstacle avoidance, and provides a more reliable obstacle avoidance solution for warehousing and logistics robots.

[0006] The purpose of the present invention can be achieved through the following technical solutions:

[0007] An obstacle avoidance algorithm for a warehouse environment suitable for a multi-double-wheel differential robot system, the algorithm comprising:

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

[0009] Step 2: Obtain the position error shape polygon according to the change in the obstacle position information as the robot moves away;

[0010] Step 3: Based on the outer contour polygons and position error polygons of the robot and the obstacle, these polygons are combined and calculated by Minkowski sum algorithm to generate a polygon of the obstacle shape including the position error;

[0011] Step 4: According to the representation polygons of the robot and the obstacle, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon;

[0012] Step 5: Set relative speed obstacles and generate position obstacle information based on the relationship between the mass point and the combined polygon;

[0013] Step 6: Set the collision probability of the mobile robot in the warehouse scene according to the position obstacles and the error information size in the environment;

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

[0015] Step 8: Define the evaluation function based on the speed sampling information, combine environmental factors, robot motion requirements and safety assessment, evaluate the advantages and disadvantages of different paths, and select the best path.

[0016] Preferably, the position error shape polygon is obtained in step 2 by:

[0017]

[0018] Among them, (x i ,y i ) is the coordinate of the discretized point, (h, k) is the center point of the ellipse, a is the major axis information of the ellipse, b is the minor axis information of the ellipse, N is the total number of points to be generated, and these points are connected into an approximate polygon representation.

[0019] Preferably, in step 4, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon according to the representation polygon of the robot and the obstacle:

[0020] Assume that the center coordinates of the robot representation polygon are There are n vertices, and the vertex is represented by {P r1 ,P r2 ,......P rn}, the vertex coordinates are (x ri ,y ri ), the center position of the representation polygon of an obstacle is There are m vertices, and the vertices are represented by {P o1 ,P o2 ,......P om}, the coordinates of the vertex are (x oj ,y oj ); Calculate the relative position information of the robot and obstacles through the center position information of the robot:

[0021]

[0022] Among them, ΔC(x Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x Δri ,y Δri ) are the relative position coordinates of other robots; is the center coordinate of the polygon represented by the robot, (x oj ,y oj ) are the vertex coordinates of the obstacle representation polygon, (x ri ,y ri ) are the vertex coordinates of the polygons represented by other robots;

[0023] The Minkowski sum is used to calculate the combined result of the polygon vertices representing the robot and the obstacle under the relative position information:

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

[0025] Among them, S is used as the combination result representing the polygon vertices, (x Δri ,y Δri ) is the position coordinate of other robots in relative position, (x Δoj ,y Δoj ) is the coordinate of the obstacle position in relative position; n is the number of vertices of the polygon representing other robots, and m is the number of vertices of the polygon representing the obstacle;

[0026] Extract the outermost contour points {P1, P2, ... P k} to describe the final combination result, and obtain an external shape polygon that can contain all Minkowski and results as the combined polygon of the robot and the obstacle.

[0027] Preferably, in step five:

[0028] According to the mass points and the combined polygons, the relative speed obstacles are set, and the point set at time τ is obtained according to the combined polygons obtained by the robot and the obstacles. Construct a new closed polygon;

[0029] Calculate the angle of each vertex in the new closed polygon, assuming that the coordinates corresponding to each vertex are (a ix ,a iy ),i∈k, then:

[0030]

[0031] Among them, θ i is the angle corresponding to each vertex, (a ix ,a iy ) is the coordinate corresponding to each vertex, k is the number of vertices of the new closed polygon; get the vertices with the largest and smallest angles, use them as tangent points, and thus get 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, puts the vertices in the two parts into different point sets, and solves the distance from each vertex to the origin:

[0033]

[0034] Among them, d i is the distance between each vertex and the origin, (a ix ,a iy ) is the coordinate corresponding to each vertex, k is the number of vertices of the new closed polygon; determine the minimum distance at this time, connect the corresponding point set in sequence, and use it as the information between the two tangent points; the area surrounded by the tangent line and the sequentially connected point set is the relative speed obstacle;

[0035] For a moving obstacle, obtain its velocity v(v x ,v y ), using the Minkowski sum, the relative speed barrier R is converted into an absolute speed barrier A:

[0036] A=R⊕v

[0037] Among them, R represents the relative speed obstacle, A represents the 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 ) and the planned moving time t, the absolute speed obstacle A is converted into the position obstacle L in the position space:

[0039] L=A*t⊕p.

[0040] Preferably, the method of obtaining the collision probability in step 6 is:

[0041] Taking the current position of the robot as the center, the operating space around it is divided into a series of fine pixels. Each pixel represents 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 position obstacle is calculated. For the pixels directly inside the position obstacle, the corresponding shortest distance is set to 0 to indicate that there is a higher collision risk in this area.

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

[0043]

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

[0045] For each obstacle, a position obstacle set will be formed with the robot. When M obstacles exist, the probability information setting of each obstacle set to the grid map is calculated, and the total collision probability information of each pixel point is:

[0046]

[0047] Among them, p all Represents the collision probability information of a certain pixel under the action of all obstacles in the environment, p j It represents the collision probability information of a certain pixel under the action of a certain obstacle in the environment, and M represents the number of obstacles in the environment.

[0048] Preferably, the process of generating the moving trajectory in step seven is:

[0049] In velocity space, the robot’s linear velocity range is [0,v max ], the angular velocity range is [w min ,w max ], we can get the velocity space:

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

[0051] Among them, S all is the speed space set, (v,w) represents the speed combination, [0,v max ] represents the linear velocity range, [w min ,w max ] represents the angular velocity range;

[0052] When sampling the velocity, different setting methods are used for linear velocity and angular velocity. For linear velocity, a uniform interval is set according to 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 length of the overall interval of linear velocity, and n is the number of samples within the linear velocity range;

[0055] For angular velocity, the increment is calculated differently: for each index j, the increment is expressed as:

[0056]

[0057] Wherein, Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, j represents the index information, so that the angular velocity increment has different results when the index changes; q is a set parameter used to adjust the change trend of the increment. A smaller q value will result in a smooth increment area, while a larger q value will result in a larger increment change gap;

[0058] The result of linear velocity and angular velocity is:

[0059]

[0060]

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

[0062] The speed combination results are:

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

[0064] According to the speed and angular velocity, the moving trajectory within a period of time is calculated.

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

[0066] The moving time is divided into multiple time periods, and the position points reached in each time period are calculated, and then the corresponding collision probability is calculated to obtain the approximate collision information of the trajectory:

[0067] Assume that the position coordinates of the mobile robot at time t are (x(t), y(t)), the linear velocity at this time is v(t), the angular velocity is w(t), the heading angle is θ(t), the initial heading quaternion is (x(t), y(t), z(t), w(t)), and the sampling interval is set to Δt. Then the robot's position at time t is (x(t), y(t), θ(t));

[0068] Assume that the speed of the mobile robot does not change between two adjacent moments [t, t+1]. When the angular velocity is 0, the robot is in uniform linear motion. Under this assumption, the movement of the robot between two adjacent moments can be regarded as a straight line. Therefore, the position information at the next time 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, (x(t), y(t), θ(t)) is the position information of the robot at time t;

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

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

[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 position 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, Δt is the sampling interval, is the rotation angle of the robot at time t;

[0082] The current position is:

[0083]

[0084] Among them, (x(t), y(t)) is the position coordinate of the robot at time t, (center_x, center_y) is the center position of the robot's rotation trajectory at time t, is the rotation angle of the robot at time t;

[0085] The orientation quaternion at this time 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] Among them, (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 of selecting the best path in step eight includes defining an evaluation function based on speed sampling information, and evaluating the advantages and disadvantages of different paths in combination with environmental factors, robot motion requirements, and safety assessment:

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

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

[0095] Among them, angle(v,w) is the angle evaluation function; obs(v,w) is the collision information of the entire trajectory detected and calculated; dist(v,w) is the distance evaluation function; vel(v,w) is used to evaluate the speed of movement, ensuring efficient and smooth movement under the premise of meeting the dynamic constraints; goal(v,w) further strengthens the robot's guidance to the final target point, and can maintain a continuous approach to the end point even when avoiding obstacles or adjusting the path, thereby enhancing the robustness and practicality of trajectory planning.

[0096] Beneficial effects of the present invention: In the obstacle information acquisition stage, the present invention optimizes the representation shape of obstacles based on the characteristics of narrow passages in the storage environment without significantly increasing the calculation cost and calculation burden, thereby effectively improving the movement speed of the robot in narrow passages. During the movement of the robot, the algorithm solves the problem of error in the position of obstacles obtained by the robot. By setting the initial error polygon and the position collision probability, the safety of the robot's movement is greatly improved. In addition, the algorithm selects the best output in the position space based on the evaluation results of the speed sampling trajectory points based on the execution ability of the two-wheel differential robot in the storage environment, thereby effectively avoiding the situation in which the two-wheel differential robot rubs against obstacles due to the movement ability of the two-wheel differential robot in the traditional obstacle avoidance algorithm. In summary, the present invention not only enhances the obstacle avoidance ability of the two-wheel differential robot, but also improves the operating efficiency in complex storage environments, providing a more reliable solution for robot obstacle avoidance in warehousing logistics.

[0097] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0099] Figure 1 The overall framework diagram of the location obstacle and dynamic window fusion method considering the obstacle representation shape;

[0100] Figure 2The flowchart of the algorithm for setting the position obstacle probability according to the obstacles and robot information in the environment;

[0101] Figure 3 A probability grayscale diagram of a single obstacle in the present invention (the darker the color, the higher the collision probability, and the lighter the color, the lower the collision probability);

[0102] Figure 4 Flowchart of the improved dynamic window method for integrating obstacle probability into the dynamic window algorithm. DETAILED DESCRIPTION

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

[0104] See also Figure 1-4 As shown, the present invention is a warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system, and the specific steps include:

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

[0106] Step 2: Obtain the position error shape polygon according to the change in the obstacle position information as the robot moves away;

[0107] Step 3: Based on the outer contour polygons and position error polygons of the robot and the obstacle, these polygons are combined and calculated by Minkowski sum algorithm to generate a polygon of the obstacle shape including the position error;

[0108] Step 4: According to the representation polygons of the robot and the obstacle, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon;

[0109] Step 5: Set relative speed obstacles and generate position obstacle information based on the relationship between the mass point and the combined polygon;

[0110] Step 6: Set the collision probability of the mobile robot in the warehouse scene according to the position obstacles and the error information size in the environment;

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

[0112] Step 8: Define the evaluation function based on the speed sampling information, combine environmental factors, robot motion requirements and safety assessment, evaluate the advantages and disadvantages of different paths, and select the best path.

[0113] In the above technical solution:

[0114] Step 1: Data collection and polygon construction. The robot collects the outline information of static obstacles (such as shelf goods, etc.) and other robots in the warehouse environment; based on this data, the outer outline polygon of the obstacle is constructed, simplifying the actual warehouse environment into a scene composed of multiple polygons, which facilitates more efficient handling of obstacle avoidance problems in the future.

[0115] Step 2: Position error shape polygon setting. During the movement of the robot, there are two ways to obtain obstacle position information: using sensors and communication transmission. However, in actual warehousing environments, obstacles such as shelves may cause communication delays, and there will also be errors in the scanning results of the sensors. In order to improve the safety of the robot's movement, a position error polygon is set based on the position information errors of other robots and obstacles. Taking into account the different error effects of the robot in the forward direction and the left and right offset directions, the position error polygon is designed to be an ellipse to more accurately reflect the size of the specific error. This elliptical shape can effectively contain most of the position error information, thereby improving the safety of the robot during movement.

[0116] To reduce the amount of calculation, the ellipse is discretized into multiple points and connected into polygons that approximate the ellipse:

[0117]

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

[0119] Step 3: Generate comprehensive representation polygons. 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 the obstacle shape including the position error; for the sake of simplicity, only the outer points of the polygons after the combined calculation are left, and the extraction method is set.

[0120] Since the result of the Minkowski sum is usually composed of multiple points and the initial shape may be a concave polygon, in order to ensure the accuracy of the final result, a concave polygon extraction strategy based on the k-nearest neighbor (k-NN) algorithm is adopted to extract the outermost points and obtain a representative polygon that integrates the position error.

[0121] Step 4: Simplified processing of obstacle avoidance problem. According to the representation polygons of the robot and the obstacle, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon. Assume that the coordinates of the center position of the robot representation polygon are There are n vertices, and the vertex is represented by {P r1 ,P r2 ,......P rn}, the vertex coordinates are (x ri ,y ri ), the center position of the representation polygon of an obstacle is There are m vertices, and the vertices are represented by {P o1 ,P o2 ,......P om}, the coordinates of the vertex are (x oj ,y oj ). Calculate the relative position information of the robot and the obstacle through the center position information of the robot:

[0122]

[0123] Among them, ΔC(x Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x Δri ,y Δri ) are the relative position coordinates of other robots; is the center coordinate of the polygon represented by the robot, (x oj ,y oj ) are the vertex coordinates of the obstacle representation polygon, (x ri ,y ri ) are the vertex coordinates of the polygon represented by other robots.

[0124] The Minkowski Sum is used to calculate the combined result of the polygon vertices representing the robot and the obstacle under the relative position information:

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

[0126] Among them, S is used as the combination result representing the polygon vertices, (xΔri ,y Δri ) is the position coordinate of other robots in relative position, (x Δoj ,y Δoj ) is the relative position coordinate of the obstacle. n is the number of vertices of the polygon representing the other robot, and m is the number of vertices of the polygon representing the obstacle.

[0127] Extract the outermost contour points {P1, P2, ... P k} to describe the final combination result, and obtain an external shape polygon that can contain all Minkowski and results as the combined polygon of the robot and the obstacle.

[0128] At this stage, the center point of the robot is regarded as a mass point and obstacle avoidance is performed; at this point, the problem between the robot and the obstacle is transformed into the obstacle avoidance problem between the mass point and the combined polygon.

[0129] Step 5: Setting of relative speed obstacles and generation of position obstacle information; Setting of relative speed obstacles based on mass points and combined polygons; Obtaining the point set at time τ based on the combined polygons obtained between the robot and the obstacles Create a new closed polygon.

[0130] Calculate the angle of each vertex in the new closed polygon, assuming that the coordinates corresponding to each vertex are (a ix ,a iy ),i∈k, then

[0131]

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

[0133] Get the vertices with the largest and smallest angles and use them as the tangent points to get the tangent line from the origin of the velocity space to the polygon. At time τ, the tangent point divides the polygon into two parts, puts the vertices in the two parts into different point sets, and solves the distance from each vertex to the origin

[0134]

[0135] Among them, d i is the distance between each vertex and the origin, (a ix ,a iy ) are the coordinates corresponding to each vertex, and k is the number of vertices of the new closed polygon.

[0136] After determining which combination the minimum distance belongs to, the corresponding point sets are connected in sequence and used as the information between the two tangent points. The area enclosed by the tangent line and the sequentially connected point sets is the relative speed obstacle.

[0137] For a moving obstacle, obtain its velocity v(v x ,v y ), using the Minkowski sum, the relative speed barrier R is converted into an absolute speed barrier A

[0138] A=R⊕v (6)

[0139] Among them, 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 ) and the planned travel time t, converting the absolute speed obstacle A into the position obstacle L in the position space

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

[0142] Among them, A is the absolute speed obstacle, L is the position obstacle in the position space, p is the position information of the robot itself, and t is the planned movement time of the robot.

[0143] Step 6: Setting the collision probability. According to the positional obstacles and the size of the error information in the environment, set the collision probability of the location area of ​​the mobile robot in the warehouse scene. In order to ensure the high utilization of the location space in the warehouse environment and the stable calculation cost, a fine grid map with small pixels is designed. Specifically, the current position of the robot is taken as the center, and the operating space around it is divided into a series of fine pixels, each pixel representing a tiny area in the environment. Using the acquired obstacle information, calculate the shortest distance from the center of each pixel to the boundary of the positional obstacle. For pixels directly inside the positional obstacle, the corresponding shortest distance is set to 0 to indicate that there is a high risk of collision in this area.

[0144] In order to accurately reflect the change in collision probability, the collision probability of each pixel can be calculated by the following formula based on the standard deviation σ of the error information in the environment:

[0145]

[0146] Among them, p represents the collision probability of a certain pixel point obtained according to a certain obstacle or obstacle information of other robots, σ ​​is the standard deviation of the error information in the environment, and d represents the shortest distance from a certain pixel point to the boundary of the obstacle.

[0147] For each obstacle, a position obstacle set will be formed with the robot. When there are M obstacles, the probability information setting of each obstacle set to the grid map can be calculated, and the total collision probability information of each pixel point is

[0148]

[0149] Among them, p all Represents the collision probability information of a certain pixel under the action of all obstacles in the environment, p j It represents the collision probability information of a certain pixel under the action of a certain obstacle in the environment, and M represents the number of obstacles in the environment.

[0150] Step 7: Generation of movement trajectory and calculation of collision information. Sample from the robot's executable speed range to generate multiple robot movement trajectories. According to the probability information in the position space, calculate the collision information for each trajectory. In the speed space, the robot's linear speed range is [0,v max ], the angular velocity range is [w min ,w max ], we can get the speed space

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

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

[0153] When sampling the velocity, different setting methods are used for linear velocity and angular velocity. For linear velocity, a uniform interval is set according to 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 the linear velocity, and n is the number of samples within the linear velocity range.

[0156] For angular velocity, the increment is calculated differently: for each index j, the increment is expressed as

[0157]

[0158] Among them, Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, and j represents the index information, so that the angular velocity increment has different results when the index changes. q is a set parameter used to adjust the change trend of the increment. A smaller q value will result in a smooth increment area, while a larger q value will result in a larger increment change gap; the result of the linear velocity angular velocity is:

[0159]

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

[0161] The speed combination result is

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

[0163] According to the speed and angular velocity, the moving trajectory within a period of time is calculated. In order to facilitate the calculation of the collision probability on the trajectory, when calculating the trajectory, the moving time is divided into multiple time periods, and the position points reached in each time period are calculated, and then the corresponding collision probability is calculated, so as to obtain the approximate collision information of the trajectory.

[0164] Assume 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 heading angle is θ(t), the initial heading quaternion is (x(t), y(t), z(t), w(t)), and the sampling interval is set to Δt. Then the robot's position at time t is (x(t), y(t), θ(t)).

[0165] Assume that the speed of the mobile robot does not change between two adjacent moments [t, t+1]. When the angular velocity is 0, the robot is in uniform linear motion. Under this assumption, the movement of the robot between two adjacent moments can be regarded as a straight line. Therefore, the position information at the next time 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] Among them, 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, and θ(t+1) is the angle information at time t+1. Δt is the sampling interval, and (x(t), y(t), θ(t)) is the position information of the robot at time t.

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

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

[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, w(t) is the angular velocity of the robot at time t, and (x(t), y(t), θ(t)) is the position 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, Δt is the sampling interval, is the rotation angle of the robot at time t.

[0179] The current position is:

[0180]

[0181] Among them, (x(t), y(t)) is the position coordinate of the robot at time t, (center_x, center_y) is the center position of the robot's rotation trajectory at time t, is the rotation angle of the robot at time t.

[0182] The orientation quaternion at this time 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] Among them, (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] According to the calculated position information, the corresponding pixel point is searched in the grid map storing the position obstacle probability 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: Selection of the best path: Based on the speed sampling information, define the evaluation function, and evaluate the advantages and disadvantages of different paths by combining environmental factors, robot motion requirements, and safety assessment.

[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 an angle evaluation function: it evaluates the angle difference between the calculated trajectory end direction and the expected trajectory direction under the current speed combination of the robot, ensuring the high consistency of the movement direction. obs(v,w) is the collision information of the entire trajectory detected and calculated. According to the settings in the previous step, the collision probability of each trajectory corresponding to the speed sampling can be obtained. dist(v,w) is a distance evaluation function, which evaluates the distance difference between the trajectory end and the expected trajectory, so as to ensure that the robot can move well along the given path; vel(v,w) is used to evaluate the speed of movement, ensuring efficient and smooth movement under the premise of meeting the dynamic constraints; goal(v,w) further strengthens the robot's guidance to the final target point, and even when avoiding obstacles or adjusting the path, it can keep approaching the end point, 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 final motion plan of the robot. The best result is output after comparison and contrast; the optimal decision after this comprehensive evaluation will serve as the execution instruction of the robot to guide it to complete the task safely and efficiently in the actual environment.

[0195] The above contents are merely examples and explanations of the concept of the present invention. The technicians in this technical field may make various modifications or additions to the specific embodiments described or replace them in a similar manner. As long as they do not deviate from the concept of the invention or exceed the scope defined by the claims, they should all fall within the protection scope of the present invention.

Claims

1. An obstacle avoidance algorithm for a warehouse environment suitable for a multi-double-wheel differential robot system, characterized in that: The algorithm includes: Step 1: Collect the external contour information of the robot and obstacles to construct an external contour polygon; Step 2: Obtain the position error shape polygon according to the change in the obstacle position information as the robot moves away; Step 3: Based on the outer contour polygons and position error polygons of the robot and the obstacle, these polygons are combined and calculated by Minkowski sum algorithm to generate a polygon of the obstacle shape including the position error; Step 4: According to the representation polygons of the robot and the obstacle, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon; Step 5: Set relative speed obstacles and generate position obstacle information based on the relationship between the mass point and the combined polygon; Step 6: Set the collision probability of the mobile robot in the warehouse scene according to the position obstacles and the error information size in the environment; Step 7: Sampling is performed from the robot's executable speed range to generate multiple robot movement trajectories, and the collision information corresponding to each trajectory is calculated based on the probability information in the position space; Step 8: Define the evaluation function based on the speed sampling information, combine environmental factors, robot motion requirements and safety assessment, evaluate the advantages and disadvantages of different paths, and select the best path.

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

3. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 1, characterized in that: In the step 4, according to the representation polygons of the robot and the obstacle, the obstacle avoidance of the robot and the obstacle is converted into the obstacle avoidance of the mass point and the combined polygon: Assume that the center coordinates of the robot representation polygon are There are n vertices, and the vertex is represented by {P r1 ,P r2 ,......P rn }, the vertex coordinates are (x ri ,y ri ), the center position of the representation polygon of an obstacle is There are m vertices, and the vertices are represented by {P o1 ,P o2 ,......P om }, the coordinates of the vertex are (x oj ,y oj ); Calculate the relative position information of the robot and obstacles through the center position information of the robot: Among them, ΔC(x Δoj ,y Δoj ) is the relative position coordinate of the obstacle, ΔC(x Δri ,y Δri ) are the relative position coordinates of other robots; is the center coordinate of the polygon represented by the robot, (x oj ,y oj ) are the vertex coordinates of the obstacle representation polygon, (x ri ,y ri ) are the vertex coordinates of the polygons represented by other robots; The Minkowski sum is used to calculate the combined result of the polygon vertices representing the robot and the obstacle under the relative position information: S={(x Δri +x Δoj ,y Δri +y Δoj )∣i∈[1,n],j∈[1,m]} Among them, S is used as the combination result representing the polygon vertices, (x Δri ,y Δri ) is the position coordinate of other robots in relative position, (x Δoj ,y Δoj ) is the coordinate of the obstacle position in relative position; n is the number of vertices of the polygon representing other robots, and m is the number of vertices of the polygon representing the obstacle; Extract the outermost contour points {P1, P2, ... P k } to describe the final combination result, and obtain an external shape polygon that can contain all Minkowski and results as the combined polygon of the robot and the obstacle.

4. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 3, characterized in that: In the step 5: According to the mass points and the combined polygons, the relative speed obstacles are set, and the point set at time τ is obtained according to the combined polygons obtained by the robot and the obstacles. Construct a new closed polygon; Calculate the angle of each vertex in the new closed polygon, assuming that the coordinates corresponding to each vertex are (a ix ,a iy ),i∈k, then: Among them, θ i is the angle corresponding to each vertex, (a ix ,a iy ) are the coordinates corresponding to each vertex, and k is the number of vertices of the new closed polygon; Get the vertices with the largest and smallest angles and use them as tangent points, thereby obtaining the tangent line from the origin of the velocity space to the polygon; At time τ, the tangent point divides the polygon into two parts, puts the vertices in the two parts into different point sets, and solves the distance from each vertex to the origin: Among them, d i is the distance between each vertex and the origin, (a ix ,a iy ) are the coordinates corresponding to 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 set in sequence, and use it as the information between the two tangent points; the area surrounded by the tangent line and the sequentially connected point set is the relative speed obstacle; For a moving obstacle, get its velocity v(v x ,v y ), using the Minkowski sum, the relative speed barrier R is converted into an absolute speed barrier A: Among them, R represents the relative speed obstacle, A represents the 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 ) and the planned moving time t, the absolute speed obstacle A is converted into the position obstacle L in the position space:

5. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 1, characterized in that: The method for obtaining the collision probability in step 6 is: Taking the current position of the robot as the center, the operating space around it is divided into a series of fine pixels. Each pixel represents 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 position obstacle is calculated. For the pixels directly inside the position obstacle, the corresponding shortest distance is set to 0 to indicate that there is a higher collision risk in this area.

6. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 5, characterized in that: According to the standard deviation σ of the error information in the environment, the collision probability of each pixel can be calculated by the following formula: Among them, p represents the collision probability of a certain pixel point obtained based on a certain obstacle or obstacle information of other robots, σ ​​is the standard deviation of the error information in the environment, and d represents the shortest distance from a certain pixel point to the boundary of the position obstacle; For each obstacle, a position obstacle set will be formed with the robot. When M obstacles exist, the probability information setting of each obstacle set to the grid map is calculated, and the total collision probability information of each pixel point is: Among them, p all Represents the collision probability information of a certain pixel under the action of all obstacles in the environment, p j It represents the collision probability information of a certain pixel under the action of a certain obstacle in the environment, and M represents the number of obstacles in the environment.

7. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 1, characterized in that: The generation process of the moving trajectory in step 7 is: In velocity space, the robot’s linear velocity range is [0,v max ], the angular velocity range is [w min ,w max ], we can get the velocity space: S all ={(v,w)|v∈[0,v max ]∩w∈[w min ,w max ]} Among them, S all is the speed space set, (v,w) represents the speed combination; When sampling the velocity, the linear velocity and angular velocity are set in different ways; for the linear velocity: Where Δv represents the uniform increment of linear velocity, L is the length of the overall interval of linear velocity, and n is the number of samples within the linear velocity range; For angular velocity: Wherein, Δw represents the angular velocity increment, n is the number of samples within the angular velocity range, j represents the index information, so that the angular velocity increment has different results when the index changes; q is a set parameter used to adjust the change trend of the increment. A smaller q value will result in a smooth increment area, while a larger q value will result in a larger increment change gap; The result of linear velocity and angular velocity is: v v (i) represents the linear velocity result under the action of sampling number n and uniform linear velocity increment Δv, v w (j) represents the angular velocity result under the action of sampling number and index; The speed combination results are: twist_combinations={(v v (i),v w (j))∣i∈[0,n-1],j∈[0,2n-1]} twist_combinations is the speed combination (v v (i),v w (j)) The combined results of all speeds and angular velocities under the action of the moving trajectory within a period of time are calculated based on the speed and angular velocity.

8. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 7, characterized in that: The calculation process of the collision information is as follows: The moving time is divided into multiple time periods, and the position points reached in each time period are calculated, and then the corresponding collision probability is calculated to obtain the approximate collision information of the trajectory: Assume that the position coordinates of the mobile robot at time t are (x(t), y(t)), the linear velocity at this time is v(t), the angular velocity is w(t), the heading angle is θ(t), the initial heading quaternion is (x(t), y(t), z(t), w(t)), and the sampling interval is set to Δt. Then the robot's position at time t is (x(t), y(t), θ(t)); Assume that the speed of the mobile robot does not change between two adjacent moments [t, t+1]. When the angular velocity is 0, the robot is in uniform linear motion. Under this assumption, the movement of the robot between two adjacent moments can be regarded as a straight line. Therefore, the position information at the next time 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, (x(t), y(t), θ(t)) is the position information of the robot at time t; When the robot's angular velocity and linear velocity are not zero, the motion trajectory is an arc; the arc radius is expressed as: The center position of the rotating arc at this time: 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 position information of the robot at time t; The rotation angle is: w(t) is the angular velocity of the robot at time t, Δt is the sampling interval, is the rotation angle of the robot at time t; The location at this time is: Among them, (x(t), y(t)) is the position coordinate of the robot at time t, (center_x, center_y) is the center position of the robot's rotation trajectory at time t, is the rotation angle of the robot at time t; The orientation quaternion at this time 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)) is the initial orientation quaternion, and γ(t) is the angle calculated based on the quaternion and the rotation angle.

9. The warehouse environment obstacle avoidance algorithm suitable for a multi-double-wheel differential robot system according to claim 1, characterized in that: The method of selecting the best path in step eight includes defining an evaluation function based on speed sampling information, and evaluating the advantages and disadvantages of different paths in combination with environmental factors, robot motion requirements, and safety assessment: F(v,ω)=α*angle(v,w)+β*dist(v,w)+λ*obs(v,w)+μ*vel(v,w) +γ*goal(v,w) Among them, angle(v,w) is the angle evaluation function; obs(v,w) is the collision information of the entire trajectory detected and calculated; dist(v,w) is the path distance evaluation function; vel(v,w) is the motion speed evaluation function; goal(v,w) is the distance evaluation function from the target point.

Images