CN101750012A - 一种测量物体六维位姿的装置 - Google Patents

Abstract

一种测量物体六维位姿的装置，由激光跟踪仪、接收器、计算处理单元和至少一个小型激光发射器组成，计算处理单元与激光跟踪仪固装在地面上；接收器安在待测运动物体上；激光跟踪仪和接收器与计算处理单元通讯，小型激光发射器安装在激光跟踪仪上；激光跟踪仪的水平转角和俯仰转角可控，上面安装有激光测距仪；激光测距仪和小型激光发射器分别投射激光到接收器的投射面板上。激光跟踪仪获得接收器的三维位置，计算处理单元根据激光跟踪仪的方位角和投射面板上的激光光斑图像数据求解接收器相对于地面固定坐标系的三维姿态。本发明可以连续测量在较大空间中运动的物体或者静止的物体，精度高、测量速度快、成本低和便于布置，可以替代昂贵的六维位姿测量激光跟踪仪。

Description

一种测量物体六维位姿的装置

技术领域

本发明涉及较大空间中物体六维位姿的测量技术，具体地说是一种利用激光跟踪仪实现三维坐标测量，利用激光束在接收器上生成光斑并通过近距离图像处理技术和并联测量原理实现物体三维姿态测量的装置。该可以广泛应用于需要测量物体六维位姿的场合，既可以测量静止物体，也可以测量运动物体，可以替代昂贵的六维位姿测量激光跟踪仪。

背景技术

对运动物体的三维位置(x，y，z)和三维姿态(α，β，γ)进行高精度的测量在工业界有着广泛的需求。

为了测量在大范围运动的物体的六维位姿，通常采用全局定位系统。根据测量精度的高低，全局定位系统可以分为激光跟踪仪、室内GPS、基于激光导航方法的装置、基于激光信标方法的装置和基于视觉处理的装置。其中基于激光跟踪仪的装置具有精度高、测量速度快、能测量运动物体等优点，因此应用最为广泛。

激光跟踪仪和激光全站仪都采用经纬仪测量接受器(通常也称为靶标)的方位角(通常是水平转角和俯仰转角)，而采用测距仪测量接收器与经纬仪基准点之间的距离，并将接收器的球坐标转换为笛卡儿坐标系中的三维坐标。其中经纬仪的静态测角精度一般可以达到1″量级(约等效于空间综合精度4.85ppm，其中1ppm＝1μm/m)，而测距仪的测距精度根据测距技术的不同有较大差别。激光跟踪仪通常采用内置的激光位移干涉仪(英文简称IFM)来测距，测量方式为增量式测量，测距精度在50m以内可以达到±0.5ppm量级，具有采样频率高和测量精度非常高的显著优点，但是装置十分昂贵。美国Optodyne公司采用激光多普勒测距仪(英文简称LDDM)进行测距，测量方式也是增量式测量，测距精度在50m以内可以达到±1ppm量级。激光全站仪相当于低端的激光跟踪仪，大部分采用激光绝对距离测量(英文简称ADM)仪器来测距，测量方式通常是间接测尺频率方式(例如差频测相法)，调制方式通常采用激光波幅调制，高端的激光全站仪的测距精度在100m以内可以达到±0.2mm，测量范围可以达到1km，在全量程范围内测距精度达到±1mm+2ppm；而美国NASA的一项新技术采用偏振角调制方式，测距精度与激光多普勒测距仪的性能较接近。测距仪还可以采用飞行时间原理测距，例如Leica公司的Disto测距仪在50m以内的测距精度可以达到±2mm，美国Automated Precision Inc.(以下简称为API)公司提出的Repetitive-Time-of-Flight技术的测距精度甚至可以达到2.5ppm±25μm。测距仪还可以是采用三角法测距的激光结构光位置传感器，通常用于量程在1000mm以内的高精度测量，例如日本Keyence公司的LK-G系列激光位移传感器的重复精度可达到2μm以下；这类测距仪通常被安装在一个或多个旋转轴上组成激光扫描仪。为了简化论述，下面将经纬仪、两自由度转台和其他类似的指向装置统称为经纬仪，将激光跟踪仪、激光全站仪、激光扫描仪和其他类似的二维或三维坐标测量设备统称为激光跟踪仪。

激光跟踪仪通常只用于测量接受器的三维坐标，接受器通常采用反射球形式，反射球里面包括使激光后向反射的角锥棱镜，而且角锥棱镜的定点与球心重合。如果进行六维位姿测量则需要专用的六维位姿测量激光跟踪仪。

