A Collision Detection Method for Motion Planning of Mobile Robots

Abstract

The present invention discloses a collision detection method for mobile robot motion planning, which belongs to the technical field of mobile robot motion planning. The present invention searches for the vertex of the mobile robot penetrating into an obstacle based on two intersecting polygons, constructs a danger circle inside the obstacle, and then stores the corresponding sampling points, penetration points and danger circles in a data set; then, extracts the sampling point x closest to the random sampling point x from the data set. o And the corresponding danger circle and penetration point; finally, we only need to judge x o Is the coordinate of the corresponding penetration point at x still at x after kinematic transformation? o The corresponding danger circle. As the number of danger circles increases, the robot's C-space danger zone is approached more and more accurately, and the new sampling point can determine its collision state through the danger circle method without going through the conventional complex steps. It can significantly improve the efficiency of collision detection and expand the application capabilities of probabilistic sampling planning methods in the field of mobile robots.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot motion planning, and specifically discloses a collision detection method for mobile robot motion planning. Background Art

[0002] With the continuous improvement of the intelligence level of mobile robots, they have been widely used in scenarios such as home service, scenic area guiding, workshop handling, mobile processing, parts assembly, and driverless. The common characteristics of these application scenarios are large working space, many obstacles in the space, and complex motion paths. Therefore, fast, efficient, and safe motion planning technology is a key factor in improving the intelligence level of mobile robots. The motion planning algorithm based on probabilistic sampling is not limited by the dimension of the robot C-space and can be widely applied to complex scenario planning problems, and it has played a significant application value in the motion planning problem of robotic arms.

[0003] The probabilistic sampling planning algorithm is simple. Compared with methods such as graph search and artificial potential function, this method does not require an accurate representation of the obstacles in the robot C-space. It mainly includes two steps: (1) randomly sampling in the robot C-space and searching for the connection path between the sampling point and the previous sampling point; (2) collision detection of the sampling point and the connection path. From the steps of the probabilistic sampling planning algorithm, it can be seen that for the motion planning problem of large space with many obstacles, the collision detection efficiency is the main factor restricting its wide-scale application. Therefore, the collision detection efficiency in a large space range is an urgent problem to be solved in the field of mobile robots. Summary of the Invention

[0004] Aiming at the defects and improvement requirements of the prior art, the present invention provides a collision detection method for mobile robot motion planning, aiming to improve the collision detection efficiency in a large space range to expand the application ability of the probabilistic sampling planning method in the field of mobile robots. The present invention uses a planar convex polygon to accurately approximate the geometric shapes of the mobile robot and the obstacles. In the steps of the existing probabilistic sampling planning algorithm, a danger circle is constructed using the collision detection results, and then the conventional complex collision detection problem is simplified to a point-circle inclusion test problem, and the point-circle inclusion test is considered the simplest collision detection method. Therefore, the fast collision detection method provided by the present invention can significantly improve the collision detection efficiency and thus greatly improve the calculation speed of the probabilistic sampling method in a large space range.

[0005] To achieve the above object, in a first aspect, the present invention provides a collision detection method for mobile robot motion planning, including the following steps:

[0006] S1, using a planar convex polygon to approximate the geometric shapes of the mobile robot and each obstacle, denoted as figure A and figure B respectively m, and obtain the vertex coordinates of each graph and the outer normal vector of each edge; where m = 1, 2, …, M, and M is the total number of obstacles;

[0007] S2, randomly sample the position of graph A. If the dataset S obs is empty, execute S3; otherwise, execute S4; the dataset S obs is the set of sampling points where graph A and graph B m intersect;

[0008] S3, if graph A and graph B m intersect at the randomly sampled point x, then the mobile robot collides with the obstacle, and for each vertex in graph A, calculate the maximum value of the signed distance from this vertex to all edges of graph B m ; randomly select a value less than zero from these maximum values, and use the vertex of graph A corresponding to this value less than zero as the penetration point, and construct a danger circle with its absolute value as the radius and the penetration point as the center. The signed distance is the inner product of the vector from a vertex of a certain edge of graph B to this vertex and the outer normal vector of the certain edge; then store x, the danger circle, and the penetration point in the dataset S m ; otherwise, no collision occurs; obs

