一种SSRMS构型机械臂单关节故障的运动学重构方法

本发明涉及机械臂运动能力分析，具体是一种ssrms构型机械臂单关节故障的运动学重构方法。

背景技术：

1、ssrms构型机械臂是与国际空间站遥操作系统(ssrms：space station remotemanipulator system)关节数量一致、结构相同的空间机械臂的统称，由于该构型机械臂具有一个冗余自由度，且整体结构对称分布，在避关节限位、力矩优化、关节避障等方面具有灵活的操控能力，在空间在轨操作任务中具有广阔的应用范围，如国际空间站的canadarm2、era(european robotic arm)以及中国空间站的实验舱机械臂、核心舱机械臂均属于此构型机械臂。冗余自由度的存在，不但增加了机械臂的灵活性，而且在执行预期任务时，对于给定的末端位姿，ssrms构型机械臂有无数种操作构型，即存在无数组关节角逆解。

2、但受极端复杂的太空环境等在轨工作条件的影响，机械臂在轨执行任务过程中可能会出现关节故障等突发情况导致某一关节停止运动，此时七自由度冗余机械臂等效为六自由度机械臂，则不能按照预期规划的路径轨迹正常运动或工作，无法完成预期任务甚至对机械臂自身和周围设备造成损伤。何川甫等人(何川甫，等。一种多自由度机械臂的逆运动学通用求解方法及装置[p]，2022)虽然提出一种运动学求解方法，但其未考虑冗余机械臂存在多组逆解导致迭代过程不收敛、迭代次数达到上限等情况，并且，由于ssrms构型机械臂的肩、肘、腕关节均存在偏置等构型特征，不同关节出现故障时，等效六自由度机械臂的结构特点不完全一致，进一步增加了采用同一种运动学重构方法控制机械臂运动的难度。

3、因此，针对ssrms构型机械臂出现单关节故障无法转动等情况，亟需考虑不同关节故障情况下的运动学重构解决方案，根据实时情况重新计算规划路径轨迹或实现动作控制，使ssrms构型机械臂在故障状态下仍能继续执行任务，以最大程度地减少故障对机械臂操作的影响，使其顺利完成在轨工作任务。

技术实现思路

1、为解决背景技术存在的不足，本发明提供一种ssrms构型机械臂单关节故障的运动学重构方法，它基于旋量理论建立机械臂的关节旋量坐标，利用结构对称的特点分类讨论不同关节故障时的运动学重构方案，保障机械臂在单关节故障的情况下也能够完成任务。

2、为实现上述目的，本发明采取下述技术方案：一种ssrms构型机械臂单关节故障的运动学重构方法，包括以下步骤：

3、步骤一：建立ssrms构型机械臂的关节运动旋量坐标

4、以{xbybzb}和{xeyeze}分别表示机械臂的基坐标系和末端坐标系，以qi{i＝1,…,7}表示位于各关节螺旋轴上的点，以si表示以基坐标系为参考建立的各关节螺旋轴，以at{t＝0,1,…,8}表示臂杆长度，从而建立ssrms构型机械臂的关节运动旋量坐标，各关节螺旋轴的运动旋量坐标si＝(wi,vi)，其中，wi为各关节螺旋轴si的单位方向向量，vi＝-wi×qi为与基坐标系原点重合的点相对各关节螺旋轴si的线速度；

5、各关节旋转角度θi后，各关节的矩阵指数为则以基坐标系为参考坐标系的机械臂正运动学指数积表达式ttarget为：

6、

7、式中，mstart为末端坐标系相对基坐标系的初始齐次坐标矩阵bte或基坐标系相对末端坐标系的初始齐次坐标矩阵etb，bte与etb相等；

8、步骤二：关节1故障时的求解

9、2.1、关节1故障时的机械臂运动学重构

10、关节1故障时的角度值为以基坐标系为参考坐标系，等效六自由度机械臂的初始位姿矩阵ml1为：

11、

12、对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为：

13、

14、式中，为关节螺旋轴s1转动角度后的矩阵指数，为给定的末端位姿矩阵，为变换后的各关节螺旋轴转动角度θi后的矩阵指数；

15、2.2、特征位置点“自旋转”运动轨迹

16、令关节6与关节7的旋转轴线交点为并将其选为特征位置点，在基坐标系中的位置齐次坐标表示为得到下述关系式：

17、

18、进而得到关系式：

19、

20、根据公式(9)认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ2至点

21、2.3、特征位置点坐标求解

22、令点的位置齐次坐标表示为代入公式(9)得到关系式：

23、

24、通过求解点的位置坐标已知，令为关节1与关节2旋转轴线的交点，点到的距离与到的距离相等，故得到等式：

25、

