Occlusion-aware Motion Planning with Visibility Maximization via Active Lateral Position Adjustment

As the operational domain of autonomous vehicles expands, encountering occlusions during navigation becomes unavoidable. Most of the existing research on occlusion-aware motion planning focuses only on the longitudinal motion of the ego vehicle and neglects its lateral motion, resulting in output motion that can be overly conservative. This paper proposes a motion planner capable of actively adjusting the ego vehicle’s lateral position to minimize occlusions. The proposed planner is applicable in various scenarios and can function under perception uncertainty. This work also extends our previously proposed 3D visibility estimation approach for addressing occlusions caused by objects which are not present in HD maps. The proposed planner first generates candidate trajectories. The current and future visibility of each trajectory is then estimated using live LiDAR data and HD maps. These estimated visibilities are converted into visibility costs, which are then used to determine the optimal output trajectory in conjunction with other planning costs. The proposed planner is tested in three scenarios using the CARLA simulator: an occluded T-junction crossing, turning at a low-visibility corner and preparing to pass a parked vehicle, using live localization and object detection results. The experimental results reveal that the proposed planner allows the ego vehicle to minimize occlusions by diverging from the center of the lane and, consequently, to discover occluded vehicles earlier than a baseline planner in most situations. Moreover, occlusions caused by a parked vehicle not present in the HD maps were estimated correctly using our extended visibility estimation method.


I. INTRODUCTION
A S a result of intensive research and development, the operational scope of autonomous vehicles has been continuously expanding. Autonomous vehicles are no longer restricted to navigating only in simple and controlled environments and can now operate in increasingly complicated landscapes. In complex driving scenarios, ranging from urban streets full of obstacles and low-visibility intersections to hilly and winding roads in rural areas, it is almost impossible for autonomous vehicles to obtain complete information about the driving environment at all times due to occlusions, i.e., sensor blind spots caused by obstructions.
Although the ego vehicle's imperfect perception of the driving environment could potentially be alleviated through the use of external aids, such as permanent roadway sensor infrastructure or communications between vehicles, these services may not be available at all locations. Dependence on additional information from external sources to operate safely could limit the wide-scale deployment of autonomous vehicles since it raises the expense and limits the speed of autonomous vehicle adoption. Therefore, it is essential that autonomous vehicles are aware of the limitations of their sensing systems and are thus able to operate safely in complex environments where occlusions are present, even without external support.
In order to navigate safely in areas with occlusions, the visibility conditions of the driving environment must be considered during the motion planning stage. The topic of visibility-aware (or occlusion-aware) motion planning has gained increasing attention in recent years. However, despite the steadily growing body of research on motion planning in areas with limited visibility, the majority of the existing studies have focused almost exclusively on longitudinal motion planning, i.e., the autonomous vehicle is either assumed to be driving in the center of a lane or along a pregenerated trajectory. In many situations, strategically making the autonomous vehicle deviate from the lane's center can significantly reduce occlusion and increase the chance of discovering hidden traffic participants. Therefore, ignoring possible lateral motion by the autonomous vehicle can result in inefficient output motion.
A few studies have considered optimizing the ego vehicle's lateral position in order to proactively improve visibility. However, these approaches tend to be scenario-specific, i.e., they are designed for a particular scenario such as intersection crossing or passing parked vehicles. Another component missing from existing studies is the use of highly accurate visibility estimation. In most of these approaches, visible region estimation is typically done by representing surrounding obstacles with simple 2D geometric shapes and then performing ray-casting. While using 2D representation yields efficient computations, it also results in inaccurate estimation of visibility in areas with complexly-shaped obstacles or undulated terrain. Lastly, uncertainty within the perception modules, e.g., those used for localization and object detection, is rarely taken into consideration. This paper proposes a motion planner capable of actively adjusting the ego vehicle's lateral position to minimize occlusions by estimating current and future visibility. The planner proposed in this work is applicable in various scenarios and tolerates realistic uncertainty from perception modules. Additionally, this paper extends an approach for 3D visibility estimation proposed in our previous work [1], which allows the proposed system to handle occlusions caused by objects that are not present in high-definition (HD) maps by incorporating live sensing data from the onboard LiDAR unit during the estimation process.
The proposed planner first generates several physically feasible trajectories for the ego vehicle from its current position, with different lateral offsets from the reference path. The visibility conditions along each candidate trajectory are then estimated in 3D using HD maps and live sensing data in order to achieve accurate estimation in complex environments. Subsequently, the visibility conditions of each trajectory are quantified and converted into a visibility cost. Finally, the optimal trajectory is selected from the generated candidates based on visibility and other planning costs.
The proposed planner was tested in the CARLA simula-tor [2] in three different scenarios where occlusions were present. These test scenarios consisted of crossing a Tjunction, making a right turn, and preparing to pass a parked vehicle not present in the HD maps. In order to verify that the proposed planner is practical under conditions of perception uncertainty, actual localization and object detection results obtained in real-time were used. Our experimental results showed that the proposed planner could generate a trajectory that allowed the ego vehicle to minimize occlusions and consequently detect occluded traffic participants earlier than the baseline planner in most cases. Furthermore, our improved visibility estimation approach was able to accurately estimate occlusions caused by the parked vehicle, even though it was not present in the HD maps.
The main contributions of this paper are as follows: • A motion planner capable of actively adjusting the ego vehicle's lateral position to minimize occlusions, which is applicable in various scenarios and under conditions where perception uncertainty is present. • Extension of our previously proposed visibility estimation approach for handling occlusions caused by objects which do not appear in HD maps. The remainder of this paper is organized as follows: In Section II, previous research regarding occlusion-aware motion planning for autonomous vehicles is reviewed. In Section III, details of our proposed motion planner are provided, including conversion of visibility conditions into trajectory costs, as well as optimal trajectory selection. In Section IV, details of our approach for estimation of visibility at specified locations using 3D HD maps and live sensing data are provided. In Section V, the details of the experimental procedure used to assess the proposed planner are provided. Our experimental results when using the proposed and baseline planners in multiple variations of the three scenarios described earlier, i.e., T-junction crossing, turning right, and preparing to pass, are presented in Sections V-A, V-B, and V-C, respectively. Finally, the conclusions of this paper are given in Section VI.

