基于反光柱的融合定位方法研究

摘要：針對復雜場景下，因環境特征變化導致的定位穩定性和精度下降等問題，提出基于反光柱的融合定位方法。通過基于最小二乘的反光柱圓心擬合、基于反光柱觀測的粒子濾波定位、基于擴展卡爾曼濾波的即時定位與地圖構建與反光柱的融合定位方法，實現了一種輕量的、具有較高精度及魯棒性的機器人定位系統。經試驗驗證，該融合定位方法在動態環境下有較大的容錯糾偏范圍，在靜態定位上可達毫米級的定位精度，能滿足移動機器人、搬運AGV等場景使用要求。

關鍵詞：反光柱；融合定位；圓心擬合；粒子濾波定位；即時定位與地圖構建

中圖分類號：TP242.6" " " " " "文獻標志碼：A" " " " " 文章編號：1674-2605（2024）02-0002-07

DOI：10.3969/j.issn.1674-2605.2024.02.002

Research on Fusion Localization Method Based on Reflective Columns

HUANG Xiaochun1，2

（1.Fujian （Quanzhou） Institute of Advanced Manufacturing Technology， Quanzhou 362000， China

2. Fujian Key Laboratory of Intelligent Operation and Maintenance Robot Technology， Quanzhou 362000， China）

Abstract： A fusion localization method based on reflective columns is proposed to address the issues of localization stability and accuracy degradation caused by changes in environmental features in complex scenes. A lightweight， highly accurate， and robust robot positioning system has been achieved through the fitting of the center of the reflective column based on least squares， particle filtering positioning based on reflective column observation， real-time positioning based on extended Kalman filtering， simultaneous localization and mapping， and fusion positioning method with reflective columns. Through experimental verification， this fusion positioning method has a large range of fault tolerance and correction in dynamic environments， and can achieve millimeter level positioning accuracy in static positioning， which can meet the requirements of mobile robots， handling AGVs and other scenarios.

Keywords： reflective column; fusion positioning; center fitting; particle filter localization; simultaneous localization and mapping

0 引言

隨著人工智能技術的高速發展，智能機器人在各行業的應用日漸廣泛。定位系統是智能機器人自主導航完成作業的關鍵。定位是指機器人在慣性參考系中確定自身位置及姿態的過程。近年來，多種定位技術應運而生，如全球導航衛星系統[1]（global navigation satellite system， GNSS）定位；通過里程計、慣性測量單元（inertial measurement unit， IMU）實現航跡推算的慣性導航；借助激光雷達或相機的即時定位與地圖構建[2]（simultaneous localization and mapping， SLAM）；基于激光雷達的反光柱定位等。其中，反光柱定位方法因定位精度高、易于維護，在工業場景中應用廣泛。周凱月等[3]提出反光柱定位誤差與激光定位誤差聯合優化的緊耦合定位方法。陳新宇等[4]采用特征角點提取和scan-to-map匹配，再利用列文伯格-馬夸爾特進行位姿優化。吳波[5]提出三邊定位與里程計融合的定位方法。吳衛國等[6]提出最小二乘的三邊定位方法。然而上述方法對反光柱的部署要求較高，通常至少需要觀測2～3個反光柱。基于優化的方法，如文獻[3]、[4]對算力要求較高，實時性是個挑戰；三邊定位法，如文獻[5]、[6]在實際應用中可能受障礙物遮擋、信號干擾等因素影響，容易造成定位誤差，且對反光柱幾何關系要求較高，需要較為精確的標定參數。

為進一步提升復雜、動態場景的定位性能，本文利用粒子濾波[7]實現反光柱定位，將反光柱定位與SLAM定位融合，獲得高魯棒性、高精度的定位信息。

1 反光柱定位

反光柱定位主要包括反光柱圓心擬合和粒子濾波定位兩部分。其中，反光柱圓心擬合是利用原始激光數據計算各反光柱的圓心；粒子濾波定位是以各反光柱的圓心作為觀測進行定位。

1.1" 反光柱圓心擬合

反光柱圓心擬合主要包括反光柱部署、激光雷達調平、反光柱分割與檢測、圓心擬合4個步驟。

1.1.1" 反光柱部署

