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

摘要：針對(duì)復(fù)雜場(chǎng)景下，因環(huán)境特征變化導(dǎo)致的定位穩(wěn)定性和精度下降等問(wèn)題，提出基于反光柱的融合定位方法。通過(guò)基于最小二乘的反光柱圓心擬合、基于反光柱觀測(cè)的粒子濾波定位、基于擴(kuò)展卡爾曼濾波的即時(shí)定位與地圖構(gòu)建與反光柱的融合定位方法，實(shí)現(xiàn)了一種輕量的、具有較高精度及魯棒性的機(jī)器人定位系統(tǒng)。經(jīng)試驗(yàn)驗(yàn)證，該融合定位方法在動(dòng)態(tài)環(huán)境下有較大的容錯(cuò)糾偏范圍，在靜態(tài)定位上可達(dá)毫米級(jí)的定位精度，能滿足移動(dòng)機(jī)器人、搬運(yùn)AGV等場(chǎng)景使用要求。

關(guān)鍵詞：反光柱；融合定位；圓心擬合；粒子濾波定位；即時(shí)定位與地圖構(gòu)建

中圖分類號(hào)：TP242.6" " " " " "文獻(xiàn)標(biāo)志碼：A" " " " " 文章編號(hào)：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 引言

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

為進(jìn)一步提升復(fù)雜、動(dòng)態(tài)場(chǎng)景的定位性能，本文利用粒子濾波[7]實(shí)現(xiàn)反光柱定位，將反光柱定位與SLAM定位融合，獲得高魯棒性、高精度的定位信息。

1 反光柱定位

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

1.1" 反光柱圓心擬合

反光柱圓心擬合主要包括反光柱部署、激光雷達(dá)調(diào)平、反光柱分割與檢測(cè)、圓心擬合4個(gè)步驟。

1.1.1" 反光柱部署

因?yàn)闃?biāo)準(zhǔn)圓柱體具有更大的可視角度，所以通常將反光膠貼貼于圓柱體表面作為反光柱。在部署反光柱時(shí)，應(yīng)盡量避免采用相似的布局，以增強(qiáng)環(huán)境特征；避免部署在反射率較高的物體附近，如玻璃等，以防止干擾；反光柱間的距離不宜過(guò)小，否則可能導(dǎo)致不同反光柱的激光點(diǎn)云無(wú)法有效分割、誤識(shí)別或不識(shí)別的現(xiàn)象。

1.1.2" 激光雷達(dá)調(diào)平

在平整地面，以激光雷達(dá)安裝高度為準(zhǔn)，在距離激光雷達(dá)約15 m的前、后、左、右4個(gè)方向各部署1個(gè)反光柱；微調(diào)激光雷達(dá)姿態(tài)和反光柱高度，直到激光雷達(dá)能同時(shí)檢測(cè)到4個(gè)反光柱，則認(rèn)為激光雷達(dá)已經(jīng)調(diào)平。若地面存在微小坡度，導(dǎo)致激光雷達(dá)在較遠(yuǎn)處檢測(cè)不到反光柱時(shí)，可通過(guò)增加反光柱的高度來(lái)增加垂直可視范圍。

1.1.3" 反光柱分割與檢測(cè)

單線激光雷達(dá)的掃描平面與反光柱的相交面理論上是一個(gè)正圓，其掃描結(jié)果是由一系列離散的激光點(diǎn)構(gòu)成的圓弧。由于打在反光柱上的激光點(diǎn)反射強(qiáng)度遠(yuǎn)高于其他材料，通過(guò)設(shè)定合適的反射強(qiáng)度閾值，可有效過(guò)濾出反光柱的反射激光點(diǎn)。在機(jī)器人操作系統(tǒng)（robot operating system， ROS）中，單線激光雷達(dá)采集的數(shù)據(jù)以極坐標(biāo)的方式表示，且按照掃描角度從小到大依次排列。本文采用區(qū)域生長(zhǎng)算法對(duì)過(guò)濾出的反射激光點(diǎn)按歐式距離進(jìn)行聚類，如圖1所示。

