徐振宇，張永康，董文博

（中國科學(xué)院空間應(yīng)用工程與技術(shù)中心，中國科學(xué)院太空應(yīng)用重點實驗室，北京100094）

基于激光雷達的氣浮臺定位系統(tǒng)設(shè)計與實現(xiàn)*

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

（中國科學(xué)院空間應(yīng)用工程與技術(shù)中心，中國科學(xué)院太空應(yīng)用重點實驗室，北京100094）

設(shè)計并實現(xiàn)了一種基于嵌入式平臺的激光雷達定位系統(tǒng)，用于特定場景下氣浮臺的定位與控制。系統(tǒng)主要由激光雷達傳感器，DSP嵌入式控制器以及氣浮臺組成。DSP獲取并處理激光雷達傳感器原始數(shù)據(jù)、識別目標(biāo)特征點、解算坐標(biāo)值、實現(xiàn)對氣浮臺位置的實時控制。文章給出室內(nèi)環(huán)境特征點的提取方法、雷達坐標(biāo)系與系統(tǒng)坐標(biāo)系坐標(biāo)轉(zhuǎn)換算法，采用最小二乘法對識別點曲線擬合，并提取有效坐標(biāo)點，解算物體坐標(biāo)。實驗結(jié)果表明，系統(tǒng)的平均定位誤差為5 mm，角度誤差小于1°，滿足氣浮臺姿態(tài)控制所需的定位精度。

市內(nèi)定位；激光雷達；特征提?。蛔鴺?biāo)轉(zhuǎn)換

精確的室內(nèi)定位技術(shù)對于商業(yè)應(yīng)用、公共安全以及軍事應(yīng)用都有非常重要的意義。典型的定位系統(tǒng)包括：AT&T開發(fā)的Active Badge室內(nèi)定位系統(tǒng)［1］，系統(tǒng)在待測移動物體上附一個電子標(biāo)識，標(biāo)識向室內(nèi)固定放置的紅外接收機周期發(fā)送該待測物體唯一的ID和距離信息，接收機再通過有線網(wǎng)絡(luò)將數(shù)據(jù)傳輸給數(shù)據(jù)庫進行定位。由于采用的是紅外線技術(shù)，當(dāng)環(huán)境中有熒光干擾或直射陽光時，會產(chǎn)紅外干擾從而使得Active Badge產(chǎn)生定位誤差。Cricket定位系統(tǒng)通過發(fā)射、接收超聲波信號來測量固定信標(biāo)點與移動節(jié)點之間的距離進而實現(xiàn)定位的［2-3］。Cricket定位技術(shù)容易受到障礙物阻擋和反射的影響使定位精度下降。RADAR定位系統(tǒng)是將接收信號強度作為估計發(fā)射器與接收器間距的重要依據(jù)［4］。系統(tǒng)通過建立無線信號模型，確定信號強度的衰減與收發(fā)設(shè)備間距離的關(guān)系，從而獲得最優(yōu)的模型估算參數(shù)。該系統(tǒng)的最大優(yōu)勢在于它不需要特定的硬件開發(fā)和大量信號強度采樣工作。但射頻信號傳輸環(huán)境的不穩(wěn)定性嚴重影響系統(tǒng)的定位精度。

激光測距雷達在機器人導(dǎo)航和智能車輛定位中的使用日益增多［5-7］。激光測距雷達能夠以較高的速度和準(zhǔn)確度提供室內(nèi)環(huán)境信息，其基本測距原理如圖1所示。在每次測距過程中，激光雷達發(fā)射經(jīng)調(diào)制的紅外激光信號，該激光信號照射到目標(biāo)物體后產(chǎn)生的反射光，該反射光被激光雷達的視覺系統(tǒng)接收，經(jīng)激光雷達內(nèi)部DSP處理器進行實時解算。激光雷達以雷達坐標(biāo)系為基準(zhǔn)輸出物體坐標(biāo)，d為激光雷達中心與物體的直線距離，θ表示物體相對于雷達朝向之間的夾角。在電機驅(qū)動下激光雷達將進行順時針旋轉(zhuǎn)，從而實現(xiàn)360度全方位環(huán)境的掃描測距。THMR-V新一代智能車上安裝有二維激光雷達、全景攝像機等傳感器，其設(shè)計車速最高為80 km/h，目前已能夠在校園非結(jié)構(gòu)化道路環(huán)境下，進行道路跟蹤和避障自主行駛。

