KR101339480B1 - Trajectory planning method for mobile robot using dual tree structure based on rrt - Google Patents

Abstract

The present invention relates to a trajectory planning method for a mobile robot using a dual tree structure based on RRT. The present invention comprises the steps of: sampling an arbitrary new point on a work space; extracting a new work space node corresponding to the new point and an adjacent work space node adjacent to the new point; extracting at least one state node corresponding to a parent node of the adjacent work space node and a state node corresponding to the adjacent work space node from a tree and registering the state nodes as candidate state nodes; extracting a candidate state node having the minimum candidate path cost among the candidate state nodes as a minimum cost parent state node by calculating a candidate path cost including path costs between the candidate state nodes and the new point; and registering the new work space node and the adjacent work space node in a configuration tree, and registering the minimum cost parent state node and the new state node corresponding to the new work space node as a parent node and a child node, respectively. Accordingly, the configuration tree and the state tree are mutually connected to each other to create a trajectory, so that the optimal trajectory can be searched at low calculation costs. [Reference numerals] (AA) START;(BB) END;(S10) New point sampling;(S20) Q_nev, Q_near extraction;(S30) Candidate state node registration;(S40) Minimum cost parent state node extraction;(S60) Re-connection

Description

Trajectory planning method of mobile robot using RRT-based dual tree structure {TRAJECTORY PLANNING METHOD FOR MOBILE ROBOT USING DUAL TREE STRUCTURE BASED ON RRT}

The present invention relates to a trajectory planning technique of a mobile robot, and more particularly, based on the Rapidly exploring random tree (RTT) technique, a work space tree and a state tree are interconnected to each other. The present invention relates to a trajectory planning method of a mobile robot using an RRT-based dual tree structure.

Many researches have been conducted on path planning, motion control techniques, and navigation schemes of mobile robots. The path planning technique may be classified into an A * search based technique using a grid map and a sampling based technique based on a rapid exploring random tree (RRT).

When using A * search-based techniques, it is practically difficult to deal with path planning issues with differential constraints. Therefore, in recent years, the RRT-based path planning technique has been receiving a lot of attention because it can handle the high-dimensional path planning problem.

In order to use a sampling-based path planning technique such as an RRT technique in an actual driving environment of a mobile robot, there are problems to be determined as follows.

First, the proper design of the motion primitives should be considered in the structure of the mobile robot. In particular, in the case of a two-wheeled mobile robot, it is preferable to include both a holonomic motion primitive and a non-holonomic motion primitive. For example, spot turning-and-move motion is advantageous in narrow and untidy places, since continuous curve trajectories are suitable for high speed travel.

Second, metric functions must be carefully designed.

Finally, the Region of Inevitable Collision Set (RICs) should be calculated efficiently and efficiently to ensure safety. For the calculation of RICs, a trajectory planning method for planning paths and speeds is required. However, since accurate calculation of RICs requires high computational cost, it is desirable to reduce the computational cost by simplification.

On the other hand, in the sampling-based path planning scheme, the node expansion may be performed by regional path planning. Regional route planning techniques using pre-planned motion primitives in vehicles such as cars where differential constraints are considered are described by A. Lacaze, Y. Moscovitz, N. DeClaris and K. Murphy. The paper "Path planning for autonomous vehicles driving over rough terrain (Proceedings of the 1998 IEEE ISlClCl RAASAS Joint Conference, Gaithersburg, MD, pp. 50-55, Sept. 14-17, 1998.)" and M. Pivtoraiko, RA Knepper And A. Kelly's paper "Differentially Constrained Mobile Robot Motion Planning in State Lattices" ( Journal of Field Robotics , vol. 26, no. 3, pp. 308-333, Mar. 2009.).

In general, vehicles such as automobiles have constraints on curvature. Planned regional paths generated off-line need to avoid resolution completeness problems and the calculation of accurate speed constraints for RICs. Alternatively, a method of adjusting the resolution of a planned local route to ensure the completeness of the resolution is presented in the aforementioned paper by M. Pivtoraiko et al.

On the other hand, vehicles such as cars and other two-wheeled mobile robots are not affected by curvature constraints. Therefore, there is a need for a planned local trajectory to handle all useful motions of a two-wheel mobile robot considering dynamic constraints.

RRT-based route planning among the sampling-based route planning techniques is described in A. Brooks, T. Kaupp and A. Makarenko's paper "Randomised MPC-based motion-planning for mobile robot obstacle avoidance (IEEE International Conference on Robotics and Automation, Kobe). , Japan, May. 12-17, 2009.) and Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. How, "Real-time motion planning with applications to autonomous urban driving ( IEEE Trans. on Control Systems , vol. 17, no. 5, pp. 1105-1118, Sept., 2009.).

In the paper by A. Brooks et al., A controller-based regional route planning scheme using MPC (Model Predictive Control) has been proposed. By using path planners that generate trajectories through forward simulation, such as controllers using MPC (Model Predictive Control), and the ideal control method for tracking control of mobile robots, Y. Kuwata et al. It provides accurate tracking performance.

