An adaptive motion planning algorithm for obstacle avoidance in autonomous vehicle parking

Received Dec 3, 2020 Revised Apr 14, 2021 Accepted May 15, 2021 In the recent era, machine learning-based autonomous vehicle parking and obstacle avoidance navigation have drawn increased attention. An intelligent design is needed to solve the autonomous vehicles related problems. Presently, autonomous parking systems follow path planning techniques that generally do not possess a quality and a skill of natural adapting behavior of a human. Most of these designs are built on pre-defined and fixed criteria. It needs to be adaptive with respect to the vehicle dynamics. A novel adaptive motion planning algorithm is proposed in this paper that incorporates obstacle avoidance capability into a standalone parking controller that is kept adaptive to vehicle dimensions to provide human-like intelligence for parking problems. This model utilizes fuzzy membership thresholds concerning vehicle dimensions and vehicle localization to enhance the vehicle’s trajectory during parking when taking into consideration obstacles. It is generalized for all segments of cars, and simulation results prove the proposed algorithm’s effectiveness.


INTRODUCTION
Autonomous parking systems and navigation issues have attracted much research recently due to the development of autonomous vehicle technologies. One prominent reason for using this technique is to improve human performances via advanced robotic technologies, such as 'smart parking' and 'smart highways'. In modern times, these technologies demand that autonomous robots function intelligently as a human in an unknown and unstructured environment. A mobile robot must sense its surroundings and determine appropriate actions to work in such under-constrained, dangerous, and dynamic domains.
Autonomous parking involves proper path planning and control for a car-like mobile robot (CLMR) without human intervention. Parking is considered a challenging task for a machine due when the environment considered is dynamic and uncertain. In such an environment, online parking systems give better performance compared to traditional offline parking systems. Online parking systems carried out path planning in a parallel while; i) moving towards a goal and ii) perceiving the environment. This perceived information is used in multiple ways in parking systems, e.g., environment mapping, parking space detection, obstacle detection and avoidance, collision detection, and following to walls. Past studies involved different types of sensors in the parking system for various purposes. In earlier work, [1] presented a skill-based fuzzy logic approach for forward and reverse parking. They used six infrared sensors to achieve wall following behavior and past parking posture correction. Another skill-based fuzzy logic approach is given in [2]. Their algorithm generates the parking path based on the vehicle's position and orientation relative to the parking space. It is designed for a tight parking space, and sensors are utilized to detect parking dimensions and ready to reverse points for proper parking. Demirli and Khoshnejad [3] used a fifth-order polynomial path data along with sensors for training their adaptive neuro-fuzzy control algorithm. This approach does not require a reference path, and sensors provide nearby information to adapt parameters in the network. However, the system is built on the assumption of having a large parking space and a mostly obstacle-free environment. A fuzzy logic-based parking system is also used in [4]. They utilized infrared sensors for detecting parking spaces. A multifunctional parking controller is presented in [5] using a skill-based fuzzy logic control. They used infrared sensors to detect obstacles in the forward path. This system is designed to handle environment dynamicity, but its outcome is limited. As their system does not have any target steer functionality, it bypasses the obstacle and the parking space even though the path is available for parking. An ultrasonic sensor-based fuzzy control system is used in [6]. Two model predictive control schemes for autonomous parallel parking are developed in [7], [8]. Their path tracking method claims improved accuracy and stability compared to fuzzy logic but increases the computational burden. Similar to previous methods, both of these approaches utilize their sensors to detect a parking slot or avoid curbs and corners of the parked vehicles along with the parking space. A similar approach can also be found in [9]. All of these methods utilize a fuzzy logic theory to implement human intelligence linguistically. These systems are built on the assumption that the environment is fully or partially known and slowly moving. Also, the systems do not consider the impact on the system's performance compared to the vehicle's non-holonomic dimensions. We aim to address both the challenges in this work by utilizing the knowledge of the navigation problems and integrating it into the existing parking architectures.
The solution of an autonomous parking system in a dynamic environment can be achieved by doing motion planning with the navigation algorithm. Many behavior-based reactive navigation architectures can be found [10]- [17] in literature. In earlier work, [18] proposed dynamic path planning for non-holonomic CLMRs using the bubble band concept. A human heuristics-based rule base for reactive navigation is generated in [19]. Joshi and Zaveri [20] proposed a velocity control-based fuzzy navigation controller using ultrasonic sensors for the dynamic environment. The artificial potential field has been widely used for collision-free obstacle avoidance in a known environment. Non-holonomic vehicle navigation through narrow passage was developed in [21] using a potential field. A fuzzy-based potential field method was presented in [22] for dynamic motion planning, including static and moving obstacles. A modified bacterial potential field was proposed in [23] for dynamic motion planning. A fuzzy logic theory optimized by the PSO algorithm is integrated with the potential field for dynamic motion planning in [24]. A sampling-based motion planning rapidly-exploring random tree (RRT) and the potential field was presented in [25] for autonomous valet parking. In another study, an optimization [26] and artificial potential field [27] based mobile robot navigation are presented.
A professional human driver can maneuver between ideal gaps with little knowledge of obstacles and an understanding of vehicle dynamics. When driving a compact hatchback or a big special utility vehicle, the drive-through experience is different. Correlation of such human driving skills with different segments of vehicles required adaptive changes in the design specification of the fuzzy inference system. So, for obstacle avoidance, one set of fuzzy membership functions used in literature may not work for all different dimensions of the vehicle. One must note that the perceptual inference of distances in a similar environment by various vehicles with distinct dimensions is different, as conceptually shown in Figure 1(a)-(c). It reveals an important fact that a fixed and predetermined set of thresholds used in the existing fuzzy-based vehicle control system must be made adaptive by considering vehicle size parameters.
In this paper, we propose a unique approach for adaptation of the fuzzy inferences using the potential field approach for obstacle avoidance. Our proposed adaptive motion planning controller utilizes a parameter derived from the vehicle's actual dimensions and then provides the capability to adapt fuzzy variable membership values and successfully park different size cars. In earlier work [28], a hybrid intelligent autonomous vehicle parking system with an inbuilt obstacle avoidance module was established. We incorporate our proposed algorithm into the parking module in this paper. Simulations results are generated to understand the impact of the vehicle dimensions on the motion planning and a possible way to make it generalized for autonomous parking infrastructures like valet parking, and on-street parallel parking. The use of an adaptive fuzzy system removes the local minima's limitation in the potential field due to the adaptation. This paper's organization is: in section 2, a hybrid autonomous parking system architecture is explained, and it is followed by an explanation of the proposed adaptive motion planning algorithm.

