An intelligent agent trajectory planning control method and system based on an HDMap

Abstract

The present application relates to the technical field of vehicle automatic driving, in particular to a kind of intelligent Agent trajectory planning control method and system based on HDMap;The method mainly includes four steps of the initialization of vehicle Agent, formal path query, entry mode confirmation and automatic driving according to path;High-precision semantic map and the idea of intelligent Agent are used in the present scheme, and a kind of vehicle Agent implementation scheme for flexible operation along lane is presented;In the entry mode of vehicle Agent, the present scheme designs a kind of ingenious vehicle Agent entry mode from the angle of time and space;The present scheme utilizes lane center line segment information in map, regulates the motion trajectory of vehicle, and provides flexible acceleration setting interface, so that the speed of vehicle Agent can change in real time according to the demand of user or traffic condition while automatically driving along lane, which reserves interface for the access of subsequent vehicle anti-collision strategy.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, specifically to an intelligent agent trajectory planning and control method and system based on HDMap. Background Technology

[0002] Because road testing is too time-consuming and costly to optimize autonomous driving algorithms, and open road testing is subject to regulatory restrictions, traffic conditions, and difficulties in scenario reproduction, it poses significant safety risks. Therefore, autonomous driving simulation testing is now widely accepted in the industry.

[0003] Intelligent agents, also known as agent systems, are a concept in distributed artificial intelligence that emerged in the mid-1980s. Intelligent agents and multi-intelligence systems have experienced rapid development and have become a mature and promising research and application field.

[0004] In the field of autonomous driving simulation, each traffic participant in a virtual scenario (pedestrian, motor vehicle, motorcycle, bicycle, other unknown obstacles, etc.) can be abstracted as an intelligent agent. The behaviors of numerous agents constantly moving back and forth on the road constitute a random traffic flow; therefore, simulating the behavior of obstacle vehicle agents is crucial for autonomous driving simulation. In the real world, the behavior of obstacle vehicle agents is not random; it is constrained by kinematic laws, traffic rules, road conditions, and human driving behavior. Therefore, how to design path selection algorithms for vehicle agents based on these constraints has become a pressing problem in the industry. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this invention is to provide an intelligent agent trajectory planning and control method and system based on HDMap that enables autonomous vehicles to make more accurate decisions.

[0006] To solve the above-mentioned technical problems, the technical solution provided by the present invention is: the intelligent agent trajectory planning and control method based on HDMap, which includes the following steps:

[0007] S1: Initialize the vehicle agent;

[0008] S2: Confirm the vehicle's driving route, specifically by entering the vehicle Agent's initial location, destination location, and the IDs of the key lanes traversed in the order that determine the unique path from the starting point to the destination;

[0009] S3: Determine the entry method. Entry methods include time entry and location entry, which can be selected by the user. In time entry, the timer starts from the start of the simulation. When the time exceeds the user's preset duration_reaches_sec, the vehicle agent begins to enter. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range.

[0010] S4: The vehicle agent drives automatically according to the route.

[0011] Furthermore, in step S1, each Agent needs to predefine its own configuration. The configuration required by the user includes the starting point and ending point of the vehicle Agent's movement, the key lane ID that determines the order of the unique path, the initial velocity, and the entry method of the Agent.

[0012] Furthermore, the specific steps for initializing the vehicle agent are as follows:

[0013] S1.1: Obtain map data;

[0014] S1.2: Obtain the starting point location, ending point location, and IDs of the key lanes traversed in the order of the unique path from the starting point to the ending point for the vehicle Agent;

[0015] S1.3: Obtain vehicle agent entry configuration;

[0016] S1.4: Set the initial speed of the vehicle agent, let the value of latest_speed be the initial speed, the default initial speed value is 0;

[0017] S1.5: Set the initial acceleration of the vehicle agent to 0, that is, set the initial value of acc to 0;

[0018] S1.6: Calculate the initial heading value of the vehicle agent.

[0019] Furthermore, in step S2, the lane and lane id of the starting and ending positions are obtained by using HDMap::GetNearestLaneWithHeading() based on the initial and ending positions and map information.

[0020] Furthermore, the query method is to start from the current lane, search sequentially for the IDs of the key lanes passed through along the way according to the successor relationship of the lanes, and finally find the ID of the destination lane, and save the query path.

[0021] Furthermore, the entire step S2 includes

