Car测试

代码1

"""
cubic spline planner
"""
import math
import numpy as np
import bisect


class Spline:
    u"""
    Cubic Spline class
    """

    def __init__(self, x, y):
        self.b, self.c, self.d, self.w = [], [], [], []

        self.x = x
        self.y = y

        self.nx = len(x)  # dimension of x
        h = np.diff(x)

        # calc coefficient c
        self.a = [iy for iy in y]

        # calc coefficient c
        A = self.__calc_A(h)
        B = self.__calc_B(h)
        self.c = np.linalg.solve(A, B)
        #  print(self.c1)

        # calc spline coefficient b and d
        for i in range(self.nx - 1):
            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
            tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
                (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
            self.b.append(tb)

    def calc(self, t):
        u"""
        Calc position

        if t is outside of the input x, return None

        """

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index(t)
        dx = t - self.x[i]
        result = self.a[i] + self.b[i] * dx + \
            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

        return result

    def calcd(self, t):
        u"""
        Calc first derivative

        if t is outside of the input x, return None
        """

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index(t)
        dx = t - self.x[i]
        result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
        return result

    def calcdd(self, t):
        u"""
        Calc second derivative
        """

        if t < self.x[0]:
            return None
        elif t > self.x[-1]:
            return None

        i = self.__search_index(t)
        dx = t - self.x[i]
        result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
        return result

    def __search_index(self, x):
        u"""
        search data segment index
        """
        return bisect.bisect(self.x, x) - 1

    def __calc_A(self, h):
        u"""
        calc matrix A for spline coefficient c
        """
        A = np.zeros((self.nx, self.nx))
        A[0, 0] = 1.0
        for i in range(self.nx - 1):
            if i != (self.nx - 2):
                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
            A[i + 1, i] = h[i]
            A[i, i + 1] = h[i]

        A[0, 1] = 0.0
        A[self.nx - 1, self.nx - 2] = 0.0
        A[self.nx - 1, self.nx - 1] = 1.0
        #  print(A)
        return A

    def __calc_B(self, h):
        u"""
        calc matrix B for spline coefficient c
        """
        B = np.zeros(self.nx)
        for i in range(self.nx - 2):
            B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
                h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
        #  print(B)
        return B


class Spline2D:
    u"""
    2D Cubic Spline class

    """

    def __init__(self, x, y):
        self.s = self.__calc_s(x, y)
        self.sx = Spline(self.s, x)
        self.sy = Spline(self.s, y)

    def __calc_s(self, x, y):
        dx = np.diff(x)
        dy = np.diff(y)
        self.ds = [math.sqrt(idx ** 2 + idy ** 2)
                   for (idx, idy) in zip(dx, dy)]
        s = [0]
        s.extend(np.cumsum(self.ds))
        return s

    def calc_position(self, s):
        u"""
        calc position
        """
        x = self.sx.calc(s)
        y = self.sy.calc(s)

        return x, y

    def calc_curvature(self, s):
        u"""
        calc curvature
        """
        dx = self.sx.calcd(s)
        ddx = self.sx.calcdd(s)
        dy = self.sy.calcd(s)
        ddy = self.sy.calcdd(s)
        k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
        return k

    def calc_yaw(self, s):
        u"""
        calc yaw
        """
        dx = self.sx.calcd(s)
        dy = self.sy.calcd(s)
        yaw = math.atan2(dy, dx)
        return yaw


def calc_spline_course(x, y, ds=0.1):
    sp = Spline2D(x, y)
    s = list(np.arange(0, sp.s[-1], ds))

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        ix, iy = sp.calc_position(i_s)
        rx.append(ix)
        ry.append(iy)
        ryaw.append(sp.calc_yaw(i_s))
        rk.append(sp.calc_curvature(i_s))

    return rx, ry, ryaw, rk, s


def main():
    print("Spline 2D test")
    import matplotlib.pyplot as plt
    x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
    y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
    ds = 0.1  # [m] distance of each intepolated points

    sp = Spline2D(x, y)
    s = np.arange(0, sp.s[-1], ds)

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        ix, iy = sp.calc_position(i_s)
        rx.append(ix)
        ry.append(iy)
        ryaw.append(sp.calc_yaw(i_s))
        rk.append(sp.calc_curvature(i_s))

    flg, ax = plt.subplots(1)
    plt.plot(x, y, "xb", label="input")
    plt.plot(rx, ry, "-r", label="spline")
    plt.grid(True)
    plt.axis("equal")
    plt.xlabel("x[m]")
    plt.ylabel("y[m]")
    plt.legend()

    flg, ax = plt.subplots(1)
    plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw")
    plt.grid(True)
    plt.legend()
    plt.xlabel("line length[m]")
    plt.ylabel("yaw angle[deg]")

    flg, ax = plt.subplots(1)
    plt.plot(s, rk, "-r", label="curvature")
    plt.grid(True)
    plt.legend()
    plt.xlabel("line length[m]")
    plt.ylabel("curvature [1/m]")

    plt.show()


if __name__ == '__main__':
    main()

代码2

"""

Path tracking simulation with Stanley steering control and PID speed control.

author: Atsushi Sakai (@Atsushi_twi)
控制增益k，它决定了转向角命令对横向误差的敏感程度。
k越大，转向角越大，横向误差越小，但也可能导致过冲或振荡2。

车辆轴距L，它影响了车辆的转弯半径和稳定性。
L越大，车辆越难转弯，横向误差越大3。

参考速度target_speed，它决定了车辆在路径上的运动状态。
target_speed越大，车辆越难跟随曲线路径，横向误差越大3。

"""
import sys
#sys.path.append("../../PathPlanning/CubicSpline/")
import numpy as np
import math
import matplotlib.pyplot as plt
import cubic_spline_planner


# 导入xlwt库，用于操作excel文件
import xlwt



#k = 0.5  # control gain
k = 0.5 # control gain

Kp = 0.5  # speed propotional gain
dt = 0.1  # [s] time difference
#L = 2.9  # [m] Wheel base of vehicle
L = 2.9  # [m] Wheel base of vehicle
max_steer = math.radians(30.0)  # [rad] max steering angle

show_animation = True
#########################################
########### 是  否  保  存################
Save_data = True
#Save_data = False
#########################################
steering_angle = []

""" # 定义PID参数
Kp = 1.5# 比例增益
Ki = 0.00 # 积分增益
Kd = 0.00 # 微分增益 """


""" # 定义PID参数 DU 80km
Kp = 1.555# 比例增益
Ki = 0.00 # 积分增益
Kd = 0.0 # 微分增益 """


# 定义PID参数  DU 30km
Kp = 1.16 # 比例增益
Ki = 0.0 # 积分增益
Kd = 0.0 # 微分增益


""" # 定义PID参数  DU 50km
Kp = 1.305 # 比例增益
Ki = 0.0 # 积分增益
Kd = 0.01 # 微分增益 """


# 定义全局变量
error_sum = 0.0 # 误差累积值
theta_e_prev = 0.0 # 上一次的误差




#state是定义一个类(class)的语句，
# 用于创建一个名为State的类，
# 它有四个属性(attribute)，
# 分别是x,y,yaw,v。
# 这个类可以用来表示车辆的状态(state)，
# 包括位置(x,y)、航向角(yaw)和速度(v)。
# 这个类还有一个初始化方法(init)，用于给属性赋初值。
class State:

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v

""" 
这是定义一个函数(function)的语句,用def开头,后面跟着函数名和参数列表。
函数是一段可以重复使用的代码,它可以接收一些输入(参数),并返回一些输出(return)。 
这个函数的功能是根据车辆的加速度(a)和转向角(delta),
更新车辆的状态(state),包括位置(x,y)、航向角(yaw)和速度(v)。
状态是一个类的实例(instance),可以用点号访问它的属性(attribute)。 
函数内部有一个if-elif语句,用于判断转向角是否超过最大值,如果超过,则将其限制在最大值范围内。
 然后,使用简单的运动学模型来计算车辆在dt时间内的状态变化,并赋值给状态对象。  
 """
def update(state, a, delta):

    if delta >= max_steer:
        delta = max_steer
    elif delta <= -max_steer:
        delta = -max_steer

    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    state.yaw = pi_2_pi(state.yaw)
    state.v = state.v + a * dt

    # 添加这一行代码
    steering_angle.append(delta*180/3.14)

    return state

####不要加速度了，直接给初值
def PIDControl(target, current):
    a = Kp * (target - current)

    return a


def stanley_control(state, cx, cy, cyaw, pind):
    global error_sum # 声明全局变量
    global theta_e_prev

    ind, efa = calc_target_index(state, cx, cy)

    if pind >= ind:
        ind = pind

    # 计算期望航向角和实际航向角之间的误差，并限制在[-pi, pi]范围内
    theta_e = pi_2_pi(cyaw[ind] - state.yaw)

    # 使用PID公式计算转向角delta
    delta = Kp * theta_e + Ki * error_sum + Kd * (theta_e - theta_e_prev) / dt

    # 更新error_sum和theta_e_prev的值
    error_sum += theta_e * dt 
    theta_e_prev = theta_e

    return delta, ind

#pi_2_pi函数来将航向角限制在[-pi, pi]范围内
# 并返回更新后的状态对象。
def pi_2_pi(angle):
    while (angle > math.pi):
        angle = angle - 2.0 * math.pi

    while (angle < -math.pi):
        angle = angle + 2.0 * math.pi

    return angle

#calc_target_index函数
# 用于计算车辆前轴位置与参考路径上最近点的索引和距离。
# 函数内部先计算了车辆前轴的坐标，
# 然后遍历了参考路径上的所有点，找到了距离最小的点，
# 并返回了它的索引和距离。
# 如果车辆前轴与最近点之间的夹角大于0，
# 则说明车辆偏离了路径，此时将距离取反。
def calc_target_index(state, cx, cy):

    # calc frant axle position
    fx = state.x + L * math.cos(state.yaw)
    fy = state.y + L * math.sin(state.yaw)

    # search nearest point index
    dx = [fx - icx for icx in cx]
    dy = [fy - icy for icy in cy]
    d = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
    mind = min(d)
    ind = d.index(mind)

    tyaw = pi_2_pi(math.atan2(fy - cy[ind], fx - cx[ind]) - state.yaw)
    if tyaw > 0.0:
        mind = - mind

    return ind, mind


def main():


    
    with open('E:\CarTrackingCode\S—path.txt') as f:
    # 跳过第一行标题
        next(f)
        # 创建两个空列表存储x,y坐标点
        X_path = []
        Y_path = []
        # 遍历每一行
        for line in f:
            # 去掉换行符
            line = line.strip()
            # 按空格分割字符串
            x, y = line.split()
            # 将坐标转换为浮点数
            y = float(y)
            x = float(x)
            # 将x，y坐标添加到列表中
            X_path.append(x)
            Y_path.append(y)
    
    
    #  target course
    ax = X_path
    ay = Y_path
    
    
    
    #  target course
    #ax = [0,10,20,30,40,50,60,70,80,90,100, 110,120,130, 140,150,160,170,180,190,200]
    #ay = [0, 0, 0, 0, 3, 3, 0,-3,-3, 0,  0,2.5, 5, 5,2.5,0,-2.5, -5, -5, -2.5,0,0]

    #  target course
    #ax = [0.0,30, 60, 90, 120]
    #ay = [0.0, 5,-5, 8, -8]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=0.1)


    #######################################修改仿真速度#############################################################################
    target_speed = 30 / 3.6  # [m/s]
    
    T = 50.0  # max simulation time
    #######################################修改仿真时间
    #######################################修改仿真时间
    #######################################修改仿真时间
    #######################################修改仿真时间
    #######################################修改仿真时间

    # initial state
    state = State(x=0.0, y=0.0, yaw=math.radians(0.0), v=0.0)

    lastIndex = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    Lather_err = []
    Delta_data= []
    Theta_d_data =[]
    Theta_e_data =[]
    r = []
    
    target_ind, mind = calc_target_index(state, cx, cy)

    while T >= time and lastIndex > target_ind:
        ai = PIDControl(target_speed, state.v)
        di, target_ind= stanley_control(state, cx, cy, cyaw, target_ind)
        state = update(state, ai, di)

        time = time + dt
        target_ind_NoYong, mind = calc_target_index(state, cx, cy)
        # 修改增益，调整横向误差数值########################################################################################
        Lather_err.append(mind)
        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)
        Delta_data.append(di)
        #Theta_e_data.append(theta_e_data)
        #Theta_d_data.append(theta_d_data)

        # 根据公式求取横摆角速度
        r = [((yaw[i+1] - yaw[i]))*180/3.14 / dt for i in range(len(yaw) - 1)]
        

        if show_animation:
            plt.cla()
            plt.plot(cx, cy, "-r", label="ReferenceLine")
            plt.plot(x, y, "--b", label="Trajectory")
            plt.plot(cx[target_ind], cy[target_ind], "xg")
            #plt.axis("equal")
            # 双移线
            plt.xlim([-1, 200])
            plt.ylim([-1, 5])
            # 设置X轴和Y轴的范围，双纽线
            #plt.xlim([-200, 200])
            #plt.ylim([-80, 80])
            ## 设置X轴和Y轴的范围_蛇形线
            #plt.xlim([0, 200])
            #plt.ylim([-6, 6])
            ## 设置X轴和Y轴的范围_双U形线
            #plt.ylim([-1, 50])
            #plt.xlim([-20, 40])

            # 圆形
            #plt.ylim([-5, 55])
            #plt.xlim([-30, 30])

            plt.grid(True)
            plt.legend()
            plt.title("Driving speed[km/h]:" + str(state.v * 3.6)[:4])
            #plt.title("Driving speed[km/h]:30" )
            plt.pause(0.001)


    ###############################################################################################################################        
    if Save_data:
        # 创建一个workbook对象
        wb = xlwt.Workbook()
        # 创建一个sheet对象，并命名为data
        ws = wb.add_sheet('data')
        for i in range(len(Lather_err)):
            ws.write(i, 0, Lather_err[i]) # 将Lather_err的值写入到第一行第八列
            ws.write(i, 1, Delta_data[i]) # 将Delta_data的值写入到第一行第九列
            ws.write(i, 2 ,r[i]) # 将r的值写入到第一行第十列
            #ws.write(i, 3 ,x[i]) # 将r的值写入到第一行第十列
            #ws.write(i, 4 ,y[i]) # 将r的值写入到第一行第十列

        # 使用当前时间加车速作为excel文件名，例如2021-12-15-16-30-45_30.xls，注意将速度从m/s转换为km/h并取整数部分
        file_name = 'S30_lerr_Delta_HengBaiJiaospeed' + '.xls'
        # 使用save()方法将workbook保存为excel文件，并给它一个文件名
        wb.save(file_name)
    ########################################################################################################################
    # Test
    assert lastIndex >= target_ind-5, "Cannot goal"
    ###############
    #print(Lather_err)

    if show_animation:
        #plt.plot(cx, cy, "-r", label="course")
        #plt.plot(x, y, "-b", label="trajectory")
        #plt.plot(ax, ay, "xb", label="input")
        #plt.plot(cx, cy, "-r", label="spline")
        #plt.plot(x, y, "-g", label="tracking")
        #plt.legend()
        # 设置X轴和Y轴的范围
        #plt.xlim([-1, 200])
        #plt.ylim([-1, 5])
        #plt.xlabel("x[m]")
        #plt.ylabel("y[m]")
        #plt.axis("equal")
        #plt.grid(True)


        # 绘制参考轨迹        
        flg, ax = plt.subplots(1)
        plt.plot(cx, cy, "-b", label="DoubleLineChange")
        plt.xlabel("X(m)")
        plt.ylabel("Y(m)")
        plt.grid(True)
        plt.legend()

        
        # 显示航向角
        flg, ax = plt.subplots(1)
        plt.plot(yaw, "-r")
        plt.xlabel("Time(s)")
        plt.ylabel("Yaw")
        plt.grid(True)
        plt.legend()

        # 显示转向角(这个和前轮转角以及航向角有什么区别？)
        # 就是前轮转角

        #flg, ax = plt.subplots(1)
        #plt.plot(Delta_data[5:len(Delta_data)], "-g")
        #plt.xlabel("Time[s]")
        #plt.ylabel("Delta")
        #plt.grid(True)

        # 显示横摆角速度
        flg, ax = plt.subplots(1)
        plt.plot(r, "-g")
        plt.xlabel("Time(s)")
        plt.ylabel("Yaw_Velocity(°/s)")
        plt.grid(True)
        plt.legend()

        #显示横向误差
        flg, ax = plt.subplots(1)
        plt.plot(Lather_err[:len(Lather_err)-2], "-r")
        print(Lather_err)
        #print('###############################')
        #print(Lather_err[:len(Lather_err)-1])
        plt.xlabel("Time(s)")
        plt.ylabel("Lateral err(m)")
        plt.ylim([-0.25, 0.25])
        plt.grid(True)
        plt.legend()
        
        # 在速度图下面，增加一个新的子图来显示前轮转角图
        flg2 , ax2 = plt.subplots(1)
        plt.plot(steering_angle[5:], '-b')
        plt.ylabel('Steering angle(rad)')
        plt.xlabel('Time(s)')
        plt.grid(True)
        plt.legend()


        plt.show()
        print(cx)
       

if __name__ == '__main__':
    main()