CN117817668A - 一种ssrms构型机械臂单关节故障的运动学重构方法 - Google Patents
一种ssrms构型机械臂单关节故障的运动学重构方法 Download PDFInfo
- Publication number
- CN117817668A CN117817668A CN202410133265.2A CN202410133265A CN117817668A CN 117817668 A CN117817668 A CN 117817668A CN 202410133265 A CN202410133265 A CN 202410133265A CN 117817668 A CN117817668 A CN 117817668A
- Authority
- CN
- China
- Prior art keywords
- joint
- point
- rotation
- mechanical arm
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 89
- 239000011159 matrix material Substances 0.000 claims description 65
- 230000014509 gene expression Effects 0.000 claims description 51
- 230000008569 process Effects 0.000 claims description 31
- 230000009466 transformation Effects 0.000 claims description 25
- 239000013598 vector Substances 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 11
- 238000004804 winding Methods 0.000 claims description 3
- 210000001503 joint Anatomy 0.000 description 40
- 238000013016 damping Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 210000002310 elbow joint Anatomy 0.000 description 2
- 210000000323 shoulder joint Anatomy 0.000 description 2
- 210000003857 wrist joint Anatomy 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 241000287196 Asthenes Species 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Robotics (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- Databases & Information Systems (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Software Systems (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
一种SSRMS构型机械臂单关节故障的运动学重构方法,涉及机械臂运动能力分析技术领域。建立SSRMS构型机械臂的关节运动旋量坐标;关节1故障时的求解;关节7故障时的求解;关节2故障时的求解;关节6故障时的求解;关节1或2故障时的关节角约束关系式解耦;关节6或7故障时的关节角约束关系式解耦;关节3、4、5故障时的逆运动学求解。基于旋量理论建立机械臂的关节旋量坐标,利用结构对称的特点分类讨论不同关节故障时的运动学重构方案,保障机械臂在单关节故障的情况下也能够完成任务。
Description
技术领域
本发明涉及机械臂运动能力分析技术领域,具体是一种SSRMS构型机械臂单关节故障的运动学重构方法。
背景技术
SSRMS构型机械臂是与国际空间站遥操作系统(SSRMS:Space Station RemoteManipulator System)关节数量一致、结构相同的空间机械臂的统称,由于该构型机械臂具有一个冗余自由度,且整体结构对称分布,在避关节限位、力矩优化、关节避障等方面具有灵活的操控能力,在空间在轨操作任务中具有广阔的应用范围,如国际空间站的Canadarm2、ERA(European Robotic Arm)以及中国空间站的实验舱机械臂、核心舱机械臂均属于此构型机械臂。冗余自由度的存在,不但增加了机械臂的灵活性,而且在执行预期任务时,对于给定的末端位姿,SSRMS构型机械臂有无数种操作构型,即存在无数组关节角逆解。
但受极端复杂的太空环境等在轨工作条件的影响,机械臂在轨执行任务过程中可能会出现关节故障等突发情况导致某一关节停止运动,此时七自由度冗余机械臂等效为六自由度机械臂,则不能按照预期规划的路径轨迹正常运动或工作,无法完成预期任务甚至对机械臂自身和周围设备造成损伤。何川甫等人(何川甫,等。一种多自由度机械臂的逆运动学通用求解方法及装置[P],2022)虽然提出一种运动学求解方法,但其未考虑冗余机械臂存在多组逆解导致迭代过程不收敛、迭代次数达到上限等情况,并且,由于SSRMS构型机械臂的肩、肘、腕关节均存在偏置等构型特征,不同关节出现故障时,等效六自由度机械臂的结构特点不完全一致,进一步增加了采用同一种运动学重构方法控制机械臂运动的难度。
因此,针对SSRMS构型机械臂出现单关节故障无法转动等情况,亟需考虑不同关节故障情况下的运动学重构解决方案,根据实时情况重新计算规划路径轨迹或实现动作控制,使SSRMS构型机械臂在故障状态下仍能继续执行任务,以最大程度地减少故障对机械臂操作的影响,使其顺利完成在轨工作任务。
发明内容
为解决背景技术存在的不足,本发明提供一种SSRMS构型机械臂单关节故障的运动学重构方法,它基于旋量理论建立机械臂的关节旋量坐标,利用结构对称的特点分类讨论不同关节故障时的运动学重构方案,保障机械臂在单关节故障的情况下也能够完成任务。
为实现上述目的,本发明采取下述技术方案:一种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以及和均已知,代入公式(5)并化简得到关节3、4、5、6和7之间的关系式为:
步骤三:关节7故障时的求解
根据SSRMS构型机械臂的结构特点,关节1与关节7处于对称位置,因此,关节7出现故障导致关节角固定无法转动时的逆运动学求解与关节1具有一致性,区别在于:
选择末端坐标系为参考坐标系,各关节螺旋轴相对末端坐标系的旋量坐标替换为Bi,关节7故障时的角度值为等效六自由度机械臂的初始位姿矩阵ML7为:
对机械臂各关节螺旋轴对应的运动旋量进行变换,等效六自由度机械臂的正运动学方程表示为:
选择关节1与关节2的旋转轴线交点为特征位置点Pa L7,计算公式为:
式中,Pa0 L7=[-a2-a4-a6-a8 0 -a1-a3-a5-a7 1]T为点Pa L7在关节7发生变化前的初始位置齐次坐标;
进而得到关系式:
根据公式(32)认为点Pa L7先绕旋转θ3至点Pb L7,然后绕旋转θ4至点Pc L7,再绕旋转θ5至点Pd L7,最后绕旋转θ6至点Pe L7;
令关节6与关节7的轴线交点为根据几何关系可知:
式中,为点在关节7发生变化前的初始位置齐次坐标;
点Pd L7和Pe L7之间的关系式:
将Pd L7、Pe L7和通过公式(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至点然后绕旋转θ4至点Pc L6,再绕旋转θ5至点Pd L6,最后绕旋转θ7至点Pe L6;
令关节6与关节7的轴线交点为根据几何关系可知:
点Pd L6和Pe L6之间的关系式:
求解得到和最终得到关节角θ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[(q24 2+q34 2-a3 2-a5 2)/2a3a5] (83)
θ3的求解结果为:
θ3=atan2[(a3+a5c4)q24-a5s4q34,(a3+a5c4)q34+a5s4q24] (86)
θ5的求解结果为:
θ5=θ345-θ3-θ4 (87)
步骤七:关节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=atan2(I32,I22) (96)
θ4的求解结果为:
θ4=±arccos[j24 2+j34 2-(a5 2+a3 2)]/4a3a5 (101)
θ3的求解结果为:
θ3=atan2(s3,c3)=atan2[(a5c4+a3)2j24-a5s4j34,(a5c4+a3)2j34-a5s4j24] (104)
θ5的求解结果为:
θ5=θ345-(θ3+θ4) (105)
步骤八:关节3、4、5故障时的逆运动学求解
8.1、关节旋量坐标变换
给定机械臂的期望位姿和临近参考构型θref,关节3、4或5的其中一个角度值固定,根据正运动学方程得到当前临近参考构型θref下机械臂末端的实际位姿为:
以末端坐标系为参考,期望位姿与实际位姿之间存在变换关系eT:
利用矩阵对数log求解eT的运动旋量eV:
式中,eV=[we T ve T]T,[eV]为运动旋量eV的矩阵形式;
8.2、数值解求解精度迭代
如果根据当前临近参考构型θref求解得到的角速度we或速度ve不满足精度条件:
||we||<εw或||ve||<εv (109)
式中,εw和εv分别为给定的角速度和速度的精度阈值;
则通过下列关系式进行更新:
θref+1=θref+J+(θref)eV (110)
式中,J+(θref)=JT(JJT)-1为机械臂雅可比矩阵J的右逆矩阵;
重复公式(110)进行更新,该过程中,出现故障的关节角度值θj不发生改变,直至满足公式(109)的精度条件或达到迭代上限次数,则可以得到关节3、4或5出现故障时其余关节角的逆运动学数值解。
与现有技术相比,本发明的有益效果是:本发明基于旋量理论建立机械臂的关节旋量坐标,利用结构对称的特点分类讨论不同关节故障时的运动学重构方案,关节1、2、6、7故障时,重构运动学方程,通过选取等效机械臂的特征位置点并构建其“自旋转”运动,将逆运动学问题转化为已知的Paden-Kahan子问题求解,关节3、4、5故障时,根据等效六自由度机械臂不满足Pieper原则无法求得解析解的特点,利用关节运动旋量变换通过迭代法求各关节数值解,提高了机械臂出现单关节故障时的容错性,快速地找到新的适当路径或解决方案,使机械臂尽快恢复正常运行以继续执行任务,最大程度地减少故障对机械臂整体操作的影响。
附图说明
图1是SSRMS构型机械臂的关节运动旋量坐标示意图;
图2是关节1故障时的机械臂构型变换示意图;
图3是关节1故障时的特征位置点“自旋转”运动轨迹;
图4是关节1故障时的点约束关系示意图;
图5是图4的X1截平面投影示意图;
图6是关节2故障时的机械臂构型变换示意图;
图7是关节2故障时的特征位置点“自旋转”运动轨迹;
图8是本发明的运动学重构方法流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是发明的一部分实施例,而不是全部的实施例,基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
根据七自由度SSRMS构型机械臂的构型特征,针对其中某一关节出现故障无法转动等情况,分析等效六自由度机械臂的结构特点,分类讨论不同关节失效情况下的运动学重构方法,并建立相应的逆运动学求解方案,使等效六自由度机械臂能够正常工作和运行,保障在轨操作任务的可靠性与安全性。
当关节1、2、6、7中的某个关节失效无法转动时,基于旋量理论通过坐标系变换原则重构等效六自由度机械臂的正运动学方程,通过特征位置点围绕关节螺旋轴的“自旋转”运动,将等效六自由度机械臂的逆运动学求解问题转化为Paden-Kahan子问题,得到各关节角的约束关系式,通过关节旋量坐标变换实现运动学解耦并求解各关节角的解析解表达式。
当关节3、4、5中的某个关节失效无法转动时,等效六自由度机械臂不满足Pieper原则无法得到解析解,采用基于关节旋量坐标变换的迭代法求解数值解时,由于机械臂的雅可比矩阵J(J∈R6×7)非方阵且其中一个关节角度固定不变,通过雅可比伪逆矩阵逐步迭代得到满足预期精度的数值解,同时限制迭代次数上限以避免迭代不收敛陷入死循环导致求解失败。在求解雅可比伪逆矩阵时,为避免得到奇异构型,求解过程中根据机械臂的奇异程度选择相应的阻尼系数。
如图1~图8所示,一种SSRMS构型机械臂单关节故障的运动学重构方法,结合图8所示,包括以下步骤:
步骤一:建立SSRMS构型机械臂的关节运动旋量坐标
结合图1所示,以{XbYbZb}和{XeYeZe}分别表示机械臂的基坐标系和末端坐标系,以qi{i=1,…,7}表示位于各关节螺旋轴上的点,机械臂的七个关节均为旋转关节,其中关节1、2、3为肩关节,关节4为肘关节,关节5、6、7为腕关节,以Si表示以基坐标系为参考建立的各关节螺旋轴,由于机械臂结构特点,关节1的螺旋轴S1与基坐标系的Xb轴重合,关节7的螺旋轴S7与末端坐标系的Xe轴重合,以at{t=0,1,…,8}表示臂杆长度,由于肩部、肘部和腕部存在偏置,其中a1、a4、a7均不为0,从而建立SSRMS构型机械臂的关节运动旋量坐标,各关节螺旋轴的运动旋量坐标Si=(wi,vi)结合表1所示:
表1SSRMS构型机械臂的关节旋量坐标
其中,wi为各关节螺旋轴Si的单位方向向量,vi=-wi×qi为与基坐标系原点重合的点相对各关节螺旋轴Si的线速度。
根据结构的对称性可知,末端坐标系相对基坐标系的初始齐次坐标矩阵bTe以及基坐标系相对末端坐标系的初始齐次坐标矩阵eTb均为Mstart,表达式如下:
各关节旋转角度θi后,能够得到各关节的矩阵指数为则以基坐标系为参考坐标系的机械臂正运动学指数积表达式Ttarget为:
步骤二:关节1故障时的求解
2.1、关节1故障时的机械臂运动学重构
关节1故障时的角度值为机械臂构型变换前后结合图2所示,此时关节1固定无法进行旋转运动但关节1角度值已知,以基坐标系为参考坐标系,等效六自由度机械臂的初始位姿矩阵ML1为:
式中,为关节螺旋轴S1转动角度后的矩阵指数。
机械臂各关节螺旋轴对应的运动旋量需要进行如下变换:
式中,为的伴随变换矩阵。
各关节螺旋轴的运动旋量发生变化后,各关节的矩阵指数表示为联立公式(2)-公式(4),等效六自由度机械臂的正运动学方程表示为:
式中,为给定的末端位姿矩阵,为变换后的各关节螺旋轴转动角度θi后的矩阵指数。
将已知项和ML1整理至等式同一侧得到:
2.2、特征位置点“自旋转”运动轨迹
令关节6与关节7的旋转轴线交点为并将其选为特征位置点,在基坐标系中的位置齐次坐标表示为根据位于旋转轴线上的点在旋转过程中位置不变,可以得到下述关系式:
其中,位置坐标表达式为:
式中,Pa0为机械臂关节旋转前在基坐标系中的位置坐标表达式。
将公式(7)代入公式(6)中可以得到关系式:
根据公式(9)可以认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ2至点变换过程结合图3所示;
2.3、特征位置点坐标求解
令点的位置齐次坐标表示为代入公式(9)可以得到关系式:
式中,[·]T表示向量或矩阵[·]的转置。
因为Mstart -1及均为已知项,联立公式(8)和公式(10)可以得到px、py及pz的表达式,即点的位置坐标已知。
由于关节3、4、5旋转轴线平行,所以过和两点的直线与关节3旋转轴线的方向向量垂直,过和两点的直线与关节2旋转轴线的方向向量垂直。令为关节1与关节2旋转轴线的交点,根据距离保持不变原则,点到的距离与到的距离相等,故可以得到等式:
式中,||·||表示向量的欧几里得范数。
因为点位于关节1的旋转轴线上,所以关节1旋转一定角度后的齐次坐标表达式为:
式中,Pf0为机械臂处于初始构型(图1所示)时点的位置齐次坐标。
根据公式(4)可以得到和的表达式分别为:
式中,对三角函数表达式进行简化为si=sinθi,ci=cosθi。
令点的坐标表达式为根据公式(11)中的第一个方程和公式(14),可以得到关系式:
式中,Pax为点在基坐标系中的X轴坐标。
根据上述计算分析可知,点位于以点为球心的球面上,同时位于基坐标系中X1=-a0-a2-a4-a6平面上,平面与球面的交线为空间圆,并且,点还位于与点相关的截平面内,截平面的点法式表示为过的且法向量为的平面,结合图4所示。
根据各点位置关系向基坐标系中X1平面投影,因为X1平面与关节1的旋转轴线S1垂直,得到X1截平面剖视图结合图5所示。
根据上述分析可知,点存在两组解Pd_a与Pd_b,分别对应Pd_a0与Pd_b0绕关节1的螺旋轴S1旋转角度点Pe_p对应点Pe_p0绕关节1的螺旋轴S1旋转角度故可以得到点Pe_p=[Px Py Pz]T和Pe_p0=[Px0 Py0 Pz0]T的位置坐标关系式:
式中,对三角函数表达式进行简化为
点Pd_a0与Pd_b0的Y轴坐标互为相反数,根据公式(11)中的第三个方程,即点到的距离与到Pf的距离相等,同理,Pd_a0和Pd_b0到的距离与Pe_p0到的距离相等,故可以得到关系式为:
式中:Pdy0为点Pd_a0与Pd_b0的Y轴坐标。
所以Pd_a0和Pd_b0的位置坐标Pd_0如下:
所以可以得到点的两组位置坐标表达式为:
2.4、关节1故障时的关节2解析解表达式
因为点绕螺旋轴旋转角度θ2后得到点所以和两点之间存在关系式:
由于点和的位置坐标均为已知量,为关节2螺旋轴的单位向量,根据Paden-Kahan子问题1(参考资料:M.Murray,等,A mathematical introduction torobotics manipulation,1994.),令:
式中,和分别为和在与旋转轴垂直的平面上的投影。
所以,联立公式(21)和公式(22)可以得到关节角2转过的角度值θ2:
根据公式(19)可知,点存在两组解,所以角度值θ2也有两组解;
2.5、关节1故障时关节3、4、5、6、7的约束关系式
根据公式(23)求解得到的角度值θ2以及已知项Mstart -1和代入公式(6)并化简,得到关节3、4、5、6、7之间的关系式为:
步骤三:关节7故障时的求解
根据SSRMS构型机械臂的结构特点,关节1与关节7处于对称位置,因此,关节7出现故障导致关节角固定无法转动时的逆运动学求解与关节1具有一致性,区别在于:
选择末端坐标系为参考坐标系,各关节螺旋轴相对末端坐标系的旋量坐标为Bi,则机械臂正运动学指数积达式Ttarget为:
式中,为矩阵Mstart的伴随变换矩阵。
因为关节7故障时的角度值为等效六自由度机械臂的初始位姿矩阵ML7为:
各关节螺旋轴的计算公式为:
等效六自由度机械臂的正运动学方程为:
联立公式(25)-(29)可以得到:
选择关节1与关节2的旋转轴线交点为特征位置点计算公式为:
式中,Pa0 L7=[-a2-a4-a6-a80-a1-a3-a5-a7 1]T为点Pa L7在关节7发生变化前的初始位置齐次坐标。
根据位置不变原则和公式(30),可以得到:
公式(32)可以表示点Pa L7先绕旋转θ3至点Pb L7,然后绕旋转θ4至点Pc L7,再绕旋转θ5至点Pd L7,最后绕旋转θ6至点Pe L7。令点Pe L7的齐次坐标为Pe L7=[px L7 py L7pz L71]T,因为Pa L7、ML7 -1和均为已知项,代入公式(32)可以求得Pe L7的各项表达式。
令关节6与关节7的轴线交点为根据几何关系可知:
式中,为点在关节7发生变化前的初始位置齐次坐标。
因为点Pa L7、Pe L7和为已知项,和可以通过公式(26)求解,使用点Pa L7、Pd L7、Pe L7和代替Pa L1、Pd L1、Pe L1和使用和代替和使用末端坐标系的平面X7=-a2-a4-a6-a8代替基坐标系的平面X1,代入公式(11)-(19)可以求解得到的表达式以及点Pd L7和Pe L7之间的关系式:
将Pd L7、Pe L7和通过公式(21)可以求解得到和然后代入公式(22)得到和最终得到关节角θ6的求解关系式为:
将关节角θ6的求解结果代入公式(30),可以得到关节角1、2、3、4、5之间的约束关系式为:
步骤四:关节2故障时的求解
4.1、关节2故障时的机械臂运动学重构
关节2故障时的角度值为机械臂构型变换前后结合图6所示,此时关节2固定无法进行旋转运动但关节2角度值已知,以基坐标系为参考坐标系,等效六自由度机械臂的初始位姿矩阵ML2为:
根据运动旋量的坐标系变换原则,关节2角度值发生变化后,关节1的螺旋轴不受影响,其余各关节螺旋轴对应的运动旋量表达式为:
固定关节角2后的等效六自由度机械臂正运动学方程表达式为:
将公式(39)中的已知项和ML2整理至同一侧,并联立公式(37)和(38)得到关系式:
4.2、特征位置点“自旋转”运动轨迹
令关节6与关节7的旋转轴线交点并将其选为特征位置点,可以得到下述关系式:
将公式(41)代入公式(40)得到:
根据公式(43)可以认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ1至点变换过程结合图7所示;
4.3、特征位置点坐标求解
令点的位置齐次坐标表示为根据公式(43)可以得到:
因为Mstart -1、及均为已知项,根据公式(44)可以得到点的位置坐标。
令为基坐标系原点,为关节1与关节2轴线的交点,各关节旋转过程中两点位置坐标不变,分别为:
因为点是点绕关节1的螺旋轴旋转一定角度后得到,所以和两点到点和点的距离相等,关节3、4、5的螺旋轴平行,根据几何关系过和两点的直线与关节3的螺旋轴方向向量垂直,故可以得到关系式:
根据公式(38)可以得到关节3的螺旋轴方向向量表达式为:
令点的位置坐标表达式为根据公式(47)的第一和二个方程可以得到表达式为:
将式公式(48)代入公式(47)的第三个方程,可以得到关系式:
进一步化简可以得到:
根据公式(47)的第一个方程可以得到关系式:
所以,根据公式(49)、(51)和(52)可以得到点的位置齐次坐标表达式为:
4.4、关节2故障时的关节1解析解表达式
因为点和两点之间存在关系式:
且点点和点均为已知量,令:
根据Paden-Kahan子问题1,联立公式(55)和公式(56)可以得到关节角1转过的角度值θ1:
根据公式(53)可知,点存在两组解,所以角度值θ1也有两组解;
4.5、关节2故障时关节3、4、5、6、7的约束关系式
根据公式(57)求解得到的角度值θ1以及已知项Mstart -1和代入公式(40)并化简,得到关节3、4、5、6、7之间的关系式为:
步骤五:关节6故障时的求解
根据SSRMS构型机械臂的结构特点,关节2与关节6处于对称位置,因此,关节2与关节6出现故障导致关节角固定无法转动时的逆运动学求解具有一致性,区别在于:
关节6故障时的角度值为选择末端坐标系为参考坐标系,等效六自由度机械臂的初始位姿矩阵ML6为:
各关节螺旋轴的计算公式为:
等效六自由度机械臂的正运动学方程为:
联立可以得到:
选择关节1与关节2的旋转轴线交点为特征位置点计算公式为:
根据位置不变原则和公式(62),可以得到:
公式(64)可以表示点先绕旋转θ3至点Pb L6,然后绕旋转θ4至点Pc L6,再绕旋转θ5至点Pd L6,最后绕旋转θ7至点Pe L6。令点Pe L6的齐次坐标为因为ML6 -1和均为已知项,代入公式(64)可以求得Pe L6的各项表达式。
令关节6与关节7的轴线交点为根据几何关系可知:
令为末端坐标系原点,为关节6与关节7轴线的交点,各关节旋转过程中两点位置坐标不变,分别为:
令点Pd L6的齐次坐标为因为点和均为已知项,通过公式(60)求解并代替和使用点Pe L6和Pd L6分别代替Pe L2和Pd L2,点和分别代替点和代入公式(47)-(53)可以求解得到Pd L6的表达式,因为点Pe L6是点绕旋转一定角度后得到的,所以点Pd L6和Pe L6之间的关系式:
将Pd L6、Pe L6和代入公式(55)可以求解得到和然后代入公式(56)得到和最终得到关节角θ7的求解关系式为:
将关节角θ7的求解结果代入公式(62),可以得到关节角1、2、3、4、5之间的约束关系式为:
步骤六:关节1或2故障时的关节角约束关系式解耦
6.1、关节旋量坐标变换
以基坐标系为参考坐标系时,机械臂关节p(p=1,2)因故障等原因造成角度值θp发生变化前后,关节p与关节m之间(p<m)的矩阵指数存在如下关系:
则公式(24)和公式(58)可以进行下列变换:
将公式(72)分别代入公式(24)和公式(58)可以得到关系式:
由于关节1或2固定后,得到的关节3、4、5、6、7之间的关系表达式具有相同的结构形式,且公式(73)和(74)中的左侧为包含待求关节角θ3、θ4、θ5、θ6、θ7的指数积公式右侧各项均为已知量且构成齐次变换矩阵,因此可以将公式(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的求解表达式,其中θ6有两组解:
当s6=0时,根据c6的取值可以得到关系式:
可进一步求解关节角θ7和θ345,此时为简化求解过程,s6=0时通常取θ7=0。
得到关节角θ6和θ7的角度值后,代入公式(75)并简化得到关系式:
公式(79)中存在关系式:
式中,s34=sin(θ3+θ4),c34=cos(θ3+θ4)。
可以得到:
所以:
根据公式(82)可以得到关节4的角度值θ4的两组求解结果为:
θ4=±acos[(q24 2+q34 2-a3 2-a5 2)/2a3a5] (83)
对公式(81)进一步化简得到:
可以得到:
故关节角θ3的求解结果为:
θ3=atan2[(a3+a5c4)q24-a5s4q34,(a3+a5c4)q34+a5s4q24] (86)
联公立式(77)、(83)和(86)可以得到关节角θ5的求解结果为:
θ5=θ345-θ3-θ4 (87)
通过上述分析过程,关节1或2出现故障时,可以得到其余各关节角的逆运动学解析解,在运动范围[-π,π]内共有八组解;
步骤七:关节6或7故障时的关节角约束关系式解耦
以末端坐标系为参考坐标系时,机械臂关节q(q=6,7)因故障等原因造成角度值θq发生变化前后,关节q与关节n之间(q>n)的矩阵指数存在如下关系:
根据公式(88)对公式(36)和公式(70)中的进行化简如下:
将公式(89)分别代入公式(36)和公式(70)可以得到约束关系式:
因为公式(90)和(91)的左侧均为已知项,且结果为齐次变换矩阵,将其改写为N15,矩阵内部各元素均为已知项:
根据公式(90)和(91)右侧的计算结果可以得到关系式:
当s2≠0时,根据(93)可以得到θ1、θ2和θ345的角度值,其中θ2存在两个解:
当s2=0时,可以得到关系式:
为方便求解,通常取θ1=θ2=0,因此可以得到θ345的角度值为:
得到角度值θ1、θ2以及关节3、4、5的角度和θ345后,整理公式(90)和(91)可以得到关系式:
式中,矩阵N35的各元素为已知项,根据公式(97)可以得到关系式:
整理已知项可以得到:
对公式(99)中的两个等式同时取平方并求和,得到关系式:
a5 2+4a3a5c4+a3 2=j24 2+j34 2 (100)
因此,可以得到θ4的两个值为:
θ4=±arccos[j24 2+j34 2-(a5 2+a3 2)]/4a3a5 (101)
将θ4的角度值代入公式(99)中得到:
因此,可以求解得到关系式:
因为at始终大于0,则公式(103)中的分母项(a5c4+a3)2+(a5s4)2始终大于0,所以,根据公式(103)可以得到θ3的角度值为:
θ3=atan2(s3,c3)=atan2[(a5c4+a3)2j24-a5s4j34,(a5c4+a3)2j34-a5s4j24] (104)
根据公式(94)、(101)和(104)可以得到θ5的角度值为:
θ5=θ345-(θ3+θ4) (105)
根据上述分析可知,关节6或7故障时,关节7或6与关节2和4分别存在两个解,所以根据末端位姿在[-π,π]范围内共可以得到八组解;
步骤八:关节3、4、5故障时的逆运动学求解
根据SSRMS构型机械臂的结构特点,关节3、4、5中某一关节出现故障导致关节角固定无法转动时,等效六自由度机械臂不满足Pieper原则,无法得到其余关节角的逆运动学解析解,才用以下方法进行解析:
8.1、关节旋量坐标变换
给定机械臂的期望位姿临近参考构型θref即当前各关节角度值,关节3、4或5的其中一个角度值θj(j=3,4,5)固定,根据正运动学方程,能够得到当前临近参考构型θref下机械臂末端的实际位姿为:
式中,FKin()表示机械臂的正运动学指数积公式。
以末端坐标系为参考,期望位姿与实际位姿之间存在变换关系eT:
利用矩阵对数log求解eT的运动旋量eV:
式中,eV=[we T ve T]T,[eV]为运动旋量eV的矩阵形式;
8.2、数值解求解精度迭代
如果根据当前临近参考构型θref求解得到的角速度we或速度ve不满足精度条件:
||we||<εw或||ve||<εv (109)
式中,εw和εv分别为给定的角速度和速度的精度阈值。
则通过下列关系式进行更新:
θref+1=θref+J+(θref)eV (110)
式中,J+(θref)=JT(JJT)-1为机械臂雅可比矩阵J的右逆矩阵。
为避免求解结果出现奇异构型导致关节速度突变,在雅可比伪逆矩阵J+(θref)中引入阻尼系数λ,并根据机械臂的奇异程度选择系数大小,J+(θref)表达式为:
J+(θref)=JT(JJT+λ2I)-1 (111)
式中,I为单位矩阵,σd1表示给定的机械臂奇异构型阈值,σmin为雅可比矩阵的最小奇异值,λM表示给定的最大阻尼系数。
重复公式(110)进行更新,该过程中,出现故障的关节角度值θj不发生改变,直至满足公式(109)的精度条件或达到迭代上限次数,则可以得到关节3、4或5出现故障时其余关节角的逆运动学数值解。
对于本领域技术人员而言,显然本发明不限于上述示范性实施例的细节,而且在不背离本发明的精神或基本特征的情况下,能够以其他的装体形式实现本发明。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本发明的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同条件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外,应当理解,虽然本说明书按照实施方式加以描述,但并非每个实施方式仅包含一个独立的技术方案,说明书的这种叙述方式仅仅是为清楚起见,本领域技术人员应当将说明书作为一个整体,各实施例中的技术方案也可以经适当组合,形成本领域技术人员可以理解的其他实施方式。
Claims (1)
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的旋转轴线交点为特征位置点Pa L7,计算公式为:
式中,Pa0 L7=[-a2-a4-a6-a8 0 -a1-a3-a5-a7 1]T为点Pa L7在关节7发生变化前的初始位置齐次坐标;
进而得到关系式:
根据公式(32)认为点Pa L7先绕旋转θ3至点Pb L7,然后绕旋转θ4至点Pc L7,再绕旋转θ5至点Pd L7,最后绕旋转θ6至点Pe L7;
令关节6与关节7的轴线交点为根据几何关系可知:
式中,为点在关节7发生变化前的初始位置齐次坐标;
点Pd L7和Pe L7之间的关系式:
将Pd L7、Pe L7和通过公式(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至点Pb L6,然后绕旋转θ4至点Pc L6,再绕旋转θ5至点Pd L6,最后绕旋转θ7至点Pe L6;
令关节6与关节7的轴线交点为根据几何关系可知:
点Pd L6和Pe L6之间的关系式:
求解得到和最终得到关节角θ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[(q24 2+q34 2-a3 2-a5 2)/2a3a5] (83)
θ3的求解结果为:
θ3=atan2[(a3+a5c4)q24-a5s4q34,(a3+a5c4)q34+a5s4q24] (86)
θ5的求解结果为:
θ5=θ345-θ3-θ4 (87)
步骤七:关节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=atan2(I32,I22) (96)
θ4的求解结果为:
θ4=±arccos[j24 2+j34 2-(a5 2+a3 2)]/4a3a5 (101)
θ3的求解结果为:
θ3=atan2(s3,c3)=atan2[(a5c4+a3)2j24-a5s4j34,(a5c4+a3)2j34-a5s4j24] (104)
θ5的求解结果为:
θ5=θ345-(θ3+θ4) (105)
步骤八:关节3、4、5故障时的逆运动学求解
8.1、关节旋量坐标变换
给定机械臂的期望位姿和临近参考构型θref,关节3、4或5的其中一个角度值固定,根据正运动学方程得到当前临近参考构型θref下机械臂末端的实际位姿为:
以末端坐标系为参考,期望位姿与实际位姿之间存在变换关系eT:
利用矩阵对数log求解eT的运动旋量eV:
式中,eV=[we T ve T]T,[eV]为运动旋量eV的矩阵形式;
8.2、数值解求解精度迭代
如果根据当前临近参考构型θref求解得到的角速度we或速度ve不满足精度条件:
||we||<εw或||ve||<εv (109)
式中,εw和εv分别为给定的角速度和速度的精度阈值;
则通过下列关系式进行更新:
θref+1=θref+J+(θref)eV (110)
式中,J+(θref)=JT(JJT)-1为机械臂雅可比矩阵J的右逆矩阵;
重复公式(110)进行更新,该过程中,出现故障的关节角度值θj不发生改变,直至满足公式(109)的精度条件或达到迭代上限次数,则可以得到关节3、4或5出现故障时其余关节角的逆运动学数值解。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410133265.2A CN117817668B (zh) | 2024-01-30 | 2024-01-30 | 一种ssrms构型机械臂单关节故障的运动学重构方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410133265.2A CN117817668B (zh) | 2024-01-30 | 2024-01-30 | 一种ssrms构型机械臂单关节故障的运动学重构方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117817668A true CN117817668A (zh) | 2024-04-05 |
CN117817668B CN117817668B (zh) | 2024-06-14 |
Family
ID=90517256
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202410133265.2A Active CN117817668B (zh) | 2024-01-30 | 2024-01-30 | 一种ssrms构型机械臂单关节故障的运动学重构方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117817668B (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104647378A (zh) * | 2015-01-21 | 2015-05-27 | 江南大学 | 一种单关节机械臂系统的迭代学习故障诊断方法 |
CN109895101A (zh) * | 2019-04-09 | 2019-06-18 | 大连理工大学 | 一种关节型机械臂逆运动学数值唯一解求取方法 |
CN116587289A (zh) * | 2023-07-17 | 2023-08-15 | 西北工业大学太仓长三角研究院 | 一种七自由度腕关节偏置机械臂逆解方法、系统及介质 |
CN117325143A (zh) * | 2022-06-24 | 2024-01-02 | 沈阳理工大学 | 一种冗余机械臂奇异位型下运动学优化方法 |
-
2024
- 2024-01-30 CN CN202410133265.2A patent/CN117817668B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104647378A (zh) * | 2015-01-21 | 2015-05-27 | 江南大学 | 一种单关节机械臂系统的迭代学习故障诊断方法 |
CN109895101A (zh) * | 2019-04-09 | 2019-06-18 | 大连理工大学 | 一种关节型机械臂逆运动学数值唯一解求取方法 |
CN117325143A (zh) * | 2022-06-24 | 2024-01-02 | 沈阳理工大学 | 一种冗余机械臂奇异位型下运动学优化方法 |
CN116587289A (zh) * | 2023-07-17 | 2023-08-15 | 西北工业大学太仓长三角研究院 | 一种七自由度腕关节偏置机械臂逆解方法、系统及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN117817668B (zh) | 2024-06-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP1728600B1 (en) | Controlling the trajectory of an effector | |
CN102126219A (zh) | 一种冗余度机械臂容错型运动规划方法 | |
Goldenberg et al. | A generalized solution to the inverse kinematics of robotic manipulators | |
CN109176488B (zh) | 一种柔性机器人运动学标定方法及系统 | |
Xu et al. | Dual arm-angle parameterisation and its applications for analytical inverse kinematics of redundant manipulators | |
CN108068113A (zh) | 7-dof仿人臂飞行物体作业最小加速度轨迹优化 | |
CN114589701B (zh) | 一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法 | |
Ting et al. | Kinematic analysis for trajectory planning of open-source 4-DoF robot arm | |
Nguyen et al. | Analysis and implementation of a 6 DOF Stewart Platform-based robotic wrist | |
CN117972925A (zh) | 一种面向冗余空间机械臂的奇异性分析方法 | |
CN117817668B (zh) | 一种ssrms构型机械臂单关节故障的运动学重构方法 | |
Kondo | Inverse kinematics of a human arm | |
CN105856240A (zh) | 一种基于投影几何法的单关节故障机械臂模型重构方法 | |
Tsumaki et al. | Singularity-consistent inverse kinematics of a 6-DOF manipulator with a non-spherical wrist | |
CN118003322B (zh) | 一种基于改进臂角法的ssrms机械臂位置级逆解算法 | |
CN117325143A (zh) | 一种冗余机械臂奇异位型下运动学优化方法 | |
Zhang et al. | Determination of the unique orientation of two bodies connected by a ball‐and‐socket joint from four measured displacements | |
Jazar | Forward Kinematics | |
Patarinsky-Robson et al. | Exploring new strategies for failure recovery of crippled robot manipulators | |
CN117549312B (zh) | 一种面向在轨组装的空间机械臂灵巧性评估方法 | |
Wu et al. | Volumetric error modeling and accuracy improvement by parameter identification of a compound machine tool | |
Tzafestas et al. | A solution to the velocity control of redundant robots | |
Liu et al. | Kinematic calibration of a Gough-Stewart based high-precision pointing mechanism | |
CN118143948B (zh) | 一种七自由度机械臂运动的控制方法、装置及电子设备 | |
He et al. | Self-Motion Optimization Algorithm of Redundant Manipulator Based on Cartesian Space |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |