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

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

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

Ruff Lite 開(kāi)發(fā)版 （型號(hào) TM4C1294-V1 ）

陀螺儀模塊 （型號(hào) GY-521）

直流電機(jī)驅(qū)動(dòng)模塊 （型號(hào) TB6612FNG ）

編碼器模塊（隨直流電機(jī)一體）（型號(hào) MG513-30 ）

MG513-30 是該直流電機(jī)的型號(hào)，由電機(jī)驅(qū)動(dòng)模塊進(jìn)行驅(qū)動(dòng)，這里我們用它自帶的編碼器模塊

機(jī)械元件

12V 鋰電池

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

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

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

$ rap init --board tm4c1294-v1

2. 添加陀螺儀驅(qū)動(dòng)，id 為 gyro，型號(hào)選擇 GY-521，其余參數(shù)默認(rèn)

$ rap device add gyro (GY-521)

3. 添加電機(jī)驅(qū)動(dòng)，id 為 motor，型號(hào)選擇 TB6612FNG

$ rap device add motor (TB6612FNG)

4. 添加編碼器驅(qū)動(dòng)，id 為 encoder，型號(hào)選擇 MG513-30，其余參數(shù)默認(rèn)

$ rap device add encoder (MG513-30)

5. 編寫控制算法

（見(jiàn)下文）

6. 調(diào)試

（見(jiàn)下文）

7. 掃描開(kāi)發(fā)板

$ rap scan

7. 部署應(yīng)用

$ rap deploy

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

平衡車內(nèi)部有兩個(gè)反饋環(huán)，一個(gè)是由陀螺儀反饋?zhàn)藨B(tài)傾角和角速度構(gòu)成的 直立環(huán)，一個(gè)是由編碼器反饋直流電機(jī)轉(zhuǎn)速構(gòu)成的 速度環(huán)，由此構(gòu)成一個(gè)串級(jí) PID 控制系統(tǒng)（見(jiàn)下圖），速度環(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)過(guò)程請(qǐng)參考自控控制專業(yè)書(shū)籍，這里只對(duì)算法作用和各個(gè)參數(shù)的意義進(jìn)行簡(jiǎn)要說(shuō)明

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"); // 電機(jī)驅(qū)動(dòng)控制器（TB6612FNG）

    // 直立環(huán)PD控制
    var getBalancePwm = function (actualAngle, actualGyro) {
        var targetAngle = 0.8; // 小車靜止平衡時(shí)的姿態(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; // 小車靜止平衡時(shí)的電機(jī)輸出轉(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，利用反饋值計(jì)算控制量，控制電機(jī)正反轉(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;
            }
            // 控制車身前進(jìn)（電機(jī)A正轉(zhuǎn)B反轉(zhuǎn)，A與B相差180度安裝）
            motor.forwardRotateA(pwmDuty);
            motor.backwardRotateB(pwmDuty);
        } else {
            if (pwmDuty <= -1) {
                pwmDuty = -1;
            }
            // 控制車身后退（電機(jī)A反轉(zhuǎn)B正轉(zhuǎn)，A與B相差180度安裝）
            motor.backwardRotateA(-pwmDuty);
            motor.forwardRotateB(-pwmDuty);
        }

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

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

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

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

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

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

Ruff MPU版（ruff-mbd-v1）可以作為主控平臺(tái)么？

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

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

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

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

相同點(diǎn)是都滿足實(shí)時(shí)性控制的要求（如本案例的 20ms 控制周期）。

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