張 彥，陳學(xué)京，肖獻(xiàn)強(qiáng)，王家恩

（合肥工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽 合肥 230009）

1 引言

隨著移動(dòng)機(jī)器人技術(shù)的發(fā)展，移動(dòng)機(jī)器人已廣泛應(yīng)用于工業(yè)生產(chǎn)和日常生活中的多個(gè)領(lǐng)域。移動(dòng)機(jī)器人擁有路徑規(guī)劃、實(shí)時(shí)定位、自主導(dǎo)航等能力［1-2］，就其中的實(shí)時(shí)定位能力，一些學(xué)者進(jìn)行了深入研究。2017年文獻(xiàn)［3］提出針對(duì)傳統(tǒng)蒙特卡洛定位算法在全局定位存在的粒子收斂速度緩慢、粒子貧乏等問題，提出了一種改進(jìn)的蒙特卡洛定位算法，運(yùn)動(dòng)過程中通過擴(kuò)展卡爾曼濾波估計(jì)位姿，同時(shí)引入重采樣過程，提高粒子的收斂速度以及定位的穩(wěn)定性［4］。文獻(xiàn)［5］利用深度卷積神經(jīng)網(wǎng)絡(luò)對(duì)激光雷達(dá)的數(shù)據(jù)進(jìn)行融合，提高了自動(dòng)引導(dǎo)小車的室內(nèi)定位精度。2019年文獻(xiàn)［6］提出了一種基于LSTM網(wǎng)絡(luò)的倉儲(chǔ)移動(dòng)機(jī)器人實(shí)時(shí)定位方法，通過對(duì)激光雷達(dá)數(shù)據(jù)和地圖的預(yù)處理、模型訓(xùn)練與應(yīng)用，實(shí)現(xiàn)了倉庫環(huán)境下的移動(dòng)機(jī)器人的實(shí)時(shí)定位［7］。然而，上述這些方法在實(shí)際使用過程中往往存在著定位精度低、不穩(wěn)定性以及操作復(fù)雜等問題。

針對(duì)這些問題，這里采用了激光雷達(dá)數(shù)據(jù)和地圖數(shù)據(jù)匹配的方法，通過局部地圖中的特征線段和全局地圖中的線段進(jìn)行線段匹配進(jìn)行初始定位，利用慣性導(dǎo)航推算位姿結(jié)合地圖匹配的方法進(jìn)行位姿修正，同時(shí)采用動(dòng)態(tài)重定位的方法進(jìn)行重定位，提高了移動(dòng)機(jī)器人運(yùn)動(dòng)的可靠性以及對(duì)環(huán)境的適應(yīng)性。實(shí)驗(yàn)表明此方法可以有效地提高定位的精度、實(shí)時(shí)性以及穩(wěn)定性。

2 導(dǎo)航方法技術(shù)框架

根據(jù)測試場地的實(shí)際布局，通過地圖設(shè)置站點(diǎn)，同時(shí)每個(gè)站點(diǎn)對(duì)應(yīng)著全局空間實(shí)際位姿。導(dǎo)航定位方法框架，如圖1所示。移動(dòng)機(jī)器人在起始位姿通過初始定位方法獲得全局位姿，在行駛過程中，移動(dòng)機(jī)器人上的慣性傳感器以及編碼器可以獲得移動(dòng)機(jī)器人的橫擺角速度以及左右輪轉(zhuǎn)速，同時(shí)激光雷達(dá)傳感器亦可采集環(huán)境的實(shí)時(shí)點(diǎn)云數(shù)據(jù)。

圖1 導(dǎo)航技術(shù)框架Fig.1 Navigation Technology Framework

通過獲得的橫擺角速度和左右輪轉(zhuǎn)速，以慣性導(dǎo)航推算的位姿為基礎(chǔ)，并結(jié)合獲得的點(diǎn)云數(shù)據(jù)，以及地圖匹配方法，可計(jì)算出移動(dòng)機(jī)器人的實(shí)時(shí)位姿。偏差計(jì)算模塊實(shí)時(shí)計(jì)算移動(dòng)機(jī)器人當(dāng)前位姿相對(duì)目標(biāo)站點(diǎn)的縱側(cè)向偏差以及方向偏差，并將其輸入控制模塊，進(jìn)行速度和橫擺的控制，從而實(shí)現(xiàn)移動(dòng)機(jī)器人的運(yùn)動(dòng)控制［8］。