因為標準圓柱體具有更大的可視角度，所以通常將反光膠貼貼于圓柱體表面作為反光柱。在部署反光柱時，應盡量避免采用相似的布局，以增強環境特征；避免部署在反射率較高的物體附近，如玻璃等，以防止干擾；反光柱間的距離不宜過小，否則可能導致不同反光柱的激光點云無法有效分割、誤識別或不識別的現象。

1.1.2" 激光雷達調平

在平整地面，以激光雷達安裝高度為準，在距離激光雷達約15 m的前、后、左、右4個方向各部署1個反光柱；微調激光雷達姿態和反光柱高度，直到激光雷達能同時檢測到4個反光柱，則認為激光雷達已經調平。若地面存在微小坡度，導致激光雷達在較遠處檢測不到反光柱時，可通過增加反光柱的高度來增加垂直可視范圍。

1.1.3" 反光柱分割與檢測

單線激光雷達的掃描平面與反光柱的相交面理論上是一個正圓，其掃描結果是由一系列離散的激光點構成的圓弧。由于打在反光柱上的激光點反射強度遠高于其他材料，通過設定合適的反射強度閾值，可有效過濾出反光柱的反射激光點。在機器人操作系統（robot operating system， ROS）中，單線激光雷達采集的數據以極坐標的方式表示，且按照掃描角度從小到大依次排列。本文采用區域生長算法對過濾出的反射激光點按歐式距離進行聚類，如圖1所示。

圖1中，相鄰的兩個反光柱P和Q，依次計算兩個反光柱相鄰激光點的歐式距離，并通過設定合理的閾值，即可得出屬于反光柱P，屬于反光柱Q。

為防止可選閾值太少且過于臨界，反光柱的間距不能過小。通常根據激光雷達的角度分辨率、掃描距離和反光柱直徑等因素，選擇大于2倍的最大反光柱直徑為宜。

1.1.4" 圓心擬合

擬合是指通過已知的樣本點，先求解已有模型的參數，再通過求解的參數用函數近似地表示該模型。反光柱上的激光點位于反光柱截面的正圓上，設該正圓圓心坐標為，半徑為，則圓的方程為，展開變形可得，其中。

設某個反光柱上的n個激光點為，這些激光點坐標與待估計參數構成一個超定方程組，采用最小二乘法進行擬合。設待估計參數為，利用個激光點構造矩陣、向量：

（1）

定義優化目標為，令，可使取最小值，解得

（2）

將第個反光柱上所有激光點的坐標代入公式（2），可求出第個反光柱所在正圓的參數，即圓心坐標、半徑。同理，可求出其他檢測到的所有反光柱的圓心坐標。

由于擬合的反光柱圓心坐標是激光雷達坐標系下的表示，需根據機器人基坐標系到激光雷達坐標系的位姿關系，將圓心坐標轉換到機器人基坐標系，即

（3）

式中：為機器人基坐標系下對第個反光柱的觀測，、分別為機器人基坐標系到激光雷達坐標系的旋轉矩陣和平移向量。

該方法無需預先知道反光柱半徑（理論上可以支持任意半徑的反光柱），解決了場景中無法使用同一規格反光柱的問題。但需要注意的是，若有3個待估計參數，當同一反光柱上的激光點數小于3個時，方程組有無窮多個解，將這類無法計算反光柱圓心坐標的數據直接丟棄。

1.2" 粒子濾波定位

通過各反光柱的圓心坐標，進行粒子濾波定位。首先，根據給定的初始定位位姿生成一定數量的粒子；然后，通過機器人底盤反饋的速度信息對粒子進行狀態預測，并將反光柱坐標映射到地圖上；接著，計算權重，選擇最符合觀測的粒子作為定位結果；最后，為下一輪迭代進行粒子重采樣。

1.2.1" 初始化

給定初始定位位姿和粒子分布方差；以初始定位位姿為均值，以粒子分布方差構造高斯分布；從高斯分布中隨機抽取個粒子，表示當前機器人可能的所有位姿。

1.2.2" 狀態預測

機器人底盤以一定的采樣頻率反饋其速度信息。狀態預測利用該速度信息進行航跡推算，計算各個粒子的位姿，其結果作為當前時刻各粒子的先驗估計。

假設機器人在采樣間隔期間以恒定線速度、角速度運動。記機器人底盤反饋速度為，系統狀態為。根據全向移動機器人運動模型可知其狀態微分方程為

