Dynamic safety area generation method and system of mobile robot

Abstract

The invention provides a method and a system for generating a dynamic safety area of a mobile robot, and relates to the technical field of control perception of automatic guided vehicles and autonomous mobile robots. According to the embodiment of the invention, the motion state can be divided into straight movement, external rotation and internal rotation based on the rotation radius; front-end compensation and rear-end compensation are carried out on a vehicle body to obtain a body area, compensation is carried out based on the specific volume of the vehicle body, the body area further extends in the advancing direction to determine a dynamic adjustment area related to the speed, the dynamic adjustment area is connected with the body area, and therefore a reasonable collision detection area is obtained. And the requirement of collision detection can be met.

Description

Technical Field

[0001] The present invention relates to the field of automatic guided vehicles and autonomous mobile robot control perception technology, and in particular to a method and system for generating a dynamic safety area of ​​a mobile robot. Background Art

[0002] The existing positioning and obstacle avoidance radars of automatic guided vehicles (AGV) and autonomous mobile robots (AMR) can adopt single-line radars or safety radars.

[0003] Among them, safety radar generally provides the function of dividing the safety area, which can meet the basic needs of collision detection. However, the areas supported by safety radar are generally edited by graphics, which is less flexible. At the same time, the cost of safety radar is relatively high, which has certain constraints for users who have collision detection needs and want to use general single-line radar. Using general single-line radar, the function of obstacle detection based on point cloud can be realized through software.

[0004] Safety areas are generally divided into deceleration areas and parking areas. Currently, the division of the two areas is relatively simple, mostly using rectangles or polygons. The existing method of generating safety areas is simply to extend or divide them based on the vehicle body, which cannot meet the needs of collision detection. Summary of the invention

[0005] To solve the above problems, an embodiment of the present invention provides a method for generating a dynamic safety area of ​​a mobile robot, the method comprising: obtaining the current linear velocity and angular velocity; determining the corresponding rotation radius according to the linear velocity and the angular velocity, and determining the current motion state according to the rotation radius; the motion state comprises straight-ahead movement, external rotation and internal rotation; the rotation radius of the external rotation is greater than half of the width of the vehicle body, and the rotation radius of the internal rotation is less than or equal to half of the width of the vehicle body; if it is a straight-ahead movement state, determining a rectangular dynamic adjustment area according to the linear velocity and a preset time length, and obtaining a dynamic safety area according to the main body area and the dynamic adjustment area to the safe area; if it is an external rotation movement state, the front-end compensation and the rear-end compensation are performed on the vehicle body to obtain a fan-shaped main body area, and the dynamic adjustment area of ​​the fan-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset time length, and the safe area is obtained according to the main body area and the dynamic adjustment area; if it is an internal rotation movement state, the front-end compensation and the rear-end compensation are performed on the vehicle body to obtain a bow-shaped main body area, and the dynamic adjustment area of ​​the bow-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset time length, and the safe area is obtained according to the main body area and the dynamic adjustment area; the arc of the bow is larger than a semicircle.

[0006] The dynamic safety area generation method for a mobile robot provided in an embodiment of the present invention can divide the motion state into straight movement, external rotation, and internal rotation based on the rotation radius, perform front-end compensation and rear-end compensation on the vehicle body to obtain the main body area, and thus perform compensation based on the specific volume of the vehicle body, and further extend the main body area along the forward direction to determine the speed-related dynamic adjustment area, which is connected to the above-mentioned main body area to obtain a reasonable collision detection area, which can meet the needs of collision detection.

[0007] Optionally, the front-end compensation and rear-end compensation for the vehicle body are performed to obtain a fan-shaped main body area, including: determining an inner arc line with the center of the outer rotation circle as the center of the circle and a line connecting the center of the outer rotation circle and the midpoint of the inner side of the rectangle circumscribed by the vehicle body as a first radius; determining an outer arc line with the center of the outer rotation circle as the center of the circle and a line connecting the center of the outer rotation circle and the vertices of the outer side of the rectangle circumscribed by the vehicle body as a second radius; determining the fan-shaped front-end compensation line with the second radius passing through the front vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line; determining the fan-shaped rear-end compensation line with the second radius passing through the rear vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line; the fan-shaped front-end compensation line and the fan-shaped rear-end compensation line constitute a fan-shaped main body area.

[0008] In the embodiment of the present invention, the front and rear ends of the vehicle body in the outwardly rotating state are compensated to obtain a fan-shaped main body area, so that the collision area is compensated based on the specific volume of the vehicle body.

[0009] Optionally, the dynamic adjustment area extending along the rotation direction of the main area of ​​the fan-shaped ring is determined according to the angular velocity and the preset time, including: determining the rotation angle according to the angular velocity and the preset time; a closed figure determined by the radius of the rear end of the dynamic adjustment area, the radius of the front end of the dynamic adjustment area, the inner arc line, and the outer arc line is the dynamic adjustment area; the radius of the rear end of the dynamic adjustment area is the second radius passing through the front vertex of the inner edge, and the angle between the radius of the front end of the dynamic adjustment area and the radius of the rear end of the dynamic adjustment area is the rotation angle.