OVERALL HYBRID AUTONOMOUS PARKING SYSTEM ARCHITECTURE
An Intelligent autonomous parking system integrating the proposed algorithm is developed, as shown in Figure 2. It consists of three major components, a parking controller, an adaptive motion controller, and a decision controller. The primary task of this system is to park a vehicle in a parallel mode in a dynamic environment. A dynamic environment may contain several obstacles. The vehicle must avoid them intelligently and perform parking. Figure 2 shows an adaptive motion planning controller integrated with a parking controller to accomplish this task. A parking controller is dedicated to parallel parking, and an adaptive motion planning controller is designed for obstacle avoidance. The controlling outputs of both controllers are passed to the vehicle depending upon the requirement of the task. A decision module controls the output of both controllers. Based on the decision module conditions, it will pass either of these two outputs (φ1 and φ2) shown in the block diagram. The information gathered by the ultrasonic sensors mounted on the vehicle's perimeter plays a vital role in the adaptive motion planning controller and the decision module.
The architecture shown in Figure 2 is a decentralized multi-level fuzzy system. It is designed for multi-tasking such as parallel parking, environment sensing, obstacle avoidance, wall following, target steer, and parking space detection. With a large number of inputs, the rule-based for a different fuzzy system can be designed separately and optimally, which makes our system relatively faster, reliable, and computationally efficient compared to other fuzzy and nonlinear control mechanisms found in the literature for parking. Also, integration of motion planning controller with parking controller can perform obstacle avoidance with parking in an unknown and dynamic environment. It remains a major challenge in this domain.
Our proposed adaptive motion planning controller is a four-input one output fuzzy logic controller. Its rule-base is designed for the task of obstacle avoidance. It is developed from the classical knowledge of navigation for the mobile robot. In [28], the basic rule-base for the obstacle avoidance and parallel parking controller is proposed. However, their design was fixed and invariant like major of the navigation literature. In this paper, its design is made adaptive with the shape of the non-holonomic vehicle. The system can be adaptively used for any size of the non-holonomic vehicle. The controllers' membership functions are made variable with respect to the length and the width of the vehicle. Also, it is made adaptive during runtime concerning the sensor information for optimized performance. In the following subsection, the proposed algorithm is explained in detail. Figure 2. A hybrid autonomous parking system architecture For environment sensing, a total of 12 ultrasonic sensors are assumed to be mounted on the perimeter of the vehicle, as shown in Figure 3. To utilize the collective information of a large number of ultrasonic sensors, they are grouped in three categories: left side sensor group (d1), front/rear side sensor group (d2) and, right side sensor group (d3). For the forward parking, the front side of the sensors Figure 3(a) is used, and for the backward parking, the rear side of the sensors Figure 3(b) is used. Each sensor gives distance value from the nearby obstacles, and to use them effectively; they are grouped as per (1). A similar set of equations can be assumed for backward parking with different sensor groups, as shown in Figure 3(b). This approach establishes better sensing using a large number of sensors and effective Here, G defines the Euclidean distance between the target point and the current location of the vehicle. When obstacles are detected in the environment during a parking maneuver, the decision controller will switch the vehicle's command to the adaptive motion planning controller. The adaptive motion planning controller will detour the vehicle safely away from the obstacles and avoid any collision considering the safe drivable area around the vehicle.

