Mistaken car meeting decision-making planning method based on game planning

Abstract

The invention discloses a wrong-way meeting decision-making planning method based on game planning. The method comprises the following steps: outputting a current frame state based on perception information; updating the current self-vehicle state based on the current frame state and the self-vehicle state at the previous moment, judging whether the self-vehicle needs to enter a misplaced vehicle meeting state or not, and updating interaction obstacle information stored in the self-vehicle; judging whether the self-vehicle needs to be replanned or not, and executing the following steps according to a judgment result: if the judgment result is yes, planning an optimal coarse track of the self-vehicle through an interactive game, and then selecting a differential planarization device to access the optimal coarse track of the self-vehicle for back-end refinement; and if the judgment result is no, a differential planarization device is directly selected to be connected to the optimal self-turning coarse track for back-end refinement. The method has the beneficial effects that the self-vehicle sweeping effect and the road passing efficiency can be improved under the narrow road limit condition, and the overall optimality of the driving tracks of the self-vehicle and other vehicles in the current scene can be fully guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and in particular to a game planning-based decision-making planning method for giving way to other vehicles. Background Art

[0002] In recent years, with the development of driverless technology, autonomous driving has also been widely used in the field of unmanned cleaning technology, effectively alleviating the pressure of road cleaning. However, when autonomous cleaning vehicles are driving on public roads, how to achieve overall efficient road traffic while complying with traffic rules has become a key challenge. Traditional autonomous driving prediction decision planning often forces driverless cleaning vehicles to stop suddenly, or blocks the road and reduces the traffic efficiency of other vehicles, which not only limits its cleaning efficiency in specific scenarios, but also brings additional burden to public transportation. Summary of the invention

[0003] The purpose of the present invention is to provide a game planning-based decision-making planning method for yielding vehicles to solve the problems raised in the above-mentioned background technology.

[0004] To achieve the above object, the present invention provides the following technical solution: a game planning-based decision-making planning method for giving way to other vehicles, comprising the following steps: Output the current frame state based on the perception information; Update the current vehicle state based on the current frame state and the vehicle state at the previous moment, determine whether the vehicle needs to enter the staggered meeting state, and update the interactive obstacle information stored in the vehicle; Determine whether the vehicle currently needs to be replanned, and perform the following steps based on the judgment result: If the judgment result is yes, then the optimal rough trajectory of the ego vehicle is planned through interactive game, and then the differential flattener is selected to access the optimal rough trajectory of the ego vehicle for back-end refinement; If the judgment result is no, the differential flattener is directly selected to access the optimal ego vehicle rough trajectory for back-end refinement.

[0005] Preferably, outputting the current frame state based on the perception information includes: Update the longitudinal interaction obstacle history queue; Select candidate interactive obstacles; Determine the current frame state based on the traversable space of the interactive obstacles; Output the current frame finally.

[0006] Preferably, the updating of the longitudinal interactive obstacle history queue includes: Update the status information of obstacles in the planner; Obstacles are divided into offset oncoming obstacles and yielding rear obstacles based on whether they are in front of or behind the vehicle.

[0007] Preferably, the step of selecting candidate interactive obstacles includes: From all the staggered oncoming obstacles and yielding rear obstacles, select an obstacle that is closest to the current mileage of the vehicle; Select the obstacle closest to the vehicle's current mileage; Based on its status, determine whether to enter the wrong-way-giving state.

[0008] Preferably, the determining the current frame state based on the passable space of the interactive obstacle includes: Determine whether the current maximum remaining driving space is less than the minimum passable width. If it is greater and the previous frame is not in the missed-passing state, the missed-passing state will not be entered; Otherwise, the program will update the target information of the wrong vehicle giving way and the status of the wrong vehicle giving way.

[0009] Preferably, the output of the current frame finally includes a misaligned meeting state, a yielding state or a normal driving state, and outputs relevant information of the interactive obstacle.

