基于激光雷达的气浮台定位系统设计与实现*

徐振宇，张永康，董文博

（中国科学院空间应用工程与技术中心，中国科学院太空应用重点实验室，北京100094）

基于激光雷达的气浮台定位系统设计与实现*

徐振宇，张永康，董文博*

（中国科学院空间应用工程与技术中心，中国科学院太空应用重点实验室，北京100094）

设计并实现了一种基于嵌入式平台的激光雷达定位系统，用于特定场景下气浮台的定位与控制。系统主要由激光雷达传感器，DSP嵌入式控制器以及气浮台组成。DSP获取并处理激光雷达传感器原始数据、识别目标特征点、解算坐标值、实现对气浮台位置的实时控制。文章给出室内环境特征点的提取方法、雷达坐标系与系统坐标系坐标转换算法，采用最小二乘法对识别点曲线拟合，并提取有效坐标点，解算物体坐标。实验结果表明，系统的平均定位误差为5 mm，角度误差小于1°，满足气浮台姿态控制所需的定位精度。

市内定位；激光雷达；特征提取；坐标转换

精确的室内定位技术对于商业应用、公共安全以及军事应用都有非常重要的意义。典型的定位系统包括：AT&T开发的Active Badge室内定位系统［1］，系统在待测移动物体上附一个电子标识，标识向室内固定放置的红外接收机周期发送该待测物体唯一的ID和距离信息，接收机再通过有线网络将数据传输给数据库进行定位。由于采用的是红外线技术，当环境中有荧光干扰或直射阳光时，会产红外干扰从而使得Active Badge产生定位误差。Cricket定位系统通过发射、接收超声波信号来测量固定信标点与移动节点之间的距离进而实现定位的［2-3］。Cricket定位技术容易受到障碍物阻挡和反射的影响使定位精度下降。RADAR定位系统是将接收信号强度作为估计发射器与接收器间距的重要依据［4］。系统通过建立无线信号模型，确定信号强度的衰减与收发设备间距离的关系，从而获得最优的模型估算参数。该系统的最大优势在于它不需要特定的硬件开发和大量信号强度采样工作。但射频信号传输环境的不稳定性严重影响系统的定位精度。

激光测距雷达在机器人导航和智能车辆定位中的使用日益增多［5-7］。激光测距雷达能够以较高的速度和准确度提供室内环境信息，其基本测距原理如图1所示。在每次测距过程中，激光雷达发射经调制的红外激光信号，该激光信号照射到目标物体后产生的反射光，该反射光被激光雷达的视觉系统接收，经激光雷达内部DSP处理器进行实时解算。激光雷达以雷达坐标系为基准输出物体坐标，d为激光雷达中心与物体的直线距离，θ表示物体相对于雷达朝向之间的夹角。在电机驱动下激光雷达将进行顺时针旋转，从而实现360度全方位环境的扫描测距。THMR-V新一代智能车上安装有二维激光雷达、全景摄像机等传感器，其设计车速最高为80 km/h，目前已能够在校园非结构化道路环境下，进行道路跟踪和避障自主行驶。

本文给出一种应用激光测距雷达在气浮台上确定雷达载体相对气浮台的位置与方向角方法。论文第二部分介绍系统结构；论文第三部分给出一种简单有效的特征点提取方法和测距雷达坐标计算方法；第四部分给出实际系统的定位精度与误差分析以及进一步提高系统定位精度的改进方向；最后给出论文结论。

1 系统介绍

本文所提出的定位系统由气浮台、数字信号处理器及激光雷达传感器组成，系统示意图如图2所示。水平大理石台四个拐点安装有立柱，目标物体是尺寸为360 mm×270 mm表面光滑白色平板。气浮台是依靠压缩空气在气浮轴承与轴承座之间形成的气模，使模拟台体浮起，从而实现失重和无摩擦的相对运动条件，以模拟卫星在外层空间所受扰动力矩很小的力学环境。文中所用的滑台式气浮台采用三点支撑的气浮技术，依靠压缩空气在致密大理石桌面（00级）与固定在滑台下的三个气浮块之间形成气膜，使滑台浮起。激光雷达传感器采用RoboPeak团队开发的二维激光雷达RPLIDAR［8-10］，选取TMS320F2812作为处理控制核心。激光雷达传感器放置在气浮台中心位置，气浮台周围无可扫描的障碍。