The closed-loop RRT (CL-RRT) algorithm proposed in the above paper by Y. Kuwata et al. Uses a reference guide line for the generation of trajectories. CL-RRT ensures safety by maintaining the state of the terminal node in the stop condition. However, the distance metrics used in the papers of A. Brooks et al. And in the papers of Y. Kuwata et al. Require additional search techniques to find the nearest neighbor node.

The design or selection of the distance matrix to find the nearest node has a significant impact on the performance of the RRT-based algorithm. P.Cheng and S.M. Lavalle's paper, Reducing metric sensitivity in randomized trajectory design (IEEE International Conference on Intelligent Robots and Systems, Maui, HI, USA, Oct. 29-Nov. 03, pp. 43-48, 2001.) By using this, an improved RRT technique has been proposed to search the search information for the search of the unexplored state space. The results presented in the paper by P. Cheng et al. Indicate that the use of additional information improves route planning performance. Here, since the distance matrix generally includes other physical values, such as distance and angle, parameter tuning affecting the performance of the path planner requires adjustment of the sensitivity of the combined physical values.

RRT as presented in S. Karaman and E. Frazzoli's article "Sampling-based Algorithms for Optimal Motion Planning ( International Journal of Robotics Research , vol. 30, issue. 7, pp. 846-894, June, 2011.)" The technique is characterized by asymptotic optimality.

The RRT * technique finds the best parent node that extends into an adjacent set. If the cost from the new node to the existing node is less than the previous cost, the re-wiring operation causes the former node to be the parent node and the later node to the child node. ), The new node is connected to the existing node. In the RRT * technique, the result of the re-wiring operation can converge to the optimal solution. At this time, re-wiring is performed when the sample is close to the re-wiring boundary region.

Accordingly, an object of the present invention is to provide a new sampling-based trajectory planning method similar to a motion primitive, based on an RRT technique having four edge types, and more specifically, based on the RRT technique. The purpose of the present invention is to provide a trajectory planning method of a mobile robot using an RRT-based dual tree structure in which a work space tree and a state tree are interconnected to generate a trajectory.

The object is, according to the invention, (a) sampling any new points on the workspace; (b) extracting a new workspace node corresponding to the new point and an adjacent workspace node adjacent to the new point; (c) extracting at least one state node corresponding to a parent node of the adjacent workspace node and a state node corresponding to the adjacent workspace node from a state tree and registering them as candidate state nodes; (d) calculating a candidate path cost including a path cost between the candidate state node and the new point, and extracting a candidate state node having the minimum candidate path cost among the candidate state nodes as a minimum cost parent state node; ; (e) registering the new workspace node and the adjacent workspace node in the workspace tree, and adding the minimum cost parent state node and the new state node corresponding to the new workspace node as parent and child nodes, respectively. It is achieved by the trajectory planning method of the mobile robot using the RRT-based dual tree structure, characterized in that it comprises the step of registering in the state tree.

Here, in step (d), the candidate path cost between the candidate state node and the new point may be calculated based on a previously registered trajectory generation algorithm.

The candidate path cost may be calculated including the path cost from the candidate state node to a parent node of a predetermined depth.

In addition, the step (d) (d1) extracts candidate new state nodes for each candidate state node by applying each candidate state node and the new point to the trajectory generation algorithm, and the candidate state node and the Calculating the candidate path cost between new points; (d2) extracting a candidate state node having the least of the candidate path costs among the candidate state nodes as the minimum cost parent state node; The candidate new state node corresponding to the minimum cost parent state node may be registered as the new state node corresponding to the new workspace node.

If the candidate new state node is not located within a preset error range around the new point in step (d2), the candidate state node may be excluded from the candidate of the least cost parent state node.

Here, the method may further include reconnecting the node based on the new workspace node and the new state node.

The step (f) may include: (f1) registering each of the plurality of workspace nodes within a preset boundary area around the new workspace node as a preliminary workspace node; (f2) From the new workspace node to each of the spare workspace nodes, one of the plurality of spare workspace nodes that forms the minimum path cost is extracted to the least cost workspace node, and the new Extracting, as a minimum cost state node, one of a state node corresponding to a workspace node and at least one state node corresponding to a parent node of the new workspace node to form a minimum path cost; (f3) connecting with the new workspace node and the least cost workspace node; (f4) connecting the minimum cost state node and a state node corresponding to the minimum cost workspace node; (f5) connecting the parent state node and the child state node of the state node connected with the minimum cost state node.

Here, in step (f1), the parent node of the new workspace node may be excluded from the extraction of the preliminary workspace node.

