Robot motion planning method in unstructured environment based on spatiotemporal graph decision making

Abstract

The present invention proposes a robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making, comprising trajectory planning based on static map information and robot positioning information to obtain an initialized occupancy grid map based on an initial path trajectory; constructing a spatiotemporal graph based on dynamic environment information and the initial path trajectory; the spatiotemporal graph is an occupancy grid map constructed with the initial path trajectory and time as parameters and with other traffic participants as occupation points; based on the spatiotemporal graph, a decision is made on the mobile robot's own behavior according to the influence of other traffic participants on the initial path trajectory of the mobile robot in time and space; and corresponding trajectory replanning and optimization are performed according to the decision result, and the trajectory is output.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot motion planning, and in particular relates to a robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making. Background Art

[0002] Motion planning plays a connecting role in autonomous navigation, connecting environmental map information at the top and tracking control at the bottom. It is one of the core technologies for realizing autonomous navigation functions. However, current motion planning technologies often use graph search methods, sampling methods, etc., which simplify the mobile robot into a point mass and then plan a path from the starting point to the end point. Although existing methods can find a geometric path, they do not consider the impact of static obstacles on the movement of traffic participants in dynamic environments. Furthermore, existing methods do not fully consider the impact of the terrain environment in which the mobile robot is located on the smoothness of the movement, making it difficult to ensure the safety and smoothness of the robot during autonomous navigation in unstructured environments. Therefore, designing a safe and efficient robot motion planning method and process in dynamic environments is crucial for the realization of autonomous navigation of mobile robots in the wild. Summary of the Invention

[0003] In view of this, the present invention proposes a robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making, which includes:

[0004] Perform trajectory planning based on static map information and robot positioning information to obtain an initial occupancy grid map based on the initial path trajectory;

[0005] Constructing a spatiotemporal graph based on the dynamic environment information and the initial path trajectory; the spatiotemporal graph is an occupancy grid graph constructed with the initial path trajectory and time as parameters and with other traffic participants as occupation points;

[0006] Based on the spatiotemporal graph, the mobile robot makes decisions on its own behavior according to the spatiotemporal influence of other traffic participants on the initial path trajectory of the mobile robot; and performs corresponding trajectory replanning and optimization according to the decision result and outputs the trajectory.

[0007] In particular, during initial path planning, the initial occupancy grid map is constructed based on environmental perception information and robot positioning information; the transfer cost values ​​of different nodes in the initial occupancy grid map are set according to the perceived different terrain results and obstacle results, guiding the robot to move on smooth terrain, and performing path search based on the graph search algorithm to obtain a safe initial path.

[0008] In particular, the calculation method of the occupation point is as follows: the speed information of other traffic participants (v x ,v y), where v x ,v y are the speeds of the other traffic participants on the two coordinate axes respectively; process the other traffic participants according to the linear uniform motion model to obtain the predicted trajectory straight line equation ax+by+c=0; search the initial trajectory of the mobile robot for the straight line equation of the predicted trajectory that is less than the safety threshold TH safe The pose point cluster {(x1,y2),(x2,y2),…,(x m ,y m )}, where m is a positive integer; calculate the distance cluster {s1, s2, ..., s m}, calculate the time cluster {t1, t2, ..., t m}, so far we get the occupied point cluster occupy j ={(s1,t1),(s2,t2),…,(s m ,t m ),}, where the subscript j represents the subscript of the traffic participant that affects the initial path reference point.

[0009] In particular, the decision result is applicable to a complex dynamic environment of one or more traffic participants, including cruising, overtaking, avoiding or fast passing, and specifically includes the following steps:

[0010] Take the space-time motion boundary S of the mobile robot under the space-time graph end and T end , spatial motion boundary S end The length of the initial path is consistent with the time motion boundary T end It is the time upper bound of the motion process. First, the velocity distribution is performed on the space-time grid, and then the following judgment is made:

[0011] If the space-time diagram is in width=[0:S end ],height=[0,T end ] has no occupies affected by the movement of traffic participants. j At this time, the decision result is cruising, and the mobile robot starts cruising, maintaining the posture under the initial path;

