c代码LQR一阶倒立摆

好的，以下是一个简单的一阶倒立摆控制的 C 代码，使用了 LQR 控制：

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

#define pi 3.1415926

double m = 1.0; // 质量
double g = 9.8; // 重力
double l = 0.5; // 杆长

double A[2][2] = {{0,1},{g/l,0}}; // 系统矩阵 A
double B[2] = {0,1/(m*l*l)};      // 输入矩阵 B

double Q[2][2] = {{1,0},{0,1}};   // 状态权重矩阵 Q
double R = 1;                     // 输入权重矩阵 R

// 矩阵乘法
void matmul(double A[][2], double B[], double C[]) {
    int i, j;

    for (i = 0; i < 2; i++) {
        C[i] = 0;
        for (j = 0; j < 2; j++) {
            C[i] += A[i][j] * B[j];
        }
    }
}

int main() {
    double x[2] = {pi/2, 0};  // 初始状态，杆子竖直向上
    double u = 0;             // 初始输入，不施加力

    double K[2] = {0,0};      // LQR 控制器增益矩阵 K
    double P[2][2] = {{0,0},{0,0}};  // 代数 Riccati 方程的解

    int i, j, k;
    int steps = 1000;         // 模拟步数
    double dt = 0.01;         // 时间步长

    // 计算代数 Riccati 方程的解
    for (i = 0; i < 1000; i++) {
        double P_new[2][2] = {{0,0},{0,0}};
        double AT[2][2], ATQ[2][2], KBRB[2][2], temp[2][2];

        // 计算 AT, ATQ, KBRB
        for (j = 0; j < 2; j++) {
            for (k = 0; k < 2; k++) {
                AT[j][k] = A[k][j];
            }
        }

        matmul(AT, Q[0], ATQ[0]);
        matmul(AT, Q[1], ATQ[1]);

        KBRB[0][0] = KBRB[1][1] = R + B[0] * K[0] + B[1] * K[1];
        KBRB[0][1] = KBRB[1][0] = B[1] * K[0];

        // 计算 P_new
        matmul(A, P[0], temp[0]);
        matmul(A, P[1], temp[1]);

        for (j = 0; j < 2; j++) {
            for (k = 0; k < 2; k++) {
                P_new[j][k] = ATQ[j][k] + P[j][0] * temp[0][k] + P[j][1] * temp[1][k] - P[j][k] * KBRB[k][k];
            }
        }

        // 判断是否收敛
        if (fabs(P_new[0][0] - P[0][0]) < 1e-6 && fabs(P_new[0][1] - P[0][1]) < 1e-6 && 
            fabs(P_new[1][0] - P[1][0]) < 1e-6 && fabs(P_new[1][1] - P[1][1]) < 1e-6) {
            break;
        }

        // 更新 P
        P[0][0] = P_new[0][0];
        P[0][1] = P_new[0][1];
        P[1][0] = P_new[1][0];
        P[1][1] = P_new[1][1];

        // 计算新的 K
        K[0] = -1.0 / R * (B[0] * P[0][0] + B[1] * P[1][0]);
        K[1] = -1.0 / R * (B[0] * P[0][1] + B[1] * P[1][1]);
    }

    // 模拟控制过程
    for (i = 0; i < steps; i++) {
        double x_new[2], u_new;
        double temp[2];

        // 计算新的输入
        matmul(K, x, &u_new);
        u_new = -u_new;

        // 更新状态
        x_new[0] = x[0] + x[1] * dt;
        x_new[1] = x[1] + (g/l * sin(x[0]) + u) * dt;
        x[0] = x_new[0];
        x[1] = x_new[1];

        // 更新输入
        u = u_new;

        // 输出状态
        printf("%lf %lf\n", x[0], x[1]);
    }

    return 0;
}

代码中使用了一个简单的 LQR 控制器，通过 Riccati 方程求解控制器增益矩阵 K，然后根据当前状态计算新的输入。其中涉及到矩阵乘法和一些数学计算。