제어/오픈소스 분석

[PythonRobotics 분석] LQR_steer_control 분석(Infitie Horzion)

Upright_wing 2025. 3. 27. 20:55

main 코드

def main():
    print("LQR steering control tracking start!!")
    ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
    ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=0.1)
    target_speed = 10.0 / 3.6  # simulation parameter km/h -> m/s

    sp = calc_speed_profile(cx, cy, cyaw, target_speed)

    t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)

여기서 cubic_spline_planner는 점을 ds길이에 맞게, interpolate한 점들을 연산해준다.

그래서 나오는 cx, cy, cyaw, ck, s는 생겨난 경로의 특정 지점들의 집합이라고 생각하면된다.

(k : 곡률)

interpolate된 경로

Speed Profile생성

def calc_speed_profile(cx, cy, cyaw, target_speed):
    speed_profile = [target_speed] * len(cx)

    direction = 1.0

    # Set stop point
    for i in range(len(cx) - 1):
        dyaw = abs(cyaw[i + 1] - cyaw[i])
        switch = math.pi / 4.0 <= dyaw < math.pi / 2.0

        if switch:
            direction *= -1

        if direction != 1.0:
            speed_profile[i] = - target_speed
        else:
            speed_profile[i] = target_speed

        if switch:
            speed_profile[i] = 0.0

    speed_profile[-1] = 0.0

    return speed_profile

 

$$ \Delta \theta_i = |\theta_{i+1} - \theta_i| \\ \text{switch} = \left( \frac{\pi}{4} \leq \Delta \theta_i < \frac{\pi}{2} \right) $$

\( \Delta \theta \)가 45도 이상 90도 미만일 경우, 차량의 방향을 바꾼다.

메인 loop

def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
    T = 500.0  # max simulation time
    goal_dis = 0.3
    stop_speed = 0.05

    state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0)

    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]

    e, e_th = 0.0, 0.0

    while T >= time:
        dl, target_ind, e, e_th = lqr_steering_control(
            state, cx, cy, cyaw, ck, e, e_th)

        ai = pid_control(speed_profile[target_ind], state.v)
        state = update(state, ai, dl)

        if abs(state.v) <= stop_speed:
            target_ind += 1

        time = time + dt

        # check goal
        dx = state.x - goal[0]
        dy = state.y - goal[1]
        if math.hypot(dx, dy) <= goal_dis:
            print("Goal")
            break

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)

    return t, x, y, yaw, v

LQR 제어

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

    for i in range(max_iter):
        Xn = A.T @ X @ A - A.T @ X @ B @ \
            la.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q
        if (abs(Xn - X)).max() < eps:
            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.inv(B.T @ X @ B + R) @ (B.T @ X @ A)

    eigVals, eigVecs = la.eig(A - B @ K)

    return K, X, eigVals

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

    k = ck[ind]                                        # 해당 점에서의 곡률
    v = state.v
    th_e = pi_2_pi(state.yaw - cyaw[ind])              # 방향 오차


    A = np.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 = np.zeros((4, 1))
    B[3, 0] = v / L

    K, _, _ = dlqr(A, B, Q, R)

    x = np.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 = math.atan2(L * k, 1)
    fb = pi_2_pi((-K @ x)[0, 0])

    delta = ff + fb

    return delta, ind, e, th_e

시스템 방정식

$$ x_{t+1} = A x_t + B u $$

목표는 최적의 제어입력\( u \)을 찾는 것이다. 

상태 방정식

$$ x = \begin{bmatrix} e \\ \dot{e} \\ \theta_e \\ \dot{\theta}_e \end{bmatrix} $$

  • \( e \): 횡방향 오차
  • \( \theta_e \): 방향 오차
  • \( \dot{e}, \dot{\theta}_e \): 각각의 변화율

$$ A = \begin{bmatrix} 1 & dt & 0 & 0 \\ 0 & 0 & v & 0 \\ 0 & 0 & 1 & dt \\ 0 & 0 & 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 0 \\ 0 \\ \frac{v}{L} \end{bmatrix} $$

비용함수(Cost Function)

https://upright-wing.tistory.com/22  해당 게시글 참고

