import numpy as np
import math
import matplotlib.pyplot as pltk = 0.1  # 前视距离系数
Lfc = 2.0  # 前视距离
Kp = 1.0  # 速度P控制器系数
dt = 0.1  # 时间间隔，单位：s
L = 2.9  # 车辆轴距，单位：mdef plot_arrow(x, y, yaw, length=5, width=1):dx = length * math.cos(yaw)dy = length * math.sin(yaw)plt.arrow(x, y, dx, dy, head_length=width, head_width=width)plt.plot([x, x + dx], [y, y + dy])def plot_robot(x, y, yaw, robot_length, robot_width):corner_x = x - robot_length / 2corner_y = y - robot_width / 2rectangle = plt.Rectangle((corner_x, corner_y), robot_length, robot_width, angle=math.degrees(yaw), fill=False)plt.gca().add_patch(rectangle)class VehicleState:def __init__(self, x=0.0, y=0