import time # import txt2 import numpy as np import math import matplotlib.pyplot as plt from loguru import logger # import tcp4 import sqlite3 import threading import CUR ag = [0,0] a = [] # import test5 k = 0 # 前视距离系数 Lfc =3# 前视距离 Kp = 1.0 # 速度P控制器系数 dt = 0.1 # 时间间隔，单位：s L = 2.7 # 车辆轴距，单位：m x1=[] y1=[] # yaw1 = math.radians(414) # print('y',yaw1) # arc=math.atan2(1, 1) # print('acr',arc) yaw = math.radians(450 - (344 + 180 - 360)) f=0 T = [] TX = [] curvature=0 A=0 B=0 C=0 def calculate_curvature(point_a, point_b, point_c): global curvature # 将输入点转换为 NumPy 数组 a = np.array(point_a) b = np.array(point_b) c = np.array(point_c) # 计算各边长 ab = np.linalg.norm(a - b) bc = np.linalg.norm(b - c) ca = np.linalg.norm(c - a) # 使用海伦公式计算三角形面积 s = (ab + bc + ca) / 2 area = np.sqrt(s * (s - ab) * (s - bc) * (s - ca)) # 计算曲率 curvature = 4 * area / (ab * bc * ca) print('cur',curvature) # i=0 # # a = path[i] # # b = path[i + 10] # # c = path[i + 20] # # curvature = calculate_curvature(a, b, c) # i=i+1 # print(i) # return curvature class VehicleState: # state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0) def __init__(self, x=88, y=47, yaw=yaw, v=0.0): self.x = x self.y = y self.yaw = yaw self.v = v def update(state, a, delta): state.x =state.x + state.v * math.cos(state.yaw) * dt # print("state.x",state.x) state.y = state.y + state.v * math.sin(state.yaw) * dt # print("state.y",state.y) state.yaw = state.yaw + state.v / L * math.tan(delta) * dt # print('state.yaw',state.yaw) state.v = state.v + a * dt return state def PControl(target, current): a = Kp * (target - current) return a def remove_adjacent_duplicates(coords): result = [] for coord in coords: if not result or coord != result[-1]: result.append(coord) print('coor',result) def pure_pursuit_control(state, cx, cy, pind): global ag,f, Lfc, delta, curvature, A, B, C ind = calc_target_index(state, cx, cy) # if pind >= ind: # ind = pind if ind < len(cx): tx = cx[ind] # print('tx1',tx) ty = cy[ind] # print('ty1',ty) TX.append((tx, ty)) # print(TX) else: tx = cx[-1] print('tx2',tx) ty = cy[-1] print('ty2',ty) ind = len(cx) - 1 T.append(TX) # print('T0',T[0][0]) # print('t',TX) # A=0 # if len(T)>20: # remove_adjacent_duplicates(T) # seen = set() # coords_without_duplicates = [] # for coord in T: # if coord not in seen: # coords_without_duplicates.append(coord) # seen.add(coord) # else: # coords_without_duplicates.remove(coord) # # print(coords_without_duplicates) # return result TX_TY = [x for i, x in enumerate(T[0]) if i == 0 or x != T[0][i - 1]] # print('txty',TX_TY) if len(TX_TY)>16: A=TX_TY[-1] B=TX_TY[-4] C=TX_TY[-8] calculate_curvature(A,B,C) if curvature>0: a.append(curvature) print('a', a) # if len(T)>300: # for i in range(len(T)): # global curvature # curvature=calculate_curvature(T[0][i],T[0][i+10],T[0][i+20]) # print('cur,1',curvature) # print('to',T[0][i]) # print('t1',T[0][i+10]) # print('t2',T[0][i+20]) # if curvature>=0.18: # Lfc=0.05 print('cur', curvature) # # print('lfc', Lfc) # if 0.1<=curvature< 0.18: # Lfc = 0.2 # # print('cur', curvature) # # print('lfc', Lfc) # # else: # Lfc = 1.5 # # print('cur', curvature) # # print('lfc', Lfc) # print('A',A) if len(TX_TY)>20: Lfc=Lfc*curvature print('lcf',Lfc) alpta_1=math.atan2(ty - state.y, tx - state.x) alpha = alpta_1- state.yaw # print('alpha',alpha) delta = math.atan2(2.0 * L * math.sin(alpha) / Lfc, 1.0) # print('delta',delta) if delta > np.pi / 6.0: delta = np.pi / 6.0 # print('delta1', delta) elif delta < - np.pi / 6.0: delta = - np.pi / 6.0 # print('delta2', delta) # print('deltal1',delta1) angle = delta * 57.3 # # fangxp = int((angle) *18) # # fxp=int(fangxp) # d=(fangxp-f) # print('d',d) # if d>4: # d=4 # fangxp = f + d # fangxp=f+d # # if d<-4: # d=-4 # fangxp=f+d # f = fangxp # print('f', f) # # # # print('fangx[',fangxp) # # # ag.append(fangxp) # print('ag',ag) # # print('fxp',fangxp) time.sleep(0.04) # print(delta) return delta, ind def calc_target_index(state, cx, cy): # 搜索最临近的路点 dx = [state.x - icx for icx in cx] # print('dx',dx) dy = [state.y - icy for icy in cy] # print('dy',dy) d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] # print('d',d) ind = d.index(min(d)) # print('ind',ind) L = 0.0 Lf = k * state.v + Lfc while Lf > L and (ind + 1) < len(cx): dx = cx[ind + 1] - cx[ind] dy = cx[ind + 1] - cx[ind] L += math.sqrt(dx ** 2 + dy ** 2) ind += 1 return ind def main(): global x1,y1 # 设置目标路点 cx = [ 100.012, 100.043, 100.076, 100.108, 100.135, 100.167, 100.191, 100.221, 100.252, 100.281, 100.311, 100.342, 100.371, 100.394, 100.427, 100.455, 100.49, 100.522, 100.553, 100.58, 100.614, 100.641, 100.672, 100.702, 100.728, 100.761, 100.792, 100.824, 100.849, 100.876, 100.909, 100.942, 100.967, 100.999, 101.031, 101.059, 101.09, 101.121, 101.15, 101.175, 101.204, 101.235, 101.264, 101.288, 101.32, 101.348, 101.38, 101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683, 101.716, 101.744, 101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033, 102.057, 102.082, 102.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176, 102.165, 102.141, 102.112, 102.086, 102.058, 102.031, 101.989, 101.955, 101.912, 101.861, 101.805, 101.748, 101.68, 101.606, 101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611, 100.505, 100.406, 100.299, 100.195, 100.085, 99.979, 99.867, 99.739, 99.62, 99.493, 99.363, 99.231, 99.102, 98.965, 98.83, 98.623, 98.48, 98.339, 98.198, 98.06, 97.927, 97.794, 97.659, 97.534, 97.402, 97.268, 97.143, 97.023, 96.896, 96.773, 96.645, 96.518, 96.393, 96.26, 96.127, 95.988, 95.845, 95.706, 95.558, 95.409, 95.258, 95.102, 94.945, 94.788, 94.633, 94.475, 94.315, 94.16, 94.002, 93.903, 93.75, 93.589, 93.434, 93.274, 93.118, 92.963, 92.807, 92.648, 92.487, 92.333, 92.18, 92.022, 91.864, 91.711, 91.553, 91.407, 91.252, 91.102, 90.954, 90.806, 90.652, 90.511, 90.354, 90.203, 90.049, 89.89, 89.737, 89.581, 89.422, 89.26, 89.108, 88.949, 88.793, 88.636, 88.48, 88.326, 88.168, 88.01, 87.855, 87.7, 87.548, 87.395, 87.243, 87.088, 86.939, 86.785, 86.635, 86.482, 86.334, 86.185, 86.038, 85.886, 85.666, 85.516, 85.367, 85.217, 85.067, 84.913, 84.766, 84.62, 84.473, 84.322, 84.171, 84.022, 83.871, 83.722, 83.576, 83.429, 83.282, 83.132, 82.981, 82.834, 82.687, 82.524, 82.375, 82.225, 82.072, 81.918, 81.772, 81.618, 81.457, 81.309, 81.154, 80.998, 80.843, 80.694, 80.543, 80.385, 80.237, 80.086, 79.937, 79.785, 79.639, 79.493, 79.355, 79.205, 79.063, 78.922, 78.784, 78.647, 78.517, 78.384, 78.25, 78.119, 77.986, 77.856, 77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707] cy = [ -9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395, -11.54, -11.689, -11.832, -11.978, -12.121, -12.267, -12.412, -12.559, -12.705, -12.85, -13.006, -13.15, -13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903, -15.051, -15.195, -15.346, -15.491, -15.642, -15.794, -15.945, -16.091, -16.249, -16.401, -16.546, -16.696, -16.843, -16.986, -17.132, -17.272, -17.413, -17.553, -17.694, -17.839, -17.984, -18.128, -18.271, -18.413, -18.558, -18.705, -18.857, -19.008, -19.149, -19.293, -19.441, -19.586, -19.73, -19.876, -20.023, -20.169, -20.317, -20.462, -20.61, -20.763, -20.912, -21.067, -21.225, -21.373, -21.528, -21.675, -21.818, -21.953, -22.076, -22.203, -22.372, -22.481, -22.601, -22.718, -22.832, -22.95, -23.07, -23.192, -23.315, -23.439, -23.56, -23.68, -23.799, -23.912, -24.022, -24.132, -24.242, -24.34, -24.435, -24.518, -24.605, -24.686, -24.761, -24.838, -24.911, -24.978, -25.039, -25.1, -25.154, -25.213, -25.253, -25.307, -25.361, -25.408, -25.451, -25.493, -25.53, -25.573, -25.62, -25.642, -25.665, -25.683, -25.703, -25.718, -25.732, -25.747, -25.758, -25.759, -25.769, -25.778, -25.789, -25.799, -25.799, -25.8, -25.8, -25.8, -25.8, -25.807, -25.816, -25.823, -25.836, -25.841, -25.861, -25.884, -25.908, -25.936, -25.961, -25.99, -26.017, -26.048, -26.084, -26.118, -26.141, -26.176, -26.213, -26.247, -26.274, -26.299, -26.34, -26.373, -26.403, -26.435, -26.467, -26.5, -26.532, -26.563, -26.599, -26.624, -26.661, -26.683, -26.715, -26.748, -26.78, -26.813, -26.842, -26.873, -26.908, -26.943, -26.977, -27.008, -27.043, -27.068, -27.095, -27.133, -27.164, -27.193, -27.224, -27.255, -27.287, -27.319, -27.348, -27.381, -27.407, -27.448, -27.483, -27.518, -27.55, -27.587, -27.615, -27.643, -27.673, -27.697, -27.725, -27.753, -27.782, -27.823, -27.849, -27.876, -27.901, -27.931, -27.955, -27.982, -28.009, -28.036, -28.065, -28.088, -28.117, -28.142, -28.172, -28.201, -28.232, -28.251, -28.278, -28.304, -28.333, -28.361, -28.38, -28.403, -28.436, -28.464, -28.495, -28.531, -28.558, -28.581, -28.618, -28.649, -28.679, -28.711, -28.74, -28.769, -28.792, -28.825, -28.851, -28.877, -28.901, -28.931, -28.955, -28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28, -29.304, -29.328, -29.346, -29.369, -29.384, -29.409, -29.426, -29.458, -29.479, -29.499] target_speed = 2 / 3.6 # [m/s] T = 500.0 # 最大模拟时间 # 设置车辆的出事状态 yaw=math.radians(450-(347+180-360)) x_car = np.cos(yaw) * 2.3 + float(100) # print('x',x_car) # north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位 y_car = -np.sin(yaw) * 2.3 + float(-9.78) # print('yaw_1',yaw) state = VehicleState(x =x_car, y=y_car, yaw = float(yaw), v = 0) # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0) lastIndex = len(cx) - 1 time = 0.0 x = [state.x] # print('x',x) y = [state.y] # print('y',y) yaw = [state.yaw] v = [state.v] t = [0.0] target_ind = calc_target_index(state, cx, cy) while lastIndex > target_ind: ai = PControl(target_speed, state.v) di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) # fxp=int(di*57.3*18) # print('fxp1',fxp) state = update(state, ai, di) time = time + dt x.append(state.x) x1=x[-1] # print('x1',x[-1]) y.append(state.y) y1=y[-1] # print('y1',y[-1]) yaw.append(state.yaw) yaw1=yaw[-1] # print('yaw',yaw[-1]) v.append(state.v) # print('v',v[-1]) t.append(time) # logger.add('file.log') # logger.debug('角度 {},y {},x {}',str(yaw1),str(y1),str(x1)) # f = open('data1,txt', 'a') # # print('fff') # f.write("角度"+str(yaw1)) # f.write('\n') # f.write("\n") # f.close() plt.cla() plt.plot(cx, cy, ".r", label="course") plt.plot(x, y, "-b", label="trajectory") plt.plot(cx[target_ind], cy[target_ind], "go", label="target") plt.axis("equal") plt.grid(True) plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) plt.pause(0.001) plt.ioff() plt.show() if __name__ == '__main__': main() # t1 = threading.Thread(target=test5.can165) # 读取eps状态 # t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度 # t3 = threading.Thread(target=test5.main) # t4 = threading.Thread(target=test5.update) t5 = threading.Thread(target=main) t8=threading.Thread(target=CUR.calculate_curvature) # t6 = threading.Thread(target=tcp4.zuo_biao) # t7=threading.Thread(target=txt2.txt) # # # 启动线程 # t7.start() t5.start() # t6.start() # t1.start() # t2.start() # t3.start() # t4.start()