CN109238246A - 基于全周角度约束的多站多点自适应六自由度测量方法 - Google Patents

Abstract

本发明公开了一种基于全周角度约束的多站多点自适应六自由度测量方法，所述方法包括：将六个光电接收器周向安装于待测目标上，单个光电接收器至少接收到测量场中一个发射站的光信号，计算光电接收器在发射站下的扫描角、进而计算旋转后的光平面法向量；将旋转后的法向量从发射站局部坐标系变换到全局坐标系中；将变换后的光平面，利用平面约束关系，求取参考点到光平面的距离；根据旋转矩阵的正交性、结合坐标点到光平面的距离，建立最优化方法的目标函数；获取最优化过程的六自由度的测量初值，通过Levenberg‑Marquardt优化算法进行最优化计算，求解旋转矩阵和平移矩阵，即得到六自由度的测量值。

Description

基于全周角度约束的多站多点自适应六自由度测量方法

技术领域

本发明涉及基于室内空间测量定位系统(wMPS，工作空间测量定位系统)的三维坐标精密测量领域，特别涉及基于全周角度约束的多站多点自适应六自由度测量方法。

背景技术

随着先进制造技术的发展，基于制造全过程的数字化装配技术得到越来越多的重视。大部件装配过程中，不仅需要实时测量部件上关键点位的空间坐标，还需要对部件整体的位置和姿态进行实时监控，防止部件之间产生拉伸、挤压等形变，影响装配质量和效率。根据刚体运动学原理，传统的大空间六自由度测量方法一般需要在待测物体表面选取3个以上参考点，各参考点在其自身坐标系下的坐标已知，通过外部三维测量系统(如wMPS测量网络，室内GPS等)获得参考点在全局测量坐标系下的坐标，利用坐标系间的转换关系求解待测目标的空间位置及姿态。具体应用中，要测量大型部件如火箭筒段、飞机和船舶分段、AGV(自动导引运输车)的位置和姿态，需要在其表面安装多个接收器，每个接收器采用和待测目标一体设计的工装支架固定连接。根据测量网络的工作原理，至少两个发射站的光信号扫描过接收器时，才可通过角度交会得到接收器三维坐标，且坐标测量精度在一定范围内与扫描过接收器的发射站数量正相关，因此通过求解目标上参考点的三维坐标，进而拟合刚体空间姿态的方法依赖于多个光平面空间交会条件，具有较大局限性。

然而大型装备制造现场往往设备众多，环境复杂，实际应用中上述方法容易受到障碍物遮挡影响，造成光路阻塞，破坏通视条件而影响其适用性；同时上述方法在步骤上属于两步解算，各步骤间残差容易造成累加，影响测量精度。

发明内容

本发明提供了一种基于全周角度约束的多站多点自适应六自由度测量方法，本发明不受限于wMPS测量系统的空间交会条件的制约，可在单发射站或多发射站情况下灵活实现刚体的六自由度实时测量，详见下文描述：

基于全周角度约束的多站多点自适应六自由度测量方法，所述方法包括：

将六个光电接收器周向安装于待测目标上，单个光电接收器至少接收到测量场中一个发射站的光信号，计算光电接收器在发射站下的扫描角、进而计算旋转后的光平面法向量；

将旋转后的法向量从发射站局部坐标系变换到全局坐标系中；将变换后的光平面，利用平面约束关系，求取参考点到光平面的距离；

根据旋转矩阵的正交性、结合坐标点到光平面的距离，建立最优化方法的目标函数；获取最优化过程的六自由度的测量初值，通过Levenberg-Marquardt优化算法进行最优化计算，求解旋转矩阵和平移矩阵，即得到六自由度的测量值。

进一步地，所述获取最优化过程的初值，并通过Levenberg-Marquardt优化算法进行最优化计算的步骤具体为：

根据系统测量模型和机械安装特性，将线性非齐次方程组AX＝D简化为线性齐次方程组AX＝0，求解该齐次方程得到六自由度测量初值，将初值代入目标函数中。

其中，所述目标函数具体为：

其中，M表示参考点总数，N表示发射站总数，λ为惩罚因子，d_mki为参考点到光平面的实际测量距离，f_j为根据旋转矩阵正交性得到的关系式。

所述

其中，表示刚体上第m个参考点在刚体坐标系下的三维坐标，R表示从刚体坐标系到全局坐标系的旋转关系，用r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ r₃₁ r₃₂ r₃₃表示，T表示从刚体坐标系到全局坐标系的平移关系，用t_x t_y t_z表示，并且满足：

