摘 要： 為了解決機(jī)械臂開環(huán)控制精度低的問題，設(shè)計(jì)了一套基于STM32微控制器的機(jī)械臂反饋控制系統(tǒng)。通過QT圖形界面將控制數(shù)據(jù)輸入上位機(jī)，控制數(shù)據(jù)經(jīng)串口傳輸?shù)轿⒖刂破骱篁?qū)動(dòng)機(jī)械臂運(yùn)動(dòng)；由加速度傳感器和磁通傳感器組成的慣性傳感器節(jié)點(diǎn)采集機(jī)械臂運(yùn)動(dòng)數(shù)據(jù)傳回微控制器，采用由多個(gè)相關(guān)的參數(shù)可變PID控制器構(gòu)成的控制器組對(duì)機(jī)械臂各個(gè)部位進(jìn)行反饋控制。測(cè)試結(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ī)械臂為研究對(duì)象，著重對(duì)交流伺服電機(jī)的驅(qū)動(dòng)進(jìn)行了研究。劉洋等人[3]在LabVIEW環(huán)境下開發(fā)了視覺伺服機(jī)械臂控制系統(tǒng)的實(shí)驗(yàn)平臺(tái)。以上這些系統(tǒng)采用的均是開環(huán)控制，雖也能滿足特定的應(yīng)用場(chǎng)合，但無法滿足高精度機(jī)械臂控制的需求。
趙杰等人[4]設(shè)計(jì)了雙關(guān)節(jié)機(jī)械臂控制系統(tǒng)，利用光電編碼器實(shí)現(xiàn)了機(jī)械臂的閉環(huán)控制，但光電編碼器精度仍然較低。因此，本文設(shè)計(jì)了針對(duì)機(jī)械臂的改進(jìn)型PID閉環(huán)控制系統(tǒng)，該系統(tǒng)利用慣性傳感器采集機(jī)械臂運(yùn)動(dòng)數(shù)據(jù)，在STM32平臺(tái)上實(shí)現(xiàn)機(jī)械臂的反饋控制，旨在提高機(jī)械臂的控制精度以滿足精細(xì)化動(dòng)作控制的需求。
1 系統(tǒng)概述
系統(tǒng)硬件部分由STM32微控制器、三關(guān)節(jié)三自由度機(jī)械臂和慣性傳感器節(jié)點(diǎn)組成。軟件部分包含QT圖形輸入界面、舵機(jī)驅(qū)動(dòng)、控制數(shù)據(jù)生成和反饋控制。系統(tǒng)整體結(jié)構(gòu)如圖1所示，上位機(jī)通過QT圖形界面采集用戶輸入的機(jī)械臂控制數(shù)據(jù)后通過串口傳輸?shù)絊TM32微控制器，微控制器根據(jù)控制數(shù)據(jù)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)并讀取慣性傳感器節(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)測(cè)其運(yùn)行狀態(tài)，STM32微控制器和上位機(jī)之間通過RS232串口通信。圖形界面由輸入?yún)^(qū)和狀態(tài)監(jiān)控區(qū)兩部分組成，如圖2所示，用戶可以通過監(jiān)控區(qū)觀察本次動(dòng)作的完成度以決定是否進(jìn)行下一個(gè)動(dòng)作。
3 反饋數(shù)據(jù)生成
慣性傳感器模塊由加速度傳感器ADXL345和磁通傳感器HMC5883組成，將慣性傳感器模塊綁定在機(jī)械臂各個(gè)部位，經(jīng)過數(shù)據(jù)預(yù)處理[5]可分別采集傳感器坐標(biāo)系下的重力加速度Gs和地磁通量?椎s。記地理坐標(biāo)系下的重力加速度和地磁通量分別為Ge和e，從傳感器坐標(biāo)系變換到地理坐標(biāo)系對(duì)應(yīng)的旋轉(zhuǎn)矩陣為R，其對(duì)應(yīng)的四元數(shù)表示為Q=w+xi+yi+zk。
根據(jù)梯度四元數(shù)按照一定的步長(zhǎng)對(duì)狀態(tài)四元數(shù)進(jìn)行修正，通過反復(fù)的迭代融合，求得機(jī)械臂各個(gè)部位的姿態(tài)四元數(shù)[6]。將機(jī)械臂相鄰兩個(gè)部位的姿態(tài)四元數(shù)分別記為Qn和Qn-1，若其相對(duì)姿態(tài)四元數(shù)為。將四元數(shù)轉(zhuǎn)化為歐拉角即為控制所需的反饋數(shù)據(jù)。
4 反饋控制
為了增強(qiáng)反饋控制的精度，減少抖動(dòng)的產(chǎn)生，使機(jī)械臂在短時(shí)間內(nèi)達(dá)到穩(wěn)定狀態(tài)，本系統(tǒng)采用使用簡(jiǎn)單、適應(yīng)性和魯棒性均較強(qiáng)的PID控制器進(jìn)行反饋控制。但本文中機(jī)械臂一共包含三個(gè)關(guān)節(jié)，誤差會(huì)隨著機(jī)械臂關(guān)節(jié)延伸逐漸積累。普通的PID控制器只能對(duì)手臂各個(gè)部位進(jìn)行獨(dú)立控制，不能充分利用機(jī)械臂各個(gè)部位之間的關(guān)系，且控制器參數(shù)固定，控制過程所需的時(shí)間長(zhǎng)。因此，本文在普通PID控制器的基礎(chǔ)上進(jìn)行了改進(jìn)，將機(jī)械臂各個(gè)部位獨(dú)立的PID控制器組合起來形成參數(shù)可變的PID系統(tǒng)，其控制流程如圖4所示。
上臂PID控制器的差分方程為：
其中，u（k）為輸出量，e（k）為第k次測(cè)量的誤差，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í)際測(cè)試進(jìn)行調(diào)節(jié)。同理可得手掌處的PID控制器參數(shù)。
5 舵機(jī)驅(qū)動(dòng)
本系統(tǒng)采用MG966R舵機(jī)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)，其扭力大、響應(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波。高級(jí)定時(shí)器還能產(chǎn)生多路適合電機(jī)控制的帶死區(qū)互補(bǔ)PWM波，通過單個(gè)定時(shí)器即可同時(shí)驅(qū)動(dòng)多個(gè)舵機(jī)轉(zhuǎn)動(dòng)不同的角度，可以節(jié)省出硬件資源以便后期更多功能的開發(fā)。
6 測(cè)試結(jié)果
為了驗(yàn)證利用慣性傳感器實(shí)現(xiàn)的PID反饋控制的效果，本文對(duì)有無反饋控制兩種情況下的機(jī)械臂控制效果分別進(jìn)行了測(cè)試，測(cè)試結(jié)果如表1所示。其中A組為控制數(shù)據(jù)，B組為無反饋情況下的控制結(jié)果，C組為有反饋情況下的控制結(jié)果。從表1可看出，在第一次試驗(yàn)中有反饋情況下的誤差反而較大，通過分析發(fā)現(xiàn)是由于傳感器節(jié)點(diǎn)安裝位置離舵機(jī)過近而受到舵機(jī)磁場(chǎng)干擾造成的。通過改變傳感器的安裝位置，在后期的測(cè)試中得到了較為理想的效果。在第二和第三次試驗(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)動(dòng)狀態(tài)，實(shí)現(xiàn)了友好的人機(jī)交互。采用慣性傳感器采集機(jī)械臂運(yùn)動(dòng)數(shù)據(jù)，在STM32上實(shí)現(xiàn)對(duì)機(jī)械臂的改進(jìn)型PID反饋控制，測(cè)試結(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].自動(dòng)化與儀器儀表，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è)儀表與自動(dòng)化裝置，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.