3️⃣Kino-dynamic Path Planning

State Lattice Planning

• State Space Discretization:

  • The state space (position, orientation, velocity, etc.) is discretized into a grid or lattice.

  • Each node in this lattice represents a possible state of the robot.

• Feasible Trajectories:

  • Paths between nodes are generated based on the robot's kinematic and dynamic constraints.

  • These trajectories are precomputed and stored, ensuring they are feasible and adhere to the robot's capabilities.

• Search Algorithm:

  • A search algorithm, such as A* or Dijkstra's algorithm, is used to find the optimal path through the lattice.

  • The search considers the cost of transitioning from one node to another, typically based on distance, time, or energy consumption.

State Lattice Planning Algorithm

• Lattice Generation:

  • Discretize the state space into a lattice of nodes. Each node represents a possible state (position, orientation, velocity, etc.) of the robot.

  • Define the resolution and range of states for the lattice.

• Trajectory Generation:

  • Precompute feasible trajectories between lattice nodes.

  • Store these trajectories in a lookup table for efficient access during the search.

• Search Initialization:

  • Define the start state SstartS_{\text{start}}Sstart​ and goal state SgoalS_{\text{goal}}Sgoal​.

  • Initialize the open list (priority queue) with SstartS_{\text{start}}Sstart​.

  • Initialize the closed list as empty.

  • Set the cost to reach SstartS_{\text{start}}Sstart​ to 0.

• Pathfinding:

  • While the open list is not empty:

    1. Node Selection:

       • Select the node ncurrentn_{\text{current}}ncurrent​ with the lowest cost from the open list.

       • If ncurrentn_{\text{current}}ncurrent​ is the goal state SgoalS_{\text{goal}}Sgoal​, reconstruct the path and exit.

       • Move ncurrentn_{\text{current}}ncurrent​ from the open list to the closed list.

    2. Node Expansion:

       • For each neighboring node nneighborn_{\text{neighbor}}nneighbor​ connected to ncurrentn_{\text{current}}ncurrent​ by a precomputed trajectory:

         • If nneighborn_{\text{neighbor}}nneighbor​ is in the closed list, skip it.

         • Calculate the cost to reach nneighborn_{\text{neighbor}}nneighbor​ from ncurrentn_{\text{current}}ncurrent​.

         • If nneighborn_{\text{neighbor}}nneighbor​ is not in the open list or the new cost is lower:

           • Update the cost to reach nneighborn_{\text{neighbor}}nneighbor​.

           • Set the parent of nneighborn_{\text{neighbor}}nneighbor​ to ncurrentn_{\text{current}}ncurrent​.

           • If nneighborn_{\text{neighbor}}nneighbor​ is not in the open list, add it.

• Path Reconstruction:

• Starting from SgoalS_{\text{goal}}Sgoal​, trace back through the parent pointers to SstartS_{\text{start}}Sstart​ to reconstruct the path.

• Path Smoothing and Optimization (optional):

  • Apply post-processing to smooth and optimize the path for practical execution by the robot.

def state_lattice_planning(S_start, S_goal, lattice, trajectories):
    open_list = PriorityQueue()
    open_list.put(S_start, 0)
    closed_list = set()
    cost_to_come = {S_start: 0}
    parent = {S_start: None}

    while not open_list.empty():
        n_current = open_list.get()
        
        if n_current == S_goal:
            return reconstruct_path(parent, S_start, S_goal)
        
        closed_list.add(n_current)

        for n_neighbor in lattice_neighbors(n_current, lattice, trajectories):
            if n_neighbor in closed_list:
                continue
            
            new_cost = cost_to_come[n_current] + cost(n_current, n_neighbor, trajectories)
            
            if n_neighbor not in open_list or new_cost < cost_to_come.get(n_neighbor, float('inf')):
                cost_to_come[n_neighbor] = new_cost
                priority = new_cost + heuristic(n_neighbor, S_goal)
                open_list.put(n_neighbor, priority)
                parent[n_neighbor] = n_current

    return None  # No path found

def reconstruct_path(parent, S_start, S_goal):
    path = []
    n_current = S_goal
    while n_current != S_start:
        path.append(n_current)
        n_current = parent[n_current]
    path.append(S_start)
    path.reverse()
    return path

def lattice_neighbors(n, lattice, trajectories):
    # Return neighbors of n based on the precomputed trajectories
    pass

def cost(n1, n2, trajectories):
    # Return the cost of moving from n1 to n2 using the precomputed trajectories
    pass

def heuristic(n, S_goal):
    # Return the heuristic cost from n to the goal (e.g., Euclidean distance)
    pass

Hybrid A*

1. Hybrid State Space:

   • Unlike traditional A* which works on a discretized grid, Hybrid A* operates in a continuous state space. This state space includes position, orientation, and potentially velocity of the vehicle.

2. Motion Primitives:

   • Motion primitives are precomputed feasible paths that consider the vehicle's kinematic constraints (e.g., turning radius, maximum steering angle). These primitives are used to explore the state space.

3. Heuristic Function:

   • Similar to A*, Hybrid A* uses a heuristic function to estimate the cost from the current state to the goal. This heuristic guides the search towards the goal efficiently.

4. Collision Checking:

   • Each motion primitive is checked for collisions with obstacles, ensuring that only feasible and safe paths are considered.

5. Replanning:

   • Hybrid A* can be used in dynamic environments by incorporating replanning, allowing the vehicle to adjust its path in response to changes in the environment.

Hybrid A* Algorithm

• Initialization:

  • Define the start state SstartS_{\text{start}}Sstart​ and goal state SgoalS_{\text{goal}}Sgoal​.

  • Initialize the open list (priority queue) with SstartS_{\text{start}}Sstart​.

  • Initialize the closed list as empty.

  • Set the cost to reach SstartS_{\text{start}}Sstart​ to 0.

• Pathfinding:

  • While the open list is not empty:

    1. Node Selection:

       • Select the node ncurrentn_{\text{current}}ncurrent​ with the lowest cost from the open list.

       • If ncurrentn_{\text{current}}ncurrent​ is within a predefined distance to SgoalS_{\text{goal}}Sgoal​, perform a final connection using a local planner and exit.

       • Move ncurrentn_{\text{current}}ncurrent​ from the open list to the closed list.

    2. Node Expansion:

       • For each motion primitive that generates a successor state nsuccessorn_{\text{successor}}nsuccessor​:

         • Check if nsuccessorn_{\text{successor}}nsuccessor​ is in the closed list; if so, skip it.

         • Calculate the cost to reach nsuccessorn_{\text{successor}}nsuccessor​ from ncurrentn_{\text{current}}ncurrent​.

         • Check for collisions along the motion primitive.

         • If nsuccessorn_{\text{successor}}nsuccessor​ is not in the open list or the new cost is lower:

           • Update the cost to reach nsuccessorn_{\text{successor}}nsuccessor​.

           • Set the parent of nsuccessorn_{\text{successor}}nsuccessor​ to ncurrentn_{\text{current}}ncurrent​.

           • If nsuccessorn_{\text{successor}}nsuccessor​ is not in the open list, add it.

• Path Reconstruction:

  • Starting from the final state, trace back through the parent pointers to SstartS_{\text{start}}Sstart​ to reconstruct the path.

• Path Smoothing and Optimization (optional):

  • Apply post-processing to smooth and optimize the path for practical execution by the vehicle.

def hybrid_a_star(S_start, S_goal, motion_primitives, heuristic, collision_check):
    open_list = PriorityQueue()
    open_list.put(S_start, 0)
    closed_list = set()
    cost_to_come = {S_start: 0}
    parent = {S_start: None}

    while not open_list.empty():
        n_current = open_list.get()

        if is_goal_reached(n_current, S_goal):
            return reconstruct_path(parent, S_start, n_current)

        closed_list.add(n_current)

        for primitive in motion_primitives:
            n_successor = apply_primitive(n_current, primitive)

            if n_successor in closed_list:
                continue

            new_cost = cost_to_come[n_current] + cost(n_current, n_successor)
            
            if not collision_check(n_current, n_successor) and (
                n_successor not in open_list or new_cost < cost_to_come.get(n_successor, float('inf'))
            ):
                cost_to_come[n_successor] = new_cost
                priority = new_cost + heuristic(n_successor, S_goal)
                open_list.put(n_successor, priority)
                parent[n_successor] = n_current

    return None  # No path found

def is_goal_reached(n, S_goal):
    # Check if n is within a threshold distance to S_goal
    pass

def reconstruct_path(parent, S_start, n_goal):
    path = []
    n_current = n_goal
    while n_current != S_start:
        path.append(n_current)
        n_current = parent[n_current]
    path.append(S_start)
    path.reverse()
    return path

def apply_primitive(n, primitive):
    # Apply the motion primitive to generate the successor state
    pass

def collision_check(n_current, n_successor):
    # Check if the path between n_current and n_successor is collision-free
    pass

Kinodynamic RRT*

• Kinodynamic Constraints:

  • Kinematic Constraints: These constraints involve the robot's motion capabilities, such as turning radius and steering angles.

  • Dynamic Constraints: These include limitations on speed, acceleration, and deceleration.

• Random Sampling:

  • Similar to RRT*, Kinodynamic RRT* uses random sampling to explore the state space. However, each sampled state must be reachable from the current state under the robot's kinodynamic constraints.

