冗余刚柔耦合机器人正逆运动学建模与避障方法及系统

本发明属于机器人工程与自动控制，更具体地，涉及一种基于模块化迭代的冗余刚柔耦合机器人正逆运动学建模与避障方法及系统。

背景技术：

1、在航空航天与现代电力系统等领域，精密设备内部狭小空间的检修作业面临人工触达难、效率低的瓶颈，“机器代人”已成为打破检修人员与设备数量间“剪刀差”矛盾的必然趋势。然而，现有单一构型机器人难以胜任此类复杂场景：传统刚性机器人构型固定，在受限空间中极易与设备干涉导致位姿不可达；而纯柔性机器人虽具备极佳的空间适应性，但普遍存在刚度低、定位精度差等缺陷，无法满足检修中必需的受力与精密操作要求。因此，现状迫切需要研制一种近端由刚性结构负责大范围高刚度宏观运动、远端由柔性结构实现狭小空间内灵活穿梭的“刚柔耦合”冗余自由度机器人，以有效兼顾承载力、高精度与空间适应性，从而突破复杂设备内部的检修盲区。

2、此外，由于检修场景中狭小空间的形态各异，往往需要针对性地设计不同结构与尺寸的刚柔耦合机器人。若采用传统开发模式，则需为每种特定构型的机器人单独推导正、逆运动学解，极大地增加了系统的研发成本与开发周期。在运动学建模层面，传统的d-h（denavit-hartenberg）参数法难以精确刻画柔性段连续变形的几何特征，无法在统一的坐标系框架下无缝兼容并表达柔性与刚性关节的位姿变换。同时，在狭小空间内，除了末端执行器，整个机械臂本体均面临极高的干涉风险，必须实施严格的整臂全局防碰撞检测。然而，基于d-h方法的运动学模型缺乏一种通用的数学框架来高效、统一地计算不同刚柔机器人机构尺寸的关节位姿与空间雅可比映射，也未能建立针对刚柔混合本体的高效离散点表达机制。这导致在进行机器人本体与复杂障碍物的距离测算时，不仅计算负荷庞大，还极易产生空间检测盲区，严重制约了刚柔耦合机器人在复杂受限环境中的安全与高效应用。

技术实现思路

1、针对现有技术的以上缺陷或改进需求，本发明提供一种基于模块化迭代的冗余刚柔耦合机器人正逆运动学建模与避障方法，用于解决复杂刚柔耦合机器人通用运动学建模困难和传统方法在狭小空间中对机器人整臂干涉检测计算量大、易出现盲区的问题。通过基于poe公式的迭代式正运动学推导框架、针对刚柔混合本体的高效离散点表达机制、冗余自由度和零空间投影算法，最终实现任意尺寸与构型机器人的模块化高效建模，并生成安全可靠的全局避障动作。

2、为了实现上述目的，按照本发明的第一方面，提供一种冗余刚柔耦合机器人正逆运动学建模与避障方法，包括：

3、s100：读取所需推导运动学的机器人关节类型、连杆尺寸以及初始关节角；

4、s200：计算当前机器人每一段“关节-连杆”在全局坐标系中的末端位姿以及连杆离散点位置；

5、s300：计算当前机器人每一段“关节-连杆”与末端运动的直接雅可比映射；

6、s400：提取当前迭代步下机器人末端的全局位姿矩阵，并计算其与目标位姿之间的位置误差与姿态误差，将所述位置误差与姿态误差转化为空间速度旋量，再通过伴随变换计算全局坐标系下的末端期望速度；提取s200中所述连杆离散点受到的非零障碍物排斥力，进而转化为避障期望速度，并构建零空间投影矩阵，将计算得到的所有避障次要任务关节速度进行累加，通过零空间投影矩阵映射后叠加至主任务关节速度中，得到最终的关节控制指令。

7、进一步地，步骤s100包括：

8、s101：针对机器人系统中给定的机器人关节组合，定义以下三组核心参数向量：机器人机构类型代号、机器人每一段的连杆尺寸向量、机器人每一个关节的关节角度向量；

9、s102：根据机器人机械关节结构的装配顺序，依次识别各个关节的运动类型；

10、s103：根据机器人系统的设计图纸，提取连杆长度，并按照关节顺序依次排列；

