申请/专利权人：哈尔滨工业大学

申请日：2024-01-30

公开（公告）日：2024-06-14

公开（公告）号：CN117817668B

主分类号：B25J9/16

分类号：B25J9/16;G06F17/00

优先权：

专利状态码：有效-授权

法律状态：2024.06.14#授权;2024.04.23#实质审查的生效;2024.04.05#公开

摘要：一种SSRMS构型机械臂单关节故障的运动学重构方法，涉及机械臂运动能力分析技术领域。建立SSRMS构型机械臂的关节运动旋量坐标；关节1故障时的求解；关节7故障时的求解；关节2故障时的求解；关节6故障时的求解；关节1或2故障时的关节角约束关系式解耦；关节6或7故障时的关节角约束关系式解耦；关节3、4、5故障时的逆运动学求解。基于旋量理论建立机械臂的关节旋量坐标，利用结构对称的特点分类讨论不同关节故障时的运动学重构方案，保障机械臂在单关节故障的情况下也能够完成任务。

主权项：1.一种SSRMS构型机械臂单关节故障的运动学重构方法，其特征在于：包括以下步骤：步骤一：建立SSRMS构型机械臂的关节运动旋量坐标以{XbYbZb}和{XeYeZe}分别表示机械臂的基坐标系和末端坐标系，以qi{i＝1,…,7}表示位于各关节螺旋轴上的点，以Si表示以基坐标系为参考建立的各关节螺旋轴，以at{t＝0,1,…,8}表示臂杆长度，从而建立SSRMS构型机械臂的关节运动旋量坐标，各关节螺旋轴的运动旋量坐标Si＝wi,vi，其中，wi为各关节螺旋轴Si的单位方向向量，vi＝-wi×qi为与基坐标系原点重合的点相对各关节螺旋轴Si的线速度；各关节旋转角度θi后，各关节的矩阵指数为则以基坐标系为参考坐标系的机械臂正运动学指数积表达式Ttarget为： 式中，Mstart为末端坐标系相对基坐标系的初始齐次坐标矩阵bTe或基坐标系相对末端坐标系的初始齐次坐标矩阵eTb，bTe与eTb相等；步骤二：关节1故障时的求解2.1、关节1故障时的机械臂运动学重构关节1故障时的角度值为以基坐标系为参考坐标系，等效六自由度机械臂的初始位姿矩阵ML1为： 对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为： 式中，为关节螺旋轴S1转动角度后的矩阵指数，为给定的末端位姿矩阵，为变换后的各关节螺旋轴转动角度θi后的矩阵指数；2.2、特征位置点“自旋转”运动轨迹令关节6与关节7的旋转轴线交点为并将其选为特征位置点，在基坐标系中的位置齐次坐标表示为得到下述关系式： 进而得到关系式： 根据公式9认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ2至点2.3、特征位置点坐标求解令点的位置齐次坐标表示为代入公式9得到关系式： 通过求解点的位置坐标已知，令为关节1与关节2旋转轴线的交点，点到的距离与到的距离相等，故得到等式： 式中，为关节3旋转轴线的方向向量，为关节2旋转轴线的方向向量；能够得到点的两组位置坐标表达式为： 式中，对三角函数表达式进行简化为Pdy0通过下式计算： 2.4、关节1故障时的关节2解析解表达式 和两点之间存在关系式： 根据Paden-Kahan子问题1能够得到关节角2转过的角度值θ2： 式中， 点存在两组解，角度值θ2也有两组解；2.5、关节1故障时关节3、4、5、6和7的约束关系式由于角度值θ2以及Mstart-1和均已知，代入公式5并化简得到关节3、4、5、6和7之间的关系式为： 步骤三：关节7故障时的求解根据SSRMS构型机械臂的结构特点，关节1与关节7处于对称位置，因此，关节7出现故障导致关节角固定无法转动时的逆运动学求解与关节1具有一致性，区别在于：选择末端坐标系为参考坐标系，各关节螺旋轴相对末端坐标系的旋量坐标替换为Bi，关节7故障时的角度值为等效六自由度机械臂的初始位姿矩阵ML7为： 对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为： 选择关节1与关节2的旋转轴线交点为特征位置点PaL7，计算公式为： 式中，Pa0L7＝[-a2-a4-a6-a80-a1-a3-a5-a71]T为点PaL7在关节7发生变化前的初始位置齐次坐标；进而得到关系式： 根据公式32认为点PaL7先绕旋转θ3至点PbL7，然后绕旋转θ4至点PcL7，再绕旋转θ5至点PdL7，最后绕旋转θ6至点PeL7；令关节6与关节7的轴线交点为根据几何关系可知： 式中，为点在关节7发生变化前的初始位置齐次坐标；点PdL7和PeL7之间的关系式： 将PdL7、PeL7和通过公式21求解得到和然后代入公式22得到和最终得到关节角θ6的求解关系式为： 关节角1、2、3、4和5之间的约束关系式为： 步骤四：关节2故障时的求解4.1、关节2故障时的机械臂运动学重构关节2故障时的角度值为以基坐标系为参考坐标系，等效六自由度机械臂的初始位姿矩阵ML2为： 对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为： 4.2、特征位置点“自旋转”运动轨迹令关节6与关节7的旋转轴线交点并将其选为特征位置点，可以得到下述关系式： 进而得到关系式： 根据公式43认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ1至点4.3、特征位置点坐标求解令点的位置齐次坐标表示为根据公式43可以得到： 因为和两点到点和点的距离相等，关节3、4和5的螺旋轴平行，过和两点的直线与关节3的螺旋轴方向向量垂直，故得到关系式： 式中，为基坐标系原点，为关节1与关节2轴线的交点；令点的位置坐标表达式为能够得到点的位置齐次坐标表达式为： 4.4、关节2故障时的关节1解析解表达式点和两点之间存在关系式： 根据Paden-Kahan子问题1能够得到关节角1转过的角度值θ1： 点存在两组解，所以角度值θ1也有两组解；4.5、关节2故障时关节3、4、5、6和7的约束关系式由于角度值θ1以及Mstart-1和均已知，代入公式40并化简得到关节3、4、5、6和7之间的关系式为： 步骤五：关节6故障时的求解根据SSRMS构型机械臂的结构特点，关节2与关节6处于对称位置，因此，关节2与关节6出现故障导致关节角固定无法转动时的逆运动学求解具有一致性，区别在于：选择末端坐标系为参考坐标系，各关节螺旋轴相对末端坐标系的旋量坐标替换为Bi，关节6故障时的角度值为等效六自由度机械臂的初始位姿矩阵ML6为： 对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为： 选择关节1与关节2的旋转轴线交点为特征位置点计算公式为： 进而得到关系式： 根据公式64认为点先绕旋转θ3至点PbL6，然后绕旋转θ4至点PcL6，再绕旋转θ5至点PdL6，最后绕旋转θ7至点PeL6；令关节6与关节7的轴线交点为根据几何关系可知： 点PdL6和PeL6之间的关系式： 求解得到和最终得到关节角θ7的求解关系式为： 关节角1、2、3、4和5之间的约束关系式为： 步骤六：关节1或2故障时的关节角约束关系式解耦6.1、关节旋量坐标变换以基坐标系为参考坐标系时，关节p＝1,2角度值θp发生变化前后，关节p与关节m之间的矩阵指数存在如下关系，p＜m： 进而得到关系式： 将公式73和74右侧统一改写为矩阵M37且内部各元素已知： 6.2、关节3、4、5、6、7解析解表达式求解因为关节3、4、5的螺旋轴均绕基坐标系的X轴旋转，根据公式75可以得到： 式中，c345＝cosθ3+θ4+θ5，s345＝sinθ3+θ4+θ5；当s6≠0时，根据公式76得到θ6、θ7和θ345的求解表达式： 当s6＝0时，根据c6的取值得到关系式： 进一步求解关节角θ7和θ345，为简化求解过程，s6＝0时取θ7＝0；θ4的求解结果为：θ4＝±acos[q242+q342-a32-a522a3a5]83θ3的求解结果为：θ3＝atan2[a3+a5c4q24-a5s4q34，a3+a5c4q34+a5s4q24]86θ5的求解结果为：θ5＝θ345-θ3-θ487步骤七：关节6或7故障时的关节角约束关系式解耦以末端坐标系为参考坐标系时，关节q＝6,7角度值θq发生变化前后，关节q与关节n之间的矩阵指数存在如下关系，q＞n： 进而得到关系式： 改写为矩阵N15且内部各元素已知： 根据公式90和91右侧的计算结果可以得到关系式： 当s2≠0时，根据93得到θ1、θ2和θ345的求解表达式： 当s2＝0时，得到关系式： 取θ1＝θ2＝0，因此可以得到θ345的角度值为：θ345＝atan2I32,I2296θ4的求解结果为：θ4＝±arccos[j242+j342-a52+a32]4a3a5101θ3的求解结果为：θ3＝atan2s3,c3＝atan2[a5c4+a32j24-a5s4j34,a5c4+a32j34-a5s4j24]104θ5的求解结果为：θ5＝θ345-θ3+θ4105步骤八：关节3、4、5故障时的逆运动学求解8.1、关节旋量坐标变换给定机械臂的期望位姿和临近参考构型θref，关节3、4或5的其中一个角度值固定，根据正运动学方程得到当前临近参考构型θref下机械臂末端的实际位姿为： 以末端坐标系为参考，期望位姿与实际位姿之间存在变换关系eT： 利用矩阵对数log求解eT的运动旋量eV： 式中，eV＝[weTveT]T，[eV]为运动旋量eV的矩阵形式；8.2、数值解求解精度迭代如果根据当前临近参考构型θref求解得到的角速度we或速度ve不满足精度条件：||we||＜εw或||ve||＜εv109式中，εw和εv分别为给定的角速度和速度的精度阈值；则通过下列关系式进行更新：θref+1＝θref+J+θrefeV110式中，J+θref＝JTJJT-1为机械臂雅可比矩阵J的右逆矩阵；重复公式110进行更新，该过程中，出现故障的关节角度值θj不发生改变，直至满足公式109的精度条件或达到迭代上限次数，则可以得到关节3、4或5出现故障时其余关节角的逆运动学数值解。

全文数据：

权利要求：

百度查询： 哈尔滨工业大学 一种SSRMS构型机械臂单关节故障的运动学重构方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD