Trajectory Planning of Automated Vehicles Using Real-Time Map Updates

The development of connected and automated vehicles (CAVs) presents a great opportunity to extend the current range of vehicle vision, by gathering information outside of its sensors. Two main sources could be aggregated for this extended perception; vehicles making use of vehicle-to-vehicle communication (V2V), and infrastructure using vehicle-to-infrastructure communication (V2I). In this paper, we focus on the infrastructure side and make the case for low-latency obstacle mapping using V2I communication. A map management framework is proposed, which allows vehicles to broadcast and subscribe to traffic information-related messages using the Message Queuing Telemetry Transport (MQTT) protocol. This framework makes use of our novel candidate/employed map (C/EM) model for the real-time updating of obstacles broadcast by individual vehicles. This solution has been implemented and tested using a scenario that contains real and simulated CAVs tasked with doing lane change and braking maneuvers. As a result, the simulated vehicle can optimize its trajectory planning based on information which could not be observed by its sensor suite but is instead received from the presented map-management module, while remaining capable of performing the maneuvers in an automated manner.


I. INTRODUCTION
Connected and Automated Vehicles (CAVs) have been met with an increasing amount of interest in the research of automated vehicles (AVs) lately. Their ability to communicate allows them to become more robust towards hard problems faced by AVs, where a correct assessment of traffic situations is safety critical, such as navigation in congested areas, interactions with emergency vehicles, accidents in the road, among others. This resilience is -amongst other causes -the result of the aggregating information supplied by all vehicles. Such swarm-like networks have shown strong efficiency for problem solving in traffic automation [1].
The associate editor coordinating the review of this manuscript and approving it for publication was Eyuphan Bulut .
The cornerstone of a functional and effective information sharing framework between individual CAVs is a robust communication network. In the automotive world, these technologies have been categorized depending on the agents involved; Vehicle-to-Vehicle (V2V), Vehicle-to-Infrastructure (V2I), and Vehicle-to-Network (V2N) communication have all been employed to enhance vehicle capabilities and to improve safety in the road. These technologies have been focused on the development of standardized services, and generally make use of specialized hardware. Alongside these developments, vehicles with their complex networks, increasing connectivity, sensors, and actuators, could also be considered, Internet-of-Things (IoT) entities [2].
IoT technologies and services have been on the rise in recent years, and a key solution for these has been the development of low-latency, low-overhead communication schemes and wide internet coverage focusing on connected homes, connected vehicles and smart cities. Among the established solutions available for IoT communication, MQTT is one of the most widely deployed protocols. MQTT provides a network for message delivery between information sources -i.e., publishers -, and users -i.e., subscribersthrough a broker entity. In the case of CAVs, this channel of communication can be leveraged to exchange telemetry information about the vehicle or driver, to monitor fleets of vehicles in the case of vehicle renting, to exchange high level control inputs such as changing automated system parameters remotely, or to provide channels for cooperation/perception in leveraging computational load to remote servers; as the one proposed in the below paper [3].
One particularly important bundle of information that CAVs must receive is a map of their surroundings, which can be used for locating themselves and navigating. Usecases for maps include the planning of trajectories prior to departure and during the maneuvering of these vehicles in complex environments, which often enclose other vehicles, pedestrians, and diverse obstacles. Currently, the way AVs navigate in their environments is mostly performed by internally adding real-time information to static maps -as the name suggests, these are not kept up-to-date and have low refresh frequencies. The mapping process is usually characterized by latencies in the order of years, primarily because of its costliness and resource-demanding nature. The dynamic component of the internal maps of the vehicles is therefore concatenated on-the-fly using their onboard sensors (LIDARs, cameras, etc.) [4].
In recent years, a compelling trend appeared in mapping for automated vehicle development; Namely, the use of crowdsourced data acquisition solutions. This paradigm provides a solution for high-frequency database updating. The process involves numerous data sources, which can provide raw environmental information for map construction [5]. This solution, however, has only seen scrutiny in the realm of non-connected automated agents. We argue that having access to frequently updated maps containing the real-time location of obstacles and other items enables more robust and substantially more effective trajectory planning for automated vehicles. In addition, crowdsourced data contribution facilitates the functionality of a mapping framework, which is able to provide the required information for connected vehicles in real time during maneuvering.
The intersection of the technologies previously outlined represents an opportunity to deploy a mapping solution into an IoT network for vehicles. The benefits of and ease of deployment of an IoT network, make it a real alternative to leverage the safety improvements of V2X communication. In addition to this, crowdsourced mapping is a great fit for these type of networks, leveraging information from all agents involved in the communication and improving reliability of the information provided, which in turn is vital for vehicle assessment of the road situation and decision making. This paper proposes a novel candidate/employed map (C/EM) management module, that calculates map updates (such as obstacles, vehicles, etc) and shares them through an MQTT network. To do so, this method uses real-time data from physical and simulated CAVs through an MQTT network, enabling a crowdsourcing-like framework. As a result, our solution provides a real-time updated map to CAVs, which is utilized in their internal trajectory planner for navigation. Finally, the individual vehicles are enabled to update their trajectories and switch between maneuvers based on the real-time-updated central maps on the MQTT-shared data. This framework was tested with lane change maneuvers specifically, however, it can be easily scaled and extrapolated to other maneuvers.
Our contributions in this article are: • We introduce a novel candidate/employed map management solution that enables dynamic mapping of obstacles in a central database based on information supplied by CAV sensors.
• We show that the presented solution makes use of V2N communication over the MQTT protocol providing a novel crowdmapping-based approach for data acquisition and low-latency map updating and publishing.
• We implement the real-time map management framework and integrate it with the trajectory optimization and maneuver planning control units of CAVs.
This article is structured as follows: First, we present the relevant literature, followed by a description of the automated driving framework utilized in this work. We then introduce our novel framework enabling real-time map updates and then provide insights on its integration with the vehicle network. Finally, we present and evaluate the results of testing the functionality of the combined system and formulate our conclusions.

