郭 麗，陳孟元，付 明

一種多源數(shù)據(jù)融合的機(jī)器人導(dǎo)航技術(shù)

郭 麗1，陳孟元2，付 明3

（1. 安徽電子信息職業(yè)技術(shù)學(xué)院，安徽 蚌埠 233030；2. 安徽工程大學(xué)，安徽 蕪湖 241000；3. 安徽財(cái)經(jīng)大學(xué)，安徽 蚌埠 233030）

針對移動(dòng)機(jī)器人在導(dǎo)航過程中存在的建圖不精確、效率低下等問題，提出一種多源數(shù)據(jù)融合的移動(dòng)機(jī)器人導(dǎo)航方法：利用擴(kuò)展卡爾曼濾波融合激光雷達(dá)和深度相機(jī)的環(huán)境信息，通過位姿傳感器獲取機(jī)器人的位姿和加速度信息，提出基于激光雷達(dá)、深度相機(jī)和慣性測量單元數(shù)據(jù)融合的地圖構(gòu)建方法；并兼顧全局探測與局部求精的平衡，采用改進(jìn)原子軌道搜索算法進(jìn)行全局規(guī)劃，利用動(dòng)態(tài)窗口法完成局部避障。實(shí)驗(yàn)結(jié)果表明，多源數(shù)據(jù)融合建立的地圖更接近于真實(shí)場景，改進(jìn)后的融合算法未知障礙實(shí)時(shí)避障成功率達(dá)到98%，能夠提升機(jī)器人自主導(dǎo)航效率。

數(shù)據(jù)融合；原子軌道搜索算法；動(dòng)態(tài)窗口；移動(dòng)機(jī)器人；導(dǎo)航

0 引言

隨著計(jì)算機(jī)系統(tǒng)、傳感器等技術(shù)的發(fā)展，移動(dòng)機(jī)器人已廣泛應(yīng)用于倉儲(chǔ)、工業(yè)、醫(yī)療等領(lǐng)域[1-3]。導(dǎo)航技術(shù)作為機(jī)器人領(lǐng)域內(nèi)核心技術(shù)之一，主要分為定位、環(huán)境搭建及路徑規(guī)劃。

傳統(tǒng)即時(shí)定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）方法僅僅依賴單一傳感器進(jìn)行實(shí)時(shí)定位和建圖，導(dǎo)致系統(tǒng)魯棒性差，導(dǎo)航效率低。多種傳感器數(shù)據(jù)融合可以為移動(dòng)機(jī)器人定位建圖提供更好的數(shù)據(jù)支持，彌補(bǔ)單傳感器檢測的缺陷，從而提高導(dǎo)航系統(tǒng)的準(zhǔn)確性和魯棒性。在傳感器數(shù)據(jù)融合方面，濾波算法通常用于數(shù)據(jù)融合[4]。常見的濾波方法有貝葉斯濾波[5]、粒子濾波[6]、擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）[7]等。

路徑規(guī)劃是指基于多源感知數(shù)據(jù)，在環(huán)境中生成無碰撞路徑，以保證機(jī)器人安全高效完成作業(yè)。在路徑規(guī)劃方法中，二維幾何搜索法是最成熟的路徑規(guī)劃方法。此外，還有一些廣泛使用的算法，如蟻群算法[8]、A*算法[9]、RRT算法[10]、人工勢場法[11]等。原子軌道搜索算法（atomic orbital search，AOS）是Azizi M提出的最新的智能算法之一[12]。該算法主要是基于量子力學(xué)及原子模型行為進(jìn)行搜索獲取最優(yōu)解，具有尋優(yōu)能力強(qiáng)、收斂速度快等優(yōu)勢，可較好地應(yīng)用于路徑規(guī)劃；但其同時(shí)也存在著易陷入局部最優(yōu)等問題。

基于目前研究的成果，為改善機(jī)器人依靠單一傳感器定位及建圖效果差等問題，同時(shí)提升路徑規(guī)劃算法效率，設(shè)計(jì)出一種基于多源數(shù)據(jù)融合的移動(dòng)機(jī)器人導(dǎo)航方法：一方面，搭建一種基于激光雷達(dá)（light detection and ranging，LiDAR）、深度相機(jī)和慣性測量單元（inertial measurement unit，IMU）融合的實(shí)時(shí)定位建圖方法；另一方面，融合改進(jìn)AOS算法及動(dòng)態(tài)窗口法（dynamic window approach，DWA）進(jìn)行路徑規(guī)劃，實(shí)現(xiàn)移動(dòng)機(jī)器人的實(shí)時(shí)高效導(dǎo)航。

1 傳感器建模

1.1 LiDAR模型