本文給出一種應(yīng)用激光測距雷達在氣浮臺上確定雷達載體相對氣浮臺的位置與方向角方法。論文第二部分介紹系統(tǒng)結(jié)構(gòu)；論文第三部分給出一種簡單有效的特征點提取方法和測距雷達坐標(biāo)計算方法；第四部分給出實際系統(tǒng)的定位精度與誤差分析以及進一步提高系統(tǒng)定位精度的改進方向；最后給出論文結(jié)論。

1 系統(tǒng)介紹

本文所提出的定位系統(tǒng)由氣浮臺、數(shù)字信號處理器及激光雷達傳感器組成，系統(tǒng)示意圖如圖2所示。水平大理石臺四個拐點安裝有立柱，目標(biāo)物體是尺寸為360 mm×270 mm表面光滑白色平板。氣浮臺是依靠壓縮空氣在氣浮軸承與軸承座之間形成的氣模，使模擬臺體浮起，從而實現(xiàn)失重和無摩擦的相對運動條件，以模擬衛(wèi)星在外層空間所受擾動力矩很小的力學(xué)環(huán)境。文中所用的滑臺式氣浮臺采用三點支撐的氣浮技術(shù)，依靠壓縮空氣在致密大理石桌面（00級）與固定在滑臺下的三個氣浮塊之間形成氣膜，使滑臺浮起。激光雷達傳感器采用RoboPeak團隊開發(fā)的二維激光雷達RPLIDAR［8-10］，選取TMS320F2812作為處理控制核心。激光雷達傳感器放置在氣浮臺中心位置，氣浮臺周圍無可掃描的障礙。

激光雷達將獲得的原始測距數(shù)據(jù)以特定的數(shù)據(jù)幀格式通過串口發(fā)送至DSP，DSP進行雷達載體位置解算及位置控制計算。

定位系統(tǒng)硬件與軟件框圖如圖3、圖4所示，硬件系統(tǒng)主要包括①DSP核心控制板和驅(qū)動板；②RPLIDAR激光雷達傳感器；③氣浮臺氣路及噴氣裝置等。DSP核心控制板主控芯片采用TMS320F2812數(shù)字信號處理器芯片，其最高系統(tǒng)頻率可達150MHz。TMS320F2812強大的數(shù)據(jù)處理能力和豐富的外設(shè)資源使得其廣泛應(yīng)用于實時數(shù)據(jù)處理和控制系統(tǒng)中。RPLIDAR主要分為激光測距核心、高速旋轉(zhuǎn)的機械部分。激光測距核心采用三角測距技術(shù)，配合高速的視覺采集處理機構(gòu)，可進行每秒高達2 000次的測距動作。處理器可以通過RPLIDAR的串口實時獲取RPLIDAR的掃描測距數(shù)據(jù)。

圖3 硬件系統(tǒng)框圖

軟件流程主要步驟為：①系統(tǒng)上電檢測，激光雷達傳感器上電自檢；②DSP發(fā)出掃描指令并等待傳感器響應(yīng)；③DSP處理激光雷達數(shù)據(jù)；④氣浮臺位置解算與控制。系統(tǒng)首先進行上電自檢，如果RPLIDAR檢測到自身工作異常后，則會關(guān)閉自身工作，進入保護性停機模式，但外部系統(tǒng)仍可以與RPLIDAR進行通訊，查詢其工作狀態(tài)等信息。當(dāng)RPLIDAR接收到測距命令時，其開始掃描測距同時將傳感器周圍障礙物信息按固定的數(shù)據(jù)格式通過串口發(fā)送給DSP處理板。處理板根據(jù)傳感器采集的距離和角度信息，提取目標(biāo)物體特征、解算傳感器位置、實時控制電磁氣閥噴氣量，從而實現(xiàn)對氣浮臺位置的實時確定與控制。

圖4 軟件框圖

2 定位方法

定位步驟分為兩步：①從激光雷達采集的原始數(shù)據(jù)中，提取出目標(biāo)物體特征數(shù)據(jù)點、識別特征物體的起始點；②根據(jù)目標(biāo)物體的起始點信息，解算出激光雷達在位置。

2.1 特征點識別

氣浮臺移動過程中，激光雷達得到一組周圍環(huán)境在雷達坐標(biāo)系下的點數(shù)據(jù)集。特征提取需要從點數(shù)據(jù)中提取出目標(biāo)物體特征點、起始點和終止點，示例如圖5所示。

圖5中所有數(shù)據(jù)點為氣浮臺周圍環(huán)境數(shù)據(jù)點，特征提取的目的是在圖中找到目標(biāo)物體數(shù)據(jù)點、起始點和終止點。

圖5 特征點識別示例

