一種七軸工業(yè)機械臂的逆動學求解方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設及設及一種走軸工業(yè)機械臂的逆動學求解方法，尤其設及走軸工業(yè)機械 臂的建模方法。
【背景技術(shù)】
[0002] 在現(xiàn)有技術(shù)中走軸工業(yè)機械臂的逆動學求解方法有如下兩種：
[0003] 現(xiàn)有技術(shù)的方法如二次規(guī)劃法，幾何分析法W及位姿分解方法等，
[0004] 現(xiàn)有技術(shù)的方法（一）：二次規(guī)劃法在祖迪，吳鎮(zhèn)巧的《一種冗余機器人逆運動學 求解得有效方法》中描述的較為詳細。該方法重點在首先利用雅可比矩陣W及梯度投影法 求出一組關(guān)節(jié)角，在二次計算時固定其中一個角度來求解剩余關(guān)節(jié)角；
[0005] 對于方法（二）所述；幾何分析法在崔澤，韓增軍的《基于自運動的仿人走自由度 機械臂逆解算法》中描述的較為詳細。該方法重點在自運動構(gòu)型的基礎(chǔ)上，W0坐標系為其 他坐標系的基系，根據(jù)同一向量在不同坐標系之間的投影特性與關(guān)節(jié)角之間存在著的對應 關(guān)系，實現(xiàn)對走自由度機械臂逆運動學的求解；
[0006] 對于方法（=)所述；位姿分解方法在潘博，付宜利的《面向冗余機器人實時控制 的逆運動學求解有效方法》所述比較詳細。該方法針對具有球形手腕的7自由度機械臂結(jié) 構(gòu)特點，采用位姿分離的方法，將7自由度問題降為4自由度位置冗余問題W及3自由度姿 態(tài)問題，在此基礎(chǔ)之上再利用梯度投影法進行逆運動學的求解。
[0007] 在現(xiàn)有的=種方案中，上述現(xiàn)有技術(shù)中的的缺點為：
[0008] 方法（一）所述方法中梯度投影法中，自運動放大系數(shù)k選取為常數(shù)，第一需要經(jīng) 過反復的計算機仿真通常才能確定一個k值，同時固定k值，缺少機器人運動優(yōu)化能力，常 導致機器人關(guān)節(jié)角震蕩。同時二次計算雖然精度高但是運算量大，實時性差；
[0009] 方法（二）雖然利用自運動軌跡和幾何方法可W求解逆運動學，但是該方法只適 用于兩桿之間存在軸線平行于連桿的轉(zhuǎn)動副的情況，即存在橫滾轉(zhuǎn)動副的走自由度機器人 才能夠如此簡化，不具有很強的通用性；
[0010] 方法（=)利用位姿分解能簡化逆運動學的求解，同方法二一樣也限制于機械臂 的構(gòu)型，不具有很好的通用性。

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

