CN113028990A - 一种基于加权最小二乘的激光跟踪姿态测量系统及方法 - Google Patents

Abstract

本发明提供了一种基于加权最小二乘的激光跟踪姿态测量系统及方法，包括：激光跟踪测量单元、单目视觉测量单元以及靶标，靶标包括角锥棱镜及光电位置传感器；激光跟踪测量单元和单目视觉测量单元分别固定在预设位置，靶标固定放置在被测物上；综合利用角锥棱镜与光电位置传感器测量方位角与俯仰角时的高分辨率、单目视觉测量横滚角时的高灵敏度，通过加权最小二乘，基于激光光束向量在不同坐标系之间的转换关系完成姿态角的测量。该测量系统及方法提高了大尺寸范围内激光跟踪姿态测量的整体精度，克服了单目姿态测量精度随距离增加线性递减的问题，同时改善了利用纵向投影比进行姿态测量时近距离精度较低的问题。

Description

一种基于加权最小二乘的激光跟踪姿态测量系统及方法

技术领域

本发明实施例涉及精密测量技术领域，更具体地，涉及一种基于 加权最小二乘融合的激光跟踪姿态测量系统及方法。

背景技术

随着我国国民经济和制造业的不断发展，诸如大型装备建造、航 空航天、汽车等领域，对于飞行器对接、汽车装配、机器人跟踪测量 等方面的姿态测量要求越来越高，大尺寸范围内高精度姿态测量技术 已经成为这些领域重要研究方向之一。

国内外都有对于姿态测量方面的研究，目前国外对于姿态测量方 法的研究较为成熟，已有产品进行商用的公司主要有三家：Leica、 FARO、API。Leica公司开发了T-Probe、T-MAC、T-Scan等一系列姿 态测量装置，采用驻机定位技术，配合激光跟踪仪、高精度相机实现 姿态角的测量；FARO公司提出了使用6Probe探头的方案，该探头上 布置有11个LED发光点，通过激光跟踪仪转台的双目系统进行跟踪 测量，可以覆盖隐藏区域和小型特征，即使在障碍物后边也能跟随移 动的目标，并可以在目标稳定时锁定它；API公司设计了STS特殊反 射器，配有2个编码器和1个水平传感器，根据编码器与水平传感器 确定位置和方向信息；国内华中科技大学、上海交通大学、天津大学 等基于盾构机施工中的导向系统，对激光跟踪姿态测量系统展开了研 究，通过全站仪、CCD相机、角锥棱镜、倾角仪等实现了姿态测量； 湖北工业大学的张慧娟提出了一种单目视觉自动姿态测量方法，将 EPNP与SoftPOSIT相结合，充分利用两种算法的特性实现姿态测量； 湖北工业大学的闫坤将光电位置传感器与单目视觉进行组合，基于特 定靶点的纵向投影比获取合作目标的滚动角度，通过坐标系之间的转 换关系获取姿态角。

现有技术中以激光跟踪设备和单目视觉进行姿态测量时，测量精 度随着距离的增加呈现线性递减的趋势；利用纵向投影比进行姿态测 量时，测量精度不随距离的变化而变化，但近距离测量精度较低。

发明内容

本发明实施例提供了一种克服上述问题或者至少部分地解决上述 问题的基于加权最小二乘融合的激光跟踪姿态测量系统及方法。

一方面，本发明实施例提供了一种基于加权最小二乘融合的激光 跟踪姿态测量系统，其特征在于，包括：激光跟踪测量单元、单目视 觉测量单元以及靶标；其中，

所述激光跟踪测量单元和所述单目视觉测量单元分别固定在预设 位置，所述靶标固定放置在被测物上；所述靶标包括角锥棱镜、光电 位置传感器以及多个靶点；所述角锥棱镜在顶点处开孔，所述光电位 置传感器的感光面与角锥棱镜开孔面平行，所述角锥棱镜的顶点与光 电位置传感器之间预留一定间距，所述激光跟踪测量单元所发出激光 通过所述角锥棱镜开孔打在所述光电位置传感器的感光面上形成光 斑，所述多个靶点的数量不少于4个，且所述不少于4个的靶点非共 面；

