欠自由度机械臂的零空间投影姿态控制方法、装置、设备及机器人与流程

本技术涉及机械臂控制领域，特别涉及一种欠自由度机械臂的零空间投影姿态控制方法、装置、设备及机器人。

背景技术：

1、在工业机器人领域，六自由度串联机械臂是实现末端执行器在三维空间中任意位置和姿态控制的标准构型。而在光伏清洗机器人、液压工程机械等应用场景中，受结构成本、驱动方式等因素限制，机械臂的实际可控关节数往往少于目标位姿的自由度数，例如，具有五个可控关节且末端缺失一个旋转自由度的液压机械臂，此类机械臂即为欠自由度机械臂。对于欠自由度机械臂，相关技术中通常采用仅位置逆解模式进行运动学求解，即放弃姿态约束，仅保证末端位置到达目标位置。

2、然而，在仅位置逆解得到关节角度解之后，末端姿态处于不受控状态。相关技术中，为了对末端姿态进行调整，通常采用线性姿态补偿方式，即将末端姿态偏差直接线性叠加到对应关节的当前角度上。由于姿态补偿所调整的关节同样会影响末端位置，姿态调整会破坏已到达的末端位置，位置控制与姿态调整之间存在耦合干扰，需要多次迭代交替修正位置与姿态，单次位姿求解耗时长，实时性差，无法满足在线实时控制的效率要求。

3、综上，目前欠自由度机械臂的姿态控制存在实时性较低的缺陷。

技术实现思路

1、本技术旨在提出一种欠自由度机械臂的零空间投影姿态控制方法、装置、设备及机器人，能够提高欠自由度机械臂姿态控制的实时性。

2、第一方面，本技术实施例提供一种欠自由度机械臂的零空间投影姿态控制方法，包括以下步骤：

3、获取目标位姿和所述欠自由度机械臂的当前关节状态，所述目标位姿包括目标位置和目标姿态；

4、基于所述目标位置进行仅位置逆运动学求解，得到满足末端位置约束的关节角度基础解；

5、在所述关节角度基础解对应的机械臂构型处，计算末端位置对所述欠自由度机械臂的各可控关节的位置雅可比矩阵，并提取所述位置雅可比矩阵的零空间基矩阵；

6、以所述当前关节状态对应的末端姿态与所述目标姿态之间的旋转偏差为优化目标，将关节角度修正量限制为所述零空间基矩阵的线性组合，在所述零空间基矩阵张成的低维参数空间内搜索使所述旋转偏差最小的目标参数向量；

7、根据所述关节角度基础解、所述零空间基矩阵和所述目标参数向量确定目标关节角度，并基于所述目标关节角度控制所述欠自由度机械臂运动。

8、根据本技术的一些实施例，所述提取所述位置雅可比矩阵的零空间基矩阵，包括：

9、对所述位置雅可比矩阵进行奇异值分解，得到右奇异向量矩阵；

10、将所述右奇异向量矩阵中零奇异值对应的列向量确定为所述零空间基矩阵的列向量，构成所述零空间基矩阵；

11、其中，在所述欠自由度机械臂的可控关节数为五个且所述末端位置约束的约束数为三个的情况下，所述位置雅可比矩阵为三行五列且秩为三，所述零空间基矩阵为五行二列，所述低维参数空间为二维参数空间。

12、根据本技术的一些实施例，所述旋转偏差为姿态误差向量，所述姿态误差向量通过对当前姿态旋转矩阵的转置与目标姿态旋转矩阵的乘积进行旋转矩阵对数映射得到，所述优化目标为所述姿态误差向量的范数平方；

13、所述搜索使所述旋转偏差最小的目标参数向量，包括：在所述低维参数空间内，根据所述优化目标对参数向量的梯度进行迭代搜索，所述梯度根据所述姿态误差向量的转置、姿态雅可比矩阵和所述零空间基矩阵的乘积确定，所述姿态雅可比矩阵用于描述各可控关节角度变化引起的末端姿态变化率。

14、根据本技术的一些实施例，所述方法还包括：

15、获取所述各可控关节的物理限位范围；

16、将所述物理限位范围转换为所述低维参数空间内搜索变量的上下界约束，以使搜索过程中所述各可控关节的关节角度始终处于所述物理限位范围内。

