[發(fā)明專利]一種野外機器人航跡推算方法

有效

• 申請?zhí)枺?/td>: 201711228297.7
• 申請日: 2017-11-29
• 公開（公告）號: CN108036789B
• 公開（公告）日: 2020-01-31
• 發(fā)明（設計）人: 劉葆林;賈曉敏;李鯤;鄭敏;呂文君
• 申請（專利權）人: 安徽省一一通信息科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/00
• 代理公司: 34125 合肥市上嘉專利代理事務所(普通合伙)
• 代理人: 郭華俊

1.一種野外機器人航跡推算方法，其特征在于，包括以下步驟：

步驟一：對采樣點序號k、后驗狀態(tài)估計橢球地形特征向量P_k、過程噪聲和電子羅盤噪聲的包絡矩陣Q_k和R_k、采樣間隔T、車輪半徑Φ以及車身寬度B進行初始化，其中，后驗狀態(tài)估計橢球的橢球中心中的為航向角的后驗狀態(tài)估計橢球的中心和為轉動阻力系數(shù)的后驗狀態(tài)估計；

步驟二：將采樣點序號自增k←k+1，采集加速度計關于垂直于地面軸向的加速度數(shù)據(jù)，在一個采樣周期內(nèi)按照相等時間間隔采集N次，得到加速度數(shù)據(jù)集合{a_k,i},＝1,…,N；利用朝向地面的攝像頭拍攝地面照片，得到像素矩陣M_k；采集左右輪編碼器數(shù)據(jù)，獲得左右車輪的旋轉速度ω_L,k與ω_R,k；采集電子羅盤數(shù)據(jù)，獲得機器人航向角y_k；

步驟三：根據(jù)步驟一中獲得的地形特征向量以及步驟二中獲得的加速度數(shù)據(jù)集合和地面照片像素矩陣進行地形檢測，判斷地形是否發(fā)生顯著變化；

步驟四：若判定地形發(fā)生顯著變化，在接下來五個采樣點內(nèi)將轉動阻力系數(shù)的過程噪聲包絡矩陣，乘以設定倍數(shù)；若地形沒有發(fā)生顯著變化，則保持原來的包絡矩陣；

步驟五：根據(jù)步驟一中獲得的后驗狀態(tài)估計橢球、采樣間隔、車輪半徑以及車身寬度，步驟二中獲得的左右車輪的旋轉速度以及步驟四中調整后的過程噪聲包絡矩陣，進行狀態(tài)預測，得到先驗狀態(tài)估計橢球；

步驟六：根據(jù)步驟一中獲得的電子羅盤噪聲包絡矩陣，步驟二中獲得的機器人航向角以及步驟五中獲得的先驗狀態(tài)估計橢球，進行狀態(tài)更新，得到后驗狀態(tài)估計橢球；

步驟七：根據(jù)步驟一獲得的采樣間隔、車輪半徑以及車身寬度，步驟二中獲得的左右車輪的旋轉速度以及步驟六中獲得的后驗狀態(tài)估計橢球，進行航跡推算定位，輸出機器人位置坐標，即東向坐標e_k和北向坐標n_k；以及

步驟八：重復執(zhí)行步驟二至步驟七，獲得每一個采樣點的機器人位置坐標。

2.根據(jù)權利要求1所述的野外機器人航跡推算方法，其特征在于，所述步驟三包括以下子步驟：

2.1)消除加速度數(shù)據(jù)集合的直流分量：將加速度數(shù)據(jù)集合的所有元素分別減去該集合所有元素的均值：得到消除了直流分量的加速度數(shù)據(jù)集合

2.2)提取地面照片的主色：從地面照片像素矩陣M_k中隨機抽取50個像素并求平均，得到地形主色的紅綠藍分量l_R,k，l_G,k與l_B,k；

2.3)求取地形特征向量其中的元素如下：

2.4對地形特征向量P_k進行歸一化處理；以及

2.5判斷地形是否變化：計算地形相似度距離

其中，ω_i∈(0 ， 1] 為每個特征分量的權重，如果則判斷地形沒有變化，否則判斷地形發(fā)生了顯著變化。

3.根據(jù)權利要求2所述的野外機器人航跡推算方法，其特征在于，在步驟四中所述設定倍數(shù)為10倍。

4.根據(jù)權利要求3所述的野外機器人航跡推算方法，其特征在于，在步驟五中所述先驗狀態(tài)估計橢球如下：

其中，狀態(tài)轉移方程f(·)具體為：

其中，矩陣為狀態(tài)轉移方程f(·)的雅克比矩陣，tr( · ) 表示矩陣的跡。