一种基于wMPS的动态物体的位置姿态测量方法

摘要

一种基于wMPS的动态物体的位置姿态测量方法，首先在测量试验现场放置激光发射站，在被测物表面安装激光接收器，并令激光发射站匀速自转并发送光脉冲信号，然后通过内部晶振、各个激光发射站的自转周期得到各个激光接收器的旋转角度及时间戳，进而解算得到各个激光接收器位置坐标，最后判断激光接收器位置坐标并进行修正，进而完成动态物体位置姿态的测量。本发明方法与现有技术相比，在计算结果中集成信号处理器在本地计时信息，将多个激光接收器坐标测量结果同步到同一时刻，有效降低了激光接收器由于坐标测量时刻不同步造成的被测物位置姿态测量误差，实现基于wMPS的工业现场实时高精度大尺寸位姿测量。

权利要求

1.一种基于wMPS的动态物体的位置姿态测量方法，包括：

步骤一、在测量试验现场放置n个激光发射站，记为激光发射站i，i＝1，2，3…，n，然后在被测物表面任意选取m个测点安装激光接收器，记为激光接收器j，j＝1，2，3…，m，其中，n为激光发射站数量，m为激光接收器数量；

步骤二、令n个激光发射站匀速自转，激光发射站i在自转周期Ti的起始时刻发送同步光脉冲，同时在自转周期Ti内还发送两束扫描光脉冲，其中，n个激光发射站的自转周期分别记为T1、T2、T3…Tn；所述的同步光脉冲、扫描光脉冲为光平面信号，激光发射站i的同步光脉冲、两束扫描光脉冲的角度信息各不相同且n个激光发射站的同步光脉冲、两束扫描光脉冲角度信息各不相同，其中，角度信息为光平面信号与水平面的垂直面的夹角；

步骤三、令m个激光接收器将接收到的多个激光发射站发送的同步光脉冲及扫描光脉冲并将其分别转换为同步电脉冲及扫描电脉冲信号；

步骤四，以统一的计时基准对激光发射站i的同步电脉冲及扫描电脉冲信号按照激光发射站i的自转周期Ti进行匹配计时，对于激光发射器i，将激光接收器接收到激光发射器i的同步光脉冲的时刻记为t0，将激光接收器分别接收到激光发射器i的两束扫描光脉冲的时刻分别记为t1、t2，得到在自转周期Ti内激光发射器i扫过激光接收器时的旋转角度θ1i、θ2i分别为将t0作为激光发射站i的时间戳；

其特征在于包括如下步骤：

步骤五、建立wMPS坐标系，然后在wMPS坐标系中对激光发射站i的两束扫描光平面的平面方程系数进行标定，并分别记为(a1i,b1i,c1i,d1i)与(a2i,b2i,c2i,d2i)，对激光发射站k的两束光平面的平面方程系数进行标定，并分别记为(a1k,b1k,c1k,d1k)与(a2k,b2k,c2k,d2k)，进而得到激光接收器j在tpj时刻的坐标Pj(xj,yj,zj)，遍历所有激光接收器，得到各个激光接收器坐标，并分别记为P1,P2,P3,…Pm，坐标解算时刻分别为tp1,tp2,tp3,……tpm，其中，k＝1,

2,3…n，得到(xj,yj,zj)可由下式求得

(a1icos(θ1i)-b1isin(θ1i))xj+(a1isin(θ1i)+b1icos(θ1i))yj+c1izj+d1i＝0(a2icos(θ2i)-b2isin(θ2i))xj+(a2isin(θ2i)+b2icos(θ2i))yj+c2izj+d2i＝0(a1kcos(θ1k)-b1ksin(θ1k))xj+(a1ksin(θ1k)+b1kcos(θ1k))yk+c1kzj+d1k＝0(a2kcos(θ2k)-b2ksin(θ2k))xj+(a2ksin(θ2k)+b2kcos(θ2k))yj+c2kzj+d2k＝0；

tpj为激光接收器j接收到的所有激光发射站时间戳的均值，wMPS坐标系的Z轴竖直向上，Z轴与激光发射站1的第一束同步光脉冲的交点为原点，XOY平面过原点并垂直于Z轴，第一束同步光脉冲与XOY平面的交线为X轴，Y轴指向根据右手定则确定；