LiDAR數(shù)學(xué)模型主要有光束模型和似然場模型。因?yàn)闄C(jī)器人所處環(huán)境較為復(fù)雜，位姿微小變動(dòng)就導(dǎo)致期望值發(fā)生巨大改變，對光束模型影響較大；因此實(shí)際場景中主要采用似然場模型。似然場模型基本原理是對環(huán)境進(jìn)行高斯模糊，如圖1所示。設(shè)表示目標(biāo)點(diǎn)坐標(biāo)與環(huán)境中障礙物之間的最小距離，那么LiDAR觀測模型可以表示為

式中：m為地圖；ε為高斯分布函數(shù)；Δd為運(yùn)動(dòng)過程中畸變的測量距離。

1.2 深度相機(jī)模型

本文采用KinectV2深度相機(jī)進(jìn)行研究，對其成像過程進(jìn)行建模。假設(shè)實(shí)際環(huán)境中一點(diǎn)通過小孔在成像平面為,表示在相機(jī)坐標(biāo)系---下的小孔，成像平面坐標(biāo)系?-?-?中心到點(diǎn)的距離為。此時(shí)的坐標(biāo)為[,,]T，經(jīng)過成像后點(diǎn)?的坐標(biāo)為[,,]T，在真實(shí)情況下，經(jīng)過小孔模型后的成像需要進(jìn)一步轉(zhuǎn)換成像素點(diǎn)。假設(shè)像素平面坐標(biāo)系上?點(diǎn)的坐標(biāo)為[,]T, 則其與像素坐標(biāo)系間的轉(zhuǎn)換表達(dá)式為

式中：為相機(jī)內(nèi)部參數(shù)矩陣；當(dāng)相機(jī)移動(dòng)時(shí)，為當(dāng)前相機(jī)坐標(biāo)系下位姿；w為世界坐標(biāo)系下位姿；和為相機(jī)的外參數(shù)；、分別為像素平面橫坐標(biāo)及縱坐標(biāo)；為笛卡爾坐標(biāo)系軸的坐標(biāo)。

深度相機(jī)通過飛行時(shí)間法來測量像素的距離：相機(jī)朝環(huán)境各物體發(fā)出脈沖，然后得到從發(fā)射到接收所耗時(shí)間，以此確定物體與當(dāng)前位置的距離。其具體成像模型如圖2所示。圖中：pixel為像素坐標(biāo)；p、p為像素坐標(biāo)系中橫坐標(biāo)及縱坐標(biāo)；film為圖像坐標(biāo)；f、f為圖像坐標(biāo)系中橫坐標(biāo)及縱坐標(biāo)；camera為相機(jī)坐標(biāo)；c、c、為圖像坐標(biāo)系中橫坐標(biāo)、縱坐標(biāo)及高度坐標(biāo)；world為相機(jī)坐標(biāo)；w、w、w為真實(shí)世界坐標(biāo)系中橫坐標(biāo)、縱坐標(biāo)及高度坐標(biāo)。

在日常場景中會(huì)在相機(jī)前放透鏡來提升拍攝質(zhì)量，這同時(shí)會(huì)導(dǎo)致成像與真實(shí)場景出現(xiàn)偏差。假設(shè)平面上一點(diǎn)的坐標(biāo)為[,]T，對應(yīng)的極坐標(biāo)為[,]T，其中為極徑，為極角。透鏡造成的畸變?yōu)閺较蚧?，表示?dāng)前坐標(biāo)朝著方向偏移。將相機(jī)透鏡安裝過程中存在的誤差叫做切向畸變，指在角度上存在的偏移?？梢酝ㄟ^不同的畸變參數(shù)對徑向畸變和切向畸變進(jìn)行矯正，具體表達(dá)式為

圖2 深度相機(jī)模型

式中：[,]T為矯正前坐標(biāo)；[,]T為矯正后坐標(biāo)；徑向畸變矯正參數(shù)為1和2，利用參數(shù)1矯正圖像中心畸變較小的位置，2則可以矯正圖像邊緣畸變較大的位置；1、2為切向畸變矯正參數(shù)。

當(dāng)深度相機(jī)收集到環(huán)境圖像后，因?yàn)閳D像中包含許多信息，需要根據(jù)圖像特征從中選擇出有代表性的點(diǎn)，然后再處理，這種方法稱為特征點(diǎn)法。在眾多的特征點(diǎn)法中選擇快速特征檢測法（features from accelerated segment test，F(xiàn)AST）以加速檢測。在檢測出特征點(diǎn)后，要對比相機(jī)前后圖像間的位姿，就要對2幅圖像上的特征點(diǎn)進(jìn)行關(guān)聯(lián)，即特征匹配過程。傳統(tǒng)的特征匹配方法計(jì)算量大，所耗資源多；因此本文利用光流法匹配特征?；谙∈韫饬鞣ǎ↙ucus-Kanade，LK），使用反向組合光流來實(shí)現(xiàn)圖像配準(zhǔn)，其具體流程如下：