[0010] Preferably, the updating of the current vehicle state based on the current frame state and the vehicle state at the previous moment, determining whether the vehicle needs to enter the staggered meeting state, and updating the interactive obstacle information stored inside the vehicle include: If the current ego vehicle is not in the state of misaligned meeting, and the current frame outputs the state of misaligned meeting, then the initial starting position of the ego vehicle is recorded, and the misaligned meeting obstacle information stored in the ego vehicle is updated, and the ego vehicle needs to be replanned; If the current ego vehicle is in the state of misaligned meeting, and the current frame outputs the state of non-misaligned meeting, then the ego vehicle will exit the state of misaligned meeting, and clear the information related to misaligned meeting in the ego vehicle; If the current ego vehicle is in a misaligned meeting state, and the current frame outputs the misaligned meeting state, the misaligned meeting obstacle information stored inside the ego vehicle will be updated, and it will be determined whether re-planning is required.

[0011] Preferably, the determining whether the vehicle currently needs to be replanned includes: Replan the state of entering the scene for the first time; If the vehicle has reached the last point of the previous plan, and if the current state machine is consistent with the previous state, re-planning is required; If the vehicle deviates from the last planned trajectory by a certain distance, angle, or turning angle threshold, it will be counted. If the count meets the threshold, re-planning is required. If the vehicle's last planned trajectory is counted after collision with other non-interactive obstacles, re-planning is required when the count meets the threshold; If the other vehicle deviates from the trajectory of the other vehicle predicted in the previous planning by a certain distance, the angle threshold is set and counted. If the count meets the threshold, re-planning is required.

[0012] Preferably, the method of planning the optimal rough trajectory of the vehicle through interactive game includes: For other non-interactive obstacles, a distance map is constructed according to the prediction and static obstacle fixed planning; Interactive rough planning is performed for the ego vehicle and other vehicles interacting with it, and the ego vehicle performs search tree sampling in the control space; The other car further searches and samples based on the state space; By adopting sampling trajectories and corresponding scenarios for the self-car and other cars, the optimal scenario is further obtained based on the game cost function.

[0013] Preferably, the differential flattener is connected to the optimal ego vehicle rough trajectory for back-end refinement, including: For static obstacles, the ego-vehicle rough trajectory constructs a traversable corridor for each point; For dynamic obstacles, interactive obstacles use the trajectory obtained by rough planning, and other non-interactive obstacles use the predicted trajectory.

[0014] Compared with the prior art, the present invention has the following beneficial effects: 1. The present invention fully considers the actual environment of the current road, and combines dynamic scene recognition with a finite state machine to ensure that the vehicle can flexibly switch the state of the vehicle behind it when it is misaligned and meet the vehicle, thereby improving the cleaning effect of the vehicle and the road traffic efficiency under extreme conditions of narrow roads; 2. The present invention uses a strategy of simultaneous search and sampling of two vehicles to obtain feasible trajectories in scenarios where misalignment will cause strong vehicle interaction. Therefore, various future situations of the two vehicles are taken into consideration, and the optimal rough trajectories of the self-vehicle and the other vehicle are selected based on the game cost function, which can fully guarantee the overall optimality of the driving trajectories of the self-vehicle and the other vehicle in the current scenario. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 is a flow chart of a method according to an embodiment of the present invention; Figure 2 is a specific flow chart of step S100 of an embodiment of the present invention; Figure 3 is a specific flow chart of step S200 of an embodiment of the present invention; Figure 4 This is a flow chart of planning the optimal rough trajectory of the vehicle through interactive game in step S300 of an embodiment of the present invention; Figure 5 This is a flowchart of determining whether the vehicle currently needs to be replanned in step S300 of an embodiment of the present invention; Figure 6A simulated trajectory diagram of a vehicle passing another vehicle traveling in the opposite direction in an embodiment of the present invention; Figure 7 A simulation trajectory diagram of a vehicle reversing to give way to another vehicle in an embodiment of the present invention; Figure 8 It is a simulation trajectory diagram of the vehicle giving way to the vehicle behind in an embodiment of the present invention. DETAILED DESCRIPTION

