一种基于L型定位杆的动捕扫描联合标定与位姿解算方法

本发明属于动作捕捉与三维扫描联合测量，具体涉及一种基于l型定位杆的动捕扫描联合标定与位姿解算方法。

背景技术：

1、机器人标定、光学装调、空间目标半物理仿真以及多刚体装配测量等场景，对目标刚体位姿测量的实时性与测量精度均提出较高要求。三维扫描系统可获取待测对象高精度静态三维几何模型，能够提取质心、光学中心、装配基准点等关键几何基准；动作捕捉系统可依靠marker标记点实现刚体动态实时跟踪。但两套测量系统分属独立坐标系，三维扫描模型坐标系与动捕房间坐标系之间缺少稳定统一的公共基准，导致三维扫描得到的高精度几何定义难以直接映射至动作捕捉实时测量结果中，静态几何信息无法服务于物体运动过程中的实时位姿解算。

2、动作捕捉系统建立刚体时，通常依据marker点的空间分布自动生成刚体坐标系，该默认坐标系仅反映marker点组合的几何分布，并不一定与待测刚体的真实目标坐标系重合。对于立方镜、光学镜组、机械臂末端工具这类对象，工程实际关注的往往是质心、光学中心、工具中心点等功能基准的位姿，而动捕默认生成的坐标系会同时引入位置偏差与姿态偏差。若为缩小该偏差而将marker点布置在真实目标点周边，又会受结构空间、反光、运动遮挡的限制，容易出现marker丢失、刚体跟踪失效的问题。现有技术中，也存在采用人工补偿偏置、专用标定工装或者激光跟踪仪等外部设备实现两套系统配准的方案，但普遍存在人工误差大、改造对象结构、设备成本高、操作流程繁琐的缺陷。

3、在多刚体协同测量工况下，上述问题会进一步放大。若各个刚体均直接使用marker点拟合得到的默认坐标系输出位姿，计算得到的多刚体之间相对位姿，并非各待测刚体真实目标坐标系之间的实际相对关系，误差会向下传递至后续标定、装调计算环节，降低实验结果的可靠性。因此亟需一种兼顾操作简便性与测量精度的联合标定方法，实现单个或多个待测刚体真实目标坐标系的实时位姿输出。

技术实现思路

1、为了解决现有技术中存在的上述问题，本发明通过以下技术方案实现：

2、在本发明提供了一种基于l型定位杆的动捕扫描联合标定与位姿解算方法，其特征在于，包括以下步骤：

3、将动作捕捉系统中的l型定位杆和待测刚体静止放置在同一实验空间中进行三维扫描，得到三维模型，将三维模型导入三维模型处理软件；

4、在三维扫描模型坐标系下，根据l型定位杆上的marker球，建立l型定位杆坐标系，根据三维扫描模型坐标系得到l型定位杆坐标系的位姿矩阵；

5、在三维扫描模型坐标系下，提取待测刚体的几何基准特征，根据所述几何基准特征，建立待测刚体的真实目标坐标系，根据三维扫描模型坐标系得到真实目标坐标系位姿矩阵；

6、根据l型定位杆坐标系的位姿矩阵和真实目标坐标系位姿矩阵，通过矩阵运算得到待测刚体真实目标坐标系相对于l型定位杆坐标系的相对位姿矩阵；

7、在动作捕捉软件中，获取l型定位杆坐标系在动捕房间坐标系下的位姿，根据相对位姿矩阵，得到待测刚体真实目标坐标系在动捕房间坐标系下的位姿。

8、三维扫描步骤之前，还包括：

9、动作捕捉系统识别l型定位杆自带的marker球，执行场地标定以建立动捕房间坐标系；场地标定完成后，保持l型定位杆的实物位置固定不变。

10、根据三维扫描模型坐标系得到l型定位杆坐标系的位姿矩阵，包括：