3 初始定位

3.1 初始定位流程

移動(dòng)機(jī)器人在測試場地某一位置處于靜止?fàn)顟B(tài)進(jìn)行初始定位，目的是獲得移動(dòng)機(jī)器人當(dāng)前的全局位姿。根據(jù)局部地圖的某一條線段尋找在全局地圖中與其對(duì)應(yīng)的線段，經(jīng)過線段匹配，獲得激光雷達(dá)在全局地圖中的位姿，進(jìn)而獲得機(jī)器人的全局位姿。具體步驟如下：

（1）采集一幀激光雷達(dá)數(shù)據(jù)，提取特征線段，形成由特征線段構(gòu)成的局部地圖；

（2）對(duì)全局地圖經(jīng)過OPENCV處理后，獲得地圖中的線段；

（3）將局部地圖中的某一條線段和地圖中的線段進(jìn)行線段匹配，計(jì)算激光雷達(dá)在全局地圖中的位姿；

（4）依據(jù)步驟（3）獲得的激光雷達(dá)位姿將一幀激光雷達(dá)點(diǎn)云數(shù)據(jù)映射到全局地圖中，計(jì)算匹配度，從而可以確定該位姿的準(zhǔn)確性；

（5）若不符合設(shè)定的匹配度，則回到步驟（3），計(jì)算其他重合的直線，直至找到符合條件的位姿；

（6）基于激光雷達(dá)在全局地圖中的位姿，計(jì)算機(jī)器人在全局地圖中的位姿。

3.2 特征線段提取

針對(duì)環(huán)境中存在直線特征，例如門、窗和貨架等，對(duì)獲取的一幀激光雷達(dá)點(diǎn)云數(shù)據(jù)集，首先利用自適應(yīng)閾值法進(jìn)行斷點(diǎn)提取，再基于迭代適應(yīng)點(diǎn)算法提取拐點(diǎn)，依據(jù)拐點(diǎn)分割點(diǎn)集；最后采用最小二乘法擬合直線并依據(jù)直線度的要求，進(jìn)一步提高特征提取的精度。

3.2.1 斷點(diǎn)提取

斷點(diǎn)指環(huán)境中特征的邊緣，表現(xiàn)在連續(xù)的掃描點(diǎn)中數(shù)據(jù)特征出現(xiàn)突變的情況［9］。這里采用MORAVEC［10］提出的一種自適應(yīng)閾值算法用于斷點(diǎn)檢測。

自適應(yīng)閾值法原理，如圖2所示。

圖中：Pn和Pn-1—相鄰激光雷達(dá)數(shù)據(jù)點(diǎn)；

rn-1—Pn-1對(duì)應(yīng)的距離；

假設(shè)rn—Pn對(duì)應(yīng)的最大距離；

φn-1—Pn-1對(duì)應(yīng)的角度；

φn—Pn對(duì)應(yīng)的角度；

α—直線l與rn-1的夾角。

在三角形ABC以及三角APnO形中，由正弦定理可得：

由式（1）～式（2）可得：

考慮極限情況，當(dāng)Pn和點(diǎn)C重合時(shí)，則在三角形AOC中，由正弦定理可得：

則由式（3）、式（4）可得：

式中：Rn—兩點(diǎn)間的最大距離。

考慮激光雷達(dá)測距的誤差等情況，因此修改斷點(diǎn)判定閾值，即：

式中：δ—激光雷達(dá)的測量誤差。