步驟3）計(jì)算得到海森矩陣。

步驟4）按照圖形變換的表達(dá)式，計(jì)算得出的變換((;))，并計(jì)算誤差((;))-()。

步驟7）重復(fù)步驟4）～6），直至滿足Δ≤，為一個(gè)很小的正數(shù)。

光流法可以有效降低錯(cuò)誤匹配的概率，但是仍可能存在少部分誤匹配的數(shù)據(jù)，對此本文利用隨機(jī)抽樣一致性法（random sample consensus，RANSAC）進(jìn)行剔除。具體流程如下：

步驟1）從特征集中任意選擇4個(gè)不共線的數(shù)據(jù)，計(jì)算出變換矩陣，記作模型。

步驟2）計(jì)算出特征集中所有樣本與的投影誤差，如果其值小于閾值，則放于內(nèi)點(diǎn)集合。

步驟3）若此時(shí)內(nèi)點(diǎn)集樣本數(shù)目多于最佳內(nèi)點(diǎn)集合I, 則令I=，并更新迭代次數(shù)。

步驟4）判斷當(dāng)前迭代數(shù)是否達(dá)到最大迭代數(shù)，若是則輸出內(nèi)點(diǎn)集；反之，繼續(xù)循環(huán)上述步驟。

1.3 IMU模型

相比于LiDAR和深度相機(jī)，IMU獲得的機(jī)器人位姿信息更加準(zhǔn)確。本文所采用的IMU觀測模型為

式中：上標(biāo)a為加速度；為角速度；g為陀螺儀；F為正交；為各軸誤差變換矩陣；為尺度因子；Q為非正交；表示零偏誤差；為其他誤差。

2 多傳感器數(shù)據(jù)融合建圖

在機(jī)器人導(dǎo)航領(lǐng)域，SLAM的問題可以簡單描述為：在未知環(huán)境中的機(jī)器人，可以根據(jù)位姿和地圖信息估計(jì)自身的位姿，并能夠在行駛過程中構(gòu)建地圖，從而實(shí)現(xiàn)自主導(dǎo)航[13-15]。SLAM問題的運(yùn)動(dòng)模型可表示為

式中：x為機(jī)器人位姿；z為系統(tǒng)觀測值；(·)為系統(tǒng)運(yùn)動(dòng)函數(shù)；(·)為系統(tǒng)觀測函數(shù)；u為系統(tǒng)控制值；w為系統(tǒng)過程噪聲；v為系統(tǒng)觀測噪聲。

本文提出一種基于多傳感器數(shù)據(jù)融合的建圖方法，以解決環(huán)境中低障礙物無法感知的問題。該算法擬實(shí)現(xiàn)LiDAR、深度相機(jī)、IMU的信息融合，避免單一傳感器導(dǎo)致的檢測精度低的問題，提高SLAM 算法的準(zhǔn)確性和穩(wěn)定性。整個(gè)算法過程主要分為4個(gè)部分，即傳感器標(biāo)定、數(shù)據(jù)預(yù)處理、EKF數(shù)據(jù)融合、建圖與回環(huán)檢測。

在機(jī)器人的硬件設(shè)備中，傳感器是最基本的硬件之一，它能夠幫助機(jī)器人獲取周圍環(huán)境信息，從而更好地進(jìn)行導(dǎo)航。本文機(jī)器人設(shè)備共搭載LiDAR、深度相機(jī)和IMU 3種類型傳感器，基于上述傳感器模型，獲取環(huán)境觀測數(shù)據(jù)并對其進(jìn)行標(biāo)定。

2.1 數(shù)據(jù)預(yù)處理

數(shù)據(jù)預(yù)處理旨在將深度相機(jī)采集的圖像轉(zhuǎn)換成LiDAR數(shù)據(jù)，從而更好地進(jìn)行下步一數(shù)據(jù)融合。轉(zhuǎn)換原理如圖3所示。

圖3 數(shù)據(jù)轉(zhuǎn)換原理

轉(zhuǎn)換步驟如下：

1）對采集的深度圖像進(jìn)行有效區(qū)域的劃分，從而得到待處理的深度圖像（,）。

2）通過深度圖像（,）和相機(jī)參數(shù)模型，獲取深度相機(jī)的各像素在坐標(biāo)系中的坐標(biāo)(,,)。

3）將空間點(diǎn)云(,,)投影到激光掃描區(qū)域，計(jì)算圖中角的角度。計(jì)算公式為

激光的掃描范圍是[,]，激光束被分成個(gè)部分，激光數(shù)據(jù)用激光[]表示，投射到陣列中的點(diǎn)的索引值為，二者具體表達(dá)式如下：

