六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)及方法
【專利摘要】本發(fā)明公開了六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)及方法，包括機(jī)械臂整體系統(tǒng)的若干個監(jiān)測節(jié)點(diǎn)，每個監(jiān)測節(jié)點(diǎn)處安裝有監(jiān)測裝置，監(jiān)測裝置有監(jiān)控終端通過Zigbee無線通信模塊控制啟停，Zigbee無線通信模塊與中央處理單元通信，中央處理單元與監(jiān)測裝置通信，用于接收監(jiān)測裝置所監(jiān)測的監(jiān)測節(jié)點(diǎn)的信息。利用無線傳感網(wǎng)絡(luò)對機(jī)械臂整體系統(tǒng)進(jìn)行姿態(tài)監(jiān)測，通過監(jiān)測終端和若干監(jiān)測節(jié)點(diǎn)實(shí)現(xiàn)同時(shí)對多個節(jié)點(diǎn)的監(jiān)測，克服了傳統(tǒng)只對單個點(diǎn)姿態(tài)監(jiān)測的缺陷。
【專利說明】六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)及方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)及方法。

【背景技術(shù)】
[0002]機(jī)械臂在工程機(jī)械【技術(shù)領(lǐng)域】具有重要作用，機(jī)械臂姿態(tài)不精確的問題會導(dǎo)致機(jī)械臂的控制難以滿足實(shí)際需求。對機(jī)械臂整體系統(tǒng)的姿態(tài)監(jiān)測可以使機(jī)械臂操作精準(zhǔn)，以使機(jī)械臂達(dá)到預(yù)定位置、實(shí)施預(yù)定動作。由此機(jī)械臂整體系統(tǒng)的姿態(tài)監(jiān)測的正常工作對具有機(jī)械臂的工程機(jī)械的正常工作同樣具有重要作用。
[0003]傳統(tǒng)的機(jī)械臂姿態(tài)監(jiān)測是根據(jù)機(jī)械臂的結(jié)構(gòu)特點(diǎn)和運(yùn)動學(xué)約束，對機(jī)械臂的運(yùn)動學(xué)進(jìn)行分析。采用Denavit-Hartenberg(D-H)法對機(jī)械臂操作空間進(jìn)行描述,在考慮機(jī)械臂運(yùn)動學(xué)約束的基礎(chǔ)上，得到以關(guān)節(jié)角度為變量的正運(yùn)動學(xué)模型。通過分析正運(yùn)動學(xué)模型的可解性，采用矩陣逆乘的解析法求解機(jī)械臂的正運(yùn)動學(xué)模型，進(jìn)而得到了機(jī)械臂逆運(yùn)動學(xué)的解。利用D-H方法對機(jī)械臂進(jìn)行分析建模:首先，給每個關(guān)節(jié)指定一個參考坐標(biāo)系；然后，確定如何實(shí)現(xiàn)任意2個相鄰坐標(biāo)系之間的變換；最后，寫出機(jī)械臂的總變換矩陣。
[0004]綜上所述，傳統(tǒng)方法的不足可以總結(jié)為傳統(tǒng)的機(jī)械臂整體系統(tǒng)的姿態(tài)監(jiān)測方法是通過將機(jī)械臂各個關(guān)節(jié)進(jìn)行D-H建模，然后通過各個軸之間復(fù)雜的矩陣變換得出總的變換矩陣從而推導(dǎo)出末端的姿態(tài)。這種方式存在計(jì)算量大，計(jì)算過程容易出錯，降低姿態(tài)監(jiān)測的精度，且工作復(fù)雜效率低等問題。


【發(fā)明內(nèi)容】