所述激光跟踪测量单元用于获取所述角锥棱镜顶点在第一坐标系 中的坐标；其中，所述第一坐标系为所述激光跟踪测量单元的自身坐 标系；

所述单目视觉测量单元用于通过所述多个靶点获取所述靶标在第 二坐标系中的第一姿态角；其中，所述第二坐标系为所述单目视觉测 量单元的自身坐标系；

所述光电位置传感器用于获取所述光斑在第三坐标系中的坐标； 其中，所述第三坐标系为所述光电位置传感器的自身坐标系；

所述第三坐标系下的坐标可结合所述角锥棱镜的顶点与光电位置 传感器之间间距转换为第四坐标系下的坐标；其中，所述第四坐标系 的X、Y轴与所述第二坐标系X、Y轴平行，Z轴方向指向所述光电位 置传感器感光面中心，原点位于角锥棱镜顶点；

根据所述角锥棱镜顶点在所述第一坐标系中的坐标，获取所述激 光束在所述第一坐标系中对应的第一空间向量；根据所述光斑在所述 第四坐标系中的坐标，获取所述激光束在所述第四坐标系中对应的第 二空间向量；根据所述第一空间向量、所述第二空间向量以及所述靶 标在第二坐标系中的第一姿态角，获取所述靶标在所述第一坐标系中 的第二姿态角，进而获得所述被测物的姿态角。

进一步地，所述激光跟踪测量单元为全站仪或激光跟踪仪，所述 单目视觉测量单元为工业相机。

进一步地，所述不少于4个的非共面靶点由反光标志或红外LED 制成。

另一方面本发明实例提供了一种基于加权最小二乘的激光跟踪姿 态测量的方法，其特征在于，包括：

将所述靶标固定放置在被测物上，将所述激光跟踪测量单元与所 述单目视觉测量单元分别固定在预设位置上，通过标定获取第一坐标 系与第二坐标系之间的转换矩阵；

利用所述光电位置传感器获取所属光斑在所述第三坐标系下的坐 标，利用所述单目视觉测量单元通过所述不少于4个的非共面靶点获 取所述靶标在所述第二坐标系下的第一姿态角，利用所述激光跟踪测 量单元获取所述角锥棱镜的顶点在第一坐标系下的坐标；

根据所述光斑在所述第三坐标系下的坐标和所述角锥棱镜的顶点 与光电位置传感器之间间距，获取所述光斑在所述第四坐标系下的坐 标；

根据所述角锥棱镜的顶点在所述第一坐标系下的坐标，获取所述 激光在所述第一坐标系中对应的第一空间向量，根据所述光斑在所述 第四坐标系下的坐标，获取所述激光在所述第四坐标系中对应的第二 空间向量；

根据所述靶标在所述第二坐标系下的第一姿态角、所述第一空间 向量、所述第二空间向量以及所述第一坐标系与第二坐标系之间的转 换矩阵，获取所述靶标在所述第一坐标系下的第二姿态角；

将所述第一姿态角、所述第二姿态角转换到同一坐标系下，通过 加权最小二乘融合获得最终姿态角。

进一步地，利用所述单目视觉测量单元通过所述不少于4个的非 共面靶点获取所述靶标在所述第二坐标系下的第一姿态角，具体包括：

在进行姿态角测量时，首先利用所述单目视觉测量单元获取所述 靶标的像素坐标，通过EPNP算法进行二维点到三维点的匹配，获取 所述第二坐标系与所述第四坐标系之间的转换矩阵与位移向量；

将所述转换矩阵与位移向量作为初始值，通过SoftPOSIT算法完 成后续实时状态下的姿态解算，获取单目视觉测量时姿态角，即所述 靶标在所述第二坐标系下的第一姿态角。

进一步地，根据所述光斑在所述第三坐标系下的坐标和所述角锥 棱镜的顶点与光电位置传感器之间间距，获取所述光斑在所述第四坐 标系下的坐标，具体包括：

将所述光斑在所述第三坐标系下的横坐标与纵坐标，分别作为所 述光斑在第四坐标系下的横坐标与纵坐标，将所述角锥棱镜的顶点与 光电位置传感器之间间距作为所述光斑的Z轴坐标，即所述光斑在所 述第四坐标系下的坐标。