2.2 基于EKF算法的數(shù)據(jù)融合

當(dāng)單獨(dú)使用深度相機(jī)或LiDAR進(jìn)行地圖構(gòu)建時(shí)，由于單一傳感器的缺陷，會(huì)有一定的誤差。因此，本文利用EKF將深度相機(jī)數(shù)據(jù)和LiDAR數(shù)據(jù)進(jìn)行融合，以提高定位的精度。在使用EKF進(jìn)行數(shù)據(jù)融合時(shí)，由于LiDAR比深度相機(jī)的測量更為精準(zhǔn)，因此將深度相機(jī)的測量值作為當(dāng)前時(shí)刻的系統(tǒng)預(yù)測值，將LiDAR的測量值作為系統(tǒng)測量值，從而保證數(shù)據(jù)融合的準(zhǔn)確性。EKF數(shù)據(jù)融合的步驟如下：

1）預(yù)測操作。首先，設(shè)置好機(jī)器人的位置，并計(jì)算出其與起始點(diǎn)的誤差協(xié)方差。使用深度相機(jī)收集的機(jī)器人行駛數(shù)據(jù)，將該值作為此時(shí)系統(tǒng)輸入值u，預(yù)測后一個(gè)時(shí)刻的位姿，并計(jì)算預(yù)測值的誤差協(xié)方差。具體表達(dá)式如下：

2）數(shù)據(jù)關(guān)聯(lián)。在將深度相機(jī)和LiDAR數(shù)據(jù)融合之前，需要將它們的數(shù)據(jù)進(jìn)行聯(lián)接，以防止二者數(shù)據(jù)錯(cuò)配。根據(jù)獲得的觀測信息之間的馬氏距離，判斷2個(gè)傳感器獲得的數(shù)據(jù)之間的相關(guān)性。具體表達(dá)式如下：

式中：為可靠性；τ為測量的位置誤差；l為LiDAR的測量值；c為深度相機(jī)的測量值。

3）更新狀態(tài)。首先，需要算出卡爾曼增益K，然后用數(shù)據(jù)測量值校正原始系統(tǒng)預(yù)測值。同時(shí)，通過K計(jì)算出數(shù)據(jù)融合的最佳預(yù)估值，并得出此時(shí)的誤差協(xié)方差。具體表達(dá)式如下：

式中：為變換矩陣；為測量的高斯噪聲的協(xié)方差矩陣。

2.3 地圖構(gòu)建和回環(huán)檢測

根據(jù)EKF融合后的數(shù)據(jù)與圖優(yōu)化法搭建環(huán)境。利用迭代最近點(diǎn)（iterative closest point，ICP）算法將當(dāng)前激光點(diǎn)云數(shù)據(jù)與子地圖進(jìn)行匹配，獲得姿態(tài)變換關(guān)系，形成閉環(huán)約束插入到后端優(yōu)化中。假設(shè)當(dāng)前幀的LiDAR特征點(diǎn)為P，通過EKF融合得到的機(jī)器人位置坐標(biāo)點(diǎn)為圓心，為姿態(tài)距離檢測的半徑。檢測到的目標(biāo)幀的點(diǎn)云，如果與當(dāng)前姿勢的距離小于，則作為待檢測的點(diǎn)云P。然后對和運(yùn)行ICP算法，輸出姿勢矢量，并計(jì)算出檢測到的姿態(tài)在歷史姿態(tài)中的位置k。最后，在k-1和k幀之間對機(jī)器人姿勢進(jìn)行優(yōu)化和修正，并輸出修正后的姿勢軌跡和地圖。其中ICP匹配算法具體如下：

1）選擇掃描點(diǎn)集P，目標(biāo)點(diǎn)集P，并對點(diǎn)云集進(jìn)行預(yù)處理。

2）進(jìn)行匹配操作，構(gòu)造轉(zhuǎn)換矩陣。

3）利用奇異值分解（singular value decomposition，SVD）方法，分解計(jì)算得出轉(zhuǎn)換矩陣。

4）判斷P映射到P上點(diǎn)集與P中相應(yīng)點(diǎn)集的距離平方和是否小于收斂誤差或者是否達(dá)到最大迭代次數(shù)，若是則停止迭代，反之繼續(xù)迭代。

3 移動(dòng)機(jī)器人路徑規(guī)劃算法

3.1 基于改進(jìn)AOS算法的全局路徑規(guī)劃

針對傳統(tǒng)AOS算法初始化種群不足，導(dǎo)致收斂過慢和探解的精度不高的問題，提出2種改進(jìn)方案：一是混沌種群初始化策略，豐富初代種群；二是引入自適應(yīng)非線性光子速率，使得探測與尋優(yōu)可靈活切換，提高全局規(guī)劃路徑的精度。