美国API公司的专利“Three and five axis laser tracking systems”(美国专利号US4714339)最早提出将激光跟踪仪用于五维位姿测量，采用一种同时让激光部分反射和部分透射的平面式靶标，靶标安装在两自由度转台上，根据透射光线的落点信息来控制两自由度转台，以保持靶标平面与入射激光垂直，根据两自由度转台的转角获得靶标的俯仰(Pitch)和偏转(Yaw)两维位姿，但是不能获得靶标相对激光束的滚动转角(Roll)。虽然在数控机床和测量机的精密标定中存在多种六维位姿测量技术，例如美国专利US5056921、US5064289和US5363196等，但是这些技术都需要辅助器件，因此无法应用于激光跟踪仪。API公司的专利“Five-axis，six-axis lasermeasuring system”(美国专利号US 6049377)等提出一种新的靶标，通过分光镜将入射激光分成两束，一束通过类似US4714339的方案进行测距，并控制两自由度转台使靶标平面与入射激光垂直，另一束通过偏振分光镜，获得两束偏振光，根据两束偏振光的光强比例计算滚动转角。API公司的实际产品SmartTRACK即采用了上述方案，其优点是姿态测量精度高，缺点是接收器需要配置体积较大的两自由度转台，重量较大。API公司的专利“Multi-dimensional measuring system”(美国专利US7230689)提出的一种手持三维测量设备省略了两自由度转台，测量隐藏点时由人工调整测杆姿态以使接收器与激光基本垂直，此时也可以获得接收器准确的六维位姿，缺点是增加了调整时间。

美国Leica公司专利“Method and device for determining spatial positionsand orientations”(美国专利号US6667798)通过在角锥棱镜尖头处开小孔，并在小孔后安置成像器件，通过激光在在成像器件上的斑点来测量姿态，但也不能测量滚动转角。Leica公司专利“Measurement system for determiningsix degrees of freedom of an object”(美国专利号US7312862)在上述专利基础上提出三种测量滚动转角的方案，第一种方案是在激光跟踪仪上安装摄像头，拍摄接收器上安装的发光标记物；第二种方案是激光跟踪仪上安装点光源并朝角锥棱镜发出散射光线，光线通过小孔后获得额外的斑点；第三种方案是激光跟踪仪上安装激光结构光光源，并朝角锥棱镜发出扇平面光线，在接收器上安装线阵视觉传感器，但这些方案都未见实际应用。采用角锥棱镜尖头处开小孔的缺点是难以获得清晰的光斑图像。

瑞典Geotronics公司的专利US5229828提出利用接收器内置的双摆装置来测量接收器与重力方向的两个倾角，在激光跟踪仪上额外安装一个与测距仪轴线平行的激光发射器，其发射的光束通过接收器上的光学成像单元获得入射激光与接收器的入射角，根据两个倾角和入射角进一步计算出接收器的姿态。该方案比采用上述角锥棱镜开小孔的方案更容易获得清晰的光斑图像，但是由于光学成像单元尺寸有限，因此姿态测量范围很小，而且由于双摆装置转角的动态测量精度较低，因此整体的位姿测量精度无法提高。

Leica公司的实际产品T-Probe主要采用摄影测量学实现三维姿态测量，即在普通的激光跟踪仪上方加装高分辨率摄像机，在接受器上同时安装有角锥棱镜和多个发光标记物(例如发光二级管)，通过激光跟踪仪来获得接受器的三维坐标，而通过数字摄影测量技术来计算接受器的三维姿态。摄影测量学虽然能同时测量位置和姿态，但是其位置测量精度比激光跟踪仪低很多，因此通常只取其获得的姿态测量数值。挪威Metronor公司拥有双目摄影测量学的专利(专利号：EP0880674，WO97/14015)，而瑞典MEEQ公司拥有单目摄影测量学的专利(瑞典专利号：SE444530)和双目摄影测量学的专利(美国专利号：US6131296)。摄影测量学中拍摄物与成像装置之间的距离一般很远，因此对成像装置的分辨率和光学系统的精度要求很高，导致整个系统的造价很高，而且测量精度难以提高。

加拿大Oreo Products Inc.公司的专利“Optical coordinate measuringsystem for large objects”(美国专利号：US5305091)的方案基于并联平台位姿测量原理，即在测量工作间墙壁上固定6个内置精密测距仪的经纬仪，而在接收器上安装有至少两个后向反射器，每个经纬仪具有跟踪功能，可以始终指向相应的后向反射器，根据6个测距仪的测距数值就可获得接收器的6维位姿，不需要精确测量经纬仪的转角。该方案的优点是在理论上可以提高六维位姿测量数据的空间综合精度，缺点是成本太高。

目前成本较低、基于单个激光跟踪仪的、不需要高分辨率摄像系统的高精度六维位姿测量装置还没见报道。

发明内容

为了克服现有技术中成本高、或者需要两自由度转台、或者需要高分辨率摄像系统等不足，本发明申请将现有技术中提出的近距离视觉处理方法对现有激光跟踪仪进行扩展，可以精确而方便地获得三维姿态。采用本发明可以替代昂贵的六维位姿测量激光跟踪仪。

本发明技术方案如下：

一种测量物体六维位姿的装置，其特征在于：由计算处理单元(1)、一个接收器(2)、一个激光跟踪仪(3)和至少一个激光发射器组成；计算处理单元与激光跟踪仪安装在固定地面上；接收器安装在待测运动物体(4)上；激光跟踪仪和接收器与计算处理单元(1)通讯，激光跟踪仪与接收器通过激光光路相连，激光发射器与接收器通过激光光路相连。

所述激光跟踪仪具有一个水平转动自由度和一个俯仰转动自由度；激光跟踪仪包含控制其水平转角和俯仰转角的驱动装置，还包含测量其水平转角和俯仰转角的转角测量装置；激光跟踪仪上安装有至少一个激光测距仪。

所述激光测距仪发射出激光束，所述激光测距仪是激光干涉仪，或是激光多普勒测距仪，或是激光绝对距离测量仪，或是激光结构光位置传感器。