A. LITERATURE REVIEW
In this section, we are introducing the state-of-the-art in relation to the research and results presented in the paper. First, we introduce works referring to the main maneuver presented in the article, the lane change. Secondly, the different automated vehicle trajectory planning methods are reviewed and finally, the methodology of the map creation and updating techniques are summarized.

1) LANE CHANGE
Automated Lane Change maneuvers can be tackled in three steps [6]. The first one is in the decision-making aspect, where the environmental variables (vehicles, obstacles, road shape, etc.) are taken into account in order to decide whether the lane change can be executed or not. The second step is more closely related to the trajectory planning aspect, where the trajectory followed by the vehicle is planned ahead in order to carry out a safe, feasible lane change. The final step is associated with path tracking, which aims to follow VOLUME 11, 2023 a predefined trajectory. From these steps, the first two are the points of interest in this paper.
Regarding the decision-making aspect there are numerous works using methods such as Search Algorithm [7], Finite State Machine (FSM) [8], Deep Reinforcement Learning [9], Game Theory [10], among others. Whereas, for the second step there are works using B-splines-curves [11], Bézier curves [12], further described in the following sub-section. All these works assume vehicles are not connected, relying only on their onboard sensors. Meanwhile, works such as [13], [14], or [15] use communications to coordinate vehicles lane changes in order to optimize the throughput of the road.

2) TRAJECTORY PLANNING FOR AUTOMATED VEHICLES
Trajectory planning has been widely researched in the literature since the early stages of CAV development. It usually considers the dynamic and/or kinematic models of the vehicle to go from a starting position to a final one [16]. The most relevant techniques are listed below.

a: NUMERICAL OPTIMIZATION
Numerical optimization techniques aim to minimize or maximize a function subject to different constrained variables. In the case of trajectory planning, this method is used to smooth previously calculated trajectories while taking into account the vehicle's kinematic. The main approaches are: Function Optimization [17], [18], [19] and Model Predictive Methods [20], [21], [22]. However, these methods have high computational costs, due to the optimization process, that takes place at each motion state, and depend on global waypoints [16], [23].