圖1中，相鄰的兩個(gè)反光柱P和Q，依次計(jì)算兩個(gè)反光柱相鄰激光點(diǎn)的歐式距離，并通過(guò)設(shè)定合理的閾值，即可得出屬于反光柱P，屬于反光柱Q。

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

1.1.4" 圓心擬合

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

設(shè)某個(gè)反光柱上的n個(gè)激光點(diǎn)為，這些激光點(diǎn)坐標(biāo)與待估計(jì)參數(shù)構(gòu)成一個(gè)超定方程組，采用最小二乘法進(jìn)行擬合。設(shè)待估計(jì)參數(shù)為，利用個(gè)激光點(diǎn)構(gòu)造矩陣、向量：

（1）

定義優(yōu)化目標(biāo)為，令，可使取最小值，解得

（2）

將第個(gè)反光柱上所有激光點(diǎn)的坐標(biāo)代入公式（2），可求出第個(gè)反光柱所在正圓的參數(shù)，即圓心坐標(biāo)、半徑。同理，可求出其他檢測(cè)到的所有反光柱的圓心坐標(biāo)。

由于擬合的反光柱圓心坐標(biāo)是激光雷達(dá)坐標(biāo)系下的表示，需根據(jù)機(jī)器人基坐標(biāo)系到激光雷達(dá)坐標(biāo)系的位姿關(guān)系，將圓心坐標(biāo)轉(zhuǎn)換到機(jī)器人基坐標(biāo)系，即

（3）

式中：為機(jī)器人基坐標(biāo)系下對(duì)第個(gè)反光柱的觀測(cè)，、分別為機(jī)器人基坐標(biāo)系到激光雷達(dá)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量。

該方法無(wú)需預(yù)先知道反光柱半徑（理論上可以支持任意半徑的反光柱），解決了場(chǎng)景中無(wú)法使用同一規(guī)格反光柱的問(wèn)題。但需要注意的是，若有3個(gè)待估計(jì)參數(shù)，當(dāng)同一反光柱上的激光點(diǎn)數(shù)小于3個(gè)時(shí)，方程組有無(wú)窮多個(gè)解，將這類無(wú)法計(jì)算反光柱圓心坐標(biāo)的數(shù)據(jù)直接丟棄。

1.2" 粒子濾波定位

通過(guò)各反光柱的圓心坐標(biāo)，進(jìn)行粒子濾波定位。首先，根據(jù)給定的初始定位位姿生成一定數(shù)量的粒子；然后，通過(guò)機(jī)器人底盤反饋的速度信息對(duì)粒子進(jìn)行狀態(tài)預(yù)測(cè)，并將反光柱坐標(biāo)映射到地圖上；接著，計(jì)算權(quán)重，選擇最符合觀測(cè)的粒子作為定位結(jié)果；最后，為下一輪迭代進(jìn)行粒子重采樣。

1.2.1" 初始化

給定初始定位位姿和粒子分布方差；以初始定位位姿為均值，以粒子分布方差構(gòu)造高斯分布；從高斯分布中隨機(jī)抽取個(gè)粒子，表示當(dāng)前機(jī)器人可能的所有位姿。

1.2.2" 狀態(tài)預(yù)測(cè)

機(jī)器人底盤以一定的采樣頻率反饋其速度信息。狀態(tài)預(yù)測(cè)利用該速度信息進(jìn)行航跡推算，計(jì)算各個(gè)粒子的位姿，其結(jié)果作為當(dāng)前時(shí)刻各粒子的先驗(yàn)估計(jì)。

假設(shè)機(jī)器人在采樣間隔期間以恒定線速度、角速度運(yùn)動(dòng)。記機(jī)器人底盤反饋速度為，系統(tǒng)狀態(tài)為。根據(jù)全向移動(dòng)機(jī)器人運(yùn)動(dòng)模型可知其狀態(tài)微分方程為

（4）

離散時(shí)間下的位姿遞推方程為：

當(dāng)時(shí)

（5）

當(dāng)時(shí)

（6）

式中：為采樣時(shí)間間隔。

1.2.3" 反光柱映射

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

該方法相比三邊定位方法，對(duì)反光柱的幾何參數(shù)要求更低，具有較高的容錯(cuò)能力和動(dòng)態(tài)抗擾能力。