II. RELATED WORK
As the operational design domain (ODD) of autonomous vehicles continues to broaden and encounters with occlusions become inevitable, the number of studies on occlusionaware motion planning has increased. Several of these studies have proposed utilizing additional information from external sources to ensure the safe navigation of autonomous vehicles. For example, in [3] additional sensing data from road junction infrastructure, transmitted via a low-latency mobile network, is used in parallel with the onboard perception module to plan longitudinal motion that is safe and comfortable for the passenger. Zhao et al. [4] proposed an algorithm for safely scheduling connected automated vehicles (CAVs) traveling through an uncontrolled intersection by sharing information among the vehicles. In [5], an approach for increasing the situational awareness of an automated bus by fusing information from other connected vehicles as well as from infrastructure with local sensors was introduced. While leveraging additional information from other sources can improve overall sensing coverage of surrounding environments, a high-quality network with low latency is typically required to transmit the necessary information in real-time reliably. Moreover, in the case of infrastructure, multiple sensors are often required to provide adequate coverage of an area, as demonstrated in [6] and [7]. These infrastructure requirements translate into extra time and investment needed for autonomous vehicles to operate safely. As a result, the widescale adoption of approaches that depend on information from external sources will likely be slow.
Instead of relying on additional data from external sources to reduce occlusion, many researchers have focused on ensuring that autonomous vehicles can navigate safely even without complete observation of the environment by identifying occluded regions and considering them during motion planning. Several different approaches for occlusion-aware planning have been proposed in recent years. One common approach used in various systems [8]- [15] to reduce the risk of collision with unobserved traffic participants is to assume that there are always virtual obstacles, moving at a constant speed from the boundaries of the visible region, that will intercept the autonomous vehicle's trajectory. By treating these approaching virtual traffic participants as real moving obstacles, time-to-collision (TTC) can be calculated. Thus, it is possible to regulate the speed of the ego vehicle so that collision with these potential occluded objects can generally be avoided. Since TTC is very inexpensive to calculate, the main advantage of these approaches is their efficiency. However, these methods are sometimes criticized for their lack of a safety guarantee, as virtual traffic participants are often treated as particles that lack dimensionality [8], [9]. Nonetheless, improvements in several aspects of this approach have been proposed to reduce the severity of this drawback. For instance, in [12] and [13], a reaction time or a delay before the autonomous vehicle starts braking is taken into account. In [13]- [15], potential collision zones are used instead of collision points to cover more possible collision scenarios.
In order to ensure the safety guarantee while navigating in occluded areas, Orzechowski et al. [16] proposed extending a set-based approach used for predicting the future occupancy of detected traffic participants, introduced in [17], to handle road users that are potentially occluded. The future states of virtual objects traveling from the edges of the visible area are over-approximated and represented as reachable sets. These reachable sets cover all possible future states of the virtual objects under a set of assumptions regarding their initial speed, position, orientation, and local traffic rules. Since the reachable sets include the worst-case scenario, the output motion of the ego vehicle is deemed safe if its future trajectory does not intersect with any of the reachable sets within the planning horizon. The reachable set prediction has been utilized in several studies, e.g., [18]- [25] The main benefits of using reachable sets to represent the possible future states of occluded traffic participants are the guarantee of safety and low computational cost. However, since reachable sets include the worst-case scenario, they often result in overly conservative control outputs for the ego vehicle. Nevertheless, several techniques have been proposed to address the problem of excessive caution when using reachable set prediction. For example, in [22] and [23] the reachable sets are repeatedly updated as new observations become available. As a result, more accurate estimates of the states of hidden traffic participants can be obtained and utilized to improve driving efficiency. In [24] and [25], overly cautious motion is alleviated by considering estimated future visibility and by encouraging the ego vehicle to actively improve its visibility of the driving environment.
In [26]- [32], motion planning in occluded areas is formulated as a Partially Observable Markov Decision Making Process (POMDP). By framing motion planning as a POMDP, sensing limitations and noise are directly reflected by the observation model. The state of hidden traffic participants can be estimated by utilizing both current and past observations via belief state updates. As a result, approaches based on a POMDP are less likely to generate overly conservative output motion than those that assume the worst-case scenario. Additionally, future developments of the surrounding situation over the planning horizon are considered during the decision-making process in a POMDP framework. For instance, Hubmann et al. [30] takes into account a predicted future field of view (FOV) while planning the output action of the autonomous vehicle. As a result, the vehicle actively positions itself to minimize occlusion. While a POMDP is very versatile and can be applied in various scenarios, solving for its optimal policy, which maps the current belief state to an output action, is often computationally expensive. Some prior studies have relied on pre-computing the policy offline [26]- [28] Nevertheless, as a result of continuous improvement in POMDP solvers and hardware performance over the years, several recent studies have successfully planned the output motion of an autonomous vehicle during execution time, although a coarsely discretized action space was used [29]- [32] Apart from the more common techniques mentioned above, some alternative approaches have also been proposed. In [33]- [36], a driving policy at an occluded, urban intersection is obtained using Reinforcement Learning (RL). In [37] and [38], efficient driving behaviors are learned directly from the driving data of expert drivers at low-visibility intersections using feature extraction. Similarly, driving data of experts is utilized in [39] to determine the parameters of a risk potential function for an uncontrolled intersection, which is ultimately used to decide the safe speed of the ego vehicle. In [40], contextual information is used to determine a probability distribution for the emergence of pedestrians from occluded sidewalks in order to regulate the longitudinal motion of the autonomous vehicle on a straight segment of road. Andersen et al. [41] proposed a framework for passing a parked vehicle that takes into account the size of the VOLUME 4, 2016 blind spot during trajectory optimization via Model Predictive Control (MPC). While these alternative approaches have been shown to generate efficient driving behavior when encountering occlusions, their main limitation is generalization, i.e., their ability to be applied in scenarios other than the ones they are specifically designed for.
The majority of the previously mentioned studies have focused exclusively on regulating the longitudinal motion of the autonomous vehicle, which is generally assumed to be traveling down the center of its lane or along a pregenerated trajectory. While planning complexity is reduced when lateral motion is ignored, this can lead to an overly conservative and suboptimal output motion. In many situations, adjusting the ego vehicle's lateral position can minimize occlusions and thus increase the chance of detecting hidden traffic participants earlier. Only a handful of existing studies have considered making the vehicle actively deviate from the current lane's center or its reference trajectory to improve visibility. The most notable works are [24], [25], [30], and [41]. Unfortunately, the methods proposed in [30] and [41] target specific scenarios, i.e., intersection crossing and passing a parked vehicle, respectively. As a result, their application may be limited. Moreover, in these studies, visibility estimation is performed in 2D, and surrounding objects are assumed to have simplistic shapes, such as rectangles. Although the approach proposed in [24] was shown to be applicable in various situations, it lacks highly accurate 3D visibility estimation. Zhang et al. [25] addressed these problems by utilizing Octomap [42] to estimate both current and future visibility. The importance of performing visibility estimation in 3D was shown in one of their experiments, in which an autonomous vehicle has to drive uphill and is faced with an occlusion caused by the hill's summit. Since the future visibility was estimated using a pre-constructed Octomap and predicted occupancy of other obstacles in [25], estimations of future occlusions resulting from objects that were not present during the offline generation of the Octomap may not be accurate. Additionally, ground truth information regarding the state of objects was obtained directly from the simulator. Therefore, performance with a realistic level of perception uncertainty was not tested.
As a result, there remains a need for a motion planning approach that is capable of minimizing occlusion by strategically adjusting the vehicle's lateral position based on highly accurate estimates of current and future visibility conditions, which is also applicable in various traffic scenarios, as well as being able to operate with a realistic level of perception uncertainty.

III. PROPOSED MOTION PLANNER
This section provides the operational details of our proposed motion planner. The planner proposed in this study is based on OpenPlanner, which is an open-source, integrated planner for mobile robot navigation in highly dynamic environments. OpenPlanner includes various components such as a global planner, a trajectory generator, and a behavior state gen- erator. 1 . The difference between our proposed planner and OpenPlanner is mainly in the local planning stage, where our planner generates a set of candidate trajectories and then selects the best trajectory. Additionally, the proposed planner includes a visibility estimator that can quantify the visibility condition of a specified location. Unlike the local planner in OpenPlanner, the proposed planner also considers the visibility condition of the candidate trajectories in addition to other standard planning criteria such as the risk of collision with detected obstacles, lane center deviation, and stability.
The overall architecture of our proposed planner is described in Section III-A. In Section III-B, integration of the visibility cost of the candidate trajectories, which is the primary improvement over the original OpenPlanner, is presented in detail. Fig. 1 shows the overall architecture of our proposed planner, which consists of three main components: a global planner, a local planner, and a visibility estimator. Since the proposed planner shares a similar design with OpenPlanner, some of its components, namely the global planner and the behavior state generator inside the local planner, are adopted into the planner proposed in this work. These components are shown with dashed boundaries in Fig. 1.

