[發明專利]基于航向輔助分布式SLAM的機器人自主導航方法

有效

• 申請號: 201610154634.1
• 申請日: 2016-03-17
• 公開（公告）號: CN105606104B
• 公開（公告）日: 2019-04-30
• 發明（設計）人: 裴福俊;程雨航;武小平;嚴鴻
• 申請（專利權）人: 北京工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京思海天達知識產權代理有限公司 11203
• 代理人: 沈波

1.基于航向輔助分布式SLAM的機器人自主導航方法，其特征在于：在每個路標點信息送入各自子濾波器的方式下，改變觀測模型的結構，在不添加每個分布式子濾波器中路標點個數的條件下，將航向信息作為狀態量融合到整個的擴展卡爾曼濾波過程中，下文將論述分布式架構中粒子濾波與卡爾曼濾波實現的差異，在采用分布式EKF的條件下，若需保證精度，與粒子濾波不同的是需要引入航向信息作為觀測量，從而在減少計算量同時保證可觀性的條件下，本方法通過航向引入的方式建立分布式擴展卡爾曼濾波的模型，設計相應的濾波器實現方式，并通過實驗驗證本算法的真實有效性；

首先，引入航向信息的系統模型闡述如下：

關于引入航向信息的機器人狀態矩陣描述如下：x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，其中，x_v,y_v表示機器人坐標值，φ_v表示機器人航向角，φ_v＝0表示指向x軸正向，φ_v＝π/2表示指向y軸正向；x_L,y_L表示探測得到的路標點坐標值，則系統的狀態轉移模型如下：

以上轉移矩陣中的參數與車體及里程計的固定參數有關，式中x_v＝[x_v,y_v,φ_v,x_L,y_L]^T為機器人在t時刻的位姿及路標點位置,ΔT為時間變化量，v_c為機器人移動速度，α為機器人車輪轉角，L為兩輪軸間距；

擴展卡爾曼濾波的量測更新結果是由激光傳感器測得的路標點距離信息及磁羅盤測得的本體航向信息計算得到的，針對激光傳感器及磁羅盤給出機器人的觀測模型，觀測量分別t時刻第k個路標點相對于傳感器的距離，角度以及所測得的航向；獲得路標點的觀測模型如下：

其中，m_L,x,m_L,y表示測量到的路標點相對于x_v,y_v的測量信息；

在分布式框架下，帶有航向信息的各子濾波器的模型如下：

…………

其中，γ為高斯白噪聲，每個子濾波器中對應的SLAM狀態向量分為機器人位姿估計和路標估計共五維狀態，即每次狀態轉移矩陣包含的信息由機器人本體的位置信息與一個路標點的位置信息決定；其中各子濾波器狀態向量x_v＝[x_v,y_v,φ_v,x_L,y_L]^T，h(x_v(t),m_n)表示t時刻，根據該時刻機器人狀態向量x_v(t)和對應的測量信息m_n所估計的第n個路標點的測量信息；

分布式結構的濾波方式分為粒子濾波PF和擴展卡爾曼濾波EKF，PF屬于非線性濾波方法，解決SLAM這種非線性模型問題具有精度上的優勢，但其計算量較大，并較難移植到實時性較高的移動平臺上；

對于PF，根據各子濾波器中粒子群的預測分布，針對各種傳感器的模型近似利用蒙特卡洛法模擬粒子分布，然后對每個子濾波器中的粒子進行重要性權值估計如式(4)所示；

根據公式(4)得到的權值可見，權值反映各個子濾波器中粒子的分布情況，并且由觀測信息z_t和根據狀態向量估計的所近似的分布計算得來，每個子濾波器雖然只利用單一路標點進行濾波估計，但由于PF的權值由概率分布模型決定，并不是基于線性模型迭代計算得到，而是模擬概率統計的模型得到，故可觀測性得到保證；

根據權重值更新，可輸出t時刻第j個子濾波器的位姿估計結果如式(5)

根據子濾波器位姿結果，進行主濾波器的融合；如式(6)及式(7)所示；

其中，最終位姿估計x_t由子濾波器估計結果融合而來，融合系數由各子濾波器有效粒子數決定；

根據以上各式可知，PF算法整個計算過程均與子濾波器估計的概率分布有關，可觀測性的問題并不像EKF算法影響顯著，但其精度隨著濾波器所使用的粒子數的增加而增加，所以，若想要保證良好的精度，往往濾波器所需的粒子數也較多，即計算的次數要遠遠大于其他非統計型的濾波方法；

考慮到計算量的問題，選用相對計算量可控的EKF算法，提高系統的實時性，而分布式架構又能很好的提高系統的容錯性及計算性能；故采用分布式架構上使用EKF的方法可以充分發揮兩者優勢，即每個濾波器采用固定的路標點個數，防止過多動態重構的發生，并且每個濾波器使用EKF進行濾波；該種解決方案雖然最大程度地發揮了兩者的優點，但不可忽視的是，EKF方法將非線性模型轉化為線性模型，根據線性模型的可觀測性的特質，如果濾波器中只有一個觀測點或多個觀測點但共線時，系統的可觀測性會受到影響，無法滿足濾波的可觀要求，為了兼容每個濾波器中輸入路標點相對較少且固定，并降低其對可觀測性的影響，加入航向對可觀測性進行改善，當每個子濾波器中存在一個路標點且存在當前時刻機器人本體的航向時，整個系統是可觀的；綜上，以下描述基于分布式架構下，結合航向信息輔助的分布式EKF算法；

