[發明專利]一類六自由度機械臂運動學逆解的快速簡便求法

有效

• 申請號: 201410150282.3
• 申請日: 2014-04-11
• 公開（公告）號: CN103942427B
• 公開（公告）日: 2017-02-22
• 發明（設計）人: 朱齊丹;王欣璐;蔡成濤;張智;夏桂華;林圣琳;毛爽;姜軍來;原新;劉志林
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G06F19/00
• 分類號: G06F19/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種一類六自由度機械臂運動學逆解角度估計方法，其特征在于：

（1）根據機械臂末端的位置(X,Y,Z)和姿態矩陣來確定后三個關節軸相交點O的坐標(x,y,z)，即機械臂腕部點的位置：

通過測量得知后三個關節軸相交點O與機械臂末端的距離為L，而末端的姿態矩陣是已知的，

RB0=opqrstuvw]]>

姿態矩陣中第一列元素o，r，u依次代表機械臂末端工具坐標系的X軸與機械臂基坐標系的X軸，Y軸，Z軸夾角的余弦，第二列元素p，s，v依次代表機械臂末端工具坐標系的Y軸與機械臂基坐標系X軸，Y軸，Z軸夾角的余弦，第三列元素q，t，w依次代表機械臂末端工具坐標系的Y軸與機械臂基坐標系X軸，Y軸，Z軸夾角的余弦，其中，

x＝X+L·q

y＝Y+L·t，

z＝Z+L·w

確定機械臂腕部點O的坐標(x,y,z)；

（2）利用幾何法求解出前三個關節軸轉過的角度θ_i，i＝1,2,3：

在六個關節軸上建立坐標系，依次為1～6號坐標系，通過類六自由度機械臂的幾何構造解出前三個關節軸轉過的角度θ_i(i＝1,2,3)：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂＝x/cosθ₁-d₁，

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂＝z-d₂，

tanθ₁＝y/x，

其中l₁代表2號坐標系原點與3號坐標系原點之間的距離，l₂代表3號坐標系原點與4號坐標系原點之間的距離，l₃代表4號坐標系與機械臂腕部點O之間的距離，d₁代表1號坐標系z軸與基坐標系z軸之間的距離，d₂代表1號坐標系原點與基坐標系原點在基坐標系z軸方向上的距離；根據已經求得的前三個關節軸轉過的角度和機械臂末端的姿態矩陣，求出機械臂的X-Y-Z歐拉角變換矩陣；

（3）通過歐拉角變換矩陣求得后三個關節軸轉過的角度θ_i(i＝4,5,6)：

由旋轉矩陣的變換關系得：

RE0=R40|θ4=0·R64|θ4=θ5=θ6=0·RxyzE6,]]>

其中，為6號坐標系相對于基坐標系的旋轉矩陣，由機械臂前三個關節角度所決定的，為4號坐標系到6號坐標系的變換，后三個關節轉角都為0，為X-Y-Z歐拉角的變換矩陣：

RxyzE6=R64|θ4=θ5=θ6=0-1·R|θ4=0-140·RE0]]>

其中為一個恒定的矩陣，由已經求得的前三個關節軸轉過的角度θ_i(i＝1,2,3)和R64|θ4=θ5=θ6=0-1]]>可確定R64|θ4=θ5=θ6=0-1·R|θ4=0-140,]]>

根據末端的姿態矩陣和R64|θ4=θ5=θ6=0-1·R|θ4=0-140]]>計算出

X-Y-Z歐拉角變換矩陣為：

RXYZ(θ4,θ5,θ6)=cθ5cθ6-cθ5sθ6sθ5sθ4sθ5cθ6+cθ4sθ6-sθ4sθ5sθ6+cθ4cθ6-sθ4cθ5-cθ4sθ5cθ6+sθ4sθ6cθ4sθ5sθ6+sθ4cθ6cθ4cθ5,]]>

如RXYZ(θ4,θ5,θ6)=r11r12r13r21r22r23r31r32r33,]]>

若cosθ₅≠0，

θ5=tan-1(r13/r112+r122),]]>

θ₄＝tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅))，

θ₆＝tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅))，

最后得到機械臂后三個關節軸轉過的角度θ_i(i＝4,5,6)。