CN110500990B - 一种六自由度测量系统及方法 - Google Patents

Abstract

本发明涉及一种六自由度测量系统及方法。该系统包括跟踪测量装置和靶标；跟踪测量装置包括处理器，以及分别连接处理器的相机和两个旋转镜片模块，相机和两个旋转镜片模块依次排列，且相机和两个旋转镜片模块的光轴保持同轴；每个旋转镜片模块可独立旋转；靶标安装在待测目标上，包括至少三个距离关系已知的标志点，并且至少一个标志点与其余标志点的连线的中点不重合。与现有技术相比，本发明实现对待测目标六自由度的动态实时测量。在测量过程中仅需将靶标固装在待测目标上，靶标结构简单，对待测目标的实际作业影响小，使用方便。同时，简化了六自由度测量的计算过程，提高测量方法的实时性和可靠性。

Description

一种六自由度测量系统及方法

技术领域

本发明涉及空间几何测量领域，尤其是涉及一种六自由度测量系统及方法。

背景技术

随着科学技术的高速发展，位姿测量的需求不断增加。例如工业机器人在作业过程中，由于机器人的制造误差、控制精度和环境温度变化，将极大的影响机器人末端位姿精度，因此需对工业机器人末端实时测量，反馈位姿误差，实现工业机器人的高精度动态作业。在大型设备的装配过程中，需要对零件的空间位姿实时测量，确保装配的顺利实现。目前用于测试工业机器人末端的六自由度测量装置通常操作难度较大、整机组成复杂、占用空间大、不易携带，且市场售价较高，难以在工业生产中广泛应用。

中国发明申请CN107246866A公开了一种高精度六自由度测量系统及方法，提出一种系统用于测量前被测目标和后被测目标的相对位姿。该系统采用两个倾角传感器和视觉成像系统测量被测目标位姿，两个倾角传感器需与特征点靶标固装在被测目标上，将对待测目标的实际作业造成影响，且系统安装复杂，成本高。

中国发明申请CN107505610A公开了一种六自由度位姿测量方法及装置，通过二维敏感面依次接收多个光源发射的光线并转换为相应的电信号，根据发射光线的预设规则和接收的电信号获得二维敏感面相对第一平面的六自由度位姿信息或获得第一平面相对二维敏感面的六自由度位姿，但该方法需要多个光源作为标志点实现对被测目标的六自由度测量，且需PSD即位置灵敏探测器，因此该装置成本高。

中国发明专利CN105806180A提出了一种空间六自由度测量装置，该装置采用三个角度传感器和三个位移传感器直接测量被测对象六自由度，但该装置需与待测目标接触测量，且无法实现待测目标的动态实时测量。

中国发明专利CN102636139A提出了一种空间六自由度动态测量装置和方法，该装置采用六个拉线式位移传感器测量待测目标的六自由度，但该测量装置体积较大，不易搬运，测量方式为接触式测量，适用测量对象有限。

美国专利US7312862(Measurement system for determining six degrees offreedom of an object)提出一种六自由度测量系统，该装置采用激光反射原理实现目标跟踪，通过干涉原理获得目标深度信息，通过相机获取光点图像坐标，由针孔成像原理计算待测目标六自由度，该装置测量精度高，响应速度快，但结构复杂，求解过程繁琐，需多个测量传感器，整机成本高，较难在工业领域普遍推广和使用。

美国专利US7576836(Camera based six degree-of-freedom target measuringand target tracking device)提出一种基于相机的六自由度测量方法和跟踪装置，该装置采用可旋转的相机对待测目标的实时跟踪，以及靶标光源的图像信息采集，实现对待测目标六自由度的测量，在测量过程中由于相机的转动，将极大影响靶标光源图像采集质量，直接影响待测目标六自由度的测量精度。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种六自由度测量系统及方法。

本发明的目的可以通过以下技术方案来实现：

一种六自由度测量系统，包括跟踪测量装置和靶标；

所述的跟踪测量装置包括处理器，以及分别连接处理器的相机和两个旋转镜片模块，所述的相机和两个旋转镜片模块依次排列，且相机和两个旋转镜片模块的光轴保持同轴；每个旋转镜片模块独立旋转；

