lqr ilqr代码实例

lqr 代码实例

import numpy as np
import scipy.linalg

def lqr(A, B, Q, R):
    P = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R))
    K = np.matrix(scipy.linalg.inv(R) * (B.T * P))
    return K

# 系统动力学矩阵
A = np.array([[0, 1], [0, -1]])
B = np.array([[0], [1]])

# 代价函数权重矩阵
Q = np.array([[1, 0], [0, 1]])
R = np.array([[1]])

K = lqr(A, B, Q, R)
print("LQR 控制增益：\\n", K)

ilqr 代码实例

import numpy as np

def ilqr(A, B, Q, R, N=10, tol=1e-6):
    n, m = B.shape
    K_list = [np.zeros((m, n)) for _ in range(N)]

    for _ in range(1000):
        P = Q
        K_new_list = []

        for i in range(N - 1, -1, -1):
            K = -np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
            K_new_list.append(K)

            P = Q + K.T @ R @ K + (A + B @ K).T @ P @ (A + B @ K)

        diff = np.sum([np.linalg.norm(K_new_list[i] - K_list[i]) for i in range(N)])

        K_list = K_new_list[::-1]
        if diff < tol:
            break

    return K_list

# 系统动力学矩阵
A = np.array([[0, 1], [0, -1]])
B = np.array([[0], [1]])

# 代价函数权重矩阵
Q = np.array([[1, 0], [0, 1]])
R = np.array([[1]])

K_list = ilqr(A, B, Q, R)
print("iLQR 控制增益序列：")
for i, K in enumerate(K_list):
    print(f"K_{i}:\\n", K)