激光雷达将获得的原始测距数据以特定的数据帧格式通过串口发送至DSP，DSP进行雷达载体位置解算及位置控制计算。

定位系统硬件与软件框图如图3、图4所示，硬件系统主要包括①DSP核心控制板和驱动板；②RPLIDAR激光雷达传感器；③气浮台气路及喷气装置等。DSP核心控制板主控芯片采用TMS320F2812数字信号处理器芯片，其最高系统频率可达150MHz。TMS320F2812强大的数据处理能力和丰富的外设资源使得其广泛应用于实时数据处理和控制系统中。RPLIDAR主要分为激光测距核心、高速旋转的机械部分。激光测距核心采用三角测距技术，配合高速的视觉采集处理机构，可进行每秒高达2 000次的测距动作。处理器可以通过RPLIDAR的串口实时获取RPLIDAR的扫描测距数据。

图3 硬件系统框图

软件流程主要步骤为：①系统上电检测，激光雷达传感器上电自检；②DSP发出扫描指令并等待传感器响应；③DSP处理激光雷达数据；④气浮台位置解算与控制。系统首先进行上电自检，如果RPLIDAR检测到自身工作异常后，则会关闭自身工作，进入保护性停机模式，但外部系统仍可以与RPLIDAR进行通讯，查询其工作状态等信息。当RPLIDAR接收到测距命令时，其开始扫描测距同时将传感器周围障碍物信息按固定的数据格式通过串口发送给DSP处理板。处理板根据传感器采集的距离和角度信息，提取目标物体特征、解算传感器位置、实时控制电磁气阀喷气量，从而实现对气浮台位置的实时确定与控制。

图4 软件框图

2 定位方法

定位步骤分为两步：①从激光雷达采集的原始数据中，提取出目标物体特征数据点、识别特征物体的起始点；②根据目标物体的起始点信息，解算出激光雷达在位置。

2.1 特征点识别

气浮台移动过程中，激光雷达得到一组周围环境在雷达坐标系下的点数据集。特征提取需要从点数据中提取出目标物体特征点、起始点和终止点，示例如图5所示。

图5中所有数据点为气浮台周围环境数据点，特征提取的目的是在图中找到目标物体数据点、起始点和终止点。

图5 特征点识别示例

式中：dmin=min{dk,dk+1}，ϕ是激光雷达角度分辨率，β和C0为两个系统参数。判断准则原理如图6所示，C0取值根据经验值给出，β通过式（2）、式（3）确定：

式中：f表示相邻两点之间实际距离，相邻两个点的测量距离值分别为dk和dk+1，ϕ是激光雷达角度分辨率。若dk,k+1满足1式则将放入子集Si，继续判断dk+1,k+2。若dk,k+1不满足式（1）则将放入子集Si，将放入新子集Si+1，继续判断dk+1,k+2，重复执行步骤③直到有效点数据判断完全。执行步骤③后，得到若干有效点子集Si( )i=1,2,…；④比较有效点子集中点个数，取点数据个数大于δ的子集。

图6 判别准则原理图

预处理后得到若干有效数据点子集Si，由于设计中目标物体的尺寸为360 mm×270 mm，气浮台周围无其他连续障碍物，因此判断目标物体的有效点数大于特定阈值δ，取有效数据点个数大于δ的子集表示目标物体特征点数据集。对目标特征点数据集做最小二乘法曲线拟合，得到修正的后的点数据集S′。

设计中激光雷达有两种扫描方式，由目标物内开始扫描和物体外开始扫描，如图7所示。激光雷达从目标物体外开始扫面时目标物体特征点仅有一个有效数据点集，而当激光雷达从目标物体内开始扫描时将获得两个有效数据点集。由于气浮台所处实际环境和RPLIDAR自身特性，激光雷达采集到无效点时距离和角度数据为零、始终按顺时针方向扫描，因此采集的目标物体数据点有④以下6个特征：①起点从零跳变到有效值（大于激光雷达盲区值100 mm）；②终点从有效值（大于100 mm）跳变到零；③终点角度大于起点角度（对于雷达本身）；有效线段采样点大于20（目标物有足够的扫面点）；⑤起点，终点角度之差小于180°；⑥相邻两点间距离之差小于20 mm。