所述的靶标安装在待测目标上，包括至少三个距离关系已知的标志点，并且至少一个标志点与其余标志点的连线的中点不重合。

进一步地，所述的旋转镜片模块包括驱动单元、传动单元、角度测量器和棱镜、所述的棱镜为截面为直角梯形的镜片，所述的角度测量器和棱镜均安装在传动单元上，所述驱动单元连接传动单元，所述处理器连接驱动单元。

进一步地，所述的靶标包括套筒和标志环，所述的标志环包括标志轴和套环，套环嵌套在套筒的外径上，每个套环上径向外圈分布标志轴，所标志点设置在标志轴悬空的一端。

进一步地，所述的相机和旋转镜片模块之间设置有滤光片。

进一步地，所述的标志点为球状红外发光二级管。

进一步地，光线通过两个旋转镜片模块所形成的光学扫描域不小于相机成像视场域，相机与两个旋转镜片模块共同形成的组合成像视场域不小于相机成像视场域。

一种如上任一所述的六自由度测量系统的测量方法，具体步骤如下：

S1、分别建立世界坐标系O_w-X_wY_wZ_w、相机坐标系O_c-X_cY_cZ_c、靶标坐标系O_t-X_tY_tZ_t和待测目标坐标系O_a-X_aY_aZ_a；

S2、对相机进行标定，获取相机的内参数和外参数；

S3、控制旋转镜片模块旋转调整相机的视轴，使靶标落入相机的视场范围内；

S4、相机采集至少包括三个标志点的图像，进行图像处理，获得标志点的图像坐标，i＝1～n；

S6、获取旋转镜片模块的转动角度，以标志点对应的图像坐标分别和相机焦点F的连线为入射投影光线，通过矢量折射定理获得入射投影光线在旋转镜片模块的投影光线出射点K_i，i＝1～n，以及出射投影光线向量S_i，i＝1～n；

S7、计算获取标志点的三维坐标，由如下方程组求解：

其中，X_pi、Y_pi、Z_pi为第i个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标；X_ki、Y_ki、Z_ki为K_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标；X_si、Y_si、Z_si为S_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，i＝1～n；X_pj、Y_pj、Z_pj为第j个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，j＝1～n；D_ij为第i个标志点与第j个标志点的距离；λ_i为第i个方程比例系数；

S8、所采集标志点中选取三个标志点且其中至少一个标志点与其他两个标志点连线的中点不重合，其所在平面法向量由下式求解：

式中，P₁为第一标志点，P₂为第二标志点，P₃为第三标志点，N为第一标志点、第二标志点和第三标志点所在平面法向量，同时求得在三个标志点所在平面内与

垂直的向量

以P₂为原点，

和

的方向向量为坐标方向建立坐标系，将靶标坐标系Ot-X_tY_tZ_t与此坐标系重合；

S9、通过方向余弦矩阵求解靶标坐标系O_t-X_tY_tZ_t相对相机坐标系O_c-X_cY_cZ_c的姿态矩阵R；

S10、通过三个标志点的图像坐标，使用针孔成像模型，求解靶标坐标系O_t-X_tY_tZ_t相对相机坐标系O_c-X_cY_cZ_c的平移矩阵T；

S11、获取待测目标相对世界坐标系O_w-X_wY_wZ_w的姿态矩阵，其表达式为：

M＝M₁M₂M₃

其中，M₁为相机相对世界坐标系O_w-X_wY_wZ_w的位姿转换矩阵，

M₃为待测目标坐标系O_a-X_aY_aZ_a相对靶标坐标系O_t-X_tY_tZ_t的位姿转换矩阵。

S12、待测目标运动时，通过视觉跟踪算法控制旋转镜片模块旋转调整相机的视轴，使靶标保持在相机的视场范围内，重复S4至S11。

进一步地，所述的步骤S12中，视觉跟踪算法具体为：在已知当前旋转镜片模块转角的条件下，采集标志点中任意一点和相机焦点F的连线为旋转镜片模块的入射投影光线向量，通过矢量光线追迹法求得旋转镜片模块的出射投影光线向量，通过出射投影光线向量再采用视轴逆向查表法求解旋转镜片模块的转角。