[0009] S4, extract the sampling point x obs closest to the randomly sampled point x from the dataset S o , and extract the corresponding danger circle and penetration point according to x o ; if the coordinate of the penetration point corresponding to x o at x through kinematic transformation is inside the corresponding danger circle, then the mobile robot collides with the obstacle at x; otherwise, execute step S3;

[0010] S5, repeatedly execute steps S2 to S4 until the motion planning is completed.

[0011] Further, in step S4, extracting the sampling point x obs closest to the randomly sampled point x from the dataset S o includes:

[0012]

[0013]

[0014] D(x, x′) = D1(x, x′) + 0.5D2(x, x′)

[0015]

[0016] ​Wherein, (x, y) and (x′, y′) respectively represent the translation vectors at the sampling points x and x', t and t′ respectively represent the rotation angles at the sampling points x and x', D1(x, x′) represents the translation distance between the sampling points x and x', D2(x , x′) represents the angular distance between the sampling points x and x', D(x , x′) represents the distance between the sampling points x and x', x o represents the sampling point in the dataset S obs that is closest to x in distance.

[0017] Furthermore, in the step S3, if the graphics A and B m intersect at the random sampling point x, the corresponding infiltration point is solved by the following method:

[0018]

[0019]

[0020] Wherein, and are respectively the vertex coordinates of the graphics A and B m at the random sampling point x, n a and n b represent the number of vertices; n Bj represents the outer normal vector of the j-th side of the graphic B m , represents the inner product of the vector n Bj and a i -b j , p Ai represents the maximum value of the inner product of the vectors from all vertices of the graphic B to the vertex a m of the graphic A and n i , p Bj represents the infiltration point; a The method for constructing the dangerous circle in the graphic B is: taking p

[0021] The graphic B m b = p a as the center and r b Ai = ||p Ai || as the radius to construct the dangerous circle S b .

[0022] Furthermore, in the step S4, if the following formula is satisfied, it indicates that the coordinate of the infiltration point corresponding to x o at x through kinematic transformation is inside the corresponding dangerous circle:

[0023] ||R xox p a +t xox -pb || ≤ r b

[0024] In the formula, represents the Euclidean distance, and the vertex p b is the center of the dangerous circle S b and r b is the radius of S b . represents that the figure A is rotated from the sampling point x o (x o , y o , t o ) to the rotation transformation matrix of x(x, y, t), represents the translation vector, and the calculation formula is:

[0025]

[0026]

[0027] Furthermore, in the step S1, the i-th side e i of the planar convex polygon connects the i-th vertex v i and the (i + 1)-th vertex v i+1 . According to the vertex coordinates v i (x i , y i ) and v i+1 (x i+1 , y i+1 ), the calculation formula of the outer normal vector direction n i of e i is:

[0028]

[0029] Furthermore, in the step S2, the generation method of the random sampling point x(x, y, t) of the figure A is:

[0030] x = W × Uniform(0, 1), y = H × Uniform(0, 1), t = π × Uniform(-1, 1)

[0031] In the above formula, W and H represent the side lengths of the rectangular constraint space, Uniform(a, b) represents generating random numbers uniformly distributed between a and b, (x, y) represents the translation vector of the mobile robot, and t represents the rotation angle.

[0032] Furthermore, in the step S3, it is determined whether the figure A and the figure B m intersect at the random sampling point x as follows:

[0033] If d A and d BIf both are less than zero, then figure A and figure B m intersect at the random sampling point x, otherwise they do not intersect; where

[0034]

[0035]

[0036] In the formula and are the vertex coordinates of figure A and figure B respectively m at the random sampling point x, n a and n b represent the number of vertices; n Ai represents the outer normal vector of the i-th side of figure A represents the vector n Ai and b j -a i inner product, d Ai represents the vertex a of figure A i to all vertices of figure B m vector and the minimum value of the inner product of n Ai ; d A represents the maximum value of d Ai calculated from the outer normal vectors of all sides of figure A; n Bj represents figure B m the outer normal vector of the j-th side represents the vector n Bj and a i -b j inner product, d Bj represents figure B m vertex b j to all vertices of figure A vector and the minimum value of the inner product of n Bj ; d B represents by figure B m the maximum value in the d Bj calculated from the outer normal vectors of all sides.