(F2) step (f21) extracting any one of the plurality of preliminary workspace nodes, (f22) at least one state node corresponding to a parent node of the new workspace node, and the new Extracting a state node corresponding to a workspace node from a state tree and registering the reconnection candidate state node (f23) and assigning each of the reconnection candidate state nodes and the extracted preliminary workspace node to a preset trajectory generation algorithm; Extracting a reconnection candidate new state node for each reconnection candidate state node, and calculating a reconnection candidate path cost between each reconnection candidate state node and a state node corresponding to the new workspace node; (f24) add one of the reconnection candidate state nodes having the smallest reconnection candidate path cost to the minimum cost state node; And comprising; The minimum cost workspace node and the minimum cost state node may be extracted by performing steps (f21) to (f24) for each of the preliminary workspace nodes.

(H1) detecting a non-driving work space node that makes the mobile robot unable to travel; (h2) deleting the non-working workspace node; (h3) The method may further include connecting the orphan workspace node generated by the deletion of the non-working workspace node to the workspace tree corresponding to the parent node of the state node corresponding to the orphan workspace node.

According to the present invention according to the configuration as described above, the configuration tree and the state tree (State tree) is interconnected with each other to generate a trajectory, RRT-based dual that can find the optimal trajectory with a small calculation cost A trajectory planning method of a mobile robot using a tree structure is provided.

1 is a view showing four edge types of a mobile robot in the trajectory planning method of the mobile robot according to the present invention,
2 is a control flowchart of a trajectory planning method of a mobile robot according to the present invention;
3 is a view for explaining the process of the trajectory planning method of a mobile robot according to the present invention,
4 is a view for explaining a process of extracting a minimum cost parent state node in a trajectory planning method of a mobile robot according to the present invention;
5 and 6 are diagrams showing a node reconnection process of the trajectory planning method of a mobile robot according to the present invention,
7 is a view for explaining a node reconnection process of the trajectory planning method of a mobile robot according to the present invention,
8 is a view for explaining the processing of the node blocked by the obstacle in the trajectory planning method of the mobile robot according to the present invention,
9 is a diagram illustrating a branching process and a splitting process by a new state loop node in a trajectory planning method of a mobile robot according to the present invention;
10 is a diagram showing an example of a simulation for explaining the effect of the trajectory planning method of a mobile robot according to the present invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings.

In the trajectory planning method of a mobile robot using an RRT-based dual tree structure according to the present invention, it is limited to establishing a trajectory plan for a 2-wheel differential mobile robot. Prior to describing the trajectory planning method of the mobile robot according to the present invention, a dynamic model of the two-wheel differential mobile robot will be briefly described.

[Equation 1] is a mathematical definition of a dynamic model of a two-wheel differential mobile robot (hereinafter referred to as "mobile robot").

[Equation 1]

[Equation 1] shows the state transition equation of the mobile robot. State vector x = (x, y, θ, v, ω) ^T is (x, y) which is Cartesian coordinate, θ, which is the moving direction of mobile robot, translational velocity v, rotational velocity ) is a five-dimensional space with ω.

는 경계 병진 가속도이고, 제어 입력

는 경계 각가속도에 해당한다. 또한, 병진 속도와, 각속도는 각각

와

로 제한된다. 여기서, 이동 로봇은 병진 속도가 0이고, 각속도가 0이 아닐 때, 한 점에서 터닝(Turning)할 수 있다.Control inputs to mobile robots are bounded by dynamic constraints. Control input

Is the boundary translational acceleration, and the control input

Is the boundary angular acceleration. In addition, the translation rate and the angular velocity are respectively

Wow

. Here, when the translation speed is 0 and the angular velocity is not 0, the mobile robot can turn at a point.

로 표현될 수 있다. x는 [수학식 1]에 표현된 바와 같이 이동 로봇의 상태 벡터이다. 타겟 환경의 작업 공간(Configuration space)은

으로 표현되고, 이 때 q=(x,y) 좌표이다.Meanwhile, the state of the mobile robot

It can be expressed as. x is the state vector of the mobile robot as expressed in [Equation 1]. The configuration space of the target environment

Where q = (x, y) coordinates.

In the present invention, the minimum time trajectory τ: [0, T] → C _free where the initial state is τ (0) = x (0) and the final state is τ (T) ∈B _goal is calculated. Here, the B _goal is a goal area surrounded by the target goal point.

Here, the feature of the trajectory planning method of the mobile robot according to the present invention is to find the nearest neighbor node in the working space. The existing distance function includes the heading orientation, or angle, of the mobile robot in consideration of kinematic constraints. Thus, additional tuning is needed to adjust the weights to normalize different dimensional distances, including distances and angles.

On the other hand, in the trajectory planning method of the mobile robot according to the present invention, the additional tuning problem is solved by using only the distance. To this end, the trajectory planning method of the mobile robot according to the present invention uses four edge types of the mobile robot.

1 is a view showing four edge types of a mobile robot in the trajectory planning method of the mobile robot according to the present invention. Referring to FIG. 1, the state of the mobile robot includes a stationary state in which the velocity is 0 and a state in which the velocity is not zero. Here, the stationary state is useful because it allows the mobile robot to turn over a point in a narrow or untidy environment.