进一步地，所述的视轴逆向查表法的制表步骤如下：

确定棱镜旋转步长θ_st，将棱镜的转动角度分为360°/θ_st份，计算获得不同转角下的射光向量，建立转角和出射光单位向量查找表，其计算表达式为：

其中，A_in表示入射光单位向量，A_out表示折射光单位向量；N表示折射面的法向量；n₁为入射介质的折射率，n₂为出射介质的折射率；

式中，θ_ri为第i个棱镜转角，α为棱镜楔角。

进一步地，所述的视轴逆向查表法的查表步骤如下：

根据当前旋转镜片模块的出射光线单位向量，在所建立的表中搜索最接近的出射光向量，即使得向量误差Δ达到最小值时所对应的棱镜转角值；Δ的表达式为：

其中，X_rp、Y_rp、Z_rp为表中记录的出射光单位向量坐标，x_rp、y_rp、z_rp为当前棱镜出射光单位向量坐标。

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

1、本发明通过实时调整两个的旋转镜片模块的转动角度，实现对待测目标六自由度的动态实时测量。在测量过程中仅需将靶标固装在待测目标上，靶标结构简单，对待测目标的实际作业影响小，使用方便。同时，本发明简化了六自由度测量的计算过程，提高测量方法的实时性和可靠性。

2、本发明的测量方法采用非接触式，对被测目标的适应性较强。

3、本发明测量系统的结构简单，体积小，易于携带，成本低。

附图说明

图1是本发明测量系统的结构示意图。

图2是相机、棱镜及相机棱镜组合的视场域示意图。

图3是视轴调整光路示意图。

图4是棱镜结构示意图。

图5是标志环结构示意图。

图6是本发明测量方法的流程示意图。

图7是本发明测量方法的原理示意图。

图8是测量精度分析示意图。

图9是光轴指向误差与棱镜楔角误差关系图。

图10是光轴指向误差与棱镜折射率误差关系图。

图11a是装配误差示意图。

图11b是轴承倾斜误差示意图。

图12是光轴指向误差与棱镜倾斜轴倾斜角关系图。

图13是光轴指向误差与轴承倾斜轴倾斜角关系图。

图14是靶标结构示意图。

图15是靶标主剖视示意图。

图16是探针结构示意图。

图17是基孔靶标结构示意图。

图18是基轴靶标结构示意图。

图19是柔性多铰链测量系统结构示意图。

图20是扫描器测量系统结构示意图。

附图标记：100、跟踪测量装置，101、处理器，102、相机，1021、相机感光芯片，1022、相机镜头，103、滤光片，104、驱动单元，105、传动单元，106、角度测量器，107、棱镜，108、视轴，109、调整视轴，1080、相机成像视场域，1081、组合成像视场域，1082、光学扫描域，200、靶标，201、标志环，2011、标志点，2011A、第一标志点，2011B、第二标志点，2011C、第三标志点，2012、标志轴，2013、套环，202、套筒一，203、固定座，300、待测目标，301、套筒二，302、针头，400、基孔靶标，401、第一顶部标志点，402、支柱一，403、套筒三，405、插针，500、基轴靶标，501、第二顶部标志点，502、支柱二，503、标志环套筒四，504、三爪夹盘，600、柔性多铰链，601、第一底座，602、第一转柱，603、阻尼器，604、铰杆，605、探头，700、扫描器，701、第二底座，702、第二转柱，703、扫描头。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本实施例提供了一种六自由度测量系统及方法。如图1和图2所示，测量系统包括跟踪测量装置100和靶标200。跟踪测量装置100包括处理器101，以及分别连接处理器101的相机102和两个旋转镜片模块，相机102和两个旋转镜片模块前后依次排列，且相机102和两个旋转镜片模块的光轴保持同轴；每个旋转镜片模块可独立旋转，且相机102和两个旋转镜片模块的光轴保持同轴。

