步驟1：硬件

我嘗試了多種IMU，電機(jī)，電動機(jī)驅(qū)動器，車輪和電源。

原型視頻中顯示的內(nèi)容包括：

MPU6050

Pololu低功率直流電動機(jī)（2282：590 rpm，9.68：1，17 oz-in）

霍爾編碼器464.64 cpr（齒輪后為48 cpr）

Pololu輪120 x 60 mm（通行節(jié)拍器）

Arduino UNO

Seeed Studio 4A電機(jī)罩

大沽藍(lán)牙模塊（HC06）

Raspberry Pi 2B +

2x UltraBorg板，用于讀取Raspberry上的超聲波傳感器

2x DFRobot LiPo 7.4 V電池

HOBBYWING 3A開關(guān)模式UBEC 5V 6V

慣性測量單元（IMU）

視頻中的機(jī)器人使用基于InvenSense MPU6050芯片的基本且成本非常低的模塊。我還嘗試了基于MPU9150和MPU9250的多個SeeedStudio模塊和版本。后者的芯片名為10DOF（自由度），因?yàn)樗?a href="http://m.makelele.cn/tags/陀螺儀/" target="_blank">陀螺儀（3軸），加速度計（3軸）和磁力計（3軸）上增加了氣壓計（單軸）。

所有3種芯片上都裝有數(shù)字運(yùn)動處理器（DMP）。使用此處理器有很大的優(yōu)勢。它在計算垂直角度（傳感器融合和濾波）時節(jié)省了大量代碼。其次，它具有內(nèi)置的校準(zhǔn)程序。最后但并非最不重要的一點(diǎn)是：它節(jié)省了Arduino的CPU和內(nèi)存的使用。

如果您想將DMP與Arduino結(jié)合使用，即使在“開發(fā)人員專區(qū)”中，InvenSense文檔也無濟(jì)于事。 Jeff Rowberg在這里發(fā)布了有關(guān)MPU6050寄存器的出色技巧。該庫僅適用于MPU6050和MPU9150。 MPU9250的寄存器配置不同。在使用MPU9150芯片進(jìn)行測試時，DMP偶爾會停止工作（可能是該芯片）。最后，MPU6050給出了最佳結(jié)果。 （其他優(yōu)點(diǎn)：這些模塊是最小，最薄且價格非常低的模塊。）

如果MPU6050產(chǎn)生錯誤的數(shù)據(jù)，則可能需要校準(zhǔn)傳感器的x，y，z偏移量。可以在此處找到用于校準(zhǔn)偏移量的Arduino草圖

我經(jīng)歷過，調(diào)整偏移量可以提高小角度的精度，但也會降低大角度的精度。到最后，芯片默認(rèn)偏移量在大多數(shù)情況下效果最好。

電動機(jī)，編碼器，電動機(jī)驅(qū)動器和車輪

我使用的電動機(jī)（全部為Pololu）：

大功率Micro no 1101，990 rpm，齒輪：100.37：1，失速扭矩：30 oz/in

大功率Micro no 2271，990 rpm，齒輪：9.68：1，失速扭矩： 39 oz/in

大功率no 2274，210 rpm，齒輪：100：1，失速轉(zhuǎn)矩：80 oz/in

小功率no 2282，590 rpm，齒輪：9.68 ：1，失速轉(zhuǎn)矩：17 oz/in

除微型版本外，所有電機(jī)均隨附霍爾編碼器。它們產(chǎn)生易于操作的分辨率，取決于齒輪比，分辨率為464至2248節(jié)拍。對于微型馬達(dá)，我使用了產(chǎn)生48個刻度的光學(xué)編碼器。