• Optimality:

  • Kinodynamic RRT* aims to find not just feasible paths, but optimal paths that minimize a cost function (e.g., path length, time, or energy).

• Rewiring:

  • Like RRT*, Kinodynamic RRT* includes a rewiring step to iteratively improve the path by considering the cost to reach each node and potentially rerouting the tree to reduce overall path cost.

Kinodynamic RRT* Algorithm

• Initialization:

  • Define the start state SstartS_{\text{start}}Sstart​ and goal state SgoalS_{\text{goal}}Sgoal​.

  • Initialize the tree with SstartS_{\text{start}}Sstart​.

  • Set the cost to reach SstartS_{\text{start}}Sstart​ to 0.

• Random Sampling:

  • Randomly sample a state SrandS_{\text{rand}}Srand​ in the state space.

  • Use a steering function to find the best feasible trajectory from the tree to SrandS_{\text{rand}}Srand​ that respects kinodynamic constraints.

• Nearest Neighbor:

  • Find the node SnearestS_{\text{nearest}}Snearest​ in the tree that is closest to SrandS_{\text{rand}}Srand​.

• Steering:

  • Generate a trajectory from SnearestS_{\text{nearest}}Snearest​ to SrandS_{\text{rand}}Srand​ that respects kinodynamic constraints.

  • If a feasible trajectory is found, add the new state SnewS_{\text{new}}Snew​ and the trajectory to the tree.

• Rewiring:

  • For each neighboring node SnearS_{\text{near}}Snear​ of SnewS_{\text{new}}Snew​, check if the path through SnewS_{\text{new}}Snew​ offers a lower cost.

  • If so, rewire the tree by updating the parent of SnearS_{\text{near}}Snear​ to SnewS_{\text{new}}Snew​ and adjusting the costs.

• Check for Goal:

  • If SnewS_{\text{new}}Snew​ is within a certain threshold of SgoalS_{\text{goal}}Sgoal​, attempt to connect it to the goal state.

• Path Optimization:

  • Continue sampling, steering, and rewiring to iteratively improve the path quality.

class Node:
    def __init__(self, state, parent=None):
        self.state = state
        self.parent = parent
        self.cost = float('inf')
        self.trajectory = None

def kinodynamic_rrt_star(S_start, S_goal, state_space, kinodynamic_constraints, max_iterations):
    tree = [Node(S_start)]
    tree[0].cost = 0

    for _ in range(max_iterations):
        S_rand = random_sample(state_space)
        S_nearest = nearest_neighbor(tree, S_rand)
        S_new, trajectory = steer(S_nearest, S_rand, kinodynamic_constraints)

        if is_feasible(trajectory):
            S_new_node = Node(S_new, S_nearest)
            S_new_node.cost = S_nearest.cost + trajectory.cost
            S_new_node.trajectory = trajectory
            tree.append(S_new_node)

            for S_near in find_near_nodes(tree, S_new, kinodynamic_constraints):
                if S_near != S_nearest:
                    new_cost = S_new_node.cost + cost(S_new_node, S_near, kinodynamic_constraints)
                    if new_cost < S_near.cost:
                        S_near.parent = S_new_node
                        S_near.cost = new_cost
                        S_near.trajectory = trajectory_from_to(S_new_node, S_near, kinodynamic_constraints)

            if distance(S_new, S_goal) < goal_threshold:
                final_node = connect_to_goal(S_new_node, S_goal, kinodynamic_constraints)
                if final_node:
                    return reconstruct_path(final_node)

    return None

def random_sample(state_space):
    # Randomly sample a state in the state space
    pass

def nearest_neighbor(tree, S_rand):
    # Find the nearest neighbor in the tree to the sampled state
    pass

def steer(S_nearest, S_rand, kinodynamic_constraints):
    # Generate a feasible trajectory from S_nearest to S_rand
    pass

def is_feasible(trajectory):
    # Check if the trajectory is feasible (collision-free)
    pass

def find_near_nodes(tree, S_new, kinodynamic_constraints):
    # Find nodes near S_new within a certain radius
    pass

def cost(S1, S2, kinodynamic_constraints):
    # Calculate the cost from S1 to S2
    pass

def trajectory_from_to(S_new_node, S_near, kinodynamic_constraints):
    # Generate a feasible trajectory from S_new_node to S_near
    pass

def connect_to_goal(S_new_node, S_goal, kinodynamic_constraints):
    # Try to connect S_new_node to the goal state
    pass

def distance(S1, S2):
    # Calculate the distance between two states
    pass

def reconstruct_path(goal_node):
    path = []
    node = goal_node
    while node:
        path.append(node.state)
        node = node.parent
    path.reverse()
    return path