步骤六、如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)≤Tmax，则根据各个激光接收器坐标(P1,P2,P3,……Pm)计算动态物体位置姿态，动态物体的位置姿态测量结束，如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)>Tmax，则转入步骤七，其中，Tmax＝max(T1,T2,T3,…,Ti)；

步骤七、令tpq＝max(tp1,tp2,tp3,……tpm)，得到tpq时刻激光接收器k的修正坐标P"k为其中，P′k为激光接收器k上一自转周期的坐标，q＝1,2,3…m，t′pk为激光接收器k上一自转周期的坐标P′k的时刻；

步骤八、根据各个激光接收器坐标P1”,P2”,P3”,…,Pm”计算动态物体位置姿态。

2.根据权利要求1所述的一种基于wMPS的动态物体的位置姿态测量方法，其特征在于：

所述的m≥3。

3.根据权利要求1或2所述的一种基于wMPS的动态物体的位置姿态测量方法，其特征在于：所述的统一的计时基准为信号处理器的内部晶振。

说明书

一种基于wMPS的动态物体的位置姿态测量方法

技术领域

[0001] 本发明涉及动态物体测量领域，特别是一种基于wMPS的动态物体的位置姿态测量方法。

背景技术

[0002] 工作空间测量定位系统(wMPS：workspace Measurement Positioning System)是针对航天、航空、造船等大型制造业位置姿态测量需求及全局测量控制网的特点而发展起来的一种新型多站网络式室内空间测量定位系统，可实现大尺度空间整体坐标系下的高精度自动并行测量。现有技术中的工作空间测量定位系统(《扫描平面激光空间定位系统测量网络的构建》所描述的工作空间测量定位系统)如图1包括多个激光发射站、多个激光接收器(接收器)、信号处理器和解算工作站，系统采用基于光电扫描的空间角度自动测量方法对单个接收器进行定位，其中，激光发射站在工作时不负责解算接收器坐标，而是轴系匀速旋转向外发射带有角度信息的光平面信号，为测量空间内的接收器提供定位服务信号；激光接收器接收光平面信号并转换为电信号发送给信号处理器，信号处理器以内部晶振为时钟计时时间基准对发射站发出的光信号进行计时测量，并从中得到每个信号处理器自身在每个发射站坐标系下的角度信息，角度信息在上传给解算工作站后，通过多个发射站之间的角度交汇关系可计算出接收器的三维坐标。在上述工作模式中，发射站单向发送信息，连接有接收器的信号处理器以本地晶振为计时基础完成计时测角，发送-接收之间采用广播模式不存在闭环，因此确认等待环节，可实现全自动多点并行的高精度三维坐标测量，并可通过增加发射站个数来达到增加量程的目的。目前，wMPS系统在航空航天制造现场等需要多工序并行和整体精度控制的加工装配过程中已经获得大量成功应用。

[0003] 但是在如航天设备对接过程全物理仿真等动态测量应用中，关注重点不仅包括多个设备在全局坐标系下的任一时刻空间位置和姿态，还需要在统一的时间轴上重建整个运动过程。对于wMPS系统而言，在静态或准静态(测量时处于相对静止状态)测量的应用中，被测物的空间位置和方位角度(姿态)可通过在被测物表面安装多个(3个以上)激光接收器以最优化拟合算法解决被测物在全局坐标系(或wMPS坐标系)下的位置和姿态测量。但是动态测量时，由于系统采用多个发射站发射旋转光信号对空间进行空间扫描，被测物的运动必然会引入动态误差，影响物体位置姿态计算精度，主要包括以下方面：

[0004] (1)单个接收器接收多个发射站光信号不同步引入的坐标测量误差，该误差指在物体运动过程中激光发射站光信号在不同位置先后到达单个接收器表面而引起的交会误差；