[0016] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0017] See also Figure 1 , the present invention provides a technical solution: a game planning-based decision-making planning method for giving way to other vehicles. The method can be that when an unmanned cleaning vehicle meets another vehicle, it can combine dynamic scene recognition and a finite state machine to ensure that the vehicle can flexibly switch the state of the vehicle behind it when giving way to other vehicles according to the actual environment of the current road, so that the unmanned cleaning vehicle can improve its own cleaning effect and road traffic efficiency under extreme conditions of narrow roads. The implementation scheme of the technical solution of the present application is that the unmanned cleaning vehicle detects the current frame based on the perception information, determines whether the current frame needs to enter the state of giving way to other vehicles and outputs the obstacle information that needs to be interacted with, and then updates the current state of the vehicle based on the state output by the current frame and the state of the vehicle at the previous moment, and then determines whether the vehicle needs to enter the state of giving way to other vehicles, and updates the interactive obstacle information stored in the vehicle; then determines whether the vehicle currently needs to be re-planned; Then, an interactive game planning scheme is used. In this process, multiple trajectories of the self-car and the other car are obtained through dual-car search sampling, and the optimal rough trajectory of the self-car and the other car is selected through the game cost function. Finally, the optimal rough trajectory of the self-car is selected as the reference trajectory of the differential flat optimizer for back-end fine optimization planning.

[0018] In one embodiment of the present invention, the implementation of the method includes the following steps: S100: Outputting the current frame state based on the perception information; S200: updating the current vehicle state based on the current frame state and the vehicle state at the previous moment, determining whether the vehicle needs to enter the staggered meeting state, and updating the interactive obstacle information stored in the vehicle; S300: Determine whether the vehicle currently needs to be replanned, and execute the following steps according to the determination result: If the judgment result is yes, then the optimal rough trajectory of the ego vehicle is planned through interactive game, and then the differential flattener is selected to access the optimal rough trajectory of the ego vehicle for back-end refinement; If the judgment result is no, the differential flattener is directly selected to access the optimal ego vehicle rough trajectory for back-end refinement.

[0019] In one embodiment of the present invention, the unmanned cleaning vehicle detects the current frame based on the perception information, determines whether the current frame needs to enter the missed vehicle giving way state, and outputs the obstacle information that needs to be interacted. Therefore, step S100 specifically includes: S110: Update the longitudinal interactive obstacle history queue.

[0020] In this embodiment, the first step specifically includes: Update the state information of obstacles in the planner. That is, remove the historical non-misaligned oncoming and yielding rear obstacles and add the misaligned oncoming and yielding rear obstacles within a certain range from the current vehicle (only vehicles and non-motor vehicles are counted).

[0021] Obstacles are divided into offset oncoming obstacles and yielding rear obstacles based on whether they are in front of or behind the vehicle.

[0022] S120: Select candidate interactive obstacles.

[0023] In this embodiment, specifically, step S120 includes: S121: Select an obstacle closest to the current mileage of the vehicle from all the staggered oncoming obstacles and yielding rear obstacles.

[0024] S122: Select the obstacle closest to the current mileage of the vehicle.

[0025] S123: Determine whether to enter the wrong-way-giving state based on its state.

[0026] S130: Determine the current frame state based on the passable space of the interactive obstacle.

[0027] In this embodiment, step S130 includes: S131: Determine whether the current maximum remaining driving space is less than the minimum passable width. If it is greater and the previous frame is not in the missed giving way state, the missed giving way state will not be entered.

[0028] S132: Otherwise, the program updates the wrong-passing target information and the wrong-passing status.

[0029] In this embodiment, specifically, the value of the counter is updated according to the situation of the current frame through the counter. If the previous frame of the current scene did not enter the wrong giving way state and the current counter is greater than a certain threshold, then the wrong giving way state will be entered; conversely, if the previous frame was the wrong wrong giving way state and the current counter is less than or equal to 0, then the wrong giving way state will be exited.

[0030] S140: Output the current frame finally.

[0031] In this embodiment, the final state output includes a staggered meeting state, a yielding state or a normal driving state, and relevant information of the interactive obstacle is output.

