[發明專利]一種針對四輪機器人的轉動瞬心估計方法

有效

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

1.一種針對四輪機器人的轉動瞬心估計方法，其特征在于，包括以下步驟：

步驟一：對采樣點序號k、后驗狀態估計后驗誤差協方差P_k、新息協方差ε_k、地形特征向量p_k、過程噪聲和觀測噪聲的方差Q_k和R_k、采樣間隔f以及車身寬度B進行初始化，其中，后驗狀態估計中的與分別表示東向坐標、北向坐標與航向角的后驗狀態估計，與為轉動瞬心的3個運動學參數的后驗狀態估計；

步驟二：將采樣點序號自增k←k+1，采集加速度計關于垂直于地面軸向的加速度數據，并在一個采樣周期內按照相等時間間隔采集N次，得到加速度數據集合{a_k，i}，i＝1，…，N；利用朝向地面的攝像頭拍攝地面照片，得到像素矩陣K_k；采集左右輪編碼器數據，獲得左右車輪的旋轉速度v_L，k與v_R，k；采集電子羅盤數據與GPS模塊數據，獲得觀測向量z_k＝[z_e，k z_n，kz_θ，k]′，其中，z_e，k與z_n，k為東向坐標與北向坐標的觀測值，由GPS模塊采集獲得，z_θ，k為航向角的觀測值，由電子羅盤采集獲得；

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

步驟四：若判定地形發生顯著變化，在接下來五個采樣點內將轉動瞬心的過程噪聲方差，乘以設定倍數；若地形沒有發生變化，則保持原來的方差；

步驟五：根據步驟一中獲得的后驗狀態估計、后驗誤差協方差、采樣間隔以及車身寬度，步驟二中獲得的左右車輪的旋轉速度以及步驟四中調整后的過程噪聲方差，進行狀態預測，得到先驗狀態估計與先驗誤差協方差；

步驟六：根據步驟一中獲得的新息協方差、觀測噪聲方差，步驟二中獲得的觀測向量以及步驟五中獲得的先驗狀態估計和先驗誤差協方差，計算得到后驗狀態估計后驗誤差協方差P_k；以及

步驟七：重復步驟二至步驟六，以輸出每一個采樣點的后驗狀態估計中第4至6元素即轉動瞬心的估計值。

2.根據權利要求1所述的針對四輪機器人的轉動瞬心估計方法，其特征在于，所述步驟三包括以下子步驟：

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

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

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

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

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

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

3.根據權利要求2所述的針對四輪機器人的轉動瞬心估計方法，其特征在于，所述步驟四中的設定倍數為10倍。

4.根據權利要求3所述的針對四輪機器人的轉動瞬心估計方法，其特征在于，所述步驟五包括以下子步驟：

4.1)創建一組Sigma點集合其中m為狀態維度，λ為比例因子；

4.2)用步驟(4.1)中的Sigma點預測狀態，得到一組狀態預測Sigma點的集合其中，與分別表示集合與中的第i個元素，狀態轉移方程f(·)具體為：

4.3)對步驟(4.2)中的狀態預測Sigma點加權，得到先驗狀態估計與先驗誤差協方差P_k，k-1：

以及

其中，W_i^m與W_i^c分別為均值與協方差的權重系數，其中，2n是Sigma點的數量。

5.根據權利要求1所述的針對四輪機器人的轉動瞬心估計方法，其特征在于，所述步驟六包括以下子步驟：

5.1)計算新息∈_k如下：其中，為觀測矩陣；

5.2)計算新息協方差ε_k如下：

5.3)計算卡爾曼增益K_k如下：

5.4)計算后驗狀態估計如下：以及

5.5)計算后驗誤差協方差P_k如下：P_k＝(I_6×6-K_kC)P_k，k-1，其中，矩陣I_6×6為6維單位矩陣。