[0005]為解決現(xiàn)有技術(shù)存在的不足，本發(fā)明公開了六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)及方法，本發(fā)明從機(jī)械臂整體結(jié)構(gòu)為出發(fā)點(diǎn)，在機(jī)械臂各個關(guān)節(jié)之間協(xié)調(diào)工作時(shí)，求取某個臂的姿態(tài)坐標(biāo)時(shí)不需要經(jīng)過復(fù)雜的坐標(biāo)變換得出，可以直接經(jīng)過姿態(tài)監(jiān)測模塊測量計(jì)算得出。本發(fā)明將復(fù)雜的矩陣變換轉(zhuǎn)化為三角函數(shù)的計(jì)算，直接通過角度相關(guān)計(jì)算推導(dǎo)出末端的姿態(tài)。簡化了計(jì)算過程，提高了測量精度。同時(shí)姿態(tài)監(jiān)測模塊可直接安裝在多自由度機(jī)械臂的關(guān)節(jié)處，不需要改變機(jī)械臂原有的安裝來進(jìn)行復(fù)雜的安裝過程，并且其結(jié)構(gòu)簡單、抗干擾性強(qiáng)。
[0006]為實(shí)現(xiàn)上述目的，本發(fā)明的具體方案如下:
[0007]六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)，包括布置在機(jī)械臂整體系統(tǒng)的若干個監(jiān)測節(jié)點(diǎn)，每個監(jiān)測節(jié)點(diǎn)處安裝有監(jiān)測裝置，每個監(jiān)測裝置有監(jiān)控終端通過Zigbee無線通信模塊控制啟停，Zigbee無線通信模塊與中央處理單元通信，中央處理單元與監(jiān)測裝置通信，中央處理單元接收監(jiān)測裝置所監(jiān)測的監(jiān)測節(jié)點(diǎn)的信息。
[0008]所述監(jiān)測裝置包括用于測量機(jī)械臂關(guān)節(jié)的加速度值的三軸加速度傳感器，用于測量機(jī)械臂關(guān)節(jié)的地磁感應(yīng)強(qiáng)度分量的三軸地磁傳感器及用于測量械臂關(guān)節(jié)的角速度值的三軸陀螺儀。
[0009]所述若干個監(jiān)測節(jié)點(diǎn)包括機(jī)械臂基座的第一監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第二監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第三監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第四監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第五監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第六監(jiān)測節(jié)點(diǎn)及機(jī)械臂關(guān)節(jié)處的第七監(jiān)測節(jié)點(diǎn)。
[0010]六自由度機(jī)械臂姿態(tài)監(jiān)測方法，包括以下步驟:
[0011]步驟一:中央處理單元讀取三軸地磁傳感器測量出的機(jī)械臂各關(guān)節(jié)的三維地磁感應(yīng)強(qiáng)度分量值mx、my、mz、三軸加速度傳感器測量出的機(jī)械臂各關(guān)節(jié)的三維加速度分量值ax> ay> αζ及三軸陀螺儀測量出的機(jī)械臂各關(guān)節(jié)的角速度；
[0012]步驟二:對讀取的各關(guān)節(jié)的三維地磁感應(yīng)強(qiáng)度分量、各關(guān)節(jié)的三維加速度分量值利用四元數(shù)算法進(jìn)行歸一化處理，對讀取的陀螺儀角速度進(jìn)行偏差補(bǔ)償后的四元數(shù)向量進(jìn)行歸一化處理；
[0013]步驟三:將步驟二歸一化處理后所得四元數(shù)轉(zhuǎn)換成歐拉角即旋轉(zhuǎn)角；將各部分進(jìn)行多次測量測得的多個旋轉(zhuǎn)角取其均值；
[0014]步驟四:根據(jù)步驟三中的旋轉(zhuǎn)角、機(jī)械臂的臂長及各關(guān)節(jié)坐標(biāo)系通過三角函數(shù)計(jì)算得出機(jī)械臂最終姿態(tài)。
[0015]所述步驟一中還包括測量得到三維加速度值分量a x、a y、a z及三維地磁感應(yīng)分量mx、my、mz，對分量進(jìn)行校準(zhǔn)，校準(zhǔn)的目的是為了去除由外界環(huán)境和測量過程中造成的誤差，即實(shí)驗(yàn)前先分別對多次測得的加速度值和地磁感應(yīng)三個分量取平均值，以此平均值作為標(biāo)定值校準(zhǔn)當(dāng)次測量值的準(zhǔn)確性。計(jì)算加速度參考值acc和地磁感應(yīng)強(qiáng)度參考值mag ；
[0016]其中:acc - a +a~.+ az ； mag = +m2y+m2z。
[0017]所述步驟二中具體包括:
[0018]當(dāng)加速度測量值和參考值acc的偏差小于設(shè)定閾值時(shí)，加速度的四元數(shù)表示通過
s (X " ΓI
歸一化處理得到:=卩” =1° aV., a否則，由對地坐標(biāo)系中的加速度通過四元數(shù)轉(zhuǎn)換得到:^A,=At_^za?At^ ；
[0019]其中各式中參數(shù)含義如下:
[0020]加速度測量值W? +K ，其中sat = sab,t+sgt,sat是經(jīng)過誤差補(bǔ)償?shù)木€性加速度，sab，t是重力補(bǔ)償后物體的線性加速度，ba，t是由低頻偏移引起的偏差，ea，t為零均值白噪聲，sgt的取值取決于傳感器定位；sAt為加速度測量值的歸一化表示即加速度的四元數(shù)表示，||4<|為加速度測量值的幅值，a x；t> a y；t> a z；t分別為時(shí)間t時(shí)的三個方向的加速度分量，&為在測量時(shí)間內(nèi)的單位四元數(shù)表示，:qt—Mh的共軛，X = [0 0 0 I]。
[0021]當(dāng)?shù)卮鸥袘?yīng)強(qiáng)度測量值^<和參考值mag的偏差小于設(shè)定閾值時(shí)，地磁感應(yīng)強(qiáng)度的四元數(shù)表示通過歸一化處理得到=否則，由對地坐標(biāo)
^? *? *S
系中的地磁通過四元數(shù)轉(zhuǎn)換得到sM=eqt?eznJ%eqt ;
[0022]其中，地磁感應(yīng)強(qiáng)度測量值sdit +dm，t +bmt+emJ，其中，smt為地磁場的磁感應(yīng)強(qiáng)度，dm；t為磁場干擾，bm，t為磁場偏差，em；t為噪聲，mx，t、my，t、mz，t分別為時(shí)間t時(shí)的三個方向的地磁感應(yīng)強(qiáng)度分量，sMt為時(shí)間t時(shí)的地磁感應(yīng)強(qiáng)度測量值的歸一化表示即地磁感應(yīng)強(qiáng)度的四兀數(shù)表不，為測量時(shí)間內(nèi)的地磁感應(yīng)強(qiáng)度，= O mx my mz，&為在時(shí)間t時(shí)的單位四元數(shù)表示，K的共軛。
[0023]根據(jù)高斯牛頓法則，尋找e n。gx qy qz，使/(； %) = 士 ?.( )v ^,: q,)取最小值，以便于求取使陀螺儀角速度誤差最小的7其中為誤差函數(shù)；
[0024]求解陀螺儀偏差漂移補(bǔ)償四元數(shù)梯度向量，并對四元數(shù)梯度向量進(jìn)行歸一化處理.eHsMht — Λ ，
?Ag
[0025]其中^ 為傳感器坐標(biāo)系相對于地坐標(biāo)系的陀螺儀方向的四元數(shù)的變化率，

