蘭建軍

(福建水利電力職業(yè)技術(shù)學(xué)院電力工程學(xué)院,福建 永安 366000)

0 引 言

隨著機(jī)器人技術(shù)的迅速發(fā)展,電力檢修機(jī)械臂已逐步應(yīng)用于輸電線路檢修作業(yè)中,在提高輸電線路檢修工作效率的同時(shí),降低了專業(yè)人員培養(yǎng)成本,提高了作業(yè)安全性[1-2]。但機(jī)械臂的運(yùn)動(dòng)軌跡需要根據(jù)作業(yè)任務(wù)的不同進(jìn)行調(diào)整[3],因此,電力檢修機(jī)械臂動(dòng)作的快速規(guī)劃成為一種新的發(fā)展需求[4]?；谌梭w手臂動(dòng)作捕捉及學(xué)習(xí)的控制方案成為一種新的機(jī)械臂路徑規(guī)劃方式[5,6]。黃啟友利用慣性數(shù)據(jù)手套與紅外檢測(cè)桿進(jìn)行手勢(shì)識(shí)別,識(shí)別率可達(dá)99%[7],李立早等介紹了一種基于加速度計(jì)和彎曲度傳感器的人體手臂運(yùn)動(dòng)角度測(cè)量系統(tǒng)[8]。文獻(xiàn)[9]提出一種基于近鄰法的進(jìn)行慣性融合的聾啞手語(yǔ)識(shí)別方案,準(zhǔn)確率可達(dá)96.5%。后續(xù)的許多基于慣性方式的動(dòng)作捕捉研究中,通常采用3Dsuit、Moven等集成慣性捕捉系統(tǒng)實(shí)現(xiàn)[10-13],也有利用智能可穿戴人機(jī)交互設(shè)備實(shí)現(xiàn)跌倒檢測(cè)[14]。上述研究取得了較好成果,但實(shí)際應(yīng)用中存在弊端:部分系統(tǒng)只采用6軸慣性傳感器進(jìn)行測(cè)量,精度較低;多數(shù)采用類似貝葉斯等較為復(fù)雜的算法進(jìn)行多傳感器數(shù)據(jù)融合,在嵌入式應(yīng)用時(shí)姿態(tài)測(cè)量實(shí)時(shí)性差;另外上述研究主要依賴成套集成設(shè)備,價(jià)格高、失去慣性測(cè)量低成本的意義,且不提供底層操作接口。

為實(shí)現(xiàn)多自由度電力檢修機(jī)械臂動(dòng)作學(xué)習(xí)和控制的低成本嵌入式應(yīng)用,方案采用慣性傳感器實(shí)現(xiàn)人體手臂各關(guān)節(jié)運(yùn)動(dòng)姿態(tài)的測(cè)量和解算,依據(jù)李群理論采用剛體變換方法求解機(jī)械臂各舵機(jī)控制參數(shù),并在Labview中對(duì)6自由度機(jī)械臂動(dòng)作學(xué)習(xí)和控制方案進(jìn)行了動(dòng)態(tài)仿真驗(yàn)證。

1 系統(tǒng)方案

系統(tǒng)在結(jié)構(gòu)上主要包含手臂姿態(tài)檢測(cè)系統(tǒng)和機(jī)械臂舵機(jī)控制系統(tǒng)兩部分,圖1所示為系統(tǒng)結(jié)構(gòu)框圖。其中,手臂姿態(tài)檢測(cè)系統(tǒng)由控制器、慣性傳感器和WIFI通信模塊組成。兩個(gè)慣性傳感器MPU9250布置于人體的大臂和小臂,用于人體手臂姿態(tài)檢測(cè),傳感器數(shù)據(jù)經(jīng)控制器進(jìn)行解算后,形成各舵機(jī)的轉(zhuǎn)動(dòng)目標(biāo)角度,并通過(guò)WIFI模塊發(fā)送于機(jī)械臂舵機(jī)控制系統(tǒng)。機(jī)械臂舵機(jī)控制系統(tǒng)由控制器、數(shù)字舵機(jī)、機(jī)械臂和WIFI模塊組成。WIFI模塊接收各舵機(jī)目標(biāo)角度參數(shù)后,由控制器依據(jù)目標(biāo)角度參數(shù)完成舵機(jī)控制,從而實(shí)現(xiàn)機(jī)械臂對(duì)人體手臂姿態(tài)的識(shí)別和學(xué)習(xí)。

圖1 系統(tǒng)結(jié)構(gòu)框圖Fig.1 Block diagram of system structure