In order to increase the mobility of the mobile robot, the edge type of the RRT technique includes two terminal conditions. Accordingly, the two states and the two terminal conditions of the mobile robot include four edge types, as shown in FIG. 1.

The edges shown in FIGS. 1A and 1B rotate at a point in the initial stop condition until the mobile robot faces the target coordination point. Edges shown in (a) and (d) of FIG. 1 are stopped at the target point because the target workspace configuration point of the node is a stationary node. And, if there is no obstacle, the mobile robot will select the edge type shown in Fig. 1C.

Hereinafter, the trajectory planning method of the mobile robot according to the present invention will be described in detail with reference to FIGS. 2 to 3. 2 is a control flowchart of a trajectory planning method of a mobile robot according to the present invention, and FIG. 3 is a view for explaining a process of the trajectory planning method of a mobile robot according to the present invention.

Referring to FIGS. 2 and 3, first, an arbitrary new point q _new is sampled in the working space (S10). Then, extract the new workspace node (Q _new), and a work area adjacent node _(near Q) adjacent to the new point (q _new) corresponding to the new point (q _new) from the working space (S20). In this case, the adjacent workspace node Q _near is an example in which the workspace node closest to the new point q _new (q _near ) is selected. At this time, the new workspace node Q _new and the adjacent workspace node Q _near are extracted through the expansion of the workspace node.

When the new workspace node Q _new and the adjacent workspace node Q _near are extracted as described above, candidate state nodes for extraction of the minimum cost parent state node X _parent are registered. In the present invention, at least one state node corresponding to a parent node of an adjacent workspace node Q _near as a candidate state node and a state node corresponding to an adjacent workspace node Q _near are extracted from the state tree TX. For example, registration as a candidate status node.

The candidate path cost is then calculated including the path cost between each candidate state node and the new point q _new . The candidate state node having the minimum candidate path cost among the candidate state nodes is extracted as the minimum cost parent state node X _parent .

When the minimum cost parent state node X _parent is extracted as described above, the new workspace node Q _new and the adjacent workspace node Q _near are registered in the workspace tree TC. The new state node X _new corresponding to the minimum cost parent state node X _parent and the new workspace node Q _new is registered in the state tree TX as the parent node and the child node, respectively.

When the above registration is completed, it is possible to generate the trajectory based on the registered state tree TX and the workspace tree TC.

Through the above configuration, the state tree TX searches for and expands the minimum cost parent state node X _parent having the minimum path cost from the parent list of the workspace tree TC. It is possible to have a parent different from the parent of the workspace tree (TC). This means that each parent node of the new state node X _new and the new workspace node Q _new corresponding to the new point q _new according to the present invention may be different, and the new state node X _new The parent node of is determined to have the least path cost, so that the optimal path can be found with the lowest computational cost.

Hereinafter, a description of each process illustrated in FIG. 2 will be described in more detail with reference to FIG. 3.

[Algorithm 1] shows the entire process of the trajectory planning method of the mobile robot according to the present invention, as shown in FIG.

[Algorithm 1]

The notation used in the algorithm according to the invention is as follows. Lowercase letters are numeric values or vectors, and uppercase letters are data types with additional information, such as status nodes or workspace nodes. The upper case of the bold type indicates a data structure having a concatenated structure, such as a state tree (TX), a workspace tree (TC), and a list.

In [Algorithm 1], TC is a workspace tree TC, TX is a state tree TX, and q _new is a new point q _new sampled as described above.

If a new point (q _new ) is sampled on line 4, then on line 5 the workspace tree (TC) is expanded by the subroutine ExtendConfiguration ( TC, q _new ) to produce a new workspace node (Q _new ) and an adjacent workspace node ( Q _near ) is extracted. In line 6, the minimum cost parent state node X _parent is extracted through the subroutine FindParentState (Q _near , q _new , type). Here, the registration process of the candidate state node in step S30 of FIG. 2 is performed in the subroutine FindParentState (Q _near , q _new , type).

Then, as described above in lines 8 and 9, the new workspace node Q _new and the adjacent workspace node Q _near are registered in the workspace tree TC, and the minimum cost parent state node X _parent . And a new state node (X _new ) are registered in the state tree (TX).

Here, line 10 is a node reconnection process through ReConnectTrees (Q _new , X _new ), as shown in FIG. 2, based on the new workspace node Q _new and the new state node X _new . It will go through the process of reconnecting, a detailed description thereof will be described later.

In the trajectory planning method of the mobile robot according to the present invention, the candidate path cost for obtaining the minimum cost parent state node X _parent is calculated based on a previously registered trajectory generation algorithm. In the present invention, an example of applying a trajectory generation algorithm applied to the existing RRT technique is used. For example, the calculation is performed through Equation 2 and Equation 3. Equations 2 and 3 apply to the subroutine FindParentState (Q _near , q _new , type) of Algorithm 1.