所述激光发射器通常安装在激光跟踪仪上；所述每个激光发射器发射出至少一条的激光束，所述激光束与激光测距仪发射的激光束平行。

所述激光发射器也可以安装在两自由度转台上；所述每个激光发射器发射出至少一条的激光束；所述两自由度转台安装在激光跟踪仪上或固定安装在地面上；所述两自由度转台具有一个水平转动自由度和一个俯仰转动自由度；所述两自由度转台包括控制其水平转角和俯仰转角的驱动装置，还包括测量其水平转角和俯仰转角的转角测量装置。

所述激光发射器的个数为1时，激光发射器发射线结构光，或发射十字线结构光，或发射点阵结构光。

所述接收器包括反射靶标、至少一块投射面板和至少一个成像单元，而且每个投射面板至少对应一个成像单元；其中反射靶标是后向反射镜，或是半透明的反射贴片；投射面板的形状选自平面，或是曲面，或是多个平面的组合，或是多个曲面的组合；投射面板的材料是散射透光板，或是表面粗糙的散射反光板；成像单元的视场范围与投射面板的大小基本相等。

当投射面板的材料为散射透光板时，所述成像单元处于投射面板的背侧或者内部，当投射面板的材料为散射反光板时，所述成像单元处于投射面板的前侧。

所述成像单元包括成像电子装置和位于成像电子装置与投射面板之间的成像镜头；其中成像电子装置选自位置敏感探测器、电荷耦合器件、电荷注入器件、或基于互补金属氧化物半导体的光学成像装置。

所述成像单元包括至少一个直接安装在投射面板表面上的感光测量装置，感光测量装置选自位置敏感探测器、电荷耦合器件、电荷注入器件、或基于互补金属氧化物半导体的光学成像装置。

采用所述装置测量物体六维位姿的方法：通过现有的激光跟踪仪测量方法获得物体的三维位置，通过激光发射器发射激光束或者激光平面投射在接收器的投射面板上，产生激光光斑或者激光条纹；再通过近距离的图像捕捉和视觉处理技术和并联测量原理建立约束方程，求解约束方程获得物体的三维姿态，得到物体的六维位姿。

本发明具有如下特点：

1.本发明装置的三维位置测量通过激光跟踪仪获得，因此位置测量精度高。

2.本发明装置三维姿态测量精度高而成本低。本发明基于并联测量原理，通过两条以上的激光结构光实现三维姿态测量，与采用单条激光进行串联测量的激光跟踪仪相比，接收器不需要两自由度转台，因此接收器重量大大减轻，而且成本大大降低；本发明采用近距离的图像捕捉和视觉处理，投射面板与成像单元之间的距离只有最大测量范围的百分之一或是千分之一，与采用远距离图像捕捉和视觉处理的摄影测量学技术相比，不需要高分辨率的视觉传感器，而且定位精度更高。

3.本发明可以容易的简化成测量平面内物体的三维位姿，即二维位置(x，y)和一维转动θ_Z。

4.本发明可以在接收器上安装测杆或者三维激光扫描仪，从而可以测量经纬仪发射的激光光束难以达到的地方。

附图说明

图1为六维位姿测量装置的第一个实例的示意图；

图2为六维位姿测量装置进行三维姿态测量时的工作流程示意图；

图3为六维位姿测量装置的第二个实例的示意图；

图4为六维位姿测量装置的第三个实例的示意图；

图5为六维位姿测量装置的第四个实例的示意图。

具体实施方式

下面结合附图对本发明作进一步详述。

实施例1

如图1所示为本发明提出的六维位姿测量装置的第一个实例，该装置由一个计算处理单元1、一个接收器2和一个激光跟踪仪3组成。激光跟踪仪3通常固定安装在地面上。接收器2通过连接件或者直接安装在六维位姿待测的运动物体4上。图中接收器坐标系20为O′-X′Y′Z′，固定在接收器上，而全局坐标系10为O-XYZ，通常固定在激光跟踪仪上，运动物体坐标系为O_m-X_mY_mZ_m(未显示)，固定在运动物体4上。由于接收器坐标系20相对于运动物体坐标系的位姿是始终固定的，而且可以通过标定方法获得，因此测量运动物体4的六维位姿等效于测量接收器坐标系20相对于全局坐标系10的六维位姿。为简化论述，后面所指的六维位姿测量均指对接收器2的六维位姿测量，因为接收器坐标系20的六维位姿与接收器2的六维位姿完全等效。

激光跟踪仪3具有水平转动θ_V和俯仰转动θ_H两个转动自由度，每个经纬仪的水平转角和俯仰转角通过驱动装置进行控制，并且转角值可以快速的测量。驱动装置通常采用伺服电机，也可采用精度更高的压电陶瓷电机和直接驱动电机，转角测量装置安装在经纬仪上，通常采用高精度编码器，编码器分辨率达到0.1″量级，测量重复精度均达到1″量级。激光跟踪仪3上装有高精度的测距仪30，通常采用激光干涉仪或者绝对距离测量仪。由于激光跟踪仪(包括激光全站仪和激光扫描仪等)的商业化产品十分常见，在此不作更详细的说明。

