基于重力感應(yīng)傳感器的六自由度車載機械手控制系統(tǒng)設(shè)計

孟祥斌,張雅彬,田衛(wèi)華,劉 煒

(1.沈陽工程學(xué)院自動化學(xué)院，遼寧沈陽 110136；2.哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，黑龍江哈爾濱 150001)

0 引言

重力感應(yīng)傳感器是新型傳感器技術(shù)范疇，實質(zhì)是利用彈性敏感元件制成懸臂式位移器，與采用彈性敏感元件制成的儲能彈簧來驅(qū)動電觸點，實現(xiàn)從重力變化值到電信號的轉(zhuǎn)變[1-2]。

重力感應(yīng)傳感器以其低功耗、小體積、高過載、使用簡便等優(yōu)勢廣泛應(yīng)用于運動載體姿態(tài)測量中。由于重力感應(yīng)傳感器微慣性數(shù)據(jù)處理簡單可靠，并不會改變傳感器彈載結(jié)構(gòu)，對其在車載測量應(yīng)用方面有著重要意義。但是，重力感應(yīng)傳感器測量精度有限，具體應(yīng)用需進一步研究。文獻[3]提出重力加速度數(shù)值積分的多項式擬合算法，平臺移動速度快，但定位精度不夠。文獻[4]研發(fā)了上位主控平臺，根據(jù)需要實時接收車載機械手狀態(tài)信息，實現(xiàn)遙控平臺操控，定位誤差精準，但車體移動速度較慢。文獻[5]設(shè)計了基于MMA7260 加速度傳感器的姿態(tài)平衡小車控制系統(tǒng)，可實現(xiàn)車體快速控制，但定位精度誤差有待提高。

為解決以上問題，本文通過重力感應(yīng)傳感器MMA7660FC測量重力引起的加速度，計算出設(shè)備相對于水平面的傾斜角度。設(shè)計了一種基于遙控操作平臺的雙控制系統(tǒng)，該系統(tǒng)利用重力感應(yīng)傳感器對車體運行姿態(tài)進行粗調(diào)整，快速到達接近抓取目標位置時，采用按鈕連接STM32控制模塊進行車體速度、方向精準步長控制，可實現(xiàn)車載機械手位置的精準校準，實驗表明車體移動速度和定位精度都滿足設(shè)計要求。

1 系統(tǒng)設(shè)計

車載機械手控制系統(tǒng)由遙控操作平臺、控制模塊、無線通信模塊、驅(qū)動模塊、AD轉(zhuǎn)換模塊、重力感應(yīng)模塊、穩(wěn)壓濾波模塊等組成，系統(tǒng)框架如圖1所示。

圖1 系統(tǒng)框架圖

車載機械手是按照制訂程序、軌跡與要求實現(xiàn)自行抓取、放置的自動化機械系統(tǒng)。車載機械手通常由2部分構(gòu)成，即移動車體平臺和機械手。依托固定車輛底座，在此底座上層放置機械手，利用機械手實現(xiàn)空間動作，完成抓取物體、機械手臂伸展、動作復(fù)位等動作。通過借助車體的快速移動性，擴展了機械手工作范圍。

設(shè)計的遙控操作平臺分別對車體和六自由度機械手機械控制。只有保證車體穩(wěn)定靜止后，才可以對機械手進行動作操作。車體除搭載機械手和傳感器外，車體自身具有單獨STM32控制模塊。控制模塊通過車體所攜帶超聲波傳感器和視覺傳感器識別周圍環(huán)境狀態(tài)，將采集的信號處理后，通過無線傳輸模塊發(fā)送給遙控操作平臺，以便遙控操作平臺進行及時處理。PID算法調(diào)整直流電機的轉(zhuǎn)速，車體STM32控制器內(nèi)含特殊安全機制，保證遇到突發(fā)路面障礙和阻礙物體時，及時暫停車輛行進，直到遇到遙控操作平臺確認信息2次后(間隔時間短于2 s)，才可以繼續(xù)車體行進和機械手操作動作。

