LQR控制2D飞行器完成特技飞行动作

LRQ 控制器是一种经典的最优控制器，从简单的倒立摆到复杂的机器人行走控制中都有应用

对于一个 2D 的双轴飞行器，可以通过控制两个轴提供的升力来调整飞行器的姿态

如何自动控制飞行器完成一个翻转动作并回到原位？

构建模拟环境

物理模型

飞行器的状态由位置、速度、角度和角速度组成，控制由两个轴提供的升力组成

同时定义飞行器的质量、惯性、长度及重力常数等参数

MASS = 0.600  # mass of the quadrotor; denoted as m
INERTIA = 0.15  # inertia of the quadrotor; denoted as I
LENGTH = 0.2  # length of the quadrotor; denoted as r
GRAVITY = 9.81  # gravity constant; denoted as g
DELTA_T = 0.01  # integration step; denoted as Δt

N_STATES = 6  # number of states; z = (x, vx, y, vy, theta, omega)
N_CONTROLS = 2  # number of controls; u = (u1, u2)

动力学模型

飞行器的动力学模型可由以下方程组描述：

使用计算机进行模拟时需要将上述连续时间的微分方程离散化：

根据方程即可基于当前时刻的状态和控制输入计算下一时刻的状态

import numpy as np

def get_next_state(z, u):
    """
    Inputs:
    z: state of the quadrotor as a numpy array (x, vx, y, vy, theta, omega)
    u: control as a numpy array (u1, u2)

    Output:
    the new state of the quadrotor as a numpy array
    """
    x, vx, y, vy, theta, omega = z

    dzdt = np.zeros([N_STATES, ])
    dzdt[0] = vx
    dzdt[1] = (-(u[0] + u[1]) * np.sin(theta)) / MASS
    dzdt[2] = vy
    dzdt[3] = ((u[0] + u[1]) * np.cos(theta) - MASS * GRAVITY) / MASS
    dzdt[4] = omega
    dzdt[5] = (LENGTH * (u[0] - u[1])) / INERTIA

    z_next = z + dzdt * DELTA_T
    return z_next

过程模拟

对于给定的初始状态与控制输入的函数，就可以模拟出飞行器在一段时间内的运动状态

def simulate(z0, controller, horizon_length, disturbance=False):
    """
    This function simulates the quadrotor for horizon_length steps from initial state z0

    Inputs:
    z0: the initial conditions of the quadrotor as a numpy array (x, vx, y, vy, theta, omega)
    controller: a function that takes a state z as argument and index i of the time step and returns a control u
    horizon_length: the horizon length

    disturbance: if True will generate a random push every seconds during the simulation

    Output:
    t[time_horizon+1] contains the simulation time
    z[4, time_horizon+1] and u[2, time_horizon] containing the time evolution of states and control
    """

    t = np.zeros([horizon_length + 1, ])
    z = np.empty([N_STATES, horizon_length + 1])
    z[:, 0] = z0
    u = np.zeros([N_CONTROLS, horizon_length])
    for i in range(horizon_length):
        u[:, i] = controller(z[:, i], i)
        z[:, i + 1] = get_next_state(z[:, i], u[:, i])
        if disturbance and np.mod(i, 100) == 0:
            dist = np.zeros([N_STATES, ])
            dist[1::2] = np.random.uniform(-1., 1, (3,))
            z[:, i + 1] += dist
        t[i + 1] = t[i] + DELTA_T
    return t, z, u

这里的 controller 函数是一个控制器函数，接受当前状态和时间步长作为输入，返回控制输入

另外通过 disturbance 参数可以模拟环境中的随机扰动，用于测试控制器能否应对外部干扰

可视化

最后使用 matplotlib 将模拟结果绘制成动画

