Motion Planning

Advanced techniques for planning and executing smooth, optimal robot trajectories.

Path Planning Algorithms

RoboControl provides multiple path planning strategies for different applications.

Waypoint-Based Planning

Define a sequence of waypoints for the robot to follow sequentially.

Copy

var waypoints = new List
{
    new Vector2(12, 12),      // Start
    new Vector2(36, 24),      // Intermediate
    new Vector2(60, 12),      // End
};

var planner = new WaypointPlanner();
var path = planner.GeneratePath(waypoints);

Trajectory Generation

Generate smooth trajectories with velocity and acceleration profiles.

$$p(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5$$

Copy

var trajectory = new QuinticTrajectory(
    startPosition: 0,
    endPosition: 100,
    startVelocity: 0,
    endVelocity: 0,
    startAcceleration: 0,
    endAcceleration: 0,
    duration: 5.0  // seconds
);

for (double t = 0; t <= 5.0; t += 0.01)
{
    double pos = trajectory.GetPosition(t);
    double vel = trajectory.GetVelocity(t);
    double acc = trajectory.GetAcceleration(t);
}

RRT (Rapidly-Exploring Random Tree)

Probabilistic Path Planning

RRT algorithm for finding paths in high-dimensional configuration spaces with obstacle avoidance.

Copy

// RRT path planning with bidirectional search
var planner = new RRTPlanner
{
    StartConfig = new Config(0, 0, 0),
    GoalConfig = new Config(100, 100, 45),
    
    // Algorithm parameters
    MaxIterations = 5000,
    GoalBias = 0.1,  // 10% chance to go toward goal
    StepSize = 5.0,
    
    // Search strategy
    BidirectionalSearch = true,
    GoalThreshold = 2.0,
    
    // Optimization
    PathSmoothing = true,
    SmoothedIterations = 100
};

// Add obstacles
planner.AddObstacle(new CircleObstacle(50, 50, 15));
planner.AddObstacle(new RectangleObstacle(30, 70, 20, 30));

var path = planner.Plan();
Console.WriteLine($"Path found with {path.Nodes.Count} nodes");
Console.WriteLine($"Total path length: {path.TotalLength:F1} units");

Collision Avoidance Algorithms

Velocity Obstacle (VO) Method

Real-time collision avoidance for dynamic environments with moving obstacles.

Copy

// Velocity obstacle collision avoidance
var avoidanceController = new VelocityObstacleController
{
    RobotRadius = 0.3,
    DesiredVelocity = new Vector2(1.0, 0.0),  // 1 m/s forward
    MaxVelocity = 1.5,
    
    // Time horizon for collision checking
    TimeHorizon = 5.0,  // seconds
    TimeHorizonObst = 2.0
};

// Register moving obstacles
var obstacles = new[]
{
    new MovingObstacle { 
        Position = new Vector2(5, 2), 
        Velocity = new Vector2(-0.5, 0),
        Radius = 0.4
    },
    new MovingObstacle { 
        Position = new Vector2(-3, 4), 
        Velocity = new Vector2(0, -0.3),
        Radius = 0.35
    }
};

avoidanceController.SetObstacles(obstacles);

// Compute safe velocity
var safeVelocity = avoidanceController.ComputeSafeVelocity();
robot.SetVelocity(safeVelocity);

Reciprocal Collision Avoidance (RCA)

Multi-agent collision avoidance with symmetric velocity adjustment.

Copy

// Reciprocal collision avoidance for multi-agent systems
var multiAgentController = new ReciprocalCollisionAvoidanceController
{
    MaxAgents = 50,
    TimeHorizon = 10.0,
    TimeHorizonObst = 2.0,
    
    // Agent-specific parameters
    NeighborDist = 25.0,  // meters
    MaxNeighbors = 10,
    
    // Velocity constraints
    MaxSpeed = 5.0,
    PreferredSpeed = 3.0
};

// Register agents
for (int i = 0; i < robotCount; i++)
{
    multiAgentController.RegisterAgent(robots[i].Id, 
        position: robots[i].Position,
        velocity: robots[i].Velocity,
        radius: 0.3
    );
}

// Update in control loop
foreach (var robot in robots)
{
    var newVelocity = multiAgentController.ComputeVelocity(robot.Id);
    robot.SetVelocity(newVelocity);
}

Advanced Trajectory Optimization

Time-Optimal Trajectory Planning

Generate minimum-time trajectories subject to velocity and acceleration limits.

Copy

// Time-optimal trajectory generation
var optimizer = new TimeOptimalTrajectoryOptimizer
{
    StartPosition = 0,
    EndPosition = 100,
    StartVelocity = 0,
    EndVelocity = 0,
    
    // Physical constraints
    MaxVelocity = 50.0,
    MaxAcceleration = 30.0,
    MaxDeceleration = 35.0
};

// Generate bang-bang control profile (max accel then decel)
var trajectory = optimizer.GenerateTrajectory();

Console.WriteLine($"Minimum time to goal: {trajectory.Duration:F2}s");
Console.WriteLine($"Max velocity achieved: {trajectory.MaxVelocity:F2}");
Console.WriteLine($"Segments:");
foreach (var segment in trajectory.Segments)
{
    Console.WriteLine($"  {segment.Type}: {segment.Duration:F3}s, a={segment.Acceleration:F1}");
}

Spline-Based Path Smoothing

Smooth out jagged waypoint paths using cubic or quintic splines with curvature optimization.

Copy

// Cubic spline smoothing with curvature constraints
var pathSmoother = new SplinePathSmoother
{
    InterpolationType = SplineType.CubicSpline,
    NumControlPoints = 50,
    
    // Curvature constraints
    MaxCurvature = 0.5,          // 1/radius
    CurvatureSmoothness = 0.8,
    
    // Smoothing parameters
    RegularizationWeight = 0.1,
    BoundaryConditions = BoundaryCondition.Natural
};

var rawWaypoints = new List<Vector2>
{
    new(0, 0), new(30, 15), new(60, 10), new(90, 30), new(120, 20)
};

var smoothedPath = pathSmoother.Smooth(rawWaypoints);
Console.WriteLine($"Original path length: {CalculatePathLength(rawWaypoints):F1}");
Console.WriteLine($"Smoothed path length: {CalculatePathLength(smoothedPath):F1}");
Console.WriteLine($"Max curvature: {smoothedPath.MaxCurvature:F3}");

Implement dynamic obstacle avoidance for safe autonomous navigation.

Copy

var obstacles = new List
{
    new CircularObstacle(centerX: 30, centerY: 30, radius: 8),
    new RectangularObstacle(minX: 50, maxX: 55, minY: 10, maxY: 20)
};

var safePathPlanner = new ObstacleAvoidingPlanner(obstacles);
var safePath = safePathPlanner.GeneratePath(waypoints);

Path Following Control

Execute planned paths with feedback control to maintain accuracy.

$$\delta = \arctan\left(\frac{2L e_{cte}}{v^2}\right)$$

where $L$ is wheelbase, $e_{cte}$ is cross-track error, and $v$ is velocity.

Copy

var controller = new PathFollowingController(path);

while (!controller.IsPathComplete())
{
    controller.UpdateCurrentPose(robotX, robotY, robotTheta);
    var (linearVel, angularVel) = controller.ComputeVelocities();
    
    robot.SetVelocities(linearVel, angularVel);
}