[0010] In the embodiment of the present invention, a specific range of a dynamic adjustment area obtained by extending the sector ring-shaped main body area along the rotation direction is defined, and the dynamic adjustment area is an extension of the sector ring-shaped main body area.

[0011] Optionally, the front-end compensation and rear-end compensation of the vehicle body are performed to obtain a bow-shaped main body area, including: determining an outer arc line with the center of the inner rotation circle as the center of the circle and a line connecting the center of the inner rotation circle and the vertices of the outer side of the rectangle circumscribed by the vehicle body as the radius; determining the bow-shaped main body area with the outer arc line and the extension line of the inner side of the rectangle circumscribed by the vehicle body.

[0012] In the embodiment of the present invention, the front and rear ends of the vehicle body in the internal rotation state are compensated to obtain a bow-shaped body area, so that the collision area is compensated based on the specific volume of the vehicle body, and the calculation method is simplified under the premise of meeting the collision detection requirements.

[0013] Optionally, the dynamic adjustment area extending along the rotation direction of the bow-shaped main body area determined according to the angular velocity and the preset time duration includes: determining the rotation angle according to the angular velocity and the preset time duration; a closed figure determined by the rear edge, the front edge, and the outer arc line is the dynamic adjustment area; the rear edge is the extension line of the inner side edge of the body circumscribed rectangle, the front edge is the line connecting the midpoint of the inner side edge of the body circumscribed rectangle and the outer arc line, and the angle between the rear edge and the front edge is the rotation angle.

[0014] In the embodiment of the present invention, a specific range of a dynamic adjustment area obtained by extending the arcuate main body area along the rotation direction is defined. The dynamic adjustment area can be regarded as an area obtained by rotating the arcuate main body area.

[0015] Optionally, if it is in a state of self-rotation, the method also includes: determining an outer arc with the geometric center of the body circumscribed rectangle as the center of the circle and the line connecting the center of the circle and the vertices of the body circumscribed rectangle as the radius, and a closed figure determined by the outer arc, the inner side of the body circumscribed rectangle and the extension line of the outer side as the main body area; determining the rotation angle according to the angular velocity and the preset time length; a closed figure determined by the rear edge, the front edge and the outer arc is the dynamic adjustment area of ​​the front of the vehicle; the rear edge is the inner side of the body circumscribed rectangle, and the front edge is the line connecting the midpoint of the inner side of the body circumscribed rectangle and the outer arc; a closed figure determined by the rear edge, the front edge and the outer arc is the dynamic adjustment area of ​​the rear of the vehicle; the rear edge is the outer side of the body circumscribed rectangle, and the front edge is the line connecting the midpoint of the outer side of the body circumscribed rectangle and the outer arc; the angle between the rear edge and the front edge is the rotation angle.

[0016] The embodiment of the present invention provides a method for generating a safe area for the boundary condition of rotation, thereby determining a variable safe area.

[0017] Optionally, the method further comprises: expanding each boundary of the safety area to obtain an expanded area, and the expanded area can be adjusted according to the actual environment.

[0018] The embodiment of the present invention provides an expansion method for a safety area, so as to expand the safety area in a manner parallel to the boundary.

[0019] Optionally, each boundary of the safety area is expanded separately to obtain an expanded area, including: if it is an external rotation movement state, the inner arc line of the safety area expands inward, the outer arc line expands outward, the front straight line edge expands forward in a direction perpendicular to the front straight line edge, and the rear straight line edge expands backward in a direction perpendicular to the rear straight line edge; if it is an internal rotation movement state, the outer arc line of the safety area expands outward, the front straight line edge expands forward in a direction perpendicular to the front straight line edge, and the rear straight line edge expands backward in a direction perpendicular to the rear straight line edge.

[0020] The embodiment of the present invention provides a specific expansion method for external rotation and internal rotation, so as to expand the safety area in a manner parallel to the boundary.

[0021] Optionally, each boundary of the safety area is expanded separately to obtain an expanded area, including: if it is in a rotational motion state, the outer arc line of the safety area expands outward, the front straight line edge expands forward in a direction perpendicular to the front straight line edge, and the rear straight line edge expands backward in a direction perpendicular to the rear straight line edge.

[0022] The embodiment of the present invention provides a specific expansion method for rotation, so as to expand the safety area in a manner parallel to the boundary.

[0023] An embodiment of the present invention provides a dynamic safety zone generation system for a mobile robot, which is used in the dynamic safety zone generation method for the mobile robot.

[0024] The dynamic safety zone generation system for a mobile robot provided by an embodiment of the present invention can achieve the same technical effect as the dynamic safety zone generation method for a mobile robot described above. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0026] Figure 1 A schematic flow chart of a method for generating a dynamic safety zone for a mobile robot provided in an embodiment of the present invention;

[0027] Figure 2A schematic diagram of a collision detection area during linear motion provided by an embodiment of the present invention;

