本發(fā)明涉及一種基于六維力傳感器的碰撞位置感知方法，提出利用六維力傳感器計算點接觸時剛性機械臂受力點位置，主要用于機器人交互領(lǐng)域。
背景技術(shù)：
：隨著機器人技術(shù)的不斷發(fā)展，傳統(tǒng)的無交互能力的機器人已經(jīng)逐漸向具有環(huán)境感知及環(huán)境交互方向發(fā)展，相關(guān)技術(shù)如視覺技術(shù)，力反饋技術(shù)，溫度感知技術(shù)等，同時對于環(huán)境的交互和感知能力要求越來越高。力感知不僅是機器人與外界交互的重要方式，而且還是部分裝配技術(shù)的關(guān)鍵，可以使機器人具備更強的環(huán)境適應(yīng)能力。依靠力信號，調(diào)整機器人相關(guān)狀態(tài)，以完成相關(guān)的任務(wù)。機械臂碰撞點的檢測，有助于輔助機器人更好的適應(yīng)環(huán)境，能夠更加精確的了解環(huán)境的狀況，從而做出決策。隨著對信息的進一步了解，要求機器人具有類似于人皮膚的感知性能，以滿足應(yīng)用需求。麻省理工學院研究出一款QCT的復合材料做成電子皮膚，其能夠感知到機器人與外界發(fā)生作用時的作用點及力的大小。其原理是這種材料在微小的壓力下，改變相應(yīng)的電阻值，從而或者綜合感知信息，類似的產(chǎn)品還有日本和飛利浦公司研制的電子皮膚，此類系統(tǒng)的優(yōu)點在于能夠直接感知力的作用點，并計算出力的作用方向和大小。但安裝于機器人中，要求完全覆蓋機器人外表面，線路較為復雜，且算法需要針對外形進行復雜的設(shè)計，解析度低、采樣速度慢、價格昂貴，適用性較差，易破損。技術(shù)實現(xiàn)要素：針對上述技術(shù)不足，本發(fā)明的目的提供一種通過計算的方式間接得到作用點的位置信息和力信息的方法。本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是：基于六維力傳感器的碰撞位置感知方法，包括以下步驟：1)在機械臂的第一關(guān)節(jié)與基座之間安置六維力傳感器；2)將六維力傳感器反饋的力和力矩信息代入考慮機械臂重力和運動學影響的力分析總體方程，得到碰撞點的作用力和力矩；3)根據(jù)碰撞點的作用力和力矩得到碰撞點在基坐標系的位置。所述力分析總體方程為：f=f0+fcB+fBdm=τ0+τcB+τBd]]>f和m分別為六維力傳感器測量的力和力矩；f0和τ0分別為僅考慮重力影響下機械臂的重力補償和重力力矩補償；fcB和τcB分別為不考慮重力影響及機械臂運動的動力學影響下、基于基坐標系原點的碰撞點的作用力和力矩；fBd和τBd分別為表示僅考慮動力學補償?shù)牧ρa償和力矩補償。所述僅考慮重力影響下機械臂的重力補償為各關(guān)節(jié)所受重力之和，重力力矩補償為各關(guān)節(jié)重力與基于基座標系的質(zhì)心的叉乘之和。所述僅考慮動力學補償?shù)牧ρa償和力矩補償分別為和τBd=Σn=1iτBdn;]]>其中，fBdn表示關(guān)節(jié)n運動產(chǎn)生的加速度力和科氏力轉(zhuǎn)換到基座標下的等效力；τBdn表示關(guān)節(jié)n運動產(chǎn)生的對基座標原點的等效力矩，i表示總的關(guān)節(jié)數(shù)量。所述根據(jù)碰撞點的作用力和力矩得到碰撞點在基坐標系的位置通過對下述公式求解得到：fcBmcB=M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)]]>fcB和τcB分別為不考慮重力影響及機械臂運動的動力學影響下、基于基坐標系原點的碰撞點的作用力和力矩；M為關(guān)系矩陣；表示碰撞點在碰撞點坐標系的位置；表示碰撞點在基坐標系的位置；表示由碰撞點坐標到基坐標的變換矩陣；T(·)3,1表示取矩陣的前3行前1列的元素。所述通過對下述公式求解根據(jù)關(guān)系矩陣與機械臂輪廓方程的相容性得到唯一的碰撞點，包括以下步驟：關(guān)系矩陣M的秩rank(M)＝6時，依據(jù)方程fcBmcB=M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)]]>并聯(lián)立機械臂輪廓方程，得到6組解；去除奇異解后，求取剩余解的平均值；然后根據(jù)機械臂輪廓方程得到距離平均解最近的輪廓上的點，即為碰撞點。本發(fā)明具有以下有益效果及優(yōu)點：1.本發(fā)明可快速、方便、準確地實現(xiàn)機械臂上點接觸的位置信息計算，使機器人具有更強的環(huán)境適應(yīng)能力。2.本發(fā)明充分考慮地面的機器人系統(tǒng)的運行條件，引入了重力補償和動力學補償，使計算碰撞點的位置更加精確。3.本發(fā)明利用1個六維力傳感器，使系統(tǒng)能夠感知到任意點的碰撞，相對于采用皮膚傳感器的方式降低了成本，且系統(tǒng)便于維護，可廣泛的應(yīng)用于力裝配機械臂、服務(wù)機器人等。4.本發(fā)明對于通過相容性求解，能夠提高求解碰撞位置的精度，提供更精確的碰撞位置信息，為后續(xù)決策提供更有效的信息。附圖說明圖1為本發(fā)明的方法流程圖。圖2為機械臂輪廓建模示意圖。圖3為理想狀態(tài)下的受力分析示意圖。圖4為引入重力補償時受力分析示意圖。圖5為機械臂運動狀態(tài)下的動態(tài)力分析示意圖。圖6為碰撞點相容性分析步驟示意圖。具體實施方式下面結(jié)合實施例對本發(fā)明做進一步的詳細說明。本發(fā)明為實現(xiàn)點接觸時剛性機械臂受力點位置信息的獲取的一種基于力、 力矩傳感器的位置感知方法，在機械臂的第一關(guān)節(jié)與基座之間設(shè)置六維力傳感器，提出利用六維力傳感器計算點接觸時剛性機械臂受力點位置。如圖1所示，包括以下步驟：(1)引入重力補償和動力學補償，列出力分析總體方程；(2)結(jié)合力分析約束、電流約束、法向向量約束和機械臂輪廓約束對算法可解性進行分析；(3)根據(jù)相容原理，消除無解現(xiàn)象，解出受力點位置。所述重力補償為考慮重力效應(yīng)對傳感器測量結(jié)果的影響，添加對應(yīng)的力補償項f0和力矩補償項τ0；其中，f0＝G0+G1+G2+...+Gn-1+Gn+..+Gi(1)Gi＝(0,0,Gi)T為關(guān)節(jié)i所受的重力；τ0＝r0×G0+r1×G1+...+rn-1×Gn-1+rn×Gn+...+ri×Gi(2)rn為關(guān)節(jié)n的質(zhì)心基于基座標系(B坐標系)的矢量，n＝1…i。所述動力學補償為機械臂各關(guān)節(jié)運動時施加于B坐標系原點的等效力和等效力矩，fBd=Σn=1ifBdnτBd=Σn=1iτBdn---(3)]]>其中，fBdn表示n關(guān)節(jié)運動產(chǎn)生的加速度力和科氏力轉(zhuǎn)換到基座標下的等效力；τBdn表示關(guān)節(jié)n運動產(chǎn)生的對基座標原點的等效力矩。i表示總的關(guān)節(jié)數(shù)量。所述力分析約束可以描述為通過對關(guān)系矩陣M秩的分析將力和力矩方程組分為5個求解方程和1個線性相關(guān)方程，進而得到結(jié)果的相容性。所述電流約束可以描述為假設(shè)碰撞發(fā)生在第n關(guān)節(jié)，則第n關(guān)節(jié)的電流必然會發(fā)生突變，由于力的相互作用，編號小于n的關(guān)節(jié)都會存在電流突變的現(xiàn)象。而對于關(guān)節(jié)編號大于n，則電流不存在相應(yīng)的突變影響。所述法向向量約束可以表示為pTn＜0其中，pT表示碰撞點所受外界作用力的轉(zhuǎn)置；n表示碰撞點的法向量。所述機械臂輪廓約束可以描述為碰撞點位于機械臂表面，且滿足方程S(c)＝0；其中S()表示機械臂輪廓方程。所述相容性分析即根據(jù)相容原理，消除無解現(xiàn)象，解出受力點位置；可以描述為由于噪聲等因素所導致的解的偏離并不大，因此可以通過相容分析求得碰撞的估計點，其將接近實際的碰撞點。本發(fā)明利用力、力矩傳感器來間接計算受力點位置信息，其流程圖如圖1所示。具體實施步驟如下：(1)列出力分析總體方程；圖3表示理想狀態(tài)下的受力分析。在每個關(guān)節(jié)上設(shè)置本地坐標系；圖中顯示了機械臂的某一關(guān)節(jié)(n關(guān)節(jié))上任意點c的受力分析，坐標系B為力傳感器的本地坐標系即基座標系，c坐標系為接觸點的坐標系，Tn為接觸點c所在剛體的本地坐標系即關(guān)節(jié)n的本地坐標系，Ti為i關(guān)節(jié)的本地坐標系(i＝1、2…)，c坐標系與B坐標系之間可通過坐標轉(zhuǎn)換得到。假設(shè)接觸點位于第n關(guān)節(jié)，接觸點c的坐標系通過第n關(guān)節(jié)(受碰撞的關(guān)節(jié))的本地坐標系平移得到，假設(shè)由坐標c變換到B的變換矩陣為則變換矩陣為為關(guān)節(jié)1的本地坐標變換到基座標系的轉(zhuǎn)換矩陣，依次類推，為坐標c變換到關(guān)節(jié)n本地坐標系的轉(zhuǎn)換矩陣。當發(fā)生點接觸時，假設(shè)c點的外界作用力為p＝(fcx,fcy,fcz)T，fcx,fcy,fcz分別為c點在c坐標系x、y、z軸上的力，在無重力影響下，力傳感器的測量值分別為f＝(fx,fy,fz)T和m＝(τx,τy,τz)Tfx,fy,fz為六維力傳感器沿基座標系x,y,z方向的力，τx,τy,τz為六維力傳感器相對基座標系x,y,z軸的力矩。將碰撞點c的力及力矩通過坐標變換可轉(zhuǎn)換相對于基座標的力和力矩。則fcB=(fcBx,fcBy,fcBz)T=(T(p‾TTBc)3,1)T---(4)]]>p‾=(fcx,fcy,fcz,1)T---(5)]]>τcB=(τcBx,τcBy,τcBz)T=cB×p(T(c‾nTBc)3,1)×p---(6)]]>c‾n=(xcn,ycn,zcn,1)T---(7)]]>fcB和τcB分別為不考慮重力影響下基于基坐標系原點的碰撞點的作用力和力矩；表示碰撞力在碰撞點坐標系的力向量，fcx,fcy,fcz分別表示碰撞力在碰撞點坐標系x,y,z方向的力；表示碰撞點在相應(yīng)碰撞關(guān)節(jié)坐標系的位置向量，xcn,ycn,zcn表示碰撞點在相應(yīng)碰撞關(guān)節(jié)坐標系沿x,y,z方向的位置；表示由碰撞點所在關(guān)節(jié)的坐標到基坐標的變換矩陣；T(·)3,1表示取矩陣的前3行前1列的元素，cB表示碰撞點在基座標系的坐標位置。由于力矩傳感器的數(shù)值是由c點的力及力矩產(chǎn)生的，因此在理想狀態(tài)下，有如下關(guān)系式f=fcBm=τcB---(8)]]>圖4表示重力效應(yīng)下受力分析，事實上，重力對傳感器讀數(shù)偏差的影響可以從本質(zhì)上改變計算結(jié)果。而讀數(shù)偏差是關(guān)于各連桿參數(shù)(連桿重心和質(zhì)量)及關(guān)節(jié)轉(zhuǎn)角的函數(shù)。為消除由于重力產(chǎn)生的偏差影響，應(yīng)對重力效應(yīng)下模型進行分析。如圖4所示，假設(shè)連桿的重心位于軸線上。重力效應(yīng)作用下的力分析，假設(shè)重力效應(yīng)下傳感器讀數(shù)為f0∈R3，τ0∈R3，則兩者都是桿件質(zhì)心、桿件重量、關(guān)節(jié)轉(zhuǎn)角(rn是關(guān)節(jié)轉(zhuǎn)角和桿件質(zhì)心位置的函數(shù)；Gn表示重力，是連桿質(zhì)量的函數(shù))的函數(shù)。根據(jù)圖4求取之間關(guān)系為：f0＝G0+G1+G2+...+Gn-1+Gn+..+Gi(9)Gn＝(0,0,Gn)Tτ0=r0×G0+r1×G1+...+rn-1×Gn-1+rn×Gn+...+ri×Gi=G^0r0+G^1r1+...+G^n-1rn-1+G^nrn+...+G^iri---(10)]]>其中Gn的置換矩陣G^n=00-Gn000Gn00,]]>Gn為n關(guān)節(jié)的重力。則，考慮重力效應(yīng)下的受力點位置分析方程為：f=f0+fcBm=τ0+τcB---(11)]]>fm=M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)+M0f0r0+...+Mifiri=M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)+Σn=0iMnfnrn---(12)]]>M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)]]>這一項就是碰撞產(chǎn)生的力和力矩，相當于fcB和τcB；M0f0r0+...+Mifiri]]>為Σn=0iMnfnrn]]>相當于所有關(guān)節(jié)從1到i個關(guān)節(jié)的重力影響的和，相當于f0和τ0；M為關(guān)系矩陣。f和m分別為六維力傳感器測量的力和力矩；f0和τ0分別為僅考慮重力影響、基于基坐標系原點的作用力和力矩；fcB和τcB分別為不考慮重力影響、碰撞時碰撞點基于基坐標系原點的作用力和力矩；同樣對于式(12)，將得到5個非線性方程，需要通過機械臂的輪廓的邊界約束，得到解。機械臂的各關(guān)節(jié)在運動時，必然會產(chǎn)生反向力，作用于力傳感器。在分析碰撞點時，需要將該力進行考慮，并計算其大小。圖5表示機械臂運動狀態(tài)下動態(tài)力分析，是所有運動物體的累加效果。假設(shè)n關(guān)節(jié)的角速度為ωn，加速度為αn，關(guān)節(jié)轉(zhuǎn)角為θn；則關(guān)節(jié)n的重心的加速度為an＝αn×Dn，關(guān)節(jié)n此刻產(chǎn)生的力和力矩為fαdn和ταdn；關(guān)節(jié)重心的坐標相對于關(guān)節(jié)n的坐標系為Dn＝(xdn,ydn,zdn)，則mn表示關(guān)節(jié)n的質(zhì)量，anx、any、anz,表示關(guān)節(jié)n的重心相對第n關(guān)節(jié)坐標系沿x,y,z方向的加速度。α^n=0-anzanyanz0-anx-anyanx0]]>ταdn＝Inαn(13)其中In為第n關(guān)節(jié)的轉(zhuǎn)動慣量，力矩可以等效到任一點，力進行等效時，則需要等效為力和力矩。第n關(guān)節(jié)相對于Tn(第n關(guān)節(jié)的坐標系)坐標系的科氏力 fkdn＝2mnvTn×ωTn，，vTn表示Tn坐標系相對基座標的速度，ωTn表示Tn相對于基座標系的角速度。ffBdn=(T(fαdnTTBD)3,1)TfkBdn=(T(fkdnTTBn)3,1)T---(14)]]>τfBdn=ffBdn×DBn=f^fBdn(T(D‾nTBn)3,1)TτkBdn=fkBdn×Tn=f^kBdn(T(TBn)(1,2,3),4)T---(15)]]>從而得到總的作用力和力矩為fBdn＝ffBdn+fkBdn(16)τBdn＝ταBdn+τfBdn++τkBdn(17)其中ταBdn＝ταdnffBdn表示fαdn轉(zhuǎn)換到基座標系的力，τfBdn表示fαdn轉(zhuǎn)換時產(chǎn)生的力矩，fkBdn表示fkdn轉(zhuǎn)換到基座標系的力，τkBdn表示fkdn轉(zhuǎn)換時產(chǎn)生的力矩。是對Dn＝(xdn,ydn,zdn)的變換。Tn表示n坐標系在基座標系下的位置矢量，T(·)(1,2,3),4表示取矩陣的第四列的前三行元素，表示關(guān)節(jié)n坐標系到基座標系的轉(zhuǎn)換矩陣；DBn表示第n關(guān)節(jié)重心在基座標系中的矢量。ταBdn表示力矩ταdn轉(zhuǎn)換到基座標系下的力矩。fBdn表示由于動力學基座產(chǎn)生的總的力。τBdn表示由于動力學基座產(chǎn)生的總的力矩對于機械臂的多個關(guān)節(jié)，可以得到運動產(chǎn)生的機械臂的fBd=Σn=1ifBdnτBd=Σn=1iτBdn---(18)]]>f=f0+fcB+fBdm=τ0+τcB+τBd---(19)]]>因此，考慮重力和機械臂運動時反作用力的影響，列出力分析的總體方程f=f0+fcB+fBdm=τ0+τcB+τBd---(20)]]>其中f和m分別表示在考慮重力效應(yīng)和機械臂運動影響的情況下，傳感器 測出接觸點的力和力矩；f0和τ0分別表示僅考慮重力效應(yīng)時的機械臂的重力補償(項)和力矩補償(項)；fBd和τBd分別表示僅考慮機械臂運動所帶來的力補償(項)和力矩補償(項)；fcB和τcB分別表示理想狀態(tài)下，即不考慮重力影響及機械臂運動的動力學影響下，靜態(tài)機械臂點接觸時基坐標下原點的等效力和力矩。(2)可解性分析，具體包含以下4個方面：1.力分析約束將作用于剛體上的力p表示為矩陣形式p^=0-fczfcyfcz0-fcx-fcyfcx0]]>其中，fcx,fcy,fcz分別表示碰撞點所受作用力在碰撞點坐標系(c坐標系)下沿x軸，y軸，z軸分力。此時傳感器測出的理想狀態(tài)下力矩其中表示由c坐標到B坐標的變換矩陣；為碰撞點在第n關(guān)節(jié)的碰撞位置，xcn，xyn，xzn表示碰撞點在第n關(guān)節(jié)坐標系的坐標位置；T(·)3,1表示取矩陣的前3行前1列的元素。將m=τcB=p^(T(c‾nTBc)3,1)]]>和f=fcBm=τcB]]>聯(lián)立，可得fm=M(T(p‾TTBc)3,1)T(T(c‾nTBc)3,1)---(21)M=1000000100000010000000-fczfcy000fcz0-fcx000-fcyfcx0---(22)]]>在去除重力因素和動力學影響因素后，通過對公式(21)求解得到碰撞點在基坐標系的位置。其中M為關(guān)系矩陣，理想狀態(tài)下，rank(M)＝5，因此M為奇異矩陣，不能解出相應(yīng)接觸點的位置及作用力大小。通過式(21)可得到5個 相關(guān)的求解方程，另一方程與其線性相關(guān)，可作為對方程求解結(jié)果的驗證。2.電流約束根據(jù)各關(guān)節(jié)電流的檢測，如果某一關(guān)節(jié)的電流突變，則該關(guān)節(jié)受碰撞。假設(shè)碰撞發(fā)生在第n關(guān)節(jié)，則第n關(guān)節(jié)的電流必然會發(fā)生突變，由于力的相互作用，編號小于n的關(guān)節(jié)都會存在電流突變的現(xiàn)象。而對于關(guān)節(jié)編號大于n，電流不存在相應(yīng)的突變影響。根據(jù)該約束，可直接判斷作用點位于哪一關(guān)節(jié)，一方面避免了多解的存在，另一方面，加快了解的求解速度。3.法向向量約束通過法向向量約束pTn＜0可以將方程產(chǎn)生的多解進行篩選，保證所求的力符合客觀事實。即滿足接觸點的法向向量與作用力向量的夾角小于90°的方程約束。4.機械臂輪廓約束為尋求解的存在，將需要對機械臂的輪廓進行幾何建模，碰撞點位于機械臂的表面，方程可表示為S(c)＝0，S(c)＝cTATAc＝1；其中，A為常系數(shù)的對角陣，且A=1/α0001/β0001/γ,]]>c表示接觸點在所在關(guān)節(jié)坐標系的坐標。如圖2所示的機械臂輪廓建模方法示意，常見的幾何外形多為平面、球形、圓柱形，及橢球形，四種形狀基本能夠構(gòu)建所有的簡單曲面。機械臂的外觀形狀可以基本完全由四種形狀拼接構(gòu)建起來。從幾何變化的角度講，四種幾何形狀的核心形狀為橢球形，其余三種形狀都可由其變化產(chǎn)生。本文將針對橢球形球面的接觸點方程進行計算與研究。我們假設(shè)，坐標系原點在橢球球心，本地坐標系的xyz三軸分別為橢球的三軸，則可得到橢球面方程：S(c)＝cTATAc＝1(23)其中，A為常系數(shù)的對角陣，且A=1/α0001/β0001/γ]]>則橢球形的三個軸的軸長分別為2α，2β，2γ，且α＞0β＞0γ＞0。當α＝β＝γ則 方程表示球形表面；當α→+∞趨于無窮時，則表示半徑為1/β圓柱，當α→+∞且β→+∞，γ→0則表示平面。點c位于曲面上，則c處的單位法向量n為n=▿S(c)||▿S(c)||=2A2c||2A2c||=A2c||A2c||---(24)]]>式中，表示梯度，c表示接觸點坐標。(3)相容性分析步驟(2)中所得關(guān)系矩陣M的秩rank(M)＝5時，所求的解具有唯一性，此時相容性最佳。但是許多因素都將導致rank(M)＝6，這就導致所求解的矛盾性。由于噪聲等因素所導致的解的偏離并不大，因此通過相容分析方法求得碰撞的估計點，如圖6所述，求將接近實際的碰撞點。當rank(M)＝6時，聯(lián)立式(21)與輪廓描述方程，通過從式(21)中選取5個方程，并結(jié)合輪廓方程的方式，共計得到6組解，去除奇異解后，求取剩余解的平均值，然后依靠輪廓描述方程得到距離平均解最近的輪廓上的點，即為碰撞點，之后可求取碰撞力。因此，在對式(21)的求解具體步驟如下(如圖6)：首先，理論上通過式(21)，Rank(M)＝5，通過聯(lián)立機械臂的輪廓方程，可得到碰撞點的解，但是由于測量的偏差，通過式(21)可得到6個非線性方程，聯(lián)立機械臂輪廓方程S(c)＝0，將產(chǎn)生6個未知數(shù)，7個方程的狀況，因此在式(21)中任意選擇5個方程，并聯(lián)立輪廓方程可求得方程的一組解；上述中的輪廓方程是根據(jù)各關(guān)節(jié)電流的檢測，電流突變的關(guān)節(jié)中，編號最大的關(guān)節(jié)(電流突變的關(guān)節(jié)很多，是從0開始，0,1,2,3….等，一直到碰撞關(guān)節(jié)，并且不是判斷誰變的大，而是變的這些關(guān)節(jié)中，編號最大的)為受碰撞的關(guān)節(jié)，從而可以迅速確定具體關(guān)節(jié)的輪廓方程，而無需建立整個機械臂輪廓方程。在求解的過程中，根據(jù)法向向量約束pTn＜0，滿足接觸點的法向向量與作用力向量的夾角小于90°，用于排除求解過程中無效解的狀況。上述中，式(21)中六個方程中選擇5個方程，有六種選擇方式，因此可得到六組解。最后，通過去除奇異解，求取剩余解的平均值，然后依靠輪廓描述方程得到距離平均解最近的輪廓上的點，即為碰撞點。當前第1頁1 2 3