一種雙車把兩輪車機(jī)器人測控系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)

黃用華，楊 煉，何淑通，莊 未

（桂林電子科技大學(xué)機(jī)電工程學(xué)院，廣西 桂林541004）

0 引言

雙車把兩輪車機(jī)器人結(jié)合了自行車和Segway的結(jié)構(gòu)特點(diǎn)和運(yùn)動(dòng)特性，是一種可滿足不同地形和任務(wù)要求的新式平衡車。

目前，對自行車機(jī)器人和Segway平衡車的平衡研究取得了諸多成果。韓國學(xué)者Lee SI[1]針對自行車機(jī)器人，開發(fā)了一種平衡駕駛控制系統(tǒng)，利用輪擺的反作用力進(jìn)而實(shí)現(xiàn)自行車在行駛時(shí)自動(dòng)平衡。日本學(xué)者Yang C[2]提出了一種新型的無車把自平衡電動(dòng)摩托車，其實(shí)現(xiàn)車體自平衡的措施是調(diào)節(jié)車輪的速度。泰國學(xué)者Suebsomran[3]設(shè)計(jì)了一款自行車機(jī)器人平衡動(dòng)態(tài)補(bǔ)償裝置，主要是利用陀螺效應(yīng)原理，通過產(chǎn)生的扭矩進(jìn)行車體平衡調(diào)節(jié)。Suryanrayanan[4]發(fā)現(xiàn)當(dāng)自行車機(jī)器人運(yùn)動(dòng)速度過高時(shí)，車把的轉(zhuǎn)角對車體平衡的影響較大，遂提出高速低速分別控制的方法。對于Segway的平衡研究，Kamen立足于倒立擺原理，率先發(fā)明了一種名為Segway HT[5]的兩輪車，進(jìn)而對該平衡車進(jìn)行了商業(yè)化，并取得成功。本文研究的雙車把兩輪車機(jī)器人是近幾年的新式平衡車[6-8]，前期北京郵電大學(xué)郭磊、桂林電子科技大學(xué)黃用華從結(jié)構(gòu)設(shè)計(jì)、模型建立、定位平衡控制方面做了一系列仿真與實(shí)驗(yàn)研究，為雙車把兩輪車機(jī)器人的結(jié)構(gòu)設(shè)計(jì)以及平衡運(yùn)動(dòng)控制研究提供了借鑒意義。但進(jìn)行平衡控制的前提是具有可靠的測控系統(tǒng)，基于此設(shè)計(jì)一種采用工控機(jī)作為主控單元，DSP（TMS320F28-335）芯片作為下層控制單元的分布式測控系統(tǒng)，以雙車把兩輪車機(jī)器人作為研究載體，利用慣性測量單元、光電編碼器、電流傳感器等多種傳感器融合進(jìn)而檢測系統(tǒng)狀態(tài)，PC端與控制終端的遠(yuǎn)程通訊利用ZigBee無線模塊來實(shí)現(xiàn)?；谝陨戏桨?，完成機(jī)器人測控系統(tǒng)的搭建，并進(jìn)行試驗(yàn)驗(yàn)證該系統(tǒng)的可行性。

1 機(jī)械結(jié)構(gòu)

綜合自行車和Segway兩輪車結(jié)構(gòu)特點(diǎn)和運(yùn)動(dòng)特性，設(shè)計(jì)一款具有4個(gè)可驅(qū)動(dòng)關(guān)節(jié)的雙車把兩輪車機(jī)器人如圖1所示。

圖1 雙車把兩輪車機(jī)器人

該機(jī)器人由5個(gè)部分組成，分別為車架B1、左車把B2、右車把B3、左車輪B4和右車輪B5。在直流電機(jī)的驅(qū)動(dòng)下，左右車把可相對車架做周轉(zhuǎn)運(yùn)動(dòng)。

2 測控系統(tǒng)設(shè)計(jì)

結(jié)合雙車把兩輪車機(jī)器人的結(jié)構(gòu)特點(diǎn)，搭建如圖2所示的測控系統(tǒng)。

圖2 測控系統(tǒng)結(jié)構(gòu)框圖

圖2 中，IMU為慣性測量傳感器，用來檢測系統(tǒng)的姿態(tài)信息，編碼器用來測速以及檢測轉(zhuǎn)動(dòng)角度，由于沒有力矩傳感器，因此采用電流傳感器實(shí)現(xiàn)力矩與電流的轉(zhuǎn)換。其中，左右車把、車輪與IMU分別連接TMS320F28335，實(shí)現(xiàn)數(shù)據(jù)的采集與處理，同時(shí)5塊下層控制單元分別與主控核心工控機(jī)相連。

2.1 上位機(jī)單元

