CN101738161B - 一种测量运动物体六维位姿的设备和方法 - Google Patents

Abstract

本发明涉及一种测量运动物体六维位姿的设备及其方法。该设备由一个或多个经纬仪、一个或多个接收器和一个计算处理单元组成，每个经纬仪的水平转角和俯仰转角可控，而且经纬仪上安装有激光发射器，可以发射一条或多条激光结构光，投射到接收器上面；接收器包括投射面板和成像单元。计算处理单元利用经纬仪的转角和接收器获取的激光光斑图像数据建立方程组，求解方程组可以获得运动物体的六维位姿。本发明可以连续测量在较大空间中运动的物体，具有精度高、成本低和便于布置等优点，可以替代激光跟踪仪、坐标测量机和室内GPS等昂贵的位姿测量设备。

Description

一种测量运动物体六维位姿的设备和方法

技术领域

本发明涉及测量运动物体在较大空间中六维位姿的设备，具体地说是一种基于激光束直线特性和并联机构位姿测量原理，采用图像处理技术实现运动平台六维位姿测量的设备和方法。该可以广泛应用于需要实现平移运动的场合，可以替代昂贵的激光跟踪仪和坐标测量机。

背景技术

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

在很多应用中，采用机械式测量装置就可以解决问题，例如利用设备自身的关节编码器进行运动学正解，如果设备本身不具有编码器或者编码器和机械装置的精度较低，则可以采用传统的三坐标测量机和便携关节式坐标测量机进行测量。但是坐标测量机的测量范围一般十分有限，而且难以在线测量运动物体。

为了测量在大范围运动的物体的六维位姿，通常采用全局定位系统。根据测量精度的高低，全局定位系统可以分为激光跟踪仪、室内GPS、基于激光导航方法的设备、基于激光信标方法的设备和基于视觉处理的设备。

激光跟踪仪采用经纬仪测量接收器的方位，而采用激光干涉原理测量接收器与经纬仪之间的距离，具有采样频率高和测量精度高的显著优点，但是设备十分昂贵。

室内GPS技术有多种，最典型的一种方案美国Arc Second公司的方案，在地面安装至少两个激光发射器，每个发射器产生两个成固定夹角的激光平面。发射器不停高速水平旋转，旋转频率大约在3000转/分左右，接收器上的传感器可获得激光平面经过的准确时间，接收器根据发射器投射来的光线时间特征参数，计算出接收器所在点的位置和角度。室内GPS的精度较高，而且可以同时测量大量接收器的位置，所有接收器在100m×100m测量空间内的位置测量精度可以达到0.1mm量级。不过由于该技术采用周期扫描方式，不同激光平面扫过同一传感器的时间有时间差，因此更适合静态物体或者缓慢移动物体的测量，如果要用于高速运动物体的测量，必须增加激光平面的数量，或采用超高速旋转的激光发射器。

基于激光导航方法的设备和基于激光信标方法的设备主要用于工厂自动移动小车的定位，目前主要用于平面位置和方位定位。激光导航方法的典型例子是在移动小车上安装可以水平旋转的激光站，激光站发射出激光光束，而将三个以上的反射镜安装在地面上，且反射镜安装位置已知。地面上的反射镜通常由若干个锥体棱镜组成，投射到反射镜上的激光可以沿入射方向返回，但与入射激光错开小段距离。移动小车上激光站不停旋转扫描，如果在三个角度有激光返回，则可计算出移动小车的位置和方位。激光信标方法与激光导航方法相反，在小车上安装三个以上的传感器，而在地面安装多个可以水平旋转的激光站，如果三个以上的传感器可以接受到激光则可计算出移动小车的位置和方位。为了实现连续测量，激光站的水平转角通常自动跟随小车的运动。激光导航方法和激光信标方法价格相对便宜，其原理可以扩展应用到空间六维位姿测量，但是目前尚未见报道。在这类方法中，传感器只是开关型传感器，即只检测信号的有无，并不测量激光光束在传感器上的落点的准确位置，因此测量精度较低。另一方面，在建筑施工行业常用的经纬仪交汇法采用可发射激光的经纬仪和能够判断激光是否打中靶心的光电接收靶，可以用于精确测量固定物体的位置和姿态，但这种测量系统缺乏跟踪运动物体的功能，因此无法测量运动物体的位姿。

基于视觉处理的全局定位技术有多种，最典型的是瑞典MEEQ公司的PosEye技术，通过在测量环境中布置多个位置已知的发光标志物，而在运动物体上安置摄像头，根据发光标志物在摄像头拍摄的图像中的坐标来计算运动物体的位姿。当测量距离大于10米时，发光标志物通常采用功率大于100mW的激光发光二极管，激光发光二极管的激光发散角一般大于30°，以保证摄像头能接收到足够的光线。但是该技术要求成像单元的分辨率很高，目前还很难获得较高的测量精度。另一种常见技术是双目视觉技术，即在移动物体上安装高亮标记物，通过双目视觉原理来计算标记物的位置，进一步计算运动物体的位姿，该技术同样受限于成像单元的分辨率，测量的精度较低。

