一种冗余机械臂的多候选逆运动学求解与零空间姿态保持方法、系统及存储介质与流程

本发明涉及机器人运动控制，尤其涉及一种适用于冗余机械臂的逆运动学求解方法、零空间姿态保持方法、关节限位保护方法以及候选关节构型仲裁方法。

背景技术：

1、冗余机械臂是指其关节自由度数量大于完成末端位姿任务所需自由度数量的机械臂。由于冗余自由度的存在，机械臂在满足末端位置和姿态约束的同时，还可以进一步调整肘部位置、腕部姿态、关节远离限位程度或整臂构型。因此，冗余机械臂在复杂抓取、狭窄空间操作、避障、遥操作和人形机器人控制中具有较高价值。

2、然而，冗余自由度也使逆运动学问题不再具有唯一解。对于同一末端目标位姿，机械臂可能存在多组可行关节解。若控制系统仅根据末端误差进行求解或选择，容易出现臂型突变、关节跳变、局部构型不自然、个别关节接近限位、耦合关节相互挤压运动空间等问题。

3、现有技术中，通常采用单一初值进行逆运动学迭代，例如以上一控制周期的关节角作为当前周期的初始值。该方式具有较好的实时性，但在目标位姿快速变化、机械臂接近奇异位形、关节接近限位或冗余构型需要重新分配时，容易陷入局部解，导致机械臂无法获得理想的关节构型。

4、为解决上述问题，一些方法会在求解结果上施加关节限位截断，但这种方式往往在关节触及限位后才采取行动，容易造成速度突变和机械冲击。另一些方法在优化目标中加入统一的姿态回归项，引导关节朝参考姿态收敛，但这类方法通常对所有关节施加相同的回归强度，缺少对不同关节功能角色的差异化处理。此外，当系统并行求解多个候选解时，现有技术缺少对候选解之间稳定切换的有效机制，容易出现候选分支的频繁跳变，影响控制输出的平滑性。

5、综上，现有冗余机械臂逆运动学控制方法在求解鲁棒性、耦合关节协调性、姿态保持适应性和候选切换稳定性等方面仍存在明显不足，需要一种能够兼顾末端跟踪精度、构型连续性、关节限位安全性和实时控制稳定性的综合解决方案。

技术实现思路

1、针对现有冗余机械臂逆运动学控制方法中单一初值求解易陷入局部构型、耦合关节近限位逃逸困难、统一零空间姿态保持适应性不足、关节近限位保护不连续以及多个候选解之间切换不稳定等技术问题，本发明提供一种冗余机械臂的多候选逆运动学求解与零空间姿态保持方法、系统及存储介质。

2、其中，采用在每个控制周期内构造多个具有不同构型倾向的逆运动学候选种子，分别求解后通过综合评价函数对各候选解进行评分，再结合历史状态执行有状态仲裁，最后对被选中的候选解执行近限位渐进输出保护，使冗余机械臂在保持末端位姿精度的同时获得更稳定、更安全、更自然的关节构型。

3、为实现上述目的，本发明提供如下技术方案。

4、一种冗余机械臂的多候选逆运动学求解与零空间姿态保持方法，包括以下步骤：

5、步骤一：获取机械臂末端目标位姿、上一控制周期输出关节状态、当前关节状态以及关节限位信息。

6、步骤二：根据所述关节状态和限位信息构造多个逆运动学候选种子。

7、步骤三：对每个候选种子分别执行逆运动学求解，得到多个候选关节解。

8、步骤四：根据末端误差、关节连续性、参考姿态偏离、限位风险、耦合关节风险和候选切换代价，对候选关节解进行综合评价。

9、步骤五：依据候选评价结果和历史分支状态，执行有状态候选仲裁，确定目标候选关节解。

10、步骤六：根据各关节限位裕度对目标候选关节解执行近限位渐进输出约束。

11、步骤七：输出最终机械臂关节指令。