11、s104：根据机器人的初始位姿状态，为各关节设置初始关节角度。

12、进一步地，步骤s200包括：

13、s201：基于当前的关节信息计算局部位姿变换矩阵，根据当前关节类型的编码，获取该关节对应的关节运动旋量，以及对应的关节角；若当前段位刚性关节，则直接为0；若当前段为柔性关节，则需进一步区分是否为0；

14、s202：生成连杆本体的局部离散点位姿矩阵，对当前每个连杆的几何实体进行空间等距采样，并计算每个采样点在连杆局部坐标系下的位姿；

15、s203：计算当前关节-连杆单元的全局末端位姿，以及全局离散化表示；将局部坐标系下的变换阵左乘上一迭代段的全局初始位姿，映射至全局坐标系中。

16、进一步地，步骤s201中，所述当前段关节的局部位姿变换矩阵表示为：

17、

18、其中，对应为当前关节旋量的反对称矩阵，为关节变量；

19、当时，对应的当前段关节的局部位姿变换矩阵表示为：

20、

21、其中，，l为该柔性段的物理长度。

22、进一步地，步骤s202中，所述针对刚性段连杆，其在零位形下沿纵轴生成的离散点相对位姿矩阵表示为：

23、

24、其中，为当前关节-连杆的第k个采样点至世界坐标原点的累加连杆长度，为3阶单位矩阵；

25、针对柔性段连杆，进行参数插值后，第k个离散点对应的局部变换矩阵表示为：

26、

27、其中，为当前关节旋量的反对称矩阵，为关节变量，为总离散点数。

28、进一步地，步骤s203中，所述针对刚性关节-连杆单元，其全局末端位姿表示为：

29、

30、

31、

32、其中，为中间位姿矩阵（不考虑刚性连杆长度），为前一级连杆的中间位姿矩阵（不考虑刚性连杆长度），为当前段关节的局部位姿变换矩阵，为本段末端位姿变换矩阵（考虑刚性连杆长度），为前一段连杆的末端位姿变换矩阵（考虑刚性连杆长度），为初始位移矩阵，为刚性连杆上第k个采样点的全局末端位姿矩阵；

33、针对柔性关节-连杆单元，考虑柔性段长度，其全局末端位姿表示为：

34、

35、

36、

37、其中，为柔性连杆上第k个采样点的全局末端位姿矩阵。

38、进一步地，步骤s300包括：

39、s301：判断关节类型与初始化运算，系统读取并根据当前段的关节类型标识符，进入不同的雅可比求解分支；

40、s302：根据不同的关节类型，计算对应的关节空间雅可比矩阵；

41、当前段若为刚性关节，则直接提取对应该关节的单位运动旋量作为局部雅可比矩阵，再将局部运动旋量映射至机器人的全局基础坐标系下，从而得到当前刚性关节的空间雅可比矩阵：

42、

43、其中，为当前关节在全局的初始位姿矩阵，为该关节对应的关节运动旋量，伴随变换矩阵表示为：

44、

45、其中，p为位姿变换矩阵t中的位置向量，r为位姿变换矩阵t中的姿态旋转矩阵；

46、随后，记机器人刚性关节-连杆单元有n段，则机器人刚性部分关节的空间雅可比可表示为：

47、

48、若当前段为柔性关节，则计算对应该关节的局部运动旋量对弯曲角和扭转角的偏导数向量与，分别表示为：

49、

50、；

51、所述针对柔性连续体在未弯曲时所产生的运动学模型奇异性，采取分段解析处理策略：

52、当时，系统直接赋予局部雅可比矩阵确定的初值：

53、

54、

55、其中，为连杆对应的长度；

56、当时，系统引入se(3)李代数上的指数映射微分算子，求解其局部雅可比列向量：

57、

58、

59、其中，；

60、随后，将局部雅可比矩阵列向量与映射至全局坐标系，并按列组合，得到对应当前柔性关节（在机器人中排第i个关节）两个自由度的空间雅可比矩阵：

61、

62、随后，记机器人柔性段有m段，则机器人柔性部分关节的空间雅可比可表示为：

63、

64、此时，机器人所有关节的空间雅可比矩阵表示为：

65、