目前成本较低、不需要高分辨率摄像系统的高精度全局定位设备还没见报道。

发明内容

为了克服现有技术中成本高、需要高分辨率摄像系统的全局定位设备的不足，本发明的目的在于提出一种成本较低、不需要高分辨率摄像系统的测量运动物体六维位姿，从而实现全局定位的设备和方法，采用本发明可以替代昂贵的激光跟踪仪、坐标测量机和室内GPS等设备。

本发明技术方案如下：

测量运动物体六维位姿的设备，由计算处理单元、至少一个接收器和至少一个经纬仪组成，计算处理单元与经纬仪安装在固定地面上；接收器安装在待测运动物体上；经纬仪和接收器通过通信线缆或者无线通信方式与计算处理单元通讯，经纬仪与接收器通过激光光路相连；

其中：所述经纬仪具有至少一个水平转动自由度和至少一个俯仰转动自由度；经纬仪包含驱动装置，用以控制其水平转角和俯仰转角；还包括转角测量装置，用以测量转角；每个经纬仪上安装有至少一个激光发射器；所述经纬仪个数为1时，经纬仪上安装的激光发射器个数至少为2；所述每个激光发射器发射出激光结构光，该激光结构光是至少一条光线的激光点束，或是至少一个条形光平面的激光线束，或是至少一个扇形光平面的激光线束，或是至少一个光圆柱面的激光圆形束，或是上述多种不同激光结构光的组合光束；

另外，所述经纬仪的俯仰自由度可以被固定或者被拆卸，得到具有一个水平转动自由度的简化结构，上面安装有至少一个激光发射器，每个激光发射器发射出一个扇形光平面的激光线束；

所述接收器包括至少一块投射面板和至少一个成像单元，而且每个投射面板至少对应一个成像单元；投射面板的尺寸以及成像单元与投射面板的距离均是所述设备最大测量范围的百分之一量级～千分之一量级范围内；假设设备测量范围为10m，则投射面板尺寸不大于100mm×100mm，成像单元与投射面板的距离在100mm之内；其中投射面板是光滑的反光镜面，或是表面粗糙的散射反光板，或是散射透光板；投射面板的形状选自平面，或是曲面，或是多个平面的组合，或是多个曲面的组合；

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

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

采用所述设备测量运动物体六维位姿的方法，通过自动跟踪控制方法、经纬仪位姿标定方法、成像单元图像处理方法、成像单元标定方法和同步触发测量方法实现；具体是：

-通过自动跟踪控制步骤控制经纬仪的方位角，使经纬仪的激光发射器始终指向接收器的投射面板，激光发射器发射激光线条或者激光平面投射在投射面板上，产生激光光斑或者激光条纹；

-通过对经纬仪位姿的标定获得经纬仪相对于固定在地面上的全局坐标系的位姿，进一步通过经纬仪的旋转变换矩阵获得激光发射器发射的激光线条或者激光平面在全局坐标系中的数学方程；

-通过成像单元对投射面板上的激光光斑或者激光条纹进行拍摄和图像处理，获得激光光斑或者激光条纹在成像单元中的映像的像素坐标；

-通过对成像单元的标定获得成像单元上每一个像素坐标相对于接收器坐标系的位置，利用像素坐标与接收器坐标系位置的一一对应关系获得激光光斑或者激光条纹相对于接收器坐标系的局部坐标值；

-通过设定接收器坐标系相对于全局坐标系的六维位姿获得成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系中的全局坐标值；

-通过同步触发测量步骤建立激光结构光生成的直线或者平面在全局坐标系中的数学方程和成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系的全局坐标值之间的约束关系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿；

其中：接收器坐标系为O′-X′Y′Z′，固定在接收器上，全局坐标系为O-XYZ，固定在地面上。

本发明所述成像单元标定方法是通过外部设备标定投射面板和成像单元相对于接收器坐标系的位置和姿态；

所述经纬仪位姿的标定采用外部标定或是自行标定两种方式；外部标定方法借助外部设备进行标定；自行标定方法通过将接收器固定、让经纬仪的转动的方式获得经纬仪相对于地面固定全局坐标系的位姿；具体为：将接收器固定，使经纬仪的水平转角和俯仰转角选取一组以上的不同数组，对于选取的每组数值均执行如下步骤：固定经纬仪的转角，经纬仪将水平转角和俯仰角数据发送给计算处理单元，计算处理单元根据经纬仪相对于全局坐标系的位姿以及水平转角和俯仰角数据计算出激光线条或者激光平面的数学方程；同时接收器将拍摄获得的激光光斑相对于接收器坐标系的局部坐标值发送给计算处理单元；计算处理单元以接收器坐标系相对于全局坐标系的六维位姿为变量，根据位姿坐标变换获得激光光斑在全局坐标系中的函数表达式；计算处理单元根据激光光斑位于激光线条或者结构光平面之上的约束条件，将激光光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方程，建立以经纬仪六维位姿为未知变量的多个方程，每次执行上述步骤获得若干个方程，将所有转角数组对应的方程合成一个方程组，求解该方程组获得所有经纬仪相对于地面固定全局坐标系的位姿；