同時(shí)基于以上測控系統(tǒng)設(shè)計(jì)出上位機(jī)（工控機(jī)）的控制程序，其中工控機(jī)的參數(shù)主要包括：1.8GHZ的雙核處理器，2G運(yùn)行內(nèi)存；14個(gè)RS-232/485接口，其作用是運(yùn)行動(dòng)力學(xué)模型、控制算法，下發(fā)數(shù)據(jù)請求指令以及接收采集處理好的數(shù)據(jù)。運(yùn)行環(huán)境為Windows系統(tǒng)，采用VS2010開發(fā)上位機(jī)程序。其控制流程如圖3。

圖3 測控系統(tǒng)工作流程圖

圖3 中，首先完成程序的初始化，設(shè)置一個(gè)定時(shí)器 SetTimer（0，0，NULL）函數(shù)，由于定時(shí)時(shí)間為 0 s，系統(tǒng)會(huì)立即響應(yīng)到OnTimer（）函數(shù)，進(jìn)入另一個(gè)計(jì)時(shí)器 OnTimer（） 函數(shù)，上個(gè)定時(shí)器 SetTimer（0，0，NULL）函數(shù)由KillTimer（）進(jìn)行終止，緊接著利用QueryPerformanceCounter（）函數(shù)完成當(dāng)前CPU振蕩次數(shù)的采集，為獲取系統(tǒng)的狀態(tài)數(shù)據(jù)，通過Require－DataFromDSP（）函數(shù)實(shí)現(xiàn)DSP給傳感器發(fā)送數(shù)據(jù)采集指令來實(shí)現(xiàn)，為判斷是否完成數(shù)據(jù)的接收，利用flag標(biāo)志位進(jìn)行判別，其中1表示完成接收，0表示未完成數(shù)據(jù)接收，globalrunning（）函數(shù)為所建系統(tǒng)的數(shù)學(xué)模型，通過模型計(jì)算出最新的控制量。為避免硬件層出現(xiàn)數(shù)據(jù)傳輸不及時(shí)等問題，因此硬件等待25，以此提高系統(tǒng)的可靠性，完成上述的響應(yīng)后，將所得最新控制量通過SendCurrenttoDSP（）函數(shù)發(fā)送到對于的驅(qū)動(dòng)關(guān)節(jié)，完成系統(tǒng)的平衡控制，并對狀態(tài)數(shù)據(jù)進(jìn)行保存。ElapseTime為進(jìn)到OnTimer（）函數(shù)到當(dāng)前時(shí)刻所花費(fèi)的時(shí)間，系統(tǒng)的伺服周期用表示。

2.2 下位機(jī)單元

DSP芯片作為下位機(jī)，其功能是采集各個(gè)傳感器的數(shù)據(jù)并處理，當(dāng)上位機(jī)請求數(shù)據(jù)時(shí)，作為應(yīng)答，將采集的數(shù)據(jù)上傳給工控機(jī)；對下主要為電機(jī)驅(qū)動(dòng)器發(fā)送指令，激勵(lì)電機(jī)進(jìn)行電流控制。圖4為DSP程序的工作流程。

圖4 DSP程序工作流程

圖4 中對DSP初始化后，將各個(gè)傳感器的數(shù)據(jù)采集并打包上傳至工控機(jī)；同時(shí)，將獲得的工控機(jī)指令的進(jìn)行解析作出對應(yīng)的響應(yīng)。DSP與IMU之間的通信協(xié)議為陀螺儀內(nèi)置協(xié)議，IMU的狀態(tài)數(shù)據(jù)包括3個(gè)歐拉角、3個(gè)歐拉角速度、3個(gè)線加速度以及地磁場信息，但本系統(tǒng)主要獲取前三種狀態(tài)信息。DSP與電機(jī)驅(qū)動(dòng)器之間的通信協(xié)議采用驅(qū)動(dòng)器廠商提供的協(xié)議。工控機(jī)與DSP之間的控制指令主要包括D指令、C指令、V指令、P指令，分別表示數(shù)據(jù)采集、電流控制、速度控制以及位置控制。

3 各功能模塊

3.1 慣性測量單元

為實(shí)現(xiàn)機(jī)器人平衡運(yùn)動(dòng)控制，實(shí)時(shí)準(zhǔn)確獲取機(jī)器人的姿態(tài)信息，本測控系統(tǒng)采用荷蘭Xsens公司生產(chǎn)的MTi-G-700來檢測機(jī)器人的姿態(tài)信息。其三維視圖如圖5所示。在系統(tǒng)工作時(shí)，慣性測量單元與控制板通過RS232進(jìn)行通信。

同時(shí)該傳感器可采集系統(tǒng)3個(gè)歐拉角、3個(gè)線加速度、3個(gè)歐拉角速度以及地磁場信息，在本系統(tǒng)中，主要采集除地磁場以外的全部信息。