b: GRAPH-BASED
Graph-based techniques search for the best paths between the car's current state and a goal state in a state space represented as a graph [16], [23]. These techniques discretize the search space imposing a graph on an occupancy grid map with centers of cells acting as neighbors in the search graph [23]. The Graph-Based techniques can be used in both Global Route Planning and Local Trajectory Planning. However, in this study, only the ones used in trajectory planning are considered, being: State Lattice [24], [25], [26], Elastic Band [27], [28], [29], and A-star [23]. However, these methods require a lot of memory and cause heavy computational costs, leading to low planning efficiency [30].

c: GEOMETRY-BASED
The geometry-based techniques interpolate a set of previously defined waypoints to build a smooth trajectory that considers the vehicle's kinematics and dynamics, alongside the passenger's comfort [16], [23]. The most common methods are: clothoid curves [31], [32] and Bézier curves [33], [34]. This method presents lower computational costs than the two mentioned before. However, the clothoid curves still have a significant computational cost due to the integration process, as opposed to the Bézier curves that have lower computational cost, because the curvature is defined by control points.

3) MAPS FOR AUTOMATED VEHICLE NAVIGATION
A high-resolution map of the environment is required by most vehicles with higher levels of autonomy in order to ensure their safe and successful operation. In the literature, the map layers that can be used for this purpose are listed as follows [35]: • Road graph layer, which contains basic structural information about roads -e.g., number of lanes, intersections, and road segments.
• Lane-level maps containing information on lane markings and road segments with sub-centimeter accuracy. These maps are mostly based on detailed information collected from roads by road-infrastructure maintenance organizations.
• High Definition (HD) maps containing high-resolution features for road identification: Geometric spatial maps with labeled elements of road infrastructure, built-up of grouped and labeled points in 3D space. The raw data used in this case are unstructured point clouds, collected predominantly by costly active sensors -e.g., Light detection and ranging units (LIDARs) -that are then processed and labeled.
Methods that enable the creation and management of such maps are introduced in the following section. For clarity, these methods are also listed in Table 1.

a: CROWDSOURCED MAPS
Crowdsourcing is a social computing paradigm that appeared during the late 2000s. It is defined as a method for solving tasks, which require many resources to be used that are not necessary to be in a geographically or temporally joined environment [36].
Gathering data for map-related usage has also seen scrutiny in the literature; using crowdsourced data for achieving this goal is often referred to as Volunteered Geographical Information (VGI) [37], [38]. Arguably, the most widely spread VGI platform is OpenStreetMap (OSM) [39], which is an online platform that became an industry standard for VGI schemes in the past decade.
Crowdmapping, a subcategory of VGI applications, has appeared in the technical literature in the last few years and has shown great usability for traffic developments, such as passenger or freight vehicle automation. For self-driving vehicles, it is crucial to sense and interpret their surroundings correctly, in order to recognize their pose -i.e., location and orientation -and the traffic situation in their environment. As a result, the use of crowdmapping has appeared in several research endeavors [5], [40], [41] as well as industrial applications (Waymo [42], Lyft [43], Uber [44], and Google [45]).

b: MAP UPDATING FOR LOW-LATENCY MAPS
One key problem with maps used for automated vehicle control, and the decision-making algorithms contained therein, is the high latency with regard to the updates these maps receive [46]. The navigation systems of current AVs augment the built-in static maps with real-time sensed obstacles from their environment during movement [23]. As a possible solution, CAVs have been developed to be able to share their individual status with each other. A number of works have been published in the literature that tackle the problem of vehicle-to-infrastructure (V2I) or vehicle-to-vehicle (V2V) communication transfer times [47], [48] to optimize data sharing rates and as a result improve traffic flow.
Numerous methods have been published for optimized object detection and avoidance for singular -i.e., nonconnected -automated cars. Occupancy grid mapping algorithms exist for this domain, wherein the environment of a vehicle is divided into small regions, and the probability of it being taken up by an obstacle is constantly updated [49], [50]. Many articles show solutions for mapping dynamic regions in maps via different methods: using hysteretic validation of dynamic obstacle hypotheses [51], using map-decay [52], and particle filtering based on occupancy grids [53].
However, to the best of our knowledge, no work has been published so far that scrutinizes the online mapping of dynamic obstacles making use of the collaborating nature of connected and cooperating cars.

