Building a Netduino Rover – Part 5

See other posts in this series

Fuzzy Logic

In this post I’ll show you how to build an object avoidance behaviour using Fuzzy Logic.

In contrast with traditional logic [Fuzzy Logic] can have varying values, where binary sets have two-valued logic, true or false, fuzzy logic variables may have a truth value that ranges in degree between 0 and 1.
Wikipedia

Since my own knowledge of fuzzy systems is somewhat limited, I’d recommend you read Fuzzy Logic Obstacle Avoidance by Seshi. This is where I found the equations that this post is based on.

Like my last post, this code is designed to be ‘plugged’ into my Netduino Rover project as a behaviour. The behaviour starts by initializing the weights in the FAMM (fuzzy associative memory matrix), and calculating the total sum of these weights. This will be used later. Since this behaviour should not prevent others from being executed, the Execute method returns false.

using System;
using Microsoft.SPOT;
using NetduinoRover.Outputs;
using NetduinoRover.Sensors;

namespace NetduinoRover.Behaviours
{
    public class FuzzyBehaviour : IBehaviour
    {
        private Motor _leftMotor;
        private Motor _rightMotor;
        private RangeSensor _leftSensor;
        private RangeSensor _rightSensor;

        private int[][] _weights = new int[3][];
        private double _sumOfWeights;

        public FuzzyBehaviour(Motor leftMotor, Motor rightMotor, RangeSensor leftSensor, RangeSensor rightSensor)
        {
            _leftMotor = leftMotor;
            _rightMotor = rightMotor;
            _leftSensor = leftSensor;
            _rightSensor = rightSensor;

            // Define FAMM weights (fuzzy associative memory matrix)
            _weights[0] = new int[3] { 3, 4, 5 };
            _weights[1] = new int[3] { 2, 3, 4 };
            _weights[2] = new int[3] { 1, 2, 3 };

            // Calculate sum of weights
            _sumOfWeights = 0;
            for (int x = 0; x < 3; x++)
            {
                for (int y = 0; y < 3; y++)
                {
                    _sumOfWeights += _weights[x][y];
                }
            }
        }

        public bool Execute()
        {
            // Pass the sensor readings into the fuzzy system
            double delta = GetFuzzyResult(_leftSensor.Read(), _rightSensor.Read());

            // Change the motor speeds based on the value of delta
            ChangeDirection(delta);
            return false;
        }

        // More methods to follow...
    }
}

GetFuzzyResults takes both sensors readings (in cm) and calculates to what degree each reading belongs to each fuzzy set (Near, Far or VeryFar). To get the fuzzy value, multiply each weight in the FAMM by the left and right membership, then divide the total by the sum of all weights in the FAMM. The output should be on the scale 0.07 (turn left) to 0.15 (turn right). 0.11 means go straight ahead.

private double GetFuzzyResult(int leftDistance, int rightDistance)
{
    // Membership function (left sensor)
    double[] leftMembership = new double[3];
    leftMembership[0] = Near(leftDistance);
    leftMembership[1] = Far(leftDistance);
    leftMembership[2] = VeryFar(leftDistance);

    // Membership function (right sensor)
    double[] rightMembership = new double[3];
    rightMembership[0] = Near(rightDistance);
    rightMembership[1] = Far(rightDistance);
    rightMembership[2] = VeryFar(rightDistance);

    // Defuzzifier
    double total = 0;
    for (int x = 0; x < 3; x++)
    {
        for (int y = 0; y < 3; y++)
        {
            total += _weights[x][y] * (leftMembership[x] * rightMembership[y]);
        }
    }
    return total / _sumOfWeights;
}

Each Fuzzy Set function calculates to what degree the distance belongs to the function. This is a core concept in fuzzy logic; the idea that things are not true/false, rather they exist on a scale from 0 to 1. A given distance can be both near and far at the same time, but to different degrees.

private double Near(double distance)
{
    return Bound(-(distance / 50) + 1);
}

private double Far(double distance)
{
    if (distance < 50)
        return Bound(distance / 50);
    else
        return Bound(-(distance / 50) + 2);
}

private double VeryFar(double distance)
{
    return Bound((distance / 50) - 1);
}

The Bound method ensures the supplied value is kept within the range 0 to 1, rounding it up or down as required.

private double Bound(double value)
{
    if (value < 0)
        return 0;
    else if (value > 1)
        return 1;
    else
        return value;
}

Finally, the result of GetFuzzyResults needs to be converted into something the motors can use. Remember, delta will be in the range 0.07 to 0.15 (with 0.11 in the centre). With a little bit of maths, the input is shaped into a percentage of power for each motor.

private void ChangeDirection(double delta)
{
    // The scale is now -0.04 to +0.04
    delta -= 0.11;

    // Convert to the scale -0.40 to +0.40
    delta *= 10;

    double leftSpeed = 0.5;  // 50% power as a starting point
    double rightSpeed = 0.5; // 50% power as a starting point
    leftSpeed -= delta;
    rightSpeed += delta;

    // Send steering to motors
    _leftMotor.SetSpeed(leftSpeed);
    _rightMotor.SetSpeed(rightSpeed);
}

With the behaviour complete, it can be plugged into the behaviour stack (see my last post).

Here’s a video of the finished rover:

Advertisements
This entry was posted in Projects and tagged , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s