圖5 MT三維方向視圖

工作模式采用Xsens公司提供的通信協(xié)議，通過請求-應(yīng)答方式獲取機(jī)器人的姿態(tài)信息。其應(yīng)答接收指令每幀數(shù)據(jù)形式如圖6所示。

圖6 每幀數(shù)據(jù)組成

幀頭用來識別數(shù)據(jù)頭、數(shù)據(jù)ID以及判別數(shù)據(jù)長度。中間36個(gè)字節(jié)為陀螺儀采集的3個(gè)歐拉角、3個(gè)線加速度、3個(gè)歐拉角速度，每個(gè)數(shù)據(jù)占用4個(gè)字節(jié)，最后校驗(yàn)位用來判斷數(shù)據(jù)是否正確。

對于讀取數(shù)據(jù)，目前有兩種方式，應(yīng)答式以及連續(xù)發(fā)送。所謂應(yīng)答式，就是上位機(jī)發(fā)送請求指令，下位機(jī)應(yīng)答響應(yīng)往回發(fā)送數(shù)據(jù)，但這種效率過低；第二種是持續(xù)發(fā)送，但會(huì)存在接收數(shù)據(jù)不是完整幀的情況，根據(jù)抽屜原理，通過接收82個(gè)字節(jié)，該方法可以保證任意連續(xù)時(shí)間段，提取82個(gè)字節(jié)，一定存在一個(gè)完整的有效的數(shù)據(jù)幀，再通過搜索數(shù)據(jù)頭進(jìn)行上傳完整數(shù)據(jù)。

3.2 光電隔離模塊

為了檢測雙車把兩輪車機(jī)器人左、右車把和左、右車輪的轉(zhuǎn)速，分別裝有增量式光電編碼器，其中，為了測量左、右車把轉(zhuǎn)動(dòng)角度，分別裝有絕對式光電編碼器。編碼器用OMRON公司的E6CP-AG5C和E6B2-CWZ6C兩種型號的光電編碼器。同時(shí)由于DSP采用的是TTL電平，光電編碼器輸出信號要經(jīng)過電平轉(zhuǎn)換之后才能連接到DSPI/O輸入端，因此采用高速光耦EL6N137實(shí)現(xiàn)編碼器輸出信號快速電平轉(zhuǎn)換，圖7為單路光耦隔離模塊的工作電路：

圖7 單路光耦隔離模塊電路圖

3.3 電機(jī)驅(qū)動(dòng)模塊

在兩輪車機(jī)器人的左、右車輪和左、右車把4個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)分別安裝有直流電機(jī)。電機(jī)驅(qū)動(dòng)器型號為BDMC3610。該驅(qū)動(dòng)器與控制板通過RS232通訊口連接，而且具有支持多塊驅(qū)動(dòng)器間基于節(jié)點(diǎn)地址編址，實(shí)現(xiàn)多驅(qū)動(dòng)器訪問的特點(diǎn)?；诖四K，4個(gè)關(guān)節(jié)驅(qū)動(dòng)器與TMS320F28335的SCI_A串口通過RS232總線相連，其連接示意圖如圖8所示。

圖8 驅(qū)動(dòng)器總線網(wǎng)絡(luò)

3.4 電流傳感器

針對本文電機(jī)需要恒轉(zhuǎn)矩控制的情形，BDMC-3610驅(qū)動(dòng)器選擇電流模式進(jìn)行控制。當(dāng)機(jī)器人系統(tǒng)處于自動(dòng)平衡控制時(shí)，不僅需要給驅(qū)動(dòng)器發(fā)送驅(qū)動(dòng)指令，同時(shí)還需要反饋上一時(shí)刻的力矩，以此形成力矩的閉環(huán)控制。但事實(shí)上，不能直接檢測力矩的大小，因此采用電流傳感器檢測，再進(jìn)行轉(zhuǎn)化，電流傳感器的輸出信號是一個(gè)模擬電壓，該電壓與輸入電流的大小成正比關(guān)系，利用TMS320F28335中的AD轉(zhuǎn)換模塊，將其轉(zhuǎn)換為數(shù)字量，進(jìn)而得到電壓值，然后根據(jù)線性的換算關(guān)系（i=（Vout-2.5）/0.006）即可計(jì)算出對應(yīng)的電流值。經(jīng)過大量試驗(yàn)，最終獲得電流與力矩的線性關(guān)系，τ＝cTi，其中：τ為力矩，cT為力矩系數(shù)，通過試驗(yàn)測試得到，i為電機(jī)電流，以此來計(jì)算力矩的大小。

3.5 硬件系統(tǒng)測試