26、式中，为关节3旋转轴线的方向向量，为关节2旋转轴线的方向向量；

27、能够得到点的两组位置坐标表达式为：

28、

29、式中，对三角函数表达式进行简化为pdy0通过下式计算：

30、

31、2.4、关节1故障时的关节2解析解表达式

32、和两点之间存在关系式：

33、

34、根据paden-kahan子问题1能够得到关节角2转过的角度值θ2：

35、

36、式中，

37、

38、

39、点存在两组解，角度值θ2也有两组解；

40、2.5、关节1故障时关节3、4、5、6和7的约束关系式

41、由于角度值θ2以及和均已知，代入公式(5)并化简得到关节3、4、5、6和7之间的关系式为：

42、

43、步骤三：关节7故障时的求解

44、根据ssrms构型机械臂的结构特点，关节1与关节7处于对称位置，因此，关节7出现故障导致关节角固定无法转动时的逆运动学求解与关节1具有一致性，区别在于：

45、选择末端坐标系为参考坐标系，各关节螺旋轴相对末端坐标系的旋量坐标替换为bi，关节7故障时的角度值为等效六自由度机械臂的初始位姿矩阵ml7为：

46、

47、对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为：

48、

49、选择关节1与关节2的旋转轴线交点为特征位置点pal7,计算公式为：

50、

51、式中，pa0l7＝[-a2-a4-a6-a8 0 -a1-a3-a5-a7 1]t为点pal7在关节7发生变化前的初始位置齐次坐标；

52、进而得到关系式：

53、

54、根据公式(32)认为点pal7先绕旋转θ3至点pbl7，然后绕旋转θ4至点pcl7，再绕旋转θ5至点pdl7，最后绕旋转θ6至点pel7；

55、令关节6与关节7的轴线交点为根据几何关系可知：

56、

57、式中，为点在关节7发生变化前的初始位置齐次坐标；

58、点pdl7和pel7之间的关系式：

59、

60、将pdl7、pel7和通过公式(21)求解得到和然后代入公式(22)得到和最终得到关节角θ6的求解关系式为：

61、

62、关节角1、2、3、4和5之间的约束关系式为：

63、

64、步骤四：关节2故障时的求解

65、4.1、关节2故障时的机械臂运动学重构

66、关节2故障时的角度值为以基坐标系为参考坐标系，等效六自由度机械臂的初始位姿矩阵ml2为：

67、

68、对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为：

69、

70、4.2、特征位置点“自旋转”运动轨迹

71、令关节6与关节7的旋转轴线交点并将其选为特征位置点，可以得到下述关系式：

72、

73、进而得到关系式：

74、

75、根据公式(43)认为先绕旋转θ5至点然后绕旋转θ4至点再绕旋转θ3至点最后绕旋转θ1至点

76、4.3、特征位置点坐标求解

77、令点的位置齐次坐标表示为根据公式(43)可以得到：

78、

79、因为和两点到点和点的距离相等，关节3、4和5的螺旋轴平行，过和两点的直线与关节3的螺旋轴方向向量垂直，故得到关系式：

80、

81、式中，为基坐标系原点，为关节1与关节2轴线的交点；

82、令点的位置坐标表达式为能够得到点的位置齐次坐标表达式为：

83、

84、4.4、关节2故障时的关节1解析解表达式

85、点和两点之间存在关系式：

86、

87、根据paden-kahan子问题1能够得到关节角1转过的角度值θ1：

88、

89、点存在两组解，所以角度值θ1也有两组解；

90、4.5、关节2故障时关节3、4、5、6和7的约束关系式

91、由于角度值θ1以及mstart-1和均已知，代入公式(40)并化简得到关节3、4、5、6和7之间的关系式为：

92、

93、步骤五：关节6故障时的求解

94、根据ssrms构型机械臂的结构特点，关节2与关节6处于对称位置，因此，关节2与关节6出现故障导致关节角固定无法转动时的逆运动学求解具有一致性，区别在于：

95、选择末端坐标系为参考坐标系，各关节螺旋轴相对末端坐标系的旋量坐标替换为bi，关节6故障时的角度值为等效六自由度机械臂的初始位姿矩阵ml6为：

96、

97、对机械臂各关节螺旋轴对应的运动旋量进行变换，等效六自由度机械臂的正运动学方程表示为：

98、

99、选择关节1与关节2的旋转轴线交点为特征位置点计算公式为：

100、

101、进而得到关系式：

102、

103、根据公式(64)认为点先绕旋转θ3至点然后绕旋转θ4至点pcl6,再绕旋转θ5至点pdl6,最后绕旋转θ7至点pel6；

104、令关节6与关节7的轴线交点为根据几何关系可知：

105、

106、点pdl6和pel6之间的关系式：

107、

