ÖNVEZETŐ AUTÓ SZIMULÁCIÓ

Az általam írt program algoritmusa, egy zárt hurok vezérlésü többszenzoros modell által 8 irányú lidar approximationm Potenciálmező módszer, attraction + repulsion és klasszikus robotika. Dinamikai rendszer differenciális mozgás nemlineáris viselkedéssel. Az autó „kikerüli” az akadályokat

cél felé konvergál. 

---------------

import numpy as np

import matplotlib.pyplot as plt

print("=" * 70)

print("ÖNVEZETŐ AUTÓ SZIMULÁCIÓ (SENSOR-BASED CONTROL)")

print("Potenciálmező + lidar-szerű szenzorok")

print("=" * 70)

# -----------------------------

# TÉR

# -----------------------------

world_size = 20

# akadályok (városi blokkok)

obstacles = [

    (5, 5, 2),

    (12, 10, 3),

    (7, 15, 2),

    (15, 5, 2),

    (10, 14, 2)

]

# cél

goal = np.array([18.0, 18.0])

# autó kezdőpozíció

pos = np.array([1.0, 1.0])

# -----------------------------

# PARAMÉTEREK

# -----------------------------

dt = 0.2

steps = 200

sensor_range = 4.0

repulsion_strength = 5.0

attraction_strength = 1.2

path = []

# -----------------------------

# SEGÉDFÜGGVÉNYEK

# -----------------------------

def distance_to_obstacle(p, obs):

    ox, oy, r = obs

    return np.linalg.norm(p - np.array([ox, oy])) - r

def sensor(p):

    """

    egyszerű 8 irányú lidar-szimuláció

    """

    angles = np.linspace(0, 2*np.pi, 8, endpoint=False)

    readings = []

    for a in angles:

        direction = np.array([np.cos(a), np.sin(a)])

        dist = 0

        while dist < sensor_range:

            test = p + direction * dist

            hit = False

            for obs in obstacles:

                if np.linalg.norm(test - np.array([obs[0], obs[1]])) < obs[2]:

                    hit = True

                    break

            if hit:

                break

            dist += 0.1

        readings.append(dist)

    return np.array(readings)

def compute_force(p, sensor_data):

    """

    potenciálmező alapú irányítás

    """

    # vonzó erő (cél felé)

    to_goal = goal - p

    F_attr = attraction_strength * to_goal

    # taszító erő (akadályok)

    F_rep = np.zeros(2)

    angles = np.linspace(0, 2*np.pi, 8, endpoint=False)

    for i, d in enumerate(sensor_data):

        if d < sensor_range:

            strength = repulsion_strength * (1.0 / (d + 1e-3)**2)

            direction = np.array([np.cos(angles[i]), np.sin(angles[i])])

            F_rep -= strength * direction

    return F_attr + F_rep

# -----------------------------

# SZIMULÁCIÓ

# -----------------------------

print("\nSzimuláció indul...")

for t in range(steps):

    sensor_data = sensor(pos)

    force = compute_force(pos, sensor_data)

    # mozgás

    pos = pos + dt * force

    # határok

    pos = np.clip(pos, 0, world_size)

    path.append(pos.copy())

    if np.linalg.norm(pos - goal) < 0.5:

        print(f"Cél elérve t={t}")

        break

    if t % 20 == 0:

        print(f"t={t:3d} | pozíció={pos}")

print("\nSzimuláció kész.")

# -----------------------------

# VIZUALIZÁCIÓ

# -----------------------------

path = np.array(path)

plt.figure(figsize=(7,7))

# akadályok

for ox, oy, r in obstacles:

    circle = plt.Circle((ox, oy), r, color='red', alpha=0.4)

    plt.gca().add_patch(circle)

# útvonal

plt.plot(path[:,0], path[:,1], label="Autó pályája")

# cél

plt.scatter(goal[0], goal[1], c='green', s=100, label="Cél")

# start

plt.scatter(1,1, c='blue', s=100, label="Start")

plt.xlim(0, world_size)

plt.ylim(0, world_size)

plt.title("Önvezető autó – szenzor + potenciálmező vezérlés")

plt.legend()

plt.grid()

plt.show()

-------------

======================================================================

ÖNVEZETŐ AUTÓ SZIMULÁCIÓ (SENSOR-BASED CONTROL)

Potenciálmező + lidar-szerű szenzorok

======================================================================

Szimuláció indul...

t=  0 | pozíció=[5.02837657 5.02837657]

Cél elérve t=11

Szimuláció kész.

https://carla.readthedocs.io/en/latest/core_sensors/