def animate_robot(x, u, dt=0.01, trajectory=None):
    """
    This function makes an animation showing the behavior of the quadrotor
    takes as input the result of a simulation (with dt=0.01s)
    """

    min_dt = 0.1
    if (dt < min_dt):
        steps = int(min_dt / dt)
        use_dt = int(np.round(min_dt * 1000))
    else:
        steps = 1
        use_dt = int(np.round(dt * 1000))

    # what we need to plot
    plotx = x[:, ::steps]
    plotx = plotx[:, :-1]
    plotu = u[:, ::steps]
    if trajectory is not None:
        trajectory = trajectory[:, ::steps]

    fig, ax = plt.subplots(figsize=[8.5, 8.5])
    ax.set_xlim([-4, 4])
    ax.set_ylim([-4, 4])
    ax.grid()

    list_of_lines = []

    # create the robot
    # the main frame
    line, = ax.plot([], [], 'k', lw=6)
    list_of_lines.append(line)
    # the left propeller
    line, = ax.plot([], [], 'b', lw=4)
    list_of_lines.append(line)
    # the right propeller
    line, = ax.plot([], [], 'b', lw=4)
    list_of_lines.append(line)
    # the left thrust
    line, = ax.plot([], [], 'r', lw=1)
    list_of_lines.append(line)
    # the right thrust
    line, = ax.plot([], [], 'r', lw=1)
    list_of_lines.append(line)
    # the trajectory
    line, = ax.plot([], [], 'y-', lw=1)
    list_of_lines.append(line)

    def _animate(i):
        for l in list_of_lines:  # reset all lines
            l.set_data([], [])

        theta = plotx[4, i]
        x = plotx[0, i]
        y = plotx[2, i]
        trans = np.array([[x, x], [y, y]])
        rot = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])

        main_frame = np.array([[-LENGTH, LENGTH], [0, 0]])
        main_frame = rot @ main_frame + trans

        left_propeller = np.array([[-1.3 * LENGTH, -0.7 * LENGTH], [0.1, 0.1]])
        left_propeller = rot @ left_propeller + trans

        right_propeller = np.array([[1.3 * LENGTH, 0.7 * LENGTH], [0.1, 0.1]])
        right_propeller = rot @ right_propeller + trans

        left_thrust = np.array([[LENGTH, LENGTH], [0.1, 0.1 + plotu[0, i] * 0.04]])
        left_thrust = rot @ left_thrust + trans

        right_thrust = np.array([[-LENGTH, -LENGTH], [0.1, 0.1 + plotu[0, i] * 0.04]])
        right_thrust = rot @ right_thrust + trans

        list_of_lines[0].set_data(main_frame[0, :], main_frame[1, :])
        list_of_lines[1].set_data(left_propeller[0, :], left_propeller[1, :])
        list_of_lines[2].set_data(right_propeller[0, :], right_propeller[1, :])
        list_of_lines[3].set_data(left_thrust[0, :], left_thrust[1, :])
        list_of_lines[4].set_data(right_thrust[0, :], right_thrust[1, :])

        if trajectory is not None:
            list_of_lines[5].set_data(trajectory[0, :][:i], trajectory[2, :][:i])

        return list_of_lines

    def _init():
        return _animate(0)

    ani = animation.FuncAnimation(
        fig,
        _animate,
        np.arange(0, len(plotx[0, :])),
        interval=use_dt,
        blit=True,
        init_func=_init,
        repeat=True
    )

    plt.show()

假设给定一个简单的控制器，在任何情况下两个轴都提供所受重力一半的升力

G = MASS * GRAVITY

def controller(state, i):
    return np.array([G / 2, G / 2])

在没有扰动的情况下飞行器可以停留在原地不动，而在有扰动的情况下会脱离平衡状态

LQR 控制器

最优控制问题可以用数学模型描述：

给定一个成本函数，用于衡量当前系统状态与期望状态之间的差异，目标是找到一种控制策略，使得总成本最小

而 LQR (Linear–Quadratic Regulator) 线性二次调节器是一种经典的最优控制器，适用于线性系统且成本函数为二次型的情况

