基于双振镜的近一维高速运动目标三维姿态测算方法及系统

本发明涉及三维姿态测算，具体涉及基于双振镜的近一维高速运动目标三维姿态测算方法及系统。

背景技术：

1、近一维高速运动目标是指在一维空间中以高速进行运动的目标；这种目标的运动轨迹只能在一条直线上进行，不能在二维或三维空间中自由移动。

2、目前，基于单振镜的凝视跟踪系统是一种利用振镜反射和眼动追踪技术相结合的方法，通过调整振镜的角度来改变相机视场，实现对移动物体的精确跟踪；然而现有基于单振镜的凝视跟踪系统缺少视觉深度信息，所以无法对近一维高速运动目标的三维姿态进行测算；因此，需要设计基于双振镜的近一维高速运动目标三维姿态测算方法及系统。

技术实现思路

1、本发明的目的是克服现有技术的不足，为更好的有效解决现有基于单振镜的凝视跟踪系统缺少视觉深度信息，所以无法对近一维高速运动目标的三维姿态进行测算的问题，提供了基于双振镜的近一维高速运动目标三维姿态测算方法及系统，其实现了通过采用霍夫直线提取和特征点匹配的方法能精确测算目标的位置及位姿，再通过振镜的旋转角度将测算的姿态转化到初始坐标轴能更加清晰的反映高速目标的运动姿态，本发明所提测算方法及系统具有低成本、大视野、高精度、简单结构等优点，具有较高的应用价值。

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

3、基于双振镜的近一维高速运动目标三维姿态测算方法及系统，包括以下步骤，

4、步骤a，构建基于双振镜的近一维高速运动目标三维姿态拍摄组件，并对近一维高速运动目标进行拍摄，获得拍摄图像；

5、步骤b，根据拍摄图像对近一维高速运动目标进行初始目标测算，获得初始目标测算数据；

6、步骤c，基于初始目标测算数据对近一维高速运动目标进行三维姿态测算，获得三维姿态测算数据；

7、步骤d，根据初始目标测算数据和三维姿态测算数据对振镜旋转后的虚拟相机外部参数进行计算，并通过振镜的旋转角度将三维姿态测算数据转化到初始坐标轴，获得近一维高速运动目标三维姿态测算结果。

8、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤a，构建基于双振镜的近一维高速运动目标三维姿态拍摄组件，并对近一维高速运动目标进行拍摄，获得拍摄图像，其中基于双振镜的近一维高速运动目标三维姿态拍摄组件具体包括一个仰角相机、两个可旋转振镜和一台上位机，所述基于双振镜的近一维高速运动目标三维姿态拍摄组件具体是通过两个可旋转振镜的旋转差角使单仰角相机对同一目标形成两个视野的图像。

9、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤b，根据拍摄图像对近一维高速运动目标进行初始目标测算，获得初始目标测算数据，其中初始目标测算包含双目标定和三角测算，具体是将拍摄图像进行分离并设定双目标定的外参为r和t，再将标定后的外参r和t用于三角测算。

10、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤c，基于初始目标测算数据对近一维高速运动目标进行三维姿态测算，获得三维姿态测算数据，其中三维姿态测算包含初始位姿测算和相对位置测算，具体步骤如下，

11、步骤c1，对近一维高速运动目标进行初始位姿测算，其中初始位姿测算是以目标的主轴反映目标的位姿；

12、步骤c2，对近一维高速运动目标进行相对位置测算，其中相对位置测算是通过相对相对姿态结合点云变换测算出目标的相对位置。

13、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤c1中初始位姿测算具体是将测算目标作为一个质心点，再通过左右图像的质心点计算世界坐标系下的三维点作为目标在世界坐标系下的位置，具体步骤如下，

14、步骤c11，计算目标的位置，具体是对左右目标图像进行canny边缘检测，并分别提取左图像目标轮廓和右图像目标轮廓的质心，具体步骤如下，

15、步骤c111，计算目标区域二阶以内原点矩mij，如公式(1)所示，

16、mij＝∫∫xiyjf(x,y)dxdy    (1)