所述同步触发测量的具体步骤为：计算处理单元发送触发信号分别给经纬仪和接收器，经纬仪将触发时刻的水平转角和俯仰角数据发送给计算处理单元，计算处理单元根据经纬仪相对于全局坐标系的位姿以及水平转角和俯仰角数据计算出激光线条或者结构光平面的数学方程；同时接收器将触发时刻计算获得的激光光斑相对于接收器坐标系的局部坐标值发送给计算处理单元；计算处理单元以接收器坐标系相对于全局坐标系的六维位姿为变量，根据坐标变换获得激光光斑在全局坐标系中的函数表达式；计算处理单元根据激光光斑位于激光线条或者结构光平面之上的约束条件，将激光光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方程，建立以接收器坐标系六维位姿为未知变量的方程组，求解该方程组获得接收器坐标系相对于全局坐标系的三维位置和三维姿态；

所述的同步触发测量方式也可以采用连续测量和测量值插补相结合的方式代替，即计算处理单元不发送触发信号给经纬仪和接收器，而经纬仪连续将水平转角和俯仰角的最新数据发送给计算处理单元，接收器也连续将计算获得的激光光斑相对于接收器坐标系的最新局部坐标值发送给计算处理单元。计算处理单元根据接收到的数值和接收到数值的时间插补出对应某固定时刻的数值，进一步根据插补出的数值建立方程组并求解出接收器坐标系在全局坐标系中的六维位姿。

所述自动跟踪控制的实现步骤具体为：由计算处理单元根据接收器的当前位置和之前位置推测接收器将到达的位置，计算出所有经纬仪需要到达的新转角位置，并将新转角指令发送给相应的经纬仪，经纬仪根据新转角指令控制其水平转角和俯仰转角至需要到达的新转角位置。

本发明具有如下特点：

1.本发明设备成本低而测量精度高。本发明通过两条以上的激光结构光实现采用并联测量，与采用单条激光结构光进行串联测量的激光跟踪仪相比，不需要高精度的激光干涉仪，因此成本大大降低；本发明采用同步测量方式或者轮询测量方式，与采用周期扫描测量方式的室内GPS系统相比，可以测量运动速度更高的物体；本发明采用短距离的视觉捕捉和处理，投射面板与成像单元之间的距离只有最大测量范围的百分之一或是千分之一，与采用远距离视觉捕捉和处理的PosEye技术相比，不需要高分辨率的视觉传感器，而且定位精度更高。

2.本发明可以在安装现场进行快速的自行标定，因此可以方便的增加经纬仪和接收器，也可以方便的移走冗余的经纬仪。

3.本发明可以通过增加经纬仪和接收器的数量来扩大测量范围和提高测量结果的准确性和鲁棒性。例如将三个经纬仪按照三角形分布布置在待测运动物体周围，可以使运动物体在较大空间内的位姿测量准确性波动幅度减小。

4.以三个经纬仪作为一个类似蜂窝的测量网格，可以通过坐标切换机制实现不同测量网格之间的过渡，实现大范围高精度测量。

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

6.本发明激光器发射的激光始终落在接收器上，投入人眼的概率非常低。即使偶尔进入人眼，由于激光可以选择红外波段，并且功率非常低，对人眼没有任何伤害。

附图说明

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

图2为六维位姿测量设备进行实际测量时的工作流程示意图；

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

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

图5为六维位姿测量设备的第四个实例的示意图；

图6为六维位姿测量设备的第五个实例的示意图；

图7为六维位姿测量设备的第六个实例的示意图；

图8为六维位姿测量设备的第七个实例的示意图；

图9为六维位姿测量设备的第八个实例的示意图。

具体实施方式

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

如图1所示为本发明提出的六维位姿测量设备的第一个实例，该设备由一个计算处理单元1、一个接收器2和三个经纬仪3、4、5组成。三个经纬仪3、4、5固定安装在地面上，通常布置成三角形。接收器2通过连接件7或者直接安装在六维位姿待测的运动物体8上。图中接收器坐标系20为O′-X′Y′Z′，固定在接收器上，而全局坐标系10为O-XYZ，固定在地面上，运动物体坐标系80为Om-Xm Ym Zm，固定在运动物体8上。由于接收器坐标系20相对于运动物体坐标系80的位姿是始终固定的，而且可以通过标定方法获得，因此测量运动物体8的六维位姿可以通过测量接收器坐标系20相对于全局坐标系10的六维位姿获得。

经纬仪3、4、5都具有水平转动和俯仰转动两个转动自由度，每个经纬仪的水平转角和俯仰转角通过驱动装置进行控制，并且转角值可以快速的测量。驱动装置通常采用伺服电机，也可采用精度更高的压电陶瓷电机，转角测量装置安装在经纬仪上，通常采用高精度编码器，编码器分辨率和测量重复精度均达到1″量级。经纬仪3、4、5上分别安装有一个激光发射器30、40、50。激光发射器30、40、50均发射至少一条激光束，分别是31、41、51。在实际应用中，为使测量结果具有更好的准确性和鲁棒性，可以让每个激光发射器发射两条或者更多的相互平行的激光光束。激光器安装在经纬仪上，通常采用半导体激光器，波长通常选择红色可见光或者红外光的波段，如635nm，激光器的输出功率一般小于5mW。