[0037] In a second aspect, the present invention provides a collision detection device for mobile robot motion planning, including:

[0038] An acquisition module, configured to approximate the geometric shapes of the mobile robot and each obstacle by a planar convex polygon, denoted as figure A and figure B respectively m , and obtain the vertex coordinates of each figure and the outer normal vector of each side; where m = 1, 2,..., M, and M is the total number of obstacles;

[0039] A sampling module, configured to randomly sample the position of figure A. If the data set S obsIf it is empty, perform the operation of the first detection module; otherwise, perform the operation of the second detection module; the dataset S obs is the set of graphic A and graphic B m for the set of sampling points where they intersect;

[0040] The first detection module is used to determine whether graphic A and graphic B m intersect at the random sampling point x. If so, the mobile robot collides with the obstacle, and for each vertex in graphic A, calculate the maximum value of the signed distance from this vertex to all sides of graphic B m Take any value less than zero from these maximum values, and use the vertex of graphic A corresponding to this value less than zero as the infiltration point. Construct a danger circle with its absolute value as the radius and the infiltration point as the center. The signed distance is the inner product of the vector from a vertex of a certain side of graphic B to this vertex and the outer normal vector of the certain side; then store x, the danger circle, and the infiltration point in the dataset S m ; if not, there is no collision; obs

[0041] The second detection module is used to extract from the dataset S obs the sampling point x closest to the random sampling point x o o and extract the corresponding danger circle and infiltration point according to x o ; if the coordinate of the infiltration point corresponding to x o at x is inside the corresponding danger circle through kinematic transformation, the mobile robot collides with the obstacle at x; otherwise, perform the operation of the first detection module;

[0042] The repetition module is used to repeatedly perform the operations of the sampling module, the first detection module, and the second detection module until the motion planning is completed.

[0043] In a third aspect, the present invention provides a computer-readable storage medium. The computer-readable storage medium includes a stored computer program. When the computer program is run by a processor, it controls the device where the computer-readable storage medium is located to execute the collision detection method for mobile robot motion planning as described in the first aspect.

[0044] Generally speaking, through the above technical solutions conceived by the present invention, the following beneficial effects can be achieved:

[0045] (1) According to two intersecting polygons, the present invention finds the vertices where the mobile robot infiltrates into the obstacle, constructs a danger circle inside the obstacle, and then stores the corresponding sampling points, infiltration points, and danger circles in the dataset; then, extracts the sampling point x closest to the random sampling point x o and the corresponding danger circle and infiltration point from the dataset; then, judge xo Whether the coordinates of the corresponding infiltration point at x after kinematic transformation are still at x o Inside the corresponding danger circle (the danger circle is inside the obstacle, and the infiltration point is the vertex of the mobile robot. If the infiltration point is still inside the danger circle, the mobile robot will definitely collide with the obstacle). Thus, the conventional complex collision detection problem is simplified to a point-circle inclusion test problem, which can significantly improve the collision detection efficiency and thus greatly enhance the calculation speed of the probabilistic sampling method in a large space range.

[0046] (2) The construction of the internal danger circle can be regarded as a spherical approximation of an object with a complex geometric shape, and the method provided by the present invention is an online construction method. That is, the present invention does not require the object to be approximated by a sphere in advance, but constructs the danger circle in real time online during the execution of the algorithm, and constructs the danger circle only when needed (that is, when it is impossible to judge whether the sampling point collides). Therefore, as the number of danger circles increases, the dangerous area of the robot's C-space is approximated more and more accurately, and the collision state of the new sampling point can be judged by the danger circle method without using the conventional complex collision detection method anymore. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a flowchart of the fast collision detection method provided by the embodiment of the present invention;

[0048] FIG. 2(a) and FIG. 2(b) are schematic diagrams of the approximation of a mobile robot and an obstacle polygon provided by the embodiment of the present invention;

[0049] Figure 3 It is a schematic diagram for explaining the sampling of the mobile robot's C-space and the collision detection with the obstacle provided by the embodiment of the present invention;

[0050] FIG. 4(a) and FIG. 4(b) are schematic diagrams for judging the intersection of two objects provided by the embodiment of the present invention;

