用于具有三角形远心点机构的串联机械臂逆运动学数值解法
阅读：1022发布：2020-07-13
专利汇可以提供用于具有三角形远心点机构的串联机械臂逆运动学数值解法专利检索，专利查询，专利分析的服务。并且用于具有三 角 形远心点机构的 串联 机械臂 逆运动学数值解法，它涉及一种串联机械臂逆运动学数值解法，具体涉及一种用于具有三角形远心点机构的串联机械臂逆运动学数值解法。本 发明 为了解决现有微创 手术机械臂 控制系统 精度 较低、实时性较差的问题。本发明的具体步骤为：假设机械臂末端点与腕部起点O4重合，即忽略 手腕 处三个关节，这是假设A；根据假设A，输入量末端点 位姿 矩阵的 位置 分量与O0 坐标系 下的远心点位置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中间计算值，命名为q13。本发明属于微创医疗器械领域。，下面是用于具有三角形远心点机构的串联机械臂逆运动学数值解法专利的具体信息内容。
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、第五关节中间计算值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，若未达到事先设定好的循环次数，则由步骤五重新进入循环，否则，退出循环；
步骤十、输出新的一组六个关节的位移值。
用于具有三角形远心点机构的串联机械臂逆运动学数值解法
技术领域
[0001] 本发明涉及一种串联机械臂逆运动学数值解法，具体涉及一种用于具有三角形远心点机构的串联机械臂逆运动学数值解法，属于微创医疗器械领域。
背景技术
[0002] 微创手术与传统的开口手术相比具有手术切口小、术后疤痕小、恢复时间快等优点，因此微创手术被广泛地应用于临床手术。采用主从控制形式的微创手术机器人提高了微创手术的精度和应用范围，故而有广阔的市场前景。微创手术机器人的机械臂通常采用远心定点运动机构来实现手术器械在人体内的定位。三角形远心机构依靠关节轴线相交于定点的方式来实现机构的远心运动。前两个关节是转动关节，轴线相交于远心点，最后一个关节是移动关节，轴线通过远心点。在远心机构后面串联腕部三个关节，实现手术器械的运动。在远心点机构前面可串联数个被动关节，用于手术前调整远心点的位置，手术中锁死。机械臂逆运动学求解是根据机械臂在空间内的末端位置姿态矩阵，求解各个关节的位移量。逆运动学求解是实现机械臂定位控制和轨迹跟踪控制的基础，且逆运动学算法直接影响机械臂的运动控制器的性能。逆运动学方程是非线性超越方程，其求解过程非常复杂，而且可能存在多解、无解或者奇异等问题。对于六自由度串联机械臂，仅当其结构满足准则——机械臂腕部三个相邻的关节轴相交于一点，或者机械臂腕部三个相邻的关节的轴线相互平行，此时三个相邻关节的位置和姿态是解耦的，可以求出机械臂逆运动学的封闭解。
但一般情况下，考虑到使用性能和设计制造和装配过程中的困难和误差，机械臂都不满足这一准则。故而需要釆用数值解法完成机械臂的逆运动学求解。一般采用的数值解法的基本原理是根据机械臂当前的关节角度求解正运动学的计算结果，通过比较其与目标位置姿态值的差值，对关节角度进行修正，反复试探并控制定位误差计算关节角度最优值。这样的方法计算量大，不利于控制系统的实时性。
但一般情况下，考虑到使用性能和设计制造和装配过程中的困难和误差，机械臂都不满足这一准则。故而需要釆用数值解法完成机械臂的逆运动学求解。一般采用的数值解法的基本原理是根据机械臂当前的关节角度求解正运动学的计算结果，通过比较其与目标位置姿态值的差值，对关节角度进行修正，反复试探并控制定位误差计算关节角度最优值。这样的方法计算量大，不利于控制系统的实时性。
发明内容
[0004] 本发明为解决上述问题采取的技术方案是：本发明的具体步骤如下：
[0007] 步骤三、根据假设A，由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小，依次求出第一关节中间计算值q11和第二关节中间计算值q12；
[0008] 步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16；
[0009] 步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等，这是假设B；
[0010] 步骤六、在第二关节坐标系O2下，根据假设B，用第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计算值q23；
[0011] 步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22；
[0012] 步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26；
[0013] 步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，重新进入循环；
[0014] 步骤十、到达事先设定好的循环次数，推出循环，输出新的一组六个关节的位移。
[0015] 本发明的有益效果是：1、本发明求出的六个关节的位移量经过几次循环快速逼近真值，经过机械臂正运动学计算得到的位置姿态矩阵的位置分量与输入的末端点位置姿态矩阵的位置分量间的误差呈指数速度减小；2、位置姿态分离求解，先用位置信息，再用姿态信息，每一次循环求出的一组解，可保证再正解出来的姿态矩阵没有误差，只有位置矢量有误差。即在忽略其他误差的情况下，运用到机械臂上，末端位置有误差，姿态无误差；3、求解过程在关节空间内完成，计算量小。附图说明
[0016] 图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是第二关节杆件扭转角。
具体实施方式
[0017] 具体实施方式一：结合图1至图3说明本实施方式，本实施方式所述用于具有三角形远心点机构的串联机械臂逆运动学数值解法的具体步骤如下：
[0018] 步骤一、假设机械臂末端点与腕部起点O4重合，即忽略手腕处三个关节，这是假设A；
[0019] 步骤二、根据假设A，输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量，求出第三关节位移量q3的中间计算值，命名为q13；
[0020] 步骤三、根据假设A，由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小，依次求出第一关节中间计算值q11和第二关节中间计算值q12；
[0021] 步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16；
[0022] 步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等，这是假设B；
[0023] 步骤六、在第二关节坐标系O2下，根据假设B，用第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计算值q23；
[0024] 步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22；
[0025] 步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26；
[0026] 步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，由步骤五重新进入循环；
[0027] 步骤十、到达事先设定好的循环次数，退出循环，输出新的一组六个关节的位移值。
[0028] 本实施方式中第二关节位移量q2是通过公式(1)得出的：
[0029]
[0030] 公式(1)中 表示参考坐标系下第三关节坐标系的齐次变换矩阵， 表示参考坐标系下第一关节坐标系的齐次变换矩阵， 表示第一关节坐标系下第二关节坐标系的齐次变换矩阵， 表示第二关节坐标系下第三关节坐标系的齐次变换矩阵， 表示参考坐标系下第三关节坐标系的齐次变换矩阵的姿态矩阵分量， 表示参考坐标系下第三关节坐标系的齐次变换矩阵的位置矢量分量， 表示算法输入量末端点位置姿态矩阵的姿态矩阵分量， 表示算法输入量末端点位置姿态矩阵的位置矢量分量， 表示算法输入量末端点位置姿态矩阵；根据假设A， 与 与 相等。
[0031] 再求第一关节中间计算值q11。对公式(1)，等式两边左乘 得到公式(3)：
[0032]
[0033] 公式(3)中 表示矩阵 的位置分量, 表示 的逆矩阵，依旧使等式两边对应量相等，求第一关节中间计算值q11。这里为了计算简便，可将两个对应项方程联立求出sinq11或者cosq11。
[0034] 步骤二和步骤三利用输入矩阵的位置分量的三个值求出了假设条件下的第一关节位移量q1、第二关节位移量q2和第三关节位移量q3的值，即第一关节中间计算值q11、第二关节中间计算值q12和第三关节中间计算值q13。
[0035] 步骤四是在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13，依次求出第五关节中间计算值q15、第六关节中间计算值q16、第四关节中间计算值q14。所以步骤四中只计算矩阵的姿态部分。
[0036] 本实施方式步骤四中第五关节中间计算值q15是通过公式(4)、(5)得出的：
[0037]
[0038] -cosq15＝az (5)，
[0039] 公式(4)中 表示第三关节坐标系下第四关节坐标系的齐次变换矩阵， 表示第四关节坐标系下第五关节坐标系的齐次变换矩阵， 表示第五关节坐标系下第六关节坐标系的齐次变换矩阵， 表示 的逆矩阵。
[0040] 对公式(4)，选择较简单的对应项相等，得到公式(5)。
[0041] 公式(5)中az表示等式右侧矩阵对应项。
[0042] 再求第六关节中间计算值q16。对公式(4)，选择较简单的对应项相等，得到公式(6)。
[0043] -sinq15·sinq16＝oz (6)，
[0044] 公式(6)中oz表示等式右侧矩阵对应项。
[0045] 再求第四关节中间计算值q14。对公式(4)，选择较简单的对应项相等，得到公式(7)。
[0046] sinq14·sinq15＝ay (7)，
[0047] 公式(7)中ay表示等式右侧矩阵对应项。
[0048] 因第一关节中间计算值q11、第二关节中间计算值q12和第三关节中间计算值q13是在假设条件下求出的，存在误差，所以第四关节中间计算值q14、第五关节中间计算值q15和第六关节中间计算值q16也存在误差。其中第三关节中间计算值q13比真值大，大体体现出真实腕部位置和真实末端位置的差。而从第一关节中间计算值q11、第二关节中间计算值q12求解的表达式来看，因为是两个比真值大的数相除，所以误差相对较小。第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16的情况类似。而且，因为存在远心点机构，所以第一关节真实值q01、第二关节真实值q02和上面求得的第一关节中间计算值q11、第二关节中间计算值q12都使得坐标系原点O2处在远心点上。而第三关节的轴线A3的真实方向与第一关节中间计算值q11和第二关节中间计算值q12作用下的A3方向不同。根据上述分析，现在需要将求得的第三关节中间计算值q13的大小尽量接近真值。
[0049] 这里进入循环。因为第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16与真值的误差相对较小，为了使第三关节中间计算值q13尽量接近真值，第三关节中间计算值q13的轴线方向尽量接近真值方向，则假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与刚计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等。此为假设B。
[0050] 步骤六是在第二关节的坐标系O2下(坐标系O2的原点即为远心点)，根据假设B，用第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，求出第三关节新计算值q23。
[0051] 由附图3所示：根据假设B，将第三关节新计算值q23、第四关节中间计算值q14、第五关节中间计算值q15和第六关节中间计算值q16代入从第二关节坐标系到第六关节坐标系的运动学正解，可求得机械臂末端在第二关节坐标系O2下的位置。根据假设B，该位置可认为是机械臂末端的真实位置。而机械臂末端位置到第二关节坐标系原点(即远心点)O2的距离即为q13。
[0052] 上述过程用矩阵形式表示为：
[0053]
[0054]
[0055] 公式(8)中 表示代入第三关节新计算值q23的第二关节坐标系下的第三关节坐标系的齐次变换矩阵， 表示代入第三关节新计算值q23的第二关节坐标系下的第六关节坐标系的齐次变换矩阵的姿态矩阵， 表示代入第三关节新计算值q23的第二关节坐标系下的第六关节坐标系的齐次变换矩阵的位置矢量， 表示 在第二关节坐标系X轴上的分量， 表示 在第二关节坐标系Y轴上的分量， 表示 在第二关节坐标系Z轴上的分量。
[0056] 对公式(8)展开得到公式(10)：
[0057]
[0058] 将公式(10)代入公式(9)，求出第三关节新计算值q23。
[0059] 将第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23代入齐次变换方程，得到公式(11)：
[0060]
[0061] 公式(11)中 表示代入第三关节新计算值q23的参考坐标系下的第一关节坐标系的齐次变换矩阵、 表示代入第三关节新计算值q23的第一关节坐标系下的第二关节坐标系的齐次变换矩阵。
[0062] 将公式(11)中的 右乘到方程右边去，得到公式(12)：
[0063]
[0064] 对公式(12)，矩阵位置分量对应项相等，选择较简单的一对，得到公式(13)[0065] d1+q23·(cosα1·cosα2-cosq22·sinα1·sinα2)＝P'z (13)，
[0066] 公式(13)中，P'z表示等式右侧对应项，从而求出第二关节的新计算值q22。
[0067] 再求第一关节的新计算值q21。将公式(12)中的 左乘过去，得到公式(14)：
[0068]
[0069] 依旧按照矩阵对应项相等，求出第一关节的新计算值q21。这里为了计算简便，可将两个对应项方程联立，求出sinq21或者cosq21。
[0070] 求出第一关节的新计算值q21和第二关节的新计算值q22，使得第三关节的轴线A3方向也变得比较准。
[0071] 步骤八是利用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依次求出第五关节新计算值q25、第六关节新计算值q26、第四关节新计算值q24；
[0072] 抛弃假设B。对公式(11)，将第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26代入,得到公式(15)：
[0073]
[0074] 公式(15)中， 表示代入第四关节新计算值q24的第三关节坐标系下的第四关节坐标系的齐次变换矩阵， 表示代入第五关节新计算值q25的第四关节坐标系下的第五关节坐标系的齐次变换矩阵， 表示代入第六关节新计算值q26的第五关节坐标系下的第六关节坐标系的齐次变换矩阵。
[0075] 将 左乘到方程右侧，得到公式(16)：
[0076]
[0077] 公式(16)中， 表示 的逆矩阵， 表示 的逆矩阵， 表示 的逆矩阵。
[0078] 与之前的过程类似，对公式(16)，矩阵的姿态分量各对应项相等，寻找较简单的一对，得到公式(17)：
[0079] -cosq25＝az′ (17)，
[0080] 公式(17)中az′表示等式右侧矩阵对应项。求出第五关节新计算值q25。
[0081] 再求第六关节新计算值q26。对公式(16)，选择较简单的对应项相等，得到公式(18)。
[0082] -sinq25·sinq26＝oz' (18)，
[0083] 公式(18)中oz′表示等式右侧矩阵对应项。求出第六关节新计算值q26。
[0084] 再求第四关节新计算值q24。对公式(16)，选择较简单的对应项相等，得到公式(19)。
[0085] sinq24·sinq25＝ay' (19)，
[0086] 公式(19)中ay′表示等式右侧矩阵对应项。求出第四关节新计算值q24。
[0087] 至此，就求出了新的一组比较准的解第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26。新的一组解比旧的一组解精度更高。
[0088] 步骤九是用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16，重新进入循环。这样不断地用算出的结果作为循环的输入，从而迭代得出误差更小的解。
[0089] 需要强调的是，因为第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26是根据算法输入的姿态矩阵的对应项相等求出的，所以求出的一组解根据正运动学计算得到的姿态是正确的，只有位置是有误差的。
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种辅助手术用的医疗机械臂 | 2020-05-14 | 658 |
| 手术机器人及其机械臂 | 2020-05-11 | 66 |
| 用于机械臂的制动系统及手术机器人 | 2020-05-14 | 740 |
| 手术机械臂定位系统 | 2020-05-12 | 1007 |
| 手术机器人及其机械臂 | 2020-05-12 | 49 |
| 一种手术机器人变刚度腕部结构及手术机械臂 | 2020-05-13 | 13 |
| 手术机器人及其机械臂 | 2020-05-12 | 190 |
| 一种医用微创手术机械臂 | 2020-05-15 | 501 |
| 一种辅助手术用的医疗机械臂 | 2020-05-15 | 966 |
| 一种医学诊疗手术机器人的机械臂 | 2020-05-15 | 165 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
分析报告
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
手术机械臂热门专利
QQ群二维码
意见反馈
意见反馈