Technical Document - Function Description - Navigator class

Table of Contents

1. Overview


Navigator is an algorithm designed for multiple robots (toio™Core Cube) to move well in the presence of each other while taking into account the movements of other robots.

This algorithm is mainly based on two algorithms: “Human Like Collision Avoidance” (HLAvoid) and “Boids”.

The directory structure is as shown in the figure below.

Navigation  +----------------------+ Navigator Related Directories
├── Boids.cs  +--------------------+ Boyd's algorithm implementation class
├── CubeInterface.cs  +------------+ CubeNavigator class and CubeEntity class
├── Entities.cs  +-----------------+ Entity class
├── HLAvoid.cs  +------------------+ HLAVoid Collision Avoidance Algorithm Implementation Class
└── Navigator.cs  +----------------+ Navigator class

1.1. Class Diagram

Navigator module of toio SDK for Unity consists of the Navigator class(“Cube-independent Navigator” in the figure), which is implemented independently of toio™, and the CubeNavigator and CubeEntity classes(Figure “Interfavce for Cube”) , which are the interfaces between Navigator and CubeHandle.

2. Navigator class

2.1. Control block diagram

The control block diagram of Navigator class is shown below.

2.2. Mode

There are three Navigator modes (Navigator.Mode).

Depending on this mode, the path of the information differs as follows

Control block diagram for each mode（AVOID、BOIDS、BOIDS_AVOID）


3. CubeNavigator class

CubeNavigator class inherits from Navigator class and uses CubeHandle to allow Cube to avoid collisions and control groups.

3.1. Control block diagram

3.1.1. Control block diagram of CubeHandle

First of all, the control block diagram of CubeHandle is shown below.

CubeHandle control block diagram


CubeHandle does the following. (See usage_cubehandle.md for details.)

3.1.2. Control block diagram of CubeNavigator

The control block diagram of CubeNavigator is shown below.


CubeNavigator uses a combination of Navigator and CubeHandle to control Cube using the following procedure.

  1. Call the CubeHandle.Update method to set the prediction result and source information to CubeEntity (CubeNavigator.Update method)
  2. Running the Navigator’s navigation algorithm
  3. Give the navigation result (waypoint coordinates, speed) to the CubeHandle.Move2Target method to calculate the move instruction (CubeNavigator.Navi2Target, CubeNavigator.NaviAwayTarget methods)
  4. Execute the output move instruction with the CubeHandle.Move or Movemnet.Exec method.

3.2. Explanation of internal processing

3.2.1. Update

Call the CubeHandle.Update method to set the prediction result and source information to CubeEntity.

// CubeNavigator.Update
public class CubeNavigator : Navigator
{
    public void Update(bool usePred)
    {
        if (Time.time - updateLastTime < 0.015) return;
        updateLastTime = Time.time;
        this.handle.Update();
        (ego as CubeEntity).Update(usePred);
    }
    ...
}

// CubeEntity.Update
public class CubeEntity : Entity
{
    public void Update(bool usePred=false){
        if (usePred)
        {
            x = handle.xPred;
            y = handle.yPred;
            pos = new Vector(x, y);
            rad = handle.radPred;
            spdL = handle.spdPredL;
            spdR = handle.spdPredR;
            spd = handle.spdPred;
            w = handle.wPred;
            v = handle.vPred;
        }
        else{
            x = handle.x;
            y = handle.y;
            pos = handle.pos;
            rad = handle.rad;
            spdL = handle.spdL;
            spdR = handle.spdR;
            spd = handle.spd;
            w = handle.w;
            v = handle.v;
        }
    }
    ...
}

3.2.2. Navi2Target, NaviAwayTarget

It calculates waypoints, etc., from the input target value using the algorithm implemented in Navigator, and passes the result to CubeHandle.Move2Target.

