变更

pure_test.py

@@ -13,7 +13,7 @@ a = []
 
 # import test5
 k = 0  # 前视距离系数
-Lfc =3# 前视距离
+Lfc =2# 前视距离
 Kp = 1.0  # 速度P控制器系数
 dt = 0.1  # 时间间隔，单位：s
 L = 2.7  # 车辆轴距，单位：m
@@ -126,10 +126,10 @@ def pure_pursuit_control(state, cx, cy, pind):
     # 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:
+    if len(TX_TY)>15:
         A=TX_TY[-1]
-        B=TX_TY[-4]
-        C=TX_TY[-8]
+        B=TX_TY[-15]
+        C=TX_TY[-7]
     calculate_curvature(A,B,C)
     if curvature>0:
 
@@ -149,21 +149,29 @@ def pure_pursuit_control(state, cx, cy, pind):
     #     Lfc=0.05
     print('cur', curvature)
     #     # print('lfc', Lfc)
-    # if 0.1<=curvature< 0.18:
-    #     Lfc = 0.2
+    # if 0.1<=curvature< 0.15:
+    #     Lfc = 0.3
     #     # print('cur', curvature)
     #     # print('lfc', Lfc)
-    #
+    # if 0.15 <= curvature < 0.2:
+    #     Lfc = 0.05
     # else:
-    #     Lfc = 1.5
-    #     # print('cur', curvature)
-    #     # print('lfc', Lfc)
-
-    # print('A',A)
+    #     Lfc =0.8
+        # print('cur', curvature)
+        # print('lfc', Lfc)
+    #
     if  len(TX_TY)>20:
         Lfc=Lfc*curvature*0.5
-        if Lfc==0:
-            Lfc=2
+        # Lfc=round(Lfc)
+    if curvature<0.05:
+        Lfc=0.4558
+    if Lfc==0:
+        Lfc=1
+
+
+    if np.isnan(Lfc):
+        Lfc = 1
+        print('a is NaN')
     print('lcf',Lfc)
     alpta_1=math.atan2(ty - state.y, tx - state.x)
 
@@ -175,7 +183,7 @@ def pure_pursuit_control(state, cx, cy, pind):
 
     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)
@@ -188,8 +196,8 @@ def pure_pursuit_control(state, cx, cy, pind):
     angle = delta * 57.3
 
     #
-    # fangxp = int((angle) *18)
-    # # fxp=int(fangxp)
+    fangxp = int((angle) *18)
+    fxp=int(fangxp)
     # d=(fangxp-f)
     # print('d',d)
     # if d>4:
@@ -210,7 +218,7 @@ def pure_pursuit_control(state, cx, cy, pind):
     # ag.append(fangxp)
     # print('ag',ag)
     #
-    # print('fxp',fangxp)
+    print('fxp',fangxp)
 
 
 
@@ -328,18 +336,18 @@ def main():
           -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]
+    target_speed = 5/ 3.6  # [m/s]
     T = 500.0  # 最大模拟时间
     # 设置车辆的出事状态
     yaw=math.radians(450-(347+180-360))
-    x_car = np.cos(yaw) * 2.3 + float(96)
+    x_car = np.cos(yaw) * 2.3 + float(96.048)
     # print('x',x_car)
     # north = beidou[15]  # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
-    y_car = -np.sin(yaw) * 2.3 + float(9.9)
+    y_car = -np.sin(yaw) * 2.3 + float( 9.935)
 
     # print('yaw_1',yaw)
 
-    state = VehicleState(x =x_car, y=y_car, yaw = float(yaw), v = 0)
+    state = VehicleState(x =96, y=10, yaw = float(yaw), v = 0)
     # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
     lastIndex = len(cx) - 1
     time = 0.0