[發明專利]一種八軸多功能機械臂的逆解工程及其碰撞檢測算法

有效

• 申請號: 201810378543.5
• 申請日: 2018-04-25
• 公開（公告）號: CN108481324B
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 燕勝;宋云濤;程勇;史善爽;熊宇;楊松竹;潘洪濤;郝志偉;李陽
• 申請（專利權）人: 中國科學院合肥物質科學研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J19/00
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 楊學明

1.一種八軸多功能機械臂的逆解工程及其碰撞檢測算法，其特征在于：包括方位角逆解算法、位置精度逆解算法、碰撞檢測算法，其中：

(1)、方位角逆解算法其過程如下：

(1.1)、目標姿態位置矩陣TEND由姿態矩陣REND和位置矢量PEND組成，其中R_END＝[x_END y_END z_END]，則第七關節空間位置坐標P₇＝P_END-(d₇+d₈)*z_END；

其中，px、py、pz指該點在空間中的三維坐標；d7、d8是建立D-H坐標系過程中產生的對應的參數，根據機器人關節坐標系的建立得出；d₁-d₈參數表如表1所示：

表1 D-H坐標系過程中d₁-d₈參數

(1.2)、將第七關節空間位置坐標P7導入位置精度逆解算法，可以求出前段關節逆解Pos_1To5＝[d₁ θ₂ θ₃ θ₄ θ₅]，通過碰撞檢測算法驗證前段關節逆解的可行性，得到滿足要求的前段關節逆解Pos_1To5；其中d₁建立D-H坐標系過程中產生的對應的參數；θ₂-θ₅表示的是八軸多功能機械臂中第二-第五關節的轉動變量，這些編號對應的是關節坐標系的下標，轉動方向為繞著各個坐標系的Z軸轉動；

(1.3)、通過關節逆解Pos_1To5＝[Pos_1To5 0]得到第六關節的姿態位置矩陣根據D-H法建立的關節坐標系，滿足P₆＝P₇，θ₇＝arccos(z₆*z_END/(|z₆|*|z_END|))，其中z₆為第六關節Z軸方向向量，R₆指的是第六關節的坐標轉換后的方向矩陣、P₆指的則是轉換后的位置向量，P₇指的是經過前7個關節坐標系轉換之后的坐標中心點的位置向量，Zend指的是機器人末端點目標姿態的Z軸向量，θ₇是八軸多功能機械臂中第七關節的轉動變量；

(1.4)、由第六關節Z軸方向向量z₆和目標姿態Z軸方向向量z_END得到法向量V_public1，方向符合右手準則；

(1.5)、通過第六關節Y軸方向向量y₆與法向量V_public1的夾角θ_jud＝arccos(z₆*z_END/(|z₆|*|z_END|))來判定θ₆的值：

θ₆是八軸多功能機械臂中第六關節的轉動變量；

(1.6)、通過前段關節逆解Pos_1To7＝[d₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]得到第七關節的姿態位置矩陣其中第七關節X軸方向向量x₇和目標姿態X軸方向向量x_END，得到法向量V_public2，方向符合右手準則，通過V_public2與z_END的方向關系判定θ₈的值：

其中x₇為X軸方向向量，R₇為第七關節的坐標轉換后的方向矩陣，P₇為經過前7個關節坐標系轉換之后的坐標中心點的位置向量，θ₈是八軸多功能機械臂中第八關節的轉動變量；

(2)、位置精度逆解算法其過程如下：

(2.1)、根據前三個關節的位置相互約束關系進行逆解算法的簡化：

由d₁劃分三種情況：d₁∈[0，2705)，d₁∈[2705，4455)，d₁＝4455，分別對應三個子算法：全臂約束逆解算法、半臂約束逆解算法、平動約束逆解算法；

(2.2)、通過方位角逆解算法得到第七關節空間位置坐標P₇，按照d₁遞增的順序進行逆解，依次運行全臂約束逆解算法、半臂約束逆解算法、平動約束逆解算法，在上一個算法無法求得解的情況下運行下一個算法，如果得到解，則運行方位角逆解算法，求得最終的解；

(2.3)、運行全臂約束逆解算法，輸入關節變量Pos_1To6＝[d₁ π/2 -π/2 θ₄ θ₅ 0]得到第六關節的姿態位置矩陣其中且P₆＝P₇，通過方程聯立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始條件范圍內，判定通過后導入碰撞檢測算法，驗證機械臂姿態的空間可行性；

(2.4)、運行半臂約束逆解算法，輸入關節變量Pos_1To5＝[d₁ π/2 θ₃ θ₄ θ₅ 0]得到第六關節的姿態位置矩陣其中且P₆＝P₇，由于機械臂存在冗余自由度，為了降低逆解求解過程運算量，間隔5°遍歷θ₃∈[-π，0]，再通過方程聯立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始條件范圍內，判定通過后導入碰撞檢測算法，驗證機械臂姿態的空間可行性；

(2.5)、運行平動約束逆解算法，輸入關節變量Pos_1To5＝[4455 θ₂ θ₃ θ₄ θ₅ 0]得到第六關節的姿態位置矩陣其中且P₆＝P₇，由于機械臂存在冗余自由度，為了降低逆解求解過程運算量，間隔5°遍歷θ₅∈[-π，0]，由p_z(θ₄，θ₅)得到θ₄，再通過方程聯立求解得到d₁，θ₂，θ₃，然后判定d₁是否在算法初始條件范圍內，判定通過后導入碰撞檢測算法，驗證機械臂姿態的空間可行性；

(3)、碰撞檢測算法其過程如下：

(3.1)、簡化機械臂環狀工作空間，空間切面折線以極坐標方程曲線代替，極點位于空間切面折線的中心位置，該位置由待檢測點決定，環狀臂極坐標方程：

l(θ)＝(0.000277500372481*θ¹⁰-0.008523502454628*θ⁹+0.106922940971428*θ⁸-0.696566524761767*θ⁷+2.447743902396733*θ⁶-4.160347429809979*θ⁵+1.503776922304815*θ⁴+3.856518561349923*θ³-3.237876202808929*θ²+1.010407153490720*θ¹+1.623553179189534)*(1.0e+03)

安全域極坐標方程：l_safe(θ)＝0.62*l(θ)；

(3.2)、根據關節變量求得各關節的空間位置坐標：

在P_i與P_i+1之間等間距取100個待檢測點，

(3.3)、根據i遍歷多個指定關節，根據k遍歷臨近指定關節之間的待監測點，待檢測點與環狀空間中心軸組成平面T₁，相對環狀空間中心軸與機械臂基座標原點組成的豎直平面T₂，確定T₁與T₂的角度θ_T1T2，極點P_polar相對環狀空間中心軸坐標系的位置坐標(center_x、center_y、center_z)：

center_x＝5738.4*cos(theta_center_axis)；

center_y＝5738.4*sin(theta_center_axis)；

center_z＝0

待檢測點相對極點P_polar的空間向量得到極坐標半徑和與xoy平面的夾角即極角導入安全域極坐標方程：

在的情況下，待檢測點通過碰撞檢測。