&Quot; (2) "

&Quot; (3) "

As shown in Equation 2, the path cost function is defined by a suitable cost distance function L. In Equation 2, the derivative factor is an item that applies the intermediate path cost. That is, in the present invention, in calculating the candidate path cost, the path cost from the new point q _new to the candidate state node, that is, l _f () on the right side of Equation 2 and the candidate path cost is calculated. For example, the path cost from the candidate state node to its parent nodes, that is, the path cost from the root node to the root node, is calculated. Here, the path cost may be set to sum up the path cost to the parent node of a predetermined depth, for example, to the same ancestor node. Then, the solution of Equation 3 becomes the minimum cost parent state node X _parent .

3 is a diagram illustrating a method of extracting a minimum cost parent state node X _parent . And the thick black line is the workspace tree (TC), and the red dotted line status tree (TX) In Figure 3, the blue line is adjacent to the workspace node (Q _near the present invention to place the node corresponding to the candidate state node ) And its parent node. The blue dotted line indicates the candidate trajectory of the mobile robot from the candidate state node to the new point q _new .

3A is a diagram illustrating an expansion of the workspace tree TC. As described above, when the new point q _new is sampled, the nearest work space node is extracted to the adjacent work space node Q _near . 3B illustrates candidate trajectories within the error range e _r , which will be described in detail later. 3C illustrates that edges of the state tree TX are connected according to extraction of the minimum cost parent state node X _parent .

Here, as shown in (a) of FIG. 3, the adjacent workspace nose Q _near , which is the workspace node closest to the new point q _new , is generated using a random sample α _i , which is a conventional RRT. Since the technique is applied, a detailed description thereof will be omitted.

Hereinafter, a process of extracting the minimum cost parent state node X _parent in the trajectory planning method of the mobile robot according to the present invention will be described in detail with reference to FIG. 4. Here, the process of extracting the minimum cost parent state node X _parent corresponds to S30 and S40 of FIG. 2, and corresponds to FindParentState (Q _near , q _new , type) of line 6 of [Algorithm 1].

Referring to FIG. 4, as described above, when the candidate state node is registered (S30), the extraction process S40 of the minimum cost parent state node X _parent is performed.

More specifically by 3 and 4, the node corresponding to the parent node of the node and a neighboring work area node (Q _near) corresponding to the adjacent working space node (Q _near) to the candidate state node Is registered as a candidate status node. In FIG. 3A, the adjacent work space node Q _near CX1 and three state nodes CX2, CX3, and CX4 corresponding to the parent node become candidate state nodes.

Here, in order to extract the minimum cost parent state node (X _parent ), each candidate state node and a new point (q _new ) are applied to a trajectory generation algorithm to extract a candidate new state node (X _new ) for each candidate state node. The candidate path cost for each candidate state node is calculated. The candidate state node having the minimum candidate path cost among the candidate state nodes is extracted as the minimum cost parent state node X _parent .

Referring to FIG. 4, an example of the extraction process of the minimum cost parent state node X _parent is described in more detail. One of the candidate state nodes is extracted (S41). Here, the extracted candidate state nodes are allocated as X _cur .

Then, the extracted candidate state node X _cur and the new point q _new are applied to the locus generation algorithm to perform the locus generation algorithm (S42). Through the execution of trajectory generation algorithm candidate new state node (X _new) to the candidate state node (X _cur), the distance (er _cur) with the candidate path cost (cost _cur), and a new point (q _new) is output ( S43). Here, the distance e _rcur corresponds to the distance between the candidate new state node X _cand and the new point q _new .

When the candidate new state node X _cand , the candidate path cost cost _cur , and the distance e _rcur from the new point q _new are output for the current candidate state node X _cur , the candidate path in step S44. It is determined whether the cost _cur is less than the registered reference path cost and whether the distance e _rcur is located within the error range e _r .

Here, by leaving the reference path cost (cost) of the initially set to a very large value, such as infinity, to update the candidate path cost (cost _cur) of the candidate state node (X _cur) through the update process of S45 will be described later, of at least We will extract the minimum cost parent state node (X _parent ) with the path cost.

Further, even when the candidate new state node X _cand is not located within the preset error range e _r , as shown in FIG. 3A, the candidate state node X _curd has a minimum cost. Exclude from candidates of the parent state node (X _parent ). In FIG. 3A, CX4 is excluded because it is not located within the error range e _r .

On the other hand, if the condition of step S44 is satisfied, the candidate state node X _cur becomes a candidate of the least cost parent state node X _parent , and the update process of step S45 is performed for comparison with other candidate state nodes. do. Update step in the candidate new state node (X _cand) a new state is updated to the node (X _new), a candidate path cost (cost _cur) is updated to the reference path cost (cost), candidates state node (X _cur) have at least It is updated to the cost parent state node X _parent (S45).

