根据LQR轨迹跟踪算法Python/Matlab算法实现2的代码，我们转化成Python，后续上车使用。代码仅开源到这，可以进行仿真，函数都可以直接使用。工程代码就不开源了。
from numpy import *from math import *import matplotlib.pyplot as pltimport scipy.linalg as laimport timeKp = 1dt = 0.1L = 2.9Q = eye(4)Q[0,0] = 19.7Q[1,1] =0.01Q[2,2] = 18.3Q[3,3] = 7.3R = 1max_steer =60 * pi/180#in radtarget_v =10.0 / 3.6cx = linspace(0,200,2000)cy = zeros(len(cx))pd= zeros(len(cx))pdd = zeros(len(cx))ck = zeros(len(cx))cyaw = zeros(len(cx))for i in range(len(cx)):cy[i] = -sin(cx[i]/10) * cx[i]/8for i in range (len(cx)-1):pd[i] = (cy[i+1]-cy[i])/(cx[i+1]-cx[i])for i in range (len(cx)-1):pdd[i] = (cy[i+1]-2*cy[i] + cy[i-1])/(0.5* (cx[i+1]- cx[i-1]))**2for i in range(len(cx)-1):ck[i] = pdd[i]/((1+pd[i]**2)**1.5)for i in range(len(pd)):cyaw[i] = atan(pd[i])pe = 0pth_e = 0i = 1x = 0y = -0.1yaw = 0v = 0ind =0class State:def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):self.x = xself.y = yself.yaw = yawself.v = vdef update(state, a, delta):if delta >= max_steer:delta = max_steerif delta <= - max_steer:delta = - max_steerstate.x = state.x + state.v * cos(state.yaw) * dtstate.y = state.y + state.v * sin(state.yaw) * dtstate.yaw = state.yaw + state.v / L * tan(delta) * dtstate.v = state.v + a * dtreturn statedef PIDControl(target, current):a = Kp * (target - current)return a## def pi_2_pi(angle): # the unit of angle is in rad;#while (angle > pi):# angle = angle - 2.0 * pi##while (angle < -pi):# angle = angle + 2.0 * pi##return angledef solve_DARE(A, B, Q, R):"""solve a discrete time_Algebraic Riccati equation (DARE)"""X = Qmaxiter = 500eps = 0.01for i in range(maxiter):Xn = A.T * X * A - A.T * X * B * la.pinv(R + B.T * X * B) * B.T * X * A + Qif (abs(Xn - X)).max() < eps:X = XnbreakX = Xnreturn Xndef dlqr(A, B, Q, R):"""Solve the discrete time lqr controller.x[k+1] = A x[k] + B u[k]cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]# ref Bertsekas, p.151"""# first, try to solve the ricatti equationX = solve_DARE(A, B, Q, R)# compute the LQR gainK = la.pinv(B.T * X * B + R) * (B.T * X * A)return Kdef calc_nearest_index(state, cx, cy):dx = [state.x - icx for icx in cx]dy = [state.y - icy for icy in cy]d = [abs(sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]error = min(d)ind = d.index(error)dy = cy[ind] - state.yif dy > 0:error = -errorreturn ind, errordef lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e):ind, e = calc_nearest_index(state, cx, cy)k = ck[ind]v = state.vth_e = (state.yaw - cyaw[ind])A = mat(zeros((4, 4)))A[0, 0] = 1.0A[0, 1] = dtA[1, 2] = vA[2, 2] = 1.0A[2, 3] = dt# print(A)B = mat(zeros((4, 1)))B[3, 0] = v / LK = dlqr(A, B, Q, R)print('K is', K)x =mat(zeros((4, 1)))x[0, 0] = ex[1, 0] = (e - pe) / dtx[2, 0] = th_ex[3, 0] = (th_e - pth_e) / dtff = atan(L * k)fb = (-K * x)print(ff,fb)delta = 1*ff + 1 * fbprint(delta)return delta, ind, e, th_estate = State(x=0.0, y= -0.5, yaw=0.0, v=0.0)x = state.xy = state.yyaw = state.yawv = state.vi = 0x_pos = zeros(len(cx))y_pos = zeros(len(cx))while ind < len(cx):delta,ind, e, th_e = lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e)pth_e = th_epe = eprint('lateral error is ',e)v = state.vprint("v is",v)#print('Index is ', ind)if abs(e) > 4:print('too far from reference!\n')breaka = PIDControl(target_v, v)state = update(state, a, delta)x = state.xy = state.yx_pos[i] = xy_pos[i] = yi = i + 1plt.plot(cx, cy,"-b")for i in range(len(x_pos)):plt.plot(x_pos[i],y_pos[i],".r",markersize = 1)plt.grid(True)plt.axis("equal")plt.xlabel("x[m]")plt.ylabel("y[m]")plt.show()print(cyaw[0:20])
效果如图：