如图3所示，每个旋转镜片模块可独立旋转，视轴108经过第一个棱镜107发生折射，经过第二个棱镜107再次折射获得调整视轴109，随着第一个棱镜107与第二棱镜107的旋转，可获得不同角度的调整视轴109。旋转镜片模块包括驱动单元104、传动单元105、角度测量器106和棱镜107。如图4所示，该棱镜107的截屏为直角梯形的镜片。角度测量器106和棱镜107均安装在传动单元105上，驱动单元104连接传动单元105，处理器101连接驱动单元104。光线通过两个旋转镜片模块所形成的光学扫描域1082不小于相机成像视场域1080，相机102与两个旋转镜片模块共同形成的组合成像视场域1081不小于相机成像视场域1080。相机102和旋转镜片模块之间还设置有滤光片103。

传动单元105的传动方式采用齿轮传动、蜗轮蜗杆传动、同步带传动或链传动中任一种。棱镜107可以采用不同折射率光学透射材料，如光学玻璃、光学晶体、光学塑料(如PC、PMMA等)、陶瓷、金属中任一种或几种。驱动单元104为伺服电机、步进电机或力矩电机任一种。

如图1和图5所示，靶标200包括套筒和标志环201。所述标志环201包括标志轴2012和套环2013。套环2013嵌套在套筒的外径上，每个套环2013上径向外圈分布标志轴2012。标志点2011设置在标志轴2012悬空的一端。靶标200安装在待测目标上，包括至少三个距离关系已知的标志点2011，并且至少一个标志点2011与其余标志点2011的连线的中点不重合。标志点2011采用球状红外发光二级管。本实施例中，靶标200固装有第一标志点2011A、第二标志点2011B和第三标志点2011C，第一标志点2011A和第二标志点2011B之间的距离为30mm，第二标志点2011B和第三标志点2011C之间的距离为40mm，第一标志点2011A和第三标志点2011C之间的距离为55mm，靶标200固装在待测目标300上。

如图6所示，该六自由度测量方法为：

步骤S1、建立世界坐标系O_w-X_wY_wZ_w，建立相机坐标系O_c-X_cY_cZ_c，建立靶标坐标系O_t-X_tY_tZ_t，以及建立待测目标300坐标系O_a-X_aY_aZ_a。

步骤S2、采用合适的标定方法标定相机，所述的标定方法包括直接线性法、Tsai两步标定法、张正友标定法或神经网络标定法，记录相机102内参数和外参数，并在测量的过程中保持相机102不动。

步骤S3、处理器101控制旋转镜片模块旋转调整相机102视轴，使靶标200落入相机102的视场范围内。

步骤S4、相机102采集第一标志点2011A、第二标志点2011B和第三标志点2011C的图像，并将图像传输至处理器101。

步骤S5、处理器101获得图像后，进行图像处理，获得第一标志点2011A、第二标志点2011B和第三标志点2011C的图像坐标分别为(-0.462，0)、(0，0)、(0，0.616)。

步骤S6、如图7所示，已知此时旋转镜片模块的转角，以第一标志点2011A、第二标志点2011B和第三标志点2011C对应的图像M₁、M₂和M₃分别和相机102焦点F的连线为入射投影光线，通过矢量折射定理获得入射投影光线在旋转镜片模块的投影光线出射点K1、K2和K3，以及出射投影光线向量S1、S2和S3，得到在测量系统坐标系O_c-X_cY_cZ_c中，K1坐标为(-1.471，-2.616，33)，K2坐标为(-0.6514，-2.1373，33)，K3坐标为(-0.6311，-1.043，33)，向量S1为(0.0244，-0.0486，0.9985)，向量S2为(0.0528，-0.0456，0.997)，向量S3为(0.0526，-0.0095，0.9986)。

步骤S7、计算获取标志点2011的三维坐标，i＝1～n，可由如下方程组求解：

其中，X_pi、Y_pi、Z_pi为第i个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，X_ki、Y_ki、Z_ki为K_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，X_si、Y_si、Z_si为S_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，i＝1～3，X_pj、Y_pj、Z_pj为第j个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，j＝1～3，D_ij为第i个标志点与第j个标志点的距离。

求得在测量系统坐标系O_c-X_cY_cZ_c下，第一标志点2011A的三维坐标(22.36，-49.63，1008.2)，第二标志点2011B的三维坐标(50.5，-46.3，998.3)和第三标志点2011C的三维坐标(49.32，-10.0651，981.3)。

步骤S8、三个标志点2011所在平面法向量可由下式求解：