[0028] Figure 3 A schematic diagram of a collision detection area during external rotation motion provided by an embodiment of the present invention;

[0029] Figure 4 A schematic diagram of a collision detection area during an internal rotation movement provided by an embodiment of the present invention;

[0030] Figure 5 A schematic diagram of a collision detection area in a critical state of external rotation and internal rotation provided by an embodiment of the present invention;

[0031] Figure 6 A schematic diagram of a collision detection area in which the rotation center of a vehicle body is located at the geometric center of the vehicle body provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0032] In order to make the above-mentioned objects, features and advantages of the present invention more clearly understood, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0033] The embodiment of the present invention proposes a new dynamic safety zone generation method, which is applied to mobile robots such as AGV and AMR. The method can divide the motion state into three states based on speed and angular velocity: straight, external rotation, and internal rotation, generate dynamic safety zones respectively, and dynamically adjust the size of the safety zone according to the linear velocity and angular velocity.

[0034] Some parameters in the embodiments of the present invention are described as follows: body width: W, body length: L, linear velocity: v, angular velocity: w, center line of the dynamic adjustment area: s (the straight line is the center line of the dynamic adjustment area, and the rotating line is the arc length of the center line of the dynamic adjustment area).

[0035] The embodiment of the present invention proposes a method for generating a dynamic safety zone, which is different from a general static collision zone. For the application of the above method in the embodiment of the present invention, it is only necessary to regard the static collision zone as the vehicle body, so the subsequent content does not further discuss the static safety zone. At the same time, the linear velocity and angular velocity equal to zero or close to zero are regarded as a static state, which belongs to a straight-ahead state, and the dynamic adjustment area is close to zero, so it is continuous with the static collision zone.

[0036] Figure 1 The following is a schematic flow chart of a method for generating a dynamic safety zone of a mobile robot in an embodiment of the present invention. The method includes:

[0037] S102, obtaining the current linear velocity and angular velocity.

[0038] The body of the mobile robot can be in a straight line or a turning state during the movement. The linear velocity and angular velocity can be collected in real time during the movement. The angular velocity can be the angular velocity of the center of the body rotating around the center of the circle. In this embodiment, the body can be in the form of a simplified body circumscribed rectangle.

[0039] S104, determining a corresponding rotation radius according to the linear velocity and angular velocity, and determining a current motion state according to the rotation radius.

[0040] The motion states include straight-ahead, external rotation and internal rotation. The rotation radius of external rotation is greater than half of the vehicle width, and the rotation radius of internal rotation is less than or equal to half of the vehicle width;

[0041] The embodiment of the present invention divides the driving state into three states according to the linear velocity and angular velocity: straight driving, external rotation, and internal rotation. The rotation radius is calculated as follows: r = v / w

[0042] Straight state: r>r l (angular velocity is not zero) or the angular velocity is less than a certain minimum value; generally, the case of extremely large rotation radius is also equated with straight movement (for example, r l =100)

[0043] External rotation state: L / 2 <r<r l

[0044] Internal rotation state: 0≤r≤L / 2

[0045] Among them, the three states of vehicle operation are determined by the value of the rotation radius. Straight travel refers to when the angular velocity of rotation is zero or the rotation radius is very large. At this time, the short-distance operation of the mobile machine can be considered as straight-line travel; external rotation refers to when the center of rotation is outside the vehicle body, that is, when the rotation radius is greater than half of the width of the vehicle body. At this time, after one circle of movement, there is an uncovered circular area near the center of the circle; internal rotation refers to when the center of rotation is inside the vehicle body, located on the line connecting the midpoint of the side of the vehicle body and the center of the vehicle body. At this time, after one circle of movement, the center of rotation is completely covered by the vehicle body. The two sides of the vehicle body parallel to the direction of travel are side edges.

[0046] S106: If the vehicle is in a straight-line motion state, a rectangular dynamic adjustment area is determined according to the linear speed and the preset duration, and a safety area is obtained according to the main body area and the dynamic adjustment area.

[0047] The main body area is the area occupied by the vehicle body, which can be in the form of a rectangle inscribed outside the vehicle body. The dynamic adjustment area is in front of the movement direction of the above-mentioned main body area and is connected to the main body area. The preset duration can be flexibly determined based on the actual application scenario. The length of the dynamic adjustment area can be calculated based on the preset duration and the linear speed, and the length changes with the change of the linear speed. The width of the dynamic adjustment area is consistent with the width of the main body area.

[0048] The main area and the dynamic adjustment area are continuous and together constitute a safe area.

[0049] S108, if it is an external rotation motion state, the front-end compensation and the rear-end compensation are performed on the vehicle body to obtain the fan-shaped main body area, and the dynamic adjustment area of ​​the fan-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset duration, and the safety area is obtained according to the above-mentioned main body area and the dynamic adjustment area.