1）初始化改進(jìn)。傳統(tǒng)AOS算法是基于高斯分布來初始化電子在原子核周圍電子云的層級(jí)。改進(jìn)后的AOS是基于混沌映射進(jìn)行電子初始化，表達(dá)式為

式中：為階次；為原子核周圍電子的總量，也即搜索空間候選解的數(shù)量；為候選解的位置。

根據(jù)原子軌道模型，每個(gè)軌道的電子都有自身能級(jí)狀態(tài)，在數(shù)學(xué)模型中將每個(gè)電子的能量值作為候選解的目標(biāo)函數(shù)值，有

式中：為包含個(gè)電子目標(biāo)函數(shù)值的向量；E為第個(gè)候選解的能量值。各能量層級(jí)的電子的位置向量和目標(biāo)函數(shù)值表達(dá)式如下：

原子須根據(jù)每一層候選解的位置和目標(biāo)函數(shù)值來確定從該電子層中移除電子所需的能量，這個(gè)能量稱之為結(jié)合能。通過計(jì)算解空間內(nèi)所有候選解的平均位置和平均能量值來確定原子所需的結(jié)合態(tài)S和結(jié)合能E，其表達(dá)式如下：

2）光子速率改進(jìn)。AOS的全局搜索階段為表示光子對原子核周圍電子的作用，對每個(gè)電子在(0,1)范圍內(nèi)生成一個(gè)均勻分布的隨機(jī)數(shù)與參數(shù)光子速率R進(jìn)行比較。原算法的R為一個(gè)定值參數(shù)，并不能合理切換探測與尋優(yōu)的過程，因此引入非線性化函數(shù)修正光子速率，為

式中：0為光子對電子的作用的概率；為隨機(jī)變量。當(dāng)

式中為隨機(jī)數(shù)向量。

AOS的局部尋優(yōu)階段，此時(shí)≥R，則存在光子對電子的作用，再根據(jù)光子的發(fā)射或吸收，考慮電子在原子核周圍不同層間的運(yùn)動(dòng)。此時(shí)候選解的位置更新過程為

3.2 七階mini-Snap軌跡優(yōu)化

傳統(tǒng)路徑平滑算法解析能力要求高、規(guī)劃時(shí)間長。因此本文利用mini-Snap軌跡參照點(diǎn)，在路徑附近插入平衡點(diǎn)，從而盡可能減少和原軌跡之間的偏移量，提升優(yōu)化質(zhì)量。mini-Snap軌跡平滑算法表達(dá)式如下：

式中：為時(shí)間；為軌跡系數(shù)。其目標(biāo)函數(shù)為：

3.3 局部路徑規(guī)劃

DWA可以實(shí)現(xiàn)局部路徑規(guī)劃和實(shí)時(shí)避障。移動(dòng)機(jī)器人運(yùn)行過程中不存在速度空間組(v,ω)，可以在窗口區(qū)域時(shí)間內(nèi)模擬出機(jī)器人的可行軌跡。結(jié)合全局規(guī)劃計(jì)算評(píng)價(jià)函數(shù)，確保最終局部路徑規(guī)劃也是全局最優(yōu)路徑。本文采用的評(píng)價(jià)函數(shù)如下：

式中：(,)為路徑最終方向與終點(diǎn)之間的角度差；(,)為靜態(tài)障礙與當(dāng)前路徑之間的最短距離；(v,ω)為未知障礙與當(dāng)前路徑之間的最短距離；(,)為機(jī)器人速度評(píng)價(jià)函數(shù)；1、2、3、4為4種因素的權(quán)重系數(shù)。

3.4 算法流程

本文導(dǎo)航算法流程如圖4所示。

圖4 算法流程

4 實(shí)驗(yàn)與結(jié)果分析

基于機(jī)器人操作系統(tǒng)（robot operating system，ROS）及Kobuki移動(dòng)平臺(tái)對算法進(jìn)行驗(yàn)證和分析。同時(shí)配備二維LiDAR、深度相機(jī)（Kinect V2）、慣性導(dǎo)航、微機(jī)等設(shè)備，如圖5所示。其他運(yùn)行條件為Windows11系統(tǒng)、AMD R7-5800H處理器、16GB運(yùn)行內(nèi)存。其他相關(guān)數(shù)據(jù)如表1所示。

圖5 實(shí)驗(yàn)移動(dòng)平臺(tái)

表1 實(shí)驗(yàn)參數(shù)設(shè)置

4.1 建圖仿真及分析

