本發(fā)明屬于工業(yè)機器人運動控制領(lǐng)域，更具體地，涉及一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制方法及系統(tǒng)。
背景技術(shù)：
：運動控制指令是實現(xiàn)以指定速度、特定路線模式等將工具從一個位置移動到另一個指定位置，在使用運動指令時需指定采用什么運動方式來控制到達指定位置的運動路徑，機器人運動的類型有三種：關(guān)節(jié)運動(j)、直線運動(l)、圓弧運動(c)。六關(guān)節(jié)工業(yè)機器人的后三個關(guān)節(jié)軸線交于一點，當機器人第四關(guān)節(jié)和第六關(guān)節(jié)軸線重合，運動相互抵消，機器人將喪失一個自由度，此為機器人俯/仰關(guān)節(jié)屬性的臨界點，也是機器人的內(nèi)部奇異形位，可稱為機器人的姿態(tài)奇點。當處于姿態(tài)奇點時，機器人的運動類型為直線運動或者圓弧運動，此時運動控制器不知該如何選取第四關(guān)節(jié)和第六關(guān)節(jié)的值，導(dǎo)致機械臂失控并發(fā)出警報，使機器人無法按照規(guī)劃的路徑通過姿態(tài)奇點，這種情況在實際應(yīng)用中經(jīng)常出現(xiàn)，不僅使機器人的運動空間縮小，而且給生產(chǎn)帶來了很大的不便。目前對于工業(yè)機器人的姿態(tài)奇點主要有以下三種處理方法：1)避讓機器人的姿態(tài)奇點：當遇到姿態(tài)奇點時，改變機器人的運動路徑從而避讓奇異形位；2)路徑規(guī)劃方法：采用對姿態(tài)奇點進行路徑規(guī)劃的方法使得機器人通過姿態(tài)奇點，通過迭代運算得到關(guān)節(jié)變量值的解析解；3)調(diào)整姿態(tài)法：當處于姿態(tài)奇點時，第四關(guān)節(jié)和第六關(guān)節(jié)的關(guān)節(jié)變量的和或差為定值，通過協(xié)調(diào)兩者的值使得機器人通過姿態(tài)奇點。然而進一步的研究表明，上述方法仍然存在以下問題：避讓姿態(tài)奇點法雖然使機器人從姿態(tài)奇點附近的路徑通過，避開了姿態(tài)奇點，但是其改變了機器人原有的路徑并且具有盲目性，需要對機器人所有的姿態(tài)奇點進行避讓，可操作性不強，同時也使得機器人的空間范圍縮??；路徑規(guī)劃方法是對姿態(tài)奇點所處的路徑重新進行規(guī)劃，采用復(fù)雜的迭代運算求得機器人關(guān)節(jié)變量的解析解，不能得到精確的數(shù)值解，使得機器人不能準確的通過姿態(tài)奇點，并且計算繁瑣，工作量較大；調(diào)整姿態(tài)法雖然可以使得機器人準確的通過姿態(tài)奇點，但是在協(xié)調(diào)運動的過程中機器人處于停留狀態(tài)，使得機器人的效率降低，并且可能導(dǎo)致協(xié)調(diào)后的運動速度不連續(xù)，給實際生產(chǎn)帶來不便。技術(shù)實現(xiàn)要素：針對現(xiàn)有技術(shù)的以上缺陷或改進需求，本發(fā)明提供了一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制方法及系統(tǒng)，其采用插補替換機器人末端姿態(tài)的方式，在不改變原有規(guī)劃路徑的前提下使得機器人順利通過姿態(tài)奇點，由此解決目前因姿態(tài)奇點導(dǎo)致機器人運動范圍受限的技術(shù)問題，具有簡單易行、運算效率高、適用性強等特點。為實現(xiàn)上述目的，按照本發(fā)明的一個方面，提出了一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制方法，其包括如下步驟：(1)截取姿態(tài)奇點所處工業(yè)機器人預(yù)設(shè)的規(guī)劃路徑中的一段軌跡，獲取該軌跡起點對應(yīng)的位置值和后三個關(guān)節(jié)變量值以及該軌跡終點對應(yīng)的位置值和后三個關(guān)節(jié)變量值；(2)根據(jù)所述起點的后三個關(guān)節(jié)變量值以及終點的后三個關(guān)節(jié)變量值以插補的方式獲得姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值；(3)建立運動學反解方程求取姿態(tài)奇點位置對應(yīng)的前三個關(guān)節(jié)變量值；(4)以所述姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值以及前三個關(guān)節(jié)變量值作為工業(yè)機器人在姿態(tài)奇點位置對應(yīng)的運動控制參數(shù)，實現(xiàn)工業(yè)機器人的運動控制，以此使得六關(guān)節(jié)工業(yè)機器人順利通過姿態(tài)奇點。作為進一步優(yōu)選的，所述步驟(3)包括如下子步驟：(3.1)首先建立如下矩陣方程：其中，xt,yt,zt為工具坐標系原點相對于基座標系的三個坐標分量，s1＝sin(θ1)，s2＝sin(θ2)，s23＝sin(θ2+θ3)，c1＝cos(θ1)，c2＝cos(θ2)，c23＝cos(θ2+θ3)，θ1,θ2,θ3為工業(yè)機器人第一關(guān)節(jié)至第三關(guān)節(jié)的關(guān)節(jié)變量，a1和a2分別表示第一連桿和第二連桿的長度，d2表示第一連桿和第二連桿之間的偏置，x3,y3,z3為工具坐標系原點相對于第三連桿坐標系的三個坐標分量；(3.2)對步驟(3.1)中建立的矩陣方程兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第一關(guān)節(jié)和第三關(guān)節(jié)的關(guān)節(jié)值θ1和θ3：其中，a1和a2分別表示第一連桿和第二連桿的長度，d2表示第一連桿、第二連桿之間的偏置；(3.3)對步驟(3.1)中建立的矩陣方程兩端同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第二關(guān)節(jié)的關(guān)節(jié)值θ2：其中，c3＝cos(θ3)，s3＝sin(θ3)。作為進一步優(yōu)選的，所述插補的總點數(shù)n優(yōu)選采用下述方式確定：其中：x1,y1,z1為軌跡起點的位置值，x2,y2,z2為軌跡終點的位置值，v為插補速度，t為插補周期。作為進一步優(yōu)選的，所述插補速度優(yōu)選為10mm/s，插補周期優(yōu)選為1ms。作為進一步優(yōu)選的，所述步驟(1)之前還包括如下步驟：首先根據(jù)六關(guān)節(jié)工業(yè)機器人建立連桿坐標系，并對預(yù)設(shè)的規(guī)劃路徑進行運動學反解獲得工業(yè)機器人的各關(guān)節(jié)變量值，然后判斷六關(guān)節(jié)工業(yè)機器人是否處于姿態(tài)奇點，若是，則進入步驟(1)，若否，則以反解獲得的工業(yè)機器人的各關(guān)節(jié)變量值實現(xiàn)工業(yè)機器人的運動控制。按照本發(fā)明的另一個方面，提供了一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制系統(tǒng)，其包括：數(shù)據(jù)獲取模塊，用于截取姿態(tài)奇點所處工業(yè)機器人預(yù)設(shè)的規(guī)劃路徑中的一段軌跡，獲取該軌跡起點對應(yīng)的位置值和后三個關(guān)節(jié)變量值以及該軌跡終點對應(yīng)的位置值和后三個關(guān)節(jié)變量值；插補模塊，用于根據(jù)所述起點的后三個關(guān)節(jié)變量值以及終點的后三個關(guān)節(jié)變量值以插補的方式獲得姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值；數(shù)據(jù)處理模塊，用于建立運動學反解方程并求取姿態(tài)奇點位置對應(yīng)的前三個關(guān)節(jié)變量值；運動控制模塊，用于以所述姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值以及前三個關(guān)節(jié)變量值作為工業(yè)機器人在姿態(tài)奇點位置對應(yīng)的運動控制參數(shù)，實現(xiàn)工業(yè)機器人的運動控制，以此使得六關(guān)節(jié)工業(yè)機器人順利通過姿態(tài)奇點。作為進一步優(yōu)選的，所述數(shù)據(jù)處理模塊包括如下子模塊：矩陣方程建立子模塊，用于建立如下方程：其中，xt,yt,zt為工具坐標系原點相對于基座標系的三個坐標分量，s1＝sin(θ1)，s2＝sin(θ2)，s23＝sin(θ2+θ3)，c1＝cos(θ1)，c2＝cos(θ2)，c23＝cos(θ2+θ3)，θ1,θ2,θ3為工業(yè)機器人第一關(guān)節(jié)至第三關(guān)節(jié)的關(guān)節(jié)變量，a1和a2分別表示第一連桿和第二連桿的長度，d2表示第一連桿和第二連桿之間的偏置，x3,y3,z3為工具坐標系原點相對于第三連桿坐標系的三個坐標分量；第一關(guān)節(jié)值計算子模塊，用于對矩陣方程建立子模塊建立的矩陣方程兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第一關(guān)節(jié)和第三關(guān)節(jié)的關(guān)節(jié)值θ1和θ3：第二關(guān)節(jié)值計算子模塊，用于對矩陣方程建立子模塊建立的矩陣方程兩端同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第二關(guān)節(jié)的關(guān)節(jié)值θ2：其中，c3＝cos(θ3)，s3＝sin(θ3)。作為進一步優(yōu)選的，所述插補模塊進行插補時的插補總點數(shù)n優(yōu)選采用下述方式確定：其中：x1,y1,z1為軌跡起點的位置值，x2,y2,z2為軌跡終點的位置值，v為插補速度，t為插補周期。作為進一步優(yōu)選的，所述插補速度優(yōu)選為10mm/s，插補周期優(yōu)選為1ms。作為進一步優(yōu)選的，所述控制系統(tǒng)還包括判斷模塊，用于根據(jù)六關(guān)節(jié)工業(yè)機器人建立連桿坐標系，并對預(yù)設(shè)的規(guī)劃路徑進行運動學反解獲得工業(yè)機器人的各關(guān)節(jié)變量值，然后判斷六關(guān)節(jié)工業(yè)機器人是否處于姿態(tài)奇點，若是，則進入步驟(1)，若否，則以反解獲得的工業(yè)機器人的各關(guān)節(jié)變量值實現(xiàn)工業(yè)機器人的運動控制。總體而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，主要具備以下的技術(shù)優(yōu)點：1.本發(fā)明采用將工業(yè)機器人末端原有位置保持不變，插補得到工業(yè)機器人末端姿態(tài)的方式，在不改變原有規(guī)劃路徑的前提下使得工業(yè)機器人順利通過姿態(tài)奇點，大大降低了通過外部設(shè)備渡過姿態(tài)奇點的生產(chǎn)成本，避免了采用路徑規(guī)劃方式得到解析解的復(fù)雜迭代運算，可直接得到關(guān)節(jié)位置的數(shù)值解，簡單易行且運算效率高，適用于后三個關(guān)節(jié)交于一點的工業(yè)機器人。2.本發(fā)明從工業(yè)機器人運動控制領(lǐng)域出發(fā)，使得工業(yè)機器人順利通過姿態(tài)奇點，擴展了工業(yè)機器人的運動控制指令范圍，使得工業(yè)機器人的運動空間得到擴大，更加的靈活和智能化，避免了因姿態(tài)奇點造成運動范圍縮小或報警等問題，可直接將程序植入到工業(yè)機器人運動控制器中，使得工業(yè)機器人不用停留即可順利通過姿態(tài)奇點，給實際生產(chǎn)帶來很大便利。3.本發(fā)明還可以應(yīng)用到無姿態(tài)奇點的路徑規(guī)劃，不通過運動學反解得到工作路徑中一系列點的關(guān)節(jié)值，進而避免了反解過程中第四關(guān)節(jié)的關(guān)節(jié)變量值無法求取的現(xiàn)象，可直接通過插補得到關(guān)節(jié)值，簡單易行，實用性強。附圖說明圖1是六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的流程圖；圖2(a)和(b)是華中數(shù)控6012工業(yè)機器人的構(gòu)型圖；圖3是華中數(shù)控6012工業(yè)機器人的各連桿坐標系。具體實施方式為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合附圖及實施例，對本發(fā)明進行進一步詳細說明。應(yīng)當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個實施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。本發(fā)明的一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制方法，具體包括如下步驟：(1)截取姿態(tài)奇點所處工業(yè)機器人預(yù)設(shè)的規(guī)劃路徑中的一段軌跡，獲取該軌跡起始點的位置值和后三個關(guān)節(jié)變量值以及軌跡終點的位置值和后三個關(guān)節(jié)變量值；(2)根據(jù)所述起始點的后三個關(guān)節(jié)變量值以及終點的后三個關(guān)節(jié)變量值以插補的方式獲得姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值；(3)求取姿態(tài)奇點位置對應(yīng)的前三個關(guān)節(jié)變量值；(4)以所述姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值以及前三個關(guān)節(jié)變量值作為工業(yè)機器人在姿態(tài)奇點位置對應(yīng)的運動控制參數(shù)，實現(xiàn)工業(yè)機器人的運動控制，以此使得六關(guān)節(jié)工業(yè)機器人順利通過姿態(tài)奇點。為了便于各個變量的求取與計算，在步驟(1)之前需做如下準備工作：根據(jù)六關(guān)節(jié)工業(yè)機器人(包括六個關(guān)節(jié)和六根連桿，分別為第一關(guān)節(jié)、第二關(guān)節(jié)、第三關(guān)節(jié)、第四關(guān)節(jié)、第五關(guān)節(jié)、第六關(guān)節(jié)，第一連桿、第二連桿、第三連桿、第四連桿、第五連桿、第六連桿)的結(jié)構(gòu)特點建立連桿坐標系，該坐標系可采用諸多現(xiàn)有方式進行建立，本發(fā)明以d-h法建立坐標系為例進行示例性說明，不作為對本發(fā)明的限定。根據(jù)工業(yè)機器人的結(jié)構(gòu)特點利用d-h法建立連桿坐標系：將關(guān)節(jié)i的軸線方向作為坐標系{i}的z軸zi，將關(guān)節(jié)i和i+1的軸線公垂線方向作為坐標系{i}的x軸xi，指向由關(guān)節(jié)i到關(guān)節(jié)i+1，根據(jù)右手法則規(guī)定坐標系{i}的y軸yi，xi和yi的交點作為坐標系{i}的原點oi，基坐標系任意選定，為了簡單方便起見，優(yōu)選的基坐標系{0}與連桿1的坐標系{1}重合，將工業(yè)機器人后三個關(guān)節(jié)軸線的交點選作為連桿坐標系{4}，{5}和{6}的原點，至此建立工業(yè)機器人的各連桿坐標系。根據(jù)上述所設(shè)定的連桿坐標系，相應(yīng)的連桿參數(shù)可定義如下：ai-1＝從zi-1到zi沿xi-1測量的距離；αi-1＝從zi-1到zi繞xi-1旋轉(zhuǎn)的角度；di＝從xi-1到xi沿zi測量的距離；θi＝從xi-1到xi繞zi旋轉(zhuǎn)的角度。根據(jù)建立的各連桿坐標系將相應(yīng)的連桿參數(shù)列出，利用所列的連桿參數(shù)計算出各個連桿變換矩陣推導(dǎo)出第三連桿坐標系相對于基坐標系的等價其次變換矩陣和第六連桿坐標系相對于第三連桿坐標系的齊次變換矩陣其中，i＝0,1...,6，θ1,θ2,θ3為第一關(guān)節(jié)至第三關(guān)節(jié)的關(guān)節(jié)變量，{i}表示與連桿i固接的坐標系,為工業(yè)機器人連桿坐標系{i}相對于{i+1}的齊次變換矩陣。具體的，如圖3所示，第一關(guān)節(jié)的軸線為鉛直方向，將其作為坐標系{1}的z軸z1，優(yōu)選的指向向上，將第一關(guān)節(jié)和第二關(guān)節(jié)的軸線公垂線方向作為坐標系{1}的x軸x1，指向由第一關(guān)節(jié)到第二關(guān)節(jié)，那么x1和z1的交點即為坐標系{1}的原點o1；第二關(guān)節(jié)和第三關(guān)節(jié)的軸線為水平方向，分別作為坐標系{2}和{3}的z軸z2和z3，且z2和z3相互平行，將第二關(guān)節(jié)和第三關(guān)節(jié)的軸線公垂線方向作為坐標系{2}的x軸x2，x2和z2的交點即為坐標系{2}的原點o2；將第三關(guān)節(jié)和第四關(guān)節(jié)的軸線公垂線方向作為坐標系{3}的x軸x3，x3和z3的交點即為坐標系{3}的原點o3；基坐標系可以任意選定，為了簡單方便起見，優(yōu)選的基坐標系{0}與第一連桿的坐標系{1}重合；第四關(guān)節(jié),第五關(guān)節(jié)和第六關(guān)節(jié)的軸線交于一點，將該點選作為連桿坐標系{4}，{5}和{6}的原點o4,5,6，將第四關(guān)節(jié)的軸線方向作為坐標系{4}的z軸z4，優(yōu)選的坐標系{4}的x軸x4的方向與x3一致，將第五關(guān)節(jié)的軸線方向作為坐標系{5}的z軸z5，優(yōu)選的坐標系{6}的z軸z6的方向與z4一致，優(yōu)選的坐標系{5}的x軸x5和坐標系{6}的x軸x6的方向與x4一致；優(yōu)選的工具末端中心作為工具坐標系{t}的原點ot，優(yōu)選的工具坐標系{t}的x軸xt的方向與x6一致，優(yōu)選的工具坐標系{t}的z軸zt的方向與z6一致。從z1到z2沿x1的距離為a1，從x1到x2沿z2的距離為d2，從z2到z3沿x2的距離為a2，從z3到z4沿x3的距離為a3，從x3到x4沿z4的距離為d4。根據(jù)建立的各連桿坐標系將相應(yīng)的連桿參數(shù)列出，利用所列的連桿參數(shù)計算出各個連桿變換矩陣推導(dǎo)出第三連桿坐標系相對于基坐標系的等價其次變換矩陣和第六連桿坐標系相對于第三連桿坐標系的齊次變換矩陣其中，i＝0,1...,6，θi為關(guān)節(jié)i的關(guān)節(jié)變量，si＝sin(θi)，ci＝cos(θi)，s23＝sin(θ2+θ3)，c23＝cos(θ2+θ3)，{i}表示與連桿i固接的坐標系,為工業(yè)機器人連桿坐標系{i}相對于{i-1}的齊次變換矩陣。建立連桿坐標系后，對預(yù)設(shè)的規(guī)劃路徑(該路徑由實際運動需求確定，本發(fā)明不做限定)進行運動學反解獲得工業(yè)機器人的各關(guān)節(jié)變量值，其為現(xiàn)有技術(shù)，在此不贅述，正常情況下工業(yè)機器人根據(jù)預(yù)設(shè)的規(guī)劃路徑進行運動，也即不處于姿態(tài)奇點時，工業(yè)機器人正常運行，當?shù)谒年P(guān)節(jié)的關(guān)節(jié)變量值無法求取，即第五關(guān)節(jié)的關(guān)節(jié)變量為零時，此時工業(yè)機器人處于姿態(tài)奇點，由于第五關(guān)節(jié)變量為零，難以順利通過，因此本發(fā)明對工業(yè)機器人的運動位姿進行以下處理：(1)截取姿態(tài)奇點所處規(guī)劃路徑中的一段軌跡，為了提高運算效率，優(yōu)選的軌跡為姿態(tài)奇點所在的整個圓弧段或者直線段，然后獲取此軌跡起始點的位置值和后三個關(guān)節(jié)變量值，以及軌跡終點的位置值和后三個關(guān)節(jié)變量值；(2)然后將起始點的位置值和后三個關(guān)節(jié)變量值，以及軌跡終點的位置值和后三個關(guān)節(jié)變量值輸送到插補器中進行插補獲得姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值，具體的根據(jù)起始和終點位置值，插補速度v和插補周期t可得到要插補的總點數(shù)n，插補器的插補速度和插補周期可根據(jù)工業(yè)機器人的實際需求給定，考慮到插補的精度和效率，優(yōu)選的插補速度為10mm/s，插補周期為1ms，根據(jù)插補總點數(shù)n將軌跡起點和軌跡終點之間插補成n份，也即軌跡起點和軌跡終點之間的后三個關(guān)節(jié)變量值之間也插補成n份，得到姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值；其中，軌跡起點、軌跡終點的和插補得到的姿態(tài)奇點的位置值和后三個關(guān)節(jié)變量值分別為：x1,y1,z1,θ41,θ51,θ61,x2,y2,z2,θ42,θ52,θ62,xt,yt,zt,θ4t,θ5t,θ6t，當處于姿態(tài)奇點時，工業(yè)機器人的運動類型為直線運動或者圓弧運動，因此通過插補器插補得到的各點均在原有規(guī)劃路徑上。(3)求取姿態(tài)奇點位置對應(yīng)的前三個關(guān)節(jié)變量值(3.1)首先建立如下矩陣方程：其中，xt,yt,zt為工具坐標系原點相對于基坐標系的三個坐標分量，s1＝sin(θ1)，s2＝sin(θ2)，s23＝sin(θ2+θ3)，c1＝cos(θ1)，c2＝cos(θ2)，c23＝cos(θ2+θ3)，θ1,θ2,θ3為工業(yè)機器人第一關(guān)節(jié)至第三關(guān)節(jié)的關(guān)節(jié)變量，a1和a2分別表示第一連桿和第二連桿的長度，d2表示第一連桿和第二連桿之間的偏置，x3,y3,z3為工具坐標系原點相對于第三連桿坐標系的三個坐標分量；具體的采用如下方式建立矩陣方程：將后三個連桿變換矩陣和工具坐標系相對于第六連桿坐標系的齊次變換矩陣相乘可以得到：即其中，表示工具坐標系相對于第三連桿坐標系的旋轉(zhuǎn)矩陣，表示工具坐標系相對于第六連桿坐標系的旋轉(zhuǎn)矩陣，表示工具坐標系相對于第三連桿坐標系的位置矢量，表示工具坐標系相對于第六連桿坐標系的位置矢量；令等式兩端的第四列元素對應(yīng)相等，姿態(tài)奇點的后三個關(guān)節(jié)變量值已經(jīng)通過插補獲得，等式的右邊均為已知：將工業(yè)機器人的前三個關(guān)節(jié)進行固連(即無相對運動)，已知工具坐標系相對于基座標系的位置矢量為：工具坐標系相對于第三連桿坐標系的位置矢量為：后三個關(guān)節(jié)變量已通過插補獲得，將工業(yè)機器人的運動學方程寫成：即令上述矩陣方程兩端的第四列元素對應(yīng)相等，可建立如下方程：即(3.2)對步驟(3.1)中建立的矩陣方程兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第一關(guān)節(jié)和第三關(guān)節(jié)的關(guān)節(jié)值θ1和θ3：具體為：在步驟(3.1)中建立的矩陣方程兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣建立運動學反解方程如下：化解得：式中，可由求逆獲得。令矩陣方程(1)兩端第二行第一列的元素對應(yīng)相等得：-s1xt+c1yt＝-z3-d2；若則得到第一關(guān)節(jié)的關(guān)節(jié)變量值θ1：選定θ1其中的一個解后，令矩陣方程(1)兩端第一行第四列和第三行第四列的元素分別對應(yīng)相等得：上述兩式的平方和為：-s3y3+c3x3＝k，若則得到第三關(guān)節(jié)的關(guān)節(jié)變量值θ3：(3.3)對步驟(3.1)中建立的矩陣方程兩端同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第二關(guān)節(jié)的關(guān)節(jié)值θ2：其中，c3＝cos(θ3)，s3＝sin(θ3)。具體為：在步驟(3.1)中建立的矩陣方程兩端同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣建立運動學反解方程如下：化解得式中，可由求逆獲得。令矩陣方程(2)兩端第一行第四列和第二行第四列的元素分別對應(yīng)相等得：將上面兩個方程聯(lián)立求解得到s23和：可以看出s23和c23表達式的分母相等，且為正，于是求得第二關(guān)節(jié)的關(guān)節(jié)變量值θ2：其中，s2＝sin(θ2)；c2＝cos(θ2)；s23＝sin(θ2+θ3)；c23＝cos(θ2+θ3)。(4)以所述姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值以及前三個關(guān)節(jié)變量值作為工業(yè)機器人在姿態(tài)奇點位置對應(yīng)的運動控制參數(shù)，實現(xiàn)工業(yè)機器人的運動控制，以此使得六關(guān)節(jié)工業(yè)機器人順利通過姿態(tài)奇點。也即將由上述算法得到的前三個關(guān)節(jié)變量值和插補得到的后三個關(guān)節(jié)變量值傳送到工業(yè)機器人運動控制器中，使得工業(yè)機器人的六個關(guān)節(jié)在姿態(tài)奇點均具有確定數(shù)值，以此方式，實現(xiàn)工業(yè)機器人順利通過姿態(tài)奇點。本發(fā)明還提供了一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制系統(tǒng)，包括：數(shù)據(jù)獲取模塊，用于截取姿態(tài)奇點所處工業(yè)機器人預(yù)設(shè)的規(guī)劃路徑中的一段軌跡，獲取該軌跡起點對應(yīng)的位置值和后三個關(guān)節(jié)變量值以及該軌跡終點對應(yīng)的位置值和后三個關(guān)節(jié)變量值；插補模塊，用于根據(jù)所述起點的后三個關(guān)節(jié)變量值以及終點的后三個關(guān)節(jié)變量值以插補的方式獲得姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值；數(shù)據(jù)處理模塊，用于建立運動學反解方程并求取姿態(tài)奇點位置對應(yīng)的前三個關(guān)節(jié)變量值；運動控制模塊，用于以所述姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值以及前三個關(guān)節(jié)變量值作為工業(yè)機器人在姿態(tài)奇點位置對應(yīng)的運動控制參數(shù)，實現(xiàn)工業(yè)機器人的運動控制，以此使得六關(guān)節(jié)工業(yè)機器人順利通過姿態(tài)奇點。具體的，所述數(shù)據(jù)處理模塊包括如下子模塊：矩陣方程建立子模塊，用于建立如下方程：其中，xt,yt,zt分別為工具坐標系原點相對于基座標系的三個坐標分量，s1＝sin(θ1)，s2＝sin(θ2)，s23＝sin(θ2+θ3)，c1＝cos(θ1)，c2＝cos(θ2)，c23＝cos(θ2+θ3)，θ1,θ2,θ3為工業(yè)機器人第一關(guān)節(jié)至第三關(guān)節(jié)的關(guān)節(jié)變量，a1和a2分別表示第一連桿和第二連桿的長度，d2表示第一連桿和第二連桿之間的偏置，x3,y3,z3分別為工具坐標系原點相對于第三連桿坐標系的三個坐標分量；第一關(guān)節(jié)值計算子模塊，用于對矩陣方程建立子模塊建立的矩陣方程兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第一關(guān)節(jié)和第三關(guān)節(jié)的關(guān)節(jié)值θ1和θ3：第二關(guān)節(jié)值計算子模塊，用于對矩陣方程建立子模塊建立的矩陣方程兩端同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣，建立運動學反解方程，求得第二關(guān)節(jié)的關(guān)節(jié)值θ2：其中，c3＝cos(θ3)，s3＝sin(θ3)。具體的，所述插補模塊進行插補時的插補總點數(shù)n優(yōu)選采用下述方式確定：其中：x1,y1,z1為軌跡起點的位置值，x2,y2,z2為軌跡終點的位置值，v為插補速度，t為插補周期。所述插補速度優(yōu)選為10mm/s，插補周期優(yōu)選為1ms。具體的，所述控制系統(tǒng)還包括判斷模塊，用于根據(jù)六關(guān)節(jié)工業(yè)機器人建立連桿坐標系，并對預(yù)設(shè)的規(guī)劃路徑進行運動學反解獲得工業(yè)機器人的各關(guān)節(jié)變量值，然后判斷六關(guān)節(jié)工業(yè)機器人是否處于姿態(tài)奇點，若是，則進入步驟(1)，若否，則以反解獲得的工業(yè)機器人的各關(guān)節(jié)變量值實現(xiàn)工業(yè)機器人的運動控制。以下為具體實施例：本實施例以華中數(shù)控6012工業(yè)機器人為例，其構(gòu)型如圖2(a)和(b)所示，華中數(shù)控6012工業(yè)機器人屬于關(guān)節(jié)式機器人，其由6個連桿(如圖2所示的第一連桿1、第二連桿2、第三連桿3、第四連桿4、第五連桿5、第六連桿6)和6個轉(zhuǎn)動關(guān)節(jié)(如圖3所示的點a、b、c、d、e、f，)組成，基座固定不動稱為連桿0，第一關(guān)節(jié)連接第一連桿和第二連桿，第二關(guān)節(jié)連接第二連桿和第三連桿，以此類推，工具與連桿6固接。其通過姿態(tài)奇點的過程主要包括以下步驟：步驟1)：根據(jù)華中數(shù)控6012工業(yè)機器人的結(jié)構(gòu)特點和d-h法，設(shè)定的連桿坐標系{0}，{1}，{2}，{3}，{4}，{5}，{6}和{t}如圖3所示：基坐標系{0}與連桿1的坐標系{1}重合，坐標系{1}的z軸z1為第一關(guān)節(jié)的軸線方向，指向向上，坐標系{1}的x軸x1為第一關(guān)節(jié)和2的軸線公垂線方向，指向由第一關(guān)節(jié)到第二關(guān)節(jié)，那么坐標系{1}的原點o1即為x1和z1的交點；至此坐標系{1}建立完成，以此類推建立坐標{2}和{3}；連桿坐標系{4}，{5}和{6}的原點o4,5,6為第四關(guān)節(jié),5和6的軸線交點，坐標系{4}的z軸z4為第四關(guān)節(jié)的軸線方向，坐標系{4}的x軸x4的方向與x3一致，坐標系{5}的z軸z5為第五關(guān)節(jié)的軸線方向，坐標系{6}的z軸z6的方向與z4一致，坐標系{5}的x軸x5和坐標系{6}的x軸x6的方向與x4一致；工具坐標系{t}的原點ot為工具末端中心，工具坐標系{t}的x軸xt的方向與x6一致，工具坐標系{t}的z軸zt的方向與z6一致。根據(jù)建立的各連桿坐標系將相應(yīng)的連桿參數(shù)列出，如表1所示。表1華中數(shù)控6012工業(yè)機器人的連桿參數(shù)利用所列的連桿參數(shù)計算出各個連桿變換矩陣，推導(dǎo)出第三連桿坐標系相對于基坐標系的等價其次變換矩陣步驟2)：工業(yè)機器人處于姿態(tài)奇點時的位置為：(1323.1,226.8,703.8)，此姿態(tài)奇點所在的路徑為直線，截取其所處規(guī)劃路徑中的整個直線段，此直線段起點的位置值為(999.2,450.9,532.3)，后三個關(guān)節(jié)變量的關(guān)節(jié)值為(-108.5°,-30.6°,223.8°),終點的位置值為(1512.3,105.6,796.4)，后三個關(guān)節(jié)變量的關(guān)節(jié)值為(36.3°,-15.1°,77.1°)。步驟3)：將起始點、終點的位置和后三個關(guān)節(jié)變量值均輸送到插補器中，插補速度為10mm/s，插補周期為1ms，插補的總點數(shù)n：將軌跡起點和軌跡終點的后三個關(guān)節(jié)變量值之間也插補成68份，得到姿態(tài)奇點位置對應(yīng)的后三個關(guān)節(jié)變量值：(-11.9°,-20.2°,119.3°)。步驟4)：已知工具坐標系相對于第六連桿坐標系的位置矢量為：將通過插補得到的后三個關(guān)節(jié)變量值θ4，θ5，θ6代入式(2)得：故工具坐標系相對于第三連桿坐標系的位置矢量為：將工業(yè)機器人的前三個關(guān)節(jié)進行固連，在姿態(tài)奇點，已知工具坐標系相對于基座標系的位置矢量為：后三個關(guān)節(jié)變量θ4，θ5，θ6已通過插補獲得，將工業(yè)機器人的運動學方程寫成：令矩陣方程(4)兩端的第四列元素對應(yīng)相等，可建立如下方程：矩陣方程(5)兩端同時左乘第一連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣建立運動學反解方程如下：令矩陣方程(6)兩端第二行第一列的元素對應(yīng)相等得：-1332.1s1+226.8c1＝-346.8(7)；解得第一關(guān)節(jié)的關(guān)節(jié)變量值：θ1＝174.8°或θ1＝24.5°。選定θ1其中的一個解后，令矩陣方程(6)兩端第一行第四列和第三行第四列的元素分別對應(yīng)相等得：上述兩式的平方和為：-(-1254)s3+(-42.1)c3＝k(9)；其中，解得當θ1＝174.8°時第三關(guān)節(jié)的關(guān)節(jié)變量值：θ3＝-343.5°或θ3＝-192.6°；當θ1＝24.5°時第三關(guān)節(jié)的關(guān)節(jié)變量值：θ3＝-9.8°或θ3＝-166.4°。矩陣方程(5)兩邊同時左乘第三連桿坐標系相對于基座標系的齊次變換矩陣的逆矩陣建立運動學反解方程如下：令矩陣方程(10)兩端第一行第四列和第二行第四列的元素分別對應(yīng)相等得：將上面兩個方程聯(lián)立求解得到s23和c23，解得：θ23＝-141°，θ23＝-86.1°，θ23＝88.8°，θ23＝158.5°。故第二關(guān)節(jié)的關(guān)節(jié)變量值為：θ2＝202.5°，θ2＝106.5°，θ2＝98.6°，θ2＝324,9°。前三個關(guān)節(jié)變量反解一共有四組解，如表2所示：表2華中數(shù)控6012工業(yè)機器人前三個關(guān)節(jié)變量的反解值θ1θ2θ3第一組174.8°202.5°-343.5°第二組174.8°106.5°-192.6°第三組24.5°98.6°-9.8°第四組24.5°324.9°-166.4°只有第三組解符合表1給定的關(guān)節(jié)的運動范圍，即前三個關(guān)節(jié)變量分別為：θ1＝24.5°，θ2＝98.6°，θ3＝-9.8°。步驟5)：將由上述算法得到的前三個關(guān)節(jié)變量值和插補得到的后三個關(guān)節(jié)變量值傳送到工業(yè)機器人運動控制器中，使得工業(yè)機器人的六個關(guān)節(jié)在姿態(tài)奇點均具有確定數(shù)值；以此方式，實現(xiàn)工業(yè)機器人順利通過姿態(tài)奇點。本實例根據(jù)上述位置及運動學反解方程，求得機器人位于姿態(tài)奇點時的前三個關(guān)節(jié)變量，并通過插補得到后三個關(guān)節(jié)的關(guān)節(jié)變量，經(jīng)過運動學正解驗算得到的機器人末端位置與實際末端位置一致。本發(fā)明提供了一種六關(guān)節(jié)工業(yè)機器人通過姿態(tài)奇點的控制方法，在不改變原有規(guī)劃路徑的前提下使得機器人順利通過姿態(tài)奇點，大大降低了通過外部設(shè)備渡過姿態(tài)奇點的生產(chǎn)成本，避免了路徑規(guī)劃方式到解析解的復(fù)雜迭代運算，可直接得到機器人處于姿態(tài)奇點時關(guān)節(jié)位置的數(shù)值解，簡單易行且運算效率高，此外本發(fā)明實現(xiàn)了機器人運動控制指令范圍的擴展，可直接將程序植入到機器人運動控制器中，給實際生產(chǎn)帶來很大便利。本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。當前第1頁12