2 硬件設(shè)計

車載機械手控制系統(tǒng)包含車體運行姿態(tài)的調(diào)控和六自由度機械手抓取控制，其中通過控制直流電機來實現(xiàn)車體移動，當(dāng)車體靜止后，再控制機械手上的6個舵機實現(xiàn)物體抓取的設(shè)計。

2.1 車體驅(qū)動設(shè)計

車體STM32控制模塊通過無線模塊，接收到遙控操作平臺指令，據(jù)此執(zhí)行相應(yīng)的控制任務(wù)。由于控制模塊STM32輸出的PWM信號為3.3 V電平，不能直接用來驅(qū)動直流電機進行轉(zhuǎn)動。因此，采用直流驅(qū)動電路來提高驅(qū)動能力。設(shè)計采用L298N芯片，L298N是雙H橋電機驅(qū)動芯片，可以驅(qū)動2個直流電機。依據(jù)L298N設(shè)計的驅(qū)動電路可提供5～35 V范圍的驅(qū)動電壓，最大功率為25 W。L298N價格較低，性能可靠，滿足本設(shè)計需求[6]。

車體STM32控制模塊并產(chǎn)生穩(wěn)定度較高PWM信號和I/O電流。電流信號方向的正負來確定使能端上的信號電平，利用使能端的信號狀態(tài)來改變直流電動機的順時針、逆時針旋轉(zhuǎn)狀態(tài)。PWM信號的占空比決定直流電機的運行速度。PWM信號經(jīng)L298N驅(qū)動電路放大后，可以滿足車體直流電機的運行功率。車體電機驅(qū)動電路圖如圖2所示。

圖2 電機驅(qū)動電路圖

2.2 重力感應(yīng)傳感器電路設(shè)計

重力感應(yīng)傳感器MMA7660FC是一個 1.5g的三軸加速度計，支持I2C數(shù)字輸出。同時這款傳感器是電容MEMS型的傳感器，具有非常低的功耗，它內(nèi)部具有低通濾波器，可用于補償0g偏移和增益誤差，用戶可根據(jù)要求為該傳感器配置為6位分辨率。重力感應(yīng)傳感器功能框圖見圖3。

圖3 重力感應(yīng)傳感器功能框圖

車輪的轉(zhuǎn)速和方向由重力感應(yīng)的傾角值決定。重力感應(yīng)傳感器電路見圖4。遙控操作平臺內(nèi)控制模塊將捕獲重力感應(yīng)傳感器的姿態(tài)數(shù)值與設(shè)定程序進行匹配，并將匹配數(shù)值發(fā)送給車體控制模塊。通過車體控制模塊內(nèi)的閉環(huán)PID控制算法確定車輛的車輪運動方向和角度。

圖4 重力感應(yīng)傳感器電路

2.3 遙控操作平臺通信電路設(shè)計

車載機械手遙控操作平臺系統(tǒng)中，設(shè)計通信模塊采用無線通信NRF24L01芯片，這款無線射頻收發(fā)芯片的工作頻率是2.4 GHz。該芯片內(nèi)部自帶CRC檢驗功能，可自動完成應(yīng)答功能，內(nèi)部硬件的CRC具有查錯能力，可以實現(xiàn)一點對多點通信地址操控。同時，該款芯片在1.9～3.6 V之間進行工作，可實現(xiàn)低功耗處理數(shù)據(jù)，待機狀態(tài)下電流為22 μA，掉電模式下電流為900 nA。內(nèi)置2.4 GHz天線，該芯片有126個頻道，符合多點通信和跳頻通信的需求。此外具有高可靠性的數(shù)據(jù)傳送[7-8]。通信模塊與控制器STM32F103RBT6構(gòu)成無線通信傳輸。無線模塊采用3.3 V電源電壓供電，因為STM32F103RBT6同樣采用3.3 V供電，所以無線模塊的通信引腳可以和控制器STM32的引腳直接相連。IRQ是可設(shè)置的中斷標志的引腳，當(dāng)接收有效數(shù)據(jù)時或者發(fā)送數(shù)據(jù)成功時，引腳會產(chǎn)生電平的變化。