[0011] 本發(fā)明的實施例提供了一種走軸工業(yè)機械臂的逆動學求解方法，本發(fā)明提供了如 下方案，包括：
[0012] 步驟1、將虛擬機械臂末端執(zhí)行器任務設定的起始點和終止點中間對應的位置離 散化，獲取離散點；
[0013] 步驟2、建立第一關(guān)節(jié)角和第走姿態(tài)的坐標原點的平面方程，根據(jù)每兩個離散點得 到第一關(guān)節(jié)角的變化趨勢，根據(jù)所述變化趨勢得到離散點集合中每一點對應的第一關(guān)節(jié)角 的值；
[0014] 其中，第一平面坐標系為基座底部坐標點、第零姿態(tài)的坐標原點W及第一或第二 姿態(tài)的坐標原點構(gòu)成的平面；
[0015] 步驟3、采用每個離散點的末端位姿矩陣作為初始末端位姿矩陣T，根據(jù)變換矩陣 ;:'廠=;i7;7';節(jié)7巧;7' = 獲取第五或第六姿態(tài)的坐標原點，再確定Sr位姿矩陣，根據(jù) ^位姿矩陣求出第=姿態(tài)的坐標原點；根據(jù)第五或第六姿態(tài)的坐標原點和第=姿態(tài)的坐 標原點的距離方程式，求出第二關(guān)節(jié)角；
[0016] 步驟4、建立第四關(guān)節(jié)角、第四關(guān)節(jié)角的第一特征角W及第四關(guān)節(jié)角的第二特征角 之間的條件關(guān)系，根據(jù)所述條件關(guān)系求解第四關(guān)節(jié)角；其中，
[0017] 所述第四關(guān)節(jié)角的第一特征角為W第S姿態(tài)的坐標原點為頂點，與第四姿態(tài)的坐 標原點及第五或第六姿態(tài)坐標原點形成的夾角；
[0018] 所述第四關(guān)節(jié)角的第二特征角W第=姿態(tài)的坐標原點為頂點，與第一或第二姿態(tài) 的坐標原點及第五或第六姿態(tài)坐標原點形成的夾角；
[001引步驟5、由正運動學方程片7；巧;C7；，rr=(片)-吁的左右兩邊的第四列對應相 等得到的方程得到關(guān)于第一關(guān)節(jié)角、第二關(guān)節(jié)角、第四關(guān)節(jié)角W及第=關(guān)節(jié)角的兩個等式， 從而求出第=關(guān)節(jié)角；
[0020] 步驟6、確定巧位姿矩陣，根據(jù)辟位姿矩陣得到第四姿態(tài)的坐標原點坐標，再求出 第四姿態(tài)的坐標原點和第走姿態(tài)的坐標原點之間的空間距離，再由第四姿態(tài)的坐標原點、 第五或第六姿態(tài)的坐標原點和第走姿態(tài)的坐標原點構(gòu)成的=角形中，求出第六關(guān)節(jié)角的特 征角，根據(jù)第六關(guān)節(jié)角的特征角求解第六關(guān)節(jié)角；其中，第六關(guān)節(jié)角的特征角為W第五或第 六姿態(tài)的坐標原點為頂點，與第四姿態(tài)的坐標原點、第走姿態(tài)的坐標原點形成的夾角；
[0021] 步驟7、根據(jù)第六關(guān)節(jié)角的求解結(jié)果，結(jié)合末端坐標的關(guān)系，分情況求解第五關(guān)節(jié) 角和第走關(guān)節(jié)角。
[0022] 根據(jù)本發(fā)明的上述方法，在規(guī)劃第一關(guān)節(jié)角之前，包括：
[0023]W虛擬機械臂末端執(zhí)行器的W基座的底部點0為基坐標點，再另外依次確定走個 位姿狀態(tài)對應的空間坐標系的坐標原點，其中第一姿態(tài)和第二姿態(tài)公用同一個坐標原點為 第一或第二姿態(tài)的坐標原點，第五姿態(tài)和第六姿態(tài)公用同一個坐標原點為第五或第六姿態(tài) 的坐標原點；
[0024] 根據(jù)本發(fā)明的上述方法，所述建立第四關(guān)節(jié)角、第四關(guān)節(jié)角的第一特征角W及第 四關(guān)節(jié)角的第二特征角之間的條件關(guān)系，包括；第四關(guān)節(jié)角的第一特征角與第四關(guān)節(jié)軸的 變換關(guān)系；當?shù)谒年P(guān)節(jié)角的第一特征角變小時，該變化量是由于第四關(guān)節(jié)軸向上俯仰造成， 同理當?shù)谒年P(guān)節(jié)角的第一特征角變大時，該變化量是由于第四關(guān)節(jié)軸向下俯仰造成，根據(jù) 所述變換關(guān)系建立第四關(guān)節(jié)角、第四關(guān)節(jié)角的第一特征角W及第四關(guān)節(jié)角的第二特征角之 間的條件關(guān)系。
[00巧]根據(jù)本發(fā)明的上述方法，包括；在建立第一關(guān)節(jié)角和第走姿態(tài)的坐標原點的平面 方程之前，
[0026] 旋轉(zhuǎn)第一關(guān)節(jié)角，將第走姿態(tài)的坐標原點旋轉(zhuǎn)至第一平面坐標系內(nèi)，使第一關(guān)節(jié) 角和第走姿態(tài)的坐標原點處于同一平面。
[0027] 根據(jù)本發(fā)明的上述方法，包括：
[0028] 所述第四關(guān)節(jié)角的第一特征角通過W第S姿態(tài)的坐標原點為頂點，與第四姿態(tài)的 坐標原點及第五或第六姿態(tài)坐標原點形成的直角=角形中的=角函數(shù)求解；
[0029] 所述第四關(guān)節(jié)角的第二特征角通過第=姿態(tài)的坐標原點為頂點，與第一或第 二姿態(tài)的坐標原點及第五或第六姿態(tài)坐標原點形成的=角形的=角函數(shù)求解。
[0030] 根據(jù)本發(fā)明的上述方法，根據(jù)第六關(guān)節(jié)角的特征角求解第六關(guān)節(jié)角，包括；第六關(guān) 節(jié)角對應為兩個解，具體為第六關(guān)節(jié)角的特征角的補角的正負值。
[0031] 根據(jù)本發(fā)明的上述方法，根據(jù)第六關(guān)節(jié)角的求解結(jié)果，結(jié)合末端坐標的關(guān)系，分情 況求解第五關(guān)節(jié)角和第走關(guān)節(jié)角，包括：
[0032] 當?shù)诹P(guān)節(jié)角不為為零時，由末端坐標的關(guān)系求出第五姿態(tài)角，再由末端姿態(tài)的 關(guān)系求出第走姿態(tài)角；
[0033] 當?shù)诹P(guān)節(jié)角為零時，機械臂處于奇異位置，不能唯一確定第五關(guān)節(jié)角和第走關(guān) 節(jié)角，令第五關(guān)節(jié)角等于零，則可求出第走關(guān)節(jié)角，得到第五關(guān)節(jié)角和第走關(guān)節(jié)角的一組特 解。
[0034] 由上述本發(fā)明的實施例提供的技術(shù)方案可W看出，本發(fā)明實施例根據(jù)每兩個離散 點得到第一關(guān)節(jié)角的變化趨勢得到離散點集合中每一點對應的第一關(guān)節(jié)角的值；采用每個 離散點的末端位姿矩陣，獲取第五或第六姿態(tài)的坐標原點，再求出第=姿態(tài)的坐標原點，求 出第二關(guān)節(jié)角；根據(jù)第四關(guān)節(jié)角、第四關(guān)節(jié)角的第一特征角W及第四關(guān)節(jié)角的第二特征角 之間的條件關(guān)系求解第四關(guān)節(jié)角；由關(guān)于第一關(guān)節(jié)角、第二關(guān)節(jié)角、第四關(guān)節(jié)角的方程W及 第=關(guān)節(jié)角的兩個等式，求出第=關(guān)節(jié)角；獲取第四姿態(tài)的坐標原點坐標，求出第六關(guān)節(jié)角 的特征角，根據(jù)第六關(guān)節(jié)角的特征角求解