11、获取l型定位杆长轴方向上的两个marker球质心和短轴方向上的两个marker球质心，将长轴方向上两个marker球质心的连线方向定义为l型定位杆坐标系的x轴方向；将短轴方向上两个marker球质心的连线方向定义为l型定位杆坐标系的y轴方向，根据右手坐标系规则，由x轴方向与y轴方向叉乘得到基准平面的法向量作为z轴方向，将z轴方向移动预设偏置距离，得到l型定位杆坐标系原点，根据三维扫描模型坐标系s和l型定位杆坐标系原点，得到l型定位杆坐标系的位姿矩阵。

12、获取l型定位杆长轴方向上的两个marker球质心和短轴方向上的两个marker球质心，包括：

13、在三维扫描模型内框选l型定位杆各个marker球对应的球面点云区域，对所选球面点云做几何求解，得到各个marker球的质心。

14、在三维扫描模型坐标系下，提取待测刚体的几何基准特征，根据所述几何基准特征，建立待测刚体的真实目标坐标系，包括：

15、在三维扫描模型中选取待测刚体至少两个相互垂直的基准平面点云区域，分别对平面点云进行平面拟合，得到各基准平面对应的法向量；基于基准平面法向量确定临时坐标系的坐标轴方向，再根据待测刚体的设计尺寸或者装配偏置量，对临时坐标系进行平移，得到待测刚体的真实目标坐标系。

16、根据l型定位杆坐标系的位姿矩阵和真实目标坐标系位姿矩阵，通过矩阵运算得到待测刚体真实目标坐标系相对于l型定位杆坐标系的相对位姿矩阵，公式为：

17、

18、其中，为l型定位杆坐标系的位姿矩阵，为真实目标坐标系位姿矩阵。

19、获取相对位姿矩阵之后，还包括：

20、在动作捕捉软件中，选择待测刚体上的若干marker点，根据marker点空间分布建立动捕待测刚体坐标系；所述动捕待测刚体坐标系用于marker点遮挡、或脱落的识别状态监测。

21、l型定位杆坐标系和待测刚体真实目标坐标系在三维扫描模型坐标系下的位姿，均采用4阶齐次变换矩阵表达，表达式为：

22、

23、其中，其中，rsa为3×3旋转矩阵，为坐标系的原点三维扫描模型坐标系s下的位置向量。

24、待测刚体为立方镜。

25、当存在多个待测刚体时，对每一个待测刚体分别执行三维扫描、提取几何基准特征和建立对应真实目标坐标系步骤，分别求解各个待测刚体真实目标坐标系相对于l型定位杆坐标系的相对位姿矩阵；结合l型定位杆坐标系在动捕房间坐标系下的位姿，分别解算得到每一个待测刚体真实目标坐标系在动捕房间坐标系下的位姿，并依据各待测刚体真实目标坐标系的位姿，计算多个待测刚体真实目标坐标系相互之间的相对位姿。

26、与现有技术相比，本发明的有益效果在于：

27、1、本发明复用动捕系统现有的l型定位杆作为两套系统的公共基准，无需额外配置标定板、激光跟踪仪等外部设备，通过一次整体三维扫描，即可将扫描模型内的目标几何定义直接关联至动捕房间坐标系，操作简便，降低实验硬件成本；

28、2、在三维扫描模型中独立定义待测刚体真实目标坐标系，坐标系原点可设置在质心、光学中心、工具中心点、装配基准点等功能位置，与marker点布置相互解耦；marker仅用于刚体识别跟踪，可布置在不易遮挡区域，避免受目标本体空间、反光限制造成的刚体跟踪失效，充分发挥三维扫描的几何测量精度；

29、3、通过相对位姿矩阵对动捕刚体坐标系完成修正，直接输出真实目标坐标系实时位姿；多刚体可共用同一套基准与解算逻辑，完成坐标系统一并计算各目标之间的相对位姿，适配立方镜、机械臂末端工具、精密装配件等多种测量对象，适用范围广。

30、以下将结合附图及实施例对本发明做进一步详细说明。