接收器2主要包括一块投射面板21和一个成像单元22。投射面板的尺寸以及成像单元与投射面板的距离均是所述设备最大测量范围的百分之一量级～千分之一量级；本实施例设备测量范围为10m，则投射面板尺寸不大于100mm×100mm，成像单元与投射面板的距离在100mm之内。本实例中的投射面板21采用类似360度全景相机中的双曲面反光镜面。成像单元22通常处于投射面板21的下方，通过结构件29与投射面板21相固连。结构件29可以采用透明材料，或者镂空的不透明材料。成像单元22包括一个成像镜头221和一个成像电子装置222。成像电子装置可以是PSD、CCD、CMOS等任意光学成像装置。当成像电子装置采用PSD时，由于单个PSD不能同时相应两个或者多个光点，可以采用多个PSD组成阵列。

成像镜头221的透镜中心通常放在双曲面的下侧焦点上，从整个环境射向双曲面上侧焦点的光被反射到下焦点，即成像镜头221的透镜中心。虽然经纬仪的转角可以控制，但是很难始终保证经纬仪发出的激光束能射向双曲面反射镜的上焦点，因此反光镜面通常是具有一定的粗糙度的散射反光板，如典型的朗贝反光板(Lambertian reflector)。投射面板21与成像单元22相对于接收器坐标系20的位置和姿态可以通过标定精确获得。

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

计算处理单元1通过通信线缆12、13、14、15与接收器12和经纬仪3、4、5相连。

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

通过自动跟踪控制步骤控制经纬仪的方位角，使经纬仪的激光发射器始终指向接收器的投射面板，激光发射器发射激光线条或者激光平面投射在投射面板上，产生激光光斑或者激光条纹。如图1所示，经纬仪3、4、5上的激光发射器30、40、50发射的激光束31、41、51落在接收器上的投射面板上，形成相应的激光光斑32、42、52。根据目前的小型半导体激光器技术水平，在10m距离处，激光光斑的直径通常小于0.5mm，在100m距离处，激光光斑的直径通常小于5mm。

再通过对经纬仪位姿的标定获得经纬仪相对于固定在地面上的全局坐标系的位姿，进一步通过经纬仪的旋转变换矩阵获得激光发射器发射的激光线条或者激光平面在全局坐标系中的数学方程。一方面，经纬仪3、4、5相对于绝对坐标系10的位姿可以采用外部设备预先测定，也可以采用本专利后面介绍的自行标定方法获得。再结合经纬仪水平转角和俯仰转角的测量值就可以获得三条激光点束31、41、51在全局坐标系10中的方程，每条激光束对应两个方程。假设三条激光点束31、41、51对应的代数方程分别为：

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

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

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

上述方程的系数只取决于经纬仪3、4、5在全局坐标系10中的位姿和经纬仪3、4、5的转角测量值。

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

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

$\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\mathrm{Trans}(x,y,z)\mathrm{Rot}(Z,\mathrm{\γ})\mathrm{Rot}(X,\mathrm{\β})\mathrm{Rot}(Z,\mathrm{\α})\left[\begin{array}{c}X\′\\ Y\′\\ Z\′\\ 1\end{array}\right]$

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

$\left[\begin{array}{c}X\\ Y\\ Z\\ 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\′\\ Y\′\\ Z\′\\ 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′，Y′，Z′)，该激光光斑在全局坐标系10中的全局坐标(X，Y，Z)分别对应三个以接收器坐标系20的六维位姿(x，y，z，α，β，γ)为变量的函数表达式。

最后通过同步触发测量步骤建立激光结构光生成的直线或者平面在全局坐标系中的数学方程和成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系的全局坐标值之间的约束关系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿。由于三个光斑32、42、52必须分别位于三条激光束对应的直线上，将三个光斑的三维全局坐标对应的函数表达式分别代入相应激光束的两个方程，可以获得具有六个变量的六个方程，

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

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

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

在以上六个方程中，假设激光光斑32的全局坐标为(x₃₂，y₃₂，z₃₂)，激光光斑42的全局坐标为(x₄₂，y₄₂，z₄₂)，激光光斑52的全局坐标为(x₅₂，y₅₂，z₅₂)，显然它们都是以接收器坐标系20的六维位姿(x，y，z，α，β，γ)为变量的函数表达式。

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

事实上，上述激光光斑必须分别位于激光束对应的直线上的约束关系可以采用其他表达方法，例如三个光斑32、42、52与三条激光束的距离等于零，并获得不同的数学方程。接收器坐标系20相对于全局坐标系10的六维位姿也可以采用其他等效表示方法进行表示，例如三维位置采用(x，y，z)表示，而三维姿态(α，β，γ)采用四元数表示，或者三维位置和三维姿态统一采用双四元数表示。