[0022] S2.1: Obtain the starting lane ID from the starting position and the ending lane ID from the ending position;

[0023] S2.2: Save the key lane IDs and the destination lane ID, which determine the unique path from the start point to the end point, into the queue key_lane_id_deque in sequence;

[0024] S2.3: Define queue variables path and path_deque;

[0025] S2.4: Push the starting lane_id_ to the path queue, and push path_ to the path_deque queue;

[0026] S2.5: Determine if the path_deque queue is not empty. If it is, proceed to S2.6. If not, set the Agent's movement status to false, exit, and return to the path query failure status.

[0027] S2.6: Pop a path from the path_deque queue, named curr_path;

[0028] S2.7: Retrieve the last lane ID in the curr_path queue, and name it last_lane_of_curr_path_id;

[0029] S2.8: Determine if key_lane_id_deque is not empty. If it is, retrieve a key lane id as target_lane_id. If not, save the current path curr_path, set the agent's movement status to true, exit, and return the path query success status.

[0030] S2.9: Determine if last_lane_of_curr_path_id is the target_lane id. If not, proceed to S2.10; if yes, return to S2.8.

[0031] S2.10: Iterate through all successor lane IDs of the lane corresponding to last_lane_of_curr_path_id, and determine whether each successor lane ID has been visited. If the successor lane ID has not been visited, construct a new path from curr_path, name it new_path, push the successor lane ID into it, and push new_path into the path_deque queue.

[0032] S2.11: Jump to S2.5.

[0033] Furthermore, the entire step S3 includes:

[0034] S3.1: If the entry configuration status is not configured, proceed to S3.2; otherwise, return and exit.

[0035] S3.2: Determine whether the entry configuration is based on time or configuration. If it is based on time, proceed to S3.3; if it is based on configuration, proceed to S3.4; otherwise, set the entry configuration status to failure and return to exit.

[0036] S3.3: Determine if the duration is greater than duration_reaches_sec. If yes, proceed to S3.6; otherwise, return and exit.

[0037] S3.4: Calculate the distance D between the main vehicle and the position;

[0038] S3.5: Determine if the distance D is less than distance. If yes, proceed to S3.6; otherwise, return and exit.

[0039] S3.6: Set the Agent Visibility property to true, record the current time, assign it to per_time, set the entry configuration status to successful, and return to exit.

[0040] Furthermore, the steps of the entire S4 process include:

[0041] S4.1: Determine whether the Agent's motion state and Agent Visibility attribute are true. If yes, proceed to S4.2; otherwise, return and exit.

[0042] S4.2: Get the current time as curr_time;

[0043] S4.3: Calculate the s value in the lane where the Agent is located, i.e., s_distance_in_curr_lane, based on the current lane and current position;

[0044] S4.4: Calculate the time interval, time_distance = curr_time - pre_time, and then assign curr_time to pre_time;

[0045] S4.5: Calculate the new speed new_speed, new_speed = latest_speed + acc * time_distance, where latest_speed is the most recent speed value and acc is the acceleration;

[0046] S4.6: s_distance_in_curr_lane+=0.5*(new_speed+latest_speed)*time_distance;

[0047] S4.7: latest_speed=new_speed;

[0048] S4.8: Determine if s_distance_in_curr_lane is less than the length of the current lane. If yes, proceed to S4.11; otherwise, proceed to S4.9.

[0049] S4.9: s_distance_in_curr_lane=s_distance_in_curr_lane;

[0050] S4.10: Retrieve the lane ID following the current lane ID in the path. If it exists, set the retrieved next lane as the current lane. If it does not exist, set the Agent's movement state to false and return to exit.

[0051] S4.11: The new position of the vehicle agent is calculated using s_distance_in_curr_lane and the interface LaneInfo::GetSmoothPoint();

[0052] S4.12: Calculate the heading value of the vehicle agent's driving direction using s_distance_in_curr_lane and the interface LaneInfo::Heading();

[0053] S4.13: Calculate the x and y values ​​of the Agent's speed from the heading value, new_speed_x = new_speed * cos(heading), new_speed_y = new_speed * sin(heading), and the new_speed_z value is 0;

[0054] S4.14: Calculate the x and y values ​​of the agent's acceleration from the vehicle's heading value, acc_x = acc * cos(heading), acc_y = acc * sin(heading), and acc_z is 0;

