基于圆形非编码点的物体运动轨迹与姿态实时检测方法

本发明属于单目视觉位姿测量，特别涉及一种物体运动轨迹与姿态实时检测方法，可用于目标跟踪、运动分析、自动驾驶和机器人导航。

背景技术：

1、单目视觉技术是一种基于单个相机的计算机视觉技术，通过计算图像中物体的位置、大小、形状这些特征来获取物体的信息。该技术以其成本低、实现简单、无需复杂设备的优点，广泛应用于机器人导航、运动分析、虚拟现实、自动驾驶等领域。单目视觉位姿测量使用人工标志点和自然模型角点作为测量目标，其测量精度高，应用广泛。

2、2014年赵永生等人在公开号为cn 105783872 a的专利文献中公开了《一种基于单目视觉技术的动态点目标运动轨迹测算方法》，其通过构建单目相机的针孔模型，获得单目相机图像中像点与动态点之间的映射关系，并通过多次拍摄来拟合轨迹曲线。该方法由于是对所布设动态点进行测量，因而只可观察到所布设动态点的运动轨迹，而无法测量到动态点之外其他点的运动且无法进行实时检测。

3、2019年安徽建筑大学的雷经发等人在专利号为cn201910850083.6的专利文献中公开了一种圆环标识的运动姿态视觉测量方法及其应用，其先使用单目相机拍摄一张已知内外圆半径的圆环标识图片；然后提取边缘点拟合得到二次型参数矩阵，结合射影几何原理计算出真实圆心像点；最后根据真实圆心像点求出消隐线方程，依据曲线二次型和消隐线理论最终解算出圆环标识的转动参数和平移参数。该方法由于需要使用已知内外径的圆环标识，且要以圆环标识的运动姿态代表目标物体的运动姿态，故当圆环标识被部分遮挡，单目相机无法拍摄到圆环标识时，则无法计算圆环标识的运动姿态。

4、2023年合肥工业大学孙海等人在《基于单目视觉的特定工件位姿检测研究》文章中，建立了一种基于单目视觉的特定立方体工件检测模型。该模型利用霍夫变换提取拟合目标物的临近特征角点，再结合相机标定的参数与特征工件的先验信息换算出拟合角点的实际坐标，最后，根据图像特征角点像素坐标与用户坐标间的映射关系，得到目标物的位姿信息。该方法由于只可检测到特征角点的坐标，因而只能求得特征角点在运动过程中的位姿信息，非角点处的位姿信息将无法获得，导致物体整体的位姿测量精度不高。

技术实现思路

1、本发明的目的在于针对上述现有技术的不足，提出一种基于圆形非编码点的物体运动轨迹与姿态实时检测方法，以实时求解物体在运动过程中甚至圆形非编码点被部分遮挡时，物体上任一点包括质心的运动轨迹与姿态，提高检测的鲁棒性。

2、为了实现上述目的，本发明的技术方案包括如下步骤：

3、(1)获取待测物体及其cad模型，在被测物体表面布设一些圆形非编码标志点作为控制点，用于求解物体运动轨迹与姿态；

4、(2)在被测物体表面及周围再布设一些编码标志点与至少一根比例尺，采用近景摄影测量技术重建出所布设的控制点的三维坐标，并对控制点重新编号；

5、(3)采用icp算法将所重建的摄影测量坐标系转换至被测物体cad模型坐标系，同时将控制点的三维坐标变换至转换后的坐标系中；

6、(4)采用基于约束搜索球的点数据归属算法，确定步骤(3)坐标转换后的控制点在被测物体cad模型表面的对应三角面片，并将控制点投影至所对应的三角面片，获得控制点在被测物体cad模型上对应的模型点；

7、(5)通过单目相机采集不同位置、不同姿态下的m幅十字靶标图像，以对单目相机进行标定，获得单目相机的内参矩阵k，其中m>8；

8、(6)物体开始运动前，通过单目相机采集n帧被测物体图像，采用圆形非编码标志点实时检测算法提取n帧图像中控制点的中心坐标，并基于管道滤波算法为这些检测到的控制点分别构建一个管道，并手动将管道id更新为该管道所对应的模型点id，其中n为管道长度，n>7；

9、(7)物体开始运动后，控制单目相机实时采集一帧被测物体图像，采用圆形非编码标志点实时检测算法提取被测物体运动图像中控制点的中心坐标；

10、(8)基于管道滤波算法，对步骤(7)所检测的单目相机中所有控制点进行实时跟踪；

11、(9)根据步骤(7)中的控制点的二维坐标(u,v)t和对应的模型点在cad模型坐标系下的坐标(xw,yw,zw)t，基于直接线性变换法，建立起像素坐标系到cad模型坐标系的映射关系：