[0050] Optionally, the main body area can be based on the circumscribed rectangle of the vehicle body. Considering that the calculation basis is based on the center of the vehicle body, only the center point of the central axis of the vehicle body is colinear with the travel route. The center point is selected as the starting point of the dynamic area travel route. Therefore, it is necessary to start from the center point and perform area compensation for the front and rear ends based on the volume of the vehicle body. In the external rotation state, the main body area is a fan ring. The inner and outer arcs of the fan ring have a common center, and the endpoints of the two arcs on the same side are on the same radius. The closed area enclosed by the two inner and outer arcs and the two radii passing through the endpoints on the same side of the two arcs is the main body area of ​​the fan ring.

[0051] Specifically, the front-end compensation and the back-end compensation can be performed in the following manner to obtain the main body area:

[0052] First, the inner arc is determined by taking the center of the outer rotation as the center of the circle and the line connecting the center of the outer rotation and the midpoint of the inner side of the body circumscribed rectangle as the first radius; the outer arc is determined by taking the center of the outer rotation as the center of the circle and the line connecting the center of the outer rotation and the vertex of the outer side of the body circumscribed rectangle as the second radius. The inner side refers to the side of the body circumscribed rectangle close to the center of the circle, and the outer side refers to the side of the body circumscribed rectangle away from the center of the circle.

[0053] Secondly, the fan ring of the front end compensation is determined by the second radius passing through the front vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line; the fan ring of the rear end compensation is determined by the second radius passing through the rear vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line. The front vertex refers to the intersection of the side in front of the moving direction of the body circumscribed rectangle and the inner side, and the rear vertex refers to the intersection of the side in the rear of the moving direction of the body circumscribed rectangle and the inner side.

[0054] Finally, the fan ring body area is composed of the front-end compensation fan ring and the rear-end compensation fan ring.

[0055] A dynamic adjustment area needs to be set in front of the main body area, so that the dynamic adjustment area can be changed with the speed change to avoid collision. Specifically, the dynamic adjustment area can be determined in the following way:

[0056] First, the rotation angle is determined according to the current angular velocity and the preset duration.

[0057] The preset duration can be flexibly determined based on the actual application scenario, the rotation angle can be calculated based on the preset duration and the angular velocity, and the main body area is extended along the rotation direction by the rotation angle to obtain a dynamic adjustment area. The shape of the dynamic adjustment area is similar to that of the main body area.

[0058] Secondly, a closed figure defined by the radius of the rear end of the dynamic adjustment area, the radius of the front end of the dynamic adjustment area, the inner arc line, and the outer arc line is the dynamic adjustment area. The radius of the rear end of the dynamic adjustment area is the second radius passing through the front vertex of the inner side, and the angle between the radius of the front end of the dynamic adjustment area and the radius of the rear end of the dynamic adjustment area is the rotation angle.

[0059] S110, if it is an inner rotation motion state, the front-end compensation and the rear-end compensation are performed on the vehicle body to obtain the bow-shaped main body area, and the dynamic adjustment area of ​​the bow-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset duration, and the safety area is obtained according to the above-mentioned main body area and the dynamic adjustment area.

[0060] The arc refers to the area surrounded by the chord and its corresponding arc. In this embodiment, the arc of the arc is larger than the semicircle, that is, the major arc. The arc of the major arc is larger than the semicircle, and the area of ​​the main body of the arc is larger than the area of ​​the semicircle. Specifically, the main body of the arc can be obtained by performing front-end compensation and rear-end compensation in the following manner:

[0061] First, the outer arc is determined with the center of the inner rotation as the center and the line connecting the center of the inner rotation and the outer side vertex of the body circumscribed rectangle as the radius, and the main body area of ​​the bow is determined with the outer arc and the extension line of the inner side of the body circumscribed rectangle. The extension line of the inner side of the body circumscribed rectangle is the chord, and the closed figure enclosed by the chord and the outer arc is the main body area of ​​the bow.

[0062] Specifically, the dynamic adjustment area may be determined in the following manner:

[0063] First, the rotation angle is determined according to the current angular velocity and the preset duration. Secondly, the closed figure determined by the rear edge, the front edge, and the outer arc is the dynamic adjustment area. Among them, the rear edge is the extension line of the inner side of the body's circumscribed rectangle, the front edge is the line connecting the midpoint of the inner side of the body's circumscribed rectangle and the outer arc, and the angle between the rear edge and the front edge is the above-mentioned rotation angle.

[0064] The dynamic adjustment area is in a fan-shaped shape, and the dynamic adjustment area is connected to the main body area.

[0065] The dynamic safety area generation method for a mobile robot provided in an embodiment of the present invention can divide the motion state into straight movement, external rotation, and internal rotation based on the rotation radius, perform front-end compensation and rear-end compensation on the vehicle body to obtain the main body area, and thus perform compensation based on the specific volume of the vehicle body, and further extend the main body area along the forward direction to determine the speed-related dynamic adjustment area, which is connected to the above-mentioned main body area to obtain a reasonable collision detection area, which can meet the needs of collision detection.

[0066] Rotation with the center of rotation located at the geometric center of the vehicle body (abbreviated as self-rotation) is a common mode of movement in the daily use of AGVs and AMRs. It occurs frequently in movement, so a supplementary method for self-rotation is proposed here.