2 手臂姿態(tài)測(cè)量

2.1 坐標(biāo)系構(gòu)建

人體手臂姿態(tài)測(cè)量其本質(zhì)是在人體手臂上構(gòu)建一套捷聯(lián)式慣導(dǎo)系統(tǒng)[15,16],從而需要獲取手臂的姿態(tài)、位置以及速度信息。通常姿態(tài)信息采用載體坐標(biāo)系描述,位置信息采用導(dǎo)航坐標(biāo)系(采用地理坐標(biāo)系)來(lái)描述。本次選用東北天ENU坐標(biāo)系為導(dǎo)航坐標(biāo)系(n系),載體坐標(biāo)系為b系,最初n系和b系重合,將b系繞其Z軸旋轉(zhuǎn)角度φ,再繞b系新X軸旋轉(zhuǎn)角度θ,最后再繞b系新Y軸旋轉(zhuǎn)角度γ,從而獲取一個(gè)新的姿態(tài),旋轉(zhuǎn)關(guān)系示意圖如圖2所示。依據(jù)剛體三維旋轉(zhuǎn)群理論,由于旋轉(zhuǎn)是相對(duì)動(dòng)坐標(biāo)系進(jìn)行的,因此遵循矩陣右乘原則,兩坐標(biāo)系上的點(diǎn)可通過(guò)旋轉(zhuǎn)矩陣進(jìn)行轉(zhuǎn)換,轉(zhuǎn)換關(guān)系如公式(1)所示。

圖2 兩坐標(biāo)系旋轉(zhuǎn)關(guān)系圖Fig.2 Rotation diagram of two coordinate systems

(1)

2.2 姿態(tài)解算

四元數(shù)法以其計(jì)算量小、精度高和可避免奇異性等優(yōu)點(diǎn)被廣泛用于姿態(tài)解算[17]。四元數(shù)法姿態(tài)解算主要包含四元數(shù)初始化、四元數(shù)更新、四元數(shù)轉(zhuǎn)換輸出三個(gè)部分。通過(guò)微分方程的求解來(lái)實(shí)現(xiàn)四元數(shù)更新和轉(zhuǎn)換輸出,從而不斷獲取新的姿態(tài),其具體步驟如下:

Step1 四元數(shù)初始化

公式(2)所示為四元數(shù)初值計(jì)算公式,初始化時(shí)需要用到人體手臂的初始姿態(tài)數(shù)據(jù)。因?yàn)闊o(wú)法確保每次初始化時(shí)人體手臂的姿態(tài)都處于同一位置,所以常用的方法是測(cè)量一個(gè)靜態(tài)姿態(tài)(φ,θ,γ)作為四元數(shù)初值計(jì)算時(shí)的初始姿態(tài)數(shù)據(jù)。

靜態(tài)姿態(tài)解算時(shí),手臂處于靜止?fàn)顟B(tài),因?yàn)閼T性傳感器水平放置,加速度計(jì)只受重力影響,無(wú)法利用加速度計(jì)測(cè)量偏航角φ。偏航角只能利用磁力計(jì)測(cè)量,其測(cè)量公式如公式(3)所示。

(2)

(3)

公式中:mx,my分別為磁力計(jì)在X軸和Y軸上的磁場(chǎng)強(qiáng)度分量輸出。

(4)

Step2 四元數(shù)更新

獲取四元數(shù)初始值后,后續(xù)需要不斷的更新四元數(shù)來(lái)連續(xù)解算姿態(tài),四元數(shù)更新過(guò)程的核心就是根據(jù)姿態(tài)變換矩陣和陀螺儀測(cè)量值不斷迭代的過(guò)程,求解姿態(tài)變換矩陣和載體坐標(biāo)系中角速度關(guān)系的微分方程,可將四元數(shù)的求解轉(zhuǎn)換成公式(5)所示的矩陣表達(dá)形式。

(5)

公式中:ωx,ωy,ωz分別為陀螺儀測(cè)得的三個(gè)方向角速度分量。

Step3 四元數(shù)轉(zhuǎn)換輸出

采用四元數(shù)進(jìn)行姿態(tài)解算時(shí)有很多優(yōu)點(diǎn),但是描述不夠直觀,特別是在后續(xù)的機(jī)械臂舵機(jī)控制參數(shù)計(jì)算時(shí)帶來(lái)較多不便。因此,需要將四元數(shù)形式的姿態(tài)轉(zhuǎn)換成以歐拉角的形式,其轉(zhuǎn)換公式如公式(6)所示。

(6)

