（1-6-01） Dijkstra算法：基于Pygame模拟的自动驾驶系统（1）

1.6  综合实战：基于Pygame模拟的自动驾驶系统

本项目是一个基于Pygame的汽车自动驾驶模拟系统，旨在评估和优化自动驾驶系统的性能。通过模拟环境和不同的路径规划算法，用户可以生成最佳行驶路径，并在各种交通环境中测试自动驾驶系统的效果。该系统不仅用于研究和开发自动驾驶技术，还可用于教育和培训目的，为学生和从业人员提供了一个了解自动驾驶技术的平台。

实例1-6：基于Pygame模拟的自动驾驶系统（codes/1/Motion-Planning-for-Self-Driving）

1.6.1  项目介绍

在当今社会，自动驾驶技术正日益成为汽车行业的焦点之一。随着人工智能和机器学习的发展，自动驾驶汽车的研发和应用正迅速推进。自动驾驶技术不仅能够提高驾驶的安全性和效率，还能为人们的出行带来更多便利。为了评估和验证自动驾驶系统的性能，以及研究路径规划算法在不同情景下的应用，开发了一个包含路径规划功能的汽车驾驶模拟系统。这个模拟系统提供了一个虚拟的驾驶环境，允许开发人员和研究人员测试和优化自动驾驶系统。

通过该模拟系统，用户可以使用不同的路径规划算法，如A*算法、Dijkstra算法等，为汽车生成最佳行驶路径。同时，用户还可以将汽车置于各种不同的交通环境中，例如简单、中等、困难或极限难度的道路，以评估路径规划算法的效果。

本项目是一个包含路径规划功能的汽车驾驶模拟系统，本项目的主要组成模块如下所示。

1. 路径规划功能：通过实现不同的路径规划算法，例如A*算法、Dijkstra算法、BFS（广度优先搜索）和DFS（深度优先搜索），为汽车提供从起点到终点的最优路径。这些算法在不同的情景下被使用，比如在简单、中等、困难或极限难度下的游戏模式中。
2. 模拟系统：通过模拟汽车在各种不同道路和交通条件下的行驶，来评估路径规划算法的效果。这包括了汽车的运动、道路的可行驶区域、障碍物的布置等。
3. 游戏环境构建：提供了一个游戏环境构建工具，可以在游戏中手动设置蓝色汽车的位置，以及道路的长度等参数。这为路径规划算法的测试和验证提供了灵活性。
4. 自动驾驶模式：实现了自动驾驶模式，使得汽车可以根据路径规划算法生成的路径自主行驶，从起点到终点，期间实时调整行驶方向，以避开障碍物并最终到达目的地。

通过这些功能，项目提供了一个全面的路径规划和汽车驾驶模拟环境，可以用于评估不同路径规划算法的性能和有效性，以及研究自动驾驶系统的实现和优化。另外，这个模拟系统不仅可以用于评估自动驾驶系统的性能，还可以用于教育和培训目的，帮助学生和从业人员了解自动驾驶技术的原理和应用。同时，这也是一个开放式平台，可以持续地添加新的功能和改进，以适应不断发展的自动驾驶技术和需求。

1.6.2  数据文件

文件car_positions_Easy.data和road_length_Easy.data是与Easy难度模式相关的数据文件，这些文件包含了Easy模式下车辆位置和道路长度的数据。“.data”文件是一种通用的数据文件格式，可以包含各种类型的数据。在这个项目中，这些文件被用来存储Easy模式下的初始车辆位置信息和道路长度信息，这些数据可能被程序读取并用于生成Easy模式下的场景。我们可以使用如下代码读取和查看这些数据文件的内容。

import pickle

# 读取car_positions_Easy.data文件

with open('car_positions_Easy.data', 'rb') as f:

    car_positions_data = pickle.load(f)

# 读取road_length_Easy.data文件

with open('road_length_Easy.data', 'rb') as f:

    road_length_data = pickle.load(f)

# 打印内容

