pygame轨迹跟踪仿真（二）（一的优化版）

03 Jan 2022 4757字 16分
如果这篇博客帮助到你，可以给我充个电加个油哦~
CC BY 4.0 （除特别声明或转载文章外）

本文将讲解我的部分关键pygame轨迹跟踪仿真代码，想看完整的源码可以去下方的程序源码开源地址处领取，觉得好的话记得给我star，点赞哦~

使用我的开源代码时请引用我的文章并表明出处，不得盗用！

应网友需求，更新了pygame轨迹跟踪仿真代码，在pygame轨迹跟踪仿真（一）的基础上，pygame轨迹跟踪仿真（二）（一的优化版）文章中讲解了新增的多智能体仿真，初始化功能，为了后期做深度强化学习，还添加了轨迹误差项，程序运行结束后会返回轨迹误差。

一、所用环境和基础（程序源码开源地址）（代码已更新为pygame轨迹跟踪仿真（二）（一的优化版）的代码）

一台电脑
vscode
python3.8.5
熟悉pygame模块的基本应用，pygame官网

二、常量设置

# 屏幕大小的常量
SCREEN_RECT = pygame.Rect(0, 0, 720, 720)
# 刷新的帧率
FRAME_PER_SEC = 60
# 创建小车的定时器常量
CREATE_CAR_EVENT = pygame.USEREVENT
# 定义圆形小车的大小（直径）
CAR_SIZE = 10
# 圆形小车颜色
CAR_COLOUR = [0,0,0]
# 使用连线命令画小车移动轨迹时需要两个初始点（就是你定义的小车初始坐标点）

# POINT_COLOUR = []
# car1相关参数
car1_TARGET_POSITION = (600,150)
car1_POINT_COLOUR_xy = []
car1_positionx = 100
car1_positiony = 620
car1_path_colour = (255,0,0)
car1_path_screen_colour = (255,0,0,255)
car1_track_colour = (0,255,0)
car1_track_screen_colour = (0,255,0,255)

# car2相关参数
car2_TARGET_POSITION = (600,300)
car2_POINT_COLOUR_xy = []
car2_positionx = 300
car2_positiony = 620
car2_path_colour = (0,0,255)
car2_path_screen_colour = (0,0,255,255)
car2_track_colour = (255,255,0)
car2_track_screen_colour = (255,255,0,255)

三、初始化功能

创建小车精灵，可以在想要多个小车时，使用精灵组进行统一更改和调用。
本代码使用pygame.draw.circle创建的小车是一个圆，想要导入小车图片的可以去源码模仿游戏背景精灵创建方式，导入小车图片。
定义小车的加速，减速和带惯性的刹车

    def env_restart(self):
        self.LINES_LIST = [(self.init_positionx,self.init_positiony),(self.init_positionx,self.init_positiony)]
        self.LINES_LIST_NEW = []
        self.car_screen.fill((255,255,255))
        self.k = 1
        self.positionx = self.init_positionx
        self.positiony = self.init_positiony

四、修改后的键盘控制（为了适应多智能体环境，方便多智能体的任意添加）

上下左右和加减速可以自行设置，统一按键是（r重置，空格惯性刹车）（刹车后可以看到小车向前动了一段距离）

    def key_test(self, car_right, car_left, car_up, car_down, car_accelerate, car_moderate):
        keys_pressed = pygame.key.get_pressed()
        if keys_pressed[car_right]:
            self.car_speedx = self.k
        elif keys_pressed[car_left]:
            self.car_speedx = -self.k
        else:
            self.car_speedx = 0

        if keys_pressed[car_up]:
            self.car_speedy = -self.k
        elif keys_pressed[car_down]:
            self.car_speedy = self.k
        else:
            self.car_speedy = 0
        
        if keys_pressed[car_accelerate]:
            self.accelerate()
        elif keys_pressed[car_moderate]:
            self.moderate()
        elif keys_pressed[pygame.K_SPACE]:
            self.bracking()
        
        if keys_pressed[pygame.K_r]:
            self.env_restart()

五、添加多智能体

在__create_sprites中添加所需要的car个数

    def __create_sprites(self):
        bg = Background()
        self.back_group = pygame.sprite.Group(bg)

        self.car1 = CarSimulation(self.screen,car1_positionx,car1_positiony)
        self.car2 = CarSimulation(self.screen,car2_positionx,car2_positiony)