在我看來，在室內(nèi)使用扭矩不太重要。我也經(jīng)歷了高功率版本，因?yàn)檎{(diào)音更難。它們具有更大的靜摩擦力（“死功率區(qū)” =開始移動所需的功率），因此，我發(fā)現(xiàn)很難以很小的平衡差（即小的垂直角度）來調(diào)節(jié)功率。轉(zhuǎn)速與最大速度直接相關(guān)。在我看來，對于首次建立平衡器也不太重要。但是，極低的轉(zhuǎn)速幾乎不可能達(dá)到穩(wěn)定的平衡。我的建議：使用普通電動機(jī)，最好以600 rpm左右的速度運(yùn)轉(zhuǎn)。

總體而言，盡管來自相同系列的直流電動機(jī)顯示出不同且不一致的靜摩擦力。內(nèi)置差動控制被證明是必不可少的。也許步進(jìn)電機(jī)將減少此問題（它們移動直到到達(dá)某個位置或角度）。步進(jìn)電機(jī)也消除了對編碼器的需求。

我已經(jīng)使用L298N雙（2A）電動機(jī)控制器板和Seeed Studio 4A電動機(jī)控制器護(hù)罩進(jìn)行了測試。我更喜歡后者，因?yàn)樗嵌芘?。易于安裝，占用空間少。此外：我發(fā)現(xiàn)屏蔽罩4Amp可以更好地緩沖大功率電機(jī)的堵轉(zhuǎn)電流。 （注意：請注意我的Arduino草圖中的引腳排列。我體驗(yàn)到的屏蔽線與Seeed示例不同。）

輪子 您的生活輕松。我使用的直徑分別為4.2、9和12厘米。大輪轂帶來更大的穩(wěn)定性。這也適用于表面上的抓地力。我的最愛：“野生ump擊器”車輪（12x6厘米）。

身高和體重 對機(jī)器人的行為也很重要。較大的重量會使機(jī)器人保持惰性：反應(yīng)緩慢，因此一開始會顯示較大的過沖。當(dāng)然也需要大功率的電動機(jī)。重心（COG）較高將使機(jī)器人更加穩(wěn)定，但也可能會導(dǎo)致后果。高度加上頂部的重量也會使機(jī)器人保持惰性。因此，這完全是調(diào)整的問題。當(dāng)機(jī)器人保持穩(wěn)定平衡時，如果發(fā)現(xiàn)它在向前或向后移動時反應(yīng)緩慢，請嘗試減輕重量和/或減輕重量。

電源

該機(jī)器人由兩節(jié)LiPo電池供電。一種用于為Arduino和電機(jī)供電。一種用于為Raspberry供電，并通過它與HC-SR04超聲波傳感器配合使用。我從所有組件的一個來源開始。當(dāng)電動機(jī)處于“重載”狀態(tài)時，它們吸收了太多電流，以至于超聲波讀數(shù)變得錯誤，甚至RPi有時也會復(fù)位。為RPi和Ultraborgs使用單獨(dú)的LiPo解決了這一問題，但我必須添加UBEC才能獲得5V 3A的穩(wěn)定電流。

步驟2：邏輯和編碼

網(wǎng)上有很多有關(guān)平衡機(jī)器人和/或倒立擺的信息。但是，我使用了一個非常簡化的邏輯，并具有以下假設(shè)：

1-電機(jī)軸應(yīng)該旋轉(zhuǎn)的角度（至少）與機(jī)器人掉落的角度相同。因此，請嘗試控制電動機(jī)軸的角度，而不是距離和/或速度。 （如前所述，使用步進(jìn)電機(jī)可能更容易實(shí)現(xiàn)。）

2-為了平衡，需要控制角度及其導(dǎo)數(shù)，即電機(jī)軸的角速度。這可以通過使用PD控制回路來完成。它不像PID回路那樣常見，但是由于運(yùn)動方向會迅速變化，因此使用積分因子幾乎沒有用處。

3 –要保持位置，必須控制水平移動和速度。這可以通過將傳感器（編碼器）輸出轉(zhuǎn)換為補(bǔ)償角度來完成。出于與垂直控制相同的原因，所需的控制循環(huán)也是PD循環(huán)。

可以簡單地將2和3的結(jié)果相加，但是最好在水平方向使用嵌套（級聯(lián)）控制循環(huán)控制回路為垂直控制回路產(chǎn)生目標(biāo)角度。

按照這些假設(shè)進(jìn)行操作可以節(jié)省機(jī)器人的高度和質(zhì)量（甚至是線速度）的計算負(fù)擔(dān)（如上所述：這些是重要因素，但可以輕松調(diào)整）。這樣，在不更改（大部分）編碼參數(shù)的情況下調(diào)整機(jī)器人的高度和COG相對容易。

保持機(jī)器人直面是通過差動控制（轉(zhuǎn)彎）完成的，并且可以獨(dú)立于機(jī)器人水平和垂直控制。輸入可以來自編碼器（即滴答聲數(shù)量的差異）或來自IMU（即偏航數(shù)據(jù)）。后者所需的編碼要少得多。

要對其進(jìn)行總結(jié)，為了平衡，需要：

1將水平位移轉(zhuǎn)換為垂直位移角度

2補(bǔ)償該角度與測得的角度之差

3將電機(jī)軸移動此補(bǔ)償角度

要翻譯成代碼，需要機(jī)器人的某些功能：

最大電動機(jī)速度以編碼器刻度表示。這是編碼器可以在特定電機(jī)上產(chǎn)生的最大滴答聲。在公式中： RPM/60 *每轉(zhuǎn)刻度數(shù)

編碼器刻度數(shù)和度之間的關(guān)系。在公式中： 360/每轉(zhuǎn)刻度數(shù)

使用這兩個公式，我們可以推論度數(shù)與電動機(jī)功率之間的關(guān)系（PWM值0-255）。

Arduino草圖作為具有以下功能的機(jī)器人“固件”運(yùn)行：

根據(jù)車輪編碼器的輸入進(jìn)行水平控制

垂直和差分基于IMU輸入（俯仰和偏航）的控制

用于調(diào)整，輸出監(jiān)視和運(yùn)動測試的藍(lán)牙通信

與Raspberry Pi的串行通信

草圖在這里。用于遠(yuǎn)程控制和調(diào)整的android應(yīng)用程序是使用MIT App Inventor開發(fā)的，可以在此處找到這些塊。用于在Raspberry和Arduino之間進(jìn)行通信和控制的Python腳本位于同一存儲庫中。我不會在此贅述，因?yàn)樵撎觾H用于平衡基本面。

有關(guān)Arduino草圖的注釋很好。因此，我將只介紹一下其功能。

初始化

UNO的插針已被占用，沒有將傳感器連接到Arduino的空間很大（pwm，中斷和模擬引腳已售罄）。

一些機(jī)器人/電機(jī)常數(shù)進(jìn)行計算（如上所述）。

松弛或靜摩擦常數(shù)。通過試用和測試收集。我從草圖開始，以獲取空載條件下的平均值。您會在這里找到它。在實(shí)踐中，我在測試平衡和運(yùn)動的同時根據(jù)觀察結(jié)果對它們進(jìn)行了重大調(diào)整。這些常數(shù)就足夠了，但是在很大程度上取決于電動機(jī)的性能。跳過這些功率量會使機(jī)器人在（很?。┬〗嵌葧r反應(yīng)更快。但是將這些值設(shè)置得太高會在調(diào)整控制循環(huán)時帶來一些挑戰(zhàn)！

maxAngle

假設(shè)無法進(jìn)行補(bǔ)償?shù)慕嵌?。用于確定機(jī)器人是站立還是躺下。

間隔

穩(wěn)定的間隔對于控制回路至關(guān)重要。在Arduino站點(diǎn)上有一個“ TimedAction”庫，但是一個計時器的直接編碼非常簡單，并且節(jié)省了一些內(nèi)存。

pitchOffset

通過調(diào)整IMU獲得。最簡單的方法是將機(jī)器人固定在絕對90度的位置（由頂部的數(shù)字水平儀測量）并讀取IMU值。

cogOffset

沒有此常數(shù)，控制回路也將獲得平衡。第一個原型機(jī)帶有8個超聲波傳感器，這使它的前部有點(diǎn)沉重。正如您在視頻中看到的那樣，控制循環(huán)通過讓機(jī)器人稍微向后傾斜來進(jìn)行調(diào)整。使用 cogOffset 可以補(bǔ)償傾斜，并使機(jī)器人前進(jìn)時更加靈活。

