基于角動(dòng)量守恒的位姿平衡系統(tǒng)設(shè)計(jì)＊

劉江昊，杜艷紅，董禹呈

（天津農(nóng)學(xué)院工程技術(shù)學(xué)院，天津 300384）

目前，隨著計(jì)算機(jī)技術(shù)、集成電路和通訊技術(shù)的飛速發(fā)展，被動(dòng)穩(wěn)定逐步發(fā)展為半主動(dòng)控制，以增加穩(wěn)定性、提高姿態(tài)精度、調(diào)節(jié)指向等為目標(biāo)[1]。動(dòng)量輪實(shí)現(xiàn)的姿態(tài)控制方法稱(chēng)為自旋穩(wěn)定，具有原理簡(jiǎn)單、效果好、成本低等優(yōu)點(diǎn)，被廣泛應(yīng)用于人造衛(wèi)星和航天飛機(jī)的姿態(tài)調(diào)整，大大提高了飛行器的發(fā)射成功率，減輕了研究人員的負(fù)擔(dān)，提高了系統(tǒng)的穩(wěn)定性。

1 設(shè)計(jì)原理

設(shè)計(jì)三維空間上控制角動(dòng)量保持平衡時(shí)，以A 電機(jī)為例，通過(guò)控制A 電機(jī)帶動(dòng)飛輪左右旋轉(zhuǎn)產(chǎn)生加速度來(lái)控制平衡[2]，如圖1 所示。

圖1 電機(jī)定義

三維空間上控制角動(dòng)量保持平衡時(shí)，需調(diào)用A、B、C 這3 路電機(jī)，如圖2 所示。其中A 電機(jī)控制Y軸平衡，B、C 電機(jī)控制X軸平衡，A、B、C 這3 個(gè)電機(jī)共同控制Z軸旋轉(zhuǎn)，原理仍為角動(dòng)量平衡，僅增加了2 個(gè)軸的控制，其中A 電機(jī)控制Y軸，即左右傾斜，B、C 電機(jī)合力控制X軸，即前后傾斜，3 個(gè)電機(jī)又額外控制Z軸旋轉(zhuǎn)。

圖2 三維空間定義

在不考慮系統(tǒng)冗余度的情況下，本次設(shè)計(jì)采用3個(gè)正交安裝的動(dòng)量輪，與機(jī)體X、Y、Z這3 個(gè)軸平行，正常狀態(tài)為3 個(gè)正交安裝的反作用輪工作[3]。

2 平衡系統(tǒng)設(shè)計(jì)

2.1 系統(tǒng)結(jié)構(gòu)

基于蘇黎世聯(lián)邦理工學(xué)院所研發(fā)的Cubli 方形機(jī)器人的機(jī)械結(jié)構(gòu)設(shè)計(jì)了以STM32 單片機(jī)作為核心處理器的平衡系統(tǒng)，主要包括電源電路、電機(jī)控制電路、無(wú)刷電機(jī)、動(dòng)量輪，如圖3 所示。電源電路采用AMS1117 芯片和LM2596 芯片；角動(dòng)量平衡系統(tǒng)則采用Nidec 生產(chǎn)的功率較大且自帶電機(jī)驅(qū)動(dòng)的24H404H無(wú)刷電機(jī)；動(dòng)量輪結(jié)構(gòu)為合金制的輪形結(jié)構(gòu)，在確保能夠提供足夠大的動(dòng)量基礎(chǔ)上整機(jī)結(jié)構(gòu)不會(huì)過(guò)于沉重。

圖3 系統(tǒng)結(jié)構(gòu)圖

2.2 位姿信息采集

設(shè)計(jì)主要使用MPU6050 傳感器來(lái)獲得動(dòng)量輪在XYZ平面內(nèi)的位姿信息，通過(guò)IIC（Inter-Integrated Circuit）接口向主控器輸出姿態(tài)數(shù)據(jù)，并將傳感器直接與MPU6050 掛載在同一條總線(xiàn)上，使用主機(jī)直接進(jìn)行控制。