e^ls.t+At
Ac/力單位四元數(shù)的變化。
[0026]所述對讀取的陀螺儀角速度進(jìn)行偏差補(bǔ)償，角度誤差=Hwd，角度偏差5?‘ - C0^At ’補(bǔ)償后的傳感器坐標(biāo)系下的角度向;
[0027]其中，sCOetMt為陀螺儀軸中補(bǔ)償后的角誤差，$?+Δ/表示互補(bǔ)濾波融合后被估計(jì)方向的四元數(shù)表示的共軛，:1/+&被用作陀螺儀偏差漂移補(bǔ)償?shù)乃脑獢?shù)表示變化率，5ωΜ+Δ?為角偏移，S為抵消非零均值陀螺儀測量誤差的收斂率，s?t+,t為角速度。
[0028]計(jì)算由陀螺儀數(shù)據(jù)估算的四元數(shù)梯度向量:Ju- =\eqCOJ?sG)i+M其中、為陀螺儀測量的四元數(shù)變化率，g為陀螺儀的被估計(jì)方向的單位四元數(shù)表示；
[0029]計(jì)算經(jīng)過互補(bǔ)濾波器對陀螺儀偏差漂移進(jìn)行的補(bǔ)償，最終得到的角速度四元數(shù)向量油At，其中aIfJmfiecL帥，并進(jìn)行歸一化處理；
[0030]其中互補(bǔ)濾波器融合的被估計(jì)方向的四元數(shù)表示，eqf t+At是互補(bǔ)濾波融合后被估計(jì)方向的四元數(shù)變化率，、為陀螺儀測量的四元數(shù)變化率，β為濾波器增益；


