[发明专利]一种六自由度测量系统及方法

权利要求书

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。