[0055] S4.15: Determine whether the vehicle agent has reached the destination. The criterion is whether the distance between the current position of the vehicle agent and the destination is less than 0.5m. If so, set the agent's movement status to false and return to exit. Otherwise, return to exit.

[0056] Furthermore, in step S4, two timers, a first timer and a second timer, are created with a period of 100ms. The first timer periodically executes the Agent entry determination interface and the Agent automatic driving interface along the path. The first timer calculates the Agent's speed, driving direction, and position information in real time and updates the Agent's motion state. The second timer periodically calls the Agent acceleration control interface to update the Agent's acceleration value in real time.

[0057] The aforementioned HDMap-based intelligent agent trajectory planning and control system includes an initialization and operation module, a path planning module, an entry method confirmation module, and an automatic driving module.

[0058] The initialization and operation module is used to initialize the vehicle agent;

[0059] The route planning module is used to confirm the vehicle's path.

[0060] The entry method confirmation module is used to determine the entry method, which includes time entry and location entry, and the user can choose one of the two. In time entry, the timer starts from the start of the simulation, and the vehicle agent starts to enter when the time exceeds the user's preset duration_reaches_sec. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range.

[0061] The automatic driving module is used to enable the vehicle agent to drive automatically according to the route.

[0062] Compared with existing technologies, the significant advantages of this solution are:

[0063] 1. This solution adopts the concepts of high-precision semantic maps and intelligent agents, and proposes a vehicle agent implementation scheme that can flexibly operate along the lane;

[0064] 2. Regarding the vehicle agent's entry method, this solution proposes a clever entry method for vehicle agents from both time and space perspectives;

[0065] 3. This solution utilizes lane centerline information in the map to standardize vehicle trajectory and provides a flexible acceleration setting interface, enabling the vehicle agent to automatically drive along the lane while its speed changes in real time according to user needs or traffic conditions, reserving an interface for the subsequent integration of vehicle collision avoidance strategies. Attached Figure Description

[0066] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0067] Figure 1 This is a flowchart of the control method described in this invention;

[0068] Figure 2 This is a diagram of the control system architecture described in this invention;

[0069] Figure 3 This is a schematic diagram of lane analysis in the control method described in this invention;

[0070] Figure 4 This is a flowchart illustrating the operation of the control system described in this invention. Detailed Implementation

[0071] The preferred embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0072] like Figure 1 As shown, the intelligent agent trajectory planning and control method based on HDMap described in this invention includes the following steps:

[0073] S1: Initialize the vehicle Agent. Each Agent needs to have its own configuration defined in advance. The configuration required by the user includes the starting point and ending point of the vehicle Agent's movement, the key lane ID that determines the order of the unique path, the initial velocity, and the entry method of the Agent.

[0074] The specific steps are as follows:

[0075] S1.1: Obtain map data;

[0076] S1.2: Obtain the starting point location, ending point location, and IDs of the key lanes traversed in the order of the unique path from the starting point to the ending point for the vehicle Agent;

[0077] S1.3: Obtain vehicle agent entry configuration;

[0078] S1.4: Set the initial speed of the vehicle agent, let the value of latest_speed be the initial speed, the default initial speed value is 0;

[0079] S1.5: Set the initial acceleration of the vehicle agent to 0, that is, set the initial value of acc to 0;

[0080] S1.6: Calculate the initial heading value of the vehicle agent.

[0081] S2: Confirm the vehicle's driving route. Specifically, based on the initial position and the destination position, and combined with map information, obtain the lane and lane ID of the starting position and the destination position using HDMap::GetNearestLaneWithHeading().

[0082] The entire step S2 includes

[0083] S2.1: Obtain the starting lane ID from the starting position and the ending lane ID from the ending position;

[0084] S2.2: Save the key lane IDs and the destination lane ID, which determine the unique path from the starting point to the end point, into the queue key_lane_id_deque in sequence;

[0085] S2.3: Define queue variables path and path_deque;

[0086] S2.4: Push the starting lane_id_ to the path queue, and push path_ to the path_deque queue;

[0087] S2.5: Determine if the path_deque queue is not empty. If it is, proceed to S2.6. If not, set the Agent's movement status to false, exit, and return to the path query failure status.

[0088] S2.6: Pop a path from the path_deque queue, named curr_path;

[0089] S2.7: Retrieve the last lane ID in the curr_path queue, and name it last_lane_of_curr_path_id;

