3兩輪自平衡機(jī)器人的控制系統(tǒng)設(shè)計(jì)

????3.1控制系統(tǒng)的硬件設(shè)計(jì)

????兩輪自平衡機(jī)器人Opyanbot的硬件主要由電源管理模塊、測(cè)量單元、運(yùn)動(dòng)控制單元、伺服電機(jī)和主控機(jī)組成，如圖2所示。
?

圖 2：兩輪自平衡機(jī)器人硬件結(jié)構(gòu)圖

　????電源管理模塊提供各單元所需電壓；測(cè)量單元包括傾角傳感器、陀螺儀和電機(jī)碼盤，分別測(cè)量機(jī)器人豎直傾斜角度、豎直水平轉(zhuǎn)動(dòng)角速度和兩個(gè)電機(jī)各自的位置和轉(zhuǎn)速，測(cè)量單元將數(shù)據(jù)傳入運(yùn)動(dòng)控制單元。運(yùn)動(dòng)控制單元是基于DSP的IPM100全數(shù)字智能伺服驅(qū)動(dòng)單元，內(nèi)嵌高水平的Technosoft運(yùn)動(dòng)語(yǔ)言，可以按照內(nèi)部EEROM的程序獨(dú)立運(yùn)行，或者在主控制器的監(jiān)督下利用串口通訊運(yùn)行命令程序，通過(guò)內(nèi)設(shè)閉合電流環(huán)、位置環(huán)和電流環(huán)，精確控制伺服電機(jī)的工作狀態(tài)，如圖3所示；
?

圖 3：IPM100 閉環(huán)控制方框圖

?????主控機(jī)內(nèi)寫有控制算法模塊和指令模塊，可以通過(guò)RS485實(shí)時(shí)采集由運(yùn)動(dòng)控制單元傳入的測(cè)量信號(hào)，并向運(yùn)動(dòng)控制單元發(fā)送命令。
????
????3.2 控制算法的設(shè)計(jì)

????3.2.1平衡控制

????兩輪自平衡機(jī)器人是一個(gè)多輸入、單輸出系統(tǒng)，分別以機(jī)器人豎直角度、豎直方向角速度和電機(jī)轉(zhuǎn)速為輸入，車輪轉(zhuǎn)動(dòng)角加速度為輸出，

????由上面得到的微分方程，得出狀態(tài)方程：??????????????
??????????????????（4）

????其中：
???
?????????????????????（5）?

????利用線性系統(tǒng)的可控性秩判據(jù)rank(B AB A²B)=3，系統(tǒng)可控；利用線性系統(tǒng)的可觀性秩判據(jù)rank(C CA CA²)=3，系統(tǒng)可觀。因此近似線性化系統(tǒng)狀態(tài)完全可控可觀，系統(tǒng)滿足最有控制使用條件。[4]

????下面我們應(yīng)用LQR 控制算法對(duì)其進(jìn)行計(jì)算，設(shè)最優(yōu)控制向量的矩陣K：

??????????????（6）?
?
????u_b(t)??是滿足兩輪自平衡機(jī)器人處于平衡狀態(tài)的加速度，這樣就得到了滿足平衡控制要求的控制量。
????
????3.2.2 直行、轉(zhuǎn)彎控制

????兩輪自平衡機(jī)器人的其他運(yùn)動(dòng)（直行、轉(zhuǎn)彎），都是在這個(gè)控制量的基礎(chǔ)上進(jìn)行加減。具體控制方法如圖４所示：
?

圖 4：兩輪自平衡機(jī)器人控制結(jié)構(gòu)圖

????機(jī)器人的行進(jìn)，是在機(jī)器人平衡的基礎(chǔ)上，通過(guò)給兩輪增加相同的控制量u_s來(lái)實(shí)現(xiàn)的。由于兩輪分別獨(dú)立驅(qū)動(dòng)，即便獲得同樣的直行信號(hào)u_s，也不能保證兩輪行進(jìn)速度始終相同，行進(jìn)路線保持直線，這樣就需要機(jī)器人對(duì)于行進(jìn)方向具有自動(dòng)校正能力。

????機(jī)器人直線行走模型是一個(gè)雙輸入、單輸出系統(tǒng)，輸入它與水平轉(zhuǎn)速和兩輪行進(jìn)距離的差成線性關(guān)系：
??????????????????（7）
????
????水平轉(zhuǎn)速可以通過(guò)陀螺儀的水平分量得到，兩輪行進(jìn)距離可以通過(guò)編碼器讀到，利用最優(yōu)控制算法同樣可以得到系數(shù)h₁、h₂的值，從而得到用于校正行進(jìn)方向的輸出的。

????機(jī)器人的轉(zhuǎn)向運(yùn)動(dòng)，可以通過(guò)兩輪差動(dòng)方法實(shí)現(xiàn)，分別給左右兩輪加上一對(duì)大小相同、方向相反的控制量u_r。