c: EXTERNAL PERCEPTION
External perception allows vehicles to gain knowledge about the existence and characteristics of objects, which are occluded or outside their sensors range. The foundation of this paradigm is that the knowledge base of individual vehicles can be increased and augmented using the information supplied by sensors of other vehicles or infrastructural elements (e.g., traffic signs, bus stops).
The use of external data can result in progress the smoothness of the navigation of automated vehicles as well as increasing road safety [54]. Müller et al. [55] extended the situational awareness of automated vehicles using cameras mounted on top of lampposts. They used the environmental model created via adopting both internally and externally obtained data to optimize and raise the safety levels of the complex maneuver of automated merging at an intersection.
Currently, to the best of our knowledge, there are no solutions that make use of crowdsourcing-based data acquisition for dynamic obstacle mapping using the external perceptionlike V2N approach.

II. AUTOMATED DRIVING FRAMEWORK
In this section, the automated driving (AD) framework used to test our approach is described, as well as the trajectory planning method used for the lane change maneuver. The AD framework is based on the six blocks architecture defined previously in [56] and [57], consisting of the following components: 1) Acquisition: This module is in charge of obtaining the information coming through the different sensors (LIDAR, Camera, GNSS, Laser, etc) and sending it to the perception module. 2) Perception: This module interprets these data and generates a representation of the environment and the surrounding obstacles. Furthermore, it provides vehicle positioning.

3) Communication: This module is in charge of obtain-
ing and processing the information coming from other vehicles (V2V), infrastructure (V2I), and/or the cloud (V2N). 4) Decision: Using the information from the perception and communication modules, this module generates a trajectory followed by the vehicle. It is subdivided into a global, behavioral, and local planner. 5) Control: This module is subdivided into a Longitudinal module, which produces throttle and brake signals in order to follow a speed reference and a Lateral module that produces steering signals to follow the trajectory. 6) Actuation: this block interprets the references produced by the control block, so real actuators or simulated vehicle models can read these signals. The six-block architecture framework is shown in Figure 1. As the decision module is of key importance in relation to the main subject of this study, it is further explained below.

A. DECISION MODULE
As previously mentioned, the decision module is divided into 3 blocks: • Global Planner: in this block, the first representation of a trajectory is carried out using a predefined OSM map. This definition follows a simplified version of the Lanelet2 standard [58], which is used in an A-star algorithm to find the optimal trajectory from point A to point B. Once this trajectory is found, a drivable space is defined containing the lanes in which the vehicle has to get through to reach its destination [59]. • Behavioral Planner: the decision-making is implemented in this block. To do so, a Finite State Machine (FSM) is defined with possible maneuvers that can be executed during the driving process. As has been mentioned, the maneuver analyzed in this work is a lane change, so, two states are possible: keep lane (the base state that is in charge of the basic driving task) and lane change, which will execute depending on environmental variables.
• Local Planner: with the information of the drivable space already supervised by the behavioral planner. This block generates a safe, smooth, and continuous trajectory with a speed profile to be tracked by the vehicle controllers. This trajectory is derived from the Bézier curves approaches presented in previous works [60], [61].