激光跟踪仪3的激光测距仪30上额外安装有两个小型的激光发射器31和32。激光测距仪发射一条激光束301，激光发射器31和32均发射一条激光束，分别是311和321。激光发射器31和32的指向通常与激光测距仪30的指向相平行，以获得较大的测量范围。激光发射器31和32与激光测距仪30之间的距离取决于三维姿态的测量精度，一般选择在50～100mm。激光发射器31和32通常采用半导体激光器，波长通常选择红色可见光或者红外光的波段，如635～690nm，激光器的输出功率一般为毫瓦级，如1mW。激光发射器31和32发射的激光也可以从激光测距仪30的光束中通过分光器件分离出来。一般来说，激光测距仪发射的激光束直径较大，例如激光干涉仪的光束直径在25mm量级，而本发明采用的小型激光发射器发射的激光束直径很小，根据目前的小型半导体激光器技术水平，在10m距离处，激光束的直径通常小于0.5mm，在100m距离处，激光束的直径通常小于5mm。本发明要求小型激光发射器具有较好的方向稳定性，例如环境温度升降1°时，激光束指向的变化最好小于1″，这样采用普通温控装置即可保证激光束指向的稳定性。

接收器2主要包括一个后向反射镜21、一块投射面板22和一个成像单元23。后向反射镜21最好采用角锥棱镜或360度反射棱镜，角锥棱镜中的三个垂直平面相交于尖点P。后向反射镜21镶嵌在投射面板22中间。本实例中的投射面板22的形状为平面，投射面板22的材料为具有漫透射性能的材料，例如各种半透明材料。成像单元23通常安装于投射面板22的后方，成像单元23包括一个成像镜头231和一个成像电子装置232。成像电子装置232可以是CCD、CMOS等常见光学成像器件。投射面板22的尺寸以及成像单元23与投射面板22的距离通常是所述装置最大测量范围的百分之一量级～千分之一量级；假设本实施例装置测量范围为10m，则投射面板尺寸不大于100mm×100mm，成像单元与投射面板的距离在100mm之内。后向反射镜21、投射面板22与成像单元23都固定在接收器上，因此它们相对于接收器坐标系20的位置和姿态可以通过标定精确获得。

计算处理单元1通过通信方式12、13与接收器2和激光跟踪仪3相连。通信方式12、13是有线方式，或是无线方式(包括通过测距仪的激光束进行激光通信)。

需要指出的是：

1)本实例中投射面板22采用具有漫透射性能的材料，但投射面板22也可以是具有一定的粗糙度的散射反光板，如典型的朗贝反光板(Lambertian reflector)，此时成像单元23通常安装于投射面板22的侧前方。投射面板22还可以是能够直接成像的感光器件，例如CCD、CMOS和PSD等常见光学成像器件，这种情况下可以省去成像单元23，但是考虑到大面积的光学成像器件成本很高，这种方案更适合测量距离较小(例如小于1m)的情况。

2)本实例中投射面板22的形状是平面，但也可以是球面或其他曲面。当投射面板22采用平面形状时，成像单元23推荐采用基于Scheimpflug原理的布局，可以在大范围内获得清晰的图像。成像镜头231中通常包括带通滤光镜片，只允许所采用激光波段的激光能通过，减少环境光影响，从而提高图像质量。

3)本实例中采用了两个小型的激光发射器31和32，但是可以用一个能生成点阵或激光条纹的激光结构光发射器代替。在实际应用中，为使测量结果具有更好的准确性和鲁棒性，可以安装两个以上的激光发射器，也可以让每个激光发射器发射两条或者更多的相互平行的激光光束。

4)图1中为避免示意图过于拥挤而将全局坐标系10放置在激光跟踪仪3的基座上，将接收器坐标系20放置在接收器的边框上。上述坐标系的位置可以根据算法是否方便进行调整。根据矩阵变换的基本规律，对上述坐标系进行平移不会改变接收器坐标系20相对于全局坐标系10的三维姿态，为了简化论述和便于理解，在本实例中假设全局坐标系10的原点O与激光跟踪仪3的垂直转轴和水平转轴的交点重合，而假设接收器坐标系20的原点O′与后向反射镜21的尖点P重合。

本发明测量运动物体六维位姿的方法结合所述装置的基本工作过程一并描述如下：

通过自动跟踪控制步骤控制激光跟踪仪的方位角，使激光跟踪仪上的激光测距仪始终指向接收器上的后向反射镜，同时激光发射器发射的激光落在接收器上的投射面板上。如图1所示，激光测距仪30发射出激光束301，激光束301命中后向反射镜21后被后向反射，形成激光束302，激光束302本质上与激光束301平行，并进入激光测距仪30。激光发射器31、32分别发射的激光束311、321落在接收器2上的投射面板22上，形成相应的激光光斑312、322。

通过激光跟踪仪测量步骤获得接收器坐标系的原点在全局坐标系中的三维坐标。激光测距仪30根据激光束301和302的特征计算出后向反射镜21的尖点P与激光测距仪30的距离，激光跟踪仪3进一步根据方位角θ_V和θ_H计算出后向反射镜21的尖点P相对于全局坐标系10的三维坐标(X_P，Y_P，Z_P)，这即是接收器坐标系20的原点O′在全局坐标系10中的三维坐标(x，y，z)。