[0051] Figure 5 It is the internal danger circle of the obstacle obtained by solving from the two intersecting objects provided by the embodiment of the present invention;

[0052] Figure 6 It is a schematic diagram of the fast collision detection criterion provided by the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0053] In order to make the purpose, system composition, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. 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. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0054] In the present invention, terms such as "first" and "second" in the present invention and the accompanying drawings (if any) are used to distinguish similar objects and do not necessarily describe a specific order or sequence.

[0055] Referring to Figure 1 and in combination with Figures 2(a) to 6 , the present invention provides a collision detection method for mobile robot motion planning, including operations S1 to S6.

[0056] Operation S1: Approximate the geometric shapes of the mobile robot and each obstacle with planar convex polygons, denoted as figure A and figure B respectively m , and obtain the vertex coordinates of each figure and the outer normal vector of each edge; where m = 1, 2,..., M, and M is the total number of obstacles.

[0057] As shown in FIGS. 2(a) and 2(b), taking a triangle and a rectangle as examples, the polygon vertices are stored in counterclockwise order. Specifically, the i-th edge e i of the polygon connects the i-th vertex v i and the (i + 1)-th vertex v i+1 . According to the vertex coordinates v i (x i , y i ) and v i+1 (x i+1 , y i+1 ), the calculation formula for the outer normal vector direction n i of e i is:

[0058]

[0059] Operation S2: Randomly sample the position of figure A. If the data set S obs is empty, execute step S3; otherwise, execute step S4; the data set S obs is the set of sampled points where figure A and figure B m intersect.

[0060] As Figure 3 shown, this environment is the feasible space for the mobile robot, and the global coordinate system is defined at the lower left corner of the rectangular constraint space. The generation method of the random sampling point x (x, y, t) of the mobile robot is:

[0061] x = W × Uniform(0, 1), y = H × Uniform(0, 1), t = π × Uniform(-1, 1)

[0062] In the above formula, W and H represent the side lengths of the rectangular constraint space, Uniform(a, b) represents generating a random number uniformly distributed between a and b, (x, y) represents the translation vector of the mobile robot, and t represents the rotation angle.

[0063] From the positive kinematic transformation of the random sampling point x, the position and shape of the mobile robot in the workspace can be known. Specifically, the vertex coordinates v i ' of the robot and the outer normal vector n i ' are calculated by the formula:

[0064]

[0065] Operation S3: If the graphics A and B m intersect at the random sampling point x, then the mobile robot collides with the obstacle, and for each vertex in the graphics A, calculate the maximum value of the signed distance from this vertex to all the edges of the graphics B m . Take any value less than zero from these maximum values, and use the vertex of the graphics A corresponding to this value less than zero as the infiltration point. Construct a danger circle with its absolute value as the radius and the infiltration point as the center. The signed distance is the inner product of the vector from a vertex of a certain edge of the graphics B to the said vertex and the outer normal vector of the said certain edge; then store x, the danger circle, and the infiltration point in the dataset S m ; Otherwise, no collision occurs. obs

[0066] Taking the mobile robot at the random sampling point x as an example, the conventional collision detection method needs to sequentially determine whether there is a collision between the mobile robot and a certain obstacle. Specifically, it is judged whether the graphics A and B m intersect at the random sampling point x in the following way:

[0067]

[0068]

[0069] In the formula, and are the vertex coordinates of the graphics A and B respectively m at the random sampling point x, n a and n b represent the number of vertices; n Ai represents the outer normal vector of the i-th edge of the graphics A, represents the vector n Ai and b j -a i The inner product of, d Ai represents the vector from the vertex a i of the graphics A to all the vertices of the graphics B m and n​Ai The minimum value of the inner product, d A represents d calculated from the outer normal vectors of all the sides of figure A Ai The maximum value in; n Bj represents figure B m The outer normal vector of the j-th side of represents the vector n Bj and a i -b j The inner product of, d Bj represents figure B m Vertex b j The vectors from all vertices of figure A to n Bj The minimum value of the inner product of, d B represents the d calculated from m all the outer normal vectors of figure B Bj The maximum value in.

[0070] If d A and d B are both less than zero, then figure A and figure B m intersect at the random sampling point x, otherwise they do not intersect.