A. PLANNER ARCHITECTURE
The objective of the global planner is to find the optimal route from the vehicle's starting position to its destination. A road network map, which contains information on lanes, intersections, traffic directions, speed limits, and traffic signs, is used by the global planner to construct a routing graph. Consequently, dynamic programming is applied to calculate the best route from the starting vertex to the goal vertex in the routing graph.
After the global route is determined, it is utilized by the local planner as a reference for creating candidate trajectories (i.e., rollout generation). The local planner generates a total of N rollout rollouts along the global reference path. The generated rollouts are denoted by {R r } Nrollout r=1 . All rollouts are parallel to the global path and are spaced evenly to each  side of the global path, with each rollout being W rollout apart from the global path and the adjacent candidate trajectories. The rollouts originate from the ego vehicle's current position and extend forward longitudinally to the end of the planning horizon (H planning ). Fig. 2 shows an example of rollouts generated by the local planner.
Each rollout has associated costs, including center cost (c center ), transition cost (c transition ), longitudinal collision cost (c lon collision ), lateral collision cost (c lat collision ), and visibility cost (c visibility ). Each cost indicates how good or bad a candidate trajectory is relative to other candidate trajectories regarding that particular aspect of the trajectory. A summation of the costs for all of the generated rollouts in a particular cost category is equal to one. The calculation of common planning costs, i.e., c center , c transition , c lon collision , c lat collision , is the same as described in [43]. Cost c visibility is calculated from visibility ratio V, which is a numerical value that represents the visibility condition at a specified location from the perspective of the ego vehicle, as previously proposed in [1] and improved in this paper. As shown in Fig. 1, the local planner utilizes V from the visibility estimator, which takes a road network map, a point cloud map, the ego vehicle's current pose, and live LiDAR data as inputs. More information about the visibility estimator is provided in Section IV. A weighted sum of all of the costs associated with each rollout (C) is calculated as follows: C = w center c center + w transition c transition + w lon collision c lon collision + w lat collision c lat collision + w visibility c visibility (1) where w center , w stability , w lon collision , w lat collision , and, w visibility are normalized weights. Finally, the rollout with the lowest C is selected as the output trajectory.
After the output trajectory is selected, the behavior state generator, which is adopted from [43], determines the output action, i.e., going forward along the same trajectory, stopping, or changing the trajectory, based on information about surrounding obstacles and traffic light status.

B. VISIBILITY COST CALCULATION
The visibility cost of each rollout (c visibility ) is calculated from visibility ratio V of positions sampled evenly along the rollout. Let a total of N sampling sampled locations along a rollout R r be {l r,s } Nsampling s=1 where l r,1 represents the sampled location closest to the current position. The visibility ratio of l r,s is denoted by V r,s and can be calculated using the approach detailed in Section IV. This visibility ratio is positively correlated with the visibility condition, i.e., the higher the value, the better the ego vehicle's visibility at location l r,s . However, visibility cost c visibility has an inverse relationship with the visibility condition. Therefore, visibility ratio V r,s is first converted to occlusion ratio O r,s using the following equation: The value of O r,s also ranges from zero to one, similar to V r,s . However, higher values of O r,s are associated with a higher degree of occlusion. Theoretically, if l r,s is located on a straight road segment without any obstacles, the value of O r,s should be zero. However, O r,s is estimated using live sensing data and point cloud maps, which can be noisy. As a result, O r,s sometimes ends up being a very small number close to zero instead of exactly zero. Similarly, there are also cases where O r,s is very close to one, despite l r,s being completely occluded. In order to mitigate the noise issue in practice, extreme values of r,s are squashed to zero or one using the following equation: where T lower and T upper are the lower and upper thresholds of O r,s , respectively. The squashed O r,s is denoted by O r,s . In order to calculate the visibility cost of an entire rollout R r , a discounted sum of O r,s of all sampled locations along the rollout (c visibility r ) is first calculated as follows: where γ ∈ (0, 1) is a discount factor used to account for an increasing level of uncertainty in the calculated value of O r,s along rollout R r . The further the sampled position is from the current position, i.e., the higher the s index, the more delay there is before the ego vehicle will actually reach that position. During the delay, the surrounding circumstances may change, e.g., occluding obstacles may move, or new obstacles may be detected. Therefore, the significance of O r,s is discounted by γ. The closer to zero the value of γ, the VOLUME 4, 2016 lower the impact of O r,s at distant positions on the overall rollout visibility cost c visibility r . This cost c visibility r is further normalized to obtain the final visibility cost of a rollout (c visibility r ) such that the sum of c visibility r over all generated rollouts is equal to one. The normalization process begins by identifying a set of rollouts with the least c visibility r . The set of rollouts is denoted by M and formally defined as follows: The value of visibility cost c visibility r can then be calculated using the following equation: Finally, c visibility r is used in Eq. 1 to calculate the overall cost of a rollout. In special cases where every rollout has an indistinguishable visibility condition, i.e., where c visibility r is equal for all r, all of the outputs of Eq. 7 automatically become zero. Thus, the visibility cost does not have any impact on the overall rollout cost calculation in Eq. 1.

IV. VISIBILITY ESTIMATOR
In this section, our visibility estimator is explained in detail. The goal of the visibility estimator is to calculate a numerical value that is indicative of the visibility condition of an input location. The visibility estimation method used in this paper is based on an algorithm previously proposed in [1] that leverages the information available in HD maps, i.e., a combination road network and point cloud map. The main improvement introduced in this paper over the method proposed in [1] is the ability to take into account occlusions caused by objects that are not present in the point cloud map, which is achieved by incorporating a 3D scan captured by a LiDAR sensor in real-time.
An approach for identifying the visible regions of surrounding roads using a 3D scan and a road network map is first explained in Section IV-A. In Section IV-B, a method for estimating the required local 3D scan from a point cloud map and a live 3D scan is then described. Finally, a method of calculating a visibility ratio from the estimated visible regions is presented in Section IV-C.

A. ESTIMATION OF VISIBLE REGIONS
Regions visible from a specified location are estimated using the z-buffering (depth buffering) algorithm. As shown in  Pixel position (u, v) and depth value d of the projected point can be calculated as follows: where S horizontal FOV and S vertical FOV are the horizontal and vertical fields of view in radians, respectively. The variable S lower angle denotes the elevation angle, with respect to the ground plane, from the specified viewing point to the lowest 3D point. During the projection, only the points that fall within the boundaries of the depth image, i.e., Moreover, if more than one point is projected onto the same pixel position, only the point with the lowest depth value is preserved as it is not occluded by other points.
Visible areas of an object of interest with an associated depth image D interest can subsequently be identified by comparing D interest with a depth image of other surrounding objects D other . This comparison is performed pixel-wise. If the depth value of D interest in pixel [u, v] is smaller than that of the other depth images located at the same pixel position, surface point [x, y, z] of the object of interest, which corresponds to the pixel [u, v] , is visible from the specified viewing location.
In driving environments, the space above the surfaces of the surrounding lanes that other traffic participants or obstacles may occupy is considered the object of interest for visible region estimation. Therefore, a set of 3D points that represents the surfaces of those lanes is first extracted from a road network map. The set of these points is denoted by where N lane is the total number of the lane surface points. Each point in the set can be defined as follows: Representative points of the area above the lanes' surfaces can then be obtained by offsetting the z-coordinate of each lane surface point upward by fixed value H object : The resulting points P interest = {p interesti } Nlane i=1 are subsequently projected onto an image plane to create depth image D interest . Similarly, surface points of all surrounding objects are projected onto a depth image D other . As D other can be sparse, depending on its source, a box filter is utilized to fill the empty pixels of D other based on the value of their neighboring pixels. Finally, the filled D other is compared with D interest to determine which parts of the surrounding lanes are visible from the specified viewing location.
If a 3D scan of the driving environment, taken by a LiDAR unit from the specified viewing point, is available, the captured scan points can be used as the surface points of the objects in the scene. In that case, depth image D other can then be generated directly. As a result, this approach can be used to estimate the visible regions of the surrounding lanes from any viewing location within the road network map if a 3D scan captured from that location is available.