[0032] In one embodiment of the present invention, step S200 includes: S201: If the current ego vehicle is not in a misaligned meeting state, and the current frame outputs a misaligned meeting state, then record the initial ego vehicle starting position, update the misaligned meeting obstacle information stored in the ego vehicle, and set the ego vehicle to require re-planning; S202: If the current ego vehicle is in a misaligned meeting state, and the current frame outputs a non-misaligned meeting state, the ego vehicle will exit the misaligned meeting state, and clear the information related to the misaligned meeting in the ego vehicle; S203: If the current vehicle is in a misaligned meeting state, and the current frame outputs the misaligned meeting state, then the misaligned meeting obstacle information stored inside the vehicle will be updated, and it will be determined whether re-planning is required.

[0033] In this embodiment, step S200 is a step for the vehicle to give way to the vehicle in front, and the same applies to the vehicle to give way to the vehicle behind, which will not be described in detail.

[0034] In one embodiment of the present invention, the specific process of determining whether the vehicle currently needs to be replanned in step S300 is as follows: S301: re-planning the scene state entered for the first time; S302: If the vehicle has reached the vicinity of the last point of the previous plan, and if the current state machine is consistent with the previous state, re-planning is required; S303: If the vehicle deviates from the last planned trajectory by a certain distance, angle, or turning angle threshold, counting is performed, and re-planning is required when the count meets the threshold; S304: If the last planned trajectory of the vehicle is counted after collision with other non-interactive obstacles, re-planning is required after the count meets the threshold; S305: If the other vehicle deviates from the trajectory of the other vehicle predicted by our last planning by a certain distance, angle threshold and count, re-planning is required after the count meets the threshold.

[0035] In a specific embodiment of the present invention, in step S300, the optimal rough trajectory of the vehicle is planned through interactive game, and the specific process includes: S310: For other non-interactive obstacles, a distance map is constructed according to the prediction and static obstacle fixed planning.

[0036] S320: Execute interactive rough planning for the ego vehicle and other vehicles interacting with it, and the ego vehicle performs search tree sampling in the control space.

[0037] In this embodiment, the vehicle performs search tree sampling in the control space: the turning angle and step length are used as each step of expansion, and each step of expansion uses the distance map to eliminate the collision points with non-interactive obstacles. The maximum distance and maximum angle change are set as the cutoff extension, and finally M paths are obtained, and a trajectory with a fixed expected speed is assigned to each path.

[0038] S330: The other vehicle further searches and samples based on the state space.

[0039] In this embodiment, the last point on the predicted multimodal path is selected as the intended endpoint k of the other vehicle, and then K endpoints are sampled laterally based on the intended endpoint k using Gaussian distribution. The sampling range is the lane width predicted for each endpoint. StateLatticePlanner planning based on model prediction is performed for the K endpoints, N paths are sampled, and a distance graph is used to perform collision detection on each path. The points after collision detection are removed, and finally different speeds are assigned to the N paths, and M trajectories are also obtained in the end.

[0040] S340: By adopting the sampling trajectories and corresponding scenarios of the self-vehicle and other vehicles, the optimal scenario is further obtained based on the game cost function.

[0041] In this embodiment, each of the M trajectories of the self-vehicle and the other vehicles corresponds to a total of The simulation results are as follows: Figure 5 As shown in Figure 2, the process of obtaining the optimal scenario based on the game cost function is as follows: S3401: The self-car and other cars first need to calculate their respective current trajectory costs and , mainly includes two parts: first, the respective kinematic constraint costs, that is, whether the respective physical constraints are met; then judge whether the end point of the current trajectory is close enough to the set end point. For the other car, the game cost function is designed as the weighted sum of the distances from multiple intended end points, and the weight coefficient is the probability of each predicted intended end point, that is, calculating the uncertainty of the intention of the other car's movement.

