def _DynModel(x, x_glob, u, np, dt, PointAndTangent):
# This function computes the system evolution. Note that the discretization is deltaT and therefore is needed that
# dt <= deltaT and ( dt / deltaT) = integer value
# Vehicle Parameters
m = 1.98
lf = 0.125
lr = 0.125
Iz = 0.024
Df = 0.8 * m * 9.81 / 2.0
Cf = 1.25
Bf = 1.0
Dr = 0.8 * m * 9.81 / 2.0
Cr = 1.25
Br = 1.0
# Discretization Parameters
deltaT = 0.001
x_next = np.zeros(x.shape[0])
cur_x_next = np.zeros(x.shape[0])
# Extract the value of the states
delta = u[0]
a = u[1]
psi = x_glob[3]
X = x_glob[4]
Y = x_glob[5]
vx = x[0]
vy = x[1]
wz = x[2]
epsi = x[3]
s = x[4]
ey = x[5]
# Initialize counter
i = 0
while ((i + 1) * deltaT <= dt):
# Compute tire split angle
alpha_f = delta - np.arctan2(vy + lf * wz, vx)
alpha_r = -np.arctan2(vy - lf * wz, vx)
# Compute lateral force at front and rear tire
Fyf = 2 * Df * np.sin(Cf * np.arctan(Bf * alpha_f))
Fyr = 2 * Dr * np.sin(Cr * np.arctan(Br * alpha_r))
# Propagate the dynamics of deltaT
x_next[0] = vx + deltaT * (a - 1 / m * Fyf * np.sin(delta) + wz * vy)
x_next[1] = vy + deltaT * (1 / m *
(Fyf * np.cos(delta) + Fyr) - wz * vx)
x_next[2] = wz + deltaT * (1 / Iz *
(lf * Fyf * np.cos(delta) - lr * Fyr))
x_next[3] = psi + deltaT * (wz)
x_next[4] = X + deltaT * ((vx * np.cos(psi) - vy * np.sin(psi)))
x_next[5] = Y + deltaT * (vx * np.sin(psi) + vy * np.cos(psi))
cur = Curvature(s, PointAndTangent)
cur_x_next[0] = vx + deltaT * (a - 1 / m * Fyf * np.sin(delta) +
wz * vy)
cur_x_next[1] = vy + deltaT * (1 / m *
(Fyf * np.cos(delta) + Fyr) - wz * vx)
cur_x_next[2] = wz + deltaT * (1 / Iz *
(lf * Fyf * np.cos(delta) - lr * Fyr))
cur_x_next[3] = epsi + deltaT * (
wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey) * cur)
cur_x_next[4] = s + deltaT * ((vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey))
cur_x_next[5] = ey + deltaT * (vx * np.sin(epsi) + vy * np.cos(epsi))
# Update the value of the states
psi = x_next[3]
X = x_next[4]
Y = x_next[5]
vx = cur_x_next[0]
vy = cur_x_next[1]
wz = cur_x_next[2]
epsi = cur_x_next[3]
s = cur_x_next[4]
ey = cur_x_next[5]
if (s < 0):
print("Start Point: ", x, " Input: ", u)
print("x_next: ", x_next)
# Increment counter
i = i + 1
# Noises
noise_vx = np.maximum(-0.05, np.minimum(np.random.randn() * 0.01, 0.05))
noise_vy = np.maximum(-0.1, np.minimum(np.random.randn() * 0.01, 0.1))
noise_wz = np.maximum(-0.05, np.minimum(np.random.randn() * 0.005, 0.05))
cur_x_next[0] = cur_x_next[0] + 0.1 * noise_vx
cur_x_next[1] = cur_x_next[1] + 0.1 * noise_vy
cur_x_next[2] = cur_x_next[2] + 0.1 * noise_wz
return cur_x_next, x_next
def _EstimateABC(Controller):
LinPoints = Controller.LinPoints
N = Controller.N
n = Controller.n
d = Controller.d
x = Controller.Datax
u = Controller.Datau
PointAndTangent = Controller.map.PointAndTangent
dt = Controller.dt
Atv = []
Btv = []
Ctv = []
for i in range(0, N + 1): #从0到N计数，共N+1个点
MaxNumPoint = 500 # Need to reason on how these points are selected
x0 = LinPoints[i, :]
Ai = np.zeros((n, n))
Bi = np.zeros((n, d))
Ci = np.zeros((n, 1))
# =========================
# ====== Identify vx ======
h = 2
stateFeatures = [0, 1, 2]
inputFeatures = [1] #输入U(1)表示输出的加速度
lamb = 0.0
yIndex = 0
scaling = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
Ai[yIndex,
stateFeatures], Bi[yIndex,
inputFeatures], Ci[yIndex], _ = LocLinReg(
h, x, u, x0, yIndex, stateFeatures,
inputFeatures, scaling, qp, matrix, lamb,
MaxNumPoint)
# =========================
# ====== Identify vy ======
h = 2
stateFeatures = [0, 1, 2]
inputFeatures = [0] # May want to add acceleration here 输入U（0）表示输入的角度
lamb = 0.0
yIndex = 1
# scaling = np.array([[1.0, 0.0, 0.0],
# [0.0, 1.0, 0.0],
# [0.0, 0.0, 1.0]])
scaling = np.eye(
len(stateFeatures)) #len返回statefeatures向量的长度，最后生成3*3的单位矩阵
Ai[yIndex,
stateFeatures], Bi[yIndex,
inputFeatures], Ci[yIndex], _ = LocLinReg(
h, x, u, x0, yIndex, stateFeatures,
inputFeatures, scaling, qp, matrix, lamb,
MaxNumPoint)
# =========================
# ====== Identify wz ======
h = 2
stateFeatures = [0, 1, 2]
inputFeatures = [0] # May want to add acceleration here
lamb = 0.0
yIndex = 2
# scaling = np.array([[1.0, 0.0, 0.0],
# [0.0, 1.0, 0.0],
# [0.0, 0.0, 1.0]])
scaling = np.eye(len(stateFeatures))
Ai[yIndex,
stateFeatures], Bi[yIndex,
inputFeatures], Ci[yIndex], _ = LocLinReg(
h, x, u, x0, yIndex, stateFeatures,
inputFeatures, scaling, qp, matrix, lamb,
MaxNumPoint)
# ===========================
# ===== Linearization =======
vx = x0[0]
vy = x0[1]
wz = x0[2]
epsi = x0[3]
s = x0[4]
ey = x0[5]
if s < 0:
print "s is negative, here the state: \n", LinPoints
cur = Curvature(s, PointAndTangent) #S处的曲率
den = 1 - cur * ey
# ===========================
# ===== Linearize epsi ======
# epsi_{k+1} = epsi + dt * ( wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur )
#depsi_vx、_vy、_wz 分别代表上式对vx、vy、wz的偏导数
depsi_vx = -dt * np.cos(epsi) / den * cur
depsi_vy = dt * np.sin(epsi) / den * cur
depsi_wz = dt
depsi_epsi = 1 - dt * (-vx * np.sin(epsi) -
vy * np.cos(epsi)) / den * cur
depsi_s = 0 # Because cur = constant
depsi_ey = -dt * (vx * np.cos(epsi) -
vy * np.sin(epsi)) / (den**2) * cur * (-cur)
Ai[3, :] = [
depsi_vx, depsi_vy, depsi_wz, depsi_epsi, depsi_s, depsi_ey
]
Ci[3] = epsi + dt * (wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey) * cur) - np.dot(Ai[3, :], x0)
#Ci[3]=epsi{k+1}-Ai[3,:]*x0
# ===========================+ ·
# ===== Linearize s =========
# s_{k+1} = s + dt * ( (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) )
ds_vx = dt * (np.cos(epsi) / den)
ds_vy = -dt * (np.sin(epsi) / den)
ds_wz = 0
ds_epsi = dt * (-vx * np.sin(epsi) - vy * np.cos(epsi)) / den
ds_s = 1 # + Ts * (Vx * cos(epsi) - Vy * sin(epsi)) / (1 - ey * rho) ^ 2 * (-ey * drho);
ds_ey = -dt * (vx * np.cos(epsi) - vy * np.sin(epsi)) / (den *
2) * (-cur)
Ai[4, :] = [ds_vx, ds_vy, ds_wz, ds_epsi, ds_s, ds_ey]
Ci[4] = s + dt * ((vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey)) - np.dot(Ai[4, :], x0)
# ===========================
# ===== Linearize ey ========
# ey_{k+1} = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi))
dey_vx = dt * np.sin(epsi)
dey_vy = dt * np.cos(epsi)
dey_wz = 0
dey_epsi = dt * (vx * np.cos(epsi) - vy * np.sin(epsi))
dey_s = 0
dey_ey = 1
Ai[5, :] = [dey_vx, dey_vy, dey_wz, dey_epsi, dey_s, dey_ey]
Ci[5] = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi)) - np.dot(
Ai[5, :], x0)
Atv.append(Ai)
Btv.append(Bi)
Ctv.append(Ci)
return Atv, Btv, Ctv
def _DynModel(self, x, x_glob, u, np, dt, PointAndTangent):
# This function computes the system evolution. Note that the discretization is deltaT and therefore is needed that
# dt <= deltaT and ( dt / deltaT) = integer value
# Discretization Parameters
deltaT = 0.001
x_next = np.array([0., 0., 0., 0., 0., 0.], dtype=float)
cur_x_next = np.array([0., 0., 0., 0., 0., 0.], dtype=float)
# Extract the value of the states
delta = u[0]
a = u[1]
psi = x_glob[3]
X = x_glob[4]
Y = x_glob[5]
vx = x[0]
vy = x[1]
wz = x[2]
epsi = x[3]
s = x[4]
ey = x[5]
# Initialize counter
i = 0
while ((i + 1) * deltaT <= dt):
# Compute tire slip angle
alpha_f = delta - math.atan2(vy + self.lf * wz, vx)
alpha_r = -math.atan2(vy - self.lr * wz, vx)
# Compute lateral force at front and rear tire
Fyf = self.Df * math.sin(self.Cf * math.atan(self.Bf * alpha_f))
Fyr = self.Dr * math.sin(self.Cr * math.atan(self.Br * alpha_r))
# Propagate the dynamics of deltaT
x_next[0] = vx + deltaT * (a - 1 / self.m * Fyf * math.sin(delta) +
wz * vy)
x_next[1] = vy + deltaT * (1 / self.m *
(Fyf * math.cos(delta) + Fyr) - wz * vx)
x_next[2] = wz + deltaT * (
1 / self.Iz *
(self.lf * Fyf * math.cos(delta) - self.lr * Fyr))
x_next[3] = psi + deltaT * (wz)
x_next[4] = X + deltaT * (
(vx * math.cos(psi) - vy * math.sin(psi)))
x_next[5] = Y + deltaT * (vx * math.sin(psi) + vy * math.cos(psi))
cur = Curvature(s, PointAndTangent)
cur_x_next[0] = x_next[0]
cur_x_next[1] = x_next[1]
cur_x_next[2] = x_next[2]
cur_x_next[3] = epsi + deltaT * (
wz - (vx * math.cos(epsi) - vy * math.sin(epsi)) /
(1 - cur * ey) * cur)
cur_x_next[4] = s + deltaT * (
(vx * math.cos(epsi) - vy * math.sin(epsi)) / (1 - cur * ey))
cur_x_next[5] = ey + deltaT * (vx * math.sin(epsi) +
vy * math.cos(epsi))
# Update the value of the states
psi = x_next[3]
X = x_next[4]
Y = x_next[5]
vx = cur_x_next[0]
vy = cur_x_next[1]
wz = cur_x_next[2]
epsi = cur_x_next[3]
s = cur_x_next[4]
ey = cur_x_next[5]
# if (s < 0):
# print("Start Point: ", x, " Input: ", u)
# print("x_next: ", x_next)
# Increment counter
i = i + 1
# Noises
noise_vx = np.max([-0.05, np.min([np.random.randn() * 0.01, 0.05])])
noise_vy = np.max([-0.1, np.min([np.random.randn() * 0.01, 0.1])])
noise_wz = np.max([-0.05, np.min([np.random.randn() * 0.005, 0.05])])
cur_x_next[0] = cur_x_next[0] + 0.1 * noise_vx
cur_x_next[1] = cur_x_next[1] + 0.1 * noise_vy
cur_x_next[2] = cur_x_next[2] + 0.1 * noise_wz
return cur_x_next, x_next
def RegressionAndLinearization(LinPoints, LinInput, usedIt, SS, uSS,
LapCounter, MaxNumPoint, qp, n, d, matrix,
PointAndTangent, dt, i):
#i是滚动时域N中遍历的第i个点
x0 = LinPoints[i, :]
Ai = np.zeros((n, n))
Bi = np.zeros((n, d))
Ci = np.zeros((n, 1))
# Compute Index to use
h = 5
lamb = 0.0
stateFeatures = [0, 1, 2]
ConsiderInput = 1
if ConsiderInput == 1:
scaling = np.array([[0.1, 0.0, 0.0, 0.0,
0.0], [0.0, 1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0,
0.0], [0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.0]])
xLin = np.hstack((LinPoints[i, stateFeatures], LinInput[i, :]))
else:
scaling = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
xLin = LinPoints[i, stateFeatures]
indexSelected = []
K = []
for ii in usedIt:
indexSelected_i, K_i = ComputeIndex(h, SS, uSS, LapCounter, ii, xLin,
stateFeatures, scaling,
MaxNumPoint, ConsiderInput)
indexSelected.append(indexSelected_i)
K.append(K_i)
# =========================
# ====== Identify vx ======
inputFeatures = [1]
Q_vx, M_vx = Compute_Q_M(SS, uSS, indexSelected, stateFeatures,
inputFeatures, usedIt, np, matrix, lamb, K)
yIndex = 0
b = Compute_b(SS, yIndex, usedIt, matrix, M_vx, indexSelected, K, np)
Ai[yIndex,
stateFeatures], Bi[yIndex, inputFeatures], Ci[yIndex] = LMPC_LocLinReg(
Q_vx, b, stateFeatures, inputFeatures, qp)
# =======================================
# ====== Identify Lateral Dynamics ======
inputFeatures = [0]
Q_lat, M_lat = Compute_Q_M(SS, uSS, indexSelected, stateFeatures,
inputFeatures, usedIt, np, matrix, lamb, K)
yIndex = 1 # vy
b = Compute_b(SS, yIndex, usedIt, matrix, M_lat, indexSelected, K, np)
Ai[yIndex,
stateFeatures], Bi[yIndex, inputFeatures], Ci[yIndex] = LMPC_LocLinReg(
Q_lat, b, stateFeatures, inputFeatures, qp)
yIndex = 2 # wz
b = Compute_b(SS, yIndex, usedIt, matrix, M_lat, indexSelected, K, np)
Ai[yIndex,
stateFeatures], Bi[yIndex, inputFeatures], Ci[yIndex] = LMPC_LocLinReg(
Q_lat, b, stateFeatures, inputFeatures, qp)
#在速度变量x0[0,1,2]的情况下，位置变量x0[3,4,5]的变化
# ===========================
# ===== Linearization =======
vx = x0[0]
vy = x0[1]
wz = x0[2]
epsi = x0[3]
s = x0[4]
ey = x0[5]
if s < 0:
print "s is negative, here the state: \n", LinPoints
startTimer = datetime.datetime.now() # Start timer for LMPC iteration
cur = Curvature(s, PointAndTangent)
cur = Curvature(s, PointAndTangent)
den = 1 - cur * ey
# ===========================
# ===== Linearize epsi ======
# epsi_{k+1} = epsi + dt * ( wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur )
depsi_vx = -dt * np.cos(epsi) / den * cur
depsi_vy = dt * np.sin(epsi) / den * cur
depsi_wz = dt
depsi_epsi = 1 - dt * (-vx * np.sin(epsi) - vy * np.cos(epsi)) / den * cur
depsi_s = 0 # Because cur = constant
depsi_ey = dt * (vx * np.cos(epsi) -
vy * np.sin(epsi)) / (den**2) * cur * (-cur)
Ai[3, :] = [depsi_vx, depsi_vy, depsi_wz, depsi_epsi, depsi_s, depsi_ey]
Ci[3] = epsi + dt * (wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey) * cur) - np.dot(Ai[3, :], x0)
# ===========================
# ===== Linearize s =========
# s_{k+1} = s + dt * ( (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) )
ds_vx = dt * (np.cos(epsi) / den)
ds_vy = -dt * (np.sin(epsi) / den)
ds_wz = 0
ds_epsi = dt * (-vx * np.sin(epsi) - vy * np.cos(epsi)) / den
ds_s = 1 # + Ts * (Vx * cos(epsi) - Vy * sin(epsi)) / (1 - ey * rho) ^ 2 * (-ey * drho);
ds_ey = -dt * (vx * np.cos(epsi) - vy * np.sin(epsi)) / (den * 2) * (-cur)
Ai[4, :] = [ds_vx, ds_vy, ds_wz, ds_epsi, ds_s, ds_ey]
Ci[4] = s + dt * ((vx * np.cos(epsi) - vy * np.sin(epsi)) /
(1 - cur * ey)) - np.dot(Ai[4, :], x0)
# ===========================
# ===== Linearize ey ========
# ey_{k+1} = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi))
dey_vx = dt * np.sin(epsi)
dey_vy = dt * np.cos(epsi)
dey_wz = 0
dey_epsi = dt * (vx * np.cos(epsi) - vy * np.sin(epsi))
dey_s = 0
dey_ey = 1
Ai[5, :] = [dey_vx, dey_vy, dey_wz, dey_epsi, dey_s, dey_ey]
Ci[5] = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi)) - np.dot(
Ai[5, :], x0)
endTimer = datetime.datetime.now()
deltaTimer_tv = endTimer - startTimer
return Ai, Bi, Ci, indexSelected
def solve(self, x0, uOld=np.zeros([0, 0])):
"""Computes control action
Arguments:
x0: current state position
"""
n = self.n
d = self.d
SS = self.SS
# SS_glob=self.SS_glob
bcur = self.bcur
arclen = self.arclen
arclens = self.arclens
uSS = self.uSS
TimeSS = self.TimeSS
LapCounter = self.LapCounter
OldInput = self.OldInput
N = self.N
dt = self.dt
it = self.it
numSS_Points = self.numSS_Points
numSR_Points = self.numSR_points
LinPoints = self.LinPoints
LinInput = self.LinInput
map = self.map
PointAndTangent = map.PointAndTangent
# Select Points from SS
# self.LinPoints[4, -1] = self.LinPoints[4, -1] - map.TrackLength
#判断相似路径
SR_PointSelectedTot = np.empty((n, 0))
Succ_BSS_PointSelectedTot = np.empty((n, 0))
Succ_BuSS_PointSelectedTot = np.empty((d, 0))
Qfun_SelectedTot = np.empty((0))
reallyroadarclen = np.empty(numSR_Points)
maxsspoint = max(TimeSS)
Mindiff = 100.0 #先整个大的
jdelerror = [] #由于删除长度不够的列，导致的jdel与实际MINJJ的误差
MinJarray = range(0, 42)
for ii in range(0, maxsspoint):
#剔除长度不够的列
jdell = 2
for jdel in range(2, 42): #jdel是真正的圈次数
if jdell >= bcur.shape[1]:
break
while jdel in jdelerror:
jdel += 1 #有可能jdel+1也在jdelerror里面，因此需要while
if ii + numSR_Points > TimeSS[jdel] - 1 or ii == LapCounter[
jdel]: #注意最后一个点没办法计算曲率，因此实际的点个数应少1
bcur = np.delete(bcur, jdell, axis=1)
arclens = np.delete(arclens, jdell, axis=1)
jdell = jdell - 1
if jdel not in jdelerror:
jdelerror.append(jdel)
MinJarray.remove(jdel)
jdell = jdell + 1
#记忆中的numSR_Points个点的曲率
curb = bcur[ii:ii + numSR_Points, 2:]
#当前路径中心线的numSR_Points个点的对应s值及曲率
nowarclen = arclens[ii:ii + numSR_Points, 2:] #s值
if curb.shape[1] == 0: #如果曲率是空集，则跳出循环
continue
nowxs = self.zVector[4] #修正s值
# diffarclens=nowxs-nowarclen[9,:]
# nowarclens=nowarclen+diffarclens
#如果在最开始，zvector[4]很小，使得nowarclens中出现负数，则片段路径的需要以第一个点为基准点
# if np.any(nowarclens<0):
if nowarclen[-1, -1] == 0: #最后一行最后一列会出现长度是0的情况
break
diffarclens = nowxs - nowarclen[0, :]
nowarclens = nowarclen + diffarclens
#利用弧长求曲率
nowcur = np.empty((nowarclen.shape[0], nowarclen.shape[1]))
for i in range(0, nowarclen.shape[0]):
for j in range(0, nowarclen.shape[1]):
if nowarclens[i, j] < 0:
# print nowarclens[i,j]+map.TrackLength
nowcur[i, j] = Curvature(
nowarclens[i, j] + map.TrackLength,
PointAndTangent)
else:
nowcur[i, j] = Curvature(nowarclens[i, j],
PointAndTangent)
#现实路径与过往轨迹的曲率差的范数
diffcur = nowcur - curb
diffcurnorm = la.norm(diffcur, 2, axis=0)
MinJ = np.argmin(diffcurnorm)
subMinarclens = nowarclens[:, MinJ] #待定的最小路程
subnowcur = nowcur[:, MinJ]
#将从被删除后的矩阵中选取的MinJ变成完整矩阵中的MinJ
if Mindiff > diffcurnorm[MinJ]:
MinJJ = MinJ
MinII = ii
Mindiff = diffcurnorm[MinJ]
Minarclens = subMinarclens
MinJJ = MinJarray[MinJJ + 2] #原矩阵的索引值
minnowcur = subnowcur
# 虚拟路径跟踪轨迹
similarroadpoint = SS[MinII:MinII + numSR_Points, :, MinJJ]
self.TSR_PointSelectedTot = similarroadpoint
# 现实道路中心线
reallyroadarclen = Minarclens.reshape((numSR_Points, 1))
self.reallyroadPoint = hstack(
(hstack((np.zeros(
(reallyroadarclen.shape[0], 4)), reallyroadarclen)),
np.zeros((reallyroadarclen.shape[0], 1)))) # 实际道路为只有s量，其余各量为0的点
#对虚拟路径跟踪轨迹进行插值，先求样条参数
similarroadpoint_s = SS[MinII:MinII + numSR_Points, 4,
MinJJ] #以s值为参考进行插值
similarroadpoint_Vx = SS[MinII:MinII + numSR_Points, 0, MinJJ]
similarroadpoint_Vy = SS[MinII:MinII + numSR_Points, 1, MinJJ]
similarroadpoint_psi = SS[MinII:MinII + numSR_Points, 3,
MinJJ] #先找到虚拟路径中心线的各参数
similarroadpoint_wz = SS[MinII:MinII + numSR_Points, 2, MinJJ]
similarroadpoint_arclens = Minarclens.reshape(
(1, numSR_Points)) #arclens其实就是当前坐标系下的s值
similarroadpoint_ey = SS[MinII:MinII + numSR_Points, 5, MinJJ]
tck_Vx = interpolate.interp1d(similarroadpoint_s, similarroadpoint_Vx)
tck_Vy = interpolate.interp1d(similarroadpoint_s, similarroadpoint_Vy)
tck_wz = interpolate.interp1d(similarroadpoint_s, similarroadpoint_wz)
tck_psi = interpolate.interp1d(similarroadpoint_s,
similarroadpoint_psi,
'cubic') #虚拟路径跟踪轨迹的样条插值参数
tck_arclens = interpolate.interp1d(similarroadpoint_s,
similarroadpoint_arclens, 3)
tck_y = interpolate.interp1d(similarroadpoint_s, similarroadpoint_ey,
3)
#开始经验迁移，找到最优轨迹
Sorigin = SS[MinII, 4, MinJJ] #相似道路中心线起点的s值
Send = SS[MinII + numSR_Points - 1, 4, MinJJ] #相似道路中心线终点的s值
MinNumPoints = 10000 #先整个大的初始值
MinNumerror = 10000
#上一段最优轨迹的最后一个点
if self.transSSTime[self.TransTime - 1] == 0:
BestzVector = self.zVector
BestzVectors = self.zVector
else:
BestzVector = self.transSS[self.transSSTime[self.TransTime - 1] -
1, :]
BestzVectors = self.transSS[self.transSSTime[self.TransTime - 1] -
11:self.transSSTime[self.TransTime -
1], :]
for J in range(SS.shape[2] - 1, 1, -1): #为了快点找到最优轨迹，倒着遍历
totalpointS = SS[:, 4, J] #所有点的s值
TrajectoriesIndex = np.argwhere(
(totalpointS <= Send) &
(totalpointS >= Sorigin)) #s值在相似道路中心线范围内的轨迹点索引值
selectedTrajecties = SS[TrajectoriesIndex[:, 0], :, J]
difflastpoint = selectedTrajecties - BestzVector
normselectedtrajecties = la.norm(difflastpoint, 1, axis=1)
MinNormselectedtrajecties = np.argmin(normselectedtrajecties)
if MinNormselectedtrajecties - 10 < 0:
similarLastpoint = selectedTrajecties[
MinNormselectedtrajecties, :]
else:
similarLastpoint = selectedTrajecties[
MinNormselectedtrajecties - 10:MinNormselectedtrajecties +
1, :]
# if selectedTrajecties.shape[0]>numSR_Points: #待选轨迹不能比沿着路径中心线跑还要慢
# continue
# print la.norm(self.zVector[0:3]-selectedTrajecties[0,0:3],1)
if selectedTrajecties.shape[0] + la.norm(
similarLastpoint - BestzVectors,
1).sum() * 10 > MinNumerror: #待选轨迹比最优轨迹还慢的也扔了吧,同时保证速度变化慢
continue
if selectedTrajecties.shape[
0] < numSS_Points / 2 + 1: #待选轨迹如果太快，会导致点数过少无法建立ss集，加1是为了划分当前SS集与下一步的SS集
continue
#X1表示待迁移的轨迹点，X2表示虚拟道路中心线上的点
TransX1 = selectedTrajecties[:, :]
TransX2_s = TransX1[:, 4]
#找到X2
# TransX2_Vx = tck_Vx(TransX2_s)
# TransX2_Vy = tck_Vy(TransX2_s)
# TransX2_WZ = tck_wz(TransX2_s)
TransX2_epsi = tck_psi(TransX2_s)
TransX2_arclens = tck_arclens(TransX2_s)
TransX2_ey = tck_y(TransX2_s)
#迁移X1
TransX1_ey = (TransX1[:, 5] - TransX2_ey) * np.cos(TransX2_epsi)
if max(TransX1_ey) > map.halfWidth: #删除大于路宽约束的点
continue
TransX1_s = TransX2_arclens + (TransX1[:, 5] -
TransX2_ey) * np.sin(
TransX2_epsi) #先是按原来轨迹计算的s值你
TransX1_epsi = TransX1[:, 3] - TransX2_epsi
# X1处输入变量
TransU1 = uSS[TrajectoriesIndex[:, 0], :, J]
# TransX1_WZ=TransX1[:,2]
# TransX1_Vy=TransX1[:,1]*np.cos(TransX2_epsi)-TransX1[:, 0] * np.sin(TransX2_epsi)
# TransX1_Vx = TransX1[:, 0] * np.cos(TransX2_epsi)+TransX1[:, 1] * np.sin(TransX2_epsi)
MinNumPoints = selectedTrajecties.shape[0]
MinNumerror = MinNumPoints + la.norm(
similarLastpoint - BestzVectors, 1).sum() * 10
BestSS_PointSelect = np.empty((n, MinNumPoints))
BestSS_PointSelect[0:3, :] = TransX1[:, 0:3].T
BestSS_PointSelect[3, :] = TransX1_epsi
BestSS_PointSelect[4, :] = TransX1_s
BestSS_PointSelect[5, :] = TransX1_ey
#检验插值
chazhi_PointSelect = np.empty((n, MinNumPoints))
chazhi_PointSelect[3, :] = TransX2_epsi
chazhi_PointSelect[4, :] = TransX2_arclens
chazhi_PointSelect[5, :] = TransX2_ey
BestbeforePoint = TransX1 #迁移前的轨迹
TransTime = self.TransTime #仿真的第几段路段，在Virtualsimulater程序中每次加1
transSSTime = self.transSSTime[
TransTime - 1] #该路段内最优轨迹总共有几个点，self.transSSTime是统计所有transSSTime的集合
# if transSSTime+MinNumPoints >= self.transSS.shape[0] :
# self.transSS[transSSTime:, :] = BestSS_PointSelect[:, :self.transSS.shape[0]-transSSTime].T #防止超过最大仿真时间
# self.BestbeforePoint[transSSTime: , :] = BestbeforePoint[:, :self.transSS.shape[0]-transSSTime]
# self.chazhi_PointSelect[transSSTime:, :] = chazhi_PointSelect[:, :self.transSS.shape[0]-transSSTime].T
# self.overflag=1 #超出最大仿真时间了
self.transSS[transSSTime:transSSTime +
MinNumPoints, :] = BestSS_PointSelect.T #将迁移后的最优轨迹加入新的ss集
self.transuSS[transSSTime:transSSTime + MinNumPoints, :] = TransU1
self.BestbeforePoint[transSSTime:transSSTime +
MinNumPoints, :] = BestbeforePoint
self.chazhi_PointSelect[transSSTime:transSSTime +
MinNumPoints, :] = chazhi_PointSelect.T
self.transSSTime[
TransTime] = transSSTime + MinNumPoints #每一段结束后总共共多少个点(统计所有圈数)
self.tSSparaTime[self.TranslapTime, self.overlap] = self.tSSparaTime[
self.TranslapTime - 1,
self.overlap] + MinNumPoints #每一段结束后，当前圈数总共有多少个点
self.TranslapTime = self.TranslapTime + 1 #TranslapTime表示这一圈的第几小段轨迹
self.zVector = self.reallyroadPoint[10, :]
#快到达终点处时处理
if (self.zVector[4] > map.TrackLength): #为了制造圈与圈之间的起始点差，到终点时让经验轨迹倒退几步
# houtui=int(np.floor(self.reallyroadPoint.shape[0]/3)) #到终点时后退1/3
# self.zVector = self.reallyroadPoint[-houtui, :]- map.TrackLength
self.zVector[4] = self.zVector[4] - map.TrackLength
self.tSSlapsTime[self.overlap] = self.transSSTime[
TransTime] # 第overlap圈的时候，总共有多少个点（所有圈数）
self.TranslapTime = 0
# self.passway=1 #防止后退完回到终点再次被后退
print "迁移第%d圈" % (self.overlap)
self.overlap = self.overlap + 1