[0067] If the state is a self-rotation state, the above method further includes:

[0068] First, the outer arc is determined with the geometric center of the body circumscribed rectangle as the center of the circle and the line connecting the center of the circle and the vertex of the body circumscribed rectangle as the radius, and the closed figure determined by the outer arc, the inner side and the outer side of the body circumscribed rectangle is the body area. The inner side and the outer side are at the same distance from the center of the circle.

[0069] Secondly, the rotation angle is determined according to the angular velocity and the preset duration.

[0070] Then, the closed figure determined by the rear edge, front edge, and outer arc is the dynamic adjustment area of ​​the front of the vehicle. Among them, the rear edge is the inner side of the body circumscribed rectangle, and the front edge is the line connecting the midpoint of the inner side of the body circumscribed rectangle and the outer arc. The closed figure determined by the rear edge, front edge, and outer arc is the dynamic adjustment area of ​​the rear of the vehicle. Among them, the rear edge is the outer side of the body circumscribed rectangle, and the front edge is the line connecting the midpoint of the outer side of the body circumscribed rectangle and the outer arc. The angle between the rear edge and the front edge is the rotation angle.

[0071] The following describes the specific content of the method for generating a dynamic safety zone in each motion state. The safety zone can also be called a collision detection zone.

[0072] Figure 2The collision detection area includes a static collision area A and a dynamic adjustment area B. In this embodiment, the static collision area can be regarded as the vehicle body ( Figure 2 middle rectangular shaded area).

[0073] Set the time parameter of the dynamic adjustment area to Δt, then the straight-line dynamic adjustment area s=v*Δt. The dynamic adjustment area is a rectangle with a length of s and a width of W. The final collision detection area is a rectangular area with a length of s+L and a width of W.

[0074] Among them, Δt is an adjustable time parameter, which can be adjusted according to the actual deceleration of the vehicle. Generally, the larger the deceleration, the smaller the parameter, and the smaller the deceleration, the larger the parameter.

[0075] Figure 3 The schematic diagram of the collision detection area during the external rotation movement is shown. The method for generating the collision detection area during the external rotation movement is as follows:

[0076] Set the dynamic adjustment area time parameter to Δt

[0077] Radius of rotation: r = v / w

[0078] The radius r of the arc from the center of rotation to the left s =rW / 2

[0079] The rotation radius of the arc from the rotation center to the right:

[0080] Sector angle of front and rear compensation areas

[0081] Left arc length

[0082] Right arc length

[0083] The front and rear compensations for external rotation are proposed because the arc length calculated by this method starts from the center point of the vehicle body. This is different from straight-line driving, because when driving in a straight line, the travel route and the center point of the front edge of the vehicle body are collinear, so the front boundary of the vehicle body is used as the starting point of the dynamic area in straight-line driving. However, in the external rotation route, only the center point of the vehicle body's central axis is collinear with the travel route, so the center point should be selected as the starting point of the dynamic area travel route. Therefore, it is necessary to start with the center point and perform regional compensation for the front and rear ends based on the volume of the vehicle body.

[0084] Figure 3 Shows The corresponding front-end compensation area a1 and the back-end compensation area a2, θ corresponds to the dynamic adjustment area B. Figure 3The right arc marked by the curly brackets at a1, the area surrounded by the corresponding left arc and the radius r1 and r2 on both sides is the front compensation area; the right arc marked by the curly brackets at a2, the area surrounded by the corresponding left arc and the radius on both sides is the rear compensation area; the right arc marked by the curly brackets at B, the area surrounded by the corresponding left arc and the radius on both sides is the front compensation area. The front compensation area, the rear compensation area and the dynamic adjustment area are continuous, forming a continuous fan ring.

[0085] There are two compensation methods that can be used in this embodiment. One is to use the above-mentioned fan-shaped area compensation, and the angle of the fan-shaped area is as follows: Figure 3 middle As shown; another method is that the termination boundary of the compensation area is parallel to the vehicle body.

[0086] Figure 4 The schematic diagram of the collision detection area during the inner rotation movement is shown. The method for generating the inner rotation driving collision detection area is as follows:

[0087] Radius of rotation: r = v / w (at this time 0 ≤ r ≤ W / 2)

[0088] The compensation of the front and rear ends in the internal rotation state can be considered as π

[0089] The radius of the arc from the center of rotation to the right:

[0090] It should be pointed out that in Figure 4 The left side of the vehicle body is not the diameter of the circle passing through the outer vertex of the vehicle body. In fact, the center of the circle is on the line connecting the midpoint of the left side and the geometric center of the vehicle body. The front compensation area a1 and the rear compensation area a2 are fan-shaped areas whose vertices are not at the center of the circle. Figure 4 The arc marked by the curly brackets at a1, and the area enclosed by the extension line L1 of the left side of the vehicle body and the diameter R1 of the circle are the front-end compensation area; the arc marked by the curly brackets at a2, and the area enclosed by the extension line L1 of the left side of the vehicle body and the diameter R1 of the circle are the rear-end compensation area.