1）實(shí)驗(yàn)場景建圖。實(shí)驗(yàn)場景通過添加水桶、紙箱等作為障礙物，如圖6（a）所示。其中障礙物2的高度低于車載LiDAR的安裝高度，障礙物1、3高于LiDAR高度，而障礙物4與LiDAR的安裝高度一致。為驗(yàn)證本文提出的方法的可靠性，在模擬環(huán)境中進(jìn)行了地圖構(gòu)建實(shí)驗(yàn)。圖6（b）是僅使用LiDAR的環(huán)境地圖，圖6（c）是僅使用深度相機(jī)的環(huán)境地圖。圖6（d）是數(shù)據(jù)融合后的環(huán)境地圖。

圖6 建圖實(shí)驗(yàn)結(jié)果

從圖6中可以看出，由于受LiDAR裝置高度的影響，無法檢測到障礙物2，因此通過掃描獲得的地面數(shù)據(jù)不完整，導(dǎo)致測繪精度低。深度相機(jī)比LiDAR可以獲得更多的環(huán)境信息。然而，深度相機(jī)的檢測精度很容易受到環(huán)境光的影響，導(dǎo)致目標(biāo)檢測精度低，構(gòu)建的地圖模糊。本文通過將深度相機(jī)和LiDAR的探測數(shù)據(jù)進(jìn)行融合構(gòu)建地圖，比較可知，二者數(shù)據(jù)融合后建立的環(huán)境優(yōu)于單一傳感器建圖。主要是因?yàn)楸疚乃惴▽⒍邇?yōu)勢互補(bǔ)，使得建立的地圖信息更加精確。因此本文算法建立的環(huán)境圖更加接近真實(shí)環(huán)境，有利于機(jī)器人更好地進(jìn)行導(dǎo)航。

2）定位精度分析。為檢驗(yàn)本文算法定位精度及魯棒性，在室內(nèi)環(huán)境、走廊環(huán)境2種場景下，分別基于單一LiDAR、單一深度相機(jī)及本文算法進(jìn)行定位建圖實(shí)驗(yàn)。比較機(jī)器人行駛中各方向上均方根誤差（root mean square error，RMSE），具體結(jié)果如表2所示。

表2 定位結(jié)果

由表2可知，本文算法在2種場景下，相較于單一的LiDAR及深度相機(jī)定位方法，在各個(gè)方向上的RMSE值均更小，定位精度更優(yōu)，無論是室內(nèi)場景或是長廊場景均有較強(qiáng)魯棒性，具有良好的建圖表現(xiàn)。

4.2 路徑規(guī)劃仿真及分析

在進(jìn)行實(shí)車實(shí)驗(yàn)前，首先對算法進(jìn)行仿真測試，驗(yàn)證改進(jìn)算法的性能。先后對改進(jìn)AOS算法與基本AOS法進(jìn)行仿真對比，結(jié)果如圖7所示，指標(biāo)對比如表3所示。

圖7中，各算法均規(guī)劃出無碰路徑；但具體而言，改進(jìn)AOS算法相較于傳統(tǒng)AOS、文獻(xiàn)[11]算法在路徑長度方面分別縮短約2.6%及1.3%，規(guī)劃時(shí)間上分別減少約17.6%及8.7%。另外，改進(jìn)前AOS及文獻(xiàn)[11]算法的路徑存在較多轉(zhuǎn)彎點(diǎn)，在實(shí)際運(yùn)行過程中對機(jī)器人的轉(zhuǎn)彎能力要求較高。而經(jīng)過七階mini-Snap平滑后的本文算法則相對更加平滑，沒有尖銳轉(zhuǎn)彎點(diǎn)，同時(shí)能更快到達(dá)目標(biāo)點(diǎn)。

圖7 已知障礙環(huán)境仿真圖

表3 指標(biāo)對比

在環(huán)境中設(shè)置未知障礙檢驗(yàn)本文算法局部規(guī)劃性能，結(jié)果如圖8所示。從圖中可看出：全局路徑上不存在未知障礙物時(shí)，算法直接執(zhí)行全局最優(yōu)路徑；當(dāng)原線路出現(xiàn)未知障礙物時(shí)，執(zhí)行動(dòng)態(tài)窗口局部避障策略，根據(jù)檢測到的未知障礙物數(shù)據(jù)，更新局部目標(biāo)點(diǎn)，對障礙物進(jìn)行繞行，同時(shí)對目標(biāo)點(diǎn)實(shí)施追蹤；當(dāng)繞行完成后，會(huì)根據(jù)最近的局部目標(biāo)點(diǎn)返回全局最優(yōu)路徑。由表4可知，本文算法相較文獻(xiàn)[9]、文獻(xiàn)[11]算法，路徑長度分別減少2.8%和3.8%，規(guī)劃時(shí)間分別減少14.6%和13.1%。

圖8 含未知障礙環(huán)境仿真圖

表4 指標(biāo)對比