（4）

離散時間下的位姿遞推方程為：

當時

（5）

當時

（6）

式中：為采樣時間間隔。

1.2.3" 反光柱映射

設某一時刻觀測到個反光柱，這些反光柱在機器人基坐標系下的圓心坐標集合記為，則根據上述個粒子，將變換到地圖坐標系下的圓心坐標集合分別記為。以為例，設，反光柱地圖上的圓心坐標集合為，采用不放回的一一對比方式，從中取出一個反光柱圓心坐標點，依次計算該坐標點與集合中各坐標點的歐式距離，并從中取距離最小值對應的，同時記下對應關系。如此反復，直至中的所有反光柱均與地圖上的反光柱完成映射。

該方法相比三邊定位方法，對反光柱的幾何參數要求更低，具有較高的容錯能力和動態抗擾能力。

1.2.4" 權重計算

根據反光柱的對應關系，以為均值，以方差、構造二元高斯分布，計算在該分布中的概率密度值，并作為權重：

式中：。

則第0個粒子的權重為

（7）

同理可計算其他粒子的權重，權重最高的粒子代表了機器人的最佳后驗位姿估計。

1.2.5" 重采樣

以后驗位姿估計為均值，動態方差構造高斯分布，再從該分布中進行粒子重采樣，為下一次迭代提供新的粒子。采用動態方差策略可兼顧機器人定位的魯棒性與精度，其原理是當所有粒子的總權重低于一定水平，則放大方差，增加粒子的多樣性，以便更有機會命中真值附近的位姿，進而提高這些粒子的權重。隨著粒子權重的逐步提高，動態方差也隨之減小，粒子的多樣性也減小，所有粒子收斂到真值附近，并穩定在一定的范圍內。

由于所有粒子的總權重與粒子總數相關，根據工程經驗，取，記，收斂時的最小標準差為，最大標準差為。

當時，表示定位不收斂，則

（8）

當時，表示定位已收斂，則

（9）

2 融合定位

實際工程應用具有復雜性，如場景中部分區域無法部署反光柱，導致粒子濾波定位無法持續對其進行觀測。若僅通過航跡推算進行定位，存在較大的累積誤差；若僅通過SLAM進行定位，因作業環境易變，其精度下降甚至失效。

卡爾曼濾波[8]（Kalman filter， KF）是一種閉式計算，有解析解，具有算力要求低、實時性好等特點。但該方法基于2個前提假設：1） 狀態轉移和狀態觀測都是線性模型；2） 系統的相關噪聲都是高斯白噪聲。恒定線速度與角速度的運動軌跡是圓，并不是線性的，需要對其進行線性化，即擴展卡爾曼濾波[9]（extended Kalman filter， EKF）。

本文采用EKF實現反光柱與SLAM的融合定位。在該方法中，濾波器以速度信息（線速度和角速度）進行預測，SLAM定位和反光柱定位均作為觀測（位姿和協方差）。設SLAM定位信息為，反光柱定位信息為，融合定位流程如圖2所示。

設系統狀態為，表示機器人在地圖坐標系下的位姿；控制輸入為，表示機器人本體的線速度和角速度。

1） 通過初始定位位姿及初始協方差對系統狀態進行初始化；

2） 設系統狀態轉移方程為

式中：，為過程噪聲；為雅可比矩陣。

定義為先驗估計，為后驗估計，則狀態預測表示為，即通過上一時刻的后驗位姿估計和速度信息計算當前時刻的先驗估計具體如下：

當時

（10）

當時

（11）

機器人在運行過程中可能存在打滑等異常情況，很難對類似的未知情況進行建模，則估計協方表示為

（12）

3） 由于觀測量與狀態量一致，故觀測矩陣。當濾波器接收到反光柱定位信息時，，；當接收到SLAM定位信息時，，，將參數代入公式（13），可依次求解出當前時刻的卡爾曼增益、當前時刻的狀態估計、當前狀態估計的協方差矩陣。

（13）

上述的形式有別于，稱為Joseph form，由于某些極端情況下，可能因為浮點運算誤差變為非對稱矩陣，使結果出現較大偏差[10]。

4） 在濾波器執行過程中，當部分環境特征發生變化時，可能使變大，系統更傾向于預測值和反光柱的定位更新；當機器人經過沒有反光柱的區域時，系統仍可以依靠預測值和SLAM定位更新進行導航。