Then, after checking whether the candidate state node exists (S46), if there is a candidate state node, a new candidate state node is extracted to perform steps S41 to S45. At this time, the new candidate state node is performed by comparing with the updated values in step S45.

When the process is performed for all candidate state nodes, in step S45, the minimum cost parent state node X _parent and the corresponding new state node X _new are updated, and the minimum cost parent state node is updated. (X _parent ) and the new state node (X _new ) can be extracted (S47).

Hereinafter, the process of reconnecting the nodes in the trajectory planning method of the mobile robot according to the present invention will be described in detail.

First, as described in S. Karaman and E. Frazzoli's article "Sampling-based Algorithms for Optimal Motion Planning ( International Journal of Robotics Research , vol. 30, issue. 7, pp. 846-894, June, 2011.) In other words, the existing RRT technique is close to the next best option, that is, the probability that the existing RRT technique generates the optimal path within a limited number of iterations is close to 0. That means you can get

In order to ensure asymptotic optimality, if the cost from the new node to the existing node around the new node is smaller than the previous cost, the 'Rewire' process of the paper of S. Karaman et al. do. The rewire process affects all child nodes of the rewired node. This is based on the assumption that it has a state from an optimal state or an optimal cost. However, if sampling of the new point q _new is performed in the work space, the assumption about the optimal cost described above is not guaranteed and the trajectory from the rewire node may be impossible to execute.

JH Jeon, S. Karaman and E. Frazzoli, "Anytime Computation of Time-Optimal Off-road Vehicle Maneuvers using the RRT * (IEEE Conference on Decision and Control (CDC), Dec. 12-15, 2011.) As described above, the child node can be re-propagated or deleted to deal with the inability of the child node, and the technique proposed by JH Jeon et al. Can be effective in offline path planning. Deleting an infeasible child node increases the probability that a new new point (q _new ) exists in the empty space, but has the disadvantage that having many trajectories capable of real-time operation can be blocked by obstacles or moving obstacles not shown on the map. Holding it.

In the present invention, in order to compensate for this disadvantage, the rewire algorithm disclosed in the paper of S. Karaman et al. In the trajectory planning method of the mobile robot according to the present invention, a local rewire process is performed, and the child node is maintained by replacing the parent of the child node. That is, the above reconnection process of reconnecting the child node to the new parent node is performed.

The trajectory planning method of the mobile robot according to the present invention can be easily operated since both the work space tree TC and the state tree TX are used for the trajectory planning. Here, the maintenance of the child node is advantageous to use a _near est search algorithm such as the Kd trees algorithm, since the delete operation increases the computational cost of the algorithm such as Kd trees.

5 and 6 are views illustrating a process of reconnecting nodes of a trajectory planning method of a mobile robot according to the present invention, and FIG. 7 is a view illustrating a process of reconnecting nodes of a trajectory planning method of a mobile robot according to the present invention. Drawing.

When the new workspace node Q _new and the new state node X _new are extracted through the above-described process, the nodes are reconnected using the same. 5 to 7, each of the plurality of workspace nodes within the preset boundary range Br (see FIG. 7) around the _new workspace node Q _new is registered as a spare workspace node (see FIG. 7). S110).

Here, the boundary range Br for extracting the preliminary workspace node is set in consideration of the increase in the amount of calculation and the frequency of the reconnection process. For example, when the boundary range Br is set wide, reconnection with low path cost is possible, but the amount of calculation increases. On the other hand, if the boundary range (Br) is set to be narrow, there is a disadvantage in that it operates only when a very dense node is generated in order for the reconnection process to occur.

In FIG. 7A, C ₀ , C ₁ , and C ₂ among the workspace nodes are included in the boundary range Br and extracted as a preliminary workspace node. At this time, the parent node of the _new workspace node Q _new , that is, C ₀ , is excluded from the extraction of the spare workspace node. This method uses the path cost to search for a reconnection target with the new workspace node Q _{new as} described below. As described above, the new workspace node Q _new and its parent node have a minimum path. Since it was generated through cost, it is excluded.

As described above, when a spare workspace node other than the parent node of the _new workspace node Q _new is registered, each spare workspace node from the new workspace node Q _new for extraction of the minimum cost state node X _min . Through the expansion of the furnace, any one of the plurality of preliminary workspace nodes forming the minimum path cost is extracted as the minimum cost workspace node. In addition, the corresponding state node and at least one of the conditions any one of forming the minimum of the path cost is a minimum cost of nodes the node corresponding to the parent node of the new workspace node (Q _new) of the new workspace node (Q _new) Extracted as (X _min ).

5 and 6, one of the preliminary workspace nodes is first extracted (S120). Here, in the case of registering the preliminary workspace node, the nodes are sorted and registered in the depth order of the nodes and extracted in the sorting order. In FIG. 7A, C ₁ and C ₂ are registered in order, and C ₁ is extracted first.

Then, steps S131 to S136, S140, and S137 are performed using the extracted preliminary workspace node and the new workspace node Q _new , and the steps S131 to S137 are S41 to S47 of FIG. 4. Corresponds to the step.