[0071] Taking the intersection test of the two rectangles shown in Fig. 4(a) and Fig. 4(b) as an example, the solution process of d B1 is as follows:

[0072]

[0073]

[0074]

[0075] In Fig. 4(a), d B1 is greater than zero, so figure A and figure B m do not intersect. While in Fig. 4(b), d B1 is less than zero, so it is necessary to calculate d B2 , d B3 , d B4 , and obtain d B = max(d B1 , d B2 , d B3 , d B4 ), to determine whether figure A and figure B m intersect.

[0076] Furthermore, if figure A and figure B m intersect at the random sampling point x, it means that the mobile robot collides with the obstacle, and the penetration point is solved by the following method:

[0077]

[0078]

[0079] In the formula, and are the vertex coordinates of the respective vertices of the graphics A and B m at the random sampling point x, and n a and n b represent the number of vertices; n Bj represents the outer normal vector of the j-th side of the graphic B m , represents the vector n Bj and a i -b j inner product, p Ai represents the maximum value of the inner product of the vectors from all vertices of the graphic B to the vertex a m of the graphic A and n i ; p Bj represents the infiltration point, that is, the vertex a a corresponding to p Ai less than zero i .

[0080] The method for constructing the dangerous circle in the graphic B is: with p m = p b as the center and r a = p b as the radius to construct the dangerous circle S Ai . b . Figure 5 Shows the constructed dangerous circle S b , and it can be seen from the above calculation that S b is completely inside the obstacle B.

[0081] Taking the intersecting rectangular robot A and rectangular obstacle B shown in Fig. 4(b) as an example, the solution process of p A1 is as follows:

[0082]

[0083]

[0084] p A1 = max(p A1-1 , p A1-2 , p A1-3 , p A1-4 )

[0085] It can be seen from Fig. 4(b) that p A1 is less than zero, that is, the vertex a1 is inside the obstacle B, so p a = a1.

[0086] Operation S4, extract from the dataset S obs the sampling point x' that is closest to the random sampling point x o , and based on x' o extract the corresponding danger circle and infiltration point; if the infiltration point corresponding to x' o has coordinates at x that are inside the corresponding danger circle after kinematic transformation, then the mobile robot collides with the obstacle at x; otherwise, execute step S3.

[0087] In this embodiment, operation S4 includes sub-operations S41 and S42.

[0088] In sub-operation S41, extract the sampling point x' that is closest to the random sampling point x from the dataset in the following manner o :

[0089]

[0090]

[0091] D(x, x') = D1(x, x') + 0.5D2(x, x')

[0092]

[0093] where (x, y) and (x', y') respectively represent the translation vectors at the sampling points x and x', t and t' respectively represent the rotation angles at the sampling points x and x', D1(x, x') represents the translation distance between the sampling points x and x', D2(x , x') represents the angular distance between the sampling points x and x', D(x , x') represents the distance between the sampling points x and x', x o represents the sampling point in the dataset S obs that is closest to x.

[0094] In sub-operation S42, determine whether the coordinates of the infiltration point corresponding to x o after kinematic transformation at x are still inside the corresponding danger circle.

[0095] As Figure 6 shown, if the following formula is satisfied, it means that the coordinates of the infiltration point corresponding to x o after kinematic transformation at x are inside the corresponding danger circle:

[0096]

[0097] where represents the Euclidean distance, and the vertex p b is the center of the danger circle S b , and r b is Sb The radius of indicates that the graph A consists of sampling points x o (x o , y o , t o ) is the rotation transformation matrix for moving to x(x, y, t), represents the translation vector, and the calculation formula is:

[0098]

[0099] Furthermore, if the dangerous circle method is not sufficient to determine whether the new sampling point collides, it means that the coverage area of the dangerous circle is not large enough. Then, the result of the traditional collision detection method is used to expand its coverage area in real time online. For the specific steps, refer to the specific implementation manner of determining whether graph A and graph B m intersect at the random sampling point x, which will not be elaborated here.

[0100] Operation S5, repeat steps S2 to S4 until the motion planning is completed.