e I Δ?
[0031]所述步驟三中將步驟二所得四元數(shù)轉(zhuǎn)換成歐拉角。由于機(jī)械臂每個部分只能進(jìn)行單一運(yùn)動方式如只能是俯仰、旋轉(zhuǎn)、擺動中的一種，所以求取的旋轉(zhuǎn)角只能是由相應(yīng)的運(yùn)動產(chǎn)生的橫擺角、俯仰角、滾擺角中的一種角度。其中，Ψ、Θ分別為繞Z軸、Y軸、X軸的旋轉(zhuǎn)角度對應(yīng)橫擺角、俯仰角、滾擺角。
[0032]四元數(shù)與歐拉角的轉(zhuǎn)換關(guān)系如下:
[0033]若四元數(shù)q = [w X y z]τ，則旋轉(zhuǎn)角度可由下式得出:
[0034]

【權(quán)利要求】
1.六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)，其特征是，包括布置在機(jī)械臂整體系統(tǒng)的若干個監(jiān)測節(jié)點(diǎn)，每個監(jiān)測節(jié)點(diǎn)處安裝有監(jiān)測裝置，每個監(jiān)測裝置有監(jiān)控終端通過Zigbee無線通信模塊控制啟停，Zigbee無線通信模塊與中央處理單元通信，中央處理單元與監(jiān)測裝置通信，中央處理單元接收監(jiān)測裝置所監(jiān)測的監(jiān)測節(jié)點(diǎn)的信息。
2.如權(quán)利要求1所述的六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)，其特征是，所述監(jiān)測裝置包括用于測量機(jī)械臂關(guān)節(jié)的加速度值的三軸加速度傳感器，用于測量機(jī)械臂關(guān)節(jié)的地磁感應(yīng)強(qiáng)度分量的三軸地磁傳感器及用于測量械臂關(guān)節(jié)的角速度值的三軸陀螺儀。
3.如權(quán)利要求1所述的六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)，其特征是，所述若干個監(jiān)測節(jié)點(diǎn)包括機(jī)械臂基座的第一監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第二監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第三監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第四監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第五監(jiān)測節(jié)點(diǎn)、機(jī)械臂關(guān)節(jié)處的第六監(jiān)測節(jié)點(diǎn)及機(jī)械臂關(guān)節(jié)處的第七監(jiān)測節(jié)點(diǎn)。
4.如權(quán)利要求1所述的六自由度機(jī)械臂姿態(tài)監(jiān)測系統(tǒng)的監(jiān)測方法，其特征是，包括以下步驟: 步驟一:中央處理單元讀取三軸地磁傳感器測量出的機(jī)械臂各關(guān)節(jié)的三維地磁感應(yīng)強(qiáng)度分量值mx、my、mz、三軸加速度傳感器測量出的機(jī)械臂各關(guān)節(jié)的三維加速度分量值α x、a y、az及三軸陀螺儀測量出的機(jī)械臂各關(guān)節(jié)的角速度； 步驟二:對讀取的各關(guān)節(jié)的三維地磁感應(yīng)強(qiáng)度分量、各關(guān)節(jié)的三維加速度分量值利用四元數(shù)算法進(jìn)行歸一化處理，對讀取的陀螺儀角速度進(jìn)行偏差補(bǔ)償后的四元數(shù)向量進(jìn)行歸一化處理； 步驟三:將步驟二歸一化處理后所得四元數(shù)轉(zhuǎn)換成歐拉角即旋轉(zhuǎn)角；將各部分進(jìn)行多次測量測得的多個旋轉(zhuǎn)角取其均值； 步驟四:根據(jù)步驟三中的旋轉(zhuǎn)角、機(jī)械臂的臂長及各關(guān)節(jié)坐標(biāo)系通過三角函數(shù)計(jì)算得出機(jī)械臂最終姿態(tài)。
5.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述步驟一中還包括測量得到三維加速度值分量ax、ay、αζ及三維地磁感應(yīng)分量mx、my、mz，對分量進(jìn)行校準(zhǔn)，校準(zhǔn)的目的是為了去除由外界環(huán)境和測量過程中造成的誤差，即實(shí)驗(yàn)前先分別對多次測得的加速度值和地磁感應(yīng)三個分量取平均值，以此平均值作為標(biāo)定值校準(zhǔn)當(dāng)次測量值的準(zhǔn)確性；計(jì)算加速度參考值acc和地磁感應(yīng)強(qiáng)度參考值mag ；
其中 ml = ^ja2t + a] + a: ； mag=小η2' + tn~ + ml。
6.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述步驟二中具體包括: 當(dāng)加速度測量值和參考值acc的偏差小于設(shè)定閾值時(shí)，加速度的四元數(shù)表示通過歸一
s (Xm YI化處理得到= 0W ay,t α#.Ι，否則，由對地坐標(biāo)系中的加速度通過四元數(shù)轉(zhuǎn)換得到 其中各式中參數(shù)含義如下: 加速度測量值X1=X+Am+&，其中sat = sab，t+sgt，sat是經(jīng)過誤差補(bǔ)償?shù)木€性加速度，sab，t是重力補(bǔ)償后物體的線性加速度，ba，t是由低頻偏移引起的偏差，ea，t為零均值白噪聲，sgt的取值取決于傳感器定位；sAt為加速度測量值的歸一化表示即加速度的四元數(shù)表示，I <||為值的幅值，a x,t> a y，t、a z，t分別為時(shí)間t時(shí)的三個方向的加速度分量q泣為在測量時(shí)間內(nèi)的單位四元數(shù)表示，fQ、為q、的共軛，eZa = [0 0 0 I]。



e J t—At e -J- ι—At
7.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述步驟二中還包括:當(dāng)?shù)卮鸥袘?yīng)強(qiáng)度測量值sKn和參考值mag的偏差小于設(shè)定閾值時(shí)，地磁感應(yīng)強(qiáng)度的四
s m'H r?元數(shù)表示通過歸一化處理得到:1^ =P^ = 10 mU my,t 小否則，由對地坐標(biāo)系中的地磁通過四元數(shù)轉(zhuǎn)換得到:(S)Ja，sM=eql?ezml?eqi ； 其中，地磁感應(yīng)強(qiáng)度測量值+dm, +bm., +?^，其中，smt為地磁場的磁感應(yīng)強(qiáng)度，dm；t為磁場干擾，bm，t為磁場偏差，em；t為噪聲，mx，t、my，t、mz，t分別為時(shí)間t時(shí)的三個方向的地磁感應(yīng)強(qiáng)度分量，sMt為時(shí)間t時(shí)的地磁感應(yīng)強(qiáng)度測量值的歸一化表示即地磁感應(yīng)強(qiáng)度的四兀數(shù)表不，為測量時(shí)間內(nèi)的地磁感應(yīng)強(qiáng)度，= P mx m} m，q為在時(shí)間t時(shí)的單位四元數(shù)表示，%為1的共軛；根據(jù)聞斯牛頓法則，尋找cI = q。qx cIy qz，使/O,) =取最小值，以便于求取使陀螺儀角速度誤差最小的:^，其中為誤差函數(shù)； 求解陀螺儀偏差漂移補(bǔ)償四元數(shù)梯度向量，并對四元數(shù)梯度向量進(jìn)行歸一化處理:".Aq
sj+At — ~ "
Aq 其中為傳感器坐標(biāo)系相對于地坐標(biāo)系的陀螺儀方向的四元數(shù)的變化率，Δq為單位四元數(shù)的變化。
8.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述對讀取的陀螺儀角速度進(jìn)行偏差補(bǔ)償，角度誤差\_，角度偏差、_’補(bǔ)償后的傳感器坐標(biāo)系下的角度向量5氣._=''崎+&-''1+& ?’其中，sG^t+,t為陀螺儀軸中補(bǔ)償后的角誤差，&?+Δ?表示互補(bǔ)濾波融合后被估計(jì)方向的四元數(shù)表示的共軛，3"_被用作陀螺儀偏差漂移補(bǔ)償?shù)乃脑獢?shù)表示變化率，s?b，t+,tS角偏移，?為抵消非零均值陀螺儀測量誤差的收斂率，s?t+,t為角速度；計(jì)算由陀螺儀數(shù)據(jù)估算的四元數(shù)梯度向量其中=為陀螺