[0042] S3402: Further calculation of the interactive game cost between the current trajectory of the vehicle and the current trajectory of other vehicles is required Specifically, it is calculated whether the two trajectories will collide and the severity of the collision: for the state of the ego vehicle trajectory at a certain time t, by calculating the number of collisions and distances of all states with the other vehicle trajectory within a certain range near time t, the calculation result is the interactive game cost of the ego vehicle's current trajectory at time t. For the entire trajectory, the game costs at all times are added together; the state of the other vehicle trajectory at a certain time t is also the same. The number of collisions and distances will be calculated with the trajectory state within a certain range of the ego vehicle trajectory at time t, and then the game cost will be obtained; finally, the interactive game costs of the ego vehicle and the other vehicle trajectory are added together to obtain the interactive game cost in the current scenario; the collision number and distance calculation are implemented using GPU acceleration to improve the overall computing efficiency.

[0043] S3403: Calculate the final cost, which is the weighted sum of the above three costs, i.e.

[0044] in, is the vehicle cost weight, is the weight of his car cost, is the interaction cost weight, For the cost of own car, For the price of his car, The cost of interaction.

[0045] S3404: Select the trajectory of the vehicle in the scene corresponding to the minimum game cost as the optimal rough trajectory of the vehicle, and the trajectory of the other vehicle as the optimal trajectory of the other vehicle in the current prediction plan. The simulation results are as follows: Figure 6 , Figure 7 and Figure 8 As shown, the game planning results of the game planning-based decision-making method for passing a vehicle, reversing to give way, and giving way to the vehicle behind provided by the present invention are respectively shown. Further, by continuously adjusting the weights, the selection priority of different scenarios can be adjusted, and the optimal trajectory of the other vehicle can be stored for re-planning judgment at the next moment.

[0046] In one embodiment of the present invention, in order to obtain a smoother and more accurate trajectory of the ego vehicle, a differential flat optimizer is used to access the optimal rough trajectory of the ego vehicle for back-end refinement. In the fine optimization part, the kinematic cost of the ego vehicle is used, including the longitudinal speed limit cost, the acceleration limit cost, and the front wheel angle limit cost. The environmental cost includes the following two parts: For avoiding static obstacles, a passable corridor is constructed for each point based on the rough trajectory of the vehicle. Each passable corridor is expressed by a convex polygon, and the vertex set of the convex polygon is:

[0047] in For each end point of a navigable corridor, is the vehicle trajectory point, are the coordinates of the vertex of the traversable corridor in the vehicle coordinate system, and R is the rotation matrix.

[0048] Then calculate each pose point to be optimized In every accessible corridor Constraint cost within:

[0049] For avoiding m dynamic obstacles, the vehicle will interact with the obstacle using the trajectory obtained by game planning as the predicted trajectory. , other non-strongly interactive obstacles use the GNN-based LTP prediction learning model to output the predicted trajectory , the model can output a high-precision predicted trajectory in the case of non-strong interaction. Calculate the minimum distance between each time trajectory point and the convex polygon of each dynamic obstacle predicted trajectory at the corresponding moment, and add a penalty constraint cost when the minimum distance threshold is less than the safety threshold. Here the distance is expressed using the signed distance: Finally, construct the constraint cost with dynamic obstacles:

[0050] in is the distance between the ego vehicle and each dynamic obstacle at time t.

[0051] The trajectory of the ego vehicle obtained through the above differential flattening fine optimization is smoother, which not only has good interactivity with the vehicles passing by, but also can avoid collisions with other static and dynamic obstacles.

[0052] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A game planning-based decision-making method for yielding vehicles, characterized in that: The steps include: Output the current frame state based on the perception information; Update the current vehicle state based on the current frame state and the vehicle state at the previous moment, determine whether the vehicle needs to enter the staggered meeting state, and update the interactive obstacle information stored in the vehicle; Determine whether the vehicle currently needs to be replanned, and perform the following steps based on the judgment result: If the judgment result is yes, then the optimal rough trajectory of the ego vehicle is planned through interactive game, and then the differential flattener is selected to access the optimal rough trajectory of the ego vehicle for back-end refinement; If the judgment result is no, the differential flattener is directly selected to access the optimal ego vehicle rough trajectory for back-end refinement.

2. The game planning-based decision-making planning method for yielding vehicles according to claim 1 is characterized in that: The outputting the current frame state based on the perception information includes: Update the longitudinal interaction obstacle history queue; Select candidate interactive obstacles; Determine the current frame state based on the traversable space of the interactive obstacles; Output the current frame.