离散时间 LQR

对于离散时间的线性系统

其中和为线性的状态转移矩阵，和分别为状态和控制输入

其成本函数为二次型

其中和为人工设定的权重矩阵，一般需要是正定或半正定矩阵才能保证最优解的存在性
可以理解为是对状态的惩罚，是对控制输入的惩罚
较大的会使系统更倾向于使用激进的控制策略尽快达到目标状态
而较大的会使系统更倾向于使用较小的控制输入，适用于操作很昂贵的情况

那么寻找最优的控制策略就可用优化问题描述

迭代求解

假设时刻的最佳控制策略存在且为

使用常量矩阵定义从时刻开始的代价 (cost to go) 的递归形式, 可得

为了最小化代价函数，对求导且令其为零

整理即可得到时刻的最优控制增益矩阵

及的递推关系

初始化，递推计算所有的和即可得到任意时刻的最优控制策略

接下来看具体如何使用 LQR 控制器来控制飞行器姿态

控制飞行器保持稳定

先从一个简单的问题开始：如何让飞行器飞到指定目标位置并保持稳定？

优化问题

记目标状态与目标控制输入为和，设定目标状态为原点，即

则优化问题可描述为

线性化

显然我们的飞行器动力学模型是非线性的

因此在计算时可通过在目标点泰勒展开取一次项来近似原系统

令，则有

其中

原优化问题也可转化为标准的 LQR 问题

该问题的最优控制策略为，对应原系统的最优控制策略为

计算模拟

对于这种简单的情况，和均可以使用定值

令

对位置和角度的差异惩罚较大，希望飞行器能够尽快达到目标状态

根据前面的推导，可以实现对应的线性化函数与 LQR 求解函数

Q = np.mat(np.diag([100, 1, 100, 1, 100, 1]))
R = np.mat(np.diag([1, 1]))

def get_linearization(z, u):
    A = np.mat([
        [1, DELTA_T, 0, 0, 0, 0],
        [0, 1, 0, 0, -np.cos(z[4]) * (u[0] + u[1]) * DELTA_T / MASS, 0],
        [0, 0, 1, DELTA_T, 0, 0],
        [0, 0, 0, 1, -np.sin(z[4]) * (u[0] + u[1]) * DELTA_T / MASS, 0],
        [0, 0, 0, 0, 1, DELTA_T],
        [0, 0, 0, 0, 0, 1]
    ])
    B = np.mat([
        [0, 0],
        [-np.sin(z[4]) * DELTA_T / MASS, -np.sin(z[4]) * DELTA_T / MASS],
        [0, 0],
        [np.cos(z[4]) * DELTA_T / MASS, np.cos(z[4]) * DELTA_T / MASS],
        [0, 0],
        [LENGTH / INERTIA * DELTA_T, -LENGTH / INERTIA * DELTA_T]
    ])
    return A, B
    
def solve_LQR(z_target, u_target, n):
    A, B = get_linearization(z_target, u_target)
    P = np.zeros((n + 1, N_STATES, N_STATES))
    P[-1] = Q
    K = np.zeros((n, N_CONTROLS, N_STATES))

    for i in range(n - 1, -1, -1):
        K[i] = -1 * (B.T * P[i + 1] * B + R).I * B.T * P[i + 1] * A
        P[i] = Q + A.T * P[i + 1] * A + A.T * P[i + 1] * B * K[i]

    return K
    

可以发现，这里解出的控制增益矩阵仅与目标状态和步长有关，而与当前状态无关

即无论初始状态如何，通过该组值计算出的控制输入都能保证总的代价最小，而的大小决定了系统能有多久的时间来进行操作

假设系统初始状态为，模拟时长为 1000 步即 10 秒进行计算

horizon_length = 1000
z_target = np.zeros([N_STATES, ])
u_target = np.array([G / 2, G / 2])
K = solve_LQR(z_target, u_target, horizon_length)


def controller(state, i):
    return u_target + (K[i] * np.mat(state - z_target).T).A1


z0 = np.array([3., 0., 3., 0., 0., 0.])
t, state, u = simulate(z0, controller, horizon_length, disturbance=True)
animate_robot(state, u)

可以得到如下结果

可见系统仅用了大约 3 秒的时间就将飞行器移动到了目标位置，并且在有扰动的情况下也能保持稳定

考虑题目中的问题，如果初始化状态为，目标状态为，适当调参后可以得到如下结果

虽然实现了原地翻转，但是有种差点翻车的感觉

参数分析

接下来看看调整各项参数对控制系统的影响，模拟时不添加随机扰动

调整初始状态

初始状态离目标状态越远，由线性化带来的误差就越大，控制系统的性能也会受到影响

逐渐增大初始状态离目标状态的距离，系统会出现震荡，最终失控不再收敛

另外如果初始状态改变的不是位置而是速度或角度，控制系统也能正常工作

调整和的权重

提高的权重会使得系统收敛速度变慢，但控制输入更加平滑

提高中对速度的惩罚权重同样会限制控制的大小，导致和上面一样的结果

减少对角度状态的惩罚会导致系统出现翻滚，放大线性化带来的误差最终导致失控

调整计算步长

在和不变的情况下，控制增益矩阵在迭代过程中会逐渐收敛

可以仅使用最终收敛的来计算控制输入，控制效果与使用每一时刻的基本相同

如果过小还未收敛，控制系统的效果会受到影响

因此可以在迭代过程检查是否收敛，如果收敛则停止迭代，否则继续迭代直到收敛

控制飞行器跟踪轨迹

接下来考虑一个更复杂的问题：如何让飞行器沿着指定轨迹飞行？

优化问题

假设目标轨迹为，目标控制输入为，则优化问题可描述为

该问题等价于

其中

迭代求解

首先对每一时刻的目标状态进行线性化，得到

假设最佳控制策略为

使用两个矩阵和来定义代价函数的递归形式

令则可以通过迭代计算出

最终得到最优控制策略

计算模拟

假设目标轨迹为一个圆心在原点，半径为 r 的圆形，飞行周期为 T；目标控制输入依然为保持平衡

定义轨迹生成函数，并修改 LQR 求解函数如下

Q = np.mat(np.diag([100, 1, 100, 1, 100, 1]))
R = np.mat(np.diag([1, 1]))

def get_target_trajectory(r, T, horizon_length):
    z = np.empty([N_STATES, horizon_length])
    for i in range(horizon_length):
        phase = 2 * np.pi / T * i * DELTA_T
        zt = np.zeros([N_STATES, ])
        zt[0] = np.cos(phase) * r
        zt[1] = -2 * np.pi / T * np.sin(phase) * r
        zt[2] = np.sin(phase) * r
        zt[3] = 2 * np.pi / T * np.cos(phase) * r
        z[:, i] = zt
    return z

def solve_LQR(z_target, u_target, n):
    P = np.zeros((n + 1, N_STATES, N_STATES))
    P[-1] = Q
    K = np.zeros((n, N_CONTROLS, N_STATES))
    q = -1 * Q * z_target
    p = np.zeros((n + 1, N_STATES, 1))
    p[-1] = q[:, -1]
    k = np.zeros((n, N_CONTROLS, 1))
    for i in range(n - 1, -1, -1):
        A, B = get_linearization(z_target[:, i], u_target)
        K[i] = -1 * (B.T * P[i + 1] * B + R).I * B.T * P[i + 1] * A
        P[i] = Q + A.T * P[i + 1] * A + A.T * P[i + 1] * B * K[i]
        k[i] = -1 * (B.T * P[i + 1] * B + R).I * B.T * p[i + 1]
        p[i] = q[:, i] + A.T * p[i + 1] + A.T * P[i + 1] * B * k[i]
    return K, k

取，初始状态进行计算

horizon_length = 1000
z_target = get_target_trajectory(2, 10, horizon_length)
u_target = np.array([G / 2, G / 2])
K, k = solve_LQR(z_target, u_target, horizon_length)


def controller(state, i):
    return u_target + (K[i] * np.mat(state).T).A1 + k[i].squeeze()


z0 = np.array([0., 0., 0., 0., 0, 0.])
t, state, u = simulate(z0, controller, horizon_length, disturbance=False)
animate_robot(state, u)

可以得到如下结果，完美符合预期

控制飞行器自动规划轨迹

最后考虑题目中的问题，如何自动控制飞行器完成一个翻转动作并回到原位？

优化问题

显然对于这个问题，我们只能给定初始和目标状态，而无法给定目标轨迹

如果动力学系统是线性的，系统中只有线性约束的情况下作为凸问题可以直接使用 cvx 等工具求解

但是对于非线性系统，我们可以使用迭代法来逼近最优解

iLQR (Iterative Linear Quadratic Regulator) 是一种常用的方法，其算法如下

初始化一组控制策略

使用当前控制策略计算状态轨迹

利用线性化计算当前轨迹的二次近似代价

使用 LQR 求解当前轨迹的最优控制增益矩阵

更新控制策略

重复 2-5 直到代价收敛

因此对于目标轨迹和目标控制策略，记

将上面优化目标代价函数进行二次展开，可以得到

其中

迭代求解

对于某一次迭代，通过线性化近似

可以得到递推关系

在更新控制策略时，使用优化理论中常用的线搜索方法来保证每次迭代都能使代价函数减小

即在更新策略时，使用以下算法

初始化步长，计算当前代价函数值

更新控制策略

计算新的代价函数值

如果，则接受更新，否则减小步长

重复 2-4 直到

计算模拟

自动规划路径时，需要给定若干目标点，且在这些目标点处施加较大的惩罚，以保证优化结果

假如我们希望时刻飞行器已经完成翻转到达原点，定义航点及惩罚权重如下

horizon_length = 500
u_stable = np.array([G / 2, G / 2])

waypoints = {
    499: np.array([0, 0, 0, 0, 2 * np.pi, 0])
}

def Q(t):
    if t in waypoints:
        return np.mat(np.identity(N_STATES)) * 100000
    else:
        return np.mat(np.identity(N_STATES)) * 0.1


R = np.mat(np.diag([1, 1])) * 10

对于某一组状态与控制输入，将其定义的航点对应的状态与控制输入进行替换，作为期望达到的目标，即可计算代价函数

def target(z, u):
    z_target = z.copy()
    u_target = u.copy()
    for t, zt in waypoints.items():
        z_target[:, t] = zt
        u_target[:, t] = u_stable
    return z_target, u_target


def compute_cost(z, u):
    n = u.shape[1]
    z_target, u_target = target(z, u)

    j = 0.
    for i in range(n):
        z_hat = np.mat(z[:, i] - z_target[:, i]).T
        u_hat = np.mat(u[:, i] - u_target[:, i]).T

        jt = z_hat.T * Q(i) * z_hat + u_hat.T * R * u_hat
        j += jt[0, 0]

    z_hat = np.mat(z[:, -1] - z_target[:, -1]).T
    jn = z_hat.T * Q(n - 1) * z_hat
    j += jn[0, 0]
    return j

实现 iLQR 算法如下

MAX_ITERATION = 50
TOLERANCE = 0.1