图7 激光雷达扫面示意图

根据特征提取得到的有效点数据 (di,θi) (i=1,2,…,360)，判断激光雷达的扫描方式。具体方法如下：取有线点数据集中6个点数据，判断相邻两点距离值dk,k+1。若dk,k+1不为 0（采集到有效数据点），则激光雷达从目标物体内开始扫描，否则从目标物体外扫描。

（1）目标物体外扫描：目标物体有一个特征点子集Sn，从Sn中取出第一个点和最后一个点分别作为目标物体的起始点，终止点。

（2）目标物体内扫描：目标物体有两个特征点子集S1、Sn（子集Sn中最后一个点数据与S1中第一个点数据为相邻点），取S1中最后点为终止点，Sn中第一个点为开始点

2.2 坐标解算

定位系统中设定两个坐标系，目标物体所在的全局坐标系、以激光雷达为原点的局部坐标系。全局坐标系根据实际环境预先设定，假设目标物体在全局坐标系下的位置为。气浮台移动过程中，激光雷达测量得到目标物体在局部坐标系下的位置，设为由两组局部坐标系下位置数据解算气浮台在全局坐标系下的位置［12-14］。具体数学描述如下：

图8 坐标转换原理图

假设标记点P在全局坐标系和激光雷达坐标系中的坐标分别激光雷达采集P点的数据为(r,θ)，转换成直角坐标：

矩阵表示为：

3 实验及结果分析

在水平大理石平台上验证气浮台定位算法和控制精度测试实验。大理石台前侧放置360 mm× 270 mm固定尺寸表面光滑白色纸板，拐点安装有四个立柱，支撑目标物体，气浮台周围无其他障碍点。设定全局坐标系、标定目标物体在全局坐标系中的坐标。测试定位系统目标特征点提取算法精确度，特征提取算法实验结果如图9所示。

右图为激光雷达从目标物体中间开始扫描时的特征提取结果，左图为一般扫描条件下特征提取结果。由图可知特征提取算法准确识别了目标物体特征点，标定目标物体的起始点和终止点，系统的特征识别算法较稳定。

图9 特征提取结果

系统定位测试在同一位置作20次坐标位置解算，表1为4组不同位置20次坐标解算时定位系统的精度参数，图10为气浮台在同一位置时各参数变化图。图10中x轴与y轴有个别坐标点变化较大，由于目标物体不平整、大理石台周围存在其他障碍物体干扰、激光雷达自身扫描速率不稳定等因素导致定位误差。测试结果可以得出：当前系统的定位精度为5 mm，角度精度小于1°。

激光测距雷达采集数据量大，在设计中可以使用处理器的FIFO功能合理规划逻辑提高实时性；数据处理时，由于激光雷达存在系统误差可以对传感器采集的原始数据进行预处理［15］；对于误差分析与处理，应用加权的最小二乘法或总体最小二乘方法，分析计算提高定位精度。

表1 同一位置20次坐标解算 单位：mm

图10 同一位置20次坐标定位结果

4 结论

针对具体室内环境特点，本文设计实现了一种简单的基于激光雷达的气浮台定位系统，该系统用于航天设备在地面进行各项运动功能测试、姿态模拟测试及控制性能测试。文中根据所采用激光传感器特点，提出一种简单有效的目标点识别、提取算法，在减少定位系统数据运算量同时满足系统定位精度需求。实验结果表明，该系统定位精度与方向角测量精度高，达到室内定位系统精度设计要求。同时也注意到，系统在定位过程中仅采用单个目标障碍物作为环境特征信息，这使得系统定位和控制的鲁棒性有所降低，因此提高系统定位与控制的可靠性将是下一步的研究方向。

［1］Want R，Hoper A，Falcao V.The Active Badge Location System［J］.ACM Transactions on Information System（TOIS）.1：91-102.

［2］刘君，吴建国，褚曦丹.Cricket室内定位系统的研究与改进［J］.计算机技术与发展，2011（5）：206-209.