首先，卡爾曼濾波解決的是線性系統模型，而SLAM模型屬于非線性模型，將其進行線性化處理如下，航向信息作為觀測量引入：

其中

Δx＝[x_v-x_L]Δy＝[y_v-y_L] (12)

在模型的線性化操作后，針對每個路標點建立相應的子濾波器，并匹配相關t-1時刻的路標點，將各個子濾波器中代入相應的航向值；子濾波器的位姿估計過程為：

根據將公式(1)線性化后的姿態轉移矩陣進行以上的位姿預測估計，得到的結果作為以下量測更新的輸入；

量測更新的過程在各子濾波器中進行，結合線性化后的觀測模型H_i(k)，進行以下的卡爾曼濾波量測更新過程，更新卡爾曼濾波增益K_i(t)和協方差

得到子濾波器更新的結果后，根據協方差矩陣進行融合過程，從而輸出最終的位置：

其中η_i為加權系數，加權平均法將來自不同子濾波器的機器人位姿估計信息計算權值，進行加權平均后的結果作為融合值。

2.根據權利要求1所述的基于航向輔助分布式SLAM的機器人自主導航方法，其特征在于：

步驟1：模型的建立

系統模型的建立是基于機器人、環境特征和傳感器這三個坐標系來定義的，通過狀態轉移方程和坐標系建立里程計預測模型，本方法選用笛卡爾坐標系統作為直角坐標的表示形式，把傳感器坐標系統一到機器人坐標系中，只采用全局坐標系和機器人坐標系；狀態轉移矩陣包含的信息由機器人本體的位置信息與一個路標點的位置信息決定；即x_v＝[x_v,y_v,φ_v,x_i,y_i]^T，建立運動模型和觀測模型見公式(1)(2)，兩者分別利用里程計的輸出和激光傳感器的輸出來估計相應的機器人及路標點位置信息；

步驟2：濾波初始化及數據預處理

設定卡爾曼濾波的P/Q/R矩陣初值，并進行全局坐標下機器人位置的初始化，對準磁羅盤激光傳感器傳感器時鐘以及讀取數據路徑；設置匹配路標點閾值初始化參數；此外，對傳感器數據進行預處理，根據每一時刻掃描的點云中設計函數提取激光傳感器特征點，并進行航向弧度值的計算；

步驟3：路標點匹配

在機器人運行過程中，將t時刻觀測到路標點與已在全局地圖中存儲的路標點相匹配，進行相關的數據關聯運算，若匹配成功，則送入所匹配上的t-1時刻對應路標點的子濾波器進行后續運算；若沒有匹配成功，則說明該路標點為新增路標點，直接將其添加至全局地圖并建立相關子濾波器；對于t時刻未能觀測到t-1時刻路標點的子濾波器，進行刪除；

步驟4：濾波器運行過程

將運動模型及觀測模型進行線性化處理，將傳感器輸入的結果通過線性化的模型處理后進行卡爾曼濾波，每個路標點的信息及t時刻的機器人航向信息送入同一個子濾波器，通過卡爾曼濾波算法對上述步驟中建立的各個子濾波器的狀態向量和協方差進行處理，分別進行狀態轉移矩陣和相應協方差矩陣的迭代更新，每個子濾波器中進行如下兩個關鍵的計算步驟：

預測過程：

首先將t-1時刻的每個子濾波器的估計狀態和協方差矩陣，結合t時刻里程計的輸出數據，代入運動模型(1)中，再經過一步預測方程(14)和(15)進行一步遞推，得到由里程計和狀態轉移矩陣推算出的機器人位置；

量測更新過程：

通過第一步遞推的方式，得到機器人位置的預測值后，根據預測值的結果，加入激光傳感器和磁羅盤的觀測信息進行量測更新的計算，具體見式(16)，(17)和(18)，輸出每個子濾波器對位置的量測修正值及路標點的更新值，并同時輸出相應的協方差矩陣供后續迭代；

每次的濾波重復以上兩個過程，能夠持續觀測到的路標點所建立的濾波器，在t時刻繼承t-1時刻的協方差矩陣作為t時刻的初始輸入矩陣，新觀測到的路標點對應的子濾波器則采用t-1時刻最終估計得協方差矩陣作為初始輸入矩陣；

步驟5：濾波器融合輸出最終結果

將各個子濾波器的估計結果輸入到主濾波器中，根據各子濾波器協方差確定各子濾波器在主濾波器中所占的權重并歸一化，最后利用信息融合公式(20)輸出最終位置估計結果。