LQR轨迹跟踪算法Python算法实现3

根据LQR轨迹跟踪算法Python/Matlab算法实现2的代码，我们转化成Python，后续上车使用。代码仅开源到这，可以进行仿真，函数都可以直接使用。工程代码就不开源了。

from numpy import *
from math import *
import matplotlib.pyplot as plt
import scipy.linalg as la
import time
Kp = 1
dt = 0.1
L = 2.9
Q = eye(4)
Q[0,0] = 19.7
Q[1,1] =0.01
Q[2,2] = 18.3
Q[3,3] = 7.3
R = 1
max_steer =60 * pi/180#in rad
target_v =10.0 / 3.6


cx = 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]/8

for 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]))**2

for 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 = 0
pth_e = 0
i = 1
x = 0
y = -0.1
yaw = 0
v = 0
ind =0

class State:

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v

def update(state, a, delta):
    if delta >= max_steer:
        delta = max_steer
    if delta <= - max_steer:
        delta = - max_steer
    state.x = state.x + state.v * cos(state.yaw) * dt
    state.y = state.y + state.v * sin(state.yaw) * dt
    state.yaw = state.yaw + state.v / L * tan(delta) * dt
    state.v = state.v + a * dt

    return state

def 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 angle

def solve_DARE(A, B, Q, R):
    """
    solve a discrete time_Algebraic Riccati equation (DARE)
    """
    X = Q
    maxiter = 500
    eps = 0.01

    for i in range(maxiter):
        Xn = A.T * X * A - A.T * X * B * la.pinv(R + B.T * X * B) * B.T * X * A + Q
        if (abs(Xn - X)).max()  < eps:
            X = Xn
            break
        X = Xn

    return Xn

def 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 equation
    X = solve_DARE(A, B, Q, R)

    # compute the LQR gain
    K = la.pinv(B.T * X * B + R) * (B.T * X * A)

    return K

def 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.y
    if dy > 0:
        error = -error

    return ind, error

def lqr_steering_control(state, cx, cy, cyaw, ck, pe, pth_e):
    ind, e = calc_nearest_index(state, cx, cy)

    k = ck[ind]
    v = state.v
    th_e = (state.yaw - cyaw[ind])

    A = mat(zeros((4, 4)))
    A[0, 0] = 1.0
    A[0, 1] = dt
    A[1, 2] = v
    A[2, 2] = 1.0
    A[2, 3] = dt
    # print(A)

    B = mat(zeros((4, 1)))
    B[3, 0] = v / L

    K = dlqr(A, B, Q, R)
    print('K is', K)
    x =mat(zeros((4, 1)))

    x[0, 0] = e
    x[1, 0] = (e - pe) / dt
    x[2, 0] = th_e
    x[3, 0] = (th_e - pth_e) / dt

    ff = atan(L * k)
    fb = (-K * x)
    print(ff,fb)
    delta = 1*ff + 1 * fb
    print(delta)
    return delta, ind, e, th_e

state = State(x=0.0, y= -0.5, yaw=0.0, v=0.0)
x = state.x
y = state.y
yaw = state.yaw
v = state.v
i = 0
x_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_e
    pe = e
    print('lateral error is ',e)
    v = state.v
    print("v is",v)
    #print('Index is ', ind)
    if abs(e) > 4:
       print('too far from reference!\n')
       break
    a = PIDControl(target_v, v)

    state = update(state, a, delta)
    x = state.x
    y = state.y
    x_pos[i] = x
    y_pos[i] = y
    i = i + 1


plt.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])

效果如图：