龍 騰
(長沙理工大學(xué) 電氣與信息工程學(xué)院,湖南 長沙 410005)
機(jī)械臂作為工業(yè)自動化的一種設(shè)備,在整個(gè)工業(yè)領(lǐng)域有著非常廣泛的應(yīng)用。實(shí)現(xiàn)對機(jī)械臂的精準(zhǔn)調(diào)控是當(dāng)下研究比較多的課題,也是提高生產(chǎn)效率,改善產(chǎn)品品質(zhì)的一種方式[1]。針對機(jī)械臂的精準(zhǔn)調(diào)控,人們進(jìn)行了大量的研究,當(dāng)下研究比較多的是滕冠[2]等人提出的基于模糊PID控制算法進(jìn)行機(jī)械臂的運(yùn)動控制,采用角度測量傳感器作為反饋單元,然后將輸入?yún)?shù)和反饋參數(shù)進(jìn)行比較,再傳遞給模糊PID核心算法,實(shí)現(xiàn)對機(jī)械臂的運(yùn)動控制。李進(jìn)生[3]等人提出的機(jī)械臂運(yùn)動軌跡的算法設(shè)計(jì),通過分析機(jī)械臂的正運(yùn)動學(xué)和逆運(yùn)動學(xué),然后建立對應(yīng)的仿真模型進(jìn)行軌跡設(shè)計(jì),實(shí)現(xiàn)機(jī)械臂的軌跡更優(yōu)、避障等功能。陳楊洋[4]等人實(shí)現(xiàn)了基于總線控制的機(jī)械臂控制系統(tǒng),該系統(tǒng)采用PROFIBUS總線實(shí)現(xiàn)對機(jī)械臂運(yùn)動軌跡的在線實(shí)時(shí)監(jiān)測。上述方法都能較好地實(shí)現(xiàn)對機(jī)械臂的運(yùn)動控制,但是運(yùn)動精度和響應(yīng)效率不是那么理想。
針對以上分析,本文選用意法半導(dǎo)體公司的STM32F4作為主控制器,關(guān)節(jié)姿態(tài)信息檢測器選用陀螺儀傳感器,關(guān)節(jié)的運(yùn)動控制采用舵機(jī)驅(qū)動模塊,以上三部分構(gòu)成了機(jī)械臂運(yùn)動單元的硬件控制系統(tǒng)。機(jī)械臂運(yùn)動單元的軟件部分采用擴(kuò)展卡爾曼濾波算法,將陀螺儀采集的關(guān)節(jié)狀態(tài)參數(shù)作為反饋參數(shù),傳遞給擴(kuò)展卡爾曼濾波算法模塊,進(jìn)行預(yù)估機(jī)械臂的運(yùn)動狀態(tài),實(shí)現(xiàn)對機(jī)械臂運(yùn)動過程中的偏差調(diào)控[5-6],這樣從硬件和軟件兩個(gè)方面保證了機(jī)械臂控制系統(tǒng)的閉環(huán)控制。
圖1所示是整個(gè)機(jī)械臂控制系統(tǒng)的架構(gòu)圖,從圖中可以看出,整個(gè)控制系統(tǒng)分為4層,分別為用戶層、估計(jì)規(guī)劃層、關(guān)節(jié)控制層以及關(guān)節(jié)狀態(tài)檢測層。用戶層主要是用戶輸入想要機(jī)械臂實(shí)現(xiàn)的運(yùn)動軌跡;軌跡規(guī)劃層主要是根據(jù)機(jī)械臂的逆運(yùn)動模型和用戶層的輸入,進(jìn)行運(yùn)動軌跡規(guī)劃,同時(shí)還具有預(yù)估機(jī)械臂的碰撞狀態(tài);關(guān)節(jié)控制層主要是根據(jù)軌跡規(guī)劃層和關(guān)節(jié)狀態(tài)檢測層進(jìn)行軌跡調(diào)控。關(guān)節(jié)狀態(tài)檢測層主要是作為反饋環(huán)節(jié),給軌跡規(guī)劃層和關(guān)節(jié)控制層提供關(guān)節(jié)的當(dāng)前狀態(tài)信息。而關(guān)節(jié)控制器層的調(diào)控,本文選用擴(kuò)展卡爾曼濾波算法進(jìn)行估算和調(diào)節(jié)。
圖1 機(jī)械臂控制系統(tǒng)架構(gòu)圖
機(jī)械臂的控制系統(tǒng)主要包括硬件部分和軟件部分,根據(jù)機(jī)械臂的設(shè)計(jì)需求,下面分別從硬件和軟件兩個(gè)方面進(jìn)行闡述。
圖2所示為機(jī)械臂的控制系統(tǒng)硬件構(gòu)成。用于和用戶交互的液晶顯示部分選用正點(diǎn)原子設(shè)計(jì)10寸的觸摸屏,主控制器選用ST公司的STM32F4,STM32的時(shí)鐘可以達(dá)到168 MHz,能夠處理各種復(fù)雜的指令,響應(yīng)速度非常快,舵機(jī)驅(qū)動模塊主要是實(shí)現(xiàn)對機(jī)械的運(yùn)動控制,主控制器通過脈寬調(diào)制波(PWM)進(jìn)行對舵機(jī)的調(diào)控[7]。陀螺儀主要是作為反饋單元,采集機(jī)械臂的運(yùn)動狀態(tài),給軌跡規(guī)劃層和關(guān)節(jié)控制層提供關(guān)節(jié)進(jìn)行決策使用。陀螺儀傳感器和主控制器之間采用I2C接口進(jìn)行通信,I2C總線的通信主要是通過時(shí)鐘線和數(shù)據(jù)線兩根線進(jìn)行的,在空閑的時(shí)候,時(shí)鐘線和數(shù)據(jù)線都是高電平,當(dāng)需要進(jìn)行數(shù)據(jù)通信時(shí),都是在時(shí)鐘的邊沿觸發(fā)的。舵機(jī)驅(qū)動器和主控制器之間采用PWM進(jìn)行通信,PWM主要是通過調(diào)節(jié)主控制器輸出的脈沖寬度進(jìn)行對舵機(jī)驅(qū)動器的控制。
圖2 機(jī)械臂控制系統(tǒng)硬件框圖
機(jī)械臂控制系統(tǒng)的軟件設(shè)計(jì)主要包括硬件各個(gè)模塊(液晶顯示模塊、舵機(jī)驅(qū)動模塊、陀螺儀數(shù)據(jù)采集模塊等)的驅(qū)動程序設(shè)計(jì)和EKF算法的設(shè)計(jì)。軟件設(shè)計(jì)的主流程圖如圖3所示。
圖3 機(jī)械臂控制系統(tǒng)軟件主流程圖
系統(tǒng)主流程首先實(shí)現(xiàn)主控制器硬件的初始化,然后等待用戶輸入對應(yīng)的軌跡坐標(biāo),接著陀螺儀開始采集關(guān)節(jié)的狀態(tài),將采集的參數(shù)傳遞給EKF算法,主控制器根據(jù)EKF算法輸出的結(jié)果判斷是否需要進(jìn)行舵機(jī)調(diào)控位姿,最后系統(tǒng)接著檢測是否有新的用戶任務(wù)進(jìn)來,這樣機(jī)械臂控制系統(tǒng)進(jìn)行循環(huán)運(yùn)行,直到到達(dá)設(shè)定的坐標(biāo)位置。
EKF算法是在卡爾曼濾波算法的基礎(chǔ)上擴(kuò)展出來的,該算法是一個(gè)隨著時(shí)間變化的離散的動態(tài)系統(tǒng),它當(dāng)前的系統(tǒng)狀態(tài)可以由系統(tǒng)的過程方程和描述系統(tǒng)的觀測方程進(jìn)行表述,如式(1)所示:
y(n)=C(n)x(n)+v2(n)
(1)
式(1)中,y代表一個(gè)NX1的向量,表示系統(tǒng)在n時(shí)刻的觀測向量矩陣;C代表的是系統(tǒng)的觀測矩陣;v2代表的是擾動信號向量,它的矩陣維數(shù)大小和觀測向量的維數(shù)一致。
狀態(tài)方程為:
x(n+1)=F(n+1,n)x(n)+v1
(2)
式中,x代表一個(gè)MX1的向量,表示系統(tǒng)在n時(shí)刻的系統(tǒng)狀態(tài)向量矩陣;F是一個(gè)MXM的常量矩陣,定義為狀態(tài)轉(zhuǎn)移矩陣;v1表示帶入系統(tǒng)的擾動信號向量。
式(3)是基于條件概率模型,也是貝葉斯先驗(yàn)?zāi)P蚚8]。已知x、y事件發(fā)生的概率,并且知道在x事件發(fā)生的情況下y事件發(fā)生的概率,如果已知上面的三種概率事件,就可以通過貝葉斯先驗(yàn)?zāi)P屯茖?dǎo)出在事件y發(fā)生的情況下x發(fā)生的概率是多少。EKF就是基于以上概率模型進(jìn)行條件預(yù)測的。
(3)
該系統(tǒng)采用擴(kuò)展卡爾曼濾波算法,將陀螺儀采集的關(guān)節(jié)狀態(tài)參數(shù)傳遞給EKF核心算法進(jìn)行預(yù)估機(jī)械臂的運(yùn)動狀態(tài),實(shí)現(xiàn)對機(jī)械臂運(yùn)動過程中的偏差調(diào)控。本文分析和仿真了滕冠[2]等人提出的基于模糊PID控制算法進(jìn)行機(jī)械臂的運(yùn)動控制,通過和本系統(tǒng)提出的EKF算法進(jìn)行對比分析,發(fā)現(xiàn)該系統(tǒng)在控制精度上優(yōu)于模糊PID的控制策略。圖4為滕冠[2]等人提出的基于模糊PID控制算法仿真結(jié)果,從圖中可以看出,模糊PID在整個(gè)軌跡調(diào)節(jié)過程中波動相對較大,實(shí)際軌跡和調(diào)整后的軌跡偏差達(dá)到25 mm,偏差的中心值在10 mm。圖5為本文提出的基于EKF算法的仿真結(jié)果,相比模糊PID算法,本文提出的EKF算法波動相對較少,實(shí)際軌跡和調(diào)整后的軌跡偏差最大達(dá)到18 mm,偏差的中心值為6 mm。
綜上所述,相比于模糊PID控制算法,本文采用的EKF算法在機(jī)械臂精度調(diào)節(jié)、軌跡平滑度等方面具有一定的優(yōu)勢,對于機(jī)械臂的性能優(yōu)化具有一定的實(shí)際意義。
圖4 模糊PID控制算法仿真圖
圖5 EKF算法仿真圖