1.2.4" 權(quán)重計(jì)算

根據(jù)反光柱的對(duì)應(yīng)關(guān)系，以為均值，以方差、構(gòu)造二元高斯分布，計(jì)算在該分布中的概率密度值，并作為權(quán)重：

式中：。

則第0個(gè)粒子的權(quán)重為

（7）

同理可計(jì)算其他粒子的權(quán)重，權(quán)重最高的粒子代表了機(jī)器人的最佳后驗(yàn)位姿估計(jì)。

1.2.5" 重采樣

以后驗(yàn)位姿估計(jì)為均值，動(dòng)態(tài)方差構(gòu)造高斯分布，再?gòu)脑摲植贾羞M(jìn)行粒子重采樣，為下一次迭代提供新的粒子。采用動(dòng)態(tài)方差策略可兼顧機(jī)器人定位的魯棒性與精度，其原理是當(dāng)所有粒子的總權(quán)重低于一定水平，則放大方差，增加粒子的多樣性，以便更有機(jī)會(huì)命中真值附近的位姿，進(jìn)而提高這些粒子的權(quán)重。隨著粒子權(quán)重的逐步提高，動(dòng)態(tài)方差也隨之減小，粒子的多樣性也減小，所有粒子收斂到真值附近，并穩(wěn)定在一定的范圍內(nèi)。

由于所有粒子的總權(quán)重與粒子總數(shù)相關(guān)，根據(jù)工程經(jīng)驗(yàn)，取，記，收斂時(shí)的最小標(biāo)準(zhǔn)差為，最大標(biāo)準(zhǔn)差為。

當(dāng)時(shí)，表示定位不收斂，則

（8）

當(dāng)時(shí)，表示定位已收斂，則

（9）

2 融合定位

實(shí)際工程應(yīng)用具有復(fù)雜性，如場(chǎng)景中部分區(qū)域無(wú)法部署反光柱，導(dǎo)致粒子濾波定位無(wú)法持續(xù)對(duì)其進(jìn)行觀測(cè)。若僅通過(guò)航跡推算進(jìn)行定位，存在較大的累積誤差；若僅通過(guò)SLAM進(jìn)行定位，因作業(yè)環(huán)境易變，其精度下降甚至失效。

卡爾曼濾波[8]（Kalman filter， KF）是一種閉式計(jì)算，有解析解，具有算力要求低、實(shí)時(shí)性好等特點(diǎn)。但該方法基于2個(gè)前提假設(shè)：1） 狀態(tài)轉(zhuǎn)移和狀態(tài)觀測(cè)都是線性模型；2） 系統(tǒng)的相關(guān)噪聲都是高斯白噪聲。恒定線速度與角速度的運(yùn)動(dòng)軌跡是圓，并不是線性的，需要對(duì)其進(jìn)行線性化，即擴(kuò)展卡爾曼濾波[9]（extended Kalman filter， EKF）。

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

設(shè)系統(tǒng)狀態(tài)為，表示機(jī)器人在地圖坐標(biāo)系下的位姿；控制輸入為，表示機(jī)器人本體的線速度和角速度。

1） 通過(guò)初始定位位姿及初始協(xié)方差對(duì)系統(tǒng)狀態(tài)進(jìn)行初始化；

2） 設(shè)系統(tǒng)狀態(tài)轉(zhuǎn)移方程為

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

定義為先驗(yàn)估計(jì)，為后驗(yàn)估計(jì)，則狀態(tài)預(yù)測(cè)表示為，即通過(guò)上一時(shí)刻的后驗(yàn)位姿估計(jì)和速度信息計(jì)算當(dāng)前時(shí)刻的先驗(yàn)估計(jì)具體如下：

當(dāng)時(shí)

（10）

當(dāng)時(shí)

（11）

機(jī)器人在運(yùn)行過(guò)程中可能存在打滑等異常情況，很難對(duì)類似的未知情況進(jìn)行建模，則估計(jì)協(xié)方表示為

（12）

