A Motion Planning Method for Mobile Robots Based on Linear Temporal Logic

Abstract

This invention relates to the field of robot motion planning technology, specifically a mobile robot motion planning algorithm based on linear temporal logic, comprising: S1, constructing a grid map and performing dilation processing on the grid map; constructing a robot temporal task and converting it into an automaton; S2, incrementally constructing a prefix sampling tree; S3, obtaining candidate prefix paths; S4, incrementally constructing a suffix sampling tree based on the candidate prefix paths; S5, obtaining the prefix path and suffix path with minimum cost, and constructing a safety corridor based on the obtained paths; S6, constructing an optimization problem based on the safety corridor to solve for the robot's motion trajectory. The trajectory planning algorithm proposed in this invention utilizes a differential flat model of the mobile robot, which can constrain the trajectory based on the kinematics and dynamics of the mobile robot in a lower dimension, resulting in a higher quality generated trajectory.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning technology, and in particular to a mobile robot motion planning method based on linear sequential logic. Background Technology

[0002] In recent years, intelligent mobile robots have developed rapidly and are increasingly widely used, such as service robots in homes, restaurants, and hospitals, logistics robots in smart warehousing, and special-purpose robots in military scenarios. Specific research areas for mobile robots include: vision- or radar-based environmental perception and SLAM localization, autonomous motion planning, and behavior control. Among these, motion planning has always been a hot research topic in mobile robots. Motion planning includes path planning and trajectory planning, and efficient motion planning methods enable robots to complete tasks efficiently and accurately.

[0003] Many scholars have conducted in-depth research on path planning for mobile robots and achieved numerous results. However, these studies mostly address simple problems, namely, planning a feasible path from a starting point to a destination based on evaluation criteria such as shortest distance and shortest time, while avoiding obstacles in the environment—this is known as a point-to-point problem. Traditional path planning methods, due to their singular task mode, cannot be applied to path planning problems involving sequential or cyclical tasks, and therefore cannot meet the application needs of mobile robots. People need mobile robots to complete complex sequential tasks, such as factory security patrols, military reconnaissance, express delivery, and fire rescue. Therefore, it is necessary to design a motion planning algorithm framework that can plan feasible paths based on sequential tasks, using a language capable of describing complex sequential tasks.

[0004] Over the past decade, as a cutting-edge interdisciplinary field in robotics and artificial intelligence, research on robot planning methods based on model detection has received significant attention and pioneering research from a number of renowned scholars both domestically and internationally. Among these, planning methods using Linear Temporal Logic (LTL) as the task description language have become a major research focus. LTL can conveniently express the behavior of things, describe various temporal task requirements, and easily integrate with existing planning frameworks. Designing a motion planning method based on LTL is of great significance for expanding the application areas of mobile robots. Summary of the Invention

[0005] This invention provides a mobile robot motion planning method based on linear temporal logic to solve the technical problem that existing robot motion path planning is difficult to apply to complex temporal tasks.

[0006] To achieve the above objectives, the technical solution of the present invention is implemented as follows:

[0007] This invention provides a motion planning method for mobile robots based on linear sequential logic, comprising the following steps:

[0008] Step S1: Construct a grid map and dilate the grid map according to the robot radius; construct the robot's sequential task and convert it into a Büchi automaton;

[0009] Step S2: Construct a prefix sampling tree based on the Büchi automata. ;

[0010] Step S3: Construct the prefix sampling tree based on the increment. Get candidate prefix path list ;

[0011] Step S4: Based on the Büchi automaton and the candidate prefix path list Constructing a suffix sampling tree ;

[0012] Step S5: Construct the suffix sampling tree based on the incremental changes. Obtain candidate suffix sampling paths, use the candidate suffix sampling paths to obtain the prefix path and suffix path with the minimum cost, and construct a safe corridor based on the minimum prefix path and suffix path.

[0013] Step S6: Plan the robot's movement trajectory based on the safety corridor.

[0014] Furthermore, the prefix sampling tree in step S2 and the suffix sampling tree in step S4 Collectively referred to as sampling trees:

[0015] The sampling tree is based on a finite set of Büchi automata. It is divided into multiple sampling subtrees, and the sampling subtrees can be used This indicates that the subscript is... Represents the state of the Büchi automaton; nodes of the sampled subtree can be represented by tuples. This indicates that the location information , Represents two-dimensional coordinates. Indicates the heading angle. Represents a node The parent node, Indicates connection and The shortest Dubins curve, Represents the node's value; sampled subtree and sampling subtree According to the conversion pair The regions of interest corresponding to the input alphabet are concatenated, where and Indicate two different states of the Büchi automaton; let Indicates the transformation pair Input alphabet, It corresponds to one or more regions of interest.

[0016] Furthermore, step S1 specifically includes the following steps:

[0017] Step S11: Use the open-source algorithm Cartographer to construct a grid map and expand the map according to the robot radius;

[0018] Step S12: Construct the robot's temporal task, represent the robot's temporal task using LTL formulas, and convert it into a Büchi automaton using the open-source package spot.

[0019] Büchi automata can use tuples It means that among them A finite set representing states, The set representing the initial state. ; This indicates that the alphabet should be entered. This indicates an uncertain transfer relationship. ; Represents the set of accepting states. .

[0020] Step S13: Remove the transformation relations of multiple atomic propositions in the Büchi automaton that simultaneously satisfy the LTL formula.

[0021] Furthermore, step S2 specifically includes the following steps:

