Autonomous Parking Using Fuzzy Inference System

This example uses:

This example shows how to tune a fuzzy inference system (FIS) for an autonomous parking application with nonholonomic constraints. This example requires Global Optimization Toolbox™ software.

Autonomous parking is an essential capability of intelligent vehicles (autonomous cars). Nonholonomic kinematics impose additional constraints on autonomous parking, where a car cannot move sideways and instead using a curving motion.

Kinematic Model

The following figure shows the kinematics of a nonholonomic car with a standard Ackermann steering mechanism.

The kinematic model has the following parameters.

$θ$ is the current orientation of the car with respect to a global reference frame.
$ϕ$ is the steering angle with respect to the car orientation.
$F$ is the front wheel center, $(x_{f}, y_{f})$ .
$R$ is the rear wheel center, $(x_{r}, y_{r})$ .
$| R F |$ is the length of the wheelbase.
$O$ is the center of curvature for the car.
$| O R |$ is the radius of curvature for the car.

In this model, the rear-wheel orientation is fixed and parallel to the car body, that is the rear wheels have the same orientation as the car, $θ$ . The front wheels are parallel to each other and rotate with the steering angle, $ϕ$ . The steering angle is constrained to an angle of $\pm$ $Φ$ . For this example, $Φ = 30$ deg.

The front and rear wheel centers have the following relationship.

$x_{f} - x_{r} = | RF | * \cos (θ)$

$y_{f} - y_{r} = | RF | * \sin (θ)$

The kinematic equations for the front wheel center velocity and car orientation velocity are as follows, where $s$ is the speed of the car.

${\dot{x}}_{f} = s * \cos (θ + ϕ)$

${\dot{y}}_{f} = s * \sin (θ + ϕ)$

$\dot{θ} = \frac{s * \sin (ϕ)}{| RF |}$

Autonomous Parking

The minimum radius of curvature ( $O R$ ) for a car depends on the wheelbase length ( $R F$ ). This minimum radius constrains the motion of the car during parking maneuvers.

When a human driver parks, they often fail to maintain the required car speed and orientation when approaching an empty parking space. To successfully park without a collision, they must compensate by switching between forward and backward motion while adjusting the speed and steering angle of the car.

Human drivers do not consciously perform geometric computations based on the kinematic model of their car. Instead, based on their own trial-and-error experience, they use natural rules and reasoning to understand the constraints of their car within a parking situation. You can use fuzzy systems to model such rule-based reasoning.

This example uses the following environment for simulating head-on parking of a nonholonomic car.

Here:

The simulation environment is a $45 \times 15 f t$ parking lot.
The hatched area shows occupied parking spots.
The empty parking spot is of size $6 \times 7 f t$ .
The car is of size $5 \times 3 f t$ and the length of the wheelbase ( $| R F |$ ) is $3 f t$ , providing $1 f t$ offset from the wheelbase to both the front and rear of the car.

This example assumes the following.

The car is equipped with an intelligent system that can detect an empty parking spot and then stop the car near the starting edge of the parking spot.
The autonomous parking system takes control of the car after it stops. Ideally, at the starting position, the car is almost vertically centered in the road and parallel to the road ( $θ = 0$ deg or $θ = 180$ deg).
Due to the dynamic nature of a parking lot,the kinematic motion constraints, and physical car attributes, a car does stop at the exact desired position and orientation. Therefore, the parking system assumes that the car stops somewhere in front of the empty parking spot with $θ \approx 0$ or $θ \approx 180$ deg and unequal space to the sides of the car.
For collision-free parking, the car is equipped with range sensors to provide range data for the front, rear, left, and right sides of the car. The following figure shows an example of the range data obtained from the sensors in the simulation environment.

The maximum sensor range is assumed to be $50 f t$ , which covers the entire simulation environment.

Human Reasoning for Car Parking

Generally, as shown below, human drivers maintain the appropriate speed and steering angle when approaching an empty head-on parking spot. In this case, they can park without any forward and backward oscillating motions.

However, sometimes drivers fail to maintain the desired speed and steering angle for oscillation-free parking. As shown in the following example, the drivers must then compensate using back-and-forth motions.

In this case, the driver:

Turns right and moves forward
Fails to enter the parking spot since the front of the car approaches the car in the occupied space.
Turns left and backs up to make enough room to enter the parking spot
Enters the parking spot with forward motion while adjusting the car orientation to align with the parking direction
Stops when the front of the car is a safe minimal distance from the end of the parking spot and the vehicle is aligned with the parking spot (90 deg orientation within the simulation environment)

The following section uses these motions patterns to construct fuzzy systems for autonomous parking.

Generate Training Data

For tuning the fuzzy systems, this example artificially generates training data using the kinematic model of the car and the motion patterns described in the previous section. The data generation process uses the following discrete form of the kinematic model, where $Δ t$ is 0.1.

$x_{f} (k + 1) = x_{f} (k) + Δ t . s (k + 1) . \cos (θ (k) + ϕ (k + 1))$

$y_{f} (k + 1) = y_{f} (k) + Δ t . s (k + 1) . \sin (θ (k) + ϕ (k + 1))$

$θ (k + 1) = \frac{s (k + 1) . \sin ϕ (k + 1)}{| RF |}$

$x_{r} (k + 1) = x_{f} (k + 1) - | RF | . \cos θ (k + 1)$

$y_{r} (k + 1) = y_{f} (k + 1) - | RF | . \sin θ (k + 1)$

The steering angle ( $ϕ$ ) and speed ( $s$ ) values are generated based on the typical human driver patterns discussed previously. The steering angle and speed are constrained to the following limits.

$- Φ \leq ϕ \leq Φ,$

$Φ = 30^{\circ}$

$- 3 . S \leq s \leq S,$

$S = 5$

$\frac{ft}{\sec}$

In order to make space for safe turning, the backward motion uses higher speed when the car gets closer to the occupied space. Alternatively, the car can use the same speed, however, for longer period in case of backward motion to make adequate space for safe turning.

Load the training data structure.

trainingData = load('trainingData');

Each training data point includes five inputs.

Angular deviation ( $Δ θ$ ) between the car orientation and the parking spot orientation
Minimum distances to the front ( $d_{f r o n t}$ ), left ( $d_{l e f t}$ ), rear ( $d_{r e a r}$ ), and right ( $d_{r i g h t}$ ) of the car

Each training data point includes two outputs.

Steering angle ( $ϕ$ )
Speed ( $s$ ) of the car

Since the angular deviation and distance values have different units and scales, the training data is normalized to the range [0 1]. Doing so removes any sensitivity of the cost function to errors in the larger magnitude inputs. The training data structure contains both the original input and output values (x and y) and their normalized values (xn and yn).

During data generation, a successful parking condition is achieved when the car reaches a minimum safe distance from the end of the parking spot and is aligned with the parking direction.

Construct and Train Initial Fuzzy Systems

This example uses a FIS tree as the fuzzy parking system. The first stage of the tuning process is to construct and train the initial FISs that you later assemble into a FIS tree. You then improve performance by fine tuning tune the parameters of the entire FIS tree.

To construct and tune the initial fuzzy systems, this example uses ANFIS, which provides faster convergence compared to other tuning methods.

The design of the FIS tree and its component fuzzy systems addresses the following considerations.

The FIS tree has five inputs and two outputs that match the values in the training data set.
Since ANFIS supports a single output, separate fuzzy subsystems are constructed for steering angle ( $ϕ$ ) and speed ( $s$ ).
For better performance, each of these subsystems uses separate FISs for forward and backward motion.
The forward and backward motion controllers for each subsystem are combined using an additional FIS.

Train the forward motion controller for steering, forwardPhiFIS, using the training input and output data. To do so, first extract the normalized steering angle training data.

forwardPhi = trainingData.yn(:,1);

Then, since this system is only for forward motion, set the steering angle output values to 0 for negative speed values.

negSpeedId = trainingData.y(:,2) < 0;
forwardPhi(negSpeedId) = 0;

Create options for ANFIS training.

aoptions = anfisOptions;

Use three MFs for the first input ( $Δ θ$ ) since it has both positive and negative values. Use two MFs for the distance inputs.

aoptions.InitialFIS = [3 2 2 2 2];

Display only final training results.