盡管人體手臂實(shí)時(shí)姿態(tài)可以通過(guò)單一陀螺儀數(shù)據(jù)測(cè)量完成。但是由于積分特性,陀螺儀的測(cè)量誤差會(huì)隨著時(shí)間不斷積累,所以單純的采用陀螺儀將無(wú)法準(zhǔn)確測(cè)量手臂姿態(tài)。

2.3 卡爾曼濾波

為了準(zhǔn)確測(cè)量人體手臂運(yùn)動(dòng)姿態(tài),通常需要融合陀螺儀、加速度計(jì)、磁力計(jì)的數(shù)據(jù)完成?？柭鼮V波算法作為一種較為常用的數(shù)據(jù)融合算法,其基本原理是利用線性系統(tǒng)狀態(tài)方程,通過(guò)觀測(cè)系統(tǒng)的輸入和輸出,對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)[1]。首先構(gòu)建如公式(7)所示的線性隨機(jī)微分方程和系統(tǒng)觀測(cè)方程。

xk=Axk-1+Buk-1+wk-1
zk=Hxk+vk

(7)

公式中:xk為k時(shí)刻的狀態(tài)值;A為轉(zhuǎn)移矩陣;B為控制輸入矩陣;uk-1為k-1時(shí)刻的輸入;wk-1為過(guò)程噪聲;zk為k時(shí)刻觀測(cè)值;H為觀測(cè)矩陣;vk為測(cè)量噪聲。

卡爾曼最優(yōu)值估計(jì)的本質(zhì)是一個(gè)遞歸濾波過(guò)程[19],包括系統(tǒng)預(yù)測(cè)和測(cè)量修正兩個(gè)環(huán)節(jié)。系統(tǒng)預(yù)測(cè)環(huán)節(jié)中,由于系統(tǒng)初始時(shí)并沒(méi)有當(dāng)前狀態(tài)的觀測(cè)信息,所以需要用k-1時(shí)刻的后驗(yàn)估計(jì)狀態(tài)值計(jì)算k時(shí)刻的先驗(yàn)狀態(tài)估計(jì)值,并計(jì)算先驗(yàn)估計(jì)協(xié)方差。

修正環(huán)節(jié)中主要根據(jù)k時(shí)刻的先驗(yàn)狀態(tài)估計(jì)值及先驗(yàn)狀態(tài)協(xié)方差計(jì)算當(dāng)前的后驗(yàn)狀態(tài)估計(jì)值(最優(yōu)估計(jì)值),完成迭代更新。通過(guò)動(dòng)態(tài)調(diào)整卡爾曼增益從而求解有著最小均方差的最優(yōu)估計(jì)值,其軟件流程圖如圖3所示。

圖3 軟件流程圖Fig.3 Software flow

3 機(jī)械臂控制

3.1 姿態(tài)映射

人體手臂關(guān)節(jié)是運(yùn)動(dòng)的支點(diǎn)和樞紐,運(yùn)動(dòng)機(jī)理較為復(fù)雜。因此,機(jī)械臂對(duì)人體手臂動(dòng)作學(xué)習(xí)的關(guān)鍵在于姿態(tài)映射,其核心問(wèn)題就是將人體手臂姿態(tài)數(shù)據(jù)轉(zhuǎn)換成機(jī)械臂上各舵機(jī)的目標(biāo)角度。設(shè)計(jì)的機(jī)械臂結(jié)構(gòu)圖如圖4所示,其中1號(hào)、2號(hào)舵機(jī)模擬人體大臂的兩個(gè)自由度,可完成俯仰和偏航運(yùn)動(dòng);3號(hào)、4號(hào)、5號(hào)舵機(jī)模擬人體小臂的三個(gè)自由度,可完成俯仰、橫滾和偏航運(yùn)動(dòng);6號(hào)舵機(jī)簡(jiǎn)單模擬人體手部的抓取動(dòng)作。

圖4 仿人機(jī)械臂結(jié)構(gòu)圖Fig.4 Structure diagram of humanoid robot arm

3.2 姿態(tài)分解

圖5 偏航角X-Y平面投影圖Fig.5 X-Y plane projection of yaw Angle

4 系統(tǒng)建模與測(cè)試

4.1 機(jī)械臂建模

機(jī)械臂建模分別在Soildworks和Labview軟件中完成[20]。Soildworks中主要完成舵機(jī)及其機(jī)械連接件的3D建模,包括零件草圖構(gòu)建、二維圖形特征添加、零件約束關(guān)系構(gòu)建和零件裝配4個(gè)步驟,最后生成3D模型的WRL文件,設(shè)計(jì)的機(jī)械臂組件3D模型圖如圖6所示。

