A path planning method for a non-regular shape mobile robot based on RRT

Abstract

The application provides a path planning method for a non-regular shape mobile robot based on RRT, comprising the following steps: random path generation: in the random path process, a workspace network and a mobile robot shape kernel matrix need to be constructed, and the kinematics constraint of the mobile robot is determined; firstly, the initial path node is respectively set as the initial state and the target state of the mobile robot in the workspace, and two exploration trees are simultaneously generated; then, a random sampling point is generated, and the feasibility of the sampling point as the path node is judged according to the kinematics constraint, the node state and the path feasibility; the growth of the two exploration trees is realized through the continuous generation of the random sampling point and the feasibility judgment of the path node, until the two exploration trees are connected with each other under the kinematics constraint condition, and the random path is obtained; path optimization: the path optimization method based on the node state transition on the random path is adopted, the path optimization is realized through the cyclic iteration of the node state, and the shortest path with stable convergence is obtained.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of mobile robot path planning, and particularly relates to a path planning method for a mobile robot with irregular shape based on RRT. BACKGROUND

[0002] Path planning is a basic problem in the field of mobile robot research, and the main goal is to find a suitable trajectory in the working environment to effectively avoid obstacles. With the in-depth research of path planning, in order to meet the needs of different working environments, many planning algorithms have been proposed, such as Dijkstra, A*, cellular decomposition method, artificial potential field method, evolutionary algorithm, etc. Rapidly-exploring Random Tree (RRT) is a kind of sampling-based path planning algorithm, which is suitable for solving path planning problems in high-dimensional space and complex constraints. The basic idea is to search for the target point by a step in the form of generating random points, effectively avoid obstacles, and avoid path falling into local minimum value, which shows high efficiency and strong path search ability.

[0003] Path optimization is an important direction of RRT research, and RRT* is the main improvement of RRT algorithm. The basic idea is to increase the number of generated random points and optimize the connection mode of nodes (i.e. parent node selection and rewiring) during the growth of the exploring random tree, so as to improve the rationality of the path. However, in the case of kinematic constraints, the state of the robot in the working space includes position coordinates and direction angle. With the increase of sampling points, a large amount of calculation is required to repeatedly calculate whether the sampling point meets the kinematic constraints of the robot and compare the rationality with the original path node, which is very time-consuming and seriously restricts the efficiency of path planning. On the other hand, the planned path is connected by sampling points, and since the sampling points are random, the path obtained each time is also random in the same working space, that is, the planned path does not converge. SUMMARY

[0004] In view of the above technical problems, one of the purposes of one embodiment of the present application proposes a path planning method for irregular shape mobile robots based on RRT, which grows two exploration trees only under the kinematic conditions of the robot, and effectively improves the exploration efficiency of the initial path without considering path optimization during the growth process; one of the purposes of one embodiment of the present application proposes a path node state transition method under the kinematic constraint condition, which can optimize the random path to the shortest path; one of the purposes of one embodiment of the present application is that the path node state transition is only for the nodes on the random path, and the sampling nodes on the non-path are not operated, which greatly reduces the optimization calculation workload, and the optimized path has good convergence. One of the purposes of one embodiment of the present application is to describe the shape characteristics of the robot by constructing a kernel matrix, and to analyze the interaction between the shape of the robot and the surrounding obstacles by convolution calculation, so that the proposed method can be applied to path planning of mobile robots of any shape.

[0005] Note that the description of these purposes does not hinder the existence of other purposes. One embodiment of the present application does not need to realize all the above purposes. The purposes other than the above purposes can be extracted from the description, drawings and claims.

[0006] The present application proposes a path planning method for irregular shape mobile robots based on RRT, which is different from the RRT* algorithm, and is a fast random path planning algorithm, and the nodes of the random path are subjected to state transition under the condition of kinematic constraint, so as to obtain a stable and convergent shortest path.

[0007] The present application uniformly divides the working space into a network structure, describes the shape characteristics of the robot by using a kernel matrix, and describes the shape characteristics of the robot at different azimuth angles by kernel matrix rotation, considers the interaction between the shape of the robot and the shape of the obstacle in the working space, and obtains whether the state of the robot in the working space (x, y, θ) is feasible through convolution calculation; in the random path generation, two exploration trees are generated from the initial state and the target state, the path nodes are explored by using bidirectional RRT, the new nodes only need to satisfy the kinematic characteristics of the robot and the (x, y, θ) state feasible two conditions, the two exploration trees generate path nodes in opposite directions at the same time, and a connection under strict kinematic constraint is proposed to find the initial random path to the target at the fastest speed; for the nodes on the random path, a node state transition method satisfying the kinematic constraint is proposed, and the initial random path gradually converges to the shortest path through iterative calculation.

[0008] The present application achieves the above technical purposes through the following technical means.

[0009] A path planning method for irregular shape mobile robots based on RRT, comprising the following steps:

[0010] Step S1, random path generation: in the random path process, the workspace network and the mobile robot shape kernel matrix need to be constructed, the kinematics constraint of the mobile robot is determined, first, the initial state S0 and the target state S g of the mobile robot in the workspace are respectively taken as the initial path node, and two exploration trees are generated at the same time; then, random sampling points are generated, and the feasibility of the sampling points as path nodes is judged according to the kinematics constraint, node state and path feasibility; the growth of the two exploration trees is realized through continuous generation of random sampling points and feasibility judgment of path nodes, until the two exploration trees are connected to each other under the kinematics constraint condition, and the random path is obtained;

[0011] Step S2, path optimization: the path optimization method based on the state transition of the nodes on the random path is adopted, and the path optimization is realized through the cyclic iteration of the node state.

[0012] In the above scheme, the step of constructing the workspace network is specifically:

[0013] The workspace of the mobile robot is uniformly divided into a network structure along the x direction and the y direction, defined as T, and the network T is divided by a regular shape;

[0014] Each node T(x, y) of the network T includes two states of free space and obstacle occupation, the node function value of the free space is set to T(x, y) = 0, and the node function value of the obstacle occupation area is set to T(x, y) = 1;

[0015] For a non-regular workspace, the area outside the workspace is set as an obstacle T(x, y) = 1, and a regular workspace network T is established.

[0016] In the above scheme, the step of constructing the robot shape kernel matrix is specifically:

[0017] The area occupied by the mobile robot is expanded into a regular rectangular or square area and uniformly divided to form a kernel matrix W, the form of the network is the same as that of the workspace network T, and the robot shape kernel matrix W is placed in the workspace network T, and the node positions are one-to-one corresponding;

[0018] Each element of the kernel matrix W includes two states of free space and robot occupation, the element value of the free space is set to W θ (m, n) = 0, and the element value of the robot occupation is set to W θ (m, n) = 1;

[0019] When the mobile robot is at different direction angles, the kernel matrix W is obtained by rotating around the rear wheel center coordinate (x, y), and the element value of the free space is still set to W θ(m, n) = 0, the element value of the mobile robot occupying is set as W θ (m, n) = 1.

[0020] In the above scheme, the requirement of the kinematic constraint of the mobile robot is specifically:

[0021] Suppose the state of the mobile robot in the workspace is S = (x, y, θ), according to the Ackermann steering constraint condition, the front wheel steering angle of the robot is Constrained by the mechanical structure, Therefore, the turning radius ρ of the robot cannot be smaller than its minimum value, that is, Where L is the front-rear wheel distance, is the maximum front wheel steering angle.

[0022] In the above scheme, the step of constructing the initial random path is specifically:

[0023] From the initial state S0 and the target state S g Two exploration trees are established at the same time, which are defined as FTree and BTree respectively, which are essentially a set of path nodes, the first node of FTree is the initial state S0 = (x0, y0, θ0) of the robot, and the first node of BTree is the target state S g = (x g , y g , θ g ) of the robot; the random path generation includes three processes: random sampling point generation, path node connection and two exploration tree connection.

[0024] Further, the step of generating the random sampling point is specifically:

[0025] Set the sampling probability a and the step length l, 0 < a < 100%, and l is a normal number;

[0026] The growth of FTree selects the target state S g = (x g , y g , θ g ) as the growth direction with a probability of a, and grows in a random direction with a probability of (1-a), and defines the randomly generated point as S r = (x r , y r , θ r );

[0027] The extension exploration step length is l, and the sampling point is set as S s = (x s , y s , θ s );

[0028] The growth method of B-trees differs from that of FT-trees in that: on the one hand, B-trees choose to grow in the direction of the initial state S0 = (x0, y0, θ0) with probability a, and grow in a random direction with probability (1-a); on the other hand, the robot moves in the opposite direction, i.e., it reverses.

[0029] Furthermore, the connection steps for the path nodes are as follows:

[0030] Find the distance S from the existing nodes in the FTree. r The nearest node S p As the parent node, along S p S r The directional exploration step size is l, and the number of sampling points generated is S. s =(x s y s θ s ), and then perform S p →S s The feasibility assessment of a path includes three aspects: kinematic constraints, node state assessment, and path feasibility assessment.

[0031] Furthermore, the kinematic constraint determination specifically refers to: if the sampling point S s If the robot can reach the area, proceed to the next step; otherwise, S s If the kinematic constraints are not met, resampling is required.

[0032] The node state determination is specifically based on sampling point S. s The state of the robot sampling point S is calculated. s kernel matrix W θs (x s y s ), connect the workspace network T with the sampling points S s kernel matrix W θs Perform convolution calculation, h (xs，ys，θs) =T*W θs Where "*" is the convolution operator, if h (xs，ys，θs) =0 indicates sampling point S s If the current state is feasible, proceed to the next step; otherwise, resample.

