基于几何约束和自由度限定的位姿求解方法、系统、设备及介质

本发明属于机器人，具体涉及一种基于几何约束和自由度限定的位姿求解方法、系统、设备及介质。

背景技术：

1、在机器人工程应用中常常需要进行位姿求解计算，如在物流分拣应用中需要识别并计算零件的位姿以进行抓取分拣、在自动驾驶应用中需要根据采集的环境数据确定自身的位姿以实现定位及导航、在加工应用中确定工件位姿以进行自动化加工。

2、基于视觉的工件定位是在加工之前通过视觉传感器确定待加工工件的三维位姿，从而生成加工轨迹，进一步完成工件加工。工件定位通常基于六点定位原理进行，需要分步确定工件在不同自由度上的位姿并进行自由度限定，最终确定工件的位姿。现有的工件位姿求解都是基于视觉完成的，较为常见的位姿求解方法可以分为三类：对极几何估计位姿、pnp估计位姿与icp方法估计位姿。对极几何约束方法求解位姿是针对连续两帧间2d-2d间的匹配；pnp是求解3d到2d点的运动估计方法；icp是针对多个3d到3d点的位姿估计问题，求解icp的方法可以分为线性和非线性。机器人工程中的问题属于是多个3d到3d点的位姿求解问题，而现有的这些位姿求解方法如icp匹配并未考虑实际工件定位中的几何约束和自由度限定，在采用数字孪生技术对实际加工过程进行模拟时会发生失真。

技术实现思路

1、本发明的主要目的在于克服现有技术的缺点与不足，提出一种基于几何约束和自由度限定的位姿求解方法、系统、设备及介质，该方法可求解在指定自由度上不发生变化的情况下满足几何约束的位姿。

2、为了达到上述目的，本发明采用以下技术方案：

3、基于几何约束和自由度限定的位姿求解方法，包括以下步骤：

4、s1、确定需满足的几何约束、需限定的自由度以及初始位姿；

5、s2、根据需满足的几何约束求解位姿，得到满足几何约束的位姿变换矩阵；

6、s3、对得到的位姿变换矩阵进行自由度限定，使位姿变换矩阵满足自由度限定条件；

7、s4、采用自由度限定后得到的位姿变换矩阵更新当前位姿；

8、s5、对更新位姿后得到的位姿结果进行几何约束验算，若不满足约束则跳转至步骤s2并进行后续步骤，若满足约束，则结束迭代，输出位姿结果。

9、本发明还包括基于几何约束和自由度限定的位姿求解系统，系统应用本发明提供的位姿求解方法，系统包括：

10、输入限定模块、位姿变换矩阵求解模块、自由度限定模块、位姿变换矩阵更新模块以及约束验算模块；

11、输入限定模块，用于输入确定需满足的几何约束、需限定的自由度以及初始位姿；

12、位姿变换矩阵求解模块，用于根据需满足的几何约束求解位姿，得到满足几何约束的位姿变换矩阵；

13、自由度限定模块，用于对得到的位姿变换矩阵进行自由度限定；

14、位姿变换矩阵更新模块，用于根据自由度限定后得到的位姿变换矩阵更新当前位姿；

15、约束验算模块，对更新位姿后得到的位姿结果进行几何约束验算。

16、本发明还包括一种计算机设备，包括存储器以及处理器，存储器存储有计算机程序，处理器执行计算机程序时实现本发明提供的基于几何约束和自由度限定的位姿求解方法。

17、本发明还包括一种计算机可读存储介质，存储有计算机程序，当计算机程序被处理器执行时，实现本发明提供的基于几何约束和自由度限定的位姿求解方法。

18、本发明与现有技术相比，具有如下优点和有益效果：

19、1、本发明考虑了自由度限定的问题并且实现了求解，相比于传统的位姿求解方法具有更广泛的使用范围；并且，使用四元数处理旋转自由度限定问题，相比于旋转矩阵求解更直观高效。

20、2、本发明在视觉处理时不需要对整个工件进行扫描并重建完整的工件点云，只需要采集部分点云完成工件数个几何特征的重建即可完成定位。

21、3、依据六点定位原理，分步确定工件在不同自由度上的位姿并进行自由度限定，最终确定工件的位姿，与实际生产流程中的定位方案更为接近。通过更改视觉扫描方案实现生产中定位基准的改变，而无需重新装夹，从而避免由于多次装夹带来的装夹误差。

22、4、相较于传统的定位方法适用性更强；传统的实体定位元件需要与定位基面接触，当工件形状较为复杂时，定位元件会变得难以设计，而视觉重建则具有更高的自由度，可以对无法直接接触的表面进行重建，进而实现定位；同时解决了加工过程对实体定位元件的避障需求。

技术特征：

1.基于几何约束和自由度限定的位姿求解方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，步骤s1中，几何约束包括角度约束和距离约束；

3.根据权利要求1所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，步骤s2具体为根据需满足的几何约束进行位姿变换矩阵求解，得到的位姿变换矩阵使得变换前后的物体位姿满足指定的几何约束，该位姿变换矩阵表示为：

4.根据权利要求3所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，步骤s3中，对得到的位姿变换矩阵进行自由度限定，当限定的是平移自由度时，将平移向量的对应分量置为零，通过以下公式限制沿x轴的平移自由度：

5.根据权利要求4所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，步骤s4中，更新当前位姿具体表示为：

6.根据权利要求5所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，位姿更新的最终期望结果是限定目标工件的所有自由度，使工件的几何元素满足具体的角度约束或距离约束；每次更新的位姿仅限制工件的某几个自由度，根据当前限定的自由度进行修正，使其仅在特定自由度上进行变换；

7.根据权利要求1所述的基于几何约束和自由度限定的位姿求解方法，其特征在于，步骤s5中，对更新位姿后得到的位姿结果进行几何约束验算具体为：判断两个几何元素之间的距离或夹角是否小于预设的阈值，小于则认为满足几何约束，迭代结束；否则不满足约束，继续迭代。

8.基于几何约束和自由度限定的位姿求解系统，其特征在于，应用权利要求1-7任一项所述位姿求解方法，系统包括：

9.一种计算机设备，包括存储器以及处理器，存储器存储有计算机程序，其特征在于，处理器执行计算机程序时实现权利要求1-7任一项所述的基于几何约束和自由度限定的位姿求解方法。

10.一种计算机可读存储介质，存储有计算机程序，其特征在于，当计算机程序被处理器执行时，实现权利要求1-7任一项所述的基于几何约束和自由度限定的位姿求解方法。

技术总结
本发明公开了基于几何约束和自由度限定的位姿求解方法、系统、设备及介质，方法包括以下步骤：S1、确定需满足的几何约束、需限定的自由度以及初始位姿；S2、根据需满足的几何约束求解位姿，得到满足几何约束的位姿变换矩阵；S3、对得到的位姿变换矩阵进行自由度限定，使位姿变换矩阵满足自由度限定条件；S4、采用自由度限定后得到的位姿变换矩阵更新当前位姿；S5、对更新位姿后得到的位姿结果进行几何约束验算，若不满足约束则跳转至步骤S2并进行后续步骤，若满足约束，则结束迭代，输出位姿结果。本发明方法可以实现在限定自由度的情况下基于几何约束求解工件的位姿。

技术研发人员：王念峰,谢伟镛,李力佳,郑永忠
受保护的技术使用者：华南理工大学
技术研发日：
技术公布日：2024/4/7