[0005] (2)多个接收器间的测量时刻不同步而引入的姿态测量误差，由于被测物的空间位置和方位角度(姿态)需要通过在被测物表面安装多个(3个以上)接收器进行拟合测量，多接收器间的测量时刻同步误差主要指在物体运动过程中由于多个接收器空间坐标测量时刻存在时间先后差异而引入的位姿测量误差。该同步误差主要来源于两方面，一方面由于发射站在扫描空间多个接收器过程中光信号到达接收器时间先后顺序不同而引起的坐标解算时刻差异，另一方面源于多个接收器数据在信号处理器内处理先后顺序不同而解算时刻差异。

发明内容

[0006] 本发明解决的技术问题是：克服现有技术的不足，提供了一种在计算结果中集成信号处理器在本地计时信息，将激光接收器坐标测量结果同步到同一时刻，有效降低不同激光接收器由于坐标测量时刻不同步造成被测物位置姿态测量误差的基于wMPS的动态物体的位置姿态测量方法。

[0007] 本发明的技术解决方案是：一种基于wMPS的动态物体的位置姿态测量方法，包括如下步骤：

[0008] 步骤一、在测量试验现场放置n个激光发射站，记为激光发射站i，i＝1，2，3…，n，然后在被测物表面任意选取m个测点安装激光接收器，记为激光接收器j，j＝1，2，3…，m，其中，n为激光发射站数量，m为激光接收器数量；

[0009] 步骤二、令n个激光发射站匀速自转，激光发射站i在自转周期Ti的起始时刻发送同步光脉冲，同时在自转周期Ti内还发送两束扫描光脉冲，其中，n个激光发射站的自转周期分别记为T1、T2、T3…Tn；所述的同步光脉冲、扫描光脉冲为光平面信号，激光发射站i的同步光脉冲、两束扫描光脉冲的角度信息各不相同且n个激光发射站的同步光脉冲、两束扫描光脉冲角度信息各不相同，其中，角度信息为光平面信号与水平面的垂直面的夹角；

[0010] 步骤三、令m个激光接收器将接收到的多个激光发射站发送的同步光脉冲及扫描光脉冲并将其分别转换为同步电脉冲及扫描电脉冲信号；

[0011] 步骤四，以统一的计时基准对激光发射站i的同步电脉冲及扫描电脉冲信号按照激光发射站i的自转周期Ti进行匹配计时，对于激光发射器i，将激光接收器接收到激光发射器i的同步光脉冲的时刻记为t0，将激光接收器分别接收到激光发射器i的两束扫描光脉冲的时刻分别记为t1、t2，得到在自转周期Ti内激光发射器i扫过激光接收器时的旋转角度θ1i、θ2i分别为

[0012]

[0013]

[0014] 将t0作为激光发射站i的时间戳；

[0015] 步骤五、建立wMPS坐标系，然后在wMPS坐标系中对激光发射站i的两束扫描光平面的平面方程系数进行标定，并分别记为(a1i,b1i,c1i,d1i)与(a2i,b2i,c2i,d2i)，对激光发射站k的两束光平面的平面方程系数进行标定，并分别记为(a1k,b1k,c1k,d1k)与(a2k,b2k,c2k,d2k)，进而得到激光接收器j在tpj时刻的坐标Pj(xj,yj,zj)，遍历所有激光接收器，得到各个激光接收器坐标，并分别记为P1,P2,P3,…Pm，坐标解算时刻分别为tp1,tp2,tp3,……tpm，其中，k＝1,2,3…n，得到(xj,yj,zj)可由下式求得

[0016] (a1icos(θ1i)-b1isin(θ1i))xj+(a1isin(θ1i)+b1icos(θ1i))yj+c1izj+d1i＝0[0017] (a2icos(θ2i)-b2isin(θ2i))xj+(a2isin(θ2i)+b2icos(θ2i))yj+c2izj+d2i＝0[0018] (a1kcos(θ1k)-b1ksin(θ1k))xj+(a1ksin(θ1k)+b1kcos(θ1k))yj+c1kzj+d1k＝0[0019] (a2kcos(θ2k)-b2ksin(θ2k))xj+(a2ksin(θ2k)+b2kcos(θ2k))yj+c2kzj+d2k＝0；

[0020] tpj为激光接收器j接收到的所有激光发射站时间戳的均值，wMPS坐标系的Z轴竖直向上，Z轴与激光发射站1的第一束同步光脉冲的交点为原点，XOY平面过原点并垂直于Z轴，第一束同步光脉冲与XOY平面的交线为X轴，Y轴指向根据右手定则确定；