4.3 實(shí)車導(dǎo)航實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證本文算法性能，根據(jù)4.1節(jié)中的實(shí)際環(huán)境進(jìn)行100次實(shí)車實(shí)驗(yàn)，其中一次結(jié)果如圖9所示。圖9（a）為在已知障礙環(huán)境下3種算法結(jié)果，圖9（b）為含未知障礙環(huán)境下本文算法結(jié)果。

圖9 實(shí)車實(shí)驗(yàn)結(jié)果

根據(jù)圖9可知，各算法都能夠避開障礙物，到達(dá)環(huán)境中的目標(biāo)位置，但本文算法路徑更為平滑簡短。另外，當(dāng)全局路徑上存在未知障礙物時(shí)，采用本文算法的機(jī)器人在行駛過程中可利用DWA進(jìn)行實(shí)時(shí)避障，并保持向全局最優(yōu)路徑的終點(diǎn)方向前進(jìn)，從而使得機(jī)器人的行駛更加安全平穩(wěn)且路徑規(guī)劃效率更高。

進(jìn)行多次實(shí)驗(yàn)，對各算法路徑長度及行駛時(shí)間平均值進(jìn)行對比，對未知障礙物規(guī)避次數(shù)進(jìn)行統(tǒng)計(jì)，結(jié)果如表5所示。本文算法相較于文獻(xiàn)[9]、文獻(xiàn)[11]算法在路徑長度方面分別縮短約10.1%及12.4%，行駛時(shí)間上分別減少約13.9%及31.5%，且本文算法未知障礙規(guī)避率達(dá)98%，文獻(xiàn)[9]、文獻(xiàn)[11]算法分別只有71%與76%。表明本文算法在真實(shí)動(dòng)態(tài)場景下的規(guī)劃效率更高，避障能力明顯提升。

表5 指標(biāo)對比

5 結(jié)束語

為解決機(jī)器人在復(fù)雜環(huán)境下的自主導(dǎo)航問題，設(shè)計(jì)了一種基于多源數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法。主要結(jié)論如下：

1）相比單一數(shù)據(jù)來源，采用LiDAR、深度相機(jī)、IMU多源數(shù)據(jù)融合的方法，可獲取更全面的環(huán)境信息，地圖構(gòu)建更為精準(zhǔn)，可避免光影影響下的模糊成像問題，對各種高度、形狀的物體均可準(zhǔn)確識(shí)別并建圖。

2）針對傳統(tǒng)AOS算法存在的不能兼顧全局最優(yōu)與局部精度和拐點(diǎn)多等問題，設(shè)計(jì)了多種策略進(jìn)行改進(jìn)，同時(shí)采用DWA算法進(jìn)行局部規(guī)劃，避免無法跳出局部最優(yōu)，提高了路徑規(guī)劃效率。實(shí)驗(yàn)結(jié)果顯示：在靜態(tài)仿真中，本文算法相較于對比算法，路徑長度縮短約1.3%～2.6%，規(guī)劃時(shí)間減少約8.7%～17.6%；動(dòng)態(tài)仿真中，本文算法相較于對比算法，路徑長度減少約2.8%～3.8%，規(guī)劃時(shí)間減少約13.1%～14.6%；實(shí)車試驗(yàn)中，本文算法相較于對比文獻(xiàn)算法，路徑長度縮短約10.1%～12.4%，行駛時(shí)間減少約13.9%～31.5%，未知障礙規(guī)避成功率提升至98%。驗(yàn)證了所提算法的先進(jìn)性和有效性。

未來可以嘗試引入新的融合算法及路徑規(guī)劃方法，使得導(dǎo)航效率進(jìn)一步提升。

[1] MA X, GONG R, TAN Y B, et al. Path planning of mobile robot based on improved PRM based on cubic spline[J]. Wireless Communications and Mobile Computing, 2022, 45-51.

[2] WANG K, YANG Y M, LI R X. Travel time models for the rackmoving mobile robot system[J]. International Journal of Production Research, 2020, 58(14): 4367-4385.

[3] LIAN Y D. A spatio-temporal constrained hierarchical scheduling strategy for multiple warehouse mobile robots under industrial cyber–physical system[J]. Advanced Engineering Informatics, 2022, 5:52-59.

[4] CECCHERINI S, ZOPPETTI N, CARLI B. An improved formula for the complete data fusion[J]. Atmospheric Measurement Techniques, 2022, 15(23) : 7039-7048.

[5] CHAMPION T, GRZES M, BOWMAN H. Branching time active inference with bayesian filtering[J]. Neural Computation, 2022, 34(10) :11-13.

[6] KOTSUKI S, MIYOSHI T, KONDO K, et al. A local particle filter and its Gaussian mixture extension implemented with minor modifications to the LETKF[J]. Geoscientific Model Development, 2022, 15(22) : 8325-8348.

