Önvezető autó szenzorok

Az algoritmus pythonban egy kétdimenziós felületen szimulálja a járművet, amely érzékeli a falakat/sávokat, és a szenzorok távolsága alapján dinamikusan módosítja a kormányzási szögét.

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

import pygame

import math

import sys

# --- Paraméterek és Konfiguráció ---

WINDOW_WIDTH = 1200

WINDOW_HEIGHT = 800

FPS = 60

# Jármű paraméterei

CAR_LENGTH = 50

CAR_WIDTH = 30

MAX_SPEED = 5

MAX_STEERING_ANGLE = 45 # fok

class Car:

    def __init__(self, x, y):

        self.x = x

        self.y = y

        self.angle = 0.0 # Irányszög radiánban

        self.speed = 2.0

        self.steering_angle = 0.0 # Kormányzási szög fokban

        self.sensors = []

    def update(self, obstacles):

        # Kinematikai mozgásmodell (Bicycle Model)

        radians = math.radians(self.steering_angle)

        self.x += math.cos(self.angle) * self.speed

        self.y += math.sin(self.angle) * self.speed

        

        # Akadémiai szintű megközelítés: kerék alapon számított irányszög változás

        turning_radius = CAR_LENGTH / math.sin(radians) if abs(radians) > 0.01 else float('inf')

        if turning_radius != float('inf'):

            self.angle += self.speed / turning_radius

        self.angle = self.angle % (2 * math.pi)

        self.cast_sensors(obstacles)

    def cast_sensors(self, obstacles):

        self.sensors = []

        # Szenzorok szögei: -60, -30, 0, 30, 60 fok

        sensor_angles = [-60, -30, 0, 30, 60]

        

        for angle_deg in sensor_angles:

            current_angle = self.angle + math.radians(angle_deg)

            length = 0

            found = False

            start_x = self.x

            start_y = self.y

            

            while not found and length < 150:

                length += 5

                end_x = start_x + math.cos(current_angle) * length

                end_y = start_y + math.sin(current_angle) * length

                

                # Ellenőrizzük, hogy a sugár ütközik-e akadállyal

                for obs in obstacles:

                    if obs.collidepoint(end_x, end_y):

                        found = True

                        break

                

                if found:

                    break

            self.sensors.append((end_x, end_y, length))

    def draw(self, surface):

        car_surface = pygame.Surface((CAR_LENGTH, CAR_WIDTH), pygame.SRCALPHA)

        pygame.draw.rect(car_surface, (0, 100, 255), (0, 0, CAR_LENGTH, CAR_WIDTH))

        

        # Forgatás

        rotated_surface = pygame.transform.rotate(car_surface, -math.degrees(self.angle))

        rect = rotated_surface.get_rect(center=(self.x, self.y))

        surface.blit(rotated_surface, rect.topleft)

        # Szenzorok kirajzolása

        for sx, sy, dist in self.sensors:

            pygame.draw.line(surface, (255, 0, 0), (self.x, self.y), (sx, sy), 1)

            pygame.draw.circle(surface, (255, 0, 0), (int(sx), int(sy)), 3)

    def get_sensor_distances(self):

        return [s[2] for s in self.sensors]

class PIDController:

    def __init__(self, Kp, Ki, Kd):

        self.Kp = Kp

        self.Ki = Ki

        self.Kd = Kd

        self.prev_error = 0.0

        self.integral = 0.0

    def compute(self, error):

        self.integral += error

        derivative = error - self.prev_error

        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative

        self.prev_error = error

        return output

# --- Szimuláció Inicializálása ---

def main():

    pygame.init()

    screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))

    pygame.display.set_caption("Akadémikus Önvezető Autó Szimuláció")

    clock = pygame.time.Clock()

    car = Car(100, 400)

    

    # Útpálya akadályokból (külső és belső sáv)

    obstacles = [

        pygame.Rect(0, 0, WINDOW_WIDTH, 50),

        pygame.Rect(0, WINDOW_HEIGHT - 50, WINDOW_WIDTH, 50),

        pygame.Rect(300, 200, 600, 400) # Belső sziget

    ]

    # PID Kormányzás vezérléshez

    pid = PIDController(Kp=0.8, Ki=0.01, Kd=0.2)

    while True:

        for event in pygame.event.get():

            if event.type == pygame.QUIT:

                pygame.quit()

                sys.exit()

        # --- Vezérlési Logika ---

        distances = car.get_sensor_distances()

        left_dist = distances[0]

        right_dist = distances[-1]

        

        # Cél: Tartsuk a távolságot a bal és jobb akadályok között

        error = right_dist - left_dist

        

        # A PID szabályozza a kormányszöget a hibajel alapján

        steer_adjustment = pid.compute(error)

        car.steering_angle = max(-MAX_STEERING_ANGLE, min(MAX_STEERING_ANGLE, steer_adjustment))

        # Frissítés

        car.update(obstacles)

        # --- Grafikus Megjelenítés ---

        screen.fill((255, 255, 255))

        

        # Akadályok rajzolása

        for obs in obstacles:

            pygame.draw.rect(screen, (100, 100, 100), obs)

        car.draw(screen)

        pygame.display.flip()

        clock.tick(FPS)

if __name__ == '__main__':

    main()

pygame.init()

screen = pygame.display.set_mode((600, 400))

pygame.display.set_caption("Pygame Sample")

clock = pygame.time.Clock()

x, y, dx, dy, r = 300, 200, 4, 3, 24

running = True

while running:

    for event in pygame.event.get():

        if event.type == pygame.QUIT:

            running = False

    x += dx

    y += dy

    if x - r < 0 or x + r > 600: dx = -dx

    if y - r < 0 or y + r > 400: dy = -dy

    screen.fill((20, 24, 40))

    pygame.draw.circle(screen, (233, 30, 99), (x, y), r)

    pygame.display.flip()

    clock.tick(60)

pygame.quit()

-----------