[0091] exist Figure 4 The front boundary of the dynamic adjustment area is br1, and br2 and br3 are also shown. The starting point of br1 is the midpoint of the left side, the starting point of br2 is the rotation center, and the starting point of br3 is the tangent point of the circle with the rotation center as the center and passing through the geometric center of the vehicle body. The end points of the three are the same and are all on the circle passing through the outer vertex of the vehicle body. The boundary finally selected in this embodiment is the above br1. Figure 4 The arc marked by the curly brackets at θ, and the area enclosed by the above-mentioned br1 and the extension line L1 of the left side of the vehicle body are the dynamic adjustment area.

[0092] Figure 5A schematic diagram of the collision detection area in the critical state of external rotation and internal rotation is shown. Figure 5 This is a critical situation, that is, the center of rotation is exactly at the middle point of the left vehicle boundary line, which can be included as a critical value when calculating the safety area. Figure 4 The situations shown are considered uniformly.

[0093] Figure 6 A schematic diagram of the collision detection area is shown in which the rotation center of the vehicle body is located at the geometric center of the vehicle body. Figure 6 It is another critical state, that is, the center of rotation is exactly located at the geometric center of the plane shape of the vehicle body.

[0094] Rotation with the center of rotation located at the geometric center of the vehicle body (abbreviated as self-rotation) is a common mode of movement in the daily use of AGVs and AMRs. It occurs frequently in movement, so a supplementary method for self-rotation is proposed here.

[0095] When the rotation radius (the distance between the rotation center and the geometric center) r is less than a certain minimum value, the vehicle can be approximately considered to be in a state of self-rotation. For example, the minimum value is 0.05 meters, and the value can be adjusted according to the size of the vehicle and the accuracy of the safety zone. In general, the value is proportional to the size of the vehicle and inversely proportional to the accuracy of the safety zone.

[0096] In the autorotation state, the safety area consists of three parts, namely the vehicle body area, the predicted safety area of ​​the front of the vehicle and the predicted safety area of ​​the rear of the vehicle. Both predicted safety areas are fan-shaped areas. Since the rear of the vehicle moves symmetrically with the front of the vehicle during the autorotation, in order to ensure a more accurate division of the safety area during the autorotation, a symmetrical safety area division method is adopted between the rear of the vehicle and the front of the vehicle. This method is different from the method in which the safety area of ​​the right part of the inner rotation is a complete fan-shaped area.

[0097] like Figure 6 As shown, the predicted safety area at the front of the vehicle and the predicted safety area at the rear of the vehicle are angles The corresponding fan-shaped area is the arc-shaped area adjacent to the front and rear edges of the body rectangle. Figure 6 The area marked with diagonal lines in the middle) is obtained by combining

[0098] In this embodiment, a method for generating an expansion area is also designed for the above-mentioned safety area. The expansion area is an area obtained by expanding the periphery of the collision area in a manner parallel to the boundary. Based on this, the above-mentioned method also includes: expanding each boundary of the above-mentioned safety area respectively to obtain an expansion area, and the expansion area can be adjusted according to the actual environment.

[0099] For example, for a straight-line driving area, the expansion area is a new rectangular area generated outside the original rectangular area based on the collision coefficient. The expansion coefficients can be set separately for the front, back, left and right of the expansion process.

[0100] If it is in an external rotation state, the inner arc line of the safety area expands inward, the outer arc line expands outward, the front straight line side expands forward in a direction perpendicular to the front straight line side, and the rear straight line side expands backward in a direction perpendicular to the rear straight line side.

[0101] For external rotation, based on the current fan-shaped area (strictly speaking, it should be a fan-shaped area formed by digging out a fan-shaped area with a smaller radius from a fan-shaped area with a larger radius), the front and back are straight edges, and the left and right are arc edges. They are expanded outward respectively, and then the area after expansion is obtained as the expanded area.

[0102] If it is in an internal rotation state, the outer arc of the safety area expands outward, the front straight line side expands forward in a direction perpendicular to the front straight line side, and the rear straight line side expands backward in a direction perpendicular to the rear straight line side.

[0103] For internal rotation, there are only the front and back straight edges and the large arc on the right side. The expansion is the same as the external rotation. Since there is no small arc on the left side, the left side is not expanded.

[0104] If it is in a state of rotation, the outer arc of the safety area expands outward, the front straight line side expands forward in a direction perpendicular to the front straight line side, and the rear straight line side expands backward in a direction perpendicular to the rear straight line side.

[0105] For rotation, the outer arcs of the front and rear of the vehicle expand outward, and the straight edges of the front and rear sides expand forward and backward in vertical directions respectively.