More specifically, the extract from at least one of the node and the status tree (TX) state node corresponding to the new workspace node (Q _new) corresponding to the parent node of the new workspace node (Q _new) material It is registered as a connection candidate status node (S121). Then, any one of the reconnection candidate state nodes is extracted (S131). Here, the extracted reconnection candidate state node is allocated to X _cur .

Thereafter, the extracted reconnection candidate state node X _cur and the new workspace node Q _new are applied to the trajectory generation algorithm to perform the trajectory generation algorithm (S132). By performing the trajectory generation algorithm, the reconnection candidate new state node (X _cand ) for the reconnection candidate state node (X _cur ), the reconnection candidate path cost (cost _cur ), and the reconnection candidate state node (X _cur ) The distance e _rcur is output (S133).

Distance to the reconnection candidate new state node (X _cand), reconnection candidate path cost (cost _cur), and the reconnection candidate state node (X _cur) for the current reconnection candidate state node (X _cur) of (e _rcur) If is output, it is determined in step S134 whether the reconnection candidate path cost (cost _cur ) is less than the registered reference path cost (cost), and whether the distance (e _rcur ) is located within the error range (e _r ).

Here, the reference path cost (cost) of the initial is the cost of the existing route, by using the update process of S135 will be described later updates the reconnection candidate path cost (cost _cur) of the reconnection candidate state node (X _cur), at least It extracts the minimum cost state node (X _min ) having a path cost of.

In addition, even when the candidate new state node X _cand is not located within the preset error range er, the candidate new state node X _cand is excluded from the candidate of the minimum cost state node X _{min of the} corresponding reconnection candidate state node X _cur . .

On the other hand, if the condition of step S134 is satisfied, the corresponding reconnection candidate state node X _cur becomes a candidate of the least cost state node X _min , and the update of step S135 is performed for comparison with other reconnection candidate state nodes. The process is carried out. In the update process, the reconnection candidate new state node (X _cand ) is updated to the reconnection new state node (X _new ), the reconnection candidate path cost (cost _cur ) is updated to the reference path cost, and the reconnection candidate is The state node X _cur is updated to the minimum cost state node X _min (S135).

Then, after checking whether a reconnection candidate state node exists (S46), if there is a reconnection candidate state node, a new reconnection candidate state node is extracted to perform steps S131 to S135. At this time, the new reconnection candidate state node is performed through comparison with the values updated in step S135.

If the above process is performed for all reconnection candidate state nodes, it is checked in step S140 of FIG. 6 whether a preliminary workspace node exists. When the preliminary workspace node exists in step S140, the process after step S120 described above is performed, and finally, step S135 through the process of calculating path costs for all the preliminary workspace nodes through the above process. The updated minimum cost state node X _min and corresponding reconnect new state node X _new are extracted at step S137.

Through the above process, a preliminary workspace node corresponding to the reconnect new state node X _new , the minimum cost state node X _min , and the reconnect new state node X _new , that is, the minimum cost workspace node Proceed with reconnection.

That is, the new workspace node Q _new and the minimum cost workspace node are connected to connect the workspace nodes S150. In addition, the minimum cost state node X _min and the state node corresponding to the minimum cost workspace node, that is, the reconnection new state node X _new , are connected for the connection S60 of the state node.

And, as shown in (c) of FIG. 7, as the reconnect new state node X _{new is} connected to the minimum cost state node X _min , the parent node of the reconnect new state node X _new and As the child node is connected (S170), the reconnection process is terminated.

Hereinafter, with reference to Figure 8 will be described in detail with respect to the processing of the node blocked by the obstacle in the trajectory planning method of the mobile robot according to the present invention.

First, as illustrated in FIG. 8A, when a situation in which the mobile robot cannot travel due to an obstacle or the like occurs, the non-travelable work space node is detected. Then, the non-driving workspace node is deleted from the workspace tree TC.

At this time, as shown in (b) of FIG. 8, if an orphan work node occurs due to deletion of the non-driving workspace node, the orphan work node is as shown in (c) of FIG. 8, It is associated with the workspace tree (TC) that corresponds to the parent node of that state node.

Through this, more trajectories can be maintained, and new trajectories can be generated more widely when other collisions or obstacles that occur during the trajectory tracking of the mobile robot occur.

Meanwhile, FIG. 9 is a diagram illustrating a branching process and a splitting process by a new state loop node in a trajectory planning method of a mobile robot according to the present invention. FIG. 9A is a diagram illustrating a selection of a new state loop node, and FIG. 9B is a diagram illustrating a pruning result of the state tree TX and the workspace tree TC. 9 (c) is a view showing that the edge of the orphan node is connected and the loop trajectory is divided along a constant length.

Non-executable workspace nodes that do not start from the target state are deleted. Here, the partitioning process has the following effects. First, make sure that the target trajectory has a sufficiently short execution time and length. Second, if the trajectory is too long, it is possible to create an intermediate node.