[0022] Step S211: Initial node Add to prefix sampling tree In the middle, the initial position information , The initial coordinates of the mobile robot, Represents the i-th discrete heading angle. Let i = 0, 1, 2, ..., n, where the value of n determines the heading angle resolution. Indicates the initial state. ;

[0023] Step S212: Initial state Add it to the first list and search the Büchi automaton for its initial state. Find the neighboring nodes, obtain the state transition pairs to be expanded, and add them to the second list; create a new transition list. .

[0024] The first list represents the prefix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of The second list indicates A series of transformation pairs to be expanded ,in For two different states in the Büchi automaton; Transition list It is used to store nodes whose heading angle needs to be adjusted during the prefix sampling tree update process.

[0025] Prefix sampling tree The Büchi automaton is constructed based on the first and second lists, and both lists are updated simultaneously during construction. The update rule is: when the first list has a new Büchi automaton state... Search when joining Neighboring nodes form one or more transformation pairs, except The existing transformation pairs are added to the second list; when Successfully expanded the transformation pairs in the second list At that time, determine the sampling subtree and Has the connection node been accessed for the conversion pair? Input alphabet All corresponding regions of interest, if so, will Remove from the second list, also if Then Add to the first list (ignore if the state already exists in the first list).

[0026] Step S213: Set the iteration increment for prefix sampling C1 is a constant, and the prefix sampling iteration value is... Initialize to the number of transformation pairs in the second list and the iteration increment. The product;

[0027] Step S214: Set the iteration count value ;

[0028] Step S215, if ,but If necessary, proceed to step S216; otherwise, exit the program.

[0029] Step S216, Given probability ,by The probability is sampled from the first list, with (1- The probability of ) is sampled from the second list to obtain the conversion pair. ;

[0030] Step S217: According to the conversion pair Input alphabet Location sampling is performed to obtain ;

[0031] Step S218: Traverse the prefix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve;

[0032] Step S219, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold;

[0033] Step S220: Traverse the sampling subtree Select the node that makes the point Parent node with minimum cost , The position is and update For point The corresponding Büchi automaton state;

[0034] Step S221, Solve from Time The shortest Dubins curve Q;

[0035] Step S222: Perform collision detection on the Dubins curve Q. If there is no collision, proceed to step S223; otherwise, return to step S215.

[0036] Step S223: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q;

[0037] Step S224: Add the new node Add to sampling subtree In the middle, and for the sampling subtree New nodes in Rewire nearby nodes to reduce the cost to nearby nodes;

[0038] Step S225, Judgment Check if the condition is met. If it is, proceed to step S229; otherwise, execute step S226.

[0039] Step S226: Add the new node Add to conversion list middle;

[0040] Step S227: Construct a series of nodes ,in x, y and The same, Let i = 0, 1, 2, ..., n, and add these nodes to the sampling subtree. middle;

[0041] Step S228: Update the first list and the second list, and whenever a new transformation pair is added to the second list, the iteration value is... Increase ;

[0042] Step S229: Adjust the conversion list by sampling Middle node If re-electing the parent node reduces the node's cost, then update the value. If the corresponding node is selected, no processing is performed. Finally, proceed to step S215.

[0043] Furthermore, step S3 specifically includes the following steps:

[0044] Step S31: Traverse the prefix sampling tree Middle accept state sampling subtree The nodes, among which To accept the status, ;

[0045] Step S32: Access each traversed node upwards to the root node through its parent pointer, thereby obtaining a series of candidate prefix paths, and add them to the candidate prefix path list. .

[0046] Furthermore, step S4 specifically includes the following steps:

[0047] Step S411: Convert the prefix sampling tree Acceptance state sampling subtree Add the node to the suffix sampling tree The text also mentions that if a Büchi automaton state exists, it is specified that... ,satisfy , and arrive Input alphabet If the value is "1", then these nodes will be added to... The Büchi automaton needs to be set to the correct state. Updated to ;

[0048] Step S412, to Add to the third list and search for information about the Büchi automata. Obtain the neighboring nodes, retrieve the state transition pairs to be expanded, and add them to the fourth list; create a new transition list. .

[0049] The third list represents the suffix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of The fourth list represents A series of transformation pairs to be expanded ,in For two different states in the Büchi automaton; Transition list Used to store nodes whose heading angle needs to be adjusted during the suffix sampling tree update process.

[0050] Suffix sampling tree The system constructs a new list based on the third and fourth lists, updating both lists simultaneously. The update rules for the third and fourth lists are the same as those for the first and second lists.

[0051] Step S413: Set the iteration increment for suffix sampling Set the suffix sampling iteration value to a constant C2. The number of transformation pairs in the fourth list and The product;

[0052] Step S414: Set the iteration count value ;

[0053] Step S415, if ,but If necessary, proceed to step S416; otherwise, exit the program.

[0054] Step S416, Given probability ,by The probability is sampled in the third list, with (1- The probability of ) is sampled in the fourth list to obtain the conversion pair. ;

[0055] Step S417: According to the conversion pair Input alphabet Location sampling is performed to obtain ;

[0056] Step S418: Traverse the suffix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve;

[0057] Step S419, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold;

[0058] Step S420: Traverse the sampling subtree Select the node that makes Parent node with minimum cost , The position is and update for The corresponding Büchi automaton state;

[0059] Step S421, Solve from arrive The shortest Dubins curve Q;

[0060] Step S422: Perform collision detection on Q. If there is no collision, proceed to step S423; otherwise, return to step S415.

[0061] Step S423: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q;

[0062] Step S424, will Add to sampling subtree In the middle, and on middle Rewire nearby nodes to reduce the cost to nearby nodes;