[0021] 步骤六、如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)≤Tmax，则根据各个激光接收器坐标(P1,P2,P3,……Pm)计算动态物体位置姿态，动态物体的位置姿态测量结束，如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)>Tmax，则转入步骤七，其中，Tmax＝max(T1,T2,T3,…,Ti)；

[0022] 步骤七、令tpq＝max(tp1,tp2,tp3,……tpm)，得到tpq时刻激光接收器k的修正坐标P"k为

[0023]

[0024] 其中，P′k为激光接收器k上一自转周期的坐标，q＝1,2,3…m，t′pk为激光接收器k上一自转周期的坐标P′k的时刻；

[0025] 步骤八、根据各个激光接收器坐标P1”,P2”,P3”,…,Pm”计算动态物体位置姿态。

[0026] 所述的m≥3。

[0027] 所述的统一的计时基准为信号处理器的内部晶振。

[0028] 本发明与现有技术相比的优点在于：

[0029] (1)本发明方法以现有wMPS系统信号处理器时钟为时标，在计算结果中集成信号处理器在本地计时信息，将连接信号处理器的多个激光接收器坐标测量结果同步到同一时刻，可有效降低不同激光接收器由于坐标测量时刻不同步而造成的被测物位置姿态测量误差，实现基于wMPS的工业现场实时高精度大尺寸位姿测量；

[0030] (2)本发明方法克服了现有的动态物体坐标测量方法仅局限于空间坐标的获取、每个测量结果不包含时间信息的缺陷，将所有动态坐标测量和时间轴相结合，使数据结果更加全面、准确地反映动态物体的时空信息；

[0031] (3)本发明方法克服了现有的动态物体坐标测量方法包含由于激光发射站扫描光信号不同步所造成的误差，即不同激光发射站光信号到达激光接收器的时间不同，导致实际测量得的原始观测量(扫描角度)非同一抽象点的缺陷，通过时间补齐过程，将该误差大大消除，提高了动态测量精度；

[0032] (4)本发明方法客服了现有的动态物体坐标测量方法中每个激光接收器接收到发射站信号的时刻不相同带来激光接收器不同步误差的缺陷，并对此误差进行了补偿，提高了动态测量精度。

附图说明

[0033] 图1为本发明一种基于wMPS的动态物体的位置姿态测量方法中装置分布图；

[0034] 图2为本发明一种基于wMPS的动态物体的位置姿态测量方法原理流程图；

[0035] 图3为本发明方法中多个激光接收器的测量时刻不同步引入的姿态测量误差原理图。

具体实施方式

[0036] wMPS系统在动态测量时，由于系统采用多个发射站发射旋转光信号对空间进行空间扫描，被测物的运动位置姿态测量必然会引入动态误差，影响物体位置姿态计算精度，主要包括两方面：

[0037] (1)单个接收器接收多个发射站光信号不同步引入的坐标测量误差，该误差指在物体运动过程中激光发射站光信号在不同位置先后到达单个接收器表面而引起的交会误差；

[0038] (2)多个接收器间的测量时刻不同步而引入的姿态测量误差，由于被测物的空间位置和方位角度(姿态)需要通过在被测物表面安装多个(3个以上)接收器进行拟合测量，多接收器间的测量时刻同步误差主要指在物体运动过程中由于多个接收器空间坐标测量时刻存在时间先后差异而引入的位姿测量误差。该同步误差主要来源于两方面，一方面由于发射站在扫描空间多个接收器过程中光信号到达接收器时间先后顺序不同而引起的坐标解算时刻差异，另一方面源于多个接收器数据在信号处理器内处理先后顺序不同而解算时刻差异。

[0039] 上述第一种误差由系统测量原理决定，理想状态下可保证在可测得的最慢转速发射站周期之内(例如，多个发射站间如果转速最慢为1800rpm则同步时间误差可保证在33.33ms之内)，补偿较为困难。考虑到信号处理器内部采用单个晶振对接收器接收到的光脉冲进行计时测量，因此本发明方法重点关注如何通过在计算结果中集成信号处理器在本地计时信息对上述第二种误差进行同步和补偿，从而最大限度提高被测物姿态测量精度。