p和q的取值分别为1、2和3。

进一步地，所述测量方法还包括：

采用固连式测量系统，接收器安装于被测目标上，与目标在测量空间内同步运动。

进一步地，所述测量方法还包括：

采用手持式测靶，测量时用针尖接触被测物体可实现接触式测量。

本发明提供的技术方案的有益效果是：

1、本方法依托于室内空间测量定位系统，在已知若干个参考点空间位置关系的前提下，三个以上的参考点收到至少一个发射站的光信号，即可在独立于空间角度交会的情况下，自适应选择冗余测量信息，通过发射站激光平面约束和非线性最优化方法，实现六自由度测量，位置测量精度达到0.2mm+0.01mm/m，姿态角测量精度达到5”；

2、与传统的刚体六自由度测量方法相比，本方法在无法测量得到参考点坐标的情况下，可通过优化算法直接求得刚体的六自由度，灵活运用多站和多参考点冗余信息，自适应选择测量信息进行解算；

3、本方法一方面降低了测量条件要求，具有较好的现场适用性；另一方面，该方法属于单步测量法，可以避免传统多步法造成的误差传递和累积，具有更高的测量精度和测量效率。

附图说明

图1为基于全周角度约束的多站多点自适应六自由度测量方法的流程图；

图2为发射站模型的示意图；

图3为系统原理示意图；

图4为手持式测靶示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

实施例1

为了保证现场测量系统的健壮可靠，提高测量系统的自适应性和稳定性，结合多站分布式测量网络的特性，本发明实施例给出了一种独立于空间角度交会条件的刚体六自由度直接测量方法，即基于全周角度约束的多站多点自适应六自由度测量方法，如图1所示，该方法包括以下步骤：

101：将六个光电接收器周向安装于待测目标上，单个光电接收器至少接收到测量场中一个发射站的光信号，计算光电接收器在发射站下的扫描角、进而计算旋转后的光平面法向量；

102：将旋转后的法向量从发射站局部坐标系变换到全局坐标系中；

103：将变换后的光平面，利用平面约束关系，即光平面扫过接收器时，接收器的坐标点应处于平面内，结合公式求取实际测量的坐标点到光平面的距离；

104：根据旋转矩阵的正交性、结合坐标点到光平面的距离，建立最优化方法的目标函数；

105：获取最优化过程的初值，并利用Levenberg-Marquardt算法进行最优化计算，求解旋转矩阵和平移矩阵。

综上所述，本发明实施例与传统的刚体六自由度测量方法相比，本方法在无法测量得到参考点坐标的情况下，可通过优化算法直接求得刚体的六自由度，灵活运用多站和多参考点冗余信息，自适应选择测量信息进行解算。

实施例2

下面结合具体的计算公式、实例、图2-图4对实施例1中的方案进行进一步地介绍，详见下文描述：

wMPS测量系统由发射站、光电接收器、信号处理器等部分组成。使用前根据测量范围与测量需求在空间内布置多台发射站构成测量网络，测量网络在使用前需要完成全局定向，即标定得到全局坐标系与每个发射站局部坐标系的位姿关系，通常用旋转矩阵R_k和平移矩阵T_k表示，其中，

其中，r_k1…r_k9和t_kx…t_kz分别为全局坐标系与发射站k局部坐标系的旋转矩阵和平移矩阵中各元素。

单个发射站具有两个可以绕自身局部坐标系Z轴高速旋转的光平面，其平面参数在发射站装配完成后作为自身结构参数可唯一确定，表示为：

a_kix+b_kiy+c_kiz+d_ki＝0(i＝1,2；k＝1,2,…)

(2)

其中，a_ki、b_ki、c_ki和d_ki分别为发射站k的第i个光平面法向量参数。

旋转过程中，光平面扫过固定在被测目标参考点处的光电接收器，光电接收器通过信号处理返回两个光平面的扫描时间t₁和t₂，进而根据发射站的旋转角速度求得扫描角θ₁和θ₂：

其中，ω表示光平面旋转的角速度。此时，旋转后的光平面的参数为：

其中，θ_ki为发射站k扫过接收器时的扫描角；a_kθi…d_kθi分别为接收器k的第i个光平面扫过接收器时的法向量参数。

将旋转后的光平面从发射站自身局部坐标系变换到全局坐标系，该转换可根据外参标定结果R_k和T_k实现，变换后的光平面参数表示为：