12、下面对上述各步骤的具体实现方式进行详细说明。

13、关于候选种子的构造：

14、为避免单一初值导致的局部收敛，本发明在每个控制周期内构造多个候选种子。所述候选种子包括但不限于以下类型：

15、（1）连续性种子：以上一控制周期输出的关节状态作为初值，用于保持运动连续性；

16、（2）参考姿态种子：在上一关节状态基础上加入朝参考姿态回归的偏置，用于保持自然构型；

17、（3）限位逃逸种子：当一个或多个关节接近限位时，加入远离限位方向的偏置；

18、（4）耦合关节协调种子：当存在运动学或控制上相互影响的关节组时，对该关节组施加协调偏置；

19、（5）历史稳定种子：采用历史上被判定为稳定的候选构型作为初值；

20、（6）任务偏好种子：根据操作任务、避障需求或末端姿态偏好构造特定候选初值。

21、上述候选种子的数量、类型和偏置强度可根据控制周期、算力预算、机械臂结构和任务需求动态调整。本发明不限定候选种子的具体数量，也不限定具体偏置公式。

22、设冗余机械臂具有 个可控关节，当前控制周期为，上一周期输出关节向量为，当前测得关节向量为，参考姿态或舒适姿态为，关节下限和上限分别为和。候选种子集合表示为：;

23、其中为候选种子数量，可为固定值，也可根据算力预算、目标变化幅度或限位风险动态调整。每一候选种子统一表示为:

24、其中为参考姿态回归偏置，为限位逃逸偏置，为耦合关节协调偏置，

25、为任务偏好偏置；，，，，为第个候选种子对应的组合系数。不同候选种子通过组合系数的差异化配置，可覆盖连续性、回归性、限位规避性、耦合协调性和任务偏好性等不同构型倾向。

26、参考姿态回归偏置写为：

27、

28、其中表示对向量按幅值阈值进行限幅。关节限位裕度定义为：

29、

30、限位风险激活系数定义为：

31、其中 为软保护边界，为强保护边界。限位逃逸偏置表示为：

32、

33、其中表示远离最近限制边界并趋向关节安全区的方向，为第个关节的限位逃逸尺度。

34、关于耦合关节逃逸机制：

35、对于冗余机械臂中存在相互耦合关系的关节组，本发明设置有耦合关节逃逸机制。所述耦合关节组可以是前臂旋转关节与腕部旋转关节，也可以是肩部、肘部、腕部中存在运动空间相互影响的任意两个或多个关节。

36、当耦合关节组中的某一关节接近物理限位、软限位、速度限位或任务约束边界时，系统在候选种子中对该关节施加远离限制边界的偏置，同时对耦合关节组中的其他相关关节施加朝参考姿态、舒适区间或可用空间较大区域回归的偏置。该偏置用于引导逆运动学求解器进入更安全的局部解空间，而不是直接作为最终输出。

37、设存在一个或多个耦合关节组。令为从全关节向量中选取第个耦合关节组的选择矩阵，则该组耦合关节向量为：

38、

39、第个耦合关节组的风险函数表示为：

40、

41、其中为关节近限位激活系数，为耦合组姿态偏离权重为加权范数。耦合关节协调偏置表示为：

42、

43、其中为第个耦合关节组的回归矩阵，为根据限位方向生成的逃逸项，为该耦合组偏置限幅。逃逸项进一步表示为：

44、

45、其中，为耦合关节逃逸尺度。

46、该表达不限定耦合关节组的具体关节编号和数量，适用于任意具有运动学耦合、限位耦合或任务耦合关系的冗余机械臂。通过该机制，本发明能够在关节触及限位之前主动调整耦合关节组的构型，降低后续控制周期内出现硬限位截断、腕部扭转或冗余空间耗尽的风险。

47、关于逆运动学求解：