aoptions.DisplayANFISInformation = false;
aoptions.DisplayErrorValues = false;
aoptions.DisplayStepSize = false;

Train forwardPhiFIS.

forwardPhiFIS = anfis([trainingData.xn forwardPhi],aoptions);

Minimal training RMSE = 0.100129

Replace the default FIS name with forwardPhiFIS.

forwardPhiFIS.Name = 'forwardPhiFIS';

In a similar manner, train the forward motion controller for speed, forwardSpeedFIS.

forwardSpeed = trainingData.yn(:,2);
forwardSpeed(negSpeedId) = 0;

forwardSpeedFIS = anfis([trainingData.xn forwardSpeed],aoptions);

Minimal training RMSE = 0.161479

forwardSpeedFIS.Name = 'forwardSpeedFIS';

Next, tune the backward motion controllers for steering angle and speed, backwardPhiFIS and backwardSpeedFIS, respectively. In this case, set the output values to 0 for positive speed values.

Train backwardPhiFIS.

backwardPhi = trainingData.yn(:,1);
backwardPhi(~negSpeedId) = 0;

backwardPhiFIS = anfis([trainingData.xn backwardPhi],aoptions);

Minimal training RMSE = 0.112362

backwardPhiFIS.Name = 'backwardPhiFIS';

Train backwardSpeedFIS.

backwardSpeed = trainingData.yn(:,2);
backwardSpeed(~negSpeedId) = 0;

backwardSpeedFIS = anfis([trainingData.xn backwardSpeed],aoptions);

Minimal training RMSE = 0.0642125

backwardSpeedFIS.Name = 'backwardSpeedFIS';

Next, train phiFIS, which combines the forward and backward steering angle values generated by forwardPhiFIS and backwardPhiFIS. To generate input training data for phiFIS, evaluate forwardPhiFIS and backwardPhiFIS using the normalized input training data.

eoptions = evalfisOptions;
eoptions.EmptyOutputFuzzySetMessage ='none';
eoptions.NoRuleFiredMessage = 'none';
eoptions.OutOfRangeInputValueMessage = 'none';

forwardPhi = evalfis(forwardPhiFIS,trainingData.xn,eoptions);
backwardPhi = evalfis(backwardPhiFIS,trainingData.xn,eoptions);

Use five MFs for each input. In this case, you can use higher number of MFs since phiFIS has only two inputs.

aoptions.InitialFIS = 5;

Train phiFIS using the generated input data and the normalized output training data.

phiFIS = anfis([forwardPhi backwardPhi trainingData.yn(:,1)],aoptions);

Minimal training RMSE = 0.120349

phiFIS.Name = 'phiFIS';

Similarly, train speedFIS, which combines the forward and backward speed values generated by forwardSpeedFIS and backwardSpeedFIS. To generate input training data for speedFIS, evaluate forwardSpeedFIS and backwardSpeedFIS using the normalized input training data.

forwardSpeed = evalfis(forwardSpeedFIS,trainingData.xn,eoptions);
backwardSpeed = evalfis(backwardSpeedFIS,trainingData.xn,eoptions);

speedFIS = anfis([forwardSpeed backwardSpeed trainingData.yn(:,2)],aoptions);

Minimal training RMSE = 0.0969036

speedFIS.Name = 'speedFIS';

Construct and Tune FIS Tree

The next stage of the tuning process is to construct and tune and fuzzy tree using the previously tuned component FISs. To create the FIS tree, first define the connections between the component FISs according to the overall FIS tree design.