[0063] Step S425, Judgment If the condition is true, proceed to step S629; otherwise, execute step S626.

[0064] Step S426, Add to conversion list ;

[0065] Step S427: Traverse candidate prefix paths Check if a node exists on the list. Its x, y and The same and its s is If it is, skip to step S428; otherwise, update the third and fourth lists, and whenever a new pair of transformations is added to the fourth list, the iteration value is updated. Increase After the update is complete, proceed to step S429;

[0066] Step S428, of and its descendant nodes added to In the corresponding sampling subtree, where let The parent node is and will Remove from the fourth list;

[0067] Step S429: Adjust the list by sampling Middle node If re-electing the parent node reduces the node's cost, then update the value. If the corresponding node is selected, no processing is performed. Finally, proceed to step S415.

[0068] Furthermore, step S5 specifically includes the following steps:

[0069] Step S51: Traverse the suffix sampling tree Middle accept state sampling subtree The nodes, among which And by accessing each node up to the root node through the parent pointer, candidate suffix paths are obtained;

[0070] Step S52: Based on the candidate suffix paths, select the prefix path and suffix path with the lowest cost to complete the path planning part;

[0071] Step S53: Divide the path obtained in step S52 according to the Büchi automaton state, with each path segment having the same Büchi automaton state.

[0072] Step S54: Divide each path into multiple line segments evenly at a certain step length. Generate a convex polygon around each line segment according to the grid map. Finally, multiple convex polygons are obtained. There are no obstacles inside the convex polygons. Multiple convex polygons form a safe corridor.

[0073] Furthermore, step S6 specifically includes the following steps:

[0074] Step S61: Using the Bézier curve as the expression for the trajectory, and applying the differential flat model of the mobile robot, construct an optimization problem; n-order m-segment Bézier trajectory The expression is as follows:

[0075] (1)

[0076] Where the function expression function variables Dimension , for The (i+1)th control point of the j-th segment of the trajectory. Let be the scale factor of the j-th segment of the trajectory. , , ... Here are the termination times for each segment of the trajectory, and the total trajectory time is... ;

[0077] The (i+1)th control point of the k-th derivative (k=0,1,2) of the j-th segment of the Bézier curve. It can be represented as:

[0078] (2)

[0079] in, , ;

[0080] The optimization variables for this problem are the coordinates of the control points for each trajectory segment and the total time. The time percentage for each trajectory segment is the ratio of the initial path length to the total path length for that segment. The optimization problem is as follows:

[0081] (3a)

[0082] (3b)

[0083] (3c)

[0084] (3d)

[0085] (3e)

[0086] (3f)

[0087] Equation (3a) is the minimum objective function There are multiple optimization objectives. The first is to minimize the trajectory jerk integral to maintain the stability of the mobile robot's motion; the second is to minimize the time; and the third is to minimize the penalty functional cost. , The weighting coefficients are given by equations (3b)-(3f), which represent the constraints. , Let be the coefficient matrix of the j-th convex polygon. This represents the coordinates of the (i+1)th control point of the j-th trajectory segment. , , They represent The k-th order states of the starting point of the first segment of the trajectory and the ending point of the m-th segment of the trajectory. Maximum speed; Indicates parameters The 1-k powers of the product, j=1,2,...,m; the specific definition of the penalty functional cost S is as follows:

[0088] make , , Define the tangential acceleration limiting function. Normal acceleration constraint function and angular velocity limit function as follows:

[0089] (6)

[0090] (7)

[0091] (8)

[0092] in , , , These are the maximum tangential acceleration, maximum normal acceleration, and maximum angular velocity, respectively.

[0093] Divide each trajectory segment into q discrete points at equal intervals (excluding the endpoints of the trajectory), let , , They represent the p-th discrete points of the j-th trajectory respectively. and The tangential acceleration limit, normal acceleration limit, and angular velocity limit are given, and the penalty functional cost S is:

[0094] (9)

[0095] Among them, the penalty function The definition is given in equation (10), and the orthogonality coefficients are... ;

[0096] (10)

[0097] Step S62: Solve the above optimization problem numerically using the open-source tools Cppad and ipopt to obtain the final feasible trajectory planning result.

[0098] The beneficial effects of this invention are:

[0099] 1. Path planning is more in line with kinematic characteristics than existing methods.

[0100] In the path planning section, this invention uses Dubins curves to connect the sampling node states. Compared to using straight lines, this is more in line with the kinematic characteristics of the mobile robot and is more conducive to the generation of subsequent trajectories. This is because a path with kinematic constraints greatly reduces the probability of unreasonable time allocation in subsequent trajectory planning, resulting in a higher quality generated trajectory. In addition, the heading angles of the intersection points (i.e., the task-designated locations) of paths in different time states are different, which can increase the probability of successful path planning in space-constrained environments. After the mobile robot reaches the task-designated location and completes the corresponding task, it can adjust its attitude by turning in place before moving on to the next task-designated location.

[0101] 2. High robustness of path planning

[0102] Different LTL formulas may result in Büchi automata of varying sizes. If the number of iterations in the path planning sampling method is constant, it may lead to over-solving or solution failure. For larger Büchi automata, a larger number of iterations should be chosen, and vice versa. To address this issue, the number of iterations in the path planning sampling method of this invention is automatically adjusted. When a new Büchi automaton state transition pair appears during sampling, the number of iterations increases by a given constant. This ensures a reasonable number of iterations for different LTL formulas, increasing the probability of successful solution and improving robustness.

[0103] 3. High-quality trajectory planning solution