17、其中，i和j分别表示几阶空间矩，x和y分别表示像素的列号和像素的行号；

18、步骤c112，设f(x,y)＝1，则目标轮廓质心如公式(2)所示，再通过三角测算的方法计算目标在世界坐标系下的位置信息；

19、

20、其中，m00表示图像的零阶空间矩，m10和m10表示图像的一阶空间矩；

21、步骤c12，提取左侧图像目标轮廓上最长的上下两条直线，并计算最长直线对应的角度a0和a1，再通过平均值的方法计算目标主轴的角度，如公式(3)所示，

22、

23、其中，a表示主轴对应的角度；

24、步骤c13，将左右两张目标图像进行特征点匹配并得到相匹配的n组特征点，设左图像匹配到的特征点为point1，右图像匹配到的特征点为point2，求取特征点point1中任意两点像素坐标的像素坐标角度，并计算像素坐标角度与目标主轴的角度差，提取最小角度差对应的一组像素坐标，如公式(4)所示，

25、

26、其中，(x1,y1),(x2,y2)表示最小角度差对应的一组像素坐标；

27、步骤c14，将最小角度差对应的一组像素坐标当作主轴上的两点，并进行三角测算得到世界坐标系下两个三维点(x1,y1,z1),(x2,y2,z2)，再通过这两个三维点(x1,y1,z1),(x2,y2,z2)计算目标的位姿，如公式(5)所示，

28、

29、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤c2中相对位置测算具体是通过初始提取到的中轴线角度结合四图像特征点匹配并获得当前目标图像上代表中轴线的两点，再计算当前中轴线的角度并获得相对姿态，具体步骤如下，

30、步骤c21，相对姿态测算，具体步骤如下，

31、步骤c211，通过在初始位置左侧图像提取最小一组特征点，从而得到对应三张图像中轴线上的特征点；

32、步骤c212，通过三角化分别计算两个时刻的位姿，再通过相减的方式求得相对位姿，并构建旋转矩阵r；

33、步骤c22，相对位置测算，具体步骤如下，

34、步骤c221，将初始位置和当前位置匹配到的三维点全部三角化，得到初始位置三维点云p0和当前位置三维点云p1；

35、步骤c222，将得到的初始位置三维点云p0和当前位置三维点云p1去中心化并将大值减去，如公式(6)所示，

36、

37、其中，p’0为去中心化后的初始位置三维点云，p1'为去中心化后的当前位置三维点云；

38、步骤c223，将去中心化后的当前位置三维点云p1'乘上旋转矩阵得到去旋转化后的当前位置三维点云p1”，如公式(7)所示，

39、p1”＝p1'×r    (7)

40、其中，p1”为去旋转化后的当前位置三维点云；

41、步骤c224，将当前位置三维点云p1的平均值减去初始位置三维点云p0的平均值，再加上去旋转化后的当前位置三维点云p1”平均值并减掉去中心化后的初始位置三维点云的平均值，获得相对位置δp，如公式(8)所示，