B. LANE CHANGE DEVELOPMENT
The first step in the maneuver is the behavioral planner FSM and the second step is the Bézier local planner. As it can be seen in Figure 2, the FSM states considered for this work, due to the maneuver analyzed, are the Lane Change and the Keep Lane. To go to the Lane change state there are three possible ways: 1) An obstacle is detected in the trajectory of the vehicle.
2) The user decides to change the lane.
3) The lane came to an end or it is closed. Whereas to go from the lane change state to the keep lane: 1) The lane change is finished.
2) The lane change is aborted. This work's main case study is when an obstacle is detected in the trajectory.
The second step is the generation of the smooth trajectory used to change lanes. To do so, the work done by Lattarulo et al. [62] was followed, in which different configurations of Bézier parametric curves are defined to design trajectories for different road components and maneuvers such as lane change.

III. REAL-TIME MAP UPDATES
In this section, our novel C/EM scenario is explained. First, we give an overview of the mapping setup, then provide a more in-depth explanation of the mapping algorithm.

A. MAPPING SETUP
Our aim with the mapping framework is to supply the dynamically updated map through the MQTT network: All vehicles  as well as the map management module are connected to the MQTT broker (Figure 3).
The individual CAVs are publishing pre-processed observation data obtained by their LIDAR sensors over a designated MQTT topic. The map management module is subscribed to this topic and thus receives observations from the vehicles. The information is then processed by the map management module (see Section III-B). If necessary, the updated map is published on another specified topic to the MQTT broker. Since all vehicles are subscribed to this latter topic, they immediately receive map updates when those get published.

B. MAP MANAGEMENT
The map management module's functionality is made possible using our novel C/EM scheme. In this approach, we define two separate maps that are constantly being updated: the candidate map (M c ) and the employed map (M e ). The candidate map is kept private whereas the employed map is published through the MQTT broker. The ap management module has been implemented using Python as a programming language.
The implemented software uses a state machine architecture. The states are initialization, idle, update map, broadcast map, and exit. During the initialization state, both the employed and the candidate maps are initialized as empty maps.
The functionality of the module following initialization is demonstrated in Figure 4. Once the idle state is reached, the map management module waits for new observations published by the individual CAVs over the ''obstacles'' topic of MQTT. When a new observation is received, the module decodes it into separate items containing the following parameter fields: The payload published over MQTT also contains information on the location of the sensor at the time of the observation -that is, the latitude and longitude coordinates of the LIDAR sensor of the vehicle. This information is necessary since the map management module calculates the observable area prior to the map comparison steps shown in the top row of Figure 4. Two observable maps are calculated as subsets from the employed and candidate map respectively, whose points are closer than a given radius -the default is 50 meters -to the sensor position at time of observation. This results in two sets of items: observable employed items and observable candidate items (denoted by P e, m , and P c, m , respectively).
Once the observable map subsets are calculated, the newly observed items are matched against the items in the observable subset of the employed map. Then, if any newly observed obstacles remain unmatched, these are matched against the observable subset of the candidate map. For the process steps of matching, see Section III-B1. Finally, if an observed item cannot be assigned to any previously mapped (employed or candidate) item, it is added to the candidate map.
Any obstacle or item listed in one of the maps has the following additional parameters: • Obstacle UID: a globally unique identifier for the item, which is assigned to it as it first appears on either the employed or the candidate map.  incremented or decremented during the map update steps (see Section III-B1).
• First timestamp: timestamp of the first observation of the item.
• Latest timestamp: timestamp of the most recent observation of the item.

