def PID(self, gains, eta):
'''
# 航向角偏差横摆角速度双闭环PID
'''
Kp = gains['Kp']
Ki = gains['Ki']
Kd = gains['Kd']
# 求解偏差
K_eta2r = 0.5
yawRateActual = recv['w_z']
yawRateDesire = eta * K_eta2r # 依据神龙园右上弯 < MAX_STEER_RATE设置
yawRateDesire = commonFcn.saturation(MIN_STEER_RATE, MAX_STEER_RATE,
yawRateDesire)
yawRateErr = yawRateDesire - yawRateActual
# print 'yawRateErr', yawRateErr
# PID控制
print 'eta', eta
if abs(eta) > 0.2:
# Ki = 0.
self.steerPID.integral = 0.
print 'integral', self.steerPID.integral * MAX_STEER_ANGLE
steerAngle = self.steerPID.update(eta, Kp, Ki, Kd)
steerAngle = steerAngle * MAX_STEER_ANGLE # 0-1
print 'PID', steerAngle
# print 'self.steerPID.integral', self.steerPID.integral
return steerAngle
def steer_angle_gen(self, gains):
algorithm = 'PID' # 'PID', 'purePursuit'
global etaLast
global GLBlogEta
'''
可调整参数pureGain， distPreview
但distPreview有些特殊，在setCalibParam.py中的直接影响CSD，对A60无效。
A60目前预瞄距离是采用延迟时间*速度的方式计算的，欲调整，需调整内部计算参数
'''
# init local variable
etaOriginal, Lfw = self.estimateErr(gains)
gainPlus = gains['pure'] # 尝试值，需进一步确认
directionDrive = recv['curDirDrive']
# # 修正偏差信息
# if not vehicle_model.name == 'A60':
# if directionDrive == FORWARD:
# plusAngle = (gainPlus * self.steerAngleLast /25.)*np.pi/180.
# eta = etaOriginal +plusAngle
# else:
# eta = etaOriginal
# else:
# eta = etaOriginal
eta = etaOriginal
self.eta = eta
if algorithm == 'PID':
steeringAngleGen = self.PID(gains, eta)
# 纯跟踪控制
elif algorithm == 'purePursuit':
# 计算适用于纯跟踪的航向偏差
# fuzzy PID
delta_eta = eta - etaLast
etaSat = commonFcn.saturation(-0.4, 0.4, eta)
delta_etaSat = commonFcn.saturation(-0.035, 0.035, delta_eta)
startTime = time.time()
gainFuzzy = fuzzyPID.fuzzyPID(etaSat, delta_etaSat)
endTime = time.time()
timecost = endTime - startTime
# steeringAngleGen = self.purePursuit(eta,Lfw,gains)*gainFuzzy
steeringAngleGen = self.purePursuit(eta, Lfw, gains)
# 根据纯滞后时间，进行更新, A60: 0.2s truck: 0.3s
self.etaCount += 1
if self.etaCount >= 4:
self.etaCount = 0
etaLast = eta
logVar = np.array([eta, delta_eta])
GLBlogEta.append(logVar)
return steeringAngleGen
def update(self):
directionDrive = recv['curDirDrive']
flagIsShifting = recv['flagIsShifting']
modeDrive = recv['modeDrive']
shift = recv['shift']
#相关代码用于检测是否起步完成，从而清除起步阶段的积分
if np.abs(recv['x']) > 0 and np.abs(
recv['y']) > 0 and self.flagPositon == 0:
self.flagPositon = 1
self.initPoint = np.asarray([recv['x'], recv['y']])
if np.abs(self.initPoint[0]) > 0:
curPoint = np.asarray([recv['x'], recv['y']])
distToInit = np.linalg.norm(curPoint - self.initPoint)
if distToInit > 1:
self.flagClearInitIntegral = 1
if distToInit < 1. and self.flagClearInitIntegral == 0:
self.integral = 0.
self.steerPID.integral = 0.
self.steerPID.filter = 0.
if directionDrive == FORWARD:
steerAngle = self.steer_angle_gen(gainsForward) * gainA60toCSD
else:
steerAngle = -self.steer_angle_gen(gainsBack) * gainA60toCSD
print 'pidangle', steerAngle
# modeDrive = NORMAL
# flagIsShifting = 0
if modeDrive == STOP or flagIsShifting == 1 or shift == shift_N:
# if not vehicle_model.name == 'A60':
steerAngle = 0. # 回正
self.integral = 0.
self.steerPID.integral = 0.
self.steerPID.filter = 0.
# 考虑回差
if not vehicle_model.name == 'A60':
sign1 = np.sign(steerAngle - self.steerAngleLast)
sign2 = np.sign(self.steerAngleLast - self.steerAngleLastOld)
if sign1 == sign2:
self.signCount += 1
else:
self.signCount = 0
if self.signCount > 8:
if sign1 > 0:
self.steerAnglePlus = 30
else:
self.steerAnglePlus = -30
steerAngle += self.steerAnglePlus
self.steerAngleLastOld = self.steerAngleLast
self.steerAngleLast = steerAngle
steerAngle = commonFcn.saturation(MIN_STEER_ANGLE, MAX_STEER_ANGLE,
steerAngle) #
steerAngleDisp = steerAngle / 360. #7560=3.5*360,
print('steerAngle', steerAngle)
# print('steerAngleDisp',steerAngleDisp)
self.steerAngle = steerAngle
def estimateEta(self, prePoint, position, gains):
'''
estimate azimuth error by method of pure pursuit
'''
preXLoc, preYLoc = commonFcn.global_2_local_point(position, prePoint)
# print 'position,prepoint', prePoint, preXLoc, preYLoc
# 将控制点提前，eta可以尽快响应实际变化情况，避免超调
# 分普通情形和终点
distAnchor = gains['pure'] # [m]
curPosition = np.array([recv['x'], recv['y']])
distPre2Cur = np.linalg.norm(prePoint - curPosition)
# if recv['distYClose'] >1.:
# distAnchor = 0.
# print 'distAnchor', distAnchor
if distPre2Cur > distAnchor:
# eta = -np.arctan(preYLoc/(preXLoc-distAnchor)) # '-'确保右转，eta为正
pointAnchor = np.array([distAnchor, 0.])
pointAnchorGlobal = commonFcn.local_2_global_point(
position, pointAnchor)
curPosition = pointAnchorGlobal
# curPosition = [42.,-7.]
# prePoint = [36.9,-4.4]
# curPosition = [2.,0.]
# prePoint = [-1,0]
angle = commonFcn.xAxis2VectCounter(curPosition, prePoint) #正北顺时针
print 'angle,azimuth', angle, recv['azimuth'], curPosition
eta = (recv['azimuth'] - angle)
if recv['azimuth'] < 90. and angle > 350.:
eta = recv['azimuth'] + 360. - angle
if angle < 90. and recv['azimuth'] > 350.:
eta = angle + 360. - recv['azimuth']
eta = eta * np.pi / 180.
# print 'angle111',angle
else:
# 应该取消锚点，同时也间接意味着到终点，所以直接转向回位
eta = 0.
# # 考虑大于90°情况，也就是x<0
# if preXLoc < 0 :
# if preYLoc < 0:
# # 位于YX坐标系第四象限
# eta = np.pi - np.abs(eta)
# else:
# eta = -(np.pi - np.abs(eta))
# print 'xxx is negative'
# print 'eta', eta
etaSat = commonFcn.saturation(-np.pi / 2., np.pi / 2., eta)
print 'etaSat', etaSat
return etaSat
a.displayCount()
print 'dd', a.empCount
sensor = {'d':10,'ddddddddd':45,'eeeeeeeeeeeeeeeeee':25, \
'df':78,'ww':89}
print 'sensor', sensor
a = 1
func(4)
print sensor
input = 1
b = Person('d')
b.ddd()
print 'bbbb', b.aw
recv = {'x':0., 'y':0., 'z':0., 'roll':0., 'pitch':0., 'azimuth':0., \
'polyCoeffiCenter':np.zeros([3]), }
print 'recv', recv
d = commonFcn.saturation(10, 20, 35)
print 'd', d
x = 1
polyCoeffiCenter = np.array([1, 2, 3])
# 一阶导 ref:http://blog.csdn.net/blog_empire/article/details/39298557
polyder1CoeffiCenter = np.polyder(polyCoeffiCenter) #导函数多项式系数
polyder1Center = np.polyval(polyder1CoeffiCenter, x)
# 二阶导
polyder2CoeffiCenter = np.polyder(polyder1CoeffiCenter)
polyder2Center = np.polyval(polyder2CoeffiCenter, x)
#曲率数组,如何指定方向?
curvatureCenter = np.abs(polyder2Center) / (1 + polyder1Center**2)**(3. /
2.)
curvature = np.abs(2 * polyCoeffiCenter[0]) / (
(1 + (2 * polyCoeffiCenter[0] * x + polyCoeffiCenter[1])**2)**1.5)
def estimateErr(self, gains):
'''
estimate tracking error for control
Coordinate:
global
Return:
eta: tracking error [rad] (-np.pi/2., np.pi/2.)
'''
global flagNewDateGLB
global prePointLastGLB
T_lag = vehicle_model.T_lag
speed = recv['speed']
directionDrive = recv['curDirDrive']
position = recv
path = recv
if recv['xCtrPath'].size <= 10: # note: np.zeros([]).size = 1
# system is initializing or no available xCtrPath
print 'system is initializing'
eta = 0. # default value
Lfw = 5.5
else:
# generate path when receive new wayPoint in global coordinate
if flagNewDateGLB == True:
flagNewDateGLB = False
self.indLastClosest = 0
self.wayPoint = np.column_stack(
(recv['xCtrPath'], recv['yCtrPath']))
# set preview distance
if not vehicle_model.name == 'A60':
distPreview = self.changeDistPreview(position, prePointLastGLB,
path, gains)
distYClose = recv['distYClose']
distPreVect = np.array([distYClose, distPreview])
distPreview = np.linalg.norm(distPreVect)
else:
distPreview = T_lag * speed / 3.6
# distPreview = 10.
# 5.56 --> 8; 13.89 --> 20
distYClose = recv['distYClose']
distPreview = 5.
distPreVect = np.array([distYClose, distPreview])
distPreview = np.linalg.norm(distPreVect)
distPreview = commonFcn.saturation(5.56 * 1., 13.89,
distPreview)
# print 'distPreview', distPreview
if directionDrive == BACK:
distPreview = 1. * distPreview
# find closest and preview point
indClosest, indPre = commonFcn.findClosestPointSteer( \
position, self.wayPoint, self.indLastClosest, distPreview)
self.indLastClosest = indClosest
preX = self.wayPoint[indPre, 0]
preY = self.wayPoint[indPre, 1]
prePoint = np.array(
[preX, preY]) # why not to assign with wayPoint, for robust
prePointLastGLB = prePoint
eta = self.estimateEta(prePoint, position, gains)
# calculate Lfw
# Lfw = 5.5 is csd value at 12.29 test
curPoint = np.array([position['x'], position['y']])
Lfw = np.linalg.norm(prePoint - curPoint)
# calculate curvature at preview point
# self.curvature = self.calcCurvature(prePoint,path) # 0312 temp disable
return eta, Lfw