基于MCU的球動(dòng)式平衡機(jī)器人

陳天海 劉宇森 鄭冰欣

摘要 目前可移動(dòng)的輪式機(jī)器人由于受到運(yùn)動(dòng)方向的限制，無(wú)法在小范圍內(nèi)實(shí)現(xiàn)全向移動(dòng)，因此出現(xiàn)了球動(dòng)式平衡機(jī)器人。本文通過(guò)硬件機(jī)械結(jié)構(gòu)，傳感器數(shù)據(jù)采集和處理，控制算法程序設(shè)計(jì)等方面進(jìn)行研究。

【關(guān)鍵詞】球動(dòng)式平衡機(jī)器人 姿態(tài)控制 PID算法STM32

1 引言

現(xiàn)代社會(huì)的科技水平不斷提高，機(jī)器人作為科技發(fā)展的產(chǎn)物，在實(shí)際應(yīng)用中極大地提高了生產(chǎn)效率，現(xiàn)在越來(lái)越普遍的應(yīng)用的社會(huì)生活的各個(gè)方面。

球動(dòng)式平衡機(jī)器人作為輪式機(jī)器人的一種，通過(guò)驅(qū)動(dòng)電機(jī)的運(yùn)轉(zhuǎn)，球體作為驅(qū)動(dòng)輪在平面上移動(dòng)。由于球體具有全向移動(dòng)的特性，使用此結(jié)構(gòu)可以實(shí)現(xiàn)機(jī)器人的全方位移動(dòng)，在工作范圍極小的空間中也可以靈活運(yùn)動(dòng)。

2 系統(tǒng)原理

基于MCU的球動(dòng)式平衡機(jī)器人主要由姿態(tài)檢測(cè)部分，驅(qū)動(dòng)模塊，控制中心等組成。工作狀態(tài)下，將機(jī)器人放在平地上，初始化姿態(tài)檢測(cè)傳感器，待姿態(tài)檢測(cè)初始化完成，開(kāi)啟電機(jī)驅(qū)動(dòng)，機(jī)器人開(kāi)始運(yùn)行，自此后整個(gè)過(guò)程中不斷采集姿態(tài)狀態(tài)，控制機(jī)器人運(yùn)動(dòng)。系統(tǒng)原理如圖1所示。

3 系統(tǒng)硬件設(shè)計(jì)

3.1 控制中心

微控制單元（MCU）集成內(nèi)存、計(jì)數(shù)器、A/D轉(zhuǎn)換、UART、DMA等接口，在不同的使用場(chǎng)合設(shè)計(jì)出不同的控制組合。STM32系列芯片是專為嵌入式應(yīng)用而設(shè)計(jì)，使用了ARM公司的高性能”Cortex-M3”內(nèi)核，lμs的雙12位ADC，4兆位/秒的UART，18兆位/秒的SPI，18MHz的I/O翻轉(zhuǎn)速度，低功耗設(shè)計(jì)可以達(dá)到在72MHz時(shí)消耗36mA（所有外設(shè)處于工作狀態(tài)），待機(jī)時(shí)下降到2μA。為了使用最佳的控制方案，系統(tǒng)使用STM32F407VET6作為控制中心。

3.2 電機(jī)及驅(qū)動(dòng)系統(tǒng)

目前設(shè)計(jì)應(yīng)用的主流驅(qū)動(dòng)電機(jī)是直流電機(jī)和步進(jìn)電機(jī)，直流電機(jī)通常采用連續(xù)移動(dòng)的控制方式，而步進(jìn)電機(jī)是以步階方式分段移動(dòng)。為了更好的控制機(jī)器人，驅(qū)動(dòng)控制系統(tǒng)采用MG42L1步進(jìn)電機(jī)和路LV873IV步進(jìn)電機(jī)驅(qū)動(dòng)模塊。MG42L1步進(jìn)電機(jī)適用電壓12-25.2V，電感/相：3.2mH，靜力矩：3.6Kg-cm，轉(zhuǎn)動(dòng)慣量：35g-cm，電機(jī)重量292.lg。LV873IV步進(jìn)電機(jī)驅(qū)動(dòng)單路典型最大電流2.5A，典型值2A。

3.3 姿態(tài)檢測(cè)系統(tǒng)

為了精確獲取機(jī)器人的姿態(tài)信息，該機(jī)器人使用融合3軸加速度計(jì)和三軸陀螺儀傳感器的六軸姿態(tài)傳感器MPU6050作為姿態(tài)檢測(cè)傳設(shè)備，具體參數(shù)見(jiàn)表1。

3.4 底盤(pán)設(shè)計(jì)