[0106] In the above-described process of the embodiment of the present invention, the vehicle body turns left, and the principles of turning right and turning left and the division of the safety area are consistent. In general operation, due to the small turning radius during self-rotation or internal rotation, the corresponding linear speed is also very small or equal to zero. Therefore, in the actual movement process, if the vehicle body adopts the form of self-rotation or internal rotation when turning, it is often necessary to slow down a very small speed when going straight or directly stop and then rotate; when the external rotation method is adopted, a rotation radius can be determined according to the current vehicle speed, and then the vehicle body can turn as needed while the straight-line running speed remains unchanged.

[0107] The embodiment of the present invention illustrates the role of the safety area division in practice through two implementation methods, namely, turning in the form of self-rotation and turning in the form of external rotation without changing the linear speed. These two situations are also the most commonly used working conditions in operation. The method proposed in the invention can be applied to operating conditions with different linear speeds and angular velocities. For example, when external rotation is used, the turning radius cannot meet the current speed requirement, and the speed needs to be reduced to meet the turning radius requirement, and then the vehicle can be operated in the form of external rotation. They will not be listed here one by one.

[0108] (1) The vehicle body rotates in a U-shaped pattern (turning is done by self-rotation)

[0109] Constant speed driving stage: the vehicle drives at a constant speed according to the set speed, and the safety area is fixed;

[0110] Linear deceleration stage: the vehicle body slows down from the uniform speed stage to zero linear speed, and the safety area decreases linearly with the speed;

[0111] Rotation stage: The size of the sector-shaped safety area is determined based on the size of the rotation angular velocity. During the rotation process, the angular velocity increases from zero to a constant value and then decreases to zero, so the sector-shaped safety area changes accordingly.

[0112] Straight-line acceleration stage: The vehicle accelerates from a straight-line speed of zero to the maximum driving speed. This situation is exactly the opposite of the straight-line deceleration stage, and the safety area increases linearly with the speed.

[0113] (2) The vehicle body performs a U-shaped rotation (turning is performed in an outward rotation form)

[0114] Constant speed driving stage: the vehicle drives at a constant speed according to the set speed, and the safety area is fixed;

[0115] Turning deceleration phase: If the turning radius does not match the linear speed, there will be a deceleration phase, which does not occur during the driving phase. The deceleration phase generates a safety area based on the real-time linear speed and angular velocity;

[0116] Turning stage: Generate a safe area based on the angular velocity of rotation;

[0117] Turning acceleration phase: If the turning radius does not match the straight-line speed, the acceleration phase generates a safety zone based on the real-time linear speed and angular velocity.

[0118] The embodiment of the present invention proposes a variable safety zone method based on speed and angular velocity, and proposes a dead zone control strategy based on speed and angular velocity, so as to achieve the continuity and solvability of the entire set of equations.

[0119] The embodiment of the present invention proposes a method for dividing and segmenting an area based on the critical state of vehicle body rotation, and divides the rotation based on the vehicle body into external rotation (the rotation center of the vehicle body is located outside the vehicle body) and internal rotation (the rotation center of the vehicle body is located inside the vehicle body); a front and rear end compensation method for safety area division is proposed to compensate for the collision area based on the specific volume of the vehicle body; an approximate method for the safety area of ​​internal rotation is proposed to simplify the calculation method under the premise of meeting the collision detection requirements; and a safety area generation method based on rotation is proposed.

[0120] The embodiment of the present invention proposes a method for generating an expansion region based on such a safety region.

[0121] An embodiment of the present invention provides a dynamic safety zone generation system for a mobile robot, which is used to execute the dynamic safety zone generation method for a mobile robot.

[0122] The embodiment of the present invention also provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, each process of the above-mentioned embodiment of the method for generating a dynamic safety area of ​​a mobile robot is implemented, and the same technical effect can be achieved. To avoid repetition, it is not repeated here. The computer-readable storage medium is, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0123] Of course, those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the control device through a computer, and the program can be stored in a computer-readable storage medium. When the program is executed, it may include the processes of the above-mentioned method embodiments, wherein the storage medium may be a memory, a disk, an optical disk, etc.

[0124] In this article, relational terms such as first and second, etc. are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprise", "include" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the statement "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0125] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0126] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for generating a dynamic safety zone of a mobile robot, characterized in that: The method comprises: Get the current linear velocity and angular velocity; Determine a corresponding rotation radius according to the linear velocity and the angular velocity, and determine a current motion state according to the rotation radius; the motion state includes straight travel, external rotation and internal rotation; the rotation radius of the external rotation is greater than half of the width of the vehicle body, and the rotation radius of the internal rotation is less than or equal to half of the width of the vehicle body; If it is a straight-line motion state, a rectangular dynamic adjustment area is determined according to the linear speed and the preset duration, and a safety area is obtained according to the main body area and the dynamic adjustment area; If it is in an external rotation state, the front end compensation and the rear end compensation of the vehicle body are performed to obtain a fan-shaped main body area, and the dynamic adjustment area of ​​the fan-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset time, and the safety area is obtained according to the main body area and the dynamic adjustment area; If it is in an internal rotation state, the front-end compensation and the rear-end compensation are performed on the vehicle body to obtain a bow-shaped main body area, and the dynamic adjustment area of ​​the bow-shaped main body area extending along the rotation direction is determined according to the angular velocity and the preset duration, and the safety area is obtained according to the main body area and the dynamic adjustment area; the arc of the bow is larger than a semicircle.