B. 3D SCAN ESTIMATION
As mentioned in Section IV-A, visible regions from any position within a road network map can be estimated if a 3D scan of the surrounding area captured at that position is available. Estimating regions that are visible from the ego vehicle's current position is straightforward, as the 3D scan can be directly acquired from the onboard LiDAR unit. However, as described in Section III-B, the visibility cost calculation requires information regarding visibility conditions at future ego vehicle positions. Therefore, the ability to predict 3D scans at those future positions is essential.
In order to estimate 3D scans at future ego vehicle positions, a point cloud map and a scan obtained from the LiDAR unit at the current position are used. The incorporation of the live scan acquired at the vehicle's current position is the main distinction between our approach and the algorithm proposed in [1]. The live scan is used to account for objects that were not present during point cloud map generation.
First, the point cloud map is used to obtain a base scan at a specified future location. Let P map = {p map i } Nmap i=1 be the input 3D point cloud map, where N map represents the total number of points in the map. In order to simulate a 3D scan obtained at a target future position p target by a LiDAR unit with the sensing range of S range , the 3D points in P map that are within range S range from p target are denoted as base scan points and can be defined as follows: The resulting P base cannot be used directly for visible region estimation as it only contains points from objects that were present during map generation.
In order to add objects that do not appear in the point cloud map to the estimated scan, a live scan captured by the LiDAR unit at its current position p current is used. Each point in the live scan, [x livei , y livei , z livei ] , is first transformed to point cloud map coordinates using the following equation: where map current R 3×3 is a 3D rotation matrix, and map current T 3×1 is a 3D translation vector, representing the current position and orientation of the LiDAR sensor, respectively, relative to the coordinate system of point cloud map P map . The resulting transformed live scan is denoted as: (15) where N live is the number of live scan points. The scan P live is now in the same coordinate system as P base and therefore can be combined directly as follows: All of the points in the resulting P estimate are in point cloud map coordinates. Therefore, they need to be further transformed to the local coordinates of the simulated LiDAR scanner, whose origin is located at p target . The transformation of each point [x estimatei , y estimatei , z estimatei ] in P estimate is performed using the following equation: where the target 3D position and orientation of the simulated LiDAR sensor with respect to the coordinate system of the point cloud map P map are represented by map target R 3×3 and map target T 3×1 , respectively. The resulting estimated scan is finally projected onto a 2D image plane using the method described in Section IV-A, in order to generate depth image D other .

C. CALCULATION OF VISIBILITY RATIO
The approach described in Sections IV-A and IV-B can be used to identify which regions of the surfaces of surrounding lanes are visible from a specified viewing point. However, the proposed motion planner cannot easily use the resulting visible regions for candidate trajectory selection as they are VOLUME 4, 2016 not quantified. Therefore, this section explains an approach for calculating a quantitative value that encapsulates the visibility condition from visible region estimation results. A visibility ratio V is a numerical value that represents the degree of visibility from a particular location, a concept which was first introduced in [1]. Its value is equal to a ratio of the area visible from the specified viewing location in relation to the total area of interest: where A interest is the total area of interest, and A visible is the portion of the total area of interest which is visible from a particular viewing location. As A visible ⊆ A interest , the value of a visibility ratio ranges from zero to one, where zero indicates that the area of interest is completely occluded, while one indicates the area of interest is entirely visible from the viewing position. In order to calculate the visibility ratio of a given position p target , the area of interest A interest first needs to be defined. Let L neighbor be a set of lane segments that are within sensing range S range from location p target . Assuming that no road users will travel against the designated traffic directions, areas that are relevant in the context of driving from position p target , i.e., A interest , is comprised of the space above the following lane segments within L neighbor : • Lanes with the same driving direction as the ego vehicle's lane that are reachable from position p target , which are denoted by L reachable . • Lanes with the opposite driving direction (in relation to the ego vehicle) from position p target that intersect any of the L reachable lanes, which are denoted by L intersect . Examples of lane segments L reachable and L intersect are shown in Fig. 4 as green and red lines, respectively. Once the relevant lane segments are identified, a set of 3D points representing the area above those segments, i.e., P interest , is obtained from the road network map and used to generate a corresponding depth image D interest using the approach described in Section IV-A. The resulting depth image D interest is then compared with D other , which is estimated using the method explained in Section IV-B. Finally, assuming that the points in P interest are uniformly distributed across the surface of the lanes, A interest in Eq. 18 can be substituted by the total number of points in P interest . Similarly, the number of points in P interest that are visible from p target can be used as A visible .

V. EXPERIMENTS
In order to verify the effectiveness and applicability of the proposed planner in reducing the risk of collision with potentially occluded obstacles in various situations, experiments were carried out in three different traffic scenarios: T-junction crossing, turning right, and preparing to pass a parked vehicle. One or more target vehicles were placed within an occluded area in each scenario. The position of the ego vehicle when these hidden vehicles were first detected, measured by the distance from its starting position, was used for evaluation. In other words, shorter distances from the ego vehicle's starting position represent better performance when detecting occluded targets. In the T-junction crossing and right turn scenarios, the primary sources of occlusion are static objects such as buildings and walls. In order to verify the ability of the proposed system to take into account occlusions caused by objects that are not included in the HD maps, in the third scenario, the occlusion is caused by a parked vehicle that was not present when the map was generated.
Town01 in the CARLA simulator [2], which represents a flat, urban area, was chosen as the experimental environment.
All of the roads in the town are two-lane roads, and the traffic is right-hand. A map of Town01 is shown in Fig. 5, where the orange, blue, and red rectangles represent the locations of the T-junction crossing, right turn, and preparing to pass a parked vehicle traffic scenarios, respectively. The performance of the proposed planner was compared to that of a baseline planner (OpenPlanner [43]), which does not consider visibility when planning the vehicle's output trajectory. Real-time object detection and localization results were achieved using modules available in Autoware [44]. The same parameters were used in both planners, except for those which were specific to each planner, as shown in Table 1. All parameters remained the same in every tested scenario.