图2为六维位姿测量设备进行实际测量时的工作流程示意图，包括经纬仪位姿自行标定的步骤和同步触发测量的步骤。该工作流程针对第一个实例进行解释，但是可以推广用于后述的其他实例。假设接收器2可放置在一个初始位置，在该初始位置时，接收器坐标系20与全局坐标系10重合。首先需要标定三个经纬仪3、4、5的六维位姿，共有十八个未知变量，因此需构建至少十八个方程才能得到一个确定性方程组，从而求解出这十八个变量。具体工作流程如下：

首先执行经纬仪位姿自行标定方法的步骤。在步骤101，将三个经纬仪3、4、5固定在地面。步骤102将接收器2移动到初始位置，使接收器坐标系20与全局坐标系10重合。在步骤103，调节经纬仪3、4、5的水平转角和俯仰转角使激光光束落在接收器2的投射面板21上。在步骤104，计算处理单元1发送触发信号分别给经纬仪3、4、5和接收器2。在步骤105，经纬仪3、4、5将触发时刻的水平转角和俯仰角数据发送给计算处理单元1，与此同时接收器2的成像单元22触发高速快门，捕获投射面板21上的光斑图像，并将计算获得的激光光斑相对于接收器坐标系10的局部坐标值发送给计算处理单元1。在步骤106计算处理单元1根据激光光斑位于激光直线上的约束关系，将激光光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方程，建立以三个经纬仪的18个位姿参数为未知变量的第1-6个方程。第二次执行步骤103，调节经纬仪3、4、5的水平转角和俯仰转角到第二组数值，但仍使激光光束落在接收器的投射面板上。然后依次执行步骤104、105和106，获得第7-12个方程。第三次执行重复步骤103，调节经纬仪3、4、5的水平转角和俯仰转角到第三组数值，但仍使激光光束落在接收器的投射面板上。然后依次执行步骤104、105和106，获得第13-18个方程。在步骤107，求解具有十八个变量和十八个方程的方程组，得出三个经纬仪相对于全局坐标系10的位姿参数，完成经纬仪位姿自行标定。

然后执行同步触发测量的步骤。在步骤108，将接收器2固定安装在待测移动物体上。在步骤109，启动自动跟踪控制算法，控制经纬仪3、4、5的水平转角和俯仰转角来跟踪接收器2的运动，使激光光束落在接收器2的投射面板21上。然后重复步骤104、步骤105和步骤106，在步骤110，求解在步骤106获得的六个方程，获得接收器坐标系20在全局坐标系10中的六维位姿。此后循环执行步骤109、步骤104、步骤105、步骤106和步骤110，即可连续获得接收器坐标系20在全局坐标系10中的六维位姿。

上述使经纬仪自动跟踪接收器2运动的自动跟踪控制算法的流程简介如下，在每次测量时，由计算处理单元1计算出接收器2的当前位姿，将当前位姿与接收器2在上一次测量时的位姿之差除以采样间隔得出接收器2的运动速度，进一步根据接收器2的运动速度推测接收器2在下一次测量时将到达的位姿，计算出所有经纬仪3、4、5需要到达的新转角位置，以保证激光光斑32、42、52仍然落在接收器2的投射面板21上，并将转角指令发送给相应的经纬仪32、42、52。求解接收器2的运动速度还可以利用接收器2之前的运动轨迹，通过滤波和预测方法获得。

上述的同步触发测量方法也可以采用连续测量和测量值插补相结合的方法代替，即计算处理单元1不必发送触发信号给经纬仪3、4、5和接收器2，而经纬仪3、4、5连续将水平转角和俯仰角的最新数据发送给计算处理单元1，接收器2也连续将计算获得的激光光斑相对于接收器坐标系10的最新局部坐标值发送给计算处理单元1。计算处理单元1根据接收到的数值和接收到数值的时间插补出对应某固定时刻的数值，进一步根据插补出的数值建立方程组并求解出接收器坐标系20在全局坐标系10中的六维位姿。

本设备的位置测量精度和姿态测量精度可以达到如下指标：采用高精度角编码器时，普通经纬仪的转角动态测量精度可以达到2″以下，转角静态测量精度可以达到1″以下。假设所采用的经纬仪的转角精度为2″，经纬仪与测量物体的距离为10m，经纬仪发射的激光束在10m距离处的置信区间半径小于0.1mm。而采用成像单元的图像分辨率可以轻易的达到0.1mm以下，例如采用1024像素×1024像素的成像电子装置拍摄面积为50mm×50mm的投射面板，可以获得0.05mm的分辨率。采用本发明方案，触发信号的传输延时和成像单元的快门曝光时间都可以达到10μs量级，当运动物体速度为1m/s时，成像单元的测量偏差为0.01mm量级。由于本发明采用多组方程进行求解，具有类似并联机构运动学求逆解的特点，多个激光束的指向误差不累积，因此本发明设备的位置测量精度可以达到0.1mm量级。已知接收器的投射面板的尺寸为50mm×50mm，假设激光光斑不规则的落在投射面板上，其中所有激光光斑与全部激光光斑的几何重心之间的最小距离为5mm，则在10m距离处，姿态测量精度可以达到(0.1/5)×(180°/π)＝1.14°，即1°量级。在后述的第八个实例中，在运动物体上安装三个接收器并使接收器之间的距离达到50mm，本发明设备的姿态测量精度可以达到0.1°量级。本实例的投射面板21只能接收来自四周的激光束，而不能接收来自上方和下方的激光束，因此可以测量的运动物体姿态变化范围较小，一般在100°左右。在后述实例中将介绍一些姿态测量范围更大的接收器。