[0012] If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j Appears on the left side of the space-time grid curve; and meets the overtaking condition: max(s i in occupy j )=S endand t end -max(t i in occupy j ) <T th , then perform the operation on occupy j The overtaking of the traffic participants represented by T th Indicates the target time to reach the local target point, max (s i in occupy j )、max(t i in occupy j ) represent traffic participants occupy j The largest space occupied and the largest time occupied;

[0013] If the occupied point only appears on the left side of the space-time occupied grid curve and does not meet the overtaking condition, the decision result is to avoid;

[0014] If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j If the time-space occupancy grid curve is on the right, the decision result is to quickly pass the traffic participant.

[0015] In particular, if the decision result is cruising, avoiding, or fast passing, the speed distribution module is activated to optimize the speed and acceleration under the time-space diagram;

[0016] If the decision result is overtaking, the trajectory replanning module and the speed distribution module are activated. The activated trajectory replanning module will plan two overtaking trajectories, and the speed distribution module will distribute the speed based on the initial path trajectory. The three obtained trajectories will be comprehensively evaluated and scored in the trajectory selection and output module, and the optimal trajectory will be selected as the local trajectory of the robot.

[0017] In particular, if the decision result is overtaking, the speed distribution module adds a quadratic programming linear constraint to the distributed speed according to the influence of different terrains on the speed.

[0018] In particular, if the decision result is overtaking, the trajectory replanning module of the unit sequentially executes the steps of generating a reference sequence and building and solving a hypergraph model; the reference sequence is determined as follows: generating an overtaking decision and taking the corresponding traffic participant as the overtaking target, obtaining the center point and contour points of the overtaking target projected on the horizontal plane at the time t = k, taking all contour points and the center point o at each time center The maximum distance is the maximum radius R of the overtaking target obj , multiply this by the safety factor Ec , obtain the safety radius R c =E c *R obj , at the overtaking target center point o center As the center of the circle, with R c Calculate two tangent lines passing through the robot coordinates on the circumference of the radius and set the tangent point and as a reference point; obtain reference points at multiple moments in this way, and use the set of tangent points on the same side of the overtaking target as a reference sequence, obtaining a total of two reference sequences; perform two independent hypergraph model optimizations, evaluate the independent optimized trajectories together with the initial path trajectory, and output the trajectory with the highest evaluation as the optimal trajectory.

[0019] In particular, the hypergraph model optimization includes adding a reference point cost function to the objective function of the hypergraph optimization model, determining the initial value of the position variable by linear sampling between the reference sequence, the starting point, and the end point, and selecting the position variable X whose initial value is within 1m of the reference point neighborhood. i ={(x i ,y i )|(x i ,y i )∈neibor(tp)}, for each position variable that meets the conditions, calculate the minimum value d between the position variable and the reference sequence min And the minimum safety time T tp,min , the reference sequence constraint is added to the objective function in the form of a penalty function:

[0020]

[0021] where r pmax is the safety radius of the reference sequence target, ∈ and n, S affect the approximation accuracy;

[0022] Obstacle constraints are added to the objective function through the following function:

[0023]

[0024] In particular, the independent optimized trajectories are evaluated together with the initial path trajectory, and the one with the highest evaluation is output as the optimal trajectory, which includes: performing a safety collision test on the trajectory output by the trajectory replanning module. If both overtaking trajectories do not meet the safety collision test, the initial trajectory generated by the speed distribution module that takes longer is selected; if at least one overtaking trajectory meets the safety collision test, a cost function is calculated between the overtaking trajectory that meets the requirements and the initial trajectory, and the cost function consists of acceleration cost, jerk cost, curvature cost, and time cost; the safety test includes: for the new replanned overtaking trajectory, constructing a spatiotemporal occupancy grid map containing other traffic participants under the trajectory, and depicting the coordinate points corresponding to the time and distance under the trajectory on the map. If any coordinate point falls on an inaccessible occupancy grid, then the requirements are not met. Otherwise, the trajectory is safe.