[0101] It should be noted that the internal dangerous circles of the obstacles constructed in the present invention are constructed in real time online during the execution of the probabilistic sampling planning algorithm; for new sampling points, the nearest points are selected from the recorded sampling points that have collided, and the corresponding internal dangerous circles of the obstacles and the infiltration points of the mobile robot are extracted according to the nearest collision points. Then, it is judged whether the infiltration points are still inside the dangerous circles at the new sampling points through kinematic transformation; if the dangerous circle method is not sufficient to determine whether the new sampling point collides, it means that the coverage area of the dangerous circle is not large enough, and the result of the traditional collision detection method is used to expand its coverage area in real time online.

[0102] Therefore, as the number of dangerous circles increases, the dangerous area of the robot C-space is approximated more and more accurately, and the collision state of the new sampling point can be judged through step S4 without going through the conventional and complex step S3.

[0103] Those skilled in the art can easily understand that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A collision detection method for motion planning of a mobile robot, characterized in that, It includes the following steps: S1. Approximately represent the geometric shapes of the mobile robot and each obstacle with planar convex polygons, denoted as Shape A and Shape B respectively m , and obtain the vertex coordinates of each shape and the outer normal vector of each edge; where m = 1, 2, …, M, and M is the total number of obstacles S2. Randomly sample the position of graphic A. If the dataset S obs is empty, execute step S3; otherwise, execute step S4. The dataset S obs is the set of sampling points where graphic A and graphic B m intersect. S3. If the graphic A and the graphic B m intersect at the random sampling point x, the mobile robot collides with the obstacle, and for each vertex in the graphic A, calculate the maximum value of the signed distances from the vertex to all the edges of the graphic B m Select any value less than zero from the maximum values, and use the vertex of the graphic A corresponding to the value less than zero as the infiltration point. Construct a danger circle with the absolute value of the value as the radius and the infiltration point as the center. The signed distance is the inner product of the vector from the vertex of a certain edge of the graphic B to the vertex and the outer normal vector of the certain edge; then store x, the danger circle, and the infiltration point in the dataset S m ; Otherwise, no collision occurs; obs ​ S4. Extract the sampling point x that is closest to the random sampling point x from the dataset S obs and extract the corresponding danger circle and infiltration point according to x o . If the coordinates of the infiltration point corresponding to x o at x through kinematic transformation are inside the corresponding danger circle, the mobile robot collides with the obstacle at x; otherwise, execute step S3; o ​ S5. Repeat steps S2 to S4 until the motion planning is completed.

2. The collision detection method for motion planning of a mobile robot according to claim 1, wherein In the step S4, the sampling point x obs closest to the random sampling point x is extracted from the data set S obs and o , It includes: D(x,x′) = D1(x,x′) + 0.5D2(x,x′) Where, (x, y) and (x′, y′) respectively represent the translation vectors at the sampling points x and x', t and t′ respectively represent the rotation angles at the sampling points x and x', D1(x, x′) represents the translation distance between the sampling points x and x', D2(x, x′) represents the angular distance between the sampling points x and x', D(x, x′) represents the distance between the sampling points x and x', and x o represents the sampling point in the dataset S obs that is closest to x in distance.

3. The collision detection method for motion planning of a mobile robot according to claim 1 or 2, characterized in that, In step S3, if graphic A and graphic B m intersect at a random sampling point x, the corresponding infiltration point is solved as follows: In the formula, and are the vertex coordinates of graphic A and graphic B respectively m at the random sampling point x, n a and n b represent the number of vertices; n Bj represents graphic B m the outer normal vector of the j-th edge, represents the vector n Bj the inner product of the vector n i and a j -b Ai represents graphic B m the maximum value of the inner product of the vectors from all vertices of graphic B to the vertex a i of graphic A and n Bj ; p a represents the infiltration point; Graph B m The construction method of the dangerous circle in m is as follows: taking p b = p a as the center and r b = ||p Ai || as the radius to construct the dangerous circle S b ; among them, the vertex p b is the center of the dangerous circle S b and r b is the radius of S b .