connections = [...
    "forwardPhiFIS/output" "phiFIS/input1"; ...
    "backwardPhiFIS/output" "phiFIS/input2"; ...
    "forwardSpeedFIS/output" "speedFIS/input1"; ...
    "backwardSpeedFIS/output" "speedFIS/input2"; ...
    "forwardPhiFIS/input1" "backwardPhiFIS/input1"; ...
    "forwardPhiFIS/input1" "forwardSpeedFIS/input1"; ...
    "forwardPhiFIS/input1" "backwardSpeedFIS/input1"; ...
    "forwardPhiFIS/input2" "backwardPhiFIS/input2"; ...
    "forwardPhiFIS/input2" "forwardSpeedFIS/input2"; ...
    "forwardPhiFIS/input2" "backwardSpeedFIS/input2"; ...
    "forwardPhiFIS/input3" "backwardPhiFIS/input3"; ...
    "forwardPhiFIS/input3" "forwardSpeedFIS/input3"; ...
    "forwardPhiFIS/input3" "backwardSpeedFIS/input3"; ...
    "forwardPhiFIS/input4" "backwardPhiFIS/input4"; ...
    "forwardPhiFIS/input4" "forwardSpeedFIS/input4"; ...
    "forwardPhiFIS/input4" "backwardSpeedFIS/input4"; ...
    "forwardPhiFIS/input5" "backwardPhiFIS/input5"; ...
    "forwardPhiFIS/input5" "forwardSpeedFIS/input5"; ...
    "forwardPhiFIS/input5" "backwardSpeedFIS/input5"; ...
    ];

Construct the FIS tree.

fuzzySystems = [forwardPhiFIS backwardPhiFIS forwardSpeedFIS backwardSpeedFIS phiFIS speedFIS];
fisT = fistree(fuzzySystems,connections);

Next, fine-tune both MF and rule parameter values of fisT. For better performance, tune the fuzzy system parameters for individual outputs.

First, tune FIS parameters for steering angle.
Second, tune FIS parameters for speed.

To tune FIS tree parameters for one output without considering the other output value, you can temporarily remove the other output from the FIS tree.

To tune the FIS tree for the steering angle output, remove the second output from fisT and get tunable settings from forwardPhiFIS, backwardPhiFIS, and phiFIS.

fisTin1 = fisT;
fisTin1.Outputs(2) = [];
[in,out,rule] = getTunableSettings(fisTin1,'FIS',...
    ["forwardPhiFIS" "backwardPhiFIS" "phiFIS"]);

Create a tuning option set.

toptions = tunefisOptions;

Use patternsearch method for fine tuning and set the maximum iteration number to 10. If you have Parallel Computing Toolbox™ software, you can improve the speed of the tuning process by setting toptions.UseParallel to true. If you do not have Parallel Computing Toolbox software, set options.UseParallel to false.

toptions.Method = 'patternsearch';
toptions.MethodOptions.MaxIterations = 10;

To improve pattern search results, set method option UseCompletePoll to true.

toptions.MethodOptions.UseCompletePoll = true;

For reproducibility, set the random number generator seed to default.

rng('default')

Training can be computationally intensive and take several hours to complete. To save time, load a pretrained FIS tree by setting runtunefis to false. To run the tuning, you can set runtunefis to true.

runtunefis = false;

Tune the FIS tree.

if runtunefis
    fisTout1 = tunefis(fisTin1,[in;out;rule],...
        trainingData.xn,trainingData.yn(:,1),toptions);
else
    preTunedFIST = load('tunedFIST');
    fisTout1 = preTunedFIST.fisTout1;
end

Next, tune the other FIS tree output by first removing the speed output and adding steering angle output. Then get tunable settings from forwardSpeedFIS, backwardSpeedFIS, and speedFIS.

fisTin2 = fisTout1;
fisTin2.Outputs(1) = "speedFIS/output";
[in,out,rule] = getTunableSettings(fisTin2,'FIS',...
    ["forwardSpeedFIS" "backwardSpeedFIS" "speedFIS"]);

Tune the FIS tree. After training, reset the outputs of the FIS tree.

if runtunefis
    rng('default')
    fisTout = tunefis(fisTin2,[in;out;rule],...
        trainingData.xn,trainingData.yn(:,2),toptions);
    fisTout.Outputs(1) = "phiFIS/output";
    fisTout.Outputs(2) = "speedFIS/output";
else
    fisTout = preTunedFIST.fisTout;
end

Check the individual training errors (root-mean-square error) of the outputs.

err = trainingData.yn - evalfis(fisTout,trainingData.xn,eoptions);
err = err.*err;
rmsePhi = sqrt(mean(err(:,1)))

rmsePhi = 0.1186

rmseSpeed = sqrt(mean(err(:,2)))

rmseSpeed = 0.0967

The performance is not significantly better compared to the ANFIS-trained fuzzy systems.

Autonomous Parking Simulation