public virtual Movement Navi2Target(double x, double y, int maxSpd=70, int rotateTime=250, double tolerance=20)
{
    // Navigator waypoint calculation
    this.result = base.GetWaypointTo(x, y);
    // Set speed
    var spd = Min(this.result.speedLimit, maxSpd*this.result.speedRatio);

    // CubeHandle.Move2Target calculates the move instruction
    var mv = handle.Move2Target(this.result.waypoint,
        maxSpd:spd, rotateTime:rotateTime, tolerance:8
    );

    // Achievement Decision
    if (ego.pos.distTo(new Vector(x,y)) <= tolerance && mv.reached)
        return mv;
    else{
        mv.reached = false; return mv;
    }
}

3.3. About expansion

If you want to achieve these, inherit from CubeNavigator and override the Navi2Target and NaviAwayTarget methods.

4. Algorithm Details

The algorithms described below are also explained in the Morikatron Engineer Blog.
Please refer to that as well.

A Method for Controlling Collective Behavior Used in Urochoros(Morikatron Engineer Blog)

4.1. Human-like collision avoidance HLAvoid

To summarize HLAvoid.

Calculating the distance in each direction is called “Scanning”.

The figure is taken from Meta-Thesis.

HLAvoid class in toio SDK for Unity has two methods as functions.

// Calculate navigation to target (waypoints, collision conditions, speed limits)
public (Vector, bool, double) RunTowards(List<Navigator> others, Entity target, List<Wall> walls);
// Calculate escape from target (waypoint, collision state, speed limit)
public (Vector, bool, double) RunAway(List<Navigator> others, Entity target, List<Wall> walls);

The calling relationship of the internal functions is as follows.

RunTowards / RunAway

4.1.1. ScanResult structure representing the result of the scan.

public struct ScanResult
{
    public bool isCollision;    // Collision status
    public double[] rads;       // Scanning direction
    public double[] dists;      // Distance
    public double[] safety;     // Safety
    public Vector[] points;     // Relative coordinates of points determined by direction and distance.

    // For creating an initialized ScanResult.
    public static ScanResult init(double[] rads, double maxRange);
    // For debugging
    public void print(Action<string> func);
    // Calculate points with rads and dists
    public void calcPoints();
}

4.1.2. CombineScanRes method to merge the results of a scan

private ScanResult CombineScanRes(List<ScanResult> results, bool isCol, double[] rads);

Merge multiple ScanResult as follows.

4.1.3. Algorithm: How to calculate scans and how to choose waypoints

See orignal thesis Guzzi, Jérôme, et al. “Human-friendly robot navigation in dynamic environments.” 2013 IEEE International Conference on Robotics and Automation. IEEE, 2013.
and see Blog explaining the improved method.

_ScanEntity Implementation Code

private ScanResult _ScanEntity(Navigator other, double[] rads){
    ScanResult res = ScanResult.init(rads, maxRange);
    var o = other.entity;
    var marginSum = margin + other.avoid.margin;

    // Scan for safe distance to other
    for (int i=0; i<rads.Length; ++i)
    {
        var rad = rads[i];
        var v = Vector.fromRadMag(rad, Max(ego.spd, 10));
        var vUnit = Vector.fromRadMag(rad, 1);

        var dV = v - o.v;
        var dPos = ego.pos - o.pos;

        var a = dV * dV;
        var b = 2 * dV * dPos;
        var c = dPos * dPos - marginSum * marginSum;
        var delta = b * b - 4 * a * c;

        double dist;
        if (delta < 0 || Abs(a) < 1)    // No collision
        {
            dist = maxRange;
        }
        else
        {
            var t1 = (-b - Sqrt(delta)) / 2 / a;
            var t2 = (-b + Sqrt(delta)) / 2 / a;

            if (t2 <= 0)            // No collision in future
                dist = maxRange;
            else if (t1 > 0)        // Collsion in future
                dist = t1 * v.mag;
            else {                   // Collsion Now
                dist = Max(0.1, vUnit * dPos.unit * 100);
                res.isCollision = true;
            }
        }

        res.dists[i] = dist;
    }

    // When Collison, Find available rads.
    if (res.isCollision){
        var dPos = o.pos - ego.pos;
        var colSeverity = Max(0, margin+o.margin - dPos.mag)/margin;

        for (int i=0; i<rads.Length; ++i){
            res.safety[i] = Cos(AbsRad(rads[i] - (-dPos).rad)) * (1 + colSeverity) ;
        }
    }

    return res;
}