12、

13、其中p为待求的投影矩阵，zc为控制点在相机坐标系下的z坐标，即该标志点的深度；

14、(10)选取6个以上不共面的控制点的二维坐标和对应的模型点在cad模型坐标系下坐标，计算出投影矩阵p，并利用投影矩阵p求解物体上任意一点包括质心的运动轨迹与姿态；

15、(11)判断是否停止图像的实时采集：

16、若停止采集图像，则结束物体运动轨迹与姿态的实时求解；

17、若继续采集图像，则根据步骤(8)控制点实时跟踪结果及步骤(10)得到的第m帧的投影矩阵pm，对第m帧时的模型点进行重投影，以对管道进行实时更新，返回步骤(7)。

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

19、1)本发明由于采用了近景摄影测量技术与投影映射，将物体表面粘贴的控制点与物体的cad模型结合，通过求解每一帧中单目相机的外参矩阵，计算物体运动变换矩阵，从而获取到物体cad模型上任意一点包括质心的运动轨迹与姿态，并采用圆形非编码标志点检测物体的运动轨迹与姿态，因而与现有法相比本发明方法适应性更强、应用更广泛；

20、2)本发明由于在物体表面的控制点被部分遮挡时，根据图像中检测到的至少六个控制点的二维坐标求解单目相机的外参矩阵，进而计算出物体的运动变换矩阵，因而可获得被遮挡控制点的运动轨迹与姿态，克服了现有技术在受遮挡情况下无法测量的问题；

21、3)本发明由于采用单目相机来计算物体的运动轨迹与姿态，相对于使用双目相机，本发明的效率更高，成本更低，安装布线简单，使用方便。

22、经实验测得，本发明在配置为i9-11900hcpu与rtx3060显卡的笔记本电脑上，处理分辨率为500万像素，在包含7个以上控制点的单目序列图像时，物体运动轨迹与姿态实时在线检测的效率可以达到100hz以上，满足实时在线检测的应用需求。

技术特征：

1.一种基于圆形非编码点的物体运动轨迹与姿态实时检测方法，其特征在于，包括如下步骤：

2.如权利要求1所述的方法，其特征在于，步骤(2)采用近景摄影测量技术重建出所布设的控制点的三维坐标，实现如下：

3.如权利要求1所述的方法，其特征在于，步骤(2)中对控制点重新编号，是基于等式pid＝p0+i进行编号，其中，pid为某一控制点的新id，p0是本次编号的起始位置，i为该控制点编号的次序。

4.如权利要求1所述的方法，其特征在于，步骤(3)采用icp算法将所重建的摄影测量坐标系转换至被测物体cad模型坐标系，实现如下：

5.如权利要求1所述的方法，其特征在于，步骤(6)中基于管道滤波算法为检测到的控制点分别构建一个管道，并手动将管道id更新为该管道所对应的模型点id，实现如下：

6.如权利要求1所述的方法，其特征在于，步骤(7)中采用圆形非编码标志点实时检测算法提取被测物体运动图像中控制点的中心坐标，实现如下：

7.如权利要求1所述的方法，其特征在于，步骤(8)中对检测的单目相机中所有控制点进行实时跟踪，是判断控制点的中心是否落入步骤(6)所构建的某一个管道中：

8.如权利要求1所述的方法，其特征在于，步骤(10)中计算出投影矩阵p，并利用投影矩阵p求解物体上任意一点包括质心的运动轨迹与姿态，实现如下：

9.如权利要求1所述的方法，其特征在于，步骤(10)中对第m帧时的模型点进行重投影，以对管道进行实时更新，实现如下：

技术总结
本发明公开了一种基于圆形非编码点的物体运动轨迹与姿态实时检测方法，主要解决现有视觉位姿测量技术无法获得物体上任意一点及被遮挡位置的运动轨迹与姿态，且实时性差的问题。其实现方案为：计算控制点的三维坐标，并对其映射得到模型点；建立控制点与模型点的对应关系，求解出单目相机在物体CAD模型坐标系下的基准位姿；物体开始运动后，单目相机实时采集物体运动图像对其控制点实时跟踪求解单目相机的当前位姿；根据该位姿与基准位姿的变换关系计算物体的运动变换矩阵，以对CAD模型及模型点进行变换，实时求解物体上任意一点包括质心的运动轨迹与姿态。本发明实时性强，效率高，成本低，可用于目标跟踪、运动分析、自动驾驶和机器人导航。

技术研发人员：史宝全,李孟攀,蔡林生,苏伟杰,郑思琪,孙浩宇,梁健焯,叶俊杰
受保护的技术使用者：西安电子科技大学
技术研发日：
技术公布日：2024/1/16