1) MAP UPDATING
The maps are updated during every iteration when a new observation is received through MQTT (see 2 nd and 3 rd columns of Figure 4). The updating process finds the potential pairings between the observable subset of a given map and the list of new observations. It does so by calculating the cost matrix (C) using Gaussian Radial Basis Functions (RBF) between the numerical parameters -i.e., latitude, longitude, speed, length, and width -of the mapped and the newly observed items. C is formulated as It is calculated between mapped items P m i , i = (1, 2, . . . , n) and newly observed items P o j , j = (1, 2, . . . , m). The elements of C are calculated as where is the Gaussian Radial Basis Function formulated as In the above equation, k ∈ {latitude, longitude, speed, length, width}, and P m i and P o j are the numerical parameters of the observable obstacles: In (3), σ is the array of variances corresponding with the numerical parameters describing the individual items. It is formulated as Prior to matching the newly observed items with the mapped ones, a thresholding step is carried out on the cost matrix to forego any weak assignments. In Equation 2, β denotes the masking function: where T is the preset assignment cost threshold value for the map under inspection -i.e., employed or candidate. Once the cost matrix is finalized, the obstacle matching is calculated using the Hungarian algorithm [63]. This step is denoted by H(C) in lines 5 and 14 of Algorithm 1. After the matching is calculated, the numeric parameters of the matched mapped items are updated using an average filter.
where the subscript matched denotes that the averaging is performed on the mapped and the newly observed obstacles that have been assigned to each other by the Hungarian algorithm. α is the number of observations parameter of 67474 VOLUME 11, 2023 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.
the mapped item. If a newly observed item is assigned to a previously mapped item, then this counter is incremented. However, if the item should have been observed -i.e., it is in the observable subset of the map, but it has not been matched with any one of the observed items -then the number of observations counter of the item is decremented. For matched items, the latest timestamp parameter is also updated to contain the timestamp of this new observation. As shown in Figure 4, this assignment and updating is first executed on the employed map, after which the matched items are removed from the list of new observations. Next, the same matching-updating-removing sequence is performed for the candidate map. The remaining items in the new observation are then added to the candidate map with α = 1 -meaning that this item has only been observed once.
The final step of the map updating process is item promotion and demotion, which is performed once all the items of the new observation have either been assigned to an item in one of the maps or added as a new item to the candidate map. This is the process of transferring any strong candidate items from the candidate map into the employed map (i.e., promotion) and any weak employed items from the employed map to the candidate map (i.e., demotion). This is performed via hysteretic thresholding to minimize type I. and type II. errors (i.e., falsely demoted employed items and falsely promoted candidate items, respectively) of the transferring process. The threshold values for promotion and demotion therefore must be carefully optimized.
Finally, when the new observation is fully processed, the employed map must be published over MQTT by the map management module, if any of the following conditions are met: • An item was promoted from the candidate map to the employed map.
• An item was demoted from the employed map to the candidate map.
• The threshold for map re-broadcast has been reached. This threshold parameter contains two separate values -one in the temporal, and one in the spatial dimension. This clause is therefore activated, if the map has not been published for a sufficiently long time period -regardless of whether there have been any new observations published by CAVs -or if the spatial location -i.e., longitude and latitude -of a listed item has changed more then a preset value as a result of the parameter updating shown in Equation 7.
The steps of a map management cycle are shown in Algorithm 1.

IV. RESULTS AND DISCUSSIONS
In this section, the functionality of the map management solution is presented using simulated observation broadcasts. Then, the test scenario is described, taking into account the if P o ̸ = ∅ then 22: for item in P o do platforms and procedures used. Furthermore, the results are presented, and so is a discussion of them.

A. MAP MANAGEMENT MODULE
To test the functionality of the C/EM solution, a simulated sequence of observations was presented to the system. The candidate and the employed maps were compared using a series of experiments. First, we tested a simple simulation where a single obstacle was broadcast by the ego-vehicle over MQTT. The broadcast frequency was 3.33 Hz -i.e., a new broadcast was sent by the simulated ego-vehicle once every 0.3 s. While optimizing the threshold values of the mapping algorithm, we found that there is a trade-off between the long-term precision of the location of an employed obstacle and the time delay it takes the method to promote an obstacle. To understand this phenomenon, we tested the effect of different promotion threshold values on mapping.
We have carried out numerous tests varying the value of the promotion threshold. With the threshold pre-sets, we measured the following values during mapping: • Location differences between the latitude and longitude values of the obstacle in consecutive time frames were 67476 VOLUME 11, 2023 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.  calculated using the haversine formula. The total jitter was then calculated by summing up the individual location differences through the entire simulation.
• Time delay was calculated between the first time frame when the obstacle was broadcast by the ego-vehicle and  Figure 5.
Based on the results of this test, we opted to use the value 8 for the promotion threshold in our later experiments. We decided to use this value since according to our experimental results, it provided an acceptable time delay while ensuring tolerable noise in the mapped location of the obstacle.
Using the same test simulation as before we tested the functionality of the map management module. We have concluded that the mapping solution successfully included the obstacle to the employed map and published them again over a different MQTT topic. During the test shown in Figure 6, the obstacle was promoted to the employed map once the preset promotion threshold was reached -this value was set to 8, hence it took the mapping system 8 subsequent observations to promote the obstacle.
Moreover, we have tested the capabilities of our newly introduced C/EM solution using a simulation with numerous observed obstacles. Figure 7 shows the functionality of our implementation at a randomly selected time-frame of a series of observations acquired by an ego-vehicle in a simulated environment.

