摘要：每個控制周期需要做的內(nèi)容包括獲取陀螺儀和編碼器兩個傳感器的數(shù)據(jù)，傳入直立環(huán)和速度環(huán)算法中進行計算得到控制量，將控制量作用于直流電機上。

Ruff Lite
Ruff Lite 是 Ruff 團隊針對 MCU（MicroController Unit，微控制器）推出的 Ruff OS
，具有高實時性，占用內(nèi)存小等特點。目前官方支持的開發(fā)板為TI TM4C1294-LaunchPad ，Ruff Lite支持的硬件接口包括：GPIO、UART、I2C、ADC、PWM、QEI。

兩輪自動平衡車是一個典型的自動控制系統(tǒng)，由執(zhí)行元件（直流電機），傳感模塊（陀螺儀和編碼器）和主控平臺控制系統(tǒng)（Ruff 開發(fā)板）組成。直流電機控制兩輪正反轉(zhuǎn)，陀螺儀檢測車身姿態(tài)，編碼器檢測電機轉(zhuǎn)速，這兩組傳感器數(shù)據(jù)反饋給控制系統(tǒng)，經(jīng)由 PID 控制算法計算，給出控制直流電機的控制量，通過這一閉環(huán)過程，從而形成負反饋，保證車身平衡

Ruff Lite 開發(fā)版 （型號 TM4C1294-V1 ）

陀螺儀模塊 （型號 GY-521）

直流電機驅(qū)動模塊 （型號 TB6612FNG ）

編碼器模塊（隨直流電機一體）（型號 MG513-30 ）

MG513-30 是該直流電機的型號，由電機驅(qū)動模塊進行驅(qū)動，這里我們用它自帶的編碼器模塊

機械元件

12V 鋰電池

電壓轉(zhuǎn)換模塊（12V-5V）

直流電機 12V 供電，開發(fā)板 5V 供電

1. 初始化 APP，選擇 tm4c1294-v1 開發(fā)板（對應(yīng) TI TM4C1294-LaunchPad）

$ rap init --board tm4c1294-v1

2. 添加陀螺儀驅(qū)動，id 為 gyro，型號選擇 GY-521，其余參數(shù)默認

$ rap device add gyro (GY-521)

3. 添加電機驅(qū)動，id 為 motor，型號選擇 TB6612FNG

$ rap device add motor (TB6612FNG)

4. 添加編碼器驅(qū)動，id 為 encoder，型號選擇 MG513-30，其余參數(shù)默認

$ rap device add encoder (MG513-30)

5. 編寫控制算法

（見下文）

6. 調(diào)試

（見下文）

7. 掃描開發(fā)板

$ rap scan

7. 部署應(yīng)用

$ rap deploy

PID（比例-積分-微分）控制算法是工程上最常用的自動控制算法，參數(shù) P 實現(xiàn)基本控制作用，參數(shù) D 避免系統(tǒng)震蕩，參數(shù)I用來消除系統(tǒng)靜差，本平衡小車系統(tǒng)由 PID 算法進行控制，從而保持平衡

平衡車內(nèi)部有兩個反饋環(huán)，一個是由陀螺儀反饋姿態(tài)傾角和角速度構(gòu)成的 直立環(huán)，一個是由編碼器反饋直流電機轉(zhuǎn)速構(gòu)成的 速度環(huán)，由此構(gòu)成一個串級 PID 控制系統(tǒng)（見下圖），速度環(huán)控制的輸出作為直立環(huán)控制的輸入，直立環(huán)由 PD 控制（比例-微分控制）系統(tǒng)構(gòu)成，保證小車的基本平衡（參數(shù) P 的作用）和避免震蕩（參數(shù) D 的作用），速度環(huán)由 PI 控制（比例-積分控制）系統(tǒng)，消除姿態(tài)傾角的靜差（參數(shù)I的作用）

具體 PID 算法的原理及推導(dǎo)過程請參考自控控制專業(yè)書籍，這里只對算法作用和各個參數(shù)的意義進行簡要說明