本设备的自动跟踪控制步骤的跟踪性能可以达到如下指标：假设自动跟踪控制算法的控制周期为100Hz，运动物体与经纬仪的距离为10m，当运动物体运动速度为1m/s时，在每个控制周期，运动物体移动距离的预测准确度为10±2mm。假设经纬仪的水平转角和俯仰转角采用位置闭环控制，采用伺服电机和机械传动方式时，转角的控制准确度为±0.05°，因此激光光斑在投射面板上落点的准确度为9mm。假设接收器的投射面板的尺寸为50mm×50mm，每个控制周期都可以控制激光光斑落在以投射面板中心为圆心而半径为19mm的圆以内，即激光光斑始终落在投射面板上，因此本设备可以跟踪距离在10米外、运动速度在1m/s的运动物体。当测量距离为100m的运动物体时，可以采用压电陶瓷电机或者直接驱动电机来提高经纬仪转角控制精度，转角控制准确度可以达到±0.005°以下，同时还可以通过缩短跟踪控制的控制周期和增大投射面板尺寸等措施来保证准确的跟踪。

图3所示为本发明提出的六维位姿测量设备的第二个实例，该设备由一个计算处理单元1、一个接收器2和三个经纬仪3、4、5组成。与第一个实例不同的是，该设备中三个经纬仪3、4、5上的激光发射器30、40、50发射的是生成条形光平面的激光线束33、43、53。该设备中的接收器2采用了六面体形状，接收器2中的成像单元也采用另一种方式，即不采用基于透镜成像的成像电子装置，而是在投射面板21上直接安装感光测量装置23、24、25等，感光测量装置可以包括一个或者多个PSD、CCD、CMOS等感光传感器。感光测量装置表面通常覆盖带通滤光镜片，只允许所采用激光波段的激光能通过，减少环境光影响，从而提高图像。滤光镜片的厚度要求较薄，避免由于折射影响测量精度。

本实例的工作原理与第一个实例基本相同：激光线束33、43、53生成三个光平面，根据经纬仪水平转角和俯仰转角的测量值就可以获得三个激光线束33、43、53对应的光平面在全局坐标系10中的方程，每个激光线束生成的光平面对应一个方程，假设光平面33、43、53对应的代数方程分别为：

a₃₃x+b₃₃y+c₃₃z+d₃₃＝0

a₄₃x+b₄₃y+c₄₃z+d₄₃＝0

a₅₃x+b₅₃y+c₅₃z+d₅₃＝0

上述方程的系数只取决于经纬仪3、4、5在全局坐标系10中的位姿和经纬仪3、4、5的转角测量值。

激光线束33、43、53落在感光测量装置23、24、25上形成至少三条激光条纹26、27、28。感光测量装置23、24、25等可以获得激光条纹26、27、28相对于感光测量装置基准的像素坐标，由于感光测量装置23、24、25相对于接收器坐标系20的位置是固定不变而且可以通过标定方法获得，因此可以获得这些激光条纹相对于接收器坐标系20的三维位置。激光条纹上的每一个点在全局坐标系10中的全局坐标都是以接收器坐标系20六维位姿为变量的函数表示式。在激光条纹26、27、28上分别选取两个点，总共六个点。这六个点分别位于光平面33、43、53上，因此可以获得具有六个变量的六个方程。求解该方程组成的方程组可以获得接收器坐标系20相对于全局坐标系10的六维位姿。采用激光线束的好处是可以在每条激光条纹上选取两个以上的点，从而获得更多的约束方程，提高方程组求解的准确性和可靠性。

图4所示为本发明提出的六维位姿测量设备的第三个实例，该设备由一个计算处理单元1、一个接收器2和两个经纬仪3、4组成。其中经纬仪3上的激光发射器发射30发射出生成光平面的激光线束33，而经纬仪4上的激光发射器发射40发射至少两条激光束41、42。根据前面两个实例的分析方法不难得到具有六个变量的六个方程，因此可以获得接收器坐标系20相对于全局坐标系10的六维位姿。