式中：dmin=min{dk,dk+1}，?是激光雷達角度分辨率，β和C0為兩個系統(tǒng)參數(shù)。判斷準(zhǔn)則原理如圖6所示，C0取值根據(jù)經(jīng)驗值給出，β通過式（2）、式（3）確定：

式中：f表示相鄰兩點之間實際距離，相鄰兩個點的測量距離值分別為dk和dk+1，?是激光雷達角度分辨率。若dk,k+1滿足1式則將放入子集Si，繼續(xù)判斷dk+1,k+2。若dk,k+1不滿足式（1）則將放入子集Si，將放入新子集Si+1，繼續(xù)判斷dk+1,k+2，重復(fù)執(zhí)行步驟③直到有效點數(shù)據(jù)判斷完全。執(zhí)行步驟③后，得到若干有效點子集Si( )i=1,2,…；④比較有效點子集中點個數(shù)，取點數(shù)據(jù)個數(shù)大于δ的子集。

圖6 判別準(zhǔn)則原理圖

預(yù)處理后得到若干有效數(shù)據(jù)點子集Si，由于設(shè)計中目標(biāo)物體的尺寸為360 mm×270 mm，氣浮臺周圍無其他連續(xù)障礙物，因此判斷目標(biāo)物體的有效點數(shù)大于特定閾值δ，取有效數(shù)據(jù)點個數(shù)大于δ的子集表示目標(biāo)物體特征點數(shù)據(jù)集。對目標(biāo)特征點數(shù)據(jù)集做最小二乘法曲線擬合，得到修正的后的點數(shù)據(jù)集S′。

設(shè)計中激光雷達有兩種掃描方式，由目標(biāo)物內(nèi)開始掃描和物體外開始掃描，如圖7所示。激光雷達從目標(biāo)物體外開始掃面時目標(biāo)物體特征點僅有一個有效數(shù)據(jù)點集，而當(dāng)激光雷達從目標(biāo)物體內(nèi)開始掃描時將獲得兩個有效數(shù)據(jù)點集。由于氣浮臺所處實際環(huán)境和RPLIDAR自身特性，激光雷達采集到無效點時距離和角度數(shù)據(jù)為零、始終按順時針方向掃描，因此采集的目標(biāo)物體數(shù)據(jù)點有④以下6個特征：①起點從零跳變到有效值（大于激光雷達盲區(qū)值100 mm）；②終點從有效值（大于100 mm）跳變到零；③終點角度大于起點角度（對于雷達本身）；有效線段采樣點大于20（目標(biāo)物有足夠的掃面點）；⑤起點，終點角度之差小于180°；⑥相鄰兩點間距離之差小于20 mm。

圖7 激光雷達掃面示意圖

根據(jù)特征提取得到的有效點數(shù)據(jù) (di,θi) (i=1,2,…,360)，判斷激光雷達的掃描方式。具體方法如下：取有線點數(shù)據(jù)集中6個點數(shù)據(jù)，判斷相鄰兩點距離值dk,k+1。若dk,k+1不為 0（采集到有效數(shù)據(jù)點），則激光雷達從目標(biāo)物體內(nèi)開始掃描，否則從目標(biāo)物體外掃描。

（1）目標(biāo)物體外掃描：目標(biāo)物體有一個特征點子集Sn，從Sn中取出第一個點和最后一個點分別作為目標(biāo)物體的起始點，終止點。

（2）目標(biāo)物體內(nèi)掃描：目標(biāo)物體有兩個特征點子集S1、Sn（子集Sn中最后一個點數(shù)據(jù)與S1中第一個點數(shù)據(jù)為相鄰點），取S1中最后點為終止點，Sn中第一個點為開始點

2.2 坐標(biāo)解算

定位系統(tǒng)中設(shè)定兩個坐標(biāo)系，目標(biāo)物體所在的全局坐標(biāo)系、以激光雷達為原點的局部坐標(biāo)系。全局坐標(biāo)系根據(jù)實際環(huán)境預(yù)先設(shè)定，假設(shè)目標(biāo)物體在全局坐標(biāo)系下的位置為。氣浮臺移動過程中，激光雷達測量得到目標(biāo)物體在局部坐標(biāo)系下的位置，設(shè)為由兩組局部坐標(biāo)系下位置數(shù)據(jù)解算氣浮臺在全局坐標(biāo)系下的位置［12-14］。具體數(shù)學(xué)描述如下：

圖8 坐標(biāo)轉(zhuǎn)換原理圖

假設(shè)標(biāo)記點P在全局坐標(biāo)系和激光雷達坐標(biāo)系中的坐標(biāo)分別激光雷達采集P點的數(shù)據(jù)為(r,θ)，轉(zhuǎn)換成直角坐標(biāo)：

