基于SLAM的连续运动测量坐标转换算法研究

訾安琪，柴晓冬，郑树彬，李立明，张　磊

(上海工程技术大学 城市轨道交通学院，上海　201620)



基于SLAM的连续运动测量坐标转换算法研究

訾安琪，柴晓冬，郑树彬，李立明，张磊

(上海工程技术大学 城市轨道交通学院，上海201620)

近年来，视觉导航开始成为人们的研究热点;在轨道检测中，现有运动状态下的测量方法所采用的坐标系都是建立在测量系统自身上的一种连续动态变化的坐标系，不能与静态基准点建立明确的解析联系;针对这一问题，结合同步定位与地图创建的思想，提出了一种新的检测算法;基于SLAM原理，通过双目视觉系统的坐标转换方法，建立动态坐标系与静态基准点之间的关系，求得旋转平移矩阵，将后面N个位置的坐标均转换到初始坐标系下，实现连续运动测量;通过实验，设计运动路线，验证坐标转换算法，并得到了建立在世界坐标系下的空间线形曲线;实验结果表明：转换后的坐标，x、y、z三个方向的最大误差不超过0.734 mm。

同步定位与地图创建；坐标转换；连续测量；世界坐标系

0　引言

轨道是机车车辆运行的基础，直接影响列车行驶安全以及舒适性。由于行车安全的要求，高速铁路、城市轨道线路必须具有高精度的几何线形参数，做到高平顺性。目前，高速无砟轨道结构中人们仍借助全站仪、GPS精测网进行非连续测量，这种在静止状态下才能进行测量的方法效率较低，无法较好地保证长距离的轨道线路的高平顺性要求。而现有的运动状态下的测量方法所采用的坐标系都是建立在测量系统自身上的一种连续动态变化的坐标系，这种动态坐标系还不能与静态基准点建立明确的解析联系，也就无法解决现实生活中的一些问题[1-2]。

近年来，视觉的方法越来越多的应用到轨道检测、机器人等领域。视觉导航更是开始成为人们的研究热点，它具有获取信息完整、探索范围比较大等优点，在机器人导航领域有着重要的研究意义。视觉导航系统中，目前应用最多的是在移动机器人上面安装一个或若干个视觉传感器，通过控制移动机器人，拍摄周围环境的局部图形，然后通过特征提取、图像匹配、环境建模等[3]，创建地图实现定位，即利用了SLAM的基本原理。本文将SLAM原理与连续运动测量结合起来，在SLAM原理下，通过双目视觉系统的坐标变换方法，将动态测量值转换到世界坐标系下，实现非接触连续运动的测量。

1　SLAM原理

在SLAM研究中，有3个关键性的问题，即：1)Where am I？2)Where am I going？3)How should I get there？要求机器人在一个完全未知的环境中从一个未知的位置出发，利用传感器对环境的观测递增地建立环境的导航地图，同时根据已建立的地图同步确定自己的位置[4]。其基本原理图如图1所示。

图1　SLAM原理图

本文中将两个CCD相机安装在控制平台的固定位置，通过设计好的路径连续地运动获取物体几何信息，进行坐标转换，获得空间线形曲线，从而实现全局定位与地图创建。

2　立体视觉基本原理

1)立体视觉介绍：

立体视觉是指从多幅拍摄的图像中获取目标物体三维几何信息的一种方法，它的目的在于重构场景的三维几何信息。立体视觉的研究主要包括图像获取、摄像机模型、特征提取、图像匹配和深度计算等部分。

双目立体视觉的方法，即从所拍摄的两幅图中获取物体三维几何结构信息，从而恢复出空间线形。双目视觉在生活中并不少见，其中最完美的双目视觉结构就是人的双眼。当用双眼观察物体时，会有远近的感觉，而使用两个相机实现立体视觉系统正是参照此原理，通过建立所拍摄的两幅图像之间的位置关系，根据对应点的视差恢复三维信息，计算深度[5]。

2)空间点重建：

对于一个三维多面体，如果知道了它的各个顶点的三维坐标与相邻关系，那么这个多面体的形状和位置也就是唯一确定的。利用立体视觉的方法获取物体的三维点坐标，也是立体视觉中最基本的。假设，在图2中空间中任意点X在两个相机上的投影点分别为XL和XR，两相机的投影矩阵分别为M1与M2，于是有

其中，(u1,v1,1)与(u2,v2,1)分别为点X在两个相机中对应点XL和XR的齐次坐标；(X,Y,Z,1)为点X在世界坐标系下的齐次坐标。联立上式，消去可得：

由解析几何的知识可知，直线OLXL与直线ORXR一定相交，即上述方程组一定有解，而且解是唯一的[6-7]。由此就可以确定空间中点的三维信息，即实现自定位。

3　坐标转换算法原理

连续运动测量基本原理框图如图2所示。

图2　测量原理框图

以左摄像机为例，初始化位置记为P1，将标定板固定，并在标定板上建立绝对世界坐标系，则标定板和摄像机之间的旋转矩阵和平移向量[R0;t0]可以求得。

1)前后位置关系：

记前后位置公共特征点在位置P1的坐标为(x1i,y1i,z1i)，在位置P2下的坐标为(x2i,y2i,z2i)，则P1和P2之间的位置关系可以表示为：

记

A=

构造伪逆矩阵(ATA)-1ATA·AX=b，则最小二乘解为X=(ATA)-1ATb。

即可求得，位置P1和位置P2的之间的[R1;t1]。

以此类推，可以求得连续运动状态下所有位置之间的R、t。

2)递推关系：

通过连续测量，可以获得每个位置上数据点的坐标，而又求得了前后位置之间的关系。这样就可以将所有位置上的坐标转换到初始位置P1的世界坐标系下，从而反映出整个变化的情况。最后一个位置上面的点在世界坐标系下的坐标可以通过前面位置的旋转和平移递推公式求得。即可以通过位置1在相机坐标系下的坐标和相机与世界坐标系之间的旋转、平移矩阵获得位置1上的点在世界坐标系下的坐标。即：

(1)

其中:(x1,y1,z1,1)为P1位置的相机所拍摄的坐标，(X0,Y0,Z0,1)为其在绝对世界坐标上点的坐标。

同理，P1和P2之间的关系为:

(2)

由此递推可得最后一个位置上面的点在世界坐标系下的坐标：

(3)

4.4算法验证与分析

实验采用如图3所示的控制平台进行验证。

图3　实验平台示意图

如上图所示，整个实验平台由三部分组成：NI工控机、三维移动平台、检测平台。实验平台中X向位于最底端，可以左右移动；Y向支座设置在X向底座上，可以前后移动；而Z向支架位于Y向支架的上表面，可以上下移动。3个方向移动的最小单位均为0.05mm，重复精度为0.02～0.04mm。在检测平台上固定着两个相同型号的CCD相机；相机正前方放置与之垂直的定标板[8]。

具体实验步骤如下：

1)为了得到高精度的旋转矩阵，在左右相机都能获取完整标定板信息的前提下，尽量多的改变标定板的姿态，同步采集图像20次进行定标，获得相机的M矩阵；

2)在初始位置建立世界坐标系，确定相机坐标系与世界坐标系之间的位置关系；

3)按照设计的路线移动控制平台，拍摄图像，提取当前

图4　实验平台　　　 图5　CCD相机标定界面

位置与上一位置的公共特征点，确定前后位置的旋转矩阵和平移矩阵，对特征点进行匹配；

4)继续移动，重复步骤3)，获取所有点的三维坐标；

5)将后面所有位置特征点的坐标转换到初始位置的摄像机坐标系下，然后再转到世界坐标系下，从而获得空间线形曲线。

图6　前后位置特征点提取对比图

表1所示为移动到位置1、位置2和位置3时所取点转换到初始位置的坐标，由转换后点的坐标可知，三轴的最大误差不超过0.734 mm。

坐标本实验中得到的空间线形曲线如图7所示，x轴向绝对差为7.104 mm，y轴向绝对差为105.549 mm，z轴向绝对差

图7　获得的空间线形曲线

点号12345678坐标99.50233-41.62987822.21321éëêêùûúú89.67947-41.49898819.79879éëêêùûúú79.76407-41.29574819.58482éëêêùûúú69.83310-41.26779817.15907éëêêùûúú69.92934-41.31517817.49254éëêêùûúú60.08264-41.29006816.10782éëêêùûúú59.79792-41.36494816.37154éëêêùûúú49.86938-41.29062816.20929éëêêùûúú39.92954-41.22923815.25984éëêêùûúú30.08574-41.04571812.30183éëêêùûúú点号789101112坐标40.40898-41.05182814.52661éëêêùûúú30.05706-41.12708812.14222éëêêùûúú20.12307-41.16644812.3183410.35778-40.83537807.82512éëêêùûúú10.01477-40.69145807.26611éëêêùûúú0.59748-40.97581809.36656éëêêùûúú0.61204-40.90625808.92763éëêêùûúú-9.90029-41.09266809.71264éëêêùûúú

为5.217 mm。而实际设计的路线：x轴的绝对位移为6 mm；误差为1.104 mm；y轴的绝对位移为105 mm，误差为0.549 mm；z轴的绝对位移为5 mm，误差为0.217 mm。

5　结语

本文基于SLAM原理，对连续的运动轨迹进行测量，并通过双目视觉系统的坐标变换方法，将动态测量值转换到世界坐标系下，从而将动态坐标系与静态基准点建立明确的解析联系。实验证明，本文的方法可以进行连续长距离的非接触快速检测，具有转换精度高、重复性好等特点，可实现连续运动的测量，并能够有效地检测出空间线形曲线。

[1]赵国堂. 高速铁路轨道几何状态测量与轨道精调[R]. 北京:京沪高速铁路股份有限公司, 2010.

[2]Shen S L, Wub H N, Cui Y J, et al. Long-term settlement behaviour of metro tunnels in the soft deposits of Shanghai[J]. Tunnelling and Underground Space Technology, 2014, 40:309-323.

[3] 韩锐. 未知环境下基于SLAM的移动机器人导航算法研究[D]. 武汉：武汉理工大学,2006.

[4] 郑宏. 移动机器人导航和SLAM系统研究[D]. 上海：上海交通大学,2007.

[5] 吴福朝. 计算机视觉中的数学方法[M].北京：科学出版社,2008.

[6] Zhang Z Y, Membe S. A flexible new technique for camera calibration[J]. IEEE Transaction on Pattern Analysis and Machine Intelligence, 2000,22(11):1330-1334.

[7] 马颂德, 张正友. 计算机视觉[M]. 北京:科学出版社, 1997.

[8] 罗伟. 多传感信息融合的轨道长波不平顺检测实验平台研究[D]. 上海工程技术大学, 2015.

Continuous Movement Measuring Coordinate Transformation Algorithm Based on SLAM

Zi Anqi，Chai Xiaodong，Zheng Shubin，Li Liming，Zhang Lei

(College of Urban Rail Transportation, Shanghai University of Engineering Science,Shanghai201620, China)

In recent years, the visual navigation began to become the focus. In rail detection, the existing sports under the state of measuring methods adopted by the coordinate system are based on the measurement system itself a continuous dynamic changes of the coordinate system, cannot establish clear resolution contact with static reference point. In order to solve this problem, a new detection algorithm is proposed combining with simultaneous localization and mapping. In this paper, a relationship between a dynamic coordinate system and the static reference point was built to realize continuous motion measurement, which was based on the principle of simultaneous localization and mapping (SLAM) and the coordinate transformation method of binocular vision system. We designed a movement course in the experiment to verify the coordinate transformation algorithm, and got a space linear curve based on the world coordinate system. Experimental result shows that after the transformed coordinates, x, y, z three directions of the maximum error is less than 0.734 mm.

simultaneous localization and mapping；coordinate transformation；continuous motion measurement；world coordinate system

2015-08-05；

2015-09-16。

国家自然科学基金资助(51478258)；上海市科委重点支撑项目资助(13510501300)；上海市研究生教育创新计划学位点引导布局与建设培育项目(13sc002)。

訾安琪(1990-)，女，山东邹城人，硕士研究生，主要从事智能检测与机器视觉方向的研究。

柴晓冬(1962-)，男，安徽人，教授，硕士研究生导师，主要从事智能信息处理、光信息处理方向的研究。

1671-4598(2016)01-0289-03

10.16526/j.cnki.11-4762/tp.2016.01.080

TB114.3

A