图5所示为本发明提出的六维位姿测量设备的第四个实例，该实例具有部件数量较少的特点。该设备由一个计算处理单元1、一个接收器2和一个经纬仪6组成。经纬仪6与前述经纬仪3、4、5的不同之处是增加了一个俯仰转动自由度，在两个俯仰转动自由度上分别安装有激光发射器60、61。激光发射器60发射出两条激光束62、63，而激光发射器61发射出两条激光束64、65。激光束62、63、64、65落在接收器2的投射板21上形成四个光斑。根据第一个实例的分析方法不难得到具有六个变量的八个方程，因此可以获得接收器坐标系20相对于全局坐标系10的六维位姿。在此指出，虽然三条激光束即可得到具有六个变量的六个方程，但是采用四条激光束有利于提高系统测量结果的准确性和鲁棒性。接收器2与计算处理单元1之间的通信采用无线通信12′取代通信线缆12，从而避免运动物体8与通信线缆12的运动干涉。

图6所示为本发明提出的六维位姿测量设备的第五个实例，该实例与第四个实例的区别在于，接收器2上的投射面板21为球面状曲面，采用较薄的散射透光材料。成像单元22从投射面板21内部拍摄投射面板21上出现的激光光斑。成像单元22的成像镜头221采用广角近摄镜头，例如最短工作距离在10～200mm左右的广角近摄镜头。根据第一个实例的分析方法不难得到具有六个变量的八个方程，因此可以获得接收器坐标系20相对于全局坐标系10的六维位姿。与第一个实例相比，本实例的投射面板可以接收来自更大范围的激光，可以测量的运动物体姿态变化范围可以达到270°以上。

图7所示为本发明提出的六维位姿测量设备的第六个实例，该实例与第四个实例的区别在于，接收器2上具有两块投射面板21和21′，以及两个成像单元22和22′。投射面板21和投射面板21′都是曲面薄壳，其中投射面板21采用较薄的散射透光材料，而投射面板21′采用相对较厚的散射透光材料，在两个投射面板上均有一部分激光被反射，而另一部分激光通过。经纬仪6上的激光发射器60、61发射出两个激光点束62、64。激光点束62在投射面板21和21′上分别形成两个激光光斑66、67，激光点束64在投射面板21和21′上分别形成两个激光光斑68、69。成像单元22拍摄投射面板21上的激光光斑，而成像单元22′拍摄投射面板21′上的激光光斑。根据第一个实例的分析方法不难得到具有六个变量的八个方程，因此可以获得接收器坐标系20相对于全局坐标系10的六维位姿。事实上，由于投射面板21是较薄的散射透光材料，投射面板21′上的光斑也可能透过投射面板21在成像单元22中成像，因此成像单元22′可以省略，在这种情况下，要求成像单元22的图像处理算法能排除由于多重反射形成的干扰光斑。

图8所示为本发明提出的六维位姿测量设备的第七个实例，该设备由一个计算处理单元1、一个接收器2和两个结构经过简化的经纬仪9、11组成。该设备中的经纬仪9、11的俯仰自由度被固定或者被移除，接收器2采用了六面体形状。经纬仪9、11只具有一个水平旋转自由度。经纬仪9、11上安装激光发射器91、111。激光发射器91发射出一个扇形光平面的激光线束92，激光发射器111发射出一个扇形光平面的激光线束112，而且激光线束92和激光线束112通常与地面垂直。激光线束92投射到接收器2上的投射面板21上生成两段激光条纹93、94，激光线束92投射到投射面板21上生成两段激光条纹113、114。根据第二个实例的分析方法不难得到具有六个变量的八个方程，因此可以获得接收器坐标系20相对于全局坐标系10的六维位姿。

图9所示为本发明提出的六维位姿测量设备的第八个实例。三个经纬仪3、4、5固定在地面上，通常布置成三角形。移动物体8上安装三个分开放置的接收器20、20a、20b，接收器20与计算处理单元1通过无线通信12′进行连接，接收器20a、20b与接收器20之间通过有线通信或者无线通信方式进行连接。其中接收器2对应的接收器坐标系20为O′-X′Y′Z′，接收器2a对应的接收器坐标系20a为O＂-X＂Y＂Z＂，接收器2b对应的接收器坐标系20b为O＂′-X＂′Y＂′Z＂′。经纬仪3、4、5上分别安装有一个激光发射器30、40、50。激光发射器30、40、50均发射至少一条激光束，分别是31、41、51。激光束31、41、51分别落在接收器2、2a、2b的投射面板21、21a、21b上，生成三个激光光斑32、42、52。在实际测量时，每个经纬仪优先将激光结构光投射到距离该经纬仪较近而且未被遮挡的接收器上。由于本实例中的接收器20、20a、20b之间的距离通常远大于接收器的尺寸，因此获得更准确的姿态测量结果。

本实例的工作原理如下：在实际测量之前先标定各个接收器之间的相对位姿。选取接收器2为主接收器，使运动物体8固定不动，并测量所有接收器坐标系20、20a、20b相对于全局坐标系10的六维位姿，通过坐标变换矩阵可以获得其他两个接收器2a、2b上的接收器坐标系20a、20b相对于主接收器2的接收器坐标系20的相对六维位姿。在实际测量时，运动物体8开始运动，接收器2a将拍摄和处理获得的激光光斑42在本接收器坐标系20a上的坐标值发送给接收器20，接收器2b将拍摄和处理获得的激光光斑52在本接收器坐标系20b上的坐标值发送给接收器20，接收器20根据接收器坐标系20a、20b相对于接收器坐标系20的位姿关系，获得接收器2a、2b上的激光光斑42、52在接收器坐标系20中的局部坐标，然后采用第一个实例中论述的方程组构建方法计算得到接收器坐标20相对于全局坐标系10的六维位姿。