17、根据本技术的一些实施例，所述根据所述关节角度基础解、所述零空间基矩阵和所述目标参数向量确定目标关节角度，包括：

18、根据所述关节角度基础解与所述零空间基矩阵和所述目标参数向量的乘积之和，确定优化关节角度；

19、确定所述优化关节角度对应的末端位置与所述目标位置之间的位置偏差；

20、根据所述位置雅可比矩阵的伪逆矩阵与所述位置偏差，确定位置微调修正量，所述位置微调修正量为消除所述位置偏差的各关节调整量中的最小范数解；

21、根据所述优化关节角度与所述位置微调修正量之和，确定所述目标关节角度。

22、根据本技术的一些实施例，所述欠自由度机械臂包括平行连杆机构，所述平行连杆机构的几何约束关系预先嵌入于正运动学模型中；

23、所述位置雅可比矩阵基于所述正运动学模型在几何空间角度下计算得到，所述搜索在所述几何空间角度下进行；

24、所述基于所述目标关节角度控制所述欠自由度机械臂运动之前，还包括：将几何空间角度下的所述目标关节角度统一转换为执行器空间角度。

25、根据本技术的一些实施例，所述基于所述目标关节角度控制所述欠自由度机械臂运动，包括：

26、以所述当前关节状态为起点、所述目标关节角度为终点，生成满足关节速度限制和关节加速度限制的平滑运动轨迹；

27、将所述平滑运动轨迹下发至所述欠自由度机械臂的运动控制器，以控制所述欠自由度机械臂运动。

28、第二方面，本技术实施例提供了一种欠自由度机械臂的零空间投影姿态控制装置，包括：

29、数据获取模块，用于获取目标位姿和所述欠自由度机械臂的当前关节状态，所述目标位姿包括目标位置和目标姿态；

30、逆解模块，用于基于所述目标位置进行仅位置逆运动学求解，得到满足末端位置约束的关节角度基础解；

31、零空间提取模块，用于在所述关节角度基础解对应的机械臂构型处，计算末端位置对所述欠自由度机械臂的各可控关节的位置雅可比矩阵，并提取所述位置雅可比矩阵的零空间基矩阵；

32、姿态优化模块，用于以所述当前关节状态对应的末端姿态与所述目标姿态之间的旋转偏差为优化目标，将关节角度修正量限制为所述零空间基矩阵的线性组合，在所述零空间基矩阵张成的低维参数空间内搜索使所述旋转偏差最小的目标参数向量；

33、运动控制模块，用于根据所述关节角度基础解、所述零空间基矩阵和所述目标参数向量确定目标关节角度，并基于所述目标关节角度控制所述欠自由度机械臂运动。

34、第三方面，本技术实施例提供了一种电子设备，所述设备包括：处理器以及存储有计算机程序指令的存储器；

35、所述处理器执行所述计算机程序指令时实现如第一方面所述的欠自由度机械臂的零空间投影姿态控制方法。

36、第四方面，本技术实施例提供了一种机器人，包括机器人本体、设置在机器人本体上的机械臂和控制器，所述机械臂为欠自由度机械臂，所述控制器用于运行如第一方面所述的方法。

37、本技术实施例的欠自由度机械臂的零空间投影姿态控制方法、装置、设备及机器人，至少具有如下有益效果：

38、本实施方式中，通过在仅位置逆解得到的关节角度基础解对应的机械臂构型处，提取位置雅可比矩阵的零空间基矩阵，由于沿零空间基矩阵的列向量方向的关节运动在一阶意义上不改变末端位置，将关节角度修正量限制为零空间基矩阵的线性组合，即相当于将姿态调整严格限制在不改变末端位置的关节运动方向上，从而位置精度在姿态优化的全过程中被解析地保持，无需像相关技术那样在位置修正与姿态调整之间多次迭代交替修正，一次求解即可获得位置精确且姿态最优的关节角度；同时，姿态优化的搜索空间由高维关节空间解析地降维为零空间基矩阵张成的低维参数空间，搜索规模大幅减小，单次求解耗时可达到毫秒级。如此，可以提高欠自由度机械臂姿态控制的实时性。

39、本技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。