2.3 平衡控制

本設(shè)計(jì)采用的電機(jī)自帶電機(jī)驅(qū)動(dòng)，因此只需控制器輸出PWM（Pulse Width Modulation）信號(hào)控制即可，由于轉(zhuǎn)子后端具有光柵傳感器，因此控制器也可實(shí)時(shí)獲得電機(jī)的運(yùn)行狀態(tài)并根據(jù)狀態(tài)進(jìn)行控制。在獲得光柵傳感器所捕獲的電機(jī)運(yùn)行數(shù)據(jù)后，經(jīng)過(guò)一次低通濾波得到的電機(jī)數(shù)據(jù)傳遞給主控并參與電機(jī)控制的解算。電機(jī)控制算法主要利用了PID（Proportion Integration Differentiation）控制進(jìn)行調(diào)節(jié)，其控制方案如圖4 所示。

圖4 控制方案

3 軟件流程

首先進(jìn)行系統(tǒng)初始化，系統(tǒng)控制器依據(jù)傳感器采集到的數(shù)據(jù)對(duì)系統(tǒng)進(jìn)行實(shí)時(shí)控制，主要包括電機(jī)的速度控制、位姿控制和電流控制，進(jìn)而調(diào)節(jié)系統(tǒng)的姿態(tài)使它達(dá)到動(dòng)態(tài)平衡[4]，總體流程如圖5 所示。

圖5 軟件流程圖

4 樣機(jī)制作及測(cè)試

采用了集成化的設(shè)計(jì)，將控制電路及電源電路整合，本機(jī)本體即為電路主體。樣機(jī)主要由3 部分組成，前板為主控單元電路、MPU6050 傳感器和電源電路，中層為連接電機(jī)和動(dòng)量輪的電機(jī)輸出接口，控制電機(jī)為整機(jī)提供動(dòng)力力矩，背板連接電池組，為整機(jī)提供電源支持。3 部分由M3 銅柱連接，以此保證樣機(jī)結(jié)構(gòu)的強(qiáng)度。位姿平衡裝置結(jié)構(gòu)如圖6 所示。

圖6 位姿平衡裝置結(jié)構(gòu)圖

在樣機(jī)平衡時(shí)測(cè)得控制曲線(xiàn)如圖7 所示，可見(jiàn)系統(tǒng)達(dá)到位姿平衡，滿(mǎn)足設(shè)計(jì)要求。

圖7 樣機(jī)PID 控制曲線(xiàn)

在樣機(jī)平衡時(shí)對(duì)它進(jìn)行擾動(dòng)實(shí)驗(yàn)，角度動(dòng)量輪在XYZ平面輸出曲線(xiàn)，如圖8—圖10 所示。

圖8 X 軸動(dòng)量輪輸出曲線(xiàn)

圖9 Y 軸動(dòng)量輪輸出曲線(xiàn)

圖10 Z 軸動(dòng)量輪輸出曲線(xiàn)

由圖8—圖10 可以看出，樣機(jī)可以保持平衡，且在受到一定擾動(dòng)時(shí)仍能夠擁有較好的魯棒性，說(shuō)明PID控制算法對(duì)樣機(jī)整體有較好的適應(yīng)性與控制性，驗(yàn)證了設(shè)計(jì)和控制算法在一定程度上的可行性。

5 結(jié)論

本文基于角動(dòng)量守恒定律設(shè)計(jì)了一種動(dòng)量輪平衡的位姿控制器，以整機(jī)的角速度與角度作為主要控制量，以電機(jī)的轉(zhuǎn)速及它產(chǎn)生的角動(dòng)量作為次變量構(gòu)成了PID 控制，即使整機(jī)受到一定程度的干擾時(shí)，仍能保持較好的魯棒性。