在實(shí)際的測試時(shí)，α取100、δ取10mm 時(shí)，斷點(diǎn)的檢測效果最好。同時(shí)如果兩個(gè)點(diǎn)之間的角度差值在2°以上也可作為斷點(diǎn)。對(duì)一幀激光雷達(dá)的數(shù)據(jù)進(jìn)行斷點(diǎn)檢測，斷點(diǎn)檢測結(jié)果，如圖3所示。

圖3 斷點(diǎn)檢測結(jié)果圖Fig.3 Breakpoint Detection Result Chart

3.2.2 特征直線獲取

因采用自適應(yīng)閾值法本身的缺陷，斷點(diǎn)提取后仍不能完整體現(xiàn)出環(huán)境中的特征，因此繼續(xù)采用迭代適應(yīng)點(diǎn)算法（IEPF）［11］提取數(shù)據(jù)點(diǎn)中的拐點(diǎn)。IEPF算法原理，如圖4所示。IEPF方法采用遞歸的思想，連接點(diǎn)P1和P4得到線段Lp1p4，的距離，獲得距離最大值Tmax，判斷是否超過設(shè)定的點(diǎn)到直線距離的閾值λ，若Tmax超過λ，則找到對(duì)應(yīng)的數(shù)據(jù)點(diǎn)P2，即為拐點(diǎn)，則將分成線段Lp1p2和線段Lp2p4。若Tmax未超過λ，說明該點(diǎn)集內(nèi)沒有其他的拐點(diǎn)。當(dāng)Tmax=100mm時(shí)提取的拐點(diǎn)結(jié)果，如圖5所示。

圖4 IEPF算法原理圖Fig.4 Schematic Diagram of IEPF Algorithm

圖5 提取拐點(diǎn)結(jié)果圖Fig.5 Extract Inflection Point Result Graph

通過拐點(diǎn)進(jìn)一步分割數(shù)據(jù)集合獲得相應(yīng)點(diǎn)集，通過最小二乘法擬合特征直線，同時(shí)進(jìn)行直線度檢測，剔除其中到擬合直線的距離超過閾值的點(diǎn)，再重新擬合，以獲得較高的擬合直線準(zhǔn)確度，提高特征提取的精度，提取的特征線段結(jié)果，如圖6所示。

圖6 提取特征線段結(jié)果圖Fig.6 Extracting Feature Line Segment Result Graph

3.3 線段匹配

假設(shè)全局地圖中的某一個(gè)線段和局部地圖中的線段是同一條線段，即可通過線段匹配計(jì)算出激光雷達(dá)在全局地圖坐標(biāo)系下的位姿。線段匹配原理，如圖7所示。

圖7 線段匹配原理圖Fig.7 Schematic Diagram of Line Segment Matching

圖中：AB—全局地圖坐標(biāo)系OMXMYM中某一線段；mn—雷達(dá)坐標(biāo)系ORXRYR中某一線段。

假設(shè)AB和mn為同一條線段，實(shí)際計(jì)算中線段端點(diǎn)B和線段端點(diǎn)n不一定重合，假設(shè)n和T重合，則通過式（7）可以求得T在OMXMYM坐標(biāo)系下的位置（XT，YT）。

式中：l—線段mn的長度；

L—線段AB的長度。

根據(jù)圖7中的幾何關(guān)系，可知激光雷達(dá)坐標(biāo)系在全局地圖坐標(biāo)系下的位姿（xe，ye，φe）。

式中：（Xm，Ym）—m點(diǎn)在雷達(dá)坐標(biāo)系下的坐標(biāo)；（Xn，Yn）—n點(diǎn)在雷達(dá)坐標(biāo)系下的坐標(biāo)；（XA，YA）—A點(diǎn)在全局地圖坐標(biāo)系下的坐標(biāo)；（XT，YT）—T點(diǎn)在全局地圖坐標(biāo)系下的坐標(biāo)。

則由式（9），即可計(jì)算得到激光雷達(dá)在全局地圖坐標(biāo)系下的位姿。

其中，P=XA-XQ，Q=YA-YQ，N=Xm-Xn，M=Ym-Yn。

4 動(dòng)態(tài)定位

4.1 慣性導(dǎo)航位姿推算

在一個(gè)采樣周期內(nèi)，通過安裝在移動(dòng)機(jī)器人慣性傳感器采集的橫擺角速度以及通過電機(jī)編碼器反饋的左右輪轉(zhuǎn)速，計(jì)算在k時(shí)刻移動(dòng)機(jī)器人質(zhì)心的速度vk和橫擺wk，基于移動(dòng)機(jī)器人k-1時(shí)刻的位姿（xk-1，yk-1，θk-1），對(duì)移動(dòng)機(jī)器人基于慣性導(dǎo)航的位姿推算，從而獲得移動(dòng)機(jī)器人在k時(shí)刻位姿，如式（10）所示。

式中：ΔT—系統(tǒng)采樣時(shí)間；

vk—移動(dòng)機(jī)器人質(zhì)心速度；

nl—左輪轉(zhuǎn)速；

nr—右輪轉(zhuǎn)速；

d—差速驅(qū)動(dòng)輪的直徑；

（xk，yk，θk）—移動(dòng)機(jī)器人的橫坐標(biāo)、縱坐標(biāo)、方向角。

4.2 地圖匹配的位姿計(jì)算

這里所采用的導(dǎo)航定位方法，如圖8所示。以T-1時(shí)刻位姿為基礎(chǔ)經(jīng)過慣性導(dǎo)航位姿推算獲得慣性導(dǎo)航位姿。因獲取一幀完整激光雷達(dá)點(diǎn)云數(shù)據(jù)需要一定的時(shí)間，所以在未檢測到激光數(shù)據(jù)的情況下，就以慣性導(dǎo)航位姿作為T時(shí)刻位姿；若檢測到激光數(shù)據(jù)，則以慣性導(dǎo)航位姿為基礎(chǔ)劃分一定區(qū)域結(jié)合激光雷達(dá)數(shù)據(jù)進(jìn)行地圖匹配獲得地圖匹配位姿及其對(duì)應(yīng)的重合度，從而找出最高重合度，若重合度符合閾值條件，則對(duì)應(yīng)的匹配位姿即為T時(shí)刻位姿；若重合度不符合閾值條件，則以慣性導(dǎo)航位姿作為T時(shí)刻位姿。

圖8 導(dǎo)航定位方法框圖Fig.8 Navigation and Positioning Method Block Diagram

將激光雷達(dá)末端點(diǎn)坐標(biāo)由激光雷達(dá)坐標(biāo)系轉(zhuǎn)化到全局坐標(biāo)系下，為了提高計(jì)算速度，首先計(jì)算一定變化角度δ下的激光雷達(dá)末端點(diǎn)坐標(biāo)，再將激光雷達(dá)末端點(diǎn)橫、縱坐標(biāo)在一定范圍內(nèi)移動(dòng)，從而計(jì)算出在激光雷達(dá)在全局地圖坐標(biāo)系下的位姿一定變化角度和一定范圍內(nèi)對(duì)應(yīng)的激光雷達(dá)末端點(diǎn)在全局地圖坐標(biāo)系下的坐標(biāo)，如式（11）、式（12）所示。

式中：（Xr，Yr，θr）—激光雷達(dá)在全局地圖坐標(biāo)系下的位姿；（xr，yr）—激光雷達(dá)末端點(diǎn)在激光雷達(dá)坐標(biāo)系下的橫、縱坐標(biāo)；（xm，ym）—激光雷達(dá)末端點(diǎn)在全局地圖坐標(biāo)系下的橫、縱坐標(biāo)；δ—角度變化值。

式中：（xc，yc）—激光雷達(dá)末端點(diǎn)在全局地圖坐標(biāo)系下的橫、縱坐標(biāo)；Δx—橫坐標(biāo)變化值；Δy—縱坐標(biāo)變化值。

將獲得的激光雷達(dá)末端點(diǎn)映射到地圖中，判斷落在地圖中是否是障礙點(diǎn)，從而計(jì)算該激光雷達(dá)位姿對(duì)應(yīng)的可信度，即衡量激光雷達(dá)末端點(diǎn)落在地圖中障礙物上的數(shù)量；再將激光雷達(dá)在全局坐標(biāo)系下的位姿角度和橫、縱坐標(biāo)在一定范圍內(nèi)移動(dòng)，尋找可信度最高的位姿；再將激光雷達(dá)在全局地圖坐標(biāo)系下的位姿轉(zhuǎn)化到移動(dòng)機(jī)器人在全局地圖坐標(biāo)系下的位姿，如式（13）所示。從而作為地圖匹配的移動(dòng)機(jī)器人位姿。

式中：（Xr，Yr，θr）—激光雷達(dá)在全局地圖坐標(biāo)系下的位姿；（Xc，Yc，θc）—機(jī)器人在全局地圖坐標(biāo)系下的位姿；（xp，yp）—激光雷達(dá)和機(jī)器人質(zhì)心的橫向距離和縱向距離。

4.3 動(dòng)態(tài)重定位

移動(dòng)機(jī)器人在行駛過程中，若因環(huán)境變化導(dǎo)致用于地圖匹配的特征數(shù)據(jù)點(diǎn)過少，達(dá)不到地圖匹配的重合度閾值，超過規(guī)定時(shí)間150ms后，移動(dòng)機(jī)器人就會(huì)減速停車。此時(shí)，仍采集激光雷達(dá)數(shù)據(jù)，并以慣性導(dǎo)航的位姿為基礎(chǔ)去計(jì)算移動(dòng)機(jī)器人的實(shí)時(shí)位姿。但是，因慣性導(dǎo)航計(jì)算期間存在累積誤差，因此，在進(jìn)行重定位過程中就需要擴(kuò)大地圖匹配的范圍。在實(shí)際測試中，為確保定位的準(zhǔn)確性和實(shí)時(shí)性，選擇角度范圍為當(dāng)前角度±5°，橫縱坐標(biāo)范圍為當(dāng)前橫、縱坐標(biāo)各±100mm，由式（11）～式（13）再次計(jì)算擴(kuò)大范圍后的激光雷達(dá)位姿，即可重新獲得此刻機(jī)器人的位姿，從而提高移動(dòng)機(jī)器人對(duì)工作環(huán)境的適應(yīng)能力和可靠性。

5 移動(dòng)機(jī)器人設(shè)計(jì)與系統(tǒng)測試

5.1 移動(dòng)機(jī)器人運(yùn)動(dòng)模型與運(yùn)動(dòng)控制

這里設(shè)計(jì)的移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，如圖9所示。圖中（XR，YR，θR）為移動(dòng)機(jī)器人在全局坐標(biāo)系下的位姿，采用差速驅(qū)動(dòng)的方式，其包括兩個(gè)驅(qū)動(dòng)輪和四個(gè)萬向輪，安裝相應(yīng)的傳感器，可以實(shí)現(xiàn)相應(yīng)的運(yùn)動(dòng)與控制。

圖9 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型示意圖Fig.9 Diagram Kinematic Model of Mobile Robot

根據(jù)移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型，結(jié)合參考文獻(xiàn)［12］提出的控制方法，計(jì)算質(zhì)心速度vc以及期望橫擺角速度wc［12］，再計(jì)算得到左輪控制轉(zhuǎn)速nl和右輪控制轉(zhuǎn)速nr，下發(fā)到驅(qū)動(dòng)模塊中的電機(jī)，從而實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)控制。

5.2 實(shí)驗(yàn)設(shè)備與參數(shù)

實(shí)驗(yàn)設(shè)備主要是差速驅(qū)動(dòng)的實(shí)驗(yàn)平臺(tái)以及激光雷達(dá)。移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)實(shí)物，如圖10所示。移動(dòng)機(jī)器人的基本參數(shù)，如表1所示。激光雷達(dá)采用萬集公司的WRJ-716型激光雷達(dá)，其掃描角度為270°，角度分辨率為0.33°，掃描頻率為15Hz。

表1 移動(dòng)機(jī)器人基本參數(shù)Tab.1 Basic Parameters of Mobile Robot

