欠自由度机械臂的位置约束姿态优化控制方法、装置、设备及机器人与流程

本技术涉及机械臂控制领域，特别涉及一种欠自由度机械臂的位置约束姿态优化控制方法、装置、设备及机器人。

背景技术：

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