其中，a_Tkθi…d_Tkθi分别为发射站k扫过接收器时的第i个光平面转换到全局坐标系下的法向量参数。

进而，根据发射站平面模型，光平面扫过接收器时接收器应在理论上位于平面内，因此，得到参考点到光平面的实际测量距离，即：

其中，表示刚体上第m个参考点在刚体坐标系下的三维坐标，R表示从刚体坐标系到全局坐标系的旋转关系，用r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ r₃₁ r₃₂ r₃₃表示，T表示从刚体坐标系到全局坐标系的平移关系，用t_x t_y t_z表示，并且满足：

其中，p，q表示旋转矩阵的行号。

根据旋转矩阵的正交性，R矩阵的九个元素之间共包含三个未知量，显然待求量R和T共包含6个未知量，只要有至少三个参考点接收到至少一个发射站的光信号，即可建立最优化目标函数，通过Levenberg-Marquardt优化算法^[1]求解R和T，进而实现空间六自由度测量，最优化目标函数可表示为：

其中，M表示参考点总数，N表示发射站总数，λ为惩罚因子，这种非线性最优化问题可以采用优化算法进行求解，进而得到刚体坐标系与全局坐标系的旋转和平移关系，即得到R和T。

Levenberg-Marquardt优化算法需选择合适的迭代初值使优化结果收敛。结合发射站的光平面模型，即光平面扫过接收器时接收器的坐标点应位于平面内，得到如下的初值计算过程：

将发射站平面方程(公式2)改成以下的矩阵形式：

AX＝D

(9)

其中，

X＝[r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ r₃₁ r₃₂ r₃₃ t_x t_y t_z]^T

(11)

D＝[-d_Tkθ1 -d_Tkθ2 … -d_Tkθ1 -d_Tkθ2]^T

(12)

由于单个发射站的激光平面1经过自身坐标系原点，所以d_Tkθ1＝0；另外，在安装过程中，可以通过调整激光平面2使其大致经过坐标系原点，所以d_Tkθ2也是个绝对值很小的值，

可以认为d₂≈0；因此，方程可以化简为线性齐次方程：

AX＝0

(13)

将未知量X分解：

X₁＝[r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ t_x t_y t_z]^T

(14)

X₂＝[r₃₁ r₃₂ r₃₃]^T

(15)

则方程可写为：

PX₁+QX₂＝0

(16)

根据正交矩阵的特性||X₂||²＝1，最优化方程可写(即对公式(8)的改写)为：

F＝||PX₁+QX₂||²+λ(||X₂||²-1)

(19)

将上式展开并对X₁和X₂分别求偏导数，化简得到如下方程组：

式中，S＝Q^TQ-Q^TP(P^TP)^-1P^TQ是一个3*3的方阵。X₂是方阵S的特征向量，从而解出X₂。将X₂带入方程组(20)的第二个方程中，即可求解得到X₁，从而得到迭代初值。

将迭代初值X₁带入最终的优化目标方程(公式8)，得到刚体坐标系与全局坐标系的旋转矩阵R和平移矩阵T。根据旋转矩阵R，可以得到刚体在全局坐标系中的欧拉角αβγ(即三个姿态自由度)：

α＝arctan(r₃₂/r₃₃)

γ＝arctan(r₂₁/r₁₁) (21)

同时，平移矩阵的三个元素即代表了待测目标在全局坐标系三个坐标轴方向的位置(即三个位置自由度)。至此，刚体在全局坐标系下三个轴向的位置和姿态，即六个自由度可全部得到。

根据此测量原理，实际应用中衍生出一种手持式六自由度测量靶，测量靶由接收器、信号处理模块、供电模块等组成。测量靶配备不同长度的测头，测头接触被测点也可实现接触式测量。

综上所述，本发明实施例通过上述步骤不受限于wMPS测量系统的空间交会条件的制约，可在单发射站或多发射站情况下灵活实现刚体的六自由度实时测量。

实施例3

本方法提供了一种基于全周角度约束的自适应六自由度测量方法，该测量方法配置了两种测量实施方案：一种为固连式测量系统(参见图3)，接收器安装于被测目标上，与目标在测量空间内同步运动；另一种为手持式测靶(参见图4)，测量时用针尖接触被测物体即可实现接触式测量。具体地：