5.3 路徑測試及分析

Z字形路徑測試在直線加原地轉(zhuǎn)彎路徑行駛時(shí)的路徑跟蹤性能和精度，八邊形路徑測試在直線加曲線路徑行駛時(shí)的路徑跟蹤性能和精度，上述兩種路徑可以組合構(gòu)成移動(dòng)機(jī)器人在實(shí)際應(yīng)用過程中的各種復(fù)雜路徑，因此，路徑測試往往采用Z字形路徑和八邊形路徑，并進(jìn)行重復(fù)測試。Z字型路徑示意圖，如圖11所示。移動(dòng)機(jī)器人以當(dāng)前位姿為起始站點(diǎn)，并在起始站點(diǎn)進(jìn)行初始定位，按箭頭的指示方向跟蹤軌跡，經(jīng)過六個(gè)站點(diǎn)到達(dá)目標(biāo)站點(diǎn)。八邊形路徑示意圖，如圖12所示。移動(dòng)機(jī)器人以當(dāng)前位姿為起始站點(diǎn)，并在起始站點(diǎn)進(jìn)行初始定位，按箭頭的指示方向跟蹤軌跡，經(jīng)過八個(gè)站點(diǎn)到達(dá)目標(biāo)站點(diǎn)，即回到原點(diǎn)。

圖11 Z字形規(guī)劃路徑示意圖Fig.11 Schematic Diagram of Z-Shaped Planning Path

圖12 八邊形規(guī)劃路徑示意圖Fig.12 Schematic Diagram of Octagonal Planning Path

移動(dòng)機(jī)器人從起始站點(diǎn)到目標(biāo)站點(diǎn)的路徑上的部分站點(diǎn)的數(shù)據(jù)與地圖標(biāo)定的站點(diǎn)坐標(biāo)數(shù)據(jù)進(jìn)行比較，所得行駛到站點(diǎn)的精度偏差。路徑站點(diǎn)位姿定位精度結(jié)果，如表2所示。表2中最后一個(gè)站點(diǎn)位姿經(jīng)過100次重復(fù)測量后的位置和方向角的變化結(jié)果，如圖13所示。實(shí)驗(yàn)結(jié)果表明，通過基于地圖匹配的移動(dòng)機(jī)器人的定位方法，該移動(dòng)機(jī)器人可以實(shí)現(xiàn)原地轉(zhuǎn)彎、直線與曲線行駛的功能，路徑跟蹤控制精度偏差被限制在±40mm 以內(nèi)?？紤]到激光雷達(dá)傳感器的測距存在±20mm的誤差以及采用分辨率為30mm柵格地圖，±40mm的精度偏差結(jié)果是合理的。上述實(shí)際測試結(jié)果表明，該種移動(dòng)機(jī)器人的定位方法具有可行性與可靠性。

表2 路徑站點(diǎn)位姿定位精度Tab.2 Pose Positioning Accuracy of Path Station

圖13 位置和方向角變化圖Fig.13 Position and Bearing Variation Diagram

6 結(jié)論

這里采用基于地圖匹配的移動(dòng)機(jī)器人定位的方法，將局部地圖中的特征線段和全局地圖中的線段通過線段匹配的方法進(jìn)行初始定位，再以慣性導(dǎo)航進(jìn)行位姿估計(jì)，并基于地圖匹配的方法進(jìn)行動(dòng)態(tài)定位，計(jì)算出移動(dòng)機(jī)器人的實(shí)時(shí)位姿，然后結(jié)合相應(yīng)的控制方法進(jìn)行運(yùn)動(dòng)控制。

最后，通過實(shí)驗(yàn)的測試，得到了該種方法下移動(dòng)機(jī)器人的位置精度偏差在±40mm以內(nèi)，表明了該定位方法具有很高的可行性與穩(wěn)定性。后期通過選用測量精度更高的激光雷達(dá)以及優(yōu)化構(gòu)建地圖的方法，可以使移動(dòng)機(jī)器人的位置精度偏差進(jìn)一步降低。