［3］Nissanka B Priyantha，Anit Chakraborty.The Cricket Location-Support System［R］.6th ACM International Conference on Mobile Computing and Networking，2000.8

［4］陈斌涛，刘任任，陈益强.动态环境中的WiFi指纹自适应室内定位方法［J］.传感技术学报，2015，28（5）：729-738.

［5］Gonzalez J，Ollero A，Reina A.Map Building for a Mobile Robot Equipped with a 2D Laser Rangefinder［R］.Robotics and Automation，1994.Proceedings.1994 IEEE International Conference on，San Diego，CA，1994，3：1904-1909.

［6］Haffner O，Duchon F.Making an Environment Map Using Laser Rangefinder［R］.Control Conference（ICCC），2014 15th International Carpathian，Velke Karlovice，2014：139-144.

［7］黄佳，邾继贵，王一.激光线扫式形貌测量机器人的标定研究［J］.传感技术学报，2012，25（1）：62-66.

［8］RoboPeak.RPLIDAR Low Cost 360 Degree 2D Laser Scanner System［EB/OL］.http：//www.robopeak.com，2015.12.

［9］RoboPeak.etc.RPLIDAR Low Cost 360 Degree 2D Laser Scanner System Design Tool Kit［EB/OL］.http：//www.robopeak.com，2015.12.

［10］RoboPeak.RPLIDAR Low Cost 360 Degree 2D Laser Scanner System Communication［EB/OL］.http：//www.robopeak.com，2015.12.

［11］Mendes A，Nunes U.Situation-Based Multi-Target Detection and Tracking with Laser Scanner in Outdoor Semi-Structured Environment［R］.Intelligent Robots and Systems，2004.（IROS 2004）.Proceedings.2004 IEEE/RSJ International Conference，2004，1：88-93.

［12］Gonzalez J，Stentz A，Ollero A.An Iconic Position Estimator for a 2D Laser Rangefinder［R］.Robotics and Automation，1992.Proceedings，1992 IEEE International Conference on，Nice，1992，3：2646-2651.

［13］Corregedor A R，Meyer J，Du Plessis F.Design Principles for 2D Local Mapping Using a Laser Range Finder［R］.AFRICON，2011，Livingstone，2011：1-6.

［14］项志宇.基于激光雷达的移动机器人障碍检测和自定位［D］.浙江：浙江大学，2002.

［15］Borges Geovany Araujo，Aldon Marie-José.Line Extraction in 2D Range Images for Mobile Robotics［J］.Journal of Intelligent and Robotic Systems，2004，3：267-297.

徐振宇（1993-），男，中国科学院空间应用工程与技术中心在读硕士，研究方向为综合电子技术；

张永康（1986-），男，中国科学院空间应用工程与技术中心研究员助理，博士，研究方向为控制工程与技术；

董文博（1980-），男，副研究员，博士，研究方向为空间电子学，测量和控制技术。

Laser Range Scanner Based Gas Levitation Unit Positioning System Design and Implementation*

XU Zhenyu，ZHANG Yongkang，DONG Wenbo*
（Key Laboratory of Space Utilization，Technology and Engineering Center for space Utilization，Chinese Academy of Science，Beijing100094，China）

An indoor positioning（localization）system based on laser range scanner and embedded platform is designed and implemented.The system is used for a gas levitation unit's positioning and control.The system is composed of a laser range scanner，a digital signal processor and the gas levitation unit.Digital signal processor captures and process the raw data of the environment，extracts the feature points of the object，calculates the location of laser range scanner and finally achieves the real-time control of gas levitation device.The algorithm of feature points of identification points and extraction，coordinate conversion and the method of least squares curve fit is proposed in this paper.Experiment results show that the average positioning error of the system is within 5 mm while the bearing error is less than 1 degree，the system meets the required positioning accuracy of attitude control for our gas levitation system.

indoor localization；laser range scanner；feature extraction；coordinate conversion

TP23

A

1004-1699（2016）12-1924-07

��4360E；7210X

10.3969/j.issn.1004-1699.2016.12.024

项目来源：中科院太空应用创新基金项目（CXJJ-16S064）

2016-06-02修改日期：2016-06-29