A. SCENARIO I: T-JUNCTION CROSSING
In this scenario, the ego vehicle must drive straight through an uncontrolled T-junction, where the merging roadway is on its right. The ego vehicle starting position in this scenario is at the bottom edge of the orange rectangle in Fig. 5. From the start position, the ego vehicle has to go straight, pass through the T-junction, and reach its goal, which is located at the top edge of the orange rectangle in Fig. 5. We tested two variations of this scenario, the first of which involves occlusion of the intersection primarily by a large building set back from the road, which we called the "clear intersection" variation. The second variation included additional structures located very close to the corner, which we called the "occluded intersection" variation.
The challenge in this scenario mainly stems from the  after the experiment began and kept left until finally moving back closer to the lane's center as it crossed the intersection. As can be observed in Fig. 8 (bottom), although the lateral position shift differed slightly between each run for both planners, their output trajectories were consistent overall. Fig. 8 (top) also shows the change in the visibility ratio V along the ego vehicle's trajectory, averaged over 50 runs. At the beginning of the experiment, the visibility ratio remained constant at approximately 1.0 for both planners, which indicates that all relevant areas of the driving environment could be fully observed during that period. As the ego vehicle approached the intersection, i.e., when the intersection came within sensor range S range , the visibility ratio started to drop, signifying incomplete observation of the merging roadway from that point onward. As shown by the shaded area in Fig.  8 (top), there is a slight difference in the drop in the visibility ratio between the two planners, with the visibility ratio along the proposed planner's trajectories remaining relatively higher than those of the baseline planner, suggesting that the proposed planner was able to choose an output trajectory with better visibility conditions compared to the baseline planner.
Although the output trajectories of both planners were consistent during all three target vehicle location configurations, the locations where the ego vehicle first detected the target vehicle, i.e., the discovery positions, in each configuration are clearly different, as shown by the orange and blue stars in Fig. 7 where a red rectangle depicts the hidden vehicle in each configuration. Differences in the discovery positions when using the various target vehicle locations can also be observed in Fig. 9, in which distributions of the discovery positions for both planners in the tested configurations are shown using histograms and cumulative histograms. The xaxes in Fig. 9 show the distance traveled along the lane's center from the starting position of the ego vehicle. Note that the distance traveled along the lane's center is exactly the same as the actual distance the ego vehicle has traveled if, and only if, the ego vehicle remains at the center of the lane without any lateral deviation during the entire run. However, for the sake of conciseness, distance traveled along the lane's center will be referred to as distance traveled, travel distance, or distance in this study unless stated otherwise.
As can be observed in Fig. 9a, a distribution of the proposed planner's discovery position when the target vehicle was placed close to the intersection, at x = 105 m, is clearly bimodal, with the first and second peaks located at approximately 48 m and 56 m from the ego vehicle's starting position, respectively. The discovery position distribution of the baseline planner appears to be bimodal as well. However, the two modes are very close to each other, with all of the discovery positions clustered around 55.5 m. Fig. 9b shows distributions of the discovery positions in the second configuration, where the target vehicle was located at x = 110 m. The resulting discovery positions are very consistent for both planners in this configuration. For the baseline planner, the discovery positions appear to be normally distributed with their center approximately at   Fig. 9c. The distribution results for the proposed planner have two visible peaks that are relatively close to each other. The first peak is located at roughly 64.5 m from the starting position, while the second peak occurs slightly later at approximately 65 m. For the baseline planner, the resulting discovery position distribution generally resembles the second mode of the distribution for the proposed planner, with its peak located at 65 m. However, as can be observed in Fig. 9c, in a few cases, the ego vehicle detected the hidden vehicle significantly later under the baseline planner.
A summary of the statistics of the ego vehicle positions when discovering the hidden vehicles in each hidden vehicle configuration during the "clear intersection" variation of the T-junction traffic scenario, including the total number of experiments where the ego vehicle successfully detected the target vehicle (count), mean, standard deviation, minimum, median, and maximum distance from the starting point until the detection, are given in the top part of Table 2. As can be seen from the table, the ego vehicle was able to detect the hidden vehicles in all configurations with both planners. Moreover, the table shows that both mean and median dis-tance before detecting the hidden vehicle is smaller when using the proposed planner than when using the baseline planner in every target vehicle configuration tested.
From the results shown in Fig. 7, 8, and 9, it is clear that the output trajectories and discovery positions during each run differed slightly, even when using the same planner and hidden target configuration. This slight deviation was expected since an actual localization and detection module were used in the experiment instead of directly utilizing the ground truth information from the simulation. The effects of using live perception modules seem to be most evident in the first configuration, as shown in Fig. 7a and Fig. 9a, where the discovery position distribution appears to be bimodal, with the two modes being significantly far apart in the case of the proposed planner. In this configuration, the hidden vehicle was initially occluded by the corner of the building and a street light pole, located at (x = 101 m, y = −202 m) in Fig. 7a, from the ego vehicle's point of view. Although the size of the pole is small, when it is far from the ego vehicle it can effectively prevent the front end of the target vehicle from being detected by the LiDAR-based detector used in the experiments, as point cloud sparsity positively correlates with range. Therefore, there are two possible ways the ego vehicle can detect the target vehicle. The first option, which allows the target vehicle to be discovered early, is to detect it through the gap between the building corner and the street light pole. This early detection through the opening between two occluding objects could explain the first peak of the proposed planner's discovery position distribution, shown in Fig. 9a, which occurs at approximately 48 m from the ego vehicle's starting position. By keeping the ego vehicle on the left side of the lane, the proposed planner widened the gap between the building and the pole and thus briefly allowed the ego vehicle to detect the target vehicle earlier. While early detection was physically possible, the target vehicle was not discovered in some runs due to the imperfect performance of the perception module. Another relatively more straightforward way to observe the hidden vehicle is to get close enough to the intersection such that both the pole and the corner of the building no longer obstruct the view. The second peak of the proposed planner's discovery position distribution, and the entire position distribution for the baseline planner, reflect this later but simpler method of detecting the hidden vehicle. Similar situations also happened in the third configuration, as can be observed in Fig. 7c and 9c. In the third configuration, the gap occurred between the corner of the building and a tree located at (x = 107 m, y = −202 m) in Fig.  7c. Despite some minor variation, the results are generally consistent, suggesting that the proposed planner could cope with noisy perception to some degree in this scenario. The smaller mean and median discovery distances when using the proposed planner to detect the hidden vehicles in all three configurations, as shown in Table 2, suggest that the ego vehicle would likely detect a hidden vehicle earlier in this scenario when using the proposed planner, as compared to the baseline planner.
2) The "occluded intersection" variation In order to investigate how different levels of occlusion can affect the output trajectories produced by the proposed planner, additional experiments were conducted using a modified T-junction, i.e., the "occluded intersection" variation. Several obstructions were placed close to the corner of the building, as can be seen in Fig. 6b. Except for the modifications to the nearest corner of the T-junction, the experiments were set up in exactly the same way as before, i.e., 50 runs for each planner in each of the three target vehicle location configurations, resulting in a total of 150 trials per planner. Fig. 10 shows the output trajectories for both planners at the modified intersection for all three hidden target vehicle placements. Similar to the results at the original T-junction, shown in Fig. 7, the ego vehicle generally stayed close to the lane's center when using the baseline planner, while the ego vehicle gradually moved to the left side of the lane soon after navigation began when using the proposed planner, as happened in the previous scenario. However, unlike its behavior at the original Town01 T-junction, where the occlusion is less severe, the ego vehicle did not move back to the center as it approached the modified intersection when using the proposed planner; instead, it deviated further to the left side of the lane before entering and eventually crossing the intersection. Differences in the lateral deviation profiles when using the proposed planner at the two experimental T-junctions can be further observed by comparing Fig. 11 with Fig. 8. As can be seen, when comparing these two figures, apart from the lateral deviation, the visibility ratio along the output trajectories of both planners at the heavily occluded T-junction is also dissimilar to that of the original intersection. The drop in the visibility ratios is sharper at the modified intersection. Moreover, the visibility ratios dropped to a lower level than when the planners encountered the less cluttered T-junction. Another distinction between the two tested intersections is the recovery of the visibility ratio as the ego vehicle approaches the intersection. The increase in the visibility ratios at the modified intersection happened later than at the original intersection, although the increase was relatively smoother and more steady, as can be seen in Fig.  11.
The stable gain of visibility at the modified intersection also directly affected the distributions of the discovery positions, as the distributions at this heavily occluded junction were all unimodal, as can be seen in Fig. 12. As shown in Fig. 12a, in the first configuration, where the hidden vehicle is placed closest to the intersection, the ego vehicle usually discovered the target vehicle after having traveled approximately 63 m from its starting position, regardless of which planner was used, thus both planners achieved similar discovery positions. However, when the target vehicle was placed at x = 110 m in the second configuration, the difference in the discovery position distributions when using the two planners is apparent. As can be observed in Fig.  12b, the majority of initial detections occurred at 68.75 m and 69.75 m when using the proposed and baseline planners, respectively. These results indicate that the ego vehicle could detect the occluded target vehicle earlier in this hidden vehicle configuration when the proposed planner was used. Fig.  12c shows the resulting distributions of discovery positions for both planners in the last configuration, where the hidden vehicle is furthest from the intersection, i.e., at x = 115 m. The resulting distributions for both planners appear to be very similar. The discovery of the hidden vehicle occurred at around 72.9 m, whether the proposed or baseline planner was used.
The summary statistics of the discovery positions in the experiments carried out at the modified junction, i.e., the "occluded intersection" variation, are given in the bottom half of Table 2. The table shows that the proposed planner resulted in lower mean and median distances from the ego vehicle starting position when the hidden target vehicles were detected in the second and third configurations. However, both mean and median distances were lower in the first configuration when using the baseline planner.
The experimental results, which are shown in Fig. 10 and 11, indicate that the ego vehicle also moved to the left side of the lane prior to crossing the heavily occluded Tjunction when using the proposed planner. This movement was anticipated because being on the left side of the lane provides the ego vehicle with a better view of the occluded, merging road on the right side of the ego vehicle, even with additional occlusion. However, unlike at the original, lessoccluded T-junction, where the road to the right becomes minimally obstructed as soon as the ego vehicle passes the building corner, the view of the merging roadway is blocked right up to the corner of the modified T-junction. Therefore, when using the proposed planner, the ego vehicle continued traveling on the left side of the lane and entered the occluded T-junction without reverting to the center of the lane in order to maintain additional visibility of the intersection. A sharper and deeper decline in the visibility ratio and its later but smoother recovery can be observed at the modified junction compared to the original T-junction. These are the effects of a higher degree of occlusion caused by the additional obstacles. The more severe occlusion also resulted in the smooth and steady increase in the visibility ratio, as there is no gap between these closely placed obstructing objects where the ego vehicle can "peek" through. Therefore, the occlusion is relatively more consistent. This consistent occlusion near the modified junction is reflected by the unimodal distributions of the discovery positions in all of the tested hidden vehicle configurations, as shown in Fig. 12. It is evident from Table  2 that, in every configuration, the detections at the modified intersection generally happened later than that at the original intersection. The table also shows that the proposed planner resulted in earlier detections compared to the baseline planner in all hidden vehicle configurations, except when the hidden target is placed very close to the heavily occluded intersection.