108、求解得到和最终得到关节角θ7的求解关系式为：

109、

110、关节角1、2、3、4和5之间的约束关系式为：

111、

112、步骤六：关节1或2故障时的关节角约束关系式解耦

113、6.1、关节旋量坐标变换

114、以基坐标系为参考坐标系时，关节p＝1,2角度值θp发生变化前后，关节p与关节m之间的矩阵指数存在如下关系，p＜m：

115、

116、进而得到关系式：

117、

118、

119、将公式(73)和(74)右侧统一改写为矩阵m37且内部各元素已知：

120、

121、6.2、关节3、4、5、6、7解析解表达式求解

122、因为关节3、4、5的螺旋轴均绕基坐标系的x轴旋转，根据公式(75)可以得到：

123、

124、式中，c345＝cos(θ3+θ4+θ5)，s345＝sin(θ3+θ4+θ5)；

125、当s6≠0时，根据公式(76)得到θ6、θ7和θ345的求解表达式：

126、

127、当s6＝0时，根据c6的取值得到关系式：

128、

129、进一步求解关节角θ7和θ345，为简化求解过程，s6＝0时取θ7＝0；

130、θ4的求解结果为：

131、θ4＝±acos[(q242+q342-a32-a52)/2a3a5]           (83)

132、θ3的求解结果为：

133、θ3＝atan2[(a3+a5c4)q24-a5s4q34，(a3+a5c4)q34+a5s4q24]    (86)

134、θ5的求解结果为：

135、θ5＝θ345-θ3-θ4                   (87)

136、步骤七：关节6或7故障时的关节角约束关系式解耦

137、以末端坐标系为参考坐标系时，关节q＝6,7角度值θq发生变化前后，关节q与关节n之间的矩阵指数存在如下关系，q＞n：

138、

139、进而得到关系式：

140、

141、

142、改写为矩阵n15且内部各元素已知：

143、

144、根据公式(90)和(91)右侧的计算结果可以得到关系式：

145、

146、当s2≠0时，根据(93)得到θ1、θ2和θ345的求解表达式：

147、

148、当s2＝0时，得到关系式：

149、

150、取θ1＝θ2＝0，因此可以得到θ345的角度值为：

151、θ345＝atan2(i32,i22)                       (96)

152、θ4的求解结果为：

153、θ4＝±arccos[j242+j342-(a52+a32)]/4a3a5             (101)

154、θ3的求解结果为：

155、θ3＝atan2(s3,c3)＝atan2[(a5c4+a3)2j24-a5s4j34,(a5c4+a3)2j34-a5s4j24]    (104)

156、θ5的求解结果为：

157、θ5＝θ345-(θ3+θ4)                       (105)

158、步骤八：关节3、4、5故障时的逆运动学求解

159、8.1、关节旋量坐标变换

160、给定机械臂的期望位姿和临近参考构型θref，关节3、4或5的其中一个角度值固定，根据正运动学方程得到当前临近参考构型θref下机械臂末端的实际位姿为：

161、

162、以末端坐标系为参考，期望位姿与实际位姿之间存在变换关系et：

163、

164、利用矩阵对数log求解et的运动旋量ev：

165、

166、式中，ev＝[wet vet]t，[ev]为运动旋量ev的矩阵形式；

167、8.2、数值解求解精度迭代

168、如果根据当前临近参考构型θref求解得到的角速度we或速度ve不满足精度条件：

169、||we||＜εw或||ve||＜εv                       (109)

170、式中，εw和εv分别为给定的角速度和速度的精度阈值；

171、则通过下列关系式进行更新：

172、θref+1＝θref+j+(θref)ev                  (110)

173、式中，j+(θref)＝jt(jjt)-1为机械臂雅可比矩阵j的右逆矩阵；

174、重复公式(110)进行更新，该过程中，出现故障的关节角度值θj不发生改变，直至满足公式(109)的精度条件或达到迭代上限次数，则可以得到关节3、4或5出现故障时其余关节角的逆运动学数值解。

175、与现有技术相比，本发明的有益效果是：本发明基于旋量理论建立机械臂的关节旋量坐标，利用结构对称的特点分类讨论不同关节故障时的运动学重构方案，关节1、2、6、7故障时，重构运动学方程，通过选取等效机械臂的特征位置点并构建其“自旋转”运动，将逆运动学问题转化为已知的paden-kahan子问题求解，关节3、4、5故障时，根据等效六自由度机械臂不满足pieper原则无法求得解析解的特点，利用关节运动旋量变换通过迭代法求各关节数值解，提高了机械臂出现单关节故障时的容错性，快速地找到新的适当路径或解决方案，使机械臂尽快恢复正常运行以继续执行任务，最大程度地减少故障对机械臂整体操作的影响。