Proposed motion planning algorithm 
Let CLMR be with the size of length L and width W.  Let the minimum safety distances are defined as per the geometry of the CLMR's size, as shown in Figure 4.
where, 1 = 1 2 √ 2 + 2 − 2 and ∈ ⌊0,3 1 ⌋ for d1 and d3  Similarly, let the thresholds of d2 is defined as: where, 2 = 1 2 √ 2 + 2 − 2 and ∈ ⌊0,3 2 ⌋ for d2.  These thresholds are varied based on the length and width of the vehicle. These values are smaller for the small hatchback-sized car, and for the large sedan and luxurious sized car, these values are larger.

Adaptive dimension algorithm 
We define three constants K1, K2, and K3 for the three distances d1,d2 and, d3. These constants are used to modify thresholds of fuzzy membership functions adaptively. Let their range be like (8).
where, i=1, 2, 3 for the distances d1, d2, d3  We define a change in factor 'Ki' from the knowledge of repulsive potential field used in motion planning problems [18], [25], Here, η is the learning rate, and dis is min. safe distance for each distance group d1, d2 and d3.  Algorithm 1 is used to modify thresholds of the distances used for obstacle avoidance with adaptive 'Ki'. As the vehicle approaches near to the obstacle, di is reduced, and ΔKi reduces Ki. If any distance goes below dis, ΔKi becomes positive, and that increases Ki. Hence it maintains the minimum safe distances from the obstacle. The value of dis is the minimum safe distance one has to achieve for autonomous driving. It can be select heuristically or experimentally depending upon the requirement.

SIMULATION RESULTS
This work aims to show the impact of the vehicle's dimension on the motion planning for obstacle avoidance. For further tuning of the parameter used for motion planning, knowledge of the repulsive potential field is utilized to adapt the algorithm. In literature, the potential field is used to compute the target's attractive force and repulsive force for the obstacle. In the proposed algorithm, thresholds of the fuzzy membership function for the sensor group inputs are kept adaptive to the repulsive potential field. When the vehicle is away from the obstacle, the repulsive function is smaller, and the factors 'K' are reduces so that the vehicle moves closer to the walls or the obstacle. If the vehicle has arrived at a safe distance from the obstacle, the repulsive function is kept as positive for the increment of factor 'K'. It keeps the vehicle away from the obstacle with the increase in thresholds of the fuzzy membership function.
A parking scenario with a static obstacle is developed in MATLAB software to demonstrate the proposed algorithm's practicality and efficacy. Different dimension of the CLMR is used to check the effect of the length and width on the static rule-based design for the motion planning controller. Also, the parking is shown as only forward parking, but the algorithm is generalized for reverse parking also. A MATLAB simulation environment of size 20 by 20 ms is created with a combination of a parking slot and a static obstacle in the path of the parking. Here, the static obstacle is assumed to appear during the runtime of the vehicle, and the vehicle does not have any prior information about the environment. It only has the start and finishes point of the parking path. The static obstacle is obstructing the road, and the vehicle has supposed to detour itself and navigate to the parking slot. d1s and d3s are set as 0.5, and d2s are set as 1.
Initially, the system shown in Figure 2 is tested for the two different vehicles with different dimensions without using an adaptive motion planning algorithm. For this, the constant membership functions for each variable [28] are used. This rule-base is a handcrafted rule-based design for obstacle avoidance during the parking maneuver. As the previous work is tested for the small hatchback vehicles, the membership functions are designed to work for all the passenger vehicles for them only. However, the system's results shown in Figure 5 show the difference in the motion planning for the larger vehicle with the same membership function used for the smaller vehicle. Figure 5(a) shows successful parking and detouring of the Hyundai i20 in the static obstacle's presence in the forward direction. But when the Range rover is tested for the same system, it collides with the obstacle, as shown in Figure 5(b). It shows that the inference of the near and medium distances has to be changed as per the vehicle's length and width. So, every time, it needs to build a different membership function for the different vehicle. Figure 5 illustrates the two paths followed by two different vehicles in the same system. Our proposed algorithm gives a generalized solution for the motion planning of any segment of the vehicle. The forward parking for Hyundai i20 with the proposed adaptive motion planning algorithm is shown in Figure 6(a). It shows successful motion planning of the vehicle around the static obstacle. At each stepsequence, the adaptation in the three factors 'K1', 'K2', and 'K3' is shown in Figure 6(b). It can be observed that initially, when the vehicle is safe away from the obstacle and the wall, both the K2 and K3 tend to decrease slightly so that vehicle moves closer to the wall. When it detects the front side obstacle, the algorithm increases the value of K2 for the adaptation of the front sensor membership function. An increment in K2 results in sharper detouring of the vehicle around the static obstacle. All three 'K' parameters are adjusted to perform the motion planning corresponding to the vehicle's dimensions.  Figure 7(a) shows the larger vehicle's successful avoidance using the proposed algorithm compared to Figure 5(b). It can be noted that when the vehicle approaches the upper road boundary wall, the value of K1 increases that indicates the vehicle approaches near to the wall. A previous work [25] shows the risk assessment for the obstacle avoidance using the potential field approach, as shown in Figure 8. Here the vehicle is supposed to move in valet parking, and it avoids another vehicle coming in front of the ego vehicle. This approach's major drawback is when the ego vehicle is taken with the larger size, and if there is a narrow passage between two vehicles, the risk assessment is higher. So, the vehicle may not follow a solution and stop due to excessive repulsive forces between the wall and the obstacle. However, our algorithm's adaptiveness makes it possible to go through the narrow passage by scaling down the inferences of the near, medium, and far membership function using K1, K2, and K3. Figure 8. Motion planning using the repulsive potential field and RRT from [25] A result of the proposed algorithm for the Range rover vehicle in the presence of the narrow passage is shown in Figure 9(a). The corresponding changes in K1, K2 and K3 is also shown in Figure 9(b). Using this approach, the limitation of the potential field is also addressed. Our algorithm utilized the knowledge of the potential field widely used in motion planning algorithms and adapted the fuzzy inference system for a better possible solution.
In summary, our proposed algorithm provides a unique solution for addressing the motion planning in autonomous parking systems to be used for the different segments of the vehicle. Due to the non-holonomic shape of the CLMR, path planning is challenging compared to Omni-directional robots. Our system tries to mimic the inference similar to the expert human driver up to a certain degree. In future work, further optimization of this adaptation by learning rate and initialization can be done.

CONCLUSION
In this paper, a generalized solution of a motion planning-based parking system is derived that is adaptive enough to the vehicles' different dimensions. The system architecture integrates the navigation behavior into the standalone parking controller using ultrasonic sensors. The proposed algorithm intelligently adapts the fuzzy membership function utilizing the vehicle's actual dimension and the perceived sensor information from the localization of the vehicle. The simulation results indicate that the vehicle's motion path is adapted with the repulsive potential field's value. Also, three different factors make it possible to adjust the inputs of the system separately. When the narrow passing gap is available, lowering the values of K1, K2, and K3 can solve the problem. This adaptive algorithm that enables autonomous parking for any sized vehicle for scenarios including dynamic situations can be made available to solve real-time parking solutions.