[0090] S2.8: Determine if key_lane_id_deque is not empty. If it is, retrieve a key lane id as target_lane_id. If not, save the current path curr_path, set the agent's movement status to true, exit, and return the path query success status.

[0091] S2.9: Determine if last_lane_of_curr_path_id is the target_lane id. If not, proceed to S2.10; if yes, return to S2.8.

[0092] S2.10: Iterate through all successor lane IDs of the lane corresponding to last_lane_of_curr_path_id, and determine whether each successor lane ID has been visited. If the successor lane ID has not been visited, construct a new path from curr_path, name it new_path, push the successor lane ID into it, and push new_path into the path_deque queue.

[0093] S2.11: Jump to S2.5.

[0094] S3: Determine the entry method. Entry methods include time entry and location entry, which can be selected by the user. In time entry, the timer starts from the start of the simulation. When the time exceeds the user's preset duration_reaches_sec, the vehicle agent begins to enter. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range.

[0095] The entire step S3 includes:

[0096] S3.1: If the entry configuration status is not configured, proceed to S3.2; otherwise, return and exit.

[0097] S3.2: Determine whether the entry configuration is based on time or configuration. If it is based on time, proceed to S3.3; if it is based on configuration, proceed to S3.4; otherwise, set the entry configuration status to failure and return to exit.

[0098] S3.3: Determine if the duration is greater than duration_reaches_sec. If yes, proceed to S3.6; otherwise, return and exit.

[0099] S3.4: Calculate the distance D between the main vehicle and the position;

[0100] S3.5: Determine if the distance D is less than distance. If yes, proceed to S3.6; otherwise, return and exit.

[0101] S3.6: Set the Agent Visibility property to true, record the current time, assign it to per_time, set the entry configuration status to successful, and return to exit.

[0102] S4: The vehicle agent automatically drives according to the route; the entire S4 process includes:

[0103] S4.1: Determine whether the Agent's motion state and Agent Visibility attribute are true. If yes, proceed to S4.2; otherwise, return and exit.

[0104] S4.2: Get the current time as curr_time;

[0105] S4.3: Calculate the s value in the lane where the Agent is located, i.e., s_distance_in_curr_lane, based on the current lane and current position;

[0106] S4.4: Calculate the time interval, time_distance = curr_time - pre_time, and then assign curr_time to pre_time;

[0107] S4.5: Calculate the new speed new_speed, new_speed = latest_speed + acc * time_distance, where latest_speed is the most recent speed value and acc is the acceleration;

[0108] S4.6: s_distance_in_curr_lane+=0.5*(new_speed+latest_speed)*time_distance;

[0109] S4.7: latest_speed=new_speed;

[0110] S4.8: Determine if s_distance_in_curr_lane is less than the length of the current lane. If yes, proceed to S4.11; otherwise, proceed to S4.9.

[0111] S4.9: s_distance_in_curr_lane=s_distance_in_curr_lane;

[0112] S4.10: Retrieve the lane ID following the current lane ID in the path. If it exists, set the retrieved next lane as the current lane. If it does not exist, set the Agent's movement state to false and return to exit.

[0113] S4.11: The new position of the vehicle agent is calculated using s_distance_in_curr_lane and the interface LaneInfo::GetSmoothPoint();

[0114] S4.12: Calculate the heading value of the vehicle agent's driving direction using s_distance_in_curr_lane and the interface LaneInfo::Heading();

[0115] S4.13: Calculate the x and y values ​​of the Agent's speed from the heading value, new_speed_x = new_speed * cos(heading), new_speed_y = new_speed * sin(heading), and the new_speed_z value is 0;

[0116] S4.14: Calculate the x and y values ​​of the agent's acceleration from the vehicle's heading value, acc_x = acc * cos(heading), acc_y = acc * sin(heading), and acc_z is 0;

[0117] S4.15: Determine whether the vehicle agent has reached the destination. The criterion is whether the distance between the current position of the vehicle agent and the destination is less than 0.5m. If so, set the agent's movement status to false and return to exit. Otherwise, return to exit.

[0118] The intelligent agent trajectory planning and control system based on HDMap described in this invention has the following overall architecture design: Figure 2As shown, the system can be divided into two main parts: the Agent's internal system and the external system. The external system primarily provides map data and initialization configurations for the Agent's operation. Developers need to pass this data as parameters to the Agent's internal system. High-precision semantic map files are in .bin or .txt format and are stored according to the protobuf standard data structure. Loading and parsing of the high-precision semantic map can be achieved by porting and using the interfaces of the Apollo Map module. The loading method can use the HDMap's LoadMapFromFile() interface to load the map file.