src/index.js

$.ready(function(error) {
    if (error) {
        console.log("error", error);
        return;
    }

    var gyro = $("#gyro"); // 陀螺儀傳感器（GY-521）
    var enc = $("#encoder"); // 編碼器傳感器（MG513-30）
    var motor = $("#motor"); // 電機驅(qū)動控制器（TB6612FNG）

    // 直立環(huán)PD控制
    var getBalancePwm = function (actualAngle, actualGyro) {
        var targetAngle = 0.8; // 小車靜止平衡時的姿態(tài)角度
        var kP = 80; // 直立環(huán)比例(P)控制參數(shù)
        var kD = 2; // 直立環(huán)微分(D)控制參數(shù)

        // 直立環(huán)控制分量
        var balancePwm = kP * (actualAngle - targetAngle) + kD * actualGyro;
        return balancePwm / 1000;
    };

    // 速度環(huán)PI控制
    var encoder = 0;
    var sumEncoder = 0;
    var getVelocityPwm = function (actualLEncoder, actualREncoder) {
        var targetVelocity = 0; // 小車靜止平衡時的電機輸出轉(zhuǎn)速
        var kP = 3; // 速度環(huán)比例(P)控制參數(shù)
        var kI = kP / 200; // 速度環(huán)積分(I)控制參數(shù)

        // FIR二階低通濾波
        encoder = 0.2 * (actualLEncoder + actualREncoder - targetVelocity) + 0.8 * encoder;
        sumEncoder += encoder;

        // 積分限幅
        if (sumEncoder >= 3000) {
            sumEncoder = 3000;
        }
        if (sumEncoder <= -3000) {
            sumEncoder = -3000;
        }

        // 速度換控制分量
        var velocityPwm = kP * encoder + kI * sumEncoder;
        return (velocityPwm / 1000);
    };

    var cycle = 20; // 采樣/控制周期均為20ms，即1s采樣/控制50次
    var gyroAcquire, encAcquire, balanceControl;

    var angleX = 0;
    var gyroY = 0;
    var rpm = 0;

    // 每隔20ms，獲取陀螺儀沿X軸的姿態(tài)傾角和角速度
    gyroAcquire = setInterval(function() {
        gyro.getFusedMotionX(cycle, function (error, _angleX, _gyroY) {
            angleX = _angleX;
            gyroY = _gyroY;
        });
    }, cycle);

    // 每隔20ms，獲取編碼器的速度值
    encAcquire = setInterval(function() {
        enc.getRpm(function (error, _rpm) {
            rpm = _rpm;
        });
    }, cycle);

    // 每隔20ms，利用反饋值計算控制量，控制電機正反轉(zhuǎn)
    balanceControl = setInterval(function() {
        var balancePwm = getBalancePwm(angleX, gyroY);
        var velocityPwm = getVelocityPwm(rpm, rpm);
        var pwmDuty = balancePwm - velocityPwm;

        if (pwmDuty >= 0) {
            if (pwmDuty >= 1) {
                pwmDuty = 1;
            }
            // 控制車身前進（電機A正轉(zhuǎn)B反轉(zhuǎn)，A與B相差180度安裝）
            motor.forwardRotateA(pwmDuty);
            motor.backwardRotateB(pwmDuty);
        } else {
            if (pwmDuty <= -1) {
                pwmDuty = -1;
            }
            // 控制車身后退（電機A反轉(zhuǎn)B正轉(zhuǎn)，A與B相差180度安裝）
            motor.backwardRotateA(-pwmDuty);
            motor.forwardRotateB(-pwmDuty);
        }

        // 若傾角超過30度，停止整個控制系統(tǒng)運行
        if (angleX >= 30 || angleX <= -30) {
            // 停止陀螺儀采樣
            clearInterval(gyroAcquire);
            // 停止編碼器采樣
            clearInterval(encAcquire);
            // 停止電機控制邏輯
            clearInterval(balanceControl);
            // 停止電機A/B轉(zhuǎn)動
            motor.stopRotateA();
            motor.stopRotateB();
        }
    }, cycle);
});

