Wednesday, July 2, 2025

 The following sample is an addition to the Hungarian algorithm described in the previous article to allow a drone to calculate its flight path from the current to the next position using the shortest possible route. It uses the dubins package from PyPI — a lightweight and widely used implementation for shortest-path planning under curvature constraints. 

#! /usr/bin/python 

# pip install dubins matplotlib numpy 

import dubins 
import numpy as np 
import matplotlib.pyplot as plt 
 
def compute_dubins_path( 
    start_pos: tuple[float, float, float], 
    end_pos: tuple[float, float, float], 
    turning_radius: float = 10.0, 
    step_size: float = 1.0 
): 
    """ 
    Compute the shortest Dubins path between two configurations. 
 
    Parameters: 
        start_pos: (x, y, heading in radians) 
        end_pos: (x, y, heading in radians) 
        turning_radius: Minimum turning radius of the drone 
        step_size: Distance between sampled points along the path 
 
    Returns: 
        List of (x, y, heading) tuples along the path 
    """ 
    path = dubins.shortest_path(start_pos, end_pos, turning_radius) 
    configurations, _ = path.sample_many(step_size) 
    return configurations 
 
# Example usage 
if __name__ == "__main__": 
    # Initial and final positions: (x, y, heading in radians) 
    q0 = (0.0, 0.0, np.deg2rad(0))       # Start at origin, facing east 
    q1 = (50.0, 30.0, np.deg2rad(90))    # End at (50,30), facing north 
 
    path_points = compute_dubins_path(q0, q1, turning_radius=15.0, step_size=0.5) 
 
    # Plotting the path 
    x_vals, y_vals = zip(*[(x, y) for x, y, _ in path_points]) 
    plt.figure(figsize=(8, 6)) 
    plt.plot(x_vals, y_vals, label="Dubins Path", linewidth=2) 
    plt.scatter(*q0[:2], color='green', label="Start") 
    plt.scatter(*q1[:2], color='red', label="End") 
    plt.axis("equal") 
    plt.title("Dubins Path for Drone Flight") 
    plt.xlabel("X") 
    plt.ylabel("Y") 
    plt.legend() 
    plt.grid(True) 
    plt.show() 


#Codingexercise: https://1drv.ms/w/c/d609fb70e39b65c8/EYzGgu5Fc4dEoCUHWQYxMbUBSfvC36iKh8ESBaLtozvdqA?e=uXE4Jm

No comments:

Post a Comment