$$ J = \sum_{k=0}^{\infty} \left( x[k]^T Q x[k] + u[k]^T R u[k] \right) $$

  • \( Q \): 상태\( x \) 에 대한 가중치 행렬 (상태를 0으로 수렴시키기 위한 비용)
  • \( R \): 입력 \( u \) 에 대한 가중치 행렬 (너무 큰 제어 입력을 방지)
  • 해당 cost function의 형태를 보았을떄, Infinite Horizon LQR 형태임을 알수 있다.(terminal cost function이 없다.)

해당 가중치는 모두 4×4 identity matrix 사용했다.

 

cost function이 최소화 될때의 입력이 최적입력임을 알 수 있는데, 이때, ricatti equation을 사용한다.

ricatti equation

https://upright-wing.tistory.com/25 해당 게시글 참고

$$ P = A^T P A - A^T P B (R + B^T P B)^{-1} B^T P A + Q $$

Ricatti equatuin을 반복적으로 풀어, 최소화되는 \( P \)를 찾는다.

Matrix 형태의 \( P \)가 최소화된다는 의미는 Riccati 방정식을 통해 구한 행렬 \( P \)를 사용하면 비용 함수가 \( J = \frac{1}{2} x_0^T P x_0 \)로 표현되기 때문에, 수치적으로 \( \| P_{n+1} - P_n \| < \epsilon \)일때, 최소화 된 P를 찾았다고 한다.

 

P를 구한뒤 최적이득 \( K \)를 계산한다.

$$ K = (B^T P B + R)^{-1} B^T P A $$

최적입력은 \( u = Kx \)이다.

또한 폐루프 행렬 \( A - BK \)의 고유값을 통해 안정성도 확인가능하다.(고윳값이 0보다 작으면 시스템이 수렴을 향해감)

https://upright-wing.tistory.com/28 해당 게시글 참고

feed forward 조향각 + feedback 조향각

이상치의 feed forward 조향각(차량이 이상적으로 경로를 쫓아갈때의 필요한 조향)과 feeback조향각(현재 차량이 경로에서 벗어나는지 반영하여 보정하는 조향각)을 더하여, 다음 목표 조향각을 구한다.

feed forward 조향각 :\( \delta_{\text{fb}} = -Kx \)

feedback 조향각 : \( \delta_{\text{ff}} = \tan^{-1}(L \cdot \kappa) \)

 

P 제어

ai = pid_control(speed_profile[target_ind], state.v)


def pid_control(target, current):
    a = Kp * (target - current)

    return a

앞서 구했던 목표 profile 속도를 사용하여, P제어를 진행한다.

상태 업데이트

state = update(state, ai, dl)


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 * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    state.v = state.v + a * dt

    return state

수식으로 표현하면 다음과 같다.

$$  \begin{aligned} \delta &= \text{clip}(\delta, -\delta_{\max}, +\delta_{\max}) \\ x_{t+1} &= x_t + v \cdot \cos(\theta) \cdot dt \\ y_{t+1} &= y_t + v \cdot \sin(\theta) \cdot dt \\ \theta_{t+1} &= \theta_t + \frac{v}{L} \cdot \tan(\delta) \cdot dt \\ v_{t+1} &= v_t + a \cdot dt \end{aligned} $$

  • \( \delta \): 조향각

과거 상태와 가속도, 조향각을 사용하여 상태를 업데이트한다.

 

 

Infinite Horzion LQR은 P가 수렴할때까지 반복적으로 Ricatti equation을 풀었다.

P가 수렴할때의 K를 구할수 있다.

그러나 Terminal cost가 없기에 목표 정밀도달에는 한계를 보인다.

Reference

https://github.com/AtsushiSakai/PythonRobotics?tab=readme-ov-file#linearquadratic-regulator-lqr-speed-and-steering-control

 

GitHub - AtsushiSakai/PythonRobotics: Python sample codes and textbook for robotics algorithms.

Python sample codes and textbook for robotics algorithms. - AtsushiSakai/PythonRobotics

github.com

https://blog.naver.com/deepfahren/221166929497

 

[차량동역학] 차량 저속 선회와 애커만 기하학 모델 (Vehicle Low Speed Cornering and Ackerman Geometry Model)

■ 저속 선회 차량의 선회(Cornering)를 모델링 하기 위해서 우선 저속(Low-speed)에서의 선회 특성을 살...

blog.naver.com