[0040] 本发明提出一种工作空间测量定位系统(wMPS)中动态物体的位置姿态测量方法，充分利用工作空间测量定位系统中信号处理器内部已有的时钟信息，将连接信号处理器的多个激光接收器坐标测量结果同步到同一时刻，提高现场动态位置姿态测量精度。下面结合附图对本发明方法进行详细说明，如图2所示本发明一种基于wMPS的动态物体的位置姿态测量方法包括如下步骤：

[0041] 步骤一，测量试验现场工作空间尺寸，然后根据试验现场工作空间测量尺寸放置n个激光发射站，激光发射站匀速自转，每个激光发射站在自转过程中分别向外发射两束带有角度信息的光平面信号(分别记为第一光平面、第二光平面信号)，两束光平面信号的角度信息不同且n个激光发射站的光平面信号角度信息各不相同，使n个激光发射站的光脉冲发射范围能够覆盖试验现场工作空间，然后使用基准尺对各个激光发射站进行外部参数标定，建立wMPS坐标系(wMPS坐标系定义如下：Z轴竖直向上，当激光发射站i的同步光被触发时，Z轴与激光发射站i的第一束光平面的交点为原点，XOY平面过原点并垂直于Z轴，此时第一束光平面与XOY平面的交线定义为X轴，指向为指向激光接收器的方向，Y轴指向根据右手定则确定，激光发射站i为任意选取)，并在被测物表面任意选取三个以上测点安装激光接收器用于动态物体位置姿态测量，其中，n个激光发射站分别为激光发射站1、激光发射站2、激光发射站3…激光发射站n，角度信息为光平面信号与水平面的垂直面的夹角且部位0。

[0042] 步骤二，在信号处理器的通讯数据包中为每个激光发射站的角度信息附加本地时钟信息(即为测量数据加盖时间戳)，其中，信号处理器的通讯数据帧包括各个激光发射站的旋转角度信息及响应时间戳，具体方法为:

[0043] (1)令n个激光发射站以其轴线(平行于Z轴)为对称轴匀速旋转，其中，激光发射站i在旋转的每个周期Ti的起始时刻发送同步光脉冲，每个周期Ti内还产生并发送两束扫描光脉冲，Ti为激光发射站i匀速旋转的周期且每个激光发射站的旋转轴器均不相同；

[0044] (2)激光接收器接收到多个激光发射站发射的同步光脉冲及扫描光脉冲并将其分别转换为同步电脉冲及扫描电脉冲信号，然后送至信号处理器；

[0045] (3)信号处理器接收到激光接收器发送的同步电脉冲及扫描电脉冲信号后，以内部晶振为计时基准对不同激光发射站的同步光脉冲和扫描光脉冲形成的电脉冲按照发射站旋转周期进行匹配计时，如t0时刻接收器接收到激光发射站i的同步光脉冲信号并在接下来的t1时刻和t2时刻连续接收到激光发射站i的两束扫描光脉冲信号，则在该激光发射站i的旋转周期Ti内激光发射站i扫过当前激光接收器时的旋转角度分别为：

[0046]

[0047]

[0048] 由于同步光脉冲时刻t0标志了激光发射站i本次周期信号传输的时间起点,因此将信号处理器记录的同步光时刻t0作为发射站旋转角度θ1，θ2的时间戳；信号处理器将各个激光接收器接收到的激光发射站i的旋转角度信息(θ1i，θ2i)及相应时间戳打包形成激光发射站i的数据帧，并上传到计算工作站。

[0049] 步骤三，计算工作站接收信号处理器发送来的带有旋转角度信息和相应时间戳的激光发射站i的数据帧，根据激光接收器发送来两个以上激光发射站的旋转角度信息则可根据角度交会原理计算接收器j此时的坐标Pj(x,y,z)，其中，本发明方法中假定动态物体上置于m个激光接收器，并记为激光接收器1、激光接收器2、激光接收器3…激光接收器m，由于Pj(x,y,z)是各个激光发射站角度信息交会的综合结果，因此，此时以各个发射站角度信息时间戳的均值作为坐标Pj(x,y,z)的测量时刻tp；其中，根据激光发射站i在wMPS坐标系中的旋转角度信息(θ1i，θ2i)、激光发射站k在wMPS坐标系中的旋转角度信息(θ1k，θ2k)计算接收器j此时的坐标Pj(x,y,z)的计算过程为：