[0025] Beneficial effects:

[0026] (1) The present invention provides a method and process for robot motion planning in an unstructured environment based on spatiotemporal graph decision-making, and performs trajectory planning based on existing environmental map information and robot positioning information. On the one hand, the method provides an overall framework process for robot motion planning in a dynamic unstructured environment, and the implementation of some sub-modules is flexible and changeable, compatible with many algorithms, adaptable to different platforms, and convenient for debugging and improvement.

[0027] (2) The initial path planning takes into account the impact of different terrains on the planned path. By setting different node transfer costs, the robot is guided to move on smooth terrain, which greatly improves the comfort and safety of the robot's movement.

[0028] (3) The present invention utilizes a uniform motion model to fully consider the motion status of other traffic participants, and this is reflected in the trajectory planning results of the robot itself, thereby greatly reducing the risk of collision between the robot and other dynamic objects in a dynamic unstructured environment.

[0029] (4) The various decision-making results obtained based on the spatiotemporal graph in the present invention are applicable to the complex dynamic environment of multiple traffic participants. The decision-making results include various forms such as cruising, overtaking, avoidance or fast passing, which improves the intelligent planning ability of the robot.

[0030] (5) After the decision to overtake is made in the present invention, three trajectories will be planned and comprehensively evaluated and scored in the trajectory selection and output module, taking into account the path of the overtaking strategy and the original path as alternatives; the independent optimized trajectory will be evaluated together with the initial path trajectory, and the one with the highest evaluation will be output as the optimal trajectory; and two independent hypergraph model optimizations will be performed to improve the safety and reliability of the overtaking strategy.

[0031] (6) The post-decision speed distribution module in the present invention also considers the impact of bumpy terrain and sudden terrain on the smoothness of robot movement. By specially processing the objective function and speed constraints, the speed and acceleration of the trajectory points under such terrain are optimized, thereby improving the comfort of movement and reducing robot wear.

[0032] (7) The trajectory replanning module in the present invention sets the spatiotemporal reference sequence of the overtaking object and ensures the safety of the trajectory through collision detection. The speed distribution module ensures the redundancy of the planning system, thereby realizing the safe and autonomous "overtaking" behavior of the robot in a dynamic unstructured environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 Schematic diagram of the reference sequence for trajectory replanning in the present invention;

[0034] Figure 2 Schematic diagram of the robot motion planning process based on spatiotemporal graph decision-making in the present invention. DETAILED DESCRIPTION

[0035] The specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0036] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0037] The present invention proposes a robot motion planning method based on spatiotemporal graph decision-making, comprising:

[0038] Step 1: Perform trajectory planning based on static map information and robot positioning information to obtain an initial occupancy grid map based on the initial path trajectory;

[0039] During the initial path planning process, an occupancy grid map is constructed based on environmental perception information and robot positioning information. Based on the perceived terrain and obstacle results, transfer costs are set for different nodes in the occupancy grid map to guide the robot to move on smooth terrain. Graph search methods such as A* and Hybrid A* are used to search for paths and obtain a safe, collision-free initial path that conforms to the vehicle's kinematics.

[0040] Step 2: Robot motion decision. Based on the generation of the initial path and the acquisition of the motion status of dynamic traffic participants, a spatiotemporal graph is constructed and the robot motion decision is made to complete the robot's next motion behavior decision.

[0041] Step 2.1: Construct a space-time graph.

[0042] The space-time graph occupies a point (s i ,t i ) The physical meaning is that for the mobile robot body, at t iThe path length at time s i The position is unsafe. The entity will collide with other traffic participants at this time and space coordinate. i ,t i ) is calculated as follows:

[0043] Perceive and obtain the speed information of other traffic participants (v x ,v y ), where v x ,v y are the speeds of the other traffic participants on the two coordinate axes respectively; process the other traffic participants according to the linear uniform motion model to obtain the predicted trajectory straight line equation ax+by+c=0; search the initial trajectory of the mobile robot for the straight line equation of the predicted trajectory that is less than the safety threshold TH safe The pose point cluster {(x1,y2),(x2,y2),…,(x m ,y m )}, calculate the distance cluster {s1,s2,…,s m}, calculate the time cluster {t1, t2, ..., t m}, so far we get the occupied point cluster occupy j = {(s1,t1),(s2,t2),…,(s m ,t m ),}, where the subscript j represents the subscript of the traffic participant that affects the initial path reference point.

[0044] Step 2.2: Spatiotemporal graph decision making.

[0045] The spatiotemporal graph contains the speed and acceleration information of the mobile robot's plan. The spatiotemporal graph decision is to make decisions on the mobile robot's own behavior based on the influence of other traffic participants on the mobile robot's initial trajectory in time and space. The decision results include cruising, overtaking, avoiding, and fast passing. Cruising refers to the position (x) of the mobile robot tracking the initial trajectory when the initial path is not affected by other traffic participants in time and space. k ,y k ,θ k ) movement; overtaking means that when the initial path of the mobile robot is seriously affected by other traffic participants and it is unable to reach the local target point within the specified time, the mobile robot replans a safe trajectory to overtake the traffic participants; avoiding means that the mobile robot moves at a lower speed and waits for the influence of traffic participants on the initial trajectory in time and space to disappear, and then reaches the affected spatial position after the moment when the initial trajectory is affected has passed; fast passing means that the mobile robot moves at a higher speed and leaves the affected spatial position before the moment when the traffic participants influence the initial trajectory arrives.

[0046] The decision is made as follows: Take the spatiotemporal motion boundary S of the mobile robot under the spatiotemporal graph end and T end , the former is consistent with the length of the initial path, and the latter is the time bound of the movement process. First, a graph search is performed under the spatiotemporal occupancy grid graph. The graph search process has its own particularity, which is reflected in two aspects: ① the direction of node expansion, and ② the terminal moment under the spatiotemporal graph is variable. The former requires that the node can only expand to the right and the upper right, and the latter is in consideration of time optimality. The heuristic function is set to The termination condition is set to node s =S end ; Then make the following judgment:

[0047] (1) If the space-time diagram is in width = [0:S end ],height=[0,T end ] has no occupies affected by the movement of traffic participants. j , the mobile robot starts to cruise, and the cruising maintains the posture under the initial path;

[0048] (2) If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j Appears on the left side of the space-time grid curve and satisfies max(s i in occupy j )= S end and t end -max(t i in occupy j ) <T th , then perform the operation on occupy j Overtaking of the indicated traffic participant;

[0049] (3) If only the occupied point appears on the left side and the judgment condition (2) is not met, avoidance is performed;

[0050] (4) If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j If the time-space occupancy grid curve is on the right, the traffic participant will be quickly passed. The above judgment method is still applicable to the complex dynamic environment of multiple traffic participants.

[0051] Step 3: Trajectory replanning and optimization. If the decision result is cruising, avoiding, or fast passing, execute step 3.1. If the decision result is overtaking, because the initial trajectory is significantly affected by other traffic participants, continuing with the initial trajectory will result in poor safety and a long time consumption. Therefore, this decision activates the speed allocation module and the trajectory replanning module. The former allocates a lower speed based on the initial path to achieve safe driving, while the latter plans two overtaking trajectories. The overtaking decision executes steps 3.1 and 3.2.

[0052] Step 3.1: Original path speed distribution

[0053] This step is Figure 2 The velocity allocation module distributes velocity along the original trajectory, ensuring that the robot leaves the affected area before the moment other traffic participants affect the initial trajectory, or arrives at the affected area after the moment. Initial velocity allocation is performed using a graph search method, as shown in step 2.2. This method searches for an ST trajectory with a fixed initial state, a fixed terminal position, and terminal time relaxation. This trajectory represents the robot's velocity, acceleration, and jerk in the s direction. Secondary planning is performed on this ST trajectory to ensure that the robot's velocity and acceleration are smooth and meet the constraints.