式中，P₁为第一标志点，P₂为第二标志点，P₃为第三标志点，N为第一标志点、第二标志点和第三标志点所在平面法向量；获得第一标志点2011A、第二标志点2011B和第三标志点2011C所在平面法向量为(301.8,489.1,1021.6)，同时求得在三个标志点2011所在平面内与

垂直的向量

以P₂为原点，

和

的方向向量为坐标方向建立坐标系，将靶标坐标系Ot-X_tY_tZ_t与此坐标系重合。

步骤S9、通过方向余弦矩阵求解靶标坐标系O_t-X_tY_tZ_t相对测量系统坐标系O_c-X_cY_cZ_c的姿态矩阵R；

优选的，方向余弦矩阵为

其中i，j，k分别为靶标坐标系O_t-X_tY_tZ_t对应的单位方向向量；I，J，K分别为测量系统坐标系O_c-X_cY_cZ_c的对应的单位方向向量，方向余弦矩阵中每一个元素值为两个单位方向向量夹角的余弦值，得到：

步骤S10、已知第一标志点2011A、第二标志点2011B和第三标志点2011C的图像坐标，通过针孔成像模型，求解靶标坐标系O_t-X_tY_tZ_t相对测量系统坐标系O_c-X_cY_cZ_c的平移矩阵T；

T＝[50.5 -46.3 998.3]；

步骤S11、待测目标300相对世界坐标系O_w-X_wY_wZ_w的姿态矩阵可由下式得出：

M＝M₁M₂M₃；

其中M₁为相机102相对世界坐标系O_w-X_wY_wZ_w的位姿转换矩阵，

M₃为待测目标坐标系O_a-X_aY_aZ_a相对靶标坐标系O_t-X_tY_tZ_t的位姿转换矩阵。由于待测目标的世界坐标系O_w-X_wY_wZ_w和测量系统坐标系O_c-X_cY_cZ_c重合，靶标坐标系O_t-X_tY_tZ_t和待测目标坐标系O_a-X_aY_aZ_a重合，可得的待测目标300相对世界坐标系O_w-X_wY_wZ_w下的位姿转换矩阵为：

步骤S12、待测目标300运动时，处理器101通过视觉跟踪算法控制旋转镜片模块旋转调整相机102视轴，使靶标200保持在相机102的视场范围内，重复S4至S11操作。

优选的，视觉跟踪算法采用在已知当前旋转镜片模块转角的条件下，采集标志点2011中任意一点和相机102焦点F的连线为旋转镜片模块的入射投影光线向量，通过矢量光线追迹法求得旋转镜片模块的出射投影光线向量，已知出射投影光线向量再采用视轴逆向查表法求解旋转镜片模块的转角。

视轴逆向查表法的制表步骤如下：

确定棱镜107旋转步长θ_st，本实施例中为0.1度，将棱镜107的转动角度分为360°/θ_st份，计算获得不同转角下的射光向量，建立转角和出射光单位向量查找表，其计算表达式为：

其中，A_in表示入射光单位向量，A_out表示折射光单位向量；N表示折射面的法向量；n₁为入射介质的折射率，n₂为出射介质的折射率；

式中，θ_ri为第i个棱镜107转角，α为棱镜107楔角。

视轴逆向查表法的查表步骤如下：

根据当前旋转镜片模块的出射光线单位向量，在所建立的表中搜索最接近的出射光向量，即使得向量误差Δ达到最小值时所对应的棱镜107转角值；Δ的表达式为：

其中，X_rp、Y_rp、Z_rp为表中记录的出射光单位向量坐标，x_rp、y_rp、z_rp为当前棱镜107出射光单位向量坐标。

本实施例的工作原理为：通过六自由度测量方法获得靶标200在测量系统坐标系O_c-X_cY_cZ_c下的位姿矩阵，再通过测量系统坐标系O_c-X_cY_cZ_c相对世界坐标系O_w-X_wY_wZ_w的位姿转换矩阵，待测目标坐标系O_a-X_aY_aZ_a相对靶标坐标系O_t-X_tY_tZ_t的位姿转换矩阵，最终得到待测目标300相对世界坐标系O_w-X_wY_wZ_w的姿态矩阵，处理器101通过视觉跟踪算法控制驱动单元104，驱动单元104通过传动单元105驱动旋转棱镜107调整相机102视轴，实现对靶标200的跟踪，进而实现对待测目标300的实时动态测量。