矩陣表示為：

3 實驗及結(jié)果分析

在水平大理石平臺上驗證氣浮臺定位算法和控制精度測試實驗。大理石臺前側(cè)放置360 mm× 270 mm固定尺寸表面光滑白色紙板，拐點安裝有四個立柱，支撐目標(biāo)物體，氣浮臺周圍無其他障礙點。設(shè)定全局坐標(biāo)系、標(biāo)定目標(biāo)物體在全局坐標(biāo)系中的坐標(biāo)。測試定位系統(tǒng)目標(biāo)特征點提取算法精確度，特征提取算法實驗結(jié)果如圖9所示。

右圖為激光雷達從目標(biāo)物體中間開始掃描時的特征提取結(jié)果，左圖為一般掃描條件下特征提取結(jié)果。由圖可知特征提取算法準(zhǔn)確識別了目標(biāo)物體特征點，標(biāo)定目標(biāo)物體的起始點和終止點，系統(tǒng)的特征識別算法較穩(wěn)定。

圖9 特征提取結(jié)果

系統(tǒng)定位測試在同一位置作20次坐標(biāo)位置解算，表1為4組不同位置20次坐標(biāo)解算時定位系統(tǒng)的精度參數(shù)，圖10為氣浮臺在同一位置時各參數(shù)變化圖。圖10中x軸與y軸有個別坐標(biāo)點變化較大，由于目標(biāo)物體不平整、大理石臺周圍存在其他障礙物體干擾、激光雷達自身掃描速率不穩(wěn)定等因素導(dǎo)致定位誤差。測試結(jié)果可以得出：當(dāng)前系統(tǒng)的定位精度為5 mm，角度精度小于1°。

激光測距雷達采集數(shù)據(jù)量大，在設(shè)計中可以使用處理器的FIFO功能合理規(guī)劃邏輯提高實時性；數(shù)據(jù)處理時，由于激光雷達存在系統(tǒng)誤差可以對傳感器采集的原始數(shù)據(jù)進行預(yù)處理［15］；對于誤差分析與處理，應(yīng)用加權(quán)的最小二乘法或總體最小二乘方法，分析計算提高定位精度。

表1 同一位置20次坐標(biāo)解算 單位：mm

圖10 同一位置20次坐標(biāo)定位結(jié)果

4 結(jié)論

針對具體室內(nèi)環(huán)境特點，本文設(shè)計實現(xiàn)了一種簡單的基于激光雷達的氣浮臺定位系統(tǒng)，該系統(tǒng)用于航天設(shè)備在地面進行各項運動功能測試、姿態(tài)模擬測試及控制性能測試。文中根據(jù)所采用激光傳感器特點，提出一種簡單有效的目標(biāo)點識別、提取算法，在減少定位系統(tǒng)數(shù)據(jù)運算量同時滿足系統(tǒng)定位精度需求。實驗結(jié)果表明，該系統(tǒng)定位精度與方向角測量精度高，達到室內(nèi)定位系統(tǒng)精度設(shè)計要求。同時也注意到，系統(tǒng)在定位過程中僅采用單個目標(biāo)障礙物作為環(huán)境特征信息，這使得系統(tǒng)定位和控制的魯棒性有所降低，因此提高系統(tǒng)定位與控制的可靠性將是下一步的研究方向。

［1］Want R，Hoper A，F(xiàn)alcao V.The Active Badge Location System［J］.ACM Transactions on Information System（TOIS）.1：91-102.

［2］劉君，吳建國，褚曦丹.Cricket室內(nèi)定位系統(tǒng)的研究與改進［J］.計算機技術(shù)與發(fā)展，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］陳斌濤，劉任任，陳益強.動態(tài)環(huán)境中的WiFi指紋自適應(yīng)室內(nèi)定位方法［J］.傳感技術(shù)學(xué)報，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］黃佳，邾繼貴，王一.激光線掃式形貌測量機器人的標(biāo)定研究［J］.傳感技術(shù)學(xué)報，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］.浙江：浙江大學(xué)，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-），男，中國科學(xué)院空間應(yīng)用工程與技術(shù)中心在讀碩士，研究方向為綜合電子技術(shù)；

張永康（1986-），男，中國科學(xué)院空間應(yīng)用工程與技術(shù)中心研究員助理，博士，研究方向為控制工程與技術(shù)；

董文博（1980-），男，副研究員，博士，研究方向為空間電子學(xué)，測量和控制技術(shù)。

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

項目來源：中科院太空應(yīng)用創(chuàng)新基金項目（CXJJ-16S064）

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