4. The collision detection method for motion planning of a mobile robot according to claim 3, characterized in that, In the step S4, if the following formula is satisfied, it indicates that x o The coordinates of the corresponding infiltration point at x through kinematic transformation are inside the corresponding danger circle: Wherein, |||| represents the Euclidean distance, and vertex p b is the center of the dangerous circle S b , r b is the radius of S b . represents the rotation transformation matrix of the graphic A from the sampling point x o (x o , y o , t o ) moving to x(x, y, t), and represents the translation vector, and the calculation formula is:

5. The collision detection method for motion planning of a mobile robot according to claim 1, characterized in that, In the step S1, for the i-th side e of the planar convex polygon i connect the i-th vertex v i and the (i + 1)-th vertex v i+1 , according to the vertex coordinates v i (x i , y i ) and v i+1 (x i+1 , y i+1 ), the calculation formula for the outer normal vector direction n i of e i is as follows:

6. The collision detection method for motion planning of a mobile robot according to claim 1, characterized in that, In step S2, the random sampling point x(x, y, t) of figure A is generated as follows: x = W × Uniform(0,1), y = H × Uniform(0,1), t = π × Uniform(-1,1) In the above formula, W and H represent the side lengths of the rectangular constraint space, Uniform(a,b) represents generating a random number uniformly distributed between a and b, (x, y) represents the translation vector of the mobile robot, and t represents the rotation angle.

7. The collision detection method for motion planning of a mobile robot according to claim 1, characterized in that In the step S3, the following method is used to determine whether the graph A and the graph B m intersect at the random sampling point x: If d A and d B are both less than zero, then graphic A and graphic B m intersect at the random sampling point x; otherwise, they do not intersect. Among them, Wherein, and are the vertex coordinates of graphic A and graphic B respectively m at the random sampling point x, and n a and n b represent the number of vertices; n Ai represents the outer normal vector of the i-th edge of graphic A, represents the vector n Ai and b j -a i is the inner product of d, Ai represents the vertex a of graphic A i to all vertices of graphic B m is the minimum value of the inner product of the vector and n Ai ; d A represents the maximum value of d Ai calculated from the outer normal vectors of all edges of graphic A; n Bj represents the outer normal vector of the j-th edge of graphic B, m represents the vector n Bj and a i -b j is the inner product of d, Bj represents the vertex b of graphic B m to all vertices of graphic A j is the minimum value of the inner product of the vector and n Bj ; d B represents the maximum value of d m calculated from the outer normal vectors of all edges of graphic B Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj Bj [[ 8. A collision detection device for the motion planning of a mobile robot, characterized in that, It includes: An acquisition module, which is used to approximate the geometric shapes of the mobile robot and each obstacle by plane convex polygons, denoted as figure A and figure B respectively m , and obtain the vertex coordinates of each figure and the outer normal vector of each edge; where m = 1, 2, …, M, and M is the total number of obstacles Sampling module, for randomly sampling the position of graphic A. If the data set S obs is empty, perform the operations of the first detection module; otherwise, perform the operations of the second detection module. The data set S obs is the set of sampling points where graphic A and graphic B m intersect; The first detection module is used to determine whether figure A and figure B m intersect at the random sampling point x. If so, the mobile robot collides with the obstacle, and for each vertex in figure A, calculate the maximum value of the signed distance from the vertex to all sides of figure B m Take any value less than zero from the maximum values, and use the vertex of figure A corresponding to the value less than zero as the infiltration point. Construct a danger circle with the absolute value of the value as the radius and the infiltration point as the center. The signed distance is the inner product of the vector from the vertex of a certain side of figure B to the vertex and the outer normal vector of the certain side; then store x, the danger circle, and the infiltration point in the dataset S m ; If not, no collision occurs; obs ​ The second detection module is configured to extract, from the dataset S obs the sampling point x that is closest to the randomly sampled point x o , and extract the corresponding danger circle and infiltration point according to x o ; if the coordinates of the infiltration point corresponding to x o at x through kinematic transformation are inside the corresponding danger circle, then the mobile robot collides with the obstacle at x; otherwise, perform the operation of the first detection module; A repetition module, configured to repeatedly execute the operations of the sampling module, the first detection module, and the second detection module until the motion planning is completed.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored computer program, wherein when the computer program is run by a processor, it controls the device where the computer-readable storage medium is located to execute the collision detection method for mobile robot motion planning according to any one of claims 1 to 7.

Images