B. TEST SCENARIO
In order to test our solution, the Tecnalia Test Track (Figure 8) was used, where the real CAV detects static obstacles and sends them through MQTT to the map management module. Additionally, a virtual CAV is implemented, which receives information about the obstacles and executes the corresponding maneuver. The goal of the scenario is to show the capabilities of the map and the reaction of vehicles that can neither rely on internal perception nor V2V communication.
The real platform is a Renault Twizy 80 (Figure 9a), instrumented with different sensors such as LIDAR, GNSS, throttle, brake, and steering actuators. Furthermore, it is connecting to the internet safely, thanks to the protection of the IoTAC platform [64] and also enables V2X connectivity through a Commsignia OBU. The virtual CAV (Figure 9b) is simulated with a desktop computer with internet connectivity. Both platforms run the AUDRIC2 architecture described in Section II, with the difference in the model of the actuator. In the Twizy, the control outputs are sent to the real actuators, whereas the virtual CAV runs the architecture using a kinematic bicycle model as an actuator model in Robot Operating System (ROS). By implementing this testing method, situations where one vehicle can not count on its perception system or has one that is compromised can be mitigated in a safe manner without removing the conditions provided by real environments.

C. RESULTS
Mainly, two cases were studied. First, the vehicle's ability to change the lane and avoid obstacles, and second, if the lane change is not possible, execute other maneuvers (emergency braking, re-route, etc.). The first case can be observed in Figure 10 from the perspective of both vehicles. Figure 10a shows the Twizy (green box) and the obstacles detected (red boxes) on the real test track in ROS. These obstacles, due to the clustering algorithm used, were separated into smaller rectangles that were sent to the map management module. Figure 10b shows the virtual CAV (blue box) in ROS with a filtrated amount of obstacles, received by the map management module (red boxes). Furthermore, it can be seen that if one obstacle is in the same lane as the virtual CAV, the lane change trajectory is planned. Thus, the vehicle is able to avoid the obstacle and accomplish the maneuver.
The second case is shown in Figure 11, where obstacles block both lanes as can be seen in Figure 11a. The virtual CAV re-planning the trajectory can be observed in Figure 11b, however, as it detects the obstacle in the other lane, it aborts the maneuver and proceeds with braking in front of the obstacles.

D. DISCUSSIONS
The map management module is capable of introducing obstacles to the candidate and then, to the employed map and broadcasting it over MQTT (Figures 6 and 7). The time delay of this process is characterized by the promotion threshold. We have identified a trade-off between this time-delay and the location jitter of the individual employed obstacles -i.e., how much their location inappropriately changes over time. This trade-off is shown in Figure 5.
As shown in Figure 10 and Figure 11, the proposed approach combining the map management solution with the AUDRIC2 framework through MQTT connection presents promising results as in both cases the maneuvers were completed successfully. In the case of the lane change, the vehicle could react with enough distance (16 m) to perform a safe lane change in 3 s. That is within the time limits of an average lane change [65], [66]. This is achieved without compromising the driving performance since a smooth trajectory is generated without disturbing the lateral behavior of the vehicle. In the case of the braking maneuver, it can be observed that the lane change is activated, however, since it still detects an obstacle blocking the other lane, the virtual CAV proceeds with the braking maneuver, stopping at 6.3 m from the obstacle.

