[發明專利]一種基于層次結構的動態粒子樹SLAM算法

無效

• 申請號: 201410150310.1
• 申請日: 2014-04-12
• 公開（公告）號: CN103901891A
• 公開（公告）日: 2014-07-02
• 發明（設計）人: 金城;楊昭;馮瑞;薛向陽
• 申請（專利權）人: 復旦大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 上海正旦專利代理有限公司 31200
• 代理人: 陸飛;盛志范

1.一種基于層次的動態粒子樹SLAM算法，其特征在于具體步驟為：?

（1）建立數據模型，數據模型包括機器人運動模型和激光數據模型：?

（1.1）?建立機器人運動模型

在機器人系統采用的運動模型中，把機器人抽象成笛卡爾坐標系中的一點，它的正向方向與????????????????????????????????????????????????軸的夾角是機器人的轉向角，坐標是機器人在地圖中的位置；

在這一模型下，機器人的每一次運動都可以看作是從坐標到，其中位置移動為兩點間的歐式距離，如公式(1)所示：

?????????????????????????(1)

轉向角的變化分為移動轉向角和原地轉向角變化，具體如下：

???????????????????(2)

????????????????????????????????(3)

對于存在著較大的誤差，采用高斯誤差進行估計，即在原有的上添加均值為0，方差為的高斯分布；所以從粒子濾波角度來說，機器人的位置是一個在地圖空間上概率分布；

機器人每一次運動后，新的位置會分布在真實位置周圍；

（1.2）?建立激光數據模型

把激光傳感器抽象成從原點發射出一定數量的射線，當遇到障礙時則停止發射，返回遇到障礙時的距離；

機器人當前所在的位置為中心，從它出發的射線在遇到障礙時會停下，從而得出障礙里機器人的大致距離，同時估計機器人周圍的地圖存在障礙的概率；當某條射線超過激光的最大可探測距離時，認為在那個位置上存在障礙；

對于每一條射線，它的實際作用范圍是一個扇形；在扇形的圓弧上，障礙物存在的概率是一個平緩的高斯分布，中央點擁有最高的可能性,?而圓弧兩邊只有；同時在扇形內部，也存在一個障礙概率下降曲線，離原點越近，該點是非障礙的概率越高；原點的概率設定為；在每次得到激光的數據后，就得到概率地圖；

根據這一模型，對每條激光射線估計出在射線掃描范圍內地圖主要信息，即；激光概率模型按公式(4)估算出概率分布：

??????????????????????????(4)

（2）采用動態粒子樹SLAM算法進行計算?

（2.1）進行基于粒子濾波的狀態估計?

SLAM?算法的原理是機器人在未知環境中從一個未知位置開始運動，在運動過程中根據位置估計和傳感器數據進行自身定位，同時建造增量式地圖；其核心部分之一，就是在已知觀察序列和操作序列的條件下，計算當前機器人的狀態以及更新地圖信息，并持續對該序列進行更新；由于機器人的運動以及傳感器的數據采集都屬于概率模型，在給定的時刻以及從的觀測序列后，根據貝葉斯概率模型，有公式(5)：

???(5)

其中，是機器人的狀態信息向量，代表二維地圖信息，它是一個矩陣，每一個元素代表實際場景中對于采樣塊的障礙信息，即該采樣塊是障礙的概率，代表每一個時刻傳感器的數據信息，它是對機器人周圍環境的具體描述，也是繪制地圖的重要信息，則是對機器人的基本操作指令；

根據概率分布進行采樣，選擇粒子濾波算法對這個概率模型進行描述，機器人狀態粒子為機器人位置與地圖信息：

粒子濾波的步驟包括：

（1）采樣，已知前一階段的粒子狀態，根據求出這一階段所有粒子可能產生的狀態；

（2）計算每一個粒子的權重，每個粒子的和真實概率的比值，對于那些和實際概率差距很大的粒子在下一步舍去；

（3）重采樣，為了保持一定的粒子數量，舍去那些權重比較低的粒子；在一段迭代次數后，機器人的狀態和地圖信息會收斂到與真實環境較接近的位置；

（4）地圖信息估計，對于每一個粒子，都對應了一個地圖，根據進行計算；

在步驟（4）中，SLAM算法為機器人的整個運動軌跡和環境地圖的信息構建一個概率分布，并從中得出最有可能的數據；

（2.2）?動態粒子樹SLAM算法

動態粒子樹SLAM算法是為環境地圖的每一個采樣點維護一棵粒子樹，樹中的所有粒子都已完成它所在的采樣點的更新；

粒子樹的葉子結點是重采樣后產生的新粒子，對每個粒子來說，它都擁有一張完整的環境地圖；?

在粒子樹中，父結點在重采樣過程后，生成一些新的粒子，作為下一階段使用，這些粒子作為它的子結點；同時，某一粒子在對地圖作更新時，直接把傳感器的數據信息保存在樹的結點中，無需到地圖上更改；所以在粒子樹的任意一個結點為根的子樹中，所有粒子都滿足祖先所觀察的數據；

另一方面，粒子樹的任何內結點都至少有兩個子節點；對于沒有子節點的粒子結點，把它從樹中刪除；對于只有一個子節點的粒子，把子節點和父節點的傳感器數據進行合并，等效成一個粒子，然后將子結點刪除；?

最后，在生成地圖時，對于每一個采樣點，只需檢查該方格上最近訪問的粒子，然后提取它們所保存的數據信息，計算這個方格存在障礙物的概率；

（三）層次化粒子信息分析

采用層次算法進行分析，層次算法的基本思想是在低層依然使用前一部分所提到的算法，但低層只是處理整個環境中的一部分，然后將這一部分的結果“提交”給高層過程，它對各個低層所產生的局部地圖進行重組，制作出完整的地圖；具體來說：

層次化SLAM算法給導航和地圖繪制提供多分辨率操作：低層的SLAM算法可以比較精確地探知一小塊地圖，然后得到機器人的行走軌跡和小地圖的概率分布；將得到的軌跡和地圖信息提交給高層后，高層中的每個粒子包含的是機器人的軌跡和地圖數據；最后高層SLAM算法過程對這些粒子進行權重計算，重采樣等操作，得到新的高層地圖。