48、各候选种子分别输入逆运动学求解模块。逆运动学求解模块可采用任意适合实时控制的数据求解方法，包括基于雅可比伪逆的迭代求解、基于阻尼最小二乘的求解、基于二次规划的约束求解、基于非线性优化的求解、以及基于模型预测或递推优化的关节增量求解。

49、在每个候选分支中，求解目标至少包括末端位姿误差项，也可进一步包括姿态保持项、关节限位项、速度约束项、加速度约束项、自碰撞约束项、避障约束项或任务偏好项。

50、设机械臂正运动学映射为，其中为末端位置，为末端姿态。设目标位姿为末端任务误差表示为：

51、

52、其中为旋转群到旋转向量的对数映射。对于基于关节增量 的迭代求解，采用如下统一形式：

53、

54、约束条件包括：

55、

56、其中为末端雅可比矩阵，为末端误差权重矩阵，为姿态保持权重矩阵，为平滑权重矩阵，

57、为限位势函数，为避障、自碰撞、任务偏好或其他附加代价。

58、为单步最大增量，

59、为单步二阶关节增量上限；若采用物理加速度上限，则应满足δ²qmax,j=amax,jδt²。

60、限位势函数采用分段连续形式：

61、

62、其中为第个关节的限位风险权重，为强保护区惩罚系数。该势函数在关节进入软保护区后逐渐增大，使限位风险在越界之前即被纳入优化考量。

63、关于差异化零空间姿态保持：

64、本发明在零空间姿态保持中采用差异化策略，而非对所有关节施加相同的回归强度。系统根据各关节的机械功能、运动范围、限位风险、任务贡献或与其他关节的耦合关系，为不同关节设置不同的姿态保持系数。

65、对于第个关节，其参考姿态表示为：

66、

67、其中为第个关节的姿态保持参考值，为候选种子中的关节值，为参考姿态或舒适姿态，为与第个关节对应的差异化映射函数。该映射函数可为固定比例函数、限幅函数、非线性函数或随任务状态变化的自适应函数。

68、在一种实施方式中，根据以下原则配置各关节姿态保持强度：对决定末端大范围可达性的近端关节设置较弱姿态保持以保留操作灵活性；对决定整臂构型稳定性的中间关节设置中等姿态保持以抑制臂型突变；对易产生缠绕、饱和或耦合风险的远端关节设置较强姿态保持以提高构型稳定性；对任务敏感关节则根据末端目标变化、障碍物位置或操作阶段动态调整姿态保持强度。

69、从冗余机械臂零空间角度，差异化姿态保持可写为：

70、

71、其中为雅可比广义逆或加权广义逆，为零空间投影矩阵，为用于姿态保持、限位规避或耦合协调的次级关节增量。差异化零空间增量为：

72、

73、其中为逐关节姿态保持增益矩阵，为可操作度、舒适度或其他构型质量指标，为限位风险势函数，和分别为构型质量和限位规避权重矩阵。和分别为构型质量指标和限位势函数的梯度。由于，，均可设置为对角矩阵、分块矩阵或状态相关矩阵，因此能够对不同关节施加不同的零空间作用强度。

74、构型质量指标可选用如下形式之一：

75、

76、其中表示可操作度，表示参考姿态舒适度，表示关节限位裕度质量，防止数值奇异的小常数。上述指标可单独使用，也可加权组合使用。

77、关于候选解综合评价：

78、多个候选关节解生成后，本发明通过综合评价函数选择更适合当前控制状态的候选解。所述评价函数包括以下一项或多项：末端位置误差、末端姿态误差、相对上一输出关节状态的连续性代价、相对参考姿态或舒适姿态的偏离代价、关节限位风险、耦合关节风险、关节速度或加速度风险、自碰撞或外部碰撞风险、候选分支切换代价、与任务偏好相关的代价。

79、综合评价函数表示为：

80、