print("Car Positions in Easy Mode:")

print(car_positions_data)

print("\nRoad Length in Easy Mode:")

print(road_length_data)

执行后会输出：

Car Positions in Easy Mode:

[(1149, 405), (1935, 502), (1596, 221), (2781, 334), (3451, 504), (4043, 297), (4697, 233), (5112, 406), (6020, 503), (6260, 334), (6843, 231), (7412, 408), (7903, 495)]

Road Length in Easy Mode:

170

通过上面的输出结果可知：

1. 在文件car_positions_Easy.data 中包含了Easy模式下的车辆位置，每个位置表示为一个(x, y)坐标对的列表。
2. 在文件road_length_Easy.data 中包含了Easy模式下的道路长度，以单位（例如，米）表示。

这些数据可以被用于程序中的路径规划和可视化，以创建Easy模式下的场景。

1.6.3  汽车运动规划

在“Game”目录中包含了一系列文件，这些文件共同构成了一个自动驾驶汽车运动规划的游戏项目。这些文件负责实现游戏的各种功能，包括游戏的初始化、世界的创建、汽车运动控制、窗口绘制等，通过这些功能，用户可以在游戏中体验到自动驾驶汽车的运动规划过程，并且可以根据不同的难度模式调整游戏体验。

（1）在文件trigfunctions.py中定义了如下所示的几个方法，这些方法用于计算三角方法的值，但以角度（而不是弧度）作为输入。

1. cosd(deg_angle)：计算给定角度（以度为单位）的余弦值。
2. sind(deg_angle)：计算给定角度（以度为单位）的正弦值。
3. tand(deg_angle)：计算给定角度（以度为单位）的正切值。
4. cotd(deg_angle)：计算给定角度（以度为单位）的余切值。
5. cot(rad_angle)：计算给定角度（以弧度为单位）的余切值。

这些方法使用了Python的math和numpy库来执行三角方法的计算。它们将角度转换为弧度并使用数学方法来计算三角方法的值，然后返回结果。

import math
import numpy as np
def cosd(deg_angle):
	a=math.cos(np.deg2rad(deg_angle))
	return a

def sind(deg_angle):
	a=math.sin(np.deg2rad(deg_angle))
	return a

def tand(deg_angle):
	a=math.tan(np.deg2rad(deg_angle))
	return a

def cotd(deg_angle):
	a=math.sin(np.deg2rad(deg_angle))
	b=math.cos(np.deg2rad(deg_angle))
	cotd=b/a
	return cotd

def cot(rad_angle):
	a=math.sin(rad_angle)
	b=math.cos(rad_angle)
	cot=b/a
	return cot

（2）文件car.py定义了一个名为car的类，这是一个Pygame精灵（Sprite）。类car的功能如下所示。

1. __init__ 方法用于初始化汽车对象。它接受初始位置 (x, y)、汽车类型、以及游戏对象作为参数。根据汽车类型，选择相应的汽车图像，并设置汽车的属性，如宽度、高度、速度等。
2. updateSpriteOrigin 方法用于更新汽车的图像原点的位置，以确保精灵对象的位置正确。
3. updateCarOrigin 方法用于更新汽车的实际位置，以确保汽车的位置正确。
4. turnCar 方法用于模拟汽车转向的过程。根据给定的轮子转角，计算汽车的旋转角度和位移，并更新汽车的位置和方向。

类car主要用于模拟汽车在游戏中的移动和转向行为，包含了一些物理参数和数学计算，以便准确地模拟汽车的运动。