B. SCENARIO II: TURNING
In this scenario, the ego vehicle initially drives straight along a short road segment, then turns right at an intersection. This scenario was set up in the region indicated by the blue rectangle in Fig. 5. Similar to the T-junction crossing scenario, two variations of this scenario were tested. The first of which, referred to as the "clear corner," involves occlusion mainly from a roadside wall. In the second variation, which we called the "occluded corner," additional structures were added close to the corner to further obstruct the view.

1) The "clear corner" variation
The "clear corner" variation will be first discussed. Although the ego vehicle does not need to cross into other lanes in this scenario, there is still the risk of collision with vehicles stopped in the ego vehicle's lane, especially if the road to the right is not fully observable. As shown in Fig. 13a, a corner in the original CARLA Town01 has an opaque wall that partially obstructs the view. Therefore, the hidden target vehicle was placed in various locations in the same lane the ego vehicle will travel in after rounding the corner. In the first, second, and third configurations, the target vehicles were placed in the middle of the ego vehicle's lane at x = 105 m, x = 110 m, and x = 115 m, respectively. The proposed and baseline planners were tested 50 times each in each configuration. Fig. 14 shows the output trajectories generated in all three hidden vehicle configurations by the proposed and baseline planners, represented by the orange and blue lines, respectively. The initial output trajectories appear to be similar for all three configurations. However, the ego vehicle then started moving towards the left side of the lane when using the proposed planner. In contrast, when using the baseline planner, the ego vehicle stayed close to the lane's center up to the entrance of the intersection. As shown by the end of the orange and blue lines in Fig. 14, during or just after its right turn maneuver, the ego vehicle then stopped at a different position in each configuration to avoid colliding with the hidden vehicles, approximately 5 m to 6 m from the rear end of the parked target vehicles.
Differences in the stopping position of the ego vehicle in each configuration can also be observed in the lateral deviation plot in Fig. 15, where a dashed red line indicates the entrance of the intersection. As can be seen from the lateral deviation profiles of the tested configurations, when using the proposed planner, the ego vehicle initially moved to the left of the lane, then generally moved back to the center of the lane before it started turning right at the corner.
Nonetheless, there appears to be a slight difference in the output trajectories produced by the two planners during the first hidden vehicle configuration. The ego vehicle briefly inched to the left immediately before turning, regardless of which planner was used. Fig. 15 also shows similar visibility ratio profiles for the two planners in the tested configurations. When the ego vehicle was close enough to the corner, the visibility ratio dropped, indicating that the area around the corner was not fully observable from these positions. The visibility ratio increased again once the ego vehicle almost reached the intersection's entrance and started to turn. Of the two tested planners, the proposed planner seems to have maintained a higher visibility ratio during the visibility drop.
As indicated by orange and blue markers in Fig. 14, the locations where the ego vehicle first discovered the hidden target vehicles in this scenario are very consistent for all configurations under both planners. The distributions of the discovery positions, which are shown in Fig. 16, also confirm this consistency, as all of the distributions are unimodal. In the configuration where the target vehicle was placed closest to the corner, i.e., the first configuration, the ego vehicle using the baseline planner made the majority of its detections at around 58.8 m from its starting position, whereas these detections mainly occurred at approximately 59 m when using the proposed planner, as shown in Fig. 16a. Thus the baseline planner yielded earlier detection in the first hidden vehicle configuration. However, as shown in Fig. 16b, in the second configuration where the target vehicle was placed at x = 110 m, when using the baseline planner, the hidden vehicle was detected later, compared to the proposed planner. The distribution's peak in the case of the baseline planner is at 66.9 m, and 66.7 m for the proposed planner. In the last configuration, the peaks of the distributions for the proposed and baseline planners are close to each other at around 3: Summary of the statistics of ego vehicle positions when discovering hidden vehicles in the "right turn" traffic scenario (Scenario II), for both the "clear" and "occluded" variations of the corners, for both the proposed and baseline methods. Distances are measured from the ego vehicle's starting point. "Count" represents the number of successful detections per 50 trials. 69.8 m, as can be observed in Fig. 16c, although there were a number of trials in this last hidden vehicle configuration where the ego vehicle using the proposed planner was able to detect the target vehicle relatively earlier, as indicated by the portion of the orange distribution on the left that is not overlapped by the blue distribution. The distributions of the ego vehicle' positions when discovering the hidden vehicles for both planners under the three hidden vehicle placement configurations in the "clear corner" variation are also summarized using the number of successful detections (count), mean, median, standard deviation, minimum value, and maximum value in Table 3 (top). As can be observed from the table, the proposed planner resulted in lower mean and median distances from the ego vehicle's starting position when first detecting the hidden vehicle in the second and third configurations but in higher values in the first configuration.
As the results in Fig. 14 and 15 show, when using the proposed planner, the ego vehicle initially moved to the left and then reverted back to the center of the lane before turning right at the corner. The initial movement to the left was expected. Since the ego vehicle's view of the corner on its right is blocked by the wall close to the intersection, being on the left side of the lane increases the visible portion of the road around the corner. This increased visibility is also reflected by a relatively higher visibility ratio along the trajectories generated by the proposed planner compared to those output by the baseline planner, as shown in Fig. 15. This figure also shows that the output lateral deviation profiles are similar for all of the hidden vehicle configurations, with a slight difference in the first configuration where the hidden vehicle was placed closest to the corner. The difference shown in Fig.  15a is likely caused by the ego vehicle detecting the target vehicle earlier while still on the first segment of the road and trying to avoid the parked vehicle by moving to the left. However, when the ego vehicle arrived at the intersection, it had to abort its diversion and start turning right. In other configurations, the ego vehicle detected the target vehicles later, i.e., closer to the turning point. Therefore, no brief diversion to the left occurred. As can be observed from Fig.  16, the difference in the resulting discovery positions for both planners is modest at this corner. The reason for this marginal difference is believed to be an adequate level of visibility at this corner. Although the wall indeed blocks the view of the intersection to the right of the ego vehicle, it is located quite far from the corner. As a result, visibility increases rapidly as the ego vehicle moves sufficiently close to the intersection.
2) The "occluded corner" variation In order to investigate the effects of severe occlusion on motion planner output trajectories and discovery positions during this turning scenario, modifications were made to the original corner in Town01. Two kiosks were added next to the wall, very close to the intersection, as shown in Fig. 13b. The experimental conditions were identical to those used in the previous experiment, i.e., three different placements of the hidden vehicle and a total of 50 trials per planner in each placement configuration.
The output trajectories of both planners at the highly occluded corner are shown in Fig. 17. Similar to the output trajectories at the original corner, when using the proposed planner, the ego vehicle moved to the left before reverting to the center immediately before turning. After turning, the ego vehicle came to a complete stop 5 m to 6 m away from the rear end of the parked target vehicle in all of the hidden vehicle configurations.
Unlike at the original, slightly occluded corner, Fig. 18 shows that at the modified, heavily occluded corner, the lane deviation profiles for each planner, during trials with the three hidden vehicle configurations, were nearly indistinguishable from each other when using each planner, i.e., although the profiles differed between the two planners, the results were very consistent for each planner. When using the proposed planner, the ego vehicle did not divert to the left immediately before turning at the occluded corner in the first hidden vehicle configuration, as can be observed in Fig. 18a. Moreover, the ego vehicle stayed on the left side of the lane longer before moving back to the center when turning, as compared to the less occluded corner, i.e., the "clear corner." Fig. 18 also shows that the visibility ratio dropped to a lower level and then increased later, compared to the right turn scenario at the less occluded corner.
The distributions of the discovery positions appear to be the major distinction between the planner results at the original and the modified corners, as can be observed in Fig.  19. Except for the third hidden vehicle configuration, the difference between the ego vehicle's discovery positions when using the proposed and baseline planners is apparent. In the first configuration, when using the proposed planner, there was a bimodal distribution of the discovery positions, with the first and second peaks occurring at approximately 64.5 m and 65.75 m, respectively. On the other hand, when using the baseline planner, there was a unimodal distribution in the discovery position results, with its peak at roughly 66.75 m in the first hidden vehicle configuration. When the target vehicle was placed at x = 110 m, i.e., during the second configuration, results for both planners show a unimodal distribution of the discovery positions, with the proposed planner achieving earlier detection overall, as the ego vehicle was generally able to spot the hidden vehicle at around 70.8 m from its starting position when using the proposed planner compared to 72.5 m with the baseline planner. However, in the last configuration, where the target was placed furthest from the corner, the distribution results for both planners were very similar, with both planners having their discovery peaks at around 74.25 m.
The difference in discovery positions when using each planner can be further observed in the lower half of Table 3. During the first two hidden vehicle configurations, the proposed planner achieved significantly lower mean and median distances from the ego vehicle's starting position when the target vehicles were discovered. Nonetheless, no difference in discovery position is apparent in the last configuration, as the proposed planner yielded a lower median but higher mean distance compared to the baseline planner. Fig. 17 shows that the overall movement of the ego vehicle when using the proposed planner at the heavily occluded intersection is similar to that at the original intersection, i.e., moving to the left side of the lane soon after the experiment begins, then returning to the center of the lane just before turning right. However, by comparing the results shown in Fig. 18, we can see that the ego vehicle stayed on the left side of the lane longer in the scenario with the occluded corner, as compared to the original, less occluded corner. The extended deviation to the left is the direct effect of the higher degree of occlusion at the modified corner. Unlike at the original corner, the additional obstacles at the modified corner obstructed the view of the road ahead right up to the junction's entrance. Therefore, the proposed planner kept the ego vehicle on the left side of the lane to gain additional visibility as long as possible before having to turn. The increased occlusion is also responsible for the absence of the brief diversion to the left that occurred at the original corner in the first configuration, as shown in Fig. 15a. The ego vehicle could not detect the target vehicle as early when encountering the heavily occluded corner compared to the original, less obstructed corner. Hence, the ego vehicle did not initiate an avoidance maneuver before turning in the first configuration. The proposed planner offers a clear advantage over the baseline planner when detecting the hidden target  vehicles behind the heavily occluded corner. As shown in Fig.  19, the majority of detections occurred significantly earlier when traveling along the output trajectory of the proposed planner, except during the last hidden vehicle configuration. During the last configuration, the target vehicle was not in detection range until the ego vehicle had already reverted to the center of its lane for turning. Therefore, the proposed planner did not offer any advantage over the baseline planner in detecting the target vehicle.

