基于SLAM的停车场定位与导航系统设计

王苏昊

（浙江省温州中学，浙江温州，325014）

0 前言

21 世纪以来，人民消费水平逐步提高，与此同时，对便捷交通的需求促使人们越来越多地购买私家车。为了满足人们停车的需要，停车场的规模也日趋扩大。但是由于停车场面积大且拥有大量重复性场景，人们容易在停车场迷路。目前已经有的导航技术如全球定位系统（Global Positioning System，GPS）等一方面由于技术本身的缺陷，在室内环境中信号的强度不够，所以导航难度大，另一方面，民用的GPS 难以达到用于要求的精度。而目前SLAM 作为一种可以在未知环境中进行定位和导航的技术已经大规模应用于虚拟现实，增强现实，自动驾驶等领域[1]。此外SLAM 可以作为在室内进行定位、导航的关键性技术。SLAM 技术是利用传感器感知周围环境进行工作的，它目前已大量被应用于自动驾驶、机器人领域，在定位与建图过程中发挥着举足轻重的作用[2]。SLAM 技术可以主要分为视觉SLAM 和激光SLAM 等，激光SLAM 是早期发展起来的SLAM 技术，其使用激光传感器对周围环境进行感知，因此可以获得周围环境比较准确的位置，但是激光传感器的缺点在于成本较高，体积很大，在使用的过程中易受天气变化影响[3]，同时存在有玻璃的环境中无法工作的问题。视觉SLAM 由于传感器价格低廉、易获取，且视觉传感器获取到的周围环境信息更丰富，因而受到了研究者们的青睐。目前视觉SLAM 所使用的传感器主要分为单目[4]，双目，深度相机等。单目传感器由于无法获取到环境中物体的准确位置，所以算法上相对来说比较复杂。双目传感器相对来说可以获取到比较准确的深度信息，但是计算起来相对较慢。深度相机可以通过物理的方法获取到深度信息[5]。

本文将使用视觉SLAM 完成构建三维地图、定位以及重定位的功能。视觉传感器分为多种类型，单目相机虽然运算速度快，但是会损失深度信息；深度相机（RGB-D）在弥补了单目相机不足之处时[6]，也出现了距离过远红外线难以返回的问题，在实际操作中问题较多；而双目相机则有运算复杂度大的缺陷。综上考虑，本文将使用双目相机。同时，单一传感器无法满足SLAM 在精度和速度上的要求，在使用过程中可能会出现图像模糊等问题以致于出现偏差。因此多传感器融合SLAM 系统成为目前主流发展方向[7]。由于单纯的视觉技术在使用的过程中会存在稳定性不足，计算消耗大的情况，本文中将使用视觉加imu 技术进行定位。三维重构是将一些客观的二维图片在计算机中进行虚拟重建过程。作为一门新兴的技术，三维重构在文物保护、医疗、娱乐等领域皆有广泛的应用，本文中将使用三维重构技术构建停车场三维地图[8]。

1 相关技术

1.1 相机模型

假设在空间中有一点P，它的世界坐标为Pw。运用外参数（即平移和旋转）将它转化为P 点的相机坐标Pc。点P 通过光心投影到相机的物理成像平面上，映射出点P'，点P’坐标可以通过Pc 坐标利用相似三角形知识计算。在这幅图里就可以发现传统相机模型的缺陷：点P'无法反推出空间中P 点的z 坐标，P 点可以在PP'连线上的任意一点。这时候，为了将无数P'点映射到一个平面内，就不得不损失“深度”信息，而采用归一化平面（这时不再考虑物理成像平面，同时将P'点默认移至归一化平面），人为规定z=1，从而得到P'点坐标，即P 点的“归一化“相机坐标Pc。最后，由于图片在计算机中以像素的形式存储，所以要将归一化平面上的点通过内参数K 转化为像素平面上的坐标。同时为了方便观测，还要将像素平面上的坐标原点平移至平面左上角，如图1 所示。

1.2 视觉里程计

经典SLAM 模型中运用路标（特征点）来描述SLAM过程[9]。这里要求的特征点应是三维空间中固定不变的点，并且能在光照、视角发生少量变化时仍能保持一致，比较常见的算法有SIFT，SURF，ORB 等。这一类算法都首先在一张图片中找到很多对于光照，视角，尺度等变化比较稳定的特征点如角点等，通常情况下一张图片会有1000 个左右的特征点，在得到这些特征点之后还需要对这些特征点进行描述，这也就是生成描述符的步骤，这一步需要利用到特征点周围区域的信息，以关键点为中心取一个区域，利用这个区域的特征比如直方图等生成一个高维度的向量称之为描述符。