[0050] 通过摄像机内参数标定方法分别对激光发射站i、激光发射站k的两束扫描光平面的平面方程系数进行标定，并分别记为：(a1i,b1i,c1i,d1i)与(a2i,b2i,c2i,d2i)，(a1k,b1k,c1k,d1k)与(a2k,b2k,c2k,d2k)，则当激光发射站i和激光发射站k分别扫过接收器j时，接收器j此时的坐标Pj(xj,yj,zj)满足如下方程：

[0051] (a1icos(θ1i)-b1isin(θ1i))xj+(a1isin(θ1i)+b1icos(θ1i))yj+c1izj+d1i＝0[0052] (a2icos(θ2i)-b2isin(θ2i))xj+(a2isin(θ2i)+b2icos(θ2i))yj+c2izj+d2i＝0[0053] (a1kcos(θ1k)-b1ksin(θ1k))xj+(a1ksin(θ1k)+b1kcos(θ1k))yj+c1kzj+d1k＝0[0054] (a2kcos(θ2k)-b2ksin(θ2k))xj+(a2ksin(θ2k)+b2kcos(θ2k))yj+c2kzj+d2k＝0，[0055] 求解可得到接收器j(在wMPS坐标系中)此时的坐标(xj,yj,zj)。

[0056] 步骤四，由于进行准确的位姿测量需要同一时刻获得三个以上激光接收器在全局坐标系下(此处默认为wMPS坐标系)的空间坐标，因此可根据步骤三测量计算得到的各个激光接收器坐标为P1,P2,P3,……Pm，其坐标解算时刻分别为tp1,tp2,tp3,……tpm，现场可观测到的最慢激光发射站周期为Tmax(即为n个激光发射站发射周期的最大值，Tmax＝max[T1,T2,T3,…,Ti])，此时进行判断，如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)≤Tmax，则认为各个激光接收器坐标测量计算时刻之间不存在同步误差(或误差较小)，各个激光接收器坐标为P1,P2,P3,……Pm，如果max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)>Tmax，则认为各个激光接收器坐标测量计算时刻之间存在同步误差，需要进行时刻数据同步，时刻数据同步基于以下前提：

[0057] 1)在大型设备运动过程中，由于被测物本身质量惯性较大，短时间内运动可以认为是向某一方向以速度v匀速前进的；

[0058] 2)在测量场中信号处理器数据输出在时间上连续，根据两次测量的相邻坐标及坐标解算时刻可推算速度v及短时间内任意时刻坐标。

[0059] 时刻数据同步方法如图3所示，具体过程为：

[0060] (1)在可测量接收器中选择解算时刻(tp1,tp2,tp3,……tpm)中最大的激光接收器坐标Pm(结果最新，最接近运动中的实时坐标)，其测量时刻为tpm；

[0061] (2)假设需要时刻数据同步的激光接收器在本帧数据(当前旋转周期)及上帧数据(上一旋转周期)中解算的激光接收器坐标分别为Pm及P’m，其坐标测量时刻为tpm及t’pm，则tpm时刻当前激光接收器的修正坐标为

[0062]

[0063] (3)时刻数据同步后的各个激光接收器在全局坐标系下修正坐标为P1”,P2”,P3”,…,Pm”。

[0064] 步骤五，当max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)≤Tmax时，根据各个激光接收器坐标(P1,P2,P3,……Pm)计算动态物体位置姿态；

[0065] 当max(tp1,tp2,tp3,…tpm)-min(tp1,tp2,tp3,…tpm)>Tmax时，根据各个激光接收器坐标(P1”,P2”,P3”,…,Pm”)计算动态物体位置姿态的过程，其中上述计算过程为根据与刚体固联的空间坐标点解算刚体姿态，可参考专利《基于光电扫描的室内移动机器人位姿测量方法》。

[0066] 本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。