class car(pygame.sprite.Sprite):
    def __init__(self,x,y,car_type,game):
        pygame.sprite.Sprite.__init__(self)
        # 汽车原点的起始位置
        self.x=x  # 起始位置
        self.y=y  # 起始位置
        self.theta=0  # 初始角度
        # 汽车的物理尺寸（米）
        self.car_width = 4  # 汽车宽度
        self.car_height = 1.8  # 汽车高度
        self.wheel_radius = .46  # 轮子半径
        # 汽车精灵的尺寸
        self.car_width_px=int(self.car_width*game.pixpermeter)  # 汽车的总宽度（用于边界框）
        self.car_height_px=int(.6*self.car_width_px)  # 汽车的总高度
        self.dt=.1  # 秒
        # 更新精灵原点的起始位置
        self.updateSpriteOrigin()

        if car_type == "protagonist":
            self.car_image = pygame.image.load('../assets/orange_car.png')
            self.body_color=(0,153,255,1)
            self.stationary=False
            self.vel=2  # 米/秒
            self.wheel_speed = self.vel/self.wheel_radius  # 弧度/秒

        if car_type == "obstacle":
            # 汽车不动
            self.car_image = pygame.image.load('../assets/blue_car.png')
            self.body_color=(255,0,0,1)
            self.stationary=True

        if car_type == "dynamic":
            # 汽车自动移动
            self.car_image = pygame.image.load('../assets/green_car.png')
            self.body_color=(0,255,0,1)
            self.stationary=False
            self.vel=random.randint(1,6)

        self.car_image = pygame.transform.scale(self.car_image, (self.car_width_px, self.car_height_px))
        self.car_image_new=self.car_image
        self.rect = self.car_image.get_rect()

        if not self.stationary:
            # 前轮驱动汽车，使用Ackermann转向
            self.l=self.car_width  # 前轮和后轮轴之间的长度（轴距）
            self.a2=self.l/2  # 从后轴到汽车质心的距离
            self.W=self.car_height  # 左右轮之间的距离

    # 更新精灵原点
    def updateSpriteOrigin(self):
        self.spritex=self.x-self.car_width_px//2
        self.spritey=self.y-self.car_height_px//2

    # 更新汽车原点
    def updateCarOrigin(self):
        self.x=self.spritex+self.car_width_px//2
        self.y=self.spritey+self.car_height_px//2

    # 转动汽车
    def turnCar(self,wheel_angle,game):
        # print("Turning")
        if wheel_angle != 0:
            R=math.sqrt(self.a2**2+self.l**2*cotd(wheel_angle)**2)
            alpha=math.asin(self.a2/R)
            R1=R*math.cos(alpha)
            ang_vel=self.wheel_radius*self.wheel_speed/(R1+self.W/2)
            if wheel_angle<0:
                ang_vel*=-1
            dtheta=np.rad2deg(ang_vel*self.dt)
            B=(180-abs(dtheta))/2-np.rad2deg(alpha)
            L=abs(2*R*sind(abs(dtheta)/2))
            # 汽车在汽车坐标系下的位置变化（x，y）
            d_c=(L*sind(B)*self.dt, L*cosd(B)*self.dt)
            deltax_m=d_c[0]*cosd(self.theta)+self.vel*cosd(self.theta)*self.dt+d_c[1]*cosd(self.theta)#+(COR_f[0]-COR_i[0])
            deltay_m=d_c[1]*sind(self.theta)+self.vel*sind(self.theta)*self.dt+d_c[0]*sind(self.theta)#+(COR_f[1]-COR_i[1])
            self.theta+=dtheta

        else:
            deltax_m=self.vel*cosd(self.theta)*self.dt
            deltay_m=self.vel*sind(self.theta)*self.dt
            # print(deltax_m)     
        self.x+=deltax_m*game.pixpermeter
        self.y-=deltay_m*game.pixpermeter
        theta = self.theta % 360
        self.updateSpriteOrigin()
        self.car_image_new = pygame.transform.rotate(self.car_image, theta)

（3）文件window.py定义了一个名为Window的类，用于创建游戏窗口，并绘制游戏场景。主要功能如下所示。

1. __init__ 方法用于初始化游戏窗口。在这个方法中，设置了窗口的大小、背景图像以及其他游戏元素的图像。
2. redrawGameWindow 方法用于重新绘制游戏窗口。在这个方法中，绘制了背景、车道线、草地、起点和终点等游戏元素，并根据游戏中的物体位置更新窗口的显示内容。