2.4 車體無線通信電路設(shè)計

車體的無線通信模塊主要功能是接收遙控操作平臺發(fā)送的控制指令，并將車體搭載傳感器采集的現(xiàn)場環(huán)境數(shù)據(jù)傳送到遙控操作平臺端。NRF24L01的外接天線傳輸距離提高到20 m，其車體無線通信接收電路見圖5，受外界環(huán)境影響，傳輸距離有一定的降低。

圖5 車體無線通信接收電路

2.5 液晶顯示接口電路

顯示模塊使用低功耗NOKIA5110的LCD液晶顯示屏。該屏是以點陣的方式來顯示的，具有48像素×84像素的分辨率。傳輸最高速率可達4.0 Mbit/s，可以顯示英文字符、漢字和圖片。液晶顯示模塊采取SPI的通信方式。硬件使用了5個普通的IO口的資源作為顯示屏的輸出接口。通過液晶顯示模塊可以實時觀測車體運行速度信息和方向角度數(shù)值[9]。圖6為液晶接口圖。

3 程序設(shè)計

該車載機械手系統(tǒng)的軟件主要用C語言編寫，開發(fā)環(huán)境為Keil MDK，軟件采用模塊化設(shè)計。車載機械手系統(tǒng)的主程序先進行初始化配置，確保車體和機械手正常工作。初始化主要包括系統(tǒng)變量初始化、中斷初始化、傳感器模塊初始化、A/D轉(zhuǎn)換程序初始化、機械手初始化等。系統(tǒng)變量初始化包含霍爾測速模塊的設(shè)置，重力感應(yīng)傳感器、超聲波模塊測距離賦初值，視覺傳感器Pixy 2代CMUcam5的各類函數(shù)的初始化，供電電源監(jiān)測初始化等。系統(tǒng)初始化后，遙控操作平臺中重力感應(yīng)模塊測定數(shù)值超過設(shè)定靜止閾值后，通過遙控操作無線模塊發(fā)出移動信息，啟動車體。車體在移動后，利用超聲波傳感器和視覺傳感器感知車體周圍環(huán)境信息，如遇障礙物，及時將信息反饋給遙控操作平臺端。遙控操作平臺改變車體運行速度和方向，規(guī)避障礙物。當(dāng)車體接近目標物體時，首先通過重力感應(yīng)模塊進行車體姿態(tài)調(diào)整，其次通過按鈕進行位置微調(diào)修正。當(dāng)滿足設(shè)定誤差范圍±5 mm內(nèi)時，保持車體靜止。啟動機械手控制，利用PID算法控制六自由度機械手舵機角度方向角度，進行目標物體抓取，抓取目標物體放入小車后，進行機械手歸位。最后，重新啟動小車運動。車載機械手主程序的流程圖如圖7所示。

車載機械手系統(tǒng)需要確保電量支持，各類傳感器模塊、無線通信模塊、車體驅(qū)動電機和機械手舵機等均需要電能支持，為保證系統(tǒng)電量充足，需設(shè)置電源監(jiān)測程序。通過一定延遲時間對車體攜帶電源兩端電壓監(jiān)測，保證系統(tǒng)正常工作。當(dāng)電壓值低于設(shè)置電壓(7.2 V的20%)，紅燈閃爍，需要及時充電。正常時保持綠燈點亮。圖8為電池電能監(jiān)測子程序流程圖。

圖8 電池電能監(jiān)測子程序流程

4 測試結(jié)果及分析

4.1 機械手臂夾緊力測試