3. The game planning-based decision-making planning method for giving way to other vehicles according to claim 2, characterized in that: The updating of the longitudinal interaction obstacle history queue includes: Update the status information of obstacles in the planner; Obstacles are divided into offset oncoming obstacles and yielding rear obstacles based on whether they are in front of or behind the vehicle.

4. The game planning-based decision-making planning method for yielding vehicles according to claim 3 is characterized in that: The step of selecting candidate interactive obstacles includes: From all the staggered oncoming obstacles and yielding rear obstacles, select an obstacle that is closest to the current mileage of the vehicle; Select the obstacle closest to the vehicle's current mileage; Based on its status, determine whether to enter the wrong-way-giving state.

5. The game planning-based decision-making planning method for yielding vehicles according to claim 4 is characterized in that: The determining the current frame state based on the traversable space of the interactive obstacle includes: Determine whether the current maximum remaining driving space is less than the minimum passable width. If it is greater and the previous frame is not in the missed-passing state, the missed-passing state will not be entered; Otherwise, the program will update the target information of the wrong vehicle giving way and the status of the wrong vehicle giving way.

6. The game planning-based decision-making planning method for yielding vehicles according to claim 5, characterized in that: The output of the current frame finally includes the state of misaligned meeting, the state of yielding to the rear or the state of normal driving, and the relevant information of the interactive obstacle will be output.

7. The game planning-based decision-making planning method for yielding vehicles according to claim 6 is characterized by: The updating of the current vehicle state based on the current frame state and the vehicle state at the previous moment, determining whether the vehicle needs to enter the staggered meeting state, and updating the interactive obstacle information stored in the vehicle include: If the current ego vehicle is not in the state of misaligned meeting, and the current frame outputs the state of misaligned meeting, then the initial starting position of the ego vehicle is recorded, and the misaligned meeting obstacle information stored in the ego vehicle is updated, and the ego vehicle needs to be replanned; If the current ego vehicle is in the state of misaligned meeting, and the current frame outputs the state of non-misaligned meeting, then the ego vehicle will exit the state of misaligned meeting, and clear the information related to misaligned meeting in the ego vehicle; If the current ego vehicle is in a misaligned meeting state, and the current frame outputs the misaligned meeting state, the misaligned meeting obstacle information stored inside the ego vehicle will be updated, and it will be determined whether re-planning is required.

8. The game planning-based decision-making planning method for yielding vehicles according to claim 7, characterized in that: The determining whether the vehicle currently needs to be replanned includes: Replan the state of entering the scene for the first time; If the vehicle has reached the last point of the previous plan, and if the current state machine is consistent with the previous state, re-planning is required; If the vehicle deviates from the last planned trajectory by a certain distance, angle, or turning angle threshold, it will be counted. If the count meets the threshold, re-planning is required. If the vehicle's last planned trajectory is counted after collision with other non-interactive obstacles, re-planning is required when the count meets the threshold; If the other vehicle deviates from the trajectory of the other vehicle predicted in the previous planning by a certain distance and angle threshold and counts, re-planning is required when the count meets the threshold.

9. The game planning-based decision-making planning method for yielding vehicles according to claim 8, characterized in that: The method of planning the optimal rough trajectory of the vehicle through interactive game includes: For other non-interactive obstacles, a distance map is constructed based on the prediction and static obstacle fixed planning; Interactive rough planning is performed for the ego vehicle and other vehicles interacting with it, and the ego vehicle performs search tree sampling in the control space; The other car further searches and samples based on the state space; By adopting sampling trajectories and corresponding scenarios for the self-car and other cars, the optimal scenario is further obtained based on the game cost function.

10. The game planning-based decision-making planning method for giving way to other vehicles according to claim 9, characterized in that: The differential flattener is connected to the optimal ego vehicle rough trajectory for back-end refinement, including: For static obstacles, the ego-vehicle rough trajectory constructs a traversable corridor for each point; For dynamic obstacles, interactive obstacles use the trajectory obtained by rough planning, and other non-interactive obstacles use the predicted trajectory.

Images