81、其中为第个评价项为对应权重，为评价项数量。权重为固定值，或根据任务阶段、关节限位裕度、目标速度、候选历史状态或机械臂负载状态自适应调整。

82、候选解的综合评价函数展开为：

83、

84、其中和为历史输出关节状态，为控制周期， 为耦合关节风险函数，

85、为候选分支切换指示函数， 为上一周期选中的候选分支。和分别为候选解的关节速度和加速度。该评分函数同时考虑任务精度、连续性、参考姿态、限位安全、速度平滑、加速度平滑、耦合风险和切换成本。

86、候选解的权重也可自适应调整。例如限位权重随最小限位裕度减小而增大：

87、其中为限位权重基准值，为限位权重调节系数。

88、连续性权重随目标速度降低而增大：

89、

90、其中为目标末端速度或目标变化率，为连续性权重基准值，和为调节参数。通过自适应权重，系统可在高速运动时优先跟踪目标，在低速或静止状态下优先保持稳定构型。

91、关于有状态候选仲裁：

92、为避免多个候选解之间频繁切换，本发明引入有状态候选仲裁机制，结合历史状态进行判断而非逐帧无记忆地选择最低评分候选。

93、有状态候选仲裁包括：（1）最小保持时间——候选分支被选中后在预设时间或帧数内优先保持；（2）等效候选判定——当多个候选解的评分、末端误差和限位风险差异均小于阈值时优先保持当前候选；（3）滞回切换阈值——仅当新候选显著优于当前候选时才允许切换；（4）关节方向翻转惩罚——对容易引起冗余构型摆动的关节方向反复变化施加惩罚；（5）目标静止守卫——当末端目标静止或低速变化时提高对关节微小跳变的惩罚；（6）候选失效检测——当当前候选出现不可接受的末端误差、限位风险或约束违反时允许立即切换。

94、有状态候选仲裁形式化表示为：设第个候选解的原始评分为，当前历史分支，保持计数为修正评分定义为：

95、

96、其中为切换滞回惩罚，为关节方向翻转惩罚，为目标静止守卫惩罚。方向翻转惩罚表示为：

97、

98、其中为方向翻转惩罚权重，为第个关节的方向翻转敏感度，

99、

100、

101、目标静止守卫惩罚表示为：

102、

103、其中为静止守卫惩罚权重，为静止守卫系数，表示末端目标状态， 为尺度参数，越接近1表示目标越接近静止。

104、最终候选分支由下式确定：

105、

106、其中为最终选中的候选分支索引。

107、在最小保持时间内若当前候选未失效，则强制保持：

108、

109、其中为最小保持帧数，为候选有效性判定函数。

110、候选等效集合定义为：

111、

112、其中为等效候选集合，、、分别为评分、末端误差和限位风险的等效阈值。

113、若新候选属于等效集合，则优先保持历史候选。

114、关于近限位渐进输出保护：

115、为避免关节接近限位时发生硬截断，本发明根据关节到限制边界的距离设置渐进输出预算。所述限制边界可以是物理位置限位、软限位、速度限位、加速度限位、力矩限位或任务定义的安全边界。

116、系统将关节状态划分为安全区、渐进保护区和强保护区：在安全区内允许关节按常规速度或增量预算运动；在渐进保护区内随关节接近限制边界逐渐降低单周期允许增量；在强保护区内限制关节继续朝危险方向运动，并保留朝安全方向退出的能力。

117、抽象表示为：

118、其中为关节限位裕度，direction为候选运动方向，state为关节历史状态或任务状态，为渐进保护函数，该函数可以是线性分段函数、非线性连续函数或查表函数。

119、最终输出满足：

120、

121、其中为候选输出关节状态，为限幅函数。

122、该机制使关节在接近限制边界时平滑减速，避免硬截断造成速度突变和机械冲击。

123、在分段连续实现中，第个关节的限位裕度为，基础单周期增量为δ，最小安全退出增量为，允许增量定义为：

124、

