基于激光雷達的氣浮臺定位系統設計與實現*

徐振宇，張永康，董文博

（中國科學院空間應用工程與技術中心，中國科學院太空應用重點實驗室，北京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