????這樣就可以得到兩輪自平衡機(jī)器人總輸出：

?????????????????（8）

3.3 機(jī)器人運(yùn)動(dòng)的精確控制

????本機(jī)器人由于采用基于DSP數(shù)字電路的全數(shù)字智能伺服驅(qū)動(dòng)單元IPM100作為運(yùn)動(dòng)控制器，并且可以利用上位機(jī)編寫控制復(fù)雜的控制指令，對(duì)機(jī)器人可以進(jìn)行實(shí)時(shí)控制，因此精確的控制機(jī)器人的運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)軌跡成為可能。

????機(jī)器人速度精確控制可以通過(guò)改變（6）中狀態(tài)變量v來(lái)實(shí)現(xiàn)。如果目標(biāo)速度是v_t，則通過(guò)

??????????????（9）???

????可以使移動(dòng)速度穩(wěn)定在v_t。如果目標(biāo)加速度是u_t，只需令

????u_s?= u_t????????????????????????????????(10)?

????可以使機(jī)器人按照加速度u_t加速。轉(zhuǎn)動(dòng)角度可以通過(guò)改變方程（7）來(lái)實(shí)現(xiàn)，如果需要向右轉(zhuǎn)動(dòng)，則

??????????????（11）

????這樣就實(shí)現(xiàn)了速度、加速度、轉(zhuǎn)角控制的精確控制。

????通過(guò)讀兩輪編碼器，可以得到機(jī)器人位置、朝向、運(yùn)動(dòng)速度等信息。這樣就可以通過(guò)編寫程序，讓機(jī)器人按照預(yù)先指定的運(yùn)動(dòng)狀態(tài)和路線運(yùn)動(dòng)。

????4 仿真與實(shí)驗(yàn)結(jié)果

????可以利用Matlab函數(shù)求得矩陣：調(diào)整對(duì)角矩陣Q中的參數(shù)即對(duì)輸入量的加權(quán)值，得到不同的仿真結(jié)果，進(jìn)而得到最終的參數(shù)。

????在初始值為時(shí)的仿真曲線如圖5所示：

?

圖 5 兩輪自平衡機(jī)器人平衡仿真曲線

????從仿真中可以看到豎直偏離角度在0.8秒時(shí)歸零，角速度也在5秒內(nèi)歸零，機(jī)器人重新回到穩(wěn)態(tài)平衡狀態(tài)。
?

圖 6 兩輪自平衡機(jī)器人Opyanbot
?

????Opyanbot的上位機(jī)使用PC機(jī)、應(yīng)用VC編寫控制程序、RS485傳輸。
????
????平衡試驗(yàn)：

????根據(jù)測(cè)試，從IPM板向上傳輸?shù)絀PM接受到指令需要耗費(fèi)存在大約50ms的時(shí)間，但是不影響平衡控制效果，并且總延遲在80ms內(nèi)可以保證平衡控制效果。
?

圖 7??機(jī)器人有外部干擾時(shí)的豎直傾角曲線

????在外界干擾使機(jī)器人豎直傾角達(dá)到時(shí)，機(jī)器人可以在3秒內(nèi)重新回到平衡狀態(tài)，如圖7所示，說(shuō)明機(jī)器人具有很好的魯棒性。在穩(wěn)定狀態(tài)時(shí)豎直擺動(dòng)角度在以內(nèi)，機(jī)器人控制過(guò)程平緩柔和。

????行進(jìn)試驗(yàn)：

????機(jī)器人最大時(shí)速可以超過(guò)10km/h，最大加速度0.5m/s²。在直線行進(jìn)實(shí)驗(yàn)中，20米的行進(jìn)距離偏移角度5%以內(nèi)。

????機(jī)器人可以以任何半徑轉(zhuǎn)彎，零半徑原地轉(zhuǎn)動(dòng)最小需時(shí)2.1s。

5 結(jié)論

????本文針對(duì)兩輪自平衡機(jī)器人在實(shí)際應(yīng)用中存在的問(wèn)題，應(yīng)用最優(yōu)控制及兩輪差動(dòng)等控制方法設(shè)計(jì)了控制器，提出了針對(duì)兩輪自平衡機(jī)器人平衡和行進(jìn)的新策略。為了提高兩輪自平衡機(jī)器人的控制效果，利用基于DSP數(shù)字電路的全數(shù)字智能伺服驅(qū)動(dòng)單元IPM100分別精確控制左右輪電機(jī)，并利用上位機(jī)實(shí)時(shí)控制機(jī)器人的運(yùn)動(dòng)狀態(tài)，提高了控制精度、可靠度以及集成度，最終得到了很好的控制效果，最終提高了控制精度、可靠度以及集成度。本文可以為更為復(fù)雜的、具有其他用途的自平衡機(jī)器人提供借鑒。