C. SCENARIO III: PREPARING TO PASS A PARKED VEHICLE
In this scenario, the ego vehicle has to travel along a straight road segment to where a truck is parked in the ego vehicle's lane. The location in Town01 where this scenario takes place is indicated by the red rectangle in Fig. 5. The ego vehicle has to come to a complete stop behind the parked truck to prepare to pass it. This scenario is different from the previously tested scenarios as the occlusion is not caused by static structures present in the point cloud map, e.g., buildings or walls, but instead, the road ahead is obstructed by a parked truck, which is not a part of the mapped environment. Experiments in this scenario aim to verify improvements in our previously proposed visibility estimation method for handling occlusions caused by objects not included in HD maps. As safe passing requires additional computation and other considerations that are not currently supported by the proposed planner, the focus of the experiments in this traffic scenario is exclusively the output motion of the ego vehicle prior to passing. Two target vehicles were used in these experiments. One of which is a second truck parked in the same lane as the oc-  Fig. 22 also shows a stark difference in the visibility ratios for the ego vehicle as it moved along the trajectories produced by the proposed and baseline planners, respectively, especially near the ego vehicle's stopping position. Initially, the visibility ratios declined linearly for both planners. However, in the case of the proposed planner, the visibility ratio started to increase again as soon as the ego vehicle reached the left border of the lane. When stopping behind the occluding vehicle, the proposed planner generally achieved a higher visibility ratio compared to the baseline planner.
The positions where the three vehicles in this scenario (the occluding truck, the occluded truck, and the occluded car) were detected are indicated by markers in Fig. 21. The distributions of these discovery positions are shown in Fig.  23. By comparing Fig. 23a, 23b, and 23c we can see that the discovery positions for the parked occluding truck, i.e., the first truck, are similar for both planners in all vehicle placement configurations. The ego vehicle generally detected the occluding truck after traveling approximately 7 m from its starting position, regardless of the planner used. However, Fig. 23d, 23e, and 23f show a significant difference in the discovery distributions between the two planners when detecting the target truck hidden behind the first parked truck. As can be seen in Fig. 23d, when the target truck was placed closest to the occluding truck, i.e., only 10 m ahead of it, and the proposed planner was used, the hidden truck was generally discovered by the ego vehicle at 40 m or 43.5 m from its starting position. As shown by the cumulative histograms in Fig. 23d, the proposed planner detected the hidden truck during 17 out of the 50 trials, whereas the same hidden truck was only detected during two of the total 50 trials when using the baseline planner. Similarly, Fig. 23e shows that the baseline planner only made six successful detections of the target truck when it was parked 15 m ahead of the first truck, while the proposed planner yielded a significantly higher number of detections (39), most of which occurred at approximately 43.75 m from its starting position. The same pattern can also be observed in the last configuration, as shown in Fig. 23f. The distribution peak for the proposed planner is roughly at 43.75 m when the second truck was parked 20 m ahead of the first. In this configuration, the proposed planner resulted in 45 successful detections of the truck out of the 50 trials. On the other hand, the second truck was only detected during five of the trials when using the baseline planner. The distributions of the ego vehicle's positions when detecting the target vehicle in the opposite lane, i.e., the passenger car, in all three configurations are shown in Fig.  23g, 23h, and 23i. As shown in Fig. 23g and 23h, the resulting distributions for both planners appear to be very wide, with no prominent peak in the first and second vehicle location configurations. In both configurations, the proposed planner achieved slightly earlier discovery as can be seen in Fig. 23g and 23h. The car was detected in every trial by both planners in both the first and second target vehicle configurations. In the last configuration, where the target vehicle in the opposite lane was at a considerable distance from the occluding truck, most detections occurred at 45 m and 45.5 m when using the proposed and baseline planners, respectively. Nevertheless, the proposed planner achieved a noticeably higher rate of detections (32/50) compared to the baseline planner (18/50), as indicated by the cumulative histograms in Fig. 23i. Table 4 shows a summary of the statistics of the ego vehicle discovery success rates and positions when detecting the other vehicles in this scenario in each of the three vehicle configurations tested. As can be seen from the table, the occluding truck was detected by the ego vehicle in every trial, regardless of the planner used. The proposed planner achieved lower mean and median travel distances before detecting the occluding truck in the first and third configurations. However, it yielded a lower mean but higher median travel distance in the second configuration. In the case of the hidden truck parked in front of the first truck, the proposed planner achieved a significantly higher number of successful detections by the ego vehicle. Although the baseline planner achieved lower mean and median travel distances before discovery, it is worth noting that the ego vehicle only detected the hidden truck during six trials in the second vehicle configuration and five trials in the third configuration, out of 50 total trials in each configuration. As shown in the lower part of Table 4, the second target vehicle, i.e., the car parked on the other side of the road, was discovered in all experimental trials in the first and second configurations when using both planners, but not in the third configuration. The proposed planner yielded a lower mean and median travel distance prior to the detection of the parked car in all three vehicle configurations. As can be seen in Fig. 21 and 22, in the majority of the trials when using the proposed planner, the ego vehicle deviated from the center of the lane and eventually came to a complete stop on the left side of the lane, because moving the ego vehicle to the left allowed it to achieve a better view of the road ahead. Since there was not enough space for the ego vehicle to pass the parked truck without briefly entering the opposite lane, it had to come to a stop behind the truck and prepare for a passing maneuver. Although the ego vehicle was not able to simply pass the parked truck, coming to a stop on the left side of the lane benefited the ego vehicle by enabling it to observe both the area behind the truck and obstacles in the opposite lane, as indicated by its higher visibility ratio at the stopping position in Fig. 22. Moreover, Fig. 23 and Table 4 show that the ego vehicle was more likely to discover target vehicles placed behind the occluding truck and in the opposite lane compared to the baseline planner. Information regarding obstacles in these areas is crucial in determining whether a safe passing maneuver can be performed. Therefore, the proposed planner put the ego vehicle in a relatively safer position for passing in this scenario compared to the baseline planner. Note that the deviation to the left of the lane by the ego vehicle when using the proposed planner did not occur immediately after the beginning of the experiment. As can be observed in Fig.  21 and 22, there is a brief segment after departing from its starting position where the ego vehicle was going straight along the center of the lane. This behavior was expected because the parked truck was initially outside of the ego vehicle's field of perception, i.e., its sensing range. Therefore, the proposed planner calculated the output motion as if the road ahead was clear. Thus, there was no benefit in moving away from the lane's center. However, the planner shifted the ego vehicle to the left as soon as the parked truck was in its detection range. It is important to emphasize that the parked truck was not included in the HD maps and that the ego vehicle relied on its live detection results to evaluate unexpected occlusions it encountered in the surrounding traffic environment. Nevertheless, the detection results in the experiments were not perfect. The shape and orientation of several obstacles in the vicinity of the ego vehicle were sometimes incorrectly estimated, and these occasional incorrect detection results caused the baseline planner to veer the ego vehicle to the left in a few experiments, as can be observed in Fig. 21 and 22. In those incidents, the obstacles were mistakenly judged to be very close to the right side of the ego vehicle's lane. As a result, the baseline planner tried to avoid them. These sporadic deviations to the left account for why the ego vehicle was sometimes able to discover the hidden target truck when using the baseline planner, as shown in Fig. 23d, 23e, and 23f, and in Table 4. In most of the trials where the incorrect deviation did not occur, the target truck could not be detected when using the baseline planner. Similarly, as can be observed in Fig. 23i and Table  4, the number of successful detections of the target vehicle in the opposite lane when using the baseline planner in the third vehicle configuration was inflated by these irregular swerves. These deviations increased the chance of successful detections because the visibility of the target vehicle in the opposite lane was better when the ego vehicle was on the left side of the lane compared to the lane's center. Furthermore, when the target vehicles were placed very far away from the occluding truck in the third configuration, almost at the edge of the sensing field of the ego vehicle from its stopping position, reducing the distance to the target vehicle, even marginally, may have significantly increased the successful detection rate.