在start_game中添加第二辆车的贝塞尔曲线，在程序的开头读取贝塞尔曲线的所有点的坐标是防止后面小车的路径覆盖。

    def start_game(self):
        print("游戏开始...")
        pygame.gfxdraw.bezier(self.screen, [(100,620),(300,300),(400,600),(600,150)], 5, car1_path_colour)
        self.get_xy(car1_path_screen_colour,car1_POINT_COLOUR_xy)
        pygame.gfxdraw.bezier(self.screen, [(300,620),(300,300),(400,600),(600,300)], 5, car2_path_colour)
        self.get_xy(car2_path_screen_colour,car2_POINT_COLOUR_xy)

在__update_sprites中模仿car1复制粘贴一段car1的代码，将car1全部更换成car2即可（所有的参数都在二.常量设置中自行设置）

    def __update_sprites(self):
        self.back_group.draw(self.screen)
        pygame.draw.circle(self.screen,CAR_COLOUR,car1_TARGET_POSITION,CAR_SIZE,0)  #最后一个0表示填充，数字代表线宽
        # self.car_group.car_add(self.screen)
        pygame.gfxdraw.bezier(self.screen, [(car1_positionx,car1_positiony),(300,300),(400,600),car1_TARGET_POSITION], 5, car1_path_colour)
        pygame.draw.lines(self.screen, car1_track_colour, 0, self.car1.LINES_LIST)
        self.car1.car_add()
        self.car1.car_update()

        pygame.draw.circle(self.screen,CAR_COLOUR,car2_TARGET_POSITION,CAR_SIZE,0)  #最后一个0表示填充，数字代表线宽
        # self.car_group.car_add(self.screen)
        pygame.gfxdraw.bezier(self.screen, [(car2_positionx,car2_positiony),(300,300),(400,600),car2_TARGET_POSITION], 5, car2_path_colour)
        pygame.draw.lines(self.screen, car2_track_colour, 0, self.car2.LINES_LIST)
        self.car2.car_add()
        self.car2.car_update()

muti_pathing

六、轨迹误差

自定义detection_error函数显示轨迹误差

 def detection_error(self, track_colour,error_POINT_COLOUR_xy):
     # c = 0
     d = 100000
     dis = 0
     for i in range(1,720):
         for j in range(1,720):
             a = tuple(pygame.Surface.get_at(self.car_screen,(i,j)))
             if a == track_colour:
                 self.path_point.append((i,j))

     for i in self.path_point:
         if not i in self.LINES_LIST_NEW:
             self.LINES_LIST_NEW.append(i)
     for i in self.LINES_LIST_NEW:
         d = 100000
         for j in error_POINT_COLOUR_xy:
             b = abs(i[0]-j[0])+abs(i[1]-j[1])
             if b<d:
                 d = b
             else:
                 d = d
             # if i[0] == j[0]:
             #     c += abs(i[1]-j[1])
         dis += d
     print(dis)

difference

在__event_handler中设置car2的按键（不要跟car1重复）并调用detection_error进行误差检测

 def __event_handler(self):
     for event in pygame.event.get():
         # 判断是否退出游戏
         if event.type == pygame.QUIT:
             self.car1.detection_error(car1_track_screen_colour,car1_POINT_COLOUR_xy)
             self.car2.detection_error(car2_track_screen_colour,car2_POINT_COLOUR_xy)
             TrackGame.__game_over()
             # 使用键盘提供的方法获取键盘按键 - 按键元组
     self.car1.key_test(pygame.K_RIGHT, pygame.K_LEFT, pygame.K_UP, pygame.K_DOWN, pygame.K_z, pygame.K_x)
     self.car2.key_test(pygame.K_l, pygame.K_j, pygame.K_i, pygame.K_k, pygame.K_c, pygame.K_v)

muti_track

七、展示视频b站链接,知乎链接,youtube链接

八、预告

将使用本环境，结合深度强化学习代码，进行自主轨迹跟踪
将在本代码中加入可替换的路径规划程序，以代替自己设置的贝塞尔曲线
结合路径规划程序和深度强化学习程序，形成一套完整的轨迹规划仿真