抓取物體的夾緊力是設(shè)計機械手重要指標，夾緊力大會使物體在拾取物體過程中表面破損或形狀變形，從而導(dǎo)致拾取失??；夾緊力過小，導(dǎo)致車輛行走過程中，物體滑落，也會導(dǎo)致拾取失敗。夾緊力需要克服物體重力所引起的靜載荷和物體運動狀態(tài)變化產(chǎn)生的載荷，以保證機械手對物體穩(wěn)定的夾緊狀態(tài)。機械手對物體夾緊力依據(jù)式(1)計算[10]:

FN≥β1β2β3G

(1)

式中：β1為安全系數(shù)，通常取值1.2～2.0；β2為狀況系數(shù)，主要考慮慣性力的作用；β3為方向系數(shù)，依據(jù)手臂與物體之間的不同位置進行判斷；G為被抓取物件所受重力，N。

機械手夾緊力測試如表1所示。

表1 機械手夾緊力測試

由測試結(jié)果可知，機械手對于質(zhì)量≤6.05 g物體，抓取率過低。對于質(zhì)量>10.0 g物體，抓取率高。此外，測試中機械手所夾取物體質(zhì)量不可超過300 g，否則會造成微型車載搬運機器人重心不穩(wěn)。

4.2 車輛控制系統(tǒng)測試

遙控操作平臺連接無線通信模塊后，對車載機械手狀態(tài)進行20次發(fā)送指令測試，觀察搬運機器人運動狀態(tài)的變化，再改變遙控操作平臺與搬運機器人之間距離后重復(fù)上面的測試。不同距離控制指令準確性測試如表2所示。

表2 不同距離控制指令準確性測試

其中重力傳感器粗略調(diào)整為模式1，按鈕連接STM32控制器，進入模式2，實現(xiàn)精細調(diào)整。根據(jù)表2可以獲得實測模式1和模式2的接收指令準確次數(shù)和該模式對應(yīng)的準確率。

以上測試環(huán)境是在空曠的地面上，測試通信質(zhì)量較好，在無線通信NRF24L01芯片有效范圍20 m內(nèi)，通信指令模式1準確率的平均值為95.6%,模式2準確率的平均值為98.8%。原因為：重力感應(yīng)模式1下，數(shù)據(jù)變化過快，控制器對數(shù)據(jù)的處理和發(fā)送延遲。

4.3 六自由度機械手仿真及定位測試

控制STM32模塊給機械手的6個舵機發(fā)送相應(yīng)的旋轉(zhuǎn)角度，控制機械手的關(guān)節(jié)運行，完成抓取工作。圖9給出了基于Proteus軟件仿真的PWM脈寬仿真波形圖。

圖9 PWM脈寬波形圖

依據(jù)虛擬示波器顯示的PWM波形，可以檢驗編寫的PWM子程序是否滿足舵機控制要求。舵機的轉(zhuǎn)動角度是通過調(diào)節(jié)PWM信號的占空比來完成的。標準PWM信號的周期固定為20 ms(50 Hz)，理論上仿真脈寬分布應(yīng)在1～2 ms之間，實測脈寬可為0.5～2.5 ms之間。

在設(shè)計中，每個數(shù)字舵機方向旋轉(zhuǎn)角度設(shè)置為8個定值(6個舵機共有48個方向角)。受方向角在空間數(shù)量不足影響，存在定位誤差，實測數(shù)值與理論值計算相比在-3～3 mm之間。圖10為車載機械手實物圖。

圖10 車載機械手實物圖

5 結(jié)論

本文設(shè)計基于重力感應(yīng)傳感器的車載機械手，完成了車輛確定的運動路徑規(guī)劃，實現(xiàn)運動路徑連續(xù)可控。通過按鈕可在20 m內(nèi)，遠程操作機械手進行抓取、運送等動作執(zhí)行，并對夾緊力進行實測，實現(xiàn)了機械臂的精準定位與操控。