【从0开始自动驾驶】用python做一个简单的自动驾驶仿真可视化界面
废话几句
- 自动驾驶开发离不开仿真软件
- 成品仿真软件种类多
- 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
python
import json
import copy
def 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_ori
def 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_cfgsimulator.py
- 使用class编写的仿真器
- 输入为车辆x、y、yaw，进行可视化
- 后段接入运动学等函数可随意进行扩展
python
import numpy
import matplotlib.pyplot as plt
class simulator:
def init_simulator(self, veh_cfg):
plt.ion()
self.veh_cfg = veh_cfg
def draws(self, x, y, yaw, xmin, xmax, ymin, ymax):
self.veh_x = x
self.veh_y = y
self.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"] / 2
self.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
- 主函数入口
python
import time
from 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, ymax
time.sleep(0.1)
plt.pause(1000) # 暂停几秒看一看结果vehicle_config.json
-车辆配置文件
yaml
{
"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
}