[0119] The map uses the protobuf data protocol and includes basic information such as roads (center lines, lane lines, road surface materials, etc.), traffic elements (traffic lights and traffic signs), and surrounding elements (streetlights, bus stops, trash cans, green belts, and buildings). The Proto format definition of roads in the map is as follows:

[0120]

[0121]

[0122]

[0123] Among them, central_curve stores all points on the center line of the lane; predecessor_id stores all the predecessor lane IDs of the current lane; successor_id stores all the successor lane IDs of the current lane.

[0124] After being loaded and parsed by the Apollo Map module's interface, each lane in the map is parsed into a corresponding LaneInfo. LaneInfo contains the center points (points_) of each lane, the headings_ of each lane segment, the accumulated distance (accumulated_s_) of each segment, and the successor ID and predecessor ID of that lane. Specific details are as follows: Figure 3 As shown.

[0125] The system includes an initialization and operation module, a route planning module, an entry method confirmation module, and an automatic driving module;

[0126] The initialization and runtime module is used to initialize the vehicle agent;

[0127] The route planning module is used to determine the vehicle's path.

[0128] The entry method confirmation module is used to determine the entry method, which includes time entry and location entry, and the user can choose one of the two. In time entry, the timer starts from the start of the simulation, and the vehicle agent begins to enter when the time exceeds the user's preset duration_reaches_sec. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range.

[0129] The automatic driving module is used to enable the vehicle agent to drive automatically according to the route.

[0130] Specifically, the system operation process is as follows: Figure 4 As shown, the user constructs the program entry point `main`, loads the map, instantiates the agent, and passes in the configuration parameters required by the agent during `main`'s initialization. The specific implementation steps are as follows:

[0131] Step 1: Call the LoadMapFromFile() interface of apollo hdmap to load the apollo high-precision semantic map;

[0132] Step 2: Construct the agent object, passing in the necessary parameters. For example, pass in the map data loaded in Step 1, and set the agent's initial speed, starting position, ending position, key lane ID for determining the unique path, and entry method, etc., as initialization configurations;

[0133] Step 3: Based on the starting point, ending point, and key lane IDs that determine the unique path from the starting point to the ending point, call the agent query driving path interface to obtain the agent driving path;

[0134] Step 4: Create a timer with a period of 100ms to periodically execute the agent entry confirmation interface and the agent automatic path driving interface. This timer will calculate the agent's speed, driving direction, and position information in real time and continuously update the agent's movement status;

[0135] Step 5: Create a timer with a period of 100ms to periodically call the agent acceleration control interface and update the agent acceleration value in real time;

[0136] Step 6: Start two timers.

[0137] Compared with existing technologies, the significant advantages of this solution are:

[0138] 1. This solution adopts the concepts of high-precision semantic maps and intelligent agents, and proposes a vehicle agent implementation scheme that can flexibly operate along the lane;

[0139] 2. Regarding the vehicle agent's entry method, this solution proposes a clever entry method for vehicle agents from both time and space perspectives;

[0140] 3. This solution utilizes lane centerline information in the map to standardize vehicle trajectory and provides a flexible acceleration setting interface, enabling the vehicle agent to automatically drive along the lane while its speed changes in real time according to user needs or traffic conditions, reserving an interface for the subsequent integration of vehicle collision avoidance strategies.

[0141] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for intelligent agent trajectory planning and control based on HDMap, characterized in that, The method includes the following steps: S1: Initialize the vehicle agent; S2: Confirm the vehicle's driving route, specifically by entering the vehicle Agent's initial location, destination location, and the IDs of the key lanes traversed in the order that determine the unique path from the starting point to the destination; S3: Determine the entry method. Entry methods include time entry and location entry, which can be selected by the user. In time entry, the timer starts from the start of the simulation. When the time exceeds the user's preset duration_reaches_sec, the vehicle agent begins to enter. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range. S4: The vehicle agent automatically drives according to the route, including: S4.1: Determine whether the Agent's motion state and Agent Visibility attribute are true. If yes, proceed to S4.2; otherwise, return and exit. S4.2: Get the current time as curr_time; S4.3: Calculate the s value in the lane where the Agent is located, i.e., s_distance_in_curr_lane, based on the current lane and current position; S4.4: Calculate the time interval, time_distance = curr_time - pre_time, and then assign curr_time to pre_time; S4.5: Calculate the new speed new_speed, new_speed = latest_speed + acc * time_distance, where latest_speed is the most recent speed value and acc is the acceleration; S4.6: s_distance_in_curr_lane+=0.5*(new_speed+latest_speed)*time_distance; S4.7: latest_speed=new_speed; S4.8: Determine if s_distance_in_curr_lane is less than the length of the current lane. If yes, proceed to S4.11; otherwise, proceed to S4.