125、为区分朝危险方向和朝安全方向的运动，定义方向相关预算：

126、

127、其中为候选输出方向，为安全方向增益。当m_j(t)≤m_h且u_j(t)指向危险侧时，令δq_safe,j(t)=0，以禁止关节继续朝危险方向运动。最终输出为：

128、

129、其中为第个关节的最终输出关节角。

130、若同时考虑速度和加速度约束，还可满足：

131、

132、其中和随限位裕度减小而降低。

133、关于系统实现：

134、本发明还提供一种冗余机械臂的多候选逆运动学求解与零空间姿态保持系统，包括：

135、目标输入模块，用于接收或生成机械臂末端目标位姿，并进行坐标转换、目标限幅和目标状态判定；

136、机械臂模型模块，用于获取当前关节状态，提供机械臂正运动学、雅可比矩阵、关节限位、参考姿态和关节拓扑信息，并计算关节限位裕度；

137、候选种子生成模块，用于根据上一周期输出、参考姿态、限位裕度和耦合关节状态生成多个逆运动学初值；

138、逆运动学求解模块，用于基于数值优化、二次规划、迭代雅可比或其他求解方式生成候选关节解；

139、零空间姿态保持模块，用于对冗余自由度施加差异化姿态保持约束或偏置；

140、候选解评价模块，用于对候选解的精度、连续性、安全性和自然性进行综合评分；

141、有状态仲裁模块，用于根据历史候选状态、评分滞回、保持帧数和静止守卫选择目标候选解；

142、输出保护模块，用于对目标候选解执行速度、加速度、增量或近限位渐进约束。

143、一个控制周期内的整体计算过程概括为：

144、输入：

145、其中x_d(t)为末端目标状态。

146、限位裕度及激活系数：

147、候选种子：

148、逆运动学求解：其中表示逆运动学求解函数。

149、综合评分：

150、其中为时变权重，为第 个评价项。

151、有状态仲裁：

152、其中表示有状态仲裁函数。

153、输出保护：

154、其中表示输出保护函数。

155、上述数学模型仅限定各模块之间的函数关系和信息流，不限定具体求解器、具体机器人型号、具体关节编号、具体参数取值或具体通信实现。

156、本发明还提供一种冗余机械臂控制系统，包括处理器、存储器和机械臂控制接口，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时实现上述方法。

157、本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法。

158、与现有单一初值逆运动学方法相比，本发明具有以下有益效果：

159、（1）本发明在同一控制周期内构造多个具有不同构型倾向的逆运动学候选种子，扩大了局部解搜索范围，降低了单一初值陷入不利构型的概率，使机械臂在目标位姿快速变化、接近奇异位形或关节接近限位时仍能获得可行且理想的关节构型，提高求解鲁棒性。

160、（2）通过限位逃逸种子和近限位渐进输出保护机制，在关节触及限位之前主动调整构型，并在接近限位时连续减速，有效降低了关节触及限位或硬截断的风险，提高关节安全性。

161、（3）针对存在运动耦合或安全相关性的关节组，在候选种子层面生成远离限位和回归参考姿态的协调偏置，能够主动释放耦合关节的运动空间，避免耦合关节相互挤压导致可用空间耗尽，改善耦合关节协调性。

162、（4）差异化零空间姿态保持策略根据不同关节的功能角色、风险程度和任务贡献设置不同的姿态保持强度，使机械臂在保持操作灵活性的同时维持更合理的臂型，保持自然构型。

163、（5）有状态候选仲裁机制通过最小保持时间、等效判定、滞回切换、方向翻转惩罚和静止守卫，有效减少了多个候选解之间的频繁切换，提高了实时控制输出的稳定性。

164、（6）候选数量和求解步数可根据算力预算动态配置，能够适配不同实时控制系统的计算资源要求，同时不依赖于特定机器人型号、求解器框架或通信接口，具有广泛的适用性。