倒立擺，Inverted Pendulum ，是典型地多變量、高階次 ，非線性、強耦合、自然不穩定系統。倒立擺系統地穩定控制是控制理論中地典型問題 ，在倒立擺地控制過程中能有效反映控制理論中地許多關鍵問題 ，如非線性問題、魯棒性問題、隨動問題、鎮定、跟蹤問題等。因此倒立擺系統作為控制理論教學與科研中典型地物理模型 ，常被用來檢驗新地控制理論和算法地正確性及其在實際應用中地有效性。

一、原理介紹：

開環系統，狀態變量的倒數 = 系統的狀態空間矩陣A * 系統狀態變量x

A狀態矩陣：描述系統本身物理特性的一個矩陣，它是由系統本身的機械結構、物理結構決定的，無法改變。

系統狀態變量x：用四個變量描述了倒立擺整個系統的運動狀態（倒立擺角度、角速度、飛輪速度、飛輪角速度）。

閉環系統：

B狀態矩陣也是由系統本身的物理特性決定的。u就是反饋項，u = - kx。

整個閉環算法中最重要的就是求出最佳的K，來使得系統穩定。

如何求最優K是核心問題，LQR實際上提供了求解最優K的方法。

通過代價函數來求K，可以看到里面還有Q和R，這兩個與系統的收斂有關。Q是系統狀態變量的權重，也叫權重矩陣。

根據這個來看，比如Q11就是倒立擺角度的權重，Q22就是角速度的權重，Q33就是飛輪角度的權重、Q44就是飛輪角速度的權重。如果希望道理擺角度收斂的更快（更快穩定），可以給Q11的權重增加.

R的話，可以理解為決定系統輸出量大小的項。一般而言R都設為1,不去改變它。

結合這兩個式子

R越大，u的輸出越?。籖越小，u的輸出越大。

一句話，要想求出K，就需要A矩陣、B矩陣、Q矩陣、R矩陣（一般為1）

其實只需要三個。A矩陣和B矩陣得通過建模得到。

物理建模 --- 建立動力學模型（為了A矩陣和B矩陣）

這個是動量輪動力擺系統，可以用牛頓第二定律來分析力，但是我們用更常用的求法。

用拉格朗日方程：

因為拉格朗日方程不需要列出系統具體的力是怎樣平衡的

它只需要列出系統的動能減去勢能得到算子，然后爭對這個算子對系統不同的廣義坐標求偏導，最后得到系統的廣義力

這個θ是擺桿的擺角

這個φ是動量輪自身的轉動角度

得：

其MATLAB實現為：

二、實例：

這里介紹“DengFOC動量輪倒立擺項目”

DengFOC動量輪倒立擺是基于SimpleFOC官方開源的動量輪倒立擺項目，低成本化和小型化后應用在開源的DengFOC雙路無刷電機驅動器上的DengFOC配套項目。

只要你手頭有一塊DengFOC V3就可以快速搭接出此項目，項目供電默認電壓為16.8V，除DengFOC外，其它電子元器件主要由一個2204無刷電機，兩個as5600編碼器組成。

其實現代碼為：

#include <SimpleFOC.h>

// 無刷直流電機初始化

BLDCMotor motor = BLDCMotor(7); //2208電機

// 定義無刷直流驅動器

BLDCDriver3PWM driver = BLDCDriver3PWM(32,33,25,12);

// 電機編碼器初始化

MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);

MagneticSensorI2C pendulum = MagneticSensorI2C(AS5600_I2C);

TwoWire I2Cone = TwoWire(0);

TwoWire I2Ctwo = TwoWire(1);

float adjust_ang= 60;

float adjust_p_vel= 3;

float adjust_motor_vel= 0.18;

float adjust_total_ang= 0; //微調角度

// 通訊命令預設

Commander command = Commander(Serial);

void doTarget_motor_vel(char* cmd) { Serial.print("Change LQR motor_v");command.scalar(&adjust_motor_vel, cmd); }

void doTarget_p_vel(char* cmd) { Serial.print("Change LQR P_v");command.scalar(&adjust_p_vel, cmd); }

void doTarget_ang_vel(char* cmd) { Serial.print("Change LQR ang");command.scalar(&adjust_ang, cmd); }

void doTarget_total_ang(char* cmd) { Serial.print("Change total ang");command.scalar(&adjust_total_ang, cmd); }

void setup() {

// 初始化電機編碼器硬件

I2Cone.begin(19,18, 400000UL);

I2Ctwo.begin(23,5, 400000UL);

sensor0.init(&I2Cone);

pendulum.init(&I2Ctwo);

// 設置要使用的控制回路類型

motor.controller = MotionControlType::torque;

// 將電機連接到編碼器

motor.linkSensor(&sensor0);

// driver

driver.voltage_power_supply =16.8;

driver.voltage_limit = 16;

driver.init();

// 把電機連接到驅動器上

motor.linkDriver(&driver);

Serial.begin(115200);

motor.useMonitoring(Serial);

// 初始化運動

motor.init();

// 校準編碼器并啟動FOC

motor.initFOC();

command.add('M', doTarget_motor_vel, "adjust_motor_vel");

command.add('P', doTarget_p_vel, "adjust_p_vel");

command.add('A', doTarget_ang_vel, "adjust_ang");

command.add('T',doTarget_total_ang, "adjust_total_ang");

}

// 循環下采樣計數器

long loop_count = 0;

float target_voltage;

void loop() {

// ~1ms

motor.loopFOC();

command.run();

// 控制回路每次~25ms

if(loop_count++ > 25){

// 倒立擺傳感器讀取

pendulum.update();

sensor0.update();

// 計算擺角

float pendulum_angle = constrainAngle(pendulum.getAngle()-2.6-adjust_total_ang + M_PI);//先看上電串口打印的角度然后替換這里的2.6

Serial.println(pendulum.getAngle());

//Serial.println(sensor0.getAngle());

if( abs(pendulum_angle) < 0.5){ // 如果角度足夠小穩定

target_voltage = controllerLQR(pendulum_angle, pendulum.getVelocity(), motor.shaftVelocity());}

else{ // 倒立擺

//Serial.println("swing-up");

// 設置100%的最大電壓到電機，以便擺動

//target_voltage = -_sign(pendulum.getVelocity())*motor.voltage_limit*1;

target_voltage = -_sign(pendulum.getVelocity())*16.8;

//Serial.println(target_voltage);

//target_voltage=0;

}

// 將目標電壓設置到電機上

motor.move(target_voltage);

// 重置計數器

loop_count=0;

}

}

// 函數限制-和之間的夾角，以-180度和180度表示

float constrainAngle(float x){

x = fmod(x + M_PI, _2PI);

if (x < 0)

x += _2PI;

return x - M_PI;

}

// LQR穩定控制器功能

// 計算需要設置電機的電壓，以穩定擺

float controllerLQR(float p_angle, float p_vel, float m_vel){

// 如果角度可控

// 計算控制律

// LQR controller u = k*x

// - k = [40, 7, 0.3]

// - x = [擺角，擺速度，電機速度]'

float u = adjust_ang*p_angle + adjust_p_vel*p_vel + adjust_motor_vel*m_vel;

// 限制設定給電機的電壓

//if(abs(u) > motor.voltage_limit*1) u = _sign(u)*motor.voltage_limit*1;

if(abs(u) > driver.voltage_power_supply*0.7) u = _sign(u)*driver.voltage_power_supply*0.7;

return u;

}

來源: 來自于網絡