图1 相机模型

在得到描述符之后需要对两幅图片的描述符进行匹配，这一步的目的是知道那些特征点对应于真实世界的同一个点。寻找两幅照片中的相同特征点进行匹配的过程中，计算两幅图片不同特征点之间的距离如欧式距离或海明距离等，距离最小的可以认为是最相似的点。在得到大量的匹配点之后，可以通过PnP 或者ICP 算法对机器人的运动进行计算。用相机移动拍摄中，总是以当前“机器人”（可以是手机）为原点构建坐标系，将像素坐标转换为这个坐标系中的三维坐标，这时，可通过计算将当前坐标系中的点的坐标转换至世界坐标或参考坐标系的坐标。

1.3 IMU 技术

机器人在运动过程中会有加速度与速度变化，内置的IMU 可以计算得到不同时刻机器人的朝向、速度以及移动距离[10]。由于视觉SLAM在运行的过程中需要很大的计算量，在资源受限的嵌入式设备中运行比较困难，加入IMU 之后，可以通过IMU 测量得到机器人的速度，加速度等通过积分的方式可以计算到比较准确的姿态，此时将不需要一直使用视觉跟踪的技术，从而可以极大地减轻计算量。

2 基于SLAM 的导航系统

基于SLAM 的导航系统结构如图2 所示。首先应该有一个停车场的三维地图，这一步可以由停车场的工作人员进行扫描得到，拿一台手持的深度相机或者双目相机采集在运动的过程中采集到连续的图片，通过这些图片可以用SLAM技术恢复停车场的三维结构。当停车场出现比较大的修整时需要及时对地图进行相应的调整。

首先，将整个停车场置于规定好的参照坐标系，确定坐标原点，然后通过SLAM 技术双目相机传感器拍摄采集图像，重要标志物的无数P 点组成无数像素平面，并得到各个P 点坐标。运用三维空间刚体运动模型，将不同角度得到的同一特征点（P 点）的坐标结合计算分析，得到该点在参照坐标系中的坐标，以此类推，得到某几样标志物的坐标，构建主体模型。在SLAM 移动拍摄中，使用视觉里程计模型测量照片偏移距离及坐标，并以此反算出参照坐标系中偏移距离及坐标。在移动过程中，同时采用imu 技术计算机器人当前位置。由于imu 可能打滑出现误判，这时可以通过回环检测进行非线性优化，完善地图。

图2 基于SLAM 的导航系统

用户在使用的过程中首先通过GPS 定位到当前所在的商场然后将对应数据库中的地图加载到客户端。需要在停好车后将车停下后，客户端记录下车的坐标，记录坐标采用重定位的方法即可，打开客户端，客户端将自动地访问相机，通过相机采集到图片，在地图中进行重定位，并记录下当前的位置。购物返程时，可以再次打开客户端，同时，在起始位置，相机将被打开并拍摄周围环境，系统从数据库中寻找到所有相似照片。何为相似？即运用人工智能在不同照片标注收集特征点，描述，若一个吻合，则在照片中寻找下一个距离此点想通过的特征点，并用“1，0”标注此点原照片是否存在，以此类推，不断逼近，最后，比对代码得到大量相似图片后，寻找在同一坐标附近的相似照片作为真实的“相似”，然后通过距离，偏角计算得此照片真实的坐标及人在坐标系中所处的位置及面朝的方向，相机关闭。接下来在跟踪的过程中由于人的速度比较慢可以使用IMU 完成，这可以极大地减少计算量，但是由于IMU 跟踪可能存在误差，所以隔一段时间需要通过相机进行一次重定位。这时，客户端能够根据人的位置坐标与车位置坐标在坐标系中规划得到最佳路径。通过手机内置IMU 感受人的前进方向及偏转角，实时在地图显示。同时，还是由于IMU 存在误差，每隔10-30s 拍摄一次进行坐标偏角校正，当然，如偏离轨道，可重新进行轨迹规划。

3 总结

本文中设计了一套可以用于在室内停车场进行导航的系统，这套系统应用了多传感器融合的SLAM 算法，在计算量，精度等各个方面都有比较好的表现。但是由于目前实验环境的限制，本文中只针对理论部分做了设计没有进行实验的操作，这是后续可以进行工作的方向。此外，在停车场这种重复性场景比较多的环境中，单纯的SLAM 可能会有比较大的挑战，可以通过wifi 定位或者贴标签的方式完成重定位。