序列。 begin（）和Bluetooth.begin（）

與RPi的通信通過USB串行進(jìn)行。 SoftwareSerial接線庫用于與Android應(yīng)用進(jìn)行藍(lán)牙通信。

wdt_enable（WDTO_500MS）

初始化IMU錯誤時，設(shè)置了Averdude看門狗定時器，這將強(qiáng)制Arduino復(fù)位。

resetRobot（）

每次運(yùn)行時都會初始化所有變量（并停止電動機(jī)）機(jī)器人放下。這對拾起機(jī)器人很有幫助。

loop（）

只要機(jī)器人站立時，將檢查所有輸入。當(dāng)間隔過去后，將執(zhí)行控制循環(huán)。我嘗試了不同的時間間隔。最佳選擇取決于傳感器和所用電機(jī)的類型。超過100Hz（《0.01秒）時，我遇到了錯誤的編碼器數(shù)據(jù)，并且機(jī)器人的動作也太“辣”了。在50Hz時，機(jī)器人變得“笨拙”。在大多數(shù)情況下，100Hz環(huán)路效果最佳。

horizontalControl（）

檢查是否已經(jīng)確定了目標(biāo)距離，并向目標(biāo)刻度添加了速度偏移。速度偏移是在間隔內(nèi)以特定速度進(jìn)行的滴答聲數(shù)量。如果未設(shè)置距離和速度目標(biāo)，則在機(jī)器人上升時設(shè)置目標(biāo)刻度數(shù)。這使機(jī)器人可以返回其位置（如果命令移動一定距離，則可以返回新位置）。

differentialControl（）

這對于IMU的偏航輸出有點(diǎn)特殊在這里使用。 IMU返回-π《=偏航《=π（弧度）。

getAngles（） 例程將讀數(shù)轉(zhuǎn)換為以度為單位的角度0-360范圍。

resetRobot（） 例程將機(jī)器人上升時的偏航角設(shè)置為yawTarget（基本上是360度刻度上的角度偏移量） ）。

最后一個角度讀數(shù)和偏移之間的差被視為錯誤。

冒充360度的刻度（例如在指南針上）可以輕松確定最小角度（=旋轉(zhuǎn)方向）?？梢愿鶕?jù)向機(jī)器人補(bǔ)償?shù)姆较颍ㄗ?右）以及機(jī)器人必須移動的方向（向前/向后），簡單地將結(jié)果添加到發(fā)送到各個電動機(jī)的功率中或從中減去。

僅當(dāng)在 getMessage（） 例程中通過藍(lán)牙或從Raspberry接收命令時，才會執(zhí)行以下3個例程。

setSpeed（）

取一個電機(jī)一次可產(chǎn)生的最大滴答聲的百分比（ = RPM/60 *每轉(zhuǎn)刻度數(shù)*時間間隔（秒）。）

setDistance（）

將以米為單位的行駛距離乘以每米的刻度數(shù)（ =每轉(zhuǎn)刻度數(shù)/以米為單位的車輪周長）

setTurn（）

為偏航偏移量添加所需的度數(shù)，將新目標(biāo)調(diào)整為360度并重置轉(zhuǎn)彎命令以防止連續(xù)轉(zhuǎn)彎。

傳感器和電機(jī)例程非常簡單。 resetEncoders（） 例程可防止由于編碼器“翻轉(zhuǎn)”（編碼器輸出為整數(shù)）而引起的事故。

交互例程檢查串行和藍(lán)牙隊(duì)列中是否有新命令，如果有，則通過通用的案例結(jié)構(gòu)對其進(jìn)行處理。

第3步：調(diào)整控制循環(huán)

出于明顯的原因，PID調(diào)整有時被稱為“黑手藝”。這個過程可能非常繁瑣和令人沮喪。盡管有許多獲取控制器值的半結(jié)構(gòu)化方法，但為獲得機(jī)器人平衡，幾乎總是不可避免地要進(jìn)行手動調(diào)整。

一些注意事項(xiàng)將在這里有所幫助：

