买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：中国地质大学(武汉)

摘要：本发明提供了一种绳驱蛇形机械臂逆运动学宏微双级混合解析求解方法，S1：对蛇形机械臂进行自由度重构，得到“宏观”机械臂和“微观”机械臂；S2：引入臂形角变量，基于冗余机械臂的自运动特性，得到“宏观”机械臂各个角度的值；S3：根据求解得到的“宏观”机械臂各个角度的值，得到“微观”机械臂各个角度的值。本发明的有益效果是：求解过程简单，降低了蛇形机械臂的误差，提高了蛇形机械臂到达目标的精确度。

主权项：1.一种绳驱蛇形机械臂逆运动学宏微双级混合解析求解方法，其特征在于：包括：S1：对蛇形机械臂进行自由度重构，得到“宏观”机械臂和“微观”机械臂；自由度重构时，选取七个关节作为关键关节，组成“宏观”机械臂，两相邻关键关节间的其他关节组成“微观”机械臂，两相邻关键关节间的距离视为等效连杆，连杆长度L满足： 其中l为蛇形机械臂实际单连杆长度，n为两关键关节间实际连杆个数；S2：引入臂形角变量，基于冗余机械臂的自运动特性，得到“宏观”机械臂各个角度的值；得到“宏观”机械臂各个角度的值的过程如下：1给定蛇形机械臂末端位姿Tf，机械臂的第二、四、六关节分别为S、E、W，SE、SW、EW为向量模，θi为第i个关节绕其自身的z轴的旋转角度，由余弦定理可得： 式中∠SEW为第二、四、六关节所成角，因此第四关节绕其自身的z轴的旋转角度θ4＝±π-arccoscos∠SEW；2对前三自由度，视为一个球铰，记第一关节为G，取第二关节S点为坐标原点，方向为x轴正方向，可得在第一关节、第二关节、第三关节绕其自身z轴的旋转角度均为零时第六关节W的位置为： 在第一关节、第二关节、第三关节绕其自身z轴的旋转角度不全为零时，根据臂形角角度φ，可以从式3.2-3.33.10以及绕原点S转动的角度得到第二关节S处的坐标系∑s，即∑s＝∑Sφ＝0×Rotxs,φ，∑Sφ＝0为臂形角为零时的坐标系，Rotxs,φ表示绕s点的x轴转动角度φ，x的下标s表示坐标系原点，绕y或z轴转动某角度同理，进而根据末端位姿Tf得到以S点为中心组成的球铰的变换矩阵Rs： 其中，Rotz,θ1表示第一关节绕其自身z轴旋转θ1角度，Roty,θ2表示第二关节绕其自身y轴旋转θ2角度，Rotz,θ3表示第三关节绕其自身z轴旋转θ3角度；由此可以得到三个角度： 其中Rsi,j表示矩阵Rs的第i行第j列元素，i＝1,2,3，j＝1,2,3，θ1表示第一关节绕其自身z轴的旋转角度，θ2表示第二关节绕其自身z轴的旋转角度，θ3表示第三关节绕其自身z轴的旋转角度；3同理可以求解出剩余三个角度： 其中arctan·表示反曲正切函数，Rw为以第六关节W点为中心组成的球铰的变换矩阵，Rwi,j表示矩阵Rw的第i行第j列元素，θ5表示第五关节绕其自身z轴的旋转角度，θ6表示第六关节绕其自身z轴的旋转角度，θ7表示第七关节绕其自身z轴的旋转角度；S3：根据求解得到的“宏观”机械臂各个角度的值，得到“微观”机械臂各个角度的值；得到“微观”机械臂各个角度的值的过程如下：由于在“宏观”机械臂的求解中已经得到了部分关节的角度，因而可以在基座坐标系或末端坐标系的基础上得到各关键关节的位姿，所有“微观”机械臂的角度求解方法相同，以第二个“微观”机械臂的角度求解方法为例，其末端位姿是已知的，记其末端变换矩阵为： 其中n＝[nx,ny,nz]T为第二个“微观”机械臂末端的法线矢量，o＝[ox,oy,oz]T为第二个“微观”机械臂的方向矢量，a＝[ax,ay,az]T为第二个“微观”机械臂的接近矢量，p＝[px,py,pz]T为第二个“微观”机械臂的位置矢量，“微观”机械臂的关节是两两垂直的，基于各连杆变换矩阵和末端位姿求取关节角度；机械臂的正运动学可以表示为：为第h到第m坐标系的变换矩阵，h＝0,1,2,3,4,5，m＝1,2,3,4,5,6，结合机械臂的正运动学公式，求解第二个“微观”机械臂的角度过程如下：1利用欧拉变换来表示第二个“微观”机械臂的末端变换矩阵α,β,表示第二个“微观”机械臂末端的姿态， 令矩阵元素一一对应，可以得到一共12组隐式方程，联立这些方程可以求解部分角度；2求解其余角度时，利用逆变换中某一个左乘正运动学式子两边，令矩阵方程两端元素对应相等，求得各角度；3通过上述步骤1-2，得到所有第二个“微观”机械臂的各个角度，同理，可得到所有“微观”机械臂的各个角度。

全文数据：

权利要求：

百度查询： 中国地质大学(武汉) 一种绳驱蛇形机械臂逆运动学宏微双级混合解析求解方法