According to the training data, the tuned FIS is valid for the following initial conditions:

The car front must be initially located in front of the parking spot with $21 \leq x_{f} \leq 25$ .
The car must be closely aligned with the road direction, that is, the initial car orientation must be $θ \approx 0$ or $θ \approx 180$ deg.
The car must initially be at least $1.5$ ft from either road edge, that is $d_{l e f t} > 1.5$ ft and $d_{r i g h t} > 1.5$ ft.

Parking from Right Side

The tuned FIS is trained for head-on parking from right side ( $θ \approx 180$ deg) of the road.

The following result shows a head-on parking simulation for $x_{f} = 24$ ft, $y_{f} = 4.5$ ft, and $θ = 180$ deg.

parkFromRight = true;
xf = 24;
yf = 4.5;
theta = 180;
figure
simulateParking(parkFromRight,fisTout,trainingData,xf,yf,theta)

Parked

Simulate parking for a different initial condition where $x_{f} = 22$ ft, $y_{f} = 5$ ft, and $θ = 170$ deg.

parkFromRight = true;
xf = 22;
yf = 5;
theta = 170;
figure
simulateParking(parkFromRight,fisTout,trainingData,xf,yf,theta)

Parked

In both cases, the car can autonomously park using the back-and-forth motion pattern. However, the car does not maintain equal distance values on the left and right sides of the parking space. This behavior is common for human drivers who do not generally park in the exact middle of a parking space. Instead, they use a safe distance from each side.

Parking from Left Side

You can use the same fuzzy system for head-on parking from the left side ( $θ \approx 0$ deg). To do so: with input $d_{r i g h t}$ interchanged with $d_{l e f t}$ . Hence, the input values are changed as follows:

Switch the sign of the angular deviation input $Δ θ$ .
Switch the distance inputs for the left ( $d_{l e f t}$ ) and right ( $d_{r i g h t}$ ) sides.

Simulate autonomous parking from left side for $x_{f} = 22$ ft, $y_{f} = 5$ .5 ft, and $θ = 0$ deg.

parkFromRight = false;
xf = 22;
yf = 5;
theta = 0;
figure
simulateParking(parkFromRight,fisTout,trainingData,xf,yf,theta)

Parked

Simulate parking for another initial condition where $x_{f} = 22$ ft, $y_{f} = 5$ .5 ft, and $θ = 0$ deg.

parkFromRight = false;
xf = 23.8;
yf = 5.2;
theta = 7;
figure
simulateParking(parkFromRight,fisTout,trainingData,xf,yf,theta)

Parked

In this case, similar to the right side parking results, the car follows a back-and-forth motion to safely park the car.

Conclusion

The current fuzzy system design has the following shortcomings:

The generated data only considers two motion patterns for autonomous parking. Therefore, the autonomous parking system has limited robustness and does not represent all common skills of a human driver. For example, the following figure shows another common scenario where the driver moves back and turn right to make space on right from the occupied space.

The generated data uses a $\pm 30$ deg limit for steering angles. Therefore, it is difficult for the car to park without oscillation due to the higher radius of curvature.
The sensor model used in this example is a trivial occupancy detection model where the range values are radially detected from the center of the car. Furthermore, when range data is similar from each corner of the car, the fuzzy system can produce unexpected results and local optima within the simulation. A better alternative is to model range data normal to each side of the car as shown below, where clearance on each side is distinct from each other.

ANFIS supports only Sugeno FISs, which may not always produce a smooth control surface.

To update the FIS tree design, you can consider the following potential changes.

Use Mamdani FISs, which support additional tuning methods beyond ANFIS.
Design the initial rulebase of the fuzzy inference systems using human reasoning and then tune with the training data.
Use a custom cost function to automatically generate data and optimize the parking trajectory using reward-based parking simulation. For an example, see Tune Fuzzy Robot Obstacle Avoidance System Using Custom Cost Function.

Documentation

Autonomous Parking Using Fuzzy Inference System

Kinematic Model

Autonomous Parking

Generate Training Data

Construct and Train Initial Fuzzy Systems

Construct and Tune FIS Tree

Autonomous Parking Simulation

Conclusion

See Also

Related Topics

Fuzzy Logic Toolbox Documentation

Support