废话几句

• 自动驾驶开发离不开仿真软件
• 成品仿真软件种类多
• https://zhuanlan.zhihu.com/p/321771761#:~:text=%E5%8D%95%E5%B0%B1%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6%E4%BB%BF%E7%9C%9F,%E5%90%84%E8%87%AA%E7%9A%84%E5%8F%AF%E5%8F%96%E4%B9%8B%E5%A4%84%E3%80%82
• 问题在于
  • 软件大多为WINDOWS环境，不一定满足开发环境需求
  • 软件大多收费，受版权影响
  • 软件较为复杂，普通开发任务无需复杂仿真软件
• 那么
  • 以最简单的方式从0开始搭建一个可视化仿真软件
  • 直接使用python matplot库进行可视化
  • 后段采用简单车辆运动学控制可视化界面
    

废话不多说，直接上源码

目录结构

• src
  • simple_simulator_app.py
  • config
    • vehicle_config.json
  • lib
    • init.py
    • simulator.py

init.py

import json
import copydef import_veh_cfg(veh_cfg_path):with open(veh_cfg_path, "r", encoding="utf-8") as load_f:veh_cfg_ori = json.load(load_f)return veh_cfg_oridef init_veh_cfg(veh_cfg_path):veh_cfg_ori = import_veh_cfg(veh_cfg_path)# print(veh_cfg_ori)veh_cfg = copy.deepcopy(veh_cfg_ori)return veh_cfg

simulator.py

• 使用class编写的仿真器
• 输入为车辆x、y、yaw，进行可视化
• 后段接入运动学等函数可随意进行扩展

import numpy
import matplotlib.pyplot as pltclass simulator:def init_simulator(self, veh_cfg):plt.ion()self.veh_cfg = veh_cfgdef draws(self, x, y, yaw, xmin, xmax, ymin, ymax):self.veh_x = xself.veh_y = yself.veh_yaw = yaw  # 角度plt.clf()  # 清除之前画的图plt.xlim(xmin, xmax)plt.ylim(ymin, ymax)plt.title("simulator")plt.xlabel("X/m")plt.ylabel("Y/m")ax = plt.gca()ax.set_aspect(1)  # 保持纵横比self.draw_veh()plt.pause(0.001)def draws_close(self):plt.ioff()def draw_veh(self):  # yaw以x轴为0，逆时针为正plt.plot(self.veh_x, self.veh_y, "o", color="r")self.ca_veh_points()plt.plot(self.veh_x_points, self.veh_y_points, color="r")def ca_veh_points(self):  # 计算车辆包络框的所有点half_veh_width = self.veh_cfg["width"] / 2self.veh_yaw_rad = numpy.deg2rad(self.veh_yaw)self.veh_x_points = [self.veh_x+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.cos(self.veh_yaw_rad)+ half_veh_width * numpy.sin(self.veh_yaw_rad),self.veh_x+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.cos(self.veh_yaw_rad)- half_veh_width * numpy.sin(self.veh_yaw_rad),self.veh_x- (self.veh_cfg["rear_overhang"]) * numpy.cos(self.veh_yaw_rad)- half_veh_width * numpy.sin(self.veh_yaw_rad),self.veh_x- (self.veh_cfg["rear_overhang"]) * numpy.cos(self.veh_yaw_rad)+ half_veh_width * numpy.sin(self.veh_yaw_rad),self.veh_x+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.cos(self.veh_yaw_rad)+ half_veh_width * numpy.sin(self.veh_yaw_rad),]self.veh_y_points = [self.veh_y+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.sin(self.veh_yaw_rad)- half_veh_width * numpy.cos(self.veh_yaw_rad),self.veh_y+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.sin(self.veh_yaw_rad)+ half_veh_width * numpy.cos(self.veh_yaw_rad),self.veh_y- (self.veh_cfg["rear_overhang"]) * numpy.sin(self.veh_yaw_rad)+ half_veh_width * numpy.cos(self.veh_yaw_rad),self.veh_y- (self.veh_cfg["rear_overhang"]) * numpy.sin(self.veh_yaw_rad)- half_veh_width * numpy.cos(self.veh_yaw_rad),self.veh_y+ (self.veh_cfg["length"] - self.veh_cfg["rear_overhang"])* numpy.sin(self.veh_yaw_rad)- half_veh_width * numpy.cos(self.veh_yaw_rad),]

simple_simulator_app.py

• 主函数入口

import timefrom lib.init import *
from lib.simulator import *if __name__ == "__main__":veh_cfg_path = r"./config/vehicle_config.json"veh_cfg = init_veh_cfg(veh_cfg_path)simulator_ = simulator()simulator_.init_simulator(veh_cfg)for i in range(0, 20):simulator_.draws(i, 15 + i * 0.3, 30 + i, 0, 30, 0, 30)  # veh_x, veh_y, veh_yaw, xmin, xmax, ymin, ymaxtime.sleep(0.1)plt.pause(1000)  # 暂停几秒看一看结果

vehicle_config.json

-车辆配置文件

{"vehicle_type": "test","front_wheel_base": 1.3,"rear_wheel_base": 1.3,"width": 1.9,"length": 4,"rear_overhang": 0.4,"max_steer_wheel_angle": 35.0,"steer_ratio": 17.5
}