class Window():
    def __init__(self, game, WorldSize_px):
        pygame.init()
        # 不变的宽度和高度，永远不会改变
        self.width_m = 30  # 米
        self.height_m = 13  # 米

        # 将米转换为像素
        self.width_px = self.width_m * game.pixpermeter
        self.height_px = self.height_m * game.pixpermeter
        self.win = pygame.display.set_mode((self.width_px, self.height_px))
        # 视窗左上角的位置，以世界坐标（像素）表示；随着橙色汽车的移动而移动
        self.x = 0
        self.y = 120
        self.vel = 10  # 移动速度
        self.lane_width = 90  # 像素
        # 导入游戏素材
        self.lane_line = pygame.image.load('../assets/lane_line_long.png')
        self.solid_line = pygame.image.load('../assets/solid_line.png')
        self.grass = pygame.image.load('../assets/grass.jpg')
        self.grass = pygame.transform.scale(self.grass, (self.lane_width, self.lane_width))
 
        self.finish = pygame.image.load('../assets/finish_line2.png')
        self.finish_line = WorldSize_px[0] - 1.5 * self.finish.get_width()  # 终点线中心的x位置
        self.start_line = pygame.image.load('../assets/start_line.png')

    # 重新绘制游戏窗口
    def redrawGameWindow(self, game, WorldSize_px):
        # 重新绘制背景
        self.win.fill((56, 56, 59))
        self.lane_count = 4  # 车道数
        # 重新绘制车道线
        shoulder_count = WorldSize_px[0] // self.solid_line.get_width() + 1
        self.top_shoulder_pos_y = 2 * self.lane_width - self.y
        top_shoulder_pos_x = 0
        self.bot_shoulder_pos_y = WorldSize_px[1] - 2 * self.lane_width - self.y
        bot_shoulder_pos_x = 0   
        # 顶部肩膀线
        if self.top_shoulder_pos_y <= self.y + self.height_px:
            self.win.blit(self.solid_line, (top_shoulder_pos_x, self.top_shoulder_pos_y))
            for i in range(shoulder_count):
                # 顶部肩膀线
                top_shoulder_pos_x = i * self.solid_line.get_width() - self.x
                if top_shoulder_pos_x < self.x + self.width_px:
                    self.win.blit(self.solid_line, (top_shoulder_pos_x, self.top_shoulder_pos_y))
        # 底部肩膀线
        if self.bot_shoulder_pos_y <= self.y + self.height_px:
            self.win.blit(self.solid_line, (bot_shoulder_pos_x, self.bot_shoulder_pos_y))
            for i in range(shoulder_count):
                bot_shoulder_pos_x = i * self.solid_line.get_width() - self.x
                if bot_shoulder_pos_x < self.x + self.width_px:
                    self.win.blit(self.solid_line, (bot_shoulder_pos_x, self.bot_shoulder_pos_y))
        # 重新绘制车道线
        lane_art_count = WorldSize_px[0] // self.lane_line.get_width() + 1
        self.lane_pos_y = []
        for i in range(self.lane_count - 1):
            self.lane_pos_y.append(i * self.lane_width - self.y + 3 * self.lane_width)
            # 只绘制可见的线（为了提高速度）
            if self.lane_pos_y[i] <= self.y + self.height_px:
                self.win.blit(self.lane_line, (0 - self.x, self.lane_pos_y[i]))
                for j in range(lane_art_count):
                    lane_pos_x = j * self.lane_line.get_width() - self.x
                    self.win.blit(self.lane_line, (lane_pos_x, self.lane_pos_y[i]))
        # 绘制草地（顶部）
        grass_count = WorldSize_px[0] // self.grass.get_width()
        if self.y < self.grass.get_height():
            for i in range(grass_count):
                grass_pos = i * self.grass.get_width()
                if grass_pos < self.x + self.width_px:
                    self.win.blit(self.grass, (grass_pos - self.x, 0 - self.y))
        # 绘制草地（底部）
        if self.y + self.height_px > WorldSize_px[1] - self.grass.get_height():
            for i in range(grass_count):
                grass_pos = i * self.grass.get_width()
                if grass_pos < self.x + self.width_px:
                    self.win.blit(self.grass, (grass_pos - self.x, WorldSize_px[1] - self.grass.get_height() - self.y))
        # 绘制终点线
        if self.x + self.width_px > self.finish_line - 0.5 * self.finish.get_width():
            self.win.blit(self.finish, (self.finish_line - 0.5 * self.finish.get_width() - self.x, -self.y))
        # 绘制起点线
        self.win.blit(self.start_line, (410 + game.orange_car.car_width_px / 2 - self.x, -self.y))
        # 重新绘制所有汽车
        for sprite in game.all_sprites:
            # 只绘制在屏幕上的物体
            if (sprite.spritex + sprite.car_width_px < self.x) or (sprite.spritex > self.x + self.width_px) or \
                    (sprite.spritey + sprite.car_height_px < self.y) or (sprite.spritey > self.y + self.height_px):
                pass
            else:
                self.win.blit(sprite.car_image_new, (int(sprite.spritex - self.x), int(sprite.spritey - self.y)))
        pygame.display.update()