def solve_ilqr(z0, z, u, n):
    def backward(z, z_target, u, u_target):
        K = np.zeros((n, N_CONTROLS, N_STATES))
        P = np.zeros((n + 1, N_STATES, N_STATES))
        P[-1] = Q(n - 1)
        k = np.zeros((n, N_CONTROLS, 1))
        p = np.zeros((n + 1, N_STATES, 1))
        p[-1] = Q(n - 1) * np.mat(z[:, -1] - z_target[:, -1]).T

        for i in range(n - 1, -1, -1):
            Qt = Q(i)
            qt = Qt * np.mat(z[:, i] - z_target[:, i]).T
            rt = R * np.mat(u_target[:, i]).T

            A, B = get_linearization(z[:, i], u[:, i])

            K[i] = -1 * (R + B.T * P[i + 1] * B).I * B.T * P[i + 1] * A
            P[i] = Qt + A.T * P[i + 1] * A + A.T * P[i + 1] * B * K[i]
            k[i] = -1 * (R + B.T * P[i + 1] * B).I * (B.T * p[i + 1] + rt)
            p[i] = qt + A.T * p[i + 1] + A.T * P[i + 1] * B * k[i]
        return K, k

    def forward(z0, z_target, u_target, K, k, alpha=1):
        z_new = np.zeros_like(z)
        z_new[:, 0] = z0
        u_new = np.zeros_like(u)

        for i in range(n):
            u_new[:, i] = u_target[:, i] + (K[i] * np.mat(z_new[:, i] - z_target[:, i]).T).A1 + alpha * k[i].squeeze()
            z_new[:, i + 1] = get_next_state(z_new[:, i], u_new[:, i])

        return z_new, u_new

    j_prev = compute_cost(z, u, n)

    it = 0
    while it < MAX_ITERATION:

        z_target, u_target = target(z, u)
        k, kt = backward(z, z_target, u, u_target)

        alpha = 1
        while alpha > 0.01:
            z_ls, u_ls = forward(z0, z_target, u_target, k, kt, alpha)

            j = compute_cost(z_ls, u_ls, n)
            if j < j_prev:
                z, u = z_ls, u_ls
                break
            else:
                alpha *= 0.5

        if abs(j - j_prev) < TOLERANCE:
            break

        j_prev = j
        it += 1

    return z, u

可见算法主体就是通过 backward 更新控制策略，通过 forward 更新状态轨迹的反复迭代，最终找到总代价最小的控制策略与状态轨迹

取初始状态，初始控制策略为，进行计算

z0 = np.zeros([N_STATES, ])
z_init = np.empty([N_STATES, horizon_length + 1])
z_init[:, 0] = z0
u_init = np.zeros((N_CONTROLS, horizon_length))
for i in range(horizon_length):
    u_init[:, i] = u_stable
    z_init[:, i + 1] = get_next_state(z_init[:, i], u_init[:, i])

z, u = solve_ilqr(z0, z_init, u_init, horizon_length)
animate_robot(z, u)

最终即可得到丝滑的翻转动作

稍微换一组航点，可以得到更加复杂的轨迹

horizon_length = 1000

waypoints = {
    199: np.array([3, 0, 3, 0, 0, 0]),
    399: np.array([3, 0, -3, 0, 0, 0]),
    599: np.array([-3, 0, 3, 0, 0, 0]),
    799: np.array([-3, 0, -3, 0, 0, 0]),
    999: np.array([0, 0, 0, 0, 0, 0])
}

总结

实际上这里的控制算法没有考虑飞行器的动力学约束，控制值的大小可能会超出实际的物理限制，在实际应用时还得把对应的不等式约束项加入到优化问题中

等有时间弄一台四轴无人机，再来玩一玩这个控制算法，应该会更有趣

完整代码见 GitHub

LQR控制2D飞行器完成特技飞行动作

构建模拟环境

物理模型

动力学模型

过程模拟

可视化

LQR 控制器

离散时间 LQR

迭代求解

控制飞行器保持稳定

优化问题

线性化

计算模拟

参数分析

调整初始状态

调整 和 的权重

调整计算步长

控制飞行器跟踪轨迹

优化问题

迭代求解

计算模拟

控制飞行器自动规划轨迹

优化问题

迭代求解

计算模拟

总结

调整和的权重