系统精度分析：

本实施例的测量方法要获得高精度测量结果，需满足以下条件：通过滤光片103滤掉有害波段光线；相机镜头1022质量较好，图像的畸变可矫正；通过误差补偿方法将相机系统误差去除；标志点2011在相机感光芯片1021中的成像应完全填满在半径为1000像素区域内。图8为测量精度分析示意图，令S为相机镜头1022到标志点2011的距离，f为相机102的焦距，D为标志点2011的最大外形尺寸，N为标志点2011的最大外形尺寸对应像素数量。S、f、D和N的关系为：

因此测量精度可由下式决定：

N为1000像素，单位像素值为0.0044mm，dD为标志点的加工误差取0.001mm，当f取16mm时，dS为3.6um，即测量精度可达3.6um，随着测量距离的增加，f取值越大，测量精度降低。

本实施例中测量方法和系统的跟踪功能主要由处理器、驱动单元、传动单元、角度测量器和棱镜配合实现。棱镜的制造误差和装配误差主要影响跟踪光轴指向精度。以双棱镜为例，假设两棱镜的楔角误差分别为Δα₁和Δα₂，则两棱镜的实际楔角分别为α+Δα₁和α+Δα₂；两棱镜的折射率误差分别为Δn₁和Δn₂，则两棱镜的实际折射率分别为n+Δn₁和n+Δn₂。取棱镜楔角理论值为α＝20°，折射率理论值为n＝1.517，图9和图10分别为跟踪光轴指向范围内最大误差Δ_max随楔角误差Δα和折射率误差Δn的变化关系。显然，对于两个棱镜，随着楔角误差和折射率误差的增加，视轴指向误差均增大。

棱镜装配误差也直接影响着视轴指向精度，根据棱镜装配误差的表现形式，可将棱镜装配误差分为棱镜倾斜误差和轴承倾斜误差，如图11a和图11b所示。棱镜倾斜误差可通过倾斜轴线角β_p和倾斜误差δ_p来表示。轴承倾斜误差可通过倾斜轴线角β_b和倾斜误差δ_b来表示。棱镜和轴承的倾斜轴线可表示为U₁＝(cosβ_p,sinβ_p,0)^T和U₂＝(sinβ_bsinδ_b,cosβ_bsinδ_b,cosδ_b)^T。根据Rodrigues旋转公式和矢量折射定律，可得到棱镜倾斜光轴指向误差。取棱镜楔角α＝20°，折射率n＝1.517，棱镜倾斜轴线角β_p范围为0°～360°，倾斜误差为δ_p＝1″，图12为光轴指向范围内最大误差Δ_max随棱镜倾斜轴线角β_p的变化关系。当棱镜倾斜轴线角β_p＝90°时，两棱镜倾斜误差引起的视轴指向误差Δ_max均最大，分别为0.499μrad和0.437μrad。轴承倾斜轴线角β_b范围为0°～360°，倾斜误差为δ_b＝1″，图13为成像视轴指向范围内最大光轴指向误差Δ_max随轴承倾斜轴线角βb的变化关系。当轴承倾斜轴线角β_b＝90°时，两棱镜轴承倾斜误差引起的视轴指向误差Δ_max均最大，分别为0.998μrad和0.874μrad。

在不同的实施例中，靶标还可以具有以下多种形式。

一、如图14和图15所示，靶标200包括标志环201、套筒一202和固定座203。套筒一202固装在固定座203上，至少三个标志环201套装在套筒一202上，且相邻三个标志环201中至少有一个标志环201的外径与另外标志环201外径不相等。

二、如图16所示，探针300为靶标200的另一种布置形式，探针300包括标志环201、套筒二301和针头302。至少三个标志环201套装在套筒二301上，且相邻三个标志环201中至少有一个标志环201的外径与另外两个标志环201外径不相等，针头固装在套筒二301底部。其它组成、连接关系及测量方法与上述具体实施方式相同。