10 is a view showing an example of a simulation for explaining the effect of the trajectory planning method of a mobile robot according to the present invention. 10 (a) is a trajectory generated by the existing RRT technique, Figure 10 (b) is a trajectory generated by the trajectory planning method of the mobile robot according to the present invention. As can be seen through Figure 10, it can be confirmed that the trajectory technique of the mobile robot according to the present invention is more effective to find the optimal path than the conventional RRT.

Although several embodiments of the present invention have been shown and described, those skilled in the art will appreciate that various modifications may be made without departing from the principles and spirit of the invention . The scope of the invention will be determined by the appended claims and their equivalents.

100: spiral wound membrane module

Claims (10)

(a) sampling any new points on the workspace;
(b) extracting a new workspace node corresponding to the new point and an adjacent workspace node adjacent to the new point;
(c) extracting at least one state node corresponding to a parent node of the adjacent workspace node and a state node corresponding to the adjacent workspace node from a state tree and registering them as candidate state nodes;
(d) calculating a candidate path cost including a path cost between the candidate state node and the new point, and extracting a candidate state node having the minimum candidate path cost among the candidate state nodes as a minimum cost parent state node; ;
(e) registering the new workspace node and the adjacent workspace node in the workspace tree, and adding the minimum cost parent state node and the new state node corresponding to the new workspace node as parent and child nodes, respectively. Trajectory planning method of a mobile robot using the RRT-based dual tree structure, comprising the step of registering in the state tree. The method of claim 1,
In step (d), the candidate path cost between the candidate state node and the new point is calculated based on a previously registered trajectory generation algorithm. 3. The method of claim 2,
The candidate path cost is calculated by including a path cost from the candidate state node to a parent node of a predetermined depth, using the RRT-based dual tree structure. The method of claim 3,
The step (d)
(d1) extracting candidate new state nodes for each candidate state node by applying each candidate state node and the new point to the trajectory generation algorithm, and calculating the candidate path cost between each candidate state node and the new point. Calculating step,
(d2) extracting candidate state nodes having the least of the candidate path costs among the candidate state nodes as the minimum cost parent state node;
And the candidate new state node corresponding to the minimum cost parent state node is registered as the new state node corresponding to the new workspace node. 5. The method of claim 4,
In the step (d2), if the candidate new state node is not located within a preset error range around the new point, the candidate state node is excluded from the candidate of the least cost parent state node. Trajectory Planning Method of Mobile Robot Using Dual Tree Structure The method of claim 1,
and (f) reconnecting nodes based on the new workspace node and the new state node. The method according to claim 6,
The step (f)
(f1) registering each of the plurality of workspace nodes within a predetermined boundary around the new workspace node as a preliminary workspace node;
(f2) From the new workspace node to each of the spare workspace nodes, one of the plurality of spare workspace nodes that forms the minimum path cost is extracted to the least cost workspace node, and the new Extracting, as a minimum cost state node, one of a state node corresponding to a workspace node and at least one state node corresponding to a parent node of the new workspace node to form a minimum path cost;
(f3) connecting with the new workspace node and the least cost workspace node;
(f4) connecting the minimum cost state node and a state node corresponding to the minimum cost workspace node;
and (f5) connecting the parent state node and the child state node of the state node connected with the least cost state node. The trajectory planning method of the mobile robot using the RRT-based dual tree structure. The method of claim 7, wherein
In step (f1), the parent node of the new workspace node is excluded from the extraction of the preliminary workspace node. The trajectory planning method of the mobile robot using the RRT-based dual tree structure. 9. The method of claim 8,
Step (f2) is
(f21) extracting any one of the plurality of preliminary workspace nodes;
(f22) at least one state node corresponding to a parent node of the new workspace node and a state node corresponding to the new workspace node are extracted from a state tree and registered as a reconnection candidate state node;
(f23) extracting reconnection candidate new state nodes for each reconnection candidate state node by applying each reconnection candidate state node and the extracted preliminary workspace node to a preset trajectory generation algorithm, and reconnecting each reconnection candidate state node. Calculating a reconnection candidate path cost between a candidate state node and a state node corresponding to the new workspace node;
(f24) extracting any one of the reconnection candidate state nodes having the least reconnection candidate path cost to the minimum cost state node;
Moving using the RRT-based dual tree structure, performing steps (f21) to (f24) for each preliminary workspace node to extract the minimum cost workspace node and the minimum cost state node. How to plan the trajectory of the robot. 10. The method according to any one of claims 1 to 9,
(h1) detecting a non-travelable workspace node which makes the mobile robot unable to travel;
(h2) deleting the non-working workspace node;
(h3) further comprising connecting an orphaned workspace node resulting from deletion of the non-driving workspace node to a workspace tree corresponding to a parent node of a state node corresponding to the corresponding orphaned workspace node; Trajectory planning method of mobile robot using RRT-based dual tree structure.

Images