V. CONCLUSION
In this paper, we have introduced a map management module that communicates with CAVs using MQTT. We have shown that this novel solution is capable of introducing low-latency externally perceived obstacles to CAVs connected to it. Using a novel candidate/employed mapping solution, we have proven that objects observed on road segments by a physical CAV can be registered to a map with low-latency and thus known to other CAVs. We have demonstrated the abilities of the novel candidate/employed mapping in two maneuvers; first, a successful and safe lane change maneuver was carried out with sufficient clearance -that is not coming closer than 16 m to the obstacles and performing the lane change in 3 s. Secondly, if the lane change is unavailable (i.e., the traffic lane is blocked), the maneuver is aborted successfully and the vehicle stops at a safe distance (6.3 m) from all obstacles. Both maneuvers were successful using our solution based on information from external sources (i.e., sensors of other CAVs) gathered in the employed map and distributed over MQTT.

A. FUTURE WORK
Seeing the results of our novel proposed solution, we plan on further investigating its usage in more complex and realistic scenarios. One way to scale up the complexity presented in this study, is to increase the number of agents participating in the sharing of data; physical and virtual CAVs as well as independent road users. Another generalization that could be further researched, is to extend the study into more complex and general maneuvers, which would be representative of CAVs continuous operation, and its effects in and its effects with regard to crowdsourced mapping solutions.
The robust functionality of our proposed solution -i.e., its resilience to faulty data points and outliers -can be substantially increased by involving more data acquisition vehicles. Information resulting in faulty data points can be caused by malfunctions in the acquisition, perception, or even communication systems in the vehicle, including possible attacks in a vehicle asset. It is important to correctly model the effects in the mapping system, to have better understanding on the requirements needed for a high level of availability and correctness in the mapping solution proposed.
A way of testing this, would be to increase the number of physical as well as virtual vehicles in complex scenarios.
In the case of larger scenarios, a grid-based version of our solution could be implemented, that would enable the use of locally crowdsourced maps. In this setup, the map could be divided into smaller regions, over which different map management modules would be run simultaneously. This development however raises questions regarding the different modalities involved in our solution, which can be researched in a future study.
A key component for crowdsourcing is to define the task in a way that makes it as inclusive as possible. In the case of our proposed solution, this was achieved through the use of MQTT. Our MQTT solution used the currently supplied mechanisms of cybersecurity present in the protocol; each vehicle was authorized and whitelisted for the topics, and certificates were necessary to establish connectivity to the broker. Nonetheless, other channels of communication can be evaluated for the functionality of the map management solution in scenarios. An in depth analysis of risks and threats for this communication channels need to be performed before a full deployment. An alternative is to make use of V2X communication protocols -e.g., Dedicated Short-Range Communication (DSRC), cellular V2X -which are being studied and employed in the CAV sector, and extend their cooperative services by using our proposed solution.
MÁTYÁS SZÁNTÓ received the B.Sc. and M.Sc. degrees in mechatronics engineering from the Budapest University of Technology and Economics and the Ph.D. degree from the Department of Control Engineering and Information Technology, Budapest University of Technology and Economics, while being involved in the mentoring of students and the teaching of image processing subjects. His research interests include crowdsourcing for mapping purposes, 3D vision, and sim-to-real technologies. Since 1989, he has been the CEO of Telekodex Ltd. He is currently an Associate Professor with the Department of Control Engineering and Information Technology. His special fields are control engineering, computerized image processing, robotics, and industrial and energy informatics. He worked at many companies in the frame of co-operation participating in numerous industrial projects. He lead numerous research projects financed by national and EU grants. He has over 200 publications mostly in international journals and conference proceedings. He owns eight patents.