[0054] The quadratic programming objective function adds the cost of considering the impact of terrain mutation on comfort. Assuming that the optimization variable in the quadratic programming proposition is According to the combination of planned path and terrain information, generally at the place where the terrain changes, such as the junction of horizontal ground and slope, if the speed and acceleration remain large, it will affect the stability and comfort. Add the following constraints to the objective function:

[0055]

[0056] The linear constraint of the quadratic optimization of velocity distribution is increased by considering the constraint of the bumpy ground on the velocity. Assume that the set of points to be optimized on the bumpy ground is Add the following constraints:

[0057]

[0058] Step 3.2: Overtaking trajectory replanning

[0059] Overtaking trajectory re-planning phase Figure 2 The trajectory replanning module is used in this process. This process includes the determination of the reference sequence and the construction of the hypergraph model. The former is used to provide the reference sequence cost function in the hypergraph model to achieve safe "overtaking" of dynamic targets. The reference sequence diagram is shown in the figure below. Figure 1As shown, it is generated in the following way: generate an overtaking decision and take the corresponding traffic participant as the overtaking target, obtain the center point and contour point of the overtaking target at the time t = k on the horizontal plane, and take all contour points and the center point o at each time center The maximum distance is the maximum radius R of the overtaking target obj , multiply this by the safety factor E c , obtain the safety radius R c =E c *R obj , at the overtaking target center point o center As the center of the circle, with R c Calculate two tangent lines passing through the robot coordinates on the circumference of the radius and set the tangent point and As reference points. This method obtains reference points at multiple moments. The set of tangent points on the same side of the overtaking target is used as a reference sequence, resulting in two reference sequences. Two independent hypergraph model optimizations are then performed. The independently optimized trajectories are evaluated together with the initial trajectory, and the trajectory with the highest evaluation is output as the optimal trajectory.

[0060] The objective function of the hypergraph optimization model adds a reference point cost function. The initial value of the position variable is determined by linear sampling between the starting point and the end point in the reference sequence. The position variable X whose initial value is within 1m of the reference point neighborhood is selected. i ={(x i ,y i )|(x i ,y i )∈neibor(tp)}, for each position variable that meets the conditions, calculate the minimum value d between the position variable and the reference sequence min And the minimum safety time T tp,min , the reference sequence constraint is added to the objective function in the form of a penalty function:

[0061]

[0062] where r pmax is the safety radius of the reference sequence target, ∈ and n, S affect the approximation accuracy.

[0063] Obstacle constraints are added to the objective function through the following function:

[0064]

[0065] During the optimization process, min will approach r pmax , when r pmax When it is large enough, the trajectory can be kept away from obstacles.

[0066] The penalty functions for speed and acceleration are:

[0067]

[0068] The objective function for time optimality is

[0069]

[0070] Step 4: Trajectory output. If the decision result is cruise, avoid, or fast pass, the trajectory is directly backtracked and the trajectory points containing posture, speed, and time information are output; if the decision result is overtaking, the generated candidate trajectories need to be scored and selected as follows:

[0071] The trajectory effects are scored and evaluated. First, the trajectory output by the trajectory replanning module is subjected to collision detection. If both overtaking trajectories fail the safety test, the initial trajectory generated by the speed distribution module, which takes longer, is selected. If at least one overtaking trajectory meets the safety test, the cost function of the overtaking trajectory that meets the requirements and the initial trajectory is calculated. The cost function consists of acceleration cost, jerk cost, curvature cost, and time cost.

[0072] Safety checking is performed as follows: for a newly planned overtaking trajectory, a spatiotemporal occupancy grid diagram of the trajectory, including other traffic participants, is constructed. The coordinate points corresponding to the time and distance of the trajectory are plotted on the diagram. If any coordinate point falls on an inaccessible occupancy grid, the requirement is not met; otherwise, the trajectory is safe.

[0073] The cost function is calculated as follows:

[0074]

[0075] For the trajectories output by the two modules, the velocity, acceleration, and jerk are calculated using the first-order, second-order, and third-order derivatives of the ST curve.

[0076]

[0077]

[0078]

[0079]