2. The method according to claim 1, characterized in that The front end compensation and the rear end compensation of the vehicle body are performed to obtain a sector ring-shaped main body area, including: The inner arc is determined by taking the center of the outer rotation as the center of the circle and the line connecting the center of the outer rotation and the midpoint of the inner side of the rectangle inscribed outside the vehicle body as the first radius; the outer arc is determined by taking the center of the outer rotation as the center of the circle and the line connecting the center of the outer rotation and the vertex of the outer side of the rectangle inscribed outside the vehicle body as the second radius; The fan ring of front-end compensation is determined by the second radius passing through the front vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line; the fan ring of rear-end compensation is determined by the second radius passing through the rear vertex of the inner side, the second radius passing through the midpoint of the inner side, the inner arc line, and the outer arc line; The front-end compensation fan ring and the rear-end compensation fan ring form a main body area of ​​the fan ring.

3. The method according to claim 2, characterized in that The method of determining the dynamic adjustment area of ​​the main body area of ​​the fan ring extending along the rotation direction according to the angular velocity and the preset time length includes: Determine the rotation angle according to the angular velocity and the preset duration; A closed figure defined by the radius of the rear end of the dynamic adjustment area, the radius of the front end of the dynamic adjustment area, the inner arc line, and the outer arc line is the dynamic adjustment area; The radius of the rear end of the dynamic adjustment area is the second radius passing through the front vertex of the inner side, and the angle between the radius of the front end of the dynamic adjustment area and the radius of the rear end of the dynamic adjustment area is the rotation angle.

4. The method according to claim 1, characterized in that: The method of performing front end compensation and rear end compensation on the vehicle body to obtain an arched body area includes: The outer arc is determined with the center of the inner rotation as the center and the line connecting the center of the inner rotation and the outer side vertices of the body circumscribed rectangle as the radius, and the bow-shaped main body area is determined with the outer arc and the extension line of the inner side of the body circumscribed rectangle.

5. The method according to claim 4, characterized in that The method of determining the dynamic adjustment area of ​​the arcuate body area extending along the rotation direction according to the angular velocity and the preset duration includes: Determine the rotation angle according to the angular velocity and the preset duration; The closed figure defined by the rear edge, the front edge and the outer arc line is the dynamic adjustment area; The rear side is the extension line of the inner side of the body circumscribed rectangle, the front side is the line connecting the midpoint of the inner side of the body circumscribed rectangle and the outer arc line, and the angle between the rear side and the front side is the rotation angle.

6. The method according to claim 1, characterized in that If it is in the state of self-rotation, the method further comprises: An outer arc is determined with the geometric center of the body circumscribed rectangle as the center of the circle and the line connecting the center of the circle and the vertices of the body circumscribed rectangle as the radius, and a closed figure determined by the outer arc, the inner side edge of the body circumscribed rectangle and the extension line of the outer side edge is the body area; Determine the rotation angle according to the angular velocity and the preset duration; The closed figure defined by the rear edge, the front edge, and the outer arc is the dynamic adjustment area of ​​the vehicle head; the rear edge is the inner side of the rectangle circumscribing the vehicle body, and the front edge is the line connecting the midpoint of the inner side of the rectangle circumscribing the vehicle body and the outer arc; The closed figure defined by the rear edge, the front edge, and the outer arc is the dynamic adjustment area of ​​the rear of the vehicle; the rear edge is the outer edge of the rectangle circumscribing the vehicle body, and the front edge is the line connecting the midpoint of the outer edge of the rectangle circumscribing the vehicle body and the outer arc; The angle between the rear edge and the front edge is the rotation angle.

7. The method according to any one of claims 1 to 6, characterized in that: The method further comprises: Each boundary of the safety area is expanded respectively to obtain an expanded area, and the expanded area can be adjusted according to the actual environment.

8. The method according to claim 7, characterized in that The step of expanding each boundary of the safety area to obtain an expanded area includes: If it is in an external rotation state, the inner arc of the safety area expands inward, the outer arc expands outward, the front straight line expands forward in a direction perpendicular to the front straight line, and the rear straight line expands backward in a direction perpendicular to the rear straight line; If it is in the internal rotation movement state, the outer arc line of the safety area expands outward, the front straight line side expands forward in a direction perpendicular to the front straight line side, and the rear straight line side expands backward in a direction perpendicular to the rear straight line side.

9. The method according to claim 7, characterized in that: The step of expanding each boundary of the safety area to obtain an expanded area includes: If it is in a state of self-rotation, the outer arc of the safety area expands outward, the front straight line side expands forward in a direction perpendicular to the front straight line side, and the rear straight line side expands backward in a direction perpendicular to the rear straight line side.

10. A dynamic safety zone generation system for a mobile robot, characterized in that: A method for generating a dynamic safety area for a mobile robot used to execute any one of claims 1-9.

Images