使用梯度下降法优化MPC实现倒立摆小车控制

前言

        梯度下降法是最常用的优化算法之一，广泛应用于机器学习、深度学习、最优化问题等领域。它是一种通过迭代的方式找到函数的局部最小值的方法。

        梯度下降法在MPC（模型预测控制）中，目标函数通常是某个状态偏差和控制输入的加权平方和。梯度下降法通过计算目标函数相对于控制输入的梯度，然后沿着梯度的反方向更新控制输入，直到收敛到最优解。

一、梯度下降法基本原理

为了方便解释，我们构建如下模型：
    假设我们要最小化一个目标函数 f(x)，其中 x 是参数向量。梯度下降法的基本思想是通过不断调整参数 x，使得目标函数的值逐步减小，最终找到局部最小值。

在每一步迭代中，梯度下降法更新参数的规则可表示为：

其中：

1. 是当前未做处理的数值
2. 是该函数在当前点的梯度
3. 是每次更新的步长，也就是学习效率

我们来构建一个函数进一步讲解梯度下降法

我们用一个一元二次函数进行分析，来求出最小的x值

• 步骤一：找出目标参数，本问题中为x
• 步骤二：计算目标函数的梯度（本问题中为对X的偏导数）

• 步骤三：检查是否符合条件。如果满足条件（如梯度接近零或达到最大迭代次数），停止；否则，返回步骤二。

对于如上步骤，可通过代码实现（因为MPC倒立摆小车用C语言，所以演示代码全部用C语言）

#include <stdio.h>
#include <math.h>

// 目标函数 f(x) = x^2 + 3x + 2
double f(double x) {
    return x * x + 3 * x + 2;
}

// 目标函数的梯度 f'(x) = 2x + 3
double gradient(double x) {
    return 2 * x + 3;
}

// 梯度下降法函数
double gradient_descent(double learning_rate, int max_iterations, double initial_x) {
    double x = initial_x;
    double grad;
    int iteration;

    printf("梯度下降过程：\n");

    // 迭代更新 x，直到收敛或达到最大迭代次数
    for (iteration = 0; iteration < max_iterations; iteration++) {
        grad = gradient(x);
        x = x - learning_rate * grad; // 更新参数

        // 打印当前的 x 和目标函数值
        printf("迭代 %d: x = %.6f, f(x) = %.6f\n", iteration + 1, x, f(x));

        // 如果梯度接近零，停止迭代
        if (fabs(grad) < 1e-6) {
            break;
        }
    }
    
    return x;
}

int main() {
    double initial_x = 5.0;  // 初始值
    double learning_rate = 0.1;  // 学习率
    int max_iterations = 100;  // 最大迭代次数

    // 执行梯度下降法
    double final_x = gradient_descent(learning_rate, max_iterations, initial_x);

    // 输出最终结果
    printf("\n最终的最小值发生在 x = %.6f\n", final_x);
    printf("目标函数的最小值为 f(x) = %.6f\n", f(final_x));

    return 0;
}

代码解析

• f(x) 是我们要最小化的目标函数
• gradient(x) 是目标函数的梯度，即对 x的偏导数 f′(x)=2x+3。
• gradient_descent() 是实现梯度下降法的函数，接受学习率、最大迭代次数和初始值作为输入。每次迭代中，我们计算梯度并更新参数 x。
• x_history 保存了每次迭代时 x 的值，用于绘制梯度下降的轨迹。

运行结果：-1

二、梯度下降法在MPC控制中的应用

我们用一个典型的一阶倒立摆来进行讲解

1.建立MPC控制模型

我们用拉格朗日方程和牛顿力学建立模型（建模过程不多赘述，可以出一期）

这里我们建立的损失函数为：

其中：

• J 是损失函数（目标函数）
• 是当前状态向量
• 是目标状态向量
• 是控制输入
• Q是状态误差权重矩阵
• R是控制输入权重矩阵
• N是预测步长

2.定义参数

#include <stdio.h>
#include <math.h>

// 物理参数
#define DT 0.01  // 时间步长
#define G 9.81   // 重力加速度
#define MC 1.0   // 小车质量
#define MP 0.1   // 摆杆质量
#define L 0.5    // 摆杆长度

// 状态变量
typedef struct {
    double x;      // 小车位置
    double dx;     // 小车速度
    double theta;  // 摆角（弧度）
    double dtheta; // 摆角速度
} State;

// 控制输入
typedef struct {
    double force;  // 施加在小车上的力
} Control;

3.计算下一步状态

void update_state(State *s, Control u) {
    double ddx = (u.force + MP * G * sin(s->theta)) / (MC + MP);//计算小车加速度
    double ddtheta = (-G * sin(s->theta) - (u.force / (MP * L))) / (1 + MC / MP);//计算小车摆角加速度
//欧拉法对数值积分，更新小车的位置、速度和摆角状态。
    s->x += s->dx * DT;
    s->dx += ddx * DT;
    s->theta += s->dtheta * DT;
    s->dtheta += ddtheta * DT;
}

4.定义损失函数

损失函数在1中已经建立如下是C语言实现

double compute_loss(State s, State X_ref) {
    double Q[4] = {1.0, 1.0, 10.0, 1.0};  // 状态权重
    double loss = 0;

    loss += Q[0] * pow(s.x - X_ref.x, 2);
    loss += Q[1] * pow(s.dx - X_ref.dx, 2);
    loss += Q[2] * pow(s.theta - X_ref.theta, 2);
    loss += Q[3] * pow(s.dtheta - X_ref.dtheta, 2);

    return loss;
}

• 误差越大，损失函数值越大，MPC 尝试找到合适的控制输入让损失变小。

5.梯度下降优化控制输入

我们使用梯度下降法来调整控制输入F，使得损失函数最小。

#define ALPHA 0.01  // 梯度下降学习率
#define ITERATIONS 100  // 梯度下降迭代次数

double gradient_descent(State s, State X_ref) {
    double U = 0;  // 初始控制输入
    for (int i = 0; i < ITERATIONS; i++) {
        // 计算当前损失
        double loss_before = compute_loss(s, X_ref);

        // 计算梯度（数值微分）
        double epsilon = 0.01;
        Control u1 = {U + epsilon};
        Control u2 = {U - epsilon};
        State s1 = s, s2 = s;

        update_state(&s1, u1);
        update_state(&s2, u2);
        double grad = (compute_loss(s1, X_ref) - compute_loss(s2, X_ref)) / (2 * epsilon);

        // 梯度下降更新 U
        U -= ALPHA * grad;
    }
    return U;
}

• 计算控制输入F对损失函数的梯度，并用梯度下降法优化F。
• 采用数值微分计算梯度，使用微小扰动 ϵ来估算梯度方向。
• 迭代 ITERATIONS 次，让F逐渐优化到最优值。

6.构建主函数

现在所有的步骤都已经完成我们需要把所有代码嵌入到主循环中

int main() {
    // 初始状态
    State X = {0, 0, 0.1, 0};  // 初始摆角 0.1 弧度（约5.7度）
    State X_ref = {0, 0, 0, 0}; // 目标状态（直立平衡）

    for (int t = 0; t < 1000; t++) {
        double U = gradient_descent(X, X_ref); // 计算优化后的控制输入
        Control u = {U};
        update_state(&X, u); // 更新系统状态

        // 打印数据
        printf("t=%d, x=%.3f, theta=%.3f, U=%.3f\n", t, X.x, X.theta, U);
    }

    return 0;
}

三、总结

如上是控制部分的所有程序，我们可以增加电机控制的程序使小车实现MPC控制，我们可以修改 Q 和 R 的权重，实现更稳定的控制。