需要指出的是，虽然这里只给出了六维位姿测量设备的八个实例，但是通过对上述实例的部件进行组合可以得到更多的实例。此外，在上述八个实例中，经纬仪安装在固定地面上，接收器安装在待测运动物体上，但是也可以将经纬仪和接收器中的一种安装在待测运动物体上，另一种安装在固定地面上。

Claims (5)

1.一种测量运动物体六维位姿的方法，其特征在于：通过自动跟踪控制步骤控制经纬仪的方位角，使经纬仪的激光发射器始终指向接收器的投射面板，激光发射器发射激光线条或者激光平面投射在投射面板上，产生激光光斑或者激光条纹；再通过对经纬仪位姿的标定获得经纬仪相对于固定在地面上的全局坐标系的位姿，进一步通过经纬仪的旋转变换矩阵获得激光发射器发射的激光线条或者激光平面在全局坐标系中的数学方程；通过成像单元对投射面板上的激光光斑或者激光条纹进行拍摄和图像处理，获得激光光斑或者激光条纹在成像单元中的映像的像素坐标；通过对成像单元的标定方法获得成像单元上每一个像素坐标相对于接收器坐标系的位置，利用像素坐标与接收器坐标系位置的一一对应关系获得激光光斑或者激光条纹相对于接收器坐标系的局部坐标值；通过设定接收器坐标系相对于全局坐标系的六维位姿获得成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系中的全局坐标值；然后通过同步触发测量步骤建立激光结构光生成的直线或者平面在全局坐标系中的数学方程和成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系的全局坐标值之间的约束关系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿；其中：接收器坐标系为O′-X′Y′Z′，固定在接收器上，全局坐标系为O-XYZ，固定在地面上。

2.按权利要求1所述的方法，其特征在于：所述对成像单元的标定是通过外部设备标定投射面板和成像单元相对于接收器坐标系的位置和姿态。

3.按权利要求1所述的方法，其特征在于：所述经纬仪位姿的标定采用外部标定或是自行标定两种方式；外部标定方法借助外部设备进行标定；自行标定方法通过将接收器固定、让经纬仪的转动的方式获得经纬仪相对于地面固定全局坐标系的位姿；具体为：将接收器固定，使经纬仪的水平转角和俯仰转角选取一组以上的不同数组，对于选取的每组数值均执行如下步骤：固定经纬仪的转角，经纬仪将水平转角和俯仰角数据发送给计算处理单元，计算处理单元根据经纬仪相对于全局坐标系的位姿以及水平转角和俯仰角数据计算出激光线条或者激光平面的数学方程；同时接收器将拍摄获得的激光光斑相对于接收器坐标系的局部坐标值发送给计算处理单元；计算处理单元以接收器坐标系相对于全局坐标系的六维位姿为变量，根据位姿坐标变换获得激光光斑在全局坐标系中的函数表达式；计算处理单元根据激光光斑位于激光线条或者结构光平面之上的约束条件，将激光光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方程，建立以经纬仪六维位姿为未知变量的多个方程，每次执行上述步骤获得若干个方程，将所有转角数组对应的方程合成一个方程组，求解该方程组获得所有经纬仪相对于地面固定全局坐标系的位姿。

4.按权利要求1所述的方法，其特征在于：所述同步触发测量的具体步骤为：计算处理单元发送触发信号分别给经纬仪和接收器，经纬仪将触发时刻的水平转角和俯仰角数据发送给计算处理单元，计算处理单元根据经纬仪相对于全局坐标系的位姿以及水平转角和俯仰角数据计算出激光线条或者结构光平面的数学方程；同时接收器将触发时刻计算获得的激光光斑相对于接收器坐标系的局部坐标值发送给计算处理单元；计算处理单元以接收器坐标系相对于全局坐标系的六维位姿为变量，根据坐标变换获得激光光斑在全局坐标系中的函数表达式；计算处理单元根据激光光斑位于激光线条或者结构光平面之上的约束条件，将激光光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方程，建立以接收器坐标系六维位姿为未知变量的方程组，求解该方程组获得接收器坐标系相对于全局坐标系的三维位置和三维姿态。

5.按权利要求1所述的方法，其特征在于：所述自动跟踪控制的实现步骤具体为：由计算处理单元根据接收器的当前位姿和之前位姿计算接收器的运动速度，根据接收器的运动速度推测下一采样时刻接收器将到达的位姿，计算出经纬仪需要到达的新转角位置，并将新转角指令发送给相应的经纬仪，经纬仪根据新转角指令控制其水平转角和俯仰转角至需要到达的新转角位置。

Images