9. S4.9: s_distance_in_curr_lane=s_distance_in_curr_lane; S4.10: Retrieve the lane ID following the current lane ID in the path. If it exists, set the retrieved next lane as the current lane. If it does not exist, set the Agent's movement state to false and return to exit. S4.11: The new position of the vehicle agent is calculated using s_distance_in_curr_lane and the interface LaneInfo::GetSmoothPoint(); S4.12: Calculate the heading value of the vehicle agent's driving direction using s_distance_in_curr_lane and the interface LaneInfo::Heading(); S4.13: Calculate the x and y values ​​of the agent's speed from the heading value, new_speed_x = new_speed * cos(heading), new_speed_y = new_speed * sin(heading), and the value of new_speed_z is 0; S4.14: Calculate the x and y values ​​of the agent's acceleration from the vehicle's heading value, acc_x = acc * cos(heading), acc_y = acc * sin(heading), and acc_z is 0; S4.15: Determine whether the vehicle agent has reached the destination. The criterion is whether the distance between the current position of the vehicle agent and the destination is less than 0.5m. If so, set the agent's movement status to false and return to exit. Otherwise, return to exit.

2. The intelligent agent trajectory planning and control method based on HDMap according to claim 1, characterized in that, In step S1, each Agent needs to predefine its own configuration. The configuration required by the user includes the starting point and ending point of the vehicle Agent's movement, the key lane ID that determines the order of the unique path, the initial velocity, and the entry method of the Agent.

3. The intelligent agent trajectory planning and control method based on HDMap according to claim 2, characterized in that, The specific steps for initializing the vehicle agent are as follows: S1.1: Obtain map data; S1.2: Obtain the starting point location, ending point location, and IDs of the key lanes traversed in the order of the unique path from the starting point to the ending point for the vehicle Agent; S1.3: Obtain vehicle agent entry configuration; S1.4: Set the initial speed of the vehicle agent, let the value of latest_speed be the initial speed, the default initial speed value is 0; S1.5: Set the initial acceleration of the vehicle agent to 0, that is, set the initial value of acc to 0; S1.6: Calculate the initial heading value of the vehicle agent.

4. The intelligent agent trajectory planning and control method based on HDMap according to claim 3, characterized in that, In step S2, the lane and lane ID of the starting and ending positions are obtained by using HDMap::GetNearestLaneWithHeading() based on the initial and ending positions and map information.

5. The intelligent agent trajectory planning and control method based on HDMap according to claim 4, characterized in that, The query method is to start from the current lane, search for the IDs of the key lanes passed through in sequence according to the successor relationship of the lanes, and finally find the ID of the destination lane, and save the query path.

6. The intelligent agent trajectory planning and control method based on HDMap according to claim 5, characterized in that, The entire step S2 includes S2.1: Obtain the starting lane ID from the starting position and the ending lane ID from the ending position; S2.2: Save the key lane IDs and the destination lane ID, which determine the unique path from the starting point to the end point, into the queue key_lane_id_deque in sequence; S2.3: Define queue variables path and path_deque; S2.4: Push the starting lane_id_ to the path queue, and push path_ to the path_deque queue; S2.5: Determine if the path_deque queue is not empty. If it is, proceed to S2.

6. If not, set the Agent's movement status to false, exit, and return to the path query failure status. S2.6: Pop a path from the path_deque queue, named curr_path; S2.7: Retrieve the last lane ID in the curr_path queue, and name it last_lane_of_curr_path_id; S2.8: Determine if key_lane_id_deque is not empty. If it is, retrieve a key lane id as target_lane_id. If not, save the current path curr_path, set the agent's movement status to true, exit, and return the path query success status. S2.9: Determine if last_lane_of_curr_path_id is the target_lane id. If not, proceed to S2.10; if yes, return to S2.