[0080] It is obvious to those skilled in the art that the embodiments of the present invention are not limited to the details of the above-mentioned exemplary embodiments, and that the embodiments of the present invention can be implemented in other specific forms without departing from the spirit or essential features of the embodiments of the present invention. Therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive, and the scope of the embodiments of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes that fall within the meaning and scope of the equivalent elements of the claims are included in the embodiments of the present invention. Any figure marks in the claims should not be regarded as limiting the claims involved. In addition, it is obvious that the word "comprising" does not exclude other units or steps, and the singular does not exclude the plural. Multiple units, modules or devices stated in the system, device or terminal claims may also be implemented by the same unit, module or device through software or hardware. Words such as first and second are used to indicate names and do not indicate any particular order.

[0081] Finally, it should be noted that the above implementation methods are only used to illustrate the technical solutions of the embodiments of the present invention and are not limiting. Although the embodiments of the present invention are described in detail with reference to the above preferred implementation methods, ordinary technicians in this field should understand that the technical solutions of the embodiments of the present invention can be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot motion planning method in an unstructured environment based on spatiotemporal graph decision making, characterized by: Perform trajectory planning based on static map information and robot positioning information to obtain an initial occupancy grid map based on the initial path trajectory; Constructing a spatiotemporal graph based on the dynamic environment information and the initial path trajectory; the spatiotemporal graph is an occupancy grid graph constructed with the initial path trajectory and time as parameters and with other traffic participants as occupation points; Based on the spatiotemporal graph, the mobile robot makes decisions on its own behavior according to the spatiotemporal influence of other traffic participants on the initial path trajectory of the mobile robot; and performs corresponding trajectory replanning and optimization according to the decision result and outputs the trajectory; The decision result is applicable to a complex dynamic environment of one or more traffic participants, including cruising, overtaking, avoiding or fast passing, and specifically includes the following steps: Take the space-time motion boundary S of the mobile robot under the space-time graph end and T end , spatial motion boundary S end The length of the initial path is consistent with the time motion boundary T end It is the time upper bound of the motion process. First, the velocity distribution is performed on the space-time grid, and then the following judgment is made: If the space-time diagram is in width=[0:S end ],height=[0,T end ] has no occupies affected by the movement of traffic participants. j At this time, the decision result is cruising, and the mobile robot starts cruising, maintaining the posture under the initial path; If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j Appears on the left side of the space-time grid curve; and meets the overtaking condition: max(s i in occupy j )=S end and t end -max(t i in occupy j ) <T th , then perform the operation on occupy j The overtaking of the traffic participants represented by T th Indicates the target time to reach the local target point, max(s i in occupy j )、max(t i in occupy j ) represent traffic participants occupy j The largest space occupied and the largest time occupied; If the occupied point only appears on the left side of the space-time occupied grid curve and does not meet the overtaking condition, the decision result is to avoid; If the space-time graph has an occupied point within the space-time boundary of the occupied grid, and the corresponding occupied point (s i ,t i )∈occupy j If the time-space occupancy grid curve is on the right, the decision result is to quickly pass the traffic participant.

2. The robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making according to claim 1, characterized in that: During initial path planning, the initial occupancy grid map is constructed based on environmental perception information and robot positioning information. The transfer cost values ​​of different nodes in the initial occupancy grid map are set according to the perceived different terrain results and obstacle results, guiding the robot to move on smooth terrain. Based on the graph search algorithm, a path search is performed to obtain a safe initial path.

3. The robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making according to claim 1, characterized in that: The calculation method of the occupation point is as follows: the speed information of other traffic participants is obtained by sensing (v x ,v y ), where v x ,v y are the speeds of the other traffic participants on the two coordinate axes respectively; process the other traffic participants according to the linear uniform motion model to obtain the predicted trajectory straight line equation ax+by+c=0; search the initial trajectory of the mobile robot for the straight line equation of the predicted trajectory that is less than the safety threshold TH safe The pose point cluster {(x1,y2),(x2,y2),…,(x m ,y m )}, where m is a positive integer; calculate the distance cluster {s1, s2, ..., s m }, calculate the time cluster {t1, t2, ..., t m }, so far we get the occupied point cluster occupy j ={(s1,t1),{s2,t2),…,(s m ,t m ),}, where the subscript j represents the subscript of the traffic participant that affects the initial path reference point.

