1.一种基于加权最小二乘的激光跟踪姿态测量系统,其特征在于,包括:激光跟踪测量单元、单目视觉测量单元以及靶标;其中,所述激光跟踪测量单元和所述单目视觉测量单元分别固定在预设位置,所述靶标固定放置在被测物上;所述靶标包括角锥棱镜、光电位置传感器以及多个靶点;所述角锥棱镜在顶点处开孔,所述光电位置传感器的感光面与角锥棱镜开孔面平行,所述角锥棱镜的顶点与光电位置传感器之间预留一定间距,所述激光跟踪测量单元所发出激光通过所述角锥棱镜开孔打在所述光电位置传感器的感光面上形成光斑,所述多个靶点的数量不少于4个,且所述不少于4个的靶点非共面;
所述激光跟踪测量单元用于获取所述角锥棱镜顶点在第一坐标系中的坐标;其中,所述第一坐标系为所述激光跟踪测量单元的自身坐标系;
所述单目视觉测量单元用于通过所述多个靶点获取所述靶标在第二坐标系中的第一姿态角;其中,所述第二坐标系为所述单目视觉测量单元的自身坐标系;
所述光电位置传感器用于获取所述光斑在第三坐标系中的坐标;其中,所述第三坐标系为所述光电位置传感器的自身坐标系;
所述第三坐标系下的坐标可结合所述角锥棱镜的顶点与光电位置传感器之间间距转换为第四坐标系下的坐标;其中,所述第四坐标系的X、Y轴与所述第二坐标系X、Y轴平行,Z轴方向指向所述光电位置传感器感光面中心,原点位于角锥棱镜顶点;
根据所述角锥棱镜顶点在所述第一坐标系中的坐标,获取所述激光束在所述第一坐标系中对应的第一空间向量;根据所述光斑在所述第四坐标系中的坐标,获取所述激光束在所述第四坐标系中对应的第二空间向量;根据所述第一空间向量、所述第二空间向量以及所述靶标在第二坐标系中的第一姿态角,获取所述靶标在所述第一坐标系中的第二姿态角,进而获得所述被测物的姿态角。
2.根据权利要求1所述基于加权最小二乘融合的激光跟踪姿态测量系统,其特征在于,所述激光跟踪测量单元为全站仪或激光跟踪仪,所述单目视觉测量单元为工业相机。
3.根据权利要求1所述基于加权最小二乘融合的激光跟踪姿态测量系统,其特征在于,所述不少于4个的非共面靶点由反光标志或红外LED制成。
4.一种利用权利要求1‑3所述基于加权最小二乘融合的激光跟踪姿态测量的方法,其特征在于,包括:
将所述靶标固定放置在被测物上,将所述激光跟踪测量单元与所述单目视觉测量单元分别固定在预设位置上,通过标定获取第一坐标系与第二坐标系之间的转换矩阵;
利用所述光电位置传感器获取所属光斑在所述第三坐标系下的坐标,利用所述单目视觉测量单元通过所述不少于4个的非共面靶点获取所述靶标在所述第二坐标系下的第一姿态角,利用所述激光跟踪测量单元获取所述角锥棱镜的顶点在第一坐标系下的坐标;
根据所述光斑在所述第三坐标系下的坐标和所述角锥棱镜的顶点与光电位置传感器之间间距,获取所述光斑在所述第四坐标系下的坐标;
根据所述角锥棱镜的顶点在所述第一坐标系下的坐标,获取所述激光在所述第一坐标系中对应的第一空间向量,根据所述光斑在所述第四坐标系下的坐标,获取所述激光在所述第四坐标系中对应的第二空间向量;
根据所述靶标在所述第二坐标系下的第一姿态角、所述第一空间向量、所述第二空间向量以及所述第一坐标系与第二坐标系之间的转换矩阵,获取所述靶标在所述第一坐标系下的第二姿态角;
将所述第一姿态角、所述第二姿态角转换到同一坐标系下,通过加权最小二乘融合获得最终姿态角。
5.根据权利要求4所述方法,其特征在于,所述利用所述单目视觉测量单元通过所述不少于4个的非共面靶点获取所述靶标在所述第二坐标系下的第一姿态角,具体包括:在进行姿态角测量时,首先利用所述单目视觉测量单元获取所述靶标的像素坐标,通过EPNP算法进行二维点到三维点的匹配,获取所述第二坐标系与所述第四坐标系之间的转换矩阵与位移向量;
将所述转换矩阵与位移向量作为初始值,通过SoftPOSIT算法完成后续实时状态下的姿态解算,获取单目视觉测量时姿态角,即所述靶标在所述第二坐标系下的第一姿态角。
6.根据权利要求4所述方法,其特征在于,所述根据所述光斑在所述第三坐标系下的坐标和所述角锥棱镜的顶点与光电位置传感器之间间距,获取所述光斑在所述第四坐标系下的坐标,具体包括:
将所述光斑在所述第三坐标系下的横坐标与纵坐标,分别作为所述光斑在第四坐标系下的横坐标与纵坐标,将所述角锥棱镜的顶点与光电位置传感器之间间距作为所述光斑的Z轴坐标,即所述光斑在所述第四坐标系下的坐标。
7.根据权利要求4所述方法,其特征在于,所述根据所述角锥棱镜的顶点在所述第一坐标系下的坐标,获取所述激光在所述第一坐标系中对应的第一空间向量,根据所述光斑在所述第四坐标系下的坐标,获取所述激光在所述第四坐标系中对应的第二空间向量,具体包括:
将所述第一坐标系的原点作为起点,所述角锥棱镜的顶点作为终点,获取所述第一空间向量,将所述第四坐标系的原点作为起点,所述光斑作为终点,获取所述第二空间向量。
8.根据权利要求4所述方法,其特征在于,所述根据所述靶标在所述第二坐标系下的第一姿态角、所述第一空间向量、所述第二空间向量以及所述第一坐标系与第二坐标系之间的转换矩阵,获取所述靶标在所述第一坐标系下的第二姿态角,具体包括:对所述第一空间向量和所述第二空间向量分别进行单位化处理,得到第一单位向量和第二单位向量;
根据所述靶标在所述第二坐标系下的第一姿态角、所述第一单位向量、所述第二单位向量以及所述第一坐标系与第二坐标系之间的转换矩阵,基于光束向量在不同坐标系下的转换关系,获取靶标在所述第一坐标系下的第二姿态角。
9.根据权利要求4所述方法,其特征在于,所述将所述第一姿态角、所述第二姿态角转换到同一坐标系下,通过加权最小二乘融合获得最终姿态角,具体包括:将所述第一姿态角通过所述第一坐标系与第二坐标系之间的转换矩阵转换到第一坐标系下,获取第三姿态角;
假设所述第二姿态角、所述第三姿态角为传感器所测结果,将两个传感器对于姿态角的观测方程设置为:
Y=Hx+e (1)T
式中:x为一维状态量;Y为二维测量向量,设Y=[y1,y2] ;e为二维测量噪声向量,包含T
传感器的内部噪声和环境干扰噪声,设e=[e1,e2] ,若传感器的测量方差为Ri,i=1,2,则E
2 2 T
[ei]=E[(x‑yi) ]=Ri;H为已知的二维常向量,设H=[1,1]。
采用加权最小二乘法从测量向量Y中估计出状态量x的估计量 加权最小二乘法估计的准则是使加权误差平方和 取极小值,对 求偏导数并令其等于零得加权最小二乘估计:
设 为估计误差,则 可表示为:
x=x‑x (3)由式(2)和式(3),可得估计方差为:对式(4)求极小值,取wi的偏导数,并令其等于零得将(5)代入式(4),可得估计误差为由式(6)不难看出,采用加权融合的估计方差比任何一个传感器的测量方差都小。
通过上述过程进行加权最小二乘融合,获取最终姿态角。