8. S2.10: Iterate through all successor lane IDs of the lane corresponding to last_lane_of_curr_path_id, and determine whether each successor lane ID has been visited. If the successor lane ID has not been visited, construct a new path from curr_path, name it new_path, push the successor lane ID into it, and push new_path into the path_deque queue. S2.11: Jump to S2.

5.

7. The intelligent agent trajectory planning and control method based on HDMap according to claim 6, characterized in that, The entire step S3 includes: S3.1: If the entry configuration status is not configured, proceed to S3.2; otherwise, return and exit. S3.2: Determine whether the entry configuration is based on time or configuration. If it is based on time, proceed to S3.3; if it is based on configuration, proceed to S3.4; otherwise, set the entry configuration status to failure and return to exit. S3.3: Determine if the duration is greater than duration_reaches_sec. If yes, proceed to S3.6; otherwise, return and exit. S3.4: Calculate the distance D between the main vehicle and the position; S3.5: Determine if the distance D is less than distance. If yes, proceed to S3.6; otherwise, return and exit. S3.6: Set the Agent Visibility property to true, record the current time, assign it to per_time, set the entry configuration status to successful, and return to exit.

8. The intelligent agent trajectory planning and control method based on HDMap according to claim 1, characterized in that, In step S4, two timers, a first timer and a second timer, are created with a period of 100ms. The first timer periodically executes the Agent entry determination interface and the Agent automatic path driving interface. The first timer calculates the Agent's speed, driving direction, and position information in real time and updates the Agent's motion state. The second timer periodically calls the Agent acceleration control interface to update the Agent's acceleration value in real time.

9. A smart agent trajectory planning and control system based on HDMap, characterized in that, The system includes an initialization and operation module, a route planning module, an entry method confirmation module, and an automatic driving module; The initialization and operation module is used to initialize the vehicle agent; The route planning module is used to confirm the vehicle's path. The entry method confirmation module is used to determine the entry method, which includes time entry and location entry, and the user can choose one of the two. In time entry, the timer starts from the start of the simulation, and the vehicle agent starts to enter when the time exceeds the user's preset duration_reaches_sec. In location entry, the vehicle agent enters when the distance between the main vehicle and the user's preset position is within the user's preset distance range. The automatic driving module is used to enable the vehicle agent to drive automatically according to the route, including: S4.1: Determine whether the Agent's motion state and Agent Visibility attribute are true. If yes, proceed to S4.2; otherwise, return and exit. S4.2: Get the current time as curr_time; S4.3: Calculate the s value in the lane where the Agent is located, i.e., s_distance_in_curr_lane, based on the current lane and current position; S4.4: Calculate the time interval, time_distance = curr_time - pre_time, and then assign curr_time to pre_time; S4.5: Calculate the new speed new_speed, new_speed = latest_speed + acc * time_distance, where latest_speed is the most recent speed value and acc is the acceleration; S4.6: s_distance_in_curr_lane+=0.5*(new_speed+latest_speed)*time_distance; S4.7: latest_speed=new_speed; S4.8: Determine if s_distance_in_curr_lane is less than the length of the current lane. If yes, proceed to S4.11; otherwise, proceed to S4.

9. S4.9: s_distance_in_curr_lane=s_distance_in_curr_lane; S4.10: Retrieve the lane ID following the current lane ID in the path. If it exists, set the retrieved next lane as the current lane. If it does not exist, set the Agent's movement state to false and return to exit. S4.11: The new position of the vehicle agent is calculated using s_distance_in_curr_lane and the interface LaneInfo::GetSmoothPoint(); S4.12: Calculate the heading value of the vehicle agent's driving direction using s_distance_in_curr_lane and the interface LaneInfo::Heading(); S4.13: Calculate the x and y values ​​of the agent's speed from the heading value, new_speed_x = new_speed * cos(heading), new_speed_y = new_speed * sin(heading), and the value of new_speed_z is 0; S4.14: Calculate the x and y values ​​of the agent's acceleration from the vehicle's heading value, acc_x = acc * cos(heading), acc_y = acc * sin(heading), and acc_z is 0; S4.15: Determine whether the vehicle agent has reached the destination. The criterion is whether the distance between the current position of the vehicle agent and the destination is less than 0.5m. If so, set the agent's movement status to false and return to exit. Otherwise, return to exit.

Images