裝好整個機械元件后，要進行目標角度調(diào)試，即 targetAngle 變量，具體方法，將控制 motor 前后轉(zhuǎn)動的代碼全部注釋掉，然后在 balanceControl 這個函數(shù)中，打印 angleX，得到小車趨于平衡靜止時的角度，應(yīng)該大約在正負3度以內(nèi)。

首先確定參數(shù)的極性。

先屏蔽掉外反饋環(huán) PI 控制，保持 kP 和 kD 參數(shù)不變，看是否小車有平衡的趨勢，及車輪是否會向傾倒的一側(cè)轉(zhuǎn)動，若是，則 kP 和 kD 參數(shù)為正數(shù)不需要改變，若否，則 kP 和 kI 參數(shù)需要改為負數(shù)。

之后屏蔽掉內(nèi)反饋環(huán) PD 控制，保持 kI 和 kD 參數(shù)不變，用手去轉(zhuǎn)動連接編碼器的那個車輪，看是否是此PI控制是正反饋，即給車輪一個小的轉(zhuǎn)動，車輪是否會一直加速到最大速度，若是，則 kP 和 kD 參數(shù)為正數(shù)不需要改變，若否，則 kP 和 kI 參數(shù)需要改為負數(shù)（上述程序中只需要改 kP 即可，kI 為 kP/200）。

然后確定參數(shù)的數(shù)值。一般情況下，整個機械元件裝穩(wěn)定后，PD 算法參數(shù)（kP 和 kD）和 PI 算法中的參數(shù)（kP 和 kI）應(yīng)該不需要變動就可以直接運行在你的平衡小車上。

Ruff MPU版（ruff-mbd-v1）可以作為主控平臺么？

不能，因為底層的 OpenWRT（基于 Linux）不是實時操作系統(tǒng)，系統(tǒng)啟動后會運行很多進程，Ruff 進程不一定時刻占有 CPU，因此不能穩(wěn)定地每隔一個控制周期（這里是 20ms）獲得傳感器數(shù)據(jù)，不滿足控制系統(tǒng)的實時性要求。而 MCU 版 Ruff 的底層操作系統(tǒng)是 Nuttx RTOS，能夠保證實時操作。

控制系統(tǒng)中控制周期是多少？

控制周期為 20ms，即1秒內(nèi)控制系統(tǒng)控制 50 次。每個控制周期需要做的內(nèi)容包括 1) 獲取陀螺儀和編碼器兩個傳感器的數(shù)據(jù)，2) 傳入直立環(huán)和速度環(huán)算法中進行計算得到控制量，3) 將控制量作用于直流電機上。

用Ruff MCU開發(fā)板開發(fā)平衡車與用其它開發(fā)板（如 stm32）進行裸板開發(fā)，有什么相同點與不同點？

相同點是都滿足實時性控制的要求（如本案例的 20ms 控制周期）。

不同點主要體現(xiàn)在 開發(fā)效率和 可移植性兩個方面，若用其它 MCU 開發(fā)板進行裸板開發(fā)，要面對 硬件接口協(xié)議（I2C接口陀螺儀，QEI接口編碼器，PWM 和 GPIO 接口的直流電機控制器），外設(shè)模塊協(xié)議（比如給陀螺儀發(fā)送什么命令獲取到加速度值和角速度值）和 硬件定時器中斷（通過配置寄存器設(shè)置20ms定時器中斷）等其它問題，且代碼不具備可移植性和復(fù)用性，但在整個開發(fā)過程中若用 Ruff 開發(fā)，可直面業(yè)務(wù)邏輯，即自動控制算法，而不用關(guān)心硬件模塊的任何細節(jié)，你面對的只有外設(shè)模塊的 API，并且由于沒有任何硬件平臺的邏輯，程序本身具備可復(fù)用性。