三、如图17所示，基孔靶标400为靶标200的另一种布置形式，基孔靶标400包括第一顶部标志点401、支柱一402、标志环201、套筒三403、固定座203和插针405。支柱一402固装在套筒三403顶部，第一顶部标志点401固装在支柱一402顶部，至少一个标志环201套装在套筒三403上，固定座203固装在套筒三403底部，插针405固装在固定座203中部。固定座203为磁力吸盘或负压吸盘。其它组成、连接关系及测量方法与上述具体实施方式相同。

四、如图18所示，基轴靶标500为靶标200的另一种布置形式，基轴靶标500包括第二顶部标志点501、支柱二502、标志环201、套筒四503和三爪夹盘504。支柱二502固装在套筒四503上，第二顶部标志点501固装在支柱二502顶部，至少一个标志环201套装在套筒四503上，三爪夹盘504固装在套筒四503底部。其它组成、连接关系及测量方法与上述具体实施方式相同。

五、如图19所示，柔性多铰链600为靶标200的另一种布置形式，柔性多铰链600包括第一底座601、第一转柱602、标志环201、阻尼器603、铰杆604和探头605。第一转柱602与第一底座601转动连接，至少三个标志环201套装在第一转柱602上，且相邻三个标志环201中至少有一个标志环201的外径与另外两个标志环201外径不相等，多个铰杆604首尾铰接，且铰杆604直接固装阻尼器603，首个铰杆604的首端与第一转柱602铰接，尾端铰杆604的末端与探头605连接。其它组成、连接关系及测量方法与上述具体实施方式相同。

六、如图20所示，扫描器700为靶标200的另一种布置形式，扫描器700包括第二底座701、第二转柱702、标志环201和扫描头703。第二转柱702与第二底座701转动连接，至少三个标志环201套装在第二转柱702上，且相邻三个标志环201中至少有一个标志环201的外径与另外两个标志环201外径不相等，扫描头703与第二转柱702铰接。其它组成、连接关系及测量方法与上述具体实施方式相同。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (7)

1.一种六自由度测量系统，其特征在于，包括跟踪测量装置（100）和靶标（200）；

所述的跟踪测量装置（100）包括处理器（101），以及分别连接处理器（101）的相机（102）和两个旋转镜片模块，所述的相机（102）和两个旋转镜片模块依次排列，且相机（102）和两个旋转镜片模块的光轴保持同轴；每个旋转镜片模块可独立旋转，旋转镜片模块包括驱动单元（104）、传动单元（105）、角度测量器（106）和棱镜（107）、所述的棱镜（107）为截面为直角梯形的镜片，所述的角度测量器（106）和棱镜（107）均安装在传动单元（105）上，所述驱动单元（104）连接传动单元（105），所述处理器（101）连接驱动单元（104）；

所述的靶标（200）安装在待测目标（300）上，包括至少三个距离关系已知的标志点（2011），并且至少一个标志点（2011）与其余标志点（2011）的连线的中点不重合；靶标（200）包括套筒和标志环（201），所述的标志环（201）包括标志轴（2012）和套环（2013），套环（2013）嵌套在套筒的外径上，每个套环（2013）上径向外圈分布标志轴（2012），所标志点（2011）设置在标志轴（2012）悬空的一端；

通过该六自由度测量系统进行六自由度测量方法，具体步骤如下：

S1、分别建立世界坐标系O_w-X_wY_wZ_w、相机坐标系O_c-X_cY_cZ_c、靶标坐标系O_t-X_tY_tZ_t和待测目标坐标系O_a-X_aY_aZ_a；

S2、对相机（102）进行标定，获取相机（102）的内参数和外参数；

S3、控制旋转镜片模块旋转调整相机（102）的视轴（108），使靶标（200）落入相机（102）的视场范围内；

S4、相机（102）采集至少包括三个标志点（2011）的图像，进行图像处理，获得标志点（2011）的图像坐标（X_i，Y_i），i=1~n；

S5、获取旋转镜片模块的转动角度，以标志点（2011）对应的图像坐标分别和相机（102）焦点F的连线为入射投影光线，通过矢量折射定理获得入射投影光线在旋转镜片模块的投影光线出射点K_i，i=1~n，以及出射投影光线向量S_i，i=1~n；