結(jié)合以上的硬件測控系統(tǒng)，進(jìn)行系統(tǒng)時(shí)間片測試，所設(shè)計(jì)的伺服周期為40 ms，且其中有12 ms的裕量，用來拓展其他任務(wù)以及避免系統(tǒng)不確定的延時(shí)。各個(gè)模塊時(shí)間片如圖9所示。

圖9 時(shí)間片占比

圖9中，硬件等待占比62%，請求數(shù)據(jù)占比2%，運(yùn)行模型、控制量的計(jì)算、發(fā)送指令以及保存數(shù)據(jù)總體約占1%，其他占比35%，因此有足夠的時(shí)間裕度來進(jìn)行擴(kuò)展復(fù)雜的控制算法。

通過以上測試，說明該硬件系統(tǒng)不僅能完成數(shù)據(jù)采集、運(yùn)行模型等運(yùn)算，而且還有足夠的裕量用來進(jìn)行復(fù)雜的控制策略的設(shè)計(jì)，為后續(xù)研究留下了足夠的時(shí)間。

4 運(yùn)動(dòng)控制實(shí)驗(yàn)

固定車把、車把轉(zhuǎn)動(dòng)45°的控制實(shí)驗(yàn)及驗(yàn)證分析：

調(diào)整車把轉(zhuǎn)角，使左、右車輪軸線共線，系統(tǒng)處于Segway狀態(tài)。給左、右車輪相同控制力矩τ使左右車輪同步轉(zhuǎn)動(dòng)。采用簡單PD控制算法設(shè)計(jì)Segway態(tài)俯仰平衡控制器。具體如下：

式中：q3、q˙3為車架俯仰角和俯仰角速度；q5、q˙5為車輪轉(zhuǎn)角和轉(zhuǎn)角速度。kp、kd、k1、k2為控制器參數(shù)，進(jìn)行物理樣機(jī)試驗(yàn)時(shí)。

同樣采用上述控制方法進(jìn)行車把轉(zhuǎn)動(dòng)45°時(shí)平衡控制實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖10所示。

（續(xù)下圖）

（接上圖）

圖10 兩種狀態(tài)試驗(yàn)效果圖

固定車把時(shí)的控制試驗(yàn)數(shù)據(jù)如圖11所示。

圖11 實(shí)驗(yàn)數(shù)據(jù)曲線

圖11 所示數(shù)據(jù)隨時(shí)間變化的曲線是由慣性測量單元、光電編碼器、電流傳感器等傳感器實(shí)時(shí)測量得到。由此可以驗(yàn)證在該狀態(tài)下測控系統(tǒng)的可行性。其試驗(yàn)效果如圖10所示。

綜上，不僅驗(yàn)證了測控系統(tǒng)的可靠性，也說明了樣機(jī)可以實(shí)現(xiàn)俯仰平衡運(yùn)動(dòng)控制。

以上實(shí)驗(yàn)結(jié)果分析表明：

（1）上文介紹的雙車把兩輪車機(jī)器人測控系統(tǒng)硬件平臺(tái)，包括數(shù)據(jù)采集、與上位機(jī)的通信、可以正常工作，且工作性能良好。

（2）基于上文設(shè)計(jì)的測控系統(tǒng)硬件平臺(tái)，利用簡單的PD控制器，合理選擇控制器參數(shù)，可以快速地實(shí)現(xiàn)物理樣機(jī)的俯仰定車平衡。

5 結(jié)束語

（1）本文提出了一種以工控機(jī)為上位機(jī)，DSP為下位機(jī)，融合多種傳感器的分布式測控系統(tǒng)，利用ZigBee實(shí)現(xiàn)PC端與上位機(jī)的遠(yuǎn)程控制，該測控系統(tǒng)能對機(jī)器人系統(tǒng)進(jìn)行有效地實(shí)時(shí)閉環(huán)控制。

（2）本文完成了雙車把兩輪車機(jī)器人物理樣機(jī)測控系統(tǒng)的搭建，并且通過其俯仰定車平衡運(yùn)動(dòng)控制以及車把轉(zhuǎn)動(dòng)時(shí)的平衡運(yùn)動(dòng)試驗(yàn)，試驗(yàn)結(jié)果有力地證明了測控系統(tǒng)的有效性。

（3）本文所提出的測控系統(tǒng)，可實(shí)現(xiàn)對類似平臺(tái)的移植，同時(shí)，也為平衡車機(jī)器人物理樣機(jī)實(shí)驗(yàn)研究提供了一種簡單可靠的測控系統(tǒng)硬件平臺(tái)。接下來可以利用該平臺(tái)開展一些更高層次的復(fù)雜平衡運(yùn)動(dòng)控制實(shí)驗(yàn)研究。