66、s303：将对应关节的空间雅可比矩阵，按照关节顺序和列顺序排列组合，形成完整的机器人整体空间的雅可比矩阵，再将机器人空间雅可比映射转换为直接雅可比矩阵：

67、

68、其中，为转换后的直接雅可比矩阵，为全局姿态旋转矩阵，为机器人末端执行器位姿矩阵，为最终的空间雅可比矩阵。

69、进一步地，步骤s400包括：

70、s401：基于末端目标跟踪与误差计算的主任务建模，计算全局坐标系下的末端期望速度：

71、

72、

73、其中，为当前迭代步下机器人末端的全局位姿矩阵，为跟踪目标的全局位姿矩阵，为全局姿态旋转矩阵，为将位姿偏差转换后的空间速度旋量；

74、随后，采用基于加权伪逆矩阵，计算完成主任务所需的关节速度增量：

75、

76、其中，为末端雅可比矩阵的伪逆，为转换后的直接雅可比矩阵，为矩阵的特征值，为末端期望速度；

77、s402：基于人工势场的本体避障的次要任务建模，在进行主任务计算的同时，系统并行处理防碰撞的次要任务，通过伪逆计算驱动该碰撞点远离障碍物所需的局部关节速度增量：

78、

79、其中，k为避障响应增益系数，为局部平移直接雅可比矩阵的伪逆，为连杆碰撞点对应的局部平移直接雅可比矩阵，为避障期望速度；

80、s403：多任务融合与关节状态更新，若零空间非平凡，则构建零空间投影矩阵：

81、

82、其中，为提取末端雅可比矩阵的零空间正交基；

83、再将所有避障次要任务关节速度进行累加，得到最终的关节控制指令：

84、

85、其中，为完成主任务所需的关节速度增量，为零空间投影矩阵；最后，将总速度增量更新至当前关节角度中，在更新过程中系统会同步进行关节物理极限约束的检测，防止指令超出机械硬件的运行范围；若即将或已经超限，则立即触发约束处理。

86、进一步地，一种冗余刚柔耦合机器人正逆运动学建模与避障系统，其特征在于，包括：

87、数据读取模块，用于读取所需推导运动学的机器人关节类型、连杆尺寸以及初始关节角；

88、计算模块，用于计算当前机器人每一段“关节-连杆”在全局坐标系中的末端位姿以及连杆离散点位置；

89、映射模块，用于计算当前机器人每一段“关节-连杆”与末端运动的直接雅可比映射；

90、以及指令控制模块，用于提取当前迭代步下机器人末端的全局位姿矩阵，并计算其与目标位姿之间的位置误差与姿态误差，将所述位置误差与姿态误差转化为空间速度旋量，再通过伴随变换计算全局坐标系下的末端期望速度；提取所述连杆离散点受到的非零障碍物排斥力，进而转化为避障期望速度，并构建零空间投影矩阵，将计算得到的所有避障次要任务关节速度进行累加，通过零空间投影矩阵映射后叠加至主任务关节速度中，得到最终的关节控制指令。

91、总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

92、1.本发明的方法，通过基于模块化迭代的思想，将伸缩、扭转、俯仰、偏摆等多种单自由度刚性关节与双自由度柔性段，统一纳入类似“拼积木”的标准化工作流程中，进行正向运动学推导与微分运动学分析，从而实现复杂刚柔耦合机器人的通用、高效的运动学建模。该方法不仅实现了对连杆的离散点空间位置生成，还能够统一且高效地计算出复杂机械臂系统的全局雅可比矩阵，从而为后续的逆运动学避障规划与速度控制提供了精确的微分映射模型。

93、2.本发明的方法，通过对机械臂连杆进行空间等距采样，针对复杂零件狭小空间内的作业，实现了高精度的防碰撞检测与主次控制任务的严格物理解耦。同时，该方法充分利用了刚柔耦合机器人的冗余自由度，能够灵敏地感知环境约束，从而提供了可靠的空间感知保障。

94、3.本发明的方法，通过结合人工势场法与零空间投影算法，将避障次要任务的关节速度以正交投影约束至主任务的零空间内，从而使得机械臂本体在实现自主避障的同时，能够完全不影响机器人末端执行器精准跟踪目标位姿的执行精度，进而保障了作业的安全性与任务执行的高效性。