[0104] The trajectory planning method proposed in this invention utilizes the differential flat model of mobile robots, which can constrain the trajectory based on the kinematics and dynamics of the mobile robot in a lower dimension, resulting in higher quality generated trajectories. Attached Figure Description

[0105] Figure 1 This is the overall flowchart of the mobile robot motion planning method based on linear sequential logic of the present invention;

[0106] Figure 2 It is the LTL formula A schematic diagram of the Büchi automata;

[0107] Figure 3 It is a sampling tree structure diagram;

[0108] Figure 4 These are the path planning results corresponding to the four LTL formulas;

[0109] Figure 5 These are the trajectory planning results corresponding to the four LTL formulas;

[0110] Figure 6 It is the LTL formula The relevant parameter curves for trajectory planning from the starting point to position p0. Detailed Implementation

[0111] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0112] In this invention, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number or order of the indicated technical features. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0113] Reference Figure 1 This application provides a motion planning method for mobile robots based on linear sequential logic, including the following steps:

[0114] Step S1: Construct a grid map and dilate the grid map according to the robot radius; construct the robot's sequential task and convert it into a Büchi automaton;

[0115] Step S2: Construct a prefix sampling tree based on the Büchi automata. ;

[0116] Step S3: Construct the prefix sampling tree based on the increment. Get candidate prefix path list ;

[0117] Step S4: Based on the Büchi automaton and the candidate prefix path list Constructing a suffix sampling tree ;

[0118] Step S5: Construct the suffix sampling tree based on the incremental changes. Obtain candidate suffix sampling paths, use the candidate suffix sampling paths to obtain the prefix path and suffix path with the minimum cost, and construct a safe corridor based on the minimum prefix path and suffix path.

[0119] Step S6: Based on the safety corridor, construct an optimization problem to plan the robot's motion trajectory.

[0120] In this embodiment, the prefix sampling tree and suffix sampling tree This is the key structure of the method.

[0121] Prefix sampling tree and suffix sampling tree Structure such as Figure 3 As shown (hereinafter collectively referred to as the sampling tree). This sampling tree is based on a finite set of Büchi automata. It is divided into multiple sampling subtrees, and the sampling subtrees can be used This indicates that the subscript is... Represents the state of the Büchi automaton; nodes of the sampled subtree can be represented by tuples. This indicates that the location information , Represents two-dimensional coordinates. Indicates the heading angle. Represents a node The parent node, Indicates connection and The shortest Dubins curve, Represents the node's value; sampled subtree and sampling subtree According to the conversion pair The regions of interest corresponding to the input alphabet are concatenated, where and Indicate two different states of the Büchi automaton; let Indicates the transformation pair Input alphabet, It corresponds to one or more regions of interest.

[0122] In this embodiment, step S1 specifically includes the following steps:

[0123] Step S11: Use the open-source algorithm Cartographer (Google's open-source mapping and localization algorithm) to construct a grid map, and inflate the map according to the robot's radius;

[0124] Step S12: Construct the robot's timing task, represent the robot's timing task using LTL formulas, and convert it into a Büchi automaton using the open-source function package spot.

[0125] Büchi automata can use tuples It means that among them A finite set representing states, The set representing the initial state. ; This indicates that the alphabet should be entered. This indicates an uncertain transfer relationship. ; Represents the set of accepting states. .

[0126] In this embodiment, there are four examples of the application of the LTL formula in step S12, which are as follows:

[0127] (1)

[0128] (2)

[0129] (3)

[0130] (4)

[0131] in, Indicates accessing in sequence. , , , , Five regions; Indicates that the order of access is not specified. , , Three regions; Indicates accessing in sequence. or , , , Four regions; Indicates that the order of access is not specified. , Two regions, visited in a loop at the end. , , Three regions; the Büchi automaton corresponding to formula (1) is as follows: Figure 2 As shown.

[0132] Step S13: Remove the transformation relations of multiple atomic propositions in the Büchi automaton that need to simultaneously satisfy the LTL formula.

[0133] In this embodiment, step S2 specifically includes the following steps:

[0134] Step S211: Initial node Add to prefix sampling tree In the middle, the initial position information , The initial coordinates of the mobile robot, Represents the i-th discrete heading angle. Let i = 0, 1, 2, ..., n, where the value of n determines the heading angle resolution. Indicates the initial state. ;

[0135] Step S212: Initial state Add it to the first list and search the Büchi automaton for its initial state. Find the neighboring nodes, obtain the state transition pairs to be expanded, and add them to the second list; create a new transition list. .

[0136] The first list represents the prefix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of The second list indicates A series of transformation pairs to be expanded ,in For two different states in the Büchi automaton; Transition list It is used to store nodes whose heading angle needs to be adjusted during the prefix sampling tree update process.

[0137] Prefix sampling tree The Büchi automaton is constructed based on the first and second lists, and both lists are updated simultaneously during construction. The update rule is: when the first list has a new Büchi automaton state... Search when joining Neighboring nodes form one or more transformation pairs, except The existing transformation pairs are added to the second list; when Successfully expanded the transformation pairs in the second list At that time, determine the sampling subtree and Has the connection node been accessed for the conversion pair? Input alphabet All corresponding regions of interest, if so, will Remove from the second list, also if Then Add to the first list (ignore if the state already exists in the first list).

[0138] Step S213: Set the iteration increment for prefix sampling C1 is a constant, and the prefix sampling iteration value is... Initialize to the number of transformation pairs in the second list and the iteration increment. The product of.

[0139] Step S214: Set the iteration count value ;

[0140] Step S215, if ,but If necessary, proceed to step S216; otherwise, exit the program.

[0141] Step S216, Given probability ,by The probability is sampled from the first list, with (1- The probability of ) is sampled from the second list to obtain the conversion pair. ;

[0142] Step S217: According to the conversion pair Input alphabet Location sampling is performed to obtain ;

[0143] Step S218: Traverse the prefix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve;

[0144] Step S219, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold;

[0145] Step S220: Traverse the sampling subtree Select the node that makes the point Parent node with minimum cost , The position is and update For point The corresponding Büchi automaton state;

[0146] Step S221, Solve from Time The shortest Dubins curve Q;

[0147] Step S222: Perform collision detection on the Dubins curve Q. If there is no collision, proceed to step S223; otherwise, return to step S215.

[0148] Step S223: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q;

[0149] Step S224: Add the new node Add to sampling subtree In the middle, and for the sampling subtree New nodes in Rewire nearby nodes to reduce the cost to nearby nodes;

[0150] Step S225, Judgment Check if the condition is met. If it is, proceed to step S229; otherwise, execute step S226.

[0151] Step S226: Add the new node Add to conversion list middle;

[0152] Step S227: Construct a series of nodes ,in x, y and The same, Let i = 0, 1, 2, ..., n, and add these nodes to the sampling subtree. middle;

[0153] Step S228: Update the first list and the second list, and whenever a new transformation pair is added to the second list, the iteration value is... Increase ;

[0154] Step S229: Adjust the conversion list by sampling Middle node If re-electing the parent node reduces the node's cost, then update the value. If the corresponding node is selected, no processing is performed. Finally, proceed to step S215.

[0155] Note: Number of iterations during the method execution. It will change as the second list is updated; whenever a new transformation pair is added to the second list, it will be set to... .

[0156] In this embodiment, step S3 specifically includes the following steps:

[0157] Step S31: Traverse the prefix sampling tree Middle accept state sampling subtree The nodes, among which To accept the status, ;

[0158] Step S32: Access each traversed node upwards to the root node through its parent pointer, thereby obtaining a series of candidate prefix paths, and add them to the candidate prefix path list. .

[0159] In this embodiment, step S4 specifically includes the following steps:

[0160] Step S411: Convert the prefix sampling tree Acceptance state sampling subtree Add the node to the suffix sampling tree The text also mentions that if a Büchi automaton state exists, it is specified that... ,satisfy , and arrive Input alphabet If the value is "1", then these nodes will be added to... The Büchi automaton needs to be set to the correct state. Updated to ;

[0161] Step S412, to Add to the third list and search for information about the Büchi automata. Obtain the neighboring nodes, retrieve the state transition pairs to be expanded, and add them to the fourth list; create a new transition list. .

[0162] The third list represents the suffix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of The fourth list represents A series of transformation pairs to be expanded ,in For two different states in the Büchi automaton; Transition list Used to store nodes whose heading angle needs to be adjusted during the suffix sampling tree update process.

[0163] Suffix sampling tree The system constructs a new list based on the third and fourth lists, updating both lists simultaneously. The update rules for the third and fourth lists are the same as those for the first and second lists.

[0164] Step S413: Set the iteration increment for suffix sampling Set the suffix sampling iteration value to a constant C2. The number of transformation pairs in the fourth list and The product;

[0165] Step S414: Set the iteration count value ;

[0166] Step S415, if ,but If necessary, proceed to step S416; otherwise, exit the program.

[0167] Step S416, Given probability ,by The probability is sampled in the third list, with (1- The probability of ) is sampled in the fourth list to obtain the conversion pair. ;

[0168] Step S417: According to the conversion pair Input alphabet Location sampling is performed to obtain ;

[0169] Step S418: Traverse the suffix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve;

[0170] Step S419, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold;

[0171] Step S420: Traverse the sampling subtree Select the node that makes Parent node with minimum cost , The position is and update for The corresponding Büchi automaton state;

[0172] Step S421, Solve from arrive The shortest Dubins curve Q;

[0173] Step S422: Perform collision detection on Q. If there is no collision, proceed to step S423; otherwise, return to step S415.

[0174] Step S423: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q;

[0175] Step S424, will Add to sampling subtree In the middle, and on middle Rewire nearby nodes to reduce the cost to nearby nodes;

[0176] Step S425, Judgment Check if the condition is met. If it is, proceed to step S429; otherwise, execute step S426.

[0177] Step S426, Add to conversion list ;

[0178] Step S427: Traverse candidate prefix paths Check if a node exists on the list. Its x, y and The same and its s is If it is, skip to step S428; otherwise, update the third and fourth lists, and whenever a new pair of transformations is added to the fourth list, the iteration value is updated. Increase After the update is complete, proceed to step S429;

[0179] Step S428, of and its descendant nodes added to In the corresponding sampling subtree, where let The parent node is and will Remove from the fourth list;

[0180] Step S429: Adjust the list by sampling Middle node If re-electing the parent node reduces the node's cost, then update the value. If the corresponding node is selected, no processing is performed. Finally, proceed to step S415.

[0181] In this embodiment, step S5 specifically includes the following steps:

[0182] Step S51: Traverse the suffix sampling tree Middle accept state sampling subtree The nodes, among which And by accessing each node up to the root node through the parent pointer, candidate suffix paths are obtained;

[0183] Step S52: Based on the candidate suffix paths, select the prefix path and suffix path with the lowest cost to complete the path planning part; visualize the path planning results of equations (1)-(4) on the RVIZ simulation platform, respectively corresponding to Figure 4 (a)-(d), where the raster map size is 15m x 15m;

[0184] Step S53: Divide the path obtained in step S52 according to the Büchi automaton state, with each path segment having the same Büchi automaton state.

[0185] Step S54: Divide each path into multiple line segments evenly at a certain step length. Generate a convex polygon around each line segment according to the grid map. Finally, multiple convex polygons are obtained. There are no obstacles inside the convex polygons. Multiple convex polygons form a safe corridor.

[0186] In this embodiment, step S6 specifically includes the following steps:

[0187] Step S61: Using the Bézier curve as the expression for the trajectory, and applying the differential flat model of the mobile robot, construct an optimization problem; n-order m-segment Bézier trajectory The expression is as follows:

[0188] (1)

[0189] Where the function expression function variables Dimension , for The (i+1)th control point of the j-th segment of the trajectory. Let be the scale factor of the j-th segment of the trajectory. , , ... Here are the termination times for each segment of the trajectory, and the total trajectory time is... ;

[0190] The (i+1)th control point of the k-th derivative (k=0,1,2) of the j-th segment of the Bézier curve. It can be represented as:

[0191] (2)

[0192] in, , ;

[0193] The optimization variables for this problem are the coordinates of the control points for each trajectory segment and the total time. The time percentage for each trajectory segment is the ratio of the initial path length to the total path length for that segment. The optimization problem is as follows:

[0194] (3a)

[0195] (3b)

[0196] (3c)

[0197] (3d)

[0198] (3e)

[0199] (3f)

[0200] Equation (3a) is the minimum objective function There are multiple optimization objectives. The first is to minimize the trajectory jerk integral to maintain the stability of the mobile robot's motion; the second is to minimize the time; and the third is to minimize the penalty functional cost. , The weighting coefficients are given by equations (3b)-(3f), which represent the constraints. , Let be the coefficient matrix of the j-th convex polygon. This represents the coordinates of the (i+1)th control point of the j-th trajectory segment. , , They represent The k-th order states of the starting point of the first segment of the trajectory and the ending point of the m-th segment of the trajectory. Maximum speed; Indicates parameters The 1-k powers of the product, j=1,2,...,m; the specific definition of the penalty functional cost S is as follows:

[0201] make , , Define the tangential acceleration limiting function. Normal acceleration constraint function and angular velocity limit function as follows:

[0202] (6)

[0203] (7)

[0204] (8)

[0205] in , , , These are the maximum tangential acceleration, maximum normal acceleration, and maximum angular velocity, respectively.

[0206] Divide each trajectory segment into q discrete points at equal intervals (excluding the endpoints of the trajectory), let , , They represent the p-th discrete points of the j-th trajectory respectively. and The tangential acceleration limit, normal acceleration limit, and angular velocity limit are given, and the penalty functional cost S is:

[0207] (9)

[0208] Among them, the penalty function The definition is given in equation (10), and the orthogonality coefficients are... ;

[0209] (10)

[0210] Step S62: Solve the above optimization problem using the open-source tools Cppad (for automatic differentiation) and ipopt (for solving nonlinear optimization problems) to obtain the final feasible trajectory planning result. , , , Based on the path planning solved in step eight, trajectory planning is performed and visualized on the RVIZ simulation platform. The trajectories corresponding to the four time-series tasks are as follows: Figure 5 As shown in (a)-(d). The velocity, angular velocity, tangential acceleration, and normal acceleration curves corresponding to the trajectory from the starting point to position p0 in the planning results of Equation (1) are respectively shown in (a)-(d). Figure 6 As shown in (a)-(d).

[0211] This invention uses linear temporal logic formulas to describe robot task requirements and transforms these formulas into Büchi automata using the open-source toolkit Spot. This automaton guides a path planning sampling method. As the sampling tree expands, the number of Büchi automaton states increases, as does the number of Büchi automaton state transition pairs to be expanded, until all Büchi automaton state transition pairs have been explored. During sampling, the static 2D map used for collision detection is acquired using the open-source algorithm Cartographer, and nodes are connected using Dubins curves. Trajectory optimization is then performed on the shortest path obtained from sampling that meets the temporal task requirements. First, a safe corridor is constructed based on the path. Then, a differential flat model of the mobile robot is used, with Bézier curves as the trajectory expression, to construct the optimization problem. Finally, the open-source tools Cppad and ipopt are used to solve the problem, obtaining a trajectory that satisfies the kinematics and dynamics of the mobile robot.

[0212] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A motion planning method for mobile robots based on linear sequential logic, characterized in that: Includes the following steps: Step S1: Construct a grid map and dilate the grid map according to the robot radius; construct the robot's sequential task and convert it into a Büchi automaton; Step S2: Construct a prefix sampling tree based on the Büchi automata. ; Step S3: Construct the prefix sampling tree based on the increment. Get candidate prefix path list ; Step S4: Based on the Büchi automaton and the candidate prefix path list Constructing a suffix sampling tree ; Step S5: Construct the suffix sampling tree based on the incremental changes. Obtain candidate suffix sampling paths, use the candidate suffix sampling paths to obtain the prefix path and suffix path with the minimum cost, and construct a safe corridor based on the minimum prefix path and suffix path. Step S6: Based on the safety corridor, construct an optimization problem to solve the mobile robot's motion trajectory.

2. The mobile robot motion planning method according to claim 1, characterized in that, Prefix sampling tree in step S2 and the suffix sampling tree in step S4 Collectively referred to as a sampling tree; The sampling tree is based on a finite set of Büchi automata. It is divided into multiple sampling subtrees, and the sampling subtrees can be used This indicates that the subscript is... Represents the state of the Büchi automaton; nodes of the sampled subtree can be represented by tuples. This indicates that the location information , Represents two-dimensional coordinates. Indicates the heading angle. Represents a node The parent node, Indicates connection and The shortest Dubins curve, Represents the node's value; sampled subtree and sampling subtree According to the conversion pair The regions of interest corresponding to the input alphabet are concatenated, where and Indicate two different states of the Büchi automaton; let Indicates the transformation pair Input alphabet, It corresponds to one or more regions of interest.

3. The mobile robot motion planning method according to claim 2, characterized in that, Step S1 specifically includes the following steps: Step S11: Use the open-source algorithm Cartographer to construct a grid map and expand the map according to the robot radius; Step S12: Construct the robot's timing task, represent the robot's timing task using LTL formulas, and convert it into a Büchi automaton using the open-source function package spot. Büchi automata can use tuples It means that, among them A finite set representing states, The set representing the initial state. ; This indicates that the alphabet should be entered. This indicates an uncertain transfer relationship. ; Represents the set of accepting states. ; Step S13: Remove the transformation relations of multiple atomic propositions in the Büchi automaton that simultaneously satisfy the LTL formula.

4. The mobile robot motion planning method according to claim 3, characterized in that, Step S2 specifically includes the following steps: Step S211: Initial node Add to prefix sampling tree In the middle, the initial position information , The initial coordinates of the mobile robot, Represents the i-th discrete heading angle. Let i = 0, 1, 2, ..., n, where the value of n determines the heading angle resolution. Indicates the initial state. ; Step S212: Initial state Add it to the first list and search the Büchi automaton for its initial state. Find the neighboring nodes, obtain the state transition pairs to be expanded, and add them to the second list; create a new transition list. ; The first list represents the prefix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of ; The second list represents the prefix sampling tree. A series of transformation pairs to be expanded ,in , For two different states in the Büchi automaton; Transition list Used to store the prefix sampling tree The node that needs to be adjusted during the update process; Prefix sampling tree The Büchi automaton is constructed based on the first and second lists, and both lists are updated simultaneously during the construction process. The update rule is: when the first list contains a new Büchi automaton state... Search when joining The neighboring nodes form one or more transformation pairs, except for the prefix sampling tree. Existing transformation pairs are added to the second list; when the prefix sampling tree... Successfully expanded the transformation pairs in the second list At that time, determine the sampling subtree and Has the connection node been accessed for the conversion pair? Input alphabet All corresponding regions of interest, if so, will Remove from the second list, and if Then Add it to the first list; if the state already exists in the first list, ignore it. Step S213: Set the iteration increment for prefix sampling C1 is a constant, and the prefix sampling iteration value is... Initialize to the number of transformation pairs in the second list and the iteration increment. The product; Step S214: Set the iteration count value ; Step S215, if ,but If necessary, proceed to step S216; otherwise, exit the program. Step S216, Given probability ,by The probability is sampled from the first list, with (1- The probability of ) is sampled from the second list to obtain the conversion pair. ; Step S217: According to the conversion pair Input alphabet Location sampling is performed to obtain ; Step S218: Traverse the prefix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve; Step S219, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold; Step S220: Traverse the sampling subtree Select the node that makes the point Parent node with minimum cost , The position is and update For point The corresponding Büchi automaton state; Step S221, Solve from Time The shortest Dubins curve Q; Step S222: Perform collision detection on the Dubins curve Q. If there is no collision, proceed to step S223; otherwise, return to step S215. Step S223: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q; Step S224: Add the new node Add to sampling subtree In the middle, and for the sampling subtree New nodes in Rewire nearby nodes to reduce the cost to nearby nodes; Step S225, Judgment Check if the condition is met. If it is, proceed to step S229; otherwise, execute step S226. Step S226: Add the new node Add to conversion list middle; Step S227: Construct a series of nodes ,in x, y and The same, Let i = 0, 1, 2, ..., n, and add these nodes to the sampling subtree. middle; Step S228: Update the first list and the second list, and whenever a new transformation pair is added to the second list, the iteration value is updated. Increase ; Step S229: Adjust the conversion list by sampling Middle node If reselecting a parent node reduces the cost of that node, then update the prefix sampling tree. If the corresponding node is selected, no processing is performed; finally, proceed to step S215.

5. The mobile robot motion planning method according to claim 4, characterized in that, Step S3 specifically includes the following steps: Step S31: Traverse the prefix sampling tree Middle accept state sampling subtree The nodes, among which To accept the status, ; Step S32: Access each traversed node upwards to the root node through its parent pointer, thereby obtaining a series of candidate prefix paths, and add them to the candidate prefix path list. .

6. The mobile robot motion planning method according to claim 5, characterized in that, Step S4 specifically includes the following steps: Step S411: Convert the prefix sampling tree Acceptance state sampling subtree Add the node to the suffix sampling tree The text also mentions that if a Büchi automaton state exists, it is specified that... ,satisfy , and arrive Input alphabet If the value is "1", then these nodes are added to the suffix sampling tree. The Büchi automaton needs to be set to the correct state. Updated to ; Step S412, to Add to the third list and search for information about the Büchi automata. Obtain the neighboring nodes, retrieve the state transition pairs to be expanded, and add them to the fourth list; create a new transition list. ; The third list represents the suffix sampling tree. The existing Büchi automaton states can be sampled from this list to obtain the subtrees for expansion. The conversion of The fourth list represents the suffix sampling tree. A series of transformation pairs to be expanded ,in For two different states in the Büchi automaton; Transition list Used to store suffix sampling trees The node that needs to be adjusted during the update process; Suffix sampling tree The system is built based on the third and fourth lists, and these two lists are updated simultaneously during the construction process. The update rules for the third and fourth lists are the same as those for the first and second lists. Step S413: Set the iteration increment for suffix sampling Set the suffix sampling iteration value to a constant C2. The number of transformation pairs in the fourth list and The product; Step S414: Set the iteration count value ; Step S415, if ,but If necessary, proceed to step S416; otherwise, exit the program. Step S416, Given probability ,by The probability is sampled in the third list, with (1- The probability of ) is sampled in the fourth list to obtain the conversion pair. ; Step S417: According to the conversion pair Input alphabet Location sampling is performed to obtain ; Step S418: Traverse the suffix sampling tree Mid-sampling subtree Find the node with the smallest distance to X. This distance is measured by the length of the Dubins curve; Step S419, Judgment If true, then Otherwise from Find a point on the Dubins curve of X. To satisfy Here, the distance between the two points is the length of the Dubins curve. Distance threshold; Step S420: Traverse the sampling subtree Select the node that makes Parent node with minimum cost , The position is and update for The corresponding Büchi automaton state; Step S421, Solve from arrive The shortest Dubins curve Q; Step S422: Perform collision detection on Q. If there is no collision, proceed to step S423; otherwise, return to step S415. Step S423: Construct a new node The cost is the cost of the parent node. The cost is the sum of the length of curve Q; Step S424, will Add to sampling subtree In the middle, and on middle Rewire nearby nodes to reduce the cost to nearby nodes; Step S425, Judgment If the condition is true, proceed to step S629; otherwise, execute step S626. Step S426, Add to conversion list ; Step S427: Traverse candidate prefix paths Check if a node exists on the list. Its x, y and The same and its s is If it is, skip to step S428; otherwise, update the third and fourth lists, and whenever a new pair of transformations is added to the fourth list, the iteration value is updated. Increase After the update is complete, proceed to step S429; Step S428, of and its descendant nodes added to In the corresponding sampling subtree, where let The parent node is and will Remove from the fourth list; Step S429: Adjust the list by sampling Middle node If re-electing the parent node reduces the node's cost, then update the value. If the corresponding node is selected, no processing is performed; finally, proceed to step S415.

7. The mobile robot motion planning method according to claim 6, characterized in that, Step S5 specifically includes the following steps: Step S51: Traverse the suffix sampling tree Middle accept state sampling subtree The nodes, among which And by accessing each node up to the root node through the parent pointer, candidate suffix paths are obtained; Step S52: Based on the candidate suffix paths, select the prefix path and suffix path with the lowest cost to complete the path planning part; Step S53: Divide the path obtained in step S52 according to the Büchi automaton state, with each path segment having the same Büchi automaton state. Step S54: Divide each path into multiple line segments evenly at a certain step length. Generate a convex polygon around each line segment according to the grid map. Finally, multiple convex polygons are obtained. There are no obstacles inside the convex polygons. Multiple convex polygons form a safe corridor.

8. The mobile robot motion planning method according to claim 7, characterized in that, Step S6 specifically includes the following steps: Step S61: Using the Bézier curve as the expression for the trajectory, and applying the differential flat model of the mobile robot, construct an optimization problem; n-order m-segment Bézier trajectory The expression is as follows: （1） Where the function expression function variables Dimension , for The (i+1)th control point of the j-th segment of the trajectory. Let be the scale factor of the j-th segment of the trajectory. , , ... Here are the termination times for each segment of the trajectory, and the total trajectory time is... ; The (i+1)th control point of the k-th derivative (k=0,1,2) of the j-th segment of the Bézier curve. It can be represented as: （2） in, , ; The optimization variables for this problem are the coordinates of the control points for each trajectory segment and the total time. The time percentage for each trajectory segment is the ratio of the initial path length to the total path length for that segment. The optimization problem is as follows: （3a） （3b） （3c） （3d） （3e） （3f） Equation (3a) is the minimum objective function There are multiple optimization objectives. The first is to minimize the trajectory jerk integral to maintain the stability of the mobile robot's motion; the second is to minimize the time; and the third is to minimize the penalty functional cost. , The weighting coefficients are given by equations (3b)-(3f), which represent the constraints. , Let be the coefficient matrix of the j-th convex polygon. This represents the coordinates of the (i+1)th control point of the j-th trajectory segment. , , They represent The k-th order states of the starting point of the first segment of the trajectory and the ending point of the m-th segment of the trajectory. Maximum speed; Indicates parameters The 1-k powers of the product, j=1,2,...,m; the specific definition of the penalty functional cost S is as follows: make , , Define the tangential acceleration limiting function. Normal acceleration constraint function and angular velocity limit function as follows: （6） （7） （8） in , , , These are the maximum tangential acceleration, maximum normal acceleration, and maximum angular velocity, respectively. Each trajectory segment is divided into q discrete points at equal intervals, excluding the endpoints of the trajectory. , , These represent the tangential acceleration limit, normal acceleration limit, and angular velocity limit values ​​at the p-th discrete point of the j-th trajectory segment, respectively. and The penalty functional cost S is: (9) Among them, the penalty function The definition is given in equation (10), and the orthogonality coefficients are... ; （10） Step S62: Solve the above optimization problem using the open-source tools Cppad and ipopt to obtain the final feasible trajectory planning result.

Images