（4）文件game.py定义了一个名为car_game的类，用于管理游戏的初始化和运行。其中__init__ 方法用于初始化游戏，在这个方法中分别设置了游戏模式、像素与米的比率、游戏时钟、游戏运行状态以及游戏中的精灵组（包括所有精灵、障碍物精灵和活动精灵组）。

import pygame

class car_game():
    def __init__(self,gameMode):
        pygame.init()
        self.gameMode=gameMode
        self.pixpermeter=30 #pixels/meter

        self.clock = pygame.time.Clock()
        self.run = True

        self.all_sprites = pygame.sprite.Group()
        self.obst_list = pygame.sprite.Group()
        self.active_list=pygame.sprite.Group()

（5）文件world.py定义了一个名为World的类，用于管理游戏世界的初始化设置和生成游戏中的各种元素。其中__init__ 方法用于初始化游戏世界，根据游戏模式确定道路的长度，并生成相应数量和位置的蓝色车辆或随机障碍物。然后创建窗口对象，并在窗口中生成游戏世界的地图。最后，在游戏中生成橙色汽车，并将其添加到精灵组中。

class World():
    def __init__(self, game):
        # 如果游戏模式不是随机模式
        if game.gameMode != 'Random':
            # 获取道路长度数据文件路径
            filepath = 'world_files/road_length' + game.gameMode + '.data'
            # 如果文件存在
            if os.path.exists(filepath):
                # 从文件中读取道路长度数据
                with open('world_files/road_length' + game.gameMode + '.data', 'rb') as filehandle:
                    width = pickle.load(filehandle)
            else:
                # 默认道路长度为300米
                width = 300
        else:
            # 在随机模式下生成随机道路长度
            width = random.randint(150, 1500)
        # 设置世界的宽度和高度
        self.width_m = width  # 米
        self.height_m = 24  # 米
        # 将宽度和高度转换为像素
        self.width_px = self.width_m * game.pixpermeter
        self.height_px = self.height_m * game.pixpermeter
        self.WorldSize_px = (self.width_px, self.height_px)
        # 创建窗口对象
        self.window = Window(game, self.WorldSize_px)

        # 如果游戏模式是随机模式
        if game.gameMode == 'Random':
            # 随机生成障碍物
            for i in range(5, random.randint(20, 45)):
                self.generateRandomObstacle(game)
        else:
            # 生成蓝色车辆
            self.generateBlueCars(game)
        
        # 创建橙色汽车
        print("生成橙色汽车")
        game.orange_car = car(410, 408, "protagonist", game)
        game.all_sprites.add(game.orange_car)
        print("所有车辆已生成.")

未完待续