用于具有三角形远心点机构的串联机械臂逆运动学数值解法制造技术

用于具有三角形远心点机构的串联机械臂逆运动学数值解法，它涉及一种串联机械臂逆运动学数值解法，具体涉及一种用于具有三角形远心点机构的串联机械臂逆运动学数值解法。本发明专利技术为了解决现有微创手术机械臂控制系统精度较低、实时性较差的问题。本发明专利技术的具体步骤为：假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中间计算值，命名为q13。本发明专利技术属于微创医疗器械领域。

【技术实现步骤摘要】

本专利技术涉及一种串联机械臂逆运动学数值解法，具体涉及一种用于具有三角形远心点机构的串联机械臂逆运动学数值解法，属于微创医疗器械领域。
技术介绍
微创手术与传统的开口手术相比具有手术切口小、术后疤痕小、恢复时间快等优点，因此微创手术被广泛地应用于临床手术。采用主从控制形式的微创手术机器人提高了微创手术的精度和应用范围，故而有广阔的市场前景。微创手术机器人的机械臂通常采用远心定点运动机构来实现手术器械在人体内的定位。三角形远心机构依靠关节轴线相交于定点的方式来实现机构的远心运动。前两个关节是转动关节，轴线相交于远心点，最后一个关节是移动关节，轴线通过远心点。在远心机构后面串联腕部三个关节，实现手术器械的运动。在远心点机构前面可串联数个被动关节，用于手术前调整远心点的位置，手术中锁死。机械臂逆运动学求解是根据机械臂在空间内的末端位置姿态矩阵，求解各个关节的位移量。逆运动学求解是实现机械臂定位控制和轨迹跟踪控制的基础，且逆运动学算法直接影响机械臂的运动控制器的性能。逆运动学方程是非线性超越方程，其求解过程非常复杂，而且可能存在多解、无解或者奇异等问题。对于六自由度串联机械臂，仅当其结构满足准则——机械臂腕部三个相邻的关节轴相交于一点，或者机械臂腕部三个相邻的关节的轴线相互平行，此时三个相邻关节的位置和姿态是解耦的，可以求出机械臂逆运动学的封闭解。但一般情况下，考虑到使用性能和设计制造和装配过程中的困难和误差，机械臂都不满足r>这一准则。故而需要釆用数值解法完成机械臂的逆运动学求解。一般采用的数值解法的基本原理是根据机械臂当前的关节角度求解正运动学的计算结果，通过比较其与目标位置姿态值的差值，对关节角度进行修正，反复试探并控制定位误差计算关节角度最优值。这样的方法计算量大，不利于控制系统的实时性。
技术实现思路
本专利技术为解决现有微创手术机械臂控制系统精度较低、实时性较差的问题，进而提出具有三角形远心点机构的串联机械臂逆运动学数值解法。本专利技术为解决上述问题采取的技术方案是：本专利技术的具体步骤如下：步骤一、假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；步骤二、根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中间计算值，命名为q13；步骤三、根据假设A，由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小，依次求出第一关节中间计算值q11和第二关节中间计算值q12；步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16；步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等，这是假设B；步骤六、在第二关节坐标系O2下，根据假设B，用第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计算值q23；步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22；步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26；步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，重新进入循环；步骤十、到达事先设定好的循环次数，推出循环，输出新的一组六个关节的位移。本专利技术的有益效果是：1、本专利技术求出的六个关节的位移量经过几次循环快速逼近真值，经过机械臂正运动学计算得到的位置姿态矩阵的位置分量与输入的末端点位置姿态矩阵的位置分量间的误差呈指数速度减小；2、位置姿态分离求解，先用位置信息，再用姿态信息，每一次循环求出的一组解，可保证再正解出来的姿态矩阵没有误差，只有位置矢量有误差。即在忽略其他误差的情况下，运用到机械臂上，末端位置有误差，姿态无误差；3、求解过程在关节空间内完成，计算量小。附图说明图1是本专利技术的算法流程图，图2是具有三角形远心机构的六自由度串联机械臂的坐标关系示意图，图3是由q13、q14、q15、q16求q23的空间关系图，图2和图3中A1是第一关节轴线，A2是第二关节轴线，A3是第三关节轴线，A4是第四关节轴线，A5是第五关节轴线，A6是第六关节轴线，q1是第一关节位移量，q2是第二关节位移量，q3是第三关节位移量，q4是第四关节位移量，q5是第五关节位移量，q6是第六关节位移量，q01第一关节真实值，q02是第二关节真实值，q03是第三关节真实值，q04是第四关节真实值，q05是第五关节真实值，q06是第六关节真实值，q11是第一关节中间计算值，q12是第二关节中间计算值，q13是第三关节中间计算值，q14是第四关节中间计算值，q15是第五关节中间计算值，q16是第六关节中间计算值，q21是第一关节新计算值，q22是第二关节新计算值，q23是跌三关节新计算值，q24是第四关节新计算值，q25是第五关节新计算值，q26是第六关节新计算值，O0是参考坐标系，O1是第一关节坐标系，O2是第二关节坐标系，O3是第三关节坐标系，O4是第四关节坐标系，O5是第五关节坐标系，O6是第六关节坐标系，d1第一关节杆件偏移量，d3是第三关节杆件偏移量，L5是第五关节杆件长度，L6是第六关节杆件长度，α1是第一关节杆件扭转角，α2是第二关节杆件扭转角。具体实施方式具体实施方式一：结合图1至图3说明本实施方式，本实施方式所述用于具有三角形远心点机构的串联机械臂逆运动学数值解法的具体步骤如下：步骤一、假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；步骤二、根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心本文档来自技高网...

【技术保护点】
用于具有三角形远心点机构的串联机械臂逆运动学数值解法，其特征在于：所述用于具有三角形远心点机构的串联机械臂逆运动学数值解法的具体步骤如下：步骤一、假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；步骤二、根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中间计算值，命名为q13；步骤三、根据假设A，由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小，依次求出第一关节中间计算值q11和第二关节中间计算值q12；步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16；步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等，这是假设B；步骤六、在第二关节坐标系O2下，根据假设B，用第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计算值q23；步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22；步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26；步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，重新进入循环；步骤十、到达事先设定好的循环次数，推出循环，输出新的一组六个关节的位移值。...

【技术特征摘要】
1.用于具有三角形远心点机构的串联机械臂逆运动学数值解法，其特征在于：所述用
于具有三角形远心点机构的串联机械臂逆运动学数值解法的具体步骤如下：
步骤一、假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；
步骤二、根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位置
矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中
间计算值，命名为q13；
步骤三、根据假设A，由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和
第二关节位移量q2的大小，依次求出第一关节中间计算值q11和第二关节中间计算值q12；
步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿
矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、
第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第六
关节中间计算值q16；
步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤四
中计算出的第四关节中间计算值q14、第五关节中间计算值...

【专利技术属性】
技术研发人员：潘博，付宜利，曲效锋，王树国，艾跃，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江;23