[0033] The feasibility assessment of the path specifically involves: establishing S p →S s The path is divided into multiple sub-paths, and the node state S of each sub-path is calculated. 11 =(x 11 y 11 θ 11 ), S 12 =(x 12 y12 θ 12 Then, the node state judgment method is used to determine S. 11 and S 12 Is it feasible? If both are feasible, then select sampling point S. s Add FTree and define S s The parent node is S p Otherwise, resample.

[0034] Furthermore, the steps for connecting the two exploration trees are as follows:

[0035] Through continuous sampling, FTree and BTree will grow towards each other, meaning the distance between nodes will shorten.

[0036] Define a distance threshold ε0, set to 3~4ρ. min ;

[0037] Calculate the distance ε between the nearest nodes on the FTree and BTree. If ε > ε0, continue sampling; if ε < ε0, determine the connection between the two exploration trees. The method for determining the connection between the two exploration trees is as follows:

[0038] Assume node S Fm and S Bn For the nearest node on FTree and BTree, according to S Fm and S Bn In each state, construct their respective minimum turning radius circles;

[0039] Find the internal and external common tangents of two circles with the smallest turning radius. If the common tangents exist and satisfy S... Fm and S Bn If the direction angle is determined, then the FTree and BTree can be connected to each other under kinematic constraints, resulting in a connection between the initial state S0 and the target state S. g A random path, defined as P rand ={S0,S F1 ,…,S Fm ,S Bn ,…,S B1 ,S g Otherwise, continue sampling until the FTree and BTree can be connected to each other.

[0040] Furthermore, the specific steps of the path optimization method based on node state transition are as follows:

[0041] From initial state S0 to target state S g By sequentially selecting three consecutive path nodes to perform path node transfer calculations, a path optimization is completed, resulting in a new optimized path.

[0042] The path node transfer calculation is specifically: assuming that an initially generated random path P rand There are three consecutive nodes S1, S2 and S3;

[0043] First, two minimum turning radius circles of the nodes S1 and S3 are calculated;

[0044] Second, according to the direction angles of S1 and S3, the outer common tangent or the inner common tangent of the two minimum turning radius circles is calculated;

[0045] Then, the common tangent center S2' is found, and the path node connection method is used to determine the feasibility of the path from S1→S2'→S3, if feasible, the path node is transferred from S2 to S2', otherwise, S2 remains unchanged;

[0046] From the initial state S0 to the target state S g The above path optimization process is repeated, and the initial random path gradually converges to the shortest path.

[0047] Compared with the prior art, the beneficial effects of the present application are: according to one mode of the present application, two exploration trees are grown only under the kinematic conditions of the robot, and in the growth process, the path optimization problem is not considered, which effectively improves the exploration efficiency of the initial path; according to one mode of the present application, under the kinematic constraint condition, a path node state transfer method is proposed, which can optimize the random path to the shortest path; according to one mode of the present application, the path node state transfer is only for the nodes on the random path, and the sampling nodes on the non-path are not operated, which greatly reduces the optimization calculation workload, and the optimized path has good convergence. According to one mode of the present application, the shape features of the robot are described by constructing a kernel matrix, and the interaction between the shape of the robot and the surrounding obstacles is analyzed by convolution calculation, and the proposed method can be applied to path planning of mobile robots of any shape.

[0048] Note that the description of these effects does not hinder the existence of other effects. One mode of the present application does not necessarily have all the above effects. Effects other than the above can be clearly seen and extracted from the description, drawings, claims, etc. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a schematic diagram of the workspace division of the robot of an embodiment of the present application, wherein Figure 1 (a) is a schematic diagram of a regular workspace, Figure 1 (b) is a schematic diagram of an irregular workspace.

[0050] Figure 2 is a schematic diagram of the construction of the shape kernel matrix of the robot of an embodiment of the present application, wherein Figure 2(a) is a schematic diagram of the direction where θ = 0°. Figure 2 (b) is a schematic diagram of the θ = 45° direction.

[0051] Figure 3 This is a schematic diagram of robot kinematic constraints according to an embodiment of the present invention.

[0052] Figure 4 This is a schematic diagram illustrating the feasibility of connecting path nodes according to an embodiment of the present invention, wherein... Figure 4 (a) is a schematic diagram of path feasibility. Figure 4 (b) is the sampling point S s Infeasible diagram Figure 4 (c) is a schematic diagram of the infeasibility during the path process.

[0053] Figure 5 This is a schematic diagram of bidirectional exploratory tree growth according to one embodiment of the present invention.

[0054] Figure 6 This is a schematic diagram of a bidirectional exploration tree connection under kinematic constraints according to an embodiment of the present invention, wherein... Figure 6 (a) is a schematic diagram of the internal common tangent. Figure 6 (b) is a schematic diagram of the external tangent.

[0055] Figure 7 This is a flowchart of an initial random path planning method according to an embodiment of the present invention.

[0056] Figure 8 This is a schematic diagram of a path node state transition method according to an embodiment of the present invention, wherein... Figure 8 (a) is a schematic diagram of the external tangent. Figure 8 (b) is a schematic diagram of the internal common tangent.

[0057] Figure 9 This is a planned path in an open working environment according to one embodiment of the present invention.

[0058] Figure 10 This is a planned path in an obstacle-prone working environment according to one embodiment of the present invention.

[0059] Figure 11 This is a path optimization method for working environments with minor obstacles, according to one embodiment of the present invention. Detailed Implementation

[0060] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0061] The path planning method for the irregular shape mobile robot based on RRT comprises the following steps:

[0062] Step S1, random path generation: in the random path process, a workspace network and a mobile robot shape kernel matrix need to be constructed, and the kinematic constraint of the mobile robot is determined, first, the initial state S0 and the target state S g of the mobile robot in the workspace are respectively taken as the initial path node, and two exploration trees are generated at the same time; then, a random sampling point is generated, and the feasibility of the sampling point as a path node is judged according to the kinematic constraint, the node state and the path feasibility, the growth of the two exploration trees is realized through continuous generation of the random sampling point and the feasibility judgment of the path node, until the two exploration trees are connected to each other under the kinematic constraint condition, and the random path is obtained;

[0063] Step S2, path optimization: the path optimization method based on the node state transition on the random path is adopted, and the path optimization is realized through the cyclic iteration of the node state.

[0064] Construct the workspace network:

[0065] The workspace of the mobile robot is evenly divided into a network structure along the x and y directions, and is defined as T, and the network form can adopt regular shapes such as square and triangle.

[0066] Each node T(x, y) of the network T includes two states of free space and obstacle occupation, the node function value of the free space is set as T(x, y)=0, and the node function value of the obstacle occupation area is set as T(x, y)=1.

[0067] For the irregular workspace, the area outside the workspace is all set as the obstacle T(x, y)=1, and the regular workspace network T can also be established.

[0068] Construct the robot shape kernel matrix:

[0069] The area occupied by the mobile robot is expanded into a regular rectangular or square area, and is evenly divided, and the form of the network should be the same as the workspace network T, so as to ensure that the robot shape kernel matrix W is placed in the workspace network T, and the node positions are one-to-one corresponding.

[0070] The kernel matrix W is composed, each element of the kernel matrix W includes two states of free space and robot occupation, the element value of the free space is set as W θ (m, n)=0, and the element value of the robot occupation is set as W θ (m, n)=1.

[0071] When the robot is in different direction angles, the element values of the free space are still set as W, which can be obtained by rotating the kernel matrix around the rear wheel center coordinates (x, y) θ (m, n) = 0, the element value of the robot occupying is set as W θ (m, n) = 1.

[0072] Kinematic constraints of the robot:

[0073] Assuming that the state of the robot in the workspace is S = (x, y, θ), according to the Ackermann steering constraint condition, the front wheel steering angle of the robot is Constrained by the mechanical structure, Therefore, in order to meet the kinematic constraints, the turning radius p of the robot cannot be smaller than its minimum value, that is, where L is the front-rear wheel distance, is the maximum front wheel steering angle.

[0074] Constructing an initial random path:

[0075] First, from the initial state S0and the target state S g At the same time, two exploration trees are established, which are defined as FTree and BTree, which are essentially a collection of path nodes, the first node of FTree is the initial state of the robot S0= (x0, y0, θ0), and the first node of BTree is the target state of the robot S g = (x g , y g , θ g ).

[0076] Random path generation includes three processes: random sampling point generation, path node connection and two exploration tree connection.

[0077] First step: random sampling point generation

[0078] Set the sampling probability a and the step length l, 0 < a < 100%, and l is a normal number.

[0079] Take the growth of FTree as an example. With probability a, select the target state S g = (x g , y g , θ g ) as the growth direction, and with probability (1-a), grow in a random direction, define the randomly generated point as S r = (x r , y r , θ r ).

[0080] Extend the exploration step length to l, and set the sampling point as S s = (xs , y s , θ s ).

[0081] Second step: connection of path nodes

[0082] Find the nearest node S r from the nodes already in FTree p as the parent node, extend along S p S r in the direction of S s = (x s , y s , θ s ) by a step length of l to generate a sampling point S p → S s , and then judge the feasibility of the S s → S s path, which includes three aspects.

[0083] ① Kinematic constraint judgment: if the sampling point S s is in the area that the robot can reach, then proceed to the next step; otherwise, S s does not satisfy the kinematic constraint, and resample.

[0084] ② Node state judgment: according to the state of the sampling point S θs , calculate the kernel matrix W s (x s , y θs ) of the robot sampling point S (xs，ys，θs) , and perform convolution calculation between the workspace network T and the kernel matrix W θs , h (xs，ys，θs) = T * W s , where "*" is the convolution calculation operator. If h p = 0, it means that the sampling point state S s is feasible, and then proceed to the next step; otherwise, resample.

[0085] ③ Path feasibility judgment: establish the S 11 → S 11 path, divide it into multiple sub-paths, and calculate the node state S 11 = (x 11 , y 12 , θ 12 ) and S 12 = (x 12 , y 11 , θ 12 ) of each sub-path. Then, use the above node state judgment method to judge whether S sAdd FTree and define S s whose parent is S p , otherwise, resample. This is mainly to avoid the interference of small obstacles in the path.

[0086] The growth of BTree is the same as FTree, but with two differences: one is that BTree grows in the direction of the initial state S0=(x0, y0, θ0) with probability a, and in a random direction with probability (1-a); the other is that the robot moves in the opposite direction, i.e., backward.

[0087] Step 3: Connection of the two exploration trees

[0088] Through continuous sampling, FTree and BTree grow towards each other, i.e., the distance between nodes decreases.

[0089] Define a distance threshold ε0, which can be set to 3-4ρ min .

[0090] Calculate the distance ε between the nearest nodes on FTree and BTree. If ε>ε0, continue sampling; if ε<ε0, perform connection judgment of the two exploration trees.

[0091] The connection judgment method is as follows:

[0092] Suppose that S Fm and S Bn are the nearest nodes on FTree and BTree, respectively. According to the states of S Fm and S Bn , construct their respective minimum turning radius circles.

[0093] Find the inner common tangent and outer common tangent of the two minimum turning radius circles. If the common tangent exists and satisfies the direction angles of S Fm and S Bn , then FTree and BTree can be connected to each other under the kinematic constraint, obtaining a random path connecting the initial state S0and the target state S g , defined as P rand ={S0, S F1 ,…, S Fm , S Bn ,…, S B1 , S g}, otherwise continue sampling until FTree and BTree can be connected to each other.

[0094] Path optimization method based on node state transition:

[0095] From the initial state S0to the target state S g, sequentially select three continuous path nodes to perform path node transfer calculation, complete path optimization once, and obtain a new optimized path;

[0096] from the initial state S0 to the target state S g The above path optimization process is repeated, and the initial random path gradually converges to the shortest path.

[0097] The path node transfer calculation is specifically: assuming that the initial generated random path P rand The three nodes S1, S2 and S3 are continuous.

[0098] First, calculate the two minimum turning radius circles of nodes S1 and S3.

[0099] Second, according to the direction angle of S1 and S3, the outer common tangent or inner common tangent of the two minimum turning radius circles is calculated.

[0100] Then, find the common tangent center S2' and use the above path node connection method to judge the feasibility of the path from S1→S2'→S3, if feasible, the path node is transferred from S2 to S2', otherwise, S2 remains unchanged. Specific embodiments:

[0102] A path planning method for a non-regular shape mobile robot based on RRT, comprising the following steps:

[0103] The workspace network division method combines Figure 1 as shown:

[0104] The workspace of the mobile robot is evenly divided into a network structure along the x and y directions, defined as T, and the network form can adopt regular shapes such as squares and triangles.

[0105] As shown in Figure 1 (a), according to the embodiment, the workspace is preferably divided into a 400x400 square network structure, each node T(x, y) of the network T includes two states of free space and obstacle occupation, the node function value of the free space (white) is set to T(x, y)=0, and the node function value of the obstacle occupation area (black) is set to T(x, y)=1.

[0106] As shown in Figure 1 (b), for a non-regular workspace, the area outside the workspace is set as an obstacle T(x, y)=1, and a regular workspace network T can also be established.

[0107] The robot shape description method combines Figure 2 as shown:

[0108] As shown in Figure 2(a) shown, assuming the shape of the robot is "T", (x, y) represents the center coordinates of the rear wheel of the robot, and θ represents the direction angle, θ = 0°. The area occupied by the mobile robot is extended into a regular rectangular or square area and is uniformly divided, and the form of the network should be the same as the workspace network T to ensure that the robot shape kernel matrix W is placed in the workspace network T, and the node positions are one-to-one corresponding.

[0109] The kernel matrix W is constructed, according to the embodiment, preferably as Figure 2 (a) shown, divided into a 40x40 square matrix, each element of the matrix includes two states of free space and robot occupation, the element value of the free space is set to W 0 (m, n) = 0, and the element value of the robot occupation is set to W 0 (m, n) = 1.

[0110] When the robot is in different direction angles, the kernel matrix can be obtained by rotating around the rear wheel center P coordinates (x, y), as shown in Figure 2 (b) shown, θ = 45°, the element value of the free space is still set to W θ (m, n) = 0, and the element value of the robot occupation is set to W θ (m, n) = 1.

[0111] The kinematic constraint of the robot is combined Figure 3 as shown:

[0112] Assuming that the state of the robot in the workspace is S = (x, y, θ), according to the Ackermann steering constraint condition, the steering angle of the front wheel of the robot is subject to mechanical structure constraints, Therefore, in order to meet the kinematic constraint, the turning radius p of the robot cannot be smaller than its minimum value where L is the front-rear wheel distance.

[0113] Figure 3 The minimum turning radius circular arcs of left and right steering are given, and the area in the middle of the two circular arcs is the area that the robot can travel to, that is, it meets the kinematic constraint, and the area outside the two circular arcs does not meet the kinematic constraint.

[0114] The random path generation method is combined Figure 4 , Figure 5 , Figure 6 and Figure 7 as shown:

[0115] Firstly, two exploration trees are built from initial state and target state, defined as FTree and BTree, which are actually the sets of path nodes. The first node of FTree is the initial state of robot S0 = (x0, y0, θ0), and the first node of BTree is the target state of robot S g = (x g , y g , θ g ).

[0116] Random path generation includes three processes: random sampling point generation, path node connection and two exploration trees connection.

[0117] First step: random sampling point generation

[0118] Set sampling probability a and step length l, 0 < a < 100%, l is a normal number.

[0119] Take the growth of FTree as an example. Select target state S g = (x g , y g , θ g ) as the growth direction with probability a, and grow in a random direction with probability (1-a), define the randomly generated point as S r = (x r , y r , θ r ).

[0120] Extend the exploration step length to l, and set the sampling point as S s = (x s , y s , θ s ).

[0121] Second step: path node connection

[0122] Find the node S p closest to S r from the nodes already in FTree as the parent node, extend the exploration step length to l along the direction of S p S r , and generate the sampling point S s = (x s , y s , θ s ), then judge the feasibility of S p → S s path, which includes three aspects:

[0123] ① Kinematics constraint judgment: if the sampling point S s is in the area that the robot can reach, then proceed to the next step; otherwise, S sIf the kinematic constraint is not satisfied, resample.

[0124] 2. Node state judgment: according to the state of the sampling point S s , the computer calculates the kernel matrix W s (x s , y s ) of the robot sampling point S s , convolves the workspace network T with the sampling point S θs kernel matrix W (xs，ys，θs) , h θs = T*W (xs，ys，θs) , where "*" is the convolution calculation symbol, if h s = 0, it means that the sampling point state S p is feasible, then proceed to the next step; otherwise, resample.

[0125] 3. Path feasibility judgment: establish S s → S 11 path, divide it into multiple sub-paths, as shown in Figure 4 , preferably divided into 3 sub-paths, respectively calculate the node state S 11 = (x 11 , y 11 , θ 12 ), S 12 = (x 12 , y 12 , θ 11 ), then use the above node state judgment method to judge whether S 12 and S s are feasible, if both are feasible, add the sampling point S s to FTree, and define the parent node of S p as S p , otherwise, resample. This is mainly to avoid the interference of small obstacles in the path.

[0126] As shown in Figure 4 , Figure 4 (a) can generate S s → S s path, Figure 4 (b) because the sampling point S p is not feasible, it collides with the obstacle, and cannot generate S s → S p path, Figure 4 (c) because there is a small obstacle in the S s → S min path, it is also not feasible.

[0127] The growth method of BTree is the same as that of FTree, but there are two differences: on the one hand, FTree chooses to grow towards the initial state S0=(x0,y0,θ0) with probability a, and grows in a random direction with probability (1-a); on the other hand, the robot moves in the opposite direction, that is, it reverses.

[0128] Step 3: Connecting the two exploration trees

[0129] like Figure 5 As shown, through continuous sampling, FTree and BTree will grow towards each other, that is, the distance between nodes will shorten.

[0130] Define a distance threshold ε0, which can typically be set to 3 to 4ρ. min .

[0131] Calculate the distance ε between the nearest nodes on the FTree and BTree. If ε > ε0, continue sampling. If ε < ε0, determine the connection between the two exploration trees.

[0132] Connection determination method as follows Figure 6 As shown.

[0133] Assume node S Fm and S Bn For the nearest node on FTree and BTree, according to S Fm and S Bn In each state, construct their respective minimum turning radius circles.

[0134] Find the common internal tangent of the two circles with the smallest turning radius, as shown below. Figure 6 As shown in (a), the external common tangent is as follows: Figure 6 As shown in (b), if the common tangent exists and satisfies S Fm and S Bn If the direction angle is determined, then the FTree and BTree can be connected to each other under kinematic constraints, resulting in a connection between the initial state S0 and the target state S. g A random path, defined as P rand ={S0,S F1 ,…,S Fm ,S Bn ,…,S B1 ,S g Otherwise, continue sampling until the FTree and BTree can be connected to each other.

[0135] The process for generating random paths is as follows: Figure 7 As shown.

[0136] Path optimization methods based on node state transitions, combined with Figure 8 As shown:

[0137] Assume the initial generated random path P rand There are three consecutive nodes S1, S2 and S3.

[0138] First, calculate the two minimum turning radius circles of nodes S1 and S3.

[0139] Second, according to the direction angle of S1 and S3, calculate the outer common tangent of the two minimum turning radius circles Figure 8 (a) or the inner common tangent Figure 8 (b).

[0140] Then, find the common tangent center S2' and use the above path node connection method to judge the feasibility of the path from S1→S2'→S3. If feasible, transfer the path node from S2 to S2', otherwise, S2 remains unchanged.

[0141] From the initial state S0 to the target state S g , select three consecutive path nodes in turn to perform the above path node transfer calculation, complete the optimization of a path, and get a new optimized path.

[0142] From the initial state S0 to the target state S g Repeat the above path optimization process, and the initial random path will gradually converge to the shortest path.

[0143] The planning path and the optimization result are combined, Figure 9 , Figure 10 and Figure 11 as shown:

[0144] Open working environment: the state of the robot in the workspace is represented by Figure 2 P points, two exploration trees FTree (Forward tree) and BTree (Backward tree) grow from the initial state Initial and the target state Goal respectively, and a random path is obtained. Figure 9 Two groups of simulation experiments are given, and two different random paths are obtained. After optimization using the node state transfer method, the optimized path is obtained, and the two optimized paths almost coincide, with good convergence. The basic characteristics of the converged path are: the robot first adjusts the direction along the minimum turning radius circle from the initial state, then travels in a straight line along the tangent direction to the minimum turning radius circle of the target state, and finally reaches the target state along the minimum turning radius circle. This is the shortest path that satisfies the kinematic constraints of the robot.

[0145] Obstacle working environment: Figure 10Two groups of simulation experiments are given, and two different random paths are obtained. After optimization by using the node state transition method, the two optimized paths almost coincide, further verifying that the proposed method has good convergence. The basic characteristics of the converged path are: the robot first adjusts the direction along the minimum turning radius circle from the initial state, and then successively passes through the A, B, C three corners of the obstacle along the minimum turning radius circle, and drives in the tangent direction to the minimum turning radius circle at the target state, and finally reaches the target state along the minimum turning radius circle, as shown in the Convergence path of Figure 10 .

[0146] Path optimization in a micro-obstacle working environment: as shown in Figure 11 , the initially generated path is random, as shown in the path formed by connecting the hollow small circles in Figure 11 , and the proposed node state transition method can effectively avoid the influence of small obstacles, and the optimized converged path is obtained, as shown in the path formed by connecting the solid black small circles in Figure 11 .

[0147] The present application is mainly used for path planning of an arbitrary shape mobile robot with Ackermann steering constraint, and the method includes two processes of random path exploration and path optimization. First, the working space of the robot is uniformly divided into a network structure, the shape characteristics of the arbitrary shape mobile robot in different azimuth angles are described by using the kernel matrix and kernel matrix rotation method, and the state of the robot in the working space is calculated by convolution calculation of the working space network and the kernel matrix; two exploration trees are generated from the initial state and the target state, and the bidirectional RRT (Rapidly-Exploring Random Tree) method is used to explore the path nodes, and the generation of the path nodes needs to meet the two conditions of the steering constraint of the robot and the state of the robot in the working space, until the two exploration trees are connected to each other under the strict kinematic constraint condition, and the initial random path is obtained; then the minimum turning circle tangent connection method is used to optimize the state (position and azimuth angle) of the nodes on the initial random path, and the stable converged local shortest path is obtained through continuous iteration calculation.

[0148] It should be understood that although the present specification is described in terms of various embodiments, not every embodiment contains only one independent technical solution, and the description manner of the specification is only for the sake of clarity, and those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that those skilled in the art can understand.

[0149] The above detailed description merely illustrates feasible embodiments of the present application, and is not intended to limit the protection scope of the present application, and equivalent embodiments or changes made without departing from the spirit of the present application shall be included in the protection scope of the present application.

Claims

1. A path planning method for an RRT-based irregularly shaped mobile robot, characterized by, The method comprises the following steps: Step S1, random path generation: in the random path process, the workspace network and the mobile robot shape kernel matrix need to be constructed, the kinematic constraints of the mobile robot are determined, first, the initial state S0 and the target state S g of the mobile robot in the workspace are respectively taken as the initial path node and the target path node, while two exploration trees are generated; then, random sampling points are generated, and the feasibility of the sampling points as path nodes is judged according to the kinematic constraints, the node state and the path feasibility; through the continuous generation of random sampling points and the feasibility judgment of path nodes, the growth of the two exploration trees is realized, until the two exploration trees are connected to each other under the kinematic constraint condition, and the random path is obtained; Step S2, path optimization: a path optimization method based on node state transition on a random path, which realizes path optimization through cyclic iteration of node states; The step of constructing the initial random path is specifically as follows: From the initial state S0 and the target state S g Two exploration trees are built simultaneously, defined as FTree and BTree, which are essentially sets of path nodes. The first node of FTree is the initial state of the robot S0 = (x0, y0, θ0), and the first node of BTree is the target state of the robot S g = (x g , y g , θ g ); Random path generation includes three processes: random sampling point generation, path node connection, and two exploration tree connection; The node state judgment is specifically: according to the state of the sampling point S s , calculating the kernel matrix W θs (x s , y s ) of the robot sampling point S s , performing convolution calculation on the workspace network T and the kernel matrix W θs of the sampling point S s , h (xs，ys，θs) =T*W θs , wherein "*" is a convolution calculation symbol, if h (xs，ys，θs) =0, it indicates that the state of the sampling point S s is feasible, then the next step is judged; otherwise, resampling is performed; The step of connecting the two exploration trees is specifically as follows: Through continuous sampling, the FTree and the BTree grow towards each other, that is, the distance between nodes is shortened; define a distance threshold value ε0, set to 3-4 p min ; The distance ε between the nearest nodes on the FTree and the BTree is calculated, if ε>ε0, the sampling is continued, if ε<ε0, the connection of the two exploration trees is judged; the connection judgment method of the two exploration trees is as follows: Assume node S Fm and S Bn are the most recent nodes on FTree and BTree, respectively, construct the minimum turning radius circle for each based on the state of S Fm and S Bn , respectively; find the inner and outer common tangent of the two minimum turning radius circles, if the common tangent exists and satisfies the direction angle of S Fm and S Bn , then FTree and BTree can be connected with each other under the kinematic constraints to obtain a random path connecting the initial state S0 and the target state S g , which is defined as P rand ={S0, S F1 ,…, S Fm , S Bn ,…, S B1 , S g}, otherwise continue sampling until FTree and BTree can be connected with each other; The step of the path optimization method based on node state transition on a random path is specifically as follows: from an initial state S0 to a target state S g , sequentially select three continuous path nodes to perform path node transfer calculation, complete the optimization of a path, and obtain a new optimized path; The path node transfer calculation is specifically: assuming that the initially generated random path P rand There are three consecutive nodes S1, S2 and S3 above; Firstly, two minimum turning radius circles of nodes S1 and S3 are calculated; Secondly, according to the direction angles of S1 and S3, the outer common tangent or the inner common tangent of the two minimum turning radius circles is calculated; Then, the common tangent center S2' is found, the connection method of path nodes is used to judge the feasibility of the path from S1 to S2' to S3, if feasible, the path node is transferred from S2 to S2', otherwise, S2 remains unchanged; From an initial state S0 to a target state S g The above path optimization process is repeated, and the initial random path gradually converges to the shortest path. 2.The path planning method for a non-regular shape mobile robot based on RRT according to claim 1, wherein, The step of constructing the workspace network is specifically as follows: The workspace of the mobile robot is evenly divided into a network structure along the x direction and the y direction, defined as T, the network T is divided in a regular shape; Each node T(x, y) of the network T includes two states of free space and obstacle occupation, the node function value of the free space is set as T(x, y)=0, and the node function value of the obstacle occupation area is set as T(x, y)=1; For a non-regular workspace, all areas outside the workspace are set as obstacles T(x, y)=1, and a regular workspace network T is established. 3.The path planning method for a non-regular shape mobile robot based on RRT according to claim 1, wherein, The step of constructing the robot shape kernel matrix is specifically as follows: The area occupied by the mobile robot is expanded into a regular rectangular or square area and evenly divided to form a kernel matrix W, the network form is the same as the workspace network T, the robot shape kernel matrix W is placed in the workspace network T, and the node positions are one-to-one corresponding; Each element of the kernel matrix W includes two states, free space and robot occupancy, with the element value set to W θ (m, n) = 0, with the element value set to W θ (m, n) = 1; When the mobile robot is at different orientation angles, the element values of the free space are still set as W obtained by rotating the kernel matrix W around the rear wheel center coordinates (x, y) θ (m, n) = 0, the element value of the mobile robot occupying is set as W θ (m, n) = 1. 4.The path planning method for a non-regular shape mobile robot based on RRT according to claim 1, wherein, The requirement of the kinematic constraint of the mobile robot is specifically as follows: Assume the state of the mobile robot in the workspace as S = (x, y, θ), according to the Ackermann steering constraint, the front wheel steering angle φ of the robot is constrained by the mechanical structure, |φ|≤φ max Therefore, the turning radius p of the robot cannot be smaller than its minimum value, i.e., p≥p min = L / tanφ max , where L is the front-rear wheel distance, and φ max is the maximum front wheel steering angle.

5. The path planning method for a non-regular shape mobile robot based on RRT according to claim 1, wherein, The step of generating the random sampling point is specifically as follows: Set the sampling probability a and the step length l, 0 The growth of FTree selects a target state S with probability a g = (x g , y g , θ g ) as the growth direction, and grows in a random direction with probability (1-a), defining the randomly generated point as S r = (x r , y r , θ r ); The extension exploration step is l, and the sampling point is S s = (x s , y s , θ s ); The growth mode of the BTree is different from that of the FTree, on one hand, the BTree grows towards the initial state S0=(x0, y0, θ0) with a probability a and grows in a random direction with a probability (1-a); on the other hand, the robot is in reverse driving, that is, reversing.

6. The path planning method for a non-regular shape mobile robot based on RRT according to claim 5, wherein, The connection step of the path node is specifically as follows: Find the node from FTree that is closest to S r The closest node S p As the parent node, along S p S r Extend in the direction with a step length of l to generate a sampling point S s = (x s , y s , θ s ), and then determine the feasibility of the S p → S s path, specifically including kinematic constraints, node state determination, and path feasibility determination.

7. The path planning method of the non-regular shape mobile robot based on RRT according to claim 6, characterized in that, The kinematic constraint judgment specifically is: if the sampling point S s In the area that the robot can reach, the next step is judged; otherwise, S s Does not meet the kinematic constraint, resample; The path feasibility judgment specifically comprises: establishing S p → S s path, dividing the path into multiple sub-paths, respectively calculating node states S 11 =(x 11 ,y 11 ,θ 11 ), S 12 =(x 12 ,y 12 ,θ 12 ) of the sub-paths, then, using a node state judgment method, judging whether S 11 and S 12 are feasible, if both are feasible, adding the sampling point S s to the FTree, and defining a parent node of S s as S p , otherwise, resampling.

Images