1：根据测量空间范围布置发射站位置，建立发射站测量场，利用全局控制场标定方法进行测量网络全局定向，得到单个发射站的局部坐标系(O_k-X_kY_kZ_k)到全局坐标系(O_G-X_GY_GZ_G)的旋转矩阵R_k和平移矩阵T_k。

2：接收器在测量场运动时，实时接收测量场内所有发射站(图2)的光信号，当三个以上(包含三个)参考点收到至少一个发射站的光信号时，测量系统自适应选取冗余测量信息，根据发射站的光平面旋转特性(图2)得到绕发射站自身Z轴旋转后的光平面方程，进而根据全局定向参数，将旋转后的光平面进行坐标系变换，得到全局坐标系下的光平面的参数。

3：根据光平面约束建立非线性最优化问题，求解得到参考点坐标系O_V-X_VY_VZ_V与全局坐标O_G-X_GY_GZ_G之间的旋转矩阵R和平移矩阵T。

求解最优化问题的初值时，根据发射站局部坐标系定义规格，光平面1通过局部坐标系原点，故d_TKθ1为0。同时，发射站机械装调过程中，通过微调可使光平面2近似通过坐标原点，其坐标系Z轴截距可控制在±2mm以内，因此可将线性非齐次方程组AX＝D简化为线性齐次方程组AX＝0，求解该齐次方程得到六自由度测量初值，将初值代入目标函数：

通过最优化算法得到最终的六自由度测量结果。

具体实现时，图3中的测量系统的六个处理器(即接收器)周向固定于被测目标上，结构较为灵活，根据目标大小、形状可灵活设置。图4中的测量靶具备电池、信号接收、处理等功能，具体应用形式也是灵活的，如测靶大小，形状等。实际应用中，只要能满足上述功能要求的测量系统以及测靶均可，本发明实施例对测量系统以及测靶的结构不做限制。

综上所述，本方法独立于角度交会的测量原理，无需测量接收器的空间坐标即可实现六自由度测量，属于一步测量法，充分利用多接收器接收与多发射站的冗余测量信息，自适应选择相应信息进行解算，可实现六自由度实时精确解算，具有良好的现场适用性。参考文献

[1]MoréJ J 1978 The Levenberg–Marquardt algorithm:implementation andtheory Lect.Notes Math.630 105–16

本发明实施例对各器件的型号除做特殊说明的以外，其他器件的型号不做限制，只要能完成上述功能的器件均可。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述方法包括：

将六个光电接收器周向安装于待测目标上，单个光电接收器至少接收到测量场中一个发射站的光信号，计算光电接收器在发射站下的扫描角、进而计算旋转后的光平面法向量；

将旋转后的法向量从发射站局部坐标系变换到全局坐标系中；将变换后的光平面，利用平面约束关系，求取参考点到光平面的距离；

根据旋转矩阵的正交性、结合坐标点到光平面的距离，建立最优化方法的目标函数；获取最优化过程的六自由度的测量初值，通过Levenberg-Marquardt优化算法进行最优化计算，求解旋转矩阵和平移矩阵，即得到六自由度的测量值。

2.根据权利要求1所述的基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述获取最优化过程的初值，并通过Levenberg-Marquardt优化算法进行最优化计算的步骤具体为：

根据系统测量模型和机械安装特性，将线性非齐次方程组AX＝D简化为线性齐次方程组AX＝0，求解该齐次方程得到六自由度测量初值，将初值代入目标函数中。

3.根据权利要求1或2所述的基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述目标函数具体为：

其中，M表示参考点总数，N表示发射站总数，λ为惩罚因子，d_mki为参考点到光平面的实际测量距离，f_j为根据旋转矩阵正交性得到的关系式。

4.根据权利要求3所述的基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述

其中，表示刚体上第m个参考点在刚体坐标系下的三维坐标，R表示从刚体坐标系到全局坐标系的旋转关系，用r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ r₃₁ r₃₂ r₃₃表示，T表示从刚体坐标系到全局坐标系的平移关系，用t_x t_y t_z表示，并且满足：

p和q的取值分别为1、2和3。

5.根据权利要求1-4中任一权利要求所述的基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述测量方法还包括：

采用固连式测量系统，接收器安装于被测目标上，与目标在测量空间内同步运动。

6.根据权利要求1-4中任一权利要求所述的基于全周角度约束的多站多点自适应六自由度测量方法，其特征在于，所述测量方法还包括：

采用手持式测靶，测量时用针尖接触被测物体可实现接触式测量。