摘 要: 為了解決機(jī)械臂開環(huán)控制精度低的問題,,設(shè)計(jì)了一套基于STM32微控制器的機(jī)械臂反饋控制系統(tǒng),。通過QT圖形界面將控制數(shù)據(jù)輸入上位機(jī),控制數(shù)據(jù)經(jīng)串口傳輸?shù)轿⒖刂破骱篁?qū)動機(jī)械臂運(yùn)動,;由加速度傳感器和磁通傳感器組成的慣性傳感器節(jié)點(diǎn)采集機(jī)械臂運(yùn)動數(shù)據(jù)傳回微控制器,,采用由多個相關(guān)的參數(shù)可變PID控制器構(gòu)成的控制器組對機(jī)械臂各個部位進(jìn)行反饋控制。測試結(jié)果表明,,利用慣性傳感器實(shí)現(xiàn)的改進(jìn)型PID的反饋控制系統(tǒng)比無反饋控制系統(tǒng)精度有較大提高,,可用于實(shí)現(xiàn)更高精度的機(jī)械臂控制。
關(guān)鍵詞: 機(jī)械臂,;改進(jìn)型PID反饋控制,;慣性傳感器;嵌入式系統(tǒng)
0 引言
機(jī)械臂作為一種實(shí)用的機(jī)器人設(shè)備,,被廣泛地應(yīng)用在工業(yè)生產(chǎn)和日常生活中,。周舟等人[1]控制機(jī)械臂進(jìn)行番茄采摘,根據(jù)仿真結(jié)果調(diào)節(jié)控制參數(shù),。李鯉[2]以鉆孔機(jī)械臂為研究對象,,著重對交流伺服電機(jī)的驅(qū)動進(jìn)行了研究。劉洋等人[3]在LabVIEW環(huán)境下開發(fā)了視覺伺服機(jī)械臂控制系統(tǒng)的實(shí)驗(yàn)平臺,。以上這些系統(tǒng)采用的均是開環(huán)控制,,雖也能滿足特定的應(yīng)用場合,但無法滿足高精度機(jī)械臂控制的需求,。
趙杰等人[4]設(shè)計(jì)了雙關(guān)節(jié)機(jī)械臂控制系統(tǒng),,利用光電編碼器實(shí)現(xiàn)了機(jī)械臂的閉環(huán)控制,但光電編碼器精度仍然較低,。因此,,本文設(shè)計(jì)了針對機(jī)械臂的改進(jìn)型PID閉環(huán)控制系統(tǒng),該系統(tǒng)利用慣性傳感器采集機(jī)械臂運(yùn)動數(shù)據(jù),,在STM32平臺上實(shí)現(xiàn)機(jī)械臂的反饋控制,,旨在提高機(jī)械臂的控制精度以滿足精細(xì)化動作控制的需求,。
1 系統(tǒng)概述
系統(tǒng)硬件部分由STM32微控制器、三關(guān)節(jié)三自由度機(jī)械臂和慣性傳感器節(jié)點(diǎn)組成,。軟件部分包含QT圖形輸入界面,、舵機(jī)驅(qū)動、控制數(shù)據(jù)生成和反饋控制,。系統(tǒng)整體結(jié)構(gòu)如圖1所示,,上位機(jī)通過QT圖形界面采集用戶輸入的機(jī)械臂控制數(shù)據(jù)后通過串口傳輸?shù)絊TM32微控制器,微控制器根據(jù)控制數(shù)據(jù)驅(qū)動機(jī)械臂運(yùn)動并讀取慣性傳感器節(jié)點(diǎn)采集的數(shù)據(jù)進(jìn)行反饋控制,。
2 QT圖形輸入界面
為了實(shí)現(xiàn)良好的用戶體驗(yàn)和方便遠(yuǎn)程控制,,本文設(shè)計(jì)了相應(yīng)的QT圖形界面。用戶通過上位機(jī)可以遠(yuǎn)程控制機(jī)械臂并監(jiān)測其運(yùn)行狀態(tài),,STM32微控制器和上位機(jī)之間通過RS232串口通信,。圖形界面由輸入?yún)^(qū)和狀態(tài)監(jiān)控區(qū)兩部分組成,如圖2所示,,用戶可以通過監(jiān)控區(qū)觀察本次動作的完成度以決定是否進(jìn)行下一個動作,。
3 反饋數(shù)據(jù)生成
慣性傳感器模塊由加速度傳感器ADXL345和磁通傳感器HMC5883組成,將慣性傳感器模塊綁定在機(jī)械臂各個部位,,經(jīng)過數(shù)據(jù)預(yù)處理[5]可分別采集傳感器坐標(biāo)系下的重力加速度Gs和地磁通量?椎s,。記地理坐標(biāo)系下的重力加速度和地磁通量分別為Ge和e,從傳感器坐標(biāo)系變換到地理坐標(biāo)系對應(yīng)的旋轉(zhuǎn)矩陣為R,,其對應(yīng)的四元數(shù)表示為Q=w+xi+yi+zk,。
根據(jù)梯度四元數(shù)按照一定的步長對狀態(tài)四元數(shù)進(jìn)行修正,通過反復(fù)的迭代融合,,求得機(jī)械臂各個部位的姿態(tài)四元數(shù)[6],。將機(jī)械臂相鄰兩個部位的姿態(tài)四元數(shù)分別記為Qn和Qn-1,若其相對姿態(tài)四元數(shù)為,。將四元數(shù)轉(zhuǎn)化為歐拉角即為控制所需的反饋數(shù)據(jù)。
4 反饋控制
為了增強(qiáng)反饋控制的精度,,減少抖動的產(chǎn)生,,使機(jī)械臂在短時(shí)間內(nèi)達(dá)到穩(wěn)定狀態(tài),本系統(tǒng)采用使用簡單,、適應(yīng)性和魯棒性均較強(qiáng)的PID控制器進(jìn)行反饋控制,。但本文中機(jī)械臂一共包含三個關(guān)節(jié),誤差會隨著機(jī)械臂關(guān)節(jié)延伸逐漸積累,。普通的PID控制器只能對手臂各個部位進(jìn)行獨(dú)立控制,,不能充分利用機(jī)械臂各個部位之間的關(guān)系,且控制器參數(shù)固定,,控制過程所需的時(shí)間長,。因此,,本文在普通PID控制器的基礎(chǔ)上進(jìn)行了改進(jìn),將機(jī)械臂各個部位獨(dú)立的PID控制器組合起來形成參數(shù)可變的PID系統(tǒng),,其控制流程如圖4所示,。
上臂PID控制器的差分方程為:
其中,u(k)為輸出量,,e(k)為第k次測量的誤差,,Kpu、Kiu和Kdu分別為上臂控制器的比例系數(shù),、積分項(xiàng)系數(shù)和微分項(xiàng)系數(shù),。
小臂PID控制器的參數(shù)為:
其中,Kif和Kdf分別為小臂控制器可變的積分項(xiàng)系數(shù)和微分項(xiàng)系數(shù),,Kif0和Kdu0為基礎(chǔ)值,,λ為比例系數(shù),可通過實(shí)際測試進(jìn)行調(diào)節(jié),。同理可得手掌處的PID控制器參數(shù),。
5 舵機(jī)驅(qū)動
本系統(tǒng)采用MG966R舵機(jī)驅(qū)動機(jī)械臂運(yùn)動,其扭力大,、響應(yīng)速度快并且可以使機(jī)械臂在靜止時(shí)仍然保持特定姿態(tài),。控制過程中可以通過調(diào)節(jié)周期為20 ms的PWM波的占空比直接控制機(jī)械臂姿態(tài),,且占空比與舵機(jī)轉(zhuǎn)角基本成線性關(guān)系,,避免了通過速度間接控制轉(zhuǎn)角過程中出現(xiàn)的延遲和精度低的問題。STM32F107微控制器包含PWM波產(chǎn)生模塊,,通過配置相應(yīng)的定時(shí)器即可產(chǎn)生特定占空比的PWM波,。高級定時(shí)器還能產(chǎn)生多路適合電機(jī)控制的帶死區(qū)互補(bǔ)PWM波,通過單個定時(shí)器即可同時(shí)驅(qū)動多個舵機(jī)轉(zhuǎn)動不同的角度,,可以節(jié)省出硬件資源以便后期更多功能的開發(fā),。
6 測試結(jié)果
為了驗(yàn)證利用慣性傳感器實(shí)現(xiàn)的PID反饋控制的效果,本文對有無反饋控制兩種情況下的機(jī)械臂控制效果分別進(jìn)行了測試,,測試結(jié)果如表1所示,。其中A組為控制數(shù)據(jù),B組為無反饋情況下的控制結(jié)果,,C組為有反饋情況下的控制結(jié)果,。從表1可看出,在第一次試驗(yàn)中有反饋情況下的誤差反而較大,,通過分析發(fā)現(xiàn)是由于傳感器節(jié)點(diǎn)安裝位置離舵機(jī)過近而受到舵機(jī)磁場干擾造成的,。通過改變傳感器的安裝位置,在后期的測試中得到了較為理想的效果,。在第二和第三次試驗(yàn)中,,無反饋情況下的平均誤差為5.66°,,而有反饋情況下的平均誤差僅為1.5°,控制精度提高了2~7倍,,可見改進(jìn)型的PID反饋控制大大提高了機(jī)械臂的控制精度,。
7 結(jié)論
本文設(shè)計(jì)了一種基于STM32的機(jī)械臂反饋控制系統(tǒng)。本系統(tǒng)通過上位機(jī)QT圖形界面遠(yuǎn)程控制和顯示機(jī)械臂運(yùn)動狀態(tài),,實(shí)現(xiàn)了友好的人機(jī)交互,。采用慣性傳感器采集機(jī)械臂運(yùn)動數(shù)據(jù),在STM32上實(shí)現(xiàn)對機(jī)械臂的改進(jìn)型PID反饋控制,,測試結(jié)果表明,,利用慣性傳感器實(shí)現(xiàn)的改進(jìn)型PID反饋控制較大地提高了機(jī)械臂控制的精度,為高精度的機(jī)械臂控制提供了一種新的方案,。
參考文獻(xiàn)
[1] 周舟,,王俊.采摘機(jī)器人機(jī)械臂的控制與聯(lián)合仿真[J].電子技術(shù),2013(1):60-62.
[2] 李鯉.基于ARM的機(jī)械臂控制系統(tǒng)分析[J].自動化與儀器儀表,,2012(2):176-177.
[3] 劉洋,,方敏.基于LabVIEW的視覺伺服機(jī)械臂控制系統(tǒng)[J].微型機(jī)與應(yīng)用,2012,,31(10):25-27.
[4] 趙杰,,任思瓂,崔崇信.基于ARM的危險(xiǎn)作業(yè)機(jī)器人機(jī)械臂控制系統(tǒng)設(shè)計(jì)[J].工業(yè)儀表與自動化裝置,,2012(3):111-112.
[5] DONG W,, LIM K Y, GOH Y K,, et al. A low-cost motion tracker and its error analysis[C]. IEEE International Conference on Robotics and Automation,, Pasadena, 2008: 311-316.
[6] SABATINI A M. Quaternion based attitude estimation algorithm applied to signals from body-mounted gyroscopes[J]. Electronics Letters,, 2004,,40(10):584-586.