3 試驗

針對本文提出的基于反光柱的融合定位方法，對搭載了單線激光雷達的對角舵輪的全向移動機器人進行模擬試驗，試驗參數如表1所示。

在完成反光柱標定以及柵格地圖構建后，將全向移動機器人置于地圖場景中運行。

1） 平滑運行試驗。設計兩個作業區域，同時在這兩個作業區域及其連通路徑上構建柵格地圖，并只在兩個作業區域部署反光柱。經測試，全向移動機器人可順利在兩個作業區域之間穩定運行，且由于反光柱的作用，定位精度較高（詳見精度試驗）。

2） 抗干擾能力試驗。通過人為給定錯誤的狀態來模擬異常情況。經測試，該融合定位方法具有較強的糾偏能力，其位置偏差容錯范圍為一個半徑大于2 m的圓，姿態偏差容錯范圍約為。抗干擾能力試驗糾正過程如圖3所示。

給定錯誤的狀態，如圖3（a）所示位置；當前定位與真值偏差較大，粒子較為分散，如圖3（d）所示；隨著部分粒子靠近真值，權重逐步提高，方差逐步下降，粒子越來越集中，如圖3（b）、（c）所示；直到收斂至真值附近，粒子高度集中，擬合的反光柱圓心與地圖中的反光柱高度重合，如圖3（f）、（e）所示。

3） 精度試驗。在機器人能穩定觀測到一定數量的反光柱時，采集500組定位數據，其位姿分布近似為一個多元高斯分布（如圖4所示）；機器人定位具有較小的標準差和偏差范圍（如表2所示），表明該融合定位方法定位精度較高。

4 結論

本文提出了一種基于點云擬合的反光柱圓心坐標計算方法，解決了反光柱規格不統一的限制；提出動態方差的粒子濾波定位方法，解決了魯棒性與高精度之間的矛盾；采用EKF位姿融合的方式為系統的擴展性保留了較大的靈活性，并在一定程度上降低了算力的要求，解決了環境特征變化及定位精度的問題。本文的研究還存在一些局限性，如在更極端的復雜環境下需要更全面的數據集進行實驗驗證等，未來的研究可以繼續改進算法、探索其他定位傳感器的融合方式，以進一步提高定位精度和魯棒性。

參考文獻

[1] 王楚揚. GNSS多頻松/緊組合RTK定位算法研究[D].阜新：遼寧工程技術大學，2022.

[2] Raúl Mur-Artal， Juan D Tardós. ORB-SLAM2： An open-source SLAM system for monocular， stereo， and RGB-D cameras[J]. IEEE Transactions on Robotics， 2017，33（5）：1255-1262.

[3] 周凱月，張建偉.融合反光柱的2D激光SLAM和高精度定位系統[J].現代計算機，2020（11）：3-7.

[4] 陳新宇，雷斌，蔣林，等.基于三維激光雷達和反光柱的機器人定位[J].組合機床與自動化加工技術， 2022（9）：11-15;19.

[5] 吳波.基于激光雷達和反光板的移動機器人定位方法研究[D].武漢：華中科技大學，2019.

[6] 吳衛國，丁銳，孫宏圖，等.基于激光雷達和反光板的重載AGV定位方法研究[J].工程機械， 2021， 52（7）：11-16;8.

[7] Sebastian Thrun， Wolfram Burgard， Dieter Fox.概率機器人[M].曹紅玉，譚志，史曉霞，等，譯.北京：機械工業出版社，2017.

[8] YAVUZ S， KURT Z， M. SERDAR Bi?er. Simultaneous localization and mapping using extended Kalman filter[C]// IEEE Signal Processing amp; Communications Applications Conference. IEEE， 2009：700-703

[9] MOORE T， STOUCH D. A generalized extended Kalman filter implementation for the robot operating system[C]// Intelligent Autonomous Systems 13： Proceedings of the 13th International Conference IAS-13. Springer International Publishing， 2016： 335-348.

[10] BUCY R S， JOSEPH P D. Filtering for stochastic processes with applications to guidance[M]. New York： AMS Chelsea Publ， 2005.

作者簡介：

黃小春，男，1987年生，本科，高級工程師，主要研究方向：機器人導航相關技術。E-mail： syntax@dingtalk.com