4. The method for robot motion planning in an unstructured environment based on spatiotemporal graph decision-making according to claim 1, characterized in that: If the decision result is cruising, avoiding or fast passing, the speed distribution module is activated to optimize the speed and acceleration under the time-space diagram; If the decision result is overtaking, the trajectory replanning module and the speed distribution module are activated. The activated trajectory replanning module will plan two overtaking trajectories, and the speed distribution module will distribute the speed based on the initial path trajectory. The three obtained trajectories will be comprehensively evaluated and scored in the trajectory selection and output module, and the optimal trajectory will be selected as the local trajectory of the robot.

5. The method for robot motion planning in an unstructured environment based on spatiotemporal graph decision-making according to claim 4, characterized in that: If the decision result is overtaking, the speed distribution module adds a quadratic programming linear constraint to the distributed speed according to the influence of different terrains on the speed.

6. The robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making according to claim 4 or 5, characterized in that: If the decision result is overtaking, the trajectory replanning module of the unit sequentially executes the steps of generating a reference sequence and building and solving a hypergraph model; the reference sequence is determined as follows: generating an overtaking decision and taking the corresponding traffic participant as the overtaking target, obtaining the center point and contour points of the overtaking target projected on the horizontal plane at the time t = k, taking all contour points and the center point o at each time center The maximum distance is the maximum radius R of the overtaking target obj , multiply this by the safety factor E c , obtain the safety radius R c =E c *R obj , at the overtaking target center point o center As the center of the circle, with R c Calculate two tangent lines passing through the robot coordinates on the circumference of the radius and set the tangent point and as a reference point; obtain reference points at multiple moments in this way, and use the set of tangent points on the same side of the overtaking target as a reference sequence, obtaining a total of two reference sequences; perform two independent hypergraph model optimizations, evaluate the independent optimized trajectories together with the initial path trajectory, and output the trajectory with the highest evaluation as the optimal trajectory.

7. The method for robot motion planning in an unstructured environment based on spatiotemporal graph decision-making according to claim 6, characterized in that: The hypergraph model optimization includes adding a reference point cost function to the objective function of the hypergraph optimization model, determining the initial value of the position variable by linear sampling between the reference sequence, the starting point, and the end point, and selecting the position variable X whose initial value is within 1m of the reference point neighborhood. i ={(x i ,y i )|(x i ,y i )∈neibor(tp)}, for each position variable that meets the conditions, calculate the minimum value d between the position variable and the reference sequence min And the minimum safety time T tp,min , the reference sequence constraint is added to the objective function in the form of a penalty function: where r pmax is the safety radius of the reference sequence target, ∈ and n, S affect the approximation accuracy; Obstacle constraints are added to the objective function through the following function:

8. The robot motion planning method in an unstructured environment based on spatiotemporal graph decision-making according to claim 7, characterized in that: The independent optimized trajectories are evaluated together with the initial path trajectory, and the highest-evaluated one is output as the optimal trajectory. The following steps are performed: a safety collision test is performed on the trajectory output by the trajectory replanning module. If both overtaking trajectories do not meet the safety collision test, the initial trajectory generated by the speed distribution module that takes longer is selected; if at least one overtaking trajectory meets the safety collision test, a cost function is calculated between the overtaking trajectory that meets the requirements and the initial trajectory. The cost function consists of acceleration cost, jerk cost, curvature cost, and time cost. The safety test includes: for the new replanned overtaking trajectory, a spatiotemporal occupancy grid diagram containing other traffic participants is constructed under the trajectory, and the coordinate points corresponding to the time and distance under the trajectory are plotted on the diagram. If any coordinate point falls on an inaccessible occupancy grid, then the requirements are not met. Otherwise, the trajectory is safe.

Images