機(jī)器人底盤(pán)上安裝驅(qū)動(dòng)模塊，步進(jìn)電機(jī)，QL萬(wàn)向輪以及所有的控制中心，檢測(cè)模塊等。在實(shí)際的機(jī)器人運(yùn)動(dòng)模型中是通過(guò)萬(wàn)向輪的轉(zhuǎn)動(dòng)來(lái)帶動(dòng)球體運(yùn)動(dòng)，所以萬(wàn)向輪的位置是影響機(jī)器人的整體性能的重要因素。底盤(pán)安裝示意圖如圖2所示。

4 系統(tǒng)軟件設(shè)計(jì)

4.1 確立相對(duì)空間坐標(biāo)系

機(jī)器人的平衡過(guò)程中需要獲得整體的姿態(tài)信息，而整體的姿態(tài)信息是建立在相對(duì)空間位置中的。相對(duì)坐標(biāo)系的確立需要三個(gè)參數(shù)：俯仰角（Pitch）、航偏角（Yaw）、翻滾角（RoII），這三個(gè)參數(shù)的定義如下：

（1）俯仰角0：載體縱軸與縱向水平軸之間的夾角，向上為正，向下為負(fù)。

（2）橫滾角γ：載體縱向?qū)ΨQ面與縱向鉛垂面之間的夾角，橫滾角從鉛垂面算起，右傾為正，左傾為負(fù)。

（3）偏航角ψ：載體縱軸在水平面上的投影與地理子午線之間的夾角，偏航角數(shù)值以地理北向?yàn)槠瘘c(diǎn)沿逆時(shí)針?lè)较驗(yàn)檎?/p>

在機(jī)器人初始化的過(guò)程中用以上三個(gè)參數(shù)確立機(jī)器人所處位置的相對(duì)空間坐標(biāo)系。

4.2 數(shù)值濾波

使用簡(jiǎn)易卡爾曼濾波對(duì)得到數(shù)值進(jìn)行處理：

X[i+l]-（l-K[i+ll）x[i]+K[i+1]*z[i+1]

x即估計(jì)值，z即測(cè)量值。

K[i+1]=（P[i]+Q）/（P[i]+Q+R）

P是上一次估計(jì)值的方差，Q是高斯噪聲的方差，R是測(cè)量值的方差，Q和R都是常數(shù)。

P[i+1]=（1一K[i+1]）P[i]

這一次的方差由上一次的方差和這次的增益系數(shù)決定

4.3 速度PID控制

建立如下速度模型對(duì)機(jī)器人的平衡進(jìn)行控制：

PID控制器結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、可靠性高，因此作為工業(yè)控制中應(yīng)用最為廣泛最基本的控制方法。但是在實(shí)際應(yīng)用中需要根據(jù)具體的結(jié)構(gòu)情況更改參數(shù)。PID控制結(jié)構(gòu)圖如圖3所示。

5 結(jié)束語(yǔ)

在此機(jī)器人設(shè)計(jì)過(guò)程中出現(xiàn)了很多問(wèn)題，最開(kāi)始的機(jī)械結(jié)構(gòu)設(shè)計(jì)不合理，后來(lái)反復(fù)修改才得到最終的結(jié)構(gòu)，調(diào)試過(guò)程中對(duì)數(shù)據(jù)的處理不夠，沒(méi)有利用Matlab等軟件對(duì)程序進(jìn)行仿真，這些問(wèn)題值得在后面的研究設(shè)計(jì)中解決。

本設(shè)計(jì)通過(guò)濾波算法對(duì)傳感器采集的數(shù)據(jù)進(jìn)行處理，結(jié)合電機(jī)的速度反饋值，以機(jī)器人自平衡算法和PID控制算法構(gòu)成平衡系統(tǒng)核心，從而實(shí)現(xiàn)機(jī)器人的自平衡。該機(jī)器人在硬件設(shè)計(jì)方面獨(dú)具創(chuàng)新，體積小，結(jié)構(gòu)合理，性能穩(wěn)定，質(zhì)量較小，成本較低等特點(diǎn)。由于該機(jī)器人獨(dú)特的運(yùn)動(dòng)方式，尤其適用于空間結(jié)構(gòu)較小的工作空間，市場(chǎng)應(yīng)用價(jià)值較高。

參考文獻(xiàn)

[1]張少昆，單球輪移動(dòng)機(jī)器人控制方法的研究[D].北京：北京科技大學(xué)，2013.

[2]朱磊磊，輪式移動(dòng)機(jī)器人研究綜述[J].機(jī)床與液壓，2009，37 （08）：142-246.

[3]陳小磊，魏世民，于秀麗，獨(dú)輪車(chē)機(jī)器人的發(fā)展和技術(shù)研究[J].機(jī)電產(chǎn)品開(kāi)發(fā)與創(chuàng)新，2010 （23）：6-14.

[4]韋巍，何衍，智能控制基礎(chǔ)[M]，北京：清華大學(xué)出版社，2008.