亚星游戏官网-www.yaxin868.com

专利名称：一种测量运动物体六维位姿的设备的制作方法
技术领域：
本实用新型涉及测量运动物体在较大空间中六维位姿的设备，具体地 说是一种基于激光束直线特性和并联机构位姿测量原理，采用图像处理技 术实现运动平台六维位姿测量的设备和方法。该可以广泛应用于需要实现 平移运动的场合，可以替代昂贵的激光跟踪仪和坐标测量机。
背景技术：
对运动物体的三维位置(x， y， z)和三维姿态(ct， (3， y)进行高精度 的测量在工业界有着广泛的需求。
在很多应用中，采用机械式测量装置就可以解决问题，例如利用设备 自身的关节编码器进行运动学正解，如果设备本身不具有编码器或者编码 器和机械装置的精度较低，则可以采用传统的三坐标测量机和便携关节式 坐标测量机进行测量。但是坐标测量机的测量范围一般十分有限，而且难 以在线测量运动物体。
为了测量在大范围运动的物体的六维位姿，通常采用全局定位系统。 根据测量精度的高低，全局定位系统可以分为激光跟踪仪、室内GPS、基 于激光导航方法的设备、基于激光信标方法的设备和基于视觉处理的设备。
激光跟踪仪采用经纬仪测量接收器的方位，而采用激光干涉原理测量 接收器与经纬仪之间的距离，具有采样频率高和测量精度高的显著优点， 但是设备十分昂贵。
室内GPS技术有多种，最典型的一种方案美国Arc Second公司的方案， 在地面安装至少两个激光发射器，每个发射器产生两个成固定夹角的激光 平面。发射器不停高速水平旋转，旋转频率大约在3000转/分左右，接收器 上的传感器可获得激光平面经过的准确时间，接收器根据发射器投射来的 光线时间特征参数，计算出接收器所在点的位置和角度。室内GPS的精度 较高，而且可以同时测量大量接收器的位置，所有接收器在100mxl00m测 量空间内的位置测量精度可以达到O.lmm量级。不过由于该技术采用周期 扫描方式，不同激光平面扫过同一传感器的时间有时间差，因此更适合静 态物体或者缓慢移动物体的测量，如果要用于高速运动物体的测量，必须 增加激光平面的数量，或采用超高速旋转的激光发射器。
基于激光导航方法的设备和基于激光信标方法的设备主要用于工厂自 动移动小车的定位，目前主要用于平面位置和方位定位。激光导航方法的典 型例子是在移动小车上安装可以水平旋转的激光站，激光站发射出激光光
3束，而将三个以上的反射镜安装在地面上，且反射镜安装位置己知。地面上 的反射镜通常由若干个锥体棱镜组成，投射到反射镜上的激光可以沿入射方 向返回，但与入射激光错开小段距离。移动小车上激光站不停旋转扫描，如 果在三个角度有激光返回，则可计算出移动小车的位置和方位。激光信标方 法与激光导航方法相反，在小车上安装三个以上的传感器，而在地面安装多 个可以水平旋转的激光站，如果三个以上的传感器可以接受到激光则可计算 出移动小车的位置和方位。为了实现连续测量，激光站的水平转角通常自动 跟随小车的运动。激光导航方法和激光信标方法价格相对便宜，其原理可以 扩展应用到空间六维位姿测量，但是目前尚未见报道。在这类方法中，传感 器只是开关型传感器，即只检测信号的有无，并不测量激光光束在传感器上 的落点的准确位置，因此测量精度较低。另一方面，在建筑施工行业常用的 经纬仪交汇法采用可发射激光的经纬仪和能够判断激光是否打中靶心的光 电接收靶，可以用于精确测量固定物体的位置和姿态，但这种测量系统缺乏 跟踪运动物体的功能，因此无法测量运动物体的位姿。
基于视觉处理的全局定位技术有多种，最典型的是瑞典MEEQ公司的 PosEye技术，通过在测量环境中布置多个位置已知的发光标志物，而在运 动物体上安置摄像头，根据发光标志物在摄像头拍摄的图像中的坐标来计 算运动物体的位姿。当测量距离大于10米时，发光标志物通常采用功率大 于100mW的激光发光二极管，激光发光二极管的激光发散角一般大于30°， 以保证摄像头能接收到足够的光线。但是该技术要求成像单元的分辨率很 高，目前还很难获得较高的测量精度。另一种常见技术是双目视觉技术， 即在移动物体上安装高亮标记物，通过双目视觉原理来计算标记物的位置， 进一步计算运动物体的位姿，该技术同样受限于成像单元的分辨率，测量 的精度较低。
目前成本较低、不需要高分辨率摄像系统的高精度全局定位设备还没 见报道。

实用新型内容
为了克服现有技术中成本高、需要高分辨率摄像系统的全局定位设备 的不足，本实用新型的目的在于提出一种成本较低、不需要高分辨率摄像 系统的测量运动物体六维位姿，从而实现全局定位的设备，采用本实用新 型可以替代昂贵的激光跟踪仪、坐标测量机和室内GPS等设备。
本实用新型技术方案是由计算处理单元、至少一个接收器和至少一 个经纬仪组成，计算处理单元与经纬仪安装在固定地面上；接收器安装在 待测运动物体上；经纬仪和接收器通过通信线缆或者无线通信方式与计算 处理单元通讯，经纬仪与接收器通过激光光路相连；
其中所述经纬仪具有至少一个水平转动自由度和至少一个俯仰转动 自由度；经纬仪包含驱动装置，用以控制其水平转角和俯仰转角；还包括转角测量装置，用以测量转角;每个经纬仪上安装有至少一个激光发射器； 所述经纬仪个数为1时，经纬仪上安装的激光发射器个数至少为2;所述每 个激光发射器发射出激光结构光，该激光结构光是至少一条光线的激光点 束，或是至少一个条形光平面的激光线束，或是至少一个扇形光平面的激 光线束，或是至少一个光圆柱面的激光圆形束，或是上述多种不词激光结 构光的组合光束；
另外，所述经纬仪的俯仰自由度可以被固定或者被拆卸，得到具有一 个水平转动自由度的简化结构，上面安装有至少一个激光发射器，每个激 光发射器发射出 一个扇形光平面的激光线束；
所述接收器包括至少一块投射面板和至少一个成像单元，而且每个投 射面板至少对应一个成像单元；投射面板的尺寸以及成像单元与投射面板 的距离均是所述设备最大测量范围的百分之一量级 千分之一量级范围内； 假设设备测量范围为10m，则投射面板尺寸不大于100mmxl00mm，成像 单元与投射面板的距离在100mm之内；其中投射面板是光滑的反光镜面， 或是表面粗糙的散射反光板，或是散射透光板；投射面板的形状选自平面， 或是曲面，或是多个平面的组合，或是多个曲面的组合；
所述接收器由投射面板和成像单元组成，所述成像单元处于投射面板的 下方、上方或者内部；成像单元包括成像电子装置和位于成像电子装置与投 射面板之间的成像镜头；其中成像电子装置选自位置敏感探测器、电荷耦合 器件、电荷注入器件、或基于互补金属氧化物半导体的光学成像装置；
所述接收器由投射面板和成像单元组成，所述成像单元直接安装在投 射面板表面，包括至少一个感光测量装置，感光测量装置选自位置敏感探 测器、电荷耦合器件、电荷注入器件、或基于互补金属氧化物半导体的光 学成像装置。
本实用新型具有如下特点
1. 本实用新型设备成本低而测量精度高。本实用新型通过两条以上的 激光结构光实现采用并联测量，与采用单条激光结构光进行串联测量的激 光跟踪仪相比，不需要高精度的激光干涉仪，因此成本大大降低；本实用 新型采用同步测量方式或者轮询测量方式，与采用周期扫描测量方式的室 内GPS系统相比，可以测量运动速度更高的物体；本实用新型采用短距离 的视觉捕捉和处理，投射面板与成像单元之间的距离只有最大测量范围的 百分之一或是千分之一，与采用远距离视觉捕捉和处理的PosEye技术相比， 不需要高分辨率的视觉传感器，而且定位精度更高。
2. 本实用新型可以在安装现场进行快速的自行标定，因此可以方便的 增加经纬仪和接收器，也可以方便的移走冗余的经纬仪。
3. 本实用新型可以通过增加经纬仪和接收器的数量来扩大测量范围和 提高测量结果的准确性和鲁棒性。例如将三个经纬仪按照三角形分布布置在待测运动物体周围，可以使运动物体在较大空间内的位姿测量准确性波 动幅度减小。
4. 以三个经讳仪作为一个类似蜂窝的测量网格，可以通过坐标切换机 制实现不同测量网格之间的过渡，实现大范围高精度测量。
5. 本实用新型可以在接收器上安装测杆或者三维激光扫描仪，从而可
以测量经纬仪发射的激光光束难以达到的地方。
6. 本实用新型激光器发射的激光始终落在接收器上，投入人眼的概率 非常低。即使偶尔进入人眼，由于激光可以选择红外波段，并且功率非常 低，对人眼没有任何伤害。


图1为六维位姿测量设备的第一个实例的示意图2为六维位姿测量设备进行实际测量时的工作流程示意图3为六维位姿测量设备的第二个实例的示意图4为六维位姿测量设备的第三个实例的示意图5为六维位姿测量设备的第四个实例的示意图6为六维位姿测量设备的第五个实例的示意图7为六维位姿测量设备的第六个实例的示意图8为六维位姿测量设备的第七个实例的示意图9为六维位姿测量设备的第八个实例的示意图。
具体实施方式
以下结合附图对本实用新型作进一步详述。
如图1所示为本实用新型提出的六维位姿测量设备的第一个实例，该 设备由一个计算处理单元l、 一个接收器2和三个经纬仪3、 4、 5组成。三 个经纬仪3、 4、 5固定安装在地面上，通常布置成三角形。接收器2通过 连接件7或者直接安装在六维位姿待测的运动物体8上。图中接收器坐标 系20为O'-X'Y'Z'，固定在接收器上，而全局坐标系10为O-XYZ，固定在 地面上，运动物体坐标系80为Om-XmYmZm，固定在运动物体8上。由 于接收器坐标系20相对于运动物体坐标系80的位姿是始终固定的，而且 可以通过标定方法获得，因此测量运动物体8的六维位姿可以通过测量接 收器坐标系20相对于全局坐标系10的六维位姿获得。
经纬仪3、 4、 5都具有水平转动和俯仰转动两个转动自由度，每个经 纬仪的水平转角和俯仰转角通过驱动装置进行控制，并且转角值可以快速 的测量。驱动装置通常采用伺服电机，也可采用精度更高的压电陶瓷电机， 转角测量装置安装在经纬仪上，通常采用高精度编码器，编码器分辨率和 测量重复精度均达到r量级。经纬仪3、 4、 5上分别安装有一个激光发射 器30、 40、 50。激光发射器30、 40、 50均发射至少一条激光束，分别是 31、 41、 51。在实际应用中，为使测量结果具有更好的准确性和鲁棒性，可以让每个激光发射器发射两条或者更多的相互平行的激光光束。激光器安装在经讳仪上，通常采用半导体激光器，波长通常选择红色可见光或者红
外光的波段，如635nm，激光器的输出功率一般小于5mW。
接收器2主要包括一块投射面板21和一个成像单元22。投射面板的尺寸以及成像单元与投射面板的距离均是所述设备最大测量范围的百分之一量级 千分之一量级；本实施例设备测量范围为10m，则投射面板尺寸不大于100mmxl00mm，成像单元与投射面板的距离在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相对于绝对坐标系IO的位姿可以采用外部设备预先测定，也可以采用本专利后面介绍的自行标定方法获得。再结合经纬仪水平转角和俯仰转角的测量值就可以获得三条激光点束31、 41、 51在全局坐标系10中的方程，每
条激光束对应两个方程。假设三条激光点束31、 41、 51对应的代数方程分
上述方程的系数只取决于经纬仪3、 4、 5在全局坐标系10中的位姿和经纬仪3、 4、 5的转角测量值。
另一方面，通过成像单元对投射面板上的激光光斑或者激光条纹进行拍摄和图像处理，获得激光光斑或者激光条纹在成像单元中的映像的像素坐标；通过对成像单元的标定获得成像单元上像素坐标中每一个像素点相对于接收器坐标系的位置，利用像素点与接收器坐标系的一一对应关系获得激光光斑或者激光条纹相对于接收器坐标系的局部坐标值。如图1所示，通过对成像电子装置222拍摄的图像进行处理，可以获得激光光斑在成像电子装置222上的坐标。根据小孔成像原理，激光光斑与其在成像电子装置222上的映像之间的连线必然经过成像镜头221的透镜中心。由于投射面板21、成像镜头221和成像电子装置222相对于接收器坐标系20的位置是固定的，因此可以通过小孔成像数学模型获得这些激光光斑相对于接收器坐标系20的三维位置；或者采用直接标定方法，即对于成像电子装置222上的每一个像素，直接标定并记录下其在投射面板21上对应点在接收器坐标系20中的坐标。假设投射面板21的大小为100mmxl00mm，成像电子装置222的像素阵列为1024像素xl024像素，成像单元22的视场大小与投射面板大小基本相等，则成像电子装置222的视觉分辨率小于O.lmm。
再通过假设接收器坐标系相对于全局坐标系的六维位姿获得成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系中的全局坐标值。由于接收器坐标系20相对于全局坐标系10的位姿是六个未知变量(x， y， z，a, (3， y)，如果已知一个激光光斑在接收器坐标系20中的相对坐标(X'，Y'， Z')，可以根据以下齐次坐标变换获得该激光光斑在全局坐标系IO中的三维全局坐标(X， Y， Z):
a41x + 641少+ c41z + <i41 = 0二 rra似O, z)i o/(Z, "i o《X， j3)7 o/(Z， a)
1
z'1
其中7 o"Z,"i W(X,戶)7 o/(Z,cO表示先绕全局坐标系的Z轴旋转a角度，接着绕X轴旋转/ 角度，再绕Z轴旋转y角度，7Va似(x,y,z)表示沿着矢量[x,3^]平移。因此可以获得
r12n3Z'
&少7'
zr33Z'
100011
其中，
^, = cos y cos a - cos j0 sin a sin yr12 = cos y si緒+ cos j8 cos a sin yr13 = sin y sin j8
r21 = - sin y cos a — cos /3 sin a cos yr22 = _ sin y sin a + cos ^ cos a cos yr23 = cos y sinr31 = sin sincer32 = - sin cos ar33 = cos j3
可见给定一个激光光斑在接收器坐标系20中的相对坐标(X'， Y'， Z')，该激光光斑在全局坐标系IO中的全局坐标(X， Y， Z)分别对应三个以接收器坐标系20的六维位姿(x， y， z， ot， p， Y)为变量的函数表达式。
最后通过同步触发测量步骤建立激光结构光生成的直线或者平面在全局坐标系中的数学方程和成像单元标定得到的激光光斑或者激光条纹相对于全局坐标系的全局坐标值之间的约束关系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿。由于三个光斑32、 42、 52必须分别位于三条激光束对应的直线上，将三个光斑的三维全局坐标对应的函数表达式分别代入相应激光束的两个方程，可以获得具有六个变量的六个方程，
9["3,32 +631^32 +C31Z32 +"31 = W(X乂Z，",/^，？0 = 0
le31x32+/31>y32+g31z32+/z31 ^(x,少，z，a，〃，"二0 j "41x42 + 641_y42 + c41z42 + J41 = r(x, 乂 z, a, j8，" = 0 le41x42 + /41y42 + g41z42 + & = 少," = 0
"51X52 + + C51Z52 +《1 =乂 Z， a, /3, " 二 0
e51x52 + /51y52 + g51z52 + /251 = Q(x， 乂 ； a ， /3，" = 0
在以上六个方程中，假设激光光斑32的全局坐标为(x32， y32， z32)， 激光光斑42的全局坐标为(x42， y42， z42)，激光光斑52的全局坐标为(x52， y52， z52)，显然它们都是以接收器坐标系20的六维位姿(x， y， z， a，卩， y)为变量的函数表达式。
求解以上六个方程所组成的方程组可以获得接收器坐标系20相对于全 局坐标系10的六维位姿(x， y， z， a， (3， Y)。求解方程组的算法可以是 经典的牛顿-拉夫逊方法、各种最优方法、同伦法和区间分析法等等。
事实上，上述激光光斑必须分别位于激光束对应的直线上的约束关系 可以采用其他表达方法，例如三个光斑32、 42、 52与三条激光束的距离等 于零，并获得不同的数学方程。接收器坐标系20相对于全局坐标系10的 六维位姿也可以采用其他等效表示方法进行表示，例如三维位置采用(x， y， z)表示，而三维姿态(ct， (3， y)采用四元数表示，或者三维位置和三 维姿态统一采用双四元数表示。
图2为六维位姿测量设备进行实际测量时的工作流程示意图，包括经 纬仪位姿自行标定的步骤和同步触发测量的步骤。该工作流程针对第一个 实例进行解释，但是可以推广用于后述的其他实例。假设接收器2可放置 在一个初始位置，在该初始位置时，接收器坐标系20与全局坐标系10重 合。首先需要标定三个经纬仪3、 4、 5的六维位姿，共有十八个未知变量， 因此需构建至少十八个方程才能得到一个确定性方程组，从而求解出这十 八个变量。具体工作流程如下
首先执行经纬仪位姿自行标定方法的步骤。在步骤IOI，将三个经纬仪 3、 4、 5固定在地面。步骤102将接收器2移动到初始位置，使接收器坐标 系20与全局坐标系IO重合。在步骤103，调节经纬仪3、 4、 5的水平转角 和俯仰转角使激光光束落在接收器2的投射面板21上。在步骤104，计算 处理单元1发送触发信号分别给经纬仪3、 4、 5和接收器2。在步骤105， 经讳仪3、4、5将触发时刻的水平转角和俯仰角数据发送给计算处理单元1， 与此同时接收器2的成像单元22触发高速快门，捕获投射面板21上的光 斑图像，并将计算获得的激光光斑相对于接收器坐标系10的局部坐标值发
10送给计算处理单元1。在步骤106计算处理单元1根据激光光斑位于激光直 线上的约束关系，将激光光斑在全局坐标系中的函数表达式代入激光线条 或者结构光平面的数学方程，建立以三个经纬仪的18个位姿参数为未知变
量的第l-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"以下，转角静 态测量精度可以达到r以下。假设所采用的经纬仪的转角精度为2"，经纬 仪与测量物体的距离为10m，经纬仪发射的激光束在10m距离处的置信区间半径小于O.lmm。而采用成像单元的图像分辨率可以轻易的达到0.1mm 以下，例如采用1024像素xl024像素的成像电子装置拍摄面积为 50mmx50mm的投射面板，可以获得0.05mm的分辨率。采用本实用新型方 案，触发信号的传输延时和成像单元的快门曝光时间都可以达到lO)is量级， 当运动物体速度为lm/s时，成像单元的测量偏差为O.Olmm量级。由于本 实用新型采用多组方程进行求解，具有类似并联机构运动学求逆解的特点， 多个激光束的指向误差不累积，因此本实用新型设备的位置测量精度可以 达到O.lmm量级。已知接收器的投射面板的尺寸为50mmx50mm，假设激 光光斑不规则的落在投射面板上，其中所有激光光斑与全部激光光斑的几 何重心之间的最小距离为5mm，则在10m距离处，姿态测量精度可以达到 (0.1/5) x (1S07兀)=1.14°，即1°量级。在后述的第八个实例中，在运动 物体上安装三个接收器并使接收器之间的距离达到50mm，本实用新型设备 的姿态测量精度可以达到0. 1°量级。本实例的投射面板21只能接收来自四 周的激光束，而不能接收来自上方和下方的激光束，因此可以测量的运动 物体姿态变化范围较小， 一般在100°左右。在后述实例中将介绍一些姿态 测量范围更大的接收器。
本设备的自动跟踪控制步骤的跟踪性能可以达到如下指标假设自动 跟踪控制算法的控制周期为100Hz，运动物体与经纬仪的距离为10m，当 运动物体运动速度为lm/s时，在每个控制周期，运动物体移动距离的预测 准确度为10士2mm。假设经纬仪的水平转角和俯仰转角采用位置闭环控制， 采用伺服电机和机械传动方式时，转角的控制准确度为±0.05°，因此激光光 斑在投射面板上落点的准确度为9mm。假设接收器的投射面板的尺寸为 50mmx50mm，每个控制周期都可以控制激光光斑落在以投射面板中心为圆 心而半径为19mm的圆以内，即激光光斑始终落在投射面板上，因此本设 备可以跟踪距离在10米外、运动速度在lm/s的运动物体。当测量距离为 100m的运动物体时，可以采用压电陶瓷电机或者直接驱动电机来提高经纬 仪转角控制精度，转角控制准确度可以达到±0.005°以下，同时还可以通过 縮短跟踪控制的控制周期和增大投射面板尺寸等措施来保证准确的跟踪。
图3所示为本实用新型提出的六维位姿测量设备的第二个实例，该设 备由一个计算处理单元l、 一个接收器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对应的代数方程分 别为
a33x + 633_y + c33z + <i33 =0 a43x + 643_y + c43z + <i43 = 0 a53x + 653少+ c53z + d53 =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相 对于全局坐标系IO的六维位姿。采用激光线束的好处是可以在每条激光条 纹上选取两个以上的点，从而获得更多的约束方程，提高方程组求解的准 确性和可靠性。
图4所示为本实用新型提出的六维位姿测量设备的第三个实例，该设 备由一个计算处理单元l、 一个接收器2和两个经纬仪3、 4组成。其中经 纬仪3上的激光发射器发射30发射出生成光平面的激光线束33，而经纬仪 4上的激光发射器发射40发射至少两条激光束41、 42。根据前面两个实例
的分析方法不难得到具有六个变量的六个方程，因此可以获得接收器坐标 系20相对于全局坐标系10的六维位姿。
图5所示为本实用新型提出的六维位姿测量设备的第四个实例，该实 例具有部件数量较少的特点。该设备由一个计算处理单元l、 一个接收器2 和一个经纬仪6组成。经纬仪6与前述经纬仪3、 4、 5的不同之处是增加 了一个俯仰转动自由度，在两个俯仰转动自由度上分别安装有激光发射器 60、 61。激光发射器60发射出两条激光束62、 63，而激光发射器61发射 出两条激光束64、 65。激光束62、 63、 64、 65落在接收器2的投射板21 上形成四个光斑。根据第一个实例的分析方法不难得到具有六个变量的八个方程，因此可以获得接收器坐标系20相对于全局坐标系IO的六维位姿。 在此指出，虽然三条激光束即可得到具有六个变量的六个方程，但是采用 四条激光束有利于提高系统测量结果的准确性和鲁棒性。接收器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。根据第二个实例的分析方法不
14难得到具有六个变量的八个方程，因此可以获得接收器坐标系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的六维位姿。
需要指出的是，虽然这里只给出了六维位姿测量设备的八个实例，但 是通过对上述实例的部件进行组合可以得到更多的实例。此外，在上述八 个实例中，经纬仪安装在固定地面上，接收器安装在待测运动物体上，但 是也可以将经纬仪和接收器中的一种安装在待测运动物体上，另一种安装 在固定地面上。
1权利要求1. 一种测量运动物体六维位姿的设备，其特征在于由计算处理单元(1)、至少一个接收器和至少一个经纬仪组成；计算处理单元与经纬仪安装在固定地面上；接收器安装在待测运动物体(8)上；经纬仪和接收器与计算处理单元(1)通讯，经纬仪与接收器通过激光光路相连。
2. 按权利要求1所述的设备，其特征在于所述经纬仪具有至少一个 水平转动自由度和至少一个俯仰转动自由度；经纬仪包含驱动装置，用以 控制其水平转角和俯仰转角；还包含转角测量装置，用以测量其水平转角和 俯仰转角；经纬仪上安装有至少一个激光发射器。
3. 按权利要求l所述的设备，其特征在于所述经纬仪具有一个水平 转动自由度，还配有控制所述水平转动自由度的驱动装置及用以测量水平 转角的转角测量装置；经纬仪上安装有至少一个激光发射器。
4. 按权利要求2或3所述的设备，其特征在于所述经纬仪个数为l 时，经纬仪上安装的激光发射器个数至少为2。
5. 按权利要求2所述的设备，其特征在于所述每个激光发射器发射 出激光结构光，该激光结构光是至少一条光线的激光点束，或是至少一个 条形光平面的激光线束，或是至少一个扇形光平面的激光线束，或是至少 一个光圆柱面的激光圆形束，或是上述多种不同激光结构光的组合光束。
6. 按权利要求3所述的设备，其特征在于所述每个激光发射器发射 出 一个扇形光平面的激光线束。
7. 按权利要求l所述的设备，其特征在于所述接收器包括至少一块 投射面板和至少一个成像单元，而且每个投射面板至少对应一个成像单元; 其中投射面板是光滑的反光镜面，或是表面粗糙的散射反光板，或是散射 透光板；投射面板的形状选自平面，或是曲面，或是多个平面的组合，或 是多个曲面的组合。
8. 按权利要求7所述的设备，其特征在于所述接收器由投射面板和 成像单元组成，所述成像单元处于投射面板的下方、上方或者内部，包括 成像电子装置和位于成像电子装置与投射面板之间的成像镜头；其中成像 电子装置选自位置敏感探测器、电荷耦合器件、电荷注入器件、或基于互 补金属氧化物半导体的光学成像装置。
9. 按权利要求7所述的设备，其特征在于所述接收器由投射面板和 成像单元组成，所述成像单元直接安装在投射面板表面，包括至少一个感 光测量装置，感光测量装置选自位置敏感探测器、电荷耦合器件、电荷注 入器件、或基于互补金属氧化物半导体的光学成像装置。
专利摘要本实用新型涉及一种测量运动物体六维位姿的设备。该设备由一个或多个经纬仪、一个或多个接收器和一个计算处理单元组成，每个经纬仪的水平转角和俯仰转角可控，而且经纬仪上安装有激光发射器，可以发射一条或多条激光结构光，投射到接收器上面；接收器包括投射面板和成像单元。计算处理单元利用经纬仪的转角和接收器获取的激光光斑图像数据建立方程组，求解方程组可以获得运动物体的六维位姿。本实用新型可以连续测量在较大空间中运动的物体，具有精度高、成本低和便于布置等优点，可以替代激光跟踪仪、坐标测量机和室内GPS等昂贵的位姿测量设备。
文档编号G01C1/02GK201285280SQ200820219188
公开日2009年8月5日 申请日期2008年11月14日 优先权日2008年11月14日
发明者王琛元, 田永利, 罗振军, 邹媛媛 申请人:中国科学院沈阳自动化研究所