此過程需要耐心和觀察：如果更改增益值，可以觀察到哪種行為？

比例收益代表補(bǔ)償?shù)?P 下位。即所供應(yīng)的電動機(jī)功率。

微分增益代表 D 放大所供電功率的效果。

請記住，這一切都與加速度有關(guān)。所提供的動力很快就會加速。

從兩個增益都為零開始，然后以小步長開始增加 P （向下）。當(dāng)漫游器自行停留或多或少時停止。也許太“搖晃”或“搖擺”，但它應(yīng)該使自己保持幾秒鐘。 （通常稱為“ 穩(wěn)定振蕩”。）

如果您真的無法獲得穩(wěn)定振蕩，則可能是物理原因造成的。某些可能的信號是：振幅不均勻（可能表示齒輪，質(zhì)量或高度問題）或僅沿一個方向移動（可能是缺乏電機(jī)動力）

機(jī)器人在劇烈振動時（機(jī)器人向前和向后移動），并且移動變得越來越野，加速度太大，因?yàn)橐呀?jīng)提供了太多的動力。

當(dāng)機(jī)器人開始越來越快地運(yùn)行時，在一個方向上以崩潰告終，由于電力供應(yīng)不足，機(jī)器人無法充分加速。

穩(wěn)定振蕩意味著不斷的超調(diào)，就像坐在秋千上一樣。添加 D （放大）后，過沖將減少（并最終消失）。

使用小步長，將值保持盡可能小，并徹底觀察效果。

當(dāng)可以從外部調(diào)整增益時，生活會變得不那么艱難。草圖（否則必須一遍又一遍地編輯，重新編譯和重建草圖）。我從2個微調(diào)電位器開始調(diào)整增益。我發(fā)現(xiàn)它們很難使用。因?yàn)槲乙婚_始對增益值一無所知，所以我不得不多次編輯草圖以進(jìn)行新的縮放（？。？？？？：1024），并且在沒有直接視覺反饋的情況下，我不知道這些更改我是通過轉(zhuǎn)動花盆來做的。

Android應(yīng)用最適合我：幾乎所有常量都可以通過輸入一個值來更改。屏幕頂部概述了所有當(dāng)前值。在屏幕底部，可以得到誤差或角度本身的圖形顯示。請注意，這樣的顯示只是一個延遲的指示（串行和Wifi），而不是真實(shí)的實(shí)時反饋。該應(yīng)用程序還可以用于測試一些命令（距離，速度，轉(zhuǎn)彎）。

一旦我達(dá)到了球場標(biāo)準(zhǔn)，我就需要一些半自動化的測試工具，因?yàn)閹缀跛懈臋C(jī)器人或其代碼或多或少需要重新調(diào)整。特別是在水平控制回路上工作需要多次。

我編寫了一個Python腳本，該腳本使用了通常稱為“旋轉(zhuǎn)”算法的協(xié)調(diào)上升算法?？梢栽谶@里找到Sebastian Thrun（Udacity，Google，Stanford）對算法的解釋

可以在此處找到腳本：twiddleVert.py

我嘗試在其中構(gòu)建算法Arduino草圖。盡管它只是安裝在可用的32K內(nèi)存中，但它使整體周期延遲太多。因此，該腳本旨在在Raspberry上運(yùn)行，并使用其他一些Python腳本：

control.py，用于向Arduino和與UltraBorgs的I2C通信發(fā)送命令。如果未附加Ultraborg，則最好將導(dǎo)入和初始化注釋掉。否則，還需要UltraBorg.py。

messaging.py是用于與Arduino進(jìn)行實(shí)際通信的線程類

linkingStorage.py，它充當(dāng)主腳本與主腳本之間的共享內(nèi)存。線程腳本

measurements.py，用于收集，顯示和保存所有測試數(shù)據(jù)。

注意：

Python腳本將根據(jù)進(jìn)一步的實(shí)驗(yàn)而更改。如果您想避免更改，請克隆存儲庫。

我認(rèn)為現(xiàn)在已經(jīng)涵蓋了所有基本知識，并有望幫助您構(gòu)建自己的平衡器。