42、δp＝average(p1)-average(p0)+average(p1”)-average(p0')    (8)

43、其中，δp表示相对位置。

44、前述的基于双振镜的近一维高速运动目标三维姿态测算方法，步骤d，根据初始目标测算数据和三维姿态测算数据对振镜旋转后的虚拟相机外部参数进行计算，并通过振镜的旋转角度将三维姿态测算数据转化到初始坐标轴，获得近一维高速运动目标三维姿态测算结果，其中对振镜旋转后的虚拟相机外部参数进行计算的具体步骤如下，

45、步骤d1，基于初始位置的外参r和t，设振镜b形成虚拟相机的主点为pb1及振镜c形成虚拟相机的主点为pc1，则主点pb1和主点pc1之间的关系如公式(9)所示，

46、pb1＝rpc1+t    (9)；

47、步骤d2，随着高速目标从t1时刻位置运动到tn时刻位置，振镜b对应的虚拟相机从ob1移动到ob2，振镜c对应的虚拟相机从oc1移动到oc2；

48、步骤d3，振镜b旋转了△b，而对应的虚拟相机旋转了2△b，且虚拟相机的光心从ob1移动到ob2，设旋转前虚拟相机的光心ob1为坐标轴中心并建立坐标系，计算旋转后虚拟相机光心ob2到ob1坐标系下的旋转和平移矩阵，θ为振镜平面到光心与振镜平面的夹角，l为振镜中心到光心距离在水平面的夹角，设在振镜b旋转后相机坐标系下目标的三维坐标为pb2，求对应在初始相机坐标系下的三维坐标pb1如公式(10)和公式(11)所示，

49、pb1＝rbpb2+tb    (10)

50、

51、其中，rb和tb表示振镜b旋转后的外参；

52、步骤d4，设在振镜c旋转后相机坐标系下的目标的三维坐标为pc2，求对应在初始相机坐标系下的三维坐标pc1如公式(12)和(13)所示，

53、pc1＝rcpc2+tc    (12)

54、

55、其中，rc和tc表示振镜c旋转后的外参；

56、步骤d5，综合初始标定的r和t及公式(9)、公式(10)和公式(12)，这样通过初始位置标定的外参结合旋转角度得到旋转后的外参，如公式(14)所示，

57、

58、基于双振镜的近一维高速运动目标三维姿态测算系统，包括拍摄模块、初始目标测算模块、三维姿态测算单元和三维姿态测算结果获取模块，所述拍摄模块用于构建基于双振镜的近一维高速运动目标三维姿态拍摄组件，并对近一维高速运动目标进行拍摄，获得拍摄图像；所述初始目标测算模块用于根据拍摄图像对近一维高速运动目标进行初始目标测算，获得初始目标测算数据；所述三维姿态测算单元用于基于初始目标测算数据对近一维高速运动目标进行三维姿态测算，获得三维姿态测算数据；所述三维姿态测算结果获取模块用于根据初始目标测算数据和三维姿态测算数据对振镜旋转后的虚拟相机外部参数进行计算，并通过振镜的旋转角度将三维姿态测算数据转化到初始坐标轴，获得近一维高速运动目标三维姿态测算结果。

59、前述的基于双振镜的近一维高速运动目标三维姿态测算系统，所述三维姿态测算单元包含初始位姿测算模块和相对位置测算模块，所述初始位姿测算模块用于将测算目标作为一个质心点，再通过左右图像的质心点计算世界坐标系下的三维点作为目标在世界坐标系下的位置；所述相对位置测算模块用于通过初始提取到的中轴线角度结合四图像特征点匹配并获得当前目标图像上代表中轴线的两点，再计算当前中轴线的角度并获得相对姿态。

60、本发明的有益效果是：本发明的基于双振镜的近一维高速运动目标三维姿态测算方法及系统，首先构建基于双振镜的近一维高速运动目标三维姿态拍摄组件并对近一维高速运动目标进行拍摄获得拍摄图像，接着根据拍摄图像对近一维高速运动目标进行初始目标测算并获得初始目标测算数据，随后基于初始目标测算数据对近一维高速运动目标进行三维姿态测算并获得三维姿态测算数据，然后根据初始目标测算数据和三维姿态测算数据对振镜旋转后的虚拟相机外部参数进行计算，并通过振镜的旋转角度将三维姿态测算数据转化到初始坐标轴，获得近一维高速运动目标三维姿态测算结果；有效的实现了本发明具有通过振镜的旋转改变虚拟相机的位置和姿态从而对近一维高速运动目标的凝视跟踪观测，进而可以扩大系统观测视野并缓解运动模糊现象获得更加清晰的图像的功能，而通过采用霍夫直线提取和特征点匹配的方法能精确测算目标的位置及位姿，再通过振镜的旋转角度将测算的姿态转化到初始坐标轴能更加清晰的反映高速目标的运动姿态，结合实施例显示，本发明的位置测算平均相对误差为3％左右，初始姿态角误差最小可达到0.08°，本发明所提测算方法及系统具有低成本、大视野、高精度、简单结构等优点，具有较高的应用价值。