3） 由于觀測(cè)量與狀態(tài)量一致，故觀測(cè)矩陣。當(dāng)濾波器接收到反光柱定位信息時(shí)，，；當(dāng)接收到SLAM定位信息時(shí)，，，將參數(shù)代入公式（13），可依次求解出當(dāng)前時(shí)刻的卡爾曼增益、當(dāng)前時(shí)刻的狀態(tài)估計(jì)、當(dāng)前狀態(tài)估計(jì)的協(xié)方差矩陣。

（13）

上述的形式有別于，稱為Joseph form，由于某些極端情況下，可能因?yàn)楦↑c(diǎn)運(yùn)算誤差變?yōu)榉菍?duì)稱矩陣，使結(jié)果出現(xiàn)較大偏差[10]。

4） 在濾波器執(zhí)行過(guò)程中，當(dāng)部分環(huán)境特征發(fā)生變化時(shí)，可能使變大，系統(tǒng)更傾向于預(yù)測(cè)值和反光柱的定位更新；當(dāng)機(jī)器人經(jīng)過(guò)沒(méi)有反光柱的區(qū)域時(shí)，系統(tǒng)仍可以依靠預(yù)測(cè)值和SLAM定位更新進(jìn)行導(dǎo)航。

3 試驗(yàn)

針對(duì)本文提出的基于反光柱的融合定位方法，對(duì)搭載了單線激光雷達(dá)的對(duì)角舵輪的全向移動(dòng)機(jī)器人進(jìn)行模擬試驗(yàn)，試驗(yàn)參數(shù)如表1所示。

在完成反光柱標(biāo)定以及柵格地圖構(gòu)建后，將全向移動(dòng)機(jī)器人置于地圖場(chǎng)景中運(yùn)行。

1） 平滑運(yùn)行試驗(yàn)。設(shè)計(jì)兩個(gè)作業(yè)區(qū)域，同時(shí)在這兩個(gè)作業(yè)區(qū)域及其連通路徑上構(gòu)建柵格地圖，并只在兩個(gè)作業(yè)區(qū)域部署反光柱。經(jīng)測(cè)試，全向移動(dòng)機(jī)器人可順利在兩個(gè)作業(yè)區(qū)域之間穩(wěn)定運(yùn)行，且由于反光柱的作用，定位精度較高（詳見精度試驗(yàn)）。

2） 抗干擾能力試驗(yàn)。通過(guò)人為給定錯(cuò)誤的狀態(tài)來(lái)模擬異常情況。經(jīng)測(cè)試，該融合定位方法具有較強(qiáng)的糾偏能力，其位置偏差容錯(cuò)范圍為一個(gè)半徑大于2 m的圓，姿態(tài)偏差容錯(cuò)范圍約為?？垢蓴_能力試驗(yàn)糾正過(guò)程如圖3所示。

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

3） 精度試驗(yàn)。在機(jī)器人能穩(wěn)定觀測(cè)到一定數(shù)量的反光柱時(shí)，采集500組定位數(shù)據(jù)，其位姿分布近似為一個(gè)多元高斯分布（如圖4所示）；機(jī)器人定位具有較小的標(biāo)準(zhǔn)差和偏差范圍（如表2所示），表明該融合定位方法定位精度較高。

4 結(jié)論

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

參考文獻(xiàn)

[1] 王楚揚(yáng). GNSS多頻松/緊組合RTK定位算法研究[D].阜新：遼寧工程技術(shù)大學(xué)，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和高精度定位系統(tǒng)[J].現(xiàn)代計(jì)算機(jī)，2020（11）：3-7.

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

[5] 吳波.基于激光雷達(dá)和反光板的移動(dòng)機(jī)器人定位方法研究[D].武漢：華中科技大學(xué)，2019.

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

[7] Sebastian Thrun， Wolfram Burgard， Dieter Fox.概率機(jī)器人[M].曹紅玉，譚志，史曉霞，等，譯.北京：機(jī)械工業(yè)出版社，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.

作者簡(jiǎn)介：

黃小春，男，1987年生，本科，高級(jí)工程師，主要研究方向：機(jī)器人導(dǎo)航相關(guān)技術(shù)。E-mail： syntax@dingtalk.com