S6、计算获取标志点（2011）的三维坐标，可由如下方程组求解：

其中，X_pi、Y_pi、Z_pi为第i个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标；X_ki、Y_ki、Z_ki为K_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标；X_si、Y_si、Z_si为S_i在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，i=1~n；X_pj、Y_pj、Z_pj为第j个标志点在测量系统坐标系O_c-X_cY_cZ_c下的三维坐标，j=1~n；D_ij为第i个标志点与第j个标志点的距离；

为第i个方程比例系数；

S7、所采集标志点中选取三个标志点（2011）且其中至少一个标志点（2011）与其他两个标志点（2011）连线的中点不重合，其所在平面法向量可由下式求解：

式中，P₁为第一标志点，P₂为第二标志点，P₃为第三标志点，N为第一标志点、第二标志点和第三标志点所在平面法向量，同时求得在三个标志点（2011）所在平面内与

垂直的向量

，以P₂为原点，

、

和

的方向向量为坐标方向建立坐标系，将靶标坐标系Ot-X_tY_tZ_t与此坐标系重合；

S8、通过方向余弦矩阵求解靶标坐标系O_t-X_tY_tZ_t相对相机坐标系O_c-X_cY_cZ_c的姿态矩阵R；

S9、通过三个标志点（2011）的图像坐标，使用针孔成像模型，求解靶标坐标系O_t-X_tY_tZ_t相对相机坐标系O_c-X_cY_cZ_c的平移矩阵T；

S10、获取待测目标（300）相对世界坐标系O_w-X_wY_wZ_w的姿态矩阵，其表达式为：

其中，M ₁为相机（102）相对世界坐标系O_w-X_wY_wZ_w的位姿转换矩阵，

，M ₃为待测目标坐标系O_a-X_aY_aZ_a相对靶标坐标系O_t-X_tY_tZ_t的位姿转换矩阵；

S11、待测目标运动时，通过视觉跟踪算法控制旋转镜片模块旋转调整相机（102）的视轴（108），使靶标（200）保持在相机（102）的视场范围内，重复S4至S10。

2.根据权利要求1所述的六自由度测量系统，其特征在于，所述的相机（102）和旋转镜片模块之间设置有滤光片（103）。

3.根据权利要求1所述的六自由度测量系统，其特征在于，所述的标志点（2011）为球状红外发光二级管。

4.根据权利要求1所述的六自由度测量系统，其特征在于，光线通过两个旋转镜片模块所形成的光学扫描域（1082）不小于相机成像视场域（1080），相机（102）与两个旋转镜片模块共同形成的组合成像视场域（1081）不小于相机成像视场域（1080）。

5.根据权利要求1所述的六自由度测量系统，其特征在于，所述的步骤S11 中，视觉跟踪算法具体为：在已知当前旋转镜片模块转角的条件下，采集标志点（2011）中任意一点和相机（102）焦点F的连线为旋转镜片模块的入射投影光线向量，通过矢量光线追迹法求得旋转镜片模块的出射投影光线向量，通过出射投影光线向量再采用视轴逆向查表法求解旋转镜片模块的转角。

6.根据权利要求5所述的六自由度测量系统，其特征在于，所述的视轴逆向查表法的制表步骤如下：

确定棱镜（107）旋转步长

，将棱镜（107）的转动角度分为360°/

份，计算获得不同转角下的射光向量，建立转角和出射光单位向量查找表，其计算表达式为：

其中，A _in 表示入射光单位向量，A _out 表示折射光单位向量；N表示折射面的法向量；n ₁ 为入射介质的折射率，n ₂ 为出射介质的折射率；

式中，

为第i个棱镜（107）转角，

为棱镜（107）楔角。

7.根据权利要求6所述的六自由度测量系统，其特征在于，所述的视轴逆向查表法的查表步骤如下：

根据当前旋转镜片模块的出射光线单位向量，在所建立的表中搜索最接近的出射光向量，即使得向量误差

达到最小值时所对应的棱镜（107）转角值；

的表达式为：

其中，

、

、

为表中记录的出射光单位向量坐标，

、

、

为当前棱镜（107）出射光单位向量坐标。

Images