RunTowards Implementation Code

public (Vector, bool, double) RunTowards(List<Navigator> others, Entity target, List<Wall> walls){

    var rads = SampleRads(target);

    var (res, resCol) = Scan(others, target, walls, rads);
    res.calcPoints(); resCol.calcPoints();

    waypointIndex = 0;
    Vector waypoint = Vector.zero;

    // find waypoint by res, which is the scan result of objects NOT in collision
    {
        double minDist = double.MaxValue;
        for (int i=0; i<rads.Length; ++i)
        {
            // nearest point at certain rad
            var nearestMag = Max( Cos((target.pos-ego.pos).rad - rads[i]) * ego.pos.distTo(target.pos) , 1 );
            var mv = res.points[i].clip(nearestMag);
            res.points[i] = mv;

            // distance of waypoint to target
            var dist = (ego.pos + mv).distTo(target.pos);

            // penalty of turning
            // NOTE may cause not turning when target right behind ego
            dist += Max(AbsRad(ego.rad - rads[i]), 0.5) * 0.1;
            dist += Max((AbsRad(ego.rad - rads[i]) - PI/2), 0) * 2;

            // Search Nearest waypoint away from target, with SAFE direction
            if (dist < minDist && resCol.safety[i] > p_waypoint_safety_threshold)
            {
                minDist = dist; waypointIndex = i;
            }
        }

        if (minDist < double.MaxValue){
            waypoint = res.points[waypointIndex];
        }
    }

    // ====  Speed Limit  ====
    double speedLimit = double.MaxValue;
    var waypointRad = rads[waypointIndex];
    // slow down when colliders are close around.
    {
        double minRadius = 1e10;
        for (int i=0; i<rads.Length; ++i){
            var rad = rads[i];
            if (res.dists[i] < 50 &&
                (Rad(rad-ego.rad)>0.1 && Rad(waypointRad-rad)>0.1 ||
                    Rad(ego.rad-rad)>0.1 && Rad(rad-waypointRad)>0.1 ))
            {
                // minimal maximal turning radius
                minRadius = Min(minRadius, (res.dists[i]+20) / (2* Sin(Abs(rad-ego.rad))) );
            }
        }
        speedLimit = Max(8, minRadius * Max(1, Abs(ego.w)));
    }
    // stop when ego.rad is pointing insides colliders.
    if (resCol.isCollision)
    {
        for (int i=0; i<rads.Length; ++i){
            var rad = rads[i];
            if (AbsRad(rad - ego.rad) < PI*2/nsample
                && resCol.safety[i] <= 0
            ){
                speedLimit = 100 * Max(0, resCol.safety[i] * 2 +1);
                break;
            }
        }
    }

    // make result
    res.rads = rads;
    scanResult = res;
    return (waypoint, resCol.isCollision, speedLimit);
}


4.2. Boids

Boids is an algorithm that simulates the collective behavior of birds, and it is implemented with three simple rules, as shown in the figure below.

Adapted from Reynolds, Craig. Boids(Webpage).

Boids class in toio SDK for Unity has two methods as functions.

// Calculate the force vector of the void, including the "gravitational force" to the target.
public Vector Run(List<Navigator> others, Vector tarPos);
// Calculate the force vector of the void without "attraction" to the target.
public Vector Run(List<Navigator> others);

Accessorial relation

Run

For a more detailed description of the algorithm, see Reynolds, Craig. Boids(Webpage) .