进一步地，根据所述角锥棱镜的顶点在所述第一坐标系下的坐标， 获取所述激光在所述第一坐标系中对应的第一空间向量，根据所述光 斑在所述第四坐标系下的坐标，获取所述激光在所述第四坐标系中对 应的第二空间向量，具体包括：

将所述第一坐标系的原点作为起点，所述角锥棱镜的顶点作为终 点，获取所述第一空间向量，将所述第四坐标系的原点作为起点，所 述光斑作为终点，获取所述第二空间向量。

进一步地，根据所述靶标在所述第二坐标系下的第一姿态角、所 述第一空间向量、所述第二空间向量以及所述第一坐标系与第二坐标 系之间的转换矩阵，获取所述靶标在所述第一坐标系下的第二姿态角， 具体包括：

对所述第一空间向量和所述第二空间向量分别进行单位化处理， 得到第一单位向量和第二单位向量；

根据所述靶标在所述第二坐标系下的第一姿态角、所述第一单位 向量、所述第二单位向量以及所述第一坐标系与第二坐标系之间的转 换矩阵，基于光束向量在不同坐标系下的转换关系，获取靶标在所述 第一坐标系下的第二姿态角；

进一步地，将所述第一姿态角、所述第二姿态角转换到同一坐标 系下，通过加权最小二乘融合获得最终姿态角，具体包括：

将所述第一姿态角通过所述第一坐标系与第二坐标系之间的转换 矩阵转换到第一坐标系下，获取第三姿态角；

假设所述第二姿态角、所述第三姿态角为传感器所测结果，将两个 传感器对于姿态角的观测方程设置为：

Y＝Hx+e (7)

式中：x为一维状态量；Y为二维测量向量，设Y＝[y₁,y₂]^T；e为二 维测量噪声向量，包含传感器的内部噪声和环境干扰噪声，设 e＝[e₁,e₂]^T，若传感器的测量方差为R_i,i＝1,2，则E[e_i ²]＝E[(x-y_i)²]＝R_i；H 为已知的二维常向量，设H＝[1,1]^T。

采用加权最小二乘法从测量向量Y中估计出状态量x的估计量

加权最小二乘法估计的准则是使加权误差平方和

取极小值，对

求偏导数并令其等于零得加 权最小二乘估计：

设

为估计误差，则

可表示为：

x＝x-x (9)

由式(8)和式(9)，可得估计方差为：

对式(10)求极小值，取w_i的偏导数，并令其等于零得

将(11)代入式(10)，可得估计误差为

由式(12)不难看出，采用加权融合的估计方差比任何一个传感 器的测量方差都小。

通过上述过程进行加权最小二乘融合，获取最终姿态角。

本发明实施例提供了一种基于加权最小二乘融合的激光跟踪姿态 测量系统及方法，通过使用加权最小二乘融合的方法，综合利用角锥 棱镜与光电位置传感器测量方位角与俯仰角时的高分辨率，单目视觉 测量横滚角时的高灵敏度，基于激光光束向量在不同坐标系之间的转 换关系完成姿态角的测量。该测量系统及方法提高了大尺寸范围内激 光跟踪姿态测量的整体精度，克服了单目姿态测量时精度随距离增加 线性递减的问题，同时改善了利用纵向投影比进行姿态测量时近距离 精度较低的问题。

附图说明

为了更清楚的说明本发明实施例或现有技术中的技术方案，下面 将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而 易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通 技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图 获得其他的附图。

图1为本发明实施例提供的一种基于加权最小二乘融合的激光跟 踪姿态测量系统示意图；

图2为本发明实施例提供的一种靶标的剖面图；

图3为本发明实施例提供的一种利用上述基于加权最小二乘融合 的激光跟踪姿态测量系统进行姿态角测量的方法的流程图。

图4为本发明实施例提供的以单目视觉自动测量、基于纵向投影 比结合光束向量在不同坐标系下的转换关系解算、EPNP与SoftPOSIT 联合解算并融合光束向量在不同坐标系下的转换关系解算三种方法的 仿真结果对比图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结 合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地 描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的 实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创 造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明实例提供了一种基于加权最小二乘融合的激光跟踪 姿态测量系统，包括：激光跟踪测量单元1、单目视觉测量单元2以及 靶标3；其中，