[7] 張雯濤, 吳飛, 朱海. 自適應(yīng)強(qiáng)跟蹤AST-ESKF無人車室內(nèi)導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào), 2022, 10(4): 34-42.

[8] 許倫輝, 曾豫豪. 基于改進(jìn)ACO和三次B樣條曲線的路徑規(guī)劃[J]. 計(jì)算機(jī)仿真, 2022, 39(7): 407-411.

[9] XIANG D, LIN H X, OUYANG J, et al. Combined improved A* and greedy algorithm for path planning of multi-objective mobile robot[J]. Scientific Reports, 2022, 12(1): 455-461.

[10] 笪晨, 宋天麟, 施維. 多策略模式下RRT算法的優(yōu)化[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2022(12): 128-131, 135.

[11] LI J C, FANG Y W, CHEN H Y, et al. Unmanned aerial vehicle formation obstacle avoidance control based on light transmission model and improved artificial potential field[J]. Transactions of the Institute of Measurement and Control, 2022, 44(16): 3229-3242.

[12] AZIZI M. Atomic orbital search: a novel metaheuristic algorithm[J]. Applied Mathematical Modelling, 2021, 93(1): 59-67.

[13] YATHARTH A, TUSHAR N, UTKARSH S, et al. Depth reconstruction based visual SLAM using ORB feature extraction[J]. Journal of Image and Graphics, 2022, 10(4): 88-95.

[14] ZHANG X G, ZHANG Z H, WANG Q, et al. Using a two-stage method to reject alse loop closures and improve the accuracy of collaborative SLAM systems[J]. Electronics,2021, 10(21): 74-85.

[15] WANG J Y, YASUTAKA F. High accuracy real-time 6D SLAM with feature extraction using a neural network:paper[J]. IEEE Journal of Industry Applications, 2021, 10(5): 512-519.

A robot navigation technology with multi-source data fusion

GUO Li1, CHEN Mengyuan2, FU Ming3

（1. Anhui Vocational College of Electronics & Information Technology, Bengbu, Anhui 233030, China; 2. Anhui Polytechnic University, Wuhu, Anhui 241000, China;3. Anhui University of Finance and Economics, Bengbu, Anhui 233030, China）

Aiming at the problems of inaccuracy and low efficiency of map construction in the navigation process of mobile robot, the paper proposed a robot navigation method based on multi-source data fusion: the extended Kalman filter was used to integrate the environment information of the laser radar and the depth camera, the pose and acceleration information of the robot was obtained by the pose sensor, and a map construction method based on the data fusion of the laser radar, the depth camera and the inertial measurement unit was proposed; then in order to balance global detection and local refinement, the improved atomic orbit search algorithm was used for global planning, and the dynamic window method was used for local obstacle avoidance. Result showed that the map built by multi-source data fusion would be closer to the real scene, and the success rate of real-time obstacle avoidance of unknown obstacles could be increased to 98% with the improved fusion algorithm, which would help improve the autonomous navigation efficiency of the robot.

data fusion; atomic orbital search algorithm; dynamic window; mobile robot; navigation

郭麗, 陳孟元, 付明. 一種多源數(shù)據(jù)融合的機(jī)器人導(dǎo)航技術(shù)[J]. 導(dǎo)航定位學(xué)報(bào), 2023, 11(3): 96-104.（GUO Li, CHEN Mengyuan, FU Ming. A robot navigation technology with multi-source data fusion[J]. Journal of Navigation and Positioning, 2023, 11(3): 96-104.）

10.16547/j.cnki.10-1096.20230313.

P228；TP242

A

2095-4999(2023)03-096-09

2022-12-23

國家自然科學(xué)基金(61903002)；安徽省質(zhì)量工程重大項(xiàng)目(2020zdxsjg029)；安徽省教育廳重點(diǎn)項(xiàng)目（gxyqZD2021110)；安徽省高校人文社會(huì)科學(xué)研究重點(diǎn)項(xiàng)目(SK2019A0920)；安徽省高校優(yōu)秀青年人才支持計(jì)劃重點(diǎn)項(xiàng)目(gxyqZD2018131)；安徽省職業(yè)教育創(chuàng)新發(fā)展試驗(yàn)區(qū)項(xiàng)目(WJ-PTZT-041)；安徽省質(zhì)量工程人工智能技術(shù)應(yīng)用專業(yè)教學(xué)創(chuàng)新團(tuán)隊(duì)項(xiàng)目（2021jxtd024）；安徽省質(zhì)量工程教學(xué)示范課項(xiàng)目（2020SJJXSFK021）。

郭麗（1982—），女，安徽壽縣人，碩士，副教授，研究方向?yàn)橛?jì)算機(jī)技術(shù)、導(dǎo)航技術(shù)等。