再通过激光跟踪仪的矩阵变换获得激光测距仪和激光发射器所发射的激光线条在全局坐标系中的数学方程。一般来说，激光发射器31、32相对于激光测距仪30的相对位置和姿态可以精确的标定，根据激光跟踪仪的方位角θ_V和θ_H的测量值就可以获得两条激光束311、321在全局坐标系10中的方程，根据几何代数的基本常识，空间中的一条直线由两个三元一次线性方程确定。假设两条激光束311、321所对应直线的代数方程分别为：

$\left\{\begin{array}{c}{a}_{311}x+b_{311}y+c_{311}z+d_{311}=0\\ e_{311}x+f_{311}y+g_{311}z+h_{311}=0\end{array}\right.$

$\left\{\begin{array}{c}{a}_{321}x+b_{321}y+c_{321}z+d_{321}=0\\ e_{321}x+f_{321}y+g_{321}z+h_{321}=0\end{array}\right.$

其中：下标311表示激光束311对应的直线方程，下标321表示激光束321对应的直线方程。上述方程中的系数只取决于激光跟踪仪的方位角测量值和激光跟踪仪的固定尺寸。

另一方面，通过成像单元对投射面板上的激光光斑或者激光条纹进行拍摄和图像处理，获得激光光斑或者激光条纹在成像单元中的映像的像素坐标；通过对成像单元的标定获得成像单元上像素坐标中每一个像素点相对于接收器坐标系的位置，利用像素点与接收器坐标系的一一对应关系获得激光光斑或者激光条纹相对于接收器坐标系的局部坐标值。如图1所示，通过对成像电子装置232拍摄的图像进行处理，可以获得激光光斑在成像电子装置232上的二维坐标。根据小孔成像原理，激光光斑与其在成像电子装置232上的映像之间的连线必然经过成像镜头231的透镜中心。由于投射面板22、成像镜头231和成像电子装置232相对于接收器坐标系20的位置是固定的，因此激光光斑在成像电子装置232上的二维坐标与激光光斑相对于接收器坐标系20的三维坐标之间存在固定的一一映射，因此可以通过小孔成像数学模型获得这些激光光斑相对于接收器坐标系20的三维坐标；或者采用直接标定方法，即对于成像电子装置222上的每一个像素，直接标定并记录下其在投射面板22上对应点在接收器坐标系20中的三维坐标。假设投射面板22的大小为100mm×100mm，成像电子装置232的像素阵列为1024像素×1024像素，成像单元23的视场大小与投射面板大小基本相等，则成像电子装置222的视觉分辨率小于0.1mm。

再通过假设接收器坐标系相对于全局坐标系的三维姿态获得成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系的全局三维坐标值。由于接收器坐标系20相对于全局坐标系10的三维姿态是三个未知变量(α，β，γ)，如果已知一个激光光斑A在接收器坐标系20中的相对坐标(X_A′，Y_A′，Z_A′)，可以根据以下齐次坐标变换获得该激光光斑在全局坐标系10中的三维全局坐标(X_A，Y_A，Z_A)：

其中Rot(Z，γ)Rot(X，β)Rot(Z，α)表示先绕全局坐标系的Z轴旋转α角度，接着绕X轴旋转β角度，再绕Z轴旋转γ角度，Trans(x，y，z)表示沿着矢量[x，y，z]平移。因此可以获得

$\left[\begin{array}{c}{X}_A\\ Y_A\\ Z_A\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& x\\ r_{21}& r_{22}& r_{23}& y\\ r_{31}& r_{32}& r_{33}& z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_A^{\′}\\ Y_A^{\′}\\ Z_A^{\′}\\ 1\end{array}\right]$

其中，

r₁₁＝cosγcosα-cosβsinαsinγ

r₁₂＝cosγsinα+cosβcosαsinγ

r₁₃＝sinγsinβ

r₂₁＝-sinγcosα-cosβsinαcosγ

r₂₂＝-sinγsinα+cosβcosαcosγ

r₂₃＝cosγsinβ

r₃₁＝sinβsinα

r₃₂＝-sinβcosα

r₃₃＝cosβ

可见给定一个激光光斑在接收器坐标系20中的相对坐标(X_A′，Y_A′，Z_A′)，同时已测量出接收器坐标系20的原点在全局坐标系10中的三维坐标(x，y，z)，则该激光光斑在全局坐标系10中的全局坐标(X，Y，Z)分别是三个以接收器坐标系20的三维姿态(α，β，γ)为变量的函数表达式。

最后通过同步触发测量步骤建立激光线条在全局坐标系中的数学方程和激光光斑全局坐标的函数表达式之间的约束关系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿。由于光斑312、322必须分别位于两条激光束311、312对应的直线上，将两个光斑的三维全局坐标对应的函数表达式分别代入相应激光束的两个方程，可以获得具有三个变量的四个方程，

$\left\{\begin{array}{c}{a}_{311}{x}_{312}+b_{311}{y}_{312}+c_{311}{z}_{312}+d_{311}=R(\mathrm{\α},\mathrm{\β},\mathrm{\γ})=0\\ e_{311}{x}_{312}+f_{311}{y}_{312}+g_{311}{z}_{312}+h_{311}=S(\mathrm{\α},\mathrm{\β},\mathrm{\γ})=0\end{array}\right.$

$\left\{\begin{array}{c}{a}_{321}{x}_{322}+b_{321}{y}_{322}+c_{321}{z}_{322}+d_{321}=T(\mathrm{\α},\mathrm{\β},\mathrm{\γ})=0\\ e_{321}{x}_{322}+f_{321}{y}_{322}+g_{321}{z}_{322}+h_{321}=O(\mathrm{\α},\mathrm{\β},\mathrm{\γ})=0\end{array}\right.$

在以上四个方程中，假设激光光斑312的全局坐标为(x₃₁₂，y₃₁₂，z₃₁₂)，激光光斑322的全局坐标为(x₃₂₂，y₃₂₂，z₃₂₂)，显然它们都是以接收器坐标系20的三维姿态(α，β，γ)为变量的函数表达式。

求解以上四个方程所组成的方程组可以获得接收器坐标系20相对于全局坐标系10的三维姿态(α，β，γ)。求解方程组的算法可以是经典的牛顿-拉夫逊方法、各种最优方法、同伦法和区间分析法等等。

事实上，上述激光光斑必须分别位于激光束对应的直线上的约束关系可以采用其他表达方法，例如光斑与相应激光束的距离等于零，并获得不同的数学方程。接收器坐标系20相对于全局坐标系10的三维姿态也可以采用其他等效表示方法进行表示，例如采用四元数表示。

图2为六维位姿测量装置进行实际测量时的工作流程示意图，包括同步触发测量的步骤。该工作流程针对第一个实例进行解释，但是可以推广用于后述的其他实例。具体工作流程如下：

步骤101，将激光跟踪仪3固定在地面。步骤102将接收器2固定安装在待测移动物体上。步骤103，调节激光跟踪仪3的水平转角和俯仰转角，使激光光束301落在接收器2的后向反射镜21上，而激光光束311和321落在接收器2的投射面板22上。步骤104，计算处理单元1发送触发信号分别给激光跟踪仪3和接收器2。步骤105，激光跟踪仪3将触发时刻所测量得到的接收器2的三维坐标(x，y，z)和自身的方位角(θ_V，θ_H)测量值发送给计算处理单元1，与此同时接收器2的成像单元23触发高速快门，捕获投射面板22上的光斑图像，并将计算获得的激光光斑在成像电子装置232中的二维坐标值发送给计算处理单元1。步骤106，计算处理单元1根据光斑的二维坐标和标定好的映射关系计算得到光斑相对于接收器坐标系20的局部三维坐标值；计算处理单元1根据激光光斑位于激光直线上的约束关系，将激光光斑在全局坐标系中的函数表达式代入激光线条对应的数学方程，建立以接收器2的三个姿态参数(α，β，γ)为未知变量的四个方程，求解这四个方程，得出接收器坐标系20相对于全局坐标系10的姿态参数，获得接收器坐标系20的六维位姿(x，y，z，α，β，γ)。步骤107，激光跟踪仪3执行自动跟踪控制算法。然后循环执行步骤104至步骤107直到完成测量。

上述使激光跟踪仪的自动跟踪控制算法的流程简介如下，在每次测量时，由激光跟踪仪3计算出接收器2的当前位姿，将当前位姿与接收器2在上一次测量时的位姿之差除以采样间隔得出接收器2的运动速度，进一步根据接收器2的运动速度推测接收器2在下一次测量时将到达的位姿，计算出激光跟踪仪3需要到达的新转角位置，以保证激光束301仍然落在接收器2的后向反射镜21上，并。求解接收器2的运动速度还可以利用接收器2之前的运动轨迹，通过滤波和预测方法获得。

上述的同步触发测量方法也可以采用连续测量和测量值插补相结合的方法代替，即计算处理单元1不必发送触发信号给激光跟踪仪3和接收器2，而激光跟踪仪3连续将接收器2的三维坐标(x，y，z)和自身的方位角(θ_V，θ_H)测量值发送给计算处理单元1，接收器2也连续将计算获得的激光光斑在成像电子装置232中的二维坐标值发送给计算处理单元1。计算处理单元1根据接收到的数据和数据到达的时间插补出某固定时刻的数据，进一步根据插补出的数据建立方程组并求解出接收器坐标系20在全局坐标系10中的六维位姿。

本装置的位置测量精度和跟踪性能基本取决于激光跟踪仪的位置测量精度，设激光跟踪仪的方位角的测量精度为1″，当测量范围为10m时，位置测量精度可以达到0.05mm量级，横向跟踪速度可以达到3m/s。

而本装置的姿态测量精度可以达到如下指标：设成像采用1024像素×1024像素的成像电子装置，拍摄面积为50mm×50mm的投射面板，可以获得高于0.05mm的分辨率，因此假设光斑312和322的位置分辨率为0.05mm。采用本发明方案，触发信号的传输延时和成像单元的快门曝光时间都可以达到10μs量级，当运动物体速度为1m/s时，成像单元的测量偏差为0.01mm量级。已知接收器的投射面板的等效半径为25mm，因此姿态测量精度可以达到(0.05/25)×(180°/π)＝0.114°，即0.1°量级，如果采用1/10亚像素图像处理，可以达到0.01°量级。

实施例2

图3所示为本发明提出的六维位姿测量装置的第二个实例。与第一个实例不同之处是，接收器2上的投射面板22采用了球形，而成像单元23位于在投射面板的内侧；投射面板22上设有磁性支座24；后向反射镜21采用三棱镜反射球，可以可靠的吸附在磁性支座24上面，操作者可以手动调节后向反射镜21的方位，上述两项修改可以获得更大的三维姿态测量范围。当然磁性支座24也可以设计成具有一个自由度或两个自由度的手动转台或自动转台。手动转台和自动转台的转动轴线都经过三棱镜反射球的球心，可以保持三棱镜反射球的球心相对于接收器2的位置不变。手动转台可以选用普通的高精度转盘，自动转台通过在高精度转盘的转轴上安装旋转电机进行驱动，或通过球面电机直接驱动三棱镜反射球的表面实现转动。由于自动转台不需要准确控制后向反射镜与入射激光的角度，只需使后向反射镜能接收到入射激光，因此与美国专利US6667798中的方案相比，可以很容易实现自动转台的轻量化。

实施例3

图4所示为本发明提出的六维位姿测量装置的第三个实例。该实例主要适合小范围的测量。激光跟踪仪3上的测距仪30为采用三角法测量的结构光位置传感器，可选择日本Keyence公司的LK-G500，测量范围为250mm-1000mm。由于三角法测量不适合测量高反光表面，因此本实例中用反射贴片代替后向反射器21。本实例中投射面板22采用散射透光材料，可使入射激光的一部分被漫反射，一部分被漫透射，因此反射贴片的漫反射功能可以由投射面板22实现。本实例中投射面板22是球面，但也可以是平面或其它曲面。由于没有后向反射镜21，在本实例中假设接收器坐标系20的原点O′位于成像单元23的框架上。测距仪30上的激光发射器304发射的激光束301落在投射面板22上，形成光斑302，部分激光被散射并沿方向302返回至测距仪30上的接收窗口303，并最终在测距仪30内部的线阵成像器件上成像，因此激光跟踪仪3可以准确地计算出光斑302的三维全局坐标(x₁₂，y₁₂，z₁₂)。激光跟踪仪3上的另两个激光发射器31、32分别发射的激光束311、321在投射面板上也分别形成光斑312、322，因此成像单元23拍摄的图像包括三个光斑302、312、322。

本实例的计算方法如下：假设已知接收器坐标系20的六维位姿(x，y，z，α，β，γ)，根据接收器2的位姿变换可以计算出激光光斑302的全局坐标(x₃₀₂，y₃₀₂，z₃₀₂)，激光光斑312的全局坐标(x₃₁₂，y₃₁₂，z₃₁₂)，激光光斑322的全局坐标(x₃₂₂，y₃₂₂，z₃₂₂)，这些都是显然它们都是以接收器坐标系20的六维位姿为未知变量的函数表达式。根据实例1中建立约束方程的基本思想，即激光光斑的全局坐标应满足相应的直线方程，以及计算得到的光斑全局坐标应与激光跟踪仪测量得到的光斑全局坐标一致，不难获得七个约束方程，求解这七个方程即可获得接收器坐标系20的六维位姿(x，y，z，α，β，γ)。

本实例的另一种计算方法是：首先将接收器坐标系20的原点O′临时平移到与激光光斑302重合的位置，这时只需根据第一个实例中介绍的方法获得4个方程即可求解接收器坐标系20的三维位姿(α，β，γ)，然后根据接收器坐标系20的三维姿态计算出平移矢量在全局坐标系10中的数值，从而获得尚未平移时原点O′的三维位置(x，y，z)。

实施例4

图5所示为本发明提出的六维位姿测量装置的第四个实例。该实例中将小型激光发射器31、32不再安装在激光跟踪仪3上，而是安装在两个两自由度转台5、6上。两自由度转台通常采用经纬仪的形式，即每个两自由度转台具有一个水平转动自由度和一个俯仰转动自由度，包括控制其水平转角和俯仰转角的驱动装置，还包括测量其水平转角和俯仰转角的转角测量装置。两自由度转台5、6分别通过通信方式15、16与计算处理单元1连接，将各自的水平转角和俯仰转角发送给计算处理单元1。

接受器2包括一个后向反射镜21和两个投射面板22a、22b，均固定在刚性支架25上，投射面板22a、22b分别对应一个成像单元23a和23b。其中后向反射镜21和投射面板22a、22b相对于接受器坐标系的相对位置和姿态可以通过标定方法获得。激光跟踪仪3将激光束射向后向反射镜21，而两个两自由度转台5、6上的激光发射器31和32将激光束311、321分别射向对应的投射面板22a、22b，形成激光光斑312和322。

该实施例的计算方法与实施例1基本相同，即由激光跟踪仪3进行三维位置测量，而根据激光光斑位于相应直线上的约束进行三维姿态测量。唯一的区别在于计算激光束311、321的直线方程时是根据两自由度转台5、6的方位角以及它们相对于激光跟踪仪3的位置和姿态，而不是根据激光跟踪仪3的方位角。其中两自由转台5、6相对于激光跟踪仪3的位置和姿态可以通过外部设备直接标定方法或者自标定程序获得。一种直接标定方法是将两自由度转台5、6和激光跟踪仪3固定安装在一个尺寸已知的支架上，或者将两自由度转台5、6固定安装在激光跟踪仪3上。自标定程序则通过固定接受器2，并增量调整两自由度转台的方位角，通过求解方程实现。

该实施例的优点在于由于投射面板和后向反射镜三者之间距离较远，可以获得很高的三维姿态测量精度。

需要指出的是，虽然这里只给出了六维位姿测量装置的四个实施例，但是可对上述实施例进行一些显而易见的修改以得到更多的实例，例如将本发明的激光光斑投射与近距离拍摄方法与Leica公司专利US7312862中的锥角棱镜开小孔的方法结合。

此外，在上述实例中，激光跟踪仪安装在固定地面上，接收器安装在待测运动物体上，但是也可以将激光跟踪仪和接收器中的一个安装在待测运动物体上，另一种安装在固定地面上。在上述实例中，计算处理单元和激光跟踪仪作为两个独立单元，但是显然计算处理单元也可以被集成到激光跟踪仪内部。

还需要指出的是本发明主要用于六维位姿测量，但是可以容易的简化成测量平面内物体的三维位姿，即二维位置(x，y)和一维转动θ_Z。具体方案是将激光跟踪仪的俯仰转动自由度去除，并且只需一个小型激光发射器。

本发明覆盖的范畴包括：采用二维或三维位置测量仪器实现物体的位置测量，同时采用小直径激光束在接收器投射面板上生成多个光斑，并对激光光斑图像进行近距离拍摄和处理以获得光斑的局部坐标，采用并联测量原理进行方程求解以得到物体姿态的相关方案。需要指出的是Leica公司专利US7312862中第三个方案中的部分结构与本发明看起来较为近似，但该方案中采用大直径激光束通过小孔后落在成像电子器件上生成光斑，成像电子器件必须位于后向反射镜的后侧；而本发明中激光光斑落在投射面板上，而且投射面板和成像电子器件可以位于后向反射镜的旁边、后侧或前侧。此外Leica公司专利US7312862中对三维姿态的测量本质上是基于串联测量原理，先利用小孔后的成像电子器件测量二维位姿(即俯仰转角和偏转转角)，然后通过外部测量器件单独计算滚动转角；而本发明中三维姿态的测量采用并联测量原理，同时计算俯仰转角、偏转转角和滚动转角。

Claims (11)

1.一种测量物体六维位姿的装置，其特征在于：由计算处理单元(1)、一个接收器(2)、一个激光跟踪仪(3)和至少一个激光发射器组成；计算处理单元与激光跟踪仪安装在固定地面上；接收器安装在待测运动物体(4)上；激光跟踪仪和接收器与计算处理单元(1)通讯，激光跟踪仪与接收器通过激光光路相连，激光发射器与接收器通过激光光路相连。

2.按权利要求1所述的装置，其特征在于：所述激光跟踪仪具有一个水平转动自由度和一个俯仰转动自由度；激光跟踪仪包含控制其水平转角和俯仰转角的驱动装置，还包含测量其水平转角和俯仰转角的转角测量装置；激光跟踪仪上安装有至少一个激光测距仪。

3.按权利要求2所述的装置，其特征在于：所述激光测距仪发射出激光束，所述激光测距仪是激光干涉仪，或是激光多普勒测距仪，或是激光绝对距离测量仪，或是激光结构光位置传感器。

4.按权利要求1所述的装置，其特征在于：所述激光发射器安装在激光跟踪仪上；所述每个激光发射器发射出至少一条的激光束，所述激光束与激光测距仪发射的激光束平行。

5.按权利要求1所述的装置，其特征在于：所述激光发射器安装在两自由度转台上；所述每个激光发射器发射出至少一条的激光束；所述两自由度转台安装在激光跟踪仪上或固定安装在地面上；

6.按权利要求5所述的装置，其特征在于：所述两自由度转台具有一个水平转动自由度和一个俯仰转动自由度；所述两自由度转台包括控制其水平转角和俯仰转角的驱动装置，还包括测量其水平转角和俯仰转角的转角测量装置。

7.按权利要求4或5所述的装置，其特征在于：所述激光发射器的个数为1时，激光发射器发射线结构光，或发射十字线结构光，或发射点阵结构光。

8.按权利要求1所述的装置，其特征在于：所述接收器包括反射靶标、至少一块投射面板和至少一个成像单元，而且每个投射面板至少对应一个成像单元；其中反射靶标是后向反射镜，或是半透明的反射贴片；投射面板的形状选自平面，或是曲面，或是多个平面的组合，或是多个曲面的组合；投射面板的材料是散射透光板，或是表面粗糙的散射反光板；成像单元的视场范围与投射面板的大小基本相等。

9.按权利要求8所述的装置，其特征在于：当投射面板的材料为散射透光板时，所述成像单元处于投射面板的背侧或者内部，当投射面板的材料为散射反光板时，所述成像单元处于投射面板的前侧。

10.按权利要求8所述的装置，其特征在于：所述成像单元包括成像电子装置和位于成像电子装置与投射面板之间的成像镜头；其中成像电子装置选自位置敏感探测器、电荷耦合器件、电荷注入器件、或基于互补金属氧化物半导体的光学成像装置。

11.按权利要求8所述的装置，其特征在于：所述成像单元包括至少一个直接安装在投射面板表面上的感光测量装置，感光测量装置选自位置敏感探测器、电荷耦合器件、电荷注入器件、或基于互补金属氧化物半导体的光学成像装置。

Images