所述激光跟踪测量单元1和所述单目视觉测量单元2分别固定在 预设位置，所述靶标3固定放置在被测物上；如图2所示，所述靶标 包括角锥棱镜4、光电位置传感器5以及多个靶点6；所述角锥棱镜4 在顶点处开孔，所述光电位置传感器5的感光面与所述角锥棱镜4开 孔面平行，所述角锥棱镜4的顶点与所述光电位置传感器5之间预留 一定间距，所述激光跟踪测量单元1所发出激光通过所述角锥棱镜4 开孔打在所述光电位置传感器5的感光面上形成光斑，所述多个靶点6 的数量不少于4个，且所述不少于4个的靶点6非共面；

所述激光跟踪测量单元用于获取所述角锥棱镜顶点在第一坐标系 中的坐标；其中，所述第一坐标系为所述激光跟踪测量单元的自身坐 标系；

所述单目视觉测量单元用于通过所述多个靶点获取所述靶标在第 二坐标系中的第一姿态角；其中，所述第二坐标系为所述单目视觉测 量单元的自身坐标系；

所述光电位置传感器用于获取所述光斑在第三坐标系中的坐标； 其中，所述第三坐标系为所述光电位置传感器的自身坐标系；

所述第二坐标系下的坐标可结合所述角锥棱镜的顶点与光电位置 传感器之间间距转换为第四坐标系下的坐标；其中，所述第四坐标系 的X、Y轴与所述第二坐标系X、Y轴平行，Z轴方向指向所述光电位 置传感器感光面中心，原点位于角锥棱镜顶点。

根据所述角锥棱镜顶点在所述第一坐标系中的坐标，获取所述激 光束在所述第一坐标系中对应的第一空间向量；根据所述光斑在所述 第四坐标系中的坐标，获取所述激光束在所述第四坐标系中对应的第 二空间向量；根据所述第一空间向量、所述第二空间向量以及所述靶 标在第二坐标系中的第一姿态角，获取所述靶标在所述第一坐标系中 的第二姿态角，进而获得所述被测物的姿态角。

其中，第四坐标系可以理解为对应于靶标的坐标系，第四坐标系 在靶标运动(即被测物运动)时也跟随靶标做相应的运动，故第四坐 标系相对于第二坐标系的姿态角即为靶标在第二坐标系中的第一姿态 角。激光跟踪测量单元所发出激光通过角锥棱镜开孔打在光电位置传 感器上，该激光光束可以在第一坐标系与第四坐标系下分别表示，即 第一空间向量和第二空间向量，根据向量在不同坐标系之间的转换关 系即可以得到第四坐标系相对于第一坐标系的第二姿态角，进而得到 靶标在激光跟踪测量单元中的姿态角。

具体地，根据光斑在第三坐标系中的坐标结合预设距离，获取光 斑在第四坐标系中的坐标；根据角锥棱镜的顶点在第一坐标系中的坐 标，获取激光束在第三坐标系中对应的第一向量，根据光斑在第四坐 标系中的坐标，获取激光束在第四坐标系中对应的第二向量；根据第 一坐标向量，第二坐标向量以及靶标在第二坐标系中的第一姿态角， 获取靶标在第一坐标系的第二姿态角，将第一姿态角、第二姿态角转 换到同一坐标系下，通过加权最小二乘融合获得最终姿态角。

本发明实施例提供了一种基于加权最小二乘融合的激光跟踪姿态 测量系统，通过使用加权最小二乘融合的方法，综合利用角锥棱镜与 光电位置传感器测量方位角与俯仰角时的高分辨率，单目视觉测量横 滚角时的高灵敏度，基于激光光束向量在不同坐标系之间的转换关系 完成姿态角的测量。该测量系统及方法提高了大尺寸范围内激光跟踪 姿态测量的整体精度，克服了单目姿态测量时精度随距离增加线性递 减的问题，同时改善了利用纵向投影比进行姿态测量时近距离精度较 低的问题。

在上述实施例中，所述激光跟踪测量单元为全站仪或激光跟踪仪， 所述单目视觉测量单元为工业相机。

在上述实施例中，所述多个靶点的数量不少于4个且非共面，且 每个靶点由反光标志或红外LED制成。

具体地，每个靶点由反光标志或红外LED制成，在实际测量中， 可以根据具体需求进行选用。

图3为本发明实施例提供的一种利用上述基于加权最小二乘融合 的激光跟踪姿态测量系统进行姿态角测量的方法的流程图，如图3所 示，包括：

S301，将所述靶标固定放置在被测物上，将所述激光跟踪测量单 元与所述单目视觉测量单元分别固定在预设位置上，通过标定获取第 一坐标系与第二坐标系之间的转换矩阵；

S302，利用所述光电位置传感器获取所属光斑在所述第三坐标系 下的坐标，利用所述单目视觉测量单元通过所述不少于4个的非共面 靶点获取所述靶标在所述第二坐标系下的第一姿态角，利用所述激光 跟踪测量单元获取所述角锥棱镜的顶点在第一坐标系下的坐标；

S303，根据所述光斑在所述第三坐标系下的坐标和所述角锥棱镜 的顶点与光电位置传感器之间间距，获取所述光斑在所述第四坐标系 下的坐标；

S304，根据所述角锥棱镜的顶点在所述第一坐标系下的坐标，获 取所述激光在所述第一坐标系中对应的第一空间向量，根据所述光斑 在所述第四坐标系下的坐标，获取所述激光在所述第四坐标系中对应 的第二空间向量；

S305，根据所述靶标在所述第二坐标系下的第一姿态角、所述第 一空间向量、所述第二空间向量以及所述第一坐标系与第二坐标系之 间的转换矩阵，获取所述靶标在所述第一坐标系下的第二姿态角；

S306，将所述第一姿态角、所述第二姿态角转换到同一坐标系下， 通过加权最小二乘融合获得最终姿态角。

在步骤S301中，将所述靶标固定放置在被测物上，将所述激光跟 踪测量单元与所述单目视觉测量单元分别固定在预设位置上，通过标 定获取第一坐标系与第二坐标系之间的转换矩阵，其中标定方法可以 理解为现有技术，在此不再赘述。

在步骤S302-S305中，第四坐标系可以理解为对应于靶标的坐标 系，第四坐标系在靶标运动(即被测物运动)时也跟随靶标做相应的 运动，故第四坐标系相对于第二坐标系的姿态角即为靶标在第二坐标 系中的第一姿态角。激光跟踪测量单元所发出激光通过角锥棱镜开孔 打在光电位置传感器上，该激光光束可以在第一坐标系与第四坐标系 下分别表示，即第一空间向量和第二空间向量，根据向量在不同坐标 系之间的转换关系即可以得到第四坐标系相对于第一坐标系的第二姿 态角，进而得到靶标在激光跟踪测量单元中的姿态角。

本发明实施例提供了一种姿态角测量方法，通过使用加权最小二 乘融合的方法，综合利用角锥棱镜与光电位置传感器测量方位角与俯 仰角时的高分辨率，单目视觉测量横滚角时的高灵敏度，基于激光光 束向量在不同坐标系之间的转换关系完成姿态角的测量。该测量系统 及方法提高了大尺寸范围内激光跟踪姿态测量的整体精度，克服了单 目姿态测量时精度随距离增加线性递减的问题，同时改善了利用纵向 投影比进行姿态测量时近距离精度较低的问题。

在上述实施例中，所述利用所述单目视觉测量单元通过所述不少 于4个的非共面靶点获取所述靶标在所述第二坐标系下的第一姿态角， 具体包括：

在进行姿态角测量时，首先利用所述单目视觉测量单元获取所述 靶标的像素坐标，通过EPNP算法进行二维点到三维点的匹配，获取 所述第二坐标系与所述第四坐标系之间的转换矩阵与位移向量；

具体地，利用所述单目视觉测量单元采集所述靶标在第一状态下 的所述多个靶点的图像，利用预设图像处理算法对第一状态下的图像 进行处理，得到所述多个靶点的像素坐标(u_i,v_i),i＝1,2,3,…,n。假设靶标 中靶点的坐标为

右上标t用来标注在第四坐标系下的坐 标，虚拟控制点在第四坐标系下的非齐次坐标为

靶标靶点的坐标与虚拟控制点的坐标关系如 式(13)。由线性关系在欧式变换下的不变性可知，存在式(14)，右 上标c用来标注在相机坐标系下的坐标。

转换系数满足

为权重，即靶标特征点

在以 虚拟控制点

为基的欧式空间中的坐标。设像点坐标为(u_i,v_i)，则每个 靶标特征点和像点之间的对应关系可得到两个方程。

式中，f_x，f_y，u_x，u_y为相机的内部参数，当有n个空间和图像 点对应时，可得到含2n个方程的线性方程组，记为矩阵形式Mx＝0， 其中M是2n×12的矩阵，向量

为12×1的向量， 包含了n个虚拟控制点在第二坐标系下的非齐次坐标。根据欧式变换 的保距性，由n个靶标靶点之间的距离已知，即可求出n个靶标靶点 在第二坐标系下的坐标，进而求得第二坐标系与所述第四坐标系之间 的转换矩阵与位移向量。

将所述转换矩阵与位移向量作为初始值，通过SoftPOSIT算法完 成后续实时状态下的姿态解算，获取单目视觉测量时姿态角，即所述 靶标在所述第二坐标系下的第一姿态角。

具体地，由于现实中镜头畸变的存在，空间点P的像点会偏离实 际位置，假设偏离前后像点坐标分别为Q(x_i,y_i)与Q_L(x_i',y_i')，位姿求解 问题可转变为求解两者距离最小值的问题，可表示为：

设旋转矩阵R＝[R₁ R₂ R₃]^T，其中

为矩阵R的行向量，平 移矩阵T＝[T_x T_yT_z]^T，f为相机的焦距，w为比例系数，P₀为靶标坐 标系中心，空间点P的齐次坐标为P₀P＝(X_i,Y_i,Z_i,1)^T，图像坐标为(x_i,y_i)， 则有

设w＝f/T_z，代入式(17)并除以T_z可得

将式(18)带入式(16)，设M₁＝s(R₁,T_x),M₂＝s(R₂,T_y),S_i＝(X_i,Y_i,Z_i,1)^T， 即可得

位姿估计的过程转变为求解D的最小值，满足D最小值的位姿为 SoftPOSIT最后一次求解得到的位姿信息，位姿信息包括位置信息与姿 态信息，求解所得姿态信息即靶标在所述第二坐标系下的第一姿态角， 具体包括方位角α₁、俯仰角β₁、横滚角γ₁。

在上述实施例中，所述根据所述光斑在所述第三坐标系下的坐标 和所述角锥棱镜的顶点与光电位置传感器之间间距，获取所述光斑在 所述第四坐标系下的坐标，具体包括：

将所述光斑在所述第三坐标系下的横坐标与纵坐标，分别作为所 述光斑在第四坐标系下的横坐标与纵坐标，将所述角锥棱镜的顶点与 光电位置传感器之间间距作为所述光斑的Z轴坐标，即所述光斑在所 述第四坐标系下的坐标。

具体地，由于第三坐标系与第四坐标系X、Y轴方向平行，第四 坐标系的Z轴方向指向第三坐标系原点，故光斑在第三坐标系下的横 坐标与纵坐标及其在第四坐标系下的横坐标与纵坐标，光斑在第四坐 标系下的Z轴方向坐标即为预设间距。

预设间距的具体值可结合所需被测物相对于激光跟踪测量单元转 动角度确定，假设间距为h，光电位置传感器感光面边长为L，转动角 度为λ，则

在上述实施例中，所述根据所述角锥棱镜的顶点在所述第一坐标 系下的坐标，获取所述激光在所述第一坐标系中对应的第一空间向量， 根据所述光斑在所述第四坐标系下的坐标，获取所述激光在所述第四 坐标系中对应的第二空间向量，具体包括：

将所述第一坐标系的原点作为起点，所述角锥棱镜的顶点作为终 点，获取所述第一空间向量，将所述第四坐标系的原点作为起点，所 述光斑作为终点，获取所述第二空间向量。

具体地，根据第一坐标系原点的坐标值与角锥棱镜的顶点的坐标 值，可以计算得到第一空间向量的坐标值；根据第四坐标系的原点的 坐标值与光斑的坐标值，可以计算得到第二空间向量的坐标值。

在上述实施例中，所述根据所述靶标在所述第二坐标系下的第一 姿态角、所述第一空间向量、所述第二空间向量以及所述第一坐标系 与第二坐标系之间的转换矩阵，获取所述靶标在所述第一坐标系下的 第二姿态角，具体包括：

对所述第一空间向量和所述第二空间向量分别进行单位化处理， 得到第一单位向量和第二单位向量；

根据所述靶标在所述第二坐标系下的第一姿态角、所述第一单位 向量、所述第二单位向量以及所述第一坐标系与第二坐标系之间的转 换矩阵，基于光束向量在不同坐标系下的转换关系，获取靶标在所述 第一坐标系下的第二姿态角。

具体地，在实际测量中可以得到第一空间向量

与第二空间向量

通过单位化处理可得第一单位向量

与第二单位向量

这两个单位向量其实上是同一激光束在不同坐标系下的表现 形式，具体计算公式如下：

其中，

为第二坐标系与第四坐标系之间的转换矩阵，

为第二 坐标系与第一坐标系之间的转换矩阵。

假设物体姿态角为方位角θ、俯仰角σ、横滚角

则旋转矩阵R可 表示为

将单目视觉解算横滚角γ₁与式(22)代入式(21)即可求解其余两 个角度α₂、β₂，将这三个角通过第一坐标系与第二坐标系之间的关系转 化到第一坐标系下，获取第二姿态角α'₂、β′₂、γ'₂。

在上述实施例中，所述将所述第一姿态角、所述第二姿态角转换 到同一坐标系下，通过加权最小二乘融合获得最终姿态角，具体包括：

将所述第一姿态角通过所述第一坐标系与第二坐标系之间的转换 矩阵转换到第一坐标系下，获取第三姿态角α′₁、β′₁、γ′₁；

将第二姿态角与第三姿态角的测量方差代入式(5)，即可得到各 个姿态角归一化之前的权值；

通过式(2)便可求得加权最小二乘估计后的姿态角，即最终姿态 角θ、σ、

本发明的优点可以通过以下仿真实验进行进一步说明：

假定激光跟踪设备使用激光跟踪仪，测点误差为0.01mm/m，单目 视觉测量单元分辨率为2590×2048pixel，像元尺寸大小为4.8um，工 业相机位于激光跟踪仪Z方向上0.3m处，光电位置传感器测点误差为 0.005mm，图像处理误差为0.1pixel。预设姿态角为[0°,0°,0°]，激光跟 踪仪与靶标之间间距为3～15m。

本专利将单目视觉自动测量(简称为单目视觉法)、基于纵向投影 比结合光束向量在不同坐标系下的转换关系解算(简称为纵向投影比 法)、EPNP与SoftPOSIT联合解算并融合光束向量在不同坐标系下的 转换关系解算(简称为加权融合法)三种方法进行对比，以激光跟踪 仪与靶标之间距离为约束，姿态评价的方式采用均方差的方式，假设 姿态均方差为ζ_i,i＝1,2,3，则：

仿真结果如图4所示，从图中可以看出，在距离为3～8m时，使用 单目视觉法精度较高，在距离为9～15m时纵向投影比法表现较好，而 加权融合法在各个位置都能取得较好的效果。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而 非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领 域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技 术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修 改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方 案的精神和范围。

Claims (9)

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)

式中：x为一维状态量；Y为二维测量向量，设Y＝[y₁,y₂]^T；e为二维测量噪声向量，包含传感器的内部噪声和环境干扰噪声，设e＝[e₁,e₂]^T，若传感器的测量方差为R_i,i＝1,2，则E[e_i ²]＝E[(x-y_i)²]＝R_i；H为已知的二维常向量，设H＝[1,1]^T。

采用加权最小二乘法从测量向量Y中估计出状态量x的估计量

加权最小二乘法估计的准则是使加权误差平方和

取极小值，对

求偏导数并令其等于零得加权最小二乘估计：

设

为估计误差，则

可表示为：

x＝x-x (3)

由式(2)和式(3)，可得估计方差为：

对式(4)求极小值，取w_i的偏导数，并令其等于零得

将(5)代入式(4)，可得估计误差为

由式(6)不难看出，采用加权融合的估计方差比任何一个传感器的测量方差都小。

通过上述过程进行加权最小二乘融合，获取最终姿态角。

Images