VI. CONCLUSIONS
In this paper, we have proposed a motion planner capable of actively adjusting the ego vehicle's lateral position in order to minimize occlusions in various traffic scenarios under perception uncertainty. We have also extended an approach for 3D visibility estimation proposed in our previous work [1] for handling occlusions caused by objects not present in HD maps. The proposed planner first generates candidate trajectories with different lateral offsets from the reference path. The current and future visibility along each trajectory is then estimated using live sensing data from the LiDAR unit and HD maps, which are then converted into a visibility cost. Finally, the visibility cost and other planning costs are used to determine the optimal output trajectory.
Experiments were conducted in the CARLA simulator [2] to evaluate our proposed and improved methods. Live localization and object detection results were used in three traffic scenarios where occlusions were present; crossing an occluded T-junction, making a right turn at an occluded corner, and preparing to pass a large, parked vehicle not present in the HD maps. Our results showed that the ego vehicle was able to effectively minimize occlusions and consequently discover occluded vehicles earlier in most cases when the proposed planner was used, in comparison to a baseline planner. Moreover, the occlusions caused by the parked vehicle that was not present in the HD maps were correctly estimated when using our extended visibility estimation approach.
During our review of related research in Section II, we noted that there is a common assumption that autonomous vehicles will generally travel along the lane's center, which is adopted in several of the approaches that have been proposed for regulating the speed of vehicles when they encounter occluded areas in the driving environment. Instead of simply using the center of the lane as a reference, the planner proposed in this paper can be used to plan a reference trajectory that will minimize occlusions by altering the lateral position of the vehicle within its lane. Therefore, the use of trajectories generated by our proposed motion planner, in conjunction with existing longitudinal motion planners, should result in less conservative and more efficient driving in traffic scenarios involving occlusions.
This work has focused only on occlusions caused by surrounding objects. However, occlusions can also be caused by several other factors, such as sensor failure or adverse weather conditions. These occlusions should also be considered in future work to ensure the safe navigation of autonomous vehicles in broader operational domains. One potential solution is to extend the visibility estimation method improved in this work to support multiple sources of sensing information. Although only one sensing modality, namely LiDAR, was considered in this paper, the concept of quantifying estimated visibility conditions along candidate trajectories and converting them into visibility costs for trajectory selection could also be applied when using multiple sensing modalities.

ACKNOWLEDGMENT
This work was partially supported by the New Energy and Industrial Technology Development Organization (NEDO) on Next-Generation AI and Robot Core Technology Development Project (P15009) and by the Japan Science and Technology Agency (JST) project on Open Innovation Platform with Enterprises, Research Institutes and Academia (OPERA).