环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）_人工智能

前言

上一篇中简述了模型预测控制在多旋翼飞行器运动规划中的应用。本篇将针对采用线性MPC的多旋翼飞行器的运动规划方案进行详细推导。

系统模型的离散化

对于多旋翼飞行器这类三阶积分器模型，我们可以很容易得到其连续时间系统模型：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）对其离散化，可得离散时间系统模型：
在得到离散时间系统模型之后，我们将其进行参数化，得到预测模型：

预测模型参数的计算

在每一个预测周期，我们首先需要计算预测模型的相关参数，Matlab代码如下：

function [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K, dt, p_0, v_0, a_0)
    Ta = zeros(K);
    Tv = zeros(K);
    Tp = zeros(K);
    
    for i = 1 : K
        Ta(i, 1:i) = ones(1, i) * dt;
    end
    
    for i = 1 : K
        for j = 1 : i
            Tv(i, j) = (i - j + 0.5) * dt ^ 2;
        end
    end
    
    for i = 1 : K
        for j = 1 : i
            Tp(i, j) = ((i - j + 1) * (i - j) / 2 + 1 /6) * dt ^ 3;
        end
    end
    
    Ba = ones(K, 1) * a_0;
    Bv = ones(K, 1) * v_0;
    Bp = ones(K, 1) * p_0;
    
    for i = 1 : K
        Bv(i) = Bv(i) + i * dt * a_0;
        Bp(i) = Bp(i) + i * dt * v_0 + i ^ 2 / 2 * a_0 * dt ^ 2;
    end
end

二次规划问题（QP）的求解

在已知预测模型的前提下，我们需要针对我们的规划目标设计对应的代价函数。这里以设定终点速度为指定值，终点加速度为0，终点位置不做约束的情况作为案例，其他情况可自行推导。我们设计如下代价函数：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）将预测模型代入上式，可得：
以上代价函数可转化为典型的二次规划问题：
这里我们优化的变量为
$J$ 。下一步，我们需要确定二次规划问题的约束条件。

硬约束与软约束

硬约束与软约束均能够界定优化问题的边界条件，以
$Ax\leq b$ 的形式表述。以多旋翼飞行器的运动规划为例，由于飞行器存在明确的动力学极限，即有限的速度、加速度等状态参数，我们需要对规划的轨迹设定对应的约束条件。 硬约束界定了一个不可逾越的边界条件，运动规划的结果将严格遵守此边界条件。
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）由此可得对应的边界参数:

H = blkdiag(w2 * eye(K) + w1 * (Tv' * Tv) + w2 * (Ta' * Ta));
F = w1 * (Bv - v_t)' * Tv +  w2 * Ba' * Ta;
A = [Tv; -Tv; Ta; -Ta];
b = [v_max * ones(K,1) - Bv; v_max * ones(K,1) + Bv; a_max *ones(K,1) - Ba; a_max * ones(K,1) + Ba];
J = quadprog(H, F, A, b);

通过调节不同的权重值，能够改变不同的规划结果，可根据实际情况调整：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二） 软约束则通过扩展优化变量、修改代价函数的方式进行优化问题的求解，适用于初始状态超过边界条件的情况。例如，多旋翼飞行器的最大速度为3
$m/s$ ，而初始速度为5
$m/s$ ，若采用硬约束，则二次规划问题无解，此时，需要通过软约束的方式解决该问题。软约束的实质是在原本的代价函数的基础上，扩展一个惩罚函数
$S(L)$ ，由此得到新的代价函数：
其约束条件则变为：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）由此可得对应的边界参数:

H = blkdiag(w3 * eye(K) + w1 * (Tv' * Tv) + w2 * (Ta' * Ta), w4 * eye(K));
F = [w1 * (Bv - v_t)' * Tv +  w2 * Ba' * Ta zeros(1,K)];
A = [Tv eye(K); -Tv -eye(K); Ta eye(K); -Ta -eye(K); zeros(size(Ta)) -eye(K)];
b = [v_max * ones(K,1) - Bv; v_max * ones(K,1) + Bv; a_max * ones(K,1) - Ba; a_max * ones(K,1) + Ba; zeros(K,1)];
J = quadprog(H, F, A, b);

对于初始状态在上下界之外的情况，软约束能够使得规划之后的状态首先收敛于上下界范围内，然后再进行常规的二次规划问题求解，如下图：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）施加软约束的二次规划结果不一定会严格遵守其边界条件，具体的优化结果取决于其扩展变量的权重。

运动规划后的控制算法框架

经过运动规划生成的轨迹，我们需要一个良好的控制器去跟踪，这里以多旋翼飞行器为例：
环境感知与规划专题（六）——模型预测控制（MPC）在运动规划中的应用（二）参考轨迹的位置设定值作为位置控制器的输入； 速度设定值加上位置控制器的输出（即速度前馈补偿量）作为速度控制器的输入； 加速度设定值加上速度控制器的输出（即加速度前馈补偿量）作为加速度控制器的输入；加速度控制器的输出即为姿态控制器的输入；通过内环控制器使得飞行器各状态量跟踪参考轨迹。该控制框架即为上一篇中提到的Tube based MPC。