步驟1：硬件

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

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

MPU6050

Pololu低功率直流電動(dòng)機(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

慣性測(cè)量單元（IMU）

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

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

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

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

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

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

我使用的電動(dòng)機(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é)拍。對(duì)于微型馬達(dá)，我使用了產(chǎn)生48個(gè)刻度的光學(xué)編碼器。

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

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

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

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

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

電源

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

步驟2：邏輯和編碼

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

與Raspberry Pi的串行通信

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

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

初始化

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

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

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

maxAngle

假設(shè)無(wú)法進(jìn)行補(bǔ)償?shù)慕嵌取Ｓ糜诖_定機(jī)器人是站立還是躺下。

間隔

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

pitchOffset

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

cogOffset

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

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

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

wdt_enable（WDTO_500MS）

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

resetRobot（）

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

loop（）

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

horizontalControl（）

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

differentialControl（）

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

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

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

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

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

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

setSpeed（）

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

setDistance（）

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

setTurn（）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

我編寫了一個(gè)Python腳本，該腳本使用了通常稱為“旋轉(zhuǎn)”算法的協(xié)調(diào)上升算法。可以在這里找到Sebastian Thrun（Udacity，Google，Stanford）對(duì)算法的解釋

可以在此處找到腳本：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，用于收集，顯示和保存所有測(cè)試數(shù)據(jù)。

注意：

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

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