ω,?+Α?儀測量的四元數(shù)變化率，q ,為陀螺儀的被估計(jì)方向的單位四元數(shù)表示；


e I ω,? 計(jì)算經(jīng)過互補(bǔ)濾波器對陀螺儀偏差漂移進(jìn)行的補(bǔ)償，最終得到的角速度四元數(shù)向量e^fJ^eQfJ+e^f.^ ^其中m油，并進(jìn)行歸一化處理； 其中油互補(bǔ)濾波器融合的被估計(jì)方向的四元數(shù)表示，:q紳是互補(bǔ)濾波融合后被估計(jì)方向的四元數(shù)變化率為陀螺儀測量的四元數(shù)變化率，β為濾波器增益。
9.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述步驟三中將步驟二所得四元數(shù)轉(zhuǎn)換成歐拉角，由于機(jī)械臂每個部分只能進(jìn)行單一運(yùn)動方式如只能是俯仰、旋轉(zhuǎn)、擺動中的一種，所以求取的旋轉(zhuǎn)角只能是由相應(yīng)的運(yùn)動產(chǎn)生的橫擺角、俯仰角、滾擺角中的一種角度；四元數(shù)與歐拉角的轉(zhuǎn)換關(guān)系如下: 若四元數(shù)q= [w X y ζ]τ，則旋轉(zhuǎn)角度可由下式得出:
所述步驟三中，將各部分進(jìn)行多次測量測得的多個旋轉(zhuǎn)角取其均值，以確保測量的準(zhǔn)確性；在每個關(guān)節(jié)處最終的旋轉(zhuǎn)角《 = 1(Α+α2+...+ β ) 4中α為已求取的旋轉(zhuǎn)角，η為測量的次數(shù)。
10.如權(quán)利要求4所述的監(jiān)測方法，其特征是，所述步驟四的具體步驟為:通過旋轉(zhuǎn)角和臂長在相應(yīng)坐標(biāo)系中的投影測得各部分的姿態(tài)后，通過各部分的疊加得到機(jī)械臂最終的姿態(tài)。
【文檔編號】H04L29/06GK104182614SQ201410360729
【公開日】2014年12月3日 申請日期:2014年7月25日 優(yōu)先權(quán)日:2014年7月25日
【發(fā)明者】劉存根, 魯守銀, 王濤, 田婭, 孫麗萍 申請人:山東建筑大學(xué)