圖6 機(jī)械臂3D模型圖Fig.6 3D model of mechanical arm

組件編號(hào)分別對(duì)應(yīng)于圖4中的舵機(jī)編號(hào)。在Labview軟件中導(dǎo)入Soildworks中生成的WRL文件,完成機(jī)械臂組件3D模型的導(dǎo)入,為了保證機(jī)械臂中各自由度舵機(jī)的獨(dú)立旋轉(zhuǎn),需要確定好械臂各組件相對(duì)于原點(diǎn)的坐標(biāo)信息。導(dǎo)入各組件時(shí),需要設(shè)置各組件之間的父子關(guān)系,并要求嚴(yán)格根據(jù)組件編號(hào)按照順序?qū)搿?/p>

4.2 靜態(tài)數(shù)據(jù)測(cè)試

為了驗(yàn)證人體手臂測(cè)量方案的可行性,以人體手臂小臂測(cè)量為例進(jìn)行了靜態(tài)測(cè)試效果驗(yàn)證。由于手臂無(wú)法精確保持在特定角度,測(cè)試時(shí)利用機(jī)械臂替代人體手臂,在機(jī)械臂3號(hào)組件中心位置上固定一個(gè)MPU9520,由單片機(jī)控制3-5號(hào)舵機(jī)分別旋轉(zhuǎn),旋轉(zhuǎn)范圍均為10°～60°(步進(jìn)角5°),記錄解算出的姿態(tài)角度,繪制的誤差曲線如圖7所示,各測(cè)點(diǎn)誤差均在±1.5°內(nèi),滿足測(cè)量精度要求。

圖7 靜態(tài)測(cè)試誤差曲線Fig.7 Static test error curve

4.3 機(jī)械臂動(dòng)態(tài)仿真

雖然Labview支持3D對(duì)象進(jìn)行相對(duì)旋轉(zhuǎn)、平移和縮放功能,但旋轉(zhuǎn)函數(shù)僅支持“旋轉(zhuǎn)對(duì)象”繞坐標(biāo)系原點(diǎn)進(jìn)行旋轉(zhuǎn),這樣無(wú)法滿足各自由度舵機(jī)的獨(dú)立旋轉(zhuǎn)。為解決上述問(wèn)題,采用插入連接點(diǎn)的方式通過(guò)“創(chuàng)建對(duì)象”函數(shù)創(chuàng)建體積相對(duì)機(jī)械臂可以忽略的小球,并將小球移動(dòng)到各組件的連接支點(diǎn)處。利用父子關(guān)系的約束,執(zhí)行旋轉(zhuǎn)函數(shù)時(shí),會(huì)帶動(dòng)下一級(jí)機(jī)械臂繞支點(diǎn)旋轉(zhuǎn),從而同時(shí)實(shí)現(xiàn)獨(dú)立旋轉(zhuǎn)和公共旋轉(zhuǎn),其動(dòng)態(tài)旋轉(zhuǎn)效果圖如圖8所示。

圖8 機(jī)械臂動(dòng)態(tài)運(yùn)動(dòng)效果圖Fig.8 Mechanical arm dynamic motion effect

5 結(jié) 論

針對(duì)電力檢修機(jī)械臂快速動(dòng)作規(guī)劃需求,利用慣性測(cè)量方式實(shí)現(xiàn)人體手臂運(yùn)動(dòng)參數(shù)檢測(cè),經(jīng)姿態(tài)解算獲取手臂各關(guān)節(jié)運(yùn)動(dòng)姿態(tài),分解姿態(tài)后求解出對(duì)應(yīng)機(jī)械臂各舵機(jī)運(yùn)動(dòng)控制參數(shù)后,實(shí)現(xiàn)電力檢修機(jī)械臂對(duì)人體手臂動(dòng)作的學(xué)習(xí)。為提高測(cè)量精度,采用卡爾曼濾波算法對(duì)多傳感器數(shù)據(jù)進(jìn)行融合,最后在Labview中完成了機(jī)械臂的建模和動(dòng)態(tài)測(cè)試。實(shí)驗(yàn)數(shù)據(jù)表明,在考慮噪聲及傳感器誤差的情況下,測(cè)量誤差在±1.5°內(nèi),該方案精度高、成本低,可在機(jī)械臂動(dòng)作學(xué)習(xí)和控制應(yīng)用場(chǎng)合推廣應(yīng)用。